Merge branch 'linaro.linux-3.0-android-ux500.master' into linux-linaro-3.0-2011.09-0-android-1

Signed-off-by: Mathieu J. Poirier <mathieu.poirier@linaro.org>
Conflicts:
	arch/arm/common/Makefile
	arch/arm/configs/android_omap3_defconfig
	arch/arm/configs/android_omap4_defconfig
	arch/arm/kernel/entry-armv.S
	drivers/gpu/ion/ion.c
	drivers/misc/Kconfig
	drivers/misc/Makefile
	drivers/mmc/card/block.c
	drivers/mmc/core/core.c
	drivers/mmc/host/mmci.c
	drivers/net/wireless/bcmdhd/Makefile
	drivers/net/wireless/bcmdhd/bcmevent.c
	drivers/net/wireless/bcmdhd/bcmsdh_linux.c
	drivers/net/wireless/bcmdhd/dhd.h
	drivers/net/wireless/bcmdhd/dhd_bus.h
	drivers/net/wireless/bcmdhd/dhd_cdc.c
	drivers/net/wireless/bcmdhd/dhd_common.c
	drivers/net/wireless/bcmdhd/dhd_linux.c
	drivers/net/wireless/bcmdhd/dhd_linux_mon.c
	drivers/net/wireless/bcmdhd/dhd_sdio.c
	drivers/net/wireless/bcmdhd/include/bcmutils.h
	drivers/net/wireless/bcmdhd/include/epivers.h
	drivers/net/wireless/bcmdhd/include/linuxver.h
	drivers/net/wireless/bcmdhd/include/proto/bcmevent.h
	drivers/net/wireless/bcmdhd/include/sdio.h
	drivers/net/wireless/bcmdhd/include/wlioctl.h
	drivers/net/wireless/bcmdhd/wl_android.c
	drivers/net/wireless/bcmdhd/wl_android.h
	drivers/net/wireless/bcmdhd/wl_cfg80211.c
	drivers/net/wireless/bcmdhd/wl_cfg80211.h
	drivers/net/wireless/bcmdhd/wl_cfgp2p.c
	drivers/net/wireless/bcmdhd/wl_cfgp2p.h
	drivers/net/wireless/bcmdhd/wl_iw.c
	drivers/net/wireless/bcmdhd/wldev_common.c
	drivers/staging/android/Kconfig
	drivers/staging/android/Makefile
	drivers/staging/android/binder.c
	drivers/staging/android/logger.c
	drivers/usb/gadget/android.c
	drivers/usb/otg/otg_id.c
	include/linux/android_aid.h
	include/linux/earlysuspend.h
	include/linux/ion.h
	include/linux/usb/otg_id.h
	include/linux/wakelock.h
	kernel/irq/pm.c
	kernel/power/consoleearlysuspend.c
	kernel/power/wakelock.c
	net/bluetooth/l2cap_core.c
	net/bluetooth/l2cap_sock.c
	net/netfilter/Kconfig
	net/netfilter/xt_qtaguid.c
	net/netfilter/xt_quota2.c

866 files changed, 266767 insertions, 46400 deletions

diff --git a/Documentation/ABI/testing/sysfs-devices-platform-_UDC_-gadget b/Documentation/ABI/testing/sysfs-devices-platform-_UDC_-gadget
index d548eaac230..1424c746fda 100644
--- a/Documentation/ABI/testing/sysfs-devices-platform-_UDC_-gadget
+++ b/Documentation/ABI/testing/sysfs-devices-platform-_UDC_-gadget
@@ -7,15 +7,15 @@ Description:
 		0 -> resumed
 
 		(_UDC_ is the name of the USB Device Controller driver)
-
-What:           /sys/devices/platform/_UDC_/gadget/gadget-lunX/nofua
-Date:           July 2010
-Contact:        Andy Shevchenko <andy.shevchenko@gmail.com>
+What:		/sys/devices/platform/_UDC_/gadget/host_request
+Date:		December 2010
+Contact:	Pavan Kondeti <pkondeti@...>
 Description:
-		Show or set the reaction on the FUA (Force Unit Access) bit in
-		the SCSI WRITE(10,12) commands when a gadget in USB Mass
-		Storage mode.
+		OTG 2.0 compliant host keeps polling OTG2.0 peripheral
+		for host role. Set host_request flag, which tells host
+		to give up the host role to peripheral.
+
+		1 -> host role is requested
+		0 -> no effect (automatically cleared upon reset/disconnect)
 
-		Possible values are:
-			1 -> ignore the FUA flag
-			0 -> obey the FUA flag
+		(_UDC_ is the name of the USB Device Controller driver)
\ No newline at end of file
diff --git a/Documentation/ABI/testing/sysfs-socinfo b/Documentation/ABI/testing/sysfs-socinfo
new file mode 100644
index 00000000000..afd9da2fa76
--- /dev/null
+++ b/Documentation/ABI/testing/sysfs-socinfo
@@ -0,0 +1,16 @@
+What:		/sys/socinfo
+Date:		March 2011
+contact:	Maxime Coquelin <maxime.coquelin-nonst@stericsson.com>
+Description:
+		The /sys/socinfo directory contains information about the
+		System-on-Chip.	It is only available if platform implements it.
+		This directory contains two kind of attributes :
+			- common attributes: 
+				* machine: the name of the machine.
+				* family: the family name of the SoC
+			- SoC-specific attributes: The SoC vendor can declare attributes
+			  to export some strings to user-space, like the serial-number for
+			  example.
+
+Users:
+		User-space applications which needs these kind of attributes.
diff --git a/Documentation/DocBook/Makefile b/Documentation/DocBook/Makefile
index 3cebfa0d161..c19617a55eb 100644
--- a/Documentation/DocBook/Makefile
+++ b/Documentation/DocBook/Makefile
@@ -14,7 +14,13 @@ DOCBOOKS := z8530book.xml mcabook.xml device-drivers.xml \
 	    genericirq.xml s390-drivers.xml uio-howto.xml scsi.xml \
 	    80211.xml debugobjects.xml sh.xml regulator.xml \
 	    alsa-driver-api.xml writing-an-alsa-driver.xml \
-	    tracepoint.xml media.xml drm.xml
+	    tracepoint.xml media.xml drm.xml \
+	    i2s.xml msp.xml shrm.xml stmpe.xml touchp.xml \
+	    tc_keypad.xml prcmu-fw-api.xml cg2900_fm_radio.xml \
+	    synaptics_rmi4_touchp.xml db5500_keypad.xml \
+	    u5500_LogicalMailbox.xml \
+	    lsm303dlh.xml \
+	    ste_timed_vibra.xml
 
 ###
 # The build process is as follows (targets):
diff --git a/Documentation/DocBook/cg2900_fm_radio.tmpl b/Documentation/DocBook/cg2900_fm_radio.tmpl
new file mode 100644
index 00000000000..5832c389cfd
--- /dev/null
+++ b/Documentation/DocBook/cg2900_fm_radio.tmpl
@@ -0,0 +1,1809 @@
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="STE-CG2900-fm-driver-template">
+ <bookinfo>
+  <title>V4L FM Radio Driver for CG2900</title>
+  <authorgroup>
+   <author>
+    <firstname>Hemant</firstname>
+    <surname>Gupta</surname>
+    <affiliation>
+     <address>
+      <email>hemant.gupta@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+  </authorgroup>
+  <copyright>
+   <year>2010</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+  <subjectset>
+    <subject>
+      <subjectterm>Connectivity</subjectterm>
+    </subject>
+  </subjectset>
+  <legalnotice>
+   <!-- Do NOT remove the legal notice below -->
+  <para>
+     This documentation 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.
+   </para>
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+  </legalnotice>
+ </bookinfo>
+ <toc></toc>
+ <chapter id="intro">
+  <title>Introduction</title>
+  <!-- Do NOT change the chapter id or title! -->
+  <para>
+    This documentation describes the functions provided by the CG2900 FM Driver.
+  </para>
+ </chapter>
+ <chapter id="gettingstarted">
+  <title>Getting Started</title>
+  <!-- Do NOT change the chapter id or title! -->
+  <para>
+     There are no special compilation flags needed to build the CG2900
+     FM Driver.
+  </para>
+  <para>
+    There must be coeffecient and firmware files that match the used chip version inside the firmware folder.
+    The files:
+    <itemizedlist>
+      <listitem><para>cg2900_fm_bt_src_coeff_info.fw.org</para></listitem>
+      <listitem><para>cg2900_fm_ext_src_coeff_info.fw.org</para></listitem>
+      <listitem><para>cg2900_fm_fm_coeff_info.fw.org</para></listitem>
+      <listitem><para>cg2900_fm_fm_prog_info.fw.org</para></listitem>
+    </itemizedlist>
+    handle the mapping between chip version and correct firmware files (firmware and coeffecient files).
+    The necessary firmware and coeffecient files should be placed with the extension <constant>.fw.org</constant>.
+    Note that there is a limitation in the Kernel firmware system regarding name length of a file.
+  </para>
+  <section id="basic-tutorial">
+    <title>Basic Tutorial</title>
+    <para>
+      To enable the CG2900 FM Driver using KConfig go to <constant>Device Drivers -> Multimedia devices </constant>
+      and enable the following:
+    <itemizedlist>
+      <listitem><para>Video For Linux</para></listitem>
+      <listitem><para>Enable Video For Linux API 1 compatible Layer</para></listitem>
+      <listitem><para>Radio Adapters</para></listitem>
+      <listitem><para>Radio Adapter -> ST-Ericsson CG2900 FM Radio support</para></listitem>
+    </itemizedlist>
+      Select the driver as built in kernel object.
+    </para>
+    </section>
+ </chapter>
+ <chapter id="concepts">
+  <title>Concepts</title>
+  <!-- Do NOT change the chapter id or title! -->
+  <para>
+     The CG2900 FM driver acts as an interface between Video4Linux and CG2900-Protocol Driver. It configures the FM chip in FM Rx or FM Tx mode. It also sends the unformatted RDS data to the application for decoding while in FM rx mode and sends the formatted RDS data to FM Chip while in Tx mode.
+   </para>
+   <para>
+     <variablelist>
+       <varlistentry>
+         <term>FM Driver Working</term>
+         <listitem>
+         <para>
+           In order to send and receive data on an H:4 channel, the FM Driver opens the channel by registering with the CG2900 Protocol driver. After this the FM driver encapsulates the user operation into specific HCI comamnds and sends that data to the CG2900 Connectivity Controller and waits till the response for the previous command is received. FM Driver in this way maintains the flow control.
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </para>
+ </chapter>
+ <chapter id="Tasks">
+   <title>Tasks</title>
+   <!-- Do NOT change the chapter id or title! -->
+   <section id="Switching-On-FM">
+     <title>Switch On FM</title>
+     <para>
+		FM specific tasks
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Switching On FM</term>
+         <listitem>
+         <para>
+           For switching on FM the character device /dev/radio0 should be opened from user space. This sets the FM Radio in Idle mode. For configuring the FM Radio in Rx or Tx mode the IOCTL's VIDIOC_S_TUNER and VIDIOC_S_MODULATOR respectively.
+           <programlisting>
+		int fd;
+		fd = open("/dev/radio0", O_RDONLY);
+		if(fd &lt; 0) {
+			printf("open:error!!!\n");
+			goto err;
+		}
+           </programlisting>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="Switching-Off-FM">
+     <title>Switch Off FM</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Switching Off FM</term>
+         <listitem>
+         <para>
+           For switching OFF FM the character device /dev/radio0 should be closed from user space.
+           <programlisting>
+		if(fd &gt;= 0)
+			close(fd);
+           </programlisting>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+  <section id="Rx-Mode">
+     <title>Switching To FM Rx Mode</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Switching To FM Rx Mode</term>
+         <listitem>
+         <para>
+           For switching on FM Rx mode the IOCTL VIDIOC_S_TUNER should be called with appropriate parameters.
+           <programlisting>
+			memset(&amp;tuner, 0, sizeof(tuner));
+			tuner.index = 0;
+			tuner.rxsubchans |= V4L2_TUNER_SUB_STEREO;
+			if (ioctl(fd, VIDIOC_S_TUNER, &amp;tuner) &lt; 0) {
+				  printf("VIDIOC_S_TUNER:error!!\n");
+				  return;
+           </programlisting>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="Tx-Mode">
+     <title>Switching To FM Tx Mode</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Switching To FM Tx Mode</term>
+         <listitem>
+         <para>
+           For switching on FM Tx mode the IOCTL VIDIOC_S_MODULATOR should be
+		   called with appropriate parameters.
+           <programlisting>
+			memset(&amp;modulator, 0, sizeof(modulator));
+			modulator.index = 0;
+			modulator.txsubchans |= V4L2_TUNER_SUB_STEREO;
+			if (ioctl(fd, VIDIOC_S_MODULATOR, &amp;modulator) &lt; 0) {
+				  printf("VIDIOC_S_MODULATOR:error!!\n");
+				  return;
+           </programlisting>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="FM-Standby">
+     <title>Standby</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Making the FM Radio go in Standby Mode</term>
+         <listitem>
+         <para>
+           For making the FM Radio go in Standby mode, the IOCTL VIDIOC_S_CTRL should be used. The id of the v4l2_control structure should be set to V4L2_CID_CG2900_RADIO_CHIP_STATE and the value of v4l2_control structure should be set as V4L2_CG2900_RADIO_STANDBY.
+           <programlisting>
+		struct v4l2_control sctrl;
+		int ret;
+		sctrl.id = V4L2_CID_CG2900_RADIO_CHIP_STATE;
+                sctrl.value = V4L2_CG2900_RADIO_STANDBY;
+                ret = ioctl(fd, VIDIOC_S_CTRL, &amp;sctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_CTRL:error!!\n");
+		}
+           </programlisting>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="Powerup-from-standby">
+     <title>Powering Up FM From Standby Mode</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Powering Up FM Radio from Standby Mode</term>
+         <listitem>
+         <para>
+           For powering up FM radio again from standby mode, the IOCTL VIDIOC_S_CTRL should be used. The id of the v4l2_control structure should be set to V4L2_CID_CG2900_RADIO_CHIP_STATE and the value of v4l2_control structure should be set as V4L2_CG2900_RADIO_POWERUP.
+           <programlisting>
+		struct v4l2_control sctrl;
+		int ret;
+		sctrl.id = V4L2_CID_CG2900_RADIO_CHIP_STATE;
+                sctrl.value = V4L2_CG2900_RADIO_POWERUP;
+                ret = ioctl(fd, VIDIOC_S_CTRL, &amp;sctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_CTRL:error!!\n");
+		}
+           </programlisting>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="tune-frequency">
+     <title>Tune Channel</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Tune to a particular station</term>
+         <listitem>
+         <para>
+           for tuning to a particular station, the IOCTL VIDIOC_S_FREQUENCY should be used. The frequency of the v4l2_frequency structure should be converted to V4L2 format.
+           <programlisting>
+		struct v4l2_frequency freq;
+		int ret;
+		/* Convert frequency in Hz to V4L2 Format */
+		freq.frequency = (frequency * 2)/ 125;
+                ret = ioctl(fd, VIDIOC_S_FREQUENCY, &amp;freq);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_FREQUENCY:error!!\n");
+		}
+           </programlisting>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="get-frequency">
+     <title>Get Tuned Channel</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Get the Currently tuned Station Frequncy</term>
+         <listitem>
+         <para>
+           for tuning to a particular station, the IOCTL VIDIOC_G_FREQUENCY should be used. The frequency returned in the v4l2_frequency structure would be in V4L2 format.
+           <programlisting>
+		struct v4l2_frequency freq;
+		int ret;
+                ret = ioctl(fd, VIDIOC_G_FREQUENCY, &amp;freq);
+		if (ret &lt; 0) {
+			printf("VIDIOC_G_FREQUENCY:error!!\n");
+			*frequency = 0;
+			return;
+		}
+		/* Convert frequency to Hz from V4L2 Format */
+		*frequency = (freq.frequency * 125)/2;
+           </programlisting>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="get-signal-strength">
+     <title>Retreive Signal Strength</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Retreive Signal Strength</term>
+         <listitem>
+         <para>
+           For retreiving the Signal strength of the currently tuned channel in FM Rx mode, IOCTL VIDIOC_G_TUNER should be called. The current signal strength would be represented by the parameter signal of the v4l2_tuner strucure.
+           <programlisting>
+		void get_signal_strength(int *rssi)
+		{
+			struct v4l2_tuner tuner;
+			int ret;
+			memset(&amp;tuner, 0, sizeof(tuner));
+			tuner.index = 0;
+			ret = ioctl(fd, VIDIOC_G_TUNER, &amp;tuner);
+			if (ret &lt; 0) {
+				printf("VIDIOC_G_TUNER:error!!\n");
+				*rssi = 0;
+				return;
+			}
+			*rssi = tuner.signal;
+		}
+           </programlisting>
+Note: Currently the retrieved signal strength is in decimals and not in "dBuV", proper external conversion required.
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="band-scan">
+     <title>Band Scan</title>
+     <para>
+       <!-- Driver loading Parameters:Not Applicable -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Band Scan</term>
+         <listitem>
+         <para>
+           For doing a band scan, ie search for all available stations in the entire FM band, IOCTL VIDIOC_S_CTRL should be used with parameter id of the v4l2_control structure should be set to V4L2_CID_CG2900_RADIO_BANDSCAN and the value of v4l2_control structure should be set as V4L2_CG2900_RADIO_BANDSCAN_START. If the IOCTL returns successfully, a thread should be created to start polling to FM driver, to wait till scan is complete. When poll is complete, the found stations along with RSSI should be retrieved using the IOCTL VIDIOC_G_EXT_CTRLS should be used with parameters as described in example code.
+           <programlisting>
+			void Band_Scan()
+		{
+			struct v4l2_control sctrl
+			int ret;
+			sctrl.id = V4L2_CID_CG2900_RADIO_BANDSCAN;
+			sctrl.value = V4L2_CG2900_RADIO_BANDSCAN_START;
+			ret = ioctl(fd, VIDIOC_S_CTRL, &amp;sctrl);
+			if (ret &lt; 0) {
+				printf("VIDIOC_S_CTRL:error!!\n");
+			}
+			pthread_create(&amp;fmScanThread, NULL, FmScanThread, NULL);
+		}
+		static void *FmScanThread(void *param)
+		{
+			struct v4l2_ext_controls scanResult;
+			struct pollfd pollFd;
+			long * p = NULL;
+			int index, ret, count = 0;
+			int err;
+			pollFd.fd = fd;
+			pollFd.events = POLLRDNORM;
+			ret = poll(&amp;pollFd, 1, MAX_SCAN_SEEK_TIME);
+			if(ret)
+			{
+				if(pollFd.revents &amp; POLLRDNORM)
+				{
+					/* Get the Number OF Channels */
+					scanResult.count = 0;
+					scanResult.ctrl_class = V4L2_CTRL_CLASS_USER;
+					scanResult.controls = (struct v4l2_ext_control *) malloc(sizeof(struct v4l2_ext_control));
+					scanResult.controls->id = V4L2_CID_CG2900_RADIO_BANDSCAN_GET_RESULTS;
+					scanResult.controls->size = 0;
+					scanResult.controls->string = NULL;
+					err = ioctl(fd, VIDIOC_G_EXT_CTRLS, &amp; scanResult);
+				if (err &lt; 0 &amp; &amp; errno !=  ENOSPC) {
+					printf("VIDIOC_G_EXT_CTRLS:error!!\n");
+					goto err;
+				}
+				/* Allocate memory to receive the data from driver */
+				if(scanResult.controls->size &gt; 0 )
+				{
+					scanResult.controls->string = (long *)malloc(sizeof(long) * 2 * scanResult.controls->size );
+					p = scanResult.controls->string;
+					printf("\nNumber of Channels Found = %d \n", scanResult.controls->size);
+					/* Retrieve the Data now */
+					ret = ioctl(fd, VIDIOC_G_EXT_CTRLS, &amp;scanResult);
+					if (ret &lt; 0) {
+						printf("VIDIOC_G_EXT_CTRLS:error!!\n");
+						goto err;
+					}
+					for (index = 0, count = 0; index &lt; scanResult.controls->size; index ++, count +=2) {
+						printf("%d	 %d.%d		%d\n", index + 1, MEGAHRTZ((*(p +count + 0) * 125) / 2), *(p + count + 1));
+					}
+					err:
+					free(p);
+					free(scanResult.controls);
+				}
+				else if( pollFd.revents &amp; POLLHUP)
+				{
+					printf("\nScan Cancelled By User!!\n");
+				}
+				free(scanResult.controls);
+			}
+			else if( ret == 0){
+				printf("\nError in Scanning, Timeout!!!\n");
+			}
+			return 0;
+		}
+           </programlisting>
+           Note: Currently the retrieved signal strength is in decimals and not in "dBuV", proper external conversion required.
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+    <section id="block-scan">
+     <title>Block Scan</title>
+     <para>
+       <!-- Driver loading Parameters:Not Applicable -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Block Scan</term>
+         <listitem>
+         <para>
+           The Block Scan functionality will take two inputs, start and stop frequency (V4L2 compliance) and enables the host to scan all channels with in that range for RSSI values. The measured channels will be stored in a list in order of channel number and the  block scan feature to identify "empty" channels for transmission. And for doing a block scan, IOCTL VIDIOC_S_EXT_CTRL with parameter id of the v4l2_control structure should be set to V4L2_CID_FM_RADIO_BLOCKSCAN_START. If the IOCTL returns successfully, a thread should be created to start polling to FM driver, to wait till scan is complete. When poll is complete, the found stations along with RSSI should be retrieved using the IOCTL VIDIOC_G_EXT_CTRLS should be used with parameters as described in example code.
+           <programlisting>
+		void Block_Scan()
+		{
+			struct v4l2_ext_controls ext_ctrl;
+			long *p = NULL;
+		        int index;
+		        int ret_val;
+			if(1 == mode) {
+		        otherOperationInProgress = 1;
+			ext_ctrl.ctrl_class = V4L2_CTRL_CLASS_USER;
+			ext_ctrl.controls = (struct v4l2_ext_control *) malloc(sizeof(struct v4l2_ext_control));
+			ext_ctrl.count = 0;
+			ext_ctrl.controls->id = V4L2_CID_CG2900_RADIO_BLOCKSCAN_START;
+			ext_ctrl.controls->size = 2;
+			ext_ctrl.controls->string = (long *)malloc(sizeof(long) * ext_ctrl.controls->size);
+			p = ext_ctrl.controls->string;
+			*p = (StartFreq * 2)/ 125;
+			*(p + 1) = (EndFreq * 2)/ 125;;
+			if (ioctl(fd, VIDIOC_S_EXT_CTRLS, &amp;ext_ctrl) &lt; 0)
+				printf("APP_BlockScanStart:VIDIOC_S_EXT_CTRLS:error!!\n");
+			free(ext_ctrl.controls->string);
+			free(ext_ctrl.controls);
+			pthread_create(&amp;fmBlockScanThread, NULL, FmBlockScanThread, NULL);
+		}
+		static void *FmBlockScanThread(void *param)
+		{
+			struct v4l2_ext_controls blockscanResult;
+			struct pollfd pollFd;
+			long * p = NULL;
+			int index, ret;
+			int err;
+			int current_grid = -1;
+			FILE *fp;
+			long start_freq = StartFreq;
+			long next_freq_offset = 0;
+			fp = fopen("/sys/module/radio_cg2900/parameters/grid", "r");
+			if(fp != NULL)
+			{
+				/*
+				 * Retrieve the currently set grid to determine the next channel is
+				 * 50 Khz, 100 Khz or 200 Khz apart
+				 */
+				fscanf(fp, "%d", &amp;current_grid);
+				fclose(fp);
+			}
+			if(current_grid == 0) {
+				next_freq_offset = 50000;
+			} else if	(current_grid == 1) {
+				next_freq_offset = 100000;
+			} else if (current_grid == 2) {
+				next_freq_offset = 200000;
+			}
+			pollFd.fd = fd;
+			pollFd.events = POLLRDNORM;
+			ret = poll(&amp;pollFd, 1, MAX_SCAN_SEEK_TIME);
+			if(ret) {
+			        if(pollFd.revents &amp; POLLRDNORM)
+			        {
+				        /* Get the Number Of Channels */
+				        blockscanResult.count = 0;
+				        blockscanResult.ctrl_class = V4L2_CTRL_CLASS_USER;
+				        blockscanResult.controls = (struct v4l2_ext_control *) malloc(sizeof(struct v4l2_ext_control));
+				        blockscanResult.controls->id = V4L2_CID_CG2900_RADIO_BLOCKSCAN_GET_RESULTS;
+				        blockscanResult.controls->size = 0;
+				        blockscanResult.controls->string = NULL;
+				        err = ioctl(fd, VIDIOC_G_EXT_CTRLS, &amp;blockscanResult);
+					if (err &lt; 0 &amp;&amp; errno !=  ENOSPC) {
+						printf("VIDIOC_G_EXT_CTRLS:error!!\n");
+						goto err;
+					}
+			if(blockscanResult.controls->size > 0 )
+			{
+				blockscanResult.controls->string = (long *)malloc(sizeof(long) * blockscanResult.controls->size );
+				p = blockscanResult.controls->string;
+			        if (ioctl(fd, VIDIOC_G_EXT_CTRLS, &amp;blockscanResult) &lt; 0) {
+			                printf("VIDIOC_G_EXT_CTRLS:error!!\n");
+			                goto err;
+			        }
+			        printf("\nSNo. 	RSSI\n");
+				for (index = 0; index &lt; blockscanResult.controls->size; index ++) {
+					printf("%d.%d   	%d\n", MEGAHRTZ(start_freq), *(p + index));
+					start_freq += next_freq_offset;
+			        }
+				err:
+				    free(p);
+			        }
+				else if(blockscanResult.controls->size == 0)
+				{
+				      printf("\nNo channels found during Block Scan!!\n");
+				}
+				free(blockscanResult.controls);
+			}
+		        else if( pollFd.revents &amp; POLLHUP)
+		        {
+		                printf("\nBlock Scan Cancelled By User!!\n");
+			}
+		  }
+		  else if(ret == 0) {
+			printf("\nError in Block Scan, Timeout!!!\n");
+		  }
+		  otherOperationInProgress = 0;
+		  return 0;
+	   }
+
+           </programlisting>
+           Note: Currently the retrieved signal strength is in decimals and not in "dBuV", proper external conversion required.
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="cancel-scan-seek">
+     <title>Cancel Scan</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Cancel Scan/Seek</term>
+         <listitem>
+         <para>
+           This is used for stopping an active Band Scan, Seek operation and Block Scan. IOCTL VIDIOC_S_CTRL should be used with parameter id of the v4l2_control structure. For Eg incase of Band scan the parameter id should be set to V4L2_CID_CG2900_RADIO_BANDSCAN and the value of v4l2_control structure should be set as V4L2_CG2900_RADIO_BANDSCAN_STOP.
+           <programlisting>
+		struct v4l2_control sctrl;
+		int ret;
+		sctrl.id = V4L2_CID_CG2900_RADIO_BANDSCAN;
+		sctrl.value = V4L2_CG2900_RADIO_BANDSCAN_STOP;
+		ret = ioctl(fd, VIDIOC_S_CTRL, &amp;sctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_CTRL:error!!\n");
+		}
+           </programlisting>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="rds-receive">
+     <title>RDS Receive</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>RDS Receive</term>
+         <listitem>
+         <para>
+           For enabling/disabling RDS for FM Rx, IOCTL VIDIOC_S_TUNER should be used with parameter rxsubchans of the v4l2_tuner structure set to V4L2_TUNER_SUB_RDS if rds needs to be enabled and the same value must not be set in case rds is to be disabled. A thread should be created and read() should be called to receive RDS data from driver. The RDS data received from FM Driver should be parsed in user space to retrive RDS information i.e Radio Text, Program Service Name, Program Identification, Program Type, Alternate Frequency, etc.
+           <programlisting>
+           void rds_rx_set(bool enable_rds)
+           {
+		struct v4l2_tuner tuner;
+		int ret;
+		memset(&amp;tuner, 0, sizeof(tuner));
+		tuner.index = 0;
+		if(enable_rds)
+			tuner.rxsubchans |= V4L2_TUNER_SUB_RDS;
+		else
+			tuner.rxsubchans &amp; = ~V4L2_TUNER_SUB_RDS;
+		ret = ioctl(fd, VIDIOC_S_TUNER, &amp;tuner);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_TUNER:error!!\n");
+		}
+	   }
+           </programlisting>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="af-update_switch">
+     <title>AF Update and Switching</title>
+     <para>
+      <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>AF Update &amp; Switching</term>
+         <listitem>
+	 <para>Alternate Frequency (AF) Handling needs to be done in user space.</para>
+	 <para>The application should use the AF RDS group data to compose a list of AFs when tuned to a new channel.</para>
+	 <para>When the reception of the currently tuned frequency falls below a set threshold, it can decide to switch to one of the alternative frequencies for this channel.
+	 </para>
+	 <para>The application can perform an AF Update, which returns the RSSI value for all or some of the channel's AFs. Thus allowing the hardware to switch to the AF with the highest RSSI.  The AF Update could be designed to stop as soon as it finds an AF with an acceptable RSSI level. In the event that all the AF RSSI values are lower than the base channel, the AF Switch would not be necessary.
+	 </para>
+	 <para>To know the RSSI of the alternative frequencies (V4L2 compliance), the application can use the IOCTL VIDIOC_S_CTRL with parameter id set to V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_START, and the parameter value be set as the frequency in Hz for a channel from the AF List. If this call returns successfully,  the RSSI of the frequency can then be retrieved. Using IOCTL VIDIOC_G_CTRL, with the parameter id set to V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_GET_RESULT, and the output parameter value will contain the RSSI of the AF frequency.
+	 </para>
+	 <para>If it is still deemed necessary to switch channels, the next step is then to switch to an alternative frequency in the AF list. This can be done using the IOCTL VIDIOC_S_EXT_CTRLS, with:
+	 </para>
+	  <itemizedlist>
+	   <listitem><para>Parameter id set to V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START</para></listitem>
+	   <listitem><para>Parameter size set to 2</para></listitem>
+	   <listitem><para>Parameters filled as below (string field of the parameter) </para></listitem>
+	   <listitem><para>Control class parameter set to V4L2_CTRL_CLASS_USER</para></listitem>
+	   <listitem><para>The AF switch frequency in Hz</para></listitem>
+	   <listitem><para>Expected PI code </para></listitem>
+           </itemizedlist>
+         <para>The application can check if the AF switch succeeded or not using the IOCTL VIDIOC_G_CTRL, with parameter id set to V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_GET_RESULT, and the output parameter value will contain the AF switch conclusion.
+         </para>
+	 <para> The example code below illustrates both the aforementioned functionalities.</para>
+	<para>
+	   <programlisting>
+           void PerformAFUpdate(long AF_Frequency, int *AF_Rssi)
+           {
+		struct v4l2_control sctrl, gctrl;
+		int ret;
+		sctrl.id = V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_START;
+		sctrl.value = AF_Frequency;
+		ret = ioctl(fd, VIDIOC_S_CTRL, &amp; sctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_CTRL:error!!\n");
+		}
+		gctrl.id = V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_GET_RESULT;
+		ret = ioctl(fd, VIDIOC_G_CTRL, &amp; gctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_G_CTRL:error!!\n");
+		}
+		*AF_Rssi = gctrl.value;
+	   }
+           void PerformAFSwitch(long AF_BestFrequency, int AF_ExpectedPI, int *AF_SwitchConclusion)
+           {
+		struct v4l2_control gctrl;
+		struct v4l2_ext_controls ext_ctrl;
+		int ret;
+		int conclusion;
+		long freq;
+		long *p = NULL;
+		ext_ctrl.ctrl_class = V4L2_CTRL_CLASS_USER;
+		ext_ctrl.controls = (struct v4l2_ext_control *) malloc(sizeof(struct v4l2_ext_control));
+		ext_ctrl.count = 0;
+		ext_ctrl.controls->id = V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START;
+		ext_ctrl.controls->size = 2;
+		ext_ctrl.controls->string = (long *)malloc(sizeof(long) * ext_ctrl.controls->size);
+		p = ext_ctrl.controls->string;
+		*p = (AF_BestFrequency * 2)/ 125;
+		*(p+1) = (long)AF_ExpectedPI;
+		ret = ioctl(fd, VIDIOC_S_EXT_CTRLS, &amp; ext_ctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_EXT_CTRLS:error!!\n");
+		}
+		free(ext_ctrl.controls->string);
+		free(ext_ctrl.controls);
+		gctrl.id = V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_GET_RESULT;
+		ret = ioctl(fd, VIDIOC_G_CTRL, &amp; gctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_G_CTRL:error!!\n");
+		}
+		*AF_SwitchConclusion = gctrl.value;
+	   }
+           </programlisting>
+              Note: For V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_GET_RESULT returned values are:
+               <itemizedlist>
+                 <listitem><para> -1 AF Switch failed, the AF-RSSI was too low.</para></listitem>
+                 <listitem><para> -2 AF Switch failed, the AF-PI Doesn't correlate.</para></listitem>
+                 <listitem><para> -3 AF Switch failed, the AF-RDS SYNC Lost.</para></listitem>
+               </itemizedlist>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="rds-transmit">
+     <title>RDS Transmit</title>
+     <para>
+      <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>RDS Transmit</term>
+         <listitem>
+         <para>
+           For enabling/disabling RDS for FM Tx, IOCTL VIDIOC_S_MODULATOR should be used with parameter txsubchans of the v4l2_modulator structure set to V4L2_TUNER_SUB_RDS if rds needs to be enabled and the same value must not be set in case rds is to be disabled. For Trasmitting RDS Data like PI, PTY, PSN, RT, VIDIOC_S_EXT_CTRLS IOCTL should be used with the id set to V4L2_CID_RDS_TX_PI, V4L2_CID_RDS_TX_PTY, V4L2_CID_RDS_TX_PS_NAME and V4L2_CID_RDS_TX_RADIO_TEXT respectively. Below example shows how to transmit various RDS functionalities.
+           <programlisting>
+           void rds_tx_set(bool enable_rds)
+           {
+		struct v4l2_modulator modulator;
+		int ret;
+		memset(&amp;modulator, 0, sizeof(modulator));
+		modulator.index = 0;
+		if(enable_rds)
+			modulator.txsubchans |= V4L2_TUNER_SUB_RDS;
+		else
+			modulator.txsubchans &amp; = ~V4L2_TUNER_SUB_RDS;
+		ret = ioctl(fd, VIDIOC_S_MODULATOR, &amp; modulator);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_MODULATOR:error!!\n");
+		}
+	   }
+           void rds_tx_PI(void *value)
+           {
+		struct v4l2_ext_controls ext_ctrl;
+		int ret;
+		unsigned short *pi_code = (unsigned short *)value;
+		ext_ctrl.ctrl_class = V4L2_CTRL_CLASS_FM_TX;
+		ext_ctrl.controls = (struct v4l2_ext_control *) malloc(sizeof(struct v4l2_ext_control));
+		ext_ctrl.count = 0;
+		ext_ctrl.controls->id = V4L2_CID_RDS_TX_PI;
+		ext_ctrl.controls->size = 0;
+		ext_ctrl.controls->string = NULL;
+		ext_ctrl.controls->value = *pi_code;
+		ret = ioctl(fd, VIDIOC_S_EXT_CTRLS, &amp; ext_ctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_EXT_CTRLS:error!!\n");
+		}
+		free(ext_ctrl.controls);
+	   }
+           void rds_tx_PTY(void *value)
+           {
+		struct v4l2_ext_controls ext_ctrl;
+		int ret;
+		unsigned short *pty_code = (unsigned short *)value;
+		ext_ctrl.ctrl_class = V4L2_CTRL_CLASS_FM_TX;
+		ext_ctrl.controls = (struct v4l2_ext_control *) malloc(sizeof(struct v4l2_ext_control));
+		ext_ctrl.count = 0;
+		ext_ctrl.controls->id = V4L2_CID_RDS_TX_PTY;
+		ext_ctrl.controls->size = 0;
+		ext_ctrl.controls->string = NULL;
+		ext_ctrl.controls->value = *pty_code;
+		ret = ioctl(fd, VIDIOC_S_EXT_CTRLS, &amp; ext_ctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_EXT_CTRLS:error!!\n");
+		}
+		free(ext_ctrl.controls);
+	   }
+           void rds_tx_PSN(void *value)
+           {
+		struct v4l2_ext_controls ext_ctrl;
+		int ret;
+		char *psn = (char *)value;
+		ext_ctrl.ctrl_class = V4L2_CTRL_CLASS_FM_TX;
+		ext_ctrl.controls = (struct v4l2_ext_control *) malloc(sizeof(struct v4l2_ext_control));
+		ext_ctrl.count = 0;
+		ext_ctrl.controls->id = V4L2_CID_RDS_TX_PS_NAME;
+		ext_ctrl.controls->size = strlen(psn);
+		ext_ctrl.controls->value = 0;
+		ext_ctrl.controls->string = (char *)malloc(ext_ctrl.controls->size);
+		memcpy(ext_ctrl.controls->string, psn, ext_ctrl.controls->size);
+		ret = ioctl(fd, VIDIOC_S_EXT_CTRLS, &amp; ext_ctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_EXT_CTRLS:error!!\n");
+		}
+		free(ext_ctrl.controls->string);
+		free(ext_ctrl.controls);
+	   }
+           void rds_tx_RT(void *value)
+           {
+		struct v4l2_ext_controls ext_ctrl;
+		int ret;
+		char *radio_text = (char *)value;
+		ext_ctrl.ctrl_class = V4L2_CTRL_CLASS_FM_TX;
+		ext_ctrl.controls = (struct v4l2_ext_control *) malloc(sizeof(struct v4l2_ext_control));
+		ext_ctrl.count = 0;
+		ext_ctrl.controls->id = V4L2_CID_RDS_TX_RADIO_TEXT;
+		ext_ctrl.controls->size = strlen(radio_text);
+		ext_ctrl.controls->value = 0;
+		ext_ctrl.controls->string = (char *)malloc(ext_ctrl.controls->size);
+		memcpy(ext_ctrl.controls->string, radio_text, ext_ctrl.controls->size);
+		ret = ioctl(fd, VIDIOC_S_EXT_CTRLS, &amp; ext_ctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_EXT_CTRLS:error!!\n");
+		}
+		free(ext_ctrl.controls->string);
+		free(ext_ctrl.controls);
+	   }
+	   </programlisting>
+		 Note: RDS default parameters
+                <itemizedlist>
+                   <listitem><para>Programme Identification code[PI]: Default value -> 0x1234</para></listitem>
+                   <listitem><para>Programme Type[PTY]: Default value -> OTHER_MUSIC</para></listitem>
+                   <listitem><para>Music/Speech switch[M/S]: Default value -> Music</para></listitem>
+                   <listitem><para>Programme Service name[PS]: Default value -> FM Xmit</para></listitem>
+                   <listitem><para>Radio text[RT]: Default value -> Default Radio Text</para></listitem>
+                 </itemizedlist>
+        </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="internal-test-tone-generator">
+     <title>Test Tone Generation</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>1. Enabling-Disabling test tone</term>
+         <listitem>
+         <para>
+         1. For setting the test tone status, application should use IOCTL VIDIOC_S_CTRL with its parameter id set to V4L2_CID_CG2900_RADIO_TEST_TONE_GENERATOR_SET_STATUS and parameter value set to any of the following :
+	  <itemizedlist>
+	   <listitem><para>Turn off test tone - use V4L2_CG2900_RADIO_TEST_TONE_GEN_OFF</para></listitem>
+	   <listitem><para>Turn on test tone - use  V4L2_CG2900_RADIO_TEST_TONE_GEN_ON_WO_SRC</para></listitem>
+	   <listitem><para>Turn on with sample rate correction - use V4L2_CG2900_RADIO_TEST_TONE_GEN_ON_W_SRC</para></listitem>
+           </itemizedlist>
+           <programlisting>
+           void SetTestToneStatus(int state)
+           {
+		struct v4l2_control sctrl;
+		int ret;
+		sctrl.id = V4L2_CID_CG2900_RADIO_TEST_TONE_GENERATOR_SET_STATUS;
+		sctrl.value = state;
+		ret = ioctl(fd, VIDIOC_S_CTRL, &amp; sctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_CTRL:error!!\n");
+		}
+	   }
+           </programlisting>
+         </para>
+         </listitem>
+         </varlistentry>
+
+         <varlistentry>
+	 <term>2. Test Tone Connect</term>
+         <listitem>
+         <para>
+	 2. The internal test tone can be used following modes: FMR (receiver mode) and FMT (transmitter mode). Each of the waves, or the sum of them, can be used as audio source for the receiver audio outputs, or for the transmitter audio input. This can be done by means of command "Test Tone Connect". For the receiver, all available audio outputs will be connected to the tone generator. After switching FM to another mode, the command "Test Tone Connect" must be executed again to set up a connection in the new mode.
+	 </para>
+
+         <para>
+         The IOCTL VIDIOC_S_EXT_CTRLS is used to perform Test Tone Connect operation, with following parameters:
+         </para>
+           <itemizedlist>
+	   <listitem><para>Parameter id set to V4L2_CID_CG2900_RADIO_TEST_TONE_CONNECT</para></listitem>
+	   <listitem><para>Parameter size set to 2</para></listitem>
+	   <listitem><para>Control class parameter set to V4L2_CTRL_CLASS_USER</para></listitem>
+	   <listitem><para>Parameters value filled as below in code snippet(string field of the parameter) </para></listitem>
+	   <listitem><para>First byte of Parameter value shall contain connect parameter for left audio output </para></listitem>
+	   <listitem><para>Second byte of Parameter value shall contain connect parameter forright audio output </para></listitem>
+           </itemizedlist>
+
+         <para>
+         Value of either of Parameter values (for left or right audio outputs)can assume values:
+         </para>
+	   <itemizedlist>
+	   <listitem><para>V4L2_CG2900_RADIO_TEST_TONE_NORMAL_AUDIO - Normal Audio</para></listitem>
+	   <listitem><para>V4L2_CG2900_RADIO_TEST_TONE_ZERO - Zero</para></listitem>
+	   <listitem><para>V4L2_CG2900_RADIO_TEST_TONE_TONE_1 - Tone_1</para></listitem>
+	   <listitem><para>V4L2_CG2900_RADIO_TEST_TONE_TONE_2 - Tone_2</para></listitem>
+	   <listitem><para>V4L2_CG2900_RADIO_TEST_TONE_TONE_SUM - Tone_Sum</para></listitem>
+           </itemizedlist>
+
+           <programlisting>
+           void TestToneConnect(u8 left_audio_mode, u8 right_audio_mode)
+           {
+		u8 *test_tone_connect_ptr = NULL;
+		ext_ctrl.ctrl_class = V4L2_CTRL_CLASS_USER;
+		ext_ctrl.controls = (struct v4l2_ext_control *) malloc(sizeof(struct v4l2_ext_control));
+		ext_ctrl.count = 0;
+		ext_ctrl.controls-&gt;id = V4L2_CID_CG2900_RADIO_TEST_TONE_CONNECT;
+		ext_ctrl.controls-&gt;size = 2;
+		ext_ctrl.controls-&gt;string = (u8 *)malloc(sizeof(u8) * ext_ctrl.controls-&gt;size);
+		test_tone_connect_ptr = ext_ctrl.controls-&gt;string;
+		*(test_tone_connect_ptr) = left_audio_mode;
+		*(test_tone_connect_ptr + 1) = right_audio_mode;
+
+		ret = ioctl(fd, VIDIOC_S_EXT_CTRLS, &amp; ext_ctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_EXT_CTRL:error!!\n");
+		}
+	   }
+           </programlisting>
+
+         </listitem>
+         </varlistentry>
+
+         <varlistentry>
+         <term>3. Test Tone Set Parameters</term>
+         <listitem>
+         <para>
+         3. The tone generator is capable of generating two different sine waves with adjustable offset, amplitude, phase offset and frequency. These properties can be changed with command "Test Tone Set Params"
+	 </para>
+
+         <para>
+         The IOCTL VIDIOC_S_EXT_CTRLS is used to perform Test Tone Set Parameters operation, with following parameters:
+         </para>
+           <itemizedlist>
+	   <listitem><para>Parameter id set to V4L2_CID_CG2900_RADIO_TEST_TONE_SET_PARAMS</para></listitem>
+	   <listitem><para>Parameter size set to 6</para></listitem>
+	   <listitem><para>Control class parameter set to V4L2_CTRL_CLASS_USER</para></listitem>
+	   <listitem><para>Parameters value filled as below in code snippet(string field of the parameter) </para></listitem>
+	   <listitem><para>First word of Parameter value shall contain tone_gen (0: tone_1, 1:tone_2)</para></listitem>
+	   <listitem><para>Second word of Parameter value shall contain frequency ([0x0000..0x7FFF], (default = 0x064D))</para></listitem>
+	   <listitem><para>Third word of Parameter value shall contain volume ([0x0000..0x7FFF], (default = 0x0CCD))</para></listitem>
+	   <listitem><para>Fourth word of Parameter value shall contain phase offset([0x8000..0x7FFF], (default = 0x0000))</para></listitem>
+	   <listitem><para>Fifth word of Parameter value shall contain DC to add to tone([0x8000..0x7FFF], (default = 0x0000))</para></listitem>
+	   <listitem><para>Sixth word of Parameter value shall contain waveform type(0=sine shaped, 1=Pulse shaped)</para></listitem>
+           </itemizedlist>
+
+           <programlisting>
+           void Sample_TestToneSetParams()
+           {
+		u8 *test_tone_connect_ptr = NULL;
+                u8 tone_gen =0, waveform = 1;     /* Tone_Gen = Tone_1, waveform type = pulse shaped */
+                u16 frequency = 0x064D, volume = 0x0CCD, phase_offset = 0x0000, dc=0x0000;
+
+		ext_ctrl.ctrl_class = V4L2_CTRL_CLASS_USER;
+		ext_ctrl.controls = (struct v4l2_ext_control *) malloc(sizeof(struct v4l2_ext_control));
+		ext_ctrl.count = 0;
+		ext_ctrl.controls-&gt;id = V4L2_CID_CG2900_RADIO_TEST_TONE_CONNECT;
+		ext_ctrl.controls-&gt;size = 6;
+		ext_ctrl.controls-&gt;string = (u16 *)malloc(sizeof(u16) * ext_ctrl.controls-&gt;size);
+		test_tone_connect_ptr = ext_ctrl.controls-&gt;string;
+		*(test_tone_connect_ptr) = tone_gen;
+		*(test_tone_connect_ptr + 1) = frequency;
+		*(test_tone_connect_ptr + 2) = volume;
+		*(test_tone_connect_ptr + 3) = phase_offset;
+		*(test_tone_connect_ptr + 4) = dc;
+		*(test_tone_connect_ptr + 5) = waveform;
+
+		ret = ioctl(fd, VIDIOC_S_EXT_CTRLS, &amp; ext_ctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_EXT_CTRL:error!!\n");
+		}
+	}
+           </programlisting>
+
+         </listitem>
+
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="de-emphasis-filter">
+     <title>Test Tone Generation</title>
+     <para>
+        <!-- Do NOT change the chapter id or title! -->
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term>Set De-Emphasis Filter</term>
+         <listitem>
+         <para>
+         To apply de-emphasis filter to the FM received signal to compansate for
+         preemphasis that has been applied to the signal by the FM transmitter. IOCTL VIDIOC_S_CTRL is used in with parameter id set to V4L2_CID_CG2900_RADIO_TUNE_DEEMPHASIS and parameter value can take following values:
+	 <itemizedlist>
+	   <listitem><para>Disable de-emphasis - use V4L2_CG2900_RADIO_DEEMPHASIS_DISABLED</para></listitem>
+	   <listitem><para>De-emphasis with 50 micro seconds - use  V4L2_CG2900_RADIO_DEEMPHASIS_50_uS</para></listitem>
+	   <listitem><para>De-emphasis filter with 75 micro seconds - use V4L2_CG2900_RADIO_DEEMPHASIS_75_uS</para></listitem>
+         </itemizedlist>
+
+           <programlisting>
+           void SetDeemphasisLevel(int deemphasis_level)
+           {
+		struct v4l2_control sctrl;
+		int ret;
+		sctrl.id = V4L2_CID_CG2900_RADIO_TUNE_DEEMPHASIS ;
+		sctrl.value = deemphasis_level;
+		ret = ioctl(fd, VIDIOC_S_CTRL, &amp; sctrl);
+		if (ret &lt; 0) {
+			printf("VIDIOC_S_CTRL:error!!\n");
+		}
+	   }
+           </programlisting>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+
+
+ </chapter>
+ <chapter id="driver-configuration">
+   <title>Driver Configuration and Interaction</title>
+   <!-- Do NOT change the chapter id or title! -->
+   <para>
+     For debug purposes the variable cg2900_fm_debug_level in the file cg2900_fm_driver.c can be changed to set how much debug printouts
+     that shall be generated.
+     <itemizedlist>
+       <listitem><para>1 = Error logs</para></listitem>
+       <listitem><para>2 = Info logs, e.g. function entries</para></listitem>
+       <listitem><para>3 = Debug logs</para></listitem>
+       <listitem><para>4 = HCI logs, i.e. contents of the transferred data</para></listitem>
+     </itemizedlist>
+       </para>
+   <section id="driver-implemented-operations">
+     <title>Implemented operations in driver</title>
+     <para>
+       </para>
+     <para>
+       <table>
+         <title> Supported device driver operations when using character device </title>
+         <tgroup cols="2"><tbody>
+           <row><entry> open </entry> <entry> Opening a character device will Initialize the FM Chip and download the firmware files.</entry> </row>
+           <row><entry> close </entry> <entry> Closes a character device will deinitialize the FM Chip.</entry> </row>
+           <row><entry> poll </entry> <entry> Polling a character device will check if there is requested data is available or not.</entry> </row>
+           <row><entry> read </entry> <entry> Reading from a character device reads RDS data from the Chip</entry> </row>
+         </tbody></tgroup>
+       </table>
+     </para>
+   </section>
+   <section id="driver-loading">
+     <title>Driver loading parameters</title>
+     <para>
+      </para>
+     <variablelist>
+       <varlistentry>
+         <term>radio_nr</term>
+         <listitem>
+         <para>
+           <variablelist>
+             <varlistentry>
+               <term>Parameter type</term>
+               <listitem><synopsis><type>int</type></synopsis></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Default value</term>
+               <listitem><para>0</para></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Runtime readable/modifiable</term>
+               <listitem><para>Readable</para></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Description</term>
+               <listitem>
+               <para>
+                 The parameter radio_nr in radio-cg2900.c can be set to register a particular minor number with Video4Linux. Currently this parameter is set to 0 by default, signifying that the "\dev\radio0" is the character device assigned to CG2900 FM Driver in Video4Linux.
+		 If the Platform has more than 1 radio drivers, the radio_nr parameter should be changed in file radio-cg2900.c.
+               </para>
+               </listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Checking the Radio Number</term>
+               <listitem>
+               <para>
+	       cat sys/module/radio_cg2900/parameters/radio_nr
+               </para>
+               <para>
+		The above command gets the radio number registered with
+		Video4Linux. This is used for opening the FM Radio
+		character device from user space.
+               </para>
+               </listitem>
+             </varlistentry>
+           </variablelist>
+         </para>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term>grid</term>
+         <listitem>
+         <para>
+           <variablelist>
+             <varlistentry>
+               <term>Parameter type</term>
+               <listitem><synopsis><type>int</type></synopsis></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Default value</term>
+               <listitem><para>1</para></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Runtime readable/modifiable</term>
+               <listitem><para>Readable</para></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Description</term>
+               <listitem>
+               <para>
+                 The parameter grid in radio-cg2900.c defines the spacing to be used in Khz while switching on FM Radio.
+                 <itemizedlist>
+                   <listitem><para>0:  50 kHz (China)</para></listitem>
+                   <listitem><para>1: 100 kHz (Europe, Japan)</para></listitem>
+                   <listitem><para>2: 200 kHz (USA)</para></listitem>
+                </itemizedlist>
+               </para>
+               </listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Changing the Grid</term>
+               <listitem>
+               <para>
+		echo 1 &gt; /sys/module/radio_cg2900/parameters/grid.
+               </para>
+               <para>
+		The above command sets the radio band spacing between
+		two adjacent radio channels, in this case sets to 100KHz
+		suitable for Europe. The change is applicable before
+		switching on FM Radio, otherwise the change takes effect
+		from next FM switch on.
+               </para>
+               <para>
+		Note: The Grid parameter cannot be changed during FM radio is operational.
+               </para>
+               <para>
+		The user must change the grid value and restart the FM radio when moving into a different radio region.
+               </para>
+               </listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Checking the current Grid Value</term>
+               <listitem>
+               <para>
+		cat sys/module/radio_cg2900/parameters/grid.
+               </para>
+               <para>
+		The above command gets the radio band spacing
+		between two adjacent radio channels currently set.
+               </para>
+               </listitem>
+             </varlistentry>
+            </variablelist>
+         </para>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term>band</term>
+         <listitem>
+         <para>
+           <variablelist>
+             <varlistentry>
+               <term>Parameter type</term>
+               <listitem><synopsis><type>int</type></synopsis></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Default value</term>
+               <listitem><para>0</para></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Runtime readable/modifiable</term>
+               <listitem><para>Readable</para></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Description</term>
+               <listitem>
+               <para>
+                 The parameter band in radio-cg2900.c defines the band to be used while switching on FM Radio.
+                 <itemizedlist>
+                   <listitem><para>0: 87.5 - 108 MHz (USA, Europe)</para></listitem>
+                   <listitem><para>1: 76   -  90 MHz (Japan)</para></listitem>
+                   <listitem><para>2: 70   - 108 MHz (China wide band)</para></listitem>
+                </itemizedlist>
+               </para>
+               </listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Changing the Band</term>
+               <listitem>
+               <para>
+		echo 0 &gt; /sys/module/radio_cg2900/parameters/band.
+               </para>
+               <para>
+		The above command sets the FM band to be used.
+		In this case, it sets the FM band 87.5 - 100 MHz.
+		The change is applicable before switching on FM Radio,
+		otherwise the change takes effect from next FM switch on.
+               </para>
+               <para>
+		Note: The band parameter cannot be changed during FM radio is operational.
+               </para>
+               <para>
+		The user must change the band value and restart the FM radio when moving into a different radio region.
+               </para>
+               </listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Checking the current Band Value</term>
+               <listitem>
+               <para>
+		cat sys/module/radio_cg2900/parameters/band.
+               </para>
+               <para>
+		The above command gets the current radio band set.
+               </para>
+               </listitem>
+             </varlistentry>
+           </variablelist>
+         </para>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term>cg2900_fm_debug_level</term>
+         <listitem>
+         <para>
+           <variablelist>
+             <varlistentry>
+               <term>Parameter type</term>
+               <listitem><synopsis><type>int</type></synopsis></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Default value</term>
+               <listitem><para>1</para></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Runtime readable/modifiable</term>
+               <listitem><para>Readable</para></listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Description</term>
+               <listitem>
+               <para>
+                 The parameter CG2900_fm_debug_level in platformosapi.c defines the debug level that is currently used.
+                 The higher the debug level the more print-outs are received in the terminal window.
+                 The following values are supported:
+                <itemizedlist>
+				<listitem><para>1 = Error logs</para></listitem>
+				<listitem><para>2 = Info logs, e.g. function entries</para></listitem>
+				<listitem><para>3 = Debug logs</para></listitem>
+				<listitem><para>4 = HCI logs, i.e. contents of the transferred data</para></listitem>
+				</itemizedlist>
+               </para>
+               </listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Changing the Log Level</term>
+               <listitem>
+               <para>
+		echo 3 &gt; /sys/module/radio_cg2900/parameters/cg2900_fm_debug_level.
+               </para>
+               <para>
+		The above command sets the Logging level of FM Driver.
+		In this case, it set will print all the debug messages
+		except the HCI commands exchanged with FM Chip.
+               </para>
+               </listitem>
+             </varlistentry>
+             <varlistentry>
+               <term>Checking the current Log Level</term>
+               <listitem>
+               <para>
+		cat sys/module/radio_cg2900/parameters/cg2900_fm_debug_level.
+               </para>
+               <para>
+		The above command gets the current debug log level set.
+               </para>
+               </listitem>
+             </varlistentry>
+           </variablelist>
+         </para>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+     <para>
+     </para>
+   </section>
+   <section id="driver-ioctl">
+     <title>Driver IO Control</title>
+     <para>
+       Describes the FM driver IO control parameters
+     </para>
+     <variablelist>
+       <varlistentry>
+         <term><constant>VIDIOC_QUERYCAP</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Get</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_capability</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>
+               The <constant>VIDIOC_QUERYCAP</constant> IOCTL is used to query the capabilities supported by FM Driver. IF the FM Driver supports FM Rx it should set the capabilities field bit should be bitwise OR'd with V4L2_CAP_TUNER, otherwise if it supports FM Tx, the capabilities field bit should  be bitwise OR'd with V4L2_CAP_MODULATOR.
+               Returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to retrive the Capabilities successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+               </itemizedlist>
+             </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+	<varlistentry>
+         <term><constant>VIDIOC_G_TUNER</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Get</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_tuner</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>
+               The <constant>VIDIOC_G_TUNER</constant> IOCTL gets the FM Radio Tuner properties supported by FM Radio. It is also used to retrieve RDS status, mono/stereo status and Signal strength of the tuned channel. These values are valid when FM is configured using IOCTL VIDIOC_S_TUNER, i.e in FM Rx mode.
+               Returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to retrive the tuner properties successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+                 </itemizedlist>
+		Note: Currently the retrieved signal strength is in decimals and not in "dBuV", proper external conversion required.
+             </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term><constant>VIDIOC_S_TUNER</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Set</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_tuner</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>
+               The <constant>VIDIOC_S_TUNER</constant> IOCTL configures the FM radio in Rx mode. Only 1 FM Tuner is supported by FM Driver.
+               Returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to set the tuner properties successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+               </itemizedlist>
+             </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+	<varlistentry>
+         <term><constant>VIDIOC_G_MODULATOR</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Get</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_tuner</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>
+                The <constant>VIDIOC_G_MODULATOR</constant> IOCTL gets the FM Radio Modulator properties supported by FM Radio. It is also used to retrieve RDS status and mono/stereo status. These values are valid when FM is configured using IOCTL VIDIOC_S_MODULATOR, i.e in FM Tx mode.
+               Returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to retrive the tuner properties successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+               </itemizedlist>
+             </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term><constant>VIDIOC_S_MODULATOR</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Set</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_modulator</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>
+               The <constant>VIDIOC_S_MODULATOR</constant> IOCTL configures the FM radio in Tx mode. Only 1 FM Modulator is supported by FM Driver.
+               Returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to set the modulator properties successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+               </itemizedlist>
+             </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term><constant>VIDIOC_S_FREQUENCY</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Set</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_frequency</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>
+               The <constant>VIDIOC_S_FREQUENCY</constant> IOCTL sets the frequency on FM radio in Rx or Tx mode. The frequency parameter passed is in V4L2 format.
+               Returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to set the frequency successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+               </itemizedlist>
+             </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term><constant>VIDIOC_G_FREQUENCY</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Set</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_modulator</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>
+               The <constant>VIDIOC_G_FREQUENCY</constant> IOCTL retrives the currently set frequency on FM Radio in Rx or Tx mode.
+               Returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to get the frequency successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+               </itemizedlist>
+             </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term><constant>VIDIOC_S_HW_FREQ_SEEK</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Set</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_hw_freq_seek</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>
+               The <constant>VIDIOC_S_HW_FREQ_SEEK</constant> IOCTL starts the seek operation when FM Radio is configured in Rx mode. The direction parameter indicates the direction of seeking from the current station. At present the FM Driver ignores the wrap_Around parameter and unconditional wrap around is supported. If the operation is started successfully, the application should use poll() to identify when the seek is over.
+               Returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to start the seek successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+               </itemizedlist>
+             </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term><constant>VIDIOC_G_CTRL</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Get</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_control</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>
+               The <constant>VIDIOC_G_CTRL</constant> IOCTL to retrive value of a paticular control. The following controls are supported by FM Driver:
+               <itemizedlist>
+               <listitem><para>
+               V4L2_CID_AUDIO_VOLUME
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_AUDIO_MUTE
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_AUDIO_BALANCE
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_CG2900_RADIO_RSSI_THRESHOLD
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_CG2900_RADIO_SELECT_ANTENNA
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_GET_RESULT
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_GET_RESULT
+               </para></listitem>
+               </itemizedlist>
+               Generic returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to retrive the value of the control successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+               </itemizedlist>
+              Note: For V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_GET_RESULT returned values are:
+               <itemizedlist>
+                 <listitem><para> -1 AF Switch failed, the AF-RSSI was too low.</para></listitem>
+                 <listitem><para> -2 AF Switch failed, the AF-PI Doesn't correlate.</para></listitem>
+                 <listitem><para> -3 AF Switch failed, the AF-RDS SYNC Lost.</para></listitem>
+               </itemizedlist>
+              </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term><constant>VIDIOC_S_CTRL</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Set</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_control</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+              <para>
+               The <constant>VIDIOC_S_CTRL</constant> IOCTL to set value of a paticular control. The following controls are supported by FM Driver:
+               <itemizedlist>
+               <listitem><para>
+               V4L2_CID_CG2900_RADIO_CHIP_STATE
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_CG2900_RADIO_BANDSCAN
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_CG2900_RADIO_BLOCKSCAN_START
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_CG2900_RADIO_SELECT_ANTENNA
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_CG2900_RADIO_RSSI_THRESHOLD
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_START
+               </para></listitem>
+               </itemizedlist>
+               Returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to set the value of the control successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+               </itemizedlist>
+             </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term><constant>VIDIOC_G_EXT_CTRLS</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Get</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_ext_controls</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>
+               The <constant>VIDIOC_G_EXT_CTRLS</constant> IOCTL to retrive value of a paticular control. This is used when a control class is defined or when the value to be retrieved is more than 1 parameter(s). Only V4L2_CTRL_CLASS_FM_TX class is supported for this IOCTL in FM Driver. The following controls are supported by FM Driver:
+               <itemizedlist>
+               <listitem><para>
+               V4L2_CID_RDS_TX_DEVIATION
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_PILOT_TONE_ENABLED
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_PILOT_TONE_DEVIATION
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_TUNE_PREEMPHASIS
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_TUNE_POWER_LEVEL
+               </para></listitem>
+               </itemizedlist>
+               Returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to retrive the value(s) of the control successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+               </itemizedlist>
+             </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+       <varlistentry>
+         <term><constant>VIDIOC_S_EXT_CTRLS</constant></term>
+         <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>Direction</term>
+             <listitem><para>Set</para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Parameter</term>
+             <listitem><synopsis><type>v4l2_ext_controls</type></synopsis></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>
+               The <constant>VIDIOC_S_CTRL</constant> IOCTL to set value of a paticular control when the parameters to be set are more than 1 parameter or when a control class is defined. At present only the V4L2_CTRL_CLASS_FM_TX and V4L2_CTRL_CLASS_USER control classes are supported by FM Driver. The following controls are supported by FM Driver:
+               <itemizedlist>
+               <listitem><para>
+               V4L2_CID_RDS_TX_DEVIATION
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_RDS_TX_PI
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_RDS_TX_PTY
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_RDS_TX_PS_NAME
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_RDS_TX_RADIO_TEXT
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_PILOT_TONE_ENABLED
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_PILOT_TONE_DEVIATION
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_TUNE_PREEMPHASIS
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_TUNE_POWER_LEVEL
+               </para></listitem>
+               <listitem><para>
+               V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START
+               </para></listitem>
+               </itemizedlist>
+               Returned values are:
+               <itemizedlist>
+                 <listitem><para>If IOCTL is able to set the value of the control successfully without errors the IOCTL function will return 0.</para></listitem>
+                 <listitem><para>A negative value will indicate error.</para></listitem>
+               </itemizedlist>
+             </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+     </variablelist>
+   </section>
+   <section id="driver-sysfs">
+     <title>Driver Interaction with Sysfs</title>
+     <para>
+       Not Applicable
+     </para>
+   </section>
+   <section id="driver-proc">
+     <title>Driver Interaction using /proc filesystem</title>
+     <para>
+       Not Applicable
+      </para>
+   </section>
+   <section id="driver-other">
+     <title>Other means for Driver Interaction</title>
+     <para>
+      Not Applicable
+     </para>
+   </section>
+ <section id="driver-node">
+   <title>Driver Node File</title>
+     <variablelist>
+     <varlistentry>
+       <term>FM Radio Device</term>
+       <listitem>
+         <variablelist>
+           <varlistentry>
+             <term>File</term>
+             <listitem><para><filename>/dev/radio0</filename></para></listitem>
+           </varlistentry>
+           <varlistentry>
+             <term>Description</term>
+             <listitem>
+             <para>The radio device for FM Radio.</para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+       </listitem>
+     </varlistentry>
+    </variablelist>
+  </section>
+ </chapter>
+ <chapter id="bugs">
+   <title>Known Bugs And Limitations</title>
+     <para>
+     <variablelist>
+     <varlistentry>
+       <term>No known issues.</term>
+       <listitem>
+         <para>
+           <!-- Do NOT change the chapter id or title! -->
+         </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+ </chapter>
+ <chapter id="internal-functions">
+   <title>Internal Functions Provided</title>
+   <para>
+	List of internal functions used in FM Driver.
+   </para>
+   <!-- Do NOT change the chapter id or title! -->
+    <section id="radio-cg2900.c">
+    <title>radio-cg2900.c</title>
+!Idrivers/media/radio/CG2900/radio-cg2900.c
+  </section>
+  <section id="cg2900_fm_api.h">
+    <title>cg2900_fm_api.h</title>
+!Idrivers/media/radio/CG2900/cg2900_fm_api.h
+  </section>
+  <section id="cg2900_fm_api.c">
+    <title>cg2900_fm_api.c</title>
+!Idrivers/media/radio/CG2900/cg2900_fm_api.c
+  </section>
+  <section id="cg2900_fm_driver.h">
+    <title>cg2900_fm_driver.h</title>
+!Idrivers/media/radio/CG2900/cg2900_fm_driver.h
+  </section>
+  <section id="cg2900_fm_driver.c">
+    <title>cg2900_fm_driver.c</title>
+!Idrivers/media/radio/CG2900/cg2900_fm_driver.c
+  </section>
+ </chapter>
+</book>
diff --git a/Documentation/DocBook/db5500_keypad.tmpl b/Documentation/DocBook/db5500_keypad.tmpl
new file mode 100644
index 00000000000..a25fc990516
--- /dev/null
+++ b/Documentation/DocBook/db5500_keypad.tmpl
@@ -0,0 +1,91 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="keypad-API-Guide">
+ <bookinfo>
+  <title>DB5500 Keypad</title>
+
+  <authorgroup>
+   <author>
+    <firstname>NaveenKumar</firstname>
+    <surname>Gaddipati</surname>
+    <affiliation>
+     <address>
+      <email>naveen.gaddipati@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2010</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <para>
+     License terms: GNU General Public License (GPL) version 2.
+   </para>
+
+   </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This documentation describes the API provided by the keypad
+	driver for internal keypad.
+    </para>
+    </chapter>
+
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		None.
+           </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+  <chapter id="pubfunctions">
+     <title>Public Functions Provided</title>
+     <para>
+	This db5500-keypad driver doesn't export any functions.
+     </para>
+  </chapter>
+
+  <chapter id="structs">
+     <title>Structures</title>
+     <para>
+     This chapter contains the autogenerated documentation of the
+     structures which are used in the keypad driver.
+     </para>
+!Iarch/arm/mach-ux500/include/mach/db5500-keypad.h
+  </chapter>
+
+  <chapter id="intfunctions">
+     <title>Internal Functions Provided</title>
+     <para>
+     This chapter contains the autogenerated documentation of the
+     internal functions.
+     </para>
+!Idrivers/input/keyboard/db5500_keypad.c
+  </chapter>
+
+  </book>
diff --git a/Documentation/DocBook/device-drivers.tmpl b/Documentation/DocBook/device-drivers.tmpl
index b638e50cf8f..5f70f734e8b 100644
--- a/Documentation/DocBook/device-drivers.tmpl
+++ b/Documentation/DocBook/device-drivers.tmpl
@@ -437,4 +437,21 @@ X!Idrivers/video/console/fonts.c
 !Edrivers/i2c/i2c-core.c
   </chapter>
 
+  <chapter id="hsi">
+     <title>High Speed Synchronous Serial Interface (HSI)</title>
+
+     <para>
+	High Speed Synchronous Serial Interface (HSI) is a
+	serial interface mainly used for connecting application
+	engines (APE) with cellular modem engines (CMT) in cellular
+	handsets.
+
+	HSI provides multiplexing for up to 16 logical channels,
+	low-latency and full duplex communication.
+     </para>
+
+!Iinclude/linux/hsi/hsi.h
+!Edrivers/hsi/hsi.c
+  </chapter>
+
 </book>
diff --git a/Documentation/DocBook/gpio.tmpl b/Documentation/DocBook/gpio.tmpl
new file mode 100644
index 00000000000..b69c2770210
--- /dev/null
+++ b/Documentation/DocBook/gpio.tmpl
@@ -0,0 +1,112 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="GPIO">
+ <bookinfo>
+  <title>GPIO1B</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Alessandro</firstname>
+    <surname>Rubini</surname>
+    <affiliation>
+     <address>
+      <email>rubini@unipv.it</email>
+     </address>
+    </affiliation>
+   </author>
+   <author>
+    <firstname>Prafulla</firstname>
+    <surname>WADASKAR</surname>
+    <affiliation>
+     <address>
+      <email>prafulla.wadaskar@st.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2008-2010</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <!-- Do NOT remove the legal notice below -->
+
+  <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+  </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This Documentation describes the API's provided by the GPIO controller Driver.
+    </para>
+    <para>
+	Only the API specific to the Ux500 platform is listed here.  For the generic GPIO
+	API, see <filename>Documentation/gpio.txt</filename> in the kernel source tree.
+    </para>
+  </chapter>
+
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		None.
+           </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+  <chapter id="pubfunctions">
+    <title> Public Interface </title>
+    <para>
+	This Section lists the API's provided by the GPIO controller driver to client drivers.
+    </para>
+    <para>
+	Only the API specific to the Ux500 platform is listed here.  For the generic GPIO
+	API, see <filename>Documentation/gpio.txt</filename> in the kernel source tree.
+    </para>
+!Earch/arm/plat-nomadik/gpio.c
+  </chapter>
+</book>
diff --git a/Documentation/DocBook/i2c.tmpl b/Documentation/DocBook/i2c.tmpl
new file mode 100644
index 00000000000..8a4cb49204e
--- /dev/null
+++ b/Documentation/DocBook/i2c.tmpl
@@ -0,0 +1,116 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="I2C">
+ <bookinfo>
+  <title>I2C</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Srinidhi</firstname>
+    <surname>Kasagar</surname>
+    <affiliation>
+     <address>
+      <email>srinidhi.kasagar@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+   <author>
+    <firstname>Sachin</firstname>
+    <surname>Verma</surname>
+    <affiliation>
+     <address>
+      <email>sachin.verma@st.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2009-2010</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <!-- Do NOT remove the legal notice below -->
+
+  <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+  </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This Documentation describes the API's provided by the I2C controller Driver.
+	Since this driver registers the transferfunction with kernel framework, there
+	are only private functions in this I2C bus driver. This driver currently
+	works only in master mode and does 7 bit adderssing only. There is no support
+	for 10 bit addressing. The driver currently supports standard mode (100KHz)
+	and Fast mode (400KHz) operation.
+    </para>
+  </chapter>
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		None.
+           </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+  <chapter id="pubfunctions">
+     <title>Public Functions Provided</title>
+     <para>
+	Not Applicable
+     </para>
+  </chapter>
+
+  <chapter id="private">
+    <title>Private Functions</title>
+    <para>
+	This Section lists the functions used internally  by the I2C controller driver.
+    </para>
+!Idrivers/i2c/busses/i2c-nomadik.c
+  </chapter>
+
+</book>
diff --git a/Documentation/DocBook/i2s.tmpl b/Documentation/DocBook/i2s.tmpl
new file mode 100644
index 00000000000..6b6c50572e2
--- /dev/null
+++ b/Documentation/DocBook/i2s.tmpl
@@ -0,0 +1,97 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="I2S">
+ <bookinfo>
+  <title>I2S</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Sandeep</firstname>
+    <surname>Kaushik</surname>
+    <affiliation>
+     <address>
+      <email>sandeep.kaushik@st.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2008-2009</year>
+   <holder>STMicroelectronics Pvt Ltd</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <!-- Do NOT remove the legal notice below -->
+
+  <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+  </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This Documentation describes the APIs provided by the I2S Bus Driver. I2S bus supports different
+	protocols like I2S, PCM, SPI etc.
+    </para>
+  </chapter>
+
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		None.
+           </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+  <chapter id="pubfunctions">
+    <title>Public Functions Provided</title>
+	<para>
+	This Section lists the functions exported by the I2S bus driver. These functions cater to all the protocols
+	supported namely: I2S, PCM, SPI.
+	</para>
+!Edrivers/misc/i2s/i2s.c
+  </chapter>
+</book>
diff --git a/Documentation/DocBook/lsm303dlh.tmpl b/Documentation/DocBook/lsm303dlh.tmpl
new file mode 100644
index 00000000000..3b1c6afa65f
--- /dev/null
+++ b/Documentation/DocBook/lsm303dlh.tmpl
@@ -0,0 +1,90 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="LSM303DLH-API-Guide">
+ <bookinfo>
+  <title>LSM303DLH Accelerometer and Magnetometer</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Chethan Krishna</firstname>
+    <surname>N</surname>
+    <affiliation>
+     <address>
+      <email>chethan.krishna@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2010</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <para>
+     License terms: GNU General Public License (GPL) version 2.
+   </para>
+
+   </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This documentation describes the accelerometer and magnetometer drivers for lsm303dlh sensor chip.
+    </para>
+    </chapter>
+
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		None.
+           </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+  <chapter id="pubfunctions">
+     <title>Public Functions Provided</title>
+     <para>
+	This accelerometer/magnetometer drivers don't export any functions.
+     </para>
+  </chapter>
+
+  <chapter id="structs">
+     <title>Structures</title>
+     <para>
+     This chapter contains the autogenerated documentation of the structures which are
+     used in the accelerometer/magnetometer drivers.
+     </para>
+!Iinclude/linux/lsm303dlh.h
+  </chapter>
+
+  <chapter id="intfunctions">
+     <title>Internal Functions Provided</title>
+     <para>
+     This chapter contains the autogenerated documentation of the internal functions.
+     </para>
+!Idrivers/hwmon/lsm303dlh_a.c
+!Idrivers/hwmon/lsm303dlh_m.c
+  </chapter>
+
+  </book>
diff --git a/Documentation/DocBook/msp.tmpl b/Documentation/DocBook/msp.tmpl
new file mode 100644
index 00000000000..55cec352a76
--- /dev/null
+++ b/Documentation/DocBook/msp.tmpl
@@ -0,0 +1,104 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="MSP">
+ <bookinfo>
+  <title>MSP</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Sandeep</firstname>
+    <surname>Kaushik</surname>
+    <affiliation>
+     <address>
+      <email>sandeep.kaushik@st.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2008-2009</year>
+   <holder>STMicroelectronics Pvt Ltd</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <!-- Do NOT remove the legal notice below -->
+
+  <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+  </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This Documentation describes the API's provided by the MSP controller Driver.
+	MSP controller supports different protocols like I2S, PCM, SPI etc.
+    </para>
+  </chapter>
+
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		None.
+           </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+  <chapter id="pubfunctions">
+     <title>Public Functions Provided</title>
+     <para>
+	Not Applicable.
+     </para>
+  </chapter>
+
+  <chapter id="private">
+    <title>Private Functions</title>
+	<para>
+	This Section lists the functions used by the MSP controller driver.
+	These functions cater to all the protocols supported namely: I2S, PCM, SPI.
+	</para>
+!Idrivers/misc/i2s/msp_i2s.c
+  </chapter>
+</book>
diff --git a/Documentation/DocBook/prcmu-fw-api.tmpl b/Documentation/DocBook/prcmu-fw-api.tmpl
new file mode 100644
index 00000000000..2d9419de53e
--- /dev/null
+++ b/Documentation/DocBook/prcmu-fw-api.tmpl
@@ -0,0 +1,109 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="STw4500">
+ <bookinfo>
+  <title>PRCMU Driver</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Sudeep Karkada</firstname>
+    <surname>Nagesha</surname>
+    <affiliation>
+     <address>
+      <email>sudeepkarkada.nagesha@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2009-2010</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+   <legalnotice>
+   <!-- Do NOT remove the legal notice below -->
+
+  <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+  </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This documentation describes the API provided by the PRCMU firmware interface driver.
+    </para>
+    </chapter>
+
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		None.
+           </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+     <chapter id="enum">
+     <title>Enumerations</title>
+     <para>
+     This chapter contains the autogenerated documentation of the structures
+     and enumerations which are used in the PRCMU firmware interface driver.
+     It is also required by the client drivers.
+     </para>
+!Iarch/arm/mach-ux500/include/mach/prcmu-fw-api.h
+  </chapter>
+
+  <chapter id="pubfunctions">
+     <title>Public Functions Provided</title>
+     <para>
+     This chapter contains the autogenerated documentation of the kernel
+     API functions which are exported to the client drivers.
+     </para>
+!Earch/arm/mach-ux500/prcmu-db8500.c
+  </chapter>
+
+
+  </book>
diff --git a/Documentation/DocBook/shrm.tmpl b/Documentation/DocBook/shrm.tmpl
new file mode 100644
index 00000000000..2773b821bf5
--- /dev/null
+++ b/Documentation/DocBook/shrm.tmpl
@@ -0,0 +1,139 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="SHRM">
+ <bookinfo>
+  <title>Shared Memory</title>
+  <authorgroup>
+   <author>
+    <firstname>Biju</firstname>
+    <surname>Das</surname>
+    <affiliation>
+     <address>
+      <email>biju.das@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+   <author>
+    <firstname>Kumar</firstname>
+    <surname>Sanghvi</surname>
+    <affiliation>
+     <address>
+      <email>kumar.sanghvi@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+   <author>
+    <firstname>Arun</firstname>
+    <surname>Murthy</surname>
+    <affiliation>
+     <address>
+      <email>arun.murthy@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2009-2010</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <!-- Do NOT remove the legal notice below -->
+   <para>
+     Licence terms: GNU General Public Licence (GPL) version 2.
+   </para>
+  </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This Documentation describes the ST-Ericsson's adaptation on protocol used for CMT/APE communication when SHaRedMemory is used as IPC link.
+    </para>
+  </chapter>
+
+  <chapter id="design">
+    <title>Design</title>
+    <para>
+	The APE consists Cortex A9 dual core SMP, a multimedia DSP and PRCMU. Modem consists of 2 Cortex R4 ARM processor.
+	The exchange of messages between CMT(Cellular Mobile Terminal) and APE includes copying the data to a shared area DDR. This region is accessible by both CMT and APE. The design includes 2 channels common and audio. Common channel is used for exchanging ISI, RPC and SECURITY messages. Audio channel is used for exchanging AUDIO messages. Each channel consists of 2 FIFO. One FIFO for sending message from CMT to APE and other from APE to CMT. Each of these FIFO have write and read pointer shared between APE and CMT. Writer pointer is updated on copying the message to FIFO and reader will read the messages from the read pointer upto the writer pointer. Writer and reader notifications are used to notify the completion of read/write operation(seperate for APE and CMT). Driver includes 4 queues. Once the messages are sent from CMT to APE it resides in the FIFO and then copied to one of the 4 queues based on the message type(ISI, RPC, AUDIO, SECURITY) and then the net/char device interface fetches this message from the queue and copies to the user space buffer.
+    </para>
+  </chapter>
+
+  <chapter id="concepts">
+    <title>Concepts</title>
+    <para>
+	The user space application sends ISI/RPC/AUDIO/SECURITY messages. ISI is sent through the phonet to shrm driver. For achieving this there are 2 interfaces to the shrm driver. Net interface used for exchanging the ISI message and char interface for RPC, AUDIO and SECURITY messages. On receiving any of these messages from the user space application, it is copied to a memory in kernel space. From here it is then copied to respective FIFO from where the CMT reads the message.
+	CMT(Cellular Mobile Terminal) writes messages to the respective FIFO and thereafter to respective queue. The net/char device copies this message from the queue to the user space buffer.
+    </para>
+  </chapter>
+
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		Assumptions
+		1. ApeShmFifo#0 is of 128kB in size. As this is used for transmission except CS audio call data. Expected message size is 1.5kB with a max of 16kB.
+		2. ApeShmFifo#1 is of 4kB in size. This is used for transmission of CS audio call data. Expected message size is 24kb.
+		3. CmtShmFifo#0 is of 128kB in size. As this is used for transmission except CS audio call data. Expected message size is 1.5kB with a max of 16kB.
+		4. CmtShmFifo#1 is of 4kB in size. This is used for transmission of CS audio call data. Expected message size is 24kb.
+		The total size of the FIFO is 264 kB.
+         </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+  <chapter id="pubfunctions">
+     <title>Public Functions Provided</title>
+     <para>
+	This Section lists the API's provided by the SHRM driver to phonet drivers.
+     </para>
+!Edrivers/net/u8500_shrm.c
+     <para>
+	This Section lists the API's provided by the SHRM driver used in transmission of RPC, AUDIO and SECURITY messages.
+     </para>
+!Edrivers/char/shrm_char.c
+
+  </chapter>
+
+  <chapter id="private">
+    <title>Private Functions</title>
+    <para>
+	This Section lists the functions used internally by the SHRM driver to implement FIFO management. It physically reads/writes data to/from memory.
+    </para>
+!Idrivers/modem/shrm/shrm_fifo.c
+    <para>
+	This Section lists the functions used internally by the SHRM driver to implement the SHM protocol and handle all interrupt callback.
+    </para>
+!Idrivers/modem/shrm/shrm_protocol.c
+    <para>
+	This Section lists the functions used internally by the SHRM driver to implement Modem-Host communication L1 interface specifications.
+    </para>
+!Idrivers/modem/shrm/modem_shrm_driver.c
+  </chapter>
+
+  <chapter id="Other">
+    <title>Other Data Structures</title>
+    <para>
+	This Section lists some of the Data structure used by the SHRM driver.
+    </para>
+!Iinclude/linux/modem/shrm/shrm_driver.h
+!Iinclude/linux/modem/shrm/shrm_private.h
+  </chapter>
+</book>
diff --git a/Documentation/DocBook/ste_timed_vibra.tmpl b/Documentation/DocBook/ste_timed_vibra.tmpl
new file mode 100644
index 00000000000..ae9e62c9fb1
--- /dev/null
+++ b/Documentation/DocBook/ste_timed_vibra.tmpl
@@ -0,0 +1,278 @@
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="STE-Timed-Output-Vibrator-API-Guide">
+ <bookinfo>
+  <title>Timed Output Vibrator Driver</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Marcin</firstname>
+    <surname>Mielczarczyk</surname>
+    <affiliation>
+     <address>
+      <email>marcin.mielczarczyk@tieto.com</email>
+     </address>
+    </affiliation>
+   </author>
+  </authorgroup>
+
+  <copyright>
+   <year>2010</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+
+  <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+ </legalnotice>
+ </bookinfo>
+
+ <toc></toc>
+
+ <chapter id="intro">
+  <title>Introduction</title>
+  <para>
+     This documentation describes the implementation of ST-Ericsson's
+     Timed Output Vibrator driver for the ST-Ericsson Linux platforms.
+  </para>
+ </chapter>
+
+ <chapter id="gettingstarted">
+  <title>Getting Started</title>
+  <para>
+     There are no special compilation flags needed to build the
+     Timed Output Vibrator driver.
+  </para>
+
+  <section id="basic-tutorial">
+    <title>Basic Tutorial</title>
+    <para>
+      Before using this driver few parameters shall be defined in the platform data structure:
+    <itemizedlist>
+      <listitem><para>Boost level - vibrator speed in the the startup stage</para></listitem>
+      <listitem><para>Boost time - vibrator startup period</para></listitem>
+      <listitem><para>On level - vibrator moderate speed</para></listitem>
+      <listitem><para>On time - vibrator moderate period</para></listitem>
+      <listitem><para>Off level - vibrator speed in the stop stage</para></listitem>
+      <listitem><para>Off time - vibrator stop period</para></listitem>
+    </itemizedlist>
+    </para>
+    <para>
+      NOTE: If the time elapsing between the ON and OFF sequence is smaller than the 'boost' period,
+      then OFF is delayed until the 'boost' expires. If the time elapsing between the OFF and
+      ON sequence is smaller than the 'off' period, then ON is delayed until the 'off' period expires.
+      The ON request when vibrator is in 'boost' or 'on' stage is ignored.
+      The OFF request when vibrator is in the 'off' stage is ignored.
+    </para>
+    <para>
+      To enable the Timed Output Vibrator driver using Kconfig, go to
+      <constant> Device Drivers -&gt; Staging -&gt; Android </constant>
+      and enable the following:
+    </para>
+    <itemizedlist>
+      <listitem><para>ST-Ericsson Timed Output Vibrator driver</para></listitem>
+    </itemizedlist>
+  </section>
+
+ </chapter>
+
+ <chapter id="concepts">
+  <title>Concepts</title>
+  <para>
+    Vibrator driver registers as timed output class device (implemented for Android).
+  </para>
+ </chapter>
+
+ <chapter id="tasks">
+  <title>Tasks</title>
+   <para>
+    Timed Output Vibrator can be controlled from user space using following device:
+    <itemizedlist>
+     <listitem><para>/sys/class/timed_output/vibrator/enable</para></listitem>
+    </itemizedlist>
+     To turn on the vibrator for 1s, following command should be executed:
+    <itemizedlist>
+     <listitem><para>echo 1000 &gt; /sys/class/timed_output/vibrator/enable</para></listitem>
+    </itemizedlist>
+     To turn off the vibrator if enabled, then the following command should be executed:
+    <itemizedlist>
+     <listitem><para>echo 0 &gt; /sys/class/timed_output/vibrator/enable</para></listitem>
+    </itemizedlist>
+     To get the remaining time, the following command should be executed:
+    <itemizedlist>
+     <listitem><para>cat /sys/class/timed_output/vibrator/enable</para></listitem>
+    </itemizedlist>
+   </para>
+
+ </chapter>
+
+ <chapter id="driver-configuration">
+  <title>Driver Configuration and Interaction</title>
+  <para>
+     There are no configuration parameters for Timed Output Vibrator Driver.
+  </para>
+  <section id="driver-implemented-operations">
+    <title>Implemented operations in driver</title>
+    <para>
+     All available operations are provided by Timed Output class driver.
+    </para>
+    <para>
+           <table>
+             <title> Supported device driver operations </title>
+             <tgroup cols="2"><tbody>
+               <row><entry> enable </entry> <entry> Calls vibra_enable() function which controls timer and workqueue </entry> </row>
+               <row><entry> get_time </entry> <entry> Calls vibra_get_time() function which returns time remaining to the end of vibration </entry> </row>
+             </tbody></tgroup>
+           </table>
+    </para>
+
+  </section>
+    <section id="driver-loading">
+    <title>Driver loading parameters</title>
+    <para>
+     Not Applicable.
+    </para>
+  </section>
+  <section id="driver-ioctl">
+    <title>Driver IO Control</title>
+    <para>
+     Not Applicable.
+    </para>
+ </section>
+
+ <section id="driver-sysfs">
+    <title>Driver Interaction with Sysfs</title>
+    <para>
+     Timed Output Vibrator driver is available in sysfs as <filename>/sys/class/timed_output/vibrator</filename>
+    </para>
+    <variablelist>
+    <varlistentry>
+      <term>enable</term>
+      <listitem>
+        <variablelist>
+          <varlistentry>
+              <term>Direction</term>
+              <listitem><para>read-write</para></listitem>
+          </varlistentry>
+          <varlistentry>
+             <term>Attribute file type</term>
+             <listitem>
+             <para>Text file</para>
+             </listitem>
+          </varlistentry>
+          <varlistentry>
+             <term>File path</term>
+             <listitem><para><filename>/sys/class/timed_output/vibrator/enable</filename></para>
+              </listitem>
+          </varlistentry>
+          <varlistentry>
+             <term>Description</term>
+             <listitem>
+		<para>
+			Vibrator is enabled for given period of time on file write (in miliseconds).
+		</para>
+		<para>
+			When written 0, vibrator stops.
+		</para>
+		<para>
+			When file is read, it gives remaining time to disable vibrator (in miliseconds).
+                </para>
+             </listitem>
+           </varlistentry>
+         </variablelist>
+         </listitem>
+       </varlistentry>
+
+    </variablelist>
+  </section>
+ <section id="driver-proc">
+    <title>Driver Interaction using /proc filesystem</title>
+    <para>
+     Not Applicable.
+    </para>
+
+  </section>
+
+ <section id="driver-other">
+    <title>Other means for Driver Interaction</title>
+    <para>
+     Not Applicable.
+    </para>
+  </section>
+
+ <section id="driver-node">
+    <title>Driver Node File</title>
+    <para>
+      Not aplicable.
+    </para>
+  </section>
+
+
+ </chapter>
+
+
+ <chapter id="bugs">
+  <title>Known Bugs And Assumptions</title>
+  <para>
+    <variablelist>
+     <varlistentry>
+      <term>None.</term>
+      <listitem>
+       <para>
+       </para>
+      </listitem>
+     </varlistentry>
+
+    </variablelist>
+
+  </para>
+ </chapter>
+
+<chapter id="pubfunctions">
+   <title>Public Functions Provided</title>
+    <para>
+     Not Applicable.
+    </para>
+</chapter>
+
+<chapter id="internal-functions">
+   <title>Internal Functions Provided</title>
+     <para>
+     This chapter contains the autogenerated documentation of the internal functions.
+     </para>
+!Edrivers/staging/android/ste_timed_vibra.c
+</chapter>
+
+</book>
diff --git a/Documentation/DocBook/stmpe.tmpl b/Documentation/DocBook/stmpe.tmpl
new file mode 100644
index 00000000000..9e64a00f6b3
--- /dev/null
+++ b/Documentation/DocBook/stmpe.tmpl
@@ -0,0 +1,115 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="STMPE MFD devices">
+ <bookinfo>
+  <title>STMPE IO-Port Expander guide</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Rabin</firstname>
+    <surname>Vincent</surname>
+    <affiliation>
+     <address>
+      <email>rabin.vincent@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+  </authorgroup>
+
+  <copyright>
+   <year>2010</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+
+   </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This documentation describes the driver for STMicroelectronics
+	STMPExxxx port expander devices.
+    </para>
+   </chapter>
+
+ <chapter id="bugs">
+  <title>Known Bugs And Assumptions</title>
+  <para>
+    <variablelist>
+     <varlistentry>
+      <term>None.</term>
+      <listitem>
+       <para>
+		None.
+       </para>
+      </listitem>
+     </varlistentry>
+    </variablelist>
+  </para>
+ </chapter>
+
+ <chapter id="pubfunctions">
+   <title>Public Functions Provided</title>
+   <para>
+	List of public interfaces in stmpe driver
+   </para>
+!Edrivers/mfd/stmpe.c
+ </chapter>
+
+ <chapter id="private">
+   <title>Private Functions</title>
+   <para>
+	STMPE Keypad driver
+	STMPE GPIO driver
+   </para>
+  <section id="stmpe-keypad.c">
+    <title>stmpe-keypad.c</title>
+!Idrivers/input/keyboard/stmpe-keypad.c
+ </section>
+ </chapter>
+
+  <chapter id="Other">
+    <title>Other Data Structures</title>
+    <para>
+       This Section lists some of the Data structure used by the stmpe driver and client drivers.
+    </para>
+!Iinclude/linux/mfd/stmpe.h
+!Idrivers/mfd/stmpe.h
+</chapter>
+</book>
diff --git a/Documentation/DocBook/synaptics_rmi4_touchp.tmpl b/Documentation/DocBook/synaptics_rmi4_touchp.tmpl
new file mode 100644
index 00000000000..bc104eb4840
--- /dev/null
+++ b/Documentation/DocBook/synaptics_rmi4_touchp.tmpl
@@ -0,0 +1,106 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="Synaptics RMI4 API Guide">
+ <bookinfo>
+  <title>Synaptics RMI4 Touch screen</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Naveen Kumar</firstname>
+    <surname>Gaddipati</surname>
+    <affiliation>
+     <address>
+      <email>naveen.gaddipati@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2010</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <!-- Do NOT remove the legal notice below -->
+
+  <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+  </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This documentation describes the functions provided by the
+	driver of touch panel for Synaptics RMI4 controller
+    </para>
+    </chapter>
+
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		None.
+           </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+  <chapter id="pubfunctions">
+     <title>Public Functions Provided</title>
+     <para>
+	Not Applicable.
+     </para>
+  </chapter>
+
+    <chapter id="intfunctions">
+     <title>Internal Functions Provided</title>
+     <para>
+     This chapter contains the autogenerated documentation of the internal
+     functions of the Tocuh panel driver.
+     </para>
+!Idrivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
+  </chapter>
+
+  </book>
diff --git a/Documentation/DocBook/tc_keypad.tmpl b/Documentation/DocBook/tc_keypad.tmpl
new file mode 100644
index 00000000000..3f2630d9d6c
--- /dev/null
+++ b/Documentation/DocBook/tc_keypad.tmpl
@@ -0,0 +1,113 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="TC35893">
+ <bookinfo>
+  <title>TC35893 Keypad</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Jayeeta</firstname>
+    <surname>Banerjee</surname>
+    <affiliation>
+     <address>
+      <email>jayeeta.banerjee@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2010</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <!-- Do NOT remove the legal notice below -->
+
+  <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+  </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This documentation describes the API provided by the keypad driver for TC35893 controller
+    </para>
+    </chapter>
+
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		None.
+           </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+     <chapter id="structs">
+     <title>Structures</title>
+     <para>
+     This chapter contains the autogenerated documentation of the structures which are
+     used in the keypad driver.
+     </para>
+!Iinclude/linux/mfd/tc3589x.h
+  </chapter>
+
+  <chapter id="pubfunctions">
+     <title>Public Functions Provided</title>
+     <para>
+	Not Applicable.
+     </para>
+  </chapter>
+
+  <chapter id="intfunctions">
+     <title>Internal Functions Provided</title>
+     <para>
+     This chapter contains the autogenerated documentation of the internal functions.
+     </para>
+!Idrivers/input/keyboard//tc3589x-keypad.c
+  </chapter>
+
+  </book>
diff --git a/Documentation/DocBook/touchp.tmpl b/Documentation/DocBook/touchp.tmpl
new file mode 100644
index 00000000000..4301b23bfc0
--- /dev/null
+++ b/Documentation/DocBook/touchp.tmpl
@@ -0,0 +1,104 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="bu21013_ts">
+ <bookinfo>
+  <title>Touch screen ROHM BU21013MWV</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Naveen Kumar</firstname>
+    <surname>Gaddipati</surname>
+    <affiliation>
+     <address>
+      <email>naveen.gaddipati@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2009</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <!-- Do NOT remove the legal notice below -->
+
+  <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+  </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This documentation describes the functions provided by the driver of touch panel for BU21013 controller
+    </para>
+    </chapter>
+
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		None.
+           </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+  <chapter id="pubfunctions">
+     <title>Public Functions Provided</title>
+     <para>
+	Not Applicable.
+     </para>
+  </chapter>
+
+    <chapter id="intfunctions">
+     <title>Internal Functions Provided</title>
+     <para>
+     This chapter contains the autogenerated documentation of the internal functions of the Tocuh panel driver.
+     </para>
+!Idrivers/input/touchscreen/bu21013_ts.c
+  </chapter>
+
+  </book>
diff --git a/Documentation/DocBook/u5500_LogicalMailbox.tmpl b/Documentation/DocBook/u5500_LogicalMailbox.tmpl
new file mode 100644
index 00000000000..71a5d6c7c28
--- /dev/null
+++ b/Documentation/DocBook/u5500_LogicalMailbox.tmpl
@@ -0,0 +1,114 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="Mailbox LD">
+ <bookinfo>
+  <title>u5500 Mailbox Logical Driver</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Bibek</firstname>
+    <surname>Basu</surname>
+    <affiliation>
+     <address>
+      <email>bibek.basu@stericsson.com</email>
+     </address>
+    </affiliation>
+   </author>
+   </authorgroup>
+
+  <copyright>
+   <year>2011</year>
+   <holder>ST-Ericsson</holder>
+  </copyright>
+
+  <subjectset>
+    <subject>
+      <subjectterm>Linux standard functions</subjectterm>
+    </subject>
+  </subjectset>
+
+  <legalnotice>
+   <!-- Do NOT remove the legal notice below -->
+
+  <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This program is distributed in the hope that it will be
+     useful, but WITHOUT ANY WARRANTY; without even the implied
+     warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+     See the GNU General Public License for more details.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public
+     License along with this program; if not, write to the Free
+     Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+     MA 02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the source
+     distribution of Linux.
+   </para>
+  </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+
+  <chapter id="intro">
+    <title>Introduction</title>
+    <para>
+	This documentation describes the API provided by the U5500 Mailbox Logical Driver.
+    </para>
+    </chapter>
+
+  <chapter id="bugs">
+     <title>Known Bugs And Assumptions</title>
+  <para>
+     <variablelist>
+     <varlistentry>
+       <term>None</term>
+       <listitem>
+         <para>
+		None.
+           </para>
+       </listitem>
+     </varlistentry>
+     </variablelist>
+  </para>
+  </chapter>
+
+     <chapter id="structs">
+     <title>Structures</title>
+     <para>
+     This chapter contains the autogenerated documentation of the structures which are
+     used in the U5500 Mailbox Logical Driver.
+     </para>
+!Iarch/arm/mach-ux500/include/mach/mbox_channels-db5500.h
+  </chapter>
+
+  <chapter id="pubfunctions">
+     <title>Public Functions Provided</title>
+     <para>
+	List of public interfaces in stmpe driver
+     </para>
+!Edrivers/misc/mbox_channels-db5500.c
+  </chapter>
+
+  <chapter id="intfunctions">
+     <title>Internal Functions Provided</title>
+     <para>
+     This chapter contains the autogenerated documentation of the internal functions.
+     </para>
+!Idrivers/misc/mbox_channels-db5500.c
+  </chapter>
+
+  </book>
diff --git a/Documentation/ioctl/ioctl-number.txt b/Documentation/ioctl/ioctl-number.txt
index 3a46e360496..016ee45522d 100644
--- a/Documentation/ioctl/ioctl-number.txt
+++ b/Documentation/ioctl/ioctl-number.txt
@@ -223,6 +223,7 @@ Code  Seq#(hex)	Include File		Comments
 'j'	00-3F	linux/joystick.h
 'k'	00-0F	linux/spi/spidev.h	conflict!
 'k'	00-05	video/kyro.h		conflict!
+'k'	10-17	linux/hsi/hsi_char.h	HSI character device
 'l'	00-3F	linux/tcfs_fs.h		transparent cryptographic file system
 					<http://web.archive.org/web/*/http://mikonos.dia.unisa.it/tcfs>
 'l'	40-7F	linux/udf_fs_i.h	in development:
diff --git a/Documentation/trace/stm-trace.txt b/Documentation/trace/stm-trace.txt
new file mode 100644
index 00000000000..cd73c2b87b7
--- /dev/null
+++ b/Documentation/trace/stm-trace.txt
@@ -0,0 +1,193 @@
+		MIPI System Trace Module driver
+		===============================
+
+Copyright (C) ST-Ericsson SA 2011
+  Authors:   Pierre Peiffer <pierre dot peiffer at stericsson dot com>
+             Philippe Langlais <philippe dot langlais at linaro dot org>
+  License:   The GNU Free Documentation License, Version 1.2
+               (dual licensed under the GPL v2)
+
+Hardware overview
+=================
+  This hardware collects and provides simple tracepoints,
+  so a system processor (in our case the main ARM CPU,
+  or some small CPUs and DSPs) can write some data,
+  up to 8 bytes, into a register and out comes a log entry
+  with a time stamp (20ns resolution) on one of 256 channels. Also
+  hardware tracepoints are supported.
+
+  This module external interface is a pad on the chip
+  which complies to the MIPI System Trace Protocol v1.0
+  (see http://www.mipi.org/specifications/debug)
+  and the actual trace output can be read by an
+  electronic probe, not by software so it cannot be intercepted by
+  the CPU and reach Linux userspace.
+
+  Bandwidth depends on number of lines & bus frequency (for example on ux500
+  SoC 4 lines at max 100MHz eg max 400Mbit/s shared between 7 cores).
+  Transmit FIFO size: 256 samples up to 8 bytes.
+  On ux500 platform there is 2 contiguous STM blocks (eg 512 channels)
+
+Software Overview
+=================
+  Write atomicity and write order on STM trace channels is ensured by the fact
+  we try to allocate one channel by execution thread (no concurrent access).
+  There is 2 modes one lossless but intrusive aka Software mode and
+  another lossy mode less intrusive aka Hardware mode, by default
+  all sources are configured in Hardware mode and enabled.
+  The end of data packet is marked by a time stamp on latest byte(s) only.
+
+Kernel API
+----------
+  Configuration functions:
+    output trace clock frequency, trace mode, output port configuration
+    and enable/disable STM trace sources
+    Expose a debugfs interface too for STM trace control
+
+  Alloc/free STM trace channel functions
+
+  Set of low level atomic trace functions for 1, 2, 4 or 8 bytes
+  with & w/o time stamp
+
+  Higher level lockless trace functions:
+  stm_trace_buffer:
+     allocate a channel in 128 highest channels available
+     output the trace buffer with arbitrary length
+     (latest byte(s) automatically time stamped) then free the channel
+  stm_trace_buffer_onchannel:
+     use given channel to output the trace buffer
+     with arbitrary length (latest byte(s) automatically time stamped)
+
+  File IO output console like interface (open, close, write)
+
+  See <trace/stm.h> & drivers/misc/stm.c for more detail
+
+debugfs API
+-----------
+clockdiv:
+	This is used to set or display the current clock divisor
+	that is configured
+
+connection:
+	This is used to set or display the current output connector
+	that is configured (common values, 0 not connected, 1 for default
+	connection, 3 on Ux500 for APE MIPI34 connection)
+
+free_channels:
+	This is used to display the total number of free channels
+
+masters_enable:
+	This sets or displays whether the STM trace sources
+	are activated. Each bits represent the state of corresponding source:
+	0 for disable or 1 to enable it.
+
+masters_modes:
+	This sets or displays the STM trace sources modes.
+	Each bits represent the mode of corresponding source:
+	0 for Sofware lossless mode or 1 for Hardware lossy mode.
+
+User API
+--------
+  IOCTLs or debugfs for controls
+  2 levels API for tracing:
+     - Standard write function, in this case a channel is automatically
+       allocated at first write, after you can channel number
+       with IOCTL STM_GET_CHANNEL_NO
+     - mmap for direct access of all STM trace channels port plus
+       a set of IOCTLs for alloc/free channels, in this case you can
+       write your own lib to easiest its usage
+
+Examples of using the STM
+=========================
+First mount debugfs with:
+mount -t debugfs none /sys/kernel/debug
+
+In a shell scipt
+----------------
+It's as easy as:
+  echo "My trace point" > /dev/stm
+
+To avoid trace overflow, increase STM clock by decreasing the clockdiv with:
+  echo 1 >/sys/kernel/debug/stm/clockdiv  # now use DIV2 instead of default DIV8
+If not enough you can disable some sources with:
+  echo YourEnableSources > /sys/kernel/debug/stm/masters_enable
+If always not enough the ultime intrusive way is to change the sources mode
+and set the corresponding sources in Software mode (set corresponding source
+bit to 0) with:
+  echo YourModeSources > /sys/kernel/debug/stm/masters_modes
+  (be aware some source doesn't support Software mode => keep it in HW mode)
+
+NB: on Ux500 platform, first you have to configure STM output port to switch
+APE tracing on MIPI34 connector with:
+  echo 3 > /sys/kernel/debug/stm/connection
+
+In C language
+-------------
+
+The easy way more intrusive (with STM buffer recopy):
+
+#include <trace/stm.h>
+
+int fd, i;
+char buf[1024];  // Try to align this buffer on 64 bits if possible
+
+  fd = open("/dev/stm", O_WRONLY);
+  snprintf(buf, 1024, "STM0 Hello world\n");
+  write(fd, buf, strlen(buf));
+  ioctl(fd, STM_GET_CHANNEL_NO, &i);
+  snprintf(buf, 1024, "Use channel #%d\n", i);
+  write(fd, buf, strlen(buf));
+  close(fd);
+
+NB: You can call open("/dev/stm", O_WRONLY) as many times as necessary
+to allocate a different channel to avoid concurrency in your
+multithreaded application.
+
+The more efficient way, use mmap'ed STM channels memory (to put in a lib):
+
+#include <trace/stm.h>
+
+int fd, i, c, l, maxChannels;
+char buf[1024]; // Try to align this buffer on 64 bits if possible
+volatile struct stm_channel *channels;  // mmap'ed channels area
+
+  fd = open("/dev/stm", O_RDWR);
+  ioctl(fd0, STM_GET_NB_MAX_CHANNELS, &maxChannels);
+  channels = (struct stm_channel *)mmap(0, maxChannels*sizeof(*channels),
+	PROT_WRITE, MAP_SHARED, fd, 0);
+  assert(channels != MAP_FAILED);
+
+  if (!ioctl(fd, STM_GET_FREE_CHANNEL, &c)) {
+	l = snprintf(buf, 1024, "STM0 Hello world on channel #%d\n", c);
+	// lazy implementation you have to send buffer by 8 Bytes when possible
+	// and be sure you don't share this channel with others threads
+	for (i=0; i<l; i++) {
+		channels[c].stamp8 = buf[i];
+	}
+	ioctl(fd, STM_RELEASE_CHANNEL, c);
+  }
+  munmap((void *)channels, maxChannels*sizeof(*channels));
+  close(fd);
+
+Kernel Internal usages
+======================
+Dynamically channels dedicated for the kernel are allocated
+in the 128 highest ones
+
+Via menuconfig you can:
+- Duplicate printk output on a STM dedicated channel (255)
+- Have realtime ftrace output to a STM dedicated channel (254),
+  if corresponding TRACER is enabled
+- Have realtime sched context switch & sched wakeup output on dedicated channels
+  (253, 252),  if corresponding TRACER is enabled
+- Have Stack Trace on dedicated channels (251)
+- Duplicate trace_printk output on dedicated channels (250 & 249)
+
+
+And in the future:
+------------------
+- Use it in standard kernel tracing infrastucture,
+  possibilities:
+    - Insert other STM trace calls before trace ring buffer write
+    - Substitute time stamping & trace ring buffer by STM
+
diff --git a/Makefile b/Makefile
index 7d2192c9751..46b3447e277 100644
--- a/Makefile
+++ b/Makefile
@@ -947,7 +947,7 @@ $(vmlinux-dirs): prepare scripts
 # Store (new) KERNELRELASE string in include/config/kernel.release
 include/config/kernel.release: include/config/auto.conf FORCE
 	$(Q)rm -f $@
-	$(Q)echo "$(KERNELVERSION)$$($(CONFIG_SHELL) $(srctree)/scripts/setlocalversion $(srctree))" > $@
+	$(Q)echo "$(KERNELVERSION)$$($(CONFIG_SHELL) $(srctree)/scripts/setlocalversion -s $(srctree) -t v$(KERNELVERSION))" > $@
 
 
 # Things we need to do before we recursively start building the kernel
diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index cd0c9523496..41aa0b823eb 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -71,6 +71,10 @@ config KTIME_SCALAR
 	bool
 	default y
 
+config KTIME_SCALAR
+	bool
+	default y
+
 config HAVE_TCM
 	bool
 	select GENERIC_ALLOCATOR
@@ -822,7 +826,9 @@ config ARCH_U8500
 	select GENERIC_CLOCKEVENTS
 	select CLKDEV_LOOKUP
 	select ARCH_REQUIRE_GPIOLIB
+	select HAVE_CLK
 	select ARCH_HAS_CPUFREQ
+	select NOMADIK_GPIO
 	help
 	  Support for ST-Ericsson's Ux500 architecture
 
diff --git a/arch/arm/Kconfig.debug b/arch/arm/Kconfig.debug
index 81cbe40c159..5816849fa15 100644
--- a/arch/arm/Kconfig.debug
+++ b/arch/arm/Kconfig.debug
@@ -80,6 +80,15 @@ config EARLY_PRINTK
 	  kernel low-level debugging functions. Add earlyprintk to your
 	  kernel parameters to enable this console.
 
+config PRINTK_LL
+	bool "Use printascii in printk"
+	depends on DEBUG_LL
+	help
+	  Say Y here if you want to have printk send its output via the
+	  kernel low-level debugging functions.  This is useful if you
+	  are debugging code that executes before the earlyprintk console
+	  is initialized.
+
 config DEBUG_ICEDCC
 	bool "Kernel low-level debugging via EmbeddedICE DCC channel"
 	depends on DEBUG_LL
diff --git a/arch/arm/common/Makefile b/arch/arm/common/Makefile
index 3ab5d765fed..a1d44926837 100644
--- a/arch/arm/common/Makefile
+++ b/arch/arm/common/Makefile
@@ -19,3 +19,4 @@ obj-$(CONFIG_PCI_HOST_ITE8152)  += it8152.o
 obj-$(CONFIG_ARM_TIMER_SP804)	+= timer-sp.o
 obj-$(CONFIG_FIQ_GLUE)		+= fiq_glue.o fiq_glue_setup.o
 obj-$(CONFIG_FIQ_DEBUGGER)	+= fiq_debugger.o
+obj-$(CONFIG_BOOTTIME)		+= boottime.o
diff --git a/arch/arm/common/boottime.c b/arch/arm/common/boottime.c
new file mode 100644
index 00000000000..73e9e04ed37
--- /dev/null
+++ b/arch/arm/common/boottime.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2009-2010
+ *
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Store boot times measured during for example u-boot startup.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/boottime.h>
+#include <linux/string.h>
+#include <asm/setup.h>
+
+static u32 bootloader_idle;
+static u32 bootloader_total;
+
+static int __init boottime_parse_tag(const struct tag *tag)
+{
+	int i;
+	char buff[BOOTTIME_MAX_NAME_LEN];
+
+	bootloader_idle = tag->u.boottime.idle;
+	bootloader_total = tag->u.boottime.total;
+
+	for (i = 0; i < tag->u.boottime.num; i++) {
+		snprintf(buff, BOOTTIME_MAX_NAME_LEN, "%s+0x0/0x0",
+			 tag->u.boottime.entry[i].name);
+		buff[BOOTTIME_MAX_NAME_LEN - 1] = '\0';
+		boottime_mark_wtime(buff, tag->u.boottime.entry[i].time);
+	}
+
+	return 0;
+}
+
+__tagtable(ATAG_BOOTTIME,  boottime_parse_tag);
+
+int boottime_bootloader_idle(void)
+{
+	if (bootloader_total == 0)
+		return 0;
+
+	return (int) ((bootloader_idle) / (bootloader_total / 100));
+}
diff --git a/arch/arm/configs/android_omap3_defconfig b/arch/arm/configs/android_omap3_defconfig
deleted file mode 100644
index d4cec073b9e..00000000000
--- a/arch/arm/configs/android_omap3_defconfig
+++ /dev/null
@@ -1,234 +0,0 @@
-CONFIG_EXPERIMENTAL=y
-CONFIG_SYSVIPC=y
-CONFIG_POSIX_MQUEUE=y
-CONFIG_BSD_PROCESS_ACCT=y
-CONFIG_IKCONFIG=y
-CONFIG_IKCONFIG_PROC=y
-CONFIG_LOG_BUF_SHIFT=16
-CONFIG_BLK_DEV_INITRD=y
-# CONFIG_SYSCTL_SYSCALL is not set
-CONFIG_ASHMEM=y
-CONFIG_EMBEDDED=y
-CONFIG_PERF_COUNTERS=y
-CONFIG_SLAB=y
-CONFIG_PROFILING=y
-CONFIG_OPROFILE=y
-CONFIG_KPROBES=y
-CONFIG_MODULES=y
-CONFIG_MODULE_FORCE_LOAD=y
-CONFIG_MODULE_UNLOAD=y
-CONFIG_MODULE_FORCE_UNLOAD=y
-CONFIG_MODVERSIONS=y
-CONFIG_MODULE_SRCVERSION_ALL=y
-# CONFIG_BLK_DEV_BSG is not set
-CONFIG_ARCH_OMAP=y
-CONFIG_OMAP_RESET_CLOCKS=y
-# CONFIG_ARCH_OMAP2 is not set
-# CONFIG_ARCH_OMAP4 is not set
-CONFIG_NO_HZ=y
-CONFIG_HIGH_RES_TIMERS=y
-CONFIG_LEDS=y
-CONFIG_ZBOOT_ROM_TEXT=0x0
-CONFIG_ZBOOT_ROM_BSS=0x0
-CONFIG_CMDLINE="root=/dev/mmcblk0p2 rootwait console=ttyO2,115200"
-CONFIG_KEXEC=y
-CONFIG_CPU_FREQ=y
-CONFIG_CPU_IDLE=y
-CONFIG_FPE_NWFPE=y
-CONFIG_BINFMT_MISC=y
-CONFIG_WAKELOCK=y
-# CONFIG_EARLYSUSPEND is not set
-CONFIG_PM_DEBUG=y
-CONFIG_NET=y
-CONFIG_PACKET=y
-CONFIG_UNIX=y
-CONFIG_XFRM_USER=y
-CONFIG_NET_KEY=y
-CONFIG_NET_KEY_MIGRATE=y
-CONFIG_INET=y
-CONFIG_IP_MULTICAST=y
-CONFIG_IP_PNP=y
-CONFIG_IP_PNP_DHCP=y
-CONFIG_IP_PNP_BOOTP=y
-CONFIG_IP_PNP_RARP=y
-# CONFIG_INET_LRO is not set
-# CONFIG_IPV6 is not set
-CONFIG_NETFILTER=y
-CONFIG_CFG80211=y
-CONFIG_MAC80211=y
-CONFIG_MAC80211_RC_PID=y
-CONFIG_MAC80211_RC_DEFAULT_PID=y
-CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
-CONFIG_CONNECTOR=y
-CONFIG_MTD=y
-CONFIG_MTD_CMDLINE_PARTS=y
-CONFIG_MTD_CHAR=y
-CONFIG_MTD_BLOCK=y
-CONFIG_MTD_OOPS=y
-CONFIG_MTD_CFI=y
-CONFIG_MTD_CFI_INTELEXT=y
-CONFIG_MTD_NAND=y
-CONFIG_MTD_NAND_OMAP2=y
-CONFIG_MTD_ONENAND=y
-CONFIG_MTD_ONENAND_VERIFY_WRITE=y
-CONFIG_MTD_ONENAND_OMAP2=y
-CONFIG_MTD_UBI=y
-CONFIG_BLK_DEV_LOOP=y
-CONFIG_BLK_DEV_RAM=y
-CONFIG_BLK_DEV_RAM_SIZE=16384
-CONFIG_SCSI=y
-CONFIG_BLK_DEV_SD=y
-CONFIG_SCSI_MULTI_LUN=y
-CONFIG_SCSI_SCAN_ASYNC=y
-CONFIG_MD=y
-CONFIG_NETDEVICES=y
-CONFIG_SMSC_PHY=y
-CONFIG_NET_ETHERNET=y
-CONFIG_SMC91X=y
-CONFIG_SMC911X=y
-CONFIG_SMSC911X=y
-CONFIG_KS8851=y
-CONFIG_KS8851_MLL=y
-CONFIG_LIBERTAS=y
-CONFIG_LIBERTAS_USB=y
-CONFIG_LIBERTAS_SDIO=y
-CONFIG_LIBERTAS_DEBUG=y
-CONFIG_USB_USBNET=y
-CONFIG_USB_NET_SMSC75XX=y
-CONFIG_USB_NET_SMSC95XX=y
-CONFIG_USB_ALI_M5632=y
-CONFIG_USB_AN2720=y
-CONFIG_USB_EPSON2888=y
-CONFIG_USB_KC2190=y
-CONFIG_INPUT_JOYDEV=y
-CONFIG_INPUT_EVDEV=y
-CONFIG_KEYBOARD_GPIO=y
-CONFIG_KEYBOARD_TWL4030=y
-CONFIG_INPUT_TOUCHSCREEN=y
-CONFIG_TOUCHSCREEN_ADS7846=y
-CONFIG_INPUT_MISC=y
-CONFIG_INPUT_TWL4030_PWRBUTTON=y
-CONFIG_INPUT_UINPUT=y
-CONFIG_INPUT_GPIO=y
-CONFIG_VT_HW_CONSOLE_BINDING=y
-# CONFIG_LEGACY_PTYS is not set
-CONFIG_SERIAL_8250_NR_UARTS=32
-CONFIG_SERIAL_8250_EXTENDED=y
-CONFIG_SERIAL_8250_MANY_PORTS=y
-CONFIG_SERIAL_8250_SHARE_IRQ=y
-CONFIG_SERIAL_8250_DETECT_IRQ=y
-CONFIG_SERIAL_8250_RSA=y
-CONFIG_HW_RANDOM=y
-CONFIG_I2C_CHARDEV=y
-CONFIG_SPI=y
-CONFIG_SPI_OMAP24XX=y
-CONFIG_DEBUG_GPIO=y
-CONFIG_GPIO_SYSFS=y
-CONFIG_GPIO_TWL4030=y
-CONFIG_W1=y
-CONFIG_POWER_SUPPLY=y
-CONFIG_WATCHDOG=y
-CONFIG_OMAP_WATCHDOG=y
-CONFIG_TWL4030_WATCHDOG=y
-CONFIG_REGULATOR_TWL4030=y
-CONFIG_REGULATOR_TPS65023=y
-CONFIG_REGULATOR_TPS6507X=y
-CONFIG_FB=y
-CONFIG_FIRMWARE_EDID=y
-CONFIG_FB_MODE_HELPERS=y
-CONFIG_FB_TILEBLITTING=y
-CONFIG_FB_OMAP_LCD_VGA=y
-CONFIG_OMAP2_DSS=y
-CONFIG_OMAP2_VRAM_SIZE=4
-CONFIG_FB_OMAP2=y
-CONFIG_FB_OMAP2_NUM_FBS=1
-CONFIG_PANEL_GENERIC_DPI=y
-CONFIG_BACKLIGHT_LCD_SUPPORT=y
-CONFIG_LCD_CLASS_DEVICE=y
-CONFIG_LCD_PLATFORM=y
-CONFIG_DISPLAY_SUPPORT=y
-CONFIG_LOGO=y
-CONFIG_SOUND=y
-CONFIG_SND=y
-CONFIG_SND_MIXER_OSS=y
-CONFIG_SND_PCM_OSS=y
-CONFIG_SND_VERBOSE_PRINTK=y
-CONFIG_SND_DEBUG=y
-CONFIG_SND_USB_AUDIO=y
-CONFIG_SND_SOC=y
-CONFIG_SND_OMAP_SOC=y
-CONFIG_SND_OMAP_SOC_OMAP3_PANDORA=y
-CONFIG_USB=y
-CONFIG_USB_DEBUG=y
-CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
-CONFIG_USB_SUSPEND=y
-# CONFIG_USB_OTG_WHITELIST is not set
-CONFIG_USB_MON=y
-CONFIG_USB_EHCI_HCD=y
-CONFIG_USB_EHCI_ROOT_HUB_TT=y
-CONFIG_USB_MUSB_HDRC=y
-CONFIG_USB_MUSB_OMAP2PLUS=y
-CONFIG_USB_MUSB_OTG=y
-CONFIG_USB_GADGET_MUSB_HDRC=y
-CONFIG_MUSB_PIO_ONLY=y
-CONFIG_USB_WDM=y
-CONFIG_USB_STORAGE=y
-CONFIG_USB_LIBUSUAL=y
-CONFIG_USB_TEST=y
-CONFIG_USB_GADGET=y
-CONFIG_USB_GADGET_DEBUG=y
-CONFIG_USB_GADGET_DEBUG_FILES=y
-CONFIG_USB_GADGET_DEBUG_FS=y
-CONFIG_USB_G_ANDROID=y
-CONFIG_MMC=y
-CONFIG_MMC_UNSAFE_RESUME=y
-CONFIG_SDIO_UART=y
-CONFIG_MMC_OMAP=y
-CONFIG_MMC_OMAP_HS=y
-CONFIG_SWITCH=y
-CONFIG_RTC_CLASS=y
-CONFIG_RTC_DRV_TWL4030=y
-CONFIG_STAGING=y
-CONFIG_ANDROID=y
-CONFIG_ANDROID_BINDER_IPC=y
-CONFIG_ANDROID_LOGGER=y
-CONFIG_ANDROID_RAM_CONSOLE=y
-CONFIG_ANDROID_TIMED_GPIO=y
-CONFIG_ANDROID_LOW_MEMORY_KILLER=y
-CONFIG_EXT2_FS=y
-CONFIG_EXT3_FS=y
-CONFIG_EXT4_FS=y
-CONFIG_BTRFS_FS=y
-CONFIG_QUOTA=y
-CONFIG_QFMT_V2=y
-CONFIG_MSDOS_FS=y
-CONFIG_VFAT_FS=y
-CONFIG_TMPFS=y
-CONFIG_JFFS2_FS=y
-CONFIG_JFFS2_SUMMARY=y
-CONFIG_JFFS2_FS_XATTR=y
-CONFIG_JFFS2_COMPRESSION_OPTIONS=y
-CONFIG_JFFS2_LZO=y
-CONFIG_JFFS2_RUBIN=y
-CONFIG_UBIFS_FS=y
-CONFIG_CRAMFS=y
-CONFIG_PARTITION_ADVANCED=y
-CONFIG_NLS_CODEPAGE_437=y
-CONFIG_NLS_ISO8859_1=y
-CONFIG_PRINTK_TIME=y
-CONFIG_MAGIC_SYSRQ=y
-CONFIG_DEBUG_KERNEL=y
-CONFIG_SCHEDSTATS=y
-CONFIG_TIMER_STATS=y
-CONFIG_PROVE_LOCKING=y
-CONFIG_DEBUG_SPINLOCK_SLEEP=y
-# CONFIG_DEBUG_BUGVERBOSE is not set
-CONFIG_DEBUG_INFO=y
-CONFIG_SECURITY=y
-CONFIG_CRYPTO_MICHAEL_MIC=y
-# CONFIG_CRYPTO_ANSI_CPRNG is not set
-CONFIG_CRC_CCITT=y
-CONFIG_CRC_T10DIF=y
-CONFIG_CRC_ITU_T=y
-CONFIG_CRC7=y
diff --git a/arch/arm/configs/android_omap4_defconfig b/arch/arm/configs/android_omap4_defconfig
deleted file mode 100644
index 518cd1694ac..00000000000
--- a/arch/arm/configs/android_omap4_defconfig
+++ /dev/null
@@ -1,235 +0,0 @@
-CONFIG_EXPERIMENTAL=y
-CONFIG_SYSVIPC=y
-CONFIG_POSIX_MQUEUE=y
-CONFIG_BSD_PROCESS_ACCT=y
-CONFIG_IKCONFIG=y
-CONFIG_IKCONFIG_PROC=y
-CONFIG_LOG_BUF_SHIFT=16
-CONFIG_BLK_DEV_INITRD=y
-# CONFIG_SYSCTL_SYSCALL is not set
-CONFIG_ASHMEM=y
-CONFIG_EMBEDDED=y
-CONFIG_PERF_COUNTERS=y
-CONFIG_SLAB=y
-CONFIG_PROFILING=y
-CONFIG_OPROFILE=y
-CONFIG_KPROBES=y
-CONFIG_MODULES=y
-CONFIG_MODULE_FORCE_LOAD=y
-CONFIG_MODULE_UNLOAD=y
-CONFIG_MODULE_FORCE_UNLOAD=y
-CONFIG_MODVERSIONS=y
-CONFIG_MODULE_SRCVERSION_ALL=y
-# CONFIG_BLK_DEV_BSG is not set
-CONFIG_ARCH_OMAP=y
-# CONFIG_ARCH_OMAP2 is not set
-CONFIG_NO_HZ=y
-CONFIG_HIGH_RES_TIMERS=y
-CONFIG_SMP=y
-CONFIG_SCHED_MC=y
-CONFIG_LEDS=y
-CONFIG_ZBOOT_ROM_TEXT=0x0
-CONFIG_ZBOOT_ROM_BSS=0x0
-CONFIG_CMDLINE="root=/dev/mmcblk0p2 rootwait console=ttyO2,115200"
-CONFIG_KEXEC=y
-CONFIG_CPU_FREQ=y
-CONFIG_CPU_IDLE=y
-CONFIG_FPE_NWFPE=y
-CONFIG_BINFMT_MISC=y
-CONFIG_WAKELOCK=y
-# CONFIG_EARLYSUSPEND is not set
-CONFIG_PM_DEBUG=y
-CONFIG_NET=y
-CONFIG_PACKET=y
-CONFIG_UNIX=y
-CONFIG_XFRM_USER=y
-CONFIG_NET_KEY=y
-CONFIG_NET_KEY_MIGRATE=y
-CONFIG_INET=y
-CONFIG_IP_MULTICAST=y
-CONFIG_IP_PNP=y
-CONFIG_IP_PNP_DHCP=y
-CONFIG_IP_PNP_BOOTP=y
-CONFIG_IP_PNP_RARP=y
-# CONFIG_INET_LRO is not set
-# CONFIG_IPV6 is not set
-CONFIG_NETFILTER=y
-CONFIG_CFG80211=y
-CONFIG_MAC80211=y
-CONFIG_MAC80211_RC_PID=y
-CONFIG_MAC80211_RC_DEFAULT_PID=y
-CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
-CONFIG_CONNECTOR=y
-CONFIG_MTD=y
-CONFIG_MTD_CMDLINE_PARTS=y
-CONFIG_MTD_CHAR=y
-CONFIG_MTD_BLOCK=y
-CONFIG_MTD_OOPS=y
-CONFIG_MTD_CFI=y
-CONFIG_MTD_CFI_INTELEXT=y
-CONFIG_MTD_NAND=y
-CONFIG_MTD_NAND_OMAP2=y
-CONFIG_MTD_ONENAND=y
-CONFIG_MTD_ONENAND_VERIFY_WRITE=y
-CONFIG_MTD_ONENAND_OMAP2=y
-CONFIG_MTD_UBI=y
-CONFIG_BLK_DEV_LOOP=y
-CONFIG_BLK_DEV_RAM=y
-CONFIG_BLK_DEV_RAM_SIZE=16384
-CONFIG_SCSI=y
-CONFIG_BLK_DEV_SD=y
-CONFIG_SCSI_MULTI_LUN=y
-CONFIG_SCSI_SCAN_ASYNC=y
-CONFIG_MD=y
-CONFIG_NETDEVICES=y
-CONFIG_SMSC_PHY=y
-CONFIG_NET_ETHERNET=y
-CONFIG_SMC91X=y
-CONFIG_SMC911X=y
-CONFIG_SMSC911X=y
-CONFIG_KS8851=y
-CONFIG_KS8851_MLL=y
-CONFIG_LIBERTAS=y
-CONFIG_LIBERTAS_USB=y
-CONFIG_LIBERTAS_SDIO=y
-CONFIG_LIBERTAS_DEBUG=y
-CONFIG_USB_USBNET=y
-CONFIG_USB_NET_SMSC75XX=y
-CONFIG_USB_NET_SMSC95XX=y
-CONFIG_USB_ALI_M5632=y
-CONFIG_USB_AN2720=y
-CONFIG_USB_EPSON2888=y
-CONFIG_USB_KC2190=y
-CONFIG_INPUT_JOYDEV=y
-CONFIG_INPUT_EVDEV=y
-CONFIG_KEYBOARD_GPIO=y
-CONFIG_KEYBOARD_TWL4030=y
-CONFIG_INPUT_TOUCHSCREEN=y
-CONFIG_TOUCHSCREEN_ADS7846=y
-CONFIG_INPUT_MISC=y
-CONFIG_INPUT_TWL4030_PWRBUTTON=y
-CONFIG_INPUT_UINPUT=y
-CONFIG_INPUT_GPIO=y
-CONFIG_VT_HW_CONSOLE_BINDING=y
-# CONFIG_LEGACY_PTYS is not set
-CONFIG_SERIAL_8250_NR_UARTS=32
-CONFIG_SERIAL_8250_EXTENDED=y
-CONFIG_SERIAL_8250_MANY_PORTS=y
-CONFIG_SERIAL_8250_SHARE_IRQ=y
-CONFIG_SERIAL_8250_DETECT_IRQ=y
-CONFIG_SERIAL_8250_RSA=y
-CONFIG_HW_RANDOM=y
-CONFIG_I2C_CHARDEV=y
-CONFIG_SPI=y
-CONFIG_SPI_OMAP24XX=y
-CONFIG_DEBUG_GPIO=y
-CONFIG_GPIO_SYSFS=y
-CONFIG_GPIO_TWL4030=y
-CONFIG_W1=y
-CONFIG_POWER_SUPPLY=y
-CONFIG_WATCHDOG=y
-CONFIG_OMAP_WATCHDOG=y
-CONFIG_TWL4030_WATCHDOG=y
-CONFIG_REGULATOR_TWL4030=y
-CONFIG_REGULATOR_TPS65023=y
-CONFIG_REGULATOR_TPS6507X=y
-CONFIG_FB=y
-CONFIG_FIRMWARE_EDID=y
-CONFIG_FB_MODE_HELPERS=y
-CONFIG_FB_TILEBLITTING=y
-CONFIG_FB_OMAP_LCD_VGA=y
-CONFIG_OMAP2_DSS=y
-CONFIG_OMAP2_VRAM_SIZE=4
-# CONFIG_OMAP2_DSS_VENC is not set
-CONFIG_FB_OMAP2=y
-CONFIG_FB_OMAP2_NUM_FBS=1
-CONFIG_PANEL_GENERIC_DPI=y
-CONFIG_BACKLIGHT_LCD_SUPPORT=y
-CONFIG_LCD_CLASS_DEVICE=y
-CONFIG_LCD_PLATFORM=y
-CONFIG_DISPLAY_SUPPORT=y
-CONFIG_LOGO=y
-CONFIG_SOUND=y
-CONFIG_SND=y
-CONFIG_SND_MIXER_OSS=y
-CONFIG_SND_PCM_OSS=y
-CONFIG_SND_VERBOSE_PRINTK=y
-CONFIG_SND_DEBUG=y
-CONFIG_SND_USB_AUDIO=y
-CONFIG_SND_SOC=y
-CONFIG_SND_OMAP_SOC=y
-CONFIG_SND_OMAP_SOC_OMAP3_PANDORA=y
-CONFIG_USB=y
-CONFIG_USB_DEBUG=y
-CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
-CONFIG_USB_SUSPEND=y
-# CONFIG_USB_OTG_WHITELIST is not set
-CONFIG_USB_MON=y
-CONFIG_USB_EHCI_HCD=y
-CONFIG_USB_EHCI_ROOT_HUB_TT=y
-CONFIG_USB_MUSB_HDRC=y
-CONFIG_USB_MUSB_OMAP2PLUS=y
-CONFIG_USB_MUSB_OTG=y
-CONFIG_USB_GADGET_MUSB_HDRC=y
-CONFIG_MUSB_PIO_ONLY=y
-CONFIG_USB_WDM=y
-CONFIG_USB_STORAGE=y
-CONFIG_USB_LIBUSUAL=y
-CONFIG_USB_TEST=y
-CONFIG_USB_GADGET=y
-CONFIG_USB_GADGET_DEBUG=y
-CONFIG_USB_GADGET_DEBUG_FILES=y
-CONFIG_USB_GADGET_DEBUG_FS=y
-CONFIG_USB_G_ANDROID=y
-CONFIG_MMC=y
-CONFIG_MMC_UNSAFE_RESUME=y
-CONFIG_SDIO_UART=y
-CONFIG_MMC_OMAP=y
-CONFIG_MMC_OMAP_HS=y
-CONFIG_SWITCH=y
-CONFIG_RTC_CLASS=y
-CONFIG_RTC_DRV_TWL4030=y
-CONFIG_STAGING=y
-CONFIG_ANDROID=y
-CONFIG_ANDROID_BINDER_IPC=y
-CONFIG_ANDROID_LOGGER=y
-CONFIG_ANDROID_RAM_CONSOLE=y
-CONFIG_ANDROID_TIMED_GPIO=y
-CONFIG_ANDROID_LOW_MEMORY_KILLER=y
-CONFIG_EXT2_FS=y
-CONFIG_EXT3_FS=y
-CONFIG_EXT4_FS=y
-CONFIG_BTRFS_FS=y
-CONFIG_QUOTA=y
-CONFIG_QFMT_V2=y
-CONFIG_MSDOS_FS=y
-CONFIG_VFAT_FS=y
-CONFIG_TMPFS=y
-CONFIG_JFFS2_FS=y
-CONFIG_JFFS2_SUMMARY=y
-CONFIG_JFFS2_FS_XATTR=y
-CONFIG_JFFS2_COMPRESSION_OPTIONS=y
-CONFIG_JFFS2_LZO=y
-CONFIG_JFFS2_RUBIN=y
-CONFIG_UBIFS_FS=y
-CONFIG_CRAMFS=y
-CONFIG_PARTITION_ADVANCED=y
-CONFIG_NLS_CODEPAGE_437=y
-CONFIG_NLS_ISO8859_1=y
-CONFIG_PRINTK_TIME=y
-CONFIG_MAGIC_SYSRQ=y
-CONFIG_DEBUG_KERNEL=y
-CONFIG_SCHEDSTATS=y
-CONFIG_TIMER_STATS=y
-CONFIG_PROVE_LOCKING=y
-CONFIG_DEBUG_SPINLOCK_SLEEP=y
-# CONFIG_DEBUG_BUGVERBOSE is not set
-CONFIG_DEBUG_INFO=y
-CONFIG_SECURITY=y
-CONFIG_CRYPTO_MICHAEL_MIC=y
-# CONFIG_CRYPTO_ANSI_CPRNG is not set
-CONFIG_CRC_CCITT=y
-CONFIG_CRC_T10DIF=y
-CONFIG_CRC_ITU_T=y
-CONFIG_CRC7=y
diff --git a/arch/arm/configs/u8500_defconfig b/arch/arm/configs/u8500_defconfig
index a5cce242a77..9bba2c91e99 100644
--- a/arch/arm/configs/u8500_defconfig
+++ b/arch/arm/configs/u8500_defconfig
@@ -1,105 +1,313 @@
 CONFIG_EXPERIMENTAL=y
+# CONFIG_LOCALVERSION_AUTO is not set
 # CONFIG_SWAP is not set
 CONFIG_SYSVIPC=y
+CONFIG_BSD_PROCESS_ACCT=y
+CONFIG_LOG_BUF_SHIFT=18
+CONFIG_CGROUPS=y
+CONFIG_CGROUP_CPUACCT=y
+CONFIG_RELAY=y
 CONFIG_BLK_DEV_INITRD=y
 CONFIG_KALLSYMS_ALL=y
+CONFIG_ASHMEM=y
+CONFIG_EMBEDDED=y
+CONFIG_PERF_EVENTS=y
+CONFIG_SLAB=y
+CONFIG_BOOTTIME=y
 CONFIG_MODULES=y
 CONFIG_MODULE_UNLOAD=y
-# CONFIG_LBDAF is not set
+CONFIG_MODVERSIONS=y
+CONFIG_MODULE_SRCVERSION_ALL=y
 # CONFIG_BLK_DEV_BSG is not set
+CONFIG_DEFAULT_DEADLINE=y
 CONFIG_ARCH_U8500=y
 CONFIG_UX500_SOC_DB5500=y
 CONFIG_UX500_SOC_DB8500=y
-CONFIG_MACH_U8500=y
+CONFIG_MACH_HREFV60=y
+CONFIG_MACH_SNOWBALL=y
 CONFIG_MACH_U5500=y
+CONFIG_UX500_PRCMU_TIMER=y
+CONFIG_DB8500_MLOADER=y
+CONFIG_U5500_MLOADER=y
+CONFIG_MCDE_DISPLAY_PRIMARY_32BPP=y
+CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_VSYNC=y
+CONFIG_DISPLAY_AV8100_TERTIARY=y
+CONFIG_DISPLAY_AV8100_TRIPPLE_BUFFER=y
+CONFIG_UX500_SUSPEND=y
+CONFIG_UX500_SUSPEND_STANDBY=y
+CONFIG_UX500_SUSPEND_MEM=y
 CONFIG_NO_HZ=y
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_SMP=y
 CONFIG_NR_CPUS=2
-CONFIG_HOTPLUG_CPU=y
 CONFIG_PREEMPT=y
 CONFIG_AEABI=y
-CONFIG_CMDLINE="root=/dev/ram0 console=ttyAMA2,115200n8"
+CONFIG_HIGHMEM=y
+CONFIG_CMDLINE="root=/dev/ram0 init=init rw console=ttyAMA2,115200n8 mem=256M initrd=0x800000,72M"
+CONFIG_KEXEC=y
+CONFIG_CRASH_DUMP=y
 CONFIG_CPU_FREQ=y
+CONFIG_CPU_FREQ_STAT_DETAILS=y
 CONFIG_CPU_FREQ_DEFAULT_GOV_ONDEMAND=y
+CONFIG_CPU_FREQ_GOV_POWERSAVE=y
+CONFIG_CPU_FREQ_GOV_USERSPACE=y
+CONFIG_CPU_FREQ_GOV_CONSERVATIVE=y
+CONFIG_CPU_IDLE=y
+CONFIG_FPE_NWFPE=y
 CONFIG_VFP=y
 CONFIG_NEON=y
+# CONFIG_CORE_DUMP_DEFAULT_ELF_HEADERS is not set
+CONFIG_WAKELOCK=y
+CONFIG_PM_RUNTIME=y
 CONFIG_NET=y
+CONFIG_PACKET=y
+CONFIG_UNIX=y
+CONFIG_INET=y
+CONFIG_IP_PNP=y
+CONFIG_IP_PNP_DHCP=y
+# CONFIG_INET_LRO is not set
+CONFIG_IPV6=y
+# CONFIG_INET6_XFRM_MODE_TRANSPORT is not set
+# CONFIG_INET6_XFRM_MODE_TUNNEL is not set
+# CONFIG_INET6_XFRM_MODE_BEET is not set
+# CONFIG_IPV6_SIT is not set
+CONFIG_NETFILTER=y
+CONFIG_NF_CONNTRACK=y
+CONFIG_NETFILTER_XT_MATCH_MULTIPORT=y
+CONFIG_NETFILTER_XT_MATCH_STATE=y
+CONFIG_NF_CONNTRACK_IPV4=y
+CONFIG_IP_NF_IPTABLES=y
+CONFIG_IP_NF_FILTER=y
+CONFIG_NF_NAT=y
+CONFIG_IP_NF_TARGET_MASQUERADE=y
 CONFIG_PHONET=y
-CONFIG_PHONET_PIPECTRLR=y
-# CONFIG_WIRELESS is not set
+CONFIG_NET_SCHED=y
+CONFIG_BT_L2CAP=y
+CONFIG_BT_SCO=y
+CONFIG_BT_RFCOMM=y
+CONFIG_BT_RFCOMM_TTY=y
+CONFIG_BT_BNEP=y
+CONFIG_BT_BNEP_MC_FILTER=y
+CONFIG_BT_BNEP_PROTO_FILTER=y
+CONFIG_BT_HIDP=y
+CONFIG_CFG80211=m
+CONFIG_MAC80211=m
+CONFIG_MAC80211_LEDS=y
+CONFIG_RFKILL=y
+CONFIG_RFKILL_INPUT=y
+CONFIG_NET_9P=y
 CONFIG_CAIF=y
 CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
+# CONFIG_STANDALONE is not set
+CONFIG_BLK_DEV_LOOP=y
 CONFIG_BLK_DEV_RAM=y
-CONFIG_BLK_DEV_RAM_SIZE=65536
+CONFIG_BLK_DEV_RAM_SIZE=73728
 CONFIG_MISC_DEVICES=y
 CONFIG_AB8500_PWM=y
 CONFIG_SENSORS_BH1780=y
-# CONFIG_INPUT_MOUSEDEV_PSAUX is not set
+CONFIG_STE_TRACE_MODEM=y
+CONFIG_U8500_SIM_DETECT=y
+CONFIG_STM_TRACE=y
+# CONFIG_STE_AUDIO_IO_DEV is not set
+CONFIG_SCSI=y
+CONFIG_BLK_DEV_SD=y
+CONFIG_MD=y
+CONFIG_BLK_DEV_DM=y
+CONFIG_DM_CRYPT=y
+CONFIG_DM_UEVENT=y
+CONFIG_NETDEVICES=y
+CONFIG_TUN=y
+CONFIG_SMSC_PHY=y
+CONFIG_NET_ETHERNET=y
+CONFIG_SMSC911X=y
+# CONFIG_NETDEV_1000 is not set
+# CONFIG_NETDEV_10000 is not set
+# CONFIG_WLAN is not set
+# CONFIG_INPUT_MOUSEDEV is not set
 CONFIG_INPUT_EVDEV=y
 # CONFIG_KEYBOARD_ATKBD is not set
 CONFIG_KEYBOARD_GPIO=y
-CONFIG_KEYBOARD_NOMADIK=y
+CONFIG_KEYBOARD_NOMADIK_SKE=y
 CONFIG_KEYBOARD_STMPE=y
 CONFIG_KEYBOARD_TC3589X=y
 # CONFIG_INPUT_MOUSE is not set
 CONFIG_INPUT_TOUCHSCREEN=y
 CONFIG_TOUCHSCREEN_BU21013=y
 CONFIG_INPUT_MISC=y
+CONFIG_INPUT_AB8500_ACCDET=y
 CONFIG_INPUT_AB8500_PONKEY=y
-# CONFIG_SERIO is not set
+CONFIG_INPUT_UINPUT=y
 CONFIG_VT_HW_CONSOLE_BINDING=y
+# CONFIG_LEGACY_PTYS is not set
+# CONFIG_DEVKMEM is not set
 CONFIG_SERIAL_AMBA_PL011=y
 CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
-# CONFIG_LEGACY_PTYS is not set
 CONFIG_HW_RANDOM=y
 CONFIG_HW_RANDOM_NOMADIK=y
 CONFIG_I2C=y
+CONFIG_I2C_CHARDEV=y
 CONFIG_I2C_NOMADIK=y
 CONFIG_SPI=y
+# CONFIG_STM_MSP_SPI is not set
 CONFIG_SPI_PL022=y
-CONFIG_GPIO_STMPE=y
+CONFIG_GPIO_SYSFS=y
 CONFIG_GPIO_TC3589X=y
-# CONFIG_HWMON is not set
+CONFIG_GPIO_AB8500=y
+CONFIG_POWER_SUPPLY=y
+CONFIG_AB8500_BM=y
+CONFIG_AB8500_BATTERY_THERM_ON_BATCTRL=y
+CONFIG_AB5500_BM=y
+CONFIG_AB5500_BATTERY_THERM_ON_BATCTRL=y
+CONFIG_SENSORS_AB8500=y
+CONFIG_SENSORS_DB8500=y
+CONFIG_SENSORS_LSM303DLH=y
+CONFIG_SENSORS_L3G4200D=y
+CONFIG_WATCHDOG=y
 CONFIG_MFD_STMPE=y
 CONFIG_MFD_TC3589X=y
+CONFIG_AB5500_CORE=y
 CONFIG_AB8500_CORE=y
-CONFIG_REGULATOR=y
+CONFIG_REGULATOR_DEBUG=y
+CONFIG_REGULATOR_VIRTUAL_CONSUMER=y
 CONFIG_REGULATOR_AB8500=y
-# CONFIG_HID_SUPPORT is not set
-# CONFIG_USB_SUPPORT is not set
+CONFIG_MEDIA_SUPPORT=y
+CONFIG_VIDEO_DEV=y
+# CONFIG_MEDIA_TUNER_CUSTOMISE is not set
+# CONFIG_VIDEO_CAPTURE_DRIVERS is not set
+CONFIG_FB=y
+CONFIG_FB_MCDE=y
+CONFIG_AV8100_HWTRIG_I2SDAT3=y
+CONFIG_FB_B2R2=y
+CONFIG_B2R2_PLUG_CONF=y
+CONFIG_SOUND=y
+CONFIG_SND=y
+CONFIG_SND_USB_AUDIO=y
+CONFIG_SND_SOC=y
+CONFIG_SND_SOC_UX500=y
+CONFIG_SND_SOC_UX500_AB5500=y
+CONFIG_SND_SOC_UX500_AB8500=y
+CONFIG_SND_SOC_UX500_CG29XX=y
+CONFIG_SND_SOC_UX500_AV8100=y
+CONFIG_USB=y
+# CONFIG_USB_DEVICE_CLASS is not set
+CONFIG_USB_SUSPEND=y
+# CONFIG_USB_OTG_WHITELIST is not set
+CONFIG_USB_MON=y
+CONFIG_USB_MUSB_HDRC=y
+CONFIG_USB_MUSB_UX500=y
+CONFIG_USB_MUSB_OTG=y
+CONFIG_USB_GADGET_MUSB_HDRC=y
+CONFIG_USB_ACM=y
+CONFIG_USB_STORAGE=y
+CONFIG_USB_LIBUSUAL=y
+CONFIG_USB_GADGET=y
+CONFIG_USB_GADGET_VBUS_DRAW=500
+CONFIG_USB_G_ANDROID=y
+CONFIG_AB8500_USB=y
 CONFIG_MMC=y
+CONFIG_MMC_UNSAFE_RESUME=y
+CONFIG_MMC_CLKGATE=y
+CONFIG_MMC_PARANOID_SD_INIT=y
+# CONFIG_MMC_BLOCK_BOUNCE is not set
 CONFIG_MMC_ARMMMCI=y
 CONFIG_NEW_LEDS=y
 CONFIG_LEDS_CLASS=y
+CONFIG_LEDS_LM3530=y
 CONFIG_LEDS_LP5521=y
+CONFIG_LEDS_PWM=y
+CONFIG_LEDS_TRIGGER_TIMER=y
+CONFIG_SWITCH=y
 CONFIG_RTC_CLASS=y
+CONFIG_RTC_DRV_AB=y
 CONFIG_RTC_DRV_AB8500=y
-CONFIG_RTC_DRV_PL031=y
 CONFIG_DMADEVICES=y
 CONFIG_STE_DMA40=y
 CONFIG_STAGING=y
-# CONFIG_STAGING_EXCLUDE_BUILD is not set
+CONFIG_AB5500_SIM=y
+CONFIG_ANDROID=y
+CONFIG_ANDROID_BINDER_IPC=y
+CONFIG_ANDROID_LOGGER=y
+CONFIG_ANDROID_RAM_CONSOLE=y
+CONFIG_ANDROID_TIMED_GPIO=y
+CONFIG_ANDROID_LOW_MEMORY_KILLER=y
 CONFIG_TOUCHSCREEN_SYNAPTICS_I2C_RMI4=y
+CONFIG_CG2900=y
+CONFIG_CG2900_CHIP=y
+CONFIG_STLC2690_CHIP=y
+CONFIG_CG2900_UART=y
+CONFIG_CG2900_AUDIO=y
+CONFIG_CG2900_TEST=y
+CONFIG_BT_CG2900=y
+CONFIG_U8500_MMIO=y
+CONFIG_U5500_MMIO=y
+CONFIG_U8500_CM=y
+CONFIG_U8500_FLASH=y
+CONFIG_MODEM=y
+CONFIG_MODEM_U8500=y
+CONFIG_U8500_SHRM=y
+CONFIG_U8500_SHRM_MODEM_SILENT_RESET=y
 CONFIG_EXT2_FS=y
 CONFIG_EXT2_FS_XATTR=y
 CONFIG_EXT2_FS_POSIX_ACL=y
 CONFIG_EXT2_FS_SECURITY=y
 CONFIG_EXT3_FS=y
+# CONFIG_EXT3_DEFAULTS_TO_ORDERED is not set
+CONFIG_EXT3_FS_POSIX_ACL=y
+CONFIG_EXT4_FS=y
+CONFIG_EXT4_FS_POSIX_ACL=y
+CONFIG_QUOTA=y
+CONFIG_QFMT_V2=y
+CONFIG_AUTOFS4_FS=m
+CONFIG_ISO9660_FS=y
+CONFIG_JOLIET=y
+CONFIG_ZISOFS=y
+CONFIG_UDF_FS=m
+CONFIG_MSDOS_FS=y
 CONFIG_VFAT_FS=y
 CONFIG_TMPFS=y
 CONFIG_TMPFS_POSIX_ACL=y
-CONFIG_CONFIGFS_FS=m
-# CONFIG_MISC_FILESYSTEMS is not set
+CONFIG_HFS_FS=m
+CONFIG_BEFS_FS=m
+CONFIG_CRAMFS=m
+CONFIG_VXFS_FS=m
+CONFIG_MINIX_FS=m
+CONFIG_ROMFS_FS=m
+CONFIG_SYSV_FS=m
+CONFIG_UFS_FS=m
+CONFIG_9P_FS=y
+CONFIG_PARTITION_ADVANCED=y
+CONFIG_BLKDEV_PARTITION=y
+CONFIG_BSD_DISKLABEL=y
+CONFIG_MINIX_SUBPARTITION=y
+CONFIG_SOLARIS_X86_PARTITION=y
+CONFIG_UNIXWARE_DISKLABEL=y
+CONFIG_SGI_PARTITION=y
+CONFIG_SUN_PARTITION=y
 CONFIG_NLS_CODEPAGE_437=y
 CONFIG_NLS_ISO8859_1=y
+CONFIG_PRINTK_TIME=y
 CONFIG_MAGIC_SYSRQ=y
-CONFIG_DEBUG_FS=y
+CONFIG_UNUSED_SYMBOLS=y
 CONFIG_DEBUG_KERNEL=y
-# CONFIG_SCHED_DEBUG is not set
+CONFIG_DETECT_HUNG_TASK=y
+CONFIG_SCHEDSTATS=y
+CONFIG_TIMER_STATS=y
 # CONFIG_DEBUG_PREEMPT is not set
+CONFIG_DEBUG_SPINLOCK_SLEEP=y
 CONFIG_DEBUG_INFO=y
-# CONFIG_RCU_CPU_STALL_DETECTOR is not set
-# CONFIG_FTRACE is not set
+CONFIG_SYSCTL_SYSCALL_CHECK=y
+CONFIG_FUNCTION_TRACER=y
 CONFIG_DEBUG_USER=y
-CONFIG_DEBUG_ERRORS=y
+CONFIG_KEYS=y
+CONFIG_CRYPTO_ECB=y
+CONFIG_CRYPTO_MD5=m
+CONFIG_CRYPTO_SHA1=m
+CONFIG_CRYPTO_AES=y
+CONFIG_CRYPTO_ARC4=y
+CONFIG_CRYPTO_TWOFISH=y
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_DEV_UX500=y
+CONFIG_CRYPTO_DEV_UX500_CRYP=y
+CONFIG_CRYPTO_DEV_UX500_HASH=y
+CONFIG_CRC7=y
+CONFIG_LIBCRC32C=m
diff --git a/arch/arm/configs/u8500_snowball_android_defconfig b/arch/arm/configs/u8500_snowball_android_defconfig
new file mode 100644
index 00000000000..c35e93dc39e
--- /dev/null
+++ b/arch/arm/configs/u8500_snowball_android_defconfig
@@ -0,0 +1,286 @@
+CONFIG_EXPERIMENTAL=y
+# CONFIG_SWAP is not set
+CONFIG_SYSVIPC=y
+CONFIG_BSD_PROCESS_ACCT=y
+CONFIG_LOG_BUF_SHIFT=18
+CONFIG_CGROUPS=y
+CONFIG_CGROUP_CPUACCT=y
+CONFIG_RELAY=y
+CONFIG_BLK_DEV_INITRD=y
+CONFIG_KALLSYMS_ALL=y
+CONFIG_ASHMEM=y
+CONFIG_EMBEDDED=y
+CONFIG_PERF_EVENTS=y
+CONFIG_SLAB=y
+CONFIG_BOOTTIME=y
+CONFIG_MODULES=y
+CONFIG_MODULE_UNLOAD=y
+CONFIG_MODVERSIONS=y
+CONFIG_MODULE_SRCVERSION_ALL=y
+# CONFIG_BLK_DEV_BSG is not set
+CONFIG_DEFAULT_DEADLINE=y
+CONFIG_ARCH_U8500=y
+CONFIG_UX500_SOC_DB8500=y
+CONFIG_MACH_HREFV60=y
+CONFIG_MACH_SNOWBALL=y
+CONFIG_UX500_PRCMU_TIMER=y
+# CONFIG_DISPLAY_GENERIC_PRIMARY is not set
+CONFIG_DISPLAY_AV8100_TERTIARY=y
+CONFIG_UX500_SUSPEND=y
+CONFIG_UX500_SUSPEND_STANDBY=y
+CONFIG_UX500_SUSPEND_MEM=y
+CONFIG_NO_HZ=y
+CONFIG_HIGH_RES_TIMERS=y
+CONFIG_SMP=y
+CONFIG_NR_CPUS=2
+CONFIG_PREEMPT=y
+CONFIG_AEABI=y
+CONFIG_HIGHMEM=y
+CONFIG_CMDLINE="root=/dev/ram0 init=init rw console=ttyAMA2,115200n8 mem=256M initrd=0x800000,72M"
+CONFIG_KEXEC=y
+CONFIG_CRASH_DUMP=y
+CONFIG_CPU_FREQ=y
+CONFIG_CPU_FREQ_STAT_DETAILS=y
+CONFIG_CPU_FREQ_DEFAULT_GOV_ONDEMAND=y
+CONFIG_CPU_FREQ_GOV_POWERSAVE=y
+CONFIG_CPU_FREQ_GOV_USERSPACE=y
+CONFIG_CPU_FREQ_GOV_CONSERVATIVE=y
+CONFIG_CPU_IDLE=y
+CONFIG_FPE_NWFPE=y
+CONFIG_VFP=y
+CONFIG_NEON=y
+# CONFIG_CORE_DUMP_DEFAULT_ELF_HEADERS is not set
+CONFIG_WAKELOCK=y
+CONFIG_PM_RUNTIME=y
+CONFIG_NET=y
+CONFIG_PACKET=y
+CONFIG_UNIX=y
+CONFIG_INET=y
+CONFIG_IP_PNP=y
+CONFIG_IP_PNP_DHCP=y
+# CONFIG_INET_LRO is not set
+CONFIG_IPV6=y
+# CONFIG_INET6_XFRM_MODE_TRANSPORT is not set
+# CONFIG_INET6_XFRM_MODE_TUNNEL is not set
+# CONFIG_INET6_XFRM_MODE_BEET is not set
+# CONFIG_IPV6_SIT is not set
+CONFIG_NETFILTER=y
+CONFIG_NF_CONNTRACK=y
+CONFIG_NETFILTER_XTABLES=y
+CONFIG_NETFILTER_XT_MATCH_MULTIPORT=y
+CONFIG_NETFILTER_XT_MATCH_STATE=y
+CONFIG_NF_CONNTRACK_IPV4=y
+CONFIG_IP_NF_IPTABLES=m
+CONFIG_IP_NF_FILTER=m
+CONFIG_NF_NAT=m
+CONFIG_IP_NF_TARGET_MASQUERADE=m
+CONFIG_NET_SCHED=y
+CONFIG_BT_L2CAP=y
+CONFIG_BT_SCO=y
+CONFIG_BT_RFCOMM=y
+CONFIG_BT_RFCOMM_TTY=y
+CONFIG_BT_BNEP=y
+CONFIG_BT_BNEP_MC_FILTER=y
+CONFIG_BT_BNEP_PROTO_FILTER=y
+CONFIG_BT_HIDP=y
+CONFIG_CFG80211=m
+CONFIG_MAC80211=m
+CONFIG_MAC80211_LEDS=y
+CONFIG_RFKILL=y
+CONFIG_RFKILL_INPUT=y
+CONFIG_NET_9P=y
+CONFIG_CAIF=y
+CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
+# CONFIG_STANDALONE is not set
+CONFIG_BLK_DEV_LOOP=y
+CONFIG_BLK_DEV_RAM=y
+CONFIG_BLK_DEV_RAM_SIZE=73728
+CONFIG_MISC_DEVICES=y
+CONFIG_AB8500_PWM=y
+CONFIG_DISPDEV=y
+CONFIG_STM_TRACE=y
+CONFIG_SCSI=y
+CONFIG_BLK_DEV_SD=y
+CONFIG_MD=y
+CONFIG_BLK_DEV_DM=y
+CONFIG_DM_CRYPT=y
+CONFIG_DM_UEVENT=y
+CONFIG_NETDEVICES=y
+CONFIG_TUN=y
+CONFIG_SMSC_PHY=y
+CONFIG_NET_ETHERNET=y
+CONFIG_SMSC911X=y
+# CONFIG_NETDEV_1000 is not set
+# CONFIG_NETDEV_10000 is not set
+# CONFIG_WLAN is not set
+# CONFIG_INPUT_MOUSEDEV is not set
+CONFIG_INPUT_EVDEV=y
+# CONFIG_KEYBOARD_ATKBD is not set
+CONFIG_KEYBOARD_GPIO=y
+CONFIG_KEYBOARD_NOMADIK_SKE=y
+CONFIG_KEYBOARD_TC3589X=y
+# CONFIG_INPUT_MOUSE is not set
+CONFIG_INPUT_TOUCHSCREEN=y
+CONFIG_TOUCHSCREEN_BU21013=y
+CONFIG_INPUT_MISC=y
+CONFIG_INPUT_AB8500_PONKEY=y
+CONFIG_INPUT_UINPUT=y
+CONFIG_VT_HW_CONSOLE_BINDING=y
+# CONFIG_LEGACY_PTYS is not set
+# CONFIG_DEVKMEM is not set
+CONFIG_SERIAL_AMBA_PL011=y
+CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
+CONFIG_HW_RANDOM=y
+CONFIG_HW_RANDOM_NOMADIK=y
+CONFIG_I2C=y
+CONFIG_I2C_CHARDEV=y
+CONFIG_I2C_NOMADIK=y
+CONFIG_SPI=y
+# CONFIG_STM_MSP_SPI is not set
+CONFIG_SPI_PL022=y
+CONFIG_GPIO_SYSFS=y
+CONFIG_GPIO_TC3589X=y
+CONFIG_GPIO_AB8500=y
+CONFIG_POWER_SUPPLY=y
+CONFIG_AB8500_BM=y
+CONFIG_AB8500_BATTERY_THERM_ON_BATCTRL=y
+CONFIG_SENSORS_AB8500=y
+CONFIG_SENSORS_DB8500=y
+CONFIG_SENSORS_LSM303DLH=y
+CONFIG_SENSORS_L3G4200D=y
+CONFIG_WATCHDOG=y
+CONFIG_MFD_TC3589X=y
+CONFIG_AB8500_CORE=y
+CONFIG_REGULATOR_DEBUG=y
+CONFIG_REGULATOR_VIRTUAL_CONSUMER=y
+CONFIG_REGULATOR_AB8500=y
+CONFIG_MEDIA_SUPPORT=y
+CONFIG_VIDEO_DEV=y
+# CONFIG_MEDIA_TUNER_CUSTOMISE is not set
+# CONFIG_VIDEO_CAPTURE_DRIVERS is not set
+CONFIG_FB=y
+CONFIG_FB_MCDE=y
+CONFIG_MCDE_DISPLAY_GENERIC_DSI=y
+CONFIG_AV8100_HWTRIG_I2SDAT3=y
+CONFIG_FB_B2R2=y
+CONFIG_B2R2_PLUG_CONF=y
+CONFIG_LOGO=y
+CONFIG_SOUND=y
+CONFIG_SND=y
+CONFIG_SND_USB_AUDIO=y
+CONFIG_SND_SOC=y
+CONFIG_SND_SOC_UX500=y
+CONFIG_SND_SOC_UX500_CG29XX=y
+CONFIG_SND_SOC_UX500_AV8100=y
+CONFIG_USB=y
+# CONFIG_USB_DEVICE_CLASS is not set
+CONFIG_USB_SUSPEND=y
+# CONFIG_USB_OTG_WHITELIST is not set
+CONFIG_USB_MON=y
+CONFIG_USB_MUSB_HDRC=y
+CONFIG_USB_MUSB_UX500=y
+CONFIG_USB_MUSB_OTG=y
+CONFIG_USB_GADGET_MUSB_HDRC=y
+CONFIG_USB_ACM=y
+CONFIG_USB_STORAGE=y
+CONFIG_USB_LIBUSUAL=y
+CONFIG_USB_GADGET=y
+CONFIG_USB_GADGET_VBUS_DRAW=500
+CONFIG_USB_G_ANDROID=y
+CONFIG_AB8500_USB=y
+CONFIG_MMC=y
+CONFIG_MMC_UNSAFE_RESUME=y
+CONFIG_MMC_CLKGATE=y
+CONFIG_MMC_PARANOID_SD_INIT=y
+# CONFIG_MMC_BLOCK_BOUNCE is not set
+CONFIG_MMC_ARMMMCI=y
+CONFIG_NEW_LEDS=y
+CONFIG_LEDS_CLASS=y
+CONFIG_LEDS_PWM=y
+CONFIG_LEDS_TRIGGER_TIMER=y
+CONFIG_SWITCH=y
+CONFIG_RTC_CLASS=y
+CONFIG_RTC_DRV_AB8500=y
+CONFIG_DMADEVICES=y
+CONFIG_STE_DMA40=y
+CONFIG_STAGING=y
+CONFIG_ANDROID=y
+CONFIG_ANDROID_BINDER_IPC=y
+CONFIG_ANDROID_LOGGER=y
+CONFIG_ANDROID_RAM_CONSOLE=y
+CONFIG_ANDROID_TIMED_GPIO=y
+CONFIG_ANDROID_LOW_MEMORY_KILLER=y
+CONFIG_TOUCHSCREEN_SYNAPTICS_I2C_RMI4=y
+CONFIG_CG2900=y
+CONFIG_CG2900_CHIP=y
+CONFIG_STLC2690_CHIP=y
+CONFIG_CG2900_UART=y
+CONFIG_CG2900_AUDIO=y
+CONFIG_CG2900_TEST=y
+CONFIG_BT_CG2900=y
+CONFIG_U8500_CM=y
+CONFIG_EXT2_FS=y
+CONFIG_EXT2_FS_XATTR=y
+CONFIG_EXT2_FS_POSIX_ACL=y
+CONFIG_EXT2_FS_SECURITY=y
+CONFIG_EXT3_FS=y
+# CONFIG_EXT3_DEFAULTS_TO_ORDERED is not set
+CONFIG_EXT3_FS_POSIX_ACL=y
+CONFIG_EXT4_FS=y
+CONFIG_EXT4_FS_POSIX_ACL=y
+CONFIG_QUOTA=y
+CONFIG_QFMT_V2=y
+CONFIG_AUTOFS4_FS=m
+CONFIG_ISO9660_FS=y
+CONFIG_JOLIET=y
+CONFIG_ZISOFS=y
+CONFIG_UDF_FS=m
+CONFIG_MSDOS_FS=y
+CONFIG_VFAT_FS=y
+CONFIG_TMPFS=y
+CONFIG_TMPFS_POSIX_ACL=y
+CONFIG_HFS_FS=m
+CONFIG_BEFS_FS=m
+CONFIG_CRAMFS=m
+CONFIG_VXFS_FS=m
+CONFIG_MINIX_FS=m
+CONFIG_ROMFS_FS=m
+CONFIG_SYSV_FS=m
+CONFIG_UFS_FS=m
+CONFIG_NFS_FS=y
+CONFIG_ROOT_NFS=y
+CONFIG_CIFS=y
+CONFIG_9P_FS=y
+CONFIG_PARTITION_ADVANCED=y
+CONFIG_BLKDEV_PARTITION=y
+CONFIG_BSD_DISKLABEL=y
+CONFIG_MINIX_SUBPARTITION=y
+CONFIG_SOLARIS_X86_PARTITION=y
+CONFIG_UNIXWARE_DISKLABEL=y
+CONFIG_SGI_PARTITION=y
+CONFIG_SUN_PARTITION=y
+CONFIG_NLS_CODEPAGE_437=y
+CONFIG_NLS_ISO8859_1=y
+CONFIG_PRINTK_TIME=y
+CONFIG_MAGIC_SYSRQ=y
+CONFIG_UNUSED_SYMBOLS=y
+CONFIG_DEBUG_KERNEL=y
+CONFIG_DETECT_HUNG_TASK=y
+CONFIG_SCHEDSTATS=y
+CONFIG_TIMER_STATS=y
+# CONFIG_DEBUG_PREEMPT is not set
+CONFIG_DEBUG_SPINLOCK_SLEEP=y
+CONFIG_DEBUG_INFO=y
+CONFIG_SYSCTL_SYSCALL_CHECK=y
+CONFIG_FUNCTION_TRACER=y
+CONFIG_DEBUG_USER=y
+CONFIG_KEYS=y
+CONFIG_CRYPTO_SHA1=m
+CONFIG_CRYPTO_AES=y
+CONFIG_CRYPTO_TWOFISH=y
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_CRYPTO_DEV_UX500=y
+CONFIG_CRYPTO_DEV_UX500_CRYP=y
+CONFIG_CRYPTO_DEV_UX500_HASH=y
+CONFIG_CRC7=y
+CONFIG_LIBCRC32C=m
diff --git a/arch/arm/include/asm/cacheflush.h b/arch/arm/include/asm/cacheflush.h
index 1252a2675ca..bbaeaef03b0 100644
--- a/arch/arm/include/asm/cacheflush.h
+++ b/arch/arm/include/asm/cacheflush.h
@@ -84,6 +84,14 @@
  *		- kaddr  - page address
  *		- size   - region size
  *
+ *	clean_dcache_all()
+ *
+ *		Cleans the entire d-cache.
+ *
+ *	flush_dcache_all()
+ *
+ *		Flushes the entire d-cache.
+ *
  *	DMA Cache Coherency
  *	===================
  *
@@ -104,6 +112,9 @@ struct cpu_cache_fns {
 	void (*coherent_user_range)(unsigned long, unsigned long);
 	void (*flush_kern_dcache_area)(void *, size_t);
 
+	void (*clean_dcache_all)(void);
+	void (*flush_dcache_all)(void);
+
 	void (*dma_map_area)(const void *, size_t, int);
 	void (*dma_unmap_area)(const void *, size_t, int);
 
@@ -124,6 +135,8 @@ extern struct cpu_cache_fns cpu_cache;
 #define __cpuc_coherent_kern_range	cpu_cache.coherent_kern_range
 #define __cpuc_coherent_user_range	cpu_cache.coherent_user_range
 #define __cpuc_flush_dcache_area	cpu_cache.flush_kern_dcache_area
+#define __cpuc_clean_dcache_all		cpu_cache.clean_dcache_all
+#define __cpuc_flush_dcache_all		cpu_cache.flush_dcache_all
 
 /*
  * These are private to the dma-mapping API.  Do not use directly.
@@ -144,6 +157,8 @@ extern void __cpuc_flush_user_range(unsigned long, unsigned long, unsigned int);
 extern void __cpuc_coherent_kern_range(unsigned long, unsigned long);
 extern void __cpuc_coherent_user_range(unsigned long, unsigned long);
 extern void __cpuc_flush_dcache_area(void *, size_t);
+extern void __cpuc_clean_dcache_all(void);
+extern void __cpuc_flush_dcache_all(void);
 
 /*
  * These are private to the dma-mapping API.  Do not use directly.
diff --git a/arch/arm/include/asm/delay.h b/arch/arm/include/asm/delay.h
index b2deda18154..91063a3976f 100644
--- a/arch/arm/include/asm/delay.h
+++ b/arch/arm/include/asm/delay.h
@@ -8,7 +8,7 @@
 
 #include <asm/param.h>	/* HZ */
 
-extern void __delay(int loops);
+extern void __delay(unsigned long loops);
 
 /*
  * This function intentionally does not exist; if you see references to
@@ -40,5 +40,14 @@ extern void __const_udelay(unsigned long);
 			__const_udelay((n) * ((2199023U*HZ)>>11))) :	\
 	  __udelay(n))
 
+extern void (*delay_fn)(unsigned long);
+
+static inline void set_delay_fn(void (*fn)(unsigned long))
+{
+	delay_fn = fn;
+}
+
+extern void read_current_timer_delay_loop(unsigned long loops);
+
 #endif /* defined(_ARM_DELAY_H) */
 
diff --git a/arch/arm/include/asm/elf.h b/arch/arm/include/asm/elf.h
index 0e9ce8d9686..85cf3655914 100644
--- a/arch/arm/include/asm/elf.h
+++ b/arch/arm/include/asm/elf.h
@@ -96,8 +96,8 @@ struct elf32_hdr;
 /*
  * This is used to ensure we don't load something for the wrong architecture.
  */
-extern int elf_check_arch(const struct elf32_hdr *);
-#define elf_check_arch elf_check_arch
+extern int arm_elf_check_arch(const struct elf32_hdr *);
+#define elf_check_arch(x) arm_elf_check_arch((const struct elf32_hdr *)(x))
 
 #define vmcore_elf64_check_arch(x) (0)
 
diff --git a/arch/arm/include/asm/io.h b/arch/arm/include/asm/io.h
index d66605dea55..ffa4430e106 100644
--- a/arch/arm/include/asm/io.h
+++ b/arch/arm/include/asm/io.h
@@ -63,6 +63,12 @@ extern void __raw_readsl(const void __iomem *addr, void *data, int longlen);
 #define MT_DEVICE_CACHED	2
 #define MT_DEVICE_WC		3
 /*
+ * NOTE : U8500 v1.0/ED cut specific hack.
+ * look at the commit message for more details
+ */
+#define MT_BACKUP_RAM           4
+
+/*
  * types 4 onwards can be found in asm/mach/map.h and are undefined
  * for ioremap
  */
diff --git a/arch/arm/include/asm/outercache.h b/arch/arm/include/asm/outercache.h
index d8387437ec5..7ed8f98b217 100644
--- a/arch/arm/include/asm/outercache.h
+++ b/arch/arm/include/asm/outercache.h
@@ -33,6 +33,8 @@ struct outer_cache_fns {
 #ifdef CONFIG_OUTER_CACHE_SYNC
 	void (*sync)(void);
 #endif
+	void (*prefetch_enable)(void);
+	void (*prefetch_disable)(void);
 	void (*set_debug)(unsigned long);
 };
 
@@ -74,6 +76,18 @@ static inline void outer_disable(void)
 		outer_cache.disable();
 }
 
+static inline void outer_prefetch_enable(void)
+{
+	if (outer_cache.prefetch_enable)
+		outer_cache.prefetch_enable();
+}
+
+static inline void outer_prefetch_disable(void)
+{
+	if (outer_cache.prefetch_disable)
+		outer_cache.prefetch_disable();
+}
+
 #else
 
 static inline void outer_inv_range(phys_addr_t start, phys_addr_t end)
diff --git a/arch/arm/include/asm/setup.h b/arch/arm/include/asm/setup.h
index 915696dd9c7..ad937bc348f 100644
--- a/arch/arm/include/asm/setup.h
+++ b/arch/arm/include/asm/setup.h
@@ -143,6 +143,23 @@ struct tag_memclk {
 	__u32 fmemclk;
 };
 
+/* for automatic boot timing testcases */
+#define ATAG_BOOTTIME  0x41000403
+#define BOOTTIME_MAX_NAME_LEN 64
+#define BOOTTIME_MAX 10
+
+struct boottime_entry {
+	u32 time; /* in us */
+	u8  name[BOOTTIME_MAX_NAME_LEN];
+};
+
+struct tag_boottime {
+	struct boottime_entry entry[BOOTTIME_MAX];
+	u32 idle;  /* in us */
+	u32 total; /* in us */
+	u8 num;
+};
+
 struct tag {
 	struct tag_header hdr;
 	union {
@@ -165,6 +182,10 @@ struct tag {
 		 * DC21285 specific
 		 */
 		struct tag_memclk	memclk;
+		/*
+		 * Boot time
+		 */
+		struct tag_boottime	boottime;
 	} u;
 };
 
diff --git a/arch/arm/include/asm/smp_twd.h b/arch/arm/include/asm/smp_twd.h
index fed9981fba0..51197634059 100644
--- a/arch/arm/include/asm/smp_twd.h
+++ b/arch/arm/include/asm/smp_twd.h
@@ -17,12 +17,24 @@
 #define TWD_TIMER_CONTROL_ONESHOT	(0 << 1)
 #define TWD_TIMER_CONTROL_PERIODIC	(1 << 1)
 #define TWD_TIMER_CONTROL_IT_ENABLE	(1 << 2)
+#define TWD_TIMER_CONTROL_PRESCALE_MASK	(0xFF << 8)
 
 struct clock_event_device;
 
 extern void __iomem *twd_base;
 
+void twd_timer_stop(void);
 int twd_timer_ack(void);
 void twd_timer_setup(struct clock_event_device *);
 
+/*
+ * Use this setup function on systems that support cpufreq.
+ * periphclk_prescaler is the fixed divider value between the cpu
+ * clock and the PERIPHCLK clock that feeds the TWD.  target_rate should be
+ * low enough that the prescaler can accurately reach the target rate from the
+ * lowest cpu frequency, but high enough to give a reasonable timer accuracy.
+ */
+void twd_timer_setup_scalable(struct clock_event_device *,
+	unsigned long target_rate, unsigned int periphclk_prescaler);
+
 #endif
diff --git a/arch/arm/include/asm/system.h b/arch/arm/include/asm/system.h
index 4adf71b2b54..97777d4d160 100644
--- a/arch/arm/include/asm/system.h
+++ b/arch/arm/include/asm/system.h
@@ -111,6 +111,8 @@ extern void cpu_init(void);
 void arm_machine_restart(char mode, const char *cmd);
 extern void (*arm_pm_restart)(char str, const char *cmd);
 
+void cpu_idle_wait(void);
+
 #define UDBG_UNDEFINED	(1 << 0)
 #define UDBG_SYSCALL	(1 << 1)
 #define UDBG_BADABORT	(1 << 2)
diff --git a/arch/arm/kernel/armksyms.c b/arch/arm/kernel/armksyms.c
index acca35aebe2..263eaaf3f26 100644
--- a/arch/arm/kernel/armksyms.c
+++ b/arch/arm/kernel/armksyms.c
@@ -52,10 +52,6 @@ extern void fpundefinstr(void);
 
 EXPORT_SYMBOL(__backtrace);
 
-	/* platform dependent support */
-EXPORT_SYMBOL(__udelay);
-EXPORT_SYMBOL(__const_udelay);
-
 	/* networking */
 EXPORT_SYMBOL(csum_partial);
 EXPORT_SYMBOL(csum_partial_copy_from_user);
diff --git a/arch/arm/kernel/elf.c b/arch/arm/kernel/elf.c
index 9b05c6a0dce..ddd49b23365 100644
--- a/arch/arm/kernel/elf.c
+++ b/arch/arm/kernel/elf.c
@@ -4,11 +4,13 @@
 #include <linux/binfmts.h>
 #include <linux/elf.h>
 
-int elf_check_arch(const struct elf32_hdr *x)
+int arm_elf_check_arch(const struct elf32_hdr *x)
 {
 	unsigned int eflags;
 
 	/* Make sure it's an ARM executable */
+	if (x->e_ident[EI_CLASS] != ELF_CLASS)
+		return 0;
 	if (x->e_machine != EM_ARM)
 		return 0;
 
@@ -35,7 +37,7 @@ int elf_check_arch(const struct elf32_hdr *x)
 	}
 	return 1;
 }
-EXPORT_SYMBOL(elf_check_arch);
+EXPORT_SYMBOL(arm_elf_check_arch);
 
 void elf_set_personality(const struct elf32_hdr *x)
 {
diff --git a/arch/arm/kernel/machine_kexec.c b/arch/arm/kernel/machine_kexec.c
index e59bbd496c3..ae1d73a15d3 100644
--- a/arch/arm/kernel/machine_kexec.c
+++ b/arch/arm/kernel/machine_kexec.c
@@ -47,6 +47,7 @@ void machine_crash_nonpanic_core(void *unused)
 	printk(KERN_DEBUG "CPU %u will stop doing anything useful since another CPU has crashed\n",
 	       smp_processor_id());
 	crash_save_cpu(&regs, smp_processor_id());
+	atomic_notifier_call_chain(&crash_percpu_notifier_list, 0, NULL);
 	flush_cache_all();
 
 	atomic_dec(&waiting_for_crash_ipi);
diff --git a/arch/arm/kernel/process.c b/arch/arm/kernel/process.c
index 34fca2e272c..23db0a90003 100644
--- a/arch/arm/kernel/process.c
+++ b/arch/arm/kernel/process.c
@@ -215,8 +215,17 @@ void cpu_idle(void)
 		idle_notifier_call_chain(IDLE_START);
 		while (!need_resched()) {
 #ifdef CONFIG_HOTPLUG_CPU
-			if (cpu_is_offline(smp_processor_id()))
+			if (cpu_is_offline(smp_processor_id())) {
+
+				/* NOTE : preempt_count() should be 0 for dying CPU
+				*        as the CPU will use this very thread when
+				*        it is alive
+				*/
+				if (preempt_count())
+					preempt_enable_no_resched();
+
 				cpu_die();
+			}
 #endif
 
 			local_irq_disable();
diff --git a/arch/arm/kernel/return_address.c b/arch/arm/kernel/return_address.c
index 0b13a72f855..b7fe66d270a 100644
--- a/arch/arm/kernel/return_address.c
+++ b/arch/arm/kernel/return_address.c
@@ -58,10 +58,6 @@ void *return_address(unsigned int level)
 
 #else /* if defined(CONFIG_FRAME_POINTER) && !defined(CONFIG_ARM_UNWIND) */
 
-#if defined(CONFIG_ARM_UNWIND)
-#warning "TODO: return_address should use unwind tables"
-#endif
-
 void *return_address(unsigned int level)
 {
 	return NULL;
diff --git a/arch/arm/kernel/smp.c b/arch/arm/kernel/smp.c
index 5991a1a980c..ec620dac0e7 100644
--- a/arch/arm/kernel/smp.c
+++ b/arch/arm/kernel/smp.c
@@ -316,8 +316,6 @@ asmlinkage void __cpuinit secondary_start_kernel(void)
 	 */
 	percpu_timer_setup();
 
-	calibrate_delay();
-
 	smp_store_cpu_info(cpu);
 
 	/*
@@ -490,7 +488,7 @@ void show_local_irqs(struct seq_file *p, int prec)
 #endif
 
 #ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
-static void smp_timer_broadcast(const struct cpumask *mask)
+void smp_timer_broadcast(const struct cpumask *mask)
 {
 	smp_cross_call(mask, IPI_TIMER);
 }
diff --git a/arch/arm/kernel/smp_twd.c b/arch/arm/kernel/smp_twd.c
index 01c186222f3..ed90519e54e 100644
--- a/arch/arm/kernel/smp_twd.c
+++ b/arch/arm/kernel/smp_twd.c
@@ -17,6 +17,7 @@
 #include <linux/clockchips.h>
 #include <linux/irq.h>
 #include <linux/io.h>
+#include <linux/cpufreq.h>
 
 #include <asm/smp_twd.h>
 #include <asm/hardware/gic.h>
@@ -25,18 +26,26 @@
 void __iomem *twd_base;
 
 static unsigned long twd_timer_rate;
+static unsigned long twd_periphclk_prescaler;
+static unsigned long twd_target_rate;
+
+static DEFINE_PER_CPU(unsigned long, twd_ctrl);
+static DEFINE_PER_CPU(unsigned long, twd_load);
 
 static void twd_set_mode(enum clock_event_mode mode,
 			struct clock_event_device *clk)
 {
 	unsigned long ctrl;
+	int this_cpu = smp_processor_id();
+
+	__raw_writel(per_cpu(twd_load, this_cpu),
+				 twd_base + TWD_TIMER_LOAD);
 
 	switch (mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
 		/* timer load already set up */
 		ctrl = TWD_TIMER_CONTROL_ENABLE | TWD_TIMER_CONTROL_IT_ENABLE
 			| TWD_TIMER_CONTROL_PERIODIC;
-		__raw_writel(twd_timer_rate / HZ, twd_base + TWD_TIMER_LOAD);
 		break;
 	case CLOCK_EVT_MODE_ONESHOT:
 		/* period set, and timer enabled in 'next_event' hook */
@@ -48,18 +57,22 @@ static void twd_set_mode(enum clock_event_mode mode,
 		ctrl = 0;
 	}
 
+	ctrl |= per_cpu(twd_ctrl, this_cpu) & TWD_TIMER_CONTROL_PRESCALE_MASK;
+
 	__raw_writel(ctrl, twd_base + TWD_TIMER_CONTROL);
+	per_cpu(twd_ctrl, this_cpu) = ctrl;
 }
 
 static int twd_set_next_event(unsigned long evt,
 			struct clock_event_device *unused)
 {
-	unsigned long ctrl = __raw_readl(twd_base + TWD_TIMER_CONTROL);
+	int this_cpu = smp_processor_id();
 
-	ctrl |= TWD_TIMER_CONTROL_ENABLE;
+	per_cpu(twd_ctrl, this_cpu) |= TWD_TIMER_CONTROL_ENABLE;
 
 	__raw_writel(evt, twd_base + TWD_TIMER_COUNTER);
-	__raw_writel(ctrl, twd_base + TWD_TIMER_CONTROL);
+	__raw_writel(per_cpu(twd_ctrl, this_cpu),
+				 twd_base + TWD_TIMER_CONTROL);
 
 	return 0;
 }
@@ -80,6 +93,51 @@ int twd_timer_ack(void)
 	return 0;
 }
 
+/*
+ * must be called with interrupts disabled and on the cpu that is being changed
+ */
+static void twd_update_cpu_frequency(unsigned long new_rate)
+{
+	int prescaler;
+	int this_cpu = smp_processor_id();
+
+	BUG_ON(twd_periphclk_prescaler == 0 || twd_target_rate == 0);
+
+	twd_timer_rate = new_rate / twd_periphclk_prescaler;
+
+	prescaler = DIV_ROUND_UP(twd_timer_rate, twd_target_rate);
+	prescaler = clamp(prescaler - 1, 0, 0xFF);
+
+	per_cpu(twd_ctrl, this_cpu) &= ~TWD_TIMER_CONTROL_PRESCALE_MASK;
+	per_cpu(twd_ctrl, this_cpu) |= prescaler << 8;
+	__raw_writel(per_cpu(twd_ctrl, this_cpu),
+				 twd_base + TWD_TIMER_CONTROL);
+}
+
+static void twd_update_cpu_frequency_on_cpu(void *data)
+{
+	struct cpufreq_freqs *freq = data;
+
+	twd_update_cpu_frequency(freq->new * 1000);
+}
+
+static int twd_cpufreq_notifier(struct notifier_block *nb,
+			unsigned long event, void *data)
+{
+	struct cpufreq_freqs *freq = data;
+
+	if (event == CPUFREQ_RESUMECHANGE ||
+	    (event == CPUFREQ_PRECHANGE && freq->new > freq->old) ||
+	    (event == CPUFREQ_POSTCHANGE && freq->new < freq->old))
+		on_each_cpu(twd_update_cpu_frequency_on_cpu, freq, 1);
+
+	return 0;
+}
+
+static struct notifier_block twd_cpufreq_notifier_block = {
+	.notifier_call = twd_cpufreq_notifier,
+};
+
 static void __cpuinit twd_calibrate_rate(void)
 {
 	unsigned long count;
@@ -102,7 +160,11 @@ static void __cpuinit twd_calibrate_rate(void)
 		waitjiffies += 5;
 
 				 /* enable, no interrupt or reload */
-		__raw_writel(0x1, twd_base + TWD_TIMER_CONTROL);
+		__raw_writel(TWD_TIMER_CONTROL_ENABLE,
+					 twd_base + TWD_TIMER_CONTROL);
+
+		per_cpu(twd_ctrl, smp_processor_id()) =
+			TWD_TIMER_CONTROL_ENABLE;
 
 				 /* maximum value */
 		__raw_writel(0xFFFFFFFFU, twd_base + TWD_TIMER_COUNTER);
@@ -122,18 +184,39 @@ static void __cpuinit twd_calibrate_rate(void)
 /*
  * Setup the local clock events for a CPU.
  */
-void __cpuinit twd_timer_setup(struct clock_event_device *clk)
+static void __cpuinit __twd_timer_setup(struct clock_event_device *clk,
+	unsigned long target_rate, unsigned int periphclk_prescaler)
 {
+	unsigned long cpu_rate;
+	unsigned long twd_tick_rate;
+
 	twd_calibrate_rate();
 
+	if (target_rate && periphclk_prescaler) {
+		cpu_rate = twd_timer_rate * periphclk_prescaler;
+		twd_target_rate = target_rate;
+		twd_periphclk_prescaler = periphclk_prescaler;
+		twd_update_cpu_frequency(cpu_rate);
+		twd_tick_rate = twd_target_rate;
+	} else {
+		twd_tick_rate = twd_timer_rate;
+	}
+
+	per_cpu(twd_load, smp_processor_id()) = twd_tick_rate / HZ;
+
 	clk->name = "local_timer";
+#if defined(CONFIG_GENERIC_CLOCKEVENTS_BROADCAST) && \
+	defined(CONFIG_LOCAL_TIMERS)
 	clk->features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT |
 			CLOCK_EVT_FEAT_C3STOP;
+#else
+	clk->features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT;
+#endif
 	clk->rating = 350;
 	clk->set_mode = twd_set_mode;
 	clk->set_next_event = twd_set_next_event;
 	clk->shift = 20;
-	clk->mult = div_sc(twd_timer_rate, NSEC_PER_SEC, clk->shift);
+	clk->mult = div_sc(twd_tick_rate, NSEC_PER_SEC, clk->shift);
 	clk->max_delta_ns = clockevent_delta2ns(0xffffffff, clk);
 	clk->min_delta_ns = clockevent_delta2ns(0xf, clk);
 
@@ -142,3 +225,38 @@ void __cpuinit twd_timer_setup(struct clock_event_device *clk)
 	/* Make sure our local interrupt controller has this enabled */
 	gic_enable_ppi(clk->irq);
 }
+
+void __cpuinit twd_timer_setup_scalable(struct clock_event_device *clk,
+	unsigned long target_rate, unsigned int periphclk_prescaler)
+{
+	__twd_timer_setup(clk, target_rate, periphclk_prescaler);
+}
+
+void __cpuinit twd_timer_setup(struct clock_event_device *clk)
+{
+	__twd_timer_setup(clk, 0, 0);
+}
+
+static int twd_timer_setup_cpufreq(void)
+{
+	if (twd_periphclk_prescaler)
+		cpufreq_register_notifier(&twd_cpufreq_notifier_block,
+			CPUFREQ_TRANSITION_NOTIFIER);
+
+	return 0;
+}
+arch_initcall(twd_timer_setup_cpufreq);
+
+#ifdef CONFIG_HOTPLUG_CPU
+/*
+ * take a local timer down
+ */
+void twd_timer_stop(void)
+{
+	int this_cpu = smp_processor_id();
+	per_cpu(twd_ctrl, this_cpu) &= ~(TWD_TIMER_CONTROL_ENABLE |
+					 TWD_TIMER_CONTROL_IT_ENABLE);
+	__raw_writel(per_cpu(twd_ctrl, this_cpu),
+				 twd_base + TWD_TIMER_CONTROL);
+}
+#endif
diff --git a/arch/arm/lib/delay.S b/arch/arm/lib/delay.S
deleted file mode 100644
index 3c9a05c8d20..00000000000
--- a/arch/arm/lib/delay.S
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- *  linux/arch/arm/lib/delay.S
- *
- *  Copyright (C) 1995, 1996 Russell King
- *
- * 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/linkage.h>
-#include <asm/assembler.h>
-#include <asm/param.h>
-		.text
-
-.LC0:		.word	loops_per_jiffy
-.LC1:		.word	(2199023*HZ)>>11
-
-/*
- * r0  <= 2000
- * lpj <= 0x01ffffff (max. 3355 bogomips)
- * HZ  <= 1000
- */
-
-ENTRY(__udelay)
-		ldr	r2, .LC1
-		mul	r0, r2, r0
-ENTRY(__const_udelay)				@ 0 <= r0 <= 0x7fffff06
-		mov	r1, #-1
-		ldr	r2, .LC0
-		ldr	r2, [r2]		@ max = 0x01ffffff
-		add	r0, r0, r1, lsr #32-14
-		mov	r0, r0, lsr #14		@ max = 0x0001ffff
-		add	r2, r2, r1, lsr #32-10
-		mov	r2, r2, lsr #10		@ max = 0x00007fff
-		mul	r0, r2, r0		@ max = 2^32-1
-		add	r0, r0, r1, lsr #32-6
-		movs	r0, r0, lsr #6
-		moveq	pc, lr
-
-/*
- * loops = r0 * HZ * loops_per_jiffy / 1000000
- *
- * Oh, if only we had a cycle counter...
- */
-
-@ Delay routine
-ENTRY(__delay)
-		subs	r0, r0, #1
-#if 0
-		movls	pc, lr
-		subs	r0, r0, #1
-		movls	pc, lr
-		subs	r0, r0, #1
-		movls	pc, lr
-		subs	r0, r0, #1
-		movls	pc, lr
-		subs	r0, r0, #1
-		movls	pc, lr
-		subs	r0, r0, #1
-		movls	pc, lr
-		subs	r0, r0, #1
-		movls	pc, lr
-		subs	r0, r0, #1
-#endif
-		bhi	__delay
-		mov	pc, lr
-ENDPROC(__udelay)
-ENDPROC(__const_udelay)
-ENDPROC(__delay)
diff --git a/arch/arm/lib/delay.c b/arch/arm/lib/delay.c
new file mode 100644
index 00000000000..b8d636e8ef8
--- /dev/null
+++ b/arch/arm/lib/delay.c
@@ -0,0 +1,81 @@
+/*
+ *  Originally from linux/arch/arm/lib/delay.S
+ *
+ *  Copyright (C) 1995, 1996 Russell King
+ *
+ * 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 <linux/delay.h>
+#include <linux/timex.h>
+
+/*
+ * Oh, if only we had a cycle counter...
+ */
+static void delay_loop(unsigned long loops)
+{
+	asm volatile(
+	"1:	subs %0, %0, #1 \n"
+	"	bhi 1b		\n"
+	: /* No output */
+	: "r" (loops)
+	);
+}
+
+#ifdef ARCH_HAS_READ_CURRENT_TIMER
+/*
+ * Assumes read_current_timer() is monotonically increasing
+ * across calls and wraps at most once within MAX_UDELAY_MS.
+ */
+void read_current_timer_delay_loop(unsigned long loops)
+{
+	unsigned long bclock, now;
+
+	read_current_timer(&bclock);
+	do {
+		read_current_timer(&now);
+	} while ((now - bclock) < loops);
+}
+#endif
+
+void (*delay_fn)(unsigned long) = delay_loop;
+
+/*
+ * loops = usecs * HZ * loops_per_jiffy / 1000000
+ */
+void __delay(unsigned long loops)
+{
+	delay_fn(loops);
+}
+EXPORT_SYMBOL(__delay);
+
+/*
+ * 0 <= xloops <= 0x7fffff06
+ * loops_per_jiffy <= 0x01ffffff (max. 3355 bogomips)
+ */
+void __const_udelay(unsigned long xloops)
+{
+	unsigned long lpj;
+	unsigned long loops;
+
+	xloops >>= 14;			/* max = 0x01ffffff */
+	lpj = loops_per_jiffy >> 10;	/* max = 0x0001ffff */
+	loops = lpj * xloops;		/* max = 0x00007fff */
+	loops >>= 6;			/* max = 2^32-1 */
+
+	if (likely(loops))
+		__delay(loops);
+}
+EXPORT_SYMBOL(__const_udelay);
+
+/*
+ * usecs  <= 2000
+ * HZ  <= 1000
+ */
+void __udelay(unsigned long usecs)
+{
+	__const_udelay(usecs * ((2199023UL*HZ)>>11));
+}
+EXPORT_SYMBOL(__udelay);
diff --git a/arch/arm/mach-nomadik/board-nhk8815.c b/arch/arm/mach-nomadik/board-nhk8815.c
index 139930350d9..de7a1c10cec 100644
--- a/arch/arm/mach-nomadik/board-nhk8815.c
+++ b/arch/arm/mach-nomadik/board-nhk8815.c
@@ -16,6 +16,7 @@
 #include <linux/amba/bus.h>
 #include <linux/interrupt.h>
 #include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
 #include <linux/mtd/onenand.h>
diff --git a/arch/arm/mach-nomadik/cpu-8815.c b/arch/arm/mach-nomadik/cpu-8815.c
index ac58e3b03b1..671bdeea391 100644
--- a/arch/arm/mach-nomadik/cpu-8815.c
+++ b/arch/arm/mach-nomadik/cpu-8815.c
@@ -22,6 +22,7 @@
 #include <linux/amba/bus.h>
 #include <linux/platform_device.h>
 #include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
 
 #include <mach/hardware.h>
 #include <mach/irqs.h>
diff --git a/arch/arm/mach-nomadik/i2c-8815nhk.c b/arch/arm/mach-nomadik/i2c-8815nhk.c
index abfe25a08d6..a6c7dc465b4 100644
--- a/arch/arm/mach-nomadik/i2c-8815nhk.c
+++ b/arch/arm/mach-nomadik/i2c-8815nhk.c
@@ -3,7 +3,7 @@
 #include <linux/i2c.h>
 #include <linux/i2c-algo-bit.h>
 #include <linux/i2c-gpio.h>
-#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
 #include <linux/platform_device.h>
 
 /*
diff --git a/arch/arm/mach-nomadik/include/mach/gpio.h b/arch/arm/mach-nomadik/include/mach/gpio.h
index 7a81a042034..2bbfa36e9e6 100644
--- a/arch/arm/mach-nomadik/include/mach/gpio.h
+++ b/arch/arm/mach-nomadik/include/mach/gpio.h
@@ -1,6 +1,12 @@
 #ifndef __ASM_ARCH_GPIO_H
 #define __ASM_ARCH_GPIO_H
 
-#include <plat/gpio.h>
+#include <asm-generic/gpio.h>
+
+/* Invoke gpiolibs gpio_chip abstraction */
+#define gpio_get_value  __gpio_get_value
+#define gpio_set_value  __gpio_set_value
+#define gpio_cansleep   __gpio_cansleep
+#define gpio_to_irq     __gpio_to_irq
 
 #endif /* __ASM_ARCH_GPIO_H */
diff --git a/arch/arm/mach-omap2/Makefile b/arch/arm/mach-omap2/Makefile
index b1480779440..3287ff667bf 100644
--- a/arch/arm/mach-omap2/Makefile
+++ b/arch/arm/mach-omap2/Makefile
@@ -172,6 +172,9 @@ ifneq ($(CONFIG_TIDSPBRIDGE),)
 obj-y					+= dsp.o
 endif
 
+omap-ssi-$(CONFIG_OMAP_SSI)		:= ssi.o
+obj-y					+= $(omap-ssi-m) $(omap-ssi-y)
+
 # Specific board support
 obj-$(CONFIG_MACH_OMAP_GENERIC)		+= board-generic.o
 obj-$(CONFIG_MACH_OMAP_H4)		+= board-h4.o
diff --git a/arch/arm/mach-omap2/ssi.c b/arch/arm/mach-omap2/ssi.c
new file mode 100644
index 00000000000..e822a77f5ca
--- /dev/null
+++ b/arch/arm/mach-omap2/ssi.c
@@ -0,0 +1,134 @@
+/*
+ * linux/arch/arm/mach-omap2/ssi.c
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <plat/omap-pm.h>
+#include <plat/ssi.h>
+
+static struct omap_ssi_platform_data ssi_pdata = {
+	.num_ports			= SSI_NUM_PORTS,
+	.get_dev_context_loss_count	= omap_pm_get_dev_context_loss_count,
+};
+
+static struct resource ssi_resources[] = {
+	/* SSI controller */
+	[0] = {
+		.start	= 0x48058000,
+		.end	= 0x48058fff,
+		.name	= "omap_ssi_sys",
+		.flags	= IORESOURCE_MEM,
+	},
+	/* GDD */
+	[1] = {
+		.start	= 0x48059000,
+		.end	= 0x48059fff,
+		.name	= "omap_ssi_gdd",
+		.flags	= IORESOURCE_MEM,
+	},
+	[2] = {
+		.start	= 71,
+		.end	= 71,
+		.name	= "ssi_gdd",
+		.flags	= IORESOURCE_IRQ,
+	},
+	/* SSI port 1 */
+	[3] = {
+		.start	= 0x4805a000,
+		.end	= 0x4805a7ff,
+		.name	= "omap_ssi_sst1",
+		.flags	= IORESOURCE_MEM,
+	},
+	[4] = {
+		.start	= 0x4805a800,
+		.end	= 0x4805afff,
+		.name	= "omap_ssi_ssr1",
+		.flags	= IORESOURCE_MEM,
+	},
+	[5] = {
+		.start	= 67,
+		.end	= 67,
+		.name	= "ssi_p1_mpu_irq0",
+		.flags	= IORESOURCE_IRQ,
+	},
+	[6] = {
+		.start	= 68,
+		.end	= 68,
+		.name	= "ssi_p1_mpu_irq1",
+		.flags	= IORESOURCE_IRQ,
+	},
+	[7] = {
+		.start	= 0,
+		.end	= 0,
+		.name	= "ssi_p1_cawake",
+		.flags	= IORESOURCE_IRQ | IORESOURCE_UNSET,
+	},
+};
+
+static struct platform_device ssi_pdev = {
+	.name		= "omap_ssi",
+	.id		= 0,
+	.num_resources	= ARRAY_SIZE(ssi_resources),
+	.resource	= ssi_resources,
+	.dev		= {
+				.platform_data	= &ssi_pdata,
+	},
+};
+
+int __init omap_ssi_config(struct omap_ssi_board_config *ssi_config)
+{
+	unsigned int port, offset, cawake_gpio;
+	int err;
+
+	ssi_pdata.num_ports = ssi_config->num_ports;
+	for (port = 0, offset = 7; port < ssi_config->num_ports;
+							port++, offset += 5) {
+		cawake_gpio = ssi_config->cawake_gpio[port];
+		if (!cawake_gpio)
+			continue; /* Nothing to do */
+		err = gpio_request(cawake_gpio, "cawake");
+		if (err < 0)
+			goto rback;
+		gpio_direction_input(cawake_gpio);
+		ssi_resources[offset].start = gpio_to_irq(cawake_gpio);
+		ssi_resources[offset].flags &= ~IORESOURCE_UNSET;
+		ssi_resources[offset].flags |= IORESOURCE_IRQ_HIGHEDGE |
+							IORESOURCE_IRQ_LOWEDGE;
+	}
+
+	return 0;
+rback:
+	dev_err(&ssi_pdev.dev, "Request cawake (gpio%d) failed\n", cawake_gpio);
+	while (port > 0)
+		gpio_free(ssi_config->cawake_gpio[--port]);
+
+	return err;
+}
+
+static int __init ssi_init(void)
+{
+	return platform_device_register(&ssi_pdev);
+}
+subsys_initcall(ssi_init);
diff --git a/arch/arm/mach-ux500/Kconfig b/arch/arm/mach-ux500/Kconfig
index f8b9392ee34..06254c7b14a 100644
--- a/arch/arm/mach-ux500/Kconfig
+++ b/arch/arm/mach-ux500/Kconfig
@@ -4,23 +4,31 @@ config UX500_SOC_COMMON
 	bool
 	default y
 	select ARM_GIC
-	select HAS_MTU
+	select NOMADIK_GPIO
 	select ARM_ERRATA_753970
+	select SYS_SOC
+
+config UX500_SOC_DBX500
+	depends on UX500_SOC_DB5500 || UX500_SOC_DB8500
+	bool
 
 menu "Ux500 SoC"
 
 config UX500_SOC_DB5500
 	bool "DB5500"
 	select MFD_DB5500_PRCMU
+	select REGULATOR_DB5500_PRCMU
+	select UX500_SOC_DBX500
 
 config UX500_SOC_DB8500
 	bool "DB8500"
 	select MFD_DB8500_PRCMU
 	select REGULATOR_DB8500_PRCMU
+	select UX500_SOC_DBX500
 
 endmenu
 
-menu "Ux500 target platform"
+menu "Ux500 target platform (boards)"
 
 config MACH_U8500
 	bool "U8500 Development platform"
@@ -29,6 +37,20 @@ config MACH_U8500
 	help
 	  Include support for the mop500 development platform.
 
+config MACH_HREFV60
+       bool "U85000 Development platform, HREFv60 version"
+       depends on UX500_SOC_DB8500
+	select MACH_U8500
+       help
+         Include support for the HREFv60 new development platform.
+
+config MACH_SNOWBALL
+	bool "U8500 Snowball platform"
+	depends on UX500_SOC_DB8500
+	select MACH_U8500
+	help
+	  Include support for the snowball development platform.
+
 config MACH_U5500
 	bool "U5500 Development platform"
 	depends on UX500_SOC_DB5500
@@ -36,6 +58,56 @@ config MACH_U5500
 	  Include support for the U5500 development platform.
 endmenu
 
+choice
+	prompt "Ux500 UIB Keylayout"
+	default KEYLAYOUT_LAYOUT1
+
+config KEYLAYOUT_LAYOUT1
+	bool "UIB Keylayout 1; for generic users"
+	help
+	  Supported keylayout for some numerics, power/call buttons,
+	  volume control etc
+
+config KEYLAYOUT_LAYOUT2
+	bool "UIB Keylayout 2; for connectivity users"
+	help
+	  Supports keylayout numerics 0-9, left/right/up/down/back/
+	  enter keys and special character "."(dot)
+
+endchoice
+
+choice
+	prompt "Ux500 sched_clock timer"
+	default UX500_MTU_TIMER
+
+config UX500_PRCMU_TIMER
+	bool "PRCMU Timer sched_clock"
+	depends on !HAS_MTU
+	help
+	  Add support for an always on sched_clock, required for
+	  proper cpuidle and suspend.
+
+config UX500_MTU_TIMER
+	bool "Multi Timer Unit sched_clock"
+	help
+	  Add sched_clock support for the Multi Timer Unit.
+	  Since mtu isn't always on cpuidle will not
+	  work with this clock.
+
+endchoice
+
+config UX500_PRCMU_QOS_POWER
+	bool "UX500 PRCMU power QoS support"
+	default y
+	help
+	  Add support for PRCMU power Quality of Service
+
+config UX500_PRCMU_DEBUG
+	bool "PRCMU debug"
+	depends on DEBUG_FS
+	help
+	  Add support for PRCMU debug
+
 config UX500_DEBUG_UART
 	int "Ux500 UART to use for low-level debug"
 	default 2
@@ -43,6 +115,12 @@ config UX500_DEBUG_UART
 	  Choose the UART on which kernel low-level debug messages should be
 	  output.
 
+config U8500_REGULATOR_DEBUG
+	bool "Regulator debug support"
+	depends on REGULATOR_VIRTUAL_CONSUMER
+	help
+	  Add support for U8500 regulator debug
+
 config U5500_MODEM_IRQ
 	bool "Modem IRQ support"
 	depends on UX500_SOC_DB5500
@@ -50,11 +128,56 @@ config U5500_MODEM_IRQ
 	help
 	  Add support for handling IRQ:s from modem side
 
-config U5500_MBOX
-	bool "Mailbox support"
-	depends on U5500_MODEM_IRQ
+config TEE_UX500
+	bool "Trusted Execution Environment (TEE) ux500 hardware support"
+	depends on (TEE_SUPPORT && MACH_U8500)
+	default y
+	help
+	  Adds TEE hardware support for ux500 platforms.
+
+config TEE_SVP
+	bool "Trusted Execution Environment (TEE) ux500 SVP support"
+	depends on (TEE_SUPPORT && (MACH_U8500_SIMULATOR || MACH_U5500_SIMULATOR))
+	default y
+	help
+	  Adds TEE support for SVP in ux500 platforms.
+
+config DB8500_MLOADER
+	bool "Modem firmware upload/download support"
+	depends on UX500_SOC_DB8500
+	select DBX500_MLOADER
+	help
+	  Adds Modem firmware upload/download support to DB8500.
+
+config U5500_MLOADER
+	bool "mLoader, mem config from kernel boot args exported to sysfs"
+	depends on UX500_SOC_DB5500
+	help
+	  Link between boot args and user space program that loads the modem ELF.
+	  This is used to expose the modem parameters using sysfs interface.
+
+config UX500_DEBUG_HWREG
+	bool "Debug hardware registers from userspace"
+	depends on (DEBUG_FS && UX500_SOC_DB8500)
+	help
+	  Adds various debug files to access registers.
+	  This should never ever be used for anything else than debugging.
+
+config UX500_DEBUG_NO_LAUTERBACH
+       bool "Disable clocks needed for Lauterbach debugging"
+       help
+	  Disable clocks needed for Lauterbach debugging at boot.
+	  If yes, you will reduce the power consumption.
+
+config UX500_L2X0_PREFETCH_CTRL
+	bool "PL310 prefetch control"
+	depends on (UX500_SOC_DB8500 || UX500_SOC_DB5500) && \
+			(TEE_UX500 && CACHE_L2X0)
 	default y
 	help
-	  Add support for U5500 mailbox communication with modem side
+	  Adds interface to control instruction and data prefetch.
+	  Communication with Trustzone is done through TEE driver.
 
+source "arch/arm/mach-ux500/Kconfig-arch"
+source "arch/arm/mach-ux500/pm/Kconfig"
 endif
diff --git a/arch/arm/mach-ux500/Kconfig-arch b/arch/arm/mach-ux500/Kconfig-arch
new file mode 100644
index 00000000000..7f7a52de9eb
--- /dev/null
+++ b/arch/arm/mach-ux500/Kconfig-arch
@@ -0,0 +1,160 @@
+config U8500_SECURE
+	bool "Support for running in Secure mode"
+	default n
+	help
+		Build the kernel to run in Secure mode.
+
+#Configuration for MCDE setup
+
+if FB_MCDE
+
+menu "Display selection"
+
+config DISPLAY_GENERIC_PRIMARY
+	bool "Generic primary display support"
+	depends on (MACH_U8500 || MACH_U5500)
+	default y
+
+choice
+	prompt "Display port type"
+	depends on DISPLAY_GENERIC_PRIMARY
+	default DISPLAY_GENERIC_DSI_PRIMARY
+	help
+	  Select the kind of display port used for the primary display
+
+config DISPLAY_GENERIC_DSI_PRIMARY
+	bool "DSI display"
+	select MCDE_DISPLAY_GENERIC_DSI
+	help
+	  Say yes here when using a DSI display
+
+config MCDE_DISPLAY_DPI_PRIMARY
+	bool "DPI display"
+	select MCDE_DISPLAY_DPI
+	depends on MACH_U8500
+	help
+	  Say yes here when using a DPI display
+
+endchoice
+
+choice
+	prompt "Color depth"
+	depends on DISPLAY_GENERIC_PRIMARY
+	default MCDE_DISPLAY_PRIMARY_16BPP
+	help
+	  Select color depth for primary display
+
+config MCDE_DISPLAY_PRIMARY_16BPP
+	bool "16 bpp"
+	help
+	  16 bpp color depth
+
+config MCDE_DISPLAY_PRIMARY_32BPP
+	bool "32 bpp"
+	help
+	  32 bpp color depth
+
+endchoice
+
+choice DISPLAY_GENERIC_DSI_PRIMARY_ROTATION
+	prompt "Enable main display rotation"
+	depends on DISPLAY_GENERIC_DSI_PRIMARY
+	default DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_90
+	help
+	  Set rotation of main display
+
+config DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_0
+	bool "0 degrees"
+config DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_90
+	bool "90 degrees"
+config DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_180
+	bool "180 degrees"
+config DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_270
+	bool "270 degrees"
+endchoice
+
+config DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_ANGLE
+	int
+	depends on DISPLAY_GENERIC_DSI_PRIMARY
+	default "0" if DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_0
+	default "90" if DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_90
+	default "180" if DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_180
+	default "270" if DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_270
+
+config DISPLAY_SONY_SY35560_DSI_PRIMARY
+	bool "Sony SY35560 primary display support"
+	depends on MACH_U8500_PDP
+	select MCDE_DISPLAY_SONY_SY35560_DSI
+	default y
+	help
+	  Say yes here if main display exists
+
+config DISPLAY_GENERIC_DSI_PRIMARY_VSYNC
+	bool "Enable v-sync for primary display"
+	depends on DISPLAY_GENERIC_DSI_PRIMARY || DISPLAY_SONY_SY35560_DSI_PRIMARY
+	default n
+	help
+	  Say yes to enable v-sync for primary display
+
+config DISPLAY_GENERIC_DSI_PRIMARY_AUTO_SYNC
+	bool "Enable auto sync for primary display"
+	depends on DISPLAY_GENERIC_DSI_PRIMARY
+	default n
+	help
+	  Say yes to enable auto sync for primary display
+
+config SONY_SY35560_ENABLE_ESD_CHECK
+	bool "Enable esd status check for primary display"
+	depends on DISPLAY_SONY_SY35560_DSI_PRIMARY
+	default n
+	help
+	  Say yes to enable esd status check for primary display
+
+config DISPLAY_GENERIC_DSI_SECONDARY
+	bool "Sub display support"
+	depends on MACH_U8500
+	select MCDE_DISPLAY_GENERIC_DSI
+	help
+	  Say yes here if sub display exists
+
+config DISPLAY_GENERIC_DSI_SECONDARY_VSYNC
+	bool "Enable v-sync for secondary display"
+	depends on DISPLAY_GENERIC_DSI_SECONDARY
+	help
+	  Say yes to enable v-sync for secondary display
+
+config DISPLAY_GENERIC_DSI_SECONDARY_AUTO_SYNC
+	bool "Enable auto sync for secondary display"
+	depends on DISPLAY_GENERIC_DSI_SECONDARY
+	help
+	  Say yes to enable auto sync for secondary display
+
+config DISPLAY_AB8500_TERTIARY
+	bool "AB8500 TVout display support"
+	depends on MACH_U8500 && !AV8100_SDTV
+	select MCDE_DISPLAY_AB8500_DENC
+	help
+	  Say yes here if tv out support
+
+config DISPLAY_AV8100_TERTIARY
+	bool "AV8100 HDMI/CVBS display support"
+	depends on (MACH_U8500 || MACH_U5500)
+	select MCDE_DISPLAY_AV8100
+	help
+	  Say yes here if HDMI output support
+
+config DISPLAY_AV8100_TRIPPLE_BUFFER
+	bool "Enable tripple buffer for HDMI display"
+	depends on DISPLAY_AV8100_TERTIARY
+	help
+	  Say yes to enable tripple buffer. You'll get double buffer otherwise
+
+config DISPLAY_FICTIVE
+	bool "DISPLAY fictive"
+	default n
+	---help---
+	  Say Y if you want a fictive display that doesn't access hardware
+
+endmenu
+
+endif
diff --git a/arch/arm/mach-ux500/Makefile b/arch/arm/mach-ux500/Makefile
index 1694916e682..6304c43a6d8 100644
--- a/arch/arm/mach-ux500/Makefile
+++ b/arch/arm/mach-ux500/Makefile
@@ -2,19 +2,47 @@
 # Makefile for the linux kernel, U8500 machine.
 #
 
-obj-y				:= clock.o cpu.o devices.o devices-common.o \
-				   id.o usb.o
-obj-$(CONFIG_UX500_SOC_DB5500)	+= cpu-db5500.o dma-db5500.o
-obj-$(CONFIG_UX500_SOC_DB8500)	+= cpu-db8500.o devices-db8500.o
+obj-y				:= clock.o cpu.o devices.o dcache.o \
+				   devices-common.o id.o pins.o \
+				   timer.o timer-mtu.o usb.o reboot_reasons.o
+obj-y				+= pm/
+ifeq ($(CONFIG_UX500_SOC_DB5500), y)
+obj-$(CONFIG_UX500_SOC_DBX500)	+= cpu-db5500.o dma-db5500.o \
+				   devices-db5500.o clock-db5500.o \
+				   clock-debug.o
+endif
+ifeq ($(CONFIG_UX500_SOC_DB8500), y)
+obj-$(CONFIG_UX500_SOC_DBX500)	+= cpu-db8500.o devices-db8500.o \
+				   clock-db8500.o
+endif
 obj-$(CONFIG_MACH_U8500)	+= board-mop500.o board-mop500-sdi.o \
 				board-mop500-regulators.o \
 				board-mop500-uib.o board-mop500-stuib.o \
-				board-mop500-u8500uib.o \
-				board-mop500-pins.o
-obj-$(CONFIG_MACH_U5500)	+= board-u5500.o board-u5500-sdi.o
+				board-mop500-u8500uib.o board-mop500-pins.o \
+				board-mop500-mcde.o \
+				board-mop500-msp.o board-mop500-bm.o \
+				board-pins-sleep-force.o \
+				board-mop500-wlan.o
+obj-$(CONFIG_ANDROID_STE_TIMED_VIBRA)	+= board-mop500-vibra.o
+obj-$(CONFIG_MACH_U5500)	+= board-u5500.o board-u5500-sdi.o \
+				   board-u5500-mcde.o board-u5500-regulators.o \
+				   board-u5500-pins.o
+obj-$(CONFIG_U5500_MMIO)	+= board-u5500-mmio.o
+obj-$(CONFIG_U8500_MMIO)	+= board-mop500-mmio.o
 obj-$(CONFIG_SMP)		+= platsmp.o headsmp.o
 obj-$(CONFIG_HOTPLUG_CPU)	+= hotplug.o
 obj-$(CONFIG_LOCAL_TIMERS)	+= localtimer.o
 obj-$(CONFIG_U5500_MODEM_IRQ)	+= modem-irq-db5500.o
-obj-$(CONFIG_U5500_MBOX)	+= mbox-db5500.o
-
+obj-$(CONFIG_TEE_UX500) 	+= tee_ux500.o
+obj-$(CONFIG_TEE_SVP)		+= tee_service_svp.o
+obj-$(CONFIG_TEE_SVP)		+= tee_ta_start_modem_svp.o
+obj-$(CONFIG_DB8500_MLOADER) 	+= mloader-db8500.o
+obj-$(CONFIG_U5500_MLOADER)	+= mloader-db5500.o
+obj-$(CONFIG_UX500_PRCMU_TIMER)	+= timer-prcmu.o
+obj-$(CONFIG_U8500_REGULATOR_DEBUG)	+= virt-regulator-u8500.o
+obj-$(CONFIG_UX500_PRCMU_DEBUG)		+= prcmu-debug.o
+obj-$(CONFIG_UX500_PRCMU_QOS_POWER)	+= prcmu-qos-power.o
+obj-$(CONFIG_UX500_DEBUG_HWREG) 	+= hwreg.o
+obj-$(CONFIG_HWMEM)			+= hwmem-int.o
+obj-$(CONFIG_UX500_L2X0_PREFETCH_CTRL)	+= l2x0-prefetch.o
+obj-$(CONFIG_AB5500_BM)			+= board-u5500-bm.o
diff --git a/arch/arm/mach-ux500/board-mop500-bm.c b/arch/arm/mach-ux500/board-mop500-bm.c
new file mode 100644
index 00000000000..66ced8e088e
--- /dev/null
+++ b/arch/arm/mach-ux500/board-mop500-bm.c
@@ -0,0 +1,418 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * U8500 board specific charger and battery initialization parameters.
+ *
+ * Author: Johan Palsson <johan.palsson@stericsson.com> for ST-Ericsson.
+ * Author: Johan Gardsmark <johan.gardsmark@stericsson.com> for ST-Ericsson.
+ *
+ */
+
+#include <linux/power_supply.h>
+#include <linux/mfd/ab8500/bm.h>
+#include "board-mop500-bm.h"
+
+#ifdef CONFIG_AB8500_BATTERY_THERM_ON_BATCTRL
+/*
+ * These are the defined batteries that uses a NTC and ID resistor placed
+ * inside of the battery pack.
+ * Note that the res_to_temp table must be strictly sorted by falling resistance
+ * values to work.
+ */
+static struct res_to_temp temp_tbl_A[] = {
+	{-5, 53407},
+	{ 0, 48594},
+	{ 5, 43804},
+	{10, 39188},
+	{15, 34870},
+	{20, 30933},
+	{25, 27422},
+	{30, 24347},
+	{35, 21694},
+	{40, 19431},
+	{45, 17517},
+	{50, 15908},
+	{55, 14561},
+	{60, 13437},
+	{65, 12500},
+};
+static struct res_to_temp temp_tbl_B[] = {
+	{-5, 165418},
+	{ 0, 159024},
+	{ 5, 151921},
+	{10, 144300},
+	{15, 136424},
+	{20, 128565},
+	{25, 120978},
+	{30, 113875},
+	{35, 107397},
+	{40, 101629},
+	{45,  96592},
+	{50,  92253},
+	{55,  88569},
+	{60,  85461},
+	{65,  82869},
+};
+static struct v_to_cap cap_tbl_A[] = {
+	{4171,	100},
+	{4114,	 95},
+	{4009,	 83},
+	{3947,	 74},
+	{3907,	 67},
+	{3863,	 59},
+	{3830,	 56},
+	{3813,	 53},
+	{3791,	 46},
+	{3771,	 33},
+	{3754,	 25},
+	{3735,	 20},
+	{3717,	 17},
+	{3681,	 13},
+	{3664,	  8},
+	{3651,	  6},
+	{3635,	  5},
+	{3560,	  3},
+	{3408,    1},
+	{3247,	  0},
+};
+static struct v_to_cap cap_tbl_B[] = {
+	{4161,	100},
+	{4124,	 98},
+	{4044,	 90},
+	{4003,	 85},
+	{3966,	 80},
+	{3933,	 75},
+	{3888,	 67},
+	{3849,	 60},
+	{3813,	 55},
+	{3787,	 47},
+	{3772,	 30},
+	{3751,	 25},
+	{3718,	 20},
+	{3681,	 16},
+	{3660,	 14},
+	{3589,	 10},
+	{3546,	  7},
+	{3495,	  4},
+	{3404,	  2},
+	{3250,	  0},
+};
+#endif
+static struct v_to_cap cap_tbl[] = {
+	{4186,	100},
+	{4163,	 99},
+	{4114,	 95},
+	{4068,	 90},
+	{3990,	 80},
+	{3926,	 70},
+	{3898,	 65},
+	{3866,	 60},
+	{3833,	 55},
+	{3812,	 50},
+	{3787,	 40},
+	{3768,	 30},
+	{3747,	 25},
+	{3730,	 20},
+	{3705,	 15},
+	{3699,	 14},
+	{3684,	 12},
+	{3672,	  9},
+	{3657,	  7},
+	{3638,	  6},
+	{3556,	  4},
+	{3424,	  2},
+	{3317,	  1},
+	{3094,	  0},
+};
+
+/*
+ * Note that the res_to_temp table must be strictly sorted by falling
+ * resistance values to work.
+ */
+static struct res_to_temp temp_tbl[] = {
+	{-5, 214834},
+	{ 0, 162943},
+	{ 5, 124820},
+	{10,  96520},
+	{15,  75306},
+	{20,  59254},
+	{25,  47000},
+	{30,  37566},
+	{35,  30245},
+	{40,  24520},
+	{45,  20010},
+	{50,  16432},
+	{55,  13576},
+	{60,  11280},
+	{65,   9425},
+};
+
+static const struct battery_type bat_type[] = {
+	[BATTERY_UNKNOWN] = {
+		/* First element always represent the UNKNOWN battery */
+		.name = POWER_SUPPLY_TECHNOLOGY_UNKNOWN,
+		.resis_high = 0,
+		.resis_low = 0,
+		.battery_resistance = 300,
+		.charge_full_design = 612,
+		.nominal_voltage = 3700,
+		.termination_vol = 4050,
+		.termination_curr = 200,
+		.recharge_vol = 3990,
+		.normal_cur_lvl = 400,
+		.normal_vol_lvl = 4100,
+		.maint_a_cur_lvl = 400,
+		.maint_a_vol_lvl = 4050,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 400,
+		.maint_b_vol_lvl = 4000,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl),
+		.r_to_t_tbl = temp_tbl,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl),
+		.v_to_cap_tbl = cap_tbl,
+	},
+
+#ifdef CONFIG_AB8500_BATTERY_THERM_ON_BATCTRL
+	{
+		.name = POWER_SUPPLY_TECHNOLOGY_LIPO,
+		.resis_high = 53407,
+		.resis_low = 12500,
+		.battery_resistance = 300,
+		.charge_full_design = 900,
+		.nominal_voltage = 3600,
+		.termination_vol = 4150,
+		.termination_curr = 80,
+		.recharge_vol = 4130,
+		.normal_cur_lvl = 700,
+		.normal_vol_lvl = 4200,
+		.maint_a_cur_lvl = 600,
+		.maint_a_vol_lvl = 4150,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 600,
+		.maint_b_vol_lvl = 4100,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl_A),
+		.r_to_t_tbl = temp_tbl_A,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl_A),
+		.v_to_cap_tbl = cap_tbl_A,
+
+	},
+	{
+		.name = POWER_SUPPLY_TECHNOLOGY_LIPO,
+		.resis_high = 165418,
+		.resis_low = 82869,
+		.battery_resistance = 300,
+		.charge_full_design = 900,
+		.nominal_voltage = 3600,
+		.termination_vol = 4150,
+		.termination_curr = 80,
+		.recharge_vol = 4130,
+		.normal_cur_lvl = 700,
+		.normal_vol_lvl = 4200,
+		.maint_a_cur_lvl = 600,
+		.maint_a_vol_lvl = 4150,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 600,
+		.maint_b_vol_lvl = 4100,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl_B),
+		.r_to_t_tbl = temp_tbl_B,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl_B),
+		.v_to_cap_tbl = cap_tbl_B,
+	},
+#else
+/*
+ * These are the batteries that doesn't have an internal NTC resistor to measure
+ * its temperature. The temperature in this case is measure with a NTC placed
+ * near the battery but on the PCB.
+ */
+	{
+		.name = POWER_SUPPLY_TECHNOLOGY_LIPO,
+		.resis_high = 76000,
+		.resis_low = 53000,
+		.battery_resistance = 300,
+		.charge_full_design = 900,
+		.nominal_voltage = 3700,
+		.termination_vol = 4150,
+		.termination_curr = 100,
+		.recharge_vol = 4130,
+		.normal_cur_lvl = 700,
+		.normal_vol_lvl = 4200,
+		.maint_a_cur_lvl = 600,
+		.maint_a_vol_lvl = 4150,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 600,
+		.maint_b_vol_lvl = 4100,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl),
+		.r_to_t_tbl = temp_tbl,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl),
+		.v_to_cap_tbl = cap_tbl,
+	},
+	{
+		.name = POWER_SUPPLY_TECHNOLOGY_LION,
+		.resis_high = 30000,
+		.resis_low = 10000,
+		.battery_resistance = 300,
+		.charge_full_design = 950,
+		.nominal_voltage = 3700,
+		.termination_vol = 4150,
+		.termination_curr = 100,
+		.recharge_vol = 4130,
+		.normal_cur_lvl = 700,
+		.normal_vol_lvl = 4200,
+		.maint_a_cur_lvl = 600,
+		.maint_a_vol_lvl = 4150,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 600,
+		.maint_b_vol_lvl = 4100,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl),
+		.r_to_t_tbl = temp_tbl,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl),
+		.v_to_cap_tbl = cap_tbl,
+	},
+	{
+		.name = POWER_SUPPLY_TECHNOLOGY_LION,
+		.resis_high = 95000,
+		.resis_low = 76001,
+		.battery_resistance = 300,
+		.charge_full_design = 950,
+		.nominal_voltage = 3700,
+		.termination_vol = 4150,
+		.termination_curr = 100,
+		.recharge_vol = 4130,
+		.normal_cur_lvl = 700,
+		.normal_vol_lvl = 4200,
+		.maint_a_cur_lvl = 600,
+		.maint_a_vol_lvl = 4150,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 600,
+		.maint_b_vol_lvl = 4100,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl),
+		.r_to_t_tbl = temp_tbl,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl),
+		.v_to_cap_tbl = cap_tbl,
+	},
+#endif
+};
+
+static char *ab8500_charger_supplied_to[] = {
+	"ab8500_chargalg",
+	"ab8500_fg",
+	"ab8500_btemp",
+};
+
+static char *ab8500_btemp_supplied_to[] = {
+	"ab8500_chargalg",
+	"ab8500_fg",
+};
+
+static char *ab8500_fg_supplied_to[] = {
+	"ab8500_chargalg",
+};
+
+static char *ab8500_chargalg_supplied_to[] = {
+	"ab8500_fg",
+};
+
+struct ab8500_charger_platform_data ab8500_charger_plat_data = {
+	.supplied_to = ab8500_charger_supplied_to,
+	.num_supplicants = ARRAY_SIZE(ab8500_charger_supplied_to),
+};
+
+struct ab8500_btemp_platform_data ab8500_btemp_plat_data = {
+	.supplied_to = ab8500_btemp_supplied_to,
+	.num_supplicants = ARRAY_SIZE(ab8500_btemp_supplied_to),
+};
+
+struct ab8500_fg_platform_data ab8500_fg_plat_data = {
+	.supplied_to = ab8500_fg_supplied_to,
+	.num_supplicants = ARRAY_SIZE(ab8500_fg_supplied_to),
+};
+
+struct ab8500_chargalg_platform_data ab8500_chargalg_plat_data = {
+	.supplied_to = ab8500_chargalg_supplied_to,
+	.num_supplicants = ARRAY_SIZE(ab8500_chargalg_supplied_to),
+};
+
+static const struct ab8500_bm_capacity_levels cap_levels = {
+	.critical	= 2,
+	.low		= 10,
+	.normal		= 70,
+	.high		= 95,
+	.full		= 100,
+};
+
+static const struct ab8500_fg_parameters fg = {
+	.recovery_sleep_timer = 10,
+	.recovery_total_time = 100,
+	.init_timer = 1,
+	.init_discard_time = 5,
+	.init_total_time = 40,
+	.high_curr_time = 60,
+	.accu_charging = 30,
+	.accu_high_curr = 30,
+	.high_curr_threshold = 50,
+	.lowbat_threshold = 3100,
+};
+
+static const struct ab8500_maxim_parameters maxi_params = {
+	.ena_maxi = true,
+	.chg_curr = 910,
+	.wait_cycles = 10,
+	.charger_curr_step = 100,
+};
+
+static const struct ab8500_bm_charger_parameters chg = {
+	.usb_volt_max		= 5500,
+	.usb_curr_max		= 1500,
+	.ac_volt_max		= 7500,
+	.ac_curr_max		= 1500,
+};
+
+struct ab8500_bm_data ab8500_bm_data = {
+	.temp_under		= 3,
+	.temp_low		= 8,
+	.temp_high		= 55,
+	.temp_over		= 60,
+	.main_safety_tmr_h	= 4,
+	.usb_safety_tmr_h	= 4,
+	.bkup_bat_v		= BUP_VCH_SEL_2P6V,
+	.bkup_bat_i		= BUP_ICH_SEL_150UA,
+	.no_maintenance		= false,
+#ifdef CONFIG_AB8500_BATTERY_THERM_ON_BATCTRL
+	.adc_therm		= ADC_THERM_BATCTRL,
+#else
+	.adc_therm		= ADC_THERM_BATTEMP,
+#endif
+	.chg_unknown_bat	= false,
+	.enable_overshoot	= false,
+	.fg_res			= 10,
+	.cap_levels		= &cap_levels,
+	.bat_type		= bat_type,
+	.n_btypes		= ARRAY_SIZE(bat_type),
+	.batt_id		= 0,
+	.interval_charging	= 5,
+	.interval_not_charging	= 120,
+	.temp_hysteresis	= 3,
+	.maxi			= &maxi_params,
+	.chg_params		= &chg,
+	.fg_params		= &fg,
+};
diff --git a/arch/arm/mach-ux500/board-mop500-bm.h b/arch/arm/mach-ux500/board-mop500-bm.h
new file mode 100644
index 00000000000..eb2450f1ab5
--- /dev/null
+++ b/arch/arm/mach-ux500/board-mop500-bm.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * U8500 board specific charger and battery initialization parameters.
+ *
+ * Author: Johan Palsson <johan.palsson@stericsson.com> for ST-Ericsson.
+ * Author: Johan Gardsmark <johan.gardsmark@stericsson.com> for ST-Ericsson.
+ *
+ */
+
+#ifndef __BOARD_MOP500_BM_H
+#define __BOARD_MOP500_BM_H
+
+#include <linux/mfd/ab8500/bm.h>
+
+extern struct ab8500_charger_platform_data ab8500_charger_plat_data;
+extern struct ab8500_btemp_platform_data ab8500_btemp_plat_data;
+extern struct ab8500_fg_platform_data ab8500_fg_plat_data;
+extern struct ab8500_chargalg_platform_data ab8500_chargalg_plat_data;
+extern struct ab8500_bm_data ab8500_bm_data;
+
+#endif
diff --git a/arch/arm/mach-ux500/board-mop500-mcde.c b/arch/arm/mach-ux500/board-mop500-mcde.c
new file mode 100644
index 00000000000..79c09f98a50
--- /dev/null
+++ b/arch/arm/mach-ux500/board-mop500-mcde.c
@@ -0,0 +1,729 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/gpio.h>
+#include <linux/mfd/ab8500/denc.h>
+#include <linux/workqueue.h>
+#include <asm/mach-types.h>
+#include <video/av8100.h>
+#include <video/mcde_display.h>
+#include <video/mcde_display-generic_dsi.h>
+#include <video/mcde_display-vuib500-dpi.h>
+#include <video/mcde_display-av8100.h>
+#include <video/mcde_display-ab8500.h>
+#include <video/mcde_fb.h>
+#include <video/mcde_dss.h>
+#include <plat/pincfg.h>
+#include "pins-db8500.h"
+#include "pins.h"
+#include "board-mop500.h"
+
+#define DSI_UNIT_INTERVAL_0	0x9
+#define DSI_UNIT_INTERVAL_1	0x9
+#define DSI_UNIT_INTERVAL_2	0x5
+
+#ifdef CONFIG_FB_MCDE
+
+/* The initialization of hdmi disp driver must be delayed in order to
+ * ensure that inputclk will be available (needed by hdmi hw) */
+#ifdef CONFIG_DISPLAY_AV8100_TERTIARY
+static struct delayed_work work_dispreg_hdmi;
+#define DISPREG_HDMI_DELAY 6000
+#endif
+
+enum {
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_PRIMARY
+	PRIMARY_DISPLAY_ID,
+#endif
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_SECONDARY
+	SECONDARY_DISPLAY_ID,
+#endif
+#ifdef CONFIG_DISPLAY_FICTIVE
+	FICTIVE_DISPLAY_ID,
+#endif
+#ifdef CONFIG_DISPLAY_AV8100_TERTIARY
+	AV8100_DISPLAY_ID,
+#endif
+#ifdef CONFIG_DISPLAY_AB8500_TERTIARY
+	AB8500_DISPLAY_ID,
+#endif
+	MCDE_NR_OF_DISPLAYS
+};
+static int display_initialized_during_boot;
+
+static int __init startup_graphics_setup(char *str)
+{
+
+	if (get_option(&str, &display_initialized_during_boot) != 1)
+		display_initialized_during_boot = 0;
+
+	switch (display_initialized_during_boot) {
+	case 1:
+		pr_info("Startup graphics support\n");
+		break;
+	case 0:
+	default:
+		pr_info("No startup graphics supported\n");
+		break;
+	};
+
+	return 1;
+}
+__setup("startup_graphics=", startup_graphics_setup);
+
+#if defined(CONFIG_DISPLAY_AB8500_TERTIARY) ||\
+					defined(CONFIG_DISPLAY_AV8100_TERTIARY)
+static struct mcde_col_transform rgb_2_yCbCr_transform = {
+	.matrix = {
+		{0x0042, 0x0081, 0x0019},
+		{0xffda, 0xffb6, 0x0070},
+		{0x0070, 0xffa2, 0xffee},
+	},
+	.offset = {0x10, 0x80, 0x80},
+};
+#endif
+
+#ifdef CONFIG_DISPLAY_FICTIVE
+static struct mcde_display_device fictive_display = {
+	.name = "mcde_disp_fictive",
+	.id = FICTIVE_DISPLAY_ID,
+	.fictive = true,
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGB565,
+	.native_x_res = 1280,
+	.native_y_res = 720,
+};
+#endif /* CONFIG_DISPLAY_FICTIVE */
+
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_PRIMARY
+static struct mcde_port port0 = {
+	.type = MCDE_PORTTYPE_DSI,
+	.mode = MCDE_PORTMODE_CMD,
+	.pixel_format = MCDE_PORTPIXFMT_DSI_24BPP,
+	.ifc = 1,
+	.link = 0,
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_AUTO_SYNC
+	.sync_src = MCDE_SYNCSRC_OFF,
+	.update_auto_trig = true,
+#else
+	.sync_src = MCDE_SYNCSRC_BTA,
+	.update_auto_trig = false,
+#endif
+	.phy = {
+		.dsi = {
+			.virt_id = 0,
+			.num_data_lanes = 2,
+			.ui = DSI_UNIT_INTERVAL_0,
+			.clk_cont = false,
+			.data_lanes_swap = false,
+		},
+	},
+};
+
+static struct mcde_display_generic_platform_data generic_display0_pdata = {
+	.reset_delay = 1,
+#ifdef CONFIG_REGULATOR
+	.regulator_id = "vaux12v5",
+	.min_supply_voltage = 2500000, /* 2.5V */
+	.max_supply_voltage = 2700000 /* 2.7V */
+#endif
+};
+
+static struct mcde_display_device generic_display0 = {
+	.name = "mcde_disp_generic",
+	.id = PRIMARY_DISPLAY_ID,
+	.port = &port0,
+	.chnl_id = MCDE_CHNL_A,
+	/*
+	 * A large fifo is needed when ddr is clocked down to 25% to not get
+	 * latency problems.
+	 */
+	.fifo = MCDE_FIFO_A,
+#ifdef CONFIG_MCDE_DISPLAY_PRIMARY_16BPP
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGB565,
+#else
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGBA8888,
+#endif
+	.native_x_res = 864,
+	.native_y_res = 480,
+	.synchronized_update = false,
+	/* TODO: Remove rotation buffers once ESRAM driver is completed */
+	.rotbuf1 = U8500_ESRAM_BASE + 0x20000 * 4,
+	.rotbuf2 = U8500_ESRAM_BASE + 0x20000 * 4 + 0x10000,
+	.dev = {
+		.platform_data = &generic_display0_pdata,
+	},
+};
+#endif /* CONFIG_DISPLAY_GENERIC_DSI_PRIMARY */
+
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_SECONDARY
+static struct mcde_port subdisplay_port = {
+	.type = MCDE_PORTTYPE_DSI,
+	.mode = MCDE_PORTMODE_CMD,
+	.pixel_format = MCDE_PORTPIXFMT_DSI_24BPP,
+	.ifc = 1,
+	.link = 1,
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_SECONDARY_AUTO_SYNC
+	.sync_src = MCDE_SYNCSRC_OFF,
+	.update_auto_trig = true,
+#else
+	.sync_src = MCDE_SYNCSRC_BTA,
+	.update_auto_trig = false,
+#endif
+	.phy = {
+		.dsi = {
+			.virt_id = 0,
+			.num_data_lanes = 2,
+			.ui = DSI_UNIT_INTERVAL_1,
+			.clk_cont = false,
+			.data_lanes_swap = false,
+		},
+	},
+
+};
+
+static struct mcde_display_generic_platform_data generic_subdisplay_pdata = {
+	.reset_delay = 1,
+#ifdef CONFIG_REGULATOR
+	.regulator_id = "v-display",
+	.min_supply_voltage = 2500000, /* 2.5V */
+	.max_supply_voltage = 2700000 /* 2.7V */
+#endif
+};
+
+static struct mcde_display_device generic_subdisplay = {
+	.name = "mcde_disp_generic_subdisplay",
+	.id = SECONDARY_DISPLAY_ID,
+	.port = &subdisplay_port,
+	.chnl_id = MCDE_CHNL_C1,
+	.fifo = MCDE_FIFO_C1,
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGB565,
+	.native_x_res = 864,
+	.native_y_res = 480,
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_SECONDARY_VSYNC
+	.synchronized_update = true,
+#else
+	.synchronized_update = false,
+#endif
+	.dev = {
+		.platform_data = &generic_subdisplay_pdata,
+	},
+};
+#endif /* CONFIG_DISPLAY_GENERIC_DSI_SECONDARY */
+
+#ifdef CONFIG_MCDE_DISPLAY_DPI_PRIMARY
+static struct mcde_port port0 = {
+	.type = MCDE_PORTTYPE_DPI,
+	.pixel_format = MCDE_PORTPIXFMT_DPI_24BPP,
+	.ifc = 0,
+	.link = 1,		/* DPI channel B can only be on link 1 */
+	.sync_src = MCDE_SYNCSRC_OFF,   /* sync from output formatter  */
+	.update_auto_trig = true,
+	.phy = {
+		.dpi = {
+			.tv_mode = false,
+			.clock_div = 2,
+			.polarity = DPI_ACT_LOW_VSYNC | DPI_ACT_LOW_HSYNC,
+		},
+	},
+};
+
+static struct mcde_display_dpi_platform_data generic_display0_pdata = {0};
+static struct ux500_pins *dpi_pins;
+
+static int dpi_display_platform_enable(struct mcde_display_device *ddev)
+{
+	int res;
+
+	if (!dpi_pins) {
+		dpi_pins = ux500_pins_get("mcde-dpi");
+		if (!dpi_pins)
+			return -EINVAL;
+	}
+
+	dev_info(&ddev->dev, "%s\n", __func__);
+	res = ux500_pins_enable(dpi_pins);
+	if (res)
+		dev_warn(&ddev->dev, "Failure during %s\n", __func__);
+
+	return res;
+}
+
+static int dpi_display_platform_disable(struct mcde_display_device *ddev)
+{
+	int res;
+
+	dev_info(&ddev->dev, "%s\n", __func__);
+
+	res = ux500_pins_disable(dpi_pins);
+	if (res)
+		dev_warn(&ddev->dev, "Failure during %s\n", __func__);
+
+	return res;
+
+}
+
+static struct mcde_display_device generic_display0 = {
+	.name = "mcde_display_dpi",
+	.id = 0,
+	.port = &port0,
+	.chnl_id = MCDE_CHNL_B,
+	.fifo = MCDE_FIFO_B,
+#ifdef CONFIG_MCDE_DISPLAY_PRIMARY_16BPP
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGB565,
+#else
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGBA8888,
+#endif
+	.native_x_res = 640,
+	.native_y_res = 480,
+	/* .synchronized_update: Don't care: port is set to update_auto_trig */
+	.dev = {
+		.platform_data = &generic_display0_pdata,
+	},
+	.platform_enable = dpi_display_platform_enable,
+	.platform_disable = dpi_display_platform_disable,
+};
+#endif /* CONFIG_MCDE_DISPLAY_DPI_PRIMARY */
+
+#ifdef CONFIG_DISPLAY_AB8500_TERTIARY
+static struct mcde_port port_tvout1 = {
+	.type = MCDE_PORTTYPE_DPI,
+	.pixel_format = MCDE_PORTPIXFMT_DPI_24BPP,
+	.ifc = 0,
+	.link = 1,			/* channel B */
+	.sync_src = MCDE_SYNCSRC_OFF,
+	.update_auto_trig = true,
+	.phy = {
+		.dpi = {
+			.bus_width = 4, /* DDR mode */
+			.tv_mode = true,
+			.clock_div = MCDE_PORT_DPI_NO_CLOCK_DIV,
+		},
+	},
+};
+
+static struct ab8500_display_platform_data ab8500_display_pdata = {
+	.nr_regulators = 2,
+	.regulator_id  = {"v-tvout", "v-ab8500-AV-switch"},
+	.rgb_2_yCbCr_transform = &rgb_2_yCbCr_transform,
+};
+
+static struct ux500_pins *tvout_pins;
+
+static int ab8500_platform_enable(struct mcde_display_device *ddev)
+{
+	int res = 0;
+
+	if (!tvout_pins) {
+		tvout_pins = ux500_pins_get("mcde-tvout");
+		if (!tvout_pins)
+			return -EINVAL;
+	}
+
+	dev_info(&ddev->dev, "%s\n", __func__);
+	res = ux500_pins_enable(tvout_pins);
+	if (res != 0)
+		goto failed;
+
+	return res;
+
+failed:
+	dev_warn(&ddev->dev, "Failure during %s\n", __func__);
+	return res;
+}
+
+static int ab8500_platform_disable(struct mcde_display_device *ddev)
+{
+	int res;
+
+	dev_info(&ddev->dev, "%s\n", __func__);
+
+	res = ux500_pins_disable(tvout_pins);
+	if (res != 0)
+		goto failed;
+	return res;
+
+failed:
+	dev_warn(&ddev->dev, "Failure during %s\n", __func__);
+	return res;
+}
+
+struct mcde_display_device tvout_ab8500_display = {
+	.name = "mcde_tv_ab8500",
+	.id = AB8500_DISPLAY_ID,
+	.port = &port_tvout1,
+	.chnl_id = MCDE_CHNL_B,
+	.fifo = MCDE_FIFO_B,
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGB565,
+	.native_x_res = 720,
+	.native_y_res = 576,
+	/* .synchronized_update: Don't care: port is set to update_auto_trig */
+	.dev = {
+		.platform_data = &ab8500_display_pdata,
+	},
+
+	/*
+	* We might need to describe the std here:
+	* - there are different PAL / NTSC formats (do they require MCDE
+	*   settings?)
+	*/
+	.platform_enable = ab8500_platform_enable,
+	.platform_disable = ab8500_platform_disable,
+};
+#endif /* CONFIG_DISPLAY_AB8500_TERTIARY */
+
+#ifdef CONFIG_DISPLAY_AV8100_TERTIARY
+static struct mcde_port port2 = {
+	.type = MCDE_PORTTYPE_DSI,
+	.mode = MCDE_PORTMODE_CMD,
+	.pixel_format = MCDE_PORTPIXFMT_DSI_24BPP,
+	.ifc = 1,
+	.link = 2,
+#ifdef CONFIG_AV8100_HWTRIG_INT
+	.sync_src = MCDE_SYNCSRC_TE0,
+#endif
+#ifdef CONFIG_AV8100_HWTRIG_I2SDAT3
+	.sync_src = MCDE_SYNCSRC_TE1,
+#endif
+#ifdef CONFIG_AV8100_HWTRIG_DSI_TE
+	.sync_src = MCDE_SYNCSRC_TE_POLLING,
+#endif
+#ifdef CONFIG_AV8100_HWTRIG_NONE
+	.sync_src = MCDE_SYNCSRC_OFF,
+#endif
+	.update_auto_trig = true,
+	.phy = {
+		.dsi = {
+			.virt_id = 0,
+			.num_data_lanes = 2,
+			.ui = DSI_UNIT_INTERVAL_2,
+			.clk_cont = false,
+			.data_lanes_swap = false,
+		},
+	},
+	.hdmi_sdtv_switch = HDMI_SWITCH,
+};
+
+static struct mcde_display_hdmi_platform_data av8100_hdmi_pdata = {
+	.reset_gpio = 0,
+	.reset_delay = 1,
+	.regulator_id = NULL, /* TODO: "display_main" */
+	.cvbs_regulator_id = "v-av8100-AV-switch",
+	.ddb_id = 1,
+	.rgb_2_yCbCr_transform = &rgb_2_yCbCr_transform,
+};
+
+static struct mcde_display_device av8100_hdmi = {
+	.name = "av8100_hdmi",
+	.id = AV8100_DISPLAY_ID,
+	.port = &port2,
+	.chnl_id = MCDE_CHNL_B,
+	.fifo = MCDE_FIFO_B,
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGB565,
+	.native_x_res = 1280,
+	.native_y_res = 720,
+	.dev = {
+		.platform_data = &av8100_hdmi_pdata,
+	},
+	.platform_enable = NULL,
+	.platform_disable = NULL,
+};
+
+static void delayed_work_dispreg_hdmi(struct work_struct *ptr)
+{
+	if (mcde_display_device_register(&av8100_hdmi))
+		pr_warning("Failed to register av8100_hdmi\n");
+}
+#endif /* CONFIG_DISPLAY_AV8100_TERTIARY */
+
+/*
+* This function will create the framebuffer for the display that is registered.
+*/
+static int display_postregistered_callback(struct notifier_block *nb,
+	unsigned long event, void *dev)
+{
+	struct mcde_display_device *ddev = dev;
+	u16 width, height;
+	u16 virtual_width, virtual_height;
+	u32 rotate = FB_ROTATE_UR;
+	struct fb_info *fbi;
+
+	if (event != MCDE_DSS_EVENT_DISPLAY_REGISTERED)
+		return 0;
+
+	if (ddev->id < 0 || ddev->id >= MCDE_NR_OF_DISPLAYS)
+		return 0;
+
+	mcde_dss_get_native_resolution(ddev, &width, &height);
+
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_PRIMARY
+	if (ddev->id == PRIMARY_DISPLAY_ID) {
+		switch (CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_ANGLE) {
+		case 0:
+			rotate = FB_ROTATE_UR;
+			break;
+		case 90:
+			rotate = FB_ROTATE_CW;
+			swap(width, height);
+			break;
+		case 180:
+			rotate = FB_ROTATE_UD;
+			break;
+		case 270:
+			rotate = FB_ROTATE_CCW;
+			swap(width, height);
+			break;
+		}
+	}
+#endif
+
+	virtual_width = width;
+	virtual_height = height * 2;
+
+#if defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY) &&	\
+			defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_AUTO_SYNC)
+	if (ddev->id == PRIMARY_DISPLAY_ID)
+		virtual_height = height;
+#endif
+
+#if defined(CONFIG_DISPLAY_GENERIC_DSI_SECONDARY) &&	\
+			defined(CONFIG_DISPLAY_GENERIC_DSI_SECONDARY_AUTO_SYNC)
+	if (ddev->id == SECONDARY_DISPLAY_ID)
+		virtual_height = height;
+#endif
+
+#ifdef CONFIG_DISPLAY_AV8100_TRIPPLE_BUFFER
+	if (ddev->id == AV8100_DISPLAY_ID)
+		virtual_height = height * 3;
+#endif
+
+	if (ddev->id == AV8100_DISPLAY_ID) {
+#ifdef CONFIG_MCDE_DISPLAY_HDMI_FB_AUTO_CREATE
+		hdmi_fb_onoff(ddev, 1, 0, 0);
+#endif /* CONFIG_MCDE_DISPLAY_HDMI_FB_AUTO_CREATE */
+	} else {
+		/* Create frame buffer */
+		fbi = mcde_fb_create(ddev,
+			width, height,
+			virtual_width, virtual_height,
+			ddev->default_pixel_format,
+			rotate);
+
+		if (IS_ERR(fbi)) {
+			dev_warn(&ddev->dev,
+				"Failed to create fb for display %s\n",
+						ddev->name);
+			goto display_postregistered_callback_err;
+		} else
+			dev_info(&ddev->dev, "Framebuffer created (%s)\n",
+						ddev->name);
+	}
+
+	return 0;
+
+display_postregistered_callback_err:
+	return -1;
+}
+
+static struct notifier_block display_nb = {
+	.notifier_call = display_postregistered_callback,
+};
+
+/*
+* This function is used to refresh the display (lcd, hdmi, tvout) with black
+* when the framebuffer is registered.
+* The main display will not be updated if startup graphics is displayed
+* from u-boot.
+*/
+#if defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_AUTO_SYNC) || \
+			defined(CONFIG_DISPLAY_GENERIC_DSI_SECONDARY_AUTO_SYNC)
+static int framebuffer_postregistered_callback(struct notifier_block *nb,
+	unsigned long event, void *data)
+{
+	int ret = 0;
+	struct fb_event *event_data = data;
+	struct fb_info *info;
+	struct fb_var_screeninfo var;
+	struct fb_fix_screeninfo fix;
+	struct mcde_fb *mfb;
+	int i;
+
+	if (event != FB_EVENT_FB_REGISTERED)
+		return 0;
+
+	if (!event_data)
+		return 0;
+
+	info = event_data->info;
+	mfb = to_mcde_fb(info);
+	var = info->var;
+	fix = info->fix;
+
+	/* Apply overlay info */
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		struct mcde_overlay *ovly = mfb->ovlys[i];
+		struct mcde_overlay_info ovly_info;
+		struct mcde_fb *mfb = to_mcde_fb(info);
+		int num_buffers;
+		memset(&ovly_info, 0, sizeof(ovly_info));
+		ovly_info.paddr = fix.smem_start +
+			fix.line_length * var.yoffset;
+		if (ovly_info.paddr + fix.line_length * var.yres
+			 > fix.smem_start + fix.smem_len)
+			ovly_info.paddr = fix.smem_start;
+		ovly_info.fmt = mfb->pix_fmt;
+		ovly_info.stride = fix.line_length;
+		ovly_info.w = var.xres;
+		ovly_info.h = var.yres;
+		ovly_info.dirty.w = var.xres;
+		ovly_info.dirty.h = var.yres;
+		(void) mcde_dss_apply_overlay(ovly, &ovly_info);
+		num_buffers = var.yres_virtual / var.yres;
+		ret = mcde_dss_update_overlay(ovly, num_buffers == 3);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+#else
+static int framebuffer_postregistered_callback(struct notifier_block *nb,
+	unsigned long event, void *data)
+{
+	int ret = 0;
+	struct fb_event *event_data = data;
+	struct fb_info *info;
+	struct fb_var_screeninfo var;
+	struct fb_fix_screeninfo fix;
+	struct mcde_fb *mfb;
+
+	if (event != FB_EVENT_FB_REGISTERED)
+		return 0;
+
+	if (!event_data)
+		return 0;
+
+	info = event_data->info;
+	mfb = to_mcde_fb(info);
+	if (mfb->id == 0 && display_initialized_during_boot)
+		goto out;
+
+	var = info->var;
+	fix = info->fix;
+	var.yoffset = var.yoffset ? 0 : var.yres;
+	if (info->fbops->fb_pan_display)
+		ret = info->fbops->fb_pan_display(&var, info);
+out:
+	return ret;
+}
+#endif
+
+
+static struct notifier_block framebuffer_nb = {
+	.notifier_call = framebuffer_postregistered_callback,
+};
+
+int __init init_display_devices(void)
+{
+	int ret = 0;
+
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_VSYNC
+	struct i2c_adapter *i2c0;
+#endif
+
+	if (!(machine_is_u8500() || machine_is_snowball()))
+		return ret;
+
+	ret = fb_register_client(&framebuffer_nb);
+	if (ret)
+		pr_warning("Failed to register framebuffer notifier\n");
+
+	ret = mcde_dss_register_notifier(&display_nb);
+	if (ret)
+		pr_warning("Failed to register dss notifier\n");
+#ifdef CONFIG_DISPLAY_FICTIVE
+	ret = mcde_display_device_register(&fictive_display);
+	if (ret)
+		pr_warning("Failed to register fictive display device\n");
+#endif
+
+#ifdef CONFIG_DISPLAY_GENERIC_PRIMARY
+	if (machine_is_hrefv60())
+		generic_display0_pdata.reset_gpio = HREFV60_DISP1_RST_GPIO;
+	else
+		generic_display0_pdata.reset_gpio = MOP500_DISP1_RST_GPIO;
+
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_VSYNC
+	i2c0 = i2c_get_adapter(0);
+	if (i2c0) {
+		/*
+		* U8500-UIB has the TC35893 at 0x44 on I2C0, the
+		* ST-UIB has not.
+		*/
+		ret = i2c_smbus_xfer(i2c0, 0x44, 0, I2C_SMBUS_WRITE, 0,
+							I2C_SMBUS_QUICK, NULL);
+		i2c_put_adapter(i2c0);
+
+		/* ret == 0 => U8500 UIB connected */
+		generic_display0.synchronized_update = (ret == 0);
+	}
+#endif
+
+	if (display_initialized_during_boot)
+		generic_display0.power_mode = MCDE_DISPLAY_PM_STANDBY;
+	ret = mcde_display_device_register(&generic_display0);
+	if (ret)
+		pr_warning("Failed to register generic display device 0\n");
+#endif
+
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_SECONDARY
+	if (machine_is_hrefv60())
+		generic_subdisplay_pdata.reset_gpio = HREFV60_DISP2_RST_GPIO;
+	else
+		generic_subdisplay_pdata.reset_gpio = MOP500_DISP2_RST_GPIO;
+	ret = mcde_display_device_register(&generic_subdisplay);
+	if (ret)
+		pr_warning("Failed to register generic sub display device\n");
+#endif
+
+#ifdef CONFIG_DISPLAY_AV8100_TERTIARY
+	INIT_DELAYED_WORK_DEFERRABLE(&work_dispreg_hdmi,
+			delayed_work_dispreg_hdmi);
+
+	schedule_delayed_work(&work_dispreg_hdmi,
+			msecs_to_jiffies(DISPREG_HDMI_DELAY));
+#endif
+#ifdef CONFIG_DISPLAY_AB8500_TERTIARY
+	ret = mcde_display_device_register(&tvout_ab8500_display);
+	if (ret)
+		pr_warning("Failed to register ab8500 tvout device\n");
+#endif
+
+	return ret;
+}
+
+struct mcde_display_device *mcde_get_main_display(void)
+{
+#if defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY)
+	return &generic_display0;
+#elif defined(CONFIG_DISPLAY_GENERIC_DSI_SECONDARY)
+	return &generic_subdisplay;
+#elif defined(CONFIG_DISPLAY_AV8100_TERTIARY)
+	return &av8100_hdmi;
+#elif defined(CONFIG_DISPLAY_AB8500_TERTIARY)
+	return &tvout_ab8500_display;
+#else
+	return NULL;
+#endif
+}
+EXPORT_SYMBOL(mcde_get_main_display);
+
+module_init(init_display_devices);
+
+#endif
diff --git a/arch/arm/mach-ux500/board-mop500-mcde.h b/arch/arm/mach-ux500/board-mop500-mcde.h
new file mode 100644
index 00000000000..66447c95828
--- /dev/null
+++ b/arch/arm/mach-ux500/board-mop500-mcde.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Author: Marcel Tunnissen <marcel.tuennissen@stericsson.com> for ST-Ericsson
+ *
+ * MOP500 board specific initialization for regulators
+ */
+
+#ifndef __BOARD_MOP500_MCDE_H
+#define __BOARD_MOP500_MCDE_H
+
+#include <video/mcde_display.h>
+
+#endif /* __BOARD_MOP500_MCDE_H */
diff --git a/arch/arm/mach-ux500/board-mop500-mmio.c b/arch/arm/mach-ux500/board-mop500-mmio.c
new file mode 100644
index 00000000000..71028bd2f8f
--- /dev/null
+++ b/arch/arm/mach-ux500/board-mop500-mmio.c
@@ -0,0 +1,511 @@
+/*
+ * Copyright (C) 2011 ST-Ericsson
+ * Author: Joakim Axelsson <joakim.axelsson@stericsson.com> for ST-Ericsson
+ * Author: Rajat Verma <rajat.verma@stericsson.com> for ST-Ericsson.
+ *
+ * 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/types.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/regulator/consumer.h>
+#include <linux/vmalloc.h>
+#include <asm/mach-types.h>
+#include <plat/pincfg.h>
+#include <mach/gpio.h>
+#include <mach/devices.h>
+#include <mach/hardware.h>
+
+#include "pins-db8500.h"
+#include "pins.h"
+#include "board-mop500.h"
+#include "../drivers/staging/mmio/mmio.h"
+
+static pin_cfg_t i2c2_pins[] = {
+	GPIO8_I2C2_SDA,
+	GPIO9_I2C2_SCL
+};
+static pin_cfg_t ipi2c_pins[] = {
+	GPIO8_IPI2C_SDA,
+	GPIO9_IPI2C_SCL
+};
+static pin_cfg_t i2c_disable_pins[] = {
+	GPIO8_GPIO,
+	GPIO9_GPIO
+};
+static pin_cfg_t xshutdown_host[] = {
+	GPIO141_GPIO,
+	GPIO142_GPIO
+};
+static pin_cfg_t xshutdown_fw[] = {
+	GPIO141_IP_GPIO2,
+	GPIO142_IP_GPIO3
+};
+static pin_cfg_t xshutdown_disable[] = {
+	GPIO141_GPIO | PIN_OUTPUT_LOW,
+	GPIO142_GPIO | PIN_OUTPUT_LOW
+};
+
+struct mmio_board_data{
+	int number_of_regulators;
+	struct regulator **mmio_regulators;
+	/* Pin configs  */
+	int xenon_charge;
+	struct mmio_gpio xshutdown_pins[CAMERA_SLOT_END];
+	/* Internal clocks */
+	struct clk *clk_ptr_bml;
+	struct clk *clk_ptr_ipi2c;
+	/* External clocks */
+	struct clk *clk_ptr_ext[CAMERA_SLOT_END];
+};
+
+/* Fill names of regulators required for powering up the
+ * camera sensor in below array */
+static char *regulator_names[] = {"v-mmio-camera" , "v-ana"};
+
+/* This function is used to translate the physical GPIO used for reset GPIO
+ * to logical IPGPIO that needs to be communicated to Firmware. so that
+ * firmware can control reset GPIO of a RAW Bayer sensor */
+static int mmio_get_ipgpio(struct mmio_platform_data *pdata, int gpio,
+			   int *ip_gpio)
+{
+	int err = 0;
+	dev_dbg(pdata->dev, "%s() : IPGPIO requested for %d", __func__, gpio);
+	switch (gpio) {
+	case 67:
+	case 140:
+		*ip_gpio = 7;
+		break;
+	case 5:
+	case 66:
+		*ip_gpio = 6;
+		break;
+	case 81:
+	case 65:
+		*ip_gpio = 5;
+		break;
+	case 80:
+	case 64:
+		*ip_gpio = 4;
+		break;
+	case 10:
+	case 79:
+	case 142:
+		*ip_gpio = 3;
+		break;
+	case 11:
+	case 78:
+	case 141:
+		*ip_gpio = 2;
+		break;
+	case 7:
+	case 150:
+		*ip_gpio = 1;
+		break;
+	case 6:
+	case 149:
+		*ip_gpio = 0;
+		break;
+	default:
+		*ip_gpio = -1;
+		err = -1;
+		break;
+	}
+	return err;
+}
+
+static int mmio_clock_init(struct mmio_platform_data *pdata)
+{
+	int err;
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+
+	extra->clk_ptr_bml = clk_get_sys("bml", NULL);
+	if (IS_ERR(extra->clk_ptr_bml)) {
+		err = PTR_ERR(extra->clk_ptr_bml);
+		dev_err(pdata->dev, "Error %d getting clock 'bml'\n", err);
+		goto err_bml_clk;
+	}
+	extra->clk_ptr_ipi2c = clk_get_sys("ipi2", NULL);
+	if (IS_ERR(extra->clk_ptr_ipi2c)) {
+		err = PTR_ERR(extra->clk_ptr_ipi2c);
+		dev_err(pdata->dev, "Error %d getting clock 'ipi2'\n", err);
+		goto err_ipi2c_clk;
+	}
+	extra->clk_ptr_ext[PRIMARY_CAMERA] = clk_get_sys("pri-cam", NULL);
+	if (IS_ERR(extra->clk_ptr_ext[PRIMARY_CAMERA])) {
+		err = PTR_ERR(extra->clk_ptr_ext[PRIMARY_CAMERA]);
+		dev_err(pdata->dev, "Error %d getting clock 'pri-cam'\n", err);
+		goto err_pri_ext_clk;
+	}
+	extra->clk_ptr_ext[SECONDARY_CAMERA] = clk_get_sys("sec-cam", NULL);
+	if (IS_ERR(extra->clk_ptr_ext[SECONDARY_CAMERA])) {
+		err = PTR_ERR(extra->clk_ptr_ext[SECONDARY_CAMERA]);
+		dev_err(pdata->dev, "Error %d getting clock 'sec-cam'\n", err);
+		goto err_sec_ext_clk;
+	}
+	dev_dbg(pdata->dev , "Board %s() Exit\n", __func__);
+	return 0;
+err_sec_ext_clk:
+	clk_put(extra->clk_ptr_ext[PRIMARY_CAMERA]);
+err_pri_ext_clk:
+	clk_put(extra->clk_ptr_ipi2c);
+err_ipi2c_clk:
+	clk_put(extra->clk_ptr_bml);
+err_bml_clk:
+	return err;
+}
+static void mmio_clock_exit(struct mmio_platform_data *pdata)
+{
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	clk_put(extra->clk_ptr_bml);
+	clk_put(extra->clk_ptr_ipi2c);
+	clk_put(extra->clk_ptr_ext[PRIMARY_CAMERA]);
+	clk_put(extra->clk_ptr_ext[SECONDARY_CAMERA]);
+}
+
+
+static int mmio_pin_cfg_init(struct mmio_platform_data *pdata)
+{
+	int err;
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+
+	extra->xshutdown_pins[PRIMARY_CAMERA].gpio = XSHUTDOWN_PRIMARY_SENSOR;
+	extra->xshutdown_pins[PRIMARY_CAMERA].active_high = 0;
+	extra->xshutdown_pins[PRIMARY_CAMERA].udelay = 500;
+
+	extra->xshutdown_pins[SECONDARY_CAMERA].active_high = 0;
+	extra->xshutdown_pins[SECONDARY_CAMERA].udelay = 500;
+
+	/* Update GPIO mappings according to board */
+	if (machine_is_hrefv60()) {
+		extra->xenon_charge = HREFV60_MMIO_XENON_CHARGE;
+		xshutdown_host[SECONDARY_CAMERA] = GPIO140_GPIO;
+		xshutdown_fw[SECONDARY_CAMERA] = GPIO140_IP_GPIO7;
+		xshutdown_disable[SECONDARY_CAMERA] = GPIO140_GPIO;
+		extra->xshutdown_pins[SECONDARY_CAMERA].gpio = 140;
+	} else {
+		extra->xenon_charge = GPIO_MMIO_XENON_CHARGE;
+		xshutdown_host[SECONDARY_CAMERA] = GPIO142_GPIO;
+		xshutdown_fw[SECONDARY_CAMERA] = GPIO142_IP_GPIO3;
+		xshutdown_disable[SECONDARY_CAMERA] = GPIO142_GPIO;
+		extra->xshutdown_pins[SECONDARY_CAMERA].gpio = 142;
+	}
+	/* Setup Xenon Charge */
+	err = gpio_request(extra->xenon_charge, "xenon charge");
+	if (err) {
+		dev_err(pdata->dev, "Error %d while requesting xenon charge\n",
+			err);
+		goto err_xenon_gpio_req;
+	}
+	err = gpio_direction_output(extra->xenon_charge, 0);
+	if (err) {
+		dev_err(pdata->dev, "Error %d while setting xenon charge in"
+			"output mode\n", err);
+		goto err_xenon_gpio_set_dir;
+	}
+	dev_dbg(pdata->dev , "Board %s() Exit\n", __func__);
+	return 0;
+err_xenon_gpio_set_dir:
+	gpio_free(extra->xenon_charge);
+err_xenon_gpio_req:
+	return err;
+}
+
+static void mmio_pin_cfg_exit(struct mmio_platform_data *pdata)
+{
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	gpio_free(extra->xenon_charge);
+}
+
+/* For now, both sensors on HREF have some power up sequence. If different
+ * sequences are needed for primary and secondary sensors, it can be
+ * implemented easily. Just use camera_slot field of mmio_platform_data
+ * to determine which camera needs to be powered up */
+static int mmio_power_init(struct mmio_platform_data *pdata)
+{
+	int err = 0, i = 0;
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	extra->number_of_regulators = sizeof(regulator_names)/
+					sizeof(regulator_names[0]);
+	extra->mmio_regulators =
+	    kzalloc(sizeof(struct regulator *) * extra->number_of_regulators,
+		    GFP_KERNEL);
+	if (!extra->mmio_regulators) {
+		dev_err(pdata->dev , "Error while allocating memory for mmio"
+				"regulators\n");
+		err = -ENOMEM;
+		goto err_no_mem_reg;
+	}
+	for (i = 0; i <
+		extra->number_of_regulators; i++) {
+		extra->mmio_regulators[i] =
+			regulator_get(pdata->dev, regulator_names[i]);
+		if (IS_ERR(extra->mmio_regulators[i])) {
+			err = PTR_ERR(extra->mmio_regulators[i]);
+			dev_err(pdata->dev , "Error %d getting regulator '%s'"
+			"\n", err, regulator_names[i]);
+			goto err_regulator;
+		}
+	}
+	dev_dbg(pdata->dev , "Board %s() Exit\n", __func__);
+	return 0;
+err_regulator:
+	/* Return regulators we have already requested */
+	while (i--)
+		regulator_put(extra->mmio_regulators[i]);
+	kfree(extra->mmio_regulators);
+err_no_mem_reg:
+	return err;
+}
+static void mmio_power_exit(struct mmio_platform_data *pdata)
+{
+	int i = 0;
+	struct mmio_board_data *extra = pdata->extra;;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	for (i = 0; i < extra->number_of_regulators; i++)
+		regulator_put(extra->mmio_regulators[i]);
+	kfree(extra->mmio_regulators);
+}
+
+static int mmio_platform_init(struct mmio_platform_data *pdata)
+{
+	int err = 0;
+	struct mmio_board_data *extra = NULL;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	/* Alloc memory for our own extra data */
+	extra = kzalloc(sizeof(struct mmio_board_data), GFP_KERNEL);
+	if (!extra) {
+		dev_err(pdata->dev, "%s: memory alloc failed for "
+		"mmio_board_data\n", __func__);
+		err = -ENOMEM;
+		goto err_no_mem_extra;
+	}
+	/* Hook the data for other callbacks to use */
+	pdata->extra = extra;
+
+	pdata->camera_slot = -1;
+
+	err = mmio_power_init(pdata);
+	if (err)
+		goto err_regulator;
+	err = mmio_clock_init(pdata);
+	if (err)
+		goto err_clock;
+	err = mmio_pin_cfg_init(pdata);
+	if (err)
+		goto err_pin_cfg;
+	/* Store logical IPGPIO for physical reset GPIOs used */
+	err = mmio_get_ipgpio(pdata,
+		extra->xshutdown_pins[PRIMARY_CAMERA].gpio,
+		&(pdata->reset_ipgpio[PRIMARY_CAMERA]));
+	if (err) {
+		dev_err(pdata->dev, "Error getting ipgpio for pri cam\n");
+		goto err_ipgpio;
+	}
+	err = mmio_get_ipgpio(pdata,
+		extra->xshutdown_pins[SECONDARY_CAMERA].gpio,
+		&(pdata->reset_ipgpio[SECONDARY_CAMERA]));
+	if (err) {
+		dev_err(pdata->dev, "Error getting ipgpio for sec cam\n");
+		goto err_ipgpio;
+	}
+	dev_dbg(pdata->dev , "Board %s() Exit\n", __func__);
+	return 0;
+err_ipgpio:
+	mmio_pin_cfg_exit(pdata);
+err_pin_cfg:
+	mmio_clock_exit(pdata);
+err_clock:
+	mmio_power_exit(pdata);
+err_regulator:
+	kfree(extra);
+err_no_mem_extra:
+	return err;
+}
+static void mmio_platform_exit(struct mmio_platform_data *pdata)
+{
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	mmio_power_exit(pdata);
+	mmio_clock_exit(pdata);
+	mmio_pin_cfg_exit(pdata);
+	kfree(extra);
+	pdata->extra = NULL;
+}
+
+static int mmio_power_enable(struct mmio_platform_data *pdata)
+{
+	int err = 0, i = 0;
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	/* Enable the regulators */
+	for (i = 0; i < extra->number_of_regulators; i++) {
+		err = regulator_enable(extra->mmio_regulators[i]);
+		if (IS_ERR(extra->mmio_regulators[i])) {
+			err = PTR_ERR(extra->mmio_regulators[i]);
+			dev_err(pdata->dev , "Error %d enabling regulator '%s'"
+			"\n", err, regulator_names[i]);
+			goto err_regulator;
+		}
+	}
+	/* Set Xenon Charge */
+	gpio_set_value(extra->xenon_charge, 1);
+	dev_dbg(pdata->dev , "Board %s() Exit\n", __func__);
+	return 0;
+err_regulator:
+	/* Disable regulators we already enabled */
+	while (i--)
+		regulator_disable(extra->mmio_regulators[i]);
+	return err;
+}
+
+static void mmio_power_disable(struct mmio_platform_data *pdata)
+{
+	int i;
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	/* Disable the regulators */
+	for (i = 0; i < extra->number_of_regulators; i++)
+		regulator_disable(extra->mmio_regulators[i]);
+	/* Disable Xenon Charge */
+	gpio_set_value(extra->xenon_charge, 0);
+}
+static int mmio_clock_enable(struct mmio_platform_data *pdata)
+{
+	int err = 0;
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	/* Enable internal clocks */
+	err = clk_enable(extra->clk_ptr_bml);
+	if (err) {
+		dev_err(pdata->dev, "Error activating bml clock %d\n", err);
+		goto err_bml_clk;
+	}
+	err = clk_enable(extra->clk_ptr_ipi2c);
+	if (err) {
+		dev_err(pdata->dev, "Error activating i2c2 clock %d\n", err);
+		goto err_ipi2c_clk;
+	}
+	/* Enable appropriate external clock */
+	err = clk_enable(extra->clk_ptr_ext[pdata->camera_slot]);
+	if (err) {
+		dev_err(pdata->dev, "Error activating clock for sensor %d, err"
+			"%d\n", pdata->camera_slot, err);
+		goto err_ext_clk;
+	}
+	dev_dbg(pdata->dev , "Board %s() Exit\n", __func__);
+	return 0;
+err_ext_clk:
+	clk_disable(extra->clk_ptr_ipi2c);
+err_ipi2c_clk:
+	clk_disable(extra->clk_ptr_bml);
+err_bml_clk:
+	return err;
+}
+
+static void mmio_clock_disable(struct mmio_platform_data *pdata)
+{
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	clk_disable(extra->clk_ptr_bml);
+	clk_disable(extra->clk_ptr_ipi2c);
+	clk_disable(extra->clk_ptr_ext[pdata->camera_slot]);
+}
+
+
+static int mmio_config_xshutdown_pins(struct mmio_platform_data *pdata,
+				      enum mmio_select_xshutdown_t select,
+				      int is_active_high)
+{
+	int err = 0;
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	switch (select) {
+	case MMIO_ENABLE_XSHUTDOWN_HOST:
+		extra->xshutdown_pins[pdata->camera_slot].active_high =
+			is_active_high;
+		err = nmk_config_pin(xshutdown_host[pdata->camera_slot] |
+			(is_active_high ? PIN_OUTPUT_LOW : PIN_OUTPUT_HIGH),
+			0);
+		break;
+	case MMIO_ENABLE_XSHUTDOWN_FW:
+		err = nmk_config_pin(xshutdown_fw[pdata->camera_slot], 0);
+		break;
+	case MMIO_DISABLE_XSHUTDOWN:
+		err = nmk_config_pin(xshutdown_disable[pdata->camera_slot],
+			0);
+		break;
+	default:
+		break;
+	}
+	if (err)
+		dev_dbg(pdata->dev , "Error configuring xshutdown, err = %d\n",
+		err);
+	return err;
+}
+static void mmio_set_xshutdown(struct mmio_platform_data *pdata)
+{
+	struct mmio_board_data *extra = pdata->extra;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	gpio_set_value(extra->xshutdown_pins[pdata->camera_slot].gpio ,
+		(extra->xshutdown_pins[pdata->camera_slot].active_high ? 1 :
+		0));
+	udelay(extra->xshutdown_pins[pdata->camera_slot].udelay);
+}
+static int mmio_config_i2c_pins(struct mmio_platform_data *pdata,
+				enum mmio_select_i2c_t select)
+{
+	int err = 0;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	switch (select) {
+	case MMIO_ACTIVATE_I2C_HOST:
+		err = nmk_config_pins(i2c2_pins, ARRAY_SIZE(i2c2_pins));
+		break;
+	case MMIO_ACTIVATE_IPI2C2:
+		err = nmk_config_pins(ipi2c_pins, ARRAY_SIZE(ipi2c_pins));
+		break;
+	case MMIO_DEACTIVATE_I2C:
+		err = nmk_config_pins(i2c_disable_pins,
+			ARRAY_SIZE(i2c_disable_pins));
+		break;
+	default:
+		break;
+	}
+
+	return err;
+}
+static struct mmio_platform_data mmio_config = {
+	.platform_init = mmio_platform_init,
+	.platform_exit = mmio_platform_exit,
+	.power_enable = mmio_power_enable,
+	.power_disable = mmio_power_disable,
+	.clock_enable = mmio_clock_enable,
+	.clock_disable = mmio_clock_disable,
+	.config_i2c_pins = mmio_config_i2c_pins,
+	.config_xshutdown_pins = mmio_config_xshutdown_pins,
+	.set_xshutdown = mmio_set_xshutdown,
+	.sia_base = U8500_SIA_BASE,
+	.cr_base = U8500_CR_BASE
+};
+
+struct platform_device ux500_mmio_device = {
+	.name = MMIO_NAME,
+	.id = -1,
+	.dev = {
+		.platform_data = &mmio_config,
+	}
+};
diff --git a/arch/arm/mach-ux500/board-mop500-msp.c b/arch/arm/mach-ux500/board-mop500-msp.c
new file mode 100644
index 00000000000..631dbe30a40
--- /dev/null
+++ b/arch/arm/mach-ux500/board-mop500-msp.c
@@ -0,0 +1,217 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#include <linux/platform_device.h>
+#include <linux/i2s/i2s.h>
+#include <linux/init.h>
+#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
+
+#include <plat/ste_dma40.h>
+#include <plat/pincfg.h>
+
+#include <mach/devices.h>
+#include <mach/ste-dma40-db8500.h>
+#include <mach/hardware.h>
+#include <mach/irqs.h>
+#include <mach/msp.h>
+
+#include "board-mop500.h"
+#include "devices-db8500.h"
+#include "pins-db8500.h"
+
+/* MSP1/3 Tx/Rx usage protection */
+static DEFINE_SPINLOCK(msp_rxtx_lock);
+
+/* Reference Count */
+static int msp_rxtx_ref;
+
+static pin_cfg_t mop500_msp1_pins_init[] = {
+		GPIO33_MSP1_TXD | PIN_OUTPUT_LOW   | PIN_SLPM_WAKEUP_DISABLE,
+		GPIO34_MSP1_TFS | PIN_INPUT_NOPULL | PIN_SLPM_WAKEUP_DISABLE,
+		GPIO35_MSP1_TCK | PIN_INPUT_NOPULL | PIN_SLPM_WAKEUP_DISABLE,
+		GPIO36_MSP1_RXD | PIN_INPUT_NOPULL | PIN_SLPM_WAKEUP_DISABLE,
+};
+
+static pin_cfg_t mop500_msp1_pins_exit[] = {
+		GPIO33_MSP1_TXD | PIN_OUTPUT_LOW   | PIN_SLPM_WAKEUP_ENABLE,
+		GPIO34_MSP1_TFS | PIN_INPUT_NOPULL | PIN_SLPM_WAKEUP_ENABLE,
+		GPIO35_MSP1_TCK | PIN_INPUT_NOPULL | PIN_SLPM_WAKEUP_ENABLE,
+		GPIO36_MSP1_RXD | PIN_INPUT_NOPULL | PIN_SLPM_WAKEUP_ENABLE,
+};
+
+int msp13_i2s_init(void)
+{
+	int retval = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&msp_rxtx_lock, flags);
+	if (msp_rxtx_ref == 0)
+		retval = nmk_config_pins(
+				ARRAY_AND_SIZE(mop500_msp1_pins_init));
+	if (!retval)
+		msp_rxtx_ref++;
+	spin_unlock_irqrestore(&msp_rxtx_lock, flags);
+
+	return retval;
+}
+
+int msp13_i2s_exit(void)
+{
+	int retval = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&msp_rxtx_lock, flags);
+	WARN_ON(!msp_rxtx_ref);
+	msp_rxtx_ref--;
+	if (msp_rxtx_ref == 0)
+		retval = nmk_config_pins_sleep(
+				ARRAY_AND_SIZE(mop500_msp1_pins_exit));
+	spin_unlock_irqrestore(&msp_rxtx_lock, flags);
+
+	return retval;
+}
+
+static struct stedma40_chan_cfg msp0_dma_rx = {
+	.high_priority = true,
+	.dir = STEDMA40_PERIPH_TO_MEM,
+
+	.src_dev_type = DB8500_DMA_DEV31_MSP0_RX_SLIM0_CH0_RX,
+	.dst_dev_type = STEDMA40_DEV_DST_MEMORY,
+
+	.src_info.psize = STEDMA40_PSIZE_LOG_4,
+	.dst_info.psize = STEDMA40_PSIZE_LOG_4,
+
+	/* data_width is set during configuration */
+};
+
+static struct stedma40_chan_cfg msp0_dma_tx = {
+	.high_priority = true,
+	.dir = STEDMA40_MEM_TO_PERIPH,
+
+	.src_dev_type = STEDMA40_DEV_DST_MEMORY,
+	.dst_dev_type = DB8500_DMA_DEV31_MSP0_TX_SLIM0_CH0_TX,
+
+	.src_info.psize = STEDMA40_PSIZE_LOG_4,
+	.dst_info.psize = STEDMA40_PSIZE_LOG_4,
+
+	/* data_width is set during configuration */
+};
+
+static struct msp_i2s_platform_data msp0_platform_data = {
+	.id = MSP_0_I2S_CONTROLLER,
+	.msp_i2s_dma_rx = &msp0_dma_rx,
+	.msp_i2s_dma_tx = &msp0_dma_tx,
+};
+
+static struct stedma40_chan_cfg msp1_dma_rx = {
+	.high_priority = true,
+	.dir = STEDMA40_PERIPH_TO_MEM,
+
+	.src_dev_type = DB8500_DMA_DEV30_MSP3_RX,
+	.dst_dev_type = STEDMA40_DEV_DST_MEMORY,
+
+	.src_info.psize = STEDMA40_PSIZE_LOG_4,
+	.dst_info.psize = STEDMA40_PSIZE_LOG_4,
+
+	/* data_width is set during configuration */
+};
+
+static struct stedma40_chan_cfg msp1_dma_tx = {
+	.high_priority = true,
+	.dir = STEDMA40_MEM_TO_PERIPH,
+
+	.src_dev_type = STEDMA40_DEV_DST_MEMORY,
+	.dst_dev_type = DB8500_DMA_DEV30_MSP1_TX,
+
+	.src_info.psize = STEDMA40_PSIZE_LOG_4,
+	.dst_info.psize = STEDMA40_PSIZE_LOG_4,
+
+	/* data_width is set during configuration */
+};
+
+static struct msp_i2s_platform_data msp1_platform_data = {
+	.id = MSP_1_I2S_CONTROLLER,
+	.msp_i2s_dma_rx = NULL,
+	.msp_i2s_dma_tx = &msp1_dma_tx,
+	.msp_i2s_init = msp13_i2s_init,
+	.msp_i2s_exit = msp13_i2s_exit,
+};
+
+static struct stedma40_chan_cfg msp2_dma_rx = {
+	.high_priority = true,
+	.dir = STEDMA40_PERIPH_TO_MEM,
+
+	.src_dev_type = DB8500_DMA_DEV14_MSP2_RX,
+	.dst_dev_type = STEDMA40_DEV_DST_MEMORY,
+
+	/* MSP2 DMA doesn't work with PSIZE == 4 on DB8500v2 */
+	.src_info.psize = STEDMA40_PSIZE_LOG_1,
+	.dst_info.psize = STEDMA40_PSIZE_LOG_1,
+
+	/* data_width is set during configuration */
+};
+
+static struct stedma40_chan_cfg msp2_dma_tx = {
+	.high_priority = true,
+	.dir = STEDMA40_MEM_TO_PERIPH,
+
+	.src_dev_type = STEDMA40_DEV_DST_MEMORY,
+	.dst_dev_type = DB8500_DMA_DEV14_MSP2_TX,
+
+	.src_info.psize = STEDMA40_PSIZE_LOG_4,
+	.dst_info.psize = STEDMA40_PSIZE_LOG_4,
+
+	/* data_width is set during configuration */
+};
+
+static struct msp_i2s_platform_data msp2_platform_data = {
+	.id = MSP_2_I2S_CONTROLLER,
+	.msp_i2s_dma_rx = &msp2_dma_rx,
+	.msp_i2s_dma_tx = &msp2_dma_tx,
+};
+
+static struct msp_i2s_platform_data msp3_platform_data = {
+	.id		= MSP_3_I2S_CONTROLLER,
+	.msp_i2s_dma_rx	= &msp1_dma_rx,
+	.msp_i2s_dma_tx	= NULL,
+	.msp_i2s_init = msp13_i2s_init,
+	.msp_i2s_exit = msp13_i2s_exit,
+};
+
+static struct i2s_board_info stm_i2s_board_info[] __initdata = {
+	{
+		.modalias	= "i2s_device.0",
+		.id		= 0,
+		.chip_select	= 0,
+	},
+	{
+		.modalias	= "i2s_device.1",
+		.id		= 1,
+		.chip_select	= 1,
+	},
+	{
+		.modalias	= "i2s_device.2",
+		.id		= 2,
+		.chip_select	= 2,
+	},
+	{
+		.modalias	= "i2s_device.3",
+		.id		= 3,
+		.chip_select	= 3,
+	},
+};
+
+void __init mop500_msp_init(void)
+{
+	db8500_add_msp0_i2s(&msp0_platform_data);
+	db8500_add_msp1_i2s(&msp1_platform_data);
+	db8500_add_msp2_i2s(&msp2_platform_data);
+	db8500_add_msp3_i2s(&msp3_platform_data);
+
+	i2s_register_board_info(stm_i2s_board_info,
+				ARRAY_SIZE(stm_i2s_board_info));
+}
diff --git a/arch/arm/mach-ux500/board-mop500-pins.c b/arch/arm/mach-ux500/board-mop500-pins.c
index 70cdbd60596..52da47b6331 100644
--- a/arch/arm/mach-ux500/board-mop500-pins.c
+++ b/arch/arm/mach-ux500/board-mop500-pins.c
@@ -7,24 +7,29 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/gpio.h>
+#include <linux/io.h>
+#include <linux/string.h>
 
 #include <asm/mach-types.h>
 #include <plat/pincfg.h>
+#include <linux/gpio/nomadik.h>
+
 #include <mach/hardware.h>
+#include <mach/suspend.h>
 
 #include "pins-db8500.h"
+#include "pins.h"
 
-static pin_cfg_t mop500_pins_common[] = {
-	/* I2C */
-	GPIO147_I2C0_SCL,
-	GPIO148_I2C0_SDA,
-	GPIO16_I2C1_SCL,
-	GPIO17_I2C1_SDA,
-	GPIO10_I2C2_SDA,
-	GPIO11_I2C2_SCL,
-	GPIO229_I2C3_SDA,
-	GPIO230_I2C3_SCL,
+#include "board-pins-sleep-force.h"
+
+enum custom_pin_cfg_t {
+	PINS_FOR_DEFAULT,
+	PINS_FOR_U9500_21,
+};
+
+static enum custom_pin_cfg_t pinsfor = PINS_FOR_DEFAULT;
 
+static pin_cfg_t mop500_pins_common[] = {
 	/* MSP0 */
 	GPIO12_MSP0_TXD,
 	GPIO13_MSP0_TFS,
@@ -43,72 +48,6 @@ static pin_cfg_t mop500_pins_common[] = {
 	/* STMPE1601/tc35893 keypad  IRQ */
 	GPIO218_GPIO	| PIN_INPUT_PULLUP,
 
-	/* MMC0 (MicroSD card) */
-	GPIO18_MC0_CMDDIR	| PIN_OUTPUT_HIGH,
-	GPIO19_MC0_DAT0DIR	| PIN_OUTPUT_HIGH,
-	GPIO20_MC0_DAT2DIR	| PIN_OUTPUT_HIGH,
-
-	GPIO22_MC0_FBCLK	| PIN_INPUT_NOPULL,
-	GPIO23_MC0_CLK		| PIN_OUTPUT_LOW,
-	GPIO24_MC0_CMD		| PIN_INPUT_PULLUP,
-	GPIO25_MC0_DAT0		| PIN_INPUT_PULLUP,
-	GPIO26_MC0_DAT1		| PIN_INPUT_PULLUP,
-	GPIO27_MC0_DAT2		| PIN_INPUT_PULLUP,
-	GPIO28_MC0_DAT3		| PIN_INPUT_PULLUP,
-
-	/* SDI1 (SDIO) */
-	GPIO208_MC1_CLK		| PIN_OUTPUT_LOW,
-	GPIO209_MC1_FBCLK	| PIN_INPUT_NOPULL,
-	GPIO210_MC1_CMD		| PIN_INPUT_PULLUP,
-	GPIO211_MC1_DAT0	| PIN_INPUT_PULLUP,
-	GPIO212_MC1_DAT1	| PIN_INPUT_PULLUP,
-	GPIO213_MC1_DAT2	| PIN_INPUT_PULLUP,
-	GPIO214_MC1_DAT3	| PIN_INPUT_PULLUP,
-
-	/* MMC2 (On-board DATA INTERFACE eMMC) */
-	GPIO128_MC2_CLK		| PIN_OUTPUT_LOW,
-	GPIO129_MC2_CMD		| PIN_INPUT_PULLUP,
-	GPIO130_MC2_FBCLK	| PIN_INPUT_NOPULL,
-	GPIO131_MC2_DAT0	| PIN_INPUT_PULLUP,
-	GPIO132_MC2_DAT1	| PIN_INPUT_PULLUP,
-	GPIO133_MC2_DAT2	| PIN_INPUT_PULLUP,
-	GPIO134_MC2_DAT3	| PIN_INPUT_PULLUP,
-	GPIO135_MC2_DAT4	| PIN_INPUT_PULLUP,
-	GPIO136_MC2_DAT5	| PIN_INPUT_PULLUP,
-	GPIO137_MC2_DAT6	| PIN_INPUT_PULLUP,
-	GPIO138_MC2_DAT7	| PIN_INPUT_PULLUP,
-
-	/* MMC4 (On-board STORAGE INTERFACE eMMC) */
-	GPIO197_MC4_DAT3	| PIN_INPUT_PULLUP,
-	GPIO198_MC4_DAT2	| PIN_INPUT_PULLUP,
-	GPIO199_MC4_DAT1	| PIN_INPUT_PULLUP,
-	GPIO200_MC4_DAT0	| PIN_INPUT_PULLUP,
-	GPIO201_MC4_CMD		| PIN_INPUT_PULLUP,
-	GPIO202_MC4_FBCLK	| PIN_INPUT_NOPULL,
-	GPIO203_MC4_CLK		| PIN_OUTPUT_LOW,
-	GPIO204_MC4_DAT7	| PIN_INPUT_PULLUP,
-	GPIO205_MC4_DAT6	| PIN_INPUT_PULLUP,
-	GPIO206_MC4_DAT5	| PIN_INPUT_PULLUP,
-	GPIO207_MC4_DAT4	| PIN_INPUT_PULLUP,
-
-	/* SKE keypad */
-	GPIO153_KP_I7,
-	GPIO154_KP_I6,
-	GPIO155_KP_I5,
-	GPIO156_KP_I4,
-	GPIO157_KP_O7,
-	GPIO158_KP_O6,
-	GPIO159_KP_O5,
-	GPIO160_KP_O4,
-	GPIO161_KP_I3,
-	GPIO162_KP_I2,
-	GPIO163_KP_I1,
-	GPIO164_KP_I0,
-	GPIO165_KP_O3,
-	GPIO166_KP_O2,
-	GPIO167_KP_O1,
-	GPIO168_KP_O0,
-
 	/* UART */
 	/* uart-0 pins gpio configuration should be
 	 * kept intact to prevent glitch in tx line
@@ -131,6 +70,16 @@ static pin_cfg_t mop500_pins_common[] = {
 	/* Display & HDMI HW sync */
 	GPIO68_LCD_VSI0	| PIN_INPUT_PULLUP,
 	GPIO69_LCD_VSI1	| PIN_INPUT_PULLUP,
+
+	/* HSI */
+	GPIO219_HSIR_FLA0,
+	GPIO220_HSIR_DAT0,
+	GPIO221_HSIR_RDY0,
+	GPIO222_HSIT_FLA0,
+	GPIO223_HSIT_DAT0,
+	GPIO224_HSIT_RDY0,
+	GPIO225_GPIO	| PIN_INPUT_PULLDOWN, /* CA_WAKE0 */
+	GPIO226_GPIO	| PIN_OUTPUT_HIGH, /* AC_WAKE0 */
 };
 
 static pin_cfg_t mop500_pins_default[] = {
@@ -140,10 +89,13 @@ static pin_cfg_t mop500_pins_default[] = {
 	GPIO145_SSP0_RXD | PIN_PULL_DOWN,
 	GPIO146_SSP0_TXD,
 
+	/* XENON Flashgun INTERFACE */
+	GPIO6_IP_GPIO0	| PIN_INPUT_PULLUP,/* XENON_FLASH_ID */
+	GPIO7_IP_GPIO1	| PIN_INPUT_PULLUP,/* XENON_READY */
 
 	GPIO217_GPIO	| PIN_INPUT_PULLUP, /* TC35892 IRQ */
 
-	/* SDI0 (MicroSD card) */
+	/* sdi0 (removable MMC/SD/SDIO cards) not handled by pm_runtime */
 	GPIO21_MC0_DAT31DIR	| PIN_OUTPUT_HIGH,
 
 	/* UART */
@@ -161,7 +113,6 @@ static pin_cfg_t mop500_pins_hrefv60[] = {
 	/* XENON Flashgun INTERFACE */
 	GPIO6_IP_GPIO0	| PIN_INPUT_PULLUP,/* XENON_FLASH_ID */
 	GPIO7_IP_GPIO1	| PIN_INPUT_PULLUP,/* XENON_READY */
-	GPIO170_GPIO	| PIN_OUTPUT_LOW, /* XENON_CHARGE */
 
 	/* Assistant LED INTERFACE */
 	GPIO21_GPIO | PIN_OUTPUT_LOW,  /* XENON_EN1 */
@@ -218,7 +169,7 @@ static pin_cfg_t mop500_pins_hrefv60[] = {
 	GPIO145_GPIO	| PIN_INPUT_PULLDOWN,/* HAL_SW */
 
 	/* Audio Amplifier Interface */
-	GPIO149_GPIO	| PIN_OUTPUT_LOW, /* VAUDIO_HF_EN */
+	GPIO149_GPIO	| PIN_OUTPUT_HIGH, /* VAUDIO_HF_EN, enable MAX8968 */
 
 	/* GBF INTERFACE */
 	GPIO171_GPIO	| PIN_OUTPUT_LOW, /* GBF_ENA_RESET */
@@ -233,9 +184,717 @@ static pin_cfg_t mop500_pins_hrefv60[] = {
 	/* Proximity Sensor */
 	GPIO217_GPIO		| PIN_INPUT_PULLUP,
 
+	/* SD card detect */
+	GPIO95_GPIO	| PIN_INPUT_PULLUP,
+};
+
+static pin_cfg_t u9500_21_pins[] = {
+	GPIO4_U1_RXD    | PIN_INPUT_PULLUP,
+	GPIO5_U1_TXD    | PIN_OUTPUT_HIGH,
+};
+
+static pin_cfg_t snowball_pins[] = {
+	/* SSP0, to AB8500 */
+	GPIO143_SSP0_CLK,
+	GPIO144_SSP0_FRM,
+	GPIO145_SSP0_RXD	| PIN_PULL_DOWN,
+	GPIO146_SSP0_TXD,
+
+	/* MMC0: MicroSD card */
+	GPIO21_MC0_DAT31DIR     | PIN_OUTPUT_HIGH,
+
+	/* MMC2: LAN */
+	GPIO86_SM_ADQ0,
+	GPIO87_SM_ADQ1,
+	GPIO88_SM_ADQ2,
+	GPIO89_SM_ADQ3,
+	GPIO90_SM_ADQ4,
+	GPIO91_SM_ADQ5,
+	GPIO92_SM_ADQ6,
+	GPIO93_SM_ADQ7,
+
+	GPIO94_SM_ADVn,
+	GPIO95_SM_CS0n,
+	GPIO96_SM_OEn,
+	GPIO97_SM_WEn,
+
+	GPIO128_SM_CKO,
+	GPIO130_SM_FBCLK,
+	GPIO131_SM_ADQ8,
+	GPIO132_SM_ADQ9,
+	GPIO133_SM_ADQ10,
+	GPIO134_SM_ADQ11,
+	GPIO135_SM_ADQ12,
+	GPIO136_SM_ADQ13,
+	GPIO137_SM_ADQ14,
+	GPIO138_SM_ADQ15,
+
+	/* RSTn_LAN */
+	GPIO141_GPIO		| PIN_OUTPUT_HIGH,
+
+	/* WLAN/GBF */
+	GPIO171_GPIO		| PIN_OUTPUT_HIGH,/* GBF_ENA */
+	GPIO215_GPIO		| PIN_OUTPUT_LOW,/* WLAN_ENA */
+	GPIO216_GPIO		| PIN_INPUT_PULLUP,/* WLAN_IRQ */
+};
+
+/*
+ * I2C
+ */
+
+static UX500_PINS(mop500_pins_i2c0,
+	GPIO147_I2C0_SCL,
+	GPIO148_I2C0_SDA,
+);
+
+static UX500_PINS(mop500_pins_i2c1,
+	GPIO16_I2C1_SCL,
+	GPIO17_I2C1_SDA,
+);
+
+static UX500_PINS(mop500_pins_i2c2,
+	GPIO10_I2C2_SDA,
+	GPIO11_I2C2_SCL,
+);
+
+static UX500_PINS(mop500_pins_i2c3,
+	GPIO229_I2C3_SDA,
+	GPIO230_I2C3_SCL,
+);
+
+static UX500_PINS(mop500_pins_mcde_dpi,
+	GPIO64_LCDB_DE,
+	GPIO65_LCDB_HSO,
+	GPIO66_LCDB_VSO,
+	GPIO67_LCDB_CLK,
+	GPIO70_LCD_D0,
+	GPIO71_LCD_D1,
+	GPIO72_LCD_D2,
+	GPIO73_LCD_D3,
+	GPIO74_LCD_D4,
+	GPIO75_LCD_D5,
+	GPIO76_LCD_D6,
+	GPIO77_LCD_D7,
+	GPIO153_LCD_D24,
+	GPIO154_LCD_D25,
+	GPIO155_LCD_D26,
+	GPIO156_LCD_D27,
+	GPIO157_LCD_D28,
+	GPIO158_LCD_D29,
+	GPIO159_LCD_D30,
+	GPIO160_LCD_D31,
+	GPIO161_LCD_D32,
+	GPIO162_LCD_D33,
+	GPIO163_LCD_D34,
+	GPIO164_LCD_D35,
+	GPIO165_LCD_D36,
+	GPIO166_LCD_D37,
+	GPIO167_LCD_D38,
+	GPIO168_LCD_D39,
+);
+
+static UX500_PINS(mop500_pins_mcde_tvout,
+	GPIO78_LCD_D8,
+	GPIO79_LCD_D9,
+	GPIO80_LCD_D10,
+	GPIO81_LCD_D11,
+	GPIO150_LCDA_CLK,
+);
+
+static UX500_PINS(mop500_pins_ske,
+	GPIO153_KP_I7 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO154_KP_I6 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO155_KP_I5 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO156_KP_I4 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO161_KP_I3 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO162_KP_I2 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO163_KP_I1 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO164_KP_I0 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO157_KP_O7 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO158_KP_O6 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO159_KP_O5 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO160_KP_O4 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO165_KP_O3 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO166_KP_O2 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO167_KP_O1 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO168_KP_O0 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+);
+
+/* sdi0 (removable MMC/SD/SDIO cards) */
+static UX500_PINS(mop500_pins_sdi0,
+	GPIO18_MC0_CMDDIR	| PIN_OUTPUT_HIGH,
+	GPIO19_MC0_DAT0DIR	| PIN_OUTPUT_HIGH,
+	GPIO20_MC0_DAT2DIR	| PIN_OUTPUT_HIGH,
+
+	GPIO22_MC0_FBCLK	| PIN_INPUT_NOPULL,
+	GPIO23_MC0_CLK		| PIN_OUTPUT_LOW,
+	GPIO24_MC0_CMD		| PIN_INPUT_PULLUP,
+	GPIO25_MC0_DAT0		| PIN_INPUT_PULLUP,
+	GPIO26_MC0_DAT1		| PIN_INPUT_PULLUP,
+	GPIO27_MC0_DAT2		| PIN_INPUT_PULLUP,
+	GPIO28_MC0_DAT3		| PIN_INPUT_PULLUP,
+);
+
+/* sdi1 (WLAN CW1200) */
+static UX500_PINS(mop500_pins_sdi1,
+	GPIO208_MC1_CLK		| PIN_OUTPUT_LOW,
+	GPIO209_MC1_FBCLK	| PIN_INPUT_NOPULL,
+	GPIO210_MC1_CMD		| PIN_INPUT_PULLUP,
+	GPIO211_MC1_DAT0	| PIN_INPUT_PULLUP,
+	GPIO212_MC1_DAT1	| PIN_INPUT_PULLUP,
+	GPIO213_MC1_DAT2	| PIN_INPUT_PULLUP,
+	GPIO214_MC1_DAT3	| PIN_INPUT_PULLUP,
+);
+
+/* sdi2 (POP eMMC) */
+static UX500_PINS(mop500_pins_sdi2,
+	GPIO128_MC2_CLK		| PIN_OUTPUT_LOW,
+	GPIO129_MC2_CMD		| PIN_INPUT_PULLUP,
+	GPIO130_MC2_FBCLK	| PIN_INPUT_NOPULL,
+	GPIO131_MC2_DAT0	| PIN_INPUT_PULLUP,
+	GPIO132_MC2_DAT1	| PIN_INPUT_PULLUP,
+	GPIO133_MC2_DAT2	| PIN_INPUT_PULLUP,
+	GPIO134_MC2_DAT3	| PIN_INPUT_PULLUP,
+	GPIO135_MC2_DAT4	| PIN_INPUT_PULLUP,
+	GPIO136_MC2_DAT5	| PIN_INPUT_PULLUP,
+	GPIO137_MC2_DAT6	| PIN_INPUT_PULLUP,
+	GPIO138_MC2_DAT7	| PIN_INPUT_PULLUP,
+);
+
+/* sdi4 (PCB eMMC) */
+static UX500_PINS(mop500_pins_sdi4,
+	GPIO197_MC4_DAT3	| PIN_INPUT_PULLUP,
+	GPIO198_MC4_DAT2	| PIN_INPUT_PULLUP,
+	GPIO199_MC4_DAT1	| PIN_INPUT_PULLUP,
+	GPIO200_MC4_DAT0	| PIN_INPUT_PULLUP,
+	GPIO201_MC4_CMD		| PIN_INPUT_PULLUP,
+	GPIO202_MC4_FBCLK	| PIN_INPUT_NOPULL,
+	GPIO203_MC4_CLK		| PIN_OUTPUT_LOW,
+	GPIO204_MC4_DAT7	| PIN_INPUT_PULLUP,
+	GPIO205_MC4_DAT6	| PIN_INPUT_PULLUP,
+	GPIO206_MC4_DAT5	| PIN_INPUT_PULLUP,
+	GPIO207_MC4_DAT4	| PIN_INPUT_PULLUP,
+);
+
+/* USB */
+static UX500_PINS(mop500_pins_usb,
+	GPIO256_USB_NXT,
+	GPIO257_USB_STP		| PIN_OUTPUT_HIGH,
+	GPIO258_USB_XCLK,
+	GPIO259_USB_DIR,
+	GPIO260_USB_DAT7,
+	GPIO261_USB_DAT6,
+	GPIO262_USB_DAT5,
+	GPIO263_USB_DAT4,
+	GPIO264_USB_DAT3,
+	GPIO265_USB_DAT2,
+	GPIO266_USB_DAT1,
+	GPIO267_USB_DAT0,
+);
+
+static struct ux500_pin_lookup mop500_pins[] = {
+	PIN_LOOKUP("mcde-dpi", &mop500_pins_mcde_dpi),
+	PIN_LOOKUP("mcde-tvout", &mop500_pins_mcde_tvout),
+	PIN_LOOKUP("nmk-i2c.0", &mop500_pins_i2c0),
+	PIN_LOOKUP("nmk-i2c.1", &mop500_pins_i2c1),
+	PIN_LOOKUP("nmk-i2c.2", &mop500_pins_i2c2),
+	PIN_LOOKUP("nmk-i2c.3", &mop500_pins_i2c3),
+	PIN_LOOKUP("ske", &mop500_pins_ske),
+	PIN_LOOKUP("sdi0", &mop500_pins_sdi0),
+	PIN_LOOKUP("sdi1", &mop500_pins_sdi1),
+	PIN_LOOKUP("sdi2", &mop500_pins_sdi2),
+	PIN_LOOKUP("sdi4", &mop500_pins_sdi4),
+	PIN_LOOKUP("ab8500-usb.0", &mop500_pins_usb),
+};
+
+/*
+ * Sleep pin configuration for u8500 platform.
+ * If another HW is used the GPIO's must be configured
+ * correctly when entering sleep for optimal power
+ * consumption.
+ */
+static pin_cfg_t mop500_pins_common_power_save_bank0[] = {
+	GPIO0_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO1_GPIO | PIN_SLPM_OUTPUT_HIGH  | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO2_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO3_GPIO | PIN_SLPM_DIR_OUTPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO4_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO5_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO6_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO7_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO8_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO9_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO10_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO11_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO12_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO13_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO14_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO15_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO16_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO17_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO18_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO19_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO20_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO21_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO22_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO23_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO24_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO25_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO26_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO27_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO28_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO29_U2_RXD | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO30_U2_TXD | PIN_SLPM_DIR_OUTPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO31_U2_CTSn | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank0_href60[] = {
+	GPIO0_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO1_GPIO | PIN_SLPM_OUTPUT_HIGH  | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO2_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO3_GPIO | PIN_SLPM_DIR_OUTPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO4_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO5_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO6_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO7_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO8_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO9_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO10_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO11_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO12_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO13_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO14_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO15_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO16_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO17_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO18_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO19_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO20_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO21_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO22_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO23_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO24_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO25_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO26_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO27_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO28_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO29_U2_RXD | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO30_U2_TXD | PIN_SLPM_DIR_OUTPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO31_U2_CTSn | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank1[] = {
+	GPIO32_U2_RTSn | PIN_SLPM_DIR_OUTPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO33_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO34_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO35_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO36_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank2[] = {
+	GPIO64_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO65_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO66_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO67_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO68_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO69_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO70_STMAPE_CLK | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_DISABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO71_STMAPE_DAT3 | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_DISABLE  | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO72_STMAPE_DAT2 | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_DISABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO73_STMAPE_DAT1 | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_DISABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO74_STMAPE_DAT0 | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_DISABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO75_U2_RXD | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO76_U2_TXD | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO77_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO78_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO79_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO80_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO81_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO82_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO83_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO84_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO85_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO86_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO87_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO88_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO89_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO90_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO91_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO92_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO93_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO94_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO95_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank2_href60[] = {
+	GPIO64_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO65_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO66_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO67_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO68_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO69_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO70_STMAPE_CLK | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_DISABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO71_STMAPE_DAT3 | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_DISABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO72_STMAPE_DAT2 | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_DISABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO73_STMAPE_DAT1 | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_DISABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO74_STMAPE_DAT0 | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_DISABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO75_U2_RXD | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO76_U2_TXD | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO77_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO78_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO79_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO80_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO81_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO82_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO83_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO84_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO85_GPIO,
+	GPIO86_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO87_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO88_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO89_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO90_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO91_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO92_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO93_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO94_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO95_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank3[] = {
+	GPIO96_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO97_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank4[] = {
+	GPIO128_MC2_CLK | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO129_MC2_CMD | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO130_MC2_FBCLK | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO131_MC2_DAT0 | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO132_MC2_DAT1 | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO133_MC2_DAT2 | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO134_MC2_DAT3 | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO135_MC2_DAT4 | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO136_MC2_DAT5 | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO137_MC2_DAT6 | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO138_MC2_DAT7 | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO139_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO140_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO141_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO142_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO143_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO144_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO145_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO146_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO147_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO148_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO149_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO150_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO151_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO152_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO153_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO154_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO155_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO156_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO157_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO158_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO159_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank5[] = {
+	GPIO160_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO161_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO162_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO163_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO164_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO165_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO166_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO167_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO168_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO169_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO170_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO171_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank5_href60[] = {
+	GPIO160_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO161_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO162_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO163_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO164_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO165_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO166_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO167_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO168_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO169_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO170_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO171_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank6[] = {
+	GPIO192_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO193_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO194_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO195_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO196_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO197_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO198_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO199_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO200_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO201_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO202_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO203_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO204_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO205_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO206_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO207_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO208_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO209_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO210_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO211_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO212_GPIO,
+	GPIO213_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO214_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO215_GPIO,
+
+	GPIO216_GPIO,
+	GPIO217_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO218_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO219_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO220_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO221_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO222_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO223_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank6_href60[] = {
+	GPIO192_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO193_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO194_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO195_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO196_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO197_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO198_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO199_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO200_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO201_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO202_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO203_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO204_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO205_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO206_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO207_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO208_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO209_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO210_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO211_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO212_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO213_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO214_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO215_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO216_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO217_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO218_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO219_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO220_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO221_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO222_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO223_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank7[] = {
+	GPIO224_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO225_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO226_GPIO | PIN_SLPM_DIR_OUTPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO227_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
+	GPIO228_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO229_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO230_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+};
+
+static pin_cfg_t mop500_pins_common_power_save_bank7_href60[] = {
+	GPIO224_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO225_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO226_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO227_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 
+	GPIO228_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO229_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO230_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 };
 
+static pin_cfg_t mop500_pins_common_power_save_bank8[] = {
+	GPIO256_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO257_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	GPIO258_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO259_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+
+	GPIO260_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO261_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO262_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO263_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+
+	GPIO264_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO265_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO266_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+	GPIO267_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_ENABLED,
+};
+
+void mop500_pins_suspend_force(void)
+{
+	if (machine_is_hrefv60())
+		sleep_pins_config_pm(mop500_pins_common_power_save_bank0_href60,
+			ARRAY_SIZE(mop500_pins_common_power_save_bank0_href60));
+	else
+		sleep_pins_config_pm(mop500_pins_common_power_save_bank0,
+			ARRAY_SIZE(mop500_pins_common_power_save_bank0));
+
+	sleep_pins_config_pm(mop500_pins_common_power_save_bank1,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank1));
+
+	if (machine_is_hrefv60())
+		sleep_pins_config_pm(mop500_pins_common_power_save_bank2_href60,
+			ARRAY_SIZE(mop500_pins_common_power_save_bank2_href60));
+	else
+		sleep_pins_config_pm(mop500_pins_common_power_save_bank2,
+			ARRAY_SIZE(mop500_pins_common_power_save_bank2));
+
+	sleep_pins_config_pm(mop500_pins_common_power_save_bank3,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank3));
+
+	sleep_pins_config_pm(mop500_pins_common_power_save_bank4,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank4));
+
+	if (machine_is_hrefv60())
+		sleep_pins_config_pm(mop500_pins_common_power_save_bank5_href60,
+			ARRAY_SIZE(mop500_pins_common_power_save_bank5_href60));
+	else
+		sleep_pins_config_pm(mop500_pins_common_power_save_bank5,
+			ARRAY_SIZE(mop500_pins_common_power_save_bank5));
+
+	if (machine_is_hrefv60())
+		sleep_pins_config_pm(mop500_pins_common_power_save_bank6_href60,
+			ARRAY_SIZE(mop500_pins_common_power_save_bank6_href60));
+	else
+		sleep_pins_config_pm(mop500_pins_common_power_save_bank6,
+			ARRAY_SIZE(mop500_pins_common_power_save_bank6));
+
+	if (machine_is_hrefv60())
+		sleep_pins_config_pm(mop500_pins_common_power_save_bank7_href60,
+			ARRAY_SIZE(mop500_pins_common_power_save_bank7_href60));
+	else
+	sleep_pins_config_pm(mop500_pins_common_power_save_bank7,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank7));
+
+	sleep_pins_config_pm(mop500_pins_common_power_save_bank8,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank8));
+}
+
+/*
+ * This function is called to force gpio power save
+ * mux settings during suspend.
+ * This is a temporary solution until all drivers are
+ * controlling their pin settings when in inactive mode.
+ */
+static void mop500_pins_suspend_force_mux(void)
+{
+	sleep_pins_config_pm_mux(mop500_pins_common_power_save_bank0,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank0));
+
+	sleep_pins_config_pm_mux(mop500_pins_common_power_save_bank1,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank1));
+
+	sleep_pins_config_pm_mux(mop500_pins_common_power_save_bank2,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank2));
+
+	sleep_pins_config_pm_mux(mop500_pins_common_power_save_bank3,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank3));
+
+	sleep_pins_config_pm_mux(mop500_pins_common_power_save_bank4,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank4));
+
+	sleep_pins_config_pm_mux(mop500_pins_common_power_save_bank5,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank5));
+
+	sleep_pins_config_pm_mux(mop500_pins_common_power_save_bank6,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank6));
+
+	sleep_pins_config_pm_mux(mop500_pins_common_power_save_bank7,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank7));
+
+	sleep_pins_config_pm_mux(mop500_pins_common_power_save_bank8,
+		ARRAY_SIZE(mop500_pins_common_power_save_bank8));
+}
+
+/*
+ * passing "pinsfor=" in kernel cmdline allows for custom
+ * configuration of GPIOs on u8500 derived boards.
+ */
+static int __init early_pinsfor(char *p)
+{
+	pinsfor = PINS_FOR_DEFAULT;
+
+	if (strcmp(p, "u9500-21") == 0)
+		pinsfor = PINS_FOR_U9500_21;
+
+	return 0;
+}
+early_param("pinsfor", early_pinsfor);
+
 void __init mop500_pins_init(void)
 {
 	nmk_config_pins(mop500_pins_common,
@@ -243,7 +902,26 @@ void __init mop500_pins_init(void)
 	if (machine_is_hrefv60())
 		nmk_config_pins(mop500_pins_hrefv60,
 				ARRAY_SIZE(mop500_pins_hrefv60));
+	else if (machine_is_snowball())
+		nmk_config_pins(snowball_pins,
+				ARRAY_SIZE(snowball_pins));
 	else
 		nmk_config_pins(mop500_pins_default,
 				ARRAY_SIZE(mop500_pins_default));
+
+	ux500_pins_add(mop500_pins, ARRAY_SIZE(mop500_pins));
+
+	switch (pinsfor) {
+	case PINS_FOR_U9500_21:
+		nmk_config_pins(u9500_21_pins, ARRAY_SIZE(u9500_21_pins));
+		break;
+
+	case PINS_FOR_DEFAULT:
+	default:
+		break;
+	}
+
+	suspend_set_pins_force_fn(mop500_pins_suspend_force,
+				  mop500_pins_suspend_force_mux);
 }
+
diff --git a/arch/arm/mach-ux500/board-mop500-regulators.c b/arch/arm/mach-ux500/board-mop500-regulators.c
index 9ed0f90cfe2..d2bd668c8dc 100644
--- a/arch/arm/mach-ux500/board-mop500-regulators.c
+++ b/arch/arm/mach-ux500/board-mop500-regulators.c
@@ -12,6 +12,34 @@
 #include <linux/regulator/machine.h>
 #include <linux/regulator/ab8500.h>
 #include "board-mop500-regulators.h"
+#include "board-mop500-mcde.h"
+
+#ifdef CONFIG_U8500_REGULATOR_DEBUG
+#define REGULATOR_SUPPLY_DEBUG	 REGULATOR_SUPPLY
+#else
+#define REGULATOR_SUPPLY_DEBUG(_name, _dev_name)
+#endif
+
+/*
+ * GPIO regulator controlled by the ab8500 GPIO16
+ */
+static struct regulator_consumer_supply gpio_wlan_vbat_consumers[] = {
+	/* for cg2900 chip */
+	REGULATOR_SUPPLY("vdd", "cg2900-uart.0"),
+	/* for cw1200 chip */
+	REGULATOR_SUPPLY("vdd", "cw1200_wlan"),
+};
+
+struct regulator_init_data gpio_wlan_vbat_regulator = {
+	.constraints = {
+		.name = "WLAN-VBAT",
+		.min_uV = 3600000,
+		.max_uV = 3600000,
+		.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+	},
+	.num_consumer_supplies = ARRAY_SIZE(gpio_wlan_vbat_consumers),
+	.consumer_supplies = gpio_wlan_vbat_consumers,
+};
 
 /*
  * TPS61052 regulator
@@ -39,32 +67,55 @@ struct regulator_init_data tps61052_regulator = {
 
 static struct regulator_consumer_supply ab8500_vaux1_consumers[] = {
 	/* External displays, connector on board 2v5 power supply */
-	REGULATOR_SUPPLY("vaux12v5", "mcde.0"),
+	REGULATOR_SUPPLY("vaux12v5", "mcde_disp_generic.0"),
 	/* SFH7741 proximity sensor */
 	REGULATOR_SUPPLY("vcc", "gpio-keys.0"),
 	/* BH1780GLS ambient light sensor */
 	REGULATOR_SUPPLY("vcc", "2-0029"),
 	/* lsm303dlh accelerometer */
-	REGULATOR_SUPPLY("vdd", "3-0018"),
+	REGULATOR_SUPPLY("vdd", "lsm303dlh.0"),
 	/* lsm303dlh magnetometer */
-	REGULATOR_SUPPLY("vdd", "3-001e"),
+	REGULATOR_SUPPLY("vdd", "lsm303dlh.1"),
 	/* Rohm BU21013 Touchscreen devices */
 	REGULATOR_SUPPLY("avdd", "3-005c"),
 	REGULATOR_SUPPLY("avdd", "3-005d"),
 	/* Synaptics RMI4 Touchscreen device */
 	REGULATOR_SUPPLY("vdd", "3-004b"),
+	/* L3G4200D Gyroscope device */
+	REGULATOR_SUPPLY("vdd", "l3g4200d"),
+	/* Camera sensors */
+	REGULATOR_SUPPLY("v-mmio-camera", "mmio_camera"),
+	REGULATOR_SUPPLY_DEBUG("aux1", "reg-virt-consumer.0")
 };
 
 static struct regulator_consumer_supply ab8500_vaux2_consumers[] = {
 	/* On-board eMMC power */
 	REGULATOR_SUPPLY("vmmc", "sdi4"),
 	/* AB8500 audio codec */
-	REGULATOR_SUPPLY("vcc-N2158", "ab8500-codec.0"),
+	REGULATOR_SUPPLY("vcc-avswitch", "ab8500-codec.0"),
+	REGULATOR_SUPPLY("vcc-avswitch", "ab8500-acc-det.0"),
+#ifdef CONFIG_DISPLAY_AB8500_TERTIARY
+	REGULATOR_SUPPLY("v-ab8500-AV-switch", "mcde_tv_ab8500.2"),
+#endif
+#ifdef CONFIG_DISPLAY_AV8100_TERTIARY
+	REGULATOR_SUPPLY("v-av8100-AV-switch", "av8100_hdmi.0"),
+	REGULATOR_SUPPLY("v-av8100-AV-switch", "av8100_hdmi.1"),
+#endif
+	REGULATOR_SUPPLY_DEBUG("aux2", "reg-virt-consumer.1")
 };
 
 static struct regulator_consumer_supply ab8500_vaux3_consumers[] = {
 	/* External MMC slot power */
 	REGULATOR_SUPPLY("vmmc", "sdi0"),
+	REGULATOR_SUPPLY_DEBUG("aux3", "reg-virt-consumer.2")
+};
+
+static struct regulator_consumer_supply ab8500_vintcore_consumers[] = {
+	/* SoC core supply, no device */
+	REGULATOR_SUPPLY("v-intcore", NULL),
+	/* USB Transciever */
+	REGULATOR_SUPPLY("vddulpivio18", "ab8500-usb.0"),
+	REGULATOR_SUPPLY_DEBUG("intcore", "reg-virt-consumer.3")
 };
 
 static struct regulator_consumer_supply ab8500_vtvout_consumers[] = {
@@ -72,18 +123,57 @@ static struct regulator_consumer_supply ab8500_vtvout_consumers[] = {
 	REGULATOR_SUPPLY("vtvout", "ab8500-denc.0"),
 	/* Internal general-purpose ADC */
 	REGULATOR_SUPPLY("vddadc", "ab8500-gpadc.0"),
+	REGULATOR_SUPPLY("vddadc", "ab8500-charger.0"),
+#ifdef CONFIG_DISPLAY_AB8500_TERTIARY
+	REGULATOR_SUPPLY("v-tvout", "mcde_tv_ab8500.2"),
+#endif
+	REGULATOR_SUPPLY_DEBUG("tvout", "reg-virt-consumer.4")
 };
 
-static struct regulator_consumer_supply ab8500_vintcore_consumers[] = {
-	/* SoC core supply, no device */
-	REGULATOR_SUPPLY("v-intcore", NULL),
-	/* USB Transciever */
-	REGULATOR_SUPPLY("vddulpivio18", "ab8500-usb.0"),
+static struct regulator_consumer_supply ab8500_vaudio_consumers[] = {
+	/* vaudio regulator, supply for ab8500-vaudio */
+	REGULATOR_SUPPLY("v-audio", NULL),
+	REGULATOR_SUPPLY_DEBUG("audio", "reg-virt-consumer.5")
+};
+
+static struct regulator_consumer_supply ab8500_vamic1_consumers[] = {
+	/* vamic1 regulator */
+	REGULATOR_SUPPLY("v-amic1", NULL),
+	REGULATOR_SUPPLY_DEBUG("anamic1", "reg-virt-consumer.6")
+};
+
+static struct regulator_consumer_supply ab8500_vamic2_consumers[] = {
+	/* vamic2 regulator */
+	REGULATOR_SUPPLY("v-amic2", NULL),
+	REGULATOR_SUPPLY_DEBUG("anamic2", "reg-virt-consumer.7")
+};
+
+static struct regulator_consumer_supply ab8500_vdmic_consumers[] = {
+	/* supply for v-dmic, VDMIC LDO */
+	REGULATOR_SUPPLY("v-dmic", NULL),
+	REGULATOR_SUPPLY_DEBUG("dmic", "reg-virt-consumer.8")
 };
 
 static struct regulator_consumer_supply ab8500_vana_consumers[] = {
 	/* External displays, connector on board, 1v8 power supply */
 	REGULATOR_SUPPLY("vsmps2", "mcde.0"),
+	/* for analogue part of displays */
+	REGULATOR_SUPPLY("v-ana", "mcde"),
+	/* camera supply */
+	REGULATOR_SUPPLY("v-ana", "mmio_camera"),
+	REGULATOR_SUPPLY_DEBUG("ana", "reg-virt-consumer.9")
+};
+
+/* supply for CG2900 */
+static struct regulator_consumer_supply ab8500_sysclkreq_2_consumers[] = {
+	REGULATOR_SUPPLY("gbf_1v8", "cg2900-uart.0"),
+	REGULATOR_SUPPLY_DEBUG("sysclkreq-2", "reg-virt-consumer.10")
+};
+
+/* supply for CW1200 */
+static struct regulator_consumer_supply ab8500_sysclkreq_4_consumers[] = {
+	REGULATOR_SUPPLY("wlan_1v8", "cw1200"),
+	REGULATOR_SUPPLY_DEBUG("sysclkreq-4", "reg-virt-consumer.11")
 };
 
 /* ab8500 regulator register initialization */
@@ -91,6 +181,7 @@ struct ab8500_regulator_reg_init
 ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS] = {
 	/*
 	 * VanaRequestCtrl          = HP/LP depending on VxRequest
+	 * VpllRequestCtrl          = HP/LP depending on VxRequest
 	 * VextSupply1RequestCtrl   = HP/LP depending on VxRequest
 	 */
 	INIT_REGULATOR_REGISTER(AB8500_REGUREQUESTCTRL2, 0x00),
@@ -107,12 +198,16 @@ ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS] = {
 	 */
 	INIT_REGULATOR_REGISTER(AB8500_REGUREQUESTCTRL4, 0x00),
 	/*
+	 * Vsmps1SysClkReq1HPValid  = enabled
+	 * Vsmps2SysClkReq1HPValid  = enabled
+	 * Vsmps3SysClkReq1HPValid  = enabled
 	 * VanaSysClkReq1HPValid    = disabled
+	 * VpllSysClkReq1HPValid    = enabled
 	 * Vaux1SysClkReq1HPValid   = disabled
 	 * Vaux2SysClkReq1HPValid   = disabled
 	 * Vaux3SysClkReq1HPValid   = disabled
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_REGUSYSCLKREQ1HPVALID1, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_REGUSYSCLKREQ1HPVALID1, 0x17),
 	/*
 	 * VextSupply1SysClkReq1HPValid = disabled
 	 * VextSupply2SysClkReq1HPValid = disabled
@@ -199,6 +294,50 @@ ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS] = {
 	 */
 	INIT_REGULATOR_REGISTER(AB8500_REGUCTRL1VAMIC, 0x00),
 	/*
+	 * VBBN                     = force OFF
+	 * VBBP                     = force OFF
+	 * NOTE! PRCMU register
+	 */
+	INIT_REGULATOR_REGISTER(AB8500_ARMREGU2,               0x00),
+	/*
+	 * VBBNSel1                 = VBBP = VBBPFB
+	 * VBBPSel1                 = 0 V
+	 * NOTE! PRCMU register
+	 */
+	INIT_REGULATOR_REGISTER(AB8500_VBBSEL1,                0x00),
+	/*
+	 * VBBNSel2                 = VBBP = VBBPFB
+	 * VBBPSel2                 = 0 V
+	 * NOTE! PRCMU register
+	 */
+	INIT_REGULATOR_REGISTER(AB8500_VBBSEL2,                0x00),
+	/*
+	 * Vsmps1Regu               = HW control
+	 * Vsmps1SelCtrl            = Vsmps1 voltage defined by Vsmsp1Sel2
+	 */
+	INIT_REGULATOR_REGISTER(AB8500_VSMPS1REGU,             0x06),
+	/*
+	 * Vsmps2Regu               = HW control
+	 * Vsmps2SelCtrl            = Vsmps2 voltage defined by Vsmsp2Sel2
+	 */
+	INIT_REGULATOR_REGISTER(AB8500_VSMPS2REGU,             0x06),
+	/*
+	 * Vsmps3Sel2               = 1.2125 V
+	 * NOTE! PRCMU register
+	 */
+	INIT_REGULATOR_REGISTER(AB8500_VSMPS3SEL2,             0x29),
+	/*
+	 * Vsmps3Regu               = HW control
+	 * Vsmps3SelCtrl            = Vsmps3 voltage defined by Vsmps3Sel2
+	 * NOTE! PRCMU register
+	 */
+	INIT_REGULATOR_REGISTER(AB8500_VSMPS3REGU,             0x06),
+	/*
+	 * Vsmps3Sel1               = 1.0 V
+	 * NOTE! PRCMU register
+	 */
+	INIT_REGULATOR_REGISTER(AB8500_VSMPS3SEL1,             0x18),
+	/*
 	 * VPll                     = Hw controlled
 	 * VanaRegu                 = force off
 	 */
@@ -211,24 +350,25 @@ ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS] = {
 	/*
 	 * VextSupply1Regu          = HW control
 	 * VextSupply2Regu          = HW control
-	 * VextSupply3Regu          = HW control
+	 * VextSupply3Regu          = Low Power mode
 	 * ExtSupply2Bypass         = ExtSupply12LPn ball is 0 when Ena is 0
 	 * ExtSupply3Bypass         = ExtSupply3LPn ball is 0 when Ena is 0
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_EXTSUPPLYREGU, 0x2a),
+	INIT_REGULATOR_REGISTER(AB8500_EXTSUPPLYREGU, 0x1a),
 	/*
 	 * Vaux1Regu                = force HP
 	 * Vaux2Regu                = force off
 	 */
 	INIT_REGULATOR_REGISTER(AB8500_VAUX12REGU, 0x01),
 	/*
-	 * Vaux3regu                = force off
+	 * Vrf1Regu                 = HW control
+	 * Vaux3Regu                = force off
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_VRF1VAUX3REGU, 0x00),
+	INIT_REGULATOR_REGISTER(AB8500_VRF1VAUX3REGU, 0x08),
 	/*
-	 * Vsmps1                   = 1.15V
+	 * Vsmps1Sel1               = 1.2 V
 	 */
-	INIT_REGULATOR_REGISTER(AB8500_VSMPS1SEL1, 0x24),
+	INIT_REGULATOR_REGISTER(AB8500_VSMPS1SEL1, 0x28),
 	/*
 	 * Vaux1Sel                 = 2.5 V
 	 */
@@ -272,7 +412,14 @@ struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 			.max_uV = 2900000,
 			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
 					  REGULATOR_CHANGE_STATUS,
-			.boot_on = 1, /* must be on for display */
+			.boot_on = 1, /* display is on at boot */
+			/*
+			 * This voltage cannot be disabled right now because
+			 * it is somehow affecting the external MMC
+			 * functionality, though that typically will use
+			 * AUX3.
+			 */
+			.always_on = 1,
 		},
 		.num_consumer_supplies = ARRAY_SIZE(ab8500_vaux1_consumers),
 		.consumer_supplies = ab8500_vaux1_consumers,
@@ -284,7 +431,10 @@ struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 			.min_uV = 1100000,
 			.max_uV = 3300000,
 			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
-					  REGULATOR_CHANGE_STATUS,
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
 		},
 		.num_consumer_supplies = ARRAY_SIZE(ab8500_vaux2_consumers),
 		.consumer_supplies = ab8500_vaux2_consumers,
@@ -296,7 +446,10 @@ struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 			.min_uV = 1100000,
 			.max_uV = 3300000,
 			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
-					  REGULATOR_CHANGE_STATUS,
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
 		},
 		.num_consumer_supplies = ARRAY_SIZE(ab8500_vaux3_consumers),
 		.consumer_supplies = ab8500_vaux3_consumers,
@@ -316,6 +469,8 @@ struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 			.name = "V-AUD",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vaudio_consumers),
+		.consumer_supplies = ab8500_vaudio_consumers,
 	},
 	/* supply for v-anamic1 VAMic1-LDO */
 	[AB8500_LDO_ANAMIC1] = {
@@ -323,6 +478,8 @@ struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 			.name = "V-AMIC1",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vamic1_consumers),
+		.consumer_supplies = ab8500_vamic1_consumers,
 	},
 	/* supply for v-amic2, VAMIC2 LDO, reuse constants for AMIC1 */
 	[AB8500_LDO_ANAMIC2] = {
@@ -330,6 +487,8 @@ struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 			.name = "V-AMIC2",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vamic2_consumers),
+		.consumer_supplies = ab8500_vamic2_consumers,
 	},
 	/* supply for v-dmic, VDMIC LDO */
 	[AB8500_LDO_DMIC] = {
@@ -337,12 +496,22 @@ struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 			.name = "V-DMIC",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
+		.num_consumer_supplies = ARRAY_SIZE(ab8500_vdmic_consumers),
+		.consumer_supplies = ab8500_vdmic_consumers,
 	},
 	/* supply for v-intcore12, VINTCORE12 LDO */
 	[AB8500_LDO_INTCORE] = {
 		.constraints = {
 			.name = "V-INTCORE",
-			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+			.min_uV = 1250000,
+			.max_uV = 1350000,
+			.input_uV = 1800000,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE |
+					  REGULATOR_CHANGE_DRMS,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
 		},
 		.num_consumer_supplies = ARRAY_SIZE(ab8500_vintcore_consumers),
 		.consumer_supplies = ab8500_vintcore_consumers,
@@ -350,10 +519,30 @@ struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS] = {
 	/* supply for U8500 CSI/DSI, VANA LDO */
 	[AB8500_LDO_ANA] = {
 		.constraints = {
-			.name = "V-CSI/DSI",
+			.name = "V-CSI-DSI",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
 		.num_consumer_supplies = ARRAY_SIZE(ab8500_vana_consumers),
 		.consumer_supplies = ab8500_vana_consumers,
 	},
+	/* sysclkreq 2 pin */
+	[AB8500_SYSCLKREQ_2] = {
+		.constraints = {
+			.name = "ab8500-sysclkreq-2",
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+		},
+		.num_consumer_supplies =
+			ARRAY_SIZE(ab8500_sysclkreq_2_consumers),
+		.consumer_supplies = ab8500_sysclkreq_2_consumers,
+	},
+	/* sysclkreq 4 pin */
+	[AB8500_SYSCLKREQ_4] = {
+		.constraints = {
+			.name = "ab8500-sysclkreq-4",
+			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+		},
+		.num_consumer_supplies =
+			ARRAY_SIZE(ab8500_sysclkreq_4_consumers),
+		.consumer_supplies = ab8500_sysclkreq_4_consumers,
+	},
 };
diff --git a/arch/arm/mach-ux500/board-mop500-regulators.h b/arch/arm/mach-ux500/board-mop500-regulators.h
index 94992158d96..d8c5694a35d 100644
--- a/arch/arm/mach-ux500/board-mop500-regulators.h
+++ b/arch/arm/mach-ux500/board-mop500-regulators.h
@@ -18,5 +18,6 @@ extern struct ab8500_regulator_reg_init
 ab8500_regulator_reg_init[AB8500_NUM_REGULATOR_REGISTERS];
 extern struct regulator_init_data ab8500_regulators[AB8500_NUM_REGULATORS];
 extern struct regulator_init_data tps61052_regulator;
+extern struct regulator_init_data gpio_wlan_vbat_regulator;
 
 #endif
diff --git a/arch/arm/mach-ux500/board-mop500-sdi.c b/arch/arm/mach-ux500/board-mop500-sdi.c
index 7c6cb4fa47a..e1fc157b22e 100644
--- a/arch/arm/mach-ux500/board-mop500-sdi.c
+++ b/arch/arm/mach-ux500/board-mop500-sdi.c
@@ -7,41 +7,56 @@
 
 #include <linux/kernel.h>
 #include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
 #include <linux/amba/bus.h>
 #include <linux/amba/mmci.h>
 #include <linux/mmc/host.h>
 #include <linux/platform_device.h>
 
 #include <asm/mach-types.h>
+#include <plat/pincfg.h>
 #include <plat/ste_dma40.h>
 #include <mach/devices.h>
 #include <mach/hardware.h>
 
 #include "devices-db8500.h"
 #include "board-mop500.h"
-#include "ste-dma40-db8500.h"
+#include <mach/ste-dma40-db8500.h>
+#include "../drivers/mmc/host/mmci.h" /* to avoid MCI_ST* redefinition */
 
 /*
  * SDI 0 (MicroSD slot)
  */
 
-/* MMCIPOWER bits */
-#define MCI_DATA2DIREN		(1 << 2)
-#define MCI_CMDDIREN		(1 << 3)
-#define MCI_DATA0DIREN		(1 << 4)
-#define MCI_DATA31DIREN		(1 << 5)
-#define MCI_FBCLKEN		(1 << 7)
+/* GPIO pins used by the sdi0 level shifter */
+static int sdi0_en = -1;
+static int sdi0_vsel = -1;
 
 static u32 mop500_sdi0_vdd_handler(struct device *dev, unsigned int vdd,
 				   unsigned char power_mode)
 {
-	if (power_mode == MMC_POWER_UP)
-		gpio_set_value_cansleep(GPIO_SDMMC_EN, 1);
-	else if (power_mode == MMC_POWER_OFF)
-		gpio_set_value_cansleep(GPIO_SDMMC_EN, 0);
+	switch (power_mode) {
+	case MMC_POWER_UP:
+	case MMC_POWER_ON:
+		/*
+		 * Level shifter voltage should depend on vdd to when deciding
+		 * on either 1.8V or 2.9V. Once the decision has been made the
+		 * level shifter must be disabled and re-enabled with a changed
+		 * select signal in order to switch the voltage. Since there is
+		 * no framework support yet for indicating 1.8V in vdd, use the
+		 * default 2.9V.
+		 */
+		gpio_direction_output(sdi0_vsel, 0);
+		gpio_direction_output(sdi0_en, 1);
+		break;
+	case MMC_POWER_OFF:
+		gpio_direction_output(sdi0_vsel, 0);
+		gpio_direction_output(sdi0_en, 0);
+		break;
+	}
 
-	return MCI_FBCLKEN | MCI_CMDDIREN | MCI_DATA0DIREN |
-	       MCI_DATA2DIREN | MCI_DATA31DIREN;
+	return MCI_ST_FBCLKEN | MCI_ST_CMDDIREN | MCI_ST_DATA0DIREN |
+	       MCI_ST_DATA2DIREN;
 }
 
 #ifdef CONFIG_STE_DMA40
@@ -67,8 +82,10 @@ static struct stedma40_chan_cfg mop500_sdi0_dma_cfg_tx = {
 static struct mmci_platform_data mop500_sdi0_data = {
 	.vdd_handler	= mop500_sdi0_vdd_handler,
 	.ocr_mask	= MMC_VDD_29_30,
-	.f_max		= 100000000,
-	.capabilities	= MMC_CAP_4_BIT_DATA,
+	.f_max		= 50000000,
+	.capabilities	= MMC_CAP_4_BIT_DATA |
+				MMC_CAP_SD_HIGHSPEED |
+				MMC_CAP_MMC_HIGHSPEED,
 	.gpio_wp	= -1,
 #ifdef CONFIG_STE_DMA40
 	.dma_filter	= stedma40_filter,
@@ -77,13 +94,55 @@ static struct mmci_platform_data mop500_sdi0_data = {
 #endif
 };
 
-/* GPIO pins used by the sdi0 level shifter */
-static int sdi0_en = -1;
-static int sdi0_vsel = -1;
+#if defined(CONFIG_CW1200) || defined(CONFIG_CW1200_MODULE)
+/*
+ * SDI1 (SDIO WLAN)
+ */
+#ifdef CONFIG_STE_DMA40
+static struct stedma40_chan_cfg sdi1_dma_cfg_rx = {
+	.dir = STEDMA40_PERIPH_TO_MEM,
+	.src_dev_type = DB8500_DMA_DEV32_SD_MM1_RX,
+	.dst_dev_type = STEDMA40_DEV_DST_MEMORY,
+	.src_info.data_width = STEDMA40_WORD_WIDTH,
+	.dst_info.data_width = STEDMA40_WORD_WIDTH,
+};
+
+static struct stedma40_chan_cfg sdi1_dma_cfg_tx = {
+	.dir = STEDMA40_MEM_TO_PERIPH,
+	.src_dev_type = STEDMA40_DEV_SRC_MEMORY,
+	.dst_dev_type = DB8500_DMA_DEV32_SD_MM1_TX,
+	.src_info.data_width = STEDMA40_WORD_WIDTH,
+	.dst_info.data_width = STEDMA40_WORD_WIDTH,
+};
+#endif
 
-static void sdi0_configure(void)
+/*
+ * TODO 1: SDIO power management not fully supported.
+ */
+static struct mmci_platform_data mop500_sdi1_data = {
+	.ocr_mask	= MMC_VDD_29_30,
+	.f_max		= 15000000,
+	.capabilities	= MMC_CAP_4_BIT_DATA |
+				MMC_CAP_SDIO_IRQ |
+				MMC_CAP_BROKEN_SDIO_CMD53,
+	.gpio_cd	= -1,
+	.gpio_wp	= -1,
+#ifdef CONFIG_STE_DMA40
+	.dma_filter	= stedma40_filter,
+	.dma_rx_param	= &sdi1_dma_cfg_rx,
+	.dma_tx_param	= &sdi1_dma_cfg_tx,
+#endif
+};
+#endif /* CONFIG_CW1200 */
+
+static void sdi0_sdi1_configure(void)
 {
 	int ret;
+	u32 periphid = 0;
+
+	/* v2 has a new version of this block that need to be forced */
+	if (cpu_is_u8500v2())
+		periphid = 0x10480180;
 
 	ret = gpio_request(sdi0_en, "level shifter enable");
 	if (!ret)
@@ -99,11 +158,13 @@ static void sdi0_configure(void)
 	gpio_direction_output(sdi0_vsel, 0);
 	gpio_direction_output(sdi0_en, 1);
 
-	/* Add the device, force v2 to subrevision 1 */
-	if (cpu_is_u8500v2())
-		db8500_add_sdi0(&mop500_sdi0_data, 0x10480180);
-	else
-		db8500_add_sdi0(&mop500_sdi0_data, 0);
+	db8500_add_sdi0(&mop500_sdi0_data, periphid);
+
+#if defined(CONFIG_CW1200) || defined(CONFIG_CW1200_MODULE)
+	/* SDIO WLAN must be the last SDI device initialized
+	 * to wait CW1200 detection without blocking others SDI init */
+	db8500_add_sdi1(&mop500_sdi1_data, periphid);
+#endif
 }
 
 void mop500_sdi_tc35892_init(void)
@@ -111,7 +172,7 @@ void mop500_sdi_tc35892_init(void)
 	mop500_sdi0_data.gpio_cd = GPIO_SDMMC_CD;
 	sdi0_en = GPIO_SDMMC_EN;
 	sdi0_vsel = GPIO_SDMMC_1V8_3V_SEL;
-	sdi0_configure();
+	sdi0_sdi1_configure();
 }
 
 /*
@@ -140,8 +201,10 @@ static struct stedma40_chan_cfg mop500_sdi2_dma_cfg_tx = {
 
 static struct mmci_platform_data mop500_sdi2_data = {
 	.ocr_mask	= MMC_VDD_165_195,
-	.f_max		= 100000000,
-	.capabilities	= MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA,
+	.f_max		= 50000000,
+	.capabilities	= MMC_CAP_4_BIT_DATA |
+				MMC_CAP_8_BIT_DATA |
+				MMC_CAP_MMC_HIGHSPEED,
 	.gpio_cd	= -1,
 	.gpio_wp	= -1,
 #ifdef CONFIG_STE_DMA40
@@ -177,7 +240,7 @@ static struct stedma40_chan_cfg mop500_sdi4_dma_cfg_tx = {
 
 static struct mmci_platform_data mop500_sdi4_data = {
 	.ocr_mask	= MMC_VDD_29_30,
-	.f_max		= 100000000,
+	.f_max		= 50000000,
 	.capabilities	= MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA |
 			  MMC_CAP_MMC_HIGHSPEED,
 	.gpio_cd	= -1,
@@ -196,22 +259,39 @@ void __init mop500_sdi_init(void)
 	/* v2 has a new version of this block that need to be forced */
 	if (cpu_is_u8500v2())
 		periphid = 0x10480180;
-	/* PoP:ed eMMC on top of DB8500 v1.0 has problems with high speed */
-	if (!cpu_is_u8500v10())
-		mop500_sdi2_data.capabilities |= MMC_CAP_MMC_HIGHSPEED;
-	db8500_add_sdi2(&mop500_sdi2_data, periphid);
+	/* sdi2 on snowball is in ATL_B mode for FSMC (LAN) */
+	if (!machine_is_snowball())
+		db8500_add_sdi2(&mop500_sdi2_data, periphid);
 
 	/* On-board eMMC */
-	db8500_add_sdi4(&mop500_sdi4_data, periphid);
 
-	if (machine_is_hrefv60()) {
-		mop500_sdi0_data.gpio_cd = HREFV60_SDMMC_CD_GPIO;
-		sdi0_en = HREFV60_SDMMC_EN_GPIO;
-		sdi0_vsel = HREFV60_SDMMC_1V8_3V_GPIO;
-		sdi0_configure();
+	/* HACK Aleft - This needs to be reverted !
+	 * This kernel has problem discovering the SD card when it's 
+	 * assigned to mmc1, yielding an unusable android image.  
+	 * Disabling the emmc is not the right fix but allowing the
+	 * SD card to be discovered earlier seems to help.
+	 */ 
+#ifndef CONFIG_MACH_SNOWBALL 
+	db8500_add_sdi4(&mop500_sdi4_data, periphid);
+#endif
+	if (machine_is_hrefv60() || machine_is_snowball()) {
+		if (machine_is_hrefv60()) {
+			mop500_sdi0_data.gpio_cd = HREFV60_SDMMC_CD_GPIO;
+			sdi0_en = HREFV60_SDMMC_EN_GPIO;
+			sdi0_vsel = HREFV60_SDMMC_1V8_3V_GPIO;
+		} else if (machine_is_snowball()) {
+			mop500_sdi0_data.capabilities = (MMC_CAP_4_BIT_DATA |
+							 MMC_CAP_MMC_HIGHSPEED);
+			mop500_sdi0_data.gpio_cd = SNOWBALL_SDMMC_CD_GPIO;
+			mop500_sdi0_data.cd_invert = true;
+			sdi0_en = SNOWBALL_SDMMC_EN_GPIO;
+			sdi0_vsel = SNOWBALL_SDMMC_1V8_3V_GPIO;
+		}
+		sdi0_sdi1_configure();
 	}
+
 	/*
-	 * On boards with the TC35892 GPIO expander, sdi0 will finally
+	 * On boards with the TC35892 GPIO expander, sdi0 & sdi1 will finally
 	 * be added when the TC35892 initializes and calls
 	 * mop500_sdi_tc35892_init() above.
 	 */
diff --git a/arch/arm/mach-ux500/board-mop500-stuib.c b/arch/arm/mach-ux500/board-mop500-stuib.c
index 8c979770d87..fc0fad94052 100644
--- a/arch/arm/mach-ux500/board-mop500-stuib.c
+++ b/arch/arm/mach-ux500/board-mop500-stuib.c
@@ -10,34 +10,123 @@
 #include <linux/input/bu21013.h>
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
+#include <linux/lsm303dlh.h>
+#include <linux/l3g4200d.h>
 #include <linux/i2c.h>
 #include <linux/input/matrix_keypad.h>
+#include <linux/input/lps001wp.h>
 #include <asm/mach-types.h>
 
 #include "board-mop500.h"
 
-/* STMPE/SKE keypad use this key layout */
+/*
+ * LSM303DLH accelerometer + magnetometer sensors
+ */
+static struct lsm303dlh_platform_data __initdata lsm303dlh_pdata= {
+	.name_a = "lsm303dlh.0",
+	.name_m = "lsm303dlh.1",
+	.axis_map_x = 0,
+	.axis_map_y = 1,
+	.axis_map_z = 2,
+	.negative_x = 1,
+	.negative_y = 1,
+	.negative_z = 0,
+};
+
+static struct l3g4200d_gyr_platform_data  __initdata l3g4200d_pdata_u8500 = {
+	.name_gyr = "l3g4200d",
+	.axis_map_x = 1,
+	.axis_map_y = 0,
+	.axis_map_z = 2,
+	.negative_x = 0,
+	.negative_y = 0,
+	.negative_z = 1,
+};
+
+static struct lps001wp_prs_platform_data __initdata lps001wp_pdata = {
+	.poll_interval = 500,
+	.min_interval = 10,
+};
+
+static struct i2c_board_info __initdata mop500_i2c2_devices[] = {
+	{
+		/* LSM303DLH Accelerometer */
+		I2C_BOARD_INFO("lsm303dlh_a", 0x18),
+		.platform_data = &lsm303dlh_pdata,
+	},
+	{
+		/* LSM303DLH Magnetometer */
+		I2C_BOARD_INFO("lsm303dlh_m", 0x1E),
+		.platform_data = &lsm303dlh_pdata,
+	},
+	{
+		/* L3G4200D Gyroscope */
+		I2C_BOARD_INFO("l3g4200d", 0x68),
+		.platform_data = &l3g4200d_pdata_u8500,
+	},
+	{
+		/* LSP001WM Barometer */
+		I2C_BOARD_INFO("lps001wp_prs_sysfs", 0x5C),
+		.platform_data = &lps001wp_pdata,
+	},
+};
+
+/*
+ * ux500 keymaps
+ *
+ * Organized row-wise as on the UIB, starting at the top-left
+ *
+ * we support two key layouts, specific to requirements. The first
+ * keylayout includes controls for power/volume a few generic keys;
+ * the second key layout contains the full numeric layout, enter/back/left
+ * buttons along with a "."(dot), specifically for connectivity testing
+ */
 static const unsigned int mop500_keymap[] = {
+#if defined(CONFIG_KEYLAYOUT_LAYOUT1)
 	KEY(2, 5, KEY_END),
-	KEY(4, 1, KEY_POWER),
+	KEY(4, 1, KEY_HOME),
 	KEY(3, 5, KEY_VOLUMEDOWN),
-	KEY(1, 3, KEY_3),
+	KEY(1, 3, KEY_EMAIL),
 	KEY(5, 2, KEY_RIGHT),
-	KEY(5, 0, KEY_9),
+	KEY(5, 0, KEY_BACKSPACE),
 
 	KEY(0, 5, KEY_MENU),
 	KEY(7, 6, KEY_ENTER),
 	KEY(4, 5, KEY_0),
-	KEY(6, 7, KEY_2),
+	KEY(6, 7, KEY_DOT),
 	KEY(3, 4, KEY_UP),
 	KEY(3, 3, KEY_DOWN),
 
 	KEY(6, 4, KEY_SEND),
 	KEY(6, 2, KEY_BACK),
 	KEY(4, 2, KEY_VOLUMEUP),
-	KEY(5, 5, KEY_1),
+	KEY(5, 5, KEY_SPACE),
 	KEY(4, 3, KEY_LEFT),
+	KEY(3, 2, KEY_SEARCH),
+#elif defined(CONFIG_KEYLAYOUT_LAYOUT2)
+	KEY(2, 5, KEY_RIGHT),
+	KEY(4, 1, KEY_ENTER),
+	KEY(3, 5, KEY_MENU),
+	KEY(1, 3, KEY_3),
+	KEY(5, 2, KEY_6),
+	KEY(5, 0, KEY_9),
+
+	KEY(0, 5, KEY_UP),
+	KEY(7, 6, KEY_DOWN),
+	KEY(4, 5, KEY_0),
+	KEY(6, 7, KEY_2),
+	KEY(3, 4, KEY_5),
+	KEY(3, 3, KEY_8),
+
+	KEY(6, 4, KEY_LEFT),
+	KEY(6, 2, KEY_BACK),
+	KEY(4, 2, KEY_KPDOT),
+	KEY(5, 5, KEY_1),
+	KEY(4, 3, KEY_4),
 	KEY(3, 2, KEY_7),
+#else
+#warning "No keypad layout defined."
+#endif
 };
 
 static const struct matrix_keymap_data mop500_keymap_data = {
@@ -133,7 +222,7 @@ static int bu21013_gpio_board_exit(int reset_pin)
 					__func__);
 			return retval;
 		}
-		gpio_set_value(reset_pin, 0);
+		gpio_free(reset_pin);
 	}
 	bu21013_devices--;
 
@@ -157,9 +246,15 @@ static struct bu21013_platform_device tsc_plat_device = {
 	.irq = NOMADIK_GPIO_TO_IRQ(TOUCH_GPIO_PIN),
 	.touch_x_max = TOUCH_XMAX,
 	.touch_y_max = TOUCH_YMAX,
-	.ext_clk = false,
-	.x_flip = false,
-	.y_flip = true,
+	.has_ext_clk = false,
+#if defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_ANGLE) &&	\
+		CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_ANGLE == 270
+	.x_flip		= true,
+	.y_flip		= false,
+#else
+	.x_flip		= false,
+	.y_flip		= true,
+#endif
 };
 
 static struct bu21013_platform_device tsc_plat2_device = {
@@ -169,9 +264,15 @@ static struct bu21013_platform_device tsc_plat2_device = {
 	.irq = NOMADIK_GPIO_TO_IRQ(TOUCH_GPIO_PIN),
 	.touch_x_max = TOUCH_XMAX,
 	.touch_y_max = TOUCH_YMAX,
-	.ext_clk = false,
-	.x_flip = false,
-	.y_flip = true,
+	.has_ext_clk = false,
+#if defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_ANGLE) &&	\
+		CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_ANGLE == 270
+	.x_flip		= true,
+	.y_flip		= false,
+#else
+	.x_flip		= false,
+	.y_flip		= true,
+#endif
 };
 
 static struct i2c_board_info __initdata u8500_i2c3_devices_stuib[] = {
@@ -202,4 +303,16 @@ void __init mop500_stuib_init(void)
 
 	mop500_uib_i2c_add(3, u8500_i2c3_devices_stuib,
 			ARRAY_SIZE(u8500_i2c3_devices_stuib));
+
+	if (machine_is_hrefv60()) {
+		lsm303dlh_pdata.irq_a1 = HREFV60_ACCEL_INT1_GPIO;
+		lsm303dlh_pdata.irq_a2 = HREFV60_ACCEL_INT2_GPIO;
+		lsm303dlh_pdata.irq_m = HREFV60_MAGNET_DRDY_GPIO;
+	} else {
+		lsm303dlh_pdata.irq_a1 = GPIO_ACCEL_INT1;
+		lsm303dlh_pdata.irq_a2 = GPIO_ACCEL_INT2;
+		lsm303dlh_pdata.irq_m = GPIO_MAGNET_DRDY;
+	}
+	mop500_uib_i2c_add(2, mop500_i2c2_devices,
+			ARRAY_SIZE(mop500_i2c2_devices));
 }
diff --git a/arch/arm/mach-ux500/board-mop500-u8500uib.c b/arch/arm/mach-ux500/board-mop500-u8500uib.c
index d8a8734a0eb..912f884a09e 100644
--- a/arch/arm/mach-ux500/board-mop500-u8500uib.c
+++ b/arch/arm/mach-ux500/board-mop500-u8500uib.c
@@ -10,16 +10,60 @@
 #include <linux/i2c.h>
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
+#include <linux/lsm303dlh.h>
+#include <linux/l3g4200d.h>
 #include <linux/mfd/tc3589x.h>
 #include <linux/input/matrix_keypad.h>
 #include <../drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h>
 
-#include <mach/gpio.h>
+#include <asm/mach-types.h>
+#include <linux/gpio.h>
 #include <mach/irqs.h>
 
 #include "board-mop500.h"
 
 /*
+ * LSM303DLH accelerometer + magnetometer & L3G4200D Gyroscope sensors
+ */
+static struct lsm303dlh_platform_data __initdata lsm303dlh_pdata_u8500 = {
+	.name_a = "lsm303dlh.0",
+	.name_m = "lsm303dlh.1",
+	.axis_map_x = 1,
+	.axis_map_y = 0,
+	.axis_map_z = 2,
+	.negative_x = 1,
+	.negative_y = 1,
+	.negative_z = 1,
+};
+
+static struct l3g4200d_gyr_platform_data  __initdata l3g4200d_pdata_u8500 = {
+	.name_gyr = "l3g4200d",
+	.axis_map_x = 1,
+	.axis_map_y = 0,
+	.axis_map_z = 2,
+	.negative_x = 0,
+	.negative_y = 0,
+	.negative_z = 1,
+};
+static struct i2c_board_info __initdata mop500_i2c2_devices_u8500[] = {
+	{
+		/* LSM303DLH Accelerometer */
+		I2C_BOARD_INFO("lsm303dlh_a", 0x18),
+		.platform_data = &lsm303dlh_pdata_u8500,
+	},
+	{
+		/* LSM303DLH Magnetometer */
+		I2C_BOARD_INFO("lsm303dlh_m", 0x1E),
+		.platform_data = &lsm303dlh_pdata_u8500,
+	},
+	{
+		/* L3G4200D Gyroscope */
+		I2C_BOARD_INFO("l3g4200d", 0x68),
+		.platform_data = &l3g4200d_pdata_u8500,
+	},
+};
+
+/*
  * Synaptics RMI4 touchscreen interface on the U8500 UIB
  */
 
@@ -32,7 +76,6 @@ static struct synaptics_rmi4_platform_data rmi4_i2c_dev_platformdata = {
 	.irq_type       = (IRQF_TRIGGER_FALLING | IRQF_SHARED),
 	.x_flip		= false,
 	.y_flip		= true,
-	.regulator_en   = false,
 };
 
 static struct i2c_board_info __initdata mop500_i2c3_devices_u8500[] = {
@@ -108,4 +151,16 @@ void __init mop500_u8500uib_init(void)
 	mop500_uib_i2c_add(0, mop500_i2c0_devices_u8500,
 			ARRAY_SIZE(mop500_i2c0_devices_u8500));
 
+	if (machine_is_hrefv60()) {
+		lsm303dlh_pdata_u8500.irq_a1 = HREFV60_ACCEL_INT1_GPIO;
+		lsm303dlh_pdata_u8500.irq_a2 = HREFV60_ACCEL_INT2_GPIO;
+		lsm303dlh_pdata_u8500.irq_m = HREFV60_MAGNET_DRDY_GPIO;
+	} else {
+		lsm303dlh_pdata_u8500.irq_a1 = GPIO_ACCEL_INT1;
+		lsm303dlh_pdata_u8500.irq_a2 = GPIO_ACCEL_INT2;
+		lsm303dlh_pdata_u8500.irq_m = GPIO_MAGNET_DRDY;
+	}
+
+	mop500_uib_i2c_add(2, mop500_i2c2_devices_u8500,
+			ARRAY_SIZE(mop500_i2c2_devices_u8500));
 }
diff --git a/arch/arm/mach-ux500/board-mop500-uib.c b/arch/arm/mach-ux500/board-mop500-uib.c
index 69cce41f602..f4dfd52541c 100644
--- a/arch/arm/mach-ux500/board-mop500-uib.c
+++ b/arch/arm/mach-ux500/board-mop500-uib.c
@@ -25,7 +25,7 @@ struct uib {
 	void (*init)(void);
 };
 
-static struct __initdata uib mop500_uibs[] = {
+static struct uib __initdata mop500_uibs[] = {
 	[STUIB] = {
 		.name	= "ST-UIB",
 		.option	= "stuib",
@@ -38,7 +38,7 @@ static struct __initdata uib mop500_uibs[] = {
 	},
 };
 
-static struct uib *mop500_uib;
+static struct uib __initdata *mop500_uib;
 
 static int __init mop500_uib_setup(char *str)
 {
@@ -64,7 +64,7 @@ __setup("uib=", mop500_uib_setup);
  * The UIBs are detected after the I2C host controllers are registered, so
  * i2c_register_board_info() can't be used.
  */
-void mop500_uib_i2c_add(int busnum, struct i2c_board_info *info,
+void mop500_uib_i2c_add(int busnum, struct i2c_board_info const *info,
 		unsigned n)
 {
 	struct i2c_adapter *adap;
diff --git a/arch/arm/mach-ux500/board-mop500-usb.h b/arch/arm/mach-ux500/board-mop500-usb.h
new file mode 100644
index 00000000000..85288463a73
--- /dev/null
+++ b/arch/arm/mach-ux500/board-mop500-usb.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Saketh Ram Bommisetti <sakethram.bommisetti@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __BOARD_MOP500_USB_H
+#define __BOARD_MOP500_USB_H
+
+extern struct ab8500_usbgpio_platform_data ab8500_usbgpio_plat_data;
+
+#endif
diff --git a/arch/arm/mach-ux500/board-mop500-vibra.c b/arch/arm/mach-ux500/board-mop500-vibra.c
new file mode 100644
index 00000000000..bb4ba28774f
--- /dev/null
+++ b/arch/arm/mach-ux500/board-mop500-vibra.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2010 ST-Ericsson SA
+ *
+ * License terms:GNU General Public License (GPL) version 2
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <asm/mach-types.h>
+
+#include <linux/ste_timed_vibra.h>
+#include <sound/ux500_ab8500_ext.h>
+
+/* For details check ste_timed_vibra docbook */
+static struct ste_timed_vibra_platform_data rotary_vibra_plat_data = {
+	.is_linear_vibra = false,
+	.boost_level    = 100,
+	.boost_time     = 60,
+	.on_level       = 50,
+	.off_level      = 50,
+	.off_time       = 60,
+	.timed_vibra_control = ux500_ab8500_audio_pwm_vibra,
+};
+
+static struct ste_timed_vibra_platform_data linear_vibra_plat_data = {
+	.is_linear_vibra = true,
+	.boost_level    = 80,
+	.boost_time     = 40,
+	.on_level       = 80,
+	.off_level      = 80,
+	.off_time       = 0,
+	.timed_vibra_control = ux500_ab8500_audio_pwm_vibra,
+};
+
+/* Timed output vibrator device */
+static struct platform_device ux500_vibra_device = {
+	.name = "ste_timed_output_vibra",
+};
+
+void __init mop500_vibra_init(void)
+{
+	int ret;
+
+	if (machine_is_hrefv60())
+		ux500_vibra_device.dev.platform_data = &linear_vibra_plat_data;
+	else
+		ux500_vibra_device.dev.platform_data = &rotary_vibra_plat_data;
+
+	ret = platform_device_register(&ux500_vibra_device);
+	if (ret < 0)
+		pr_err("vibra dev register failed");
+}
diff --git a/arch/arm/mach-ux500/board-mop500-wlan.c b/arch/arm/mach-ux500/board-mop500-wlan.c
new file mode 100644
index 00000000000..65863efc78c
--- /dev/null
+++ b/arch/arm/mach-ux500/board-mop500-wlan.c
@@ -0,0 +1,180 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <asm/mach-types.h>
+#include <mach/irqs.h>
+#include <plat/pincfg.h>
+#include "pins.h"
+#include "../drivers/staging/cw1200/cw1200_plat.h"
+
+static void cw1200_release(struct device *dev);
+static int cw1200_power_ctrl(const struct cw1200_platform_data *pdata,
+			     bool enable);
+
+static struct resource cw1200_href_resources[] = {
+	{
+		.start = 215,
+		.end = 215,
+		.flags = IORESOURCE_IO,
+		.name = "cw1200_reset",
+	},
+#ifdef CONFIG_CW1200_USE_GPIO_IRQ
+	{
+		.start = NOMADIK_GPIO_TO_IRQ(216),
+		.end = NOMADIK_GPIO_TO_IRQ(216),
+		.flags = IORESOURCE_IRQ,
+		.name = "cw1200_irq",
+	},
+#endif /* CONFIG_CW1200_USE_GPIO_IRQ */
+};
+
+static struct resource cw1200_href60_resources[] = {
+	{
+		.start = 85,
+		.end = 85,
+		.flags = IORESOURCE_IO,
+		.name = "cw1200_reset",
+	},
+#ifdef CONFIG_CW1200_USE_GPIO_IRQ
+	{
+		.start = NOMADIK_GPIO_TO_IRQ(4),
+		.end = NOMADIK_GPIO_TO_IRQ(4),
+		.flags = IORESOURCE_IRQ,
+		.name = "cw1200_irq",
+	},
+#endif /* CONFIG_CW1200_USE_GPIO_IRQ */
+};
+
+static struct cw1200_platform_data cw1200_platform_data = { 0 };
+
+static struct platform_device cw1200_device = {
+	.name = "cw1200_wlan",
+	.dev = {
+		.platform_data = &cw1200_platform_data,
+		.release = cw1200_release,
+		.init_name = "cw1200_wlan",
+	},
+};
+
+const struct cw1200_platform_data *cw1200_get_platform_data(void)
+{
+	return &cw1200_platform_data;
+}
+EXPORT_SYMBOL_GPL(cw1200_get_platform_data);
+
+static int cw1200_pins_enable(bool enable)
+{
+	struct ux500_pins *pins;
+	int ret = 0;
+
+	pins = ux500_pins_get("sdi1");
+	if (!pins) {
+		printk(KERN_ERR "cw1200: Pins are not found. "
+				"Check platform data.\n");
+		return -ENOENT;
+	}
+
+	if (enable)
+		ret = ux500_pins_enable(pins);
+	else
+		ret = ux500_pins_disable(pins);
+
+	if (ret)
+		printk(KERN_ERR "cw1200: Pins can not be %s: %d.\n",
+				enable ? "enabled" : "disabled",
+				ret);
+
+	ux500_pins_put(pins);
+
+	return ret;
+}
+
+static int cw1200_power_ctrl(const struct cw1200_platform_data *pdata,
+			     bool enable)
+{
+	static const char *vdd_name = "vdd";
+	struct regulator *vdd;
+	int ret = 0;
+
+	vdd = regulator_get(&cw1200_device.dev, vdd_name);
+	if (IS_ERR(vdd)) {
+		ret = PTR_ERR(vdd);
+		dev_warn(&cw1200_device.dev,
+				"%s: Failed to get regulator '%s': %d\n",
+				__func__, vdd_name, ret);
+	} else {
+		if (enable)
+			ret = regulator_enable(vdd);
+		else
+			ret = regulator_disable(vdd);
+
+		if (ret) {
+			dev_warn(&cw1200_device.dev,
+				"%s: Failed to %s regulator '%s': %d\n",
+				__func__, enable ? "enable" : "disable",
+				vdd_name, ret);
+		}
+		regulator_put(vdd);
+	}
+	return  ret;
+}
+
+int __init mop500_wlan_init(void)
+{
+	int ret;
+	if (machine_is_snowball() ||
+			machine_is_u8500() ||
+			machine_is_u5500() ||
+			machine_is_nomadik()) {
+		cw1200_device.num_resources =
+				ARRAY_SIZE(cw1200_href_resources);
+		cw1200_device.resource = cw1200_href_resources;
+	} else if (machine_is_hrefv60()) {
+		cw1200_device.num_resources =
+				ARRAY_SIZE(cw1200_href60_resources);
+		cw1200_device.resource = cw1200_href60_resources;
+	} else {
+		dev_err(&cw1200_device.dev,
+				"Unsupported mach type %d "
+				"(check mach-types.h)\n",
+				__machine_arch_type);
+		return -ENOTSUPP;
+	}
+
+	if (machine_is_snowball())
+		cw1200_platform_data.mmc_id = "mmc2";
+	else
+		cw1200_platform_data.mmc_id = "mmc3";
+
+	cw1200_platform_data.reset = &cw1200_device.resource[0];
+#ifdef CONFIG_CW1200_USE_GPIO_IRQ
+	cw1200_platform_data.irq = &cw1200_device.resource[1];
+#endif /* #ifdef CONFIG_CW1200_USE_GPIO_IRQ */
+
+	cw1200_device.dev.release = cw1200_release;
+	if (machine_is_snowball())
+		cw1200_platform_data.power_ctrl = cw1200_power_ctrl;
+
+	ret = cw1200_pins_enable(true);
+	if (WARN_ON(ret))
+		return ret;
+
+	ret = platform_device_register(&cw1200_device);
+	if (ret)
+		cw1200_pins_enable(false);
+
+	return ret;
+}
+
+static void cw1200_release(struct device *dev)
+{
+	cw1200_pins_enable(false);
+}
diff --git a/arch/arm/mach-ux500/board-mop500-wlan.h b/arch/arm/mach-ux500/board-mop500-wlan.h
new file mode 100644
index 00000000000..c6788adc46f
--- /dev/null
+++ b/arch/arm/mach-ux500/board-mop500-wlan.h
@@ -0,0 +1,17 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * U8500 board specific cw1200 (WLAN device) initialization.
+ *
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ */
+
+#ifndef __BOARD_MOP500_WLAN_H
+#define __BOARD_MOP500_WLAN_H
+
+int mop500_wlan_init(void);
+
+#endif
diff --git a/arch/arm/mach-ux500/board-mop500.c b/arch/arm/mach-ux500/board-mop500.c
index 2a08c07dec6..1cbff203157 100644
--- a/arch/arm/mach-ux500/board-mop500.c
+++ b/arch/arm/mach-ux500/board-mop500.c
@@ -19,15 +19,24 @@
 #include <linux/amba/pl022.h>
 #include <linux/amba/serial.h>
 #include <linux/spi/spi.h>
+#include <linux/hsi/hsi.h>
 #include <linux/mfd/ab8500.h>
 #include <linux/regulator/ab8500.h>
 #include <linux/mfd/tc3589x.h>
 #include <linux/mfd/tps6105x.h>
 #include <linux/mfd/ab8500/gpio.h>
+#include <linux/regulator/fixed.h>
 #include <linux/leds-lp5521.h>
 #include <linux/input.h>
+#include <linux/smsc911x.h>
 #include <linux/gpio_keys.h>
 #include <linux/delay.h>
+#include <linux/mfd/ab8500/denc.h>
+#include <linux/spi/stm_msp.h>
+#include <linux/leds_pwm.h>
+#include <linux/pwm_backlight.h>
+#include <linux/gpio/nomadik.h>
+#include <linux/leds.h>
 
 #include <asm/mach-types.h>
 #include <asm/mach/arch.h>
@@ -39,43 +48,227 @@
 #include <mach/hardware.h>
 #include <mach/setup.h>
 #include <mach/devices.h>
+#include <mach/ab8500-accdet.h>
 #include <mach/irqs.h>
+#include <mach/ste_audio.h>
+#include <mach/ste-dma40-db8500.h>
+#ifdef CONFIG_U8500_SIM_DETECT
+#include <mach/sim_detect.h>
+#endif
+#include <mach/ste_audio_io_vibrator.h>
+#include <video/av8100.h>
 
 #include "pins-db8500.h"
-#include "ste-dma40-db8500.h"
 #include "devices-db8500.h"
 #include "board-mop500.h"
 #include "board-mop500-regulators.h"
+#include "board-mop500-bm.h"
+#include "board-mop500-wlan.h"
+#include "board-mop500-usb.h"
+
+#ifdef CONFIG_AB8500_DENC
+static struct ab8500_denc_platform_data ab8500_denc_pdata = {
+	.ddr_enable = true,
+	.ddr_little_endian = false,
+};
+#endif
+
+static struct ab8500_audio_platform_data ab8500_audio_plat_data = {
+	.ste_gpio_altf_init = msp13_i2s_init,
+	.ste_gpio_altf_exit = msp13_i2s_exit,
+};
+
+static struct gpio_led snowball_led_array[] = {
+	{
+		.name = "user_led",
+		.default_trigger = "none",
+		.gpio = 142,
+	},
+};
+
+static struct gpio_led_platform_data snowball_led_data = {
+	.leds = snowball_led_array,
+	.num_leds = ARRAY_SIZE(snowball_led_array),
+};
+
+static struct platform_device snowball_led_dev = {
+	.name = "leds-gpio",
+	.dev = {
+		.platform_data = &snowball_led_data,
+	},
+};
 
 static struct ab8500_gpio_platform_data ab8500_gpio_pdata = {
 	.gpio_base		= MOP500_AB8500_GPIO(0),
 	.irq_base		= MOP500_AB8500_VIR_GPIO_IRQ_BASE,
-	/* config_reg is the initial configuration of ab8500 pins.
+	/* initial_pin_config is the initial configuration of ab8500 pins.
 	 * The pins can be configured as GPIO or alt functions based
 	 * on value present in GpioSel1 to GpioSel6 and AlternatFunction
 	 * register. This is the array of 7 configuration settings.
 	 * One has to compile time decide these settings. Below is the
 	 * explanation of these setting
-	 * GpioSel1 = 0x00 => Pins GPIO1 to GPIO8 are not used as GPIO
-	 * GpioSel2 = 0x1E => Pins GPIO10 to GPIO13 are configured as GPIO
-	 * GpioSel3 = 0x80 => Pin GPIO24 is configured as GPIO
-	 * GpioSel4 = 0x01 => Pin GPIo25 is configured as GPIO
-	 * GpioSel5 = 0x7A => Pins GPIO34, GPIO36 to GPIO39 are conf as GPIO
-	 * GpioSel6 = 0x00 => Pins GPIO41 & GPIo42 are not configured as GPIO
+	 * GpioSel1 = 0x0F => Pin GPIO1 (SysClkReq2)
+	 *                    Pin GPIO2 (SysClkReq3)
+	 *                    Pin GPIO3 (SysClkReq4)
+	 *                    Pin GPIO4 (SysClkReq6) are configured as GPIO
+	 * GpioSel2 = 0x9E => Pins GPIO10 to GPIO13 are configured as GPIO
+	 * GpioSel3 = 0x80 => Pin GPIO24 (SysClkReq7) is configured as GPIO
+	 * GpioSel4 = 0x01 => Pin GPIO25 (SysClkReq8) is configured as GPIO
+	 * GpioSel5 = 0x78 => Pin GPIO36 (ApeSpiClk)
+			      Pin GPIO37 (ApeSpiCSn)
+			      Pin GPIO38 (ApeSpiDout)
+			      Pin GPIO39 (ApeSpiDin) are configured as GPIO
+	 * GpioSel6 = 0x02 => Pin GPIO42 (SysClkReq5) is configured as GPIO
 	 * AlternaFunction = 0x00 => If Pins GPIO10 to 13 are not configured
-	 * as GPIO then this register selectes the alternate fucntions
+	 * as GPIO then this register selectes the alternate functions
 	 */
-	.config_reg		= {0x00, 0x1E, 0x80, 0x01,
-					0x7A, 0x00, 0x00},
+	.initial_pin_config     = {0x0F, 0x9E, 0x80, 0x01, 0x78, 0x02, 0x00},
+
+	/* initial_pin_direction allows for the initial GPIO direction to
+	 * be set. For Snowball we set GPIO26 to output.
+	 */
+	.initial_pin_direction  = {0x00, 0x00, 0x00, 0x02, 0x00, 0x00},
+
+	/*
+	 * initial_pin_pullups allows for the intial configuration of the
+	 * GPIO pullup/pulldown configuration.
+	 */
+	.initial_pin_pullups    = {0xE0, 0x01, 0x00, 0x00, 0x00, 0x00},
+};
+
+#ifdef CONFIG_INPUT_AB8500_ACCDET
+static struct ab8500_accdet_platform_data ab8500_accdet_pdata = {
+	.btn_keycode = KEY_MEDIA,
+	.accdet1_dbth = ACCDET1_TH_1200mV | ACCDET1_DB_70ms,
+	.accdet2122_th = ACCDET21_TH_1000mV | ACCDET22_TH_1000mV,
+	.video_ctrl_gpio = 35,  /*  AB8500_GPIO offset */
+};
+#endif
+
+static struct gpio_keys_button snowball_key_array[] = {
+	{
+		.gpio           = 32,
+		.type           = EV_KEY,
+		.code           = KEY_1,
+		.desc           = "userpb",
+		.active_low     = 1,
+		.debounce_interval = 50,
+		.wakeup         = 1,
+	},
+	{
+		.gpio           = 151,
+		.type           = EV_KEY,
+		.code           = KEY_2,
+		.desc           = "extkb1",
+		.active_low     = 1,
+		.debounce_interval = 50,
+		.wakeup         = 1,
+	},
+	{
+		.gpio           = 152,
+		.type           = EV_KEY,
+		.code           = KEY_3,
+		.desc           = "extkb2",
+		.active_low     = 1,
+		.debounce_interval = 50,
+		.wakeup         = 1,
+	},
+	{
+		.gpio           = 161,
+		.type           = EV_KEY,
+		.code           = KEY_4,
+		.desc           = "extkb3",
+		.active_low     = 1,
+		.debounce_interval = 50,
+		.wakeup         = 1,
+	},
+	{
+		.gpio           = 162,
+		.type           = EV_KEY,
+		.code           = KEY_5,
+		.desc           = "extkb4",
+		.active_low     = 1,
+		.debounce_interval = 50,
+		.wakeup         = 1,
+	},
+};
+
+static struct gpio_keys_platform_data snowball_key_data = {
+	.buttons        = snowball_key_array,
+	.nbuttons       = ARRAY_SIZE(snowball_key_array),
+};
+
+static struct platform_device snowball_key_dev = {
+	.name           = "gpio-keys",
+	.id             = -1,
+	.dev            = {
+		.platform_data  = &snowball_key_data,
+	}
+};
+
+static struct smsc911x_platform_config snowball_sbnet_cfg = {
+	.irq_polarity = SMSC911X_IRQ_POLARITY_ACTIVE_HIGH,
+	.irq_type = SMSC911X_IRQ_TYPE_PUSH_PULL,
+	.flags = SMSC911X_USE_16BIT | SMSC911X_FORCE_INTERNAL_PHY,
+	.shift = 1,
+};
+
+static struct resource sbnet_res[] = {
+	{
+		.name = "smsc911x-memory",
+		.start = (0x5000 << 16),
+		.end  =  (0x5000 << 16) + 0xffff,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = NOMADIK_GPIO_TO_IRQ(140),
+		.end = NOMADIK_GPIO_TO_IRQ(140),
+		.flags = IORESOURCE_IRQ | IORESOURCE_IRQ_HIGHEDGE,
+	},
+};
+
+static struct platform_device snowball_sbnet_dev = {
+	.name           = "smsc911x",
+	.num_resources  = ARRAY_SIZE(sbnet_res),
+	.resource       = sbnet_res,
+	.dev            = {
+		.platform_data = &snowball_sbnet_cfg,
+	},
 };
 
+#ifdef CONFIG_MODEM_U8500
+static struct platform_device u8500_modem_dev = {
+	.name = "u8500-modem",
+	.id   = 0,
+	.dev  = {
+		.platform_data = NULL,
+	},
+};
+#endif
+
 static struct ab8500_platform_data ab8500_platdata = {
 	.irq_base	= MOP500_AB8500_IRQ_BASE,
 	.regulator_reg_init = ab8500_regulator_reg_init,
 	.num_regulator_reg_init	= ARRAY_SIZE(ab8500_regulator_reg_init),
 	.regulator	= ab8500_regulators,
 	.num_regulator	= ARRAY_SIZE(ab8500_regulators),
+#ifdef CONFIG_AB8500_DENC
+	.denc		= &ab8500_denc_pdata,
+#endif
+	.audio		= &ab8500_audio_plat_data,
+	.battery	= &ab8500_bm_data,
+	.charger	= &ab8500_charger_plat_data,
+	.btemp		= &ab8500_btemp_plat_data,
+	.fg		= &ab8500_fg_plat_data,
+	.chargalg	= &ab8500_chargalg_plat_data,
 	.gpio		= &ab8500_gpio_pdata,
+	.usb		= &ab8500_usbgpio_plat_data,
+#ifdef CONFIG_INPUT_AB8500_ACCDET
+	.accdet = &ab8500_accdet_pdata,
+#endif
+#ifdef CONFIG_PM
+       .pm_power_off = true,
+#endif
 };
 
 static struct resource ab8500_resources[] = {
@@ -106,6 +299,19 @@ static struct tps6105x_platform_data mop500_tps61052_data = {
 };
 
 /*
+ * GPIO-regulator wlan vbat data
+ */
+
+static struct fixed_voltage_config snowball_gpio_wlan_vbat_data = {
+	.supply_name 		= "WLAN-VBAT",
+	.gpio			= SNOWBALL_EN_3V6_GPIO,
+	.microvolts		= 3600000,
+	.enable_high		= 1,
+	.init_data		= &gpio_wlan_vbat_regulator,
+	.startup_delay		= 3500, /* Startup time */
+};
+
+/*
  * TC35892
  */
 
@@ -126,53 +332,63 @@ static struct tc3589x_platform_data mop500_tc35892_data = {
 };
 
 static struct lp5521_led_config lp5521_pri_led[] = {
-       [0] = {
-	       .chan_nr = 0,
-	       .led_current = 0x2f,
-	       .max_current = 0x5f,
-       },
-       [1] = {
-	       .chan_nr = 1,
-	       .led_current = 0x2f,
-	       .max_current = 0x5f,
-       },
-       [2] = {
-	       .chan_nr = 2,
-	       .led_current = 0x2f,
-	       .max_current = 0x5f,
-       },
+	[0] = {
+		.chan_nr = 0,
+		.led_current = 0x2f,
+		.max_current = 0x5f,
+	},
+	[1] = {
+		.chan_nr = 1,
+		.led_current = 0x2f,
+		.max_current = 0x5f,
+	},
+	[2] = {
+		.chan_nr = 2,
+		.led_current = 0x2f,
+		.max_current = 0x5f,
+	},
 };
 
+static struct av8100_platform_data av8100_plat_data = {
+	.irq			= NOMADIK_GPIO_TO_IRQ(192),
+	.reset			= MOP500_HDMI_RST_GPIO,
+	.inputclk_id		= "sysclk2",
+	.regulator_pwr_id	= "hdmi_1v8",
+	.alt_powerupseq		= true,
+	.mclk_freq		= 3, /* MCLK_RNG_31_38 */
+};
+
+
 static struct lp5521_platform_data __initdata lp5521_pri_data = {
-       .label = "lp5521_pri",
-       .led_config     = &lp5521_pri_led[0],
-       .num_channels   = 3,
-       .clock_mode     = LP5521_CLOCK_EXT,
+	.label		= "lp5521_pri",
+	.led_config	= &lp5521_pri_led[0],
+	.num_channels	= 3,
+	.clock_mode	= LP5521_CLOCK_EXT,
 };
 
 static struct lp5521_led_config lp5521_sec_led[] = {
-       [0] = {
-	       .chan_nr = 0,
-	       .led_current = 0x2f,
-	       .max_current = 0x5f,
-       },
-       [1] = {
-	       .chan_nr = 1,
-	       .led_current = 0x2f,
-	       .max_current = 0x5f,
-       },
-       [2] = {
-	       .chan_nr = 2,
-	       .led_current = 0x2f,
-	       .max_current = 0x5f,
-       },
+	[0] = {
+		.chan_nr = 0,
+		.led_current = 0x2f,
+		.max_current = 0x5f,
+	},
+	[1] = {
+		.chan_nr = 1,
+		.led_current = 0x2f,
+		.max_current = 0x5f,
+	},
+	[2] = {
+		.chan_nr = 2,
+		.led_current = 0x2f,
+		.max_current = 0x5f,
+	},
 };
 
 static struct lp5521_platform_data __initdata lp5521_sec_data = {
-       .label = "lp5521_sec",
-       .led_config     = &lp5521_sec_led[0],
-       .num_channels   = 3,
-       .clock_mode     = LP5521_CLOCK_EXT,
+	.label		= "lp5521_sec",
+	.led_config	= &lp5521_sec_led[0],
+	.num_channels	= 3,
+	.clock_mode	= LP5521_CLOCK_EXT,
 };
 
 static struct i2c_board_info __initdata mop500_i2c0_devices[] = {
@@ -181,6 +397,10 @@ static struct i2c_board_info __initdata mop500_i2c0_devices[] = {
 		.irq		= NOMADIK_GPIO_TO_IRQ(217),
 		.platform_data  = &mop500_tc35892_data,
 	},
+	{
+		I2C_BOARD_INFO("av8100", 0x70),
+		.platform_data = &av8100_plat_data,
+	},
 	/* I2C0 devices only available prior to HREFv60 */
 	{
 		I2C_BOARD_INFO("tps61052", 0x33),
@@ -230,13 +450,13 @@ static struct nmk_i2c_controller u8500_i2c##id##_data = { \
 /*
  * The board uses 4 i2c controllers, initialize all of
  * them with slave data setup time of 250 ns,
- * Tx & Rx FIFO threshold values as 8 and standard
+ * Tx & Rx FIFO threshold values as 1 and standard
  * mode of operation
  */
-U8500_I2C_CONTROLLER(0, 0xe, 1, 8, 100000, 200, I2C_FREQ_MODE_FAST);
-U8500_I2C_CONTROLLER(1, 0xe, 1, 8, 100000, 200, I2C_FREQ_MODE_FAST);
-U8500_I2C_CONTROLLER(2,	0xe, 1, 8, 100000, 200, I2C_FREQ_MODE_FAST);
-U8500_I2C_CONTROLLER(3,	0xe, 1, 8, 100000, 200, I2C_FREQ_MODE_FAST);
+U8500_I2C_CONTROLLER(0, 0xe, 1, 8, 400000, 200, I2C_FREQ_MODE_FAST);
+U8500_I2C_CONTROLLER(1, 0xe, 1, 8, 400000, 200, I2C_FREQ_MODE_FAST);
+U8500_I2C_CONTROLLER(2,	0xe, 1, 8, 400000, 200, I2C_FREQ_MODE_FAST);
+U8500_I2C_CONTROLLER(3,	0xe, 1, 8, 400000, 200, I2C_FREQ_MODE_FAST);
 
 static void __init mop500_i2c_init(void)
 {
@@ -253,18 +473,25 @@ static struct gpio_keys_button mop500_gpio_keys[] = {
 		.code			= SW_FRONT_PROXIMITY,
 		.active_low		= 0,
 		.can_disable		= 1,
+	},
+	{
+		.desc			= "HED54XXU11 Hall Effect Sensor",
+		.type			= EV_SW,
+		.code			= SW_LID, /* FIXME arbitrary usage */
+		.active_low		= 0,
+		.can_disable		= 1,
 	}
 };
 
-static struct regulator *prox_regulator;
-static int mop500_prox_activate(struct device *dev);
-static void mop500_prox_deactivate(struct device *dev);
+static struct regulator *sensors1p_regulator;
+static int mop500_sensors1p_activate(struct device *dev);
+static void mop500_sensors1p_deactivate(struct device *dev);
 
 static struct gpio_keys_platform_data mop500_gpio_keys_data = {
 	.buttons	= mop500_gpio_keys,
 	.nbuttons	= ARRAY_SIZE(mop500_gpio_keys),
-	.enable		= mop500_prox_activate,
-	.disable	= mop500_prox_deactivate,
+	.enable		= mop500_sensors1p_activate,
+	.disable	= mop500_sensors1p_deactivate,
 };
 
 static struct platform_device mop500_gpio_keys_device = {
@@ -275,30 +502,232 @@ static struct platform_device mop500_gpio_keys_device = {
 	},
 };
 
-static int mop500_prox_activate(struct device *dev)
+static struct platform_device snowball_gpio_wlan_vbat_regulator_device = {
+	.name	= "reg-fixed-voltage",
+	.id	= 0,
+	.dev	= {
+		.platform_data	= &snowball_gpio_wlan_vbat_data,
+	},
+};
+
+static int mop500_sensors1p_activate(struct device *dev)
 {
-	prox_regulator = regulator_get(&mop500_gpio_keys_device.dev,
+	sensors1p_regulator = regulator_get(&mop500_gpio_keys_device.dev,
 						"vcc");
-	if (IS_ERR(prox_regulator)) {
-		dev_err(&mop500_gpio_keys_device.dev,
-			"no regulator\n");
-		return PTR_ERR(prox_regulator);
+	if (IS_ERR(sensors1p_regulator)) {
+		dev_err(&mop500_gpio_keys_device.dev, "no regulator\n");
+		return PTR_ERR(sensors1p_regulator);
 	}
-	regulator_enable(prox_regulator);
+	regulator_enable(sensors1p_regulator);
 	return 0;
 }
 
-static void mop500_prox_deactivate(struct device *dev)
+static void mop500_sensors1p_deactivate(struct device *dev)
 {
-	regulator_disable(prox_regulator);
-	regulator_put(prox_regulator);
+	regulator_disable(sensors1p_regulator);
+	regulator_put(sensors1p_regulator);
 }
 
+#ifdef CONFIG_LEDS_PWM
+static struct led_pwm pwm_leds_data[] = {
+	[0] = {
+		.name = "lcd-backlight",
+		.pwm_id = 1,
+		.max_brightness = 255,
+		.lth_brightness = 90,
+		.pwm_period_ns = 1023,
+	},
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_SECONDARY
+	[1] = {
+		.name = "sec-lcd-backlight",
+		.pwm_id = 2,
+		.max_brightness = 255,
+		.lth_brightness = 90,
+		.pwm_period_ns = 1023,
+	},
+#endif
+};
+
+static struct led_pwm_platform_data u8500_leds_data = {
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_SECONDARY
+	.num_leds = 2,
+#else
+	.num_leds = 1,
+#endif
+	.leds = pwm_leds_data,
+};
+
+static struct platform_device ux500_leds_device = {
+	.name = "leds_pwm",
+	.dev = {
+		.platform_data = &u8500_leds_data,
+	},
+};
+#endif
+
+#ifdef CONFIG_BACKLIGHT_PWM
+static struct platform_pwm_backlight_data u8500_backlight_data[] = {
+	[0] = {
+	.pwm_id = 1,
+	.max_brightness = 255,
+	.dft_brightness = 200,
+	.lth_brightness = 90,
+	.pwm_period_ns = 1023,
+	},
+	[1] = {
+	.pwm_id = 2,
+	.max_brightness = 255,
+	.dft_brightness = 200,
+	.lth_brightness = 90,
+	.pwm_period_ns = 1023,
+	},
+};
+
+static struct platform_device ux500_backlight_device[] = {
+	[0] = {
+		.name = "pwm-backlight",
+		.id = 0,
+		.dev = {
+			.platform_data = &u8500_backlight_data[0],
+		},
+	},
+	[1] = {
+		.name = "pwm-backlight",
+		.id = 1,
+		.dev = {
+			.platform_data = &u8500_backlight_data[1],
+		},
+	},
+};
+#endif
+
+#ifdef CONFIG_HSI
+static struct hsi_board_info __initdata u8500_hsi_devices[] = {
+	{
+		.name = "hsi_char",
+		.hsi_id = 0,
+		.port = 0,
+		.tx_cfg = {
+			.mode = HSI_MODE_STREAM,
+			.channels = 2,
+			.speed = 100000,
+			{.arb_mode = HSI_ARB_RR},
+		},
+		.rx_cfg = {
+			.mode = HSI_MODE_STREAM,
+			.channels = 2,
+			.speed = 200000,
+			{.flow = HSI_FLOW_SYNC},
+		},
+	},
+	{
+		.name = "hsi_test",
+		.hsi_id = 0,
+		.port = 0,
+		.tx_cfg = {
+			.mode = HSI_MODE_FRAME,
+			.channels = 2,
+			.speed = 100000,
+			{.arb_mode = HSI_ARB_RR},
+		},
+		.rx_cfg = {
+			.mode = HSI_MODE_FRAME,
+			.channels = 2,
+			.speed = 200000,
+			{.flow = HSI_FLOW_SYNC},
+		},
+	},
+	{
+		.name = "cfhsi_v3_driver",
+		.hsi_id = 0,
+		.port = 0,
+		.tx_cfg = {
+			.mode = HSI_MODE_STREAM,
+			.channels = 2,
+			.speed = 20000,
+			{.arb_mode = HSI_ARB_RR},
+		},
+		.rx_cfg = {
+			.mode = HSI_MODE_STREAM,
+			.channels = 2,
+			.speed = 200000,
+			{.flow = HSI_FLOW_SYNC},
+		},
+	},
+};
+#endif
+
+#ifdef CONFIG_U8500_SIM_DETECT
+static struct sim_detect_platform_data sim_detect_pdata = {
+	.irq_num		= NOMADIK_GPIO_TO_IRQ(MOP500_AB8500_GPIO(6)),
+};
+struct platform_device u8500_sim_detect_device = {
+	.name	= "sim_detect",
+	.id	= 0,
+	.dev	= {
+			.platform_data          = &sim_detect_pdata,
+	},
+};
+#endif
+
 /* add any platform devices here - TODO */
-static struct platform_device *platform_devs[] __initdata = {
+static struct platform_device *mop500_platform_devs[] __initdata = {
+#ifdef CONFIG_U8500_SIM_DETECT
+	&u8500_sim_detect_device,
+#endif
+	&u8500_shrm_device,
+#ifdef CONFIG_U8500_MMIO
+	&ux500_mmio_device,
+#endif
+	&ux500_hwmem_device,
+	&u8500_mcde_device,
+	&u8500_b2r2_device,
+	&u8500_thsens_device,
+#ifdef CONFIG_STE_TRACE_MODEM
+	&u8500_trace_modem,
+#endif
+#ifdef CONFIG_CRYPTO_DEV_UX500_HASH
+	&ux500_hash1_device,
+#endif
+	&ux500_cryp1_device,
 	&mop500_gpio_keys_device,
+#ifdef CONFIG_LEDS_PWM
+	&ux500_leds_device,
+#endif
+#ifdef CONFIG_BACKLIGHT_PWM
+	&ux500_backlight_device[0],
+	&ux500_backlight_device[1],
+#endif
+#ifdef CONFIG_DB8500_MLOADER
+	&mloader_fw_device,
+#endif
+#ifdef CONFIG_HSI
+	&u8500_hsi_device,
+#endif
+#ifdef CONFIG_MODEM_U8500
+	&u8500_modem_dev,
+#endif
 };
 
+/*
+ * MSP-SPI
+ */
+
+#define NUM_MSP_CLIENTS 10
+
+static struct stm_msp_controller mop500_msp2_spi_data = {
+	.id		= 2,
+	.num_chipselect	= NUM_MSP_CLIENTS,
+	.base_addr	= U8500_MSP2_BASE,
+	.device_name	= "msp2",
+};
+
+/*
+ * SSP
+ */
+
+#define NUM_SSP_CLIENTS 10
+
 #ifdef CONFIG_STE_DMA40
 static struct stedma40_chan_cfg ssp0_dma_cfg_rx = {
 	.mode = STEDMA40_MODE_LOGICAL,
@@ -326,21 +755,21 @@ static struct pl022_ssp_controller ssp0_platform_data = {
 	.dma_filter = stedma40_filter,
 	.dma_rx_param = &ssp0_dma_cfg_rx,
 	.dma_tx_param = &ssp0_dma_cfg_tx,
-#else
-	.enable_dma = 0,
 #endif
 	/* on this platform, gpio 31,142,144,214 &
 	 * 224 are connected as chip selects
 	 */
-	.num_chipselect = 5,
+	.num_chipselect = NUM_SSP_CLIENTS,
 };
 
 static void __init mop500_spi_init(void)
 {
 	db8500_add_ssp0(&ssp0_platform_data);
+	if (!machine_is_snowball())
+		db8500_add_msp2_spi(&mop500_msp2_spi_data);
 }
 
-#ifdef CONFIG_STE_DMA40
+#ifdef CONFIG_STE_DMA40_REMOVE
 static struct stedma40_chan_cfg uart0_dma_cfg_rx = {
 	.mode = STEDMA40_MODE_LOGICAL,
 	.dir = STEDMA40_PERIPH_TO_MEM,
@@ -445,7 +874,7 @@ static void ux500_uart0_exit(void)
 }
 
 static struct amba_pl011_data uart0_plat = {
-#ifdef CONFIG_STE_DMA40
+#ifdef CONFIG_STE_DMA40_REMOVE
 	.dma_filter = stedma40_filter,
 	.dma_rx_param = &uart0_dma_cfg_rx,
 	.dma_tx_param = &uart0_dma_cfg_tx,
@@ -456,7 +885,7 @@ static struct amba_pl011_data uart0_plat = {
 };
 
 static struct amba_pl011_data uart1_plat = {
-#ifdef CONFIG_STE_DMA40
+#ifdef CONFIG_STE_DMA40_REMOVE
 	.dma_filter = stedma40_filter,
 	.dma_rx_param = &uart1_dma_cfg_rx,
 	.dma_tx_param = &uart1_dma_cfg_tx,
@@ -464,7 +893,7 @@ static struct amba_pl011_data uart1_plat = {
 };
 
 static struct amba_pl011_data uart2_plat = {
-#ifdef CONFIG_STE_DMA40
+#ifdef CONFIG_STE_DMA40_REMOVE
 	.dma_filter = stedma40_filter,
 	.dma_rx_param = &uart2_dma_cfg_rx,
 	.dma_tx_param = &uart2_dma_cfg_tx,
@@ -478,6 +907,30 @@ static void __init mop500_uart_init(void)
 	db8500_add_uart2(&uart2_plat);
 }
 
+static struct platform_device *snowball_platform_devs[] __initdata = {
+	&ux500_hwmem_device,
+	&snowball_led_dev,
+	&snowball_key_dev,
+	&snowball_sbnet_dev,
+	&snowball_gpio_wlan_vbat_regulator_device,
+	&u8500_mcde_device,
+	&u8500_b2r2_device,
+};
+
+/*
+ *  On boards hrefpv60 and later, the accessory insertion/removal,
+ *  button press/release are inverted.
+*/
+static void accessory_detect_config(void)
+{
+#ifdef CONFIG_INPUT_AB8500_ACCDET
+	if (machine_is_hrefv60())
+		ab8500_accdet_pdata.is_detection_inverted = true;
+	else
+		ab8500_accdet_pdata.is_detection_inverted = false;
+#endif
+}
+
 static void __init mop500_init_machine(void)
 {
 	int i2c0_devs;
@@ -487,21 +940,44 @@ static void __init mop500_init_machine(void)
 	 * all these GPIO pins to the internal GPIO controller
 	 * instead.
 	 */
-	if (machine_is_hrefv60())
-		mop500_gpio_keys[0].gpio = HREFV60_PROX_SENSE_GPIO;
-	else
-		mop500_gpio_keys[0].gpio = GPIO_PROX_SENSOR;
+	if (!machine_is_snowball()) {
+		if (machine_is_hrefv60()) {
+			mop500_gpio_keys[0].gpio = HREFV60_PROX_SENSE_GPIO;
+			mop500_gpio_keys[1].gpio = HREFV60_HAL_SW_GPIO;
+		} else {
+			mop500_gpio_keys[0].gpio = GPIO_PROX_SENSOR;
+			mop500_gpio_keys[1].gpio = GPIO_HAL_SENSOR;
+		}
+	}
+
+	accessory_detect_config();
 
 	u8500_init_devices();
 
 	mop500_pins_init();
 
-	platform_add_devices(platform_devs, ARRAY_SIZE(platform_devs));
+#ifdef CONFIG_HSI
+	hsi_register_board_info(u8500_hsi_devices,
+				ARRAY_SIZE(u8500_hsi_devices));
+#endif
+
+	if (machine_is_snowball())
+		platform_add_devices(snowball_platform_devs,
+					ARRAY_SIZE(snowball_platform_devs));
+	else
+		platform_add_devices(mop500_platform_devs,
+					ARRAY_SIZE(mop500_platform_devs));
 
 	mop500_i2c_init();
 	mop500_sdi_init();
+	mop500_msp_init();
 	mop500_spi_init();
 	mop500_uart_init();
+	mop500_wlan_init();
+
+#ifdef CONFIG_ANDROID_STE_TIMED_VIBRA
+	mop500_vibra_init();
+#endif
 
 	platform_device_register(&ab8500_device);
 
@@ -512,6 +988,9 @@ static void __init mop500_init_machine(void)
 	i2c_register_board_info(0, mop500_i2c0_devices, i2c0_devs);
 	i2c_register_board_info(2, mop500_i2c2_devices,
 				ARRAY_SIZE(mop500_i2c2_devices));
+
+	/* This board has full regulator constraints */
+	regulator_has_full_constraints();
 }
 
 MACHINE_START(U8500, "ST-Ericsson MOP500 platform")
@@ -531,3 +1010,12 @@ MACHINE_START(HREFV60, "ST-Ericsson U8500 Platform HREFv60+")
 	.timer		= &ux500_timer,
 	.init_machine	= mop500_init_machine,
 MACHINE_END
+
+MACHINE_START(SNOWBALL, "ST-Ericsson Snowball platform")
+	.boot_params	= 0x100,
+	.map_io		= u8500_map_io,
+	.init_irq	= ux500_init_irq,
+	/* we re-use nomadik timer here */
+	.timer		= &ux500_timer,
+	.init_machine	= mop500_init_machine,
+MACHINE_END
diff --git a/arch/arm/mach-ux500/board-mop500.h b/arch/arm/mach-ux500/board-mop500.h
index 03a31cc9b08..7d25acde335 100644
--- a/arch/arm/mach-ux500/board-mop500.h
+++ b/arch/arm/mach-ux500/board-mop500.h
@@ -7,6 +7,14 @@
 #ifndef __BOARD_MOP500_H
 #define __BOARD_MOP500_H
 
+/* This defines the NOMADIK_NR_GPIO */
+#include <mach/gpio.h>
+
+/* snowball GPIO for MMC card */
+#define SNOWBALL_SDMMC_EN_GPIO 217
+#define SNOWBALL_SDMMC_1V8_3V_GPIO 228
+#define SNOWBALL_SDMMC_CD_GPIO 218
+
 /* HREFv60-specific GPIO assignments, this board has no GPIO expander */
 #define HREFV60_TOUCH_RST_GPIO		143
 #define HREFV60_PROX_SENSE_GPIO		217
@@ -19,12 +27,26 @@
 #define HREFV60_MAGNET_DRDY_GPIO	32
 #define HREFV60_DISP1_RST_GPIO		65
 #define HREFV60_DISP2_RST_GPIO		66
+#define HREFV60_MMIO_XENON_CHARGE 	170
+#define HREFV60_XSHUTDOWN_SECONDARY_SENSOR 140
+#define XSHUTDOWN_PRIMARY_SENSOR 	141
+#define XSHUTDOWN_SECONDARY_SENSOR 	142
+
+/* MOP500 generic GPIOs */
+#define MOP500_HDMI_RST_GPIO		196
 
 /* GPIOs on the TC35892 expander */
 #define MOP500_EGPIO(x)			(NOMADIK_NR_GPIO + (x))
+#define GPIO_MAGNET_DRDY		MOP500_EGPIO(1)
 #define GPIO_SDMMC_CD			MOP500_EGPIO(3)
+#define GPIO_MMIO_XENON_CHARGE 		MOP500_EGPIO(5)
 #define GPIO_PROX_SENSOR		MOP500_EGPIO(7)
+#define GPIO_HAL_SENSOR			MOP500_EGPIO(8)
+#define GPIO_ACCEL_INT1			MOP500_EGPIO(10)
+#define GPIO_ACCEL_INT2			MOP500_EGPIO(11)
 #define GPIO_BU21013_CS			MOP500_EGPIO(13)
+#define MOP500_DISP2_RST_GPIO		MOP500_EGPIO(14)
+#define MOP500_DISP1_RST_GPIO		MOP500_EGPIO(15)
 #define GPIO_SDMMC_EN			MOP500_EGPIO(17)
 #define GPIO_SDMMC_1V8_3V_SEL		MOP500_EGPIO(18)
 #define MOP500_EGPIO_END		MOP500_EGPIO(24)
@@ -32,15 +54,35 @@
 /* GPIOs on the AB8500 mixed-signals circuit */
 #define MOP500_AB8500_GPIO(x)		(MOP500_EGPIO_END + (x))
 
+/* mop500 AB8500 GPIO */
+
+/*
+ * Snowball AB8500 GPIO
+ * Documentation starts on pin 1
+ * and framework enumerates from 0
+ */
+#define SNOWBALL_VSMPS2_1V8_GPIO	MOP500_AB8500_GPIO(0)	/* SYSCLKREQ2/GPIO1 */
+#define SNOWBALL_PM_GPIO1_GPIO		MOP500_AB8500_GPIO(1)	/* SYSCLKREQ3/GPIO2 */
+#define SNOWBALL_WLAN_CLK_REQ_GPIO	MOP500_AB8500_GPIO(2)	/* SYSCLKREQ4/GPIO3 */
+#define SNOWBALL_PM_GPIO4_GPIO		MOP500_AB8500_GPIO(3)	/* SYSCLKREQ6/GPIO4 */
+#define SNOWBALL_EN_3V6_GPIO		MOP500_AB8500_GPIO(15)	/* PWMOUT3/GPIO16 */
+#define SNOWBALL_PME_ETH_GPIO		MOP500_AB8500_GPIO(23)	/* SYSCLKREQ7/GPIO24 */
+#define SNOWBALL_EN_3V3_ETH_GPIO	MOP500_AB8500_GPIO(25)	/* GPIO26 */
+
 struct i2c_board_info;
 
 extern void mop500_sdi_init(void);
 extern void mop500_sdi_tc35892_init(void);
 void __init mop500_u8500uib_init(void);
 void __init mop500_stuib_init(void);
+void __init mop500_msp_init(void);
 void __init mop500_pins_init(void);
+void __init mop500_vibra_init(void);
 
-void mop500_uib_i2c_add(int busnum, struct i2c_board_info *info,
+void mop500_uib_i2c_add(int busnum, struct i2c_board_info const *info,
 		unsigned n);
 
+int msp13_i2s_init(void);
+int msp13_i2s_exit(void);
+
 #endif
diff --git a/arch/arm/mach-ux500/board-pins-sleep-force.c b/arch/arm/mach-ux500/board-pins-sleep-force.c
new file mode 100644
index 00000000000..bef6f4e6df4
--- /dev/null
+++ b/arch/arm/mach-ux500/board-pins-sleep-force.c
@@ -0,0 +1,267 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio.h>
+#include <linux/io.h>
+#include <linux/string.h>
+
+#include <linux/gpio/nomadik.h>
+#include <mach/hardware.h>
+
+#include "board-pins-sleep-force.h"
+#include "pins-db8500.h"
+#include "pins.h"
+
+static u32 u8500_gpio_banks[] = {U8500_GPIOBANK0_BASE,
+				 U8500_GPIOBANK1_BASE,
+				 U8500_GPIOBANK2_BASE,
+				 U8500_GPIOBANK3_BASE,
+				 U8500_GPIOBANK4_BASE,
+				 U8500_GPIOBANK5_BASE,
+				 U8500_GPIOBANK6_BASE,
+				 U8500_GPIOBANK7_BASE,
+				 U8500_GPIOBANK8_BASE};
+
+/*
+ * This function is called to force gpio power save
+ * settings during suspend.
+ */
+void sleep_pins_config_pm(pin_cfg_t *cfgs, int num)
+{
+	int i = 0;
+	int gpio = 0;
+	u32 w_imsc = 0;
+	u32 imsc = 0;
+	u32 offset;
+	u32 bitmask = 1;
+	u32 dirs_register = 0;
+	u32 dirc_register = 0;
+	u32 dats_register = 0;
+	u32 datc_register = 0;
+	u32 pdis_register_disable = 0;
+	u32 pdis_register_enabled = 0;
+	u32 slpm_register_disabled = 0;
+	u32 slpm_register_enabled = 0;
+	u32 bankaddr = 0;
+
+	gpio = PIN_NUM(cfgs[i]);
+
+	/* Get the bank number the pin is mapped to */
+	bankaddr = IO_ADDRESS(u8500_gpio_banks[(gpio >> GPIO_BLOCK_SHIFT)]);
+
+	w_imsc = readl(bankaddr + NMK_GPIO_RWIMSC) |
+	readl(bankaddr + NMK_GPIO_FWIMSC);
+
+	imsc = readl(bankaddr + NMK_GPIO_RIMSC) |
+	readl(bankaddr + NMK_GPIO_FIMSC);
+
+	for (i = 0; i < num; i++) {
+		/* Get the pin number  */
+		gpio = PIN_NUM(cfgs[i]);
+
+		/* get the offest into the register */
+		offset = gpio % NMK_GPIO_PER_CHIP;
+		/* Set the bit to toggle */
+		bitmask = 1 << offset ;
+
+		/* Next we check for direction (INPUT/OUTPUT) */
+		switch (PIN_SLPM_DIR(cfgs[i])) {
+		case GPIO_IS_INPUT:
+			/* GPIO is set to input */
+			dirc_register |= bitmask;
+
+			/*
+			 * Next check for pull (PULLUP/PULLDOWN)
+			 * and configure accordingly.
+			 */
+			switch (PIN_SLPM_PULL(cfgs[i])) {
+			case GPIO_PULL_UPDOWN_DISABLED:
+				pdis_register_disable |= bitmask;
+				break;
+
+			case GPIO_IS_PULLUP:
+				dats_register |= bitmask;
+				pdis_register_enabled |= bitmask;
+				break;
+
+			case GPIO_IS_PULLDOWN:
+				datc_register |= bitmask;
+				pdis_register_enabled |= bitmask;
+				break;
+
+			case GPIO_PULL_NO_CHANGE:
+				break;
+
+			default:
+				BUG();
+				break;
+
+			}
+			break;
+
+		case GPIO_IS_OUTPUT:
+			/* GPIO is set to output */
+			dirs_register |= bitmask;
+
+			/*
+			 * Since its output there should not
+			 * be a need to disable  PULL UP/DOWN
+			 * but better safe than sorry.
+			 */
+			pdis_register_disable |= bitmask;
+			/* Next we check for setting GPIO HIGH/LOW */
+			switch (PIN_SLPM_VAL(cfgs[i])) {
+			case GPIO_IS_OUTPUT_LOW:
+				/* GPIO is set to LOW */
+				datc_register |= bitmask;
+				break;
+
+			case GPIO_IS_OUTPUT_HIGH:
+				/* GPIO is set to high */
+				dats_register |= bitmask;
+				break;
+
+			case GPIO_IS_NO_CHANGE:
+				break;
+
+			default:
+				BUG();
+				break;
+			}
+
+			break;
+		case GPIO_IS_NOT_CHANGED:
+			break;
+
+		default:
+			BUG();
+			break;
+		}
+
+		/* Next check for Sleep Power Managment (SLPM) */
+		switch (PIN_SLPM(cfgs[i])) {
+		case GPIO_WAKEUP_IS_ENABLED:
+			slpm_register_enabled |= bitmask;
+			break;
+
+		case GPIO_WAKEUP_IS_DISBLED:
+			slpm_register_disabled |= bitmask;
+			break;
+
+		default:
+			BUG();
+			break;
+		}
+
+		/* Next check for Sleep Power Managment (SLPM) */
+		switch (PIN_SLPM_PDIS(cfgs[i])) {
+		case GPIO_PDIS_NO_CHANGE:
+			break;
+
+		case GPIO_PDIS_DISABLED:
+			pdis_register_disable |= bitmask;
+			break;
+
+		case GPIO_PDIS_ENABLED:
+			pdis_register_enabled |= bitmask;
+			break;
+
+		default:
+			BUG();
+			break;
+		}
+
+	}
+
+	/* Write the register settings GPIO direction */
+	writel(dirs_register & ~w_imsc, bankaddr + NMK_GPIO_DIRS);
+	writel(dirc_register, bankaddr + NMK_GPIO_DIRC);
+
+	writel(datc_register & ~w_imsc, bankaddr + NMK_GPIO_DATC);
+	writel(dats_register & ~w_imsc, bankaddr + NMK_GPIO_DATS);
+
+	/* Write the PDIS enable/disable */
+	writel(readl(bankaddr + NMK_GPIO_PDIS)
+		| (pdis_register_disable & ~w_imsc & ~imsc), bankaddr + NMK_GPIO_PDIS);
+	writel(readl(bankaddr + NMK_GPIO_PDIS)
+		& (~pdis_register_enabled & ~w_imsc & ~imsc), bankaddr + NMK_GPIO_PDIS);
+
+	/* Write the SLPM enable/disable */
+	writel(readl(bankaddr + NMK_GPIO_SLPC) | slpm_register_disabled,
+	       bankaddr + NMK_GPIO_SLPC);
+	writel(readl(bankaddr + NMK_GPIO_SLPC) & ~slpm_register_enabled,
+	       bankaddr + NMK_GPIO_SLPC);
+}
+
+void sleep_pins_config_pm_mux(pin_cfg_t *cfgs, int num)
+{
+	int i = 0;
+	int gpio = 0;
+	u32 offset;
+	u32 bitmask = 1;
+	u32 gpio_afsla_register_set = 0;
+	u32 gpio_afslb_register_set = 0;
+	u32 gpio_afsla_register_clear = 0;
+	u32 gpio_afslb_register_clear = 0;
+	u32 bankaddr = 0;
+
+	gpio = PIN_NUM(cfgs[i]);
+
+	/* Get the bank number the pin is mapped to */
+	bankaddr = IO_ADDRESS(u8500_gpio_banks[(gpio >> GPIO_BLOCK_SHIFT)]);
+
+	for (i = 0; i < num; i++) {
+		/* Get the pin number  */
+		gpio = PIN_NUM(cfgs[i]);
+
+		/* get the offset into the register */
+		offset = gpio % NMK_GPIO_PER_CHIP;
+		/* Set the bit to toggle */
+		bitmask = 1 << offset ;
+
+		/* First check for ALT pin configuration */
+		switch (PIN_ALT(cfgs[i])) {
+		case NMK_GPIO_ALT_GPIO:
+			/* Set bit to configured as GPIO */
+			gpio_afsla_register_clear |= bitmask;
+			gpio_afslb_register_clear |= bitmask;
+			break;
+
+		case NMK_GPIO_ALT_A:
+			/* ALT A  setting so set corresponding bit */
+			gpio_afsla_register_set |= bitmask;
+			break;
+
+		case NMK_GPIO_ALT_B:
+			/* ALT B  setting so set corresponding bit */
+			gpio_afslb_register_set |= bitmask;
+			break;
+
+		case NMK_GPIO_ALT_C:
+			/* ALT C  setting so set corresponding bits */
+			gpio_afsla_register_set |= bitmask;
+			gpio_afslb_register_set |= bitmask;
+			break;
+
+		default:
+			BUG();
+			break;
+		}
+	}
+	/* Set bits that configures GPIO */
+	writel(readl(bankaddr + NMK_GPIO_AFSLA)
+		& ~gpio_afsla_register_clear, bankaddr + NMK_GPIO_AFSLA);
+	writel(readl(bankaddr + NMK_GPIO_AFSLB)
+		& ~gpio_afslb_register_clear, bankaddr + NMK_GPIO_AFSLB);
+
+	/* Set bits that configures ALT_X */
+	writel(readl(bankaddr + NMK_GPIO_AFSLA)
+		| gpio_afsla_register_set, bankaddr + NMK_GPIO_AFSLA);
+	writel(readl(bankaddr + NMK_GPIO_AFSLB)
+		| gpio_afslb_register_set, bankaddr + NMK_GPIO_AFSLB);
+}
diff --git a/arch/arm/mach-ux500/board-pins-sleep-force.h b/arch/arm/mach-ux500/board-pins-sleep-force.h
new file mode 100644
index 00000000000..0949c9bfcda
--- /dev/null
+++ b/arch/arm/mach-ux500/board-pins-sleep-force.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __BOARD_PINS_SLEEP_FORCE_H
+#define __BOARD_PINS_SLEEP_FORCE_H
+
+#include <plat/pincfg.h>
+
+#define NMK_GPIO_PER_CHIP 32
+#define GPIO_BLOCK_SHIFT 5
+
+#define GPIO_IS_NOT_CHANGED 0
+#define GPIO_IS_INPUT 1
+#define GPIO_IS_OUTPUT 2
+
+#define GPIO_WAKEUP_IS_ENABLED 0
+#define GPIO_WAKEUP_IS_DISBLED 1
+
+#define GPIO_IS_NO_CHANGE 0
+#define GPIO_IS_OUTPUT_LOW 1
+#define GPIO_IS_OUTPUT_HIGH 2
+
+#define GPIO_PULL_NO_CHANGE 0
+#define GPIO_PULL_UPDOWN_DISABLED 1
+#define GPIO_IS_PULLUP 2
+#define GPIO_IS_PULLDOWN 3
+
+#define GPIO_PDIS_NO_CHANGE 0
+#define GPIO_PDIS_DISABLED 1
+#define GPIO_PDIS_ENABLED 2
+
+void sleep_pins_config_pm_mux(pin_cfg_t *cfgs, int num);
+void sleep_pins_config_pm(pin_cfg_t *cfgs, int num);
+
+#endif
diff --git a/arch/arm/mach-ux500/board-u5500-bm.c b/arch/arm/mach-ux500/board-u5500-bm.c
new file mode 100644
index 00000000000..7cfc6386b35
--- /dev/null
+++ b/arch/arm/mach-ux500/board-u5500-bm.c
@@ -0,0 +1,495 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * U5500 board specific charger and battery initialization parameters.
+ *
+ * License Terms: GNU General Public License v2
+ * Authors:
+ *	Johan Palsson <johan.palsson@stericsson.com>
+ *	Karl Komierowski <karl.komierowski@stericsson.com>
+ */
+
+#include <linux/power_supply.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500-bm.h>
+#include "board-u5500-bm.h"
+
+#ifdef CONFIG_AB5500_BATTERY_THERM_ON_BATCTRL
+/*
+ * These are the defined batteries that uses a NTC and ID resistor placed
+ * inside of the battery pack.
+ * Note that the abx500_res_to_temp table must be strictly sorted by falling resistance
+ * values to work.
+ */
+static struct abx500_res_to_temp temp_tbl_type1[] = {
+	{-20, 67400},
+	{  0, 49200},
+	{  5, 44200},
+	{ 10, 39400},
+	{ 15, 35000},
+	{ 20, 31000},
+	{ 25, 27400},
+	{ 30, 24300},
+	{ 35, 21700},
+	{ 40, 19400},
+	{ 45, 17500},
+	{ 50, 15900},
+	{ 55, 14600},
+	{ 60, 13500},
+	{ 65, 12500},
+	{ 70, 11800},
+	{100, 9200},
+};
+
+static struct abx500_res_to_temp temp_tbl_type2[] = {
+	{-20, 180700},
+	{  0, 160000},
+	{  5, 152700},
+	{ 10, 144900},
+	{ 15, 136800},
+	{ 20, 128700},
+	{ 25, 121000},
+	{ 30, 113800},
+	{ 35, 107300},
+	{ 40, 101500},
+	{ 45, 96500},
+	{ 50, 92200},
+	{ 55, 88600},
+	{ 60, 85600},
+	{ 65, 83000},
+	{ 70, 80900},
+	{100, 73900},
+};
+
+static struct abx500_res_to_temp temp_tbl_A[] = {
+	{-5, 53407},
+	{ 0, 48594},
+	{ 5, 43804},
+	{10, 39188},
+	{15, 34870},
+	{20, 30933},
+	{25, 27422},
+	{30, 24347},
+	{35, 21694},
+	{40, 19431},
+	{45, 17517},
+	{50, 15908},
+	{55, 14561},
+	{60, 13437},
+	{65, 12500},
+};
+
+static struct abx500_res_to_temp temp_tbl_B[] = {
+	{-5, 165418},
+	{ 0, 159024},
+	{ 5, 151921},
+	{10, 144300},
+	{15, 136424},
+	{20, 128565},
+	{25, 120978},
+	{30, 113875},
+	{35, 107397},
+	{40, 101629},
+	{45,  96592},
+	{50,  92253},
+	{55,  88569},
+	{60,  85461},
+	{65,  82869},
+};
+
+static struct abx500_v_to_cap cap_tbl_type1[] = {
+	{4171,	100},
+	{4114,	 95},
+	{4009,	 83},
+	{3947,	 74},
+	{3907,	 67},
+	{3863,	 59},
+	{3830,	 56},
+	{3813,	 53},
+	{3791,	 46},
+	{3771,	 33},
+	{3754,	 25},
+	{3735,	 20},
+	{3717,	 17},
+	{3681,	 13},
+	{3664,	  8},
+	{3651,	  6},
+	{3635,	  5},
+	{3560,	  3},
+	{3408,    1},
+	{3247,	  0},
+};
+
+static struct abx500_v_to_cap cap_tbl_A[] = {
+	{4171,	100},
+	{4114,	 95},
+	{4009,	 83},
+	{3947,	 74},
+	{3907,	 67},
+	{3863,	 59},
+	{3830,	 56},
+	{3813,	 53},
+	{3791,	 46},
+	{3771,	 33},
+	{3754,	 25},
+	{3735,	 20},
+	{3717,	 17},
+	{3681,	 13},
+	{3664,	  8},
+	{3651,	  6},
+	{3635,	  5},
+	{3560,	  3},
+	{3408,    1},
+	{3247,	  0},
+};
+static struct abx500_v_to_cap cap_tbl_B[] = {
+	{4161,	100},
+	{4124,	 98},
+	{4044,	 90},
+	{4003,	 85},
+	{3966,	 80},
+	{3933,	 75},
+	{3888,	 67},
+	{3849,	 60},
+	{3813,	 55},
+	{3787,	 47},
+	{3772,	 30},
+	{3751,	 25},
+	{3718,	 20},
+	{3681,	 16},
+	{3660,	 14},
+	{3589,	 10},
+	{3546,	  7},
+	{3495,	  4},
+	{3404,	  2},
+	{3250,	  0},
+};
+#endif
+static struct abx500_v_to_cap cap_tbl[] = {
+	{4186,	100},
+	{4163,	 99},
+	{4114,	 95},
+	{4068,	 90},
+	{3990,	 80},
+	{3926,	 70},
+	{3898,	 65},
+	{3866,	 60},
+	{3833,	 55},
+	{3812,	 50},
+	{3787,	 40},
+	{3768,	 30},
+	{3747,	 25},
+	{3730,	 20},
+	{3705,	 15},
+	{3699,	 14},
+	{3684,	 12},
+	{3672,	  9},
+	{3657,	  7},
+	{3638,	  6},
+	{3556,	  4},
+	{3424,	  2},
+	{3317,	  1},
+	{3094,	  0},
+};
+
+/*
+ * Note that the abx500_res_to_temp table must be strictly sorted by falling
+ * resistance values to work.
+ */
+static struct abx500_res_to_temp temp_tbl[] = {
+	{-5, 214834},
+	{ 0, 162943},
+	{ 5, 124820},
+	{10,  96520},
+	{15,  75306},
+	{20,  59254},
+	{25,  47000},
+	{30,  37566},
+	{35,  30245},
+	{40,  24520},
+	{45,  20010},
+	{50,  16432},
+	{55,  13576},
+	{60,  11280},
+	{65,   9425},
+};
+
+static const struct abx500_battery_type bat_type[] = {
+	[BATTERY_UNKNOWN] = {
+		/* First element always represent the UNKNOWN battery */
+		.name = POWER_SUPPLY_TECHNOLOGY_UNKNOWN,
+		.resis_high = 0,
+		.resis_low = 0,
+		.battery_resistance = 300,
+		.charge_full_design = 612,
+		.nominal_voltage = 3700,
+		.termination_vol = 4050,
+		.termination_curr = 200,
+		.recharge_vol = 3990,
+		.normal_cur_lvl = 400,
+		.normal_vol_lvl = 4100,
+		.maint_a_cur_lvl = 400,
+		.maint_a_vol_lvl = 4050,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 400,
+		.maint_b_vol_lvl = 4025,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl),
+		.r_to_t_tbl = temp_tbl,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl),
+		.v_to_cap_tbl = cap_tbl,
+	},
+
+#ifdef CONFIG_AB5500_BATTERY_THERM_ON_BATCTRL
+	{
+		.name = POWER_SUPPLY_TECHNOLOGY_LIPO,
+		.resis_high = 70000,
+		.resis_low = 8200,
+		.battery_resistance = 300,
+		.charge_full_design = 900,
+		.nominal_voltage = 3600,
+		.termination_vol = 4150,
+		.termination_curr = 80,
+		.recharge_vol = 4025,
+		.normal_cur_lvl = 700,
+		.normal_vol_lvl = 4200,
+		.maint_a_cur_lvl = 600,
+		.maint_a_vol_lvl = 4150,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 600,
+		.maint_b_vol_lvl = 4025,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl_type1),
+		.r_to_t_tbl = temp_tbl_type1,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl_type1),
+		.v_to_cap_tbl = cap_tbl_type1,
+
+	},
+	{
+		.name = POWER_SUPPLY_TECHNOLOGY_LIPO,
+		.resis_high = 165418,
+		.resis_low = 82869,
+		.battery_resistance = 300,
+		.charge_full_design = 900,
+		.nominal_voltage = 3600,
+		.termination_vol = 4150,
+		.termination_curr = 80,
+		.recharge_vol = 4025,
+		.normal_cur_lvl = 700,
+		.normal_vol_lvl = 4200,
+		.maint_a_cur_lvl = 600,
+		.maint_a_vol_lvl = 4150,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 600,
+		.maint_b_vol_lvl = 4025,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl_B),
+		.r_to_t_tbl = temp_tbl_B,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl_B),
+		.v_to_cap_tbl = cap_tbl_B,
+	},
+#else
+/*
+ * These are the batteries that doesn't have an internal NTC resistor to measure
+ * its temperature. The temperature in this case is measure with a NTC placed
+ * near the battery but on the PCB.
+ */
+	{
+		.name = POWER_SUPPLY_TECHNOLOGY_LIPO,
+		.resis_high = 76000,
+		.resis_low = 53000,
+		.battery_resistance = 300,
+		.charge_full_design = 900,
+		.nominal_voltage = 3700,
+		.termination_vol = 4150,
+		.termination_curr = 100,
+		.recharge_vol = 4025,
+		.normal_cur_lvl = 700,
+		.normal_vol_lvl = 4200,
+		.maint_a_cur_lvl = 600,
+		.maint_a_vol_lvl = 4150,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 600,
+		.maint_b_vol_lvl = 4025,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl),
+		.r_to_t_tbl = temp_tbl,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl),
+		.v_to_cap_tbl = cap_tbl,
+	},
+	{
+		.name = POWER_SUPPLY_TECHNOLOGY_LION,
+		.resis_high = 30000,
+		.resis_low = 10000,
+		.battery_resistance = 300,
+		.charge_full_design = 950,
+		.nominal_voltage = 3700,
+		.termination_vol = 4150,
+		.termination_curr = 100,
+		.recharge_vol = 4025,
+		.normal_cur_lvl = 700,
+		.normal_vol_lvl = 4200,
+		.maint_a_cur_lvl = 600,
+		.maint_a_vol_lvl = 4150,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 600,
+		.maint_b_vol_lvl = 4025,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl),
+		.r_to_t_tbl = temp_tbl,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl),
+		.v_to_cap_tbl = cap_tbl,
+	},
+	{
+		.name = POWER_SUPPLY_TECHNOLOGY_LION,
+		.resis_high = 95000,
+		.resis_low = 76001,
+		.battery_resistance = 300,
+		.charge_full_design = 950,
+		.nominal_voltage = 3700,
+		.termination_vol = 4150,
+		.termination_curr = 100,
+		.recharge_vol = 4025,
+		.normal_cur_lvl = 700,
+		.normal_vol_lvl = 4200,
+		.maint_a_cur_lvl = 600,
+		.maint_a_vol_lvl = 4150,
+		.maint_a_chg_timer_h = 60,
+		.maint_b_cur_lvl = 600,
+		.maint_b_vol_lvl = 4025,
+		.maint_b_chg_timer_h = 200,
+		.low_high_cur_lvl = 300,
+		.low_high_vol_lvl = 4000,
+		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl),
+		.r_to_t_tbl = temp_tbl,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl),
+		.v_to_cap_tbl = cap_tbl,
+	},
+#endif
+};
+
+static char *ab5500_charger_supplied_to[] = {
+	"abx500_chargalg",
+	"ab5500_fg",
+	"ab5500_btemp",
+};
+
+static char *ab5500_btemp_supplied_to[] = {
+	"abx500_chargalg",
+	"ab5500_fg",
+};
+
+static char *ab5500_fg_supplied_to[] = {
+	"abx500_chargalg",
+};
+
+static char *abx500_chargalg_supplied_to[] = {
+	"ab5500_fg",
+};
+
+struct abx500_charger_platform_data ab5500_charger_plat_data = {
+	.supplied_to = ab5500_charger_supplied_to,
+	.num_supplicants = ARRAY_SIZE(ab5500_charger_supplied_to),
+};
+
+struct abx500_btemp_platform_data ab5500_btemp_plat_data = {
+	.supplied_to = ab5500_btemp_supplied_to,
+	.num_supplicants = ARRAY_SIZE(ab5500_btemp_supplied_to),
+};
+
+struct abx500_fg_platform_data ab5500_fg_plat_data = {
+	.supplied_to = ab5500_fg_supplied_to,
+	.num_supplicants = ARRAY_SIZE(ab5500_fg_supplied_to),
+};
+
+struct abx500_chargalg_platform_data abx500_chargalg_plat_data = {
+	.supplied_to = abx500_chargalg_supplied_to,
+	.num_supplicants = ARRAY_SIZE(abx500_chargalg_supplied_to),
+};
+
+static const struct abx500_bm_capacity_levels cap_levels = {
+	.critical	= 2,
+	.low		= 10,
+	.normal		= 70,
+	.high		= 95,
+	.full		= 100,
+};
+
+static const struct abx500_fg_parameters fg = {
+	.recovery_sleep_timer = 10,
+	.recovery_total_time = 100,
+	.init_timer = 1,
+	.init_discard_time = 5,
+	.init_total_time = 40,
+	.high_curr_time = 60,
+	.accu_charging = 30,
+	.accu_high_curr = 30,
+	.high_curr_threshold = 50,
+	.lowbat_threshold = 3100,
+};
+
+static const struct abx500_maxim_parameters maxi_params = {
+	.ena_maxi = true,
+	.chg_curr = 910,
+	.wait_cycles = 10,
+	.charger_curr_step = 100,
+};
+
+static const struct abx500_bm_charger_parameters chg = {
+	.usb_volt_max		= 5500,
+	.usb_curr_max		= 1500,
+	.ac_volt_max		= 7500,
+	.ac_curr_max		= 1500,
+};
+
+struct abx500_bm_data ab5500_bm_data = {
+	.temp_under		= 3,
+	.temp_low		= 8,
+	/* TODO: Need to verify the temp values */
+	.temp_high		= 155,
+	.temp_over		= 160,
+	.main_safety_tmr_h	= 4,
+	.usb_safety_tmr_h	= 4,
+	.bkup_bat_v		= 0x00,
+	.bkup_bat_i		= 0x00,
+	.no_maintenance		= true,
+#ifdef CONFIG_AB5500_BATTERY_THERM_ON_BATCTRL
+	.adc_therm		= ABx500_ADC_THERM_BATCTRL,
+#else
+	.adc_therm		= ABx500_ADC_THERM_BATTEMP,
+#endif
+	.chg_unknown_bat	= false,
+	.enable_overshoot	= false,
+	.fg_res			= 20,
+	.cap_levels		= &cap_levels,
+	.bat_type		= bat_type,
+	.n_btypes		= ARRAY_SIZE(bat_type),
+	.batt_id		= 0,
+	.interval_charging	= 5,
+	.interval_not_charging	= 120,
+	.temp_hysteresis	= 3,
+	.maxi			= &maxi_params,
+	.chg_params		= &chg,
+	.fg_params		= &fg,
+};
+
+/* ab5500 energy management platform data */
+struct abx500_bm_plat_data abx500_bm_pt_data = {
+	.battery	= &ab5500_bm_data,
+	.charger	= &ab5500_charger_plat_data,
+	.btemp		= &ab5500_btemp_plat_data,
+	.fg		= &ab5500_fg_plat_data,
+	.chargalg	= &abx500_chargalg_plat_data,
+};
diff --git a/arch/arm/mach-ux500/board-u5500-bm.h b/arch/arm/mach-ux500/board-u5500-bm.h
new file mode 100644
index 00000000000..a6346905911
--- /dev/null
+++ b/arch/arm/mach-ux500/board-u5500-bm.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * U5500 board specific charger and battery initialization parameters.
+ *
+ * License Terms: GNU General Public License v2
+ * Authors:
+ *	Johan Palsson <johan.palsson@stericsson.com>
+ *	Karl Komierowski <karl.komierowski@stericsson.com>
+ */
+
+#ifndef __BOARD_U5500_BM_H
+#define __BOARD_U5500_BM_H
+
+#include <linux/mfd/abx500/ab5500-bm.h>
+
+extern struct abx500_charger_platform_data ab5500_charger_plat_data;
+extern struct abx500_btemp_platform_data ab5500_btemp_plat_data;
+extern struct abx500_fg_platform_data ab5500_fg_plat_data;
+extern struct abx500_chargalg_platform_data abx500_chargalg_plat_data;
+extern struct abx500_bm_data ab5500_bm_data;
+extern struct abx500_bm_plat_data abx500_bm_pt_data;
+
+#endif
diff --git a/arch/arm/mach-ux500/board-u5500-mcde.c b/arch/arm/mach-ux500/board-u5500-mcde.c
new file mode 100644
index 00000000000..535f3d41bba
--- /dev/null
+++ b/arch/arm/mach-ux500/board-u5500-mcde.c
@@ -0,0 +1,424 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <video/av8100.h>
+#include <asm/mach-types.h>
+#include <video/mcde_display.h>
+#include <video/mcde_display-generic_dsi.h>
+#include <video/mcde_display-av8100.h>
+#include <video/mcde_fb.h>
+#include <video/mcde_dss.h>
+
+#define DSI_UNIT_INTERVAL_0	0xA
+#define DSI_UNIT_INTERVAL_2	0x5
+
+enum {
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_PRIMARY
+	PRIMARY_DISPLAY_ID,
+#endif
+#ifdef CONFIG_DISPLAY_AV8100_TERTIARY
+	AV8100_DISPLAY_ID,
+#endif
+	MCDE_NR_OF_DISPLAYS
+};
+
+
+#ifdef CONFIG_FB_MCDE
+
+/* The initialization of hdmi disp driver must be delayed in order to
+ * ensure that inputclk will be available (needed by hdmi hw) */
+#ifdef CONFIG_DISPLAY_AV8100_TERTIARY
+static struct delayed_work work_dispreg_hdmi;
+#define DISPREG_HDMI_DELAY 6000
+#endif
+
+static int display_initialized_during_boot;
+
+static int __init startup_graphics_setup(char *str)
+{
+
+	if (get_option(&str, &display_initialized_during_boot) != 1)
+		display_initialized_during_boot = 0;
+
+	switch (display_initialized_during_boot) {
+	case 1:
+		pr_info("Startup graphics support\n");
+		break;
+	case 0:
+	default:
+		pr_info("No startup graphics supported\n");
+		break;
+	};
+
+	return 1;
+}
+__setup("startup_graphics=", startup_graphics_setup);
+
+#ifdef CONFIG_DISPLAY_AV8100_TERTIARY
+static struct mcde_col_transform rgb_2_yCbCr_transform = {
+	.matrix = {
+		{0x0042, 0x0081, 0x0019},
+		{0xffda, 0xffb6, 0x0070},
+		{0x0070, 0xffa2, 0xffee},
+	},
+	.offset = {0x10, 0x80, 0x80},
+};
+#endif
+
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_PRIMARY
+static struct mcde_port port0 = {
+	.type = MCDE_PORTTYPE_DSI,
+	.mode = MCDE_PORTMODE_CMD,
+	.pixel_format = MCDE_PORTPIXFMT_DSI_24BPP,
+	.ifc = DSI_VIDEO_MODE,
+	.link = 0,
+#if defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY) &&	\
+			defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_AUTO_SYNC)
+	.sync_src = MCDE_SYNCSRC_OFF,
+	.update_auto_trig = true,
+#else
+	.sync_src = MCDE_SYNCSRC_BTA,
+	.update_auto_trig = false,
+#endif
+	.phy = {
+		.dsi = {
+			.virt_id = 0,
+			.num_data_lanes = 2,
+			.ui = DSI_UNIT_INTERVAL_0,
+			.clk_cont = false,
+			.data_lanes_swap = false,
+		},
+	},
+};
+
+struct mcde_display_generic_platform_data u5500_generic_display0_pdata = {
+	.reset_gpio = 226,
+	.reset_delay = 10,
+	.sleep_out_delay = 140,
+#ifdef CONFIG_REGULATOR
+	.regulator_id = "v-display",
+	.min_supply_voltage = 2500000, /* 2.5V */
+	.max_supply_voltage = 2700000 /* 2.7V */
+#endif
+};
+
+struct mcde_display_device u5500_generic_display0 = {
+	.name = "mcde_disp_generic",
+	.id = PRIMARY_DISPLAY_ID,
+	.port = &port0,
+	.chnl_id = MCDE_CHNL_A,
+	.fifo = MCDE_FIFO_A,
+#ifdef CONFIG_MCDE_DISPLAY_PRIMARY_16BPP
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGB565,
+#else
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGBA8888,
+#endif
+	.native_x_res = 864,
+	.native_y_res = 480,
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_VSYNC
+	.synchronized_update = true,
+#else
+	.synchronized_update = false,
+#endif
+	/* TODO: Remove rotation buffers once ESRAM driver is completed */
+	.rotbuf1 = U5500_ESRAM_BASE + 0x20000 * 2,
+	.rotbuf2 = U5500_ESRAM_BASE + 0x20000 * 2 + 0x10000,
+	.dev = {
+		.platform_data = &u5500_generic_display0_pdata,
+	},
+};
+#endif /* CONFIG_DISPLAY_GENERIC_DSI_PRIMARY */
+
+#ifdef CONFIG_DISPLAY_AV8100_TERTIARY
+static struct mcde_port port2 = {
+	.type = MCDE_PORTTYPE_DSI,
+	.mode = MCDE_PORTMODE_CMD,
+	.pixel_format = MCDE_PORTPIXFMT_DSI_24BPP,
+	.ifc = DSI_VIDEO_MODE,
+	.link = 1,
+#ifdef CONFIG_AV8100_HWTRIG_INT
+	.sync_src = MCDE_SYNCSRC_TE0,
+#endif
+#ifdef CONFIG_AV8100_HWTRIG_I2SDAT3
+	.sync_src = MCDE_SYNCSRC_TE1,
+#endif
+#ifdef CONFIG_AV8100_HWTRIG_DSI_TE
+	.sync_src = MCDE_SYNCSRC_TE_POLLING,
+#endif
+#ifdef CONFIG_AV8100_HWTRIG_NONE
+	.sync_src = MCDE_SYNCSRC_OFF,
+#endif
+	.update_auto_trig = true,
+	.phy = {
+		.dsi = {
+			.virt_id = 0,
+			.num_data_lanes = 2,
+			.ui = DSI_UNIT_INTERVAL_2,
+			.clk_cont = false,
+			.data_lanes_swap = false,
+		},
+	},
+	.hdmi_sdtv_switch = HDMI_SWITCH,
+};
+
+static struct mcde_display_hdmi_platform_data av8100_hdmi_pdata = {
+	.reset_gpio = 0,
+	.reset_delay = 1,
+	.regulator_id = NULL,
+	.cvbs_regulator_id = "v-av8100-AV-switch",
+	.ddb_id = 1,
+	.rgb_2_yCbCr_transform = &rgb_2_yCbCr_transform,
+};
+
+static struct mcde_display_device av8100_hdmi = {
+	.name = "av8100_hdmi",
+	.id = AV8100_DISPLAY_ID,
+	.port = &port2,
+	.chnl_id = MCDE_CHNL_B,
+	.fifo = MCDE_FIFO_B,
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGB888,
+	.native_x_res = 1280,
+	.native_y_res = 720,
+	.synchronized_update = false,
+	.dev = {
+		.platform_data = &av8100_hdmi_pdata,
+	},
+	.platform_enable = NULL,
+	.platform_disable = NULL,
+};
+
+static void delayed_work_dispreg_hdmi(struct work_struct *ptr)
+{
+	if (mcde_display_device_register(&av8100_hdmi))
+		pr_warning("Failed to register av8100_hdmi\n");
+}
+#endif /* CONFIG_DISPLAY_AV8100_TERTIARY */
+
+/*
+* This function will create the framebuffer for the display that is registered.
+*/
+static int display_postregistered_callback(struct notifier_block *nb,
+	unsigned long event, void *dev)
+{
+	struct mcde_display_device *ddev = dev;
+	u16 width, height;
+	u16 virtual_width, virtual_height;
+	u32 rotate = FB_ROTATE_UR;
+	struct fb_info *fbi;
+
+	if (event != MCDE_DSS_EVENT_DISPLAY_REGISTERED)
+		return 0;
+
+	if (ddev->id < PRIMARY_DISPLAY_ID || ddev->id >= MCDE_NR_OF_DISPLAYS)
+		return 0;
+
+	mcde_dss_get_native_resolution(ddev, &width, &height);
+
+#ifdef CONFIG_DISPLAY_GENERIC_DSI_PRIMARY
+	if (ddev->id == PRIMARY_DISPLAY_ID) {
+		switch (CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_ANGLE) {
+		case 0:
+			rotate = FB_ROTATE_UR;
+			break;
+		case 90:
+			rotate = FB_ROTATE_CW;
+			swap(width, height);
+			break;
+		case 180:
+			rotate = FB_ROTATE_UD;
+			break;
+		case 270:
+			rotate = FB_ROTATE_CCW;
+			swap(width, height);
+			break;
+		}
+	}
+#endif
+
+	virtual_width = width;
+	virtual_height = height * 2;
+
+#if defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY) &&	\
+			defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_AUTO_SYNC)
+	if (ddev->id == PRIMARY_DISPLAY_ID)
+		virtual_height = height;
+#endif
+
+#ifdef CONFIG_DISPLAY_AV8100_TRIPPLE_BUFFER
+	if (ddev->id == AV8100_DISPLAY_ID)
+		virtual_height = height * 3;
+#endif
+#ifdef CONFIG_DISPLAY_AV8100_TERTIARY
+	if (ddev->id == AV8100_DISPLAY_ID)
+#ifdef CONFIG_MCDE_DISPLAY_HDMI_FB_AUTO_CREATE
+		hdmi_fb_onoff(ddev, 1, 0, 0);
+#endif /* CONFIG_MCDE_DISPLAY_HDMI_FB_AUTO_CREATE */
+	else
+#endif /* CONFIG_DISPLAY_AV8100_TERTIARY */
+	{
+		/* Create frame buffer */
+		fbi = mcde_fb_create(ddev,
+			width, height,
+			virtual_width, virtual_height,
+			ddev->default_pixel_format,
+			rotate);
+
+		if (IS_ERR(fbi)) {
+			dev_warn(&ddev->dev,
+				"Failed to create fb for display %s\n",
+						ddev->name);
+			goto display_postregistered_callback_err;
+		} else
+			dev_info(&ddev->dev, "Framebuffer created (%s)\n",
+						ddev->name);
+	}
+
+	return 0;
+
+display_postregistered_callback_err:
+	return -1;
+}
+
+static struct notifier_block display_nb = {
+	.notifier_call = display_postregistered_callback,
+};
+
+/*
+* This function is used to refresh the display (lcd, hdmi, tvout) with black
+* when the framebuffer is registered.
+* The main display will not be updated if startup graphics is displayed
+* from u-boot.
+*/
+#if defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY) &&	\
+			defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_AUTO_SYNC)
+static int framebuffer_postregistered_callback(struct notifier_block *nb,
+	unsigned long event, void *data)
+{
+	int ret = 0;
+	struct fb_event *event_data = data;
+	struct fb_info *info;
+	struct fb_var_screeninfo var;
+	struct fb_fix_screeninfo fix;
+	struct mcde_fb *mfb;
+	int i;
+
+	if (event != FB_EVENT_FB_REGISTERED)
+		return 0;
+
+	if (!event_data)
+		return 0;
+
+	info = event_data->info;
+	mfb = to_mcde_fb(info);
+	var = info->var;
+	fix = info->fix;
+
+	/* Apply overlay info */
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		struct mcde_overlay *ovly = mfb->ovlys[i];
+		struct mcde_overlay_info ovly_info;
+		struct mcde_fb *mfb = to_mcde_fb(info);
+		int num_buffers;
+		memset(&ovly_info, 0, sizeof(ovly_info));
+		ovly_info.paddr = fix.smem_start +
+			fix.line_length * var.yoffset;
+		if (ovly_info.paddr + fix.line_length * var.yres
+			 > fix.smem_start + fix.smem_len)
+			ovly_info.paddr = fix.smem_start;
+		ovly_info.fmt = mfb->pix_fmt;
+		ovly_info.stride = fix.line_length;
+		ovly_info.w = var.xres;
+		ovly_info.h = var.yres;
+		ovly_info.dirty.w = var.xres;
+		ovly_info.dirty.h = var.yres;
+		(void) mcde_dss_apply_overlay(ovly, &ovly_info);
+		num_buffers = var.yres_virtual / var.yres;
+		ret = mcde_dss_update_overlay(ovly, num_buffers == 3);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+#else
+static int framebuffer_postregistered_callback(struct notifier_block *nb,
+	unsigned long event, void *data)
+{
+	int ret = 0;
+	struct fb_event *event_data = data;
+	struct fb_info *info;
+	struct fb_var_screeninfo var;
+	struct fb_fix_screeninfo fix;
+	struct mcde_fb *mfb;
+
+	if (event != FB_EVENT_FB_REGISTERED)
+		return 0;
+
+	if (!event_data)
+		return 0;
+
+	info = event_data->info;
+	mfb = to_mcde_fb(info);
+	if (mfb->id == 0 && display_initialized_during_boot)
+		goto out;
+
+	var = info->var;
+	fix = info->fix;
+	var.yoffset = var.yoffset ? 0 : var.yres;
+	if (info->fbops->fb_pan_display)
+		ret = info->fbops->fb_pan_display(&var, info);
+out:
+	return ret;
+}
+#endif
+
+
+static struct notifier_block framebuffer_nb = {
+	.notifier_call = framebuffer_postregistered_callback,
+};
+
+int __init init_display_devices_u5500(void)
+{
+	int ret = 0;
+
+	if (!machine_is_u5500())
+		return ret;
+
+	ret = fb_register_client(&framebuffer_nb);
+	if (ret)
+		pr_warning("Failed to register framebuffer notifier\n");
+
+	ret = mcde_dss_register_notifier(&display_nb);
+	if (ret)
+		pr_warning("Failed to register dss notifier\n");
+
+#ifdef CONFIG_DISPLAY_GENERIC_PRIMARY
+	if (display_initialized_during_boot)
+		u5500_generic_display0.power_mode = MCDE_DISPLAY_PM_STANDBY;
+	ret = mcde_display_device_register(&u5500_generic_display0);
+	if (ret)
+		pr_warning("Failed to register generic display device 0\n");
+#endif
+
+#ifdef CONFIG_DISPLAY_AV8100_TERTIARY
+	INIT_DELAYED_WORK_DEFERRABLE(&work_dispreg_hdmi,
+			delayed_work_dispreg_hdmi);
+
+	schedule_delayed_work(&work_dispreg_hdmi,
+			msecs_to_jiffies(DISPREG_HDMI_DELAY));
+#endif
+
+	return ret;
+}
+
+module_init(init_display_devices_u5500);
+#endif
diff --git a/arch/arm/mach-ux500/board-u5500-mmio.c b/arch/arm/mach-ux500/board-u5500-mmio.c
new file mode 100644
index 00000000000..3326d69639d
--- /dev/null
+++ b/arch/arm/mach-ux500/board-u5500-mmio.c
@@ -0,0 +1,386 @@
+/*
+ * Copyright (C) 2011 ST-Ericsson
+ * Author: Joakim Axelsson <joakim.axelsson@stericsson.com> for ST-Ericsson
+ * Author: Rajat Verma <rajat.verma@stericsson.com> for ST-Ericsson.
+ *
+ * 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/types.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/regulator/consumer.h>
+#include <linux/vmalloc.h>
+#include <plat/pincfg.h>
+#include <mach/gpio.h>
+#include <mach/devices.h>
+#include "board-u5500.h"
+#include "../drivers/staging/mmio/mmio.h"
+
+struct mmio_board_data{
+	int number_of_regulators;
+	struct regulator **mmio_regulators;
+	/* * Pin configs */
+	struct mmio_gpio xshutdown_pins[CAMERA_SLOT_END];
+	/* * Internal clocks */
+	struct clk *clk_ptr_bml;
+	struct clk *clk_ptr_ipi2c;
+	/* * External clocks */
+	struct clk *clk_ptr_ext[CAMERA_SLOT_END];
+};
+
+/*
+ * Fill names of regulators required for powering up the
+ * camera sensor in below array
+ */
+static char *regulator_names[] = {"v-mmio-camera" , "v-ana"};
+
+static int mmio_clock_init(struct mmio_platform_data *pdata)
+{
+	int err;
+	struct mmio_board_data *extra = pdata->extra;
+
+	extra->clk_ptr_ext[PRIMARY_CAMERA] =
+		clk_get(pdata->dev, "primary-cam");
+	if (IS_ERR(extra->clk_ptr_ext[PRIMARY_CAMERA])) {
+		err = PTR_ERR(extra->clk_ptr_ext[PRIMARY_CAMERA]);
+		dev_err(pdata->dev,
+				"Error %d clock 'primary-cam'\n", err);
+		goto err_pri_ext_clk;
+	}
+	extra->clk_ptr_ext[SECONDARY_CAMERA] =
+		clk_get(pdata->dev, "secondary-cam");
+	if (IS_ERR(extra->clk_ptr_ext[SECONDARY_CAMERA])) {
+		err = PTR_ERR(extra->clk_ptr_ext[SECONDARY_CAMERA]);
+		dev_err(pdata->dev,
+				"Error %d clock 'secondary-cam'\n", err);
+		goto err_sec_ext_clk;
+	}
+
+	return 0;
+err_sec_ext_clk:
+	clk_put(extra->clk_ptr_ext[PRIMARY_CAMERA]);
+err_pri_ext_clk:
+	return err;
+}
+
+static void mmio_clock_exit(struct mmio_platform_data *pdata)
+{
+	struct mmio_board_data *extra = pdata->extra;
+
+	clk_put(extra->clk_ptr_ext[PRIMARY_CAMERA]);
+	clk_put(extra->clk_ptr_ext[SECONDARY_CAMERA]);
+}
+
+static int mmio_pin_cfg_init(struct mmio_platform_data *pdata)
+{
+	struct mmio_board_data *extra = pdata->extra;
+
+	extra->xshutdown_pins[PRIMARY_CAMERA].gpio =
+		GPIO_PRIMARY_CAM_XSHUTDOWN;
+	extra->xshutdown_pins[PRIMARY_CAMERA].active_high = 0;
+	extra->xshutdown_pins[PRIMARY_CAMERA].udelay = 250;
+
+	extra->xshutdown_pins[SECONDARY_CAMERA].gpio =
+		GPIO_SECONDARY_CAM_XSHUTDOWN;
+	extra->xshutdown_pins[SECONDARY_CAMERA].active_high = 0;
+	extra->xshutdown_pins[SECONDARY_CAMERA].udelay = 250;
+
+	return 0;
+}
+
+static void mmio_pin_cfg_exit(struct mmio_platform_data *pdata)
+{
+}
+
+/*
+ * For now, both sensors on B5500/S5500 have some power up sequence. If
+ * different sequences are needed for primary and secondary sensors, it can
+ * be implemented easily. Just use camera_slot field of mmio_platform_data
+ * to determine which camera needs to be powered up
+ */
+static int mmio_power_init(struct mmio_platform_data *pdata)
+{
+	int err = 0, i = 0;
+	struct mmio_board_data *extra = pdata->extra;
+
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	extra->number_of_regulators = ARRAY_SIZE(regulator_names);
+	extra->mmio_regulators =
+	    kzalloc(sizeof(struct regulator *) * extra->number_of_regulators,
+		    GFP_KERNEL);
+	if (!extra->mmio_regulators) {
+		dev_err(pdata->dev
+			, "Error allocating memory for mmio regulators\n");
+		err = -ENOMEM;
+		goto err_no_mem_reg;
+	}
+	for (i = 0; i <
+		extra->number_of_regulators; i++) {
+		extra->mmio_regulators[i] =
+			regulator_get(pdata->dev, regulator_names[i]);
+		if (IS_ERR(extra->mmio_regulators[i])) {
+			err = PTR_ERR(extra->mmio_regulators[i]);
+			dev_err(pdata->dev
+					, "Error %d getting regulator '%s'\n"
+					, err, regulator_names[i]);
+			goto err_regulator;
+		}
+	}
+	dev_dbg(pdata->dev , "Board %s() Exit\n", __func__);
+	return 0;
+err_regulator:
+	/*
+	 * Return regulators we have already requested
+	 */
+	while (i--)
+		regulator_put(extra->mmio_regulators[i]);
+	kfree(extra->mmio_regulators);
+err_no_mem_reg:
+	return err;
+}
+
+static void mmio_power_exit(struct mmio_platform_data *pdata)
+{
+	int i = 0;
+	struct mmio_board_data *extra = pdata->extra;;
+
+	for (i = 0; i < extra->number_of_regulators; i++)
+		regulator_put(extra->mmio_regulators[i]);
+	kfree(extra->mmio_regulators);
+}
+
+static int mmio_platform_init(struct mmio_platform_data *pdata)
+{
+	int err = 0;
+	struct mmio_board_data *extra = NULL;
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	/*
+	 * Alloc memory for our own extra data
+	 */
+	extra = kzalloc(sizeof(struct mmio_board_data), GFP_KERNEL);
+	if (!extra) {
+		dev_err(pdata->dev, "%s: memory alloc failed for "
+		"mmio_board_data\n", __func__);
+		err = -ENOMEM;
+		goto err_no_mem_extra;
+	}
+	/*
+	 * Hook the data for other callbacks to use
+	 */
+	pdata->extra = extra;
+
+	pdata->camera_slot = -1;
+
+	err = mmio_power_init(pdata);
+	if (err)
+		goto err_regulator;
+	err = mmio_clock_init(pdata);
+	if (err)
+		goto err_clock;
+	err = mmio_pin_cfg_init(pdata);
+	if (err)
+		goto err_pin_cfg;
+
+	dev_dbg(pdata->dev , "Board %s() Exit\n", __func__);
+	return 0;
+
+err_pin_cfg:
+	mmio_clock_exit(pdata);
+err_clock:
+	mmio_power_exit(pdata);
+err_regulator:
+	kfree(extra);
+err_no_mem_extra:
+	return err;
+}
+
+static void mmio_platform_exit(struct mmio_platform_data *pdata)
+{
+	struct mmio_board_data *extra = pdata->extra;
+
+	mmio_power_exit(pdata);
+	mmio_clock_exit(pdata);
+	mmio_pin_cfg_exit(pdata);
+	kfree(extra);
+	pdata->extra = NULL;
+}
+
+static int mmio_power_enable(struct mmio_platform_data *pdata)
+{
+	int err = 0, i = 0;
+	struct mmio_board_data *extra = pdata->extra;
+
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	/*
+	 * Enable the regulators
+	 */
+	for (i = 0; i < extra->number_of_regulators; i++) {
+		err = regulator_enable(extra->mmio_regulators[i]);
+		if (IS_ERR(extra->mmio_regulators[i])) {
+			err = PTR_ERR(extra->mmio_regulators[i]);
+			dev_err(pdata->dev , "Error %d enabling regulator '%s'"
+			"\n", err, regulator_names[i]);
+			goto err_regulator;
+		}
+	}
+	dev_dbg(pdata->dev , "Board %s() Exit\n", __func__);
+	return 0;
+err_regulator:
+	/*
+	 * Disable regulators we already enabled
+	 */
+	while (i--)
+		regulator_disable(extra->mmio_regulators[i]);
+	return err;
+}
+
+static void mmio_power_disable(struct mmio_platform_data *pdata)
+{
+	int i;
+	struct mmio_board_data *extra = pdata->extra;
+	/*
+	 * Disable the regulators
+	 */
+	for (i = 0; i < extra->number_of_regulators; i++)
+		regulator_disable(extra->mmio_regulators[i]);
+}
+
+static int mmio_clock_enable(struct mmio_platform_data *pdata)
+{
+	int err = 0;
+	struct mmio_board_data *extra = pdata->extra;
+
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+
+	/*
+	 * Enable appropriate external clock
+	 */
+	err = clk_enable(extra->clk_ptr_ext[pdata->camera_slot]);
+	if (err) {
+		dev_err(pdata->dev, "Error activating clock for sensor %d, err"
+			"%d\n", pdata->camera_slot, err);
+		goto err_ext_clk;
+	}
+	dev_dbg(pdata->dev , "Board %s() Exit\n", __func__);
+	return 0;
+err_ext_clk:
+	return err;
+}
+
+static void mmio_clock_disable(struct mmio_platform_data *pdata)
+{
+	struct mmio_board_data *extra = pdata->extra;
+
+	clk_disable(extra->clk_ptr_ext[pdata->camera_slot]);
+}
+
+static int mmio_config_xshutdown_pins(struct mmio_platform_data *pdata,
+				      enum mmio_select_xshutdown_t select,
+				      int is_active_high)
+{
+	int err = 0;
+	struct mmio_board_data *extra = pdata->extra;
+
+	dev_dbg(pdata->dev , "Board %s() Enter\n", __func__);
+	switch (select) {
+	case MMIO_ENABLE_XSHUTDOWN_HOST:
+		extra->xshutdown_pins[pdata->camera_slot].active_high =
+			is_active_high;
+		dev_dbg(pdata->dev , "Enabling Xshutdown GPIO PIN = %d",
+			extra->xshutdown_pins[pdata->camera_slot].gpio);
+
+		err = gpio_request
+			(extra->xshutdown_pins[pdata->camera_slot].gpio,
+				"MMIO GPIO");
+		if (err) {
+			dev_err(pdata->dev, "Error %d while requesting"
+					"Xshutdown MMIO GPIO\n",
+					err);
+			return err;
+		}
+
+		err = gpio_direction_output
+			(extra->xshutdown_pins[pdata->camera_slot].gpio,
+				0);
+		if (err) {
+			dev_err(pdata->dev, "Error %d while setting"
+					"Xshutdown MMIO GPIO"
+					"output mode\n", err);
+			return err;
+		}
+		break;
+	case MMIO_DISABLE_XSHUTDOWN:
+		dev_dbg(pdata->dev , "Disabling Xshutdown GPIO PIN = %d",
+			extra->xshutdown_pins[pdata->camera_slot].gpio);
+		gpio_free(extra->xshutdown_pins[pdata->camera_slot].gpio);
+		break;
+	default:
+		break;
+	}
+	if (err)
+		dev_err(pdata->dev , "Error configuring xshutdown, err = %d\n",
+		err);
+	return err;
+}
+
+static void mmio_set_xshutdown(struct mmio_platform_data *pdata)
+{
+	struct mmio_board_data *extra = pdata->extra;
+
+	gpio_set_value(extra->xshutdown_pins[pdata->camera_slot].gpio ,
+			(extra->xshutdown_pins[pdata->camera_slot].active_high
+			? 1 : 0));
+	udelay(extra->xshutdown_pins[pdata->camera_slot].udelay);
+}
+
+/*
+ * TODO: This function would be removed in futute.
+ * Since this function is called frequently
+ * from HSM Camera code , it is kept for Legacy.
+ */
+static int mmio_config_i2c_pins(struct mmio_platform_data *pdata,
+				enum mmio_select_i2c_t select)
+{
+	int err = 0;
+
+	switch (select) {
+	case MMIO_ACTIVATE_I2C_HOST:
+		dev_dbg(pdata->dev , "Activate I2C from Host called\n");
+		break;
+	case MMIO_DEACTIVATE_I2C:
+		dev_dbg(pdata->dev , "DeActivate I2C from Host called\n");
+		break;
+	default:
+		break;
+	}
+
+	return err;
+}
+
+static struct mmio_platform_data mmio_config = {
+	.platform_init = mmio_platform_init,
+	.platform_exit = mmio_platform_exit,
+	.power_enable = mmio_power_enable,
+	.power_disable = mmio_power_disable,
+	.clock_enable = mmio_clock_enable,
+	.clock_disable = mmio_clock_disable,
+	.config_i2c_pins = mmio_config_i2c_pins,
+	.config_xshutdown_pins = mmio_config_xshutdown_pins,
+	.set_xshutdown = mmio_set_xshutdown
+};
+
+struct platform_device u5500_mmio_device = {
+	.name = MMIO_NAME,
+	.id = -1,
+	.dev = {
+		.platform_data = &mmio_config,
+	}
+};
diff --git a/arch/arm/mach-ux500/board-u5500-pins.c b/arch/arm/mach-ux500/board-u5500-pins.c
new file mode 100644
index 00000000000..5fe41d78901
--- /dev/null
+++ b/arch/arm/mach-ux500/board-u5500-pins.c
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
+#include <plat/pincfg.h>
+
+#include "pins-db5500.h"
+#include "pins.h"
+
+static pin_cfg_t u5500_pins_default[] = {
+	/* Keypad */
+	GPIO128_KP_I0	| PIN_INPUT_PULLUP,
+	GPIO130_KP_I1	| PIN_INPUT_PULLUP,
+	GPIO132_KP_I2	| PIN_INPUT_PULLUP,
+	GPIO134_KP_I3	| PIN_INPUT_PULLUP,
+	GPIO137_KP_O4	| PIN_INPUT_PULLUP,
+	GPIO139_KP_O5	| PIN_INPUT_PULLUP,
+
+	/* MSP */
+	GPIO32_MSP0_TCK		| PIN_INPUT_PULLDOWN,
+	GPIO33_MSP0_TFS		| PIN_INPUT_PULLDOWN,
+	GPIO34_MSP0_TXD		| PIN_INPUT_PULLDOWN,
+	GPIO35_MSP0_RXD		| PIN_INPUT_PULLDOWN,
+	GPIO96_MSP1_TCK		| PIN_INPUT_PULLDOWN,
+	GPIO97_MSP1_TFS		| PIN_INPUT_PULLDOWN,
+	GPIO98_MSP1_TXD		| PIN_INPUT_PULLDOWN,
+	GPIO99_MSP1_RXD		| PIN_INPUT_PULLDOWN,
+	GPIO220_MSP2_TCK	| PIN_OUTPUT_LOW,
+	GPIO221_MSP2_TFS	| PIN_OUTPUT_LOW,
+	GPIO222_MSP2_TXD	| PIN_OUTPUT_LOW,
+
+	/* DISPLAY_ENABLE */
+	GPIO226_GPIO        | PIN_OUTPUT_LOW,
+
+	/* Backlight Enable */
+	GPIO224_GPIO        | PIN_OUTPUT_HIGH,
+
+	/* MMC0 (POP eMMC) */
+	GPIO5_MC0_DAT0		| PIN_INPUT_PULLUP,
+	GPIO6_MC0_DAT1		| PIN_INPUT_PULLUP,
+	GPIO7_MC0_DAT2		| PIN_INPUT_PULLUP,
+	GPIO8_MC0_DAT3		| PIN_INPUT_PULLUP,
+	GPIO9_MC0_DAT4		| PIN_INPUT_PULLUP,
+	GPIO10_MC0_DAT5		| PIN_INPUT_PULLUP,
+	GPIO11_MC0_DAT6		| PIN_INPUT_PULLUP,
+	GPIO12_MC0_DAT7		| PIN_INPUT_PULLUP,
+	GPIO13_MC0_CMD		| PIN_INPUT_PULLUP,
+	GPIO14_MC0_CLK		| PIN_OUTPUT_LOW,
+
+	/* UART3 */
+	GPIO165_U3_RXD		| PIN_INPUT_PULLUP,
+	GPIO166_U3_TXD		| PIN_OUTPUT_HIGH,
+	GPIO167_U3_RTSn		| PIN_OUTPUT_HIGH,
+	GPIO168_U3_CTSn		| PIN_INPUT_PULLUP,
+
+	/* AB5500 */
+	GPIO78_IRQn             | PIN_SLPM_NOCHANGE,
+	GPIO100_I2C0_SCL        | PIN_INPUT_PULLUP | PIN_SLPM_NOCHANGE,
+	GPIO101_I2C0_SDA        | PIN_SLPM_NOCHANGE,
+
+	/* TOUCH_IRQ */
+	GPIO179_GPIO	| PIN_INPUT_PULLUP,
+
+	/* SDI1 (SD-CARD) */
+	GPIO191_MC1_DAT0	| PIN_INPUT_PULLUP,
+	GPIO192_MC1_DAT1	| PIN_INPUT_PULLUP,
+	GPIO193_MC1_DAT2	| PIN_INPUT_PULLUP,
+	GPIO194_MC1_DAT3	| PIN_INPUT_PULLUP,
+	GPIO195_MC1_CLK		| PIN_OUTPUT_LOW,
+	GPIO196_MC1_CMD		| PIN_INPUT_PULLUP,
+	GPIO197_MC1_CMDDIR	| PIN_OUTPUT_HIGH,
+	GPIO198_MC1_FBCLK	| PIN_INPUT_NOPULL,
+	GPIO199_MC1_DAT0DIR	| PIN_OUTPUT_HIGH,
+	/* SD-CARD detect/levelshifter pins */
+	GPIO180_GPIO		| PIN_INPUT_PULLUP,
+	GPIO227_GPIO,
+	GPIO185_GPIO,
+
+	/* SDI3 (SDIO) */
+	GPIO171_MC3_DAT0	| PIN_INPUT_PULLUP,
+	GPIO172_MC3_DAT1	| PIN_INPUT_PULLUP,
+	GPIO173_MC3_DAT2	| PIN_INPUT_PULLUP,
+	GPIO174_MC3_DAT3	| PIN_INPUT_PULLUP,
+	GPIO175_MC3_CMD		| PIN_INPUT_PULLUP,
+	GPIO176_MC3_CLK		| PIN_OUTPUT_LOW,
+
+	/* Display & HDMI HW sync */
+	GPIO204_LCD_VSI1	| PIN_INPUT_PULLUP,
+
+	/* Camera & MMIO XshutDown*/
+	GPIO1_GPIO		| PIN_OUTPUT_LOW,
+	GPIO2_GPIO		| PIN_OUTPUT_LOW,
+
+	/* USB chip select */
+	GPIO76_GPIO		| PIN_OUTPUT_LOW,
+
+	GPIO202_ACCU0_RXD	| PIN_INPUT_PULLUP,
+	GPIO203_ACCU0_TXD	| PIN_OUTPUT_HIGH,
+};
+
+static UX500_PINS(u5500_pins_i2c1,
+	GPIO3_I2C1_SCL,
+	GPIO4_I2C1_SDA,
+);
+
+static UX500_PINS(u5500_pins_i2c2,
+	GPIO218_I2C2_SCL,
+	GPIO219_I2C2_SDA,
+);
+
+static UX500_PINS(u5500_pins_spi3,
+	GPIO187_SPI3_CS0n	| PIN_OUTPUT_HIGH,
+	GPIO188_SPI3_RXD	| PIN_INPUT_PULLDOWN,
+	GPIO189_SPI3_TXD	| PIN_OUTPUT_LOW,
+	GPIO190_SPI3_CLK	| PIN_OUTPUT_LOW,
+);
+
+static struct ux500_pin_lookup u5500_pins[] = {
+	PIN_LOOKUP("nmk-i2c.1", &u5500_pins_i2c1),
+	PIN_LOOKUP("nmk-i2c.2", &u5500_pins_i2c2),
+	PIN_LOOKUP("spi3", &u5500_pins_spi3),
+};
+
+void __init u5500_pins_init(void)
+{
+	nmk_config_pins(u5500_pins_default, ARRAY_SIZE(u5500_pins_default));
+	ux500_pins_add(u5500_pins, ARRAY_SIZE(u5500_pins));
+}
diff --git a/arch/arm/mach-ux500/board-u5500-regulators.c b/arch/arm/mach-ux500/board-u5500-regulators.c
new file mode 100644
index 00000000000..ab1213cd422
--- /dev/null
+++ b/arch/arm/mach-ux500/board-u5500-regulators.c
@@ -0,0 +1,162 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/ab5500.h>
+
+#include "board-u5500.h"
+
+/*
+ * AB5500
+ */
+
+static struct regulator_consumer_supply ab5500_ldo_d_consumers[] = {
+};
+
+static struct regulator_consumer_supply ab5500_ldo_g_consumers[] = {
+	REGULATOR_SUPPLY("v-MMC-SD", "sdi1"),
+};
+
+static struct regulator_consumer_supply ab5500_ldo_h_consumers[] = {
+	REGULATOR_SUPPLY("v-display", NULL),
+	REGULATOR_SUPPLY("vdd", "1-004b"), /* Synaptics */
+	REGULATOR_SUPPLY("vin", "2-0036"), /* LM3530 */
+};
+
+static struct regulator_consumer_supply ab5500_ldo_k_consumers[] = {
+	REGULATOR_SUPPLY("v-accel", "lsm303dlh.0"),
+	REGULATOR_SUPPLY("v-mag", "lsm303dlh.1"),
+	REGULATOR_SUPPLY("v-mmio-camera", "mmio_camera"),
+};
+
+static struct regulator_consumer_supply ab5500_ldo_l_consumers[] = {
+};
+
+static struct regulator_consumer_supply ab5500_ldo_s_consumers[] = {
+	REGULATOR_SUPPLY("v-ana", "mcde"),
+	REGULATOR_SUPPLY("v-ana", "mmio_camera"),
+};
+
+static struct regulator_consumer_supply ab5500_ldo_vdigmic_consumers[] = {
+};
+
+static struct regulator_consumer_supply ab5500_ldo_sim_consumers[] = {
+	REGULATOR_SUPPLY("debug", "reg-virt-consumer.5"),
+};
+
+static struct regulator_init_data
+ab5500_regulator_init_data[AB5500_NUM_REGULATORS] = {
+	/* AB internal analog */
+	[AB5500_LDO_D] = {
+		.constraints = {
+			.valid_ops_mask	= REGULATOR_CHANGE_STATUS,
+		},
+		.consumer_supplies	= ab5500_ldo_d_consumers,
+		.num_consumer_supplies	= ARRAY_SIZE(ab5500_ldo_d_consumers),
+	},
+	/* SD Card */
+	[AB5500_LDO_G] = {
+		.constraints = {
+			.min_uV		= 1200000,
+			.max_uV		= 2910000,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS |
+					  REGULATOR_CHANGE_MODE,
+			.valid_modes_mask = REGULATOR_MODE_NORMAL |
+					    REGULATOR_MODE_IDLE,
+		},
+		.consumer_supplies	= ab5500_ldo_g_consumers,
+		.num_consumer_supplies	= ARRAY_SIZE(ab5500_ldo_g_consumers),
+	},
+	/* Display */
+	[AB5500_LDO_H] = {
+		.constraints = {
+			.min_uV		= 2790000,
+			.max_uV		= 2790000,
+			.apply_uV	= 1,
+			.boot_on	= 1, /* display on during boot */
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS,
+		},
+		.consumer_supplies	= ab5500_ldo_h_consumers,
+		.num_consumer_supplies	= ARRAY_SIZE(ab5500_ldo_h_consumers),
+	},
+	/* Camera */
+	[AB5500_LDO_K] = {
+		.constraints = {
+			.min_uV		= 2790000,
+			.max_uV		= 2790000,
+			.apply_uV	= 1,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS,
+		},
+		.consumer_supplies	= ab5500_ldo_k_consumers,
+		.num_consumer_supplies	= ARRAY_SIZE(ab5500_ldo_k_consumers),
+	},
+	/* External eMMC */
+	[AB5500_LDO_L] = {
+		.constraints = {
+			.min_uV		= 1200000,
+			.max_uV		= 2910000,
+			.valid_ops_mask = REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS,
+		},
+		.consumer_supplies	= ab5500_ldo_l_consumers,
+		.num_consumer_supplies	= ARRAY_SIZE(ab5500_ldo_l_consumers),
+	},
+	[AB5500_LDO_S] = {
+		.constraints = {
+			.name		= "VANA",
+			.valid_ops_mask	= REGULATOR_CHANGE_STATUS,
+		},
+		.consumer_supplies	= ab5500_ldo_s_consumers,
+		.num_consumer_supplies	= ARRAY_SIZE(ab5500_ldo_s_consumers),
+	},
+	[AB5500_LDO_VDIGMIC] = {
+		.constraints = {
+			.valid_ops_mask	= REGULATOR_CHANGE_STATUS,
+		},
+		.consumer_supplies	= ab5500_ldo_vdigmic_consumers,
+		.num_consumer_supplies	=
+			ARRAY_SIZE(ab5500_ldo_vdigmic_consumers),
+	},
+	[AB5500_LDO_SIM] = {
+		.constraints = {
+			.boot_on	= 1,
+			.always_on	= 1,
+			.min_uV		= 2900000,
+			.max_uV		= 2900000,
+			.apply_uV	= 1,
+			.valid_ops_mask	= REGULATOR_CHANGE_VOLTAGE |
+					  REGULATOR_CHANGE_STATUS,
+		},
+		.consumer_supplies	= ab5500_ldo_sim_consumers,
+		.num_consumer_supplies	= ARRAY_SIZE(ab5500_ldo_sim_consumers),
+	},
+};
+
+struct ab5500_regulator_platform_data u5500_ab5500_regulator_data = {
+	.regulator	= ab5500_regulator_init_data,
+	.num_regulator	= ARRAY_SIZE(ab5500_regulator_init_data),
+};
+
+static void __init u5500_regulators_init_debug(void)
+{
+	const char data[] = "debug";
+	int i;
+
+	for (i = 0; i < 6; i++)
+		platform_device_register_data(NULL, "reg-virt-consumer", i,
+			data, sizeof(data));
+}
+
+void __init u5500_regulators_init(void)
+{
+	u5500_regulators_init_debug();
+}
diff --git a/arch/arm/mach-ux500/board-u5500-sdi.c b/arch/arm/mach-ux500/board-u5500-sdi.c
index 739fb4c5b16..495734ca072 100644
--- a/arch/arm/mach-ux500/board-u5500-sdi.c
+++ b/arch/arm/mach-ux500/board-u5500-sdi.c
@@ -7,32 +7,24 @@
 
 #include <linux/amba/mmci.h>
 #include <linux/mmc/host.h>
+#include <linux/delay.h>
 #include <linux/gpio.h>
 
 #include <plat/pincfg.h>
-#include <mach/db5500-regs.h>
 #include <plat/ste_dma40.h>
+#include <mach/db5500-regs.h>
+#include <mach/ste-dma40-db5500.h>
 
 #include "pins-db5500.h"
 #include "devices-db5500.h"
-#include "ste-dma40-db5500.h"
-
-static pin_cfg_t u5500_sdi_pins[] = {
-	/* SDI0 (POP eMMC) */
-	GPIO5_MC0_DAT0		| PIN_DIR_INPUT | PIN_PULL_UP,
-	GPIO6_MC0_DAT1		| PIN_DIR_INPUT | PIN_PULL_UP,
-	GPIO7_MC0_DAT2		| PIN_DIR_INPUT | PIN_PULL_UP,
-	GPIO8_MC0_DAT3		| PIN_DIR_INPUT | PIN_PULL_UP,
-	GPIO9_MC0_DAT4		| PIN_DIR_INPUT | PIN_PULL_UP,
-	GPIO10_MC0_DAT5		| PIN_DIR_INPUT | PIN_PULL_UP,
-	GPIO11_MC0_DAT6		| PIN_DIR_INPUT | PIN_PULL_UP,
-	GPIO12_MC0_DAT7		| PIN_DIR_INPUT | PIN_PULL_UP,
-	GPIO13_MC0_CMD		| PIN_DIR_INPUT | PIN_PULL_UP,
-	GPIO14_MC0_CLK		| PIN_DIR_OUTPUT | PIN_VAL_LOW,
-};
+#include "board-u5500.h"
+#include "../../../drivers/mmc/host/mmci.h" /* to avoid MCI_ST* redefinition */
 
+/*
+ * SDI0 (EMMC)
+ */
 #ifdef CONFIG_STE_DMA40
-struct stedma40_chan_cfg u5500_sdi0_dma_cfg_rx = {
+struct stedma40_chan_cfg sdi0_dma_cfg_rx = {
 	.mode = STEDMA40_MODE_LOGICAL,
 	.dir = STEDMA40_PERIPH_TO_MEM,
 	.src_dev_type = DB5500_DMA_DEV24_SDMMC0_RX,
@@ -41,7 +33,7 @@ struct stedma40_chan_cfg u5500_sdi0_dma_cfg_rx = {
 	.dst_info.data_width = STEDMA40_WORD_WIDTH,
 };
 
-static struct stedma40_chan_cfg u5500_sdi0_dma_cfg_tx = {
+static struct stedma40_chan_cfg sdi0_dma_cfg_tx = {
 	.mode = STEDMA40_MODE_LOGICAL,
 	.dir = STEDMA40_MEM_TO_PERIPH,
 	.src_dev_type = STEDMA40_DEV_SRC_MEMORY,
@@ -49,6 +41,46 @@ static struct stedma40_chan_cfg u5500_sdi0_dma_cfg_tx = {
 	.src_info.data_width = STEDMA40_WORD_WIDTH,
 	.dst_info.data_width = STEDMA40_WORD_WIDTH,
 };
+
+/*
+ * SDI1 (SD/MMC)
+ */
+
+static struct stedma40_chan_cfg sdi1_dma_cfg_rx = {
+	.dir = STEDMA40_PERIPH_TO_MEM,
+	.src_dev_type = DB5500_DMA_DEV25_SDMMC1_RX,
+	.dst_dev_type = STEDMA40_DEV_DST_MEMORY,
+	.src_info.data_width = STEDMA40_WORD_WIDTH,
+	.dst_info.data_width = STEDMA40_WORD_WIDTH,
+};
+
+static struct stedma40_chan_cfg sdi1_dma_cfg_tx = {
+	.dir = STEDMA40_MEM_TO_PERIPH,
+	.src_dev_type = STEDMA40_DEV_SRC_MEMORY,
+	.dst_dev_type = DB5500_DMA_DEV25_SDMMC1_TX,
+	.src_info.data_width = STEDMA40_WORD_WIDTH,
+	.dst_info.data_width = STEDMA40_WORD_WIDTH,
+};
+
+/*
+ * SDI3 (SDIO)
+ */
+static struct stedma40_chan_cfg sdi3_dma_cfg_rx = {
+	.dir = STEDMA40_PERIPH_TO_MEM,
+	.src_dev_type = DB5500_DMA_DEV27_SDMMC3_RX,
+	.dst_dev_type = STEDMA40_DEV_DST_MEMORY,
+	.src_info.data_width = STEDMA40_WORD_WIDTH,
+	.dst_info.data_width = STEDMA40_WORD_WIDTH,
+};
+
+static struct stedma40_chan_cfg sdi3_dma_cfg_tx = {
+	.dir = STEDMA40_MEM_TO_PERIPH,
+	.src_dev_type = STEDMA40_DEV_SRC_MEMORY,
+	.dst_dev_type = DB5500_DMA_DEV27_SDMMC3_TX,
+	.src_info.data_width = STEDMA40_WORD_WIDTH,
+	.dst_info.data_width = STEDMA40_WORD_WIDTH,
+};
+
 #endif
 
 static struct mmci_platform_data u5500_sdi0_data = {
@@ -61,14 +93,103 @@ static struct mmci_platform_data u5500_sdi0_data = {
 	.gpio_wp	= -1,
 #ifdef CONFIG_STE_DMA40
 	.dma_filter	= stedma40_filter,
-	.dma_rx_param	= &u5500_sdi0_dma_cfg_rx,
-	.dma_tx_param	= &u5500_sdi0_dma_cfg_tx,
+	.dma_rx_param	= &sdi0_dma_cfg_rx,
+	.dma_tx_param	= &sdi0_dma_cfg_tx,
 #endif
 };
 
-void __init u5500_sdi_init(void)
+static u32 u5500_sdi1_vdd_handler(struct device *dev, unsigned int vdd,
+		unsigned char power_mode)
 {
-	nmk_config_pins(u5500_sdi_pins, ARRAY_SIZE(u5500_sdi_pins));
+	switch (power_mode) {
+	case MMC_POWER_UP:
+	case MMC_POWER_ON:
+		/*
+		 * Level shifter voltage should depend on vdd to when deciding
+		 * on either 1.8V or 2.9V. Once the decision has been made the
+		 * level shifter must be disabled and re-enabled with a changed
+		 * select signal in order to switch the voltage. Since there is
+		 * no framework support yet for indicating 1.8V in vdd, use the
+		 * default 2.9V.
+		 */
+
+		/* Enable level shifter */
+		gpio_set_value_cansleep(GPIO_MMC_CARD_CTRL, 1);
+		udelay(100);
+		break;
+	case MMC_POWER_OFF:
+		/* Disable level shifter */
+		gpio_set_value_cansleep(GPIO_MMC_CARD_CTRL, 0);
+		break;
+	}
+
+	return MCI_ST_FBCLKEN | MCI_ST_CMDDIREN | MCI_ST_DATA0DIREN |
+	       MCI_ST_DATA2DIREN;
+}
+
+static struct mmci_platform_data u5500_sdi1_data = {
+	.vdd_handler    = u5500_sdi1_vdd_handler,
+	.ocr_mask       = MMC_VDD_29_30,
+	.f_max          = 50000000,
+	.capabilities	= MMC_CAP_4_BIT_DATA |
+				MMC_CAP_MMC_HIGHSPEED,
+	.gpio_cd        = GPIO_SDMMC_CD,
+	.gpio_wp        = -1,
+	.cd_invert	= true,
+#ifdef CONFIG_STE_DMA40
+	.dma_filter	= stedma40_filter,
+	.dma_rx_param	= &sdi1_dma_cfg_rx,
+	.dma_tx_param	= &sdi1_dma_cfg_tx,
+#endif
+};
+
+static void sdi1_configure(void)
+{
+	int pin[2];
+	int ret;
+
+	/* Level-shifter GPIOs */
+	pin[0] = GPIO_MMC_CARD_CTRL;
+	pin[1] = GPIO_MMC_CARD_VSEL;
+
+	ret = gpio_request(pin[0], "MMC_CARD_CTRL");
+	if (!ret)
+		ret = gpio_request(pin[1], "MMC_CARD_VSEL");
+
+	if (ret) {
+		pr_err("mach-u5500: error in configuring \
+			GPIO pins for MMC\n");
+		return;
+	}
+	 /* Select the default 2.9V and eanble level shifter */
+	gpio_direction_output(pin[0], 1);
+	gpio_direction_output(pin[1], 0);
 
+}
+
+/*
+ * SDI3 (SDIO WLAN)
+ */
+
+static struct mmci_platform_data u5500_sdi3_data = {
+	.ocr_mask	= MMC_VDD_29_30,
+	.f_max		= 50000000,
+	.capabilities	= MMC_CAP_4_BIT_DATA |
+				MMC_CAP_SDIO_IRQ |
+				MMC_CAP_BROKEN_SDIO_CMD53,
+	.gpio_cd	= -1,
+	.gpio_wp	= -1,
+#ifdef CONFIG_STE_DMA40
+	.dma_filter	= stedma40_filter,
+	.dma_rx_param	= &sdi3_dma_cfg_rx,
+	.dma_tx_param	= &sdi3_dma_cfg_tx,
+#endif
+};
+
+void __init u5500_sdi_init(void)
+{
 	db5500_add_sdi0(&u5500_sdi0_data);
+	sdi1_configure();
+	db5500_add_sdi1(&u5500_sdi1_data);
+	db5500_add_sdi3(&u5500_sdi3_data);
 }
diff --git a/arch/arm/mach-ux500/board-u5500.c b/arch/arm/mach-ux500/board-u5500.c
index 44fd3b5c33e..9abc6cf06a7 100644
--- a/arch/arm/mach-ux500/board-u5500.c
+++ b/arch/arm/mach-ux500/board-u5500.c
@@ -1,24 +1,387 @@
 /*
  * Copyright (C) ST-Ericsson SA 2010
  *
- * Author: Rabin Vincent <rabin.vincent@stericsson.com> for ST-Ericsson
  * License terms: GNU General Public License (GPL) version 2
  */
 
 #include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/amba/bus.h>
+#include <linux/amba/pl022.h>
 #include <linux/gpio.h>
 #include <linux/irq.h>
+#include <linux/i2c.h>
+#include <linux/i2s/i2s.h>
+#include <linux/mfd/abx500.h>
+#include <linux/led-lm3530.h>
+#include <../drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h>
+#include <linux/input/matrix_keypad.h>
+#include <linux/lsm303dlh.h>
+#include <linux/leds-ab5500.h>
+
+#include <video/av8100.h>
 
 #include <asm/mach/arch.h>
 #include <asm/mach-types.h>
 
+#include <plat/pincfg.h>
+#include <plat/i2c.h>
+
 #include <mach/hardware.h>
+#include <mach/ste-dma40-db5500.h>
+#include <mach/msp.h>
 #include <mach/devices.h>
 #include <mach/setup.h>
+#include <mach/db5500-keypad.h>
 
+#include "pins-db5500.h"
 #include "devices-db5500.h"
+#include "board-u5500.h"
+#include "board-u5500-bm.h"
+
+/*
+ * LSM303DLH
+ */
+
+static struct lsm303dlh_platform_data __initdata lsm303dlh_pdata = {
+	.name_a = "lsm303dlh.0",
+	.name_m = "lsm303dlh.1",
+	.axis_map_x = 1,
+	.axis_map_y = 0,
+	.axis_map_z = 2,
+	.negative_x = 0,
+	.negative_y = 0,
+	.negative_z = 1,
+};
+
+/*
+ * Touchscreen
+ */
+static struct synaptics_rmi4_platform_data rmi4_i2c_platformdata = {
+	.irq_number	= NOMADIK_GPIO_TO_IRQ(179),
+	.irq_type	= (IRQF_TRIGGER_FALLING | IRQF_SHARED),
+#if defined(CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_ANGLE) &&      \
+			CONFIG_DISPLAY_GENERIC_DSI_PRIMARY_ROTATION_ANGLE == 270
+	.x_flip		= true,
+	.y_flip		= false,
+#else
+	.x_flip		= false,
+	.y_flip		= true,
+#endif
+	.regulator_en	= true,
+};
+
+static struct av8100_platform_data av8100_plat_data = {
+	.irq = NOMADIK_GPIO_TO_IRQ(223),
+	.reset = 225,
+	.alt_powerupseq = true,
+	.mclk_freq = 1, /* MCLK_RNG_22_27 */
+};
+
+
+/*
+ * leds LM3530
+ */
+static struct lm3530_platform_data u5500_als_platform_data = {
+	.mode = LM3530_BL_MODE_MANUAL,
+	.als_input_mode = LM3530_INPUT_ALS1,
+	.max_current = LM3530_FS_CURR_26mA,
+	.pwm_pol_hi = true,
+	.als_avrg_time = LM3530_ALS_AVRG_TIME_512ms,
+	.brt_ramp_law = 1,	/* Linear */
+	.brt_ramp_fall = LM3530_RAMP_TIME_8s,
+	.brt_ramp_rise = LM3530_RAMP_TIME_8s,
+	.als1_resistor_sel = LM3530_ALS_IMPD_13_53kOhm,
+	.als2_resistor_sel = LM3530_ALS_IMPD_Z,
+	.als_vmin = 730,	/* mV */
+	.als_vmax = 1020,	/* mV */
+	.brt_val = 0x7F,	/* Max brightness */
+};
+
+
+/* leds-ab5500 */
+static struct ab5500_hvleds_platform_data ab5500_hvleds_data = {
+	.hw_blink = false,
+	.leds = {
+		[0] = {
+			.name = "red",
+			.led_id = 0,
+			.status = AB5500_LED_ON,
+			.max_current = 10, /* wrong value may damage h/w */
+		},
+		[1] = {
+			.name = "green",
+			.led_id = 1,
+			.status = AB5500_LED_ON,
+			.max_current = 10, /* wrong value may damage h/w */
+		},
+		[2] {
+			.name = "blue",
+			.led_id = 2,
+			.status = AB5500_LED_ON,
+			.max_current = 10, /* wrong value may damage h/w */
+		},
+	},
+};
+
+/*
+ * I2C
+ */
+
+#define U5500_I2C_CONTROLLER(id, _slsu, _tft, _rft, clk, t_out, _sm) \
+static struct nmk_i2c_controller u5500_i2c##id##_data = { \
+	/*				\
+	 * slave data setup time, which is	\
+	 * 250 ns,100ns,10ns which is 14,6,2	\
+	 * respectively for a 48 Mhz	\
+	 * i2c clock			\
+	 */				\
+	.slsu		= _slsu,	\
+	/* Tx FIFO threshold */		\
+	.tft		= _tft,		\
+	/* Rx FIFO threshold */		\
+	.rft		= _rft,		\
+	/* std. mode operation */	\
+	.clk_freq	= clk,		\
+	/* Slave response timeout(ms) */\
+	.timeout	= t_out,	\
+	.sm		= _sm,		\
+}
+
+/*
+ * The board uses 3 i2c controllers, initialize all of
+ * them with slave data setup time of 250 ns,
+ * Tx & Rx FIFO threshold values as 1 and standard
+ * mode of operation
+ */
+
+U5500_I2C_CONTROLLER(1,	0xe, 1, 10, 400000, 200, I2C_FREQ_MODE_FAST);
+U5500_I2C_CONTROLLER(2,	0xe, 1, 10, 400000, 200, I2C_FREQ_MODE_FAST);
+U5500_I2C_CONTROLLER(3,	0xe, 1, 10, 400000, 200, I2C_FREQ_MODE_FAST);
+
+static struct i2c_board_info __initdata u5500_i2c1_devices[] = {
+	{
+		I2C_BOARD_INFO("synaptics_rmi4_i2c", 0x4B),
+		.platform_data = &rmi4_i2c_platformdata,
+	},
+};
+
+static struct i2c_board_info __initdata u5500_i2c2_devices[] = {
+	{
+		/* LSM303DLH Accelerometer */
+		I2C_BOARD_INFO("lsm303dlh_a", 0x19),
+		.platform_data = &lsm303dlh_pdata,
+	},
+	{
+		/* LSM303DLH Magnetometer */
+		I2C_BOARD_INFO("lsm303dlh_m", 0x1E),
+		.platform_data = &lsm303dlh_pdata,
+	},
+	{
+		/* Backlight */
+		I2C_BOARD_INFO("lm3530-led", 0x36),
+		.platform_data = &u5500_als_platform_data,
+	},
+	{
+		I2C_BOARD_INFO("av8100", 0x70),
+		.platform_data = &av8100_plat_data,
+	},
+};
+
+/*
+ * Keypad
+ */
+
+static const unsigned int u5500_keymap[] = {
+	KEY(4, 0, KEY_CAMERA), /* Camera2 */
+	KEY(4, 1, KEY_CAMERA_FOCUS), /* Camera1 */
+	KEY(4, 2, KEY_MENU),
+	KEY(4, 3, KEY_BACK),
+	KEY(5, 2, KEY_SEND),
+	KEY(5, 3, KEY_HOME),
+#ifndef CONFIG_INPUT_AB8500_PONKEY
+	/* AB5500 ONSWa is also hooked up to this key */
+	KEY(8, 0, KEY_END),
+#endif
+	KEY(8, 1, KEY_VOLUMEUP),
+	KEY(8, 2, KEY_VOLUMEDOWN),
+};
+
+static struct matrix_keymap_data u5500_keymap_data = {
+	.keymap		= u5500_keymap,
+	.keymap_size	= ARRAY_SIZE(u5500_keymap),
+};
+
+static struct db5500_keypad_platform_data u5500_keypad_board = {
+	.keymap_data	= &u5500_keymap_data,
+	.no_autorepeat	= true,
+	.debounce_ms	= 40, /* milliseconds */
+};
+
+/*
+ * MSP
+ */
+
+#define MSP_DMA(num, eventline)					\
+static struct stedma40_chan_cfg msp##num##_dma_rx = {		\
+	.high_priority = true,					\
+	.dir = STEDMA40_PERIPH_TO_MEM,				\
+	.src_dev_type = eventline##_RX,				\
+	.dst_dev_type = STEDMA40_DEV_DST_MEMORY,		\
+	.src_info.psize = STEDMA40_PSIZE_LOG_4,			\
+	.dst_info.psize = STEDMA40_PSIZE_LOG_4,			\
+};								\
+								\
+static struct stedma40_chan_cfg msp##num##_dma_tx = {		\
+	.high_priority = true,					\
+	.dir = STEDMA40_MEM_TO_PERIPH,				\
+	.src_dev_type = STEDMA40_DEV_SRC_MEMORY,		\
+	.dst_dev_type = eventline##_TX,				\
+	.src_info.psize = STEDMA40_PSIZE_LOG_4,			\
+	.dst_info.psize = STEDMA40_PSIZE_LOG_4,			\
+}
+
+MSP_DMA(0, DB5500_DMA_DEV9_MSP0);
+MSP_DMA(1, DB5500_DMA_DEV10_MSP1);
+MSP_DMA(2, DB5500_DMA_DEV11_MSP2);
+
+static struct msp_i2s_platform_data u5500_msp0_data = {
+	.id		= MSP_0_I2S_CONTROLLER,
+	.msp_i2s_dma_rx	= &msp0_dma_rx,
+	.msp_i2s_dma_tx	= &msp0_dma_tx,
+};
+
+static struct msp_i2s_platform_data u5500_msp1_data = {
+	.id		= MSP_1_I2S_CONTROLLER,
+	.msp_i2s_dma_rx	= &msp1_dma_rx,
+	.msp_i2s_dma_tx	= &msp1_dma_tx,
+};
+
+static struct msp_i2s_platform_data u5500_msp2_data = {
+	.id		= MSP_2_I2S_CONTROLLER,
+	.msp_i2s_dma_rx	= &msp2_dma_rx,
+	.msp_i2s_dma_tx	= &msp2_dma_tx,
+};
+
+static struct i2s_board_info stm_i2s_board_info[] __initdata = {
+	{
+		.modalias	= "i2s_device.0",
+		.id		= 0,
+		.chip_select	= 0,
+	},
+	{
+		.modalias	= "i2s_device.1",
+		.id		= 1,
+		.chip_select	= 1,
+	},
+	{
+		.modalias	= "i2s_device.2",
+		.id		= 2,
+		.chip_select	= 2,
+	},
+};
+
+static void __init u5500_msp_init(void)
+{
+	db5500_add_msp0_i2s(&u5500_msp0_data);
+	db5500_add_msp1_i2s(&u5500_msp1_data);
+	db5500_add_msp2_i2s(&u5500_msp2_data);
+
+	i2s_register_board_info(ARRAY_AND_SIZE(stm_i2s_board_info));
+}
+
+/*
+ * SPI
+ */
+
+static struct pl022_ssp_controller u5500_spi3_data = {
+	.bus_id		= 1,
+	.num_chipselect	= 4,	/* 3 possible CS lines + 1 for tests */
+};
+
+static void __init u5500_spi_init(void)
+{
+	db5500_add_spi3(&u5500_spi3_data);
+}
+
+static struct resource ab5500_resources[] = {
+	[0] = {
+		.start = IRQ_DB5500_PRCMU_ABB,
+		.end = IRQ_DB5500_PRCMU_ABB,
+		.flags = IORESOURCE_IRQ
+	}
+};
+
+static struct ab5500_platform_data ab5500_plf_data = {
+	.irq = {
+		.base = IRQ_AB5500_BASE,
+		.count = AB5500_NR_IRQS,
+	},
+	.pm_power_off	= true,
+	.regulator	= &u5500_ab5500_regulator_data,
+	.dev_data[AB5500_DEVID_LEDS] = &ab5500_hvleds_data,
+	.dev_data_sz[AB5500_DEVID_LEDS] = sizeof(ab5500_hvleds_data),
+	.init_settings = (struct abx500_init_settings[]){
+			{
+				.bank = 0x3,
+				.reg = 0x17,
+				.setting = 0x0F,
+			},
+			{
+				.bank = 0x3,
+				.reg = 0x18,
+				.setting = 0x10,
+			},
+	},
+	.init_settings_sz = 2,
+#if defined(CONFIG_AB5500_BM)
+	.dev_data[AB5500_DEVID_CHARGALG] = &abx500_bm_pt_data,
+	.dev_data_sz[AB5500_DEVID_CHARGALG] = sizeof(abx500_bm_pt_data),
+	.dev_data[AB5500_DEVID_CHARGER] = &abx500_bm_pt_data,
+	.dev_data_sz[AB5500_DEVID_CHARGER] = sizeof(abx500_bm_pt_data),
+	.dev_data[AB5500_DEVID_FG] = &abx500_bm_pt_data,
+	.dev_data_sz[AB5500_DEVID_FG] = sizeof(abx500_bm_pt_data),
+	.dev_data[AB5500_DEVID_BTEMP] = &abx500_bm_pt_data,
+	.dev_data_sz[AB5500_DEVID_BTEMP] = sizeof(abx500_bm_pt_data),
+#endif
+};
+
+static struct platform_device u5500_ab5500_device = {
+	.name = "ab5500-core",
+	.id = 0,
+	.dev = {
+		.platform_data = &ab5500_plf_data,
+	},
+	.num_resources = 1,
+	.resource = ab5500_resources,
+};
+
+static struct platform_device u5500_mloader_device = {
+	.name = "db5500_mloader",
+	.id = -1,
+	.num_resources = 0,
+};
+
+static struct platform_device *u5500_platform_devices[] __initdata = {
+	&u5500_ab5500_device,
+	&u5500_mcde_device,
+	&ux500_hwmem_device,
+	&u5500_b2r2_device,
+	&u5500_mloader_device,
+#ifdef CONFIG_U5500_MMIO
+	&u5500_mmio_device,
+#endif
+};
+
+static void __init u5500_i2c_init(void)
+{
+	db5500_add_i2c1(&u5500_i2c1_data);
+	db5500_add_i2c2(&u5500_i2c2_data);
+	db5500_add_i2c3(&u5500_i2c3_data);
+
+	i2c_register_board_info(1, ARRAY_AND_SIZE(u5500_i2c1_devices));
+	i2c_register_board_info(2, ARRAY_AND_SIZE(u5500_i2c2_devices));
+}
 
 static void __init u5500_uart_init(void)
 {
@@ -29,10 +392,21 @@ static void __init u5500_uart_init(void)
 
 static void __init u5500_init_machine(void)
 {
+	u5500_regulators_init();
 	u5500_init_devices();
+	u5500_pins_init();
+
+	u5500_i2c_init();
+	u5500_msp_init();
+	u5500_spi_init();
 
 	u5500_sdi_init();
 	u5500_uart_init();
+
+	db5500_add_keypad(&u5500_keypad_board);
+
+	platform_add_devices(u5500_platform_devices,
+			     ARRAY_SIZE(u5500_platform_devices));
 }
 
 MACHINE_START(U5500, "ST-Ericsson U5500 Platform")
@@ -42,3 +416,11 @@ MACHINE_START(U5500, "ST-Ericsson U5500 Platform")
 	.timer		= &ux500_timer,
 	.init_machine	= u5500_init_machine,
 MACHINE_END
+
+MACHINE_START(B5500, "ST-Ericsson U5500 Big Board")
+	.boot_params	= 0x00000100,
+	.map_io		= u5500_map_io,
+	.init_irq	= ux500_init_irq,
+	.timer		= &ux500_timer,
+	.init_machine	= u5500_init_machine,
+MACHINE_END
diff --git a/arch/arm/mach-ux500/board-u5500.h b/arch/arm/mach-ux500/board-u5500.h
new file mode 100644
index 00000000000..dda59f39bdf
--- /dev/null
+++ b/arch/arm/mach-ux500/board-u5500.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __BOARD_U5500_H
+#define __BOARD_U5500_H
+
+#define GPIO_SDMMC_CD          180
+#define GPIO_MMC_CARD_CTRL     227
+#define GPIO_MMC_CARD_VSEL     185
+#define GPIO_PRIMARY_CAM_XSHUTDOWN  1
+#define GPIO_SECONDARY_CAM_XSHUTDOWN  2
+
+
+struct ab5500_regulator_platform_data;
+extern struct ab5500_regulator_platform_data u5500_ab5500_regulator_data;
+
+extern void u5500_pins_init(void);
+extern void __init u5500_regulators_init(void);
+
+#endif
diff --git a/arch/arm/mach-ux500/clock-db5500.c b/arch/arm/mach-ux500/clock-db5500.c
new file mode 100644
index 00000000000..37e52e704ff
--- /dev/null
+++ b/arch/arm/mach-ux500/clock-db5500.c
@@ -0,0 +1,762 @@
+/*
+ *  Copyright (C) 2009 ST-Ericsson SA
+ *  Copyright (C) 2009 STMicroelectronics
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/errno.h>
+#include <linux/mutex.h>
+#include <linux/debugfs.h>
+#include <linux/module.h>
+#include <linux/gpio/nomadik.h>
+#include <linux/mfd/ab8500/sysctrl.h>
+#include <linux/workqueue.h>
+#include <linux/regulator/consumer.h>
+#include <linux/mfd/abx500.h>
+
+#include <plat/pincfg.h>
+
+#include <mach/hardware.h>
+#include <mach/prcmu.h>
+#include <mach/prcmu-regs.h>
+
+#include "clock.h"
+#include "pins-db5500.h"
+
+static DEFINE_MUTEX(sysclk_mutex);
+static DEFINE_MUTEX(pll_mutex);
+static DEFINE_MUTEX(ab_ulpclk_mutex);
+static DEFINE_MUTEX(audioclk_mutex);
+
+/* SysClk operations. */
+static int sysclk_enable(struct clk *clk)
+{
+	return prcmu_request_clock(PRCMU_SYSCLK, true);
+}
+
+static void sysclk_disable(struct clk *clk)
+{
+
+	prcmu_request_clock(PRCMU_SYSCLK, false);
+	return;
+}
+
+static struct clkops sysclk_ops = {
+	.enable = sysclk_enable,
+	.disable = sysclk_disable,
+};
+
+/* AB8500 UlpClk operations */
+
+static int ab_ulpclk_enable(struct clk *clk)
+{
+	int err;
+
+	if (clk->regulator == NULL) {
+		struct regulator *reg;
+
+		reg = regulator_get(NULL, "v-intcore");
+		if (IS_ERR(reg))
+			return PTR_ERR(reg);
+		clk->regulator = reg;
+	}
+	err = regulator_enable(clk->regulator);
+	if (err)
+		return err;
+	err = ab8500_sysctrl_clear(AB8500_SYSULPCLKCONF,
+		AB8500_SYSULPCLKCONF_ULPCLKCONF_MASK);
+	if (err)
+		return err;
+	return ab8500_sysctrl_set(AB8500_SYSULPCLKCTRL1,
+		AB8500_SYSULPCLKCTRL1_ULPCLKREQ);
+}
+
+static void ab_ulpclk_disable(struct clk *clk)
+{
+	if (ab8500_sysctrl_clear(AB8500_SYSULPCLKCTRL1,
+		AB8500_SYSULPCLKCTRL1_ULPCLKREQ))
+		goto out_err;
+	if (clk->regulator != NULL) {
+		if (regulator_disable(clk->regulator))
+			goto out_err;
+	}
+	return;
+
+out_err:
+	pr_err("clock: %s failed to disable %s.\n", __func__, clk->name);
+}
+
+static struct clkops ab_ulpclk_ops = {
+	.enable = ab_ulpclk_enable,
+	.disable = ab_ulpclk_disable,
+};
+
+/* AB8500 audio clock operations */
+
+static int audioclk_enable(struct clk *clk)
+{
+	return ab8500_sysctrl_set(AB8500_SYSULPCLKCTRL1,
+		AB8500_SYSULPCLKCTRL1_AUDIOCLKENA);
+}
+
+static void audioclk_disable(struct clk *clk)
+{
+	if (ab8500_sysctrl_clear(AB8500_SYSULPCLKCTRL1,
+		AB8500_SYSULPCLKCTRL1_AUDIOCLKENA)) {
+		pr_err("clock: %s failed to disable %s.\n", __func__,
+			clk->name);
+	}
+}
+
+static int audioclk_set_parent(struct clk *clk, struct clk *parent)
+{
+	if (parent->ops == &sysclk_ops) {
+		return ab8500_sysctrl_clear(AB8500_SYSULPCLKCTRL1,
+			AB8500_SYSULPCLKCTRL1_SYSULPCLKINTSEL_MASK);
+	} else if (parent->ops == &ab_ulpclk_ops) {
+		return ab8500_sysctrl_write(AB8500_SYSULPCLKCTRL1,
+			AB8500_SYSULPCLKCTRL1_SYSULPCLKINTSEL_MASK,
+			(1 << AB8500_SYSULPCLKCTRL1_SYSULPCLKINTSEL_SHIFT));
+	} else {
+		return -EINVAL;
+	}
+}
+
+static struct clkops audioclk_ops = {
+	.enable = audioclk_enable,
+	.disable = audioclk_disable,
+	.set_parent = audioclk_set_parent,
+};
+
+static int rtc_clk_enable(struct clk *clk)
+{
+	return ab5500_clock_rtc_enable(clk->cg_sel, true);
+}
+
+static void rtc_clk_disable(struct clk *clk)
+{
+	int ret = ab5500_clock_rtc_enable(clk->cg_sel, false);
+
+	if (ret)
+		pr_err("clock: %s failed to disable: %d\n", clk->name, ret);
+}
+
+static struct clkops rtc_clk_ops = {
+	.enable		= rtc_clk_enable,
+	.disable	= rtc_clk_disable,
+};
+
+static pin_cfg_t clkout0_pins[] = {
+	GPIO161_CLKOUT_0 | PIN_OUTPUT_LOW,
+};
+
+static pin_cfg_t clkout1_pins[] = {
+	GPIO162_CLKOUT_1 | PIN_OUTPUT_LOW,
+};
+
+static int clkout0_enable(struct clk *clk)
+{
+	unsigned int val = readl(PRCM_CLKOCR);
+
+	val &= ~PRCM_CLKOCR_CLKOUT0_MASK;
+	val |= PRCM_CLKOCR_CLKOUT0_REF_CLK;
+
+	writel(val, PRCM_CLKOCR);
+
+	return nmk_config_pins(clkout0_pins, ARRAY_SIZE(clkout0_pins));
+}
+
+static void clkout0_disable(struct clk *clk)
+{
+	int r;
+
+	r = nmk_config_pins_sleep(clkout0_pins, ARRAY_SIZE(clkout0_pins));
+	if (!r)
+		return;
+
+	pr_err("clock: failed to disable %s.\n", clk->name);
+}
+
+static int clkout1_enable(struct clk *clk)
+{
+	unsigned int val = readl(PRCM_CLKOCR);
+
+	val &= ~PRCM_CLKOCR_CLKOUT1_MASK;
+	val |= PRCM_CLKOCR_CLKOUT1_REF_CLK;
+
+	writel(val, PRCM_CLKOCR);
+
+	return nmk_config_pins(clkout1_pins, ARRAY_SIZE(clkout0_pins));
+}
+
+static void clkout1_disable(struct clk *clk)
+{
+	int r;
+
+	r = nmk_config_pins_sleep(clkout1_pins, ARRAY_SIZE(clkout1_pins));
+	if (!r)
+		return;
+
+	pr_err("clock: failed to disable %s.\n", clk->name);
+}
+
+static struct clkops clkout0_ops = {
+	.enable = clkout0_enable,
+	.disable = clkout0_disable,
+};
+
+static struct clkops clkout1_ops = {
+	.enable = clkout1_enable,
+	.disable = clkout1_disable,
+};
+
+#define DEF_PER1_PCLK(_cg_bit, _name) \
+	DEF_PRCC_PCLK(_name, U5500_CLKRST1_BASE, _cg_bit, &per1clk)
+#define DEF_PER2_PCLK(_cg_bit, _name) \
+	DEF_PRCC_PCLK(_name, U5500_CLKRST2_BASE, _cg_bit, &per2clk)
+#define DEF_PER3_PCLK(_cg_bit, _name) \
+	DEF_PRCC_PCLK(_name, U5500_CLKRST3_BASE, _cg_bit, &per3clk)
+#define DEF_PER5_PCLK(_cg_bit, _name) \
+	DEF_PRCC_PCLK(_name, U5500_CLKRST5_BASE, _cg_bit, &per5clk)
+#define DEF_PER6_PCLK(_cg_bit, _name) \
+	DEF_PRCC_PCLK(_name, U5500_CLKRST6_BASE, _cg_bit, &per6clk)
+
+#define DEF_PER1_KCLK(_cg_bit, _name, _parent) \
+	DEF_PRCC_KCLK(_name, U5500_CLKRST1_BASE, _cg_bit, _parent, &per1clk)
+#define DEF_PER2_KCLK(_cg_bit, _name, _parent) \
+	DEF_PRCC_KCLK(_name, U5500_CLKRST2_BASE, _cg_bit, _parent, &per2clk)
+#define DEF_PER3_KCLK(_cg_bit, _name, _parent) \
+	DEF_PRCC_KCLK(_name, U5500_CLKRST3_BASE, _cg_bit, _parent, &per3clk)
+#define DEF_PER5_KCLK(_cg_bit, _name, _parent) \
+	DEF_PRCC_KCLK(_name, U5500_CLKRST5_BASE, _cg_bit, _parent, &per5clk)
+#define DEF_PER6_KCLK(_cg_bit, _name, _parent) \
+	DEF_PRCC_KCLK(_name, U5500_CLKRST6_BASE, _cg_bit, _parent, &per6clk)
+
+#define DEF_MTU_CLK(_cg_sel, _name, _bus_parent) \
+	struct clk _name = { \
+		.name = #_name, \
+		.ops = &mtu_clk_ops, \
+		.cg_sel = _cg_sel, \
+		.bus_parent = _bus_parent, \
+	}
+
+/* Clock sources. */
+
+static struct clk soc0_pll = {
+	.name = "soc0_pll",
+	.ops = &prcmu_clk_ops,
+	.mutex = &pll_mutex,
+	.cg_sel = PRCMU_PLLSOC0,
+};
+
+static struct clk soc1_pll = {
+	.name = "soc1_pll",
+	.ops = &prcmu_clk_ops,
+	.mutex = &pll_mutex,
+	.cg_sel = PRCMU_PLLSOC1,
+};
+
+static struct clk ddr_pll = {
+	.name = "ddr_pll",
+	.ops = &prcmu_clk_ops,
+	.mutex = &pll_mutex,
+	.cg_sel = PRCMU_PLLDDR,
+};
+
+static struct clk ulp38m4 = {
+	.name = "ulp38m4",
+};
+
+static struct clk sysclk = {
+	.name = "sysclk",
+	.ops = &sysclk_ops,
+	.rate = 26000000,
+	.mutex = &sysclk_mutex,
+};
+
+static struct clk rtc32k = {
+	.name = "rtc32k",
+	.rate = 32768,
+};
+
+static struct clk kbd32k = {
+	.name = "kbd32k",
+	.rate = 32768,
+};
+
+static struct clk clk_dummy = {
+	.name = "dummy",
+};
+
+static struct clk clk_msp1 = {
+	.name = "msp1",
+	.rate = 26000000,
+};
+
+static struct clk rtc_clk1 = {
+	.name	= "rtc_clk1",
+	.ops	= &rtc_clk_ops,
+	.cg_sel	= 1,
+	.mutex = &sysclk_mutex,
+};
+
+static struct clk clkout0 = {
+	.name = "clkout0",
+	.ops = &clkout0_ops,
+	.parent = &sysclk,
+	.mutex = &sysclk_mutex,
+};
+
+static struct clk clkout1 = {
+	.name = "clkout1",
+	.ops = &clkout1_ops,
+	.parent = &sysclk,
+	.mutex = &sysclk_mutex,
+};
+
+static struct clk ab_ulpclk = {
+	.name = "ab_ulpclk",
+	.ops = &ab_ulpclk_ops,
+	.rate = 38400000,
+	.mutex = &ab_ulpclk_mutex,
+};
+
+static struct clk *audioclk_parents[] = { &sysclk, &ab_ulpclk, NULL };
+
+static struct clk audioclk = {
+	.name = "audioclk",
+	.ops = &audioclk_ops,
+	.mutex = &audioclk_mutex,
+	.parent = &sysclk,
+	.parents = audioclk_parents,
+};
+
+static DEF_PRCMU_CLK(dmaclk, PRCMU_DMACLK, 200000000);
+static DEF_PRCMU_CLK(b2r2clk, PRCMU_B2R2CLK, 200000000);
+static DEF_PRCMU_CLK(sgaclk, PRCMU_SGACLK, 199900000);
+static DEF_PRCMU_CLK(uartclk, PRCMU_UARTCLK, 36360000);
+static DEF_PRCMU_CLK(msp02clk, PRCMU_MSP02CLK, 13000000);
+static DEF_PRCMU_CLK(i2cclk, PRCMU_I2CCLK, 24000000);
+static DEF_PRCMU_CLK(irdaclk, PRCMU_IRDACLK, 48000000);
+static DEF_PRCMU_CLK(irrcclk, PRCMU_IRRCCLK, 48000000);
+static DEF_PRCMU_CLK(rngclk, PRCMU_RNGCLK, 26000000);
+static DEF_PRCMU_CLK(pwmclk, PRCMU_PWMCLK, 26000000);
+static DEF_PRCMU_CLK(sdmmcclk, PRCMU_SDMMCCLK, 100000000);
+static DEF_PRCMU_CLK(per1clk, PRCMU_PER1CLK, 133330000);
+static DEF_PRCMU_CLK(per2clk, PRCMU_PER2CLK, 133330000);
+static DEF_PRCMU_CLK(per3clk, PRCMU_PER3CLK, 133330000);
+static DEF_PRCMU_CLK(per5clk, PRCMU_PER5CLK, 133330000);
+static DEF_PRCMU_CLK(per6clk, PRCMU_PER6CLK, 133330000);
+static DEF_PRCMU_CLK(hdmiclk, PRCMU_HDMICLK, 26000000);
+static DEF_PRCMU_CLK(apeatclk, PRCMU_APEATCLK, 200000000);
+static DEF_PRCMU_CLK(apetraceclk, PRCMU_APETRACECLK, 266000000);
+static DEF_PRCMU_CLK(mcdeclk, PRCMU_MCDECLK, 160000000);
+static DEF_PRCMU_CLK(tvclk, PRCMU_TVCLK, 40000000);
+static DEF_PRCMU_CLK(dsialtclk, PRCMU_DSIALTCLK, 400000000);
+static DEF_PRCMU_CLK(timclk, PRCMU_TIMCLK, 3250000);
+static DEF_PRCMU_CLK(svaclk, PRCMU_SVACLK, 156000000);
+static DEF_PRCMU_CLK(siaclk, PRCMU_SIACLK, 133330000);
+
+/* PRCC PClocks */
+
+static DEF_PER1_PCLK(0, p1_pclk0);
+static DEF_PER1_PCLK(1, p1_pclk1);
+static DEF_PER1_PCLK(2, p1_pclk2);
+static DEF_PER1_PCLK(3, p1_pclk3);
+static DEF_PER1_PCLK(4, p1_pclk4);
+static DEF_PER1_PCLK(5, p1_pclk5);
+static DEF_PER1_PCLK(6, p1_pclk6);
+
+static DEF_PER2_PCLK(0, p2_pclk0);
+static DEF_PER2_PCLK(1, p2_pclk1);
+
+static DEF_PER3_PCLK(0, p3_pclk0);
+static DEF_PER3_PCLK(1, p3_pclk1);
+static DEF_PER3_PCLK(2, p3_pclk2);
+
+static DEF_PER5_PCLK(0, p5_pclk0);
+static DEF_PER5_PCLK(1, p5_pclk1);
+static DEF_PER5_PCLK(2, p5_pclk2);
+static DEF_PER5_PCLK(3, p5_pclk3);
+static DEF_PER5_PCLK(4, p5_pclk4);
+static DEF_PER5_PCLK(5, p5_pclk5);
+static DEF_PER5_PCLK(6, p5_pclk6);
+static DEF_PER5_PCLK(7, p5_pclk7);
+static DEF_PER5_PCLK(8, p5_pclk8);
+static DEF_PER5_PCLK(9, p5_pclk9);
+static DEF_PER5_PCLK(10, p5_pclk10);
+static DEF_PER5_PCLK(11, p5_pclk11);
+static DEF_PER5_PCLK(12, p5_pclk12);
+static DEF_PER5_PCLK(13, p5_pclk13);
+static DEF_PER5_PCLK(14, p5_pclk14);
+static DEF_PER5_PCLK(15, p5_pclk15);
+
+static DEF_PER6_PCLK(0, p6_pclk0);
+static DEF_PER6_PCLK(1, p6_pclk1);
+static DEF_PER6_PCLK(2, p6_pclk2);
+static DEF_PER6_PCLK(3, p6_pclk3);
+static DEF_PER6_PCLK(4, p6_pclk4);
+static DEF_PER6_PCLK(5, p6_pclk5);
+static DEF_PER6_PCLK(6, p6_pclk6);
+static DEF_PER6_PCLK(7, p6_pclk7);
+
+/* MSP0 */
+static DEF_PER1_KCLK(0, p1_msp0_kclk, &msp02clk);
+static DEF_PER_CLK(p1_msp0_clk, &p1_pclk0, &p1_msp0_kclk);
+
+/* SDI0 */
+static DEF_PER1_KCLK(1, p1_sdi0_kclk, &sdmmcclk);
+static DEF_PER_CLK(p1_sdi0_clk, &p1_pclk1, &p1_sdi0_kclk);
+
+/* SDI2 */
+static DEF_PER1_KCLK(2, p1_sdi2_kclk, &sdmmcclk);
+static DEF_PER_CLK(p1_sdi2_clk, &p1_pclk2, &p1_sdi2_kclk);
+
+/* UART0 */
+static DEF_PER1_KCLK(3, p1_uart0_kclk, &uartclk);
+static DEF_PER_CLK(p1_uart0_clk, &p1_pclk3, &p1_uart0_kclk);
+
+/* I2C1 */
+static DEF_PER1_KCLK(4, p1_i2c1_kclk, &i2cclk);
+static DEF_PER_CLK(p1_i2c1_clk, &p1_pclk4, &p1_i2c1_kclk);
+
+/* PWM */
+static DEF_PER3_KCLK(0, p3_pwm_kclk, &pwmclk);
+static DEF_PER_CLK(p3_pwm_clk, &p3_pclk1, &p3_pwm_kclk);
+
+/* KEYPAD */
+static DEF_PER3_KCLK(0, p3_keypad_kclk, &kbd32k);
+static DEF_PER_CLK(p3_keypad_clk, &p3_pclk0, &p3_keypad_kclk);
+
+/* MSP2 */
+static DEF_PER5_KCLK(0, p5_msp2_kclk, &msp02clk);
+static DEF_PER_CLK(p5_msp2_clk, &p5_pclk0, &p5_msp2_kclk);
+
+/* UART1 */
+static DEF_PER5_KCLK(1, p5_uart1_kclk, &uartclk);
+static DEF_PER_CLK(p5_uart1_clk, &p5_pclk1, &p5_uart1_kclk);
+
+/* UART2 */
+static DEF_PER5_KCLK(2, p5_uart2_kclk, &uartclk);
+static DEF_PER_CLK(p5_uart2_clk, &p5_pclk2, &p5_uart2_kclk);
+
+/* UART3 */
+static DEF_PER5_KCLK(3, p5_uart3_kclk, &uartclk);
+static DEF_PER_CLK(p5_uart3_clk, &p5_pclk3, &p5_uart3_kclk);
+
+/* SDI1 */
+static DEF_PER5_KCLK(4, p5_sdi1_kclk, &sdmmcclk);
+static DEF_PER_CLK(p5_sdi1_clk, &p5_pclk4, &p5_sdi1_kclk);
+
+/* SDI3 */
+static DEF_PER5_KCLK(5, p5_sdi3_kclk, &sdmmcclk);
+static DEF_PER_CLK(p5_sdi3_clk, &p5_pclk5, &p5_sdi3_kclk);
+
+/* SDI4 */
+static DEF_PER5_KCLK(6, p5_sdi4_kclk, &sdmmcclk);
+static DEF_PER_CLK(p5_sdi4_clk, &p5_pclk6, &p5_sdi4_kclk);
+
+/* I2C2 */
+static DEF_PER5_KCLK(7, p5_i2c2_kclk, &i2cclk);
+static DEF_PER_CLK(p5_i2c2_clk, &p5_pclk7, &p5_i2c2_kclk);
+
+/* I2C3 */
+static DEF_PER5_KCLK(8, p5_i2c3_kclk, &i2cclk);
+static DEF_PER_CLK(p5_i2c3_clk, &p5_pclk8, &p5_i2c3_kclk);
+
+/* IRRC */
+static DEF_PER5_KCLK(9, p5_irrc_kclk, &irrcclk);
+static DEF_PER_CLK(p5_irrc_clk, &p5_pclk9, &p5_irrc_kclk);
+
+/* IRDA */
+static DEF_PER5_KCLK(10, p5_irda_kclk, &irdaclk);
+static DEF_PER_CLK(p5_irda_clk, &p5_pclk10, &p5_irda_kclk);
+
+/* RNG */
+static DEF_PER6_KCLK(0, p6_rng_kclk, &rngclk);
+static DEF_PER_CLK(p6_rng_clk, &p6_pclk0, &p6_rng_kclk);
+
+/* MTU:S */
+
+/* MTU0 */
+static DEF_PER_CLK(p6_mtu0_clk, &p6_pclk6, &timclk);
+
+/* MTU1 */
+static DEF_PER_CLK(p6_mtu1_clk, &p6_pclk7, &timclk);
+
+static struct clk *db5500_dbg_clks[] __initdata = {
+	/* Clock sources */
+	&soc0_pll,
+	&soc1_pll,
+	&ddr_pll,
+	&ulp38m4,
+	&sysclk,
+	&rtc32k,
+
+	/* PRCMU clocks */
+	&sgaclk,
+	&siaclk,
+	&svaclk,
+	&uartclk,
+	&msp02clk,
+	&i2cclk,
+	&irdaclk,
+	&irrcclk,
+	&sdmmcclk,
+	&per1clk,
+	&per2clk,
+	&per3clk,
+	&per5clk,
+	&per6clk,
+	&hdmiclk,
+	&apeatclk,
+	&apetraceclk,
+	&mcdeclk,
+	&dsialtclk,
+	&dmaclk,
+	&b2r2clk,
+	&tvclk,
+	&rngclk,
+	&pwmclk,
+
+	/* PRCC clocks */
+	&p1_pclk0,
+	&p1_pclk1,
+	&p1_pclk2,
+	&p1_pclk3,
+	&p1_pclk4,
+	&p1_pclk5,
+	&p1_pclk6,
+
+	&p2_pclk0,
+	&p2_pclk1,
+
+	&p3_pclk0,
+	&p3_pclk1,
+	&p3_pclk2,
+
+	&p5_pclk0,
+	&p5_pclk1,
+	&p5_pclk2,
+	&p5_pclk3,
+	&p5_pclk4,
+	&p5_pclk5,
+	&p5_pclk6,
+	&p5_pclk7,
+	&p5_pclk8,
+	&p5_pclk9,
+	&p5_pclk10,
+	&p5_pclk11,
+	&p5_pclk12,
+	&p5_pclk13,
+	&p5_pclk14,
+	&p5_pclk15,
+
+	&p6_pclk0,
+	&p6_pclk1,
+	&p6_pclk2,
+	&p6_pclk3,
+	&p6_pclk4,
+	&p6_pclk5,
+	&p6_pclk6,
+	&p6_pclk7,
+
+	/* Clock sources */
+	&clkout0,
+	&clkout1,
+	&rtc_clk1,
+};
+
+#define CLK_LOOKUP(_clk, _dev_id, _con_id) \
+	{ .dev_id = _dev_id, .con_id = _con_id, .clk = &_clk }
+
+static struct clk_lookup u8500_common_clock_sources[] = {
+	CLK_LOOKUP(soc0_pll, NULL, "soc0_pll"),
+	CLK_LOOKUP(soc1_pll, NULL, "soc1_pll"),
+	CLK_LOOKUP(ddr_pll, NULL, "ddr_pll"),
+	CLK_LOOKUP(ulp38m4, NULL, "ulp38m4"),
+	CLK_LOOKUP(sysclk, NULL, "sysclk"),
+	CLK_LOOKUP(rtc32k, NULL, "clk32k"),
+	CLK_LOOKUP(sysclk, "ab8500-usb.0", "sysclk"),
+	CLK_LOOKUP(sysclk, "ab8500-codec.0", "sysclk"),
+	CLK_LOOKUP(ab_ulpclk, "ab8500-codec.0", "ulpclk"),
+	CLK_LOOKUP(audioclk, "ab8500-codec.0", "audioclk"),
+};
+
+static struct clk_lookup db5500_prcmu_clocks[] = {
+	CLK_LOOKUP(sgaclk, "mali", NULL),
+	CLK_LOOKUP(siaclk, "mmio_camera", "sia"),
+	CLK_LOOKUP(svaclk, "hva", NULL),
+	CLK_LOOKUP(uartclk, "UART", NULL),
+	CLK_LOOKUP(msp02clk, "MSP02", NULL),
+	CLK_LOOKUP(i2cclk, "I2C", NULL),
+	CLK_LOOKUP(sdmmcclk, "sdmmc", NULL),
+	CLK_LOOKUP(per1clk, "PERIPH1", NULL),
+	CLK_LOOKUP(per2clk, "PERIPH2", NULL),
+	CLK_LOOKUP(per3clk, "PERIPH3", NULL),
+	CLK_LOOKUP(per5clk, "PERIPH5", NULL),
+	CLK_LOOKUP(per6clk, "PERIPH6", NULL),
+	CLK_LOOKUP(hdmiclk, "mcde", "hdmi"),
+	CLK_LOOKUP(apeatclk, "apeat", NULL),
+	CLK_LOOKUP(apetraceclk, "apetrace", NULL),
+	CLK_LOOKUP(mcdeclk, "mcde", NULL),
+	CLK_LOOKUP(mcdeclk, "mcde", "mcde"),
+	CLK_LOOKUP(dmaclk, "dma40.0", NULL),
+	CLK_LOOKUP(b2r2clk, "b2r2", NULL),
+	CLK_LOOKUP(b2r2clk, "b2r2_bus", NULL),
+	CLK_LOOKUP(b2r2clk, "U8500-B2R2.0", NULL),
+	CLK_LOOKUP(tvclk, "tv", NULL),
+	CLK_LOOKUP(tvclk, "mcde", "tv"),
+};
+
+static struct clk_lookup db5500_prcc_clocks[] = {
+	CLK_LOOKUP(p1_msp0_clk, "MSP_I2S.0", NULL),
+	CLK_LOOKUP(p1_sdi0_clk, "sdi0", NULL),
+	CLK_LOOKUP(p1_sdi2_clk, "sdi2", NULL),
+	CLK_LOOKUP(p1_uart0_clk, "uart0", NULL),
+	CLK_LOOKUP(p1_i2c1_clk, "nmk-i2c.1", NULL),
+	CLK_LOOKUP(p1_pclk5, "gpio.0", NULL),
+	CLK_LOOKUP(p1_pclk5, "gpio.1", NULL),
+	CLK_LOOKUP(p1_pclk6, "fsmc", NULL),
+
+	CLK_LOOKUP(p2_pclk0, "musb_hdrc.0", NULL),
+	CLK_LOOKUP(p2_pclk1, "gpio.2", NULL),
+
+	CLK_LOOKUP(p3_keypad_clk, "db5500-keypad", NULL),
+	CLK_LOOKUP(p3_pwm_clk, "pwm", NULL),
+	CLK_LOOKUP(p3_pclk2, "gpio.4", NULL),
+
+	CLK_LOOKUP(p5_msp2_clk, "MSP_I2S.2", NULL),
+	CLK_LOOKUP(p5_uart1_clk, "uart1", NULL),
+	CLK_LOOKUP(p5_uart2_clk, "uart2", NULL),
+	CLK_LOOKUP(p5_uart3_clk, "uart3", NULL),
+	CLK_LOOKUP(p5_sdi1_clk, "sdi1", NULL),
+	CLK_LOOKUP(p5_sdi3_clk, "sdi3", NULL),
+	CLK_LOOKUP(p5_sdi4_clk, "sdi4", NULL),
+	CLK_LOOKUP(p5_i2c2_clk, "nmk-i2c.2", NULL),
+	CLK_LOOKUP(p5_i2c3_clk, "nmk-i2c.3", NULL),
+	CLK_LOOKUP(p5_irrc_clk, "irrc", NULL),
+	CLK_LOOKUP(p5_irda_clk, "irda", NULL),
+	CLK_LOOKUP(p5_pclk11, "spi0", NULL),
+	CLK_LOOKUP(p5_pclk12, "spi1", NULL),
+	CLK_LOOKUP(p5_pclk13, "spi2", NULL),
+	CLK_LOOKUP(p5_pclk14, "spi3", NULL),
+	CLK_LOOKUP(p5_pclk15, "gpio.5", NULL),
+	CLK_LOOKUP(p5_pclk15, "gpio.6", NULL),
+	CLK_LOOKUP(p5_pclk15, "gpio.7", NULL),
+
+	CLK_LOOKUP(p6_rng_clk, "rng", NULL),
+	CLK_LOOKUP(p6_pclk1, "cryp", NULL),
+	CLK_LOOKUP(p6_pclk2, "hash0", NULL),
+	CLK_LOOKUP(p6_pclk3, "pka", NULL),
+	CLK_LOOKUP(p6_pclk4, "hash1", NULL),
+	CLK_LOOKUP(p6_pclk5, "cfgreg", NULL),
+	CLK_LOOKUP(p6_mtu0_clk, "mtu0", NULL),
+	CLK_LOOKUP(p6_mtu1_clk, "mtu1", NULL),
+
+	/*
+	 * TODO: Clarify whether MSP1 need to be accessed from Linux, and who
+	 * sets up the GPIOs.
+	 */
+	CLK_LOOKUP(clk_dummy, "gpio.3", NULL),
+	CLK_LOOKUP(clk_msp1, "MSP_I2S.1", NULL),
+};
+
+static struct clk_lookup db5500_clkouts[] = {
+	CLK_LOOKUP(clkout1, "mmio_camera", "primary-cam"),
+	CLK_LOOKUP(clkout1, "mmio_camera", "secondary-cam"),
+};
+
+static struct clk_lookup u5500_clocks[] = {
+	CLK_LOOKUP(rtc_clk1, "cg2900-uart.0", "lpoclk"),
+};
+
+static const char *db5500_boot_clk[] __initdata = {
+	"spi0",
+	"spi1",
+	"spi2",
+	"spi3",
+	"uart0",
+	"uart1",
+	"uart2",
+	"uart3",
+	"sdi0",
+	"sdi1",
+	"sdi2",
+	"sdi3",
+	"sdi4",
+};
+
+static struct clk *boot_clks[ARRAY_SIZE(db5500_boot_clk)] __initdata;
+
+static int __init db5500_boot_clk_disable(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(db5500_boot_clk); i++) {
+		clk_disable(boot_clks[i]);
+		clk_put(boot_clks[i]);
+	}
+
+	return 0;
+}
+late_initcall_sync(db5500_boot_clk_disable);
+
+static void __init db5500_boot_clk_enable(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(db5500_boot_clk); i++) {
+		boot_clks[i] = clk_get_sys(db5500_boot_clk[i], NULL);
+		BUG_ON(IS_ERR(boot_clks[i]));
+		clk_enable(boot_clks[i]);
+	}
+}
+
+int __init db5500_clk_init(void)
+{
+	if (ux500_is_svp()) {
+		prcmu_clk_ops.enable = NULL;
+		prcmu_clk_ops.disable = NULL;
+		prcc_pclk_ops.enable = NULL;
+		prcc_pclk_ops.disable = NULL;
+		prcc_kclk_ops.enable = NULL;
+		prcc_kclk_ops.disable = NULL;
+	}
+
+	clks_register(u8500_common_clock_sources,
+		ARRAY_SIZE(u8500_common_clock_sources));
+
+	clks_register(db5500_prcmu_clocks, ARRAY_SIZE(db5500_prcmu_clocks));
+	clks_register(db5500_prcc_clocks, ARRAY_SIZE(db5500_prcc_clocks));
+	clks_register(db5500_clkouts, ARRAY_SIZE(db5500_clkouts));
+	clks_register(u5500_clocks, ARRAY_SIZE(u5500_clocks));
+
+	db5500_boot_clk_enable();
+
+	/*
+	 * The following clks are shared with secure world.
+	 * Currently this leads to a limitation where we need to
+	 * enable them at all times.
+	 */
+	clk_enable(&p6_pclk1);
+	clk_enable(&p6_pclk2);
+	clk_enable(&p6_pclk3);
+	clk_enable(&p6_rng_clk);
+
+	return 0;
+}
+
+static int __init db5500_clk_debug_init(void)
+{
+	return dbx500_clk_debug_init(db5500_dbg_clks,
+				     ARRAY_SIZE(db5500_dbg_clks));
+}
+module_init(db5500_clk_debug_init);
diff --git a/arch/arm/mach-ux500/clock-db8500.c b/arch/arm/mach-ux500/clock-db8500.c
new file mode 100644
index 00000000000..8d4c14db819
--- /dev/null
+++ b/arch/arm/mach-ux500/clock-db8500.c
@@ -0,0 +1,1281 @@
+/*
+ *  Copyright (C) 2009-2011 ST-Ericsson SA
+ *  Copyright (C) 2009 STMicroelectronics
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/errno.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <linux/debugfs.h>
+#include <linux/module.h>
+#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
+#include <linux/mfd/ab8500/sysctrl.h>
+#include <linux/workqueue.h>
+#include <linux/regulator/consumer.h>
+
+#include <asm/mach-types.h>
+#include <plat/pincfg.h>
+
+#include <mach/hardware.h>
+#include <mach/prcmu.h>
+
+#include "clock.h"
+#include "pins-db8500.h"
+
+#define PRCM_SDMMCCLK_MGT	0x024
+#define PRCM_TCR		0x1C8
+#define PRCM_TCR_STOPPED	(1 << 16)
+#define PRCM_TCR_DOZE_MODE	(1 << 17)
+#define SD_CLK_DIV_MASK		0x1F
+#define SD_CLK_DIV_VAL		8
+
+static DEFINE_MUTEX(soc1_pll_mutex);
+static DEFINE_MUTEX(sysclk_mutex);
+static DEFINE_MUTEX(ab_ulpclk_mutex);
+static DEFINE_MUTEX(ab_intclk_mutex);
+
+static struct delayed_work sysclk_disable_work;
+
+/* PLL operations. */
+
+static int clk_pllsrc_enable(struct clk *clk)
+{
+	/* To enable pll */
+	return 0;
+}
+
+static void clk_pllsrc_disable(struct clk *clk)
+{
+	/* To disable pll */
+}
+
+static struct clkops pll_clk_ops = {
+	.enable = clk_pllsrc_enable,
+	.disable = clk_pllsrc_disable,
+};
+
+/* SysClk operations. */
+
+static int request_sysclk(bool enable)
+{
+	static int requests;
+
+	if ((enable && (requests++ == 0)) || (!enable && (--requests == 0)))
+		return prcmu_request_clock(PRCMU_SYSCLK, enable);
+	return 0;
+}
+
+static int sysclk_enable(struct clk *clk)
+{
+	static bool swat_enable;
+	int r;
+
+	if (!swat_enable) {
+		r = ab8500_sysctrl_set(AB8500_SWATCTRL,
+			AB8500_SWATCTRL_SWATENABLE);
+		if (r)
+			return r;
+
+		swat_enable = true;
+	}
+
+	r = request_sysclk(true);
+	if (r)
+		return r;
+
+	if (clk->cg_sel) {
+		r = ab8500_sysctrl_set(AB8500_SYSULPCLKCTRL1, (u8)clk->cg_sel);
+		if (r)
+			(void)request_sysclk(false);
+	}
+	return r;
+}
+
+static void sysclk_disable(struct clk *clk)
+{
+	int r;
+
+	if (clk->cg_sel) {
+		r = ab8500_sysctrl_clear(AB8500_SYSULPCLKCTRL1,
+			(u8)clk->cg_sel);
+		if (r)
+			goto disable_failed;
+	}
+	r = request_sysclk(false);
+	if (r)
+		goto disable_failed;
+	return;
+
+disable_failed:
+	pr_err("clock: failed to disable %s.\n", clk->name);
+}
+
+static struct clkops sysclk_ops = {
+	.enable = sysclk_enable,
+	.disable = sysclk_disable,
+};
+
+/* AB8500 UlpClk operations */
+
+static int ab_ulpclk_enable(struct clk *clk)
+{
+	int err;
+
+	if (clk->regulator == NULL) {
+		struct regulator *reg;
+
+		reg = regulator_get(NULL, "v-intcore");
+		if (IS_ERR(reg))
+			return PTR_ERR(reg);
+		clk->regulator = reg;
+	}
+	err = regulator_set_optimum_mode(clk->regulator, 1500);
+	if (unlikely(err < 0))
+		goto regulator_enable_error;
+	err = regulator_enable(clk->regulator);
+	if (unlikely(err))
+		goto regulator_enable_error;
+	err = ab8500_sysctrl_clear(AB8500_SYSULPCLKCONF,
+		AB8500_SYSULPCLKCONF_ULPCLKCONF_MASK);
+	if (unlikely(err))
+		goto enable_error;
+	err = ab8500_sysctrl_set(AB8500_SYSULPCLKCTRL1,
+		AB8500_SYSULPCLKCTRL1_ULPCLKREQ);
+	if (unlikely(err))
+		goto enable_error;
+	/* Unknown/undocumented PLL locking time => wait 1 ms. */
+	msleep(1);
+	return 0;
+
+enable_error:
+	(void)regulator_disable(clk->regulator);
+regulator_enable_error:
+	return err;
+}
+
+static void ab_ulpclk_disable(struct clk *clk)
+{
+	int err;
+
+	err = ab8500_sysctrl_clear(AB8500_SYSULPCLKCTRL1,
+		AB8500_SYSULPCLKCTRL1_ULPCLKREQ);
+	if (unlikely(regulator_disable(clk->regulator) || err))
+		goto out_err;
+
+	regulator_set_optimum_mode(clk->regulator, 0);
+
+	return;
+
+out_err:
+	pr_err("clock: %s failed to disable %s.\n", __func__, clk->name);
+}
+
+static struct clkops ab_ulpclk_ops = {
+	.enable = ab_ulpclk_enable,
+	.disable = ab_ulpclk_disable,
+};
+
+/* AB8500 intclk operations */
+
+enum ab_intclk_parent {
+	AB_INTCLK_PARENT_SYSCLK,
+	AB_INTCLK_PARENT_ULPCLK,
+	AB_INTCLK_PARENTS_END,
+	NUM_AB_INTCLK_PARENTS
+};
+
+static int ab_intclk_enable(struct clk *clk)
+{
+	if (clk->parent == clk->parents[AB_INTCLK_PARENT_ULPCLK]) {
+		return ab8500_sysctrl_write(AB8500_SYSULPCLKCTRL1,
+			AB8500_SYSULPCLKCTRL1_SYSULPCLKINTSEL_MASK,
+			(1 << AB8500_SYSULPCLKCTRL1_SYSULPCLKINTSEL_SHIFT));
+	}
+	return 0;
+}
+
+static void ab_intclk_disable(struct clk *clk)
+{
+	if (clk->parent == clk->parents[AB_INTCLK_PARENT_SYSCLK])
+		return;
+
+	if (ab8500_sysctrl_clear(AB8500_SYSULPCLKCTRL1,
+		AB8500_SYSULPCLKCTRL1_SYSULPCLKINTSEL_MASK)) {
+		pr_err("clock: %s failed to disable %s.\n", __func__,
+			clk->name);
+	}
+}
+
+static int ab_intclk_set_parent(struct clk *clk, struct clk *parent)
+{
+	int err;
+
+	if (!clk->enabled)
+		return 0;
+
+	err = __clk_enable(parent, clk->mutex);
+
+	if (unlikely(err))
+		goto parent_enable_error;
+
+	if (parent == clk->parents[AB_INTCLK_PARENT_ULPCLK]) {
+		err = ab8500_sysctrl_write(AB8500_SYSULPCLKCTRL1,
+			AB8500_SYSULPCLKCTRL1_SYSULPCLKINTSEL_MASK,
+			(1 << AB8500_SYSULPCLKCTRL1_SYSULPCLKINTSEL_SHIFT));
+	} else {
+		err = ab8500_sysctrl_clear(AB8500_SYSULPCLKCTRL1,
+			AB8500_SYSULPCLKCTRL1_SYSULPCLKINTSEL_MASK);
+	}
+	if (unlikely(err))
+		goto config_error;
+
+	__clk_disable(clk->parent, clk->mutex);
+
+	return 0;
+
+config_error:
+	__clk_disable(parent, clk->mutex);
+parent_enable_error:
+	return err;
+}
+
+static struct clkops ab_intclk_ops = {
+	.enable = ab_intclk_enable,
+	.disable = ab_intclk_disable,
+	.set_parent = ab_intclk_set_parent,
+};
+
+/* AB8500 audio clock operations */
+
+static int audioclk_enable(struct clk *clk)
+{
+	return ab8500_sysctrl_set(AB8500_SYSULPCLKCTRL1,
+		AB8500_SYSULPCLKCTRL1_AUDIOCLKENA);
+}
+
+static void audioclk_disable(struct clk *clk)
+{
+	if (ab8500_sysctrl_clear(AB8500_SYSULPCLKCTRL1,
+		AB8500_SYSULPCLKCTRL1_AUDIOCLKENA)) {
+		pr_err("clock: %s failed to disable %s.\n", __func__,
+			clk->name);
+	}
+}
+
+static struct clkops audioclk_ops = {
+	.enable = audioclk_enable,
+	.disable = audioclk_disable,
+};
+
+/* Primary camera clock operations */
+static int clkout0_enable(struct clk *clk)
+{
+	int r;
+
+	if (clk->regulator == NULL) {
+		struct regulator *reg;
+
+		reg = regulator_get(NULL, "v-ape");
+		if (IS_ERR(reg))
+			return PTR_ERR(reg);
+		clk->regulator = reg;
+	}
+	r = regulator_enable(clk->regulator);
+	if (r)
+		goto regulator_failed;
+	r = prcmu_config_clkout(0, PRCMU_CLKSRC_SYSCLK, 4);
+	if (r)
+		goto config_failed;
+	r = nmk_config_pin(GPIO227_CLKOUT1, false);
+	if (r)
+		goto gpio_failed;
+	return r;
+
+gpio_failed:
+	(void)prcmu_config_clkout(0, PRCMU_CLKSRC_SYSCLK, 0);
+config_failed:
+	(void)regulator_disable(clk->regulator);
+regulator_failed:
+	return r;
+}
+
+static void clkout0_disable(struct clk *clk)
+{
+	int r;
+
+	r = nmk_config_pin(GPIO227_GPIO, false);
+	if (r)
+		goto disable_failed;
+	(void)prcmu_config_clkout(0, PRCMU_CLKSRC_SYSCLK, 0);
+	(void)regulator_disable(clk->regulator);
+	return;
+
+disable_failed:
+	pr_err("clock: failed to disable %s.\n", clk->name);
+}
+
+/* Touch screen/secondary camera clock operations. */
+static int clkout1_enable(struct clk *clk)
+{
+	int r;
+
+	if (clk->regulator == NULL) {
+		struct regulator *reg;
+
+		reg = regulator_get(NULL, "v-ape");
+		if (IS_ERR(reg))
+			return PTR_ERR(reg);
+		clk->regulator = reg;
+	}
+	r = regulator_enable(clk->regulator);
+	if (r)
+		goto regulator_failed;
+	r = prcmu_config_clkout(1, PRCMU_CLKSRC_SYSCLK, 4);
+	if (r)
+		goto config_failed;
+	r = nmk_config_pin(GPIO228_CLKOUT2, false);
+	if (r)
+		goto gpio_failed;
+	return r;
+
+gpio_failed:
+	(void)prcmu_config_clkout(1, PRCMU_CLKSRC_SYSCLK, 0);
+config_failed:
+	(void)regulator_disable(clk->regulator);
+regulator_failed:
+	return r;
+}
+
+static void clkout1_disable(struct clk *clk)
+{
+	int r;
+
+	r = nmk_config_pin(GPIO228_GPIO, false);
+	if (r)
+		goto disable_failed;
+	(void)prcmu_config_clkout(1, PRCMU_CLKSRC_SYSCLK, 0);
+	(void)regulator_disable(clk->regulator);
+	return;
+
+disable_failed:
+	pr_err("clock: failed to disable %s.\n", clk->name);
+}
+
+static struct clkops clkout0_ops = {
+	.enable = clkout0_enable,
+	.disable = clkout0_disable,
+};
+
+static struct clkops clkout1_ops = {
+	.enable = clkout1_enable,
+	.disable = clkout1_disable,
+};
+
+#define DEF_PER1_PCLK(_cg_bit, _name) \
+	DEF_PRCC_PCLK(_name, U8500_CLKRST1_BASE, _cg_bit, &per1clk)
+#define DEF_PER2_PCLK(_cg_bit, _name) \
+	DEF_PRCC_PCLK(_name, U8500_CLKRST2_BASE, _cg_bit, &per2clk)
+#define DEF_PER3_PCLK(_cg_bit, _name) \
+	DEF_PRCC_PCLK(_name, U8500_CLKRST3_BASE, _cg_bit, &per3clk)
+#define DEF_PER5_PCLK(_cg_bit, _name) \
+	DEF_PRCC_PCLK(_name, U8500_CLKRST5_BASE, _cg_bit, &per5clk)
+#define DEF_PER6_PCLK(_cg_bit, _name) \
+	DEF_PRCC_PCLK(_name, U8500_CLKRST6_BASE, _cg_bit, &per6clk)
+
+#define DEF_PER1_KCLK(_cg_bit, _name, _parent) \
+	DEF_PRCC_KCLK(_name, U8500_CLKRST1_BASE, _cg_bit, _parent, &per1clk)
+#define DEF_PER2_KCLK(_cg_bit, _name, _parent) \
+	DEF_PRCC_KCLK(_name, U8500_CLKRST2_BASE, _cg_bit, _parent, &per2clk)
+#define DEF_PER3_KCLK(_cg_bit, _name, _parent) \
+	DEF_PRCC_KCLK(_name, U8500_CLKRST3_BASE, _cg_bit, _parent, &per3clk)
+#define DEF_PER5_KCLK(_cg_bit, _name, _parent) \
+	DEF_PRCC_KCLK(_name, U8500_CLKRST5_BASE, _cg_bit, _parent, &per5clk)
+#define DEF_PER6_KCLK(_cg_bit, _name, _parent) \
+	DEF_PRCC_KCLK(_name, U8500_CLKRST6_BASE, _cg_bit, _parent, &per6clk)
+
+#define DEF_MTU_CLK(_cg_sel, _name, _bus_parent) \
+	struct clk _name = { \
+		.name = #_name, \
+		.ops = &mtu_clk_ops, \
+		.cg_sel = _cg_sel, \
+		.bus_parent = _bus_parent, \
+	}
+
+/* Clock sources. */
+
+static struct clk soc0_pll = {
+	.name = "soc0_pll",
+	.ops = &pll_clk_ops,
+};
+
+static struct clk soc1_pll = {
+	.name = "soc1_pll",
+	.ops = &prcmu_clk_ops,
+	.cg_sel = PRCMU_PLLSOC1,
+	.mutex = &soc1_pll_mutex,
+};
+
+static struct clk ddr_pll = {
+	.name = "ddr_pll",
+	.ops = &pll_clk_ops,
+};
+
+static struct clk ulp38m4 = {
+	.name = "ulp38m4",
+};
+
+static struct clk sysclk = {
+	.name = "sysclk",
+	.ops = &sysclk_ops,
+	.rate = 38400000,
+	.mutex = &sysclk_mutex,
+};
+
+static struct clk sysclk2 = {
+	.name = "sysclk2",
+	.ops = &sysclk_ops,
+	.cg_sel = AB8500_SYSULPCLKCTRL1_SYSCLKBUF2REQ,
+	.mutex = &sysclk_mutex,
+};
+
+static struct clk sysclk3 = {
+	.name = "sysclk3",
+	.ops = &sysclk_ops,
+	.cg_sel = AB8500_SYSULPCLKCTRL1_SYSCLKBUF3REQ,
+	.mutex = &sysclk_mutex,
+};
+
+static struct clk sysclk4 = {
+	.name = "sysclk4",
+	.ops = &sysclk_ops,
+	.cg_sel = AB8500_SYSULPCLKCTRL1_SYSCLKBUF4REQ,
+	.mutex = &sysclk_mutex,
+};
+
+static struct clk rtc32k = {
+	.name = "rtc32k",
+	.rate = 32768,
+};
+
+static struct clk clkout0 = {
+	.name = "clkout0",
+	.ops = &clkout0_ops,
+	.parent = &sysclk,
+	.rate = 9600000,
+	.mutex = &sysclk_mutex,
+};
+
+static struct clk clkout1 = {
+	.name = "clkout1",
+	.ops = &clkout1_ops,
+	.parent = &sysclk,
+	.rate = 9600000,
+	.mutex = &sysclk_mutex,
+};
+
+static struct clk ab_ulpclk = {
+	.name = "ab_ulpclk",
+	.ops = &ab_ulpclk_ops,
+	.rate = 38400000,
+	.mutex = &ab_ulpclk_mutex,
+};
+
+static struct clk *ab_intclk_parents[NUM_AB_INTCLK_PARENTS] = {
+	[AB_INTCLK_PARENT_SYSCLK] = &sysclk,
+	[AB_INTCLK_PARENT_ULPCLK] = &ab_ulpclk,
+	[AB_INTCLK_PARENTS_END] = NULL,
+};
+
+static struct clk ab_intclk = {
+	.name = "ab_intclk",
+	.ops = &ab_intclk_ops,
+	.mutex = &ab_intclk_mutex,
+	.parent = &sysclk,
+	.parents = ab_intclk_parents,
+};
+
+static struct clk audioclk = {
+	.name = "audioclk",
+	.ops = &audioclk_ops,
+	.mutex = &ab_intclk_mutex,
+	.parent = &ab_intclk,
+};
+
+static DEF_PRCMU_CLK(sgaclk, PRCMU_SGACLK, 320000000);
+static DEF_PRCMU_CLK(uartclk, PRCMU_UARTCLK, 38400000);
+static DEF_PRCMU_CLK(msp02clk, PRCMU_MSP02CLK, 19200000);
+static DEF_PRCMU_CLK(msp1clk, PRCMU_MSP1CLK, 19200000);
+static DEF_PRCMU_CLK(i2cclk, PRCMU_I2CCLK, 24000000);
+static DEF_PRCMU_CLK(slimclk, PRCMU_SLIMCLK, 19200000);
+static DEF_PRCMU_CLK(per1clk, PRCMU_PER1CLK, 133330000);
+static DEF_PRCMU_CLK(per2clk, PRCMU_PER2CLK, 133330000);
+static DEF_PRCMU_CLK(per3clk, PRCMU_PER3CLK, 133330000);
+static DEF_PRCMU_CLK(per5clk, PRCMU_PER5CLK, 133330000);
+static DEF_PRCMU_CLK(per6clk, PRCMU_PER6CLK, 133330000);
+static DEF_PRCMU_CLK(per7clk, PRCMU_PER7CLK, 100000000);
+static DEF_PRCMU_CLK(lcdclk, PRCMU_LCDCLK, 48000000);
+static DEF_PRCMU_OPP100_CLK(bmlclk, PRCMU_BMLCLK, 200000000);
+static DEF_PRCMU_CLK(hsitxclk, PRCMU_HSITXCLK, 100000000);
+static DEF_PRCMU_CLK(hsirxclk, PRCMU_HSIRXCLK, 200000000);
+static DEF_PRCMU_CLK(hdmiclk, PRCMU_HDMICLK, 76800000);
+static DEF_PRCMU_CLK(apeatclk, PRCMU_APEATCLK, 160000000);
+static DEF_PRCMU_CLK(apetraceclk, PRCMU_APETRACECLK, 160000000);
+static DEF_PRCMU_CLK(mcdeclk, PRCMU_MCDECLK, 160000000);
+static DEF_PRCMU_OPP100_CLK(ipi2cclk, PRCMU_IPI2CCLK, 24000000);
+static DEF_PRCMU_CLK(dsialtclk, PRCMU_DSIALTCLK, 384000000);
+static DEF_PRCMU_CLK(dmaclk, PRCMU_DMACLK, 200000000);
+static DEF_PRCMU_CLK(b2r2clk, PRCMU_B2R2CLK, 200000000);
+static DEF_PRCMU_CLK(tvclk, PRCMU_TVCLK, 76800000);
+/* TODO: For SSPCLK, the spec says 24MHz, while the old driver says 48MHz. */
+static DEF_PRCMU_CLK(sspclk, PRCMU_SSPCLK, 24000000);
+static DEF_PRCMU_CLK(rngclk, PRCMU_RNGCLK, 19200000);
+static DEF_PRCMU_CLK(uiccclk, PRCMU_UICCCLK, 48000000);
+static DEF_PRCMU_CLK(timclk, PRCMU_TIMCLK, 2400000);
+/* 100 MHz until 3.0.1, 50 MHz Since PRCMU FW 3.0.2 */
+static DEF_PRCMU_CLK(sdmmcclk, PRCMU_SDMMCCLK, 50000000);
+
+/* PRCC PClocks */
+
+static DEF_PER1_PCLK(0, p1_pclk0);
+static DEF_PER1_PCLK(1, p1_pclk1);
+static DEF_PER1_PCLK(2, p1_pclk2);
+static DEF_PER1_PCLK(3, p1_pclk3);
+static DEF_PER1_PCLK(4, p1_pclk4);
+static DEF_PER1_PCLK(5, p1_pclk5);
+static DEF_PER1_PCLK(6, p1_pclk6);
+static DEF_PER1_PCLK(7, p1_pclk7);
+static DEF_PER1_PCLK(8, p1_pclk8);
+static DEF_PER1_PCLK(9, p1_pclk9);
+static DEF_PER1_PCLK(10, p1_pclk10);
+static DEF_PER1_PCLK(11, p1_pclk11);
+
+static DEF_PER2_PCLK(0, p2_pclk0);
+static DEF_PER2_PCLK(1, p2_pclk1);
+static DEF_PER2_PCLK(2, p2_pclk2);
+static DEF_PER2_PCLK(3, p2_pclk3);
+static DEF_PER2_PCLK(4, p2_pclk4);
+static DEF_PER2_PCLK(5, p2_pclk5);
+static DEF_PER2_PCLK(6, p2_pclk6);
+static DEF_PER2_PCLK(7, p2_pclk7);
+static DEF_PER2_PCLK(8, p2_pclk8);
+static DEF_PER2_PCLK(9, p2_pclk9);
+static DEF_PER2_PCLK(10, p2_pclk10);
+static DEF_PER2_PCLK(11, p2_pclk11);
+
+static DEF_PER3_PCLK(0, p3_pclk0);
+static DEF_PER3_PCLK(1, p3_pclk1);
+static DEF_PER3_PCLK(2, p3_pclk2);
+static DEF_PER3_PCLK(3, p3_pclk3);
+static DEF_PER3_PCLK(4, p3_pclk4);
+static DEF_PER3_PCLK(5, p3_pclk5);
+static DEF_PER3_PCLK(6, p3_pclk6);
+static DEF_PER3_PCLK(7, p3_pclk7);
+static DEF_PER3_PCLK(8, p3_pclk8);
+
+static DEF_PER5_PCLK(0, p5_pclk0);
+static DEF_PER5_PCLK(1, p5_pclk1);
+
+static DEF_PER6_PCLK(0, p6_pclk0);
+static DEF_PER6_PCLK(1, p6_pclk1);
+static DEF_PER6_PCLK(2, p6_pclk2);
+static DEF_PER6_PCLK(3, p6_pclk3);
+static DEF_PER6_PCLK(4, p6_pclk4);
+static DEF_PER6_PCLK(5, p6_pclk5);
+static DEF_PER6_PCLK(6, p6_pclk6);
+static DEF_PER6_PCLK(7, p6_pclk7);
+
+/* UART0 */
+static DEF_PER1_KCLK(0, p1_uart0_kclk, &uartclk);
+static DEF_PER_CLK(p1_uart0_clk, &p1_pclk0, &p1_uart0_kclk);
+
+/* UART1 */
+static DEF_PER1_KCLK(1, p1_uart1_kclk, &uartclk);
+static DEF_PER_CLK(p1_uart1_clk, &p1_pclk1, &p1_uart1_kclk);
+
+/* I2C1 */
+static DEF_PER1_KCLK(2, p1_i2c1_kclk, &i2cclk);
+static DEF_PER_CLK(p1_i2c1_clk, &p1_pclk2, &p1_i2c1_kclk);
+
+/* MSP0 */
+static DEF_PER1_KCLK(3, p1_msp0_kclk, &msp02clk);
+static DEF_PER_CLK(p1_msp0_clk, &p1_pclk3, &p1_msp0_kclk);
+
+/* MSP1 */
+static DEF_PER1_KCLK(4, p1_msp1_kclk, &msp1clk);
+static DEF_PER_CLK(p1_msp1_clk, &p1_pclk4, &p1_msp1_kclk);
+
+/* SDI0 */
+static DEF_PER1_KCLK(5, p1_sdi0_kclk, &sdmmcclk);
+static DEF_PER_CLK(p1_sdi0_clk, &p1_pclk5, &p1_sdi0_kclk);
+
+/* I2C2 */
+static DEF_PER1_KCLK(6, p1_i2c2_kclk, &i2cclk);
+static DEF_PER_CLK(p1_i2c2_clk, &p1_pclk6, &p1_i2c2_kclk);
+
+/* SLIMBUS0 */
+static DEF_PER1_KCLK(3, p1_slimbus0_kclk, &slimclk);
+static DEF_PER_CLK(p1_slimbus0_clk, &p1_pclk8, &p1_slimbus0_kclk);
+
+/* I2C4 */
+static DEF_PER1_KCLK(9, p1_i2c4_kclk, &i2cclk);
+static DEF_PER_CLK(p1_i2c4_clk, &p1_pclk10, &p1_i2c4_kclk);
+
+/* MSP3 */
+static DEF_PER1_KCLK(10, p1_msp3_kclk, &msp1clk);
+static DEF_PER_CLK(p1_msp3_clk, &p1_pclk11, &p1_msp3_kclk);
+
+/* I2C3 */
+static DEF_PER2_KCLK(0, p2_i2c3_kclk, &i2cclk);
+static DEF_PER_CLK(p2_i2c3_clk, &p2_pclk0, &p2_i2c3_kclk);
+
+/* SDI4 */
+static DEF_PER2_KCLK(2, p2_sdi4_kclk, &sdmmcclk);
+static DEF_PER_CLK(p2_sdi4_clk, &p2_pclk4, &p2_sdi4_kclk);
+
+/* MSP2 */
+static DEF_PER2_KCLK(3, p2_msp2_kclk, &msp02clk);
+static DEF_PER_CLK(p2_msp2_clk, &p2_pclk5, &p2_msp2_kclk);
+
+/* SDI1 */
+static DEF_PER2_KCLK(4, p2_sdi1_kclk, &sdmmcclk);
+static DEF_PER_CLK(p2_sdi1_clk, &p2_pclk6, &p2_sdi1_kclk);
+
+/* SDI3 */
+static DEF_PER2_KCLK(5, p2_sdi3_kclk, &sdmmcclk);
+static DEF_PER_CLK(p2_sdi3_clk, &p2_pclk7, &p2_sdi3_kclk);
+
+/* HSIR */
+static DEF_PER2_KCLK(6, p2_ssirx_kclk, &hsirxclk);
+
+/* HSIT */
+static DEF_PER2_KCLK(7, p2_ssitx_kclk, &hsitxclk);
+
+/* SSP0 */
+static DEF_PER3_KCLK(1, p3_ssp0_kclk, &sspclk);
+static DEF_PER_CLK(p3_ssp0_clk, &p3_pclk1, &p3_ssp0_kclk);
+
+/* SSP1 */
+static DEF_PER3_KCLK(2, p3_ssp1_kclk, &sspclk);
+static DEF_PER_CLK(p3_ssp1_clk, &p3_pclk2, &p3_ssp1_kclk);
+
+/* I2C0 */
+static DEF_PER3_KCLK(3, p3_i2c0_kclk, &i2cclk);
+static DEF_PER_CLK(p3_i2c0_clk, &p3_pclk3, &p3_i2c0_kclk);
+
+/* SDI2 */
+static DEF_PER3_KCLK(4, p3_sdi2_kclk, &sdmmcclk);
+static DEF_PER_CLK(p3_sdi2_clk, &p3_pclk4, &p3_sdi2_kclk);
+
+/* SKE */
+static DEF_PER3_KCLK(5, p3_ske_kclk, &rtc32k);
+static DEF_PER_CLK(p3_ske_clk, &p3_pclk5, &p3_ske_kclk);
+
+/* UART2 */
+static DEF_PER3_KCLK(6, p3_uart2_kclk, &uartclk);
+static DEF_PER_CLK(p3_uart2_clk, &p3_pclk6, &p3_uart2_kclk);
+
+/* SDI5 */
+static DEF_PER3_KCLK(7, p3_sdi5_kclk, &sdmmcclk);
+static DEF_PER_CLK(p3_sdi5_clk, &p3_pclk7, &p3_sdi5_kclk);
+
+/* RNG */
+static DEF_PER6_KCLK(0, p6_rng_kclk, &rngclk);
+static DEF_PER_CLK(p6_rng_clk, &p6_pclk0, &p6_rng_kclk);
+
+/* MTU:S */
+
+/* MTU0 */
+static DEF_PER_CLK(p6_mtu0_clk, &p6_pclk6, &timclk);
+
+/* MTU1 */
+static DEF_PER_CLK(p6_mtu1_clk, &p6_pclk7, &timclk);
+
+#ifdef CONFIG_DEBUG_FS
+
+struct clk_debug_info {
+	struct clk *clk;
+	struct dentry *dir;
+	struct dentry *enable;
+	struct dentry *requests;
+	int enabled;
+};
+
+static struct dentry *clk_dir;
+static struct dentry *clk_show;
+static struct dentry *clk_show_enabled_only;
+
+static struct clk_debug_info dbg_clks[] = {
+	/* Clock sources */
+	{ .clk = &soc0_pll, },
+	{ .clk = &soc1_pll, },
+	{ .clk = &ddr_pll, },
+	{ .clk = &ulp38m4, },
+	{ .clk = &sysclk, },
+	{ .clk = &rtc32k, },
+	/* PRCMU clocks */
+	{ .clk = &sgaclk, },
+	{ .clk = &uartclk, },
+	{ .clk = &msp02clk, },
+	{ .clk = &msp1clk, },
+	{ .clk = &i2cclk, },
+	{ .clk = &sdmmcclk, },
+	{ .clk = &slimclk, },
+	{ .clk = &per1clk, },
+	{ .clk = &per2clk, },
+	{ .clk = &per3clk, },
+	{ .clk = &per5clk, },
+	{ .clk = &per6clk, },
+	{ .clk = &per7clk, },
+	{ .clk = &lcdclk, },
+	{ .clk = &bmlclk, },
+	{ .clk = &hsitxclk, },
+	{ .clk = &hsirxclk, },
+	{ .clk = &hdmiclk, },
+	{ .clk = &apeatclk, },
+	{ .clk = &apetraceclk, },
+	{ .clk = &mcdeclk, },
+	{ .clk = &ipi2cclk, },
+	{ .clk = &dsialtclk, },
+	{ .clk = &dmaclk, },
+	{ .clk = &b2r2clk, },
+	{ .clk = &tvclk, },
+	{ .clk = &sspclk, },
+	{ .clk = &rngclk, },
+	{ .clk = &uiccclk, },
+	{ .clk = &sysclk2, },
+	{ .clk = &clkout0, },
+	{ .clk = &clkout1, },
+};
+
+static int clk_show_print(struct seq_file *s, void *p)
+{
+	int i;
+	int enabled_only = (int)s->private;
+
+	seq_printf(s, "\n%-20s %s\n", "name", "enabled (kernel + debug)");
+	for (i = 0; i < ARRAY_SIZE(dbg_clks); i++) {
+		if (enabled_only && !dbg_clks[i].clk->enabled)
+			continue;
+		seq_printf(s,
+			   "%-20s %5d + %d\n",
+			   dbg_clks[i].clk->name,
+			   dbg_clks[i].clk->enabled - dbg_clks[i].enabled,
+			   dbg_clks[i].enabled);
+	}
+
+	return 0;
+}
+
+static int clk_show_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, clk_show_print, inode->i_private);
+}
+
+static int clk_enable_print(struct seq_file *s, void *p)
+{
+	struct clk_debug_info *cdi = s->private;
+
+	return seq_printf(s, "%d\n", cdi->enabled);
+}
+
+static int clk_enable_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, clk_enable_print, inode->i_private);
+}
+
+static ssize_t clk_enable_write(struct file *file, const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct clk_debug_info *cdi;
+	char buf[32];
+	ssize_t buf_size;
+	long user_val;
+	int err;
+
+	cdi = ((struct seq_file *)(file->private_data))->private;
+
+	buf_size = min(count, (sizeof(buf) - 1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = '\0';
+
+	err = strict_strtol(buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	if ((user_val > 0) && (!cdi->enabled)) {
+		err = clk_enable(cdi->clk);
+		if (err) {
+			pr_err("clock: clk_enable(%s) failed.\n",
+				cdi->clk->name);
+			return -EFAULT;
+		}
+		cdi->enabled = 1;
+	} else if ((user_val <= 0) && (cdi->enabled)) {
+		clk_disable(cdi->clk);
+		cdi->enabled = 0;
+	}
+	return buf_size;
+}
+
+static int clk_requests_print(struct seq_file *s, void *p)
+{
+	struct clk_debug_info *cdi = s->private;
+
+	return seq_printf(s, "%d\n", cdi->clk->enabled);
+}
+
+static int clk_requests_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, clk_requests_print, inode->i_private);
+}
+
+static const struct file_operations clk_enable_fops = {
+	.open = clk_enable_open,
+	.write = clk_enable_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations clk_requests_fops = {
+	.open = clk_requests_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations clk_show_fops = {
+	.open = clk_show_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int create_clk_dirs(struct clk_debug_info *cdi, int size)
+{
+	int i;
+
+	for (i = 0; i < size; i++) {
+		cdi[i].dir = debugfs_create_dir(cdi[i].clk->name, clk_dir);
+		if (!cdi[i].dir)
+			goto no_dir;
+	}
+
+	for (i = 0; i < size; i++) {
+		cdi[i].enable = debugfs_create_file("enable",
+						    (S_IRUGO | S_IWUGO),
+						    cdi[i].dir, &cdi[i],
+						    &clk_enable_fops);
+		if (!cdi[i].enable)
+			goto no_enable;
+	}
+	for (i = 0; i < size; i++) {
+		cdi[i].requests = debugfs_create_file("requests", S_IRUGO,
+						       cdi[i].dir, &cdi[i],
+						       &clk_requests_fops);
+		if (!cdi[i].requests)
+			goto no_requests;
+	}
+	return 0;
+
+no_requests:
+	while (i--)
+		debugfs_remove(cdi[i].requests);
+	i = size;
+no_enable:
+	while (i--)
+		debugfs_remove(cdi[i].enable);
+	i = size;
+no_dir:
+	while (i--)
+		debugfs_remove(cdi[i].dir);
+
+	return -ENOMEM;
+}
+
+static void remove_clk_dirs(struct clk_debug_info *cdi, int size)
+{
+	int i;
+	for (i = 0; i < size; i++) {
+		debugfs_remove(cdi[i].requests);
+		debugfs_remove(cdi[i].enable);
+		debugfs_remove(cdi[i].dir);
+	}
+}
+
+static int __init clk_debug_init(void)
+{
+	clk_dir = debugfs_create_dir("clk", NULL);
+	if (!clk_dir)
+		goto no_dir;
+
+	clk_show = debugfs_create_file("show", S_IRUGO, clk_dir, (void *)0,
+				       &clk_show_fops);
+	if (!clk_show)
+		goto no_show;
+
+	clk_show_enabled_only = debugfs_create_file("show-enabled-only",
+					       S_IRUGO, clk_dir, (void *)1,
+					       &clk_show_fops);
+	if (!clk_show_enabled_only)
+		goto no_enabled_only;
+
+	if (create_clk_dirs(&dbg_clks[0], ARRAY_SIZE(dbg_clks)))
+		goto no_clks;
+
+	return 0;
+no_clks:
+	debugfs_remove(clk_show_enabled_only);
+no_enabled_only:
+	debugfs_remove(clk_show);
+no_show:
+	debugfs_remove(clk_dir);
+no_dir:
+	return -ENOMEM;
+}
+
+static void __exit clk_debug_exit(void)
+{
+	remove_clk_dirs(&dbg_clks[0], ARRAY_SIZE(dbg_clks));
+
+	debugfs_remove(clk_show);
+	debugfs_remove(clk_show_enabled_only);
+	debugfs_remove(clk_dir);
+}
+
+subsys_initcall(clk_debug_init);
+module_exit(clk_debug_exit);
+#endif /* CONFIG_DEBUG_FS */
+
+/*
+ * TODO: Ensure names match with devices and then remove unnecessary entries
+ * when all drivers use the clk API.
+ */
+
+#define CLK_LOOKUP(_clk, _dev_id, _con_id) \
+	{ .dev_id = _dev_id, .con_id = _con_id, .clk = &_clk }
+
+static struct clk_lookup u8500_clocks[] = {
+	CLK_LOOKUP(soc0_pll, NULL, "soc0_pll"),
+	CLK_LOOKUP(soc1_pll, NULL, "soc1_pll"),
+	CLK_LOOKUP(ddr_pll, NULL, "ddr_pll"),
+	CLK_LOOKUP(ulp38m4, NULL, "ulp38m4"),
+	CLK_LOOKUP(sysclk, NULL, "sysclk"),
+	CLK_LOOKUP(rtc32k, NULL, "clk32k"),
+	CLK_LOOKUP(sysclk, "ab8500-usb.0", "sysclk"),
+	CLK_LOOKUP(sysclk, "ab8500-codec.0", "sysclk"),
+	CLK_LOOKUP(ab_ulpclk, "ab8500-codec.0", "ulpclk"),
+	CLK_LOOKUP(ab_intclk, "ab8500-codec.0", "intclk"),
+	CLK_LOOKUP(audioclk, "ab8500-codec.0", "audioclk"),
+	CLK_LOOKUP(ab_intclk, "ab8500-pwm.1", NULL),
+	CLK_LOOKUP(ab_intclk, "ab8500-pwm.2", NULL),
+	CLK_LOOKUP(ab_intclk, "ab8500-pwm.3", NULL),
+
+	CLK_LOOKUP(clkout0, "pri-cam", NULL),
+	CLK_LOOKUP(clkout1, "3-005c", NULL),
+	CLK_LOOKUP(clkout1, "3-005d", NULL),
+	CLK_LOOKUP(clkout1, "sec-cam", NULL),
+
+	/* prcmu */
+	CLK_LOOKUP(sgaclk, "mali", NULL),
+	CLK_LOOKUP(uartclk, "UART", NULL),
+	CLK_LOOKUP(msp02clk, "MSP02", NULL),
+	CLK_LOOKUP(i2cclk, "I2C", NULL),
+	CLK_LOOKUP(sdmmcclk, "sdmmc", NULL),
+	CLK_LOOKUP(slimclk, "slim", NULL),
+	CLK_LOOKUP(per1clk, "PERIPH1", NULL),
+	CLK_LOOKUP(per2clk, "PERIPH2", NULL),
+	CLK_LOOKUP(per3clk, "PERIPH3", NULL),
+	CLK_LOOKUP(per5clk, "PERIPH5", NULL),
+	CLK_LOOKUP(per6clk, "PERIPH6", NULL),
+	CLK_LOOKUP(per7clk, "PERIPH7", NULL),
+	CLK_LOOKUP(lcdclk, "lcd", NULL),
+	CLK_LOOKUP(bmlclk, "bml", NULL),
+	CLK_LOOKUP(p2_ssitx_kclk, "ste_hsi.0", "hsit_hsitxclk"),
+	CLK_LOOKUP(p2_ssirx_kclk, "ste_hsi.0", "hsir_hsirxclk"),
+	CLK_LOOKUP(lcdclk, "mcde", "lcd"),
+	CLK_LOOKUP(hdmiclk, "hdmi", NULL),
+	CLK_LOOKUP(hdmiclk, "mcde", "hdmi"),
+	CLK_LOOKUP(apeatclk, "apeat", NULL),
+	CLK_LOOKUP(apetraceclk, "apetrace", NULL),
+	CLK_LOOKUP(mcdeclk, "mcde", NULL),
+	CLK_LOOKUP(mcdeclk, "mcde", "mcde"),
+	CLK_LOOKUP(ipi2cclk, "ipi2", NULL),
+	CLK_LOOKUP(dmaclk, "dma40.0", NULL),
+	CLK_LOOKUP(b2r2clk, "b2r2", NULL),
+	CLK_LOOKUP(b2r2clk, "b2r2_bus", NULL),
+	CLK_LOOKUP(b2r2clk, "U8500-B2R2.0", NULL),
+	CLK_LOOKUP(tvclk, "tv", NULL),
+	CLK_LOOKUP(tvclk, "mcde", "tv"),
+	CLK_LOOKUP(msp1clk, "MSP1", NULL),
+	CLK_LOOKUP(dsialtclk, "dsialt", NULL),
+	CLK_LOOKUP(sspclk, "SSP", NULL),
+	CLK_LOOKUP(rngclk, "rngclk", NULL),
+	CLK_LOOKUP(uiccclk, "uicc", NULL),
+
+	/* PERIPH 1 */
+	CLK_LOOKUP(p1_msp3_clk, "msp3", NULL),
+	CLK_LOOKUP(p1_msp3_clk, "MSP_I2S.3", NULL),
+	CLK_LOOKUP(p1_msp3_kclk, "ab8500-codec.0", "msp3-kernel"),
+	CLK_LOOKUP(p1_pclk11, "ab8500-codec.0", "msp3-bus"),
+	CLK_LOOKUP(p1_uart0_clk, "uart0", NULL),
+	CLK_LOOKUP(p1_uart1_clk, "uart1", NULL),
+	CLK_LOOKUP(p1_i2c1_clk, "nmk-i2c.1", NULL),
+	CLK_LOOKUP(p1_msp0_clk, "msp0", NULL),
+	CLK_LOOKUP(p1_msp0_clk, "MSP_I2S.0", NULL),
+	CLK_LOOKUP(p1_sdi0_clk, "sdi0", NULL),
+	CLK_LOOKUP(p1_i2c2_clk, "nmk-i2c.2", NULL),
+	CLK_LOOKUP(p1_slimbus0_clk, "slimbus0", NULL),
+	CLK_LOOKUP(p1_pclk9, "gpio.0", NULL),
+	CLK_LOOKUP(p1_pclk9, "gpio.1", NULL),
+	CLK_LOOKUP(p1_pclk9, "gpioblock0", NULL),
+	CLK_LOOKUP(p1_msp1_clk, "msp1", NULL),
+	CLK_LOOKUP(p1_msp1_clk, "MSP_I2S.1", NULL),
+	CLK_LOOKUP(p1_msp1_kclk, "ab8500-codec.0", "msp1-kernel"),
+	CLK_LOOKUP(p1_pclk4, "ab8500-codec.0", "msp1-bus"),
+	CLK_LOOKUP(p1_pclk7, "spi3", NULL),
+	CLK_LOOKUP(p1_i2c4_clk, "nmk-i2c.4", NULL),
+
+	/* PERIPH 2 */
+	CLK_LOOKUP(p2_i2c3_clk, "nmk-i2c.3", NULL),
+	CLK_LOOKUP(p2_pclk1, "spi2", NULL),
+	CLK_LOOKUP(p2_pclk2, "spi1", NULL),
+	CLK_LOOKUP(p2_pclk3, "pwl", NULL),
+	CLK_LOOKUP(p2_sdi4_clk, "sdi4", NULL),
+	CLK_LOOKUP(p2_msp2_clk, "msp2", NULL),
+	CLK_LOOKUP(p2_msp2_clk, "MSP_I2S.2", NULL),
+	CLK_LOOKUP(p2_sdi1_clk, "sdi1", NULL),
+	CLK_LOOKUP(p2_sdi3_clk, "sdi3", NULL),
+	CLK_LOOKUP(p2_pclk8, "spi0", NULL),
+	CLK_LOOKUP(p2_pclk9, "ste_hsi.0", "hsir_hclk"),
+	CLK_LOOKUP(p2_pclk10, "ste_hsi.0", "hsit_hclk"),
+	CLK_LOOKUP(p2_pclk11, "gpio.6", NULL),
+	CLK_LOOKUP(p2_pclk11, "gpio.7", NULL),
+	CLK_LOOKUP(p2_pclk11, "gpioblock1", NULL),
+
+	/* PERIPH 3 */
+	CLK_LOOKUP(p3_pclk0, "fsmc", NULL),
+	CLK_LOOKUP(p3_i2c0_clk, "nmk-i2c.0", NULL),
+	CLK_LOOKUP(p3_sdi2_clk, "sdi2", NULL),
+	CLK_LOOKUP(p3_ske_clk, "ske", NULL),
+	CLK_LOOKUP(p3_ske_clk, "nmk-ske-keypad", NULL),
+	CLK_LOOKUP(p3_uart2_clk, "uart2", NULL),
+	CLK_LOOKUP(p3_sdi5_clk, "sdi5", NULL),
+	CLK_LOOKUP(p3_pclk8, "gpio.2", NULL),
+	CLK_LOOKUP(p3_pclk8, "gpio.3", NULL),
+	CLK_LOOKUP(p3_pclk8, "gpio.4", NULL),
+	CLK_LOOKUP(p3_pclk8, "gpio.5", NULL),
+	CLK_LOOKUP(p3_pclk8, "gpioblock2", NULL),
+	CLK_LOOKUP(p3_ssp0_clk, "ssp0", NULL),
+	CLK_LOOKUP(p3_ssp1_clk, "ssp1", NULL),
+
+	/* PERIPH 5 */
+	CLK_LOOKUP(p5_pclk1, "gpio.8", NULL),
+	CLK_LOOKUP(p5_pclk1, "gpioblock3", NULL),
+	CLK_LOOKUP(p5_pclk0, "musb-ux500.0", "usb"),
+
+	/* PERIPH 6 */
+	CLK_LOOKUP(p6_pclk1, "cryp0", NULL),
+	CLK_LOOKUP(p6_pclk2, "hash0", NULL),
+	CLK_LOOKUP(p6_pclk3, "pka", NULL),
+	CLK_LOOKUP(p6_pclk5, "cfgreg", NULL),
+	CLK_LOOKUP(p6_mtu0_clk, "mtu0", NULL),
+	CLK_LOOKUP(p6_mtu1_clk, "mtu1", NULL),
+	CLK_LOOKUP(p6_pclk4, "hash1", NULL),
+	CLK_LOOKUP(p6_pclk1, "cryp1", NULL),
+	CLK_LOOKUP(p6_rng_clk, "rng", NULL),
+
+};
+
+static struct clk_lookup u8500_v2_sysclks[] = {
+	CLK_LOOKUP(sysclk2, NULL, "sysclk2"),
+	CLK_LOOKUP(sysclk3, NULL, "sysclk3"),
+	CLK_LOOKUP(sysclk4, NULL, "sysclk4"),
+};
+
+/* these are the clocks which are default from the bootloader */
+static const char *u8500_boot_clk[] = {
+	"uart0",
+	"uart1",
+	"uart2",
+	"gpioblock0",
+	"gpioblock1",
+	"gpioblock2",
+	"gpioblock3",
+	"mtu0",
+	"mtu1",
+	"ssp0",
+	"ssp1",
+	"spi0",
+	"spi1",
+	"spi2",
+	"spi3",
+	"msp0",
+	"msp2",
+	"nmk-i2c.0",
+	"nmk-i2c.1",
+	"nmk-i2c.2",
+	"nmk-i2c.3",
+	"nmk-i2c.4",
+};
+
+static void sysclk_init_disable(struct work_struct *not_used)
+{
+	int i;
+
+	mutex_lock(&sysclk_mutex);
+
+	/* Enable SWAT  */
+	if (ab8500_sysctrl_set(AB8500_SWATCTRL, AB8500_SWATCTRL_SWATENABLE))
+		goto err_swat;
+
+	for (i = 0; i < ARRAY_SIZE(u8500_v2_sysclks); i++) {
+		struct clk *clk = u8500_v2_sysclks[i].clk;
+
+		/* Disable sysclks */
+		if (!clk->enabled && clk->cg_sel) {
+			if (ab8500_sysctrl_clear(AB8500_SYSULPCLKCTRL1,
+				(u8)clk->cg_sel))
+				goto err_sysclk;
+		}
+	}
+	goto unlock_and_exit;
+
+err_sysclk:
+	pr_err("clock: Disable %s failed", u8500_v2_sysclks[i].clk->name);
+	ab8500_sysctrl_clear(AB8500_SWATCTRL, AB8500_SWATCTRL_SWATENABLE);
+	goto unlock_and_exit;
+
+err_swat:
+	pr_err("clock: Enable SWAT failed");
+
+unlock_and_exit:
+	mutex_unlock(&sysclk_mutex);
+}
+
+static struct clk *boot_clks[ARRAY_SIZE(u8500_boot_clk)];
+
+/* we disable a majority of peripherals enabled by default
+ * but without drivers
+ */
+static int __init u8500_boot_clk_disable(void)
+{
+	unsigned int i = 0;
+
+	for (i = 0; i < ARRAY_SIZE(u8500_boot_clk); i++) {
+		if (!boot_clks[i])
+			continue;
+
+		clk_disable(boot_clks[i]);
+		clk_put(boot_clks[i]);
+	}
+
+	INIT_DELAYED_WORK(&sysclk_disable_work, sysclk_init_disable);
+	schedule_delayed_work(&sysclk_disable_work, 10 * HZ);
+
+	return 0;
+}
+late_initcall_sync(u8500_boot_clk_disable);
+
+static void u8500_amba_clk_enable(void)
+{
+	unsigned int i = 0;
+
+	writel(~0x0  & ~(1 << 9), __io_address(U8500_PER1_BASE + 0xF000
+					       + 0x04));
+	writel(~0x0, __io_address(U8500_PER1_BASE + 0xF000 + 0x0C));
+
+	writel(~0x0 & ~(1 << 11), __io_address(U8500_PER2_BASE + 0xF000
+					       + 0x04));
+	writel(~0x0, __io_address(U8500_PER2_BASE + 0xF000 + 0x0C));
+
+	if(machine_is_snowball()){
+		/* Leave FSMC alone, it is needed for ethernet controller */
+		writel(0xFE, __io_address(U8500_PER3_BASE + 0xF000 + 0x04));
+		writel(~0x0 & ~(1 << 1), __io_address(U8500_PER3_BASE + 0xF000
+							      + 0x0C));
+	} else {
+		/* GPIO,UART2 are enabled for booting */
+		writel(0xBF, __io_address(U8500_PER3_BASE + 0xF000 + 0x04));
+		writel(~0x0 & ~(1 << 6), __io_address(U8500_PER3_BASE + 0xF000
+							      + 0x0C));
+	}
+
+	for (i = 0; i < ARRAY_SIZE(u8500_boot_clk); i++) {
+		boot_clks[i] = clk_get_sys(u8500_boot_clk[i], NULL);
+		clk_enable(boot_clks[i]);
+	}
+}
+
+static int __init init_clock_states(void)
+{
+	/*
+	 * The following clks are shared with secure world.
+	 * Currently this leads to a limitation where we need to
+	 * enable them at all times.
+	 */
+	clk_enable(&p6_pclk1);
+	clk_enable(&p6_pclk2);
+	clk_enable(&p6_pclk3);
+	clk_enable(&p6_rng_clk);
+	/*
+	 * Disable clocks that are on at boot, but should be off.
+	 */
+	if (!clk_enable(&bmlclk))
+		clk_disable(&bmlclk);
+	if (!clk_enable(&dsialtclk))
+		clk_disable(&dsialtclk);
+	if (!clk_enable(&hsirxclk))
+		clk_disable(&hsirxclk);
+	if (!clk_enable(&hsitxclk))
+		clk_disable(&hsitxclk);
+	if (!clk_enable(&ipi2cclk))
+		clk_disable(&ipi2cclk);
+	if (!clk_enable(&lcdclk))
+		clk_disable(&lcdclk);
+	if (!clk_enable(&per7clk))
+		clk_disable(&per7clk);
+	/*
+	 * APEATCLK and APETRACECLK are enabled at boot and needed
+	 * in order to debug with lauterbach
+	 */
+	if (!clk_enable(&apeatclk)) {
+#ifdef CONFIG_UX500_DEBUG_NO_LAUTERBACH
+		clk_disable(&apeatclk);
+#endif
+	}
+	if (!clk_enable(&apetraceclk)) {
+#ifdef CONFIG_UX500_DEBUG_NO_LAUTERBACH
+		clk_disable(&apetraceclk);
+#endif
+	}
+	return 0;
+}
+late_initcall(init_clock_states);
+
+int __init db8500_clk_init(void)
+{
+
+	clks_register(u8500_v2_sysclks,
+		      ARRAY_SIZE(u8500_v2_sysclks));
+	clks_register(u8500_clocks,
+		      ARRAY_SIZE(u8500_clocks));
+
+	u8500_amba_clk_enable();
+
+	return 0;
+}
diff --git a/arch/arm/mach-ux500/clock-debug.c b/arch/arm/mach-ux500/clock-debug.c
new file mode 100644
index 00000000000..939edfaa94a
--- /dev/null
+++ b/arch/arm/mach-ux500/clock-debug.c
@@ -0,0 +1,228 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ * Author: Mattias Nilsson <mattias.i.nilsson@stericsson.com> for ST-Ericsson
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <linux/clk.h>
+
+#include "clock.h"
+
+struct clk_debug_info {
+	struct clk *clk;
+	struct dentry *dir;
+	struct dentry *enable;
+	struct dentry *requests;
+	int enabled;
+};
+
+#ifdef CONFIG_DEBUG_FS
+
+static struct dentry *clk_dir;
+static struct dentry *clk_show;
+static struct dentry *clk_show_enabled_only;
+
+static struct clk_debug_info *cdi;
+static int num_clks;
+
+static int clk_show_print(struct seq_file *s, void *p)
+{
+	int i;
+	int enabled_only = (int)s->private;
+
+	seq_printf(s, "\n%-20s %s\n", "name", "enabled (kernel + debug)");
+	for (i = 0; i < num_clks; i++) {
+		if (enabled_only && !cdi[i].clk->enabled)
+			continue;
+		seq_printf(s,
+			   "%-20s %5d + %d\n",
+			   cdi[i].clk->name,
+			   cdi[i].clk->enabled - cdi[i].enabled,
+			   cdi[i].enabled);
+	}
+
+	return 0;
+}
+
+static int clk_show_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, clk_show_print, inode->i_private);
+}
+
+static int clk_enable_print(struct seq_file *s, void *p)
+{
+	struct clk_debug_info *cdi = s->private;
+
+	return seq_printf(s, "%d\n", cdi->enabled);
+}
+
+static int clk_enable_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, clk_enable_print, inode->i_private);
+}
+
+static ssize_t clk_enable_write(struct file *file, const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct clk_debug_info *cdi;
+	char buf[32];
+	ssize_t buf_size;
+	long user_val;
+	int err;
+
+	cdi = ((struct seq_file *)(file->private_data))->private;
+
+	buf_size = min(count, (sizeof(buf) - 1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = '\0';
+
+	err = strict_strtol(buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	if ((user_val > 0) && (!cdi->enabled)) {
+		err = clk_enable(cdi->clk);
+		if (err) {
+			pr_err("clock: clk_enable(%s) failed.\n",
+				cdi->clk->name);
+			return -EFAULT;
+		}
+		cdi->enabled = 1;
+	} else if ((user_val <= 0) && (cdi->enabled)) {
+		clk_disable(cdi->clk);
+		cdi->enabled = 0;
+	}
+	return buf_size;
+}
+
+static int clk_requests_print(struct seq_file *s, void *p)
+{
+	struct clk_debug_info *cdi = s->private;
+
+	return seq_printf(s, "%d\n", cdi->clk->enabled);
+}
+
+static int clk_requests_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, clk_requests_print, inode->i_private);
+}
+
+static const struct file_operations clk_enable_fops = {
+	.open = clk_enable_open,
+	.write = clk_enable_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations clk_requests_fops = {
+	.open = clk_requests_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations clk_show_fops = {
+	.open = clk_show_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int create_clk_dirs(struct clk_debug_info *cdi, int size)
+{
+	int i;
+
+	for (i = 0; i < size; i++) {
+		cdi[i].dir = debugfs_create_dir(cdi[i].clk->name, clk_dir);
+		if (!cdi[i].dir)
+			goto no_dir;
+	}
+
+	for (i = 0; i < size; i++) {
+		cdi[i].enable = debugfs_create_file("enable",
+						    (S_IRUGO | S_IWUGO),
+						    cdi[i].dir, &cdi[i],
+						    &clk_enable_fops);
+		if (!cdi[i].enable)
+			goto no_enable;
+	}
+	for (i = 0; i < size; i++) {
+		cdi[i].requests = debugfs_create_file("requests", S_IRUGO,
+						       cdi[i].dir, &cdi[i],
+						       &clk_requests_fops);
+		if (!cdi[i].requests)
+			goto no_requests;
+	}
+	return 0;
+
+no_requests:
+	while (i--)
+		debugfs_remove(cdi[i].requests);
+	i = size;
+no_enable:
+	while (i--)
+		debugfs_remove(cdi[i].enable);
+	i = size;
+no_dir:
+	while (i--)
+		debugfs_remove(cdi[i].dir);
+
+	return -ENOMEM;
+}
+
+int __init dbx500_clk_debug_init(struct clk **clks, int num)
+{
+	int i;
+
+	cdi = kcalloc(sizeof(struct clk_debug_info), num, GFP_KERNEL);
+	if (!cdi)
+		return -ENOMEM;
+
+	for (i = 0; i < num; i++)
+		cdi[i].clk = clks[i];
+
+	num_clks = num;
+
+	clk_dir = debugfs_create_dir("clk", NULL);
+	if (!clk_dir)
+		goto no_dir;
+
+	clk_show = debugfs_create_file("show", S_IRUGO, clk_dir, (void *)0,
+				       &clk_show_fops);
+	if (!clk_show)
+		goto no_show;
+
+	clk_show_enabled_only = debugfs_create_file("show-enabled-only",
+					       S_IRUGO, clk_dir, (void *)1,
+					       &clk_show_fops);
+	if (!clk_show_enabled_only)
+		goto no_enabled_only;
+
+	if (create_clk_dirs(cdi, num))
+		goto no_clks;
+
+	return 0;
+
+no_clks:
+	debugfs_remove(clk_show_enabled_only);
+no_enabled_only:
+	debugfs_remove(clk_show);
+no_show:
+	debugfs_remove(clk_dir);
+no_dir:
+	kfree(cdi);
+	return -ENOMEM;
+}
+
+#endif /* CONFIG_DEBUG_FS */
diff --git a/arch/arm/mach-ux500/clock.c b/arch/arm/mach-ux500/clock.c
index 32ce90840ee..d40e058fd87 100644
--- a/arch/arm/mach-ux500/clock.c
+++ b/arch/arm/mach-ux500/clock.c
@@ -7,581 +7,432 @@
  * published by the Free Software Foundation.
  */
 #include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/errno.h>
 #include <linux/err.h>
-#include <linux/clk.h>
+#include <linux/errno.h>
 #include <linux/io.h>
-#include <linux/clkdev.h>
+#include <linux/spinlock.h>
+#include <linux/mfd/ab8500/sysctrl.h>
+#ifdef CONFIG_DEBUG_FS
+#include <linux/clk.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>	/* for copy_from_user */
+#include <linux/kernel.h>
+#endif
+
+#include <mach/prcmu.h>
 
-#include <plat/mtu.h>
-#include <mach/hardware.h>
 #include "clock.h"
 
 #ifdef CONFIG_DEBUG_FS
-#include <linux/debugfs.h>
-#include <linux/uaccess.h>	/* for copy_from_user */
 static LIST_HEAD(clk_list);
 #endif
 
-#define PRCC_PCKEN		0x00
-#define PRCC_PCKDIS		0x04
-#define PRCC_KCKEN		0x08
-#define PRCC_KCKDIS		0x0C
-
-#define PRCM_YYCLKEN0_MGT_SET	0x510
-#define PRCM_YYCLKEN1_MGT_SET	0x514
-#define PRCM_YYCLKEN0_MGT_CLR	0x518
-#define PRCM_YYCLKEN1_MGT_CLR	0x51C
-#define PRCM_YYCLKEN0_MGT_VAL	0x520
-#define PRCM_YYCLKEN1_MGT_VAL	0x524
-
-#define PRCM_SVAMMDSPCLK_MGT	0x008
-#define PRCM_SIAMMDSPCLK_MGT	0x00C
-#define PRCM_SGACLK_MGT		0x014
-#define PRCM_UARTCLK_MGT	0x018
-#define PRCM_MSP02CLK_MGT	0x01C
-#define PRCM_MSP1CLK_MGT	0x288
-#define PRCM_I2CCLK_MGT		0x020
-#define PRCM_SDMMCCLK_MGT	0x024
-#define PRCM_SLIMCLK_MGT	0x028
-#define PRCM_PER1CLK_MGT	0x02C
-#define PRCM_PER2CLK_MGT	0x030
-#define PRCM_PER3CLK_MGT	0x034
-#define PRCM_PER5CLK_MGT	0x038
-#define PRCM_PER6CLK_MGT	0x03C
-#define PRCM_PER7CLK_MGT	0x040
-#define PRCM_LCDCLK_MGT		0x044
-#define PRCM_BMLCLK_MGT		0x04C
-#define PRCM_HSITXCLK_MGT	0x050
-#define PRCM_HSIRXCLK_MGT	0x054
-#define PRCM_HDMICLK_MGT	0x058
-#define PRCM_APEATCLK_MGT	0x05C
-#define PRCM_APETRACECLK_MGT	0x060
-#define PRCM_MCDECLK_MGT	0x064
-#define PRCM_IPI2CCLK_MGT	0x068
-#define PRCM_DSIALTCLK_MGT	0x06C
-#define PRCM_DMACLK_MGT		0x074
-#define PRCM_B2R2CLK_MGT	0x078
-#define PRCM_TVCLK_MGT		0x07C
-#define PRCM_TCR		0x1C8
-#define PRCM_TCR_STOPPED	(1 << 16)
-#define PRCM_TCR_DOZE_MODE	(1 << 17)
-#define PRCM_UNIPROCLK_MGT	0x278
-#define PRCM_SSPCLK_MGT		0x280
-#define PRCM_RNGCLK_MGT		0x284
-#define PRCM_UICCCLK_MGT	0x27C
-
-#define PRCM_MGT_ENABLE		(1 << 8)
-
-static DEFINE_SPINLOCK(clocks_lock);
-
-static void __clk_enable(struct clk *clk)
-{
-	if (clk->enabled++ == 0) {
-		if (clk->parent_cluster)
-			__clk_enable(clk->parent_cluster);
-
-		if (clk->parent_periph)
-			__clk_enable(clk->parent_periph);
-
-		if (clk->ops && clk->ops->enable)
-			clk->ops->enable(clk);
+#define PRCC_PCKEN 0x0
+#define PRCC_PCKDIS 0x4
+#define PRCC_KCKEN 0x8
+#define PRCC_KCKDIS 0xC
+#define PRCC_PCKSR 0x10
+#define PRCC_KCKSR 0x14
+
+DEFINE_MUTEX(clk_opp100_mutex);
+static DEFINE_SPINLOCK(clk_spin_lock);
+#define NO_LOCK &clk_spin_lock
+
+static void __iomem *prcmu_base;
+
+static void __clk_lock(struct clk *clk, void *last_lock, unsigned long *flags)
+{
+	if (clk->mutex != last_lock) {
+		if (clk->mutex == NULL)
+			spin_lock_irqsave(&clk_spin_lock, *flags);
+		else
+			mutex_lock(clk->mutex);
 	}
 }
 
-int clk_enable(struct clk *clk)
+static void __clk_unlock(struct clk *clk, void *last_lock, unsigned long flags)
+{
+	if (clk->mutex != last_lock) {
+		if (clk->mutex == NULL)
+			spin_unlock_irqrestore(&clk_spin_lock, flags);
+		else
+			mutex_unlock(clk->mutex);
+	}
+}
+
+void __clk_disable(struct clk *clk, void *current_lock)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&clocks_lock, flags);
-	__clk_enable(clk);
-	spin_unlock_irqrestore(&clocks_lock, flags);
+	if (clk == NULL)
+		return;
 
-	return 0;
+	__clk_lock(clk, current_lock, &flags);
+
+	if (clk->enabled && (--clk->enabled == 0)) {
+		if ((clk->ops != NULL) && (clk->ops->disable != NULL))
+			clk->ops->disable(clk);
+		__clk_disable(clk->parent, clk->mutex);
+		__clk_disable(clk->bus_parent, clk->mutex);
+	}
+
+	__clk_unlock(clk, current_lock, flags);
+
+	return;
 }
-EXPORT_SYMBOL(clk_enable);
 
-static void __clk_disable(struct clk *clk)
+int __clk_enable(struct clk *clk, void *current_lock)
 {
-	if (--clk->enabled == 0) {
-		if (clk->ops && clk->ops->disable)
-			clk->ops->disable(clk);
+	int err;
+	unsigned long flags;
+
+	if (clk == NULL)
+		return 0;
+
+	__clk_lock(clk, current_lock, &flags);
+
+	if (!clk->enabled) {
+		err = __clk_enable(clk->bus_parent, clk->mutex);
+		if (unlikely(err))
+			goto bus_parent_error;
 
-		if (clk->parent_periph)
-			__clk_disable(clk->parent_periph);
+		err = __clk_enable(clk->parent, clk->mutex);
+		if (unlikely(err))
+			goto parent_error;
 
-		if (clk->parent_cluster)
-			__clk_disable(clk->parent_cluster);
+		if ((clk->ops != NULL) && (clk->ops->enable != NULL)) {
+			err = clk->ops->enable(clk);
+			if (unlikely(err))
+				goto enable_error;
+		}
 	}
+	clk->enabled++;
+
+	__clk_unlock(clk, current_lock, flags);
+
+	return 0;
+
+enable_error:
+	__clk_disable(clk->parent, clk->mutex);
+parent_error:
+	__clk_disable(clk->bus_parent, clk->mutex);
+bus_parent_error:
+
+	__clk_unlock(clk, current_lock, flags);
+
+	return err;
 }
 
-void clk_disable(struct clk *clk)
+static struct clk *__clk_get_parent(struct clk *clk)
+{
+	if (clk->parent != NULL)
+		return clk->parent;
+	else if ((clk->bus_parent != NULL))
+		return clk->bus_parent;
+
+	return NULL;
+}
+
+unsigned long __clk_get_rate(struct clk *clk, void *current_lock)
 {
+	unsigned long rate;
 	unsigned long flags;
 
-	WARN_ON(!clk->enabled);
+	if (clk == NULL)
+		return 0;
 
-	spin_lock_irqsave(&clocks_lock, flags);
-	__clk_disable(clk);
-	spin_unlock_irqrestore(&clocks_lock, flags);
+	__clk_lock(clk, current_lock, &flags);
+
+	if ((clk->ops != NULL) && (clk->ops->get_rate != NULL))
+		rate = clk->ops->get_rate(clk);
+	else if (clk->rate)
+		rate = clk->rate;
+	else
+		rate = __clk_get_rate(clk->parent, clk->mutex);
+
+	__clk_unlock(clk, current_lock, flags);
+
+	return rate;
 }
-EXPORT_SYMBOL(clk_disable);
 
-/*
- * The MTU has a separate, rather complex muxing setup
- * with alternative parents (peripheral cluster or
- * ULP or fixed 32768 Hz) depending on settings
- */
-static unsigned long clk_mtu_get_rate(struct clk *clk)
+static unsigned long __clk_round_rate(struct clk *clk, unsigned long rate)
 {
-	void __iomem *addr;
-	u32 tcr;
-	int mtu = (int) clk->data;
-	/*
-	 * One of these is selected eventually
-	 * TODO: Replace the constant with a reference
-	 * to the ULP source once this is modeled.
-	 */
-	unsigned long clk32k = 32768;
-	unsigned long mturate;
-	unsigned long retclk;
-
-	if (cpu_is_u5500())
-		addr = __io_address(U5500_PRCMU_BASE);
-	else if (cpu_is_u8500())
-		addr = __io_address(U8500_PRCMU_BASE);
-	else
-		ux500_unknown_soc();
+	if ((clk->ops != NULL) && (clk->ops->round_rate != NULL))
+		return clk->ops->round_rate(clk, rate);
 
-	/*
-	 * On a startup, always conifgure the TCR to the doze mode;
-	 * bootloaders do it for us. Do this in the kernel too.
-	 */
-	writel(PRCM_TCR_DOZE_MODE, addr + PRCM_TCR);
+	return -ENOSYS;
+}
 
-	tcr = readl(addr + PRCM_TCR);
+static int __clk_set_rate(struct clk *clk, unsigned long rate)
+{
+	if ((clk->ops != NULL) && (clk->ops->set_rate != NULL))
+		return clk->ops->set_rate(clk, rate);
 
-	/* Get the rate from the parent as a default */
-	if (clk->parent_periph)
-		mturate = clk_get_rate(clk->parent_periph);
-	else if (clk->parent_cluster)
-		mturate = clk_get_rate(clk->parent_cluster);
-	else
-		/* We need to be connected SOMEWHERE */
-		BUG();
+	return -ENOSYS;
+}
 
-	/* Return the clock selected for this MTU */
-	if (tcr & (1 << mtu))
-		retclk = clk32k;
-	else
-		retclk = mturate;
+int clk_enable(struct clk *clk)
+{
+	if (clk == NULL)
+		return -EINVAL;
 
-	pr_info("MTU%d clock rate: %lu Hz\n", mtu, retclk);
-	return retclk;
+	return __clk_enable(clk, NO_LOCK);
 }
+EXPORT_SYMBOL(clk_enable);
 
-unsigned long clk_get_rate(struct clk *clk)
+void clk_disable(struct clk *clk)
 {
-	unsigned long rate;
 
-	/*
-	 * If there is a custom getrate callback for this clock,
-	 * it will take precedence.
-	 */
-	if (clk->get_rate)
-		return clk->get_rate(clk);
-
-	if (clk->ops && clk->ops->get_rate)
-		return clk->ops->get_rate(clk);
-
-	rate = clk->rate;
-	if (!rate) {
-		if (clk->parent_periph)
-			rate = clk_get_rate(clk->parent_periph);
-		else if (clk->parent_cluster)
-			rate = clk_get_rate(clk->parent_cluster);
-	}
+	if (clk == NULL)
+		return;
 
-	return rate;
+	WARN_ON(!clk->enabled);
+	__clk_disable(clk, NO_LOCK);
+}
+EXPORT_SYMBOL(clk_disable);
+
+struct clk *clk_get_parent(struct clk *clk)
+{
+	if (clk == NULL)
+		return ERR_PTR(-EINVAL);
+
+	return __clk_get_parent(clk);
+}
+EXPORT_SYMBOL(clk_get_parent);
+
+unsigned long clk_get_rate(struct clk *clk)
+{
+	if (clk == NULL)
+		return 0;
+
+	return __clk_get_rate(clk, NO_LOCK);
 }
 EXPORT_SYMBOL(clk_get_rate);
 
 long clk_round_rate(struct clk *clk, unsigned long rate)
 {
-	/*TODO*/
+	unsigned long flags;
+
+	if (clk == NULL)
+		return -EINVAL;
+
+	__clk_lock(clk, NO_LOCK, &flags);
+
+	rate = __clk_round_rate(clk, rate);
+
+	__clk_unlock(clk, NO_LOCK, flags);
+
 	return rate;
 }
 EXPORT_SYMBOL(clk_round_rate);
 
 int clk_set_rate(struct clk *clk, unsigned long rate)
 {
-	clk->rate = rate;
-	return 0;
+	int err;
+	unsigned long flags;
+
+	if (clk == NULL)
+		return -EINVAL;
+
+	__clk_lock(clk, NO_LOCK, &flags);
+
+	err =  __clk_set_rate(clk, rate);
+
+	__clk_unlock(clk, NO_LOCK, flags);
+
+	return err;
 }
 EXPORT_SYMBOL(clk_set_rate);
 
-static void clk_prcmu_enable(struct clk *clk)
+int clk_set_parent(struct clk *clk, struct clk *parent)
 {
-	void __iomem *cg_set_reg = __io_address(U8500_PRCMU_BASE)
-				   + PRCM_YYCLKEN0_MGT_SET + clk->prcmu_cg_off;
+	int err = 0;
+	unsigned long flags;
+	struct clk **p;
+
+	if ((clk == NULL) || (clk->parents == NULL))
+		return -EINVAL;
+	for (p = clk->parents; *p != parent; p++) {
+		if (*p == NULL) /* invalid parent */
+			return -EINVAL;
+	}
+
+	__clk_lock(clk, NO_LOCK, &flags);
+
+	if ((clk->ops != NULL) && (clk->ops->set_parent != NULL)) {
+		err = clk->ops->set_parent(clk, parent);
+		if (err)
+			goto unlock_and_return;
+	} else if (clk->enabled) {
+		err = __clk_enable(parent, clk->mutex);
+		if (err)
+			goto unlock_and_return;
+		__clk_disable(clk->parent, clk->mutex);
+	}
+
+	clk->parent = parent;
+
+unlock_and_return:
+	__clk_unlock(clk, NO_LOCK, flags);
 
-	writel(1 << clk->prcmu_cg_bit, cg_set_reg);
+	return err;
 }
 
-static void clk_prcmu_disable(struct clk *clk)
-{
-	void __iomem *cg_clr_reg = __io_address(U8500_PRCMU_BASE)
-				   + PRCM_YYCLKEN0_MGT_CLR + clk->prcmu_cg_off;
+/* PRCMU clock operations. */
 
-	writel(1 << clk->prcmu_cg_bit, cg_clr_reg);
+static int prcmu_clk_enable(struct clk *clk)
+{
+	return prcmu_request_clock(clk->cg_sel, true);
 }
 
-/* ED doesn't have the combined set/clr registers */
-static void clk_prcmu_ed_enable(struct clk *clk)
+static void prcmu_clk_disable(struct clk *clk)
 {
-	void __iomem *addr = __io_address(U8500_PRCMU_BASE)
-			     + clk->prcmu_cg_mgt;
+	if (prcmu_request_clock(clk->cg_sel, false)) {
+		pr_err("clock: %s failed to disable %s.\n", __func__,
+			clk->name);
+	}
+}
 
-	writel(readl(addr) | PRCM_MGT_ENABLE, addr);
+static int request_ape_opp100(bool enable)
+{
+	static unsigned int requests;
+
+	if (enable) {
+	       if (0 == requests++) {
+		       return prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP,
+			       "clock", 100);
+	       }
+	} else if (1 == requests--) {
+		prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP, "clock");
+	}
+	return 0;
 }
 
-static void clk_prcmu_ed_disable(struct clk *clk)
+static int prcmu_opp100_clk_enable(struct clk *clk)
 {
-	void __iomem *addr = __io_address(U8500_PRCMU_BASE)
-			     + clk->prcmu_cg_mgt;
+	int r;
+
+	r = request_ape_opp100(true);
+	if (r) {
+		pr_err("clock: %s failed to request APE OPP 100%% for %s.\n",
+			__func__, clk->name);
+		return r;
+	}
+	return prcmu_request_clock(clk->cg_sel, true);
+}
 
-	writel(readl(addr) & ~PRCM_MGT_ENABLE, addr);
+static void prcmu_opp100_clk_disable(struct clk *clk)
+{
+	if (prcmu_request_clock(clk->cg_sel, false))
+		goto out_error;
+	if (request_ape_opp100(false))
+		goto out_error;
+	return;
+
+out_error:
+	pr_err("clock: %s failed to disable %s.\n", __func__, clk->name);
 }
 
-static struct clkops clk_prcmu_ops = {
-	.enable = clk_prcmu_enable,
-	.disable = clk_prcmu_disable,
+struct clkops prcmu_clk_ops = {
+	.enable = prcmu_clk_enable,
+	.disable = prcmu_clk_disable,
 };
 
-static unsigned int clkrst_base[] = {
-	[1] = U8500_CLKRST1_BASE,
-	[2] = U8500_CLKRST2_BASE,
-	[3] = U8500_CLKRST3_BASE,
-	[5] = U8500_CLKRST5_BASE,
-	[6] = U8500_CLKRST6_BASE,
-	[7] = U8500_CLKRST7_BASE_ED,
+struct clkops prcmu_opp100_clk_ops = {
+	.enable = prcmu_opp100_clk_enable,
+	.disable = prcmu_opp100_clk_disable,
 };
 
-static void clk_prcc_enable(struct clk *clk)
-{
-	void __iomem *addr = __io_address(clkrst_base[clk->cluster]);
+/* PRCC clock operations. */
 
-	if (clk->prcc_kernel != -1)
-		writel(1 << clk->prcc_kernel, addr + PRCC_KCKEN);
+static int prcc_pclk_enable(struct clk *clk)
+{
+	void __iomem *io_base = __io_address(clk->io_base);
 
-	if (clk->prcc_bus != -1)
-		writel(1 << clk->prcc_bus, addr + PRCC_PCKEN);
+	writel(clk->cg_sel, (io_base + PRCC_PCKEN));
+	while (!(readl(io_base + PRCC_PCKSR) & clk->cg_sel))
+		cpu_relax();
+	return 0;
 }
 
-static void clk_prcc_disable(struct clk *clk)
+static void prcc_pclk_disable(struct clk *clk)
 {
-	void __iomem *addr = __io_address(clkrst_base[clk->cluster]);
-
-	if (clk->prcc_bus != -1)
-		writel(1 << clk->prcc_bus, addr + PRCC_PCKDIS);
+	void __iomem *io_base = __io_address(clk->io_base);
 
-	if (clk->prcc_kernel != -1)
-		writel(1 << clk->prcc_kernel, addr + PRCC_KCKDIS);
+	writel(clk->cg_sel, (io_base + PRCC_PCKDIS));
 }
 
-static struct clkops clk_prcc_ops = {
-	.enable = clk_prcc_enable,
-	.disable = clk_prcc_disable,
+struct clkops prcc_pclk_ops = {
+	.enable = prcc_pclk_enable,
+	.disable = prcc_pclk_disable,
 };
 
-static struct clk clk_32khz = {
-	.name =  "clk_32khz",
-	.rate = 32000,
-};
+static int prcc_kclk_enable(struct clk *clk)
+{
+	int err;
+	void __iomem *io_base = __io_address(clk->io_base);
 
-/*
- * PRCMU level clock gating
- */
+	err = __clk_enable(clk->clock, clk->mutex);
+	if (err)
+		return err;
 
-/* Bank 0 */
-static DEFINE_PRCMU_CLK(svaclk,		0x0, 2, SVAMMDSPCLK);
-static DEFINE_PRCMU_CLK(siaclk,		0x0, 3, SIAMMDSPCLK);
-static DEFINE_PRCMU_CLK(sgaclk,		0x0, 4, SGACLK);
-static DEFINE_PRCMU_CLK_RATE(uartclk,	0x0, 5, UARTCLK, 38400000);
-static DEFINE_PRCMU_CLK(msp02clk,	0x0, 6, MSP02CLK);
-static DEFINE_PRCMU_CLK(msp1clk,	0x0, 7, MSP1CLK); /* v1 */
-static DEFINE_PRCMU_CLK_RATE(i2cclk,	0x0, 8, I2CCLK, 48000000);
-static DEFINE_PRCMU_CLK_RATE(sdmmcclk,	0x0, 9, SDMMCCLK, 100000000);
-static DEFINE_PRCMU_CLK(slimclk,	0x0, 10, SLIMCLK);
-static DEFINE_PRCMU_CLK(per1clk,	0x0, 11, PER1CLK);
-static DEFINE_PRCMU_CLK(per2clk,	0x0, 12, PER2CLK);
-static DEFINE_PRCMU_CLK(per3clk,	0x0, 13, PER3CLK);
-static DEFINE_PRCMU_CLK(per5clk,	0x0, 14, PER5CLK);
-static DEFINE_PRCMU_CLK_RATE(per6clk,	0x0, 15, PER6CLK, 133330000);
-static DEFINE_PRCMU_CLK_RATE(per7clk,	0x0, 16, PER7CLK, 100000000);
-static DEFINE_PRCMU_CLK(lcdclk,		0x0, 17, LCDCLK);
-static DEFINE_PRCMU_CLK(bmlclk,		0x0, 18, BMLCLK);
-static DEFINE_PRCMU_CLK(hsitxclk,	0x0, 19, HSITXCLK);
-static DEFINE_PRCMU_CLK(hsirxclk,	0x0, 20, HSIRXCLK);
-static DEFINE_PRCMU_CLK(hdmiclk,	0x0, 21, HDMICLK);
-static DEFINE_PRCMU_CLK(apeatclk,	0x0, 22, APEATCLK);
-static DEFINE_PRCMU_CLK(apetraceclk,	0x0, 23, APETRACECLK);
-static DEFINE_PRCMU_CLK(mcdeclk,	0x0, 24, MCDECLK);
-static DEFINE_PRCMU_CLK(ipi2clk,	0x0, 25, IPI2CCLK);
-static DEFINE_PRCMU_CLK(dsialtclk,	0x0, 26, DSIALTCLK); /* v1 */
-static DEFINE_PRCMU_CLK(dmaclk,		0x0, 27, DMACLK);
-static DEFINE_PRCMU_CLK(b2r2clk,	0x0, 28, B2R2CLK);
-static DEFINE_PRCMU_CLK(tvclk,		0x0, 29, TVCLK);
-static DEFINE_PRCMU_CLK(uniproclk,	0x0, 30, UNIPROCLK); /* v1 */
-static DEFINE_PRCMU_CLK_RATE(sspclk,	0x0, 31, SSPCLK, 48000000); /* v1 */
-
-/* Bank 1 */
-static DEFINE_PRCMU_CLK(rngclk,		0x4, 0, RNGCLK); /* v1 */
-static DEFINE_PRCMU_CLK(uiccclk,	0x4, 1, UICCCLK); /* v1 */
+	writel(clk->cg_sel, (io_base + PRCC_KCKEN));
+	while (!(readl(io_base + PRCC_KCKSR) & clk->cg_sel))
+		cpu_relax();
 
-/*
- * PRCC level clock gating
- * Format: per#, clk, PCKEN bit, KCKEN bit, parent
- */
+	__clk_disable(clk->clock, clk->mutex);
 
-/* Peripheral Cluster #1 */
-static DEFINE_PRCC_CLK(1, i2c4,		10, 9, &clk_i2cclk);
-static DEFINE_PRCC_CLK(1, gpio0,	9, -1, NULL);
-static DEFINE_PRCC_CLK(1, slimbus0,	8,  8, &clk_slimclk);
-static DEFINE_PRCC_CLK(1, spi3_ed,	7,  7, NULL);
-static DEFINE_PRCC_CLK(1, spi3_v1,	7, -1, NULL);
-static DEFINE_PRCC_CLK(1, i2c2,		6,  6, &clk_i2cclk);
-static DEFINE_PRCC_CLK(1, sdi0,		5,  5, &clk_sdmmcclk);
-static DEFINE_PRCC_CLK(1, msp1_ed,	4,  4, &clk_msp02clk);
-static DEFINE_PRCC_CLK(1, msp1_v1,	4,  4, &clk_msp1clk);
-static DEFINE_PRCC_CLK(1, msp0,		3,  3, &clk_msp02clk);
-static DEFINE_PRCC_CLK(1, i2c1,		2,  2, &clk_i2cclk);
-static DEFINE_PRCC_CLK(1, uart1,	1,  1, &clk_uartclk);
-static DEFINE_PRCC_CLK(1, uart0,	0,  0, &clk_uartclk);
-
-/* Peripheral Cluster #2 */
-
-static DEFINE_PRCC_CLK(2, gpio1_ed,	12, -1, NULL);
-static DEFINE_PRCC_CLK(2, ssitx_ed,	11, -1, NULL);
-static DEFINE_PRCC_CLK(2, ssirx_ed,	10, -1, NULL);
-static DEFINE_PRCC_CLK(2, spi0_ed,	 9, -1, NULL);
-static DEFINE_PRCC_CLK(2, sdi3_ed,	 8,  6, &clk_sdmmcclk);
-static DEFINE_PRCC_CLK(2, sdi1_ed,	 7,  5, &clk_sdmmcclk);
-static DEFINE_PRCC_CLK(2, msp2_ed,	 6,  4, &clk_msp02clk);
-static DEFINE_PRCC_CLK(2, sdi4_ed,	 4,  2, &clk_sdmmcclk);
-static DEFINE_PRCC_CLK(2, pwl_ed,	 3,  1, NULL);
-static DEFINE_PRCC_CLK(2, spi1_ed,	 2, -1, NULL);
-static DEFINE_PRCC_CLK(2, spi2_ed,	 1, -1, NULL);
-static DEFINE_PRCC_CLK(2, i2c3_ed,	 0,  0, &clk_i2cclk);
-
-static DEFINE_PRCC_CLK(2, gpio1_v1,	11, -1, NULL);
-static DEFINE_PRCC_CLK(2, ssitx_v1,	10,  7, NULL);
-static DEFINE_PRCC_CLK(2, ssirx_v1,	 9,  6, NULL);
-static DEFINE_PRCC_CLK(2, spi0_v1,	 8, -1, NULL);
-static DEFINE_PRCC_CLK(2, sdi3_v1,	 7,  5, &clk_sdmmcclk);
-static DEFINE_PRCC_CLK(2, sdi1_v1,	 6,  4, &clk_sdmmcclk);
-static DEFINE_PRCC_CLK(2, msp2_v1,	 5,  3, &clk_msp02clk);
-static DEFINE_PRCC_CLK(2, sdi4_v1,	 4,  2, &clk_sdmmcclk);
-static DEFINE_PRCC_CLK(2, pwl_v1,	 3,  1, NULL);
-static DEFINE_PRCC_CLK(2, spi1_v1,	 2, -1, NULL);
-static DEFINE_PRCC_CLK(2, spi2_v1,	 1, -1, NULL);
-static DEFINE_PRCC_CLK(2, i2c3_v1,	 0,  0, &clk_i2cclk);
-
-/* Peripheral Cluster #3 */
-static DEFINE_PRCC_CLK(3, gpio2,	8, -1, NULL);
-static DEFINE_PRCC_CLK(3, sdi5,		7,  7, &clk_sdmmcclk);
-static DEFINE_PRCC_CLK(3, uart2,	6,  6, &clk_uartclk);
-static DEFINE_PRCC_CLK(3, ske,		5,  5, &clk_32khz);
-static DEFINE_PRCC_CLK(3, sdi2,		4,  4, &clk_sdmmcclk);
-static DEFINE_PRCC_CLK(3, i2c0,		3,  3, &clk_i2cclk);
-static DEFINE_PRCC_CLK(3, ssp1_ed,	2,  2, &clk_i2cclk);
-static DEFINE_PRCC_CLK(3, ssp0_ed,	1,  1, &clk_i2cclk);
-static DEFINE_PRCC_CLK(3, ssp1_v1,	2,  2, &clk_sspclk);
-static DEFINE_PRCC_CLK(3, ssp0_v1,	1,  1, &clk_sspclk);
-static DEFINE_PRCC_CLK(3, fsmc,		0, -1, NULL);
-
-/* Peripheral Cluster #4 is in the always on domain */
-
-/* Peripheral Cluster #5 */
-static DEFINE_PRCC_CLK(5, gpio3,	1, -1, NULL);
-static DEFINE_PRCC_CLK(5, usb_ed,	0,  0, &clk_i2cclk);
-static DEFINE_PRCC_CLK(5, usb_v1,	0,  0, NULL);
-
-/* Peripheral Cluster #6 */
-
-/* MTU ID in data */
-static DEFINE_PRCC_CLK_CUSTOM(6, mtu1_v1, 8, -1, NULL, clk_mtu_get_rate, 1);
-static DEFINE_PRCC_CLK_CUSTOM(6, mtu0_v1, 7, -1, NULL, clk_mtu_get_rate, 0);
-static DEFINE_PRCC_CLK(6, cfgreg_v1,	6,  6, NULL);
-static DEFINE_PRCC_CLK(6, dmc_ed,	6,  6, NULL);
-static DEFINE_PRCC_CLK(6, hash1,	5, -1, NULL);
-static DEFINE_PRCC_CLK(6, unipro_v1,	4,  1, &clk_uniproclk);
-static DEFINE_PRCC_CLK(6, cryp1_ed,	4, -1, NULL);
-static DEFINE_PRCC_CLK(6, pka,		3, -1, NULL);
-static DEFINE_PRCC_CLK(6, hash0,	2, -1, NULL);
-static DEFINE_PRCC_CLK(6, cryp0,	1, -1, NULL);
-static DEFINE_PRCC_CLK(6, rng_ed,	0,  0, &clk_i2cclk);
-static DEFINE_PRCC_CLK(6, rng_v1,	0,  0, &clk_rngclk);
-
-/* Peripheral Cluster #7 */
-
-static DEFINE_PRCC_CLK(7, tzpc0_ed,	4, -1, NULL);
-/* MTU ID in data */
-static DEFINE_PRCC_CLK_CUSTOM(7, mtu1_ed, 3, -1, NULL, clk_mtu_get_rate, 1);
-static DEFINE_PRCC_CLK_CUSTOM(7, mtu0_ed, 2, -1, NULL, clk_mtu_get_rate, 0);
-static DEFINE_PRCC_CLK(7, wdg_ed,	1, -1, NULL);
-static DEFINE_PRCC_CLK(7, cfgreg_ed,	0, -1, NULL);
-
-static struct clk clk_dummy_apb_pclk = {
-	.name = "apb_pclk",
-};
+	return 0;
+}
 
-static struct clk_lookup u8500_common_clks[] = {
-	CLK(dummy_apb_pclk, NULL,	"apb_pclk"),
-
-	/* Peripheral Cluster #1 */
-	CLK(gpio0,	"gpio.0",	NULL),
-	CLK(gpio0,	"gpio.1",	NULL),
-	CLK(slimbus0,	"slimbus0",	NULL),
-	CLK(i2c2,	"nmk-i2c.2",	NULL),
-	CLK(sdi0,	"sdi0",		NULL),
-	CLK(msp0,	"msp0",		NULL),
-	CLK(i2c1,	"nmk-i2c.1",	NULL),
-	CLK(uart1,	"uart1",	NULL),
-	CLK(uart0,	"uart0",	NULL),
-
-	/* Peripheral Cluster #3 */
-	CLK(gpio2,	"gpio.2",	NULL),
-	CLK(gpio2,	"gpio.3",	NULL),
-	CLK(gpio2,	"gpio.4",	NULL),
-	CLK(gpio2,	"gpio.5",	NULL),
-	CLK(sdi5,	"sdi5",		NULL),
-	CLK(uart2,	"uart2",	NULL),
-	CLK(ske,	"ske",		NULL),
-	CLK(ske,	"nmk-ske-keypad",	NULL),
-	CLK(sdi2,	"sdi2",		NULL),
-	CLK(i2c0,	"nmk-i2c.0",	NULL),
-	CLK(fsmc,	"fsmc",		NULL),
-
-	/* Peripheral Cluster #5 */
-	CLK(gpio3,	"gpio.8",	NULL),
-
-	/* Peripheral Cluster #6 */
-	CLK(hash1,	"hash1",	NULL),
-	CLK(pka,	"pka",		NULL),
-	CLK(hash0,	"hash0",	NULL),
-	CLK(cryp0,	"cryp0",	NULL),
-
-	/* PRCMU level clock gating */
-
-	/* Bank 0 */
-	CLK(svaclk,	"sva",		NULL),
-	CLK(siaclk,	"sia",		NULL),
-	CLK(sgaclk,	"sga",		NULL),
-	CLK(slimclk,	"slim",		NULL),
-	CLK(lcdclk,	"lcd",		NULL),
-	CLK(bmlclk,	"bml",		NULL),
-	CLK(hsitxclk,	"stm-hsi.0",	NULL),
-	CLK(hsirxclk,	"stm-hsi.1",	NULL),
-	CLK(hdmiclk,	"hdmi",		NULL),
-	CLK(apeatclk,	"apeat",	NULL),
-	CLK(apetraceclk,	"apetrace",	NULL),
-	CLK(mcdeclk,	"mcde",		NULL),
-	CLK(ipi2clk,	"ipi2",		NULL),
-	CLK(dmaclk,	"dma40.0",	NULL),
-	CLK(b2r2clk,	"b2r2",		NULL),
-	CLK(tvclk,	"tv",		NULL),
-};
+static void prcc_kclk_disable(struct clk *clk)
+{
+	void __iomem *io_base = __io_address(clk->io_base);
 
-static struct clk_lookup u8500_ed_clks[] = {
-	/* Peripheral Cluster #1 */
-	CLK(spi3_ed,	"spi3",		NULL),
-	CLK(msp1_ed,	"msp1",		NULL),
-
-	/* Peripheral Cluster #2 */
-	CLK(gpio1_ed,	"gpio.6",	NULL),
-	CLK(gpio1_ed,	"gpio.7",	NULL),
-	CLK(ssitx_ed,	"ssitx",	NULL),
-	CLK(ssirx_ed,	"ssirx",	NULL),
-	CLK(spi0_ed,	"spi0",		NULL),
-	CLK(sdi3_ed,	"sdi3",		NULL),
-	CLK(sdi1_ed,	"sdi1",		NULL),
-	CLK(msp2_ed,	"msp2",		NULL),
-	CLK(sdi4_ed,	"sdi4",		NULL),
-	CLK(pwl_ed,	"pwl",		NULL),
-	CLK(spi1_ed,	"spi1",		NULL),
-	CLK(spi2_ed,	"spi2",		NULL),
-	CLK(i2c3_ed,	"nmk-i2c.3",	NULL),
-
-	/* Peripheral Cluster #3 */
-	CLK(ssp1_ed,	"ssp1",		NULL),
-	CLK(ssp0_ed,	"ssp0",		NULL),
-
-	/* Peripheral Cluster #5 */
-	CLK(usb_ed,	"musb-ux500.0",	"usb"),
-
-	/* Peripheral Cluster #6 */
-	CLK(dmc_ed,	"dmc",		NULL),
-	CLK(cryp1_ed,	"cryp1",	NULL),
-	CLK(rng_ed,	"rng",		NULL),
-
-	/* Peripheral Cluster #7 */
-	CLK(tzpc0_ed,	"tzpc0",	NULL),
-	CLK(mtu1_ed,	"mtu1",		NULL),
-	CLK(mtu0_ed,	"mtu0",		NULL),
-	CLK(wdg_ed,	"wdg",		NULL),
-	CLK(cfgreg_ed,	"cfgreg",	NULL),
-};
+	(void)__clk_enable(clk->clock, clk->mutex);
+	writel(clk->cg_sel, (io_base + PRCC_KCKDIS));
+	__clk_disable(clk->clock, clk->mutex);
+}
 
-static struct clk_lookup u8500_v1_clks[] = {
-	/* Peripheral Cluster #1 */
-	CLK(i2c4,	"nmk-i2c.4",	NULL),
-	CLK(spi3_v1,	"spi3",		NULL),
-	CLK(msp1_v1,	"msp1",		NULL),
-
-	/* Peripheral Cluster #2 */
-	CLK(gpio1_v1,	"gpio.6",	NULL),
-	CLK(gpio1_v1,	"gpio.7",	NULL),
-	CLK(ssitx_v1,	"ssitx",	NULL),
-	CLK(ssirx_v1,	"ssirx",	NULL),
-	CLK(spi0_v1,	"spi0",		NULL),
-	CLK(sdi3_v1,	"sdi3",		NULL),
-	CLK(sdi1_v1,	"sdi1",		NULL),
-	CLK(msp2_v1,	"msp2",		NULL),
-	CLK(sdi4_v1,	"sdi4",		NULL),
-	CLK(pwl_v1,	"pwl",		NULL),
-	CLK(spi1_v1,	"spi1",		NULL),
-	CLK(spi2_v1,	"spi2",		NULL),
-	CLK(i2c3_v1,	"nmk-i2c.3",	NULL),
-
-	/* Peripheral Cluster #3 */
-	CLK(ssp1_v1,	"ssp1",		NULL),
-	CLK(ssp0_v1,	"ssp0",		NULL),
-
-	/* Peripheral Cluster #5 */
-	CLK(usb_v1,	"musb-ux500.0",	"usb"),
-
-	/* Peripheral Cluster #6 */
-	CLK(mtu1_v1,	"mtu1",		NULL),
-	CLK(mtu0_v1,	"mtu0",		NULL),
-	CLK(cfgreg_v1,	"cfgreg",	NULL),
-	CLK(hash1,	"hash1",	NULL),
-	CLK(unipro_v1,	"unipro",	NULL),
-	CLK(rng_v1,	"rng",		NULL),
-
-	/* PRCMU level clock gating */
-
-	/* Bank 0 */
-	CLK(uniproclk,	"uniproclk",	NULL),
-	CLK(dsialtclk,	"dsialt",	NULL),
-
-	/* Bank 1 */
-	CLK(rngclk,	"rng",		NULL),
-	CLK(uiccclk,	"uicc",		NULL),
+struct clkops prcc_kclk_ops = {
+	.enable = prcc_kclk_enable,
+	.disable = prcc_kclk_disable,
 };
 
+void clks_register(struct clk_lookup *clks, size_t num)
+{
+	unsigned int i;
+
+	for (i = 0; i < num; i++) {
+		clkdev_add(&clks[i]);
+#ifdef CONFIG_DEBUG_FS
+		/* Check that the clock has not been already registered */
+		if (!(clks[i].clk->list.prev != clks[i].clk->list.next))
+			list_add_tail(&clks[i].clk->list, &clk_list);
+#endif
+	}
+}
+
+int __init clk_init(void)
+{
+	if (cpu_is_u8500()) {
+		prcmu_base = __io_address(U8500_PRCMU_BASE);
+	} else if (cpu_is_u5500()) {
+		prcmu_base = __io_address(U5500_PRCMU_BASE);
+	} else {
+		pr_err("clock: Unknown DB Asic.\n");
+		return -EIO;
+	}
+
+	if (cpu_is_u8500())
+		db8500_clk_init();
+	else if (cpu_is_u5500())
+		db5500_clk_init();
+
+	return 0;
+}
+
 #ifdef CONFIG_DEBUG_FS
 /*
  *	debugfs support to trace clock tree hierarchy and attributes with
@@ -589,16 +440,61 @@ static struct clk_lookup u8500_v1_clks[] = {
  */
 static struct dentry *clk_debugfs_root;
 
-void __init clk_debugfs_add_table(struct clk_lookup *cl, size_t num)
+#ifdef CONFIG_DEBUG_FS_WRITE
+static ssize_t enable_dbg_write(struct file *file, const char __user *buf,
+						 size_t size, loff_t *off)
 {
-	while (num--) {
-		/* Check that the clock has not been already registered */
-		if (!(cl->clk->list.prev != cl->clk->list.next))
-			list_add_tail(&cl->clk->list, &clk_list);
+	struct clk *clk = file->f_dentry->d_inode->i_private;
 
-		cl++;
+	clk_enable(clk);
+	return size;
+}
+
+static ssize_t disable_dbg_write(struct file *file, const char __user *buf,
+						  size_t size, loff_t *off)
+{
+	struct clk *clk = file->f_dentry->d_inode->i_private;
+
+	clk_disable(clk);
+	return size;
+}
+
+static ssize_t rate_dbg_write(struct file *file, const char __user *buf,
+					   size_t size, loff_t *off)
+{
+	struct clk *clk = file->f_dentry->d_inode->i_private;
+	char crate[128];
+	unsigned long rate;
+
+	if (size < (sizeof(crate)-1)) {
+		if (copy_from_user(crate, buf, size))
+			return -EFAULT;
+		crate[size] = 0;
+		if (!strict_strtoul(crate, 10, &rate))
+			clk_set_rate(clk, rate);
+		return size;
+	}
+	return -EINVAL;
+}
+
+static ssize_t parent_dbg_write(struct file *file, const char __user *buf,
+						 size_t size, loff_t *off)
+{
+	struct clk *clk = file->f_dentry->d_inode->i_private;
+	char cname[128];
+	struct clk *pclk;
+
+	if (size < (sizeof(cname) - 1)) {
+		if (copy_from_user(cname, buf, size))
+			return -EFAULT;
+		cname[size] = 0;
+		pclk = clk_get_sys(cname, cname);
+		clk_set_parent(clk, pclk);
+		return size;
 	}
+	return -EINVAL;
 }
+#endif
 
 static ssize_t usecount_dbg_read(struct file *file, char __user *buf,
 						  size_t size, loff_t *off)
@@ -624,14 +520,50 @@ static ssize_t rate_dbg_read(struct file *file, char __user *buf,
 	return simple_read_from_buffer(buf, size, off, crate, len);
 }
 
+static ssize_t parent_dbg_read(struct file *file, char __user *buf,
+						size_t size, loff_t *off)
+{
+	struct clk *clk = file->f_dentry->d_inode->i_private;
+	char cname[128];
+	struct clk *pclk;
+	unsigned int len;
+
+	pclk = clk_get_parent(clk);
+	if (pclk)
+		len = sprintf(cname, "%s\n", pclk->name);
+	else
+		len = sprintf(cname, "No parent\n");
+	return simple_read_from_buffer(buf, size, off, cname, len);
+}
+
+#ifdef CONFIG_DEBUG_FS_WRITE
+static const struct file_operations enable_fops = {
+	.write = enable_dbg_write,
+};
+
+static const struct file_operations disable_fops = {
+	.write = disable_dbg_write,
+};
+#endif
+
 static const struct file_operations usecount_fops = {
 	.read = usecount_dbg_read,
 };
 
 static const struct file_operations set_rate_fops = {
+#ifdef CONFIG_DEBUG_FS_WRITE
+	.write = rate_dbg_write,
+#endif
 	.read = rate_dbg_read,
 };
 
+static const struct file_operations set_parent_fops = {
+#ifdef CONFIG_DEBUG_FS_WRITE
+	.write = parent_dbg_write,
+#endif
+	.read = parent_dbg_read,
+};
+
 static struct dentry *clk_debugfs_register_dir(struct clk *c,
 						struct dentry *p_dentry)
 {
@@ -649,13 +581,36 @@ static struct dentry *clk_debugfs_register_dir(struct clk *c,
 		return NULL;
 
 	d = debugfs_create_file("usecount", S_IRUGO,
-				clk_d, c, &usecount_fops);
+			clk_d, c, &usecount_fops);
+	if (!d)
+		goto err_out;
+#ifdef CONFIG_DEBUG_FS_WRITE
+	d = debugfs_create_file("enable", S_IWUGO,
+			clk_d, c, &enable_fops);
+	if (!d)
+		goto err_out;
+	d = debugfs_create_file("disable", S_IWUGO,
+			clk_d, c, &disable_fops);
+	if (!d)
+		goto err_out;
+	d = debugfs_create_file("rate", S_IRUGO | S_IWUGO,
+			clk_d, c, &set_rate_fops);
 	if (!d)
 		goto err_out;
+	d = debugfs_create_file("parent",  S_IRUGO | S_IWUGO,
+			clk_d, c, &set_parent_fops);
+	if (!d)
+		goto err_out;
+#else
 	d = debugfs_create_file("rate", S_IRUGO,
-				clk_d, c, &set_rate_fops);
+			clk_d, c, &set_rate_fops);
+	if (!d)
+		goto err_out;
+	d = debugfs_create_file("parent",  S_IRUGO,
+			clk_d, c, &set_parent_fops);
 	if (!d)
 		goto err_out;
+#endif
 	/*
 	 * TODO : not currently available in ux500
 	 * d = debugfs_create_x32("flags", S_IRUGO, clk_d, (u32 *)&c->flags);
@@ -684,10 +639,11 @@ static void clk_debugfs_remove_dir(struct dentry *cdentry)
 	return ;
 }
 
+
 static int clk_debugfs_register_one(struct clk *c)
 {
-	struct clk *pa = c->parent_periph;
-	struct clk *bpa = c->parent_cluster;
+	struct clk *pa = c->parent;
+	struct clk *bpa = c->bus_parent;
 
 	if (!(bpa && !pa)) {
 		c->dent = clk_debugfs_register_dir(c,
@@ -711,8 +667,8 @@ static int clk_debugfs_register_one(struct clk *c)
 static int clk_debugfs_register(struct clk *c)
 {
 	int err;
-	struct clk *pa = c->parent_periph;
-	struct clk *bpa = c->parent_cluster;
+	struct clk *pa = c->parent;
+	struct clk *bpa = c->bus_parent;
 
 	if (pa && (!pa->dent && !pa->dent_bus)) {
 		err = clk_debugfs_register(pa);
@@ -759,32 +715,4 @@ err_out:
 late_initcall(clk_debugfs_init);
 #endif /* defined(CONFIG_DEBUG_FS) */
 
-int __init clk_init(void)
-{
-	if (cpu_is_u8500ed()) {
-		clk_prcmu_ops.enable = clk_prcmu_ed_enable;
-		clk_prcmu_ops.disable = clk_prcmu_ed_disable;
-		clk_per6clk.rate = 100000000;
-	} else if (cpu_is_u5500()) {
-		/* Clock tree for U5500 not implemented yet */
-		clk_prcc_ops.enable = clk_prcc_ops.disable = NULL;
-		clk_prcmu_ops.enable = clk_prcmu_ops.disable = NULL;
-		clk_uartclk.rate = 36360000;
-		clk_sdmmcclk.rate = 99900000;
-	}
 
-	clkdev_add_table(u8500_common_clks, ARRAY_SIZE(u8500_common_clks));
-	if (cpu_is_u8500ed())
-		clkdev_add_table(u8500_ed_clks, ARRAY_SIZE(u8500_ed_clks));
-	else
-		clkdev_add_table(u8500_v1_clks, ARRAY_SIZE(u8500_v1_clks));
-
-#ifdef CONFIG_DEBUG_FS
-	clk_debugfs_add_table(u8500_common_clks, ARRAY_SIZE(u8500_common_clks));
-	if (cpu_is_u8500ed())
-		clk_debugfs_add_table(u8500_ed_clks, ARRAY_SIZE(u8500_ed_clks));
-	else
-		clk_debugfs_add_table(u8500_v1_clks, ARRAY_SIZE(u8500_v1_clks));
-#endif
-	return 0;
-}
diff --git a/arch/arm/mach-ux500/clock.h b/arch/arm/mach-ux500/clock.h
index 07449070522..f9daba62a9a 100644
--- a/arch/arm/mach-ux500/clock.h
+++ b/arch/arm/mach-ux500/clock.h
@@ -1,11 +1,59 @@
 /*
- *  Copyright (C) 2010 ST-Ericsson
+ *  Copyright (C) 2010 ST-Ericsson SA
  *  Copyright (C) 2009 STMicroelectronics
  *
  * 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 UX500_CLOCK_H
+#define UX500_CLOCK_H
+
+#include <linux/clkdev.h>
+
+/**
+ * struct clk
+ * @ops:	The hardware specific operations defined for the clock.
+ * @name:	The name of the clock.
+ * @mutex:	The mutex to lock when operating on the clock. %NULL means that
+ *		the common clock spinlock will be used.
+ * @enabled:	A reference counter of the enable requests for the clock.
+ * @opp100:	A flag saying whether the clock is requested to run at the
+ *		OPP 100%% frequency.
+ * @rate:	The frequency of the clock. For scalable and scaling clocks,
+ *		this is the OPP 100%% frequency.
+ * @io_base:	An IO memory base address, meaningful only when considered
+ *		together with the defined @ops.
+ * @cg_sel:	Clock gate selector, meaningful only when considered together
+ *		with the specified @ops.
+ * @parent:	The current (or only) parent clock of the clock.
+ * @bus_parent:	The (optional) auxiliary bus clock "parent" of the clock.
+ * @parents:	A list of the possible parents the clock can have. This should
+ *		be a %NULL-terminated &struct_clk array. Present if and only
+ *		if clk_set_parent() is implemented for the clock.
+ * @regulator:	The regulator needed to have the clock functional, if any.
+ * @clock:	The clock needed to control the clock, if any.
+ */
+struct clk {
+	const struct clkops *ops;
+	const char *name;
+	struct mutex *mutex;
+	unsigned int enabled;
+	bool opp100;
+	unsigned long rate;
+	unsigned int io_base;
+	u32 cg_sel;
+	struct clk *parent;
+	struct clk *bus_parent;
+	struct clk **parents;
+	struct regulator *regulator;
+	struct clk *clock;
+	struct list_head list;
+#if defined(CONFIG_DEBUG_FS)
+	struct dentry		*dent;	/* For visible tree hierarchy */
+	struct dentry		*dent_bus;	/* For visible tree hierarchy */
+#endif
+};
 
 /**
  * struct clkops - ux500 clock operations
@@ -18,134 +66,93 @@
  * NULL, the rate in the struct clk will be used.
  */
 struct clkops {
-	void (*enable) (struct clk *);
-	void (*disable) (struct clk *);
-	unsigned long (*get_rate) (struct clk *);
+	int (*enable)(struct clk *);
+	void (*disable)(struct clk *);
+	unsigned long (*get_rate)(struct clk *);
+	int (*set_rate)(struct clk *, unsigned long);
+	unsigned long (*round_rate)(struct clk *, unsigned long);
+	int (*set_parent)(struct clk *, struct clk *);
 };
 
-/**
- * struct clk - ux500 clock structure
- * @ops:		pointer to clkops struct used to control this clock
- * @name:		name, for debugging
- * @enabled:		refcount. positive if enabled, zero if disabled
- * @get_rate:		custom callback for getting the clock rate
- * @data:		custom per-clock data for example for the get_rate
- *			callback
- * @rate:		fixed rate for clocks which don't implement
- * 			ops->getrate
- * @prcmu_cg_off:	address offset of the combined enable/disable register
- * 			(used on u8500v1)
- * @prcmu_cg_bit:	bit in the combined enable/disable register (used on
- * 			u8500v1)
- * @prcmu_cg_mgt:	address of the enable/disable register (used on
- * 			u8500ed)
- * @cluster:		peripheral cluster number
- * @prcc_bus:		bit for the bus clock in the peripheral's CLKRST
- * @prcc_kernel:	bit for the kernel clock in the peripheral's CLKRST.
- * 			-1 if no kernel clock exists.
- * @parent_cluster:	pointer to parent's cluster clk struct
- * @parent_periph:	pointer to parent's peripheral clk struct
- *
- * Peripherals are organised into clusters, and each cluster has an associated
- * bus clock.  Some peripherals also have a parent peripheral clock.
- *
- * In order to enable a clock for a peripheral, we need to enable:
- * 	(1) the parent cluster (bus) clock at the PRCMU level
- * 	(2) the parent peripheral clock (if any) at the PRCMU level
- * 	(3) the peripheral's bus & kernel clock at the PRCC level
- *
- * (1) and (2) are handled by defining clk structs (DEFINE_PRCMU_CLK) for each
- * of the cluster and peripheral clocks, and hooking these as the parents of
- * the individual peripheral clocks.
- *
- * (3) is handled by specifying the bits in the PRCC control registers required
- * to enable these clocks and modifying them in the ->enable and
- * ->disable callbacks of the peripheral clocks (DEFINE_PRCC_CLK).
- *
- * This structure describes both the PRCMU-level clocks and PRCC-level clocks.
- * The prcmu_* fields are only used for the PRCMU clocks, and the cluster,
- * prcc, and parent pointers are only used for the PRCC-level clocks.
- */
-struct clk {
-	const struct clkops	*ops;
-	const char 		*name;
-	unsigned int		enabled;
-	unsigned long		(*get_rate)(struct clk *);
-	void			*data;
-
-	unsigned long		rate;
-	struct list_head	list;
+extern struct clkops prcmu_clk_ops;
+extern struct clkops prcmu_opp100_clk_ops;
+extern struct mutex clk_opp100_mutex;
+extern struct clkops prcc_pclk_ops;
+extern struct clkops prcc_kclk_ops;
+extern struct clkops sga_clk_ops;
 
-	/* These three are only for PRCMU clks */
-
-	unsigned int		prcmu_cg_off;
-	unsigned int		prcmu_cg_bit;
-	unsigned int		prcmu_cg_mgt;
-
-	/* The rest are only for PRCC clks */
-
-	int			cluster;
-	unsigned int		prcc_bus;
-	unsigned int		prcc_kernel;
-
-	struct clk		*parent_cluster;
-	struct clk		*parent_periph;
-#if defined(CONFIG_DEBUG_FS)
-	struct dentry		*dent;		/* For visible tree hierarchy */
-	struct dentry		*dent_bus;	/* For visible tree hierarchy */
-#endif
-};
-
-#define DEFINE_PRCMU_CLK(_name, _cg_off, _cg_bit, _reg)		\
-struct clk clk_##_name = {					\
-		.name		= #_name,			\
-		.ops    	= &clk_prcmu_ops, 		\
-		.prcmu_cg_off	= _cg_off, 			\
-		.prcmu_cg_bit	= _cg_bit,			\
-		.prcmu_cg_mgt	= PRCM_##_reg##_MGT		\
+/* Define PRCMU Clock */
+#define DEF_PRCMU_CLK(_name, _cg_sel, _rate) \
+	struct clk _name = { \
+		.name = #_name, \
+		.ops = &prcmu_clk_ops, \
+		.cg_sel = _cg_sel, \
+		.rate = _rate, \
 	}
 
-#define DEFINE_PRCMU_CLK_RATE(_name, _cg_off, _cg_bit, _reg, _rate)	\
-struct clk clk_##_name = {						\
-		.name		= #_name,				\
-		.ops    	= &clk_prcmu_ops, 			\
-		.prcmu_cg_off	= _cg_off, 				\
-		.prcmu_cg_bit	= _cg_bit,				\
-		.rate		= _rate,				\
-		.prcmu_cg_mgt	= PRCM_##_reg##_MGT			\
+/* Use this for clocks that are only defined at OPP 100%. */
+#define DEF_PRCMU_OPP100_CLK(_name, _cg_sel, _rate) \
+	struct clk _name = { \
+		.name = #_name, \
+		.ops = &prcmu_opp100_clk_ops, \
+		.cg_sel = _cg_sel, \
+		.rate = _rate, \
+		.mutex = &clk_opp100_mutex, \
 	}
 
-#define DEFINE_PRCC_CLK(_pclust, _name, _bus_en, _kernel_en, _kernclk)	\
-struct clk clk_##_name = {						\
-		.name		= #_name,				\
-		.ops    	= &clk_prcc_ops, 			\
-		.cluster 	= _pclust,				\
-		.prcc_bus 	= _bus_en, 				\
-		.prcc_kernel 	= _kernel_en, 				\
-		.parent_cluster = &clk_per##_pclust##clk,		\
-		.parent_periph 	= _kernclk				\
+/* Define PRCC clock */
+#define DEF_PRCC_PCLK(_name, _io_base, _cg_bit, _parent) \
+	struct clk _name = { \
+		.name = #_name, \
+		.ops = &prcc_pclk_ops, \
+		.io_base = _io_base, \
+		.cg_sel = BIT(_cg_bit), \
+		.parent = _parent, \
 	}
 
-#define DEFINE_PRCC_CLK_CUSTOM(_pclust, _name, _bus_en, _kernel_en, _kernclk, _callback, _data) \
-struct clk clk_##_name = {						\
-		.name		= #_name,				\
-		.ops		= &clk_prcc_ops,			\
-		.cluster	= _pclust,				\
-		.prcc_bus	= _bus_en,				\
-		.prcc_kernel	= _kernel_en,				\
-		.parent_cluster = &clk_per##_pclust##clk,		\
-		.parent_periph	= _kernclk,				\
-		.get_rate	= _callback,				\
-		.data		= (void *) _data			\
+#define DEF_PRCC_KCLK(_name, _io_base, _cg_bit, _parent, _clock) \
+	struct clk _name = { \
+		.name = #_name, \
+		.ops = &prcc_kclk_ops, \
+		.io_base = _io_base, \
+		.cg_sel = BIT(_cg_bit), \
+		.parent = _parent, \
+		.clock = _clock, \
 	}
 
-
-#define CLK(_clk, _devname, _conname)			\
-	{						\
-		.clk	= &clk_##_clk,			\
-		.dev_id	= _devname,			\
-		.con_id = _conname,			\
+#define DEF_PER_CLK(_name, _bus_parent, _parent) \
+	struct clk _name = { \
+		.name = #_name, \
+		.parent = _parent, \
+		.bus_parent = _bus_parent, \
 	}
 
-int __init clk_db8500_ed_fixup(void);
+/* Functions defined in clock.c */
 int __init clk_init(void);
+void clks_register(struct clk_lookup *clks, size_t num);
+int __clk_enable(struct clk *clk, void *current_lock);
+void __clk_disable(struct clk *clk, void *current_lock);
+unsigned long __clk_get_rate(struct clk *clk, void *current_lock);
+
+#ifdef CONFIG_DEBUG_FS
+int dbx500_clk_debug_init(struct clk **clks, int num);
+#else
+static inline int dbx500_clk_debug_init(struct clk **clks, int num)
+{
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_UX500_SOC_DB8500
+int __init db8500_clk_init(void);
+#else
+static inline int db8500_clk_init(void) { return 0; }
+#endif
+
+#ifdef CONFIG_UX500_SOC_DB5500
+int __init db5500_clk_init(void);
+#else
+static inline int db5500_clk_init(void) { return 0; }
+#endif
+
+#endif
diff --git a/arch/arm/mach-ux500/cpu-db5500.c b/arch/arm/mach-ux500/cpu-db5500.c
index c01bc19e3c5..8eac99fa925 100644
--- a/arch/arm/mach-ux500/cpu-db5500.c
+++ b/arch/arm/mach-ux500/cpu-db5500.c
@@ -9,20 +9,22 @@
 #include <linux/amba/bus.h>
 #include <linux/io.h>
 #include <linux/irq.h>
+#include <linux/gpio/nomadik.h>
 
+#include <asm/pmu.h>
 #include <asm/mach/map.h>
 #include <asm/pmu.h>
 
-#include <plat/gpio.h>
+#include <linux/gpio.h>
 
 #include <mach/hardware.h>
 #include <mach/devices.h>
 #include <mach/setup.h>
 #include <mach/irqs.h>
 #include <mach/usb.h>
+#include <mach/ste-dma40-db5500.h>
 
 #include "devices-db5500.h"
-#include "ste-dma40-db5500.h"
 
 static struct map_desc u5500_uart_io_desc[] __initdata = {
 	__IO_DEV_DESC(U5500_UART0_BASE, SZ_4K),
@@ -44,6 +46,12 @@ static struct map_desc u5500_io_desc[] __initdata = {
 	__IO_DEV_DESC(U5500_GPIO3_BASE, SZ_4K),
 	__IO_DEV_DESC(U5500_GPIO4_BASE, SZ_4K),
 	__IO_DEV_DESC(U5500_PRCMU_BASE, SZ_4K),
+	__IO_DEV_DESC(U5500_PRCMU_TCDM_BASE, SZ_4K),
+	__IO_DEV_DESC(U5500_CLKRST1_BASE, SZ_4K),
+	__IO_DEV_DESC(U5500_CLKRST2_BASE, SZ_4K),
+	__IO_DEV_DESC(U5500_CLKRST3_BASE, SZ_4K),
+	__IO_DEV_DESC(U5500_CLKRST5_BASE, SZ_4K),
+	__IO_DEV_DESC(U5500_CLKRST6_BASE, SZ_4K),
 };
 
 static struct resource db5500_pmu_resources[] = {
@@ -150,11 +158,16 @@ static struct platform_device mbox2_device = {
 	.num_resources = ARRAY_SIZE(mbox2_resources),
 };
 
+static struct platform_device db5500_prcmu_device = {
+	.name			= "db5500-prcmu",
+};
+
 static struct platform_device *db5500_platform_devs[] __initdata = {
 	&db5500_pmu_device,
 	&mbox0_device,
 	&mbox1_device,
 	&mbox2_device,
+	&db5500_prcmu_device,
 };
 
 static resource_size_t __initdata db5500_gpio_base[] = {
@@ -192,6 +205,25 @@ void __init u5500_map_io(void)
 	_PRCMU_BASE = __io_address(U5500_PRCMU_BASE);
 }
 
+static void __init db5500_pmu_init(void)
+{
+	struct resource res[] = {
+		[0] = {
+			.start		= IRQ_DB5500_PMU0,
+			.end		= IRQ_DB5500_PMU0,
+			.flags		= IORESOURCE_IRQ,
+		},
+		[1] = {
+			.start		= IRQ_DB5500_PMU1,
+			.end		= IRQ_DB5500_PMU1,
+			.flags		= IORESOURCE_IRQ,
+		},
+	};
+
+	platform_device_register_simple("arm-pmu", ARM_PMU_DEVICE_CPU,
+					res, ARRAY_SIZE(res));
+}
+
 static int usb_db5500_rx_dma_cfg[] = {
 	DB5500_DMA_DEV4_USB_OTG_IEP_1_9,
 	DB5500_DMA_DEV5_USB_OTG_IEP_2_10,
@@ -216,7 +248,12 @@ static int usb_db5500_tx_dma_cfg[] = {
 
 void __init u5500_init_devices(void)
 {
+#ifdef CONFIG_STM_TRACE
+	/* Early init for STM tracing */
+	/* platform_device_register(&u5500_stm_device); */
+#endif
 	db5500_add_gpios();
+	db5500_pmu_init();
 	db5500_dma_init();
 	db5500_add_rtc();
 	db5500_add_usb(usb_db5500_rx_dma_cfg, usb_db5500_tx_dma_cfg);
diff --git a/arch/arm/mach-ux500/cpu-db8500.c b/arch/arm/mach-ux500/cpu-db8500.c
index 4598b06c8c5..fa059ec86e8 100644
--- a/arch/arm/mach-ux500/cpu-db8500.c
+++ b/arch/arm/mach-ux500/cpu-db8500.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2008-2009 ST-Ericsson
+ * Copyright (C) 2008-2009 ST-Ericsson SA
  *
  * Author: Srinidhi KASAGAR <srinidhi.kasagar@stericsson.com>
  *
@@ -14,19 +14,22 @@
 #include <linux/amba/bus.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
-#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
+#include <linux/sys_soc.h>
 
 #include <asm/mach/map.h>
 #include <asm/pmu.h>
 #include <mach/hardware.h>
 #include <mach/setup.h>
 #include <mach/devices.h>
+#include <mach/prcmu.h>
+#include <mach/reboot_reasons.h>
 #include <mach/usb.h>
+#include <mach/ste-dma40-db8500.h>
 
 #include "devices-db8500.h"
-#include "ste-dma40-db8500.h"
 
 /* minimum static i/o mapping required to boot U8500 platforms */
 static struct map_desc u8500_uart_io_desc[] __initdata = {
@@ -40,6 +43,8 @@ static struct map_desc u8500_io_desc[] __initdata = {
 	__IO_DEV_DESC(U8500_L2CC_BASE, SZ_4K),
 	__IO_DEV_DESC(U8500_TWD_BASE, SZ_4K),
 	__IO_DEV_DESC(U8500_MTU0_BASE, SZ_4K),
+	__IO_DEV_DESC(U8500_MTU1_BASE, SZ_4K),
+	__IO_DEV_DESC(U8500_RTC_BASE, SZ_4K),
 	__IO_DEV_DESC(U8500_SCU_BASE, SZ_4K),
 	__IO_DEV_DESC(U8500_BACKUPRAM0_BASE, SZ_8K),
 
@@ -54,19 +59,6 @@ static struct map_desc u8500_io_desc[] __initdata = {
 	__IO_DEV_DESC(U8500_GPIO1_BASE, SZ_4K),
 	__IO_DEV_DESC(U8500_GPIO2_BASE, SZ_4K),
 	__IO_DEV_DESC(U8500_GPIO3_BASE, SZ_4K),
-};
-
-static struct map_desc u8500_ed_io_desc[] __initdata = {
-	__IO_DEV_DESC(U8500_MTU0_BASE_ED, SZ_4K),
-	__IO_DEV_DESC(U8500_CLKRST7_BASE_ED, SZ_8K),
-};
-
-static struct map_desc u8500_v1_io_desc[] __initdata = {
-	__IO_DEV_DESC(U8500_MTU0_BASE, SZ_4K),
-	__IO_DEV_DESC(U8500_PRCMU_TCDM_BASE_V1, SZ_4K),
-};
-
-static struct map_desc u8500_v2_io_desc[] __initdata = {
 	__IO_DEV_DESC(U8500_PRCMU_TCDM_BASE, SZ_4K),
 };
 
@@ -81,13 +73,6 @@ void __init u8500_map_io(void)
 
 	iotable_init(u8500_io_desc, ARRAY_SIZE(u8500_io_desc));
 
-	if (cpu_is_u8500ed())
-		iotable_init(u8500_ed_io_desc, ARRAY_SIZE(u8500_ed_io_desc));
-	else if (cpu_is_u8500v1())
-		iotable_init(u8500_v1_io_desc, ARRAY_SIZE(u8500_v1_io_desc));
-	else if (cpu_is_u8500v2())
-		iotable_init(u8500_v2_io_desc, ARRAY_SIZE(u8500_v2_io_desc));
-
 	_PRCMU_BASE = __io_address(U8500_PRCMU_BASE);
 }
 
@@ -138,6 +123,7 @@ static struct platform_device db8500_prcmu_device = {
 static struct platform_device *platform_devs[] __initdata = {
 	&u8500_dma40_device,
 	&db8500_pmu_device,
+	&ux500_prcmu_wdt_device,
 	&db8500_prcmu_device,
 };
 
@@ -193,8 +179,10 @@ static int usb_db8500_tx_dma_cfg[] = {
  */
 void __init u8500_init_devices(void)
 {
-	if (cpu_is_u8500ed())
-		dma40_u8500ed_fixup();
+#ifdef CONFIG_STM_TRACE
+	/* Early init for STM tracing */
+	platform_device_register(&ux500_stm_device);
+#endif
 
 	db8500_add_rtc();
 	db8500_add_gpios();
@@ -205,3 +193,82 @@ void __init u8500_init_devices(void)
 
 	return ;
 }
+
+#ifdef CONFIG_SYS_SOC
+#define U8500_BB_UID_BASE (U8500_BACKUPRAM1_BASE + 0xFC0)
+#define U8500_BB_UID_LENGTH 5
+
+static ssize_t ux500_get_machine(char *buf, struct sysfs_soc_info *si)
+{
+	return sprintf(buf, "DB%2x00\n", dbx500_id.partnumber);
+}
+
+static ssize_t ux500_get_soc_id(char *buf, struct sysfs_soc_info *si)
+{
+	void __iomem *uid_base;
+	int i;
+	ssize_t sz = 0;
+
+	if (dbx500_id.partnumber == 0x85) {
+		uid_base = __io_address(U8500_BB_UID_BASE);
+		for (i = 0; i < U8500_BB_UID_LENGTH; i++)
+			sz += sprintf(buf + sz, "%08x", readl(uid_base + i * sizeof(u32)));
+		sz += sprintf(buf + sz, "\n");
+	}
+	else {
+		/* Don't know where it is located for U5500 */
+		sz = sprintf(buf, "N/A\n");
+	}
+
+	return sz;
+}
+
+static ssize_t ux500_get_revision(char *buf, struct sysfs_soc_info *si)
+{
+	unsigned int rev = dbx500_id.revision;
+
+	if (rev == 0x01)
+		return sprintf(buf, "%s\n", "ED");
+	else if (rev >= 0xA0)
+		return sprintf(buf, "%d.%d\n" , (rev >> 4) - 0xA + 1, rev & 0xf);
+
+	return sprintf(buf, "%s", "Unknown\n");
+}
+
+static ssize_t ux500_get_process(char *buf, struct sysfs_soc_info *si)
+{
+	if (dbx500_id.process == 0x00)
+		return sprintf(buf, "Standard\n");
+
+	return sprintf(buf, "%02xnm\n", dbx500_id.process);
+}
+
+static ssize_t ux500_get_reset_code(char *buf, struct sysfs_soc_info *si)
+{
+	return sprintf(buf, "0x%04x\n", prcmu_get_reset_code());
+}
+
+static ssize_t ux500_get_reset_reason(char *buf, struct sysfs_soc_info *si)
+{
+	return sprintf(buf, "%s\n",
+		reboot_reason_string(prcmu_get_reset_code()));
+}
+
+static struct sysfs_soc_info soc_info[] = {
+	SYSFS_SOC_ATTR_CALLBACK("machine", ux500_get_machine),
+	SYSFS_SOC_ATTR_VALUE("family", "Ux500"),
+	SYSFS_SOC_ATTR_CALLBACK("soc_id", ux500_get_soc_id),
+	SYSFS_SOC_ATTR_CALLBACK("revision", ux500_get_revision),
+	SYSFS_SOC_ATTR_CALLBACK("process", ux500_get_process),
+	SYSFS_SOC_ATTR_CALLBACK("reset_code", ux500_get_reset_code),
+	SYSFS_SOC_ATTR_CALLBACK("reset_reason", ux500_get_reset_reason),
+};
+
+static int __init ux500_sys_soc_init(void)
+{
+	return register_sysfs_soc(soc_info, ARRAY_SIZE(soc_info));
+}
+
+module_init(ux500_sys_soc_init);
+#endif
+
diff --git a/arch/arm/mach-ux500/cpu.c b/arch/arm/mach-ux500/cpu.c
index 1da23bb87c1..f28f7858040 100644
--- a/arch/arm/mach-ux500/cpu.c
+++ b/arch/arm/mach-ux500/cpu.c
@@ -8,8 +8,7 @@
 #include <linux/platform_device.h>
 #include <linux/io.h>
 #include <linux/clk.h>
-#include <linux/mfd/db8500-prcmu.h>
-#include <linux/mfd/db5500-prcmu.h>
+#include <linux/delay.h>
 
 #include <asm/cacheflush.h>
 #include <asm/hardware/cache-l2x0.h>
@@ -21,6 +20,8 @@
 #include <mach/hardware.h>
 #include <mach/setup.h>
 #include <mach/devices.h>
+#include <mach/prcmu.h>
+#include <mach/reboot_reasons.h>
 
 #include "clock.h"
 
@@ -30,6 +31,19 @@ void __iomem *_PRCMU_BASE;
 static void __iomem *l2x0_base;
 #endif
 
+
+static void ux500_restart(char mode, const char *cmd)
+{
+	unsigned short reset_code;
+
+	reset_code = reboot_reason_code(cmd);
+	prcmu_system_reset(reset_code);
+
+	mdelay(1000);
+	printk("Reboot via PRCMU failed -- System halted\n");
+	while (1);
+}
+
 void __init ux500_init_irq(void)
 {
 	void __iomem *dist_base;
@@ -50,10 +64,8 @@ void __init ux500_init_irq(void)
 	 * Init clocks here so that they are available for system timer
 	 * initialization.
 	 */
-	if (cpu_is_u5500())
-		db5500_prcmu_early_init();
-	if (cpu_is_u8500())
-		prcmu_early_init();
+	prcmu_early_init();
+	arm_pm_restart = ux500_restart;
 	clk_init();
 }
 
@@ -119,30 +131,3 @@ static int ux500_l2x0_init(void)
 }
 early_initcall(ux500_l2x0_init);
 #endif
-
-static void __init ux500_timer_init(void)
-{
-#ifdef CONFIG_LOCAL_TIMERS
-	/* Setup the local timer base */
-	if (cpu_is_u5500())
-		twd_base = __io_address(U5500_TWD_BASE);
-	else if (cpu_is_u8500())
-		twd_base = __io_address(U8500_TWD_BASE);
-	else
-		ux500_unknown_soc();
-#endif
-	if (cpu_is_u5500())
-		mtu_base = __io_address(U5500_MTU0_BASE);
-	else if (cpu_is_u8500ed())
-		mtu_base = __io_address(U8500_MTU0_BASE_ED);
-	else if (cpu_is_u8500())
-		mtu_base = __io_address(U8500_MTU0_BASE);
-	else
-		ux500_unknown_soc();
-
-	nmdk_timer_init();
-}
-
-struct sys_timer ux500_timer = {
-	.init	= ux500_timer_init,
-};
diff --git a/arch/arm/mach-ux500/dcache.c b/arch/arm/mach-ux500/dcache.c
new file mode 100644
index 00000000000..b117d4e8283
--- /dev/null
+++ b/arch/arm/mach-ux500/dcache.c
@@ -0,0 +1,254 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Cache handler integration and data cache helpers.
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/dma-mapping.h>
+
+#include <asm/pgtable.h>
+#include <asm/cacheflush.h>
+#include <asm/outercache.h>
+#include <asm/system.h>
+
+/*
+ * Values are derived from measurements on HREFP_1.1_V32_OM_S10 running
+ * u8500-android-2.2_r1.1_v0.21.
+ *
+ * A lot of time can be spent trying to figure out the perfect breakpoints but
+ * for now I've chosen the following simple way.
+ *
+ * breakpoint = best_case + (worst_case - best_case) * 0.666
+ * The breakpoint is moved slightly towards the worst case because a full
+ * clean/flush affects the entire system so we should be a bit careful.
+ *
+ * BEST CASE:
+ * Best case is that the cache is empty and the system is idling. The case
+ * where the cache contains only targeted data could be better in some cases
+ * but it's hard to do measurements and calculate on that case so I choose the
+ * easier alternative.
+ *
+ * inner_clean_breakpoint = time_2_range_clean_on_empty_cache(
+ *					complete_clean_on_empty_cache_time)
+ * inner_flush_breakpoint = time_2_range_flush_on_empty_cache(
+ *					complete_flush_on_empty_cache_time)
+ *
+ * outer_clean_breakpoint = time_2_range_clean_on_empty_cache(
+ *					complete_clean_on_empty_cache_time)
+ * outer_flush_breakpoint = time_2_range_flush_on_empty_cache(
+ *					complete_flush_on_empty_cache_time)
+ *
+ * WORST CASE:
+ * Worst case is that the cache is filled with dirty non targeted data that
+ * will be used after the synchronization and the system is under heavy load.
+ *
+ * inner_clean_breakpoint = time_2_range_clean_on_empty_cache(
+ *				complete_clean_on_full_cache_time * 1.5)
+ * Times 1.5 because it runs on both cores half the time.
+ * inner_flush_breakpoint = time_2_range_flush_on_empty_cache(
+ *				complete_flush_on_full_cache_time * 1.5 +
+ *					complete_flush_on_full_cache_time / 2)
+ * Plus "complete_flush_on_full_cache_time / 2" because all data has to be read
+ * back, here we assume that both cores can fill their cache simultaneously
+ * (seems to be the case as operations on full and empty inner cache takes
+ * roughly the same amount of time ie the bus to outer is not the bottle neck).
+ *
+ * outer_clean_breakpoint = time_2_range_clean_on_empty_cache(
+ *					complete_clean_on_full_cache_time +
+ *					(complete_clean_on_full_cache_time -
+ *					complete_clean_on_empty_cache_time))
+ * Plus "(complete_flush_on_full_cache_time -
+ * complete_flush_on_empty_cache_time)" because no one else can work when we
+ * hog the bus with our unecessary transfer.
+ * outer_flush_breakpoint = time_2_range_flush_on_empty_cache(
+ *					complete_flush_on_full_cache_time * 2 +
+ *					(complete_flush_on_full_cache_time -
+ *				complete_flush_on_empty_cache_time) * 2)
+ *
+ * These values might have to be updated if changes are made to the CPU, L2$,
+ * memory bus or memory.
+ */
+/* 28930 */
+static const u32 inner_clean_breakpoint = 21324 + (32744 - 21324) * 0.666;
+/* 36224 */
+static const u32 inner_flush_breakpoint = 21324 + (43697 - 21324) * 0.666;
+/* 254069 */
+static const u32 outer_clean_breakpoint = 68041 + (347363 - 68041) * 0.666;
+/* 485414 */
+static const u32 outer_flush_breakpoint = 68041 + (694727 - 68041) * 0.666;
+
+static void __clean_inner_dcache_all(void *param);
+static void clean_inner_dcache_all(void);
+
+static void __flush_inner_dcache_all(void *param);
+static void flush_inner_dcache_all(void);
+
+static bool is_cache_exclusive(void);
+
+void drain_cpu_write_buf(void)
+{
+	dsb();
+	outer_cache.sync();
+}
+
+void clean_cpu_dcache(void *vaddr, u32 paddr, u32 length, bool inner_only,
+						bool *cleaned_everything)
+{
+	/*
+	 * There is no problem with exclusive caches here as the Cortex-A9
+	 * documentation (8.1.4. Exclusive L2 cache) says that when a dirty
+	 * line is moved from L2 to L1 it is first written to mem. Because
+	 * of this there is no way a line can avoid the clean by jumping
+	 * between the cache levels.
+	 */
+	*cleaned_everything = true;
+
+	if (length < inner_clean_breakpoint) {
+		/* Inner clean range */
+		dmac_map_area(vaddr, length, DMA_TO_DEVICE);
+		*cleaned_everything = false;
+	} else {
+		clean_inner_dcache_all();
+	}
+
+	if (!inner_only) {
+		/*
+		 * There is currently no outer_cache.clean_all() so we use
+		 * flush instead, which is ok as clean is a subset of flush.
+		 * Clean range and flush range take the same amount of time
+		 * so we can use outer_flush_breakpoint here.
+		 */
+		if (length < outer_flush_breakpoint) {
+			outer_cache.clean_range(paddr, paddr + length);
+			*cleaned_everything = false;
+		} else {
+			outer_cache.flush_all();
+		}
+	}
+}
+
+void flush_cpu_dcache(void *vaddr, u32 paddr, u32 length, bool inner_only,
+						bool *flushed_everything)
+{
+	/*
+	 * There might still be stale data in the caches after this call if the
+	 * cache levels are exclusive. The follwing can happen.
+	 * 1. Clean L1 moves the data to L2.
+	 * 2. Speculative prefetch, preemption or loads on the other core moves
+	 * all the data back to L1, any dirty data will be written to mem as a
+	 * result of this.
+	 * 3. Flush L2 does nothing as there is no targeted data in L2.
+	 * 4. Flush L1 moves the data to L2. Notice that this does not happen
+	 * when the cache levels are non-exclusive as clean pages are not
+	 * written to L2 in that case.
+	 * 5. Stale data is still present in L2!
+	 * I see two possible solutions, don't use exclusive caches or
+	 * (temporarily) disable prefetching to L1, preeemption and the other
+	 * core.
+	 *
+	 * A situation can occur where the operation does not seem atomic from
+	 * the other core's point of view, even on a non-exclusive cache setup.
+	 * Replace step 2 in the previous scenarion with a write from the other
+	 * core. The other core will write on top of the old data but the
+	 * result will not be written to memory. One would expect either that
+	 * the write was performed on top of the old data and was written to
+	 * memory (the write occured before the flush) or that the write was
+	 * performed on top of the new data and was not written to memory (the
+	 * write occured after the flush). The same problem can occur with one
+	 * core if kernel preemption is enabled. The solution is to
+	 * (temporarily) disable the other core and preemption. I can't think
+	 * of any situation where this would be a problem and disabling the
+	 * other core for the duration of this call is mighty expensive so for
+	 * now I just ignore the problem.
+	 */
+
+	*flushed_everything = true;
+
+	if (!inner_only) {
+		/*
+		 * Beautiful solution for the exclusive problems :)
+		 */
+		if (is_cache_exclusive())
+			panic("%s can't handle exclusive CPU caches\n",
+								__func__);
+
+		if (length < inner_clean_breakpoint) {
+			/* Inner clean range */
+			dmac_map_area(vaddr, length, DMA_TO_DEVICE);
+			*flushed_everything = false;
+		} else {
+			clean_inner_dcache_all();
+		}
+
+		if (length < outer_flush_breakpoint) {
+			outer_cache.flush_range(paddr, paddr + length);
+			*flushed_everything = false;
+		} else {
+			outer_cache.flush_all();
+		}
+	}
+
+	if (length < inner_flush_breakpoint) {
+		/* Inner flush range */
+		dmac_flush_range(vaddr, (void *)((u32)vaddr + length));
+		*flushed_everything = false;
+	} else {
+		flush_inner_dcache_all();
+	}
+}
+
+bool speculative_data_prefetch(void)
+{
+	return true;
+}
+
+u32 get_dcache_granularity(void)
+{
+	return 32;
+}
+
+/*
+ * Local functions
+ */
+
+static void __clean_inner_dcache_all(void *param)
+{
+	__cpuc_clean_dcache_all();
+}
+
+static void clean_inner_dcache_all(void)
+{
+	on_each_cpu(__clean_inner_dcache_all, NULL, 1);
+}
+
+static void __flush_inner_dcache_all(void *param)
+{
+	__cpuc_flush_dcache_all();
+}
+
+static void flush_inner_dcache_all(void)
+{
+	on_each_cpu(__flush_inner_dcache_all, NULL, 1);
+}
+
+static bool is_cache_exclusive(void)
+{
+	static const u32 CA9_ACTLR_EXCL = 0x80;
+
+	u32 armv7_actlr;
+
+	asm (
+		"mrc	p15, 0, %0, c1, c0, 1"
+		: "=r" (armv7_actlr)
+	);
+
+	if (armv7_actlr & CA9_ACTLR_EXCL)
+		return true;
+	else
+		return false;
+}
diff --git a/arch/arm/mach-ux500/devices-common.c b/arch/arm/mach-ux500/devices-common.c
index 13a4ce046ae..9580bbde792 100644
--- a/arch/arm/mach-ux500/devices-common.c
+++ b/arch/arm/mach-ux500/devices-common.c
@@ -12,11 +12,17 @@
 #include <linux/slab.h>
 #include <linux/platform_device.h>
 #include <linux/amba/bus.h>
+#include <linux/pm.h>
+#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
 
-#include <plat/gpio.h>
+#include <video/mcde_display.h>
+#include <video/mcde_display-av8100.h>
+#include <video/mcde_fb.h>
 
 #include <mach/hardware.h>
 
+#include "pm/pm.h"
 #include "devices-common.h"
 
 struct amba_device *
@@ -38,6 +44,7 @@ dbx500_add_amba_device(const char *name, resource_size_t base,
 
 	dev->dma_mask = DMA_BIT_MASK(32);
 	dev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+	dev->dev.pwr_domain = &ux500_dev_power_domain;
 
 	dev->irq[0] = irq;
 	dev->irq[1] = NO_IRQ;
@@ -68,6 +75,7 @@ dbx500_add_platform_device(const char *name, int id, void *pdata,
 
 	dev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
 	dev->dev.dma_mask = &dev->dev.coherent_dma_mask;
+	dev->dev.pwr_domain = &ux500_dev_power_domain;
 
 	ret = platform_device_add_resources(dev, res, resnum);
 	if (ret)
@@ -140,7 +148,68 @@ void dbx500_add_gpios(resource_size_t *base, int num, int irq,
 		pdata->first_gpio = first;
 		pdata->first_irq = NOMADIK_GPIO_TO_IRQ(first);
 		pdata->num_gpio = 32;
-
 		dbx500_add_gpio(i, base[i], irq, pdata);
 	}
 }
+
+void hdmi_fb_onoff(struct mcde_display_device *ddev,
+		bool enable, u8 cea, u8 vesa_cea_nr)
+{
+	struct fb_info *fbi;
+	u16 w, h;
+	u16 vw, vh;
+	u32 rotate = FB_ROTATE_UR;
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+
+	dev_dbg(&ddev->dev, "%s\n", __func__);
+	dev_dbg(&ddev->dev, "en:%d cea:%d nr:%d\n", enable, cea, vesa_cea_nr);
+
+	if (enable) {
+		if (ddev->enabled) {
+			dev_dbg(&ddev->dev, "Display is already enabled.\n");
+			return;
+		}
+
+		/* Create fb */
+		if (ddev->fbi == NULL) {
+			/* Note: change when dynamic buffering is available */
+			int buffering = 2;
+
+			/* Get default values */
+			mcde_dss_get_native_resolution(ddev, &w, &h);
+			vw = w;
+			vh = h * buffering;
+
+			if (vesa_cea_nr != 0)
+				ddev->ceanr_convert(ddev, cea, vesa_cea_nr,
+						buffering, &w, &h, &vw, &vh);
+
+			fbi = mcde_fb_create(ddev, w, h, vw, vh,
+				ddev->default_pixel_format, rotate);
+
+			if (IS_ERR(fbi)) {
+				dev_warn(&ddev->dev,
+					"Failed to create fb for display %s\n",
+							ddev->name);
+				goto hdmi_fb_onoff_end;
+			} else {
+				dev_info(&ddev->dev,
+					"Framebuffer created (%s)\n",
+							ddev->name);
+			}
+			driver_data->fbdevname = (char *)dev_name(fbi->dev);
+		}
+	} else {
+		if (!ddev->enabled) {
+			dev_dbg(&ddev->dev, "Display %s is already disabled.\n",
+					ddev->name);
+			return;
+		}
+		mcde_fb_destroy(ddev);
+	}
+
+hdmi_fb_onoff_end:
+	return;
+}
+EXPORT_SYMBOL(hdmi_fb_onoff);
+
diff --git a/arch/arm/mach-ux500/devices-common.h b/arch/arm/mach-ux500/devices-common.h
index 7825705033b..8ee40ff9825 100644
--- a/arch/arm/mach-ux500/devices-common.h
+++ b/arch/arm/mach-ux500/devices-common.h
@@ -17,18 +17,20 @@ dbx500_add_platform_device_4k1irq(const char *name, int id,
 				  resource_size_t base,
 				  int irq, void *pdata);
 
-struct spi_master_cntlr;
+struct stm_msp_controller;
 
 static inline struct amba_device *
 dbx500_add_msp_spi(const char *name, resource_size_t base, int irq,
-		   struct spi_master_cntlr *pdata)
+		   struct stm_msp_controller *pdata)
 {
 	return dbx500_add_amba_device(name, base, irq, pdata, 0);
 }
 
+struct pl022_ssp_controller;
+
 static inline struct amba_device *
 dbx500_add_spi(const char *name, resource_size_t base, int irq,
-	       struct spi_master_cntlr *pdata,
+	       struct pl022_ssp_controller *pdata,
 	       u32 periphid)
 {
 	return dbx500_add_amba_device(name, base, irq, pdata, periphid);
diff --git a/arch/arm/mach-ux500/devices-db5500.c b/arch/arm/mach-ux500/devices-db5500.c
new file mode 100644
index 00000000000..7e8410ea5cd
--- /dev/null
+++ b/arch/arm/mach-ux500/devices-db5500.c
@@ -0,0 +1,256 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Rabin Vincent <rabin.vincent@stericsson.com> for ST-Ericsson
+ *
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * for the System Trace Module part.
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/gpio/nomadik.h>
+
+#include <mach/hardware.h>
+#include <mach/devices.h>
+
+#include <video/mcde.h>
+#include <mach/db5500-regs.h>
+
+#include <mach/prcmu.h>
+
+#include "pm/pm.h"
+
+#define GPIO_DATA(_name, first, num)					\
+	{								\
+		.name		= _name,				\
+		.first_gpio	= first,				\
+		.first_irq	= NOMADIK_GPIO_TO_IRQ(first),		\
+		.num_gpio	= num,					\
+		.get_secondary_status = ux500_pm_gpio_read_wake_up_status, \
+		.set_ioforce	= ux500_pm_prcmu_set_ioforce,		\
+	}
+
+#define GPIO_RESOURCE(block)						\
+	{								\
+		.start	= U5500_GPIOBANK##block##_BASE,			\
+		.end	= U5500_GPIOBANK##block##_BASE + 127,		\
+		.flags	= IORESOURCE_MEM,				\
+	},								\
+	{								\
+		.start	= IRQ_DB5500_GPIO##block,			\
+		.end	= IRQ_DB5500_GPIO##block,			\
+		.flags	= IORESOURCE_IRQ,				\
+	},								\
+	{								\
+		.start	= IRQ_DB5500_PRCMU_GPIO##block,			\
+		.end	= IRQ_DB5500_PRCMU_GPIO##block,			\
+		.flags	= IORESOURCE_IRQ,				\
+	}
+
+#define GPIO_DEVICE(block)						\
+	{								\
+		.name		= "gpio",				\
+		.id		= block,				\
+		.num_resources 	= 3,					\
+		.resource	= &u5500_gpio_resources[block * 3],	\
+		.dev = {						\
+			.platform_data = &u5500_gpio_data[block],	\
+		},							\
+	}
+
+static struct nmk_gpio_platform_data u5500_gpio_data[] = {
+	GPIO_DATA("GPIO-0-31", 0, 32),
+	GPIO_DATA("GPIO-32-63", 32, 4), /* 36..63 not routed to pin */
+	GPIO_DATA("GPIO-64-95", 64, 19), /* 83..95 not routed to pin */
+	GPIO_DATA("GPIO-96-127", 96, 6), /* 102..127 not routed to pin */
+	GPIO_DATA("GPIO-128-159", 128, 21), /* 149..159 not routed to pin */
+	GPIO_DATA("GPIO-160-191", 160, 32),
+	GPIO_DATA("GPIO-192-223", 192, 32),
+	GPIO_DATA("GPIO-224-255", 224, 4), /* 228..255 not routed to pin */
+};
+
+static struct resource u5500_gpio_resources[] = {
+	GPIO_RESOURCE(0),
+	GPIO_RESOURCE(1),
+	GPIO_RESOURCE(2),
+	GPIO_RESOURCE(3),
+	GPIO_RESOURCE(4),
+	GPIO_RESOURCE(5),
+	GPIO_RESOURCE(6),
+	GPIO_RESOURCE(7),
+};
+
+struct platform_device u5500_gpio_devs[] = {
+	GPIO_DEVICE(0),
+	GPIO_DEVICE(1),
+	GPIO_DEVICE(2),
+	GPIO_DEVICE(3),
+	GPIO_DEVICE(4),
+	GPIO_DEVICE(5),
+	GPIO_DEVICE(6),
+	GPIO_DEVICE(7),
+};
+
+#define U5500_PWM_SIZE 0x20
+static struct resource u5500_pwm0_resource[] = {
+	{
+		.name = "PWM_BASE",
+		.start = U5500_PWM_BASE,
+		.end = U5500_PWM_BASE + U5500_PWM_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static struct resource u5500_pwm1_resource[] = {
+	{
+		.name = "PWM_BASE",
+		.start = U5500_PWM_BASE + U5500_PWM_SIZE,
+		.end = U5500_PWM_BASE + U5500_PWM_SIZE * 2 - 1,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static struct resource u5500_pwm2_resource[] = {
+	{
+		.name = "PWM_BASE",
+		.start = U5500_PWM_BASE + U5500_PWM_SIZE * 2,
+		.end = U5500_PWM_BASE + U5500_PWM_SIZE * 3 - 1,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static struct resource u5500_pwm3_resource[] = {
+	{
+		.name = "PWM_BASE",
+		.start = U5500_PWM_BASE + U5500_PWM_SIZE * 3,
+		.end = U5500_PWM_BASE + U5500_PWM_SIZE * 4 - 1,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+struct platform_device u5500_pwm0_device = {
+	.id = 0,
+	.name = "pwm",
+	.resource = u5500_pwm0_resource,
+	.num_resources = ARRAY_SIZE(u5500_pwm0_resource),
+};
+
+struct platform_device u5500_pwm1_device = {
+	.id = 1,
+	.name = "pwm",
+	.resource = u5500_pwm1_resource,
+	.num_resources = ARRAY_SIZE(u5500_pwm1_resource),
+};
+
+struct platform_device u5500_pwm2_device = {
+	.id = 2,
+	.name = "pwm",
+	.resource = u5500_pwm2_resource,
+	.num_resources = ARRAY_SIZE(u5500_pwm2_resource),
+};
+
+struct platform_device u5500_pwm3_device = {
+	.id = 3,
+	.name = "pwm",
+	.resource = u5500_pwm3_resource,
+	.num_resources = ARRAY_SIZE(u5500_pwm3_resource),
+};
+
+static struct resource mcde_resources[] = {
+	[0] = {
+		.name  = MCDE_IO_AREA,
+		.start = U5500_MCDE_BASE,
+		.end   = U5500_MCDE_BASE + U5500_MCDE_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.name  = MCDE_IO_AREA,
+		.start = U5500_DSI_LINK1_BASE,
+		.end   = U5500_DSI_LINK1_BASE + U5500_DSI_LINK_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	[2] = {
+		.name  = MCDE_IO_AREA,
+		.start = U5500_DSI_LINK2_BASE,
+		.end   = U5500_DSI_LINK2_BASE + U5500_DSI_LINK_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	[3] = {
+		.name  = MCDE_IRQ,
+		.start = IRQ_DB5500_DISP,
+		.end   = IRQ_DB5500_DISP,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static int mcde_platform_enable_dsipll(void)
+{
+	return prcmu_enable_dsipll();
+}
+
+static int mcde_platform_disable_dsipll(void)
+{
+	return prcmu_disable_dsipll();
+}
+
+static int mcde_platform_set_display_clocks(void)
+{
+	return prcmu_set_display_clocks();
+}
+
+static struct mcde_platform_data mcde_pdata = {
+	.num_dsilinks = 2,
+	.syncmux = 0x01,
+	.num_channels = 2,
+	.num_overlays = 3,
+	.regulator_vana_id = "v-ana",
+	.regulator_mcde_epod_id = "vsupply",
+	.regulator_esram_epod_id = "v-esram12",
+	.clock_dsi_id = "hdmi",
+	.clock_dsi_lp_id = "tv",
+	.clock_mcde_id = "mcde",
+	.platform_set_clocks = mcde_platform_set_display_clocks,
+	.platform_enable_dsipll = mcde_platform_enable_dsipll,
+	.platform_disable_dsipll = mcde_platform_disable_dsipll,
+};
+
+struct platform_device u5500_mcde_device = {
+	.name = "mcde",
+	.id = -1,
+	.dev = {
+		.platform_data = &mcde_pdata,
+	},
+	.num_resources = ARRAY_SIZE(mcde_resources),
+	.resource = mcde_resources,
+};
+
+static struct resource b2r2_resources[] = {
+	[0] = {
+		.start	= U5500_B2R2_BASE,
+		.end	= U5500_B2R2_BASE + ((4*1024)-1),
+		.name	= "b2r2_base",
+		.flags	= IORESOURCE_MEM,
+	},
+	[1] = {
+		.name  = "B2R2_IRQ",
+		.start = IRQ_DB5500_B2R2,
+		.end   = IRQ_DB5500_B2R2,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+struct platform_device u5500_b2r2_device = {
+	.name	= "b2r2",
+	.id	= 0,
+	.dev	= {
+		.init_name = "b2r2_bus",
+		.coherent_dma_mask = ~0,
+	},
+	.num_resources	= ARRAY_SIZE(b2r2_resources),
+	.resource	= b2r2_resources,
+};
+
diff --git a/arch/arm/mach-ux500/devices-db5500.h b/arch/arm/mach-ux500/devices-db5500.h
index 0c4bccd02b9..20d921d43c6 100644
--- a/arch/arm/mach-ux500/devices-db5500.h
+++ b/arch/arm/mach-ux500/devices-db5500.h
@@ -17,6 +17,16 @@
 #define db5500_add_i2c3(pdata) \
 	dbx500_add_i2c(3, U5500_I2C3_BASE, IRQ_DB5500_I2C3, pdata)
 
+struct db5500_keypad_platform_data;
+
+static inline struct platform_device *
+db5500_add_keypad(struct db5500_keypad_platform_data *pdata)
+{
+	return dbx500_add_platform_device_4k1irq("db5500-keypad", -1,
+						 U5500_KEYPAD_BASE,
+						 IRQ_DB5500_KBD, pdata);
+}
+
 #define db5500_add_msp0_i2s(pdata) \
 	dbx500_add_msp_i2s(0, U5500_MSP0_BASE, IRQ_DB5500_MSP0, pdata)
 #define db5500_add_msp1_i2s(pdata) \
@@ -44,13 +54,13 @@
 	dbx500_add_sdi("sdi1", U5500_SDI1_BASE, IRQ_DB5500_SDMMC1, pdata, \
 		       0x10480180)
 #define db5500_add_sdi2(pdata) \
-	dbx500_add_sdi("sdi2", U5500_SDI2_BASE, IRQ_DB5500_SDMMC2, pdata \
+	dbx500_add_sdi("sdi2", U5500_SDI2_BASE, IRQ_DB5500_SDMMC2, pdata, \
 		       0x10480180)
 #define db5500_add_sdi3(pdata) \
-	dbx500_add_sdi("sdi3", U5500_SDI3_BASE, IRQ_DB5500_SDMMC3, pdata \
+	dbx500_add_sdi("sdi3", U5500_SDI3_BASE, IRQ_DB5500_SDMMC3, pdata, \
 		       0x10480180)
 #define db5500_add_sdi4(pdata) \
-	dbx500_add_sdi("sdi4", U5500_SDI4_BASE, IRQ_DB5500_SDMMC4, pdata \
+	dbx500_add_sdi("sdi4", U5500_SDI4_BASE, IRQ_DB5500_SDMMC4, pdata, \
 		       0x10480180)
 
 /* This one has a bad peripheral ID in the U5500 silicon */
@@ -61,10 +71,10 @@
 	dbx500_add_spi("spi1", U5500_SPI1_BASE, IRQ_DB5500_SPI1, pdata, \
 		       0x10080023)
 #define db5500_add_spi2(pdata) \
-	dbx500_add_spi("spi2", U5500_SPI2_BASE, IRQ_DB5500_SPI2, pdata \
+	dbx500_add_spi("spi2", U5500_SPI2_BASE, IRQ_DB5500_SPI2, pdata, \
 		       0x10080023)
 #define db5500_add_spi3(pdata) \
-	dbx500_add_spi("spi3", U5500_SPI3_BASE, IRQ_DB5500_SPI3, pdata \
+	dbx500_add_spi("spi3", U5500_SPI3_BASE, IRQ_DB5500_SPI3, pdata, \
 		       0x10080023)
 
 #define db5500_add_uart0(plat) \
diff --git a/arch/arm/mach-ux500/devices-db8500.c b/arch/arm/mach-ux500/devices-db8500.c
index 73b17404b19..41ced007cf4 100644
--- a/arch/arm/mach-ux500/devices-db8500.c
+++ b/arch/arm/mach-ux500/devices-db8500.c
@@ -2,6 +2,10 @@
  * Copyright (C) ST-Ericsson SA 2010
  *
  * Author: Rabin Vincent <rabin.vincent@stericsson.com> for ST-Ericsson
+ *
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * for the System Trace Module part.
+ *
  * License terms: GNU General Public License (GPL) version 2
  */
 
@@ -10,15 +14,24 @@
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
 #include <linux/amba/bus.h>
 #include <linux/amba/pl022.h>
+#include <plat/pincfg.h>
 
 #include <plat/ste_dma40.h>
 
+#include <mach/devices.h>
 #include <mach/hardware.h>
 #include <mach/setup.h>
+#include <video/mcde.h>
+#include <mach/prcmu.h>
+#include <mach/prcmu-regs.h>
+#include <mach/hsi.h>
+#include <mach/ste-dma40-db8500.h>
+#include <trace/stm.h>
 
-#include "ste-dma40-db8500.h"
+#include "pins-db8500.h"
 
 static struct resource dma40_resources[] = {
 	[0] = {
@@ -87,20 +100,63 @@ static const dma_addr_t dma40_tx_map[DB8500_DMA_NR_DEV] = {
 	[DB8500_DMA_DEV1_SD_MMC0_TX] = -1,
 	[DB8500_DMA_DEV2_SD_MMC1_TX] = -1,
 	[DB8500_DMA_DEV3_SD_MMC2_TX] = -1,
-	[DB8500_DMA_DEV8_SSP0_TX] = -1,
+	[DB8500_DMA_DEV4_I2C1_TX] = -1,
+	[DB8500_DMA_DEV5_I2C3_TX] = -1,
+	[DB8500_DMA_DEV6_I2C2_TX] = -1,
+	[DB8500_DMA_DEV7_I2C4_TX] = -1,
+	[DB8500_DMA_DEV8_SSP0_TX] = U8500_SSP0_BASE + SSP_TX_RX_REG_OFFSET,
 	[DB8500_DMA_DEV9_SSP1_TX] = -1,
 	[DB8500_DMA_DEV11_UART2_TX] = -1,
 	[DB8500_DMA_DEV12_UART1_TX] = -1,
 	[DB8500_DMA_DEV13_UART0_TX] = -1,
+	[DB8500_DMA_DEV14_MSP2_TX] = U8500_MSP2_BASE + MSP_TX_RX_REG_OFFSET,
+	[DB8500_DMA_DEV15_I2C0_TX] = -1,
+	[DB8500_DMA_DEV20_SLIM0_CH0_TX_HSI_TX_CH0]
+		= U8500_HSIT_BASE + 0x0 + STE_HSI_TX_BUFFERX,
+	[DB8500_DMA_DEV21_SLIM0_CH1_TX_HSI_TX_CH1]
+		= U8500_HSIT_BASE + 0x4 + STE_HSI_TX_BUFFERX,
+	[DB8500_DMA_DEV22_SLIM0_CH2_TX_HSI_TX_CH2]
+		= U8500_HSIT_BASE + 0x8 + STE_HSI_TX_BUFFERX,
+	[DB8500_DMA_DEV23_SLIM0_CH3_TX_HSI_TX_CH3]
+		= U8500_HSIT_BASE + 0xC + STE_HSI_TX_BUFFERX,
+	[DB8500_DMA_DEV24_DST_SXA0_RX_TX] = -1,
+	[DB8500_DMA_DEV25_DST_SXA1_RX_TX] = -1,
+	[DB8500_DMA_DEV26_DST_SXA2_RX_TX] = -1,
+	[DB8500_DMA_DEV27_DST_SXA3_RX_TX] = -1,
 	[DB8500_DMA_DEV28_SD_MM2_TX] = -1,
 	[DB8500_DMA_DEV29_SD_MM0_TX] = -1,
+	[DB8500_DMA_DEV30_MSP1_TX]
+		= U8500_MSP1_BASE + MSP_TX_RX_REG_OFFSET,
+	[DB8500_DMA_DEV31_MSP0_TX_SLIM0_CH0_TX]
+		= U8500_MSP0_BASE + MSP_TX_RX_REG_OFFSET,
 	[DB8500_DMA_DEV32_SD_MM1_TX] = -1,
 	[DB8500_DMA_DEV33_SPI2_TX] = -1,
+	[DB8500_DMA_DEV34_I2C3_TX2] = -1,
 	[DB8500_DMA_DEV35_SPI1_TX] = -1,
 	[DB8500_DMA_DEV40_SPI3_TX] = -1,
 	[DB8500_DMA_DEV41_SD_MM3_TX] = -1,
 	[DB8500_DMA_DEV42_SD_MM4_TX] = -1,
 	[DB8500_DMA_DEV43_SD_MM5_TX] = -1,
+	[DB8500_DMA_DEV44_DST_SXA4_RX_TX] = -1,
+	[DB8500_DMA_DEV45_DST_SXA5_RX_TX] = -1,
+	[DB8500_DMA_DEV46_SLIM0_CH8_TX_DST_SXA6_RX_TX] = -1,
+	[DB8500_DMA_DEV47_SLIM0_CH9_TX_DST_SXA7_RX_TX] = -1,
+	[DB8500_DMA_DEV48_CAC1_TX] = U8500_CRYP1_BASE + CRYP1_TX_REG_OFFSET,
+	[DB8500_DMA_DEV49_CAC1_TX_HAC1_TX] = -1,
+	[DB8500_DMA_DEV50_HAC1_TX] = -1,
+	[DB8500_DMA_MEMCPY_TX_0] = -1,
+	[DB8500_DMA_DEV52_SLIM1_CH4_TX_HSI_TX_CH4] = -1,
+	[DB8500_DMA_DEV53_SLIM1_CH5_TX_HSI_TX_CH5] = -1,
+	[DB8500_DMA_DEV54_SLIM1_CH6_TX_HSI_TX_CH6] = -1,
+	[DB8500_DMA_DEV55_SLIM1_CH7_TX_HSI_TX_CH7] = -1,
+	[DB8500_DMA_MEMCPY_TX_1] = -1,
+	[DB8500_DMA_MEMCPY_TX_2] = -1,
+	[DB8500_DMA_MEMCPY_TX_3] = -1,
+	[DB8500_DMA_MEMCPY_TX_4] = -1,
+	[DB8500_DMA_MEMCPY_TX_5] = -1,
+	[DB8500_DMA_DEV61_CAC0_TX] = -1,
+	[DB8500_DMA_DEV62_CAC0_TX_HAC0_TX] = -1,
+	[DB8500_DMA_DEV63_HAC0_TX] = -1,
 };
 
 /* Mapping between source event lines and physical device address */
@@ -119,20 +175,56 @@ static const dma_addr_t dma40_rx_map[DB8500_DMA_NR_DEV] = {
 	[DB8500_DMA_DEV1_SD_MMC0_RX] = -1,
 	[DB8500_DMA_DEV2_SD_MMC1_RX] = -1,
 	[DB8500_DMA_DEV3_SD_MMC2_RX] = -1,
+	[DB8500_DMA_DEV4_I2C1_RX] = -1,
+	[DB8500_DMA_DEV5_I2C3_RX] = -1,
+	[DB8500_DMA_DEV6_I2C2_RX] = -1,
+	[DB8500_DMA_DEV7_I2C4_RX] = -1,
 	[DB8500_DMA_DEV8_SSP0_RX] = -1,
 	[DB8500_DMA_DEV9_SSP1_RX] = -1,
 	[DB8500_DMA_DEV11_UART2_RX] = -1,
 	[DB8500_DMA_DEV12_UART1_RX] = -1,
 	[DB8500_DMA_DEV13_UART0_RX] = -1,
+	[DB8500_DMA_DEV14_MSP2_RX] = U8500_MSP2_BASE + MSP_TX_RX_REG_OFFSET,
+	[DB8500_DMA_DEV15_I2C0_RX] = -1,
+	[DB8500_DMA_DEV20_SLIM0_CH0_RX_HSI_RX_CH0]
+		= U8500_HSIR_BASE + 0x0 + STE_HSI_RX_BUFFERX,
+	[DB8500_DMA_DEV21_SLIM0_CH1_RX_HSI_RX_CH1]
+		= U8500_HSIR_BASE + 0x4 + STE_HSI_RX_BUFFERX,
+	[DB8500_DMA_DEV22_SLIM0_CH2_RX_HSI_RX_CH2]
+		= U8500_HSIR_BASE + 0x8 + STE_HSI_RX_BUFFERX,
+	[DB8500_DMA_DEV23_SLIM0_CH3_RX_HSI_RX_CH3]
+		= U8500_HSIR_BASE + 0xC + STE_HSI_RX_BUFFERX,
+	[DB8500_DMA_DEV24_SRC_SXA0_RX_TX] = -1,
+	[DB8500_DMA_DEV25_SRC_SXA1_RX_TX] = -1,
+	[DB8500_DMA_DEV26_SRC_SXA2_RX_TX] = -1,
+	[DB8500_DMA_DEV27_SRC_SXA3_RX_TX] = -1,
 	[DB8500_DMA_DEV28_SD_MM2_RX] = -1,
 	[DB8500_DMA_DEV29_SD_MM0_RX] = -1,
+	[DB8500_DMA_DEV30_MSP3_RX]
+		= U8500_MSP3_BASE + MSP_TX_RX_REG_OFFSET,
+	[DB8500_DMA_DEV31_MSP0_RX_SLIM0_CH0_RX]
+		= U8500_MSP0_BASE + MSP_TX_RX_REG_OFFSET,
 	[DB8500_DMA_DEV32_SD_MM1_RX] = -1,
 	[DB8500_DMA_DEV33_SPI2_RX] = -1,
+	[DB8500_DMA_DEV34_I2C3_RX2] = -1,
 	[DB8500_DMA_DEV35_SPI1_RX] = -1,
 	[DB8500_DMA_DEV40_SPI3_RX] = -1,
 	[DB8500_DMA_DEV41_SD_MM3_RX] = -1,
 	[DB8500_DMA_DEV42_SD_MM4_RX] = -1,
 	[DB8500_DMA_DEV43_SD_MM5_RX] = -1,
+	[DB8500_DMA_DEV44_SRC_SXA4_RX_TX] = -1,
+	[DB8500_DMA_DEV45_SRC_SXA5_RX_TX] = -1,
+	[DB8500_DMA_DEV46_SLIM0_CH8_RX_SRC_SXA6_RX_TX] = -1,
+	[DB8500_DMA_DEV47_SLIM0_CH9_RX_SRC_SXA7_RX_TX] = -1,
+	[DB8500_DMA_DEV48_CAC1_RX] = U8500_CRYP1_BASE + CRYP1_RX_REG_OFFSET,
+	/* 49, 50 and 51 are not used */
+	[DB8500_DMA_DEV52_SLIM0_CH4_RX_HSI_RX_CH4] = -1,
+	[DB8500_DMA_DEV53_SLIM0_CH5_RX_HSI_RX_CH5] = -1,
+	[DB8500_DMA_DEV54_SLIM0_CH6_RX_HSI_RX_CH6] = -1,
+	[DB8500_DMA_DEV55_SLIM0_CH7_RX_HSI_RX_CH7] = -1,
+	/* 56, 57, 58, 59 and 60 are not used */
+	[DB8500_DMA_DEV61_CAC0_RX] = -1,
+	/* 62 and 63 are not used */
 };
 
 /* Reserved event lines for memcpy only */
@@ -166,16 +258,339 @@ struct platform_device u8500_dma40_device = {
 	.resource = dma40_resources
 };
 
-void dma40_u8500ed_fixup(void)
+static struct resource u8500_shrm_resources[] = {
+	[0] = {
+		.start = U8500_SHRM_GOP_INTERRUPT_BASE,
+		.end = U8500_SHRM_GOP_INTERRUPT_BASE + ((4*4)-1),
+		.name = "shrm_gop_register_base",
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.start = IRQ_CA_WAKE_REQ_V1,
+		.end = IRQ_CA_WAKE_REQ_V1,
+		.name = "ca_irq_wake_req",
+		.flags = IORESOURCE_IRQ,
+	},
+	[2] = {
+		.start = IRQ_AC_READ_NOTIFICATION_0_V1,
+		.end = IRQ_AC_READ_NOTIFICATION_0_V1,
+		.name = "ac_read_notification_0_irq",
+		.flags = IORESOURCE_IRQ,
+	},
+	[3] = {
+		.start = IRQ_AC_READ_NOTIFICATION_1_V1,
+		.end = IRQ_AC_READ_NOTIFICATION_1_V1,
+		.name = "ac_read_notification_1_irq",
+		.flags = IORESOURCE_IRQ,
+	},
+	[4] = {
+		.start = IRQ_CA_MSG_PEND_NOTIFICATION_0_V1,
+		.end = IRQ_CA_MSG_PEND_NOTIFICATION_0_V1,
+		.name = "ca_msg_pending_notification_0_irq",
+		.flags = IORESOURCE_IRQ,
+	},
+	[5] = {
+		.start = IRQ_CA_MSG_PEND_NOTIFICATION_1_V1,
+		.end = IRQ_CA_MSG_PEND_NOTIFICATION_1_V1,
+		.name = "ca_msg_pending_notification_1_irq",
+		.flags = IORESOURCE_IRQ,
+	}
+};
+
+struct platform_device u8500_shrm_device = {
+	.name = "u8500_shrm",
+	.id = 0,
+	.dev = {
+		.init_name = "shrm_bus",
+		.coherent_dma_mask = ~0,
+	},
+
+	.num_resources = ARRAY_SIZE(u8500_shrm_resources),
+	.resource = u8500_shrm_resources
+};
+
+static struct resource mcde_resources[] = {
+	[0] = {
+		.name  = MCDE_IO_AREA,
+		.start = U8500_MCDE_BASE,
+		.end   = U8500_MCDE_BASE + U8500_MCDE_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.name  = MCDE_IO_AREA,
+		.start = U8500_DSI_LINK1_BASE,
+		.end   = U8500_DSI_LINK1_BASE + U8500_DSI_LINK_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	[2] = {
+		.name  = MCDE_IO_AREA,
+		.start = U8500_DSI_LINK2_BASE,
+		.end   = U8500_DSI_LINK2_BASE + U8500_DSI_LINK_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	[3] = {
+		.name  = MCDE_IO_AREA,
+		.start = U8500_DSI_LINK3_BASE,
+		.end   = U8500_DSI_LINK3_BASE + U8500_DSI_LINK_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	[4] = {
+		.name  = MCDE_IRQ,
+		.start = IRQ_DB8500_DISP,
+		.end   = IRQ_DB8500_DISP,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static int mcde_platform_enable_dsipll(void)
+{
+	return prcmu_enable_dsipll();
+}
+
+static int mcde_platform_disable_dsipll(void)
+{
+	return prcmu_disable_dsipll();
+}
+
+static int mcde_platform_set_display_clocks(void)
 {
-	dma40_plat_data.memcpy = NULL;
-	dma40_plat_data.memcpy_len = 0;
-	dma40_resources[0].start = U8500_DMA_BASE_ED;
-	dma40_resources[0].end = U8500_DMA_BASE_ED + SZ_4K - 1;
-	dma40_resources[1].start = U8500_DMA_LCPA_BASE_ED;
-	dma40_resources[1].end = U8500_DMA_LCPA_BASE_ED + 2 * SZ_1K - 1;
+	return prcmu_set_display_clocks();
 }
 
+static struct mcde_platform_data mcde_pdata = {
+	.num_dsilinks = 3,
+	/*
+	 * [0] = 3: 24 bits DPI: connect LSB Ch B to D[0:7]
+	 * [3] = 4: 24 bits DPI: connect MID Ch B to D[24:31]
+	 * [4] = 5: 24 bits DPI: connect MSB Ch B to D[32:39]
+	 *
+	 * [1] = 3: TV out     : connect LSB Ch B to D[8:15]
+	 */
+#define DONT_CARE 0
+	.outmux = { 3, 3, DONT_CARE, 4, 5 },
+#undef DONT_CARE
+	.syncmux = 0x00,  /* DPI channel A and B on output pins A and B resp */
+	.num_channels = 4,
+	.num_overlays = 6,
+	.regulator_vana_id = "v-ana",
+	.regulator_mcde_epod_id = "vsupply",
+	.regulator_esram_epod_id = "v-esram34",
+	.clock_dsi_id = "hdmi",
+	.clock_dsi_lp_id = "tv",
+	.clock_dpi_id = "lcd",
+	.clock_mcde_id = "mcde",
+	.platform_set_clocks = mcde_platform_set_display_clocks,
+	.platform_enable_dsipll = mcde_platform_enable_dsipll,
+	.platform_disable_dsipll = mcde_platform_disable_dsipll,
+};
+
+struct platform_device u8500_mcde_device = {
+	.name = "mcde",
+	.id = -1,
+	.dev = {
+		.platform_data = &mcde_pdata,
+	},
+	.num_resources = ARRAY_SIZE(mcde_resources),
+	.resource = mcde_resources,
+};
+
+static struct resource b2r2_resources[] = {
+	[0] = {
+		.start	= U8500_B2R2_BASE,
+		.end	= U8500_B2R2_BASE + ((4*1024)-1),
+		.name	= "b2r2_base",
+		.flags	= IORESOURCE_MEM,
+	},
+	[1] = {
+		.name  = "B2R2_IRQ",
+		.start = IRQ_DB8500_B2R2,
+		.end   = IRQ_DB8500_B2R2,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+struct platform_device u8500_b2r2_device = {
+	.name	= "b2r2",
+	.id	= 0,
+	.dev	= {
+		.init_name = "b2r2_bus",
+		.coherent_dma_mask = ~0,
+	},
+	.num_resources	= ARRAY_SIZE(b2r2_resources),
+	.resource	= b2r2_resources,
+};
+
+/*
+ * WATCHDOG
+ */
+
+static struct resource ux500_wdt_resources[] = {
+	[0] = {
+		.start  = U8500_TWD_BASE,
+		.end    = U8500_TWD_BASE+0x37,
+		.flags  = IORESOURCE_MEM,
+	},
+	[1] = {
+		.start  = IRQ_LOCALWDOG,
+		.end  = IRQ_LOCALWDOG,
+		.flags  = IORESOURCE_IRQ,
+	}
+};
+
+struct platform_device ux500_wdt_device = {
+	.name           = "mpcore_wdt",
+	.id             = -1,
+	.resource       = ux500_wdt_resources,
+	.num_resources  = ARRAY_SIZE(ux500_wdt_resources),
+};
+
+struct platform_device ux500_prcmu_wdt_device = {
+	.name		= "ux500_wdt",
+	.id		= -1,
+};
+
+/*
+ * HSI
+ */
+#define HSIR_OVERRUN(num) {			    \
+	.start  = IRQ_DB8500_HSIR_CH##num##_OVRRUN, \
+	.end    = IRQ_DB8500_HSIR_CH##num##_OVRRUN, \
+	.flags  = IORESOURCE_IRQ,		    \
+	.name   = "hsi_rx_overrun_ch"#num	    \
+}
+
+#define STE_HSI_PORT0_TX_CHANNEL_CFG(n) { \
+       .dir = STEDMA40_MEM_TO_PERIPH, \
+       .high_priority = false, \
+       .mode = STEDMA40_MODE_LOGICAL, \
+       .mode_opt = STEDMA40_LCHAN_SRC_LOG_DST_LOG, \
+       .src_dev_type = STEDMA40_DEV_SRC_MEMORY, \
+       .dst_dev_type = n,\
+       .src_info.big_endian = false,\
+       .src_info.data_width = STEDMA40_WORD_WIDTH,\
+       .dst_info.big_endian = false,\
+       .dst_info.data_width = STEDMA40_WORD_WIDTH,\
+},
+
+#define STE_HSI_PORT0_RX_CHANNEL_CFG(n) { \
+       .dir = STEDMA40_PERIPH_TO_MEM, \
+       .high_priority = false, \
+       .mode = STEDMA40_MODE_LOGICAL, \
+       .mode_opt = STEDMA40_LCHAN_SRC_LOG_DST_LOG, \
+       .src_dev_type = n,\
+       .dst_dev_type = STEDMA40_DEV_DST_MEMORY, \
+       .src_info.big_endian = false,\
+       .src_info.data_width = STEDMA40_WORD_WIDTH,\
+       .dst_info.big_endian = false,\
+       .dst_info.data_width = STEDMA40_WORD_WIDTH,\
+},
+
+static struct resource u8500_hsi_resources[] = {
+       {
+	       .start  = U8500_HSIR_BASE,
+	       .end    = U8500_HSIR_BASE + SZ_4K - 1,
+	       .flags  = IORESOURCE_MEM,
+	       .name   = "hsi_rx_base"
+       },
+       {
+	       .start  = U8500_HSIT_BASE,
+	       .end    = U8500_HSIT_BASE + SZ_4K - 1,
+	       .flags  = IORESOURCE_MEM,
+	       .name   = "hsi_tx_base"
+       },
+       {
+	       .start  = IRQ_DB8500_HSIRD0,
+	       .end    = IRQ_DB8500_HSIRD0,
+	       .flags  = IORESOURCE_IRQ,
+	       .name   = "hsi_rx_irq0"
+       },
+       {
+	       .start  = IRQ_DB8500_HSITD0,
+	       .end    = IRQ_DB8500_HSITD0,
+	       .flags  = IORESOURCE_IRQ,
+	       .name   = "hsi_tx_irq0"
+       },
+       {
+	       .start  = IRQ_DB8500_HSIR_EXCEP,
+	       .end    = IRQ_DB8500_HSIR_EXCEP,
+	       .flags  = IORESOURCE_IRQ,
+	       .name   = "hsi_rx_excep0"
+       },
+       HSIR_OVERRUN(0),
+       HSIR_OVERRUN(1),
+       HSIR_OVERRUN(2),
+       HSIR_OVERRUN(3),
+       HSIR_OVERRUN(4),
+       HSIR_OVERRUN(5),
+       HSIR_OVERRUN(6),
+       HSIR_OVERRUN(7),
+};
+
+#ifdef CONFIG_STE_DMA40
+static struct stedma40_chan_cfg ste_hsi_port0_dma_tx_cfg[] = {
+       STE_HSI_PORT0_TX_CHANNEL_CFG(DB8500_DMA_DEV20_SLIM0_CH0_TX_HSI_TX_CH0)
+       STE_HSI_PORT0_TX_CHANNEL_CFG(DB8500_DMA_DEV21_SLIM0_CH1_TX_HSI_TX_CH1)
+       STE_HSI_PORT0_TX_CHANNEL_CFG(DB8500_DMA_DEV22_SLIM0_CH2_TX_HSI_TX_CH2)
+       STE_HSI_PORT0_TX_CHANNEL_CFG(DB8500_DMA_DEV23_SLIM0_CH3_TX_HSI_TX_CH3)
+};
+
+static struct stedma40_chan_cfg ste_hsi_port0_dma_rx_cfg[] = {
+       STE_HSI_PORT0_RX_CHANNEL_CFG(DB8500_DMA_DEV20_SLIM0_CH0_RX_HSI_RX_CH0)
+       STE_HSI_PORT0_RX_CHANNEL_CFG(DB8500_DMA_DEV21_SLIM0_CH1_RX_HSI_RX_CH1)
+       STE_HSI_PORT0_RX_CHANNEL_CFG(DB8500_DMA_DEV22_SLIM0_CH2_RX_HSI_RX_CH2)
+       STE_HSI_PORT0_RX_CHANNEL_CFG(DB8500_DMA_DEV23_SLIM0_CH3_RX_HSI_RX_CH3)
+};
+#endif
+
+static struct ste_hsi_port_cfg ste_hsi_port0_cfg = {
+#ifdef CONFIG_STE_DMA40
+       .dma_filter = stedma40_filter,
+       .dma_tx_cfg = ste_hsi_port0_dma_tx_cfg,
+       .dma_rx_cfg = ste_hsi_port0_dma_rx_cfg
+#endif
+};
+
+struct ste_hsi_platform_data u8500_hsi_platform_data = {
+       .num_ports = 1,
+       .use_dma = 1,
+       .port_cfg = &ste_hsi_port0_cfg,
+};
+
+struct platform_device u8500_hsi_device = {
+       .dev = {
+		.platform_data = &u8500_hsi_platform_data,
+       },
+       .name = "ste_hsi",
+       .id = 0,
+       .resource = u8500_hsi_resources,
+       .num_resources = ARRAY_SIZE(u8500_hsi_resources)
+};
+
+/*
+ * Thermal Sensor
+ */
+
+static struct resource u8500_thsens_resources[] = {
+	{
+		.name = "IRQ_HOTMON_LOW",
+		.start  = IRQ_PRCMU_HOTMON_LOW,
+		.end    = IRQ_PRCMU_HOTMON_LOW,
+		.flags  = IORESOURCE_IRQ,
+	},
+	{
+		.name = "IRQ_HOTMON_HIGH",
+		.start  = IRQ_PRCMU_HOTMON_HIGH,
+		.end    = IRQ_PRCMU_HOTMON_HIGH,
+		.flags  = IORESOURCE_IRQ,
+	},
+};
+
+struct platform_device u8500_thsens_device = {
+	.name           = "db8500_temp",
+	.resource       = u8500_thsens_resources,
+	.num_resources  = ARRAY_SIZE(u8500_thsens_resources),
+};
+
 struct resource keypad_resources[] = {
 	[0] = {
 		.start = U8500_SKE_BASE,
@@ -195,3 +610,182 @@ struct platform_device u8500_ske_keypad_device = {
 	.num_resources = ARRAY_SIZE(keypad_resources),
 	.resource = keypad_resources,
 };
+
+#ifdef CONFIG_STM_TRACE
+static pin_cfg_t mop500_stm_mipi34_pins[] = {
+	GPIO70_STMAPE_CLK | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO71_STMAPE_DAT3 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO72_STMAPE_DAT2 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO73_STMAPE_DAT1 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO74_STMAPE_DAT0 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO75_U2_RXD | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO76_U2_TXD | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+};
+
+static pin_cfg_t mop500_stm_mipi60_pins[] = {
+	GPIO153_U2_RXD,
+	GPIO154_U2_TXD,
+	GPIO155_STMAPE_CLK,
+	GPIO156_STMAPE_DAT3,
+	GPIO157_STMAPE_DAT2,
+	GPIO158_STMAPE_DAT1,
+	GPIO159_STMAPE_DAT0,
+};
+
+static pin_cfg_t mop500_ske_pins[] = {
+	GPIO153_KP_I7 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO154_KP_I6 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO155_KP_I5 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO156_KP_I4 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO161_KP_I3 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO162_KP_I2 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO163_KP_I1 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO164_KP_I0 | PIN_INPUT_PULLDOWN | PIN_SLPM_INPUT_PULLUP,
+	GPIO157_KP_O7 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO158_KP_O6 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO159_KP_O5 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO160_KP_O4 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO165_KP_O3 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO166_KP_O2 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO167_KP_O1 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+	GPIO168_KP_O0 | PIN_INPUT_PULLUP | PIN_SLPM_OUTPUT_LOW,
+};
+
+static int stm_ste_disable_ape_on_mipi60(void)
+{
+	int retval;
+
+	retval = nmk_config_pins_sleep(ARRAY_AND_SIZE(mop500_stm_mipi60_pins));
+	if (retval)
+		pr_err("STM: Failed to disable MIPI60\n");
+	else {
+		retval = nmk_config_pins(ARRAY_AND_SIZE(mop500_ske_pins));
+		if (retval)
+			pr_err("STM: Failed to enable SKE gpio\n");
+	}
+	return retval;
+}
+
+/*
+ * Manage STM output pins connection (MIP34/MIPI60 connectors)
+ */
+static int stm_ste_connection(enum stm_connection_type con_type)
+{
+	int retval = -EINVAL;
+	u32 gpiocr = readl(PRCM_GPIOCR);
+
+	if (con_type != STM_DISCONNECT) {
+		/*  Always enable MIPI34 GPIO pins */
+		retval = nmk_config_pins(
+				ARRAY_AND_SIZE(mop500_stm_mipi34_pins));
+		if (retval) {
+			pr_err("STM: Failed to enable MIPI34\n");
+			return retval;
+		}
+	}
+
+	switch (con_type) {
+	case STM_DEFAULT_CONNECTION:
+	case STM_STE_MODEM_ON_MIPI34_NONE_ON_MIPI60:
+		/* Enable altC3 on GPIO70-74 (STMMOD) & GPIO75-76 (UARTMOD) */
+		gpiocr |= (PRCM_GPIOCR_DBG_STM_MOD_CMD1
+				| PRCM_GPIOCR_DBG_UARTMOD_CMD0);
+		writel(gpiocr, PRCM_GPIOCR);
+		retval = stm_ste_disable_ape_on_mipi60();
+		break;
+
+	case STM_STE_APE_ON_MIPI34_NONE_ON_MIPI60:
+		/* Disable altC3 on GPIO70-74 (STMMOD) & GPIO75-76 (UARTMOD) */
+		gpiocr &= ~(PRCM_GPIOCR_DBG_STM_MOD_CMD1
+				| PRCM_GPIOCR_DBG_UARTMOD_CMD0);
+		writel(gpiocr, PRCM_GPIOCR);
+		retval = stm_ste_disable_ape_on_mipi60();
+		break;
+
+	case STM_STE_MODEM_ON_MIPI34_APE_ON_MIPI60:
+		/* Enable altC3 on GPIO70-74 (STMMOD) and GPIO75-76 (UARTMOD) */
+		gpiocr |= (PRCM_GPIOCR_DBG_STM_MOD_CMD1
+				| PRCM_GPIOCR_DBG_UARTMOD_CMD0);
+		writel(gpiocr, PRCM_GPIOCR);
+
+		/* Enable APE on MIPI60 */
+		retval = nmk_config_pins_sleep(ARRAY_AND_SIZE(mop500_ske_pins));
+		if (retval)
+			pr_err("STM: Failed to disable SKE GPIO\n");
+		else {
+			retval = nmk_config_pins(
+					ARRAY_AND_SIZE(mop500_stm_mipi60_pins));
+			if (retval)
+				pr_err("STM: Failed to enable MIPI60\n");
+		}
+		break;
+
+	case STM_DISCONNECT:
+		retval = nmk_config_pins_sleep(
+				ARRAY_AND_SIZE(mop500_stm_mipi34_pins));
+		if (retval)
+			pr_err("STM: Failed to disable MIPI34\n");
+
+		retval = stm_ste_disable_ape_on_mipi60();
+		break;
+
+	default:
+		pr_err("STM: bad connection type\n");
+		break;
+	}
+	return retval;
+}
+
+/* Possible STM sources (masters) on ux500 */
+enum stm_master {
+	STM_ARM0 =	0,
+	STM_ARM1 =	1,
+	STM_SVA =	2,
+	STM_SIA =	3,
+	STM_SIA_XP70 =	4,
+	STM_PRCMU =	5,
+	STM_MCSBAG =	9
+};
+
+#define STM_ENABLE_ARM0		BIT(STM_ARM0)
+#define STM_ENABLE_ARM1		BIT(STM_ARM1)
+#define STM_ENABLE_SVA		BIT(STM_SVA)
+#define STM_ENABLE_SIA		BIT(STM_SIA)
+#define STM_ENABLE_SIA_XP70	BIT(STM_SIA_XP70)
+#define STM_ENABLE_PRCMU	BIT(STM_PRCMU)
+#define STM_ENABLE_MCSBAG	BIT(STM_MCSBAG)
+
+/*
+ * These are the channels used by NMF and some external softwares
+ * expect the NMF traces to be output on these channels
+ * For legacy reason, we need to reserve them.
+ */
+static const s16 stm_channels_reserved[] = {
+	100,	/* NMF MPCEE channel */
+	101,	/* NMF CM channel */
+	151,	/* NMF HOSTEE channel */
+};
+
+/* On Ux500 we 2 consecutive STMs therefore 512 channels available */
+static struct stm_platform_data stm_pdata = {
+	.regs_phys_base       = U8500_STM_REG_BASE,
+	.channels_phys_base   = U8500_STM_BASE,
+	.id_mask              = 0x000fffff,   /* Ignore revisions differences */
+	.channels_reserved    = stm_channels_reserved,
+	.channels_reserved_sz = ARRAY_SIZE(stm_channels_reserved),
+	/* Enable all except MCSBAG */
+	.masters_enabled      = STM_ENABLE_ARM0 | STM_ENABLE_ARM1 |
+				STM_ENABLE_SVA | STM_ENABLE_PRCMU |
+				STM_ENABLE_SIA | STM_ENABLE_SIA_XP70,
+	/* Provide function for MIPI34/MIPI60 STM connection */
+	.stm_connection       = stm_ste_connection,
+};
+
+struct platform_device ux500_stm_device = {
+	.name = "stm",
+	.id = -1,
+	.dev = {
+		.platform_data = &stm_pdata,
+	},
+};
+#endif /* CONFIG_UX500_STM */
diff --git a/arch/arm/mach-ux500/devices.c b/arch/arm/mach-ux500/devices.c
index ea0a2f92ca7..59e25f21c3f 100644
--- a/arch/arm/mach-ux500/devices.c
+++ b/arch/arm/mach-ux500/devices.c
@@ -11,9 +11,120 @@
 #include <linux/io.h>
 #include <linux/amba/bus.h>
 
+#include <mach/crypto-ux500.h>
 #include <mach/hardware.h>
 #include <mach/setup.h>
 
+#ifdef CONFIG_STE_TRACE_MODEM
+#include <linux/db8500-modem-trace.h>
+#endif
+
+#ifdef CONFIG_STE_TRACE_MODEM
+static struct resource trace_resource = {
+	.start	= 0,
+	.end	= 0,
+	.name	= "db8500-trace-area",
+	.flags	= IORESOURCE_MEM
+};
+
+static struct db8500_trace_platform_data trace_pdata = {
+	.ape_base = U8500_APE_BASE,
+	.modem_base = U8500_MODEM_BASE,
+};
+
+struct platform_device u8500_trace_modem = {
+	.name	= "db8500-modem-trace",
+	.id = 0,
+	.dev = {
+		.init_name = "db8500-modem-trace",
+		.platform_data = &trace_pdata,
+	},
+	.num_resources = 1,
+	.resource = &trace_resource,
+};
+
+static int __init early_trace_modem(char *p)
+{
+	struct resource *data = &trace_resource;
+	u32 size = memparse(p, &p);
+
+	if (*p == '@')
+		data->start = memparse(p + 1, &p);
+	data->end = data->start + size;
+
+	return 0;
+}
+
+early_param("mem_mtrace", early_trace_modem);
+#endif
+
+#ifdef CONFIG_HWMEM
+struct platform_device ux500_hwmem_device = {
+	.name = "hwmem",
+};
+#endif
+
+static struct resource ux500_hash1_resources[] = {
+	[0] = {
+		.start = U8500_HASH1_BASE,
+		.end = U8500_HASH1_BASE + SZ_4K - 1,
+		.flags = IORESOURCE_MEM,
+	}
+};
+
+struct platform_device ux500_hash1_device = {
+	.name = "hash1",
+	.id = -1,
+	.num_resources = 1,
+	.resource = ux500_hash1_resources
+};
+
+static struct resource ux500_cryp1_resources[] = {
+	[0] = {
+		.start = U8500_CRYP1_BASE,
+		.end = U8500_CRYP1_BASE + SZ_4K - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.start = IRQ_DB8500_CRYP1,
+		.end = IRQ_DB8500_CRYP1,
+		.flags = IORESOURCE_IRQ
+	}
+};
+
+static struct cryp_platform_data cryp1_platform_data = {
+	.mem_to_engine = {
+		.dir = STEDMA40_MEM_TO_PERIPH,
+		.src_dev_type = STEDMA40_DEV_SRC_MEMORY,
+		.dst_dev_type = DB8500_DMA_DEV48_CAC1_TX,
+		.src_info.data_width = STEDMA40_WORD_WIDTH,
+		.dst_info.data_width = STEDMA40_WORD_WIDTH,
+		.mode = STEDMA40_MODE_LOGICAL,
+		.src_info.psize = STEDMA40_PSIZE_LOG_4,
+		.dst_info.psize = STEDMA40_PSIZE_LOG_4,
+	},
+	.engine_to_mem = {
+		.dir = STEDMA40_PERIPH_TO_MEM,
+		.src_dev_type = DB8500_DMA_DEV48_CAC1_RX,
+		.dst_dev_type = STEDMA40_DEV_DST_MEMORY,
+		.src_info.data_width = STEDMA40_WORD_WIDTH,
+		.dst_info.data_width = STEDMA40_WORD_WIDTH,
+		.mode = STEDMA40_MODE_LOGICAL,
+		.src_info.psize = STEDMA40_PSIZE_LOG_4,
+		.dst_info.psize = STEDMA40_PSIZE_LOG_4,
+	}
+};
+
+struct platform_device ux500_cryp1_device = {
+	.name = "cryp1",
+	.id = -1,
+	.dev = {
+		.platform_data = &cryp1_platform_data
+	},
+	.num_resources = ARRAY_SIZE(ux500_cryp1_resources),
+	.resource = ux500_cryp1_resources
+};
+
 void __init amba_add_devices(struct amba_device *devs[], int num)
 {
 	int i;
diff --git a/arch/arm/mach-ux500/dma-db5500.c b/arch/arm/mach-ux500/dma-db5500.c
index 1cfab68ae41..3fe4e142e98 100644
--- a/arch/arm/mach-ux500/dma-db5500.c
+++ b/arch/arm/mach-ux500/dma-db5500.c
@@ -14,8 +14,7 @@
 #include <plat/ste_dma40.h>
 #include <mach/setup.h>
 #include <mach/hardware.h>
-
-#include "ste-dma40-db5500.h"
+#include <mach/ste-dma40-db5500.h>
 
 static struct resource dma40_resources[] = {
 	[0] = {
diff --git a/arch/arm/mach-ux500/headsmp.S b/arch/arm/mach-ux500/headsmp.S
index 64fa451edcf..a6be2cdf2b2 100644
--- a/arch/arm/mach-ux500/headsmp.S
+++ b/arch/arm/mach-ux500/headsmp.S
@@ -23,6 +23,7 @@ ENTRY(u8500_secondary_startup)
 	ldmia	r4, {r5, r6}
 	sub	r4, r4, r5
 	add	r6, r6, r4
+	dsb
 pen:	ldr	r7, [r6]
 	cmp	r7, r0
 	bne	pen
diff --git a/arch/arm/mach-ux500/hotplug.c b/arch/arm/mach-ux500/hotplug.c
index dd8037ebccf..607567f2618 100644
--- a/arch/arm/mach-ux500/hotplug.c
+++ b/arch/arm/mach-ux500/hotplug.c
@@ -14,21 +14,41 @@
 
 #include <asm/cacheflush.h>
 
+#ifdef CONFIG_U8500_CPUIDLE
+#include "pm/cpuidle.h"
+#endif
+#include "pm/context.h"
+
 extern volatile int pen_release;
 
 static inline void platform_do_lowpower(unsigned int cpu)
 {
 	flush_cache_all();
 
-	/* we put the platform to just WFI */
 	for (;;) {
+#ifndef CONFIG_U8500_CPUIDLE
 		__asm__ __volatile__("dsb\n\t" "wfi\n\t"
 				: : : "memory");
-		if (pen_release == cpu) {
-			/*
-			 * OK, proper wakeup, we're done
-			 */
-			break;
+#endif
+
+		context_varm_save_core();
+		context_save_cpu_registers();
+
+		context_save_to_sram_and_wfi(true, false);
+
+		context_restore_cpu_registers();
+		context_varm_restore_core();
+
+		/* we put the platform to just WFI */
+		for (;;) {
+			__asm__ __volatile__("dsb\n\t" "wfi\n\t"
+					: : : "memory");
+			if (pen_release == cpu) {
+				/*
+				 * OK, proper wakeup, we're done
+				 */
+				break;
+			}
 		}
 	}
 }
@@ -57,3 +77,4 @@ int platform_cpu_disable(unsigned int cpu)
 	 */
 	return cpu == 0 ? -EPERM : 0;
 }
+
diff --git a/arch/arm/mach-ux500/hwmem-int.c b/arch/arm/mach-ux500/hwmem-int.c
new file mode 100644
index 00000000000..c23049df4a6
--- /dev/null
+++ b/arch/arm/mach-ux500/hwmem-int.c
@@ -0,0 +1,165 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Hardware memory driver integration
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/hwmem.h>
+#include <linux/init.h>
+#include <linux/mm.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+
+/* CONA API */
+void *cona_create(const char *name, phys_addr_t region_paddr,
+							size_t region_size);
+void *cona_alloc(void *instance, size_t size);
+void cona_free(void *instance, void *alloc);
+phys_addr_t cona_get_alloc_paddr(void *alloc);
+void *cona_get_alloc_kaddr(void *instance, void *alloc);
+size_t cona_get_alloc_size(void *alloc);
+
+struct hwmem_mem_type_struct *hwmem_mem_types;
+unsigned int hwmem_num_mem_types;
+
+static phys_addr_t hwmem_paddr;
+static size_t hwmem_size;
+
+static int __init parse_hwmem_param(char *p)
+{
+	hwmem_size = memparse(p, &p);
+
+	if (*p != '@')
+		goto no_at;
+
+	hwmem_paddr = memparse(p + 1, &p);
+
+	return 0;
+
+no_at:
+	hwmem_size = 0;
+
+	return -EINVAL;
+}
+early_param("hwmem", parse_hwmem_param);
+
+static int __init setup_hwmem(void)
+{
+	static const unsigned int NUM_MEM_TYPES = 2;
+
+	int ret;
+
+	if (hwmem_paddr != PAGE_ALIGN(hwmem_paddr) ||
+		hwmem_size != PAGE_ALIGN(hwmem_size) || hwmem_size == 0) {
+		printk(KERN_WARNING "HWMEM: hwmem_paddr !="
+		" PAGE_ALIGN(hwmem_paddr) || hwmem_size !="
+		" PAGE_ALIGN(hwmem_size) || hwmem_size == 0\n");
+		return -ENOMSG;
+	}
+
+	hwmem_mem_types = kzalloc(sizeof(struct hwmem_mem_type_struct) *
+						NUM_MEM_TYPES, GFP_KERNEL);
+	if (hwmem_mem_types == NULL)
+		return -ENOMEM;
+
+	hwmem_mem_types[0].id = HWMEM_MEM_SCATTERED_SYS;
+	hwmem_mem_types[0].allocator_api.alloc = cona_alloc;
+	hwmem_mem_types[0].allocator_api.free = cona_free;
+	hwmem_mem_types[0].allocator_api.get_alloc_paddr =
+							cona_get_alloc_paddr;
+	hwmem_mem_types[0].allocator_api.get_alloc_kaddr =
+							cona_get_alloc_kaddr;
+	hwmem_mem_types[0].allocator_api.get_alloc_size = cona_get_alloc_size;
+	hwmem_mem_types[0].allocator_instance = cona_create("hwmem",
+						hwmem_paddr, hwmem_size);
+	if (IS_ERR(hwmem_mem_types[0].allocator_instance)) {
+		ret = PTR_ERR(hwmem_mem_types[0].allocator_instance);
+		goto hwmem_ima_init_failed;
+	}
+
+	hwmem_mem_types[1] = hwmem_mem_types[0];
+	hwmem_mem_types[1].id = HWMEM_MEM_CONTIGUOUS_SYS;
+
+	hwmem_num_mem_types = NUM_MEM_TYPES;
+
+	return 0;
+
+hwmem_ima_init_failed:
+	kfree(hwmem_mem_types);
+
+	return ret;
+}
+arch_initcall_sync(setup_hwmem);
+
+enum hwmem_alloc_flags cachi_get_cache_settings(
+			enum hwmem_alloc_flags requested_cache_settings)
+{
+	static const u32 CACHE_ON_FLAGS_MASK = HWMEM_ALLOC_HINT_CACHED |
+		HWMEM_ALLOC_HINT_CACHE_WB | HWMEM_ALLOC_HINT_CACHE_WT |
+		HWMEM_ALLOC_HINT_CACHE_NAOW | HWMEM_ALLOC_HINT_CACHE_AOW |
+				HWMEM_ALLOC_HINT_INNER_AND_OUTER_CACHE |
+					HWMEM_ALLOC_HINT_INNER_CACHE_ONLY;
+
+	enum hwmem_alloc_flags cache_settings;
+
+	if (!(requested_cache_settings & CACHE_ON_FLAGS_MASK) &&
+		requested_cache_settings & (HWMEM_ALLOC_HINT_NO_WRITE_COMBINE |
+		HWMEM_ALLOC_HINT_UNCACHED | HWMEM_ALLOC_HINT_WRITE_COMBINE))
+		/*
+		 * We never use uncached as it's extremely slow and there is
+		 * no scenario where it would be better than buffered memory.
+		 */
+		return HWMEM_ALLOC_HINT_WRITE_COMBINE;
+
+	/*
+	 * The user has specified cached or nothing at all, both are treated as
+	 * cached.
+	 */
+	cache_settings = (requested_cache_settings &
+		 ~(HWMEM_ALLOC_HINT_UNCACHED |
+		HWMEM_ALLOC_HINT_NO_WRITE_COMBINE |
+		HWMEM_ALLOC_HINT_INNER_CACHE_ONLY |
+		HWMEM_ALLOC_HINT_CACHE_NAOW)) |
+		HWMEM_ALLOC_HINT_WRITE_COMBINE | HWMEM_ALLOC_HINT_CACHED |
+		HWMEM_ALLOC_HINT_CACHE_AOW |
+		HWMEM_ALLOC_HINT_INNER_AND_OUTER_CACHE;
+	if (!(cache_settings & (HWMEM_ALLOC_HINT_CACHE_WB |
+						HWMEM_ALLOC_HINT_CACHE_WT)))
+		cache_settings |= HWMEM_ALLOC_HINT_CACHE_WB;
+	/*
+	 * On ARMv7 "alloc on write" is just a hint so we need to assume the
+	 * worst case ie "alloc on write". We would however like to remember
+	 * the requested "alloc on write" setting so that we can pass it on to
+	 * the hardware, we use the reserved bit in the alloc flags to do that.
+	 */
+	if (requested_cache_settings & HWMEM_ALLOC_HINT_CACHE_AOW)
+		cache_settings |= HWMEM_ALLOC_RESERVED_CHI;
+	else
+		cache_settings &= ~HWMEM_ALLOC_RESERVED_CHI;
+
+	return cache_settings;
+}
+
+void cachi_set_pgprot_cache_options(enum hwmem_alloc_flags cache_settings,
+							pgprot_t *pgprot)
+{
+	if (cache_settings & HWMEM_ALLOC_HINT_CACHED) {
+		if (cache_settings & HWMEM_ALLOC_HINT_CACHE_WT)
+			*pgprot = __pgprot_modify(*pgprot, L_PTE_MT_MASK,
+							L_PTE_MT_WRITETHROUGH);
+		else {
+			if (cache_settings & HWMEM_ALLOC_RESERVED_CHI)
+				*pgprot = __pgprot_modify(*pgprot,
+					L_PTE_MT_MASK, L_PTE_MT_WRITEALLOC);
+			else
+				*pgprot = __pgprot_modify(*pgprot,
+					L_PTE_MT_MASK, L_PTE_MT_WRITEBACK);
+		}
+	} else {
+		*pgprot = pgprot_writecombine(*pgprot);
+	}
+}
diff --git a/arch/arm/mach-ux500/hwreg.c b/arch/arm/mach-ux500/hwreg.c
new file mode 100644
index 00000000000..1a47e60ed46
--- /dev/null
+++ b/arch/arm/mach-ux500/hwreg.c
@@ -0,0 +1,651 @@
+/*
+ * Copyright (C) 2011 ST-Ericsson SA
+ *
+ * Author: Etienne CARRIERE <etienne.carriere@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ *  HWREG: debug purpose module to map declared IOs and read/write
+ *         access from debugfs entries.
+ *
+ * HWREG 32bit DB8500 v2.0 register access
+ * =======================================
+ *
+ * 32bit read:
+ * # echo <addr>  >  <debugfs>/mem/reg-addr
+ * # cat <debugfs>/mem/reg-val
+ *
+ * 32bit write:
+ * # echo <addr>   >  <debugfs>/mem/reg-addr
+ * # echo <value>  >  <debugfs>/mem/reg-val
+ *
+ * <addr>   0x-prefixed hexadecimal
+ * <value>  decimal or 0x-prefixed hexadecimal
+ *
+ * HWREG DB8500 formated read/write access
+ * =======================================
+ *
+ * Read:  read data, data>>SHIFT, data&=MASK, output data
+ *        [0xABCDEF98] shift=12 mask=0xFFF => 0x00000CDE
+ * Write: read data, data &= ~(MASK<<SHIFT), data |= (VALUE<<SHIFT), write data
+ *        [0xABCDEF98] shift=12 mask=0xFFF value=0x123 => [0xAB123F98]
+ *
+ * Usage:
+ * # echo "CMD [OPTIONS] ADRESS [VALUE]" > $debugfs/mem/hwreg
+ *
+ * CMD      read      read access
+ *          write     write access
+ *
+ * ADDRESS  target reg physical addr (0x-hexa)
+ *
+ * VALUE    (write) value to be updated
+ *
+ * OPTIONS
+ *  -d|-dec            (read) output in decimal
+ *  -h|-hexa           (read) output in 0x-hexa (default)
+ *  -l|-w|-b           32bit (default), 16bit or 8bit reg access
+ *  -m|-mask MASK      0x-hexa mask (default 0xFFFFFFFF)
+ *  -s|-shift SHIFT    bit shift value (read:left, write:right)
+ *  -o|-offset OFFSET  address offset to add to ADDRESS value
+ *
+ * Warning: bit shift operation is applied to bit-mask.
+ * Warning: bit shift direction depends on read or right command.
+ *
+ * Examples:
+ *
+ *  before: [*ADDRESS = 0xABCDEF98]
+ *  # echo read -h -mask 0xFFF -shift 12 ADDRESS > hwreg
+ *  # cat hwreg-shift
+ *  0x0000CDE
+ *  # echo write -h -mask 0xFFF -shift 12 ADDRESS 0x123 > hwreg
+ *  # cat hwreg-shift
+ *  0x0000123
+ *  after [*ADDRESS = 0xAB123F98]
+ *
+ *  before: [*ADDRESS = 0xABCDEF98]
+ *  # echo read -h -mask 0x00F0F000 ADDRESS 0x12345678 > hwreg
+ *  # cat hwreg-shift
+ *  0x00C0E000
+ *  # echo write -h -mask 0x00F0F000 ADDRESS 0x12345678 > hwreg
+ *  # cat hwreg-shift
+ *  0xAB3D5F98
+ *  after [*ADDRESS = 0xAB123F98]
+ *
+ * Read DB8500 version (full ID, chip version ID, chip version ID):
+ *
+ * echo read 0x9001DBF4 > hwreg
+ * cat hwreg
+ * echo read -m 0xFFFF -s 8 0x9001DBF4 > hwreg
+ * cat hwreg
+ * echo read -m 0xFF -s 0 0x9001DBF4 > hwreg
+ * cat hwreg
+ *
+ * Read and Enable/Disable I2C PRCMU clock:
+ *
+ * printf "I2CCLK = "  && echo read  -m 1 -s 8 0x80157520 > hwreg
+ * cat /sys/kernel/debug/db8500/hwreg
+ * printf "I2CCLK off" && echo write -m 1 -s 8 0x80157518 1 > hwreg
+ * printf "I2CCLK on"  && echo write -m 1 -s 8 0x80157510 1 > hwreg
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/string.h>
+#include <linux/ctype.h>
+
+#include <asm/uaccess.h>
+#include <asm/io.h>
+
+#include <mach/hardware.h>
+
+/*
+ * temporary definitions
+ * The following declarations are to be removed
+ * when kernel/arch/arm/mach-ux8500/include/mach/db8500-regs.h is up-to-date
+ */
+
+/* DDR-SDRAM chip-select 0 (0x0000 0000 : 0x1FFF FFFF) */
+#ifndef U8500_SCU_CD_R4_BASE
+#define U8500_SCU_CD_R4_BASE      0x17c40000
+#endif
+
+#ifndef U8500_SCU_AD_R4_BASE
+#define U8500_SCU_AD_R4_BASE      0x17d40000
+#endif
+
+#ifndef U8500_HSI2CMODEMR4_BASE
+#define U8500_HSI2CMODEMR4_BASE   0x17e02000
+#endif
+/* End of temporary definitions */
+
+static struct dentry	*hwreg_debugfs_dir;
+
+/* 32bit read/write ressources */
+static u32 debug_address; /* shared: single read/write access */
+
+/* hwreg entry ressources */
+struct hwreg_cfg {
+	uint addr;		/* target physical addr to access */
+	uint fmt;		/* format */
+	uint mask;		/* read/write mask, applied before any bit shift */
+	int  shift;		/* bit shift (read:right shift, write:left shift */
+};
+#define REG_FMT_DEC(c) ((c)->fmt & 0x1)		/* bit 0: 0=hexa, 1=dec */
+#define REG_FMT_HEX(c) (!REG_FMT_DEC(c))	/* bit 0: 0=hexa, 1=dec */
+#define REG_FMT_32B(c) (((c)->fmt & 0x6)==0x0)	/* bit[2:1]=0 => 32b access */
+#define REG_FMT_16B(c) (((c)->fmt & 0x6)==0x2)	/* bit[2:1]=1 => 16b access */
+#define REG_FMT_8B(c)  (((c)->fmt & 0x6)==0x4)	/* bit[2:1]=2 => 8b access */
+
+static struct hwreg_cfg hwreg_cfg = {
+	.addr = 0,			/* default: invalid phys addr */
+	.fmt = 0,			/* default: 32bit access, hex output */
+	.mask = 0xFFFFFFFF,		/* default: no mask */
+	.shift = 0,			/* default: no bit shift */
+};
+
+/* HWREG guts: mapping table */
+
+struct hwreg_io_range {
+	u32 base;
+	u32 size;
+	u8 *addr;
+};
+
+/*
+ * HWREG guts: mapping table
+ */
+static struct hwreg_io_range hwreg_io_map[] = {
+	/* Periph1 Peripherals */
+	{.base = U8500_PER1_BASE,	.size = 0x10000},
+	/* Periph2 Peripherals */
+	{.base = U8500_PER2_BASE,	.size = 0x10000},
+	/* Periph3 Peripherals */
+	{.base = U8500_PER3_BASE,	.size = 0x10000},
+	/* Periph4 Peripherals */
+	{.base = U8500_PER4_BASE,	.size = 0x70000},
+	/* Periph5 Periphals */
+	{.base = U8500_PER5_BASE,	.size = 0x20000},
+	/* Periph6 Peripherals */
+	{.base = U8500_PER6_BASE,	.size = 0x10000},
+	/*
+	 * Snoop Control Unit, A9 Private interrupt IF,
+	 * A9 private peripherals, Level-2 Cache Configuration registers,
+	 * and some reserved area
+	 */
+	{.base = U8500_SCU_BASE,	.size = 0x4000},
+
+	/* DISPLAY Ctrl. configuration registers */
+	{.base = U8500_MCDE_BASE,	.size = SZ_4K},
+
+	/* DSI1 link registers */
+	{.base = U8500_DSI_LINK1_BASE,	.size = SZ_4K},
+
+	/* DSI2 link registers */
+	{.base = U8500_DSI_LINK2_BASE,	.size = SZ_4K},
+
+	/* DSI3 link registers */
+	{.base = U8500_DSI_LINK3_BASE,	.size = SZ_4K},
+
+	/* DMA Ctrl. configuration registers (base address changed in V1) */
+	{.base = U8500_DMA_BASE,	.size = SZ_4K},
+
+	/* 0xB7A00000 -> 0xB7E04000: Modem I2C */
+	{.base = U8500_MODEM_I2C,	.size = 0x404000},
+
+	/* 0xA0390000 -> 0xA039FFFF: SBAG configuration registers */
+	{.base = U8500_SBAG_BASE,	.size = SZ_4K},
+
+	/* 0xA0300000 -> 0xA031FFFF: SGA configuration registers */
+	{.base = U8500_SGA_BASE,	.size = 0x10000},
+
+	/* 0xA0200000 -> 0xA02FFFFF: Smart Imaging Acc. Data Memory space (SIA) */
+	{.base = U8500_SIA_BASE,	.size = 0x60000},
+
+	/* 0xA0100000 -> 0xA01FFFFF: Smart Video Acc. Data Memory space (SVA) */
+	{.base = U8500_SVA_BASE,	.size = 0x60000},
+
+	/* 0x81000000 -> 0x8103FFFF: Main ICN Crossbar configuration registers */
+	{.base = U8500_ICN_BASE,	.size = 0x2000},
+
+	/* 0x80140000 -> 0x8014FFFF: HSEM (Semaphores) configuration  */
+	{.base = U8500_HSEM_BASE,	.size = SZ_4K},
+
+	/* 0x80130000 -> 0x8013FFFF: B2R2 configuration registers */
+	{.base = U8500_B2R2_BASE,	.size = SZ_4K},
+
+	/* 0x80100000 -> 0x8010FFFF: STM */
+	{.base = U8500_STM_BASE,	.size = 0x10000},
+
+	/* High part of embedded boot ROM */
+	{.base = U8500_ASIC_ID_BASE,	.size = SZ_4K},
+
+	/* 0x17C4 0000 : 0x17C4 007C */
+	{.base = U8500_SCU_CD_R4_BASE,  .size = SZ_4K},
+
+	/* 0x17D4 0000 : 0x17D4 041C */
+	{.base = U8500_SCU_AD_R4_BASE,  .size = SZ_4K},
+
+	/* 0x17E0 2000 : 0x17E0 2FFC */
+	{.base = U8500_HSI2CMODEMR4_BASE, .size = SZ_4K},
+
+	{.base = 0, .size = 0, },
+
+};
+
+static void hwreg_io_init(void)
+{
+	int i;
+
+	for (i = 0; hwreg_io_map[i].base; ++i) {
+		hwreg_io_map[i].addr = ioremap(hwreg_io_map[i].base,
+						 hwreg_io_map[i].size);
+		if (!hwreg_io_map[i].addr)
+			printk(KERN_WARNING
+				"%s: ioremap for %d (%08x) failed\n",
+				__func__, i, hwreg_io_map[i].base);
+	}
+}
+
+static void hwreg_io_exit(void)
+{
+	int i;
+
+	for (i = 0; hwreg_io_map[i].base; ++i)
+		if (hwreg_io_map[i].addr)
+			iounmap(hwreg_io_map[i].addr);
+}
+
+static void *hwreg_io_ptov(u32 phys)
+{
+	int i;
+
+	for (i = 0; hwreg_io_map[i].base; ++i) {
+		u32 base = hwreg_io_map[i].base;
+		u32 size = hwreg_io_map[i].size;
+		u8 *addr = hwreg_io_map[i].addr;
+
+		if (phys < base || phys >= base + size)
+			continue;
+
+		if (addr)
+			return addr + phys - base;
+
+		break;
+	}
+
+	return NULL;
+}
+
+
+/*
+ * HWREG 32bit DB8500 register read/write access debugfs part
+ */
+
+static int hwreg_address_print(struct seq_file *s, void *p)
+{
+	return seq_printf(s, "0x%08X\n", debug_address);
+}
+
+static int hwreg_address_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, hwreg_address_print, inode->i_private);
+}
+
+static ssize_t hwreg_address_write(struct file *file,
+	const char __user *user_buf, size_t count, loff_t *ppos)
+{
+	int err;
+	unsigned long user_address;
+
+	err = kstrtoul_from_user(user_buf, count, 0, &user_address);
+
+	if (err)
+		return err;
+
+	if (hwreg_io_ptov(user_address)==NULL)
+		return -EADDRNOTAVAIL;
+
+	debug_address = user_address;
+	return count;
+}
+
+static int hwreg_value_print(struct seq_file *s, void *p)
+{
+	void *ptr;
+
+	ptr = hwreg_io_ptov(debug_address);
+	if (ptr == NULL)
+		return -EADDRNOTAVAIL;
+	seq_printf(s, "0x%X\n", readl(ptr));
+	return 0;
+}
+
+static int hwreg_value_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, hwreg_value_print, inode->i_private);
+}
+
+static ssize_t hwreg_value_write(struct file *file,
+	const char __user *user_buf, size_t count, loff_t *ppos)
+{
+	int err;
+	unsigned long user_val;
+	void *ptr;
+
+	err = kstrtoul_from_user(user_buf, count, 0, &user_val);
+
+	if (err)
+		return err;
+
+	if ((ptr = hwreg_io_ptov(debug_address)) == NULL)
+		return -EFAULT;
+	writel(user_val, ptr);
+	return count;
+}
+
+static const struct file_operations hwreg_address_fops = {
+	.open = hwreg_address_open,
+	.write = hwreg_address_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+static const struct file_operations hwreg_value_fops = {
+	.open = hwreg_value_open,
+	.write = hwreg_value_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+/* 'map' read entry: display current HWREG IO mapping table */
+static int hwreg_map_print(struct seq_file *s, void *p)
+{
+	int err, i;
+	for (i = 0; hwreg_io_map[i].base; ++i) {
+		err = seq_printf(s, "%d: 0x%08X => 0x%08X\n",
+				 i, hwreg_io_map[i].base,
+				 hwreg_io_map[i].base+hwreg_io_map[i].size);
+		if (err < 0)
+			return -ENOMEM;
+	}
+	return 0;
+}
+static int hwreg_map_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, hwreg_map_print, inode->i_private);
+}
+
+static const struct file_operations hwreg_map_fops = {
+	.open = hwreg_map_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+/*
+ * HWREG DB8500 formated routines
+ */
+
+static int hwreg_print(struct seq_file *s, void *d)
+{
+	struct hwreg_cfg *c = (struct hwreg_cfg*) s->private;
+	void *p;
+	uint  v;
+
+	if ((c==NULL) || ((p = hwreg_io_ptov(c->addr))==NULL))
+		return -EADDRNOTAVAIL;
+
+	v = (uint) (REG_FMT_32B(c) ? readl(p) : REG_FMT_16B(c) ? readw(p) : readb(p));
+	v = (c->shift>=0) ? v >> c->shift : v << (-c->shift);
+	v = v & c->mask;
+
+	if (REG_FMT_DEC(c))
+		seq_printf(s, "%d\n", v);
+	else if (REG_FMT_32B(c))
+		seq_printf(s, "0x%08X\n", v);
+	else if (REG_FMT_32B(c))
+		seq_printf(s, "0x%04X\n", v);
+	else
+		seq_printf(s, "0x%02X\n", v);
+	return 0;
+}
+
+static int hwreg_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, hwreg_print, inode->i_private);
+}
+
+/*
+ * return length of an ASCII numerical value, 0 is string is not a numerical
+ * value. string shall start at value 1st char.
+ * string can be tailed with \0 or space or newline chars only.
+ * value can be decimal or hexadecimal (prefixed 0x or 0X).
+ */
+static int strval_len(char *b)
+{
+	char *s = b;
+	if((*s=='0') && ((*(s+1)=='x') || (*(s+1)=='X'))) {
+		s += 2;
+		for (; *s && (*s!=' ') && (*s!='\n'); s++) {
+			if (!isxdigit(*s))
+				return 0;
+		}
+	} else {
+		if (*s=='-')
+			s++;
+		for (; *s && (*s!=' ') && (*s!='\n'); s++) {
+			if (!isdigit(*s))
+				return 0;
+		}
+	}
+	return (int) (s-b);
+}
+
+/*
+ * parse hwreg input data.
+ * update global hwreg_cfg only if input data syntax is ok.
+ */
+static ssize_t hwreg_common_write(char *b, struct hwreg_cfg *cfg)
+{
+	uint write, val=0, offset=0;
+	struct hwreg_cfg loc = {
+		.addr = 0,		/* default: invalid phys addr */
+		.fmt = 0,		/* default: 32bit access, hex output */
+		.mask = 0xFFFFFFFF,	/* default: no mask */
+		.shift = 0,		/* default: no bit shift */
+	};
+
+	/* read or write ? */
+	if(!strncmp(b, "read ", 5)) {
+		write = 0;
+		b += 5;
+	} else if (!strncmp(b, "write ", 6)) {
+		write = 1;
+		b += 6;
+	} else {
+		return -EINVAL;
+	}
+
+	/* OPTIONS -l|-w|-b -s -m -o */
+	while((*b==' ') || (*b=='-')) {
+		if (*(b-1)!=' ') {
+			b++;
+			continue;
+		}
+		if ((!strncmp(b, "-d ", 3)) || (!strncmp(b, "-dec ", 5))) {
+			b += (*(b+2)==' ') ? 3 : 5;
+			loc.fmt |= (1<<0);
+		} else if ((!strncmp(b, "-h ", 3)) || (!strncmp(b, "-hex ", 5))) {
+			b += (*(b+2)==' ') ? 3 : 5;
+			loc.fmt &= ~(1<<0);
+		} else if ((!strncmp(b, "-m ", 3)) || (!strncmp(b, "-mask ", 6))) {
+			b += (*(b+2)==' ') ? 3 : 6;
+			if (strval_len(b)==0)
+				return -EINVAL;
+			loc.mask = simple_strtoul(b, &b, 0);
+		} else if ((!strncmp(b, "-s ", 3)) || (!strncmp(b,"-shift ", 7))) {
+			b += (*(b+2)==' ') ? 3 : 7;
+			if (strval_len(b)==0)
+				return -EINVAL;
+			loc.shift = simple_strtol(b, &b, 0);
+
+		} else if ((!strncmp(b, "-o ", 3)) || (!strncmp(b,"-offset ", 8))) {
+			b += (*(b+2)==' ') ? 3 : 8;
+			if (strval_len(b)==0)
+				return -EINVAL;
+			offset = simple_strtol(b, &b, 0);
+		} else if (!strncmp(b, "-l ", 3)) {
+			b += 3;
+			loc.fmt = (loc.fmt & ~(3<<1)) | (0<<1);
+		} else if (!strncmp(b, "-w ", 3)) {
+			b += 3;
+			loc.fmt = (loc.fmt & ~(3<<1)) | (1<<1);
+		} else if (!strncmp(b, "-b ", 3)) {
+			b += 3;
+			loc.fmt = (loc.fmt & ~(3<<1)) | (2<<1);
+		} else {
+			return -EINVAL;
+		}
+	}
+	/* get arg ADDRESS */
+	if (strval_len(b)==0)
+		return -EINVAL;
+	loc.addr = simple_strtoul(b, &b, 0);
+	loc.addr += offset;
+	if (hwreg_io_ptov(loc.addr) == NULL)
+		return -EINVAL;
+
+	if (write) {
+		while(*b==' ') b++; /* skip spaces up to arg VALUE */
+		if (strval_len(b)==0)
+			return -EINVAL;
+		val = simple_strtoul(b, &b, 0);
+	}
+
+	/* args are ok, update target cfg (mainly for read) */
+	*cfg = loc;
+
+#ifdef DEBUG
+	printk(KERN_INFO "HWREG request: %s %d-bit reg, %s, addr=0x%08X, "
+	       "mask=0x%X, shift=%d value=0x%X\n",
+	       (write)?"write":"read",
+	       REG_FMT_32B(cfg)?32:REG_FMT_16B(cfg)?16:8,
+	       REG_FMT_DEC(cfg)?"decimal":"hexa",
+	       cfg->addr, cfg->mask, cfg->shift, val);
+#endif
+
+	if (write) {
+		void *p = hwreg_io_ptov(cfg->addr);
+		uint d = (uint) (REG_FMT_32B(cfg)) ? readl(p) :
+			(REG_FMT_16B(cfg)) ? readw(p) : readb(p);
+
+		if (cfg->shift>=0) {
+			d &= ~(cfg->mask << (cfg->shift));
+			val = (val & cfg->mask) << (cfg->shift);
+		} else {
+			d &= ~(cfg->mask >> (-cfg->shift));
+			val = (val & cfg->mask) >> (-cfg->shift);
+		}
+		val = val | d;
+
+		/*  read reg, reset mask field and update value bit-field */
+		if (REG_FMT_32B(cfg))
+			writel(val, p);
+		else if (REG_FMT_16B(cfg))
+			writew(val, p);
+		else
+			writeb(val, p);
+	}
+	return 0;
+}
+
+static ssize_t hwreg_write(struct file *file,
+			   const char __user *user_buf,
+			   size_t count, loff_t *ppos)
+{
+	char buf[128];
+	int buf_size, ret;
+
+	/* Get userspace string and assure termination */
+	buf_size = min(count, (sizeof(buf)-1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	/* get args and process */
+	ret = hwreg_common_write(buf, &hwreg_cfg);
+	return (ret) ? ret : buf_size;
+}
+
+static const struct file_operations hwreg_fops = {
+	.open = hwreg_open,
+	.write = hwreg_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+/*
+ * hwreg module init/cleanup
+ */
+static int __init hwreg_initialize(void)
+{
+	static struct dentry *file;
+	hwreg_io_init();
+
+	hwreg_debugfs_dir = debugfs_create_dir("mem", NULL);
+	if (!hwreg_debugfs_dir)
+		goto debugfs_err;
+
+	file = debugfs_create_file("reg-addr",
+				   (S_IRUGO | S_IWUGO), hwreg_debugfs_dir,
+				   NULL, &hwreg_address_fops);
+	if (!file)
+		goto debugfs_err;
+	file = debugfs_create_file("reg-val",
+				   (S_IRUGO | S_IWUGO), hwreg_debugfs_dir,
+				   NULL, &hwreg_value_fops);
+	if (!file)
+		goto debugfs_err;
+	file = debugfs_create_file("reg-map",
+				   (S_IRUGO),
+				   hwreg_debugfs_dir, NULL, &hwreg_map_fops);
+	if (!file)
+		goto debugfs_err;
+	file = debugfs_create_file("hwreg",
+				   (S_IRUGO),
+				   hwreg_debugfs_dir, &hwreg_cfg, &hwreg_fops);
+	if (!file)
+		goto debugfs_err;
+	return 0;
+
+debugfs_err:
+	if (hwreg_debugfs_dir)
+		debugfs_remove_recursive(hwreg_debugfs_dir);
+	printk(KERN_ERR "hwreg: failed to register debugfs entries.\n");
+	return -1;
+}
+
+static void __exit hwreg_finalize(void)
+{
+	debugfs_remove_recursive(hwreg_debugfs_dir);
+	hwreg_io_exit();
+}
+
+module_init(hwreg_initialize);
+module_exit(hwreg_finalize);
+
+MODULE_AUTHOR("ST-Ericsson");
+MODULE_DESCRIPTION("DB8500 HW registers access through debugfs");
+MODULE_LICENSE("GPL");
diff --git a/arch/arm/mach-ux500/include/mach/ab8500-accdet.h b/arch/arm/mach-ux500/include/mach/ab8500-accdet.h
new file mode 100644
index 00000000000..b1b157e317e
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/ab8500-accdet.h
@@ -0,0 +1,96 @@
+/*
+ * Copyright ST-Ericsson 2011.
+ *
+ * Author: Jarmo K. Kuronen <jarmo.kuronen@symbio.com> for ST Ericsson.
+ * Licensed under GPLv2.
+ */
+
+#ifndef _AB8500_ACCDET_H
+#define _AB8500_ACCDET_H
+
+/*
+* Debounce times for AccDet1 input
+* @0x880 [2:0]
+*/
+#define ACCDET1_DB_0ms		0x00
+#define ACCDET1_DB_10ms		0x01
+#define ACCDET1_DB_20ms		0x02
+#define ACCDET1_DB_30ms		0x03
+#define ACCDET1_DB_40ms		0x04
+#define ACCDET1_DB_50ms		0x05
+#define ACCDET1_DB_60ms		0x06
+#define ACCDET1_DB_70ms		0x07
+
+/*
+* Voltage threshold for AccDet1 input
+* @0x880 [6:3]
+*/
+#define ACCDET1_TH_1100mV	0x40
+#define ACCDET1_TH_1200mV	0x48
+#define ACCDET1_TH_1300mV	0x50
+#define ACCDET1_TH_1400mV	0x58
+#define ACCDET1_TH_1500mV	0x60
+#define ACCDET1_TH_1600mV	0x68
+#define ACCDET1_TH_1700mV	0x70
+#define ACCDET1_TH_1800mV	0x78
+
+/*
+* Voltage threshold for AccDet21 input
+* @0x881 [3:0]
+*/
+#define ACCDET21_TH_300mV	0x00
+#define ACCDET21_TH_400mV	0x01
+#define ACCDET21_TH_500mV	0x02
+#define ACCDET21_TH_600mV	0x03
+#define ACCDET21_TH_700mV	0x04
+#define ACCDET21_TH_800mV	0x05
+#define ACCDET21_TH_900mV	0x06
+#define ACCDET21_TH_1000mV	0x07
+#define ACCDET21_TH_1100mV	0x08
+#define ACCDET21_TH_1200mV	0x09
+#define ACCDET21_TH_1300mV	0x0a
+#define ACCDET21_TH_1400mV	0x0b
+#define ACCDET21_TH_1500mV	0x0c
+#define ACCDET21_TH_1600mV	0x0d
+#define ACCDET21_TH_1700mV	0x0e
+#define ACCDET21_TH_1800mV	0x0f
+
+/*
+* Voltage threshold for AccDet22 input
+* @0x881 [7:4]
+*/
+#define ACCDET22_TH_300mV	0x00
+#define ACCDET22_TH_400mV	0x10
+#define ACCDET22_TH_500mV	0x20
+#define ACCDET22_TH_600mV	0x30
+#define ACCDET22_TH_700mV	0x40
+#define ACCDET22_TH_800mV	0x50
+#define ACCDET22_TH_900mV	0x60
+#define ACCDET22_TH_1000mV	0x70
+#define ACCDET22_TH_1100mV	0x80
+#define ACCDET22_TH_1200mV	0x90
+#define ACCDET22_TH_1300mV	0xa0
+#define ACCDET22_TH_1400mV	0xb0
+#define ACCDET22_TH_1500mV	0xc0
+#define ACCDET22_TH_1600mV	0xd0
+#define ACCDET22_TH_1700mV	0xe0
+#define ACCDET22_TH_1800mV	0xf0
+
+/**
+ * struct ab8500_accdet_platform_data - AV Accessory detection specific
+ * platform data
+ * @btn_keycode		Keycode to be sent when accessory button is pressed.
+ * @accdet1_dbth	Debounce time + voltage threshold for accdet 1 input.
+ * @accdet2122_th	Voltage thresholds for accdet21 and accdet22 inputs.
+ * @is_detection_inverted	Whether the accessory insert/removal, button
+ * press/release irq's are inverted.
+ */
+struct ab8500_accdet_platform_data {
+	int btn_keycode;
+	u8 accdet1_dbth;
+	u8 accdet2122_th;
+	unsigned int video_ctrl_gpio;
+	bool is_detection_inverted;
+};
+
+#endif /* _AB8500_ACCDET_H */
diff --git a/arch/arm/mach-ux500/include/mach/ab8500_codec.h b/arch/arm/mach-ux500/include/mach/ab8500_codec.h
new file mode 100644
index 00000000000..d45dea66b5d
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/ab8500_codec.h
@@ -0,0 +1,327 @@
+/*****************************************************************************/
+/**
+*  © ST-Ericsson, 2009 - All rights reserved
+* Reproduction and Communication of this document is strictly prohibited
+* unless specifically authorized in writing by ST-Ericsson
+*
+* \brief   Public header file for AB8500 Codec
+* \author  ST-Ericsson
+*/
+/*****************************************************************************/
+
+#ifndef _AB8500_CODEC_H_
+#define _AB8500_CODEC_H_
+
+/*---------------------------------------------------------------------
+ * Includes
+ *--------------------------------------------------------------------*/
+#include "hcl_defs.h"
+#include "debug.h"
+#include <mach/ab8500_codec_p.h>
+
+/*---------------------------------------------------------------------
+ * Define
+ *--------------------------------------------------------------------*/
+#ifdef __cplusplus
+extern "C" {
+#endif
+	typedef enum {
+		AB8500_CODEC_OK,
+		AB8500_CODEC_ERROR,
+		AB8500_CODEC_UNSUPPORTED_FEATURE,
+		AB8500_CODEC_INVALID_PARAMETER,
+		AB8500_CODEC_CONFIG_NOT_COHERENT,
+		AB8500_CODEC_TRANSACTION_FAILED
+	} t_ab8500_codec_error;
+
+	typedef enum {
+		AB8500_CODEC_MASTER_MODE_DISABLE,
+		AB8500_CODEC_MASTER_MODE_ENABLE
+	} t_ab8500_codec_master_mode;
+
+	typedef enum {
+		AB8500_CODEC_SLOT0,
+		AB8500_CODEC_SLOT1,
+		AB8500_CODEC_SLOT2,
+		AB8500_CODEC_SLOT3,
+		AB8500_CODEC_SLOT4,
+		AB8500_CODEC_SLOT5,
+		AB8500_CODEC_SLOT6,
+		AB8500_CODEC_SLOT7,
+		AB8500_CODEC_SLOT8,
+		AB8500_CODEC_SLOT9,
+		AB8500_CODEC_SLOT10,
+		AB8500_CODEC_SLOT11,
+		AB8500_CODEC_SLOT12,
+		AB8500_CODEC_SLOT13,
+		AB8500_CODEC_SLOT14,
+		AB8500_CODEC_SLOT15,
+		AB8500_CODEC_SLOT16,
+		AB8500_CODEC_SLOT17,
+		AB8500_CODEC_SLOT18,
+		AB8500_CODEC_SLOT19,
+		AB8500_CODEC_SLOT20,
+		AB8500_CODEC_SLOT21,
+		AB8500_CODEC_SLOT22,
+		AB8500_CODEC_SLOT23,
+		AB8500_CODEC_SLOT24,
+		AB8500_CODEC_SLOT25,
+		AB8500_CODEC_SLOT26,
+		AB8500_CODEC_SLOT27,
+		AB8500_CODEC_SLOT28,
+		AB8500_CODEC_SLOT29,
+		AB8500_CODEC_SLOT30,
+		AB8500_CODEC_SLOT31,
+		AB8500_CODEC_SLOT_UNDEFINED
+	} t_ab8500_codec_slot;
+
+	typedef enum {
+		AB8500_CODEC_DA_CHANNEL_NUMBER_1,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_2,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_3,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_4,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_5,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_6,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_UNDEFINED
+	} t_ab8500_codec_da_channel_number;
+
+	typedef enum {
+		AB8500_CODEC_SRC_STATE_DISABLE,
+		AB8500_CODEC_SRC_STATE_ENABLE
+	} t_ab8500_codec_src_state;
+
+	typedef enum {
+		AB8500_CODEC_DEST_STATE_DISABLE,
+		AB8500_CODEC_DEST_STATE_ENABLE
+	} t_ab8500_codec_dest_state;
+
+	typedef struct {
+		t_ab8500_codec_cr27_if1_bitclk_osr cr27_if1_bitclk_osr;
+		t_ab8500_codec_cr27_if0_bitclk_osr cr27_if0_bitclk_osr;
+		t_ab8500_codec_cr28_if0wl cr28_if0wl;
+		t_ab8500_codec_cr30_if1wl cr30_if1wl;
+		t_ab8500_codec_cr28_bitclk0p cr28_bitclk0p;
+		t_ab8500_codec_cr28_if0del cr28_if0del;
+	} t_ab8500_codec_tdm_config;
+
+	typedef struct {
+		t_ab8500_codec_cr104_bfifoint cr104_bfifoint;
+		t_ab8500_codec_cr105_bfifotx cr105_bfifotx;
+		t_ab8500_codec_cr106_bfifofsext cr106_bfifofsext;
+		t_ab8500_codec_cr106_bfifomsk cr106_bfifomsk;
+		t_ab8500_codec_cr106_bfifomstr cr106_bfifomstr;
+		t_ab8500_codec_cr106_bfifostrt cr106_bfifostrt;
+		t_ab8500_codec_cr107_bfifosampnr cr107_bfifosampnr;
+		t_ab8500_codec_cr108_bfifowakeup cr108_bfifowakeup;
+	} t_ab8500_codec_burst_fifo_config;
+
+/************************************************************/
+/*---------------------------------------------------------------------
+ * Exported APIs
+ *--------------------------------------------------------------------*/
+/* Initialization */
+	t_ab8500_codec_error AB8500_CODEC_Init(IN t_uint8
+					       slave_address_of_codec);
+	t_ab8500_codec_error AB8500_CODEC_Reset(void);
+
+/* Audio Codec basic configuration */
+	t_ab8500_codec_error AB8500_CODEC_SetModeAndDirection(IN
+							      t_ab8500_codec_direction
+							      ab8500_codec_direction,
+							      IN
+							      t_ab8500_codec_mode
+							      ab8500_codec_mode_in,
+							      IN
+							      t_ab8500_codec_mode
+							      ab8500_codec_mode_out,
+							      IN
+							      t_ab8500_codec_tdm_config
+							      const *const
+							      p_tdm_config);
+	t_ab8500_codec_error AB8500_CODEC_SelectInput(IN t_ab8500_codec_src
+						      ab8500_codec_src);
+	t_ab8500_codec_error AB8500_CODEC_SelectOutput(IN t_ab8500_codec_dest
+						       ab8500_codec_dest);
+
+/* Burst FIFO configuration */
+	t_ab8500_codec_error AB8500_CODEC_ConfigureBurstFifo(IN
+							     t_ab8500_codec_burst_fifo_config
+							     const *const
+							     p_burst_fifo_config);
+	t_ab8500_codec_error AB8500_CODEC_EnableBurstFifo(void);
+	t_ab8500_codec_error AB8500_CODEC_DisableBurstFifo(void);
+
+/* Audio Codec Master mode configuration */
+	t_ab8500_codec_error AB8500_CODEC_SetMasterMode(IN
+							t_ab8500_codec_master_mode
+							mode);
+
+/* APIs to be implemented by user */
+	t_ab8500_codec_error AB8500_CODEC_Write(IN t_uint8 register_offset,
+						IN t_uint8 count,
+						IN t_uint8 * p_data);
+	t_ab8500_codec_error AB8500_CODEC_Read(IN t_uint8 register_offset,
+					       IN t_uint8 count,
+					       IN t_uint8 * p_dummy_data,
+					       IN t_uint8 * p_data);
+
+/* Volume Management */
+	t_ab8500_codec_error AB8500_CODEC_SetSrcVolume(IN t_ab8500_codec_src
+						       src_device,
+						       IN t_uint8
+						       in_left_volume,
+						       IN t_uint8
+						       in_right_volume);
+	t_ab8500_codec_error AB8500_CODEC_SetDestVolume(IN t_ab8500_codec_dest
+							dest_device,
+							IN t_uint8
+							out_left_volume,
+							IN t_uint8
+							out_right_volume);
+
+/* Power management */
+	t_ab8500_codec_error AB8500_CODEC_PowerDown(void);
+	t_ab8500_codec_error AB8500_CODEC_PowerUp(void);
+
+/* Interface Management */
+	t_ab8500_codec_error AB8500_CODEC_SelectInterface(IN
+							  t_ab8500_codec_audio_interface
+							  audio_interface);
+	t_ab8500_codec_error AB8500_CODEC_GetInterface(OUT
+						       t_ab8500_codec_audio_interface
+						       * p_audio_interface);
+
+/* Slot Allocation */
+	t_ab8500_codec_error AB8500_CODEC_ADSlotAllocation(IN
+							   t_ab8500_codec_slot
+							   ad_slot,
+							   IN
+							   t_ab8500_codec_cr31_to_cr46_ad_data_allocation
+							   value);
+	t_ab8500_codec_error AB8500_CODEC_DASlotAllocation(IN
+							   t_ab8500_codec_da_channel_number
+							   channel_number,
+							   IN
+							   t_ab8500_codec_cr51_to_cr56_sltoda
+							   slot);
+
+/* Loopback Management */
+	t_ab8500_codec_error AB8500_CODEC_SetAnalogLoopback(IN t_uint8
+							    out_left_volume,
+							    IN t_uint8
+							    out_right_volume);
+	t_ab8500_codec_error AB8500_CODEC_RemoveAnalogLoopback(void);
+
+/* Bypass Management */
+	t_ab8500_codec_error AB8500_CODEC_EnableBypassMode(void);
+	t_ab8500_codec_error AB8500_CODEC_DisableBypassMode(void);
+
+/* Power Control Management */
+	t_ab8500_codec_error AB8500_CODEC_SrcPowerControl(IN t_ab8500_codec_src
+							  src_device,
+							  t_ab8500_codec_src_state
+							  state);
+	t_ab8500_codec_error AB8500_CODEC_DestPowerControl(IN
+							   t_ab8500_codec_dest
+							   dest_device,
+							   t_ab8500_codec_dest_state
+							   state);
+
+/* Version Management */
+	t_ab8500_codec_error AB8500_CODEC_GetVersion(OUT t_version * p_version);
+
+#if 0
+/* Debug management */
+	t_ab8500_codec_error AB8500_CODEC_SetDbgLevel(IN t_dbg_level dbg_level);
+	t_ab8500_codec_error AB8500_CODEC_GetDbgLevel(OUT t_dbg_level *
+						      p_dbg_level);
+#endif
+
+/*
+** following is added by $kardad$
+*/
+
+/* duplicate copy of enum from msp.h */
+/* for MSPConfiguration.in_clock_freq parameter to select msp clock freq */
+	typedef enum {
+		CODEC_MSP_INPUT_FREQ_1MHZ = 1024,
+		CODEC_MSP_INPUT_FREQ_2MHZ = 2048,
+		CODEC_MSP_INPUT_FREQ_3MHZ = 3072,
+		CODEC_MSP_INPUT_FREQ_4MHZ = 4096,
+		CODEC_MSP_INPUT_FREQ_5MHZ = 5760,
+		CODEC_MSP_INPUT_FREQ_6MHZ = 6144,
+		CODEC_MSP_INPUT_FREQ_8MHZ = 8192,
+		CODEC_MSP_INPUT_FREQ_11MHZ = 11264,
+		CODEC_MSP_INPUT_FREQ_12MHZ = 12288,
+		CODEC_MSP_INPUT_FREQ_16MHZ = 16384,
+		CODEC_MSP_INPUT_FREQ_22MHZ = 22579,
+		CODEC_MSP_INPUT_FREQ_24MHZ = 24576,
+		CODEC_MSP_INPUT_FREQ_48MHZ = 49152
+	} codec_msp_in_clock_freq_type;
+
+/* msp clock source internal/external for srg_clock_sel */
+	typedef enum {
+		CODEC_MSP_APB_CLOCK = 0,
+		CODEC_MSP_SCK_CLOCK = 2,
+		CODEC_MSP_SCK_SYNC_CLOCK = 3
+	} codec_msp_srg_clock_sel_type;
+
+/* Sample rate supported by Codec */
+
+	typedef enum {
+		CODEC_FREQUENCY_DONT_CHANGE = -100,
+		CODEC_SAMPLING_FREQ_RESET = -1,
+		CODEC_SAMPLING_FREQ_MINLIMIT = 7,
+		CODEC_SAMPLING_FREQ_8KHZ = 8,	/*default */
+		CODEC_SAMPLING_FREQ_11KHZ = 11,
+		CODEC_SAMPLING_FREQ_12KHZ = 12,
+		CODEC_SAMPLING_FREQ_16KHZ = 16,
+		CODEC_SAMPLING_FREQ_22KHZ = 22,
+		CODEC_SAMPLING_FREQ_24KHZ = 24,
+		CODEC_SAMPLING_FREQ_32KHZ = 32,
+		CODEC_SAMPLING_FREQ_44KHZ = 44,
+		CODEC_SAMPLING_FREQ_48KHZ = 48,
+		CODEC_SAMPLING_FREQ_64KHZ = 64,	/*the frequencies below this line are not supported in stw5094A */
+		CODEC_SAMPLING_FREQ_88KHZ = 88,
+		CODEC_SAMPLING_FREQ_96KHZ = 96,
+		CODEC_SAMPLING_FREQ_128KHZ = 128,
+		CODEC_SAMPLING_FREQ_176KHZ = 176,
+		CODEC_SAMPLING_FREQ_192KHZ = 192,
+		CODEC_SAMPLING_FREQ_MAXLIMIT = 193
+	} t_codec_sample_frequency;
+
+#define RESET       -1
+#define DEFAULT     -100
+/***********************************************************/
+/*
+** following stuff is added to compile code without debug print support $kardad$
+*/
+
+#define DBGEXIT(cr)
+#define DBGEXIT0(cr)
+#define DBGEXIT1(cr,ch,p1)
+#define DBGEXIT2(cr,ch,p1,p2)
+#define DBGEXIT3(cr,ch,p1,p2,p3)
+#define DBGEXIT4(cr,ch,p1,p2,p3,p4)
+#define DBGEXIT5(cr,ch,p1,p2,p3,p4,p5)
+#define DBGEXIT6(cr,ch,p1,p2,p3,p4,p5,p6)
+
+#define DBGENTER()
+#define DBGENTER0()
+#define DBGENTER1(ch,p1)
+#define DBGENTER2(ch,p1,p2)
+#define DBGENTER3(ch,p1,p2,p3)
+#define DBGENTER4(ch,p1,p2,p3,p4)
+#define DBGENTER5(ch,p1,p2,p3,p4,p5)
+#define DBGENTER6(ch,p1,p2,p3,p4,p5,p6)
+
+#define DBGPRINT(dbg_level,dbg_string)
+#define DBGPRINTHEX(dbg_level,dbg_string,uint32)
+#define DBGPRINTDEC(dbg_level,dbg_string,uint32)
+/***********************************************************/
+
+#ifdef __cplusplus
+}				/* allow C++ to use these headers */
+#endif				/* __cplusplus */
+#endif				/* _AB8500_CODEC_H_ */
+/* End of file ab8500_codec.h*/
diff --git a/arch/arm/mach-ux500/include/mach/ab8500_codec_p.h b/arch/arm/mach-ux500/include/mach/ab8500_codec_p.h
new file mode 100644
index 00000000000..847a1729e44
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/ab8500_codec_p.h
@@ -0,0 +1,3082 @@
+/*****************************************************************************/
+/**
+*  © ST-Ericsson, 2009 - All rights reserved
+* Reproduction and Communication of this document is strictly prohibited
+* unless specifically authorized in writing by ST-Ericsson
+ *
+* \brief   Private Header file for AB8500 CODEC
+* \author  ST-Ericsson
+ */
+/*****************************************************************************/
+
+#ifndef _AB8500_CODECP_H_
+#define _AB8500_CODECP_H_
+
+/*----------------------------------------------------------------------------
+ * Includes
+ *---------------------------------------------------------------------------*/
+#include "hcl_defs.h"
+
+#define AB8500_CODEC_HCL_VERSION_ID                    2
+#define AB8500_CODEC_HCL_MAJOR_ID                      0
+#define AB8500_CODEC_HCL_MINOR_ID                      0
+
+#define AB8500_CODEC_MASK_ONE_BIT                      0x1UL
+#define AB8500_CODEC_MASK_TWO_BITS                     0x3UL
+#define AB8500_CODEC_MASK_THREE_BITS                   0x7UL
+#define AB8500_CODEC_MASK_FOUR_BITS                    0xFUL
+#define AB8500_CODEC_MASK_FIVE_BITS                    0x1FUL
+#define AB8500_CODEC_MASK_SIX_BITS                     0x3FUL
+#define AB8500_CODEC_MASK_SEVEN_BITS                   0x7FUL
+#define AB8500_CODEC_MASK_EIGHT_BITS                   0xFFUL
+
+#define AB8500_CODEC_WRITE_BITS(reg, val, bit_nb, pos) (reg) = ((t_uint8) ((((reg) & (~(bit_nb << pos))) | (((val) & bit_nb) << pos))))
+
+#define AB8500_CODEC_BLOCK 0x0D
+
+#define AB8500_CODEC_MASK_TWO_MS_BITS                     0xC0UL
+#define AB8500_CODEC_MASK_SIX_LS_BITS                     0x3FUL
+
+/*  Genepi AudioCodec Control Registers */
+
+#define AB8500_CODEC_CR0    0x00
+#define AB8500_CODEC_CR1    0x01
+#define AB8500_CODEC_CR2    0x02
+#define AB8500_CODEC_CR3    0x03
+#define AB8500_CODEC_CR4    0x04
+#define AB8500_CODEC_CR5    0x05
+#define AB8500_CODEC_CR6    0x06
+#define AB8500_CODEC_CR7    0x07
+#define AB8500_CODEC_CR8    0x08
+#define AB8500_CODEC_CR9    0x09
+#define AB8500_CODEC_CR10   0x0A
+#define AB8500_CODEC_CR11   0x0B
+#define AB8500_CODEC_CR12   0x0C
+#define AB8500_CODEC_CR13   0x0D
+#define AB8500_CODEC_CR14   0x0E
+#define AB8500_CODEC_CR15   0x0F
+#define AB8500_CODEC_CR16   0x10
+#define AB8500_CODEC_CR17   0x11
+#define AB8500_CODEC_CR18   0x12
+#define AB8500_CODEC_CR19   0x13
+#define AB8500_CODEC_CR20   0x14
+#define AB8500_CODEC_CR21   0x15
+#define AB8500_CODEC_CR22   0x16
+#define AB8500_CODEC_CR23   0x17
+#define AB8500_CODEC_CR24   0x18
+#define AB8500_CODEC_CR25   0x19
+#define AB8500_CODEC_CR26   0x1A
+#define AB8500_CODEC_CR27   0x1B
+#define AB8500_CODEC_CR28   0x1C
+#define AB8500_CODEC_CR29   0x1D
+#define AB8500_CODEC_CR30   0x1E
+#define AB8500_CODEC_CR31   0x1F
+#define AB8500_CODEC_CR32   0x20
+#define AB8500_CODEC_CR33   0x21
+#define AB8500_CODEC_CR34   0x22
+#define AB8500_CODEC_CR35   0x23
+#define AB8500_CODEC_CR36   0x24
+#define AB8500_CODEC_CR37   0x25
+#define AB8500_CODEC_CR38   0x26
+#define AB8500_CODEC_CR39   0x27
+#define AB8500_CODEC_CR40   0x28
+#define AB8500_CODEC_CR41   0x29
+#define AB8500_CODEC_CR42   0x2A
+#define AB8500_CODEC_CR43   0x2B
+#define AB8500_CODEC_CR44   0x2C
+#define AB8500_CODEC_CR45   0x2D
+#define AB8500_CODEC_CR46   0x2E
+#define AB8500_CODEC_CR47   0x2F
+#define AB8500_CODEC_CR48   0x30
+#define AB8500_CODEC_CR49   0x31
+#define AB8500_CODEC_CR50   0x32
+#define AB8500_CODEC_CR51   0x33
+#define AB8500_CODEC_CR52   0x34
+#define AB8500_CODEC_CR53   0x35
+#define AB8500_CODEC_CR54   0x36
+#define AB8500_CODEC_CR55   0x37
+#define AB8500_CODEC_CR56   0x38
+#define AB8500_CODEC_CR57   0x39
+#define AB8500_CODEC_CR58   0x3A
+#define AB8500_CODEC_CR59   0x3B
+#define AB8500_CODEC_CR60   0x3C
+#define AB8500_CODEC_CR61   0x3D
+#define AB8500_CODEC_CR62   0x3E
+#define AB8500_CODEC_CR63   0x3F
+#define AB8500_CODEC_CR64   0x40
+#define AB8500_CODEC_CR65   0x41
+#define AB8500_CODEC_CR66   0x42
+#define AB8500_CODEC_CR67   0x43
+#define AB8500_CODEC_CR68   0x44
+#define AB8500_CODEC_CR69   0x45
+#define AB8500_CODEC_CR70   0x46
+#define AB8500_CODEC_CR71   0x47
+#define AB8500_CODEC_CR72   0x48
+#define AB8500_CODEC_CR73   0x49
+#define AB8500_CODEC_CR74   0x4A
+#define AB8500_CODEC_CR75   0x4B
+#define AB8500_CODEC_CR76   0x4C
+#define AB8500_CODEC_CR77   0x4D
+#define AB8500_CODEC_CR78   0x4E
+#define AB8500_CODEC_CR79   0x4F
+#define AB8500_CODEC_CR80   0x50
+#define AB8500_CODEC_CR81   0x51
+#define AB8500_CODEC_CR82   0x52
+#define AB8500_CODEC_CR83   0x53
+#define AB8500_CODEC_CR84   0x54
+#define AB8500_CODEC_CR85   0x55
+#define AB8500_CODEC_CR86   0x56
+#define AB8500_CODEC_CR87   0x57
+#define AB8500_CODEC_CR88   0x58
+#define AB8500_CODEC_CR89   0x59
+#define AB8500_CODEC_CR90   0x5A
+#define AB8500_CODEC_CR91   0x5B
+#define AB8500_CODEC_CR92   0x5C
+#define AB8500_CODEC_CR93   0x5D
+#define AB8500_CODEC_CR94   0x5E
+#define AB8500_CODEC_CR95   0x5F
+#define AB8500_CODEC_CR96   0x60
+#define AB8500_CODEC_CR97   0x61
+#define AB8500_CODEC_CR98   0x62
+#define AB8500_CODEC_CR99   0x63
+#define AB8500_CODEC_CR100  0x64
+#define AB8500_CODEC_CR101  0x65
+#define AB8500_CODEC_CR102  0x66
+#define AB8500_CODEC_CR103  0x67
+#define AB8500_CODEC_CR104  0x68
+#define AB8500_CODEC_CR105  0x69
+#define AB8500_CODEC_CR106  0x6A
+#define AB8500_CODEC_CR107  0x6B
+#define AB8500_CODEC_CR108  0x6C
+#define AB8500_CODEC_CR109  0x6D
+
+/* CR0-CR0x0000 */
+#define AB8500_CODEC_CR0_POWERUP   7
+#define AB8500_CODEC_CR0_ENAANA    3
+
+/* CR1-CR0x0001 */
+#define AB8500_CODEC_CR1_SWRESET   7
+
+/* CR2-CR0x0002 */
+#define AB8500_CODEC_CR2_ENAD1   7
+#define AB8500_CODEC_CR2_ENAD2   6
+#define AB8500_CODEC_CR2_ENAD3   5
+#define AB8500_CODEC_CR2_ENAD4   4
+#define AB8500_CODEC_CR2_ENAD5   3
+#define AB8500_CODEC_CR2_ENAD6   2
+
+/* CR3-CR0x0003 */
+#define AB8500_CODEC_CR3_ENDA1   7
+#define AB8500_CODEC_CR3_ENDA2   6
+#define AB8500_CODEC_CR3_ENDA3   5
+#define AB8500_CODEC_CR3_ENDA4   4
+#define AB8500_CODEC_CR3_ENDA5   3
+#define AB8500_CODEC_CR3_ENDA6   2
+
+/* CR4-CR0x0004 */
+#define AB8500_CODEC_CR4_LOWPOWHS      7
+#define AB8500_CODEC_CR4_LOWPOWDACHS   5
+#define AB8500_CODEC_CR4_LOWPOWEAR     4
+#define AB8500_CODEC_CR4_EAR_SEL_CM    2
+#define AB8500_CODEC_CR4_HS_HP_DIS     1
+#define AB8500_CODEC_CR4_EAR_HP_DIS    0
+
+/* CR5-CR0x0005 */
+#define AB8500_CODEC_CR5_ENMIC1     7
+#define AB8500_CODEC_CR5_ENMIC2     6
+#define AB8500_CODEC_CR5_ENLINL     5
+#define AB8500_CODEC_CR5_ENLINR     4
+#define AB8500_CODEC_CR5_MUTMIC1    3
+#define AB8500_CODEC_CR5_MUTMIC2    2
+#define AB8500_CODEC_CR5_MUTELINL   1
+#define AB8500_CODEC_CR5_MUTELINR   0
+
+/* CR6-CR0x0006 */
+#define AB8500_CODEC_CR6_ENDMIC1      7
+#define AB8500_CODEC_CR6_ENDMIC2      6
+#define AB8500_CODEC_CR6_ENDMIC3      5
+#define AB8500_CODEC_CR6_ENDMIC4      4
+#define AB8500_CODEC_CR6_ENDMIC5      3
+#define AB8500_CODEC_CR6_ENDMIC6      2
+
+/* CR7-CR0x0007 */
+#define AB8500_CODEC_CR7_MIC1SEL     7
+#define AB8500_CODEC_CR7_LINRSEL     6
+#define AB8500_CODEC_CR7_ENDRVHSL    5
+#define AB8500_CODEC_CR7_ENDRVHSR    4
+#define AB8500_CODEC_CR7_ENADCMIC    2
+#define AB8500_CODEC_CR7_ENADCLINL   1
+#define AB8500_CODEC_CR7_ENADCLINR   0
+
+/* CR8-CR0x0008 */
+#define AB8500_CODEC_CR8_CP_DIS_PLDWN   7
+#define AB8500_CODEC_CR8_ENEAR          6
+#define AB8500_CODEC_CR8_ENHSL          5
+#define AB8500_CODEC_CR8_ENHSR          4
+#define AB8500_CODEC_CR8_ENHFL          3
+#define AB8500_CODEC_CR8_ENHFR          2
+#define AB8500_CODEC_CR8_ENVIBL         1
+#define AB8500_CODEC_CR8_ENVIBR         0
+
+/* CR9-CR0x0009 */
+#define AB8500_CODEC_CR9_ENADACEAR     6
+#define AB8500_CODEC_CR9_ENADACHSL     5
+#define AB8500_CODEC_CR9_ENADACHSR     4
+#define AB8500_CODEC_CR9_ENADACHFL     3
+#define AB8500_CODEC_CR9_ENADACHFR     2
+#define AB8500_CODEC_CR9_ENADACVIBL    1
+#define AB8500_CODEC_CR9_ENADACVIBR    0
+
+/* CR10-CR0x000A */
+#define AB8500_CODEC_CR10_MUTEEAR     6
+#define AB8500_CODEC_CR10_MUTEHSL     5
+#define AB8500_CODEC_CR10_MUTEHSR     4
+#define AB8500_CODEC_CR10_MUTEHFL     3
+#define AB8500_CODEC_CR10_MUTEHFR     2
+#define AB8500_CODEC_CR10_MUTEVIBL    1
+#define AB8500_CODEC_CR10_MUTEVIBR    0
+
+/* CR11-CR0x000B */
+#define AB8500_CODEC_CR11_ENSHORTPWD      7
+#define AB8500_CODEC_CR11_EARSHORTDIS     6
+#define AB8500_CODEC_CR11_HSLSHORTDIS     5
+#define AB8500_CODEC_CR11_HSRSHORTDIS     4
+#define AB8500_CODEC_CR11_HFLSHORTDIS     3
+#define AB8500_CODEC_CR11_HFRSHORTDIS     2
+#define AB8500_CODEC_CR11_VIBLSHORTDIS    1
+#define AB8500_CODEC_CR11_VIBRSHORTDIS    0
+
+/* CR12-CR0x000C */
+#define AB8500_CODEC_CR12_ENCPHS        7
+#define AB8500_CODEC_CR12_HSAUTOTIME    4
+#define AB8500_CODEC_CR12_HSAUTOENSEL   1
+#define AB8500_CODEC_CR12_HSAUTOEN      0
+
+/* CR13-CR0x000D */
+#define AB8500_CODEC_CR13_ENVDET_HTHRESH   4
+#define AB8500_CODEC_CR13_ENVDET_LTHRESH   0
+
+/* CR14-CR0x000E */
+#define AB8500_CODEC_CR14_SMPSLVEN       7
+#define AB8500_CODEC_CR14_ENVDETSMPSEN   6
+#define AB8500_CODEC_CR14_CPLVEN         5
+#define AB8500_CODEC_CR14_ENVDETCPEN     4
+#define AB8500_CODEC_CR14_ENVDET_TIME    0
+
+/* CR15-CR0x000F */
+#define AB8500_CODEC_CR15_PWMTOVIBL   7
+#define AB8500_CODEC_CR15_PWMTOVIBR   6
+#define AB8500_CODEC_CR15_PWMLCTRL    5
+#define AB8500_CODEC_CR15_PWMRCTRL    4
+#define AB8500_CODEC_CR15_PWMNLCTRL   3
+#define AB8500_CODEC_CR15_PWMPLCTRL   2
+#define AB8500_CODEC_CR15_PWMNRCTRL   1
+#define AB8500_CODEC_CR15_PWMPRCTRL   0
+
+/* CR16-CR0x0010 */
+#define AB8500_CODEC_CR16_PWMNLPOL         7
+#define AB8500_CODEC_CR16_PWMNLDUTYCYCLE   0
+
+/* CR17-CR0x0011 */
+#define AB8500_CODEC_CR17_PWMPLPOL         7
+#define AB8500_CODEC_CR17_PWMLPDUTYCYCLE   0
+
+/* CR18-CR0x0012 */
+#define AB8500_CODEC_CR18_PWMNRPOL         7
+#define AB8500_CODEC_CR18_PWMNRDUTYCYCLE   0
+
+/* CR19-CR0x0013 */
+#define AB8500_CODEC_CR19_PWMPRPOL         7
+#define AB8500_CODEC_CR19_PWMRPDUTYCYCLE   0
+
+/* CR20-CR0x0014 */
+#define AB8500_CODEC_CR20_EN_SE_MIC1   7
+#define AB8500_CODEC_CR20_MIC1_GAIN    0
+
+/* CR21-CR0x0015 */
+#define AB8500_CODEC_CR21_EN_SE_MIC2   7
+#define AB8500_CODEC_CR21_MIC2_GAIN    0
+
+/* CR22-CR0x0016 */
+#define AB8500_CODEC_CR22_HSL_GAIN    5
+#define AB8500_CODEC_CR22_LINL_GAIN   0
+
+/* CR23-CR0x0017 */
+#define AB8500_CODEC_CR23_HSR_GAIN    5
+#define AB8500_CODEC_CR23_LINR_GAIN   0
+
+/* CR24-CR0x0018 */
+#define AB8500_CODEC_CR24_LINTOHSL_GAIN   0
+
+/* CR25-CR0x0019 */
+#define AB8500_CODEC_CR25_LINTOHSR_GAIN   0
+
+/* CR26-CR0x001A */
+#define AB8500_CODEC_CR26_AD1NH       7
+#define AB8500_CODEC_CR26_AD2NH       6
+#define AB8500_CODEC_CR26_AD3NH       5
+#define AB8500_CODEC_CR26_AD4NH       4
+#define AB8500_CODEC_CR26_AD1_VOICE   3
+#define AB8500_CODEC_CR26_AD2_VOICE   2
+#define AB8500_CODEC_CR26_AD3_VOICE   1
+#define AB8500_CODEC_CR26_AD4_VOICE   0
+
+/* CR27-CR0x001B */
+#define AB8500_CODEC_CR27_EN_MASTGEN       7
+#define AB8500_CODEC_CR27_IF1_BITCLK_OSR   5
+#define AB8500_CODEC_CR27_ENFS_BITCLK1     4
+#define AB8500_CODEC_CR27_IF0_BITCLK_OSR   1
+#define AB8500_CODEC_CR27_ENFS_BITCLK0     0
+
+/* CR28-CR0x001C */
+#define AB8500_CODEC_CR28_FSYNC0P     6
+#define AB8500_CODEC_CR28_BITCLK0P    5
+#define AB8500_CODEC_CR28_IF0DEL      4
+#define AB8500_CODEC_CR28_IF0FORMAT   2
+#define AB8500_CODEC_CR28_IF0WL       0
+
+/* CR29-CR0x001D */
+#define AB8500_CODEC_CR29_IF0DATOIF1AD   7
+#define AB8500_CODEC_CR29_IF0CKTOIF1CK   6
+#define AB8500_CODEC_CR29_IF1MASTER      5
+#define AB8500_CODEC_CR29_IF1DATOIF0AD   3
+#define AB8500_CODEC_CR29_IF1CKTOIF0CK   2
+#define AB8500_CODEC_CR29_IF0MASTER      1
+#define AB8500_CODEC_CR29_IF0BFIFOEN     0
+
+/* CR30-CR0x001E */
+#define AB8500_CODEC_CR30_FSYNC1P     6
+#define AB8500_CODEC_CR30_BITCLK1P    5
+#define AB8500_CODEC_CR30_IF1DEL      4
+#define AB8500_CODEC_CR30_IF1FORMAT   2
+#define AB8500_CODEC_CR30_IF1WL       0
+
+/* CR31-CR0x001F */
+#define AB8500_CODEC_CR31_ADOTOSLOT1   4
+#define AB8500_CODEC_CR31_ADOTOSLOT0   0
+
+/* CR32-CR0x0020 */
+#define AB8500_CODEC_CR32_ADOTOSLOT3   4
+#define AB8500_CODEC_CR32_ADOTOSLOT2   0
+
+/* CR33-CR0x0021 */
+#define AB8500_CODEC_CR33_ADOTOSLOT5   4
+#define AB8500_CODEC_CR33_ADOTOSLOT4   0
+
+/* CR34-CR0x0022 */
+#define AB8500_CODEC_CR34_ADOTOSLOT7   4
+#define AB8500_CODEC_CR34_ADOTOSLOT6   0
+
+/* CR35-CR0x0023 */
+#define AB8500_CODEC_CR35_ADOTOSLOT9   4
+#define AB8500_CODEC_CR35_ADOTOSLOT8   0
+
+/* CR36-CR0x0024 */
+#define AB8500_CODEC_CR36_ADOTOSLOT11   4
+#define AB8500_CODEC_CR36_ADOTOSLOT10   0
+
+/* CR37-CR0x0025 */
+#define AB8500_CODEC_CR37_ADOTOSLOT13   4
+#define AB8500_CODEC_CR37_ADOTOSLOT12   0
+
+/* CR38-CR0x0026 */
+#define AB8500_CODEC_CR38_ADOTOSLOT15   4
+#define AB8500_CODEC_CR38_ADOTOSLOT14   0
+
+/* CR39-CR0x0027 */
+#define AB8500_CODEC_CR39_ADOTOSLOT17   4
+#define AB8500_CODEC_CR39_ADOTOSLOT16   0
+
+/* CR40-CR0x0028 */
+#define AB8500_CODEC_CR40_ADOTOSLOT19   4
+#define AB8500_CODEC_CR40_ADOTOSLOT18   0
+
+/* CR41-CR0x0029 */
+#define AB8500_CODEC_CR41_ADOTOSLOT21   4
+#define AB8500_CODEC_CR41_ADOTOSLOT20   0
+
+/* CR42-CR0x002A */
+#define AB8500_CODEC_CR42_ADOTOSLOT23   4
+#define AB8500_CODEC_CR42_ADOTOSLOT22   0
+
+/* CR43-CR0x002B */
+#define AB8500_CODEC_CR43_ADOTOSLOT25   4
+#define AB8500_CODEC_CR43_ADOTOSLOT24   0
+
+/* CR44-CR0x002C */
+#define AB8500_CODEC_CR44_ADOTOSLOT27   4
+#define AB8500_CODEC_CR44_ADOTOSLOT26   0
+
+/* CR45-CR0x002D */
+#define AB8500_CODEC_CR45_ADOTOSLOT29   4
+#define AB8500_CODEC_CR45_ADOTOSLOT28   0
+
+/* CR46-CR0x002E */
+#define AB8500_CODEC_CR46_ADOTOSLOT31   4
+#define AB8500_CODEC_CR46_ADOTOSLOT30   0
+
+/* CR47-CR0x002F */
+#define AB8500_CODEC_CR47_HIZ_SL7   7
+#define AB8500_CODEC_CR47_HIZ_SL6   6
+#define AB8500_CODEC_CR47_HIZ_SL5   5
+#define AB8500_CODEC_CR47_HIZ_SL4   4
+#define AB8500_CODEC_CR47_HIZ_SL3   3
+#define AB8500_CODEC_CR47_HIZ_SL2   2
+#define AB8500_CODEC_CR47_HIZ_SL1   1
+#define AB8500_CODEC_CR47_HIZ_SL0   0
+
+/* CR48-CR0x0030 */
+#define AB8500_CODEC_CR48_HIZ_SL15   7
+#define AB8500_CODEC_CR48_HIZ_SL14   6
+#define AB8500_CODEC_CR48_HIZ_SL13   5
+#define AB8500_CODEC_CR48_HIZ_SL12   4
+#define AB8500_CODEC_CR48_HIZ_SL11   3
+#define AB8500_CODEC_CR48_HIZ_SL10   2
+#define AB8500_CODEC_CR48_HIZ_SL9    1
+#define AB8500_CODEC_CR48_HIZ_SL8    0
+
+/* CR49-CR0x0031 */
+#define AB8500_CODEC_CR49_HIZ_SL23   7
+#define AB8500_CODEC_CR49_HIZ_SL22   6
+#define AB8500_CODEC_CR49_HIZ_SL21   5
+#define AB8500_CODEC_CR49_HIZ_SL20   4
+#define AB8500_CODEC_CR49_HIZ_SL19   3
+#define AB8500_CODEC_CR49_HIZ_SL18   2
+#define AB8500_CODEC_CR49_HIZ_SL17   1
+#define AB8500_CODEC_CR49_HIZ_SL16   0
+
+/* CR50-CR0x0032 */
+#define AB8500_CODEC_CR50_HIZ_SL31   7
+#define AB8500_CODEC_CR50_HIZ_SL30   6
+#define AB8500_CODEC_CR50_HIZ_SL29   5
+#define AB8500_CODEC_CR50_HIZ_SL28   4
+#define AB8500_CODEC_CR50_HIZ_SL27   3
+#define AB8500_CODEC_CR50_HIZ_SL26   2
+#define AB8500_CODEC_CR50_HIZ_SL25   1
+#define AB8500_CODEC_CR50_HIZ_SL24   0
+
+/* CR51-CR0x0033 */
+#define AB8500_CODEC_CR51_DA12_VOICE       7
+#define AB8500_CODEC_CR51_SLDAI1TOSLADO1   5
+#define AB8500_CODEC_CR51_SLTODA1          0
+
+/* CR52-CR0x0034 */
+#define AB8500_CODEC_CR52_SLDAI1TOSLADO2   5
+#define AB8500_CODEC_CR52_SLTODA2          0
+
+/* CR53-CR0x0035 */
+#define AB8500_CODEC_CR53_DA34_VOICE       7
+#define AB8500_CODEC_CR53_SLDAI1TOSLADO3   5
+#define AB8500_CODEC_CR53_SLTODA3          0
+
+/* CR54-CR0x0036 */
+#define AB8500_CODEC_CR54_SLDAI1TOSLADO4   5
+#define AB8500_CODEC_CR54_SLTODA4          0
+
+/* CR55-CR0x0037 */
+#define AB8500_CODEC_CR55_DA56_VOICE       7
+#define AB8500_CODEC_CR55_SLDAI1TOSLADO5   5
+#define AB8500_CODEC_CR55_SLTODA5          0
+
+/* CR56-CR0x0038 */
+#define AB8500_CODEC_CR56_SLDAI1TOSLADO6   5
+#define AB8500_CODEC_CR56_SLTODA6          0
+
+/* CR57-CR0x0039 */
+#define AB8500_CODEC_CR57_BFIFULL_MSK   6
+#define AB8500_CODEC_CR57_BFIEMPT_MSK   5
+#define AB8500_CODEC_CR57_DACHAN_MSK    4
+#define AB8500_CODEC_CR57_GAIN_MSK      3
+#define AB8500_CODEC_CR57_DSPAD_MSK     2
+#define AB8500_CODEC_CR57_DSPDA_MSK     1
+#define AB8500_CODEC_CR57_STFIR_MSK     0
+
+/* CR58-CR0x003A */
+#define AB8500_CODEC_CR58_BFIFULL_EV   6
+#define AB8500_CODEC_CR58_BFIEMPT_EV   5
+#define AB8500_CODEC_CR58_DACHAN_EV    4
+#define AB8500_CODEC_CR58_GAIN_EV      3
+#define AB8500_CODEC_CR58_DSPAD_EV     2
+#define AB8500_CODEC_CR58_DSPDA_EV     1
+#define AB8500_CODEC_CR58_STFIR_EV     0
+
+/* CR59-CR0x003B */
+#define AB8500_CODEC_CR59_VSSREADY_MSK   7
+#define AB8500_CODEC_CR59_SHRTVIBL_MSK   6
+#define AB8500_CODEC_CR59_SHRTVIBR_MSK   5
+#define AB8500_CODEC_CR59_SHRTHFL_MSK    4
+#define AB8500_CODEC_CR59_SHRTHFR_MSK    3
+#define AB8500_CODEC_CR59_SHRTHSL_MSK    2
+#define AB8500_CODEC_CR59_SHRTHSR_MSK    1
+#define AB8500_CODEC_CR59_SHRTEAR_MSK    0
+
+/* CR60-CR0x003C */
+#define AB8500_CODEC_CR60_VSSREADY_EV   7
+#define AB8500_CODEC_CR60_SHRTVIBL_EV   6
+#define AB8500_CODEC_CR60_SHRTVIBR_EV   5
+#define AB8500_CODEC_CR60_SHRTHFL_EV    4
+#define AB8500_CODEC_CR60_SHRTHFR_EV    3
+#define AB8500_CODEC_CR60_SHRTHSL_EV    2
+#define AB8500_CODEC_CR60_SHRTHSR_EV    1
+#define AB8500_CODEC_CR60_SHRTEAR_EV    0
+
+/* CR61-CR0x003D */
+#define AB8500_CODEC_CR61_REVISION     2
+#define AB8500_CODEC_CR61_FADE_SPEED   0
+
+/* CR62-CR0x003E */
+#define AB8500_CODEC_CR62_DMIC1SINC3   5
+#define AB8500_CODEC_CR62_DMIC2SINC3   4
+#define AB8500_CODEC_CR62_DMIC3SINC3   3
+#define AB8500_CODEC_CR62_DMIC4SINC3   2
+#define AB8500_CODEC_CR62_DMIC5SINC3   1
+#define AB8500_CODEC_CR62_DMIC6SINC3   0
+
+/* CR63-CR0x003F */
+#define AB8500_CODEC_CR63_DATOHSLEN   7
+#define AB8500_CODEC_CR63_DATOHSREN   6
+#define AB8500_CODEC_CR63_AD1SEL      5
+#define AB8500_CODEC_CR63_AD2SEL      4
+#define AB8500_CODEC_CR63_AD3SEL      3
+#define AB8500_CODEC_CR63_AD5SEL      2
+#define AB8500_CODEC_CR63_AD6SEL      1
+#define AB8500_CODEC_CR63_ANCSEL      0
+
+/* CR64-CR0x0040 */
+#define AB8500_CODEC_CR64_DATOHFREN   7
+#define AB8500_CODEC_CR64_DATOHFLEN   6
+#define AB8500_CODEC_CR64_HFRSEL      5
+#define AB8500_CODEC_CR64_HFLSEL      4
+#define AB8500_CODEC_CR64_STFIR1SEL   2
+#define AB8500_CODEC_CR64_STFIR2SEL   0
+
+/* CR65-CR0x0041 */
+#define AB8500_CODEC_CR65_FADEDIS_AD1   6
+#define AB8500_CODEC_CR65_AD1GAIN       0
+
+/* CR66-CR0x0042 */
+#define AB8500_CODEC_CR66_FADEDIS_AD2   6
+#define AB8500_CODEC_CR66_AD2GAIN       0
+
+/* CR67-CR0x0043 */
+#define AB8500_CODEC_CR67_FADEDIS_AD3   6
+#define AB8500_CODEC_CR67_AD3GAIN       0
+
+/* CR68-CR0x0044 */
+#define AB8500_CODEC_CR68_FADEDIS_AD4   6
+#define AB8500_CODEC_CR68_AD4GAIN       0
+
+/* CR69-CR0x0045 */
+#define AB8500_CODEC_CR69_FADEDIS_AD5   6
+#define AB8500_CODEC_CR69_AD5GAIN       0
+
+/* CR70-CR0x0046 */
+#define AB8500_CODEC_CR70_FADEDIS_AD6   6
+#define AB8500_CODEC_CR70_AD6GAIN       0
+
+/* CR71-CR0x0047 */
+#define AB8500_CODEC_CR71_FADEDIS_DA1   6
+#define AB8500_CODEC_CR71_DA1GAIN       0
+
+/* CR72-CR0x0048 */
+#define AB8500_CODEC_CR72_FADEDIS_DA2   6
+#define AB8500_CODEC_CR72_DA2GAIN       0
+
+/* CR73-CR0x0049 */
+#define AB8500_CODEC_CR73_FADEDIS_DA3   6
+#define AB8500_CODEC_CR73_DA3GAIN       0
+
+/* CR74-CR0x004A */
+#define AB8500_CODEC_CR74_FADEDIS_DA4   6
+#define AB8500_CODEC_CR74_DA4GAIN       0
+
+/* CR75-CR0x004B */
+#define AB8500_CODEC_CR75_FADEDIS_DA5   6
+#define AB8500_CODEC_CR75_DA5GAIN       0
+
+/* CR76-CR0x004C */
+#define AB8500_CODEC_CR76_FADEDIS_DA6   6
+#define AB8500_CODEC_CR76_DA6GAIN       0
+
+/* CR77-CR0x004D */
+#define AB8500_CODEC_CR77_FADEDIS_AD1L   6
+#define AB8500_CODEC_CR77_AD1LBGAIN      0
+
+/* CR78-CR0x004E */
+#define AB8500_CODEC_CR78_FADEDIS_AD2L   6
+#define AB8500_CODEC_CR78_AD2LBGAIN      0
+
+/* CR79-CR0x004F */
+#define AB8500_CODEC_CR79_HSSINC1       7
+#define AB8500_CODEC_CR79_FADEDIS_HSL   4
+#define AB8500_CODEC_CR79_HSLDGAIN      0
+
+/* CR80-CR0x0050 */
+#define AB8500_CODEC_CR80_FADEDIS_HSR   4
+#define AB8500_CODEC_CR80_HSRDGAIN      0
+
+/* CR81-CR0x0051 */
+#define AB8500_CODEC_CR81_STFIR1GAIN   0
+
+/* CR82-CR0x0052 */
+#define AB8500_CODEC_CR82_STFIR2GAIN   0
+
+/* CR83-CR0x0053 */
+#define AB8500_CODEC_CR83_ENANC          2
+#define AB8500_CODEC_CR83_ANCIIRINIT     1
+#define AB8500_CODEC_CR83_ANCFIRUPDATE   0
+
+/* CR84-CR0x0054 */
+#define AB8500_CODEC_CR84_ANCINSHIFT   0
+
+/* CR85-CR0x0055 */
+#define AB8500_CODEC_CR85_ANCFIROUTSHIFT   0
+
+/* CR86-CR0x0056 */
+#define AB8500_CODEC_CR86_ANCSHIFTOUT   0
+
+/* CR87-CR0x0057 */
+#define AB8500_CODEC_CR87_ANCFIRCOEFF_MSB   0
+
+/* CR88-CR0x0058 */
+#define AB8500_CODEC_CR88_ANCFIRCOEFF_LSB   0
+
+/* CR89-CR0x0059 */
+#define AB8500_CODEC_CR89_ANCIIRCOEFF_MSB   0
+
+/* CR90-CR0x005A */
+#define AB8500_CODEC_CR90_ANCIIRCOEFF_LSB   0
+
+/* CR91-CR0x005B */
+#define AB8500_CODEC_CR91_ANCWARPDEL_MSB   0
+
+/* CR92-CR0x005C */
+#define AB8500_CODEC_CR92_ANCWARPDEL_LSB   0
+
+/* CR93-CR0x005D */
+#define AB8500_CODEC_CR93_ANCFIRPEAK_MSB   0
+
+/* CR94-CR0x005E */
+#define AB8500_CODEC_CR94_ANCFIRPEAK_LSB   0
+
+/* CR95-CR0x005F */
+#define AB8500_CODEC_CR95_ANCIIRPEAK_MSB   0
+
+/* CR96-CR0x0060 */
+#define AB8500_CODEC_CR96_ANCIIRPEAK_LSB   0
+
+/* CR97-CR0x0061 */
+#define AB8500_CODEC_CR97_STFIR_SET    7
+#define AB8500_CODEC_CR97_STFIR_ADDR   0
+
+/* CR98-CR0x0062 */
+#define AB8500_CODEC_CR98_STFIR_COEFF_MSB   0
+
+/* CR99-CR0x0063 */
+#define AB8500_CODEC_CR99_STFIR_COEFF_LSB   0
+
+/* CR100-CR0x0064 */
+#define AB8500_CODEC_CR100_ENSTFIRS      2
+#define AB8500_CODEC_CR100_STFIRSTOIF1   1
+#define AB8500_CODEC_CR100_STFIR_BUSY    0
+
+/* CR101-CR0x0065 */
+#define AB8500_CODEC_CR101_PARLHF             7
+#define AB8500_CODEC_CR101_PARLVIB            6
+#define AB8500_CODEC_CR101_CLASSDVIBLSWAPEN   3
+#define AB8500_CODEC_CR101_CLASSDVIBRSWAPEN   2
+#define AB8500_CODEC_CR101_CLASSDHFLSWAPEN    1
+#define AB8500_CODEC_CR101_CLASSDHFRSWAPEN    0
+
+/* CR102-CR0x0066 */
+#define AB8500_CODEC_CR102_CLASSD_FIRBYP      4
+#define AB8500_CODEC_CR102_CLASSD_HIGHVOLEN   0
+
+/* CR103-CR0x0067 */
+#define AB8500_CODEC_CR103_CLASSD_DITHERHPGAIN   4
+#define AB8500_CODEC_CR103_CLASSD_DITHERWGAIN    0
+
+/* CR104-CR0x0068 */
+#define AB8500_CODEC_CR104_BFIFOINT   0
+
+/* CR105-CR0x0069 */
+#define AB8500_CODEC_CR105_BFIFOTX   0
+
+/* CR106-CR0x006A */
+#define AB8500_CODEC_CR106_BFIFOFSEXT   4
+#define AB8500_CODEC_CR106_BFIFOMSK     2
+#define AB8500_CODEC_CR106_BFIFOMSTR    1
+#define AB8500_CODEC_CR106_BFIFOSTRT    0
+
+/* CR107-CR0x006B */
+#define AB8500_CODEC_CR107_BFIFOSAMPNR   0
+
+/* CR108-CR0x006C */
+#define AB8500_CODEC_CR108_BFIFOWAKEUP   0
+
+/* CR109-CR0x006D */
+#define AB8500_CODEC_CR109_BFIFOSAMPLES   0
+
+/* For SetVolume API*/
+#define AB8500_CODEC_MAX_VOLUME      100
+
+/* Analog MIC1 & MIC2 */
+#define AB8500_CODEC_MIC_VOLUME_MAX      31
+#define AB8500_CODEC_MIC_VOLUME_MEDIUM   15
+#define AB8500_CODEC_MIC_VOLUME_MIN      0
+
+/* Line-in */
+#define AB8500_CODEC_LINEIN_VOLUME_MAX     31
+#define AB8500_CODEC_LINEIN_VOLUME_MEDIUM  15
+#define AB8500_CODEC_LINEIN_VOLUME_MIN     0
+
+/* HeadSet */
+#define AB8500_CODEC_HEADSET_VOLUME_MAX     0
+#define AB8500_CODEC_HEADSET_VOLUME_MEDIUM  3
+#define AB8500_CODEC_HEADSET_VOLUME_MIN     7
+
+/* HeadSet Digital */
+#define AB8500_CODEC_HEADSET_D_VOLUME_MAX     0
+#define AB8500_CODEC_HEADSET_D_VOLUME_MEDIUM  7
+#define AB8500_CODEC_HEADSET_D_VOLUME_MIN     15
+#define AB8500_CODEC_HEADSET_D_VOLUME_0DB     8
+
+/* Digital AD Path */
+#define AB8500_CODEC_AD_D_VOLUME_MAX     0
+#define AB8500_CODEC_AD_D_VOLUME_MEDIUM  31
+#define AB8500_CODEC_AD_D_VOLUME_MIN     63
+
+/* Digital DA Path */
+#define AB8500_CODEC_DA_D_VOLUME_MAX     0
+#define AB8500_CODEC_DA_D_VOLUME_MEDIUM  31
+#define AB8500_CODEC_DA_D_VOLUME_MIN     63
+
+/* EarPiece Digital */
+#define AB8500_CODEC_EARPIECE_D_VOLUME_MAX     0
+#define AB8500_CODEC_EARPIECE_D_VOLUME_MEDIUM  7
+#define AB8500_CODEC_EARPIECE_D_VOLUME_MIN     15
+
+/* AD1 loopback to HFL & HFR Digital */
+#define AB8500_CODEC_AD_LB_TO_HF_L_R_VOLUME_MAX     0
+#define AB8500_CODEC_AD_LB_TO_HF_L_R_VOLUME_MEDIUM  31
+#define AB8500_CODEC_AD_LB_TO_HF_L_R_VOLUME_MIN     63
+
+/* Line-in to HSL & HSR */
+#define AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MAX     0
+#define AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MEDIUM  9
+#define AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MIN     18
+#define AB8500_CODEC_LINEIN_TO_HS_L_R_LOOP_OPEN      19
+
+/* Vibrator */
+#define AB8500_CODEC_VIBRATOR_VOLUME_MAX     100
+#define AB8500_CODEC_VIBRATOR_VOLUME_MEDIUM  50
+#define AB8500_CODEC_VIBRATOR_VOLUME_MIN     0
+
+/* CR0 - 7 */
+typedef enum {
+	AB8500_CODEC_CR0_POWERUP_OFF,
+	AB8500_CODEC_CR0_POWERUP_ON
+} t_ab8500_codec_cr0_powerup;
+
+/* CR0 - 3 */
+typedef enum {
+	AB8500_CODEC_CR0_ENAANA_OFF,
+	AB8500_CODEC_CR0_ENAANA_ON
+} t_ab8500_codec_cr0_enaana;
+
+/* CR1 - 7 */
+typedef enum {
+	AB8500_CODEC_CR1_SWRESET_DISABLED,
+	AB8500_CODEC_CR1_SWRESET_ENABLED
+} t_ab8500_codec_cr1_swreset;
+
+/* CR2 - 7 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD1_DISABLED,
+	AB8500_CODEC_CR2_ENAD1_ENABLED
+} t_ab8500_codec_cr2_enad1;
+
+/* CR2 - 6 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD2_DISABLED,
+	AB8500_CODEC_CR2_ENAD2_ENABLED
+} t_ab8500_codec_cr2_enad2;
+
+/* CR2 - 5 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD3_DISABLED,
+	AB8500_CODEC_CR2_ENAD3_ENABLED
+} t_ab8500_codec_cr2_enad3;
+
+/* CR2 - 4 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD4_DISABLED,
+	AB8500_CODEC_CR2_ENAD4_ENABLED
+} t_ab8500_codec_cr2_enad4;
+
+/* CR2 - 3 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD5_DISABLED,
+	AB8500_CODEC_CR2_ENAD5_ENABLED
+} t_ab8500_codec_cr2_enad5;
+
+/* CR2 - 2 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD6_DISABLED,
+	AB8500_CODEC_CR2_ENAD6_ENABLED
+} t_ab8500_codec_cr2_enad6;
+
+/* CR3 - 7 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA1_DISABLED,
+	AB8500_CODEC_CR3_ENDA1_ENABLED
+} t_ab8500_codec_cr3_enda1;
+
+/* CR3 - 6 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA2_DISABLED,
+	AB8500_CODEC_CR3_ENDA2_ENABLED
+} t_ab8500_codec_cr3_enda2;
+
+/* CR3 - 5 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA3_DISABLED,
+	AB8500_CODEC_CR3_ENDA3_ENABLED
+} t_ab8500_codec_cr3_enda3;
+
+/* CR3 - 4 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA4_DISABLED,
+	AB8500_CODEC_CR3_ENDA4_ENABLED
+} t_ab8500_codec_cr3_enda4;
+
+/* CR3 - 3 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA5_DISABLED,
+	AB8500_CODEC_CR3_ENDA5_ENABLED
+} t_ab8500_codec_cr3_enda5;
+
+/* CR3 - 2 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA6_DISABLED,
+	AB8500_CODEC_CR3_ENDA6_ENABLED
+} t_ab8500_codec_cr3_enda6;
+
+/* CR4 - 7 */
+typedef enum {
+	AB8500_CODEC_CR4_LOWPOWHS_NORMAL,
+	AB8500_CODEC_CR4_LOWPOWHS_LP
+} t_ab8500_codec_cr4_lowpowhs;
+
+/* CR4 - 6:5 */
+typedef enum {
+	AB8500_CODEC_CR4_LOWPOWDACHS_NORMAL,
+	AB8500_CODEC_CR4_LOWPOWDACHS_DRIVERS_LP,
+	AB8500_CODEC_CR4_LOWPOWDACHS_LP,
+	AB8500_CODEC_CR4_LOWPOWDACHS_BOTH_LP
+} t_ab8500_codec_cr4_lowpowdachs;
+
+/* CR4 - 4 */
+typedef enum {
+	AB8500_CODEC_CR4_LOWPOWEAR_NORMAL,
+	AB8500_CODEC_CR4_LOWPOWEAR_LP
+} t_ab8500_codec_cr4_lowpowear;
+
+/* CR4 - 3:2 */
+typedef enum {
+	AB8500_CODEC_CR4_EAR_SEL_CM_0_95V,
+	AB8500_CODEC_CR4_EAR_SEL_CM_1_1V,
+	AB8500_CODEC_CR4_EAR_SEL_CM_1_27V,
+	AB8500_CODEC_CR4_EAR_SEL_CM_1_58V
+} t_ab8500_codec_cr4_ear_sel_cm;
+
+/* CR4 - 1 */
+typedef enum {
+	AB8500_CODEC_CR4_HS_HP_DIS_FILTER_ENABLED,
+	AB8500_CODEC_CR4_HS_HP_DIS_FILTER_DISABLED
+} t_ab8500_codec_cr4_hs_hp_dis;
+
+/* CR4 - 0 */
+typedef enum {
+	AB8500_CODEC_CR4_EAR_HP_DIS_FILTER_ENABLED,
+	AB8500_CODEC_CR4_EAR_HP_DIS_FILTER_DISABLED
+} t_ab8500_codec_cr4_ear_hp_dis;
+
+/* CR5 - 7 */
+typedef enum {
+	AB8500_CODEC_CR5_ENMIC1_DISABLED,
+	AB8500_CODEC_CR5_ENMIC1_ENABLED
+} t_ab8500_codec_cr5_enmic1;
+
+/* CR5 - 6 */
+typedef enum {
+	AB8500_CODEC_CR5_ENMIC2_DISABLED,
+	AB8500_CODEC_CR5_ENMIC2_ENABLED
+} t_ab8500_codec_cr5_enmic2;
+
+/* CR5 - 5 */
+typedef enum {
+	AB8500_CODEC_CR5_ENLINL_DISABLED,
+	AB8500_CODEC_CR5_ENLINL_ENABLED
+} t_ab8500_codec_cr5_enlinl;
+
+/* CR5 - 4 */
+typedef enum {
+	AB8500_CODEC_CR5_ENLINR_DISABLED,
+	AB8500_CODEC_CR5_ENLINR_ENABLED
+} t_ab8500_codec_cr5_enlinr;
+
+/* CR5 - 3 */
+typedef enum {
+	AB8500_CODEC_CR5_MUTMIC1_DISABLED,
+	AB8500_CODEC_CR5_MUTMIC1_ENABLED
+} t_ab8500_codec_cr5_mutmic1;
+
+/* CR5 - 2 */
+typedef enum {
+	AB8500_CODEC_CR5_MUTMIC2_DISABLED,
+	AB8500_CODEC_CR5_MUTMIC2_ENABLED
+} t_ab8500_codec_cr5_mutmic2;
+
+/* CR5 - 1 */
+typedef enum {
+	AB8500_CODEC_CR5_MUTLINL_DISABLED,
+	AB8500_CODEC_CR5_MUTLINL_ENABLED
+} t_ab8500_codec_cr5_mutlinl;
+
+/* CR5 - 0 */
+typedef enum {
+	AB8500_CODEC_CR5_MUTLINR_DISABLED,
+	AB8500_CODEC_CR5_MUTLINR_ENABLED
+} t_ab8500_codec_cr5_mutlinr;
+
+/* CR6 - 7 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC1_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC1_ENABLED
+} t_ab8500_codec_cr6_endmic1;
+
+/* CR6 - 6 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC2_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC2_ENABLED
+} t_ab8500_codec_cr6_endmic2;
+
+/* CR6 - 5 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC3_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC3_ENABLED
+} t_ab8500_codec_cr6_endmic3;
+
+/* CR6 - 4 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC4_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC4_ENABLED
+} t_ab8500_codec_cr6_endmic4;
+
+/* CR6 - 3 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC5_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC5_ENABLED
+} t_ab8500_codec_cr6_endmic5;
+
+/* CR6 - 2 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC6_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC6_ENABLED
+} t_ab8500_codec_cr6_endmic6;
+
+/* CR7 - 7 */
+typedef enum {
+	AB8500_CODEC_CR7_MIC1SEL_MIC1A,
+	AB8500_CODEC_CR7_MIC1SEL_MIC1B
+} t_ab8500_codec_cr7_mic1sel;
+
+/* CR7 - 6 */
+typedef enum {
+	AB8500_CODEC_CR7_LINRSEL_MIC2,
+	AB8500_CODEC_CR7_LINRSEL_LINR
+} t_ab8500_codec_cr7_linrsel;
+
+/* CR7 - 5 */
+typedef enum {
+	AB8500_CODEC_CR7_ENDRVHSL_DISABLED,
+	AB8500_CODEC_CR7_ENDRVHSL_ENABLED
+} t_ab8500_codec_cr7_endrvhsl;
+
+/* CR7 - 4 */
+typedef enum {
+	AB8500_CODEC_CR7_ENDRVHSR_DISABLED,
+	AB8500_CODEC_CR7_ENDRVHSR_ENABLED
+} t_ab8500_codec_cr7_endrvhsr;
+
+/* CR7 - 2 */
+typedef enum {
+	AB8500_CODEC_CR7_ENADCMIC_DISABLED,
+	AB8500_CODEC_CR7_ENADCMIC_ENABLED
+} t_ab8500_codec_cr7_enadcmic;
+
+/* CR7 - 1 */
+typedef enum {
+	AB8500_CODEC_CR7_ENADCLINL_DISABLED,
+	AB8500_CODEC_CR7_ENADCLINL_ENABLED
+} t_ab8500_codec_cr7_enadclinl;
+
+/* CR7 - 0 */
+typedef enum {
+	AB8500_CODEC_CR7_ENADCLINR_DISABLED,
+	AB8500_CODEC_CR7_ENADCLINR_ENABLED
+} t_ab8500_codec_cr7_enadclinr;
+
+/* CR8 - 7 */
+typedef enum {
+	AB8500_CODEC_CR8_CP_DIS_PLDWN_ENABLED,
+	AB8500_CODEC_CR8_CP_DIS_PLDWN_DISABLED
+} t_ab8500_codec_cr8_cp_dis_pldwn;
+
+/* CR8 - 6 */
+typedef enum {
+	AB8500_CODEC_CR8_ENEAR_DISABLED,
+	AB8500_CODEC_CR8_ENEAR_ENABLED
+} t_ab8500_codec_cr8_enear;
+
+/* CR8 - 5 */
+typedef enum {
+	AB8500_CODEC_CR8_ENHSL_DISABLED,
+	AB8500_CODEC_CR8_ENHSL_ENABLED
+} t_ab8500_codec_cr8_enhsl;
+
+/* CR8 - 4 */
+typedef enum {
+	AB8500_CODEC_CR8_ENHSR_DISABLED,
+	AB8500_CODEC_CR8_ENHSR_ENABLED
+} t_ab8500_codec_cr8_enhsr;
+
+/* CR8 - 3 */
+typedef enum {
+	AB8500_CODEC_CR8_ENHFL_DISABLED,
+	AB8500_CODEC_CR8_ENHFL_ENABLED
+} t_ab8500_codec_cr8_enhfl;
+
+/* CR8 - 2 */
+typedef enum {
+	AB8500_CODEC_CR8_ENHFR_DISABLED,
+	AB8500_CODEC_CR8_ENHFR_ENABLED
+} t_ab8500_codec_cr8_enhfr;
+
+/* CR8 - 1 */
+typedef enum {
+	AB8500_CODEC_CR8_ENVIBL_DISABLED,
+	AB8500_CODEC_CR8_ENVIBL_ENABLED
+} t_ab8500_codec_cr8_envibl;
+
+/* CR8 - 0 */
+typedef enum {
+	AB8500_CODEC_CR8_ENVIBR_DISABLED,
+	AB8500_CODEC_CR8_ENVIBR_ENABLED
+} t_ab8500_codec_cr8_envibr;
+
+/* CR9 - 6 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACEAR_DISABLED,
+	AB8500_CODEC_CR9_ENDACEAR_ENABLED
+} t_ab8500_codec_cr9_endacear;
+
+/* CR9 - 5 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACHSL_DISABLED,
+	AB8500_CODEC_CR9_ENDACHSL_ENABLED
+} t_ab8500_codec_cr9_endachsl;
+
+/* CR9 - 4 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACHSR_DISABLED,
+	AB8500_CODEC_CR9_ENDACHSR_ENABLED
+} t_ab8500_codec_cr9_endachsr;
+
+/* CR9 - 3 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACHFL_DISABLED,
+	AB8500_CODEC_CR9_ENDACHFL_ENABLED
+} t_ab8500_codec_cr9_endachfl;
+
+/* CR9 - 2 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACHFR_DISABLED,
+	AB8500_CODEC_CR9_ENDACHFR_ENABLED
+} t_ab8500_codec_cr9_endachfr;
+
+/* CR9 - 1 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACVIBL_DISABLED,
+	AB8500_CODEC_CR9_ENDACVIBL_ENABLED
+} t_ab8500_codec_cr9_endacvibl;
+
+/* CR9 - 0 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACVIBR_DISABLED,
+	AB8500_CODEC_CR9_ENDACVIBR_ENABLED
+} t_ab8500_codec_cr9_endacvibr;
+
+/* CR10 - 6 */
+typedef enum {
+	AB8500_CODEC_CR10_MUTEEAR_DISABLED,
+	AB8500_CODEC_CR10_MUTEEAR_ENABLED
+} t_ab8500_codec_cr10_muteear;
+
+/* CR10 - 5 */
+typedef enum {
+	AB8500_CODEC_CR10_MUTEHSL_DISABLED,
+	AB8500_CODEC_CR10_MUTEHSL_ENABLED
+} t_ab8500_codec_cr10_mutehsl;
+
+/* CR10 - 4 */
+typedef enum {
+	AB8500_CODEC_CR10_MUTEHSR_DISABLED,
+	AB8500_CODEC_CR10_MUTEHSR_ENABLED
+} t_ab8500_codec_cr10_mutehsr;
+
+/* CR10 - 3 */
+typedef enum {
+	AB8500_CODEC_CR10_MUTEHFL_DISABLED,
+	AB8500_CODEC_CR10_MUTEHFL_ENABLED
+} t_ab8500_codec_cr10_mutehfl;
+
+/* CR10 - 2 */
+typedef enum {
+	AB8500_CODEC_CR10_MUTEHFR_DISABLED,
+	AB8500_CODEC_CR10_MUTEHFR_ENABLED
+} t_ab8500_codec_cr10_mutehfr;
+
+/* CR10 - 1 */
+typedef enum {
+	AB8500_CODEC_CR10_MUTEVIBL_DISABLED,
+	AB8500_CODEC_CR10_MUTEVIBL_ENABLED
+} t_ab8500_codec_cr10_mutevibl;
+
+/* CR10 - 0 */
+typedef enum {
+	AB8500_CODEC_CR10_MUTEVIBR_DISABLED,
+	AB8500_CODEC_CR10_MUTEVIBR_ENABLED
+} t_ab8500_codec_cr10_mutevibr;
+
+/* CR11 - 7 */
+typedef enum {
+	AB8500_CODEC_CR11_EARSHORTPWD_DISABLED,
+	AB8500_CODEC_CR11_EARSHORTPWD_ENABLED
+} t_ab8500_codec_cr11_earshortpwd;
+
+/* CR11 - 6 */
+typedef enum {
+	AB8500_CODEC_CR11_EARSHORTDIS_ENABLED,
+	AB8500_CODEC_CR11_EARSHORTDIS_DISABLED
+} t_ab8500_codec_cr11_earshortdis;
+
+/* CR11 - 5 */
+typedef enum {
+	AB8500_CODEC_CR11_HSLSHORTDIS_ENABLED,
+	AB8500_CODEC_CR11_HSLSHORTDIS_DISABLED
+} t_ab8500_codec_cr11_hslshortdis;
+
+/* CR11 - 4 */
+typedef enum {
+	AB8500_CODEC_CR11_HSRSHORTDIS_ENABLED,
+	AB8500_CODEC_CR11_HSRSHORTDIS_DISABLED
+} t_ab8500_codec_cr11_hsrshortdis;
+
+/* CR11 - 3 */
+typedef enum {
+	AB8500_CODEC_CR11_HFLSHORTDIS_ENABLED,
+	AB8500_CODEC_CR11_HFLSHORTDIS_DISABLED
+} t_ab8500_codec_cr11_hflshortdis;
+
+/* CR11 - 2 */
+typedef enum {
+	AB8500_CODEC_CR11_HFRSHORTDIS_ENABLED,
+	AB8500_CODEC_CR11_HFRSHORTDIS_DISABLED
+} t_ab8500_codec_cr11_hfrshortdis;
+
+/* CR11 - 1 */
+typedef enum {
+	AB8500_CODEC_CR11_VIBLSHORTDIS_ENABLED,
+	AB8500_CODEC_CR11_VIBLSHORTDIS_DISABLED
+} t_ab8500_codec_cr11_viblshortdis;
+
+/* CR11 - 0 */
+typedef enum {
+	AB8500_CODEC_CR11_VIBRSHORTDIS_ENABLED,
+	AB8500_CODEC_CR11_VIBRSHORTDIS_DISABLED
+} t_ab8500_codec_cr11_vibrshortdis;
+
+/* CR12 - 7 */
+typedef enum {
+	AB8500_CODEC_CR12_ENCPHS_DISABLED,
+	AB8500_CODEC_CR12_ENCPHS_ENABLED
+} t_ab8500_codec_cr12_encphs;
+
+/* CR12 - 6:4 */
+typedef enum {
+	AB8500_CODEC_CR12_HSAUTOTIME_6_6USEC,
+	AB8500_CODEC_CR12_HSAUTOTIME_13_3USEC,
+	AB8500_CODEC_CR12_HSAUTOTIME_26_6USEC,
+	AB8500_CODEC_CR12_HSAUTOTIME_53_2USEC,
+	AB8500_CODEC_CR12_HSAUTOTIME_106_4USEC,
+	AB8500_CODEC_CR12_HSAUTOTIME_212_8USEC,
+	AB8500_CODEC_CR12_HSAUTOTIME_425_6USEC,
+	AB8500_CODEC_CR12_HSAUTOTIME_851_2USEC,
+} t_ab8500_codec_cr12_hsautotime;
+
+/* CR12 - 1 */
+typedef enum {
+	AB8500_CODEC_CR12_HSAUTOENSEL_DISABLED,
+	AB8500_CODEC_CR12_HSAUTOENSEL_ENABLED
+} t_ab8500_codec_cr12_hsautoensel;
+
+/* CR12 - 0 */
+typedef enum {
+	AB8500_CODEC_CR12_HSAUTOEN_DISABLED,
+	AB8500_CODEC_CR12_HSAUTOEN_ENABLED
+} t_ab8500_codec_cr12_hsautoen;
+
+/* CR13 - 7:4 */
+typedef enum {
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_25,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_50,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_100,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_150,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_200,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_250,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_300,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_350,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_400,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_450,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_500,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_550,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_600,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_650,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_700,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_750
+} t_ab8500_codec_cr13_envdet_hthresh;
+
+/* CR13 - 3:0 */
+typedef enum {
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_25,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_50,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_100,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_150,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_200,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_250,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_300,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_350,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_400,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_450,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_500,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_550,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_600,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_650,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_700,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_750
+} t_ab8500_codec_cr13_envdet_lthresh;
+
+/* CR14 - 7 */
+typedef enum {
+	AB8500_CODEC_CR14_SMPSLVEN_HIGHVOLTAGE,
+	AB8500_CODEC_CR14_SMPSLVEN_LOWVOLTAGE
+} t_ab8500_codec_cr14_smpslven;
+
+/* CR14 - 6 */
+typedef enum {
+	AB8500_CODEC_CR14_ENVDETSMPSEN_DISABLED,
+	AB8500_CODEC_CR14_ENVDETSMPSEN_ENABLED
+} t_ab8500_codec_cr14_envdetsmpsen;
+
+/* CR14 - 5 */
+typedef enum {
+	AB8500_CODEC_CR14_CPLVEN_HIGHVOLTAGE,
+	AB8500_CODEC_CR14_CPLVEN_LOWVOLTAGE
+} t_ab8500_codec_cr14_cplven;
+
+/* CR14 - 4 */
+typedef enum {
+	AB8500_CODEC_CR14_ENVDETCPEN_DISABLED,
+	AB8500_CODEC_CR14_ENVDETCPEN_ENABLED
+} t_ab8500_codec_cr14_envdetcpen;
+
+/* CR14 - 3:0 */
+typedef enum {
+	AB8500_CODEC_CR14_ENVET_TIME_27USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_53USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_106USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_212USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_424USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_848USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_1MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_3MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_6MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_13MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_27MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_54MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_109MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_218MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_436MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_872MSEC,
+} t_ab8500_codec_cr14_envet_time;
+
+/* CR15 - 7 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMTOVIBL_DA_PATH,
+	AB8500_CODEC_CR15_PWMTOVIBL_PWM
+} t_ab8500_codec_cr15_pwmtovibl;
+
+/* CR15 - 6 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMTOVIBR_DA_PATH,
+	AB8500_CODEC_CR15_PWMTOVIBR_PWM
+} t_ab8500_codec_cr15_pwmtovibr;
+
+/* CR15 - 5 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMLCTRL_PWMNPLGPOL,
+	AB8500_CODEC_CR15_PWMLCTRL_PWMNPLDUTYCYCLE
+} t_ab8500_codec_cr15_pwmlctrl;
+
+/* CR15 - 4 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMRCTRL_PWMNPRGPOL,
+	AB8500_CODEC_CR15_PWMRCTRL_PWMNPRDUTYCYCLE
+} t_ab8500_codec_cr15_pwmrctrl;
+
+/* CR15 - 3 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMNLCTRL_PWMNLGPOL,
+	AB8500_CODEC_CR15_PWMNLCTRL_PWMNLDUTYCYCLE
+} t_ab8500_codec_cr15_pwmnlctrl;
+
+/* CR15 - 2 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMPLCTRL_PWMPLGPOL,
+	AB8500_CODEC_CR15_PWMPLCTRL_PWMPLDUTYCYCLE
+} t_ab8500_codec_cr15_pwmplctrl;
+
+/* CR15 - 1 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMNRCTRL_PWMNRGPOL,
+	AB8500_CODEC_CR15_PWMNRCTRL_PWMNRDUTYCYCLE
+} t_ab8500_codec_cr15_pwmnrctrl;
+
+/* CR15 - 0 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMPRCTRL_PWMPRGPOL,
+	AB8500_CODEC_CR15_PWMPRCTRL_PWMPRDUTYCYCLE
+} t_ab8500_codec_cr15_pwmprctrl;
+
+/* CR16 - 7 */
+typedef enum {
+	AB8500_CODEC_CR16_PWMNLPOL_GNDVIB,
+	AB8500_CODEC_CR16_PWMNLPOL_VINVIB
+} t_ab8500_codec_cr16_pwmnlpol;
+
+/* CR16 - 6:0 */
+typedef t_uint8 t_ab8500_codec_cr16_pwmnldutycycle;
+
+/* CR17 - 7 */
+typedef enum {
+	AB8500_CODEC_CR17_PWMPLPOL_GNDVIB,
+	AB8500_CODEC_CR17_PWMPLPOL_VINVIB
+} t_ab8500_codec_cr17_pwmplpol;
+
+/* CR17 - 6:0 */
+typedef t_uint8 t_ab8500_codec_cr17_pwmpldutycycle;
+
+/* CR18 - 7 */
+typedef enum {
+	AB8500_CODEC_CR18_PWMNRPOL_GNDVIB,
+	AB8500_CODEC_CR18_PWMNRPOL_VINVIB
+} t_ab8500_codec_cr18_pwmnrpol;
+
+/* CR18 - 6:0 */
+typedef t_uint8 t_ab8500_codec_cr18_pwmnrdutycycle;
+
+/* CR19 - 7 */
+typedef enum {
+	AB8500_CODEC_CR19_PWMPRPOL_GNDVIB,
+	AB8500_CODEC_CR19_PWMPRPOL_VINVIB
+} t_ab8500_codec_cr19_pwmprpol;
+
+/* CR19 - 6:0 */
+typedef t_uint8 t_ab8500_codec_cr19_pwmprdutycycle;
+
+/* CR20 - 7 */
+typedef enum {
+	AB8500_CODEC_CR20_EN_SE_MIC1_DIFFERENTIAL,
+	AB8500_CODEC_CR20_EN_SE_MIC1_SINGLE
+} t_ab8500_codec_cr20_en_se_mic1;
+
+/* CR20 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr20_mic1_gain;
+
+/* CR21 - 7 */
+typedef enum {
+	AB8500_CODEC_CR21_EN_SE_MIC2_DIFFERENTIAL,
+	AB8500_CODEC_CR21_EN_SE_MIC2_SINGLE
+} t_ab8500_codec_cr21_en_se_mic2;
+
+/* CR21 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr21_mic2_gain;
+
+/* CR22 - 7:5 */
+typedef t_uint8 t_ab8500_codec_cr22_hsl_gain;
+
+/* CR22 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr22_linl_gain;
+
+/* CR23 - 7:5 */
+typedef t_uint8 t_ab8500_codec_cr23_hsr_gain;
+
+/* CR23 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr23_linr_gain;
+
+/* CR24 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr24_lintohsl_gain;
+
+/* CR25 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr25_lintohsr_gain;
+
+/* CR26 - 7 */
+typedef enum {
+	AB8500_CODEC_CR26_AD1NH_FILTER_ENABLED,
+	AB8500_CODEC_CR26_AD1NH_FILTER_DISABLED
+} t_ab8500_codec_cr26_ad1nh;
+
+/* CR26 - 6 */
+typedef enum {
+	AB8500_CODEC_CR26_AD2NH_FILTER_ENABLED,
+	AB8500_CODEC_CR26_AD2NH_FILTER_DISABLED
+} t_ab8500_codec_cr26_ad2nh;
+
+/* CR26 - 5 */
+typedef enum {
+	AB8500_CODEC_CR26_AD3NH_FILTER_ENABLED,
+	AB8500_CODEC_CR26_AD3NH_FILTER_DISABLED
+} t_ab8500_codec_cr26_ad3nh;
+
+/* CR26 - 4 */
+typedef enum {
+	AB8500_CODEC_CR26_AD4NH_FILTER_ENABLED,
+	AB8500_CODEC_CR26_AD4NH_FILTER_DISABLED
+} t_ab8500_codec_cr26_ad4nh;
+
+/* CR26 - 3 */
+typedef enum {
+	AB8500_CODEC_CR26_AD1_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR26_AD1_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr26_ad1_voice;
+
+/* CR26 - 2 */
+typedef enum {
+	AB8500_CODEC_CR26_AD2_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR26_AD2_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr26_ad2_voice;
+
+/* CR26 - 1 */
+typedef enum {
+	AB8500_CODEC_CR26_AD3_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR26_AD3_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr26_ad3_voice;
+
+/* CR26 - 0 */
+typedef enum {
+	AB8500_CODEC_CR26_AD4_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR26_AD4_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr26_ad4_voice;
+
+/* CR27 - 7 */
+typedef enum {
+	AB8500_CODEC_CR27_EN_MASTGEN_DISABLED,
+	AB8500_CODEC_CR27_EN_MASTGEN_ENABLED
+} t_ab8500_codec_cr27_en_mastgen;
+
+/* CR27 - 6:5 */
+typedef enum {
+	AB8500_CODEC_CR27_IF1_BITCLK_OSR_32,
+	AB8500_CODEC_CR27_IF1_BITCLK_OSR_64,
+	AB8500_CODEC_CR27_IF1_BITCLK_OSR_128,
+	AB8500_CODEC_CR27_IF1_BITCLK_OSR_256
+} t_ab8500_codec_cr27_if1_bitclk_osr;
+
+/* CR27 - 4 */
+typedef enum {
+	AB8500_CODEC_CR27_ENFS_BITCLK1_DISABLED,
+	AB8500_CODEC_CR27_ENFS_BITCLK1_ENABLED
+} t_ab8500_codec_cr27_enfs_bitclk1;
+
+/* CR27 - 2:1 */
+typedef enum {
+	AB8500_CODEC_CR27_IF0_BITCLK_OSR_32,
+	AB8500_CODEC_CR27_IF0_BITCLK_OSR_64,
+	AB8500_CODEC_CR27_IF0_BITCLK_OSR_128,
+	AB8500_CODEC_CR27_IF0_BITCLK_OSR_256
+} t_ab8500_codec_cr27_if0_bitclk_osr;
+
+/* CR27 - 0 */
+typedef enum {
+	AB8500_CODEC_CR27_ENFS_BITCLK0_DISABLED,
+	AB8500_CODEC_CR27_ENFS_BITCLK0_ENABLED
+} t_ab8500_codec_cr27_enfs_bitclk0;
+
+/* CR28 - 6 */
+typedef enum {
+	AB8500_CODEC_CR28_FSYNC0P_RISING_EDGE,
+	AB8500_CODEC_CR28_FSYNC0P_FALLING_EDGE
+} t_ab8500_codec_cr28_fsync0p;
+
+/* CR28 - 5 */
+typedef enum {
+	AB8500_CODEC_CR28_BITCLK0P_RISING_EDGE,
+	AB8500_CODEC_CR28_BITCLK0P_FALLING_EDGE
+} t_ab8500_codec_cr28_bitclk0p;
+
+/* CR28 - 4 */
+typedef enum {
+	AB8500_CODEC_CR28_IF0DEL_NOT_DELAYED,
+	AB8500_CODEC_CR28_IF0DEL_DELAYED
+} t_ab8500_codec_cr28_if0del;
+
+/* CR28 - 3:2 */
+typedef enum {
+	AB8500_CODEC_CR28_IF0FORMAT_DISABLED,
+	AB8500_CODEC_CR28_IF0FORMAT_TDM,
+	AB8500_CODEC_CR28_IF0FORMAT_I2S_LEFTALIGNED
+} t_ab8500_codec_cr28_if0format;
+
+/* CR28 - 1:0 */
+typedef enum {
+	AB8500_CODEC_CR28_IF0WL_16BITS,
+	AB8500_CODEC_CR28_IF0WL_20BITS,
+	AB8500_CODEC_CR28_IF0WL_24BITS,
+	AB8500_CODEC_CR28_IF0WL_32BITS
+} t_ab8500_codec_cr28_if0wl;
+
+/* CR29 - 7 */
+typedef enum {
+	AB8500_CODEC_CR29_IF0DATOIF1AD_NOTSENT,
+	AB8500_CODEC_CR29_IF0DATOIF1AD_SENT
+} t_ab8500_codec_cr29_if0datoif1ad;
+
+/* CR29 - 6 */
+typedef enum {
+	AB8500_CODEC_CR29_IF0CKTOIF1CK_NOTSENT,
+	AB8500_CODEC_CR29_IF0CKTOIF1CK_SENT
+} t_ab8500_codec_cr29_if0cktoif1ck;
+
+/* CR29 - 5 */
+typedef enum {
+	AB8500_CODEC_CR29_IF1MASTER_FS1CK1_INPUT,
+	AB8500_CODEC_CR29_IF1MASTER_FS1CK1_OUTPUT
+} t_ab8500_codec_cr29_if1master;
+
+/* CR29 - 3 */
+typedef enum {
+	AB8500_CODEC_CR29_IF1DATOIF0AD_NOTSENT,
+	AB8500_CODEC_CR29_IF1DATOIF0AD_SENT
+} t_ab8500_codec_cr29_if1datoif0ad;
+
+/* CR29 - 2 */
+typedef enum {
+	AB8500_CODEC_CR29_IF1CKTOIF0CK_NOTSENT,
+	AB8500_CODEC_CR29_IF1CKTOIF0CK_SENT
+} t_ab8500_codec_cr29_if1cktoif0ck;
+
+/* CR29 - 1 */
+typedef enum {
+	AB8500_CODEC_CR29_IF0MASTER_FS0CK0_INPUT,
+	AB8500_CODEC_CR29_IF0MASTER_FS0CK0_OUTPUT
+} t_ab8500_codec_cr29_if0master;
+
+/* CR29 - 0 */
+typedef enum {
+	AB8500_CODEC_CR29_IF0BFIFOEN_NORMAL_MODE,
+	AB8500_CODEC_CR29_IF0BFIFOEN_BURST_MODE
+} t_ab8500_codec_cr29_if0bfifoen;
+
+/* CR30 - 6 */
+typedef enum {
+	AB8500_CODEC_CR30_FSYNC1P_RISING_EDGE,
+	AB8500_CODEC_CR30_FSYNC1P_FALLING_EDGE
+} t_ab8500_codec_cr30_fsync1p;
+
+/* CR30 - 5 */
+typedef enum {
+	AB8500_CODEC_CR30_BITCLK1P_RISING_EDGE,
+	AB8500_CODEC_CR30_BITCLK1P_FALLING_EDGE
+} t_ab8500_codec_cr30_bitclk1p;
+
+/* CR30 - 4 */
+typedef enum {
+	AB8500_CODEC_CR30_IF1DEL_NOT_DELAYED,
+	AB8500_CODEC_CR30_IF1DEL_DELAYED
+} t_ab8500_codec_cr30_if1del;
+
+/* CR30 - 3:2 */
+typedef enum {
+	AB8500_CODEC_CR30_IF1FORMAT_DISABLED,
+	AB8500_CODEC_CR30_IF1FORMAT_TDM,
+	AB8500_CODEC_CR30_IF1FORMAT_I2S_LEFTALIGNED
+} t_ab8500_codec_cr30_if1format;
+
+/* CR30 - 1:0 */
+typedef enum {
+	AB8500_CODEC_CR30_IF1WL_16BITS,
+	AB8500_CODEC_CR30_IF1WL_20BITS,
+	AB8500_CODEC_CR30_IF1WL_24BITS,
+	AB8500_CODEC_CR30_IF1WL_32BITS
+} t_ab8500_codec_cr30_if1wl;
+
+/* CR31:46 - 7:4 or 3:0 */
+/* In ab8500_codec.h */
+
+/* CR47:50 - 7/6/5/4/3/2/1/0 */
+typedef enum {
+	AB8500_CODEC_CR47_TO_CR50_HIZ_SL_LOW_IMPEDANCE,
+	AB8500_CODEC_CR47_TO_CR50_HIZ_SL_HIGH_IMPEDANCE,
+} t_ab8500_codec_cr47_to_cr50_hiz_sl;
+
+/* CR51 - 7 */
+typedef enum {
+	AB8500_CODEC_CR51_DA12_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR51_DA12_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr51_da12_voice;
+
+/* CR51 - 5 */
+typedef enum {
+	AB8500_CODEC_CR51_SLDAI1TOSLADO1_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR51_SLDAI1TOSLADO1_LOOPEDBACK
+} t_ab8500_codec_cr51_sldai1toslado1;
+
+/* CR51:56 - 4:0 */
+/* In ab8500_codec.h */
+
+/* CR52 - 5 */
+typedef enum {
+	AB8500_CODEC_CR52_SLDAI2TOSLADO2_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR52_SLDAI2TOSLADO2_LOOPEDBACK
+} t_ab8500_codec_cr52_sldai2toslado2;
+
+/* CR53 - 7 */
+typedef enum {
+	AB8500_CODEC_CR53_DA34_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR53_DA34_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr53_da34_voice;
+
+/* CR53 - 5 */
+typedef enum {
+	AB8500_CODEC_CR53_SLDAI3TOSLADO3_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR53_SLDAI3TOSLADO3_LOOPEDBACK
+} t_ab8500_codec_cr53_sldai3toslado3;
+
+/* CR54 - 5 */
+typedef enum {
+	AB8500_CODEC_CR54_SLDAI4TOSLADO4_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR54_SLDAI4TOSLADO4_LOOPEDBACK
+} t_ab8500_codec_cr54_sldai4toslado4;
+
+/* CR55 - 7 */
+typedef enum {
+	AB8500_CODEC_CR55_DA56_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR55_DA56_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr55_da56_voice;
+
+/* CR55 - 6:5 */
+typedef enum {
+	AB8500_CODEC_CR55_SLDAI5TOSLADO5_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR55_SLDAI5TOSLADO5_DA_IN1_LOOPEDBACK,
+	AB8500_CODEC_CR55_SLDAI5TOSLADO5_DA_IN3_LOOPEDBACK,
+	AB8500_CODEC_CR55_SLDAI5TOSLADO5_DA_IN5_LOOPEDBACK
+} t_ab8500_codec_cr55_sldai5toslado5;
+
+/* CR56 - 6:5 */
+typedef enum {
+	AB8500_CODEC_CR56_SLDAI6TOSLADO7_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR56_SLDAI6TOSLADO7_DA_IN2_LOOPEDBACK,
+	AB8500_CODEC_CR56_SLDAI6TOSLADO7_DA_IN4_LOOPEDBACK,
+	AB8500_CODEC_CR56_SLDAI6TOSLADO7_DA_IN6_LOOPEDBACK
+} t_ab8500_codec_cr56_sldai6toslado7;
+
+/* CR57 - 6 */
+typedef enum {
+	AB8500_CODEC_CR57_BFIFULL_MSK_MASKED,
+	AB8500_CODEC_CR57_BFIFULL_MSK_ENABLED
+} t_ab8500_codec_cr57_bfifull_msk;
+
+/* CR57 - 5 */
+typedef enum {
+	AB8500_CODEC_CR57_BFIEMPT_MSK_MASKED,
+	AB8500_CODEC_CR57_BFIEMPT_MSK_ENABLED
+} t_ab8500_codec_cr57_bfiempt_msk;
+
+/* CR57 - 4 */
+typedef enum {
+	AB8500_CODEC_CR57_DACHAN_MSK_MASKED,
+	AB8500_CODEC_CR57_DACHAN_MSK_ENABLED
+} t_ab8500_codec_cr57_dachan_msk;
+
+/* CR57 - 3 */
+typedef enum {
+	AB8500_CODEC_CR57_GAIN_MSK_MASKED,
+	AB8500_CODEC_CR57_GAIN_MSK_ENABLED
+} t_ab8500_codec_cr57_gain_msk;
+
+/* CR57 - 2 */
+typedef enum {
+	AB8500_CODEC_CR57_DSPAD_MSK_MASKED,
+	AB8500_CODEC_CR57_DSPAD_MSK_ENABLED
+} t_ab8500_codec_cr57_dspad_msk;
+
+/* CR57 - 1 */
+typedef enum {
+	AB8500_CODEC_CR57_DSPDA_MSK_MASKED,
+	AB8500_CODEC_CR57_DSPDA_MSK_ENABLED
+} t_ab8500_codec_cr57_dspda_msk;
+
+/* CR57 - 0 */
+typedef enum {
+	AB8500_CODEC_CR57_STFIR_MSK_MASKED,
+	AB8500_CODEC_CR57_STFIR_MSK_ENABLED
+} t_ab8500_codec_cr57_stfir_msk;
+
+/* CR58 - Read Only */
+/* CR58 - 6 */
+typedef enum {
+	AB8500_CODEC_CR58_BFIFULL_EV_NOT_FULL,
+	AB8500_CODEC_CR58_BFIFULL_EV_FULL
+} t_ab8500_codec_cr58_bfifull_ev;
+
+/* CR58 - 5 */
+typedef enum {
+	AB8500_CODEC_CR58_BFIEMPT_EV_NOT_EMPTY,
+	AB8500_CODEC_CR58_BFIEMPT_EV_EMPTY
+} t_ab8500_codec_cr58_bfiempt_ev;
+
+/* CR58 - 4 */
+typedef enum {
+	AB8500_CODEC_CR58_DACHAN_EV_NO_SATURATION,
+	AB8500_CODEC_CR58_DACHAN_EV_SATURATION
+} t_ab8500_codec_cr58_dachan_ev;
+
+/* CR58 - 3 */
+typedef enum {
+	AB8500_CODEC_CR58_GAIN_EV_NO_SATURATION,
+	AB8500_CODEC_CR58_GAIN_EV_SATURATION
+} t_ab8500_codec_cr58_gain_ev;
+
+/* CR58 - 2 */
+typedef enum {
+	AB8500_CODEC_CR58_DSPAD_EV_NO_SATURATION,
+	AB8500_CODEC_CR58_DSPAD_EV_SATURATION
+} t_ab8500_codec_cr58_dspad_ev;
+
+/* CR58 - 1 */
+typedef enum {
+	AB8500_CODEC_CR58_DSPDA_EV_NO_SATURATION,
+	AB8500_CODEC_CR58_DSPDA_EV_SATURATION
+} t_ab8500_codec_cr58_dspda_ev;
+
+/* CR58 - 0 */
+typedef enum {
+	AB8500_CODEC_CR58_STFIR_EV_NO_SATURATION,
+	AB8500_CODEC_CR58_STFIR_EV_SATURATION
+} t_ab8500_codec_cr58_stfir_ev;
+
+/* CR59 - 7 */
+typedef enum {
+	AB8500_CODEC_CR59_VSSREADY_MSK_MASKED,
+	AB8500_CODEC_CR59_VSSREADY_MSK_ENABLED
+} t_ab8500_codec_cr59_vssready_msk;
+
+/* CR59 - 6 */
+typedef enum {
+	AB8500_CODEC_CR59_SHRTVIBL_MSK_MASKED,
+	AB8500_CODEC_CR59_SHRTVIBL_MSK_ENABLED
+} t_ab8500_codec_cr59_shrtvibl_msk;
+
+/* CR59 - 5 */
+typedef enum {
+	AB8500_CODEC_CR59_SHRTVIBR_MSK_MASKED,
+	AB8500_CODEC_CR59_SHRTVIBR_MSK_ENABLED
+} t_ab8500_codec_cr59_shrtvibr_msk;
+
+/* CR59 - 4 */
+typedef enum {
+	AB8500_CODEC_CR59_SHRTHFL_MSK_MASKED,
+	AB8500_CODEC_CR59_SHRTHFL_MSK_ENABLED
+} t_ab8500_codec_cr59_shrthfl_msk;
+
+/* CR59 - 3 */
+typedef enum {
+	AB8500_CODEC_CR59_SHRTHFR_MSK_MASKED,
+	AB8500_CODEC_CR59_SHRTHFR_MSK_ENABLED
+} t_ab8500_codec_cr59_shrthfr_msk;
+
+/* CR59 - 2 */
+typedef enum {
+	AB8500_CODEC_CR59_SHRTHSL_MSK_MASKED,
+	AB8500_CODEC_CR59_SHRTHSL_MSK_ENABLED
+} t_ab8500_codec_cr59_shrthsl_msk;
+
+/* CR59 - 1 */
+typedef enum {
+	AB8500_CODEC_CR59_SHRTHSR_MSK_MASKED,
+	AB8500_CODEC_CR59_SHRTHSR_MSK_ENABLED
+} t_ab8500_codec_cr59_shrthsr_msk;
+
+/* CR59 - 0 */
+typedef enum {
+	AB8500_CODEC_CR59_SHRTEAR_MSK_MASKED,
+	AB8500_CODEC_CR59_SHRTEAR_MSK_ENABLED
+} t_ab8500_codec_cr59_shrtear_msk;
+
+/* CR60 - Read Only */
+/* CR60 - 7 */
+typedef enum {
+	AB8500_CODEC_CR60_VSSREADY_EV_NOT_READY,
+	AB8500_CODEC_CR60_VSSREADY_EV_READY
+} t_ab8500_codec_cr60_vssready_ev;
+
+/* CR60 - 6 */
+typedef enum {
+	AB8500_CODEC_CR60_SHRTVIBL_EV_NO_SHORTCIRCUIT,
+	AB8500_CODEC_CR60_SHRTVIBL_EV_SHORTCIRCUIT
+} t_ab8500_codec_cr60_shrtvibl_ev;
+
+/* CR60 - 5 */
+typedef enum {
+	AB8500_CODEC_CR60_SHRTVIBR_EV_NO_SHORTCIRCUIT,
+	AB8500_CODEC_CR60_SHRTVIBR_EV_SHORTCIRCUIT
+} t_ab8500_codec_cr60_shrtvibr_ev;
+
+/* CR60 - 4 */
+typedef enum {
+	AB8500_CODEC_CR60_SHRTHFL_EV_NO_SHORTCIRCUIT,
+	AB8500_CODEC_CR60_SHRTHFL_EV_SHORTCIRCUIT
+} t_ab8500_codec_cr60_shrthfl_ev;
+
+/* CR60 - 3 */
+typedef enum {
+	AB8500_CODEC_CR60_SHRTHFR_EV_NO_SHORTCIRCUIT,
+	AB8500_CODEC_CR60_SHRTHFR_EV_SHORTCIRCUIT
+} t_ab8500_codec_cr60_shrthfr_ev;
+
+/* CR60 - 2 */
+typedef enum {
+	AB8500_CODEC_CR60_SHRTHSL_EV_NO_SHORTCIRCUIT,
+	AB8500_CODEC_CR60_SHRTHSL_EV_SHORTCIRCUIT
+} t_ab8500_codec_cr60_shrthsl_ev;
+
+/* CR60 - 1 */
+typedef enum {
+	AB8500_CODEC_CR60_SHRTHSR_EV_NO_SHORTCIRCUIT,
+	AB8500_CODEC_CR60_SHRTHSR_EV_SHORTCIRCUIT
+} t_ab8500_codec_cr60_shrthsr_ev;
+
+/* CR60 - 0 */
+typedef enum {
+	AB8500_CODEC_CR60_SHRTEAR_EV_NO_SHORTCIRCUIT,
+	AB8500_CODEC_CR60_SHRTEAR_EV_SHORTCIRCUIT
+} t_ab8500_codec_cr60_shrtear_ev;
+
+/* CR61 - 6:2 - Read Only */
+typedef enum {
+	AB8500_CODEC_CR61_REVISION_1_0,
+	AB8500_CODEC_CR61_REVISION_TBD
+} t_ab8500_codec_cr61_revision;
+
+/* CR61 - 1:0 */
+typedef enum {
+	AB8500_CODEC_CR61_FADE_SPEED_1MS,
+	AB8500_CODEC_CR61_FADE_SPEED_4MS,
+	AB8500_CODEC_CR61_FADE_SPEED_8MS,
+	AB8500_CODEC_CR61_FADE_SPEED_16MS
+} t_ab8500_codec_cr61_fade_speed;
+
+/* CR62 - Read Only */
+/* CR62 - 5 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC1SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC1SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic1sinc3;
+
+/* CR62 - 4 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC2SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC2SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic2sinc3;
+
+/* CR62 - 3 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC3SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC3SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic3sinc3;
+
+/* CR62 - 2 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC4SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC4SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic4sinc3;
+
+/* CR62 - 1 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC5SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC5SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic5sinc3;
+
+/* CR62 - 0 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC6SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC6SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic6sinc3;
+
+/* CR63 - 7 */
+typedef enum {
+	AB8500_CODEC_CR63_DATOHSLEN_DISABLED,
+	AB8500_CODEC_CR63_DATOHSLEN_ENABLED
+} t_ab8500_codec_cr63_datohslen;
+
+/* CR63 - 6 */
+typedef enum {
+	AB8500_CODEC_CR63_DATOHSREN_DISABLED,
+	AB8500_CODEC_CR63_DATOHSREN_ENABLED
+} t_ab8500_codec_cr63_datohsren;
+
+/* CR63 - 5 */
+typedef enum {
+	AB8500_CODEC_CR63_AD1SEL_LINLADL_SELECTED,
+	AB8500_CODEC_CR63_AD1SEL_DMIC1_SELECTED
+} t_ab8500_codec_cr63_ad1sel;
+
+/* CR63 - 4 */
+typedef enum {
+	AB8500_CODEC_CR63_AD2SEL_LINRADR_SELECTED,
+	AB8500_CODEC_CR63_AD2SEL_DMIC2_SELECTED
+} t_ab8500_codec_cr63_ad2sel;
+
+/* CR63 - 3 */
+typedef enum {
+	AB8500_CODEC_CR63_AD3SEL_ADMO_SELECTED,
+	AB8500_CODEC_CR63_AD3SEL_DMIC3_SELECTED
+} t_ab8500_codec_cr63_ad3sel;
+
+/* CR63 - 2 */
+typedef enum {
+	AB8500_CODEC_CR63_AD5SEL_AMADR_SELECTED,
+	AB8500_CODEC_CR63_AD5SEL_DMIC5_SELECTED
+} t_ab8500_codec_cr63_ad5sel;
+
+/* CR63 - 1 */
+typedef enum {
+	AB8500_CODEC_CR63_AD6SEL_ADMO_SELECTED,
+	AB8500_CODEC_CR63_AD6SEL_DMIC6_SELECTED
+} t_ab8500_codec_cr63_ad6sel;
+
+/* CR63 - 0 */
+typedef enum {
+	AB8500_CODEC_CR63_ANCSEL_NOT_MIXED_IN_EAR,
+	AB8500_CODEC_CR63_ANCSEL_MIXED_IN_EAR
+} t_ab8500_codec_cr63_ancsel;
+
+/* CR64 - 7 */
+typedef enum {
+	AB8500_CODEC_CR64_DATOHFREN_NOT_MIXED_TO_HFR,
+	AB8500_CODEC_CR64_DATOHFREN_MIXED_TO_HFR
+} t_ab8500_codec_cr64_datohfren;
+
+/* CR64 - 6 */
+typedef enum {
+	AB8500_CODEC_CR64_DATOHFLEN_NOT_MIXED_TO_HFL,
+	AB8500_CODEC_CR64_DATOHFLEN_MIXED_TO_HFL
+} t_ab8500_codec_cr64_datohflen;
+
+/* CR64 - 5 */
+typedef enum {
+	AB8500_CODEC_CR64_HFRSEL_DA4_MIXED_TO_HFR,
+	AB8500_CODEC_CR64_HFRSEL_ANC_MIXED_TO_HFR
+} t_ab8500_codec_cr64_hfrsel;
+
+/* CR64 - 4 */
+typedef enum {
+	AB8500_CODEC_CR64_HFLSEL_DA3_MIXED_TO_HFL,
+	AB8500_CODEC_CR64_HFLSEL_ANC_MIXED_TO_HFL
+} t_ab8500_codec_cr64_hflsel;
+
+/* CR64 - 3:2 */
+typedef enum {
+	AB8500_CODEC_CR64_STFIR1SEL_AD_OUT1_SELECTED,
+	AB8500_CODEC_CR64_STFIR1SEL_AD_OUT3_SELECTED,
+	AB8500_CODEC_CR64_STFIR1SEL_DA_IN1_SELECTED
+} t_ab8500_codec_cr64_stfir1sel;
+
+/* CR64 - 1:0 */
+typedef enum {
+	AB8500_CODEC_CR64_STFIR2SEL_AD_OUT2_SELECTED,
+	AB8500_CODEC_CR64_STFIR2SEL_AD_OUT4_SELECTED,
+	AB8500_CODEC_CR64_STFIR2SEL_DA_IN2_SELECTED
+} t_ab8500_codec_cr64_stfir2sel;
+
+/* CR65 - 6 */
+typedef enum {
+	AB8500_CODEC_CR65_FADEDIS_AD1_ENABLED,
+	AB8500_CODEC_CR65_FADEDIS_AD1_DISABLED
+} t_ab8500_codec_cr65_fadedis_ad1;
+
+/* CR65 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr65_ad1gain;
+
+/* CR66 - 6 */
+typedef enum {
+	AB8500_CODEC_CR66_FADEDIS_AD2_ENABLED,
+	AB8500_CODEC_CR66_FADEDIS_AD2_DISABLED
+} t_ab8500_codec_cr66_fadedis_ad2;
+
+/* CR66 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr66_ad2gain;
+
+/* CR67 - 6 */
+typedef enum {
+	AB8500_CODEC_CR67_FADEDIS_AD3_ENABLED,
+	AB8500_CODEC_CR67_FADEDIS_AD3_DISABLED
+} t_ab8500_codec_cr67_fadedis_ad3;
+
+/* CR67 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr67_ad3gain;
+
+/* CR68 - 6 */
+typedef enum {
+	AB8500_CODEC_CR68_FADEDIS_AD4_ENABLED,
+	AB8500_CODEC_CR68_FADEDIS_AD4_DISABLED
+} t_ab8500_codec_cr68_fadedis_ad4;
+
+/* CR68 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr68_ad4gain;
+
+/* CR69 - 6 */
+typedef enum {
+	AB8500_CODEC_CR69_FADEDIS_AD5_ENABLED,
+	AB8500_CODEC_CR69_FADEDIS_AD5_DISABLED
+} t_ab8500_codec_cr69_fadedis_ad5;
+
+/* CR69 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr69_ad5gain;
+
+/* CR70 - 6 */
+typedef enum {
+	AB8500_CODEC_CR70_FADEDIS_AD6_ENABLED,
+	AB8500_CODEC_CR70_FADEDIS_AD6_DISABLED
+} t_ab8500_codec_cr70_fadedis_ad6;
+
+/* CR70 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr70_ad6gain;
+
+/* CR71 - 6 */
+typedef enum {
+	AB8500_CODEC_CR71_FADEDIS_DA1_ENABLED,
+	AB8500_CODEC_CR71_FADEDIS_DA1_DISABLED
+} t_ab8500_codec_cr71_fadedis_da1;
+
+/* CR71 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr71_da1gain;
+
+/* CR72 - 6 */
+typedef enum {
+	AB8500_CODEC_CR72_FADEDIS_DA2_ENABLED,
+	AB8500_CODEC_CR72_FADEDIS_DA2_DISABLED
+} t_ab8500_codec_cr72_fadedis_da2;
+
+/* CR72 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr72_da2gain;
+
+/* CR73 - 6 */
+typedef enum {
+	AB8500_CODEC_CR73_FADEDIS_DA3_ENABLED,
+	AB8500_CODEC_CR73_FADEDIS_DA3_DISABLED
+} t_ab8500_codec_cr73_fadedis_da3;
+
+/* CR73 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr73_da3gain;
+
+/* CR74 - 6 */
+typedef enum {
+	AB8500_CODEC_CR74_FADEDIS_DA4_ENABLED,
+	AB8500_CODEC_CR74_FADEDIS_DA4_DISABLED
+} t_ab8500_codec_cr74_fadedis_da4;
+
+/* CR74 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr74_da4gain;
+
+/* CR75 - 6 */
+typedef enum {
+	AB8500_CODEC_CR75_FADEDIS_DA5_ENABLED,
+	AB8500_CODEC_CR75_FADEDIS_DA5_DISABLED
+} t_ab8500_codec_cr75_fadedis_da5;
+
+/* CR75 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr75_da5gain;
+
+/* CR76 - 6 */
+typedef enum {
+	AB8500_CODEC_CR76_FADEDIS_DA6_ENABLED,
+	AB8500_CODEC_CR76_FADEDIS_DA6_DISABLED
+} t_ab8500_codec_cr76_fadedis_da6;
+
+/* CR76 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr76_da6gain;
+
+/* CR77 - 6 */
+typedef enum {
+	AB8500_CODEC_CR77_FADEDIS_AD1L_TO_HFL_ENABLED,
+	AB8500_CODEC_CR77_FADEDIS_AD1L_TO_HFL_DISABLED
+} t_ab8500_codec_cr77_fadedis_ad1l;
+
+/* CR77 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr77_ad1lbgain_to_hfl;
+
+/* CR78 - 6 */
+typedef enum {
+	AB8500_CODEC_CR78_FADEDIS_AD2L_TO_HFR_ENABLED,
+	AB8500_CODEC_CR78_FADEDIS_AD2L_TO_HFR_DISABLED
+} t_ab8500_codec_cr78_fadedis_ad2l;
+
+/* CR78 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr78_ad2lbgain_to_hfr;
+
+/* CR79 - 7 */
+typedef enum {
+	AB8500_CODEC_CR79_HSSINC1_SINC3_CHOOSEN,
+	AB8500_CODEC_CR79_HSSINC1_SINC1_CHOOSEN
+} t_ab8500_codec_cr79_hssinc1;
+
+/* CR79 - 4 */
+typedef enum {
+	AB8500_CODEC_CR79_FADEDIS_HSL_ENABLED,
+	AB8500_CODEC_CR79_FADEDIS_HSL_DISABLED
+} t_ab8500_codec_cr79_fadedis_hsl;
+
+/* CR79 - 3:0 */
+typedef t_uint8 t_ab8500_codec_cr79_hsldgain;
+
+/* CR80 - 4 */
+typedef enum {
+	AB8500_CODEC_CR80_FADEDIS_HSR_ENABLED,
+	AB8500_CODEC_CR80_FADEDIS_HSR_DISABLED
+} t_ab8500_codec_cr80_fadedis_hsr;
+
+/* CR80 - 3:0 */
+typedef t_uint8 t_ab8500_codec_cr80_hsrdgain;
+
+/* CR81 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr81_stfir1gain;
+
+/* CR82 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr82_stfir2gain;
+
+/* CR83 - 2 */
+typedef enum {
+	AB8500_CODEC_CR83_ENANC_DISABLED,
+	AB8500_CODEC_CR83_ENANC_ENABLED
+} t_ab8500_codec_cr83_enanc;
+
+/* CR83 - 1 */
+typedef enum {
+	AB8500_CODEC_CR83_ANCIIRINIT_NOT_STARTED,
+	AB8500_CODEC_CR83_ANCIIRINIT_STARTED
+} t_ab8500_codec_cr83_anciirinit;
+
+/* CR83 - 0 */
+typedef enum {
+	AB8500_CODEC_CR83_ANCFIRUPDATE_RESETTED,
+	AB8500_CODEC_CR83_ANCFIRUPDATE_NOT_RESETTED
+} t_ab8500_codec_cr83_ancfirupdate;
+
+/* CR84 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr84_ancinshift;
+
+/* CR85 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr85_ancfiroutshift;
+
+/* CR86 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr86_ancshiftout;
+
+/* CR87 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr87_ancfircoeff_msb;
+
+/* CR88 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr88_ancfircoeff_lsb;
+
+/* CR89 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr89_anciircoeff_msb;
+
+/* CR90 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr90_anciircoeff_lsb;
+
+/* CR91 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr91_ancwarpdel_msb;
+
+/* CR92 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr92_ancwarpdel_lsb;
+
+/* CR93 - Read Only */
+/* CR93 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr93_ancfirpeak_msb;
+
+/* CR94 - Read Only */
+/* CR94 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr94_ancfirpeak_lsb;
+
+/* CR95 - Read Only */
+/* CR95 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr95_anciirpeak_msb;
+
+/* CR96 - Read Only */
+/* CR96 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr96_anciirpeak_lsb;
+
+/* CR97 - 7 */
+typedef enum {
+	AB8500_CODEC_CR97_STFIR_SET_LAST_NOT_APPLIED,
+	AB8500_CODEC_CR97_STFIR_SET_LAST_APPLIED
+} t_ab8500_codec_cr97_stfir_set;
+
+/* CR97 - 6:0 */
+typedef t_uint8 t_ab8500_codec_cr97_stfir_addr;
+
+/* CR98 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr98_stfir_coeff_msb;
+
+/* CR99 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr99_stfir_coeff_lsb;
+
+/* CR100 - 2 */
+typedef enum {
+	AB8500_CODEC_CR100_ENSTFIRS_DISABLED,
+	AB8500_CODEC_CR100_ENSTFIRS_ENABLED
+} t_ab8500_codec_cr100_enstfirs;
+
+/* CR100 - 1 */
+typedef enum {
+	AB8500_CODEC_CR100_STFIRSTOIF1_AUD_IF0_DATA_RATE,
+	AB8500_CODEC_CR100_STFIRSTOIF1_AUD_IF1_DATA_RATE
+} t_ab8500_codec_cr100_stfirstoif1;
+
+/* CR100 - 0 */
+typedef enum {
+	AB8500_CODEC_CR100_STFIR_BUSY_READY,
+	AB8500_CODEC_CR100_STFIR_BUSY_NOT_READY
+} t_ab8500_codec_cr100_stfir_busy;
+
+/* CR101 - 7 */
+typedef enum {
+	AB8500_CODEC_CR101_PARLHF_INDEPENDENT,
+	AB8500_CODEC_CR101_PARLHF_BRIDGED
+} t_ab8500_codec_cr101_parlhf;
+
+/* CR101 - 6 */
+typedef enum {
+	AB8500_CODEC_CR101_PARLVIB_INDEPENDENT,
+	AB8500_CODEC_CR101_PARLVIB_BRIDGED
+} t_ab8500_codec_cr101_parlvib;
+
+/* CR101 - 3 */
+typedef enum {
+	AB8500_CODEC_CR101_CLASSD_VIBLSWAPEN_DISABLED,
+	AB8500_CODEC_CR101_CLASSD_VIBLSWAPEN_ENABLED
+} t_ab8500_codec_cr101_classd_viblswapen;
+
+/* CR101 - 2 */
+typedef enum {
+	AB8500_CODEC_CR101_CLASSD_VIBRSWAPEN_DISABLED,
+	AB8500_CODEC_CR101_CLASSD_VIBRSWAPEN_ENABLED
+} t_ab8500_codec_cr101_classd_vibrswapen;
+
+/* CR101 - 1 */
+typedef enum {
+	AB8500_CODEC_CR101_CLASSD_HFLSWAPEN_DISABLED,
+	AB8500_CODEC_CR101_CLASSD_HFLSWAPEN_ENABLED
+} t_ab8500_codec_cr101_classd_hflswapen;
+
+/* CR101 - 0 */
+typedef enum {
+	AB8500_CODEC_CR101_CLASSD_HFRSWAPEN_DISABLED,
+	AB8500_CODEC_CR101_CLASSD_HFRSWAPEN_ENABLED
+} t_ab8500_codec_cr101_classd_hfrswapen;
+
+/* CR102 - 7:4 */
+typedef enum {
+	AB8500_CODEC_CR102_CLASSD_FIRBYP_ALL_ENABLED = 0,
+	AB8500_CODEC_CR102_CLASSD_FIRBYP_HFL_BYPASSED = 1,
+	AB8500_CODEC_CR102_CLASSD_FIRBYP_HFR_BYPASSED = 2,
+	AB8500_CODEC_CR102_CLASSD_FIRBYP_VIBL_BYPASSED = 4,
+	AB8500_CODEC_CR102_CLASSD_FIRBYP_VIBR_BYPASSED = 8
+} t_ab8500_codec_cr102_classd_firbyp;
+
+/* CR102 - 3:0 */
+typedef enum {
+	AB8500_CODEC_CR102_CLASSD_HIGHVOLEN_DISABLED = 0,
+	AB8500_CODEC_CR102_CLASSD_HIGHVOLEN_HFL_HIGHVOL = 1,
+	AB8500_CODEC_CR102_CLASSD_HIGHVOLEN_HFR_HIGHVOL = 2,
+	AB8500_CODEC_CR102_CLASSD_HIGHVOLEN_VIBL_HIGHVOL = 4,
+	AB8500_CODEC_CR102_CLASSD_HIGHVOLEN_VIBR_HIGHVOL = 8
+} t_ab8500_codec_cr102_classd_highvolen;
+
+/* CR103 - 7:4 */
+typedef t_uint8 t_ab8500_codec_cr103_classd_ditherhpgain;
+
+/* CR103 - 3:0 */
+typedef t_uint8 t_ab8500_codec_cr103_classd_ditherwgain;
+
+/* CR104 - 5:0 */
+/* In ab8500_codec.h */
+
+/* CR105 - 7:0 */
+/* In ab8500_codec.h */
+
+/* CR106 - 6:4 */
+/* In ab8500_codec.h */
+
+/* CR106 - 2 */
+/* In ab8500_codec.h */
+
+/* CR106 - 1 */
+/* In ab8500_codec.h */
+
+/* CR106 - 0 */
+/* In ab8500_codec.h */
+
+/* CR107 - 7:0 */
+/* In ab8500_codec.h */
+
+/* CR108 - 7:0 */
+/* In ab8500_codec.h */
+
+/* CR109 - Read Only */
+/* CR109 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr109_bfifosamples;
+
+typedef enum {
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT1,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT2,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT3,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT4,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT5,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT6,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT7,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT8,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_ZEROS,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_IS_TRISTATE = 15,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_UNDEFINED
+} t_ab8500_codec_cr31_to_cr46_ad_data_allocation;
+
+typedef enum {
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT00,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT01,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT02,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT03,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT04,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT05,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT06,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT07,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT08,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT09,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT10,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT11,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT12,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT13,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT14,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT15,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT16,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT17,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT18,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT19,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT20,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT21,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT22,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT23,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT24,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT25,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT26,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT27,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT28,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT29,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT30,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT31,
+	AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT_UNDEFINED
+} t_ab8500_codec_cr51_to_cr56_sltoda;
+
+/* CR104 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr104_bfifoint;
+
+/* CR105 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr105_bfifotx;
+
+/* CR106 - 6:4 */
+typedef enum {
+	AB8500_CODEC_CR106_BFIFOFSEXT_NO_EXTRA_CLK,
+	AB8500_CODEC_CR106_BFIFOFSEXT_1SLOT_EXTRA_CLK,
+	AB8500_CODEC_CR106_BFIFOFSEXT_2SLOT_EXTRA_CLK,
+	AB8500_CODEC_CR106_BFIFOFSEXT_3SLOT_EXTRA_CLK,
+	AB8500_CODEC_CR106_BFIFOFSEXT_4SLOT_EXTRA_CLK,
+	AB8500_CODEC_CR106_BFIFOFSEXT_5SLOT_EXTRA_CLK,
+	AB8500_CODEC_CR106_BFIFOFSEXT_6SLOT_EXTRA_CLK
+} t_ab8500_codec_cr106_bfifofsext;
+
+/* CR106 - 2 */
+typedef enum {
+	AB8500_CODEC_CR106_BFIFOMSK_AD_DATA0_UNMASKED,
+	AB8500_CODEC_CR106_BFIFOMSK_AD_DATA0_MASKED
+} t_ab8500_codec_cr106_bfifomsk;
+
+/* CR106 - 1 */
+typedef enum {
+	AB8500_CODEC_CR106_BFIFOMSTR_SLAVE_MODE,
+	AB8500_CODEC_CR106_BFIFOMSTR_MASTER_MODE
+} t_ab8500_codec_cr106_bfifomstr;
+
+/* CR106 - 0 */
+typedef enum {
+	AB8500_CODEC_CR106_BFIFOSTRT_STOPPED,
+	AB8500_CODEC_CR106_BFIFOSTRT_RUNNING
+} t_ab8500_codec_cr106_bfifostrt;
+
+/* CR107 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr107_bfifosampnr;
+
+/* CR108 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr108_bfifowakeup;
+
+/*configuration structure for AB8500 Codec*/
+typedef struct {
+	/* CR0  */
+	t_ab8500_codec_cr0_powerup cr0_powerup;
+	t_ab8500_codec_cr0_enaana cr0_enaana;
+
+	/* CR1  */
+	t_ab8500_codec_cr1_swreset cr1_swreset;
+
+	/* CR2  */
+	t_ab8500_codec_cr2_enad1 cr2_enad1;
+	t_ab8500_codec_cr2_enad2 cr2_enad2;
+	t_ab8500_codec_cr2_enad3 cr2_enad3;
+	t_ab8500_codec_cr2_enad4 cr2_enad4;
+	t_ab8500_codec_cr2_enad5 cr2_enad5;
+	t_ab8500_codec_cr2_enad6 cr2_enad6;
+
+	/* CR3  */
+	t_ab8500_codec_cr3_enda1 cr3_enda1;
+	t_ab8500_codec_cr3_enda2 cr3_enda2;
+	t_ab8500_codec_cr3_enda3 cr3_enda3;
+	t_ab8500_codec_cr3_enda4 cr3_enda4;
+	t_ab8500_codec_cr3_enda5 cr3_enda5;
+	t_ab8500_codec_cr3_enda6 cr3_enda6;
+
+	/* CR4  */
+	t_ab8500_codec_cr4_lowpowhs cr4_lowpowhs;
+	t_ab8500_codec_cr4_lowpowdachs cr4_lowpowdachs;
+	t_ab8500_codec_cr4_lowpowear cr4_lowpowear;
+	t_ab8500_codec_cr4_ear_sel_cm cr4_ear_sel_cm;
+	t_ab8500_codec_cr4_hs_hp_dis cr4_hs_hp_dis;
+	t_ab8500_codec_cr4_ear_hp_dis cr4_ear_hp_dis;
+
+	/* CR5  */
+	t_ab8500_codec_cr5_enmic1 cr5_enmic1;
+	t_ab8500_codec_cr5_enmic2 cr5_enmic2;
+	t_ab8500_codec_cr5_enlinl cr5_enlinl;
+	t_ab8500_codec_cr5_enlinr cr5_enlinr;
+	t_ab8500_codec_cr5_mutmic1 cr5_mutmic1;
+	t_ab8500_codec_cr5_mutmic2 cr5_mutmic2;
+	t_ab8500_codec_cr5_mutlinl cr5_mutlinl;
+	t_ab8500_codec_cr5_mutlinr cr5_mutlinr;
+
+	/* CR6  */
+	t_ab8500_codec_cr6_endmic1 cr6_endmic1;
+	t_ab8500_codec_cr6_endmic2 cr6_endmic2;
+	t_ab8500_codec_cr6_endmic3 cr6_endmic3;
+	t_ab8500_codec_cr6_endmic4 cr6_endmic4;
+	t_ab8500_codec_cr6_endmic5 cr6_endmic5;
+	t_ab8500_codec_cr6_endmic6 cr6_endmic6;
+
+	/* CR7  */
+	t_ab8500_codec_cr7_mic1sel cr7_mic1sel;
+	t_ab8500_codec_cr7_linrsel cr7_linrsel;
+	t_ab8500_codec_cr7_endrvhsl cr7_endrvhsl;
+	t_ab8500_codec_cr7_endrvhsr cr7_endrvhsr;
+	t_ab8500_codec_cr7_enadcmic cr7_enadcmic;
+	t_ab8500_codec_cr7_enadclinl cr7_enadclinl;
+	t_ab8500_codec_cr7_enadclinr cr7_enadclinr;
+
+	/* CR8  */
+	t_ab8500_codec_cr8_cp_dis_pldwn cr8_cp_dis_pldwn;
+	t_ab8500_codec_cr8_enear cr8_enear;
+	t_ab8500_codec_cr8_enhsl cr8_enhsl;
+	t_ab8500_codec_cr8_enhsr cr8_enhsr;
+	t_ab8500_codec_cr8_enhfl cr8_enhfl;
+	t_ab8500_codec_cr8_enhfr cr8_enhfr;
+	t_ab8500_codec_cr8_envibl cr8_envibl;
+	t_ab8500_codec_cr8_envibr cr8_envibr;
+
+	/* CR9  */
+	t_ab8500_codec_cr9_endacear cr9_endacear;
+	t_ab8500_codec_cr9_endachsl cr9_endachsl;
+	t_ab8500_codec_cr9_endachsr cr9_endachsr;
+	t_ab8500_codec_cr9_endachfl cr9_endachfl;
+	t_ab8500_codec_cr9_endachfr cr9_endachfr;
+	t_ab8500_codec_cr9_endacvibl cr9_endacvibl;
+	t_ab8500_codec_cr9_endacvibr cr9_endacvibr;
+
+	/* CR10  */
+	t_ab8500_codec_cr10_muteear cr10_muteear;
+	t_ab8500_codec_cr10_mutehsl cr10_mutehsl;
+	t_ab8500_codec_cr10_mutehsr cr10_mutehsr;
+	t_ab8500_codec_cr10_mutehfl cr10_mutehfl;
+	t_ab8500_codec_cr10_mutehfr cr10_mutehfr;
+	t_ab8500_codec_cr10_mutevibl cr10_mutevibl;
+	t_ab8500_codec_cr10_mutevibr cr10_mutevibr;
+
+	/* CR11  */
+	t_ab8500_codec_cr11_earshortpwd cr11_earshortpwd;
+	t_ab8500_codec_cr11_earshortdis cr11_earshortdis;
+	t_ab8500_codec_cr11_hslshortdis cr11_hslshortdis;
+	t_ab8500_codec_cr11_hsrshortdis cr11_hsrshortdis;
+	t_ab8500_codec_cr11_hflshortdis cr11_hflshortdis;
+	t_ab8500_codec_cr11_hfrshortdis cr11_hfrshortdis;
+	t_ab8500_codec_cr11_viblshortdis cr11_viblshortdis;
+	t_ab8500_codec_cr11_vibrshortdis cr11_vibrshortdis;
+
+	/* CR12  */
+	t_ab8500_codec_cr12_encphs cr12_encphs;
+	t_ab8500_codec_cr12_hsautotime cr12_hsautotime;
+	t_ab8500_codec_cr12_hsautoensel cr12_hsautoensel;
+	t_ab8500_codec_cr12_hsautoen cr12_hsautoen;
+
+	/* CR13  */
+	t_ab8500_codec_cr13_envdet_hthresh cr13_envdet_hthresh;
+	t_ab8500_codec_cr13_envdet_lthresh cr13_envdet_lthresh;
+
+	/* CR14  */
+	t_ab8500_codec_cr14_smpslven cr14_smpslven;
+	t_ab8500_codec_cr14_envdetsmpsen cr14_envdetsmpsen;
+	t_ab8500_codec_cr14_cplven cr14_cplven;
+	t_ab8500_codec_cr14_envdetcpen cr14_envdetcpen;
+	t_ab8500_codec_cr14_envet_time cr14_envet_time;
+
+	/* CR15  */
+	t_ab8500_codec_cr15_pwmtovibl cr15_pwmtovibl;
+	t_ab8500_codec_cr15_pwmtovibr cr15_pwmtovibr;
+	t_ab8500_codec_cr15_pwmlctrl cr15_pwmlctrl;
+	t_ab8500_codec_cr15_pwmrctrl cr15_pwmrctrl;
+	t_ab8500_codec_cr15_pwmnlctrl cr15_pwmnlctrl;
+	t_ab8500_codec_cr15_pwmplctrl cr15_pwmplctrl;
+	t_ab8500_codec_cr15_pwmnrctrl cr15_pwmnrctrl;
+	t_ab8500_codec_cr15_pwmprctrl cr15_pwmprctrl;
+
+	/* CR16  */
+	t_ab8500_codec_cr16_pwmnlpol cr16_pwmnlpol;
+	t_ab8500_codec_cr16_pwmnldutycycle cr16_pwmnldutycycle;
+
+	/* CR17  */
+	t_ab8500_codec_cr17_pwmplpol cr17_pwmplpol;
+	t_ab8500_codec_cr17_pwmpldutycycle cr17_pwmpldutycycle;
+
+	/* CR18  */
+	t_ab8500_codec_cr18_pwmnrpol cr18_pwmnrpol;
+	t_ab8500_codec_cr18_pwmnrdutycycle cr18_pwmnrdutycycle;
+
+	/* CR19  */
+	t_ab8500_codec_cr19_pwmprpol cr19_pwmprpol;
+	t_ab8500_codec_cr19_pwmprdutycycle cr19_pwmprdutycycle;
+
+	/* CR20  */
+	t_ab8500_codec_cr20_en_se_mic1 cr20_en_se_mic1;
+	t_ab8500_codec_cr20_mic1_gain cr20_mic1_gain;
+
+	/* CR21  */
+	t_ab8500_codec_cr21_en_se_mic2 cr21_en_se_mic2;
+	t_ab8500_codec_cr21_mic2_gain cr21_mic2_gain;
+
+	/* CR22  */
+	t_ab8500_codec_cr22_hsl_gain cr22_hsl_gain;
+	t_ab8500_codec_cr22_linl_gain cr22_linl_gain;
+
+	/* CR23  */
+	t_ab8500_codec_cr23_hsr_gain cr23_hsr_gain;
+	t_ab8500_codec_cr23_linr_gain cr23_linr_gain;
+
+	/* CR24  */
+	t_ab8500_codec_cr24_lintohsl_gain cr24_lintohsl_gain;
+
+	/* CR25  */
+	t_ab8500_codec_cr25_lintohsr_gain cr25_lintohsr_gain;
+
+	/* CR26  */
+	t_ab8500_codec_cr26_ad1nh cr26_ad1nh;
+	t_ab8500_codec_cr26_ad2nh cr26_ad2nh;
+	t_ab8500_codec_cr26_ad3nh cr26_ad3nh;
+	t_ab8500_codec_cr26_ad4nh cr26_ad4nh;
+	t_ab8500_codec_cr26_ad1_voice cr26_ad1_voice;
+	t_ab8500_codec_cr26_ad2_voice cr26_ad2_voice;
+	t_ab8500_codec_cr26_ad3_voice cr26_ad3_voice;
+	t_ab8500_codec_cr26_ad4_voice cr26_ad4_voice;
+
+	/* CR27  */
+	t_ab8500_codec_cr27_en_mastgen cr27_en_mastgen;
+	t_ab8500_codec_cr27_if1_bitclk_osr cr27_if1_bitclk_osr;
+	t_ab8500_codec_cr27_enfs_bitclk1 cr27_enfs_bitclk1;
+	t_ab8500_codec_cr27_if0_bitclk_osr cr27_if0_bitclk_osr;
+	t_ab8500_codec_cr27_enfs_bitclk0 cr27_enfs_bitclk0;
+
+	/* CR28  */
+	t_ab8500_codec_cr28_fsync0p cr28_fsync0p;
+	t_ab8500_codec_cr28_bitclk0p cr28_bitclk0p;
+	t_ab8500_codec_cr28_if0del cr28_if0del;
+	t_ab8500_codec_cr28_if0format cr28_if0format;
+	t_ab8500_codec_cr28_if0wl cr28_if0wl;
+
+	/* CR29  */
+	t_ab8500_codec_cr29_if0datoif1ad cr29_if0datoif1ad;
+	t_ab8500_codec_cr29_if0cktoif1ck cr29_if0cktoif1ck;
+	t_ab8500_codec_cr29_if1master cr29_if1master;
+	t_ab8500_codec_cr29_if1datoif0ad cr29_if1datoif0ad;
+	t_ab8500_codec_cr29_if1cktoif0ck cr29_if1cktoif0ck;
+	t_ab8500_codec_cr29_if0master cr29_if0master;
+	t_ab8500_codec_cr29_if0bfifoen cr29_if0bfifoen;
+
+	/* CR30  */
+	t_ab8500_codec_cr30_fsync1p cr30_fsync1p;
+	t_ab8500_codec_cr30_bitclk1p cr30_bitclk1p;
+	t_ab8500_codec_cr30_if1del cr30_if1del;
+	t_ab8500_codec_cr30_if1format cr30_if1format;
+	t_ab8500_codec_cr30_if1wl cr30_if1wl;
+
+	/* CR31  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr31_adotoslot1;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr31_adotoslot0;
+
+	/* CR32  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr32_adotoslot3;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr32_adotoslot2;
+
+	/* CR33  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr33_adotoslot5;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr33_adotoslot4;
+
+	/* CR34  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr34_adotoslot7;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr34_adotoslot6;
+
+	/* CR35  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr35_adotoslot9;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr35_adotoslot8;
+
+	/* CR36  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr36_adotoslot11;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr36_adotoslot10;
+
+	/* CR37  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr37_adotoslot13;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr37_adotoslot12;
+
+	/* CR38  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr38_adotoslot15;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr38_adotoslot14;
+
+	/* CR39  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr39_adotoslot17;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr39_adotoslot16;
+
+	/* CR40  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr40_adotoslot19;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr40_adotoslot18;
+
+	/* CR41  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr41_adotoslot21;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr41_adotoslot20;
+
+	/* CR42  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr42_adotoslot23;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr42_adotoslot22;
+
+	/* CR43  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr43_adotoslot25;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr43_adotoslot24;
+
+	/* CR44  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr44_adotoslot27;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr44_adotoslot26;
+
+	/* CR45  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr45_adotoslot29;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr45_adotoslot28;
+
+	/* CR46  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr46_adotoslot31;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr46_adotoslot30;
+
+	/* CR47  */
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl7;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl6;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl5;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl4;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl3;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl2;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl1;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl0;
+
+	/* CR48  */
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl15;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl14;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl13;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl12;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl11;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl10;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl9;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl8;
+
+	/* CR49  */
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl23;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl22;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl21;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl20;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl19;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl18;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl17;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl16;
+
+	/* CR50  */
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl31;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl30;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl29;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl28;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl27;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl26;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl25;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl24;
+
+	/* CR51  */
+	t_ab8500_codec_cr51_da12_voice cr51_da12_voice;
+	t_ab8500_codec_cr51_sldai1toslado1 cr51_sldai1toslado1;
+	t_ab8500_codec_cr51_to_cr56_sltoda cr51_sltoda1;
+
+	/* CR52  */
+	t_ab8500_codec_cr52_sldai2toslado2 cr52_sldai2toslado2;
+	t_ab8500_codec_cr51_to_cr56_sltoda cr52_sltoda2;
+
+	/* CR53  */
+	t_ab8500_codec_cr53_da34_voice cr53_da34_voice;
+	t_ab8500_codec_cr53_sldai3toslado3 cr53_sldai3toslado3;
+	t_ab8500_codec_cr51_to_cr56_sltoda cr53_sltoda3;
+
+	/* CR54  */
+	t_ab8500_codec_cr54_sldai4toslado4 cr54_sldai4toslado4;
+	t_ab8500_codec_cr51_to_cr56_sltoda cr54_sltoda4;
+
+	/* CR55  */
+	t_ab8500_codec_cr55_da56_voice cr55_da56_voice;
+	t_ab8500_codec_cr55_sldai5toslado5 cr55_sldai5toslado5;
+	t_ab8500_codec_cr51_to_cr56_sltoda cr55_sltoda5;
+
+	/* CR56  */
+	t_ab8500_codec_cr56_sldai6toslado7 cr56_sldai6toslado7;
+	t_ab8500_codec_cr51_to_cr56_sltoda cr56_sltoda6;
+
+	/* CR57  */
+	t_ab8500_codec_cr57_bfifull_msk cr57_bfifull_msk;
+	t_ab8500_codec_cr57_bfiempt_msk cr57_bfiempt_msk;
+	t_ab8500_codec_cr57_dachan_msk cr57_dachan_msk;
+	t_ab8500_codec_cr57_gain_msk cr57_gain_msk;
+	t_ab8500_codec_cr57_dspad_msk cr57_dspad_msk;
+	t_ab8500_codec_cr57_dspda_msk cr57_dspda_msk;
+	t_ab8500_codec_cr57_stfir_msk cr57_stfir_msk;
+
+	/* CR58  */
+	t_ab8500_codec_cr58_bfifull_ev cr58_bfifull_ev;
+	t_ab8500_codec_cr58_bfiempt_ev cr58_bfiempt_ev;
+	t_ab8500_codec_cr58_dachan_ev cr58_dachan_ev;
+	t_ab8500_codec_cr58_gain_ev cr58_gain_ev;
+	t_ab8500_codec_cr58_dspad_ev cr58_dspad_ev;
+	t_ab8500_codec_cr58_dspda_ev cr58_dspda_ev;
+	t_ab8500_codec_cr58_stfir_ev cr58_stfir_ev;
+
+	/* CR59  */
+	t_ab8500_codec_cr59_vssready_msk cr59_vssready_msk;
+	t_ab8500_codec_cr59_shrtvibl_msk cr59_shrtvibl_msk;
+	t_ab8500_codec_cr59_shrtvibr_msk cr59_shrtvibr_msk;
+	t_ab8500_codec_cr59_shrthfl_msk cr59_shrthfl_msk;
+	t_ab8500_codec_cr59_shrthfr_msk cr59_shrthfr_msk;
+	t_ab8500_codec_cr59_shrthsl_msk cr59_shrthsl_msk;
+	t_ab8500_codec_cr59_shrthsr_msk cr59_shrthsr_msk;
+	t_ab8500_codec_cr59_shrtear_msk cr59_shrtear_msk;
+
+	/* CR60  */
+	t_ab8500_codec_cr60_vssready_ev cr60_vssready_ev;
+	t_ab8500_codec_cr60_shrtvibl_ev cr60_shrtvibl_ev;
+	t_ab8500_codec_cr60_shrtvibr_ev cr60_shrtvibr_ev;
+	t_ab8500_codec_cr60_shrthfl_ev cr60_shrthfl_ev;
+	t_ab8500_codec_cr60_shrthfr_ev cr60_shrthfr_ev;
+	t_ab8500_codec_cr60_shrthsl_ev cr60_shrthsl_ev;
+	t_ab8500_codec_cr60_shrthsr_ev cr60_shrthsr_ev;
+	t_ab8500_codec_cr60_shrtear_ev cr60_shrtear_ev;
+
+	/* CR61  */
+	t_ab8500_codec_cr61_revision cr61_revision;
+	t_ab8500_codec_cr61_fade_speed cr61_fade_speed;
+
+	/* CR62  */
+	t_ab8500_codec_cr62_dmic1sinc3 cr62_dmic1sinc3;
+	t_ab8500_codec_cr62_dmic2sinc3 cr62_dmic2sinc3;
+	t_ab8500_codec_cr62_dmic3sinc3 cr62_dmic3sinc3;
+	t_ab8500_codec_cr62_dmic4sinc3 cr62_dmic4sinc3;
+	t_ab8500_codec_cr62_dmic5sinc3 cr62_dmic5sinc3;
+	t_ab8500_codec_cr62_dmic6sinc3 cr62_dmic6sinc3;
+
+	/* CR63  */
+	t_ab8500_codec_cr63_datohslen cr63_datohslen;
+	t_ab8500_codec_cr63_datohsren cr63_datohsren;
+	t_ab8500_codec_cr63_ad1sel cr63_ad1sel;
+	t_ab8500_codec_cr63_ad2sel cr63_ad2sel;
+	t_ab8500_codec_cr63_ad3sel cr63_ad3sel;
+	t_ab8500_codec_cr63_ad5sel cr63_ad5sel;
+	t_ab8500_codec_cr63_ad6sel cr63_ad6sel;
+	t_ab8500_codec_cr63_ancsel cr63_ancsel;
+
+	/* CR64  */
+	t_ab8500_codec_cr64_datohfren cr64_datohfren;
+	t_ab8500_codec_cr64_datohflen cr64_datohflen;
+	t_ab8500_codec_cr64_hfrsel cr64_hfrsel;
+	t_ab8500_codec_cr64_hflsel cr64_hflsel;
+	t_ab8500_codec_cr64_stfir1sel cr64_stfir1sel;
+	t_ab8500_codec_cr64_stfir2sel cr64_stfir2sel;
+
+	/* CR65  */
+	t_ab8500_codec_cr65_fadedis_ad1 cr65_fadedis_ad1;
+	t_ab8500_codec_cr65_ad1gain cr65_ad1gain;
+
+	/* CR66  */
+	t_ab8500_codec_cr66_fadedis_ad2 cr66_fadedis_ad2;
+	t_ab8500_codec_cr66_ad2gain cr66_ad2gain;
+
+	/* CR67  */
+	t_ab8500_codec_cr67_fadedis_ad3 cr67_fadedis_ad3;
+	t_ab8500_codec_cr67_ad3gain cr67_ad3gain;
+
+	/* CR68  */
+	t_ab8500_codec_cr68_fadedis_ad4 cr68_fadedis_ad4;
+	t_ab8500_codec_cr68_ad4gain cr68_ad4gain;
+
+	/* CR69  */
+	t_ab8500_codec_cr69_fadedis_ad5 cr69_fadedis_ad5;
+	t_ab8500_codec_cr69_ad5gain cr69_ad5gain;
+
+	/* CR70  */
+	t_ab8500_codec_cr70_fadedis_ad6 cr70_fadedis_ad6;
+	t_ab8500_codec_cr70_ad6gain cr70_ad6gain;
+
+	/* CR71  */
+	t_ab8500_codec_cr71_fadedis_da1 cr71_fadedis_da1;
+	t_ab8500_codec_cr71_da1gain cr71_da1gain;
+
+	/* CR72  */
+	t_ab8500_codec_cr72_fadedis_da2 cr72_fadedis_da2;
+	t_ab8500_codec_cr72_da2gain cr72_da2gain;
+
+	/* CR73  */
+	t_ab8500_codec_cr73_fadedis_da3 cr73_fadedis_da3;
+	t_ab8500_codec_cr73_da3gain cr73_da3gain;
+
+	/* CR74  */
+	t_ab8500_codec_cr74_fadedis_da4 cr74_fadedis_da4;
+	t_ab8500_codec_cr74_da4gain cr74_da4gain;
+
+	/* CR75  */
+	t_ab8500_codec_cr75_fadedis_da5 cr75_fadedis_da5;
+	t_ab8500_codec_cr75_da5gain cr75_da5gain;
+
+	/* CR76  */
+	t_ab8500_codec_cr76_fadedis_da6 cr76_fadedis_da6;
+	t_ab8500_codec_cr76_da6gain cr76_da6gain;
+
+	/* CR77  */
+	t_ab8500_codec_cr77_fadedis_ad1l cr77_fadedis_ad1l;
+	t_ab8500_codec_cr77_ad1lbgain_to_hfl cr77_ad1lbgain_to_hfl;
+
+	/* CR78  */
+	t_ab8500_codec_cr78_fadedis_ad2l cr78_fadedis_ad2l;
+	t_ab8500_codec_cr78_ad2lbgain_to_hfr cr78_ad2lbgain_to_hfr;
+
+	/* CR79  */
+	t_ab8500_codec_cr79_hssinc1 cr79_hssinc1;
+	t_ab8500_codec_cr79_fadedis_hsl cr79_fadedis_hsl;
+	t_ab8500_codec_cr79_hsldgain cr79_hsldgain;
+
+	/* CR80  */
+	t_ab8500_codec_cr80_fadedis_hsr cr80_fadedis_hsr;
+	t_ab8500_codec_cr80_hsrdgain cr80_hsrdgain;
+
+	/* CR81  */
+	t_ab8500_codec_cr81_stfir1gain cr81_stfir1gain;
+
+	/* CR82  */
+	t_ab8500_codec_cr82_stfir2gain cr82_stfir2gain;
+
+	/* CR83  */
+	t_ab8500_codec_cr83_enanc cr83_enanc;
+	t_ab8500_codec_cr83_anciirinit cr83_anciirinit;
+	t_ab8500_codec_cr83_ancfirupdate cr83_ancfirupdate;
+
+	/* CR84  */
+	t_ab8500_codec_cr84_ancinshift cr84_ancinshift;
+
+	/* CR85  */
+	t_ab8500_codec_cr85_ancfiroutshift cr85_ancfiroutshift;
+
+	/* CR86  */
+	t_ab8500_codec_cr86_ancshiftout cr86_ancshiftout;
+
+	/* CR87  */
+	t_ab8500_codec_cr87_ancfircoeff_msb cr87_ancfircoeff_msb;
+
+	/* CR88  */
+	t_ab8500_codec_cr88_ancfircoeff_lsb cr88_ancfircoeff_lsb;
+
+	/* CR89  */
+	t_ab8500_codec_cr89_anciircoeff_msb cr89_anciircoeff_msb;
+
+	/* CR90  */
+	t_ab8500_codec_cr90_anciircoeff_lsb cr90_anciircoeff_lsb;
+
+	/* CR91  */
+	t_ab8500_codec_cr91_ancwarpdel_msb cr91_ancwarpdel_msb;
+
+	/* CR92  */
+	t_ab8500_codec_cr92_ancwarpdel_lsb cr92_ancwarpdel_lsb;
+
+	/* CR93  */
+	t_ab8500_codec_cr93_ancfirpeak_msb cr93_ancfirpeak_msb;
+
+	/* CR94  */
+	t_ab8500_codec_cr94_ancfirpeak_lsb cr94_ancfirpeak_lsb;
+
+	/* CR95  */
+	t_ab8500_codec_cr95_anciirpeak_msb cr95_anciirpeak_msb;
+
+	/* CR96  */
+	t_ab8500_codec_cr96_anciirpeak_lsb cr96_anciirpeak_lsb;
+
+	/* CR97  */
+	t_ab8500_codec_cr97_stfir_set cr97_stfir_set;
+	t_ab8500_codec_cr97_stfir_addr cr97_stfir_addr;
+
+	/* CR98  */
+	t_ab8500_codec_cr98_stfir_coeff_msb cr98_stfir_coeff_msb;
+
+	/* CR99  */
+	t_ab8500_codec_cr99_stfir_coeff_lsb cr99_stfir_coeff_lsb;
+
+	/* CR100  */
+	t_ab8500_codec_cr100_enstfirs cr100_enstfirs;
+	t_ab8500_codec_cr100_stfirstoif1 cr100_stfirstoif1;
+	t_ab8500_codec_cr100_stfir_busy cr100_stfir_busy;
+
+	/* CR101  */
+	t_ab8500_codec_cr101_parlhf cr101_parlhf;
+	t_ab8500_codec_cr101_parlvib cr101_parlvib;
+	t_ab8500_codec_cr101_classd_viblswapen cr101_classd_viblswapen;
+	t_ab8500_codec_cr101_classd_vibrswapen cr101_classd_vibrswapen;
+	t_ab8500_codec_cr101_classd_hflswapen cr101_classd_hflswapen;
+	t_ab8500_codec_cr101_classd_hfrswapen cr101_classd_hfrswapen;
+
+	/* CR102  */
+	t_ab8500_codec_cr102_classd_firbyp cr102_classd_firbyp;
+	t_ab8500_codec_cr102_classd_highvolen cr102_classd_highvolen;
+
+	/* CR103  */
+	t_ab8500_codec_cr103_classd_ditherhpgain cr103_classd_ditherhpgain;
+	t_ab8500_codec_cr103_classd_ditherwgain cr103_classd_ditherwgain;
+
+	/* CR104  */
+	t_ab8500_codec_cr104_bfifoint cr104_bfifoint;
+
+	/* CR105  */
+	t_ab8500_codec_cr105_bfifotx cr105_bfifotx;
+
+	/* CR106  */
+	t_ab8500_codec_cr106_bfifofsext cr106_bfifofsext;
+	t_ab8500_codec_cr106_bfifomsk cr106_bfifomsk;
+	t_ab8500_codec_cr106_bfifomstr cr106_bfifomstr;
+	t_ab8500_codec_cr106_bfifostrt cr106_bfifostrt;
+
+	/* CR107  */
+	t_ab8500_codec_cr107_bfifosampnr cr107_bfifosampnr;
+
+	/* CR108  */
+	t_ab8500_codec_cr108_bfifowakeup cr108_bfifowakeup;
+
+	/* CR109  */
+	t_ab8500_codec_cr109_bfifosamples cr109_bfifosamples;
+
+} t_ab8500_codec_configuration;
+
+typedef enum {
+	AB8500_CODEC_DIRECTION_IN,
+	AB8500_CODEC_DIRECTION_OUT,
+	AB8500_CODEC_DIRECTION_INOUT
+} t_ab8500_codec_direction;
+
+typedef enum {
+	AB8500_CODEC_MODE_HIFI,
+	AB8500_CODEC_MODE_VOICE,
+	AB8500_CODEC_MODE_MANUAL_SETTING
+} t_ab8500_codec_mode;
+
+typedef enum {
+	AB8500_CODEC_AUDIO_INTERFACE_0,
+	AB8500_CODEC_AUDIO_INTERFACE_1
+} t_ab8500_codec_audio_interface;
+
+typedef enum {
+	AB8500_CODEC_SRC_LINEIN,
+	AB8500_CODEC_SRC_MICROPHONE_1A,
+	AB8500_CODEC_SRC_MICROPHONE_1B,
+	AB8500_CODEC_SRC_MICROPHONE_2,
+	AB8500_CODEC_SRC_D_MICROPHONE_1,
+	AB8500_CODEC_SRC_D_MICROPHONE_2,
+	AB8500_CODEC_SRC_D_MICROPHONE_3,
+	AB8500_CODEC_SRC_D_MICROPHONE_4,
+	AB8500_CODEC_SRC_D_MICROPHONE_5,
+	AB8500_CODEC_SRC_D_MICROPHONE_6,
+	AB8500_CODEC_SRC_FM_RX,
+	AB8500_CODEC_SRC_ALL
+} t_ab8500_codec_src;
+
+typedef enum {
+	AB8500_CODEC_DEST_HEADSET,
+	AB8500_CODEC_DEST_EARPIECE,
+	AB8500_CODEC_DEST_HANDSFREE,
+	AB8500_CODEC_DEST_VIBRATOR_L,
+	AB8500_CODEC_DEST_VIBRATOR_R,
+	AB8500_CODEC_DEST_ALL
+} t_ab8500_codec_dest;
+
+typedef struct {
+	t_uint8 slave_address_of_ab8500_codec;
+	t_ab8500_codec_direction ab8500_codec_direction;
+	t_ab8500_codec_mode ab8500_codec_mode_in;
+	t_ab8500_codec_mode ab8500_codec_mode_out;
+	t_ab8500_codec_audio_interface audio_interface;
+	t_ab8500_codec_src ab8500_codec_src;
+	t_ab8500_codec_dest ab8500_codec_dest;
+	t_uint8 in_left_volume;
+	t_uint8 in_right_volume;
+	t_uint8 out_left_volume;
+	t_uint8 out_right_volume;
+
+	t_ab8500_codec_configuration ab8500_codec_configuration;
+} t_ab8500_codec_system_context;
+#endif /* _AB8500_CODECP_H_ */
+
+/* End of file AB8500_CODECP.h */
diff --git a/arch/arm/mach-ux500/include/mach/ab8500_codec_p_v1_0.h b/arch/arm/mach-ux500/include/mach/ab8500_codec_p_v1_0.h
new file mode 100644
index 00000000000..866cd0c80f1
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/ab8500_codec_p_v1_0.h
@@ -0,0 +1,3037 @@
+/*****************************************************************************/
+/**
+*  © ST-Ericsson, 2009 - All rights reserved
+*  Reproduction and Communication of this document is strictly prohibited
+*  unless specifically authorized in writing by ST-Ericsson
+*
+* \brief   Private Header file for AB8500 CODEC
+* \author  ST-Ericsson
+*/
+/*****************************************************************************/
+
+#ifndef _AB8500_CODECP_V1_0_H_
+#define _AB8500_CODECP_V1_0_H_
+
+/*----------------------------------------------------------------------------
+ * Includes
+ *---------------------------------------------------------------------------*/
+#include "hcl_defs.h"
+
+#define AB8500_CODEC_HCL_VERSION_ID                    3
+#define AB8500_CODEC_HCL_MAJOR_ID                      0
+#define AB8500_CODEC_HCL_MINOR_ID                      0
+
+#define AB8500_CODEC_MASK_ONE_BIT                      0x1UL
+#define AB8500_CODEC_MASK_TWO_BITS                     0x3UL
+#define AB8500_CODEC_MASK_THREE_BITS                   0x7UL
+#define AB8500_CODEC_MASK_FOUR_BITS                    0xFUL
+#define AB8500_CODEC_MASK_FIVE_BITS                    0x1FUL
+#define AB8500_CODEC_MASK_SIX_BITS                     0x3FUL
+#define AB8500_CODEC_MASK_SEVEN_BITS                   0x7FUL
+#define AB8500_CODEC_MASK_EIGHT_BITS                   0xFFUL
+
+#define AB8500_CODEC_WRITE_BITS(reg, val, bit_nb, pos) (reg) = ((t_uint8) ((((reg) & (~(bit_nb << pos))) | (((val) & bit_nb) << pos))))
+
+#define AB8500_CODEC_BLOCK 0x0D
+
+#define AB8500_CODEC_MASK_TWO_MS_BITS                     0xC0UL
+#define AB8500_CODEC_MASK_SIX_LS_BITS                     0x3FUL
+
+/*  Genepi AudioCodec Control Registers */
+
+#define AB8500_CODEC_CR0    0x00
+#define AB8500_CODEC_CR1    0x01
+#define AB8500_CODEC_CR2    0x02
+#define AB8500_CODEC_CR3    0x03
+#define AB8500_CODEC_CR4    0x04
+#define AB8500_CODEC_CR5    0x05
+#define AB8500_CODEC_CR6    0x06
+#define AB8500_CODEC_CR7    0x07
+#define AB8500_CODEC_CR8    0x08
+#define AB8500_CODEC_CR9    0x09
+#define AB8500_CODEC_CR10   0x0A
+#define AB8500_CODEC_CR11   0x0B
+#define AB8500_CODEC_CR12   0x0C
+#define AB8500_CODEC_CR13   0x0D
+#define AB8500_CODEC_CR14   0x0E
+#define AB8500_CODEC_CR15   0x0F
+#define AB8500_CODEC_CR16   0x10
+#define AB8500_CODEC_CR17   0x11
+#define AB8500_CODEC_CR18   0x12
+#define AB8500_CODEC_CR19   0x13
+#define AB8500_CODEC_CR20   0x14
+#define AB8500_CODEC_CR21   0x15
+#define AB8500_CODEC_CR22   0x16
+#define AB8500_CODEC_CR23   0x17
+#define AB8500_CODEC_CR24   0x18
+#define AB8500_CODEC_CR25   0x19
+#define AB8500_CODEC_CR26   0x1A
+#define AB8500_CODEC_CR27   0x1B
+#define AB8500_CODEC_CR28   0x1C
+#define AB8500_CODEC_CR29   0x1D
+#define AB8500_CODEC_CR30   0x1E
+#define AB8500_CODEC_CR31   0x1F
+#define AB8500_CODEC_CR32   0x20
+#define AB8500_CODEC_CR33   0x21
+#define AB8500_CODEC_CR34   0x22
+#define AB8500_CODEC_CR35   0x23
+#define AB8500_CODEC_CR36   0x24
+#define AB8500_CODEC_CR37   0x25
+#define AB8500_CODEC_CR38   0x26
+#define AB8500_CODEC_CR39   0x27
+#define AB8500_CODEC_CR40   0x28
+#define AB8500_CODEC_CR41   0x29
+#define AB8500_CODEC_CR42   0x2A
+#define AB8500_CODEC_CR43   0x2B
+#define AB8500_CODEC_CR44   0x2C
+#define AB8500_CODEC_CR45   0x2D
+#define AB8500_CODEC_CR46   0x2E
+#define AB8500_CODEC_CR47   0x2F
+#define AB8500_CODEC_CR48   0x30
+#define AB8500_CODEC_CR49   0x31
+#define AB8500_CODEC_CR50   0x32
+#define AB8500_CODEC_CR51   0x33
+#define AB8500_CODEC_CR52   0x34
+#define AB8500_CODEC_CR53   0x35
+#define AB8500_CODEC_CR54   0x36
+#define AB8500_CODEC_CR55   0x37
+#define AB8500_CODEC_CR56   0x38
+#define AB8500_CODEC_CR57   0x39
+#define AB8500_CODEC_CR58   0x3A
+#define AB8500_CODEC_CR59   0x3B
+#define AB8500_CODEC_CR60   0x3C
+#define AB8500_CODEC_CR61   0x3D
+#define AB8500_CODEC_CR62   0x3E
+#define AB8500_CODEC_CR63   0x3F
+#define AB8500_CODEC_CR64   0x40
+#define AB8500_CODEC_CR65   0x41
+#define AB8500_CODEC_CR66   0x42
+#define AB8500_CODEC_CR67   0x43
+#define AB8500_CODEC_CR68   0x44
+#define AB8500_CODEC_CR69   0x45
+#define AB8500_CODEC_CR70   0x46
+#define AB8500_CODEC_CR71   0x47
+#define AB8500_CODEC_CR72   0x48
+#define AB8500_CODEC_CR73   0x49
+#define AB8500_CODEC_CR74   0x4A
+#define AB8500_CODEC_CR75   0x4B
+#define AB8500_CODEC_CR76   0x4C
+#define AB8500_CODEC_CR77   0x4D
+#define AB8500_CODEC_CR78   0x4E
+#define AB8500_CODEC_CR79   0x4F
+#define AB8500_CODEC_CR80   0x50
+#define AB8500_CODEC_CR81   0x51
+#define AB8500_CODEC_CR82   0x52
+#define AB8500_CODEC_CR83   0x53
+#define AB8500_CODEC_CR84   0x54
+#define AB8500_CODEC_CR85   0x55
+#define AB8500_CODEC_CR86   0x56
+#define AB8500_CODEC_CR87   0x57
+#define AB8500_CODEC_CR88   0x58
+#define AB8500_CODEC_CR89   0x59
+#define AB8500_CODEC_CR90   0x5A
+#define AB8500_CODEC_CR91   0x5B
+#define AB8500_CODEC_CR92   0x5C
+#define AB8500_CODEC_CR93   0x5D
+#define AB8500_CODEC_CR94   0x5E
+#define AB8500_CODEC_CR95   0x5F
+#define AB8500_CODEC_CR96   0x60
+#define AB8500_CODEC_CR97   0x61
+#define AB8500_CODEC_CR98   0x62
+#define AB8500_CODEC_CR99   0x63
+#define AB8500_CODEC_CR100  0x64
+#define AB8500_CODEC_CR101  0x65
+#define AB8500_CODEC_CR102  0x66
+#define AB8500_CODEC_CR103  0x67
+#define AB8500_CODEC_CR104  0x68
+#define AB8500_CODEC_CR105  0x69
+#define AB8500_CODEC_CR106  0x6A
+#define AB8500_CODEC_CR107  0x6B
+#define AB8500_CODEC_CR108  0x6C
+#define AB8500_CODEC_CR109  0x6D
+#define AB8500_CODEC_CR110  0x6E
+#define AB8500_CODEC_CR111  0x6F
+
+/* CR0-CR0x0000 */
+#define AB8500_CODEC_CR0_POWERUP   7
+#define AB8500_CODEC_CR0_ENAANA    3
+
+/* CR1-CR0x0001 */
+#define AB8500_CODEC_CR1_SWRESET   7
+
+/* CR2-CR0x0002 */
+#define AB8500_CODEC_CR2_ENAD1   7
+#define AB8500_CODEC_CR2_ENAD2   6
+#define AB8500_CODEC_CR2_ENAD3   5
+#define AB8500_CODEC_CR2_ENAD4   4
+#define AB8500_CODEC_CR2_ENAD5   3
+#define AB8500_CODEC_CR2_ENAD6   2
+
+/* CR3-CR0x0003 */
+#define AB8500_CODEC_CR3_ENDA1   7
+#define AB8500_CODEC_CR3_ENDA2   6
+#define AB8500_CODEC_CR3_ENDA3   5
+#define AB8500_CODEC_CR3_ENDA4   4
+#define AB8500_CODEC_CR3_ENDA5   3
+#define AB8500_CODEC_CR3_ENDA6   2
+
+/* CR4-CR0x0004 */
+#define AB8500_CODEC_CR4_LOWPOWHS      7
+#define AB8500_CODEC_CR4_LOWPOWDACHS   5
+#define AB8500_CODEC_CR4_LOWPOWEAR     4
+#define AB8500_CODEC_CR4_EAR_SEL_CM    2
+#define AB8500_CODEC_CR4_HS_HP_EN      1
+
+/* CR5-CR0x0005 */
+#define AB8500_CODEC_CR5_ENMIC1     7
+#define AB8500_CODEC_CR5_ENMIC2     6
+#define AB8500_CODEC_CR5_ENLINL     5
+#define AB8500_CODEC_CR5_ENLINR     4
+#define AB8500_CODEC_CR5_MUTMIC1    3
+#define AB8500_CODEC_CR5_MUTMIC2    2
+#define AB8500_CODEC_CR5_MUTELINL   1
+#define AB8500_CODEC_CR5_MUTELINR   0
+
+/* CR6-CR0x0006 */
+#define AB8500_CODEC_CR6_ENDMIC1      7
+#define AB8500_CODEC_CR6_ENDMIC2      6
+#define AB8500_CODEC_CR6_ENDMIC3      5
+#define AB8500_CODEC_CR6_ENDMIC4      4
+#define AB8500_CODEC_CR6_ENDMIC5      3
+#define AB8500_CODEC_CR6_ENDMIC6      2
+
+/* CR7-CR0x0007 */
+#define AB8500_CODEC_CR7_MIC1SEL     7
+#define AB8500_CODEC_CR7_LINRSEL     6
+#define AB8500_CODEC_CR7_ENDRVHSL    5
+#define AB8500_CODEC_CR7_ENDRVHSR    4
+#define AB8500_CODEC_CR7_ENADCMIC    2
+#define AB8500_CODEC_CR7_ENADCLINL   1
+#define AB8500_CODEC_CR7_ENADCLINR   0
+
+/* CR8-CR0x0008 */
+#define AB8500_CODEC_CR8_CP_DIS_PLDWN   7
+#define AB8500_CODEC_CR8_ENEAR          6
+#define AB8500_CODEC_CR8_ENHSL          5
+#define AB8500_CODEC_CR8_ENHSR          4
+#define AB8500_CODEC_CR8_ENHFL          3
+#define AB8500_CODEC_CR8_ENHFR          2
+#define AB8500_CODEC_CR8_ENVIBL         1
+#define AB8500_CODEC_CR8_ENVIBR         0
+
+/* CR9-CR0x0009 */
+#define AB8500_CODEC_CR9_ENADACEAR     6
+#define AB8500_CODEC_CR9_ENADACHSL     5
+#define AB8500_CODEC_CR9_ENADACHSR     4
+#define AB8500_CODEC_CR9_ENADACHFL     3
+#define AB8500_CODEC_CR9_ENADACHFR     2
+#define AB8500_CODEC_CR9_ENADACVIBL    1
+#define AB8500_CODEC_CR9_ENADACVIBR    0
+
+/* CR10-CR0x000A */
+#define AB8500_CODEC_CR10_MUTEEAR     6
+#define AB8500_CODEC_CR10_MUTEHSL     5
+#define AB8500_CODEC_CR10_MUTEHSR     4
+
+/* CR11-CR0x000B */
+#define AB8500_CODEC_CR11_ENSHORTPWD      7
+#define AB8500_CODEC_CR11_EARSHORTDIS     6
+#define AB8500_CODEC_CR11_HSSHORTDIS      5
+#define AB8500_CODEC_CR11_HSPULLDEN       4
+#define AB8500_CODEC_CR11_HSOSCEN         2
+#define AB8500_CODEC_CR11_HSFADEN         1
+#define AB8500_CODEC_CR11_HSZCDDIS        0
+
+/* CR12-CR0x000C */
+#define AB8500_CODEC_CR12_ENCPHS        7
+#define AB8500_CODEC_CR12_HSAUTOEN      0
+
+/* CR13-CR0x000D */
+#define AB8500_CODEC_CR13_ENVDET_HTHRESH   4
+#define AB8500_CODEC_CR13_ENVDET_LTHRESH   0
+
+/* CR14-CR0x000E */
+#define AB8500_CODEC_CR14_SMPSLVEN       7
+#define AB8500_CODEC_CR14_ENVDETSMPSEN   6
+#define AB8500_CODEC_CR14_CPLVEN         5
+#define AB8500_CODEC_CR14_ENVDETCPEN     4
+#define AB8500_CODEC_CR14_ENVDET_TIME    0
+
+/* CR15-CR0x000F */
+#define AB8500_CODEC_CR15_PWMTOVIBL   7
+#define AB8500_CODEC_CR15_PWMTOVIBR   6
+#define AB8500_CODEC_CR15_PWMLCTRL    5
+#define AB8500_CODEC_CR15_PWMRCTRL    4
+#define AB8500_CODEC_CR15_PWMNLCTRL   3
+#define AB8500_CODEC_CR15_PWMPLCTRL   2
+#define AB8500_CODEC_CR15_PWMNRCTRL   1
+#define AB8500_CODEC_CR15_PWMPRCTRL   0
+
+/* CR16-CR0x0010 */
+#define AB8500_CODEC_CR16_PWMNLPOL         7
+#define AB8500_CODEC_CR16_PWMNLDUTYCYCLE   0
+
+/* CR17-CR0x0011 */
+#define AB8500_CODEC_CR17_PWMPLPOL         7
+#define AB8500_CODEC_CR17_PWMLPDUTYCYCLE   0
+
+/* CR18-CR0x0012 */
+#define AB8500_CODEC_CR18_PWMNRPOL         7
+#define AB8500_CODEC_CR18_PWMNRDUTYCYCLE   0
+
+/* CR19-CR0x0013 */
+#define AB8500_CODEC_CR19_PWMPRPOL         7
+#define AB8500_CODEC_CR19_PWMRPDUTYCYCLE   0
+
+/* CR20-CR0x0014 */
+#define AB8500_CODEC_CR20_EN_SE_MIC1     7
+#define AB8500_CODEC_CR20_LOW_POW_MIC1   6
+#define AB8500_CODEC_CR20_MIC1_GAIN      0
+
+/* CR21-CR0x0015 */
+#define AB8500_CODEC_CR21_EN_SE_MIC2     7
+#define AB8500_CODEC_CR21_LOW_POW_MIC2   6
+#define AB8500_CODEC_CR21_MIC2_GAIN      0
+
+/* CR22-CR0x0016 */
+#define AB8500_CODEC_CR22_HSL_GAIN    4
+#define AB8500_CODEC_CR22_HSR_GAIN    0
+
+/* CR23-CR0x0017 */
+#define AB8500_CODEC_CR23_LINL_GAIN   4
+#define AB8500_CODEC_CR23_LINR_GAIN   0
+
+/* CR24-CR0x0018 */
+#define AB8500_CODEC_CR24_LINTOHSL_GAIN   0
+
+/* CR25-CR0x0019 */
+#define AB8500_CODEC_CR25_LINTOHSR_GAIN   0
+
+/* CR26-CR0x001A */
+#define AB8500_CODEC_CR26_AD1NH       7
+#define AB8500_CODEC_CR26_AD2NH       6
+#define AB8500_CODEC_CR26_AD3NH       5
+#define AB8500_CODEC_CR26_AD4NH       4
+#define AB8500_CODEC_CR26_AD1_VOICE   3
+#define AB8500_CODEC_CR26_AD2_VOICE   2
+#define AB8500_CODEC_CR26_AD3_VOICE   1
+#define AB8500_CODEC_CR26_AD4_VOICE   0
+
+/* CR27-CR0x001B */
+#define AB8500_CODEC_CR27_EN_MASTGEN       7
+#define AB8500_CODEC_CR27_IF1_BITCLK_OSR   5
+#define AB8500_CODEC_CR27_ENFS_BITCLK1     4
+#define AB8500_CODEC_CR27_IF0_BITCLK_OSR   1
+#define AB8500_CODEC_CR27_ENFS_BITCLK0     0
+
+/* CR28-CR0x001C */
+#define AB8500_CODEC_CR28_FSYNC0P     6
+#define AB8500_CODEC_CR28_BITCLK0P    5
+#define AB8500_CODEC_CR28_IF0DEL      4
+#define AB8500_CODEC_CR28_IF0FORMAT   2
+#define AB8500_CODEC_CR28_IF0WL       0
+
+/* CR29-CR0x001D */
+#define AB8500_CODEC_CR29_IF0DATOIF1AD   7
+#define AB8500_CODEC_CR29_IF0CKTOIF1CK   6
+#define AB8500_CODEC_CR29_IF1MASTER      5
+#define AB8500_CODEC_CR29_IF1DATOIF0AD   3
+#define AB8500_CODEC_CR29_IF1CKTOIF0CK   2
+#define AB8500_CODEC_CR29_IF0MASTER      1
+#define AB8500_CODEC_CR29_IF0BFIFOEN     0
+
+/* CR30-CR0x001E */
+#define AB8500_CODEC_CR30_FSYNC1P     6
+#define AB8500_CODEC_CR30_BITCLK1P    5
+#define AB8500_CODEC_CR30_IF1DEL      4
+#define AB8500_CODEC_CR30_IF1FORMAT   2
+#define AB8500_CODEC_CR30_IF1WL       0
+
+/* CR31-CR0x001F */
+#define AB8500_CODEC_CR31_ADOTOSLOT1   4
+#define AB8500_CODEC_CR31_ADOTOSLOT0   0
+
+/* CR32-CR0x0020 */
+#define AB8500_CODEC_CR32_ADOTOSLOT3   4
+#define AB8500_CODEC_CR32_ADOTOSLOT2   0
+
+/* CR33-CR0x0021 */
+#define AB8500_CODEC_CR33_ADOTOSLOT5   4
+#define AB8500_CODEC_CR33_ADOTOSLOT4   0
+
+/* CR34-CR0x0022 */
+#define AB8500_CODEC_CR34_ADOTOSLOT7   4
+#define AB8500_CODEC_CR34_ADOTOSLOT6   0
+
+/* CR35-CR0x0023 */
+#define AB8500_CODEC_CR35_ADOTOSLOT9   4
+#define AB8500_CODEC_CR35_ADOTOSLOT8   0
+
+/* CR36-CR0x0024 */
+#define AB8500_CODEC_CR36_ADOTOSLOT11   4
+#define AB8500_CODEC_CR36_ADOTOSLOT10   0
+
+/* CR37-CR0x0025 */
+#define AB8500_CODEC_CR37_ADOTOSLOT13   4
+#define AB8500_CODEC_CR37_ADOTOSLOT12   0
+
+/* CR38-CR0x0026 */
+#define AB8500_CODEC_CR38_ADOTOSLOT15   4
+#define AB8500_CODEC_CR38_ADOTOSLOT14   0
+
+/* CR39-CR0x0027 */
+#define AB8500_CODEC_CR39_ADOTOSLOT17   4
+#define AB8500_CODEC_CR39_ADOTOSLOT16   0
+
+/* CR40-CR0x0028 */
+#define AB8500_CODEC_CR40_ADOTOSLOT19   4
+#define AB8500_CODEC_CR40_ADOTOSLOT18   0
+
+/* CR41-CR0x0029 */
+#define AB8500_CODEC_CR41_ADOTOSLOT21   4
+#define AB8500_CODEC_CR41_ADOTOSLOT20   0
+
+/* CR42-CR0x002A */
+#define AB8500_CODEC_CR42_ADOTOSLOT23   4
+#define AB8500_CODEC_CR42_ADOTOSLOT22   0
+
+/* CR43-CR0x002B */
+#define AB8500_CODEC_CR43_ADOTOSLOT25   4
+#define AB8500_CODEC_CR43_ADOTOSLOT24   0
+
+/* CR44-CR0x002C */
+#define AB8500_CODEC_CR44_ADOTOSLOT27   4
+#define AB8500_CODEC_CR44_ADOTOSLOT26   0
+
+/* CR45-CR0x002D */
+#define AB8500_CODEC_CR45_ADOTOSLOT29   4
+#define AB8500_CODEC_CR45_ADOTOSLOT28   0
+
+/* CR46-CR0x002E */
+#define AB8500_CODEC_CR46_ADOTOSLOT31   4
+#define AB8500_CODEC_CR46_ADOTOSLOT30   0
+
+/* CR47-CR0x002F */
+#define AB8500_CODEC_CR47_HIZ_SL7   7
+#define AB8500_CODEC_CR47_HIZ_SL6   6
+#define AB8500_CODEC_CR47_HIZ_SL5   5
+#define AB8500_CODEC_CR47_HIZ_SL4   4
+#define AB8500_CODEC_CR47_HIZ_SL3   3
+#define AB8500_CODEC_CR47_HIZ_SL2   2
+#define AB8500_CODEC_CR47_HIZ_SL1   1
+#define AB8500_CODEC_CR47_HIZ_SL0   0
+
+/* CR48-CR0x0030 */
+#define AB8500_CODEC_CR48_HIZ_SL15   7
+#define AB8500_CODEC_CR48_HIZ_SL14   6
+#define AB8500_CODEC_CR48_HIZ_SL13   5
+#define AB8500_CODEC_CR48_HIZ_SL12   4
+#define AB8500_CODEC_CR48_HIZ_SL11   3
+#define AB8500_CODEC_CR48_HIZ_SL10   2
+#define AB8500_CODEC_CR48_HIZ_SL9    1
+#define AB8500_CODEC_CR48_HIZ_SL8    0
+
+/* CR49-CR0x0031 */
+#define AB8500_CODEC_CR49_HIZ_SL23   7
+#define AB8500_CODEC_CR49_HIZ_SL22   6
+#define AB8500_CODEC_CR49_HIZ_SL21   5
+#define AB8500_CODEC_CR49_HIZ_SL20   4
+#define AB8500_CODEC_CR49_HIZ_SL19   3
+#define AB8500_CODEC_CR49_HIZ_SL18   2
+#define AB8500_CODEC_CR49_HIZ_SL17   1
+#define AB8500_CODEC_CR49_HIZ_SL16   0
+
+/* CR50-CR0x0032 */
+#define AB8500_CODEC_CR50_HIZ_SL31   7
+#define AB8500_CODEC_CR50_HIZ_SL30   6
+#define AB8500_CODEC_CR50_HIZ_SL29   5
+#define AB8500_CODEC_CR50_HIZ_SL28   4
+#define AB8500_CODEC_CR50_HIZ_SL27   3
+#define AB8500_CODEC_CR50_HIZ_SL26   2
+#define AB8500_CODEC_CR50_HIZ_SL25   1
+#define AB8500_CODEC_CR50_HIZ_SL24   0
+
+/* CR51-CR0x0033 */
+#define AB8500_CODEC_CR51_DA12_VOICE       7
+#define AB8500_CODEC_CR51_SWAP_DA12_34     6
+#define AB8500_CODEC_CR51_SLDAI7TOSLADO1   5
+#define AB8500_CODEC_CR51_SLTODA1          0
+
+/* CR52-CR0x0034 */
+#define AB8500_CODEC_CR52_SLDAI8TOSLADO2   5
+#define AB8500_CODEC_CR52_SLTODA2          0
+
+/* CR53-CR0x0035 */
+#define AB8500_CODEC_CR53_DA34_VOICE       7
+#define AB8500_CODEC_CR53_SLDAI7TOSLADO3   5
+#define AB8500_CODEC_CR53_SLTODA3          0
+
+/* CR54-CR0x0036 */
+#define AB8500_CODEC_CR54_SLDAI8TOSLADO4   5
+#define AB8500_CODEC_CR54_SLTODA4          0
+
+/* CR55-CR0x0037 */
+#define AB8500_CODEC_CR55_DA56_VOICE       7
+#define AB8500_CODEC_CR55_SLDAI7TOSLADO5   5
+#define AB8500_CODEC_CR55_SLTODA5          0
+
+/* CR56-CR0x0038 */
+#define AB8500_CODEC_CR56_SLDAI8TOSLADO6   5
+#define AB8500_CODEC_CR56_SLTODA6          0
+
+/* CR57-CR0x0039 */
+#define AB8500_CODEC_CR57_SLDAI8TOSLADO7   5
+#define AB8500_CODEC_CR57_SLTODA7          0
+
+/* CR58-CR0x003A */
+#define AB8500_CODEC_CR58_SLDAI7TOSLADO8   5
+#define AB8500_CODEC_CR58_SLTODA8          0
+
+/* CR59-CR0x003B */
+#define AB8500_CODEC_CR59_PARLHF               7
+#define AB8500_CODEC_CR59_PARLVIB              6
+#define AB8500_CODEC_CR59_CLASSDVIB1SWAPEN     3
+#define AB8500_CODEC_CR59_CLASSDVIB2SWAPEN     2
+#define AB8500_CODEC_CR59_CLASSDHFLSWAPEN      1
+#define AB8500_CODEC_CR59_CLASSDHFRSWAPEN      0
+
+/* CR60-CR0x003C */
+#define AB8500_CODEC_CR60_CLASSD_FIR_BYP      4
+#define AB8500_CODEC_CR60_CLASSD_HIGHVOL_EN   0
+
+/* CR61-CR0x003D */
+#define AB8500_CODEC_CR61_CLASSD_DITH_HPGAIN   4
+#define AB8500_CODEC_CR61_CLASSD_DITH_WGAIN    0
+
+/* CR62-CR0x003E */
+#define AB8500_CODEC_CR62_DMIC1SINC3   5
+#define AB8500_CODEC_CR62_DMIC2SINC3   4
+#define AB8500_CODEC_CR62_DMIC3SINC3   3
+#define AB8500_CODEC_CR62_DMIC4SINC3   2
+#define AB8500_CODEC_CR62_DMIC5SINC3   1
+#define AB8500_CODEC_CR62_DMIC6SINC3   0
+
+/* CR63-CR0x003F */
+#define AB8500_CODEC_CR63_DATOHSLEN   7
+#define AB8500_CODEC_CR63_DATOHSREN   6
+#define AB8500_CODEC_CR63_AD1SEL      5
+#define AB8500_CODEC_CR63_AD2SEL      4
+#define AB8500_CODEC_CR63_AD3SEL      3
+#define AB8500_CODEC_CR63_AD5SEL      2
+#define AB8500_CODEC_CR63_AD6SEL      1
+#define AB8500_CODEC_CR63_ANCSEL      0
+
+/* CR64-CR0x0040 */
+#define AB8500_CODEC_CR64_DATOHFREN   7
+#define AB8500_CODEC_CR64_DATOHFLEN   6
+#define AB8500_CODEC_CR64_HFRSEL      5
+#define AB8500_CODEC_CR64_HFLSEL      4
+#define AB8500_CODEC_CR64_STFIR1SEL   2
+#define AB8500_CODEC_CR64_STFIR2SEL   0
+
+/* CR65-CR0x0041 */
+#define AB8500_CODEC_CR65_FADEDIS_AD1   6
+#define AB8500_CODEC_CR65_AD1GAIN       0
+
+/* CR66-CR0x0042 */
+#define AB8500_CODEC_CR66_FADEDIS_AD2   6
+#define AB8500_CODEC_CR66_AD2GAIN       0
+
+/* CR67-CR0x0043 */
+#define AB8500_CODEC_CR67_FADEDIS_AD3   6
+#define AB8500_CODEC_CR67_AD3GAIN       0
+
+/* CR68-CR0x0044 */
+#define AB8500_CODEC_CR68_FADEDIS_AD4   6
+#define AB8500_CODEC_CR68_AD4GAIN       0
+
+/* CR69-CR0x0045 */
+#define AB8500_CODEC_CR69_FADEDIS_AD5   6
+#define AB8500_CODEC_CR69_AD5GAIN       0
+
+/* CR70-CR0x0046 */
+#define AB8500_CODEC_CR70_FADEDIS_AD6   6
+#define AB8500_CODEC_CR70_AD6GAIN       0
+
+/* CR71-CR0x0047 */
+#define AB8500_CODEC_CR71_FADEDIS_DA1   6
+#define AB8500_CODEC_CR71_DA1GAIN       0
+
+/* CR72-CR0x0048 */
+#define AB8500_CODEC_CR72_FADEDIS_DA2   6
+#define AB8500_CODEC_CR72_DA2GAIN       0
+
+/* CR73-CR0x0049 */
+#define AB8500_CODEC_CR73_FADEDIS_DA3   6
+#define AB8500_CODEC_CR73_DA3GAIN       0
+
+/* CR74-CR0x004A */
+#define AB8500_CODEC_CR74_FADEDIS_DA4   6
+#define AB8500_CODEC_CR74_DA4GAIN       0
+
+/* CR75-CR0x004B */
+#define AB8500_CODEC_CR75_FADEDIS_DA5   6
+#define AB8500_CODEC_CR75_DA5GAIN       0
+
+/* CR76-CR0x004C */
+#define AB8500_CODEC_CR76_FADEDIS_DA6   6
+#define AB8500_CODEC_CR76_DA6GAIN       0
+
+/* CR77-CR0x004D */
+#define AB8500_CODEC_CR77_FADEDIS_AD1L   6
+#define AB8500_CODEC_CR77_AD1LBGAIN      0
+
+/* CR78-CR0x004E */
+#define AB8500_CODEC_CR78_FADEDIS_AD2L   6
+#define AB8500_CODEC_CR78_AD2LBGAIN      0
+
+/* CR79-CR0x004F */
+#define AB8500_CODEC_CR79_HSSINC1       7
+#define AB8500_CODEC_CR79_FADEDIS_HSL   4
+#define AB8500_CODEC_CR79_HSLDGAIN      0
+
+/* CR80-CR0x0050 */
+#define AB8500_CODEC_CR80_FADE_SPEED    6
+#define AB8500_CODEC_CR80_FADEDIS_HSR   4
+#define AB8500_CODEC_CR80_HSRDGAIN      0
+
+/* CR81-CR0x0051 */
+#define AB8500_CODEC_CR81_STFIR1GAIN   0
+
+/* CR82-CR0x0052 */
+#define AB8500_CODEC_CR82_STFIR2GAIN   0
+
+/* CR83-CR0x0053 */
+#define AB8500_CODEC_CR83_ENANC          2
+#define AB8500_CODEC_CR83_ANCIIRINIT     1
+#define AB8500_CODEC_CR83_ANCFIRUPDATE   0
+
+/* CR84-CR0x0054 */
+#define AB8500_CODEC_CR84_ANCINSHIFT   0
+
+/* CR85-CR0x0055 */
+#define AB8500_CODEC_CR85_ANCFIROUTSHIFT   0
+
+/* CR86-CR0x0056 */
+#define AB8500_CODEC_CR86_ANCSHIFTOUT   0
+
+/* CR87-CR0x0057 */
+#define AB8500_CODEC_CR87_ANCFIRCOEFF_MSB   0
+
+/* CR88-CR0x0058 */
+#define AB8500_CODEC_CR88_ANCFIRCOEFF_LSB   0
+
+/* CR89-CR0x0059 */
+#define AB8500_CODEC_CR89_ANCIIRCOEFF_MSB   0
+
+/* CR90-CR0x005A */
+#define AB8500_CODEC_CR90_ANCIIRCOEFF_LSB   0
+
+/* CR91-CR0x005B */
+#define AB8500_CODEC_CR91_ANCWARPDEL_MSB   0
+
+/* CR92-CR0x005C */
+#define AB8500_CODEC_CR92_ANCWARPDEL_LSB   0
+
+/* CR93-CR0x005D */
+#define AB8500_CODEC_CR93_ANCFIRPEAK_MSB   0
+
+/* CR94-CR0x005E */
+#define AB8500_CODEC_CR94_ANCFIRPEAK_LSB   0
+
+/* CR95-CR0x005F */
+#define AB8500_CODEC_CR95_ANCIIRPEAK_MSB   0
+
+/* CR96-CR0x0060 */
+#define AB8500_CODEC_CR96_ANCIIRPEAK_LSB   0
+
+/* CR97-CR0x0061 */
+#define AB8500_CODEC_CR97_STFIR_SET    7
+#define AB8500_CODEC_CR97_STFIR_ADDR   0
+
+/* CR98-CR0x0062 */
+#define AB8500_CODEC_CR98_STFIR_COEFF_MSB   0
+
+/* CR99-CR0x0063 */
+#define AB8500_CODEC_CR99_STFIR_COEFF_LSB   0
+
+/* CR100-CR0x0064 */
+#define AB8500_CODEC_CR100_ENSTFIRS      2
+#define AB8500_CODEC_CR100_STFIRSTOIF1   1
+#define AB8500_CODEC_CR100_STFIR_BUSY    0
+
+/* CR101-CR0x0065 */
+#define AB8500_CODEC_CR101_HSOFFSTMASK      7
+#define AB8500_CODEC_CR101_FIFOFULLMASK     6
+#define AB8500_CODEC_CR101_FIFOEMPTYMASK    5
+#define AB8500_CODEC_CR101_DASATMASK        4
+#define AB8500_CODEC_CR101_ADSATMASK        3
+#define AB8500_CODEC_CR101_ADDSPMASK        2
+#define AB8500_CODEC_CR101_DADSPMASK        1
+#define AB8500_CODEC_CR101_FIRSIDMASK       0
+
+/* CR102-CR0x0066 */
+#define AB8500_CODEC_CR102_IT_HSOFFST     7
+#define AB8500_CODEC_CR102_IT_FIFOFULL    6
+#define AB8500_CODEC_CR102_IT_FIFOEMPTY   5
+#define AB8500_CODEC_CR102_IT_DASAT       4
+#define AB8500_CODEC_CR102_IT_ADSAT       3
+#define AB8500_CODEC_CR102_IT_ADDSP       2
+#define AB8500_CODEC_CR102_IT_DADSP       1
+#define AB8500_CODEC_CR102_IT_FIRSID      0
+
+/* CR103-CR0x0067 */
+#define AB8500_CODEC_CR103_VSSREADYMASK   7
+#define AB8500_CODEC_CR103_SHORTHSLMASK   2
+#define AB8500_CODEC_CR103_SHORTHSRMASK   1
+#define AB8500_CODEC_CR103_SHORTEARMASK   0
+
+/* CR104-CR0x0068 */
+#define AB8500_CODEC_CR104_IT_VSSREADY   7
+#define AB8500_CODEC_CR104_IT_SHORTHSL   2
+#define AB8500_CODEC_CR104_IT_SHORTHSR   1
+#define AB8500_CODEC_CR104_IT_SHORTEAR   0
+
+/* CR105-CR0x0069 */
+#define AB8500_CODEC_CR105_BFIFOMASK   7
+#define AB8500_CODEC_CR105_BFIFOINT    0
+
+/* CR106-CR0x006A */
+#define AB8500_CODEC_CR106_BFIFOTX    0
+
+/* CR107-CR0x006B */
+#define AB8500_CODEC_CR107_BFIFOEXSL    5
+#define AB8500_CODEC_CR107_PREBITCLK0   2
+#define AB8500_CODEC_CR107_BFIFOMAST    1
+#define AB8500_CODEC_CR107_BFIFORUN     0
+
+/* CR108-CR0x006C */
+#define AB8500_CODEC_CR108_BFIFOFRAMESW  0
+
+/* CR109-CR0x006D */
+#define AB8500_CODEC_CR109_BFIFOWAKEUP   0
+
+/* CR110-CR0x006E */
+#define AB8500_CODEC_CR110_BFIFOSAMPLE   0
+
+/* CR111-CR0x006F */
+#define AB8500_CODEC_CR111_AUD_IP_REV    0
+
+/* For SetVolume API*/
+#define AB8500_CODEC_MAX_VOLUME      100
+
+/* Analog MIC1 & MIC2 */
+#define AB8500_CODEC_MIC_VOLUME_MAX      31
+#define AB8500_CODEC_MIC_VOLUME_MEDIUM   15
+#define AB8500_CODEC_MIC_VOLUME_MIN      0
+
+/* Line-in */
+#define AB8500_CODEC_LINEIN_VOLUME_MAX     15
+#define AB8500_CODEC_LINEIN_VOLUME_MEDIUM  7
+#define AB8500_CODEC_LINEIN_VOLUME_MIN     0
+
+/* HeadSet */
+#define AB8500_CODEC_HEADSET_VOLUME_MAX     13
+#define AB8500_CODEC_HEADSET_VOLUME_MEDIUM  6
+#define AB8500_CODEC_HEADSET_VOLUME_MIN     0
+
+/* HeadSet Digital */
+#define AB8500_CODEC_HEADSET_D_VOLUME_MAX     0
+#define AB8500_CODEC_HEADSET_D_VOLUME_MEDIUM  7
+#define AB8500_CODEC_HEADSET_D_VOLUME_MIN     15
+#define AB8500_CODEC_HEADSET_D_VOLUME_0DB     8
+
+/* Digital AD Path */
+#define AB8500_CODEC_AD_D_VOLUME_MAX     0
+#define AB8500_CODEC_AD_D_VOLUME_MEDIUM  31
+#define AB8500_CODEC_AD_D_VOLUME_MIN     63
+
+/* Digital DA Path */
+#define AB8500_CODEC_DA_D_VOLUME_MAX     0
+#define AB8500_CODEC_DA_D_VOLUME_MEDIUM  31
+#define AB8500_CODEC_DA_D_VOLUME_MIN     63
+
+/* EarPiece Digital */
+#define AB8500_CODEC_EARPIECE_D_VOLUME_MAX     0
+#define AB8500_CODEC_EARPIECE_D_VOLUME_MEDIUM  7
+#define AB8500_CODEC_EARPIECE_D_VOLUME_MIN     15
+
+/* AD1 loopback to HFL & HFR Digital */
+#define AB8500_CODEC_AD_LB_TO_HF_L_R_VOLUME_MAX     0
+#define AB8500_CODEC_AD_LB_TO_HF_L_R_VOLUME_MEDIUM  31
+#define AB8500_CODEC_AD_LB_TO_HF_L_R_VOLUME_MIN     63
+
+/* Line-in to HSL & HSR */
+#define AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MAX     0
+#define AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MEDIUM  9
+#define AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MIN     18
+#define AB8500_CODEC_LINEIN_TO_HS_L_R_LOOP_OPEN      19
+
+/* Vibrator */
+#define AB8500_CODEC_VIBRATOR_VOLUME_MAX     100
+#define AB8500_CODEC_VIBRATOR_VOLUME_MEDIUM  50
+#define AB8500_CODEC_VIBRATOR_VOLUME_MIN     0
+
+/* CR0 - 7 */
+typedef enum {
+	AB8500_CODEC_CR0_POWERUP_OFF,
+	AB8500_CODEC_CR0_POWERUP_ON
+} t_ab8500_codec_cr0_powerup;
+
+/* CR0 - 3 */
+typedef enum {
+	AB8500_CODEC_CR0_ENAANA_OFF,
+	AB8500_CODEC_CR0_ENAANA_ON
+} t_ab8500_codec_cr0_enaana;
+
+/* CR1 - 7 */
+typedef enum {
+	AB8500_CODEC_CR1_SWRESET_DISABLED,
+	AB8500_CODEC_CR1_SWRESET_ENABLED
+} t_ab8500_codec_cr1_swreset;
+
+/* CR2 - 7 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD1_DISABLED,
+	AB8500_CODEC_CR2_ENAD1_ENABLED
+} t_ab8500_codec_cr2_enad1;
+
+/* CR2 - 6 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD2_DISABLED,
+	AB8500_CODEC_CR2_ENAD2_ENABLED
+} t_ab8500_codec_cr2_enad2;
+
+/* CR2 - 5 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD3_DISABLED,
+	AB8500_CODEC_CR2_ENAD3_ENABLED
+} t_ab8500_codec_cr2_enad3;
+
+/* CR2 - 4 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD4_DISABLED,
+	AB8500_CODEC_CR2_ENAD4_ENABLED
+} t_ab8500_codec_cr2_enad4;
+
+/* CR2 - 3 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD5_DISABLED,
+	AB8500_CODEC_CR2_ENAD5_ENABLED
+} t_ab8500_codec_cr2_enad5;
+
+/* CR2 - 2 */
+typedef enum {
+	AB8500_CODEC_CR2_ENAD6_DISABLED,
+	AB8500_CODEC_CR2_ENAD6_ENABLED
+} t_ab8500_codec_cr2_enad6;
+
+/* CR3 - 7 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA1_DISABLED,
+	AB8500_CODEC_CR3_ENDA1_ENABLED
+} t_ab8500_codec_cr3_enda1;
+
+/* CR3 - 6 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA2_DISABLED,
+	AB8500_CODEC_CR3_ENDA2_ENABLED
+} t_ab8500_codec_cr3_enda2;
+
+/* CR3 - 5 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA3_DISABLED,
+	AB8500_CODEC_CR3_ENDA3_ENABLED
+} t_ab8500_codec_cr3_enda3;
+
+/* CR3 - 4 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA4_DISABLED,
+	AB8500_CODEC_CR3_ENDA4_ENABLED
+} t_ab8500_codec_cr3_enda4;
+
+/* CR3 - 3 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA5_DISABLED,
+	AB8500_CODEC_CR3_ENDA5_ENABLED
+} t_ab8500_codec_cr3_enda5;
+
+/* CR3 - 2 */
+typedef enum {
+	AB8500_CODEC_CR3_ENDA6_DISABLED,
+	AB8500_CODEC_CR3_ENDA6_ENABLED
+} t_ab8500_codec_cr3_enda6;
+
+/* CR4 - 7 */
+typedef enum {
+	AB8500_CODEC_CR4_LOWPOWHS_NORMAL,
+	AB8500_CODEC_CR4_LOWPOWHS_LP
+} t_ab8500_codec_cr4_lowpowhs;
+
+/* CR4 - 6:5 */
+typedef enum {
+	AB8500_CODEC_CR4_LOWPOWDACHS_NORMAL,
+	AB8500_CODEC_CR4_LOWPOWDACHS_DRIVERS_LP,
+	AB8500_CODEC_CR4_LOWPOWDACHS_LP,
+	AB8500_CODEC_CR4_LOWPOWDACHS_BOTH_LP
+} t_ab8500_codec_cr4_lowpowdachs;
+
+/* CR4 - 4 */
+typedef enum {
+	AB8500_CODEC_CR4_LOWPOWEAR_NORMAL,
+	AB8500_CODEC_CR4_LOWPOWEAR_LP
+} t_ab8500_codec_cr4_lowpowear;
+
+/* CR4 - 3:2 */
+typedef enum {
+	AB8500_CODEC_CR4_EAR_SEL_CM_0_95V,
+	AB8500_CODEC_CR4_EAR_SEL_CM_1_1V,
+	AB8500_CODEC_CR4_EAR_SEL_CM_1_27V,
+	AB8500_CODEC_CR4_EAR_SEL_CM_1_58V
+} t_ab8500_codec_cr4_ear_sel_cm;
+
+/* CR4 - 1 */
+typedef enum {
+	AB8500_CODEC_CR4_HS_HP_EN_FILTER_DISABLED,
+	AB8500_CODEC_CR4_HS_HP_EN_FILTER_ENABLED
+} t_ab8500_codec_cr4_hs_hp_en;
+
+/* CR5 - 7 */
+typedef enum {
+	AB8500_CODEC_CR5_ENMIC1_DISABLED,
+	AB8500_CODEC_CR5_ENMIC1_ENABLED
+} t_ab8500_codec_cr5_enmic1;
+
+/* CR5 - 6 */
+typedef enum {
+	AB8500_CODEC_CR5_ENMIC2_DISABLED,
+	AB8500_CODEC_CR5_ENMIC2_ENABLED
+} t_ab8500_codec_cr5_enmic2;
+
+/* CR5 - 5 */
+typedef enum {
+	AB8500_CODEC_CR5_ENLINL_DISABLED,
+	AB8500_CODEC_CR5_ENLINL_ENABLED
+} t_ab8500_codec_cr5_enlinl;
+
+/* CR5 - 4 */
+typedef enum {
+	AB8500_CODEC_CR5_ENLINR_DISABLED,
+	AB8500_CODEC_CR5_ENLINR_ENABLED
+} t_ab8500_codec_cr5_enlinr;
+
+/* CR5 - 3 */
+typedef enum {
+	AB8500_CODEC_CR5_MUTMIC1_DISABLED,
+	AB8500_CODEC_CR5_MUTMIC1_ENABLED
+} t_ab8500_codec_cr5_mutmic1;
+
+/* CR5 - 2 */
+typedef enum {
+	AB8500_CODEC_CR5_MUTMIC2_DISABLED,
+	AB8500_CODEC_CR5_MUTMIC2_ENABLED
+} t_ab8500_codec_cr5_mutmic2;
+
+/* CR5 - 1 */
+typedef enum {
+	AB8500_CODEC_CR5_MUTLINL_DISABLED,
+	AB8500_CODEC_CR5_MUTLINL_ENABLED
+} t_ab8500_codec_cr5_mutlinl;
+
+/* CR5 - 0 */
+typedef enum {
+	AB8500_CODEC_CR5_MUTLINR_DISABLED,
+	AB8500_CODEC_CR5_MUTLINR_ENABLED
+} t_ab8500_codec_cr5_mutlinr;
+
+/* CR6 - 7 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC1_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC1_ENABLED
+} t_ab8500_codec_cr6_endmic1;
+
+/* CR6 - 6 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC2_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC2_ENABLED
+} t_ab8500_codec_cr6_endmic2;
+
+/* CR6 - 5 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC3_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC3_ENABLED
+} t_ab8500_codec_cr6_endmic3;
+
+/* CR6 - 4 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC4_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC4_ENABLED
+} t_ab8500_codec_cr6_endmic4;
+
+/* CR6 - 3 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC5_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC5_ENABLED
+} t_ab8500_codec_cr6_endmic5;
+
+/* CR6 - 2 */
+typedef enum {
+	AB8500_CODEC_CR6_ENDMIC6_DISABLED,
+	AB8500_CODEC_CR6_ENDMIC6_ENABLED
+} t_ab8500_codec_cr6_endmic6;
+
+/* CR7 - 7 */
+typedef enum {
+	AB8500_CODEC_CR7_MIC1SEL_MIC1A,
+	AB8500_CODEC_CR7_MIC1SEL_MIC1B
+} t_ab8500_codec_cr7_mic1sel;
+
+/* CR7 - 6 */
+typedef enum {
+	AB8500_CODEC_CR7_LINRSEL_MIC2,
+	AB8500_CODEC_CR7_LINRSEL_LINR
+} t_ab8500_codec_cr7_linrsel;
+
+/* CR7 - 5 */
+typedef enum {
+	AB8500_CODEC_CR7_ENDRVHSL_DISABLED,
+	AB8500_CODEC_CR7_ENDRVHSL_ENABLED
+} t_ab8500_codec_cr7_endrvhsl;
+
+/* CR7 - 4 */
+typedef enum {
+	AB8500_CODEC_CR7_ENDRVHSR_DISABLED,
+	AB8500_CODEC_CR7_ENDRVHSR_ENABLED
+} t_ab8500_codec_cr7_endrvhsr;
+
+/* CR7 - 2 */
+typedef enum {
+	AB8500_CODEC_CR7_ENADCMIC_DISABLED,
+	AB8500_CODEC_CR7_ENADCMIC_ENABLED
+} t_ab8500_codec_cr7_enadcmic;
+
+/* CR7 - 1 */
+typedef enum {
+	AB8500_CODEC_CR7_ENADCLINL_DISABLED,
+	AB8500_CODEC_CR7_ENADCLINL_ENABLED
+} t_ab8500_codec_cr7_enadclinl;
+
+/* CR7 - 0 */
+typedef enum {
+	AB8500_CODEC_CR7_ENADCLINR_DISABLED,
+	AB8500_CODEC_CR7_ENADCLINR_ENABLED
+} t_ab8500_codec_cr7_enadclinr;
+
+/* CR8 - 7 */
+typedef enum {
+	AB8500_CODEC_CR8_CP_DIS_PLDWN_ENABLED,
+	AB8500_CODEC_CR8_CP_DIS_PLDWN_DISABLED
+} t_ab8500_codec_cr8_cp_dis_pldwn;
+
+/* CR8 - 6 */
+typedef enum {
+	AB8500_CODEC_CR8_ENEAR_DISABLED,
+	AB8500_CODEC_CR8_ENEAR_ENABLED
+} t_ab8500_codec_cr8_enear;
+
+/* CR8 - 5 */
+typedef enum {
+	AB8500_CODEC_CR8_ENHSL_DISABLED,
+	AB8500_CODEC_CR8_ENHSL_ENABLED
+} t_ab8500_codec_cr8_enhsl;
+
+/* CR8 - 4 */
+typedef enum {
+	AB8500_CODEC_CR8_ENHSR_DISABLED,
+	AB8500_CODEC_CR8_ENHSR_ENABLED
+} t_ab8500_codec_cr8_enhsr;
+
+/* CR8 - 3 */
+typedef enum {
+	AB8500_CODEC_CR8_ENHFL_DISABLED,
+	AB8500_CODEC_CR8_ENHFL_ENABLED
+} t_ab8500_codec_cr8_enhfl;
+
+/* CR8 - 2 */
+typedef enum {
+	AB8500_CODEC_CR8_ENHFR_DISABLED,
+	AB8500_CODEC_CR8_ENHFR_ENABLED
+} t_ab8500_codec_cr8_enhfr;
+
+/* CR8 - 1 */
+typedef enum {
+	AB8500_CODEC_CR8_ENVIBL_DISABLED,
+	AB8500_CODEC_CR8_ENVIBL_ENABLED
+} t_ab8500_codec_cr8_envibl;
+
+/* CR8 - 0 */
+typedef enum {
+	AB8500_CODEC_CR8_ENVIBR_DISABLED,
+	AB8500_CODEC_CR8_ENVIBR_ENABLED
+} t_ab8500_codec_cr8_envibr;
+
+/* CR9 - 6 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACEAR_DISABLED,
+	AB8500_CODEC_CR9_ENDACEAR_ENABLED
+} t_ab8500_codec_cr9_endacear;
+
+/* CR9 - 5 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACHSL_DISABLED,
+	AB8500_CODEC_CR9_ENDACHSL_ENABLED
+} t_ab8500_codec_cr9_endachsl;
+
+/* CR9 - 4 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACHSR_DISABLED,
+	AB8500_CODEC_CR9_ENDACHSR_ENABLED
+} t_ab8500_codec_cr9_endachsr;
+
+/* CR9 - 3 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACHFL_DISABLED,
+	AB8500_CODEC_CR9_ENDACHFL_ENABLED
+} t_ab8500_codec_cr9_endachfl;
+
+/* CR9 - 2 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACHFR_DISABLED,
+	AB8500_CODEC_CR9_ENDACHFR_ENABLED
+} t_ab8500_codec_cr9_endachfr;
+
+/* CR9 - 1 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACVIBL_DISABLED,
+	AB8500_CODEC_CR9_ENDACVIBL_ENABLED
+} t_ab8500_codec_cr9_endacvibl;
+
+/* CR9 - 0 */
+typedef enum {
+	AB8500_CODEC_CR9_ENDACVIBR_DISABLED,
+	AB8500_CODEC_CR9_ENDACVIBR_ENABLED
+} t_ab8500_codec_cr9_endacvibr;
+
+/* CR10 - 6 */
+typedef enum {
+	AB8500_CODEC_CR10_MUTEEAR_DISABLED,
+	AB8500_CODEC_CR10_MUTEEAR_ENABLED
+} t_ab8500_codec_cr10_muteear;
+
+/* CR10 - 5 */
+typedef enum {
+	AB8500_CODEC_CR10_MUTEHSL_DISABLED,
+	AB8500_CODEC_CR10_MUTEHSL_ENABLED
+} t_ab8500_codec_cr10_mutehsl;
+
+/* CR10 - 4 */
+typedef enum {
+	AB8500_CODEC_CR10_MUTEHSR_DISABLED,
+	AB8500_CODEC_CR10_MUTEHSR_ENABLED
+} t_ab8500_codec_cr10_mutehsr;
+
+/* CR11 - 7 */
+typedef enum {
+	AB8500_CODEC_CR11_ENSHORTPWD_DISABLED,
+	AB8500_CODEC_CR11_ENSHORTPWD_ENABLED
+} t_ab8500_codec_cr11_enshortpwd;
+
+/* CR11 - 6 */
+typedef enum {
+	AB8500_CODEC_CR11_EARSHORTDIS_ENABLED,
+	AB8500_CODEC_CR11_EARSHORTDIS_DISABLED
+} t_ab8500_codec_cr11_earshortdis;
+
+/* CR11 - 5 */
+typedef enum {
+	AB8500_CODEC_CR11_HSSHORTDIS_ENABLED,
+	AB8500_CODEC_CR11_HSSHORTDIS_DISABLED
+} t_ab8500_codec_cr11_hsshortdis;
+
+/* CR11 - 4 */
+typedef enum {
+	AB8500_CODEC_CR11_HSPULLDEN_HIGH,
+	AB8500_CODEC_CR11_HSPULLDEN_DOWN
+} t_ab8500_codec_cr11_hspullden;
+
+/* CR11 - 2 */
+typedef enum {
+	AB8500_CODEC_CR11_HSOSCEN_SYSTEMCLOCK,
+	AB8500_CODEC_CR11_HSOSCEN_LOCALOSC
+} t_ab8500_codec_cr11_hsoscen;
+
+/* CR11 - 1 */
+typedef enum {
+	AB8500_CODEC_CR11_HSFADEN_FADING,
+	AB8500_CODEC_CR11_HSFADEN_IMMEDIATELY
+} t_ab8500_codec_cr11_hsfaden;
+
+/* CR11 - 0 */
+typedef enum {
+	AB8500_CODEC_CR11_HSZCDDIS_ONZEROCROSS,
+	AB8500_CODEC_CR11_HSZCDDIS_WITHOUTZEROCROSS
+} t_ab8500_codec_cr11_hszcddis;
+
+/* CR12 - 7 */
+typedef enum {
+	AB8500_CODEC_CR12_ENCPHS_DISABLED,
+	AB8500_CODEC_CR12_ENCPHS_ENABLED
+} t_ab8500_codec_cr12_encphs;
+
+/* CR12 - 0 */
+typedef enum {
+	AB8500_CODEC_CR12_HSAUTOEN_DISABLED,
+	AB8500_CODEC_CR12_HSAUTOEN_ENABLED
+} t_ab8500_codec_cr12_hsautoen;
+
+/* CR13 - 7:4 */
+typedef enum {
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_25,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_50,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_100,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_150,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_200,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_250,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_300,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_350,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_400,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_450,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_500,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_550,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_600,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_650,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_700,
+	AB8500_CODEC_CR13_ENVDET_HTHRESH_750
+} t_ab8500_codec_cr13_envdet_hthresh;
+
+/* CR13 - 3:0 */
+typedef enum {
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_25,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_50,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_100,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_150,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_200,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_250,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_300,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_350,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_400,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_450,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_500,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_550,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_600,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_650,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_700,
+	AB8500_CODEC_CR13_ENVDET_LTHRESH_750
+} t_ab8500_codec_cr13_envdet_lthresh;
+
+/* CR14 - 7 */
+typedef enum {
+	AB8500_CODEC_CR14_SMPSLVEN_HIGHVOLTAGE,
+	AB8500_CODEC_CR14_SMPSLVEN_LOWVOLTAGE
+} t_ab8500_codec_cr14_smpslven;
+
+/* CR14 - 6 */
+typedef enum {
+	AB8500_CODEC_CR14_ENVDETSMPSEN_DISABLED,
+	AB8500_CODEC_CR14_ENVDETSMPSEN_ENABLED
+} t_ab8500_codec_cr14_envdetsmpsen;
+
+/* CR14 - 5 */
+typedef enum {
+	AB8500_CODEC_CR14_CPLVEN_HIGHVOLTAGE,
+	AB8500_CODEC_CR14_CPLVEN_LOWVOLTAGE
+} t_ab8500_codec_cr14_cplven;
+
+/* CR14 - 4 */
+typedef enum {
+	AB8500_CODEC_CR14_ENVDETCPEN_DISABLED,
+	AB8500_CODEC_CR14_ENVDETCPEN_ENABLED
+} t_ab8500_codec_cr14_envdetcpen;
+
+/* CR14 - 3:0 */
+typedef enum {
+	AB8500_CODEC_CR14_ENVET_TIME_27USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_53USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_106USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_212USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_424USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_848USEC,
+	AB8500_CODEC_CR14_ENVET_TIME_1MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_3MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_6MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_13MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_27MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_54MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_109MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_218MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_436MSEC,
+	AB8500_CODEC_CR14_ENVET_TIME_872MSEC,
+} t_ab8500_codec_cr14_envet_time;
+
+/* CR15 - 7 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMTOVIBL_DA_PATH,
+	AB8500_CODEC_CR15_PWMTOVIBL_PWM
+} t_ab8500_codec_cr15_pwmtovibl;
+
+/* CR15 - 6 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMTOVIBR_DA_PATH,
+	AB8500_CODEC_CR15_PWMTOVIBR_PWM
+} t_ab8500_codec_cr15_pwmtovibr;
+
+/* CR15 - 5 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMLCTRL_PWMNPLGPOL,
+	AB8500_CODEC_CR15_PWMLCTRL_PWMNPLDUTYCYCLE
+} t_ab8500_codec_cr15_pwmlctrl;
+
+/* CR15 - 4 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMRCTRL_PWMNPRGPOL,
+	AB8500_CODEC_CR15_PWMRCTRL_PWMNPRDUTYCYCLE
+} t_ab8500_codec_cr15_pwmrctrl;
+
+/* CR15 - 3 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMNLCTRL_PWMNLGPOL,
+	AB8500_CODEC_CR15_PWMNLCTRL_PWMNLDUTYCYCLE
+} t_ab8500_codec_cr15_pwmnlctrl;
+
+/* CR15 - 2 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMPLCTRL_PWMPLGPOL,
+	AB8500_CODEC_CR15_PWMPLCTRL_PWMPLDUTYCYCLE
+} t_ab8500_codec_cr15_pwmplctrl;
+
+/* CR15 - 1 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMNRCTRL_PWMNRGPOL,
+	AB8500_CODEC_CR15_PWMNRCTRL_PWMNRDUTYCYCLE
+} t_ab8500_codec_cr15_pwmnrctrl;
+
+/* CR15 - 0 */
+typedef enum {
+	AB8500_CODEC_CR15_PWMPRCTRL_PWMPRGPOL,
+	AB8500_CODEC_CR15_PWMPRCTRL_PWMPRDUTYCYCLE
+} t_ab8500_codec_cr15_pwmprctrl;
+
+/* CR16 - 7 */
+typedef enum {
+	AB8500_CODEC_CR16_PWMNLPOL_GNDVIB,
+	AB8500_CODEC_CR16_PWMNLPOL_VINVIB
+} t_ab8500_codec_cr16_pwmnlpol;
+
+/* CR16 - 6:0 */
+typedef t_uint8 t_ab8500_codec_cr16_pwmnldutycycle;
+
+/* CR17 - 7 */
+typedef enum {
+	AB8500_CODEC_CR17_PWMPLPOL_GNDVIB,
+	AB8500_CODEC_CR17_PWMPLPOL_VINVIB
+} t_ab8500_codec_cr17_pwmplpol;
+
+/* CR17 - 6:0 */
+typedef t_uint8 t_ab8500_codec_cr17_pwmpldutycycle;
+
+/* CR18 - 7 */
+typedef enum {
+	AB8500_CODEC_CR18_PWMNRPOL_GNDVIB,
+	AB8500_CODEC_CR18_PWMNRPOL_VINVIB
+} t_ab8500_codec_cr18_pwmnrpol;
+
+/* CR18 - 6:0 */
+typedef t_uint8 t_ab8500_codec_cr18_pwmnrdutycycle;
+
+/* CR19 - 7 */
+typedef enum {
+	AB8500_CODEC_CR19_PWMPRPOL_GNDVIB,
+	AB8500_CODEC_CR19_PWMPRPOL_VINVIB
+} t_ab8500_codec_cr19_pwmprpol;
+
+/* CR19 - 6:0 */
+typedef t_uint8 t_ab8500_codec_cr19_pwmprdutycycle;
+
+/* CR20 - 7 */
+typedef enum {
+	AB8500_CODEC_CR20_EN_SE_MIC1_DIFFERENTIAL,
+	AB8500_CODEC_CR20_EN_SE_MIC1_SINGLE
+} t_ab8500_codec_cr20_en_se_mic1;
+
+/* CR20 - 6 */
+typedef enum {
+	AB8500_CODEC_CR20_LOW_POW_MIC1_NORMAL,
+	AB8500_CODEC_CR20_LOW_POW_MIC1_LOW_POWER
+} t_ab8500_codec_cr20_low_pow_mic1;
+
+/* CR20 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr20_mic1_gain;
+
+/* CR21 - 7 */
+typedef enum {
+	AB8500_CODEC_CR21_EN_SE_MIC2_DIFFERENTIAL,
+	AB8500_CODEC_CR21_EN_SE_MIC2_SINGLE
+} t_ab8500_codec_cr21_en_se_mic2;
+
+/* CR21 - 6 */
+typedef enum {
+	AB8500_CODEC_CR21_LOW_POW_MIC2_NORMAL,
+	AB8500_CODEC_CR21_LOW_POW_MIC2_LOW_POWER
+} t_ab8500_codec_cr21_low_pow_mic2;
+
+/* CR21 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr21_mic2_gain;
+
+/* CR22 - 7:4 */
+typedef t_uint8 t_ab8500_codec_cr22_hsl_gain;
+
+/* CR22 - 3:0 */
+typedef t_uint8 t_ab8500_codec_cr22_hsr_gain;
+
+/* CR23 - 7:4 */
+typedef t_uint8 t_ab8500_codec_cr23_linl_gain;
+
+/* CR23 - 3:0 */
+typedef t_uint8 t_ab8500_codec_cr23_linr_gain;
+
+/* CR24 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr24_lintohsl_gain;
+
+/* CR25 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr25_lintohsr_gain;
+
+/* CR26 - 7 */
+typedef enum {
+	AB8500_CODEC_CR26_AD1NH_FILTER_ENABLED,
+	AB8500_CODEC_CR26_AD1NH_FILTER_DISABLED
+} t_ab8500_codec_cr26_ad1nh;
+
+/* CR26 - 6 */
+typedef enum {
+	AB8500_CODEC_CR26_AD2NH_FILTER_ENABLED,
+	AB8500_CODEC_CR26_AD2NH_FILTER_DISABLED
+} t_ab8500_codec_cr26_ad2nh;
+
+/* CR26 - 5 */
+typedef enum {
+	AB8500_CODEC_CR26_AD3NH_FILTER_ENABLED,
+	AB8500_CODEC_CR26_AD3NH_FILTER_DISABLED
+} t_ab8500_codec_cr26_ad3nh;
+
+/* CR26 - 4 */
+typedef enum {
+	AB8500_CODEC_CR26_AD4NH_FILTER_ENABLED,
+	AB8500_CODEC_CR26_AD4NH_FILTER_DISABLED
+} t_ab8500_codec_cr26_ad4nh;
+
+/* CR26 - 3 */
+typedef enum {
+	AB8500_CODEC_CR26_AD1_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR26_AD1_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr26_ad1_voice;
+
+/* CR26 - 2 */
+typedef enum {
+	AB8500_CODEC_CR26_AD2_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR26_AD2_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr26_ad2_voice;
+
+/* CR26 - 1 */
+typedef enum {
+	AB8500_CODEC_CR26_AD3_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR26_AD3_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr26_ad3_voice;
+
+/* CR26 - 0 */
+typedef enum {
+	AB8500_CODEC_CR26_AD4_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR26_AD4_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr26_ad4_voice;
+
+/* CR27 - 7 */
+typedef enum {
+	AB8500_CODEC_CR27_EN_MASTGEN_DISABLED,
+	AB8500_CODEC_CR27_EN_MASTGEN_ENABLED
+} t_ab8500_codec_cr27_en_mastgen;
+
+/* CR27 - 6:5 */
+/* In ab8500_codec.h */
+
+/* CR27 - 4 */
+typedef enum {
+	AB8500_CODEC_CR27_ENFS_BITCLK1_DISABLED,
+	AB8500_CODEC_CR27_ENFS_BITCLK1_ENABLED
+} t_ab8500_codec_cr27_enfs_bitclk1;
+
+/* CR27 - 2:1 */
+/* In ab8500_codec.h */
+
+/* CR27 - 0 */
+typedef enum {
+	AB8500_CODEC_CR27_ENFS_BITCLK0_DISABLED,
+	AB8500_CODEC_CR27_ENFS_BITCLK0_ENABLED
+} t_ab8500_codec_cr27_enfs_bitclk0;
+
+/* CR28 - 6 */
+typedef enum {
+	AB8500_CODEC_CR28_FSYNC0P_RISING_EDGE,
+	AB8500_CODEC_CR28_FSYNC0P_FALLING_EDGE
+} t_ab8500_codec_cr28_fsync0p;
+
+/* CR28 - 5 */
+typedef enum {
+	AB8500_CODEC_CR28_BITCLK0P_RISING_EDGE,
+	AB8500_CODEC_CR28_BITCLK0P_FALLING_EDGE
+} t_ab8500_codec_cr28_bitclk0p;
+
+/* CR28 - 4 */
+typedef enum {
+	AB8500_CODEC_CR28_IF0DEL_NOT_DELAYED,
+	AB8500_CODEC_CR28_IF0DEL_DELAYED
+} t_ab8500_codec_cr28_if0del;
+
+/* CR28 - 3:2 */
+typedef enum {
+	AB8500_CODEC_CR28_IF0FORMAT_DISABLED,
+	AB8500_CODEC_CR28_IF0FORMAT_TDM,
+	AB8500_CODEC_CR28_IF0FORMAT_I2S_LEFTALIGNED
+} t_ab8500_codec_cr28_if0format;
+
+/* CR28 - 1:0 */
+/* In ab8500_codec.h */
+
+/* CR29 - 7 */
+typedef enum {
+	AB8500_CODEC_CR29_IF0DATOIF1AD_NOTSENT,
+	AB8500_CODEC_CR29_IF0DATOIF1AD_SENT
+} t_ab8500_codec_cr29_if0datoif1ad;
+
+/* CR29 - 6 */
+typedef enum {
+	AB8500_CODEC_CR29_IF0CKTOIF1CK_NOTSENT,
+	AB8500_CODEC_CR29_IF0CKTOIF1CK_SENT
+} t_ab8500_codec_cr29_if0cktoif1ck;
+
+/* CR29 - 5 */
+typedef enum {
+	AB8500_CODEC_CR29_IF1MASTER_FS1CK1_INPUT,
+	AB8500_CODEC_CR29_IF1MASTER_FS1CK1_OUTPUT
+} t_ab8500_codec_cr29_if1master;
+
+/* CR29 - 3 */
+typedef enum {
+	AB8500_CODEC_CR29_IF1DATOIF0AD_NOTSENT,
+	AB8500_CODEC_CR29_IF1DATOIF0AD_SENT
+} t_ab8500_codec_cr29_if1datoif0ad;
+
+/* CR29 - 2 */
+typedef enum {
+	AB8500_CODEC_CR29_IF1CKTOIF0CK_NOTSENT,
+	AB8500_CODEC_CR29_IF1CKTOIF0CK_SENT
+} t_ab8500_codec_cr29_if1cktoif0ck;
+
+/* CR29 - 1 */
+typedef enum {
+	AB8500_CODEC_CR29_IF0MASTER_FS0CK0_INPUT,
+	AB8500_CODEC_CR29_IF0MASTER_FS0CK0_OUTPUT
+} t_ab8500_codec_cr29_if0master;
+
+/* CR29 - 0 */
+typedef enum {
+	AB8500_CODEC_CR29_IF0BFIFOEN_NORMAL_MODE,
+	AB8500_CODEC_CR29_IF0BFIFOEN_BURST_MODE
+} t_ab8500_codec_cr29_if0bfifoen;
+
+/* CR30 - 6 */
+typedef enum {
+	AB8500_CODEC_CR30_FSYNC1P_RISING_EDGE,
+	AB8500_CODEC_CR30_FSYNC1P_FALLING_EDGE
+} t_ab8500_codec_cr30_fsync1p;
+
+/* CR30 - 5 */
+typedef enum {
+	AB8500_CODEC_CR30_BITCLK1P_RISING_EDGE,
+	AB8500_CODEC_CR30_BITCLK1P_FALLING_EDGE
+} t_ab8500_codec_cr30_bitclk1p;
+
+/* CR30 - 4 */
+typedef enum {
+	AB8500_CODEC_CR30_IF1DEL_NOT_DELAYED,
+	AB8500_CODEC_CR30_IF1DEL_DELAYED
+} t_ab8500_codec_cr30_if1del;
+
+/* CR30 - 3:2 */
+typedef enum {
+	AB8500_CODEC_CR30_IF1FORMAT_DISABLED,
+	AB8500_CODEC_CR30_IF1FORMAT_TDM,
+	AB8500_CODEC_CR30_IF1FORMAT_I2S_LEFTALIGNED
+} t_ab8500_codec_cr30_if1format;
+
+/* CR30 - 1:0 */
+/* In ab8500_codec.h */
+
+/* CR31:46 - 7:4 or 3:0 */
+/* In ab8500_codec.h */
+
+/* CR47:50 - 7/6/5/4/3/2/1/0 */
+typedef enum {
+	AB8500_CODEC_CR47_TO_CR50_HIZ_SL_LOW_IMPEDANCE,
+	AB8500_CODEC_CR47_TO_CR50_HIZ_SL_HIGH_IMPEDANCE,
+} t_ab8500_codec_cr47_to_cr50_hiz_sl;
+
+/* CR51 - 7 */
+typedef enum {
+	AB8500_CODEC_CR51_DA12_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR51_DA12_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr51_da12_voice;
+
+/* CR51 - 6 */
+typedef enum {
+	AB8500_CODEC_CR51_SWAPDA12_34_NORMAL,
+	AB8500_CODEC_CR51_SWAPDA12_34_SWAPPED
+} t_ab8500_codec_cr51_swapda12_34;
+
+/* CR51 - 5 */
+typedef enum {
+	AB8500_CODEC_CR51_SLDAI7TOSLADO1_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR51_SLDAI7TOSLADO1_LOOPEDBACK
+} t_ab8500_codec_cr51_sldai7toslado1;
+
+/* CR51:58 - 4:0 */
+/* In ab8500_codec.h */
+
+/* CR52 - 5 */
+typedef enum {
+	AB8500_CODEC_CR52_SLDAI8TOSLADO2_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR52_SLDAI8TOSLADO2_LOOPEDBACK
+} t_ab8500_codec_cr52_sldai8toslado2;
+
+/* CR53 - 7 */
+typedef enum {
+	AB8500_CODEC_CR53_DA34_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR53_DA34_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr53_da34_voice;
+
+/* CR53 - 5 */
+typedef enum {
+	AB8500_CODEC_CR53_SLDAI7TOSLADO3_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR53_SLDAI7TOSLADO3_LOOPEDBACK
+} t_ab8500_codec_cr53_sldai7toslado3;
+
+/* CR54 - 5 */
+typedef enum {
+	AB8500_CODEC_CR54_SLDAI8TOSLADO4_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR54_SLDAI8TOSLADO4_LOOPEDBACK
+} t_ab8500_codec_cr54_sldai8toslado4;
+
+/* CR55 - 7 */
+typedef enum {
+	AB8500_CODEC_CR55_DA56_VOICE_AUDIOFILTER,
+	AB8500_CODEC_CR55_DA56_VOICE_LOWLATENCYFILTER
+} t_ab8500_codec_cr55_da56_voice;
+
+/* CR55 - 5 */
+typedef enum {
+	AB8500_CODEC_CR55_SLDAI7TOSLADO5_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR55_SLDAI7TOSLADO5_LOOPEDBACK
+} t_ab8500_codec_cr55_sldai7toslado5;
+
+/* CR56 - 5 */
+typedef enum {
+	AB8500_CODEC_CR56_SLDAI8TOSLADO6_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR56_SLDAI8TOSLADO6_LOOPEDBACK
+} t_ab8500_codec_cr56_sldai8toslado6;
+
+/* CR57 - 5 */
+typedef enum {
+	AB8500_CODEC_CR57_SLDAI8TOSLADO7_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR57_SLDAI8TOSLADO7_LOOPEDBACK
+} t_ab8500_codec_cr57_sldai8toslado7;
+
+/* CR58 - 5 */
+typedef enum {
+	AB8500_CODEC_CR58_SLDAI7TOSLADO8_NOT_LOOPEDBACK,
+	AB8500_CODEC_CR58_SLDAI7TOSLADO8_LOOPEDBACK
+} t_ab8500_codec_cr58_sldai7toslado8;
+
+/* CR59 - 7 */
+typedef enum {
+	AB8500_CODEC_CR59_PARLHF_INDEPENDENT,
+	AB8500_CODEC_CR59_PARLHF_BRIDGED
+} t_ab8500_codec_cr59_parlhf;
+
+/* CR59 - 6 */
+typedef enum {
+	AB8500_CODEC_CR59_PARLVIB_INDEPENDENT,
+	AB8500_CODEC_CR59_PARLVIB_BRIDGED
+} t_ab8500_codec_cr59_parlvib;
+
+/* CR59 - 3 */
+typedef enum {
+	AB8500_CODEC_CR59_CLASSDVIB1_SWAPEN_DISABLED,
+	AB8500_CODEC_CR59_CLASSDVIB1_SWAPEN_ENABLED
+} t_ab8500_codec_cr59_classdvib1_swapen;
+
+/* CR59 - 2 */
+typedef enum {
+	AB8500_CODEC_CR59_CLASSDVIB2_SWAPEN_DISABLED,
+	AB8500_CODEC_CR59_CLASSDVIB2_SWAPEN_ENABLED
+} t_ab8500_codec_cr59_classdvib2_swapen;
+
+/* CR59 - 1 */
+typedef enum {
+	AB8500_CODEC_CR59_CLASSDHFL_SWAPEN_DISABLED,
+	AB8500_CODEC_CR59_CLASSDHFL_SWAPEN_ENABLED
+} t_ab8500_codec_cr59_classdhfl_swapen;
+
+/* CR59 - 0 */
+typedef enum {
+	AB8500_CODEC_CR59_CLASSDHFR_SWAPEN_DISABLED,
+	AB8500_CODEC_CR59_CLASSDHFR_SWAPEN_ENABLED
+} t_ab8500_codec_cr59_classdhfr_swapen;
+
+/* CR60 - 7:4 */
+typedef enum {
+	AB8500_CODEC_CR60_CLASSD_FIRBYP_ALL_ENABLED = 0,
+	AB8500_CODEC_CR60_CLASSD_FIRBYP_LEFT_HF_BYPASSED = 1,
+	AB8500_CODEC_CR60_CLASSD_FIRBYP_RIGHT_HF_BYPASSED = 2,
+	AB8500_CODEC_CR60_CLASSD_FIRBYP_VIBRA1_BYPASSED = 4,
+	AB8500_CODEC_CR60_CLASSD_FIRBYP_VIBRA2_BYPASSED = 8
+} t_ab8500_codec_cr60_classd_firbyp;
+
+/* CR60 - 3:0 */
+typedef enum {
+	AB8500_CODEC_CR60_CLASSD_HIGHVOLEN_DISABLED = 0,
+	AB8500_CODEC_CR60_CLASSD_HIGHVOLEN_LEFT_HF = 1,
+	AB8500_CODEC_CR60_CLASSD_HIGHVOLEN_RIGHT_HF = 2,
+	AB8500_CODEC_CR60_CLASSD_HIGHVOLEN_VIBRA1 = 4,
+	AB8500_CODEC_CR60_CLASSD_HIGHVOLEN_VIBRA2 = 8
+} t_ab8500_codec_cr60_classd_highvolen;
+
+/* CR61 - 7:4 */
+typedef t_uint8 t_ab8500_codec_cr61_classddith_hpgain;
+
+/* CR61 - 3:0 */
+typedef t_uint8 t_ab8500_codec_cr61_classddith_wgain;
+
+/* CR62 - Read Only */
+/* CR62 - 5 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC1SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC1SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic1sinc3;
+
+/* CR62 - 4 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC2SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC2SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic2sinc3;
+
+/* CR62 - 3 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC3SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC3SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic3sinc3;
+
+/* CR62 - 2 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC4SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC4SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic4sinc3;
+
+/* CR62 - 1 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC5SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC5SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic5sinc3;
+
+/* CR62 - 0 */
+typedef enum {
+	AB8500_CODEC_CR62_DMIC6SINC3_SINC5_SELECTED,
+	AB8500_CODEC_CR62_DMIC6SINC3_SINC3_SELECTED
+} t_ab8500_codec_cr62_dmic6sinc3;
+
+/* CR63 - 7 */
+typedef enum {
+	AB8500_CODEC_CR63_DATOHSLEN_DISABLED,
+	AB8500_CODEC_CR63_DATOHSLEN_ENABLED
+} t_ab8500_codec_cr63_datohslen;
+
+/* CR63 - 6 */
+typedef enum {
+	AB8500_CODEC_CR63_DATOHSREN_DISABLED,
+	AB8500_CODEC_CR63_DATOHSREN_ENABLED
+} t_ab8500_codec_cr63_datohsren;
+
+/* CR63 - 5 */
+typedef enum {
+	AB8500_CODEC_CR63_AD1SEL_LINLADL_SELECTED,
+	AB8500_CODEC_CR63_AD1SEL_DMIC1_SELECTED
+} t_ab8500_codec_cr63_ad1sel;
+
+/* CR63 - 4 */
+typedef enum {
+	AB8500_CODEC_CR63_AD2SEL_LINRADR_SELECTED,
+	AB8500_CODEC_CR63_AD2SEL_DMIC2_SELECTED
+} t_ab8500_codec_cr63_ad2sel;
+
+/* CR63 - 3 */
+typedef enum {
+	AB8500_CODEC_CR63_AD3SEL_ADMO_SELECTED,
+	AB8500_CODEC_CR63_AD3SEL_DMIC3_SELECTED
+} t_ab8500_codec_cr63_ad3sel;
+
+/* CR63 - 2 */
+typedef enum {
+	AB8500_CODEC_CR63_AD5SEL_AMADR_SELECTED,
+	AB8500_CODEC_CR63_AD5SEL_DMIC5_SELECTED
+} t_ab8500_codec_cr63_ad5sel;
+
+/* CR63 - 1 */
+typedef enum {
+	AB8500_CODEC_CR63_AD6SEL_ADMO_SELECTED,
+	AB8500_CODEC_CR63_AD6SEL_DMIC6_SELECTED
+} t_ab8500_codec_cr63_ad6sel;
+
+/* CR63 - 0 */
+typedef enum {
+	AB8500_CODEC_CR63_ANCSEL_NOT_MIXED_IN_EAR,
+	AB8500_CODEC_CR63_ANCSEL_MIXED_IN_EAR
+} t_ab8500_codec_cr63_ancsel;
+
+/* CR64 - 7 */
+typedef enum {
+	AB8500_CODEC_CR64_DATOHFREN_NOT_MIXED_TO_HFR,
+	AB8500_CODEC_CR64_DATOHFREN_MIXED_TO_HFR
+} t_ab8500_codec_cr64_datohfren;
+
+/* CR64 - 6 */
+typedef enum {
+	AB8500_CODEC_CR64_DATOHFLEN_NOT_MIXED_TO_HFL,
+	AB8500_CODEC_CR64_DATOHFLEN_MIXED_TO_HFL
+} t_ab8500_codec_cr64_datohflen;
+
+/* CR64 - 5 */
+typedef enum {
+	AB8500_CODEC_CR64_HFRSEL_DA4_MIXED_TO_HFR,
+	AB8500_CODEC_CR64_HFRSEL_ANC_MIXED_TO_HFR
+} t_ab8500_codec_cr64_hfrsel;
+
+/* CR64 - 4 */
+typedef enum {
+	AB8500_CODEC_CR64_HFLSEL_DA3_MIXED_TO_HFL,
+	AB8500_CODEC_CR64_HFLSEL_ANC_MIXED_TO_HFL
+} t_ab8500_codec_cr64_hflsel;
+
+/* CR64 - 3:2 */
+typedef enum {
+	AB8500_CODEC_CR64_STFIR1SEL_AD_OUT1_SELECTED,
+	AB8500_CODEC_CR64_STFIR1SEL_AD_OUT3_SELECTED,
+	AB8500_CODEC_CR64_STFIR1SEL_DA_IN1_SELECTED
+} t_ab8500_codec_cr64_stfir1sel;
+
+/* CR64 - 1:0 */
+typedef enum {
+	AB8500_CODEC_CR64_STFIR2SEL_AD_OUT2_SELECTED,
+	AB8500_CODEC_CR64_STFIR2SEL_AD_OUT4_SELECTED,
+	AB8500_CODEC_CR64_STFIR2SEL_DA_IN2_SELECTED
+} t_ab8500_codec_cr64_stfir2sel;
+
+/* CR65 - 6 */
+typedef enum {
+	AB8500_CODEC_CR65_FADEDIS_AD1_ENABLED,
+	AB8500_CODEC_CR65_FADEDIS_AD1_DISABLED
+} t_ab8500_codec_cr65_fadedis_ad1;
+
+/* CR65 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr65_ad1gain;
+
+/* CR66 - 6 */
+typedef enum {
+	AB8500_CODEC_CR66_FADEDIS_AD2_ENABLED,
+	AB8500_CODEC_CR66_FADEDIS_AD2_DISABLED
+} t_ab8500_codec_cr66_fadedis_ad2;
+
+/* CR66 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr66_ad2gain;
+
+/* CR67 - 6 */
+typedef enum {
+	AB8500_CODEC_CR67_FADEDIS_AD3_ENABLED,
+	AB8500_CODEC_CR67_FADEDIS_AD3_DISABLED
+} t_ab8500_codec_cr67_fadedis_ad3;
+
+/* CR67 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr67_ad3gain;
+
+/* CR68 - 6 */
+typedef enum {
+	AB8500_CODEC_CR68_FADEDIS_AD4_ENABLED,
+	AB8500_CODEC_CR68_FADEDIS_AD4_DISABLED
+} t_ab8500_codec_cr68_fadedis_ad4;
+
+/* CR68 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr68_ad4gain;
+
+/* CR69 - 6 */
+typedef enum {
+	AB8500_CODEC_CR69_FADEDIS_AD5_ENABLED,
+	AB8500_CODEC_CR69_FADEDIS_AD5_DISABLED
+} t_ab8500_codec_cr69_fadedis_ad5;
+
+/* CR69 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr69_ad5gain;
+
+/* CR70 - 6 */
+typedef enum {
+	AB8500_CODEC_CR70_FADEDIS_AD6_ENABLED,
+	AB8500_CODEC_CR70_FADEDIS_AD6_DISABLED
+} t_ab8500_codec_cr70_fadedis_ad6;
+
+/* CR70 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr70_ad6gain;
+
+/* CR71 - 6 */
+typedef enum {
+	AB8500_CODEC_CR71_FADEDIS_DA1_ENABLED,
+	AB8500_CODEC_CR71_FADEDIS_DA1_DISABLED
+} t_ab8500_codec_cr71_fadedis_da1;
+
+/* CR71 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr71_da1gain;
+
+/* CR72 - 6 */
+typedef enum {
+	AB8500_CODEC_CR72_FADEDIS_DA2_ENABLED,
+	AB8500_CODEC_CR72_FADEDIS_DA2_DISABLED
+} t_ab8500_codec_cr72_fadedis_da2;
+
+/* CR72 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr72_da2gain;
+
+/* CR73 - 6 */
+typedef enum {
+	AB8500_CODEC_CR73_FADEDIS_DA3_ENABLED,
+	AB8500_CODEC_CR73_FADEDIS_DA3_DISABLED
+} t_ab8500_codec_cr73_fadedis_da3;
+
+/* CR73 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr73_da3gain;
+
+/* CR74 - 6 */
+typedef enum {
+	AB8500_CODEC_CR74_FADEDIS_DA4_ENABLED,
+	AB8500_CODEC_CR74_FADEDIS_DA4_DISABLED
+} t_ab8500_codec_cr74_fadedis_da4;
+
+/* CR74 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr74_da4gain;
+
+/* CR75 - 6 */
+typedef enum {
+	AB8500_CODEC_CR75_FADEDIS_DA5_ENABLED,
+	AB8500_CODEC_CR75_FADEDIS_DA5_DISABLED
+} t_ab8500_codec_cr75_fadedis_da5;
+
+/* CR75 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr75_da5gain;
+
+/* CR76 - 6 */
+typedef enum {
+	AB8500_CODEC_CR76_FADEDIS_DA6_ENABLED,
+	AB8500_CODEC_CR76_FADEDIS_DA6_DISABLED
+} t_ab8500_codec_cr76_fadedis_da6;
+
+/* CR76 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr76_da6gain;
+
+/* CR77 - 6 */
+typedef enum {
+	AB8500_CODEC_CR77_FADEDIS_AD1L_TO_HFL_ENABLED,
+	AB8500_CODEC_CR77_FADEDIS_AD1L_TO_HFL_DISABLED
+} t_ab8500_codec_cr77_fadedis_ad1l;
+
+/* CR77 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr77_ad1lbgain_to_hfl;
+
+/* CR78 - 6 */
+typedef enum {
+	AB8500_CODEC_CR78_FADEDIS_AD2L_TO_HFR_ENABLED,
+	AB8500_CODEC_CR78_FADEDIS_AD2L_TO_HFR_DISABLED
+} t_ab8500_codec_cr78_fadedis_ad2l;
+
+/* CR78 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr78_ad2lbgain_to_hfr;
+
+/* CR79 - 7 */
+typedef enum {
+	AB8500_CODEC_CR79_HSSINC1_SINC3_CHOOSEN,
+	AB8500_CODEC_CR79_HSSINC1_SINC1_CHOOSEN
+} t_ab8500_codec_cr79_hssinc1;
+
+/* CR79 - 4 */
+typedef enum {
+	AB8500_CODEC_CR79_FADEDIS_HSL_ENABLED,
+	AB8500_CODEC_CR79_FADEDIS_HSL_DISABLED
+} t_ab8500_codec_cr79_fadedis_hsl;
+
+/* CR79 - 3:0 */
+typedef t_uint8 t_ab8500_codec_cr79_hsldgain;
+
+/* CR80 - 7:6 */
+typedef enum {
+	AB8500_CODEC_CR80_FADE_SPEED_1MS,
+	AB8500_CODEC_CR80_FADE_SPEED_4MS,
+	AB8500_CODEC_CR80_FADE_SPEED_8MS,
+	AB8500_CODEC_CR80_FADE_SPEED_16MS,
+} t_ab8500_codec_cr80_fade_speed;
+
+/* CR80 - 4 */
+typedef enum {
+	AB8500_CODEC_CR80_FADEDIS_HSR_ENABLED,
+	AB8500_CODEC_CR80_FADEDIS_HSR_DISABLED
+} t_ab8500_codec_cr80_fadedis_hsr;
+
+/* CR80 - 3:0 */
+typedef t_uint8 t_ab8500_codec_cr80_hsrdgain;
+
+/* CR81 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr81_stfir1gain;
+
+/* CR82 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr82_stfir2gain;
+
+/* CR83 - 2 */
+typedef enum {
+	AB8500_CODEC_CR83_ENANC_DISABLED,
+	AB8500_CODEC_CR83_ENANC_ENABLED
+} t_ab8500_codec_cr83_enanc;
+
+/* CR83 - 1 */
+typedef enum {
+	AB8500_CODEC_CR83_ANCIIRINIT_NOT_STARTED,
+	AB8500_CODEC_CR83_ANCIIRINIT_STARTED
+} t_ab8500_codec_cr83_anciirinit;
+
+/* CR83 - 0 */
+typedef enum {
+	AB8500_CODEC_CR83_ANCFIRUPDATE_RESETTED,
+	AB8500_CODEC_CR83_ANCFIRUPDATE_NOT_RESETTED
+} t_ab8500_codec_cr83_ancfirupdate;
+
+/* CR84 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr84_ancinshift;
+
+/* CR85 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr85_ancfiroutshift;
+
+/* CR86 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr86_ancshiftout;
+
+/* CR87 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr87_ancfircoeff_msb;
+
+/* CR88 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr88_ancfircoeff_lsb;
+
+/* CR89 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr89_anciircoeff_msb;
+
+/* CR90 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr90_anciircoeff_lsb;
+
+/* CR91 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr91_ancwarpdel_msb;
+
+/* CR92 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr92_ancwarpdel_lsb;
+
+/* CR93 - Read Only */
+/* CR93 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr93_ancfirpeak_msb;
+
+/* CR94 - Read Only */
+/* CR94 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr94_ancfirpeak_lsb;
+
+/* CR95 - Read Only */
+/* CR95 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr95_anciirpeak_msb;
+
+/* CR96 - Read Only */
+/* CR96 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr96_anciirpeak_lsb;
+
+/* CR97 - 7 */
+typedef enum {
+	AB8500_CODEC_CR97_STFIR_SET_LAST_NOT_APPLIED,
+	AB8500_CODEC_CR97_STFIR_SET_LAST_APPLIED
+} t_ab8500_codec_cr97_stfir_set;
+
+/* CR97 - 6:0 */
+typedef t_uint8 t_ab8500_codec_cr97_stfir_addr;
+
+/* CR98 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr98_stfir_coeff_msb;
+
+/* CR99 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr99_stfir_coeff_lsb;
+
+/* CR100 - 2 */
+typedef enum {
+	AB8500_CODEC_CR100_ENSTFIRS_DISABLED,
+	AB8500_CODEC_CR100_ENSTFIRS_ENABLED
+} t_ab8500_codec_cr100_enstfirs;
+
+/* CR100 - 1 */
+typedef enum {
+	AB8500_CODEC_CR100_STFIRSTOIF1_AUD_IF0_DATA_RATE,
+	AB8500_CODEC_CR100_STFIRSTOIF1_AUD_IF1_DATA_RATE
+} t_ab8500_codec_cr100_stfirstoif1;
+
+/* CR100 - 0 */
+typedef enum {
+	AB8500_CODEC_CR100_STFIR_BUSY_READY,
+	AB8500_CODEC_CR100_STFIR_BUSY_NOT_READY
+} t_ab8500_codec_cr100_stfir_busy;
+
+/* CR101 - 7 */
+typedef enum {
+	AB8500_CODEC_CR101_HSOFFST_MASK_MASKED,
+	AB8500_CODEC_CR101_HSOFFST_MASK_ENABLED
+} t_ab8500_codec_cr101_hsoffst_mask;
+
+/* CR101 - 6 */
+typedef enum {
+	AB8500_CODEC_CR101_FIFOFULL_MASK_MASKED,
+	AB8500_CODEC_CR101_FIFOFULL_MASK_ENABLED
+} t_ab8500_codec_cr101_fifofull_mask;
+
+/* CR101 - 5 */
+typedef enum {
+	AB8500_CODEC_CR101_FIFOEMPTY_MASK_MASKED,
+	AB8500_CODEC_CR101_FIFOEMPTY_MASK_ENABLED
+} t_ab8500_codec_cr101_fifoempty_mask;
+
+/* CR101 - 4 */
+typedef enum {
+	AB8500_CODEC_CR101_DASAT_MASK_MASKED,
+	AB8500_CODEC_CR101_DASAT_MASK_ENABLED
+} t_ab8500_codec_cr101_dasat_mask;
+
+/* CR101 - 3 */
+typedef enum {
+	AB8500_CODEC_CR101_ADSAT_MASK_MASKED,
+	AB8500_CODEC_CR101_ADSAT_MASK_ENABLED
+} t_ab8500_codec_cr101_adsat_mask;
+
+/* CR101 - 2 */
+typedef enum {
+	AB8500_CODEC_CR101_ADDSP_MASK_MASKED,
+	AB8500_CODEC_CR101_ADDSP_MASK_ENABLED
+} t_ab8500_codec_cr101_addsp_mask;
+
+/* CR101 - 1 */
+typedef enum {
+	AB8500_CODEC_CR101_DADSP_MASK_MASKED,
+	AB8500_CODEC_CR101_DADSP_MASK_ENABLED
+} t_ab8500_codec_cr101_dadsp_mask;
+
+/* CR101 - 0 */
+typedef enum {
+	AB8500_CODEC_CR101_FIRSID_MASK_MASKED,
+	AB8500_CODEC_CR101_FIRSID_MASK_ENABLED
+} t_ab8500_codec_cr101_firsid_mask;
+
+/* CR102 - Read Only */
+/* CR102 - 7 */
+typedef enum {
+	AB8500_CODEC_CR102_IT_HSOFFST_ON,
+	AB8500_CODEC_CR102_IT_HSOFFST_OFF
+} t_ab8500_codec_cr102_it_hsoffst;
+
+/* CR102 - 6 */
+typedef enum {
+	AB8500_CODEC_CR102_IT_FIFOFULL_NOT_FULL,
+	AB8500_CODEC_CR102_IT_FIFOFULL_FULL
+} t_ab8500_codec_cr102_it_fifofull;
+
+/* CR102 - 5 */
+typedef enum {
+	AB8500_CODEC_CR102_IT_FIFOEMPTY_NOT_EMPTY,
+	AB8500_CODEC_CR102_IT_FIFOEMPTY_EMPTY
+} t_ab8500_codec_cr102_it_fifoempty;
+
+/* CR102 - 4 */
+typedef enum {
+	AB8500_CODEC_CR102_IT_DASAT_NO_SATURATION,
+	AB8500_CODEC_CR102_IT_DASAT_SATURATION
+} t_ab8500_codec_cr102_it_dasat;
+
+/* CR102 - 3 */
+typedef enum {
+	AB8500_CODEC_CR102_IT_ADSAT_NO_SATURATION,
+	AB8500_CODEC_CR102_IT_ADSAT_SATURATION
+} t_ab8500_codec_cr102_it_adsat;
+
+/* CR102 - 2 */
+typedef enum {
+	AB8500_CODEC_CR102_IT_ADDSP_NO_SATURATION,
+	AB8500_CODEC_CR102_IT_ADDSP_SATURATION
+} t_ab8500_codec_cr102_it_addsp;
+
+/* CR102 - 1 */
+typedef enum {
+	AB8500_CODEC_CR102_IT_DADSP_NO_SATURATION,
+	AB8500_CODEC_CR102_IT_DADSP_SATURATION
+} t_ab8500_codec_cr102_it_dadsp;
+
+/* CR102 - 0 */
+typedef enum {
+	AB8500_CODEC_CR102_IT_FIRSID_NO_SATURATION,
+	AB8500_CODEC_CR102_IT_FIRSID_SATURATION
+} t_ab8500_codec_cr102_it_firsid;
+
+/* CR103 - 7 */
+typedef enum {
+	AB8500_CODEC_CR103_VSSREADY_MASK_MASKED,
+	AB8500_CODEC_CR103_VSSREADY_MASK_ENABLED
+} t_ab8500_codec_cr103_vssready_mask;
+
+/* CR103 - 2 */
+typedef enum {
+	AB8500_CODEC_CR103_SHORTHSL_MASK_MASKED,
+	AB8500_CODEC_CR103_SHORTHSL_MASK_ENABLED
+} t_ab8500_codec_cr103_shorthsl_mask;
+
+/* CR103 - 1 */
+typedef enum {
+	AB8500_CODEC_CR103_SHORTHSR_MASK_MASKED,
+	AB8500_CODEC_CR103_SHORTHSR_MASK_ENABLED
+} t_ab8500_codec_cr103_shorthsr_mask;
+
+/* CR103 - 0 */
+typedef enum {
+	AB8500_CODEC_CR103_SHORTEAR_MASK_MASKED,
+	AB8500_CODEC_CR103_SHORTEAR_MASK_ENABLED
+} t_ab8500_codec_cr103_shortear_mask;
+
+/* CR104 - Read Only */
+/* CR104 - 7 */
+typedef enum {
+	AB8500_CODEC_CR104_IT_VSSREADY_NOT_READY,
+	AB8500_CODEC_CR104_IT_VSSREADY_READY
+} t_ab8500_codec_cr104_it_vssready;
+
+/* CR104 - 2 */
+typedef enum {
+	AB8500_CODEC_CR104_IT_SHORTHSL_NOT_DETECTED,
+	AB8500_CODEC_CR104_IT_SHORTHSL_DETECTED
+} t_ab8500_codec_cr104_it_shorthsl;
+
+/* CR104 - 1 */
+typedef enum {
+	AB8500_CODEC_CR104_IT_SHORTHSR_NOT_DETECTED,
+	AB8500_CODEC_CR104_IT_SHORTHSR_DETECTED
+} t_ab8500_codec_cr104_it_shorthsr;
+
+/* CR104 - 0 */
+typedef enum {
+	AB8500_CODEC_CR104_IT_SHORTEAR_NOT_DETECTED,
+	AB8500_CODEC_CR104_IT_SHORTEAR_DETECTED
+} t_ab8500_codec_cr104_it_shortear;
+
+/* CR105 - 7 */
+/* In ab8500_codec.h */
+
+/* CR105 - 5:0 */
+/* In ab8500_codec.h */
+
+/* CR106 - 7:0 */
+/* In ab8500_codec.h */
+
+/* CR107 - 7:0 */
+/* In ab8500_codec.h */
+
+/* CR108 - 7:0 */
+/* In ab8500_codec.h */
+
+/* CR109 - 7:0 */
+/* In ab8500_codec.h */
+
+/* CR110 - Read Only */
+/* CR110 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr110_bfifosamples;
+
+/* CR111 - Read Only */
+/* CR111 - 4:0 */
+typedef t_uint8 t_ab8500_codec_cr111_aud_ip_rev;
+
+/* CR27 - 6:5 */
+typedef enum {
+	AB8500_CODEC_CR27_IF1_BITCLK_OSR_32,
+	AB8500_CODEC_CR27_IF1_BITCLK_OSR_64,
+	AB8500_CODEC_CR27_IF1_BITCLK_OSR_128,
+	AB8500_CODEC_CR27_IF1_BITCLK_OSR_256
+} t_ab8500_codec_cr27_if1_bitclk_osr;
+
+/* CR27 - 2:1 */
+typedef enum {
+	AB8500_CODEC_CR27_IF0_BITCLK_OSR_32,
+	AB8500_CODEC_CR27_IF0_BITCLK_OSR_64,
+	AB8500_CODEC_CR27_IF0_BITCLK_OSR_128,
+	AB8500_CODEC_CR27_IF0_BITCLK_OSR_256
+} t_ab8500_codec_cr27_if0_bitclk_osr;
+
+/* CR28 - 1:0 */
+typedef enum {
+	AB8500_CODEC_CR28_IF0WL_16BITS,
+	AB8500_CODEC_CR28_IF0WL_20BITS,
+	AB8500_CODEC_CR28_IF0WL_24BITS,
+	AB8500_CODEC_CR28_IF0WL_32BITS
+} t_ab8500_codec_cr28_if0wl;
+
+/* CR30 - 1:0 */
+typedef enum {
+	AB8500_CODEC_CR30_IF1WL_16BITS,
+	AB8500_CODEC_CR30_IF1WL_20BITS,
+	AB8500_CODEC_CR30_IF1WL_24BITS,
+	AB8500_CODEC_CR30_IF1WL_32BITS
+} t_ab8500_codec_cr30_if1wl;
+
+/* CR105 - 7 */
+typedef enum {
+	AB8500_CODEC_CR105_BFIFOMSK_AD_DATA0_UNMASKED,
+	AB8500_CODEC_CR105_BFIFOMSK_AD_DATA0_MASKED
+} t_ab8500_codec_cr105_bfifomsk;
+
+/* CR105 - 5:0 */
+typedef t_uint8 t_ab8500_codec_cr105_bfifoint;
+
+/* CR106 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr106_bfifotx;
+
+/* CR107 - 7:5 */
+typedef enum {
+	AB8500_CODEC_CR107_BFIFOEXSL_0_EXTRA_SLOT,
+	AB8500_CODEC_CR107_BFIFOEXSL_1_EXTRA_SLOT,
+	AB8500_CODEC_CR107_BFIFOEXSL_2_EXTRA_SLOT,
+	AB8500_CODEC_CR107_BFIFOEXSL_3_EXTRA_SLOT,
+	AB8500_CODEC_CR107_BFIFOEXSL_4_EXTRA_SLOT,
+	AB8500_CODEC_CR107_BFIFOEXSL_5_EXTRA_SLOT,
+	AB8500_CODEC_CR107_BFIFOEXSL_6_EXTRA_SLOT,
+} t_ab8500_codec_cr107_bfifoexsl;
+
+/* CR107 - 4:2 */
+typedef enum {
+	AB8500_CODEC_CR107_PREBITCLK0_0_EXTRA_CLK,
+	AB8500_CODEC_CR107_PREBITCLK0_1_EXTRA_CLK,
+	AB8500_CODEC_CR107_PREBITCLK0_2_EXTRA_CLK,
+	AB8500_CODEC_CR107_PREBITCLK0_3_EXTRA_CLK,
+	AB8500_CODEC_CR107_PREBITCLK0_4_EXTRA_CLK,
+	AB8500_CODEC_CR107_PREBITCLK0_5_EXTRA_CLK,
+	AB8500_CODEC_CR107_PREBITCLK0_6_EXTRA_CLK,
+	AB8500_CODEC_CR107_PREBITCLK0_7_EXTRA_CLK
+} t_ab8500_codec_cr107_prebitclk0;
+
+/* CR107 - 1 */
+typedef enum {
+	AB8500_CODEC_CR107_BFIFOMAST_SLAVE_MODE,
+	AB8500_CODEC_CR107_BFIFOMAST_MASTER_MODE
+} t_ab8500_codec_cr107_bfifomast;
+
+/* CR107 - 0 */
+typedef enum {
+	AB8500_CODEC_CR107_BFIFORUN_STOPPED,
+	AB8500_CODEC_CR107_BFIFORUN_RUNNING
+} t_ab8500_codec_cr107_bfiforun;
+
+/* CR108 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr108_bfifoframsw;
+
+/* CR109 - 7:0 */
+typedef t_uint8 t_ab8500_codec_cr109_bfifowakeup;
+
+typedef enum {
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT1,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT2,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT3,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT4,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT5,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT6,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT7,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT8,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_ZEROS,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_IS_TRISTATE = 15,
+	AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_UNDEFINED
+} t_ab8500_codec_cr31_to_cr46_ad_data_allocation;
+
+typedef enum {
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT00,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT01,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT02,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT03,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT04,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT05,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT06,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT07,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT08,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT09,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT10,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT11,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT12,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT13,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT14,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT15,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT16,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT17,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT18,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT19,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT20,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT21,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT22,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT23,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT24,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT25,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT26,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT27,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT28,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT29,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT30,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT31,
+	AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT_UNDEFINED
+} t_ab8500_codec_cr51_to_cr58_sltoda;
+
+/*configuration structure for AB8500 Codec*/
+typedef struct {
+	/* CR0  */
+	t_ab8500_codec_cr0_powerup cr0_powerup;
+	t_ab8500_codec_cr0_enaana cr0_enaana;
+
+	/* CR1  */
+	t_ab8500_codec_cr1_swreset cr1_swreset;
+
+	/* CR2  */
+	t_ab8500_codec_cr2_enad1 cr2_enad1;
+	t_ab8500_codec_cr2_enad2 cr2_enad2;
+	t_ab8500_codec_cr2_enad3 cr2_enad3;
+	t_ab8500_codec_cr2_enad4 cr2_enad4;
+	t_ab8500_codec_cr2_enad5 cr2_enad5;
+	t_ab8500_codec_cr2_enad6 cr2_enad6;
+
+	/* CR3  */
+	t_ab8500_codec_cr3_enda1 cr3_enda1;
+	t_ab8500_codec_cr3_enda2 cr3_enda2;
+	t_ab8500_codec_cr3_enda3 cr3_enda3;
+	t_ab8500_codec_cr3_enda4 cr3_enda4;
+	t_ab8500_codec_cr3_enda5 cr3_enda5;
+	t_ab8500_codec_cr3_enda6 cr3_enda6;
+
+	/* CR4  */
+	t_ab8500_codec_cr4_lowpowhs cr4_lowpowhs;
+	t_ab8500_codec_cr4_lowpowdachs cr4_lowpowdachs;
+	t_ab8500_codec_cr4_lowpowear cr4_lowpowear;
+	t_ab8500_codec_cr4_ear_sel_cm cr4_ear_sel_cm;
+	t_ab8500_codec_cr4_hs_hp_en cr4_hs_hp_en;
+
+	/* CR5  */
+	t_ab8500_codec_cr5_enmic1 cr5_enmic1;
+	t_ab8500_codec_cr5_enmic2 cr5_enmic2;
+	t_ab8500_codec_cr5_enlinl cr5_enlinl;
+	t_ab8500_codec_cr5_enlinr cr5_enlinr;
+	t_ab8500_codec_cr5_mutmic1 cr5_mutmic1;
+	t_ab8500_codec_cr5_mutmic2 cr5_mutmic2;
+	t_ab8500_codec_cr5_mutlinl cr5_mutlinl;
+	t_ab8500_codec_cr5_mutlinr cr5_mutlinr;
+
+	/* CR6  */
+	t_ab8500_codec_cr6_endmic1 cr6_endmic1;
+	t_ab8500_codec_cr6_endmic2 cr6_endmic2;
+	t_ab8500_codec_cr6_endmic3 cr6_endmic3;
+	t_ab8500_codec_cr6_endmic4 cr6_endmic4;
+	t_ab8500_codec_cr6_endmic5 cr6_endmic5;
+	t_ab8500_codec_cr6_endmic6 cr6_endmic6;
+
+	/* CR7  */
+	t_ab8500_codec_cr7_mic1sel cr7_mic1sel;
+	t_ab8500_codec_cr7_linrsel cr7_linrsel;
+	t_ab8500_codec_cr7_endrvhsl cr7_endrvhsl;
+	t_ab8500_codec_cr7_endrvhsr cr7_endrvhsr;
+	t_ab8500_codec_cr7_enadcmic cr7_enadcmic;
+	t_ab8500_codec_cr7_enadclinl cr7_enadclinl;
+	t_ab8500_codec_cr7_enadclinr cr7_enadclinr;
+
+	/* CR8  */
+	t_ab8500_codec_cr8_cp_dis_pldwn cr8_cp_dis_pldwn;
+	t_ab8500_codec_cr8_enear cr8_enear;
+	t_ab8500_codec_cr8_enhsl cr8_enhsl;
+	t_ab8500_codec_cr8_enhsr cr8_enhsr;
+	t_ab8500_codec_cr8_enhfl cr8_enhfl;
+	t_ab8500_codec_cr8_enhfr cr8_enhfr;
+	t_ab8500_codec_cr8_envibl cr8_envibl;
+	t_ab8500_codec_cr8_envibr cr8_envibr;
+
+	/* CR9  */
+	t_ab8500_codec_cr9_endacear cr9_endacear;
+	t_ab8500_codec_cr9_endachsl cr9_endachsl;
+	t_ab8500_codec_cr9_endachsr cr9_endachsr;
+	t_ab8500_codec_cr9_endachfl cr9_endachfl;
+	t_ab8500_codec_cr9_endachfr cr9_endachfr;
+	t_ab8500_codec_cr9_endacvibl cr9_endacvibl;
+	t_ab8500_codec_cr9_endacvibr cr9_endacvibr;
+
+	/* CR10  */
+	t_ab8500_codec_cr10_muteear cr10_muteear;
+	t_ab8500_codec_cr10_mutehsl cr10_mutehsl;
+	t_ab8500_codec_cr10_mutehsr cr10_mutehsr;
+
+	/* CR11  */
+	t_ab8500_codec_cr11_enshortpwd cr11_enshortpwd;
+	t_ab8500_codec_cr11_earshortdis cr11_earshortdis;
+	t_ab8500_codec_cr11_hsshortdis cr11_hsshortdis;
+	t_ab8500_codec_cr11_hspullden cr11_hspullden;
+	t_ab8500_codec_cr11_hsoscen cr11_hsoscen;
+	t_ab8500_codec_cr11_hsfaden cr11_hsfaden;
+	t_ab8500_codec_cr11_hszcddis cr11_hszcddis;
+
+	/* CR12  */
+	t_ab8500_codec_cr12_encphs cr12_encphs;
+	t_ab8500_codec_cr12_hsautoen cr12_hsautoen;
+
+	/* CR13  */
+	t_ab8500_codec_cr13_envdet_hthresh cr13_envdet_hthresh;
+	t_ab8500_codec_cr13_envdet_lthresh cr13_envdet_lthresh;
+
+	/* CR14  */
+	t_ab8500_codec_cr14_smpslven cr14_smpslven;
+	t_ab8500_codec_cr14_envdetsmpsen cr14_envdetsmpsen;
+	t_ab8500_codec_cr14_cplven cr14_cplven;
+	t_ab8500_codec_cr14_envdetcpen cr14_envdetcpen;
+	t_ab8500_codec_cr14_envet_time cr14_envet_time;
+
+	/* CR15  */
+	t_ab8500_codec_cr15_pwmtovibl cr15_pwmtovibl;
+	t_ab8500_codec_cr15_pwmtovibr cr15_pwmtovibr;
+	t_ab8500_codec_cr15_pwmlctrl cr15_pwmlctrl;
+	t_ab8500_codec_cr15_pwmrctrl cr15_pwmrctrl;
+	t_ab8500_codec_cr15_pwmnlctrl cr15_pwmnlctrl;
+	t_ab8500_codec_cr15_pwmplctrl cr15_pwmplctrl;
+	t_ab8500_codec_cr15_pwmnrctrl cr15_pwmnrctrl;
+	t_ab8500_codec_cr15_pwmprctrl cr15_pwmprctrl;
+
+	/* CR16  */
+	t_ab8500_codec_cr16_pwmnlpol cr16_pwmnlpol;
+	t_ab8500_codec_cr16_pwmnldutycycle cr16_pwmnldutycycle;
+
+	/* CR17  */
+	t_ab8500_codec_cr17_pwmplpol cr17_pwmplpol;
+	t_ab8500_codec_cr17_pwmpldutycycle cr17_pwmpldutycycle;
+
+	/* CR18  */
+	t_ab8500_codec_cr18_pwmnrpol cr18_pwmnrpol;
+	t_ab8500_codec_cr18_pwmnrdutycycle cr18_pwmnrdutycycle;
+
+	/* CR19  */
+	t_ab8500_codec_cr19_pwmprpol cr19_pwmprpol;
+	t_ab8500_codec_cr19_pwmprdutycycle cr19_pwmprdutycycle;
+
+	/* CR20  */
+	t_ab8500_codec_cr20_en_se_mic1 cr20_en_se_mic1;
+	t_ab8500_codec_cr20_low_pow_mic1 cr20_low_pow_mic1;
+	t_ab8500_codec_cr20_mic1_gain cr20_mic1_gain;
+
+	/* CR21  */
+	t_ab8500_codec_cr21_en_se_mic2 cr21_en_se_mic2;
+	t_ab8500_codec_cr21_low_pow_mic2 cr21_low_pow_mic2;
+	t_ab8500_codec_cr21_mic2_gain cr21_mic2_gain;
+
+	/* CR22  */
+	t_ab8500_codec_cr22_hsl_gain cr22_hsl_gain;
+	t_ab8500_codec_cr22_hsr_gain cr22_hsr_gain;
+
+	/* CR23  */
+	t_ab8500_codec_cr23_linl_gain cr23_linl_gain;
+	t_ab8500_codec_cr23_linr_gain cr23_linr_gain;
+
+	/* CR24  */
+	t_ab8500_codec_cr24_lintohsl_gain cr24_lintohsl_gain;
+
+	/* CR25  */
+	t_ab8500_codec_cr25_lintohsr_gain cr25_lintohsr_gain;
+
+	/* CR26  */
+	t_ab8500_codec_cr26_ad1nh cr26_ad1nh;
+	t_ab8500_codec_cr26_ad2nh cr26_ad2nh;
+	t_ab8500_codec_cr26_ad3nh cr26_ad3nh;
+	t_ab8500_codec_cr26_ad4nh cr26_ad4nh;
+	t_ab8500_codec_cr26_ad1_voice cr26_ad1_voice;
+	t_ab8500_codec_cr26_ad2_voice cr26_ad2_voice;
+	t_ab8500_codec_cr26_ad3_voice cr26_ad3_voice;
+	t_ab8500_codec_cr26_ad4_voice cr26_ad4_voice;
+
+	/* CR27  */
+	t_ab8500_codec_cr27_en_mastgen cr27_en_mastgen;
+	t_ab8500_codec_cr27_if1_bitclk_osr cr27_if1_bitclk_osr;
+	t_ab8500_codec_cr27_enfs_bitclk1 cr27_enfs_bitclk1;
+	t_ab8500_codec_cr27_if0_bitclk_osr cr27_if0_bitclk_osr;
+	t_ab8500_codec_cr27_enfs_bitclk0 cr27_enfs_bitclk0;
+
+	/* CR28  */
+	t_ab8500_codec_cr28_fsync0p cr28_fsync0p;
+	t_ab8500_codec_cr28_bitclk0p cr28_bitclk0p;
+	t_ab8500_codec_cr28_if0del cr28_if0del;
+	t_ab8500_codec_cr28_if0format cr28_if0format;
+	t_ab8500_codec_cr28_if0wl cr28_if0wl;
+
+	/* CR29  */
+	t_ab8500_codec_cr29_if0datoif1ad cr29_if0datoif1ad;
+	t_ab8500_codec_cr29_if0cktoif1ck cr29_if0cktoif1ck;
+	t_ab8500_codec_cr29_if1master cr29_if1master;
+	t_ab8500_codec_cr29_if1datoif0ad cr29_if1datoif0ad;
+	t_ab8500_codec_cr29_if1cktoif0ck cr29_if1cktoif0ck;
+	t_ab8500_codec_cr29_if0master cr29_if0master;
+	t_ab8500_codec_cr29_if0bfifoen cr29_if0bfifoen;
+
+	/* CR30  */
+	t_ab8500_codec_cr30_fsync1p cr30_fsync1p;
+	t_ab8500_codec_cr30_bitclk1p cr30_bitclk1p;
+	t_ab8500_codec_cr30_if1del cr30_if1del;
+	t_ab8500_codec_cr30_if1format cr30_if1format;
+	t_ab8500_codec_cr30_if1wl cr30_if1wl;
+
+	/* CR31  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr31_adotoslot1;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr31_adotoslot0;
+
+	/* CR32  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr32_adotoslot3;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr32_adotoslot2;
+
+	/* CR33  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr33_adotoslot5;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr33_adotoslot4;
+
+	/* CR34  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr34_adotoslot7;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr34_adotoslot6;
+
+	/* CR35  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr35_adotoslot9;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr35_adotoslot8;
+
+	/* CR36  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr36_adotoslot11;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr36_adotoslot10;
+
+	/* CR37  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr37_adotoslot13;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr37_adotoslot12;
+
+	/* CR38  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr38_adotoslot15;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr38_adotoslot14;
+
+	/* CR39  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr39_adotoslot17;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr39_adotoslot16;
+
+	/* CR40  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr40_adotoslot19;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr40_adotoslot18;
+
+	/* CR41  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr41_adotoslot21;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr41_adotoslot20;
+
+	/* CR42  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr42_adotoslot23;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr42_adotoslot22;
+
+	/* CR43  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr43_adotoslot25;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr43_adotoslot24;
+
+	/* CR44  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr44_adotoslot27;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr44_adotoslot26;
+
+	/* CR45  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr45_adotoslot29;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr45_adotoslot28;
+
+	/* CR46  */
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr46_adotoslot31;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation cr46_adotoslot30;
+
+	/* CR47  */
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl7;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl6;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl5;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl4;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl3;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl2;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl1;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr47_hiz_sl0;
+
+	/* CR48  */
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl15;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl14;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl13;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl12;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl11;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl10;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl9;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr48_hiz_sl8;
+
+	/* CR49  */
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl23;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl22;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl21;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl20;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl19;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl18;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl17;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr49_hiz_sl16;
+
+	/* CR50  */
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl31;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl30;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl29;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl28;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl27;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl26;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl25;
+	t_ab8500_codec_cr47_to_cr50_hiz_sl cr50_hiz_sl24;
+
+	/* CR51  */
+	t_ab8500_codec_cr51_da12_voice cr51_da12_voice;
+	t_ab8500_codec_cr51_swapda12_34 cr51_swapda12_34;
+	t_ab8500_codec_cr51_sldai7toslado1 cr51_sldai7toslado1;
+	t_ab8500_codec_cr51_to_cr58_sltoda cr51_sltoda1;
+
+	/* CR52  */
+	t_ab8500_codec_cr52_sldai8toslado2 cr52_sldai8toslado2;
+	t_ab8500_codec_cr51_to_cr58_sltoda cr52_sltoda2;
+
+	/* CR53  */
+	t_ab8500_codec_cr53_da34_voice cr53_da34_voice;
+	t_ab8500_codec_cr53_sldai7toslado3 cr53_sldai7toslado3;
+	t_ab8500_codec_cr51_to_cr58_sltoda cr53_sltoda3;
+
+	/* CR54  */
+	t_ab8500_codec_cr54_sldai8toslado4 cr54_sldai8toslado4;
+	t_ab8500_codec_cr51_to_cr58_sltoda cr54_sltoda4;
+
+	/* CR55  */
+	t_ab8500_codec_cr55_da56_voice cr55_da56_voice;
+	t_ab8500_codec_cr55_sldai7toslado5 cr55_sldai7toslado5;
+	t_ab8500_codec_cr51_to_cr58_sltoda cr55_sltoda5;
+
+	/* CR56  */
+	t_ab8500_codec_cr56_sldai8toslado6 cr56_sldai8toslado6;
+	t_ab8500_codec_cr51_to_cr58_sltoda cr56_sltoda6;
+
+	/* CR57  */
+	t_ab8500_codec_cr57_sldai8toslado7 cr57_sldai8toslado7;
+	t_ab8500_codec_cr51_to_cr58_sltoda cr57_sltoda7;
+
+	/* CR58  */
+	t_ab8500_codec_cr58_sldai7toslado8 cr58_sldai7toslado8;
+	t_ab8500_codec_cr51_to_cr58_sltoda cr58_sltoda8;
+
+	/* CR59  */
+	t_ab8500_codec_cr59_parlhf cr59_parlhf;
+	t_ab8500_codec_cr59_parlvib cr59_parlvib;
+	t_ab8500_codec_cr59_classdvib1_swapen cr59_classdvib1_swapen;
+	t_ab8500_codec_cr59_classdvib2_swapen cr59_classdvib2_swapen;
+	t_ab8500_codec_cr59_classdhfl_swapen cr59_classdhfl_swapen;
+	t_ab8500_codec_cr59_classdhfr_swapen cr59_classdhfr_swapen;
+
+	/* CR60  */
+	t_ab8500_codec_cr60_classd_firbyp cr60_classd_firbyp;
+	t_ab8500_codec_cr60_classd_highvolen cr60_classd_highvolen;
+
+	/* CR61  */
+	t_ab8500_codec_cr61_classddith_hpgain cr61_classddith_hpgain;
+	t_ab8500_codec_cr61_classddith_wgain cr61_classddith_wgain;
+
+	/* CR62  */
+	t_ab8500_codec_cr62_dmic1sinc3 cr62_dmic1sinc3;
+	t_ab8500_codec_cr62_dmic2sinc3 cr62_dmic2sinc3;
+	t_ab8500_codec_cr62_dmic3sinc3 cr62_dmic3sinc3;
+	t_ab8500_codec_cr62_dmic4sinc3 cr62_dmic4sinc3;
+	t_ab8500_codec_cr62_dmic5sinc3 cr62_dmic5sinc3;
+	t_ab8500_codec_cr62_dmic6sinc3 cr62_dmic6sinc3;
+
+	/* CR63  */
+	t_ab8500_codec_cr63_datohslen cr63_datohslen;
+	t_ab8500_codec_cr63_datohsren cr63_datohsren;
+	t_ab8500_codec_cr63_ad1sel cr63_ad1sel;
+	t_ab8500_codec_cr63_ad2sel cr63_ad2sel;
+	t_ab8500_codec_cr63_ad3sel cr63_ad3sel;
+	t_ab8500_codec_cr63_ad5sel cr63_ad5sel;
+	t_ab8500_codec_cr63_ad6sel cr63_ad6sel;
+	t_ab8500_codec_cr63_ancsel cr63_ancsel;
+
+	/* CR64  */
+	t_ab8500_codec_cr64_datohfren cr64_datohfren;
+	t_ab8500_codec_cr64_datohflen cr64_datohflen;
+	t_ab8500_codec_cr64_hfrsel cr64_hfrsel;
+	t_ab8500_codec_cr64_hflsel cr64_hflsel;
+	t_ab8500_codec_cr64_stfir1sel cr64_stfir1sel;
+	t_ab8500_codec_cr64_stfir2sel cr64_stfir2sel;
+
+	/* CR65  */
+	t_ab8500_codec_cr65_fadedis_ad1 cr65_fadedis_ad1;
+	t_ab8500_codec_cr65_ad1gain cr65_ad1gain;
+
+	/* CR66  */
+	t_ab8500_codec_cr66_fadedis_ad2 cr66_fadedis_ad2;
+	t_ab8500_codec_cr66_ad2gain cr66_ad2gain;
+
+	/* CR67  */
+	t_ab8500_codec_cr67_fadedis_ad3 cr67_fadedis_ad3;
+	t_ab8500_codec_cr67_ad3gain cr67_ad3gain;
+
+	/* CR68  */
+	t_ab8500_codec_cr68_fadedis_ad4 cr68_fadedis_ad4;
+	t_ab8500_codec_cr68_ad4gain cr68_ad4gain;
+
+	/* CR69  */
+	t_ab8500_codec_cr69_fadedis_ad5 cr69_fadedis_ad5;
+	t_ab8500_codec_cr69_ad5gain cr69_ad5gain;
+
+	/* CR70  */
+	t_ab8500_codec_cr70_fadedis_ad6 cr70_fadedis_ad6;
+	t_ab8500_codec_cr70_ad6gain cr70_ad6gain;
+
+	/* CR71  */
+	t_ab8500_codec_cr71_fadedis_da1 cr71_fadedis_da1;
+	t_ab8500_codec_cr71_da1gain cr71_da1gain;
+
+	/* CR72  */
+	t_ab8500_codec_cr72_fadedis_da2 cr72_fadedis_da2;
+	t_ab8500_codec_cr72_da2gain cr72_da2gain;
+
+	/* CR73  */
+	t_ab8500_codec_cr73_fadedis_da3 cr73_fadedis_da3;
+	t_ab8500_codec_cr73_da3gain cr73_da3gain;
+
+	/* CR74  */
+	t_ab8500_codec_cr74_fadedis_da4 cr74_fadedis_da4;
+	t_ab8500_codec_cr74_da4gain cr74_da4gain;
+
+	/* CR75  */
+	t_ab8500_codec_cr75_fadedis_da5 cr75_fadedis_da5;
+	t_ab8500_codec_cr75_da5gain cr75_da5gain;
+
+	/* CR76  */
+	t_ab8500_codec_cr76_fadedis_da6 cr76_fadedis_da6;
+	t_ab8500_codec_cr76_da6gain cr76_da6gain;
+
+	/* CR77  */
+	t_ab8500_codec_cr77_fadedis_ad1l cr77_fadedis_ad1l;
+	t_ab8500_codec_cr77_ad1lbgain_to_hfl cr77_ad1lbgain_to_hfl;
+
+	/* CR78  */
+	t_ab8500_codec_cr78_fadedis_ad2l cr78_fadedis_ad2l;
+	t_ab8500_codec_cr78_ad2lbgain_to_hfr cr78_ad2lbgain_to_hfr;
+
+	/* CR79  */
+	t_ab8500_codec_cr79_hssinc1 cr79_hssinc1;
+	t_ab8500_codec_cr79_fadedis_hsl cr79_fadedis_hsl;
+	t_ab8500_codec_cr79_hsldgain cr79_hsldgain;
+
+	/* CR80  */
+	t_ab8500_codec_cr80_fade_speed cr80_fade_speed;
+	t_ab8500_codec_cr80_fadedis_hsr cr80_fadedis_hsr;
+	t_ab8500_codec_cr80_hsrdgain cr80_hsrdgain;
+
+	/* CR81  */
+	t_ab8500_codec_cr81_stfir1gain cr81_stfir1gain;
+
+	/* CR82  */
+	t_ab8500_codec_cr82_stfir2gain cr82_stfir2gain;
+
+	/* CR83  */
+	t_ab8500_codec_cr83_enanc cr83_enanc;
+	t_ab8500_codec_cr83_anciirinit cr83_anciirinit;
+	t_ab8500_codec_cr83_ancfirupdate cr83_ancfirupdate;
+
+	/* CR84  */
+	t_ab8500_codec_cr84_ancinshift cr84_ancinshift;
+
+	/* CR85  */
+	t_ab8500_codec_cr85_ancfiroutshift cr85_ancfiroutshift;
+
+	/* CR86  */
+	t_ab8500_codec_cr86_ancshiftout cr86_ancshiftout;
+
+	/* CR87  */
+	t_ab8500_codec_cr87_ancfircoeff_msb cr87_ancfircoeff_msb;
+
+	/* CR88  */
+	t_ab8500_codec_cr88_ancfircoeff_lsb cr88_ancfircoeff_lsb;
+
+	/* CR89  */
+	t_ab8500_codec_cr89_anciircoeff_msb cr89_anciircoeff_msb;
+
+	/* CR90  */
+	t_ab8500_codec_cr90_anciircoeff_lsb cr90_anciircoeff_lsb;
+
+	/* CR91  */
+	t_ab8500_codec_cr91_ancwarpdel_msb cr91_ancwarpdel_msb;
+
+	/* CR92  */
+	t_ab8500_codec_cr92_ancwarpdel_lsb cr92_ancwarpdel_lsb;
+
+	/* CR93  */
+	t_ab8500_codec_cr93_ancfirpeak_msb cr93_ancfirpeak_msb;
+
+	/* CR94  */
+	t_ab8500_codec_cr94_ancfirpeak_lsb cr94_ancfirpeak_lsb;
+
+	/* CR95  */
+	t_ab8500_codec_cr95_anciirpeak_msb cr95_anciirpeak_msb;
+
+	/* CR96  */
+	t_ab8500_codec_cr96_anciirpeak_lsb cr96_anciirpeak_lsb;
+
+	/* CR97  */
+	t_ab8500_codec_cr97_stfir_set cr97_stfir_set;
+	t_ab8500_codec_cr97_stfir_addr cr97_stfir_addr;
+
+	/* CR98  */
+	t_ab8500_codec_cr98_stfir_coeff_msb cr98_stfir_coeff_msb;
+
+	/* CR99  */
+	t_ab8500_codec_cr99_stfir_coeff_lsb cr99_stfir_coeff_lsb;
+
+	/* CR100  */
+	t_ab8500_codec_cr100_enstfirs cr100_enstfirs;
+	t_ab8500_codec_cr100_stfirstoif1 cr100_stfirstoif1;
+	t_ab8500_codec_cr100_stfir_busy cr100_stfir_busy;
+
+	/* CR101  */
+	t_ab8500_codec_cr101_hsoffst_mask cr101_hsoffst_mask;
+	t_ab8500_codec_cr101_fifofull_mask cr101_fifofull_mask;
+	t_ab8500_codec_cr101_fifoempty_mask cr101_fifoempty_mask;
+	t_ab8500_codec_cr101_dasat_mask cr101_dasat_mask;
+	t_ab8500_codec_cr101_adsat_mask cr101_adsat_mask;
+	t_ab8500_codec_cr101_addsp_mask cr101_addsp_mask;
+	t_ab8500_codec_cr101_dadsp_mask cr101_dadsp_mask;
+	t_ab8500_codec_cr101_firsid_mask cr101_firsid_mask;
+
+	/* CR102  */
+	t_ab8500_codec_cr102_it_hsoffst cr102_it_hsoffst;
+	t_ab8500_codec_cr102_it_fifofull cr102_it_fifofull;
+	t_ab8500_codec_cr102_it_fifoempty cr102_it_fifoempty;
+	t_ab8500_codec_cr102_it_dasat cr102_it_dasat;
+	t_ab8500_codec_cr102_it_adsat cr102_it_adsat;
+	t_ab8500_codec_cr102_it_addsp cr102_it_addsp;
+	t_ab8500_codec_cr102_it_dadsp cr102_it_dadsp;
+	t_ab8500_codec_cr102_it_firsid cr102_it_firsid;
+
+	/* CR103  */
+	t_ab8500_codec_cr103_vssready_mask cr103_vssready_mask;
+	t_ab8500_codec_cr103_shorthsl_mask cr103_shorthsl_mask;
+	t_ab8500_codec_cr103_shorthsr_mask cr103_shorthsr_mask;
+	t_ab8500_codec_cr103_shortear_mask cr103_shortear_mask;
+
+	/* CR104  */
+	t_ab8500_codec_cr104_it_vssready cr104_it_vssready;
+	t_ab8500_codec_cr104_it_shorthsl cr104_it_shorthsl;
+	t_ab8500_codec_cr104_it_shorthsr cr104_it_shorthsr;
+	t_ab8500_codec_cr104_it_shortear cr104_it_shortear;
+
+	/* CR105  */
+	t_ab8500_codec_cr105_bfifomsk cr105_bfifomsk;
+	t_ab8500_codec_cr105_bfifoint cr105_bfifoint;
+
+	/* CR106  */
+	t_ab8500_codec_cr106_bfifotx cr106_bfifotx;
+
+	/* CR107  */
+	t_ab8500_codec_cr107_bfifoexsl cr107_bfifoexsl;
+	t_ab8500_codec_cr107_prebitclk0 cr107_prebitclk0;
+	t_ab8500_codec_cr107_bfifomast cr107_bfifomast;
+	t_ab8500_codec_cr107_bfiforun cr107_bfiforun;
+
+	/* CR108  */
+	t_ab8500_codec_cr108_bfifoframsw cr108_bfifoframsw;
+
+	/* CR109  */
+	t_ab8500_codec_cr109_bfifowakeup cr109_bfifowakeup;
+
+	/* CR110  */
+	t_ab8500_codec_cr110_bfifosamples cr110_bfifosamples;
+
+	/* CR111  */
+	t_ab8500_codec_cr111_aud_ip_rev cr111_aud_ip_rev;
+
+} t_ab8500_codec_configuration;
+
+typedef enum {
+	AB8500_CODEC_DIRECTION_IN,
+	AB8500_CODEC_DIRECTION_OUT,
+	AB8500_CODEC_DIRECTION_INOUT
+} t_ab8500_codec_direction;
+
+typedef enum {
+	AB8500_CODEC_AUDIO_INTERFACE_0,
+	AB8500_CODEC_AUDIO_INTERFACE_1
+} t_ab8500_codec_audio_interface;
+
+typedef enum {
+	AB8500_CODEC_MODE_HIFI,
+	AB8500_CODEC_MODE_VOICE,
+	AB8500_CODEC_MODE_MANUAL_SETTING
+} t_ab8500_codec_mode;
+
+typedef enum {
+	AB8500_CODEC_DEST_HEADSET,
+	AB8500_CODEC_DEST_EARPIECE,
+	AB8500_CODEC_DEST_HANDSFREE,
+	AB8500_CODEC_DEST_VIBRATOR_L,
+	AB8500_CODEC_DEST_VIBRATOR_R,
+	AB8500_CODEC_DEST_FM_TX,
+	AB8500_CODEC_DEST_ALL
+} t_ab8500_codec_dest;
+
+typedef enum {
+	AB8500_CODEC_SRC_LINEIN,
+	AB8500_CODEC_SRC_MICROPHONE_1A,
+	AB8500_CODEC_SRC_MICROPHONE_1B,
+	AB8500_CODEC_SRC_MICROPHONE_2,
+	AB8500_CODEC_SRC_D_MICROPHONE_1,
+	AB8500_CODEC_SRC_D_MICROPHONE_2,
+	AB8500_CODEC_SRC_D_MICROPHONE_3,
+	AB8500_CODEC_SRC_D_MICROPHONE_4,
+	AB8500_CODEC_SRC_D_MICROPHONE_5,
+	AB8500_CODEC_SRC_D_MICROPHONE_6,
+	AB8500_CODEC_SRC_D_MICROPHONE_12,
+	AB8500_CODEC_SRC_D_MICROPHONE_34,
+	AB8500_CODEC_SRC_D_MICROPHONE_56,
+	AB8500_CODEC_SRC_FM_RX,
+	AB8500_CODEC_SRC_ALL
+} t_ab8500_codec_src;
+
+typedef struct {
+	t_uint8 slave_address_of_ab8500_codec;
+	t_ab8500_codec_direction ab8500_codec_direction;
+	t_ab8500_codec_mode ab8500_codec_mode_in;
+	t_ab8500_codec_mode ab8500_codec_mode_out;
+	t_ab8500_codec_audio_interface audio_interface;
+	t_ab8500_codec_src ab8500_codec_src;
+	t_ab8500_codec_dest ab8500_codec_dest;
+	t_uint8 in_left_volume;
+	t_uint8 in_right_volume;
+	t_uint8 out_left_volume;
+	t_uint8 out_right_volume;
+
+	t_ab8500_codec_configuration ab8500_codec_configuration;
+} t_ab8500_codec_system_context;
+
+#endif /* _AB8500_CODECP_H_ */
+
+/* End of file AB8500_CODECP.h */
diff --git a/arch/arm/mach-ux500/include/mach/ab8500_codec_v1_0.h b/arch/arm/mach-ux500/include/mach/ab8500_codec_v1_0.h
new file mode 100644
index 00000000000..a5b8a57f341
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/ab8500_codec_v1_0.h
@@ -0,0 +1,329 @@
+/*****************************************************************************/
+/**
+*  © ST-Ericsson, 2009 - All rights reserved
+*  Reproduction and Communication of this document is strictly prohibited
+*  unless specifically authorized in writing by ST-Ericsson
+*
+* \brief   Public header file for AB8500 Codec
+* \author  ST-Ericsson
+*/
+/*****************************************************************************/
+
+#ifndef _AB8500_CODEC_V1_0_H_
+#define _AB8500_CODEC_V1_0_H_
+
+/*---------------------------------------------------------------------
+ * Includes
+ *--------------------------------------------------------------------*/
+#include "hcl_defs.h"
+#include "debug.h"
+#include <mach/ab8500_codec_p_v1_0.h>
+/*---------------------------------------------------------------------
+ * Define
+ *--------------------------------------------------------------------*/
+#ifdef __cplusplus
+extern "C" {
+#endif
+	typedef enum {
+		AB8500_CODEC_OK,
+		AB8500_CODEC_ERROR,
+		AB8500_CODEC_UNSUPPORTED_FEATURE,
+		AB8500_CODEC_INVALID_PARAMETER,
+		AB8500_CODEC_CONFIG_NOT_COHERENT,
+		AB8500_CODEC_TRANSACTION_FAILED
+	} t_ab8500_codec_error;
+
+	typedef enum {
+		AB8500_CODEC_SRC_STATE_DISABLE,
+		AB8500_CODEC_SRC_STATE_ENABLE
+	} t_ab8500_codec_src_state;
+
+	typedef enum {
+		AB8500_CODEC_DEST_STATE_DISABLE,
+		AB8500_CODEC_DEST_STATE_ENABLE
+	} t_ab8500_codec_dest_state;
+
+	typedef enum {
+		AB8500_CODEC_MASTER_MODE_DISABLE,
+		AB8500_CODEC_MASTER_MODE_ENABLE
+	} t_ab8500_codec_master_mode;
+
+	typedef enum {
+		AB8500_CODEC_SLOT0,
+		AB8500_CODEC_SLOT1,
+		AB8500_CODEC_SLOT2,
+		AB8500_CODEC_SLOT3,
+		AB8500_CODEC_SLOT4,
+		AB8500_CODEC_SLOT5,
+		AB8500_CODEC_SLOT6,
+		AB8500_CODEC_SLOT7,
+		AB8500_CODEC_SLOT8,
+		AB8500_CODEC_SLOT9,
+		AB8500_CODEC_SLOT10,
+		AB8500_CODEC_SLOT11,
+		AB8500_CODEC_SLOT12,
+		AB8500_CODEC_SLOT13,
+		AB8500_CODEC_SLOT14,
+		AB8500_CODEC_SLOT15,
+		AB8500_CODEC_SLOT16,
+		AB8500_CODEC_SLOT17,
+		AB8500_CODEC_SLOT18,
+		AB8500_CODEC_SLOT19,
+		AB8500_CODEC_SLOT20,
+		AB8500_CODEC_SLOT21,
+		AB8500_CODEC_SLOT22,
+		AB8500_CODEC_SLOT23,
+		AB8500_CODEC_SLOT24,
+		AB8500_CODEC_SLOT25,
+		AB8500_CODEC_SLOT26,
+		AB8500_CODEC_SLOT27,
+		AB8500_CODEC_SLOT28,
+		AB8500_CODEC_SLOT29,
+		AB8500_CODEC_SLOT30,
+		AB8500_CODEC_SLOT31,
+		AB8500_CODEC_SLOT_UNDEFINED
+	} t_ab8500_codec_slot;
+
+	typedef enum {
+		AB8500_CODEC_DA_CHANNEL_NUMBER_1,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_2,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_3,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_4,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_5,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_6,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_7,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_8,
+		AB8500_CODEC_DA_CHANNEL_NUMBER_UNDEFINED
+	} t_ab8500_codec_da_channel_number;
+
+	typedef struct {
+		t_ab8500_codec_cr105_bfifomsk cr105_bfifomsk;
+		t_ab8500_codec_cr105_bfifoint cr105_bfifoint;
+		t_ab8500_codec_cr106_bfifotx cr106_bfifotx;
+		t_ab8500_codec_cr107_bfifoexsl cr107_bfifoexsl;
+		t_ab8500_codec_cr107_prebitclk0 cr107_prebitclk0;
+		t_ab8500_codec_cr107_bfifomast cr107_bfifomast;
+		t_ab8500_codec_cr107_bfiforun cr107_bfiforun;
+		t_ab8500_codec_cr108_bfifoframsw cr108_bfifoframsw;
+		t_ab8500_codec_cr109_bfifowakeup cr109_bfifowakeup;
+	} t_ab8500_codec_burst_fifo_config;
+
+	typedef struct {
+		t_ab8500_codec_cr27_if1_bitclk_osr cr27_if1_bitclk_osr;
+		t_ab8500_codec_cr27_if0_bitclk_osr cr27_if0_bitclk_osr;
+		t_ab8500_codec_cr28_if0wl cr28_if0wl;
+		t_ab8500_codec_cr30_if1wl cr30_if1wl;
+		t_ab8500_codec_cr28_bitclk0p cr28_bitclk0p;
+		t_ab8500_codec_cr28_if0del cr28_if0del;
+	} t_ab8500_codec_tdm_config;
+
+/************************************************************/
+/*---------------------------------------------------------------------
+ * Exported APIs
+ *--------------------------------------------------------------------*/
+/* Initialization */
+	t_ab8500_codec_error AB8500_CODEC_Init(IN t_uint8
+					       slave_address_of_codec);
+	t_ab8500_codec_error AB8500_CODEC_Reset(void);
+
+/* Audio Codec basic configuration */
+	t_ab8500_codec_error AB8500_CODEC_SetModeAndDirection(IN
+							      t_ab8500_codec_direction
+							      ab8500_codec_direction,
+							      IN
+							      t_ab8500_codec_mode
+							      ab8500_codec_mode_in,
+							      IN
+							      t_ab8500_codec_mode
+							      ab8500_codec_mode_out,
+							      IN
+							      t_ab8500_codec_tdm_config
+							      const *const
+							      p_tdm_config);
+	t_ab8500_codec_error AB8500_CODEC_SelectInput(IN t_ab8500_codec_src
+						      ab8500_codec_src);
+	t_ab8500_codec_error AB8500_CODEC_SelectOutput(IN t_ab8500_codec_dest
+						       ab8500_codec_dest);
+
+/* Burst FIFO configuration */
+	t_ab8500_codec_error AB8500_CODEC_ConfigureBurstFifo(IN
+							     t_ab8500_codec_burst_fifo_config
+							     const *const
+							     p_burst_fifo_config);
+	t_ab8500_codec_error AB8500_CODEC_EnableBurstFifo(void);
+	t_ab8500_codec_error AB8500_CODEC_DisableBurstFifo(void);
+
+/* Audio Codec Master mode configuration */
+	t_ab8500_codec_error AB8500_CODEC_SetMasterMode(IN
+							t_ab8500_codec_master_mode
+							mode);
+
+/* APIs to be implemented by user */
+	t_ab8500_codec_error AB8500_CODEC_Write(IN t_uint8 register_offset,
+						IN t_uint8 count,
+						IN t_uint8 * p_data);
+	t_ab8500_codec_error AB8500_CODEC_Read(IN t_uint8 register_offset,
+					       IN t_uint8 count,
+					       IN t_uint8 * p_dummy_data,
+					       IN t_uint8 * p_data);
+
+/* Volume Management */
+	t_ab8500_codec_error AB8500_CODEC_SetSrcVolume(IN t_ab8500_codec_src
+						       src_device,
+						       IN t_uint8
+						       in_left_volume,
+						       IN t_uint8
+						       in_right_volume);
+	t_ab8500_codec_error AB8500_CODEC_SetDestVolume(IN t_ab8500_codec_dest
+							dest_device,
+							IN t_uint8
+							out_left_volume,
+							IN t_uint8
+							out_right_volume);
+
+/* Power management */
+	t_ab8500_codec_error AB8500_CODEC_PowerDown(void);
+	t_ab8500_codec_error AB8500_CODEC_PowerUp(void);
+
+/* Interface Management */
+	t_ab8500_codec_error AB8500_CODEC_SelectInterface(IN
+							  t_ab8500_codec_audio_interface
+							  audio_interface);
+	t_ab8500_codec_error AB8500_CODEC_GetInterface(OUT
+						       t_ab8500_codec_audio_interface
+						       * p_audio_interface);
+
+/* Slot Allocation */
+	t_ab8500_codec_error AB8500_CODEC_ADSlotAllocation(IN
+							   t_ab8500_codec_slot
+							   ad_slot,
+							   IN
+							   t_ab8500_codec_cr31_to_cr46_ad_data_allocation
+							   value);
+	t_ab8500_codec_error AB8500_CODEC_DASlotAllocation(IN
+							   t_ab8500_codec_da_channel_number
+							   channel_number,
+							   IN
+							   t_ab8500_codec_cr51_to_cr58_sltoda
+							   slot);
+
+/* Loopback Management */
+	t_ab8500_codec_error AB8500_CODEC_SetAnalogLoopback(IN t_uint8
+							    out_left_volume,
+							    IN t_uint8
+							    out_right_volume);
+	t_ab8500_codec_error AB8500_CODEC_RemoveAnalogLoopback(void);
+
+/* Bypass Management */
+	t_ab8500_codec_error AB8500_CODEC_EnableBypassMode(void);
+	t_ab8500_codec_error AB8500_CODEC_DisableBypassMode(void);
+
+/* Power Control Management */
+	t_ab8500_codec_error AB8500_CODEC_SrcPowerControl(IN t_ab8500_codec_src
+							  src_device,
+							  t_ab8500_codec_src_state
+							  state);
+	t_ab8500_codec_error AB8500_CODEC_DestPowerControl(IN
+							   t_ab8500_codec_dest
+							   dest_device,
+							   t_ab8500_codec_dest_state
+							   state);
+
+/* Version Management */
+	t_ab8500_codec_error AB8500_CODEC_GetVersion(OUT t_version * p_version);
+
+#if 0
+/* Debug management */
+	t_ab8500_codec_error AB8500_CODEC_SetDbgLevel(IN t_dbg_level dbg_level);
+	t_ab8500_codec_error AB8500_CODEC_GetDbgLevel(OUT t_dbg_level *
+						      p_dbg_level);
+#endif
+
+/*
+** following is added by $kardad$
+*/
+
+/* duplicate copy of enum from msp.h */
+/* for MSPConfiguration.in_clock_freq parameter to select msp clock freq */
+	typedef enum {
+		CODEC_MSP_INPUT_FREQ_1MHZ = 1024,
+		CODEC_MSP_INPUT_FREQ_2MHZ = 2048,
+		CODEC_MSP_INPUT_FREQ_3MHZ = 3072,
+		CODEC_MSP_INPUT_FREQ_4MHZ = 4096,
+		CODEC_MSP_INPUT_FREQ_5MHZ = 5760,
+		CODEC_MSP_INPUT_FREQ_6MHZ = 6144,
+		CODEC_MSP_INPUT_FREQ_8MHZ = 8192,
+		CODEC_MSP_INPUT_FREQ_11MHZ = 11264,
+		CODEC_MSP_INPUT_FREQ_12MHZ = 12288,
+		CODEC_MSP_INPUT_FREQ_16MHZ = 16384,
+		CODEC_MSP_INPUT_FREQ_22MHZ = 22579,
+		CODEC_MSP_INPUT_FREQ_24MHZ = 24576,
+		CODEC_MSP_INPUT_FREQ_48MHZ = 49152
+	} codec_msp_in_clock_freq_type;
+
+/* msp clock source internal/external for srg_clock_sel */
+	typedef enum {
+		CODEC_MSP_APB_CLOCK = 0,
+		CODEC_MSP_SCK_CLOCK = 2,
+		CODEC_MSP_SCK_SYNC_CLOCK = 3
+	} codec_msp_srg_clock_sel_type;
+
+/* Sample rate supported by Codec */
+
+	typedef enum {
+		CODEC_FREQUENCY_DONT_CHANGE = -100,
+		CODEC_SAMPLING_FREQ_RESET = -1,
+		CODEC_SAMPLING_FREQ_MINLIMIT = 7,
+		CODEC_SAMPLING_FREQ_8KHZ = 8,	/*default */
+		CODEC_SAMPLING_FREQ_11KHZ = 11,
+		CODEC_SAMPLING_FREQ_12KHZ = 12,
+		CODEC_SAMPLING_FREQ_16KHZ = 16,
+		CODEC_SAMPLING_FREQ_22KHZ = 22,
+		CODEC_SAMPLING_FREQ_24KHZ = 24,
+		CODEC_SAMPLING_FREQ_32KHZ = 32,
+		CODEC_SAMPLING_FREQ_44KHZ = 44,
+		CODEC_SAMPLING_FREQ_48KHZ = 48,
+		CODEC_SAMPLING_FREQ_64KHZ = 64,	/*the frequencies below this line are not supported in stw5094A */
+		CODEC_SAMPLING_FREQ_88KHZ = 88,
+		CODEC_SAMPLING_FREQ_96KHZ = 96,
+		CODEC_SAMPLING_FREQ_128KHZ = 128,
+		CODEC_SAMPLING_FREQ_176KHZ = 176,
+		CODEC_SAMPLING_FREQ_192KHZ = 192,
+		CODEC_SAMPLING_FREQ_MAXLIMIT = 193
+	} t_codec_sample_frequency;
+
+#define RESET       -1
+#define DEFAULT     -100
+/***********************************************************/
+/*
+** following stuff is added to compile code without debug print support $kardad$
+*/
+
+#define DBGEXIT(cr)
+#define DBGEXIT0(cr)
+#define DBGEXIT1(cr,ch,p1)
+#define DBGEXIT2(cr,ch,p1,p2)
+#define DBGEXIT3(cr,ch,p1,p2,p3)
+#define DBGEXIT4(cr,ch,p1,p2,p3,p4)
+#define DBGEXIT5(cr,ch,p1,p2,p3,p4,p5)
+#define DBGEXIT6(cr,ch,p1,p2,p3,p4,p5,p6)
+
+#define DBGENTER()
+#define DBGENTER0()
+#define DBGENTER1(ch,p1)
+#define DBGENTER2(ch,p1,p2)
+#define DBGENTER3(ch,p1,p2,p3)
+#define DBGENTER4(ch,p1,p2,p3,p4)
+#define DBGENTER5(ch,p1,p2,p3,p4,p5)
+#define DBGENTER6(ch,p1,p2,p3,p4,p5,p6)
+
+#define DBGPRINT(dbg_level,dbg_string)
+#define DBGPRINTHEX(dbg_level,dbg_string,uint32)
+#define DBGPRINTDEC(dbg_level,dbg_string,uint32)
+/***********************************************************/
+
+#ifdef __cplusplus
+}				/* allow C++ to use these headers */
+#endif				/* __cplusplus */
+#endif				/* _AB8500_CODEC_H_ */
+/* End of file ab8500_codec.h*/
diff --git a/arch/arm/mach-ux500/include/mach/ab8500_gpadc.h b/arch/arm/mach-ux500/include/mach/ab8500_gpadc.h
new file mode 100644
index 00000000000..4289dcfc0aa
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/ab8500_gpadc.h
@@ -0,0 +1,36 @@
+/*
+ * ab8500_gpadc.c - AB8500 GPADC Driver
+ *
+ * Copyright (C) 2010 ST-Ericsson SA
+ * Licensed under GPLv2.
+ *
+ * Author: Arun R Murthy <arun.murthy@stericsson.com>
+ */
+
+#ifndef	_AB8500_GPADC_H
+#define _Ab8500_GPADC_H
+
+/* GPADC source: From datasheer(ADCSwSel[4:0] in GPADCCtrl2) */
+#define BAT_CTRL        0x01
+#define ACC_DETECT1     0x04
+#define ACC_DETECT2     0x05
+#define MAIN_BAT_V      0x08
+#define BK_BAT_V        0x0C
+#define VBUS_V          0x09
+#define MAIN_CHARGER_V  0x03
+#define MAIN_CHARGER_C  0x0A
+#define USB_CHARGER_C   0x0B
+#define DIE_TEMP        0x0D
+#define BTEMP_BALL      0x02
+
+struct ab8500_gpadc_device_info {
+	struct completion ab8500_gpadc_complete;
+	struct mutex ab8500_gpadc_lock;
+#if defined(CONFIG_REGULATOR)
+	struct regulator *regu;
+#endif
+};
+
+int ab8500_gpadc_conversion(int input);
+
+#endif /* _AB8500_GPADC_H */
diff --git a/arch/arm/mach-ux500/include/mach/crypto-ux500.h b/arch/arm/mach-ux500/include/mach/crypto-ux500.h
new file mode 100644
index 00000000000..57da88398d5
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/crypto-ux500.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#ifndef _CRYPTO_UX500_H
+#include <plat/ste_dma40.h>
+#include <mach/ste-dma40-db8500.h>
+
+struct cryp_platform_data {
+	struct stedma40_chan_cfg mem_to_engine;
+	struct stedma40_chan_cfg engine_to_mem;
+};
+
+#endif
diff --git a/arch/arm/mach-ux500/include/mach/db5500-keypad.h b/arch/arm/mach-ux500/include/mach/db5500-keypad.h
new file mode 100644
index 00000000000..66b4c07f838
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/db5500-keypad.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License terms: GNU General Public License, version 2
+ * Author: Sundar Iyer <sundar.iyer@stericsson.com> for ST-Ericsson
+ */
+
+#ifndef __DB5500_KEYPAD_H
+#define __DB5500_KEYPAD_H
+
+#include <linux/input/matrix_keypad.h>
+
+/**
+ * struct db5500_keypad_platform_data - structure for platform specific data
+ * @keymap_data: matrix scan code table for keycodes
+ * @debounce_ms: platform specific debounce time
+ * @no_autorepeat: flag for auto repetition
+ */
+struct db5500_keypad_platform_data {
+	const struct matrix_keymap_data *keymap_data;
+	u8 debounce_ms;
+	bool no_autorepeat;
+};
+
+#endif
diff --git a/arch/arm/mach-ux500/include/mach/db5500-regs.h b/arch/arm/mach-ux500/include/mach/db5500-regs.h
index 6ad98329410..4b2847e00a3 100644
--- a/arch/arm/mach-ux500/include/mach/db5500-regs.h
+++ b/arch/arm/mach-ux500/include/mach/db5500-regs.h
@@ -65,6 +65,7 @@
 #define U5500_GPIO2_BASE	(U5500_PER4_BASE + 0xA000)
 #define U5500_CDETECT_BASE	(U5500_PER4_BASE + 0xF000)
 #define U5500_PRCMU_TCDM_BASE	(U5500_PER4_BASE + 0x18000)
+#define U5500_TPIU_BASE		(U5500_PER4_BASE + 0x50000)
 
 #define U5500_SPI0_BASE		(U5500_PER5_BASE + 0x0000)
 #define U5500_SPI1_BASE		(U5500_PER5_BASE + 0x1000)
diff --git a/arch/arm/mach-ux500/include/mach/db8500-regs.h b/arch/arm/mach-ux500/include/mach/db8500-regs.h
index 049997109cf..99baa905133 100644
--- a/arch/arm/mach-ux500/include/mach/db8500-regs.h
+++ b/arch/arm/mach-ux500/include/mach/db8500-regs.h
@@ -22,7 +22,6 @@
 #define U8500_ESRAM_DMA_LCPA_OFFSET     0x10000
 
 #define U8500_DMA_LCPA_BASE    (U8500_ESRAM_BANK0 + U8500_ESRAM_DMA_LCPA_OFFSET)
-#define U8500_DMA_LCPA_BASE_ED	(U8500_ESRAM_BANK4 + 0x4000)
 
 #define U8500_PER3_BASE		0x80000000
 #define U8500_STM_BASE		0x80100000
@@ -40,15 +39,14 @@
 #define U8500_ASIC_ID_BASE	0x9001D000
 
 #define U8500_PER6_BASE		0xa03c0000
+#define U8500_PER7_BASE		0xa03d0000
 #define U8500_PER5_BASE		0xa03e0000
-#define U8500_PER7_BASE_ED	0xa03d0000
 
 #define U8500_SVA_BASE		0xa0100000
 #define U8500_SIA_BASE		0xa0200000
 
 #define U8500_SGA_BASE		0xa0300000
 #define U8500_MCDE_BASE		0xa0350000
-#define U8500_DMA_BASE_ED	0xa0362000
 #define U8500_DMA_BASE		0x801C0000	/* v1 */
 
 #define U8500_SBAG_BASE		0xa0390000
@@ -66,13 +64,6 @@
 #define U8500_GPIO2_BASE	(U8500_PER2_BASE + 0xE000)
 #define U8500_GPIO3_BASE	(U8500_PER5_BASE + 0x1E000)
 
-/* per7 base addresses */
-#define U8500_CR_BASE_ED	(U8500_PER7_BASE_ED + 0x8000)
-#define U8500_MTU0_BASE_ED	(U8500_PER7_BASE_ED + 0xa000)
-#define U8500_MTU1_BASE_ED	(U8500_PER7_BASE_ED + 0xb000)
-#define U8500_TZPC0_BASE_ED	(U8500_PER7_BASE_ED + 0xc000)
-#define U8500_CLKRST7_BASE_ED	(U8500_PER7_BASE_ED + 0xf000)
-
 #define U8500_UART0_BASE	(U8500_PER1_BASE + 0x0000)
 #define U8500_UART1_BASE	(U8500_PER1_BASE + 0x1000)
 
@@ -102,7 +93,6 @@
 #define U8500_SCR_BASE		(U8500_PER4_BASE + 0x05000)
 #define U8500_DMC_BASE		(U8500_PER4_BASE + 0x06000)
 #define U8500_PRCMU_BASE	(U8500_PER4_BASE + 0x07000)
-#define U8500_PRCMU_TCDM_BASE_V1 (U8500_PER4_BASE + 0x0f000)
 #define U8500_PRCMU_TCDM_BASE	(U8500_PER4_BASE + 0x68000)
 #define U8500_PRCMU_TCPM_BASE   (U8500_PER4_BASE + 0x60000)
 
diff --git a/arch/arm/mach-ux500/include/mach/dcache.h b/arch/arm/mach-ux500/include/mach/dcache.h
new file mode 100644
index 00000000000..83fe618b04f
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/dcache.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Data cache helpers
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _MACH_UX500_DCACHE_H_
+#define _MACH_UX500_DCACHE_H_
+
+#include <linux/types.h>
+
+void drain_cpu_write_buf(void);
+void clean_cpu_dcache(void *vaddr, u32 paddr, u32 length, bool inner_only,
+						bool *cleaned_everything);
+void flush_cpu_dcache(void *vaddr, u32 paddr, u32 length, bool inner_only,
+						bool *flushed_everything);
+bool speculative_data_prefetch(void);
+/* Returns 1 if no cache is present */
+u32 get_dcache_granularity(void);
+
+#endif /* _MACH_UX500_DCACHE_H_ */
diff --git a/arch/arm/mach-ux500/include/mach/devices.h b/arch/arm/mach-ux500/include/mach/devices.h
index 020b6369a30..ae4d895eb28 100644
--- a/arch/arm/mach-ux500/include/mach/devices.h
+++ b/arch/arm/mach-ux500/include/mach/devices.h
@@ -13,11 +13,25 @@ struct amba_device;
 extern struct platform_device u5500_gpio_devs[];
 extern struct platform_device u8500_gpio_devs[];
 
+extern struct platform_device u8500_mcde_device;
+extern struct platform_device u5500_mcde_device;
+extern struct platform_device u8500_shrm_device;
+extern struct platform_device u8500_b2r2_device;
+extern struct platform_device u5500_b2r2_device;
+extern struct platform_device u8500_trace_modem;
+extern struct platform_device ux500_hwmem_device;
+extern struct platform_device ux500_stm_device;
 extern struct amba_device ux500_pl031_device;
-
+extern struct platform_device ux500_hash1_device;
+extern struct platform_device ux500_cryp1_device;
+extern struct platform_device ux500_wdt_device;
+extern struct platform_device ux500_prcmu_wdt_device;
+extern struct platform_device mloader_fw_device;
+extern struct platform_device u8500_thsens_device;
 extern struct platform_device u8500_dma40_device;
 extern struct platform_device ux500_ske_keypad_device;
-
-void dma40_u8500ed_fixup(void);
+extern struct platform_device u8500_hsi_device;
+extern struct platform_device ux500_mmio_device;
+extern struct platform_device u5500_mmio_device;
 
 #endif
diff --git a/arch/arm/mach-ux500/include/mach/gpio.h b/arch/arm/mach-ux500/include/mach/gpio.h
index 3c4cd31ad9f..8f1cd409604 100644
--- a/arch/arm/mach-ux500/include/mach/gpio.h
+++ b/arch/arm/mach-ux500/include/mach/gpio.h
@@ -5,8 +5,15 @@
  * 288 (#267 is the highest one actually hooked up) onchip GPIOs, plus enough
  * room for a couple of GPIO expanders.
  */
-#define ARCH_NR_GPIOS	350
+#define ARCH_NR_GPIOS	355
+#define NOMADIK_NR_GPIO	288
 
-#include <plat/gpio.h>
+#include <asm-generic/gpio.h>
+
+/* Invoke gpiolibs gpio_chip abstraction */
+#define gpio_get_value  __gpio_get_value
+#define gpio_set_value  __gpio_set_value
+#define gpio_cansleep   __gpio_cansleep
+#define gpio_to_irq     __gpio_to_irq
 
 #endif /* __ASM_ARCH_GPIO_H */
diff --git a/arch/arm/mach-ux500/include/mach/hardware.h b/arch/arm/mach-ux500/include/mach/hardware.h
index 470ac52663d..9b2b9c5c8ab 100644
--- a/arch/arm/mach-ux500/include/mach/hardware.h
+++ b/arch/arm/mach-ux500/include/mach/hardware.h
@@ -10,25 +10,42 @@
 #ifndef __MACH_HARDWARE_H
 #define __MACH_HARDWARE_H
 
-/* macros to get at IO space when running virtually
+/*
+ * Macros to get at IO space when running virtually
  * We dont map all the peripherals, let ioremap do
  * this for us. We map only very basic peripherals here.
  */
 #define U8500_IO_VIRTUAL	0xf0000000
 #define U8500_IO_PHYSICAL	0xa0000000
 
-/* this macro is used in assembly, so no cast */
+/* This macro is used in assembly, so no cast */
 #define IO_ADDRESS(x)           \
 	(((x) & 0x0fffffff) + (((x) >> 4) & 0x0f000000) + U8500_IO_VIRTUAL)
 
 /* typesafe io address */
 #define __io_address(n)		__io(IO_ADDRESS(n))
-/* used by some plat-nomadik code */
+/* Used by some plat-nomadik code */
 #define io_p2v(n)		__io_address(n)
 
 #include <mach/db8500-regs.h>
 #include <mach/db5500-regs.h>
 
+/*
+ * FIFO offsets for IPs
+ */
+#define MSP_TX_RX_REG_OFFSET	0
+#define SSP_TX_RX_REG_OFFSET	0x8
+#define SPI_TX_RX_REG_OFFSET	0x8
+#define SD_MMC_TX_RX_REG_OFFSET 0x80
+#define CRYP1_RX_REG_OFFSET	0x10
+#define CRYP1_TX_REG_OFFSET	0x8
+
+/* MSP related board specific declaration************************/
+
+#define MSP_DATA_DELAY       MSP_DELAY_0
+#define MSP_TX_CLOCK_EDGE    MSP_FALLING_EDGE
+#define MSP_RX_CLOCK_EDGE    MSP_FALLING_EDGE
+
 #ifndef __ASSEMBLY__
 
 #include <mach/id.h>
diff --git a/arch/arm/mach-ux500/include/mach/hcl_defs.h b/arch/arm/mach-ux500/include/mach/hcl_defs.h
new file mode 100644
index 00000000000..efd37608cb3
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/hcl_defs.h
@@ -0,0 +1,252 @@
+/*
+ * Copyright (C) 2009 ST-Ericsson SA
+ *
+ * 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 _HCL_DEFS_H
+#define _HCL_DEFS_H
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+
+//#include "platform_os.h"
+
+/*-----------------------------------------------------------------------------
+ * Type definition
+ *---------------------------------------------------------------------------*/
+typedef unsigned char t_uint8;
+typedef signed char t_sint8;
+typedef unsigned short t_uint16;
+typedef signed short t_sint16;
+typedef unsigned long t_uint32;
+typedef signed long t_sint32;
+
+typedef unsigned int t_bitfield;
+
+
+
+#if !defined(FALSE) &&  !defined(TRUE)
+typedef int  t_bool;
+#define  FALSE  0
+#define  TRUE  1
+#endif
+
+/*
+ * Definition of the different kind of addresses manipulated into a system with MMU
+ * (handle physical AND logical addresses)
+ */
+
+
+typedef t_uint32 t_physical_address;
+typedef t_uint32 t_logical_address;
+
+
+
+/*
+ * Global frequency enumuration
+ * Added to avoid frequency conversion function which is required to convert one HCL
+ * frequency enumuration values to another HCL frequency enumuration values.
+ */
+
+/*typedef enum {
+	HCL_FREQ_NOT_SUPPORTED=-1,
+	HCL_FREQ_8KHZ ,
+	HCL_FREQ_11_25KHZ,
+	HCL_FREQ_12KHZ,
+	HCL_FREQ_16KHZ,
+	HCL_FREQ_22_05KHZ,
+	HCL_FREQ_22_5KHZ,
+	HCL_FREQ_24KHZ,
+	HCL_FREQ_32KHZ,
+	HCL_FREQ_44KHZ,
+	HCL_FREQ_44_1KHZ,
+	HCL_FREQ_48KHZ,
+	HCL_FREQ_64KHZ,
+	HCL_FREQ_88KHZ,
+	HCL_FREQ_88_2KHZ,
+	HCL_FREQ_96KHZ,
+	HCL_FREQ_128KHZ,
+	HCL_FREQ_176_4KHZ,
+	HCL_FREQ_192KHZ,
+
+	HCL_FREQ_1MHZ,
+	HCL_FREQ_2MHZ,
+	HCL_FREQ_3MHZ,
+	HCL_FREQ_4MHZ,
+	HCL_FREQ_5MHZ,
+	HCL_FREQ_6MHZ,
+	HCL_FREQ_8MHZ,
+	HCL_FREQ_11MHZ,
+	HCL_FREQ_12MHZ,
+	HCL_FREQ_16MHZ,
+	HCL_FREQ_22MHZ,
+	HCL_FREQ_24MHZ,
+	HCL_FREQ_48MHZ
+} t_frequency;
+
+*/
+
+typedef struct {
+	t_physical_address physical;
+	t_logical_address logical;
+} t_system_address;
+
+
+/*
+ * Define a type used to manipulate size of various buffers
+ */
+typedef t_uint32 t_size;
+
+typedef struct {
+ t_bitfield minor:8;
+ t_bitfield major:8;
+ t_bitfield version:16;
+} t_version;
+
+
+
+
+/*-----------------------------------------------------------------------------
+ * Keyword definition
+ *---------------------------------------------------------------------------*/
+#define PUBLIC        /* Extern by default */
+#define PRIVATE      static
+
+#ifndef NULL
+#define NULL     (0)
+#endif /* ndef NULL */
+
+
+/*-----------------------------------------------------------------------------
+ * Bit setting or clearing
+ *---------------------------------------------------------------------------*/
+#define HCL_SET_BITS(reg,mask)			((reg) |=  (mask))
+#define HCL_CLEAR_BITS(reg,mask)		((reg) &= ~(mask))
+#define HCL_READ_BITS(reg,mask)			((reg) &   (mask))
+#define HCL_WRITE_BITS(reg,val,mask)	((reg) =   (((reg) & ~(mask)) | ((val) & (mask))))
+#define HCL_READ_REG(reg)				(reg)
+#define HCL_WRITE_REG(reg,val)			((reg) = (val))
+
+/*-----------------------------------------------------------------------------
+ * field offset extraction from a structure
+ *---------------------------------------------------------------------------*/
+#define HCL_BITFIELD_OFFSET(typeName, fieldName) (t_uint32)(&(((typeName *)0)->fieldName))
+
+/*-----------------------------------------------------------------------------
+ * Bit mask definition
+ *---------------------------------------------------------------------------*/
+#define MASK_NULL8    0x00
+#define MASK_NULL16   0x0000
+#define MASK_NULL32   0x00000000
+#define MASK_ALL8     0xFF
+#define MASK_ALL16    0xFFFF
+#define MASK_ALL32    0xFFFFFFFF
+
+#define MASK_BIT0     (1UL<<0)
+#define MASK_BIT1     (1UL<<1)
+#define MASK_BIT2     (1UL<<2)
+#define MASK_BIT3     (1UL<<3)
+#define MASK_BIT4     (1UL<<4)
+#define MASK_BIT5     (1UL<<5)
+#define MASK_BIT6     (1UL<<6)
+#define MASK_BIT7     (1UL<<7)
+#define MASK_BIT8     (1UL<<8)
+#define MASK_BIT9     (1UL<<9)
+#define MASK_BIT10    (1UL<<10)
+#define MASK_BIT11    (1UL<<11)
+#define MASK_BIT12    (1UL<<12)
+#define MASK_BIT13    (1UL<<13)
+#define MASK_BIT14    (1UL<<14)
+#define MASK_BIT15    (1UL<<15)
+#define MASK_BIT16    (1UL<<16)
+#define MASK_BIT17    (1UL<<17)
+#define MASK_BIT18    (1UL<<18)
+#define MASK_BIT19    (1UL<<19)
+#define MASK_BIT20    (1UL<<20)
+#define MASK_BIT21    (1UL<<21)
+#define MASK_BIT22    (1UL<<22)
+#define MASK_BIT23    (1UL<<23)
+#define MASK_BIT24    (1UL<<24)
+#define MASK_BIT25    (1UL<<25)
+#define MASK_BIT26    (1UL<<26)
+#define MASK_BIT27    (1UL<<27)
+#define MASK_BIT28    (1UL<<28)
+#define MASK_BIT29    (1UL<<29)
+#define MASK_BIT30    (1UL<<30)
+#define MASK_BIT31    (1UL<<31)
+
+/*-----------------------------------------------------------------------------
+ * quartet shift definition
+ *---------------------------------------------------------------------------*/
+#define MASK_QUARTET    (0xFUL)
+#define SHIFT_QUARTET0  0
+#define SHIFT_QUARTET1  4
+#define SHIFT_QUARTET2  8
+#define SHIFT_QUARTET3  12
+#define SHIFT_QUARTET4  16
+#define SHIFT_QUARTET5  20
+#define SHIFT_QUARTET6  24
+#define SHIFT_QUARTET7  28
+#define MASK_QUARTET0   (MASK_QUARTET << SHIFT_QUARTET0)
+#define MASK_QUARTET1   (MASK_QUARTET << SHIFT_QUARTET1)
+#define MASK_QUARTET2   (MASK_QUARTET << SHIFT_QUARTET2)
+#define MASK_QUARTET3   (MASK_QUARTET << SHIFT_QUARTET3)
+#define MASK_QUARTET4   (MASK_QUARTET << SHIFT_QUARTET4)
+#define MASK_QUARTET5   (MASK_QUARTET << SHIFT_QUARTET5)
+#define MASK_QUARTET6   (MASK_QUARTET << SHIFT_QUARTET6)
+#define MASK_QUARTET7   (MASK_QUARTET << SHIFT_QUARTET7)
+
+/*-----------------------------------------------------------------------------
+ * Byte shift definition
+ *---------------------------------------------------------------------------*/
+#define MASK_BYTE      (0xFFUL)
+#define SHIFT_BYTE0	0
+#define SHIFT_BYTE1	8
+#define SHIFT_BYTE2	16
+#define SHIFT_BYTE3	24
+#define MASK_BYTE0      (MASK_BYTE << SHIFT_BYTE0)
+#define MASK_BYTE1      (MASK_BYTE << SHIFT_BYTE1)
+#define MASK_BYTE2      (MASK_BYTE << SHIFT_BYTE2)
+#define MASK_BYTE3      (MASK_BYTE << SHIFT_BYTE3)
+
+/*-----------------------------------------------------------------------------
+ * Halfword shift definition
+ *---------------------------------------------------------------------------*/
+#define MASK_HALFWORD       (0xFFFFUL)
+#define SHIFT_HALFWORD0	    0
+#define SHIFT_HALFWORD1	    16
+#define MASK_HALFWORD0      (MASK_HALFWORD << SHIFT_HALFWORD0)
+#define MASK_HALFWORD1      (MASK_HALFWORD << SHIFT_HALFWORD1)
+
+/*-----------------------------------------------------------------------------
+ * Global constants definition
+ *---------------------------------------------------------------------------*/
+ #define ONE_KB        (1024)
+ #define ONE_MB        (ONE_KB * ONE_KB)
+
+
+/*-----------------------------------------------------------------------------
+ * Address translation macros declaration
+ *---------------------------------------------------------------------------*/
+
+#define ARM_TO_AHB_ADDR(addr) (addr)
+#define AHB_TO_ARM_ADDR(addr) (addr)
+
+/* For input parameters - would not be changed by the API */
+#define IN
+/* For output parameters - would be changes by the API */
+#define OUT
+/* For input-output parameters - provides input to the API but would be changed by the API */
+#define INOUT
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* _HCL_DEFS_H */
+
+/* End of file hcl_defs.h */
+
+
diff --git a/arch/arm/mach-ux500/include/mach/hsi.h b/arch/arm/mach-ux500/include/mach/hsi.h
new file mode 100644
index 00000000000..030e35e729b
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/hsi.h
@@ -0,0 +1,122 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ */
+
+#ifndef __MACH_HSI_H
+#define __MACH_HSI_H
+
+#include <plat/ste_dma40.h>
+
+/* HSIT register offsets */
+#define STE_HSI_TX_ID                  0x000
+#define STE_HSI_TX_MODE                0x004
+#define STE_HSI_TX_STATE               0x008
+#define STE_HSI_TX_IOSTATE             0x00C
+#define STE_HSI_TX_BUFSTATE            0x010
+#define STE_HSI_TX_DIVISOR             0x014
+#define STE_HSI_TX_PARITY              0x018
+#define STE_HSI_TX_BREAK               0x01C
+#define STE_HSI_TX_CHANNELS            0x020
+#define STE_HSI_TX_FLUSHBITS           0x024
+#define STE_HSI_TX_PRIORITY            0x028
+#define STE_HSI_TX_BURSTLEN            0x02C
+#define STE_HSI_TX_PREAMBLE            0x030
+#define STE_HSI_TX_DATASWAP            0x034
+#define STE_HSI_TX_FRAMELENX           0x080
+#define STE_HSI_TX_BUFFERX             0x0C0
+#define STE_HSI_TX_BASEX               0x100
+#define STE_HSI_TX_SPANX               0x140
+#define STE_HSI_TX_GAUGEX              0x180
+#define STE_HSI_TX_WATERMARKX          0x1C0
+#define STE_HSI_TX_DMAEN               0x200
+#define STE_HSI_TX_WATERMARKIS         0x204
+#define STE_HSI_TX_WATERMARKIM         0x208
+#define STE_HSI_TX_WATERMARKIC         0x20C
+#define STE_HSI_TX_WATERMARKID         0x210
+#define STE_HSI_TX_PERIPHID0           0xFE0
+#define STE_HSI_TX_PERIPHID1           0xFE4
+#define STE_HSI_TX_PERIPHID2           0xFE8
+#define STE_HSI_TX_PERIPHID3           0xFEC
+
+/* HSIR register offsets */
+#define STE_HSI_RX_ID                  0x000
+#define STE_HSI_RX_MODE                0x004
+#define STE_HSI_RX_STATE               0x008
+#define STE_HSI_RX_BUFSTATE            0x00C
+#define STE_HSI_RX_THRESHOLD           0x010
+#define STE_HSI_RX_PARITY              0x014
+#define STE_HSI_RX_DETECTOR            0x018
+#define STE_HSI_RX_EXCEP               0x01C
+#define STE_HSI_RX_ACK                 0x020
+#define STE_HSI_RX_CHANNELS            0x024
+#define STE_HSI_RX_REALTIME            0x028
+#define STE_HSI_RX_OVERRUN             0x02C
+#define STE_HSI_RX_OVERRUNACK          0x030
+#define STE_HSI_RX_PREAMBLE            0x034
+#define STE_HSI_RX_PIPEGAUGE           0x038
+#define STE_HSI_RX_STATICCONFID        0x03C
+#define STE_HSI_RX_BUFFERX             0x080
+#define STE_HSI_RX_FRAMELENX           0x0C0
+#define STE_HSI_RX_BASEX               0x100
+#define STE_HSI_RX_SPANX               0x140
+#define STE_HSI_RX_GAUGEX              0x180
+#define STE_HSI_RX_WATERMARKX          0x1C0
+#define STE_HSI_RX_DMAEN               0x200
+#define STE_HSI_RX_WATERMARKIS         0x204
+#define STE_HSI_RX_WATERMARKIM         0x208
+#define STE_HSI_RX_WATERMARKIC         0x20C
+#define STE_HSI_RX_WATERMARKID         0x210
+#define STE_HSI_RX_OVERRUNMIS          0x214
+#define STE_HSI_RX_OVERRUNIM           0x218
+#define STE_HSI_RX_EXCEPMIS            0x21C
+#define STE_HSI_RX_EXCEPIM             0x220
+#define STE_HSI_RX_PERIPHID0           0xFE0
+#define STE_HSI_RX_PERIPHID1           0xFE4
+#define STE_HSI_RX_PERIPHID2           0xFE8
+#define STE_HSI_RX_PERIPHID3           0xFEC
+
+/* HSI states */
+#define STE_HSI_STATE_IDLE             0x00
+#define STE_HSI_STATE_START            0x01
+#define STE_HSI_STATE_TRANSMIT         0x02
+#define STE_HSI_STATE_BREAK            0x03
+#define STE_HSI_STATE_FLUSH            0x04
+#define STE_HSI_STATE_HALT             0x05
+
+/* HSI exceptions */
+#define STE_HSI_EXCEP_TIMEOUT          0x01
+#define STE_HSI_EXCEP_OVERRUN          0x02
+#define STE_HSI_EXCEP_BREAK            0x04
+#define STE_HSI_EXCEP_PARITY           0x08
+
+/* HSI modes */
+#define STE_HSI_MODE_SLEEP             0x00
+#define STE_HSI_MODE_STREAM            0x01
+#define STE_HSI_MODE_FRAME             0x02
+#define STE_HSI_MODE_PIPELINED         0x03
+#define STE_HSI_MODE_FAILSAFE          0x04
+
+#define STE_HSI_MAX_BUFFERS            32
+
+/* Max channels of STE HSI controller */
+#define STE_HSI_MAX_CHANNELS           4
+
+struct stedma40_chan_cfg;
+
+struct ste_hsi_port_cfg {
+#ifdef CONFIG_STE_DMA40
+       bool (*dma_filter)(struct dma_chan *chan, void *filter_param);
+       struct stedma40_chan_cfg *dma_tx_cfg;
+       struct stedma40_chan_cfg *dma_rx_cfg;
+#endif
+};
+
+struct ste_hsi_platform_data {
+       int num_ports;
+       int use_dma;
+       struct ste_hsi_port_cfg *port_cfg;
+};
+
+#endif
diff --git a/arch/arm/mach-ux500/include/mach/id.h b/arch/arm/mach-ux500/include/mach/id.h
index 02b541a37ee..7bb64b08eaf 100644
--- a/arch/arm/mach-ux500/include/mach/id.h
+++ b/arch/arm/mach-ux500/include/mach/id.h
@@ -50,26 +50,6 @@ static inline bool __attribute_const__ cpu_is_u5500(void)
  * 8500 revisions
  */
 
-static inline bool __attribute_const__ cpu_is_u8500ed(void)
-{
-	return cpu_is_u8500() && dbx500_revision() == 0x00;
-}
-
-static inline bool __attribute_const__ cpu_is_u8500v1(void)
-{
-	return cpu_is_u8500() && (dbx500_revision() & 0xf0) == 0xA0;
-}
-
-static inline bool __attribute_const__ cpu_is_u8500v10(void)
-{
-	return cpu_is_u8500() && dbx500_revision() == 0xA0;
-}
-
-static inline bool __attribute_const__ cpu_is_u8500v11(void)
-{
-	return cpu_is_u8500() && dbx500_revision() == 0xA1;
-}
-
 static inline bool __attribute_const__ cpu_is_u8500v2(void)
 {
 	return cpu_is_u8500() && ((dbx500_revision() & 0xf0) == 0xB0);
@@ -85,9 +65,14 @@ static inline bool cpu_is_u8500v21(void)
 	return cpu_is_u8500() && (dbx500_revision() == 0xB1);
 }
 
+static inline bool cpu_is_u8500v22(void)
+{
+	return cpu_is_u8500() && (dbx500_revision() == 0xB2);
+}
+
 static inline bool cpu_is_u8500v20_or_later(void)
 {
-	return cpu_is_u8500() && !cpu_is_u8500v10() && !cpu_is_u8500v11();
+	return cpu_is_u8500() && ((dbx500_revision() & 0xf0) >= 0xB0);
 }
 
 static inline bool ux500_is_svp(void)
diff --git a/arch/arm/mach-ux500/include/mach/irqs-board-mop500.h b/arch/arm/mach-ux500/include/mach/irqs-board-mop500.h
index 47969909836..a58aa51fccf 100644
--- a/arch/arm/mach-ux500/include/mach/irqs-board-mop500.h
+++ b/arch/arm/mach-ux500/include/mach/irqs-board-mop500.h
@@ -43,6 +43,8 @@
 
 #define MOP500_AB8500_VIR_GPIO_IRQ_BASE		\
 	MOP500_STMPE1601_IRQ_END
+#define MOP500_AB8500_VIR_GPIO_IRQ(x)		\
+	(MOP500_AB8500_VIR_GPIO_IRQ_BASE + (x))
 #define MOP500_AB8500_VIR_GPIO_IRQ_END		\
 	(MOP500_AB8500_VIR_GPIO_IRQ_BASE + AB8500_VIR_GPIO_NR_IRQS)
 
@@ -57,7 +59,7 @@
  */
 #if MOP500_IRQ_END > IRQ_BOARD_END
 #undef IRQ_BOARD_END
-#define IRQ_BOARD_END	MOP500_IRQ_END
+#define IRQ_BOARD_END		MOP500_IRQ_END
 #endif
 
 #endif
diff --git a/arch/arm/mach-ux500/include/mach/irqs-board-u5500.h b/arch/arm/mach-ux500/include/mach/irqs-board-u5500.h
index 29d972c7717..f40508b5730 100644
--- a/arch/arm/mach-ux500/include/mach/irqs-board-u5500.h
+++ b/arch/arm/mach-ux500/include/mach/irqs-board-u5500.h
@@ -7,13 +7,18 @@
 #ifndef __MACH_IRQS_BOARD_U5500_H
 #define __MACH_IRQS_BOARD_U5500_H
 
-#define AB5500_NR_IRQS		5
+#define AB5500_NR_IRQS		(23 * 8)
 #define IRQ_AB5500_BASE		IRQ_BOARD_START
 #define IRQ_AB5500_END		(IRQ_AB5500_BASE + AB5500_NR_IRQS)
 
 #define U5500_IRQ_END		IRQ_AB5500_END
 
-#if IRQ_BOARD_END < U5500_IRQ_END
+/*
+ * We may have several boards, but only one will run at a
+ * time, so the one with most IRQs will bump this ahead,
+ * but the IRQ_BOARD_START remains the same for either board.
+ */
+#if U5500_IRQ_END > IRQ_BOARD_END
 #undef IRQ_BOARD_END
 #define IRQ_BOARD_END		U5500_IRQ_END
 #endif
diff --git a/arch/arm/mach-ux500/include/mach/irqs-db5500.h b/arch/arm/mach-ux500/include/mach/irqs-db5500.h
index 77239776a6f..d8d00b6c2ba 100644
--- a/arch/arm/mach-ux500/include/mach/irqs-db5500.h
+++ b/arch/arm/mach-ux500/include/mach/irqs-db5500.h
@@ -85,6 +85,35 @@
 
 #ifdef CONFIG_UX500_SOC_DB5500
 
+/* Virtual interrupts corresponding to the PRCMU wakeups.  */
+#define IRQ_DB5500_PRCMU_BASE IRQ_SOC_START
+
+#define IRQ_DB5500_PRCMU_RTC (IRQ_DB5500_PRCMU_BASE)
+#define IRQ_DB5500_PRCMU_RTT0 (IRQ_DB5500_PRCMU_BASE + 1)
+#define IRQ_DB5500_PRCMU_RTT1 (IRQ_DB5500_PRCMU_BASE + 2)
+#define IRQ_DB5500_PRCMU_CD_IRQ (IRQ_DB5500_PRCMU_BASE + 3)
+#define IRQ_DB5500_PRCMU_SRP_TIM (IRQ_DB5500_PRCMU_BASE + 4)
+#define IRQ_DB5500_PRCMU_APE_REQ (IRQ_DB5500_PRCMU_BASE + 5)
+#define IRQ_DB5500_PRCMU_USB (IRQ_DB5500_PRCMU_BASE + 6)
+#define IRQ_DB5500_PRCMU_ABB (IRQ_DB5500_PRCMU_BASE + 7)
+#define IRQ_DB5500_PRCMU_ARM (IRQ_DB5500_PRCMU_BASE + 8)
+#define IRQ_DB5500_PRCMU_MODEM_SW_RESET_REQ (IRQ_DB5500_PRCMU_BASE + 9)
+#define IRQ_DB5500_PRCMU_AC_WAKE_ACK (IRQ_DB5500_PRCMU_BASE + 10)
+#define IRQ_DB5500_PRCMU_GPIO0 (IRQ_DB5500_PRCMU_BASE + 11)
+#define IRQ_DB5500_PRCMU_GPIO1 (IRQ_DB5500_PRCMU_BASE + 12)
+#define IRQ_DB5500_PRCMU_GPIO2 (IRQ_DB5500_PRCMU_BASE + 13)
+#define IRQ_DB5500_PRCMU_GPIO3 (IRQ_DB5500_PRCMU_BASE + 14)
+#define IRQ_DB5500_PRCMU_GPIO4 (IRQ_DB5500_PRCMU_BASE + 15)
+#define IRQ_DB5500_PRCMU_GPIO5 (IRQ_DB5500_PRCMU_BASE + 16)
+#define IRQ_DB5500_PRCMU_GPIO6 (IRQ_DB5500_PRCMU_BASE + 17)
+#define IRQ_DB5500_PRCMU_GPIO7 (IRQ_DB5500_PRCMU_BASE + 18)
+#define IRQ_DB5500_PRCMU_AC_REL_ACK (IRQ_DB5500_PRCMU_BASE + 19)
+#define IRQ_DB5500_PRCMU_LOW_POWER_AUDIO (IRQ_DB5500_PRCMU_BASE + 20)
+#define IRQ_DB5500_PRCMU_TEMP_SENSOR (IRQ_DB5500_PRCMU_BASE + 21)
+#define IRQ_DB5500_PRCMU_END (IRQ_DB5500_PRCMU_BASE + 22)
+
+#define NUM_DB5500_PRCMU_WAKEUPS (IRQ_DB5500_PRCMU_END - IRQ_DB5500_PRCMU_BASE)
+
 /*
  * After the GPIO ones we reserve a range of IRQ:s in which virtual
  * IRQ:s representing modem IRQ:s can be allocated
diff --git a/arch/arm/mach-ux500/include/mach/irqs.h b/arch/arm/mach-ux500/include/mach/irqs.h
index 9db68d264c5..5144323d5aa 100644
--- a/arch/arm/mach-ux500/include/mach/irqs.h
+++ b/arch/arm/mach-ux500/include/mach/irqs.h
@@ -11,9 +11,7 @@
 #define ASM_ARCH_IRQS_H
 
 #include <mach/hardware.h>
-
-#define IRQ_LOCALTIMER			29
-#define IRQ_LOCALWDOG			30
+#include <linux/gpio.h>
 
 /* Shared Peripheral Interrupt (SHPI) */
 #define IRQ_SHPI_START			32
@@ -22,27 +20,31 @@
  * MTU0 preserved for now until plat-nomadik is taught not to use it.  Don't
  * add any other IRQs here, use the irqs-dbx500.h files.
  */
-#define IRQ_MTU0		(IRQ_SHPI_START + 4)
+#define IRQ_MTU0			(IRQ_SHPI_START + 4)
+
+#define IRQ_LOCALTIMER			29
+#define IRQ_LOCALWDOG			30
+
+/*********************************************************************/
 
 #define DBX500_NR_INTERNAL_IRQS		160
 
 /* After chip-specific IRQ numbers we have the GPIO ones */
-#define NOMADIK_NR_GPIO			288
 #define NOMADIK_GPIO_TO_IRQ(gpio)	((gpio) + DBX500_NR_INTERNAL_IRQS)
 #define NOMADIK_IRQ_TO_GPIO(irq)	((irq) - DBX500_NR_INTERNAL_IRQS)
 #define IRQ_GPIO_END			NOMADIK_GPIO_TO_IRQ(NOMADIK_NR_GPIO)
 
-#define IRQ_SOC_START		IRQ_GPIO_END
+#define IRQ_SOC_START			IRQ_GPIO_END
 /* This will be overridden by SoC-specific irq headers */
-#define IRQ_SOC_END		IRQ_SOC_START
+#define IRQ_SOC_END			IRQ_SOC_START
+
+#define IRQ_BOARD_START			IRQ_SOC_END
+/* This will be overridden by board-specific irq headers */
+#define IRQ_BOARD_END			IRQ_BOARD_START
 
 #include <mach/irqs-db5500.h>
 #include <mach/irqs-db8500.h>
 
-#define IRQ_BOARD_START		IRQ_SOC_END
-/* This will be overridden by board-specific irq headers */
-#define IRQ_BOARD_END		IRQ_BOARD_START
-
 #ifdef CONFIG_MACH_U8500
 #include <mach/irqs-board-mop500.h>
 #endif
@@ -51,6 +53,8 @@
 #include <mach/irqs-board-u5500.h>
 #endif
 
+#ifndef NR_IRQS
 #define NR_IRQS			IRQ_BOARD_END
+#endif
 
 #endif /* ASM_ARCH_IRQS_H */
diff --git a/arch/arm/mach-ux500/include/mach/isa_ioctl.h b/arch/arm/mach-ux500/include/mach/isa_ioctl.h
new file mode 100644
index 00000000000..b05726f8c3c
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/isa_ioctl.h
@@ -0,0 +1,51 @@
+/*---------------------------------------------------------------------------*/
+/* Copyright ST Ericsson, 2009.                                              */
+/* 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.1 of the License, or	     */
+/* (at your option) any later version.                                       */
+/*                                                                           */
+/* This program is distributed in the hope that it will be useful, but 	     */
+/* WITHOUT ANY WARRANTY; without even the implied warranty of 		     */
+/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 		     */
+/* See the GNU General Public License for more details.   		     */
+/*                                                                           */
+/* You should have received a copy of the GNU General Public License         */
+/* along with this program. If not, see <http://www.gnu.org/licenses/>.      */
+/*---------------------------------------------------------------------------*/
+#ifndef __MODEM_IPC_INCLUDED
+#define __MODEM_IPC_INCLUDED
+
+#define DLP_IOCTL_MAGIC_NUMBER 'M'
+#define COMMON_BUFFER_SIZE (1024*1024)
+
+/**
+DLP Message Structure for Userland
+*/
+struct t_dlp_message{
+	unsigned int offset;
+	unsigned int size;
+};
+
+/**
+mmap constants.
+*/
+enum t_dlp_mmap_params {
+	MMAP_DLQUEUE,
+	MMAP_ULQUEUE
+};
+
+/**
+DLP IOCTLs for Userland
+*/
+#define DLP_IOC_ALLOCATE_BUFFER \
+	_IOWR(DLP_IOCTL_MAGIC_NUMBER, 0, struct t_dlp_message *)
+#define DLP_IOC_DEALLOCATE_BUFFER \
+	_IOWR(DLP_IOCTL_MAGIC_NUMBER, 1, struct t_dlp_message *)
+#define DLP_IOC_GET_MESSAGE \
+	_IOWR(DLP_IOCTL_MAGIC_NUMBER, 2, struct t_dlp_message *)
+#define DLP_IOC_PUT_MESSAGE \
+	_IOWR(DLP_IOCTL_MAGIC_NUMBER, 3, struct t_dlp_message *)
+
+#endif /*__MODEM_IPC_INCLUDED*/
+
diff --git a/arch/arm/mach-ux500/include/mach/mbox_channels-db5500.h b/arch/arm/mach-ux500/include/mach/mbox_channels-db5500.h
new file mode 100644
index 00000000000..0b2fc604873
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/mbox_channels-db5500.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Marcin Mielczarczyk <marcin.mielczarczyk@tieto.com> for ST-Ericsson.
+ *         Bibek Basu <bibek.basu@stericsson.com>
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __INC_MBOX_CHANNELS_H
+#define __INC_MBOX_CHANNELS_H
+
+/* Maximum number of datawords which can be send in one PDU */
+#define MAILBOX_NR_OF_DATAWORDS	3
+
+/**
+ * mbox_channel_cb_t - Definition of the mailbox channel callback.
+ * @data:	Pointer to the data.
+ * @length:	Length of the data.
+ * @priv:	The client's private data.
+ *
+ * This function will be called upon reception of complete mbox channel PDU
+ * or after completion of send operation.
+ */
+typedef void mbox_channel_cb_t (u32 *data, u32 length, void *priv);
+
+/**
+ * struct mbox_channel_msg - Definition of mbox channel message
+ * @channel:	Channel number.
+ * @data:	Pointer to data to be sent.
+ * @length:	Length of data to be sent.
+ * @cb:	Pointer to the callback function to be called when send
+ *		operation will be finished.
+ * @priv:	The client's private data.
+ *
+ * This structure describes mailbox channel message.
+ */
+struct mbox_channel_msg {
+	u16			channel;
+	u32			*data;
+	u8			length;
+	mbox_channel_cb_t	*cb;
+	void			*priv;
+};
+
+/**
+ * mbox_channel_register - Set up a given mailbox channel.
+ * @channel:	Mailbox channel number.
+ * @cb:	Pointer to the callback function to be called when a new message
+ *		is received.
+ * @priv:	Client user data which will be returned in the callback.
+ *
+ * Returns 0 on success or a negative error code on error.
+ */
+int mbox_channel_register(u16 channel, mbox_channel_cb_t *cb, void *priv);
+
+/**
+ * mbox_channel_send - Send data on given mailbox channel.
+ * @msg:	Mailbox channel message to be sent.
+ *
+ * Returns 0 on success or a negative error code on error.
+ */
+int mbox_channel_send(struct mbox_channel_msg *msg);
+
+/**
+ * mbox_channel_revoke_messages - Revoke messages on given mailbox channel.
+ * @channel:	Mailbox channel number.
+ *
+ * Returns 0 on success or a negative error code on error.
+ */
+int mbox_channel_revoke_messages(u16 channel);
+
+#endif /*INC_STE_MBOX_H*/
+
diff --git a/arch/arm/mach-ux500/include/mach/memory.h b/arch/arm/mach-ux500/include/mach/memory.h
index 2ef697a6700..ada8ad0c2d8 100644
--- a/arch/arm/mach-ux500/include/mach/memory.h
+++ b/arch/arm/mach-ux500/include/mach/memory.h
@@ -15,4 +15,12 @@
 #define PLAT_PHYS_OFFSET	UL(0x00000000)
 #define BUS_OFFSET	UL(0x00000000)
 
+#ifdef CONFIG_UX500_SOC_DB8500
+/*
+ * STE NMF CM driver only used on the U8500 allocate using dma_alloc_coherent:
+ *    8M for SIA and SVA data + 2M for SIA code + 2M for SVA code
+ */
+#define CONSISTENT_DMA_SIZE SZ_16M
+#endif
+
 #endif
diff --git a/arch/arm/mach-ux500/include/mach/mloader-dbx500.h b/arch/arm/mach-ux500/include/mach/mloader-dbx500.h
new file mode 100644
index 00000000000..68fa55a3f53
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/mloader-dbx500.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Ludovic Barre <ludovic.barre@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#ifndef _MLOADER_UX500_H_
+#define _MLOADER_UX500_H_
+
+/**
+ * struct dbx500_ml_area - data structure for modem memory areas description
+ * @name: name of the area
+ * @start: start address of the area
+ * @size: size of the area
+ */
+struct dbx500_ml_area {
+	const char *name;
+	u32 start;
+	u32 size;
+};
+
+/**
+ * struct dbx500_ml_fw - data stucture for modem firmwares description
+ * @name: firmware name
+ * @area: area where firmware is uploaded
+ * @offset: offset in the area where firmware is uploaded
+ */
+struct dbx500_ml_fw {
+	const char *name;
+	struct dbx500_ml_area *area;
+	u32 offset;
+};
+
+/**
+ * struct dbx500_mloader_pdata - data structure for platform specific data
+ * @fws: pointer on firmwares table
+ * @nr_fws: number of firmwares
+ * @areas: pointer on areas table
+ * @nr_areas: number of areas
+ */
+struct dbx500_mloader_pdata {
+	struct dbx500_ml_fw *fws;
+	int nr_fws;
+	struct dbx500_ml_area *areas;
+	int	nr_areas;
+};
+
+#endif /* _MLOADER_UX500_H_ */
diff --git a/arch/arm/mach-ux500/include/mach/msp.h b/arch/arm/mach-ux500/include/mach/msp.h
new file mode 100644
index 00000000000..b5b9761ea45
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/msp.h
@@ -0,0 +1,972 @@
+/*
+ * Copyright (c) 2009 STMicroelectronics
+ *
+ * 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 _STM_MSP_HEADER
+#define _STM_MSP_HEADER
+#include <linux/device.h>
+#include <linux/spinlock.h>
+#include <linux/semaphore.h>
+#include <linux/dmaengine.h>
+#include <linux/i2s/i2s.h>
+#include <linux/irqreturn.h>
+#include <linux/bitops.h>
+#include <plat/ste_dma40.h>
+#include <linux/gpio.h>
+#include <linux/spi/stm_msp.h>
+
+/* Generic config struct. Use the actual values defined below for global
+ * control register
+ */
+
+enum msp_state {
+	MSP_STATE_IDLE = 0,
+	MSP_STATE_CONFIGURED = 1,
+	MSP_STATE_RUN = 2,
+};
+
+enum msp_rx_comparison_enable_mode {
+	MSP_COMPARISON_DISABLED = 0,
+	MSP_COMPARISON_NONEQUAL_ENABLED = 2,
+	MSP_COMPARISON_EQUAL_ENABLED = 3
+};
+
+#define RMCEN_BIT   0
+#define RMCSF_BIT   1
+#define RCMPM_BIT   3
+#define TMCEN_BIT   5
+#define TNCSF_BIT   6
+
+struct msp_multichannel_config {
+	bool rx_multichannel_enable;
+	bool tx_multichannel_enable;
+	enum msp_rx_comparison_enable_mode rx_comparison_enable_mode;
+	u8 padding;
+	u32 comparison_value;
+	u32 comparison_mask;
+	u32 rx_channel_0_enable;
+	u32 rx_channel_1_enable;
+	u32 rx_channel_2_enable;
+	u32 rx_channel_3_enable;
+	u32 tx_channel_0_enable;
+	u32 tx_channel_1_enable;
+	u32 tx_channel_2_enable;
+	u32 tx_channel_3_enable;
+};
+
+/**
+ * struct msp_protocol_desc- MSP Protocol desc structure per MSP.
+ * @rx_phase_mode: rx_phase_mode whether single or dual.
+ * @tx_phase_mode: tx_phase_mode whether single or dual.
+ * @rx_phase2_start_mode: rx_phase2_start_mode whether imediate or after
+ * some delay.
+ * @tx_phase2_start_mode: tx_phase2_start_mode whether imediate or after
+ * some delay.
+ * @rx_bit_transfer_format: MSP or LSB.
+ * @tx_bit_transfer_format: MSP or LSB.
+ * @rx_frame_length_1: Frame1 length 1,2,3..
+ * @rx_frame_length_2: Frame2 length 1,2,3..
+ * @tx_frame_length_1: Frame1 length 1,2,3..
+ * @tx_frame_length_2: Frame2 length 1,2,3..
+ * @rx_element_length_1: Element1 length 1,2,...
+ * @rx_element_length_2: Element2 length 1,2,...
+ * @tx_element_length_1: Element1 length 1,2,...
+ * @tx_element_length_2: Element2 length 1,2,...
+ * @rx_data_delay: Delay in clk cycle after frame sync
+ * @tx_data_delay: Delay in clk cycle after frame sync
+ * @rx_clock_pol: Rxpol whether rising or falling.It indicates pol of bit clock.
+ * @tx_clock_pol: Txpol whether rising or falling.It indicates pol of bit clock.
+ * @rx_frame_sync_pol: Frame sync pol whether rising or Falling.
+ * @tx_frame_sync_pol: Frame sync pol whether rising or Falling.
+ * @rx_half_word_swap: Word swap half word, full word.
+ * @tx_half_word_swap: Word swap half word, full word.
+ * @compression_mode: Compression mode whether Alaw or Ulaw or disabled.
+ * @expansion_mode: Compression mode whether Alaw or Ulaw or disabled.
+ * @spi_clk_mode: Spi clock mode to be enabled or not.
+ * @spi_burst_mode: Spi burst mode to be enabled or not.
+ * @frame_sync_ignore: Frame sync to be ignored or not. Ignore in case of Audio
+ * codec acting as Master.
+ * @frame_period: Frame period (clk cycles) after which new frame sync occurs.
+ * @frame_width: Frame width (clk cycles) after which frame sycn changes state.
+ * @total_clocks_for_one_frame: No. of clk cycles per frame.
+ *
+ * Main Msp protocol descriptor data structure to be used to store various info
+ * in transmit or recevie configuration registers of an MSP.
+ */
+
+struct msp_protocol_desc {
+	u32 rx_phase_mode;
+	u32 tx_phase_mode;
+	u32 rx_phase2_start_mode;
+	u32 tx_phase2_start_mode;
+	u32 rx_bit_transfer_format;
+	u32 tx_bit_transfer_format;
+	u32 rx_frame_length_1;
+	u32 rx_frame_length_2;
+	u32 tx_frame_length_1;
+	u32 tx_frame_length_2;
+	u32 rx_element_length_1;
+	u32 rx_element_length_2;
+	u32 tx_element_length_1;
+	u32 tx_element_length_2;
+	u32 rx_data_delay;
+	u32 tx_data_delay;
+	u32 rx_clock_pol;
+	u32 tx_clock_pol;
+	u32 rx_frame_sync_pol;
+	u32 tx_frame_sync_pol;
+	u32 rx_half_word_swap;
+	u32 tx_half_word_swap;
+	u32 compression_mode;
+	u32 expansion_mode;
+	u32 spi_clk_mode;
+	u32 spi_burst_mode;
+	u32 frame_sync_ignore;
+	u32 frame_period;
+	u32 frame_width;
+	u32 total_clocks_for_one_frame;
+};
+
+/**
+ * struct trans_data - MSP transfer data structure used during xfer.
+ * @message: i2s message.
+ * @msp: msp structure.
+ * @tx_handler: callback handler for transmit path.
+ * @rx_handler: callback handler for receive path.
+ * @tx_callback_data: callback data for transmit.
+ * @rx_callback_data: callback data for receive.
+ *
+ */
+struct trans_data {
+	struct i2s_message message;
+	struct msp *msp;
+	void (*tx_handler) (void *data);
+	void (*rx_handler) (void *data);
+	void *tx_callback_data;
+	void *rx_callback_data;
+};
+
+/**
+ * struct msp_config- MSP configuration structure used by i2s client.
+ * @input_clock_freq: Input clock frequency default is 48MHz.
+ * @rx_clock_sel: Receive clock selection (Provided by Sample Gen or external
+ * source).
+ * @tx_clock_sel: Transmit clock selection (Provided by Sample Gen or external.
+ * source).
+ * @srg_clock_sel: APB clock or clock dervied from Slave (Audio codec).
+ * @rx_frame_sync_pol: Receive frame sync polarity.
+ * @tx_frame_sync_pol: Transmit frame sync polarity.
+ * @rx_frame_sync_sel: Rx frame sync signal is provided by which source.
+ * External source or by frame generator logic.
+ * @tx_frame_sync_sel: Tx frame sync signal is provided by which source.
+ * External source or by frame generator logic.
+ * @rx_fifo_config: Receive fifo enable or not.
+ * @tx_fifo_config: Transmit fifo enable or not.
+ * @spi_clk_mode: In case of SPI protocol spi modes: Normal, Zero delay or
+ * half cycle delay.
+ * @spi_burst_mode: Spi burst mode is enabled or not.
+ * @loopback_enable: Loopback mode.
+ * @tx_data_enable: Transmit extra delay enable.
+ * @default_protocol_desc: Flag to indicate client defined protocol desc or
+ * statically defined in msp.h.
+ * @protocol_desc: Protocol desc structure filled by i2s client driver.
+ * In case client defined default_prtocol_desc as 0.
+ * @multichannel_configured: multichannel configuration structure.
+ * @multichannel_config: multichannel is enabled or not.
+ * @direction: Transmit, Receive or Both.
+ * @work_mode: Dma, Polling or Interrupt.
+ * @protocol: I2S, PCM, etc.
+ * @frame_freq: Sampling freq at which data is sampled.
+ * @frame_size: size of element.
+ * @data_size: data size which defines the format in which data is written on
+ * transmit or receive fifo. Only three modes 8,16,32 are supported.
+ * @def_elem_len: Flag to indicate whether default element length is to be used
+ * or should be changed acc to data size defined by user at run time.
+ * @iodelay: value for the MSP_IODLY register
+ * @handler: callback handler in case of interrupt or dma.
+ * @tx_callback_data: Callback data for transmit.
+ * @rx_callback_data: Callback data for receive.
+ *
+ * Main Msp configuration data structure used by i2s client driver to fill
+ * various info like data size, frequency etc.
+ */
+struct msp_config {
+	unsigned int input_clock_freq;
+	unsigned int rx_clock_sel;
+	unsigned int tx_clock_sel;
+	unsigned int srg_clock_sel;
+	unsigned int rx_frame_sync_pol;
+	unsigned int tx_frame_sync_pol;
+	unsigned int rx_frame_sync_sel;
+	unsigned int tx_frame_sync_sel;
+	unsigned int rx_fifo_config;
+	unsigned int tx_fifo_config;
+	unsigned int spi_clk_mode;
+	unsigned int spi_burst_mode;
+	unsigned int loopback_enable;
+	unsigned int tx_data_enable;
+	unsigned int default_protocol_desc;
+	struct msp_protocol_desc protocol_desc;
+	int multichannel_configured;
+	struct msp_multichannel_config multichannel_config;
+	unsigned int direction;
+	unsigned int work_mode;
+	unsigned int protocol;
+	unsigned int frame_freq;
+	unsigned int frame_size;
+	enum msp_data_size data_size;
+	unsigned int def_elem_len;
+	unsigned int iodelay;
+	void (*handler) (void *data);
+	void *tx_callback_data;
+	void *rx_callback_data;
+
+};
+
+/*** Protocols ***/
+enum msp_protocol {
+	MSP_I2S_PROTOCOL,
+	MSP_PCM_PROTOCOL,
+	MSP_PCM_COMPAND_PROTOCOL,
+	MSP_AC97_PROTOCOL,
+	MSP_MASTER_SPI_PROTOCOL,
+	MSP_SLAVE_SPI_PROTOCOL,
+	MSP_INVALID_PROTOCOL
+};
+
+/*** Sample Frequencies ***/
+/* These are no longer required, frequencies in Hz can be used directly */
+enum msp_sample_freq {
+	MSP_SAMPLE_FREQ_NOT_SUPPORTED = -1,
+	MSP_SAMPLE_FREQ_8KHZ = 8000,
+	MSP_SAMPLE_FREQ_12KHZ = 12000,
+	MSP_SAMPLE_FREQ_16KHZ = 16000,
+	MSP_SAMPLE_FREQ_24KHZ = 24000,
+	MSP_SAMPLE_FREQ_32KHZ = 32000,
+	MSP_SAMPLE_FREQ_44KHZ = 44000,
+	MSP_SAMPLE_FREQ_48KHZ = 48000,
+	MSP_SAMPLE_FREQ_64KHZ = 64000,
+	MSP_SAMPLE_FREQ_88KHZ = 88000,
+	MSP_SAMPLE_FREQ_96KHZ = 96000,
+	MSP_SAMPLE_FREQ_22KHZ = 22000,
+	MSP_SAMPLE_FREQ_11KHZ = 11000
+};
+
+/*** Input Frequencies ***/
+/* These are no longer required, frequencies in Hz can be used directly */
+enum msp_in_clock_freq {
+	MSP_INPUT_FREQ_1MHZ = 1000,
+	MSP_INPUT_FREQ_2MHZ = 2000,
+	MSP_INPUT_FREQ_3MHZ = 3000,
+	MSP_INPUT_FREQ_4MHZ = 4000,
+	MSP_INPUT_FREQ_5MHZ = 5000,
+	MSP_INPUT_FREQ_6MHZ = 6000,
+	MSP_INPUT_FREQ_8MHZ = 8000,
+	MSP_INPUT_FREQ_11MHZ = 11000,
+	MSP_INPUT_FREQ_12MHZ = 12000,
+	MSP_INPUT_FREQ_16MHZ = 16000,
+	MSP_INPUT_FREQ_22MHZ = 22000,
+	MSP_INPUT_FREQ_24MHZ = 24000,
+	MSP_INPUT_FREQ_48MHZ = 48000
+};
+
+#define MSP_INPUT_FREQ_APB 48000000
+
+/*** Stereo mode. Used for APB data accesses as 16 bits accesses (mono),
+ *   32 bits accesses (stereo).
+ ***/
+enum msp_stereo_mode {
+	MSP_MONO,
+	MSP_STEREO
+};
+
+/* Direction (Transmit/Receive mode) */
+enum msp_direction {
+	MSP_TRANSMIT_MODE,
+	MSP_RECEIVE_MODE,
+	MSP_BOTH_T_R_MODE
+};
+
+/* Dma mode should be used for large transfers,
+ * polling mode should be used for transfers of a few bytes
+ */
+enum msp_xfer_mode {
+	MSP_DMA_MODE,
+	MSP_POLLING_MODE,
+	MSP_INTERRUPT_MODE
+};
+
+/* User client for the MSP */
+enum msp_user {
+	MSP_NO_USER = 0,
+	MSP_USER_SPI,
+	MSP_USER_ALSA,
+	MSP_USER_SAA,
+};
+
+/*Flag structure for MSPx*/
+struct msp_flag {
+	struct semaphore lock;
+	enum msp_user user;
+};
+
+/* User client for the MSP */
+enum msp_mode {
+	MSP_NO_MODE = 0,
+	MSP_MODE_SPI,
+	MSP_MODE_NON_SPI,
+};
+
+/* Transmit and receive configuration register */
+#define MSP_BIG_ENDIAN           0x00000000
+#define MSP_LITTLE_ENDIAN        0x00001000
+#define MSP_UNEXPECTED_FS_ABORT  0x00000000
+#define MSP_UNEXPECTED_FS_IGNORE 0x00008000
+#define MSP_NON_MODE_BIT_MASK    0x00009000
+
+/* Global configuration register */
+#define RX_ENABLE             0x00000001
+#define RX_FIFO_ENABLE        0x00000002
+#define RX_SYNC_SRG           0x00000010
+#define RX_CLK_POL_RISING     0x00000020
+#define RX_CLK_SEL_SRG        0x00000040
+#define TX_ENABLE             0x00000100
+#define TX_FIFO_ENABLE        0x00000200
+#define TX_SYNC_SRG_PROG      0x00001800
+#define TX_SYNC_SRG_AUTO      0x00001000
+#define TX_CLK_POL_RISING     0x00002000
+#define TX_CLK_SEL_SRG        0x00004000
+#define TX_EXTRA_DELAY_ENABLE 0x00008000
+#define SRG_ENABLE            0x00010000
+#define FRAME_GEN_ENABLE      0x00100000
+#define SRG_CLK_SEL_APB       0x00000000
+#define RX_FIFO_SYNC_HI       0x00000000
+#define TX_FIFO_SYNC_HI       0x00000000
+#define SPI_CLK_MODE_NORMAL   0x00000000
+
+/* SPI Clock Modes enumertion
+ * SPI clock modes of MSP provides compatibility with
+ * the SPI protocol.MSP supports 2 SPI transfer formats.
+ * MSP_ZERO_DELAY_SPI_MODE:MSP transmits data over Tx/Rx
+ * Lines immediately after MSPTCK/MSPRCK rising/falling edge.
+ * MSP_HALF_CYCLE_DELY_SPI_MODE:MSP transmits data  one-half cycle
+ * ahead of the rising/falling edge of the MSPTCK
+ */
+
+#define MSP_FRAME_SIZE_AUTO -1
+
+
+#define MSP_DR 		0x00
+#define MSP_GCR		0x04
+#define MSP_TCF		0x08
+#define MSP_RCF		0x0c
+#define MSP_SRG		0x10
+#define MSP_FLR		0x14
+#define MSP_DMACR	0x18
+
+#define MSP_IMSC	0x20
+#define MSP_RIS		0x24
+#define MSP_MIS 	0x28
+#define MSP_ICR		0x2c
+#define MSP_MCR		0x30
+#define MSP_RCV 	0x34
+#define MSP_RCM 	0x38
+
+#define MSP_TCE0	0x40
+#define MSP_TCE1	0x44
+#define MSP_TCE2 	0x48
+#define MSP_TCE3 	0x4c
+
+#define MSP_RCE0	0x60
+#define MSP_RCE1	0x64
+#define MSP_RCE2	0x68
+#define MSP_RCE3 	0x6c
+#define MSP_IODLY	0x70
+
+#define MSP_ITCR	0x80
+#define MSP_ITIP	0x84
+#define MSP_ITOP	0x88
+#define MSP_TSTDR	0x8c
+
+#define MSP_PID0	0xfe0
+#define MSP_PID1	0xfe4
+#define MSP_PID2 	0xfe8
+#define MSP_PID3	0xfec
+
+#define MSP_CID0	0xff0
+#define MSP_CID1 	0xff4
+#define MSP_CID2	0xff8
+#define MSP_CID3 	0xffc
+
+/* Single or dual phase mode */
+enum msp_phase_mode {
+	MSP_SINGLE_PHASE,
+	MSP_DUAL_PHASE
+};
+
+/* Frame length */
+enum msp_frame_length {
+	MSP_FRAME_LENGTH_1 = 0,
+	MSP_FRAME_LENGTH_2 = 1,
+	MSP_FRAME_LENGTH_4 = 3,
+	MSP_FRAME_LENGTH_8 = 7,
+	MSP_FRAME_LENGTH_12 = 11,
+	MSP_FRAME_LENGTH_16 = 15,
+	MSP_FRAME_LENGTH_20 = 19,
+	MSP_FRAME_LENGTH_32 = 31,
+	MSP_FRAME_LENGTH_48 = 47,
+	MSP_FRAME_LENGTH_64 = 63
+};
+
+/* Element length */
+enum msp_elem_length {
+	MSP_ELEM_LENGTH_8 = 0,
+	MSP_ELEM_LENGTH_10 = 1,
+	MSP_ELEM_LENGTH_12 = 2,
+	MSP_ELEM_LENGTH_14 = 3,
+	MSP_ELEM_LENGTH_16 = 4,
+	MSP_ELEM_LENGTH_20 = 5,
+	MSP_ELEM_LENGTH_24 = 6,
+	MSP_ELEM_LENGTH_32 = 7
+};
+
+enum msp_data_xfer_width {
+	MSP_DATA_TRANSFER_WIDTH_BYTE,
+	MSP_DATA_TRANSFER_WIDTH_HALFWORD,
+	MSP_DATA_TRANSFER_WIDTH_WORD
+};
+
+enum msp_frame_sync {
+	MSP_FRAME_SYNC_UNIGNORE = 0,
+	MSP_FRAME_SYNC_IGNORE = 1,
+
+};
+
+enum msp_phase2_start_mode {
+	MSP_PHASE2_START_MODE_IMEDIATE,
+	MSP_PHASE2_START_MODE_FRAME_SYNC
+};
+
+enum msp_btf {
+	MSP_BTF_MS_BIT_FIRST = 0,
+	MSP_BTF_LS_BIT_FIRST = 1
+};
+
+enum msp_frame_sync_pol {
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH = 0,
+	MSP_FRAME_SYNC_POL_ACTIVE_LOW = 1
+};
+
+/* Data delay (in bit clock cycles) */
+enum msp_delay {
+	MSP_DELAY_0 = 0,
+	MSP_DELAY_1 = 1,
+	MSP_DELAY_2 = 2,
+	MSP_DELAY_3 = 3
+};
+
+/* Configurations of clocks (transmit, receive or sample rate generator) */
+enum msp_edge {
+	MSP_FALLING_EDGE = 0,
+	MSP_RISING_EDGE = 1,
+};
+
+enum msp_hws {
+	MSP_HWS_NO_SWAP = 0,
+	MSP_HWS_BYTE_SWAP_IN_WORD = 1,
+	MSP_HWS_BYTE_SWAP_IN_EACH_HALF_WORD = 2,
+	MSP_HWS_HALF_WORD_SWAP_IN_WORD = 3
+};
+
+enum msp_compress_mode {
+	MSP_COMPRESS_MODE_LINEAR = 0,
+	MSP_COMPRESS_MODE_MU_LAW = 2,
+	MSP_COMPRESS_MODE_A_LAW = 3
+};
+
+enum msp_spi_clock_mode {
+	MSP_SPI_CLOCK_MODE_NON_SPI = 0,
+	MSP_SPI_CLOCK_MODE_ZERO_DELAY = 2,
+	MSP_SPI_CLOCK_MODE_HALF_CYCLE_DELAY = 3
+};
+
+enum msp_spi_burst_mode {
+	MSP_SPI_BURST_MODE_DISABLE = 0,
+	MSP_SPI_BURST_MODE_ENABLE = 1
+};
+
+enum msp_expand_mode {
+	MSP_EXPAND_MODE_LINEAR = 0,
+	MSP_EXPAND_MODE_LINEAR_SIGNED = 1,
+	MSP_EXPAND_MODE_MU_LAW = 2,
+	MSP_EXPAND_MODE_A_LAW = 3
+};
+
+/* Protocol dependant parameters list */
+#define RX_ENABLE_MASK         BIT(0)
+#define RX_FIFO_ENABLE_MASK    BIT(1)
+#define RX_FRAME_SYNC_MASK     BIT(2)
+#define DIRECT_COMPANDING_MASK BIT(3)
+#define RX_SYNC_SEL_MASK       BIT(4)
+#define RX_CLK_POL_MASK        BIT(5)
+#define RX_CLK_SEL_MASK        BIT(6)
+#define LOOPBACK_MASK          BIT(7)
+#define TX_ENABLE_MASK         BIT(8)
+#define TX_FIFO_ENABLE_MASK    BIT(9)
+#define TX_FRAME_SYNC_MASK     BIT(10)
+#define TX_MSP_TDR_TSR         BIT(11)
+#define TX_SYNC_SEL_MASK       (BIT(12) | BIT(11))
+#define TX_CLK_POL_MASK        BIT(13)
+#define TX_CLK_SEL_MASK        BIT(14)
+#define TX_EXTRA_DELAY_MASK    BIT(15)
+#define SRG_ENABLE_MASK        BIT(16)
+#define SRG_CLK_POL_MASK       BIT(17)
+#define SRG_CLK_SEL_MASK       (BIT(19) | BIT(18))
+#define FRAME_GEN_EN_MASK      BIT(20)
+#define SPI_CLK_MODE_MASK      (BIT(22) | BIT(21))
+#define SPI_BURST_MODE_MASK    BIT(23)
+
+#define RXEN_SHIFT		0
+#define RFFEN_SHIFT		1
+#define RFSPOL_SHIFT		2
+#define DCM_SHIFT		3
+#define RFSSEL_SHIFT		4
+#define RCKPOL_SHIFT		5
+#define RCKSEL_SHIFT		6
+#define LBM_SHIFT		7
+#define TXEN_SHIFT		8
+#define TFFEN_SHIFT		9
+#define TFSPOL_SHIFT		10
+#define TFSSEL_SHIFT		11
+#define TCKPOL_SHIFT		13
+#define TCKSEL_SHIFT		14
+#define TXDDL_SHIFT		15
+#define SGEN_SHIFT		16
+#define SCKPOL_SHIFT		17
+#define SCKSEL_SHIFT		18
+#define FGEN_SHIFT		20
+#define SPICKM_SHIFT		21
+#define TBSWAP_SHIFT      28
+
+#define RCKPOL_MASK BIT(0)
+#define TCKPOL_MASK BIT(0)
+#define SPICKM_MASK (BIT(1) | BIT(0))
+#define MSP_RX_CLKPOL_BIT(n)     ((n & RCKPOL_MASK) << RCKPOL_SHIFT)
+#define MSP_TX_CLKPOL_BIT(n)     ((n & TCKPOL_MASK) << TCKPOL_SHIFT)
+#define MSP_SPI_CLK_MODE_BITS(n) ((n & SPICKM_MASK) << SPICKM_SHIFT)
+
+
+
+/* Use this to clear the clock mode bits to non-spi */
+#define MSP_NON_SPI_CLK_MASK (BIT(22) | BIT(21))
+
+#define P1ELEN_SHIFT		0
+#define P1FLEN_SHIFT		3
+#define DTYP_SHIFT		10
+#define ENDN_SHIFT		12
+#define DDLY_SHIFT		13
+#define FSIG_SHIFT		15
+#define P2ELEN_SHIFT		16
+#define P2FLEN_SHIFT		19
+#define P2SM_SHIFT		26
+#define P2EN_SHIFT		27
+#define FRAME_SYNC_SHIFT		15
+
+
+#define P1ELEN_MASK 0x00000007
+#define P2ELEN_MASK 0x00070000
+#define P1FLEN_MASK 0x00000378
+#define P2FLEN_MASK 0x03780000
+#define DDLY_MASK 0x00003000
+#define DTYP_MASK 0x00000600
+#define P2SM_MASK 0x04000000
+#define P2EN_MASK 0x08000000
+#define ENDN_MASK 0x00001000
+#define TFSPOL_MASK 0x00000400
+#define TBSWAP_MASK 0x30000000
+#define COMPANDING_MODE_MASK 0x00000c00
+#define FRAME_SYNC_MASK 0x00008000
+
+#define MSP_P1_ELEM_LEN_BITS(n) (n & P1ELEN_MASK)
+#define MSP_P2_ELEM_LEN_BITS(n)  (((n) << P2ELEN_SHIFT) & P2ELEN_MASK)
+#define MSP_P1_FRAME_LEN_BITS(n) (((n) << P1FLEN_SHIFT) & P1FLEN_MASK)
+#define MSP_P2_FRAME_LEN_BITS(n) (((n) << P2FLEN_SHIFT) & P2FLEN_MASK)
+#define MSP_DATA_DELAY_BITS(n)   (((n) << DDLY_SHIFT) & DDLY_MASK)
+#define MSP_DATA_TYPE_BITS(n)    (((n) << DTYP_SHIFT) & DTYP_MASK)
+#define MSP_P2_START_MODE_BIT(n) ((n << P2SM_SHIFT) & P2SM_MASK)
+#define MSP_P2_ENABLE_BIT(n)     ((n << P2EN_SHIFT) & P2EN_MASK)
+#define MSP_SET_ENDIANNES_BIT(n) ((n << ENDN_SHIFT) & ENDN_MASK)
+#define MSP_FRAME_SYNC_POL(n)	((n << TFSPOL_SHIFT) & TFSPOL_MASK)
+#define MSP_DATA_WORD_SWAP(n)	((n << TBSWAP_SHIFT) & TBSWAP_MASK)
+#define MSP_SET_COMPANDING_MODE(n) ((n << DTYP_SHIFT) & COMPANDING_MODE_MASK)
+#define MSP_SET_FRAME_SYNC_IGNORE(n) ((n << FRAME_SYNC_SHIFT) & \
+				      FRAME_SYNC_MASK)
+
+/* Flag register */
+#define RX_BUSY       BIT(0)
+#define RX_FIFO_EMPTY BIT(1)
+#define RX_FIFO_FULL  BIT(2)
+#define TX_BUSY       BIT(3)
+#define TX_FIFO_EMPTY BIT(4)
+#define TX_FIFO_FULL  BIT(5)
+
+#define RBUSY_SHIFT		0
+#define RFE_SHIFT			1
+#define RFU_SHIFT			2
+#define TBUSY_SHIFT		3
+#define TFE_SHIFT			4
+#define TFU_SHIFT			5
+
+/* Multichannel control register */
+#define RMCEN_SHIFT		0
+#define RMCSF_SHIFT		1
+#define RCMPM_SHIFT		3
+#define TMCEN_SHIFT		5
+#define TNCSF_SHIFT		6
+
+/* Sample rate generator register */
+#define SCKDIV_SHIFT		0
+#define FRWID_SHIFT		10
+#define FRPER_SHIFT		16
+
+#define SCK_DIV_MASK 0x0000003FF
+#define FRAME_WIDTH_BITS(n) (((n) << FRWID_SHIFT)  & 0x0000FC00)
+#define FRAME_PERIOD_BITS(n) (((n) << FRPER_SHIFT) & 0x1FFF0000)
+
+/* DMA controller register */
+#define RX_DMA_ENABLE BIT(0)
+#define TX_DMA_ENABLE BIT(1)
+
+#define RDMAE_SHIFT		0
+#define TDMAE_SHIFT		1
+
+/* Interrupt Register */
+#define RECEIVE_SERVICE_INT         BIT(0)
+#define RECEIVE_OVERRUN_ERROR_INT   BIT(1)
+#define RECEIVE_FRAME_SYNC_ERR_INT  BIT(2)
+#define RECEIVE_FRAME_SYNC_INT      BIT(3)
+#define TRANSMIT_SERVICE_INT        BIT(4)
+#define TRANSMIT_UNDERRUN_ERR_INT   BIT(5)
+#define TRANSMIT_FRAME_SYNC_ERR_INT BIT(6)
+#define TRANSMIT_FRAME_SYNC_INT     BIT(7)
+#define ALL_INT                     0x000000ff
+
+/*
+ *  Protocol configuration values I2S:
+ *  Single phase, 16 bits, 2 words per frame
+ */
+#define I2S_PROTOCOL_DESC				\
+{							\
+	MSP_SINGLE_PHASE,				\
+	MSP_SINGLE_PHASE,				\
+	MSP_PHASE2_START_MODE_IMEDIATE, 		\
+	MSP_PHASE2_START_MODE_IMEDIATE, 		\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_ELEM_LENGTH_32,				\
+	MSP_ELEM_LENGTH_32,				\
+	MSP_ELEM_LENGTH_32,				\
+	MSP_ELEM_LENGTH_32,				\
+	MSP_DELAY_1,					\
+	MSP_DELAY_1,					\
+	MSP_RISING_EDGE,				\
+	MSP_RISING_EDGE,				\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_HWS_NO_SWAP,				\
+	MSP_HWS_NO_SWAP,				\
+	MSP_COMPRESS_MODE_LINEAR,			\
+	MSP_EXPAND_MODE_LINEAR,				\
+	MSP_SPI_CLOCK_MODE_NON_SPI,			\
+	MSP_SPI_BURST_MODE_DISABLE,			\
+	MSP_FRAME_SYNC_IGNORE,			\
+	31,						\
+	15,						\
+	32,						\
+}
+
+#define PCM_PROTOCOL_DESC				\
+{							\
+	MSP_DUAL_PHASE,				\
+	MSP_DUAL_PHASE,				\
+	MSP_PHASE2_START_MODE_FRAME_SYNC, 		\
+	MSP_PHASE2_START_MODE_FRAME_SYNC, 		\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_ELEM_LENGTH_16,				\
+	MSP_ELEM_LENGTH_16,				\
+	MSP_ELEM_LENGTH_16,				\
+	MSP_ELEM_LENGTH_16,				\
+	MSP_DELAY_0,					\
+	MSP_DELAY_0,					\
+	MSP_FALLING_EDGE,				\
+	MSP_FALLING_EDGE,				\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_HWS_NO_SWAP,				\
+	MSP_HWS_NO_SWAP,				\
+	MSP_COMPRESS_MODE_LINEAR,			\
+	MSP_EXPAND_MODE_LINEAR,				\
+	MSP_SPI_CLOCK_MODE_NON_SPI,			\
+	MSP_SPI_BURST_MODE_DISABLE,			\
+	MSP_FRAME_SYNC_IGNORE,			\
+	255,						\
+	0,						\
+	256,						\
+}
+
+/* Companded PCM: Single phase, 8 bits, 1 word per frame */
+#define PCM_COMPAND_PROTOCOL_DESC				\
+{							\
+	MSP_SINGLE_PHASE,				\
+	MSP_SINGLE_PHASE,				\
+	MSP_PHASE2_START_MODE_FRAME_SYNC, 		\
+	MSP_PHASE2_START_MODE_FRAME_SYNC, 		\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_DELAY_0,					\
+	MSP_DELAY_0,					\
+	MSP_FALLING_EDGE,				\
+	MSP_RISING_EDGE,				\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_HWS_NO_SWAP,				\
+	MSP_HWS_NO_SWAP,				\
+	MSP_COMPRESS_MODE_LINEAR,			\
+	MSP_EXPAND_MODE_LINEAR,				\
+	MSP_SPI_CLOCK_MODE_NON_SPI,			\
+	MSP_SPI_BURST_MODE_DISABLE,			\
+	MSP_FRAME_SYNC_IGNORE,			\
+	255,						\
+	0,						\
+	256,						\
+}
+
+/*
+ * AC97: Double phase, 1 element of 16 bits during first phase,
+ * 12 elements of 20 bits in second phase.
+ */
+#define AC97_PROTOCOL_DESC				\
+{							\
+	MSP_DUAL_PHASE,				\
+	MSP_DUAL_PHASE,				\
+	MSP_PHASE2_START_MODE_FRAME_SYNC, 		\
+	MSP_PHASE2_START_MODE_FRAME_SYNC, 		\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_12,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_12,				\
+	MSP_ELEM_LENGTH_16,				\
+	MSP_ELEM_LENGTH_20,				\
+	MSP_ELEM_LENGTH_16,				\
+	MSP_ELEM_LENGTH_20,				\
+	MSP_DELAY_1,					\
+	MSP_DELAY_1,					\
+	MSP_FALLING_EDGE,				\
+	MSP_RISING_EDGE,				\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_HWS_NO_SWAP,				\
+	MSP_HWS_NO_SWAP,				\
+	MSP_COMPRESS_MODE_LINEAR,			\
+	MSP_EXPAND_MODE_LINEAR,				\
+	MSP_SPI_CLOCK_MODE_NON_SPI,			\
+	MSP_SPI_BURST_MODE_DISABLE,			\
+	MSP_FRAME_SYNC_IGNORE,			\
+	255,						\
+	0,						\
+	256,						\
+}
+
+#define SPI_MASTER_PROTOCOL_DESC				\
+{							\
+	MSP_SINGLE_PHASE,				\
+	MSP_SINGLE_PHASE,				\
+	MSP_PHASE2_START_MODE_FRAME_SYNC, 		\
+	MSP_PHASE2_START_MODE_FRAME_SYNC, 		\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_DELAY_1,					\
+	MSP_DELAY_1,					\
+	MSP_RISING_EDGE,				\
+	MSP_FALLING_EDGE,				\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_HWS_NO_SWAP,				\
+	MSP_HWS_NO_SWAP,				\
+	MSP_COMPRESS_MODE_LINEAR,			\
+	MSP_EXPAND_MODE_LINEAR,				\
+	MSP_SPI_CLOCK_MODE_NON_SPI,			\
+	MSP_SPI_BURST_MODE_DISABLE,			\
+	MSP_FRAME_SYNC_IGNORE,			\
+	255,						\
+	0,						\
+	256,						\
+}
+
+#define SPI_SLAVE_PROTOCOL_DESC				\
+{							\
+	MSP_SINGLE_PHASE,				\
+	MSP_SINGLE_PHASE,				\
+	MSP_PHASE2_START_MODE_FRAME_SYNC, 		\
+	MSP_PHASE2_START_MODE_FRAME_SYNC, 		\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_BTF_MS_BIT_FIRST,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_DELAY_1,					\
+	MSP_DELAY_1,					\
+	MSP_RISING_EDGE,				\
+	MSP_FALLING_EDGE,				\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,			\
+	MSP_HWS_NO_SWAP,				\
+	MSP_HWS_NO_SWAP,				\
+	MSP_COMPRESS_MODE_LINEAR,			\
+	MSP_EXPAND_MODE_LINEAR,				\
+	MSP_SPI_CLOCK_MODE_NON_SPI,			\
+	MSP_SPI_BURST_MODE_DISABLE,			\
+	MSP_FRAME_SYNC_IGNORE,			\
+	255,						\
+	0,						\
+	256,						\
+}
+
+#define MSP_FRAME_PERIOD_IN_MONO_MODE 256
+#define MSP_FRAME_PERIOD_IN_STEREO_MODE 32
+#define MSP_FRAME_WIDTH_IN_STEREO_MODE 16
+
+/*
+ * No of registers to backup during
+ * suspend resume
+ */
+#define MAX_MSP_BACKUP_REGS 36
+
+enum enum_i2s_controller {
+	MSP_0_I2S_CONTROLLER = 1,
+	MSP_1_I2S_CONTROLLER,
+	MSP_2_I2S_CONTROLLER,
+	MSP_3_I2S_CONTROLLER,
+};
+
+/**
+ * struct msp - Main msp controller data structure per MSP.
+ * @work_mode: Mode i.e dma, polling or interrupt.
+ * @id: Controller id like MSP1 or MSP2 etc.
+ * @msp_io_error: To indicate error while transferring.
+ * @registers: MSP's register base address.
+ * @actual_data_size: Data size in which data needs to send or receive.
+ * @irq:  MSP's irq number.
+ * @i2s_cont: MSP's Controller's structure pointer created per MSP.
+ * @lock: semaphore lock acquired while configuring msp.
+ * @dma_cfg_tx: TX DMA configuration
+ * @dma_cfg_rx: RX DMA configuration
+ * @tx_pipeid: TX DMA channel
+ * @rx_pipeid: RX DMA channel
+ * @msp_state: Current state of msp.
+ * @read: Function pointer for read, u8_msp_read,u16_msp_read,u32_msp_read.
+ * @write: Function pointer for write, u8_msp_write,u16_msp_write,u32_msp_write.
+ * @transfer: Function pointer for type of transfer i.e dma,polling or interrupt
+ * @xfer_data: MSP's transfer data structure. Contains info about current xfer.
+ * @plat_init: MSP's initialization function.
+ * @plat_exit: MSP's Exit function.
+ * @notify_timer: Timer used in Polling mode to prevent hang.
+ * @polling_flag: Flag used in error handling while polling.
+ * @def_elem_len: Flag indicates whether default elem len to be used in
+ * protocol_desc or not.
+ * @reg_enabled: Flag indicates whether regulator has been enabled or not.
+ * @vape_opp_constraint: 1 if constraint is applied to have vape at 100OPP; 0 otherwise
+ * @infinite: true if an infinite transfer has been configured
+ *
+ * Main Msp private data structure to be used to store various info of a
+ * particular MSP.Longer description
+ */
+struct msp {
+	int work_mode;
+	enum enum_i2s_controller id;
+	int msp_io_error;
+	void __iomem *registers;
+	enum msp_data_size actual_data_size;
+	int irq;
+	struct i2s_controller *i2s_cont;
+	struct semaphore lock;
+	struct stedma40_chan_cfg *dma_cfg_rx;
+	struct stedma40_chan_cfg *dma_cfg_tx;
+	struct dma_chan *tx_pipeid;
+	struct dma_chan *rx_pipeid;
+	enum msp_state msp_state;
+	void (*read) (struct trans_data *xfer_data);
+	void (*write) (struct trans_data *xfer_data);
+	int (*transfer) (struct msp *msp, struct i2s_message *message);
+	struct trans_data xfer_data;
+	int (*plat_init) (void);
+	int (*plat_exit) (void);
+	struct timer_list notify_timer;
+	int polling_flag;
+	int def_elem_len;
+	struct clk *clk;
+	unsigned int direction;
+	int users;
+	int reg_enabled;
+	int loopback_enable;
+	u32 backup_regs[MAX_MSP_BACKUP_REGS];
+	int vape_opp_constraint;
+	bool infinite;
+};
+
+/**
+ * struct msp_i2s_platform_data - Main msp controller platform data structure.
+ * @id: Controller id like MSP1 or MSP2 etc.
+ * @msp_i2s_dma_rx: RX DMA channel config
+ * @msp_i2s_dma_tx: RX DMA channel config
+ * @msp_i2s_init: MSP's initialization function.
+ * @msp_i2s_exit: MSP's Exit function.
+ * @backup_regs: used for backup registers during suspend resume.
+ *
+ * Platform data structure passed by devices.c file.
+ */
+struct msp_i2s_platform_data {
+	enum enum_i2s_controller id;
+	struct stedma40_chan_cfg *msp_i2s_dma_rx;
+	struct stedma40_chan_cfg *msp_i2s_dma_tx;
+	int (*msp_i2s_init) (void);
+	int (*msp_i2s_exit) (void);
+};
+
+#endif
diff --git a/drivers/mfd/db5500-prcmu-regs.h b/arch/arm/mach-ux500/include/mach/prcmu-regs.h
index 9a8e9e4ddd3..4f6f2f0394c 100644
--- a/drivers/mfd/db5500-prcmu-regs.h
+++ b/arch/arm/mach-ux500/include/mach/prcmu-regs.h
@@ -15,6 +15,45 @@
 
 #include <mach/hardware.h>
 
+#define BITS(_start, _end) ((BIT(_end) - BIT(_start)) + BIT(_end))
+
+#define PRCM_ACLK_MGT_OFF		0x004
+#define PRCM_SVACLK_MGT_OFF		0x008
+#define PRCM_SIACLK_MGT_OFF		0x00C
+#define PRCM_SGACLK_MGT_OFF		0x014
+#define PRCM_UARTCLK_MGT_OFF		0x018
+#define PRCM_MSP02CLK_MGT_OFF		0x01C
+#define PRCM_I2CCLK_MGT_OFF		0x020
+#define PRCM_SDMMCCLK_MGT_OFF		0x024
+#define PRCM_SLIMCLK_MGT_OFF		0x028
+#define PRCM_PER1CLK_MGT_OFF		0x02C
+#define PRCM_PER2CLK_MGT_OFF		0x030
+#define PRCM_PER3CLK_MGT_OFF		0x034
+#define PRCM_PER5CLK_MGT_OFF		0x038
+#define PRCM_PER6CLK_MGT_OFF		0x03C
+#define PRCM_PER7CLK_MGT_OFF		0x040
+#define PRCM_PWMCLK_MGT_OFF		0x044 /* for DB5500 */
+#define PRCM_IRDACLK_MGT_OFF		0x048 /* for DB5500 */
+#define PRCM_IRRCCLK_MGT_OFF		0x04C /* for DB5500 */
+#define PRCM_LCDCLK_MGT_OFF		0x044
+#define PRCM_BMLCLK_MGT_OFF		0x04C
+#define PRCM_HSITXCLK_MGT_OFF		0x050
+#define PRCM_HSIRXCLK_MGT_OFF		0x054
+#define PRCM_HDMICLK_MGT_OFF		0x058
+#define PRCM_APEATCLK_MGT_OFF		0x05C
+#define PRCM_APETRACECLK_MGT_OFF	0x060
+#define PRCM_MCDECLK_MGT_OFF		0x064
+#define PRCM_IPI2CCLK_MGT_OFF		0x068
+#define PRCM_DSIALTCLK_MGT_OFF		0x06C
+#define PRCM_DMACLK_MGT_OFF		0x074
+#define PRCM_B2R2CLK_MGT_OFF		0x078
+#define PRCM_TVCLK_MGT_OFF		0x07C
+#define PRCM_UNIPROCLK_MGT_OFF		0x278
+#define PRCM_SSPCLK_MGT_OFF		0x280
+#define PRCM_RNGCLK_MGT_OFF		0x284
+#define PRCM_UICCCLK_MGT_OFF		0x27C
+#define PRCM_MSP1CLK_MGT_OFF		0x288
+
 #define PRCM_ARM_PLLDIVPS	(_PRCMU_BASE + 0x118)
 #define PRCM_ARM_PLLDIVPS_ARM_BRM_RATE		0x3f
 #define PRCM_ARM_PLLDIVPS_MAX_MASK		0xf
@@ -30,11 +69,15 @@
 #define PRCM_PLLARM_ENABLE_PRCM_PLLARM_COUNTON	0x100
 
 #define PRCM_ARMCLKFIX_MGT	(_PRCMU_BASE + 0x0)
+#define PRCM_A9PL_FORCE_CLKEN	(_PRCMU_BASE + 0x19C)
 #define PRCM_A9_RESETN_CLR	(_PRCMU_BASE + 0x1f4)
 #define PRCM_A9_RESETN_SET	(_PRCMU_BASE + 0x1f0)
 #define PRCM_ARM_LS_CLAMP	(_PRCMU_BASE + 0x30c)
 #define PRCM_SRAM_A9		(_PRCMU_BASE + 0x308)
 
+#define PRCM_A9PL_FORCE_CLKEN_PRCM_A9PL_FORCE_CLKEN BIT(0)
+#define PRCM_A9PL_FORCE_CLKEN_PRCM_A9AXI_FORCE_CLKEN BIT(1)
+
 /* ARM WFI Standby signal register */
 #define PRCM_ARM_WFI_STANDBY    (_PRCMU_BASE + 0x130)
 #define PRCM_IOCR		(_PRCMU_BASE + 0x310)
@@ -61,12 +104,18 @@
 #define PRCM_ARMITVAL127TO96	(_PRCMU_BASE + 0x26C)
 
 #define PRCM_HOSTACCESS_REQ	(_PRCMU_BASE + 0x334)
+#define PRCM_HOSTACCESS_REQ_HOSTACCESS_REQ 0x1
 #define ARM_WAKEUP_MODEM	0x1
 
-#define PRCM_ARM_IT1_CLEAR	(_PRCMU_BASE + 0x48C)
+#define PRCM_ARM_IT1_CLR	(_PRCMU_BASE + 0x48C)
 #define PRCM_ARM_IT1_VAL	(_PRCMU_BASE + 0x494)
 #define PRCM_HOLD_EVT		(_PRCMU_BASE + 0x174)
 
+#define PRCM_MOD_AWAKE_STATUS	(_PRCMU_BASE + 0x4A0)
+#define PRCM_MOD_AWAKE_STATUS_PRCM_MOD_COREPD_AWAKE	BIT(0)
+#define PRCM_MOD_AWAKE_STATUS_PRCM_MOD_AAPD_AWAKE	BIT(1)
+#define PRCM_MOD_AWAKE_STATUS_PRCM_MOD_VMODEM_OFF_ISO	BIT(2)
+
 #define PRCM_ITSTATUS0		(_PRCMU_BASE + 0x148)
 #define PRCM_ITSTATUS1		(_PRCMU_BASE + 0x150)
 #define PRCM_ITSTATUS2		(_PRCMU_BASE + 0x158)
@@ -87,16 +136,21 @@
 #define PRCM_PLLDSI_FREQ           (_PRCMU_BASE + 0x500)
 #define PRCM_PLLDSI_ENABLE         (_PRCMU_BASE + 0x504)
 #define PRCM_PLLDSI_LOCKP          (_PRCMU_BASE + 0x508)
-#define PRCM_LCDCLK_MGT            (_PRCMU_BASE + 0x044)
-#define PRCM_MCDECLK_MGT           (_PRCMU_BASE + 0x064)
-#define PRCM_HDMICLK_MGT           (_PRCMU_BASE + 0x058)
-#define PRCM_TVCLK_MGT             (_PRCMU_BASE + 0x07c)
+#define PRCM_LCDCLK_MGT            (_PRCMU_BASE + PRCM_LCDCLK_MGT_OFF)
+#define PRCM_MCDECLK_MGT           (_PRCMU_BASE + PRCM_MCDECLK_MGT_OFF)
+#define PRCM_HDMICLK_MGT           (_PRCMU_BASE + PRCM_HDMICLK_MGT_OFF)
+#define PRCM_TVCLK_MGT             (_PRCMU_BASE + PRCM_TVCLK_MGT_OFF)
 #define PRCM_DSI_PLLOUT_SEL        (_PRCMU_BASE + 0x530)
 #define PRCM_DSITVCLK_DIV          (_PRCMU_BASE + 0x52C)
 #define PRCM_PLLDSI_LOCKP          (_PRCMU_BASE + 0x508)
 #define PRCM_APE_RESETN_SET        (_PRCMU_BASE + 0x1E4)
 #define PRCM_APE_RESETN_CLR        (_PRCMU_BASE + 0x1E8)
+
 #define PRCM_CLKOCR		   (_PRCMU_BASE + 0x1CC)
+#define PRCM_CLKOCR_CLKOUT0_REF_CLK	(1 << 0)
+#define PRCM_CLKOCR_CLKOUT0_MASK	BITS(0, 13)
+#define PRCM_CLKOCR_CLKOUT1_REF_CLK	(1 << 16)
+#define PRCM_CLKOCR_CLKOUT1_MASK	BITS(16, 29)
 
 /* ePOD and memory power signal control registers */
 #define PRCM_EPOD_C_SET            (_PRCMU_BASE + 0x410)
@@ -111,5 +165,41 @@
 #define PRCM_GPIOCR_DBG_STM_MOD_CMD1            0x800
 #define PRCM_GPIOCR_DBG_UARTMOD_CMD0            0x1
 
+/* PRCMU HW semaphore */
+#define PRCM_SEM                   (_PRCMU_BASE + 0x400)
+#define PRCM_SEM_PRCM_SEM BIT(0)
+
+#define PRCM_TCR                   (_PRCMU_BASE + 0x1C8)
+#define PRCM_TCR_TENSEL_MASK       BITS(0, 7)
+#define PRCM_TCR_STOP_TIMERS       BIT(16)
+#define PRCM_TCR_DOZE_MODE         BIT(17)
+
+#define PRCM_CLKOCR_CLKODIV0_SHIFT	0
+#define PRCM_CLKOCR_CLKODIV0_MASK	BITS(0, 5)
+#define PRCM_CLKOCR_CLKOSEL0_SHIFT	6
+#define PRCM_CLKOCR_CLKOSEL0_MASK	BITS(6, 8)
+#define PRCM_CLKOCR_CLKODIV1_SHIFT	16
+#define PRCM_CLKOCR_CLKODIV1_MASK	BITS(16, 21)
+#define PRCM_CLKOCR_CLKOSEL1_SHIFT	22
+#define PRCM_CLKOCR_CLKOSEL1_MASK	BITS(22, 24)
+#define PRCM_CLKOCR_CLK1TYPE		BIT(28)
+
+#define PRCM_CLK_MGT_CLKPLLDIV_MASK	BITS(0, 4)
+#define PRCM_CLK_MGT_CLKPLLSW_MASK	BITS(5, 7)
+#define PRCM_CLK_MGT_CLKEN		BIT(8)
+
+/* GPIOCR register */
+#define PRCM_GPIOCR_SPI2_SELECT BIT(23)
+
+#define PRCM_DDR_SUBSYS_APE_MINBW	(_PRCMU_BASE + 0x438)
+#define PRCM_CGATING_BYPASS		(_PRCMU_BASE + 0x134)
+#define PRCM_CGATING_BYPASS_ICN2	BIT(6)
+
+/* Miscellaneous unit registers */
+#define PRCM_RESOUTN_SET		(_PRCMU_BASE + 0x214)
+#define PRCM_RESOUTN_CLR		(_PRCMU_BASE + 0x218)
+
+/* System reset register */
+#define PRCM_APE_SOFTRST 		(_PRCMU_BASE + 0x228)
 
 #endif /* __MACH_PRCMU__REGS_H */
diff --git a/arch/arm/mach-ux500/include/mach/prcmu.h b/arch/arm/mach-ux500/include/mach/prcmu.h
new file mode 100644
index 00000000000..92fd7f79a4d
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/prcmu.h
@@ -0,0 +1,554 @@
+/*
+ * Copyright (C) ST Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * STE Ux500 PRCMU API
+ */
+#ifndef __MACH_PRCMU_H
+#define __MACH_PRCMU_H
+
+#include <linux/interrupt.h>
+#include <linux/notifier.h>
+#include <asm/mach-types.h>
+
+/* PRCMU Wakeup defines */
+enum prcmu_wakeup_index {
+	PRCMU_WAKEUP_INDEX_RTC,
+	PRCMU_WAKEUP_INDEX_RTT0,
+	PRCMU_WAKEUP_INDEX_RTT1,
+	PRCMU_WAKEUP_INDEX_HSI0,
+	PRCMU_WAKEUP_INDEX_HSI1,
+	PRCMU_WAKEUP_INDEX_USB,
+	PRCMU_WAKEUP_INDEX_ABB,
+	PRCMU_WAKEUP_INDEX_ABB_FIFO,
+	PRCMU_WAKEUP_INDEX_ARM,
+	PRCMU_WAKEUP_INDEX_CD_IRQ,
+	NUM_PRCMU_WAKEUP_INDICES
+};
+#define PRCMU_WAKEUP(_name) (BIT(PRCMU_WAKEUP_INDEX_##_name))
+
+/* EPOD (power domain) IDs */
+
+/*
+ * DB8500 EPODs
+ * - EPOD_ID_SVAMMDSP: power domain for SVA MMDSP
+ * - EPOD_ID_SVAPIPE: power domain for SVA pipe
+ * - EPOD_ID_SIAMMDSP: power domain for SIA MMDSP
+ * - EPOD_ID_SIAPIPE: power domain for SIA pipe
+ * - EPOD_ID_SGA: power domain for SGA
+ * - EPOD_ID_B2R2_MCDE: power domain for B2R2 and MCDE
+ * - EPOD_ID_ESRAM12: power domain for ESRAM 1 and 2
+ * - EPOD_ID_ESRAM34: power domain for ESRAM 3 and 4
+ * - NUM_EPOD_ID: number of power domains
+ *
+ * TODO: These should be prefixed.
+ */
+#define EPOD_ID_SVAMMDSP	0
+#define EPOD_ID_SVAPIPE		1
+#define EPOD_ID_SIAMMDSP	2
+#define EPOD_ID_SIAPIPE		3
+#define EPOD_ID_SGA		4
+#define EPOD_ID_B2R2_MCDE	5
+#define EPOD_ID_ESRAM12		6
+#define EPOD_ID_ESRAM34		7
+#define NUM_EPOD_ID		8
+
+/*
+ * DB5500 EPODs
+ */
+#define DB5500_EPOD_ID_BASE 0x0100
+#define DB5500_EPOD_ID_SGA (DB5500_EPOD_ID_BASE + 0)
+#define DB5500_EPOD_ID_HVA (DB5500_EPOD_ID_BASE + 1)
+#define DB5500_EPOD_ID_SIA (DB5500_EPOD_ID_BASE + 2)
+#define DB5500_EPOD_ID_DISP (DB5500_EPOD_ID_BASE + 3)
+#define DB5500_EPOD_ID_ESRAM12 (DB5500_EPOD_ID_BASE + 6)
+#define DB5500_NUM_EPOD_ID 7
+
+/*
+ * state definition for EPOD (power domain)
+ * - EPOD_STATE_NO_CHANGE: The EPOD should remain unchanged
+ * - EPOD_STATE_OFF: The EPOD is switched off
+ * - EPOD_STATE_RAMRET: The EPOD is switched off with its internal RAM in
+ *                         retention
+ * - EPOD_STATE_ON_CLK_OFF: The EPOD is switched on, clock is still off
+ * - EPOD_STATE_ON: Same as above, but with clock enabled
+ */
+#define EPOD_STATE_NO_CHANGE	0x00
+#define EPOD_STATE_OFF		0x01
+#define EPOD_STATE_RAMRET	0x02
+#define EPOD_STATE_ON_CLK_OFF	0x03
+#define EPOD_STATE_ON		0x04
+
+/*
+ * CLKOUT sources
+ */
+#define PRCMU_CLKSRC_CLK38M		0x00
+#define PRCMU_CLKSRC_ACLK		0x01
+#define PRCMU_CLKSRC_SYSCLK		0x02
+#define PRCMU_CLKSRC_LCDCLK		0x03
+#define PRCMU_CLKSRC_SDMMCCLK		0x04
+#define PRCMU_CLKSRC_TVCLK		0x05
+#define PRCMU_CLKSRC_TIMCLK		0x06
+#define PRCMU_CLKSRC_CLK009		0x07
+/* These are only valid for CLKOUT1: */
+#define PRCMU_CLKSRC_SIAMMDSPCLK	0x40
+#define PRCMU_CLKSRC_I2CCLK		0x41
+#define PRCMU_CLKSRC_MSP02CLK		0x42
+#define PRCMU_CLKSRC_ARMPLL_OBSCLK	0x43
+#define PRCMU_CLKSRC_HSIRXCLK		0x44
+#define PRCMU_CLKSRC_HSITXCLK		0x45
+#define PRCMU_CLKSRC_ARMCLKFIX		0x46
+#define PRCMU_CLKSRC_HDMICLK		0x47
+
+/*
+ * Clock identifiers.
+ */
+enum prcmu_clock {
+	PRCMU_SGACLK,
+	PRCMU_UARTCLK,
+	PRCMU_MSP02CLK,
+	PRCMU_MSP1CLK,
+	PRCMU_I2CCLK,
+	PRCMU_SDMMCCLK,
+	PRCMU_SLIMCLK,
+	PRCMU_PER1CLK,
+	PRCMU_PER2CLK,
+	PRCMU_PER3CLK,
+	PRCMU_PER5CLK,
+	PRCMU_PER6CLK,
+	PRCMU_PER7CLK,
+	PRCMU_LCDCLK,
+	PRCMU_BMLCLK,
+	PRCMU_HSITXCLK,
+	PRCMU_HSIRXCLK,
+	PRCMU_HDMICLK,
+	PRCMU_APEATCLK,
+	PRCMU_APETRACECLK,
+	PRCMU_MCDECLK,
+	PRCMU_IPI2CCLK,
+	PRCMU_DSIALTCLK,
+	PRCMU_DMACLK,
+	PRCMU_B2R2CLK,
+	PRCMU_TVCLK,
+	PRCMU_SSPCLK,
+	PRCMU_RNGCLK,
+	PRCMU_UICCCLK,
+	PRCMU_PWMCLK,
+	PRCMU_IRDACLK,
+	PRCMU_IRRCCLK,
+	PRCMU_SIACLK,
+	PRCMU_SVACLK,
+	PRCMU_NUM_REG_CLOCKS,
+	PRCMU_SYSCLK = PRCMU_NUM_REG_CLOCKS,
+	PRCMU_TIMCLK,
+	PRCMU_PLLSOC0,
+	PRCMU_PLLSOC1,
+	PRCMU_PLLDDR,
+};
+
+/**
+ * enum ape_opp - APE OPP states definition
+ * @APE_OPP_INIT:
+ * @APE_NO_CHANGE: The APE operating point is unchanged
+ * @APE_100_OPP: The new APE operating point is ape100opp
+ * @APE_50_OPP: 50%
+ * @APE_50_PARTLY_25_OPP: 50%, except some clocks at 25%.
+ */
+enum ape_opp {
+	APE_OPP_INIT = 0x00,
+	APE_NO_CHANGE = 0x01,
+	APE_100_OPP = 0x02,
+	APE_50_OPP = 0x03,
+	APE_50_PARTLY_25_OPP = 0xFF,
+};
+
+/**
+ * enum arm_opp - ARM OPP states definition
+ * @ARM_OPP_INIT:
+ * @ARM_NO_CHANGE: The ARM operating point is unchanged
+ * @ARM_100_OPP: The new ARM operating point is arm100opp
+ * @ARM_50_OPP: The new ARM operating point is arm50opp
+ * @ARM_MAX_OPP: Operating point is "max" (more than 100)
+ * @ARM_MAX_FREQ100OPP: Set max opp if available, else 100
+ * @ARM_EXTCLK: The new ARM operating point is armExtClk
+ */
+enum arm_opp {
+	ARM_OPP_INIT = 0x00,
+	ARM_NO_CHANGE = 0x01,
+	ARM_100_OPP = 0x02,
+	ARM_50_OPP = 0x03,
+	ARM_MAX_OPP = 0x04,
+	ARM_MAX_FREQ100OPP = 0x05,
+	ARM_EXTCLK = 0x07
+};
+
+/**
+ * enum ddr_opp - DDR OPP states definition
+ * @DDR_100_OPP: The new DDR operating point is ddr100opp
+ * @DDR_50_OPP: The new DDR operating point is ddr50opp
+ * @DDR_25_OPP: The new DDR operating point is ddr25opp
+ */
+enum ddr_opp {
+	DDR_100_OPP = 0x00,
+	DDR_50_OPP = 0x01,
+	DDR_25_OPP = 0x02,
+};
+
+/*
+ * Definitions for controlling ESRAM0 in deep sleep.
+ */
+#define ESRAM0_DEEP_SLEEP_STATE_OFF 1
+#define ESRAM0_DEEP_SLEEP_STATE_RET 2
+
+/**
+ * enum ddr_pwrst - DDR power states definition
+ * @DDR_PWR_STATE_UNCHANGED: SDRAM and DDR controller state is unchanged
+ * @DDR_PWR_STATE_ON:
+ * @DDR_PWR_STATE_OFFLOWLAT:
+ * @DDR_PWR_STATE_OFFHIGHLAT:
+ */
+enum ddr_pwrst {
+	DDR_PWR_STATE_UNCHANGED     = 0x00,
+	DDR_PWR_STATE_ON            = 0x01,
+	DDR_PWR_STATE_OFFLOWLAT     = 0x02,
+	DDR_PWR_STATE_OFFHIGHLAT    = 0x03
+};
+
+#include <linux/mfd/db8500-prcmu.h>
+#include <linux/mfd/db5500-prcmu.h>
+
+#if defined(CONFIG_UX500_SOC_DB8500) || defined(CONFIG_UX500_SOC_DB5500)
+
+static inline void __init prcmu_early_init(void)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_early_init();
+	else
+		return db8500_prcmu_early_init();
+}
+
+static inline int prcmu_set_power_state(u8 state, bool keep_ulp_clk,
+		bool keep_ap_pll)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_set_power_state(state, keep_ulp_clk,
+			keep_ap_pll);
+	else
+		return db8500_prcmu_set_power_state(state, keep_ulp_clk,
+			keep_ap_pll);
+}
+
+static inline int prcmu_set_epod(u16 epod_id, u8 epod_state)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_set_epod(epod_id, epod_state);
+	else
+		return db8500_prcmu_set_epod(epod_id, epod_state);
+}
+
+static inline void prcmu_enable_wakeups(u32 wakeups)
+{
+	if (machine_is_u5500())
+		db5500_prcmu_enable_wakeups(wakeups);
+	else
+		db8500_prcmu_enable_wakeups(wakeups);
+}
+
+static inline void prcmu_disable_wakeups(void)
+{
+	prcmu_enable_wakeups(0);
+}
+
+static inline void prcmu_config_abb_event_readout(u32 abb_events)
+{
+	if (machine_is_u5500())
+		db5500_prcmu_config_abb_event_readout(abb_events);
+	else
+		db8500_prcmu_config_abb_event_readout(abb_events);
+}
+
+static inline void prcmu_get_abb_event_buffer(void __iomem **buf)
+{
+	if (machine_is_u5500())
+		db5500_prcmu_get_abb_event_buffer(buf);
+	else
+		db8500_prcmu_get_abb_event_buffer(buf);
+}
+
+int prcmu_abb_read(u8 slave, u8 reg, u8 *value, u8 size);
+int prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size);
+
+int prcmu_config_clkout(u8 clkout, u8 source, u8 div);
+
+static inline int prcmu_request_clock(u8 clock, bool enable)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_request_clock(clock, enable);
+	else
+		return db8500_prcmu_request_clock(clock, enable);
+}
+
+int prcmu_set_ape_opp(u8 opp);
+int prcmu_get_ape_opp(void);
+int prcmu_set_ddr_opp(u8 opp);
+int prcmu_get_ddr_opp(void);
+
+static inline int prcmu_set_arm_opp(u8 opp)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_set_arm_opp(opp);
+	else
+		return db8500_prcmu_set_arm_opp(opp);
+}
+
+static inline int prcmu_get_arm_opp(void)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_get_arm_opp();
+	else
+		return db8500_prcmu_get_arm_opp();
+}
+
+static inline void prcmu_system_reset(u16 reset_code)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_system_reset(reset_code);
+	else
+		return db8500_prcmu_system_reset(reset_code);
+}
+
+static inline u16 prcmu_get_reset_code(void)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_get_reset_code();
+	else
+		return db8500_prcmu_get_reset_code();
+}
+
+void prcmu_ac_wake_req(void);
+void prcmu_ac_sleep_req(void);
+void prcmu_modem_reset(void);
+static inline bool prcmu_is_ac_wake_requested(void)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_is_ac_wake_requested();
+	else
+		return db8500_prcmu_is_ac_wake_requested();
+}
+
+static inline int prcmu_set_display_clocks(void)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_set_display_clocks();
+	else
+		return db8500_prcmu_set_display_clocks();
+}
+
+static inline int prcmu_disable_dsipll(void)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_disable_dsipll();
+	else
+		return db8500_prcmu_disable_dsipll();
+}
+
+static inline int prcmu_enable_dsipll(void)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_enable_dsipll();
+	else
+		return db8500_prcmu_enable_dsipll();
+}
+
+static inline int prcmu_config_esram0_deep_sleep(u8 state)
+{
+	if (machine_is_u5500())
+		return db5500_prcmu_config_esram0_deep_sleep(state);
+	else
+		return db8500_prcmu_config_esram0_deep_sleep(state);
+}
+#else
+
+static inline void __init prcmu_early_init(void) {}
+
+static inline int prcmu_set_power_state(u8 state, bool keep_ulp_clk,
+	bool keep_ap_pll)
+{
+	return 0;
+}
+
+static inline int prcmu_set_epod(u16 epod_id, u8 epod_state)
+{
+	return 0;
+}
+
+static inline void prcmu_enable_wakeups(u32 wakeups) {}
+
+static inline void prcmu_disable_wakeups(void) {}
+
+static inline int prcmu_abb_read(u8 slave, u8 reg, u8 *value, u8 size)
+{
+	return -ENOSYS;
+}
+
+static inline int prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size)
+{
+	return -ENOSYS;
+}
+
+static inline int prcmu_config_clkout(u8 clkout, u8 source, u8 div)
+{
+	return 0;
+}
+
+static inline int prcmu_request_clock(u8 clock, bool enable)
+{
+	return 0;
+}
+
+static inline int prcmu_set_ape_opp(u8 opp)
+{
+	return 0;
+}
+
+static inline int prcmu_get_ape_opp(void)
+{
+	return APE_100_OPP;
+}
+
+static inline int prcmu_set_arm_opp(u8 opp)
+{
+	return 0;
+}
+
+static inline int prcmu_get_arm_opp(void)
+{
+	return ARM_100_OPP;
+}
+
+static inline int prcmu_set_ddr_opp(u8 opp)
+{
+	return 0;
+}
+
+static inline int prcmu_get_ddr_opp(void)
+{
+	return DDR_100_OPP;
+}
+
+static inline void prcmu_system_reset(u16 reset_code) {}
+
+static inline u16 prcmu_get_reset_code(void)
+{
+	return 0;
+}
+
+static inline void prcmu_ac_wake_req(void) {}
+
+static inline void prcmu_ac_sleep_req(void) {}
+
+static inline void prcmu_modem_reset(void) {}
+
+static inline bool prcmu_is_ac_wake_requested(void)
+{
+	return false;
+}
+
+static inline int prcmu_set_display_clocks(void)
+{
+	return 0;
+}
+
+static inline int prcmu_disable_dsipll(void)
+{
+	return 0;
+}
+
+static inline int prcmu_enable_dsipll(void)
+{
+	return 0;
+}
+
+static inline int prcmu_config_esram0_deep_sleep(u8 state)
+{
+	return 0;
+}
+
+static inline void prcmu_config_abb_event_readout(u32 abb_events) {}
+
+static inline void prcmu_get_abb_event_buffer(void __iomem **buf)
+{
+	*buf = NULL;
+}
+
+#endif
+
+/* PRCMU QoS APE OPP class */
+#define PRCMU_QOS_APE_OPP 1
+#define PRCMU_QOS_DDR_OPP 2
+#define PRCMU_QOS_DEFAULT_VALUE -1
+
+#ifdef CONFIG_UX500_PRCMU_QOS_POWER
+
+unsigned long prcmu_qos_get_cpufreq_opp_delay(void);
+void prcmu_qos_set_cpufreq_opp_delay(unsigned long);
+void prcmu_qos_force_opp(int, s32);
+void prcmu_qos_voice_call_override(bool enable);
+int prcmu_qos_requirement(int pm_qos_class);
+int prcmu_qos_add_requirement(int pm_qos_class, char *name, s32 value);
+int prcmu_qos_update_requirement(int pm_qos_class, char *name, s32 new_value);
+void prcmu_qos_remove_requirement(int pm_qos_class, char *name);
+int prcmu_qos_add_notifier(int prcmu_qos_class,
+			   struct notifier_block *notifier);
+int prcmu_qos_remove_notifier(int prcmu_qos_class,
+			      struct notifier_block *notifier);
+
+#else
+
+static inline unsigned long prcmu_qos_get_cpufreq_opp_delay(void)
+{
+	return 0;
+}
+
+static inline void prcmu_qos_set_cpufreq_opp_delay(unsigned long n) {}
+
+static inline void prcmu_qos_force_opp(int prcmu_qos_class, s32 i) {}
+
+static inline void prcmu_qos_voice_call_override(bool enable) {}
+
+static inline int prcmu_qos_requirement(int prcmu_qos_class)
+{
+	return 0;
+}
+
+static inline int prcmu_qos_add_requirement(int prcmu_qos_class,
+					    char *name, s32 value)
+{
+	return 0;
+}
+
+static inline int prcmu_qos_update_requirement(int prcmu_qos_class,
+					       char *name, s32 new_value)
+{
+	return 0;
+}
+
+static inline void prcmu_qos_remove_requirement(int prcmu_qos_class, char *name)
+{
+}
+
+static inline int prcmu_qos_add_notifier(int prcmu_qos_class,
+					 struct notifier_block *notifier)
+{
+	return 0;
+}
+static inline int prcmu_qos_remove_notifier(int prcmu_qos_class,
+					    struct notifier_block *notifier)
+{
+	return 0;
+}
+
+#endif
+
+#endif /* __MACH_PRCMU_H */
diff --git a/arch/arm/mach-ux500/include/mach/reboot_reasons.h b/arch/arm/mach-ux500/include/mach/reboot_reasons.h
new file mode 100644
index 00000000000..6f96eb8794f
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/reboot_reasons.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Rickard Evertsson <rickard.evertsson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Use this file to customize your reboot / sw reset reasons. Add, remove or
+ * modify reasons in reboot_reasons[].
+ * The reboot reasons will be saved to a secure location in TCDM memory and
+ * can be read at bootup by e.g. the bootloader, or at a later stage userspace
+ * since the code is exposed through sysfs.
+ */
+
+#ifndef _REBOOT_REASONS_H
+#define _REBOOT_REASONS_H
+
+/*
+ * These defines contains the codes that will be written down to a secure
+ * location before resetting. These values are exposed through a sysfs
+ * entry under /sys/socinfo, see mach-ux500/cpu-db8500.c
+ */
+#define SW_RESET_NO_ARGUMENT 0x0
+#define SW_RESET_FACTORY_RESET 0x4242
+#define SW_RESET_CRASH 0xDEAD
+#define SW_RESET_NORMAL 0xc001
+
+/*
+ * The array reboot_reasons[] is used when you want to map a string to a reboot
+ * reason code
+ */
+struct reboot_reason {
+	const char *reason;
+	u16 code;
+};
+
+extern struct reboot_reason reboot_reasons[];
+
+extern unsigned int reboot_reasons_size;
+
+u16 reboot_reason_code(const char *cmd);
+const char *reboot_reason_string(u16 code);
+
+#endif
diff --git a/arch/arm/mach-ux500/include/mach/scu.h b/arch/arm/mach-ux500/include/mach/scu.h
new file mode 100644
index 00000000000..a09e86a9d3c
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/scu.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) 2009 ST-Ericsson SA
+ *
+ * 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.
+ */
+
+#ifndef __ASMARM_ARCH_SCU_H
+#define __ASMARM_ARCH_SCU_H
+
+#include <mach/hardware.h>
+
+#define SCU_BASE	U8500_SCU_BASE
+/*
+ *  * SCU registers
+ *   */
+#define SCU_CTRL        0x00
+#define SCU_CONFIG      0x04
+#define SCU_CPU_STATUS      0x08
+#define SCU_INVALIDATE      0x0c
+#define SCU_FPGA_REVISION   0x10
+
+#endif
diff --git a/arch/arm/mach-ux500/include/mach/sim_detect.h b/arch/arm/mach-ux500/include/mach/sim_detect.h
new file mode 100644
index 00000000000..8d6e81f1e8a
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/sim_detect.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright ST-Ericsson 2010 SA.
+ *
+ * Author: Bibek Basu <bibek.basu@stericsson.com>
+ * Licensed under GPLv2.
+ */
+
+#ifndef _AB8500_SIM_DETECT_H
+#define _AB8500_SIM_DETECT_H
+
+struct sim_detect_platform_data {
+	int irq_num;
+};
+
+#endif
+
diff --git a/arch/arm/mach-ux500/ste-dma40-db5500.h b/arch/arm/mach-ux500/include/mach/ste-dma40-db5500.h
index cb2110c3285..cb2110c3285 100644
--- a/arch/arm/mach-ux500/ste-dma40-db5500.h
+++ b/arch/arm/mach-ux500/include/mach/ste-dma40-db5500.h
diff --git a/arch/arm/mach-ux500/ste-dma40-db8500.h b/arch/arm/mach-ux500/include/mach/ste-dma40-db8500.h
index a616419bea7..65799a75199 100644
--- a/arch/arm/mach-ux500/ste-dma40-db8500.h
+++ b/arch/arm/mach-ux500/include/mach/ste-dma40-db8500.h
@@ -1,16 +1,19 @@
 /*
- * arch/arm/mach-ux500/ste_dma40_db8500.h
- * DB8500-SoC-specific configuration for DMA40
- *
- * Copyright (C) ST-Ericsson 2007-2010
+ * Copyright (C) ST-Ericsson SA 2007-2010
+ * Author: Per Friden <per.friden@stericsson.com> for ST-Ericsson
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
  * License terms: GNU General Public License (GPL) version 2
- * Author: Per Friden <per.friden@stericsson.com>
- * Author: Jonas Aaberg <jonas.aberg@stericsson.com>
+ *
+ * DB8500-SoC-specific configuration for DMA40
  */
 #ifndef STE_DMA40_DB8500_H
 #define STE_DMA40_DB8500_H
 
 #define DB8500_DMA_NR_DEV 64
+/*
+ * All entries with double names are multiplexed
+ * and can never be used at the same time.
+ */
 
 enum dma_src_dev_type {
 	DB8500_DMA_DEV0_SPI0_RX = 0,
@@ -20,7 +23,7 @@ enum dma_src_dev_type {
 	DB8500_DMA_DEV4_I2C1_RX = 4,
 	DB8500_DMA_DEV5_I2C3_RX = 5,
 	DB8500_DMA_DEV6_I2C2_RX = 6,
-	DB8500_DMA_DEV7_I2C4_RX = 7, /* Only on V1 and later */
+	DB8500_DMA_DEV7_I2C4_RX = 7,
 	DB8500_DMA_DEV8_SSP0_RX = 8,
 	DB8500_DMA_DEV9_SSP1_RX = 9,
 	DB8500_DMA_DEV10_MCDE_RX = 10,
@@ -43,8 +46,6 @@ enum dma_src_dev_type {
 	DB8500_DMA_DEV27_SRC_SXA3_RX_TX = 27,
 	DB8500_DMA_DEV28_SD_MM2_RX = 28,
 	DB8500_DMA_DEV29_SD_MM0_RX = 29,
-	DB8500_DMA_DEV30_MSP1_RX = 30,
-	/* On DB8500v2, MSP3 RX replaces MSP1 RX */
 	DB8500_DMA_DEV30_MSP3_RX = 30,
 	DB8500_DMA_DEV31_MSP0_RX_SLIM0_CH0_RX = 31,
 	DB8500_DMA_DEV32_SD_MM1_RX = 32,
@@ -82,7 +83,7 @@ enum dma_dest_dev_type {
 	DB8500_DMA_DEV4_I2C1_TX = 4,
 	DB8500_DMA_DEV5_I2C3_TX = 5,
 	DB8500_DMA_DEV6_I2C2_TX = 6,
-	DB8500_DMA_DEV7_I2C4_TX = 7, /* Only on V1 and later */
+	DB8500_DMA_DEV7_I2C4_TX = 7,
 	DB8500_DMA_DEV8_SSP0_TX = 8,
 	DB8500_DMA_DEV9_SSP1_TX = 9,
 	/* 10 is not used*/
diff --git a/arch/arm/mach-ux500/include/mach/ste_audio.h b/arch/arm/mach-ux500/include/mach/ste_audio.h
new file mode 100644
index 00000000000..c314b463701
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/ste_audio.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (C) 2009 ST-Ericsson SA
+ *
+ * This file is licensed under  the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#ifndef _UX500_STE_AUDIO_H_
+#define _UX500_STE_AUDIO_H_
+
+
+struct ab8500_audio_platform_data {
+	int (*ste_gpio_altf_init) (void);
+	int (*ste_gpio_altf_exit) (void);
+};
+
+#endif /* _UX500_STE_AUDIO_H_ */
diff --git a/arch/arm/mach-ux500/include/mach/ste_audio_io_ioctl.h b/arch/arm/mach-ux500/include/mach/ste_audio_io_ioctl.h
new file mode 100644
index 00000000000..e04734d65a5
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/ste_audio_io_ioctl.h
@@ -0,0 +1,234 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Deepak KARDA/ deepak.karda@stericsson.com for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#ifndef _AUDIOIO_IOCTL_H_
+#define _AUDIOIO_IOCTL_H_
+
+
+#define AUDIOIO_IOC_MAGIC            'N'
+#define AUDIOIO_READ_REGISTER        _IOWR(AUDIOIO_IOC_MAGIC, 1,\
+						struct audioio_data_t)
+#define AUDIOIO_WRITE_REGISTER       _IOW(AUDIOIO_IOC_MAGIC, 2,\
+						struct audioio_data_t)
+#define AUDIOIO_PWR_CTRL_TRNSDR      _IOW(AUDIOIO_IOC_MAGIC, 3,\
+						struct audioio_pwr_ctrl_t)
+#define AUDIOIO_PWR_STS_TRNSDR       _IOR(AUDIOIO_IOC_MAGIC, 4,\
+						struct audioio_pwr_ctrl_t)
+#define AUDIOIO_LOOP_CTRL            _IOW(AUDIOIO_IOC_MAGIC, 5,\
+						struct audioio_loop_ctrl_t)
+#define AUDIOIO_LOOP_STS             _IOR(AUDIOIO_IOC_MAGIC, 6,\
+						struct audioio_loop_ctrl_t)
+#define AUDIOIO_GET_TRNSDR_GAIN_CAPABILITY      _IOR(AUDIOIO_IOC_MAGIC, 7,\
+						struct audioio_get_gain_t)
+#define AUDIOIO_GAIN_CAP_LOOP        _IOR(AUDIOIO_IOC_MAGIC, 8,\
+						struct audioio_gain_loop_t)
+#define AUDIOIO_SUPPORT_LOOP         _IOR(AUDIOIO_IOC_MAGIC, 9,\
+						struct audioio_support_loop_t)
+#define AUDIOIO_GAIN_DESC_TRNSDR     _IOR(AUDIOIO_IOC_MAGIC, 10,\
+					struct audioio_gain_desc_trnsdr_t)
+#define AUDIOIO_GAIN_CTRL_TRNSDR     _IOW(AUDIOIO_IOC_MAGIC, 11,\
+					struct audioio_gain_ctrl_trnsdr_t)
+#define AUDIOIO_GAIN_QUERY_TRNSDR    _IOR(AUDIOIO_IOC_MAGIC, 12,\
+					struct audioio_gain_ctrl_trnsdr_t)
+#define AUDIOIO_MUTE_CTRL_TRNSDR     _IOW(AUDIOIO_IOC_MAGIC, 13,\
+						struct audioio_mute_trnsdr_t)
+#define AUDIOIO_MUTE_STS_TRNSDR      _IOR(AUDIOIO_IOC_MAGIC, 14,\
+						struct audioio_mute_trnsdr_t)
+#define AUDIOIO_FADE_CTRL		_IOW(AUDIOIO_IOC_MAGIC, 15,\
+						struct audioio_fade_ctrl_t)
+#define AUDIOIO_BURST_CTRL            _IOW(AUDIOIO_IOC_MAGIC, 16,\
+						struct audioio_burst_ctrl_t)
+#define AUDIOIO_READ_ALL_ACODEC_REGS_CTRL     _IOW(AUDIOIO_IOC_MAGIC, 17,\
+				struct audioio_read_all_acodec_reg_ctrl_t)
+#define AUDIOIO_FSBITCLK_CTRL            _IOW(AUDIOIO_IOC_MAGIC, 18,\
+						struct audioio_fsbitclk_ctrl_t)
+#define AUDIOIO_PSEUDOBURST_CTRL	_IOW(AUDIOIO_IOC_MAGIC, 19,\
+					struct audioio_pseudoburst_ctrl_t)
+#define AUDIOIO_AUDIOCODEC_PWR_CTRL	_IOW(AUDIOIO_IOC_MAGIC, 20, \
+					struct audioio_acodec_pwr_ctrl_t)
+#define AUDIOIO_FIR_COEFFS_CTRL		_IOW(AUDIOIO_IOC_MAGIC, 21, \
+					struct audioio_fir_coefficients_t)
+#define AUDIOIO_LOOP_GAIN_DESC_TRNSDR		_IOR(AUDIOIO_IOC_MAGIC, 22,\
+					struct audioio_gain_desc_trnsdr_t)
+#define AUDIOIO_CLK_SELECT_CTRL		_IOR(AUDIOIO_IOC_MAGIC, 23,\
+					struct	audioio_clk_select_t)
+/* audio codec channel ids */
+#define EAR_CH			0
+#define HS_CH			1
+#define IHF_CH			2
+#define VIBL_CH			3
+#define VIBR_CH			4
+#define MIC1A_CH		5
+#define MIC1B_CH		6
+#define MIC2_CH			7
+#define LIN_CH			8
+#define DMIC12_CH		9
+#define DMIC34_CH		10
+#define DMIC56_CH		11
+#define MULTI_MIC_CH		12
+#define FMRX_CH			13
+#define FMTX_CH			14
+#define BLUETOOTH_CH	15
+
+#define FIRST_CH		EAR_CH
+#define LAST_CH			BLUETOOTH_CH
+
+#define MAX_NO_TRANSDUCERS	16
+#define STE_AUDIOIO_MAX_COEFFICIENTS	128
+#define MAX_NO_OF_LOOPS			19
+
+#define AUDIOIO_TRUE		1
+#define AUDIOIO_FALSE		0
+
+enum AUDIOIO_CLK_TYPE {
+	AUDIOIO_ULP_CLK,
+	AUDIOIO_SYS_CLK
+};
+
+enum AUDIOIO_COMMON_SWITCH {
+	AUDIOIO_COMMON_OFF = 0,
+	AUDIOIO_COMMON_ON,
+	AUDIOIO_COMMON_ALLCHANNEL_UNSUPPORTED = 0xFFFF
+};
+
+enum AUDIOIO_HAL_HW_LOOPS {
+	AUDIOIO_NO_LOOP = 0x0,
+	AUDIOIO_SIDETONE_LOOP = 0x01,
+	AUDIOIO_MIC1B_TO_HFL = 0x02,
+	AUDIOIO_MIC1B_TO_HFR = 0x04,
+	AUDIOIO_MIC1B_TO_EAR = 0x08,
+	AUDIOIO_MIC1A_TO_HSL = 0x10,
+	AUDIOIO_MIC1A_TO_HSR = 0x20,
+	AUDIOIO_MIC1A_TO_HSR_HSL = 0x40,
+	AUDIOIO_LINEIN_TO_HF = 0x80,
+	AUDIOIO_DMIC12_TO_HSR_HSL = 0x100,
+	AUDIOIO_DIC34_TO_HSR_HSL = 0x200,
+	AUDIOIO_DIC56_TO_HSR_HSL = 0x400,
+	AUDIOIO_DMIC12_TO_ST = 0x800,
+	AUDIOIO_DMIC34_TO_ST = 0x1000,
+	AUDIOIO_DMIC56_TO_ST = 0x2000,
+	AUDIOIO_ANC_LOOP = 0x4000,
+	AUDIOIO_LININ_HS = 0x8000,
+	AUDIOIO_LININL_HSL = 0x10000,
+	AUDIOIO_LININ_HSR = 0x20000
+};
+
+
+enum AUDIOIO_FADE_PERIOD {
+	e_FADE_00,
+	e_FADE_01,
+	e_FADE_10,
+	e_FADE_11
+};
+
+enum AUDIOIO_CH_INDEX {
+	e_CHANNEL_1 = 0x01,
+	e_CHANNEL_2 = 0x02,
+	e_CHANNEL_3 = 0x04,
+	e_CHANNEL_4 = 0x08,
+	e_CHANNEL_ALL = 0x0f
+};
+
+struct audioio_data_t {
+	unsigned char	block;
+	unsigned char	addr;
+	unsigned char	data;
+};
+
+struct audioio_pwr_ctrl_t {
+	enum AUDIOIO_COMMON_SWITCH ctrl_switch;
+	int channel_type;
+	enum AUDIOIO_CH_INDEX channel_index;
+};
+
+struct audioio_acodec_pwr_ctrl_t {
+	enum AUDIOIO_COMMON_SWITCH ctrl_switch;
+};
+
+struct audioio_loop_ctrl_t  {
+	enum AUDIOIO_HAL_HW_LOOPS hw_loop;
+	enum AUDIOIO_COMMON_SWITCH ctrl_switch;
+	int channel_type;
+	enum AUDIOIO_CH_INDEX channel_index;
+	int loop_gain;
+};
+
+struct audioio_get_gain_t {
+	unsigned int num_channels;
+	unsigned short max_num_gain;
+};
+
+struct audioio_gain_loop_t {
+	int channel_type;
+	unsigned short num_loop;
+	unsigned short max_gains;
+};
+
+struct audioio_support_loop_t {
+	int channel_type;
+	unsigned short spprtd_loop_index;
+};
+
+struct audioio_gain_desc_trnsdr_t {
+	enum AUDIOIO_CH_INDEX channel_index;
+	int channel_type;
+	unsigned short gain_index;
+	int min_gain;
+	int max_gain;
+	unsigned int gain_step;
+};
+
+struct audioio_gain_ctrl_trnsdr_t {
+	enum AUDIOIO_CH_INDEX channel_index;
+	int channel_type;
+	unsigned short gain_index;
+	int gain_value;
+	unsigned int linear;
+};
+
+struct audioio_mute_trnsdr_t  {
+	int channel_type;
+	enum AUDIOIO_CH_INDEX channel_index;
+	enum AUDIOIO_COMMON_SWITCH ctrl_switch;
+};
+
+struct audioio_fade_ctrl_t  {
+	enum AUDIOIO_COMMON_SWITCH ctrl_switch;
+	enum AUDIOIO_FADE_PERIOD fade_period;
+	int channel_type;
+	enum AUDIOIO_CH_INDEX channel_index;
+};
+
+struct audioio_burst_ctrl_t {
+	enum AUDIOIO_COMMON_SWITCH ctrl_switch;
+	int channel_type;
+	int burst_fifo_interrupt_sample_count;
+	int burst_fifo_length;/* BFIFOTx */
+	int burst_fifo_switch_frame;
+	int burst_fifo_sample_number;
+};
+
+struct audioio_read_all_acodec_reg_ctrl_t {
+	unsigned char data[200];
+};
+
+struct audioio_fsbitclk_ctrl_t {
+	enum AUDIOIO_COMMON_SWITCH ctrl_switch;
+};
+
+struct audioio_pseudoburst_ctrl_t {
+	enum AUDIOIO_COMMON_SWITCH ctrl_switch;
+};
+
+struct audioio_fir_coefficients_t {
+	unsigned char start_addr;
+	unsigned short coefficients[STE_AUDIOIO_MAX_COEFFICIENTS];
+};
+
+struct audioio_clk_select_t {
+	enum AUDIOIO_CLK_TYPE required_clk;
+};
+#endif
diff --git a/arch/arm/mach-ux500/include/mach/ste_audio_io_vibrator.h b/arch/arm/mach-ux500/include/mach/ste_audio_io_vibrator.h
new file mode 100644
index 00000000000..6b6a558e90a
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/ste_audio_io_vibrator.h
@@ -0,0 +1,37 @@
+/*
+* Overview:
+*   Header File defining vibrator kernel space interface
+*
+* Copyright (C) 2010 ST Ericsson
+*
+* 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 _STE_AUDIO_IO_VIBRATOR_H_
+#define _STE_AUDIO_IO_VIBRATOR_H_
+
+/* Client definitions which can use vibrator, defined as bitmask */
+#define STE_AUDIOIO_CLIENT_AUDIO_L	1
+#define STE_AUDIOIO_CLIENT_AUDIO_R	2
+#define STE_AUDIOIO_CLIENT_FF_VIBRA     4
+#define STE_AUDIOIO_CLIENT_TIMED_VIBRA  8
+
+/*
+ * Define vibrator's maximum speed allowed
+ * Duty cycle supported by vibrator's PWM is 0-100
+ */
+#define STE_AUDIOIO_VIBRATOR_MAX_SPEED	100
+
+/* Vibrator speed structure */
+struct ste_vibra_speed {
+	unsigned char positive;
+	unsigned char negative;
+};
+
+/* Vibrator control function - uses PWM source */
+int ste_audioio_vibrator_pwm_control(int client,
+	struct ste_vibra_speed left_speed, struct ste_vibra_speed right_speed);
+
+#endif
diff --git a/arch/arm/mach-ux500/include/mach/suspend.h b/arch/arm/mach-ux500/include/mach/suspend.h
new file mode 100644
index 00000000000..5c9e1c48605
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/suspend.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#ifndef __MACH_SUSPEND_H
+#define __MACH_SUSPEND_H
+
+void suspend_block_sleep(void);
+void suspend_unblock_sleep(void);
+
+#ifdef CONFIG_UX500_SUSPEND
+void suspend_set_pins_force_fn(void (*force)(void), void (*force_mux)(void));
+#else
+static inline void suspend_set_pins_force_fn(void (*force)(void),
+					     void (*force_mux)(void)) { }
+#endif
+
+#endif /* __MACH_SUSPEND_H */
diff --git a/arch/arm/mach-ux500/include/mach/system.h b/arch/arm/mach-ux500/include/mach/system.h
index c0cd8006f1a..89699238681 100644
--- a/arch/arm/mach-ux500/include/mach/system.h
+++ b/arch/arm/mach-ux500/include/mach/system.h
@@ -8,6 +8,9 @@
 #ifndef __ASM_ARCH_SYSTEM_H
 #define __ASM_ARCH_SYSTEM_H
 
+#include <mach/prcmu.h>
+#include <mach/reboot_reasons.h>
+
 static inline void arch_idle(void)
 {
 	/*
@@ -19,7 +22,10 @@ static inline void arch_idle(void)
 
 static inline void arch_reset(char mode, const char *cmd)
 {
-	/* yet to be implemented - TODO */
+#ifdef CONFIG_UX500_SOC_DB8500
+	/* Call the PRCMU reset API (w/o reset reason code) */
+	prcmu_system_reset(SW_RESET_NO_ARGUMENT);
+#endif
 }
 
 #endif
diff --git a/arch/arm/mach-ux500/include/mach/tee_ta_start_modem.h b/arch/arm/mach-ux500/include/mach/tee_ta_start_modem.h
new file mode 100644
index 00000000000..6978b7314c5
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/tee_ta_start_modem.h
@@ -0,0 +1,48 @@
+/*
+ * Data types and interface for TEE application for starting the modem.
+ *
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef TEE_TA_START_MODEM_H
+#define TEE_TA_START_MODEM_H
+
+#define COMMAND_ID_START_MODEM		0x00000001
+
+#define UUID_TEE_TA_START_MODEM_LOW	0x8AD94107
+#define UUID_TEE_TA_START_MODEM_MID	0x6E50
+#define UUID_TEE_TA_START_MODEM_HIGH	0x418E
+#define UUID_TEE_TA_START_MODEM_CLOCKSEQ \
+	{0xB1, 0x14, 0x75, 0x7D, 0x60, 0x21, 0xBD, 0x36}
+
+struct mcore_segment_descr {
+	void *segment;
+	void *hash;
+	u32 size;
+};
+
+struct access_image_descr {
+	void *elf_hdr;
+	void *pgm_hdr_tbl;
+	void *signature;
+	unsigned long nbr_segment;
+	struct mcore_segment_descr *descr;
+};
+
+/* TODO: To be redefined with only info needed by Secure world. */
+struct tee_ta_start_modem {
+	void *access_mem_start;
+	u32 shared_mem_size;
+	u32 access_private_mem_size;
+	struct access_image_descr access_image_descr;
+};
+
+/**
+ * This is the function to handle the modem release.
+ */
+int tee_ta_start_modem(struct tee_ta_start_modem *data);
+
+#endif
+
diff --git a/arch/arm/mach-ux500/include/mach/timex.h b/arch/arm/mach-ux500/include/mach/timex.h
index d0942c17401..0ba497bd9d7 100644
--- a/arch/arm/mach-ux500/include/mach/timex.h
+++ b/arch/arm/mach-ux500/include/mach/timex.h
@@ -2,5 +2,6 @@
 #define __ASM_ARCH_TIMEX_H
 
 #define CLOCK_TICK_RATE		110000000
+#define ARCH_HAS_READ_CURRENT_TIMER
 
 #endif
diff --git a/arch/arm/mach-ux500/include/mach/u8500_acodec_ab8500.h b/arch/arm/mach-ux500/include/mach/u8500_acodec_ab8500.h
new file mode 100644
index 00000000000..0575bbdb730
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/u8500_acodec_ab8500.h
@@ -0,0 +1,284 @@
+/* Header file for u8500 audiocodec specific data structures, enums
+ * and private & public functions.
+ * Author: Deepak Karda
+ * Copyright (C) 2009 ST-Ericsson Pvt. Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#ifndef _U8500_ACODEC_AB8500_H_
+#define _U8500_ACODEC_AB8500_H_
+
+#include <mach/ab8500.h>
+#include <linux/i2s/i2s.h>
+
+#ifdef CONFIG_U8500_AB8500_CUT10
+#include <mach/ab8500_codec_v1_0.h>
+//#include <mach/ab8500_codec_p_v1_0.h>
+#else /*CONFIG_U8500_4500_ED */
+#include <mach/ab8500_codec.h>
+#include <mach/ab8500_codec_p.h>
+#endif
+
+#define NUMBER_OUTPUT_DEVICE				5
+#define NUMBER_INPUT_DEVICE					13
+#define NUMBER_LOOPBACK_STATE				2
+#define NUMBER_SWITCH_STATE					2
+#define NUMBER_POWER_STATE					2
+#define NUMBER_TDM_MODE_STATE				2
+#define NUMBER_DIRECT_RENDERING_STATE		2
+#define NUMBER_PCM_RENDERING_STATE			3
+
+#define CODEC_MUTE      					0x20
+#define DEFAULT_VOLUME      				0x64
+#define DEFAULT_GAIN        				0x32
+#define VOL_MAX                 			0x64
+#define VOL_MIN                 			0x00
+#define DEFAULT_OUTPUT_DEVICE   			AB8500_CODEC_DEST_HEADSET
+#define DEFAULT_INPUT_DEVICE    			AB8500_CODEC_SRC_D_MICROPHONE_1
+#define DEFAULT_LOOPBACK_STATE				DISABLE
+#define DEFAULT_SWITCH_STATE				DISABLE
+#define DEFAULT_TDM8_CH_MODE_STATE			DISABLE
+#define DEFAULT_DIRECT_RENDERING_STATE		DISABLE
+#define DEFAULT_BURST_FIFO_STATE			RENDERING_DISABLE
+#define DEFAULT_FM_PLAYBACK_STATE			RENDERING_DISABLE
+#define DEFAULT_FM_TX_STATE					RENDERING_DISABLE
+
+#define MIN_RATE_PLAYBACK   48000
+#define MAX_RATE_PLAYBACK   48000
+#define MIN_RATE_CAPTURE    48000
+#define MAX_RATE_CAPTURE    48000
+#define MAX_NO_OF_RATES		1
+
+#define ALSA_MSP_BT_NUM 		0
+#define ALSA_MSP_PCM_NUM 		1
+#define ALSA_MSP_HDMI_NUM 		2
+
+#define I2S_CLIENT_MSP0				0
+#define I2S_CLIENT_MSP1				1
+#define I2S_CLIENT_MSP2				2
+
+typedef enum {
+	DISABLE,
+	ENABLE
+} t_u8500_bool_state;
+
+typedef enum {
+	RENDERING_DISABLE,
+	RENDERING_ENABLE,
+	RENDERING_PENDING
+} t_u8500_pmc_rendering_state;
+
+typedef enum {
+	ACODEC_CONFIG_REQUIRED,
+	ACODEC_CONFIG_NOT_REQUIRED
+} t_u8500_acodec_config_need;
+
+typedef enum {
+	CLASSICAL_MODE,
+	TDM_8_CH_MODE
+} t_u8500_mode;
+
+typedef struct {
+	unsigned int left_volume;
+	unsigned int right_volume;
+	unsigned int mute_state;
+	t_u8500_bool_state power_state;
+} u8500_io_dev_config_t;
+
+typedef enum {
+	NO_USER = 0,
+	USER_ALSA = 2,		/*To make it equivalent to user id for MSP */
+	USER_SAA,
+} t_acodec_user;
+
+typedef struct {
+	u8500_io_dev_config_t output_config[NUMBER_OUTPUT_DEVICE];
+	u8500_io_dev_config_t input_config[NUMBER_INPUT_DEVICE];
+	//t_acodec_user  user;
+	t_acodec_user cur_user;
+} t_u8500_codec_system_context;
+
+typedef enum {
+	T_CODEC_SAMPLING_FREQ_48KHZ = 48,
+} acodec_sample_frequency;
+
+struct acodec_configuration {
+	t_ab8500_codec_direction direction;
+	acodec_sample_frequency input_frequency;
+	acodec_sample_frequency output_frequency;
+	codec_msp_srg_clock_sel_type mspClockSel;
+	codec_msp_in_clock_freq_type mspInClockFreq;
+	u32 channels;
+	t_acodec_user user;
+	t_u8500_acodec_config_need acodec_config_need;
+	t_u8500_bool_state direct_rendering_mode;
+	t_u8500_bool_state tdm8_ch_mode;
+	t_u8500_bool_state digital_loopback;
+	void (*handler) (void *data);
+	void *tx_callback_data;
+	void *rx_callback_data;
+};
+
+typedef enum {
+	ACODEC_DISABLE_ALL,
+	ACODEC_DISABLE_TRANSMIT,
+	ACODEC_DISABLE_RECEIVE,
+} t_acodec_disable;
+
+struct i2sdrv_data {
+	struct i2s_device *i2s;
+	spinlock_t i2s_lock;
+	/* buffer is NULL unless this device is open (users > 0) */
+	int flag;
+	u32 tx_status;
+	u32 rx_status;
+};
+
+#define MAX_I2S_CLIENTS 3	//0=BT, 1=ACODEC, 2=HDMI
+
+/*extern t_ab8500_codec_error u8500_acodec_set_volume(int input_vol_left,
+					int input_vol_right,
+					int output_vol_left,
+					int output_vol_right, t_acodec_user user);*/
+
+extern t_ab8500_codec_error u8500_acodec_open(int client_id, int stream_id);
+//extern t_ab8500_codec_error u8500_acodec_pause_transfer(void);
+//extern t_ab8500_codec_error u8500_acodec_unpause_transfer(void);
+extern t_ab8500_codec_error u8500_acodec_send_data(int client_id, void *data,
+						   size_t bytes, int dma_flag);
+extern t_ab8500_codec_error u8500_acodec_receive_data(int client_id, void *data,
+						      size_t bytes,
+						      int dma_flag);
+extern t_ab8500_codec_error u8500_acodec_close(int client_id,
+					       t_acodec_disable flag);
+extern t_ab8500_codec_error u8500_acodec_tx_rx_data(int client_id,
+						    void *tx_data,
+						    size_t tx_bytes,
+						    void *rx_data,
+						    size_t rx_bytes,
+						    int dma_flag);
+
+extern t_ab8500_codec_error u8500_acodec_set_output_volume(t_ab8500_codec_dest
+							   dest_device,
+							   int left_volume,
+							   int right_volume,
+							   t_acodec_user user);
+
+extern t_ab8500_codec_error u8500_acodec_get_output_volume(t_ab8500_codec_dest
+							   dest_device,
+							   int *p_left_volume,
+							   int *p_right_volume,
+							   t_acodec_user user);
+
+extern t_ab8500_codec_error u8500_acodec_set_input_volume(t_ab8500_codec_src
+							  src_device,
+							  int left_volume,
+							  int right_volume,
+							  t_acodec_user user);
+
+extern t_ab8500_codec_error u8500_acodec_get_input_volume(t_ab8500_codec_src
+							  src_device,
+							  int *p_left_volume,
+							  int *p_right_volume,
+							  t_acodec_user user);
+
+extern t_ab8500_codec_error u8500_acodec_toggle_analog_lpbk(t_u8500_bool_state
+							    lpbk_state,
+							    t_acodec_user user);
+
+extern t_ab8500_codec_error u8500_acodec_toggle_digital_lpbk(t_u8500_bool_state
+							     lpbk_state,
+							     t_ab8500_codec_dest
+							     dest_device,
+							     t_ab8500_codec_src
+							     src_device,
+							     t_acodec_user user,
+							     t_u8500_bool_state
+							     tdm8_ch_mode);
+
+extern t_ab8500_codec_error
+u8500_acodec_toggle_playback_mute_control(t_ab8500_codec_dest dest_device,
+					  t_u8500_bool_state mute_state,
+					  t_acodec_user user);
+extern t_ab8500_codec_error
+u8500_acodec_toggle_capture_mute_control(t_ab8500_codec_src src_device,
+					 t_u8500_bool_state mute_state,
+					 t_acodec_user user);
+
+extern t_ab8500_codec_error u8500_acodec_enable_audio_mode(struct
+							   acodec_configuration
+							   *acodec_config);
+/*extern t_ab8500_codec_error u8500_acodec_enable_voice_mode(struct acodec_configuration *acodec_config);*/
+
+extern t_ab8500_codec_error u8500_acodec_select_input(t_ab8500_codec_src
+						      input_device,
+						      t_acodec_user user,
+						      t_u8500_mode mode);
+extern t_ab8500_codec_error u8500_acodec_select_output(t_ab8500_codec_dest
+						       output_device,
+						       t_acodec_user user,
+						       t_u8500_mode mode);
+
+extern t_ab8500_codec_error u8500_acodec_allocate_ad_slot(t_ab8500_codec_src
+							  input_device,
+							  t_u8500_mode mode);
+extern t_ab8500_codec_error u8500_acodec_unallocate_ad_slot(t_ab8500_codec_src
+							    input_device,
+							    t_u8500_mode mode);
+extern t_ab8500_codec_error u8500_acodec_allocate_da_slot(t_ab8500_codec_dest
+							  output_device,
+							  t_u8500_mode mode);
+extern t_ab8500_codec_error u8500_acodec_unallocate_da_slot(t_ab8500_codec_dest
+							    output_device,
+							    t_u8500_mode mode);
+
+extern t_ab8500_codec_error u8500_acodec_set_src_power_cntrl(t_ab8500_codec_src
+							     input_device,
+							     t_u8500_bool_state
+							     pwr_state);
+extern t_ab8500_codec_error
+u8500_acodec_set_dest_power_cntrl(t_ab8500_codec_dest output_device,
+				  t_u8500_bool_state pwr_state);
+
+extern t_u8500_bool_state u8500_acodec_get_src_power_state(t_ab8500_codec_src
+							   input_device);
+extern t_u8500_bool_state u8500_acodec_get_dest_power_state(t_ab8500_codec_dest
+							    output_device);
+extern t_ab8500_codec_error
+u8500_acodec_set_burst_mode_fifo(t_u8500_pmc_rendering_state fifo_state);
+
+extern t_ab8500_codec_error u8500_acodec_unsetuser(t_acodec_user user);
+extern t_ab8500_codec_error u8500_acodec_setuser(t_acodec_user user);
+
+extern void codec_power_init(void);
+extern void u8500_acodec_powerdown(void);
+
+//t_ab8500_codec_error acodec_msp_enable(t_touareg_codec_sample_frequency freq,int channels, t_acodec_user user);
+
+#define TRG_CODEC_ADDRESS_ON_SPI_BUS 	(0x0D)
+
+extern int ab8500_write(u8 block, u32 adr, u8 data);
+extern int ab8500_read(u8 block, u32 adr);
+
+#if 0
+#define FUNC_ENTER() printk("\n -Enter : %s",__FUNCTION__)
+#define FUNC_EXIT()  printk("\n -Exit  : %s",__FUNCTION__)
+#else
+#define FUNC_ENTER()
+#define FUNC_EXIT()
+#endif
+#endif /*END OF HEADSER FILE */
diff --git a/arch/arm/mach-ux500/include/mach/uart.h b/arch/arm/mach-ux500/include/mach/uart.h
new file mode 100644
index 00000000000..22feb1f84da
--- /dev/null
+++ b/arch/arm/mach-ux500/include/mach/uart.h
@@ -0,0 +1,17 @@
+/*
+ * Copyright (C) 2009 ST-Ericsson SA
+ *
+ * This file is licensed under  the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#ifndef _U8500_UART_H_
+#define _U8500_UART_H_
+
+struct uart_amba_plat_data {
+	void (*init) (void);
+	void (*exit) (void);
+};
+
+#endif /* _U8500_UART_H_ */
diff --git a/arch/arm/mach-ux500/include/mach/uncompress.h b/arch/arm/mach-ux500/include/mach/uncompress.h
index 088b550c40d..894f7d23f06 100644
--- a/arch/arm/mach-ux500/include/mach/uncompress.h
+++ b/arch/arm/mach-ux500/include/mach/uncompress.h
@@ -52,9 +52,8 @@ static inline void arch_decomp_setup(void)
 {
 	/* Check in run time if we run on an U8500 or U5500 */
 	if (machine_is_u8500() ||
-	    machine_is_svp8500v1() ||
-	    machine_is_svp8500v2() ||
-	    machine_is_hrefv60())
+	    machine_is_hrefv60()   ||
+	    machine_is_snowball())
 		ux500_uart_base = U8500_UART2_BASE;
 	else if (machine_is_u5500())
 		ux500_uart_base = U5500_UART0_BASE;
diff --git a/arch/arm/mach-ux500/include/mach/usb.h b/arch/arm/mach-ux500/include/mach/usb.h
index d3739d41881..5c27a72c10d 100644
--- a/arch/arm/mach-ux500/include/mach/usb.h
+++ b/arch/arm/mach-ux500/include/mach/usb.h
@@ -22,4 +22,12 @@ struct ux500_musb_board_data {
 
 void ux500_add_usb(resource_size_t base, int irq, int *dma_rx_cfg,
 	int *dma_tx_cfg);
+
+struct ab8500_usbgpio_platform_data {
+	int (*get)(struct device *device);
+	void (*enable)(void);
+	void (*disable)(void);
+	void (*put)(void);
+};
+
 #endif
diff --git a/arch/arm/mach-ux500/include/mach/vmalloc.h b/arch/arm/mach-ux500/include/mach/vmalloc.h
index a4945cb4117..e0c6c408552 100644
--- a/arch/arm/mach-ux500/include/mach/vmalloc.h
+++ b/arch/arm/mach-ux500/include/mach/vmalloc.h
@@ -15,4 +15,4 @@
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
-#define VMALLOC_END	0xf0000000UL
+#define VMALLOC_END	0xf7800000UL
diff --git a/arch/arm/mach-ux500/l2x0-prefetch.c b/arch/arm/mach-ux500/l2x0-prefetch.c
new file mode 100644
index 00000000000..48a4495533f
--- /dev/null
+++ b/arch/arm/mach-ux500/l2x0-prefetch.c
@@ -0,0 +1,160 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Mian Yousaf Kaukab <mian.yousaf.kaukab@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/tee.h>
+#include <linux/io.h>
+#include <mach/hardware.h>
+#include <asm/hardware/cache-l2x0.h>
+
+static struct tee_session session;
+static struct tee_context context;
+static void __iomem *l2x0_base;
+
+#define L2X0_PREFETCH_CTRL_REG (0x00000F60)
+#define L2X0_PREFETCH_CTRL_BIT_DATA_EN (1 << 28)
+#define L2X0_PREFETCH_CTRL_BIT_INST_EN (1 << 29)
+
+#define L2X0_UUID_TEE_TA_START_LOW	0xBC765EDE
+#define L2X0_UUID_TEE_TA_START_MID	0x6724
+#define L2X0_UUID_TEE_TA_START_HIGH	0x11DF
+#define L2X0_UUID_TEE_TA_START_CLOCKSEQ  \
+	{0x8E, 0x12, 0xEC, 0xDB, 0xDF, 0xD7, 0x20, 0x85}
+
+static void prefetch_enable(void)
+{
+	struct tee_operation operation;
+	u32 data;
+	int err;
+	int origin_err;
+
+	data = readl(l2x0_base + L2X0_PREFETCH_CTRL_REG);
+
+	pr_debug("l2x0-prefetch: %s start, preftect_ctrl=0x%08x\n", __func__,
+									data);
+	if (!(data & L2X0_PREFETCH_CTRL_BIT_INST_EN) ||
+		!(data & L2X0_PREFETCH_CTRL_BIT_DATA_EN)) {
+
+		data |= (L2X0_PREFETCH_CTRL_BIT_INST_EN |
+				L2X0_PREFETCH_CTRL_BIT_DATA_EN);
+
+		operation.shm[0].buffer = &data;
+		operation.shm[0].size = sizeof(data);
+		operation.shm[0].flags = TEEC_MEM_INPUT;
+		operation.flags = TEEC_MEMREF_0_USED;
+
+		err = teec_invoke_command(&session,
+			TEE_STA_SET_L2CC_PREFETCH_CTRL_REGISTER,
+			&operation, &origin_err);
+		if (err)
+			pr_err("l2x0-prefetch: prefetch enable failed, err=%d",
+									err);
+	}
+	pr_debug("l2x0-prefetch: %s end, prefetch_ctrl=0x%08x\n", __func__,
+		readl(l2x0_base + L2X0_PREFETCH_CTRL_REG));
+}
+
+static void prefetch_disable(void)
+{
+	struct tee_operation operation;
+	u32 data;
+	int err;
+	int origin_err;
+
+	data = readl(l2x0_base + L2X0_PREFETCH_CTRL_REG);
+
+	pr_debug("l2x0-prefetch: %s start, preftect_ctrl=0x%08x\n", __func__,
+									data);
+	if (data & (L2X0_PREFETCH_CTRL_BIT_INST_EN |
+			L2X0_PREFETCH_CTRL_BIT_DATA_EN)) {
+
+		data &= ~(L2X0_PREFETCH_CTRL_BIT_INST_EN |
+				L2X0_PREFETCH_CTRL_BIT_DATA_EN);
+
+		operation.shm[0].buffer = &data;
+		operation.shm[0].size = sizeof(data);
+		operation.shm[0].flags = TEEC_MEM_INPUT;
+		operation.flags = TEEC_MEMREF_0_USED;
+
+		err = teec_invoke_command(&session,
+			TEE_STA_SET_L2CC_PREFETCH_CTRL_REGISTER,
+			&operation, &origin_err);
+		if (err)
+			pr_err("l2x0-prefetch: prefetch disable failed, err=%d",
+									err);
+	}
+	pr_debug("l2x0-prefetch: %s end, prefetch_ctrl=0x%08x\n", __func__,
+		readl(l2x0_base + L2X0_PREFETCH_CTRL_REG));
+}
+
+static int __init prefetch_ctrl_init(void)
+{
+	int err;
+	int origin_err;
+	/* Selects trustzone application needed for the job. */
+	struct tee_uuid static_uuid = {
+		L2X0_UUID_TEE_TA_START_LOW,
+		L2X0_UUID_TEE_TA_START_MID,
+		L2X0_UUID_TEE_TA_START_HIGH,
+		L2X0_UUID_TEE_TA_START_CLOCKSEQ,
+	};
+
+	/* Get PL310 base address. It will be used as readonly. */
+	if (cpu_is_u5500())
+		l2x0_base = __io_address(U5500_L2CC_BASE);
+	else if (cpu_is_u8500())
+		l2x0_base = __io_address(U8500_L2CC_BASE);
+	else
+		ux500_unknown_soc();
+
+	err = teec_initialize_context(NULL, &context);
+	if (err) {
+		pr_err("l2x0-prefetch: unable to initialize tee context,"
+			" err = %d\n", err);
+		err = -EINVAL;
+		goto error0;
+	}
+
+	err = teec_open_session(&context, &session, &static_uuid,
+			TEEC_LOGIN_PUBLIC, NULL, NULL, &origin_err);
+	if (err) {
+		pr_err("l2x0-prefetch: unable to open tee session,"
+			" tee error = %d, origin error = %d\n",
+			err, origin_err);
+		err = -EINVAL;
+		goto error1;
+	}
+
+	outer_cache.prefetch_enable = prefetch_enable;
+	outer_cache.prefetch_disable = prefetch_disable;
+
+	pr_info("l2x0-prefetch: initialized.\n");
+
+	return 0;
+
+error1:
+	(void)teec_finalize_context(&context);
+error0:
+	return err;
+}
+
+static void __exit prefetch_ctrl_exit(void)
+{
+	outer_cache.prefetch_enable = NULL;
+	outer_cache.prefetch_disable = NULL;
+
+	(void)teec_close_session(&session);
+	(void)teec_finalize_context(&context);
+}
+
+/* Wait for TEE driver to be initialized. */
+late_initcall(prefetch_ctrl_init);
+module_exit(prefetch_ctrl_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("PL310 prefetch control");
diff --git a/arch/arm/mach-ux500/localtimer.c b/arch/arm/mach-ux500/localtimer.c
index 5ba113309a0..30d4f3e1557 100644
--- a/arch/arm/mach-ux500/localtimer.c
+++ b/arch/arm/mach-ux500/localtimer.c
@@ -18,12 +18,21 @@
 #include <asm/smp_twd.h>
 #include <asm/localtimer.h>
 
+#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
+void smp_timer_broadcast(const struct cpumask *mask);
+#endif
+
 /*
  * Setup the local clock events for a CPU.
  */
 int __cpuinit local_timer_setup(struct clock_event_device *evt)
 {
 	evt->irq = IRQ_LOCALTIMER;
-	twd_timer_setup(evt);
+
+#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
+        evt->broadcast = smp_timer_broadcast;
+#endif
+
+	twd_timer_setup_scalable(evt, 2500 * 1000, 2);
 	return 0;
 }
diff --git a/arch/arm/mach-ux500/mloader-db5500.c b/arch/arm/mach-ux500/mloader-db5500.c
new file mode 100644
index 00000000000..7798a1646d4
--- /dev/null
+++ b/arch/arm/mach-ux500/mloader-db5500.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors: Jonas Aaberg <jonas.aberg@stericsson.com>
+ *          Paer-Olof Haakansson <par-olof.hakansson@stericsson.com>
+ * for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/module.h>
+#include <linux/workqueue.h>
+#include <linux/platform_device.h>
+#include <linux/sysfs.h>
+#include <linux/slab.h>
+
+static ssize_t db5500_mloader_sysfs_addr(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf);
+
+static ssize_t db5500_mloader_sysfs_finalize(struct device *dev,
+				      struct device_attribute *attr,
+				      const char *buf, size_t count);
+
+
+static DEVICE_ATTR(addr, S_IRUSR|S_IRGRP, db5500_mloader_sysfs_addr, NULL);
+static DEVICE_ATTR(finalize, S_IWUSR, NULL, db5500_mloader_sysfs_finalize);
+
+static unsigned int db5500_bootargs_memmap_modem_start;
+static unsigned int db5500_bootargs_memmap_modem_total_size;
+
+static unsigned int db5500_mloader_shm_total_size;
+module_param_named(shm_total_size, db5500_mloader_shm_total_size, uint, 0600);
+MODULE_PARM_DESC(shm_total_size, "Total Size of SHM shared memory");
+
+static int __init db5500_bootargs_modem_memmap(char *p)
+{
+	db5500_bootargs_memmap_modem_total_size = memparse(p, &p);
+	if (*p == '@')
+		db5500_bootargs_memmap_modem_start = memparse(p + 1, &p);
+
+	return 0;
+}
+early_param("mem_modem", db5500_bootargs_modem_memmap);
+
+static int __init db5500_bootargs_shm_total_size(char *str)
+{
+	int ret;
+	ret = strict_strtoul(str, 0, &db5500_mloader_shm_total_size);
+	if (ret < 0)
+		return -EINVAL;
+	return 1;
+}
+early_param("mloader.shm_total_size", db5500_bootargs_shm_total_size);
+
+static int __exit db5500_mloader_remove(struct platform_device *pdev)
+{
+	sysfs_remove_file(&pdev->dev.kobj, &dev_attr_addr.attr);
+	sysfs_remove_file(&pdev->dev.kobj, &dev_attr_finalize.attr);
+
+	return 0;
+}
+
+
+static struct platform_driver db5500_mloader_driver = {
+	.driver = {
+		.name = "db5500_mloader",
+	},
+	.remove = __exit_p(db5500_mloader_remove),
+};
+
+struct db5500_mloader {
+	struct work_struct work;
+	struct platform_device *pdev;
+};
+
+static void db5500_mloader_clean_up(struct work_struct *work)
+{
+	struct db5500_mloader *m = container_of(work,
+						struct db5500_mloader,
+						work);
+
+	/* Remove this module */
+	platform_device_unregister(m->pdev);
+
+	platform_driver_unregister(&db5500_mloader_driver);
+	kfree(m);
+
+}
+
+static ssize_t db5500_mloader_sysfs_addr(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	return sprintf(buf, "0x%x 0x%x 0x%x\n",
+		       db5500_bootargs_memmap_modem_start,
+		       db5500_bootargs_memmap_modem_total_size,
+		       db5500_mloader_shm_total_size);
+}
+
+static ssize_t db5500_mloader_sysfs_finalize(struct device *dev,
+				      struct device_attribute *attr,
+				      const char *buf, size_t count)
+{
+	struct db5500_mloader *m;
+
+	m = kmalloc(sizeof(struct db5500_mloader), GFP_KERNEL);
+
+	m->pdev = container_of(dev,
+			       struct platform_device,
+			       dev);
+
+	INIT_WORK(&m->work, db5500_mloader_clean_up);
+
+	/* The module can not remove itself while being in a sysfs function,
+	 * it has to use a workqueue.
+	 */
+	schedule_work(&m->work);
+
+	return count;
+}
+
+static void db5500_mloader_release(struct device *dev)
+{
+	/* Nothing to release */
+}
+
+static int __init db5500_mloader_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+
+	pdev->dev.release = db5500_mloader_release;
+
+	ret = sysfs_create_file(&pdev->dev.kobj, &dev_attr_addr.attr);
+	if (ret)
+		return ret;
+	ret = sysfs_create_file(&pdev->dev.kobj, &dev_attr_finalize.attr);
+
+	if (ret) {
+		sysfs_remove_file(&pdev->dev.kobj, &dev_attr_addr.attr);
+		return ret;
+	}
+
+	return 0;
+
+}
+
+static int __init db5500_mloader_init(void)
+{
+/*
+ * mloader for Fairbanks. It exports the physical
+ * address where the modem side ELF should be located in a sysfs
+ * file to make it available for a user space utility.
+ * When the mLoader utility has picked up these settings, this module is no
+ * longer needed and can be removed by writing to sysfs finalize.
+ *
+ * The modem side should be loaded via mmap'ed /dev/mem
+ *
+ */
+
+	return platform_driver_probe(&db5500_mloader_driver,
+			db5500_mloader_probe);
+}
+module_init(db5500_mloader_init);
+
+
+static void __exit mloader_exit(void)
+{
+	platform_driver_unregister(&db5500_mloader_driver);
+}
+module_exit(mloader_exit);
+
+MODULE_AUTHOR("Jonas Aaberg <jonas.aberg@stericsson.com>");
+MODULE_LICENSE("GPL");
diff --git a/arch/arm/mach-ux500/mloader-db8500.c b/arch/arm/mach-ux500/mloader-db8500.c
new file mode 100644
index 00000000000..6171a9db82f
--- /dev/null
+++ b/arch/arm/mach-ux500/mloader-db8500.c
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2011 ST-Ericsson
+ *
+ * Author: Maxime Coquelin <maxime.coquelin-nonst@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ */
+#include <linux/types.h>
+#include <linux/platform_device.h>
+
+#include <mach/mloader-dbx500.h>
+
+static struct dbx500_ml_area modem_areas[] = {
+	{ .name = "modem_trace", .start = 0x6000000, .size = 0xf00000 },
+	{ .name = "modem_shared", .start = 0x6f00000, .size = 0x100000 },
+	{ .name = "modem_priv", .start = 0x7000000, .size = 0x1000000 },
+};
+
+static struct dbx500_ml_fw modem_fws[] = {
+	{ .name = "MODEM", .area = &modem_areas[0], .offset = 0x0 },
+	{ .name = "IPL", .area = &modem_areas[1], .offset = 0x00 },
+};
+
+static struct dbx500_mloader_pdata mloader_fw_data = {
+	.fws = modem_fws,
+	.nr_fws = ARRAY_SIZE(modem_fws),
+	.areas = modem_areas,
+	.nr_areas = ARRAY_SIZE(modem_areas),
+};
+
+struct platform_device mloader_fw_device = {
+	.name = "dbx500_mloader_fw",
+	.id = -1,
+	.dev = {
+		.platform_data	= &mloader_fw_data,
+	},
+	.num_resources = 0,
+};
+
+/* Default areas can be overloaded in cmdline */
+static int __init early_modem_priv(char *p)
+{
+	struct dbx500_ml_area *area = &modem_areas[2];
+
+	area->size = memparse(p, &p);
+
+	if (*p == '@')
+		area->start = memparse(p + 1, &p);
+
+	return 0;
+}
+early_param("mem_modem", early_modem_priv);
+
+static int __init early_modem_shared(char *p)
+{
+	struct dbx500_ml_area *area = &modem_areas[1];
+
+	area->size = memparse(p, &p);
+
+	if (*p == '@')
+		area->start = memparse(p + 1, &p);
+
+	return 0;
+}
+early_param("mem_mshared", early_modem_shared);
+
+static int __init early_modem_trace(char *p)
+{
+	struct dbx500_ml_area *area = &modem_areas[0];
+
+	area->size = memparse(p, &p);
+
+	if (*p == '@')
+		area->start = memparse(p + 1, &p);
+
+	return 0;
+}
+early_param("mem_mtrace", early_modem_trace);
+
diff --git a/arch/arm/mach-ux500/pins-db8500.h b/arch/arm/mach-ux500/pins-db8500.h
index f923764ee16..062c7acf457 100644
--- a/arch/arm/mach-ux500/pins-db8500.h
+++ b/arch/arm/mach-ux500/pins-db8500.h
@@ -35,40 +35,40 @@
 
 #define GPIO4_GPIO		PIN_CFG(4, GPIO)
 #define GPIO4_U1_RXD		PIN_CFG(4, ALT_A)
-#define GPIO4_I2C4_SCL		PIN_CFG_PULL(4, ALT_B, UP)
+#define GPIO4_I2C4_SCL		PIN_CFG(4, ALT_B)
 #define GPIO4_IP_TRSTn		PIN_CFG(4, ALT_C)
 
 #define GPIO5_GPIO		PIN_CFG(5, GPIO)
 #define GPIO5_U1_TXD		PIN_CFG(5, ALT_A)
-#define GPIO5_I2C4_SDA		PIN_CFG_PULL(5, ALT_B, UP)
+#define GPIO5_I2C4_SDA		PIN_CFG(5, ALT_B)
 #define GPIO5_IP_GPIO6		PIN_CFG(5, ALT_C)
 
 #define GPIO6_GPIO		PIN_CFG(6, GPIO)
 #define GPIO6_U1_CTSn		PIN_CFG(6, ALT_A)
-#define GPIO6_I2C1_SCL		PIN_CFG_PULL(6, ALT_B, UP)
+#define GPIO6_I2C1_SCL		PIN_CFG(6, ALT_B)
 #define GPIO6_IP_GPIO0		PIN_CFG(6, ALT_C)
 
 #define GPIO7_GPIO		PIN_CFG(7, GPIO)
 #define GPIO7_U1_RTSn		PIN_CFG(7, ALT_A)
-#define GPIO7_I2C1_SDA		PIN_CFG_PULL(7, ALT_B, UP)
+#define GPIO7_I2C1_SDA		PIN_CFG(7, ALT_B)
 #define GPIO7_IP_GPIO1		PIN_CFG(7, ALT_C)
 
 #define GPIO8_GPIO		PIN_CFG(8, GPIO)
-#define GPIO8_IPI2C_SDA		PIN_CFG_PULL(8, ALT_A, UP)
-#define GPIO8_I2C2_SDA		PIN_CFG_PULL(8, ALT_B, UP)
+#define GPIO8_IPI2C_SDA		PIN_CFG(8, ALT_A)
+#define GPIO8_I2C2_SDA		PIN_CFG(8, ALT_B)
 
 #define GPIO9_GPIO		PIN_CFG(9, GPIO)
-#define GPIO9_IPI2C_SCL		PIN_CFG_PULL(9, ALT_A, UP)
-#define GPIO9_I2C2_SCL		PIN_CFG_PULL(9, ALT_B, UP)
+#define GPIO9_IPI2C_SCL		PIN_CFG(9, ALT_A)
+#define GPIO9_I2C2_SCL		PIN_CFG(9, ALT_B)
 
 #define GPIO10_GPIO		PIN_CFG(10, GPIO)
-#define GPIO10_IPI2C_SDA	PIN_CFG_PULL(10, ALT_A, UP)
-#define GPIO10_I2C2_SDA		PIN_CFG_PULL(10, ALT_B, UP)
+#define GPIO10_IPI2C_SDA	PIN_CFG(10, ALT_A)
+#define GPIO10_I2C2_SDA		PIN_CFG(10, ALT_B)
 #define GPIO10_IP_GPIO3		PIN_CFG(10, ALT_C)
 
 #define GPIO11_GPIO		PIN_CFG(11, GPIO)
-#define GPIO11_IPI2C_SCL	PIN_CFG_PULL(11, ALT_A, UP)
-#define GPIO11_I2C2_SCL		PIN_CFG_PULL(11, ALT_B, UP)
+#define GPIO11_IPI2C_SCL	PIN_CFG(11, ALT_A)
+#define GPIO11_I2C2_SCL		PIN_CFG(11, ALT_B)
 #define GPIO11_IP_GPIO2		PIN_CFG(11, ALT_C)
 
 #define GPIO12_GPIO		PIN_CFG(12, GPIO)
@@ -87,66 +87,66 @@
 
 #define GPIO16_GPIO		PIN_CFG(16, GPIO)
 #define GPIO16_MSP0_RFS		PIN_CFG(16, ALT_A)
-#define GPIO16_I2C1_SCL		PIN_CFG_PULL(16, ALT_B, UP)
+#define GPIO16_I2C1_SCL		PIN_CFG(16, ALT_B)
 #define GPIO16_SLIM0_DAT	PIN_CFG(16, ALT_C)
 
 #define GPIO17_GPIO		PIN_CFG(17, GPIO)
 #define GPIO17_MSP0_RCK		PIN_CFG(17, ALT_A)
-#define GPIO17_I2C1_SDA		PIN_CFG_PULL(17, ALT_B, UP)
+#define GPIO17_I2C1_SDA		PIN_CFG(17, ALT_B)
 #define GPIO17_SLIM0_CLK	PIN_CFG(17, ALT_C)
 
 #define GPIO18_GPIO		PIN_CFG(18, GPIO)
-#define GPIO18_MC0_CMDDIR	PIN_CFG_PULL(18, ALT_A, UP)
+#define GPIO18_MC0_CMDDIR	PIN_CFG_INPUT(18, ALT_A, PULLUP)
 #define GPIO18_U2_RXD		PIN_CFG(18, ALT_B)
 #define GPIO18_MS_IEP		PIN_CFG(18, ALT_C)
 
 #define GPIO19_GPIO		PIN_CFG(19, GPIO)
-#define GPIO19_MC0_DAT0DIR	PIN_CFG_PULL(19, ALT_A, UP)
+#define GPIO19_MC0_DAT0DIR	PIN_CFG_INPUT(19, ALT_A, PULLUP)
 #define GPIO19_U2_TXD		PIN_CFG(19, ALT_B)
 #define GPIO19_MS_DAT0DIR	PIN_CFG(19, ALT_C)
 
 #define GPIO20_GPIO		PIN_CFG(20, GPIO)
-#define GPIO20_MC0_DAT2DIR	PIN_CFG_PULL(20, ALT_A, UP)
+#define GPIO20_MC0_DAT2DIR	PIN_CFG_INPUT(20, ALT_A, PULLUP)
 #define GPIO20_UARTMOD_TXD	PIN_CFG(20, ALT_B)
 #define GPIO20_IP_TRIGOUT	PIN_CFG(20, ALT_C)
 
 #define GPIO21_GPIO		PIN_CFG(21, GPIO)
-#define GPIO21_MC0_DAT31DIR	PIN_CFG_PULL(21, ALT_A, UP)
+#define GPIO21_MC0_DAT31DIR	PIN_CFG_INPUT(21, ALT_A, PULLUP)
 #define GPIO21_MSP0_SCK		PIN_CFG(21, ALT_B)
 #define GPIO21_MS_DAT31DIR	PIN_CFG(21, ALT_C)
 
 #define GPIO22_GPIO		PIN_CFG(22, GPIO)
-#define GPIO22_MC0_FBCLK	PIN_CFG_PULL(22, ALT_A, UP)
+#define GPIO22_MC0_FBCLK	PIN_CFG_INPUT(22, ALT_A, PULLUP)
 #define GPIO22_UARTMOD_RXD	PIN_CFG(22, ALT_B)
 #define GPIO22_MS_FBCLK		PIN_CFG(22, ALT_C)
 
 #define GPIO23_GPIO		PIN_CFG(23, GPIO)
-#define GPIO23_MC0_CLK		PIN_CFG_PULL(23, ALT_A, UP)
+#define GPIO23_MC0_CLK		PIN_CFG_INPUT(23, ALT_A, PULLUP)
 #define GPIO23_STMMOD_CLK	PIN_CFG(23, ALT_B)
 #define GPIO23_MS_CLK		PIN_CFG(23, ALT_C)
 
 #define GPIO24_GPIO		PIN_CFG(24, GPIO)
-#define GPIO24_MC0_CMD		PIN_CFG_PULL(24, ALT_A, UP)
+#define GPIO24_MC0_CMD		PIN_CFG_INPUT(24, ALT_A, PULLUP)
 #define GPIO24_UARTMOD_RXD	PIN_CFG(24, ALT_B)
 #define GPIO24_MS_BS		PIN_CFG(24, ALT_C)
 
 #define GPIO25_GPIO		PIN_CFG(25, GPIO)
-#define GPIO25_MC0_DAT0		PIN_CFG_PULL(25, ALT_A, UP)
+#define GPIO25_MC0_DAT0		PIN_CFG_INPUT(25, ALT_A, PULLUP)
 #define GPIO25_STMMOD_DAT0	PIN_CFG(25, ALT_B)
 #define GPIO25_MS_DAT0		PIN_CFG(25, ALT_C)
 
 #define GPIO26_GPIO		PIN_CFG(26, GPIO)
-#define GPIO26_MC0_DAT1		PIN_CFG_PULL(26, ALT_A, UP)
+#define GPIO26_MC0_DAT1		PIN_CFG_INPUT(26, ALT_A, PULLUP)
 #define GPIO26_STMMOD_DAT1	PIN_CFG(26, ALT_B)
 #define GPIO26_MS_DAT1		PIN_CFG(26, ALT_C)
 
 #define GPIO27_GPIO		PIN_CFG(27, GPIO)
-#define GPIO27_MC0_DAT2		PIN_CFG_PULL(27, ALT_A, UP)
+#define GPIO27_MC0_DAT2		PIN_CFG_INPUT(27, ALT_A, PULLUP)
 #define GPIO27_STMMOD_DAT2	PIN_CFG(27, ALT_B)
 #define GPIO27_MS_DAT2		PIN_CFG(27, ALT_C)
 
 #define GPIO28_GPIO		PIN_CFG(28, GPIO)
-#define GPIO28_MC0_DAT3		PIN_CFG_PULL(28, ALT_A, UP)
+#define GPIO28_MC0_DAT3		PIN_CFG_INPUT(28, ALT_A, PULLUP)
 #define GPIO28_STMMOD_DAT3	PIN_CFG(28, ALT_B)
 #define GPIO28_MS_DAT3		PIN_CFG(28, ALT_C)
 
@@ -357,48 +357,48 @@
 #define GPIO97_MC5_DAT7		PIN_CFG(97, ALT_C)
 
 #define GPIO128_GPIO		PIN_CFG(128, GPIO)
-#define GPIO128_MC2_CLK		PIN_CFG_PULL(128, ALT_A, UP)
+#define GPIO128_MC2_CLK		PIN_CFG_INPUT(128, ALT_A, PULLUP)
 #define GPIO128_SM_CKO		PIN_CFG(128, ALT_B)
 
 #define GPIO129_GPIO		PIN_CFG(129, GPIO)
-#define GPIO129_MC2_CMD		PIN_CFG_PULL(129, ALT_A, UP)
+#define GPIO129_MC2_CMD		PIN_CFG_INPUT(129, ALT_A, PULLUP)
 #define GPIO129_SM_WAIT0n	PIN_CFG(129, ALT_B)
 
 #define GPIO130_GPIO		PIN_CFG(130, GPIO)
-#define GPIO130_MC2_FBCLK	PIN_CFG_PULL(130, ALT_A, UP)
+#define GPIO130_MC2_FBCLK	PIN_CFG_INPUT(130, ALT_A, PULLUP)
 #define GPIO130_SM_FBCLK	PIN_CFG(130, ALT_B)
 #define GPIO130_MC2_RSTN	PIN_CFG(130, ALT_C)
 
 #define GPIO131_GPIO		PIN_CFG(131, GPIO)
-#define GPIO131_MC2_DAT0	PIN_CFG_PULL(131, ALT_A, UP)
+#define GPIO131_MC2_DAT0	PIN_CFG_INPUT(131, ALT_A, PULLUP)
 #define GPIO131_SM_ADQ8		PIN_CFG(131, ALT_B)
 
 #define GPIO132_GPIO		PIN_CFG(132, GPIO)
-#define GPIO132_MC2_DAT1	PIN_CFG_PULL(132, ALT_A, UP)
+#define GPIO132_MC2_DAT1	PIN_CFG_INPUT(132, ALT_A, PULLUP)
 #define GPIO132_SM_ADQ9		PIN_CFG(132, ALT_B)
 
 #define GPIO133_GPIO		PIN_CFG(133, GPIO)
-#define GPIO133_MC2_DAT2	PIN_CFG_PULL(133, ALT_A, UP)
+#define GPIO133_MC2_DAT2	PIN_CFG_INPUT(133, ALT_A, PULLUP)
 #define GPIO133_SM_ADQ10	PIN_CFG(133, ALT_B)
 
 #define GPIO134_GPIO		PIN_CFG(134, GPIO)
-#define GPIO134_MC2_DAT3	PIN_CFG_PULL(134, ALT_A, UP)
+#define GPIO134_MC2_DAT3	PIN_CFG_INPUT(134, ALT_A, PULLUP)
 #define GPIO134_SM_ADQ11	PIN_CFG(134, ALT_B)
 
 #define GPIO135_GPIO		PIN_CFG(135, GPIO)
-#define GPIO135_MC2_DAT4	PIN_CFG_PULL(135, ALT_A, UP)
+#define GPIO135_MC2_DAT4	PIN_CFG_INPUT(135, ALT_A, PULLUP)
 #define GPIO135_SM_ADQ12	PIN_CFG(135, ALT_B)
 
 #define GPIO136_GPIO		PIN_CFG(136, GPIO)
-#define GPIO136_MC2_DAT5	PIN_CFG_PULL(136, ALT_A, UP)
+#define GPIO136_MC2_DAT5	PIN_CFG_INPUT(136, ALT_A, PULLUP)
 #define GPIO136_SM_ADQ13	PIN_CFG(136, ALT_B)
 
 #define GPIO137_GPIO		PIN_CFG(137, GPIO)
-#define GPIO137_MC2_DAT6	PIN_CFG_PULL(137, ALT_A, UP)
+#define GPIO137_MC2_DAT6	PIN_CFG_INPUT(137, ALT_A, PULLUP)
 #define GPIO137_SM_ADQ14	PIN_CFG(137, ALT_B)
 
 #define GPIO138_GPIO		PIN_CFG(138, GPIO)
-#define GPIO138_MC2_DAT7	PIN_CFG_PULL(138, ALT_A, UP)
+#define GPIO138_MC2_DAT7	PIN_CFG_INPUT(138, ALT_A, PULLUP)
 #define GPIO138_SM_ADQ15	PIN_CFG(138, ALT_B)
 
 #define GPIO139_GPIO		PIN_CFG(139, GPIO)
@@ -434,10 +434,10 @@
 #define GPIO146_SSP0_TXD	PIN_CFG(146, ALT_A)
 
 #define GPIO147_GPIO		PIN_CFG(147, GPIO)
-#define GPIO147_I2C0_SCL	PIN_CFG_PULL(147, ALT_A, UP)
+#define GPIO147_I2C0_SCL	PIN_CFG(147, ALT_A)
 
 #define GPIO148_GPIO		PIN_CFG(148, GPIO)
-#define GPIO148_I2C0_SDA	PIN_CFG_PULL(148, ALT_A, UP)
+#define GPIO148_I2C0_SDA	PIN_CFG(148, ALT_A)
 
 #define GPIO149_GPIO		PIN_CFG(149, GPIO)
 #define GPIO149_IP_GPIO0	PIN_CFG(149, ALT_A)
@@ -459,82 +459,82 @@
 #define GPIO152_KP_O9		PIN_CFG(152, ALT_C)
 
 #define GPIO153_GPIO		PIN_CFG(153, GPIO)
-#define GPIO153_KP_I7		PIN_CFG_PULL(153, ALT_A, DOWN)
+#define GPIO153_KP_I7		PIN_CFG(153, ALT_A)
 #define GPIO153_LCD_D24		PIN_CFG(153, ALT_B)
 #define GPIO153_U2_RXD		PIN_CFG(153, ALT_C)
 
 #define GPIO154_GPIO		PIN_CFG(154, GPIO)
-#define GPIO154_KP_I6		PIN_CFG_PULL(154, ALT_A, DOWN)
+#define GPIO154_KP_I6		PIN_CFG(154, ALT_A)
 #define GPIO154_LCD_D25		PIN_CFG(154, ALT_B)
 #define GPIO154_U2_TXD		PIN_CFG(154, ALT_C)
 
 #define GPIO155_GPIO		PIN_CFG(155, GPIO)
-#define GPIO155_KP_I5		PIN_CFG_PULL(155, ALT_A, DOWN)
+#define GPIO155_KP_I5		PIN_CFG(155, ALT_A)
 #define GPIO155_LCD_D26		PIN_CFG(155, ALT_B)
 #define GPIO155_STMAPE_CLK	PIN_CFG(155, ALT_C)
 
 #define GPIO156_GPIO		PIN_CFG(156, GPIO)
-#define GPIO156_KP_I4		PIN_CFG_PULL(156, ALT_A, DOWN)
+#define GPIO156_KP_I4		PIN_CFG(156, ALT_A)
 #define GPIO156_LCD_D27		PIN_CFG(156, ALT_B)
 #define GPIO156_STMAPE_DAT3	PIN_CFG(156, ALT_C)
 
 #define GPIO157_GPIO		PIN_CFG(157, GPIO)
-#define GPIO157_KP_O7		PIN_CFG_PULL(157, ALT_A, UP)
+#define GPIO157_KP_O7		PIN_CFG(157, ALT_A)
 #define GPIO157_LCD_D28		PIN_CFG(157, ALT_B)
 #define GPIO157_STMAPE_DAT2	PIN_CFG(157, ALT_C)
 
 #define GPIO158_GPIO		PIN_CFG(158, GPIO)
-#define GPIO158_KP_O6		PIN_CFG_PULL(158, ALT_A, UP)
+#define GPIO158_KP_O6		PIN_CFG(158, ALT_A)
 #define GPIO158_LCD_D29		PIN_CFG(158, ALT_B)
 #define GPIO158_STMAPE_DAT1	PIN_CFG(158, ALT_C)
 
 #define GPIO159_GPIO		PIN_CFG(159, GPIO)
-#define GPIO159_KP_O5		PIN_CFG_PULL(159, ALT_A, UP)
+#define GPIO159_KP_O5		PIN_CFG(159, ALT_A)
 #define GPIO159_LCD_D30		PIN_CFG(159, ALT_B)
 #define GPIO159_STMAPE_DAT0	PIN_CFG(159, ALT_C)
 
 #define GPIO160_GPIO		PIN_CFG(160, GPIO)
-#define GPIO160_KP_O4		PIN_CFG_PULL(160, ALT_A, UP)
+#define GPIO160_KP_O4		PIN_CFG(160, ALT_A)
 #define GPIO160_LCD_D31		PIN_CFG(160, ALT_B)
 #define GPIO160_NONE		PIN_CFG(160, ALT_C)
 
 #define GPIO161_GPIO		PIN_CFG(161, GPIO)
-#define GPIO161_KP_I3		PIN_CFG_PULL(161, ALT_A, DOWN)
+#define GPIO161_KP_I3		PIN_CFG(161, ALT_A)
 #define GPIO161_LCD_D32		PIN_CFG(161, ALT_B)
 #define GPIO161_UARTMOD_RXD	PIN_CFG(161, ALT_C)
 
 #define GPIO162_GPIO		PIN_CFG(162, GPIO)
-#define GPIO162_KP_I2		PIN_CFG_PULL(162, ALT_A, DOWN)
+#define GPIO162_KP_I2		PIN_CFG(162, ALT_A)
 #define GPIO162_LCD_D33		PIN_CFG(162, ALT_B)
 #define GPIO162_UARTMOD_TXD	PIN_CFG(162, ALT_C)
 
 #define GPIO163_GPIO		PIN_CFG(163, GPIO)
-#define GPIO163_KP_I1		PIN_CFG_PULL(163, ALT_A, DOWN)
+#define GPIO163_KP_I1		PIN_CFG(163, ALT_A)
 #define GPIO163_LCD_D34		PIN_CFG(163, ALT_B)
 #define GPIO163_STMMOD_CLK	PIN_CFG(163, ALT_C)
 
 #define GPIO164_GPIO		PIN_CFG(164, GPIO)
-#define GPIO164_KP_I0		PIN_CFG_PULL(164, ALT_A, UP)
+#define GPIO164_KP_I0		PIN_CFG(164, ALT_A)
 #define GPIO164_LCD_D35		PIN_CFG(164, ALT_B)
 #define GPIO164_STMMOD_DAT3	PIN_CFG(164, ALT_C)
 
 #define GPIO165_GPIO		PIN_CFG(165, GPIO)
-#define GPIO165_KP_O3		PIN_CFG_PULL(165, ALT_A, UP)
+#define GPIO165_KP_O3		PIN_CFG(165, ALT_A)
 #define GPIO165_LCD_D36		PIN_CFG(165, ALT_B)
 #define GPIO165_STMMOD_DAT2	PIN_CFG(165, ALT_C)
 
 #define GPIO166_GPIO		PIN_CFG(166, GPIO)
-#define GPIO166_KP_O2		PIN_CFG_PULL(166, ALT_A, UP)
+#define GPIO166_KP_O2		PIN_CFG(166, ALT_A)
 #define GPIO166_LCD_D37		PIN_CFG(166, ALT_B)
 #define GPIO166_STMMOD_DAT1	PIN_CFG(166, ALT_C)
 
 #define GPIO167_GPIO		PIN_CFG(167, GPIO)
-#define GPIO167_KP_O1		PIN_CFG_PULL(167, ALT_A, UP)
+#define GPIO167_KP_O1		PIN_CFG(167, ALT_A)
 #define GPIO167_LCD_D38		PIN_CFG(167, ALT_B)
 #define GPIO167_STMMOD_DAT0	PIN_CFG(167, ALT_C)
 
 #define GPIO168_GPIO		PIN_CFG(168, GPIO)
-#define GPIO168_KP_O0		PIN_CFG_PULL(168, ALT_A, UP)
+#define GPIO168_KP_O0		PIN_CFG(168, ALT_A)
 #define GPIO168_LCD_D39		PIN_CFG(168, ALT_B)
 #define GPIO168_NONE		PIN_CFG(168, ALT_C)
 
@@ -569,39 +569,39 @@
 #define GPIO196_MSP2_RXD	PIN_CFG(196, ALT_A)
 
 #define GPIO197_GPIO		PIN_CFG(197, GPIO)
-#define GPIO197_MC4_DAT3	PIN_CFG_PULL(197, ALT_A, UP)
+#define GPIO197_MC4_DAT3	PIN_CFG_INPUT(197, ALT_A, PULLUP)
 
 #define GPIO198_GPIO		PIN_CFG(198, GPIO)
-#define GPIO198_MC4_DAT2	PIN_CFG_PULL(198, ALT_A, UP)
+#define GPIO198_MC4_DAT2	PIN_CFG_INPUT(198, ALT_A, PULLUP)
 
 #define GPIO199_GPIO		PIN_CFG(199, GPIO)
-#define GPIO199_MC4_DAT1	PIN_CFG_PULL(199, ALT_A, UP)
+#define GPIO199_MC4_DAT1	PIN_CFG_INPUT(199, ALT_A, PULLUP)
 
 #define GPIO200_GPIO		PIN_CFG(200, GPIO)
-#define GPIO200_MC4_DAT0	PIN_CFG_PULL(200, ALT_A, UP)
+#define GPIO200_MC4_DAT0	PIN_CFG_INPUT(200, ALT_A, PULLUP)
 
 #define GPIO201_GPIO		PIN_CFG(201, GPIO)
-#define GPIO201_MC4_CMD		PIN_CFG_PULL(201, ALT_A, UP)
+#define GPIO201_MC4_CMD		PIN_CFG_INPUT(201, ALT_A, PULLUP)
 
 #define GPIO202_GPIO		PIN_CFG(202, GPIO)
-#define GPIO202_MC4_FBCLK	PIN_CFG_PULL(202, ALT_A, UP)
+#define GPIO202_MC4_FBCLK	PIN_CFG_INPUT(202, ALT_A, PULLUP)
 #define GPIO202_PWL		PIN_CFG(202, ALT_B)
 #define GPIO202_MC4_RSTN	PIN_CFG(202, ALT_C)
 
 #define GPIO203_GPIO		PIN_CFG(203, GPIO)
-#define GPIO203_MC4_CLK		PIN_CFG_PULL(203, ALT_A, UP)
+#define GPIO203_MC4_CLK		PIN_CFG_INPUT(203, ALT_A, PULLUP)
 
 #define GPIO204_GPIO		PIN_CFG(204, GPIO)
-#define GPIO204_MC4_DAT7	PIN_CFG_PULL(204, ALT_A, UP)
+#define GPIO204_MC4_DAT7	PIN_CFG_INPUT(204, ALT_A, PULLUP)
 
 #define GPIO205_GPIO		PIN_CFG(205, GPIO)
-#define GPIO205_MC4_DAT6	PIN_CFG_PULL(205, ALT_A, UP)
+#define GPIO205_MC4_DAT6	PIN_CFG_INPUT(205, ALT_A, PULLUP)
 
 #define GPIO206_GPIO		PIN_CFG(206, GPIO)
-#define GPIO206_MC4_DAT5	PIN_CFG_PULL(206, ALT_A, UP)
+#define GPIO206_MC4_DAT5	PIN_CFG_INPUT(206, ALT_A, PULLUP)
 
 #define GPIO207_GPIO		PIN_CFG(207, GPIO)
-#define GPIO207_MC4_DAT4	PIN_CFG_PULL(207, ALT_A, UP)
+#define GPIO207_MC4_DAT4	PIN_CFG_INPUT(207, ALT_A, PULLUP)
 
 #define GPIO208_GPIO		PIN_CFG(208, GPIO)
 #define GPIO208_MC1_CLK		PIN_CFG(208, ALT_A)
@@ -632,21 +632,25 @@
 #define GPIO215_MC1_CMDDIR	PIN_CFG(215, ALT_A)
 #define GPIO215_MC3_DAT2DIR	PIN_CFG(215, ALT_B)
 #define GPIO215_CLKOUT1		PIN_CFG(215, ALT_C)
+#define GPIO215_SPI2_TXD	PIN_CFG(215, ALT_C)
 
 #define GPIO216_GPIO		PIN_CFG(216, GPIO)
 #define GPIO216_MC1_DAT2DIR	PIN_CFG(216, ALT_A)
 #define GPIO216_MC3_CMDDIR	PIN_CFG(216, ALT_B)
-#define GPIO216_I2C3_SDA	PIN_CFG_PULL(216, ALT_C, UP)
+#define GPIO216_I2C3_SDA	PIN_CFG(216, ALT_C)
+#define GPIO216_SPI2_FRM	PIN_CFG(216, ALT_C)
 
 #define GPIO217_GPIO		PIN_CFG(217, GPIO)
 #define GPIO217_MC1_DAT0DIR	PIN_CFG(217, ALT_A)
 #define GPIO217_MC3_DAT31DIR	PIN_CFG(217, ALT_B)
 #define GPIO217_CLKOUT2		PIN_CFG(217, ALT_C)
+#define GPIO217_SPI2_CLK	PIN_CFG(217, ALT_C)
 
 #define GPIO218_GPIO		PIN_CFG(218, GPIO)
 #define GPIO218_MC1_DAT31DIR	PIN_CFG(218, ALT_A)
 #define GPIO218_MC3_DAT0DIR	PIN_CFG(218, ALT_B)
-#define GPIO218_I2C3_SCL	PIN_CFG_PULL(218, ALT_C, UP)
+#define GPIO218_I2C3_SCL	PIN_CFG(218, ALT_C)
+#define GPIO218_SPI2_RXD	PIN_CFG(218, ALT_C)
 
 #define GPIO219_GPIO		PIN_CFG(219, GPIO)
 #define GPIO219_HSIR_FLA0	PIN_CFG(219, ALT_A)
@@ -694,12 +698,12 @@
 #define GPIO229_GPIO		PIN_CFG(229, GPIO)
 #define GPIO229_CLKOUT1		PIN_CFG(229, ALT_A)
 #define GPIO229_PWL		PIN_CFG(229, ALT_B)
-#define GPIO229_I2C3_SDA	PIN_CFG_PULL(229, ALT_C, UP)
+#define GPIO229_I2C3_SDA	PIN_CFG(229, ALT_C)
 
 #define GPIO230_GPIO		PIN_CFG(230, GPIO)
 #define GPIO230_CLKOUT2		PIN_CFG(230, ALT_A)
 #define GPIO230_PWL		PIN_CFG(230, ALT_B)
-#define GPIO230_I2C3_SCL	PIN_CFG_PULL(230, ALT_C, UP)
+#define GPIO230_I2C3_SCL	PIN_CFG(230, ALT_C)
 
 #define GPIO256_GPIO		PIN_CFG(256, GPIO)
 #define GPIO256_USB_NXT		PIN_CFG(256, ALT_A)
diff --git a/arch/arm/mach-ux500/pins.c b/arch/arm/mach-ux500/pins.c
new file mode 100644
index 00000000000..38c1d47b29a
--- /dev/null
+++ b/arch/arm/mach-ux500/pins.c
@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Rabin Vincent <rabin.vincent@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/err.h>
+#include <plat/pincfg.h>
+
+#include "pins.h"
+
+static LIST_HEAD(pin_lookups);
+static DEFINE_MUTEX(pin_lookups_mutex);
+static DEFINE_SPINLOCK(pins_lock);
+
+void __init ux500_pins_add(struct ux500_pin_lookup *pl, size_t num)
+{
+	mutex_lock(&pin_lookups_mutex);
+
+	while (num--) {
+		list_add_tail(&pl->node, &pin_lookups);
+		pl++;
+	}
+
+	mutex_unlock(&pin_lookups_mutex);
+}
+
+struct ux500_pins *ux500_pins_get(const char *name)
+{
+	struct ux500_pins *pins = NULL;
+	struct ux500_pin_lookup *pl;
+
+	mutex_lock(&pin_lookups_mutex);
+
+	list_for_each_entry(pl, &pin_lookups, node) {
+		if (!strcmp(pl->name, name)) {
+			pins = pl->pins;
+			goto out;
+		}
+	}
+
+out:
+	mutex_unlock(&pin_lookups_mutex);
+	return pins;
+}
+
+int ux500_pins_enable(struct ux500_pins *pins)
+{
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&pins_lock, flags);
+
+	if (pins->usage++ == 0)
+		ret = nmk_config_pins(pins->cfg, pins->num);
+
+	spin_unlock_irqrestore(&pins_lock, flags);
+	return ret;
+}
+
+int ux500_pins_disable(struct ux500_pins *pins)
+{
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&pins_lock, flags);
+
+	if (WARN_ON(pins->usage == 0))
+		goto out;
+
+	if (--pins->usage == 0)
+		ret = nmk_config_pins_sleep(pins->cfg, pins->num);
+
+out:
+	spin_unlock_irqrestore(&pins_lock, flags);
+	return ret;
+}
+
+void ux500_pins_put(struct ux500_pins *pins)
+{
+	WARN_ON(!pins);
+}
diff --git a/arch/arm/mach-ux500/pins.h b/arch/arm/mach-ux500/pins.h
new file mode 100644
index 00000000000..52e123963f1
--- /dev/null
+++ b/arch/arm/mach-ux500/pins.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Rabin Vincent <rabin.vincent@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#ifndef __MACH_UX500_PINS_H
+#define __MACH_UX500_PINS_H
+
+#include <linux/list.h>
+
+#define PIN_LOOKUP(_name, _pins)	\
+{					\
+	.name	= _name,		\
+	.pins	= _pins,		\
+}
+
+#define UX500_PINS(name, pins...)			\
+struct ux500_pins name = {				\
+	.cfg = (pin_cfg_t[]) {pins},			\
+	.num = ARRAY_SIZE(((pin_cfg_t[]) {pins})),	\
+}
+
+struct ux500_pins {
+	int usage;
+	int num;
+	pin_cfg_t *cfg;
+};
+
+struct ux500_pin_lookup {
+	struct list_head	node;
+	const char		*name;
+	struct ux500_pins	*pins;
+};
+
+void __init ux500_pins_add(struct ux500_pin_lookup *pl, size_t num);
+struct ux500_pins *ux500_pins_get(const char *name);
+int ux500_pins_enable(struct ux500_pins *pins);
+int ux500_pins_disable(struct ux500_pins *pins);
+void ux500_pins_put(struct ux500_pins *pins);
+
+#endif
diff --git a/arch/arm/mach-ux500/pm/Kconfig b/arch/arm/mach-ux500/pm/Kconfig
new file mode 100644
index 00000000000..47727b60775
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/Kconfig
@@ -0,0 +1,84 @@
+config U8500_CPUIDLE
+	tristate "CPUIdle support"
+	depends on (UX500_SOC_DB8500 || UX500_SOC_DB5500) && CPU_IDLE && !RTC_DRV_PL031 && PM
+	default y
+	select GENERIC_CLOCKEVENTS_BROADCAST
+	select UX500_CONTEXT
+	help
+	  Add support for CPUIdle for U8500.
+
+config U8500_CPUIDLE_DEEPEST_STATE
+	int "Deepest sleep state"
+	default 4 if UX500_SOC_DB8500
+	default 1 if UX500_SOC_DB5500
+	depends on U8500_CPUIDLE
+	help
+	  Set deepest sleep state. See the cstate struct in cpuidle.c.
+	  Default is ApSleep.
+
+config U8500_CPUIDLE_APDEEPIDLE
+	bool "CPUIdle ApDeepIdle"
+	depends on U8500_CPUIDLE
+	help
+	  Adds the power level ApDeepIdle, where APE is powered on while
+	  ARM is powered off. Default n.
+
+config U8500_CPUIDLE_DEBUG
+	bool "CPUIdle debug"
+	depends on U8500_CPUIDLE && DEBUG_FS
+	help
+	  Add debugging support for CPUIdle for U8500.
+
+config UX500_SUSPEND
+	bool "Suspend to mem and standby support"
+	depends on (UX500_SOC_DB8500 || UX500_SOC_DB5500) && PM
+	select UX500_CONTEXT
+	help
+	  Add support for suspend.
+
+config UX500_SUSPEND_STANDBY
+	bool "Suspend Standby goes to ApSleep"
+	depends on UX500_SUSPEND
+	help
+	  If yes, echo standby > /sys/power/state puts the system into ApSleep.
+
+config UX500_SUSPEND_MEM
+	bool "Suspend Mem goes to ApDeepSleep"
+	depends on UX500_SUSPEND && UX500_SOC_DB8500
+	help
+	  If yes, echo mem > /sys/power/state puts the system into ApDeepSleep else
+	  it will do the same as echo standby > /sys/power/state.
+
+config UX500_SUSPEND_DBG
+	bool "Suspend debug"
+	depends on UX500_SUSPEND && DEBUG_FS
+	help
+	  Add debug support for suspend.
+
+config UX500_SUSPEND_DBG_WAKE_ON_UART
+	bool "Suspend wakes on console UART"
+	depends on UX500_SUSPEND_DBG
+	help
+	  Wake up on uart interrupts. Makes it possible for the console to wake up system.
+
+config UX500_CONSOLE_UART_GPIO_PIN
+	int "The pin number of the console UART GPIO pin"
+	default 29
+	depends on UX500_SUSPEND_DBG_WAKE_ON_UART || U8500_CPUIDLE_DEBUG
+	help
+	  GPIO pin number of the GPIO pin connected to the console UART RX line.
+
+config UX500_CONTEXT
+	bool "Context save/restore support for UX500"
+	depends on (UX500_SOC_DB8500 || UX500_SOC_DB5500) && PM
+	help
+	  This is needed for ApSleep and deeper sleep states.
+
+config UX500_USECASE_GOVERNOR
+	bool "UX500 use-case governor"
+	depends on (UX500_SOC_DB8500 || UX500_SOC_DB5500) && \
+			(CPU_FREQ && CPU_IDLE && HOTPLUG_CPU && \
+			EARLYSUSPEND && UX500_L2X0_PREFETCH_CTRL && PM)
+	default y
+	help
+	  Adjusts CPU_IDLE, CPU_FREQ, HOTPLUG_CPU and L2 cache parameters
diff --git a/arch/arm/mach-ux500/pm/Makefile b/arch/arm/mach-ux500/pm/Makefile
new file mode 100644
index 00000000000..b237ec6fa16
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/Makefile
@@ -0,0 +1,12 @@
+#
+# Power save related files
+#
+obj-y := pm.o runtime.o
+
+obj-$(CONFIG_U8500_CPUIDLE) 		+= cpuidle.o timer.o
+obj-$(CONFIG_U8500_CPUIDLE_DEBUG) 	+= cpuidle_dbg.o
+obj-$(CONFIG_UX500_CONTEXT) 		+= context.o context_arm.o context-db8500.o context-db5500.o
+obj-$(CONFIG_UX500_SUSPEND)		+= suspend.o
+obj-$(CONFIG_UX500_SUSPEND_DBG)		+= suspend_dbg.o
+obj-$(CONFIG_UX500_USECASE_GOVERNOR)	+= usecase_gov.o
+
diff --git a/arch/arm/mach-ux500/pm/context-db5500.c b/arch/arm/mach-ux500/pm/context-db5500.c
new file mode 100644
index 00000000000..78904d3486e
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/context-db5500.c
@@ -0,0 +1,405 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com>,
+ *         Rickard Andersson <rickard.andersson@stericsson.com>,
+ *         Sundar Iyer <sundar.iyer@stericsson.com>,
+ *         ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+
+#include <linux/io.h>
+
+#include <mach/hardware.h>
+
+#include "context.h"
+
+/* These registers are DB5500 specific */
+#define NODE_HIBW1_ESRAM_IN_0_PRIORITY		0x0
+#define NODE_HIBW1_ESRAM_IN_1_PRIORITY		0x4
+
+#define NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT	0x18
+#define NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT	0x1C
+#define NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT	0x20
+
+#define NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT	0x24
+#define NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT	0x28
+#define NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT	0x2C
+
+#define NODE_HIBW1_DDR_IN_0_PRIORITY		0x400
+#define NODE_HIBW1_DDR_IN_1_PRIORITY		0x404
+#define NODE_HIBW1_DDR_IN_2_PRIORITY		0x408
+
+#define NODE_HIBW1_DDR_IN_0_LIMIT		0x424
+#define NODE_HIBW1_DDR_IN_1_LIMIT		0x428
+#define NODE_HIBW1_DDR_IN_2_LIMIT		0x42C
+
+#define NODE_HIBW1_DDR_OUT_0_PRIORITY		0x430
+
+#define NODE_HIBW2_ESRAM_IN_0_PRIORITY		0x800
+#define NODE_HIBW2_ESRAM_IN_1_PRIORITY		0x804
+
+#define NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT	0x818
+#define NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT	0x81C
+#define NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT	0x820
+
+#define NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT	0x824
+#define NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT	0x828
+#define NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT	0x82C
+
+#define NODE_HIBW2_DDR_IN_0_PRIORITY		0xC00
+#define NODE_HIBW2_DDR_IN_1_PRIORITY		0xC04
+#define NODE_HIBW2_DDR_IN_2_PRIORITY		0xC08
+#define NODE_HIBW2_DDR_IN_3_PRIORITY		0xC0C
+
+#define NODE_HIBW2_DDR_IN_0_LIMIT		0xC30
+#define NODE_HIBW2_DDR_IN_1_LIMIT		0xC34
+#define NODE_HIBW2_DDR_IN_2_LIMIT		0xC38
+#define NODE_HIBW2_DDR_IN_3_LIMIT		0xC3C
+
+#define NODE_HIBW2_DDR_OUT_0_PRIORITY		0xC40
+
+#define NODE_ESRAM0_IN_0_PRIORITY		0x1000
+#define NODE_ESRAM0_IN_1_PRIORITY		0x1004
+#define NODE_ESRAM0_IN_2_PRIORITY		0x1008
+
+#define NODE_ESRAM0_IN_0_LIMIT			0x1024
+#define NODE_ESRAM0_IN_1_LIMIT			0x1028
+#define NODE_ESRAM0_IN_2_LIMIT			0x102C
+#define NODE_ESRAM0_OUT_0_PRIORITY		0x1030
+
+#define NODE_ESRAM1_2_IN_0_PRIORITY		0x1400
+#define NODE_ESRAM1_2_IN_1_PRIORITY		0x1404
+#define NODE_ESRAM1_2_IN_2_PRIORITY		0x1408
+
+#define NODE_ESRAM1_2_IN_0_ARB_1_LIMIT		0x1424
+#define NODE_ESRAM1_2_IN_1_ARB_1_LIMIT		0x1428
+#define NODE_ESRAM1_2_IN_2_ARB_1_LIMIT		0x142C
+#define NODE_ESRAM1_2_OUT_0_PRIORITY		0x1430
+
+#define NODE_ESRAM3_4_IN_0_PRIORITY		0x1800
+#define NODE_ESRAM3_4_IN_1_PRIORITY		0x1804
+#define NODE_ESRAM3_4_IN_2_PRIORITY		0x1808
+
+#define NODE_ESRAM3_4_IN_0_ARB_1_LIMIT		0x1824
+#define NODE_ESRAM3_4_IN_1_ARB_1_LIMIT		0x1828
+#define NODE_ESRAM3_4_IN_2_ARB_1_LIMIT		0x182C
+#define NODE_ESRAM3_4_OUT_0_PRIORITY		0x1830
+
+/*
+ * Save ICN (Interconnect or Interconnect nodes) configuration registers
+ * TODO: This can be optimized, for example if we have
+ * a static ICN configuration.
+ */
+
+static struct {
+	void __iomem *base;
+	u32 hibw1_esram_in_pri[2];
+	u32 hibw1_esram_in0_arb[3];
+	u32 hibw1_esram_in1_arb[3];
+	u32 hibw1_ddr_in_prio[3];
+	u32 hibw1_ddr_in_limit[3];
+	u32 hibw1_ddr_out_prio_reg;
+
+	/* HiBw2 node registers */
+	u32 hibw2_esram_in_pri[2];
+	u32 hibw2_esram_in0_arblimit[3];
+	u32 hibw2_esram_in1_arblimit[3];
+	u32 hibw2_ddr_in_prio[4];
+	u32 hibw2_ddr_in_limit[4];
+	u32 hibw2_ddr_out_prio_reg;
+
+	/* ESRAM node registers */
+	u32 esram_in_prio[3];
+	u32 esram_in_lim[3];
+	u32 esram_out_prio_reg;
+
+	u32 esram12_in_prio[3];
+	u32 esram12_in_arb_lim[3];
+	u32 esram12_out_prio_reg;
+
+	u32 esram34_in_prio[3];
+	u32 esram34_in_arb_lim[3];
+	u32 esram34_out_prio;
+} context_icn;
+
+
+void u5500_context_save_icn(void)
+{
+	/* hibw1 */
+	context_icn.hibw1_esram_in_pri[0] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_0_PRIORITY);
+	context_icn.hibw1_esram_in_pri[1] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_1_PRIORITY);
+
+	context_icn.hibw1_esram_in0_arb[0] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT);
+	context_icn.hibw1_esram_in0_arb[1] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT);
+	context_icn.hibw1_esram_in0_arb[2] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT);
+
+	context_icn.hibw1_esram_in1_arb[0] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT);
+	context_icn.hibw1_esram_in1_arb[1] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT);
+	context_icn.hibw1_esram_in1_arb[2] =
+		readl(context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT);
+
+	context_icn.hibw1_ddr_in_prio[0] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_0_PRIORITY);
+	context_icn.hibw1_ddr_in_prio[1] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_1_PRIORITY);
+	context_icn.hibw1_ddr_in_prio[2] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_2_PRIORITY);
+
+	context_icn.hibw1_ddr_in_limit[0] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_0_LIMIT);
+	context_icn.hibw1_ddr_in_limit[1] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_1_LIMIT);
+	context_icn.hibw1_ddr_in_limit[2] =
+		readl(context_icn.base + NODE_HIBW1_DDR_IN_2_LIMIT);
+
+	context_icn.hibw1_ddr_out_prio_reg =
+		readl(context_icn.base + NODE_HIBW1_DDR_OUT_0_PRIORITY);
+
+	/* hibw2 */
+	context_icn.hibw2_esram_in_pri[0] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_0_PRIORITY);
+	context_icn.hibw2_esram_in_pri[1] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_1_PRIORITY);
+
+	context_icn.hibw2_esram_in0_arblimit[0] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT);
+	context_icn.hibw2_esram_in0_arblimit[1] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT);
+	context_icn.hibw2_esram_in0_arblimit[2] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT);
+
+	context_icn.hibw2_esram_in1_arblimit[0] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT);
+	context_icn.hibw2_esram_in1_arblimit[1] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT);
+	context_icn.hibw2_esram_in1_arblimit[2] =
+		readl(context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT);
+
+	context_icn.hibw2_ddr_in_prio[0] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_0_PRIORITY);
+	context_icn.hibw2_ddr_in_prio[1] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_1_PRIORITY);
+	context_icn.hibw2_ddr_in_prio[2] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_2_PRIORITY);
+	context_icn.hibw2_ddr_in_prio[3] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_3_PRIORITY);
+
+	context_icn.hibw2_ddr_in_limit[0] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_0_LIMIT);
+	context_icn.hibw2_ddr_in_limit[1] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_1_LIMIT);
+	context_icn.hibw2_ddr_in_limit[2] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_2_LIMIT);
+	context_icn.hibw2_ddr_in_limit[3] =
+		readl(context_icn.base + NODE_HIBW2_DDR_IN_3_LIMIT);
+
+	context_icn.hibw2_ddr_out_prio_reg =
+		readl(context_icn.base + NODE_HIBW2_DDR_OUT_0_PRIORITY);
+
+	/* ESRAM0 */
+	context_icn.esram_in_prio[0] =
+		readl(context_icn.base + NODE_ESRAM0_IN_0_PRIORITY);
+	context_icn.esram_in_prio[1] =
+		readl(context_icn.base + NODE_ESRAM0_IN_1_PRIORITY);
+	context_icn.esram_in_prio[2] =
+		readl(context_icn.base + NODE_ESRAM0_IN_2_PRIORITY);
+
+	context_icn.esram_in_lim[0] =
+		readl(context_icn.base + NODE_ESRAM0_IN_0_LIMIT);
+	context_icn.esram_in_lim[1] =
+		readl(context_icn.base + NODE_ESRAM0_IN_1_LIMIT);
+	context_icn.esram_in_lim[2] =
+		readl(context_icn.base + NODE_ESRAM0_IN_2_LIMIT);
+
+	context_icn.esram_out_prio_reg =
+		readl(context_icn.base + NODE_ESRAM0_OUT_0_PRIORITY);
+
+	/* ESRAM1-2 */
+	context_icn.esram12_in_prio[0] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_0_PRIORITY);
+	context_icn.esram12_in_prio[1] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_1_PRIORITY);
+	context_icn.esram12_in_prio[2] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_2_PRIORITY);
+
+	context_icn.esram12_in_arb_lim[0] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_0_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[1] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_1_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[2] =
+		readl(context_icn.base + NODE_ESRAM1_2_IN_2_ARB_1_LIMIT);
+
+	context_icn.esram12_out_prio_reg =
+		readl(context_icn.base + NODE_ESRAM1_2_OUT_0_PRIORITY);
+
+	/* ESRAM3-4 */
+	context_icn.esram34_in_prio[0] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_0_PRIORITY);
+	context_icn.esram34_in_prio[1] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_1_PRIORITY);
+	context_icn.esram34_in_prio[2] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_2_PRIORITY);
+
+	context_icn.esram34_in_arb_lim[0] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_0_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[1] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_1_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[2] =
+		readl(context_icn.base + NODE_ESRAM3_4_IN_2_ARB_1_LIMIT);
+
+	context_icn.esram34_out_prio =
+		readl(context_icn.base + NODE_ESRAM3_4_OUT_0_PRIORITY);
+}
+
+/*
+ * Restore ICN configuration registers
+ */
+void u5500_context_restore_icn(void)
+{
+
+	/* hibw1 */
+	writel(context_icn.hibw1_esram_in_pri[0],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_0_PRIORITY);
+	writel(context_icn.hibw1_esram_in_pri[1],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_1_PRIORITY);
+
+	writel(context_icn.hibw1_esram_in0_arb[0],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT);
+	writel(context_icn.hibw1_esram_in0_arb[1],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT);
+	writel(context_icn.hibw1_esram_in0_arb[2],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT);
+
+	writel(context_icn.hibw1_esram_in1_arb[0],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT);
+	writel(context_icn.hibw1_esram_in1_arb[1],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT);
+	writel(context_icn.hibw1_esram_in1_arb[2],
+	       context_icn.base + NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT);
+
+	writel(context_icn.hibw1_ddr_in_prio[0],
+	       context_icn.base + NODE_HIBW1_DDR_IN_0_PRIORITY);
+	writel(context_icn.hibw1_ddr_in_prio[1],
+	       context_icn.base + NODE_HIBW1_DDR_IN_1_PRIORITY);
+	writel(context_icn.hibw1_ddr_in_prio[2],
+	       context_icn.base + NODE_HIBW1_DDR_IN_2_PRIORITY);
+
+	writel(context_icn.hibw1_ddr_in_limit[0],
+	       context_icn.base + NODE_HIBW1_DDR_IN_0_LIMIT);
+	writel(context_icn.hibw1_ddr_in_limit[1],
+	       context_icn.base + NODE_HIBW1_DDR_IN_1_LIMIT);
+	writel(context_icn.hibw1_ddr_in_limit[2],
+	       context_icn.base + NODE_HIBW1_DDR_IN_2_LIMIT);
+
+	writel(context_icn.hibw1_ddr_out_prio_reg,
+	       context_icn.base + NODE_HIBW1_DDR_OUT_0_PRIORITY);
+
+	/* hibw2 */
+	writel(context_icn.hibw2_esram_in_pri[0],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_0_PRIORITY);
+	writel(context_icn.hibw2_esram_in_pri[1],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_1_PRIORITY);
+
+	writel(context_icn.hibw2_esram_in0_arblimit[0],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT);
+	writel(context_icn.hibw2_esram_in0_arblimit[1],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT);
+	writel(context_icn.hibw2_esram_in0_arblimit[2],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT);
+
+	writel(context_icn.hibw2_esram_in1_arblimit[0],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT);
+	writel(context_icn.hibw2_esram_in1_arblimit[1],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT);
+	writel(context_icn.hibw2_esram_in1_arblimit[2],
+	       context_icn.base + NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT);
+
+	writel(context_icn.hibw2_ddr_in_prio[0],
+	       context_icn.base + NODE_HIBW2_DDR_IN_0_PRIORITY);
+	writel(context_icn.hibw2_ddr_in_prio[1],
+	       context_icn.base + NODE_HIBW2_DDR_IN_1_PRIORITY);
+	writel(context_icn.hibw2_ddr_in_prio[2],
+	       context_icn.base + NODE_HIBW2_DDR_IN_2_PRIORITY);
+	writel(context_icn.hibw2_ddr_in_prio[3],
+	       context_icn.base + NODE_HIBW2_DDR_IN_3_PRIORITY);
+
+	writel(context_icn.hibw2_ddr_in_limit[0],
+	       context_icn.base + NODE_HIBW2_DDR_IN_0_LIMIT);
+	writel(context_icn.hibw2_ddr_in_limit[1],
+	       context_icn.base + NODE_HIBW2_DDR_IN_1_LIMIT);
+	writel(context_icn.hibw2_ddr_in_limit[2],
+	       context_icn.base + NODE_HIBW2_DDR_IN_2_LIMIT);
+	writel(context_icn.hibw2_ddr_in_limit[3],
+	       context_icn.base + NODE_HIBW2_DDR_IN_3_LIMIT);
+
+	writel(context_icn.hibw2_ddr_out_prio_reg,
+	       context_icn.base + NODE_HIBW2_DDR_OUT_0_PRIORITY);
+
+	/* ESRAM0 */
+	writel(context_icn.esram_in_prio[0],
+	       context_icn.base + NODE_ESRAM0_IN_0_PRIORITY);
+	writel(context_icn.esram_in_prio[1],
+	       context_icn.base + NODE_ESRAM0_IN_1_PRIORITY);
+	writel(context_icn.esram_in_prio[2],
+	       context_icn.base + NODE_ESRAM0_IN_2_PRIORITY);
+
+	writel(context_icn.esram_in_lim[0],
+	       context_icn.base + NODE_ESRAM0_IN_0_LIMIT);
+	writel(context_icn.esram_in_lim[1],
+	       context_icn.base + NODE_ESRAM0_IN_1_LIMIT);
+	writel(context_icn.esram_in_lim[2],
+	       context_icn.base + NODE_ESRAM0_IN_2_LIMIT);
+
+	writel(context_icn.esram_out_prio_reg,
+	       context_icn.base + NODE_ESRAM0_OUT_0_PRIORITY);
+
+	/* ESRAM1-2 */
+	writel(context_icn.esram12_in_prio[0],
+	       context_icn.base + NODE_ESRAM1_2_IN_0_PRIORITY);
+	writel(context_icn.esram12_in_prio[1],
+	       context_icn.base + NODE_ESRAM1_2_IN_1_PRIORITY);
+	writel(context_icn.esram12_in_prio[2],
+	       context_icn.base + NODE_ESRAM1_2_IN_2_PRIORITY);
+
+	writel(context_icn.esram12_in_arb_lim[0],
+	       context_icn.base + NODE_ESRAM1_2_IN_0_ARB_1_LIMIT);
+	writel(context_icn.esram12_in_arb_lim[1],
+	       context_icn.base + NODE_ESRAM1_2_IN_1_ARB_1_LIMIT);
+	writel(context_icn.esram12_in_arb_lim[2],
+	       context_icn.base + NODE_ESRAM1_2_IN_2_ARB_1_LIMIT);
+
+	writel(context_icn.esram12_out_prio_reg,
+	       context_icn.base + NODE_ESRAM1_2_OUT_0_PRIORITY);
+
+	/* ESRAM3-4 */
+	writel(context_icn.esram34_in_prio[0],
+	       context_icn.base + NODE_ESRAM3_4_IN_0_PRIORITY);
+	writel(context_icn.esram34_in_prio[1],
+	       context_icn.base + NODE_ESRAM3_4_IN_1_PRIORITY);
+	writel(context_icn.esram34_in_prio[2],
+	       context_icn.base + NODE_ESRAM3_4_IN_2_PRIORITY);
+
+	writel(context_icn.esram34_in_arb_lim[0],
+	       context_icn.base + NODE_ESRAM3_4_IN_0_ARB_1_LIMIT);
+	writel(context_icn.esram34_in_arb_lim[1],
+	       context_icn.base + NODE_ESRAM3_4_IN_1_ARB_1_LIMIT);
+	writel(context_icn.esram34_in_arb_lim[2],
+	       context_icn.base + NODE_ESRAM3_4_IN_2_ARB_1_LIMIT);
+
+	writel(context_icn.esram34_out_prio,
+	       context_icn.base + NODE_ESRAM3_4_OUT_0_PRIORITY);
+
+}
+
+void u5500_context_init(void)
+{
+	context_icn.base = ioremap(U5500_ICN_BASE, SZ_8K);
+}
diff --git a/arch/arm/mach-ux500/pm/context-db8500.c b/arch/arm/mach-ux500/pm/context-db8500.c
new file mode 100644
index 00000000000..ad21163917d
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/context-db8500.c
@@ -0,0 +1,457 @@
+/*
+ * Copyright (C) STMicroelectronics 2009
+ * Copyright (C) ST-Ericsson SA 2010-2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Sundar Iyer for ST-Ericsson
+ *
+ */
+
+#include <linux/io.h>
+
+#include <mach/hardware.h>
+
+#include "context.h"
+
+/*
+ * ST-Interconnect context
+ */
+
+/* priority, bw limiter register offsets */
+#define NODE_HIBW1_ESRAM_IN_0_PRIORITY		0x00
+#define NODE_HIBW1_ESRAM_IN_1_PRIORITY		0x04
+#define NODE_HIBW1_ESRAM_IN_2_PRIORITY		0x08
+#define NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT	0x24
+#define NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT	0x28
+#define NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT	0x2C
+#define NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT	0x30
+#define NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT	0x34
+#define NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT	0x38
+#define NODE_HIBW1_ESRAM_IN_2_ARB_1_LIMIT	0x3C
+#define NODE_HIBW1_ESRAM_IN_2_ARB_2_LIMIT	0x40
+#define NODE_HIBW1_ESRAM_IN_2_ARB_3_LIMIT	0x44
+#define NODE_HIBW1_DDR_IN_0_PRIORITY		0x400
+#define NODE_HIBW1_DDR_IN_1_PRIORITY		0x404
+#define NODE_HIBW1_DDR_IN_2_PRIORITY		0x408
+#define NODE_HIBW1_DDR_IN_0_LIMIT		0x424
+#define NODE_HIBW1_DDR_IN_1_LIMIT		0x428
+#define NODE_HIBW1_DDR_IN_2_LIMIT		0x42C
+#define NODE_HIBW1_DDR_OUT_0_PRIORITY		0x430
+#define NODE_HIBW2_ESRAM_IN_0_PRIORITY		0x800
+#define NODE_HIBW2_ESRAM_IN_1_PRIORITY		0x804
+#define NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT	0x818
+#define NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT	0x81C
+#define NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT	0x820
+#define NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT	0x824
+#define NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT	0x828
+#define NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT	0x82C
+#define NODE_HIBW2_DDR_IN_0_PRIORITY		0xC00
+#define NODE_HIBW2_DDR_IN_1_PRIORITY		0xC04
+#define NODE_HIBW2_DDR_IN_2_PRIORITY		0xC08
+
+#define NODE_HIBW2_DDR_IN_0_LIMIT		0xC24
+#define NODE_HIBW2_DDR_IN_1_LIMIT		0xC28
+#define NODE_HIBW2_DDR_IN_2_LIMIT		0xC2C
+#define NODE_HIBW2_DDR_OUT_0_PRIORITY		0xC30
+
+/*
+ * Note the following addresses are presented in
+ * db8500 design spec v3.1 and v3.3, table 10.
+ * But their addresses are not the same as in the
+ * description. The addresses in the description
+ * of each registers are correct.
+ * NODE_HIBW2_DDR_IN_3_LIMIT is only present in v1.
+ *
+ * Faulty registers addresses in table 10:
+ * NODE_HIBW2_DDR_IN_2_LIMIT 0xC38
+ * NODE_HIBW2_DDR_IN_3_LIMIT 0xC3C
+ * NODE_HIBW2_DDR_OUT_0_PRIORITY 0xC40
+ */
+
+#define NODE_ESRAM0_IN_0_PRIORITY		0x1000
+#define NODE_ESRAM0_IN_1_PRIORITY		0x1004
+#define NODE_ESRAM0_IN_2_PRIORITY		0x1008
+#define NODE_ESRAM0_IN_3_PRIORITY		0x100C
+#define NODE_ESRAM0_IN_0_LIMIT			0x1030
+#define NODE_ESRAM0_IN_1_LIMIT			0x1034
+#define NODE_ESRAM0_IN_2_LIMIT			0x1038
+#define NODE_ESRAM0_IN_3_LIMIT			0x103C
+/* common */
+#define NODE_ESRAM1_2_IN_0_PRIORITY		0x1400
+#define NODE_ESRAM1_2_IN_1_PRIORITY		0x1404
+#define NODE_ESRAM1_2_IN_2_PRIORITY		0x1408
+#define NODE_ESRAM1_2_IN_3_PRIORITY		0x140C
+#define NODE_ESRAM1_2_IN_0_ARB_1_LIMIT		0x1430
+#define NODE_ESRAM1_2_IN_0_ARB_2_LIMIT		0x1434
+#define NODE_ESRAM1_2_IN_1_ARB_1_LIMIT		0x1438
+#define NODE_ESRAM1_2_IN_1_ARB_2_LIMIT		0x143C
+#define NODE_ESRAM1_2_IN_2_ARB_1_LIMIT		0x1440
+#define NODE_ESRAM1_2_IN_2_ARB_2_LIMIT		0x1444
+#define NODE_ESRAM1_2_IN_3_ARB_1_LIMIT		0x1448
+#define NODE_ESRAM1_2_IN_3_ARB_2_LIMIT		0x144C
+
+#define NODE_ESRAM3_4_IN_0_PRIORITY		0x1800
+#define NODE_ESRAM3_4_IN_1_PRIORITY		0x1804
+#define NODE_ESRAM3_4_IN_2_PRIORITY		0x1808
+#define NODE_ESRAM3_4_IN_3_PRIORITY		0x180C
+#define NODE_ESRAM3_4_IN_0_ARB_1_LIMIT		0x1830
+#define NODE_ESRAM3_4_IN_0_ARB_2_LIMIT		0x1834
+#define NODE_ESRAM3_4_IN_1_ARB_1_LIMIT		0x1838
+#define NODE_ESRAM3_4_IN_1_ARB_2_LIMIT		0x183C
+#define NODE_ESRAM3_4_IN_2_ARB_1_LIMIT		0x1840
+#define NODE_ESRAM3_4_IN_2_ARB_2_LIMIT		0x1844
+#define NODE_ESRAM3_4_IN_3_ARB_1_LIMIT		0x1848
+#define NODE_ESRAM3_4_IN_3_ARB_2_LIMIT		0x184C
+
+static struct {
+	void __iomem *base;
+	u32 hibw1_esram_in_pri[3];
+	u32 hibw1_esram_in0_arb[3];
+	u32 hibw1_esram_in1_arb[3];
+	u32 hibw1_esram_in2_arb[3];
+	u32 hibw1_ddr_in_prio[3];
+	u32 hibw1_ddr_in_limit[3];
+	u32 hibw1_ddr_out_prio;
+
+	/* HiBw2 node registers */
+	u32 hibw2_esram_in_pri[2];
+	u32 hibw2_esram_in0_arblimit[3];
+	u32 hibw2_esram_in1_arblimit[3];
+	u32 hibw2_ddr_in_prio[4];
+	u32 hibw2_ddr_in_limit[4];
+	u32 hibw2_ddr_out_prio;
+
+	/* ESRAM node registers */
+	u32 esram_in_prio[4];
+	u32 esram_in_lim[4];
+	u32 esram0_in_prio[4];
+	u32 esram0_in_lim[4];
+	u32 esram12_in_prio[4];
+	u32 esram12_in_arb_lim[8];
+	u32 esram34_in_prio[4];
+	u32 esram34_in_arb_lim[8];
+} context_icn;
+
+/**
+ * u8500_context_save_icn() - save ICN context
+ *
+ */
+void u8500_context_save_icn(void)
+{
+	void __iomem *b = context_icn.base;
+
+	context_icn.hibw1_esram_in_pri[0] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_0_PRIORITY);
+	context_icn.hibw1_esram_in_pri[1] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_1_PRIORITY);
+	context_icn.hibw1_esram_in_pri[2] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_2_PRIORITY);
+
+	context_icn.hibw1_esram_in0_arb[0] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT);
+	context_icn.hibw1_esram_in0_arb[1] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT);
+	context_icn.hibw1_esram_in0_arb[2] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT);
+
+	context_icn.hibw1_esram_in1_arb[0] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT);
+	context_icn.hibw1_esram_in1_arb[1] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT);
+	context_icn.hibw1_esram_in1_arb[2] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT);
+
+	context_icn.hibw1_esram_in2_arb[0] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_2_ARB_1_LIMIT);
+	context_icn.hibw1_esram_in2_arb[1] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_2_ARB_2_LIMIT);
+	context_icn.hibw1_esram_in2_arb[2] =
+		readl_relaxed(b + NODE_HIBW1_ESRAM_IN_2_ARB_3_LIMIT);
+
+	context_icn.hibw1_ddr_in_prio[0] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_0_PRIORITY);
+	context_icn.hibw1_ddr_in_prio[1] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_1_PRIORITY);
+	context_icn.hibw1_ddr_in_prio[2] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_2_PRIORITY);
+
+	context_icn.hibw1_ddr_in_limit[0] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_0_LIMIT);
+	context_icn.hibw1_ddr_in_limit[1] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_1_LIMIT);
+	context_icn.hibw1_ddr_in_limit[2] =
+		readl_relaxed(b + NODE_HIBW1_DDR_IN_2_LIMIT);
+
+	context_icn.hibw1_ddr_out_prio =
+		readl_relaxed(b + NODE_HIBW1_DDR_OUT_0_PRIORITY);
+
+	context_icn.hibw2_esram_in_pri[0] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_0_PRIORITY);
+	context_icn.hibw2_esram_in_pri[1] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_1_PRIORITY);
+
+	context_icn.hibw2_esram_in0_arblimit[0] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT);
+	context_icn.hibw2_esram_in0_arblimit[1] =
+		 readl_relaxed(b + NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT);
+	context_icn.hibw2_esram_in0_arblimit[2] =
+		 readl_relaxed(b + NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT);
+
+	context_icn.hibw2_esram_in1_arblimit[0] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT);
+	context_icn.hibw2_esram_in1_arblimit[1] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT);
+	context_icn.hibw2_esram_in1_arblimit[2] =
+		readl_relaxed(b + NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT);
+
+	context_icn.hibw2_ddr_in_prio[0] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_0_PRIORITY);
+	context_icn.hibw2_ddr_in_prio[1] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_1_PRIORITY);
+	context_icn.hibw2_ddr_in_prio[2] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_2_PRIORITY);
+
+	context_icn.hibw2_ddr_in_limit[0] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_0_LIMIT);
+	context_icn.hibw2_ddr_in_limit[1] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_1_LIMIT);
+
+	context_icn.hibw2_ddr_in_limit[2] =
+		readl_relaxed(b + NODE_HIBW2_DDR_IN_2_LIMIT);
+
+	context_icn.hibw2_ddr_out_prio =
+		readl_relaxed(b + NODE_HIBW2_DDR_OUT_0_PRIORITY);
+
+	context_icn.esram0_in_prio[0] =
+		readl_relaxed(b + NODE_ESRAM0_IN_0_PRIORITY);
+	context_icn.esram0_in_prio[1] =
+		readl_relaxed(b + NODE_ESRAM0_IN_1_PRIORITY);
+	context_icn.esram0_in_prio[2] =
+		readl_relaxed(b + NODE_ESRAM0_IN_2_PRIORITY);
+	context_icn.esram0_in_prio[3] =
+		readl_relaxed(b + NODE_ESRAM0_IN_3_PRIORITY);
+
+	context_icn.esram0_in_lim[0] =
+		readl_relaxed(b + NODE_ESRAM0_IN_0_LIMIT);
+	context_icn.esram0_in_lim[1] =
+		readl_relaxed(b + NODE_ESRAM0_IN_1_LIMIT);
+	context_icn.esram0_in_lim[2] =
+		readl_relaxed(b + NODE_ESRAM0_IN_2_LIMIT);
+	context_icn.esram0_in_lim[3] =
+		readl_relaxed(b + NODE_ESRAM0_IN_3_LIMIT);
+
+	context_icn.esram12_in_prio[0] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_0_PRIORITY);
+	context_icn.esram12_in_prio[1] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_1_PRIORITY);
+	context_icn.esram12_in_prio[2] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_2_PRIORITY);
+	context_icn.esram12_in_prio[3] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_3_PRIORITY);
+
+	context_icn.esram12_in_arb_lim[0] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_0_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[1] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_0_ARB_2_LIMIT);
+	context_icn.esram12_in_arb_lim[2] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_1_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[3] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_1_ARB_2_LIMIT);
+	context_icn.esram12_in_arb_lim[4] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_2_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[5] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_2_ARB_2_LIMIT);
+	context_icn.esram12_in_arb_lim[6] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_3_ARB_1_LIMIT);
+	context_icn.esram12_in_arb_lim[7] =
+		readl_relaxed(b + NODE_ESRAM1_2_IN_3_ARB_2_LIMIT);
+
+	context_icn.esram34_in_prio[0] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_0_PRIORITY);
+	context_icn.esram34_in_prio[1] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_1_PRIORITY);
+	context_icn.esram34_in_prio[2] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_2_PRIORITY);
+	context_icn.esram34_in_prio[3] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_3_PRIORITY);
+
+	context_icn.esram34_in_arb_lim[0] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_0_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[1] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_0_ARB_2_LIMIT);
+	context_icn.esram34_in_arb_lim[2] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_1_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[3] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_1_ARB_2_LIMIT);
+	context_icn.esram34_in_arb_lim[4] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_2_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[5] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_2_ARB_2_LIMIT);
+	context_icn.esram34_in_arb_lim[6] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_3_ARB_1_LIMIT);
+	context_icn.esram34_in_arb_lim[7] =
+		readl_relaxed(b + NODE_ESRAM3_4_IN_3_ARB_2_LIMIT);
+}
+
+/**
+ * u8500_context_restore_icn() - restore ICN context
+ *
+ */
+void u8500_context_restore_icn(void)
+{
+	void __iomem *b = context_icn.base;
+
+	writel_relaxed(context_icn.hibw1_esram_in_pri[0],
+		       b + NODE_HIBW1_ESRAM_IN_0_PRIORITY);
+	writel_relaxed(context_icn.hibw1_esram_in_pri[1],
+		       b + NODE_HIBW1_ESRAM_IN_1_PRIORITY);
+	writel_relaxed(context_icn.hibw1_esram_in_pri[2],
+		       b + NODE_HIBW1_ESRAM_IN_2_PRIORITY);
+
+	writel_relaxed(context_icn.hibw1_esram_in0_arb[0],
+		       b + NODE_HIBW1_ESRAM_IN_0_ARB_1_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in0_arb[1],
+		       b + NODE_HIBW1_ESRAM_IN_0_ARB_2_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in0_arb[2],
+		       b + NODE_HIBW1_ESRAM_IN_0_ARB_3_LIMIT);
+
+	writel_relaxed(context_icn.hibw1_esram_in1_arb[0],
+		       b + NODE_HIBW1_ESRAM_IN_1_ARB_1_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in1_arb[1],
+		       b + NODE_HIBW1_ESRAM_IN_1_ARB_2_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in1_arb[2],
+		       b + NODE_HIBW1_ESRAM_IN_1_ARB_3_LIMIT);
+
+	writel_relaxed(context_icn.hibw1_esram_in2_arb[0],
+		       b + NODE_HIBW1_ESRAM_IN_2_ARB_1_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in2_arb[1],
+		       b + NODE_HIBW1_ESRAM_IN_2_ARB_2_LIMIT);
+	writel_relaxed(context_icn.hibw1_esram_in2_arb[2],
+		       b + NODE_HIBW1_ESRAM_IN_2_ARB_3_LIMIT);
+
+	writel_relaxed(context_icn.hibw1_ddr_in_prio[0],
+		       b + NODE_HIBW1_DDR_IN_0_PRIORITY);
+	writel_relaxed(context_icn.hibw1_ddr_in_prio[1],
+		       b + NODE_HIBW1_DDR_IN_1_PRIORITY);
+	writel_relaxed(context_icn.hibw1_ddr_in_prio[2],
+		       b + NODE_HIBW1_DDR_IN_2_PRIORITY);
+
+	writel_relaxed(context_icn.hibw1_ddr_in_limit[0],
+		       b + NODE_HIBW1_DDR_IN_0_LIMIT);
+	writel_relaxed(context_icn.hibw1_ddr_in_limit[1],
+		       b + NODE_HIBW1_DDR_IN_1_LIMIT);
+	writel_relaxed(context_icn.hibw1_ddr_in_limit[2],
+		       b + NODE_HIBW1_DDR_IN_2_LIMIT);
+
+	writel_relaxed(context_icn.hibw1_ddr_out_prio,
+		       b + NODE_HIBW1_DDR_OUT_0_PRIORITY);
+
+	writel_relaxed(context_icn.hibw2_esram_in_pri[0],
+		       b + NODE_HIBW2_ESRAM_IN_0_PRIORITY);
+	writel_relaxed(context_icn.hibw2_esram_in_pri[1],
+		       b + NODE_HIBW2_ESRAM_IN_1_PRIORITY);
+
+	writel_relaxed(context_icn.hibw2_esram_in0_arblimit[0],
+		       b + NODE_HIBW2_ESRAM_IN_0_ARB_1_LIMIT);
+	writel_relaxed(context_icn.hibw2_esram_in0_arblimit[1],
+		       b + NODE_HIBW2_ESRAM_IN_0_ARB_2_LIMIT);
+	writel_relaxed(context_icn.hibw2_esram_in0_arblimit[2],
+		       b + NODE_HIBW2_ESRAM_IN_0_ARB_3_LIMIT);
+
+	writel_relaxed(context_icn.hibw2_esram_in1_arblimit[0],
+		       b + NODE_HIBW2_ESRAM_IN_1_ARB_1_LIMIT);
+	writel_relaxed(context_icn.hibw2_esram_in1_arblimit[1],
+		       b + NODE_HIBW2_ESRAM_IN_1_ARB_2_LIMIT);
+	writel_relaxed(context_icn.hibw2_esram_in1_arblimit[2],
+		       b + NODE_HIBW2_ESRAM_IN_1_ARB_3_LIMIT);
+
+	writel_relaxed(context_icn.hibw2_ddr_in_prio[0],
+		       b + NODE_HIBW2_DDR_IN_0_PRIORITY);
+	writel_relaxed(context_icn.hibw2_ddr_in_prio[1],
+		       b + NODE_HIBW2_DDR_IN_1_PRIORITY);
+	writel_relaxed(context_icn.hibw2_ddr_in_prio[2],
+		       b + NODE_HIBW2_DDR_IN_2_PRIORITY);
+	writel_relaxed(context_icn.hibw2_ddr_in_limit[0],
+		       b + NODE_HIBW2_DDR_IN_0_LIMIT);
+	writel_relaxed(context_icn.hibw2_ddr_in_limit[1],
+		       b + NODE_HIBW2_DDR_IN_1_LIMIT);
+	writel_relaxed(context_icn.hibw2_ddr_in_limit[2],
+		       b + NODE_HIBW2_DDR_IN_2_LIMIT);
+	writel_relaxed(context_icn.hibw2_ddr_out_prio,
+		       b + NODE_HIBW2_DDR_OUT_0_PRIORITY);
+
+	writel_relaxed(context_icn.esram0_in_prio[0],
+		       b + NODE_ESRAM0_IN_0_PRIORITY);
+	writel_relaxed(context_icn.esram0_in_prio[1],
+		       b + NODE_ESRAM0_IN_1_PRIORITY);
+	writel_relaxed(context_icn.esram0_in_prio[2],
+		       b + NODE_ESRAM0_IN_2_PRIORITY);
+	writel_relaxed(context_icn.esram0_in_prio[3],
+		       b + NODE_ESRAM0_IN_3_PRIORITY);
+
+	writel_relaxed(context_icn.esram0_in_lim[0],
+		       b + NODE_ESRAM0_IN_0_LIMIT);
+	writel_relaxed(context_icn.esram0_in_lim[1],
+		       b + NODE_ESRAM0_IN_1_LIMIT);
+	writel_relaxed(context_icn.esram0_in_lim[2],
+		       b + NODE_ESRAM0_IN_2_LIMIT);
+	writel_relaxed(context_icn.esram0_in_lim[3],
+		       b + NODE_ESRAM0_IN_3_LIMIT);
+
+	writel_relaxed(context_icn.esram12_in_prio[0],
+		       b + NODE_ESRAM1_2_IN_0_PRIORITY);
+	writel_relaxed(context_icn.esram12_in_prio[1],
+		       b + NODE_ESRAM1_2_IN_1_PRIORITY);
+	writel_relaxed(context_icn.esram12_in_prio[2],
+		       b + NODE_ESRAM1_2_IN_2_PRIORITY);
+	writel_relaxed(context_icn.esram12_in_prio[3],
+		       b + NODE_ESRAM1_2_IN_3_PRIORITY);
+
+	writel_relaxed(context_icn.esram12_in_arb_lim[0],
+		       b + NODE_ESRAM1_2_IN_0_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[1],
+		       b + NODE_ESRAM1_2_IN_0_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[2],
+		       b + NODE_ESRAM1_2_IN_1_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[3],
+		       b + NODE_ESRAM1_2_IN_1_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[4],
+		       b + NODE_ESRAM1_2_IN_2_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[5],
+		       b + NODE_ESRAM1_2_IN_2_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[6],
+		       b + NODE_ESRAM1_2_IN_3_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram12_in_arb_lim[7],
+		       b + NODE_ESRAM1_2_IN_3_ARB_2_LIMIT);
+
+	writel_relaxed(context_icn.esram34_in_prio[0],
+		       b + NODE_ESRAM3_4_IN_0_PRIORITY);
+	writel_relaxed(context_icn.esram34_in_prio[1],
+		       b + NODE_ESRAM3_4_IN_1_PRIORITY);
+	writel_relaxed(context_icn.esram34_in_prio[2],
+		       b + NODE_ESRAM3_4_IN_2_PRIORITY);
+	writel_relaxed(context_icn.esram34_in_prio[3],
+		       b + NODE_ESRAM3_4_IN_3_PRIORITY);
+
+	writel_relaxed(context_icn.esram34_in_arb_lim[0],
+		       b + NODE_ESRAM3_4_IN_0_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[1],
+		       b + NODE_ESRAM3_4_IN_0_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[2],
+		       b + NODE_ESRAM3_4_IN_1_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[3],
+		       b + NODE_ESRAM3_4_IN_1_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[4],
+		       b + NODE_ESRAM3_4_IN_2_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[5],
+		       b + NODE_ESRAM3_4_IN_2_ARB_2_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[6],
+		       b + NODE_ESRAM3_4_IN_3_ARB_1_LIMIT);
+	writel_relaxed(context_icn.esram34_in_arb_lim[7],
+		       b + NODE_ESRAM3_4_IN_3_ARB_2_LIMIT);
+}
+
+void u8500_context_init(void)
+{
+	context_icn.base = ioremap(U8500_ICN_BASE, SZ_8K);
+}
diff --git a/arch/arm/mach-ux500/pm/context.c b/arch/arm/mach-ux500/pm/context.c
new file mode 100644
index 00000000000..a78f2d9dbb9
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/context.c
@@ -0,0 +1,945 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010-2011
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com>,
+ *         Rickard Andersson <rickard.andersson@stericsson.com>,
+ *         Jonas Aaberg <jonas.aberg@stericsson.com>,
+ *         Sundar Iyer for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/smp.h>
+#include <linux/percpu.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/notifier.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+
+#include <asm/hardware/gic.h>
+
+#include <mach/hardware.h>
+#include <mach/irqs.h>
+#include <mach/scu.h>
+
+#include <linux/gpio/nomadik.h>
+#include <linux/gpio.h>
+
+#include "context.h"
+#include "pm.h"
+
+#define GPIO_NUM_BANKS 9
+#define GPIO_NUM_SAVE_REGISTERS 7
+
+/*
+ * TODO:
+ * - Use the "UX500*"-macros instead where possible
+ */
+
+#define U8500_BACKUPRAM_SIZE SZ_64K
+
+#define U8500_PUBLIC_BOOT_ROM_BASE (U8500_BOOT_ROM_BASE + 0x17000)
+
+/* Special dedicated addresses in backup RAM */
+#define U8500_EXT_RAM_LOC_BACKUPRAM_ADDR		0x80151FDC
+#define U8500_CPU0_CP15_CR_BACKUPRAM_ADDR		0x80151F80
+#define U8500_CPU1_CP15_CR_BACKUPRAM_ADDR		0x80151FA0
+
+/* For v1.x */
+#define U8500_CPU0_BACKUPRAM_ADDR_BACKUPRAM_LOG_ADDR	0x80151FD8
+#define U8500_CPU1_BACKUPRAM_ADDR_BACKUPRAM_LOG_ADDR	0x80151FE0
+
+/* For v2.0 and later */
+#define U8500_CPU0_BACKUPRAM_ADDR_PUBLIC_BOOT_ROM_LOG_ADDR	0x80151FD8
+#define U8500_CPU1_BACKUPRAM_ADDR_PUBLIC_BOOT_ROM_LOG_ADDR	0x80151FE0
+
+#define GIC_DIST_ENABLE_NS 0x0
+
+/* 32 interrupts fits in 4 bytes */
+#define GIC_DIST_ENABLE_SET_COMMON_NUM ((DBX500_NR_INTERNAL_IRQS - \
+					 IRQ_SHPI_START) / 32)
+#define GIC_DIST_ENABLE_SET_CPU_NUM (IRQ_SHPI_START / 32)
+#define GIC_DIST_ENABLE_SET_SPI0 GIC_DIST_ENABLE_SET
+#define GIC_DIST_ENABLE_SET_SPI32 (GIC_DIST_ENABLE_SET + IRQ_SHPI_START / 8)
+
+#define GIC_DIST_PRI_COMMON_NUM ((DBX500_NR_INTERNAL_IRQS - IRQ_SHPI_START) / 4)
+#define GIC_DIST_PRI_CPU_NUM (IRQ_SHPI_START / 4)
+#define GIC_DIST_PRI_SPI0 GIC_DIST_PRI
+#define GIC_DIST_PRI_SPI32 (GIC_DIST_PRI + IRQ_SHPI_START)
+
+#define GIC_DIST_SPI_TARGET_COMMON_NUM ((DBX500_NR_INTERNAL_IRQS - \
+					 IRQ_SHPI_START) / 4)
+#define GIC_DIST_SPI_TARGET_CPU_NUM (IRQ_SHPI_START / 4)
+#define GIC_DIST_SPI_TARGET_SPI0 GIC_DIST_TARGET
+#define GIC_DIST_SPI_TARGET_SPI32 (GIC_DIST_TARGET + IRQ_SHPI_START)
+
+/* 16 interrupts per 4 bytes */
+#define GIC_DIST_CONFIG_COMMON_NUM ((DBX500_NR_INTERNAL_IRQS - IRQ_SHPI_START) \
+				    / 16)
+#define GIC_DIST_CONFIG_CPU_NUM (IRQ_SHPI_START / 16)
+#define GIC_DIST_CONFIG_SPI0 GIC_DIST_CONFIG
+#define GIC_DIST_CONFIG_SPI32 (GIC_DIST_CONFIG + IRQ_SHPI_START / 4)
+
+/* TODO! Move STM reg offsets to suitable place */
+#define STM_CR_OFFSET	0x00
+#define STM_MMC_OFFSET	0x08
+#define STM_TER_OFFSET	0x10
+
+#define U5500_PRCMU_DBG_PWRCTRL		(U5500_PRCMU_BASE + 0x4AC)
+#define PRCMU_DBG_PWRCTRL_A9DBGCLKEN	(1 << 4)
+
+#define TPIU_PORT_SIZE 0x4
+#define TPIU_TRIGGER_COUNTER 0x104
+#define TPIU_TRIGGER_MULTIPLIER 0x108
+#define TPIU_CURRENT_TEST_PATTERN 0x204
+#define TPIU_TEST_PATTERN_REPEAT 0x208
+#define TPIU_FORMATTER 0x304
+#define TPIU_FORMATTER_SYNC 0x308
+#define TPIU_LOCK_ACCESS_REGISTER 0xFB0
+
+#define TPIU_UNLOCK_CODE 0xc5acce55
+
+#define SCU_FILTER_STARTADDR	0x40
+#define SCU_FILTER_ENDADDR	0x44
+#define SCU_ACCESS_CTRL_SAC	0x50
+
+/*
+ * Periph clock cluster context
+ */
+#define PRCC_BCK_EN		0x00
+#define PRCC_KCK_EN		0x08
+#define PRCC_BCK_STATUS		0x10
+#define PRCC_KCK_STATUS		0x14
+
+/* The context of the Trace Port Interface Unit (TPIU) */
+static struct {
+	void __iomem *base;
+	u32 port_size;
+	u32 trigger_counter;
+	u32 trigger_multiplier;
+	u32 current_test_pattern;
+	u32 test_pattern_repeat;
+	u32 formatter;
+	u32 formatter_sync;
+} context_tpiu;
+
+static struct {
+	void __iomem *base;
+	u32 cr;
+	u32 mmc;
+	u32 ter;
+} context_stm_ape;
+
+struct context_gic_cpu {
+	void __iomem *base;
+	u32 ctrl;
+	u32 primask;
+	u32 binpoint;
+};
+static DEFINE_PER_CPU(struct context_gic_cpu, context_gic_cpu);
+
+static struct {
+	void __iomem *base;
+	u32 ns;
+	u32 enable_set[GIC_DIST_ENABLE_SET_COMMON_NUM]; /* IRQ 32 to 160 */
+	u32 priority_level[GIC_DIST_PRI_COMMON_NUM];
+	u32 spi_target[GIC_DIST_SPI_TARGET_COMMON_NUM];
+	u32 config[GIC_DIST_CONFIG_COMMON_NUM];
+} context_gic_dist_common;
+
+struct context_gic_dist_cpu {
+	void __iomem *base;
+	u32 enable_set[GIC_DIST_ENABLE_SET_CPU_NUM]; /* IRQ 0 to 31 */
+	u32 priority_level[GIC_DIST_PRI_CPU_NUM];
+	u32 spi_target[GIC_DIST_SPI_TARGET_CPU_NUM];
+	u32 config[GIC_DIST_CONFIG_CPU_NUM];
+};
+static DEFINE_PER_CPU(struct context_gic_dist_cpu, context_gic_dist_cpu);
+
+static struct {
+	void __iomem *base;
+	u32 ctrl;
+	u32 cpu_pwrstatus;
+	u32 inv_all_nonsecure;
+	u32 filter_start_addr;
+	u32 filter_end_addr;
+	u32 access_ctrl_sac;
+} context_scu;
+
+#define UX500_NR_PRCC_BANKS 5
+static struct {
+	void __iomem *base;
+	struct clk *clk;
+	u32 bus_clk;
+	u32 kern_clk;
+} context_prcc[UX500_NR_PRCC_BANKS];
+
+static u32 backup_sram_storage[NR_CPUS] = {
+	IO_ADDRESS(U8500_CPU0_CP15_CR_BACKUPRAM_ADDR),
+	IO_ADDRESS(U8500_CPU1_CP15_CR_BACKUPRAM_ADDR),
+};
+
+static u32 gpio_bankaddr[GPIO_NUM_BANKS] = {IO_ADDRESS(U8500_GPIOBANK0_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK1_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK2_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK3_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK4_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK5_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK6_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK7_BASE),
+					    IO_ADDRESS(U8500_GPIOBANK8_BASE)
+};
+
+static u32 gpio_save[GPIO_NUM_BANKS][GPIO_NUM_SAVE_REGISTERS];
+
+/*
+ * Stacks and stack pointers
+ */
+static DEFINE_PER_CPU(u32, varm_registers_backup_stack[1024]);
+static DEFINE_PER_CPU(u32 *, varm_registers_pointer);
+
+static DEFINE_PER_CPU(u32, varm_cp15_backup_stack[1024]);
+static DEFINE_PER_CPU(u32 *, varm_cp15_pointer);
+
+
+static ATOMIC_NOTIFIER_HEAD(context_ape_notifier_list);
+static ATOMIC_NOTIFIER_HEAD(context_arm_notifier_list);
+
+/*
+ * Register a simple callback for handling vape context save/restore
+ */
+int context_ape_notifier_register(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_register(&context_ape_notifier_list, nb);
+}
+EXPORT_SYMBOL(context_ape_notifier_register);
+
+/*
+ * Remove a previously registered callback
+ */
+int context_ape_notifier_unregister(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_unregister(&context_ape_notifier_list,
+						    nb);
+}
+EXPORT_SYMBOL(context_ape_notifier_unregister);
+
+/*
+ * Register a simple callback for handling varm context save/restore
+ */
+int context_arm_notifier_register(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_register(&context_arm_notifier_list, nb);
+}
+EXPORT_SYMBOL(context_arm_notifier_register);
+
+/*
+ * Remove a previously registered callback
+ */
+int context_arm_notifier_unregister(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_unregister(&context_arm_notifier_list,
+						    nb);
+}
+EXPORT_SYMBOL(context_arm_notifier_unregister);
+
+static void save_prcc(void)
+{
+	int i;
+
+	for (i = 0; i < UX500_NR_PRCC_BANKS; i++) {
+		clk_enable(context_prcc[i].clk);
+
+		context_prcc[i].bus_clk =
+			readl(context_prcc[i].base + PRCC_BCK_STATUS);
+		context_prcc[i].kern_clk =
+			readl(context_prcc[i].base + PRCC_KCK_STATUS);
+
+		clk_disable(context_prcc[i].clk);
+	}
+}
+
+static void restore_prcc(void)
+{
+	int i;
+
+	for (i = 0; i < UX500_NR_PRCC_BANKS; i++) {
+		clk_enable(context_prcc[i].clk);
+
+		writel(context_prcc[i].bus_clk,
+		       context_prcc[i].base + PRCC_BCK_EN);
+		writel(context_prcc[i].kern_clk,
+		       context_prcc[i].base + PRCC_KCK_EN);
+
+		clk_disable(context_prcc[i].clk);
+	}
+}
+
+static void save_stm_ape(void)
+{
+	/*
+	 * TODO: Check with PRCMU developers how STM is handled by PRCMU
+	 * firmware. According to DB5500 design spec there is a "flush"
+	 * mechanism supposed to be used by the PRCMU before power down,
+	 * PRCMU fw might save/restore the following three registers
+	 * at the same time.
+	 */
+	context_stm_ape.cr  = readl(context_stm_ape.base +
+				    STM_CR_OFFSET);
+	context_stm_ape.mmc = readl(context_stm_ape.base +
+				    STM_MMC_OFFSET);
+	context_stm_ape.ter = readl(context_stm_ape.base +
+				    STM_TER_OFFSET);
+}
+
+static void restore_stm_ape(void)
+{
+	writel(context_stm_ape.ter,
+	       context_stm_ape.base + STM_TER_OFFSET);
+	writel(context_stm_ape.mmc,
+	       context_stm_ape.base + STM_MMC_OFFSET);
+	writel(context_stm_ape.cr,
+	       context_stm_ape.base + STM_CR_OFFSET);
+}
+
+static bool tpiu_clocked(void)
+{
+	if (cpu_is_u5500()) {
+		return readl_relaxed(__io_address(U5500_PRCMU_DBG_PWRCTRL))
+			& PRCMU_DBG_PWRCTRL_A9DBGCLKEN;
+	}
+
+	return true;
+}
+
+/*
+ * Save the context of the Trace Port Interface Unit (TPIU).
+ * Saving/restoring is needed for the PTM tracing to work together
+ * with the sleep states ApSleep and ApDeepSleep.
+ */
+static void save_tpiu(void)
+{
+	if (!tpiu_clocked())
+		return;
+
+	context_tpiu.port_size		  = readl(context_tpiu.base +
+						  TPIU_PORT_SIZE);
+	context_tpiu.trigger_counter	  = readl(context_tpiu.base +
+						  TPIU_TRIGGER_COUNTER);
+	context_tpiu.trigger_multiplier   = readl(context_tpiu.base +
+						TPIU_TRIGGER_MULTIPLIER);
+	context_tpiu.current_test_pattern = readl(context_tpiu.base +
+						  TPIU_CURRENT_TEST_PATTERN);
+	context_tpiu.test_pattern_repeat  = readl(context_tpiu.base +
+						 TPIU_TEST_PATTERN_REPEAT);
+	context_tpiu.formatter		  = readl(context_tpiu.base +
+						  TPIU_FORMATTER);
+	context_tpiu.formatter_sync	  = readl(context_tpiu.base +
+						  TPIU_FORMATTER_SYNC);
+}
+
+/*
+ * Restore the context of the Trace Port Interface Unit (TPIU).
+ * Saving/restoring is needed for the PTM tracing to work together
+ * with the sleep states ApSleep and ApDeepSleep.
+ */
+static void restore_tpiu(void)
+{
+	if (!tpiu_clocked())
+		return;
+
+	writel(TPIU_UNLOCK_CODE,
+	       context_tpiu.base + TPIU_LOCK_ACCESS_REGISTER);
+
+	writel(context_tpiu.port_size,
+	       context_tpiu.base + TPIU_PORT_SIZE);
+	writel(context_tpiu.trigger_counter,
+	       context_tpiu.base + TPIU_TRIGGER_COUNTER);
+	writel(context_tpiu.trigger_multiplier,
+	       context_tpiu.base + TPIU_TRIGGER_MULTIPLIER);
+	writel(context_tpiu.current_test_pattern,
+	       context_tpiu.base + TPIU_CURRENT_TEST_PATTERN);
+	writel(context_tpiu.test_pattern_repeat,
+	       context_tpiu.base + TPIU_TEST_PATTERN_REPEAT);
+	writel(context_tpiu.formatter,
+	       context_tpiu.base + TPIU_FORMATTER);
+	writel(context_tpiu.formatter_sync,
+	       context_tpiu.base + TPIU_FORMATTER_SYNC);
+}
+
+/*
+ * Save GIC CPU IF registers
+ *
+ * This is per cpu so it needs to be called for each one.
+ */
+
+static void save_gic_if_cpu(struct context_gic_cpu *c_gic_cpu)
+{
+	c_gic_cpu->ctrl     = readl_relaxed(c_gic_cpu->base + GIC_CPU_CTRL);
+	c_gic_cpu->primask  = readl_relaxed(c_gic_cpu->base + GIC_CPU_PRIMASK);
+	c_gic_cpu->binpoint = readl_relaxed(c_gic_cpu->base + GIC_CPU_BINPOINT);
+}
+
+/*
+ * Restore GIC CPU IF registers
+ *
+ * This is per cpu so it needs to be called for each one.
+ */
+static void restore_gic_if_cpu(struct context_gic_cpu *c_gic_cpu)
+{
+	writel_relaxed(c_gic_cpu->ctrl,     c_gic_cpu->base + GIC_CPU_CTRL);
+	writel_relaxed(c_gic_cpu->primask,  c_gic_cpu->base + GIC_CPU_PRIMASK);
+	writel_relaxed(c_gic_cpu->binpoint, c_gic_cpu->base + GIC_CPU_BINPOINT);
+}
+
+/*
+ * Save GIC Distributor Common registers
+ *
+ * This context is common. Only one CPU needs to call.
+ *
+ * Save SPI (Shared Peripheral Interrupt) settings, IRQ 32-159.
+ */
+
+static void save_gic_dist_common(void)
+{
+	int i;
+
+	context_gic_dist_common.ns = readl_relaxed(context_gic_dist_common.base
+					   + GIC_DIST_ENABLE_NS);
+
+	for (i = 0; i < GIC_DIST_ENABLE_SET_COMMON_NUM; i++)
+		context_gic_dist_common.enable_set[i] =
+			readl_relaxed(context_gic_dist_common.base +
+			      GIC_DIST_ENABLE_SET_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_PRI_COMMON_NUM; i++)
+		context_gic_dist_common.priority_level[i] =
+			readl_relaxed(context_gic_dist_common.base +
+			      GIC_DIST_PRI_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_SPI_TARGET_COMMON_NUM; i++)
+		context_gic_dist_common.spi_target[i] =
+			readl_relaxed(context_gic_dist_common.base +
+			      GIC_DIST_SPI_TARGET_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_CONFIG_COMMON_NUM; i++)
+		context_gic_dist_common.config[i] =
+			readl_relaxed(context_gic_dist_common.base +
+			      GIC_DIST_CONFIG_SPI32 +  i * 4);
+}
+
+/*
+ * Restore GIC Distributor Common registers
+ *
+ * This context is common. Only one CPU needs to call.
+ *
+ * Save SPI (Shared Peripheral Interrupt) settings, IRQ 32-159.
+ */
+static void restore_gic_dist_common(void)
+{
+
+	int i;
+
+	for (i = 0; i < GIC_DIST_CONFIG_COMMON_NUM; i++)
+		writel_relaxed(context_gic_dist_common.config[i],
+		       context_gic_dist_common.base +
+		       GIC_DIST_CONFIG_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_SPI_TARGET_COMMON_NUM; i++)
+		writel_relaxed(context_gic_dist_common.spi_target[i],
+		       context_gic_dist_common.base +
+		       GIC_DIST_SPI_TARGET_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_PRI_COMMON_NUM; i++)
+		writel_relaxed(context_gic_dist_common.priority_level[i],
+			context_gic_dist_common.base +
+		       GIC_DIST_PRI_SPI32 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_ENABLE_SET_COMMON_NUM; i++)
+		writel_relaxed(context_gic_dist_common.enable_set[i],
+		       context_gic_dist_common.base +
+		       GIC_DIST_ENABLE_SET_SPI32 +  i * 4);
+
+	writel_relaxed(context_gic_dist_common.ns,
+	       context_gic_dist_common.base + GIC_DIST_ENABLE_NS);
+}
+
+
+
+/*
+ * Save GIC Dist CPU registers
+ *
+ * This needs to be called by all cpu:s which will not call
+ * save_gic_dist_common(). Only the registers of the GIC which are
+ * banked will be saved.
+ */
+static void save_gic_dist_cpu(struct context_gic_dist_cpu *c_gic)
+{
+	int i;
+
+	for (i = 0; i < GIC_DIST_ENABLE_SET_CPU_NUM; i++)
+		c_gic->enable_set[i] =
+			readl_relaxed(c_gic->base +
+			      GIC_DIST_ENABLE_SET_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_PRI_CPU_NUM; i++)
+		c_gic->priority_level[i] =
+			readl_relaxed(c_gic->base +
+			      GIC_DIST_PRI_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_SPI_TARGET_CPU_NUM; i++)
+		c_gic->spi_target[i] =
+			readl_relaxed(c_gic->base +
+			      GIC_DIST_SPI_TARGET_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_CONFIG_CPU_NUM; i++)
+		c_gic->config[i] =
+			readl_relaxed(c_gic->base +
+			      GIC_DIST_CONFIG_SPI0 +  i * 4);
+}
+
+/*
+ * Restore GIC Dist CPU registers
+ *
+ * This needs to be called by all cpu:s which will not call
+ * restore_gic_dist_common(). Only the registers of the GIC which are
+ * banked will be saved.
+ */
+static void restore_gic_dist_cpu(struct context_gic_dist_cpu *c_gic)
+{
+
+	int i;
+
+	for (i = 0; i < GIC_DIST_CONFIG_CPU_NUM; i++)
+		writel_relaxed(c_gic->config[i],
+		       c_gic->base +
+		       GIC_DIST_CONFIG_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_SPI_TARGET_CPU_NUM; i++)
+		writel_relaxed(c_gic->spi_target[i],
+		       c_gic->base +
+		       GIC_DIST_SPI_TARGET_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_PRI_CPU_NUM; i++)
+		writel_relaxed(c_gic->priority_level[i],
+			c_gic->base +
+		       GIC_DIST_PRI_SPI0 +  i * 4);
+
+	for (i = 0; i < GIC_DIST_ENABLE_SET_CPU_NUM; i++)
+		writel_relaxed(c_gic->enable_set[i],
+		       c_gic->base +
+		       GIC_DIST_ENABLE_SET_SPI0 +  i * 4);
+}
+static void save_scu(void)
+{
+	context_scu.ctrl =
+		readl_relaxed(context_scu.base + SCU_CTRL);
+	context_scu.cpu_pwrstatus =
+		readl_relaxed(context_scu.base + SCU_CPU_STATUS);
+	context_scu.inv_all_nonsecure =
+		readl_relaxed(context_scu.base + SCU_INVALIDATE);
+	context_scu.filter_start_addr =
+		readl_relaxed(context_scu.base + SCU_FILTER_STARTADDR);
+	context_scu.filter_end_addr	=
+		readl_relaxed(context_scu.base + SCU_FILTER_ENDADDR);
+	context_scu.access_ctrl_sac	=
+		readl_relaxed(context_scu.base + SCU_ACCESS_CTRL_SAC);
+}
+
+static void restore_scu(void)
+{
+	writel_relaxed(context_scu.ctrl,
+	       context_scu.base + SCU_CTRL);
+	writel_relaxed(context_scu.cpu_pwrstatus,
+	       context_scu.base + SCU_CPU_STATUS);
+	writel_relaxed(context_scu.inv_all_nonsecure,
+	       context_scu.base + SCU_INVALIDATE);
+	writel_relaxed(context_scu.filter_start_addr,
+	       context_scu.base + SCU_FILTER_STARTADDR);
+	writel_relaxed(context_scu.filter_end_addr,
+	       context_scu.base + SCU_FILTER_ENDADDR);
+	writel_relaxed(context_scu.access_ctrl_sac,
+	       context_scu.base + SCU_ACCESS_CTRL_SAC);
+}
+
+/*
+ * Save VAPE context
+ */
+void context_vape_save(void)
+{
+	atomic_notifier_call_chain(&context_ape_notifier_list,
+				   CONTEXT_APE_SAVE, NULL);
+
+	if (cpu_is_u5500())
+		u5500_context_save_icn();
+	if (cpu_is_u8500())
+		u8500_context_save_icn();
+
+	save_stm_ape();
+
+	save_tpiu();
+
+	save_prcc();
+
+}
+
+/*
+ * Restore VAPE context
+ */
+void context_vape_restore(void)
+{
+	restore_prcc();
+
+	restore_tpiu();
+
+	restore_stm_ape();
+
+	if (cpu_is_u5500())
+		u5500_context_restore_icn();
+	if (cpu_is_u8500())
+		u8500_context_restore_icn();
+
+	atomic_notifier_call_chain(&context_ape_notifier_list,
+				   CONTEXT_APE_RESTORE, NULL);
+}
+
+/*
+ * Save GPIO registers that might be modified
+ * for power save reasons.
+ */
+void context_gpio_save(void)
+{
+	int i;
+
+	for (i = 0; i < GPIO_NUM_BANKS; i++) {
+		gpio_save[i][0] = readl(gpio_bankaddr[i] + NMK_GPIO_AFSLA);
+		gpio_save[i][1] = readl(gpio_bankaddr[i] + NMK_GPIO_AFSLB);
+		gpio_save[i][2] = readl(gpio_bankaddr[i] + NMK_GPIO_PDIS);
+		gpio_save[i][3] = readl(gpio_bankaddr[i] + NMK_GPIO_DIR);
+		gpio_save[i][4] = readl(gpio_bankaddr[i] + NMK_GPIO_DAT);
+		gpio_save[i][6] = readl(gpio_bankaddr[i] + NMK_GPIO_SLPC);
+	}
+}
+
+/*
+ * Restore GPIO registers that might be modified
+ * for power save reasons.
+ */
+void context_gpio_restore(void)
+{
+	int i;
+	u32 output_state;
+	u32 pull_up;
+	u32 pull_down;
+	u32 pull;
+
+	for (i = 0; i < GPIO_NUM_BANKS; i++) {
+		writel(gpio_save[i][2], gpio_bankaddr[i] + NMK_GPIO_PDIS);
+
+		writel(gpio_save[i][3], gpio_bankaddr[i] + NMK_GPIO_DIR);
+
+		/* Set the high outputs. outpute_state = GPIO_DIR & GPIO_DAT */
+		output_state = gpio_save[i][3] & gpio_save[i][4];
+		writel(output_state, gpio_bankaddr[i] + NMK_GPIO_DATS);
+
+		/*
+		 * Set the low outputs.
+		 * outpute_state = ~(GPIO_DIR & GPIO_DAT) & GPIO_DIR
+		 */
+		output_state = ~(gpio_save[i][3] & gpio_save[i][4]) &
+			gpio_save[i][3];
+		writel(output_state, gpio_bankaddr[i] + NMK_GPIO_DATC);
+
+		/*
+		 * Restore pull up/down.
+		 * Only write pull up/down settings on inputs where
+		 * PDIS is not set.
+		 * pull = (~GPIO_DIR & ~GPIO_PDIS)
+		 */
+		pull = (~gpio_save[i][3] & ~gpio_save[i][2]);
+		nmk_gpio_read_pull(i, &pull_up);
+
+		pull_down = pull & ~pull_up;
+		pull_up = pull & pull_up;
+		/* Set pull ups */
+		writel(pull_up, gpio_bankaddr[i] + NMK_GPIO_DATS);
+		/* Set pull downs */
+		writel(pull_down, gpio_bankaddr[i] + NMK_GPIO_DATC);
+
+		writel(gpio_save[i][6], gpio_bankaddr[i] + NMK_GPIO_SLPC);
+
+	}
+
+}
+
+/*
+ * Restore GPIO mux registers that might be modified
+ * for power save reasons.
+ */
+void context_gpio_restore_mux(void)
+{
+	int i;
+
+	/* Change mux settings */
+	for (i = 0; i < GPIO_NUM_BANKS; i++) {
+		writel(gpio_save[i][0], gpio_bankaddr[i] + NMK_GPIO_AFSLA);
+		writel(gpio_save[i][1], gpio_bankaddr[i] + NMK_GPIO_AFSLB);
+	}
+}
+
+/*
+ * Safe sequence used to switch IOs between GPIO and Alternate-C mode:
+ *  - Save SLPM registers (Not done.)
+ *  - Set SLPM=0 for the IOs you want to switch. (We assume that all
+ *    SLPM registers already are 0 except for the ones that wants to
+ *    have the mux connected in sleep (e.g modem STM)).
+ *  - Configure the GPIO registers for the IOs that are being switched
+ *  - Set IOFORCE=1
+ *  - Modify the AFLSA/B registers for the IOs that are being switched
+ *  - Set IOFORCE=0
+ *  - Restore SLPM registers (Not done.)
+ *  - Any spurious wake up event during switch sequence to be ignored
+ *    and cleared
+ */
+void context_gpio_mux_safe_switch(bool begin)
+{
+	int i;
+
+	static u32 rwimsc[GPIO_NUM_BANKS];
+	static u32 fwimsc[GPIO_NUM_BANKS];
+
+	if (begin) {
+		for (i = 0; i < GPIO_NUM_BANKS; i++) {
+			/* Save registers */
+			rwimsc[i] = readl(gpio_bankaddr[i] + NMK_GPIO_RWIMSC);
+			fwimsc[i] = readl(gpio_bankaddr[i] + NMK_GPIO_FWIMSC);
+
+			/* Prevent spurious wakeups */
+			writel(0, gpio_bankaddr[i] + NMK_GPIO_RWIMSC);
+			writel(0, gpio_bankaddr[i] + NMK_GPIO_FWIMSC);
+		}
+
+		ux500_pm_prcmu_set_ioforce(true);
+	} else {
+		ux500_pm_prcmu_set_ioforce(false);
+
+		/* Restore wake up settings */
+		for (i = 0; i < GPIO_NUM_BANKS; i++) {
+			writel(rwimsc[i], gpio_bankaddr[i] + NMK_GPIO_RWIMSC);
+			writel(fwimsc[i], gpio_bankaddr[i] + NMK_GPIO_FWIMSC);
+		}
+
+	}
+
+}
+
+/*
+ * Save common
+ *
+ * This function must be called once for all cores before going to deep sleep.
+ */
+void context_varm_save_common(void)
+{
+	atomic_notifier_call_chain(&context_arm_notifier_list,
+				   CONTEXT_ARM_COMMON_SAVE, NULL);
+
+	/* Save common parts */
+	save_gic_dist_common();
+	save_scu();
+}
+
+/*
+ * Restore common
+ *
+ * This function must be called once for all cores when waking up from deep
+ * sleep.
+ */
+void context_varm_restore_common(void)
+{
+	/* Restore common parts */
+	restore_scu();
+	restore_gic_dist_common();
+
+	atomic_notifier_call_chain(&context_arm_notifier_list,
+				   CONTEXT_ARM_COMMON_RESTORE, NULL);
+}
+
+/*
+ * Save core
+ *
+ * This function must be called once for each cpu core before going to deep
+ * sleep.
+ */
+void context_varm_save_core(void)
+{
+
+	int cpu = smp_processor_id();
+
+	atomic_notifier_call_chain(&context_arm_notifier_list,
+				   CONTEXT_ARM_CORE_SAVE, NULL);
+
+	per_cpu(varm_cp15_pointer, cpu) = per_cpu(varm_cp15_backup_stack, cpu);
+
+	/* Save core */
+	save_gic_if_cpu(&per_cpu(context_gic_cpu, cpu));
+	save_gic_dist_cpu(&per_cpu(context_gic_dist_cpu, cpu));
+	context_save_cp15_registers(&per_cpu(varm_cp15_pointer, cpu));
+}
+
+/*
+ * Restore core
+ *
+ * This function must be called once for each cpu core when waking up from
+ * deep sleep.
+ */
+void context_varm_restore_core(void)
+{
+	int cpu = smp_processor_id();
+
+	/* Restore core */
+	context_restore_cp15_registers(&per_cpu(varm_cp15_pointer, cpu));
+	restore_gic_dist_cpu(&per_cpu(context_gic_dist_cpu, cpu));
+	restore_gic_if_cpu(&per_cpu(context_gic_cpu, cpu));
+
+	atomic_notifier_call_chain(&context_arm_notifier_list,
+				   CONTEXT_ARM_CORE_RESTORE, NULL);
+
+}
+
+/*
+ * Save CPU registers
+ *
+ * This function saves ARM registers.
+ */
+void context_save_cpu_registers(void)
+{
+	int cpu = smp_processor_id();
+
+	per_cpu(varm_registers_pointer, cpu) =
+		per_cpu(varm_registers_backup_stack, cpu);
+	context_save_arm_registers(&per_cpu(varm_registers_pointer, cpu));
+}
+
+/*
+ * Restore CPU registers
+ *
+ * This function restores ARM registers.
+ */
+void context_restore_cpu_registers(void)
+{
+	int cpu = smp_processor_id();
+
+	context_restore_arm_registers(&per_cpu(varm_registers_pointer, cpu));
+}
+
+/*
+ * This function stores CP15 registers related to cache and mmu
+ * in backup SRAM. It also stores stack pointer, CPSR
+ * and return address for the PC in backup SRAM and
+ * does wait for interrupt.
+ */
+void context_save_to_sram_and_wfi(bool cleanL1cache,
+				  bool cleanL2cache)
+{
+	int cpu = smp_processor_id();
+
+	if (cpu_is_u8500())
+		context_save_to_sram_and_wfi_internal(backup_sram_storage[cpu],
+						      cleanL1cache,
+						      cleanL2cache);
+	else if (cpu_is_u5500())
+		__asm__ __volatile__("wfi\n" : : : "memory");
+}
+
+static int __init context_init(void)
+{
+	int i;
+	void __iomem *ux500_backup_ptr;
+
+	/* allocate backup pointer for RAM data */
+	ux500_backup_ptr = (void *)__get_free_pages(GFP_KERNEL,
+				  get_order(U8500_BACKUPRAM_SIZE));
+
+	if (!ux500_backup_ptr) {
+		pr_warning("context: could not allocate backup memory\n");
+		return -ENOMEM;
+	}
+
+	/*
+	 * ROM code addresses to store backup contents,
+	 * pass the physical address of back up to ROM code
+	 */
+	writel(virt_to_phys(ux500_backup_ptr),
+	       IO_ADDRESS(U8500_EXT_RAM_LOC_BACKUPRAM_ADDR));
+
+
+	if (cpu_is_u5500()) {
+		context_tpiu.base = ioremap(U5500_TPIU_BASE, SZ_4K);
+		context_stm_ape.base = ioremap(U5500_STM_REG_BASE, SZ_4K);
+		context_scu.base = ioremap(U5500_SCU_BASE, SZ_4K);
+
+		context_prcc[0].base = ioremap(U5500_CLKRST1_BASE, SZ_4K);
+		context_prcc[1].base = ioremap(U5500_CLKRST2_BASE, SZ_4K);
+		context_prcc[2].base = ioremap(U5500_CLKRST3_BASE, SZ_4K);
+		context_prcc[3].base = ioremap(U5500_CLKRST5_BASE, SZ_4K);
+		context_prcc[4].base = ioremap(U5500_CLKRST6_BASE, SZ_4K);
+
+		context_gic_dist_common.base = ioremap(U5500_GIC_DIST_BASE, SZ_4K);
+		per_cpu(context_gic_cpu, 0).base = ioremap(U5500_GIC_CPU_BASE, SZ_4K);
+	} else if (cpu_is_u8500()) {
+		/* Give logical address to backup RAM. For both CPUs */
+		writel(IO_ADDRESS(U8500_PUBLIC_BOOT_ROM_BASE),
+		       IO_ADDRESS(U8500_CPU0_BACKUPRAM_ADDR_PUBLIC_BOOT_ROM_LOG_ADDR));
+
+		writel(IO_ADDRESS(U8500_PUBLIC_BOOT_ROM_BASE),
+		       IO_ADDRESS(U8500_CPU1_BACKUPRAM_ADDR_PUBLIC_BOOT_ROM_LOG_ADDR));
+
+		context_tpiu.base = ioremap(U8500_TPIU_BASE, SZ_4K);
+		context_stm_ape.base = ioremap(U8500_STM_REG_BASE, SZ_4K);
+		context_scu.base = ioremap(U8500_SCU_BASE, SZ_4K);
+
+		/* PERIPH4 is always on, so no need saving prcc */
+		context_prcc[0].base = ioremap(U8500_CLKRST1_BASE, SZ_4K);
+		context_prcc[1].base = ioremap(U8500_CLKRST2_BASE, SZ_4K);
+		context_prcc[2].base = ioremap(U8500_CLKRST3_BASE, SZ_4K);
+		context_prcc[3].base = ioremap(U8500_CLKRST5_BASE, SZ_4K);
+		context_prcc[4].base = ioremap(U8500_CLKRST6_BASE, SZ_4K);
+
+		context_gic_dist_common.base = ioremap(U8500_GIC_DIST_BASE, SZ_4K);
+		per_cpu(context_gic_cpu, 0).base = ioremap(U8500_GIC_CPU_BASE, SZ_4K);
+	}
+
+	per_cpu(context_gic_dist_cpu, 0).base = context_gic_dist_common.base;
+
+	for (i = 1; i < num_possible_cpus(); i++) {
+		per_cpu(context_gic_cpu, i).base
+			= per_cpu(context_gic_cpu, 0).base;
+		per_cpu(context_gic_dist_cpu, i).base
+			= per_cpu(context_gic_dist_cpu, 0).base;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(context_prcc); i++) {
+		const int clusters[] = {1, 2, 3, 5, 6};
+		char clkname[10];
+
+		snprintf(clkname, sizeof(clkname), "PERIPH%d", clusters[i]);
+
+		context_prcc[i].clk = clk_get_sys(clkname, NULL);
+		BUG_ON(IS_ERR(context_prcc[i].clk));
+	}
+
+	if (cpu_is_u8500()) {
+		u8500_context_init();
+	} else if (cpu_is_u5500()) {
+		u5500_context_init();
+	} else {
+		printk(KERN_ERR "context: unknown hardware!\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+subsys_initcall(context_init);
diff --git a/arch/arm/mach-ux500/pm/context.h b/arch/arm/mach-ux500/pm/context.h
new file mode 100644
index 00000000000..5b7522b4c0a
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/context.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com>
+ *         Rickard Andersson <rickard.andersson@stericsson.com> for
+ *         ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+#ifndef CONTEXT_H
+#define CONTEXT_H
+
+#include <linux/notifier.h>
+
+#ifdef CONFIG_UX500_CONTEXT
+
+/* Defines to be with
+ * context_ape_notifier_register
+ */
+#define CONTEXT_APE_SAVE 0 /* APE save */
+#define CONTEXT_APE_RESTORE 1 /* APE restore */
+
+/* Defines to be with
+ * context_arm_notifier_register
+ */
+#define CONTEXT_ARM_CORE_SAVE 0 /* Called for each ARM core */
+#define CONTEXT_ARM_CORE_RESTORE 1 /* Called for each ARM core */
+#define CONTEXT_ARM_COMMON_SAVE 2 /* Called when ARM common is saved */
+#define CONTEXT_ARM_COMMON_RESTORE 3 /* Called when ARM common is restored */
+
+int context_ape_notifier_register(struct notifier_block *nb);
+int context_ape_notifier_unregister(struct notifier_block *nb);
+
+int context_arm_notifier_register(struct notifier_block *nb);
+int context_arm_notifier_unregister(struct notifier_block *nb);
+
+void context_vape_save(void);
+void context_vape_restore(void);
+
+void context_gpio_save(void);
+void context_gpio_restore(void);
+void context_gpio_restore_mux(void);
+void context_gpio_mux_safe_switch(bool begin);
+
+void context_varm_save_common(void);
+void context_varm_restore_common(void);
+
+void context_varm_save_core(void);
+void context_varm_restore_core(void);
+
+void context_save_cpu_registers(void);
+void context_restore_cpu_registers(void);
+
+void context_save_to_sram_and_wfi(bool cleanL1cache,
+				  bool cleanL2cache);
+
+void context_clean_l1_cache_all(void);
+void context_save_arm_registers(u32 **backup_stack);
+void context_restore_arm_registers(u32 **backup_stack);
+
+void context_save_cp15_registers(u32 **backup_stack);
+void context_restore_cp15_registers(u32 **backup_stack);
+
+void context_save_to_sram_and_wfi_internal(u32 backup_sram_storage,
+					   bool cleanL1cache,
+					   bool cleanL2cache);
+
+/* DB specific functions in either context-db8500 or context-db5500 */
+void u8500_context_save_icn(void);
+void u8500_context_restore_icn(void);
+void u8500_context_init(void);
+
+void u5500_context_save_icn(void);
+void u5500_context_restore_icn(void);
+void u5500_context_init(void);
+
+#else
+
+static inline void context_varm_save_core(void) {}
+static inline void context_save_cpu_registers(void) {}
+static inline void context_save_to_sram_and_wfi(bool cleanL1cache,
+						bool cleanL2cache) {}
+static inline void context_restore_cpu_registers(void) {}
+static inline void context_varm_restore_core(void) {}
+
+#endif
+
+#endif
diff --git a/arch/arm/mach-ux500/pm/context_arm.S b/arch/arm/mach-ux500/pm/context_arm.S
new file mode 100755
index 00000000000..d9afbcbf929
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/context_arm.S
@@ -0,0 +1,428 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com>
+ *         Rickard Andersson <rickard.andersson@stericsson.com> for
+ *         ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+
+#include <linux/linkage.h>
+#include <mach/hardware.h>
+#include <asm/hardware/cache-l2x0.h>
+
+/*
+ * Save and increment macro
+ */
+.macro SAVE_AND_INCREMENT FROM_REG TO_REG
+	str \FROM_REG, [\TO_REG], #+4
+.endm
+
+/*
+ * Decrement and restore macro
+ */
+.macro DECREMENT_AND_RESTORE FROM_REG TO_REG
+	ldr \TO_REG, [\FROM_REG, #-4]!
+.endm
+
+/*
+ * Save ARM registers
+ *
+ * This function must be called in supervisor mode.
+ *
+ * r0 = address to backup stack pointer
+ *
+ * Backup stack operations:
+ * + {sp, lr}^
+ * + cpsr
+ * + {r3, r8-r14} (FIQ mode: r3=spsr)
+ * + {r3, r13, r14} (IRQ mode: r3=spsr)
+ * + {r3, r13, r14} (abort mode: r3=spsr)
+ * + {r3, r13, r14} (undef mode: r3=spsr)
+ */
+	.align
+	.section ".text", "ax"
+ENTRY(context_save_arm_registers)
+	stmfd sp!, {r1, r2, r3, lr}		@ Save on stack
+	ldr r1, [r0]				@ Read backup stack pointer
+
+	stmia r1, {sp, lr}^			@ Store user mode sp and lr
+	                                        @ registers
+	add r1, r1, #8				@ Update backup pointer (not
+	                                        @ done in previous instruction)
+
+	mrs r2, cpsr				@ Get CPSR
+	SAVE_AND_INCREMENT r2 r1		@ Save CPSR register
+	orr r2, r2, #0xc0			@ Disable FIQ and IRQ
+	bic r2, r2, #0x1f			@ Setup r2 to change mode
+
+	@ The suffix to CPSR refers to which field(s) of the CPSR is
+	@ rereferenced (you can specify one or more). Defined fields are:
+	@
+	@ c - control
+	@ x - extension
+	@ s - status
+	@ f - flags
+
+	orr r3, r2, #0x11			@ Save FIQ mode registers
+	msr cpsr_cxsf, r3
+	mrs r3, spsr
+	stmia r1!, {r3, r8-r14}
+
+	orr r3, r2, #0x12			@ Save IRQ mode registers
+	msr cpsr_cxsf, r3
+	mrs r3, spsr
+	stmia r1!, {r3, r13, r14}
+
+	orr r3, r2, #0x17			@ Save abort mode registers +
+	                                        @ common mode registers
+	msr cpsr_cxsf, r3
+	mrs r3, spsr
+	stmia r1!, {r3, r13, r14}
+
+	orr r3, r2, #0x1B			@ Save undef mode registers
+	msr cpsr_cxsf, r3
+	mrs r3, spsr
+	stmia r1!, {r3, r13, r14}
+
+	orr r3, r2, #0x13			@ Return to supervisor mode
+	msr cpsr_cxsf, r3
+
+	str r1, [r0]				@ Write backup stack pointer
+	ldmfd sp!, {r1, r2, r3, pc}		@ Restore registers and return
+
+
+
+/*
+ * Restore ARM registers
+ *
+ * This function must be called in supervisor mode.
+ *
+ * r0 = address to backup stack pointer
+ *
+ * Backup stack operations:
+ * - {r3, r13, r14} (undef mode: spsr=r3)
+ * - {r3, r13, r14} (abort mode: spsr=r3)
+ * - {r3, r13, r14} (IRQ mode: spsr=r3)
+ * - {r3, r8-r14} (FIQ mode: spsr=r3)
+ * - cpsr
+ * - {sp, lr}^
+ */
+	.align
+	.section ".text", "ax"
+ENTRY(context_restore_arm_registers)
+	stmfd sp!, {r1, r2, r3, lr}		@ Save on stack
+	ldr r1, [r0]				@ Read backup stack pointer
+
+	mrs r2, cpsr				@ Get CPSR
+	orr r2, r2, #0xc0			@ Disable FIQ and IRQ
+	bic r2, r2, #0x1f			@ Setup r2 to change mode
+
+	orr r3, r2, #0x1b			@ Restore undef mode registers
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3, r13, r14}
+	msr spsr_cxsf, r3
+
+	orr r3, r2, #0x17			@ Restore abort mode registers
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3, r13, r14}
+	msr spsr_cxsf, r3
+
+	orr r3, r2, #0x12			@ Restore IRQ mode registers
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3, r13, r14}
+	msr spsr_cxsf, r3
+
+	orr r3, r2, #0x11			@ Restore FIQ mode registers
+	msr cpsr_cxsf, r3
+	ldmdb r1!, {r3, r8-r14}
+	msr spsr_cxsf, r3
+
+	DECREMENT_AND_RESTORE r1 r3		@ Restore cpsr register
+	msr cpsr_cxsf, r3
+
+	ldmdb r1, {sp, lr}^			@ Restore sp and lr registers
+	sub r1, r1, #8				@ Update backup pointer (not
+	                                        @ done in previous instruction)
+
+	str r1, [r0]				@ Write backup stack pointer
+	ldmfd sp!, {r1, r2, r3, pc}		@ Restore registers and return
+
+
+
+/*
+ * Save CP15 registers
+ *
+ * This function must be called in supervisor mode.
+ *
+ * r0 = address to backup stack pointer
+ *
+ * TTBR0, TTBR1, TTBRC, DACR CP15 registers are restored by boot ROM from SRAM.
+ */
+	.align 4
+	.section ".text", "ax"
+ENTRY(context_save_cp15_registers)
+	stmfd sp!, {r1, r2, lr}			@ Save on stack
+	ldr r1, [r0]				@ Read backup stack pointer
+
+	mrc p15, 0, r2, c12, c0, 0		@ Read Non-secure Vector Base
+	                                        @ Address Register
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 0, r2, c10, c2, 0		@ Access primary memory region
+	                                        @ remap register
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 0, r2, c10, c2, 1		@ Access normal memory region
+	                                        @ remap register
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 0, r2, c13, c0, 1		@ Read Context ID Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c13, c0, 2		@ Read Thread ID registers,
+	                                        @ this register is both user
+	                                        @ and privileged R/W accessible
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c13, c0, 3		@ Read Thread ID registers,
+	                                        @ this register is user
+	                                        @ read-only and privileged R/W
+	                                        @ accessible.
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c13, c0, 4		@ Read Thread ID registers,
+	                                        @ this register is privileged
+	                                        @ R/W accessible only.
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 2, r2, c0, c0, 0		@ Cache Size Selection Register
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 0, r2, c9, c12, 0		@ Read PMNC Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c12, 1		@ Read PMCNTENSET Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c12, 5		@ Read PMSELR Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c13, 0		@ Read PMCCNTR Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c13, 1		@ Read PMXEVTYPER Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c14, 0		@ Read PMUSERENR Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c14, 1		@ Read PMINTENSET Register
+	SAVE_AND_INCREMENT r2 r1
+	mrc p15, 0, r2, c9, c14, 2		@ Read PMINTENCLR Register
+	SAVE_AND_INCREMENT r2 r1
+
+	mrc p15, 0, r2, c1, c0, 2		@ Read CPACR Register
+	SAVE_AND_INCREMENT r2 r1
+
+	str r1, [r0]				@ Write backup stack pointer
+	ldmfd sp!, {r1, r2, pc}			@ Restore registers and return
+
+
+
+/*
+ * Restore CP15 registers
+ *
+ * This function must be called in supervisor mode.
+ *
+ * r0 = address to backup stack pointer
+ */
+	.align 4
+	.section ".text", "ax"
+ENTRY(context_restore_cp15_registers)
+	stmfd sp!, {r1, r2, lr}			@ Save on stack
+	ldr r1, [r0]				@ Read backup stack pointer
+
+	DECREMENT_AND_RESTORE r1 r2		@ Write CPACR register
+	mcr p15, 0, r2, c1, c0, 2
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c14, 2		@ Write PMINTENCLR Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c14, 1		@ Write PMINTENSET Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c14, 0		@ Write PMUSERENR Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c13, 1		@ Write PMXEVTYPER Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c13, 0		@ Write PMCCNTR Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c12, 5		@ Write PMSELR Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c12, 1		@ Write PMCNTENSET Register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c9, c12, 0		@ Write PMNC Register
+
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 2, r2, c0, c0, 0		@ Cache Size Selection Register
+
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c13, c0, 4		@ Write Thread ID registers,
+	                                        @ this register is privileged
+	                                        @ R/W accessible only
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c13, c0, 3		@ Write Thread ID registers,
+	                                        @ this register is user
+	                                        @ read-only and privileged R/W
+	                                        @ accessible
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c13, c0, 2		@ Write Thread ID registers,
+	                                        @ this register is both user
+	                                        @ and privileged R/W accessible
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c13, c0, 1		@ Write Context ID Register
+
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c10, c2, 1		@ Access normal memory region
+	                                        @ remap register
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c10, c2, 0		@ Access primary memory region
+	                                        @ remap register
+
+	DECREMENT_AND_RESTORE r1 r2
+	mcr p15, 0, r2, c12, c0, 0		@ Write Non-secure Vector Base
+	                                        @ Address Register
+
+	str r1, [r0]				@ Write backup stack pointer
+	ldmfd sp!, {r1, r2, pc}			@ Restore registers and return
+
+
+/*
+ * L1 cache clean function. Commit 'dirty' data from L1
+ * to L2 cache.
+ *
+ * r0, r1, r2,  used locally
+ *
+ */
+	.align 4
+	.section ".text", "ax"
+ENTRY(context_clean_l1_cache_all)
+
+	mov	r0, #0				@ swith to cache level 0
+	                                        @ (L1 cache)
+	mcr	p15, 2, r0, c0, c0, 0		@ select current cache level
+	                                        @ in cssr
+
+	dmb
+	mov     r1, #0				@ r1 = way index
+wayLoopL1clean:
+	mov     r0, #0				@ r0 = line index
+lineLoopL1clean:
+	mov     r2, r1, lsl #30			@ TODO: OK to hard-code
+	                                        @ SoC-specific L1 cache details?
+	add     r2, r0, lsl #5
+	mcr     p15, 0, r2, c7, c10, 2		@ Clean cache by set/way
+	add     r0, r0, #1
+	cmp     r0, #256			@ TODO: Ok with hard-coded
+	                                        @ set/way sizes or do we have to
+	                                        @ read them from ARM regs? Is it
+	                                        @ set correctly in silicon?
+	bne     lineLoopL1clean
+	add     r1, r1, #1
+	cmp     r1, #4				@ TODO: Ditto, sizes...
+	bne     wayLoopL1clean
+
+	dsb
+	isb
+	mov	pc, lr
+
+ENDPROC(context_clean_l1_cache_all)
+
+
+/*
+ * L2 cache clean function. Commit from PL310 L2 cache
+ * controller to DDR SDRAM.
+ *
+ * r0, r2 used locally
+ *
+ */
+	.align 4
+	.section ".text", "ax"
+ENTRY(ux500_clean_l2_cache_all)
+
+	ldr r0, =IO_ADDRESS(U8500_L2CC_BASE)
+
+	ldr r1, =0xff				@ TODO: Ok to assume 8-way cache
+	                                        @ on Ux500?
+	str r1, [r0, #L2X0_CLEAN_WAY]
+	L2busywaitLoopClean:
+	ldr r1, [r0, #L2X0_CLEAN_WAY]
+	cmp r1, #0				@ All bits in L2X0_CLEAN_WAY
+	                                        @ will be zero once clean is
+	                                        @ finished
+	bne L2busywaitLoopClean
+
+	ldr r1, =0x0
+	str r1, [r0, #L2X0_CACHE_SYNC]
+						@ l2x0 C code busy-wait here to
+	                                        @ ensure no background op is
+	                                        @ running.
+						@ In our case we have already
+	                                        @ checked this after the cache
+	                                        @ clean and CACHE_SYNC is atomic
+	                                        @ according to refman
+	mov	pc, lr
+
+
+/*
+ * Last saves and WFI
+ *
+ * r0 = address to backup_sram_storage base adress
+ * r1 = indicate whether L1 cache should be cleaned
+ * r2 = indicate whether L2 cache should be cleaned
+ */
+	.align 4
+	.section ".text", "ax"
+ENTRY(context_save_to_sram_and_wfi_internal)
+
+	stmfd sp!, {r3-r12, lr}			@ save on stack.
+
+	mrc p15, 0, r3, c1, c0, 0		@ read cp15 system control
+	                                        @ register
+	str r3, [r0, #0x00]
+	mrc p15, 0, r3, c2, c0, 0		@ read cp15 ttb0 register
+	str r3, [r0, #0x04]
+	mrc p15, 0, r3, c2, c0, 1		@ read cp15 ttb1 register
+	str r3, [r0, #0x08]
+	mrc p15, 0, r3, c2, c0, 2		@ read cp15 ttb control register
+	str r3, [r0, #0x0C]
+	mrc p15, 0, r3, c3, c0, 0		@ read domain access control
+	                                        @ register
+	str r3, [r0, #0x10]
+
+	ldr r3, =return_here
+	str r3, [r0, #0x14]			@ save program counter restore
+	                                        @ value to backup_sram_storage
+	mrs r3, cpsr
+	str r3, [r0, #0x18]			@ save cpsr to
+	                                        @ backup_sram_storage
+	str sp, [r0, #0x1c]			@ save sp to backup_sram_storage
+
+	mov r4, r2				@ Set r5 = cleanL2cache, r2
+	                                        @ will be destroyed by
+	                                        @ v7_clean_l1_cache_all
+	dsb
+	cmp r1, #0
+
+	blne context_clean_l1_cache_all		@ Commit all dirty data in L1
+	                                        @ cache to L2 without
+	                                        @ invalidating
+	dsb
+	cmp r4, #0
+
+	blne ux500_clean_l2_cache_all		@ If r4 != FALSE then clean all
+	                                        @ dirty data in L2 cache, no
+	                                        @ invalidate
+
+	dsb					@ data synchronization barrier
+	isb					@ instruction synchronization
+	                                        @ barrier
+	wfi					@ wait for interrupt
+
+return_here:					@ both cores return here
+						@ now we are out deep sleep
+	                                        @ with all the context lost
+	                                        @ except pc, sp and cpsr
+
+	ldmfd sp!, {r3-r12, pc}			@ restore from stack
+
diff --git a/arch/arm/mach-ux500/pm/cpuidle.c b/arch/arm/mach-ux500/pm/cpuidle.c
new file mode 100644
index 00000000000..cc1a4169110
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/cpuidle.c
@@ -0,0 +1,689 @@
+/*
+ * Copyright (C) STMicroelectronics 2009
+ * Copyright (C) ST-Ericsson SA 2010-2011
+ *
+ * Author: Rickard Andersson <rickard.andersson@stericsson.com>,
+ *	   Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson.
+ *
+ * Loosely based on cpuidle.c by Sundar Iyer.
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/tick.h>
+#include <linux/clockchips.h>
+#include <linux/gpio.h>
+#include <linux/regulator/db8500-prcmu.h>
+
+#include <mach/prcmu.h>
+
+#include "cpuidle.h"
+#include "cpuidle_dbg.h"
+#include "context.h"
+#include "pm.h"
+#include "timer.h"
+
+/*
+ * All measurements are with two cpus online (worst case) and at
+ * 200 MHz (worst case)
+ *
+ * Enter latency depends on cpu frequency, and is only depending on
+ * code executing on the ARM.
+ * Exit latency is both depending on "wake latency" which is the
+ * time between the PRCMU has gotten the interrupt and the ARM starts
+ * to execute and the time before everything is done on the ARM.
+ * The wake latency is more or less constant related to cpu frequency,
+ * but can differ depending on what the modem does.
+ * Wake latency is not included for plain WFI.
+ * For states that uses RTC (Sleep & DeepSleep), wake latency is reduced
+ * from clock programming timeout.
+ *
+ */
+#define DEEP_SLEEP_WAKE_UP_LATENCY 8500
+/* Wake latency from ApSleep is measured to be around 1.0 to 1.5 ms */
+#define MIN_SLEEP_WAKE_UP_LATENCY 1000
+#define MAX_SLEEP_WAKE_UP_LATENCY 1500
+
+#define UL_PLL_START_UP_LATENCY 8000 /* us */
+
+static struct cstate cstates[] = {
+	{
+		.enter_latency = 0,
+		.exit_latency = 0,
+		.threshold = 0,
+		.power_usage = 1000,
+		.APE = APE_ON,
+		.ARM = ARM_ON,
+		.ARM_PLL = ARM_PLL_ON,
+		.UL_PLL = UL_PLL_ON,
+		.ESRAM = ESRAM_RET,
+		.pwrst = PRCMU_AP_NO_CHANGE,
+		.state = CI_RUNNING,
+		.desc = "Running                ",
+	},
+	{
+		/* These figures are not really true. There is a cost for WFI */
+		.enter_latency = 0,
+		.exit_latency = 0,
+		.threshold = 0,
+		.power_usage = 10,
+		.APE = APE_ON,
+		.ARM = ARM_ON,
+		.ARM_PLL = ARM_PLL_ON,
+		.UL_PLL = UL_PLL_ON,
+		.ESRAM = ESRAM_RET,
+		.pwrst = PRCMU_AP_NO_CHANGE,
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.state = CI_WFI,
+		.desc = "Wait for interrupt     ",
+	},
+	{
+		.enter_latency = 60,
+		.exit_latency = 60,
+		.threshold = 150,
+		.power_usage = 5,
+		.APE = APE_ON,
+		.ARM = ARM_RET,
+		.ARM_PLL = ARM_PLL_ON,
+		.UL_PLL = UL_PLL_ON,
+		.ESRAM = ESRAM_RET,
+		.pwrst = PRCMU_AP_IDLE,
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.state = CI_IDLE,
+		.desc = "ApIdle                 ",
+	},
+	{
+		.enter_latency = 70,
+		.exit_latency = 70,
+		.threshold = 160,
+		.power_usage = 4,
+		.APE = APE_ON,
+		.ARM = ARM_RET,
+		.ARM_PLL = ARM_PLL_OFF,
+		.UL_PLL = UL_PLL_ON,
+		.ESRAM = ESRAM_RET,
+		.pwrst = PRCMU_AP_IDLE,
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.state = CI_IDLE,
+		.desc = "ApIdle, ARM PLL off    ",
+	},
+	{
+		.enter_latency = 250,
+		.exit_latency = MAX_SLEEP_WAKE_UP_LATENCY + 200,
+		/*
+		 * Note: Sleep time must be longer than 120 us or else
+		 * there might be issues with the RTC-RTT block.
+		 */
+		.threshold = MAX_SLEEP_WAKE_UP_LATENCY + 250 + 200,
+		.power_usage = 3,
+		.APE = APE_OFF,
+		.ARM = ARM_RET,
+		.ARM_PLL = ARM_PLL_OFF,
+		.UL_PLL = UL_PLL_ON,
+		.ESRAM = ESRAM_RET,
+		.pwrst = PRCMU_AP_SLEEP,
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.state = CI_SLEEP,
+		.desc = "ApSleep                ",
+	},
+	{
+		.enter_latency = 250,
+		.exit_latency = (MAX_SLEEP_WAKE_UP_LATENCY +
+				 UL_PLL_START_UP_LATENCY + 200),
+		.threshold = (MAX_SLEEP_WAKE_UP_LATENCY +
+			      UL_PLL_START_UP_LATENCY + 250 + 200),
+		.power_usage = 2,
+		.APE = APE_OFF,
+		.ARM = ARM_RET,
+		.ARM_PLL = ARM_PLL_OFF,
+		.UL_PLL = UL_PLL_OFF,
+		.ESRAM = ESRAM_RET,
+		.pwrst = PRCMU_AP_SLEEP,
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.state = CI_SLEEP,
+		.desc = "ApSleep, UL PLL off    ",
+	},
+#ifdef CONFIG_U8500_CPUIDLE_APDEEPIDLE
+	{
+		.enter_latency = 300,
+		.exit_latency = DEEP_SLEEP_WAKE_UP_LATENCY + 400,
+		.threshold = DEEP_SLEEP_WAKE_UP_LATENCY + 300 + 400,
+		.power_usage = 2,
+		.APE = APE_ON,
+		.ARM = ARM_OFF,
+		.ARM_PLL = ARM_PLL_OFF,
+		.UL_PLL = UL_PLL_ON,
+		.ESRAM = ESRAM_RET,
+		.pwrst = PRCMU_AP_DEEP_IDLE,
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.state = CI_DEEP_IDLE,
+		.desc = "ApDeepIdle, UL PLL off ",
+	},
+#endif
+	{
+		.enter_latency = 310,
+		.exit_latency = DEEP_SLEEP_WAKE_UP_LATENCY + 420,
+		.threshold = DEEP_SLEEP_WAKE_UP_LATENCY + 310 + 420,
+		.power_usage = 1,
+		.APE = APE_OFF,
+		.ARM = ARM_OFF,
+		.ARM_PLL = ARM_PLL_OFF,
+		.UL_PLL = UL_PLL_OFF,
+		.ESRAM = ESRAM_RET,
+		.pwrst = PRCMU_AP_DEEP_SLEEP,
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.state = CI_DEEP_SLEEP,
+		.desc = "ApDeepsleep, UL PLL off",
+	},
+};
+
+struct cpu_state {
+	int gov_cstate;
+	ktime_t sched_wake_up;
+	struct cpuidle_device dev;
+};
+
+static DEFINE_PER_CPU(struct cpu_state, *cpu_state);
+
+static DEFINE_SPINLOCK(cpuidle_lock);
+static bool restore_ape; /* protected by cpuidle_lock */
+static bool restore_arm; /* protected by cpuidle_lock */
+static ktime_t time_next;  /* protected by cpuidle_lock */
+
+extern struct clock_event_device u8500_mtu_clkevt;
+
+static atomic_t idle_cpus_counter = ATOMIC_INIT(0);
+
+struct cstate *ux500_ci_get_cstates(int *len)
+{
+	if (len != NULL)
+		(*len) = ARRAY_SIZE(cstates);
+	return cstates;
+}
+
+static void restore_sequence(ktime_t now)
+{
+	spin_lock(&cpuidle_lock);
+
+	smp_rmb();
+	if (restore_arm) {
+
+		restore_arm = false;
+		smp_wmb();
+
+		/* Restore gic settings */
+		context_varm_restore_common();
+	}
+
+	smp_rmb();
+	if (restore_ape) {
+		restore_ape = false;
+		smp_wmb();
+
+		/*
+		 * APE has been turned off. Save GPIO wake up cause before
+		 * clearing ioforce.
+		 */
+		context_vape_restore();
+
+		ux500_pm_gpio_save_wake_up_status();
+
+		/* Restore IO ring */
+		ux500_pm_prcmu_set_ioforce(false);
+
+		ux500_ci_dbg_console_handle_ape_resume();
+
+		ux500_rtcrtt_off();
+
+		/*
+		 * If we're returning from ApSleep and the RTC timer
+		 * caused the wake up, program the MTU to trigger.
+		 */
+		if ((ktime_to_us(now) > ktime_to_us(time_next)))
+			time_next = ktime_add(now, ktime_set(0, 1000));
+
+		/* Make sure have an MTU interrupt waiting for us */
+		clockevents_program_event(&u8500_mtu_clkevt,
+					  time_next,
+					  now);
+	}
+
+	spin_unlock(&cpuidle_lock);
+
+}
+
+/**
+ * get_remaining_sleep_time() - returns remaining sleep time in
+ * microseconds (us)
+ */
+static u32 get_remaining_sleep_time(ktime_t *next, int *on_cpu)
+{
+	ktime_t now, t;
+	int cpu;
+	int delta;
+	u32 remaining_sleep_time = UINT_MAX;
+
+	now = ktime_get();
+
+	/* Check next schedule to expire considering both cpus */
+
+	spin_lock(&cpuidle_lock);
+	for_each_online_cpu(cpu) {
+		t = per_cpu(cpu_state, cpu)->sched_wake_up;
+
+		delta = ktime_to_us(ktime_sub(t, now));
+		if ((delta < remaining_sleep_time) && (delta > 0)) {
+			remaining_sleep_time = (u32)delta;
+			if (next)
+				(*next) = t;
+			if (on_cpu)
+				(*on_cpu) = cpu;
+		}
+	}
+	spin_unlock(&cpuidle_lock);
+
+	return remaining_sleep_time;
+}
+
+static bool is_last_cpu_running(void)
+{
+	return atomic_read(&idle_cpus_counter) == num_online_cpus();
+}
+
+static int determine_sleep_state(u32 *sleep_time)
+{
+	int i;
+
+	int cpu;
+	int max_depth;
+	bool power_state_req;
+
+	/* If first cpu to sleep, go to most shallow sleep state */
+	if (!is_last_cpu_running())
+		return CI_WFI;
+
+	/* If other CPU is going to WFI, but not yet there wait. */
+	while (1) {
+		if (ux500_pm_other_cpu_wfi())
+			break;
+
+		if (ux500_pm_gic_pending_interrupt())
+			return -1;
+
+		if (!is_last_cpu_running())
+			return CI_WFI;
+	}
+
+	power_state_req = power_state_active_is_enabled() ||
+		prcmu_is_ac_wake_requested();
+
+	(*sleep_time) = get_remaining_sleep_time(NULL, NULL);
+
+	if ((*sleep_time) == UINT_MAX)
+		return CI_WFI;
+	/*
+	 * Never go deeper than the governor recommends even though it might be
+	 * possible from a scheduled wake up point of view
+	 */
+	max_depth = ux500_ci_dbg_deepest_state();
+
+	for_each_online_cpu(cpu) {
+		if (max_depth > per_cpu(cpu_state, cpu)->gov_cstate)
+			max_depth = per_cpu(cpu_state, cpu)->gov_cstate;
+	}
+
+	for (i = max_depth; i > 0; i--) {
+
+		if ((*sleep_time) <= cstates[i].threshold)
+			continue;
+
+		if (cstates[i].APE == APE_OFF) {
+			/* This state says APE should be off */
+			if (power_state_req ||
+			    ux500_ci_dbg_force_ape_on())
+				continue;
+		}
+
+		/* OK state */
+		break;
+	}
+
+	ux500_ci_dbg_register_reason(i, power_state_req,
+				     (*sleep_time),
+				     max_depth);
+
+	return max(CI_WFI, i);
+}
+
+static int enter_sleep(struct cpuidle_device *dev,
+		       struct cpuidle_state *ci_state)
+{
+	ktime_t time_enter, time_exit, time_wake;
+	ktime_t wake_up;
+	int sleep_time = 0;
+	s64 diff;
+	int ret;
+	int target;
+	struct cpu_state *state;
+	bool slept_well = false;
+	bool restore_local_arm = false;
+	int this_cpu = smp_processor_id();
+
+	local_irq_disable();
+
+	time_enter = ktime_get(); /* Time now */
+
+	state = per_cpu(cpu_state, smp_processor_id());
+
+	wake_up = ktime_add(time_enter, tick_nohz_get_sleep_length());
+
+	spin_lock(&cpuidle_lock);
+
+	/* Save scheduled wake up for this cpu */
+	state->sched_wake_up = wake_up;
+
+	/* Retrive the cstate that the governor recommends for this CPU */
+	state->gov_cstate = (int) cpuidle_get_statedata(ci_state);
+
+	if (state->gov_cstate > ux500_ci_dbg_deepest_state())
+		state->gov_cstate = ux500_ci_dbg_deepest_state();
+
+	spin_unlock(&cpuidle_lock);
+
+	atomic_inc(&idle_cpus_counter);
+
+	/*
+	 * Determine sleep state considering both CPUs and
+	 * shared resources like e.g. VAPE
+	 */
+	target = determine_sleep_state(&sleep_time);
+
+	if (target < 0)
+		/* "target" will be last_state in the cpuidle framework */
+		goto exit_fast;
+
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &this_cpu);
+
+	if (cstates[target].ARM != ARM_ON) {
+
+		/* Decouple GIC from the interrupt bus */
+		ux500_pm_gic_decouple();
+
+		if (!ux500_pm_other_cpu_wfi())
+			/* Other CPU was not in WFI => abort */
+			goto exit;
+		/*
+		 * Check if we have a pending interrupt or if sleep
+		 * state has changed after GIC has been frozen
+		 */
+		if (ux500_pm_gic_pending_interrupt())
+			goto exit;
+
+		if (ux500_pm_prcmu_pending_interrupt())
+			/* An interrupt found => abort */
+			goto exit;
+		/*
+		 * No PRCMU interrupt was pending => continue the
+		 * sleeping stages
+		 */
+
+		/* Copy GIC interrupt settings to PRCMU interrupt settings */
+		ux500_pm_prcmu_copy_gic_settings();
+	}
+
+	if (cstates[target].APE == APE_OFF) {
+		ktime_t est_wake_time;
+		int wake_cpu;
+
+		/* We are going to sleep or deep sleep => prepare for it */
+
+		/* Program the only timer that is available when APE is off */
+
+		sleep_time = get_remaining_sleep_time(&est_wake_time,
+						      &wake_cpu);
+
+		if (sleep_time == UINT_MAX)
+			goto exit;
+
+		if (cstates[target].UL_PLL == UL_PLL_OFF)
+			/* Compensate for ULPLL start up time */
+			sleep_time -= UL_PLL_START_UP_LATENCY;
+
+			/*
+			 * Not checking for negative sleep time since
+			 * determine_sleep_state has already checked that
+			 * there is enough time.
+			 */
+
+		/* Adjust for exit latency */
+		sleep_time -= MIN_SLEEP_WAKE_UP_LATENCY;
+
+		ux500_rtcrtt_next(sleep_time);
+
+		/*
+		 * Make sure the cpu that is scheduled first gets
+		 * the prcmu interrupt.
+		 */
+		irq_set_affinity(IRQ_DB8500_PRCMU1, cpumask_of(wake_cpu));
+
+		context_vape_save();
+
+		ux500_ci_dbg_console_handle_ape_suspend();
+		ux500_pm_prcmu_set_ioforce(true);
+
+		spin_lock(&cpuidle_lock);
+		restore_ape = true;
+		time_next = est_wake_time;
+		spin_unlock(&cpuidle_lock);
+	}
+
+	if (cstates[target].ARM == ARM_OFF) {
+		context_varm_save_common();
+		spin_lock(&cpuidle_lock);
+		restore_arm = true;
+		spin_unlock(&cpuidle_lock);
+	}
+
+	if (cstates[target].ARM == ARM_OFF ||
+	    cstates[state->gov_cstate].ARM == ARM_OFF) {
+		restore_local_arm = true;
+		context_varm_save_core();
+		context_save_cpu_registers();
+
+		/*
+		 * Due to we have only 100us between requesting a
+		 * powerstate and wfi, we clean the cache before as
+		 * well to assure the final cache clean before wfi
+		 * has as little as possible to do.
+		 */
+		context_clean_l1_cache_all();
+
+	}
+
+	ux500_ci_dbg_log(target, time_enter);
+
+	if (is_last_cpu_running() && cstates[target].ARM != ARM_ON)
+		prcmu_set_power_state(cstates[target].pwrst,
+				      cstates[target].UL_PLL,
+				      /* Is actually the AP PLL */
+				      cstates[target].UL_PLL);
+
+	/*
+	 * If deepsleep/deepidle, Save return address to SRAM and set
+	 * this CPU in WFI. This is last core to enter sleep, so we need to
+	 * clean both L2 and L1 caches
+	 */
+	if (cstates[state->gov_cstate].ARM == ARM_ON)
+		__asm__ __volatile__
+			("dsb\n\t" "wfi\n\t" : : : "memory");
+	else
+		context_save_to_sram_and_wfi(cstates[state->gov_cstate].ARM == ARM_OFF,
+					     cstates[target].ARM == ARM_OFF);
+
+	if (is_last_cpu_running())
+		ux500_ci_dbg_wake_latency(target, sleep_time);
+
+	time_wake = ktime_get();
+
+	/* The PRCMU restores ARM PLL and recouples the GIC */
+	if (restore_local_arm) {
+		context_restore_cpu_registers();
+		context_varm_restore_core();
+	}
+
+	slept_well = true;
+
+	restore_sequence(time_wake);
+
+exit:
+	if (!slept_well)
+		/* Recouple GIC with the interrupt bus */
+		ux500_pm_gic_recouple();
+
+	/* Use the ARM local timer for this cpu */
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT,
+			   &this_cpu);
+exit_fast:
+
+	if (target < 0)
+		target = CI_RUNNING;
+
+	/* 16 minutes ahead */
+	wake_up = ktime_add_us(time_enter,
+			       1000000000);
+
+	spin_lock(&cpuidle_lock);
+	/* Remove wake up time i.e. set wake up far ahead */
+	state->sched_wake_up = wake_up;
+	spin_unlock(&cpuidle_lock);
+
+	/*
+	 * We might have chosen another state than what the
+	 * governor recommended
+	 */
+	if (target != state->gov_cstate)
+		/* Update last state pointer used by CPUIDLE subsystem */
+		dev->last_state = &(dev->states[target]);
+
+	time_exit = ktime_get();
+	diff = ktime_to_us(ktime_sub(time_exit, time_enter));
+	if (diff > INT_MAX)
+		diff = INT_MAX;
+
+	ret = (int)diff;
+
+	ux500_ci_dbg_console_check_uart();
+	if (slept_well)
+		ux500_ci_dbg_exit_latency(target,
+					  time_exit, /* now */
+					  time_wake, /* exit from wfi */
+					  time_enter); /* enter cpuidle */
+
+	ux500_ci_dbg_log(CI_RUNNING, time_exit);
+
+	atomic_dec(&idle_cpus_counter);
+
+	local_irq_enable();
+
+	ux500_ci_dbg_console();
+
+	return ret;
+}
+
+static int init_cstates(int cpu, struct cpu_state *state)
+{
+	int i;
+	struct cpuidle_state *ci_state;
+	struct cpuidle_device *dev;
+
+	dev = &state->dev;
+	dev->cpu = cpu;
+
+	for (i = 0; i < ARRAY_SIZE(cstates); i++) {
+
+		ci_state = &dev->states[i];
+
+		cpuidle_set_statedata(ci_state, (void *)i);
+
+		ci_state->exit_latency = cstates[i].exit_latency;
+		ci_state->target_residency = cstates[i].threshold;
+		ci_state->flags = cstates[i].flags;
+		ci_state->enter = enter_sleep;
+		ci_state->power_usage = cstates[i].power_usage;
+		snprintf(ci_state->name, CPUIDLE_NAME_LEN, "C%d", i);
+		strncpy(ci_state->desc, cstates[i].desc, CPUIDLE_DESC_LEN);
+	}
+
+	dev->state_count = ARRAY_SIZE(cstates);
+
+	dev->safe_state = &dev->states[0]; /* Currently not used */
+
+	return cpuidle_register_device(dev);
+}
+
+struct cpuidle_driver cpuidle_drv = {
+	.name = "cpuidle_driver",
+	.owner = THIS_MODULE,
+};
+
+static int __init cpuidle_driver_init(void)
+{
+	int res = -ENODEV;
+	int cpu;
+
+	if (ux500_is_svp())
+		goto out;
+
+	/* Configure wake up reasons */
+	prcmu_enable_wakeups(PRCMU_WAKEUP(ARM) | PRCMU_WAKEUP(RTC) |
+			     PRCMU_WAKEUP(ABB));
+
+	ux500_ci_dbg_init();
+
+	for_each_possible_cpu(cpu)
+		per_cpu(cpu_state, cpu) = kzalloc(sizeof(struct cpu_state),
+						  GFP_KERNEL);
+
+	res = cpuidle_register_driver(&cpuidle_drv);
+	if (res)
+		goto out;
+
+	for_each_possible_cpu(cpu) {
+		res = init_cstates(cpu, per_cpu(cpu_state, cpu));
+		if (res)
+			goto out;
+		pr_info("cpuidle: initiated for CPU%d.\n", cpu);
+	}
+	return 0;
+out:
+	pr_err("cpuidle: initialization failed.\n");
+	return res;
+}
+
+static void __exit cpuidle_driver_exit(void)
+{
+	int cpu;
+	struct cpuidle_device *dev;
+
+	ux500_ci_dbg_remove();
+
+	for_each_possible_cpu(cpu) {
+		dev = &per_cpu(cpu_state, cpu)->dev;
+		cpuidle_unregister_device(dev);
+	}
+
+	for_each_possible_cpu(cpu)
+		kfree(per_cpu(cpu_state, cpu));
+
+	cpuidle_unregister_driver(&cpuidle_drv);
+}
+
+module_init(cpuidle_driver_init);
+module_exit(cpuidle_driver_exit);
+
+MODULE_DESCRIPTION("U8500 cpuidle driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Rickard Andersson <rickard.andersson@stericsson.com>");
diff --git a/arch/arm/mach-ux500/pm/cpuidle.h b/arch/arm/mach-ux500/pm/cpuidle.h
new file mode 100644
index 00000000000..f15338aa48b
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/cpuidle.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Rickard Andersson <rickard.andersson@stericsson.com> for
+ *         ST-Ericsson. Loosly based on cpuidle.c by Sundar Iyer.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+
+#ifndef __CPUIDLE_H
+#define __CPUIDLE_H
+
+#include <linux/cpuidle.h>
+
+enum ARM {
+	ARM_OFF,
+	ARM_RET,
+	ARM_ON
+};
+
+enum APE {
+	APE_OFF,
+	APE_ON
+};
+
+enum ARM_PLL {
+	ARM_PLL_OFF = 0,
+	ARM_PLL_ON = 1
+};
+
+enum UL_PLL {
+	UL_PLL_OFF,
+	UL_PLL_ON
+};
+
+enum ESRAM {
+	ESRAM_OFF,
+	ESRAM_RET
+};
+
+enum ci_pwrst {
+	CI_RUNNING = 0, /* Must be the same number as entry in cstates */
+	CI_WFI = 1, /* Must be the same number as entry in cstates */
+	CI_IDLE,
+	CI_SLEEP,
+	CI_DEEP_IDLE,
+	CI_DEEP_SLEEP,
+};
+
+struct cstate {
+	/* Required state of different hardwares */
+	enum ARM ARM;
+	enum APE APE;
+	enum ARM_PLL ARM_PLL;
+	enum UL_PLL UL_PLL;
+	/* ESRAM = ESRAM_RET means that ESRAM context to be kept */
+	enum ESRAM ESRAM;
+
+	u32 enter_latency;
+	u32 exit_latency;
+	u32 power_usage;
+	u32 threshold;
+	u32 flags;
+	u8 pwrst;
+
+	/* Only used for debugging purpose */
+	enum ci_pwrst state;
+	char desc[CPUIDLE_DESC_LEN];
+};
+
+struct cstate *ux500_ci_get_cstates(int *len);
+
+#endif
diff --git a/arch/arm/mach-ux500/pm/cpuidle_dbg.c b/arch/arm/mach-ux500/pm/cpuidle_dbg.c
new file mode 100644
index 00000000000..85c05d42f81
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/cpuidle_dbg.c
@@ -0,0 +1,950 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010-2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Rickard Andersson <rickard.andersson@stericsson.com>,
+ *	   Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ */
+
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/uaccess.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/amba/serial.h>
+
+#include <linux/gpio.h>
+#include <asm/hardware/gic.h>
+
+#include "cpuidle.h"
+#include "pm.h"
+#include "timer.h"
+
+#define APE_ON_TIMER_INTERVAL 5 /* Seconds */
+
+#define UART_RX_GPIO_PIN_MASK (1 << (CONFIG_UX500_CONSOLE_UART_GPIO_PIN % 32))
+
+#define UART011_MIS_RTIS (1 << 6) /* receive timeout interrupt status */
+#define UART011_MIS_RXIS (1 << 4) /* receive interrupt status */
+#define UART011_MIS 0x40 /* Masked interrupt status register */
+
+enum latency_type {
+	LATENCY_ENTER = 0,
+	LATENCY_EXIT,
+	LATENCY_WAKE,
+	NUM_LATENCY,
+};
+
+struct state_history_state {
+	u32 counter;
+	ktime_t time;
+	u32 hit_rate;
+	u32 state_ok;
+	u32 state_error;
+	u32 state_int;
+	u32 sga;
+
+	u32 latency_count[NUM_LATENCY];
+	ktime_t latency_sum[NUM_LATENCY];
+	ktime_t latency_min[NUM_LATENCY];
+	ktime_t latency_max[NUM_LATENCY];
+};
+
+struct state_history {
+	ktime_t start;
+	u32 state;
+	u32 exit_counter;
+	ktime_t measure_begin;
+	int ape_blocked;
+	int time_blocked;
+	int both_blocked;
+	int gov_blocked;
+	struct state_history_state *states;
+};
+static DEFINE_PER_CPU(struct state_history, *state_history);
+
+static struct delayed_work cpuidle_work;
+static u32 dbg_console_enable = 1;
+static void __iomem *uart_base;
+static struct clk *uart_clk;
+
+/* Blocks ApSleep and ApDeepSleep */
+static bool force_APE_on;
+static bool reset_timer;
+static int deepest_allowed_state = CONFIG_U8500_CPUIDLE_DEEPEST_STATE;
+static u32 measure_latency;
+static bool wake_latency;
+static int verbose;
+
+static bool apidle_both_blocked;
+static bool apidle_ape_blocked;
+static bool apidle_time_blocked;
+static bool apidle_gov_blocked;
+
+static struct cstate *cstates;
+static int cstates_len;
+static DEFINE_SPINLOCK(dbg_lock);
+
+bool ux500_ci_dbg_force_ape_on(void)
+{
+	clk_enable(uart_clk);
+	if (readw(uart_base + UART01x_FR) & UART01x_FR_BUSY) {
+		clk_disable(uart_clk);
+		return true;
+	}
+	clk_disable(uart_clk);
+
+	return force_APE_on;
+}
+
+int ux500_ci_dbg_deepest_state(void)
+{
+	return deepest_allowed_state;
+}
+
+void ux500_ci_dbg_console_handle_ape_suspend(void)
+{
+	if (!dbg_console_enable)
+		return;
+
+	set_irq_wake(NOMADIK_GPIO_TO_IRQ(CONFIG_UX500_CONSOLE_UART_GPIO_PIN), 1);
+	set_irq_type(NOMADIK_GPIO_TO_IRQ(CONFIG_UX500_CONSOLE_UART_GPIO_PIN),
+		     IRQ_TYPE_EDGE_BOTH);
+}
+
+void ux500_ci_dbg_console_handle_ape_resume(void)
+{
+	unsigned long flags;
+	u32 WKS_reg_value;
+
+	if (!dbg_console_enable)
+		return;
+
+	WKS_reg_value = ux500_pm_gpio_read_wake_up_status(0);
+
+	if (WKS_reg_value & UART_RX_GPIO_PIN_MASK) {
+		spin_lock_irqsave(&dbg_lock, flags);
+		reset_timer = true;
+		spin_unlock_irqrestore(&dbg_lock, flags);
+	}
+	set_irq_wake(NOMADIK_GPIO_TO_IRQ(CONFIG_UX500_CONSOLE_UART_GPIO_PIN), 0);
+
+}
+
+void ux500_ci_dbg_console_check_uart(void)
+{
+	unsigned long flags;
+	u32 status;
+
+	if (!dbg_console_enable)
+		return;
+
+	clk_enable(uart_clk);
+	spin_lock_irqsave(&dbg_lock, flags);
+	status = readw(uart_base + UART011_MIS);
+
+	if (status & (UART011_MIS_RTIS | UART011_MIS_RXIS))
+		reset_timer = true;
+
+	spin_unlock_irqrestore(&dbg_lock, flags);
+	clk_disable(uart_clk);
+}
+
+void ux500_ci_dbg_console(void)
+{
+	unsigned long flags;
+
+	if (!dbg_console_enable)
+		return;
+
+	spin_lock_irqsave(&dbg_lock, flags);
+	if (reset_timer) {
+		reset_timer = false;
+		spin_unlock_irqrestore(&dbg_lock, flags);
+
+		cancel_delayed_work(&cpuidle_work);
+		force_APE_on = true;
+		schedule_delayed_work(&cpuidle_work,
+				      msecs_to_jiffies(APE_ON_TIMER_INTERVAL *
+						       1000));
+	} else {
+		spin_unlock_irqrestore(&dbg_lock, flags);
+	}
+}
+
+static void dbg_cpuidle_work_function(struct work_struct *work)
+{
+	force_APE_on = false;
+}
+
+static void store_latency(struct state_history *sh,
+			  int ctarget,
+			  enum latency_type type,
+			  ktime_t d,
+			  bool lock)
+{
+	unsigned long flags = 0;
+
+	if (lock)
+		spin_lock_irqsave(&dbg_lock, flags);
+
+	sh->states[ctarget].latency_count[type]++;
+
+	sh->states[ctarget].latency_sum[type] =
+		ktime_add(sh->states[ctarget].latency_sum[type], d);
+
+	if (ktime_to_us(d) > ktime_to_us(sh->states[ctarget].latency_max[type]))
+		sh->states[ctarget].latency_max[type] = d;
+
+	if (ktime_to_us(d) < ktime_to_us(sh->states[ctarget].latency_min[type]))
+		sh->states[ctarget].latency_min[type] = d;
+
+	if (lock)
+		spin_unlock_irqrestore(&dbg_lock, flags);
+}
+
+void ux500_ci_dbg_exit_latency(int ctarget, ktime_t now, ktime_t exit,
+			       ktime_t enter)
+{
+	struct state_history *sh;
+	bool hit = true;
+	enum prcmu_idle_stat prcmu_status;
+	unsigned int d;
+
+	if (!verbose)
+		return;
+
+	sh = per_cpu(state_history, smp_processor_id());
+
+	sh->exit_counter++;
+
+	d = ktime_to_us(ktime_sub(now, enter));
+
+	if ((ctarget + 1) < deepest_allowed_state)
+		hit = d	< cstates[ctarget + 1].threshold;
+	if (d < cstates[ctarget].threshold)
+		hit = false;
+
+	if (hit)
+		sh->states[ctarget].hit_rate++;
+
+	if (cstates[ctarget].state < CI_IDLE)
+		return;
+
+	prcmu_status = ux500_pm_prcmu_idle_stat();
+
+	switch (prcmu_status) {
+
+	case DEEP_SLEEP_OK:
+		if (cstates[ctarget].state == CI_DEEP_SLEEP)
+			sh->states[ctarget].state_ok++;
+		break;
+	case SLEEP_OK:
+		if (cstates[ctarget].state == CI_SLEEP)
+			sh->states[ctarget].state_ok++;
+		break;
+	case IDLE_OK:
+		if (cstates[ctarget].state == CI_IDLE)
+			sh->states[ctarget].state_ok++;
+		break;
+	case DEEPIDLE_OK:
+		if (cstates[ctarget].state == CI_DEEP_IDLE)
+			sh->states[ctarget].state_ok++;
+		break;
+	case ARM2PRCMUPENDINGIT_ER:
+		sh->states[ctarget].state_int++;
+		break;
+	case HWACFCT_IN_SGA:
+		sh->states[ctarget].sga++;
+		break;
+	default:
+		pr_info("cpuidle: unknown prcmu exit code: 0x%x state: %d\n",
+			prcmu_status, cstates[ctarget].state);
+		sh->states[ctarget].state_error++;
+		break;
+	}
+
+	if (!measure_latency)
+		return;
+
+	store_latency(sh,
+		      ctarget,
+		      LATENCY_EXIT,
+		      ktime_sub(now, exit),
+		      true);
+}
+
+void ux500_ci_dbg_wake_latency(int ctarget, int sleep_time)
+{
+	struct state_history *sh;
+	ktime_t l;
+	ktime_t zero_time;
+
+	if (!wake_latency || cstates[ctarget].state < CI_IDLE)
+		return;
+
+	l = zero_time = ktime_set(0, 0);
+	sh = per_cpu(state_history, smp_processor_id());
+
+	if (cstates[ctarget].state >= CI_SLEEP)
+		l = u8500_rtc_exit_latency_get();
+
+	if (cstates[ctarget].state == CI_IDLE) {
+		ktime_t d = ktime_set(0, sleep_time * 1000);
+		ktime_t now = ktime_get();
+
+		d = ktime_add(d, sh->start);
+		if (ktime_to_us(now) > ktime_to_us(d))
+			l = ktime_sub(now, d);
+		else
+			l = zero_time;
+	}
+
+	if (!ktime_equal(zero_time, l))
+		store_latency(sh,
+			      ctarget,
+			      LATENCY_WAKE,
+			      l,
+			      true);
+}
+
+static void state_record_time(struct state_history *sh, int ctarget,
+			      ktime_t now, ktime_t start, bool latency)
+{
+	ktime_t dtime;
+
+	dtime = ktime_sub(now, sh->start);
+	sh->states[sh->state].time = ktime_add(sh->states[sh->state].time,
+					       dtime);
+
+	sh->start = now;
+	sh->state = ctarget;
+
+	if (latency && cstates[ctarget].state != CI_RUNNING && measure_latency)
+		store_latency(sh,
+			      ctarget,
+			      LATENCY_ENTER,
+			      ktime_sub(now, start),
+			      false);
+
+	sh->states[sh->state].counter++;
+}
+
+void ux500_ci_dbg_register_reason(int idx, bool power_state_req,
+				  u32 time, u32 max_depth)
+{
+	if (cstates[idx].state == CI_IDLE && verbose) {
+		apidle_ape_blocked = power_state_req;
+		apidle_time_blocked = time < cstates[idx + 1].threshold;
+		apidle_both_blocked = power_state_req && apidle_time_blocked;
+		apidle_gov_blocked = cstates[max_depth].state == CI_IDLE;
+	}
+}
+
+void ux500_ci_dbg_log(int ctarget, ktime_t enter_time)
+{
+	int i;
+	ktime_t now;
+	unsigned long flags;
+	struct state_history *sh;
+	struct state_history *sh_other;
+	int this_cpu;
+
+	this_cpu = smp_processor_id();
+
+	now = ktime_get();
+
+	sh = per_cpu(state_history, this_cpu);
+
+	spin_lock_irqsave(&dbg_lock, flags);
+
+	if (cstates[ctarget].state == CI_IDLE && verbose) {
+		if (apidle_both_blocked)
+			sh->both_blocked++;
+		if (apidle_ape_blocked)
+			sh->ape_blocked++;
+		if (apidle_time_blocked)
+			sh->time_blocked++;
+		if (apidle_gov_blocked)
+			sh->gov_blocked++;
+	}
+
+	/*
+	 * Check if current state is just a repeat of
+	 *  the state we're already in, then just quit.
+	 */
+	if (ctarget == sh->state)
+		goto done;
+
+	state_record_time(sh, ctarget, now, enter_time, true);
+
+	/*
+	 * Update other cpus, (this_cpu = A, other cpus = B) if:
+	 * - A = running and B != WFI | running: Set B to WFI
+	 * - A = WFI and then B must be running: No changes
+	 * - A = !WFI && !RUNNING and then B must be WFI: B sets to A
+	 */
+
+	if (sh->state == CI_WFI)
+		goto done;
+
+	for_each_possible_cpu(i) {
+
+		if (this_cpu == i)
+			continue;
+
+		sh_other = per_cpu(state_history, i);
+
+		/* Same state, continue */
+		if (sh_other->state == sh->state)
+			continue;
+
+		if (cstates[ctarget].state == CI_RUNNING &&
+		    cstates[sh_other->state].state != CI_WFI) {
+			state_record_time(sh_other, CI_WFI, now,
+					  enter_time, false);
+			continue;
+		}
+		/*
+		 * This cpu is something else than running or wfi, both must be
+		 * in the same state.
+		 */
+		state_record_time(sh_other, ctarget, now, enter_time, true);
+	}
+done:
+	spin_unlock_irqrestore(&dbg_lock, flags);
+}
+
+static void state_history_reset(void)
+{
+	unsigned long flags;
+	unsigned int cpu;
+	int i, j;
+	struct state_history *sh;
+
+	spin_lock_irqsave(&dbg_lock, flags);
+
+	for_each_possible_cpu(cpu) {
+		sh = per_cpu(state_history, cpu);
+		for (i = 0; i <= cstates_len; i++) {
+			sh->states[i].counter = 0;
+			sh->states[i].hit_rate = 0;
+			sh->states[i].state_ok = 0;
+			sh->states[i].state_error = 0;
+			sh->states[i].state_int = 0;
+			sh->states[i].sga = 0;
+
+			sh->states[i].time = ktime_set(0, 0);
+
+			for (j = 0; j < NUM_LATENCY; j++) {
+				sh->states[i].latency_count[j] = 0;
+				sh->states[i].latency_min[j] = ktime_set(0,
+									 10000000);
+				sh->states[i].latency_max[j] = ktime_set(0, 0);
+				sh->states[i].latency_sum[j] = ktime_set(0, 0);
+			}
+		}
+
+		sh->start = ktime_get();
+		sh->measure_begin = sh->start;
+		/* Don't touch sh->state, since that is where we are now */
+
+		sh->exit_counter = 0;
+		sh->ape_blocked = 0;
+		sh->time_blocked = 0;
+		sh->both_blocked = 0;
+		sh->gov_blocked = 0;
+	}
+	spin_unlock_irqrestore(&dbg_lock, flags);
+}
+
+static int get_val(const char __user *user_buf,
+		   size_t count, int min, int max)
+{
+	long unsigned val;
+	int err;
+
+	err = kstrtoul_from_user(user_buf, count, 0, &val);
+
+	if (err)
+		return err;
+
+	if (val > max)
+		val = max;
+	if (val < min)
+		val = min;
+
+	return val;
+}
+
+static ssize_t set_deepest_state(struct file *file,
+				 const char __user *user_buf,
+				 size_t count, loff_t *ppos)
+{
+	int val;
+
+	val = get_val(user_buf, count, CI_WFI, cstates_len - 1);
+
+	if (val < 0)
+		return val;
+
+	deepest_allowed_state = val;
+
+	pr_debug("cpuidle: changed deepest allowed sleep state to %d.\n",
+		 deepest_allowed_state);
+
+	return count;
+}
+
+static int deepest_state_print(struct seq_file *s, void *p)
+{
+	seq_printf(s, "Deepest allowed sleep state is %d\n",
+		   deepest_allowed_state);
+
+	return 0;
+}
+
+static ssize_t stats_write(struct file *file,
+			   const char __user *user_buf,
+			   size_t count, loff_t *ppos)
+{
+	state_history_reset();
+	return count;
+}
+
+static int wake_latency_read(struct seq_file *s, void *p)
+{
+	seq_printf(s, "wake latency measurements is %s\n",
+		   wake_latency ? "on" : "off");
+	return 0;
+}
+
+static ssize_t wake_latency_write(struct file *file,
+				  const char __user *user_buf,
+				  size_t count, loff_t *ppos)
+{
+	int val = get_val(user_buf, count, 0, 1);
+	if (val < 0)
+		return val;
+
+	wake_latency = val;
+	ux500_rtcrtt_measure_latency(wake_latency);
+	return count;
+}
+
+static int verbose_read(struct seq_file *s, void *p)
+{
+	seq_printf(s, "verbose debug is %s\n", verbose ? "on" : "off");
+	return 0;
+}
+
+static ssize_t verbose_write(struct file *file,
+				  const char __user *user_buf,
+				  size_t count, loff_t *ppos)
+{
+	int val = get_val(user_buf, count, 0, 1);
+	if (val < 0)
+		return val;
+
+	verbose = val;
+	state_history_reset();
+
+	return count;
+}
+
+static void stats_disp_one(struct seq_file *s, struct state_history *sh,
+			   s64 total_us, int i)
+{
+	int j;
+	s64 avg[NUM_LATENCY];
+	s64 t_us;
+	s64 perc;
+	ktime_t init_time, zero_time;
+
+	init_time = ktime_set(0, 10000000);
+	zero_time = ktime_set(0, 0);
+
+	memset(&avg, 0, sizeof(s64) * NUM_LATENCY);
+
+	for (j = 0; j < NUM_LATENCY; j++)
+		avg[j] = ktime_to_us(sh->states[i].latency_sum[j]);
+
+	t_us = ktime_to_us(sh->states[i].time);
+	perc = ktime_to_us(sh->states[i].time);
+	do_div(t_us, 1000); /* to ms */
+	do_div(total_us, 100);
+	if (total_us)
+		do_div(perc, total_us);
+
+	for (j = 0; j < NUM_LATENCY; j++) {
+		if (sh->states[i].latency_count[j])
+			do_div(avg[j], sh->states[i].latency_count[j]);
+	}
+
+	seq_printf(s, "\n%d - %s: %u",
+		   i, cstates[i].desc,
+		   sh->states[i].counter);
+
+	if (sh->states[i].counter == 0)
+		return;
+
+	if (i > CI_WFI && verbose)
+		seq_printf(s, " (%u int:%u sga: %u err:%u)",
+			   sh->states[i].state_ok,
+			   sh->states[i].state_int,
+			   sh->states[i].sga,
+			   sh->states[i].state_error);
+
+	seq_printf(s, " in %d ms %d%%",
+		   (u32) t_us, (u32)perc);
+
+	if (cstates[i].state == CI_IDLE && verbose)
+		seq_printf(s, ", reg:%d time:%d both:%d gov:%d",
+			   sh->ape_blocked, sh->time_blocked,
+			   sh->both_blocked, sh->gov_blocked);
+
+	if (sh->states[i].counter && verbose)
+		seq_printf(s, ", hit rate: %u%% ",
+			   100 * sh->states[i].hit_rate /
+			   sh->states[i].counter);
+
+	if (i == CI_RUNNING || !(measure_latency || wake_latency))
+		return;
+
+	for (j = 0; j < NUM_LATENCY; j++) {
+		bool show = false;
+		if (!ktime_equal(sh->states[i].latency_min[j], init_time)) {
+			seq_printf(s, "\n\t\t\t\t");
+			switch (j) {
+			case LATENCY_ENTER:
+				if (measure_latency) {
+					seq_printf(s, "enter: ");
+					show = true;
+				}
+				break;
+			case LATENCY_EXIT:
+				if (measure_latency) {
+					seq_printf(s, "exit:	 ");
+					show = true;
+				}
+				break;
+			case LATENCY_WAKE:
+				if (wake_latency) {
+					seq_printf(s, "wake:	 ");
+					show = true;
+				}
+				break;
+			default:
+				seq_printf(s, "unknown!: ");
+				break;
+			}
+
+			if (!show)
+				continue;
+
+			if (ktime_equal(sh->states[i].latency_min[j],
+					zero_time))
+				seq_printf(s, "min < 30");
+			else
+				seq_printf(s, "min %lld",
+					   ktime_to_us(sh->states[i].latency_min[j]));
+
+			seq_printf(s, " avg %lld max %lld us, count: %d",
+				   avg[j],
+				   ktime_to_us(sh->states[i].latency_max[j]),
+				   sh->states[i].latency_count[j]);
+		}
+	}
+}
+
+static int stats_print(struct seq_file *s, void *p)
+{
+	int cpu;
+	int i;
+	unsigned long flags;
+	struct state_history *sh;
+	ktime_t total, wall;
+	s64 total_us, total_s;
+
+	for_each_online_cpu(cpu) {
+		sh = per_cpu(state_history, cpu);
+		spin_lock_irqsave(&dbg_lock, flags);
+		seq_printf(s, "\nCPU%d\n", cpu);
+
+		total = ktime_set(0, 0);
+
+		for (i = 0; i < cstates_len; i++)
+			total = ktime_add(total, sh->states[i].time);
+
+		wall = ktime_sub(ktime_get(), sh->measure_begin);
+
+		total_us = ktime_to_us(wall);
+		total_s = ktime_to_ms(wall);
+
+		do_div(total_s, 1000);
+
+		if (verbose) {
+			if (total_s)
+				seq_printf(s,
+					   "wake ups per s: %u.%u \n",
+					   sh->exit_counter / (int) total_s,
+					   (10 * sh->exit_counter / (int) total_s) -
+					   10 * (sh->exit_counter / (int) total_s));
+
+			seq_printf(s,
+				   "\ndelta accounted vs wall clock: %lld us\n",
+				   ktime_to_us(ktime_sub(wall, total)));
+		}
+
+		for (i = 0; i < cstates_len; i++)
+			stats_disp_one(s, sh, total_us, i);
+
+		seq_printf(s, "\n");
+		spin_unlock_irqrestore(&dbg_lock, flags);
+	}
+	seq_printf(s, "\n");
+	return 0;
+}
+
+
+static int ap_family_show(struct seq_file *s, void *iter)
+{
+	int i;
+	u32 count = 0;
+	unsigned long flags;
+	struct state_history *sh;
+
+	sh = per_cpu(state_history, 0);
+	spin_lock_irqsave(&dbg_lock, flags);
+
+	for (i = 0 ; i < cstates_len; i++) {
+		if (cstates[i].state == (enum ci_pwrst)s->private)
+			count += sh->states[i].counter;
+	}
+
+	seq_printf(s, "%u\n", count);
+	spin_unlock_irqrestore(&dbg_lock, flags);
+
+	return 0;
+}
+
+static int deepest_state_open_file(struct inode *inode, struct file *file)
+{
+	return single_open(file, deepest_state_print, inode->i_private);
+}
+
+static int verbose_open_file(struct inode *inode, struct file *file)
+{
+	return single_open(file, verbose_read, inode->i_private);
+}
+
+static int stats_open_file(struct inode *inode, struct file *file)
+{
+	return single_open(file, stats_print, inode->i_private);
+}
+
+static int ap_family_open(struct inode *inode,
+			  struct file *file)
+{
+	return single_open(file, ap_family_show, inode->i_private);
+}
+
+static int wake_latency_open(struct inode *inode,
+			  struct file *file)
+{
+	return single_open(file, wake_latency_read, inode->i_private);
+}
+
+static const struct file_operations deepest_state_fops = {
+	.open = deepest_state_open_file,
+	.write = set_deepest_state,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations verbose_state_fops = {
+	.open = verbose_open_file,
+	.write = verbose_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations stats_fops = {
+	.open = stats_open_file,
+	.write = stats_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ap_family_fops = {
+	.open = ap_family_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations wake_latency_fops = {
+	.open = wake_latency_open,
+	.write = wake_latency_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static struct dentry *cpuidle_dir;
+
+static void __init setup_debugfs(void)
+{
+	cpuidle_dir = debugfs_create_dir("cpuidle", NULL);
+	if (IS_ERR_OR_NULL(cpuidle_dir))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("deepest_state",
+					       S_IWUGO | S_IRUGO, cpuidle_dir,
+					       NULL, &deepest_state_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("verbose",
+					       S_IWUGO | S_IRUGO, cpuidle_dir,
+					       NULL, &verbose_state_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("stats",
+					       S_IRUGO, cpuidle_dir, NULL,
+					       &stats_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_bool("dbg_console_enable",
+					       S_IWUGO | S_IRUGO, cpuidle_dir,
+					       &dbg_console_enable)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_bool("measure_latency",
+					       S_IWUGO | S_IRUGO, cpuidle_dir,
+					       &measure_latency)))
+		goto fail;
+
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("wake_latency",
+					       S_IWUGO | S_IRUGO, cpuidle_dir,
+					       NULL,
+					       &wake_latency_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("ap_idle", S_IRUGO,
+					       cpuidle_dir,
+					       (void *)CI_IDLE,
+					       &ap_family_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("ap_sleep", S_IRUGO,
+					       cpuidle_dir,
+					       (void *)CI_SLEEP,
+					       &ap_family_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("ap_deepidle", S_IRUGO,
+					       cpuidle_dir,
+					       (void *)CI_DEEP_IDLE,
+					       &ap_family_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("ap_deepsleep", S_IRUGO,
+					       cpuidle_dir,
+					       (void *)CI_DEEP_SLEEP,
+					       &ap_family_fops)))
+		goto fail;
+
+	return;
+fail:
+	debugfs_remove_recursive(cpuidle_dir);
+}
+
+#define __UART_BASE(soc, x)     soc##_UART##x##_BASE
+#define UART_BASE(soc, x)       __UART_BASE(soc, x)
+
+void __init ux500_ci_dbg_init(void)
+{
+	static const char clkname[] __initconst
+		= "uart" __stringify(CONFIG_UX500_DEBUG_UART);
+	unsigned long baseaddr;
+	int cpu;
+
+	struct state_history *sh;
+
+	cstates = ux500_ci_get_cstates(&cstates_len);
+
+	if (deepest_allowed_state > cstates_len)
+		deepest_allowed_state = cstates_len;
+
+	for_each_possible_cpu(cpu) {
+		per_cpu(state_history, cpu) = kzalloc(sizeof(struct state_history),
+						      GFP_KERNEL);
+		sh = per_cpu(state_history, cpu);
+		sh->states = kzalloc(sizeof(struct state_history_state)
+				     * cstates_len,
+				     GFP_KERNEL);
+	}
+
+	state_history_reset();
+
+	for_each_possible_cpu(cpu) {
+		sh = per_cpu(state_history, cpu);
+		/* Only first CPU used during boot */
+		if (cpu == 0)
+			sh->state = CI_RUNNING;
+		else
+			sh->state = CI_WFI;
+	}
+
+	setup_debugfs();
+
+	/* Uart debug init */
+
+	if (cpu_is_u8500())
+		baseaddr = UART_BASE(U8500, CONFIG_UX500_DEBUG_UART);
+	else if (cpu_is_u5500())
+		baseaddr = UART_BASE(U5500, CONFIG_UX500_DEBUG_UART);
+	else
+		ux500_unknown_soc();
+
+	uart_base = ioremap(baseaddr, SZ_4K);
+	BUG_ON(!uart_base);
+
+	uart_clk = clk_get_sys(clkname, NULL);
+	BUG_ON(IS_ERR(uart_clk));
+
+	INIT_DELAYED_WORK_DEFERRABLE(&cpuidle_work, dbg_cpuidle_work_function);
+
+}
+
+void ux500_ci_dbg_remove(void)
+{
+	int cpu;
+	struct state_history *sh;
+
+	debugfs_remove_recursive(cpuidle_dir);
+
+	for_each_possible_cpu(cpu) {
+		sh = per_cpu(state_history, cpu);
+		kfree(sh->states);
+		kfree(sh);
+	}
+
+	iounmap(uart_base);
+}
diff --git a/arch/arm/mach-ux500/pm/cpuidle_dbg.h b/arch/arm/mach-ux500/pm/cpuidle_dbg.h
new file mode 100644
index 00000000000..b660b039900
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/cpuidle_dbg.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010-2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Rickard Andersson <rickard.andersson@stericsson.com> for ST-Ericsson
+ *	   Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ */
+
+#ifndef CPUIDLE_DBG_H
+#define CPUIDLE_DBG_H
+
+#include "cpuidle.h"
+
+#ifdef CONFIG_U8500_CPUIDLE_DEBUG
+void ux500_ci_dbg_init(void);
+void ux500_ci_dbg_remove(void);
+
+void ux500_ci_dbg_log(int ctarget, ktime_t enter_time);
+void ux500_ci_dbg_wake_latency(int ctarget, int sleep_time);
+void ux500_ci_dbg_exit_latency(int ctarget, ktime_t now, ktime_t exit,
+			       ktime_t enter);
+
+void ux500_ci_dbg_register_reason(int idx, bool power_state_req,
+				  u32 sleep_time, u32 max_depth);
+
+bool ux500_ci_dbg_force_ape_on(void);
+int ux500_ci_dbg_deepest_state(void);
+
+void ux500_ci_dbg_console(void);
+void ux500_ci_dbg_console_check_uart(void);
+void ux500_ci_dbg_console_handle_ape_resume(void);
+void ux500_ci_dbg_console_handle_ape_suspend(void);
+
+#else
+
+static inline void ux500_ci_dbg_init(void) { }
+static inline void ux500_ci_dbg_remove(void) { }
+
+static inline void ux500_ci_dbg_log(int ctarget,
+				    ktime_t enter_time) { }
+
+static inline void ux500_ci_dbg_exit_latency(int ctarget,
+					     ktime_t now, ktime_t exit,
+					     ktime_t enter) { }
+static inline void ux500_ci_dbg_wake_latency(int ctarget, int sleep_time) { }
+
+
+static inline void ux500_ci_dbg_register_reason(int idx, bool power_state_req,
+						u32 sleep_time, u32 max_depth) { }
+
+static inline bool ux500_ci_dbg_force_ape_on(void)
+{
+	return false;
+}
+
+static inline int ux500_ci_dbg_deepest_state(void)
+{
+	/* This means no lower sleep state than ApIdle */
+	return CONFIG_U8500_CPUIDLE_DEEPEST_STATE;
+}
+
+static inline void ux500_ci_dbg_console(void) { }
+static inline void ux500_ci_dbg_console_check_uart(void) { }
+static inline void ux500_ci_dbg_console_handle_ape_resume(void) { }
+static inline void ux500_ci_dbg_console_handle_ape_suspend(void) { }
+
+#endif
+#endif
diff --git a/arch/arm/mach-ux500/pm/pm.c b/arch/arm/mach-ux500/pm/pm.c
new file mode 100644
index 00000000000..4617cff1594
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/pm.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Rickard Andersson <rickard.andersson@stericsson.com> for
+ *         ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+
+#include <linux/io.h>
+#include <linux/percpu.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
+#include <asm/processor.h>
+
+#include <asm/hardware/gic.h>
+
+#include <mach/hardware.h>
+#include <mach/prcmu-regs.h>
+
+#include "pm.h"
+
+#define STABILIZATION_TIME 30 /* us */
+
+#define PRCM_ARM_WFI_STANDBY_CPU0_WFI 0x8
+#define PRCM_ARM_WFI_STANDBY_CPU1_WFI 0x10
+
+static u32 u8500_gpio_banks[] = {U8500_GPIOBANK0_BASE,
+				 U8500_GPIOBANK1_BASE,
+				 U8500_GPIOBANK2_BASE,
+				 U8500_GPIOBANK3_BASE,
+				 U8500_GPIOBANK4_BASE,
+				 U8500_GPIOBANK5_BASE,
+				 U8500_GPIOBANK6_BASE,
+				 U8500_GPIOBANK7_BASE,
+				 U8500_GPIOBANK8_BASE};
+
+static u32 u5500_gpio_banks[] = {U5500_GPIOBANK0_BASE,
+				 U5500_GPIOBANK1_BASE,
+				 U5500_GPIOBANK2_BASE,
+				 U5500_GPIOBANK3_BASE,
+				 U5500_GPIOBANK4_BASE,
+				 U5500_GPIOBANK5_BASE,
+				 U5500_GPIOBANK6_BASE,
+				 U5500_GPIOBANK7_BASE};
+
+static u32 ux500_gpio_wks[ARRAY_SIZE(u8500_gpio_banks)];
+
+inline int ux500_pm_arm_on_ext_clk(bool leave_arm_pll_on)
+{
+	return 0;
+}
+
+/* Decouple GIC from the interrupt bus */
+void ux500_pm_gic_decouple(void)
+{
+	writel(readl(PRCM_A9_MASK_REQ) | PRCM_A9_MASK_REQ_PRCM_A9_MASK_REQ,
+		PRCM_A9_MASK_REQ);
+
+	while (!readl(PRCM_A9_MASK_REQ))
+		cpu_relax();
+
+	/* TODO: Use the ack bit when possible */
+}
+
+/* Recouple GIC with the interrupt bus */
+void ux500_pm_gic_recouple(void)
+{
+	writel((readl(PRCM_A9_MASK_REQ) & ~PRCM_A9_MASK_REQ_PRCM_A9_MASK_REQ),
+	       PRCM_A9_MASK_REQ);
+
+	/* TODO: Use the ack bit when possible */
+}
+
+#define GIC_NUMBER_REGS 5
+bool ux500_pm_gic_pending_interrupt(void)
+{
+	u32 pr; /* Pending register */
+	u32 er; /* Enable register */
+	int i;
+
+	/* 5 registers. STI & PPI not skipped */
+	for (i = 0; i < GIC_NUMBER_REGS; i++) {
+
+		pr = readl_relaxed(__io_address(U8500_GIC_DIST_BASE) +
+				   GIC_DIST_PENDING_SET + i * 4);
+		er = readl_relaxed(__io_address(U8500_GIC_DIST_BASE) +
+				   GIC_DIST_ENABLE_SET + i * 4);
+
+		if (pr & er)
+			return true; /* There is a pending interrupt */
+	}
+	return false;
+}
+
+#define GIC_NUMBER_SPI_REGS 4
+bool ux500_pm_prcmu_pending_interrupt(void)
+{
+	u32 it;
+	u32 im;
+	int i;
+
+	for (i = 0; i < GIC_NUMBER_SPI_REGS; i++) { /* There are 4 registers */
+
+		it = readl(PRCM_ARMITVAL31TO0 + i * 4);
+		im = readl(PRCM_ARMITMSK31TO0 + i * 4);
+
+		if (it & im)
+			return true; /* There is a pending interrupt */
+	}
+
+	return false;
+}
+
+void ux500_pm_prcmu_set_ioforce(bool enable)
+{
+	if (enable)
+		writel(readl(PRCM_IOCR) | PRCM_IOCR_IOFORCE, PRCM_IOCR);
+	else
+		writel(readl(PRCM_IOCR) & ~PRCM_IOCR_IOFORCE, PRCM_IOCR);
+}
+
+void ux500_pm_prcmu_copy_gic_settings(void)
+{
+	u32 er; /* Enable register */
+	int i;
+
+	for (i = 0; i < GIC_NUMBER_SPI_REGS; i++) { /* 4*32 SPI interrupts */
+		/* +1 due to skip STI and PPI */
+		er = readl_relaxed(__io_address(U8500_GIC_DIST_BASE) +
+			   GIC_DIST_ENABLE_SET + (i + 1) * 4);
+		writel(er, PRCM_ARMITMSK31TO0 + i * 4);
+	}
+}
+
+void ux500_pm_gpio_save_wake_up_status(void)
+{
+	int num_banks;
+	u32 *banks;
+	int i;
+
+	if (cpu_is_u5500()) {
+		num_banks = ARRAY_SIZE(u5500_gpio_banks);
+		banks = u5500_gpio_banks;
+	} else {
+		num_banks = ARRAY_SIZE(u8500_gpio_banks);
+		banks = u8500_gpio_banks;
+	}
+
+	for (i = 0; i < num_banks; i++)
+		ux500_gpio_wks[i] = readl(__io_address(banks[i]) + NMK_GPIO_WKS);
+}
+
+u32 ux500_pm_gpio_read_wake_up_status(unsigned int bank_num)
+{
+	if (WARN_ON(cpu_is_u5500() && bank_num >=
+		    ARRAY_SIZE(u5500_gpio_banks)))
+		return 0;
+
+	if (WARN_ON(cpu_is_u8500() && bank_num >=
+		    ARRAY_SIZE(u8500_gpio_banks)))
+		return 0;
+
+	return ux500_gpio_wks[bank_num];
+}
+
+/* Check if the other CPU is in WFI */
+bool ux500_pm_other_cpu_wfi(void)
+{
+	if (smp_processor_id()) {
+		/* We are CPU 1 => check if CPU0 is in WFI */
+		if (readl(PRCM_ARM_WFI_STANDBY) &
+		    PRCM_ARM_WFI_STANDBY_CPU0_WFI)
+			return true;
+	} else {
+		/* We are CPU 0 => check if CPU1 is in WFI */
+		if (readl(PRCM_ARM_WFI_STANDBY) &
+		    PRCM_ARM_WFI_STANDBY_CPU1_WFI)
+			return true;
+	}
+
+	return false;
+}
+
+/* PRCM_ACK_MB0_AP_PWRSTTR_STATUS */
+#define DB8500_PRCMU_STATUS_REGISTER	0x801b8e08
+#define DB5500_PRCMU_STATUS_REGISTER	0x80168f38
+
+enum prcmu_idle_stat ux500_pm_prcmu_idle_stat(void)
+{
+	u32 val;
+	void __iomem *prcmu_status_reg;
+
+	if (cpu_is_u8500())
+		prcmu_status_reg = __io_address(DB8500_PRCMU_STATUS_REGISTER);
+	else if (cpu_is_u5500())
+		prcmu_status_reg = __io_address(DB5500_PRCMU_STATUS_REGISTER);
+	else
+		ux500_unknown_soc();
+
+	val = readl(prcmu_status_reg) & 0xff;
+
+	return (enum prcmu_idle_stat)val;
+}
diff --git a/arch/arm/mach-ux500/pm/pm.h b/arch/arm/mach-ux500/pm/pm.h
new file mode 100644
index 00000000000..5448047b61e
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/pm.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Rickard Andersson <rickard.andersson@stericsson.com> for
+ *         ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ */
+
+#ifndef PM_COMMON_H
+#define PM_COMMON_H
+
+#ifdef CONFIG_PM
+
+enum prcmu_idle_stat {
+	SLEEP_OK		= 0xf3,
+	DEEP_SLEEP_OK		= 0xf6,
+	IDLE_OK			= 0xf0,
+	DEEPIDLE_OK		= 0xe3,
+	ARM2PRCMUPENDINGIT_ER	= 0x91,
+	HWACFCT_IN_SGA		= 0x93,
+};
+
+/**
+ * ux500_pm_gic_decouple()
+ *
+ * Decouple GIC from the interrupt bus.
+ */
+void ux500_pm_gic_decouple(void);
+
+/**
+ * ux500_pm_gic_recouple()
+ *
+ * Recouple GIC with the interrupt bus.
+ */
+void ux500_pm_gic_recouple(void);
+
+/**
+ * ux500_pm_gic_pending_interrupt()
+ *
+ * returns true, if there are pending interrupts.
+ */
+bool ux500_pm_gic_pending_interrupt(void);
+
+/**
+ * ux500_pm_prcmu_pending_interrupt()
+ *
+ * returns true, if there are pending interrupts.
+ */
+bool ux500_pm_prcmu_pending_interrupt(void);
+
+/**
+ * ux500_pm_prcmu_set_ioforce()
+ *
+ * @enable: Enable/disable
+ *
+ * Enable/disable the gpio-ring
+ */
+void ux500_pm_prcmu_set_ioforce(bool enable);
+
+/**
+ * ux500_pm_prcmu_copy_gic_settings()
+ *
+ * This function copies all the gic interrupt settings to the prcmu.
+ * This is needed for the system to catch interrupts in ApIdle
+ */
+void ux500_pm_prcmu_copy_gic_settings(void);
+
+/**
+ * ux500_pm_gpio_save_wake_up_status()
+ *
+ * This function is called when the prcmu has woken the ARM
+ * but before ioforce is disabled.
+ */
+void ux500_pm_gpio_save_wake_up_status(void);
+
+/**
+ * ux500_pm_gpio_read_wake_up_status()
+ *
+ * @bank_number: The gpio bank.
+ *
+ * Returns the WKS register settings for given bank number.
+ * The WKS register is cleared when ioforce is released therefore
+ * this function is needed.
+ */
+u32 ux500_pm_gpio_read_wake_up_status(unsigned int bank_number);
+
+/**
+ * ux500_pm_other_cpu_wfi()
+ *
+ * Returns true if the other CPU is in WFI.
+ */
+bool ux500_pm_other_cpu_wfi(void);
+
+/**
+ * ux500_pm_prcmu_idle_stat()
+ *
+ * Returns the status of the last prcmu idle/sleep
+ */
+enum prcmu_idle_stat ux500_pm_prcmu_idle_stat(void);
+
+struct dev_power_domain;
+extern struct dev_power_domain ux500_dev_power_domain;
+
+#else
+u32 ux500_pm_gpio_read_wake_up_status(unsigned int bank_number)
+{
+	return 0;
+}
+
+/**
+ * ux500_pm_prcmu_set_ioforce()
+ *
+ * @enable: Enable/disable
+ *
+ * Enable/disable the gpio-ring
+ */
+static inline void ux500_pm_prcmu_set_ioforce(bool enable) { }
+
+#endif
+
+#endif
diff --git a/arch/arm/mach-ux500/pm/runtime.c b/arch/arm/mach-ux500/pm/runtime.c
new file mode 100644
index 00000000000..265faf3085d
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/runtime.c
@@ -0,0 +1,310 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Rabin Vincent <rabin.vincent@stericsson> for ST-Ericsson
+ *
+ * Based on:
+ *  Runtime PM support code for SuperH Mobile ARM
+ *  Copyright (C) 2009-2010 Magnus Damm
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/pm_runtime.h>
+#include <linux/platform_device.h>
+#include <linux/amba/bus.h>
+#include <linux/clk.h>
+#include <plat/pincfg.h>
+#include <linux/regulator/dbx500-prcmu.h>
+
+#include "../pins.h"
+
+#ifdef CONFIG_PM_RUNTIME
+#define BIT_ONCE		0
+#define BIT_ACTIVE		1
+#define BIT_ENABLED	2
+
+struct pm_runtime_data {
+	unsigned long flags;
+	struct ux500_regulator *regulator;
+	struct ux500_pins *pins;
+};
+
+static void __devres_release(struct device *dev, void *res)
+{
+	struct pm_runtime_data *prd = res;
+
+	dev_dbg(dev, "__devres_release()\n");
+
+	if (test_bit(BIT_ENABLED, &prd->flags)) {
+		if (prd->pins)
+			ux500_pins_disable(prd->pins);
+		if (prd->regulator)
+			ux500_regulator_atomic_disable(prd->regulator);
+	}
+
+	if (test_bit(BIT_ACTIVE, &prd->flags)) {
+		if (prd->pins)
+			ux500_pins_put(prd->pins);
+		if (prd->regulator)
+			ux500_regulator_put(prd->regulator);
+	}
+}
+
+static struct pm_runtime_data *__to_prd(struct device *dev)
+{
+	return devres_find(dev, __devres_release, NULL, NULL);
+}
+
+static void platform_pm_runtime_init(struct device *dev,
+				     struct pm_runtime_data *prd)
+{
+	prd->pins = ux500_pins_get(dev_name(dev));
+
+	prd->regulator = ux500_regulator_get(dev);
+	if (IS_ERR(prd->regulator))
+		prd->regulator = NULL;
+
+	if (prd->pins || prd->regulator) {
+		dev_info(dev, "managed by runtime pm: %s%s\n",
+			 prd->pins ? "pins " : "",
+			 prd->regulator ? "regulator " : "");
+
+		set_bit(BIT_ACTIVE, &prd->flags);
+	}
+}
+
+static void platform_pm_runtime_bug(struct device *dev,
+				    struct pm_runtime_data *prd)
+{
+	if (prd && !test_and_set_bit(BIT_ONCE, &prd->flags))
+		dev_err(dev, "runtime pm suspend before resume\n");
+}
+
+static void platform_pm_runtime_used(struct device *dev,
+				       struct pm_runtime_data *prd)
+{
+	if (prd)
+		set_bit(BIT_ONCE, &prd->flags);
+}
+
+static int ux500_pd_runtime_suspend(struct device *dev)
+{
+	struct pm_runtime_data *prd = __to_prd(dev);
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	platform_pm_runtime_bug(dev, prd);
+
+	if (prd && test_bit(BIT_ACTIVE, &prd->flags)) {
+		if (prd->pins)
+			ux500_pins_disable(prd->pins);
+
+		if (prd->regulator)
+			ux500_regulator_atomic_disable(prd->regulator);
+
+		clear_bit(BIT_ENABLED, &prd->flags);
+	}
+
+	return 0;
+}
+
+static int ux500_pd_runtime_resume(struct device *dev)
+{
+	struct pm_runtime_data *prd = __to_prd(dev);
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	platform_pm_runtime_used(dev, prd);
+
+	if (prd && test_bit(BIT_ACTIVE, &prd->flags)) {
+		if (prd->pins)
+			ux500_pins_enable(prd->pins);
+
+		if (prd->regulator)
+			ux500_regulator_atomic_enable(prd->regulator);
+
+		set_bit(BIT_ENABLED, &prd->flags);
+	}
+
+	return 0;
+}
+
+static int ux500_pd_suspend_noirq(struct device *dev)
+{
+	struct pm_runtime_data *prd = __to_prd(dev);
+	int ret;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	/* Only handle devices that use runtime pm */
+	if (!prd || !test_bit(BIT_ONCE, &prd->flags))
+		return 0;
+
+	/* Already is runtime suspended?  Nothing to do. */
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	/*
+	 * We get here only if the device was not runtime suspended for some
+	 * reason.  We still need to do the power save stuff when going into
+	 * suspend, so force it here.
+	 */
+	ret = pm_generic_runtime_suspend(dev);
+	if (ret)
+		return ret;
+
+	return ux500_pd_runtime_suspend(dev);
+}
+
+static int ux500_pd_resume_noirq(struct device *dev)
+{
+	struct pm_runtime_data *prd = __to_prd(dev);
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	/* Only handle devices that use runtime pm */
+	if (!prd || !test_bit(BIT_ONCE, &prd->flags))
+		return 0;
+
+	/*
+	 * Already was runtime suspended?  No need to resume here, runtime
+	 * resume will take care of it.
+	 */
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	/*
+	 * We get here only if the device was not runtime suspended,
+	 * but we forced it down in suspend_noirq above.  Bring it
+	 * up since pm-runtime thinks it is not suspended.
+	 */
+	ux500_pd_runtime_resume(dev);
+
+	return pm_generic_runtime_resume(dev);
+}
+
+static int ux500_pd_bus_notify(struct notifier_block *nb,
+			       unsigned long action, void *data)
+{
+	struct device *dev = data;
+	struct pm_runtime_data *prd;
+
+	dev_dbg(dev, "%s() %ld !\n", __func__, action);
+
+	if (action == BUS_NOTIFY_BIND_DRIVER) {
+		prd = devres_alloc(__devres_release, sizeof(*prd), GFP_KERNEL);
+		if (prd) {
+			devres_add(dev, prd);
+			platform_pm_runtime_init(dev, prd);
+		} else
+			dev_err(dev, "unable to alloc memory for runtime pm\n");
+	}
+
+	return 0;
+}
+
+#else /* CONFIG_PM_RUNTIME */
+
+#define ux500_pd_suspend_noirq	NULL
+#define ux500_pd_resume_noirq	NULL
+#define ux500_pd_runtime_suspend	NULL
+#define ux500_pd_runtime_resume	NULL
+
+static int ux500_pd_bus_notify(struct notifier_block *nb,
+			       unsigned long action, void *data)
+{
+	struct ux500_regulator *regulator = NULL;
+	struct ux500_pins *pins = NULL;
+	struct device *dev = data;
+	const char *onoff = NULL;
+
+	dev_dbg(dev, "%s() %ld !\n", __func__, action);
+
+	switch (action) {
+	case BUS_NOTIFY_BIND_DRIVER:
+		pins = ux500_pins_get(dev_name(dev));
+		if (pins) {
+			ux500_pins_enable(pins);
+			ux500_pins_put(pins);
+		}
+
+		regulator = ux500_regulator_get(dev);
+		if (IS_ERR(regulator))
+			regulator = NULL;
+		else {
+			ux500_regulator_atomic_enable(regulator);
+			ux500_regulator_put(regulator);
+		}
+
+		onoff = "on";
+		break;
+	case BUS_NOTIFY_UNBOUND_DRIVER:
+		pins = ux500_pins_get(dev_name(dev));
+		if (pins) {
+			ux500_pins_disable(pins);
+			ux500_pins_put(pins);
+		}
+
+		regulator = ux500_regulator_get(dev);
+		if (IS_ERR(regulator))
+			regulator = NULL;
+		else {
+			ux500_regulator_atomic_disable(regulator);
+			ux500_regulator_put(regulator);
+		}
+
+		onoff = "off";
+		break;
+	}
+
+	if (pins || regulator) {
+		dev_info(dev, "runtime pm disabled, forced %s: %s%s\n",
+			 onoff,
+			 pins ? "pins " : "",
+			 regulator ? "regulator " : "");
+	}
+
+	return 0;
+}
+
+#endif /* CONFIG_PM_RUNTIME */
+
+struct dev_power_domain ux500_dev_power_domain = {
+	.ops = {
+		.suspend_noirq		= ux500_pd_suspend_noirq,
+		.resume_noirq		= ux500_pd_resume_noirq,
+		.runtime_suspend	= ux500_pd_runtime_suspend,
+		.runtime_resume		= ux500_pd_runtime_resume,
+	},
+};
+
+static struct notifier_block ux500_pd_platform_notifier = {
+	.notifier_call = ux500_pd_bus_notify,
+};
+
+static struct notifier_block ux500_pd_amba_notifier = {
+	.notifier_call = ux500_pd_bus_notify,
+};
+
+static int __init ux500_pm_runtime_platform_init(void)
+{
+	bus_register_notifier(&platform_bus_type, &ux500_pd_platform_notifier);
+	return 0;
+}
+core_initcall(ux500_pm_runtime_platform_init);
+
+/*
+ * The amba bus itself gets registered in a core_initcall, so we can't use
+ * that.
+ */
+static int __init ux500_pm_runtime_amba_init(void)
+{
+	bus_register_notifier(&amba_bustype, &ux500_pd_amba_notifier);
+	return 0;
+}
+arch_initcall(ux500_pm_runtime_amba_init);
diff --git a/arch/arm/mach-ux500/pm/suspend.c b/arch/arm/mach-ux500/pm/suspend.c
new file mode 100644
index 00000000000..35bce047754
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/suspend.c
@@ -0,0 +1,226 @@
+/*
+ * Copyright (C) STMicroelectronics 2009
+ * Copyright (C) ST-Ericsson SA 2010-2011
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Authors: Rickard Andersson <rickard.andersson@stericsson.com>,
+ *	    Jonas Aaberg <jonas.aberg@stericsson.com>,
+ *          Sundar Iyer for ST-Ericsson.
+ *
+ */
+
+#include <linux/suspend.h>
+#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
+#include <linux/delay.h>
+#include <linux/regulator/ab8500-debug.h>
+
+#include <mach/prcmu.h>
+#include <mach/prcmu-regs.h>
+
+#include "context.h"
+#include "pm.h"
+#include "suspend_dbg.h"
+
+static void (*pins_suspend_force)(void);
+static void (*pins_suspend_force_mux)(void);
+
+void suspend_set_pins_force_fn(void (*force)(void), void (*force_mux)(void))
+{
+	pins_suspend_force = force;
+	pins_suspend_force_mux = force_mux;
+}
+
+static atomic_t block_sleep = ATOMIC_INIT(0);
+
+void suspend_block_sleep(void)
+{
+	atomic_inc(&block_sleep);
+}
+
+void suspend_unblock_sleep(void)
+{
+	atomic_dec(&block_sleep);
+}
+
+static bool sleep_is_blocked(void)
+{
+	return (atomic_read(&block_sleep) != 0);
+}
+
+static int suspend(bool do_deepsleep)
+{
+	bool pins_force = pins_suspend_force_mux && pins_suspend_force;
+	int ret = 0;
+
+	if (sleep_is_blocked()) {
+		pr_info("suspend/resume: interrupted by modem.\n");
+		return -EBUSY;
+	}
+
+	ux500_suspend_dbg_add_wake_on_uart();
+	nmk_gpio_wakeups_suspend();
+
+	/* configure the prcm for a sleep wakeup */
+	prcmu_enable_wakeups(PRCMU_WAKEUP(ABB));
+
+	context_vape_save();
+
+	if (pins_force) {
+		/*
+		 * Save GPIO settings before applying power save
+		 * settings
+		 */
+		context_gpio_save();
+
+		/* Apply GPIO power save mux settings */
+		context_gpio_mux_safe_switch(true);
+		pins_suspend_force_mux();
+		context_gpio_mux_safe_switch(false);
+
+		/* Apply GPIO power save settings */
+		pins_suspend_force();
+	}
+
+	ux500_pm_gic_decouple();
+
+	/* TODO: decouple gic should look at status bit.*/
+	udelay(100);
+
+	if (ux500_pm_gic_pending_interrupt()) {
+		pr_info("suspend/resume: pending interrupt\n");
+
+		/* Recouple GIC with the interrupt bus */
+		ux500_pm_gic_recouple();
+		ret = -EBUSY;
+
+		goto exit;
+	}
+	ux500_pm_prcmu_set_ioforce(true);
+
+	if (do_deepsleep) {
+		context_varm_save_common();
+		context_varm_save_core();
+		context_save_cpu_registers();
+
+		/*
+		 * Due to we have only 100us between requesting a powerstate
+		 * and wfi, we clean the cache before as well to assure the
+		 * final cache clean before wfi has as little as possible to
+		 * do.
+		 */
+		context_clean_l1_cache_all();
+
+		(void) prcmu_set_power_state(PRCMU_AP_DEEP_SLEEP,
+					     false, false);
+		context_save_to_sram_and_wfi(true, true);
+
+		context_restore_cpu_registers();
+		context_varm_restore_core();
+		context_varm_restore_common();
+
+	} else {
+
+		context_clean_l1_cache_all();
+		(void) prcmu_set_power_state(APEXECUTE_TO_APSLEEP,
+					     false, false);
+		dsb();
+		__asm__ __volatile__("wfi\n\t" : : : "memory");
+	}
+
+	context_vape_restore();
+
+	/* If GPIO woke us up then save the pins that caused the wake up */
+	ux500_pm_gpio_save_wake_up_status();
+
+	ux500_suspend_dbg_sleep_status(do_deepsleep);
+
+	/* APE was turned off, restore IO ring */
+	ux500_pm_prcmu_set_ioforce(false);
+
+exit:
+	if (pins_force) {
+		/* Restore gpio settings */
+		context_gpio_mux_safe_switch(true);
+		context_gpio_restore_mux();
+		context_gpio_mux_safe_switch(false);
+		context_gpio_restore();
+	}
+
+	/* This is what cpuidle wants */
+	prcmu_enable_wakeups(PRCMU_WAKEUP(ARM) | PRCMU_WAKEUP(RTC) |
+			     PRCMU_WAKEUP(ABB));
+
+	nmk_gpio_wakeups_resume();
+	ux500_suspend_dbg_remove_wake_on_uart();
+
+	return ret;
+}
+
+static int ux500_suspend_enter(suspend_state_t state)
+{
+
+	if (ux500_suspend_enabled()) {
+		if (ux500_suspend_deepsleep_enabled() &&
+		    state == PM_SUSPEND_MEM)
+			return suspend(true);
+		if (ux500_suspend_sleep_enabled())
+			return suspend(false);
+	}
+
+	ux500_suspend_dbg_add_wake_on_uart();
+	/*
+	 * Set IOFORCE in order to wake on GPIO the same way
+	 * as in deeper sleep.
+	 * (U5500 is not ready for IOFORCE)
+	 */
+	if (!cpu_is_u5500())
+		ux500_pm_prcmu_set_ioforce(true);
+
+	dsb();
+	__asm__ __volatile__("wfi\n\t" : : : "memory");
+
+	if (!cpu_is_u5500())
+		ux500_pm_prcmu_set_ioforce(false);
+	ux500_suspend_dbg_remove_wake_on_uart();
+
+	return 0;
+}
+
+static int ux500_suspend_valid(suspend_state_t state)
+{
+	return state == PM_SUSPEND_MEM || state == PM_SUSPEND_STANDBY;
+}
+
+static int ux500_suspend_prepare_late(void)
+{
+	/* ESRAM to retention instead of OFF until ROM is fixed */
+	(void)prcmu_config_esram0_deep_sleep(ESRAM0_DEEP_SLEEP_STATE_RET);
+	ab8500_regulator_debug_force();
+
+	return 0;
+}
+
+static void ux500_suspend_wake(void)
+{
+	ab8500_regulator_debug_restore();
+	(void)prcmu_config_esram0_deep_sleep(ESRAM0_DEEP_SLEEP_STATE_RET);
+}
+
+static struct platform_suspend_ops ux500_suspend_ops = {
+	.enter	      = ux500_suspend_enter,
+	.valid	      = ux500_suspend_valid,
+	.prepare_late = ux500_suspend_prepare_late,
+	.wake	      = ux500_suspend_wake,
+	.begin	      = ux500_suspend_dbg_begin,
+};
+
+static __init int ux500_suspend_init(void)
+{
+	ux500_suspend_dbg_init();
+	suspend_set_ops(&ux500_suspend_ops);
+	return 0;
+}
+
+device_initcall(ux500_suspend_init);
diff --git a/arch/arm/mach-ux500/pm/suspend_dbg.c b/arch/arm/mach-ux500/pm/suspend_dbg.c
new file mode 100644
index 00000000000..ff6bb829539
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/suspend_dbg.c
@@ -0,0 +1,187 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Author: Rickard Andersson <rickard.andersson@stericsson.com>,
+ *	   Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ *
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/suspend.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+
+#include "pm.h"
+
+#ifdef CONFIG_UX500_SUSPEND_STANDBY
+static u32 sleep_enabled = 1;
+#else
+static u32 sleep_enabled;
+#endif
+
+#ifdef CONFIG_UX500_SUSPEND_MEM
+static u32 deepsleep_enabled = 1;
+#else
+static u32 deepsleep_enabled;
+#endif
+
+static u32 suspend_enabled = 1;
+
+static u32 deepsleeps_done;
+static u32 deepsleeps_failed;
+static u32 sleeps_done;
+static u32 sleeps_failed;
+static u32 suspend_count;
+
+#ifdef CONFIG_UX500_SUSPEND_DBG_WAKE_ON_UART
+void ux500_suspend_dbg_add_wake_on_uart(void)
+{
+	set_irq_wake(NOMADIK_GPIO_TO_IRQ(CONFIG_UX500_CONSOLE_UART_GPIO_PIN), 1);
+	set_irq_type(NOMADIK_GPIO_TO_IRQ(CONFIG_UX500_CONSOLE_UART_GPIO_PIN),
+		     IRQ_TYPE_EDGE_BOTH);
+}
+
+void ux500_suspend_dbg_remove_wake_on_uart(void)
+{
+	set_irq_wake(NOMADIK_GPIO_TO_IRQ(CONFIG_UX500_CONSOLE_UART_GPIO_PIN), 0);
+}
+#endif
+
+bool ux500_suspend_enabled(void)
+{
+	return suspend_enabled != 0;
+}
+
+bool ux500_suspend_sleep_enabled(void)
+{
+	return sleep_enabled != 0;
+}
+
+bool ux500_suspend_deepsleep_enabled(void)
+{
+	return deepsleep_enabled != 0;
+}
+
+void ux500_suspend_dbg_sleep_status(bool is_deepsleep)
+{
+	enum prcmu_idle_stat prcmu_status;
+
+	prcmu_status = ux500_pm_prcmu_idle_stat();
+
+	if (is_deepsleep) {
+		pr_info("Returning from ApDeepSleep. PRCMU ret: 0x%x - %s\n",
+			prcmu_status,
+			prcmu_status == DEEP_SLEEP_OK ? "Success" : "Fail!");
+		if (prcmu_status == DEEP_SLEEP_OK)
+			deepsleeps_done++;
+		else
+			deepsleeps_failed++;
+	} else {
+		pr_info("Returning from ApSleep. PRCMU ret: 0x%x - %s\n",
+			prcmu_status,
+			prcmu_status == SLEEP_OK ? "Success" : "Fail!");
+		if (prcmu_status == SLEEP_OK)
+			sleeps_done++;
+		else
+			sleeps_failed++;
+	}
+}
+
+int ux500_suspend_dbg_begin(suspend_state_t state)
+{
+	suspend_count++;
+	return 0;
+}
+
+void ux500_suspend_dbg_init(void)
+{
+	struct dentry *suspend_dir = NULL;
+	struct dentry *sleep_file = NULL;
+	struct dentry *deepsleep_file = NULL;
+	struct dentry *enable_file = NULL;
+	struct dentry *suspend_count_file = NULL;
+	struct dentry *sleeps_done_file = NULL;
+	struct dentry *deepsleeps_done_file = NULL;
+	struct dentry *sleeps_failed_file = NULL;
+	struct dentry *deepsleeps_failed_file = NULL;
+
+	suspend_dir = debugfs_create_dir("suspend", NULL);
+	if (IS_ERR_OR_NULL(suspend_dir))
+		return;
+
+	sleep_file = debugfs_create_bool("sleep", S_IWUGO | S_IRUGO,
+					 suspend_dir,
+					 &sleep_enabled);
+	if (IS_ERR_OR_NULL(sleep_file))
+		goto error;
+
+	deepsleep_file = debugfs_create_bool("deepsleep", S_IWUGO | S_IRUGO,
+					     suspend_dir,
+					     &deepsleep_enabled);
+	if (IS_ERR_OR_NULL(deepsleep_file))
+		goto error;
+
+	enable_file = debugfs_create_bool("enable", S_IWUGO | S_IRUGO,
+					  suspend_dir,
+					  &suspend_enabled);
+	if (IS_ERR_OR_NULL(enable_file))
+		goto error;
+
+	suspend_count_file = debugfs_create_u32("count", S_IRUGO,
+						suspend_dir,
+						&suspend_count);
+	if (IS_ERR_OR_NULL(suspend_count_file))
+		goto error;
+
+	sleeps_done_file = debugfs_create_u32("sleep_count", S_IRUGO,
+					      suspend_dir,
+					      &sleeps_done);
+	if (IS_ERR_OR_NULL(sleeps_done_file))
+		goto error;
+
+	deepsleeps_done_file = debugfs_create_u32("deepsleep_count", S_IRUGO,
+						  suspend_dir,
+						  &deepsleeps_done);
+	if (IS_ERR_OR_NULL(deepsleeps_done_file))
+		goto error;
+
+
+	sleeps_failed_file = debugfs_create_u32("sleep_failed", S_IRUGO,
+						suspend_dir,
+						&sleeps_failed);
+	if (IS_ERR_OR_NULL(sleeps_failed_file))
+		goto error;
+
+	deepsleeps_failed_file = debugfs_create_u32("deepsleep_failed", S_IRUGO,
+						    suspend_dir,
+						    &deepsleeps_failed);
+	if (IS_ERR_OR_NULL(deepsleeps_failed_file))
+		goto error;
+
+	return;
+error:
+	if (!IS_ERR_OR_NULL(deepsleeps_failed_file))
+		debugfs_remove(deepsleeps_failed_file);
+	if (!IS_ERR_OR_NULL(sleeps_failed_file))
+		debugfs_remove(sleeps_failed_file);
+	if (!IS_ERR_OR_NULL(deepsleeps_done_file))
+		debugfs_remove(deepsleeps_done_file);
+	if (!IS_ERR_OR_NULL(sleeps_done_file))
+		debugfs_remove(sleeps_done_file);
+	if (!IS_ERR_OR_NULL(suspend_count_file))
+		debugfs_remove(suspend_count_file);
+	if (!IS_ERR_OR_NULL(enable_file))
+		debugfs_remove(enable_file);
+	if (!IS_ERR_OR_NULL(deepsleep_file))
+		debugfs_remove(deepsleep_file);
+	if (!IS_ERR_OR_NULL(sleep_file))
+		debugfs_remove(sleep_file);
+
+	debugfs_remove(suspend_dir);
+}
diff --git a/arch/arm/mach-ux500/pm/suspend_dbg.h b/arch/arm/mach-ux500/pm/suspend_dbg.h
new file mode 100644
index 00000000000..46cbe6e7014
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/suspend_dbg.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ *
+ */
+
+#ifndef UX500_SUSPEND_DBG_H
+#define UX500_SUSPEND_DBG_H
+
+#include <linux/kernel.h>
+#include <linux/suspend.h>
+
+#ifdef CONFIG_UX500_SUSPEND_DBG_WAKE_ON_UART
+void ux500_suspend_dbg_add_wake_on_uart(void);
+void ux500_suspend_dbg_remove_wake_on_uart(void);
+#else
+static inline void ux500_suspend_dbg_add_wake_on_uart(void) { }
+static inline void ux500_suspend_dbg_remove_wake_on_uart(void) { }
+#endif
+
+#ifdef CONFIG_UX500_SUSPEND_DBG
+bool ux500_suspend_enabled(void);
+bool ux500_suspend_sleep_enabled(void);
+bool ux500_suspend_deepsleep_enabled(void);
+void ux500_suspend_dbg_sleep_status(bool is_deepsleep);
+void ux500_suspend_dbg_init(void);
+int ux500_suspend_dbg_begin(suspend_state_t state);
+
+#else
+static inline bool ux500_suspend_enabled(void)
+{
+	return true;
+}
+static inline bool ux500_suspend_sleep_enabled(void)
+{
+#ifdef CONFIG_UX500_SUSPEND_STANDBY
+	return true;
+#else
+	return false;
+#endif
+}
+static inline bool ux500_suspend_deepsleep_enabled(void)
+{
+#ifdef CONFIG_UX500_SUSPEND_MEM
+	return true;
+#else
+	return false;
+#endif
+}
+static inline void ux500_suspend_dbg_sleep_status(bool is_deepsleep) { }
+static inline void ux500_suspend_dbg_init(void) { }
+
+#define ux500_suspend_dbg_begin NULL
+
+#endif
+
+#endif
diff --git a/arch/arm/mach-ux500/pm/timer.c b/arch/arm/mach-ux500/pm/timer.c
new file mode 100644
index 00000000000..76c590a420f
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/timer.c
@@ -0,0 +1,197 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010-2011
+ *
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * The RTC timer block is a ST Microelectronics variant of ARM PL031.
+ * Clockwatch part is the same as PL031, while the timer part is only
+ * present on the ST Microelectronics variant.
+ * Here only the timer part is used.
+ *
+ * The timer part is quite troublesome to program correctly. Lots
+ * of long delays must be there in order to secure that you actually get what
+ * you wrote.
+ *
+ * In other words, this timer is and should only used from cpuidle during
+ * special conditions when the surroundings are know in order to be able
+ * to remove the number of delays.
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/ktime.h>
+#include <linux/delay.h>
+
+#include <asm/errno.h>
+
+#include <mach/hardware.h>
+
+#include "cpuidle_dbg.h"
+
+#define RTC_IMSC	0x10
+#define RTC_MIS		0x18
+#define RTC_ICR		0x1C
+#define RTC_TDR		0x20
+#define	RTC_TLR1	0x24
+#define RTC_TCR		0x28
+
+#define RTC_TLR2	0x2C
+#define RTC_TPR1	0x3C
+
+#define RTC_TCR_RTTOS	(1 << 0)
+#define RTC_TCR_RTTEN	(1 << 1)
+#define RTC_TCR_RTTSS	(1 << 2)
+
+#define RTC_IMSC_TIMSC	(1 << 1)
+#define RTC_ICR_TIC	(1 << 1)
+#define RTC_MIS_RTCTMIS	(1 << 1)
+
+#define RTC_TCR_RTTPS_2	(1 << 4)
+#define RTC_TCR_RTTPS_3	(2 << 4)
+#define RTC_TCR_RTTPS_4	(3 << 4)
+#define RTC_TCR_RTTPS_5	(4 << 4)
+#define RTC_TCR_RTTPS_6	(5 << 4)
+#define RTC_TCR_RTTPS_7	(6 << 4)
+#define RTC_TCR_RTTPS_8	(7 << 4)
+
+#define WRITE_DELAY 130 /* 4 cycles plus margin */
+
+/*
+ * Count down measure point. It just have to be high to differ
+ * from scheduled values.
+ */
+#define MEASURE_VAL 0xffffffff
+
+/* Just a value bigger than any reason able scheduled timeout. */
+#define MEASURE_VAL_LIMIT 0xf0000000
+
+
+#define TICKS_TO_NS(x) ((s64)x * 30512)
+#define US_TO_TICKS(x) ((u32)((1000 * x) / 30512))
+
+static void __iomem *rtc_base;
+
+#ifdef CONFIG_U8500_CPUIDLE_DEBUG
+
+static bool measure_latency;
+
+/*
+ * The plan here is to be able to measure the ApSleep/ApDeepSleep exit latency
+ * by having a know timer pattern.
+ * The first entry in the pattern, LR1, is the value that the scheduler
+ * wants us to sleep. The second pattern in a high value, too large to be
+ * scheduled, so we can differ between a running scheduled value and a
+ * time measure value.
+ * When a RTT interrupt has occured, the block will automatically start
+ * to execute the measure value in LR2 and when the ARM is awake, it reads
+ * how far the RTT has decreased the value loaded from LR2 and from that
+ * calculate how long time it took to wake up.
+ */
+ktime_t u8500_rtc_exit_latency_get(void)
+{
+	u32 ticks;
+
+	if (measure_latency) {
+		ticks = MEASURE_VAL - readl(rtc_base + RTC_TDR);
+
+		/*
+		 * Check if we are actually counting on a LR2 value.
+		 * If not we have woken on another interrupt.
+		 */
+		if (ticks < MEASURE_VAL_LIMIT) {
+			/* convert 32 kHz ticks to ns */
+			return ktime_set(0, TICKS_TO_NS(ticks));
+		}
+	}
+	return ktime_set(0, 0);
+}
+
+static void measure_latency_start(void)
+{
+	udelay(WRITE_DELAY);
+	/*
+	 * Disable RTT and clean self-start due to we want to restart,
+	 * not continue from current pattern. (See below)
+	 */
+	writel(0, rtc_base + RTC_TCR);
+	udelay(WRITE_DELAY);
+
+	/*
+	 * Program LR2 (load register two) to maximum value to ease
+	 * identification of timer interrupt vs other.
+	 */
+	writel(MEASURE_VAL, rtc_base + RTC_TLR2);
+	/*
+	 * Set Load Register execution pattern, bit clear
+	 * means pick LR1, bit set means LR2
+	 * 0xfe, binary 11111110 means first do LR1 then do
+	 * LR2 seven times
+	 */
+	writel(0xfe, rtc_base + RTC_TPR1);
+
+	udelay(WRITE_DELAY);
+
+	/*
+	 * Enable self-start, plus a pattern of eight.
+	 */
+	writel(RTC_TCR_RTTSS | RTC_TCR_RTTPS_8,
+	       rtc_base + RTC_TCR);
+	udelay(WRITE_DELAY);
+}
+
+void ux500_rtcrtt_measure_latency(bool enable)
+{
+	if (enable) {
+		measure_latency_start();
+	} else {
+		writel(RTC_TCR_RTTSS | RTC_TCR_RTTOS, rtc_base + RTC_TCR);
+		writel(RTC_ICR_TIC, rtc_base + RTC_ICR);
+		writel(RTC_IMSC_TIMSC, rtc_base + RTC_IMSC);
+	}
+	measure_latency = enable;
+}
+#else
+static inline void measure_latency_start(void) { }
+static inline void ux500_rtcrtt_measure_latency(bool enable) { }
+#endif
+
+void ux500_rtcrtt_off(void)
+{
+#ifdef CONFIG_U8500_CPUIDLE_DEBUG
+	if (measure_latency) {
+		measure_latency_start();
+	} else
+#endif
+	{
+		/* Clear eventual interrupts */
+		if (readl(rtc_base + RTC_MIS) & RTC_MIS_RTCTMIS)
+			writel(RTC_ICR_TIC, rtc_base + RTC_ICR);
+
+		/* Disable, self start and oneshot mode */
+		writel(RTC_TCR_RTTSS | RTC_TCR_RTTOS, rtc_base + RTC_TCR);
+	}
+}
+
+void ux500_rtcrtt_next(u32 time_us)
+{
+	writel(US_TO_TICKS(time_us), rtc_base + RTC_TLR1);
+}
+
+static int __init ux500_rtcrtt_init(void)
+{
+	if (cpu_is_u8500()) {
+		rtc_base  = __io_address(U8500_RTC_BASE);
+	} else if (cpu_is_u5500()) {
+		rtc_base  = __io_address(U5500_RTC_BASE);
+	} else {
+		pr_err("timer-rtt: Unknown DB Asic!\n");
+		return -EINVAL;
+	}
+#ifdef CONFIG_U8500_CPUIDLE_DEBUG
+	ux500_rtcrtt_measure_latency(false);
+#endif
+	return 0;
+}
+subsys_initcall(ux500_rtcrtt_init);
diff --git a/arch/arm/mach-ux500/pm/timer.h b/arch/arm/mach-ux500/pm/timer.h
new file mode 100644
index 00000000000..ec9e919e70d
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/timer.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com>
+ *
+ * License Terms: GNU General Public License v2
+ *
+ */
+
+#ifndef PM_TIMER_H
+#define  PM_TIMER_H
+
+#include <linux/ktime.h>
+
+#ifdef CONFIG_U8500_CPUIDLE_DEBUG
+ktime_t u8500_rtc_exit_latency_get(void);
+void ux500_rtcrtt_measure_latency(bool enable);
+#else
+static inline ktime_t u8500_rtc_exit_latency_get(void)
+{
+	return ktime_set(0, 0);
+}
+static inline void ux500_rtcrtt_measure_latency(bool enable) { }
+
+#endif
+
+void ux500_rtcrtt_off(void);
+void ux500_rtcrtt_next(u32 time_us);
+
+#endif
diff --git a/arch/arm/mach-ux500/pm/usecase_gov.c b/arch/arm/mach-ux500/pm/usecase_gov.c
new file mode 100644
index 00000000000..78f157d7f60
--- /dev/null
+++ b/arch/arm/mach-ux500/pm/usecase_gov.c
@@ -0,0 +1,885 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Alexandre Torgue <alexandre.torgue@stericsson.com> for ST-Ericsson
+ * Author: Vincent Guittot <vincent.guittot@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/io.h>
+#include <linux/earlysuspend.h>
+#include <linux/cpu.h>
+#include <linux/sched.h>
+#include <linux/tick.h>
+#include <linux/workqueue.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <linux/kernel_stat.h>
+#include <linux/ktime.h>
+#include <linux/cpufreq.h>
+#include <mach/prcmu.h>
+
+#define CPULOAD_MEAS_DELAY	3000 /* 3 secondes of delta */
+
+/* debug */
+static unsigned long debug;
+
+#define hp_printk \
+	if (debug) \
+		printk \
+
+enum ux500_uc {
+	UX500_UC_NORMAL	=  0,
+	UX500_UC_AUTO, /* Add use case below this. */
+	UX500_UC_VC,
+	UX500_UC_LPA,
+	UX500_UC_USER, /* Add use case above this. */
+	UX500_UC_MAX,
+};
+
+/* cpu load monitor struct */
+#define LOAD_MONITOR 4
+struct hotplug_cpu_info {
+	cputime64_t prev_cpu_wall;
+	cputime64_t prev_cpu_idle;
+	cputime64_t prev_cpu_io;
+	unsigned int load[LOAD_MONITOR];
+	unsigned int io[LOAD_MONITOR];
+	unsigned int idx;
+};
+
+static DEFINE_PER_CPU(struct hotplug_cpu_info, hotplug_info);
+
+/* Auto trigger criteria */
+/* loadavg threshold */
+static unsigned long lower_threshold = 175;
+static unsigned long upper_threshold = 450;
+/* load balancing */
+static unsigned long max_unbalance = 210;
+/* trend load */
+static unsigned long trend_unbalance = 40;
+static unsigned long min_trend = 5;
+/* instant load */
+static unsigned long max_instant = 85;
+
+/* Number of interrupts per second before exiting auto mode */
+static u32 exit_irq_per_s = 1000;
+static u64 old_num_irqs;
+
+static DEFINE_MUTEX(user_config_mutex);
+static bool user_config_updated;
+static enum ux500_uc current_uc = UX500_UC_MAX;
+static bool is_work_scheduled;
+static bool is_early_suspend;
+
+static unsigned int cpuidle_deepest_state;
+
+struct usecase_config {
+	char *name;
+	unsigned long max_freq;
+	unsigned long min_freq; /* if no requirement set 0 */
+	unsigned long cpuidle_multiplier;
+	bool second_cpu_online;
+	bool l2_prefetch_en;
+	bool enable;
+	unsigned int forced_state; /* Forced cpu idle state. */
+	bool vc_override; /* QOS override for voice-call. */
+};
+
+static struct usecase_config usecase_conf[UX500_UC_MAX] = {
+	[UX500_UC_NORMAL] = {
+		.name			= "normal",
+		.max_freq		= 1000000,
+		.min_freq		= 200000,
+		.cpuidle_multiplier	= 1024,
+		.second_cpu_online	= true,
+		.l2_prefetch_en		= true,
+		.enable			= true,
+		.forced_state		= 0,
+		.vc_override		= false,
+	},
+	[UX500_UC_AUTO] = {
+		.name			= "auto",
+		.max_freq		= 400000,
+		.min_freq		= 200000,
+		.cpuidle_multiplier	= 0,
+		.second_cpu_online	= false,
+		.l2_prefetch_en		= true,
+		.enable			= false,
+		.forced_state		= 0,
+		.vc_override		= false,
+	},
+	[UX500_UC_VC] = {
+		.name			= "voice-call",
+		.max_freq		= 200000,
+		.min_freq		= 200000,
+		.cpuidle_multiplier	= 0,
+		.second_cpu_online	= false,
+		.l2_prefetch_en		= false,
+		.enable			= false,
+		.forced_state		= 0,
+		.vc_override		= true,
+	},
+	[UX500_UC_LPA] = {
+		.name			= "low-power-audio",
+		.max_freq		= 400000,
+		.min_freq		= 400000,
+		.cpuidle_multiplier	= 0,
+		.second_cpu_online	= false,
+		.l2_prefetch_en		= false,
+		.enable			= false,
+		.forced_state		= 0, /* Updated dynamically */
+		.vc_override		= false,
+	},
+};
+
+/* daemon */
+static struct delayed_work work_usecase;
+static struct early_suspend usecase_early_suspend;
+
+/* calculate loadavg */
+#define LOAD_INT(x) ((x) >> FSHIFT)
+#define LOAD_FRAC(x) LOAD_INT(((x) & (FIXED_1-1)) * 100)
+
+extern int cpufreq_update_freq(int cpu, unsigned int min, unsigned int max);
+extern int cpuidle_set_multiplier(unsigned int value);
+extern int cpuidle_force_state(unsigned int state);
+
+static unsigned long determine_loadavg(void)
+{
+	unsigned long  avg = 0;
+	unsigned long avnrun[3];
+
+	get_avenrun(avnrun, FIXED_1 / 200, 0);
+	avg += (LOAD_INT(avnrun[0]) * 100) + (LOAD_FRAC(avnrun[0]) % 100);
+
+	return avg;
+}
+
+static unsigned long determine_cpu_load(void)
+{
+	int i;
+	unsigned long total_load = 0;
+
+	/* get cpu load of each cpu */
+	for_each_online_cpu(i) {
+		unsigned int load, iowait;
+		unsigned int idle_time, iowait_time, wall_time;
+		cputime64_t cur_wall_time, cur_idle_time, cur_iowait_time;
+		struct hotplug_cpu_info *info;
+
+		info = &per_cpu(hotplug_info, i);
+
+		/* update both cur_idle_time and cur_wall_time */
+		cur_idle_time = get_cpu_idle_time_us(i, &cur_wall_time);
+		cur_iowait_time = get_cpu_iowait_time_us(i, &cur_wall_time);
+
+		/* how much wall time has passed since last iteration? */
+		wall_time = (unsigned int) cputime64_sub(cur_wall_time,
+							info->prev_cpu_wall);
+		info->prev_cpu_wall = cur_wall_time;
+
+		/* how much idle time has passed since last iteration? */
+		idle_time = (unsigned int) cputime64_sub(cur_idle_time,
+							info->prev_cpu_idle);
+		info->prev_cpu_idle = cur_idle_time;
+
+		/* how much io wait time has passed since last iteration? */
+		iowait_time = (unsigned int) cputime64_sub(cur_iowait_time,
+							info->prev_cpu_io);
+		info->prev_cpu_io = cur_iowait_time;
+
+		if (unlikely(!wall_time || wall_time < idle_time))
+			continue;
+
+		/* load is the percentage of time not spent in idle */
+		load = 100 * (wall_time - idle_time) / wall_time;
+		info->load[info->idx] = load;
+		hp_printk("cpu %d load %u ", i, load);
+
+		/* iowait is the percentage of time not spent in io wait */
+		iowait = 100 * (iowait_time) / wall_time;
+		info->io[info->idx++] = load;
+		hp_printk("iowait %u\n", iowait);
+
+		if (info->idx >= LOAD_MONITOR)
+			info->idx = 0;
+
+		total_load += load;
+	}
+
+	return total_load;
+}
+
+static unsigned long determine_cpu_load_trend(void)
+{
+	int i, j, k;
+	unsigned long total_load = 0;
+
+	/* Get cpu load of each cpu */
+	for_each_online_cpu(i) {
+		unsigned int load = 0;
+		struct hotplug_cpu_info *info;
+
+		info = &per_cpu(hotplug_info, i);
+
+		for (k = 0, j = info->idx; k < LOAD_MONITOR; k++, j++)
+			load +=	info->load[j];
+
+		load /= LOAD_MONITOR;
+
+		hp_printk("cpu %d load trend %u\n", i, load);
+
+		total_load += load;
+	}
+
+	return total_load;
+}
+
+static unsigned long determine_cpu_balance_trend(void)
+{
+	int i, j, k;
+	unsigned long total_load = 0;
+	unsigned long min_load = (unsigned long) (-1);
+
+	/* Get cpu load of each cpu */
+	for_each_online_cpu(i) {
+		unsigned int load = 0;
+		struct hotplug_cpu_info *info;
+
+	info = &per_cpu(hotplug_info, i);
+
+		for (k = 0, j = info->idx; k < LOAD_MONITOR; k++, j++)
+			load +=	info->load[j];
+
+		load /= LOAD_MONITOR;
+
+		if (min_load > load)
+			min_load = load;
+		total_load += load;
+	}
+
+	if (min_load > min_trend)
+		total_load = (100 * total_load) / min_load;
+	else
+		total_load = 50 << num_online_cpus();
+
+	return total_load;
+}
+
+static void init_cpu_load_trend(void)
+{
+	int i;
+
+	for_each_possible_cpu(i) {
+		struct hotplug_cpu_info *info;
+		int j;
+
+		info = &per_cpu(hotplug_info, i);
+
+		info->prev_cpu_idle = get_cpu_idle_time_us(i,
+						&(info->prev_cpu_wall));
+		info->prev_cpu_io = get_cpu_iowait_time_us(i,
+						&(info->prev_cpu_wall));
+
+		for (j = 0; j < LOAD_MONITOR; j++) {
+			info->load[j] = 100;
+			info->io[j] = 100;
+		}
+		info->idx = 0;
+	}
+}
+
+static u32 get_num_interrupts_per_s(void)
+{
+	int cpu;
+	int i;
+	u64 num_irqs = 0;
+	ktime_t now;
+	static ktime_t last;
+	unsigned int delta;
+	u32 irqs = 0;
+
+	now = ktime_get();
+
+	for_each_possible_cpu(cpu) {
+		for (i = 0; i < NR_IRQS; i++)
+			num_irqs += kstat_irqs_cpu(i, cpu);
+	}
+	pr_debug("%s: total num irqs: %lld, previous %lld\n",
+					__func__, num_irqs, old_num_irqs);
+
+	if (old_num_irqs > 0) {
+		delta = (u32)ktime_to_ms(ktime_sub(now, last)) / 1000;
+		irqs = ((u32)(num_irqs - old_num_irqs)) / delta;
+	}
+
+	old_num_irqs = num_irqs;
+	last = now;
+
+	pr_debug("delta irqs per sec:%d\n", irqs);
+
+	return irqs;
+}
+
+static void set_cpu_config(enum ux500_uc new_uc)
+{
+	struct cpufreq_policy policy;
+	int err;
+	bool update = false;
+	u32 min_freq, max_freq;
+
+	if (new_uc != current_uc)
+		update = true;
+	else if ((user_config_updated) && (new_uc == UX500_UC_USER))
+		update = true;
+
+	pr_debug("%s: new_usecase=%d, current_usecase=%d, update=%d\n",
+			__func__, new_uc, current_uc, update);
+
+	if (!update)
+		goto exit;
+
+	/* Cpu hotplug */
+	if (!(usecase_conf[new_uc].second_cpu_online) &&
+	    (num_online_cpus() > 1))
+		cpu_down(1);
+	else if ((usecase_conf[new_uc].second_cpu_online) &&
+		 (num_online_cpus() < 2))
+		cpu_up(1);
+
+	/* Cpu freq */
+	err = cpufreq_get_policy(&policy, 0);
+	if (err)
+		pr_err("usecase-gov: get cpufreq policy failed\n");
+
+	/* If requirement is 0, use current policy value */
+	min_freq = usecase_conf[new_uc].min_freq ?
+		usecase_conf[new_uc].min_freq : policy.min;
+
+	max_freq = usecase_conf[new_uc].max_freq ?
+		usecase_conf[new_uc].max_freq : policy.max;
+
+	/*
+	 * cpufreq fw does not allow frequency change if
+	 * "current min freq" > "new max freq" or
+	 * "current max freq" < "new min freq".
+	 * Thus the intermediate steps below.
+	 */
+	if (policy.min > max_freq) {
+		err = cpufreq_update_freq(0, min_freq, policy.max);
+		if (err)
+			pr_err("usecase-gov: update min cpufreq failed\n");
+	}
+	if (policy.max < min_freq) {
+		err = cpufreq_update_freq(0, policy.min, max_freq);
+		if (err)
+			pr_err("usecase-gov: update max cpufreq failed\n");
+	}
+
+	err = cpufreq_update_freq(0, min_freq, max_freq);
+	if (err)
+		pr_err("usecase-gov: update min/max cpufreq failed\n");
+
+	/* Cpu idle */
+	cpuidle_set_multiplier(usecase_conf[new_uc].cpuidle_multiplier);
+
+	/* L2 prefetch */
+	if (usecase_conf[new_uc].l2_prefetch_en)
+		outer_prefetch_enable();
+	else
+		outer_prefetch_disable();
+
+	/* Force cpuidle state */
+	cpuidle_force_state(usecase_conf[new_uc].forced_state);
+
+	/* QOS override */
+	prcmu_qos_voice_call_override(usecase_conf[new_uc].vc_override);
+
+	current_uc = new_uc;
+
+exit:
+	/* Its ok to clear even if new_uc != UX500_UC_USER */
+	user_config_updated = false;
+}
+
+/*
+ * Start load measurment every 6 s in order detrmine if can unplug one CPU.
+ * In order to not corrupt measurment, the first load average is not done
+ * here call in  early suspend.
+ */
+static void usecase_earlysuspend_callback(struct early_suspend *h)
+{
+	init_cpu_load_trend();
+
+	mutex_lock(&user_config_mutex);
+
+	is_early_suspend = true;
+
+	if (usecase_conf[UX500_UC_AUTO].enable ||
+		usecase_conf[UX500_UC_USER].enable) {
+
+		is_work_scheduled = true;
+
+		schedule_delayed_work_on(0, &work_usecase,
+				msecs_to_jiffies(CPULOAD_MEAS_DELAY));
+	}
+
+	mutex_unlock(&user_config_mutex);
+}
+
+/* Stop measurement, call LCD early resume */
+static void usecase_lateresume_callback(struct early_suspend *h)
+{
+	mutex_lock(&user_config_mutex);
+
+	if (is_work_scheduled) {
+		cancel_delayed_work_sync(&work_usecase);
+		is_work_scheduled = false;
+	}
+
+	is_early_suspend = false;
+
+	set_cpu_config(UX500_UC_NORMAL);
+
+	mutex_unlock(&user_config_mutex);
+}
+
+static void delayed_usecase_work(struct work_struct *work)
+{
+	unsigned long avg, load, trend, balance;
+	bool inc_perf = false;
+	bool dec_perf = false;
+	u32 irqs_per_s;
+
+	/* determine loadavg  */
+	avg = determine_loadavg();
+	hp_printk("loadavg = %lu lower th %lu upper th %lu\n",
+					avg, lower_threshold, upper_threshold);
+
+	/* determine instant load */
+	load = determine_cpu_load();
+	hp_printk("cpu instant load = %lu max %lu\n", load, max_instant);
+
+	/* determine load trend */
+	trend = determine_cpu_load_trend();
+	hp_printk("cpu load trend = %lu min %lu unbal %lu\n",
+					trend, min_trend, trend_unbalance);
+
+	/* determine load balancing */
+	balance = determine_cpu_balance_trend();
+	hp_printk("load balancing trend = %lu min %lu\n",
+					balance, max_unbalance);
+
+	irqs_per_s = get_num_interrupts_per_s();
+
+	/* Dont let configuration change in the middle of our calculations. */
+	mutex_lock(&user_config_mutex);
+
+	/* detect "instant" load increase */
+	if (load > max_instant || irqs_per_s > exit_irq_per_s) {
+		inc_perf = true;
+	} else if (!usecase_conf[UX500_UC_USER].enable &&
+			usecase_conf[UX500_UC_AUTO].enable) {
+		/* detect high loadavg use case */
+		if (avg > upper_threshold)
+			inc_perf = true;
+		/* detect idle use case */
+		else if (trend < min_trend)
+			dec_perf = true;
+		/* detect unbalanced low cpu load use case */
+		else if ((balance > max_unbalance) && (trend < trend_unbalance))
+			dec_perf = true;
+		/* detect low loadavg use case */
+		else if (avg < lower_threshold)
+			dec_perf = true;
+		/* All user use cases disabled, current load not triggering
+		 * any change.
+		 */
+		else if (user_config_updated)
+			dec_perf = true;
+	} else {
+		dec_perf = true;
+	}
+
+	/*
+	 * set_cpu_config() will not update the config unless it has been
+	 * changed.
+	 */
+	if (dec_perf) {
+		if (usecase_conf[UX500_UC_USER].enable)
+			set_cpu_config(UX500_UC_USER);
+		else if (usecase_conf[UX500_UC_AUTO].enable)
+			set_cpu_config(UX500_UC_AUTO);
+	} else if (inc_perf) {
+		set_cpu_config(UX500_UC_NORMAL);
+	}
+
+	mutex_unlock(&user_config_mutex);
+
+	/* reprogramm scheduled work */
+	schedule_delayed_work_on(0, &work_usecase,
+				msecs_to_jiffies(CPULOAD_MEAS_DELAY));
+
+}
+
+static struct dentry *usecase_dir;
+
+#ifdef CONFIG_DEBUG_FS
+#define define_set(_name) \
+static ssize_t set_##_name(struct file *file, \
+				 const char __user *user_buf, \
+				 size_t count, loff_t *ppos) \
+{ \
+	int err; \
+	long unsigned i; \
+ \
+	err = kstrtoul_from_user(user_buf, count, 0, &i); \
+ \
+	if (err) \
+		return err; \
+ \
+	_name = i; \
+	hp_printk("New value : %lu\n", _name); \
+ \
+	return count; \
+}
+
+define_set(upper_threshold);
+define_set(lower_threshold);
+define_set(max_unbalance);
+define_set(trend_unbalance);
+define_set(min_trend);
+define_set(max_instant);
+define_set(debug);
+
+#define define_print(_name) \
+static ssize_t print_##_name(struct seq_file *s, void *p) \
+{ \
+	return seq_printf(s, "%lu\n", _name); \
+}
+
+define_print(upper_threshold);
+define_print(lower_threshold);
+define_print(max_unbalance);
+define_print(trend_unbalance);
+define_print(min_trend);
+define_print(max_instant);
+define_print(debug);
+
+#define define_open(_name) \
+static ssize_t open_##_name(struct inode *inode, struct file *file) \
+{ \
+	return single_open(file, print_##_name, inode->i_private); \
+}
+
+define_open(upper_threshold);
+define_open(lower_threshold);
+define_open(max_unbalance);
+define_open(trend_unbalance);
+define_open(min_trend);
+define_open(max_instant);
+define_open(debug);
+
+#define define_dbg_file(_name) \
+static const struct file_operations fops_##_name = { \
+	.open = open_##_name, \
+	.write = set_##_name, \
+	.read = seq_read, \
+	.llseek = seq_lseek, \
+	.release = single_release, \
+	.owner = THIS_MODULE, \
+}; \
+static struct dentry *file_##_name;
+
+define_dbg_file(upper_threshold);
+define_dbg_file(lower_threshold);
+define_dbg_file(max_unbalance);
+define_dbg_file(trend_unbalance);
+define_dbg_file(min_trend);
+define_dbg_file(max_instant);
+define_dbg_file(debug);
+
+struct dbg_file {
+	struct dentry **file;
+	const struct file_operations *fops;
+	const char *name;
+};
+
+#define define_dbg_entry(_name) \
+{ \
+	.file = &file_##_name, \
+	.fops = &fops_##_name, \
+	.name = #_name \
+}
+
+static struct dbg_file debug_entry[] = {
+	define_dbg_entry(upper_threshold),
+	define_dbg_entry(lower_threshold),
+	define_dbg_entry(max_unbalance),
+	define_dbg_entry(trend_unbalance),
+	define_dbg_entry(min_trend),
+	define_dbg_entry(max_instant),
+	define_dbg_entry(debug),
+};
+
+static int setup_debugfs(void)
+{
+	int i;
+	usecase_dir = debugfs_create_dir("usecase", NULL);
+
+	if (IS_ERR_OR_NULL(usecase_dir))
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(debug_entry); i++) {
+		if (IS_ERR_OR_NULL(debugfs_create_file(debug_entry[i].name,
+						       S_IWUGO | S_IRUGO,
+						       usecase_dir,
+						       NULL,
+						       debug_entry[i].fops)))
+			goto fail;
+	}
+
+	if (IS_ERR_OR_NULL(debugfs_create_u32("exit_irq_per_s",
+					      S_IWUGO | S_IRUGO, usecase_dir,
+					      &exit_irq_per_s)))
+		goto fail;
+	return 0;
+fail:
+	debugfs_remove_recursive(usecase_dir);
+	return -EINVAL;
+}
+#else
+static int setup_debugfs(void)
+{
+	return 0;
+}
+#endif
+
+static void usecase_update_user_config(void)
+{
+	int i;
+	bool config_enable = false;
+	struct usecase_config *user_conf = &usecase_conf[UX500_UC_USER];
+
+	mutex_lock(&user_config_mutex);
+
+	user_conf->max_freq = 0;
+	user_conf->min_freq = 0;
+	user_conf->cpuidle_multiplier = 0;
+	user_conf->second_cpu_online = false;
+	user_conf->l2_prefetch_en = false;
+	user_conf->forced_state = cpuidle_deepest_state;
+	user_conf->vc_override = true; /* A single false will clear it. */
+
+	/* Dont include Auto and Normal modes in this */
+	for (i = (UX500_UC_AUTO + 1); i < UX500_UC_USER; i++) {
+		if (!usecase_conf[i].enable)
+			continue;
+
+		config_enable = true;
+
+		if (usecase_conf[i].max_freq > user_conf->max_freq)
+			user_conf->max_freq = usecase_conf[i].max_freq;
+		/* It's the highest min freq requirement that should be used */
+		if (usecase_conf[i].min_freq > user_conf->min_freq)
+			user_conf->min_freq = usecase_conf[i].min_freq;
+
+		if (usecase_conf[i].cpuidle_multiplier >
+					user_conf->cpuidle_multiplier)
+			user_conf->cpuidle_multiplier =
+				usecase_conf[i].cpuidle_multiplier;
+
+		user_conf->second_cpu_online |=
+				usecase_conf[i].second_cpu_online;
+
+		user_conf->l2_prefetch_en |=
+				usecase_conf[i].l2_prefetch_en;
+
+		/* Take the shallowest state. */
+		if (usecase_conf[i].forced_state < user_conf->forced_state)
+			user_conf->forced_state = usecase_conf[i].forced_state;
+
+		/* Only override QOS if all enabled configurations are
+		 * requesting it.
+		 */
+		if (!usecase_conf[i].vc_override)
+			user_conf->vc_override = false;
+	}
+
+	user_conf->enable = config_enable;
+	user_config_updated = true;
+
+	mutex_unlock(&user_config_mutex);
+}
+
+struct usecase_devclass_attr {
+	struct sysdev_class_attribute class_attr;
+	u32 index;
+};
+
+static struct usecase_devclass_attr usecase_dc_attr[UX500_UC_MAX];
+
+static struct attribute *dbs_attributes[UX500_UC_MAX + 1] = {NULL};
+
+static struct attribute_group dbs_attr_group = {
+	.attrs = dbs_attributes,
+	.name = "usecase",
+};
+
+static ssize_t show_current(struct sysdev_class *class,
+			struct sysdev_class_attribute *attr, char *buf)
+{
+	enum ux500_uc display_uc = (current_uc == UX500_UC_MAX) ?
+					UX500_UC_NORMAL : current_uc;
+
+	return sprintf(buf, "max_freq: %ld\n"
+		"cpuidle_multiplier: %ld\n"
+		"second_cpu_online: %s\n"
+		"l2_prefetch_en: %s\n"
+		"forced_state: %d\n"
+		"vc_override: %s\n",
+		usecase_conf[display_uc].max_freq,
+		usecase_conf[display_uc].cpuidle_multiplier,
+		usecase_conf[display_uc].second_cpu_online ? "true" : "false",
+		usecase_conf[display_uc].l2_prefetch_en ? "true" : "false",
+		usecase_conf[display_uc].forced_state,
+		usecase_conf[display_uc].vc_override ? "true" : "false");
+}
+
+static ssize_t show_dc_attr(struct sysdev_class *class,
+			struct sysdev_class_attribute *attr, char *buf)
+{
+	struct usecase_devclass_attr *uattr =
+		container_of(attr, struct usecase_devclass_attr, class_attr);
+
+	return sprintf(buf, "%u\n",
+				usecase_conf[uattr->index].enable);
+}
+
+static ssize_t store_dc_attr(struct sysdev_class *class,
+					struct sysdev_class_attribute *attr,
+				    const char *buf, size_t count)
+{
+	unsigned int input;
+	int ret;
+
+	struct usecase_devclass_attr *uattr =
+		container_of(attr, struct usecase_devclass_attr, class_attr);
+
+	ret = sscanf(buf, "%u", &input);
+
+	/* Normal mode cant be changed. */
+	if ((ret != 1) || (uattr->index == 0))
+		return -EINVAL;
+
+	usecase_conf[uattr->index].enable = (bool)input;
+
+	usecase_update_user_config();
+
+	mutex_lock(&user_config_mutex);
+
+	if (usecase_conf[UX500_UC_AUTO].enable ||
+		usecase_conf[UX500_UC_USER].enable) {
+		if (is_early_suspend && !is_work_scheduled) {
+			schedule_delayed_work_on(0, &work_usecase,
+				msecs_to_jiffies(CPULOAD_MEAS_DELAY));
+			is_work_scheduled = true;
+		}
+	} else if (is_work_scheduled) {
+		cancel_delayed_work_sync(&work_usecase);
+		is_work_scheduled = false;
+		set_cpu_config(UX500_UC_NORMAL);
+	}
+
+	mutex_unlock(&user_config_mutex);
+
+	return count;
+}
+
+static int usecase_sysfs_init(void)
+{
+	int err;
+	int i;
+
+	for (i = 0; i < (ARRAY_SIZE(usecase_conf) - 1); i++) {
+		usecase_dc_attr[i].class_attr.attr.name = usecase_conf[i].name;
+		usecase_dc_attr[i].class_attr.attr.mode = 0644;
+		usecase_dc_attr[i].class_attr.show = show_dc_attr;
+		usecase_dc_attr[i].class_attr.store = store_dc_attr;
+		usecase_dc_attr[i].index = i;
+
+		dbs_attributes[i] = &(usecase_dc_attr[i].class_attr.attr);
+	}
+	usecase_dc_attr[UX500_UC_USER].class_attr.attr.name = "current";
+	usecase_dc_attr[UX500_UC_USER].class_attr.attr.mode = 0644;
+	usecase_dc_attr[UX500_UC_USER].class_attr.show = show_current;
+	usecase_dc_attr[UX500_UC_USER].class_attr.store = NULL;
+	usecase_dc_attr[UX500_UC_USER].index = UX500_UC_USER;
+	dbs_attributes[UX500_UC_USER] =
+		&(usecase_dc_attr[UX500_UC_USER].class_attr.attr);
+
+	err = sysfs_create_group(&(cpu_sysdev_class.kset.kobj),
+						&dbs_attr_group);
+	if (err)
+		pr_err("usecase-gov: sysfs_create_group"
+				" failed with error = %d\n", err);
+
+	return err;
+}
+
+#include "cpuidle.h"
+
+static void usecase_cpuidle_init(void)
+{
+	int max_states;
+	int i;
+	struct cstate *state = ux500_ci_get_cstates(&max_states);
+
+	for (i = 0; i < max_states; i++)
+		if ((state[i].APE == APE_OFF) && (state[i].ARM == ARM_RET))
+			break;
+
+	usecase_conf[UX500_UC_LPA].forced_state = i;
+
+	cpuidle_deepest_state = max_states - 1;
+}
+
+/*  initialize devices */
+static int __init init_usecase_devices(void)
+{
+	int err;
+
+	pr_info("Use-case governor initialized\n");
+
+	/*  add early_suspend callback */
+	usecase_early_suspend.level = 200;
+	usecase_early_suspend.suspend = usecase_earlysuspend_callback;
+	usecase_early_suspend.resume = usecase_lateresume_callback;
+	register_early_suspend(&usecase_early_suspend);
+
+	/* register delayed queuework */
+	INIT_DELAYED_WORK_DEFERRABLE(&work_usecase,
+				     delayed_usecase_work);
+
+	init_cpu_load_trend();
+
+	err = setup_debugfs();
+	if (err)
+		goto error;
+	err = usecase_sysfs_init();
+	if (err)
+		goto error2;
+
+	usecase_cpuidle_init();
+
+	return 0;
+error2:
+	debugfs_remove_recursive(usecase_dir);
+error:
+	unregister_early_suspend(&usecase_early_suspend);
+	return err;
+}
+
+device_initcall(init_usecase_devices);
diff --git a/arch/arm/mach-ux500/prcmu-debug.c b/arch/arm/mach-ux500/prcmu-debug.c
new file mode 100644
index 00000000000..8344ff36ffb
--- /dev/null
+++ b/arch/arm/mach-ux500/prcmu-debug.c
@@ -0,0 +1,556 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Author: Martin Persson <martin.persson@stericsson.com> for ST-Ericsson
+ *         Etienne Carriere <etienne.carriere@stericsson.com> for ST-Ericsson
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+#include <mach/prcmu.h>
+
+enum ape_opp_debug {
+	APE_50_OPP_DEBUG,
+	APE_100_OPP_DEBUG,
+	NUM_APE_OPP_DEBUG,
+};
+
+enum ddr_opp_debug {
+	DDR_25_OPP_DEBUG,
+	DDR_50_OPP_DEBUG,
+	DDR_100_OPP_DEBUG,
+	NUM_DDR_OPP_DEBUG,
+};
+
+struct ape_state_history {
+	ktime_t start;
+	u32 state;
+	u32 counter[NUM_APE_OPP_DEBUG];
+	ktime_t time[NUM_APE_OPP_DEBUG];
+	spinlock_t lock;
+};
+
+struct ddr_state_history {
+	ktime_t start;
+	u32 state;
+	u32 counter[NUM_DDR_OPP_DEBUG];
+	ktime_t time[NUM_DDR_OPP_DEBUG];
+	spinlock_t lock;
+};
+
+static struct ape_state_history *ape_sh;
+static struct ddr_state_history *ddr_sh;
+static int ape_voltage_count;
+
+void prcmu_debug_ape_opp_log(u8 opp)
+{
+	ktime_t now;
+	ktime_t dtime;
+	unsigned long flags;
+	int state;
+
+	if (opp == APE_50_OPP)
+		state = APE_50_OPP_DEBUG;
+	else
+		state = APE_100_OPP_DEBUG;
+
+	now = ktime_get();
+	spin_lock_irqsave(&ape_sh->lock, flags);
+
+	dtime = ktime_sub(now, ape_sh->start);
+	ape_sh->time[state] = ktime_add(ape_sh->time[state], dtime);
+	ape_sh->start = now;
+	ape_sh->counter[state]++;
+	ape_sh->state = state;
+
+	spin_unlock_irqrestore(&ape_sh->lock, flags);
+}
+
+void prcmu_debug_ddr_opp_log(u8 opp)
+{
+	ktime_t now;
+	ktime_t dtime;
+	unsigned long flags;
+	int state;
+
+	if (opp == DDR_25_OPP)
+		state = DDR_25_OPP_DEBUG;
+	else if (opp == DDR_50_OPP)
+		state = DDR_50_OPP_DEBUG;
+	else
+		state = DDR_100_OPP_DEBUG;
+
+	now = ktime_get();
+	spin_lock_irqsave(&ddr_sh->lock, flags);
+
+	dtime = ktime_sub(now, ddr_sh->start);
+	ddr_sh->time[state] = ktime_add(ddr_sh->time[state], dtime);
+	ddr_sh->start = now;
+	ddr_sh->counter[state]++;
+	ddr_sh->state = state;
+
+	spin_unlock_irqrestore(&ddr_sh->lock, flags);
+}
+
+static ssize_t ape_stats_write(struct file *file,
+			   const char __user *user_buf,
+			   size_t count, loff_t *ppos)
+{
+	unsigned long flags;
+	int i;
+
+	pr_info("/nreset\n");
+
+	spin_lock_irqsave(&ape_sh->lock, flags);
+	for (i = 0; i < NUM_APE_OPP_DEBUG; i++) {
+		ape_sh->counter[i] = 0;
+		ape_sh->time[i] = ktime_set(0, 0);
+	}
+
+	ape_sh->start = ktime_get();
+	spin_unlock_irqrestore(&ape_sh->lock, flags);
+
+	return count;
+}
+
+static ssize_t ddr_stats_write(struct file *file,
+			   const char __user *user_buf,
+			   size_t count, loff_t *ppos)
+{
+	unsigned long flags;
+	int i;
+
+	pr_info("/nreset\n");
+
+	spin_lock_irqsave(&ddr_sh->lock, flags);
+	for (i = 0; i < NUM_DDR_OPP_DEBUG; i++) {
+		ddr_sh->counter[i] = 0;
+		ddr_sh->time[i] = ktime_set(0, 0);
+	}
+
+	ddr_sh->start = ktime_get();
+	spin_unlock_irqrestore(&ddr_sh->lock, flags);
+
+	return count;
+}
+
+static int ape_stats_print(struct seq_file *s, void *p)
+{
+	int i;
+	unsigned long flags;
+	ktime_t total;
+	ktime_t now;
+	ktime_t dtime;
+	s64 t_us;
+	s64 perc;
+	s64 total_us;
+
+	spin_lock_irqsave(&ape_sh->lock, flags);
+	/* Update time in state */
+	now = ktime_get();
+	dtime = ktime_sub(now, ape_sh->start);
+	ape_sh->time[ape_sh->state] =
+		ktime_add(ape_sh->time[ape_sh->state], dtime);
+	ape_sh->start = now;
+
+	/* Now print the stats */
+	total = ktime_set(0, 0);
+
+	for (i = 0; i < NUM_APE_OPP_DEBUG; i++)
+		total = ktime_add(total, ape_sh->time[i]);
+	total_us = ktime_to_us(total);
+	do_div(total_us, 100);
+
+	for (i = 0; i < NUM_APE_OPP_DEBUG; i++) {
+		t_us = ktime_to_us(ape_sh->time[i]);
+		perc = ktime_to_us(ape_sh->time[i]);
+		do_div(t_us, 1000); /* to ms */
+		do_div(perc, total_us);
+		if (i == APE_50_OPP_DEBUG)
+			seq_printf(s, "%s: # %u in %d ms %d%%\n",
+				   "APE OPP 50% ",
+				   ape_sh->counter[i],
+				   (u32) t_us, (u32)perc);
+		else
+			seq_printf(s, "%s: # %u in %d ms %d%%\n",
+				   "APE OPP 100%",
+				   ape_sh->counter[i],
+				   (u32) t_us, (u32)perc);
+
+	}
+	spin_unlock_irqrestore(&ape_sh->lock, flags);
+	return 0;
+}
+
+static int ddr_stats_print(struct seq_file *s, void *p)
+{
+	int i;
+	unsigned long flags;
+	ktime_t total;
+	ktime_t now;
+	ktime_t dtime;
+	s64 t_us;
+	s64 perc;
+	s64 total_us;
+
+	spin_lock_irqsave(&ddr_sh->lock, flags);
+	/* Update time in state */
+	now = ktime_get();
+	dtime = ktime_sub(now, ddr_sh->start);
+	ddr_sh->time[ddr_sh->state] =
+		ktime_add(ddr_sh->time[ddr_sh->state], dtime);
+	ddr_sh->start = now;
+
+	/* Now print the stats */
+	total = ktime_set(0, 0);
+
+	for (i = 0; i < NUM_DDR_OPP_DEBUG; i++)
+		total = ktime_add(total, ddr_sh->time[i]);
+	total_us = ktime_to_us(total);
+	do_div(total_us, 100);
+
+	for (i = 0; i < NUM_DDR_OPP_DEBUG; i++) {
+		t_us = ktime_to_us(ddr_sh->time[i]);
+		perc = ktime_to_us(ddr_sh->time[i]);
+		do_div(t_us, 1000); /* to ms */
+		do_div(perc, total_us);
+		if (i == DDR_25_OPP_DEBUG)
+			seq_printf(s, "%s: # %u in %d ms %d%%\n",
+				   "DDR OPP 25% ",
+				   ddr_sh->counter[i],
+				   (u32) t_us, (u32)perc);
+		else if (i == DDR_50_OPP_DEBUG)
+			seq_printf(s, "%s: # %u in %d ms %d%%\n",
+				   "DDR OPP 50% ",
+				   ddr_sh->counter[i],
+				   (u32) t_us, (u32)perc);
+		else
+			seq_printf(s, "%s: # %u in %d ms %d%%\n",
+				   "DDR OPP 100%",
+				   ddr_sh->counter[i],
+				   (u32) t_us, (u32)perc);
+
+	}
+	spin_unlock_irqrestore(&ddr_sh->lock, flags);
+	return 0;
+}
+
+static int arm_opp_read(struct seq_file *s, void *p)
+{
+	int opp;
+
+	opp = prcmu_get_arm_opp();
+	return seq_printf(s, "%s (%d)\n",
+		(opp == ARM_MAX_OPP) ? "max" :
+		(opp == ARM_MAX_FREQ100OPP) ? "max-freq100" :
+		(opp == ARM_100_OPP) ? "100%" :
+		(opp == ARM_50_OPP) ? "50%" :
+		(opp == ARM_EXTCLK) ? "25% (extclk)" :
+		"unknown", opp);
+}
+
+static int ape_opp_read(struct seq_file *s, void *p)
+{
+	int opp;
+
+	opp = prcmu_get_ape_opp();
+	return seq_printf(s, "%s (%d)\n",
+			(opp == APE_100_OPP) ? "100%" :
+			(opp == APE_50_OPP) ? "50%" :
+			"unknown", opp);
+}
+
+static int ddr_opp_read(struct seq_file *s, void *p)
+{
+	int opp;
+
+	opp = prcmu_get_ddr_opp();
+	return seq_printf(s, "%s (%d)\n",
+			(opp == DDR_100_OPP) ? "100%" :
+			(opp == DDR_50_OPP) ? "50%" :
+			(opp == DDR_25_OPP) ? "25%" :
+			"unknown", opp);
+}
+
+static ssize_t opp_write(struct file *file,
+				   const char __user *user_buf,
+			 size_t count, loff_t *ppos, int prcmu_qos_class)
+{
+	long unsigned i;
+	int err;
+
+	err = kstrtoul_from_user(user_buf, count, 0, &i);
+
+	if (err)
+		return err;
+
+	prcmu_qos_force_opp(prcmu_qos_class, i);
+
+	pr_info("prcmu debug: forced OPP for %d to %d\n", prcmu_qos_class, (int)i);
+
+	return count;
+}
+
+static ssize_t ddr_opp_write(struct file *file,
+				   const char __user *user_buf,
+				   size_t count, loff_t *ppos)
+{
+	return opp_write(file, user_buf, count, ppos, PRCMU_QOS_DDR_OPP);
+}
+
+static ssize_t ape_opp_write(struct file *file,
+				   const char __user *user_buf,
+				   size_t count, loff_t *ppos)
+{
+	return opp_write(file, user_buf, count, ppos, PRCMU_QOS_APE_OPP);
+}
+
+static int cpufreq_delay_read(struct seq_file *s, void *p)
+{
+	return seq_printf(s, "%lu\n", prcmu_qos_get_cpufreq_opp_delay());
+}
+
+static int ape_voltage_read(struct seq_file *s, void *p)
+{
+	return seq_printf(s, "This reference count only includes "
+			  "requests via debugfs.\nCount: %d\n",
+			  ape_voltage_count);
+}
+
+static ssize_t ape_voltage_write(struct file *file,
+				   const char __user *user_buf,
+				   size_t count, loff_t *ppos)
+{
+	long unsigned i;
+	int err;
+
+	err = kstrtoul_from_user(user_buf, count, 0, &i);
+
+	if (err)
+		return err;
+
+	switch (i) {
+	case 0:
+		if (ape_voltage_count == 0)
+			pr_info("prcmu debug: reference count is already 0\n");
+		else {
+			err = prcmu_request_ape_opp_100_voltage(false);
+			if (err)
+				pr_err("prcmu debug: drop request failed\n");
+			else
+				ape_voltage_count--;
+		}
+		break;
+	case 1:
+		err = prcmu_request_ape_opp_100_voltage(true);
+		if (err)
+			pr_err("prcmu debug: request failed\n");
+		else
+			ape_voltage_count++;
+		break;
+	default:
+		pr_info("prcmu debug: value not equal to 0 or 1\n");
+	}
+	return count;
+}
+
+static ssize_t cpufreq_delay_write(struct file *file,
+				   const char __user *user_buf,
+				   size_t count, loff_t *ppos)
+{
+	int err;
+	long unsigned i;
+
+	err = kstrtoul_from_user(user_buf, count, 0, &i);
+
+	if (err)
+		return err;
+
+	prcmu_qos_set_cpufreq_opp_delay(i);
+
+	pr_info("prcmu debug: changed delay between cpufreq change and QoS "
+		 "requirement to %lu.\n", i);
+
+	return count;
+}
+
+static int arm_opp_open_file(struct inode *inode, struct file *file)
+{
+	return single_open(file, arm_opp_read, inode->i_private);
+}
+
+static int ape_opp_open_file(struct inode *inode, struct file *file)
+{
+	return single_open(file, ape_opp_read, inode->i_private);
+}
+
+static int ddr_opp_open_file(struct inode *inode, struct file *file)
+{
+	return single_open(file, ddr_opp_read, inode->i_private);
+}
+
+static int ape_stats_open_file(struct inode *inode, struct file *file)
+{
+	return single_open(file, ape_stats_print, inode->i_private);
+}
+
+static int ddr_stats_open_file(struct inode *inode, struct file *file)
+{
+	return single_open(file, ddr_stats_print, inode->i_private);
+}
+
+static int cpufreq_delay_open_file(struct inode *inode, struct file *file)
+{
+	return single_open(file, cpufreq_delay_read, inode->i_private);
+}
+
+static int ape_voltage_open_file(struct inode *inode, struct file *file)
+{
+	return single_open(file, ape_voltage_read, inode->i_private);
+}
+
+static const struct file_operations arm_opp_fops = {
+	.open = arm_opp_open_file,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ape_opp_fops = {
+	.open = ape_opp_open_file,
+	.write = ape_opp_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ddr_opp_fops = {
+	.open = ddr_opp_open_file,
+	.write = ddr_opp_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ape_stats_fops = {
+	.open = ape_stats_open_file,
+	.write = ape_stats_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ddr_stats_fops = {
+	.open = ddr_stats_open_file,
+	.write = ddr_stats_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations cpufreq_delay_fops = {
+	.open = cpufreq_delay_open_file,
+	.write = cpufreq_delay_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ape_voltage_fops = {
+	.open = ape_voltage_open_file,
+	.write = ape_voltage_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int setup_debugfs(void)
+{
+	struct dentry *dir;
+	struct dentry *file = NULL;
+
+	dir = debugfs_create_dir("prcmu", NULL);
+	if (IS_ERR_OR_NULL(dir))
+		goto fail;
+
+	file = debugfs_create_file("ape_stats", (S_IRUGO | S_IWUGO),
+				   dir, NULL, &ape_stats_fops);
+	if (IS_ERR_OR_NULL(file))
+		goto fail;
+
+	file = debugfs_create_file("ddr_stats", (S_IRUGO | S_IWUGO),
+				   dir, NULL, &ddr_stats_fops);
+	if (IS_ERR_OR_NULL(file))
+		goto fail;
+
+	file = debugfs_create_file("ape_opp", (S_IRUGO),
+				   dir, NULL, &ape_opp_fops);
+	if (IS_ERR_OR_NULL(file))
+		goto fail;
+
+	file = debugfs_create_file("ddr_opp", (S_IRUGO),
+				   dir, NULL, &ddr_opp_fops);
+	if (IS_ERR_OR_NULL(file))
+		goto fail;
+
+	file = debugfs_create_file("arm_opp", (S_IRUGO),
+				   dir, NULL, &arm_opp_fops);
+	if (IS_ERR_OR_NULL(file))
+		goto fail;
+
+	file = debugfs_create_file("opp_cpufreq_delay", (S_IRUGO),
+				   dir, NULL, &cpufreq_delay_fops);
+	if (IS_ERR_OR_NULL(file))
+		goto fail;
+
+	file = debugfs_create_file("ape_voltage", (S_IRUGO),
+				   dir, NULL, &ape_voltage_fops);
+	if (IS_ERR_OR_NULL(file))
+		goto fail;
+
+	return 0;
+fail:
+	if ((file == NULL) && (dir != NULL))
+		debugfs_remove_recursive(dir);
+
+	pr_err("prcmu debug: debugfs entry failed\n");
+	return -ENOMEM;
+}
+
+int prcmu_debug_init(void)
+{
+	ape_sh = kzalloc(sizeof(struct ape_state_history), GFP_KERNEL);
+	if (ape_sh == NULL) {
+		pr_err("prcmu debug: kzalloc failed\n");
+		return -ENOMEM;
+	}
+
+	ddr_sh = kzalloc(sizeof(struct ddr_state_history), GFP_KERNEL);
+	if (ddr_sh == NULL) {
+		pr_err("prcmu debug: kzalloc failed\n");
+		return -ENOMEM;
+	}
+
+	spin_lock_init(&ape_sh->lock);
+	spin_lock_init(&ddr_sh->lock);
+	ape_sh->start = ktime_get();
+	ddr_sh->start = ktime_get();
+	setup_debugfs();
+	return 0;
+}
diff --git a/arch/arm/mach-ux500/prcmu-debug.h b/arch/arm/mach-ux500/prcmu-debug.h
new file mode 100644
index 00000000000..79e03e5b208
--- /dev/null
+++ b/arch/arm/mach-ux500/prcmu-debug.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Author: Martin Persson <martin.persson@stericsson.com> for ST-Ericsson
+ *         Etienne Carriere <etienne.carriere@stericsson.com> for ST-Ericsson
+ *
+ */
+
+#ifndef PRCMU_DEBUG_H
+#define PRCMU_DEBUG_H
+
+#ifdef CONFIG_UX500_PRCMU_DEBUG
+void prcmu_debug_ape_opp_log(u8 opp);
+void prcmu_debug_ddr_opp_log(u8 opp);
+int prcmu_debug_init(void);
+#else
+static inline void prcmu_debug_ape_opp_log(u8 opp) {}
+static inline void prcmu_debug_ddr_opp_log(u8 opp) {}
+static inline int prcmu_debug_init(void) {return 0;}
+#endif
+#endif
diff --git a/arch/arm/mach-ux500/prcmu-qos-power.c b/arch/arm/mach-ux500/prcmu-qos-power.c
new file mode 100644
index 00000000000..082393bd7eb
--- /dev/null
+++ b/arch/arm/mach-ux500/prcmu-qos-power.c
@@ -0,0 +1,610 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Martin Persson <martin.persson@stericsson.com>
+ *         Per Fransson <per.xx.fransson@stericsson.com>
+ *
+ * Quality of Service for the U8500 PRCM Unit interface driver
+ *
+ * Strongly influenced by kernel/pm_qos_params.c.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/cpufreq.h>
+
+#include "prcmu-debug.h"
+#include <mach/prcmu.h>
+
+#define ARM_THRESHOLD_FREQ (400000)
+
+static int qos_delayed_cpufreq_notifier(struct notifier_block *,
+					unsigned long, void *);
+
+static s32 cpufreq_requirement_queued;
+static s32 cpufreq_requirement_set;
+
+/*
+ * locking rule: all changes to requirements or prcmu_qos_object list
+ * and prcmu_qos_objects need to happen with prcmu_qos_lock
+ * held, taken with _irqsave.  One lock to rule them all
+ */
+struct requirement_list {
+	struct list_head list;
+	union {
+		s32 value;
+		s32 usec;
+		s32 kbps;
+	};
+	char *name;
+};
+
+static s32 max_compare(s32 v1, s32 v2);
+
+struct prcmu_qos_object {
+	struct requirement_list requirements;
+	struct blocking_notifier_head *notifiers;
+	struct miscdevice prcmu_qos_power_miscdev;
+	char *name;
+	s32 default_value;
+	s32 force_value;
+	atomic_t target_value;
+	s32 (*comparitor)(s32, s32);
+};
+
+static struct prcmu_qos_object null_qos;
+static BLOCKING_NOTIFIER_HEAD(prcmu_ape_opp_notifier);
+static BLOCKING_NOTIFIER_HEAD(prcmu_ddr_opp_notifier);
+
+static struct prcmu_qos_object ape_opp_qos = {
+	.requirements =	{
+		LIST_HEAD_INIT(ape_opp_qos.requirements.list)
+	},
+	.notifiers = &prcmu_ape_opp_notifier,
+	.name = "ape_opp",
+	/* Target value in % APE OPP */
+	.default_value = 50,
+	.force_value = 0,
+	.target_value = ATOMIC_INIT(0),
+	.comparitor = max_compare
+};
+
+static struct prcmu_qos_object ddr_opp_qos = {
+	.requirements =	{
+		LIST_HEAD_INIT(ddr_opp_qos.requirements.list)
+	},
+	.notifiers = &prcmu_ddr_opp_notifier,
+	.name = "ddr_opp",
+	/* Target value in % DDR OPP */
+	.default_value = 25,
+	.force_value = 0,
+	.target_value = ATOMIC_INIT(0),
+	.comparitor = max_compare
+};
+
+static struct prcmu_qos_object *prcmu_qos_array[] = {
+	&null_qos,
+	&ape_opp_qos,
+	&ddr_opp_qos
+};
+
+static DEFINE_MUTEX(prcmu_qos_mutex);
+static DEFINE_SPINLOCK(prcmu_qos_lock);
+
+static bool ape_opp_forced_to_50_partly_25;
+
+static unsigned long cpufreq_opp_delay = HZ / 5;
+
+unsigned long prcmu_qos_get_cpufreq_opp_delay(void)
+{
+	return cpufreq_opp_delay;
+}
+
+static struct notifier_block qos_delayed_cpufreq_notifier_block = {
+	.notifier_call = qos_delayed_cpufreq_notifier,
+};
+
+void prcmu_qos_set_cpufreq_opp_delay(unsigned long n)
+{
+	if (n == 0) {
+		cpufreq_unregister_notifier(&qos_delayed_cpufreq_notifier_block,
+					    CPUFREQ_TRANSITION_NOTIFIER);
+		prcmu_qos_update_requirement(PRCMU_QOS_DDR_OPP, "cpufreq",
+					     PRCMU_QOS_DEFAULT_VALUE);
+		prcmu_qos_update_requirement(PRCMU_QOS_APE_OPP, "cpufreq",
+					     PRCMU_QOS_DEFAULT_VALUE);
+		cpufreq_requirement_set = PRCMU_QOS_DEFAULT_VALUE;
+		cpufreq_requirement_queued = PRCMU_QOS_DEFAULT_VALUE;
+	} else if (cpufreq_opp_delay != 0) {
+		cpufreq_register_notifier(&qos_delayed_cpufreq_notifier_block,
+					    CPUFREQ_TRANSITION_NOTIFIER);
+	}
+	cpufreq_opp_delay = n;
+}
+
+/* static helper function */
+static s32 max_compare(s32 v1, s32 v2)
+{
+	return max(v1, v2);
+}
+
+static void update_target(int target)
+{
+	s32 extreme_value;
+	struct requirement_list *node;
+	unsigned long flags;
+	int update = 0;
+	u8 op;
+
+	mutex_lock(&prcmu_qos_mutex);
+
+	spin_lock_irqsave(&prcmu_qos_lock, flags);
+	extreme_value = prcmu_qos_array[target]->default_value;
+
+	if (prcmu_qos_array[target]->force_value != 0) {
+		extreme_value = prcmu_qos_array[target]->force_value;
+		update = 1;
+	} else {
+		list_for_each_entry(node,
+				    &prcmu_qos_array[target]->requirements.list,
+				    list) {
+			extreme_value = prcmu_qos_array[target]->comparitor(
+				extreme_value, node->value);
+		}
+		if (atomic_read(&prcmu_qos_array[target]->target_value)
+		    != extreme_value) {
+			update = 1;
+			atomic_set(&prcmu_qos_array[target]->target_value,
+				   extreme_value);
+			pr_debug("prcmu qos: new target for qos %d is %d\n",
+				 target, atomic_read(
+					 &prcmu_qos_array[target]->target_value
+					 ));
+		}
+	}
+	spin_unlock_irqrestore(&prcmu_qos_lock, flags);
+
+	if (update) {
+		blocking_notifier_call_chain(prcmu_qos_array[target]->notifiers,
+			(unsigned long)extreme_value, NULL);
+
+		if (target == PRCMU_QOS_DDR_OPP) {
+			switch (extreme_value) {
+			case 25:
+				op = DDR_25_OPP;
+				pr_debug("prcmu qos: set ddr opp to 25%%\n");
+				break;
+			case 50:
+				op = DDR_50_OPP;
+				pr_debug("prcmu qos: set ddr opp to 50%%\n");
+				break;
+			case 100:
+				op = DDR_100_OPP;
+				pr_debug("prcmu qos: set ddr opp to 100%%\n");
+				break;
+			default:
+				pr_err("prcmu qos: Incorrect ddr target value (%d)",
+				       extreme_value);
+				goto unlock_and_return;
+			}
+			prcmu_debug_ddr_opp_log(op);
+			prcmu_set_ddr_opp(op);
+		} else {
+			switch (extreme_value) {
+			case 50:
+				op = APE_50_OPP;
+				pr_debug("prcmu qos: set ape opp to 50%%\n");
+				break;
+			case 100:
+				op = APE_100_OPP;
+				pr_debug("prcmu qos: set ape opp to 100%%\n");
+				break;
+			default:
+				pr_err("prcmu qos: Incorrect ape target value (%d)",
+				       extreme_value);
+				goto unlock_and_return;
+			}
+
+			if (!ape_opp_forced_to_50_partly_25)
+				(void)prcmu_set_ape_opp(op);
+
+			prcmu_debug_ape_opp_log(op);
+		}
+	}
+unlock_and_return:
+	mutex_unlock(&prcmu_qos_mutex);
+}
+
+void prcmu_qos_force_opp(int prcmu_qos_class, s32 i)
+{
+	prcmu_qos_array[prcmu_qos_class]->force_value = i;
+	update_target(prcmu_qos_class);
+}
+
+void prcmu_qos_voice_call_override(bool enable)
+{
+	u8 op;
+
+	mutex_lock(&prcmu_qos_mutex);
+
+	ape_opp_forced_to_50_partly_25 = enable;
+
+	if (enable) {
+		(void)prcmu_set_ape_opp(APE_50_PARTLY_25_OPP);
+		goto unlock_and_return;
+	}
+
+	/* Disable: set the OPP according to the current target value. */
+	switch (atomic_read(
+			&prcmu_qos_array[PRCMU_QOS_APE_OPP]->target_value)) {
+	case 50:
+		op = APE_50_OPP;
+		break;
+	case 100:
+		op = APE_100_OPP;
+		break;
+	default:
+		goto unlock_and_return;
+	}
+
+	(void)prcmu_set_ape_opp(op);
+
+unlock_and_return:
+	mutex_unlock(&prcmu_qos_mutex);
+}
+
+/**
+ * prcmu_qos_requirement - returns current prcmu qos expectation
+ * @prcmu_qos_class: identification of which qos value is requested
+ *
+ * This function returns the current target value in an atomic manner.
+ */
+int prcmu_qos_requirement(int prcmu_qos_class)
+{
+	return atomic_read(&prcmu_qos_array[prcmu_qos_class]->target_value);
+}
+EXPORT_SYMBOL_GPL(prcmu_qos_requirement);
+
+/**
+ * prcmu_qos_add_requirement - inserts new qos request into the list
+ * @prcmu_qos_class: identifies which list of qos request to us
+ * @name: identifies the request
+ * @value: defines the qos request
+ *
+ * This function inserts a new entry in the prcmu_qos_class list of requested
+ * qos performance characteristics.  It recomputes the aggregate QoS
+ * expectations for the prcmu_qos_class of parameters.
+ */
+int prcmu_qos_add_requirement(int prcmu_qos_class, char *name, s32 value)
+{
+	struct requirement_list *dep;
+	unsigned long flags;
+
+	dep = kzalloc(sizeof(struct requirement_list), GFP_KERNEL);
+	if (dep == NULL)
+		return -ENOMEM;
+
+	if (value == PRCMU_QOS_DEFAULT_VALUE)
+		dep->value = prcmu_qos_array[prcmu_qos_class]->default_value;
+	else
+		dep->value = value;
+	dep->name = kstrdup(name, GFP_KERNEL);
+	if (!dep->name)
+		goto cleanup;
+
+	spin_lock_irqsave(&prcmu_qos_lock, flags);
+	list_add(&dep->list,
+		 &prcmu_qos_array[prcmu_qos_class]->requirements.list);
+	spin_unlock_irqrestore(&prcmu_qos_lock, flags);
+	update_target(prcmu_qos_class);
+
+	return 0;
+
+cleanup:
+	kfree(dep);
+	return -ENOMEM;
+}
+EXPORT_SYMBOL_GPL(prcmu_qos_add_requirement);
+
+/**
+ * prcmu_qos_update_requirement - modifies an existing qos request
+ * @prcmu_qos_class: identifies which list of qos request to us
+ * @name: identifies the request
+ * @value: defines the qos request
+ *
+ * Updates an existing qos requirement for the prcmu_qos_class of parameters
+ * along with updating the target prcmu_qos_class value.
+ *
+ * If the named request isn't in the list then no change is made.
+ */
+int prcmu_qos_update_requirement(int prcmu_qos_class, char *name, s32 new_value)
+{
+	unsigned long flags;
+	struct requirement_list *node;
+	int pending_update = 0;
+
+	spin_lock_irqsave(&prcmu_qos_lock, flags);
+	list_for_each_entry(node,
+		&prcmu_qos_array[prcmu_qos_class]->requirements.list, list) {
+		if (strcmp(node->name, name) == 0) {
+			if (new_value == PRCMU_QOS_DEFAULT_VALUE)
+				node->value =
+				prcmu_qos_array[prcmu_qos_class]->default_value;
+			else
+				node->value = new_value;
+			pending_update = 1;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&prcmu_qos_lock, flags);
+	if (pending_update)
+		update_target(prcmu_qos_class);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(prcmu_qos_update_requirement);
+
+/**
+ * prcmu_qos_remove_requirement - modifies an existing qos request
+ * @prcmu_qos_class: identifies which list of qos request to us
+ * @name: identifies the request
+ *
+ * Will remove named qos request from prcmu_qos_class list of parameters and
+ * recompute the current target value for the prcmu_qos_class.
+ */
+void prcmu_qos_remove_requirement(int prcmu_qos_class, char *name)
+{
+	unsigned long flags;
+	struct requirement_list *node;
+	int pending_update = 0;
+
+	spin_lock_irqsave(&prcmu_qos_lock, flags);
+	list_for_each_entry(node,
+		&prcmu_qos_array[prcmu_qos_class]->requirements.list, list) {
+		if (strcmp(node->name, name) == 0) {
+			kfree(node->name);
+			list_del(&node->list);
+			kfree(node);
+			pending_update = 1;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&prcmu_qos_lock, flags);
+	if (pending_update)
+		update_target(prcmu_qos_class);
+}
+EXPORT_SYMBOL_GPL(prcmu_qos_remove_requirement);
+
+/**
+ * prcmu_qos_add_notifier - sets notification entry for changes to target value
+ * @prcmu_qos_class: identifies which qos target changes should be notified.
+ * @notifier: notifier block managed by caller.
+ *
+ * will register the notifier into a notification chain that gets called
+ * upon changes to the prcmu_qos_class target value.
+ */
+int prcmu_qos_add_notifier(int prcmu_qos_class, struct notifier_block *notifier)
+{
+	int retval;
+
+	retval = blocking_notifier_chain_register(
+			prcmu_qos_array[prcmu_qos_class]->notifiers, notifier);
+
+	return retval;
+}
+EXPORT_SYMBOL_GPL(prcmu_qos_add_notifier);
+
+/**
+ * prcmu_qos_remove_notifier - deletes notification entry from chain.
+ * @prcmu_qos_class: identifies which qos target changes are notified.
+ * @notifier: notifier block to be removed.
+ *
+ * will remove the notifier from the notification chain that gets called
+ * upon changes to the prcmu_qos_class target value.
+ */
+int prcmu_qos_remove_notifier(int prcmu_qos_class,
+			      struct notifier_block *notifier)
+{
+	int retval;
+
+	retval = blocking_notifier_chain_unregister(
+			prcmu_qos_array[prcmu_qos_class]->notifiers, notifier);
+
+	return retval;
+}
+EXPORT_SYMBOL_GPL(prcmu_qos_remove_notifier);
+
+#define USER_QOS_NAME_LEN 32
+
+static int prcmu_qos_power_open(struct inode *inode, struct file *filp,
+				long prcmu_qos_class)
+{
+	int ret;
+	char name[USER_QOS_NAME_LEN];
+
+	filp->private_data = (void *)prcmu_qos_class;
+	snprintf(name, USER_QOS_NAME_LEN, "file_%08x", (unsigned int)filp);
+	ret = prcmu_qos_add_requirement(prcmu_qos_class, name,
+					PRCMU_QOS_DEFAULT_VALUE);
+	if (ret >= 0)
+		return 0;
+
+	return -EPERM;
+}
+
+
+static int prcmu_qos_ape_power_open(struct inode *inode, struct file *filp)
+{
+	return prcmu_qos_power_open(inode, filp, PRCMU_QOS_APE_OPP);
+}
+
+static int prcmu_qos_ddr_power_open(struct inode *inode, struct file *filp)
+{
+	return prcmu_qos_power_open(inode, filp, PRCMU_QOS_DDR_OPP);
+}
+
+static int prcmu_qos_power_release(struct inode *inode, struct file *filp)
+{
+	int prcmu_qos_class;
+	char name[USER_QOS_NAME_LEN];
+
+	prcmu_qos_class = (long)filp->private_data;
+	snprintf(name, USER_QOS_NAME_LEN, "file_%08x", (unsigned int)filp);
+	prcmu_qos_remove_requirement(prcmu_qos_class, name);
+
+	return 0;
+}
+
+static ssize_t prcmu_qos_power_write(struct file *filp, const char __user *buf,
+		size_t count, loff_t *f_pos)
+{
+	s32 value;
+	int prcmu_qos_class;
+	char name[USER_QOS_NAME_LEN];
+
+	prcmu_qos_class = (long)filp->private_data;
+	if (count != sizeof(s32))
+		return -EINVAL;
+	if (copy_from_user(&value, buf, sizeof(s32)))
+		return -EFAULT;
+	snprintf(name, USER_QOS_NAME_LEN, "file_%08x", (unsigned int)filp);
+	prcmu_qos_update_requirement(prcmu_qos_class, name, value);
+
+	return  sizeof(s32);
+}
+
+/* Functions to provide QoS to user space */
+static const struct file_operations prcmu_qos_ape_power_fops = {
+	.write = prcmu_qos_power_write,
+	.open = prcmu_qos_ape_power_open,
+	.release = prcmu_qos_power_release,
+};
+
+/* Functions to provide QoS to user space */
+static const struct file_operations prcmu_qos_ddr_power_fops = {
+	.write = prcmu_qos_power_write,
+	.open = prcmu_qos_ddr_power_open,
+	.release = prcmu_qos_power_release,
+};
+
+static int register_prcmu_qos_misc(struct prcmu_qos_object *qos,
+				   const struct file_operations *fops)
+{
+	qos->prcmu_qos_power_miscdev.minor = MISC_DYNAMIC_MINOR;
+	qos->prcmu_qos_power_miscdev.name = qos->name;
+	qos->prcmu_qos_power_miscdev.fops = fops;
+
+	return misc_register(&qos->prcmu_qos_power_miscdev);
+}
+
+static void qos_delayed_work_up_fn(struct work_struct *work)
+{
+	prcmu_qos_update_requirement(PRCMU_QOS_DDR_OPP, "cpufreq", 100);
+	prcmu_qos_update_requirement(PRCMU_QOS_APE_OPP, "cpufreq", 100);
+	cpufreq_requirement_set = 100;
+}
+
+static void qos_delayed_work_down_fn(struct work_struct *work)
+{
+	prcmu_qos_update_requirement(PRCMU_QOS_DDR_OPP, "cpufreq",
+				     PRCMU_QOS_DEFAULT_VALUE);
+	prcmu_qos_update_requirement(PRCMU_QOS_APE_OPP, "cpufreq",
+				     PRCMU_QOS_DEFAULT_VALUE);
+	cpufreq_requirement_set = PRCMU_QOS_DEFAULT_VALUE;
+}
+
+static DECLARE_DELAYED_WORK(qos_delayed_work_up, qos_delayed_work_up_fn);
+static DECLARE_DELAYED_WORK(qos_delayed_work_down, qos_delayed_work_down_fn);
+
+static int qos_delayed_cpufreq_notifier(struct notifier_block *nb,
+			unsigned long event, void *data)
+{
+	struct cpufreq_freqs *freq = data;
+	s32 new_ddr_target;
+
+	/* Only react once per transition and only for one core, e.g. core 0 */
+	if (event != CPUFREQ_POSTCHANGE || freq->cpu != 0)
+		return 0;
+
+	/*
+	 * APE and DDR OPP are always handled together in this solution.
+	 * Hence no need to check both DDR and APE opp in the code below.
+	 */
+
+	/* Which DDR OPP are we aiming for? */
+	if (freq->new > ARM_THRESHOLD_FREQ)
+		new_ddr_target = 100;
+	else
+		new_ddr_target = PRCMU_QOS_DEFAULT_VALUE;
+
+	if (new_ddr_target == cpufreq_requirement_queued) {
+		/*
+		 * We're already at, or going to, the target requirement.
+		 * This is only a fluctuation within the interval
+		 * corresponding to the same DDR requirement.
+		 */
+		return 0;
+	}
+	cpufreq_requirement_queued = new_ddr_target;
+
+	if (freq->new > ARM_THRESHOLD_FREQ) {
+		cancel_delayed_work_sync(&qos_delayed_work_down);
+		/*
+		 * Only schedule this requirement if it is not the current
+		 * one.
+		 */
+		if (new_ddr_target != cpufreq_requirement_set)
+			schedule_delayed_work(&qos_delayed_work_up,
+					      cpufreq_opp_delay);
+	} else {
+		cancel_delayed_work_sync(&qos_delayed_work_up);
+		/*
+		 * Only schedule this requirement if it is not the current
+		 * one.
+		 */
+		if (new_ddr_target != cpufreq_requirement_set)
+			schedule_delayed_work(&qos_delayed_work_down,
+					      cpufreq_opp_delay);
+	}
+
+	return 0;
+}
+
+static int __init prcmu_qos_power_init(void)
+{
+	int ret = 0;
+
+	ret = register_prcmu_qos_misc(&ape_opp_qos, &prcmu_qos_ape_power_fops);
+	if (ret < 0) {
+		pr_err("prcmu ape qos: setup failed\n");
+		return ret;
+	}
+
+	ret = register_prcmu_qos_misc(&ddr_opp_qos, &prcmu_qos_ddr_power_fops);
+	if (ret < 0) {
+		pr_err("prcmu ddr qos: setup failed\n");
+		return ret;
+	}
+
+	prcmu_qos_add_requirement(PRCMU_QOS_DDR_OPP, "cpufreq",
+				  PRCMU_QOS_DEFAULT_VALUE);
+	prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP, "cpufreq",
+				  PRCMU_QOS_DEFAULT_VALUE);
+	cpufreq_requirement_set = PRCMU_QOS_DEFAULT_VALUE;
+	cpufreq_requirement_queued = PRCMU_QOS_DEFAULT_VALUE;
+
+	cpufreq_register_notifier(&qos_delayed_cpufreq_notifier_block,
+				  CPUFREQ_TRANSITION_NOTIFIER);
+
+	return ret;
+}
+
+late_initcall(prcmu_qos_power_init);
diff --git a/arch/arm/mach-ux500/reboot_reasons.c b/arch/arm/mach-ux500/reboot_reasons.c
new file mode 100644
index 00000000000..0796d3de0b3
--- /dev/null
+++ b/arch/arm/mach-ux500/reboot_reasons.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Rickard Evertsson <rickard.evertsson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Use this file to customize your reboot / sw reset reasons. Add, remove or
+ * modify reasons in reboot_reasons[].
+ */
+
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <mach/reboot_reasons.h>
+
+struct reboot_reason reboot_reasons[] = {
+	{"crash", SW_RESET_CRASH},
+	{"factory-reset", SW_RESET_FACTORY_RESET},
+	{"recovery", SW_RESET_NORMAL},
+	{"", SW_RESET_NORMAL}, /* Normal Boot */
+};
+
+unsigned int reboot_reasons_size = ARRAY_SIZE(reboot_reasons);
+
+/*
+ * The reboot reason string can be 255 characters long and the memory
+ * in which we save the sw reset reason is 2 bytes. Therefore we need to
+ * convert the string into a 16 bit pattern.
+ *
+ * See file reboot_reasons.h for conversion.
+ */
+u16 reboot_reason_code(const char *cmd)
+{
+	int i;
+
+	if (cmd == NULL)
+		/* normal reboot w/o argument */
+		return SW_RESET_NO_ARGUMENT;
+
+	/* Search through reboot reason list */
+	for (i = 0; i < reboot_reasons_size; i++) {
+		if (!strcmp(reboot_reasons[i].reason, cmd))
+			return reboot_reasons[i].code;
+	}
+
+	/* No valid reboot reason found */
+	return SW_RESET_CRASH;
+}
+
+/*
+ * The saved sw reset reason is a 2 byte code that is translated into
+ * a reboot reason string which is up to 255 characters long by this
+ * function.
+ *
+ * See file reboot_reasons.h for conversion.
+ */
+const char *reboot_reason_string(u16 code)
+{
+	int i;
+
+	/* Search through reboot reason list */
+	for (i = 0; i < reboot_reasons_size; i++) {
+		if (reboot_reasons[i].code == code)
+			return reboot_reasons[i].reason;
+	}
+
+	/* No valid reboot reason code found */
+	return "";
+}
diff --git a/arch/arm/mach-ux500/regulator-ux500.h b/arch/arm/mach-ux500/regulator-ux500.h
new file mode 100644
index 00000000000..b62863881b1
--- /dev/null
+++ b/arch/arm/mach-ux500/regulator-ux500.h
@@ -0,0 +1,39 @@
+#ifndef __REGULATOR_UX500_H
+#define __REGULATOR_UX500_H
+
+struct platform_device;
+
+/**
+ * struct u8500_regulator_info - u8500 regulator information
+ * @dev: device pointer
+ * @desc: regulator description
+ * @rdev: regulator device pointer
+ * @is_enabled: status of the regulator
+ * @epod_id: id for EPOD (power domain)
+ * @is_ramret: RAM retention switch for EPOD (power domain)
+ * @operating_point: operating point (only for vape, to be removed)
+ * @exclude_from_power_state: don't let this regulator prevent ApSLeep
+ */
+struct u8500_regulator_info {
+	struct device *dev;
+	struct regulator_desc desc;
+	struct regulator_dev *rdev;
+	bool is_enabled;
+	u16 epod_id;
+	bool is_ramret;
+	bool exclude_from_power_state;
+	unsigned int operating_point;
+};
+
+extern struct regulator_ops ux500_regulator_ops;
+extern struct regulator_ops ux500_regulator_switch_ops;
+
+int ux500_regulator_probe(struct platform_device *pdev,
+			  struct u8500_regulator_info *info,
+			  int num_regulators);
+
+int ux500_regulator_remove(struct platform_device *pdev,
+			   struct u8500_regulator_info *info,
+			   int num_regulators);
+
+#endif
diff --git a/arch/arm/mach-ux500/tee_service_svp.c b/arch/arm/mach-ux500/tee_service_svp.c
new file mode 100644
index 00000000000..aa65dd961a0
--- /dev/null
+++ b/arch/arm/mach-ux500/tee_service_svp.c
@@ -0,0 +1,66 @@
+/*
+ * TEE service to handle the calls to trusted applications in SVP.
+ *
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/tee.h>
+#include <linux/err.h>
+#include "mach/tee_ta_start_modem.h"
+
+static int cmp_uuid_start_modem(struct tee_uuid *uuid)
+{
+	int ret = -EINVAL;
+
+	if (uuid == NULL)
+		return -EINVAL;
+
+	/* This handles the calls to TA for start the modem */
+	if ((uuid->timeLow == UUID_TEE_TA_START_MODEM_LOW) &&
+	    (uuid->timeMid == UUID_TEE_TA_START_MODEM_MID) &&
+	    (uuid->timeHiAndVersion == UUID_TEE_TA_START_MODEM_HIGH)) {
+
+		u8 clockSeqAndNode[TEE_UUID_CLOCK_SIZE] =
+			UUID_TEE_TA_START_MODEM_CLOCKSEQ;
+
+		ret = memcmp(uuid->clockSeqAndNode, clockSeqAndNode,
+			     TEE_UUID_CLOCK_SIZE);
+	}
+
+	return ret;
+}
+
+int call_sec_world(struct tee_session *ts, int sec_cmd)
+{
+	int ret = 0;
+
+	if (ts == NULL)
+		return -EINVAL;
+
+	if (cmp_uuid_start_modem(ts->uuid))
+		return -EINVAL;
+
+	switch (ts->cmd) {
+	case COMMAND_ID_START_MODEM:
+		ret = tee_ta_start_modem((struct tee_ta_start_modem *)
+					 ts->op);
+		if (ret) {
+			ts->err = TEED_ERROR_GENERIC;
+			ts->origin = TEED_ORIGIN_TEE_APPLICATION;
+			pr_err("tee_ta_start_modem() failed!\n");
+			return ret;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	/* TODO: to handle more trusted applications. */
+
+	return ret;
+}
diff --git a/arch/arm/mach-ux500/tee_ta_start_modem_svp.c b/arch/arm/mach-ux500/tee_ta_start_modem_svp.c
new file mode 100644
index 00000000000..12337b93154
--- /dev/null
+++ b/arch/arm/mach-ux500/tee_ta_start_modem_svp.c
@@ -0,0 +1,56 @@
+/*
+ * Trusted application for starting the modem.
+ *
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/elf.h>
+#include <mach/hardware.h>
+
+#include "mach/tee_ta_start_modem.h"
+
+static int reset_modem(unsigned long modem_start_addr)
+{
+	void __iomem *base = ioremap(U5500_ACCCON_BASE_SEC, 0x2FF);
+	if (!base)
+		return -ENOMEM;
+
+	pr_info("[%s] Setting modem start address!\n", __func__);
+	writel(base + (U5500_ACCCON_CPUVEC_RESET_ADDR_OFFSET/sizeof(uint32_t)),
+	       modem_start_addr);
+
+	pr_info("[%s] resetting the modem!\n", __func__);
+	writel(base + (U5500_ACCCON_ACC_CPU_CTRL_OFFSET/sizeof(uint32_t)), 1);
+
+	iounmap(base);
+
+	return 0;
+}
+
+int tee_ta_start_modem(struct tee_ta_start_modem *data)
+{
+	int ret = 0;
+	struct elfhdr *elfhdr;
+	void __iomem *vaddr;
+
+	vaddr = ioremap((unsigned long)data->access_image_descr.elf_hdr,
+			sizeof(struct elfhdr));
+	if (!vaddr)
+		return -ENOMEM;
+
+	elfhdr = (struct elfhdr *)readl(vaddr);
+	pr_info("Reading in kernel:elfhdr 0x%x:elfhdr->entry=0x%x\n",
+			(uint32_t)elfhdr, (uint32_t)elfhdr->e_entry);
+
+	pr_info("[%s] reset modem()...\n", __func__);
+	ret = reset_modem(elfhdr->e_entry);
+
+	iounmap(vaddr);
+
+	return ret;
+}
diff --git a/arch/arm/mach-ux500/tee_ux500.c b/arch/arm/mach-ux500/tee_ux500.c
new file mode 100644
index 00000000000..160ca529261
--- /dev/null
+++ b/arch/arm/mach-ux500/tee_ux500.c
@@ -0,0 +1,95 @@
+/*
+ * TEE service to handle the calls to trusted applications.
+ *
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#include <linux/kernel.h>
+#include <linux/tee.h>
+#include <linux/io.h>
+#include <linux/errno.h>
+
+#include <mach/hardware.h>
+
+#define ISSWAPI_EXECUTE_TA 0x11000001
+#define ISSWAPI_CLOSE_TA   0x11000002
+
+#define SEC_ROM_NO_FLAG_MASK    0x0000
+
+static u32 call_sec_rom_bridge(u32 service_id, u32 cfg, ...)
+{
+	typedef u32 (*bridge_func)(u32, u32, va_list);
+	bridge_func hw_sec_rom_pub_bridge;
+	va_list ap;
+	u32 ret;
+
+	if (cpu_is_u8500v20_or_later())
+		hw_sec_rom_pub_bridge = (bridge_func)
+			((u32)IO_ADDRESS(U8500_BOOT_ROM_BASE + 0x17300));
+	else if (cpu_is_u5500())
+		hw_sec_rom_pub_bridge = (bridge_func)
+			((u32)IO_ADDRESS(U5500_BOOT_ROM_BASE + 0x18300));
+	else
+		ux500_unknown_soc();
+
+	va_start(ap, cfg);
+	ret = hw_sec_rom_pub_bridge(service_id, cfg, ap);
+	va_end(ap);
+
+	return ret;
+}
+
+int call_sec_world(struct tee_session *ts, int sec_cmd)
+{
+	/*
+	 * ts->ta and ts->uuid is set to NULL when opening the device,
+	 * hence it should be safe to just do the call here.
+	 */
+
+	switch (sec_cmd) {
+	case TEED_INVOKE:
+	if (!ts->uuid) {
+		call_sec_rom_bridge(ISSWAPI_EXECUTE_TA,
+				SEC_ROM_NO_FLAG_MASK,
+				virt_to_phys(&ts->id),
+				NULL,
+				virt_to_phys(ts->ta),
+				ts->cmd,
+				virt_to_phys((void *)(ts->op)),
+				virt_to_phys((void *)(&ts->err)),
+				virt_to_phys((void *)(&ts->origin)));
+	} else {
+		call_sec_rom_bridge(ISSWAPI_EXECUTE_TA,
+				SEC_ROM_NO_FLAG_MASK,
+				virt_to_phys(&ts->id),
+				virt_to_phys(ts->uuid),
+				virt_to_phys(ts->ta),
+				ts->cmd,
+				virt_to_phys((void *)(ts->op)),
+				virt_to_phys((void *)(&ts->err)),
+				virt_to_phys((void *)(&ts->origin)));
+	}
+	break;
+
+	case TEED_CLOSE_SESSION:
+		call_sec_rom_bridge(ISSWAPI_CLOSE_TA,
+				SEC_ROM_NO_FLAG_MASK,
+				ts->id,
+				NULL,
+				virt_to_phys(ts->ta),
+				virt_to_phys((void *)(&ts->err)));
+
+		/* Since the TEE Client API does NOT take care of
+		 * the return value, we print a warning here if
+		 * something went wrong in secure world.
+		 */
+		if (ts->err != TEED_SUCCESS)
+				pr_warning("[%s] failed in secure world\n",
+							__func__);
+
+	break;
+	}
+
+	return 0;
+}
diff --git a/arch/arm/mach-ux500/timer-mtu.c b/arch/arm/mach-ux500/timer-mtu.c
new file mode 100644
index 00000000000..b06559787c6
--- /dev/null
+++ b/arch/arm/mach-ux500/timer-mtu.c
@@ -0,0 +1,313 @@
+/*
+ * linux/arch/arm/mach-u8500/timer.c
+ *
+ * Copyright (C) 2008 STMicroelectronics
+ * Copyright (C) 2009 Alessandro Rubini, somewhat based on at91sam926x
+ * Copyright (C) 2009 ST-Ericsson SA
+ *	added support to u8500 platform, heavily based on 8815
+ * 	Author: Srinidhi KASAGAR <srinidhi.kasagar@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ */
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/clockchips.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/jiffies.h>
+//#include <linux/boottime.h>
+#include <linux/cnt32_to_63.h>
+#include <asm/mach/time.h>
+#include <plat/mtu.h>
+#include <mach/setup.h>
+
+#define TIMER_CTRL	0x80	/* No divisor */
+#define TIMER_PERIODIC	0x40
+#define TIMER_SZ32BIT	0x02
+
+#define MTU_CRn_DIS		(0x00 << 7)
+#define MTU_CRn_FREERUNNING	(0x00 << 6)
+
+static u32 u8500_cycle;		/* write-once */
+static __iomem void *mtu0_base;
+static bool mtu_periodic;
+
+/*
+ * U8500 sched_clock implementation. It has a resolution of
+ * at least 7.5ns (133MHz MTU rate) and a maximum value of 834 days.
+ *
+ * Because the hardware timer period is quite short (32.3 secs
+ * and because cnt32_to_63() needs to be called at
+ * least once per half period to work properly, a kernel timer is
+ * set up to ensure this requirement is always met.
+ *
+ * Based on plat-orion time.c implementation.
+ */
+#define TCLK2NS_SCALE_FACTOR 8
+
+#ifdef CONFIG_UX500_MTU_TIMER
+static unsigned long tclk2ns_scale;
+static struct timer_list cnt32_to_63_keepwarm_timer;
+
+unsigned long long sched_clock(void)
+{
+	unsigned long long v;
+
+	if (unlikely(!mtu0_base))
+		return 0;
+
+	v = cnt32_to_63(0xffffffff - readl(mtu0_base + MTU_VAL(1)));
+	return (v * tclk2ns_scale) >> TCLK2NS_SCALE_FACTOR;
+}
+
+static void cnt32_to_63_keepwarm(unsigned long data)
+{
+	mod_timer(&cnt32_to_63_keepwarm_timer, round_jiffies(jiffies + data));
+	(void) sched_clock();
+}
+
+static void __init setup_sched_clock(unsigned long tclk)
+{
+	unsigned long long v;
+	unsigned long data;
+
+	v = NSEC_PER_SEC;
+	v <<= TCLK2NS_SCALE_FACTOR;
+	v += tclk / 2;
+	do_div(v, tclk);
+	/*
+	 * We want an even value to automatically clear the top bit
+	 * returned by cnt32_to_63() without an additional run time
+	 * instruction. So if the LSB is 1 then round it up.
+	 */
+	if (v & 1)
+		v++;
+	tclk2ns_scale = v;
+
+	data = (0xffffffffUL / tclk / 2 - 2) * HZ;
+	setup_timer(&cnt32_to_63_keepwarm_timer, cnt32_to_63_keepwarm, data);
+	mod_timer(&cnt32_to_63_keepwarm_timer, round_jiffies(jiffies + data));
+}
+#else
+static void __init setup_sched_clock(unsigned long tclk)
+{
+}
+#endif
+/*
+ * clocksource: the MTU device is a decrementing counters, so we negate
+ * the value being read.
+ */
+static cycle_t u8500_read_timer(struct clocksource *cs)
+{
+	u32 count = readl(mtu0_base + MTU_VAL(1));
+	return ~count;
+}
+/*
+ * Kernel assumes that sched_clock can be called early
+ * but the MTU may not yet be initialized.
+ */
+static cycle_t u8500_read_timer_dummy(struct clocksource *cs)
+{
+	return 0;
+}
+
+void mtu_clocksource_reset(void)
+{
+	writel(MTU_CRn_DIS, mtu0_base + MTU_CR(1));
+
+	/* ClockSource: configure load and background-load, and fire it up */
+	writel(u8500_cycle, mtu0_base + MTU_LR(1));
+	writel(u8500_cycle, mtu0_base + MTU_BGLR(1));
+
+	writel(MTU_CRn_PRESCALE_1 | MTU_CRn_32BITS | MTU_CRn_ENA |
+	       MTU_CRn_FREERUNNING, mtu0_base + MTU_CR(1));
+}
+
+static struct clocksource u8500_clksrc = {
+	.name		= "mtu_1",
+	.rating		= 120,
+	.read		= u8500_read_timer_dummy,
+	.shift		= 20,
+	.mask		= CLOCKSOURCE_MASK(32),
+	.flags		= CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+#ifdef ARCH_HAS_READ_CURRENT_TIMER
+void mtu_timer_delay_loop(unsigned long loops)
+{
+	unsigned long bclock, now;
+
+	bclock = u8500_read_timer(&u8500_clksrc);
+	do {
+		now = u8500_read_timer(&u8500_clksrc);
+		/* If timer have been cleared (suspend) or wrapped we exit */
+		if (unlikely(now < bclock))
+			return;
+	} while ((now - bclock) < loops);
+}
+
+/* Used to calibrate the delay */
+int read_current_timer(unsigned long *timer_val)
+{
+	*timer_val = u8500_read_timer(&u8500_clksrc);
+	return 0;
+}
+#endif
+
+/*
+ * Clockevent
+ */
+static int u8500_mtu_clkevt_next(unsigned long evt, struct clock_event_device *ev)
+{
+	writel(1 << 0, mtu0_base + MTU_IMSC);
+	writel(evt, mtu0_base + MTU_LR(0));
+	/* Load highest value, enable device, enable interrupts */
+	writel(MTU_CRn_ONESHOT | MTU_CRn_PRESCALE_1 |
+	       MTU_CRn_32BITS | MTU_CRn_ENA,
+	       mtu0_base + MTU_CR(0));
+	return 0;
+}
+
+void mtu_clockevent_reset(void)
+{
+	if (mtu_periodic) {
+
+		/* Timer: configure load and background-load, and fire it up */
+		writel(u8500_cycle, mtu0_base + MTU_LR(0));
+		writel(u8500_cycle, mtu0_base + MTU_BGLR(0));
+
+		writel(MTU_CRn_PERIODIC | MTU_CRn_PRESCALE_1 |
+		       MTU_CRn_32BITS | MTU_CRn_ENA,
+		       mtu0_base + MTU_CR(0));
+		writel(1 << 0, mtu0_base + MTU_IMSC);
+	} else {
+		/* Generate an interrupt to start the clockevent again */
+		u8500_mtu_clkevt_next(u8500_cycle, NULL);
+	}
+}
+
+static void u8500_mtu_clkevt_mode(enum clock_event_mode mode,
+			     struct clock_event_device *dev)
+{
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		mtu_periodic = true;
+		mtu_clockevent_reset();
+		break;
+	case CLOCK_EVT_MODE_ONESHOT:
+		mtu_periodic = false;
+		break;
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	case CLOCK_EVT_MODE_UNUSED:
+		writel(MTU_CRn_DIS, mtu0_base + MTU_CR(0));
+		writel(0, mtu0_base + MTU_IMSC);
+		break;
+	case CLOCK_EVT_MODE_RESUME:
+		break;
+	}
+}
+
+/*
+ * IRQ Handler for the timer 0 of the MTU block. The irq is not shared
+ * as we are the only users of mtu0 by now.
+ */
+static irqreturn_t u8500_timer_interrupt(int irq, void *dev)
+{
+	struct clock_event_device *clkevt = dev;
+	/* ack: "interrupt clear register" */
+	writel(1 << 0, mtu0_base + MTU_ICR);
+
+	clkevt->event_handler(clkevt);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Added here as asm/smp.h is removed in v2.6.34 and
+ * this funcitons is needed for current PM setup.
+ */
+#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
+void smp_timer_broadcast(const struct cpumask *mask);
+#endif
+
+struct clock_event_device u8500_mtu_clkevt = {
+	.name		= "mtu_0",
+	.features	= CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
+	/* Must be of higher rating the timer-rtt but lower than localtimers */
+	.rating		= 310,
+	.set_mode	= u8500_mtu_clkevt_mode,
+	.set_next_event	= u8500_mtu_clkevt_next,
+	.irq		= IRQ_MTU0,
+#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
+	.broadcast      = smp_timer_broadcast,
+#endif
+	.cpumask	= cpu_all_mask,
+};
+
+/*
+ * Set up timer interrupt, and return the current time in seconds.
+ */
+static struct irqaction u8500_timer_irq = {
+	.name		= "MTU Timer Tick",
+	.flags		= IRQF_DISABLED | IRQF_TIMER,
+	.handler	= u8500_timer_interrupt,
+	.dev_id		= &u8500_mtu_clkevt,
+};
+
+void __init mtu_timer_init(void)
+{
+	unsigned long rate;
+	struct clk *clk0;
+
+	clk0 = clk_get_sys("mtu0", NULL);
+	BUG_ON(IS_ERR(clk0));
+
+	rate = clk_get_rate(clk0);
+
+	clk_enable(clk0);
+
+	/*
+	 * Set scale and timer for sched_clock
+	 */
+	setup_sched_clock(rate);
+	u8500_cycle = (rate + HZ/2) / HZ;
+
+	/* Save global pointer to mtu, used by functions above */
+	if (cpu_is_u5500()) {
+		mtu0_base = ioremap(U5500_MTU0_BASE, SZ_4K);
+	} else if (cpu_is_u8500()) {
+		mtu0_base = ioremap(U8500_MTU0_BASE, SZ_4K);
+	} else {
+		ux500_unknown_soc();
+	}
+
+	/* Restart clock source */
+	mtu_clocksource_reset();
+
+	/* Now the scheduling clock is ready */
+	u8500_clksrc.read = u8500_read_timer;
+	u8500_clksrc.mult = clocksource_hz2mult(rate, u8500_clksrc.shift);
+
+	clocksource_register(&u8500_clksrc);
+
+	/* Register irq and clockevents */
+
+	/* We can sleep for max 10s (actually max is longer) */
+	clockevents_calc_mult_shift(&u8500_mtu_clkevt, rate, 10);
+
+	u8500_mtu_clkevt.max_delta_ns = clockevent_delta2ns(0xffffffff,
+							    &u8500_mtu_clkevt);
+	u8500_mtu_clkevt.min_delta_ns = clockevent_delta2ns(0xff,
+							    &u8500_mtu_clkevt);
+
+	setup_irq(IRQ_MTU0, &u8500_timer_irq);
+	clockevents_register_device(&u8500_mtu_clkevt);
+#ifdef ARCH_HAS_READ_CURRENT_TIMER
+	set_delay_fn(mtu_timer_delay_loop);
+#endif
+}
diff --git a/arch/arm/mach-ux500/timer-prcmu.c b/arch/arm/mach-ux500/timer-prcmu.c
new file mode 100644
index 00000000000..24f6b48e970
--- /dev/null
+++ b/arch/arm/mach-ux500/timer-prcmu.c
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Sundar Iyer <sundar.iyer@stericsson.com> for ST-Ericsson
+ * Author: Mattias Wallin <mattias.wallin@stericsson.com> for ST-Ericsson
+ * sched_clock implementation is based on:
+ * plat-nomadik/timer.c Linus Walleij <linus.walleij@stericsson.com>
+ *
+ * DBx500-PRCMU Timer
+ * The PRCMU has 5 timers which are available in a always-on
+ * power domain.  We use the Timer 4 for our always-on clock
+ * source on DB8500 and Timer 3 on DB5500.
+ */
+#include <linux/clockchips.h>
+#include <linux/clk.h>
+#include <linux/jiffies.h>
+#include <linux/boottime.h>
+#include <linux/cnt32_to_63.h>
+#include <linux/sched.h>
+#include <mach/setup.h>
+#include <mach/db8500-regs.h>
+#include <mach/hardware.h>
+
+#define RATE_32K		(32768)
+
+#define TIMER_MODE_CONTINOUS	(0x1)
+#define TIMER_DOWNCOUNT_VAL	(0xffffffff)
+
+#define PRCMU_TIMER_3_BASE       0x338
+#define PRCMU_TIMER_4_BASE       0x450
+
+#define PRCMU_TIMER_REF       0x0
+#define PRCMU_TIMER_DOWNCOUNT 0x4
+#define PRCMU_TIMER_MODE      0x8
+
+static __iomem void *timer_base;
+
+#define SCHED_CLOCK_MIN_WRAP (131072) /* 2^32 / 32768 */
+
+static cycle_t prcmu_read_timer_nop(struct clocksource *cs)
+{
+	return 0;
+}
+
+static struct clocksource prcmu_clksrc = {
+	.name		= "prcmu-timer",
+	.rating		= 300,
+	.read		= prcmu_read_timer_nop,
+	.shift		= 10,
+	.mask		= CLOCKSOURCE_MASK(32),
+	.flags		= CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+static cycle_t prcmu_read_timer(struct clocksource *cs)
+{
+	u32 count, count2;
+
+	do {
+		count = readl(timer_base + PRCMU_TIMER_DOWNCOUNT);
+		count2 = readl(timer_base + PRCMU_TIMER_DOWNCOUNT);
+	} while (count2 != count);
+
+	/*
+	 * clocksource: the prcmu timer is a decrementing counters, so we negate
+	 * the value being read.
+	 */
+	return ~count;
+}
+
+#ifdef CONFIG_UX500_PRCMU_TIMER
+unsigned long long notrace sched_clock(void)
+{
+	return clocksource_cyc2ns(prcmu_clksrc.read(
+			&prcmu_clksrc),
+			prcmu_clksrc.mult,
+			prcmu_clksrc.shift);
+}
+#endif
+
+#ifdef CONFIG_BOOTTIME
+
+static unsigned long __init boottime_get_time(void)
+{
+	return div_s64(clocksource_cyc2ns(prcmu_clksrc.read(
+			&prcmu_clksrc),
+			prcmu_clksrc.mult,
+			prcmu_clksrc.shift), 1000);
+}
+
+static struct boottime_timer __initdata boottime_timer = {
+	.init     = NULL,
+	.get_time = boottime_get_time,
+	.finalize = NULL,
+};
+#endif
+
+void __init prcmu_timer_init(void)
+{
+	void __iomem *prcmu_base;
+
+	if (ux500_is_svp())
+		return;
+
+	if (cpu_is_u8500()) {
+		prcmu_base = __io_address(U8500_PRCMU_BASE);
+		timer_base = prcmu_base + PRCMU_TIMER_4_BASE;
+	} else if (cpu_is_u5500()) {
+		prcmu_base = __io_address(U5500_PRCMU_BASE);
+		timer_base = prcmu_base + PRCMU_TIMER_3_BASE;
+	} else
+		ux500_unknown_soc();
+
+	clocksource_calc_mult_shift(&prcmu_clksrc,
+		RATE_32K, SCHED_CLOCK_MIN_WRAP);
+
+	/*
+	 * The A9 sub system expects the timer to be configured as
+	 * a continous looping timer.
+	 * The PRCMU should configure it but if it for some reason
+	 * don't we do it here.
+	 */
+	if (readl(timer_base + PRCMU_TIMER_MODE) != TIMER_MODE_CONTINOUS) {
+		writel(TIMER_MODE_CONTINOUS, timer_base + PRCMU_TIMER_MODE);
+		writel(TIMER_DOWNCOUNT_VAL, timer_base + PRCMU_TIMER_REF);
+	}
+	prcmu_clksrc.read = prcmu_read_timer;
+
+	clocksource_register(&prcmu_clksrc);
+
+	if (!ux500_is_svp())
+		boottime_activate(&boottime_timer);
+}
+
diff --git a/arch/arm/mach-ux500/timer.c b/arch/arm/mach-ux500/timer.c
new file mode 100644
index 00000000000..7eb6241a924
--- /dev/null
+++ b/arch/arm/mach-ux500/timer.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Mattias Wallin <mattias.wallin@stericsson.com> for ST-Ericsson
+ */
+#include <linux/io.h>
+#include <mach/setup.h>
+#include <mach/hardware.h>
+
+#include "pm/context.h"
+
+void rtc_rtt_timer_init(unsigned int cpu);
+void prcmu_timer_init(void);
+void mtu_timer_init(void);
+void mtu_clocksource_reset(void);
+void mtu_clockevent_reset(void);
+
+#ifdef CONFIG_LOCAL_TIMERS
+#include <asm/smp_twd.h>
+#endif
+
+#ifdef CONFIG_UX500_CONTEXT
+static int mtu_context_notifier_call(struct notifier_block *this,
+				     unsigned long event, void *data)
+{
+	if (event == CONTEXT_APE_RESTORE)
+		mtu_clocksource_reset();
+	return NOTIFY_OK;
+}
+
+static struct notifier_block mtu_context_notifier = {
+	.notifier_call = mtu_context_notifier_call,
+};
+#endif
+
+static void ux500_timer_reset(void)
+{
+	mtu_clockevent_reset();
+}
+
+static void __init ux500_timer_init(void)
+{
+
+#ifdef CONFIG_LOCAL_TIMERS
+	if (cpu_is_u5500())
+		twd_base = __io_address(U5500_TWD_BASE);
+	else if (cpu_is_u8500())
+		twd_base = __io_address(U8500_TWD_BASE);
+	else
+		ux500_unknown_soc();
+#endif
+/*
+ * Here we register the timerblocks active in the system.
+ * Localtimers (twd) is started when both cpu is up and running.
+ * MTU register a clocksource, clockevent and sched_clock.
+ * Since the MTU is located in the VAPE power domain
+ * it will be cleared in sleep which makes it unsuitable.
+ * We however need it as a timer tick (clockevent)
+ * during boot to calibrate delay until twd is started.
+ * RTC-RTT have problems as timer tick during boot since it is depending
+ * on delay which is not yet calibrated. RTC-RTT is in the always-on
+ * powerdomain and is used as clockevent instead of twd when sleeping.
+ * The PRCMU timer 4 register a clocksource and sched_clock with higher
+ * rating then MTU since is always-on.
+ *
+ */
+	mtu_timer_init();
+#ifdef CONFIG_UX500_PRCMU_TIMER
+	prcmu_timer_init();
+#endif
+
+#ifdef CONFIG_UX500_CONTEXT
+	WARN_ON(context_ape_notifier_register(&mtu_context_notifier));
+#endif
+}
+
+struct sys_timer ux500_timer = {
+	.init		= ux500_timer_init,
+	.resume		= ux500_timer_reset,
+};
diff --git a/arch/arm/mach-ux500/usb.c b/arch/arm/mach-ux500/usb.c
index 82e535953fd..dc6a6970c35 100644
--- a/arch/arm/mach-ux500/usb.c
+++ b/arch/arm/mach-ux500/usb.c
@@ -9,6 +9,9 @@
 #include <plat/ste_dma40.h>
 #include <mach/hardware.h>
 #include <mach/usb.h>
+#include <plat/pincfg.h>
+#include "pins.h"
+#include "board-mop500-usb.h"
 
 #define MUSB_DMA40_RX_CH { \
 		.mode = STEDMA40_MODE_LOGICAL, \
@@ -85,6 +88,7 @@ static struct ux500_musb_board_data musb_board_data = {
 };
 
 static u64 ux500_musb_dmamask = DMA_BIT_MASK(32);
+static struct ux500_pins *usb_gpio_pins;
 
 static struct musb_hdrc_config musb_hdrc_config = {
 	.multipoint	= true,
@@ -129,6 +133,37 @@ struct platform_device ux500_musb_device = {
 	.resource = usb_resources,
 };
 
+static void enable_gpio(void)
+{
+	ux500_pins_enable(usb_gpio_pins);
+}
+static void disable_gpio(void)
+{
+	ux500_pins_disable(usb_gpio_pins);
+}
+static int get_gpio(struct device *device)
+{
+	usb_gpio_pins = ux500_pins_get(dev_name(device));
+
+	if (usb_gpio_pins == NULL) {
+		dev_err(device, "Could not get %s:usb_gpio_pins structure\n",
+				dev_name(device));
+
+		return PTR_ERR(usb_gpio_pins);
+	}
+	return 0;
+}
+static void put_gpio(void)
+{
+	ux500_pins_put(usb_gpio_pins);
+}
+struct ab8500_usbgpio_platform_data ab8500_usbgpio_plat_data = {
+	.get		= &get_gpio,
+	.enable		= &enable_gpio,
+	.disable	= &disable_gpio,
+	.put		= &put_gpio,
+};
+
 static inline void ux500_usb_dma_update_rx_ch_config(int *src_dev_type)
 {
 	u32 idx;
diff --git a/arch/arm/mach-ux500/virt-regulator-u8500.c b/arch/arm/mach-ux500/virt-regulator-u8500.c
new file mode 100644
index 00000000000..4915a4cf636
--- /dev/null
+++ b/arch/arm/mach-ux500/virt-regulator-u8500.c
@@ -0,0 +1,333 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson
+ *
+ * Board specific file for configuration of virtual regulators. These virtual
+ * regulators are used for debug purposes. They connect to the regulator device
+ * just like any other consumer and expose controls in sysfs, so that
+ * regulators can be controlled from user space.
+ */
+
+#include <linux/init.h>
+#include <linux/platform_device.h>
+
+/*
+ * Configuration for AB8500 virtual regulators
+ */
+static struct platform_device u8500_aux1_virtual_regulator_device  = {
+	.name = "reg-virt-consumer",
+	.id = 0,
+	.dev = {
+		.platform_data = "aux1",
+	},
+};
+
+static struct platform_device u8500_aux2_virtual_regulator_device  = {
+	.name = "reg-virt-consumer",
+	.id = 1,
+	.dev = {
+		.platform_data = "aux2",
+	},
+};
+
+static struct platform_device u8500_aux3_virtual_regulator_device  = {
+	.name = "reg-virt-consumer",
+	.id = 2,
+	.dev = {
+		.platform_data = "aux3",
+	},
+};
+
+static struct platform_device u8500_intcore_virtual_regulator_device  = {
+	.name = "reg-virt-consumer",
+	.id = 3,
+	.dev = {
+		.platform_data = "intcore",
+	},
+};
+
+static struct platform_device u8500_tvout_virtual_regulator_device  = {
+	.name = "reg-virt-consumer",
+	.id = 4,
+	.dev = {
+		.platform_data = "tvout",
+	},
+};
+
+static struct platform_device u8500_audio_virtual_regulator_device  = {
+	.name = "reg-virt-consumer",
+	.id = 5,
+	.dev = {
+		.platform_data = "audio",
+	},
+};
+
+static struct platform_device u8500_anamic1_virtual_regulator_device  = {
+	.name = "reg-virt-consumer",
+	.id = 6,
+	.dev = {
+		.platform_data = "anamic1",
+	},
+};
+
+static struct platform_device u8500_anamic2_virtual_regulator_device  = {
+	.name = "reg-virt-consumer",
+	.id = 7,
+	.dev = {
+		.platform_data = "anamic2",
+	},
+};
+
+static struct platform_device u8500_dmic_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 8,
+	.dev = {
+		.platform_data = "dmic",
+	},
+};
+
+static struct platform_device u8500_ana_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 9,
+	.dev = {
+		.platform_data = "ana",
+	},
+};
+
+static struct platform_device u8500_sysclkreq_2_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 10,
+	.dev = {
+		.platform_data = "sysclkreq-2",
+	},
+};
+
+static struct platform_device u8500_sysclkreq_4_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 11,
+	.dev = {
+		.platform_data = "sysclkreq-4",
+	},
+};
+
+/*
+ * Configuration for other U8500 virtual regulators
+ */
+static struct platform_device u8500_ape_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 12,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_arm_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 13,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_modem_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 14,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_pll_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 15,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_smps1_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 16,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_smps2_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 17,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_smps3_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 18,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_rf1_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 19,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+/*
+ * Configuration for U8500 power domain virtual regulators
+ */
+static struct platform_device u8500_sva_mmdsp_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 20,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_sva_mmdsp_ret_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 21,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_sva_pipe_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 22,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_sia_mmdsp_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 23,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_sia_mmdsp_ret_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 24,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_sia_pipe_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 25,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_sga_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 26,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_b2r2_mcde_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 27,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_esram12_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 28,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_esram12_ret_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 29,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_esram34_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 30,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device u8500_esram34_ret_virtual_regulator_device = {
+	.name = "reg-virt-consumer",
+	.id = 31,
+	.dev = {
+		.platform_data = "test",
+	},
+};
+
+static struct platform_device *u8500_virtual_regulator_devices[] = {
+	&u8500_aux1_virtual_regulator_device,
+	&u8500_aux2_virtual_regulator_device,
+	&u8500_aux3_virtual_regulator_device,
+	&u8500_intcore_virtual_regulator_device,
+	&u8500_tvout_virtual_regulator_device,
+	&u8500_audio_virtual_regulator_device,
+	&u8500_anamic1_virtual_regulator_device,
+	&u8500_anamic2_virtual_regulator_device,
+	&u8500_dmic_virtual_regulator_device,
+	&u8500_ana_virtual_regulator_device,
+	&u8500_sysclkreq_2_virtual_regulator_device,
+	&u8500_sysclkreq_4_virtual_regulator_device,
+	&u8500_ape_virtual_regulator_device,
+	&u8500_arm_virtual_regulator_device,
+	&u8500_modem_virtual_regulator_device,
+	&u8500_pll_virtual_regulator_device,
+	&u8500_smps1_virtual_regulator_device,
+	&u8500_smps2_virtual_regulator_device,
+	&u8500_smps3_virtual_regulator_device,
+	&u8500_rf1_virtual_regulator_device,
+	&u8500_sva_mmdsp_virtual_regulator_device,
+	&u8500_sva_mmdsp_ret_virtual_regulator_device,
+	&u8500_sva_pipe_virtual_regulator_device,
+	&u8500_sia_mmdsp_virtual_regulator_device,
+	&u8500_sia_mmdsp_ret_virtual_regulator_device,
+	&u8500_sia_pipe_virtual_regulator_device,
+	&u8500_sga_virtual_regulator_device,
+	&u8500_b2r2_mcde_virtual_regulator_device,
+	&u8500_esram12_virtual_regulator_device,
+	&u8500_esram12_ret_virtual_regulator_device,
+	&u8500_esram34_virtual_regulator_device,
+	&u8500_esram34_ret_virtual_regulator_device,
+};
+
+static int __init u8500_virtual_regulator_init(void)
+{
+	int ret;
+
+	ret = platform_add_devices(u8500_virtual_regulator_devices,
+			ARRAY_SIZE(u8500_virtual_regulator_devices));
+	if (ret != 0)
+		pr_err("Failed to register U8500 virtual regulator devices:"
+			" %d\n", ret);
+
+	return ret;
+}
+module_init(u8500_virtual_regulator_init);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Bengt Jonsson <bengt.g.jonsson@stericsson.com");
+MODULE_DESCRIPTION("Configuration of u8500 virtual regulators");
+MODULE_ALIAS("platform:u8500-virtual-regulator");
diff --git a/arch/arm/mm/cache-fa.S b/arch/arm/mm/cache-fa.S
index 07201637109..226f8736152 100644
--- a/arch/arm/mm/cache-fa.S
+++ b/arch/arm/mm/cache-fa.S
@@ -240,6 +240,24 @@ ENTRY(fa_dma_unmap_area)
 	mov	pc, lr
 ENDPROC(fa_dma_unmap_area)
 
+/*
+ *	clean_dcache_all()
+ *
+ *	Clean the whole D-cache.
+ */
+ENTRY(fa_clean_dcache_all)
+	mov	pc, lr
+ENDPROC(fa_clean_dcache_all)
+
+/*
+ *	flush_dcache_all()
+ *
+ *	Flush the whole D-cache.
+ */
+ENTRY(fa_flush_dcache_all)
+	mov	pc, lr
+ENDPROC(fa_flush_dcache_all)
+
 	__INITDATA
 
 	@ define struct cpu_cache_fns (see <asm/cacheflush.h> and proc-macros.S)
diff --git a/arch/arm/mm/cache-v3.S b/arch/arm/mm/cache-v3.S
index c2301f22610..ab5bf508a2a 100644
--- a/arch/arm/mm/cache-v3.S
+++ b/arch/arm/mm/cache-v3.S
@@ -127,6 +127,24 @@ ENTRY(v3_dma_map_area)
 ENDPROC(v3_dma_unmap_area)
 ENDPROC(v3_dma_map_area)
 
+/*
+ *	clean_dcache_all()
+ *
+ *	Clean the whole D-cache.
+ */
+ENTRY(v3_clean_dcache_all)
+	mov	pc, lr
+ENDPROC(v3_clean_dcache_all)
+
+/*
+ *	flush_dcache_all()
+ *
+ *	Flush the whole D-cache.
+ */
+ENTRY(v3_flush_dcache_all)
+	mov	pc, lr
+ENDPROC(v3_flush_dcache_all)
+
 	__INITDATA
 
 	@ define struct cpu_cache_fns (see <asm/cacheflush.h> and proc-macros.S)
diff --git a/arch/arm/mm/cache-v4.S b/arch/arm/mm/cache-v4.S
index fd9bb7addc8..9d3a055127e 100644
--- a/arch/arm/mm/cache-v4.S
+++ b/arch/arm/mm/cache-v4.S
@@ -139,6 +139,24 @@ ENTRY(v4_dma_map_area)
 ENDPROC(v4_dma_unmap_area)
 ENDPROC(v4_dma_map_area)
 
+/*
+ *	clean_dcache_all()
+ *
+ *	Clean the whole D-cache.
+ */
+ENTRY(v4_clean_dcache_all)
+	mov	pc, lr
+ENDPROC(v4_clean_dcache_all)
+
+/*
+ *	flush_dcache_all()
+ *
+ *	Flush the whole D-cache.
+ */
+ENTRY(v4_flush_dcache_all)
+	mov	pc, lr
+ENDPROC(v4_flush_dcache_all)
+
 	__INITDATA
 
 	@ define struct cpu_cache_fns (see <asm/cacheflush.h> and proc-macros.S)
diff --git a/arch/arm/mm/cache-v4wb.S b/arch/arm/mm/cache-v4wb.S
index 4f2c14151cc..54d3cda4a89 100644
--- a/arch/arm/mm/cache-v4wb.S
+++ b/arch/arm/mm/cache-v4wb.S
@@ -251,6 +251,24 @@ ENTRY(v4wb_dma_unmap_area)
 	mov	pc, lr
 ENDPROC(v4wb_dma_unmap_area)
 
+/*
+ *	clean_dcache_all()
+ *
+ *	Clean the whole D-cache.
+ */
+ENTRY(v4wb_clean_dcache_all)
+	mov	pc, lr
+ENDPROC(v4wb_clean_dcache_all)
+
+/*
+ *	flush_dcache_all()
+ *
+ *	Flush the whole D-cache.
+ */
+ENTRY(v4wb_flush_dcache_all)
+	mov	pc, lr
+ENDPROC(v4wb_flush_dcache_all)
+
 	__INITDATA
 
 	@ define struct cpu_cache_fns (see <asm/cacheflush.h> and proc-macros.S)
diff --git a/arch/arm/mm/cache-v4wt.S b/arch/arm/mm/cache-v4wt.S
index 4d7b467631c..40f7dba11f5 100644
--- a/arch/arm/mm/cache-v4wt.S
+++ b/arch/arm/mm/cache-v4wt.S
@@ -195,6 +195,24 @@ ENTRY(v4wt_dma_map_area)
 ENDPROC(v4wt_dma_unmap_area)
 ENDPROC(v4wt_dma_map_area)
 
+/*
+ *	clean_dcache_all()
+ *
+ *	Clean the whole D-cache.
+ */
+ENTRY(v4wt_clean_dcache_all)
+	mov	pc, lr
+ENDPROC(v4wt_clean_dcache_all)
+
+/*
+ *	flush_dcache_all()
+ *
+ *	Flush the whole D-cache.
+ */
+ENTRY(v4wt_flush_dcache_all)
+	mov	pc, lr
+ENDPROC(v4wt_flush_dcache_all)
+
 	__INITDATA
 
 	@ define struct cpu_cache_fns (see <asm/cacheflush.h> and proc-macros.S)
diff --git a/arch/arm/mm/cache-v6.S b/arch/arm/mm/cache-v6.S
index 2edb6f67f69..7c278122887 100644
--- a/arch/arm/mm/cache-v6.S
+++ b/arch/arm/mm/cache-v6.S
@@ -345,6 +345,24 @@ ENTRY(v6_dma_unmap_area)
 	mov	pc, lr
 ENDPROC(v6_dma_unmap_area)
 
+/*
+ *	clean_dcache_all()
+ *
+ *	Clean the whole D-cache.
+ */
+ENTRY(v6_clean_dcache_all)
+	mov	pc, lr
+ENDPROC(v6_clean_dcache_all)
+
+/*
+ *	flush_dcache_all()
+ *
+ *	Flush the whole D-cache.
+ */
+ENTRY(v6_flush_dcache_all)
+	mov	pc, lr
+ENDPROC(v6_flush_dcache_all)
+
 	__INITDATA
 
 	@ define struct cpu_cache_fns (see <asm/cacheflush.h> and proc-macros.S)
diff --git a/arch/arm/mm/cache-v7.S b/arch/arm/mm/cache-v7.S
index 3b24bfa3b82..28af05c0bd2 100644
--- a/arch/arm/mm/cache-v7.S
+++ b/arch/arm/mm/cache-v7.S
@@ -33,7 +33,7 @@ ENTRY(v7_flush_icache_all)
 ENDPROC(v7_flush_icache_all)
 
 /*
- *	v7_flush_dcache_all()
+ *	__v7_flush_dcache_all()
  *
  *	Flush the whole D-cache.
  *
@@ -41,7 +41,7 @@ ENDPROC(v7_flush_icache_all)
  *
  *	- mm    - mm_struct describing address space
  */
-ENTRY(v7_flush_dcache_all)
+ENTRY(__v7_flush_dcache_all)
 	dmb					@ ensure ordering with previous memory accesses
 	mrc	p15, 1, r0, c0, c0, 1		@ read clidr
 	ands	r3, r0, #0x7000000		@ extract loc from clidr
@@ -88,9 +88,93 @@ finished:
 	dsb
 	isb
 	mov	pc, lr
+ENDPROC(__v7_flush_dcache_all)
+
+/*
+ *	__v7_clean_dcache_all()
+ *
+ *	Clean the whole D-cache.
+ *
+ *	Corrupted registers: r0-r7, r9-r11 (r6 only in Thumb mode)
+ */
+ENTRY(__v7_clean_dcache_all)
+	dmb					@ ensure ordering with previous memory accesses
+	mrc	p15, 1, r0, c0, c0, 1		@ read clidr
+	ands	r3, r0, #0x7000000		@ extract loc from clidr
+	mov	r3, r3, lsr #23			@ left align loc bit field
+	beq	finished1			@ if loc is 0, then no need to clean
+	mov	r10, #0				@ start clean at cache level 0
+loop21:
+	add	r2, r10, r10, lsr #1		@ work out 3x current cache level
+	mov	r1, r0, lsr r2			@ extract cache type bits from clidr
+	and	r1, r1, #7			@ mask of the bits for current cache only
+	cmp	r1, #2				@ see what cache we have at this level
+	blt	skip1				@ skip if no cache, or just i-cache
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	isb					@ isb to sych the new cssr&csidr
+	mrc	p15, 1, r1, c0, c0, 0		@ read the new csidr
+	and	r2, r1, #7			@ extract the length of the cache lines
+	add	r2, r2, #4			@ add 4 (line length offset)
+	ldr	r4, =0x3ff
+	ands	r4, r4, r1, lsr #3		@ find maximum number on the way size
+	clz	r5, r4				@ find bit position of way size increment
+	ldr	r7, =0x7fff
+	ands	r7, r7, r1, lsr #13		@ extract max number of the index size
+loop22:
+	mov	r9, r4				@ create working copy of max way size
+loop23:
+ ARM(	orr	r11, r10, r9, lsl r5	)	@ factor way and cache number into r11
+ THUMB(	lsl	r6, r9, r5		)
+ THUMB(	orr	r11, r10, r6		)	@ factor way and cache number into r11
+ ARM(	orr	r11, r11, r7, lsl r2	)	@ factor index number into r11
+ THUMB(	lsl	r6, r7, r2		)
+ THUMB(	orr	r11, r11, r6		)	@ factor index number into r11
+	mcr	p15, 0, r11, c7, c10, 2		@ clean by set/way
+	subs	r9, r9, #1			@ decrement the way
+	bge	loop23
+	subs	r7, r7, #1			@ decrement the index
+	bge	loop22
+skip1:
+	add	r10, r10, #2			@ increment cache number
+	cmp	r3, r10
+	bgt	loop21
+finished1:
+	mov	r10, #0				@ swith back to cache level 0
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	dsb
+	isb
+	mov	pc, lr
+ENDPROC(__v7_clean_dcache_all)
+
+/*
+ *	v7_flush_dcache_all()
+ *
+ *	Flush the whole D-cache.
+ */
+ENTRY(v7_flush_dcache_all)
+ ARM(	stmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
+ THUMB(	stmfd	sp!, {r4-r7, r9-r11, lr}	)
+	bl	__v7_flush_dcache_all
+ ARM(	ldmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
+ THUMB(	ldmfd	sp!, {r4-r7, r9-r11, lr}	)
+	mov	pc, lr
 ENDPROC(v7_flush_dcache_all)
 
 /*
+ *	v7_clean_dcache_all()
+ *
+ *	Clean the whole D-cache.
+ */
+ENTRY(v7_clean_dcache_all)
+ ARM(	stmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
+ THUMB(	stmfd	sp!, {r4-r7, r9-r11, lr}	)
+	bl	__v7_clean_dcache_all
+ ARM(	ldmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
+ THUMB(	ldmfd	sp!, {r4-r7, r9-r11, lr}	)
+	mov	pc, lr
+ENDPROC(v7_clean_dcache_all)
+
+/*
  *	v7_flush_cache_all()
  *
  *	Flush the entire cache system.
@@ -102,14 +186,12 @@ ENDPROC(v7_flush_dcache_all)
  *
  */
 ENTRY(v7_flush_kern_cache_all)
- ARM(	stmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
- THUMB(	stmfd	sp!, {r4-r7, r9-r11, lr}	)
+	stmfd	sp!, {lr}
 	bl	v7_flush_dcache_all
 	mov	r0, #0
 	ALT_SMP(mcr	p15, 0, r0, c7, c1, 0)	@ invalidate I-cache inner shareable
 	ALT_UP(mcr	p15, 0, r0, c7, c5, 0)	@ I+BTB cache invalidate
- ARM(	ldmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
- THUMB(	ldmfd	sp!, {r4-r7, r9-r11, lr}	)
+	ldmfd	sp!, {lr}
 	mov	pc, lr
 ENDPROC(v7_flush_kern_cache_all)
 
diff --git a/arch/arm/mm/mmu.c b/arch/arm/mm/mmu.c
index 594d677b92c..df6ced286ea 100644
--- a/arch/arm/mm/mmu.c
+++ b/arch/arm/mm/mmu.c
@@ -273,6 +273,20 @@ static struct mem_type mem_types[] = {
 		.prot_l1   = PMD_TYPE_TABLE,
 		.domain    = DOMAIN_KERNEL,
 	},
+	/* NOTE : this is only a temporary hack!!!
+	 *        The U8500 ED/V1.0 cuts require such a
+	 *        memory type for deep sleep resume.
+	 *        This is expected to be solved in cut v2.0
+	 *        and we clean this up then. for more details
+	 *        look @ the commit message please
+	 */
+	[MT_BACKUP_RAM] = {
+		.prot_pte       = PROT_PTE_DEVICE | L_PTE_MT_DEV_SHARED |
+			L_PTE_SHARED,
+		.prot_l1        = PMD_TYPE_TABLE,
+		.prot_sect      = PROT_SECT_DEVICE | PMD_SECT_S,
+		.domain         = DOMAIN_IO,
+	},
 };
 
 const struct mem_type *get_mem_type(unsigned int type)
diff --git a/arch/arm/mm/proc-macros.S b/arch/arm/mm/proc-macros.S
index 307a4def8d3..411610d4973 100644
--- a/arch/arm/mm/proc-macros.S
+++ b/arch/arm/mm/proc-macros.S
@@ -303,6 +303,8 @@ ENTRY(\name\()_cache_fns)
 	.long	\name\()_coherent_kern_range
 	.long	\name\()_coherent_user_range
 	.long	\name\()_flush_kern_dcache_area
+	.long	\name\()_clean_dcache_all
+	.long	\name\()_flush_dcache_all
 	.long	\name\()_dma_map_area
 	.long	\name\()_dma_unmap_area
 	.long	\name\()_dma_flush_range
diff --git a/arch/arm/mm/proc-v7.S b/arch/arm/mm/proc-v7.S
index a30e78542cc..b787720192c 100644
--- a/arch/arm/mm/proc-v7.S
+++ b/arch/arm/mm/proc-v7.S
@@ -300,7 +300,7 @@ __v7_ca15mp_setup:
 __v7_setup:
 	adr	r12, __v7_setup_stack		@ the local stack
 	stmia	r12, {r0-r5, r7, r9, r11, lr}
-	bl	v7_flush_dcache_all
+	bl	__v7_flush_dcache_all
 	ldmia	r12, {r0-r5, r7, r9, r11, lr}
 
 	mrc	p15, 0, r0, c0, c0, 0		@ read main ID register
diff --git a/arch/arm/plat-nomadik/include/plat/pincfg.h b/arch/arm/plat-nomadik/include/plat/pincfg.h
index 05a3936ae6d..b054a48d528 100644
--- a/arch/arm/plat-nomadik/include/plat/pincfg.h
+++ b/arch/arm/plat-nomadik/include/plat/pincfg.h
@@ -24,6 +24,7 @@
  *	bit 16..18 - SLPM pull up/down state
  *	bit 19..20 - SLPM direction
  *	bit 21..22 - SLPM Value (if output)
+ *	bit 23..25 - PDIS value (if input)
  *
  * to facilitate the definition, the following macros are provided
  *
@@ -37,7 +38,6 @@
  *		     SLPM value = same as normal
  *
  * PIN_CFG	   - default config with alternate function
- * PIN_CFG_PULL	   - default config with alternate function and pull up/down
  */
 
 typedef unsigned long pin_cfg_t;
@@ -68,6 +68,7 @@ typedef unsigned long pin_cfg_t;
 /* These two replace the above in DB8500v2+ */
 #define PIN_SLPM_WAKEUP_ENABLE	(NMK_GPIO_SLPM_WAKEUP_ENABLE << PIN_SLPM_SHIFT)
 #define PIN_SLPM_WAKEUP_DISABLE	(NMK_GPIO_SLPM_WAKEUP_DISABLE << PIN_SLPM_SHIFT)
+#define PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP PIN_SLPM_WAKEUP_DISABLE
 
 #define PIN_DIR_SHIFT		14
 #define PIN_DIR_MASK		(0x1 << PIN_DIR_SHIFT)
@@ -106,6 +107,14 @@ typedef unsigned long pin_cfg_t;
 #define PIN_SLPM_VAL_LOW	((1 + 0) << PIN_SLPM_VAL_SHIFT)
 #define PIN_SLPM_VAL_HIGH	((1 + 1) << PIN_SLPM_VAL_SHIFT)
 
+#define PIN_SLPM_PDIS_SHIFT		23
+#define PIN_SLPM_PDIS_MASK		(0x3 << PIN_SLPM_PDIS_SHIFT)
+#define PIN_SLPM_PDIS(x)	\
+	(((x) & PIN_SLPM_PDIS_MASK) >> PIN_SLPM_PDIS_SHIFT)
+#define PIN_SLPM_PDIS_NO_CHANGE		(0 << PIN_SLPM_PDIS_SHIFT)
+#define PIN_SLPM_PDIS_DISABLED		(1 << PIN_SLPM_PDIS_SHIFT)
+#define PIN_SLPM_PDIS_ENABLED		(2 << PIN_SLPM_PDIS_SHIFT)
+
 /* Shortcuts.  Use these instead of separate DIR, PULL, and VAL.  */
 #define PIN_INPUT_PULLDOWN	(PIN_DIR_INPUT | PIN_PULL_DOWN)
 #define PIN_INPUT_PULLUP	(PIN_DIR_INPUT | PIN_PULL_UP)
@@ -133,10 +142,6 @@ typedef unsigned long pin_cfg_t;
 	(PIN_CFG_DEFAULT |\
 	 (PIN_NUM(num) | PIN_##alt | PIN_OUTPUT_##val))
 
-#define PIN_CFG_PULL(num, alt, pull)	\
-	((PIN_CFG_DEFAULT & ~PIN_PULL_MASK) |\
-	 (PIN_NUM(num) | PIN_##alt | PIN_PULL_##pull))
-
 extern int nmk_config_pin(pin_cfg_t cfg, bool sleep);
 extern int nmk_config_pins(pin_cfg_t *cfgs, int num);
 extern int nmk_config_pins_sleep(pin_cfg_t *cfgs, int num);
diff --git a/arch/arm/plat-nomadik/include/plat/ske.h b/arch/arm/plat-nomadik/include/plat/ske.h
index 31382fbc07d..7a4fbdf3c13 100644
--- a/arch/arm/plat-nomadik/include/plat/ske.h
+++ b/arch/arm/plat-nomadik/include/plat/ske.h
@@ -22,6 +22,9 @@
 #define SKE_MIS		0x18
 #define SKE_ICR		0x1C
 
+#define SKE_KPD_MAX_ROWS        8
+#define SKE_KPD_MAX_COLS        8
+
 /*
  * Keypad module
  */
@@ -30,21 +33,27 @@
  * struct keypad_platform_data - structure for platform specific data
  * @init:	pointer to keypad init function
  * @exit:	pointer to keypad deinitialisation function
+ * @gpio_input_pins:	pointer to gpio input pins
+ * @gpio_output_pins:	pointer to gpio output pins
  * @keymap_data: matrix scan code table for keycodes
  * @krow:	maximum number of rows
  * @kcol:	maximum number of columns
  * @debounce_ms: platform specific debounce time
  * @no_autorepeat: flag for auto repetition
  * @wakeup_enable: allow waking up the system
+ * @switch_delay: gpio switch_delay
  */
 struct ske_keypad_platform_data {
 	int (*init)(void);
 	int (*exit)(void);
+	int *gpio_input_pins;
+	int *gpio_output_pins;
 	const struct matrix_keymap_data *keymap_data;
 	u8 krow;
 	u8 kcol;
 	u8 debounce_ms;
 	bool no_autorepeat;
 	bool wakeup_enable;
+	int switch_delay;
 };
 #endif	/*__SKE_KPD_H*/
diff --git a/arch/arm/plat-omap/include/plat/ssi.h b/arch/arm/plat-omap/include/plat/ssi.h
new file mode 100644
index 00000000000..eb84c3a69f7
--- /dev/null
+++ b/arch/arm/plat-omap/include/plat/ssi.h
@@ -0,0 +1,204 @@
+/*
+ * plat/ssi.h
+ *
+ * Hardware definitions for SSI.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#ifndef __OMAP_SSI_REGS_H__
+#define __OMAP_SSI_REGS_H__
+
+#define SSI_NUM_PORTS	1
+/*
+ * SSI SYS registers
+ */
+#define SSI_REVISION_REG		0
+#	define SSI_REV_MAJOR		0xf0
+#	define SSI_REV_MINOR		0xf
+#define SSI_SYSCONFIG_REG		0x10
+#	define SSI_AUTOIDLE		(1 << 0)
+#	define SSI_SOFTRESET		(1 << 1)
+#	define SSI_SIDLEMODE_FORCE	0
+#	define SSI_SIDLEMODE_NO		(1 << 3)
+#	define SSI_SIDLEMODE_SMART	(1 << 4)
+#	define SSI_SIDLEMODE_MASK	0x18
+#	define SSI_MIDLEMODE_FORCE	0
+#	define SSI_MIDLEMODE_NO		(1 << 12)
+#	define SSI_MIDLEMODE_SMART	(1 << 13)
+#	define SSI_MIDLEMODE_MASK	0x3000
+#define SSI_SYSSTATUS_REG		0x14
+#	define SSI_RESETDONE		1
+#define SSI_MPU_STATUS_REG(port, irq)	(0x808 + ((port) * 0x10) + ((irq) * 2))
+#define SSI_MPU_ENABLE_REG(port, irq)	(0x80c + ((port) * 0x10) + ((irq) * 8))
+#	define SSI_DATAACCEPT(channel)		(1 << (channel))
+#	define SSI_DATAAVAILABLE(channel)	(1 << ((channel) + 8))
+#	define SSI_DATAOVERRUN(channel)		(1 << ((channel) + 16))
+#	define SSI_ERROROCCURED			(1 << 24)
+#	define SSI_BREAKDETECTED		(1 << 25)
+#define SSI_GDD_MPU_IRQ_STATUS_REG	0x0800
+#define SSI_GDD_MPU_IRQ_ENABLE_REG	0x0804
+#	define SSI_GDD_LCH(channel)	(1 << (channel))
+#define SSI_WAKE_REG(port)		(0xc00 + ((port) * 0x10))
+#define SSI_CLEAR_WAKE_REG(port)	(0xc04 + ((port) * 0x10))
+#define SSI_SET_WAKE_REG(port)		(0xc08 + ((port) * 0x10))
+#	define SSI_WAKE(channel)	(1 << (channel))
+#	define SSI_WAKE_MASK		0xff
+
+/*
+ * SSI SST registers
+ */
+#define SSI_SST_ID_REG			0
+#define SSI_SST_MODE_REG		4
+#	define SSI_MODE_VAL_MASK	3
+#	define SSI_MODE_SLEEP		0
+#	define SSI_MODE_STREAM		1
+#	define SSI_MODE_FRAME		2
+#	define SSI_MODE_MULTIPOINTS	3
+#define SSI_SST_FRAMESIZE_REG		8
+#	define SSI_FRAMESIZE_DEFAULT	31
+#define SSI_SST_TXSTATE_REG		0xc
+#	define	SSI_TXSTATE_IDLE	0
+#define SSI_SST_BUFSTATE_REG		0x10
+#	define	SSI_FULL(channel)	(1 << (channel))
+#define SSI_SST_DIVISOR_REG		0x18
+#	define SSI_MAX_DIVISOR		127
+#define SSI_SST_BREAK_REG		0x20
+#define SSI_SST_CHANNELS_REG		0x24
+#	define SSI_CHANNELS_DEFAULT	4
+#define SSI_SST_ARBMODE_REG		0x28
+#	define SSI_ARBMODE_ROUNDROBIN	0
+#	define SSI_ARBMODE_PRIORITY	1
+#define SSI_SST_BUFFER_CH_REG(channel)	(0x80 + ((channel) * 4))
+#define SSI_SST_SWAPBUF_CH_REG(channel)	(0xc0 + ((channel) * 4))
+
+/*
+ * SSI SSR registers
+ */
+#define SSI_SSR_ID_REG			0
+#define SSI_SSR_MODE_REG		4
+#define SSI_SSR_FRAMESIZE_REG		8
+#define SSI_SSR_RXSTATE_REG		0xc
+#define SSI_SSR_BUFSTATE_REG		0x10
+#	define SSI_NOTEMPTY(channel)	(1 << (channel))
+#define SSI_SSR_BREAK_REG		0x1c
+#define SSI_SSR_ERROR_REG		0x20
+#define SSI_SSR_ERRORACK_REG		0x24
+#define SSI_SSR_OVERRUN_REG		0x2c
+#define SSI_SSR_OVERRUNACK_REG		0x30
+#define SSI_SSR_TIMEOUT_REG		0x34
+#	define SSI_TIMEOUT_DEFAULT	0
+#define SSI_SSR_CHANNELS_REG		0x28
+#define SSI_SSR_BUFFER_CH_REG(channel)	(0x80 + ((channel) * 4))
+#define SSI_SSR_SWAPBUF_CH_REG(channel)	(0xc0 + ((channel) * 4))
+
+/*
+ * SSI GDD registers
+ */
+#define SSI_GDD_HW_ID_REG		0
+#define SSI_GDD_PPORT_ID_REG		0x10
+#define SSI_GDD_MPORT_ID_REG		0x14
+#define SSI_GDD_PPORT_SR_REG		0x20
+#define SSI_GDD_MPORT_SR_REG		0x24
+#	define SSI_ACTIVE_LCH_NUM_MASK	0xff
+#define SSI_GDD_TEST_REG		0x40
+#	define SSI_TEST			1
+#define SSI_GDD_GCR_REG			0x100
+#	define	SSI_CLK_AUTOGATING_ON	(1 << 3)
+#	define	SSI_FREE		(1 << 2)
+#	define	SSI_SWITCH_OFF		(1 << 0)
+#define SSI_GDD_GRST_REG		0x200
+#	define SSI_SWRESET		1
+#define SSI_GDD_CSDP_REG(channel)	(0x800 + ((channel) * 0x40))
+#	define SSI_DST_BURST_EN_MASK	0xc000
+#	define SSI_DST_SINGLE_ACCESS0	0
+#	define SSI_DST_SINGLE_ACCESS	(1 << 14)
+#	define SSI_DST_BURST_4x32_BIT	(2 << 14)
+#	define SSI_DST_BURST_8x32_BIT	(3 << 14)
+#	define SSI_DST_MASK		0x1e00
+#	define SSI_DST_MEMORY_PORT	(8 << 9)
+#	define SSI_DST_PERIPHERAL_PORT	(9 << 9)
+#	define SSI_SRC_BURST_EN_MASK	0x180
+#	define SSI_SRC_SINGLE_ACCESS0	0
+#	define SSI_SRC_SINGLE_ACCESS	(1 << 7)
+#	define SSI_SRC_BURST_4x32_BIT	(2 << 7)
+#	define SSI_SRC_BURST_8x32_BIT	(3 << 7)
+#	define SSI_SRC_MASK		0x3c
+#	define SSI_SRC_MEMORY_PORT	(8 << 2)
+#	define SSI_SRC_PERIPHERAL_PORT	(9 << 2)
+#	define SSI_DATA_TYPE_MASK	3
+#	define SSI_DATA_TYPE_S32	2
+#define SSI_GDD_CCR_REG(channel)	(0x802 + ((channel) * 0x40))
+#	define SSI_DST_AMODE_MASK	(3 << 14)
+#	define SSI_DST_AMODE_CONST	0
+#	define SSI_DST_AMODE_POSTINC	(1 << 12)
+#	define SSI_SRC_AMODE_MASK	(3 << 12)
+#	define SSI_SRC_AMODE_CONST	0
+#	define SSI_SRC_AMODE_POSTINC	(1 << 12)
+#	define SSI_CCR_ENABLE		(1 << 7)
+#	define SSI_CCR_SYNC_MASK	0x1f
+#define SSI_GDD_CICR_REG(channel)	(0x804 + ((channel) * 0x40))
+#	define SSI_BLOCK_IE		(1 << 5)
+#	define SSI_HALF_IE		(1 << 2)
+#	define SSI_TOUT_IE		(1 << 0)
+#define SSI_GDD_CSR_REG(channel)	(0x806 + ((channel) * 0x40))
+#	define SSI_CSR_SYNC		(1 << 6)
+#	define SSI_CSR_BLOCK		(1 << 5)
+#	define SSI_CSR_HALF		(1 << 2)
+#	define SSI_CSR_TOUR		(1 << 0)
+#define SSI_GDD_CSSA_REG(channel)	(0x808 + ((channel) * 0x40))
+#define SSI_GDD_CDSA_REG(channel)	(0x80c + ((channel) * 0x40))
+#define SSI_GDD_CEN_REG(channel)	(0x810 + ((channel) * 0x40))
+#define SSI_GDD_CSAC_REG(channel)	(0x818 + ((channel) * 0x40))
+#define SSI_GDD_CDAC_REG(channel)	(0x81a + ((channel) * 0x40))
+#define SSI_GDD_CLNK_CTRL_REG(channel)	(0x828 + ((channel) * 0x40))
+#	define SSI_ENABLE_LNK		(1 << 15)
+#	define SSI_STOP_LNK		(1 << 14)
+#	define SSI_NEXT_CH_ID_MASK	0xf
+
+/**
+ * struct omap_ssi_platform_data - OMAP SSI platform data
+ * @num_ports: Number of ports on the controller
+ * @ctxt_loss_count: Pointer to omap_pm_get_dev_context_loss_count
+ */
+struct omap_ssi_platform_data {
+	unsigned int	num_ports;
+	int (*get_dev_context_loss_count)(struct device *dev);
+};
+
+/**
+ * struct omap_ssi_config - SSI board configuration
+ * @num_ports: Number of ports in use
+ * @cawake_line: Array of cawake gpio lines
+ */
+struct omap_ssi_board_config {
+	unsigned int num_ports;
+	int cawake_gpio[SSI_NUM_PORTS];
+};
+
+#ifdef CONFIG_OMAP_SSI_CONFIG
+extern int omap_ssi_config(struct omap_ssi_board_config *ssi_config);
+#else
+static inline int omap_ssi_config(struct omap_ssi_board_config *ssi_config)
+{
+	return 0;
+}
+#endif /* CONFIG_OMAP_SSI_CONFIG */
+
+#endif /* __OMAP_SSI_REGS_H__ */
diff --git a/arch/x86/kernel/cpu/amd.c b/arch/x86/kernel/cpu/amd.c
index b13ed393dfc..ea00b48274e 100644
--- a/arch/x86/kernel/cpu/amd.c
+++ b/arch/x86/kernel/cpu/amd.c
@@ -704,7 +704,7 @@ cpu_dev_register(amd_cpu_dev);
  */
 
 const int amd_erratum_400[] =
-	AMD_OSVW_ERRATUM(1, AMD_MODEL_RANGE(0xf, 0x41, 0x2, 0xff, 0xf),
+	AMD_OSVW_ERRATUM(1, AMD_MODEL_RANGE(0x0f, 0x4, 0x2, 0xff, 0xf),
 			    AMD_MODEL_RANGE(0x10, 0x2, 0x1, 0xff, 0xf));
 EXPORT_SYMBOL_GPL(amd_erratum_400);
 
diff --git a/drivers/Kconfig b/drivers/Kconfig
index d0258eb26d8..a4bcf85c048 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -52,6 +52,8 @@ source "drivers/i2c/Kconfig"
 
 source "drivers/spi/Kconfig"
 
+source "drivers/hsi/Kconfig"
+
 source "drivers/pps/Kconfig"
 
 source "drivers/ptp/Kconfig"
@@ -124,8 +126,12 @@ source "drivers/platform/Kconfig"
 
 source "drivers/clk/Kconfig"
 
+source "drivers/tee/Kconfig"
+
 source "drivers/hwspinlock/Kconfig"
 
 source "drivers/clocksource/Kconfig"
 
+source "drivers/modem/Kconfig"
 endmenu
+
diff --git a/drivers/Makefile b/drivers/Makefile
index 4ea4ac9e57a..7901dfa45fc 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -51,6 +51,7 @@ obj-$(CONFIG_ATA)		+= ata/
 obj-$(CONFIG_TARGET_CORE)	+= target/
 obj-$(CONFIG_MTD)		+= mtd/
 obj-$(CONFIG_SPI)		+= spi/
+obj-y				+= hsi/
 obj-y				+= net/
 obj-$(CONFIG_ATM)		+= atm/
 obj-$(CONFIG_FUSION)		+= message/
@@ -121,5 +122,7 @@ obj-y				+= platform/
 obj-y				+= ieee802154/
 #common clk code
 obj-y				+= clk/
+obj-$(CONFIG_TEE_SUPPORT)	+= tee/
+obj-$(CONFIG_MODEM)		+= modem/
 
 obj-$(CONFIG_HWSPINLOCK)	+= hwspinlock/
diff --git a/drivers/base/Kconfig b/drivers/base/Kconfig
index d57e8d0fb82..372ef3afe91 100644
--- a/drivers/base/Kconfig
+++ b/drivers/base/Kconfig
@@ -168,4 +168,7 @@ config SYS_HYPERVISOR
 	bool
 	default n
 
+config SYS_SOC
+	bool
+
 endmenu
diff --git a/drivers/base/Makefile b/drivers/base/Makefile
index 4c5701c15f5..a0d246dcce1 100644
--- a/drivers/base/Makefile
+++ b/drivers/base/Makefile
@@ -18,6 +18,7 @@ ifeq ($(CONFIG_SYSFS),y)
 obj-$(CONFIG_MODULES)	+= module.o
 endif
 obj-$(CONFIG_SYS_HYPERVISOR) += hypervisor.o
+obj-$(CONFIG_SYS_SOC) += soc.o
 
 ccflags-$(CONFIG_DEBUG_DRIVER) := -DDEBUG
 
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 4282d442d35..f88c4295be6 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -927,6 +927,13 @@ static int __device_suspend(struct device *dev, pm_message_t state, bool async)
 			pm_dev_dbg(dev, state, "legacy ");
 			error = legacy_suspend(dev, state, dev->bus->suspend);
 		}
+		if (error)
+			goto End;
+	}
+
+	if (dev->pwr_domain) {
+		pm_dev_dbg(dev, state, "power domain ");
+		pm_op(dev, &dev->pwr_domain->ops, state);
 	}
 
  End:
diff --git a/drivers/base/power/runtime.c b/drivers/base/power/runtime.c
index 0d4587b15c5..69d6f1d4805 100644
--- a/drivers/base/power/runtime.c
+++ b/drivers/base/power/runtime.c
@@ -168,6 +168,7 @@ static int rpm_check_suspend_allowed(struct device *dev)
 static int rpm_idle(struct device *dev, int rpmflags)
 {
 	int (*callback)(struct device *);
+	int (*domain_callback)(struct device *);
 	int retval;
 
 	retval = rpm_check_suspend_allowed(dev);
@@ -224,10 +225,19 @@ static int rpm_idle(struct device *dev, int rpmflags)
 	else
 		callback = NULL;
 
-	if (callback) {
+	if (dev->pwr_domain)
+		domain_callback = dev->pwr_domain->ops.runtime_idle;
+	else
+		domain_callback = NULL;
+
+	if (callback || domain_callback) {
 		spin_unlock_irq(&dev->power.lock);
 
-		callback(dev);
+		if (domain_callback)
+			retval = domain_callback(dev);
+
+		if (!retval && callback)
+			callback(dev);
 
 		spin_lock_irq(&dev->power.lock);
 	}
@@ -394,6 +404,8 @@ static int rpm_suspend(struct device *dev, int rpmflags)
 		else
 			pm_runtime_cancel_pending(dev);
 	} else {
+		if (dev->pwr_domain)
+			rpm_callback(dev->pwr_domain->ops.runtime_suspend, dev);
  no_callback:
 		__update_runtime_status(dev, RPM_SUSPENDED);
 		pm_runtime_deactivate_timer(dev);
diff --git a/drivers/base/soc.c b/drivers/base/soc.c
new file mode 100644
index 00000000000..046b43bfcdb
--- /dev/null
+++ b/drivers/base/soc.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Maxime Coquelin <maxime.coquelin-nonst@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/sysfs.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/slab.h>
+#include <linux/sys_soc.h>
+
+struct kobject *soc_object;
+
+ssize_t show_soc_info(struct kobject *kobj,
+			struct kobj_attribute *attr, char *buf)
+{
+	struct sysfs_soc_info *si = container_of(attr,
+				struct sysfs_soc_info, attr);
+
+	if (si->info)
+		return sprintf(buf, "%s\n", si->info);
+
+	return si->get_info(buf, si);
+}
+
+int __init register_sysfs_soc_info(struct sysfs_soc_info *info, int nb_info)
+{
+	int i, ret;
+
+	for (i = 0; i < nb_info; i++) {
+		ret = sysfs_create_file(soc_object, &info[i].attr.attr);
+		if (ret) {
+			for (i -= 1; i >= 0; i--)
+				sysfs_remove_file(soc_object, &info[i].attr.attr);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static struct attribute *soc_attrs[] = {
+	NULL,
+};
+
+static struct attribute_group soc_attr_group = {
+	.attrs = soc_attrs,
+};
+
+int __init register_sysfs_soc(struct sysfs_soc_info *info, size_t num)
+{
+	int ret;
+
+	soc_object = kobject_create_and_add("socinfo", NULL);
+	if (!soc_object) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	ret = sysfs_create_group(soc_object, &soc_attr_group);
+	if (ret)
+		goto kset_exit;
+
+	ret = register_sysfs_soc_info(info, num);
+	if (ret)
+		goto group_exit;
+
+	return 0;
+
+group_exit:
+	sysfs_remove_group(soc_object, &soc_attr_group);
+kset_exit:
+	kobject_put(soc_object);
+exit:
+	return ret;
+}
+
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index 3f63254bbbd..39aeb70b4b4 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -50,6 +50,10 @@ obj-$(CONFIG_NSC_GPIO)		+= nsc_gpio.o
 obj-$(CONFIG_GPIO_TB0219)	+= tb0219.o
 obj-$(CONFIG_TELCLOCK)		+= tlclk.o
 
+ifdef CONFIG_PHONET
+obj-$(CONFIG_U8500_SHRM)	+= shrm_char.o
+endif
+
 obj-$(CONFIG_MWAVE)		+= mwave/
 obj-$(CONFIG_AGP)		+= agp/
 obj-$(CONFIG_PCMCIA)		+= pcmcia/
diff --git a/drivers/char/shrm_char.c b/drivers/char/shrm_char.c
new file mode 100644
index 00000000000..fa44b469b98
--- /dev/null
+++ b/drivers/char/shrm_char.c
@@ -0,0 +1,882 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghavi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/sched.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_config.h>
+#include <linux/modem/shrm/shrm.h>
+#include <asm/atomic.h>
+
+#include <mach/isa_ioctl.h>
+
+
+#define NAME "IPC_ISA"
+/* L2 header for ciq device is 0xc3 and hence 0xc3+1 = 196*/
+#define MAX_L2_HEADERS 196
+
+#define SIZE_OF_FIFO (512*1024)
+
+static u8 message_fifo[ISA_DEVICES][SIZE_OF_FIFO];
+
+static u8 wr_rpc_msg[10*1024];
+static u8 wr_sec_msg[10*1024];
+static u8 wr_audio_msg[10*1024];
+
+struct map_device {
+	u8 l2_header;
+	u8 idx;
+	char *name;
+};
+
+static struct map_device map_dev[] = {
+	{ISI_MESSAGING, 0, "isi"},
+	{RPC_MESSAGING, 1, "rpc"},
+	{AUDIO_MESSAGING, 2, "modemaudio"},
+	{SECURITY_MESSAGING, 3, "sec"},
+	{COMMON_LOOPBACK_MESSAGING, 4, "common_loopback"},
+	{AUDIO_LOOPBACK_MESSAGING, 5, "audio_loopback"},
+	{CIQ_MESSAGING, 6, "ciq"},
+};
+
+/*
+ * int major:This variable is exported to user as module_param to specify
+ * major number at load time
+ */
+static int major;
+module_param(major, int, 0);
+MODULE_PARM_DESC(major, "Major device number");
+/* global fops mutex */
+static DEFINE_MUTEX(isa_lock);
+
+/**
+ * shrm_get_cdev_index() - return the index mapped to l2 header
+ * @l2_header:	L2 header
+ *
+ * struct map_device maps the index(count) with the device L2 header.
+ * This function returns the index for the provided L2 header in case
+ * of success else -ve value.
+ */
+int shrm_get_cdev_index(u8 l2_header)
+{
+	u8 cnt;
+	for (cnt = 0; cnt < ISA_DEVICES; cnt++) {
+		if (map_dev[cnt].l2_header == l2_header)
+			return map_dev[cnt].idx;
+	}
+	return -EINVAL;
+}
+
+/**
+ * shrm_get_cdev_l2header() - return l2_header mapped to the index
+ * @idx:	index
+ *
+ * struct map_device maps the index(count) with the device L2 header.
+ * This function returns the L2 header for the given index in case
+ * of success else -ve value.
+ */
+int shrm_get_cdev_l2header(u8 idx)
+{
+	u8 cnt;
+	for (cnt = 0; cnt < ISA_DEVICES; cnt++) {
+		if (map_dev[cnt].idx == idx)
+			return map_dev[cnt].l2_header;
+	}
+	return -EINVAL;
+}
+
+void shrm_char_reset_queues(struct shrm_dev *shrm)
+{
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context;
+	struct queue_element *cur_msg = NULL;
+	struct list_head *cur_msg_ptr = NULL;
+	struct list_head *msg_ptr;
+	struct message_queue *q;
+	int no_dev;
+
+	dev_info(shrm->dev, "%s: Resetting char device queues\n", __func__);
+	isa_context = shrm->isa_context;
+	for (no_dev = 0 ; no_dev < ISA_DEVICES ; no_dev++) {
+		isadev = &isa_context->isadev[no_dev];
+		q = &isadev->dl_queue;
+
+		spin_lock_bh(&q->update_lock);
+		/* empty out the msg queue */
+		list_for_each_safe(cur_msg_ptr, msg_ptr, &q->msg_list) {
+			cur_msg = list_entry(cur_msg_ptr,
+					struct queue_element, entry);
+			list_del(cur_msg_ptr);
+			kfree(cur_msg);
+		}
+
+		/* reset the msg queue pointers */
+		q->size = SIZE_OF_FIFO;
+		q->readptr = 0;
+		q->writeptr = 0;
+		q->no = 0;
+
+		/* wake up the blocking read/select */
+		atomic_set(&q->q_rp, 1);
+		wake_up_interruptible(&q->wq_readable);
+
+		spin_unlock_bh(&q->update_lock);
+	}
+}
+
+/**
+ * create_queue() - To create FIFO for Tx and Rx message buffering.
+ * @q:		message queue.
+ * @devicetype:	device type 0-isi,1-rpc,2-audio,3-security,
+ * 4-common_loopback, 5-audio_loopback.
+ * @shrm:	pointer to the shrm device information structure
+ *
+ * This function creates a FIFO buffer of n_bytes size using
+ * dma_alloc_coherent(). It also initializes all queue handling
+ * locks, queue management pointers. It also initializes message list
+ * which occupies this queue.
+ */
+static int create_queue(struct message_queue *q, u32 devicetype,
+						struct shrm_dev *shrm)
+{
+	q->fifo_base = (u8 *)&message_fifo[devicetype];
+	q->size = SIZE_OF_FIFO;
+	q->readptr = 0;
+	q->writeptr = 0;
+	q->no = 0;
+	q->shrm = shrm;
+	spin_lock_init(&q->update_lock);
+	INIT_LIST_HEAD(&q->msg_list);
+	init_waitqueue_head(&q->wq_readable);
+	atomic_set(&q->q_rp, 0);
+
+	return 0;
+}
+
+static void delete_queue(struct message_queue *q)
+{
+	q->size = 0;
+	q->readptr = 0;
+	q->writeptr = 0;
+}
+
+/**
+ * add_msg_to_queue() - Add a message inside queue
+ * @q:		message queue
+ * @size:	size in bytes
+ *
+ * This function tries to allocate n_bytes of size in FIFO q.
+ * It returns negative number when no memory can be allocated
+ * currently.
+ */
+int add_msg_to_queue(struct message_queue *q, u32 size)
+{
+	struct queue_element *new_msg = NULL;
+	struct shrm_dev *shrm = q->shrm;
+
+	dev_dbg(shrm->dev, "%s IN q->writeptr=%d\n", __func__, q->writeptr);
+	new_msg = kmalloc(sizeof(struct queue_element), GFP_ATOMIC);
+	if (new_msg == NULL) {
+		dev_err(shrm->dev, "unable to allocate memory\n");
+		return -ENOMEM;
+	}
+	new_msg->offset = q->writeptr;
+	new_msg->size = size;
+	new_msg->no = q->no++;
+
+	/* check for overflow condition */
+	if (q->readptr <= q->writeptr) {
+		if (((q->writeptr-q->readptr) + size) >= q->size) {
+			dev_err(shrm->dev, "Buffer overflow !!\n");
+			BUG_ON(((q->writeptr-q->readptr) + size) >= q->size);
+		}
+	} else {
+		if ((q->writeptr + size) >= q->readptr) {
+			dev_err(shrm->dev, "Buffer overflow !!\n");
+			BUG_ON((q->writeptr + size) >= q->readptr);
+		}
+	}
+	q->writeptr = (q->writeptr + size) % q->size;
+	if (list_empty(&q->msg_list)) {
+		list_add_tail(&new_msg->entry, &q->msg_list);
+		/* There can be 2 blocking calls read  and another select */
+		atomic_set(&q->q_rp, 1);
+		wake_up_interruptible(&q->wq_readable);
+	} else
+		list_add_tail(&new_msg->entry, &q->msg_list);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return 0;
+}
+
+/**
+ * remove_msg_from_queue() - To remove a message from the msg queue.
+ * @q:	message queue
+ *
+ * This function delets a message from the message list associated with message
+ * queue q and also updates read ptr.
+ * If the message list is empty, then, event is set to block the select and
+ * read calls of the paricular queue.
+ *
+ * The message list is FIFO style and message is always added to tail and
+ * removed from head.
+ */
+int remove_msg_from_queue(struct message_queue *q)
+{
+	struct queue_element *old_msg = NULL;
+	struct shrm_dev *shrm = q->shrm;
+	struct list_head *msg_ptr = NULL;
+	struct list_head *old_msg_ptr = NULL;
+
+	dev_dbg(shrm->dev, "%s IN q->readptr %d\n", __func__, q->readptr);
+
+	list_for_each_safe(old_msg_ptr, msg_ptr, &q->msg_list) {
+		old_msg = list_entry(old_msg_ptr, struct queue_element, entry);
+		if (old_msg == NULL) {
+			dev_err(shrm->dev, "no message found\n");
+			return -EFAULT;
+		}
+		list_del(old_msg_ptr);
+		q->readptr = (q->readptr + old_msg->size)%q->size;
+		kfree(old_msg);
+		break;
+	}
+	if (list_empty(&q->msg_list)) {
+		dev_dbg(shrm->dev, "List is empty setting RP= 0\n");
+		atomic_set(&q->q_rp, 0);
+	}
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return 0;
+}
+
+/**
+ * get_size_of_new_msg() - retrieve new message from message list
+ * @q:	message queue
+ *
+ * This function will retrieve most recent message from the corresponding
+ * queue list. New message is always retrieved from head side.
+ * It returns new message no, offset if FIFO and size.
+ */
+int get_size_of_new_msg(struct message_queue *q)
+{
+	struct queue_element *new_msg = NULL;
+	struct list_head *msg_list;
+	struct shrm_dev *shrm = q->shrm;
+	int size = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	spin_lock_bh(&q->update_lock);
+	list_for_each(msg_list, &q->msg_list) {
+		new_msg = list_entry(msg_list, struct queue_element, entry);
+		if (new_msg == NULL) {
+			spin_unlock_bh(&q->update_lock);
+			dev_err(shrm->dev, "no message found\n");
+			return -EFAULT;
+		}
+		size = new_msg->size;
+		break;
+	}
+	spin_unlock_bh(&q->update_lock);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return size;
+}
+
+/**
+ * isa_select() - shrm char interface driver select interface
+ * @filp:	file descriptor pointer
+ * @wait:	poll_table_struct pointer
+ *
+ * This function is used to perform non-blocking read operations. It allows
+ * a process to determine whether it can read from one or more open files
+ * without blocking. These calls can also block a process until any of a
+ * given set of file descriptors becomes available for reading.
+ * If a file is ready to read, POLLIN | POLLRDNORM bitmask is returned.
+ * The driver method is called whenever the user-space program performs a select
+ * system call involving a file descriptor associated with the driver.
+ */
+static u32 isa_select(struct file *filp,
+				struct poll_table_struct *wait)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	u32 mask = 0;
+	u32 m = iminor(filp->f_path.dentry->d_inode);
+	u8 idx = shrm_get_cdev_index(m);
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (shrm->msr_flag)
+		return -ENODEV;
+
+	if (isadev->device_id != idx)
+			return -1;
+
+	q = &isadev->dl_queue;
+	poll_wait(filp, &q->wq_readable, wait);
+	if (atomic_read(&q->q_rp) == 1)
+		mask = POLLIN | POLLRDNORM;
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return mask;
+}
+
+/**
+ * isa_read() - Read from device
+ * @filp:	file descriptor
+ * @buf:	user buffer pointer
+ * @len:	size of requested data transfer
+ * @ppos:	not used
+ *
+ * It reads a oldest message from queue and copies it into user buffer and
+ * returns its size.
+ * If there is no message present in queue, then it blocks until new data is
+ * available.
+ */
+ssize_t isa_read(struct file *filp, char __user *buf, size_t len, loff_t *ppos)
+{
+	u32 size = 0;
+	int ret;
+	char *psrc;
+	struct isadev_context *isadev = (struct isadev_context *)
+							filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	u32 msgsize;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (len <= 0)
+		return -EFAULT;
+
+	q = &isadev->dl_queue;
+
+	if (shrm->msr_flag) {
+		atomic_set(&q->q_rp, 0);
+		return -ENODEV;
+	}
+
+	spin_lock_bh(&q->update_lock);
+	if (list_empty(&q->msg_list)) {
+		spin_unlock_bh(&q->update_lock);
+		dev_dbg(shrm->dev, "Waiting for Data\n");
+		if (wait_event_interruptible(q->wq_readable,
+				atomic_read(&q->q_rp) == 1))
+			return -ERESTARTSYS;
+	} else
+		spin_unlock_bh(&q->update_lock);
+
+	if (shrm->msr_flag) {
+		atomic_set(&q->q_rp, 0);
+		return -ENODEV;
+	}
+
+	msgsize = get_size_of_new_msg(q);
+
+	if (len < msgsize)
+		return -EINVAL;
+
+	if ((q->readptr+msgsize) >= q->size) {
+		dev_dbg(shrm->dev, "Inside Loop Back\n");
+		psrc = (char *)buf;
+		size = (q->size-q->readptr);
+		/* Copy First Part of msg */
+		if (copy_to_user(psrc,
+				(u8 *)(q->fifo_base+q->readptr),
+				size)) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		if (copy_to_user(psrc,
+				(u8 *)(q->fifo_base),
+				(msgsize-size))) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+	} else {
+		if (copy_to_user(buf,
+				(u8 *)(q->fifo_base + q->readptr),
+				msgsize)) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+	}
+	spin_lock_bh(&q->update_lock);
+	ret = remove_msg_from_queue(q);
+	if (ret < 0) {
+		dev_err(shrm->dev,
+				"Remove msg from message queue failed\n");
+		msgsize = ret;
+	}
+	spin_unlock_bh(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return msgsize;
+}
+
+/**
+ * isa_write() - Write to shrm char device
+ * @filp:	file descriptor
+ * @buf:	user buffer pointer
+ * @len:	size of requested data transfer
+ * @ppos:	not used
+ *
+ * It checks if there is space available in queue, and copies the message
+ * inside queue. If there is no space, it blocks until space becomes available.
+ * It also schedules transfer thread to transmit the newly added message.
+ */
+ssize_t isa_write(struct file *filp, const char __user *buf,
+				 size_t len, loff_t *ppos)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	void *addr = 0;
+	int err, l2_header;
+	int ret = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (len <= 0 || buf == NULL)
+		return -EFAULT;
+	q = &isadev->dl_queue;
+	l2_header = shrm_get_cdev_l2header(isadev->device_id);
+	if (l2_header < 0) {
+		dev_err(shrm->dev, "failed to get L2 header\n");
+		return l2_header;
+	}
+
+	switch (l2_header) {
+	case RPC_MESSAGING:
+		dev_dbg(shrm->dev, "RPC\n");
+		addr = (void *)wr_rpc_msg;
+		break;
+	case AUDIO_MESSAGING:
+		dev_dbg(shrm->dev, "Audio\n");
+		addr = (void *)wr_audio_msg;
+		break;
+	case SECURITY_MESSAGING:
+		dev_dbg(shrm->dev, "Security\n");
+		addr = (void *)wr_sec_msg;
+		break;
+	case COMMON_LOOPBACK_MESSAGING:
+		dev_dbg(shrm->dev, "Common loopback\n");
+		addr = isadev->addr;
+		break;
+	case AUDIO_LOOPBACK_MESSAGING:
+		dev_dbg(shrm->dev, "Audio loopback\n");
+		addr = isadev->addr;
+		break;
+	case CIQ_MESSAGING:
+		dev_dbg(shrm->dev, "CIQ\n");
+		addr = isadev->addr;
+		break;
+	default:
+		dev_dbg(shrm->dev, "Wrong device\n");
+		return -EFAULT;
+	}
+
+	if (copy_from_user(addr, buf, len)) {
+		dev_err(shrm->dev, "copy_from_user failed\n");
+		return -EFAULT;
+	}
+	/* Write msg to Fifo */
+	if ((l2_header == AUDIO_MESSAGING) ||
+			(l2_header == AUDIO_LOOPBACK_MESSAGING)) {
+		mutex_lock(&shrm->isa_context->tx_audio_mutex);
+		err = shm_write_msg(shrm, l2_header, addr, len);
+		if (!err)
+			ret = len;
+		else
+			ret = err;
+		mutex_unlock(&shrm->isa_context->tx_audio_mutex);
+	} else {
+		spin_lock_bh(&shrm->isa_context->common_tx);
+		err = shm_write_msg(shrm, l2_header, addr, len);
+		if (!err)
+			ret = len;
+		else
+			ret = err;
+		spin_unlock_bh(&shrm->isa_context->common_tx);
+	}
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * isa_ioctl() - To handle different ioctl commands supported by driver.
+ * @inode:	structure is used by the kernel internally to represent files
+ * @filp:	file descriptor pointer
+ * @cmd:	ioctl command
+ * @arg:	input param
+ *
+ * Following ioctls are supported by this driver.
+ * DLP_IOCTL_ALLOCATE_BUFFER - To allocate buffer for new uplink message.
+ * This ioctl is called with required message size. It returns offset for
+ * the allocates space in the queue. DLP_IOCTL_PUT_MESSAGE -  To indicate
+ * new uplink message available in queuq for  transmission. Message is copied
+ * from offset location returned by previous ioctl before calling this ioctl.
+ * DLP_IOCTL_GET_MESSAGE - To check if any downlink message is available in
+ * queue. It returns  offset for new message inside queue.
+ * DLP_IOCTL_DEALLOCATE_BUFFER - To deallocate any buffer allocate for
+ * downlink message once the message is copied. Message is copied from offset
+ * location returned by previous ioctl before calling this ioctl.
+ */
+static long isa_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	long err = 0;
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	u32 m = iminor(filp->f_path.dentry->d_inode);
+
+	isadev = (struct isadev_context *)filp->private_data;
+
+	if (isadev->device_id != m)
+		return -EINVAL;
+
+	switch (cmd) {
+	case DLP_IOC_ALLOCATE_BUFFER:
+		dev_dbg(shrm->dev, "DLP_IOC_ALLOCATE_BUFFER\n");
+		break;
+	case DLP_IOC_PUT_MESSAGE:
+		dev_dbg(shrm->dev, "DLP_IOC_PUT_MESSAGE\n");
+		break;
+	case DLP_IOC_GET_MESSAGE:
+		dev_dbg(shrm->dev, "DLP_IOC_GET_MESSAGE\n");
+		break;
+	case DLP_IOC_DEALLOCATE_BUFFER:
+		dev_dbg(shrm->dev, "DLP_IOC_DEALLOCATE_BUFFER\n");
+		break;
+	default:
+		dev_dbg(shrm->dev, "Unknown IOCTL\n");
+		err = -EFAULT;
+		break;
+	}
+	return err;
+}
+/**
+ * isa_mmap() - Maps kernel queue memory to user space.
+ * @filp:	file descriptor pointer
+ * @vma:	virtual area memory structure.
+ *
+ * This function maps kernel FIFO into user space. This function
+ * shall be called twice to map both uplink and downlink buffers.
+ */
+static int isa_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+
+	u32 m = iminor(filp->f_path.dentry->d_inode);
+	dev_dbg(shrm->dev, "%s %d\n", __func__, m);
+
+	return 0;
+}
+
+/**
+ * isa_close() - Close device file
+ * @inode:	structure is used by the kernel internally to represent files
+ * @filp:	device file descriptor
+ *
+ * This function deletes structues associated with this file, deletes
+ * queues, flushes and destroys workqueus and closes this file.
+ * It also unregisters itself from l2mux driver.
+ */
+static int isa_close(struct inode *inode, struct file *filp)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct isa_driver_context *isa_context = shrm->isa_context;
+	u8 m;
+	int idx;
+
+	mutex_lock(&isa_lock);
+	m = iminor(filp->f_path.dentry->d_inode);
+	idx = shrm_get_cdev_index(m);
+	if (idx < 0) {
+		dev_err(shrm->dev, "failed to get index\n");
+		return idx;
+	}
+	dev_dbg(shrm->dev, "isa_close %d", m);
+
+	if (atomic_dec_and_test(&isa_context->is_open[idx])) {
+		atomic_inc(&isa_context->is_open[idx]);
+		dev_err(shrm->dev, "Device not opened yet\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	}
+	atomic_set(&isa_context->is_open[idx], 1);
+
+	switch (m) {
+	case RPC_MESSAGING:
+		dev_info(shrm->dev, "Close RPC_MESSAGING Device\n");
+		break;
+	case AUDIO_MESSAGING:
+		dev_info(shrm->dev, "Close AUDIO_MESSAGING Device\n");
+		break;
+	case SECURITY_MESSAGING:
+		dev_info(shrm->dev, "CLose SECURITY_MESSAGING Device\n");
+		break;
+	case COMMON_LOOPBACK_MESSAGING:
+		kfree(isadev->addr);
+		dev_info(shrm->dev, "Close COMMON_LOOPBACK_MESSAGING Device\n");
+		break;
+	case AUDIO_LOOPBACK_MESSAGING:
+		kfree(isadev->addr);
+		dev_info(shrm->dev, "Close AUDIO_LOOPBACK_MESSAGING Device\n");
+		break;
+	case CIQ_MESSAGING:
+		kfree(isadev->addr);
+		dev_info(shrm->dev, "Close CIQ_MESSAGING Device\n");
+		break;
+	default:
+		dev_info(shrm->dev, "No such device present\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	};
+	mutex_unlock(&isa_lock);
+	return 0;
+}
+/**
+ * isa_open() -  Open device file
+ * @inode:	structure is used by the kernel internally to represent files
+ * @filp:	device file descriptor
+ *
+ * This function performs initialization tasks needed to open SHM channel.
+ * Following tasks are performed.
+ * -return if device is already opened
+ * -create uplink FIFO
+ * -create downlink FIFO
+ * -init delayed workqueue thread
+ * -register to l2mux driver
+ */
+static int isa_open(struct inode *inode, struct file *filp)
+{
+	int err = 0;
+	u8 m;
+	int idx;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context = container_of(
+						inode->i_cdev,
+						struct isa_driver_context,
+						cdev);
+	struct shrm_dev *shrm = isa_context->isadev->dl_queue.shrm;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (get_boot_state() != BOOT_DONE) {
+		dev_err(shrm->dev, "Boot is not done\n");
+		return -EBUSY;
+	}
+	mutex_lock(&isa_lock);
+	m = iminor(inode);
+
+	if ((m != RPC_MESSAGING) &&
+				(m != AUDIO_LOOPBACK_MESSAGING) &&
+				(m != COMMON_LOOPBACK_MESSAGING) &&
+				(m != AUDIO_MESSAGING) &&
+				(m != SECURITY_MESSAGING) &&
+				(m != CIQ_MESSAGING)) {
+		dev_err(shrm->dev, "No such device present\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	}
+	idx = shrm_get_cdev_index(m);
+	if (idx < 0) {
+		dev_err(shrm->dev, "failed to get index\n");
+		return idx;
+	}
+	if (!atomic_dec_and_test(&isa_context->is_open[idx])) {
+		atomic_inc(&isa_context->is_open[idx]);
+		dev_err(shrm->dev, "Device already opened\n");
+		mutex_unlock(&isa_lock);
+		return -EBUSY;
+	}
+	isadev = &isa_context->isadev[idx];
+	if (filp != NULL)
+		filp->private_data = isadev;
+
+	switch (m) {
+	case RPC_MESSAGING:
+		dev_info(shrm->dev, "Open RPC_MESSAGING Device\n");
+		break;
+	case AUDIO_MESSAGING:
+		dev_info(shrm->dev, "Open AUDIO_MESSAGING Device\n");
+		break;
+	case SECURITY_MESSAGING:
+		dev_info(shrm->dev, "Open SECURITY_MESSAGING Device\n");
+		break;
+	case COMMON_LOOPBACK_MESSAGING:
+		isadev->addr = kzalloc(10 * 1024, GFP_KERNEL);
+		if (!isadev->addr) {
+			mutex_unlock(&isa_lock);
+			return -ENOMEM;
+		}
+		dev_info(shrm->dev, "Open COMMON_LOOPBACK_MESSAGING Device\n");
+		break;
+	case AUDIO_LOOPBACK_MESSAGING:
+		isadev->addr = kzalloc(10 * 1024, GFP_KERNEL);
+		if (!isadev->addr) {
+			mutex_unlock(&isa_lock);
+			return -ENOMEM;
+		}
+		dev_info(shrm->dev, "Open AUDIO_LOOPBACK_MESSAGING Device\n");
+		break;
+	case CIQ_MESSAGING:
+		isadev->addr = kzalloc(10 * 1024, GFP_KERNEL);
+		if (!isadev->addr) {
+			mutex_unlock(&isa_lock);
+			return -ENOMEM;
+		}
+		dev_info(shrm->dev, "Open CIQ_MESSAGING Device\n");
+		break;
+	};
+
+	mutex_unlock(&isa_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return err;
+}
+
+const struct file_operations isa_fops = {
+	.owner = THIS_MODULE,
+	.open = isa_open,
+	.release = isa_close,
+	.unlocked_ioctl = isa_ioctl,
+	.mmap = isa_mmap,
+	.read = isa_read,
+	.write = isa_write,
+	.poll = isa_select,
+};
+
+/**
+ * isa_init() - module insertion function
+ * @shrm:	pointer to the shrm device information structure
+ *
+ * This function registers module as a character driver using
+ * register_chrdev_region() or alloc_chrdev_region. It adds this
+ * driver to system using cdev_add() call. Major number is dynamically
+ * allocated using alloc_chrdev_region() by default or left to user to specify
+ * it during load time. For this variable major is used as module_param
+ * Nodes to be created using
+ * mknod /dev/isi c $major 0
+ * mknod /dev/rpc c $major 1
+ * mknod /dev/audio c $major 2
+ * mknod /dev/sec c $major 3
+ */
+int isa_init(struct shrm_dev *shrm)
+{
+	dev_t	dev_id;
+	int	retval, no_dev;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context;
+
+	isa_context = kzalloc(sizeof(struct isa_driver_context),
+								GFP_KERNEL);
+	if (isa_context == NULL) {
+		dev_err(shrm->dev, "Failed to alloc memory\n");
+		return -ENOMEM;
+	}
+	shrm->isa_context = isa_context;
+	if (major) {
+		dev_id = MKDEV(major, MAX_L2_HEADERS);
+		retval = register_chrdev_region(dev_id, ISA_DEVICES, NAME);
+	} else {
+		/*
+		 * L2 header of loopback device is 192(0xc0). As per the shrm
+		 * protocol the minor id of the deivce is mapped to the
+		 * L2 header.
+		 */
+		retval = alloc_chrdev_region(&dev_id, 0, MAX_L2_HEADERS, NAME);
+		major = MAJOR(dev_id);
+	}
+	dev_dbg(shrm->dev, " major %d\n", major);
+
+	cdev_init(&isa_context->cdev, &isa_fops);
+	isa_context->cdev.owner = THIS_MODULE;
+	retval = cdev_add(&isa_context->cdev, dev_id, MAX_L2_HEADERS);
+	if (retval) {
+		dev_err(shrm->dev, "Failed to add char device\n");
+		return retval;
+	}
+	/* create class and device */
+	isa_context->shm_class = class_create(THIS_MODULE, NAME);
+	if (IS_ERR(isa_context->shm_class)) {
+		dev_err(shrm->dev, "Error creating shrm class\n");
+		cdev_del(&isa_context->cdev);
+		retval = PTR_ERR(isa_context->shm_class);
+		kfree(isa_context);
+		return retval;
+	}
+
+	for (no_dev = 0; no_dev < ISA_DEVICES; no_dev++) {
+		atomic_set(&isa_context->is_open[no_dev], 1);
+		device_create(isa_context->shm_class, NULL,
+				MKDEV(MAJOR(dev_id),
+				map_dev[no_dev].l2_header), NULL,
+				map_dev[no_dev].name);
+	}
+
+	isa_context->isadev = kzalloc(sizeof
+				(struct isadev_context)*ISA_DEVICES,
+				GFP_KERNEL);
+	if (isa_context->isadev == NULL) {
+		dev_err(shrm->dev, "Failed to alloc memory\n");
+		return -ENOMEM;
+	}
+	for (no_dev = 0 ; no_dev < ISA_DEVICES ; no_dev++) {
+		isadev = &isa_context->isadev[no_dev];
+		isadev->device_id = no_dev;
+		retval = create_queue(&isadev->dl_queue,
+					isadev->device_id, shrm);
+
+		if (retval < 0) {
+			dev_err(shrm->dev, "create dl_queue failed\n");
+			delete_queue(&isadev->dl_queue);
+			kfree(isadev);
+			return retval;
+		}
+	}
+	mutex_init(&isa_context->tx_audio_mutex);
+	spin_lock_init(&isa_context->common_tx);
+	dev_dbg(shrm->dev, " SHM Char Driver added\n");
+	return retval;
+}
+
+void isa_exit(struct shrm_dev *shrm)
+{
+	int no_dev;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context = shrm->isa_context;
+	dev_t dev_id = MKDEV(major, 0);
+
+	for (no_dev = 0 ; no_dev < ISA_DEVICES ; no_dev++) {
+		device_destroy(isa_context->shm_class,
+				MKDEV(MAJOR(dev_id),
+				map_dev[no_dev].l2_header));
+		isadev = &isa_context->isadev[no_dev];
+		delete_queue(&isadev->dl_queue);
+		kfree(isadev);
+	}
+	class_destroy(isa_context->shm_class);
+	cdev_del(&isa_context->cdev);
+	unregister_chrdev_region(dev_id, ISA_DEVICES);
+	kfree(isa_context);
+	dev_dbg(shrm->dev, " SHM Char Driver removed\n");
+}
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index c044060a4b0..a194847baac 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -42,3 +42,4 @@ obj-$(CONFIG_X86_CPUFREQ_NFORCE2)	+= cpufreq-nforce2.o
 
 # ARM SoC drivers
 obj-$(CONFIG_UX500_SOC_DB8500)		+= db8500-cpufreq.o
+obj-$(CONFIG_UX500_SOC_DB5500)		+= db5500-cpufreq.o
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 0a5bea9e358..b1bcd16d421 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -368,6 +368,27 @@ show_one(scaling_cur_freq, cur);
 static int __cpufreq_set_policy(struct cpufreq_policy *data,
 				struct cpufreq_policy *policy);
 
+int cpufreq_update_freq(int cpu, unsigned int min, unsigned int max)
+{
+	int ret;
+	struct cpufreq_policy new_policy;
+	struct cpufreq_policy *policy = cpufreq_cpu_get(cpu);
+
+	ret = cpufreq_get_policy(&new_policy, cpu);
+	if (ret)
+		return -EINVAL;
+
+	new_policy.min = min;
+	new_policy.max = max;
+
+	ret = __cpufreq_set_policy(policy, &new_policy);
+	policy->user_policy.min = policy->min;
+	policy->user_policy.max = policy->max;
+
+	return ret;
+}
+EXPORT_SYMBOL(cpufreq_update_freq);
+
 /**
  * cpufreq_per_cpu_attr_write() / store_##file_name() - sysfs write access
  */
diff --git a/drivers/cpufreq/cpufreq_ondemand.c b/drivers/cpufreq/cpufreq_ondemand.c
index 891360edecd..bcef88a89b0 100644
--- a/drivers/cpufreq/cpufreq_ondemand.c
+++ b/drivers/cpufreq/cpufreq_ondemand.c
@@ -30,7 +30,7 @@
 
 #define DEF_FREQUENCY_DOWN_DIFFERENTIAL		(10)
 #define DEF_FREQUENCY_UP_THRESHOLD		(80)
-#define DEF_SAMPLING_DOWN_FACTOR		(1)
+#define DEF_SAMPLING_DOWN_FACTOR		(10)
 #define MAX_SAMPLING_DOWN_FACTOR		(100000)
 #define MICRO_FREQUENCY_DOWN_DIFFERENTIAL	(3)
 #define MICRO_FREQUENCY_UP_THRESHOLD		(95)
@@ -390,6 +390,10 @@ static struct attribute_group dbs_attr_group = {
 	.name = "ondemand",
 };
 
+/*** delete after deprecation time ***/
+extern bool wlan_mode_on;
+extern bool usb_mode_on;
+
 /************************** sysfs end ************************/
 
 static void dbs_freq_increase(struct cpufreq_policy *p, unsigned int freq)
@@ -494,7 +498,9 @@ static void dbs_check_cpu(struct cpu_dbs_info_s *this_dbs_info)
 	}
 
 	/* Check for frequency increase */
-	if (max_load_freq > dbs_tuners_ins.up_threshold * policy->cur) {
+	if ((max_load_freq > dbs_tuners_ins.up_threshold * policy->cur) ||
+		/* A work around for wlan and usb performance issues */
+		(usb_mode_on || wlan_mode_on)) {
 		/* If switching to max speed, apply sampling_down_factor */
 		if (policy->cur < policy->max)
 			this_dbs_info->rate_mult =
diff --git a/drivers/cpufreq/db5500-cpufreq.c b/drivers/cpufreq/db5500-cpufreq.c
new file mode 100644
index 00000000000..7c78bcb4761
--- /dev/null
+++ b/drivers/cpufreq/db5500-cpufreq.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) STMicroelectronics 2009
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Sundar Iyer <sundar.iyer@stericsson.com>
+ * Author: Martin Persson <martin.persson@stericsson.com>
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com>
+ *
+ */
+#include <linux/kernel.h>
+#include <linux/cpufreq.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <mach/prcmu.h>
+#include <mach/id.h>
+
+static struct cpufreq_frequency_table freq_table[] = {
+	[0] = {
+		.index = 0,
+		.frequency = 200000,
+	},
+	[1] = {
+		.index = 1,
+		.frequency = 300000,
+	},
+	[2] = {
+		.index = 2,
+		.frequency = 600000,
+	},
+	[3] = {
+		.index = 3,
+		.frequency = CPUFREQ_TABLE_END,
+	},
+};
+
+static enum arm_opp idx2opp[] = {
+	ARM_EXTCLK,
+	ARM_50_OPP,
+	ARM_100_OPP
+};
+
+/*
+ * Below is a temporary workaround for wlan performance issues
+ */
+
+#include <linux/kernel_stat.h>
+#include <linux/workqueue.h>
+#include <linux/cpu.h>
+
+#include <mach/irqs.h>
+
+static struct freq_attr *db5500_cpufreq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static int db5500_cpufreq_verify_speed(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy, freq_table);
+}
+
+static int db5500_cpufreq_target(struct cpufreq_policy *policy,
+				unsigned int target_freq,
+				unsigned int relation)
+{
+	struct cpufreq_freqs freqs;
+	unsigned int idx;
+
+	/* scale the target frequency to one of the extremes supported */
+	if (target_freq < policy->cpuinfo.min_freq)
+		target_freq = policy->cpuinfo.min_freq;
+	if (target_freq > policy->cpuinfo.max_freq)
+		target_freq = policy->cpuinfo.max_freq;
+
+	/* Lookup the next frequency */
+	if (cpufreq_frequency_table_target
+	    (policy, freq_table, target_freq, relation, &idx)) {
+		return -EINVAL;
+	}
+
+	freqs.old = policy->cur;
+	freqs.new = freq_table[idx].frequency;
+	freqs.cpu = policy->cpu;
+
+	if (freqs.old == freqs.new)
+		return 0;
+
+	/* pre-change notification */
+	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+
+	/* request the PRCM unit for opp change */
+	if (prcmu_set_arm_opp(idx2opp[idx])) {
+		pr_err("db5500-cpufreq:  Failed to set OPP level\n");
+		return -EINVAL;
+	}
+
+	/* post change notification */
+	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+
+	return 0;
+}
+
+static unsigned int db5500_cpufreq_getspeed(unsigned int cpu)
+{
+	int i;
+	/* request the prcm to get the current ARM opp */
+	for (i = 0; prcmu_get_arm_opp() != idx2opp[i]; i++)
+		;
+	return freq_table[i].frequency;
+}
+
+static int __cpuinit db5500_cpufreq_init(struct cpufreq_policy *policy)
+{
+	int res;
+	int i = 0;
+
+	BUILD_BUG_ON(ARRAY_SIZE(idx2opp) + 1 != ARRAY_SIZE(freq_table));
+
+	pr_info("db5500-cpufreq : Available frequencies:\n");
+	while (freq_table[i].frequency != CPUFREQ_TABLE_END)
+		pr_info("  %d Mhz\n", freq_table[i++].frequency/1000);
+
+	/* get policy fields based on the table */
+	res = cpufreq_frequency_table_cpuinfo(policy, freq_table);
+	if (!res)
+		cpufreq_frequency_table_get_attr(freq_table, policy->cpu);
+	else {
+		pr_err("db5500-cpufreq : Failed to read policy table\n");
+		return res;
+	}
+
+	policy->min = policy->cpuinfo.min_freq;
+	policy->max = policy->cpuinfo.max_freq;
+	policy->cur = db5500_cpufreq_getspeed(policy->cpu);
+
+	for (i = 0; freq_table[i].frequency != policy->cur; i++)
+		;
+
+	policy->governor = CPUFREQ_DEFAULT_GOVERNOR;
+
+	/*
+	 * FIXME : Need to take time measurement across the target()
+	 *	   function with no/some/all drivers in the notification
+	 *	   list.
+	 */
+	policy->cpuinfo.transition_latency = 20 * 1000; /* in ns */
+
+	/* policy sharing between dual CPUs */
+	cpumask_copy(policy->cpus, &cpu_present_map);
+
+	policy->shared_type = CPUFREQ_SHARED_TYPE_ALL;
+
+	return 0;
+}
+
+static struct cpufreq_driver db5500_cpufreq_driver = {
+	.flags  = CPUFREQ_STICKY,
+	.verify = db5500_cpufreq_verify_speed,
+	.target = db5500_cpufreq_target,
+	.get    = db5500_cpufreq_getspeed,
+	.init   = db5500_cpufreq_init,
+	.name   = "DB5500",
+	.attr   = db5500_cpufreq_attr,
+};
+
+static int __init db5500_cpufreq_register(void)
+{
+	pr_info("cpufreq for DB5500 started\n");
+	return cpufreq_register_driver(&db5500_cpufreq_driver);
+}
+device_initcall(db5500_cpufreq_register);
diff --git a/drivers/cpufreq/db8500-cpufreq.c b/drivers/cpufreq/db8500-cpufreq.c
index d90456a809f..629b0b88e6c 100644
--- a/drivers/cpufreq/db8500-cpufreq.c
+++ b/drivers/cpufreq/db8500-cpufreq.c
@@ -12,35 +12,116 @@
 #include <linux/cpufreq.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
-#include <linux/mfd/db8500-prcmu.h>
+#include <mach/prcmu.h>
 #include <mach/id.h>
 
 static struct cpufreq_frequency_table freq_table[] = {
 	[0] = {
 		.index = 0,
-		.frequency = 300000,
+		.frequency = 200000,
 	},
 	[1] = {
 		.index = 1,
-		.frequency = 600000,
+		.frequency = 300000,
 	},
 	[2] = {
-		/* Used for MAX_OPP, if available */
 		.index = 2,
-		.frequency = CPUFREQ_TABLE_END,
+		.frequency = 600000,
 	},
 	[3] = {
+		/* Used for MAX_OPP, if available */
 		.index = 3,
 		.frequency = CPUFREQ_TABLE_END,
 	},
+	[4] = {
+		.index = 4,
+		.frequency = CPUFREQ_TABLE_END,
+	},
 };
 
 static enum arm_opp idx2opp[] = {
+	ARM_EXTCLK,
 	ARM_50_OPP,
 	ARM_100_OPP,
 	ARM_MAX_OPP
 };
 
+/*
+ * Below is a temporary workaround for wlan performance issues
+ */
+
+#include <linux/kernel_stat.h>
+#include <linux/workqueue.h>
+#include <linux/cpu.h>
+
+#include <mach/irqs.h>
+
+#define WLAN_PROBE_DELAY 3000 /* 3 seconds */
+#define WLAN_LIMIT (3000/3) /* If we have more than 1000 irqs per second */
+#define USB_PROBE_DELAY 1000 /* 1 seconds */
+#define USB_LIMIT (200) /* If we have more than 200 irqs per second */
+static struct delayed_work work_usb_workaround;
+bool usb_mode_on;
+
+static struct delayed_work work_wlan_workaround;
+bool wlan_mode_on;
+
+static void wlan_load(struct work_struct *work)
+{
+	int cpu;
+	unsigned int num_irqs = 0;
+	static unsigned int old_num_irqs = UINT_MAX;
+
+	for_each_online_cpu(cpu)
+		num_irqs += kstat_irqs_cpu(IRQ_DB8500_SDMMC1, cpu);
+
+	if ((num_irqs > old_num_irqs) &&
+	    (num_irqs - old_num_irqs) > WLAN_LIMIT)
+		wlan_mode_on = true;
+	else
+		wlan_mode_on = false;
+
+	old_num_irqs = num_irqs;
+
+	schedule_delayed_work_on(0,
+				 &work_wlan_workaround,
+				 msecs_to_jiffies(WLAN_PROBE_DELAY));
+}
+
+static void usb_load(struct work_struct *work)
+{
+	int cpu;
+	unsigned int num_irqs = 0;
+	static unsigned int old_num_irqs = UINT_MAX;
+
+	for_each_online_cpu(cpu)
+		num_irqs += kstat_irqs_cpu(IRQ_DB8500_USBOTG, cpu);
+
+	if ((num_irqs > old_num_irqs) &&
+	    (num_irqs - old_num_irqs) > USB_LIMIT)
+		usb_mode_on = true;
+	else
+		usb_mode_on = false;
+
+	old_num_irqs = num_irqs;
+
+	schedule_delayed_work_on(0,
+				 &work_usb_workaround,
+				 msecs_to_jiffies(USB_PROBE_DELAY));
+}
+
+void cpufreq_usb_connect_notify(bool connect)
+{
+	if (connect) {
+		schedule_delayed_work_on(0,
+				 &work_usb_workaround,
+				 msecs_to_jiffies(USB_PROBE_DELAY));
+	} else {
+		cancel_delayed_work_sync(&work_usb_workaround);
+		usb_mode_on = false;
+	}
+}
+
 static struct freq_attr *db8500_cpufreq_attr[] = {
 	&cpufreq_freq_attr_scaling_available_freqs,
 	NULL,
@@ -104,17 +185,31 @@ static unsigned int db8500_cpufreq_getspeed(unsigned int cpu)
 static int __cpuinit db8500_cpufreq_init(struct cpufreq_policy *policy)
 {
 	int res;
-	int i;
+	int i = 0;
 
 	BUILD_BUG_ON(ARRAY_SIZE(idx2opp) + 1 != ARRAY_SIZE(freq_table));
 
-	if (cpu_is_u8500v2() && !prcmu_is_u8400()) {
-		freq_table[0].frequency = 400000;
-		freq_table[1].frequency = 800000;
+	if (!prcmu_is_u8400()) {
+		freq_table[1].frequency = 400000;
+		freq_table[2].frequency = 800000;
 		if (prcmu_has_arm_maxopp())
-			freq_table[2].frequency = 1000000;
+			freq_table[3].frequency = 1000000;
 	}
 
+	INIT_DELAYED_WORK_DEFERRABLE(&work_wlan_workaround,
+				     wlan_load);
+
+	schedule_delayed_work_on(0,
+				 &work_wlan_workaround,
+				 msecs_to_jiffies(WLAN_PROBE_DELAY));
+
+	INIT_DELAYED_WORK_DEFERRABLE(&work_usb_workaround,
+				     usb_load);
+
+	pr_info("db8500-cpufreq : Available frequencies:\n");
+	while (freq_table[i].frequency != CPUFREQ_TABLE_END)
+		pr_info("  %d Mhz\n", freq_table[i++].frequency/1000);
+
 	/* get policy fields based on the table */
 	res = cpufreq_frequency_table_cpuinfo(policy, freq_table);
 	if (!res)
diff --git a/drivers/cpuidle/Kconfig b/drivers/cpuidle/Kconfig
index 7dbc4a83c45..23bf98a1a1c 100644
--- a/drivers/cpuidle/Kconfig
+++ b/drivers/cpuidle/Kconfig
@@ -2,6 +2,7 @@
 config CPU_IDLE
 	bool "CPU idle PM support"
 	default ACPI
+	depends on PM
 	help
 	  CPU idle is a generic framework for supporting software-controlled
 	  idle processor power management.  It includes modular cross-platform
diff --git a/drivers/cpuidle/governors/menu.c b/drivers/cpuidle/governors/menu.c
index c47f3d09c1e..ed28f774ca4 100644
--- a/drivers/cpuidle/governors/menu.c
+++ b/drivers/cpuidle/governors/menu.c
@@ -19,6 +19,8 @@
 #include <linux/tick.h>
 #include <linux/sched.h>
 #include <linux/math64.h>
+#include <linux/cpu.h>
+#include <linux/sysfs.h>
 
 #define BUCKETS 12
 #define INTERVALS 8
@@ -121,6 +123,8 @@ struct menu_device {
 	int		interval_ptr;
 };
 
+static int tune_multiplier = 1024;
+static int forced_state;
 
 #define LOAD_INT(x) ((x) >> FSHIFT)
 #define LOAD_FRAC(x) LOAD_INT(((x) & (FIXED_1-1)) * 100)
@@ -170,6 +174,9 @@ static inline int performance_multiplier(void)
 {
 	int mult = 1;
 
+	if (tune_multiplier <= 1)
+		return tune_multiplier;
+
 	/* for higher loadavg, we are more reluctant */
 
 	mult += 2 * get_loadavg();
@@ -177,6 +184,9 @@ static inline int performance_multiplier(void)
 	/* for IO wait tasks (per cpu!) we add 5x each */
 	mult += 10 * nr_iowait_cpu(smp_processor_id());
 
+	if (tune_multiplier != 1024)
+		mult = (tune_multiplier * mult) / 1024;
+
 	return mult;
 }
 
@@ -281,26 +291,34 @@ static int menu_select(struct cpuidle_device *dev)
 	if (data->expected_us > 5)
 		data->last_state_idx = CPUIDLE_DRIVER_STATE_START;
 
-	/*
-	 * Find the idle state with the lowest power while satisfying
-	 * our constraints.
-	 */
-	for (i = CPUIDLE_DRIVER_STATE_START; i < dev->state_count; i++) {
-		struct cpuidle_state *s = &dev->states[i];
-
-		if (s->flags & CPUIDLE_FLAG_IGNORE)
-			continue;
-		if (s->target_residency > data->predicted_us)
-			continue;
-		if (s->exit_latency > latency_req)
-			continue;
-		if (s->exit_latency * multiplier > data->predicted_us)
-			continue;
-
-		if (s->power_usage < power_usage) {
-			power_usage = s->power_usage;
-			data->last_state_idx = i;
-			data->exit_us = s->exit_latency;
+	WARN((forced_state >= dev->state_count), \
+		"Forced state value out of range.\n");
+
+	if ((forced_state != 0) && (forced_state < dev->state_count)) {
+		data->exit_us = dev->states[forced_state].exit_latency;
+		data->last_state_idx = forced_state;
+	} else {
+		/*
+		 * Find the idle state with the lowest power while satisfying
+		 * our constraints.
+		 */
+		for (i = CPUIDLE_DRIVER_STATE_START; i < dev->state_count; i++) {
+			struct cpuidle_state *s = &dev->states[i];
+
+			if (s->flags & CPUIDLE_FLAG_IGNORE)
+				continue;
+			if (s->target_residency > data->predicted_us)
+				continue;
+			if (s->exit_latency > latency_req)
+				continue;
+			if (s->exit_latency * multiplier > data->predicted_us)
+				continue;
+
+			if (s->power_usage < power_usage) {
+				power_usage = s->power_usage;
+				data->last_state_idx = i;
+				data->exit_us = s->exit_latency;
+			}
 		}
 	}
 
@@ -381,6 +399,63 @@ static void menu_update(struct cpuidle_device *dev)
 		data->interval_ptr = 0;
 }
 
+int cpuidle_set_multiplier(unsigned int value)
+{
+
+	if (value > 1024)
+		tune_multiplier = 1024;
+	else
+		tune_multiplier = value;
+
+	return 0;
+}
+EXPORT_SYMBOL(cpuidle_set_multiplier);
+
+/* Writing 0 will remove the forced state. */
+int cpuidle_force_state(unsigned int state)
+{
+	forced_state = state;
+
+	return 0;
+}
+EXPORT_SYMBOL(cpuidle_force_state);
+
+static ssize_t show_multiplier(struct sysdev_class *class,
+				      struct sysdev_class_attribute *attr,
+					  char *buf)
+{
+	return sprintf(buf, "%d\n", tune_multiplier);
+}
+
+static ssize_t store_multiplier(struct sysdev_class *class,
+					struct sysdev_class_attribute *attr,
+				    const char *buf, size_t count)
+{
+	unsigned int input;
+	int ret;
+	ret = sscanf(buf, "%u", &input);
+
+	if (ret != 1)
+		return -EINVAL;
+
+	cpuidle_set_multiplier(input);
+
+	return count;
+}
+
+
+static SYSDEV_CLASS_ATTR(multiplier, 0644, show_multiplier, store_multiplier);
+
+static struct attribute *dbs_attributes[] = {
+	&attr_multiplier.attr,
+	NULL
+};
+
+static struct attribute_group dbs_attr_group = {
+	.attrs = dbs_attributes,
+	.name = "cpuidle",
+};
+
 /**
  * menu_enable_device - scans a CPU's states and does setup
  * @dev: the CPU
@@ -408,7 +483,15 @@ static struct cpuidle_governor menu_governor = {
  */
 static int __init init_menu(void)
 {
-	return cpuidle_register_governor(&menu_governor);
+	int ret;
+
+	ret = cpuidle_register_governor(&menu_governor);
+
+	sysfs_merge_group(&(cpu_sysdev_class.kset.kobj),
+						&dbs_attr_group);
+
+	return ret;
+
 }
 
 /**
@@ -416,6 +499,9 @@ static int __init init_menu(void)
  */
 static void __exit exit_menu(void)
 {
+	sysfs_unmerge_group(&(cpu_sysdev_class.kset.kobj),
+						&dbs_attr_group);
+
 	cpuidle_unregister_governor(&menu_governor);
 }
 
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index e0b25de1e33..579218e43f3 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -292,4 +292,15 @@ config CRYPTO_DEV_S5P
 	  Select this to offload Samsung S5PV210 or S5PC110 from AES
 	  algorithms execution.
 
+config CRYPTO_DEV_UX500
+	tristate "Driver for ST-Ericsson UX500 crypto hardware acceleration"
+	depends on ARCH_U8500
+	select CRYPTO_ALGAPI
+	help
+	  Driver for ST-Ericsson UX500 crypto engine.
+
+if CRYPTO_DEV_UX500
+	source "drivers/crypto/ux500/Kconfig"
+endif # if CRYPTO_DEV_UX500
+
 endif # CRYPTO_HW
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index 53ea5015531..dbcc3113205 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -13,3 +13,4 @@ obj-$(CONFIG_CRYPTO_DEV_OMAP_SHAM) += omap-sham.o
 obj-$(CONFIG_CRYPTO_DEV_OMAP_AES) += omap-aes.o
 obj-$(CONFIG_CRYPTO_DEV_PICOXCELL) += picoxcell_crypto.o
 obj-$(CONFIG_CRYPTO_DEV_S5P) += s5p-sss.o
+obj-$(CONFIG_CRYPTO_DEV_UX500) += ux500/
diff --git a/drivers/crypto/ux500/Kconfig b/drivers/crypto/ux500/Kconfig
new file mode 100644
index 00000000000..165a03d46c0
--- /dev/null
+++ b/drivers/crypto/ux500/Kconfig
@@ -0,0 +1,29 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Shujuan Chen (shujuan.chen@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+
+config CRYPTO_DEV_UX500_CRYP
+	tristate "UX500 crypto driver for CRYP block"
+	depends on CRYPTO_DEV_UX500
+	select CRYPTO_DES
+	help
+	  This is the driver for the crypto block CRYP.
+
+config CRYPTO_DEV_UX500_HASH
+	tristate "UX500 crypto driver for HASH block"
+	depends on CRYPTO_DEV_UX500
+	select CRYPTO_HASH
+	select CRYPTO_HMAC
+	help
+	  This selects the UX500 hash driver for the HASH hardware.
+	  Depends on U8500/STM DMA if running in DMA mode.
+
+config CRYPTO_DEV_UX500_DEBUG
+	bool "Activate ux500 platform debug-mode for crypto and hash block"
+	depends on CRYPTO_DEV_UX500_CRYP || CRYPTO_DEV_UX500_HASH
+	default n
+	help
+	  Say Y if you want to add debug prints to ux500_hash and
+	  ux500_cryp devices.
diff --git a/drivers/crypto/ux500/Makefile b/drivers/crypto/ux500/Makefile
new file mode 100644
index 00000000000..b9a365bade8
--- /dev/null
+++ b/drivers/crypto/ux500/Makefile
@@ -0,0 +1,8 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Shujuan Chen (shujuan.chen@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+
+obj-$(CONFIG_CRYPTO_DEV_UX500_HASH) += hash/
+obj-$(CONFIG_CRYPTO_DEV_UX500_CRYP) += cryp/
diff --git a/drivers/crypto/ux500/cryp/Makefile b/drivers/crypto/ux500/cryp/Makefile
new file mode 100644
index 00000000000..fd5e6df3861
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/Makefile
@@ -0,0 +1,13 @@
+#/*
+# * Copyright (C) ST-Ericsson SA 2010
+# * Author: shujuan.chen@stericsson.com for ST-Ericsson.
+# * License terms: GNU General Public License (GPL) version 2  */
+
+ifdef CONFIG_CRYPTO_DEV_UX500_DEBUG
+CFLAGS_cryp_core.o := -DDEBUG -O0
+CFLAGS_cryp.o := -DDEBUG -O0
+CFLAGS_cryp_irq.o := -DDEBUG -O0
+endif
+
+obj-$(CONFIG_CRYPTO_DEV_UX500_CRYP) += u8500_cryp.o
+u8500_cryp-objs :=  cryp.o cryp_irq.o cryp_core.o
diff --git a/drivers/crypto/ux500/cryp/cryp.c b/drivers/crypto/ux500/cryp/cryp.c
new file mode 100644
index 00000000000..ae4fe318528
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp.c
@@ -0,0 +1,409 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+
+#include "cryp_p.h"
+#include "cryp.h"
+
+/**
+ * cryp_wait_until_done - wait until the device logic is not busy
+ */
+void cryp_wait_until_done(struct cryp_device_data *device_data)
+{
+	while (cryp_is_logic_busy(device_data))
+		cpu_relax();
+}
+
+/**
+ * cryp_check - This routine checks Peripheral and PCell Id
+ * @device_data: Pointer to the device data struct for base address.
+ */
+int cryp_check(struct cryp_device_data *device_data)
+{
+	if (NULL == device_data)
+		return -EINVAL;
+
+	/* Check Peripheral and Pcell Id Register for CRYP */
+	if ((CRYP_PERIPHERAL_ID0 ==
+	     readl_relaxed(&device_data->base->periphId0))
+	    && (CRYP_PERIPHERAL_ID1 ==
+		readl_relaxed(&device_data->base->periphId1))
+	    && (CRYP_PERIPHERAL_ID2 ==
+		readl_relaxed(&device_data->base->periphId2))
+	    && (CRYP_PERIPHERAL_ID3 ==
+		readl_relaxed(&device_data->base->periphId3))
+	    && (CRYP_PCELL_ID0 ==
+		readl_relaxed(&device_data->base->pcellId0))
+	    && (CRYP_PCELL_ID1 ==
+		readl_relaxed(&device_data->base->pcellId1))
+	    && (CRYP_PCELL_ID2 ==
+		readl_relaxed(&device_data->base->pcellId2))
+	    && (CRYP_PCELL_ID3 ==
+		readl_relaxed(&device_data->base->pcellId3))) {
+		return 0;
+	}
+
+	return -EPERM;
+}
+
+/**
+ * cryp_activity - This routine enables/disable the cryptography function.
+ * @device_data: Pointer to the device data struct for base address.
+ * @cryp_activity: Enable/Disable functionality
+ */
+void cryp_activity(struct cryp_device_data *device_data,
+		   enum cryp_crypen cryp_crypen)
+{
+	CRYP_PUT_BITS(&device_data->base->cr,
+		      cryp_crypen,
+		      CRYP_CR_CRYPEN_POS,
+		      CRYP_CR_CRYPEN_MASK);
+}
+
+/**
+ * cryp_flush_inoutfifo - Resets both the input and the output FIFOs
+ * @device_data: Pointer to the device data struct for base address.
+ */
+void cryp_flush_inoutfifo(struct cryp_device_data *device_data)
+{
+	/*
+	 * We always need to disble the hardware before trying to flush the
+	 * FIFO. This is something that isn't written in the design
+	 * specification, but we have been informed by the hardware designers
+	 * that this must be done.
+	 */
+	cryp_activity(device_data, CRYP_CRYPEN_DISABLE);
+	cryp_wait_until_done(device_data);
+
+	CRYP_SET_BITS(&device_data->base->cr, CRYP_CR_FFLUSH_MASK);
+	/*
+	 * CRYP_SR_INFIFO_READY_MASK is the expected value on the status
+	 * register when starting a new calculation, which means Input FIFO is
+	 * not full and input FIFO is empty.
+	 */
+	while (readl_relaxed(&device_data->base->sr) !=
+	       CRYP_SR_INFIFO_READY_MASK)
+		cpu_relax();
+}
+
+/**
+ * cryp_set_configuration - This routine set the cr CRYP IP
+ * @device_data: Pointer to the device data struct for base address.
+ * @cryp_config: Pointer to the configuration parameter
+ * @control_register: The control register to be written later on.
+ */
+int cryp_set_configuration(struct cryp_device_data *device_data,
+			   struct cryp_config *cryp_config,
+			   u32 *control_register)
+{
+	u32 cr_for_kse;
+
+	if (NULL == device_data || NULL == cryp_config)
+		return -EINVAL;
+
+	*control_register |= (cryp_config->keysize << CRYP_CR_KEYSIZE_POS);
+
+	/* Prepare key for decryption in AES_ECB and AES_CBC mode. */
+	if ((CRYP_ALGORITHM_DECRYPT == cryp_config->algodir) &&
+	    ((CRYP_ALGO_AES_ECB == cryp_config->algomode) ||
+	     (CRYP_ALGO_AES_CBC == cryp_config->algomode))) {
+		cr_for_kse = *control_register;
+		/*
+		 * This seems a bit odd, but it is indeed needed to set this to
+		 * encrypt even though it is a decryption that we are doing. It
+		 * also mentioned in the design spec that you need to do this.
+		 * After the keyprepartion for decrypting is done you should set
+		 * algodir back to decryption, which is done outside this if
+		 * statement.
+		 *
+		 * According to design specification we should set mode ECB
+		 * during key preparation even though we might be running CBC
+		 * when enter this function.
+		 *
+		 * Writing to KSE_ENABLED will drop CRYPEN when key preparation
+		 * is done. Therefore we need to set CRYPEN again outside this
+		 * if statement when running decryption.
+		 */
+		cr_for_kse |= ((CRYP_ALGORITHM_ENCRYPT << CRYP_CR_ALGODIR_POS) |
+			       (CRYP_ALGO_AES_ECB << CRYP_CR_ALGOMODE_POS) |
+			       (CRYP_CRYPEN_ENABLE << CRYP_CR_CRYPEN_POS) |
+			       (KSE_ENABLED << CRYP_CR_KSE_POS));
+
+		writel_relaxed(cr_for_kse, &device_data->base->cr);
+		cryp_wait_until_done(device_data);
+	}
+
+	*control_register |=
+		((cryp_config->algomode << CRYP_CR_ALGOMODE_POS) |
+		 (cryp_config->algodir << CRYP_CR_ALGODIR_POS));
+
+	return 0;
+}
+
+/**
+ * cryp_configure_protection - set the protection bits in the CRYP logic.
+ * @device_data: Pointer to the device data struct for base address.
+ * @p_protect_config:	Pointer to the protection mode and
+ *			secure mode configuration
+ */
+int cryp_configure_protection(struct cryp_device_data *device_data,
+			      struct cryp_protection_config *p_protect_config)
+{
+	if (NULL == p_protect_config)
+		return -EINVAL;
+
+	CRYP_WRITE_BIT(&device_data->base->cr,
+		       (u32) p_protect_config->secure_access,
+		       CRYP_CR_SECURE_MASK);
+	CRYP_PUT_BITS(&device_data->base->cr,
+		      p_protect_config->privilege_access,
+		      CRYP_CR_PRLG_POS,
+		      CRYP_CR_PRLG_MASK);
+
+	return 0;
+}
+
+/**
+ * cryp_is_logic_busy - returns the busy status of the CRYP logic
+ * @device_data: Pointer to the device data struct for base address.
+ */
+int cryp_is_logic_busy(struct cryp_device_data *device_data)
+{
+	return CRYP_TEST_BITS(&device_data->base->sr,
+			      CRYP_SR_BUSY_MASK);
+}
+
+/**
+ * cryp_configure_for_dma - configures the CRYP IP for DMA operation
+ * @device_data: Pointer to the device data struct for base address.
+ * @dma_req: Specifies the DMA request type value.
+ */
+void cryp_configure_for_dma(struct cryp_device_data *device_data,
+			    enum cryp_dma_req_type dma_req)
+{
+	CRYP_SET_BITS(&device_data->base->dmacr,
+		      (u32) dma_req);
+}
+
+/**
+ * cryp_configure_key_values - configures the key values for CRYP operations
+ * @device_data: Pointer to the device data struct for base address.
+ * @key_reg_index: Key value index register
+ * @key_value: The key value struct
+ */
+int cryp_configure_key_values(struct cryp_device_data *device_data,
+			      enum cryp_key_reg_index key_reg_index,
+			      struct cryp_key_value key_value)
+{
+	while (cryp_is_logic_busy(device_data))
+		cpu_relax();
+
+	switch (key_reg_index) {
+	case CRYP_KEY_REG_1:
+		writel_relaxed(key_value.key_value_left,
+		       &device_data->base->key_1_l);
+		writel_relaxed(key_value.key_value_right,
+		       &device_data->base->key_1_r);
+		break;
+	case CRYP_KEY_REG_2:
+		writel_relaxed(key_value.key_value_left,
+		       &device_data->base->key_2_l);
+		writel_relaxed(key_value.key_value_right,
+		       &device_data->base->key_2_r);
+		break;
+	case CRYP_KEY_REG_3:
+		writel_relaxed(key_value.key_value_left,
+		       &device_data->base->key_3_l);
+		writel_relaxed(key_value.key_value_right,
+		       &device_data->base->key_3_r);
+		break;
+	case CRYP_KEY_REG_4:
+		writel_relaxed(key_value.key_value_left,
+		       &device_data->base->key_4_l);
+		writel_relaxed(key_value.key_value_right,
+		       &device_data->base->key_4_r);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+
+}
+
+/**
+ * cryp_configure_init_vector - configures the initialization vector register
+ * @device_data: Pointer to the device data struct for base address.
+ * @init_vector_index: Specifies the index of the init vector.
+ * @init_vector_value: Specifies the value for the init vector.
+ */
+int cryp_configure_init_vector(struct cryp_device_data *device_data,
+			       enum cryp_init_vector_index
+			       init_vector_index,
+			       struct cryp_init_vector_value
+			       init_vector_value)
+{
+	while (cryp_is_logic_busy(device_data))
+		cpu_relax();
+
+	switch (init_vector_index) {
+	case CRYP_INIT_VECTOR_INDEX_0:
+		writel_relaxed(init_vector_value.init_value_left,
+		       &device_data->base->init_vect_0_l);
+		writel_relaxed(init_vector_value.init_value_right,
+		       &device_data->base->init_vect_0_r);
+		break;
+	case CRYP_INIT_VECTOR_INDEX_1:
+		writel_relaxed(init_vector_value.init_value_left,
+		       &device_data->base->init_vect_1_l);
+		writel_relaxed(init_vector_value.init_value_right,
+		       &device_data->base->init_vect_1_r);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * cryp_save_device_context -	Store hardware registers and
+ *				other device context parameter
+ * @device_data: Pointer to the device data struct for base address.
+ * @ctx: Crypto device context
+ */
+void cryp_save_device_context(struct cryp_device_data *device_data,
+			      struct cryp_device_context *ctx,
+			      int cryp_mode)
+{
+	enum cryp_algo_mode algomode;
+	struct cryp_register *src_reg = device_data->base;
+	struct cryp_config *config =
+		(struct cryp_config *)device_data->current_ctx;
+
+	/*
+	 * Always start by disable the hardware and wait for it to finish the
+	 * ongoing calculations before trying to reprogram it.
+	 */
+	cryp_activity(device_data, CRYP_CRYPEN_DISABLE);
+	cryp_wait_until_done(device_data);
+
+	if (cryp_mode == CRYP_MODE_DMA)
+		cryp_configure_for_dma(device_data, CRYP_DMA_DISABLE_BOTH);
+
+	if (CRYP_TEST_BITS(&src_reg->sr, CRYP_SR_IFEM_MASK) == 0)
+		ctx->din = readl_relaxed(&src_reg->din);
+
+	ctx->cr = readl_relaxed(&src_reg->cr) & CRYP_CR_CONTEXT_SAVE_MASK;
+
+	switch (config->keysize) {
+	case CRYP_KEY_SIZE_256:
+		ctx->key_4_l = readl_relaxed(&src_reg->key_4_l);
+		ctx->key_4_r = readl_relaxed(&src_reg->key_4_r);
+
+	case CRYP_KEY_SIZE_192:
+		ctx->key_3_l = readl_relaxed(&src_reg->key_3_l);
+		ctx->key_3_r = readl_relaxed(&src_reg->key_3_r);
+
+	case CRYP_KEY_SIZE_128:
+		ctx->key_2_l = readl_relaxed(&src_reg->key_2_l);
+		ctx->key_2_r = readl_relaxed(&src_reg->key_2_r);
+
+	default:
+		ctx->key_1_l = readl_relaxed(&src_reg->key_1_l);
+		ctx->key_1_r = readl_relaxed(&src_reg->key_1_r);
+	}
+
+	/* Save IV for CBC mode for both AES and DES. */
+	algomode = ((ctx->cr & CRYP_CR_ALGOMODE_MASK) >> CRYP_CR_ALGOMODE_POS);
+	if (algomode == CRYP_ALGO_TDES_CBC ||
+	    algomode == CRYP_ALGO_DES_CBC ||
+	    algomode == CRYP_ALGO_AES_CBC) {
+		ctx->init_vect_0_l = readl_relaxed(&src_reg->init_vect_0_l);
+		ctx->init_vect_0_r = readl_relaxed(&src_reg->init_vect_0_r);
+		ctx->init_vect_1_l = readl_relaxed(&src_reg->init_vect_1_l);
+		ctx->init_vect_1_r = readl_relaxed(&src_reg->init_vect_1_r);
+	}
+}
+
+/**
+ * cryp_restore_device_context -	Restore hardware registers and
+ *					other device context parameter
+ * @device_data: Pointer to the device data struct for base address.
+ * @ctx: Crypto device context
+ */
+void cryp_restore_device_context(struct cryp_device_data *device_data,
+				 struct cryp_device_context *ctx)
+{
+	struct cryp_register *reg = device_data->base;
+	struct cryp_config *config =
+		(struct cryp_config *)device_data->current_ctx;
+
+	/*
+	 * Fall through for all items in switch statement. DES is captured in
+	 * the default.
+	 */
+	switch (config->keysize) {
+	case CRYP_KEY_SIZE_256:
+		writel_relaxed(ctx->key_4_l, &reg->key_4_l);
+		writel_relaxed(ctx->key_4_r, &reg->key_4_r);
+
+	case CRYP_KEY_SIZE_192:
+		writel_relaxed(ctx->key_3_l, &reg->key_3_l);
+		writel_relaxed(ctx->key_3_r, &reg->key_3_r);
+
+	case CRYP_KEY_SIZE_128:
+		writel_relaxed(ctx->key_2_l, &reg->key_2_l);
+		writel_relaxed(ctx->key_2_r, &reg->key_2_r);
+
+	default:
+		writel_relaxed(ctx->key_1_l, &reg->key_1_l);
+		writel_relaxed(ctx->key_1_r, &reg->key_1_r);
+	}
+
+	/* Restore IV for CBC mode for AES and DES. */
+	if (config->algomode == CRYP_ALGO_TDES_CBC ||
+	    config->algomode == CRYP_ALGO_DES_CBC ||
+	    config->algomode == CRYP_ALGO_AES_CBC) {
+		writel_relaxed(ctx->init_vect_0_l, &reg->init_vect_0_l);
+		writel_relaxed(ctx->init_vect_0_r, &reg->init_vect_0_r);
+		writel_relaxed(ctx->init_vect_1_l, &reg->init_vect_1_l);
+		writel_relaxed(ctx->init_vect_1_r, &reg->init_vect_1_r);
+	}
+}
+
+/**
+ * cryp_write_indata - This routine writes 32 bit data into the data input
+ *		       register of the cryptography IP.
+ * @device_data: Pointer to the device data struct for base address.
+ * @write_data: Data word to write
+ */
+int cryp_write_indata(struct cryp_device_data *device_data, u32 write_data)
+{
+	writel_relaxed(write_data, &device_data->base->din);
+
+	return 0;
+}
+
+/**
+ * cryp_read_outdata - This routine reads the data from the data output
+ *		       register of the CRYP logic
+ * @device_data: Pointer to the device data struct for base address.
+ * @read_data: Read the data from the output FIFO.
+ */
+int cryp_read_outdata(struct cryp_device_data *device_data, u32 *read_data)
+{
+	*read_data = readl_relaxed(&device_data->base->dout);
+
+	return 0;
+}
diff --git a/drivers/crypto/ux500/cryp/cryp.h b/drivers/crypto/ux500/cryp/cryp.h
new file mode 100644
index 00000000000..df2e25d4671
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp.h
@@ -0,0 +1,308 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef _CRYP_H_
+#define _CRYP_H_
+
+#include <linux/completion.h>
+#include <linux/dmaengine.h>
+#include <linux/klist.h>
+#include <linux/mutex.h>
+
+#define DEV_DBG_NAME "crypX crypX:"
+
+/* CRYP enable/disable */
+enum cryp_crypen {
+	CRYP_CRYPEN_DISABLE = 0,
+	CRYP_CRYPEN_ENABLE = 1
+};
+
+/* CRYP Start Computation enable/disable */
+enum cryp_start {
+	CRYP_START_DISABLE = 0,
+	CRYP_START_ENABLE = 1
+};
+
+/* CRYP Init Signal enable/disable */
+enum cryp_init {
+	CRYP_INIT_DISABLE = 0,
+	CRYP_INIT_ENABLE = 1
+};
+
+/* Cryp State enable/disable */
+enum cryp_state {
+	CRYP_STATE_DISABLE = 0,
+	CRYP_STATE_ENABLE = 1
+};
+
+/* Key preparation bit enable */
+enum cryp_key_prep {
+	KSE_DISABLED = 0,
+	KSE_ENABLED = 1
+};
+
+/* Key size for AES */
+#define	CRYP_KEY_SIZE_128 (0)
+#define	CRYP_KEY_SIZE_192 (1)
+#define	CRYP_KEY_SIZE_256 (2)
+
+/* AES modes */
+enum cryp_algo_mode {
+	CRYP_ALGO_TDES_ECB,
+	CRYP_ALGO_TDES_CBC,
+	CRYP_ALGO_DES_ECB,
+	CRYP_ALGO_DES_CBC,
+	CRYP_ALGO_AES_ECB,
+	CRYP_ALGO_AES_CBC,
+	CRYP_ALGO_AES_CTR,
+	CRYP_ALGO_AES_XTS
+};
+
+/* Cryp Encryption or Decryption */
+enum cryp_algorithm_dir {
+	CRYP_ALGORITHM_ENCRYPT,
+	CRYP_ALGORITHM_DECRYPT
+};
+
+/* Hardware access method */
+enum cryp_mode {
+	CRYP_MODE_POLLING,
+	CRYP_MODE_INTERRUPT,
+	CRYP_MODE_DMA
+};
+
+/**
+ * struct cryp_config -
+ * @keysize: Key size for AES
+ * @algomode: AES modes
+ * @algodir: Cryp Encryption or Decryption
+ *
+ * CRYP configuration structure to be passed to set configuration
+ */
+struct cryp_config {
+	int keysize;
+	enum cryp_algo_mode algomode;
+	enum cryp_algorithm_dir algodir;
+};
+
+/**
+ * struct cryp_protection_config -
+ * @privilege_access: Privileged cryp state enable/disable
+ * @secure_access: Secure cryp state enable/disable
+ *
+ * Protection configuration structure for setting privilage access
+ */
+struct cryp_protection_config {
+	enum cryp_state privilege_access;
+	enum cryp_state secure_access;
+};
+
+/* Cryp status */
+enum cryp_status_id {
+	CRYP_STATUS_BUSY = 0x10,
+	CRYP_STATUS_OUTPUT_FIFO_FULL = 0x08,
+	CRYP_STATUS_OUTPUT_FIFO_NOT_EMPTY = 0x04,
+	CRYP_STATUS_INPUT_FIFO_NOT_FULL = 0x02,
+	CRYP_STATUS_INPUT_FIFO_EMPTY = 0x01
+};
+
+/* Cryp DMA interface */
+enum cryp_dma_req_type {
+	CRYP_DMA_DISABLE_BOTH,
+	CRYP_DMA_ENABLE_IN_DATA,
+	CRYP_DMA_ENABLE_OUT_DATA,
+	CRYP_DMA_ENABLE_BOTH_DIRECTIONS
+};
+
+enum cryp_dma_channel {
+	CRYP_DMA_RX = 0,
+	CRYP_DMA_TX
+};
+
+/* Key registers */
+enum cryp_key_reg_index {
+	CRYP_KEY_REG_1,
+	CRYP_KEY_REG_2,
+	CRYP_KEY_REG_3,
+	CRYP_KEY_REG_4
+};
+
+/* Key register left and right */
+struct cryp_key_value {
+	u32 key_value_left;
+	u32 key_value_right;
+};
+
+/* Cryp Initialization structure */
+enum cryp_init_vector_index {
+	CRYP_INIT_VECTOR_INDEX_0,
+	CRYP_INIT_VECTOR_INDEX_1
+};
+
+/* struct cryp_init_vector_value -
+ * @init_value_left
+ * @init_value_right
+ * */
+struct cryp_init_vector_value {
+	u32 init_value_left;
+	u32 init_value_right;
+};
+
+/**
+ * struct cryp_device_context - structure for a cryp context.
+ * @cr: control register
+ * @dmacr: DMA control register
+ * @imsc: Interrupt mask set/clear register
+ * @key_1_l: Key 1l register
+ * @key_1_r: Key 1r register
+ * @key_2_l: Key 2l register
+ * @key_2_r: Key 2r register
+ * @key_3_l: Key 3l register
+ * @key_3_r: Key 3r register
+ * @key_4_l: Key 4l register
+ * @key_4_r: Key 4r register
+ * @init_vect_0_l: Initialization vector 0l register
+ * @init_vect_0_r: Initialization vector 0r register
+ * @init_vect_1_l: Initialization vector 1l register
+ * @init_vect_1_r: Initialization vector 0r register
+ * @din: Data in register
+ * @dout: Data out register
+ *
+ * CRYP power management specifc structure.
+ */
+struct cryp_device_context {
+	u32 cr;
+	u32 dmacr;
+	u32 imsc;
+
+	u32 key_1_l;
+	u32 key_1_r;
+	u32 key_2_l;
+	u32 key_2_r;
+	u32 key_3_l;
+	u32 key_3_r;
+	u32 key_4_l;
+	u32 key_4_r;
+
+	u32 init_vect_0_l;
+	u32 init_vect_0_r;
+	u32 init_vect_1_l;
+	u32 init_vect_1_r;
+
+	u32 din;
+	u32 dout;
+};
+
+struct cryp_dma {
+	dma_cap_mask_t mask;
+	struct completion cryp_dma_complete;
+	struct dma_chan *chan_cryp2mem;
+	struct dma_chan *chan_mem2cryp;
+	struct stedma40_chan_cfg *cfg_cryp2mem;
+	struct stedma40_chan_cfg *cfg_mem2cryp;
+	int sg_src_len;
+	int sg_dst_len;
+	struct scatterlist *sg_src;
+	struct scatterlist *sg_dst;
+	int nents_src;
+	int nents_dst;
+};
+
+/**
+ * struct cryp_device_data - structure for a cryp device.
+ * @base: Pointer to the hardware base address.
+ * @dev: Pointer to the devices dev structure.
+ * @clk: Pointer to the device's clock control.
+ * @pwr_regulator: Pointer to the device's power control.
+ * @power_status: Current status of the power.
+ * @ctx_lock: Lock for current_ctx.
+ * @current_ctx: Pointer to the currently allocated context.
+ * @list_node: For inclusion into a klist.
+ * @dma: The dma structure holding channel configuration.
+ * @power_state: TRUE = power state on, FALSE = power state off.
+ * @power_state_spinlock: Spinlock for power_state.
+ * @restore_dev_ctx: TRUE = saved ctx, FALSE = no saved ctx.
+ */
+struct cryp_device_data {
+	struct cryp_register __iomem *base;
+	struct device *dev;
+	struct clk *clk;
+	struct ux500_regulator *pwr_regulator;
+	int power_status;
+	struct spinlock ctx_lock;
+	struct cryp_ctx *current_ctx;
+	struct klist_node list_node;
+	struct cryp_dma dma;
+	bool power_state;
+	struct spinlock power_state_spinlock;
+	bool restore_dev_ctx;
+};
+
+void cryp_wait_until_done(struct cryp_device_data *device_data);
+
+/* Initialization functions */
+
+int cryp_check(struct cryp_device_data *device_data);
+
+void cryp_activity(struct cryp_device_data *device_data,
+		   enum cryp_crypen cryp_crypen);
+
+void cryp_flush_inoutfifo(struct cryp_device_data *device_data);
+
+int cryp_set_configuration(struct cryp_device_data *device_data,
+			   struct cryp_config *cryp_config,
+			   u32 *control_register);
+
+void cryp_configure_for_dma(struct cryp_device_data *device_data,
+			    enum cryp_dma_req_type dma_req);
+
+int cryp_configure_key_values(struct cryp_device_data *device_data,
+			      enum cryp_key_reg_index key_reg_index,
+			      struct cryp_key_value key_value);
+
+int cryp_configure_init_vector(struct cryp_device_data *device_data,
+			       enum cryp_init_vector_index
+			       init_vector_index,
+			       struct cryp_init_vector_value
+			       init_vector_value);
+
+int cryp_configure_protection(struct cryp_device_data *device_data,
+			      struct cryp_protection_config *p_protect_config);
+
+/* Power management funtions */
+void cryp_save_device_context(struct cryp_device_data *device_data,
+			      struct cryp_device_context *ctx,
+			      int cryp_mode);
+
+void cryp_restore_device_context(struct cryp_device_data *device_data,
+				 struct cryp_device_context *ctx);
+
+/* Data transfer and status bits. */
+int cryp_is_logic_busy(struct cryp_device_data *device_data);
+
+int cryp_get_status(struct cryp_device_data *device_data);
+
+/**
+ * cryp_write_indata - This routine writes 32 bit data into the data input
+ *		       register of the cryptography IP.
+ * @device_data: Pointer to the device data struct for base address.
+ * @write_data: Data to write.
+ */
+int cryp_write_indata(struct cryp_device_data *device_data, u32 write_data);
+
+/**
+ * cryp_read_outdata - This routine reads the data from the data output
+ *		       register of the CRYP logic
+ * @device_data: Pointer to the device data struct for base address.
+ * @read_data: Read the data from the output FIFO.
+ */
+int cryp_read_outdata(struct cryp_device_data *device_data, u32 *read_data);
+
+#endif /* _CRYP_H_ */
diff --git a/drivers/crypto/ux500/cryp/cryp_core.c b/drivers/crypto/ux500/cryp/cryp_core.c
new file mode 100644
index 00000000000..f0aed67f29a
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_core.c
@@ -0,0 +1,2314 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/crypto.h>
+#include <linux/dmaengine.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irqreturn.h>
+#include <linux/klist.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/dbx500-prcmu.h>
+#include <linux/semaphore.h>
+
+#include <crypto/aes.h>
+#include <crypto/algapi.h>
+#include <crypto/ctr.h>
+#include <crypto/des.h>
+#include <crypto/scatterwalk.h>
+
+#include <plat/ste_dma40.h>
+
+#include <mach/crypto-ux500.h>
+#include <mach/hardware.h>
+#include <mach/ste-dma40-db8500.h>
+
+#include "cryp_p.h"
+#include "cryp.h"
+
+#define CRYP_MAX_KEY_SIZE	32
+#define BYTES_PER_WORD		4
+
+static int cryp_mode;
+static atomic_t session_id;
+
+static struct stedma40_chan_cfg *mem_to_engine;
+static struct stedma40_chan_cfg *engine_to_mem;
+
+/**
+ * struct cryp_driver_data - data specific to the driver.
+ *
+ * @device_list: A list of registered devices to choose from.
+ * @device_allocation: A semaphore initialized with number of devices.
+ */
+struct cryp_driver_data {
+	struct klist device_list;
+	struct semaphore device_allocation;
+};
+
+/**
+ * struct cryp_ctx - Crypto context
+ * @config: Crypto mode.
+ * @key[CRYP_MAX_KEY_SIZE]: Key.
+ * @keylen: Length of key.
+ * @iv: Pointer to initialization vector.
+ * @indata: Pointer to indata.
+ * @outdata: Pointer to outdata.
+ * @datalen: Length of indata.
+ * @outlen: Length of outdata.
+ * @blocksize: Size of blocks.
+ * @updated: Updated flag.
+ * @dev_ctx: Device dependent context.
+ * @device: Pointer to the device.
+ */
+struct cryp_ctx {
+	struct cryp_config config;
+	u8 key[CRYP_MAX_KEY_SIZE];
+	u32 keylen;
+	u8 *iv;
+	const u8 *indata;
+	u8 *outdata;
+	u32 datalen;
+	u32 outlen;
+	u32 blocksize;
+	u8 updated;
+	struct cryp_device_context dev_ctx;
+	struct cryp_device_data *device;
+	u32 session_id;
+};
+
+static struct cryp_driver_data driver_data;
+
+/**
+ * uint8p_to_uint32_be - 4*uint8 to uint32 big endian
+ * @in: Data to convert.
+ */
+static inline u32 uint8p_to_uint32_be(u8 *in)
+{
+	return  (u32)in[0]<<24 |
+		((u32)in[1]<<16) |
+		((u32)in[2]<<8) |
+		((u32)in[3]);
+}
+
+/**
+ * swap_bits_in_byte - mirror the bits in a byte
+ * @b: the byte to be mirrored
+ *
+ * The bits are swapped the following way:
+ *  Byte b include bits 0-7, nibble 1 (n1) include bits 0-3 and
+ *  nibble 2 (n2) bits 4-7.
+ *
+ *  Nibble 1 (n1):
+ *  (The "old" (moved) bit is replaced with a zero)
+ *  1. Move bit 6 and 7, 4 positions to the left.
+ *  2. Move bit 3 and 5, 2 positions to the left.
+ *  3. Move bit 1-4, 1 position to the left.
+ *
+ *  Nibble 2 (n2):
+ *  1. Move bit 0 and 1, 4 positions to the right.
+ *  2. Move bit 2 and 4, 2 positions to the right.
+ *  3. Move bit 3-6, 1 position to the right.
+ *
+ *  Combine the two nibbles to a complete and swapped byte.
+ */
+
+static inline u8 swap_bits_in_byte(u8 b)
+{
+#define R_SHIFT_4_MASK  (0xc0) /* Bits 6 and 7, right shift 4 */
+#define R_SHIFT_2_MASK  (0x28) /* (After right shift 4) Bits 3 and 5,
+				  right shift 2 */
+#define R_SHIFT_1_MASK  (0x1e) /* (After right shift 2) Bits 1-4,
+				  right shift 1 */
+#define L_SHIFT_4_MASK  (0x03) /* Bits 0 and 1, left shift 4 */
+#define L_SHIFT_2_MASK  (0x14) /* (After left shift 4) Bits 2 and 4,
+				  left shift 2 */
+#define L_SHIFT_1_MASK  (0x78) /* (After left shift 1) Bits 3-6,
+				  left shift 1 */
+
+	u8 n1;
+	u8 n2;
+
+	/* Swap most significant nibble */
+	/* Right shift 4, bits 6 and 7 */
+	n1 = ((b  & R_SHIFT_4_MASK) >> 4) | (b  & ~(R_SHIFT_4_MASK >> 4));
+	/* Right shift 2, bits 3 and 5 */
+	n1 = ((n1 & R_SHIFT_2_MASK) >> 2) | (n1 & ~(R_SHIFT_2_MASK >> 2));
+	/* Right shift 1, bits 1-4 */
+	n1 = (n1  & R_SHIFT_1_MASK) >> 1;
+
+	/* Swap least significant nibble */
+	/* Left shift 4, bits 0 and 1 */
+	n2 = ((b  & L_SHIFT_4_MASK) << 4) | (b  & ~(L_SHIFT_4_MASK << 4));
+	/* Left shift 2, bits 2 and 4 */
+	n2 = ((n2 & L_SHIFT_2_MASK) << 2) | (n2 & ~(L_SHIFT_2_MASK << 2));
+	/* Left shift 1, bits 3-6 */
+	n2 = (n2  & L_SHIFT_1_MASK) << 1;
+
+	return n1 | n2;
+}
+
+static inline void swap_words_in_key_and_bits_in_byte(const u8 *in,
+						      u8 *out, u32 len)
+{
+	unsigned int i = 0;
+	int j;
+	int index = 0;
+
+	j = len - BYTES_PER_WORD;
+	while (j >= 0) {
+		for (i = 0; i < BYTES_PER_WORD; i++) {
+			index = len - j - BYTES_PER_WORD + i;
+			out[j + i] =
+				swap_bits_in_byte(in[index]);
+		}
+		j -= BYTES_PER_WORD;
+	}
+}
+
+static void add_session_id(struct cryp_ctx *ctx)
+{
+	/*
+	 * We never want 0 to be a valid value, since this is the default value
+	 * for the software context.
+	 */
+	if (unlikely(atomic_inc_and_test(&session_id)))
+		atomic_inc(&session_id);
+
+	ctx->session_id = atomic_read(&session_id);
+}
+
+static irqreturn_t cryp_interrupt_handler(int irq, void *param)
+{
+	struct cryp_ctx *ctx;
+	int i;
+	struct cryp_device_data *device_data;
+
+	if (param == NULL) {
+		BUG_ON(!param);
+		return IRQ_HANDLED;
+	}
+
+	/* The device is coming from the one found in hw_crypt_noxts. */
+	device_data = (struct cryp_device_data *)param;
+
+	ctx = device_data->current_ctx;
+
+	if (ctx == NULL) {
+		BUG_ON(!ctx);
+		return IRQ_HANDLED;
+	}
+
+	dev_dbg(ctx->device->dev, "[%s] (len: %d) %s, ", __func__, ctx->outlen,
+		cryp_pending_irq_src(device_data, CRYP_IRQ_SRC_OUTPUT_FIFO) ?
+		"out" : "in");
+
+	if (cryp_pending_irq_src(device_data,
+				 CRYP_IRQ_SRC_OUTPUT_FIFO)) {
+		if (ctx->outlen / ctx->blocksize > 0) {
+			for (i = 0; i < ctx->blocksize / 4; i++) {
+				cryp_read_outdata(device_data,
+						 (u32 *)ctx->outdata);
+				ctx->outdata += 4;
+				ctx->outlen -= 4;
+			}
+
+			if (ctx->outlen == 0) {
+				cryp_disable_irq_src(device_data,
+						     CRYP_IRQ_SRC_OUTPUT_FIFO);
+			}
+		}
+	} else if (cryp_pending_irq_src(device_data,
+					CRYP_IRQ_SRC_INPUT_FIFO)) {
+		if (ctx->datalen / ctx->blocksize > 0) {
+			for (i = 0 ; i < ctx->blocksize / 4; i++) {
+				cryp_write_indata(device_data,
+						 *((u32 *)ctx->indata));
+				ctx->indata += 4;
+				ctx->datalen -= 4;
+			}
+
+			if (ctx->datalen == 0)
+				cryp_disable_irq_src(device_data,
+						   CRYP_IRQ_SRC_INPUT_FIFO);
+
+			if (ctx->config.algomode == CRYP_ALGO_AES_XTS) {
+				CRYP_PUT_BITS(&device_data->base->cr,
+					      CRYP_START_ENABLE,
+					      CRYP_CR_START_POS,
+					      CRYP_CR_START_MASK);
+
+				cryp_wait_until_done(device_data);
+			}
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int mode_is_aes(enum cryp_algo_mode mode)
+{
+	return	(CRYP_ALGO_AES_ECB == mode) ||
+		(CRYP_ALGO_AES_CBC == mode) ||
+		(CRYP_ALGO_AES_CTR == mode) ||
+		(CRYP_ALGO_AES_XTS == mode);
+}
+
+static int cfg_iv(struct cryp_device_data *device_data, u32 left, u32 right,
+		  enum cryp_init_vector_index index)
+{
+	struct cryp_init_vector_value vector_value;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	vector_value.init_value_left = left;
+	vector_value.init_value_right = right;
+
+	return cryp_configure_init_vector(device_data,
+					  index,
+					  vector_value);
+}
+
+static int cfg_ivs(struct cryp_device_data *device_data, struct cryp_ctx *ctx)
+{
+	int i;
+	int status = 0;
+	int num_of_regs = ctx->blocksize / 8;
+	u32 iv[AES_BLOCK_SIZE / 4];
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	/*
+	 * Since we loop on num_of_regs we need to have a check in case
+	 * someone provides an incorrect blocksize which would force calling
+	 * cfg_iv with i greater than 2 which is an error.
+	 */
+	if (num_of_regs > 2) {
+		dev_err(device_data->dev, "[%s] Incorrect blocksize %d",
+			__func__, ctx->blocksize);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < ctx->blocksize / 4; i++)
+		iv[i] = uint8p_to_uint32_be(ctx->iv + i*4);
+
+	for (i = 0; i < num_of_regs; i++) {
+		status = cfg_iv(device_data, iv[i*2], iv[i*2+1],
+				(enum cryp_init_vector_index) i);
+		if (status != 0)
+			return status;
+	}
+	return status;
+}
+
+static int set_key(struct cryp_device_data *device_data,
+		   u32 left_key,
+		   u32 right_key,
+		   enum cryp_key_reg_index index)
+{
+	struct cryp_key_value key_value;
+	int cryp_error;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	key_value.key_value_left = left_key;
+	key_value.key_value_right = right_key;
+
+	cryp_error = cryp_configure_key_values(device_data,
+					       index,
+					       key_value);
+	if (cryp_error != 0)
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_configure_key_values() failed!", __func__);
+
+	return cryp_error;
+}
+
+static int cfg_keys(struct cryp_ctx *ctx)
+{
+	int i;
+	int num_of_regs = ctx->keylen / 8;
+	u32 swapped_key[CRYP_MAX_KEY_SIZE / 4];
+	int cryp_error = 0;
+
+	dev_dbg(ctx->device->dev, "[%s]", __func__);
+
+	if (mode_is_aes(ctx->config.algomode)) {
+		swap_words_in_key_and_bits_in_byte((u8 *)ctx->key,
+						   (u8 *)swapped_key,
+						   ctx->keylen);
+	} else {
+		for (i = 0; i < ctx->keylen / 4; i++)
+			swapped_key[i] = uint8p_to_uint32_be(ctx->key + i*4);
+	}
+
+	for (i = 0; i < num_of_regs; i++) {
+		cryp_error = set_key(ctx->device,
+				     *(((u32 *)swapped_key)+i*2),
+				     *(((u32 *)swapped_key)+i*2+1),
+				     (enum cryp_key_reg_index) i);
+
+		if (cryp_error != 0) {
+			dev_err(ctx->device->dev, "[%s]: set_key() failed!",
+					__func__);
+			return cryp_error;
+		}
+	}
+	return cryp_error;
+}
+
+static int cryp_setup_context(struct cryp_ctx *ctx,
+			      struct cryp_device_data *device_data)
+{
+	u32 control_register = CRYP_CR_DEFAULT;
+
+	switch (cryp_mode) {
+	case CRYP_MODE_INTERRUPT:
+		writel_relaxed(CRYP_IMSC_DEFAULT, &device_data->base->imsc);
+		break;
+
+	case CRYP_MODE_DMA:
+		writel_relaxed(CRYP_DMACR_DEFAULT, &device_data->base->dmacr);
+		break;
+
+	default:
+		break;
+	}
+
+	if (ctx->updated == 0) {
+		cryp_flush_inoutfifo(device_data);
+		if (cfg_keys(ctx) != 0) {
+			dev_err(ctx->device->dev, "[%s]: cfg_keys failed!",
+				__func__);
+			return -EPERM;
+		}
+
+		if ((ctx->iv) &&
+		    (CRYP_ALGO_AES_ECB != ctx->config.algomode) &&
+		    (CRYP_ALGO_DES_ECB != ctx->config.algomode) &&
+		    (CRYP_ALGO_TDES_ECB != ctx->config.algomode)) {
+			if (cfg_ivs(device_data, ctx) != 0)
+				return -EPERM;
+		}
+
+		cryp_set_configuration(device_data, &ctx->config,
+				       &control_register);
+		add_session_id(ctx);
+	} else if (ctx->updated == 1 &&
+		   ctx->session_id != atomic_read(&session_id)) {
+		cryp_flush_inoutfifo(device_data);
+		cryp_restore_device_context(device_data, &ctx->dev_ctx);
+
+		add_session_id(ctx);
+		control_register = ctx->dev_ctx.cr;
+	} else
+		control_register = ctx->dev_ctx.cr;
+
+	writel(control_register |
+	       (CRYP_CRYPEN_ENABLE << CRYP_CR_CRYPEN_POS),
+	       &device_data->base->cr);
+
+	return 0;
+}
+
+static int cryp_get_device_data(struct cryp_ctx *ctx,
+				struct cryp_device_data **device_data)
+{
+	int ret;
+	struct klist_iter device_iterator;
+	struct klist_node *device_node;
+	struct cryp_device_data *local_device_data = NULL;
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	/* Wait until a device is available */
+	ret = down_interruptible(&driver_data.device_allocation);
+	if (ret)
+		return ret;  /* Interrupted */
+
+	/* Select a device */
+	klist_iter_init(&driver_data.device_list, &device_iterator);
+
+	device_node = klist_next(&device_iterator);
+	while (device_node) {
+		local_device_data = container_of(device_node,
+					   struct cryp_device_data, list_node);
+		spin_lock(&local_device_data->ctx_lock);
+		/* current_ctx allocates a device, NULL = unallocated */
+		if (local_device_data->current_ctx) {
+			device_node = klist_next(&device_iterator);
+		} else {
+			local_device_data->current_ctx = ctx;
+			ctx->device = local_device_data;
+			spin_unlock(&local_device_data->ctx_lock);
+			break;
+		}
+		spin_unlock(&local_device_data->ctx_lock);
+	}
+	klist_iter_exit(&device_iterator);
+
+	if (!device_node) {
+		/**
+		 * No free device found.
+		 * Since we allocated a device with down_interruptible, this
+		 * should not be able to happen.
+		 * Number of available devices, which are contained in
+		 * device_allocation, is therefore decremented by not doing
+		 * an up(device_allocation).
+		 */
+		return -EBUSY;
+	}
+
+	*device_data = local_device_data;
+
+	return 0;
+}
+
+static void cryp_dma_setup_channel(struct cryp_device_data *device_data,
+				   struct device *dev)
+{
+	dma_cap_zero(device_data->dma.mask);
+	dma_cap_set(DMA_SLAVE, device_data->dma.mask);
+
+	device_data->dma.cfg_mem2cryp = mem_to_engine;
+	device_data->dma.chan_mem2cryp =
+		dma_request_channel(device_data->dma.mask,
+				    stedma40_filter,
+				    device_data->dma.cfg_mem2cryp);
+
+	device_data->dma.cfg_cryp2mem = engine_to_mem;
+	device_data->dma.chan_cryp2mem =
+		dma_request_channel(device_data->dma.mask,
+				    stedma40_filter,
+				    device_data->dma.cfg_cryp2mem);
+
+	init_completion(&device_data->dma.cryp_dma_complete);
+}
+
+static void cryp_dma_out_callback(void *data)
+{
+	struct cryp_ctx *ctx = (struct cryp_ctx *) data;
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	complete(&ctx->device->dma.cryp_dma_complete);
+}
+
+static int cryp_set_dma_transfer(struct cryp_ctx *ctx,
+				 struct scatterlist *sg,
+				 int len,
+				 enum dma_data_direction direction)
+{
+	struct dma_async_tx_descriptor *desc;
+	struct dma_chan *channel = NULL;
+	dma_cookie_t cookie;
+
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	if (unlikely(!IS_ALIGNED((u32)sg, 4))) {
+		dev_err(ctx->device->dev, "[%s]: Data in sg list isn't "
+			"aligned! Addr: 0x%08x", __func__, (u32)sg);
+		return -EFAULT;
+	}
+
+	switch (direction) {
+	case DMA_TO_DEVICE:
+		channel = ctx->device->dma.chan_mem2cryp;
+		ctx->device->dma.sg_src = sg;
+		ctx->device->dma.sg_src_len = dma_map_sg(channel->device->dev,
+						 ctx->device->dma.sg_src,
+						 ctx->device->dma.nents_src,
+						 direction);
+
+		if (!ctx->device->dma.sg_src_len) {
+			dev_dbg(ctx->device->dev,
+				"[%s]: Could not map the sg list (TO_DEVICE)",
+				__func__);
+			return -EFAULT;
+		}
+
+		dev_dbg(ctx->device->dev, "[%s]: Setting up DMA for buffer "
+			"(TO_DEVICE)", __func__);
+
+		desc = channel->device->device_prep_slave_sg(channel,
+					     ctx->device->dma.sg_src,
+					     ctx->device->dma.sg_src_len,
+					     direction,
+					     DMA_CTRL_ACK);
+		break;
+
+	case DMA_FROM_DEVICE:
+		channel = ctx->device->dma.chan_cryp2mem;
+		ctx->device->dma.sg_dst = sg;
+		ctx->device->dma.sg_dst_len = dma_map_sg(channel->device->dev,
+						 ctx->device->dma.sg_dst,
+						 ctx->device->dma.nents_dst,
+						 direction);
+
+		if (!ctx->device->dma.sg_dst_len) {
+			dev_dbg(ctx->device->dev,
+				"[%s]: Could not map the sg list "
+				"(FROM_DEVICE)", __func__);
+			return -EFAULT;
+		}
+
+		dev_dbg(ctx->device->dev, "[%s]: Setting up DMA for buffer "
+			"(FROM_DEVICE)", __func__);
+
+		desc = channel->device->device_prep_slave_sg(channel,
+					     ctx->device->dma.sg_dst,
+					     ctx->device->dma.sg_dst_len,
+					     direction,
+					     DMA_CTRL_ACK |
+					     DMA_PREP_INTERRUPT);
+
+		desc->callback = cryp_dma_out_callback;
+		desc->callback_param = ctx;
+		break;
+
+	default:
+		dev_dbg(ctx->device->dev, "[%s]: Invalid DMA direction",
+			__func__);
+		return -EFAULT;
+	}
+
+	cookie = desc->tx_submit(desc);
+	dma_async_issue_pending(channel);
+
+	return 0;
+}
+
+static void cryp_dma_done(struct cryp_ctx *ctx)
+{
+	struct dma_chan *chan;
+
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	chan = ctx->device->dma.chan_mem2cryp;
+	chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+	dma_unmap_sg(chan->device->dev, ctx->device->dma.sg_src,
+		     ctx->device->dma.sg_src_len, DMA_TO_DEVICE);
+
+	chan = ctx->device->dma.chan_cryp2mem;
+	chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+	dma_unmap_sg(chan->device->dev, ctx->device->dma.sg_dst,
+		     ctx->device->dma.sg_dst_len, DMA_FROM_DEVICE);
+}
+
+static int cryp_dma_write(struct cryp_ctx *ctx, struct scatterlist *sg,
+			  int len)
+{
+	int error = cryp_set_dma_transfer(ctx, sg, len, DMA_TO_DEVICE);
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	if (error) {
+		dev_dbg(ctx->device->dev, "[%s]: cryp_set_dma_transfer() "
+			"failed", __func__);
+		return error;
+	}
+
+	return len;
+}
+
+static int cryp_dma_read(struct cryp_ctx *ctx, struct scatterlist *sg, int len)
+{
+	int error = cryp_set_dma_transfer(ctx, sg, len, DMA_FROM_DEVICE);
+	if (error) {
+		dev_dbg(ctx->device->dev, "[%s]: cryp_set_dma_transfer() "
+			"failed", __func__);
+		return error;
+	}
+
+	return len;
+}
+
+static void cryp_polling_mode(struct cryp_ctx *ctx,
+			      struct cryp_device_data *device_data)
+{
+	int len = ctx->blocksize / BYTES_PER_WORD;
+	int remaining_length = ctx->datalen;
+	u32 *indata = (u32 *)ctx->indata;
+	u32 *outdata = (u32 *)ctx->outdata;
+
+	while (remaining_length > 0) {
+		writesl(&device_data->base->din, indata, len);
+		indata += len;
+		remaining_length -= (len * BYTES_PER_WORD);
+		cryp_wait_until_done(device_data);
+
+		readsl(&device_data->base->dout, outdata, len);
+		outdata += len;
+		cryp_wait_until_done(device_data);
+	}
+}
+
+static int cryp_disable_power(struct device *dev,
+			      struct cryp_device_data *device_data,
+			      bool save_device_context)
+{
+	int ret = 0;
+
+	dev_dbg(dev, "[%s]", __func__);
+
+	spin_lock(&device_data->power_state_spinlock);
+	if (!device_data->power_state)
+		goto out;
+
+	spin_lock(&device_data->ctx_lock);
+	if (save_device_context && device_data->current_ctx) {
+		cryp_save_device_context(device_data,
+				&device_data->current_ctx->dev_ctx,
+				cryp_mode);
+		device_data->restore_dev_ctx = true;
+	}
+	spin_unlock(&device_data->ctx_lock);
+
+	clk_disable(device_data->clk);
+	ret = ux500_regulator_atomic_disable(device_data->pwr_regulator);
+	if (ret)
+		dev_err(dev, "[%s]: "
+				"regulator_disable() failed!",
+				__func__);
+
+	device_data->power_state = false;
+
+out:
+	spin_unlock(&device_data->power_state_spinlock);
+
+	return ret;
+}
+
+static int cryp_enable_power(
+		struct device *dev,
+		struct cryp_device_data *device_data,
+		bool restore_device_context)
+{
+	int ret = 0;
+
+	dev_dbg(dev, "[%s]", __func__);
+
+	spin_lock(&device_data->power_state_spinlock);
+	if (!device_data->power_state) {
+		ret = ux500_regulator_atomic_enable(device_data->pwr_regulator);
+		if (ret) {
+			dev_err(dev, "[%s]: regulator_enable() failed!",
+					__func__);
+			goto out;
+		}
+
+		ret = clk_enable(device_data->clk);
+		if (ret) {
+			dev_err(dev, "[%s]: clk_enable() failed!",
+					__func__);
+			ux500_regulator_atomic_disable(
+					       device_data->pwr_regulator);
+			goto out;
+		}
+		device_data->power_state = true;
+	}
+
+	if (device_data->restore_dev_ctx) {
+		spin_lock(&device_data->ctx_lock);
+		if (restore_device_context && device_data->current_ctx) {
+			device_data->restore_dev_ctx = false;
+			cryp_restore_device_context(device_data,
+					&device_data->current_ctx->dev_ctx);
+		}
+		spin_unlock(&device_data->ctx_lock);
+	}
+out:
+	spin_unlock(&device_data->power_state_spinlock);
+
+	return ret;
+}
+
+static int hw_crypt_noxts(struct cryp_ctx *ctx,
+			  struct cryp_device_data *device_data)
+{
+	int ret = 0;
+
+	const u8 *indata = ctx->indata;
+	u8 *outdata = ctx->outdata;
+	u32 datalen = ctx->datalen;
+	u32 outlen = datalen;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->outlen = ctx->datalen;
+
+	if (unlikely(!IS_ALIGNED((u32)indata, 4))) {
+		pr_debug(DEV_DBG_NAME " [%s]: Data isn't aligned! Addr: "
+			 "0x%08x", __func__, (u32)indata);
+		return -EINVAL;
+	}
+
+	ret = cryp_setup_context(ctx, device_data);
+
+	if (ret)
+		goto out;
+
+	if (cryp_mode == CRYP_MODE_INTERRUPT) {
+		cryp_enable_irq_src(device_data, CRYP_IRQ_SRC_INPUT_FIFO |
+				    CRYP_IRQ_SRC_OUTPUT_FIFO);
+
+		/*
+		 * ctx->outlen is decremented in the cryp_interrupt_handler
+		 * function. We had to add cpu_relax() (barrier) to make sure
+		 * that gcc didn't optimze away this variable.
+		 */
+		while (ctx->outlen > 0)
+			cpu_relax();
+	} else if (cryp_mode == CRYP_MODE_POLLING ||
+		   cryp_mode == CRYP_MODE_DMA) {
+		/*
+		 * The reason for having DMA in this if case is that if we are
+		 * running cryp_mode = 2, then we separate DMA routines for
+		 * handling cipher/plaintext > blocksize, except when
+		 * running the normal CRYPTO_ALG_TYPE_CIPHER, then we still use
+		 * the polling mode. Overhead of doing DMA setup eats up the
+		 * benefits using it.
+		 */
+		cryp_polling_mode(ctx, device_data);
+	} else {
+		dev_err(ctx->device->dev, "[%s]: Invalid operation mode!",
+			__func__);
+		ret = -EPERM;
+		goto out;
+	}
+
+	cryp_save_device_context(device_data, &ctx->dev_ctx, cryp_mode);
+	ctx->updated = 1;
+
+out:
+	ctx->indata = indata;
+	ctx->outdata = outdata;
+	ctx->datalen = datalen;
+	ctx->outlen = outlen;
+
+	return ret;
+}
+
+static int get_nents(struct scatterlist *sg, int nbytes)
+{
+	int nents = 0;
+
+	while (nbytes > 0) {
+		nbytes -= sg->length;
+		sg = scatterwalk_sg_next(sg);
+		nents++;
+	}
+
+	return nents;
+}
+
+static int ablk_dma_crypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	struct cryp_device_data *device_data;
+
+	int bytes_written = 0;
+	int bytes_read = 0;
+	int ret;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->datalen = areq->nbytes;
+	ctx->outlen = areq->nbytes;
+
+	ret = cryp_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	ret = cryp_enable_power(device_data->dev, device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_enable_power() failed!", __func__);
+		goto out;
+	}
+
+	ret = cryp_setup_context(ctx, device_data);
+	if (ret)
+		goto out_power;
+
+	/* We have the device now, so store the nents in the dma struct. */
+	ctx->device->dma.nents_src = get_nents(areq->src, ctx->datalen);
+	ctx->device->dma.nents_dst = get_nents(areq->dst, ctx->outlen);
+
+	/* Enable DMA in- and output. */
+	cryp_configure_for_dma(device_data, CRYP_DMA_ENABLE_BOTH_DIRECTIONS);
+
+	bytes_written = cryp_dma_write(ctx, areq->src, ctx->datalen);
+	bytes_read = cryp_dma_read(ctx, areq->dst, bytes_written);
+
+	wait_for_completion(&ctx->device->dma.cryp_dma_complete);
+	cryp_dma_done(ctx);
+
+	cryp_save_device_context(device_data, &ctx->dev_ctx, cryp_mode);
+	ctx->updated = 1;
+
+out_power:
+	if (cryp_disable_power(device_data->dev, device_data, false))
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_disable_power() failed!", __func__);
+
+out:
+	spin_lock(&device_data->ctx_lock);
+	device_data->current_ctx = NULL;
+	ctx->device = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	/*
+	 * The down_interruptible part for this semaphore is called in
+	 * cryp_get_device_data.
+	 */
+	up(&driver_data.device_allocation);
+
+	if (unlikely(bytes_written != bytes_read))
+		return -EPERM;
+
+	return 0;
+}
+
+static int ablk_crypt(struct ablkcipher_request *areq)
+{
+	struct ablkcipher_walk walk;
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	struct cryp_device_data *device_data;
+	unsigned long src_paddr;
+	unsigned long dst_paddr;
+	int ret;
+	int nbytes;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ret = cryp_get_device_data(ctx, &device_data);
+	if (ret)
+		goto out;
+
+	ret = cryp_enable_power(device_data->dev, device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_enable_power() failed!", __func__);
+		goto out_power;
+	}
+
+	ablkcipher_walk_init(&walk, areq->dst, areq->src, areq->nbytes);
+	ret = ablkcipher_walk_phys(areq, &walk);
+
+	if (ret) {
+		pr_err(DEV_DBG_NAME "[%s]: ablkcipher_walk_phys() failed!",
+			__func__);
+		goto out_power;
+	}
+
+	while ((nbytes = walk.nbytes) > 0) {
+		ctx->iv = walk.iv;
+		src_paddr = (page_to_phys(walk.src.page) + walk.src.offset);
+		ctx->indata = phys_to_virt(src_paddr);
+
+		dst_paddr = (page_to_phys(walk.dst.page) + walk.dst.offset);
+		ctx->outdata = phys_to_virt(dst_paddr);
+
+		ctx->datalen = nbytes - (nbytes % ctx->blocksize);
+
+		ret = hw_crypt_noxts(ctx, device_data);
+		if (ret)
+			goto out_power;
+
+		nbytes -= ctx->datalen;
+		ret = ablkcipher_walk_done(areq, &walk, nbytes);
+		if (ret)
+			goto out_power;
+	}
+	ablkcipher_walk_complete(&walk);
+
+out_power:
+	if (cryp_disable_power(device_data->dev, device_data, false))
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_disable_power() failed!", __func__);
+out:
+	/* Release the device */
+	spin_lock(&device_data->ctx_lock);
+	device_data->current_ctx = NULL;
+	ctx->device = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	/*
+	 * The down_interruptible part for this semaphore is called in
+	 * cryp_get_device_data.
+	 */
+	up(&driver_data.device_allocation);
+
+	return ret;
+}
+
+static int aes_ablkcipher_setkey(struct crypto_ablkcipher *cipher,
+				 const u8 *key, unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	switch (keylen) {
+	case AES_KEYSIZE_128:
+		ctx->config.keysize = CRYP_KEY_SIZE_128;
+		break;
+
+	case AES_KEYSIZE_192:
+		ctx->config.keysize = CRYP_KEY_SIZE_192;
+		break;
+
+	case AES_KEYSIZE_256:
+		ctx->config.keysize = CRYP_KEY_SIZE_256;
+		break;
+
+	default:
+		pr_err(DEV_DBG_NAME "[%s]: Unknown keylen!", __func__);
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+
+	return 0;
+}
+
+static int aes_setkey(struct crypto_tfm *tfm, const u8 *key,
+		      unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+	u32 *flags = &tfm->crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	if (unlikely(!IS_ALIGNED((u32)key, 4))) {
+		dev_err(ctx->device->dev, "[%s]: key isn't aligned! Addr: "
+			"0x%08x", __func__, (u32)key);
+		return -EFAULT;
+	}
+
+	/* For CTR mode */
+	if (keylen != AES_KEYSIZE_128 &&
+	    keylen != AES_KEYSIZE_192 &&
+	    keylen != AES_KEYSIZE_256) {
+
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s] invalid keylen", __func__);
+		return -EINVAL;
+	}
+
+	if (keylen == AES_KEYSIZE_128)
+		ctx->config.keysize = CRYP_KEY_SIZE_128;
+	else if (keylen == AES_KEYSIZE_192)
+		ctx->config.keysize = CRYP_KEY_SIZE_192;
+	else if (keylen == AES_KEYSIZE_256)
+		ctx->config.keysize = CRYP_KEY_SIZE_256;
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int des_ablkcipher_setkey(struct crypto_ablkcipher *cipher,
+				 const u8 *key, unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+	u32 tmp[DES_EXPKEY_WORDS];
+	int ret;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+	if (keylen != DES_KEY_SIZE) {
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_RES_BAD_KEY_LEN",
+				__func__);
+		return -EINVAL;
+	}
+
+	ret = des_ekey(tmp, key);
+	if (unlikely(ret == 0) && (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_REQ_WEAK_KEY",
+				__func__);
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int des_setkey(struct crypto_tfm *tfm, const u8 *key,
+		      unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+	u32 *flags = &tfm->crt_flags;
+	int ret;
+	u32 tmp[DES_EXPKEY_WORDS];
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	if (keylen != DES_KEY_SIZE) {
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_RES_BAD_KEY_LEN",
+			__func__);
+		return -EINVAL;
+	}
+
+	ret = des_ekey(tmp, key);
+	if (unlikely(ret == 0) && (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_REQ_WEAK_KEY",
+			__func__);
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int des3_ablkcipher_setkey(struct crypto_ablkcipher *cipher,
+				  const u8 *key, unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+	const u32 *K = (const u32 *)key;
+	u32 tmp[DES3_EDE_EXPKEY_WORDS];
+	int i, ret;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+	if (keylen != DES3_EDE_KEY_SIZE) {
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_RES_BAD_KEY_LEN",
+				__func__);
+		return -EINVAL;
+	}
+
+	/* Checking key interdependency for weak key detection. */
+	if (unlikely(!((K[0] ^ K[2]) | (K[1] ^ K[3])) ||
+				!((K[2] ^ K[4]) | (K[3] ^ K[5]))) &&
+			(*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_REQ_WEAK_KEY",
+				__func__);
+		return -EINVAL;
+	}
+	for (i = 0; i < 3; i++) {
+		ret = des_ekey(tmp, key + i*DES_KEY_SIZE);
+		if (unlikely(ret == 0) && (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+			*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+			pr_debug(DEV_DBG_NAME " [%s]: "
+					"CRYPTO_TFM_REQ_WEAK_KEY", __func__);
+			return -EINVAL;
+		}
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int des3_setkey(struct crypto_tfm *tfm, const u8 *key,
+		       unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+	u32 *flags = &tfm->crt_flags;
+	const u32 *K = (const u32 *)key;
+	u32 tmp[DES3_EDE_EXPKEY_WORDS];
+	int i, ret;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	if (keylen != DES3_EDE_KEY_SIZE) {
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_RES_BAD_KEY_LEN",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (unlikely(!((K[0] ^ K[2]) | (K[1] ^ K[3])) ||
+		     !((K[2] ^ K[4]) | (K[3] ^ K[5]))) &&
+	    (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_REQ_WEAK_KEY",
+			__func__);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < 3; i++) {
+		ret = des_ekey(tmp, key + i*DES_KEY_SIZE);
+		if (unlikely(ret == 0) && (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+			*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+			pr_debug(DEV_DBG_NAME " [%s]: "
+				 "CRYPTO_TFM_REQ_WEAK_KEY", __func__);
+			return -EINVAL;
+		}
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int cryp_hw_calculate(struct cryp_ctx *ctx)
+{
+	struct cryp_device_data *device_data;
+	int ret;
+
+	ret = cryp_get_device_data(ctx, &device_data);
+	if (ret)
+		goto out;
+
+	ret = cryp_enable_power(device_data->dev, device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_enable_power() failed!", __func__);
+		goto out;
+	}
+
+	if (hw_crypt_noxts(ctx, device_data))
+		pr_err("u8500_cryp:crypX: [%s]: hw_crypt_noxts() failed!",
+			__func__);
+
+out:
+	if (cryp_disable_power(device_data->dev, device_data, false))
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_disable_power() failed!", __func__);
+	/* Release the device */
+	spin_lock(&device_data->ctx_lock);
+	device_data->current_ctx = NULL;
+	ctx->device = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	/*
+	 * The down_interruptible part for this semaphore is called in
+	 * cryp_get_device_data.
+	 */
+	up(&driver_data.device_allocation);
+
+	return ret;
+}
+
+static void aes_encrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("u8500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static void aes_decrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("u8500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static void des_encrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("u8500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static void des_decrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("u8500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static void des3_encrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("u8500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static void des3_decrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("u8500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static int aes_ecb_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_ECB;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	if (cryp_mode == CRYP_MODE_DMA)
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int aes_ecb_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_ECB;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	if (cryp_mode == CRYP_MODE_DMA)
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int aes_cbc_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_CBC;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	/* Only DMA for ablkcipher, since givcipher not yet supported */
+	if ((cryp_mode == CRYP_MODE_DMA) &&
+			(*flags & CRYPTO_ALG_TYPE_ABLKCIPHER))
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int aes_cbc_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_CBC;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	/* Only DMA for ablkcipher, since givcipher not yet supported */
+	if ((cryp_mode == CRYP_MODE_DMA) &&
+			(*flags & CRYPTO_ALG_TYPE_ABLKCIPHER))
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int aes_ctr_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_CTR;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	/* Only DMA for ablkcipher, since givcipher not yet supported */
+	if ((cryp_mode == CRYP_MODE_DMA) &&
+			(*flags & CRYPTO_ALG_TYPE_ABLKCIPHER))
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int aes_ctr_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_CTR;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	/* Only DMA for ablkcipher, since givcipher not yet supported */
+	if ((cryp_mode == CRYP_MODE_DMA) &&
+			(*flags & CRYPTO_ALG_TYPE_ABLKCIPHER))
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int des_ecb_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_ECB;
+	ctx->blocksize = DES_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des_ecb_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_ECB;
+	ctx->blocksize = DES_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des_cbc_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_CBC;
+	ctx->blocksize = DES_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des_cbc_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_CBC;
+	ctx->blocksize = DES_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des3_ecb_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_ECB;
+	ctx->blocksize = DES3_EDE_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des3_ecb_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_ECB;
+	ctx->blocksize = DES3_EDE_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des3_cbc_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_CBC;
+	ctx->blocksize = DES3_EDE_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des3_cbc_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_CBC;
+	ctx->blocksize = DES3_EDE_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+/**
+ * struct crypto_alg aes_alg
+ */
+static struct crypto_alg aes_alg = {
+	.cra_name		=	"aes",
+	.cra_driver_name	=	"aes-u8500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_CIPHER,
+	.cra_blocksize		=	AES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(aes_alg.cra_list),
+	.cra_u			=	{
+		.cipher = {
+			.cia_min_keysize	=	AES_MIN_KEY_SIZE,
+			.cia_max_keysize	=	AES_MAX_KEY_SIZE,
+			.cia_setkey		=	aes_setkey,
+			.cia_encrypt		=	aes_encrypt,
+			.cia_decrypt		=	aes_decrypt
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des_alg
+ */
+static struct crypto_alg des_alg = {
+	.cra_name		=	"des",
+	.cra_driver_name	=	"des-u8500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_CIPHER,
+	.cra_blocksize		=	DES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des_alg.cra_list),
+	.cra_u			=	{
+		.cipher = {
+			.cia_min_keysize	=	DES_KEY_SIZE,
+			.cia_max_keysize	=	DES_KEY_SIZE,
+			.cia_setkey		=	des_setkey,
+			.cia_encrypt		=	des_encrypt,
+			.cia_decrypt		=	des_decrypt
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des3_alg
+ */
+static struct crypto_alg des3_alg = {
+	.cra_name		=	"des3_ede",
+	.cra_driver_name	=	"des3_ede-u8500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_CIPHER,
+	.cra_blocksize		=	DES3_EDE_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des3_alg.cra_list),
+	.cra_u			=	{
+		.cipher = {
+			.cia_min_keysize	=	DES3_EDE_KEY_SIZE,
+			.cia_max_keysize	=	DES3_EDE_KEY_SIZE,
+			.cia_setkey		=	des3_setkey,
+			.cia_encrypt		=	des3_encrypt,
+			.cia_decrypt		=	des3_decrypt
+		}
+	}
+};
+
+/**
+ * struct crypto_alg aes_ecb_alg
+ */
+static struct crypto_alg aes_ecb_alg = {
+	.cra_name		=	"ecb(aes)",
+	.cra_driver_name	=	"ecb-aes-u8500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	AES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(aes_ecb_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	AES_MIN_KEY_SIZE,
+			.max_keysize	=	AES_MAX_KEY_SIZE,
+			.setkey		=	aes_ablkcipher_setkey,
+			.encrypt	=	aes_ecb_encrypt,
+			.decrypt	=	aes_ecb_decrypt,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg aes_cbc_alg
+ */
+static struct crypto_alg aes_cbc_alg = {
+	.cra_name		=	"cbc(aes)",
+	.cra_driver_name	=	"cbc-aes-u8500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	AES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(aes_cbc_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	AES_MIN_KEY_SIZE,
+			.max_keysize	=	AES_MAX_KEY_SIZE,
+			.setkey		=	aes_ablkcipher_setkey,
+			.encrypt	=	aes_cbc_encrypt,
+			.decrypt	=	aes_cbc_decrypt,
+			.ivsize		=	AES_BLOCK_SIZE,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg aes_ctr_alg
+ */
+static struct crypto_alg aes_ctr_alg = {
+	.cra_name		=	"ctr(aes)",
+	.cra_driver_name	=	"ctr-aes-u8500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	AES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(aes_ctr_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	AES_MIN_KEY_SIZE,
+			.max_keysize	=	AES_MAX_KEY_SIZE,
+			.setkey		=	aes_ablkcipher_setkey,
+			.encrypt	=	aes_ctr_encrypt,
+			.decrypt	=	aes_ctr_decrypt,
+			.ivsize		=	AES_BLOCK_SIZE,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des_ecb_alg
+ */
+static struct crypto_alg des_ecb_alg = {
+	.cra_name		=	"ecb(des)",
+	.cra_driver_name	=	"ecb-des-u8500",
+	.cra_priority		=	100,
+	.cra_flags              =       CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	DES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des_ecb_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	DES_KEY_SIZE,
+			.max_keysize	=	DES_KEY_SIZE,
+			.setkey		=	des_ablkcipher_setkey,
+			.encrypt	=	des_ecb_encrypt,
+			.decrypt	=	des_ecb_decrypt,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des_cbc_alg
+ */
+static struct crypto_alg des_cbc_alg = {
+	.cra_name		=	"cbc(des)",
+	.cra_driver_name	=	"cbc-des-u8500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	DES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des_cbc_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	DES_KEY_SIZE,
+			.max_keysize	=	DES_KEY_SIZE,
+			.setkey		=	des_ablkcipher_setkey,
+			.encrypt	=	des_cbc_encrypt,
+			.decrypt	=	des_cbc_decrypt,
+			.ivsize		=	DES_BLOCK_SIZE,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des3_ecb_alg
+ */
+static struct crypto_alg des3_ecb_alg = {
+	.cra_name		=	"ecb(des3_ede)",
+	.cra_driver_name	=	"ecb-des3_ede-u8500",
+	.cra_priority		=	100,
+	.cra_flags              =       CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	DES3_EDE_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des3_ecb_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	DES3_EDE_KEY_SIZE,
+			.max_keysize	=	DES3_EDE_KEY_SIZE,
+			.setkey		=	des3_ablkcipher_setkey,
+			.encrypt	=	des3_ecb_encrypt,
+			.decrypt	=	des3_ecb_decrypt,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des3_cbc_alg
+ */
+static struct crypto_alg des3_cbc_alg = {
+	.cra_name		=	"cbc(des3_ede)",
+	.cra_driver_name	=	"cbc-des3_ede-u8500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	DES3_EDE_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des3_cbc_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	DES3_EDE_KEY_SIZE,
+			.max_keysize	=	DES3_EDE_KEY_SIZE,
+			.setkey		=	des3_ablkcipher_setkey,
+			.encrypt	=	des3_cbc_encrypt,
+			.decrypt	=	des3_cbc_decrypt,
+			.ivsize		=	DES3_EDE_BLOCK_SIZE,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg *u8500_cryp_algs[] -
+ */
+static struct crypto_alg *u8500_cryp_algs[] = {
+	&aes_alg,
+	&des_alg,
+	&des3_alg,
+	&aes_ecb_alg,
+	&aes_cbc_alg,
+	&aes_ctr_alg,
+	&des_ecb_alg,
+	&des_cbc_alg,
+	&des3_ecb_alg,
+	&des3_cbc_alg,
+};
+
+/**
+ * cryp_algs_register_all -
+ */
+static int cryp_algs_register_all(void)
+{
+	int ret;
+	int i;
+	int count;
+
+	pr_debug("[%s]", __func__);
+
+	for (i = 0; i < ARRAY_SIZE(u8500_cryp_algs); i++) {
+		ret = crypto_register_alg(u8500_cryp_algs[i]);
+		if (ret) {
+			count = i;
+			pr_err("[%s] alg registration failed",
+				u8500_cryp_algs[i]->cra_driver_name);
+			goto unreg;
+		}
+	}
+	return 0;
+unreg:
+	for (i = 0; i < count; i++)
+		crypto_unregister_alg(u8500_cryp_algs[i]);
+	return ret;
+}
+
+/**
+ * cryp_algs_unregister_all -
+ */
+static void cryp_algs_unregister_all(void)
+{
+	int i;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	for (i = 0; i < ARRAY_SIZE(u8500_cryp_algs); i++)
+		crypto_unregister_alg(u8500_cryp_algs[i]);
+}
+
+static int u8500_cryp_probe(struct platform_device *pdev)
+{
+	int ret;
+	int cryp_error = 0;
+	struct resource *res = NULL;
+	struct resource *res_irq = NULL;
+	struct cryp_device_data *device_data;
+	struct cryp_protection_config prot = {
+		.privilege_access = CRYP_STATE_ENABLE
+	};
+	struct device *dev = &pdev->dev;
+
+	dev_dbg(dev, "[%s]", __func__);
+	device_data = kzalloc(sizeof(struct cryp_device_data), GFP_ATOMIC);
+	if (!device_data) {
+		dev_err(dev, "[%s]: kzalloc() failed!", __func__);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	device_data->dev = dev;
+	device_data->current_ctx = NULL;
+
+	/* Grab the DMA configuration from platform data. */
+	mem_to_engine = &((struct cryp_platform_data *)
+			 dev->platform_data)->mem_to_engine;
+	engine_to_mem = &((struct cryp_platform_data *)
+			 dev->platform_data)->engine_to_mem;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "[%s]: platform_get_resource() failed",
+				__func__);
+		ret = -ENODEV;
+		goto out_kfree;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (res == NULL) {
+		dev_err(dev, "[%s]: request_mem_region() failed",
+				__func__);
+		ret = -EBUSY;
+		goto out_kfree;
+	}
+
+	device_data->base = ioremap(res->start, resource_size(res));
+	if (!device_data->base) {
+		dev_err(dev, "[%s]: ioremap failed!", __func__);
+		ret = -ENOMEM;
+		goto out_free_mem;
+	}
+
+	spin_lock_init(&device_data->ctx_lock);
+	spin_lock_init(&device_data->power_state_spinlock);
+
+	/* Enable power for CRYP hardware block */
+	device_data->pwr_regulator = ux500_regulator_get(&pdev->dev);
+	if (IS_ERR(device_data->pwr_regulator)) {
+		dev_err(dev, "[%s]: could not get cryp regulator", __func__);
+		ret = PTR_ERR(device_data->pwr_regulator);
+		device_data->pwr_regulator = NULL;
+		goto out_unmap;
+	}
+
+	/* Enable the clk for CRYP hardware block */
+	device_data->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(device_data->clk)) {
+		dev_err(dev, "[%s]: clk_get() failed!", __func__);
+		ret = PTR_ERR(device_data->clk);
+		goto out_regulator;
+	}
+
+	/* Enable device power (and clock) */
+	ret = cryp_enable_power(device_data->dev, device_data, false);
+	if (ret) {
+		dev_err(dev, "[%s]: cryp_enable_power() failed!", __func__);
+		goto out_clk;
+	}
+
+	cryp_error = cryp_check(device_data);
+	if (cryp_error != 0) {
+		dev_err(dev, "[%s]: cryp_init() failed!", __func__);
+		ret = -EINVAL;
+		goto out_power;
+	}
+
+	cryp_error = cryp_configure_protection(device_data, &prot);
+	if (cryp_error != 0) {
+		dev_err(dev, "[%s]: cryp_configure_protection() failed!",
+			__func__);
+		ret = -EINVAL;
+		goto out_power;
+	}
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq) {
+		dev_err(dev, "[%s]: IORESOURCE_IRQ unavailable",
+			__func__);
+		goto out_power;
+	}
+
+	ret = request_irq(res_irq->start,
+			  cryp_interrupt_handler,
+			  0,
+			  "cryp1",
+			  device_data);
+	if (ret) {
+		dev_err(dev, "[%s]: Unable to request IRQ", __func__);
+		goto out_power;
+	}
+
+	if (cryp_mode == CRYP_MODE_DMA)
+		cryp_dma_setup_channel(device_data, dev);
+
+	platform_set_drvdata(pdev, device_data);
+
+	/* Put the new device into the device list... */
+	klist_add_tail(&device_data->list_node, &driver_data.device_list);
+
+	/* ... and signal that a new device is available. */
+	up(&driver_data.device_allocation);
+
+	atomic_set(&session_id, 1);
+
+	ret = cryp_algs_register_all();
+	if (ret) {
+		dev_err(dev, "[%s]: cryp_algs_register_all() failed!",
+			__func__);
+		goto out_power;
+	}
+
+	if (cryp_disable_power(&pdev->dev, device_data, false))
+		dev_err(dev, "[%s]: cryp_disable_power() failed!", __func__);
+
+	return 0;
+
+out_power:
+	cryp_disable_power(&pdev->dev, device_data, false);
+
+out_clk:
+	clk_put(device_data->clk);
+
+out_regulator:
+	ux500_regulator_put(device_data->pwr_regulator);
+
+out_unmap:
+	iounmap(device_data->base);
+
+out_free_mem:
+	release_mem_region(res->start, resource_size(res));
+
+out_kfree:
+	kfree(device_data);
+out:
+	return ret;
+}
+
+static int u8500_cryp_remove(struct platform_device *pdev)
+{
+	struct resource *res = NULL;
+	struct resource *res_irq = NULL;
+	struct cryp_device_data *device_data;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	/* Try to decrease the number of available devices. */
+	if (down_trylock(&driver_data.device_allocation))
+		return -EBUSY;
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (device_data->current_ctx) {
+		/* The device is busy */
+		spin_unlock(&device_data->ctx_lock);
+		/* Return the device to the pool. */
+		up(&driver_data.device_allocation);
+		return -EBUSY;
+	}
+
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		cryp_algs_unregister_all();
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq)
+		dev_err(&pdev->dev, "[%s]: IORESOURCE_IRQ, unavailable",
+			__func__);
+	else {
+		disable_irq(res_irq->start);
+		free_irq(res_irq->start, device_data);
+	}
+
+	if (cryp_disable_power(&pdev->dev, device_data, false))
+		dev_err(&pdev->dev, "[%s]: cryp_disable_power() failed",
+			__func__);
+
+	clk_put(device_data->clk);
+	ux500_regulator_put(device_data->pwr_regulator);
+
+	iounmap(device_data->base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, res->end - res->start + 1);
+
+	kfree(device_data);
+
+	return 0;
+}
+
+static void u8500_cryp_shutdown(struct platform_device *pdev)
+{
+	struct resource *res_irq = NULL;
+	struct cryp_device_data *device_data;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return;
+	}
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (!device_data->current_ctx) {
+		if (down_trylock(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: Cryp still in use!"
+				"Shutting down anyway...", __func__);
+		/**
+		 * (Allocate the device)
+		 * Need to set this to non-null (dummy) value,
+		 * to avoid usage if context switching.
+		 */
+		device_data->current_ctx++;
+	}
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		cryp_algs_unregister_all();
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq)
+		dev_err(&pdev->dev, "[%s]: IORESOURCE_IRQ, unavailable",
+			__func__);
+	else {
+		disable_irq(res_irq->start);
+		free_irq(res_irq->start, device_data);
+	}
+
+	if (cryp_disable_power(&pdev->dev, device_data, false))
+		dev_err(&pdev->dev, "[%s]: cryp_disable_power() failed",
+			__func__);
+
+}
+
+static int u8500_cryp_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int ret;
+	struct cryp_device_data *device_data;
+	struct resource *res_irq;
+	struct cryp_ctx *temp_ctx = NULL;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	/* Handle state? */
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq)
+		dev_err(&pdev->dev, "[%s]: IORESOURCE_IRQ, unavailable",
+			__func__);
+	else
+		disable_irq(res_irq->start);
+
+	spin_lock(&device_data->ctx_lock);
+	if (!device_data->current_ctx)
+		device_data->current_ctx++;
+	spin_unlock(&device_data->ctx_lock);
+
+	if (device_data->current_ctx == ++temp_ctx) {
+		if (down_interruptible(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: down_interruptible() "
+					"failed", __func__);
+		ret = cryp_disable_power(&pdev->dev, device_data, false);
+
+	} else
+		ret = cryp_disable_power(&pdev->dev, device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: cryp_disable_power()", __func__);
+
+	return ret;
+}
+
+static int u8500_cryp_resume(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct cryp_device_data *device_data;
+	struct resource *res_irq;
+	struct cryp_ctx *temp_ctx = NULL;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	spin_lock(&device_data->ctx_lock);
+	if (device_data->current_ctx == ++temp_ctx)
+		device_data->current_ctx = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+
+	if (!device_data->current_ctx)
+		up(&driver_data.device_allocation);
+	else
+		ret = cryp_enable_power(&pdev->dev, device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: cryp_enable_power() failed!",
+			__func__);
+	else {
+		res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+		if (res_irq)
+			enable_irq(res_irq->start);
+	}
+
+	return ret;
+}
+
+static struct platform_driver cryp_driver = {
+	.probe  = u8500_cryp_probe,
+	.remove = u8500_cryp_remove,
+	.shutdown = u8500_cryp_shutdown,
+	.suspend  = u8500_cryp_suspend,
+	.resume   = u8500_cryp_resume,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name  = "cryp1"
+	}
+};
+
+static int __init u8500_cryp_mod_init(void)
+{
+	pr_debug("[%s] is called!", __func__);
+
+	klist_init(&driver_data.device_list, NULL, NULL);
+	/* Initialize the semaphore to 0 devices (locked state) */
+	sema_init(&driver_data.device_allocation, 0);
+	return platform_driver_register(&cryp_driver);
+}
+
+static void __exit u8500_cryp_mod_fini(void)
+{
+	pr_debug("[%s] is called!", __func__);
+	platform_driver_unregister(&cryp_driver);
+	return;
+}
+
+module_init(u8500_cryp_mod_init);
+module_exit(u8500_cryp_mod_fini);
+
+module_param(cryp_mode, int, 0);
+
+MODULE_DESCRIPTION("Driver for ST-Ericsson U8500 CRYP crypto engine.");
+MODULE_ALIAS("aes-all");
+MODULE_ALIAS("des-all");
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/crypto/ux500/cryp/cryp_irq.c b/drivers/crypto/ux500/cryp/cryp_irq.c
new file mode 100644
index 00000000000..08d291cdbe6
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_irq.c
@@ -0,0 +1,45 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/bitmap.h>
+#include <linux/device.h>
+
+#include "cryp.h"
+#include "cryp_p.h"
+#include "cryp_irq.h"
+#include "cryp_irqp.h"
+
+void cryp_enable_irq_src(struct cryp_device_data *device_data, u32 irq_src)
+{
+	u32 i;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	i = readl_relaxed(&device_data->base->imsc);
+	i = i | irq_src;
+	writel_relaxed(i, &device_data->base->imsc);
+}
+
+void cryp_disable_irq_src(struct cryp_device_data *device_data, u32 irq_src)
+{
+	u32 i;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	i = readl_relaxed(&device_data->base->imsc);
+	i = i & ~irq_src;
+	writel_relaxed(i, &device_data->base->imsc);
+}
+
+bool cryp_pending_irq_src(struct cryp_device_data *device_data, u32 irq_src)
+{
+	return (readl_relaxed(&device_data->base->mis) & irq_src) > 0;
+}
diff --git a/drivers/crypto/ux500/cryp/cryp_irq.h b/drivers/crypto/ux500/cryp/cryp_irq.h
new file mode 100644
index 00000000000..5a7837f1b8f
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_irq.h
@@ -0,0 +1,31 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef _CRYP_IRQ_H_
+#define _CRYP_IRQ_H_
+
+#include "cryp.h"
+
+enum cryp_irq_src_id {
+	CRYP_IRQ_SRC_INPUT_FIFO = 0x1,
+	CRYP_IRQ_SRC_OUTPUT_FIFO = 0x2,
+	CRYP_IRQ_SRC_ALL = 0x3
+};
+
+/**
+ * M0 Funtions
+ */
+void cryp_enable_irq_src(struct cryp_device_data *device_data, u32 irq_src);
+
+void cryp_disable_irq_src(struct cryp_device_data *device_data, u32 irq_src);
+
+bool cryp_pending_irq_src(struct cryp_device_data *device_data, u32 irq_src);
+
+#endif				/* _CRYP_IRQ_H_ */
diff --git a/drivers/crypto/ux500/cryp/cryp_irqp.h b/drivers/crypto/ux500/cryp/cryp_irqp.h
new file mode 100644
index 00000000000..8b339cc34bf
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_irqp.h
@@ -0,0 +1,125 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __CRYP_IRQP_H_
+#define __CRYP_IRQP_H_
+
+#include "cryp_irq.h"
+
+/**
+ *
+ * CRYP Registers - Offset mapping
+ *     +-----------------+
+ * 00h | CRYP_CR         |  Configuration register
+ *     +-----------------+
+ * 04h | CRYP_SR         |  Status register
+ *     +-----------------+
+ * 08h | CRYP_DIN        |  Data In register
+ *     +-----------------+
+ * 0ch | CRYP_DOUT       |  Data out register
+ *     +-----------------+
+ * 10h | CRYP_DMACR      |  DMA control register
+ *     +-----------------+
+ * 14h | CRYP_IMSC       |  IMSC
+ *     +-----------------+
+ * 18h | CRYP_RIS        |  Raw interrupt status
+ *     +-----------------+
+ * 1ch | CRYP_MIS        |  Masked interrupt status.
+ *     +-----------------+
+ *       Key registers
+ *       IVR registers
+ *       Peripheral
+ *       Cell IDs
+ *
+ *       Refer data structure for other register map
+ */
+
+/**
+ * struct cryp_register
+ * @cr			- Configuration register
+ * @status		- Status register
+ * @din			- Data input register
+ * @din_size		- Data input size register
+ * @dout		- Data output register
+ * @dout_size		- Data output size register
+ * @dmacr		- Dma control register
+ * @imsc		- Interrupt mask set/clear register
+ * @ris			- Raw interrupt status
+ * @mis			- Masked interrupt statu register
+ * @key_1_l		- Key register 1 L
+ * @key_1_r		- Key register 1 R
+ * @key_2_l		- Key register 2 L
+ * @key_2_r		- Key register 2 R
+ * @key_3_l		- Key register 3 L
+ * @key_3_r		- Key register 3 R
+ * @key_4_l		- Key register 4 L
+ * @key_4_r		- Key register 4 R
+ * @init_vect_0_l	- init vector 0 L
+ * @init_vect_0_r	- init vector 0 R
+ * @init_vect_1_l	- init vector 1 L
+ * @init_vect_1_r	- init vector 1 R
+ * @cryp_unused1	- unused registers
+ * @itcr		- Integration test control register
+ * @itip		- Integration test input register
+ * @itop		- Integration test output register
+ * @cryp_unused2	- unused registers
+ * @periphId0		- FE0 CRYP Peripheral Identication Register
+ * @periphId1		- FE4
+ * @periphId2		- FE8
+ * @periphId3		- FEC
+ * @pcellId0		- FF0  CRYP PCell Identication Register
+ * @pcellId1		- FF4
+ * @pcellId2		- FF8
+ * @pcellId3		- FFC
+ */
+struct cryp_register {
+	u32 cr;			/* Configuration register   */
+	u32 sr;			/* Status register          */
+	u32 din;		/* Data input register      */
+	u32 din_size;		/* Data input size register */
+	u32 dout;		/* Data output register     */
+	u32 dout_size;		/* Data output size register */
+	u32 dmacr;		/* Dma control register     */
+	u32 imsc;		/* Interrupt mask set/clear register */
+	u32 ris;		/* Raw interrupt status             */
+	u32 mis;		/* Masked interrupt statu register  */
+
+	u32 key_1_l;		/*Key register 1 L */
+	u32 key_1_r;		/*Key register 1 R */
+	u32 key_2_l;		/*Key register 2 L */
+	u32 key_2_r;		/*Key register 2 R */
+	u32 key_3_l;		/*Key register 3 L */
+	u32 key_3_r;		/*Key register 3 R */
+	u32 key_4_l;		/*Key register 4 L */
+	u32 key_4_r;		/*Key register 4 R */
+
+	u32 init_vect_0_l;	/*init vector 0 L */
+	u32 init_vect_0_r;	/*init vector 0 R */
+	u32 init_vect_1_l;	/*init vector 1 L */
+	u32 init_vect_1_r;	/*init vector 1 R */
+
+	u32 cryp_unused1[(0x80 - 0x58) / sizeof(u32)];	/* unused registers */
+	u32 itcr;		/*Integration test control register */
+	u32 itip;		/*Integration test input register */
+	u32 itop;		/*Integration test output register */
+	u32 cryp_unused2[(0xFE0 - 0x8C) / sizeof(u32)];	/* unused registers */
+
+	u32 periphId0;		/* FE0  CRYP Peripheral Identication Register */
+	u32 periphId1;		/* FE4 */
+	u32 periphId2;		/* FE8 */
+	u32 periphId3;		/* FEC */
+
+	u32 pcellId0;		/* FF0  CRYP PCell Identication Register */
+	u32 pcellId1;		/* FF4 */
+	u32 pcellId2;		/* FF8 */
+	u32 pcellId3;		/* FFC */
+};
+
+#endif
diff --git a/drivers/crypto/ux500/cryp/cryp_p.h b/drivers/crypto/ux500/cryp/cryp_p.h
new file mode 100644
index 00000000000..4b615a33fe9
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_p.h
@@ -0,0 +1,122 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef _CRYP_P_H_
+#define _CRYP_P_H_
+
+#include <linux/io.h>
+#include <linux/bitops.h>
+
+#include "cryp.h"
+#include "cryp_irqp.h"
+
+/**
+ * Generic Macros
+ */
+#define CRYP_SET_BITS(reg_name, mask) \
+	writel_relaxed((readl_relaxed(reg_name) | mask), reg_name)
+
+#define CRYP_WRITE_BIT(reg_name, val, mask) \
+	writel_relaxed(((readl_relaxed(reg_name) & ~(mask)) |\
+			((val) & (mask))), reg_name)
+
+#define CRYP_TEST_BITS(reg_name, val) \
+	(readl_relaxed(reg_name) & (val))
+
+#define CRYP_PUT_BITS(reg, val, shift, mask) \
+	writel_relaxed(((readl_relaxed(reg) & ~(mask)) | \
+		(((u32)val << shift) & (mask))), reg)
+
+/**
+ * CRYP specific Macros
+ */
+#define CRYP_PERIPHERAL_ID0		0xE3
+#define CRYP_PERIPHERAL_ID1		0x05
+#define CRYP_PERIPHERAL_ID2		0x28
+#define CRYP_PERIPHERAL_ID3		0x00
+
+#define CRYP_PCELL_ID0			0x0D
+#define CRYP_PCELL_ID1			0xF0
+#define CRYP_PCELL_ID2			0x05
+#define CRYP_PCELL_ID3			0xB1
+
+/**
+ * CRYP register default values
+ */
+#define MAX_DEVICE_SUPPORT		2
+
+/* Priv set, keyrden set and datatype 8bits swapped set as default. */
+#define CRYP_CR_DEFAULT			0x0482
+#define CRYP_DMACR_DEFAULT		0x0
+#define CRYP_IMSC_DEFAULT		0x0
+#define CRYP_DIN_DEFAULT		0x0
+#define CRYP_DOUT_DEFAULT		0x0
+#define CRYP_KEY_DEFAULT		0x0
+#define CRYP_INIT_VECT_DEFAULT		0x0
+
+/**
+ * CRYP Control register specific mask
+ */
+#define CRYP_CR_SECURE_MASK		BIT(0)
+#define CRYP_CR_PRLG_MASK		BIT(1)
+#define CRYP_CR_ALGODIR_MASK		BIT(2)
+#define CRYP_CR_ALGOMODE_MASK		(BIT(5) | BIT(4) | BIT(3))
+#define CRYP_CR_DATATYPE_MASK		(BIT(7) | BIT(6))
+#define CRYP_CR_KEYSIZE_MASK		(BIT(9) | BIT(8))
+#define CRYP_CR_KEYRDEN_MASK		BIT(10)
+#define CRYP_CR_KSE_MASK		BIT(11)
+#define CRYP_CR_START_MASK		BIT(12)
+#define CRYP_CR_INIT_MASK		BIT(13)
+#define CRYP_CR_FFLUSH_MASK		BIT(14)
+#define CRYP_CR_CRYPEN_MASK		BIT(15)
+#define CRYP_CR_CONTEXT_SAVE_MASK	(CRYP_CR_SECURE_MASK |\
+					 CRYP_CR_PRLG_MASK |\
+					 CRYP_CR_ALGODIR_MASK |\
+					 CRYP_CR_ALGOMODE_MASK |\
+					 CRYP_CR_DATATYPE_MASK |\
+					 CRYP_CR_KEYSIZE_MASK |\
+					 CRYP_CR_KEYRDEN_MASK |\
+					 CRYP_CR_DATATYPE_MASK)
+
+
+#define CRYP_SR_INFIFO_READY_MASK	(BIT(0) | BIT(1))
+#define CRYP_SR_IFEM_MASK		BIT(0)
+#define CRYP_SR_BUSY_MASK		BIT(4)
+
+/**
+ * Bit position used while setting bits in register
+ */
+#define CRYP_CR_PRLG_POS		1
+#define CRYP_CR_ALGODIR_POS		2
+#define CRYP_CR_ALGOMODE_POS		3
+#define CRYP_CR_DATATYPE_POS		6
+#define CRYP_CR_KEYSIZE_POS		8
+#define CRYP_CR_KEYRDEN_POS		10
+#define CRYP_CR_KSE_POS			11
+#define CRYP_CR_START_POS		12
+#define CRYP_CR_INIT_POS		13
+#define CRYP_CR_CRYPEN_POS		15
+
+#define CRYP_SR_BUSY_POS		4
+
+/**
+ * CRYP PCRs------PC_NAND control register
+ * BIT_MASK
+ */
+#define CRYP_DMA_REQ_MASK		(BIT(1) | BIT(0))
+#define CRYP_DMA_REQ_MASK_POS		0
+
+
+struct cryp_system_context {
+	/* CRYP Register structure */
+	struct cryp_register *p_cryp_reg[MAX_DEVICE_SUPPORT];
+};
+
+#endif
diff --git a/drivers/crypto/ux500/hash/Makefile b/drivers/crypto/ux500/hash/Makefile
new file mode 100644
index 00000000000..aaa5f56a2c2
--- /dev/null
+++ b/drivers/crypto/ux500/hash/Makefile
@@ -0,0 +1,11 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Shujuan Chen (shujuan.chen@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+ifdef CONFIG_CRYPTO_DEV_UX500_DEBUG
+CFLAGS_hash_core.o := -DDEBUG -O0
+endif
+
+obj-$(CONFIG_CRYPTO_DEV_UX500_HASH) += u8500_hash.o
+u8500_hash-objs :=  hash_core.o
diff --git a/drivers/crypto/ux500/hash/hash_alg.h b/drivers/crypto/ux500/hash/hash_alg.h
new file mode 100644
index 00000000000..299f0bacc2c
--- /dev/null
+++ b/drivers/crypto/ux500/hash/hash_alg.h
@@ -0,0 +1,353 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen (shujuan.chen@stericsson.com)
+ * Author: Joakim Bech (joakim.xx.bech@stericsson.com)
+ * Author: Berne Hebark (berne.hebark@stericsson.com))
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#ifndef _HASH_ALG_H
+#define _HASH_ALG_H
+
+#include <linux/bitops.h>
+
+#define HASH_BLOCK_SIZE			64
+
+/* Maximum value of the length's high word */
+#define HASH_HIGH_WORD_MAX_VAL		0xFFFFFFFFUL
+
+/* Power on Reset values HASH registers */
+#define HASH_RESET_CR_VALUE		0x0
+#define HASH_RESET_STR_VALUE		0x0
+
+/* Number of context swap registers */
+#define HASH_CSR_COUNT			52
+
+#define HASH_RESET_CSRX_REG_VALUE	0x0
+#define HASH_RESET_CSFULL_REG_VALUE	0x0
+#define HASH_RESET_CSDATAIN_REG_VALUE	0x0
+
+#define HASH_RESET_INDEX_VAL		0x0
+#define HASH_RESET_BIT_INDEX_VAL	0x0
+#define HASH_RESET_BUFFER_VAL		0x0
+#define HASH_RESET_LEN_HIGH_VAL		0x0
+#define HASH_RESET_LEN_LOW_VAL		0x0
+
+/* Control register bitfields */
+#define HASH_CR_RESUME_MASK	0x11FCF
+
+#define HASH_CR_SWITCHON_POS	31
+#define HASH_CR_SWITCHON_MASK	BIT(31)
+
+#define HASH_CR_EMPTYMSG_POS	20
+#define HASH_CR_EMPTYMSG_MASK	BIT(20)
+
+#define HASH_CR_DINF_POS	12
+#define HASH_CR_DINF_MASK	BIT(12)
+
+#define HASH_CR_NBW_POS		8
+#define HASH_CR_NBW_MASK	0x00000F00UL
+
+#define HASH_CR_LKEY_POS	16
+#define HASH_CR_LKEY_MASK	BIT(16)
+
+#define HASH_CR_ALGO_POS	7
+#define HASH_CR_ALGO_MASK	BIT(7)
+
+#define HASH_CR_MODE_POS	6
+#define HASH_CR_MODE_MASK	BIT(6)
+
+#define HASH_CR_DATAFORM_POS	4
+#define HASH_CR_DATAFORM_MASK	(BIT(4) | BIT(5))
+
+#define HASH_CR_DMAE_POS	3
+#define HASH_CR_DMAE_MASK	BIT(3)
+
+#define HASH_CR_INIT_POS	2
+#define HASH_CR_INIT_MASK	BIT(2)
+
+#define HASH_CR_PRIVN_POS	1
+#define HASH_CR_PRIVN_MASK	BIT(1)
+
+#define HASH_CR_SECN_POS	0
+#define HASH_CR_SECN_MASK	BIT(0)
+
+/* Start register bitfields */
+#define HASH_STR_DCAL_POS	8
+#define HASH_STR_DCAL_MASK	BIT(8)
+#define HASH_STR_DEFAULT	0x0
+
+#define HASH_STR_NBLW_POS	0
+#define HASH_STR_NBLW_MASK	0x0000001FUL
+
+#define HASH_NBLW_MAX_VAL	0x1F
+
+/* PrimeCell IDs */
+#define HASH_P_ID0		0xE0
+#define HASH_P_ID1		0x05
+#define HASH_P_ID2		0x38
+#define HASH_P_ID3		0x00
+#define HASH_CELL_ID0		0x0D
+#define HASH_CELL_ID1		0xF0
+#define HASH_CELL_ID2		0x05
+#define HASH_CELL_ID3		0xB1
+
+#define HASH_SET_BITS(reg_name, mask)	\
+	writel((readl(reg_name) | mask), reg_name)
+
+#define HASH_CLEAR_BITS(reg_name, mask)	\
+	writel((readl(reg_name) & ~mask), reg_name)
+
+#define HASH_PUT_BITS(reg, val, shift, mask)	\
+	writel(((readl(reg) & ~(mask)) |	\
+		(((u32)val << shift) & (mask))), reg)
+
+#define HASH_SET_DIN(val)	writel((val), &device_data->base->din)
+
+#define HASH_INITIALIZE			\
+	HASH_PUT_BITS(			\
+		&device_data->base->cr,	\
+		0x01, HASH_CR_INIT_POS,	\
+		HASH_CR_INIT_MASK)
+
+#define HASH_SET_DATA_FORMAT(data_format)				\
+		HASH_PUT_BITS(						\
+			&device_data->base->cr,				\
+			(u32) (data_format), HASH_CR_DATAFORM_POS,	\
+			HASH_CR_DATAFORM_MASK)
+#define HASH_SET_NBLW(val)					\
+		HASH_PUT_BITS(					\
+			&device_data->base->str,		\
+			(u32) (val), HASH_STR_NBLW_POS,		\
+			HASH_STR_NBLW_MASK)
+#define HASH_SET_DCAL					\
+		HASH_PUT_BITS(				\
+			&device_data->base->str,	\
+			0x01, HASH_STR_DCAL_POS,	\
+			HASH_STR_DCAL_MASK)
+
+/**
+ * struct uint64 - Structure to handle 64 bits integers.
+ * @high_word:	Most significant bits.
+ * @low_word:	Least significant bits.
+ *
+ * Used to handle 64 bits integers.
+ */
+struct uint64 {
+	u32 high_word;
+	u32 low_word;
+};
+
+/**
+ * struct hash_register - Contains all registers in u8500 hash hardware.
+ * @cr:		HASH control register (0x000).
+ * @din:	HASH data input register (0x004).
+ * @str:	HASH start register (0x008).
+ * @hx:		HASH digest register 0..7 (0x00c-0x01C).
+ * @padding0:	Reserved (0x02C).
+ * @itcr:	Integration test control register (0x080).
+ * @itip:	Integration test input register (0x084).
+ * @itop:	Integration test output register (0x088).
+ * @padding1:	Reserved (0x08C).
+ * @csfull:	HASH context full register (0x0F8).
+ * @csdatain:	HASH context swap data input register (0x0FC).
+ * @csrx:	HASH context swap register 0..51 (0x100-0x1CC).
+ * @padding2:	Reserved (0x1D0).
+ * @periphid0:	HASH peripheral identification register 0 (0xFE0).
+ * @periphid1:	HASH peripheral identification register 1 (0xFE4).
+ * @periphid2:	HASH peripheral identification register 2 (0xFE8).
+ * @periphid3:	HASH peripheral identification register 3 (0xFEC).
+ * @cellid0:	HASH PCell identification register 0 (0xFF0).
+ * @cellid1:	HASH PCell identification register 1 (0xFF4).
+ * @cellid2:	HASH PCell identification register 2 (0xFF8).
+ * @cellid3:	HASH PCell identification register 3 (0xFFC).
+ *
+ * The device communicates to the HASH via 32-bit-wide control registers
+ * accessible via the 32-bit width AMBA rev. 2.0 AHB Bus. Below is a structure
+ * with the registers used.
+ */
+struct hash_register {
+	u32 cr;
+	u32 din;
+	u32 str;
+	u32 hx[8];
+
+	u32 padding0[(0x080 - 0x02C) / sizeof(u32)];
+
+	u32 itcr;
+	u32 itip;
+	u32 itop;
+
+	u32 padding1[(0x0F8 - 0x08C) / sizeof(u32)];
+
+	u32 csfull;
+	u32 csdatain;
+	u32 csrx[HASH_CSR_COUNT];
+
+	u32 padding2[(0xFE0 - 0x1D0) / sizeof(u32)];
+
+	u32 periphid0;
+	u32 periphid1;
+	u32 periphid2;
+	u32 periphid3;
+
+	u32 cellid0;
+	u32 cellid1;
+	u32 cellid2;
+	u32 cellid3;
+};
+
+/**
+ * struct hash_state - Hash context state.
+ * @temp_cr:	Temporary HASH Control Register.
+ * @str_reg:	HASH Start Register.
+ * @din_reg:	HASH Data Input Register.
+ * @csr[52]:	HASH Context Swap Registers 0-39.
+ * @csfull:	HASH Context Swap Registers 40 ie Status flags.
+ * @csdatain:	HASH Context Swap Registers 41 ie Input data.
+ * @buffer:	Working buffer for messages going to the hardware.
+ * @length:	Length of the part of message hashed so far (floor(N/64) * 64).
+ * @index:	Valid number of bytes in buffer (N % 64).
+ * @bit_index:	Valid number of bits in buffer (N % 8).
+ *
+ * This structure is used between context switches, i.e. when ongoing jobs are
+ * interupted with new jobs. When this happens we need to store intermediate
+ * results in software.
+ *
+ * WARNING: "index" is the  member of the structure, to be sure  that "buffer"
+ * is aligned on a 4-bytes boundary. This is highly implementation dependent
+ * and MUST be checked whenever this code is ported on new platforms.
+ */
+struct hash_state {
+	u32		temp_cr;
+	u32		str_reg;
+	u32		din_reg;
+	u32		csr[52];
+	u32		csfull;
+	u32		csdatain;
+	u32		buffer[HASH_BLOCK_SIZE / sizeof(u32)];
+	struct uint64	length;
+	u8		index;
+	u8		bit_index;
+};
+
+/**
+ * enum hash_device_id - HASH device ID.
+ * @HASH_DEVICE_ID_0: Hash hardware with ID 0
+ * @HASH_DEVICE_ID_1: Hash hardware with ID 1
+ */
+enum hash_device_id {
+	HASH_DEVICE_ID_0 = 0,
+	HASH_DEVICE_ID_1 = 1
+};
+
+/**
+ * enum hash_data_format - HASH data format.
+ * @HASH_DATA_32_BITS:	32 bits data format
+ * @HASH_DATA_16_BITS:	16 bits data format
+ * @HASH_DATA_8_BITS:	8 bits data format.
+ * @HASH_DATA_1_BITS:	1 bit data format.
+ */
+enum hash_data_format {
+	HASH_DATA_32_BITS	= 0x0,
+	HASH_DATA_16_BITS	= 0x1,
+	HASH_DATA_8_BITS	= 0x2,
+	HASH_DATA_1_BIT		= 0x3
+};
+
+/**
+ * enum hash_algo - Enumeration for selecting between SHA1 or SHA2 algorithm.
+ * @HASH_ALGO_SHA1: Indicates that SHA1 is used.
+ * @HASH_ALGO_SHA2: Indicates that SHA2 (SHA256) is used.
+ */
+enum hash_algo {
+	HASH_ALGO_SHA1		= 0x0,
+	HASH_ALGO_SHA256	= 0x1
+};
+
+/**
+ * enum hash_op - Enumeration for selecting between HASH or HMAC mode.
+ * @HASH_OPER_MODE_HASH: Indicates usage of normal HASH mode.
+ * @HASH_OPER_MODE_HMAC: Indicates usage of HMAC.
+ */
+enum hash_op {
+	HASH_OPER_MODE_HASH = 0x0,
+	HASH_OPER_MODE_HMAC = 0x1
+};
+
+/**
+ * struct hash_config - Configuration data for the hardware.
+ * @data_format:	Format of data entered into the hash data in register.
+ * @algorithm:		Algorithm selection bit.
+ * @oper_mode:		Operating mode selection bit.
+ */
+struct hash_config {
+	int data_format;
+	int algorithm;
+	int oper_mode;
+};
+
+/**
+ * struct hash_ctx - The context used for hash calculations.
+ * @key:	The key used in the operation.
+ * @keylen:	The length of the key.
+ * @updated:	Indicates if hardware is initialized for new operations.
+ * @state:	The state of the current calculations.
+ * @config:	The current configuration.
+ * @digestsize	The size of current digest.
+ * @device	Pointer to the device structure.
+ */
+struct hash_ctx {
+	u8			*key;
+	u32			keylen;
+	u8			updated;
+	struct hash_state	state;
+	struct hash_config	config;
+	int			digestsize;
+	struct hash_device_data	*device;
+};
+
+/**
+ * struct hash_device_data - structure for a hash device.
+ * @base:		Pointer to the hardware base address.
+ * @list_node:		For inclusion in klist.
+ * @dev:		Pointer to the device dev structure.
+ * @ctx_lock:		Spinlock for current_ctx.
+ * @current_ctx:	Pointer to the currently allocated context.
+ * @power_state:	TRUE = power state on, FALSE = power state off.
+ * @power_state_lock:	Spinlock for power_state.
+ * @regulator:		Pointer to the device's power control.
+ * @clk:		Pointer to the device's clock control.
+ * @restore_dev_state:	TRUE = saved state, FALSE = no saved state.
+ */
+struct hash_device_data {
+	struct hash_register __iomem	*base;
+	struct klist_node		list_node;
+	struct device			*dev;
+	struct spinlock			ctx_lock;
+	struct hash_ctx			*current_ctx;
+	bool				power_state;
+	struct spinlock			power_state_lock;
+	struct ux500_regulator		*regulator;
+	struct clk			*clk;
+	bool				restore_dev_state;
+};
+
+int hash_check_hw(struct hash_device_data *device_data);
+
+int hash_setconfiguration(struct hash_device_data *device_data,
+		struct hash_config *config);
+
+void hash_begin(struct hash_device_data *device_data, struct hash_ctx *ctx);
+
+void hash_get_digest(struct hash_device_data *device_data,
+		u8 *digest, int algorithm);
+
+int hash_hw_update(struct ahash_request *req);
+
+int hash_save_state(struct hash_device_data *device_data,
+		struct hash_state *state);
+
+int hash_resume_state(struct hash_device_data *device_data,
+		const struct hash_state *state);
+
+#endif
diff --git a/drivers/crypto/ux500/hash/hash_alg_p.h b/drivers/crypto/ux500/hash/hash_alg_p.h
new file mode 100755
index 00000000000..c85faaeba6f
--- /dev/null
+++ b/drivers/crypto/ux500/hash/hash_alg_p.h
@@ -0,0 +1,26 @@
+/*****************************************************************************/
+/**
+*  � ST-Ericsson, 2009 - All rights reserved
+*  Reproduction and Communication of this document is strictly prohibited
+*  unless specifically authorized in writing by ST-Ericsson
+*
+*  static Header file of HASH Processor
+*  Specification release related to this implementation: A_V2.2
+*  AUTHOR :  ST-Ericsson
+*/
+/*****************************************************************************/
+
+#ifndef _HASH_P_H_
+#define _HASH_P_H_
+
+/*--------------------------------------------------------------------------*
+ * Includes                                                                 *
+ *--------------------------------------------------------------------------*/
+#include "hash_alg.h"
+
+/*--------------------------------------------------------------------------*
+ * Defines                                                                  *
+ *--------------------------------------------------------------------------*/
+
+#endif /* End _HASH_P_H_ */
+
diff --git a/drivers/crypto/ux500/hash/hash_core.c b/drivers/crypto/ux500/hash/hash_core.c
new file mode 100644
index 00000000000..b042808496c
--- /dev/null
+++ b/drivers/crypto/ux500/hash/hash_core.c
@@ -0,0 +1,1810 @@
+/*
+ * Cryptographic API.
+ * Support for Nomadik hardware crypto engine.
+
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/klist.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/crypto.h>
+
+#include <linux/regulator/dbx500-prcmu.h>
+#include <linux/bitops.h>
+
+#include <crypto/internal/hash.h>
+#include <crypto/sha.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/algapi.h>
+
+#include <mach/hardware.h>
+
+#include "hash_alg.h"
+
+#define DEV_DBG_NAME "hashX hashX:"
+
+/**
+ * Pre-calculated empty message digests.
+ */
+static u8 zero_message_hash_sha1[SHA1_DIGEST_SIZE] = {
+	0xda, 0x39, 0xa3, 0xee, 0x5e, 0x6b, 0x4b, 0x0d,
+	0x32, 0x55, 0xbf, 0xef, 0x95, 0x60, 0x18, 0x90,
+	0xaf, 0xd8, 0x07, 0x09
+};
+
+static u8 zero_message_hash_sha256[SHA256_DIGEST_SIZE] = {
+	0xe3, 0xb0, 0xc4, 0x42, 0x98, 0xfc, 0x1c, 0x14,
+	0x9a, 0xfb, 0xf4, 0xc8, 0x99, 0x6f, 0xb9, 0x24,
+	0x27, 0xae, 0x41, 0xe4, 0x64, 0x9b, 0x93, 0x4c,
+	0xa4, 0x95, 0x99, 0x1b, 0x78, 0x52, 0xb8, 0x55
+};
+
+/* HMAC-SHA1, no key */
+static u8 zero_message_hmac_sha1[SHA1_DIGEST_SIZE] = {
+	0xfb, 0xdb, 0x1d, 0x1b, 0x18, 0xaa, 0x6c, 0x08,
+	0x32, 0x4b, 0x7d, 0x64, 0xb7, 0x1f, 0xb7, 0x63,
+	0x70, 0x69, 0x0e, 0x1d
+};
+
+/* HMAC-SHA256, no key */
+static u8 zero_message_hmac_sha256[SHA256_DIGEST_SIZE] = {
+	0xb6, 0x13, 0x67, 0x9a, 0x08, 0x14, 0xd9, 0xec,
+	0x77, 0x2f, 0x95, 0xd7, 0x78, 0xc3, 0x5f, 0xc5,
+	0xff, 0x16, 0x97, 0xc4, 0x93, 0x71, 0x56, 0x53,
+	0xc6, 0xc7, 0x12, 0x14, 0x42, 0x92, 0xc5, 0xad
+};
+
+/**
+ * struct hash_driver_data - data specific to the driver.
+ *
+ * @device_list:	A list of registered devices to choose from.
+ * @device_allocation:	A semaphore initialized with number of devices.
+ */
+struct hash_driver_data {
+	struct klist		device_list;
+	struct semaphore	device_allocation;
+};
+
+static struct hash_driver_data	driver_data;
+
+/* Declaration of functions */
+/**
+ * hash_messagepad - Pads a message and write the nblw bits.
+ * @device_data:	Structure for the hash device.
+ * @message:		Last word of a message
+ * @index_bytes:	The number of bytes in the last message
+ *
+ * This function manages the final part of the digest calculation, when less
+ * than 512 bits (64 bytes) remain in message. This means index_bytes < 64.
+ *
+ * Reentrancy: Non Re-entrant.
+ */
+static void hash_messagepad(struct hash_device_data *device_data,
+		const u32 *message, u8 index_bytes);
+
+/**
+ * release_hash_device - Releases a previously allocated hash device.
+ * @device_data:	Structure for the hash device.
+ *
+ */
+static void release_hash_device(struct hash_device_data *device_data)
+{
+	spin_lock(&device_data->ctx_lock);
+	device_data->current_ctx->device = NULL;
+	device_data->current_ctx = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	/*
+	 * The down_interruptible part for this semaphore is called in
+	 * cryp_get_device_data.
+	 */
+	up(&driver_data.device_allocation);
+}
+
+/**
+ * get_empty_message_digest - Returns a pre-calculated digest for
+ * the empty message.
+ * @device_data:	Structure for the hash device.
+ * @zero_hash:		Buffer to return the empty message digest.
+ * @zero_hash_size:	Hash size of the empty message digest.
+ * @zero_digest:	True if zero_digest returned.
+ */
+static int get_empty_message_digest(
+		struct hash_device_data *device_data,
+		u8 *zero_hash, u32 *zero_hash_size, bool *zero_digest)
+{
+	int ret = 0;
+	struct hash_ctx *ctx = device_data->current_ctx;
+	*zero_digest = false;
+
+	/**
+	 * Caller responsible for ctx != NULL.
+	 */
+
+	if (HASH_OPER_MODE_HASH == ctx->config.oper_mode) {
+		if (HASH_ALGO_SHA1 == ctx->config.algorithm) {
+			memcpy(zero_hash, &zero_message_hash_sha1[0],
+					SHA1_DIGEST_SIZE);
+			*zero_hash_size = SHA1_DIGEST_SIZE;
+			*zero_digest = true;
+		} else if (HASH_ALGO_SHA256 ==
+				ctx->config.algorithm) {
+			memcpy(zero_hash, &zero_message_hash_sha256[0],
+					SHA256_DIGEST_SIZE);
+			*zero_hash_size = SHA256_DIGEST_SIZE;
+			*zero_digest = true;
+		} else {
+			dev_err(device_data->dev, "[%s] "
+					"Incorrect algorithm!"
+					, __func__);
+			ret = -EINVAL;
+			goto out;
+		}
+	} else if (HASH_OPER_MODE_HMAC == ctx->config.oper_mode) {
+		if (!ctx->keylen) {
+			if (HASH_ALGO_SHA1 == ctx->config.algorithm) {
+				memcpy(zero_hash, &zero_message_hmac_sha1[0],
+						SHA1_DIGEST_SIZE);
+				*zero_hash_size = SHA1_DIGEST_SIZE;
+				*zero_digest = true;
+			} else if (HASH_ALGO_SHA256 == ctx->config.algorithm) {
+				memcpy(zero_hash, &zero_message_hmac_sha256[0],
+						SHA256_DIGEST_SIZE);
+				*zero_hash_size = SHA256_DIGEST_SIZE;
+				*zero_digest = true;
+			} else {
+				dev_err(device_data->dev, "[%s] "
+						"Incorrect algorithm!"
+						, __func__);
+				ret = -EINVAL;
+				goto out;
+			}
+		} else {
+			dev_dbg(device_data->dev, "[%s] Continue hash "
+					"calculation, since hmac key avalable",
+					__func__);
+		}
+	}
+out:
+
+	return ret;
+}
+
+/**
+ * hash_disable_power - Request to disable power and clock.
+ * @device_data:	Structure for the hash device.
+ * @save_device_state:	If true, saves the current hw state.
+ *
+ * This function request for disabling power (regulator) and clock,
+ * and could also save current hw state.
+ */
+static int hash_disable_power(
+		struct hash_device_data *device_data,
+		bool			save_device_state)
+{
+	int ret = 0;
+	struct device *dev = device_data->dev;
+
+	dev_dbg(dev, "[%s]", __func__);
+
+	spin_lock(&device_data->power_state_lock);
+	if (!device_data->power_state)
+		goto out;
+
+	if (save_device_state && device_data->current_ctx) {
+		hash_save_state(device_data,
+				&device_data->current_ctx->state);
+		device_data->restore_dev_state = true;
+	}
+
+	clk_disable(device_data->clk);
+	ret = ux500_regulator_atomic_disable(device_data->regulator);
+	if (ret)
+		dev_err(dev, "[%s] regulator_disable() failed!", __func__);
+
+	device_data->power_state = false;
+
+out:
+	spin_unlock(&device_data->power_state_lock);
+
+	return ret;
+}
+
+/**
+ * hash_enable_power - Request to enable power and clock.
+ * @device_data:		Structure for the hash device.
+ * @restore_device_state:	If true, restores a previous saved hw state.
+ *
+ * This function request for enabling power (regulator) and clock,
+ * and could also restore a previously saved hw state.
+ */
+static int hash_enable_power(
+		struct hash_device_data *device_data,
+		bool			restore_device_state)
+{
+	int ret = 0;
+	struct device *dev = device_data->dev;
+	dev_dbg(dev, "[%s]", __func__);
+
+	spin_lock(&device_data->power_state_lock);
+	if (!device_data->power_state) {
+		ret = ux500_regulator_atomic_enable(device_data->regulator);
+		if (ret) {
+			dev_err(dev, "[%s]: regulator_enable() failed!",
+					__func__);
+			goto out;
+		}
+		ret = clk_enable(device_data->clk);
+		if (ret) {
+			dev_err(dev, "[%s]: clk_enable() failed!",
+					__func__);
+			ret = ux500_regulator_atomic_disable(
+					device_data->regulator);
+			goto out;
+		}
+		device_data->power_state = true;
+	}
+
+	if (device_data->restore_dev_state) {
+		if (restore_device_state) {
+			device_data->restore_dev_state = false;
+			hash_resume_state(device_data,
+				&device_data->current_ctx->state);
+		}
+	}
+out:
+	spin_unlock(&device_data->power_state_lock);
+
+	return ret;
+}
+
+/**
+ * hash_get_device_data - Checks for an available hash device and return it.
+ * @hash_ctx:		Structure for the hash context.
+ * @device_data:	Structure for the hash device.
+ *
+ * This function check for an available hash device and return it to
+ * the caller.
+ * Note! Caller need to release the device, calling up().
+ */
+static int hash_get_device_data(struct hash_ctx *ctx,
+				struct hash_device_data **device_data)
+{
+	int			ret;
+	struct klist_iter	device_iterator;
+	struct klist_node	*device_node;
+	struct hash_device_data *local_device_data = NULL;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	/* Wait until a device is available */
+	ret = down_interruptible(&driver_data.device_allocation);
+	if (ret)
+		return ret;  /* Interrupted */
+
+	/* Select a device */
+	klist_iter_init(&driver_data.device_list, &device_iterator);
+	device_node = klist_next(&device_iterator);
+	while (device_node) {
+		local_device_data = container_of(device_node,
+					   struct hash_device_data, list_node);
+		spin_lock(&local_device_data->ctx_lock);
+		/* current_ctx allocates a device, NULL = unallocated */
+		if (local_device_data->current_ctx) {
+			device_node = klist_next(&device_iterator);
+		} else {
+			local_device_data->current_ctx = ctx;
+			ctx->device = local_device_data;
+			spin_unlock(&local_device_data->ctx_lock);
+			break;
+		}
+		spin_unlock(&local_device_data->ctx_lock);
+	}
+	klist_iter_exit(&device_iterator);
+
+	if (!device_node) {
+		/**
+		 * No free device found.
+		 * Since we allocated a device with down_interruptible, this
+		 * should not be able to happen.
+		 * Number of available devices, which are contained in
+		 * device_allocation, is therefore decremented by not doing
+		 * an up(device_allocation).
+		 */
+		return -EBUSY;
+	}
+
+	*device_data = local_device_data;
+
+	return 0;
+}
+
+/**
+ * hash_hw_write_key - Writes the key to the hardware registries.
+ *
+ * @device_data:	Structure for the hash device.
+ * @key:		Key to be written.
+ * @keylen:		The lengt of the key.
+ *
+ * Note! This function DOES NOT write to the NBLW registry, even though
+ * specified in the the hw design spec. Either due to incorrect info in the
+ * spec or due to a bug in the hw.
+ */
+static void hash_hw_write_key(struct hash_device_data *device_data,
+		const u8 *key, unsigned int keylen)
+{
+	u32 word = 0;
+
+	HASH_CLEAR_BITS(&device_data->base->str, HASH_STR_NBLW_MASK);
+	while (keylen >= 4) {
+		word = ((u32) (key[3] & 0xff) << 24) |
+			((u32) (key[2] & 0xff) << 16) |
+			((u32) (key[1] & 0xff) << 8) |
+			((u32) (key[0] & 0xff));
+
+		HASH_SET_DIN(word);
+		keylen -= 4;
+		key += 4;
+	}
+
+	/* Take care of the remaining bytes in the last word */
+	if (keylen) {
+		word = 0;
+		while (keylen) {
+			word |= (key[keylen - 1] << (8 * (keylen - 1)));
+			keylen--;
+		}
+		HASH_SET_DIN(word);
+	}
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	HASH_SET_DCAL;
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+}
+
+/**
+ * init_hash_hw - Initialise the hash hardware for a new calculation.
+ * @device_data:	Structure for the hash device.
+ * @ctx:		The hash context.
+ *
+ * This function will enable the bits needed to clear and start a new
+ * calculation.
+ */
+static int init_hash_hw(struct hash_device_data *device_data,
+		struct hash_ctx *ctx)
+{
+	int ret = 0;
+
+	dev_dbg(device_data->dev, "[%s] (ctx=0x%x)!", __func__, (u32)ctx);
+
+	ret = hash_setconfiguration(device_data, &ctx->config);
+	if (ret) {
+		dev_err(device_data->dev, "[%s] hash_setconfiguration() "
+				"failed!", __func__);
+		return ret;
+	}
+
+	hash_begin(device_data, ctx);
+
+	if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC)
+		hash_hw_write_key(device_data, ctx->key, ctx->keylen);
+
+	return ret;
+}
+
+/**
+ * hash_init - Common hash init function for SHA1/SHA2 (SHA256).
+ * @req: The hash request for the job.
+ *
+ * Initialize structures.
+ */
+static int hash_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s] data size: %d", __func__, req->nbytes);
+
+	if (!ctx->key)
+		ctx->keylen = 0;
+
+	memset(&ctx->state, 0, sizeof(struct hash_state));
+	ctx->updated = 0;
+	return 0;
+}
+
+/**
+ * hash_processblock - This function processes a single block of 512 bits (64
+ *                     bytes), word aligned, starting at message.
+ * @device_data:	Structure for the hash device.
+ * @message:		Block (512 bits) of message to be written to
+ *			the HASH hardware.
+ *
+ * Reentrancy: Non Re-entrant.
+ */
+static void hash_processblock(
+		struct hash_device_data *device_data,
+		const u32 *message)
+{
+	u32 count;
+
+	/*
+	 * NBLW bits. Reset the number of bits in last word (NBLW).
+	 */
+	HASH_CLEAR_BITS(&device_data->base->str, HASH_STR_NBLW_MASK);
+
+	/*
+	 * Write message data to the HASH_DIN register.
+	 */
+	for (count = 0; count < (HASH_BLOCK_SIZE / sizeof(u32)); count += 4) {
+		HASH_SET_DIN(message[0]);
+		HASH_SET_DIN(message[1]);
+		HASH_SET_DIN(message[2]);
+		HASH_SET_DIN(message[3]);
+		message += 4;
+	}
+}
+
+/**
+ * hash_messagepad - Pads a message and write the nblw bits.
+ * @device_data:	Structure for the hash device.
+ * @message:		Last word of a message.
+ * @index_bytes:	The number of bytes in the last message.
+ *
+ * This function manages the final part of the digest calculation, when less
+ * than 512 bits (64 bytes) remain in message. This means index_bytes < 64.
+ *
+ * Reentrancy: Non Re-entrant.
+ */
+static void hash_messagepad(struct hash_device_data *device_data,
+		const u32 *message, u8 index_bytes)
+{
+	dev_dbg(device_data->dev, "[%s] (bytes in final msg=%d))",
+			__func__, index_bytes);
+
+	/*
+	 * Clear hash str register, only clear NBLW
+	 * since DCAL will be reset by hardware.
+	 */
+	writel((readl(&device_data->base->str) & ~HASH_STR_NBLW_MASK),
+			&device_data->base->str);
+
+	/* Main loop */
+	while (index_bytes >= 4) {
+		HASH_SET_DIN(message[0]);
+		index_bytes -= 4;
+		message++;
+	}
+
+	if (index_bytes)
+		HASH_SET_DIN(message[0]);
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	/* num_of_bytes == 0 => NBLW <- 0 (32 bits valid in DATAIN) */
+	HASH_SET_NBLW(index_bytes * 8);
+	dev_dbg(device_data->dev, "[%s] DIN=0x%08x NBLW=%d", __func__,
+			readl(&device_data->base->din),
+			(int)(readl(&device_data->base->str) &
+				HASH_STR_NBLW_MASK));
+	HASH_SET_DCAL;
+	dev_dbg(device_data->dev, "[%s] after dcal -> DIN=0x%08x NBLW=%d",
+			__func__, readl(&device_data->base->din),
+			(int)(readl(&device_data->base->str) &
+				HASH_STR_NBLW_MASK));
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+}
+
+/**
+ * hash_incrementlength - Increments the length of the current message.
+ * @ctx: Hash context
+ * @incr: Length of message processed already
+ *
+ * Overflow cannot occur, because conditions for overflow are checked in
+ * hash_hw_update.
+ */
+static void hash_incrementlength(struct hash_ctx *ctx, u32 incr)
+{
+	ctx->state.length.low_word += incr;
+
+	/* Check for wrap-around */
+	if (ctx->state.length.low_word < incr)
+		ctx->state.length.high_word++;
+}
+
+/**
+ * hash_setconfiguration - Sets the required configuration for the hash
+ *                         hardware.
+ * @device_data:	Structure for the hash device.
+ * @config:		Pointer to a configuration structure.
+ *
+ * Reentrancy: Non Re-entrant
+ * Reentrancy issues:
+ *              1. Global variable registry(cofiguration register,
+ *                 parameter register, divider register) is being modified
+ *
+ * Comments     1.  : User need to call hash_begin API after calling this
+ *                    API i.e. the current configuration is set only when
+ *                    bit INIT is set and we set INIT bit in hash_begin.
+ *                    Changing the configuration during a computation has
+ *                    no effect so we first set configuration by calling
+ *                    this API and then set the INIT bit for the HASH
+ *                    processor and the curent configuration is taken into
+ *                    account. As reading INIT bit (with correct protection
+ *                    rights) will always return 0b so we can't make a check
+ *                    at software level. So the user has to initialize the
+ *                    device for new configuration to take in to effect.
+ *              2.    The default value of data format is 00b ie the format
+ *                    of data entered in HASH_DIN register is 32-bit data.
+ *                    The data written in HASH_DIN is used directly by the
+ *                    HASH processing, without re ordering.
+ */
+int hash_setconfiguration(struct hash_device_data *device_data,
+		struct hash_config *config)
+{
+	int ret = 0;
+	dev_dbg(device_data->dev, "[%s] ", __func__);
+
+	if (config->algorithm != HASH_ALGO_SHA1 &&
+	    config->algorithm != HASH_ALGO_SHA256)
+		return -EPERM;
+
+	/*
+	 * DATAFORM bits. Set the DATAFORM bits to 0b11, which means the data
+	 * to be written to HASH_DIN is considered as 32 bits.
+	 */
+	HASH_SET_DATA_FORMAT(config->data_format);
+
+	/*
+	 * Empty message bit. This bit is needed when the hash input data
+	 * contain the empty message. Always set in current impl. but with
+	 * no impact on data different than empty message.
+	 */
+	HASH_SET_BITS(&device_data->base->cr, HASH_CR_EMPTYMSG_MASK);
+
+	/*
+	 * ALGO bit. Set to 0b1 for SHA-1 and 0b0 for SHA-256
+	 */
+	switch (config->algorithm) {
+	case HASH_ALGO_SHA1:
+		HASH_SET_BITS(&device_data->base->cr, HASH_CR_ALGO_MASK);
+		break;
+
+	case HASH_ALGO_SHA256:
+		HASH_CLEAR_BITS(&device_data->base->cr, HASH_CR_ALGO_MASK);
+		break;
+
+	default:
+		dev_err(device_data->dev, "[%s] Incorrect algorithm.",
+				__func__);
+		return -EPERM;
+	}
+
+	/*
+	 * MODE bit. This bit selects between HASH or HMAC mode for the
+	 * selected algorithm. 0b0 = HASH and 0b1 = HMAC.
+	 */
+	if (HASH_OPER_MODE_HASH == config->oper_mode)
+		HASH_CLEAR_BITS(&device_data->base->cr,
+				HASH_CR_MODE_MASK);
+	else if (HASH_OPER_MODE_HMAC == config->oper_mode) {
+		HASH_SET_BITS(&device_data->base->cr,
+				HASH_CR_MODE_MASK);
+		if (device_data->current_ctx->keylen > HASH_BLOCK_SIZE) {
+			/* Truncate key to blocksize */
+			dev_dbg(device_data->dev, "[%s] LKEY set", __func__);
+			HASH_SET_BITS(&device_data->base->cr,
+					HASH_CR_LKEY_MASK);
+		} else {
+			dev_dbg(device_data->dev, "[%s] LKEY cleared",
+					__func__);
+			HASH_CLEAR_BITS(&device_data->base->cr,
+					HASH_CR_LKEY_MASK);
+		}
+	} else {	/* Wrong hash mode */
+		ret = -EPERM;
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+	}
+	return ret;
+}
+
+/**
+ * hash_begin - This routine resets some globals and initializes the hash
+ *              hardware.
+ * @device_data:	Structure for the hash device.
+ * @ctx:		Hash context.
+ *
+ * Reentrancy: Non Re-entrant
+ *
+ * Comments     1.  : User need to call hash_setconfiguration API before
+ *                    calling this API i.e. the current configuration is set
+ *                    only when bit INIT is set and we set INIT bit in
+ *                    hash_begin. Changing the configuration during a
+ *                    computation has no effect so we first set
+ *                    configuration by calling this API and then set the
+ *                    INIT bit for the HASH processor and the current
+ *                    configuration is taken into account. As reading INIT
+ *                    bit (with correct protection rights) will always
+ *                    return 0b so we can't make a check at software level.
+ *                    So the user has to initialize the device for new
+ *                    configuration to take in to effect.
+ */
+void hash_begin(struct hash_device_data *device_data, struct hash_ctx *ctx)
+{
+	/* HW and SW initializations */
+	/* Note: there is no need to initialize buffer and digest members */
+	dev_dbg(device_data->dev, "[%s] ", __func__);
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	/*
+	 * INIT bit. Set this bit to 0b1 to reset the HASH processor core and
+	 * prepare the initialize the HASH accelerator to compute the message
+	 * digest of a new message.
+	 */
+	HASH_INITIALIZE;
+
+	/*
+	 * NBLW bits. Reset the number of bits in last word (NBLW).
+	 */
+	HASH_CLEAR_BITS(&device_data->base->str, HASH_STR_NBLW_MASK);
+}
+
+int hash_process_data(
+		struct hash_device_data *device_data,
+		struct hash_ctx *ctx, int msg_length, u8 *data_buffer,
+		u8 *buffer, u8 *index)
+{
+	int ret = 0;
+	u32 count;
+
+	do {
+		if ((*index + msg_length) < HASH_BLOCK_SIZE) {
+			for (count = 0; count < msg_length; count++) {
+				buffer[*index + count] =
+					*(data_buffer + count);
+			}
+			*index += msg_length;
+			msg_length = 0;
+		} else {
+			if (ctx->updated) {
+				ret = hash_resume_state(device_data,
+						&ctx->state);
+				if (ret) {
+					dev_err(device_data->dev, "[%s] "
+							"hash_resume_state()"
+							" failed!", __func__);
+					goto out;
+				}
+
+			} else {
+				ret = init_hash_hw(device_data, ctx);
+				if (ret) {
+					dev_err(device_data->dev, "[%s] "
+							"init_hash_hw()"
+							" failed!", __func__);
+					goto out;
+				}
+				ctx->updated = 1;
+			}
+			/*
+			 * If 'data_buffer' is four byte aligned and
+			 * local buffer does not have any data, we can
+			 * write data directly from 'data_buffer' to
+			 * HW peripheral, otherwise we first copy data
+			 * to a local buffer
+			 */
+			if ((0 == (((u32)data_buffer) % 4))
+					&& (0 == *index))
+				hash_processblock(device_data,
+						(const u32 *)
+						data_buffer);
+			else {
+				for (count = 0; count <
+						(u32)(HASH_BLOCK_SIZE -
+							*index);
+						count++) {
+					buffer[*index + count] =
+						*(data_buffer + count);
+				}
+				hash_processblock(device_data,
+						(const u32 *)buffer);
+			}
+			hash_incrementlength(ctx, HASH_BLOCK_SIZE);
+			data_buffer += (HASH_BLOCK_SIZE - *index);
+			msg_length -= (HASH_BLOCK_SIZE - *index);
+			*index = 0;
+
+			ret = hash_save_state(device_data,
+					&ctx->state);
+			if (ret) {
+				dev_err(device_data->dev, "[%s] "
+						"hash_save_state()"
+						" failed!", __func__);
+				goto out;
+			}
+		}
+	} while (msg_length != 0);
+out:
+
+	return ret;
+}
+
+/**
+ * hash_hw_update - Updates current HASH computation hashing another part of
+ *                  the message.
+ * @req:	Byte array containing the message to be hashed (caller
+ *		allocated).
+ *
+ * Reentrancy: Non Re-entrant
+ */
+int hash_hw_update(struct ahash_request *req)
+{
+	int ret = 0;
+	u8 index;
+	u8 *buffer;
+	struct hash_device_data *device_data;
+	u8 *data_buffer;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct crypto_hash_walk walk;
+	int msg_length = crypto_hash_walk_first(req, &walk);
+
+	pr_debug(DEV_DBG_NAME " [%s] datalength: %d", __func__, msg_length);
+
+	/* Empty message ("") is correct indata */
+	if (msg_length == 0)
+		return ret;
+
+	index = ctx->state.index;
+	buffer = (u8 *)ctx->state.buffer;
+
+	/* Check if ctx->state.length + msg_length
+	   overflows */
+	if (msg_length >
+	    (ctx->state.length.low_word + msg_length)
+	    && HASH_HIGH_WORD_MAX_VAL ==
+	    (ctx->state.length.high_word)) {
+		dev_err(device_data->dev, "[%s] HASH_MSG_LENGTH_OVERFLOW!",
+				__func__);
+		return -EPERM;
+	}
+
+	ret = hash_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	/* Enable device power (and clock) */
+	ret = hash_enable_power(device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+				"hash_enable_power() failed!", __func__);
+		goto out;
+	}
+
+	/* Main loop */
+	while (0 != msg_length) {
+		data_buffer = walk.data;
+		ret = hash_process_data(device_data, ctx,
+				msg_length, data_buffer, buffer, &index);
+
+		if (ret) {
+			dev_err(device_data->dev, "[%s] hash_internal_hw_"
+					"update() failed!", __func__);
+			goto out_power;
+		}
+
+		msg_length = crypto_hash_walk_done(&walk, 0);
+	}
+
+	ctx->state.index = index;
+
+	dev_dbg(device_data->dev, "[%s] indata length=%d, "
+		"bin=%d))", __func__, ctx->state.index,	ctx->state.bit_index);
+out_power:
+	/* Disable power (and clock) */
+	if (hash_disable_power(device_data, false))
+		dev_err(device_data->dev, "[%s]: "
+				"hash_disable_power() failed!", __func__);
+out:
+	release_hash_device(device_data);
+
+	return ret;
+}
+
+/**
+ * hash_resume_state - Function that resumes the state of an calculation.
+ * @device_data:	Pointer to the device structure.
+ * @device_state:	The state to be restored in the hash hardware
+ *
+ * Reentrancy: Non Re-entrant
+ */
+int hash_resume_state(struct hash_device_data *device_data,
+		const struct hash_state *device_state)
+{
+	u32 temp_cr;
+	s32 count;
+	int hash_mode = HASH_OPER_MODE_HASH;
+
+	dev_dbg(device_data->dev, "[%s] (state(0x%x)))",
+			__func__, (u32) device_state);
+
+	if (NULL == device_state) {
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+		return -EPERM;
+	}
+
+	/* Check correctness of index and length members */
+	if (device_state->index > HASH_BLOCK_SIZE
+	    || (device_state->length.low_word % HASH_BLOCK_SIZE) != 0) {
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+		return -EPERM;
+	}
+
+	/*
+	 * INIT bit. Set this bit to 0b1 to reset the HASH processor core and
+	 * prepare the initialize the HASH accelerator to compute the message
+	 * digest of a new message.
+	 */
+	HASH_INITIALIZE;
+
+	temp_cr = device_state->temp_cr;
+	writel(temp_cr & HASH_CR_RESUME_MASK, &device_data->base->cr);
+
+	if (device_data->base->cr & HASH_CR_MODE_MASK)
+		hash_mode = HASH_OPER_MODE_HMAC;
+	else
+		hash_mode = HASH_OPER_MODE_HASH;
+
+	for (count = 0; count < HASH_CSR_COUNT; count++) {
+		if ((count >= 36) && (hash_mode == HASH_OPER_MODE_HASH))
+			break;
+
+		writel(device_state->csr[count],
+				&device_data->base->csrx[count]);
+	}
+
+	writel(device_state->csfull, &device_data->base->csfull);
+	writel(device_state->csdatain, &device_data->base->csdatain);
+
+	writel(device_state->str_reg, &device_data->base->str);
+	writel(temp_cr, &device_data->base->cr);
+
+	return 0;
+}
+
+/**
+ * hash_save_state - Function that saves the state of hardware.
+ * @device_data:	Pointer to the device structure.
+ * @device_state:	The strucure where the hardware state should be saved.
+ *
+ * Reentrancy: Non Re-entrant
+ */
+int hash_save_state(struct hash_device_data *device_data,
+		struct hash_state *device_state)
+{
+	u32 temp_cr;
+	u32 count;
+	int hash_mode = HASH_OPER_MODE_HASH;
+
+	dev_dbg(device_data->dev, "[%s] state(0x%x)))",
+		 __func__, (u32) device_state);
+
+	if (NULL == device_state) {
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+		return -EPERM;
+	}
+
+	/* Write dummy value to force digest intermediate calculation. This
+	 * actually makes sure that there isn't any ongoing calculation in the
+	 * hardware.
+	 */
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	temp_cr = readl(&device_data->base->cr);
+
+	device_state->str_reg = readl(&device_data->base->str);
+
+	device_state->din_reg = readl(&device_data->base->din);
+
+	if (device_data->base->cr & HASH_CR_MODE_MASK)
+		hash_mode = HASH_OPER_MODE_HMAC;
+	else
+		hash_mode = HASH_OPER_MODE_HASH;
+
+	for (count = 0; count < HASH_CSR_COUNT; count++) {
+		if ((count >= 36) && (hash_mode == HASH_OPER_MODE_HASH))
+			break;
+
+		device_state->csr[count] =
+			readl(&device_data->base->csrx[count]);
+	}
+
+	device_state->csfull = readl(&device_data->base->csfull);
+	device_state->csdatain = readl(&device_data->base->csdatain);
+
+	device_state->temp_cr = temp_cr;
+
+	return 0;
+}
+
+/**
+ * hash_check_hw - This routine checks for peripheral Ids and PCell Ids.
+ * @device_data:
+ *
+ */
+int hash_check_hw(struct hash_device_data *device_data)
+{
+	int ret = 0;
+
+	dev_dbg(device_data->dev, "[%s] ", __func__);
+
+	if (NULL == device_data) {
+		ret = -EPERM;
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+			__func__);
+		goto out;
+	}
+
+	/* Checking Peripheral Ids  */
+	if ((HASH_P_ID0 == readl(&device_data->base->periphid0))
+		&& (HASH_P_ID1 == readl(&device_data->base->periphid1))
+		&& (HASH_P_ID2 == readl(&device_data->base->periphid2))
+		&& (HASH_P_ID3 == readl(&device_data->base->periphid3))
+		&& (HASH_CELL_ID0 == readl(&device_data->base->cellid0))
+		&& (HASH_CELL_ID1 == readl(&device_data->base->cellid1))
+		&& (HASH_CELL_ID2 == readl(&device_data->base->cellid2))
+		&& (HASH_CELL_ID3 == readl(&device_data->base->cellid3))
+	   ) {
+		ret = 0;
+		goto out;;
+	} else {
+		ret = -EPERM;
+		dev_err(device_data->dev, "[%s] HASH_UNSUPPORTED_HW!",
+				__func__);
+		goto out;
+	}
+out:
+	return ret;
+}
+
+/**
+ * hash_get_digest - Gets the digest.
+ * @device_data:	Pointer to the device structure.
+ * @digest:		User allocated byte array for the calculated digest.
+ * @algorithm:		The algorithm in use.
+ *
+ * Reentrancy: Non Re-entrant, global variable registry (hash control register)
+ * is being modified.
+ *
+ * Note that, if this is called before the final message has been handle it
+ * will return the intermediate message digest.
+ */
+void hash_get_digest(struct hash_device_data *device_data,
+		u8 *digest, int algorithm)
+{
+	u32 temp_hx_val, count;
+	int loop_ctr;
+
+	if (algorithm != HASH_ALGO_SHA1 && algorithm != HASH_ALGO_SHA256) {
+		dev_err(device_data->dev, "[%s] Incorrect algorithm %d",
+				__func__, algorithm);
+		return;
+	}
+
+	if (algorithm == HASH_ALGO_SHA1)
+		loop_ctr = SHA1_DIGEST_SIZE / sizeof(u32);
+	else
+		loop_ctr = SHA256_DIGEST_SIZE / sizeof(u32);
+
+	dev_dbg(device_data->dev, "[%s] digest array:(0x%x)",
+			__func__, (u32) digest);
+
+	/* Copy result into digest array */
+	for (count = 0; count < loop_ctr; count++) {
+		temp_hx_val = readl(&device_data->base->hx[count]);
+		digest[count * 4] = (u8) ((temp_hx_val >> 24) & 0xFF);
+		digest[count * 4 + 1] = (u8) ((temp_hx_val >> 16) & 0xFF);
+		digest[count * 4 + 2] = (u8) ((temp_hx_val >> 8) & 0xFF);
+		digest[count * 4 + 3] = (u8) ((temp_hx_val >> 0) & 0xFF);
+	}
+}
+
+/**
+ * hash_update - The hash update function for SHA1/SHA2 (SHA256).
+ * @req: The hash request for the job.
+ */
+static int ahash_update(struct ahash_request *req)
+{
+	int ret = 0;
+
+	pr_debug(DEV_DBG_NAME " [%s] ", __func__);
+
+	ret = hash_hw_update(req);
+	if (ret) {
+		pr_err(DEV_DBG_NAME " [%s] hash_hw_update() failed!",
+				__func__);
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+/**
+ * hash_final - The hash final function for SHA1/SHA2 (SHA256).
+ * @req:	The hash request for the job.
+ */
+static int ahash_final(struct ahash_request *req)
+{
+	int ret = 0;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct hash_device_data *device_data;
+	u8 digest[SHA256_DIGEST_SIZE];
+
+	pr_debug(DEV_DBG_NAME " [%s] ", __func__);
+
+	ret = hash_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	dev_dbg(device_data->dev, "[%s] (ctx=0x%x)!", __func__, (u32) ctx);
+
+	/* Enable device power (and clock) */
+	ret = hash_enable_power(device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+				"hash_enable_power() failed!", __func__);
+		goto out;
+	}
+
+	if (ctx->updated) {
+		ret = hash_resume_state(device_data, &ctx->state);
+
+		if (ret) {
+			dev_err(device_data->dev, "[%s] hash_resume_state() "
+					"failed!", __func__);
+			goto out_power;
+		}
+	} else if (req->nbytes == 0 && ctx->keylen == 0) {
+		u8 zero_hash[SHA256_DIGEST_SIZE];
+		u32 zero_hash_size = 0;
+		bool zero_digest = false;
+		/**
+		 * Use a pre-calculated empty message digest
+		 * (workaround since hw return zeroes, hw bug!?)
+		 */
+		ret = get_empty_message_digest(device_data, &zero_hash[0],
+				&zero_hash_size, &zero_digest);
+		if (!ret && likely(zero_hash_size == ctx->digestsize) &&
+				zero_digest) {
+			memcpy(req->result, &zero_hash[0], ctx->digestsize);
+			goto out_power;
+		} else if (!ret && !zero_digest) {
+			dev_dbg(device_data->dev, "[%s] HMAC zero msg with "
+					"key, continue...", __func__);
+		} else {
+			dev_err(device_data->dev, "[%s] ret=%d, or wrong "
+					"digest size? %s", __func__, ret,
+					(zero_hash_size == ctx->digestsize) ?
+					"true" : "false");
+			/* Return error */
+			goto out_power;
+		}
+	}
+
+	if (!ctx->updated) {
+		ret = init_hash_hw(device_data, ctx);
+		if (ret) {
+			dev_err(device_data->dev, "[%s] init_hash_hw() "
+					"failed!", __func__);
+			goto out_power;
+		}
+	}
+
+	if (ctx->state.index)
+		hash_messagepad(device_data, ctx->state.buffer,
+				ctx->state.index);
+
+	if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC && ctx->key) {
+		unsigned int keylen = ctx->keylen;
+		u8 *key = ctx->key;
+
+		dev_dbg(device_data->dev, "[%s] keylen: %d", __func__,
+				ctx->keylen);
+		hash_hw_write_key(device_data, key, keylen);
+	}
+
+	hash_get_digest(device_data, digest, ctx->config.algorithm);
+	memcpy(req->result, digest, ctx->digestsize);
+
+out_power:
+	/* Disable power (and clock) */
+	if (hash_disable_power(device_data, false))
+		dev_err(device_data->dev, "[%s] hash_disable_power() failed!",
+				__func__);
+
+out:
+	release_hash_device(device_data);
+
+	/**
+	 * Allocated in setkey, and only used in HMAC.
+	 */
+	kfree(ctx->key);
+
+	return ret;
+}
+
+static int hash_setkey(struct crypto_ahash *tfm,
+		const u8 *key, unsigned int keylen, int alg)
+{
+	int ret = 0;
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s] keylen: %d", __func__, keylen);
+
+	/**
+	 * Freed in final.
+	 */
+	ctx->key = kmalloc(keylen, GFP_KERNEL);
+	if (!ctx->key) {
+		pr_err(DEV_DBG_NAME " [%s] Failed to allocate ctx->key "
+		       "for %d\n", __func__, alg);
+		return -ENOMEM;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	return ret;
+ }
+
+static int ahash_sha1_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]: (ctx=0x%x)!", __func__, (u32) ctx);
+
+	ctx->config.data_format = HASH_DATA_8_BITS;
+	ctx->config.algorithm = HASH_ALGO_SHA1;
+	ctx->config.oper_mode = HASH_OPER_MODE_HASH;
+	ctx->digestsize = SHA1_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int ahash_sha256_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]: (ctx=0x%x)!", __func__, (u32) ctx);
+
+	ctx->config.data_format = HASH_DATA_8_BITS;
+	ctx->config.algorithm = HASH_ALGO_SHA256;
+	ctx->config.oper_mode = HASH_OPER_MODE_HASH;
+	ctx->digestsize = SHA256_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int ahash_sha1_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ret1 = ahash_sha1_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int ahash_sha256_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ret1 = ahash_sha256_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int hmac_sha1_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]: (ctx=0x%x)!", __func__, (u32) ctx);
+
+	ctx->config.data_format	= HASH_DATA_8_BITS;
+	ctx->config.algorithm	= HASH_ALGO_SHA1;
+	ctx->config.oper_mode	= HASH_OPER_MODE_HMAC;
+	ctx->digestsize		= SHA1_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int hmac_sha256_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]: (ctx=0x%x)!", __func__, (u32) ctx);
+
+	ctx->config.data_format	= HASH_DATA_8_BITS;
+	ctx->config.algorithm	= HASH_ALGO_SHA256;
+	ctx->config.oper_mode	= HASH_OPER_MODE_HMAC;
+	ctx->digestsize		= SHA256_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int hmac_sha1_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ret1 = hmac_sha1_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int hmac_sha256_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ret1 = hmac_sha256_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int hmac_sha1_setkey(struct crypto_ahash *tfm,
+		const u8 *key, unsigned int keylen)
+{
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	return hash_setkey(tfm, key, keylen, HASH_ALGO_SHA1);
+}
+
+static int hmac_sha256_setkey(struct crypto_ahash *tfm,
+		const u8 *key, unsigned int keylen)
+{
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	return hash_setkey(tfm, key, keylen, HASH_ALGO_SHA256);
+}
+
+static struct ahash_alg ahash_sha1_alg = {
+	.init			 = ahash_sha1_init,
+	.update			 = ahash_update,
+	.final			 = ahash_final,
+	.digest			 = ahash_sha1_digest,
+	.halg.digestsize	 = SHA1_DIGEST_SIZE,
+	.halg.statesize		 = sizeof(struct hash_ctx),
+	.halg.base = {
+		.cra_name	 = "sha1",
+		.cra_driver_name = "sha1-u8500",
+		.cra_flags	 = CRYPTO_ALG_TYPE_AHASH | CRYPTO_ALG_ASYNC,
+		.cra_blocksize	 = SHA1_BLOCK_SIZE,
+		.cra_ctxsize	 = sizeof(struct hash_ctx),
+		.cra_module	 = THIS_MODULE,
+	}
+};
+
+static struct ahash_alg ahash_sha256_alg = {
+	.init			 = ahash_sha256_init,
+	.update			 = ahash_update,
+	.final			 = ahash_final,
+	.digest			 = ahash_sha256_digest,
+	.halg.digestsize	 = SHA256_DIGEST_SIZE,
+	.halg.statesize		 = sizeof(struct hash_ctx),
+	.halg.base = {
+		.cra_name        = "sha256",
+		.cra_driver_name = "sha256-u8500",
+		.cra_flags       = CRYPTO_ALG_TYPE_AHASH | CRYPTO_ALG_ASYNC,
+		.cra_blocksize   = SHA256_BLOCK_SIZE,
+		.cra_ctxsize	 = sizeof(struct hash_ctx),
+		.cra_type	 = &crypto_ahash_type,
+		.cra_module      = THIS_MODULE,
+	}
+};
+
+static struct ahash_alg hmac_sha1_alg = {
+	.init			 = hmac_sha1_init,
+	.update			 = ahash_update,
+	.final			 = ahash_final,
+	.digest			 = hmac_sha1_digest,
+	.setkey			 = hmac_sha1_setkey,
+	.halg.digestsize	 = SHA1_DIGEST_SIZE,
+	.halg.statesize		 = sizeof(struct hash_ctx),
+	.halg.base = {
+		.cra_name        = "hmac(sha1)",
+		.cra_driver_name = "hmac-sha1-u8500",
+		.cra_flags       = CRYPTO_ALG_TYPE_AHASH | CRYPTO_ALG_ASYNC,
+		.cra_blocksize   = SHA1_BLOCK_SIZE,
+		.cra_ctxsize	 = sizeof(struct hash_ctx),
+		.cra_type	 = &crypto_ahash_type,
+		.cra_module      = THIS_MODULE,
+	}
+};
+
+static struct ahash_alg hmac_sha256_alg = {
+	.init			 = hmac_sha256_init,
+	.update			 = ahash_update,
+	.final			 = ahash_final,
+	.digest			 = hmac_sha256_digest,
+	.setkey			 = hmac_sha256_setkey,
+	.halg.digestsize	 = SHA256_DIGEST_SIZE,
+	.halg.statesize		 = sizeof(struct hash_ctx),
+	.halg.base = {
+		.cra_name        = "hmac(sha256)",
+		.cra_driver_name = "hmac-sha256-u8500",
+		.cra_flags       = CRYPTO_ALG_TYPE_AHASH | CRYPTO_ALG_ASYNC,
+		.cra_blocksize   = SHA256_BLOCK_SIZE,
+		.cra_ctxsize	 = sizeof(struct hash_ctx),
+		.cra_type	 = &crypto_ahash_type,
+		.cra_module      = THIS_MODULE,
+	}
+};
+
+/**
+ * struct hash_alg *u8500_hash_algs[] -
+ */
+static struct ahash_alg *u8500_ahash_algs[] = {
+	&ahash_sha1_alg,
+	&ahash_sha256_alg,
+	&hmac_sha1_alg,
+	&hmac_sha256_alg
+};
+
+/**
+ * hash_algs_register_all -
+ */
+static int ahash_algs_register_all(struct hash_device_data *device_data)
+{
+	int ret;
+	int i;
+	int count;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	for (i = 0; i < ARRAY_SIZE(u8500_ahash_algs); i++) {
+		ret = crypto_register_ahash(u8500_ahash_algs[i]);
+		if (ret) {
+			count = i;
+			dev_err(device_data->dev, "[%s] alg registration"
+					" failed",
+				u8500_ahash_algs[i]->halg.base.cra_driver_name);
+			goto unreg;
+		}
+	}
+	return 0;
+unreg:
+	for (i = 0; i < count; i++)
+		crypto_unregister_ahash(u8500_ahash_algs[i]);
+	return ret;
+}
+
+/**
+ * hash_algs_unregister_all -
+ */
+static void ahash_algs_unregister_all(struct hash_device_data *device_data)
+{
+	int i;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	for (i = 0; i < ARRAY_SIZE(u8500_ahash_algs); i++)
+		crypto_unregister_ahash(u8500_ahash_algs[i]);
+}
+
+/**
+ * u8500_hash_probe - Function that probes the hash hardware.
+ * @pdev: The platform device.
+ */
+static int u8500_hash_probe(struct platform_device *pdev)
+{
+	int			ret = 0;
+	struct resource		*res = NULL;
+	struct hash_device_data *device_data;
+	struct device		*dev = &pdev->dev;
+
+	dev_dbg(dev, "[%s] (pdev=0x%x)", __func__, (u32) pdev);
+	device_data = kzalloc(sizeof(struct hash_device_data), GFP_ATOMIC);
+	if (!device_data) {
+		dev_dbg(dev, "[%s] kzalloc() failed!", __func__);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	device_data->dev = dev;
+	device_data->current_ctx = NULL;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_dbg(dev, "[%s] platform_get_resource() failed!", __func__);
+		ret = -ENODEV;
+		goto out_kfree;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (res == NULL) {
+		dev_dbg(dev, "[%s] request_mem_region() failed!", __func__);
+		ret = -EBUSY;
+		goto out_kfree;
+	}
+
+	device_data->base = ioremap(res->start, resource_size(res));
+	if (!device_data->base) {
+		dev_err(dev, "[%s] ioremap() failed!",
+				__func__);
+		ret = -ENOMEM;
+		goto out_free_mem;
+	}
+	spin_lock_init(&device_data->ctx_lock);
+	spin_lock_init(&device_data->power_state_lock);
+
+	/* Enable power for HASH1 hardware block */
+	device_data->regulator = ux500_regulator_get(dev);
+
+	if (IS_ERR(device_data->regulator)) {
+		dev_err(dev, "[%s] regulator_get() failed!", __func__);
+		ret = PTR_ERR(device_data->regulator);
+		device_data->regulator = NULL;
+		goto out_unmap;
+	}
+
+	/* Enable the clock for HASH1 hardware block */
+	device_data->clk = clk_get(dev, NULL);
+	if (IS_ERR(device_data->clk)) {
+		dev_err(dev, "[%s] clk_get() failed!", __func__);
+		ret = PTR_ERR(device_data->clk);
+		goto out_regulator;
+	}
+
+	/* Enable device power (and clock) */
+	ret = hash_enable_power(device_data, false);
+	if (ret) {
+		dev_err(dev, "[%s]: hash_enable_power() failed!", __func__);
+		goto out_clk;
+	}
+
+	ret = hash_check_hw(device_data);
+	if (ret) {
+		dev_err(dev, "[%s] hash_check_hw() failed!", __func__);
+		goto out_power;
+	}
+
+	platform_set_drvdata(pdev, device_data);
+
+	/* Put the new device into the device list... */
+	klist_add_tail(&device_data->list_node, &driver_data.device_list);
+	/* ... and signal that a new device is available. */
+	up(&driver_data.device_allocation);
+
+	ret = ahash_algs_register_all(device_data);
+	if (ret) {
+		dev_err(dev, "[%s] ahash_algs_register_all() "
+				"failed!", __func__);
+		goto out_power;
+	}
+
+	if (hash_disable_power(device_data, false))
+		dev_err(dev, "[%s]: hash_disable_power() failed!", __func__);
+
+	dev_info(dev, "[%s] successfully probed\n", __func__);
+	return 0;
+
+out_power:
+	hash_disable_power(device_data, false);
+
+out_clk:
+	clk_put(device_data->clk);
+
+out_regulator:
+	ux500_regulator_put(device_data->regulator);
+
+out_unmap:
+	iounmap(device_data->base);
+
+out_free_mem:
+	release_mem_region(res->start, resource_size(res));
+
+out_kfree:
+	kfree(device_data);
+out:
+	return ret;
+}
+
+/**
+ * u8500_hash_remove - Function that removes the hash device from the platform.
+ * @pdev: The platform device.
+ */
+static int u8500_hash_remove(struct platform_device *pdev)
+{
+	struct resource		*res;
+	struct hash_device_data *device_data;
+	struct device		*dev = &pdev->dev;
+
+	dev_dbg(dev, "[%s] (pdev=0x%x)", __func__, (u32) pdev);
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	/* Try to decrease the number of available devices. */
+	if (down_trylock(&driver_data.device_allocation))
+		return -EBUSY;
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (device_data->current_ctx) {
+		/* The device is busy */
+		spin_unlock(&device_data->ctx_lock);
+		/* Return the device to the pool. */
+		up(&driver_data.device_allocation);
+		return -EBUSY;
+	}
+
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		ahash_algs_unregister_all(device_data);
+
+	if (hash_disable_power(device_data, false))
+		dev_err(dev, "[%s]: hash_disable_power() failed",
+			__func__);
+
+	clk_put(device_data->clk);
+	ux500_regulator_put(device_data->regulator);
+
+	iounmap(device_data->base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, resource_size(res));
+
+	kfree(device_data);
+
+	return 0;
+}
+
+/**
+ * u8500_hash_shutdown - Function that shutdown the hash device.
+ * @pdev: The platform device
+ */
+static void u8500_hash_shutdown(struct platform_device *pdev)
+{
+	struct resource *res = NULL;
+	struct hash_device_data *device_data;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
+				__func__);
+		return;
+	}
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (!device_data->current_ctx) {
+		if (down_trylock(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: Cryp still in use!"
+				"Shutting down anyway...", __func__);
+		/**
+		 * (Allocate the device)
+		 * Need to set this to non-null (dummy) value,
+		 * to avoid usage if context switching.
+		 */
+		device_data->current_ctx++;
+	}
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		ahash_algs_unregister_all(device_data);
+
+	iounmap(device_data->base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, resource_size(res));
+
+	if (hash_disable_power(device_data, false))
+		dev_err(&pdev->dev, "[%s] hash_disable_power() failed",
+				__func__);
+}
+
+/**
+ * u8500_hash_suspend - Function that suspends the hash device.
+ * @pdev:	The platform device.
+ * @state:	-
+ */
+static int u8500_hash_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int ret;
+	struct hash_device_data *device_data;
+	struct hash_ctx *temp_ctx = NULL;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
+				__func__);
+		return -ENOMEM;
+	}
+
+	spin_lock(&device_data->ctx_lock);
+	if (!device_data->current_ctx)
+		device_data->current_ctx++;
+	spin_unlock(&device_data->ctx_lock);
+
+	if (device_data->current_ctx == ++temp_ctx) {
+		if (down_interruptible(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: down_interruptible() "
+					"failed", __func__);
+		ret = hash_disable_power(device_data, false);
+
+	} else
+		ret = hash_disable_power(device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: hash_disable_power()", __func__);
+
+	return ret;
+}
+
+/**
+ * u8500_hash_resume - Function that resume the hash device.
+ * @pdev:	The platform device.
+ */
+static int u8500_hash_resume(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct hash_device_data *device_data;
+	struct hash_ctx *temp_ctx = NULL;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
+				__func__);
+		return -ENOMEM;
+	}
+
+	spin_lock(&device_data->ctx_lock);
+	if (device_data->current_ctx == ++temp_ctx)
+		device_data->current_ctx = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	if (!device_data->current_ctx)
+		up(&driver_data.device_allocation);
+	else
+		ret = hash_enable_power(device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: hash_enable_power() failed!",
+			__func__);
+
+	return ret;
+}
+
+static struct platform_driver hash_driver = {
+	.probe  = u8500_hash_probe,
+	.remove = u8500_hash_remove,
+	.shutdown = u8500_hash_shutdown,
+	.suspend  = u8500_hash_suspend,
+	.resume   = u8500_hash_resume,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name  = "hash1",
+	}
+};
+
+/**
+ * u8500_hash_mod_init - The kernel module init function.
+ */
+static int __init u8500_hash_mod_init(void)
+{
+	pr_debug(DEV_DBG_NAME " [%s] is called!", __func__);
+
+	klist_init(&driver_data.device_list, NULL, NULL);
+	/* Initialize the semaphore to 0 devices (locked state) */
+	sema_init(&driver_data.device_allocation, 0);
+
+	return platform_driver_register(&hash_driver);
+}
+
+/**
+ * u8500_hash_mod_fini - The kernel module exit function.
+ */
+static void __exit u8500_hash_mod_fini(void)
+{
+	pr_debug(DEV_DBG_NAME " [%s] is called!", __func__);
+
+	platform_driver_unregister(&hash_driver);
+	return;
+}
+
+module_init(u8500_hash_mod_init);
+module_exit(u8500_hash_mod_fini);
+
+MODULE_DESCRIPTION("Driver for ST-Ericsson U8500 HASH engine.");
+MODULE_LICENSE("GPL");
+
+MODULE_ALIAS("sha1-all");
+MODULE_ALIAS("sha256-all");
+MODULE_ALIAS("hmac-sha1-all");
+MODULE_ALIAS("hmac-sha256-all");
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index 8f222d4db7d..74b560fcfc7 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -13,6 +13,7 @@
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/err.h>
+#include <linux/amba/bus.h>
 
 #include <plat/ste_dma40.h>
 
@@ -44,9 +45,6 @@
 #define D40_ALLOC_PHY		(1 << 30)
 #define D40_ALLOC_LOG_FREE	0
 
-/* Hardware designer of the block */
-#define D40_HW_DESIGNER 0x8
-
 /**
  * enum 40_command - The different commands and/or statuses.
  *
@@ -185,6 +183,8 @@ struct d40_base;
  * @log_def: Default logical channel settings.
  * @lcla: Space for one dst src pair for logical channel transfers.
  * @lcpa: Pointer to dst and src lcpa settings.
+ * @runtime_addr: runtime configured address.
+ * @runtime_direction: runtime configured direction.
  *
  * This struct can either "be" a logical or a physical channel.
  */
@@ -2091,7 +2091,7 @@ dma40_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t dma_addr,
 	struct scatterlist *sg;
 	int i;
 
-	sg = kcalloc(periods + 1, sizeof(struct scatterlist), GFP_KERNEL);
+	sg = kcalloc(periods + 1, sizeof(struct scatterlist), GFP_NOWAIT);
 	for (i = 0; i < periods; i++) {
 		sg_dma_address(&sg[i]) = dma_addr;
 		sg_dma_len(&sg[i]) = period_len;
@@ -2158,17 +2158,78 @@ static void d40_issue_pending(struct dma_chan *chan)
 	spin_unlock_irqrestore(&d40c->lock, flags);
 }
 
+static int
+dma40_config_to_halfchannel(struct d40_chan *d40c,
+			    struct stedma40_half_channel_info *info,
+			    enum dma_slave_buswidth width,
+			    u32 maxburst)
+{
+	enum stedma40_periph_data_width addr_width;
+	int psize;
+
+	switch (width) {
+	case DMA_SLAVE_BUSWIDTH_1_BYTE:
+		addr_width = STEDMA40_BYTE_WIDTH;
+		break;
+	case DMA_SLAVE_BUSWIDTH_2_BYTES:
+		addr_width = STEDMA40_HALFWORD_WIDTH;
+		break;
+	case DMA_SLAVE_BUSWIDTH_4_BYTES:
+		addr_width = STEDMA40_WORD_WIDTH;
+		break;
+	case DMA_SLAVE_BUSWIDTH_8_BYTES:
+		addr_width = STEDMA40_DOUBLEWORD_WIDTH;
+		break;
+	default:
+		dev_err(d40c->base->dev,
+			"illegal peripheral address width "
+			"requested (%d)\n",
+			width);
+		return -EINVAL;
+	}
+
+	if (chan_is_logical(d40c)) {
+		if (maxburst >= 16)
+			psize = STEDMA40_PSIZE_LOG_16;
+		else if (maxburst >= 8)
+			psize = STEDMA40_PSIZE_LOG_8;
+		else if (maxburst >= 4)
+			psize = STEDMA40_PSIZE_LOG_4;
+		else
+			psize = STEDMA40_PSIZE_LOG_1;
+	} else {
+		if (maxburst >= 16)
+			psize = STEDMA40_PSIZE_PHY_16;
+		else if (maxburst >= 8)
+			psize = STEDMA40_PSIZE_PHY_8;
+		else if (maxburst >= 4)
+			psize = STEDMA40_PSIZE_PHY_4;
+		else
+			psize = STEDMA40_PSIZE_PHY_1;
+	}
+
+	info->data_width = addr_width;
+	info->psize = psize;
+	info->flow_ctrl = STEDMA40_NO_FLOW_CTRL;
+
+	return 0;
+}
+
 /* Runtime reconfiguration extension */
-static void d40_set_runtime_config(struct dma_chan *chan,
-			       struct dma_slave_config *config)
+static int d40_set_runtime_config(struct dma_chan *chan,
+				  struct dma_slave_config *config)
 {
 	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
 	struct stedma40_chan_cfg *cfg = &d40c->dma_cfg;
-	enum dma_slave_buswidth config_addr_width;
+	enum dma_slave_buswidth src_addr_width, dst_addr_width;
 	dma_addr_t config_addr;
-	u32 config_maxburst;
-	enum stedma40_periph_data_width addr_width;
-	int psize;
+	u32 src_maxburst, dst_maxburst;
+	int ret;
+
+	src_addr_width = config->src_addr_width;
+	src_maxburst = config->src_maxburst;
+	dst_addr_width = config->dst_addr_width;
+	dst_maxburst = config->dst_maxburst;
 
 	if (config->direction == DMA_FROM_DEVICE) {
 		dma_addr_t dev_addr_rx =
@@ -2187,8 +2248,11 @@ static void d40_set_runtime_config(struct dma_chan *chan,
 				cfg->dir);
 		cfg->dir = STEDMA40_PERIPH_TO_MEM;
 
-		config_addr_width = config->src_addr_width;
-		config_maxburst = config->src_maxburst;
+		/* Configure the memory side */
+		if (dst_addr_width == DMA_SLAVE_BUSWIDTH_UNDEFINED)
+			dst_addr_width = src_addr_width;
+		if (dst_maxburst == 0)
+			dst_maxburst = src_maxburst;
 
 	} else if (config->direction == DMA_TO_DEVICE) {
 		dma_addr_t dev_addr_tx =
@@ -2207,68 +2271,39 @@ static void d40_set_runtime_config(struct dma_chan *chan,
 				cfg->dir);
 		cfg->dir = STEDMA40_MEM_TO_PERIPH;
 
-		config_addr_width = config->dst_addr_width;
-		config_maxburst = config->dst_maxburst;
-
+		/* Configure the memory side */
+		if (src_addr_width == DMA_SLAVE_BUSWIDTH_UNDEFINED)
+			src_addr_width = dst_addr_width;
+		if (src_maxburst == 0)
+			src_maxburst = dst_maxburst;
 	} else {
 		dev_err(d40c->base->dev,
 			"unrecognized channel direction %d\n",
 			config->direction);
-		return;
+		return -EINVAL;
 	}
 
-	switch (config_addr_width) {
-	case DMA_SLAVE_BUSWIDTH_1_BYTE:
-		addr_width = STEDMA40_BYTE_WIDTH;
-		break;
-	case DMA_SLAVE_BUSWIDTH_2_BYTES:
-		addr_width = STEDMA40_HALFWORD_WIDTH;
-		break;
-	case DMA_SLAVE_BUSWIDTH_4_BYTES:
-		addr_width = STEDMA40_WORD_WIDTH;
-		break;
-	case DMA_SLAVE_BUSWIDTH_8_BYTES:
-		addr_width = STEDMA40_DOUBLEWORD_WIDTH;
-		break;
-	default:
+	if (src_maxburst * src_addr_width != dst_maxburst * dst_addr_width) {
 		dev_err(d40c->base->dev,
-			"illegal peripheral address width "
-			"requested (%d)\n",
-			config->src_addr_width);
-		return;
+			"src/dst width/maxburst mismatch: %d*%d != %d*%d\n",
+			src_maxburst,
+			src_addr_width,
+			dst_maxburst,
+			dst_addr_width);
+		return -EINVAL;
 	}
 
-	if (chan_is_logical(d40c)) {
-		if (config_maxburst >= 16)
-			psize = STEDMA40_PSIZE_LOG_16;
-		else if (config_maxburst >= 8)
-			psize = STEDMA40_PSIZE_LOG_8;
-		else if (config_maxburst >= 4)
-			psize = STEDMA40_PSIZE_LOG_4;
-		else
-			psize = STEDMA40_PSIZE_LOG_1;
-	} else {
-		if (config_maxburst >= 16)
-			psize = STEDMA40_PSIZE_PHY_16;
-		else if (config_maxburst >= 8)
-			psize = STEDMA40_PSIZE_PHY_8;
-		else if (config_maxburst >= 4)
-			psize = STEDMA40_PSIZE_PHY_4;
-		else if (config_maxburst >= 2)
-			psize = STEDMA40_PSIZE_PHY_2;
-		else
-			psize = STEDMA40_PSIZE_PHY_1;
-	}
+	ret = dma40_config_to_halfchannel(d40c, &cfg->src_info,
+					  src_addr_width,
+					  src_maxburst);
+	if (ret)
+		return ret;
 
-	/* Set up all the endpoint configs */
-	cfg->src_info.data_width = addr_width;
-	cfg->src_info.psize = psize;
-	cfg->src_info.big_endian = false;
-	cfg->src_info.flow_ctrl = STEDMA40_NO_FLOW_CTRL;
-	cfg->dst_info.data_width = addr_width;
-	cfg->dst_info.psize = psize;
-	cfg->dst_info.big_endian = false;
-	cfg->dst_info.flow_ctrl = STEDMA40_NO_FLOW_CTRL;
+	ret = dma40_config_to_halfchannel(d40c, &cfg->dst_info,
+					  dst_addr_width,
+					  dst_maxburst);
+	if (ret)
+		return ret;
 
 	/* Fill in register values */
 	if (chan_is_logical(d40c))
@@ -2281,12 +2316,14 @@ static void d40_set_runtime_config(struct dma_chan *chan,
 	d40c->runtime_addr = config_addr;
 	d40c->runtime_direction = config->direction;
 	dev_dbg(d40c->base->dev,
-		"configured channel %s for %s, data width %d, "
-		"maxburst %d bytes, LE, no flow control\n",
+		"configured channel %s for %s, data width %d/%d, "
+		"maxburst %d/%d elements, LE, no flow control\n",
 		dma_chan_name(chan),
 		(config->direction == DMA_FROM_DEVICE) ? "RX" : "TX",
-		config_addr_width,
-		config_maxburst);
+		src_addr_width, dst_addr_width,
+		src_maxburst, dst_maxburst);
+
+	return 0;
 }
 
 static int d40_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
@@ -2307,9 +2344,8 @@ static int d40_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 	case DMA_RESUME:
 		return d40_resume(d40c);
 	case DMA_SLAVE_CONFIG:
-		d40_set_runtime_config(chan,
+		return d40_set_runtime_config(chan,
 			(struct dma_slave_config *) arg);
-		return 0;
 	default:
 		break;
 	}
@@ -2501,25 +2537,6 @@ static int __init d40_phy_res_init(struct d40_base *base)
 
 static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 {
-	static const struct d40_reg_val dma_id_regs[] = {
-		/* Peripheral Id */
-		{ .reg = D40_DREG_PERIPHID0, .val = 0x0040},
-		{ .reg = D40_DREG_PERIPHID1, .val = 0x0000},
-		/*
-		 * D40_DREG_PERIPHID2 Depends on HW revision:
-		 *  DB8500ed has 0x0008,
-		 *  ? has 0x0018,
-		 *  DB8500v1 has 0x0028
-		 *  DB8500v2 has 0x0038
-		 */
-		{ .reg = D40_DREG_PERIPHID3, .val = 0x0000},
-
-		/* PCell Id */
-		{ .reg = D40_DREG_CELLID0, .val = 0x000d},
-		{ .reg = D40_DREG_CELLID1, .val = 0x00f0},
-		{ .reg = D40_DREG_CELLID2, .val = 0x0005},
-		{ .reg = D40_DREG_CELLID3, .val = 0x00b1}
-	};
 	struct stedma40_platform_data *plat_data;
 	struct clk *clk = NULL;
 	void __iomem *virtbase = NULL;
@@ -2528,8 +2545,9 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 	int num_log_chans = 0;
 	int num_phy_chans;
 	int i;
-	u32 val;
-	u32 rev;
+	u32 pid;
+	u32 cid;
+	u8 rev;
 
 	clk = clk_get(&pdev->dev, NULL);
 
@@ -2553,32 +2571,32 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 	if (!virtbase)
 		goto failure;
 
-	/* HW version check */
-	for (i = 0; i < ARRAY_SIZE(dma_id_regs); i++) {
-		if (dma_id_regs[i].val !=
-		    readl(virtbase + dma_id_regs[i].reg)) {
-			d40_err(&pdev->dev,
-				"Unknown hardware! Expected 0x%x at 0x%x but got 0x%x\n",
-				dma_id_regs[i].val,
-				dma_id_regs[i].reg,
-				readl(virtbase + dma_id_regs[i].reg));
-			goto failure;
-		}
-	}
-
-	/* Get silicon revision and designer */
-	val = readl(virtbase + D40_DREG_PERIPHID2);
+	/* This is just a regular AMBA PrimeCell ID actually */
+	for (pid = 0, i = 0; i < 4; i++)
+		pid |= (readl(virtbase + resource_size(res) - 0x20 + 4 * i)
+			& 255) << (i * 8);
+	for (cid = 0, i = 0; i < 4; i++)
+		cid |= (readl(virtbase + resource_size(res) - 0x10 + 4 * i)
+			& 255) << (i * 8);
 
-	if ((val & D40_DREG_PERIPHID2_DESIGNER_MASK) !=
-	    D40_HW_DESIGNER) {
+	if (cid != AMBA_CID) {
+		d40_err(&pdev->dev, "Unknown hardware! No PrimeCell ID\n");
+		goto failure;
+	}
+	if (AMBA_MANF_BITS(pid) != AMBA_VENDOR_ST) {
 		d40_err(&pdev->dev, "Unknown designer! Got %x wanted %x\n",
-			val & D40_DREG_PERIPHID2_DESIGNER_MASK,
-			D40_HW_DESIGNER);
+			AMBA_MANF_BITS(pid),
+			AMBA_VENDOR_ST);
 		goto failure;
 	}
-
-	rev = (val & D40_DREG_PERIPHID2_REV_MASK) >>
-		D40_DREG_PERIPHID2_REV_POS;
+	/*
+	 * HW revision:
+	 * DB8500ed has revision 0
+	 * ? has revision 1
+	 * DB8500v1 has revision 2
+	 * DB8500v2 has revision 3
+	 */
+	rev = AMBA_REV_BITS(pid);
 
 	/* The number of physical channels on this HW */
 	num_phy_chans = 4 * (readl(virtbase + D40_DREG_ICFG) & 0x7) + 4;
diff --git a/drivers/dma/ste_dma40_ll.h b/drivers/dma/ste_dma40_ll.h
index 195ee65ee7f..b44c455158d 100644
--- a/drivers/dma/ste_dma40_ll.h
+++ b/drivers/dma/ste_dma40_ll.h
@@ -184,9 +184,6 @@
 #define D40_DREG_PERIPHID0	0xFE0
 #define D40_DREG_PERIPHID1	0xFE4
 #define D40_DREG_PERIPHID2	0xFE8
-#define D40_DREG_PERIPHID2_REV_POS 4
-#define D40_DREG_PERIPHID2_REV_MASK (0xf << D40_DREG_PERIPHID2_REV_POS)
-#define D40_DREG_PERIPHID2_DESIGNER_MASK 0xf
 #define D40_DREG_PERIPHID3	0xFEC
 #define D40_DREG_CELLID0	0xFF0
 #define D40_DREG_CELLID1	0xFF4
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 2967002a9f8..49675fe7121 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -441,9 +441,9 @@ config GPIO_JANZ_TTL
 	  This driver provides support for driving the pins in output
 	  mode only. Input mode is not supported.
 
-config AB8500_GPIO
+config GPIO_AB8500
 	bool "ST-Ericsson AB8500 Mixed Signal Circuit gpio functions"
-	depends on AB8500_CORE && BROKEN
+	depends on AB8500_CORE
 	help
 	  Select this to enable the AB8500 IC GPIO driver
 
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index b605f8ec6fb..4a08af1a0cb 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -46,5 +46,5 @@ obj-$(CONFIG_GPIO_JANZ_TTL)	+= janz-ttl.o
 obj-$(CONFIG_GPIO_SX150X)	+= sx150x.o
 obj-$(CONFIG_GPIO_VX855)	+= vx855_gpio.o
 obj-$(CONFIG_GPIO_ML_IOH)	+= ml_ioh_gpio.o
-obj-$(CONFIG_AB8500_GPIO)       += ab8500-gpio.o
+obj-$(CONFIG_GPIO_AB8500)       += ab8500-gpio.o
 obj-$(CONFIG_GPIO_TPS65910)	+= tps65910-gpio.o
diff --git a/drivers/gpio/ab8500-gpio.c b/drivers/gpio/ab8500-gpio.c
index 970053c89ff..0820f008f65 100644
--- a/drivers/gpio/ab8500-gpio.c
+++ b/drivers/gpio/ab8500-gpio.c
@@ -61,7 +61,7 @@
 #define AB8500_GPIO_IN4_REG	0x43
 #define AB8500_GPIO_IN5_REG	0x44
 #define AB8500_GPIO_IN6_REG	0x45
-#define AB8500_GPIO_ALTFUN_REG	0x45
+#define AB8500_GPIO_ALTFUN_REG	0x50
 #define ALTFUN_REG_INDEX	6
 #define AB8500_NUM_GPIO		42
 #define AB8500_NUM_VIR_GPIO_IRQ	16
@@ -116,7 +116,7 @@ static int ab8500_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
 	struct ab8500_gpio *ab8500_gpio = to_ab8500_gpio(chip);
 	u8 mask = 1 << (offset % 8);
-	u8 reg = AB8500_GPIO_OUT1_REG + (offset / 8);
+	u8 reg = AB8500_GPIO_IN1_REG + (offset / 8);
 	int ret;
 	u8 data;
 	ret = abx500_get_register_interruptible(ab8500_gpio->dev, AB8500_MISC,
@@ -133,7 +133,7 @@ static void ab8500_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 	struct ab8500_gpio *ab8500_gpio = to_ab8500_gpio(chip);
 	int ret;
 	/* Write the data */
-	ret = ab8500_gpio_set_bits(chip, AB8500_GPIO_OUT1_REG, offset, 1);
+	ret = ab8500_gpio_set_bits(chip, AB8500_GPIO_OUT1_REG, offset, val);
 	if (ret < 0)
 		dev_err(ab8500_gpio->dev, "%s write failed\n", __func__);
 }
@@ -208,7 +208,7 @@ static struct gpio_chip ab8500gpio_chip = {
 
 static unsigned int irq_to_rising(unsigned int irq)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_get_chip_data(irq);
 	int offset = irq - ab8500_gpio->irq_base;
 	int new_irq = offset +  AB8500_INT_GPIO6R
 			+ ab8500_gpio->parent->irq_base;
@@ -217,7 +217,7 @@ static unsigned int irq_to_rising(unsigned int irq)
 
 static unsigned int irq_to_falling(unsigned int irq)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_get_chip_data(irq);
 	int offset = irq - ab8500_gpio->irq_base;
 	int new_irq = offset +  AB8500_INT_GPIO6F
 			+  ab8500_gpio->parent->irq_base;
@@ -262,15 +262,16 @@ static irqreturn_t handle_falling(int irq, void *dev)
 	return IRQ_HANDLED;
 }
 
-static void ab8500_gpio_irq_lock(unsigned int irq)
+static void ab8500_gpio_irq_lock(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
 	mutex_lock(&ab8500_gpio->lock);
 }
 
-static void ab8500_gpio_irq_sync_unlock(unsigned int irq)
+static void ab8500_gpio_irq_sync_unlock(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
+	unsigned int irq = data->irq;
 	int offset = irq - ab8500_gpio->irq_base;
 	bool rising = ab8500_gpio->rising & BIT(offset);
 	bool falling = ab8500_gpio->falling & BIT(offset);
@@ -317,21 +318,22 @@ static void ab8500_gpio_irq_sync_unlock(unsigned int irq)
 }
 
 
-static void ab8500_gpio_irq_mask(unsigned int irq)
+static void ab8500_gpio_irq_mask(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
 	ab8500_gpio->irq_action = MASK;
 }
 
-static void ab8500_gpio_irq_unmask(unsigned int irq)
+static void ab8500_gpio_irq_unmask(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio =  irq_data_get_irq_chip_data(data);
 	ab8500_gpio->irq_action = UNMASK;
 }
 
-static int ab8500_gpio_irq_set_type(unsigned int irq, unsigned int type)
+static int ab8500_gpio_irq_set_type(struct irq_data *data, unsigned int type)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
+	unsigned int irq = data->irq;
 	int offset = irq - ab8500_gpio->irq_base;
 
 	if (type == IRQ_TYPE_EDGE_BOTH) {
@@ -345,28 +347,28 @@ static int ab8500_gpio_irq_set_type(unsigned int irq, unsigned int type)
 	return 0;
 }
 
-unsigned int ab8500_gpio_irq_startup(unsigned int irq)
+unsigned int ab8500_gpio_irq_startup(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
 	ab8500_gpio->irq_action = STARTUP;
 	return 0;
 }
 
-void ab8500_gpio_irq_shutdown(unsigned int irq)
+void ab8500_gpio_irq_shutdown(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
 	ab8500_gpio->irq_action = SHUTDOWN;
 }
 
 static struct irq_chip ab8500_gpio_irq_chip = {
 	.name			= "ab8500-gpio",
-	.startup		= ab8500_gpio_irq_startup,
-	.shutdown		= ab8500_gpio_irq_shutdown,
-	.bus_lock		= ab8500_gpio_irq_lock,
-	.bus_sync_unlock	= ab8500_gpio_irq_sync_unlock,
-	.mask			= ab8500_gpio_irq_mask,
-	.unmask			= ab8500_gpio_irq_unmask,
-	.set_type		= ab8500_gpio_irq_set_type,
+	.irq_startup		= ab8500_gpio_irq_startup,
+	.irq_shutdown		= ab8500_gpio_irq_shutdown,
+	.irq_bus_lock		= ab8500_gpio_irq_lock,
+	.irq_bus_sync_unlock	= ab8500_gpio_irq_sync_unlock,
+	.irq_mask		= ab8500_gpio_irq_mask,
+	.irq_unmask		= ab8500_gpio_irq_unmask,
+	.irq_set_type		= ab8500_gpio_irq_set_type,
 };
 
 static int ab8500_gpio_irq_init(struct ab8500_gpio *ab8500_gpio)
@@ -375,14 +377,14 @@ static int ab8500_gpio_irq_init(struct ab8500_gpio *ab8500_gpio)
 	int irq;
 
 	for (irq = base; irq < base + AB8500_NUM_VIR_GPIO_IRQ ; irq++) {
-		set_irq_chip_data(irq, ab8500_gpio);
-		set_irq_chip_and_handler(irq, &ab8500_gpio_irq_chip,
+		irq_set_chip_data(irq, ab8500_gpio);
+		irq_set_chip_and_handler(irq, &ab8500_gpio_irq_chip,
 				handle_simple_irq);
-		set_irq_nested_thread(irq, 1);
+		irq_set_nested_thread(irq, 1);
 #ifdef CONFIG_ARM
 		set_irq_flags(irq, IRQF_VALID);
 #else
-		set_irq_noprobe(irq);
+		irq_set_noprobe(irq);
 #endif
 	}
 
@@ -398,8 +400,8 @@ static void ab8500_gpio_irq_remove(struct ab8500_gpio *ab8500_gpio)
 #ifdef CONFIG_ARM
 		set_irq_flags(irq, 0);
 #endif
-		set_irq_chip_and_handler(irq, NULL, NULL);
-		set_irq_chip_data(irq, NULL);
+		irq_set_chip_and_handler(irq, NULL, NULL);
+		irq_set_chip_data(irq, NULL);
 	}
 }
 
@@ -413,7 +415,7 @@ static int __devinit ab8500_gpio_probe(struct platform_device *pdev)
 	int i;
 
 	pdata = ab8500_pdata->gpio;
-	if (!pdata)	{
+	if (!pdata) {
 		dev_err(&pdev->dev, "gpio platform data missing\n");
 		return -ENODEV;
 	}
@@ -430,24 +432,40 @@ static int __devinit ab8500_gpio_probe(struct platform_device *pdev)
 	ab8500_gpio->chip.dev = &pdev->dev;
 	ab8500_gpio->chip.base = pdata->gpio_base;
 	ab8500_gpio->irq_base = pdata->irq_base;
+
 	/* initialize the lock */
 	mutex_init(&ab8500_gpio->lock);
+
 	/*
 	 * AB8500 core will handle and clear the IRQ
-	 * configre GPIO based on config-reg value.
-	 * These values are for selecting the PINs as
-	 * GPIO or alternate function
+	 * configure GPIO based on initial_pins_{config, direction, pullups}
+	 * value.
+	 * These values are for selecting the PINs as GPIO
+	 * or alternative function
 	 */
-	for (i = AB8500_GPIO_SEL1_REG; i <= AB8500_GPIO_SEL6_REG; i++)	{
+	for (i = AB8500_GPIO_SEL1_REG; i <= AB8500_GPIO_SEL6_REG; i++) {
 		ret = abx500_set_register_interruptible(ab8500_gpio->dev,
 				AB8500_MISC, i,
-				pdata->config_reg[i]);
+				pdata->initial_pin_config[i]);
+		if (ret < 0)
+			goto out_free;
+
+		ret = abx500_set_register_interruptible(ab8500_gpio->dev,
+				AB8500_MISC, i + AB8500_GPIO_DIR1_REG,
+				pdata->initial_pin_direction[i]);
+		if (ret < 0)
+			goto out_free;
+
+		ret = abx500_set_register_interruptible(ab8500_gpio->dev,
+				AB8500_MISC, i + AB8500_GPIO_PUD1_REG,
+				pdata->initial_pin_pullups[i]);
 		if (ret < 0)
 			goto out_free;
 	}
+
 	ret = abx500_set_register_interruptible(ab8500_gpio->dev, AB8500_MISC,
 				AB8500_GPIO_ALTFUN_REG,
-				pdata->config_reg[ALTFUN_REG_INDEX]);
+				pdata->initial_pin_config[ALTFUN_REG_INDEX]);
 	if (ret < 0)
 		goto out_free;
 
@@ -494,6 +512,94 @@ static int __devexit ab8500_gpio_remove(struct platform_device *pdev)
 	return 0;
 }
 
+/*
+ * ab8500_gpio_configpulldown() - configure pull down
+ * Either the function is used to enable pull down (enable true)
+ * or to leave it dangling (enable false) or the function it used
+ * to enable pull up (enable true) or to leave it dangling (enable false)
+ * @pdev  :Platform device registered
+ * @gpio  :gpio number
+ * @enable:pull down enabled (True) or disabled (False)
+ */
+int ab8500_config_pull_up_or_down(struct device *dev,
+				unsigned gpio_offset, bool enable)
+{
+	u8 pos = gpio_offset % 8;
+	u8 val = enable ? 0 : 1;
+	u8 reg = AB8500_GPIO_PUD1_REG + (gpio_offset / 8);
+	int ret;
+
+	ret = abx500_mask_and_set_register_interruptible(dev,
+				AB8500_MISC, reg, 1 << pos, val << pos);
+	if (ret < 0)
+		dev_err(dev, "%s write failed\n", __func__);
+	return ret;
+}
+EXPORT_SYMBOL(ab8500_config_pull_up_or_down);
+
+/*
+ * ab8500_gpio_config_select()
+ *
+ * Configure functionality of pin, either specific use or GPIO.
+ * @dev: device pointer
+ * @gpio: gpio number
+ * @gpio_select: true if the pin should be used as GPIO
+ */
+int ab8500_gpio_config_select(struct device *dev,
+			      unsigned gpio_offset, bool gpio_select)
+{
+	u8 reg = AB8500_GPIO_SEL1_REG + (gpio_offset / 8);
+	u8 pos = gpio_offset % 8;
+	u8 val = gpio_select ? 1 : 0;
+	int ret;
+
+	ret = abx500_mask_and_set_register_interruptible(dev,
+		AB8500_MISC, reg, 1 << pos, val << pos);
+	if (ret < 0)
+		dev_err(dev, "%s write failed\n", __func__);
+
+	dev_vdbg(dev, "%s (bank, addr, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
+		__func__, AB8500_MISC, reg, 1 << pos, val << pos);
+
+	return ret;
+}
+EXPORT_SYMBOL(ab8500_gpio_config_select);
+
+/*
+ * ab8500_gpio_config_get_select()
+ *
+ * Configure functionality of pin, either specific use or GPIO.
+ * @dev: device pointer
+ * @gpio: gpio number
+ * @gpio_select: pointer to pin selection status
+ */
+int ab8500_gpio_config_get_select(struct device *dev,
+				  unsigned gpio_offset, bool *gpio_select)
+{
+	u8 reg = AB8500_GPIO_SEL1_REG + (gpio_offset / 8);
+	u8 pos = gpio_offset % 8;
+	u8 val;
+	int ret;
+
+	ret = abx500_get_register_interruptible(dev,
+				AB8500_MISC, reg, &val);
+	if (ret < 0) {
+		dev_err(dev, "%s read failed\n", __func__);
+		return ret;
+	}
+
+	if (val & (1 << pos))
+		*gpio_select = true;
+	else
+		*gpio_select = false;
+
+	dev_vdbg(dev, "%s (bank, addr, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
+		__func__, AB8500_MISC, reg, 1 << pos, val);
+
+	return 0;
+}
+EXPORT_SYMBOL(ab8500_gpio_config_get_select);
+
 static struct platform_driver ab8500_gpio_driver = {
 	.driver = {
 		.name = "ab8500-gpio",
diff --git a/drivers/gpio/gpio-nomadik.c b/drivers/gpio/gpio-nomadik.c
index 2c212c732d7..7f4786fe5e7 100644
--- a/drivers/gpio/gpio-nomadik.c
+++ b/drivers/gpio/gpio-nomadik.c
@@ -23,9 +23,9 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/gpio/nomadik.h>
 
 #include <asm/mach/irq.h>
-
 #include <plat/pincfg.h>
 #include <mach/hardware.h>
 #include <mach/gpio.h>
@@ -58,7 +58,6 @@ struct nmk_gpio_chip {
 	u32 rwimsc;
 	u32 fwimsc;
 	u32 slpm;
-	u32 enabled;
 	u32 pull_up;
 };
 
@@ -276,6 +275,8 @@ static void nmk_gpio_glitch_slpm_init(unsigned int *slpm)
 		if (!chip)
 			break;
 
+		clk_enable(chip->clk);
+
 		slpm[i] = readl(chip->addr + NMK_GPIO_SLPC);
 		writel(temp, chip->addr + NMK_GPIO_SLPC);
 	}
@@ -292,6 +293,8 @@ static void nmk_gpio_glitch_slpm_restore(unsigned int *slpm)
 			break;
 
 		writel(slpm[i], chip->addr + NMK_GPIO_SLPC);
+
+		clk_disable(chip->clk);
 	}
 }
 
@@ -336,10 +339,12 @@ static int __nmk_config_pins(pin_cfg_t *cfgs, int num, bool sleep)
 			break;
 		}
 
+		clk_enable(nmk_chip->clk);
 		spin_lock(&nmk_chip->lock);
 		__nmk_config_pin(nmk_chip, pin - nmk_chip->chip.base,
 				 cfgs[i], sleep, glitch ? slpm : NULL);
 		spin_unlock(&nmk_chip->lock);
+		clk_disable(nmk_chip->clk);
 	}
 
 	if (glitch)
@@ -353,7 +358,7 @@ static int __nmk_config_pins(pin_cfg_t *cfgs, int num, bool sleep)
 /**
  * nmk_config_pin - configure a pin's mux attributes
  * @cfg: pin confguration
- *
+ * @sleep: Non-zero to apply the sleep mode configuration
  * Configures a pin's mode (alternate function or GPIO), its pull up status,
  * and its sleep mode based on the specified configuration.  The @cfg is
  * usually one of the SoC specific macros defined in mach/<soc>-pins.h.  These
@@ -424,6 +429,7 @@ int nmk_gpio_set_slpm(int gpio, enum nmk_gpio_slpm mode)
 	if (!nmk_chip)
 		return -EINVAL;
 
+	clk_enable(nmk_chip->clk);
 	spin_lock_irqsave(&nmk_gpio_slpm_lock, flags);
 	spin_lock(&nmk_chip->lock);
 
@@ -431,6 +437,7 @@ int nmk_gpio_set_slpm(int gpio, enum nmk_gpio_slpm mode)
 
 	spin_unlock(&nmk_chip->lock);
 	spin_unlock_irqrestore(&nmk_gpio_slpm_lock, flags);
+	clk_disable(nmk_chip->clk);
 
 	return 0;
 }
@@ -457,9 +464,11 @@ int nmk_gpio_set_pull(int gpio, enum nmk_gpio_pull pull)
 	if (!nmk_chip)
 		return -EINVAL;
 
+	clk_enable(nmk_chip->clk);
 	spin_lock_irqsave(&nmk_chip->lock, flags);
 	__nmk_gpio_set_pull(nmk_chip, gpio - nmk_chip->chip.base, pull);
 	spin_unlock_irqrestore(&nmk_chip->lock, flags);
+	clk_disable(nmk_chip->clk);
 
 	return 0;
 }
@@ -483,9 +492,11 @@ int nmk_gpio_set_mode(int gpio, int gpio_mode)
 	if (!nmk_chip)
 		return -EINVAL;
 
+	clk_enable(nmk_chip->clk);
 	spin_lock_irqsave(&nmk_chip->lock, flags);
 	__nmk_gpio_set_mode(nmk_chip, gpio - nmk_chip->chip.base, gpio_mode);
 	spin_unlock_irqrestore(&nmk_chip->lock, flags);
+	clk_disable(nmk_chip->clk);
 
 	return 0;
 }
@@ -502,9 +513,13 @@ int nmk_gpio_get_mode(int gpio)
 
 	bit = 1 << (gpio - nmk_chip->chip.base);
 
+	clk_enable(nmk_chip->clk);
+
 	afunc = readl(nmk_chip->addr + NMK_GPIO_AFSLA) & bit;
 	bfunc = readl(nmk_chip->addr + NMK_GPIO_AFSLB) & bit;
 
+	clk_disable(nmk_chip->clk);
+
 	return (afunc ? NMK_GPIO_ALT_A : 0) | (bfunc ? NMK_GPIO_ALT_B : 0);
 }
 EXPORT_SYMBOL(nmk_gpio_get_mode);
@@ -525,7 +540,10 @@ static void nmk_gpio_irq_ack(struct irq_data *d)
 	nmk_chip = irq_data_get_irq_chip_data(d);
 	if (!nmk_chip)
 		return;
+
+	clk_enable(nmk_chip->clk);
 	writel(nmk_gpio_get_bitmask(gpio), nmk_chip->addr + NMK_GPIO_IC);
+	clk_disable(nmk_chip->clk);
 }
 
 enum nmk_gpio_irq_type {
@@ -586,11 +604,7 @@ static int nmk_gpio_irq_maskunmask(struct irq_data *d, bool enable)
 	if (!nmk_chip)
 		return -EINVAL;
 
-	if (enable)
-		nmk_chip->enabled |= bitmask;
-	else
-		nmk_chip->enabled &= ~bitmask;
-
+	clk_enable(nmk_chip->clk);
 	spin_lock_irqsave(&nmk_gpio_slpm_lock, flags);
 	spin_lock(&nmk_chip->lock);
 
@@ -601,6 +615,7 @@ static int nmk_gpio_irq_maskunmask(struct irq_data *d, bool enable)
 
 	spin_unlock(&nmk_chip->lock);
 	spin_unlock_irqrestore(&nmk_gpio_slpm_lock, flags);
+	clk_disable(nmk_chip->clk);
 
 	return 0;
 }
@@ -628,10 +643,11 @@ static int nmk_gpio_irq_set_wake(struct irq_data *d, unsigned int on)
 		return -EINVAL;
 	bitmask = nmk_gpio_get_bitmask(gpio);
 
+	clk_enable(nmk_chip->clk);
 	spin_lock_irqsave(&nmk_gpio_slpm_lock, flags);
 	spin_lock(&nmk_chip->lock);
 
-	if (!(nmk_chip->enabled & bitmask))
+	if (irqd_irq_disabled(d))
 		__nmk_gpio_set_wake(nmk_chip, gpio, on);
 
 	if (on)
@@ -641,13 +657,15 @@ static int nmk_gpio_irq_set_wake(struct irq_data *d, unsigned int on)
 
 	spin_unlock(&nmk_chip->lock);
 	spin_unlock_irqrestore(&nmk_gpio_slpm_lock, flags);
+	clk_disable(nmk_chip->clk);
 
 	return 0;
 }
 
 static int nmk_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 {
-	bool enabled, wake = irqd_is_wakeup_set(d);
+	bool enabled = !irqd_irq_disabled(d);
+	bool wake = irqd_is_wakeup_set(d);
 	int gpio;
 	struct nmk_gpio_chip *nmk_chip;
 	unsigned long flags;
@@ -664,8 +682,7 @@ static int nmk_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 	if (type & IRQ_TYPE_LEVEL_LOW)
 		return -EINVAL;
 
-	enabled = nmk_chip->enabled & bitmask;
-
+	clk_enable(nmk_chip->clk);
 	spin_lock_irqsave(&nmk_chip->lock, flags);
 
 	if (enabled)
@@ -689,10 +706,28 @@ static int nmk_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 		__nmk_gpio_irq_modify(nmk_chip, gpio, WAKE, true);
 
 	spin_unlock_irqrestore(&nmk_chip->lock, flags);
+	clk_disable(nmk_chip->clk);
+
+	return 0;
+}
+
+static unsigned int nmk_gpio_irq_startup(struct irq_data *d)
+{
+	struct nmk_gpio_chip *nmk_chip = irq_data_get_irq_chip_data(d);
 
+	clk_enable(nmk_chip->clk);
+	nmk_gpio_irq_unmask(d);
 	return 0;
 }
 
+static void nmk_gpio_irq_shutdown(struct irq_data *d)
+{
+	struct nmk_gpio_chip *nmk_chip = irq_data_get_irq_chip_data(d);
+
+	nmk_gpio_irq_mask(d);
+	clk_disable(nmk_chip->clk);
+}
+
 static struct irq_chip nmk_gpio_irq_chip = {
 	.name		= "Nomadik-GPIO",
 	.irq_ack	= nmk_gpio_irq_ack,
@@ -700,6 +735,8 @@ static struct irq_chip nmk_gpio_irq_chip = {
 	.irq_unmask	= nmk_gpio_irq_unmask,
 	.irq_set_type	= nmk_gpio_irq_set_type,
 	.irq_set_wake	= nmk_gpio_irq_set_wake,
+	.irq_startup	= nmk_gpio_irq_startup,
+	.irq_shutdown	= nmk_gpio_irq_shutdown,
 };
 
 static void __nmk_gpio_irq_handler(unsigned int irq, struct irq_desc *desc,
@@ -726,7 +763,11 @@ static void __nmk_gpio_irq_handler(unsigned int irq, struct irq_desc *desc,
 static void nmk_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 {
 	struct nmk_gpio_chip *nmk_chip = irq_get_handler_data(irq);
-	u32 status = readl(nmk_chip->addr + NMK_GPIO_IS);
+	u32 status;
+
+	clk_enable(nmk_chip->clk);
+	status = readl(nmk_chip->addr + NMK_GPIO_IS);
+	clk_disable(nmk_chip->clk);
 
 	__nmk_gpio_irq_handler(irq, desc, status);
 }
@@ -772,7 +813,12 @@ static int nmk_gpio_make_input(struct gpio_chip *chip, unsigned offset)
 	struct nmk_gpio_chip *nmk_chip =
 		container_of(chip, struct nmk_gpio_chip, chip);
 
+	clk_enable(nmk_chip->clk);
+
 	writel(1 << offset, nmk_chip->addr + NMK_GPIO_DIRC);
+
+	clk_disable(nmk_chip->clk);
+
 	return 0;
 }
 
@@ -781,8 +827,15 @@ static int nmk_gpio_get_input(struct gpio_chip *chip, unsigned offset)
 	struct nmk_gpio_chip *nmk_chip =
 		container_of(chip, struct nmk_gpio_chip, chip);
 	u32 bit = 1 << offset;
+	int value;
 
-	return (readl(nmk_chip->addr + NMK_GPIO_DAT) & bit) != 0;
+	clk_enable(nmk_chip->clk);
+
+	value = (readl(nmk_chip->addr + NMK_GPIO_DAT) & bit) != 0;
+
+	clk_disable(nmk_chip->clk);
+
+	return value;
 }
 
 static void nmk_gpio_set_output(struct gpio_chip *chip, unsigned offset,
@@ -791,7 +844,11 @@ static void nmk_gpio_set_output(struct gpio_chip *chip, unsigned offset,
 	struct nmk_gpio_chip *nmk_chip =
 		container_of(chip, struct nmk_gpio_chip, chip);
 
+	clk_enable(nmk_chip->clk);
+
 	__nmk_gpio_set_output(nmk_chip, offset, val);
+
+	clk_disable(nmk_chip->clk);
 }
 
 static int nmk_gpio_make_output(struct gpio_chip *chip, unsigned offset,
@@ -800,8 +857,12 @@ static int nmk_gpio_make_output(struct gpio_chip *chip, unsigned offset,
 	struct nmk_gpio_chip *nmk_chip =
 		container_of(chip, struct nmk_gpio_chip, chip);
 
+	clk_enable(nmk_chip->clk);
+
 	__nmk_gpio_make_output(nmk_chip, offset, val);
 
+	clk_disable(nmk_chip->clk);
+
 	return 0;
 }
 
@@ -832,6 +893,8 @@ static void nmk_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 		[NMK_GPIO_ALT_C]	= "altC",
 	};
 
+	clk_enable(nmk_chip->clk);
+
 	for (i = 0; i < chip->ngpio; i++, gpio++) {
 		const char *label = gpiochip_is_requested(chip, i);
 		bool pull;
@@ -876,6 +939,8 @@ static void nmk_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 
 		seq_printf(s, "\n");
 	}
+
+	clk_disable(nmk_chip->clk);
 }
 
 #else
@@ -893,6 +958,34 @@ static struct gpio_chip nmk_gpio_template = {
 	.can_sleep		= 0,
 };
 
+void nmk_gpio_clocks_enable(void)
+{
+	int i;
+
+	for (i = 0; i < NUM_BANKS; i++) {
+		struct nmk_gpio_chip *chip = nmk_gpio_chips[i];
+
+		if (!chip)
+			continue;
+
+		clk_enable(chip->clk);
+	}
+}
+
+void nmk_gpio_clocks_disable(void)
+{
+	int i;
+
+	for (i = 0; i < NUM_BANKS; i++) {
+		struct nmk_gpio_chip *chip = nmk_gpio_chips[i];
+
+		if (!chip)
+			continue;
+
+		clk_disable(chip->clk);
+	}
+}
+
 /*
  * Called from the suspend/resume path to only keep the real wakeup interrupts
  * (those that have had set_irq_wake() called on them) as wakeup interrupts,
@@ -912,6 +1005,8 @@ void nmk_gpio_wakeups_suspend(void)
 		if (!chip)
 			break;
 
+		clk_enable(chip->clk);
+
 		chip->rwimsc = readl(chip->addr + NMK_GPIO_RWIMSC);
 		chip->fwimsc = readl(chip->addr + NMK_GPIO_FWIMSC);
 
@@ -926,6 +1021,8 @@ void nmk_gpio_wakeups_suspend(void)
 			/* 0 -> wakeup enable */
 			writel(~chip->real_wake, chip->addr + NMK_GPIO_SLPC);
 		}
+
+		clk_disable(chip->clk);
 	}
 }
 
@@ -939,11 +1036,15 @@ void nmk_gpio_wakeups_resume(void)
 		if (!chip)
 			break;
 
+		clk_enable(chip->clk);
+
 		writel(chip->rwimsc, chip->addr + NMK_GPIO_RWIMSC);
 		writel(chip->fwimsc, chip->addr + NMK_GPIO_FWIMSC);
 
 		if (chip->sleepmode)
 			writel(chip->slpm, chip->addr + NMK_GPIO_SLPC);
+
+		clk_disable(chip->clk);
 	}
 }
 
@@ -1010,8 +1111,6 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 		goto out_release;
 	}
 
-	clk_enable(clk);
-
 	nmk_chip = kzalloc(sizeof(*nmk_chip), GFP_KERNEL);
 	if (!nmk_chip) {
 		ret = -ENOMEM;
diff --git a/drivers/hsi/Kconfig b/drivers/hsi/Kconfig
new file mode 100644
index 00000000000..2c76de438eb
--- /dev/null
+++ b/drivers/hsi/Kconfig
@@ -0,0 +1,20 @@
+#
+# HSI driver configuration
+#
+menuconfig HSI
+	tristate "HSI support"
+	---help---
+	  The "High speed synchronous Serial Interface" is
+	  synchronous serial interface used mainly to connect
+	  application engines and cellular modems.
+
+if HSI
+
+config HSI_BOARDINFO
+	bool
+	default y
+
+source "drivers/hsi/controllers/Kconfig"
+source "drivers/hsi/clients/Kconfig"
+
+endif # HSI
diff --git a/drivers/hsi/Makefile b/drivers/hsi/Makefile
new file mode 100644
index 00000000000..d47ca5de18c
--- /dev/null
+++ b/drivers/hsi/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for HSI
+#
+obj-$(CONFIG_HSI_BOARDINFO)	+= hsi_boardinfo.o
+obj-$(CONFIG_HSI)		+= hsi.o
+obj-y				+= controllers/ clients/
diff --git a/drivers/hsi/clients/Kconfig b/drivers/hsi/clients/Kconfig
new file mode 100644
index 00000000000..46eef1f77fd
--- /dev/null
+++ b/drivers/hsi/clients/Kconfig
@@ -0,0 +1,19 @@
+#
+# HSI clients configuration
+#
+
+comment "HSI clients"
+
+config HSI_CHAR
+	tristate "HSI/SSI character driver"
+	depends on HSI
+	---help---
+	  If you say Y here, you will enable the HSI/SSI character driver.
+	  This driver provides a simple character device interface for
+	  serial communication with the cellular modem over HSI/SSI bus.
+config HSI_CAIF
+	tristate "CAIF HSI driver"
+	depends on HSI
+	default n
+	---help---
+	  Provides HSI-CAIF glue layer
diff --git a/drivers/hsi/clients/Makefile b/drivers/hsi/clients/Makefile
new file mode 100644
index 00000000000..dfe33584975
--- /dev/null
+++ b/drivers/hsi/clients/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for HSI clients
+#
+
+obj-$(CONFIG_HSI_CHAR)	+= hsi_char.o
+obj-$(CONFIG_HSI_CAIF)	+= cfhsi.o
diff --git a/drivers/hsi/clients/cfhsi.c b/drivers/hsi/clients/cfhsi.c
new file mode 100644
index 00000000000..cf7ce0cb1cb
--- /dev/null
+++ b/drivers/hsi/clients/cfhsi.c
@@ -0,0 +1,318 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author:	Daniel Martensson <Daniel.Martensson@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+
+#include <net/caif/caif_hsi.h>
+
+#include <linux/hsi/hsi.h>
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Daniel Martensson<daniel.martensson@stericsson.com>");
+MODULE_DESCRIPTION("CAIF HSI V3 glue");
+
+#define NR_OF_CAIF_HSI_CHANNELS	2
+
+struct cfhsi_v3 {
+	struct list_head list;
+	struct cfhsi_dev dev;
+	struct platform_device pdev;
+	struct hsi_msg *tx_msg;
+	struct hsi_msg *rx_msg;
+};
+
+/* TODO: Lists are not protected with regards to device removal. */
+static LIST_HEAD(cfhsi_dev_list);
+
+static struct hsi_client *cfhsi_client;
+
+static int cfhsi_tx(u8 *ptr, int len, struct cfhsi_dev *dev)
+{
+	int res;
+	struct cfhsi_v3 *cfhsi = NULL;
+
+	/* Check length and alignment. */
+	BUG_ON(((int)ptr)%4);
+	BUG_ON(len%4);
+
+	cfhsi = container_of(dev, struct cfhsi_v3, dev);
+
+	sg_init_one(cfhsi->tx_msg->sgt.sgl, (const void *)ptr,
+		    (unsigned int)len);
+
+	/* Write on HSI device. */
+	res = hsi_async_write(cfhsi_client, cfhsi->tx_msg);
+
+	return res;
+}
+
+static int cfhsi_rx(u8 *ptr, int len, struct cfhsi_dev *dev)
+{
+	int res;
+	struct cfhsi_v3 *cfhsi = NULL;
+
+	/* Check length and alignment. */
+	BUG_ON(((int)ptr)%4);
+	BUG_ON(len%4);
+
+	cfhsi = container_of(dev, struct cfhsi_v3, dev);
+
+	sg_init_one(cfhsi->rx_msg->sgt.sgl, (const void *)ptr,
+		    (unsigned int)len);
+
+	/* Read from HSI device. */
+	res = hsi_async_read(cfhsi_client, cfhsi->rx_msg);
+
+	return res;
+}
+
+void cfhsi_v3_release(struct device *dev)
+{
+	pr_warning("%s:%d cfhsi_v3_release called\n", __FILE__, __LINE__);
+}
+
+static inline void cfhsi_v3_destructor(struct hsi_msg *msg)
+{
+	pr_warning("%s:%d cfhsi_v3_destructor called\n", __FILE__, __LINE__);
+}
+
+static inline void cfhsi_v3_read_cb(struct hsi_msg *msg)
+{
+	struct cfhsi_v3 *cfhsi = (struct cfhsi_v3 *)msg->context;
+
+	/* TODO: Error checking. */
+	BUG_ON(!cfhsi->dev.drv);
+	BUG_ON(!cfhsi->dev.drv->rx_done_cb);
+
+	cfhsi->dev.drv->rx_done_cb(cfhsi->dev.drv);
+}
+
+static inline void cfhsi_v3_write_cb(struct hsi_msg *msg)
+{
+	struct cfhsi_v3 *cfhsi = (struct cfhsi_v3 *)msg->context;
+
+	/* TODO: Error checking. */
+	BUG_ON(!cfhsi->dev.drv);
+	BUG_ON(!cfhsi->dev.drv->tx_done_cb);
+
+	cfhsi->dev.drv->tx_done_cb(cfhsi->dev.drv);
+}
+
+static int hsi_proto_probe(struct device *dev)
+{
+	int res;
+	int i;
+	struct cfhsi_v3 *cfhsi = NULL;
+
+	if (cfhsi_client)
+		return -ENODEV; /* TODO: Not correct return. */
+
+	cfhsi_client = to_hsi_client(dev);
+
+	res = hsi_claim_port(cfhsi_client, 0);
+	if (res) {
+		pr_warning("hsi_proto_probe: hsi_claim_port:%d.\n", res);
+		goto err_hsi_claim;
+	}
+
+	/* Right now we don't care about AC_WAKE (No power management). */
+	cfhsi_client->hsi_start_rx = NULL;
+	cfhsi_client->hsi_stop_rx = NULL;
+
+	/* CAIF HSI TX configuration. */
+	cfhsi_client->tx_cfg.mode =  HSI_MODE_STREAM;
+	cfhsi_client->tx_cfg.flow = HSI_FLOW_SYNC;
+	cfhsi_client->tx_cfg.channels = NR_OF_CAIF_HSI_CHANNELS;
+	cfhsi_client->tx_cfg.speed = 100000; /* TODO: What speed should be used. */
+	cfhsi_client->tx_cfg.arb_mode = HSI_ARB_RR;
+
+	/* CAIF HSI RX configuration. */
+	cfhsi_client->rx_cfg.mode =  HSI_MODE_STREAM;
+	cfhsi_client->rx_cfg.flow = HSI_FLOW_SYNC;
+	cfhsi_client->rx_cfg.channels = NR_OF_CAIF_HSI_CHANNELS;
+	cfhsi_client->rx_cfg.speed = 200000; /* TODO: What speed should be used. */
+	cfhsi_client->rx_cfg.arb_mode = HSI_ARB_RR;
+
+	res = hsi_setup(cfhsi_client);
+	if (res) {
+		pr_warning("hsi_proto_probe: hsi_setup:%d.\n", res);
+		goto err_hsi_setup;
+	}
+
+	/* Make sure that AC_WAKE is high (No power management). */
+	res = hsi_start_tx(cfhsi_client);
+	if (res) {
+		pr_warning("hsi_proto_probe: hsi_start_tx:%d.\n", res);
+		goto err_hsi_start_tx;
+	}
+
+	/* Connect channels to CAIF HSI devices. */
+	for (i = 0; i < NR_OF_CAIF_HSI_CHANNELS; i++) {
+		cfhsi = kzalloc(sizeof(struct cfhsi_v3), GFP_KERNEL);
+		if (!cfhsi) {
+			res = -ENOMEM;
+			/* TODO: Error handling. */
+		}
+
+		/* Assign HSI client to this CAIF HSI device. */
+		cfhsi->dev.cfhsi_tx = cfhsi_tx;
+		cfhsi->dev.cfhsi_rx = cfhsi_rx;
+
+		/* Allocate HSI messages. */
+		cfhsi->tx_msg = hsi_alloc_msg(1, GFP_KERNEL);
+		cfhsi->rx_msg = hsi_alloc_msg(1, GFP_KERNEL);
+		if (!cfhsi->tx_msg || !cfhsi->rx_msg) {
+			res = -ENOMEM;
+			/* TODO: Error handling. */
+		}
+
+		/* Set up TX message. */
+		cfhsi->tx_msg->cl = cfhsi_client;
+		cfhsi->tx_msg->context = (void *)cfhsi;
+		cfhsi->tx_msg->complete = cfhsi_v3_write_cb;
+		cfhsi->tx_msg->destructor = cfhsi_v3_destructor;
+		cfhsi->tx_msg->channel = i;
+		cfhsi->tx_msg->ttype = HSI_MSG_WRITE;
+		cfhsi->tx_msg->break_frame = 0; /* No break frame. */
+
+		/* Set up RX message. */
+		cfhsi->rx_msg->cl = cfhsi_client;
+		cfhsi->rx_msg->context = (void *)cfhsi;
+		cfhsi->rx_msg->complete = cfhsi_v3_read_cb;
+		cfhsi->rx_msg->destructor = cfhsi_v3_destructor;
+		cfhsi->rx_msg->channel = i;
+		cfhsi->rx_msg->ttype = HSI_MSG_READ;
+		cfhsi->rx_msg->break_frame = 0; /* No break frame. */
+
+		/* Initialize CAIF HSI platform device. */
+		cfhsi->pdev.name = "cfhsi";
+		cfhsi->pdev.dev.platform_data = &cfhsi->dev;
+		cfhsi->pdev.dev.release = cfhsi_v3_release;
+		/* Use channel number as id. */
+		cfhsi->pdev.id = i;
+		/* Register platform device. */
+		res = platform_device_register(&cfhsi->pdev);
+		if (res) {
+			pr_warning("hsi_proto_probe: plat_dev_reg:%d.\n", res);
+			res = -ENODEV;
+			/* TODO: Error handling. */
+		}
+
+		/* Add HSI device to device list. */
+		list_add_tail(&cfhsi->list, &cfhsi_dev_list);
+	}
+
+	return res;
+
+ err_hsi_start_tx:
+ err_hsi_setup:
+	hsi_release_port(cfhsi_client);
+ err_hsi_claim:
+	cfhsi_client = NULL;
+
+	return res;
+}
+
+static int hsi_proto_remove(struct device *dev)
+{
+	struct cfhsi_v3 *cfhsi = NULL;
+	struct list_head *list_node;
+	struct list_head *n;
+
+	if (!cfhsi_client)
+		return -ENODEV;
+
+	list_for_each_safe(list_node, n, &cfhsi_dev_list) {
+		cfhsi = list_entry(list_node, struct cfhsi_v3, list);
+		/* Remove from list. */
+		list_del(list_node);
+		/* Our HSI device is gone, unregister CAIF HSI device. */
+		platform_device_del(&cfhsi->pdev);
+		hsi_free_msg(cfhsi->tx_msg);
+		hsi_free_msg(cfhsi->rx_msg);
+		/* Free memory. */
+		kfree(cfhsi);
+	}
+
+	hsi_stop_tx(cfhsi_client);
+	hsi_release_port(cfhsi_client);
+
+	cfhsi_client = NULL;
+
+	return 0;
+}
+
+static int hsi_proto_suspend(struct device *dev, pm_message_t mesg)
+{
+	/* Not handled. */
+	pr_info("hsi_proto_suspend.\n");
+
+	return 0;
+}
+
+static int hsi_proto_resume(struct device *dev)
+{
+	/* Not handled. */
+	pr_info("hsi_proto_resume.\n");
+
+	return 0;
+}
+
+static struct hsi_client_driver cfhsi_v3_driver = {
+	.driver = {
+		.name = "cfhsi_v3_driver",
+		.owner	= THIS_MODULE,
+		.probe = hsi_proto_probe,
+		.remove = __devexit_p(hsi_proto_remove),
+		.suspend = hsi_proto_suspend,
+		.resume = hsi_proto_resume,
+	},
+};
+
+static int __init cfhsi_v3_init(void)
+{
+	int res;
+
+	/* Register protocol driver for HSI interface. */
+	res =  hsi_register_client_driver(&cfhsi_v3_driver);
+	if (res)
+		pr_warning("Failed to register CAIF HSI V3 driver.\n");
+
+	return res;
+}
+
+static void __exit cfhsi_v3_exit(void)
+{
+	struct cfhsi_v3 *cfhsi = NULL;
+	struct list_head *list_node;
+	struct list_head *n;
+
+	/* Unregister driver. */
+	hsi_unregister_client_driver(&cfhsi_v3_driver);
+
+	if (!cfhsi_client)
+		return;
+
+	list_for_each_safe(list_node, n, &cfhsi_dev_list) {
+		cfhsi = list_entry(list_node, struct cfhsi_v3, list);
+		platform_device_del(&cfhsi->pdev);
+		hsi_free_msg(cfhsi->tx_msg);
+		hsi_free_msg(cfhsi->rx_msg);
+		kfree(cfhsi);
+	}
+
+	hsi_stop_tx(cfhsi_client);
+	hsi_release_port(cfhsi_client);
+
+	cfhsi_client = NULL;
+}
+
+module_init(cfhsi_v3_init);
+module_exit(cfhsi_v3_exit);
diff --git a/drivers/hsi/clients/hsi_char.c b/drivers/hsi/clients/hsi_char.c
new file mode 100644
index 00000000000..4281156657b
--- /dev/null
+++ b/drivers/hsi/clients/hsi_char.c
@@ -0,0 +1,1108 @@
+/*
+ * hsi-char.c
+ *
+ * HSI character device driver, implements the character device
+ * interface.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Andras Domokos <andras.domokos@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <asm/atomic.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/poll.h>
+#include <linux/ioctl.h>
+#include <linux/wait.h>
+#include <linux/fs.h>
+#include <linux/sched.h>
+#include <linux/device.h>
+#include <linux/cdev.h>
+#include <linux/uaccess.h>
+#include <linux/scatterlist.h>
+#include <linux/hsi/hsi.h>
+#include <linux/hsi/hsi_char.h>
+
+#define HSI_CHAR_CHANNELS	8
+#define HSI_CHAR_DEVS		8
+#define HSI_CHAR_MSGS		4
+
+#define HSI_CHST_UNAVAIL	0 /* SBZ! */
+#define HSI_CHST_AVAIL		1
+
+#define HSI_CHST_CLOSED		(0 << 4)
+#define HSI_CHST_CLOSING	(1 << 4)
+#define HSI_CHST_OPENING	(2 << 4)
+#define HSI_CHST_OPENED		(3 << 4)
+
+#define HSI_CHST_READOFF	(0 << 8)
+#define HSI_CHST_READON		(1 << 8)
+#define HSI_CHST_READING	(2 << 8)
+
+#define HSI_CHST_WRITEOFF	(0 << 12)
+#define HSI_CHST_WRITEON	(1 << 12)
+#define HSI_CHST_WRITING	(2 << 12)
+
+#define HSI_CHST_OC_MASK	0xf0
+#define HSI_CHST_RD_MASK	0xf00
+#define HSI_CHST_WR_MASK	0xf000
+
+#define HSI_CHST_OC(c)		((c)->state & HSI_CHST_OC_MASK)
+#define HSI_CHST_RD(c)		((c)->state & HSI_CHST_RD_MASK)
+#define HSI_CHST_WR(c)		((c)->state & HSI_CHST_WR_MASK)
+
+#define HSI_CHST_OC_SET(c, v) \
+	do { \
+		(c)->state &= ~HSI_CHST_OC_MASK; \
+		(c)->state |= v; \
+	} while (0);
+
+#define HSI_CHST_RD_SET(c, v) \
+	do { \
+		(c)->state &= ~HSI_CHST_RD_MASK; \
+		(c)->state |= v; \
+	} while (0);
+
+#define HSI_CHST_WR_SET(c, v) \
+	do { \
+		(c)->state &= ~HSI_CHST_WR_MASK; \
+		(c)->state |= v; \
+	} while (0);
+
+#define HSI_CHAR_POLL_RST	(-1)
+#define HSI_CHAR_POLL_OFF	0
+#define HSI_CHAR_POLL_ON	1
+
+#define HSI_CHAR_RX		0
+#define HSI_CHAR_TX		1
+
+struct hsi_char_channel {
+	unsigned int		ch;
+	unsigned int		state;
+	int			wlrefcnt;
+	int			rxpoll;
+	struct hsi_client	*cl;
+	struct list_head	free_msgs_list;
+	struct list_head	rx_msgs_queue;
+	struct list_head	tx_msgs_queue;
+	int			poll_event;
+	spinlock_t		lock;
+	struct fasync_struct	*async_queue;
+	wait_queue_head_t	rx_wait;
+	wait_queue_head_t	tx_wait;
+};
+
+struct hsi_char_client_data {
+	atomic_t		refcnt;
+	int			attached;
+	atomic_t		breq;
+	struct hsi_char_channel	channels[HSI_CHAR_DEVS];
+};
+
+static unsigned int max_data_size = 0x1000;
+module_param(max_data_size, uint, 1);
+MODULE_PARM_DESC(max_data_size, "max read/write data size [4,8..65536] (^2)");
+
+static int channels_map[HSI_CHAR_DEVS] = {0, -1, -1 , -1, -1, -1, -1, -1};
+module_param_array(channels_map, int, NULL, 0);
+MODULE_PARM_DESC(channels_map, "Array of HSI channels ([0...7]) to be probed");
+
+static dev_t hsi_char_dev;
+static struct hsi_char_client_data hsi_char_cl_data;
+
+static inline void hsi_char_msg_free(struct hsi_msg *msg)
+{
+	msg->complete = NULL;
+	msg->destructor = NULL;
+	kfree(sg_virt(msg->sgt.sgl));
+	hsi_free_msg(msg);
+}
+
+static inline void hsi_char_msgs_free(struct hsi_char_channel *channel)
+{
+	struct hsi_msg *msg, *tmp;
+
+	list_for_each_entry_safe(msg, tmp, &channel->free_msgs_list, link) {
+		list_del(&msg->link);
+		hsi_char_msg_free(msg);
+	}
+	list_for_each_entry_safe(msg, tmp, &channel->rx_msgs_queue, link) {
+		list_del(&msg->link);
+		hsi_char_msg_free(msg);
+	}
+	list_for_each_entry_safe(msg, tmp, &channel->tx_msgs_queue, link) {
+		list_del(&msg->link);
+		hsi_char_msg_free(msg);
+	}
+}
+
+static inline struct hsi_msg *hsi_char_msg_alloc(unsigned int alloc_size)
+{
+	struct hsi_msg *msg;
+	void *buf;
+
+	msg = hsi_alloc_msg(1, GFP_KERNEL);
+	if (!msg)
+		goto out;
+	buf = kmalloc(alloc_size, GFP_KERNEL);
+	if (!buf) {
+		hsi_free_msg(msg);
+		goto out;
+	}
+	sg_init_one(msg->sgt.sgl, buf, alloc_size);
+	msg->context = buf;
+	return msg;
+out:
+	return NULL;
+}
+
+static inline int hsi_char_msgs_alloc(struct hsi_char_channel *channel)
+{
+	struct hsi_msg *msg;
+	int i;
+
+	for (i = 0; i < HSI_CHAR_MSGS; i++) {
+		msg = hsi_char_msg_alloc(max_data_size);
+		if (!msg)
+			goto out;
+		msg->channel = channel->ch;
+		list_add_tail(&msg->link, &channel->free_msgs_list);
+	}
+	return 0;
+out:
+	hsi_char_msgs_free(channel);
+
+	return -ENOMEM;
+}
+
+static int _hsi_char_release(struct hsi_char_channel *channel, int remove)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(channel->cl);
+	int ret = 0, refcnt;
+
+	spin_lock_bh(&channel->lock);
+	if (HSI_CHST_OC(channel) != HSI_CHST_OPENED)
+		goto out;
+	HSI_CHST_OC_SET(channel, HSI_CHST_CLOSING);
+	spin_unlock_bh(&channel->lock);
+
+	while (channel->wlrefcnt > 0) {
+		hsi_stop_tx(channel->cl);
+		channel->wlrefcnt--;
+	}
+
+	if (channel->rxpoll == HSI_CHAR_POLL_ON)
+		channel->poll_event |= POLLERR;
+
+	wake_up_interruptible(&channel->rx_wait);
+	wake_up_interruptible(&channel->tx_wait);
+
+	refcnt = atomic_dec_return(&cl_data->refcnt);
+	if (!refcnt) {
+		hsi_flush(channel->cl);
+		hsi_release_port(channel->cl);
+		cl_data->attached = 0;
+	}
+	hsi_char_msgs_free(channel);
+
+	spin_lock_bh(&channel->lock);
+	HSI_CHST_OC_SET(channel, HSI_CHST_CLOSED);
+	HSI_CHST_RD_SET(channel, HSI_CHST_READOFF);
+	HSI_CHST_WR_SET(channel, HSI_CHST_WRITEOFF);
+out:
+	if (remove)
+		channel->cl = NULL;
+	spin_unlock_bh(&channel->lock);
+
+	return ret;
+}
+
+static struct hsi_client_driver hsi_char_driver;
+static struct cdev hsi_char_cdev;
+static const struct file_operations hsi_char_fops;
+static struct class *hsi_char_class;
+
+static int __devinit hsi_char_probe(struct device *dev)
+{
+	struct hsi_char_client_data *cl_data = &hsi_char_cl_data;
+	struct hsi_char_channel *channel = cl_data->channels;
+	struct hsi_client *cl = to_hsi_client(dev);
+	char devname[] = "hsi_char";
+	int i;
+	int ret;
+
+	for (i = 0; i < HSI_CHAR_DEVS; i++, channel++) {
+		if (channel->state == HSI_CHST_AVAIL)
+			channel->cl = cl;
+	}
+	cl->hsi_start_rx = NULL;
+	cl->hsi_stop_rx = NULL;
+	atomic_set(&cl_data->refcnt, 0);
+	atomic_set(&cl_data->breq, 1);
+	cl_data->attached = 0;
+	hsi_client_set_drvdata(cl, cl_data);
+
+	ret = alloc_chrdev_region(&hsi_char_dev, 0, HSI_CHAR_DEVS, devname);
+	if (ret < 0) {
+		hsi_unregister_client_driver(&hsi_char_driver);
+		return ret;
+	}
+
+	cdev_init(&hsi_char_cdev, &hsi_char_fops);
+	ret = cdev_add(&hsi_char_cdev, hsi_char_dev, HSI_CHAR_DEVS);
+	if (ret) {
+		unregister_chrdev_region(hsi_char_dev, HSI_CHAR_DEVS);
+		hsi_unregister_client_driver(&hsi_char_driver);
+		return ret;
+	}
+
+	hsi_char_class = class_create(THIS_MODULE, "hsi");
+	if (IS_ERR(hsi_char_class))
+		pr_err("ERROR: hsi class creation failed!\n");
+
+	device_create(hsi_char_class, NULL, hsi_char_cdev.dev, NULL, devname);
+
+	return 0;
+}
+
+static int __devexit hsi_char_remove(struct device *dev)
+{
+	struct hsi_client *cl = to_hsi_client(dev);
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(cl);
+	struct hsi_char_channel *channel = cl_data->channels;
+	int i;
+
+	for (i = 0; i < HSI_CHAR_DEVS; i++, channel++) {
+		if (!(channel->state & HSI_CHST_AVAIL))
+			continue;
+		_hsi_char_release(channel, 1);
+	}
+
+	return 0;
+}
+
+static inline unsigned int hsi_char_msg_len_get(struct hsi_msg *msg)
+{
+	return msg->sgt.sgl->length;
+}
+
+static inline void hsi_char_msg_len_set(struct hsi_msg *msg, unsigned int len)
+{
+	msg->sgt.sgl->length = len;
+}
+
+static void hsi_char_data_available(struct hsi_msg *msg)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(msg->cl);
+	struct hsi_char_channel *channel = cl_data->channels + msg->channel;
+	int ret;
+
+	if (msg->status == HSI_STATUS_ERROR) {
+		ret = hsi_async_read(channel->cl, msg);
+		if (ret < 0) {
+			list_add_tail(&msg->link, &channel->free_msgs_list);
+			spin_lock_bh(&channel->lock);
+			list_add_tail(&msg->link, &channel->free_msgs_list);
+			channel->rxpoll = HSI_CHAR_POLL_OFF;
+			spin_unlock_bh(&channel->lock);
+		}
+	} else {
+		spin_lock_bh(&channel->lock);
+		channel->rxpoll = HSI_CHAR_POLL_OFF;
+		channel->poll_event |= (POLLIN | POLLRDNORM);
+		spin_unlock_bh(&channel->lock);
+		spin_lock_bh(&channel->lock);
+		list_add_tail(&msg->link, &channel->free_msgs_list);
+		spin_unlock_bh(&channel->lock);
+		wake_up_interruptible(&channel->rx_wait);
+	}
+}
+
+static void hsi_char_rx_completed(struct hsi_msg *msg)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(msg->cl);
+	struct hsi_char_channel *channel = cl_data->channels + msg->channel;
+
+	spin_lock_bh(&channel->lock);
+	list_add_tail(&msg->link, &channel->rx_msgs_queue);
+	spin_unlock_bh(&channel->lock);
+	wake_up_interruptible(&channel->rx_wait);
+}
+
+static void hsi_char_rx_msg_destructor(struct hsi_msg *msg)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(msg->cl);
+	struct hsi_char_channel *channel = cl_data->channels + msg->channel;
+
+	spin_lock_bh(&channel->lock);
+	list_add_tail(&msg->link, &channel->free_msgs_list);
+	HSI_CHST_RD_SET(channel, HSI_CHST_READOFF);
+	spin_unlock_bh(&channel->lock);
+}
+
+static void hsi_char_rx_poll_destructor(struct hsi_msg *msg)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(msg->cl);
+	struct hsi_char_channel *channel = cl_data->channels + msg->channel;
+
+	spin_lock_bh(&channel->lock);
+	list_add_tail(&msg->link, &channel->free_msgs_list);
+	channel->rxpoll = HSI_CHAR_POLL_RST;
+	spin_unlock_bh(&channel->lock);
+}
+
+static int hsi_char_rx_poll(struct hsi_char_channel *channel)
+{
+	struct hsi_msg *msg;
+	int ret = 0;
+
+	spin_lock_bh(&channel->lock);
+	if (list_empty(&channel->free_msgs_list)) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	if (channel->rxpoll == HSI_CHAR_POLL_ON)
+		goto out;
+	msg = list_first_entry(&channel->free_msgs_list, struct hsi_msg, link);
+	list_del(&msg->link);
+	channel->rxpoll = HSI_CHAR_POLL_ON;
+	spin_unlock_bh(&channel->lock);
+	hsi_char_msg_len_set(msg, 0);
+	msg->complete = hsi_char_data_available;
+	msg->destructor = hsi_char_rx_poll_destructor;
+	/* don't touch msg->context! */
+	ret = hsi_async_read(channel->cl, msg);
+	spin_lock_bh(&channel->lock);
+	if (ret < 0) {
+		list_add_tail(&msg->link, &channel->free_msgs_list);
+		channel->rxpoll = HSI_CHAR_POLL_OFF;
+		goto out;
+	}
+out:
+	spin_unlock_bh(&channel->lock);
+
+	return ret;
+}
+
+static void hsi_char_tx_completed(struct hsi_msg *msg)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(msg->cl);
+	struct hsi_char_channel *channel = cl_data->channels + msg->channel;
+
+	spin_lock_bh(&channel->lock);
+	list_add_tail(&msg->link, &channel->tx_msgs_queue);
+	channel->poll_event |= (POLLOUT | POLLWRNORM);
+	spin_unlock_bh(&channel->lock);
+	wake_up_interruptible(&channel->tx_wait);
+}
+
+static void hsi_char_tx_msg_destructor(struct hsi_msg *msg)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(msg->cl);
+	struct hsi_char_channel *channel = cl_data->channels + msg->channel;
+
+	spin_lock_bh(&channel->lock);
+	list_add_tail(&msg->link, &channel->free_msgs_list);
+	HSI_CHST_WR_SET(channel, HSI_CHST_WRITEOFF);
+	spin_unlock_bh(&channel->lock);
+}
+
+static void hsi_char_rx_poll_rst(struct hsi_client *cl)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(cl);
+	struct hsi_char_channel *channel = cl_data->channels;
+	int i;
+
+	for (i = 0; i < HSI_CHAR_DEVS; i++, channel++) {
+		if ((HSI_CHST_OC(channel) == HSI_CHST_OPENED) &&
+			(channel->rxpoll == HSI_CHAR_POLL_RST))
+			hsi_char_rx_poll(channel);
+	}
+}
+
+static void hsi_char_reset(struct hsi_client *cl)
+{
+	hsi_flush(cl);
+	hsi_char_rx_poll_rst(cl);
+}
+
+static void hsi_char_rx_cancel(struct hsi_char_channel *channel)
+{
+	hsi_flush(channel->cl);
+	hsi_char_rx_poll_rst(channel->cl);
+}
+
+static void hsi_char_tx_cancel(struct hsi_char_channel *channel)
+{
+	hsi_flush(channel->cl);
+	hsi_char_rx_poll_rst(channel->cl);
+}
+
+static void hsi_char_bcast_break(struct hsi_client *cl)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(cl);
+	struct hsi_char_channel *channel = cl_data->channels;
+	int i;
+
+	for (i = 0; i < HSI_CHAR_DEVS; i++, channel++) {
+		if (HSI_CHST_OC(channel) != HSI_CHST_OPENED)
+			continue;
+		channel->poll_event |= POLLPRI;
+		wake_up_interruptible(&channel->rx_wait);
+		wake_up_interruptible(&channel->tx_wait);
+	}
+}
+
+static void hsi_char_break_received(struct hsi_msg *msg)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(msg->cl);
+	int ret;
+
+	hsi_char_bcast_break(msg->cl);
+	ret = hsi_async_read(msg->cl, msg);
+	if (ret < 0) {
+		hsi_free_msg(msg);
+		atomic_inc(&cl_data->breq);
+	}
+}
+
+static void hsi_char_break_req_destructor(struct hsi_msg *msg)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(msg->cl);
+
+	hsi_free_msg(msg);
+	atomic_inc(&cl_data->breq);
+}
+
+static int hsi_char_break_request(struct hsi_client *cl)
+{
+	struct hsi_char_client_data *cl_data = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+	int ret = 0;
+
+	if (!atomic_dec_and_test(&cl_data->breq)) {
+		atomic_inc(&cl_data->breq);
+		return -EBUSY;
+	}
+	msg = hsi_alloc_msg(0, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+	msg->break_frame = 1;
+	msg->complete = hsi_char_break_received;
+	msg->destructor = hsi_char_break_req_destructor;
+	ret = hsi_async_read(cl, msg);
+	if (ret < 0)
+		hsi_free_msg(msg);
+
+	return ret;
+}
+
+static int hsi_char_break_send(struct hsi_client *cl)
+{
+	struct hsi_msg *msg;
+	int ret = 0;
+
+	msg = hsi_alloc_msg(0, GFP_ATOMIC);
+	if (!msg)
+		return -ENOMEM;
+	msg->break_frame = 1;
+	msg->complete = hsi_free_msg;
+	msg->destructor = hsi_free_msg;
+	ret = hsi_async_write(cl, msg);
+	if (ret < 0)
+		hsi_free_msg(msg);
+
+	return ret;
+}
+
+static inline int ssi_check_common_cfg(struct hsi_config *cfg)
+{
+	if ((cfg->mode != HSI_MODE_STREAM) && (cfg->mode != HSI_MODE_FRAME))
+		return -EINVAL;
+	if ((cfg->channels == 0) || (cfg->channels > HSI_CHAR_CHANNELS))
+		return -EINVAL;
+	if (cfg->channels & (cfg->channels - 1))
+		return -EINVAL;
+
+	return 0;
+}
+
+static inline int ssi_check_rx_cfg(struct hsi_config *cfg)
+{
+	int ret;
+
+	ret = ssi_check_common_cfg(cfg);
+	if (ret < 0)
+		return ret;
+	if ((cfg->flow != HSI_FLOW_SYNC) && (cfg->flow != HSI_FLOW_PIPE))
+		return -EINVAL;
+
+	return 0;
+}
+
+static inline int ssi_check_tx_cfg(struct hsi_config *cfg)
+{
+	int ret;
+
+	ret = ssi_check_common_cfg(cfg);
+	if (ret < 0)
+		return ret;
+	if ((cfg->arb_mode != HSI_ARB_RR) && (cfg->arb_mode != HSI_ARB_PRIO))
+		return -EINVAL;
+
+	return 0;
+}
+
+static inline int hsi_char_cfg_set(struct hsi_client *cl,
+						struct hsi_config *cfg, int dir)
+{
+	struct hsi_config *rxtx_cfg;
+	int ret = 0;
+
+	if (dir == HSI_CHAR_RX) {
+		rxtx_cfg = &cl->rx_cfg;
+		ret = ssi_check_rx_cfg(cfg);
+	} else {
+		rxtx_cfg = &cl->tx_cfg;
+		ret = ssi_check_tx_cfg(cfg);
+	}
+	if (ret < 0)
+		return ret;
+
+	*rxtx_cfg = *cfg;
+	ret = hsi_setup(cl);
+	if (ret < 0)
+		return ret;
+
+	if ((dir == HSI_CHAR_RX) && (cfg->mode == HSI_MODE_FRAME))
+		hsi_char_break_request(cl);
+
+	return ret;
+}
+
+static inline void hsi_char_cfg_get(struct hsi_client *cl,
+						struct hsi_config *cfg, int dir)
+{
+	struct hsi_config *rxtx_cfg;
+
+	if (dir == HSI_CHAR_RX)
+		rxtx_cfg = &cl->rx_cfg;
+	else
+		rxtx_cfg = &cl->tx_cfg;
+	*cfg = *rxtx_cfg;
+}
+
+static inline void hsi_char_rx2icfg(struct hsi_config *cfg,
+						struct hsc_rx_config *rx_cfg)
+{
+	cfg->mode = rx_cfg->mode;
+	cfg->flow = rx_cfg->flow;
+	cfg->channels = rx_cfg->channels;
+	cfg->speed = 0;
+}
+
+static inline void hsi_char_tx2icfg(struct hsi_config *cfg,
+						struct hsc_tx_config *tx_cfg)
+{
+	cfg->mode = tx_cfg->mode;
+	cfg->channels = tx_cfg->channels;
+	cfg->speed = tx_cfg->speed;
+	cfg->arb_mode = tx_cfg->arb_mode;
+}
+
+static inline void hsi_char_rx2ecfg(struct hsc_rx_config *rx_cfg,
+							struct hsi_config *cfg)
+{
+	rx_cfg->mode = cfg->mode;
+	rx_cfg->flow = cfg->flow;
+	rx_cfg->channels = cfg->channels;
+}
+
+static inline void hsi_char_tx2ecfg(struct hsc_tx_config *tx_cfg,
+							struct hsi_config *cfg)
+{
+	tx_cfg->mode = cfg->mode;
+	tx_cfg->channels = cfg->channels;
+	tx_cfg->speed = cfg->speed;
+	tx_cfg->arb_mode = cfg->arb_mode;
+}
+
+static ssize_t hsi_char_read(struct file *file, char __user *buf,
+						size_t len, loff_t *ppos)
+{
+	struct hsi_char_channel *channel = file->private_data;
+	struct hsi_msg *msg = NULL;
+	ssize_t ret;
+
+	if (len == 0) {
+		channel->poll_event &= ~POLLPRI;
+		return 0;
+	}
+	channel->poll_event &= ~POLLPRI;
+
+	if (!IS_ALIGNED(len, sizeof(u32)))
+		return -EINVAL;
+
+	if (len > max_data_size)
+		len = max_data_size;
+
+	spin_lock_bh(&channel->lock);
+	if (HSI_CHST_OC(channel) != HSI_CHST_OPENED) {
+		ret = -ENODEV;
+		goto out;
+	}
+	if (HSI_CHST_RD(channel) != HSI_CHST_READOFF) {
+		ret = -EBUSY;
+		goto out;
+	}
+	if (channel->ch >= channel->cl->rx_cfg.channels) {
+		ret = -ENODEV;
+		goto out;
+	}
+	if (list_empty(&channel->free_msgs_list)) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	msg = list_first_entry(&channel->free_msgs_list, struct hsi_msg, link);
+	list_del(&msg->link);
+	spin_unlock_bh(&channel->lock);
+	hsi_char_msg_len_set(msg, len);
+	msg->complete = hsi_char_rx_completed;
+	msg->destructor = hsi_char_rx_msg_destructor;
+	ret = hsi_async_read(channel->cl, msg);
+	spin_lock_bh(&channel->lock);
+	if (ret < 0)
+		goto out;
+	HSI_CHST_RD_SET(channel, HSI_CHST_READING);
+	msg = NULL;
+
+	for ( ; ; ) {
+		DEFINE_WAIT(wait);
+
+		if (!list_empty(&channel->rx_msgs_queue)) {
+			msg = list_first_entry(&channel->rx_msgs_queue,
+					struct hsi_msg, link);
+			HSI_CHST_RD_SET(channel, HSI_CHST_READOFF);
+			channel->poll_event &= ~(POLLIN | POLLRDNORM);
+			list_del(&msg->link);
+			spin_unlock_bh(&channel->lock);
+			if (msg->status == HSI_STATUS_ERROR) {
+				ret = -EIO;
+			} else {
+				ret = copy_to_user((void __user *)buf,
+						msg->context,
+						hsi_char_msg_len_get(msg));
+				if (ret)
+					ret = -EFAULT;
+				else
+					ret = hsi_char_msg_len_get(msg);
+			}
+			spin_lock_bh(&channel->lock);
+			break;
+		} else if (signal_pending(current)) {
+			spin_unlock_bh(&channel->lock);
+			hsi_char_rx_cancel(channel);
+			spin_lock_bh(&channel->lock);
+			HSI_CHST_RD_SET(channel, HSI_CHST_READOFF);
+			ret = -EINTR;
+			break;
+		} else if ((HSI_CHST_OC(channel) == HSI_CHST_CLOSING) ||
+				(HSI_CHST_OC(channel) == HSI_CHST_CLOSING)) {
+			ret = -EIO;
+			break;
+		}
+		prepare_to_wait(&channel->rx_wait, &wait, TASK_INTERRUPTIBLE);
+		spin_unlock_bh(&channel->lock);
+
+		schedule();
+
+		spin_lock_bh(&channel->lock);
+		finish_wait(&channel->rx_wait, &wait);
+	}
+out:
+	if (msg)
+		list_add_tail(&msg->link, &channel->free_msgs_list);
+	spin_unlock_bh(&channel->lock);
+
+	return ret;
+}
+
+static ssize_t hsi_char_write(struct file *file, const char __user *buf,
+						size_t len, loff_t *ppos)
+{
+	struct hsi_char_channel *channel = file->private_data;
+	struct hsi_msg *msg = NULL;
+	ssize_t ret;
+
+	if ((len == 0) || !IS_ALIGNED(len, sizeof(u32)))
+		return -EINVAL;
+
+	if (len > max_data_size)
+		len = max_data_size;
+
+	spin_lock_bh(&channel->lock);
+	if (HSI_CHST_OC(channel) != HSI_CHST_OPENED) {
+		ret = -ENODEV;
+		goto out;
+	}
+	if (HSI_CHST_WR(channel) != HSI_CHST_WRITEOFF) {
+		ret = -EBUSY;
+		goto out;
+	}
+	if (channel->ch >= channel->cl->tx_cfg.channels) {
+		ret = -ENODEV;
+		goto out;
+	}
+	if (list_empty(&channel->free_msgs_list)) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	msg = list_first_entry(&channel->free_msgs_list, struct hsi_msg, link);
+	list_del(&msg->link);
+	HSI_CHST_WR_SET(channel, HSI_CHST_WRITEON);
+	spin_unlock_bh(&channel->lock);
+
+	if (copy_from_user(msg->context, (void __user *)buf, len)) {
+		spin_lock_bh(&channel->lock);
+		HSI_CHST_WR_SET(channel, HSI_CHST_WRITEOFF);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	hsi_char_msg_len_set(msg, len);
+	msg->complete = hsi_char_tx_completed;
+	msg->destructor = hsi_char_tx_msg_destructor;
+	channel->poll_event &= ~(POLLOUT | POLLWRNORM);
+	ret = hsi_async_write(channel->cl, msg);
+	spin_lock_bh(&channel->lock);
+	if (ret < 0) {
+		channel->poll_event |= (POLLOUT | POLLWRNORM);
+		HSI_CHST_WR_SET(channel, HSI_CHST_WRITEOFF);
+		goto out;
+	}
+	HSI_CHST_WR_SET(channel, HSI_CHST_WRITING);
+	msg = NULL;
+
+	for ( ; ; ) {
+		DEFINE_WAIT(wait);
+
+		if (!list_empty(&channel->tx_msgs_queue)) {
+			msg = list_first_entry(&channel->tx_msgs_queue,
+					struct hsi_msg, link);
+			list_del(&msg->link);
+			HSI_CHST_WR_SET(channel, HSI_CHST_WRITEOFF);
+			if (msg->status == HSI_STATUS_ERROR)
+				ret = -EIO;
+			else
+				ret = hsi_char_msg_len_get(msg);
+			break;
+		} else if (signal_pending(current)) {
+			spin_unlock_bh(&channel->lock);
+			hsi_char_tx_cancel(channel);
+			spin_lock_bh(&channel->lock);
+			HSI_CHST_WR_SET(channel, HSI_CHST_WRITEOFF);
+			ret = -EINTR;
+			break;
+		} else if ((HSI_CHST_OC(channel) == HSI_CHST_CLOSING) ||
+				(HSI_CHST_OC(channel) == HSI_CHST_CLOSING)) {
+			ret = -EIO;
+			break;
+		}
+		prepare_to_wait(&channel->tx_wait, &wait, TASK_INTERRUPTIBLE);
+		spin_unlock_bh(&channel->lock);
+
+		schedule();
+
+		spin_lock_bh(&channel->lock);
+		finish_wait(&channel->tx_wait, &wait);
+	}
+out:
+	if (msg)
+		list_add_tail(&msg->link, &channel->free_msgs_list);
+
+	spin_unlock_bh(&channel->lock);
+
+	return ret;
+}
+
+static unsigned int hsi_char_poll(struct file *file, poll_table *wait)
+{
+	struct hsi_char_channel *channel = file->private_data;
+	unsigned int ret;
+
+	spin_lock_bh(&channel->lock);
+	if ((HSI_CHST_OC(channel) != HSI_CHST_OPENED) ||
+		(channel->ch >= channel->cl->rx_cfg.channels)) {
+		spin_unlock_bh(&channel->lock);
+		return -ENODEV;
+	}
+	poll_wait(file, &channel->rx_wait, wait);
+	poll_wait(file, &channel->tx_wait, wait);
+	ret = channel->poll_event;
+	spin_unlock_bh(&channel->lock);
+	hsi_char_rx_poll(channel);
+
+	return ret;
+}
+
+static long hsi_char_ioctl(struct file *file, unsigned int cmd,
+							unsigned long arg)
+{
+	struct hsi_char_channel *channel = file->private_data;
+	unsigned int state;
+	struct hsi_config cfg;
+	struct hsc_rx_config rx_cfg;
+	struct hsc_tx_config tx_cfg;
+	long ret = 0;
+
+	if (HSI_CHST_OC(channel) != HSI_CHST_OPENED)
+		return -ENODEV;
+
+	switch (cmd) {
+	case HSC_RESET:
+		hsi_char_reset(channel->cl);
+		break;
+	case HSC_SET_PM:
+		if (copy_from_user(&state, (void __user *)arg, sizeof(state)))
+			return -EFAULT;
+		if (state == HSC_PM_DISABLE) {
+			ret = hsi_start_tx(channel->cl);
+			if (!ret)
+				channel->wlrefcnt++;
+		} else if ((state == HSC_PM_ENABLE)
+				&& (channel->wlrefcnt > 0)) {
+			ret = hsi_stop_tx(channel->cl);
+			if (!ret)
+				channel->wlrefcnt--;
+		} else {
+			ret = -EINVAL;
+		}
+		break;
+	case HSC_SEND_BREAK:
+		return hsi_char_break_send(channel->cl);
+	case HSC_SET_RX:
+		if (copy_from_user(&rx_cfg, (void __user *)arg, sizeof(rx_cfg)))
+			return -EFAULT;
+		hsi_char_rx2icfg(&cfg, &rx_cfg);
+		return hsi_char_cfg_set(channel->cl, &cfg, HSI_CHAR_RX);
+	case HSC_GET_RX:
+		hsi_char_cfg_get(channel->cl, &cfg, HSI_CHAR_RX);
+		hsi_char_rx2ecfg(&rx_cfg, &cfg);
+		if (copy_to_user((void __user *)arg, &rx_cfg, sizeof(rx_cfg)))
+			return -EFAULT;
+		break;
+	case HSC_SET_TX:
+		if (copy_from_user(&tx_cfg, (void __user *)arg, sizeof(tx_cfg)))
+			return -EFAULT;
+		hsi_char_tx2icfg(&cfg, &tx_cfg);
+		return hsi_char_cfg_set(channel->cl, &cfg, HSI_CHAR_TX);
+	case HSC_GET_TX:
+		hsi_char_cfg_get(channel->cl, &cfg, HSI_CHAR_TX);
+		hsi_char_tx2ecfg(&tx_cfg, &cfg);
+		if (copy_to_user((void __user *)arg, &tx_cfg, sizeof(tx_cfg)))
+			return -EFAULT;
+		break;
+	default:
+		return -ENOIOCTLCMD;
+	}
+
+	return ret;
+}
+
+static int hsi_char_open(struct inode *inode, struct file *file)
+{
+	struct hsi_char_client_data *cl_data = &hsi_char_cl_data;
+	struct hsi_char_channel *channel = cl_data->channels + iminor(inode);
+	int ret = 0, refcnt;
+
+	spin_lock_bh(&channel->lock);
+	if ((channel->state == HSI_CHST_UNAVAIL) || (!channel->cl)) {
+		ret = -ENODEV;
+		goto out;
+	}
+	if (HSI_CHST_OC(channel) != HSI_CHST_CLOSED) {
+		ret = -EBUSY;
+		goto out;
+	}
+	HSI_CHST_OC_SET(channel, HSI_CHST_OPENING);
+	spin_unlock_bh(&channel->lock);
+
+	refcnt = atomic_inc_return(&cl_data->refcnt);
+	if (refcnt == 1) {
+		if (cl_data->attached) {
+			atomic_dec(&cl_data->refcnt);
+			spin_lock_bh(&channel->lock);
+			HSI_CHST_OC_SET(channel, HSI_CHST_CLOSED);
+			ret = -EBUSY;
+			goto out;
+		}
+		ret = hsi_claim_port(channel->cl, 0);
+		if (ret < 0) {
+			atomic_dec(&cl_data->refcnt);
+			spin_lock_bh(&channel->lock);
+			HSI_CHST_OC_SET(channel, HSI_CHST_CLOSED);
+			goto out;
+		}
+		hsi_setup(channel->cl);
+	} else if (!cl_data->attached) {
+		atomic_dec(&cl_data->refcnt);
+		spin_lock_bh(&channel->lock);
+		HSI_CHST_OC_SET(channel, HSI_CHST_CLOSED);
+		ret = -ENODEV;
+		goto out;
+	}
+	ret = hsi_char_msgs_alloc(channel);
+
+	if (ret < 0) {
+		refcnt = atomic_dec_return(&cl_data->refcnt);
+		if (!refcnt)
+			hsi_release_port(channel->cl);
+		spin_lock_bh(&channel->lock);
+		HSI_CHST_OC_SET(channel, HSI_CHST_CLOSED);
+		goto out;
+	}
+	if (refcnt == 1)
+		cl_data->attached = 1;
+	channel->wlrefcnt = 0;
+	channel->rxpoll = HSI_CHAR_POLL_OFF;
+	channel->poll_event = (POLLOUT | POLLWRNORM);
+	file->private_data = channel;
+	spin_lock_bh(&channel->lock);
+	HSI_CHST_OC_SET(channel, HSI_CHST_OPENED);
+out:
+	spin_unlock_bh(&channel->lock);
+
+	return ret;
+}
+
+static int hsi_char_release(struct inode *inode, struct file *file)
+{
+	struct hsi_char_channel *channel = file->private_data;
+	return _hsi_char_release(channel, 0);
+}
+
+static int hsi_char_fasync(int fd, struct file *file, int on)
+{
+	struct hsi_char_channel *channel = file->private_data;
+
+	if (fasync_helper(fd, file, on, &channel->async_queue) < 0)
+		return -EIO;
+
+	return 0;
+}
+
+static const struct file_operations hsi_char_fops = {
+	.owner		= THIS_MODULE,
+	.read		= hsi_char_read,
+	.write		= hsi_char_write,
+	.poll		= hsi_char_poll,
+	.unlocked_ioctl	= hsi_char_ioctl,
+	.open		= hsi_char_open,
+	.release	= hsi_char_release,
+	.fasync		= hsi_char_fasync,
+};
+
+static struct hsi_client_driver hsi_char_driver = {
+	.driver = {
+		.name	= "hsi_char",
+		.owner	= THIS_MODULE,
+		.probe	= hsi_char_probe,
+		.remove	= hsi_char_remove,
+	},
+};
+
+static inline void hsi_char_channel_init(struct hsi_char_channel *channel)
+{
+	channel->state = HSI_CHST_AVAIL;
+	INIT_LIST_HEAD(&channel->free_msgs_list);
+	init_waitqueue_head(&channel->rx_wait);
+	init_waitqueue_head(&channel->tx_wait);
+	spin_lock_init(&channel->lock);
+	INIT_LIST_HEAD(&channel->rx_msgs_queue);
+	INIT_LIST_HEAD(&channel->tx_msgs_queue);
+}
+
+static struct cdev hsi_char_cdev;
+
+static int __init hsi_char_init(void)
+{
+	struct hsi_char_client_data *cl_data = &hsi_char_cl_data;
+	struct hsi_char_channel *channel = cl_data->channels;
+	unsigned long ch_mask = 0;
+	unsigned int i;
+	int ret;
+
+	if ((max_data_size < 4) || (max_data_size > 0x10000) ||
+		(max_data_size & (max_data_size - 1))) {
+		pr_err("Invalid max read/write data size");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < HSI_CHAR_DEVS && channels_map[i] >= 0; i++) {
+		if (channels_map[i] >= HSI_CHAR_DEVS) {
+			pr_err("Invalid HSI/SSI channel specified");
+			return -EINVAL;
+		}
+		set_bit(channels_map[i], &ch_mask);
+	}
+
+	if (i == 0) {
+		pr_err("No HSI channels available");
+		return -EINVAL;
+	}
+
+	memset(cl_data->channels, 0, sizeof(cl_data->channels));
+	for (i = 0; i < HSI_CHAR_DEVS; i++, channel++) {
+		channel->ch = i;
+		channel->state = HSI_CHST_UNAVAIL;
+		if (test_bit(i, &ch_mask))
+			hsi_char_channel_init(channel);
+	}
+
+	ret = hsi_register_client_driver(&hsi_char_driver);
+	if (ret) {
+		pr_err("Error while registering HSI/SSI driver %d", ret);
+		return ret;
+	}
+
+	pr_info("HSI/SSI char device loaded\n");
+
+	return 0;
+}
+module_init(hsi_char_init);
+
+static void __exit hsi_char_exit(void)
+{
+	device_destroy(hsi_char_class, hsi_char_cdev.dev);
+	class_destroy(hsi_char_class);
+	cdev_del(&hsi_char_cdev);
+	unregister_chrdev_region(hsi_char_dev, HSI_CHAR_DEVS);
+	hsi_unregister_client_driver(&hsi_char_driver);
+	pr_info("HSI char device removed\n");
+}
+module_exit(hsi_char_exit);
+
+MODULE_AUTHOR("Andras Domokos <andras.domokos@nokia.com>");
+MODULE_ALIAS("hsi:hsi_char");
+MODULE_DESCRIPTION("HSI character device");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hsi/controllers/Kconfig b/drivers/hsi/controllers/Kconfig
new file mode 100644
index 00000000000..76d339eaf32
--- /dev/null
+++ b/drivers/hsi/controllers/Kconfig
@@ -0,0 +1,33 @@
+#
+# HSI controllers configuration
+#
+comment "HSI controllers"
+
+config STE_HSI
+       tristate "STE HSI controller driver"
+       depends on (ARCH_U8500 || ARCH_NOMADIK) && HSI
+       default n
+       help
+         ST-Ericsson HSI controller.
+         If you say Y here, you will enable the U8500 HSI hardware driver.
+
+         If unsure, say N.
+
+config OMAP_SSI
+	tristate "OMAP SSI hardware driver"
+	depends on ARCH_OMAP && HSI
+	default n
+	---help---
+	  SSI is a legacy version of HSI. It is usually used to connect
+	  an application engine with a cellular modem.
+	  If you say Y here, you will enable the OMAP SSI hardware driver.
+
+	  If unsure, say N.
+
+if OMAP_SSI
+
+config OMAP_SSI_CONFIG
+	boolean
+	default y
+
+endif # OMAP_SSI
diff --git a/drivers/hsi/controllers/Makefile b/drivers/hsi/controllers/Makefile
new file mode 100644
index 00000000000..475637a0f23
--- /dev/null
+++ b/drivers/hsi/controllers/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for HSI controllers drivers
+#
+
+obj-$(CONFIG_STE_HSI)		+= ste_hsi.o
+obj-$(CONFIG_OMAP_SSI)		+= omap_ssi.o
diff --git a/drivers/hsi/controllers/omap_ssi.c b/drivers/hsi/controllers/omap_ssi.c
new file mode 100644
index 00000000000..a82ea0e13cc
--- /dev/null
+++ b/drivers/hsi/controllers/omap_ssi.c
@@ -0,0 +1,1853 @@
+/*
+ * omap_ssi.c
+ *
+ * Implements the OMAP SSI driver.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+#include <linux/compiler.h>
+#include <linux/err.h>
+#include <linux/ioport.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/seq_file.h>
+#include <linux/scatterlist.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/hsi/hsi.h>
+#include <linux/debugfs.h>
+#include <plat/omap-pm.h>
+#include <plat/clock.h>
+#include <plat/ssi.h>
+
+#define SSI_MAX_CHANNELS	8
+#define SSI_MAX_GDD_LCH		8
+#define SSI_BYTES_TO_FRAMES(x) ((((x) - 1) >> 2) + 1)
+
+/**
+ * struct ssi_clk_res - Device resource data for the SSI clocks
+ * @clk: Pointer to the clock
+ * @nb: Pointer to the clock notifier for clk, if any
+ */
+struct ssi_clk_res {
+	struct clk *clk;
+	struct notifier_block *nb;
+};
+
+/**
+ * struct gdd_trn - GDD transaction data
+ * @msg: Pointer to the HSI message being served
+ * @sg: Pointer to the current sg entry being served
+ */
+struct gdd_trn {
+	struct hsi_msg		*msg;
+	struct scatterlist	*sg;
+};
+
+/**
+ * struct omap_ssm_ctx - OMAP synchronous serial module (TX/RX) context
+ * @mode: Bit transmission mode
+ * @channels: Number of channels
+ * @framesize: Frame size in bits
+ * @timeout: RX frame timeout
+ * @divisor: TX divider
+ * @arb_mode: Arbitration mode for TX frame (Round robin, priority)
+ */
+struct omap_ssm_ctx {
+	u32	mode;
+	u32	channels;
+	u32	frame_size;
+	union	{
+			u32	timeout; /* Rx Only */
+			struct	{
+					u32	arb_mode;
+					u32	divisor;
+			}; /* Tx only */
+	};
+};
+
+/**
+ * struct omap_ssi_port - OMAP SSI port data
+ * @dev: device associated to the port (HSI port)
+ * @sst_dma: SSI transmitter physical base address
+ * @ssr_dma: SSI receiver physical base address
+ * @sst_base: SSI transmitter base address
+ * @ssr_base: SSI receiver base address
+ * @wk_lock: spin lock to serialize access to the wake lines
+ * @lock: Spin lock to serialize access to the SSI port
+ * @channels: Current number of channels configured (1,2,4 or 8)
+ * @txqueue: TX message queues
+ * @rxqueue: RX message queues
+ * @brkqueue: Queue of incoming HWBREAK requests (FRAME mode)
+ * @irq: IRQ number
+ * @wake_irq: IRQ number for incoming wake line (-1 if none)
+ * @pio_tasklet: Bottom half for PIO transfers and events
+ * @wake_tasklet: Bottom half for incoming wake events
+ * @wkin_cken: Keep track of clock references due to the incoming wake line
+ * @wk_refcount: Reference count for output wake line
+ * @sys_mpu_enable: Context for the interrupt enable register for irq 0
+ * @sst: Context for the synchronous serial transmitter
+ * @ssr: Context for the synchronous serial receiver
+ */
+struct omap_ssi_port {
+	struct device		*dev;
+	dma_addr_t		sst_dma;
+	dma_addr_t		ssr_dma;
+	void __iomem		*sst_base;
+	void __iomem		*ssr_base;
+	spinlock_t		wk_lock;
+	spinlock_t		lock;
+	unsigned int		channels;
+	struct list_head	txqueue[SSI_MAX_CHANNELS];
+	struct list_head	rxqueue[SSI_MAX_CHANNELS];
+	struct list_head	brkqueue;
+	unsigned int		irq;
+	int			wake_irq;
+	struct tasklet_struct	pio_tasklet;
+	struct tasklet_struct	wake_tasklet;
+	unsigned int		wkin_cken:1; /* Workaround */
+	int			wk_refcount;
+	/* OMAP SSI port context */
+	u32			sys_mpu_enable; /* We use only one irq */
+	struct omap_ssm_ctx	sst;
+	struct omap_ssm_ctx	ssr;
+};
+
+/**
+ * struct omap_ssi_controller - OMAP SSI controller data
+ * @dev: device associated to the controller (HSI controller)
+ * @sys: SSI I/O base address
+ * @gdd: GDD I/O base address
+ * @ick: SSI interconnect clock
+ * @fck: SSI functional clock
+ * @ck_refcount: References count for clocks
+ * @gdd_irq: IRQ line for GDD
+ * @gdd_tasklet: bottom half for DMA transfers
+ * @gdd_trn: Array of GDD transaction data for ongoing GDD transfers
+ * @lock: lock to serialize access to GDD
+ * @ck_lock: lock to serialize access to the clocks
+ * @loss_count: To follow if we need to restore context or not
+ * @max_speed: Maximum TX speed (Kb/s) set by the clients.
+ * @sysconfig: SSI controller saved context
+ * @gdd_gcr: SSI GDD saved context
+ * @get_loss: Pointer to omap_pm_get_dev_context_loss_count, if any
+ * @port: Array of pointers of the ports of the controller
+ * @dir: Debugfs SSI root directory
+ */
+struct omap_ssi_controller {
+	struct device		*dev;
+	void __iomem		*sys;
+	void __iomem		*gdd;
+	struct clk		*ick;
+	struct clk		*fck;
+	int			ck_refcount;
+	unsigned int		gdd_irq;
+	struct tasklet_struct	gdd_tasklet;
+	struct gdd_trn		gdd_trn[SSI_MAX_GDD_LCH];
+	spinlock_t		lock;
+	spinlock_t		ck_lock;
+	unsigned long		fck_rate;
+	int			loss_count;
+	u32			max_speed;
+	/* OMAP SSI Controller context */
+	u32			sysconfig;
+	u32			gdd_gcr;
+	int			(*get_loss)(struct device *dev);
+	struct omap_ssi_port	**port;
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dir;
+#endif
+};
+
+static inline unsigned int ssi_wakein(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	return gpio_get_value(irq_to_gpio(omap_port->wake_irq));
+}
+
+static int ssi_for_each_port(struct hsi_controller *ssi, void *data,
+			int (*fn)(struct omap_ssi_port *p, void *data))
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	unsigned int i = 0;
+	int err = 0;
+
+	for (i = 0; ((i < ssi->num_ports) && !err); i++)
+		err = (*fn)(omap_ssi->port[i], data);
+
+	return err;
+}
+
+static int ssi_set_port_mode(struct omap_ssi_port *omap_port, void *data)
+{
+	u32 *mode = data;
+
+	__raw_writel(*mode, omap_port->sst_base + SSI_SST_MODE_REG);
+	__raw_writel(*mode, omap_port->ssr_base + SSI_SSR_MODE_REG);
+	/* OCP barrier */
+	*mode = __raw_readl(omap_port->ssr_base + SSI_SSR_MODE_REG);
+
+	return 0;
+}
+
+static inline void ssi_set_mode(struct hsi_controller *ssi, u32 mode)
+{
+	ssi_for_each_port(ssi, &mode, ssi_set_port_mode);
+}
+
+static int ssi_restore_port_mode(struct omap_ssi_port *omap_port,
+						void *data __maybe_unused)
+{
+	u32 mode;
+
+	__raw_writel(omap_port->sst.mode,
+				omap_port->sst_base + SSI_SST_MODE_REG);
+	__raw_writel(omap_port->ssr.mode,
+				omap_port->ssr_base + SSI_SSR_MODE_REG);
+	/* OCP barrier */
+	mode =  __raw_readl(omap_port->ssr_base + SSI_SSR_MODE_REG);
+
+	return 0;
+}
+
+static int ssi_restore_divisor(struct omap_ssi_port *omap_port,
+						void *data __maybe_unused)
+{
+	__raw_writel(omap_port->sst.divisor,
+				omap_port->sst_base + SSI_SST_DIVISOR_REG);
+
+	return 0;
+}
+
+static int ssi_restore_port_ctx(struct omap_ssi_port *omap_port,
+						void *data __maybe_unused)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*base = omap_port->sst_base;
+
+	__raw_writel(omap_port->sys_mpu_enable,
+			omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	/* SST context */
+	__raw_writel(omap_port->sst.frame_size, base + SSI_SST_FRAMESIZE_REG);
+	__raw_writel(omap_port->sst.channels, base + SSI_SST_CHANNELS_REG);
+	__raw_writel(omap_port->sst.arb_mode, base + SSI_SST_ARBMODE_REG);
+	/* SSR context */
+	base = omap_port->ssr_base;
+	__raw_writel(omap_port->ssr.frame_size, base + SSI_SSR_FRAMESIZE_REG);
+	__raw_writel(omap_port->ssr.channels, base + SSI_SSR_CHANNELS_REG);
+	__raw_writel(omap_port->ssr.timeout, base + SSI_SSR_TIMEOUT_REG);
+
+	return 0;
+}
+
+static int ssi_save_port_ctx(struct omap_ssi_port *omap_port,
+						void *data __maybe_unused)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	omap_port->sys_mpu_enable = __raw_readl(omap_ssi->sys +
+					SSI_MPU_ENABLE_REG(port->num, 0));
+
+	return 0;
+}
+
+static int ssi_clk_enable(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int err = 0;
+
+	spin_lock_bh(&omap_ssi->ck_lock);
+	if (omap_ssi->ck_refcount++)
+		goto out;
+	err = clk_enable(omap_ssi->fck);
+	if (unlikely(err < 0))
+		goto out;
+	err = clk_enable(omap_ssi->ick);
+	if (unlikely(err < 0)) {
+		clk_disable(omap_ssi->fck);
+		goto out;
+	}
+	if ((omap_ssi->get_loss) && (omap_ssi->loss_count ==
+				(*omap_ssi->get_loss)(ssi->device.parent)))
+		goto mode; /* We always need to restore the mode & TX divisor */
+
+	__raw_writel(omap_ssi->sysconfig, omap_ssi->sys + SSI_SYSCONFIG_REG);
+	__raw_writel(omap_ssi->gdd_gcr, omap_ssi->gdd + SSI_GDD_GCR_REG);
+
+	ssi_for_each_port(ssi, NULL, ssi_restore_port_ctx);
+mode:
+	ssi_for_each_port(ssi, NULL, ssi_restore_divisor);
+	ssi_for_each_port(ssi, NULL, ssi_restore_port_mode);
+out:
+	spin_unlock_bh(&omap_ssi->ck_lock);
+
+	return err;
+}
+
+static void ssi_clk_disable(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	spin_lock_bh(&omap_ssi->ck_lock);
+	WARN_ON(omap_ssi->ck_refcount <= 0);
+	if (--omap_ssi->ck_refcount)
+		goto out;
+
+	ssi_set_mode(ssi, SSI_MODE_SLEEP);
+
+	if (omap_ssi->get_loss)
+		omap_ssi->loss_count =
+				(*omap_ssi->get_loss)(ssi->device.parent);
+
+	ssi_for_each_port(ssi, NULL, ssi_save_port_ctx);
+	clk_disable(omap_ssi->ick);
+	clk_disable(omap_ssi->fck);
+out:
+	spin_unlock_bh(&omap_ssi->ck_lock);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int ssi_debug_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_controller *ssi = m->private;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*sys = omap_ssi->sys;
+
+	ssi_clk_enable(ssi);
+	seq_printf(m, "REVISION\t: 0x%08x\n",
+					__raw_readl(sys + SSI_REVISION_REG));
+	seq_printf(m, "SYSCONFIG\t: 0x%08x\n",
+					__raw_readl(sys + SSI_SYSCONFIG_REG));
+	seq_printf(m, "SYSSTATUS\t: 0x%08x\n",
+					__raw_readl(sys + SSI_SYSSTATUS_REG));
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_debug_port_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_port *port = m->private;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*base = omap_ssi->sys;
+	unsigned int ch;
+
+	ssi_clk_enable(ssi);
+	if (omap_port->wake_irq > 0)
+		seq_printf(m, "CAWAKE\t\t: %d\n", ssi_wakein(port));
+	seq_printf(m, "WAKE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_WAKE_REG(port->num)));
+	seq_printf(m, "MPU_ENABLE_IRQ%d\t: 0x%08x\n", 0,
+			__raw_readl(base + SSI_MPU_ENABLE_REG(port->num, 0)));
+	seq_printf(m, "MPU_STATUS_IRQ%d\t: 0x%08x\n", 0,
+			__raw_readl(base + SSI_MPU_STATUS_REG(port->num, 0)));
+	/* SST */
+	base = omap_port->sst_base;
+	seq_printf(m, "\nSST\n===\n");
+	seq_printf(m, "ID SST\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_ID_REG));
+	seq_printf(m, "MODE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_MODE_REG));
+	seq_printf(m, "FRAMESIZE\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_FRAMESIZE_REG));
+	seq_printf(m, "DIVISOR\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_DIVISOR_REG));
+	seq_printf(m, "CHANNELS\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_CHANNELS_REG));
+	seq_printf(m, "ARBMODE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_ARBMODE_REG));
+	seq_printf(m, "TXSTATE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_TXSTATE_REG));
+	seq_printf(m, "BUFSTATE\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_BUFSTATE_REG));
+	seq_printf(m, "BREAK\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_BREAK_REG));
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		seq_printf(m, "BUFFER_CH%d\t: 0x%08x\n", ch,
+				__raw_readl(base + SSI_SST_BUFFER_CH_REG(ch)));
+	}
+	/* SSR */
+	base = omap_port->ssr_base;
+	seq_printf(m, "\nSSR\n===\n");
+	seq_printf(m, "ID SSR\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_ID_REG));
+	seq_printf(m, "MODE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_MODE_REG));
+	seq_printf(m, "FRAMESIZE\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_FRAMESIZE_REG));
+	seq_printf(m, "CHANNELS\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_CHANNELS_REG));
+	seq_printf(m, "TIMEOUT\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_TIMEOUT_REG));
+	seq_printf(m, "RXSTATE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_RXSTATE_REG));
+	seq_printf(m, "BUFSTATE\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_BUFSTATE_REG));
+	seq_printf(m, "BREAK\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_BREAK_REG));
+	seq_printf(m, "ERROR\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_ERROR_REG));
+	seq_printf(m, "ERRORACK\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_ERRORACK_REG));
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		seq_printf(m, "BUFFER_CH%d\t: 0x%08x\n", ch,
+				__raw_readl(base + SSI_SSR_BUFFER_CH_REG(ch)));
+	}
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_debug_gdd_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_controller *ssi = m->private;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*gdd = omap_ssi->gdd;
+	int lch;
+
+	ssi_clk_enable(ssi);
+	seq_printf(m, "GDD_MPU_STATUS\t: 0x%08x\n",
+		__raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_STATUS_REG));
+	seq_printf(m, "GDD_MPU_ENABLE\t: 0x%08x\n\n",
+		__raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG));
+	seq_printf(m, "HW_ID\t\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_HW_ID_REG));
+	seq_printf(m, "PPORT_ID\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_PPORT_ID_REG));
+	seq_printf(m, "MPORT_ID\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_MPORT_ID_REG));
+	seq_printf(m, "TEST\t\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_TEST_REG));
+	seq_printf(m, "GCR\t\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_GCR_REG));
+
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++) {
+		seq_printf(m, "\nGDD LCH %d\n=========\n", lch);
+		seq_printf(m, "CSDP\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CSDP_REG(lch)));
+		seq_printf(m, "CCR\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CCR_REG(lch)));
+		seq_printf(m, "CICR\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CICR_REG(lch)));
+		seq_printf(m, "CSR\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CSR_REG(lch)));
+		seq_printf(m, "CSSA\t\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_CSSA_REG(lch)));
+		seq_printf(m, "CDSA\t\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_CDSA_REG(lch)));
+		seq_printf(m, "CEN\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CEN_REG(lch)));
+		seq_printf(m, "CSAC\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CSAC_REG(lch)));
+		seq_printf(m, "CDAC\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CDAC_REG(lch)));
+		seq_printf(m, "CLNK_CTRL\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CLNK_CTRL_REG(lch)));
+	}
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_div_get(void *data, u64 *val)
+{
+	struct hsi_port *port = data;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+
+	ssi_clk_enable(ssi);
+	*val = __raw_readl(omap_port->sst_base + SSI_SST_DIVISOR_REG);
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_div_set(void *data, u64 val)
+{
+	struct hsi_port *port = data;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+
+	if (val > 127)
+		return -EINVAL;
+
+	ssi_clk_enable(ssi);
+	__raw_writel(val, omap_port->sst_base + SSI_SST_DIVISOR_REG);
+	omap_port->sst.divisor = val;
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_show, inode->i_private);
+}
+
+static int ssi_port_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_port_show, inode->i_private);
+}
+
+static int ssi_gdd_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_gdd_show, inode->i_private);
+}
+
+static const struct file_operations ssi_regs_fops = {
+	.open		= ssi_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static const struct file_operations ssi_port_regs_fops = {
+	.open		= ssi_port_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static const struct file_operations ssi_gdd_regs_fops = {
+	.open		= ssi_gdd_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+DEFINE_SIMPLE_ATTRIBUTE(ssi_sst_div_fops, ssi_div_get, ssi_div_set, "%llu\n");
+
+static int __init ssi_debug_add_port(struct omap_ssi_port *omap_port,
+								void *data)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+	struct dentry *dir = data;
+
+	dir = debugfs_create_dir(dev_name(omap_port->dev), dir);
+	if (IS_ERR(dir))
+		return PTR_ERR(dir);
+	debugfs_create_file("regs", S_IRUGO, dir, port, &ssi_port_regs_fops);
+	dir = debugfs_create_dir("sst", dir);
+	if (IS_ERR(dir))
+		return PTR_ERR(dir);
+	debugfs_create_file("divisor", S_IRUGO | S_IWUSR, dir, port,
+							&ssi_sst_div_fops);
+
+	return 0;
+}
+
+static int __init ssi_debug_add_ctrl(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct dentry *dir;
+	int err;
+
+	/* SSI controller */
+	omap_ssi->dir = debugfs_create_dir(dev_name(&ssi->device), NULL);
+	if (IS_ERR(omap_ssi->dir))
+		return PTR_ERR(omap_ssi->dir);
+
+	debugfs_create_file("regs", S_IRUGO, omap_ssi->dir, ssi,
+								&ssi_regs_fops);
+	/* SSI GDD (DMA) */
+	dir = debugfs_create_dir("gdd", omap_ssi->dir);
+	if (IS_ERR(dir))
+		goto rback;
+	debugfs_create_file("regs", S_IRUGO, dir, ssi, &ssi_gdd_regs_fops);
+	/* SSI ports */
+	err = ssi_for_each_port(ssi, omap_ssi->dir, ssi_debug_add_port);
+	if (err < 0)
+		goto rback;
+
+	return 0;
+rback:
+	debugfs_remove_recursive(omap_ssi->dir);
+
+	return PTR_ERR(dir);
+}
+
+static void ssi_debug_remove_ctrl(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	debugfs_remove_recursive(omap_ssi->dir);
+}
+#endif /* CONFIG_DEBUG_FS */
+
+static int ssi_claim_lch(struct hsi_msg *msg)
+{
+
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int lch;
+
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++)
+		if (!omap_ssi->gdd_trn[lch].msg) {
+			omap_ssi->gdd_trn[lch].msg = msg;
+			omap_ssi->gdd_trn[lch].sg = msg->sgt.sgl;
+			return lch;
+		}
+
+	return -EBUSY;
+}
+
+static int ssi_start_pio(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	u32 val;
+
+	ssi_clk_enable(ssi);
+	if (msg->ttype == HSI_MSG_WRITE) {
+		val = SSI_DATAACCEPT(msg->channel);
+		ssi_clk_enable(ssi); /* Hold clocks for pio writes */
+	} else {
+		val = SSI_DATAAVAILABLE(msg->channel) | SSI_ERROROCCURED;
+	}
+	dev_dbg(&port->device, "Single %s transfer\n",
+						msg->ttype ? "write" : "read");
+	val |= __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	ssi_clk_disable(ssi);
+	msg->actual_len = 0;
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	return 0;
+}
+
+static int ssi_start_dma(struct hsi_msg *msg, int lch)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *gdd = omap_ssi->gdd;
+	int err;
+	u16 csdp;
+	u16 ccr;
+	u32 s_addr;
+	u32 d_addr;
+	u32 tmp;
+
+	if (msg->ttype == HSI_MSG_READ) {
+		err = dma_map_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents,
+							DMA_FROM_DEVICE);
+		if (err < 0) {
+			dev_dbg(&ssi->device, "DMA map SG failed !\n");
+			return err;
+		}
+		csdp = SSI_DST_BURST_4x32_BIT | SSI_DST_MEMORY_PORT |
+			SSI_SRC_SINGLE_ACCESS0 | SSI_SRC_PERIPHERAL_PORT |
+			SSI_DATA_TYPE_S32;
+		ccr = msg->channel + 0x10 + (port->num * 8); /* Sync */
+		ccr |= SSI_DST_AMODE_POSTINC | SSI_SRC_AMODE_CONST |
+			SSI_CCR_ENABLE;
+		s_addr = omap_port->ssr_dma +
+					SSI_SSR_BUFFER_CH_REG(msg->channel);
+		d_addr = sg_dma_address(msg->sgt.sgl);
+	} else {
+		err = dma_map_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents,
+							DMA_TO_DEVICE);
+		if (err < 0) {
+			dev_dbg(&ssi->device, "DMA map SG failed !\n");
+			return err;
+		}
+		csdp = SSI_SRC_BURST_4x32_BIT | SSI_SRC_MEMORY_PORT |
+			SSI_DST_SINGLE_ACCESS0 | SSI_DST_PERIPHERAL_PORT |
+			SSI_DATA_TYPE_S32;
+		ccr = (msg->channel + 1 + (port->num * 8)) & 0xf; /* Sync */
+		ccr |= SSI_SRC_AMODE_POSTINC | SSI_DST_AMODE_CONST |
+			SSI_CCR_ENABLE;
+		s_addr = sg_dma_address(msg->sgt.sgl);
+		d_addr = omap_port->sst_dma +
+					SSI_SST_BUFFER_CH_REG(msg->channel);
+	}
+	dev_dbg(&ssi->device, "lch %d cdsp %08x ccr %04x s_addr %08x"
+			" d_addr %08x\n", lch, csdp, ccr, s_addr, d_addr);
+	ssi_clk_enable(ssi); /* Hold clocks during the transfer */
+	__raw_writew(csdp, gdd + SSI_GDD_CSDP_REG(lch));
+	__raw_writew(SSI_BLOCK_IE | SSI_TOUT_IE, gdd + SSI_GDD_CICR_REG(lch));
+	__raw_writel(d_addr, gdd + SSI_GDD_CDSA_REG(lch));
+	__raw_writel(s_addr, gdd + SSI_GDD_CSSA_REG(lch));
+	__raw_writew(SSI_BYTES_TO_FRAMES(msg->sgt.sgl->length),
+						gdd + SSI_GDD_CEN_REG(lch));
+
+	spin_lock_bh(&omap_ssi->lock);
+	tmp = __raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp |= SSI_GDD_LCH(lch);
+	__raw_writel(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	spin_unlock_bh(&omap_ssi->lock);
+	__raw_writew(ccr, gdd + SSI_GDD_CCR_REG(lch));
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	return 0;
+}
+
+static int ssi_start_transfer(struct list_head *queue)
+{
+	struct hsi_msg *msg;
+	int lch = -1;
+
+	if (list_empty(queue))
+		return 0;
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if (msg->status != HSI_STATUS_QUEUED)
+		return 0;
+	if ((msg->sgt.nents) && (msg->sgt.sgl->length > sizeof(u32)))
+		lch = ssi_claim_lch(msg);
+	if (lch >= 0)
+		return ssi_start_dma(msg, lch);
+	else
+		return ssi_start_pio(msg);
+}
+
+static void ssi_transfer(struct omap_ssi_port *omap_port,
+							struct list_head *queue)
+{
+	struct hsi_msg *msg;
+	int err = -1;
+
+	spin_lock_bh(&omap_port->lock);
+	while (err < 0) {
+		err = ssi_start_transfer(queue);
+		if (err < 0) {
+			msg = list_first_entry(queue, struct hsi_msg, link);
+			msg->status = HSI_STATUS_ERROR;
+			msg->actual_len = 0;
+			list_del(&msg->link);
+			spin_unlock_bh(&omap_port->lock);
+			msg->complete(msg);
+			spin_lock_bh(&omap_port->lock);
+		}
+	}
+	spin_unlock_bh(&omap_port->lock);
+}
+
+static u32 ssi_calculate_div(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	u32 tx_fckrate = (u32) omap_ssi->fck_rate;
+
+	/* / 2 : SSI TX clock is always half of the SSI functional clock */
+	tx_fckrate >>= 1;
+	/* Round down when tx_fckrate % omap_ssi->max_speed == 0 */
+	tx_fckrate--;
+	dev_dbg(&ssi->device, "TX div %d for fck_rate %lu Khz speed %d Kb/s\n",
+			tx_fckrate / omap_ssi->max_speed, omap_ssi->fck_rate,
+							omap_ssi->max_speed);
+
+	return tx_fckrate / omap_ssi->max_speed;
+}
+
+static void ssi_error(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 err;
+	u32 val;
+	u32 tmp;
+
+	/* ACK error */
+	err = __raw_readl(omap_port->ssr_base + SSI_SSR_ERROR_REG);
+	dev_err(&port->device, "SSI error: 0x%02x\n", err);
+	if (!err) {
+		dev_dbg(&port->device, "spurious SSI error ignored!\n");
+		return;
+	}
+	spin_lock(&omap_ssi->lock);
+	/* Cancel all GDD read transfers */
+	for (i = 0, val = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if ((msg) && (msg->ttype == HSI_MSG_READ)) {
+			__raw_writew(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+			val |= (1 << i);
+			omap_ssi->gdd_trn[i].msg = NULL;
+		}
+	}
+	tmp = __raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp &= ~val;
+	__raw_writel(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	spin_unlock(&omap_ssi->lock);
+	/* Cancel all PIO read transfers */
+	spin_lock(&omap_port->lock);
+	tmp = __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	tmp &= 0xfeff00ff; /* Disable error & all dataavailable interrupts */
+	__raw_writel(tmp, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	/* ACK error */
+	__raw_writel(err, omap_port->ssr_base + SSI_SSR_ERRORACK_REG);
+	__raw_writel(SSI_ERROROCCURED,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	/* Signal the error all current pending read requests */
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->rxqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->rxqueue[i], struct hsi_msg,
+									link);
+		list_del(&msg->link);
+		msg->status = HSI_STATUS_ERROR;
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+		/* Now restart queued reads if any */
+		ssi_transfer(omap_port, &omap_port->rxqueue[i]);
+		spin_lock(&omap_port->lock);
+	}
+	spin_unlock(&omap_port->lock);
+}
+
+static void ssi_break_complete(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	struct hsi_msg *tmp;
+	u32 val;
+
+	dev_dbg(&port->device, "HWBREAK received\n");
+
+	spin_lock(&omap_port->lock);
+	val = __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	val &= ~SSI_BREAKDETECTED;
+	__raw_writel(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(0, omap_port->ssr_base + SSI_SSR_BREAK_REG);
+	__raw_writel(SSI_BREAKDETECTED,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	spin_unlock(&omap_port->lock);
+
+	list_for_each_entry_safe(msg, tmp, &omap_port->brkqueue, link) {
+		msg->status = HSI_STATUS_COMPLETED;
+		spin_lock(&omap_port->lock);
+		list_del(&msg->link);
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+	}
+
+}
+
+static int ssi_async_break(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int err = 0;
+	u32 tmp;
+
+	ssi_clk_enable(ssi);
+	if (msg->ttype == HSI_MSG_WRITE) {
+		if (omap_port->sst.mode != SSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		__raw_writel(1, omap_port->sst_base + SSI_SST_BREAK_REG);
+		msg->status = HSI_STATUS_COMPLETED;
+		msg->complete(msg);
+	} else {
+		if (omap_port->ssr.mode != SSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		spin_lock_bh(&omap_port->lock);
+		tmp = __raw_readl(omap_ssi->sys +
+					SSI_MPU_ENABLE_REG(port->num, 0));
+		__raw_writel(tmp | SSI_BREAKDETECTED,
+			omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+		msg->status = HSI_STATUS_PROCEEDING;
+		list_add_tail(&msg->link, &omap_port->brkqueue);
+		spin_unlock_bh(&omap_port->lock);
+	}
+out:
+	ssi_clk_disable(ssi);
+
+	return err;
+}
+
+static int ssi_async(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct list_head *queue;
+	int err = 0;
+
+	BUG_ON(!msg);
+
+	if (msg->sgt.nents > 1)
+		return -ENOSYS; /* TODO: Add sg support */
+
+	if (msg->break_frame)
+		return ssi_async_break(msg);
+
+	if (msg->ttype) {
+		BUG_ON(msg->channel >= omap_port->sst.channels);
+		queue = &omap_port->txqueue[msg->channel];
+	} else {
+		BUG_ON(msg->channel >= omap_port->ssr.channels);
+		queue = &omap_port->rxqueue[msg->channel];
+	}
+	msg->status = HSI_STATUS_QUEUED;
+	spin_lock_bh(&omap_port->lock);
+	list_add_tail(&msg->link, queue);
+	err = ssi_start_transfer(queue);
+	if (err < 0) {
+		list_del(&msg->link);
+		msg->status = HSI_STATUS_ERROR;
+	}
+	spin_unlock_bh(&omap_port->lock);
+	dev_dbg(&port->device, "msg status %d ttype %d ch %d\n",
+				msg->status, msg->ttype, msg->channel);
+
+	return err;
+}
+
+static void ssi_flush_queue(struct list_head *queue, struct hsi_client *cl)
+{
+	struct list_head *node, *tmp;
+	struct hsi_msg *msg;
+
+	list_for_each_safe(node, tmp, queue) {
+		msg = list_entry(node, struct hsi_msg, link);
+		if ((cl) && (cl != msg->cl))
+			continue;
+		list_del(node);
+		pr_debug("flush queue: ch %d, msg %p len %d type %d ctxt %p\n",
+			msg->channel, msg, msg->sgt.sgl->length,
+					msg->ttype, msg->context);
+		if (msg->destructor)
+			msg->destructor(msg);
+		else
+			hsi_free_msg(msg);
+	}
+}
+
+static int ssi_setup(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sst = omap_port->sst_base;
+	void __iomem *ssr = omap_port->ssr_base;
+	u32 div;
+	u32 val;
+	int err = 0;
+
+	ssi_clk_enable(ssi);
+	spin_lock_bh(&omap_port->lock);
+	if (cl->tx_cfg.speed)
+		omap_ssi->max_speed = cl->tx_cfg.speed;
+	div = ssi_calculate_div(ssi);
+	if (div > SSI_MAX_DIVISOR) {
+		dev_err(&cl->device, "Invalid TX speed %d Mb/s (div %d)\n",
+						cl->tx_cfg.speed, div);
+		err = -EINVAL;
+		goto out;
+	}
+	/* Set TX/RX module to sleep to stop TX/RX during cfg update */
+	__raw_writel(SSI_MODE_SLEEP, sst + SSI_SST_MODE_REG);
+	__raw_writel(SSI_MODE_SLEEP, ssr + SSI_SSR_MODE_REG);
+	/* Flush posted write */
+	val = __raw_readl(ssr + SSI_SSR_MODE_REG);
+	/* TX */
+	__raw_writel(31, sst + SSI_SST_FRAMESIZE_REG);
+	__raw_writel(div, sst + SSI_SST_DIVISOR_REG);
+	__raw_writel(cl->tx_cfg.channels, sst + SSI_SST_CHANNELS_REG);
+	__raw_writel(cl->tx_cfg.arb_mode, sst + SSI_SST_ARBMODE_REG);
+	__raw_writel(cl->tx_cfg.mode, sst + SSI_SST_MODE_REG);
+	/* RX */
+	__raw_writel(31, ssr + SSI_SSR_FRAMESIZE_REG);
+	__raw_writel(cl->rx_cfg.channels, ssr + SSI_SSR_CHANNELS_REG);
+	__raw_writel(0, ssr + SSI_SSR_TIMEOUT_REG);
+	/* Cleanup the break queue if we leave FRAME mode */
+	if ((omap_port->ssr.mode == SSI_MODE_FRAME) &&
+		(cl->rx_cfg.mode != SSI_MODE_FRAME))
+		ssi_flush_queue(&omap_port->brkqueue, cl);
+	__raw_writel(cl->rx_cfg.mode, ssr + SSI_SSR_MODE_REG);
+	omap_port->channels = max(cl->rx_cfg.channels, cl->tx_cfg.channels);
+	/* Shadow registering for OFF mode */
+	/* SST */
+	omap_port->sst.divisor = div;
+	omap_port->sst.frame_size = 31;
+	omap_port->sst.channels = cl->tx_cfg.channels;
+	omap_port->sst.arb_mode = cl->tx_cfg.arb_mode;
+	omap_port->sst.mode = cl->tx_cfg.mode;
+	/* SSR */
+	omap_port->ssr.frame_size = 31;
+	omap_port->ssr.timeout = 0;
+	omap_port->ssr.channels = cl->rx_cfg.channels;
+	omap_port->ssr.mode = cl->rx_cfg.mode;
+out:
+	spin_unlock_bh(&omap_port->lock);
+	ssi_clk_disable(ssi);
+
+	return err;
+}
+
+static void ssi_cleanup_queues(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 rxbufstate = 0;
+	u32 txbufstate = 0;
+	u32 status = SSI_ERROROCCURED;
+	u32 tmp;
+
+	ssi_flush_queue(&omap_port->brkqueue, cl);
+	if (list_empty(&omap_port->brkqueue))
+		status |= SSI_BREAKDETECTED;
+
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->txqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->txqueue[i], struct hsi_msg,
+									link);
+		if ((msg->cl == cl) && (msg->status == HSI_STATUS_PROCEEDING)) {
+			txbufstate |= (1 << i);
+			status |= SSI_DATAACCEPT(i);
+			/* Release the clocks writes, also GDD ones */
+			ssi_clk_disable(ssi);
+		}
+		ssi_flush_queue(&omap_port->txqueue[i], cl);
+	}
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->rxqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->rxqueue[i], struct hsi_msg,
+									link);
+		if ((msg->cl == cl) && (msg->status == HSI_STATUS_PROCEEDING)) {
+			rxbufstate |= (1 << i);
+			status |= SSI_DATAAVAILABLE(i);
+		}
+		ssi_flush_queue(&omap_port->rxqueue[i], cl);
+		/* Check if we keep the error detection interrupt armed */
+		if (!list_empty(&omap_port->rxqueue[i]))
+			status &= ~SSI_ERROROCCURED;
+	}
+	/* Cleanup write buffers */
+	tmp = __raw_readl(omap_port->sst_base + SSI_SST_BUFSTATE_REG);
+	tmp &= ~txbufstate;
+	__raw_writel(tmp, omap_port->sst_base + SSI_SST_BUFSTATE_REG);
+	/* Cleanup read buffers */
+	tmp = __raw_readl(omap_port->ssr_base + SSI_SSR_BUFSTATE_REG);
+	tmp &= ~rxbufstate;
+	__raw_writel(tmp, omap_port->ssr_base + SSI_SSR_BUFSTATE_REG);
+	/* Disarm and ack pending interrupts */
+	tmp = __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	tmp &= ~status;
+	__raw_writel(tmp, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(status, omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+}
+
+static void ssi_cleanup_gdd(struct hsi_controller *ssi, struct hsi_client *cl)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 val = 0;
+	u32 tmp;
+
+	for (i = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if ((!msg) || (msg->cl != cl))
+			continue;
+		__raw_writew(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+		val |= (1 << i);
+		/*
+		 * Clock references for write will be handled in
+		 * ssi_cleanup_queues
+		 */
+		if (msg->ttype == HSI_MSG_READ)
+			ssi_clk_disable(ssi);
+		omap_ssi->gdd_trn[i].msg = NULL;
+	}
+	tmp = __raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp &= ~val;
+	__raw_writel(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	__raw_writel(val, omap_ssi->sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+}
+
+static int ssi_release(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	spin_lock_bh(&omap_port->lock);
+	ssi_clk_enable(ssi);
+	/* Stop all the pending DMA requests for that client */
+	ssi_cleanup_gdd(ssi, cl);
+	/* Now cleanup all the queues */
+	ssi_cleanup_queues(cl);
+	ssi_clk_disable(ssi);
+	/* If it is the last client of the port, do extra checks and cleanup */
+	if (port->claimed <= 1) {
+		/*
+		 * Drop the clock reference for the incoming wake line
+		 * if it is still kept high by the other side.
+		 */
+		if (omap_port->wkin_cken) {
+			ssi_clk_disable(ssi);
+			omap_port->wkin_cken = 0;
+		}
+		ssi_clk_enable(ssi);
+		/* Stop any SSI TX/RX without a client */
+		ssi_set_mode(ssi, SSI_MODE_SLEEP);
+		omap_port->sst.mode = SSI_MODE_SLEEP;
+		omap_port->ssr.mode = SSI_MODE_SLEEP;
+		ssi_clk_disable(ssi);
+		WARN_ON(omap_port->wk_refcount != 0);
+		WARN_ON(omap_ssi->ck_refcount != 0);
+	}
+	spin_unlock_bh(&omap_port->lock);
+
+	return 0;
+}
+
+static int ssi_flush(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	void __iomem *sst = omap_port->sst_base;
+	void __iomem *ssr = omap_port->ssr_base;
+	unsigned int i;
+	u32 err;
+
+	ssi_clk_enable(ssi);
+	spin_lock_bh(&omap_port->lock);
+	/* Stop all DMA transfers */
+	for (i = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if (!msg || (port != hsi_get_port(msg->cl)))
+			continue;
+		__raw_writew(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+		if (msg->ttype == HSI_MSG_READ)
+			ssi_clk_disable(ssi);
+		omap_ssi->gdd_trn[i].msg = NULL;
+	}
+	/* Flush all SST buffers */
+	__raw_writel(0, sst + SSI_SST_BUFSTATE_REG);
+	__raw_writel(0, sst + SSI_SST_TXSTATE_REG);
+	/* Flush all SSR buffers */
+	__raw_writel(0, ssr + SSI_SSR_RXSTATE_REG);
+	__raw_writel(0, ssr + SSI_SSR_BUFSTATE_REG);
+	/* Flush all errors */
+	err = __raw_readl(ssr + SSI_SSR_ERROR_REG);
+	__raw_writel(err, ssr + SSI_SSR_ERRORACK_REG);
+	/* Flush break */
+	__raw_writel(0, ssr + SSI_SSR_BREAK_REG);
+	/* Clear interrupts */
+	__raw_writel(0, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(0xffffff00,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	__raw_writel(0, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	__raw_writel(0xff, omap_ssi->sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	/* Dequeue all pending requests */
+	for (i = 0; i < omap_port->channels; i++) {
+		/* Release write clocks */
+		if (!list_empty(&omap_port->txqueue[i]))
+			ssi_clk_disable(ssi);
+		ssi_flush_queue(&omap_port->txqueue[i], NULL);
+		ssi_flush_queue(&omap_port->rxqueue[i], NULL);
+	}
+	ssi_flush_queue(&omap_port->brkqueue, NULL);
+	spin_unlock_bh(&omap_port->lock);
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_start_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(&port->device, "Wake out high %d\n", omap_port->wk_refcount);
+
+	spin_lock_bh(&omap_port->wk_lock);
+	if (omap_port->wk_refcount++) {
+		spin_unlock_bh(&omap_port->wk_lock);
+		return 0;
+	}
+	ssi_clk_enable(ssi); /* Grab clocks */
+	__raw_writel(SSI_WAKE(0), omap_ssi->sys + SSI_SET_WAKE_REG(port->num));
+	spin_unlock_bh(&omap_port->wk_lock);
+
+	return 0;
+}
+
+static int ssi_stop_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(&port->device, "Wake out low %d\n", omap_port->wk_refcount);
+
+	spin_lock_bh(&omap_port->wk_lock);
+	BUG_ON(!omap_port->wk_refcount);
+	if (--omap_port->wk_refcount) {
+		spin_unlock_bh(&omap_port->wk_lock);
+		return 0;
+	}
+	__raw_writel(SSI_WAKE(0),
+				omap_ssi->sys + SSI_CLEAR_WAKE_REG(port->num));
+	ssi_clk_disable(ssi); /* Release clocks */
+	spin_unlock_bh(&omap_port->wk_lock);
+
+	return 0;
+}
+
+static void ssi_pio_complete(struct hsi_port *port, struct list_head *queue)
+{
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_msg *msg;
+	u32 *buf;
+	u32 reg;
+	u32 val;
+
+	spin_lock(&omap_port->lock);
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if ((!msg->sgt.nents) || (!msg->sgt.sgl->length)) {
+		msg->actual_len = 0;
+		msg->status = HSI_STATUS_PENDING;
+	}
+	if (msg->ttype == HSI_MSG_WRITE)
+		val = SSI_DATAACCEPT(msg->channel);
+	else
+		val = SSI_DATAAVAILABLE(msg->channel);
+	if (msg->status == HSI_STATUS_PROCEEDING) {
+		buf = sg_virt(msg->sgt.sgl) + msg->actual_len;
+		if (msg->ttype == HSI_MSG_WRITE)
+			__raw_writel(*buf, omap_port->sst_base +
+					SSI_SST_BUFFER_CH_REG(msg->channel));
+		 else
+			*buf = __raw_readl(omap_port->ssr_base +
+					SSI_SSR_BUFFER_CH_REG(msg->channel));
+		dev_dbg(&port->device, "ch %d ttype %d 0x%08x\n", msg->channel,
+							msg->ttype, *buf);
+		msg->actual_len += sizeof(*buf);
+		if (msg->actual_len >= msg->sgt.sgl->length)
+			msg->status = HSI_STATUS_COMPLETED;
+		/*
+		 * Wait for the last written frame to be really sent before
+		 * we call the complete callback
+		 */
+		if ((msg->status == HSI_STATUS_PROCEEDING) ||
+				((msg->status == HSI_STATUS_COMPLETED) &&
+					(msg->ttype == HSI_MSG_WRITE))) {
+			__raw_writel(val, omap_ssi->sys +
+					SSI_MPU_STATUS_REG(port->num, 0));
+			spin_unlock(&omap_port->lock);
+
+			return;
+		}
+
+	}
+	/* Transfer completed at this point */
+	reg = __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	if (msg->ttype == HSI_MSG_WRITE)
+		ssi_clk_disable(ssi); /* Release clocks for write transfer */
+	reg &= ~val;
+	__raw_writel(reg, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(val, omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	list_del(&msg->link);
+	spin_unlock(&omap_port->lock);
+	msg->complete(msg);
+	ssi_transfer(omap_port, queue);
+}
+
+static void ssi_gdd_complete(struct hsi_controller *ssi, unsigned int lch)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg = omap_ssi->gdd_trn[lch].msg;
+	struct hsi_port *port = to_hsi_port(msg->cl->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	unsigned int dir;
+	u32 csr;
+	u32 val;
+
+	spin_lock(&omap_ssi->lock);
+
+	val = __raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	val &= ~SSI_GDD_LCH(lch);
+	__raw_writel(val, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+
+	if (msg->ttype == HSI_MSG_READ) {
+		dir = DMA_FROM_DEVICE;
+		val = SSI_DATAAVAILABLE(msg->channel);
+		ssi_clk_disable(ssi);
+	} else {
+		dir = DMA_TO_DEVICE;
+		val = SSI_DATAACCEPT(msg->channel);
+		/* Keep clocks reference for write pio event */
+	}
+	dma_unmap_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents, dir);
+	csr = __raw_readw(omap_ssi->gdd + SSI_GDD_CSR_REG(lch));
+	omap_ssi->gdd_trn[lch].msg = NULL; /* release GDD lch */
+	dev_dbg(&port->device, "DMA completed ch %d ttype %d\n",
+				msg->channel, msg->ttype);
+	spin_unlock(&omap_ssi->lock);
+	if (csr & SSI_CSR_TOUR) { /* Timeout error */
+		msg->status = HSI_STATUS_ERROR;
+		msg->actual_len = 0;
+		spin_lock(&omap_port->lock);
+		list_del(&msg->link); /* Dequeue msg */
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+		return;
+	}
+	spin_lock(&omap_port->lock);
+	val |= __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	spin_unlock(&omap_port->lock);
+
+	msg->status = HSI_STATUS_COMPLETED;
+	msg->actual_len = sg_dma_len(msg->sgt.sgl);
+}
+
+static void ssi_gdd_tasklet(unsigned long dev)
+{
+	struct hsi_controller *ssi = (struct hsi_controller *)dev;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sys = omap_ssi->sys;
+	unsigned int lch;
+	u32 status_reg;
+
+	ssi_clk_enable(ssi);
+
+	status_reg = __raw_readl(sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++) {
+		if (status_reg & SSI_GDD_LCH(lch))
+			ssi_gdd_complete(ssi, lch);
+	}
+	__raw_writel(status_reg, sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	status_reg = __raw_readl(sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	ssi_clk_disable(ssi);
+	if (status_reg)
+		tasklet_hi_schedule(&omap_ssi->gdd_tasklet);
+	else
+		enable_irq(omap_ssi->gdd_irq);
+
+}
+
+static irqreturn_t ssi_gdd_isr(int irq, void *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	tasklet_hi_schedule(&omap_ssi->gdd_tasklet);
+	disable_irq_nosync(irq);
+
+	return IRQ_HANDLED;
+}
+
+static void ssi_pio_tasklet(unsigned long ssi_port)
+{
+	struct hsi_port *port = (struct hsi_port *)ssi_port;
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sys = omap_ssi->sys;
+	unsigned int ch;
+	u32 status_reg;
+
+	ssi_clk_enable(ssi);
+	status_reg = __raw_readl(sys + SSI_MPU_STATUS_REG(port->num, 0));
+	status_reg &= __raw_readl(sys + SSI_MPU_ENABLE_REG(port->num, 0));
+
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		if (status_reg & SSI_DATAACCEPT(ch))
+			ssi_pio_complete(port, &omap_port->txqueue[ch]);
+		if (status_reg & SSI_DATAAVAILABLE(ch))
+			ssi_pio_complete(port, &omap_port->rxqueue[ch]);
+	}
+	if (status_reg & SSI_BREAKDETECTED)
+		ssi_break_complete(port);
+	if (status_reg & SSI_ERROROCCURED)
+		ssi_error(port);
+
+	status_reg = __raw_readl(sys + SSI_MPU_STATUS_REG(port->num, 0));
+	status_reg &= __raw_readl(sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	ssi_clk_disable(ssi);
+
+	if (status_reg)
+		tasklet_hi_schedule(&omap_port->pio_tasklet);
+	else
+		enable_irq(omap_port->irq);
+}
+
+static irqreturn_t ssi_pio_isr(int irq, void *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	tasklet_hi_schedule(&omap_port->pio_tasklet);
+	disable_irq_nosync(irq);
+
+	return IRQ_HANDLED;
+}
+
+static void ssi_wake_tasklet(unsigned long ssi_port)
+{
+	struct hsi_port *port = (struct hsi_port *)ssi_port;
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	if (ssi_wakein(port)) {
+		/**
+		 * We can have a quick High-Low-High transition in the line.
+		 * In such a case if we have long interrupt latencies,
+		 * we can miss the low event or get twice a high event.
+		 * This workaround will avoid breaking the clock reference
+		 * count when such a situation ocurrs.
+		 */
+		spin_lock(&omap_port->lock);
+		if (!omap_port->wkin_cken) {
+			omap_port->wkin_cken = 1;
+			ssi_clk_enable(ssi);
+		}
+		spin_unlock(&omap_port->lock);
+		dev_dbg(&ssi->device, "Wake in high\n");
+		hsi_event(port, HSI_EVENT_START_RX);
+	} else {
+		dev_dbg(&ssi->device, "Wake in low\n");
+		hsi_event(port, HSI_EVENT_STOP_RX);
+		spin_lock(&omap_port->lock);
+		if (omap_port->wkin_cken) {
+			ssi_clk_disable(ssi);
+			omap_port->wkin_cken = 0;
+		}
+		spin_unlock(&omap_port->lock);
+	}
+}
+
+static irqreturn_t ssi_wake_isr(int irq __maybe_unused, void *ssi_port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(ssi_port);
+
+	tasklet_hi_schedule(&omap_port->wake_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static int __init ssi_port_irq(struct hsi_port *port,
+						struct platform_device *pd)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct resource *irq;
+	int err;
+
+	irq = platform_get_resource(pd, IORESOURCE_IRQ, (port->num * 3) + 1);
+	if (!irq) {
+		dev_err(&port->device, "Port IRQ resource missing\n");
+		return -ENXIO;
+	}
+	omap_port->irq = irq->start;
+	tasklet_init(&omap_port->pio_tasklet, ssi_pio_tasklet,
+							(unsigned long)port);
+	err = devm_request_irq(&pd->dev, omap_port->irq, ssi_pio_isr,
+						IRQF_DISABLED, irq->name, port);
+	if (err < 0)
+		dev_err(&port->device, "Request IRQ %d failed (%d)\n",
+							omap_port->irq, err);
+	return err;
+}
+
+static int __init ssi_wake_irq(struct hsi_port *port,
+						struct platform_device *pd)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct resource *irq;
+	int err;
+
+	irq = platform_get_resource(pd, IORESOURCE_IRQ, (port->num * 3) + 3);
+	if (!irq) {
+		dev_err(&port->device, "Wake in IRQ resource missing");
+		return -ENXIO;
+	}
+	if (irq->flags & IORESOURCE_UNSET) {
+		dev_info(&port->device, "No Wake in support\n");
+		omap_port->wake_irq = -1;
+		return 0;
+	}
+	omap_port->wake_irq = irq->start;
+	tasklet_init(&omap_port->wake_tasklet, ssi_wake_tasklet,
+							(unsigned long)port);
+	err = devm_request_irq(&pd->dev, omap_port->wake_irq, ssi_wake_isr,
+		IRQF_DISABLED | IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+							irq->name, port);
+	if (err < 0)
+		dev_err(&port->device, "Request Wake in IRQ %d failed %d\n",
+						omap_port->wake_irq, err);
+	err = enable_irq_wake(omap_port->wake_irq);
+	if (err < 0)
+		dev_err(&port->device, "Enable wake on the wakeline in irq %d"
+				" failed %d\n", omap_port->wake_irq, err);
+
+	return err;
+}
+
+static void __init ssi_queues_init(struct omap_ssi_port *omap_port)
+{
+	unsigned int ch;
+
+	for (ch = 0; ch < SSI_MAX_CHANNELS; ch++) {
+		INIT_LIST_HEAD(&omap_port->txqueue[ch]);
+		INIT_LIST_HEAD(&omap_port->rxqueue[ch]);
+	}
+	INIT_LIST_HEAD(&omap_port->brkqueue);
+}
+
+static int __init ssi_get_iomem(struct platform_device *pd,
+		unsigned int num, void __iomem **pbase, dma_addr_t *phy)
+{
+	struct resource *mem;
+	struct resource *ioarea;
+	void __iomem *base;
+
+	mem = platform_get_resource(pd, IORESOURCE_MEM, num);
+	if (!mem) {
+		dev_err(&pd->dev, "IO memory region missing (%d)\n", num);
+		return -ENXIO;
+	}
+	ioarea = devm_request_mem_region(&pd->dev, mem->start,
+					resource_size(mem), dev_name(&pd->dev));
+	if (!ioarea) {
+		dev_err(&pd->dev, "%s IO memory region request failed\n",
+								mem->name);
+		return -ENXIO;
+	}
+	base = devm_ioremap(&pd->dev, mem->start, resource_size(mem));
+	if (!base) {
+		dev_err(&pd->dev, "%s IO remap failed\n", mem->name);
+		return -ENXIO;
+	}
+	*pbase = base;
+
+	if (phy)
+		*phy = mem->start;
+
+	return 0;
+}
+
+static int __init ssi_ports_init(struct hsi_controller *ssi,
+					struct platform_device *pd)
+{
+	struct hsi_port *port;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct omap_ssi_port *omap_port;
+	unsigned int i;
+	int err;
+
+	omap_ssi->port = devm_kzalloc(&pd->dev,
+				sizeof(omap_port) * ssi->num_ports, GFP_KERNEL);
+	if (!omap_ssi->port)
+		return -ENOMEM;
+
+	for (i = 0; i < ssi->num_ports; i++) {
+		port = &ssi->port[i];
+		omap_port = devm_kzalloc(&pd->dev, sizeof(*omap_port),
+								GFP_KERNEL);
+		if (!omap_port)
+			return -ENOMEM;
+		port->async = ssi_async;
+		port->setup = ssi_setup;
+		port->flush = ssi_flush;
+		port->start_tx = ssi_start_tx;
+		port->stop_tx = ssi_stop_tx;
+		port->release = ssi_release;
+		hsi_port_set_drvdata(port, omap_port);
+		/* Get SST base addresses*/
+		err = ssi_get_iomem(pd, ((i * 2) + 2), &omap_port->sst_base,
+							&omap_port->sst_dma);
+		if (err < 0)
+			return err;
+		/* Get SSR base addresses */
+		err = ssi_get_iomem(pd, ((i * 2) + 3), &omap_port->ssr_base,
+							&omap_port->ssr_dma);
+		if (err < 0)
+			return err;
+		err = ssi_port_irq(port, pd);
+		if (err < 0)
+			return err;
+		err = ssi_wake_irq(port, pd);
+		if (err < 0)
+			return err;
+		ssi_queues_init(omap_port);
+		spin_lock_init(&omap_port->lock);
+		spin_lock_init(&omap_port->wk_lock);
+		omap_port->dev = &port->device;
+		omap_ssi->port[i] = omap_port;
+	}
+
+	return 0;
+}
+
+static void ssi_ports_exit(struct hsi_controller *ssi)
+{
+	struct omap_ssi_port *omap_port;
+	unsigned int i;
+
+	for (i = 0; i < ssi->num_ports; i++) {
+		omap_port = hsi_port_drvdata(&ssi->port[i]);
+		tasklet_kill(&omap_port->wake_tasklet);
+		tasklet_kill(&omap_port->pio_tasklet);
+	}
+}
+
+static void ssi_clk_release(struct device *dev __maybe_unused, void *res)
+{
+	struct ssi_clk_res *r = res;
+
+	clk_put(r->clk);
+}
+
+static struct clk *__init ssi_devm_clk_get(struct device *dev, const char *id)
+{
+	struct ssi_clk_res *pclk;
+	struct clk *clk;
+
+	pclk = devres_alloc(ssi_clk_release, sizeof(*pclk), GFP_KERNEL);
+	if (!pclk) {
+		dev_err(dev, "Could not allocate the device resource entry\n");
+		return ERR_PTR(-ENOMEM);
+	}
+	clk = clk_get(dev, id);
+	if (IS_ERR(clk)) {
+		dev_err(dev, "clock get %s failed %li\n", id, PTR_ERR(clk));
+		devres_free(pclk);
+	} else {
+		pclk->clk = clk;
+		devres_add(dev, pclk);
+	}
+
+	return clk;
+}
+
+static int __init ssi_add_controller(struct hsi_controller *ssi,
+						struct platform_device *pd)
+{
+	struct omap_ssi_platform_data *omap_ssi_pdata = pd->dev.platform_data;
+	struct omap_ssi_controller *omap_ssi;
+	struct resource *irq;
+	int err;
+
+	omap_ssi = devm_kzalloc(&pd->dev, sizeof(*omap_ssi), GFP_KERNEL);
+	if (!omap_ssi) {
+		dev_err(&pd->dev, "not enough memory for omap ssi\n");
+		return -ENOMEM;
+	}
+	ssi->id = pd->id;
+	ssi->owner = THIS_MODULE;
+	ssi->device.parent = &pd->dev;
+	dev_set_name(&ssi->device, "ssi%d", ssi->id);
+	hsi_controller_set_drvdata(ssi, omap_ssi);
+	omap_ssi->dev = &ssi->device;
+	err = ssi_get_iomem(pd, 0, &omap_ssi->sys, NULL);
+	if (err < 0)
+		return err;
+	err = ssi_get_iomem(pd, 1, &omap_ssi->gdd, NULL);
+	if (err < 0)
+		return err;
+	irq = platform_get_resource(pd, IORESOURCE_IRQ, 0);
+	if (!irq) {
+		dev_err(&pd->dev, "GDD IRQ resource missing\n");
+		return -ENXIO;
+	}
+	omap_ssi->gdd_irq = irq->start;
+	tasklet_init(&omap_ssi->gdd_tasklet, ssi_gdd_tasklet,
+							(unsigned long)ssi);
+	err = devm_request_irq(&pd->dev, omap_ssi->gdd_irq, ssi_gdd_isr,
+						IRQF_DISABLED, irq->name, ssi);
+	if (err < 0) {
+		dev_err(&ssi->device, "Request GDD IRQ %d failed (%d)",
+							omap_ssi->gdd_irq, err);
+		return err;
+	}
+	err = ssi_ports_init(ssi, pd);
+	if (err < 0)
+		return err;
+	omap_ssi->get_loss = omap_ssi_pdata->get_dev_context_loss_count;
+	omap_ssi->max_speed = UINT_MAX;
+	spin_lock_init(&omap_ssi->lock);
+	spin_lock_init(&omap_ssi->ck_lock);
+	omap_ssi->ick = ssi_devm_clk_get(&pd->dev, "ssi_ick");
+	if (IS_ERR(omap_ssi->ick))
+		return PTR_ERR(omap_ssi->ick);
+	omap_ssi->fck = ssi_devm_clk_get(&pd->dev, "ssi_ssr_fck");
+	if (IS_ERR(omap_ssi->fck))
+		return PTR_ERR(omap_ssi->fck);
+	err = hsi_register_controller(ssi);
+
+	return err;
+}
+
+static int __init ssi_hw_init(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	unsigned int i;
+	u32 val;
+	int err;
+
+	err = ssi_clk_enable(ssi);
+	if (err < 0) {
+		dev_err(&ssi->device, "Failed to enable the clocks %d\n", err);
+		return err;
+	}
+	/* Reseting SSI controller */
+	__raw_writel(SSI_SOFTRESET, omap_ssi->sys + SSI_SYSCONFIG_REG);
+	val = __raw_readl(omap_ssi->sys + SSI_SYSSTATUS_REG);
+	for (i = 0; ((i < 20) && !(val & SSI_RESETDONE)); i++) {
+		msleep(20);
+		val = __raw_readl(omap_ssi->sys + SSI_SYSSTATUS_REG);
+	}
+	if (!(val & SSI_RESETDONE)) {
+		dev_err(&ssi->device, "SSI HW reset failed\n");
+		ssi_clk_disable(ssi);
+		return -EIO;
+	}
+	/* Reseting GDD */
+	__raw_writel(SSI_SWRESET, omap_ssi->gdd + SSI_GDD_GRST_REG);
+	/* Get FCK rate */
+	omap_ssi->fck_rate = clk_get_rate(omap_ssi->fck) / 1000; /* KHz */
+	dev_dbg(&ssi->device, "SSI fck rate %lu KHz\n", omap_ssi->fck_rate);
+	/* Set default PM settings */
+	val = SSI_AUTOIDLE | SSI_SIDLEMODE_SMART | SSI_MIDLEMODE_SMART;
+	__raw_writel(val, omap_ssi->sys + SSI_SYSCONFIG_REG);
+	omap_ssi->sysconfig = val;
+	__raw_writel(SSI_CLK_AUTOGATING_ON, omap_ssi->sys + SSI_GDD_GCR_REG);
+	omap_ssi->gdd_gcr = SSI_CLK_AUTOGATING_ON;
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static void ssi_remove_controller(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	ssi_ports_exit(ssi);
+	tasklet_kill(&omap_ssi->gdd_tasklet);
+	hsi_unregister_controller(ssi);
+}
+
+static int __init ssi_probe(struct platform_device *pd)
+{
+	struct omap_ssi_platform_data *omap_ssi_pdata = pd->dev.platform_data;
+	struct hsi_controller *ssi;
+	int err;
+
+	if (!omap_ssi_pdata) {
+		dev_err(&pd->dev, "No OMAP SSI platform data\n");
+		return -EINVAL;
+	}
+	ssi = hsi_alloc_controller(omap_ssi_pdata->num_ports, GFP_KERNEL);
+	if (!ssi) {
+		dev_err(&pd->dev, "No memory for controller\n");
+		return -ENOMEM;
+	}
+	platform_set_drvdata(pd, ssi);
+	err = ssi_add_controller(ssi, pd);
+	if (err < 0)
+		goto out1;
+	err = ssi_hw_init(ssi);
+	if (err < 0)
+		goto out2;
+#ifdef CONFIG_DEBUG_FS
+	err = ssi_debug_add_ctrl(ssi);
+	if (err < 0)
+		goto out2;
+#endif
+	return err;
+out2:
+	ssi_remove_controller(ssi);
+out1:
+	platform_set_drvdata(pd, NULL);
+	hsi_free_controller(ssi);
+
+	return err;
+}
+
+static int __exit ssi_remove(struct platform_device *pd)
+{
+	struct hsi_controller *ssi = platform_get_drvdata(pd);
+
+#ifdef CONFIG_DEBUG_FS
+	ssi_debug_remove_ctrl(ssi);
+#endif
+	ssi_remove_controller(ssi);
+	platform_set_drvdata(pd, NULL);
+	hsi_free_controller(ssi);
+
+	return 0;
+}
+
+static struct platform_driver ssi_pdriver = {
+	.remove	= __exit_p(ssi_remove),
+	.driver	= {
+			.name	= "omap_ssi",
+			.owner	= THIS_MODULE,
+	},
+};
+
+static int __init omap_ssi_init(void)
+{
+	pr_info("OMAP SSI hw driver loaded\n");
+	return platform_driver_probe(&ssi_pdriver, ssi_probe);
+}
+module_init(omap_ssi_init);
+
+static void __exit omap_ssi_exit(void)
+{
+	platform_driver_unregister(&ssi_pdriver);
+	pr_info("OMAP SSI driver removed\n");
+}
+module_exit(omap_ssi_exit);
+
+MODULE_ALIAS("platform:omap_ssi");
+MODULE_AUTHOR("Carlos Chinea <carlos.chinea@nokia.com>");
+MODULE_DESCRIPTION("Synchronous Serial Interface Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hsi/controllers/ste_hsi.c b/drivers/hsi/controllers/ste_hsi.c
new file mode 100644
index 00000000000..8ac1363c14c
--- /dev/null
+++ b/drivers/hsi/controllers/ste_hsi.c
@@ -0,0 +1,1698 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Marcin Mielczarczyk <marcin.mielczarczyk@tieto.com> for ST-Ericsson
+ * Author: Lukasz Baj <lukasz.baj@tieto.com> for ST-Ericsson
+ */
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/hsi/hsi.h>
+#include <linux/regulator/consumer.h>
+
+#ifdef CONFIG_STE_DMA40
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#endif
+
+#include <mach/hsi.h>
+
+/*
+ * Copy of HSIR/HSIT context for restoring after HW reset (Vape power off).
+ */
+struct ste_hsi_hw_context {
+	unsigned int tx_mode;
+	unsigned int tx_divisor;
+	unsigned int tx_channels;
+	unsigned int rx_mode;
+	unsigned int rx_channels;
+};
+
+/**
+ * struct ste_hsi_controller - STE HSI controller data
+ * @dev: device associated to STE HSI controller
+ * @tx_dma_base: HSI TX peripheral physical address
+ * @rx_dma_base: HSI RX peripheral physical address
+ * @rx_base: HSI RX peripheral virtual address
+ * @tx_base: HSI TX peripheral virtual address
+ * @regulator: STE HSI Vape consumer regulator
+ * @context: copy of client-configured HSI TX / HSI RX registers
+ * @tx_clk: HSI TX core clock (HSITXCLK)
+ * @rx_clk: HSI RX core clock (HSIRXCLK)
+ * @ssitx_clk: HSI TX host clock (HCLK)
+ * @ssirx_clk: HSI RX host clock (HCLK)
+ * @clk_work: structure for delayed HSI clock disabling
+ * @overrun_irq: HSI channels overrun IRQ table
+ * @ck_refcount: reference count for clock enable operation
+ * @ck_lock: locking primitive for HSI clocks
+ * @lock: locking primitive for HSI controller
+ * @use_dma: flag for DMA enabled
+ * @ck_on: flag for HSI clocks enabled
+ */
+struct ste_hsi_controller {
+	struct device *dev;
+	dma_addr_t tx_dma_base;
+	dma_addr_t rx_dma_base;
+	unsigned char __iomem *rx_base;
+	unsigned char __iomem *tx_base;
+	struct regulator *regulator;
+	struct ste_hsi_hw_context *context;
+	struct clk *tx_clk;
+	struct clk *rx_clk;
+	struct clk *ssitx_clk;
+	struct clk *ssirx_clk;
+	struct delayed_work clk_work;
+	int overrun_irq[STE_HSI_MAX_CHANNELS];
+	int ck_refcount;
+	spinlock_t ck_lock;
+	spinlock_t lock;
+	unsigned int use_dma:1;
+	unsigned int ck_on:1;
+};
+
+#ifdef CONFIG_STE_DMA40
+struct ste_hsi_channel_dma {
+	struct dma_chan *dma_chan;
+	struct dma_async_tx_descriptor *desc;
+	dma_cookie_t cookie;
+};
+#endif
+
+struct ste_hsi_port {
+	struct device *dev;
+	struct list_head txqueue[STE_HSI_MAX_CHANNELS];
+	struct list_head rxqueue[STE_HSI_MAX_CHANNELS];
+	struct list_head brkqueue;
+	int tx_irq;
+	int rx_irq;
+	int excep_irq;
+	struct tasklet_struct rx_tasklet;
+	struct tasklet_struct tx_tasklet;
+	struct tasklet_struct exception_tasklet;
+	struct tasklet_struct overrun_tasklet;
+	unsigned char channels;
+#ifdef CONFIG_STE_DMA40
+	struct ste_hsi_channel_dma tx_dma[STE_HSI_MAX_CHANNELS];
+	struct ste_hsi_channel_dma rx_dma[STE_HSI_MAX_CHANNELS];
+#endif
+};
+
+#define hsi_to_ste_port(port) (hsi_port_drvdata(port))
+#define hsi_to_ste_controller(con) (hsi_controller_drvdata(con))
+#define client_to_ste_port(cl) (hsi_port_drvdata(hsi_get_port(cl)))
+#define client_to_hsi(cl) \
+	(to_hsi_controller(hsi_get_port(cl)->device.parent))
+#define client_to_ste_controller(cl)  \
+	(hsi_controller_drvdata(client_to_hsi(cl)))
+#define ste_port_to_ste_controller(port) \
+	((struct ste_hsi_controller *)hsi_controller_drvdata(	\
+		to_hsi_controller(port->dev->parent)))
+
+static u32 ste_hsir_periphid[8] = { 0x2C, 0, 0x8, 0x18, 0xD, 0xF0, 0x5, 0xB1 };
+static u32 ste_hsit_periphid[8] = { 0x2B, 0, 0x8, 0x18, 0xD, 0xF0, 0x5, 0xB1 };
+
+/*
+ * linux/amba/bus.h macros can not be used, because 8 bytes are validated:
+ * PERIPHID0..3 and PCELLID0..3 for HSIR and HSIT.
+ */
+static inline int compare_periphid(u32 *id1, u32 *id2, int count)
+{
+	while (count && *id1++ == *id2++)
+		count--;
+
+	return count;
+}
+
+static void ste_hsi_clk_free(struct clk **pclk)
+{
+	if (IS_ERR(*pclk) && *pclk != NULL)
+		clk_put(*pclk);
+	*pclk = NULL;
+}
+
+static void ste_hsi_init_registers(struct ste_hsi_controller *ste_hsi)
+{
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_BUFSTATE);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_FLUSHBITS);
+	/* TO DO: TX channel priorities will be implemented later */
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_PRIORITY);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_BURSTLEN);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_PREAMBLE);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_DATASWAP);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_DMAEN);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKID);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIC);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+
+	/* 0x23 is reset value per DB8500 Design Spec */
+	writel(0x23, ste_hsi->rx_base + STE_HSI_RX_THRESHOLD);
+
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_BUFSTATE);
+
+	/* HSIR clock recovery mode */
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_DETECTOR);
+
+	/* Bits 0,1,2 set to 1 to clear exception flags */
+	writel(0x07, ste_hsi->rx_base + STE_HSI_RX_ACK);
+
+	/* Bits 0..7 set to 1 to clear OVERRUN IRQ  */
+	writel(0xFF, ste_hsi->rx_base + STE_HSI_RX_OVERRUNACK);
+
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_DMAEN);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIC);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM);
+
+	/* Flush all errors */
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_EXCEP);
+
+	/* 2 is Flush state, no RX exception generated afterwards */
+	writel(2, ste_hsi->rx_base + STE_HSI_RX_STATE);
+
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+}
+
+static void ste_hsi_setup_registers(struct ste_hsi_controller *ste_hsi)
+{
+	unsigned int buffers, i;
+	struct ste_hsi_hw_context *pcontext = ste_hsi->context;
+
+	/*
+	 * Configure TX
+	 */
+	writel(pcontext->tx_mode, ste_hsi->tx_base + STE_HSI_TX_MODE);
+	writel(pcontext->tx_divisor, ste_hsi->tx_base + STE_HSI_TX_DIVISOR);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_PARITY);
+	writel(pcontext->tx_channels, ste_hsi->tx_base + STE_HSI_TX_CHANNELS);
+	/* Calculate buffers number per channel */
+	buffers = STE_HSI_MAX_BUFFERS / pcontext->tx_channels;
+	for (i = 0; i < pcontext->tx_channels; i++) {
+		/* Set 32 bit long frames */
+		writel(31, ste_hsi->tx_base + STE_HSI_TX_FRAMELENX + 4 * i);
+		writel(buffers * i,
+		       ste_hsi->tx_base + STE_HSI_TX_BASEX + 4 * i);
+		writel(buffers - 1,
+		       ste_hsi->tx_base + STE_HSI_TX_SPANX + 4 * i);
+		writel(buffers - 1,
+		       ste_hsi->tx_base + STE_HSI_TX_WATERMARKX + 4 * i);
+		writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKX + 4 * i);
+	}
+
+	/*
+	 * The value read from this register gives the synchronized status
+	 * of the transmitter state and this synchronization takes 2 HSITCLK
+	 * cycles plus 3 HCLK cycles.
+	 */
+	while (STE_HSI_STATE_IDLE != readl(ste_hsi->tx_base + STE_HSI_TX_STATE))
+		cpu_relax();
+
+	/*
+	 * Configure RX
+	 */
+	writel(pcontext->rx_mode, ste_hsi->rx_base + STE_HSI_RX_MODE);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_PARITY);
+	writel(pcontext->rx_channels, ste_hsi->rx_base + STE_HSI_RX_CHANNELS);
+	/* Calculate buffers number per channel */
+	buffers = STE_HSI_MAX_BUFFERS / pcontext->rx_channels;
+	for (i = 0; i < pcontext->rx_channels; i++) {
+		/* Set 32 bit long frames */
+		writel(31, ste_hsi->rx_base + STE_HSI_RX_FRAMELENX + 4 * i);
+		writel(buffers * i,
+		       ste_hsi->rx_base + STE_HSI_RX_BASEX + 4 * i);
+		writel(buffers - 1,
+		       ste_hsi->rx_base + STE_HSI_RX_SPANX + 4 * i);
+		writel(buffers - 1,
+		       ste_hsi->rx_base + STE_HSI_RX_WATERMARKX + 4 * i);
+		writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKX + 4 * i);
+	}
+
+	/*
+	 * The value read from this register gives the synchronized status
+	 * of the receiver state and this synchronization takes 2 HSIRCLK
+	 * cycles plus 3 HCLK cycles.
+	 */
+	while (STE_HSI_STATE_IDLE != readl(ste_hsi->tx_base + STE_HSI_RX_STATE))
+		cpu_relax();
+}
+
+/*
+ * When cpuidle framework is setting the sleep or deep sleep state then
+ * the Vape is OFF. This results in re-setting the HSIT/HSIR registers
+ * to default (idle) values.
+ * Function ste_hsi_context() is checking and restoring the HSI registers
+ * to these set by the HSI client by ste_hsi_setup().
+ */
+static void ste_hsi_context(struct ste_hsi_controller *ste_hsi)
+{
+	unsigned int tx_channels;
+	unsigned int rx_channels;
+
+
+	tx_channels = readl(ste_hsi->tx_base + STE_HSI_TX_CHANNELS);
+	rx_channels = readl(ste_hsi->rx_base + STE_HSI_RX_CHANNELS);
+
+	/*
+	 * Checking if the context was lost.
+	 * The target config is at least 2 channels for both TX and RX.
+	 * TX and RX channels are set to 1 after HW reset.
+	 */
+	if ((ste_hsi->context->tx_channels != tx_channels) ||
+		(ste_hsi->context->rx_channels != rx_channels)) {
+		/*
+		 * TO DO: remove "dev_info" after thorough testing.
+		 * Debug left for getting the statistics how frequently the context
+		 * is lost during regular HSI operation.
+		 */
+		dev_info(ste_hsi->dev, "context\n");
+
+		ste_hsi_init_registers(ste_hsi);
+		ste_hsi_setup_registers(ste_hsi);
+	}
+}
+
+static void ste_hsi_clks_free(struct ste_hsi_controller *ste_hsi)
+{
+	ste_hsi_clk_free(&ste_hsi->rx_clk);
+	ste_hsi_clk_free(&ste_hsi->tx_clk);
+	ste_hsi_clk_free(&ste_hsi->ssirx_clk);
+	ste_hsi_clk_free(&ste_hsi->ssitx_clk);
+}
+
+static int ste_hsi_clock_enable(struct hsi_controller *hsi)
+{
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	int err = 0;
+
+	spin_lock_bh(&ste_hsi->ck_lock);
+	if (ste_hsi->ck_refcount++ || ste_hsi->ck_on)
+		goto out;
+
+	err = clk_enable(ste_hsi->ssirx_clk);
+	if (unlikely(err))
+		goto out;
+
+	err = clk_enable(ste_hsi->ssitx_clk);
+	if (unlikely(err)) {
+		clk_disable(ste_hsi->ssirx_clk);
+		goto out;
+	}
+
+	err = clk_enable(ste_hsi->rx_clk);
+	if (unlikely(err)) {
+		clk_disable(ste_hsi->ssitx_clk);
+		clk_disable(ste_hsi->ssirx_clk);
+		goto out;
+	}
+
+	err = clk_enable(ste_hsi->tx_clk);
+	if (unlikely(err)) {
+		clk_disable(ste_hsi->rx_clk);
+		clk_disable(ste_hsi->ssitx_clk);
+		clk_disable(ste_hsi->ssirx_clk);
+		goto out;
+	}
+
+	ste_hsi->ck_on = 1;
+out:
+	if (err)
+		ste_hsi->ck_refcount--;
+
+	spin_unlock_bh(&ste_hsi->ck_lock);
+
+	return err;
+}
+
+static void ste_hsi_delayed_disable_clock(struct work_struct *work)
+{
+	struct ste_hsi_controller *ste_hsi;
+	ste_hsi = container_of(work, struct ste_hsi_controller, clk_work.work);
+
+	spin_lock_bh(&ste_hsi->ck_lock);
+
+	/*
+	 * If clock should not be off (enable clock called in meantime)
+	 * or clock is already off nothing to do
+	 */
+	if (ste_hsi->ck_refcount || !ste_hsi->ck_on)
+		goto out;
+
+	if (readl(ste_hsi->tx_base + STE_HSI_TX_STATE) != STE_HSI_STATE_IDLE ||
+	    readl(ste_hsi->rx_base + STE_HSI_RX_STATE)
+	    != STE_HSI_STATE_IDLE ||
+	    readl(ste_hsi->rx_base + STE_HSI_RX_BUFSTATE) != 0) {
+		/* Try again later */
+		int err = schedule_delayed_work(&ste_hsi->clk_work, HZ);
+		if (err < 0)
+			dev_err(ste_hsi->dev, "Error scheduling work\n");
+		goto out;
+	}
+
+	/* Actual clocks disable */
+	clk_disable(ste_hsi->tx_clk);
+	clk_disable(ste_hsi->rx_clk);
+	clk_disable(ste_hsi->ssitx_clk);
+	clk_disable(ste_hsi->ssirx_clk);
+	ste_hsi->ck_on = 0;
+
+out:
+	spin_unlock_bh(&ste_hsi->ck_lock);
+}
+
+static void ste_hsi_clock_disable(struct hsi_controller *hsi)
+{
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+
+	spin_lock_bh(&ste_hsi->ck_lock);
+
+	/* Sanity check */
+	if (ste_hsi->ck_refcount <= 0)
+		WARN_ON(ste_hsi->ck_refcount <= 0);
+
+	/* Need clock to be disable now? */
+	if (--ste_hsi->ck_refcount)
+		goto out;
+
+	/*
+	 * If receiver or transmitter is in the middle something delay clock off
+	 */
+	if (readl(ste_hsi->tx_base + STE_HSI_TX_STATE) != STE_HSI_STATE_IDLE ||
+	    readl(ste_hsi->rx_base + STE_HSI_RX_STATE)
+	    != STE_HSI_STATE_IDLE ||
+	    readl(ste_hsi->rx_base + STE_HSI_RX_BUFSTATE) != 0) {
+		int err = schedule_delayed_work(&ste_hsi->clk_work, HZ);
+		if (err < 0)
+			dev_err(&hsi->device, "Error scheduling work\n");
+
+		goto out;
+	}
+
+	/* Actual clocks disabled */
+	clk_disable(ste_hsi->tx_clk);
+	clk_disable(ste_hsi->rx_clk);
+	clk_disable(ste_hsi->ssitx_clk);
+	clk_disable(ste_hsi->ssirx_clk);
+	ste_hsi->ck_on = 0;
+
+out:
+	spin_unlock_bh(&ste_hsi->ck_lock);
+}
+
+static int ste_hsi_start_irq(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	u32 val;
+	int err;
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		return err;
+
+	ste_hsi_context(ste_hsi);
+
+	msg->actual_len = 0;
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	if (msg->ttype == HSI_MSG_WRITE) {
+		val = readl(ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM) |
+		    (1 << msg->channel);
+		writel(val, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+	} else {
+		val = readl(ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM) |
+		    (1 << msg->channel);
+		writel(val, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+
+		val = readl(ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM) |
+		    (1 << msg->channel);
+		writel(val, ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM);
+	}
+
+	return 0;
+}
+
+static int ste_hsi_start_transfer(struct ste_hsi_port *ste_port,
+				  struct list_head *queue);
+#ifdef CONFIG_STE_DMA40
+static void ste_hsi_dma_callback(void *dma_async_param)
+{
+	struct hsi_msg *msg = dma_async_param;
+	struct hsi_controller *hsi = client_to_hsi(msg->cl);
+	struct ste_hsi_port *ste_port = client_to_ste_port(msg->cl);
+	struct ste_hsi_controller *ste_hsi = client_to_ste_controller(msg->cl);
+	struct list_head *queue;
+	struct dma_chan *chan;
+	struct ste_hsi_channel_dma *hsi_dma_chan;
+	char *dma_enable_address;
+	enum dma_data_direction direction;
+	u32 dma_mask;
+
+	/* Message finished, remove from list and notify client */
+	spin_lock_bh(&ste_hsi->lock);
+	list_del(&msg->link);
+
+	if (msg->ttype == HSI_MSG_WRITE) {
+		queue = &ste_port->txqueue[msg->channel];
+		direction = DMA_TO_DEVICE;
+		dma_enable_address = ste_hsi->tx_base + STE_HSI_TX_DMAEN;
+		hsi_dma_chan = &ste_port->tx_dma[msg->channel];
+	} else {
+		queue = &ste_port->rxqueue[msg->channel];
+		direction = DMA_FROM_DEVICE;
+		dma_enable_address = ste_hsi->rx_base + STE_HSI_RX_DMAEN;
+		hsi_dma_chan = &ste_port->rx_dma[msg->channel];
+	}
+
+	dma_sync_sg_for_cpu(&hsi->device, msg->sgt.sgl,
+			    msg->sgt.nents, direction);
+	chan = hsi_dma_chan->dma_chan;
+
+	/* disable DMA channel on HSI controller */
+	dma_mask = readl(dma_enable_address);
+	writel(dma_mask & ~(1 << msg->channel), dma_enable_address);
+
+	hsi_dma_chan->desc = NULL;
+
+	dma_unmap_sg(&hsi->device, msg->sgt.sgl, msg->sgt.nents, direction);
+
+	msg->status = HSI_STATUS_COMPLETED;
+	msg->actual_len = sg_dma_len(msg->sgt.sgl);
+
+	spin_unlock_bh(&ste_hsi->lock);
+
+	msg->complete(msg);
+
+	ste_hsi_clock_disable(hsi);
+
+	spin_lock_bh(&ste_hsi->lock);
+	ste_hsi_start_transfer(ste_port, queue);
+	spin_unlock_bh(&ste_hsi->lock);
+}
+
+static void dma_device_control(struct ste_hsi_channel_dma *chan,
+			       enum dma_ctrl_cmd cmd, unsigned long arg)
+{
+	chan->dma_chan->device->device_control(chan->dma_chan, cmd, arg);
+}
+
+static void ste_hsi_terminate_dma_chan(struct ste_hsi_channel_dma *chan)
+{
+	if (chan->desc) {
+		dma_device_control(chan, DMA_TERMINATE_ALL, 0);
+		chan->desc = NULL;
+	}
+	chan->cookie = 0;
+}
+
+static void ste_hsi_terminate_dma(struct ste_hsi_port *ste_port)
+{
+	int i;
+
+	for (i = 0; i < ste_port->channels; ++i) {
+		ste_hsi_terminate_dma_chan(&ste_port->tx_dma[i]);
+		ste_hsi_terminate_dma_chan(&ste_port->rx_dma[i]);
+	}
+}
+
+static int ste_hsi_start_dma(struct hsi_msg *msg)
+{
+	struct hsi_controller *hsi = client_to_hsi(msg->cl);
+	struct ste_hsi_port *ste_port = client_to_ste_port(msg->cl);
+	struct ste_hsi_controller *ste_hsi = client_to_ste_controller(msg->cl);
+	struct dma_async_tx_descriptor *desc;
+	struct dma_chan *chan;
+	struct ste_hsi_channel_dma *hsi_dma_chan;
+	char *dma_enable_address;
+	enum dma_data_direction direction;
+	u32 dma_mask;
+	int err;
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		return err;
+
+	ste_hsi_context(ste_hsi);
+
+	if (msg->ttype == HSI_MSG_WRITE) {
+		direction = DMA_TO_DEVICE;
+		dma_enable_address = ste_hsi->tx_base + STE_HSI_TX_DMAEN;
+		hsi_dma_chan = &ste_port->tx_dma[msg->channel];
+	} else {
+		u32 val;
+		direction = DMA_FROM_DEVICE;
+		dma_enable_address = ste_hsi->rx_base + STE_HSI_RX_DMAEN;
+		hsi_dma_chan = &ste_port->rx_dma[msg->channel];
+
+		/* enable overrun for this channel */
+		val = readl(ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM) |
+		    (1 << msg->channel);
+		writel(val, ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM);
+	}
+
+	chan = hsi_dma_chan->dma_chan;
+
+	if (0 == dma_map_sg(&hsi->device, msg->sgt.sgl, msg->sgt.nents,
+			    direction)) {
+		dev_dbg(&hsi->device, "DMA map SG failed !\n");
+		err = -ENOMEM;
+		goto out;
+	}
+	/* Prepare the scatterlist */
+	desc = chan->device->device_prep_slave_sg(chan,
+						  msg->sgt.sgl,
+						  msg->sgt.nents,
+						  direction,
+						  DMA_PREP_INTERRUPT |
+						  DMA_CTRL_ACK);
+
+	if (!desc) {
+		dma_unmap_sg(&hsi->device, msg->sgt.sgl, msg->sgt.nents,
+			     direction);
+		/* "Complete" DMA (errorpath) */
+		ste_hsi_terminate_dma_chan(hsi_dma_chan);
+		err = -EBUSY;
+		goto out;
+	}
+	desc->callback = ste_hsi_dma_callback;
+	desc->callback_param = msg;
+	hsi_dma_chan->cookie = desc->tx_submit(desc);
+	hsi_dma_chan->desc = desc;
+
+	/* Fire the DMA transaction */
+	chan->device->device_issue_pending(chan);
+
+	/* Enable DMA channel on HSI controller */
+	dma_mask = readl(dma_enable_address);
+	writel(dma_mask | 1 << msg->channel, dma_enable_address);
+
+out:
+	if (unlikely(err))
+		ste_hsi_clock_disable(hsi);
+
+	return err;
+}
+
+static void __init ste_hsi_init_dma(struct ste_hsi_platform_data *data,
+				    struct hsi_controller *hsi)
+{
+	struct hsi_port *port;
+	struct ste_hsi_port *ste_port;
+	struct ste_hsi_controller *ste_hsi = hsi_to_ste_controller(hsi);
+	dma_cap_mask_t mask;
+	int i, ch;
+
+	ste_hsi->use_dma = 1;
+	/* Try to acquire a generic DMA engine slave channel */
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	for (i = 0; i < hsi->num_ports; ++i) {
+		port = &hsi->port[i];
+		ste_port = hsi_port_drvdata(port);
+
+		for (ch = 0; ch < STE_HSI_MAX_CHANNELS; ++ch) {
+			ste_port->tx_dma[ch].dma_chan =
+			    dma_request_channel(mask,
+						data->port_cfg[i].dma_filter,
+						&data->port_cfg[i].
+						dma_tx_cfg[ch]);
+
+			ste_port->rx_dma[ch].dma_chan =
+			    dma_request_channel(mask,
+						data->port_cfg[i].dma_filter,
+						&data->port_cfg[i].
+						dma_rx_cfg[ch]);
+		}
+	}
+}
+
+static int ste_hsi_setup_dma(struct hsi_client *cl)
+{
+	int i;
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct dma_slave_config rx_conf = {
+		.src_addr = 0,	/* dynamic data */
+		.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+		.direction = DMA_FROM_DEVICE,
+		.src_maxburst = 1,
+	};
+	struct dma_slave_config tx_conf = {
+		.dst_addr = 0,	/* dynamic data */
+		.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+		.direction = DMA_TO_DEVICE,
+		.dst_maxburst = 1,
+	};
+
+	if (!ste_hsi->use_dma)
+		return 0;
+
+	for (i = 0; i < ste_port->channels; ++i) {
+		struct dma_chan *chan;
+
+		chan = ste_port->tx_dma[i].dma_chan;
+		tx_conf.dst_addr = (dma_addr_t) ste_hsi->tx_dma_base +
+		    STE_HSI_TX_BUFFERX + 4 * i;
+		chan->device->device_control(chan,
+					     DMA_SLAVE_CONFIG,
+					     (unsigned long)&tx_conf);
+
+		chan = ste_port->rx_dma[i].dma_chan;
+		rx_conf.src_addr = (dma_addr_t) ste_hsi->rx_dma_base +
+		    STE_HSI_RX_BUFFERX + 4 * i;
+		chan->device->device_control(chan,
+					     DMA_SLAVE_CONFIG,
+					     (unsigned long)&rx_conf);
+	}
+
+	return 0;
+}
+
+#else
+#define ste_hsi_init_dma(data, hsi) do { } while (0)
+#define ste_hsi_start_dma ste_hsi_start_irq
+#define ste_hsi_terminate_dma(ste_port) do { } while (0)
+#define ste_hsi_setup_dma(cl) do { } while (0)
+#endif
+
+static int ste_hsi_start_transfer(struct ste_hsi_port *ste_port,
+				  struct list_head *queue)
+{
+	struct hsi_msg *msg;
+	int err;
+
+	if (list_empty(queue))
+		return 0;
+
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if (msg->status != HSI_STATUS_QUEUED)
+		return 0;
+
+	msg->actual_len = 0;
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	if (ste_port_to_ste_controller(ste_port)->use_dma)
+		err = ste_hsi_start_dma(msg);
+	else
+		err = ste_hsi_start_irq(msg);
+
+	return err;
+}
+
+static void ste_hsi_receive_data(struct hsi_port *port, unsigned int channel)
+{
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct list_head *queue = &ste_port->rxqueue[channel];
+	struct hsi_msg *msg;
+	char *bufferx;
+	u8 *buf;
+	int span;
+
+	spin_lock_bh(&ste_hsi->lock);
+
+	if (list_empty(queue))
+		goto out;
+
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if ((!msg->sgt.nents) || (!msg->sgt.sgl->length)) {
+		msg->actual_len = 0;
+		msg->status = HSI_STATUS_PENDING;
+	}
+
+	if (msg->status == HSI_STATUS_PROCEEDING && msg->ttype == HSI_MSG_READ) {
+		unsigned char len;
+		bufferx = ste_hsi->rx_base + STE_HSI_RX_BUFFERX + 4 * channel;
+
+		len = readl(ste_hsi->rx_base + STE_HSI_RX_GAUGEX + 4 * channel);
+		buf = sg_virt(msg->sgt.sgl);
+		buf += msg->actual_len;
+		while (len--) {
+			*(u32 *) buf = readl(bufferx);
+			buf += 4;
+			msg->actual_len += 4;
+			if (msg->actual_len >= msg->sgt.sgl->length) {
+				msg->status = HSI_STATUS_COMPLETED;
+				break;
+			}
+		}
+	}
+
+	/* re-enable interrupt by watermark manipulation */
+	span = readl(ste_hsi->rx_base + STE_HSI_RX_SPANX + 4 * channel);
+	writel(span, ste_hsi->rx_base + STE_HSI_RX_WATERMARKX + 4 * channel);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKX + 4 * channel);
+
+	/*
+	 * If message was not transmitted completely enable interrupt for
+	 * further work
+	 */
+	if (msg->status == HSI_STATUS_PROCEEDING) {
+		u32 val;
+		val = readl(ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM) |
+		    (1 << channel);
+		writel(val, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+		goto out;
+	}
+
+	/* Message finished, remove from list and notify client */
+	list_del(&msg->link);
+	spin_unlock_bh(&ste_hsi->lock);
+	msg->complete(msg);
+
+	ste_hsi_clock_disable(hsi);
+
+	spin_lock_bh(&ste_hsi->lock);
+
+	ste_hsi_start_transfer(ste_port, queue);
+out:
+	spin_unlock_bh(&ste_hsi->lock);
+}
+
+static void ste_hsi_transmit_data(struct hsi_port *port, unsigned int channel)
+{
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct list_head *queue = &ste_port->txqueue[channel];
+	struct hsi_msg *msg;
+	u8 *buf;
+	int span;
+
+	if (list_empty(queue))
+		return;
+
+	spin_lock_bh(&ste_hsi->lock);
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if ((!msg->sgt.nents) || (!msg->sgt.sgl->length)) {
+		msg->actual_len = 0;
+		msg->status = HSI_STATUS_PENDING;
+	}
+
+	if (msg->status == HSI_STATUS_PROCEEDING &&
+		msg->ttype == HSI_MSG_WRITE) {
+		unsigned char free_space;
+
+		free_space = readl(ste_hsi->tx_base +
+				   STE_HSI_TX_GAUGEX + 4 * channel);
+		buf = sg_virt(msg->sgt.sgl);
+		buf += msg->actual_len;
+		while (free_space--) {
+			writel(*(u32 *) buf, ste_hsi->tx_base +
+			       STE_HSI_TX_BUFFERX + 4 * channel);
+			buf += 4;
+			msg->actual_len += 4;
+			if (msg->actual_len >= msg->sgt.sgl->length) {
+				msg->status = HSI_STATUS_COMPLETED;
+				break;
+			}
+		}
+	}
+
+	span = readl(ste_hsi->tx_base + STE_HSI_TX_SPANX + 4 * channel);
+	writel(span, ste_hsi->tx_base + STE_HSI_TX_WATERMARKX + 4 * channel);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKX + 4 * channel);
+
+	if (msg->status == HSI_STATUS_PROCEEDING) {
+		u32 val;
+		val = readl(ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM) |
+		    (1 << channel);
+		writel(val, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+		goto out;
+	}
+
+	list_del(&msg->link);
+	spin_unlock_bh(&ste_hsi->lock);
+	msg->complete(msg);
+
+	ste_hsi_clock_disable(hsi);
+
+	spin_lock_bh(&ste_hsi->lock);
+	ste_hsi_start_transfer(ste_port, queue);
+out:
+	spin_unlock_bh(&ste_hsi->lock);
+}
+
+static void ste_hsi_rx_tasklet(unsigned long data)
+{
+	struct hsi_port *port = (struct hsi_port *)data;
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	u32 irq_status, irq_mask;
+	unsigned int i;
+
+	irq_status = readl(ste_hsi->rx_base + STE_HSI_RX_WATERMARKIS);
+	if (!irq_status)
+		goto out;
+
+	irq_mask = readl(ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+	writel(irq_mask & ~irq_status,
+	       ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+	writel(irq_mask, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIC);
+
+	for (i = 0; i < STE_HSI_MAX_CHANNELS; ++i) {
+		if (irq_status & (1 << i))
+			ste_hsi_receive_data(port, i);
+	}
+out:
+	enable_irq(ste_port->rx_irq);
+}
+
+static irqreturn_t ste_hsi_rx_isr(int irq, void *data)
+{
+	struct hsi_port *port = data;
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+
+	disable_irq_nosync(irq);
+	tasklet_hi_schedule(&ste_port->rx_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t ste_hsi_tx_isr(int irq, void *data)
+{
+	struct hsi_port *port = data;
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+
+	disable_irq_nosync(irq);
+	tasklet_hi_schedule(&ste_port->tx_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static void ste_hsi_tx_tasklet(unsigned long data)
+{
+	struct hsi_port *port = (struct hsi_port *)data;
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	u32 irq_status, irq_mask;
+	unsigned int i;
+
+	irq_status = readl(ste_hsi->tx_base + STE_HSI_TX_WATERMARKIS);
+	if (!irq_status)
+		goto out;
+
+	irq_mask = readl(ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+	writel(irq_mask & ~irq_status,
+	       ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+	writel(irq_mask, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIC);
+
+	for (i = 0; i < STE_HSI_MAX_CHANNELS; ++i) {
+		if (irq_status & (1 << i))
+			ste_hsi_transmit_data(port, i);
+	}
+out:
+	enable_irq(ste_port->tx_irq);
+}
+
+static void ste_hsi_break_complete(struct hsi_port *port,
+				   struct ste_hsi_controller *ste_hsi)
+{
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_msg *msg, *tmp;
+	u32 mask;
+
+	dev_dbg(port->device.parent, "HWBREAK received\n");
+
+	spin_lock_bh(&ste_hsi->lock);
+
+	mask = readl(ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+	writel(mask & ~STE_HSI_EXCEP_BREAK,
+	       ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+
+	spin_unlock_bh(&ste_hsi->lock);
+
+	list_for_each_entry_safe(msg, tmp, &ste_port->brkqueue, link) {
+		msg->status = HSI_STATUS_COMPLETED;
+		list_del(&msg->link);
+		msg->complete(msg);
+	}
+}
+
+static void ste_hsi_error(struct hsi_port *port)
+{
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_msg *msg;
+	unsigned int i;
+
+	for (i = 0; i < ste_port->channels; i++) {
+		if (list_empty(&ste_port->rxqueue[i]))
+			continue;
+		msg = list_first_entry(&ste_port->rxqueue[i], struct hsi_msg,
+				       link);
+		list_del(&msg->link);
+		msg->status = HSI_STATUS_ERROR;
+		msg->complete(msg);
+		/* Now restart queued reads if any */
+		ste_hsi_start_transfer(ste_port, &ste_port->rxqueue[i]);
+	}
+}
+
+static void ste_hsi_exception_tasklet(unsigned long data)
+{
+	struct hsi_port *port = (struct hsi_port *)data;
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	u32 error_status;
+	u32 error_interrupts;
+
+	error_status = readl(ste_hsi->rx_base + STE_HSI_RX_EXCEP);
+	/*
+	 * sometimes interrupt that cause running this tasklet is already
+	 * inactive so base handling of exception on masked interrupt status
+	 * not on exception state register.
+	 */
+	error_interrupts = readl(ste_hsi->rx_base + STE_HSI_RX_EXCEPMIS);
+
+	if (error_interrupts & STE_HSI_EXCEP_BREAK)
+		ste_hsi_break_complete(port, ste_hsi);
+
+	if (error_interrupts & STE_HSI_EXCEP_TIMEOUT)
+		dev_err(&hsi->device, "timeout exception occurred\n");
+	if (error_interrupts & STE_HSI_EXCEP_OVERRUN)
+		dev_err(&hsi->device, "overrun exception occurred\n");
+	if (error_interrupts & STE_HSI_EXCEP_PARITY)
+		dev_err(&hsi->device, "parity exception occurred\n");
+
+	if (error_interrupts & ~STE_HSI_EXCEP_BREAK)
+		ste_hsi_error(port);
+
+	/* Acknowledge exception interrupts */
+	writel(error_status, ste_hsi->rx_base + STE_HSI_RX_ACK);
+
+	enable_irq(ste_port->excep_irq);
+}
+
+static irqreturn_t ste_hsi_exception_isr(int irq, void *data)
+{
+	struct hsi_port *port = data;
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+
+	disable_irq_nosync(irq);
+	tasklet_hi_schedule(&ste_port->exception_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static void ste_hsi_overrun_tasklet(unsigned long data)
+{
+	struct hsi_controller *hsi = (struct hsi_controller *)data;
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct hsi_port *hsi_port = &hsi->port[0];
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(hsi_port);
+	struct hsi_msg *msg;
+
+	unsigned int channel;
+	u8 rised_overrun;
+	u8 mask;
+	u8 blocked = 0;
+
+	rised_overrun = (u8) readl(ste_hsi->rx_base + STE_HSI_RX_OVERRUNMIS);
+	mask = rised_overrun;
+	for (channel = 0; mask; ++channel, mask >>= 1) {
+		if (!(mask & 1))
+			continue;
+
+		do {
+			/*
+			 * No more messages, block interrupt
+			 */
+			if (list_empty(&ste_port->rxqueue[channel])) {
+				blocked |= 1 << channel;
+				break;
+			}
+			/*
+			 * Complete with error
+			 */
+			msg = list_first_entry(&ste_port->rxqueue[channel],
+					       struct hsi_msg, link);
+			list_del(&msg->link);
+			msg->status = HSI_STATUS_ERROR;
+			msg->complete(msg);
+
+			/*
+			 * Now restart queued reads if any
+			 * If start_transfer fails, try with next message
+			 */
+			if (ste_hsi_start_transfer(ste_port,
+						   &ste_port->rxqueue[channel]))
+				continue;
+		} while (0);
+	}
+
+	/* Overrun acknowledge */
+	writel(rised_overrun, ste_hsi->rx_base + STE_HSI_RX_OVERRUNACK);
+	writel(~blocked & readl(ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM),
+	       ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM);
+
+	/*
+	 * Enable all that should not be blocked
+	 */
+	mask = rised_overrun & ~blocked;
+	for (channel = 0; mask; ++channel, mask >>= 1)
+		enable_irq(ste_hsi->overrun_irq[channel]);
+}
+
+static irqreturn_t ste_hsi_overrun_isr(int irq, void *data)
+{
+	struct hsi_port *port = data;
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+
+	disable_irq_nosync(irq);
+	tasklet_hi_schedule(&ste_port->overrun_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static void __init ste_hsi_queues_init(struct ste_hsi_port *ste_port)
+{
+	unsigned int ch;
+
+	for (ch = 0; ch < STE_HSI_MAX_CHANNELS; ch++) {
+		INIT_LIST_HEAD(&ste_port->txqueue[ch]);
+		INIT_LIST_HEAD(&ste_port->rxqueue[ch]);
+	}
+	INIT_LIST_HEAD(&ste_port->brkqueue);
+}
+
+static int __init ste_hsi_get_iomem(struct platform_device *pdev,
+				    const char *res_name,
+				    unsigned char __iomem **base,
+				    dma_addr_t *phy)
+{
+	struct resource *mem;
+	struct resource *ioarea;
+
+	mem = platform_get_resource_byname(pdev, IORESOURCE_MEM, res_name);
+	if (!mem) {
+		dev_err(&pdev->dev, "IO memory region missing!\n");
+		return -ENXIO;
+	}
+
+	ioarea = devm_request_mem_region(&pdev->dev, mem->start,
+					 resource_size(mem),
+					 dev_name(&pdev->dev));
+	if (!ioarea) {
+		dev_err(&pdev->dev, "Can't request IO memory region!\n");
+		return -ENXIO;
+	}
+
+	*base = devm_ioremap(&pdev->dev, mem->start, resource_size(mem));
+	if (!base) {
+		dev_err(&pdev->dev, "%s IO remap failed!\n", mem->name);
+		return -ENXIO;
+	}
+	if (phy)
+		*phy = (dma_addr_t) mem->start;
+
+	return 0;
+}
+
+static int __init ste_hsi_get_irq(struct platform_device *pdev,
+				  const char *res_name,
+				  irqreturn_t(*isr) (int, void *), void *data,
+				  int *irq_number)
+{
+	struct resource *irq;
+	int err;
+
+	irq = platform_get_resource_byname(pdev, IORESOURCE_IRQ, res_name);
+	if (!irq) {
+		dev_err(&pdev->dev, "IO memory region missing!\n");
+		return -ENXIO;
+	}
+
+	err = devm_request_irq(&pdev->dev, irq->start, isr,
+			       IRQF_DISABLED, irq->name, data);
+	if (err)
+		dev_err(&pdev->dev, "%s IRQ request failed!\n", irq->name);
+
+	if (irq_number)
+		*irq_number = irq->start;
+
+	return err;
+}
+
+static void ste_hsi_flush_queue(struct list_head *queue, struct hsi_client *cl)
+{
+	struct list_head *node, *tmp;
+	struct hsi_msg *msg;
+
+	list_for_each_safe(node, tmp, queue) {
+		msg = list_entry(node, struct hsi_msg, link);
+		if ((cl) && (cl != msg->cl))
+			continue;
+		list_del(node);
+
+		if (msg->destructor)
+			msg->destructor(msg);
+		else
+			hsi_free_msg(msg);
+	}
+}
+
+static int ste_hsi_async_break(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct ste_hsi_port *ste_port = hsi_to_ste_port(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	int err;
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		return err;
+
+	if (msg->ttype == HSI_MSG_WRITE) {
+		if (port->tx_cfg.mode != HSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		writel(1, ste_hsi->tx_base + STE_HSI_TX_BREAK);
+		msg->status = HSI_STATUS_COMPLETED;
+		msg->complete(msg);
+	} else {
+		u32 mask;
+		if (port->rx_cfg.mode != HSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		spin_lock_bh(&ste_hsi->lock);
+		msg->status = HSI_STATUS_PROCEEDING;
+		mask = readl(ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+		/* Enable break exception on controller */
+		if (!(mask & STE_HSI_EXCEP_BREAK))
+			writel(mask | STE_HSI_EXCEP_BREAK,
+			       ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+
+		list_add_tail(&msg->link, &ste_port->brkqueue);
+		spin_unlock_bh(&ste_hsi->lock);
+	}
+
+out:
+	ste_hsi_clock_disable(hsi);
+	return err;
+}
+
+static int ste_hsi_async(struct hsi_msg *msg)
+{
+	struct ste_hsi_controller *ste_hsi;
+	struct ste_hsi_port *ste_port;
+	struct list_head *queue;
+	int err = 0;
+
+	if (unlikely(!msg))
+		return -ENOSYS;
+
+	if (msg->sgt.nents > 1)
+		return -ENOSYS;
+
+	if (unlikely(msg->break_frame))
+		return ste_hsi_async_break(msg);
+
+	ste_port = client_to_ste_port(msg->cl);
+	ste_hsi = client_to_ste_controller(msg->cl);
+
+	if (msg->ttype == HSI_MSG_WRITE) {
+		/* TX transfer */
+		BUG_ON(msg->channel >= ste_port->channels);
+		queue = &ste_port->txqueue[msg->channel];
+	} else {
+		/* RX transfer */
+		queue = &ste_port->rxqueue[msg->channel];
+	}
+
+	spin_lock_bh(&ste_hsi->lock);
+	list_add_tail(&msg->link, queue);
+	msg->status = HSI_STATUS_QUEUED;
+
+	err = ste_hsi_start_transfer(ste_port, queue);
+	if (err)
+		list_del(&msg->link);
+
+	spin_unlock_bh(&ste_hsi->lock);
+
+	return err;
+}
+
+static int ste_hsi_setup(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	int err;
+	u32 div = 0;
+
+	if (ste_hsi->regulator)
+		regulator_enable(ste_hsi->regulator);
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		return err;
+
+	if (cl->tx_cfg.speed) {
+		div = clk_get_rate(ste_hsi->tx_clk) / 1000 / cl->tx_cfg.speed;
+		if (div)
+			--div;
+	}
+
+	if (!ste_hsi->context)
+		ste_hsi->context = kzalloc(sizeof(struct ste_hsi_hw_context), GFP_KERNEL);
+
+	if (!ste_hsi->context) {
+		dev_err(ste_hsi->dev, "Not enough memory for context!\n");
+		return -ENOMEM;
+	} else {
+		/* Save HSI context */
+		ste_hsi->context->tx_mode = cl->tx_cfg.mode;
+		ste_hsi->context->tx_divisor = div;
+		ste_hsi->context->tx_channels = cl->tx_cfg.channels;
+		ste_hsi->context->rx_mode = cl->rx_cfg.mode;
+		ste_hsi->context->rx_channels = cl->rx_cfg.channels;
+	}
+
+	port->tx_cfg = cl->tx_cfg;
+	port->rx_cfg = cl->rx_cfg;
+
+	ste_hsi_setup_registers(ste_hsi);
+
+	ste_port->channels = max(cl->tx_cfg.channels, cl->rx_cfg.channels);
+
+	ste_hsi_setup_dma(cl);
+
+	ste_hsi_clock_disable(hsi);
+
+	if (ste_hsi->regulator)
+		regulator_disable(ste_hsi->regulator);
+
+	return err;
+}
+
+static int ste_hsi_flush(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	int i;
+
+	ste_hsi_clock_enable(hsi);
+
+	/* Enter sleep mode */
+	writel(STE_HSI_MODE_SLEEP, ste_hsi->rx_base + STE_HSI_RX_MODE);
+
+	/* Disable DMA, and terminate all outstanding jobs */
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_DMAEN);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_DMAEN);
+	ste_hsi_terminate_dma(ste_port);
+
+	/* Flush all HSIR and HSIT buffers */
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_STATE);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_BUFSTATE);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_STATE);
+	/*
+	 * BUFSTATE is cleared twice on purpose:
+	 * first time all fifos are cleared
+	 * second time to clear data that was in pipline buffer
+	 * and was transfered to fifos
+	 */
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_BUFSTATE);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_BUFSTATE);
+
+	/* Flush all errors */
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_EXCEP);
+
+	/* Clear interrupts */
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIC);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIC);
+	writel(0xFF, ste_hsi->rx_base + STE_HSI_RX_OVERRUNACK);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+	writel(0x0F, ste_hsi->rx_base + STE_HSI_RX_ACK);
+
+	/* Dequeue all pending requests */
+	for (i = 0; i < ste_port->channels; i++) {
+		/* Release write clocks */
+		if (!list_empty(&ste_port->txqueue[i]))
+			ste_hsi_clock_disable(hsi);
+		if (!list_empty(&ste_port->rxqueue[i]))
+			ste_hsi_clock_disable(hsi);
+		ste_hsi_flush_queue(&ste_port->txqueue[i], NULL);
+		ste_hsi_flush_queue(&ste_port->rxqueue[i], NULL);
+	}
+	ste_hsi_flush_queue(&ste_port->brkqueue, NULL);
+
+	ste_hsi_clock_disable(hsi);
+
+	return 0;
+}
+
+static int ste_hsi_start_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+
+	if (ste_hsi->regulator)
+		regulator_enable(ste_hsi->regulator);
+
+	return 0;
+}
+
+static int ste_hsi_stop_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+
+	if (ste_hsi->regulator)
+		regulator_disable(ste_hsi->regulator);
+
+	return 0;
+}
+
+static int ste_hsi_release(struct hsi_client *cl)
+{
+	int err;
+	struct ste_hsi_controller *ste_hsi = client_to_ste_controller(cl);
+
+	err = ste_hsi_flush(cl);
+	cancel_delayed_work(&ste_hsi->clk_work);
+
+	return 0;
+}
+
+static int ste_hsi_ports_init(struct hsi_controller *hsi,
+			      struct platform_device *pdev)
+{
+	struct hsi_port *port;
+	struct ste_hsi_port *ste_port;
+	unsigned int i;
+	char irq_name[20];
+	int err;
+
+	for (i = 0; i < hsi->num_ports; i++) {
+		ste_port = devm_kzalloc(&pdev->dev, sizeof *ste_port,
+					GFP_KERNEL);
+		if (!ste_port)
+			return -ENOMEM;
+
+		port = &hsi->port[i];
+		port->async = ste_hsi_async;
+		port->setup = ste_hsi_setup;
+		port->flush = ste_hsi_flush;
+		port->start_tx = ste_hsi_start_tx;
+		port->stop_tx = ste_hsi_stop_tx;
+		port->release = ste_hsi_release;
+		hsi_port_set_drvdata(port, ste_port);
+		ste_port->dev = &port->device;
+
+		sprintf(irq_name, "hsi_rx_irq%d", i);
+		err = ste_hsi_get_irq(pdev, irq_name, ste_hsi_rx_isr, port,
+				      &ste_port->rx_irq);
+		if (err)
+			return err;
+
+		sprintf(irq_name, "hsi_tx_irq%d", i);
+		err = ste_hsi_get_irq(pdev, irq_name, ste_hsi_tx_isr, port,
+				      &ste_port->tx_irq);
+		if (err)
+			return err;
+
+		tasklet_init(&ste_port->rx_tasklet, ste_hsi_rx_tasklet,
+			     (unsigned long)port);
+
+		tasklet_init(&ste_port->tx_tasklet, ste_hsi_tx_tasklet,
+			     (unsigned long)port);
+
+		tasklet_init(&ste_port->exception_tasklet,
+			     ste_hsi_exception_tasklet, (unsigned long)port);
+
+		tasklet_init(&ste_port->overrun_tasklet,
+			     ste_hsi_overrun_tasklet, (unsigned long)port);
+
+		sprintf(irq_name, "hsi_rx_excep%d", i);
+		err = ste_hsi_get_irq(pdev, irq_name, ste_hsi_exception_isr,
+				      port, &ste_port->excep_irq);
+		if (err)
+			return err;
+
+		ste_hsi_queues_init(ste_port);
+	}
+	return 0;
+}
+
+static int __init ste_hsi_hw_init(struct hsi_controller *hsi)
+{
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	int err;
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		return err;
+
+	ste_hsi_init_registers(ste_hsi);
+
+	ste_hsi_clock_disable(hsi);
+
+	return err;
+}
+
+static int __init ste_hsi_add_controller(struct hsi_controller *hsi,
+					 struct platform_device *pdev)
+{
+	struct ste_hsi_controller *ste_hsi;
+	char overrun_name[] = "hsi_rx_overrun_chxxx";
+	unsigned char i;
+	int err;
+
+	ste_hsi = kzalloc(sizeof(struct ste_hsi_controller), GFP_KERNEL);
+	if (!ste_hsi) {
+		dev_err(&pdev->dev, "Not enough memory for ste_hsi!\n");
+		return -ENOMEM;
+	}
+
+	spin_lock_init(&ste_hsi->lock);
+	spin_lock_init(&ste_hsi->ck_lock);
+	INIT_DELAYED_WORK(&ste_hsi->clk_work, ste_hsi_delayed_disable_clock);
+
+	hsi->id = pdev->id;
+	hsi->device.parent = &pdev->dev;
+	dev_set_name(&hsi->device, "ste-hsi.%d", hsi->id);
+	ste_hsi->dev = &hsi->device;
+	hsi_controller_set_drvdata(hsi, ste_hsi);
+
+	/* Get and enable regulator */
+	ste_hsi->regulator = regulator_get(&pdev->dev, "v-hsi");
+	if (IS_ERR(ste_hsi->regulator)) {
+		dev_err(&pdev->dev, "could not get v-hsi regulator\n");
+		ste_hsi->regulator = NULL;
+	} else {
+		regulator_enable(ste_hsi->regulator);
+	}
+
+	/* Get and reserve resources for receiver */
+	err = ste_hsi_get_iomem(pdev, "hsi_rx_base", &ste_hsi->rx_base,
+				&ste_hsi->rx_dma_base);
+	if (err)
+		goto err_free_mem;
+	dev_info(&pdev->dev, "hsi_rx_base = %p\n", ste_hsi->rx_base);
+
+	/* Get and reserve resources for transmitter */
+	err = ste_hsi_get_iomem(pdev, "hsi_tx_base", &ste_hsi->tx_base,
+				&ste_hsi->tx_dma_base);
+	if (err)
+		goto err_free_mem;
+	dev_info(&pdev->dev, "hsi_tx_base = %p\n", ste_hsi->tx_base);
+
+	/* Get HSIT HSITXCLK clock */
+	ste_hsi->tx_clk = clk_get(&pdev->dev, "hsit_hsitxclk");
+	if (IS_ERR(ste_hsi->tx_clk)) {
+		dev_err(&hsi->device, "Couldn't get HSIT HSITXCLK clock\n");
+		err = PTR_ERR(ste_hsi->tx_clk);
+		goto err_free_mem;
+	}
+
+	/* Get HSIR HSIRXCLK clock */
+	ste_hsi->rx_clk = clk_get(&pdev->dev, "hsir_hsirxclk");
+	if (IS_ERR(ste_hsi->rx_clk)) {
+		dev_err(&hsi->device, "Couldn't get HSIR HSIRXCLK clock\n");
+		err = PTR_ERR(ste_hsi->rx_clk);
+		goto err_clk_free;
+	}
+
+	/* Get HSIT HCLK clock */
+	ste_hsi->ssitx_clk = clk_get(&pdev->dev, "hsit_hclk");
+	if (IS_ERR(ste_hsi->ssitx_clk)) {
+		dev_err(&hsi->device, "Couldn't get HSIT HCLK clock\n");
+		err = PTR_ERR(ste_hsi->ssitx_clk);
+		goto err_clk_free;
+	}
+
+	/* Get HSIR HCLK clock */
+	ste_hsi->ssirx_clk = clk_get(&pdev->dev, "hsir_hclk");
+	if (IS_ERR(ste_hsi->ssirx_clk)) {
+		dev_err(&hsi->device, "Couldn't get HSIR HCLK clock\n");
+		err = PTR_ERR(ste_hsi->ssirx_clk);
+		goto err_clk_free;
+	}
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		goto err_clk_free;
+
+	/* Check if controller is at specified address */
+	if (compare_periphid(ste_hsir_periphid,
+			     (u32 *) (ste_hsi->rx_base + 0xFE0), 8)) {
+		dev_err(&pdev->dev, "No hsir controller at = %p\n",
+			ste_hsi->rx_base);
+		err = -ENXIO;
+		goto err_clk_free;
+	}
+
+	/* Check if controller is at specified address */
+	if (compare_periphid(ste_hsit_periphid,
+			     (u32 *) (ste_hsi->tx_base + 0xFE0), 8)) {
+		dev_err(&pdev->dev, "No hsit controller at = %p\n",
+			ste_hsi->tx_base);
+		err = -ENXIO;
+		goto err_clk_free;
+	}
+	ste_hsi_clock_disable(hsi);
+
+	err = ste_hsi_hw_init(hsi);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to init HSI controller!\n");
+		goto err_clk_free;
+	}
+
+	for (i = 0; i < STE_HSI_MAX_CHANNELS; i++) {
+		sprintf(overrun_name, "hsi_rx_overrun_ch%d", i);
+		err = ste_hsi_get_irq(pdev, overrun_name, ste_hsi_overrun_isr,
+				      hsi, &ste_hsi->overrun_irq[i]);
+		if (err)
+			goto err_clk_free;
+	}
+
+	err = ste_hsi_ports_init(hsi, pdev);
+	if (err)
+		goto err_clk_free;
+
+	err = hsi_register_controller(hsi);
+
+	if (ste_hsi->regulator)
+		regulator_disable(ste_hsi->regulator);
+
+	if (err)
+		goto err_clk_free;
+
+	return 0;
+
+err_clk_free:
+	ste_hsi_clks_free(ste_hsi);
+err_free_mem:
+	kfree(ste_hsi);
+	return err;
+}
+
+static int ste_hsi_remove_controller(struct hsi_controller *hsi,
+				     struct platform_device *pdev)
+{
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+
+	if (ste_hsi->regulator)
+		regulator_put(ste_hsi->regulator);
+
+	ste_hsi_clks_free(ste_hsi);
+	hsi_unregister_controller(hsi);
+
+	kfree(ste_hsi->context);
+	kfree(ste_hsi);
+
+	return 0;
+}
+
+static int __init ste_hsi_probe(struct platform_device *pdev)
+{
+	struct hsi_controller *hsi;
+	struct ste_hsi_platform_data *pdata = pdev->dev.platform_data;
+	int err;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "No HSI platform data!\n");
+		return -EINVAL;
+	}
+
+	hsi = hsi_alloc_controller(pdata->num_ports, GFP_KERNEL);
+	if (!hsi) {
+		dev_err(&pdev->dev, "No memory to allocate HSI controller!\n");
+		return -ENOMEM;
+	}
+	platform_set_drvdata(pdev, hsi);
+
+	err = ste_hsi_add_controller(hsi, pdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Can't add HSI controller!\n");
+		goto err_free_controller;
+	}
+
+	if (pdata->use_dma)
+		ste_hsi_init_dma(pdata, hsi);
+
+	return 0;
+
+err_free_controller:
+	platform_set_drvdata(pdev, NULL);
+	hsi_free_controller(hsi);
+
+	return err;
+}
+
+static int __exit ste_hsi_remove(struct platform_device *pdev)
+{
+	struct hsi_controller *hsi = platform_get_drvdata(pdev);
+
+	ste_hsi_remove_controller(hsi, pdev);
+	hsi_free_controller(hsi);
+
+	return 0;
+}
+
+static struct platform_driver ste_hsi_driver __refdata = {
+	.driver = {
+		   .name = "ste_hsi",
+		   .owner = THIS_MODULE,
+		   },
+	.remove = __exit_p(ste_hsi_remove),
+};
+
+static int __init ste_hsi_init(void)
+{
+	return platform_driver_probe(&ste_hsi_driver, ste_hsi_probe);
+}
+module_init(ste_hsi_init)
+
+static void __exit ste_hsi_exit(void)
+{
+	platform_driver_unregister(&ste_hsi_driver);
+}
+module_exit(ste_hsi_exit)
+
+MODULE_AUTHOR("Lukasz Baj <lukasz.baj@tieto.com");
+MODULE_DESCRIPTION("STE HSI driver.");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/hsi/hsi.c b/drivers/hsi/hsi.c
new file mode 100644
index 00000000000..06b574394cd
--- /dev/null
+++ b/drivers/hsi/hsi.c
@@ -0,0 +1,496 @@
+/*
+ * hsi.c
+ *
+ * HSI core.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+#include <linux/hsi/hsi.h>
+#include <linux/compiler.h>
+#include <linux/rwsem.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/kobject.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include "hsi_core.h"
+
+static struct device_type hsi_ctrl = {
+	.name	= "hsi_controller",
+};
+
+static struct device_type hsi_cl = {
+	.name	= "hsi_client",
+};
+
+static struct device_type hsi_port = {
+	.name	= "hsi_port",
+};
+
+static ssize_t modalias_show(struct device *dev,
+			struct device_attribute *a __maybe_unused, char *buf)
+{
+	return sprintf(buf, "hsi:%s\n", dev_name(dev));
+}
+
+static struct device_attribute hsi_bus_dev_attrs[] = {
+	__ATTR_RO(modalias),
+	__ATTR_NULL,
+};
+
+static int hsi_bus_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	if (dev->type == &hsi_cl)
+		add_uevent_var(env, "MODALIAS=hsi:%s", dev_name(dev));
+
+	return 0;
+}
+
+static int hsi_bus_match(struct device *dev, struct device_driver *driver)
+{
+	return strcmp(dev_name(dev), driver->name) == 0;
+}
+
+static struct bus_type hsi_bus_type = {
+	.name		= "hsi",
+	.dev_attrs	= hsi_bus_dev_attrs,
+	.match		= hsi_bus_match,
+	.uevent		= hsi_bus_uevent,
+};
+
+static void hsi_client_release(struct device *dev)
+{
+	kfree(to_hsi_client(dev));
+}
+
+static void hsi_new_client(struct hsi_port *port, struct hsi_board_info *info)
+{
+	struct hsi_client *cl;
+	unsigned long flags;
+
+	cl = kzalloc(sizeof(*cl), GFP_KERNEL);
+	if (!cl)
+		return;
+	cl->device.type = &hsi_cl;
+	cl->tx_cfg = info->tx_cfg;
+	cl->rx_cfg = info->rx_cfg;
+	cl->device.bus = &hsi_bus_type;
+	cl->device.parent = &port->device;
+	cl->device.release = hsi_client_release;
+	dev_set_name(&cl->device, info->name);
+	cl->device.platform_data = info->platform_data;
+	spin_lock_irqsave(&port->clock, flags);
+	list_add_tail(&cl->link, &port->clients);
+	spin_unlock_irqrestore(&port->clock, flags);
+	if (info->archdata)
+		cl->device.archdata = *info->archdata;
+	if (device_register(&cl->device) < 0) {
+		pr_err("hsi: failed to register client: %s\n", info->name);
+		kfree(cl);
+	}
+}
+
+static void hsi_scan_board_info(struct hsi_controller *hsi)
+{
+	struct hsi_cl_info *cl_info;
+	struct hsi_port	*p;
+
+	list_for_each_entry(cl_info, &hsi_board_list, list)
+		if (cl_info->info.hsi_id == hsi->id) {
+			p = hsi_find_port_num(hsi, cl_info->info.port);
+			if (!p)
+				continue;
+			hsi_new_client(p, &cl_info->info);
+		}
+}
+
+static int hsi_remove_client(struct device *dev, void *data __maybe_unused)
+{
+	struct hsi_client *cl = to_hsi_client(dev);
+	struct hsi_port *port = to_hsi_port(dev->parent);
+	unsigned long flags;
+
+	spin_lock_irqsave(&port->clock, flags);
+	list_del(&cl->link);
+	spin_unlock_irqrestore(&port->clock, flags);
+	device_unregister(dev);
+
+	return 0;
+}
+
+static int hsi_remove_port(struct device *dev, void *data __maybe_unused)
+{
+	device_for_each_child(dev, NULL, hsi_remove_client);
+	device_unregister(dev);
+
+	return 0;
+}
+
+static void hsi_controller_release(struct device *dev __maybe_unused)
+{
+}
+
+static void hsi_port_release(struct device *dev __maybe_unused)
+{
+}
+
+/**
+ * hsi_unregister_controller - Unregister an HSI controller
+ * @hsi: The HSI controller to register
+ */
+void hsi_unregister_controller(struct hsi_controller *hsi)
+{
+	device_for_each_child(&hsi->device, NULL, hsi_remove_port);
+	device_unregister(&hsi->device);
+}
+EXPORT_SYMBOL_GPL(hsi_unregister_controller);
+
+/**
+ * hsi_register_controller - Register an HSI controller and its ports
+ * @hsi: The HSI controller to register
+ *
+ * Returns -errno on failure, 0 on success.
+ */
+int hsi_register_controller(struct hsi_controller *hsi)
+{
+	unsigned int i;
+	int err;
+
+	hsi->device.type = &hsi_ctrl;
+	hsi->device.bus = &hsi_bus_type;
+	hsi->device.release = hsi_controller_release;
+	err = device_register(&hsi->device);
+	if (err < 0)
+		return err;
+	for (i = 0; i < hsi->num_ports; i++) {
+		hsi->port[i].device.parent = &hsi->device;
+		hsi->port[i].device.bus = &hsi_bus_type;
+		hsi->port[i].device.release = hsi_port_release;
+		hsi->port[i].device.type = &hsi_port;
+		INIT_LIST_HEAD(&hsi->port[i].clients);
+		spin_lock_init(&hsi->port[i].clock);
+		err = device_register(&hsi->port[i].device);
+		if (err < 0)
+			goto out;
+	}
+	/* Populate HSI bus with HSI clients */
+	hsi_scan_board_info(hsi);
+
+	return 0;
+out:
+	hsi_unregister_controller(hsi);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(hsi_register_controller);
+
+/**
+ * hsi_register_client_driver - Register an HSI client to the HSI bus
+ * @drv: HSI client driver to register
+ *
+ * Returns -errno on failure, 0 on success.
+ */
+int hsi_register_client_driver(struct hsi_client_driver *drv)
+{
+	drv->driver.bus = &hsi_bus_type;
+
+	return driver_register(&drv->driver);
+}
+EXPORT_SYMBOL_GPL(hsi_register_client_driver);
+
+static inline int hsi_dummy_msg(struct hsi_msg *msg __maybe_unused)
+{
+	return 0;
+}
+
+static inline int hsi_dummy_cl(struct hsi_client *cl __maybe_unused)
+{
+	return 0;
+}
+
+/**
+ * hsi_alloc_controller - Allocate an HSI controller and its ports
+ * @n_ports: Number of ports on the HSI controller
+ * @flags: Kernel allocation flags
+ *
+ * Return NULL on failure or a pointer to an hsi_controller on success.
+ */
+struct hsi_controller *hsi_alloc_controller(unsigned int n_ports, gfp_t flags)
+{
+	struct hsi_controller	*hsi;
+	struct hsi_port		*port;
+	unsigned int		i;
+
+	if (!n_ports)
+		return NULL;
+
+	port = kzalloc(sizeof(*port)*n_ports, flags);
+	if (!port)
+		return NULL;
+	hsi = kzalloc(sizeof(*hsi), flags);
+	if (!hsi)
+		goto out;
+	for (i = 0; i < n_ports; i++) {
+		dev_set_name(&port[i].device, "port%d", i);
+		port[i].num = i;
+		port[i].async = hsi_dummy_msg;
+		port[i].setup = hsi_dummy_cl;
+		port[i].flush = hsi_dummy_cl;
+		port[i].start_tx = hsi_dummy_cl;
+		port[i].stop_tx = hsi_dummy_cl;
+		port[i].release = hsi_dummy_cl;
+		mutex_init(&port[i].lock);
+	}
+	hsi->num_ports = n_ports;
+	hsi->port = port;
+
+	return hsi;
+out:
+	kfree(port);
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(hsi_alloc_controller);
+
+/**
+ * hsi_free_controller - Free an HSI controller
+ * @hsi: Pointer to HSI controller
+ */
+void hsi_free_controller(struct hsi_controller *hsi)
+{
+	if (!hsi)
+		return;
+
+	kfree(hsi->port);
+	kfree(hsi);
+}
+EXPORT_SYMBOL_GPL(hsi_free_controller);
+
+/**
+ * hsi_free_msg - Free an HSI message
+ * @msg: Pointer to the HSI message
+ *
+ * Client is responsible to free the buffers pointed by the scatterlists.
+ */
+void hsi_free_msg(struct hsi_msg *msg)
+{
+	if (!msg)
+		return;
+	sg_free_table(&msg->sgt);
+	kfree(msg);
+}
+EXPORT_SYMBOL_GPL(hsi_free_msg);
+
+/**
+ * hsi_alloc_msg - Allocate an HSI message
+ * @nents: Number of memory entries
+ * @flags: Kernel allocation flags
+ *
+ * nents can be 0. This mainly makes sense for read transfer.
+ * In that case, HSI drivers will call the complete callback when
+ * there is data to be read without consuming it.
+ *
+ * Return NULL on failure or a pointer to an hsi_msg on success.
+ */
+struct hsi_msg *hsi_alloc_msg(unsigned int nents, gfp_t flags)
+{
+	struct hsi_msg *msg;
+	int err;
+
+	msg = kzalloc(sizeof(*msg), flags);
+	if (!msg)
+		return NULL;
+
+	if (!nents)
+		return msg;
+
+	err = sg_alloc_table(&msg->sgt, nents, flags);
+	if (unlikely(err)) {
+		kfree(msg);
+		msg = NULL;
+	}
+
+	return msg;
+}
+EXPORT_SYMBOL_GPL(hsi_alloc_msg);
+
+/**
+ * hsi_async - Submit an HSI transfer to the controller
+ * @cl: HSI client sending the transfer
+ * @msg: The HSI transfer passed to controller
+ *
+ * The HSI message must have the channel, ttype, complete and destructor
+ * fields set beforehand. If nents > 0 then the client has to initialize
+ * also the scatterlists to point to the buffers to write to or read from.
+ *
+ * HSI controllers relay on pre-allocated buffers from their clients and they
+ * do not allocate buffers on their own.
+ *
+ * Once the HSI message transfer finishes, the HSI controller calls the
+ * complete callback with the status and actual_len fields of the HSI message
+ * updated. The complete callback can be called before returning from
+ * hsi_async.
+ *
+ * Returns -errno on failure or 0 on success
+ */
+int hsi_async(struct hsi_client *cl, struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+
+	if (!hsi_port_claimed(cl))
+		return -EACCES;
+
+	WARN_ON_ONCE(!msg->destructor || !msg->complete);
+	msg->cl = cl;
+
+	return port->async(msg);
+}
+EXPORT_SYMBOL_GPL(hsi_async);
+
+/**
+ * hsi_claim_port - Claim the HSI client's port
+ * @cl: HSI client that wants to claim its port
+ * @share: Flag to indicate if the client wants to share the port or not.
+ *
+ * Returns -errno on failure, 0 on success.
+ */
+int hsi_claim_port(struct hsi_client *cl, unsigned int share)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	int err = 0;
+
+	mutex_lock(&port->lock);
+	if ((port->claimed) && (!port->shared || !share)) {
+		err = -EBUSY;
+		goto out;
+	}
+	if (!try_module_get(to_hsi_controller(port->device.parent)->owner)) {
+		err = -ENODEV;
+		goto out;
+	}
+	port->claimed++;
+	port->shared = !!share;
+	cl->pclaimed = 1;
+out:
+	mutex_unlock(&port->lock);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(hsi_claim_port);
+
+/**
+ * hsi_release_port - Release the HSI client's port
+ * @cl: HSI client which previously claimed its port
+ */
+void hsi_release_port(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+
+	mutex_lock(&port->lock);
+	/* Allow HW driver to do some cleanup */
+	port->release(cl);
+	if (cl->pclaimed)
+		port->claimed--;
+	BUG_ON(port->claimed < 0);
+	cl->pclaimed = 0;
+	if (!port->claimed)
+		port->shared = 0;
+	module_put(to_hsi_controller(port->device.parent)->owner);
+	mutex_unlock(&port->lock);
+}
+EXPORT_SYMBOL_GPL(hsi_release_port);
+
+static int hsi_start_rx(struct hsi_client *cl, void *data __maybe_unused)
+{
+	if (cl->hsi_start_rx)
+		(*cl->hsi_start_rx)(cl);
+
+	return 0;
+}
+
+static int hsi_stop_rx(struct hsi_client *cl, void *data __maybe_unused)
+{
+	if (cl->hsi_stop_rx)
+		(*cl->hsi_stop_rx)(cl);
+
+	return 0;
+}
+
+static int hsi_port_for_each_client(struct hsi_port *port, void *data,
+				int (*fn)(struct hsi_client *cl, void *data))
+{
+	struct hsi_client *cl;
+
+	spin_lock(&port->clock);
+	list_for_each_entry(cl, &port->clients, link) {
+		spin_unlock(&port->clock);
+		(*fn)(cl, data);
+		spin_lock(&port->clock);
+	}
+	spin_unlock(&port->clock);
+
+	return 0;
+}
+
+/**
+ * hsi_event -Notifies clients about port events
+ * @port: Port where the event occurred
+ * @event: The event type
+ *
+ * Clients should not be concerned about wake line behavior. However, due
+ * to a race condition in HSI HW protocol, clients need to be notified
+ * about wake line changes, so they can implement a workaround for it.
+ *
+ * Events:
+ * HSI_EVENT_START_RX - Incoming wake line high
+ * HSI_EVENT_STOP_RX - Incoming wake line down
+ */
+void hsi_event(struct hsi_port *port, unsigned int event)
+{
+	int (*fn)(struct hsi_client *cl, void *data);
+
+	switch (event) {
+	case HSI_EVENT_START_RX:
+		fn = hsi_start_rx;
+		break;
+	case HSI_EVENT_STOP_RX:
+		fn = hsi_stop_rx;
+		break;
+	default:
+		return;
+	}
+	hsi_port_for_each_client(port, NULL, fn);
+}
+EXPORT_SYMBOL_GPL(hsi_event);
+
+static int __init hsi_init(void)
+{
+	return bus_register(&hsi_bus_type);
+}
+postcore_initcall(hsi_init);
+
+static void __exit hsi_exit(void)
+{
+	bus_unregister(&hsi_bus_type);
+}
+module_exit(hsi_exit);
+
+MODULE_AUTHOR("Carlos Chinea <carlos.chinea@nokia.com>");
+MODULE_DESCRIPTION("High-speed Synchronous Serial Interface (HSI) framework");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hsi/hsi_boardinfo.c b/drivers/hsi/hsi_boardinfo.c
new file mode 100644
index 00000000000..3a9e4e86652
--- /dev/null
+++ b/drivers/hsi/hsi_boardinfo.c
@@ -0,0 +1,64 @@
+/*
+ * hsi_boardinfo.c
+ *
+ * HSI clients registration interface
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+#include <linux/hsi/hsi.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include "hsi_core.h"
+
+/*
+ * hsi_board_list is only used internally by the HSI framework.
+ * No one else is allowed to make use of it.
+ */
+LIST_HEAD(hsi_board_list);
+EXPORT_SYMBOL_GPL(hsi_board_list);
+
+/**
+ * hsi_register_board_info - Register HSI clients information
+ * @info: Array of HSI clients on the board
+ * @len: Length of the array
+ *
+ * HSI clients are statically declared and registered on board files.
+ *
+ * HSI clients will be automatically registered to the HSI bus once the
+ * controller and the port where the clients wishes to attach are registered
+ * to it.
+ *
+ * Return -errno on failure, 0 on success.
+ */
+int __init hsi_register_board_info(struct hsi_board_info const *info,
+							unsigned int len)
+{
+	struct hsi_cl_info *cl_info;
+
+	cl_info = kzalloc(sizeof(*cl_info) * len, GFP_KERNEL);
+	if (!cl_info)
+		return -ENOMEM;
+
+	for (; len; len--, info++, cl_info++) {
+		cl_info->info = *info;
+		list_add_tail(&cl_info->list, &hsi_board_list);
+	}
+
+	return 0;
+}
diff --git a/drivers/hsi/hsi_core.h b/drivers/hsi/hsi_core.h
new file mode 100644
index 00000000000..8005509a28b
--- /dev/null
+++ b/drivers/hsi/hsi_core.h
@@ -0,0 +1,37 @@
+/*
+ * hsi_core.h
+ *
+ * HSI framework internal interfaces,
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#ifndef __LINUX_HSI_CORE_H__
+#define __LINUX_HSI_CORE_H__
+
+#include <linux/hsi/hsi.h>
+
+struct hsi_cl_info {
+	struct list_head	list;
+	struct hsi_board_info	info;
+};
+
+extern struct list_head hsi_board_list;
+
+#endif /* __LINUX_HSI_CORE_H__ */
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 5f888f7e7dc..602ba39286d 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -39,6 +39,32 @@ config HWMON_DEBUG_CHIP
 
 comment "Native drivers"
 
+config SENSORS_AB8500
+	tristate "AB8500 thermal monitoring"
+	depends on AB8500_CORE
+	default n
+	help
+	  If you say yes here you get support for the thermal sensor part
+	  of the AB8500 chip. The driver includes thermal management for
+	  AB8500 die and two GPADC channels. The GPADC channel are preferably
+	  used to access sensors outside the AB8500 chip.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called ab8500-temp.
+
+config SENSORS_DB8500
+	tristate "DB8500 thermal monitoring"
+	depends on UX500_SOC_DB8500
+	default n
+	help
+	  If you say yes here you get support for the thermal sensor part
+	  of the DB8500 chip. The driver includes thermal management for
+	  DB8500 die.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called db8500_temp.
+
+
 config SENSORS_ABITUGURU
 	tristate "Abit uGuru (rev 1 & 2)"
 	depends on X86 && DMI && EXPERIMENTAL
@@ -660,6 +686,40 @@ config SENSORS_LTC4151
 	  This driver can also be built as a module. If so, the module will
 	  be called ltc4151.
 
+config SENSORS_LSM303DLH
+	tristate "ST LSM303DLH 3-axis accelerometer and 3-axis magnetometer"
+	depends on I2C
+	default n
+	help
+	  This driver provides support for the LSM303DLH chip which includes a
+	  3-axis accelerometer and a 3-axis magnetometer.
+
+	  This driver can also be built as modules.  If so, the module for
+	  accelerometer will be called lsm303dlh_a and for magnetometer it will
+	  be called lsm303dlh_m.
+
+	  Say Y here if you have a device containing lsm303dlh chip.
+
+config SENSORS_LSM303DLH_INPUT_DEVICE
+	bool "ST LSM303DLH INPUT DEVICE"
+	depends on SENSORS_LSM303DLH
+	default n
+	help
+	  This driver allows device to be used as an input device with
+	  interrupts, need to be enabled only when input device support
+	  is required.
+
+config SENSORS_L3G4200D
+	tristate "ST L3G4200D 3-axis gyroscope"
+	depends on I2C
+	default n
+	help
+	  If you say yes here you get support for 3-axis gyroscope device
+	  L3g4200D.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called l3g4200d.
+
 config SENSORS_LTC4215
 	tristate "Linear Technology LTC4215"
 	depends on I2C && EXPERIMENTAL
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 28061cfa0cd..6a67970af47 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -19,6 +19,8 @@ obj-$(CONFIG_SENSORS_W83795)	+= w83795.o
 obj-$(CONFIG_SENSORS_W83781D)	+= w83781d.o
 obj-$(CONFIG_SENSORS_W83791D)	+= w83791d.o
 
+obj-$(CONFIG_SENSORS_AB8500)	+= ab8500.o
+obj-$(CONFIG_SENSORS_DB8500)	+= db8500.o
 obj-$(CONFIG_SENSORS_ABITUGURU)	+= abituguru.o
 obj-$(CONFIG_SENSORS_ABITUGURU3)+= abituguru3.o
 obj-$(CONFIG_SENSORS_AD7414)	+= ad7414.o
@@ -81,6 +83,8 @@ obj-$(CONFIG_SENSORS_LM92)	+= lm92.o
 obj-$(CONFIG_SENSORS_LM93)	+= lm93.o
 obj-$(CONFIG_SENSORS_LM95241)	+= lm95241.o
 obj-$(CONFIG_SENSORS_LTC4151)	+= ltc4151.o
+obj-$(CONFIG_SENSORS_LSM303DLH) += lsm303dlh_a.o lsm303dlh_m.o
+obj-$(CONFIG_SENSORS_L3G4200D) += l3g4200d.o
 obj-$(CONFIG_SENSORS_LTC4215)	+= ltc4215.o
 obj-$(CONFIG_SENSORS_LTC4245)	+= ltc4245.o
 obj-$(CONFIG_SENSORS_LTC4261)	+= ltc4261.o
diff --git a/drivers/hwmon/ab8500.c b/drivers/hwmon/ab8500.c
new file mode 100644
index 00000000000..1cfab4948e4
--- /dev/null
+++ b/drivers/hwmon/ab8500.c
@@ -0,0 +1,742 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Martin Persson <martin.persson@stericsson.com> for
+ * ST-Ericsson.
+ * License terms: GNU Gereral Public License (GPL) version 2
+ *
+ * Note:
+ *
+ * AB8500 does not provide auto ADC, so to monitor the required
+ * temperatures, a periodic work is used. It is more important
+ * to not wake up the CPU than to perform this job, hence the use
+ * of a deferred delay.
+ *
+ * A deferred delay for thermal monitor is considered safe because:
+ * If the chip gets too hot during a sleep state it's most likely
+ * due to external factors, such as the surrounding temperature.
+ * I.e. no SW decisions will make any difference.
+ *
+ * If/when the AB8500 thermal warning temperature is reached (threshold
+ * cannot be changed by SW), an interrupt is set and the driver
+ * notifies user space via a sysfs event. If a shut down is not
+ * triggered by user space within a certain time frame,
+ * pm_power off is called.
+ *
+ * If/when AB8500 thermal shutdown temperature is reached a hardware
+ * shutdown of the AB8500 will occur.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/hwmon.h>
+#include <linux/sysfs.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/jiffies.h>
+#include <linux/mutex.h>
+#include <linux/mfd/ab8500/gpadc.h>
+#include <linux/pm.h>
+
+/*
+ * If AB8500 warm interrupt is set, user space will be notified.
+ * If user space doesn't shut down the platform within this time
+ * frame, this driver will. Time unit is ms.
+ */
+#define DEFAULT_POWER_OFF_DELAY 10000
+#define DEFAULT_MONITOR_DELAY 1000
+
+#define NUM_SENSORS 5
+
+/* The driver monitors GPADC - ADC_AUX1 and ADC_AUX2 */
+#define NUM_MONITORED_SENSORS 2
+
+struct ab8500_temp {
+	struct platform_device *pdev;
+	struct device *hwmon_dev;
+	struct ab8500_gpadc *gpadc;
+	u8 gpadc_addr[NUM_SENSORS];
+	unsigned long min[NUM_SENSORS];
+	unsigned long max[NUM_SENSORS];
+	unsigned long max_hyst[NUM_SENSORS];
+	unsigned long crit[NUM_SENSORS];
+	unsigned long min_alarm[NUM_SENSORS];
+	unsigned long max_alarm[NUM_SENSORS];
+	unsigned long max_hyst_alarm[NUM_SENSORS];
+	unsigned long crit_alarm[NUM_SENSORS];
+	struct delayed_work work;
+	struct delayed_work power_off_work;
+	struct mutex lock;
+	/* Delay (ms) between temperature readings */
+	unsigned long gpadc_monitor_delay;
+	/* Delay (ms) before power off */
+	unsigned long power_off_delay;
+};
+
+/*
+ * Thresholds are considered inactive if set to 0.
+ * To avoid confusion for user space applications,
+ * the temp monitor delay is set to 0 if all thresholds
+ * are 0.
+ */
+static bool find_active_thresholds(struct ab8500_temp *data)
+{
+	int i;
+	for (i = 0; i < NUM_MONITORED_SENSORS; i++)
+		if (data->max[i] != 0 || data->max_hyst[i] != 0
+		    || data->min[i] != 0)
+			return true;
+
+	dev_dbg(&data->pdev->dev, "No active thresholds,"
+		"cancel deferred job (if it exists)"
+		"and reset temp monitor delay\n");
+	cancel_delayed_work_sync(&data->work);
+	return false;
+}
+
+
+static inline void schedule_monitor(struct ab8500_temp *data) {
+	unsigned long delay_in_jiffies;
+	delay_in_jiffies = msecs_to_jiffies(data->gpadc_monitor_delay);
+	schedule_delayed_work(&data->work, delay_in_jiffies);
+}
+
+static void thermal_power_off(struct work_struct *work)
+{
+	struct ab8500_temp *data = container_of(work, struct ab8500_temp,
+						power_off_work.work);
+
+	dev_warn(&data->pdev->dev, "Power off due to AB8500 thermal warning\n");
+	pm_power_off();
+}
+
+static void gpadc_monitor(struct work_struct *work)
+{
+	unsigned long delay_in_jiffies;
+	int val, i, ret;
+	/* Container for alarm node name */
+	char alarm_node[30];
+
+	bool updated_min_alarm = false;
+	bool updated_max_alarm = false;
+	bool updated_max_hyst_alarm = false;
+	struct ab8500_temp *data = container_of(work, struct ab8500_temp,
+						work.work);
+
+	for (i = 0; i < NUM_MONITORED_SENSORS; i++) {
+		/* Thresholds are considered inactive if set to 0 */
+		if (data->max[i] == 0 && data->max_hyst[i] == 0
+		    && data->min[i] == 0)
+			continue;
+
+		val = ab8500_gpadc_convert(data->gpadc, data->gpadc_addr[i]);
+		if (val < 0) {
+			dev_err(&data->pdev->dev, "GPADC read failed\n");
+			continue;
+		}
+
+		mutex_lock(&data->lock);
+		if (data->min[i] != 0) {
+			if (val < data->min[i]) {
+				if (data->min_alarm[i] == 0) {
+					data->min_alarm[i] = 1;
+					updated_min_alarm = true;
+				}
+			} else {
+				if (data->min_alarm[i] == 1) {
+					data->min_alarm[i] = 0;
+					updated_min_alarm = true;
+				}
+			}
+
+		}
+		if (data->max[i] != 0) {
+			if (val > data->max[i]) {
+				if (data->max_alarm[i] == 0) {
+					data->max_alarm[i] = 1;
+					updated_max_alarm = true;
+				}
+			} else {
+				if (data->max_alarm[i] == 1) {
+					data->max_alarm[i] = 0;
+					updated_max_alarm = true;
+				}
+			}
+
+		}
+		if (data->max_hyst[i] != 0) {
+			if (val > data->max_hyst[i]) {
+				if (data->max_hyst_alarm[i] == 0) {
+					data->max_hyst_alarm[i] = 1;
+					updated_max_hyst_alarm = true;
+				}
+			} else {
+				if (data->max_hyst_alarm[i] == 1) {
+					data->max_hyst_alarm[i] = 0;
+					updated_max_hyst_alarm = true;
+				}
+			}
+		}
+		mutex_unlock(&data->lock);
+
+		/* hwmon attr index starts at 1, thus "i+1" below */
+		if (updated_min_alarm) {
+			ret = snprintf(alarm_node, 16, "temp%d_min_alarm",
+				       (i + 1));
+			if (ret < 0) {
+				dev_err(&data->pdev->dev,
+					"Unable to update alarm node (%d)",
+					ret);
+				break;
+			}
+			sysfs_notify(&data->pdev->dev.kobj, NULL, alarm_node);
+		}
+		if (updated_max_alarm) {
+			ret = snprintf(alarm_node, 16, "temp%d_max_alarm",
+				       (i + 1));
+			if (ret < 0) {
+				dev_err(&data->pdev->dev,
+					"Unable to update alarm node (%d)",
+					ret);
+				break;
+			}
+			sysfs_notify(&data->pdev->dev.kobj, NULL, alarm_node);
+		}
+		if (updated_max_hyst_alarm) {
+			ret = snprintf(alarm_node, 21, "temp%d_max_hyst_alarm",
+				       (i + 1));
+			if (ret < 0) {
+				dev_err(&data->pdev->dev,
+					"Unable to update alarm node (%d)",
+					ret);
+				break;
+			}
+			sysfs_notify(&data->pdev->dev.kobj, NULL, alarm_node);
+		}
+	}
+	delay_in_jiffies = msecs_to_jiffies(data->gpadc_monitor_delay);
+	schedule_delayed_work(&data->work, delay_in_jiffies);
+}
+
+static inline void gpadc_monitor_exit(struct platform_device *pdev)
+{
+	struct ab8500_temp *data = platform_get_drvdata(pdev);
+	cancel_delayed_work_sync(&data->work);
+}
+
+static ssize_t set_temp_monitor_delay(struct device *dev,
+				      struct device_attribute *devattr,
+				      const char *buf, size_t count)
+{
+	int res;
+	unsigned long delay_in_s;
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+
+	res = strict_strtoul(buf, 10, &delay_in_s);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	data->gpadc_monitor_delay = delay_in_s * 1000;
+
+	if (find_active_thresholds(data)) {
+		schedule_monitor(data);
+	}
+
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t set_temp_power_off_delay(struct device *dev,
+					struct device_attribute *devattr,
+					const char *buf, size_t count)
+{
+	int res;
+	unsigned long delay_in_s;
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+
+	res = strict_strtoul(buf, 10, &delay_in_s);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	data->power_off_delay = delay_in_s * 1000;
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t show_temp_monitor_delay(struct device *dev,
+				       struct device_attribute *devattr,
+				       char *buf)
+{
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	/* return time in s, not ms */
+	return sprintf(buf, "%lu\n", (data->gpadc_monitor_delay) / 1000);
+}
+
+static ssize_t show_temp_power_off_delay(struct device *dev,
+					 struct device_attribute *devattr,
+					 char *buf)
+{
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	/* return time in s, not ms */
+	return sprintf(buf, "%lu\n", (data->power_off_delay) / 1000);
+}
+
+/* HWMON sysfs interface */
+static ssize_t show_name(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
+{
+	/*
+	 * To avoid confusion between sensor label and chip name, the function
+	 * "show_label" is not used to return the chip name.
+	 */
+	return sprintf(buf, "ab8500\n");
+}
+
+static ssize_t show_label(struct device *dev,
+			  struct device_attribute *devattr, char *buf)
+{
+	char *name;
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int index = attr->index;
+
+	/*
+	 * Make sure these labels correspond to the attribute indexes
+	 * used when calling SENSOR_DEVICE_ATRR.
+	 * Temperature sensors outside ab8500 (read via GPADC) are marked
+	 * with prefix ext_
+	 */
+	switch (index) {
+	case 1:
+		name = "ext_rtc_xtal";
+		break;
+	case 2:
+		name = "ext_db8500";
+		break;
+	case 3:
+		name = "bat_temp";
+		break;
+	case 4:
+		name = "ab8500";
+		break;
+    case 5:
+		name = "bat_ctrl";
+		break;
+	default:
+		return -EINVAL;
+	}
+	return sprintf(buf, "%s\n", name);
+}
+
+static ssize_t show_input(struct device *dev,
+			  struct device_attribute *devattr, char *buf)
+{
+	int val;
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	u8 gpadc_addr = data->gpadc_addr[attr->index - 1];
+
+	val = ab8500_gpadc_convert(data->gpadc, gpadc_addr);
+	if (val < 0)
+		dev_err(&data->pdev->dev, "GPADC read failed\n");
+
+	return sprintf(buf, "%d\n", val);
+}
+
+/* set functions (RW nodes) */
+static ssize_t set_min(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	/*
+	 * Threshold is considered inactive if set to 0
+	 * hwmon attr index starts at 1, thus "attr->index-1" below
+	 */
+	if (val == 0)
+		data->min_alarm[attr->index - 1] = 0;
+
+	data->min[attr->index - 1] = val;
+
+	if (val == 0)
+		(void) find_active_thresholds(data);
+	else
+		schedule_monitor(data);
+
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t set_max(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	/*
+	 * Threshold is considered inactive if set to 0
+	 * hwmon attr index starts at 1, thus "attr->index-1" below
+	 */
+	if (val == 0)
+		data->max_alarm[attr->index - 1] = 0;
+
+	data->max[attr->index - 1] = val;
+
+	if (val == 0)
+		(void) find_active_thresholds(data);
+	else
+		schedule_monitor(data);
+
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t set_max_hyst(struct device *dev,
+			    struct device_attribute *devattr,
+			    const char *buf, size_t count)
+{
+	unsigned long val;
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	/*
+	 * Threshold is considered inactive if set to 0
+	 * hwmon attr index starts at 1, thus "attr->index-1" below
+	 */
+	if (val == 0)
+		data->max_hyst_alarm[attr->index - 1] = 0;
+
+	data->max_hyst[attr->index - 1] = val;
+
+	if (val == 0)
+		(void) find_active_thresholds(data);
+	else
+		schedule_monitor(data);
+
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+/*
+ * show functions (RO nodes)
+ * Notice that min/max/max_hyst refer to millivolts and not millidegrees
+ */
+static ssize_t show_min(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->min[attr->index - 1]);
+}
+
+static ssize_t show_max(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->max[attr->index - 1]);
+}
+
+static ssize_t show_max_hyst(struct device *dev,
+			     struct device_attribute *devattr, char *buf)
+{
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->max_hyst[attr->index - 1]);
+}
+
+/* Alarms */
+static ssize_t show_min_alarm(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
+{
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->min_alarm[attr->index - 1]);
+}
+
+static ssize_t show_max_alarm(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
+{
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->max_alarm[attr->index - 1]);
+}
+
+static ssize_t show_max_hyst_alarm(struct device *dev,
+				   struct device_attribute *devattr, char *buf)
+{
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->max_hyst_alarm[attr->index - 1]);
+}
+
+static ssize_t show_crit_alarm(struct device *dev,
+			       struct device_attribute *devattr, char *buf)
+{
+	struct ab8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->crit_alarm[attr->index - 1]);
+}
+
+/*These node are not included in the kernel hwmon sysfs interface */
+static SENSOR_DEVICE_ATTR(temp_monitor_delay, S_IRUGO | S_IWUSR,
+			  show_temp_monitor_delay, set_temp_monitor_delay, 0);
+static SENSOR_DEVICE_ATTR(temp_power_off_delay, S_IRUGO | S_IWUSR,
+			  show_temp_power_off_delay,
+			  set_temp_power_off_delay, 0);
+
+/* Chip name, required by hwmon*/
+static SENSOR_DEVICE_ATTR(name, S_IRUGO, show_name, NULL, 0);
+
+/* GPADC - ADC_AUX1 */
+static SENSOR_DEVICE_ATTR(temp1_label, S_IRUGO, show_label, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_input, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_min, set_min, 1);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_max, set_max, 1);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO,
+			  show_max_hyst, set_max_hyst, 1);
+static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_min_alarm, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_max_alarm, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst_alarm, S_IRUGO,
+			  show_max_hyst_alarm, NULL, 1);
+
+/* GPADC - ADC_AUX2 */
+static SENSOR_DEVICE_ATTR(temp2_label, S_IRUGO, show_label, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_input, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp2_min, S_IWUSR | S_IRUGO, show_min, set_min, 2);
+static SENSOR_DEVICE_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_max, set_max, 2);
+static SENSOR_DEVICE_ATTR(temp2_max_hyst, S_IWUSR | S_IRUGO,
+			  show_max_hyst, set_max_hyst, 2);
+static SENSOR_DEVICE_ATTR(temp2_min_alarm, S_IRUGO, show_min_alarm, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp2_max_alarm, S_IRUGO, show_max_alarm, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp2_max_hyst_alarm, S_IRUGO,
+			  show_max_hyst_alarm, NULL, 2);
+
+/* GPADC - BTEMP_BALL */
+static SENSOR_DEVICE_ATTR(temp3_label, S_IRUGO, show_label, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp3_input, S_IRUGO, show_input, NULL, 3);
+
+/* AB8500 */
+static SENSOR_DEVICE_ATTR(temp4_label, S_IRUGO, show_label, NULL, 4);
+static SENSOR_DEVICE_ATTR(temp4_crit_alarm, S_IRUGO,
+			  show_crit_alarm, NULL, 4);
+
+/* GPADC - BAT_CTRL */
+static SENSOR_DEVICE_ATTR(temp5_label, S_IRUGO, show_label, NULL, 5);
+static SENSOR_DEVICE_ATTR(temp5_input, S_IRUGO, show_input, NULL, 5);
+
+static struct attribute *ab8500_temp_attributes[] = {
+	&sensor_dev_attr_temp_power_off_delay.dev_attr.attr,
+	&sensor_dev_attr_temp_monitor_delay.dev_attr.attr,
+	&sensor_dev_attr_name.dev_attr.attr,
+	/* GPADC - ADC_AUX1 */
+	&sensor_dev_attr_temp1_label.dev_attr.attr,
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst_alarm.dev_attr.attr,
+	/* GPADC - ADC_AUX2 */
+	&sensor_dev_attr_temp2_label.dev_attr.attr,
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_min.dev_attr.attr,
+	&sensor_dev_attr_temp2_max.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_hyst_alarm.dev_attr.attr,
+	/* GPADC - BTEMP_BALL */
+	&sensor_dev_attr_temp3_label.dev_attr.attr,
+	&sensor_dev_attr_temp3_input.dev_attr.attr,
+	/* AB8500 */
+	&sensor_dev_attr_temp4_label.dev_attr.attr,
+	&sensor_dev_attr_temp4_crit_alarm.dev_attr.attr,
+	/* GPADC - BAT_CTRL */
+	&sensor_dev_attr_temp5_label.dev_attr.attr,
+	&sensor_dev_attr_temp5_input.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group ab8500_temp_group = {
+	.attrs = ab8500_temp_attributes,
+};
+
+static irqreturn_t ab8500_temp_irq_handler(int irq, void *irq_data)
+{
+	unsigned long delay_in_jiffies;
+	struct platform_device *pdev = irq_data;
+	struct ab8500_temp *data = platform_get_drvdata(pdev);
+
+	/*
+	 * Make sure the magic numbers below corresponds to the node
+	 * used for AB8500 thermal warning from HW.
+	 */
+	mutex_lock(&data->lock);
+	data->crit_alarm[3] = 1;
+	mutex_unlock(&data->lock);
+	sysfs_notify(&pdev->dev.kobj, NULL, "temp4_crit_alarm");
+	dev_info(&pdev->dev, "AB8500 thermal warning, power off in %lu s\n",
+		 data->power_off_delay);
+	delay_in_jiffies = msecs_to_jiffies(data->power_off_delay);
+	schedule_delayed_work(&data->power_off_work, delay_in_jiffies);
+	return IRQ_HANDLED;
+}
+
+static int setup_irqs(struct platform_device *pdev)
+{
+	int ret;
+	int irq = platform_get_irq_byname(pdev, "AB8500_TEMP_WARM");
+
+	if (irq < 0)
+		dev_err(&pdev->dev, "Get irq by name failed\n");
+
+	ret = request_threaded_irq(irq, NULL, ab8500_temp_irq_handler,
+				   IRQF_NO_SUSPEND, "ab8500-temp", pdev);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Request threaded irq failed (%d)\n", ret);
+
+	return ret;
+}
+
+static int __devinit ab8500_temp_probe(struct platform_device *pdev)
+{
+	struct ab8500_temp *data;
+	int err;
+
+	data = kzalloc(sizeof(struct ab8500_temp), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	err = setup_irqs(pdev);
+	if (err < 0)
+		goto exit;
+
+	data->gpadc = ab8500_gpadc_get();
+
+	data->hwmon_dev = hwmon_device_register(&pdev->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		dev_err(&pdev->dev, "Class registration failed (%d)\n", err);
+		goto exit;
+	}
+
+	INIT_DELAYED_WORK_DEFERRABLE(&data->work, gpadc_monitor);
+	INIT_DELAYED_WORK(&data->power_off_work, thermal_power_off);
+
+	/*
+	 * Setup HW defined data.
+	 *
+	 * Reference hardware (HREF):
+	 *
+	 * GPADC - ADC_AUX1, connected to NTC R2148 next to RTC_XTAL on HREF
+	 * GPADC - ADC_AUX2, connected to NTC R2150 near DB8500 on HREF
+	 * Hence, temp#_min/max/max_hyst refer to millivolts and not
+	 * millidegrees
+	 *
+	 * HREF HW does not support reading AB8500 temperature. BUT an
+	 * AB8500 IRQ will be launched if die crit temp limit is reached.
+	 *
+	 * Also:
+	 * Battery temperature (BatTemp and BatCtrl) thresholds will
+	 * not be exposed via hwmon.
+	 *
+	 * Make sure indexes correspond to the attribute indexes
+	 * used when calling SENSOR_DEVICE_ATRR
+	 */
+	data->gpadc_addr[0] = ADC_AUX1;
+	data->gpadc_addr[1] = ADC_AUX2;
+	data->gpadc_addr[2] = BTEMP_BALL;
+	data->gpadc_addr[4] = BAT_CTRL;
+	mutex_init(&data->lock);
+	data->pdev = pdev;
+	data->power_off_delay = DEFAULT_POWER_OFF_DELAY;
+	data->gpadc_monitor_delay = DEFAULT_MONITOR_DELAY;
+
+	platform_set_drvdata(pdev, data);
+
+	err = sysfs_create_group(&pdev->dev.kobj, &ab8500_temp_group);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Create sysfs group failed (%d)\n", err);
+		goto exit_platform_data;
+	}
+
+	return 0;
+
+exit_platform_data:
+	platform_set_drvdata(pdev, NULL);
+exit:
+	kfree(data);
+	return err;
+}
+
+static int __devexit ab8500_temp_remove(struct platform_device *pdev)
+{
+	struct ab8500_temp *data = platform_get_drvdata(pdev);
+
+	gpadc_monitor_exit(pdev);
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&pdev->dev.kobj, &ab8500_temp_group);
+	platform_set_drvdata(pdev, NULL);
+	kfree(data);
+	return 0;
+}
+
+/* No action required in suspend/resume, thus the lack of functions */
+static struct platform_driver ab8500_temp_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "ab8500-temp",
+	},
+	.probe = ab8500_temp_probe,
+	.remove = __devexit_p(ab8500_temp_remove),
+};
+
+static int __init ab8500_temp_init(void)
+{
+	return platform_driver_register(&ab8500_temp_driver);
+}
+
+static void __exit ab8500_temp_exit(void)
+{
+	platform_driver_unregister(&ab8500_temp_driver);
+}
+
+MODULE_AUTHOR("Martin Persson <martin.persson@stericsson.com>");
+MODULE_DESCRIPTION("AB8500 temperature driver");
+MODULE_LICENSE("GPL");
+
+module_init(ab8500_temp_init)
+module_exit(ab8500_temp_exit)
diff --git a/drivers/hwmon/db8500.c b/drivers/hwmon/db8500.c
new file mode 100755
index 00000000000..c08ab033ada
--- /dev/null
+++ b/drivers/hwmon/db8500.c
@@ -0,0 +1,450 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ *
+ * Author: WenHai Fang <wenhai.h.fang@stericsson.com> for
+ * ST-Ericsson.
+ * License terms: GNU Gereral Public License (GPL) version 2
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <mach/prcmu.h>
+#include <linux/hwmon.h>
+#include <linux/sysfs.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/jiffies.h>
+#include <linux/mutex.h>
+#include <linux/pm.h>
+#include <linux/io.h>
+#include <mach/hardware.h>
+
+/*
+ * If DB8500 warm interrupt is set, user space will be notified.
+ * If user space doesn't shut down the platform within this time
+ * frame, this driver will. Time unit is ms.
+ */
+#define DEFAULT_POWER_OFF_DELAY 10000
+
+/*
+ * Default measure period to 0xFF x cycle32k
+ */
+#define DEFAULT_MEASURE_TIME 0xFF
+
+/* This driver monitors DB thermal*/
+#define NUM_SENSORS 1
+
+struct db8500_temp {
+	struct platform_device *pdev;
+	struct device *hwmon_dev;
+	unsigned char min[NUM_SENSORS];
+	unsigned char max[NUM_SENSORS];
+	unsigned char crit[NUM_SENSORS];
+	unsigned char min_alarm[NUM_SENSORS];
+	unsigned char max_alarm[NUM_SENSORS];
+	unsigned short measure_time;
+	struct delayed_work power_off_work;
+	struct mutex lock;
+	/* Delay (ms) before power off */
+	unsigned long power_off_delay;
+};
+
+static void thermal_power_off(struct work_struct *work)
+{
+	struct db8500_temp *data = container_of(work, struct db8500_temp,
+						power_off_work.work);
+
+	dev_warn(&data->pdev->dev, "Power off due to DB8500 thermal warning\n");
+	pm_power_off();
+}
+
+static ssize_t set_temp_power_off_delay(struct device *dev,
+					struct device_attribute *devattr,
+					const char *buf, size_t count)
+{
+	int res;
+	unsigned long delay_in_s;
+	struct db8500_temp *data = dev_get_drvdata(dev);
+
+	res = strict_strtoul(buf, 10, &delay_in_s);
+	if (res < 0) {
+		dev_warn(&data->pdev->dev, "Set power_off_delay wrong\n");
+		return res;
+	}
+
+	mutex_lock(&data->lock);
+	data->power_off_delay = delay_in_s * 1000;
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t show_temp_power_off_delay(struct device *dev,
+					 struct device_attribute *devattr,
+					 char *buf)
+{
+	struct db8500_temp *data = dev_get_drvdata(dev);
+	/* return time in s, not ms */
+	return sprintf(buf, "%lu\n", (data->power_off_delay) / 1000);
+}
+
+/* HWMON sysfs interface */
+static ssize_t show_name(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
+{
+	return sprintf(buf, "db8500\n");
+}
+
+static ssize_t show_label(struct device *dev, struct device_attribute *devattr,
+			char *buf)
+{
+	return sprintf(buf, "db8500\n");
+}
+
+/* set functions (RW nodes) */
+static ssize_t set_min(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct db8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	val &= 0xFF;
+	if (val > data->max[attr->index - 1])
+		val = data->max[attr->index - 1];
+
+	data->min[attr->index - 1] = val;
+
+	(void)prcmu_config_hotmon(data->min[attr->index - 1],
+			data->max[attr->index - 1]);
+	mutex_unlock(&data->lock);
+	return count;
+}
+
+static ssize_t set_max(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct db8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	val &= 0xFF;
+	if (val < data->min[attr->index - 1])
+		val = data->min[attr->index - 1];
+
+	data->max[attr->index - 1] = val;
+
+	(void)prcmu_config_hotmon(data->min[attr->index - 1],
+		data->max[attr->index - 1]);
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t set_crit(struct device *dev,
+			    struct device_attribute *devattr,
+			    const char *buf, size_t count)
+{
+	unsigned long val;
+	struct db8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	val &= 0xFF;
+	data->crit[attr->index - 1] = val;
+	(void)prcmu_config_hotdog(data->crit[attr->index - 1]);
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+/* start/stop temperature measurement */
+static ssize_t start_temp(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct db8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	data->measure_time = val & 0xFFFF;
+	data->min_alarm[attr->index - 1] = 0;
+	data->max_alarm[attr->index - 1] = 0;
+	mutex_unlock(&data->lock);
+
+	(void)prcmu_start_temp_sense(data->measure_time);
+	dev_dbg(&data->pdev->dev, "DB8500 thermal start measurement\n");
+	return count;
+}
+
+static ssize_t stop_temp(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct db8500_temp *data = dev_get_drvdata(dev);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	(void)prcmu_stop_temp_sense();
+	dev_dbg(&data->pdev->dev, "DB8500 thermal stop measurement\n");
+
+	return count;
+}
+
+/*
+ * show functions (RO nodes)
+ * Notice that min/max/crit refer to degrees
+ */
+static ssize_t show_min(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	struct db8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%d\n", data->min[attr->index - 1]);
+}
+
+static ssize_t show_max(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	struct db8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%d\n", data->max[attr->index - 1]);
+}
+
+static ssize_t show_crit(struct device *dev,
+			     struct device_attribute *devattr, char *buf)
+{
+	struct db8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%d\n", data->crit[attr->index - 1]);
+}
+
+/* Alarms */
+static ssize_t show_min_alarm(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
+{
+	struct db8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%d\n", data->min_alarm[attr->index - 1]);
+}
+
+static ssize_t show_max_alarm(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
+{
+	struct db8500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%d\n", data->max_alarm[attr->index - 1]);
+}
+
+/*These node are not included in the kernel hwmon sysfs interface */
+static SENSOR_DEVICE_ATTR(temp_power_off_delay, S_IRUGO | S_IWUSR,
+			  show_temp_power_off_delay,
+			  set_temp_power_off_delay, 0);
+
+/* Chip name, required by hwmon*/
+static SENSOR_DEVICE_ATTR(name, S_IRUGO, show_name, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp1_start, S_IWUSR, NULL, start_temp, 1);
+static SENSOR_DEVICE_ATTR(temp1_stop, S_IWUSR, NULL, stop_temp, 1);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_min, set_min, 1);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_max, set_max, 1);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO,
+			show_crit, set_crit, 1);
+static SENSOR_DEVICE_ATTR(temp1_label, S_IRUGO, show_label, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_min_alarm, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_max_alarm, NULL, 1);
+
+static struct attribute *db8500_temp_attributes[] = {
+	&sensor_dev_attr_temp_power_off_delay.dev_attr.attr,
+	&sensor_dev_attr_name.dev_attr.attr,
+	&sensor_dev_attr_temp1_start.dev_attr.attr,
+	&sensor_dev_attr_temp1_stop.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_label.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group db8500_temp_group = {
+	.attrs = db8500_temp_attributes,
+};
+
+static irqreturn_t prcmu_hotmon_low_irq_handler(int irq, void *irq_data)
+{
+	struct platform_device *pdev = irq_data;
+	struct db8500_temp *data = platform_get_drvdata(pdev);
+
+	mutex_lock(&data->lock);
+	data->min_alarm[0] = 1;
+	mutex_unlock(&data->lock);
+
+	sysfs_notify(&pdev->dev.kobj, NULL, "temp1_min_alarm");
+	dev_dbg(&pdev->dev, "DB8500 thermal low warning\n");
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t prcmu_hotmon_high_irq_handler(int irq, void *irq_data)
+{
+	unsigned long delay_in_jiffies;
+	struct platform_device *pdev = irq_data;
+	struct db8500_temp *data = platform_get_drvdata(pdev);
+
+	mutex_lock(&data->lock);
+	data->max_alarm[0] = 1;
+	mutex_unlock(&data->lock);
+
+	sysfs_notify(&pdev->dev.kobj, NULL, "temp1_max_alarm");
+	dev_dbg(&pdev->dev, "DB8500 thermal warning, power off in %lu s\n",
+		 (data->power_off_delay) / 1000);
+	delay_in_jiffies = msecs_to_jiffies(data->power_off_delay);
+	schedule_delayed_work(&data->power_off_work, delay_in_jiffies);
+	return IRQ_HANDLED;
+}
+
+static int __devinit db8500_temp_probe(struct platform_device *pdev)
+{
+	struct db8500_temp *data;
+	int err = 0, i;
+	int irq;
+
+	dev_dbg(&pdev->dev, "db8500_temp: Function db8500_temp_probe.\n");
+
+	data = kzalloc(sizeof(struct db8500_temp), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	irq = platform_get_irq_byname(pdev, "IRQ_HOTMON_LOW");
+	if (irq < 0) {
+		dev_err(&pdev->dev, "Get IRQ_HOTMON_LOW failed\n");
+		goto exit;
+	}
+
+	err = request_threaded_irq(irq, NULL, prcmu_hotmon_low_irq_handler,
+		IRQF_NO_SUSPEND, "db8500_temp_low", pdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "db8500: Failed allocate HOTMON_LOW.\n");
+		goto exit;
+	} else {
+		dev_dbg(&pdev->dev, "db8500: Succeed allocate HOTMON_LOW.\n");
+	}
+
+	irq = platform_get_irq_byname(pdev, "IRQ_HOTMON_HIGH");
+	if (irq < 0) {
+		dev_err(&pdev->dev, "Get IRQ_HOTMON_HIGH failed\n");
+		goto exit;
+	}
+
+	err = request_threaded_irq(irq, NULL, prcmu_hotmon_high_irq_handler,
+		IRQF_NO_SUSPEND, "db8500_temp_high", pdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "db8500: Failed allocate HOTMON_HIGH.\n");
+		goto exit;
+	} else {
+		dev_dbg(&pdev->dev, "db8500: Succeed allocate HOTMON_HIGH.\n");
+	}
+
+	data->hwmon_dev = hwmon_device_register(&pdev->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		dev_err(&pdev->dev, "Class registration failed (%d)\n", err);
+		goto exit;
+	}
+
+	for (i = 0; i < NUM_SENSORS; i++) {
+		data->min[i] = 0;
+		data->max[i] = 0xFF;
+		data->crit[i] = 0xFF;
+		data->min_alarm[i] = 0;
+		data->max_alarm[i] = 0;
+	}
+
+	mutex_init(&data->lock);
+	INIT_DELAYED_WORK(&data->power_off_work, thermal_power_off);
+
+	data->pdev = pdev;
+	data->power_off_delay = DEFAULT_POWER_OFF_DELAY;
+	data->measure_time = DEFAULT_MEASURE_TIME;
+
+	platform_set_drvdata(pdev, data);
+
+	err = sysfs_create_group(&pdev->dev.kobj, &db8500_temp_group);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Create sysfs group failed (%d)\n", err);
+		goto exit_platform_data;
+	}
+
+	return 0;
+
+exit_platform_data:
+	platform_set_drvdata(pdev, NULL);
+exit:
+	kfree(data);
+	return err;
+}
+
+static int __devexit db8500_temp_remove(struct platform_device *pdev)
+{
+	struct db8500_temp *data = platform_get_drvdata(pdev);
+
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&pdev->dev.kobj, &db8500_temp_group);
+	platform_set_drvdata(pdev, NULL);
+	kfree(data);
+	return 0;
+}
+
+/* No action required in suspend/resume, thus the lack of functions */
+static struct platform_driver db8500_temp_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "db8500_temp",
+	},
+	.probe = db8500_temp_probe,
+	.remove = __devexit_p(db8500_temp_remove),
+};
+
+static int __init db8500_temp_init(void)
+{
+	return platform_driver_register(&db8500_temp_driver);
+}
+
+static void __exit db8500_temp_exit(void)
+{
+	platform_driver_unregister(&db8500_temp_driver);
+}
+
+MODULE_AUTHOR("WenHai Fang <wenhai.h.fang@stericsson.com>");
+MODULE_DESCRIPTION("DB8500 temperature driver");
+MODULE_LICENSE("GPL");
+
+module_init(db8500_temp_init)
+module_exit(db8500_temp_exit)
diff --git a/drivers/hwmon/l3g4200d.c b/drivers/hwmon/l3g4200d.c
new file mode 100644
index 00000000000..39a4e32ee2b
--- /dev/null
+++ b/drivers/hwmon/l3g4200d.c
@@ -0,0 +1,694 @@
+/*
+ * ST L3G4200D 3-Axis Gyroscope Driver
+ *
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Chethan Krishna N <chethan.krishna@stericsson.com> for ST-Ericsson
+ * Licence terms: GNU General Public Licence (GPL) version 2
+ */
+
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+
+#include <linux/l3g4200d.h>
+#include <linux/regulator/consumer.h>
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+
+/* l3g4200d gyroscope registers */
+
+#define WHO_AM_I				0x0F
+
+#define CTRL_REG1				0x20    /* CTRL REG1 */
+#define CTRL_REG2				0x21    /* CTRL REG2 */
+#define CTRL_REG3				0x22    /* CTRL_REG3 */
+#define CTRL_REG4				0x23    /* CTRL_REG4 */
+#define CTRL_REG5				0x24    /* CTRL_REG5 */
+
+#define AXISDATA_REG			0x28
+
+/** Registers Contents */
+
+#define WHOAMI_L3G4200D			0x00D3	/* Expected content for WAI register*/
+
+/* CTRL_REG1 */
+#define PM_OFF					0x00
+#define PM_ON					0x01
+#define ENABLE_ALL_AXES			0x07
+#define BW00					0x00
+#define BW01					0x10
+#define BW10					0x20
+#define BW11					0x30
+#define ODR00					0x00  /* ODR = 100Hz */
+#define ODR01					0x40  /* ODR = 200Hz */
+#define ODR10					0x80  /* ODR = 400Hz */
+#define ODR11					0xC0  /* ODR = 800Hz */
+#define L3G4200D_PM_BIT			3
+#define L3G4200D_PM_MASK		(0x01 << L3G4200D_PM_BIT)
+#define L3G4200D_ODR_BIT		4
+#define L3G4200D_ODR_MASK		(0x0F << L3G4200D_ODR_BIT)
+#define L3G4200D_ODR_MIN_VAL	0x00
+#define L3G4200D_ODR_MAX_VAL	0x0F
+
+/* CTRL_REG4 */
+#define FS250					0x00
+#define FS500					0x01
+#define FS2000					0x03
+#define BDU_ENABLE				0x80
+#define L3G4200D_FS_BIT			6
+#define L3G4200D_FS_MASK		(0x3 << L3G4200D_FS_BIT)
+
+/* multiple byte transfer enable */
+#define MULTIPLE_I2C_TR			0x80
+
+/* device status defines */
+#define DEVICE_OFF 0
+#define DEVICE_ON 1
+#define DEVICE_SUSPENDED 2
+
+/*
+ * L3G4200D gyroscope data
+ * brief structure containing gyroscope values for yaw, pitch and roll in
+ * signed short
+ */
+
+struct l3g4200d_gyro_values {
+	short x;	/* x-axis angular rate data. */
+	short y;	/* y-axis angluar rate data. */
+	short z;	/* z-axis angular rate data. */
+};
+
+struct l3g4200d_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	struct l3g4200d_gyro_values data;
+	struct l3g4200d_gyr_platform_data pdata;
+	struct regulator *regulator;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+	unsigned char powermode;
+	unsigned char odr;
+	unsigned char range;
+	int device_status;
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void l3g4200d_early_suspend(struct early_suspend *ddata);
+static void l3g4200d_late_resume(struct early_suspend *ddata);
+#endif
+
+static int l3g4200d_write(struct l3g4200d_data *ddata, u8 reg,
+				u8 val, char *msg)
+{
+	int ret = i2c_smbus_write_byte_data(ddata->client, reg, val);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"i2c_smbus_write_byte_data failed error %d\
+				Register (%s)\n", ret, msg);
+	return ret;
+}
+
+static int l3g4200d_read(struct l3g4200d_data *ddata, u8 reg, char *msg)
+{
+	int ret = i2c_smbus_read_byte_data(ddata->client, reg);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"i2c_smbus_read_byte_data failed error %d\
+				Register (%s)\n", ret, msg);
+	return ret;
+}
+
+static int l3g4200d_readdata(struct l3g4200d_data *ddata)
+{
+	unsigned char gyro_data[6];
+	short data[3];
+	int ret;
+
+	ret = i2c_smbus_read_i2c_block_data(ddata->client,
+			AXISDATA_REG | MULTIPLE_I2C_TR, 6, gyro_data);
+	if (ret < 0) {
+		dev_err(&ddata->client->dev,
+				"i2c_smbus_read_byte_data failed error %d\
+				Register AXISDATA_REG\n", ret);
+		return ret;
+	}
+
+	data[0] = (short) (((gyro_data[1]) << 8) | gyro_data[0]);
+	data[1] = (short) (((gyro_data[3]) << 8) | gyro_data[2]);
+	data[2] = (short) (((gyro_data[5]) << 8) | gyro_data[4]);
+
+	data[ddata->pdata.axis_map_x] = ddata->pdata.negative_x ?
+		-data[ddata->pdata.axis_map_x] : data[ddata->pdata.axis_map_x];
+	data[ddata->pdata.axis_map_y] = ddata->pdata.negative_y ?
+		-data[ddata->pdata.axis_map_y] : data[ddata->pdata.axis_map_y];
+	data[ddata->pdata.axis_map_z] = ddata->pdata.negative_z ?
+		-data[ddata->pdata.axis_map_z] : data[ddata->pdata.axis_map_z];
+
+	ddata->data.x = data[ddata->pdata.axis_map_x];
+	ddata->data.y = data[ddata->pdata.axis_map_y];
+	ddata->data.z = data[ddata->pdata.axis_map_z];
+
+	return ret;
+}
+
+static ssize_t l3g4200d_show_gyrodata(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->powermode == PM_OFF ||
+			ddata->device_status == DEVICE_SUSPENDED) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	ret = l3g4200d_readdata(ddata);
+
+	if (ret < 0) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return sprintf(buf, "%8x:%8x:%8x\n", ddata->data.x, ddata->data.y,
+			ddata->data.z);
+}
+
+static ssize_t l3g4200d_show_range(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->range);
+}
+
+static ssize_t l3g4200d_store_range(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+	long received_value;
+	unsigned char value;
+	int error;
+
+	error = strict_strtol(buf, 0, &received_value);
+	if (error)
+		return error;
+
+	/* check if the received range is in valid range */
+	if (received_value < FS250 || received_value > FS2000)
+		return -EINVAL;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->powermode == PM_OFF) {
+		dev_info(&ddata->client->dev,
+				"The device is switched off, turn it ON using powermode\n");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/* enable the BDU bit */
+	value = BDU_ENABLE;
+	value |= ((received_value << L3G4200D_FS_BIT) & L3G4200D_FS_MASK);
+
+	ddata->range = received_value;
+
+	error = l3g4200d_write(ddata, CTRL_REG4, value, "CTRL_REG4");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+	return count;
+}
+
+static ssize_t l3g4200d_show_datarate(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->odr >> L3G4200D_ODR_BIT);
+}
+
+static ssize_t l3g4200d_store_datarate(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+	long received_value;
+	unsigned char value;
+	int error;
+
+	error = strict_strtol(buf, 0, &received_value);
+	if (error)
+		return error;
+
+	/* check if the received output datarate value is in valid range */
+	if (received_value < L3G4200D_ODR_MIN_VAL ||
+			received_value > L3G4200D_ODR_MAX_VAL)
+		return -EINVAL;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->powermode == PM_OFF) {
+		dev_info(&ddata->client->dev,
+				"The device is switched off, turn it ON using powermode\n");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/*
+	 * read the current contents of CTRL_REG1
+	 * retain any bits set other than the odr bits
+	 */
+	error = l3g4200d_read(ddata, CTRL_REG1, "CTRL_REG1");
+
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	} else
+		value = error;
+
+	value &= ~L3G4200D_ODR_MASK;
+	value |= ((received_value << L3G4200D_ODR_BIT) & L3G4200D_ODR_MASK);
+
+	ddata->odr = received_value << L3G4200D_ODR_BIT;
+
+	error = l3g4200d_write(ddata, CTRL_REG1, value, "CTRL_REG1");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+	return count;
+}
+
+static ssize_t l3g4200d_show_powermode(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->powermode);
+}
+
+static ssize_t l3g4200d_store_powermode(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+	long received_value;
+	unsigned char value;
+	int error;
+
+	error = strict_strtol(buf, 0, &received_value);
+	if (error)
+		return error;
+
+	/* check if the received power mode is either 0 or 1 */
+	if (received_value < PM_OFF || received_value > PM_ON)
+		return -EINVAL;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_SUSPENDED &&
+			received_value == PM_OFF) {
+		ddata->powermode = received_value;
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/* if sent value is same as current value do nothing */
+	if (ddata->powermode == received_value) {
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/* turn on the power suppliy if it was turned off previously */
+	if (ddata->regulator && ddata->powermode == PM_OFF
+			&& (ddata->device_status == DEVICE_OFF
+				|| ddata->device_status == DEVICE_SUSPENDED)) {
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+	}
+
+	/*
+	 * read the current contents of CTRL_REG1
+	 * retain any bits set other than the power bit
+	 */
+	error = l3g4200d_read(ddata, CTRL_REG1, "CTRL_REG1");
+
+	if (error < 0) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+		mutex_unlock(&ddata->lock);
+		return error;
+	} else
+		value = error;
+
+	value &= ~L3G4200D_PM_MASK;
+	value |= ((received_value << L3G4200D_PM_BIT) & L3G4200D_PM_MASK);
+
+	ddata->powermode = received_value;
+
+	error = l3g4200d_write(ddata, CTRL_REG1, value, "CTRL_REG1");
+	if (error < 0) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	if (received_value == PM_OFF) {
+		/* set the other configuration values to defaults */
+		ddata->odr = ODR00 | BW00;
+		ddata->range = FS250;
+
+		/* turn off the power supply */
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+	mutex_unlock(&ddata->lock);
+	return count;
+}
+
+static DEVICE_ATTR(gyrodata, S_IRUGO, l3g4200d_show_gyrodata, NULL);
+
+static DEVICE_ATTR(range, S_IRUGO | S_IWUGO,
+		l3g4200d_show_range, l3g4200d_store_range);
+
+static DEVICE_ATTR(datarate, S_IRUGO | S_IWUGO,
+		l3g4200d_show_datarate, l3g4200d_store_datarate);
+
+static DEVICE_ATTR(powermode, S_IRUGO | S_IWUGO,
+		l3g4200d_show_powermode, l3g4200d_store_powermode);
+
+static struct attribute *l3g4200d_attributes[] = {
+	&dev_attr_gyrodata.attr,
+	&dev_attr_range.attr,
+	&dev_attr_datarate.attr,
+	&dev_attr_powermode.attr,
+	NULL
+};
+
+static const struct attribute_group l3g4200d_attr_group = {
+	.attrs = l3g4200d_attributes,
+};
+
+static int __devinit l3g4200d_probe(struct i2c_client *client,
+		const struct i2c_device_id *devid)
+{
+	int ret = -1;
+	struct l3g4200d_data *ddata = NULL;
+
+	if (!i2c_check_functionality(client->adapter,
+				I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_READ_I2C_BLOCK))
+		goto exit;
+
+	ddata = kzalloc(sizeof(struct l3g4200d_data), GFP_KERNEL);
+	if (ddata == NULL) {
+		ret = -ENOMEM;
+		goto error_op_failed;
+	}
+
+	ddata->client = client;
+	i2c_set_clientdata(client, ddata);
+
+	memcpy(&ddata->pdata, client->dev.platform_data, sizeof(ddata->pdata));
+	/* store default values in the data structure */
+	ddata->odr = ODR00 | BW00;
+	ddata->range = FS250;
+	ddata->powermode = PM_OFF;
+	ddata->device_status = DEVICE_OFF;
+
+	dev_set_name(&client->dev, ddata->pdata.name_gyr);
+
+	ddata->regulator = regulator_get(&client->dev, "vdd");
+	if (IS_ERR(ddata->regulator)) {
+		dev_err(&client->dev, "failed to get regulator\n");
+		ret = PTR_ERR(ddata->regulator);
+		ddata->regulator = NULL;
+	}
+
+	if (ddata->regulator) {
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+	}
+
+	ret = l3g4200d_read(ddata, WHO_AM_I, "WHO_AM_I");
+	if (ret < 0)
+		goto exit_free_regulator;
+
+	if (ret == WHOAMI_L3G4200D)
+		dev_info(&client->dev, "3-Axis Gyroscope device identification: %d\n", ret);
+	else
+		dev_info(&client->dev, "Gyroscope identification did not match\n");
+
+	mutex_init(&ddata->lock);
+
+	ret = sysfs_create_group(&client->dev.kobj, &l3g4200d_attr_group);
+	if (ret)
+		goto exit_free_regulator;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	ddata->early_suspend.level =
+		EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ddata->early_suspend.suspend = l3g4200d_early_suspend;
+	ddata->early_suspend.resume = l3g4200d_late_resume;
+	register_early_suspend(&ddata->early_suspend);
+#endif
+
+	/*
+	 * turn off the supplies until somebody turns on the device
+	 * using l3g4200d_store_powermode
+	 */
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+
+	return ret;
+
+exit_free_regulator:
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		regulator_put(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+error_op_failed:
+	kfree(ddata);
+exit:
+	dev_err(&client->dev, "probe function failed %x\n", ret);
+	return ret;
+}
+
+static int __devexit l3g4200d_remove(struct i2c_client *client)
+{
+	struct l3g4200d_data *ddata;
+	ddata = i2c_get_clientdata(client);
+	sysfs_remove_group(&client->dev.kobj, &l3g4200d_attr_group);
+
+	/* safer to turn off the device */
+	if (ddata->powermode != PM_OFF) {
+		l3g4200d_write(ddata, CTRL_REG1, PM_OFF, "CONTROL");
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			regulator_put(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+
+	i2c_set_clientdata(client, NULL);
+	kfree(ddata);
+
+	return 0;
+}
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_PM)
+
+static int l3g4200d_do_suspend(struct l3g4200d_data *ddata)
+{
+	int ret;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->powermode == PM_OFF) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
+	ret = l3g4200d_write(ddata, CTRL_REG1, PM_OFF, "CONTROL");
+
+	/* turn off the power when suspending the device */
+	if (ddata->regulator)
+		regulator_disable(ddata->regulator);
+
+	ddata->device_status = DEVICE_SUSPENDED;
+
+	mutex_unlock(&ddata->lock);
+	return ret;
+}
+
+static int l3g4200d_do_resume(struct l3g4200d_data *ddata)
+{
+	unsigned char range_value;
+	unsigned char shifted_powermode = (ddata->powermode << L3G4200D_PM_BIT);
+	unsigned char shifted_odr = (ddata->odr << L3G4200D_ODR_BIT);
+	unsigned context = ((shifted_powermode | shifted_odr) | ENABLE_ALL_AXES);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_ON)
+		goto fail;
+
+	/* in correct mode, no need to change it */
+	if (ddata->powermode == PM_OFF) {
+		ddata->device_status = DEVICE_OFF;
+		goto fail;
+	} else {
+		 ddata->device_status = DEVICE_ON;
+	}
+
+	/* turn on the power when resuming the device */
+	if (ddata->regulator)
+		regulator_enable(ddata->regulator);
+
+	ret = l3g4200d_write(ddata, CTRL_REG1, context, "CONTROL");
+	if (ret < 0)
+		goto fail;
+
+	range_value = ddata->range;
+	range_value <<= L3G4200D_FS_BIT;
+	range_value |= BDU_ENABLE;
+
+	ret = l3g4200d_write(ddata, CTRL_REG4, range_value, "RANGE");
+
+fail:
+	mutex_unlock(&ddata->lock);
+	return ret;
+}
+#endif
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#ifdef CONFIG_PM
+static int l3g4200d_suspend(struct device *dev)
+{
+	struct l3g4200d_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret = l3g4200d_do_suspend(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device\n");
+
+	return ret;
+}
+
+static int l3g4200d_resume(struct device *dev)
+{
+	struct l3g4200d_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret = l3g4200d_do_resume(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device\n");
+
+	return ret;
+}
+
+static const struct dev_pm_ops l3g4200d_dev_pm_ops = {
+	.suspend = l3g4200d_suspend,
+	.resume = l3g4200d_resume,
+};
+#endif
+#else
+static void l3g4200d_early_suspend(struct early_suspend *data)
+{
+	struct l3g4200d_data *ddata =
+		container_of(data, struct l3g4200d_data, early_suspend);
+	int ret;
+
+	ret = l3g4200d_do_suspend(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device\n");
+}
+
+static void l3g4200d_late_resume(struct early_suspend *data)
+{
+	struct l3g4200d_data *ddata =
+		container_of(data, struct l3g4200d_data, early_suspend);
+	int ret;
+
+	ret = l3g4200d_do_resume(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device\n");
+}
+#endif
+
+static const struct i2c_device_id  l3g4200d_id[] = {
+	{"l3g4200d", 0 },
+	{ },
+};
+
+static struct i2c_driver l3g4200d_driver = {
+	.driver = {
+		.name = "l3g4200d",
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+		.pm = &l3g4200d_dev_pm_ops,
+#endif
+	},
+	.probe = l3g4200d_probe,
+	.remove = l3g4200d_remove,
+	.id_table = l3g4200d_id,
+};
+
+static int __init l3g4200d_init(void)
+{
+	return i2c_add_driver(&l3g4200d_driver);
+}
+
+static void __exit l3g4200d_exit(void)
+{
+	i2c_del_driver(&l3g4200d_driver);
+}
+
+module_init(l3g4200d_init);
+module_exit(l3g4200d_exit);
+
+MODULE_DESCRIPTION("l3g4200d digital gyroscope driver");
+MODULE_AUTHOR("Chethan Krishna N");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/lsm303dlh_a.c b/drivers/hwmon/lsm303dlh_a.c
new file mode 100644
index 00000000000..8955606067d
--- /dev/null
+++ b/drivers/hwmon/lsm303dlh_a.c
@@ -0,0 +1,1338 @@
+/*
+ * lsm303dlh_a.c
+ * ST 3-Axis Accelerometer Driver
+ *
+ * Copyright (C) 2010 STMicroelectronics
+ * Author: Carmine Iascone (carmine.iascone@st.com)
+ * Author: Matteo Dameno (matteo.dameno@st.com)
+ *
+ * Copyright (C) 2010 STEricsson
+ * Author: Mian Yousaf Kaukab <mian.yousaf.kaukab@stericsson.com>
+ * Updated:Preetham Rao Kaskurthi <preetham.rao@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <mach/gpio.h>
+#endif
+
+#include <linux/lsm303dlh.h>
+#include <linux/earlysuspend.h>
+#include <linux/regulator/consumer.h>
+
+ /* lsm303dlh accelerometer registers */
+ #define WHO_AM_I    0x0F
+
+ /* ctrl 1: pm2 pm1 pm0 dr1 dr0 zenable yenable zenable */
+ #define CTRL_REG1       0x20    /* power control reg */
+ #define CTRL_REG2       0x21    /* power control reg */
+ #define CTRL_REG3       0x22    /* power control reg */
+ #define CTRL_REG4       0x23    /* interrupt control reg */
+ #define CTRL_REG5       0x24    /* interrupt control reg */
+
+ #define STATUS_REG      0x27    /* status register */
+
+ #define AXISDATA_REG    0x28    /* axis data */
+
+ #define INT1_CFG  0x30    /* interrupt 1 configuration */
+ #define INT1_SRC  0x31    /* interrupt 1 source reg    */
+ #define INT1_THS  0x32    /* interrupt 1 threshold */
+ #define INT1_DURATION  0x33    /* interrupt 1 threshold */
+
+ #define INT2_CFG  0x34    /* interrupt 2 configuration */
+ #define INT2_SRC  0x35    /* interrupt 2 source reg    */
+ #define INT2_THS  0x36    /* interrupt 2 threshold */
+ #define INT2_DURATION  0x37    /* interrupt 2 threshold */
+
+ /* Sensitivity adjustment */
+ #define SHIFT_ADJ_2G 4 /*    1/16*/
+ #define SHIFT_ADJ_4G 3 /*    2/16*/
+ #define SHIFT_ADJ_8G 2 /* ~3.9/16*/
+
+ /* Control register 1 */
+ #define LSM303DLH_A_CR1_PM_BIT 5
+ #define LSM303DLH_A_CR1_PM_MASK (0x7 << LSM303DLH_A_CR1_PM_BIT)
+ #define LSM303DLH_A_CR1_DR_BIT 3
+ #define LSM303DLH_A_CR1_DR_MASK (0x3 << LSM303DLH_A_CR1_DR_BIT)
+ #define LSM303DLH_A_CR1_EN_BIT 0
+ #define LSM303DLH_A_CR1_EN_MASK (0x7 << LSM303DLH_A_CR1_EN_BIT)
+ #define LSM303DLH_A_CR1_AXIS_ENABLE 7
+
+ /* Control register 2 */
+ #define LSM303DLH_A_CR4_ST_BIT 1
+ #define LSM303DLH_A_CR4_ST_MASK (0x1 << LSM303DLH_A_CR4_ST_BIT)
+ #define LSM303DLH_A_CR4_STS_BIT 3
+ #define LSM303DLH_A_CR4_STS_MASK (0x1 << LSM303DLH_A_CR4_STS_BIT)
+ #define LSM303DLH_A_CR4_FS_BIT 4
+ #define LSM303DLH_A_CR4_FS_MASK (0x3 << LSM303DLH_A_CR4_FS_BIT)
+ #define LSM303DLH_A_CR4_BLE_BIT 6
+ #define LSM303DLH_A_CR4_BLE_MASK (0x3 << LSM303DLH_A_CR4_BLE_BIT)
+ #define LSM303DLH_A_CR4_BDU_BIT 7
+ #define LSM303DLH_A_CR4_BDU_MASK (0x1 << LSM303DLH_A_CR4_BDU_BIT)
+
+ /* Control register 3 */
+ #define LSM303DLH_A_CR3_I1_BIT 0
+ #define LSM303DLH_A_CR3_I1_MASK (0x3 << LSM303DLH_A_CR3_I1_BIT)
+ #define LSM303DLH_A_CR3_LIR1_BIT 2
+ #define LSM303DLH_A_CR3_LIR1_MASK (0x1 << LSM303DLH_A_CR3_LIR1_BIT)
+ #define LSM303DLH_A_CR3_I2_BIT 3
+ #define LSM303DLH_A_CR3_I2_MASK (0x3 << LSM303DLH_A_CR3_I2_BIT)
+ #define LSM303DLH_A_CR3_LIR2_BIT 5
+ #define LSM303DLH_A_CR3_LIR2_MASK (0x1 << LSM303DLH_A_CR3_LIR2_BIT)
+ #define LSM303DLH_A_CR3_PPOD_BIT 6
+ #define LSM303DLH_A_CR3_PPOD_MASK (0x1 << LSM303DLH_A_CR3_PPOD_BIT)
+ #define LSM303DLH_A_CR3_IHL_BIT 7
+ #define LSM303DLH_A_CR3_IHL_MASK (0x1 << LSM303DLH_A_CR3_IHL_BIT)
+
+ #define LSM303DLH_A_CR3_I_SELF 0x0
+ #define LSM303DLH_A_CR3_I_OR   0x1
+ #define LSM303DLH_A_CR3_I_DATA 0x2
+ #define LSM303DLH_A_CR3_I_BOOT 0x3
+
+ #define LSM303DLH_A_CR3_LIR_LATCH 0x1
+
+ /* Range */
+ #define LSM303DLH_A_RANGE_2G 0x00
+ #define LSM303DLH_A_RANGE_4G 0x01
+ #define LSM303DLH_A_RANGE_8G 0x03
+
+ /* Mode */
+ #define LSM303DLH_A_MODE_OFF 0x00
+ #define LSM303DLH_A_MODE_NORMAL 0x01
+ #define LSM303DLH_A_MODE_LP_HALF 0x02
+ #define LSM303DLH_A_MODE_LP_1 0x03
+ #define LSM303DLH_A_MODE_LP_2 0x02
+ #define LSM303DLH_A_MODE_LP_5 0x05
+ #define LSM303DLH_A_MODE_LP_10 0x06
+
+ /* Rate */
+ #define LSM303DLH_A_RATE_50 0x00
+ #define LSM303DLH_A_RATE_100 0x01
+ #define LSM303DLH_A_RATE_400 0x02
+ #define LSM303DLH_A_RATE_1000 0x03
+
+ /* Sleep & Wake */
+ #define LSM303DLH_A_SLEEPWAKE_DISABLE 0x00
+ #define LSM303DLH_A_SLEEPWAKE_ENABLE 0x3
+
+/* Multiple byte transfer enable */
+#define MULTIPLE_I2C_TR 0x80
+
+/* device status defines */
+#define DEVICE_OFF 0
+#define DEVICE_ON 1
+#define DEVICE_SUSPENDED 2
+
+/* Range -2048 to 2047 */
+struct lsm303dlh_a_t {
+	short	x;
+	short	y;
+	short	z;
+};
+
+/**
+ * struct lsm303dlh_a_data - data structure used by lsm303dlh_a driver
+ * @client: i2c client
+ * @lock: mutex lock for sysfs operations
+ * @data: lsm303dlh_a_t struct containing x, y and z values
+ * @input_dev: input device
+ * @input_dev2: input device
+ * @pdata: lsm303dlh platform data
+ * @regulator: regulator
+ * @range: current range value of accelerometer
+ * @mode: current mode of operation
+ * @rate: current sampling rate
+ * @sleep_wake: sleep wake setting
+ * @shift_adjust: current shift adjust value set according to range
+ * @interrupt_control: interrupt control settings
+ * @interrupt_channel: interrupt channel 0 or 1
+ * @interrupt_configure: interrupt configurations for two channels
+ * @interrupt_duration: interrupt duration for two channels
+ * @interrupt_threshold: interrupt threshold for two channels
+ * @early_suspend: early suspend structure
+ * @device_status: device is ON, OFF or SUSPENDED
+ */
+struct lsm303dlh_a_data {
+	struct i2c_client *client;
+	/* lock for sysfs operations */
+	struct mutex lock;
+	struct lsm303dlh_a_t data;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	struct input_dev *input_dev;
+	struct input_dev *input_dev2;
+#endif
+
+	struct lsm303dlh_platform_data pdata;
+	struct regulator *regulator;
+
+	unsigned char range;
+	unsigned char mode;
+	unsigned char rate;
+	unsigned char sleep_wake;
+	int shift_adjust;
+
+	unsigned char interrupt_control;
+	unsigned int  interrupt_channel;
+
+	unsigned char interrupt_configure[2];
+	unsigned char interrupt_duration[2];
+	unsigned char interrupt_threshold[2];
+	struct early_suspend early_suspend;
+	int device_status;
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void lsm303dlh_a_early_suspend(struct early_suspend *data);
+static void lsm303dlh_a_late_resume(struct early_suspend *data);
+#endif
+
+static int lsm303dlh_a_write(struct lsm303dlh_a_data *ddata, u8 reg,
+		u8 val, char *msg)
+{
+	int ret = i2c_smbus_write_byte_data(ddata->client, reg, val);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_write_byte_data failed error %d\
+			Register (%s)\n", ret, msg);
+	return ret;
+}
+
+static int lsm303dlh_a_read(struct lsm303dlh_a_data *ddata, u8 reg, char *msg)
+{
+	int ret = i2c_smbus_read_byte_data(ddata->client, reg);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_read_byte_data failed error %d\
+			 Register (%s)\n", ret, msg);
+	return ret;
+}
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_PM)
+static int lsm303dlh_a_do_suspend(struct lsm303dlh_a_data *ddata)
+{
+	int ret;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	disable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+	disable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+#endif
+
+	ret = lsm303dlh_a_write(ddata, CTRL_REG1,
+			LSM303DLH_A_MODE_OFF, "CONTROL");
+
+	if (ddata->regulator)
+		regulator_disable(ddata->regulator);
+
+	ddata->device_status = DEVICE_SUSPENDED;
+
+	mutex_unlock(&ddata->lock);
+
+	return ret;
+}
+
+static int lsm303dlh_a_restore(struct lsm303dlh_a_data *ddata)
+{
+	unsigned char reg;
+	unsigned char shifted_mode = (ddata->mode << LSM303DLH_A_CR1_PM_BIT);
+	unsigned char shifted_rate = (ddata->rate << LSM303DLH_A_CR1_DR_BIT);
+	unsigned char context = (shifted_mode | shifted_rate);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_ON) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
+	/* in correct mode, no need to change it */
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		ddata->device_status = DEVICE_OFF;
+		mutex_unlock(&ddata->lock);
+		return 0;
+	} else
+		ddata->device_status = DEVICE_ON;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	enable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+	enable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+#endif
+
+	if (ddata->regulator)
+		regulator_enable(ddata->regulator);
+
+	/* BDU should be enabled by default/recommened */
+	reg = ddata->range;
+	reg |= LSM303DLH_A_CR4_BDU_MASK;
+	context |= LSM303DLH_A_CR1_AXIS_ENABLE;
+
+	ret = lsm303dlh_a_write(ddata, CTRL_REG1, context,
+			"CTRL_REG1");
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, CTRL_REG4, reg, "CTRL_REG4");
+
+	if (ret < 0)
+		goto fail;
+
+	/* write to the boot bit to reboot memory content */
+	ret = lsm303dlh_a_write(ddata, CTRL_REG2, 0x80, "CTRL_REG2");
+
+	if (ret < 0)
+		goto fail;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	ret = lsm303dlh_a_write(ddata, CTRL_REG3, ddata->interrupt_control,
+			"CTRL_REG3");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT1_CFG, ddata->interrupt_configure[0],
+			"INT1_CFG");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT2_CFG, ddata->interrupt_configure[1],
+			"INT2_CFG");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT1_THS, ddata->interrupt_threshold[0],
+			"INT1_THS");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT2_THS, ddata->interrupt_threshold[1],
+			"INT2_THS");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT1_DURATION,
+			ddata->interrupt_duration[0], "INT1_DURATION");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT1_DURATION,
+			ddata->interrupt_duration[1], "INT1_DURATION");
+
+	if (ret < 0)
+		goto fail;
+#endif
+
+fail:
+	if (ret < 0)
+		dev_err(&ddata->client->dev, "could not restore the device %d\n", ret);
+	mutex_unlock(&ddata->lock);
+	return ret;
+}
+#endif
+
+static int lsm303dlh_a_readdata(struct lsm303dlh_a_data *ddata)
+{
+	unsigned char acc_data[6];
+	short data[3];
+
+	int ret = i2c_smbus_read_i2c_block_data(ddata->client,
+			AXISDATA_REG | MULTIPLE_I2C_TR, 6, acc_data);
+	if (ret < 0) {
+		dev_err(&ddata->client->dev,
+				"i2c_smbus_read_byte_data failed error %d\
+				Register AXISDATA_REG \n", ret);
+		return ret;
+	}
+
+	data[0] = (short) (((acc_data[1]) << 8) | acc_data[0]);
+	data[1] = (short) (((acc_data[3]) << 8) | acc_data[2]);
+	data[2] = (short) (((acc_data[5]) << 8) | acc_data[4]);
+
+	data[0] >>= ddata->shift_adjust;
+	data[1] >>= ddata->shift_adjust;
+	data[2] >>= ddata->shift_adjust;
+
+	/* taking position and orientation of x,y,z axis into account*/
+
+	data[ddata->pdata.axis_map_x] = ddata->pdata.negative_x ?
+		-data[ddata->pdata.axis_map_x] : data[ddata->pdata.axis_map_x];
+	data[ddata->pdata.axis_map_y] = ddata->pdata.negative_y ?
+		-data[ddata->pdata.axis_map_y] : data[ddata->pdata.axis_map_y];
+	data[ddata->pdata.axis_map_z] = ddata->pdata.negative_z ?
+		-data[ddata->pdata.axis_map_z] : data[ddata->pdata.axis_map_z];
+
+	ddata->data.x = data[ddata->pdata.axis_map_x];
+	ddata->data.y = data[ddata->pdata.axis_map_y];
+	ddata->data.z = data[ddata->pdata.axis_map_z];
+
+	return ret;
+}
+
+static ssize_t lsm303dlh_a_show_data(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF ||
+			ddata->device_status == DEVICE_SUSPENDED) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	ret = lsm303dlh_a_readdata(ddata);
+
+	if (ret < 0) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return sprintf(buf, "%8x:%8x:%8x\n", ddata->data.x, ddata->data.y,
+			ddata->data.z);
+}
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+static irqreturn_t lsm303dlh_a_gpio_irq(int irq, void *device_data)
+{
+
+	struct lsm303dlh_a_data *ddata = device_data;
+	int ret;
+	unsigned char reg;
+	struct input_dev *input;
+
+	/* know your interrupt source */
+	if (irq == gpio_to_irq(ddata->pdata.irq_a1)) {
+		reg = INT1_SRC;
+		input = ddata->input_dev;
+	} else if (irq == gpio_to_irq(ddata->pdata.irq_a2)) {
+		reg = INT2_SRC;
+		input = ddata->input_dev2;
+	} else {
+		dev_err(&ddata->client->dev, "spurious interrupt");
+		return IRQ_HANDLED;
+	}
+
+	/* read the axis */
+	ret = lsm303dlh_a_readdata(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"reading data of xyz failed error %d\n", ret);
+
+	input_report_abs(input, ABS_X, ddata->data.x);
+	input_report_abs(input, ABS_Y, ddata->data.y);
+	input_report_abs(input, ABS_Z, ddata->data.z);
+	input_sync(input);
+
+	/* clear the value by reading it */
+	ret = lsm303dlh_a_read(ddata, reg, "INTTERUPT SOURCE");
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"clearing interrupt source failed error %d\n", ret);
+
+	return IRQ_HANDLED;
+
+}
+
+static ssize_t lsm303dlh_a_show_interrupt_control(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->interrupt_control);
+}
+
+static ssize_t lsm303dlh_a_store_interrupt_control(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	ddata->interrupt_control = val;
+
+	error = lsm303dlh_a_write(ddata, CTRL_REG3, val, "CTRL_REG3");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_interrupt_channel(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->interrupt_channel);
+}
+
+static ssize_t lsm303dlh_a_store_interrupt_channel(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	ddata->interrupt_channel = val;
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_interrupt_configure(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n",
+			ddata->interrupt_configure[ddata->interrupt_channel]);
+}
+
+static ssize_t lsm303dlh_a_store_interrupt_configure(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+	    mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	ddata->interrupt_configure[ddata->interrupt_channel] = val;
+
+	if (ddata->interrupt_channel == 0x0)
+		error = lsm303dlh_a_write(ddata, INT1_CFG, val, "INT1_CFG");
+	else
+		error = lsm303dlh_a_write(ddata, INT2_CFG, val, "INT2_CFG");
+
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_interrupt_duration(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n",
+			ddata->interrupt_duration[ddata->interrupt_channel]);
+}
+
+static ssize_t lsm303dlh_a_store_interrupt_duration(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	ddata->interrupt_duration[ddata->interrupt_channel] = val;
+
+	if (ddata->interrupt_channel == 0x0)
+		error = lsm303dlh_a_write(ddata, INT1_DURATION, val,
+				"INT1_DURATION");
+	else
+		error = lsm303dlh_a_write(ddata, INT2_DURATION, val,
+				"INT2_DURATION");
+
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_interrupt_threshold(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n",
+			ddata->interrupt_threshold[ddata->interrupt_channel]);
+}
+
+static ssize_t lsm303dlh_a_store_interrupt_threshold(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	ddata->interrupt_threshold[ddata->interrupt_channel] = val;
+
+	if (ddata->interrupt_channel == 0x0)
+		error = lsm303dlh_a_write(ddata, INT1_THS, val, "INT1_THS");
+	else
+		error = lsm303dlh_a_write(ddata, INT2_THS, val, "INT2_THS");
+
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+#endif
+
+static ssize_t lsm303dlh_a_show_range(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->range >> LSM303DLH_A_CR4_FS_BIT);
+}
+
+static ssize_t lsm303dlh_a_store_range(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	long val;
+	unsigned long bdu_enabled_val;
+	int error;
+
+
+	error = strict_strtol(buf, 0, &val);
+	if (error)
+		return error;
+
+	if (val < LSM303DLH_A_RANGE_2G || val > LSM303DLH_A_RANGE_8G)
+		return -EINVAL;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	ddata->range = val;
+	ddata->range <<= LSM303DLH_A_CR4_FS_BIT;
+
+	/*
+	 * Block mode update is recommended for not
+	 * ending up reading different values
+	 */
+	bdu_enabled_val = ddata->range;
+	bdu_enabled_val |= LSM303DLH_A_CR4_BDU_MASK;
+
+	error = lsm303dlh_a_write(ddata, CTRL_REG4, bdu_enabled_val,
+	"CTRL_REG4");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	switch (val) {
+	case LSM303DLH_A_RANGE_2G:
+		ddata->shift_adjust = SHIFT_ADJ_2G;
+		break;
+	case LSM303DLH_A_RANGE_4G:
+		ddata->shift_adjust = SHIFT_ADJ_4G;
+		break;
+	case LSM303DLH_A_RANGE_8G:
+		ddata->shift_adjust = SHIFT_ADJ_8G;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_mode(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->mode);
+}
+
+static ssize_t lsm303dlh_a_store_mode(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	long val;
+	unsigned char data;
+	int error;
+	bool set_boot_bit = false;
+
+	error = strict_strtol(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	/* not in correct range */
+
+	if (val < LSM303DLH_A_MODE_OFF || val > LSM303DLH_A_MODE_LP_10) {
+		mutex_unlock(&ddata->lock);
+		return -EINVAL;
+	}
+
+	if (ddata->device_status == DEVICE_SUSPENDED) {
+		if (val == LSM303DLH_A_MODE_OFF) {
+			ddata->mode = val;
+			mutex_unlock(&ddata->lock);
+			return count;
+		} else {
+			/* device is turning on after suspend, reset memory */
+			set_boot_bit = true;
+		}
+	}
+
+	/*  if same mode as existing, return */
+	if (ddata->mode == val) {
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/* turn on the supplies if already off */
+	if (ddata->regulator && ddata->mode == LSM303DLH_A_MODE_OFF
+			&& (ddata->device_status == DEVICE_OFF
+				|| ddata->device_status == DEVICE_SUSPENDED)) {
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+		enable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+		enable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+#endif
+	}
+
+	data = lsm303dlh_a_read(ddata, CTRL_REG1, "CTRL_REG1");
+
+	data &= ~LSM303DLH_A_CR1_PM_MASK;
+
+	ddata->mode = val;
+
+	data |= ((val << LSM303DLH_A_CR1_PM_BIT) & LSM303DLH_A_CR1_PM_MASK);
+
+	error = lsm303dlh_a_write(ddata, CTRL_REG1, data, "CTRL_REG1");
+	if (error < 0) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	/*
+	 * Power on request when device is in suspended state
+	 * write to the boot bit in CTRL_REG2 to reboot memory content
+	 * and ensure correct device behavior after it resumes
+	 */
+	if (set_boot_bit) {
+		error = lsm303dlh_a_write(ddata, CTRL_REG2, 0x80, "CTRL_REG2");
+		if (error < 0) {
+			if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+				regulator_disable(ddata->regulator);
+				ddata->device_status = DEVICE_OFF;
+			}
+			mutex_unlock(&ddata->lock);
+			return error;
+		}
+	}
+
+	if (val == LSM303DLH_A_MODE_OFF) {
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+		disable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+		disable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+#endif
+		/*
+		 * No need to store context here
+		 * it is not like suspend/resume
+		 * but fall back to default values
+		 */
+		ddata->rate = LSM303DLH_A_RATE_50;
+		ddata->range = LSM303DLH_A_RANGE_2G;
+		ddata->shift_adjust = SHIFT_ADJ_2G;
+
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_rate(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->rate);
+}
+
+static ssize_t lsm303dlh_a_store_rate(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	long val;
+	unsigned char data;
+	int error;
+
+	error = strict_strtol(buf, 0, &val);
+	if (error)
+		return error;
+
+	if (val < LSM303DLH_A_RATE_50 || val > LSM303DLH_A_RATE_1000)
+		return -EINVAL;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	data = lsm303dlh_a_read(ddata, CTRL_REG1, "CTRL_REG1");
+
+	data &= ~LSM303DLH_A_CR1_DR_MASK;
+
+	ddata->rate = val;
+
+	data |= ((val << LSM303DLH_A_CR1_DR_BIT) & LSM303DLH_A_CR1_DR_MASK);
+
+	error = lsm303dlh_a_write(ddata, CTRL_REG1, data, "CTRL_REG1");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_sleepwake(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->sleep_wake);
+}
+
+static ssize_t lsm303dlh_a_store_sleepwake(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	long val;
+	int error;
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		return count;
+	}
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	ddata->sleep_wake = val;
+
+	error = lsm303dlh_a_write(ddata, CTRL_REG5, ddata->sleep_wake,
+				"CTRL_REG5");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static DEVICE_ATTR(data, S_IRUGO, lsm303dlh_a_show_data, NULL);
+
+static DEVICE_ATTR(range, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_range, lsm303dlh_a_store_range);
+
+static DEVICE_ATTR(mode, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_mode, lsm303dlh_a_store_mode);
+
+static DEVICE_ATTR(rate, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_rate, lsm303dlh_a_store_rate);
+
+static DEVICE_ATTR(sleep_wake, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_sleepwake, lsm303dlh_a_store_sleepwake);
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+static DEVICE_ATTR(interrupt_control, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_interrupt_control,
+		lsm303dlh_a_store_interrupt_control);
+
+static DEVICE_ATTR(interrupt_channel, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_interrupt_channel,
+		lsm303dlh_a_store_interrupt_channel);
+
+static DEVICE_ATTR(interrupt_configure, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_interrupt_configure,
+		lsm303dlh_a_store_interrupt_configure);
+
+static DEVICE_ATTR(interrupt_duration, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_interrupt_duration,
+		lsm303dlh_a_store_interrupt_duration);
+
+static DEVICE_ATTR(interrupt_threshold, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_interrupt_threshold,
+		lsm303dlh_a_store_interrupt_threshold);
+#endif
+
+static struct attribute *lsm303dlh_a_attributes[] = {
+	&dev_attr_data.attr,
+	&dev_attr_range.attr,
+	&dev_attr_mode.attr,
+	&dev_attr_rate.attr,
+	&dev_attr_sleep_wake.attr,
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	&dev_attr_interrupt_control.attr,
+	&dev_attr_interrupt_channel.attr,
+	&dev_attr_interrupt_configure.attr,
+	&dev_attr_interrupt_duration.attr,
+	&dev_attr_interrupt_threshold.attr,
+#endif
+	NULL
+};
+
+static const struct attribute_group lsm303dlh_a_attr_group = {
+	.attrs = lsm303dlh_a_attributes,
+};
+
+static int __devinit lsm303dlh_a_probe(struct i2c_client *client,
+						const struct i2c_device_id *id)
+{
+	int ret;
+	struct lsm303dlh_a_data *ddata = NULL;
+
+	ddata = kzalloc(sizeof(struct lsm303dlh_a_data), GFP_KERNEL);
+	if (ddata == NULL) {
+		ret = -ENOMEM;
+		goto err_op_failed;
+	}
+
+	ddata->client = client;
+	i2c_set_clientdata(client, ddata);
+
+	/* copy platform specific data */
+	memcpy(&ddata->pdata, client->dev.platform_data, sizeof(ddata->pdata));
+	ddata->mode = LSM303DLH_A_MODE_OFF;
+	ddata->rate = LSM303DLH_A_RATE_50;
+	ddata->range = LSM303DLH_A_RANGE_2G;
+	ddata->sleep_wake = LSM303DLH_A_SLEEPWAKE_DISABLE;
+	ddata->shift_adjust = SHIFT_ADJ_2G;
+	ddata->device_status = DEVICE_OFF;
+	dev_set_name(&client->dev, ddata->pdata.name_a);
+
+	ddata->regulator = regulator_get(&client->dev, "vdd");
+	if (IS_ERR(ddata->regulator)) {
+		dev_err(&client->dev, "failed to get regulator\n");
+		ret = PTR_ERR(ddata->regulator);
+		ddata->regulator = NULL;
+	}
+
+	if (ddata->regulator) {
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+	}
+
+	ret = lsm303dlh_a_read(ddata, WHO_AM_I, "WHO_AM_I");
+	if (ret < 0)
+		goto exit_free_regulator;
+
+	dev_info(&client->dev, "3-Axis Accelerometer, ID : %d\n",
+			 ret);
+
+	mutex_init(&ddata->lock);
+
+	ret = sysfs_create_group(&client->dev.kobj, &lsm303dlh_a_attr_group);
+	if (ret)
+		goto exit_free_regulator;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+
+    /* accelerometer has two interrupts channels
+       (thresholds,durations and sources)
+       and can support two input devices */
+
+	ddata->input_dev = input_allocate_device();
+	if (!ddata->input_dev) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "Failed to allocate input device\n");
+		goto exit_free_regulator;
+	}
+
+	ddata->input_dev2 = input_allocate_device();
+	if (!ddata->input_dev2) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "Failed to allocate input device\n");
+		goto err_input_alloc_failed;
+	}
+
+	set_bit(EV_ABS, ddata->input_dev->evbit);
+	set_bit(EV_ABS, ddata->input_dev2->evbit);
+
+	/* x-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_X, -32768, 32767, 0, 0);
+	input_set_abs_params(ddata->input_dev2, ABS_X, -32768, 32767, 0, 0);
+	/* y-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_Y, -32768, 32767, 0, 0);
+	input_set_abs_params(ddata->input_dev2, ABS_Y, -32768, 32767, 0, 0);
+	/* z-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_Z, -32768, 32767, 0, 0);
+	input_set_abs_params(ddata->input_dev2, ABS_Z, -32768, 32767, 0, 0);
+
+	ddata->input_dev->name = "accelerometer";
+	ddata->input_dev2->name = "motion";
+
+	ret = input_register_device(ddata->input_dev);
+	if (ret) {
+		dev_err(&client->dev, "Unable to register input device: %s\n",
+			ddata->input_dev->name);
+		goto err_input_register_failed;
+	}
+
+	ret = input_register_device(ddata->input_dev2);
+	if (ret) {
+		dev_err(&client->dev, "Unable to register input device: %s\n",
+			ddata->input_dev->name);
+		goto err_input_register_failed2;
+	}
+
+	/* Register interrupt */
+	ret = request_threaded_irq(gpio_to_irq(ddata->pdata.irq_a1), NULL,
+			lsm303dlh_a_gpio_irq,
+			IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+			"lsm303dlh_a", ddata);
+	if (ret) {
+		dev_err(&client->dev, "request irq1 failed\n");
+		goto err_input_failed;
+	}
+
+	ret = request_threaded_irq(gpio_to_irq(ddata->pdata.irq_a2), NULL,
+			lsm303dlh_a_gpio_irq,
+			IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+			"lsm303dlh_a", ddata);
+	if (ret) {
+		dev_err(&client->dev, "request irq2 failed\n");
+		goto err_input_failed;
+	}
+
+	/* only mode can enable it */
+	disable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+	disable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+
+#endif
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	ddata->early_suspend.level =
+			EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ddata->early_suspend.suspend = lsm303dlh_a_early_suspend;
+	ddata->early_suspend.resume = lsm303dlh_a_late_resume;
+	register_early_suspend(&ddata->early_suspend);
+#endif
+
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+	return ret;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+err_input_failed:
+	input_unregister_device(ddata->input_dev2);
+err_input_register_failed2:
+	input_unregister_device(ddata->input_dev);
+err_input_register_failed:
+	input_free_device(ddata->input_dev2);
+err_input_alloc_failed:
+	input_free_device(ddata->input_dev);
+#endif
+exit_free_regulator:
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		regulator_put(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+err_op_failed:
+	kfree(ddata);
+	dev_err(&client->dev, "probe function fails %x", ret);
+	return ret;
+}
+
+static int __devexit lsm303dlh_a_remove(struct i2c_client *client)
+{
+	int ret;
+	struct lsm303dlh_a_data *ddata;
+
+	ddata = i2c_get_clientdata(client);
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	input_unregister_device(ddata->input_dev);
+	input_unregister_device(ddata->input_dev2);
+	input_free_device(ddata->input_dev);
+	input_free_device(ddata->input_dev2);
+#endif
+	sysfs_remove_group(&client->dev.kobj, &lsm303dlh_a_attr_group);
+
+	/* safer to make device off */
+	if (ddata->mode != LSM303DLH_A_MODE_OFF) {
+		ret = lsm303dlh_a_write(ddata, CTRL_REG1, 0, "CONTROL");
+
+		if (ret < 0) {
+			dev_err(&client->dev, "could not turn off the device %d", ret);
+			return ret;
+		}
+
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			regulator_put(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+
+	i2c_set_clientdata(client, NULL);
+	kfree(ddata);
+
+	return 0;
+}
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#ifdef CONFIG_PM
+static int lsm303dlh_a_suspend(struct device *dev)
+{
+	struct lsm303dlh_a_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret = lsm303dlh_a_do_suspend(ddata);
+
+	return ret;
+}
+
+static int lsm303dlh_a_resume(struct device *dev)
+{
+	struct lsm303dlh_a_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret	= lsm303dlh_a_restore(ddata);
+
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device");
+
+	return ret;
+}
+static const struct dev_pm_ops lsm303dlh_a_dev_pm_ops = {
+	.suspend = lsm303dlh_a_suspend,
+	.resume  = lsm303dlh_a_resume,
+};
+#endif
+#else
+static void lsm303dlh_a_early_suspend(struct early_suspend *data)
+{
+	struct lsm303dlh_a_data *ddata =
+		container_of(data, struct lsm303dlh_a_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_a_do_suspend(ddata);
+}
+
+static void lsm303dlh_a_late_resume(struct early_suspend *data)
+{
+	struct lsm303dlh_a_data *ddata =
+		container_of(data, struct lsm303dlh_a_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_a_restore(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"lsm303dlh_a late resume failed\n");
+}
+#endif /* CONFIG_PM */
+
+static const struct i2c_device_id lsm303dlh_a_id[] = {
+	{ "lsm303dlh_a", 0 },
+	{ },
+};
+
+static struct i2c_driver lsm303dlh_a_driver = {
+	.probe		= lsm303dlh_a_probe,
+	.remove		= lsm303dlh_a_remove,
+	.id_table	= lsm303dlh_a_id,
+	.driver = {
+		.name = "lsm303dlh_a",
+	#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+		.pm	=	&lsm303dlh_a_dev_pm_ops,
+	#endif
+	},
+};
+
+static int __init lsm303dlh_a_init(void)
+{
+	return i2c_add_driver(&lsm303dlh_a_driver);
+}
+
+static void __exit lsm303dlh_a_exit(void)
+{
+	i2c_del_driver(&lsm303dlh_a_driver);
+}
+
+module_init(lsm303dlh_a_init)
+module_exit(lsm303dlh_a_exit)
+
+MODULE_DESCRIPTION("lSM303DLH 3-Axis Accelerometer Driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("STMicroelectronics");
diff --git a/drivers/hwmon/lsm303dlh_m.c b/drivers/hwmon/lsm303dlh_m.c
new file mode 100644
index 00000000000..53569c7e7f9
--- /dev/null
+++ b/drivers/hwmon/lsm303dlh_m.c
@@ -0,0 +1,894 @@
+/*
+ * lsm303dlh_m.c
+ * ST 3-Axis Magnetometer Driver
+ *
+ * Copyright (C) 2010 STMicroelectronics
+ * Author: Carmine Iascone (carmine.iascone@st.com)
+ * Author: Matteo Dameno (matteo.dameno@st.com)
+ *
+ * Copyright (C) 2010 STEricsson
+ * Author: Mian Yousaf Kaukab <mian.yousaf.kaukab@stericsson.com>
+ * Updated:Preetham Rao Kaskurthi <preetham.rao@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <mach/gpio.h>
+#endif
+
+#include <linux/lsm303dlh.h>
+#include <linux/regulator/consumer.h>
+#include <linux/earlysuspend.h>
+#include <linux/kernel.h>
+
+/* lsm303dlh magnetometer registers */
+#define IRA_REG_M	0x0A
+
+/* Magnetometer registers */
+#define CRA_REG_M	0x00  /* Configuration register A */
+#define CRB_REG_M	0x01  /* Configuration register B */
+#define MR_REG_M	0x02  /* Mode register */
+#define SR_REG_M    0x09  /* Status register */
+
+/* Output register start address*/
+#define OUT_X_M		0x03
+#define OUT_Y_M		0x05
+#define OUT_Z_M		0x07
+
+/* Magnetometer X-Y gain  */
+#define XY_GAIN_1_3	1055 /* XY gain at 1.3G */
+#define XY_GAIN_1_9	 795 /* XY gain at 1.9G */
+#define XY_GAIN_2_5	 635 /* XY gain at 2.5G */
+#define XY_GAIN_4_0	 430 /* XY gain at 4.0G */
+#define XY_GAIN_4_7	 375 /* XY gain at 4.7G */
+#define XY_GAIN_5_6	 320 /* XY gain at 5.6G */
+#define XY_GAIN_8_1	 230 /* XY gain at 8.1G */
+
+/* Magnetometer Z gain  */
+#define Z_GAIN_1_3	950 /* Z gain at 1.3G */
+#define Z_GAIN_1_9	710 /* Z gain at 1.9G */
+#define Z_GAIN_2_5	570 /* Z gain at 2.5G */
+#define Z_GAIN_4_0	385 /* Z gain at 4.0G */
+#define Z_GAIN_4_7	335 /* Z gain at 4.7G */
+#define Z_GAIN_5_6	285 /* Z gain at 5.6G */
+#define Z_GAIN_8_1	205 /* Z gain at 8.1G */
+
+/* Control A regsiter. */
+#define LSM303DLH_M_CRA_DO_BIT 2
+#define LSM303DLH_M_CRA_DO_MASK (0x7 <<  LSM303DLH_M_CRA_DO_BIT)
+#define LSM303DLH_M_CRA_MS_BIT 0
+#define LSM303DLH_M_CRA_MS_MASK (0x3 <<  LSM303DLH_M_CRA_MS_BIT)
+
+/* Control B regsiter. */
+#define LSM303DLH_M_CRB_GN_BIT 5
+#define LSM303DLH_M_CRB_GN_MASK (0x7 <<  LSM303DLH_M_CRB_GN_BIT)
+
+/* Control Mode regsiter. */
+#define LSM303DLH_M_MR_MD_BIT 0
+#define LSM303DLH_M_MR_MD_MASK (0x3 <<  LSM303DLH_M_MR_MD_BIT)
+
+/* Control Status regsiter. */
+#define LSM303DLH_M_SR_RDY_BIT 0
+#define LSM303DLH_M_SR_RDY_MASK (0x1 <<  LSM303DLH_M_SR_RDY_BIT)
+#define LSM303DLH_M_SR_LOC_BIT 1
+#define LSM303DLH_M_SR_LCO_MASK (0x1 <<  LSM303DLH_M_SR_LOC_BIT)
+#define LSM303DLH_M_SR_REN_BIT 2
+#define LSM303DLH_M_SR_REN_MASK (0x1 <<  LSM303DLH_M_SR_REN_BIT)
+
+/* Magnetometer gain setting */
+#define LSM303DLH_M_RANGE_1_3G  0x01
+#define LSM303DLH_M_RANGE_1_9G  0x02
+#define LSM303DLH_M_RANGE_2_5G  0x03
+#define LSM303DLH_M_RANGE_4_0G  0x04
+#define LSM303DLH_M_RANGE_4_7G  0x05
+#define LSM303DLH_M_RANGE_5_6G  0x06
+#define LSM303DLH_M_RANGE_8_1G  0x07
+
+/* Magnetometer capturing mode  */
+#define LSM303DLH_M_MODE_CONTINUOUS 0
+#define LSM303DLH_M_MODE_SINGLE 1
+#define LSM303DLH_M_MODE_SLEEP 3
+
+/* Magnetometer output data rate */
+#define LSM303DLH_M_RATE_00_75 0x00
+#define LSM303DLH_M_RATE_01_50 0x01
+#define LSM303DLH_M_RATE_03_00 0x02
+#define LSM303DLH_M_RATE_07_50 0x03
+#define LSM303DLH_M_RATE_15_00 0x04
+#define LSM303DLH_M_RATE_30_00 0x05
+#define LSM303DLH_M_RATE_75_00 0x06
+
+/* Multiple byte transfer enable */
+#define MULTIPLE_I2C_TR 0x80
+
+/* device status defines */
+#define DEVICE_OFF 0
+#define DEVICE_ON 1
+#define DEVICE_SUSPENDED 2
+
+/**
+ * struct lsm303dlh_m_data - data structure used by lsm303dlh_m driver
+ * @client: i2c client
+ * @lock: mutex lock for sysfs operations
+ * @input_dev: input device
+ * @regulator: regulator
+ * @pdata: lsm303dlh platform data
+ * @gain: x, y and z axes gain
+ * @data: Magnetic field values of x, y and z axes
+ * @mode: current mode of operation
+ * @rate: current sampling rate
+ * @range: current range value of magnetometer
+ * @early_suspend: early suspend structure
+ * @device_status: device is ON, OFF or SUSPENDED
+ */
+struct lsm303dlh_m_data {
+	struct i2c_client *client;
+	/* lock for sysfs operations */
+	struct mutex lock;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	struct input_dev *input_dev;
+#endif
+	struct regulator *regulator;
+	struct lsm303dlh_platform_data pdata;
+
+	short gain[3];
+	short data[3];
+	unsigned char mode;
+	unsigned char rate;
+	unsigned char range;
+	struct early_suspend early_suspend;
+	int device_status;
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void lsm303dlh_m_early_suspend(struct early_suspend *data);
+static void lsm303dlh_m_late_resume(struct early_suspend *data);
+#endif
+
+static int lsm303dlh_m_set_mode(struct lsm303dlh_m_data *ddata,
+		unsigned char mode);
+static int lsm303dlh_m_write(struct lsm303dlh_m_data *ddata,
+		u8 reg, u8 val, char *msg)
+{
+	int ret = i2c_smbus_write_byte_data(ddata->client, reg, val);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_write_byte_data failed error %d\
+			Register (%s)\n", ret, msg);
+	return ret;
+}
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_PM)
+static int lsm303dlh_m_do_suspend(struct lsm303dlh_m_data *ddata)
+{
+	int ret;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	disable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+
+	ret = lsm303dlh_m_set_mode(ddata, LSM303DLH_M_MODE_SLEEP);
+
+	if (ddata->regulator)
+		regulator_disable(ddata->regulator);
+
+	ddata->device_status = DEVICE_SUSPENDED;
+
+	mutex_unlock(&ddata->lock);
+
+	return ret;
+}
+
+static int lsm303dlh_m_restore(struct lsm303dlh_m_data *ddata)
+{
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_ON) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
+	/* in correct mode, no need to change it */
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
+		ddata->device_status = DEVICE_OFF;
+		mutex_unlock(&ddata->lock);
+		return 0;
+	} else
+		 ddata->device_status = DEVICE_ON;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	enable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+
+	if (ddata->regulator)
+		regulator_enable(ddata->regulator);
+
+	ret = lsm303dlh_m_write(ddata, CRB_REG_M, ddata->range, "SET RANGE");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_m_write(ddata, CRA_REG_M, ddata->rate, "SET RATE");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_m_set_mode(ddata, ddata->mode);
+
+	if (ret < 0)
+		goto fail;
+
+fail:
+	mutex_unlock(&ddata->lock);
+	return ret;
+}
+#endif
+
+static int lsm303dlh_m_read_multi(struct lsm303dlh_m_data *ddata, u8 reg,
+		u8 count, u8 *val, char *msg)
+{
+	int ret = i2c_smbus_read_i2c_block_data(ddata->client,
+		   reg | MULTIPLE_I2C_TR, count, val);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_read_i2c_block_data failed error %d\
+			 Register (%s)\n", ret, msg);
+	return ret;
+}
+
+static ssize_t lsm303dlh_m_show_rate(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->rate >> LSM303DLH_M_CRA_DO_BIT);
+}
+
+/* set lsm303dlh magnetometer bandwidth */
+static ssize_t lsm303dlh_m_store_rate(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	unsigned char data;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	data = ((val << LSM303DLH_M_CRA_DO_BIT) & LSM303DLH_M_CRA_DO_MASK);
+	ddata->rate = data;
+
+	error = lsm303dlh_m_write(ddata, CRA_REG_M, data, "SET RATE");
+
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static int lsm303dlh_m_xyz_read(struct lsm303dlh_m_data *ddata)
+{
+	unsigned char xyz_data[6];
+	int ret = lsm303dlh_m_read_multi(ddata, OUT_X_M,
+			6, xyz_data, "OUT_X_M");
+	if (ret < 0)
+		return -EINVAL;
+
+	/* MSB is at lower address */
+	ddata->data[0] = (short)
+		(((xyz_data[0]) << 8) | xyz_data[1]);
+	ddata->data[1] = (short)
+		(((xyz_data[2]) << 8) | xyz_data[3]);
+	ddata->data[2] = (short)
+		(((xyz_data[4]) << 8) | xyz_data[5]);
+
+	/* taking orientation of x,y,z axis into account*/
+
+	ddata->data[ddata->pdata.axis_map_x] = ddata->pdata.negative_x ?
+		-ddata->data[ddata->pdata.axis_map_x] :
+		ddata->data[ddata->pdata.axis_map_x];
+	ddata->data[ddata->pdata.axis_map_y] = ddata->pdata.negative_y ?
+		-ddata->data[ddata->pdata.axis_map_y] :
+		ddata->data[ddata->pdata.axis_map_y];
+	ddata->data[ddata->pdata.axis_map_z] = ddata->pdata.negative_z ?
+		-ddata->data[ddata->pdata.axis_map_z] :
+		ddata->data[ddata->pdata.axis_map_z];
+
+	return ret;
+}
+
+static ssize_t lsm303dlh_m_gain(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%8x:%8x:%8x\n",
+			ddata->gain[ddata->pdata.axis_map_x],
+			ddata->gain[ddata->pdata.axis_map_y],
+			ddata->gain[ddata->pdata.axis_map_z]);
+}
+
+static ssize_t lsm303dlh_m_values(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP ||
+			ddata->device_status == DEVICE_SUSPENDED) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	ret = lsm303dlh_m_xyz_read(ddata);
+
+	if (ret < 0) {
+		mutex_unlock(&ddata->lock);
+		return -EINVAL;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	/* taking orientation of x,y,z axis into account*/
+
+	return sprintf(buf, "%8x:%8x:%8x\n",
+			ddata->data[ddata->pdata.axis_map_x],
+			ddata->data[ddata->pdata.axis_map_y],
+			ddata->data[ddata->pdata.axis_map_z]);
+}
+
+static int lsm303dlh_m_set_mode(struct lsm303dlh_m_data *ddata,
+		unsigned char mode)
+{
+	int ret;
+
+	mode = (mode << LSM303DLH_M_MR_MD_BIT);
+
+	ret = i2c_smbus_write_byte_data(ddata->client, MR_REG_M, mode);
+
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_write_byte_data failed error %d\
+			Register (%s)\n", ret, "MODE CONTROL");
+
+	return ret;
+}
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+
+static irqreturn_t lsm303dlh_m_gpio_irq(int irq, void *device_data)
+{
+	struct lsm303dlh_m_data *ddata = device_data;
+	int ret;
+
+	ret = lsm303dlh_m_xyz_read(ddata);
+
+	if (ret < 0) {
+		dev_err(&ddata->client->dev,
+				"reading data of xyz failed error %d\n", ret);
+		return IRQ_NONE;
+	}
+
+	/* taking orientation of x,y,z axis into account*/
+
+	input_report_abs(ddata->input_dev, ABS_X,
+			ddata->data[ddata->pdata.axis_map_x]);
+	input_report_abs(ddata->input_dev, ABS_Y,
+		   ddata->data[ddata->pdata.axis_map_y]);
+	input_report_abs(ddata->input_dev, ABS_Z,
+		   ddata->data[ddata->pdata.axis_map_z]);
+	input_sync(ddata->input_dev);
+
+	return IRQ_HANDLED;
+
+}
+#endif
+
+static ssize_t lsm303dlh_m_show_range(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->range >> LSM303DLH_M_CRB_GN_BIT);
+}
+
+static ssize_t lsm303dlh_m_store_range(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+	short xy_gain;
+	short z_gain;
+	unsigned long range;
+	int error;
+
+	error = strict_strtoul(buf, 0, &range);
+
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	switch (range) {
+	case LSM303DLH_M_RANGE_1_3G:
+		xy_gain = XY_GAIN_1_3;
+		z_gain = Z_GAIN_1_3;
+		break;
+	case LSM303DLH_M_RANGE_1_9G:
+		xy_gain = XY_GAIN_1_9;
+		z_gain = Z_GAIN_1_9;
+		break;
+	case LSM303DLH_M_RANGE_2_5G:
+		xy_gain = XY_GAIN_2_5;
+		z_gain = Z_GAIN_2_5;
+		break;
+	case LSM303DLH_M_RANGE_4_0G:
+		xy_gain = XY_GAIN_4_0;
+		z_gain = Z_GAIN_4_0;
+		break;
+	case LSM303DLH_M_RANGE_4_7G:
+		xy_gain = XY_GAIN_4_7;
+		z_gain = Z_GAIN_4_7;
+		break;
+	case LSM303DLH_M_RANGE_5_6G:
+		xy_gain = XY_GAIN_5_6;
+		z_gain = Z_GAIN_5_6;
+		break;
+	case LSM303DLH_M_RANGE_8_1G:
+		xy_gain = XY_GAIN_8_1;
+		z_gain = Z_GAIN_8_1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ddata->gain[ddata->pdata.axis_map_x] = xy_gain;
+	ddata->gain[ddata->pdata.axis_map_y] = xy_gain;
+	ddata->gain[ddata->pdata.axis_map_z] = z_gain;
+
+	range <<= LSM303DLH_M_CRB_GN_BIT;
+	range &= LSM303DLH_M_CRB_GN_MASK;
+
+	ddata->range = range;
+
+	error = lsm303dlh_m_write(ddata, CRB_REG_M, range, "SET RANGE");
+	mutex_unlock(&ddata->lock);
+
+	if (error < 0)
+		return error;
+
+	return count;
+}
+
+static ssize_t lsm303dlh_m_show_mode(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->mode);
+}
+
+static ssize_t lsm303dlh_m_store_mode(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+	unsigned long mode;
+	int error;
+
+	error = strict_strtoul(buf, 0, &mode);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_SUSPENDED &&
+			mode == LSM303DLH_M_MODE_SLEEP) {
+		ddata->mode = (mode >> LSM303DLH_M_MR_MD_BIT);
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/*  if same mode as existing, return */
+	if (ddata->mode == mode) {
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/* turn on the supplies if already off */
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP && ddata->regulator
+			&& (ddata->device_status == DEVICE_OFF
+				|| ddata->device_status == DEVICE_SUSPENDED)) {
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+		enable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+	}
+
+	error = lsm303dlh_m_set_mode(ddata, mode);
+
+	ddata->mode = (mode >> LSM303DLH_M_MR_MD_BIT);
+	if (error < 0) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	if (mode == LSM303DLH_M_MODE_SLEEP) {
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+		disable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+
+		/*
+		 * No need to store context here, it is not like
+		 * suspend/resume but fall back to default values
+		 */
+		ddata->rate = LSM303DLH_M_RATE_00_75;
+		ddata->range = LSM303DLH_M_RANGE_1_3G;
+		ddata->range <<= LSM303DLH_M_CRB_GN_BIT;
+		ddata->range &= LSM303DLH_M_CRB_GN_MASK;
+		ddata->gain[ddata->pdata.axis_map_x] = XY_GAIN_1_3;
+		ddata->gain[ddata->pdata.axis_map_y] = XY_GAIN_1_3;
+		ddata->gain[ddata->pdata.axis_map_z] = Z_GAIN_1_3;
+
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static DEVICE_ATTR(gain, S_IRUGO, lsm303dlh_m_gain, NULL);
+
+static DEVICE_ATTR(data, S_IRUGO, lsm303dlh_m_values, NULL);
+
+static DEVICE_ATTR(mode, S_IWUGO | S_IRUGO,
+		lsm303dlh_m_show_mode, lsm303dlh_m_store_mode);
+
+static DEVICE_ATTR(range, S_IWUGO | S_IRUGO,
+		lsm303dlh_m_show_range, lsm303dlh_m_store_range);
+
+static DEVICE_ATTR(rate, S_IWUGO | S_IRUGO,
+		lsm303dlh_m_show_rate, lsm303dlh_m_store_rate);
+
+static struct attribute *lsm303dlh_m_attributes[] = {
+	&dev_attr_gain.attr,
+	&dev_attr_data.attr,
+	&dev_attr_mode.attr,
+	&dev_attr_range.attr,
+	&dev_attr_rate.attr,
+	NULL
+};
+
+static const struct attribute_group lsm303dlh_m_attr_group = {
+	.attrs = lsm303dlh_m_attributes,
+};
+
+static int __devinit lsm303dlh_m_probe(struct i2c_client *client,
+						const struct i2c_device_id *id)
+{
+	int ret;
+	struct lsm303dlh_m_data *ddata = NULL;
+	unsigned char version[3];
+
+	ddata = kzalloc(sizeof(struct lsm303dlh_m_data), GFP_KERNEL);
+	if (ddata == NULL) {
+		ret = -ENOMEM;
+		goto err_op_failed;
+	}
+
+	ddata->client = client;
+	i2c_set_clientdata(client, ddata);
+
+	/* copy platform specific data */
+	memcpy(&ddata->pdata, client->dev.platform_data, sizeof(ddata->pdata));
+
+	ddata->mode = LSM303DLH_M_MODE_SLEEP;
+	ddata->rate = LSM303DLH_M_RATE_00_75;
+	ddata->range = LSM303DLH_M_RANGE_1_3G;
+	ddata->range <<= LSM303DLH_M_CRB_GN_BIT;
+	ddata->range &= LSM303DLH_M_CRB_GN_MASK;
+	ddata->gain[ddata->pdata.axis_map_x] = XY_GAIN_1_3;
+	ddata->gain[ddata->pdata.axis_map_y] = XY_GAIN_1_3;
+	ddata->gain[ddata->pdata.axis_map_z] = Z_GAIN_1_3;
+	ddata->device_status = DEVICE_OFF;
+	dev_set_name(&client->dev, ddata->pdata.name_m);
+	ddata->regulator = regulator_get(&client->dev, "vdd");
+
+	if (IS_ERR(ddata->regulator)) {
+		dev_err(&client->dev, "failed to get regulator\n");
+		ret = PTR_ERR(ddata->regulator);
+		ddata->regulator = NULL;
+	}
+
+	if (ddata->regulator) {
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+	}
+
+	ret = lsm303dlh_m_read_multi(ddata, IRA_REG_M, 3, version, "IRA_REG_M");
+	if (ret < 0)
+		goto exit_free_regulator;
+
+	dev_info(&client->dev, "Magnetometer, ID : %x:%x:%x",
+			version[0], version[1], version[2]);
+
+	mutex_init(&ddata->lock);
+
+	ret = sysfs_create_group(&client->dev.kobj, &lsm303dlh_m_attr_group);
+	if (ret)
+		goto exit_free_regulator;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+
+	ddata->input_dev = input_allocate_device();
+	if (!ddata->input_dev) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "Failed to allocate input device\n");
+		goto exit_free_regulator;
+	}
+
+	set_bit(EV_ABS, ddata->input_dev->evbit);
+
+	/* x-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_X, -32768, 32767, 0, 0);
+	/* y-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_Y, -32768, 32767, 0, 0);
+	/* z-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_Z, -32768, 32767, 0, 0);
+
+	ddata->input_dev->name = "magnetometer";
+
+	ret = input_register_device(ddata->input_dev);
+	if (ret) {
+		dev_err(&client->dev, "Unable to register input device: %s\n",
+				ddata->input_dev->name);
+		goto err_input_register_failed;
+	}
+
+	/* register interrupt */
+	ret = request_threaded_irq(gpio_to_irq(ddata->pdata.irq_m), NULL,
+		   lsm303dlh_m_gpio_irq,
+		   IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING, "lsm303dlh_m",
+		   ddata);
+	if (ret) {
+		dev_err(&client->dev, "request irq EGPIO_PIN_1 failed\n");
+		goto err_input_failed;
+	}
+
+	disable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	ddata->early_suspend.level =
+				EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ddata->early_suspend.suspend = lsm303dlh_m_early_suspend;
+	ddata->early_suspend.resume = lsm303dlh_m_late_resume;
+	register_early_suspend(&ddata->early_suspend);
+#endif
+
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+
+	return ret;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+err_input_failed:
+	input_unregister_device(ddata->input_dev);
+err_input_register_failed:
+	input_free_device(ddata->input_dev);
+#endif
+exit_free_regulator:
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		regulator_put(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+err_op_failed:
+	dev_err(&client->dev, "lsm303dlh_m_probe failed %x", ret);
+	kfree(ddata);
+	return ret;
+}
+
+static int __devexit lsm303dlh_m_remove(struct i2c_client *client)
+{
+	struct lsm303dlh_m_data *ddata;
+
+	ddata = i2c_get_clientdata(client);
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	input_unregister_device(ddata->input_dev);
+	input_free_device(ddata->input_dev);
+#endif
+
+	sysfs_remove_group(&client->dev.kobj, &lsm303dlh_m_attr_group);
+
+	/* safer to make device off */
+	if (ddata->mode != LSM303DLH_M_MODE_SLEEP) {
+		lsm303dlh_m_set_mode(ddata, LSM303DLH_M_MODE_SLEEP);
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			regulator_put(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+
+	i2c_set_clientdata(client, NULL);
+	kfree(ddata);
+
+	return 0;
+}
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#ifdef CONFIG_PM
+static int lsm303dlh_m_suspend(struct device *dev)
+{
+	struct lsm303dlh_m_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret = lsm303dlh_m_do_suspend(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device");
+
+	return ret;
+}
+
+static int lsm303dlh_m_resume(struct device *dev)
+{
+	struct lsm303dlh_m_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret = lsm303dlh_m_restore(ddata);
+
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device");
+
+	return ret;
+}
+static const struct dev_pm_ops lsm303dlh_m_dev_pm_ops = {
+	.suspend = lsm303dlh_m_suspend,
+	.resume  = lsm303dlh_m_resume,
+};
+#endif
+#else
+static void lsm303dlh_m_early_suspend(struct early_suspend *data)
+{
+	struct lsm303dlh_m_data *ddata =
+		container_of(data, struct lsm303dlh_m_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_m_do_suspend(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device");
+}
+
+static void lsm303dlh_m_late_resume(struct early_suspend *data)
+{
+	struct lsm303dlh_m_data *ddata =
+		container_of(data, struct lsm303dlh_m_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_m_restore(ddata);
+
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"lsm303dlh_m late resume failed\n");
+}
+#endif /* CONFIG_PM */
+
+static const struct i2c_device_id lsm303dlh_m_id[] = {
+	{ "lsm303dlh_m", 0 },
+	{ },
+};
+
+static struct i2c_driver lsm303dlh_m_driver = {
+	.probe		= lsm303dlh_m_probe,
+	.remove		= lsm303dlh_m_remove,
+	.id_table	= lsm303dlh_m_id,
+	.driver = {
+		.name = "lsm303dlh_m",
+	#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+		.pm = &lsm303dlh_m_dev_pm_ops,
+	#endif
+	},
+};
+
+static int __init lsm303dlh_m_init(void)
+{
+	return i2c_add_driver(&lsm303dlh_m_driver);
+}
+
+static void __exit lsm303dlh_m_exit(void)
+{
+	i2c_del_driver(&lsm303dlh_m_driver);
+}
+
+module_init(lsm303dlh_m_init);
+module_exit(lsm303dlh_m_exit);
+
+MODULE_DESCRIPTION("lSM303DLH 3-Axis Magnetometer Driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("STMicroelectronics");
diff --git a/drivers/i2c/busses/i2c-nomadik.c b/drivers/i2c/busses/i2c-nomadik.c
index 0c731ca69f1..efbc3790d43 100644
--- a/drivers/i2c/busses/i2c-nomadik.c
+++ b/drivers/i2c/busses/i2c-nomadik.c
@@ -135,18 +135,19 @@ struct i2c_nmk_client {
 };
 
 /**
- * struct nmk_i2c_dev - private data structure of the controller
- * @pdev: parent platform device
- * @adap: corresponding I2C adapter
- * @irq: interrupt line for the controller
- * @virtbase: virtual io memory area
- * @clk: hardware i2c block clock
- * @cfg: machine provided controller configuration
- * @cli: holder of client specific data
- * @stop: stop condition
- * @xfer_complete: acknowledge completion for a I2C message
- * @result: controller propogated result
- * @busy: Busy doing transfer
+ * struct nmk_i2c_dev - private data structure of the controller.
+ * @pdev: parent platform device.
+ * @adap: corresponding I2C adapter.
+ * @irq: interrupt line for the controller.
+ * @virtbase: virtual io memory area.
+ * @clk: hardware i2c block clock.
+ * @cfg: machine provided controller configuration.
+ * @cli: holder of client specific data.
+ * @stop: stop condition.
+ * @xfer_complete: acknowledge completion for a I2C message.
+ * @result: controller propogated result.
+ * @regulator: pointer to i2c regulator.
+ * @busy: Busy doing transfer.
  */
 struct nmk_i2c_dev {
 	struct platform_device		*pdev;
@@ -417,19 +418,19 @@ static int read_i2c(struct nmk_i2c_dev *dev)
 	writel(readl(dev->virtbase + I2C_IMSCR) | irq_mask,
 			dev->virtbase + I2C_IMSCR);
 
-	timeout = wait_for_completion_interruptible_timeout(
+	timeout = wait_for_completion_timeout(
 		&dev->xfer_complete, dev->adap.timeout);
 
 	if (timeout < 0) {
 		dev_err(&dev->pdev->dev,
-			"wait_for_completion_interruptible_timeout"
+			"wait_for_completion_timeout "
 			"returned %d waiting for event\n", timeout);
 		status = timeout;
 	}
 
 	if (timeout == 0) {
 		/* Controller timed out */
-		dev_err(&dev->pdev->dev, "read from slave 0x%x timed out\n",
+		dev_err(&dev->pdev->dev, "Read from Slave 0x%x timed out\n",
 				dev->cli.slave_adr);
 		status = -ETIMEDOUT;
 	}
@@ -504,19 +505,19 @@ static int write_i2c(struct nmk_i2c_dev *dev)
 	writel(readl(dev->virtbase + I2C_IMSCR) | irq_mask,
 			dev->virtbase + I2C_IMSCR);
 
-	timeout = wait_for_completion_interruptible_timeout(
+	timeout = wait_for_completion_timeout(
 		&dev->xfer_complete, dev->adap.timeout);
 
 	if (timeout < 0) {
 		dev_err(&dev->pdev->dev,
-			"wait_for_completion_interruptible_timeout"
+			"wait_for_completion_timeout "
 			"returned %d waiting for event\n", timeout);
 		status = timeout;
 	}
 
 	if (timeout == 0) {
 		/* Controller timed out */
-		dev_err(&dev->pdev->dev, "write to slave 0x%x timed out\n",
+		dev_err(&dev->pdev->dev, "Write to slave 0x%x timed out\n",
 				dev->cli.slave_adr);
 		status = -ETIMEDOUT;
 	}
@@ -632,6 +633,8 @@ static int nmk_i2c_xfer(struct i2c_adapter *i2c_adap,
 
 	clk_enable(dev->clk);
 
+	dev->busy = true;
+
 	status = init_hw(dev);
 	if (status)
 		goto out;
@@ -681,7 +684,7 @@ out:
 /**
  * disable_interrupts() - disable the interrupts
  * @dev: private data of controller
- * @irq: interrupt number
+ * @irq: interrupt number.
  */
 static int disable_interrupts(struct nmk_i2c_dev *dev, u32 irq)
 {
@@ -1043,6 +1046,7 @@ static struct platform_driver nmk_i2c_driver = {
 	},
 	.probe = nmk_i2c_probe,
 	.remove = __devexit_p(nmk_i2c_remove),
+	.suspend = nmk_i2c_suspend,
 };
 
 static int __init nmk_i2c_init(void)
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index b4dee9d5a05..ba0ae47b275 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -151,6 +151,16 @@ config KEYBOARD_BFIN
 	  To compile this driver as a module, choose M here: the
 	  module will be called bf54x-keys.
 
+config KEYBOARD_DB5500
+	tristate "DB5500 keyboard"
+	depends on UX500_SOC_DB5500
+	help
+	  Say Y here to enable the on-chip keypad controller on the
+	  ST-Ericsson U5500 platform.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called db5500_keypad.
+
 config KEYBOARD_LKKBD
 	tristate "DECstation/VAXstation LK201/LK401 keyboard"
 	select SERIO
@@ -365,7 +375,7 @@ config KEYBOARD_NEWTON
 	  To compile this driver as a module, choose M here: the
 	  module will be called newtonkbd.
 
-config KEYBOARD_NOMADIK
+config KEYBOARD_NOMADIK_SKE
 	tristate "ST-Ericsson Nomadik SKE keyboard"
 	depends on PLAT_NOMADIK
 	help
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index ddde0fd476f..e675356acb2 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_KEYBOARD_ATARI)		+= atakbd.o
 obj-$(CONFIG_KEYBOARD_ATKBD)		+= atkbd.o
 obj-$(CONFIG_KEYBOARD_BFIN)		+= bf54x-keys.o
 obj-$(CONFIG_KEYBOARD_DAVINCI)		+= davinci_keyscan.o
+obj-$(CONFIG_KEYBOARD_DB5500)		+= db5500_keypad.o
 obj-$(CONFIG_KEYBOARD_EP93XX)		+= ep93xx_keypad.o
 obj-$(CONFIG_KEYBOARD_GPIO)		+= gpio_keys.o
 obj-$(CONFIG_KEYBOARD_GPIO_POLLED)	+= gpio_keys_polled.o
@@ -30,7 +31,7 @@ obj-$(CONFIG_KEYBOARD_MAX7359)		+= max7359_keypad.o
 obj-$(CONFIG_KEYBOARD_MCS)		+= mcs_touchkey.o
 obj-$(CONFIG_KEYBOARD_MPR121)		+= mpr121_touchkey.o
 obj-$(CONFIG_KEYBOARD_NEWTON)		+= newtonkbd.o
-obj-$(CONFIG_KEYBOARD_NOMADIK)		+= nomadik-ske-keypad.o
+obj-$(CONFIG_KEYBOARD_NOMADIK_SKE)	+= nomadik-ske-keypad.o
 obj-$(CONFIG_KEYBOARD_OMAP)		+= omap-keypad.o
 obj-$(CONFIG_KEYBOARD_OMAP4)		+= omap4-keypad.o
 obj-$(CONFIG_KEYBOARD_OPENCORES)	+= opencores-kbd.o
diff --git a/drivers/input/keyboard/db5500_keypad.c b/drivers/input/keyboard/db5500_keypad.c
new file mode 100644
index 00000000000..a53d72e8ab4
--- /dev/null
+++ b/drivers/input/keyboard/db5500_keypad.c
@@ -0,0 +1,528 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License terms: GNU General Public License, version 2
+ * Author: Sundar Iyer <sundar.iyer@stericsson.com> for ST-Ericsson
+ */
+
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/io.h>
+#include <linux/input.h>
+#include <linux/input/matrix_keypad.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <mach/db5500-keypad.h>
+
+#define KEYPAD_CTR		0x0
+#define KEYPAD_IRQ_CLEAR	0x4
+#define KEYPAD_INT_ENABLE	0x8
+#define KEYPAD_INT_STATUS	0xC
+#define KEYPAD_ARRAY_01		0x18
+
+#define KEYPAD_NUM_ARRAY_REGS	5
+
+#define KEYPAD_CTR_WRITE_IRQ_ENABLE	(1 << 10)
+#define KEYPAD_CTR_WRITE_CONTROL	(1 << 8)
+#define KEYPAD_CTR_SCAN_ENABLE		(1 << 7)
+
+#define KEYPAD_ARRAY_CHANGEBIT		(1 << 15)
+
+#define KEYPAD_DEBOUNCE_PERIOD_MIN	5	/* ms */
+#define KEYPAD_DEBOUNCE_PERIOD_MAX	80	/* ms */
+
+#define KEYPAD_GND_ROW		8
+
+#define KEYPAD_MAX_ROWS		9
+#define KEYPAD_MAX_COLS		8
+#define KEYPAD_ROW_SHIFT	3
+#define KEYPAD_KEYMAP_SIZE	\
+	(KEYPAD_MAX_ROWS * KEYPAD_MAX_COLS)
+
+/**
+ * struct db5500_keypad  - data structure used by keypad driver
+ * @irq:	irq number
+ * @base:	keypad registers base address
+ * @input:	pointer to input device object
+ * @board:	keypad platform data
+ * @keymap:	matrix scan code table for keycodes
+ * @clk:	clock structure pointer
+ * @previous_set: previous set of registers
+ */
+struct db5500_keypad {
+	int irq;
+	void __iomem *base;
+	struct input_dev *input;
+	const struct db5500_keypad_platform_data *board;
+	unsigned short keymap[KEYPAD_KEYMAP_SIZE];
+	struct clk *clk;
+	u8 previous_set[KEYPAD_MAX_ROWS];
+};
+
+/**
+ * db5500_keypad_report() - reports the keypad event
+ * @keypad: pointer to device structure
+ * @row: row value of keypad
+ * @curr: current event
+ * @previous: previous event
+ *
+ * This function uses to reports the event of the keypad
+ * and returns NONE.
+ *
+ * By default all column reads are 1111 1111b.  Any press will pull the column
+ * down, leading to a 0 in any of these locations.  We invert these values so
+ * that a 1 means means "column pressed". *
+ * If curr changes from the previous from 0 to 1, we report it as a key press.
+ * If curr changes from the previous from 1 to 0, we report it as a key
+ * release.
+ */
+static void db5500_keypad_report(struct db5500_keypad *keypad, int row,
+				 u8 curr, u8 previous)
+{
+	struct input_dev *input = keypad->input;
+	u8 changed  = curr ^ previous;
+
+	while (changed) {
+		int col = __ffs(changed);
+		bool press = curr & BIT(col);
+		int code = MATRIX_SCAN_CODE(row, col, KEYPAD_ROW_SHIFT);
+
+		input_event(input, EV_MSC, MSC_SCAN, code);
+		input_report_key(input, keypad->keymap[code], press);
+		input_sync(input);
+
+		changed &= ~BIT(col);
+	}
+}
+
+/**
+ * db5500_keypad_irq() - irq handler for keypad
+ * @irq: irq value for keypad
+ * @dev_id: pointer for device id
+ *
+ * This function uses to handle the interrupt of the keypad
+ * and returns irqreturn.
+ */
+static irqreturn_t db5500_keypad_irq(int irq, void *dev_id)
+{
+	struct db5500_keypad *keypad = dev_id;
+	u8 current_set[ARRAY_SIZE(keypad->previous_set)];
+	int tries = 100;
+	bool changebit;
+	u32 data_reg;
+	u8 allrows;
+	u8 common;
+	int i;
+
+	writel(0x1, keypad->base + KEYPAD_IRQ_CLEAR);
+
+again:
+	if (!tries--) {
+		dev_warn(&keypad->input->dev, "values failed to stabilize\n");
+		return IRQ_HANDLED;
+	}
+
+	changebit = readl(keypad->base + KEYPAD_ARRAY_01)
+		    & KEYPAD_ARRAY_CHANGEBIT;
+
+	for (i = 0; i < KEYPAD_NUM_ARRAY_REGS; i++) {
+		data_reg = readl(keypad->base + KEYPAD_ARRAY_01 + 4 * i);
+
+		/* If the change bit changed, we need to reread the data */
+		if (changebit != !!(data_reg & KEYPAD_ARRAY_CHANGEBIT))
+			goto again;
+
+		current_set[2 * i] = ~(data_reg & 0xff);
+
+		/* Last array reg has only one valid set of columns */
+		if (i != KEYPAD_NUM_ARRAY_REGS - 1)
+			current_set[2 * i + 1] = ~((data_reg & 0xff0000) >> 16);
+	}
+
+	allrows = current_set[KEYPAD_GND_ROW];
+
+	/*
+	 * Sometimes during a GND row release, an incorrect report is received
+	 * where the ARRAY8 all rows setting does not match the other ARRAY*
+	 * rows.  Ignore this report; the correct one has been observed to
+	 * follow it.
+	 */
+	common = 0xff;
+	for (i = 0; i < KEYPAD_GND_ROW; i++)
+		common &= current_set[i];
+
+	if ((allrows & common) != common)
+		return IRQ_HANDLED;
+
+	for (i = 0; i < ARRAY_SIZE(current_set); i++) {
+		/*
+		 * If there is an allrows press (GND row), we need to ignore
+		 * the allrows values from the reset of the ARRAYs.
+		 */
+		if (i < KEYPAD_GND_ROW && allrows)
+			current_set[i] &= ~allrows;
+
+		if (keypad->previous_set[i] == current_set[i])
+			continue;
+
+		db5500_keypad_report(keypad, i, current_set[i],
+				     keypad->previous_set[i]);
+	}
+
+	/* update the reference set of array registers */
+	memcpy(keypad->previous_set, current_set, sizeof(keypad->previous_set));
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * db5500_keypad_writel() - write into keypad registers
+ * @keypad: pointer to device structure
+ * @val: value to write into register
+ * @reg: register offset
+ *
+ * This function uses to write into the keypad registers
+ * and returns NONE.
+ */
+static void db5500_keypad_writel(struct db5500_keypad *keypad, u32 val, u32 reg)
+{
+	int timeout = 4;
+	int allowedbit;
+
+	switch (reg) {
+	case KEYPAD_CTR:
+		allowedbit = KEYPAD_CTR_WRITE_CONTROL;
+		break;
+	case KEYPAD_INT_ENABLE:
+		allowedbit = KEYPAD_CTR_WRITE_IRQ_ENABLE;
+		break;
+	default:
+		BUG();
+	}
+
+	do {
+		u32 ctr = readl(keypad->base + KEYPAD_CTR);
+
+		if (ctr & allowedbit)
+			break;
+
+		udelay(50);
+	} while (--timeout);
+
+	/* Five 32k clk cycles (~150us) required, we waited 200us */
+	WARN_ON(!timeout);
+
+	writel(val, keypad->base + reg);
+}
+
+/**
+ * db5500_keypad_chip_init() - initialize the keypad chip
+ * @keypad: pointer to device structure
+ *
+ * This function uses to initializes the keypad controller
+ * and returns integer.
+ */
+static int db5500_keypad_chip_init(struct db5500_keypad *keypad)
+{
+	int debounce = keypad->board->debounce_ms;
+	int debounce_hits = 0;
+
+	if (debounce < KEYPAD_DEBOUNCE_PERIOD_MIN)
+		debounce = KEYPAD_DEBOUNCE_PERIOD_MIN;
+
+	if (debounce > KEYPAD_DEBOUNCE_PERIOD_MAX) {
+		debounce_hits = DIV_ROUND_UP(debounce,
+					     KEYPAD_DEBOUNCE_PERIOD_MAX) - 1;
+		debounce = KEYPAD_DEBOUNCE_PERIOD_MAX;
+	}
+
+	/* Convert the milliseconds to the bit mask */
+	debounce = DIV_ROUND_UP(debounce, KEYPAD_DEBOUNCE_PERIOD_MIN) - 1;
+
+	clk_enable(keypad->clk);
+
+	db5500_keypad_writel(keypad,
+			     KEYPAD_CTR_SCAN_ENABLE
+			     | ((debounce_hits & 0x7) << 4)
+			     | debounce,
+			     KEYPAD_CTR);
+
+	db5500_keypad_writel(keypad, 0x1, KEYPAD_INT_ENABLE);
+
+	return 0;
+}
+
+/**
+ * db5500_keypad_close() - stops the keypad driver
+ * @keypad: pointer to device structure
+ *
+ * This function uses to stop the keypad
+ * driver and returns integer.
+ */
+static void db5500_keypad_close(struct db5500_keypad *keypad)
+{
+	db5500_keypad_writel(keypad, 0, KEYPAD_CTR);
+	db5500_keypad_writel(keypad, 0, KEYPAD_INT_ENABLE);
+
+	clk_disable(keypad->clk);
+}
+
+/**
+ * db5500_keypad_probe() - Initialze the the keypad driver
+ * @pdev: pointer to platform device structure
+ *
+ * This function will allocate and initialize the instance
+ * data and request the irq and register to input subsystem driver.
+ */
+static int __devinit db5500_keypad_probe(struct platform_device *pdev)
+{
+	const struct db5500_keypad_platform_data *plat;
+	struct db5500_keypad *keypad;
+	struct resource *res;
+	struct input_dev *input;
+	void __iomem *base;
+	struct clk *clk;
+	int ret;
+	int irq;
+
+	plat = pdev->dev.platform_data;
+	if (!plat) {
+		dev_err(&pdev->dev, "invalid keypad platform data\n");
+		ret = -EINVAL;
+		goto out_ret;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to get keypad irq\n");
+		ret = -EINVAL;
+		goto out_ret;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "missing platform resources\n");
+		ret = -EINVAL;
+		goto out_ret;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (!res) {
+		dev_err(&pdev->dev, "failed to request I/O memory\n");
+		ret = -EBUSY;
+		goto out_ret;
+	}
+
+	base = ioremap(res->start, resource_size(res));
+	if (!base) {
+		dev_err(&pdev->dev, "failed to remap I/O memory\n");
+		ret = -ENXIO;
+		goto out_freerequest_memregions;
+	}
+
+	clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "failed to clk_get\n");
+		ret = PTR_ERR(clk);
+		goto out_iounmap;
+	}
+
+	keypad = kzalloc(sizeof(struct db5500_keypad), GFP_KERNEL);
+	if (!keypad) {
+		dev_err(&pdev->dev, "failed to allocate keypad memory\n");
+		ret = -ENOMEM;
+		goto out_freeclk;
+	}
+
+	input = input_allocate_device();
+	if (!input) {
+		dev_err(&pdev->dev, "failed to input_allocate_device\n");
+		ret = -ENOMEM;
+		goto out_freekeypad;
+	}
+
+	input->id.bustype = BUS_HOST;
+	input->name = "db5500-keypad";
+	input->dev.parent = &pdev->dev;
+
+	input->keycode = keypad->keymap;
+	input->keycodesize = sizeof(keypad->keymap[0]);
+	input->keycodemax = ARRAY_SIZE(keypad->keymap);
+
+	input_set_capability(input, EV_MSC, MSC_SCAN);
+
+	__set_bit(EV_KEY, input->evbit);
+	if (!plat->no_autorepeat)
+		__set_bit(EV_REP, input->evbit);
+
+	matrix_keypad_build_keymap(plat->keymap_data, KEYPAD_ROW_SHIFT,
+				   input->keycode, input->keybit);
+
+	ret = input_register_device(input);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"unable to register input device: %d\n", ret);
+		goto out_freeinput;
+	}
+
+	keypad->irq	= irq;
+	keypad->board	= plat;
+	keypad->input	= input;
+	keypad->base	= base;
+	keypad->clk	= clk;
+
+	ret = db5500_keypad_chip_init(keypad);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "unable to init keypad hardware\n");
+		goto out_unregisterinput;
+	}
+
+	ret = request_threaded_irq(keypad->irq, NULL, db5500_keypad_irq,
+				   IRQF_ONESHOT, "db5500-keypad", keypad);
+	if (ret) {
+		dev_err(&pdev->dev, "allocate irq %d failed\n", keypad->irq);
+		goto out_unregisterinput;
+	}
+
+	platform_set_drvdata(pdev, keypad);
+
+	return 0;
+
+out_unregisterinput:
+	input_unregister_device(input);
+	input = NULL;
+	clk_disable(keypad->clk);
+out_freeinput:
+	input_free_device(input);
+out_freekeypad:
+	kfree(keypad);
+out_freeclk:
+	clk_put(clk);
+out_iounmap:
+	iounmap(base);
+out_freerequest_memregions:
+	release_mem_region(res->start, resource_size(res));
+out_ret:
+	return ret;
+}
+
+/**
+ * db5500_keypad_remove() - Removes the keypad driver
+ * @pdev: pointer to platform device structure
+ *
+ * This function uses to remove the keypad
+ * driver and returns integer.
+ */
+static int __devexit db5500_keypad_remove(struct platform_device *pdev)
+{
+	struct db5500_keypad *keypad = platform_get_drvdata(pdev);
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	free_irq(keypad->irq, keypad);
+	input_unregister_device(keypad->input);
+
+	clk_disable(keypad->clk);
+	clk_put(keypad->clk);
+
+	iounmap(keypad->base);
+	release_mem_region(res->start, resource_size(res));
+	kfree(keypad);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/**
+ * db5500_keypad_suspend() - suspend the keypad controller
+ * @dev: pointer to device structure
+ *
+ * This function is used to suspend the
+ * keypad controller and returns integer
+ */
+static int db5500_keypad_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct db5500_keypad *keypad = platform_get_drvdata(pdev);
+	int irq = platform_get_irq(pdev, 0);
+
+	if (device_may_wakeup(dev))
+		enable_irq_wake(irq);
+	else {
+		disable_irq(irq);
+		db5500_keypad_close(keypad);
+	}
+
+	return 0;
+}
+
+/**
+ * db5500_keypad_resume() - resume the keypad controller
+ * @dev: pointer to device structure
+ *
+ * This function is used to resume the keypad
+ * controller and returns integer.
+ */
+static int db5500_keypad_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct db5500_keypad *keypad = platform_get_drvdata(pdev);
+	int irq = platform_get_irq(pdev, 0);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(irq);
+	else {
+		db5500_keypad_chip_init(keypad);
+		enable_irq(irq);
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops db5500_keypad_dev_pm_ops = {
+	.suspend = db5500_keypad_suspend,
+	.resume = db5500_keypad_resume,
+};
+#endif
+
+static struct platform_driver db5500_keypad_driver = {
+	.driver = {
+		.name	= "db5500-keypad",
+		.owner	= THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm	= &db5500_keypad_dev_pm_ops,
+#endif
+	},
+	.probe	= db5500_keypad_probe,
+	.remove	= __devexit_p(db5500_keypad_remove),
+};
+
+/**
+ * db5500_keypad_init() - Initialize the keypad driver
+ *
+ * This function uses to initializes the db5500
+ * keypad driver and returns integer.
+ */
+static int __init db5500_keypad_init(void)
+{
+	return platform_driver_register(&db5500_keypad_driver);
+}
+module_init(db5500_keypad_init);
+
+/**
+ * db5500_keypad_exit() - De-initialize the keypad driver
+ *
+ * This function uses to de-initialize the db5500
+ * keypad driver and returns none.
+ */
+static void __exit db5500_keypad_exit(void)
+{
+	platform_driver_unregister(&db5500_keypad_driver);
+}
+module_exit(db5500_keypad_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Sundar Iyer <sundar.iyer@stericsson.com>");
+MODULE_DESCRIPTION("DB5500 Keypad Driver");
+MODULE_ALIAS("platform:db5500-keypad");
diff --git a/drivers/input/keyboard/nomadik-ske-keypad.c b/drivers/input/keyboard/nomadik-ske-keypad.c
index 6e0f2309136..fe7243f0463 100644
--- a/drivers/input/keyboard/nomadik-ske-keypad.c
+++ b/drivers/input/keyboard/nomadik-ske-keypad.c
@@ -12,14 +12,17 @@
 
 #include <linux/platform_device.h>
 #include <linux/interrupt.h>
+#include <linux/workqueue.h>
 #include <linux/spinlock.h>
 #include <linux/io.h>
 #include <linux/delay.h>
 #include <linux/input.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
+#include <linux/regulator/consumer.h>
 
 #include <plat/ske.h>
+#include <linux/gpio/nomadik.h>
 
 /* SKE_CR bits */
 #define SKE_KPMLT	(0x1 << 6)
@@ -47,6 +50,7 @@
 #define SKE_ASR3	0x2C
 
 #define SKE_NUM_ASRX_REGISTERS	(4)
+#define	KEY_PRESSED_DELAY	10
 
 /**
  * struct ske_keypad  - data structure used by keypad driver
@@ -56,6 +60,16 @@
  * @board:	keypad platform device
  * @keymap:	matrix scan code table for keycodes
  * @clk:	clock structure pointer
+ * @enable:	flag to enable the driver event
+ * @regulator:	pointer to the regulator used for ske kyepad
+ * @gpio_input_irq: array for gpio irqs
+ * @key_pressed: hold the key state
+ * @work: delayed work variable for gpio switch
+ * @ske_rows: rows gpio array for ske
+ * @ske_cols: columns gpio array for ske
+ * @gpio_row: gpio row
+ * @gpio_col: gpio column
+ * @gpio_work: delayed work variable for release gpio key
  */
 struct ske_keypad {
 	int irq;
@@ -65,6 +79,16 @@ struct ske_keypad {
 	unsigned short keymap[SKE_KPD_KEYMAP_SIZE];
 	struct clk *clk;
 	spinlock_t ske_keypad_lock;
+	bool enable;
+	struct regulator *regulator;
+	int gpio_input_irq[SKE_KPD_MAX_ROWS];
+	int key_pressed;
+	struct delayed_work work;
+	int ske_rows[SKE_KPD_MAX_ROWS];
+	int ske_cols[SKE_KPD_MAX_COLS];
+	int gpio_row;
+	int gpio_col;
+	struct delayed_work gpio_work;
 };
 
 static void ske_keypad_set_bits(struct ske_keypad *keypad, u16 addr,
@@ -90,7 +114,7 @@ static void ske_keypad_set_bits(struct ske_keypad *keypad, u16 addr,
 static int __devinit ske_keypad_chip_init(struct ske_keypad *keypad)
 {
 	u32 value;
-	int timeout = 50;
+	int timeout = keypad->board->debounce_ms;
 
 	/* check SKE_RIS to be 0 */
 	while ((readl(keypad->reg_base + SKE_RIS) != 0x00000000) && timeout--)
@@ -133,12 +157,115 @@ static int __devinit ske_keypad_chip_init(struct ske_keypad *keypad)
 	return 0;
 }
 
-static void ske_keypad_read_data(struct ske_keypad *keypad)
+static void ske_mode_enable(struct ske_keypad *keypad, bool enable)
+{
+	int i;
+
+	if (!enable) {
+		writel(0, keypad->reg_base + SKE_CR);
+		if (keypad->board->exit)
+			keypad->board->exit();
+		for (i = 0; i < keypad->board->krow; i++) {
+			enable_irq(keypad->gpio_input_irq[i]);
+			enable_irq_wake(keypad->gpio_input_irq[i]);
+		}
+		clk_disable(keypad->clk);
+		regulator_disable(keypad->regulator);
+	} else {
+		regulator_enable(keypad->regulator);
+		clk_enable(keypad->clk);
+		for (i = 0; i < keypad->board->krow; i++) {
+			disable_irq_nosync(keypad->gpio_input_irq[i]);
+			disable_irq_wake(keypad->gpio_input_irq[i]);
+		}
+		if (keypad->board->init)
+			keypad->board->init();
+		ske_keypad_chip_init(keypad);
+	}
+}
+static void ske_enable(struct ske_keypad *keypad, bool enable)
+{
+	keypad->enable = enable;
+	if (keypad->enable) {
+		enable_irq(keypad->irq);
+		ske_mode_enable(keypad, true);
+	} else {
+		ske_mode_enable(keypad, false);
+		disable_irq(keypad->irq);
+	}
+}
+
+static ssize_t ske_show_attr_enable(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct ske_keypad *keypad = platform_get_drvdata(pdev);
+	return sprintf(buf, "%d\n", keypad->enable);
+}
+
+static ssize_t ske_store_attr_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct ske_keypad *keypad = platform_get_drvdata(pdev);
+	unsigned long val;
+
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if ((val != 0) && (val != 1))
+		return -EINVAL;
+
+	if (keypad->enable != val) {
+		keypad->enable = val ? true : false;
+		ske_enable(keypad, keypad->enable);
+	}
+	return count;
+}
+
+static DEVICE_ATTR(enable, S_IWUSR | S_IRUGO,
+	ske_show_attr_enable, ske_store_attr_enable);
+
+static struct attribute *ske_keypad_attrs[] = {
+	&dev_attr_enable.attr,
+	NULL,
+};
+
+static struct attribute_group ske_attr_group = {
+	.attrs = ske_keypad_attrs,
+};
+
+static void ske_keypad_report(struct ske_keypad *keypad, u8 status, int col)
 {
+	int row = 0, code, pos;
 	struct input_dev *input = keypad->input;
-	u16 status;
-	int col = 0, row = 0, code;
-	int ske_asr, ske_ris, key_pressed, i;
+	u32 ske_ris;
+	int num_of_rows;
+
+	/* find out the row */
+	num_of_rows = hweight8(status);
+	do {
+		pos = __ffs(status);
+		row = pos;
+		status &= ~(1 << pos);
+
+		code = MATRIX_SCAN_CODE(row, col, SKE_KEYPAD_ROW_SHIFT);
+		ske_ris = readl(keypad->reg_base + SKE_RIS);
+		keypad->key_pressed = ske_ris & SKE_KPRISA;
+
+		input_event(input, EV_MSC, MSC_SCAN, code);
+		input_report_key(input, keypad->keymap[code],
+						keypad->key_pressed);
+		input_sync(input);
+		num_of_rows--;
+	} while (num_of_rows);
+}
+
+static void ske_keypad_read_data(struct ske_keypad *keypad)
+{
+	u8 status;
+	int col = 0;
+	int ske_asr, i;
 
 	/*
 	 * Read the auto scan registers
@@ -152,59 +279,187 @@ static void ske_keypad_read_data(struct ske_keypad *keypad)
 		if (!ske_asr)
 			continue;
 
-		/* now that ASRx is zero, find out the column x and row y*/
-		if (ske_asr & 0xff) {
+		/* now that ASRx is zero, find out the coloumn x and row y */
+		status = ske_asr & 0xff;
+		if (status) {
 			col = i * 2;
-			status = ske_asr & 0xff;
-		} else {
+			ske_keypad_report(keypad, status, col);
+		}
+		status = (ske_asr & 0xff00) >> 8;
+		if (status) {
 			col = (i * 2) + 1;
-			status = (ske_asr & 0xff00) >> 8;
+			ske_keypad_report(keypad, status, col);
 		}
-
-		/* find out the row */
-		row = __ffs(status);
-
-		code = MATRIX_SCAN_CODE(row, col, SKE_KEYPAD_ROW_SHIFT);
-		ske_ris = readl(keypad->reg_base + SKE_RIS);
-		key_pressed = ske_ris & SKE_KPRISA;
-
-		input_event(input, EV_MSC, MSC_SCAN, code);
-		input_report_key(input, keypad->keymap[code], key_pressed);
-		input_sync(input);
 	}
 }
-
-static irqreturn_t ske_keypad_irq(int irq, void *dev_id)
+static void ske_keypad_scan(struct ske_keypad *keypad)
 {
-	struct ske_keypad *keypad = dev_id;
-	int retries = 20;
+	int timeout = keypad->board->debounce_ms;
 
 	/* disable auto scan interrupt; mask the interrupt generated */
 	ske_keypad_set_bits(keypad, SKE_IMSC, ~SKE_KPIMA, 0x0);
 	ske_keypad_set_bits(keypad, SKE_ICR, 0x0, SKE_KPICA);
 
-	while ((readl(keypad->reg_base + SKE_CR) & SKE_KPASON) && --retries)
-		msleep(5);
+	while ((readl(keypad->reg_base + SKE_CR) & SKE_KPASON) && --timeout)
+		cpu_relax();
 
-	if (retries) {
-		/* SKEx registers are stable and can be read */
-		ske_keypad_read_data(keypad);
-	}
+	/* SKEx registers are stable and can be read */
+	ske_keypad_read_data(keypad);
+
+	/* wait until raw interrupt is clear */
+	while ((readl(keypad->reg_base + SKE_RIS)) && --timeout)
+		msleep(KEY_PRESSED_DELAY);
 
 	/* enable auto scan interrupts */
 	ske_keypad_set_bits(keypad, SKE_IMSC, 0x0, SKE_KPIMA);
+}
+
+static void ske_gpio_switch_work(struct work_struct *work)
+{
+	struct ske_keypad *keypad = container_of(work,
+					struct ske_keypad, work.work);
+
+	ske_mode_enable(keypad, false);
+	keypad->enable = false;
+}
+
+static void ske_gpio_release_work(struct work_struct *work)
+{
+	int code;
+	struct ske_keypad *keypad = container_of(work,
+					struct ske_keypad, gpio_work.work);
+	struct input_dev *input = keypad->input;
+
+	code = MATRIX_SCAN_CODE(keypad->gpio_row, keypad->gpio_col,
+						SKE_KEYPAD_ROW_SHIFT);
+	input_event(input, EV_MSC, MSC_SCAN, code);
+	input_report_key(input, keypad->keymap[code], 1);
+	input_sync(input);
+	input_report_key(input, keypad->keymap[code], 0);
+	input_sync(input);
+}
+
+static int ske_read_get_gpio_row(struct ske_keypad *keypad)
+{
+	int row;
+	int value = 0;
+	int ret;
+
+	/* read all rows GPIO data register values */
+	for (row = 0; row < SKE_KPD_MAX_ROWS ; row++) {
+		ret  = gpio_get_value(keypad->ske_rows[row]);
+		value += (1 << row) *  ret;
+	}
+
+	/* get the exact row */
+	for (row = 0; row < keypad->board->krow; row++) {
+		if (((1 << row) & value) == 0)
+			return row;
+	}
+
+	return -1;
+}
+
+static void ske_set_cols(struct ske_keypad *keypad, int col)
+{
+	int i ;
+	int value;
+
+	/*
+	 * Set all columns except the requested column
+	 * output pin as high
+	 */
+	for (i = 0; i < SKE_KPD_MAX_COLS; i++) {
+		if (i == col)
+			value = 0;
+		else
+			value = 1;
+		gpio_request(keypad->ske_cols[i], "ske-kp");
+		gpio_direction_output(keypad->ske_cols[i], value);
+		gpio_free(keypad->ske_cols[i]);
+	}
+}
+
+static void ske_free_cols(struct ske_keypad *keypad)
+{
+	int i ;
+
+	for (i = 0; i < SKE_KPD_MAX_COLS; i++) {
+		gpio_request(keypad->ske_cols[i], "ske-kp");
+		gpio_direction_output(keypad->ske_cols[i], 0);
+		gpio_free(keypad->ske_cols[i]);
+	}
+}
+
+static void ske_manual_scan(struct ske_keypad *keypad)
+{
+	int row;
+	int col;
+
+	for (col = 0; col < keypad->board->kcol; col++) {
+		ske_set_cols(keypad, col);
+		row = ske_read_get_gpio_row(keypad);
+		if (row >= 0) {
+			keypad->key_pressed = 1;
+			keypad->gpio_row = row;
+			keypad->gpio_col = col;
+			break;
+		}
+	}
+	ske_free_cols(keypad);
+}
+
+static irqreturn_t ske_keypad_gpio_irq(int irq, void *dev_id)
+{
+	struct ske_keypad *keypad = dev_id;
+
+	if (!gpio_get_value(NOMADIK_IRQ_TO_GPIO(irq))) {
+		ske_manual_scan(keypad);
+		if (!keypad->enable) {
+			keypad->enable = true;
+			ske_mode_enable(keypad, true);
+		}
+		/*
+		 * Schedule the work queue to change it to
+		 * report the key pressed, if it is not detected in SKE mode.
+		 */
+		if (keypad->key_pressed)
+			schedule_delayed_work(&keypad->gpio_work,
+						KEY_PRESSED_DELAY);
+	}
+
+	return IRQ_HANDLED;
+}
+static irqreturn_t ske_keypad_irq(int irq, void *dev_id)
+{
+	struct ske_keypad *keypad = dev_id;
+
+	cancel_delayed_work_sync(&keypad->gpio_work);
+	cancel_delayed_work_sync(&keypad->work);
+	ske_keypad_scan(keypad);
+
+	/*
+	 * Schedule the work queue to change it to
+	 * GPIO mode, if there is no activity in SKE mode
+	 */
+	if (!keypad->key_pressed && keypad->enable)
+		schedule_delayed_work(&keypad->work,
+				keypad->board->switch_delay);
 
 	return IRQ_HANDLED;
 }
 
 static int __devinit ske_keypad_probe(struct platform_device *pdev)
 {
-	const struct ske_keypad_platform_data *plat = pdev->dev.platform_data;
 	struct ske_keypad *keypad;
+	struct resource *res = NULL;
 	struct input_dev *input;
-	struct resource *res;
+	struct clk *clk;
+	void __iomem *reg_base;
+	int ret = 0;
 	int irq;
-	int error;
+	int i;
+	struct ske_keypad_platform_data *plat = pdev->dev.platform_data;
 
 	if (!plat) {
 		dev_err(&pdev->dev, "invalid keypad platform data\n");
@@ -218,42 +473,55 @@ static int __devinit ske_keypad_probe(struct platform_device *pdev)
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
+	if (res == NULL) {
 		dev_err(&pdev->dev, "missing platform resources\n");
-		return -EINVAL;
+		return -ENXIO;
 	}
 
-	keypad = kzalloc(sizeof(struct ske_keypad), GFP_KERNEL);
-	input = input_allocate_device();
-	if (!keypad || !input) {
-		dev_err(&pdev->dev, "failed to allocate keypad memory\n");
-		error = -ENOMEM;
-		goto err_free_mem;
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (!res) {
+		dev_err(&pdev->dev, "failed to request I/O memory\n");
+		return -EBUSY;
 	}
 
-	keypad->irq = irq;
-	keypad->board = plat;
-	keypad->input = input;
-	spin_lock_init(&keypad->ske_keypad_lock);
+	reg_base = ioremap(res->start, resource_size(res));
+	if (!reg_base) {
+		dev_err(&pdev->dev, "failed to remap I/O memory\n");
+		ret = -ENXIO;
+		goto out_freerequest_memregions;
+	}
 
-	if (!request_mem_region(res->start, resource_size(res), pdev->name)) {
-		dev_err(&pdev->dev, "failed to request I/O memory\n");
-		error = -EBUSY;
-		goto err_free_mem;
+	clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "failed to clk_get\n");
+		ret = PTR_ERR(clk);
+		goto out_freeioremap;
 	}
 
-	keypad->reg_base = ioremap(res->start, resource_size(res));
-	if (!keypad->reg_base) {
-		dev_err(&pdev->dev, "failed to remap I/O memory\n");
-		error = -ENXIO;
-		goto err_free_mem_region;
+	/* resources are sane; we begin allocation */
+	keypad = kzalloc(sizeof(struct ske_keypad), GFP_KERNEL);
+	if (!keypad) {
+		dev_err(&pdev->dev, "failed to allocate keypad memory\n");
+		goto out_freeclk;
 	}
 
-	keypad->clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(keypad->clk)) {
-		dev_err(&pdev->dev, "failed to get clk\n");
-		error = PTR_ERR(keypad->clk);
-		goto err_iounmap;
+	input = input_allocate_device();
+	if (!input) {
+		dev_err(&pdev->dev, "failed to input_allocate_device\n");
+		ret = -ENOMEM;
+		goto out_freekeypad;
+	}
+	keypad->regulator = regulator_get(&pdev->dev, "v-ape");
+	if (IS_ERR(keypad->regulator)) {
+		dev_err(&pdev->dev, "regulator_get failed\n");
+		keypad->regulator = NULL;
+		goto out_regulator_get;
+	} else {
+		ret = regulator_enable(keypad->regulator);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "regulator_enable failed\n");
+			goto out_regulator_enable;
+		}
 	}
 
 	input->id.bustype = BUS_HOST;
@@ -265,38 +533,90 @@ static int __devinit ske_keypad_probe(struct platform_device *pdev)
 	input->keycodemax = ARRAY_SIZE(keypad->keymap);
 
 	input_set_capability(input, EV_MSC, MSC_SCAN);
+	input_set_drvdata(input, keypad);
 
 	__set_bit(EV_KEY, input->evbit);
 	if (!plat->no_autorepeat)
 		__set_bit(EV_REP, input->evbit);
 
 	matrix_keypad_build_keymap(plat->keymap_data, SKE_KEYPAD_ROW_SHIFT,
-			input->keycode, input->keybit);
+					input->keycode, input->keybit);
+
+	ret = input_register_device(input);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"unable to register input device: %d\n", ret);
+		goto out_freeinput;
+	}
+
+	keypad->irq	= irq;
+	keypad->board	= plat;
+	keypad->input	= input;
+	keypad->reg_base = reg_base;
+	keypad->clk	= clk;
+	INIT_DELAYED_WORK(&keypad->work, ske_gpio_switch_work);
+	INIT_DELAYED_WORK(&keypad->gpio_work, ske_gpio_release_work);
 
+	/* allocations are sane, we begin HW initialization */
 	clk_enable(keypad->clk);
 
-	/* go through board initialization helpers */
-	if (keypad->board->init)
-		keypad->board->init();
+	if (!keypad->board->init) {
+		dev_err(&pdev->dev, "init funtion not defined\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
+	}
+
+	if (keypad->board->init() < 0) {
+		dev_err(&pdev->dev, "keyboard init config failed\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
+	}
 
-	error = ske_keypad_chip_init(keypad);
-	if (error) {
-		dev_err(&pdev->dev, "unable to init keypad hardware\n");
-		goto err_clk_disable;
+	if (!keypad->board->exit) {
+		dev_err(&pdev->dev, "exit funtion not defined\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
 	}
 
-	error = request_threaded_irq(keypad->irq, NULL, ske_keypad_irq,
-				     IRQF_ONESHOT, "ske-keypad", keypad);
-	if (error) {
+	if (keypad->board->exit() < 0) {
+		dev_err(&pdev->dev,  "keyboard exit config failed\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
+	}
+	for (i = 0; i < SKE_KPD_MAX_ROWS; i++) {
+		keypad->ske_rows[i] = *plat->gpio_input_pins;
+		keypad->ske_cols[i] = *plat->gpio_output_pins;
+		keypad->gpio_input_irq[i] =
+				NOMADIK_GPIO_TO_IRQ(keypad->ske_rows[i]);
+		plat->gpio_input_pins++;
+		plat->gpio_output_pins++;
+	}
+
+	for (i = 0; i < keypad->board->krow; i++) {
+		ret =  request_threaded_irq(keypad->gpio_input_irq[i],
+				NULL, ske_keypad_gpio_irq,
+				IRQF_TRIGGER_FALLING | IRQF_NO_SUSPEND,
+				"ske-keypad-gpio", keypad);
+		if (ret) {
+			dev_err(&pdev->dev, "allocate gpio irq %d failed\n",
+						keypad->gpio_input_irq[i]);
+			goto out_unregisterinput;
+		}
+		enable_irq_wake(keypad->gpio_input_irq[i]);
+	}
+
+	ret =  request_threaded_irq(keypad->irq, NULL, ske_keypad_irq,
+				IRQF_ONESHOT, "ske-keypad", keypad);
+	if (ret) {
 		dev_err(&pdev->dev, "allocate irq %d failed\n", keypad->irq);
-		goto err_clk_disable;
+		goto out_unregisterinput;
 	}
 
-	error = input_register_device(input);
-	if (error) {
-		dev_err(&pdev->dev,
-				"unable to register input device: %d\n", error);
-		goto err_free_irq;
+	/* sysfs implementation for dynamic enable/disable the input event */
+	ret = sysfs_create_group(&pdev->dev.kobj, &ske_attr_group);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to create sysfs entries\n");
+		goto out_free_irq;
 	}
 
 	if (plat->wakeup_enable)
@@ -304,21 +624,32 @@ static int __devinit ske_keypad_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, keypad);
 
+	clk_disable(keypad->clk);
+	regulator_disable(keypad->regulator);
+
 	return 0;
 
-err_free_irq:
+out_free_irq:
 	free_irq(keypad->irq, keypad);
-err_clk_disable:
+out_unregisterinput:
+	input_unregister_device(input);
+	input = NULL;
 	clk_disable(keypad->clk);
-	clk_put(keypad->clk);
-err_iounmap:
-	iounmap(keypad->reg_base);
-err_free_mem_region:
-	release_mem_region(res->start, resource_size(res));
-err_free_mem:
+out_freeinput:
+	regulator_disable(keypad->regulator);
+out_regulator_enable:
+	regulator_put(keypad->regulator);
+out_regulator_get:
 	input_free_device(input);
+out_freekeypad:
 	kfree(keypad);
-	return error;
+out_freeclk:
+	clk_put(keypad->clk);
+out_freeioremap:
+	iounmap(reg_base);
+out_freerequest_memregions:
+	release_mem_region(res->start, resource_size(res));
+	return ret;
 }
 
 static int __devexit ske_keypad_remove(struct platform_device *pdev)
@@ -326,16 +657,21 @@ static int __devexit ske_keypad_remove(struct platform_device *pdev)
 	struct ske_keypad *keypad = platform_get_drvdata(pdev);
 	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
+	cancel_delayed_work_sync(&keypad->gpio_work);
+	cancel_delayed_work_sync(&keypad->work);
 	free_irq(keypad->irq, keypad);
 
 	input_unregister_device(keypad->input);
 
+	sysfs_remove_group(&pdev->dev.kobj, &ske_attr_group);
 	clk_disable(keypad->clk);
 	clk_put(keypad->clk);
 
 	if (keypad->board->exit)
 		keypad->board->exit();
 
+	regulator_put(keypad->regulator);
+
 	iounmap(keypad->reg_base);
 	release_mem_region(res->start, resource_size(res));
 	kfree(keypad);
@@ -352,8 +688,15 @@ static int ske_keypad_suspend(struct device *dev)
 
 	if (device_may_wakeup(dev))
 		enable_irq_wake(irq);
-	else
-		ske_keypad_set_bits(keypad, SKE_IMSC, ~SKE_KPIMA, 0x0);
+	else {
+		cancel_delayed_work_sync(&keypad->gpio_work);
+		cancel_delayed_work_sync(&keypad->work);
+		disable_irq(irq);
+		if (keypad->enable) {
+			ske_mode_enable(keypad, false);
+			keypad->enable = false;
+		}
+	}
 
 	return 0;
 }
@@ -366,8 +709,13 @@ static int ske_keypad_resume(struct device *dev)
 
 	if (device_may_wakeup(dev))
 		disable_irq_wake(irq);
-	else
-		ske_keypad_set_bits(keypad, SKE_IMSC, 0x0, SKE_KPIMA);
+	else {
+		if (!keypad->enable) {
+			keypad->enable = true;
+			ske_mode_enable(keypad, true);
+		}
+		enable_irq(irq);
+	}
 
 	return 0;
 }
@@ -392,7 +740,7 @@ struct platform_driver ske_keypad_driver = {
 
 static int __init ske_keypad_init(void)
 {
-	return platform_driver_probe(&ske_keypad_driver, ske_keypad_probe);
+	return platform_driver_register(&ske_keypad_driver);
 }
 module_init(ske_keypad_init);
 
@@ -403,6 +751,6 @@ static void __exit ske_keypad_exit(void)
 module_exit(ske_keypad_exit);
 
 MODULE_LICENSE("GPL v2");
-MODULE_AUTHOR("Naveen Kumar <naveen.gaddipati@stericsson.com> / Sundar Iyer <sundar.iyer@stericsson.com>");
+MODULE_AUTHOR("Naveen Kumar <naveen.gaddipati@stericsson.com>");
 MODULE_DESCRIPTION("Nomadik Scroll-Key-Encoder Keypad Driver");
 MODULE_ALIAS("platform:nomadik-ske-keypad");
diff --git a/drivers/input/keyboard/stmpe-keypad.c b/drivers/input/keyboard/stmpe-keypad.c
index ab7610ca10e..c1de4d77ca1 100644
--- a/drivers/input/keyboard/stmpe-keypad.c
+++ b/drivers/input/keyboard/stmpe-keypad.c
@@ -108,10 +108,52 @@ struct stmpe_keypad {
 
 	unsigned int rows;
 	unsigned int cols;
+	bool enable;
 
 	unsigned short keymap[STMPE_KEYPAD_KEYMAP_SIZE];
 };
 
+static ssize_t stmpe_show_attr_enable(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct stmpe_keypad *keypad = platform_get_drvdata(pdev);
+	return sprintf(buf, "%d\n", keypad->enable);
+}
+
+static ssize_t stmpe_store_attr_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct stmpe_keypad *keypad = platform_get_drvdata(pdev);
+	struct stmpe *stmpe = keypad->stmpe;
+	unsigned long val;
+
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if (keypad->enable != val) {
+		keypad->enable = val;
+		if (!val)
+			stmpe_disable(stmpe, STMPE_BLOCK_KEYPAD);
+		else
+			stmpe_enable(stmpe, STMPE_BLOCK_KEYPAD);
+	}
+	return count;
+}
+
+static DEVICE_ATTR(enable, S_IWUSR | S_IRUGO,
+	stmpe_show_attr_enable, stmpe_store_attr_enable);
+
+static struct attribute *stmpe_keypad_attrs[] = {
+	&dev_attr_enable.attr,
+	NULL,
+};
+
+static struct attribute_group stmpe_attr_group = {
+	.attrs = stmpe_keypad_attrs,
+};
+
 static int stmpe_keypad_read_data(struct stmpe_keypad *keypad, u8 *data)
 {
 	const struct stmpe_keypad_variant *variant = keypad->variant;
@@ -285,7 +327,7 @@ static int __devinit stmpe_keypad_probe(struct platform_device *pdev)
 		goto out_freekeypad;
 	}
 
-	input->name = "STMPE keypad";
+	input->name = "STMPE-keypad";
 	input->id.bustype = BUS_I2C;
 	input->dev.parent = &pdev->dev;
 
@@ -332,10 +374,20 @@ static int __devinit stmpe_keypad_probe(struct platform_device *pdev)
 		goto out_unregisterinput;
 	}
 
+	/* sysfs implementation for dynamic enable/disable the input event */
+	ret = sysfs_create_group(&pdev->dev.kobj, &stmpe_attr_group);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to create sysfs entries\n");
+		goto out_free_irq;
+	}
+
+	keypad->enable = true;
 	platform_set_drvdata(pdev, keypad);
 
 	return 0;
 
+out_free_irq:
+	free_irq(irq, keypad);
 out_unregisterinput:
 	input_unregister_device(input);
 	input = NULL;
@@ -354,6 +406,7 @@ static int __devexit stmpe_keypad_remove(struct platform_device *pdev)
 
 	stmpe_disable(stmpe, STMPE_BLOCK_KEYPAD);
 
+	sysfs_remove_group(&pdev->dev.kobj, &stmpe_attr_group);
 	free_irq(irq, keypad);
 	input_unregister_device(keypad->input);
 	platform_set_drvdata(pdev, NULL);
@@ -362,9 +415,43 @@ static int __devexit stmpe_keypad_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int stmpe_keypad_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct stmpe_keypad *keypad = platform_get_drvdata(pdev);
+	struct stmpe *stmpe = keypad->stmpe;
+
+	if (!device_may_wakeup(stmpe->dev))
+		stmpe_disable(stmpe, STMPE_BLOCK_KEYPAD);
+
+	return 0;
+}
+
+static int stmpe_keypad_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct stmpe_keypad *keypad = platform_get_drvdata(pdev);
+	struct stmpe *stmpe = keypad->stmpe;
+
+	if (!device_may_wakeup(stmpe->dev))
+		stmpe_enable(stmpe, STMPE_BLOCK_KEYPAD);
+
+	return 0;
+}
+
+static const struct dev_pm_ops stmpe_keypad_dev_pm_ops = {
+	.suspend = stmpe_keypad_suspend,
+	.resume = stmpe_keypad_resume,
+};
+#endif
+
 static struct platform_driver stmpe_keypad_driver = {
 	.driver.name	= "stmpe-keypad",
 	.driver.owner	= THIS_MODULE,
+#ifdef CONFIG_PM
+	.driver.pm	= &stmpe_keypad_dev_pm_ops,
+#endif
 	.probe		= stmpe_keypad_probe,
 	.remove		= __devexit_p(stmpe_keypad_remove),
 };
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 6f4ad1a7c3e..efd55625a46 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -22,12 +22,19 @@ config INPUT_88PM860X_ONKEY
 	  To compile this driver as a module, choose M here: the module
 	  will be called 88pm860x_onkey.
 
+config INPUT_AB8500_ACCDET
+	bool "AB8500 AV Accessory detection"
+	depends on AB8500_CORE && AB8500_GPADC && GPIO_AB8500
+	help
+	  Say Y here to enable AV accessory detection features for ST-Ericsson's
+	  AB8500 Mix-Sig PMIC.
+
 config INPUT_AB8500_PONKEY
-	tristate "AB8500 Pon (PowerOn) Key"
-	depends on AB8500_CORE
+	tristate "AB5500/AB8500 Pon (PowerOn) Key"
+	depends on AB5500_CORE || AB8500_CORE
 	help
-	  Say Y here to use the PowerOn Key for ST-Ericsson's AB8500
-	  Mix-Sig PMIC.
+	  Say Y here to use the PowerOn Key for ST-Ericsson's AB5500/AB8500
+	  Mix-Sig PMICs.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called ab8500-ponkey.
@@ -220,6 +227,13 @@ config INPUT_KEYSPAN_REMOTE
 	  To compile this driver as a module, choose M here: the module will
 	  be called keyspan_remote.
 
+config INPUT_LPS001WP
+	tristate "LPS0001WP pressure sensor from ST Micro"
+	default y if MACH_U8500
+	help
+	  This is a pressure sensor connected to I2C, mounted on the
+	  snowball and other ST-E boards
+
 config INPUT_POWERMATE
 	tristate "Griffin PowerMate and Contour Jog support"
 	depends on USB_ARCH_HAS_HCD
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index eb73834150b..7b16f212884 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -5,6 +5,7 @@
 # Each configuration option enables a list of files.
 
 obj-$(CONFIG_INPUT_88PM860X_ONKEY)	+= 88pm860x_onkey.o
+obj-$(CONFIG_INPUT_AB8500_ACCDET)       += ab8500-accdet.o
 obj-$(CONFIG_INPUT_AB8500_PONKEY)	+= ab8500-ponkey.o
 obj-$(CONFIG_INPUT_AD714X)		+= ad714x.o
 obj-$(CONFIG_INPUT_AD714X_I2C)		+= ad714x-i2c.o
@@ -27,6 +28,7 @@ obj-$(CONFIG_HP_SDC_RTC)		+= hp_sdc_rtc.o
 obj-$(CONFIG_INPUT_IXP4XX_BEEPER)	+= ixp4xx-beeper.o
 obj-$(CONFIG_INPUT_KEYCHORD)		+= keychord.o
 obj-$(CONFIG_INPUT_KEYSPAN_REMOTE)	+= keyspan_remote.o
+obj-$(CONFIG_INPUT_LPS001WP)		+= lps001wp_prs.o
 obj-$(CONFIG_INPUT_M68K_BEEP)		+= m68kspkr.o
 obj-$(CONFIG_INPUT_MAX8925_ONKEY)	+= max8925_onkey.o
 obj-$(CONFIG_INPUT_PCAP)		+= pcap_keys.o
diff --git a/drivers/input/misc/ab8500-accdet.c b/drivers/input/misc/ab8500-accdet.c
new file mode 100644
index 00000000000..3bb898503e7
--- /dev/null
+++ b/drivers/input/misc/ab8500-accdet.c
@@ -0,0 +1,1427 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Jarmo K. Kuronen <jarmo.kuronen@symbio.com>
+ *         for ST-Ericsson.
+ *
+ * License terms: GPL V2
+ *
+ * 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>       /* Needed by all modules */
+#include <linux/kernel.h>       /* Needed for KERN_INFO */
+#include <linux/init.h>         /* Needed for the macros */
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+#include <linux/jiffies.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <sound/soc.h>
+#include <sound/jack.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/ab8500/gpadc.h>
+#include <linux/mfd/ab8500/gpio.h>
+#include <linux/gpio.h>
+#include <mach/ab8500-accdet.h>
+#ifdef CONFIG_SND_SOC_UX500_AB8500
+#include <sound/ux500_ab8500.h>
+#else
+#define ux500_ab8500_jack_report(i)
+#endif
+
+#define MAX_DET_COUNT			10
+#define MAX_VOLT_DIFF			30
+#define MIN_MIC_POWER			-100
+
+/* Unique value used to identify Headset button input device */
+#define BTN_INPUT_UNIQUE_VALUE	"AB8500HsBtn"
+#define BTN_INPUT_DEV_NAME	"AB8500 Hs Button"
+
+#define DEBOUNCE_PLUG_EVENT_MS		100
+#define DEBOUNCE_PLUG_RETEST_MS		25
+#define DEBOUNCE_UNPLUG_EVENT_MS	0
+
+/*
+ * Register definition for accessory detection.
+ */
+#define AB8500_REGU_CTRL1_SPARE_REG	0x84
+#define AB8500_ACC_DET_DB1_REG		0x80
+#define AB8500_ACC_DET_DB2_REG		0x81
+#define AB8500_ACC_DET_CTRL_REG		0x82
+#define AB8500_IT_SOURCE5_REG		0x04
+
+/* REGISTER: AB8500_ACC_DET_CTRL_REG */
+#define BITS_ACCDETCTRL2_ENA		(0x20 | 0x10 | 0x08)
+#define BITS_ACCDETCTRL1_ENA		(0x02 | 0x01)
+
+/* REGISTER: AB8500_REGU_CTRL1_SPARE_REG */
+#define BIT_REGUCTRL1SPARE_VAMIC1_GROUND	0x01
+
+/* REGISTER: AB8500_IT_SOURCE5_REG */
+#define BIT_ITSOURCE5_ACCDET1		0x04
+
+/* After being loaded, how fast the first check is to be made */
+#define INIT_DELAY_MS			3000
+
+/* Voltage limits (mV) for various types of AV Accessories */
+#define ACCESSORY_DET_VOL_DONTCARE	-1
+#define ACCESSORY_HEADPHONE_DET_VOL_MIN	0
+#define ACCESSORY_HEADPHONE_DET_VOL_MAX	40
+#define ACCESSORY_CVIDEO_DET_VOL_MIN	41
+#define ACCESSORY_CVIDEO_DET_VOL_MAX	105
+#define ACCESSORY_CARKIT_DET_VOL_MIN	1100
+#define ACCESSORY_CARKIT_DET_VOL_MAX	1300
+#define ACCESSORY_HEADSET_DET_VOL_MIN	0
+#define ACCESSORY_HEADSET_DET_VOL_MAX	200
+#define ACCESSORY_OPENCABLE_DET_VOL_MIN	1730
+#define ACCESSORY_OPENCABLE_DET_VOL_MAX	2150
+
+/* Macros */
+
+/*
+ * Conviniency macros to check jack characteristics.
+ */
+#define jack_supports_mic(type) \
+	(type == JACK_TYPE_HEADSET || type == JACK_TYPE_CARKIT)
+#define jack_supports_spkr(type) \
+	((type != JACK_TYPE_DISCONNECTED) && (type != JACK_TYPE_CONNECTED))
+#define jack_supports_buttons(type) \
+	((type == JACK_TYPE_HEADSET) ||\
+	(type == JACK_TYPE_CARKIT) ||\
+	(type == JACK_TYPE_OPENCABLE) ||\
+	(type == JACK_TYPE_CONNECTED))
+
+
+/* Enumerations */
+
+/**
+ * @JACK_TYPE_UNSPECIFIED Not known whether any accessories are connected.
+ * @JACK_TYPE_DISCONNECTED No accessories connected.
+ * @JACK_TYPE_CONNECTED Accessory is connected but functionality was unable to
+ * detect the actual type. In this mode, possible button events are reported.
+ * @JACK_TYPE_HEADPHONE Headphone type of accessory (spkrs only) connected
+ * @JACK_TYPE_HEADSET Headset type of accessory (mic+spkrs) connected
+ * @JACK_TYPE_CARKIT Carkit type of accessory connected
+ * @JACK_TYPE_OPENCABLE Open cable connected
+ * @JACK_TYPE_CVIDEO CVideo type of accessory connected.
+ */
+enum accessory_jack_type {
+       JACK_TYPE_UNSPECIFIED,
+       JACK_TYPE_DISCONNECTED,
+       JACK_TYPE_CONNECTED,
+       JACK_TYPE_HEADPHONE,
+       JACK_TYPE_HEADSET,
+       JACK_TYPE_CARKIT,
+       JACK_TYPE_OPENCABLE,
+       JACK_TYPE_CVIDEO
+};
+
+/**
+ * @BUTTON_UNK Button state not known
+ * @BUTTON_PRESSED Button "down"
+ * @BUTTON_RELEASED Button "up"
+ */
+enum accessory_button_state {
+	BUTTON_UNK,
+	BUTTON_PRESSED,
+	BUTTON_RELEASED
+};
+
+/**
+ * @PLUG_IRQ Interrupt gen. when accessory plugged in
+ * @UNPLUG_IRQ Interrupt gen. when accessory plugged out
+ * @BUTTON_PRESS_IRQ Interrupt gen. when accessory button pressed.
+ * @BUTTON_RELEASE_IRQ Interrupt gen. when accessory button released.
+ */
+enum accessory_irq {
+	PLUG_IRQ,
+	UNPLUG_IRQ,
+	BUTTON_PRESS_IRQ,
+	BUTTON_RELEASE_IRQ
+};
+
+/**
+ * Enumerates the op. modes of the avcontrol switch
+ * @AUDIO_IN Audio input is selected
+ * @VIDEO_OUT Video output is selected
+ * @NOT_SET The av-switch control signal is disconnected.
+ */
+enum accessory_avcontrol_dir {
+	AUDIO_IN,
+	VIDEO_OUT,
+	NOT_SET,
+};
+
+/**
+ * @REGULATOR_VAUDIO v-audio regulator
+ * @REGULATOR_VAMIC1 v-amic1 regulator
+ * @REGULATOR_AVSWITCH Audio/Video select switch regulator
+ * @REGULATOR_ALL All regulators combined
+ */
+enum accessory_regulator {
+	REGULATOR_NONE = 0x0,
+	REGULATOR_VAUDIO = 0x1,
+	REGULATOR_VAMIC1 = 0x2,
+	REGULATOR_AVSWITCH = 0x4,
+	REGULATOR_ALL = 0xFF
+};
+
+/* Structures */
+
+/**
+ * Describes an interrupt
+ * @irq interrupt identifier
+ * @name name of the irq in platform data
+ * @isr interrupt service routine
+ * @register are we currently registered to receive interrupts from this source.
+ */
+struct accessory_irq_descriptor {
+	enum accessory_irq irq;
+	const char *name;
+	irq_handler_t isr;
+	int registered;
+};
+
+/**
+ * Encapsulates info of single regulator.
+ * @id regulator identifier
+ * @name name of the regulator
+ * @enabled flag indicating whether regu is currently enabled.
+ * @handle regulator handle
+ */
+struct accessory_regu_descriptor {
+	enum accessory_regulator id;
+	const char *name;
+	int enabled;
+	struct regulator *handle;
+};
+
+/**
+ * Defines attributes for accessory detection operation.
+ * @typename type as string
+ * @type Type of accessory this task tests
+ * @req_det_count How many times this particular type of accessory
+ * needs to be detected in sequence in order to accept. Multidetection
+ * implemented to avoid false detections during plug-in.
+ * @meas_mv Should ACCDETECT2 input voltage be measured just before
+ * making the decision or can cached voltage be used instead.
+ * @minvol minimum voltage (mV) for decision
+ * @maxvol maximum voltage (mV) for decision
+ */
+struct accessory_detect_task {
+	const char *typename;
+	enum accessory_jack_type type;
+	int req_det_count;
+	int meas_mv;
+	int minvol;
+	int maxvol;
+};
+
+/**
+ * Device data, capsulates all relevant device data structures.
+ *
+ * @pdev pointer to platform device
+ * @pdata Platform data
+ * @gpadc interface for ADC data
+ * @irq_work_queue Work queue for deferred interrupt processing
+ *
+ * @detect_work work item to perform detection work
+ * @unplug_irq_work work item to process unplug event
+ * @init_work work item to process initialization work.
+ *
+ * @btn_input_dev button input device used to report btn presses
+ * @btn_state Current state of accessory button
+ *
+ * @jack_type type of currently connected accessory
+ * @reported_jack_type previously reported jack type.
+ * @jack_type_temp temporary storage for currently connected accessory
+ *
+ * @jack_det_count counter how many times in sequence the accessory
+ * type detection has produced same result.
+ * @total_jack_det_count after plug-in irq, how many times detection
+ * has totally been made in order to detect the accessory type
+ *
+ * @detect_jiffies Used to save timestamp when detection was made. Timestamp
+ * used to filter out spurious button presses that might occur during the
+ * plug-in procedure.
+ *
+ * @accdet1_th_set flag to indicate whether accdet1 threshold and debounce
+ * times are configured
+ * @accdet2_th_set flag to indicate whether accdet2 thresholds are configured
+ * @gpio35_dir_set flag to indicate whether GPIO35 (VIDEOCTRL) direction
+ * has been configured.
+ */
+struct ab8500_ad {
+	struct platform_device *pdev;
+	struct ab8500_accdet_platform_data *pdata;
+	struct ab8500_gpadc *gpadc;
+	struct workqueue_struct *irq_work_queue;
+
+	struct delayed_work detect_work;
+	struct delayed_work unplug_irq_work;
+	struct delayed_work init_work;
+
+	struct input_dev *btn_input_dev;
+	enum accessory_button_state btn_state;
+
+	enum accessory_jack_type jack_type;
+	enum accessory_jack_type reported_jack_type;
+	enum accessory_jack_type jack_type_temp;
+
+	int jack_det_count;
+	int total_jack_det_count;
+
+	unsigned long detect_jiffies;
+
+	int accdet1_th_set;
+	int accdet2_th_set;
+	int gpio35_dir_set;
+};
+
+/* Forward declarations */
+
+static void config_accdetect(struct ab8500_ad *dd);
+
+static void release_irq(struct ab8500_ad *dd, enum accessory_irq irq_id);
+static void claim_irq(struct ab8500_ad *dd, enum accessory_irq irq_id);
+
+static irqreturn_t unplug_irq_handler(int irq, void *_userdata);
+static irqreturn_t plug_irq_handler(int irq, void *_userdata);
+static irqreturn_t button_press_irq_handler(int irq, void *_userdata);
+static irqreturn_t button_release_irq_handler(int irq, void *_userdata);
+
+static void unplug_irq_handler_work(struct work_struct *work);
+static void detect_work(struct work_struct *work);
+static void init_work(struct work_struct *work);
+
+static enum accessory_jack_type detect(struct ab8500_ad *dd, int *required_det);
+static void set_av_switch(struct ab8500_ad *dd,
+		enum accessory_avcontrol_dir dir);
+
+/* Static data initialization */
+
+static struct accessory_detect_task detect_ops[] = {
+	{
+		.type = JACK_TYPE_DISCONNECTED,
+		.typename = "DISCONNECTED",
+		.meas_mv = 1,
+		.req_det_count = 1,
+		.minvol = ACCESSORY_DET_VOL_DONTCARE,
+		.maxvol = ACCESSORY_DET_VOL_DONTCARE
+	},
+	{
+		.type = JACK_TYPE_HEADPHONE,
+		.typename = "HEADPHONE",
+		.meas_mv = 1,
+		.req_det_count = 1,
+		.minvol = ACCESSORY_HEADPHONE_DET_VOL_MIN,
+		.maxvol = ACCESSORY_HEADPHONE_DET_VOL_MAX
+	},
+	{
+		.type = JACK_TYPE_CVIDEO,
+		.typename = "CVIDEO",
+		.meas_mv = 0,
+		.req_det_count = 4,
+		.minvol = ACCESSORY_CVIDEO_DET_VOL_MIN,
+		.maxvol = ACCESSORY_CVIDEO_DET_VOL_MAX
+	},
+	{
+		.type = JACK_TYPE_OPENCABLE,
+		.typename = "OPENCABLE",
+		.meas_mv = 0,
+		.req_det_count = 4,
+		.minvol = ACCESSORY_OPENCABLE_DET_VOL_MIN,
+		.maxvol = ACCESSORY_OPENCABLE_DET_VOL_MAX
+	},
+	{
+		.type = JACK_TYPE_CARKIT,
+		.typename = "CARKIT",
+		.meas_mv = 1,
+		.req_det_count = 1,
+		.minvol = ACCESSORY_CARKIT_DET_VOL_MIN,
+		.maxvol = ACCESSORY_CARKIT_DET_VOL_MAX
+	},
+	{
+		.type = JACK_TYPE_HEADSET,
+		.typename = "HEADSET",
+		.meas_mv = 0,
+		.req_det_count = 2,
+		.minvol = ACCESSORY_HEADSET_DET_VOL_MIN,
+		.maxvol = ACCESSORY_HEADSET_DET_VOL_MAX
+	},
+	{
+		.type = JACK_TYPE_CONNECTED,
+		.typename = "CONNECTED",
+		.meas_mv = 0,
+		.req_det_count = 4,
+		.minvol = ACCESSORY_DET_VOL_DONTCARE,
+		.maxvol = ACCESSORY_DET_VOL_DONTCARE
+	}
+};
+
+static struct accessory_regu_descriptor regu_desc[3] = {
+	{
+		.id = REGULATOR_VAUDIO,
+		.name = "v-audio",
+	},
+	{
+		.id = REGULATOR_VAMIC1,
+		.name = "v-amic1",
+	},
+	{
+		.id = REGULATOR_AVSWITCH,
+		.name = "vcc-avswitch",
+	},
+};
+
+static struct accessory_irq_descriptor irq_desc_norm[] = {
+	{
+		.irq = PLUG_IRQ,
+		.name = "ACC_DETECT_1DB_F",
+		.isr = plug_irq_handler,
+	},
+	{
+		.irq = UNPLUG_IRQ,
+		.name = "ACC_DETECT_1DB_R",
+		.isr = unplug_irq_handler,
+	},
+	{
+		.irq = BUTTON_PRESS_IRQ,
+		.name = "ACC_DETECT_22DB_F",
+		.isr = button_press_irq_handler,
+	},
+	{
+		.irq = BUTTON_RELEASE_IRQ,
+		.name = "ACC_DETECT_22DB_R",
+		.isr = button_release_irq_handler,
+	},
+};
+
+static struct accessory_irq_descriptor irq_desc_inverted[] = {
+	{
+		.irq = PLUG_IRQ,
+		.name = "ACC_DETECT_1DB_R",
+		.isr = plug_irq_handler,
+	},
+	{
+		.irq = UNPLUG_IRQ,
+		.name = "ACC_DETECT_1DB_F",
+		.isr = unplug_irq_handler,
+	},
+	{
+		.irq = BUTTON_PRESS_IRQ,
+		.name = "ACC_DETECT_22DB_R",
+		.isr = button_press_irq_handler,
+	},
+	{
+		.irq = BUTTON_RELEASE_IRQ,
+		.name = "ACC_DETECT_22DB_F",
+		.isr = button_release_irq_handler,
+	},
+};
+
+static struct accessory_irq_descriptor *irq_desc;
+
+/*
+ * textual represenation of the accessory type
+ */
+static const char *accessory_str(enum accessory_jack_type type)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(detect_ops); i++)
+		if (type == detect_ops[i].type)
+			return detect_ops[i].typename;
+
+	return "UNKNOWN?";
+}
+
+/*
+ * enables regulator but only if it has not been enabled earlier.
+ */
+static void accessory_regulator_enable(enum accessory_regulator reg)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(regu_desc); i++) {
+		if (reg & regu_desc[i].id) {
+			if (!regu_desc[i].enabled) {
+				if (!regulator_enable(regu_desc[i].handle))
+					regu_desc[i].enabled = 1;
+			}
+		}
+	}
+}
+
+/*
+ * disables regulator but only if it has been previously enabled.
+ */
+static void accessory_regulator_disable(enum accessory_regulator reg)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(regu_desc); i++) {
+		if (reg & regu_desc[i].id) {
+			if (regu_desc[i].enabled) {
+				if (!regulator_disable(regu_desc[i].handle))
+					regu_desc[i].enabled = 0;
+			}
+		}
+	}
+}
+
+/*
+ * frees previously retrieved regulators.
+ */
+static void free_regulators(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(regu_desc); i++) {
+		if (regu_desc[i].handle) {
+			regulator_put(regu_desc[i].handle);
+			regu_desc[i].handle = NULL;
+		}
+	}
+}
+
+/*
+ * gets required regulators.
+ */
+static int create_regulators(struct ab8500_ad *dd)
+{
+	int i;
+	int status = 0;
+
+	for (i = 0; i < ARRAY_SIZE(regu_desc); i++) {
+		struct regulator *regu =
+			regulator_get(&dd->pdev->dev, regu_desc[i].name);
+		if (IS_ERR(regu)) {
+			status = PTR_ERR(regu);
+			dev_err(&dd->pdev->dev,
+				"%s: Failed to get supply '%s' (%d).\n",
+				__func__, regu_desc[i].name, status);
+			free_regulators();
+			goto out;
+		} else {
+			regu_desc[i].handle = regu;
+		}
+	}
+
+out:
+	return status;
+}
+
+/*
+ * configures accdet2 input on/off
+ */
+static void config_accdetect2_hw(struct ab8500_ad *dd, int enable)
+{
+	int ret = 0;
+
+	if (!dd->accdet2_th_set) {
+		/* Configure accdetect21+22 thresholds */
+		ret = abx500_set_register_interruptible(&dd->pdev->dev,
+				AB8500_ECI_AV_ACC,
+				AB8500_ACC_DET_DB2_REG,
+				dd->pdata->accdet2122_th);
+		if (ret < 0) {
+			dev_err(&dd->pdev->dev,
+				"%s: Failed to write reg (%d).\n", __func__,
+					ret);
+			goto out;
+		} else {
+			dd->accdet2_th_set = 1;
+		}
+	}
+
+	/* Enable/Disable accdetect21 comparators + pullup */
+	ret = abx500_mask_and_set_register_interruptible(
+			&dd->pdev->dev,
+			AB8500_ECI_AV_ACC,
+			AB8500_ACC_DET_CTRL_REG,
+			BITS_ACCDETCTRL2_ENA,
+			enable ? BITS_ACCDETCTRL2_ENA : 0);
+
+	if (ret < 0)
+		dev_err(&dd->pdev->dev, "%s: Failed to update reg (%d).\n",
+				__func__, ret);
+
+out:
+	return;
+}
+
+/*
+ * configures accdet1 input on/off
+ */
+static void config_accdetect1_hw(struct ab8500_ad *dd, int enable)
+{
+	int ret;
+
+	if (!dd->accdet1_th_set) {
+		ret = abx500_set_register_interruptible(&dd->pdev->dev,
+				AB8500_ECI_AV_ACC,
+				AB8500_ACC_DET_DB1_REG,
+				dd->pdata->accdet1_dbth);
+		if (ret < 0)
+			dev_err(&dd->pdev->dev,
+				"%s: Failed to write reg (%d).\n", __func__,
+				ret);
+		else
+			dd->accdet1_th_set = 1;
+	}
+
+	/* enable accdetect1 comparator */
+	ret = abx500_mask_and_set_register_interruptible(
+				&dd->pdev->dev,
+				AB8500_ECI_AV_ACC,
+				AB8500_ACC_DET_CTRL_REG,
+				BITS_ACCDETCTRL1_ENA,
+				enable ? BITS_ACCDETCTRL1_ENA : 0);
+
+	if (ret < 0)
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to update reg (%d).\n", __func__, ret);
+}
+
+/*
+ * create input device for button press reporting
+ */
+static int create_btn_input_dev(struct ab8500_ad *dd)
+{
+	int err;
+
+	dd->btn_input_dev = input_allocate_device();
+	if (!dd->btn_input_dev) {
+		dev_err(&dd->pdev->dev, "%s: Failed to allocate input dev.\n",
+			__func__);
+		err = -ENOMEM;
+		goto out;
+	}
+
+	input_set_capability(dd->btn_input_dev,
+				EV_KEY,
+				dd->pdata->btn_keycode);
+
+	dd->btn_input_dev->name = BTN_INPUT_DEV_NAME;
+	dd->btn_input_dev->uniq = BTN_INPUT_UNIQUE_VALUE;
+	dd->btn_input_dev->dev.parent = &dd->pdev->dev;
+
+	err = input_register_device(dd->btn_input_dev);
+	if (err) {
+		dev_err(&dd->pdev->dev,
+			"%s: register_input_device failed (%d).\n", __func__,
+			err);
+		input_free_device(dd->btn_input_dev);
+		dd->btn_input_dev = NULL;
+		goto out;
+	}
+out:
+	return err;
+}
+
+/*
+ * reports jack status
+ */
+static void report_jack_status(struct ab8500_ad *dd)
+{
+	int value = 0;
+
+	/* Never report possible open cable */
+	if (dd->jack_type == JACK_TYPE_OPENCABLE)
+		goto out;
+
+	/* Never report same state twice in a row */
+	if (dd->jack_type == dd->reported_jack_type)
+		goto out;
+	dd->reported_jack_type = dd->jack_type;
+
+	dev_info(&dd->pdev->dev, "Accessory: %s\n",
+		accessory_str(dd->jack_type));
+
+	if (dd->jack_type != JACK_TYPE_DISCONNECTED &&
+		dd->jack_type != JACK_TYPE_UNSPECIFIED)
+		value |= SND_JACK_MECHANICAL;
+	if (jack_supports_mic(dd->jack_type))
+		value |= SND_JACK_MICROPHONE;
+	if (jack_supports_spkr(dd->jack_type))
+		value |= (SND_JACK_HEADPHONE | SND_JACK_LINEOUT);
+	if (dd->jack_type == JACK_TYPE_CVIDEO) {
+		value |= SND_JACK_VIDEOOUT;
+		set_av_switch(dd, VIDEO_OUT);
+	}
+
+	ux500_ab8500_jack_report(value);
+
+out: return;
+}
+
+/*
+ * returns the high level status whether some accessory is connected (1|0).
+ */
+static int detect_plugged_in(struct ab8500_ad *dd)
+{
+	u8 value = 0;
+
+	int status = abx500_get_register_interruptible(
+				&dd->pdev->dev,
+				AB8500_INTERRUPT,
+				AB8500_IT_SOURCE5_REG,
+				&value);
+	if (status < 0) {
+		dev_err(&dd->pdev->dev, "%s: reg read failed (%d).\n",
+			__func__, status);
+		return 0;
+	}
+
+	if (dd->pdata->is_detection_inverted)
+		return value & BIT_ITSOURCE5_ACCDET1 ? 1 : 0;
+	else
+		return value & BIT_ITSOURCE5_ACCDET1 ? 0 : 1;
+}
+
+/*
+ * mic_line_voltage_stable - measures a relative stable voltage from spec. input
+ */
+static int meas_voltage_stable(struct ab8500_ad *dd, u8 input)
+{
+	int iterations = 2;
+	int v1, v2, dv;
+
+	v1 = ab8500_gpadc_convert(dd->gpadc, input);
+	do {
+		msleep(1);
+		--iterations;
+		v2 = ab8500_gpadc_convert(dd->gpadc, input);
+		dv = abs(v2 - v1);
+		v1 = v2;
+	} while (iterations > 0 && dv > MAX_VOLT_DIFF);
+
+	return v1;
+}
+
+/*
+ * worker routine to handle accessory unplug case
+ */
+static void unplug_irq_handler_work(struct work_struct *work)
+{
+	struct ab8500_ad *dd = container_of(work,
+		struct ab8500_ad, unplug_irq_work.work);
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter\n", __func__);
+
+	dd->jack_type = dd->jack_type_temp = JACK_TYPE_DISCONNECTED;
+	dd->jack_det_count = dd->total_jack_det_count = 0;
+	dd->btn_state = BUTTON_UNK;
+	config_accdetect(dd);
+
+	accessory_regulator_disable(REGULATOR_ALL);
+
+	report_jack_status(dd);
+}
+
+/*
+ * interrupt service routine for accessory unplug.
+ */
+static irqreturn_t unplug_irq_handler(int irq, void *_userdata)
+{
+	struct ab8500_ad *dd = _userdata;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter (irq=%d)\n", __func__, irq);
+
+	queue_delayed_work(dd->irq_work_queue, &dd->unplug_irq_work,
+			msecs_to_jiffies(DEBOUNCE_UNPLUG_EVENT_MS));
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * interrupt service routine for accessory plug.
+ */
+static irqreturn_t plug_irq_handler(int irq, void *_userdata)
+{
+	struct ab8500_ad *dd = _userdata;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter (irq=%d)\n",
+		__func__, irq);
+
+	switch (dd->jack_type) {
+	case JACK_TYPE_DISCONNECTED:
+	case JACK_TYPE_UNSPECIFIED:
+		queue_delayed_work(dd->irq_work_queue, &dd->detect_work,
+			msecs_to_jiffies(DEBOUNCE_PLUG_EVENT_MS));
+		break;
+
+	default:
+		dev_err(&dd->pdev->dev, "%s: Unexpected plug IRQ\n", __func__);
+		break;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * worker routine to perform detection.
+ */
+static void detect_work(struct work_struct *work)
+{
+	int req_det_count = 1;
+	enum accessory_jack_type new_type;
+	struct ab8500_ad *dd = container_of(work,
+		struct ab8500_ad, detect_work.work);
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter\n", __func__);
+
+	set_av_switch(dd, AUDIO_IN);
+
+	new_type = detect(dd, &req_det_count);
+
+	dd->total_jack_det_count++;
+	if (dd->jack_type_temp == new_type) {
+		dd->jack_det_count++;
+	} else {
+		dd->jack_det_count = 1;
+		dd->jack_type_temp = new_type;
+	}
+
+	if (dd->total_jack_det_count >= MAX_DET_COUNT) {
+		dev_err(&dd->pdev->dev,
+			"%s: MAX_DET_COUNT(=%d) reached. Bailing out.\n",
+					__func__, MAX_DET_COUNT);
+		queue_delayed_work(dd->irq_work_queue, &dd->unplug_irq_work,
+				msecs_to_jiffies(DEBOUNCE_UNPLUG_EVENT_MS));
+	} else if (dd->jack_det_count >= req_det_count) {
+		dd->total_jack_det_count = dd->jack_det_count = 0;
+		dd->jack_type = new_type;
+		dd->detect_jiffies = jiffies;
+		report_jack_status(dd);
+		config_accdetect(dd);
+	} else {
+		queue_delayed_work(dd->irq_work_queue,
+				&dd->detect_work,
+				msecs_to_jiffies(DEBOUNCE_PLUG_RETEST_MS));
+	}
+}
+
+/*
+ * reports a button event (pressed, released).
+ */
+static void report_btn_event(struct ab8500_ad *dd, int down)
+{
+	input_report_key(dd->btn_input_dev, dd->pdata->btn_keycode, down);
+	input_sync(dd->btn_input_dev);
+
+	dev_dbg(&dd->pdev->dev, "HS-BTN: %s\n", down ? "PRESSED" : "RELEASED");
+}
+
+/*
+ * interrupt service routine invoked when hs button is pressed down.
+ */
+static irqreturn_t button_press_irq_handler(int irq, void *_userdata)
+{
+	struct ab8500_ad *dd = _userdata;
+
+	unsigned long accept_jiffies = dd->detect_jiffies +
+			msecs_to_jiffies(1000);
+	if (time_before(jiffies, accept_jiffies)) {
+		dev_dbg(&dd->pdev->dev, "%s: Skipped spurious btn press.\n",
+			__func__);
+		return IRQ_HANDLED;
+	}
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter (irq=%d)\n", __func__, irq);
+
+	if (dd->jack_type == JACK_TYPE_OPENCABLE) {
+		/* Someting got connected to open cable -> detect.. */
+		config_accdetect2_hw(dd, 0);
+		queue_delayed_work(dd->irq_work_queue, &dd->detect_work,
+			msecs_to_jiffies(DEBOUNCE_PLUG_EVENT_MS));
+		return IRQ_HANDLED;
+	}
+
+	if (dd->btn_state == BUTTON_PRESSED)
+		return IRQ_HANDLED;
+
+	if (jack_supports_buttons(dd->jack_type)) {
+		dd->btn_state = BUTTON_PRESSED;
+		report_btn_event(dd, 1);
+	} else {
+		dd->btn_state = BUTTON_UNK;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * interrupts service routine invoked when hs button is released.
+ */
+static irqreturn_t button_release_irq_handler(int irq, void *_userdata)
+{
+	struct ab8500_ad *dd = _userdata;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter (irq=%d)\n", __func__, irq);
+
+	if (dd->jack_type == JACK_TYPE_OPENCABLE)
+		return IRQ_HANDLED;
+
+	if (dd->btn_state != BUTTON_PRESSED)
+		return IRQ_HANDLED;
+
+	if (jack_supports_buttons(dd->jack_type)) {
+		report_btn_event(dd, 0);
+		dd->btn_state = BUTTON_RELEASED;
+	} else {
+		dd->btn_state = BUTTON_UNK;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * configures HW so that it is possible to make decision whether
+ * accessory is connected or not.
+ */
+static void config_hw_test_plug_connected(struct ab8500_ad *dd, int enable)
+{
+	int ret;
+
+	dev_dbg(&dd->pdev->dev, "%s:%d\n", __func__, enable);
+
+	ret = ab8500_config_pull_up_or_down(&dd->pdev->dev,
+					dd->pdata->video_ctrl_gpio, !enable);
+	if (ret < 0) {
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to update reg (%d).\n", __func__, ret);
+		return;
+	}
+
+	if (enable)
+		accessory_regulator_enable(REGULATOR_VAMIC1);
+}
+
+/*
+ * configures HW so that carkit/headset detection can be accomplished.
+ */
+static void config_hw_test_basic_carkit(struct ab8500_ad *dd, int enable)
+{
+	int ret;
+
+	dev_dbg(&dd->pdev->dev, "%s:%d\n", __func__, enable);
+
+	if (enable)
+		accessory_regulator_disable(REGULATOR_VAMIC1);
+
+	/* Un-Ground the VAMic1 output when enabled */
+	ret = abx500_mask_and_set_register_interruptible(
+				&dd->pdev->dev,
+				AB8500_REGU_CTRL1,
+				AB8500_REGU_CTRL1_SPARE_REG,
+				BIT_REGUCTRL1SPARE_VAMIC1_GROUND,
+				enable ? BIT_REGUCTRL1SPARE_VAMIC1_GROUND : 0);
+	if (ret < 0)
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to update reg (%d).\n", __func__, ret);
+}
+
+/*
+ * checks whether measured voltage is in given range. depending on arguments,
+ * voltage might be re-measured or previously measured voltage is reused.
+ */
+static int mic_vol_in_range(struct ab8500_ad *dd,
+			int lo, int hi, int force_read)
+{
+	static int mv = MIN_MIC_POWER;
+
+	if (mv == -100 || force_read)
+		mv = meas_voltage_stable(dd, ACC_DETECT2);
+
+	return (mv >= lo && mv <= hi) ? 1 : 0;
+}
+
+/*
+ * checks whether the currently connected HW is of given type.
+ */
+static int detect_hw(struct ab8500_ad *dd,
+			struct accessory_detect_task *task)
+{
+	int status;
+
+	switch (task->type) {
+	case JACK_TYPE_DISCONNECTED:
+		config_hw_test_plug_connected(dd, 1);
+		status = !detect_plugged_in(dd);
+		break;
+	case JACK_TYPE_CONNECTED:
+		config_hw_test_plug_connected(dd, 1);
+		status = detect_plugged_in(dd);
+		break;
+	case JACK_TYPE_CARKIT:
+		config_hw_test_basic_carkit(dd, 1);
+		/* flow through.. */
+	case JACK_TYPE_HEADPHONE:
+	case JACK_TYPE_CVIDEO:
+	case JACK_TYPE_HEADSET:
+	case JACK_TYPE_OPENCABLE:
+		status = mic_vol_in_range(dd,
+					task->minvol,
+					task->maxvol,
+					task->meas_mv);
+		break;
+	default:
+		status = 0;
+	}
+
+	return status;
+}
+
+/*
+ * sets the av switch direction - audio-in vs video-out
+ */
+static void set_av_switch(struct ab8500_ad *dd,
+		enum accessory_avcontrol_dir dir)
+{
+	int ret;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter (%d)\n", __func__, dir);
+	if (dir == NOT_SET) {
+		ret = gpio_direction_input(dd->pdata->video_ctrl_gpio);
+		dd->gpio35_dir_set = 0;
+		ret = gpio_direction_output(dd->pdata->video_ctrl_gpio, 0);
+	} else if (!dd->gpio35_dir_set) {
+		ret = gpio_direction_output(dd->pdata->video_ctrl_gpio,
+						dir == AUDIO_IN ? 1 : 0);
+		if (ret < 0) {
+			dev_err(&dd->pdev->dev,
+				"%s: Output video ctrl signal failed (%d).\n",
+								__func__, ret);
+		} else {
+			dd->gpio35_dir_set = 1;
+			dev_dbg(&dd->pdev->dev, "AV-SWITCH: %s\n",
+				dir == AUDIO_IN ? "AUDIO_IN" : "VIDEO_OUT");
+		}
+	} else {
+		gpio_set_value(dd->pdata->video_ctrl_gpio,
+						dir == AUDIO_IN ? 1 : 0);
+	}
+}
+
+/*
+ * Tries to detect the currently attached accessory
+ */
+static enum accessory_jack_type detect(struct ab8500_ad *dd,
+			int *req_det_count)
+{
+	enum accessory_jack_type type = JACK_TYPE_DISCONNECTED;
+	int i;
+
+	accessory_regulator_enable(REGULATOR_VAUDIO | REGULATOR_AVSWITCH);
+
+	for (i = 0; i < ARRAY_SIZE(detect_ops); ++i) {
+		if (detect_hw(dd, &detect_ops[i])) {
+			type = detect_ops[i].type;
+			*req_det_count = detect_ops[i].req_det_count;
+			break;
+		}
+	}
+
+	config_hw_test_basic_carkit(dd, 0);
+	config_hw_test_plug_connected(dd, 0);
+
+	if (jack_supports_buttons(type))
+		accessory_regulator_enable(REGULATOR_VAMIC1);
+	else
+		accessory_regulator_disable(REGULATOR_VAMIC1 |
+						REGULATOR_AVSWITCH);
+
+	accessory_regulator_disable(REGULATOR_VAUDIO);
+
+	return type;
+}
+
+/*
+ * registers to specific interrupt
+ */
+static void claim_irq(struct ab8500_ad *dd, enum accessory_irq irq_id)
+{
+	int ret;
+	int irq;
+
+	if (dd->pdata->is_detection_inverted)
+		irq_desc = irq_desc_inverted;
+	else
+		irq_desc = irq_desc_norm;
+
+	if (irq_desc[irq_id].registered)
+		return;
+
+	irq = platform_get_irq_byname(
+			dd->pdev,
+			irq_desc[irq_id].name);
+	if (irq < 0) {
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to get irq %s\n", __func__,
+			irq_desc[irq_id].name);
+		return;
+	}
+
+	ret = request_threaded_irq(irq,
+				NULL,
+				irq_desc[irq_id].isr,
+				IRQF_NO_SUSPEND | IRQF_SHARED,
+				irq_desc[irq_id].name,
+				dd);
+	if (ret != 0) {
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to claim irq %s (%d)\n",
+			__func__,
+			irq_desc[irq_id].name,
+			ret);
+	} else {
+		irq_desc[irq_id].registered = 1;
+		dev_dbg(&dd->pdev->dev, "%s: %s\n",
+			__func__, irq_desc[irq_id].name);
+	}
+}
+
+/*
+ * releases specific interrupt
+ */
+static void release_irq(struct ab8500_ad *dd, enum accessory_irq irq_id)
+{
+	int irq;
+
+	if (dd->pdata->is_detection_inverted)
+		irq_desc = irq_desc_inverted;
+	else
+		irq_desc = irq_desc_norm;
+
+	if (!irq_desc[irq_id].registered)
+		return;
+
+	irq = platform_get_irq_byname(
+			dd->pdev,
+			irq_desc[irq_id].name);
+	if (irq < 0) {
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to get irq %s (%d)\n",
+			__func__,
+			irq_desc[irq_id].name, irq);
+	} else {
+		free_irq(irq, dd);
+		irq_desc[irq_id].registered = 0;
+		dev_dbg(&dd->pdev->dev, "%s: %s\n",
+			__func__, irq_desc[irq_id].name);
+	}
+}
+
+/*
+ * configures interrupts + detection hardware to meet the requirements
+ * set by currently attached accessory type.
+ */
+static void config_accdetect(struct ab8500_ad *dd)
+{
+	switch (dd->jack_type) {
+	case JACK_TYPE_UNSPECIFIED:
+		config_accdetect1_hw(dd, 1);
+		config_accdetect2_hw(dd, 0);
+
+		release_irq(dd, PLUG_IRQ);
+		release_irq(dd, UNPLUG_IRQ);
+		release_irq(dd, BUTTON_PRESS_IRQ);
+		release_irq(dd, BUTTON_RELEASE_IRQ);
+		set_av_switch(dd, NOT_SET);
+		break;
+
+	case JACK_TYPE_DISCONNECTED:
+		set_av_switch(dd, NOT_SET);
+	case JACK_TYPE_HEADPHONE:
+	case JACK_TYPE_CVIDEO:
+		config_accdetect1_hw(dd, 1);
+		config_accdetect2_hw(dd, 0);
+
+		claim_irq(dd, PLUG_IRQ);
+		claim_irq(dd, UNPLUG_IRQ);
+		release_irq(dd, BUTTON_PRESS_IRQ);
+		release_irq(dd, BUTTON_RELEASE_IRQ);
+		break;
+
+	case JACK_TYPE_CONNECTED:
+	case JACK_TYPE_HEADSET:
+	case JACK_TYPE_CARKIT:
+	case JACK_TYPE_OPENCABLE:
+		config_accdetect1_hw(dd, 1);
+		config_accdetect2_hw(dd, 1);
+
+		release_irq(dd, PLUG_IRQ);
+		claim_irq(dd, UNPLUG_IRQ);
+		claim_irq(dd, BUTTON_PRESS_IRQ);
+		claim_irq(dd, BUTTON_RELEASE_IRQ);
+		break;
+
+	default:
+		dev_err(&dd->pdev->dev, "%s: Unknown type: %d\n",
+			__func__, dd->jack_type);
+	}
+}
+
+/*
+ * Deferred initialization of the work.
+ */
+static void init_work(struct work_struct *work)
+{
+	struct ab8500_ad *dd = container_of(work,
+		struct ab8500_ad, init_work.work);
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter\n", __func__);
+
+	dd->jack_type = dd->reported_jack_type = JACK_TYPE_UNSPECIFIED;
+	config_accdetect(dd);
+	queue_delayed_work(dd->irq_work_queue,
+				&dd->detect_work,
+				msecs_to_jiffies(0));
+}
+
+/*
+ * performs platform device initialization
+ */
+static int ab8500_accessory_init(struct platform_device *pdev)
+{
+	struct ab8500_ad *dd;
+	struct ab8500_platform_data *plat;
+
+	dev_dbg(&pdev->dev, "Enter: %s\n", __func__);
+
+	dd = kzalloc(sizeof(struct ab8500_ad), GFP_KERNEL);
+	if (!dd) {
+		dev_err(&pdev->dev, "%s: Mem. alloc failed\n", __func__);
+		goto fail_no_mem_for_devdata;
+	}
+
+	dd->pdev = pdev;
+	dd->pdata = pdev->dev.platform_data;
+	plat = dev_get_platdata(pdev->dev.parent);
+
+	if (!plat || !plat->accdet) {
+		dev_err(&pdev->dev, "%s: Failed to get accdet plat data.\n",
+			__func__);
+		goto fail_no_ab8500_dev;
+	}
+	dd->pdata = plat->accdet;
+
+	if (dd->pdata->video_ctrl_gpio) {
+		if (!gpio_is_valid(dd->pdata->video_ctrl_gpio)) {
+			dev_err(&pdev->dev,
+				"%s: Video ctrl GPIO invalid (%d).\n", __func__,
+						dd->pdata->video_ctrl_gpio);
+			goto fail_video_ctrl_gpio;
+		}
+		if (gpio_request(dd->pdata->video_ctrl_gpio, "Video Control")) {
+			dev_err(&pdev->dev, "%s: Get video ctrl GPIO failed.\n",
+								__func__);
+			goto fail_video_ctrl_gpio;
+		}
+	}
+
+	if (create_btn_input_dev(dd) < 0) {
+		dev_err(&pdev->dev, "%s: create_button_input_dev failed.\n",
+			__func__);
+		goto fail_no_btn_input_dev;
+	}
+
+	if (create_regulators(dd) < 0) {
+		dev_err(&pdev->dev, "%s: failed to create regulators\n",
+			__func__);
+		goto fail_no_regulators;
+	}
+	dd->btn_state = BUTTON_UNK;
+
+	dd->irq_work_queue = create_singlethread_workqueue("ab8500_accdet_wq");
+	if (!dd->irq_work_queue) {
+		dev_err(&pdev->dev, "%s: Failed to create wq\n", __func__);
+		goto fail_no_mem_for_wq;
+	}
+	dd->gpadc = ab8500_gpadc_get();
+
+	INIT_DELAYED_WORK(&dd->detect_work, detect_work);
+	INIT_DELAYED_WORK(&dd->unplug_irq_work, unplug_irq_handler_work);
+	INIT_DELAYED_WORK(&dd->init_work, init_work);
+
+	/* Deferred init/detect since no use for the info early in boot */
+	queue_delayed_work(dd->irq_work_queue,
+				&dd->init_work,
+				msecs_to_jiffies(INIT_DELAY_MS));
+
+	platform_set_drvdata(pdev, dd);
+
+	return 0;
+
+fail_no_mem_for_wq:
+	free_regulators();
+fail_no_regulators:
+	input_unregister_device(dd->btn_input_dev);
+fail_no_btn_input_dev:
+	gpio_free(dd->pdata->video_ctrl_gpio);
+fail_video_ctrl_gpio:
+fail_no_ab8500_dev:
+	kfree(dd);
+fail_no_mem_for_devdata:
+
+	return -ENOMEM;
+}
+
+/*
+ * Performs platform device cleanup
+ */
+static void ab8500_accessory_cleanup(struct ab8500_ad *dd)
+{
+	dev_dbg(&dd->pdev->dev, "Enter: %s\n", __func__);
+
+	dd->jack_type = JACK_TYPE_UNSPECIFIED;
+	config_accdetect(dd);
+
+	gpio_free(dd->pdata->video_ctrl_gpio);
+	input_unregister_device(dd->btn_input_dev);
+	free_regulators();
+
+	cancel_delayed_work(&dd->detect_work);
+	cancel_delayed_work(&dd->unplug_irq_work);
+	cancel_delayed_work(&dd->init_work);
+	flush_workqueue(dd->irq_work_queue);
+	destroy_workqueue(dd->irq_work_queue);
+
+	kfree(dd);
+}
+
+static int __devinit ab8500_acc_detect_probe(struct platform_device *pdev)
+{
+	return ab8500_accessory_init(pdev);
+}
+
+
+static int __devexit ab8500_acc_detect_remove(struct platform_device *pdev)
+{
+	ab8500_accessory_cleanup(platform_get_drvdata(pdev));
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+#if defined(CONFIG_PM)
+static u8 acc_det_ctrl_suspend_val;
+
+static int ab8500_acc_detect_suspend(struct platform_device *pdev,
+			pm_message_t state)
+{
+	struct ab8500_ad *dd = platform_get_drvdata(pdev);
+	int irq_id, irq;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter\n", __func__);
+
+	cancel_delayed_work_sync(&dd->unplug_irq_work);
+	cancel_delayed_work_sync(&dd->detect_work);
+	cancel_delayed_work_sync(&dd->init_work);
+
+	if (dd->pdata->is_detection_inverted)
+		irq_desc = irq_desc_inverted;
+	else
+		irq_desc = irq_desc_norm;
+
+	for (irq_id = 0; irq_id < ARRAY_SIZE(irq_desc_norm); irq_id++) {
+		if (irq_desc[irq_id].registered == 1) {
+			irq = platform_get_irq_byname(
+					dd->pdev,
+					irq_desc[irq_id].name);
+
+			disable_irq(irq);
+		}
+	}
+
+	/* Turn off AccDetect comparators and pull-up */
+	(void) abx500_get_register_interruptible(
+			&dd->pdev->dev,
+			AB8500_ECI_AV_ACC,
+			AB8500_ACC_DET_CTRL_REG,
+			&acc_det_ctrl_suspend_val);
+	(void) abx500_set_register_interruptible(
+			&dd->pdev->dev,
+			AB8500_ECI_AV_ACC,
+			AB8500_ACC_DET_CTRL_REG,
+			0);
+	return 0;
+}
+
+static int ab8500_acc_detect_resume(struct platform_device *pdev)
+{
+	struct ab8500_ad *dd = platform_get_drvdata(pdev);
+	int irq_id, irq;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter\n", __func__);
+
+	/* Turn on AccDetect comparators and pull-up */
+	(void) abx500_set_register_interruptible(
+			&dd->pdev->dev,
+			AB8500_ECI_AV_ACC,
+			AB8500_ACC_DET_CTRL_REG,
+			acc_det_ctrl_suspend_val);
+
+	if (dd->pdata->is_detection_inverted)
+		irq_desc = irq_desc_inverted;
+	else
+		irq_desc = irq_desc_norm;
+
+	for (irq_id = 0; irq_id < ARRAY_SIZE(irq_desc_norm); irq_id++) {
+		if (irq_desc[irq_id].registered == 1) {
+			irq = platform_get_irq_byname(
+					dd->pdev,
+					irq_desc[irq_id].name);
+
+			enable_irq(irq);
+
+		}
+	}
+
+	/* After resume, reinitialize */
+	dd->gpio35_dir_set = dd->accdet1_th_set = dd->accdet2_th_set = 0;
+	queue_delayed_work(dd->irq_work_queue, &dd->init_work, 0);
+
+	return 0;
+}
+#else
+#define ab8500_acc_detect_suspend	NULL
+#define ab8500_acc_detect_resume	NULL
+#endif
+
+static struct platform_driver ab8500_acc_detect_platform_driver = {
+	.driver = {
+		.name = "ab8500-acc-det",
+		.owner = THIS_MODULE,
+	},
+	.probe = ab8500_acc_detect_probe,
+	.remove	= __devexit_p(ab8500_acc_detect_remove),
+	.suspend = ab8500_acc_detect_suspend,
+	.resume = ab8500_acc_detect_resume,
+};
+
+static int __init ab8500_acc_detect_init(void)
+{
+	return platform_driver_register(&ab8500_acc_detect_platform_driver);
+}
+
+static void __exit ab8500_acc_detect_exit(void)
+{
+	platform_driver_unregister(&ab8500_acc_detect_platform_driver);
+}
+
+module_init(ab8500_acc_detect_init);
+module_exit(ab8500_acc_detect_exit);
+
+MODULE_DESCRIPTION("AB8500 AV Accessory detection driver");
+MODULE_ALIAS("platform:ab8500-acc-det");
+MODULE_AUTHOR("ST-Ericsson");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/misc/ab8500-ponkey.c b/drivers/input/misc/ab8500-ponkey.c
index 3d3288a78fd..39964956de2 100644
--- a/drivers/input/misc/ab8500-ponkey.c
+++ b/drivers/input/misc/ab8500-ponkey.c
@@ -6,25 +6,36 @@
  *
  * AB8500 Power-On Key handler
  */
-
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/input.h>
 #include <linux/interrupt.h>
-#include <linux/mfd/ab8500.h>
 #include <linux/slab.h>
 
+struct ab8500_ponkey_variant {
+	const char *irq_falling;
+	const char *irq_rising;
+};
+
+static const struct ab8500_ponkey_variant ab5500_onswa = {
+	.irq_falling	= "ONSWAn_falling",
+	.irq_rising	= "ONSWAn_rising",
+};
+
+static const struct ab8500_ponkey_variant ab8500_ponkey = {
+	.irq_falling	= "ONKEY_DBF",
+	.irq_rising	= "ONKEY_DBR",
+};
+
 /**
- * struct ab8500_ponkey - ab8500 ponkey information
+ * struct ab8500_ponkey_info - ab8500 ponkey information
  * @input_dev: pointer to input device
- * @ab8500: ab8500 parent
  * @irq_dbf: irq number for falling transition
  * @irq_dbr: irq number for rising transition
  */
-struct ab8500_ponkey {
+struct ab8500_ponkey_info {
 	struct input_dev	*idev;
-	struct ab8500		*ab8500;
 	int			irq_dbf;
 	int			irq_dbr;
 };
@@ -32,110 +43,124 @@ struct ab8500_ponkey {
 /* AB8500 gives us an interrupt when ONKEY is held */
 static irqreturn_t ab8500_ponkey_handler(int irq, void *data)
 {
-	struct ab8500_ponkey *ponkey = data;
+	struct ab8500_ponkey_info *info = data;
 
-	if (irq == ponkey->irq_dbf)
-		input_report_key(ponkey->idev, KEY_POWER, true);
-	else if (irq == ponkey->irq_dbr)
-		input_report_key(ponkey->idev, KEY_POWER, false);
+	if (irq == info->irq_dbf)
+		input_report_key(info->idev, KEY_POWER, true);
+	else if (irq == info->irq_dbr)
+		input_report_key(info->idev, KEY_POWER, false);
 
-	input_sync(ponkey->idev);
+	input_sync(info->idev);
 
 	return IRQ_HANDLED;
 }
 
 static int __devinit ab8500_ponkey_probe(struct platform_device *pdev)
 {
-	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
-	struct ab8500_ponkey *ponkey;
-	struct input_dev *input;
-	int irq_dbf, irq_dbr;
-	int error;
+	const struct ab8500_ponkey_variant *variant;
+	struct ab8500_ponkey_info *info;
+	int irq_dbf, irq_dbr, ret;
+
+	variant = (const struct ab8500_ponkey_variant *)
+		  pdev->id_entry->driver_data;
 
-	irq_dbf = platform_get_irq_byname(pdev, "ONKEY_DBF");
+	irq_dbf = platform_get_irq_byname(pdev, variant->irq_falling);
 	if (irq_dbf < 0) {
-		dev_err(&pdev->dev, "No IRQ for ONKEY_DBF, error=%d\n", irq_dbf);
+		dev_err(&pdev->dev, "No IRQ for %s: %d\n",
+			variant->irq_falling, irq_dbf);
 		return irq_dbf;
 	}
 
-	irq_dbr = platform_get_irq_byname(pdev, "ONKEY_DBR");
+	irq_dbr = platform_get_irq_byname(pdev, variant->irq_rising);
 	if (irq_dbr < 0) {
-		dev_err(&pdev->dev, "No IRQ for ONKEY_DBR, error=%d\n", irq_dbr);
+		dev_err(&pdev->dev, "No IRQ for %s: %d\n",
+			variant->irq_rising, irq_dbr);
 		return irq_dbr;
 	}
 
-	ponkey = kzalloc(sizeof(struct ab8500_ponkey), GFP_KERNEL);
-	input = input_allocate_device();
-	if (!ponkey || !input) {
-		error = -ENOMEM;
-		goto err_free_mem;
-	}
+	info = kzalloc(sizeof(struct ab8500_ponkey_info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
 
-	ponkey->idev = input;
-	ponkey->ab8500 = ab8500;
-	ponkey->irq_dbf = irq_dbf;
-	ponkey->irq_dbr = irq_dbr;
+	info->irq_dbf = irq_dbf;
+	info->irq_dbr = irq_dbr;
 
-	input->name = "AB8500 POn(PowerOn) Key";
-	input->dev.parent = &pdev->dev;
+	info->idev = input_allocate_device();
+	if (!info->idev) {
+		dev_err(&pdev->dev, "Failed to allocate input dev\n");
+		ret = -ENOMEM;
+		goto out;
+	}
 
-	input_set_capability(input, EV_KEY, KEY_POWER);
+	info->idev->name = "AB8500 POn(PowerOn) Key";
+	info->idev->dev.parent = &pdev->dev;
+	info->idev->evbit[0] = BIT_MASK(EV_KEY);
+	info->idev->keybit[BIT_WORD(KEY_POWER)] = BIT_MASK(KEY_POWER);
 
-	error = request_any_context_irq(ponkey->irq_dbf, ab8500_ponkey_handler,
-					0, "ab8500-ponkey-dbf", ponkey);
-	if (error < 0) {
-		dev_err(ab8500->dev, "Failed to request dbf IRQ#%d: %d\n",
-			ponkey->irq_dbf, error);
-		goto err_free_mem;
+	ret = input_register_device(info->idev);
+	if (ret) {
+		dev_err(&pdev->dev, "Can't register input device: %d\n", ret);
+		goto out_unfreedevice;
 	}
 
-	error = request_any_context_irq(ponkey->irq_dbr, ab8500_ponkey_handler,
-					0, "ab8500-ponkey-dbr", ponkey);
-	if (error < 0) {
-		dev_err(ab8500->dev, "Failed to request dbr IRQ#%d: %d\n",
-			ponkey->irq_dbr, error);
-		goto err_free_dbf_irq;
+	ret = request_threaded_irq(info->irq_dbf, NULL, ab8500_ponkey_handler,
+					IRQF_NO_SUSPEND, "ab8500-ponkey-dbf",
+					info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to request dbf IRQ#%d: %d\n",
+				info->irq_dbf, ret);
+		goto out_unregisterdevice;
 	}
 
-	error = input_register_device(ponkey->idev);
-	if (error) {
-		dev_err(ab8500->dev, "Can't register input device: %d\n", error);
-		goto err_free_dbr_irq;
+	ret = request_threaded_irq(info->irq_dbr, NULL, ab8500_ponkey_handler,
+					IRQF_NO_SUSPEND, "ab8500-ponkey-dbr",
+					info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to request dbr IRQ#%d: %d\n",
+				info->irq_dbr, ret);
+		goto out_irq_dbf;
 	}
 
-	platform_set_drvdata(pdev, ponkey);
-	return 0;
+	platform_set_drvdata(pdev, info);
 
-err_free_dbr_irq:
-	free_irq(ponkey->irq_dbr, ponkey);
-err_free_dbf_irq:
-	free_irq(ponkey->irq_dbf, ponkey);
-err_free_mem:
-	input_free_device(input);
-	kfree(ponkey);
+	return 0;
 
-	return error;
+out_irq_dbf:
+	free_irq(info->irq_dbf, info);
+out_unregisterdevice:
+	input_unregister_device(info->idev);
+	info->idev = NULL;
+out_unfreedevice:
+	input_free_device(info->idev);
+out:
+	kfree(info);
+	return ret;
 }
 
 static int __devexit ab8500_ponkey_remove(struct platform_device *pdev)
 {
-	struct ab8500_ponkey *ponkey = platform_get_drvdata(pdev);
-
-	free_irq(ponkey->irq_dbf, ponkey);
-	free_irq(ponkey->irq_dbr, ponkey);
-	input_unregister_device(ponkey->idev);
-	kfree(ponkey);
-
-	platform_set_drvdata(pdev, NULL);
+	struct ab8500_ponkey_info *info = platform_get_drvdata(pdev);
 
+	free_irq(info->irq_dbf, info);
+	free_irq(info->irq_dbr, info);
+	input_unregister_device(info->idev);
+	kfree(info);
 	return 0;
 }
 
+static struct platform_device_id ab8500_ponkey_id_table[] = {
+	{ "ab5500-onswa", (kernel_ulong_t)&ab5500_onswa, },
+	{ "ab8500-poweron-key", (kernel_ulong_t)&ab8500_ponkey, },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, ab8500_ponkey_id_table);
+
 static struct platform_driver ab8500_ponkey_driver = {
 	.driver		= {
 		.name	= "ab8500-poweron-key",
 		.owner	= THIS_MODULE,
 	},
+	.id_table	= ab8500_ponkey_id_table,
 	.probe		= ab8500_ponkey_probe,
 	.remove		= __devexit_p(ab8500_ponkey_remove),
 };
diff --git a/drivers/input/misc/lps001wp_prs.c b/drivers/input/misc/lps001wp_prs.c
new file mode 100644
index 00000000000..9ec96ba3863
--- /dev/null
+++ b/drivers/input/misc/lps001wp_prs.c
@@ -0,0 +1,1276 @@
+
+/******************** (C) COPYRIGHT 2010 STMicroelectronics ********************
+*
+* File Name	: lps001wp_prs.c
+* Authors	: MSH - Motion Mems BU - Application Team
+*		: Matteo Dameno (matteo.dameno@st.com)
+*		: Carmine Iascone (carmine.iascone@st.com)
+*		: Both authors are willing to be considered the contact
+*		: and update points for the driver.
+* Version	: V 1.1.1
+* Date		: 2010/11/22
+* Description	: LPS001WP pressure temperature sensor driver
+*
+********************************************************************************
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* THE PRESENT SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES
+* OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, FOR THE SOLE
+* PURPOSE TO SUPPORT YOUR APPLICATION DEVELOPMENT.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*
+******************************************************************************
+
+ Revision 0.9.0 01/10/2010:
+	first beta release
+ Revision 1.1.0 05/11/2010:
+	add sysfs management
+ Revision 1.1.1 22/11/2010:
+	moved to input/misc
+******************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/device.h>
+
+#include <linux/input/lps001wp.h>
+
+
+
+#define	DEBUG	1
+
+
+#define	PR_ABS_MAX	 0xffff
+#define	PR_ABS_MIN	 0x0000
+#define	PR_DLT_MAX	 0x7ffff
+#define	PR_DLT_MIN	-0x80000 /* 16-bit signed value */
+#define	TEMP_MAX	 0x7fff
+#define TEMP_MIN	-0x80000 /* 16-bit signed value */
+
+
+#define	SENSITIVITY_T_SHIFT	6	/** =	64 LSB/degrC	*/
+#define	SENSITIVITY_P_SHIFT	4	/** =	16 LSB/mbar	*/
+
+
+#define	OUTDATA_REG	0x28
+#define	INDATA_REG	0X30
+
+#define	WHOAMI_LPS001WP_PRS	0xBA	/*	Expctd content for WAI	*/
+
+/*	CONTROL REGISTERS	*/
+#define	WHO_AM_I	0x0F		/*	WhoAmI register		*/
+#define	CTRL_REG1	0x20		/*	power / ODR control reg	*/
+#define	CTRL_REG2	0x21		/*	boot reg		*/
+#define	CTRL_REG3	0x22		/*	interrupt control reg	*/
+
+#define	STATUS_REG	0X27		/*	status reg		*/
+
+#define PRESS_OUT_L	OUTDATA_REG
+
+
+#define	REF_P_L		INDATA_REG	/*	pressure reference	*/
+#define	REF_P_H		0x31		/*	pressure reference	*/
+#define	THS_P_L		0x32		/*	pressure threshold	*/
+#define	THS_P_H		0x33		/*	pressure threshold	*/
+
+#define	INT_CFG		0x34		/*	interrupt config	*/
+#define	INT_SRC		0x35		/*	interrupt source	*/
+#define	INT_ACK		0x36		/*	interrupt acknoledge	*/
+/*	end CONTROL REGISTRES	*/
+
+
+/* Barometer and Termometer output data rate ODR */
+#define	LPS001WP_PRS_ODR_MASK	0x30	/* Mask to access odr bits only	*/
+#define	LPS001WP_PRS_ODR_7_1	0x00	/* 7Hz baro and 1Hz term ODR	*/
+#define	LPS001WP_PRS_ODR_7_7	0x01	/* 7Hz baro and 7Hz term ODR	*/
+#define	LPS001WP_PRS_ODR_12_12	0x11	/* 12.5Hz baro and 12.5Hz term ODR */
+
+
+#define	LPS001WP_PRS_ENABLE_MASK	0x40	/*  */
+#define	LPS001WP_PRS_DIFF_MASK		0x08
+#define LPS001WP_PRS_LPOW_MASK		0x80
+
+#define	LPS001WP_PRS_DIFF_ON		0x08
+#define	LPS001WP_PRS_DIFF_OFF		0x00
+
+#define	LPS001WP_PRS_LPOW_ON		0x80
+#define	LPS001WP_PRS_LPOW_OFF		0x00
+
+#define	FUZZ			0
+#define	FLAT			0
+#define	I2C_RETRY_DELAY		5
+#define	I2C_RETRIES		5
+#define	I2C_AUTO_INCREMENT	0x80
+
+/* RESUME STATE INDICES */
+#define	RES_CTRL_REG1		0
+#define	RES_CTRL_REG2		1
+#define	RES_CTRL_REG3		2
+#define	RES_REF_P_L		3
+#define	RES_REF_P_H		4
+#define	RES_THS_P_L		5
+#define	RES_THS_P_H		6
+#define	RES_INT_CFG		7
+
+#define	RESUME_ENTRIES		8
+/* end RESUME STATE INDICES */
+
+/* Pressure Sensor Operating Mode */
+#define	LPS001WP_PRS_DIFF_ENABLE	1
+#define LPS001WP_PRS_DIFF_DISABLE	0
+#define	LPS001WP_PRS_LPOWER_EN		1
+#define	LPS001WP_PRS_LPOWER_DIS		0
+
+static const struct {
+	unsigned int cutoff_ms;
+	unsigned int mask;
+} lps001wp_prs_odr_table[] = {
+		{80,	LPS001WP_PRS_ODR_12_12 },
+		{143,	LPS001WP_PRS_ODR_7_7 },
+		{1000,	LPS001WP_PRS_ODR_7_1 },
+};
+
+struct lps001wp_prs_data {
+	struct i2c_client *client;
+	struct lps001wp_prs_platform_data *pdata;
+
+	struct mutex lock;
+	struct delayed_work input_work;
+
+	struct input_dev *input_dev;
+
+	int hw_initialized;
+	/* hw_working=-1 means not tested yet */
+	int hw_working;
+	u8 diff_enabled;
+	u8 lpowmode_enabled ;
+
+	atomic_t enabled;
+	int on_before_suspend;
+
+	u8 resume_state[RESUME_ENTRIES];
+
+#ifdef DEBUG
+	u8 reg_addr;
+#endif
+};
+
+struct outputdata {
+	u16 abspress;
+	s16 temperature;
+	s16 deltapress;
+};
+
+
+static int lps001wp_prs_i2c_read(struct lps001wp_prs_data *prs,
+				  u8 *buf, int len)
+{
+	int err;
+	int tries = 0;
+
+	struct i2c_msg msgs[] = {
+		{
+		 .addr = prs->client->addr,
+		 .flags = prs->client->flags & I2C_M_TEN,
+		 .len = 1,
+		 .buf = buf,
+		 },
+		{
+		 .addr = prs->client->addr,
+		 .flags = (prs->client->flags & I2C_M_TEN) | I2C_M_RD,
+		 .len = len,
+		 .buf = buf,
+		 },
+	};
+
+	do {
+		err = i2c_transfer(prs->client->adapter, msgs, 2);
+		if (err != 2)
+			msleep_interruptible(I2C_RETRY_DELAY);
+	} while ((err != 2) && (++tries < I2C_RETRIES));
+
+	if (err != 2) {
+		dev_err(&prs->client->dev, "read transfer error\n");
+		err = -EIO;
+	} else {
+		err = 0;
+	}
+
+	return err;
+}
+
+static int lps001wp_prs_i2c_write(struct lps001wp_prs_data *prs,
+				   u8 *buf, int len)
+{
+	int err;
+	int tries = 0;
+	struct i2c_msg msgs[] = {
+		{
+		 .addr = prs->client->addr,
+		 .flags = prs->client->flags & I2C_M_TEN,
+		 .len = len + 1,
+		 .buf = buf,
+		 },
+	};
+
+	do {
+		err = i2c_transfer(prs->client->adapter, msgs, 1);
+		if (err != 1)
+			msleep_interruptible(I2C_RETRY_DELAY);
+	} while ((err != 1) && (++tries < I2C_RETRIES));
+
+	if (err != 1) {
+		dev_err(&prs->client->dev, "write transfer error\n");
+		err = -EIO;
+	} else {
+		err = 0;
+	}
+
+	return err;
+}
+
+static int lps001wp_prs_i2c_update(struct lps001wp_prs_data *prs,
+			u8 reg_address, u8 mask, u8 new_bit_values)
+{
+	int err = -1;
+	u8 rdbuf[1] = { reg_address };
+	u8 wrbuf[2] = { reg_address , 0x00 };
+
+	u8 init_val;
+	u8 updated_val;
+	err = lps001wp_prs_i2c_read(prs, rdbuf, 1);
+	if (!(err < 0)) {
+		init_val = rdbuf[0];
+		updated_val = ((mask & new_bit_values) | ((~mask) & init_val));
+		wrbuf[1] = updated_val;
+		err = lps001wp_prs_i2c_write(prs, wrbuf, 2);
+	}
+	return err;
+}
+/* */
+
+static int lps001wp_prs_register_write(struct lps001wp_prs_data *prs, u8 *buf,
+		u8 reg_address, u8 new_value)
+{
+	int err = -1;
+
+	/* Sets configuration register at reg_address
+	 *  NOTE: this is a straight overwrite  */
+	buf[0] = reg_address;
+	buf[1] = new_value;
+	err = lps001wp_prs_i2c_write(prs, buf, 1);
+	if (err < 0)
+		return err;
+	return err;
+}
+
+static int lps001wp_prs_register_read(struct lps001wp_prs_data *prs, u8 *buf,
+		u8 reg_address)
+{
+
+	int err = -1;
+	buf[0] = (reg_address);
+	err = lps001wp_prs_i2c_read(prs, buf, 1);
+
+	return err;
+}
+
+static int lps001wp_prs_register_update(struct lps001wp_prs_data *prs, u8 *buf,
+		u8 reg_address, u8 mask, u8 new_bit_values)
+{
+	int err = -1;
+	u8 init_val;
+	u8 updated_val;
+	err = lps001wp_prs_register_read(prs, buf, reg_address);
+	if (!(err < 0)) {
+		init_val = buf[0];
+		updated_val = ((mask & new_bit_values) | ((~mask) & init_val));
+		err = lps001wp_prs_register_write(prs, buf, reg_address,
+				updated_val);
+	}
+	return err;
+}
+
+/* */
+
+
+static int lps001wp_prs_hw_init(struct lps001wp_prs_data *prs)
+{
+	int err = -1;
+	u8 buf[6];
+
+	printk(KERN_DEBUG "%s: hw init start\n", LPS001WP_PRS_DEV_NAME);
+
+	buf[0] = WHO_AM_I;
+	err = lps001wp_prs_i2c_read(prs, buf, 1);
+	if (err < 0)
+		goto error_firstread;
+	else
+		prs->hw_working = 1;
+	if (buf[0] != WHOAMI_LPS001WP_PRS) {
+		err = -1; /* TODO:choose the right coded error */
+		goto error_unknown_device;
+	}
+
+
+	buf[0] = (I2C_AUTO_INCREMENT | INDATA_REG);
+	buf[1] = prs->resume_state[RES_REF_P_L];
+	buf[2] = prs->resume_state[RES_REF_P_H];
+	buf[3] = prs->resume_state[RES_THS_P_L];
+	buf[4] = prs->resume_state[RES_THS_P_H];
+	err = lps001wp_prs_i2c_write(prs, buf, 4);
+	if (err < 0)
+		goto error1;
+
+	buf[0] = (I2C_AUTO_INCREMENT | CTRL_REG1);
+	buf[1] = prs->resume_state[RES_CTRL_REG1];
+	buf[2] = prs->resume_state[RES_CTRL_REG2];
+	buf[3] = prs->resume_state[RES_CTRL_REG3];
+	err = lps001wp_prs_i2c_write(prs, buf, 3);
+	if (err < 0)
+		goto error1;
+
+	buf[0] = INT_CFG;
+	buf[1] = prs->resume_state[RES_INT_CFG];
+	err = lps001wp_prs_i2c_write(prs, buf, 1);
+	if (err < 0)
+		goto error1;
+
+
+	prs->hw_initialized = 1;
+	printk(KERN_DEBUG "%s: hw init done\n", LPS001WP_PRS_DEV_NAME);
+	return 0;
+
+error_firstread:
+	prs->hw_working = 0;
+	dev_warn(&prs->client->dev, "Error reading WHO_AM_I: is device "
+		"available/working?\n");
+	goto error1;
+error_unknown_device:
+	dev_err(&prs->client->dev,
+		"device unknown. Expected: 0x%x,"
+		" Replies: 0x%x\n", WHOAMI_LPS001WP_PRS, buf[0]);
+error1:
+	prs->hw_initialized = 0;
+	dev_err(&prs->client->dev, "hw init error 0x%x,0x%x: %d\n", buf[0],
+			buf[1], err);
+	return err;
+}
+
+static void lps001wp_prs_device_power_off(struct lps001wp_prs_data *prs)
+{
+	int err;
+	u8 buf[2] = { CTRL_REG1, LPS001WP_PRS_PM_OFF };
+
+	err = lps001wp_prs_i2c_write(prs, buf, 1);
+	if (err < 0)
+		dev_err(&prs->client->dev, "soft power off failed: %d\n", err);
+
+	if (prs->pdata->power_off) {
+		prs->pdata->power_off();
+		prs->hw_initialized = 0;
+	}
+	if (prs->hw_initialized) {
+		prs->hw_initialized = 0;
+	}
+
+}
+
+static int lps001wp_prs_device_power_on(struct lps001wp_prs_data *prs)
+{
+	int err = -1;
+
+	if (prs->pdata->power_on) {
+		err = prs->pdata->power_on();
+		if (err < 0) {
+			dev_err(&prs->client->dev,
+					"power_on failed: %d\n", err);
+			return err;
+		}
+	}
+
+	if (!prs->hw_initialized) {
+		err = lps001wp_prs_hw_init(prs);
+		if (prs->hw_working == 1 && err < 0) {
+			lps001wp_prs_device_power_off(prs);
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+
+
+int lps001wp_prs_update_odr(struct lps001wp_prs_data *prs, int poll_interval_ms)
+{
+	int err = -1;
+	int i;
+
+	u8 buf[2];
+	u8 updated_val;
+	u8 init_val;
+	u8 new_val;
+	u8 mask = LPS001WP_PRS_ODR_MASK;
+
+	/* Following, looks for the longest possible odr interval scrolling the
+	 * odr_table vector from the end (shortest interval) backward (longest
+	 * interval), to support the poll_interval requested by the system.
+	 * It must be the longest interval lower then the poll interval.*/
+	for (i = ARRAY_SIZE(lps001wp_prs_odr_table) - 1; i >= 0; i--) {
+		if (lps001wp_prs_odr_table[i].cutoff_ms <= poll_interval_ms)
+			break;
+	}
+
+	new_val = lps001wp_prs_odr_table[i].mask;
+
+	/* If device is currently enabled, we need to write new
+	 *  configuration out to it */
+	if (atomic_read(&prs->enabled)) {
+		buf[0] = CTRL_REG1;
+		err = lps001wp_prs_i2c_read(prs, buf, 1);
+		if (err < 0)
+			goto error;
+		init_val = buf[0];
+		prs->resume_state[RES_CTRL_REG1] = init_val;
+
+		buf[0] = CTRL_REG1;
+		updated_val = ((mask & new_val) | ((~mask) & init_val));
+		buf[1] = updated_val;
+		buf[0] = CTRL_REG1;
+		err = lps001wp_prs_i2c_write(prs, buf, 1);
+		if (err < 0)
+			goto error;
+		prs->resume_state[RES_CTRL_REG1] = updated_val;
+	}
+	return err;
+
+error:
+	dev_err(&prs->client->dev, "update odr failed 0x%x,0x%x: %d\n",
+			buf[0], buf[1], err);
+
+	return err;
+}
+
+static int lps001wp_prs_set_press_reference(struct lps001wp_prs_data *prs,
+				u16 new_reference)
+{
+	int err = -1;
+	u8 const reg_addressL = REF_P_L;
+	u8 const reg_addressH = REF_P_H;
+	u8 bit_valuesL, bit_valuesH;
+	u8 buf[2];
+
+	bit_valuesL = (u8) (new_reference & 0x00FF);
+	bit_valuesH = (u8)((new_reference & 0xFF00) >> 8);
+
+	err = lps001wp_prs_register_write(prs, buf, reg_addressL,
+			bit_valuesL);
+	if (err < 0)
+		return err;
+	err = lps001wp_prs_register_write(prs, buf, reg_addressH,
+			bit_valuesH);
+	if (err < 0) {
+		lps001wp_prs_register_write(prs, buf, reg_addressL,
+				prs->resume_state[RES_REF_P_L]);
+		return err;
+	}
+	prs->resume_state[RES_REF_P_L] = bit_valuesL;
+	prs->resume_state[RES_REF_P_H] = bit_valuesH;
+	return err;
+}
+
+static int lps001wp_prs_get_press_reference(struct lps001wp_prs_data *prs,
+		u16 *buf16)
+{
+	int err = -1;
+
+	u8 bit_valuesL, bit_valuesH;
+	u8 buf[2] = {0};
+	u16 temp = 0;
+
+	err = lps001wp_prs_register_read(prs, buf, REF_P_L);
+	if (err < 0)
+		return err;
+	bit_valuesL = buf[0];
+	err = lps001wp_prs_register_read(prs, buf, REF_P_H);
+	if (err < 0)
+		return err;
+	bit_valuesH = buf[0];
+
+	temp = (((u16) bit_valuesH) << 8);
+	*buf16 = (temp | ((u16) bit_valuesL));
+
+	return err;
+}
+
+static int lps001wp_prs_lpow_manage(struct lps001wp_prs_data *prs, u8 control)
+{
+	int err = -1;
+	u8 buf[2] = {0x00, 0x00};
+	u8 const mask = LPS001WP_PRS_LPOW_MASK;
+	u8 bit_values = LPS001WP_PRS_LPOW_OFF;
+
+	if (control >= LPS001WP_PRS_LPOWER_EN) {
+		bit_values = LPS001WP_PRS_LPOW_ON;
+	}
+
+	err = lps001wp_prs_register_update(prs, buf, CTRL_REG1,
+			mask, bit_values);
+
+	if (err < 0)
+		return err;
+	prs->resume_state[RES_CTRL_REG1] = ((mask & bit_values) |
+			(~mask & prs->resume_state[RES_CTRL_REG1]));
+	if (bit_values == LPS001WP_PRS_LPOW_ON)
+		prs->lpowmode_enabled = 1;
+	else
+		prs->lpowmode_enabled = 0;
+	return err;
+}
+
+static int lps001wp_prs_diffen_manage(struct lps001wp_prs_data *prs, u8 control)
+{
+	int err = -1;
+	u8 buf[2] = {0x00, 0x00};
+	u8 const mask = LPS001WP_PRS_DIFF_MASK;
+	u8 bit_values = LPS001WP_PRS_DIFF_OFF;
+
+	if (control >= LPS001WP_PRS_DIFF_ENABLE) {
+		bit_values = LPS001WP_PRS_DIFF_ON;
+	}
+
+	err = lps001wp_prs_register_update(prs, buf, CTRL_REG1,
+			mask, bit_values);
+
+	if (err < 0)
+		return err;
+	prs->resume_state[RES_CTRL_REG1] = ((mask & bit_values) |
+			(~mask & prs->resume_state[RES_CTRL_REG1]));
+	if (bit_values == LPS001WP_PRS_DIFF_ON)
+		prs->diff_enabled = 1;
+	else
+		prs->diff_enabled = 0;
+	return err;
+}
+
+
+static int lps001wp_prs_get_presstemp_data(struct lps001wp_prs_data *prs,
+						struct outputdata *out)
+{
+	int err = -1;
+	/* Data bytes from hardware	PRESS_OUT_L, PRESS_OUT_H,
+	 *				TEMP_OUT_L, TEMP_OUT_H,
+	 *				DELTA_L, DELTA_H */
+	u8 prs_data[6];
+
+	u16 abspr;
+	s16 temperature, deltapr;
+	int regToRead = 4;
+	prs_data[4] = 0;
+	prs_data[5] = 0;
+
+	if (prs->diff_enabled)
+		regToRead = 6;
+
+	prs_data[0] = (I2C_AUTO_INCREMENT | OUTDATA_REG);
+	err = lps001wp_prs_i2c_read(prs, prs_data, regToRead);
+	if (err < 0)
+		return err;
+
+	abspr = ((((u16) prs_data[1] << 8) | ((u16) prs_data[0])));
+	temperature = ((s16) (((u16) prs_data[3] << 8) | ((u16)prs_data[2])));
+
+	out->abspress = abspr;
+	out->temperature = temperature;
+
+	deltapr = ((s16) (((u16) prs_data[5] << 8) | ((u16)prs_data[4])));
+	out->deltapress = deltapr;
+
+	return err;
+}
+
+static void lps001wp_prs_report_values(struct lps001wp_prs_data *prs,
+					struct outputdata *out)
+{
+	input_report_abs(prs->input_dev, ABS_PR, out->abspress);
+	input_report_abs(prs->input_dev, ABS_TEMP, out->temperature);
+	input_report_abs(prs->input_dev, ABS_DLTPR, out->deltapress);
+	input_sync(prs->input_dev);
+}
+
+static int lps001wp_prs_enable(struct lps001wp_prs_data *prs)
+{
+	int err;
+
+	if (!atomic_cmpxchg(&prs->enabled, 0, 1)) {
+		err = lps001wp_prs_device_power_on(prs);
+		if (err < 0) {
+			atomic_set(&prs->enabled, 0);
+			return err;
+		}
+		schedule_delayed_work(&prs->input_work,
+			msecs_to_jiffies(prs->pdata->poll_interval));
+	}
+
+	return 0;
+}
+
+static int lps001wp_prs_disable(struct lps001wp_prs_data *prs)
+{
+	if (atomic_cmpxchg(&prs->enabled, 1, 0)) {
+		cancel_delayed_work_sync(&prs->input_work);
+		lps001wp_prs_device_power_off(prs);
+	}
+
+	return 0;
+}
+
+static ssize_t read_single_reg(struct device *dev, char *buf, u8 reg)
+{
+	ssize_t ret;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	int rc = 0;
+
+	u8 data = reg;
+	rc = lps001wp_prs_i2c_read(prs, &data, 1);
+	/*TODO: error need to be managed */
+	ret = sprintf(buf, "0x%02x\n", data);
+	return ret;
+
+}
+
+static int write_reg(struct device *dev, const char *buf, u8 reg)
+{
+	int rc = 0;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	u8 x[2];
+	unsigned long val;
+
+	if (strict_strtoul(buf, 16, &val))
+		return -EINVAL;
+
+	x[0] = reg;
+	x[1] = val;
+	rc = lps001wp_prs_i2c_write(prs, x, 1);
+	/*TODO: error need to be managed */
+	return rc;
+}
+
+static ssize_t attr_get_polling_rate(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	int val;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	mutex_lock(&prs->lock);
+	val = prs->pdata->poll_interval;
+	mutex_unlock(&prs->lock);
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t attr_set_polling_rate(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t size)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long interval_ms;
+
+	if (strict_strtoul(buf, 10, &interval_ms))
+		return -EINVAL;
+	if (!interval_ms)
+		return -EINVAL;
+	mutex_lock(&prs->lock);
+	prs->pdata->poll_interval = interval_ms;
+	lps001wp_prs_update_odr(prs, interval_ms);
+	mutex_unlock(&prs->lock);
+	return size;
+}
+
+static ssize_t attr_get_diff_enable(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	u8 val;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	mutex_lock(&prs->lock);
+	val = prs->diff_enabled;
+	mutex_unlock(&prs->lock);
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t attr_set_diff_enable(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t size)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long val;
+
+	if (strict_strtoul(buf, 10, &val))
+		return -EINVAL;
+
+	mutex_lock(&prs->lock);
+	lps001wp_prs_diffen_manage(prs, (u8) val);
+	mutex_unlock(&prs->lock);
+	return size;
+}
+
+static ssize_t attr_get_enable(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	int val = atomic_read(&prs->enabled);
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t attr_set_enable(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long val;
+
+	if (strict_strtoul(buf, 10, &val))
+		return -EINVAL;
+
+	if (val)
+		lps001wp_prs_enable(prs);
+	else
+		lps001wp_prs_disable(prs);
+
+	return size;
+}
+
+static ssize_t attr_get_press_ref(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	int err = -1;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	u16 val = 0;
+
+	mutex_lock(&prs->lock);
+	err = lps001wp_prs_get_press_reference(prs, &val);
+	mutex_unlock(&prs->lock);
+	if (err < 0)
+		return err;
+
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t attr_set_press_ref(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	int err = -1;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long val = 0;
+
+	if (strict_strtoul(buf, 10, &val))
+		return -EINVAL;
+
+	if (val < PR_ABS_MIN || val > PR_ABS_MAX)
+		return -EINVAL;
+
+	mutex_lock(&prs->lock);
+	err = lps001wp_prs_set_press_reference(prs, val);
+	mutex_unlock(&prs->lock);
+	if (err < 0)
+		return err;
+	return size;
+}
+
+
+static ssize_t attr_get_lowpowmode(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	u8 val;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	mutex_lock(&prs->lock);
+	val = prs->lpowmode_enabled;
+	mutex_unlock(&prs->lock);
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t attr_set_lowpowmode(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t size)
+{
+	int err = -1;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long val;
+
+	if (strict_strtoul(buf, 10, &val))
+		return -EINVAL;
+
+	mutex_lock(&prs->lock);
+	err = lps001wp_prs_lpow_manage(prs, (u8) val);
+	mutex_unlock(&prs->lock);
+	if (err < 0)
+		return err;
+	return size;
+}
+
+
+#ifdef DEBUG
+static ssize_t attr_reg_set(struct device *dev, struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	int rc;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	u8 x[2];
+	unsigned long val;
+
+	if (strict_strtoul(buf, 16, &val))
+		return -EINVAL;
+	mutex_lock(&prs->lock);
+	x[0] = prs->reg_addr;
+	mutex_unlock(&prs->lock);
+	x[1] = val;
+	rc = lps001wp_prs_i2c_write(prs, x, 1);
+	/*TODO: error need to be managed */
+	return size;
+}
+
+static ssize_t attr_reg_get(struct device *dev, struct device_attribute *attr,
+				char *buf)
+{
+	ssize_t ret;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	int rc;
+	u8 data;
+
+	mutex_lock(&prs->lock);
+	data = prs->reg_addr;
+	mutex_unlock(&prs->lock);
+	rc = lps001wp_prs_i2c_read(prs, &data, 1);
+	/*TODO: error need to be managed */
+	ret = sprintf(buf, "0x%02x\n", data);
+	return ret;
+}
+
+static ssize_t attr_addr_set(struct device *dev, struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long val;
+	if (strict_strtoul(buf, 16, &val))
+		return -EINVAL;
+	mutex_lock(&prs->lock);
+	prs->reg_addr = val;
+	mutex_unlock(&prs->lock);
+	return size;
+}
+#endif
+
+
+
+static struct device_attribute attributes[] = {
+	__ATTR(pollrate_ms, 0664, attr_get_polling_rate, attr_set_polling_rate),
+	__ATTR(enable, 0664, attr_get_enable, attr_set_enable),
+	__ATTR(diff_enable, 0664, attr_get_diff_enable, attr_set_diff_enable),
+	__ATTR(press_reference, 0664, attr_get_press_ref, attr_set_press_ref),
+	__ATTR(lowpow_enable, 0664, attr_get_lowpowmode, attr_set_lowpowmode),
+#ifdef DEBUG
+	__ATTR(reg_value, 0664, attr_reg_get, attr_reg_set),
+	__ATTR(reg_addr, 0220, NULL, attr_addr_set),
+#endif
+};
+
+static int create_sysfs_interfaces(struct device *dev)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(attributes); i++)
+		if (device_create_file(dev, attributes + i))
+			goto error;
+	return 0;
+
+error:
+	for ( ; i >= 0; i--)
+		device_remove_file(dev, attributes + i);
+	dev_err(dev, "%s:Unable to create interface\n", __func__);
+	return -1;
+}
+
+static int remove_sysfs_interfaces(struct device *dev)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(attributes); i++)
+		device_remove_file(dev, attributes + i);
+	return 0;
+}
+
+
+static void lps001wp_prs_input_work_func(struct work_struct *work)
+{
+	struct lps001wp_prs_data *prs;
+
+	struct outputdata output;
+	struct outputdata *out = &output;
+	int err;
+
+	prs = container_of((struct delayed_work *)work,
+				struct lps001wp_prs_data, input_work);
+
+	mutex_lock(&prs->lock);
+	err = lps001wp_prs_get_presstemp_data(prs, out);
+	if (err < 0)
+		dev_err(&prs->client->dev, "get_pressure_data failed\n");
+	else
+		lps001wp_prs_report_values(prs, out);
+
+	schedule_delayed_work(&prs->input_work,
+				msecs_to_jiffies(prs->pdata->poll_interval));
+	mutex_unlock(&prs->lock);
+}
+
+int lps001wp_prs_input_open(struct input_dev *input)
+{
+	struct lps001wp_prs_data *prs = input_get_drvdata(input);
+
+	return lps001wp_prs_enable(prs);
+}
+
+void lps001wp_prs_input_close(struct input_dev *dev)
+{
+	struct lps001wp_prs_data *prs = input_get_drvdata(dev);
+
+	lps001wp_prs_disable(prs);
+}
+
+
+static int lps001wp_prs_validate_pdata(struct lps001wp_prs_data *prs)
+{
+	prs->pdata->poll_interval = max(prs->pdata->poll_interval,
+					prs->pdata->min_interval);
+
+	/* Enforce minimum polling interval */
+	if (prs->pdata->poll_interval < prs->pdata->min_interval) {
+		dev_err(&prs->client->dev, "minimum poll interval violated\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int lps001wp_prs_input_init(struct lps001wp_prs_data *prs)
+{
+	int err;
+
+	INIT_DELAYED_WORK(&prs->input_work, lps001wp_prs_input_work_func);
+	prs->input_dev = input_allocate_device();
+	if (!prs->input_dev) {
+		err = -ENOMEM;
+		dev_err(&prs->client->dev, "input device allocate failed\n");
+		goto err0;
+	}
+
+	prs->input_dev->open = lps001wp_prs_input_open;
+	prs->input_dev->close = lps001wp_prs_input_close;
+	prs->input_dev->name = LPS001WP_PRS_DEV_NAME;
+	prs->input_dev->id.bustype = BUS_I2C;
+	prs->input_dev->dev.parent = &prs->client->dev;
+
+	input_set_drvdata(prs->input_dev, prs);
+
+	set_bit(EV_ABS, prs->input_dev->evbit);
+
+	input_set_abs_params(prs->input_dev, ABS_PR,
+			PR_ABS_MIN, PR_ABS_MAX, FUZZ, FLAT);
+	input_set_abs_params(prs->input_dev, ABS_TEMP,
+			PR_DLT_MIN, PR_DLT_MAX, FUZZ, FLAT);
+	input_set_abs_params(prs->input_dev, ABS_DLTPR,
+			TEMP_MIN, TEMP_MAX, FUZZ, FLAT);
+
+
+	prs->input_dev->name = "LPS001WP barometer";
+
+	err = input_register_device(prs->input_dev);
+	if (err) {
+		dev_err(&prs->client->dev,
+			"unable to register input polled device %s\n",
+			prs->input_dev->name);
+		goto err1;
+	}
+
+	return 0;
+
+err1:
+	input_free_device(prs->input_dev);
+err0:
+	return err;
+}
+
+static void lps001wp_prs_input_cleanup(struct lps001wp_prs_data *prs)
+{
+	input_unregister_device(prs->input_dev);
+	input_free_device(prs->input_dev);
+}
+
+static int lps001wp_prs_probe(struct i2c_client *client,
+					const struct i2c_device_id *id)
+{
+	struct lps001wp_prs_data *prs;
+	int err = -1;
+	int tempvalue;
+
+	pr_info("%s: probe start.\n", LPS001WP_PRS_DEV_NAME);
+
+	if (client->dev.platform_data == NULL) {
+		dev_err(&client->dev, "platform data is NULL. exiting.\n");
+		err = -ENODEV;
+		goto exit_check_functionality_failed;
+	}
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "client not i2c capable\n");
+		err = -ENODEV;
+		goto exit_check_functionality_failed;
+	}
+
+	if (!i2c_check_functionality(client->adapter,
+					I2C_FUNC_SMBUS_BYTE |
+					I2C_FUNC_SMBUS_BYTE_DATA |
+					I2C_FUNC_SMBUS_WORD_DATA)) {
+		dev_err(&client->dev, "client not smb-i2c capable:2\n");
+		err = -EIO;
+		goto exit_check_functionality_failed;
+	}
+
+
+	if (!i2c_check_functionality(client->adapter,
+						I2C_FUNC_SMBUS_I2C_BLOCK)){
+		dev_err(&client->dev, "client not smb-i2c capable:3\n");
+		err = -EIO;
+		goto exit_check_functionality_failed;
+	}
+
+
+	prs = kzalloc(sizeof(struct lps001wp_prs_data), GFP_KERNEL);
+	if (prs == NULL) {
+		err = -ENOMEM;
+		dev_err(&client->dev,
+				"failed to allocate memory for module data: "
+					"%d\n", err);
+		goto exit_alloc_data_failed;
+	}
+
+	mutex_init(&prs->lock);
+	mutex_lock(&prs->lock);
+
+	prs->client = client;
+	i2c_set_clientdata(client, prs);
+
+
+	if (i2c_smbus_read_byte(client) < 0) {
+		printk(KERN_ERR "i2c_smbus_read_byte error!!\n");
+		goto err_mutexunlockfreedata;
+	} else {
+		printk(KERN_DEBUG "%s Device detected!\n",
+							LPS001WP_PRS_DEV_NAME);
+	}
+
+	/* read chip id */
+	tempvalue = i2c_smbus_read_word_data(client, WHO_AM_I);
+	if ((tempvalue & 0x00FF) == WHOAMI_LPS001WP_PRS) {
+		printk(KERN_DEBUG "%s I2C driver registered!\n",
+							LPS001WP_PRS_DEV_NAME);
+	} else {
+		prs->client = NULL;
+		printk(KERN_DEBUG "I2C driver not registered!"
+				" Device unknown\n");
+		goto err_mutexunlockfreedata;
+	}
+
+	prs->pdata = kmalloc(sizeof(*prs->pdata), GFP_KERNEL);
+	if (prs->pdata == NULL) {
+		err = -ENOMEM;
+		dev_err(&client->dev,
+				"failed to allocate memory for pdata: %d\n",
+				err);
+		goto err_mutexunlockfreedata;
+	}
+
+	memcpy(prs->pdata, client->dev.platform_data, sizeof(*prs->pdata));
+
+	err = lps001wp_prs_validate_pdata(prs);
+	if (err < 0) {
+		dev_err(&client->dev, "failed to validate platform data\n");
+		goto exit_kfree_pdata;
+	}
+
+	i2c_set_clientdata(client, prs);
+
+
+	if (prs->pdata->init) {
+		err = prs->pdata->init();
+		if (err < 0) {
+			dev_err(&client->dev, "init failed: %d\n", err);
+			goto err2;
+		}
+	}
+
+	memset(prs->resume_state, 0, ARRAY_SIZE(prs->resume_state));
+
+	prs->resume_state[RES_CTRL_REG1] = LPS001WP_PRS_PM_NORMAL;
+	prs->resume_state[RES_CTRL_REG2] = 0x00;
+	prs->resume_state[RES_CTRL_REG3] = 0x00;
+	prs->resume_state[RES_REF_P_L] = 0x00;
+	prs->resume_state[RES_REF_P_H] = 0x00;
+	prs->resume_state[RES_THS_P_L] = 0x00;
+	prs->resume_state[RES_THS_P_H] = 0x00;
+	prs->resume_state[RES_INT_CFG] = 0x00;
+
+	err = lps001wp_prs_device_power_on(prs);
+	if (err < 0) {
+		dev_err(&client->dev, "power on failed: %d\n", err);
+		goto err2;
+	}
+
+	prs->diff_enabled = 0;
+	prs->lpowmode_enabled = 0;
+	atomic_set(&prs->enabled, 1);
+
+	err = lps001wp_prs_update_odr(prs, prs->pdata->poll_interval);
+	if (err < 0) {
+		dev_err(&client->dev, "update_odr failed\n");
+		goto err_power_off;
+	}
+
+	err = lps001wp_prs_input_init(prs);
+	if (err < 0) {
+		dev_err(&client->dev, "input init failed\n");
+		goto err_power_off;
+	}
+
+
+	err = create_sysfs_interfaces(&client->dev);
+	if (err < 0) {
+		dev_err(&client->dev,
+			"device LPS001WP_PRS_DEV_NAME sysfs register failed\n");
+		goto err_input_cleanup;
+	}
+
+
+	lps001wp_prs_device_power_off(prs);
+
+	/* As default, do not report information */
+	atomic_set(&prs->enabled, 0);
+
+
+	mutex_unlock(&prs->lock);
+
+	dev_info(&client->dev, "%s: probed\n", LPS001WP_PRS_DEV_NAME);
+
+	return 0;
+
+/*
+remove_sysfs_int:
+	remove_sysfs_interfaces(&client->dev);
+*/
+err_input_cleanup:
+	lps001wp_prs_input_cleanup(prs);
+err_power_off:
+	lps001wp_prs_device_power_off(prs);
+err2:
+	if (prs->pdata->exit)
+		prs->pdata->exit();
+exit_kfree_pdata:
+	kfree(prs->pdata);
+
+err_mutexunlockfreedata:
+	mutex_unlock(&prs->lock);
+	kfree(prs);
+exit_alloc_data_failed:
+exit_check_functionality_failed:
+	printk(KERN_ERR "%s: Driver Init failed\n", LPS001WP_PRS_DEV_NAME);
+	return err;
+}
+
+static int __devexit lps001wp_prs_remove(struct i2c_client *client)
+{
+	struct lps001wp_prs_data *prs = i2c_get_clientdata(client);
+
+	lps001wp_prs_input_cleanup(prs);
+	lps001wp_prs_device_power_off(prs);
+	remove_sysfs_interfaces(&client->dev);
+
+	if (prs->pdata->exit)
+		prs->pdata->exit();
+	kfree(prs->pdata);
+	kfree(prs);
+
+	return 0;
+}
+
+static int lps001wp_prs_resume(struct i2c_client *client)
+{
+	struct lps001wp_prs_data *prs = i2c_get_clientdata(client);
+
+	if (prs->on_before_suspend)
+		return lps001wp_prs_enable(prs);
+	return 0;
+}
+
+static int lps001wp_prs_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	struct lps001wp_prs_data *prs = i2c_get_clientdata(client);
+
+	prs->on_before_suspend = atomic_read(&prs->enabled);
+	return lps001wp_prs_disable(prs);
+}
+
+static const struct i2c_device_id lps001wp_prs_id[]
+		= { { LPS001WP_PRS_DEV_NAME, 0}, { },};
+
+MODULE_DEVICE_TABLE(i2c, lps001wp_prs_id);
+
+static struct i2c_driver lps001wp_prs_driver = {
+	.driver = {
+			.name = LPS001WP_PRS_DEV_NAME,
+			.owner = THIS_MODULE,
+	},
+	.probe = lps001wp_prs_probe,
+	.remove = __devexit_p(lps001wp_prs_remove),
+	.id_table = lps001wp_prs_id,
+	.resume = lps001wp_prs_resume,
+	.suspend = lps001wp_prs_suspend,
+};
+
+static int __init lps001wp_prs_init(void)
+{
+	printk(KERN_DEBUG "%s barometer driver: init\n",
+						LPS001WP_PRS_DEV_NAME);
+	return i2c_add_driver(&lps001wp_prs_driver);
+}
+
+static void __exit lps001wp_prs_exit(void)
+{
+	#if DEBUG
+	printk(KERN_DEBUG "%s barometer driver exit\n",
+						LPS001WP_PRS_DEV_NAME);
+	#endif
+	i2c_del_driver(&lps001wp_prs_driver);
+	return;
+}
+
+module_init(lps001wp_prs_init);
+module_exit(lps001wp_prs_exit);
+
+MODULE_DESCRIPTION("STMicrolelectronics lps001wp pressure sensor sysfs driver");
+MODULE_AUTHOR("Matteo Dameno, Carmine Iascone, STMicroelectronics");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/input/touchscreen/bu21013_ts.c b/drivers/input/touchscreen/bu21013_ts.c
index 1507ce108d5..ae6c0e82af2 100644
--- a/drivers/input/touchscreen/bu21013_ts.c
+++ b/drivers/input/touchscreen/bu21013_ts.c
@@ -1,10 +1,11 @@
 /*
- * Copyright (C) ST-Ericsson SA 2010
+ * Copyright (C) ST-Ericsson SA 2009
  * Author: Naveen Kumar G <naveen.gaddipati@stericsson.com> for ST-Ericsson
  * License terms:GNU General Public License (GPL) version 2
  */
 
 #include <linux/kernel.h>
+#include <linux/earlysuspend.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 #include <linux/i2c.h>
@@ -12,12 +13,13 @@
 #include <linux/input.h>
 #include <linux/input/bu21013.h>
 #include <linux/slab.h>
+#include <linux/clk.h>
 #include <linux/regulator/consumer.h>
 
 #define PEN_DOWN_INTR	0
-#define MAX_FINGERS	2
 #define RESET_DELAY	30
-#define PENUP_TIMEOUT	(10)
+#define PENUP_TIMEOUT	2 /* 2msecs */
+#define SCALE_FACTOR	1000
 #define DELTA_MIN	16
 #define MASK_BITS	0x03
 #define SHIFT_8		8
@@ -130,7 +132,7 @@
 #define BU21013_NUMBER_OF_X_SENSORS	(6)
 #define BU21013_NUMBER_OF_Y_SENSORS	(11)
 
-#define DRIVER_TP	"bu21013_tp"
+#define DRIVER_TP	"bu21013_ts"
 
 /**
  * struct bu21013_ts_data - touch panel data structure
@@ -141,6 +143,13 @@
  * @in_dev: pointer to the input device structure
  * @intr_pin: interrupt pin value
  * @regulator: pointer to the Regulator used for touch screen
+ * @enable: variable to indicate the enable/disable of touch screen
+ * @ext_clk_enable: true if running on ext clk
+ * @ext_clk_state: Saved state for suspend/resume of ext clk
+ * @factor_x: x scale factor
+ * @factor_y: y scale factor
+ * @tpclk: pointer to clock structure
+ * @early_suspend: early_suspend structure variable
  *
  * Touch panel device data structure
  */
@@ -148,12 +157,232 @@ struct bu21013_ts_data {
 	struct i2c_client *client;
 	wait_queue_head_t wait;
 	bool touch_stopped;
-	const struct bu21013_platform_device *chip;
+	struct bu21013_platform_device *chip;
 	struct input_dev *in_dev;
 	unsigned int intr_pin;
 	struct regulator *regulator;
+	bool enable;
+	bool ext_clk_enable;
+	bool ext_clk_state;
+	unsigned int factor_x;
+	unsigned int factor_y;
+	struct clk *tpclk;
+	struct early_suspend early_suspend;
 };
 
+static int bu21013_init_chip(struct bu21013_ts_data *data, bool on_ext_clk);
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void bu21013_ts_early_suspend(struct early_suspend *data);
+static void bu21013_ts_late_resume(struct early_suspend *data);
+#endif
+
+/**
+ * bu21013_ext_clk() - enable/disable the external clock
+ * @pdata: touch screen data
+ * @enable: enable external clock
+ * @reconfig: reconfigure chip upon external clock off.
+ *
+ * This function used to enable or disable the external clock and possible
+ * reconfigure hw.
+ */
+static int bu21013_ext_clk(struct bu21013_ts_data *pdata, bool enable,
+			   bool reconfig)
+{
+	int retval = 0;
+
+	if (!pdata->tpclk || pdata->ext_clk_enable == enable)
+		return retval;
+
+	if (enable) {
+		pdata->ext_clk_enable = true;
+		clk_enable(pdata->tpclk);
+		retval = bu21013_init_chip(pdata, true);
+	} else {
+		pdata->ext_clk_enable = false;
+		if (reconfig)
+			retval = bu21013_init_chip(pdata, false);
+		clk_disable(pdata->tpclk);
+	}
+	return retval;
+}
+
+/**
+ * bu21013_enable() - enable the touch driver event
+ * @pdata: touch screen data
+ *
+ * This function used to enable the driver and returns integer
+ */
+static int bu21013_enable(struct bu21013_ts_data *pdata)
+{
+	int retval;
+
+	if (pdata->regulator)
+		regulator_enable(pdata->regulator);
+
+	if (pdata->chip->cs_en) {
+		retval = pdata->chip->cs_en(pdata->chip->cs_pin);
+		if (retval < 0) {
+			dev_err(&pdata->client->dev, "enable hw failed\n");
+			return retval;
+		}
+	}
+
+	if (pdata->ext_clk_state)
+		retval = bu21013_ext_clk(pdata, true, true);
+	else
+		retval = bu21013_init_chip(pdata, false);
+
+	if (retval < 0) {
+		dev_err(&pdata->client->dev, "enable hw failed\n");
+		return retval;
+	}
+	pdata->touch_stopped = false;
+	enable_irq(pdata->chip->irq);
+
+	return 0;
+}
+
+/**
+ * bu21013_disable() - disable the touch driver event
+ * @pdata: touch screen data
+ *
+ * This function used to disable the driver and returns integer
+ */
+static void bu21013_disable(struct bu21013_ts_data *pdata)
+{
+	pdata->touch_stopped = true;
+
+	pdata->ext_clk_state = pdata->ext_clk_enable;
+	(void) bu21013_ext_clk(pdata, false, false);
+
+	disable_irq(pdata->chip->irq);
+	if (pdata->chip->cs_dis)
+		pdata->chip->cs_dis(pdata->chip->cs_pin);
+	if (pdata->regulator)
+		regulator_disable(pdata->regulator);
+}
+
+/**
+ * bu21013_show_attr_enable() - show the touch screen controller status
+ * @dev: pointer to device structure
+ * @attr: pointer to device attribute
+ * @buf: parameter buffer
+ *
+ * This funtion is used to show whether the touch screen is enabled or
+ * disabled
+ */
+static ssize_t bu21013_show_attr_enable(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct bu21013_ts_data *pdata = dev_get_drvdata(dev);
+	return sprintf(buf, "%d\n", pdata->enable);
+}
+
+/**
+ * bu21013_store_attr_enable() - Enable/Disable the touchscreen.
+ * @dev: pointer to device structure
+ * @attr: pointer to device attribute
+ * @buf: parameter buffer
+ * @count: number of parameters
+ *
+ * This funtion is used to enable or disable the touch screen controller.
+ */
+static ssize_t bu21013_store_attr_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int ret = 0;
+	unsigned long val;
+
+	struct bu21013_ts_data *pdata = dev_get_drvdata(dev);
+
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if ((val != 0) && (val != 1))
+		return -EINVAL;
+
+	if (pdata->enable != val) {
+		pdata->enable = val ? true : false;
+		if (pdata->enable) {
+			ret = bu21013_enable(pdata);
+			if (ret < 0)
+				return ret;
+		} else
+			bu21013_disable(pdata);
+	}
+	return count;
+}
+
+/**
+ * bu21013_show_attr_extclk() - shows the external clock status
+ * @dev: pointer to device structure
+ * @attr: pointer to device attribute
+ * @buf: parameter buffer
+ *
+ * This funtion is used to show whether the external clock for the touch
+ * screen is enabled or disabled.
+ */
+static ssize_t bu21013_show_attr_extclk(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct bu21013_ts_data *pdata = dev_get_drvdata(dev);
+	return sprintf(buf, "%d\n", pdata->ext_clk_enable);
+}
+
+/**
+ * bu21013_store_attr_extclk() - Enable/Disable the external clock
+ * for the tocuh screen controller.
+ * @dev: pointer to device structure
+ * @attr: pointer to device attribute
+ * @buf: parameter buffer
+ * @count: number of parameters
+ *
+ * This funtion is used enabled or disable the external clock for the touch
+ * screen controller.
+ */
+static ssize_t bu21013_store_attr_extclk(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval = 0;
+	struct bu21013_ts_data *pdata = dev_get_drvdata(dev);
+	unsigned long val;
+
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if ((val != 0) && (val != 1))
+		return -EINVAL;
+
+	if (pdata->chip->has_ext_clk) {
+		if (pdata->enable)
+			retval = bu21013_ext_clk(pdata, val, true);
+		else
+			pdata->ext_clk_state = val;
+		if (retval < 0)
+			return retval;
+	}
+	return count;
+}
+
+static DEVICE_ATTR(enable, S_IWUSR | S_IRUGO,
+		bu21013_show_attr_enable, bu21013_store_attr_enable);
+
+static DEVICE_ATTR(ext_clk, S_IWUSR | S_IRUGO,
+		bu21013_show_attr_extclk, bu21013_store_attr_extclk);
+
+
+static struct attribute *bu21013_attribute[] = {
+		&dev_attr_enable.attr,
+		&dev_attr_ext_clk.attr,
+		NULL,
+};
+
+static struct attribute_group bu21013_attr_group = {
+		.attrs = bu21013_attribute,
+};
+
+
 /**
  * bu21013_read_block_data(): read the touch co-ordinates
  * @data: bu21013_ts_data structure pointer
@@ -203,12 +432,14 @@ static int bu21013_do_touch_report(struct bu21013_ts_data *data)
 	if (!has_x_sensors || !has_y_sensors)
 		return 0;
 
-	for (i = 0; i < MAX_FINGERS; i++) {
+	for (i = 0; i < 2; i++) {
 		const u8 *p = &buf[4 * i + 3];
 		unsigned int x = p[0] << SHIFT_2 | (p[1] & MASK_BITS);
 		unsigned int y = p[2] << SHIFT_2 | (p[3] & MASK_BITS);
 		if (x == 0 || y == 0)
 			continue;
+		x = x * data->factor_x / SCALE_FACTOR;
+		y = y * data->factor_y / SCALE_FACTOR;
 		pos_x[finger_down_count] = x;
 		pos_y[finger_down_count] = y;
 		finger_down_count++;
@@ -216,21 +447,21 @@ static int bu21013_do_touch_report(struct bu21013_ts_data *data)
 
 	if (finger_down_count) {
 		if (finger_down_count == 2 &&
-		    (abs(pos_x[0] - pos_x[1]) < DELTA_MIN ||
-		     abs(pos_y[0] - pos_y[1]) < DELTA_MIN)) {
+				(abs(pos_x[0] - pos_x[1]) < DELTA_MIN ||
+				abs(pos_y[0] - pos_y[1]) < DELTA_MIN))
 			return 0;
-		}
 
 		for (i = 0; i < finger_down_count; i++) {
-			if (data->chip->x_flip)
-				pos_x[i] = data->chip->touch_x_max - pos_x[i];
-			if (data->chip->y_flip)
-				pos_y[i] = data->chip->touch_y_max - pos_y[i];
-
-			input_report_abs(data->in_dev,
-					 ABS_MT_POSITION_X, pos_x[i]);
-			input_report_abs(data->in_dev,
-					 ABS_MT_POSITION_Y, pos_y[i]);
+			if (data->chip->portrait && data->chip->x_flip)
+				pos_x[i] = data->chip->x_max_res - pos_x[i];
+			if (data->chip->portrait && data->chip->y_flip)
+				pos_y[i] = data->chip->y_max_res - pos_y[i];
+			input_report_abs(data->in_dev, ABS_MT_TOUCH_MAJOR,
+						max(pos_x[i], pos_y[i]));
+			input_report_abs(data->in_dev, ABS_MT_POSITION_X,
+								pos_x[i]);
+			input_report_abs(data->in_dev, ABS_MT_POSITION_Y,
+								pos_y[i]);
 			input_mt_sync(data->in_dev);
 		}
 	} else
@@ -260,24 +491,23 @@ static irqreturn_t bu21013_gpio_irq(int irq, void *device_data)
 			dev_err(&i2c->dev, "bu21013_do_touch_report failed\n");
 			return IRQ_NONE;
 		}
-
 		data->intr_pin = data->chip->irq_read_val();
 		if (data->intr_pin == PEN_DOWN_INTR)
 			wait_event_timeout(data->wait, data->touch_stopped,
-					   msecs_to_jiffies(2));
+					msecs_to_jiffies(PENUP_TIMEOUT));
 	} while (!data->intr_pin && !data->touch_stopped);
-
 	return IRQ_HANDLED;
 }
 
 /**
  * bu21013_init_chip() - power on sequence for the bu21013 controller
  * @data: device structure pointer
+ * @on_ext_clk: Run on external clock
  *
  * This function is used to power on
  * the bu21013 controller and returns integer.
  */
-static int bu21013_init_chip(struct bu21013_ts_data *data)
+static int bu21013_init_chip(struct bu21013_ts_data *data, bool on_ext_clk)
 {
 	int retval;
 	struct i2c_client *i2c = data->client;
@@ -296,28 +526,24 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_SENSOR_0_7 reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_SENSOR_8_15_REG,
 						BU21013_SENSORS_EN_8_15);
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_SENSOR_8_15 reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_SENSOR_16_23_REG,
 						BU21013_SENSORS_EN_16_23);
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_SENSOR_16_23 reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_POS_MODE1_REG,
 				(BU21013_POS_MODE1_0 | BU21013_POS_MODE1_1));
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_POS_MODE1 reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_POS_MODE2_REG,
 			(BU21013_POS_MODE2_ZERO | BU21013_POS_MODE2_AVG1 |
 			BU21013_POS_MODE2_AVG2 | BU21013_POS_MODE2_EN_RAW |
@@ -326,8 +552,7 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_POS_MODE2 reg write failed\n");
 		return retval;
 	}
-
-	if (data->chip->ext_clk)
+	if (on_ext_clk)
 		retval = i2c_smbus_write_byte_data(i2c, BU21013_CLK_MODE_REG,
 			(BU21013_CLK_MODE_EXT | BU21013_CLK_MODE_CALIB));
 	else
@@ -337,21 +562,18 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_CLK_MODE reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_IDLE_REG,
 				(BU21013_IDLET_0 | BU21013_IDLE_INTERMIT_EN));
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_IDLE reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_INT_MODE_REG,
 						BU21013_INT_MODE_LEVEL);
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_INT_MODE reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_FILTER_REG,
 						(BU21013_DELTA_0_6 |
 							BU21013_FILTER_EN));
@@ -366,14 +588,12 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_TH_ON reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_TH_OFF_REG,
 				BU21013_TH_OFF_4 | BU21013_TH_OFF_3);
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_TH_OFF reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_GAIN_REG,
 					(BU21013_GAIN_0 | BU21013_GAIN_1));
 	if (retval < 0) {
@@ -387,7 +607,6 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_OFFSET_MODE reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_XY_EDGE_REG,
 				(BU21013_X_EDGE_0 | BU21013_X_EDGE_2 |
 				BU21013_Y_EDGE_1 | BU21013_Y_EDGE_3));
@@ -395,7 +614,6 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_XY_EDGE reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_DONE_REG,
 							BU21013_DONE);
 	if (retval < 0) {
@@ -403,25 +621,15 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		return retval;
 	}
 
-	return 0;
-}
-
-/**
- * bu21013_free_irq() - frees IRQ registered for touchscreen
- * @bu21013_data: device structure pointer
- *
- * This function signals interrupt thread to stop processing and
- * frees interrupt.
- */
-static void bu21013_free_irq(struct bu21013_ts_data *bu21013_data)
-{
-	bu21013_data->touch_stopped = true;
-	wake_up(&bu21013_data->wait);
-	free_irq(bu21013_data->chip->irq, bu21013_data);
+	data->factor_x = (data->chip->x_max_res * SCALE_FACTOR /
+					data->chip->touch_x_max);
+	data->factor_y = (data->chip->y_max_res * SCALE_FACTOR /
+					data->chip->touch_y_max);
+	return retval;
 }
 
 /**
- * bu21013_probe() - initializes the i2c-client touchscreen driver
+ * bu21013_probe() - initialzes the i2c-client touchscreen driver
  * @client: i2c client structure pointer
  * @id: i2c device id pointer
  *
@@ -431,11 +639,11 @@ static void bu21013_free_irq(struct bu21013_ts_data *bu21013_data)
 static int __devinit bu21013_probe(struct i2c_client *client,
 					const struct i2c_device_id *id)
 {
+	int retval;
 	struct bu21013_ts_data *bu21013_data;
 	struct input_dev *in_dev;
-	const struct bu21013_platform_device *pdata =
+	struct bu21013_platform_device *pdata =
 					client->dev.platform_data;
-	int error;
 
 	if (!i2c_check_functionality(client->adapter,
 					I2C_FUNC_SMBUS_BYTE_DATA)) {
@@ -445,53 +653,72 @@ static int __devinit bu21013_probe(struct i2c_client *client,
 
 	if (!pdata) {
 		dev_err(&client->dev, "platform data not defined\n");
-		return -EINVAL;
+		retval = -EINVAL;
+		return retval;
 	}
 
 	bu21013_data = kzalloc(sizeof(struct bu21013_ts_data), GFP_KERNEL);
-	in_dev = input_allocate_device();
-	if (!bu21013_data || !in_dev) {
+	if (!bu21013_data) {
 		dev_err(&client->dev, "device memory alloc failed\n");
-		error = -ENOMEM;
-		goto err_free_mem;
+		retval = -ENOMEM;
+		return retval;
+	}
+	/* allocate input device */
+	in_dev = input_allocate_device();
+	if (!in_dev) {
+		dev_err(&client->dev, "input device memory alloc failed\n");
+		retval = -ENOMEM;
+		goto err_alloc;
 	}
 
 	bu21013_data->in_dev = in_dev;
 	bu21013_data->chip = pdata;
 	bu21013_data->client = client;
 
-	bu21013_data->regulator = regulator_get(&client->dev, "V-TOUCH");
+	bu21013_data->regulator = regulator_get(&client->dev, "avdd");
 	if (IS_ERR(bu21013_data->regulator)) {
-		dev_err(&client->dev, "regulator_get failed\n");
-		error = PTR_ERR(bu21013_data->regulator);
-		goto err_free_mem;
-	}
-
-	error = regulator_enable(bu21013_data->regulator);
-	if (error < 0) {
-		dev_err(&client->dev, "regulator enable failed\n");
-		goto err_put_regulator;
+		dev_warn(&client->dev, "regulator_get failed\n");
+		bu21013_data->regulator = NULL;
 	}
-
-	bu21013_data->touch_stopped = false;
-	init_waitqueue_head(&bu21013_data->wait);
+	if (bu21013_data->regulator)
+		regulator_enable(bu21013_data->regulator);
 
 	/* configure the gpio pins */
 	if (pdata->cs_en) {
-		error = pdata->cs_en(pdata->cs_pin);
-		if (error < 0) {
+		retval = pdata->cs_en(pdata->cs_pin);
+		if (retval < 0) {
 			dev_err(&client->dev, "chip init failed\n");
-			goto err_disable_regulator;
+			goto err_init_cs;
 		}
 	}
 
+	if (pdata->has_ext_clk) {
+		bu21013_data->tpclk = clk_get(&client->dev, NULL);
+		if (IS_ERR(bu21013_data->tpclk)) {
+			dev_warn(&client->dev, "get extern clock failed\n");
+			bu21013_data->tpclk = NULL;
+		}
+	}
+
+	if (pdata->enable_ext_clk && bu21013_data->tpclk) {
+		retval = clk_enable(bu21013_data->tpclk);
+		if (retval < 0) {
+			dev_err(&client->dev, "clock enable failed\n");
+			goto err_ext_clk;
+		}
+		bu21013_data->ext_clk_enable = true;
+	}
+
 	/* configure the touch panel controller */
-	error = bu21013_init_chip(bu21013_data);
-	if (error) {
+	retval = bu21013_init_chip(bu21013_data, bu21013_data->ext_clk_enable);
+	if (retval < 0) {
 		dev_err(&client->dev, "error in bu21013 config\n");
-		goto err_cs_disable;
+		goto err_init_config;
 	}
 
+	init_waitqueue_head(&bu21013_data->wait);
+	bu21013_data->touch_stopped = false;
+
 	/* register the device to input subsystem */
 	in_dev->name = DRIVER_TP;
 	in_dev->id.bustype = BUS_I2C;
@@ -502,44 +729,70 @@ static int __devinit bu21013_probe(struct i2c_client *client,
 	__set_bit(EV_ABS, in_dev->evbit);
 
 	input_set_abs_params(in_dev, ABS_MT_POSITION_X, 0,
-						pdata->touch_x_max, 0, 0);
+						pdata->x_max_res, 0, 0);
 	input_set_abs_params(in_dev, ABS_MT_POSITION_Y, 0,
-						pdata->touch_y_max, 0, 0);
+						pdata->y_max_res, 0, 0);
+	input_set_abs_params(in_dev, ABS_MT_TOUCH_MAJOR, 0,
+			max(pdata->x_max_res , pdata->y_max_res), 0, 0);
 	input_set_drvdata(in_dev, bu21013_data);
-
-	error = request_threaded_irq(pdata->irq, NULL, bu21013_gpio_irq,
-				     IRQF_TRIGGER_FALLING | IRQF_SHARED,
-				     DRIVER_TP, bu21013_data);
-	if (error) {
+	retval = input_register_device(in_dev);
+	if (retval)
+		goto err_input_register;
+
+	retval = request_threaded_irq(pdata->irq, NULL, bu21013_gpio_irq,
+					(IRQF_TRIGGER_FALLING | IRQF_SHARED),
+					DRIVER_TP, bu21013_data);
+	if (retval) {
 		dev_err(&client->dev, "request irq %d failed\n", pdata->irq);
-		goto err_cs_disable;
+		goto err_init_irq;
 	}
+	bu21013_data->enable = true;
+	i2c_set_clientdata(client, bu21013_data);
 
-	error = input_register_device(in_dev);
-	if (error) {
-		dev_err(&client->dev, "failed to register input device\n");
-		goto err_free_irq;
+	/* sysfs implementation for dynamic enable/disable the input event */
+	retval = sysfs_create_group(&client->dev.kobj,	&bu21013_attr_group);
+	if (retval) {
+		dev_err(&client->dev, "failed to create sysfs entries\n");
+		goto err_sysfs_create;
 	}
 
-	device_init_wakeup(&client->dev, pdata->wakeup);
-	i2c_set_clientdata(client, bu21013_data);
-
-	return 0;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	bu21013_data->early_suspend.level =
+				EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	bu21013_data->early_suspend.suspend = bu21013_ts_early_suspend;
+	bu21013_data->early_suspend.resume = bu21013_ts_late_resume;
+	register_early_suspend(&bu21013_data->early_suspend);
+#endif
+	return retval;
 
-err_free_irq:
-	bu21013_free_irq(bu21013_data);
-err_cs_disable:
-	pdata->cs_dis(pdata->cs_pin);
-err_disable_regulator:
-	regulator_disable(bu21013_data->regulator);
-err_put_regulator:
-	regulator_put(bu21013_data->regulator);
-err_free_mem:
-	input_free_device(in_dev);
+err_sysfs_create:
+	free_irq(pdata->irq, bu21013_data);
+	i2c_set_clientdata(client, NULL);
+err_init_irq:
+	input_unregister_device(bu21013_data->in_dev);
+err_input_register:
+	wake_up(&bu21013_data->wait);
+err_init_config:
+	if (bu21013_data->tpclk) {
+		if (bu21013_data->ext_clk_enable)
+			clk_disable(bu21013_data->tpclk);
+		clk_put(bu21013_data->tpclk);
+	}
+err_ext_clk:
+	if (pdata->cs_dis)
+		pdata->cs_dis(pdata->cs_pin);
+err_init_cs:
+	if (bu21013_data->regulator) {
+		regulator_disable(bu21013_data->regulator);
+		regulator_put(bu21013_data->regulator);
+	}
+	input_free_device(bu21013_data->in_dev);
+err_alloc:
 	kfree(bu21013_data);
 
-	return error;
+	return retval;
 }
+
 /**
  * bu21013_remove() - removes the i2c-client touchscreen driver
  * @client: i2c client structure pointer
@@ -551,22 +804,28 @@ static int __devexit bu21013_remove(struct i2c_client *client)
 {
 	struct bu21013_ts_data *bu21013_data = i2c_get_clientdata(client);
 
-	bu21013_free_irq(bu21013_data);
-
+	bu21013_data->touch_stopped = true;
+	sysfs_remove_group(&client->dev.kobj, &bu21013_attr_group);
+	wake_up(&bu21013_data->wait);
+	free_irq(bu21013_data->chip->irq, bu21013_data);
 	bu21013_data->chip->cs_dis(bu21013_data->chip->cs_pin);
-
 	input_unregister_device(bu21013_data->in_dev);
 
-	regulator_disable(bu21013_data->regulator);
-	regulator_put(bu21013_data->regulator);
-
+	if (bu21013_data->tpclk) {
+		if (bu21013_data->ext_clk_enable)
+			clk_disable(bu21013_data->tpclk);
+		clk_put(bu21013_data->tpclk);
+	}
+	if (bu21013_data->regulator) {
+		regulator_disable(bu21013_data->regulator);
+		regulator_put(bu21013_data->regulator);
+	}
 	kfree(bu21013_data);
 
-	device_init_wakeup(&client->dev, false);
-
 	return 0;
 }
 
+#ifndef CONFIG_HAS_EARLYSUSPEND
 #ifdef CONFIG_PM
 /**
  * bu21013_suspend() - suspend the touch screen controller
@@ -578,15 +837,8 @@ static int __devexit bu21013_remove(struct i2c_client *client)
 static int bu21013_suspend(struct device *dev)
 {
 	struct bu21013_ts_data *bu21013_data = dev_get_drvdata(dev);
-	struct i2c_client *client = bu21013_data->client;
-
-	bu21013_data->touch_stopped = true;
-	if (device_may_wakeup(&client->dev))
-		enable_irq_wake(bu21013_data->chip->irq);
-	else
-		disable_irq(bu21013_data->chip->irq);
 
-	regulator_disable(bu21013_data->regulator);
+	bu21013_disable(bu21013_data);
 
 	return 0;
 }
@@ -601,29 +853,8 @@ static int bu21013_suspend(struct device *dev)
 static int bu21013_resume(struct device *dev)
 {
 	struct bu21013_ts_data *bu21013_data = dev_get_drvdata(dev);
-	struct i2c_client *client = bu21013_data->client;
-	int retval;
 
-	retval = regulator_enable(bu21013_data->regulator);
-	if (retval < 0) {
-		dev_err(&client->dev, "bu21013 regulator enable failed\n");
-		return retval;
-	}
-
-	retval = bu21013_init_chip(bu21013_data);
-	if (retval < 0) {
-		dev_err(&client->dev, "bu21013 controller config failed\n");
-		return retval;
-	}
-
-	bu21013_data->touch_stopped = false;
-
-	if (device_may_wakeup(&client->dev))
-		disable_irq_wake(bu21013_data->chip->irq);
-	else
-		enable_irq(bu21013_data->chip->irq);
-
-	return 0;
+	return bu21013_enable(bu21013_data);
 }
 
 static const struct dev_pm_ops bu21013_dev_pm_ops = {
@@ -631,6 +862,26 @@ static const struct dev_pm_ops bu21013_dev_pm_ops = {
 	.resume  = bu21013_resume,
 };
 #endif
+#else
+static void bu21013_ts_early_suspend(struct early_suspend *data)
+{
+	struct bu21013_ts_data *bu21013_data =
+		container_of(data, struct bu21013_ts_data, early_suspend);
+	bu21013_disable(bu21013_data);
+}
+
+static void bu21013_ts_late_resume(struct early_suspend *data)
+{
+	struct bu21013_ts_data *bu21013_data =
+		container_of(data, struct bu21013_ts_data, early_suspend);
+	struct i2c_client *client = bu21013_data->client;
+	int retval;
+
+	retval = bu21013_enable(bu21013_data);
+	if (retval < 0)
+		dev_err(&client->dev, "bu21013 enable failed\n");
+}
+#endif
 
 static const struct i2c_device_id bu21013_id[] = {
 	{ DRIVER_TP, 0 },
@@ -642,7 +893,7 @@ static struct i2c_driver bu21013_driver = {
 	.driver	= {
 		.name	=	DRIVER_TP,
 		.owner	=	THIS_MODULE,
-#ifdef CONFIG_PM
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
 		.pm	=	&bu21013_dev_pm_ops,
 #endif
 	},
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index b84e46bdedc..aef43a3cc31 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -50,6 +50,14 @@ config LEDS_LM3530
 	  controlled manually or using PWM input or using ambient
 	  light automatically.
 
+config LEDS_AB5500
+	tristate "HVLED driver for AB5500"
+	depends on AB5500_CORE
+	help
+	  This option enables support for the HVLED in AB5500
+	  multi function device. Currently Ab5500 v1.0 chip leds
+	  are supported.
+
 config LEDS_LOCOMO
 	tristate "LED Support for Locomo device"
 	depends on LEDS_CLASS
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index cb77b9bb2f9..80af2aaa335 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -10,6 +10,7 @@ obj-$(CONFIG_LEDS_ATMEL_PWM)		+= leds-atmel-pwm.o
 obj-$(CONFIG_LEDS_BD2802)		+= leds-bd2802.o
 obj-$(CONFIG_LEDS_LOCOMO)		+= leds-locomo.o
 obj-$(CONFIG_LEDS_LM3530)		+= leds-lm3530.o
+obj-$(CONFIG_LEDS_AB5500)		+= leds-ab5500.o
 obj-$(CONFIG_LEDS_MIKROTIK_RB532)	+= leds-rb532.o
 obj-$(CONFIG_LEDS_S3C24XX)		+= leds-s3c24xx.o
 obj-$(CONFIG_LEDS_AMS_DELTA)		+= leds-ams-delta.o
diff --git a/drivers/leds/leds-ab5500.c b/drivers/leds/leds-ab5500.c
new file mode 100644
index 00000000000..e4376d1b59b
--- /dev/null
+++ b/drivers/leds/leds-ab5500.c
@@ -0,0 +1,431 @@
+/*
+ * Copyright (C) 2011 ST-Ericsson SA.
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Driver for LED in ST-Ericsson AB5500 v1.0 Analog baseband Controller
+ *
+ * Author: Shreshtha Kumar SAHU <shreshthakumar.sahu@stericsson.com>
+ */
+
+#include <linux/leds.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/input.h>
+#include <linux/mfd/abx500.h>
+#include <linux/leds-ab5500.h>
+#include <linux/types.h>
+
+#define AB5500LED_NAME		"ab5500-leds"
+
+/* Register offsets */
+#define AB5500_LED_REG_ENABLE	0x03
+#define AB5500_LED_FADE_CTL	0x0D
+
+/* LED-0 */
+#define AB5500_LED0_PWM_DUTY	0x01
+#define AB5500_LED0_PWMFREQ	0x02
+#define AB5500_LED0_SINKCTL	0x0A
+
+/* LED-1 */
+#define AB5500_LED1_PWM_DUTY	0x05
+#define AB5500_LED1_PWMFREQ	0x06
+#define AB5500_LED1_SINKCTL	0x0B
+
+/* LED-2 */
+#define AB5500_LED2_PWM_DUTY	0x08
+#define AB5500_LED2_PWMFREQ	0x09
+#define AB5500_LED2_SINKCTL	0x0C
+
+/* pwm duty cycle */
+#define AB5500_LED_PWMDUTY_OFF	0x0
+#define AB5500_LED_PWMDUTY_MAX	0x3FF
+#define AB5500_LED_PWMDUTY_STEP	(AB5500_LED_PWMDUTY_MAX/LED_FULL)
+
+/* pwm frequency */
+#define AB5500_LED_PWMFREQ_MAX	0x0F	/* 373.39 @sysclk=26MHz */
+#define AB5500_LED_PWMFREQ_SHIFT	4
+
+/* LED sink current control */
+#define AB5500_LED_SINKCURR_MAX	0x0F	/* 40mA */
+#define AB5500_LED_SINKCURR_SHIFT	4
+
+struct ab5500_led {
+	u8 id;
+	u8 max_current;
+	u16 brt_val;
+	enum ab5500_led_status status;
+	struct led_classdev led_cdev;
+	struct work_struct led_work;
+};
+
+struct ab5500_hvleds {
+	struct mutex lock;
+	struct device *dev;
+	struct ab5500_hvleds_platform_data *pdata;
+	struct ab5500_led leds[AB5500_HVLEDS_MAX];
+};
+
+static u8 ab5500_led_pwmduty_reg[] = {
+			AB5500_LED0_PWM_DUTY,
+			AB5500_LED1_PWM_DUTY,
+			AB5500_LED2_PWM_DUTY,
+};
+
+static u8 ab5500_led_pwmfreq_reg[] = {
+			AB5500_LED0_PWMFREQ,
+			AB5500_LED1_PWMFREQ,
+			AB5500_LED2_PWMFREQ,
+};
+
+static u8 ab5500_led_sinkctl_reg[] = {
+			AB5500_LED0_SINKCTL,
+			AB5500_LED1_SINKCTL,
+			AB5500_LED2_SINKCTL
+};
+
+#define to_led(_x)	container_of(_x, struct ab5500_led, _x)
+
+static inline struct ab5500_hvleds *led_to_hvleds(struct ab5500_led *led)
+{
+	return container_of(led, struct ab5500_hvleds, leds[led->id]);
+}
+
+static int ab5500_led_pwmduty_write(struct ab5500_hvleds *hvleds,
+			unsigned int led_id, u16 val)
+{
+	int ret;
+	int val_lsb = val & 0xFF;
+	int val_msb = (val & 0x300) >> 8;
+
+	mutex_lock(&hvleds->lock);
+
+	dev_dbg(hvleds->dev, "ab5500-leds: reg[%d] w val = %d\n"
+		 "reg[%d] w val = %d\n",
+		 ab5500_led_pwmduty_reg[led_id] - 1, val_lsb,
+		 ab5500_led_pwmduty_reg[led_id], val_msb);
+
+	ret = abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_pwmduty_reg[led_id] - 1, val_lsb);
+	ret |= abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_pwmduty_reg[led_id], val_msb);
+	if (ret < 0)
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+			ab5500_led_pwmduty_reg[led_id], ret);
+	mutex_unlock(&hvleds->lock);
+
+	return ret;
+}
+
+static int ab5500_led_pwmfreq_write(struct ab5500_hvleds *hvleds,
+			unsigned int led_id, u8 val)
+{
+	int ret;
+
+	val = (val & 0x0F) << AB5500_LED_PWMFREQ_SHIFT;
+
+	mutex_lock(&hvleds->lock);
+
+	dev_dbg(hvleds->dev, "ab5500-leds: reg[%d] w val=%d\n",
+			ab5500_led_pwmfreq_reg[led_id], val);
+
+	ret = abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_pwmfreq_reg[led_id], val);
+	if (ret < 0)
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+			ab5500_led_pwmfreq_reg[led_id], ret);
+	mutex_unlock(&hvleds->lock);
+
+	return ret;
+}
+
+static int ab5500_led_sinkctl_write(struct ab5500_hvleds *hvleds,
+			unsigned int led_id, u8 val)
+{
+	int ret;
+
+	val = (val & 0x0F) << AB5500_LED_SINKCURR_SHIFT;
+
+	mutex_lock(&hvleds->lock);
+
+	dev_dbg(hvleds->dev, "ab5500-leds: reg[%d] w val=%d\n",
+			ab5500_led_sinkctl_reg[led_id], val);
+
+	ret = abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_sinkctl_reg[led_id], val);
+	if (ret < 0)
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+			ab5500_led_sinkctl_reg[led_id], ret);
+	mutex_unlock(&hvleds->lock);
+
+	return ret;
+}
+
+static int ab5500_led_sinkctl_read(struct ab5500_hvleds *hvleds,
+			unsigned int led_id)
+{
+	int ret;
+	u8 val;
+
+	mutex_lock(&hvleds->lock);
+	ret = abx500_get_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_sinkctl_reg[led_id], &val);
+	if (ret < 0) {
+		dev_err(hvleds->dev, "reg[%d] r failed: %d\n",
+			ab5500_led_sinkctl_reg[led_id], ret);
+		mutex_unlock(&hvleds->lock);
+		return ret;
+	}
+	val = (val & 0xF0) >> AB5500_LED_SINKCURR_SHIFT;
+	mutex_unlock(&hvleds->lock);
+
+	return val;
+}
+
+static void ab5500_led_brightness_set(struct led_classdev *led_cdev,
+			enum led_brightness brt_val)
+{
+	struct ab5500_led *led = to_led(led_cdev);
+
+	/* adjust LED_FULL to 10bit range */
+	brt_val &= LED_FULL;
+	led->brt_val = brt_val * AB5500_LED_PWMDUTY_STEP;
+	schedule_work(&led->led_work);
+}
+
+static void ab5500_led_work(struct work_struct *led_work)
+{
+	struct ab5500_led *led = to_led(led_work);
+	struct ab5500_hvleds *hvleds = led_to_hvleds(led);
+
+	if (led->status == AB5500_LED_ON)
+		ab5500_led_pwmduty_write(hvleds, led->id, led->brt_val);
+}
+
+static ssize_t ab5500_led_show_current(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	int led_curr = 0;
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct ab5500_led *led = to_led(led_cdev);
+	struct ab5500_hvleds *hvleds = led_to_hvleds(led);
+
+	led_curr = ab5500_led_sinkctl_read(hvleds, led->id);
+
+	if (led_curr < 0)
+		return led_curr;
+
+	return sprintf(buf, "%d\n", led_curr);
+}
+
+static ssize_t ab5500_led_store_current(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t len)
+{
+	int ret;
+	unsigned long led_curr;
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct ab5500_led *led = to_led(led_cdev);
+	struct ab5500_hvleds *hvleds = led_to_hvleds(led);
+
+	if (strict_strtoul(buf, 0, &led_curr))
+		return -EINVAL;
+
+	if (led_curr > led->max_current)
+		led_curr = led->max_current;
+
+	ret = ab5500_led_sinkctl_write(hvleds, led->id, led_curr);
+	if (ret < 0)
+		return ret;
+
+	return len;
+}
+
+/* led class device attributes */
+static DEVICE_ATTR(led_current, S_IRUGO | S_IWUGO,
+	       ab5500_led_show_current, ab5500_led_store_current);
+
+static int ab5500_led_init_registers(struct ab5500_hvleds *hvleds)
+{
+	int ret = 0;
+	unsigned int led_id;
+
+	/*  fade - manual : dur mid : pwm duty mid */
+	ret = abx500_set_register_interruptible(
+				hvleds->dev, AB5500_BANK_LED,
+				AB5500_LED_REG_ENABLE, true);
+	if (ret < 0) {
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+			       AB5500_LED_REG_ENABLE,	ret);
+		return ret;
+	}
+
+	for (led_id = 0; led_id < AB5500_HVLEDS_MAX; led_id++) {
+		/* Set pwm freq. and sink current to mid values */
+		ret = ab5500_led_pwmfreq_write(
+				hvleds, led_id, AB5500_LED_PWMFREQ_MAX);
+		if (ret < 0)
+			return ret;
+
+		ret = ab5500_led_sinkctl_write(
+				hvleds, led_id, AB5500_LED_SINKCURR_MAX);
+		if (ret < 0)
+			return ret;
+
+		/* init led off */
+		ret = ab5500_led_pwmduty_write(
+				hvleds, led_id, AB5500_LED_PWMDUTY_OFF);
+		if (ret < 0)
+			return ret;
+	}
+
+	return ret;
+}
+
+static int ab5500_led_register_leds(struct device *dev,
+			struct ab5500_hvleds_platform_data *pdata,
+			struct ab5500_hvleds *hvleds)
+{
+	int i_led;
+	int err;
+	struct ab5500_led_conf *pled;
+	struct ab5500_led *led;
+
+	hvleds->dev = dev;
+	hvleds->pdata = pdata;
+	for (i_led = 0; i_led < AB5500_HVLEDS_MAX; i_led++) {
+		pled = &pdata->leds[i_led];
+		led = &hvleds->leds[i_led];
+
+		INIT_WORK(&led->led_work, ab5500_led_work);
+
+		led->id = pled->led_id;
+		led->max_current = pled->max_current;
+		led->status = pled->status;
+		led->led_cdev.name = pled->name;
+		led->led_cdev.brightness_set = ab5500_led_brightness_set;
+
+		err = led_classdev_register(dev, &led->led_cdev);
+		if (err < 0) {
+			dev_err(dev, "Register led class failed: %d\n", err);
+			goto bailout1;
+		}
+
+		err = device_create_file(led->led_cdev.dev,
+						&dev_attr_led_current);
+		if (err < 0) {
+			dev_err(dev, "sysfs device creation failed: %d\n", err);
+			goto bailout2;
+		}
+	}
+
+	return err;
+	for (; i_led >= 0; i_led--) {
+		device_remove_file(led->led_cdev.dev, &dev_attr_led_current);
+bailout2:
+		led_classdev_unregister(&hvleds->leds[i_led].led_cdev);
+bailout1:
+		cancel_work_sync(&hvleds->leds[i_led].led_work);
+	}
+	return err;
+}
+
+static int __devinit ab5500_hvleds_probe(struct platform_device *pdev)
+{
+	struct ab5500_hvleds_platform_data *pdata = pdev->dev.platform_data;
+	struct ab5500_hvleds *hvleds = NULL;
+	int err = 0, i;
+
+	if (pdata == NULL) {
+		dev_err(&pdev->dev, "platform data required\n");
+		err = -ENODEV;
+		goto err_out;
+	}
+
+	hvleds = kzalloc(sizeof(struct ab5500_hvleds), GFP_KERNEL);
+	if (hvleds == NULL) {
+		err = -ENOMEM;
+		goto err_out;
+	}
+
+	mutex_init(&hvleds->lock);
+
+	/* init leds data and register led_classdev */
+	err = ab5500_led_register_leds(&pdev->dev, pdata, hvleds);
+	if (err < 0) {
+		dev_err(&pdev->dev, "leds registeration failed\n");
+		goto err_out;
+	}
+
+	/* init device registers and set initial led current */
+	err = ab5500_led_init_registers(hvleds);
+	if (err < 0) {
+		dev_err(&pdev->dev, "reg init failed: %d\n", err);
+		goto err_reg_init;
+	}
+
+	dev_info(&pdev->dev, "enabled\n");
+
+	return err;
+
+err_reg_init:
+	for (i = 0; i < AB5500_HVLEDS_MAX; i++) {
+		struct ab5500_led *led = &hvleds->leds[i];
+
+		led_classdev_unregister(&led->led_cdev);
+		device_remove_file(led->led_cdev.dev, &dev_attr_led_current);
+		cancel_work_sync(&led->led_work);
+	}
+err_out:
+	kfree(hvleds);
+	return err;
+}
+
+static int __devexit ab5500_hvleds_remove(struct platform_device *pdev)
+{
+	struct ab5500_hvleds *hvleds = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < AB5500_HVLEDS_MAX; i++) {
+		struct ab5500_led *led = &hvleds->leds[i];
+
+		led_classdev_unregister(&led->led_cdev);
+		device_remove_file(led->led_cdev.dev, &dev_attr_led_current);
+		cancel_work_sync(&led->led_work);
+	}
+	kfree(hvleds);
+	return 0;
+}
+
+static struct platform_driver ab5500_hvleds_driver = {
+	.driver   = {
+		   .name = AB5500LED_NAME,
+		   .owner = THIS_MODULE,
+	},
+	.probe    = ab5500_hvleds_probe,
+	.remove   = __devexit_p(ab5500_hvleds_remove),
+};
+
+static int __init ab5500_hvleds_module_init(void)
+{
+	return platform_driver_register(&ab5500_hvleds_driver);
+}
+
+static void __exit ab5500_hvleds_module_exit(void)
+{
+	platform_driver_unregister(&ab5500_hvleds_driver);
+}
+
+module_init(ab5500_hvleds_module_init);
+module_exit(ab5500_hvleds_module_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Shreshtha Kumar SAHU <shreshthakumar.sahu@stericsson.com>");
+MODULE_DESCRIPTION("Driver for AB5500 HVLED");
+
diff --git a/drivers/leds/leds-lm3530.c b/drivers/leds/leds-lm3530.c
index 4d7ce7631ac..3dd7090a9a9 100644
--- a/drivers/leds/leds-lm3530.c
+++ b/drivers/leds/leds-lm3530.c
@@ -68,17 +68,16 @@
 #define LM3530_ALS2_IMP_SHIFT		(4)
 
 /* Zone Boundary Register defaults */
-#define LM3530_DEF_ZB_0			(0x33)
-#define LM3530_DEF_ZB_1			(0x66)
-#define LM3530_DEF_ZB_2			(0x99)
-#define LM3530_DEF_ZB_3			(0xCC)
+#define LM3530_ALS_ZB_MAX		(4)
+#define LM3530_ALS_WINDOW_mV		(1000)
+#define LM3530_ALS_OFFSET_mV		(4)
 
 /* Zone Target Register defaults */
-#define LM3530_DEF_ZT_0			(0x19)
-#define LM3530_DEF_ZT_1			(0x33)
+#define LM3530_DEF_ZT_0			(0x7F)
+#define LM3530_DEF_ZT_1			(0x66)
 #define LM3530_DEF_ZT_2			(0x4C)
-#define LM3530_DEF_ZT_3			(0x66)
-#define LM3530_DEF_ZT_4			(0x7F)
+#define LM3530_DEF_ZT_3			(0x33)
+#define LM3530_DEF_ZT_4			(0x19)
 
 struct lm3530_mode_map {
 	const char *mode;
@@ -150,6 +149,8 @@ static int lm3530_init_registers(struct lm3530_data *drvdata)
 	u8 als_imp_sel = 0;
 	u8 brightness;
 	u8 reg_val[LM3530_REG_MAX];
+	u8 zones[LM3530_ALS_ZB_MAX];
+	u32 als_vmin, als_vmax, als_vstep;
 	struct lm3530_platform_data *pltfm = drvdata->pdata;
 	struct i2c_client *client = drvdata->client;
 
@@ -161,6 +162,26 @@ static int lm3530_init_registers(struct lm3530_data *drvdata)
 		gen_config |= (LM3530_ENABLE_I2C);
 
 	if (drvdata->mode == LM3530_BL_MODE_ALS) {
+		if (pltfm->als_vmax == 0) {
+			pltfm->als_vmin = als_vmin = 0;
+			pltfm->als_vmin = als_vmax = LM3530_ALS_WINDOW_mV;
+		}
+
+		als_vmin = pltfm->als_vmin;
+		als_vmax = pltfm->als_vmax;
+
+		if ((als_vmax - als_vmin) > LM3530_ALS_WINDOW_mV)
+			pltfm->als_vmax = als_vmax =
+				als_vmin + LM3530_ALS_WINDOW_mV;
+
+		/* n zone boundary makes n+1 zones */
+		als_vstep = (als_vmax - als_vmin) / (LM3530_ALS_ZB_MAX + 1);
+
+		for (i = 0; i < LM3530_ALS_ZB_MAX; i++)
+			zones[i] = (((als_vmin + LM3530_ALS_OFFSET_mV) +
+					als_vstep + (i * als_vstep)) * LED_FULL)
+					/ 1000;
+
 		als_config =
 			(pltfm->als_avrg_time << LM3530_ALS_AVG_TIME_SHIFT) |
 			(LM3530_ENABLE_ALS) |
@@ -169,6 +190,7 @@ static int lm3530_init_registers(struct lm3530_data *drvdata)
 		als_imp_sel =
 			(pltfm->als1_resistor_sel << LM3530_ALS1_IMP_SHIFT) |
 			(pltfm->als2_resistor_sel << LM3530_ALS2_IMP_SHIFT);
+
 	}
 
 	if (drvdata->mode == LM3530_BL_MODE_PWM)
@@ -190,10 +212,10 @@ static int lm3530_init_registers(struct lm3530_data *drvdata)
 	reg_val[3] = 0x00;		/* LM3530_ALS_ZONE_REG */
 	reg_val[4] = als_imp_sel;	/* LM3530_ALS_IMP_SELECT */
 	reg_val[5] = brightness;	/* LM3530_BRT_CTRL_REG */
-	reg_val[6] = LM3530_DEF_ZB_0;	/* LM3530_ALS_ZB0_REG */
-	reg_val[7] = LM3530_DEF_ZB_1;	/* LM3530_ALS_ZB1_REG */
-	reg_val[8] = LM3530_DEF_ZB_2;	/* LM3530_ALS_ZB2_REG */
-	reg_val[9] = LM3530_DEF_ZB_3;	/* LM3530_ALS_ZB3_REG */
+	reg_val[6] = zones[0];		/* LM3530_ALS_ZB0_REG */
+	reg_val[7] = zones[1];		/* LM3530_ALS_ZB1_REG */
+	reg_val[8] = zones[2];		/* LM3530_ALS_ZB2_REG */
+	reg_val[9] = zones[3];		/* LM3530_ALS_ZB3_REG */
 	reg_val[10] = LM3530_DEF_ZT_0;	/* LM3530_ALS_Z0T_REG */
 	reg_val[11] = LM3530_DEF_ZT_1;	/* LM3530_ALS_Z1T_REG */
 	reg_val[12] = LM3530_DEF_ZT_2;	/* LM3530_ALS_Z2T_REG */
@@ -265,6 +287,24 @@ static void lm3530_brightness_set(struct led_classdev *led_cdev,
 	}
 }
 
+static ssize_t lm3530_mode_get(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = container_of(
+					dev->parent, struct i2c_client, dev);
+	struct lm3530_data *drvdata = i2c_get_clientdata(client);
+	int i, len = 0;
+
+	for (i = 0; i < ARRAY_SIZE(mode_map); i++)
+		if (drvdata->mode == mode_map[i].mode_val)
+			len += sprintf(buf + len, "[%s] ", mode_map[i].mode);
+		else
+			len += sprintf(buf + len, "%s ", mode_map[i].mode);
+
+	len += sprintf(buf + len, "\n");
+
+	return len;
+}
 
 static ssize_t lm3530_mode_set(struct device *dev, struct device_attribute
 				   *attr, const char *buf, size_t size)
@@ -298,8 +338,7 @@ static ssize_t lm3530_mode_set(struct device *dev, struct device_attribute
 
 	return sizeof(drvdata->mode);
 }
-
-static DEVICE_ATTR(mode, 0644, NULL, lm3530_mode_set);
+static DEVICE_ATTR(mode, 0644, lm3530_mode_get, lm3530_mode_set);
 
 static int __devinit lm3530_probe(struct i2c_client *client,
 			   const struct i2c_device_id *id)
diff --git a/drivers/leds/leds-pwm.c b/drivers/leds/leds-pwm.c
index 666daf77872..775cd67c604 100644
--- a/drivers/leds/leds-pwm.c
+++ b/drivers/leds/leds-pwm.c
@@ -27,6 +27,7 @@ struct led_pwm_data {
 	struct led_classdev	cdev;
 	struct pwm_device	*pwm;
 	unsigned int 		active_low;
+	unsigned int		lth_brightness;
 	unsigned int		period;
 };
 
@@ -42,7 +43,10 @@ static void led_pwm_set(struct led_classdev *led_cdev,
 		pwm_config(led_dat->pwm, 0, period);
 		pwm_disable(led_dat->pwm);
 	} else {
-		pwm_config(led_dat->pwm, brightness * period / max, period);
+		brightness = led_dat->lth_brightness + (brightness *
+			(led_dat->period - led_dat->lth_brightness) / max);
+		pwm_config(led_dat->pwm, brightness, led_dat->period);
+
 		pwm_enable(led_dat->pwm);
 	}
 }
@@ -79,6 +83,8 @@ static int led_pwm_probe(struct platform_device *pdev)
 		led_dat->cdev.default_trigger = cur_led->default_trigger;
 		led_dat->active_low = cur_led->active_low;
 		led_dat->period = cur_led->pwm_period_ns;
+		led_dat->lth_brightness = cur_led->lth_brightness *
+		      (cur_led->pwm_period_ns / cur_led->max_brightness);
 		led_dat->cdev.brightness_set = led_pwm_set;
 		led_dat->cdev.brightness = LED_OFF;
 		led_dat->cdev.max_brightness = cur_led->max_brightness;
diff --git a/drivers/media/radio/CG2900/cg2900_fm_api.c b/drivers/media/radio/CG2900/cg2900_fm_api.c
new file mode 100644
index 00000000000..b6be4e462e3
--- /dev/null
+++ b/drivers/media/radio/CG2900/cg2900_fm_api.c
@@ -0,0 +1,3106 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Linux FM Host API's for ST-Ericsson FM Chip.
+ *
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#include <linux/firmware.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/kthread.h>
+#include <linux/kernel.h>
+#include <linux/mutex.h>
+#include "cg2900_fm_driver.h"
+
+#define CG2900_FM_BT_SRC_COEFF_INFO_FILE "cg2900_fm_bt_src_coeff_info.fw"
+#define CG2900_FM_EXT_SRC_COEFF_INFO_FILE "cg2900_fm_ext_src_coeff_info.fw"
+#define CG2900_FM_FM_COEFF_INFO_FILE	"cg2900_fm_fm_coeff_info.fw"
+#define CG2900_FM_FM_PROG_INFO_FILE	"cg2900_fm_fm_prog_info.fw"
+#define CG2900_FM_LINE_BUFFER_LENGTH	128
+#define CG2900_FM_FILENAME_MAX		128
+#define FW_FILE_PARAM_LEN				3
+/* RDS Tx PTY set to Other music */
+#define OTHER_MUSIC			15
+
+static bool fm_rds_status;
+static bool fm_prev_rds_status;
+static u16 program_identification_code;
+static u16 default_program_identification_code = 0x1234;
+static u16 program_type_code;
+static u16 default_program_type_code = OTHER_MUSIC;
+static char program_service[MAX_PSN_SIZE];
+static char default_program_service[MAX_PSN_SIZE] = "FM-Xmit ";
+static char radio_text[MAX_RT_SIZE];
+static char default_radio_text[MAX_RT_SIZE] = "Default Radio Text "
+    "Default Radio Text Default Radio Text Default";
+static bool a_b_flag;
+u8 fm_event;
+static struct mutex rds_mutex;
+struct cg2900_fm_rds_buf fm_rds_buf[MAX_RDS_BUFFER][MAX_RDS_GROUPS];
+struct cg2900_fm_rds_info fm_rds_info;
+static enum cg2900_fm_state fm_state;
+static enum cg2900_fm_mode fm_mode;
+static struct cg2900_version_info version_info;
+
+/**
+ * cg2900_fm_get_one_line_of_text()- Get One line of text from a file.
+ *
+ * Replacement function for stdio function fgets.This function extracts one
+ * line of text from input file.
+ *
+ * @wr_buffer: Buffer to copy text to.
+ * @max_nbr_of_bytes: Max number of bytes to read, i.e. size of rd_buffer.
+ * @rd_buffer: Data to parse.
+ * @bytes_copied: Number of bytes copied to wr_buffer.
+ *
+ * Returns:
+ *   Pointer to next data to read.
+ */
+static char *cg2900_fm_get_one_line_of_text(
+			char *wr_buffer,
+			int max_nbr_of_bytes,
+			char *rd_buffer,
+			int *bytes_copied
+			)
+{
+	char *curr_wr = wr_buffer;
+	char *curr_rd = rd_buffer;
+	char in_byte;
+
+	*bytes_copied = 0;
+
+	do {
+		*curr_wr = *curr_rd;
+		in_byte = *curr_wr;
+		curr_wr++;
+		curr_rd++;
+		(*bytes_copied)++;
+	} while ((*bytes_copied <= max_nbr_of_bytes) && (in_byte != '\0')
+		 && (in_byte != '\n'));
+	*curr_wr = '\0';
+	return curr_rd;
+}
+
+/**
+ * cg2900_fm_get_file_to_load() - Parse info file and find correct target file.
+ *
+ * @fw:	Firmware structure containing file data.
+ * @file_name: (out) Pointer to name of requested file.
+ *
+ * Returns:
+ *   True,  if target file was found,
+ *   False, otherwise.
+ */
+static bool cg2900_fm_get_file_to_load(
+			const struct firmware *fw,
+			char **file_name
+			)
+{
+	char *line_buffer;
+	char *curr_file_buffer;
+	int bytes_left_to_parse = fw->size;
+	int bytes_read = 0;
+	bool file_found = false;
+
+	curr_file_buffer = (char *)&(fw->data[0]);
+
+	line_buffer = kmalloc(CG2900_FM_LINE_BUFFER_LENGTH,
+				GFP_KERNEL);
+
+	if (line_buffer == NULL) {
+		FM_ERR_REPORT("Failed to allocate:"
+			      "file_name 0x%X, line_buffer 0x%X",
+			      (unsigned int)file_name,
+			      (unsigned int)line_buffer);
+		goto error;
+	}
+
+	while (!file_found) {
+		/* Get one line of text from the file to parse */
+		curr_file_buffer =
+		    cg2900_fm_get_one_line_of_text(line_buffer,
+					min
+					(CG2900_FM_LINE_BUFFER_LENGTH,
+					 (int)(fw->size -
+						bytes_read)),
+						curr_file_buffer,
+						&bytes_read);
+
+		bytes_left_to_parse -= bytes_read;
+		if (bytes_left_to_parse <= 0) {
+			/* End of file => Leave while loop */
+			FM_ERR_REPORT("Reached end of file."
+				      "No file found!");
+			break;
+		}
+
+		/*
+		 * Check if the line of text is a comment
+		 * or not, comments begin with '#'
+		 */
+		if (*line_buffer != '#') {
+			u32 hci_rev = 0;
+			u32 lmp_sub = 0;
+
+			FM_DEBUG_REPORT("Found a valid line <%s>",
+					line_buffer);
+
+			/*
+			 * Check if we can find the correct
+			 * HCI revision and LMP subversion
+			 * as well as a file name in the text line
+			 * Store the filename if the actual file can
+			 * be found in the file system
+			 */
+			if (sscanf(line_buffer, "%x%x%s",
+				   (unsigned int *)&hci_rev,
+				   (unsigned int *)&lmp_sub,
+				   *file_name) == FW_FILE_PARAM_LEN
+			    && hci_rev == version_info.revision
+			    && lmp_sub == version_info.sub_version) {
+				FM_INFO_REPORT("File name = %s "
+					       "HCI Revision"
+					       "= 0x%04X LMP "
+					       "Subversion = 0x%04X",
+					       *file_name,
+					       (unsigned int)hci_rev,
+					       (unsigned int)lmp_sub);
+
+				/*
+				 * Name has already been stored above.
+				 * Nothing more to do
+				 */
+				file_found = true;
+			} else {
+		/*Zero the name buffer so  it is clear to next read*/
+				memset(*file_name, 0x00,
+				       CG2900_FM_FILENAME_MAX);
+			}
+		}
+	}
+	kfree(line_buffer);
+error:
+	return file_found;
+}
+
+/**
+ * cg2900_fm_load_firmware() - Loads the FM Coeffecients and F/W file(s)
+ *
+ * @device: Pointer to char device requesting the operation.
+ *
+ * Returns:
+ *  0, if firmware download is successful
+ *  -ENOENT, file not found.
+ *  -ENOMEM, out of memory
+ */
+static int cg2900_fm_load_firmware(
+			struct device *device
+			)
+{
+	int err;
+	bool file_found;
+	int result = 0;
+	const struct firmware *bt_src_coeff_info;
+	const struct firmware *ext_src_coeff_info;
+	const struct firmware *fm_coeff_info;
+	const struct firmware *fm_prog_info;
+	char *bt_src_coeff_file_name = NULL;
+	char *ext_src_coeff_file_name = NULL;
+	char *fm_coeff_file_name = NULL;
+	char *fm_prog_file_name = NULL;
+
+	FM_INFO_REPORT("+cg2900_fm_load_firmware");
+
+	/* Open bt_src_coeff info file. */
+	err = request_firmware(&bt_src_coeff_info,
+			       CG2900_FM_BT_SRC_COEFF_INFO_FILE, device);
+	if (err) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get bt_src_coeff info file");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/*
+	 * Now we have the bt_src_coeff info file.
+	 * See if we can find the right bt_src_coeff file as well
+	 */
+	bt_src_coeff_file_name = kmalloc(CG2900_FM_FILENAME_MAX,
+				GFP_KERNEL);
+	if (bt_src_coeff_file_name == NULL) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't allocate memory for "
+			      "bt_src_coeff_file_name");
+		release_firmware(bt_src_coeff_info);
+		result = -ENOMEM;
+		goto error;
+	}
+	file_found = cg2900_fm_get_file_to_load(bt_src_coeff_info,
+					     &bt_src_coeff_file_name);
+
+	/* Now we are finished with the bt_src_coeff info file */
+	release_firmware(bt_src_coeff_info);
+
+	if (!file_found) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't find bt_src_coeff file!! "
+			      "Major error!!!");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Open ext_src_coeff info file. */
+	err = request_firmware(&ext_src_coeff_info,
+			       CG2900_FM_EXT_SRC_COEFF_INFO_FILE, device);
+	if (err) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get ext_src_coeff_info info file");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/*
+	 * Now we have the ext_src_coeff info file. See if we can
+	 * find the right ext_src_coeff file as well
+	 */
+	ext_src_coeff_file_name = kmalloc(CG2900_FM_FILENAME_MAX,
+				GFP_KERNEL);
+	if (ext_src_coeff_file_name == NULL) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't allocate memory for "
+			      "ext_src_coeff_file_name");
+		release_firmware(ext_src_coeff_info);
+		result = -ENOMEM;
+		goto error;
+	}
+	file_found = cg2900_fm_get_file_to_load(ext_src_coeff_info,
+					     &ext_src_coeff_file_name);
+
+	/* Now we are finished with the ext_src_coeff info file */
+	release_firmware(ext_src_coeff_info);
+
+	if (!file_found) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't find ext_src_coeff_info "
+			      "file!!! Major error!");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Open fm_coeff info file. */
+	err = request_firmware(&fm_coeff_info,
+			       CG2900_FM_FM_COEFF_INFO_FILE, device);
+	if (err) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get fm_coeff info file");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/*
+	 * Now we have the fm_coeff_info info file.
+	 * See if we can find the right fm_coeff_info file as well
+	 */
+	fm_coeff_file_name = kmalloc(CG2900_FM_FILENAME_MAX,
+				GFP_KERNEL);
+	if (fm_coeff_file_name == NULL) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+						"Couldn't allocate memory for "
+						"fm_coeff_file_name");
+		release_firmware(fm_coeff_info);
+		result = -ENOMEM;
+		goto error;
+	}
+	file_found = cg2900_fm_get_file_to_load(fm_coeff_info,
+					     &fm_coeff_file_name);
+
+	/* Now we are finished with the fm_coeff info file */
+	release_firmware(fm_coeff_info);
+
+	if (!file_found) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't find fm_coeff file!!! "
+			      "Major error!");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Open fm_prog info file. */
+	err = request_firmware(&fm_prog_info,
+				CG2900_FM_FM_PROG_INFO_FILE, device);
+	if (err) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get fm_prog_info info file");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/*
+	 * Now we have the fm_prog info file.
+	 * See if we can find the right fm_prog file as well
+	 */
+	fm_prog_file_name = kmalloc(CG2900_FM_FILENAME_MAX,
+				GFP_KERNEL);
+	if (fm_prog_file_name == NULL) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+						"Couldn't allocate memory for "
+						"fm_prog_file_name");
+		release_firmware(fm_prog_info);
+		result = -ENOMEM;
+		goto error;
+	}
+	file_found = cg2900_fm_get_file_to_load(fm_prog_info,
+					     &fm_prog_file_name);
+
+	/* Now we are finished with fm_prog patch info file */
+	release_firmware(fm_prog_info);
+
+	if (!file_found) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't find fm_prog_info file!!! "
+			      "Major error!");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* OK. Now it is time to download the firmware */
+	err = request_firmware(&bt_src_coeff_info,
+			       bt_src_coeff_file_name, device);
+	if (err < 0) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get bt_src_coeff file, err = %d", err);
+		result = -ENOENT;
+		goto error;
+	}
+
+	FM_INFO_REPORT("cg2900_fm_load_firmware: Downloading %s of %d bytes",
+		       bt_src_coeff_file_name, bt_src_coeff_info->size);
+	if (fmd_send_fm_firmware((u8 *) bt_src_coeff_info->data,
+				 bt_src_coeff_info->size)) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: Error in "
+				"downloading %s", bt_src_coeff_file_name);
+		release_firmware(bt_src_coeff_info);
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Now we are finished with the bt_src_coeff info file */
+	release_firmware(bt_src_coeff_info);
+	err = request_firmware(&ext_src_coeff_info,
+			       ext_src_coeff_file_name, device);
+	if (err < 0) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get ext_src_coeff file, err = %d", err);
+		result = -ENOENT;
+		goto error;
+	}
+
+	FM_INFO_REPORT("cg2900_fm_load_firmware: Downloading %s of %d bytes",
+		       ext_src_coeff_file_name, ext_src_coeff_info->size);
+	if (fmd_send_fm_firmware((u8 *) ext_src_coeff_info->data,
+				 ext_src_coeff_info->size)) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: Error in "
+				"downloading %s", ext_src_coeff_file_name);
+		release_firmware(ext_src_coeff_info);
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Now we are finished with the bt_src_coeff info file */
+	release_firmware(ext_src_coeff_info);
+
+	err = request_firmware(&fm_coeff_info, fm_coeff_file_name, device);
+	if (err < 0) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get fm_coeff file, err = %d", err);
+		result = -ENOENT;
+		goto error;
+	}
+
+	FM_INFO_REPORT("cg2900_fm_load_firmware: Downloading %s of %d bytes",
+		       fm_coeff_file_name, fm_coeff_info->size);
+	if (fmd_send_fm_firmware((u8 *) fm_coeff_info->data,
+				 fm_coeff_info->size)) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: Error in "
+				"downloading %s", fm_coeff_file_name);
+		release_firmware(fm_coeff_info);
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Now we are finished with the bt_src_coeff info file */
+	release_firmware(fm_coeff_info);
+
+	err = request_firmware(&fm_prog_info, fm_prog_file_name, device);
+	if (err < 0) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get fm_prog file, err = %d", err);
+		result = -ENOENT;
+		goto error;
+	}
+
+	FM_INFO_REPORT("cg2900_fm_load_firmware: Downloading %s of %d bytes",
+		       fm_prog_file_name, fm_prog_info->size);
+	if (fmd_send_fm_firmware((u8 *) fm_prog_info->data,
+		fm_prog_info->size)) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: Error in "
+				"downloading %s", fm_prog_file_name);
+		release_firmware(fm_prog_info);
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Now we are finished with the bt_src_coeff info file */
+	release_firmware(fm_prog_info);
+
+error:
+	/* Free Allocated memory */
+	if (bt_src_coeff_file_name != NULL)
+		kfree(bt_src_coeff_file_name);
+	if (ext_src_coeff_file_name != NULL)
+		kfree(ext_src_coeff_file_name);
+	if (fm_coeff_file_name != NULL)
+		kfree(fm_coeff_file_name);
+	if (fm_prog_file_name != NULL)
+		kfree(fm_prog_file_name);
+	FM_DEBUG_REPORT("-cg2900_fm_load_firmware: returning %d",
+			result);
+	return result;
+}
+
+/**
+ * cg2900_fm_transmit_rds_groups()- Transmits the RDS Groups.
+ *
+ * Stores the RDS Groups in Chip's buffer and each group is
+ * transmitted every 87.6 ms.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise
+ */
+static int cg2900_fm_transmit_rds_groups(void)
+{
+	int result = 0;
+	u16 group_position = 0;
+	u8 block1[2];
+	u8 block2[2];
+	u8 block3[2];
+	u8 block4[2];
+	int index1 = 0;
+	int index2 = 0;
+	int group_0B_count = 0;
+	int group_2A_count = 0;
+
+	FM_INFO_REPORT("cg2900_fm_transmit_rds_groups");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_transmit_rds_groups: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	while (group_position < 20 && result == 0) {
+		if (group_position < 4) {
+			/* Transmit PSN in Group 0B */
+			block1[0] = program_identification_code;
+			block1[1] = program_identification_code >> 8;
+			/* M/S bit set to Music */
+			if (group_0B_count % 4 == 0) {
+				/* Manipulate DI bit */
+				block2[0] =
+				    (0x08 | ((program_type_code & 0x07)
+					     << 5))
+				    + group_0B_count;
+			} else {
+				block2[0] =
+				    (0x0C | ((program_type_code & 0x07)
+					     << 5))
+				    + group_0B_count;
+			}
+			block2[1] =
+			    0x08 | ((program_type_code & 0x18) >> 3);
+			block3[0] = program_identification_code;
+			block3[1] = program_identification_code >> 8;
+			block4[0] = program_service[index1 + 1];
+			block4[1] = program_service[index1 + 0];
+			index1 += 2;
+			group_0B_count++;
+		} else {
+			/* Transmit RT in Group 2A */
+			block1[0] = program_identification_code;
+			block1[1] = program_identification_code >> 8;
+			if (a_b_flag)
+				block2[0] = (0x10 |
+					     ((program_type_code & 0x07)
+					      << 5)) + group_2A_count;
+			else
+				block2[0] = (0x00 |
+					     ((program_type_code & 0x07)
+					      << 5)) + group_2A_count;
+			block2[1] = 0x20 | ((program_type_code & 0x18)
+					    >> 3);
+			block3[0] = radio_text[index2 + 1];
+			block3[1] = radio_text[index2 + 0];
+			block4[0] = radio_text[index2 + 3];
+			block4[1] = radio_text[index2 + 2];
+			index2 += 4;
+			group_2A_count++;
+		}
+		FM_DEBUG_REPORT("%02x%02x "
+				"%02x%02x "
+				"%02x%02x "
+				"%02x%02x ",
+				block1[1], block1[0],
+				block2[1], block2[0],
+				block3[1], block3[0],
+				block4[1], block4[0]);
+		result = fmd_tx_set_group(
+				group_position,
+				block1,
+				block2,
+				block3,
+				block4);
+		group_position++;
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_transmit_rds_groups: "
+				      "fmd_tx_set_group failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			break;
+		}
+	}
+	a_b_flag = !a_b_flag;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_transmit_rds_groups: returning %d",
+			result);
+	return result;
+}
+
+/**
+ * cg2900_fm_driver_callback()- Callback function indicating the event.
+ *
+ * This callback function is called on receiving irpt_CommandSucceeded,
+ * irpt_CommandFailed, irpt_bufferFull, etc from FM chip.
+ * @event: event for which the callback function was caled
+ * from FM Driver.
+ * @event_successful: Signifying whether the event is called from FM Driver
+ * on receiving irpt_OperationSucceeded or irpt_OperationFailed.
+ */
+static void cg2900_fm_driver_callback(
+			u8 event,
+			bool event_successful
+			)
+{
+	FM_INFO_REPORT("cg2900_fm_driver_callback: "
+			"event = %02x, event_successful = %x",
+			event, event_successful);
+
+	if (event_successful) {
+		switch (event) {
+		case FMD_EVENT_GEN_POWERUP:
+			FM_DEBUG_REPORT("FMD_EVENT_GEN_POWERUP");
+			break;
+		case FMD_EVENT_ANTENNA_STATUS_CHANGED:
+			FM_DEBUG_REPORT("FMD_EVENT_ANTENNA_STATUS_CHANGED");
+			break;
+		case FMD_EVENT_FREQUENCY_CHANGED:
+			FM_DEBUG_REPORT("FMD_EVENT_FREQUENCY_CHANGED ");
+			break;
+
+		case FMD_EVENT_SEEK_STOPPED:
+			FM_DEBUG_REPORT("FMD_EVENT_SEEK_STOPPED");
+			fm_event = CG2900_EVENT_SCAN_CANCELLED;
+			wake_up_poll_queue();
+			break;
+
+		case FMD_EVENT_SEEK_COMPLETED:
+			FM_DEBUG_REPORT("FMD_EVENT_SEEK_COMPLETED");
+			fm_event = CG2900_EVENT_SEARCH_CHANNEL_FOUND;
+			wake_up_poll_queue();
+			break;
+
+		case FMD_EVENT_SCAN_BAND_COMPLETED:
+			FM_DEBUG_REPORT("FMD_EVENT_SCAN_BAND_COMPLETED");
+			fm_event = CG2900_EVENT_SCAN_CHANNELS_FOUND;
+			wake_up_poll_queue();
+			break;
+
+		case FMD_EVENT_BLOCK_SCAN_COMPLETED:
+			FM_DEBUG_REPORT("FMD_EVENT_BLOCK_SCAN_COMPLETED");
+			fm_event = CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND;
+			wake_up_poll_queue();
+			break;
+
+		case FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE:
+			FM_DEBUG_REPORT("FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE");
+			break;
+
+		case FMD_EVENT_RDSGROUP_RCVD:
+			FM_DEBUG_REPORT("FMD_EVENT_RDSGROUP_RCVD");
+			fmd_set_rds_sem();
+			break;
+
+		default:
+			FM_INFO_REPORT("cg2900_fm_driver_callback: "
+				      "Unknown event = %x", event);
+			break;
+		}
+	} else {
+		switch (event) {
+			/*
+			 * Seek stop, band scan, seek, block scan could
+			 * fail for some reason so wake up poll queue
+			 */
+		case FMD_EVENT_SEEK_STOPPED:
+			FM_ERR_REPORT("FMD_EVENT_SEEK_STOPPED");
+			fm_event = CG2900_EVENT_SCAN_CANCELLED;
+			wake_up_poll_queue();
+			break;
+		case FMD_EVENT_SEEK_COMPLETED:
+			FM_ERR_REPORT("FMD_EVENT_SEEK_COMPLETED");
+			fm_event = CG2900_EVENT_SEARCH_CHANNEL_FOUND;
+			wake_up_poll_queue();
+			break;
+		case FMD_EVENT_SCAN_BAND_COMPLETED:
+			FM_ERR_REPORT("FMD_EVENT_SCAN_BAND_COMPLETED");
+			fm_event = CG2900_EVENT_SCAN_CHANNELS_FOUND;
+			wake_up_poll_queue();
+			break;
+		case FMD_EVENT_BLOCK_SCAN_COMPLETED:
+			FM_ERR_REPORT("FMD_EVENT_BLOCK_SCAN_COMPLETED");
+			fm_event = CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND;
+			wake_up_poll_queue();
+			break;
+		default:
+			FM_ERR_REPORT("cg2900_fm_driver_callback: "
+				      "event = %x failed!!!!", event);
+			break;
+		}
+	}
+}
+
+/**
+ * cg2900_fm_rds_callback()- Function to retrieve the RDS groups.
+ *
+ * This is called when the chip has received enough RDS groups
+ * so an interrupt irpt_BufferFull is generated to read the groups.
+ */
+static void cg2900_fm_rds_callback(void)
+{
+	u8 index = 0;
+	u16 rds_local_buf_count;
+	int result;
+	FM_INFO_REPORT("cg2900_fm_rds_callback");
+
+	/*
+	 * Wait till interrupt is RDS Buffer
+	 * full interrupt is received
+	 */
+	fmd_get_rds_sem();
+
+	if (!fm_rds_status)
+		return;
+
+	/* RDS Data available, Read the Groups */
+	mutex_lock(&rds_mutex);
+	result = fmd_int_bufferfull(&rds_local_buf_count);
+
+	if (0 != result)
+		goto error;
+
+	while (index < rds_local_buf_count) {
+		/*
+		 * Status are in reverse order because of Endianness
+		 * of status byte received from chip
+		 */
+		result = fmd_rx_get_low_level_rds_groups(
+			index,
+			&fm_rds_buf[fm_rds_info.rds_head][index].block1,
+			&fm_rds_buf[fm_rds_info.rds_head][index].block2,
+			&fm_rds_buf[fm_rds_info.rds_head][index].block3,
+			&fm_rds_buf[fm_rds_info.rds_head][index].block4,
+			&fm_rds_buf[fm_rds_info.rds_head][index].status2,
+			&fm_rds_buf[fm_rds_info.rds_head][index].status1,
+			&fm_rds_buf[fm_rds_info.rds_head][index].status4,
+			&fm_rds_buf[fm_rds_info.rds_head][index].status3);
+		FM_INFO_REPORT("%04x %04x %04x %04x %02x %02x %02x %02x",
+			fm_rds_buf[fm_rds_info.rds_head][index].block1,
+			fm_rds_buf[fm_rds_info.rds_head][index].block2,
+			fm_rds_buf[fm_rds_info.rds_head][index].block3,
+			fm_rds_buf[fm_rds_info.rds_head][index].block4,
+			fm_rds_buf[fm_rds_info.rds_head][index].status1,
+			fm_rds_buf[fm_rds_info.rds_head][index].status2,
+			fm_rds_buf[fm_rds_info.rds_head][index].status3,
+			fm_rds_buf[fm_rds_info.rds_head][index].status4);
+
+		if (0 != result)
+
+			goto error;
+
+		if (!fm_rds_status)
+			return;
+
+		index++;
+	}
+	fm_rds_info.rds_head++;
+	if (fm_rds_info.rds_head == MAX_RDS_BUFFER)
+		fm_rds_info.rds_head = 0;
+	wake_up_read_queue();
+error:
+	mutex_unlock(&rds_mutex);
+}
+
+int cg2900_fm_init(void)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_init");
+
+	if (CG2900_FM_STATE_DEINITIALIZED != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_init: Already Initialized");
+		result = -EINVAL;
+		goto error;
+	}
+
+	mutex_init(&rds_mutex);
+
+	memset(&fm_rds_info, 0, sizeof(struct cg2900_fm_rds_info));
+	memset(&version_info, 0, sizeof(struct cg2900_version_info));
+	memset(
+			fm_rds_buf,
+			0,
+			sizeof(struct cg2900_fm_rds_buf) *
+			MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+
+	/* Initalize the Driver */
+	if (fmd_init() != 0) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Register the callback */
+	if (fmd_register_callback(
+		(fmd_radio_cb) cg2900_fm_driver_callback) != 0) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* initialize global variables */
+	fm_event = CG2900_EVENT_NO_EVENT;
+	fm_state = CG2900_FM_STATE_INITIALIZED;
+	fm_mode = CG2900_FM_IDLE_MODE;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_init: returning %d",
+			result);
+	return result;
+
+}
+
+int cg2900_fm_deinit(void)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_deinit");
+
+	if (CG2900_FM_STATE_INITIALIZED != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_deinit: Already de-Initialized");
+		result = -EINVAL;
+		goto error;
+	}
+	fmd_exit();
+	mutex_destroy(&rds_mutex);
+	fm_state = CG2900_FM_STATE_DEINITIALIZED;
+	fm_mode = CG2900_FM_IDLE_MODE;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_deinit: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_switch_on(
+			struct device *device
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_switch_on");
+
+	if (CG2900_FM_STATE_INITIALIZED != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Enable FM IP */
+	FM_DEBUG_REPORT("cg2900_fm_switch_on: " "Sending FM IP Enable");
+
+	if (fmd_send_fm_ip_enable()) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			      "Error in fmd_send_fm_ip_enable");
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Now Download the Coefficient Files and FM Firmware */
+	if (cg2900_fm_load_firmware(device) != 0) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			"Error in downloading firmware");
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Power up FM */
+	result = fmd_power_up();
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			      "fmd_power_up failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Switch Mode To Idle */
+	result = fmd_set_mode(FMD_MODE_IDLE);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			"fmd_set_mode failed %x",
+			(unsigned int)result);
+		result =  -EINVAL;
+		goto error;
+	}
+
+	fm_state = CG2900_FM_STATE_SWITCHED_ON;
+	fm_mode = CG2900_FM_IDLE_MODE;
+	memset(&fm_rds_info, 0, sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+	       sizeof(struct cg2900_fm_rds_buf) *
+	       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_switch_on: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_switch_off(void)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_switch_off");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state &&
+		CG2900_FM_STATE_STAND_BY != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_switch_off: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Stop the RDS Thread if it is running */
+	if (fm_rds_status) {
+		fm_rds_status = false;
+		fmd_stop_rds_thread();
+	}
+	if (CG2900_FM_STATE_STAND_BY == fm_state) {
+		/* Power up FM */
+		result = fmd_power_up();
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_switch_off: "
+			      "fmd_power_up failed %x",
+			      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		} else
+			fm_state = CG2900_FM_STATE_SWITCHED_ON;
+	}
+	if (fmd_send_fm_ip_disable()) {
+		FM_ERR_REPORT("cg2900_fm_switch_off: "
+			      "Problem in fmd_send_fm_ip_"
+			      "disable");
+		result = -EINVAL;
+		goto error;
+	}
+	if (0 == result) {
+		fm_state = CG2900_FM_STATE_INITIALIZED;
+		fm_mode = CG2900_FM_IDLE_MODE;
+		memset(&fm_rds_info, 0,
+		       sizeof(struct cg2900_fm_rds_info));
+		memset(fm_rds_buf, 0,
+		       sizeof(struct cg2900_fm_rds_buf) *
+		       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_switch_off: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_standby(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_standby");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_standby: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_goto_standby();
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_standby: "
+			      "FMLGotoStandby failed, "
+			      "err = %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	fm_state = CG2900_FM_STATE_STAND_BY;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_standby: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_power_up_from_standby(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_power_up_from_standby");
+
+	if (CG2900_FM_STATE_STAND_BY != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_power_up_from_standby: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Power up FM */
+	result = fmd_power_up();
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_power_up_from_standby: "
+			      "fmd_power_up failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	} else {
+		fm_state = CG2900_FM_STATE_SWITCHED_ON;
+		if (CG2900_FM_TX_MODE == fm_mode) {
+			/* Enable the PA */
+			result = fmd_tx_set_pa(true);
+			if (0 != result) {
+				FM_ERR_REPORT
+				    ("cg2900_fm_power_up_from_standby:"
+				     " fmd_tx_set_pa " "failed %d",
+				     (unsigned int)result);
+				result = -EINVAL;
+				goto error;
+			}
+		}
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_power_up_from_standby: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_rx_default_settings(
+			u32 freq,
+			u8 band,
+			u8 grid,
+			bool enable_rds,
+			bool enable_stereo
+			)
+{
+	int result;
+	u8 vol_in_percentage;
+
+	FM_INFO_REPORT("cg2900_fm_set_rx_default_settings: freq = %d Hz, "
+		       "band = %d, grid = %d, RDS = %d, Stereo Mode = %d",
+		       freq, band, grid, enable_rds, enable_stereo);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state &&
+		CG2900_FM_STATE_STAND_BY != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_STATE_STAND_BY == fm_state) {
+		/* Power up FM */
+		result = fmd_power_up();
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_power_up failed %x",
+			      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		} else
+			fm_state = CG2900_FM_STATE_SWITCHED_ON;
+	}
+	fm_mode = CG2900_FM_RX_MODE;
+
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Sending Set mode to Rx");
+	result = fmd_set_mode(FMD_MODE_RX);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_set_mode failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Grid */
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Sending fmd_rx_set_grid ");
+	result = fmd_rx_set_grid(grid);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_rx_set_grid failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Band */
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Sending Set fmd_set_freq_range");
+	result = fmd_set_freq_range(band);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_set_freq_range failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Frequency */
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Sending Set fmd_rx_set_frequency");
+	result = fmd_rx_set_frequency(
+			freq / FREQUENCY_CONVERTOR_KHZ_HZ);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_rx_set_frequency failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"SetFrequency interrupt received, "
+			"Sending Set fmd_rx_set_stereo_mode");
+
+	if (enable_stereo) {
+		/* Set the Stereo Blending mode */
+		result = fmd_rx_set_stereo_mode(
+			FMD_STEREOMODE_BLENDING);
+	} else {
+		/* Set the Mono mode */
+		result = fmd_rx_set_stereo_mode(
+			FMD_STEREOMODE_MONO);
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_rx_set_stereo_mode "
+			      "failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Sending Set rds");
+
+	if (enable_rds) {
+		/* Enable RDS */
+		a_b_flag = false;
+		result = cg2900_fm_rds_on();
+	} else {
+		/* Disable RDS */
+		result = cg2900_fm_rds_off();
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "cg2900_fm_rds_on "
+			      "failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Analog Out Volume to Max */
+	vol_in_percentage = (u8)
+	    (((u16) (MAX_ANALOG_VOLUME) * 100)
+	     / MAX_ANALOG_VOLUME);
+	result = fmd_set_volume(vol_in_percentage);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			      "FMRSetVolume failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_tx_default_settings(
+			u32 freq,
+			u8 band,
+			u8 grid,
+			bool enable_rds,
+			bool enable_stereo
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_tx_default_settings: freq = %d Hz, "
+		       "band = %d, grid = %d, RDS = %d, Stereo Mode = %d",
+		       freq, band, grid, enable_rds, enable_stereo);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state &&
+		CG2900_FM_STATE_STAND_BY != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_STATE_STAND_BY == fm_state) {
+		/* Power up FM */
+		result = fmd_power_up();
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_power_up failed %x",
+			      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		} else
+			fm_state = CG2900_FM_STATE_SWITCHED_ON;
+	}
+	fm_mode = CG2900_FM_TX_MODE;
+	if (fm_rds_status) {
+		fm_rds_status = false;
+		fmd_stop_rds_thread();
+		memset(&fm_rds_info, 0,
+		       sizeof(struct cg2900_fm_rds_info));
+		memset(fm_rds_buf, 0,
+		       sizeof(struct cg2900_fm_rds_buf) *
+		       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+		/* Give 50 ms delay to exit the RDS thread */
+		schedule_timeout_interruptible(msecs_to_jiffies(50));
+	}
+
+	/* Switch To Tx mode */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending Set mode to Tx");
+	result = fmd_set_mode(FMD_MODE_TX);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_set_mode failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Grid */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending fmd_tx_set_grid ");
+	result = fmd_tx_set_grid(grid);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_tx_set_grid failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Band */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending fmd_tx_set_freq_range");
+	result = fmd_tx_set_freq_range(band);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_tx_set_freq_range failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Band */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending fmd_tx_set_preemphasis");
+	result = fmd_tx_set_preemphasis(FMD_EMPHASIS_75US);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			      "fmd_tx_set_preemphasis failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Frequency */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending Set fmd_tx_set_frequency");
+	result = fmd_tx_set_frequency(
+				freq / FREQUENCY_CONVERTOR_KHZ_HZ);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			      "fmd_tx_set_frequency failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"SetFrequency interrupt received, "
+			"Sending Set fmd_tx_enable_stereo_mode");
+
+	/* Set the Stereo mode */
+	result = fmd_tx_enable_stereo_mode(enable_stereo);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_tx_enable_stereo_mode "
+			      "failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending Set fmd_tx_set_pa");
+
+	/* Enable the PA */
+	result = fmd_tx_set_pa(true);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_tx_set_pa "
+			      "failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"set PA interrupt received, "
+			"Sending Set fmd_tx_set_signal_strength");
+
+	/* Set the Signal Strength to Max */
+	result = fmd_tx_set_signal_strength(
+				MAX_POWER_LEVEL);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_tx_set_signal_strength "
+			      "failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Enable Tx RDS  */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending Set cg2900_fm_tx_rds");
+	result = cg2900_fm_tx_rds(enable_rds);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "cg2900_fm_tx_rds "
+			      "failed %x", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_grid(
+			u8 grid
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_grid: Grid = %d", grid);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_grid: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_set_grid(grid);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_grid: "
+			      "fmd_rx_set_grid failed");
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_grid: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_band(
+			u8 band
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_band: Band = %d", band);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_band: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_set_freq_range(band);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_band: "
+			      "fmd_set_freq_range failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_band: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_search_up_freq(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_search_up_freq");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_search_up_freq: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (fm_rds_status) {
+		/* Stop RDS if it is active */
+		result = cg2900_fm_rds_off();
+		fm_prev_rds_status = true;
+	}
+	result = fmd_rx_seek(CG2900_DIR_UP);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_search_up_freq: "
+			      "Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	memset(&fm_rds_info, 0,
+	       sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+	       sizeof(struct cg2900_fm_rds_buf) *
+	       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	result = 0;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_search_up_freq: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_search_down_freq(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_search_down_freq");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_search_down_freq: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (fm_rds_status) {
+		/* Stop RDS if it is active */
+		result = cg2900_fm_rds_off();
+		fm_prev_rds_status = true;
+	}
+	result = fmd_rx_seek(CG2900_DIR_DOWN);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_search_down_freq: "
+			      "Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	memset(&fm_rds_info, 0,
+	       sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+	       sizeof(struct cg2900_fm_rds_buf) *
+	       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	result = 0;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_search_down_freq: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_start_band_scan(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_start_band_scan");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_start_band_scan: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (fm_rds_status) {
+		/* Stop RDS if it is active */
+		result = cg2900_fm_rds_off();
+		fm_prev_rds_status = true;
+	}
+	result = fmd_rx_scan_band(DEFAULT_CHANNELS_TO_SCAN);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_start_band_scan: "
+			      "Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	memset(&fm_rds_info, 0,
+	       sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+	       sizeof(struct cg2900_fm_rds_buf) *
+	       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	result = 0;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_start_band_scan: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_stop_scan(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_stop_scan");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_stop_scan: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_stop_seeking();
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_stop_scan: "
+			      "Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	memset(&fm_rds_info, 0,
+	       sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+	       sizeof(struct cg2900_fm_rds_buf) *
+	       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	result = 0;
+	if (fm_prev_rds_status) {
+		/* Restart RDS if it was active earlier */
+		cg2900_fm_rds_on();
+		fm_prev_rds_status = false;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_stop_scan: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_scan_result(
+			u16 *num_of_scanfreq,
+			u32 *scan_freq,
+			u32 *scan_freq_rssi_level
+			)
+{
+	int result;
+	u32 cnt;
+	u32 index;
+	u32 minfreq;
+	u32 maxfreq;
+	u16 channels[3];
+	u16 rssi[3];
+	u8 freq_range;
+	u8 max_channels = 0;
+
+	FM_INFO_REPORT("cg2900_fm_get_scan_result");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_scan_result: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_get_freq_range(&freq_range);
+
+	if (0 != result) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_get_freq_range_properties(
+				freq_range,
+				&minfreq,
+				&maxfreq);
+
+	if (0 != result) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_rx_get_max_channels_to_scan(&max_channels);
+
+	if (0 != result) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* In 1 iteration we can retreive max 3 channels */
+	cnt = (max_channels / 3) + 1;
+	while ((cnt--) && (result == 0)) {
+		/*
+		 * Get all channels, including empty ones.
+		 * In 1 iteration at max 3 channels can be found.
+		 */
+		result = fmd_rx_get_scan_band_info(cnt * 3,
+						   num_of_scanfreq,
+						   channels, rssi);
+		if (0 == result) {
+			index = cnt * 3;
+			/* Convert Freq to Hz from channel number */
+			scan_freq[index] = (minfreq +
+					channels[0] *
+					CHANNEL_FREQ_CONVERTER_MHZ) *
+					FREQUENCY_CONVERTOR_KHZ_HZ;
+			scan_freq_rssi_level[index] = rssi[0];
+			/* Convert Freq to Hz from channel number */
+			scan_freq[index + 1] = (minfreq +
+						channels[1] *
+						CHANNEL_FREQ_CONVERTER_MHZ) *
+						FREQUENCY_CONVERTOR_KHZ_HZ;
+			scan_freq_rssi_level[index + 1] = rssi[1];
+			/* Check if we donot overwrite the array */
+			if (cnt < (max_channels / 3)) {
+				/* Convert Freq to Hz from channel number */
+				scan_freq[index + 2] = (minfreq +
+					channels[2] *
+					CHANNEL_FREQ_CONVERTER_MHZ) *
+					FREQUENCY_CONVERTOR_KHZ_HZ;
+				scan_freq_rssi_level[index + 2]
+				    = rssi[2];
+			}
+		}
+	}
+	if (fm_prev_rds_status) {
+		/* Restart RDS if it was active earlier */
+		result = cg2900_fm_rds_on();
+		fm_prev_rds_status = false;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_scan_result: returning %d",
+			result);
+	return result;
+
+}
+
+int cg2900_fm_start_block_scan(
+			u32 start_freq,
+			u32 end_freq
+			)
+{
+	int result;
+	u8 antenna;
+
+	FM_INFO_REPORT("cg2900_fm_start_block_scan");
+
+	FM_DEBUG_REPORT("cg2900_fm_start_block_scan: Start Freq = %d, "
+		"End Freq = %d", start_freq, end_freq);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_start_block_scan: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (fm_rds_status) {
+		/* Stop RDS if it is active */
+		result = cg2900_fm_rds_off();
+		fm_prev_rds_status = true;
+	}
+	result = fmd_get_antenna(
+			&antenna);
+	result = fmd_rx_block_scan(
+			start_freq/FREQUENCY_CONVERTOR_KHZ_HZ,
+			end_freq/FREQUENCY_CONVERTOR_KHZ_HZ,
+			antenna);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_start_block_scan: "
+			"Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	memset(&fm_rds_info, 0,
+		sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+		sizeof(struct cg2900_fm_rds_buf) *
+		MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	result = 0;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_start_block_scan: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_block_scan_result(
+			u16 *num_of_scanchan,
+			u16 *scan_freq_rssi_level
+			)
+{
+	int result = 0;
+	u32 cnt;
+	u32 index;
+	u16 rssi[6];
+
+	FM_INFO_REPORT("cg2900_fm_get_block_scan_result");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_block_scan_result: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	cnt = 33;
+	while ((cnt--) && (result == 0)) {
+		/* Get all channels, including empty ones */
+		result = fmd_rx_get_block_scan_result(
+						      cnt * 6,
+						      num_of_scanchan,
+						      rssi);
+		if (0 == result) {
+			index = cnt * 6;
+			scan_freq_rssi_level[index]
+			    = rssi[0];
+			scan_freq_rssi_level[index + 1]
+			    = rssi[1];
+			scan_freq_rssi_level[index + 2]
+			    = rssi[2];
+			scan_freq_rssi_level[index + 3]
+			    = rssi[3];
+			scan_freq_rssi_level[index + 4]
+			    = rssi[4];
+			scan_freq_rssi_level[index + 5]
+			    = rssi[5];
+		}
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		if (fm_prev_rds_status) {
+			/* Restart RDS if it was active earlier*/
+			result = cg2900_fm_rds_on();
+			fm_prev_rds_status = false;
+		}
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_block_scan_result:"
+				" Sending Set fmd_tx_set_pa");
+
+		/* Enable the PA */
+		result = fmd_tx_set_pa(true);
+	if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_get_block_scan_result:"
+				      " fmd_tx_set_pa "
+				      "failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		}
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_block_scan_result: returning %d",
+			result);
+	return result;
+
+}
+
+int cg2900_fm_tx_rds(
+			bool enable_rds
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_rds: enable_rds = %d", enable_rds);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_rds: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (enable_rds) {
+		/* Set the Tx Buffer Size */
+		result = fmd_tx_buffer_set_size(
+						MAX_RDS_GROUPS - 2);
+	if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_tx_rds: "
+				      "fmd_tx_buffer_set_size "
+				      "failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		} else {
+			result = fmd_tx_set_rds(true);
+		}
+
+	if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_tx_rds: "
+				      "fmd_tx_set_rds "
+				      "failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		}
+		program_identification_code =
+		    default_program_identification_code;
+		program_type_code = default_program_type_code;
+		memcpy(program_service,
+			    default_program_service,
+			    MAX_PSN_SIZE);
+		memcpy(radio_text,
+			    default_radio_text, MAX_RT_SIZE);
+		radio_text[strlen(radio_text)] = 0x0D;
+		cg2900_fm_transmit_rds_groups();
+		result = 0;
+	} else {
+		result = fmd_tx_set_rds(false);
+
+	if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_tx_rds: "
+				      "fmd_tx_set_rds "
+				      "failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		}
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_rds: returning %d",
+			result);
+
+	return result;
+}
+
+int cg2900_fm_tx_set_pi_code(
+			u16 pi_code
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_pi_code: PI = %04x", pi_code);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pi_code: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	program_identification_code = pi_code;
+	result = cg2900_fm_transmit_rds_groups();
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_pi_code: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_pty_code(
+			u16 pty_code
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_pty_code: PTY = %04x", pty_code);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pty_code: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	program_type_code = pty_code;
+	result = cg2900_fm_transmit_rds_groups();
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_pty_code: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_program_station_name(
+			char *psn,
+			u8 len
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_program_station_name: PSN = %s",
+			psn);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_program_station_name: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (len < (MAX_PSN_SIZE - 1)) {
+		int count = len;
+		while (count < (MAX_PSN_SIZE - 1))
+			psn[count++] = ' ';
+	}
+	memcpy(program_service, psn, MAX_PSN_SIZE);
+	result = cg2900_fm_transmit_rds_groups();
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_program_station_name: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_radio_text(
+			char *rt,
+			u8 len
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_radio_text: RT = %s", rt);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_radio_text: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	rt[len] = 0x0D;
+	memcpy(radio_text, rt, len + 1);
+
+	result = cg2900_fm_transmit_rds_groups();
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_radio_text: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_get_rds_deviation(
+			u16 *deviation
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_get_rds_deviation");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_rds_deviation: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_get_rds_deviation(deviation);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_rds_deviation: "
+			      "fmd_tx_get_rds_deviation failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_get_rds_deviation: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_rds_deviation(
+			u16 deviation
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_rds_deviation: deviation = %d",
+		       deviation);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_rds_deviation: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_set_rds_deviation(deviation);
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_tx_set_rds_deviation: "
+			      "fmd_tx_set_rds_deviation failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_rds_deviation: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_get_pilot_tone_status(
+			bool *enable
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_get_pilot_tone_status");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_pilot_tone_status: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_get_stereo_mode(enable);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_pilot_tone_status: "
+			      "fmd_tx_get_stereo_mode failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_get_pilot_tone_status: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_pilot_tone_status(
+			bool enable
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_pilot_tone_status: enable = %d",
+			enable);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pilot_tone_status: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_enable_stereo_mode(enable);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pilot_tone_status: "
+			      "fmd_tx_enable_stereo_mode failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_pilot_tone_status: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_get_pilot_deviation(
+			u16 *deviation
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_get_pilot_deviation");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_pilot_deviation: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_get_pilot_deviation(deviation);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_pilot_deviation: "
+			      "fmd_tx_get_pilot_deviation failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_get_pilot_deviation: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_pilot_deviation(
+			u16 deviation
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_pilot_deviation: deviation = %d",
+		       deviation);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pilot_deviation: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_set_pilot_deviation(deviation);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pilot_deviation: "
+			      "fmd_tx_set_pilot_deviation failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_pilot_deviation: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_get_preemphasis(
+			u8 *preemphasis
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_get_preemphasis");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_preemphasis: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_get_preemphasis(preemphasis);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_preemphasis: "
+			      "fmd_tx_get_preemphasis failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_get_preemphasis: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_preemphasis(
+			u8 preemphasis
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_preemphasis: preemphasis = %d",
+		       preemphasis);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_preemphasis: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_set_preemphasis(preemphasis);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_preemphasis: "
+			      "fmd_tx_set_preemphasis failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_preemphasis: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_rx_set_deemphasis(
+			u8 deemphasis
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_rx_set_deemphasis: deemphasis = %02x",
+		deemphasis);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_rx_set_deemphasis: "
+			"Invalid state of FM Driver = %d", fm_state);
+			result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_set_deemphasis(deemphasis);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_rx_set_deemphasis: "
+			"fmd_rx_set_deemphasis failed %d",
+			result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_rx_set_deemphasis: returning %d", result);
+	return result;
+}
+
+int cg2900_fm_tx_get_power_level(
+			u16 *power_level
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_get_power_level");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_power_level: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_get_signal_strength(power_level);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_power_level: "
+			      "fmd_tx_get_signal_strength failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_get_power_level: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_power_level(
+			u16 power_level
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_power_level: power_level = %d",
+		       power_level);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_power_level: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_set_signal_strength(power_level);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_power_level: "
+			      "fmd_tx_set_preemphasis failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_power_level: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_audio_balance(
+			s8 balance
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_audio_balance, balance = %d", balance);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_audio_balance: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_set_balance(balance);
+	if (0 != result) {
+		FM_ERR_REPORT("FMRSetAudioBalance : "
+			      "Failed in fmd_set_balance, err = %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_audio_balance: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_volume(
+			u8 vol_level
+			)
+{
+	int result;
+	u8 vol_in_percentage;
+
+	FM_INFO_REPORT("cg2900_fm_set_volume: Volume Level = %d", vol_level);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_volume: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	vol_in_percentage =
+	    (u8) (((u16) (vol_level) * 100) / MAX_ANALOG_VOLUME);
+	result = fmd_set_volume(vol_in_percentage);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_increase_volume: "
+			      "FMRSetVolume failed, err = %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_volume: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_volume(
+			u8 *vol_level
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_get_volume");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_volume: "
+			"Invalid state of FM Driver = %d", fm_state);
+		*vol_level = 0;
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_get_volume(vol_level);
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_volume: returning %d, VolLevel = %d",
+			result, *vol_level);
+	return result;
+}
+
+int cg2900_fm_rds_off(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_rds_off");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_rds_off: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_set_rds(FMD_SWITCH_OFF_RDS);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_rds_off: fmd_rx_set_rds failed, "
+			"err = %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Stop the RDS Thread */
+	fm_rds_status = false;
+	memset(&fm_rds_info, 0,
+	       sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+	       sizeof(struct cg2900_fm_rds_buf) *
+	       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	FM_DEBUG_REPORT("cg2900_fm_rds_off: "
+			"Stopping RDS Thread");
+	fmd_stop_rds_thread();
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_rds_off: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_rds_on(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_rds_on");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_rds_on: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	FM_DEBUG_REPORT("cg2900_fm_rds_on:"
+			" Sending fmd_rx_buffer_set_size");
+	result = fmd_rx_buffer_set_size(MAX_RDS_GROUPS);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_rds_on: fmd_rx_buffer_set_size"
+			"failed, err = %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	FM_DEBUG_REPORT("cg2900_fm_rds_on: Sending "
+			"fmd_rx_buffer_set_threshold");
+	result = fmd_rx_buffer_set_threshold(MAX_RDS_GROUPS - 1);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_rds_on: fmd_rx_buffer_set_threshold "
+			"failed, err = %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	FM_DEBUG_REPORT("cg2900_fm_rds_on: Sending fmd_rx_set_rds");
+	result = fmd_rx_set_rds(FMD_SWITCH_ON_RDS_ENHANCED_MODE);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_rds_on: fmd_rx_set_rds failed, "
+			"err = %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Start the RDS Thread to read the RDS Buffers */
+	fm_rds_status = true;
+	memset(&fm_rds_info, 0,
+	       sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+	       sizeof(struct cg2900_fm_rds_buf) *
+	       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	fmd_start_rds_thread(cg2900_fm_rds_callback);
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_rds_on: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_rds_status(
+			bool *rds_status
+			)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_get_rds_status");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_rds_status: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_rds_status: "
+				"fmd_rx_get_rds");
+		result = fmd_rx_get_rds(rds_status);
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_rds_status: "
+				"fmd_tx_get_rds");
+		result = fmd_tx_get_rds(rds_status);
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_rds_status: "
+			      "fmd_get_rds failed, Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_rds_status: returning %d, "
+			"rds_status = %d", result,
+			*rds_status);
+	return result;
+}
+
+int cg2900_fm_mute(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_mute");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_mute: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Mute Analog DAC */
+	result = fmd_set_mute(true);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_mute: "
+			      "fmd_set_mute failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Mute Ext Src */
+	result = fmd_ext_set_mute(true);
+	if (0 != result) {
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_mute: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_unmute(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_unmute");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_unmute: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Unmute Analog DAC */
+	result = fmd_set_mute(false);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_mute: "
+			      "fmd_set_mute failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Unmute Ext Src */
+	result = fmd_ext_set_mute(false);
+	if (0 != result) {
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_unmute: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_frequency(
+			u32 *freq
+			)
+{
+	int result = 0;
+	u32 currentFreq = 0;
+
+	FM_INFO_REPORT("cg2900_fm_get_frequency");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_frequency: "
+			"Invalid state of FM Driver = %d", fm_state);
+		*freq = 0;
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_frequency: "
+				"fmd_rx_get_frequency");
+		result = fmd_rx_get_frequency(
+					      (u32 *) &currentFreq);
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_frequency: "
+				"fmd_tx_get_frequency");
+		result = fmd_tx_get_frequency(
+					      (u32 *) &currentFreq);
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_frequency: "
+			      "fmd_rx_get_frequency failed %d",
+			      (unsigned int)result);
+		*freq = 0;
+		result = -EINVAL;
+		goto error;
+	}
+	/* Convert To Hz */
+	*freq = currentFreq * FREQUENCY_CONVERTOR_KHZ_HZ;
+	FM_DEBUG_REPORT("cg2900_fm_get_frequency: "
+			"Current Frequency = %d Hz", *freq);
+	if (fm_prev_rds_status) {
+		/* Restart RDS if it was active earlier */
+		cg2900_fm_rds_on();
+		fm_prev_rds_status = false;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_frequency: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_frequency(
+			u32 new_freq
+			)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_set_frequency, new_freq = %d",
+			(int)new_freq);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_frequency: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_set_frequency: "
+				"fmd_rx_set_frequency");
+		result = fmd_rx_set_frequency(
+				new_freq / FREQUENCY_CONVERTOR_KHZ_HZ);
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_set_frequency: "
+				"fmd_tx_set_frequency");
+		result = fmd_tx_set_frequency(
+				new_freq / FREQUENCY_CONVERTOR_KHZ_HZ);
+	}
+	if (result != 0) {
+		FM_ERR_REPORT("cg2900_fm_set_frequency: "
+			      "fmd_rx_set_frequency failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_set_frequency:"
+				" Sending Set" "fmd_tx_set_pa");
+
+		/* Enable the PA */
+		result = fmd_tx_set_pa(true);
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_set_frequency:"
+				      " fmd_tx_set_pa "
+				      "failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		}
+		memset(&fm_rds_info, 0,
+	       sizeof(struct cg2900_fm_rds_info));
+		memset(fm_rds_buf, 0,
+		       sizeof(struct cg2900_fm_rds_buf) *
+		       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+		result = 0;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_frequency: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_signal_strength(
+			u16 *signal_strength
+			)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_get_signal_strength");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_signal_strength: "
+			"Invalid state of FM Driver = %d", fm_state);
+		*signal_strength = 0;
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_signal_strength: "
+				"fmd_rx_get_signal_strength");
+		result = fmd_rx_get_signal_strength(
+						    signal_strength);
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_signal_strength: "
+				"fmd_tx_get_signal_strength");
+		result = fmd_tx_get_signal_strength(
+						    signal_strength);
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_signal_strength: "
+			      "Error Code %d", (unsigned int)result);
+		*signal_strength = 0;
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_signal_strength: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_af_update_get_result(
+			u16 *af_update_rssi
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_af_update_get_result");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_af_update_get_result: "
+			"Invalid state of FM Driver = %d", fm_state);
+		*af_update_rssi = 0;
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_get_af_update_result(af_update_rssi);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_af_update_get_result: "
+			      "Error Code %d", (unsigned int)result);
+		*af_update_rssi = 0;
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_af_update_get_result: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_af_update_start(
+			u32 af_freq
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_af_update_start");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_af_update_start: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_af_update_start(
+					af_freq / FREQUENCY_CONVERTOR_KHZ_HZ);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_af_update_start: "
+			      "Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_af_update_start: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_af_switch_get_result(
+			u16 *af_switch_conclusion
+			)
+{
+	int result;
+	u16 af_rssi;
+	u16 af_pi;
+
+	FM_INFO_REPORT("cg2900_fm_af_switch_get_result");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_af_switch_get_result: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_get_af_switch_results(
+					      af_switch_conclusion,
+					      &af_rssi, &af_pi);
+
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_af_switch_get_result: "
+			      "Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	FM_DEBUG_REPORT("cg2900_fm_af_switch_get_result: "
+			"AF Switch conclusion = %d "
+			"AF Switch RSSI level = %d "
+			"AF Switch PI code = %d ",
+			*af_switch_conclusion, af_rssi, af_pi);
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_af_switch_get_result: returning %d",
+			result);
+	return result;
+
+}
+
+int cg2900_fm_af_switch_start(
+			u32 af_switch_freq,
+			u16 af_switch_pi
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_af_switch_start");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_af_switch_start: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_af_switch_start(
+				af_switch_freq / FREQUENCY_CONVERTOR_KHZ_HZ,
+				af_switch_pi);
+
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_af_switch_start: "
+			      "Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_af_switch_start: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_mode(
+			u8 *cur_mode
+			)
+{
+	int result = 0;
+	bool stereo_mode;
+
+	FM_INFO_REPORT("cg2900_fm_get_mode");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_mode: "
+			"Invalid state of FM Driver = %d", fm_state);
+		*cur_mode = CG2900_MODE_MONO;
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_mode: "
+				"fmd_rx_get_stereo_mode");
+		result = fmd_rx_get_stereo_mode(cur_mode);
+		switch (*cur_mode) {
+		case FMD_STEREOMODE_OFF:
+		case FMD_STEREOMODE_BLENDING:
+			*cur_mode = CG2900_MODE_STEREO;
+			break;
+		case FMD_STEREOMODE_MONO:
+		default:
+			*cur_mode = CG2900_MODE_MONO;
+			break;
+		}
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_mode: "
+				"fmd_tx_get_stereo_mode");
+		result = fmd_tx_get_stereo_mode(&stereo_mode);
+		if (stereo_mode)
+			*cur_mode = CG2900_MODE_STEREO;
+		else
+			*cur_mode = CG2900_MODE_MONO;
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_mode: "
+			      "fmd_get_stereo_mode failed, "
+			      "Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_mode: returning %d, mode = %d",
+			result, *cur_mode);
+	return result;
+}
+
+int cg2900_fm_set_mode(
+			u8 mode
+			)
+{
+	int result = 0;
+	bool enable_stereo_mode = false;
+
+	FM_INFO_REPORT("cg2900_fm_set_mode: mode = %d", mode);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_mode: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_set_mode: "
+				"fmd_rx_set_stereo_mode");
+		result = fmd_rx_set_stereo_mode(mode);
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_set_mode: "
+				"fmd_tx_set_stereo_mode");
+		if (mode == CG2900_MODE_STEREO)
+			enable_stereo_mode = true;
+		result =
+		    fmd_tx_enable_stereo_mode(
+					      enable_stereo_mode);
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_mode: "
+			      "fmd_rx_set_stereo_mode failed, "
+			      "Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_mode: returning %d",
+		result);
+	return result;
+}
+
+int cg2900_fm_select_antenna(
+			u8 antenna
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_select_antenna: Antenna = %d", antenna);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_select_antenna: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_set_antenna(antenna);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_select_antenna: "
+			      "fmd_set_antenna failed, Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_select_antenna: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_antenna(
+			u8 *antenna
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_get_antenna");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_antenna: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_get_antenna(antenna);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_antenna: "
+			      "fmd_get_antenna failed, Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_antenna: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_rssi_threshold(
+			u16 *rssi_thresold
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_get_rssi_threshold");
+
+	result = fmd_rx_get_stop_level(rssi_thresold);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_rssi_threshold: "
+			      "fmd_rx_get_stop_level failed, Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_rssi_threshold: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_rssi_threshold(
+			u16 rssi_thresold
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_rssi_threshold: "
+		       "RssiThresold = %d", rssi_thresold);
+
+	result = fmd_rx_set_stop_level(rssi_thresold);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rssi_threshold: "
+			      "fmd_rx_set_stop_level failed, Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_rssi_threshold: returning %d",
+			result);
+	return result;
+}
+
+void cg2900_fm_set_chip_version(
+			u16 revision,
+			u16 sub_version
+			)
+{
+	version_info.revision = revision;
+	version_info.sub_version = sub_version;
+}
+
+int cg2900_fm_set_test_tone_generator(
+			u8 test_tone_status
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_test_tone_generator: "
+		"test_tone_status = %02x", test_tone_status);
+
+	result = fmd_set_test_tone_generator_status(test_tone_status);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_test_tone_generator: "
+			"fmd_set_test_tone_generator_status failed"
+			", Error Code %d",
+			result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_test_tone_generator: returning %d",
+		result);
+	return result;
+}
+
+int cg2900_fm_test_tone_connect(
+			u8 left_audio_mode,
+			u8 right_audio_mode
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_test_tone_connect: "
+		"left_audio_mode = %02x right_audio_mode = %02x",
+		left_audio_mode, right_audio_mode);
+
+	result = fmd_test_tone_connect(left_audio_mode, right_audio_mode);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_test_tone_connect: "
+			"fmd_set_test_tone_connect failed, Error Code %d",
+			result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_test_tone_connect: returning %d",
+		result);
+	return result;
+}
+
+int cg2900_fm_test_tone_set_params(
+			u8 tone_gen,
+			u16 frequency,
+			u16 volume,
+			u16 phase_offset,
+			u16 dc,
+			u8 waveform
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_test_tone_set_params: "
+		"tone_gen = %02x frequency = %04x "
+		"volume = %04x phase_offset = %04x "
+		"dc offset = %04x waveform = %02x",
+		tone_gen, frequency,
+		volume, phase_offset,
+		dc, waveform);
+
+	result = fmd_test_tone_set_params(
+				tone_gen,
+				frequency,
+				volume,
+				phase_offset,
+				dc,
+				waveform);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_test_tone_set_params: "
+			"fmd_test_tone_set_params failed, Error Code %d",
+			result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_test_tone_set_params: returning %d",
+		result);
+	return result;
+}
+
+MODULE_AUTHOR("Hemant Gupta");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/radio/CG2900/cg2900_fm_api.h b/drivers/media/radio/CG2900/cg2900_fm_api.h
new file mode 100644
index 00000000000..747cdbc1e0d
--- /dev/null
+++ b/drivers/media/radio/CG2900/cg2900_fm_api.h
@@ -0,0 +1,1074 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Linux FM Host API's for ST-Ericsson FM Chip.
+ *
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#ifndef CG2900_FM_API_H
+#define CG2900_FM_API_H
+
+#include <linux/device.h>
+
+/* Callback function to receive RDS Data. */
+typedef void  (*cg2900_fm_rds_cb)(void);
+
+/**
+ * struct cg2900_fm_rds_buf - RDS Group Receiving Structure
+ *
+ * @block1: RDS Block A
+ * @block2: RDS Block B
+ * @block3: RDS Block C
+ * @block4: RDS Block D
+ * @status1: Status of received RDS Block A
+ * @status2: Status of received RDS Block B
+ * @status3: Status of received RDS Block C
+ * @status4: Status of received RDS Block D
+ *
+ * Structure for receiving the RDS Group from FM Chip.
+ */
+struct cg2900_fm_rds_buf {
+	u16 block1;
+	u16 block2;
+	u16 block3;
+	u16 block4;
+	u8 status1;
+	u8 status2;
+	u8 status3;
+	u8 status4;
+};
+
+/**
+ * struct cg2900_fm_rds_info - RDS Information Structure
+ *
+ * @rds_head: RDS Queue Head for storing next valid data.
+ * @rds_tail: RDS Queue Tail for retreiving next valid data.
+ * @rds_group_sent: Number of RDS Groups sent to Application.
+ * @rds_block_sent: Number of RDS Blocks sent to Application.
+ *
+ * Structure for storing the RDS data queue information.
+ */
+struct cg2900_fm_rds_info {
+	u8 rds_head;
+	u8 rds_tail;
+	u8 rds_group_sent;
+	u8 rds_block_sent;
+};
+
+/**
+ * struct cg2900_version_info - Chip HCI Version Info
+ *
+ * @revision:	Revision of the controller, e.g. to indicate that it is
+ *			a CG2900 controller.
+ * @sub_version:	Subversion of the controller, e.g. to indicate a certain
+ *			tape-out of the controller.
+ *
+ * Structure for storing the HCI Version Information of the Controller.
+ */
+struct cg2900_version_info {
+	u16 revision;
+	u16 sub_version;
+};
+
+/**
+ * enum cg2900_fm_state - States of FM Driver.
+ *
+ * @CG2900_FM_STATE_DEINITIALIZED: FM driver is not initialized.
+ * @CG2900_FM_STATE_INITIALIZED: FM driver is initialized.
+ * @CG2900_FM_STATE_SWITCHED_ON: FM driver is switched on and in active state.
+ * @CG2900_FM_STATE_STAND_BY: FM Radio is switched on but not in active state.
+ *
+ * Various states of FM Driver.
+ */
+enum cg2900_fm_state {
+	CG2900_FM_STATE_DEINITIALIZED,
+	CG2900_FM_STATE_INITIALIZED,
+	CG2900_FM_STATE_SWITCHED_ON,
+	CG2900_FM_STATE_STAND_BY
+};
+
+/**
+ * enum cg2900_fm_mode - FM Driver Command state .
+ *
+ * @CG2900_FM_IDLE_MODE: FM Radio is in Idle Mode.
+ * @CG2900_FM_RX_MODE: FM Radio is configured in Rx mode.
+ * @CG2900_FM_TX_MODE: FM Radio is configured in Tx mode.
+ *
+ * Various Modes of the FM Radio.
+ */
+enum cg2900_fm_mode {
+	CG2900_FM_IDLE_MODE,
+	CG2900_FM_RX_MODE,
+	CG2900_FM_TX_MODE
+};
+
+/**
+ * enum cg2900_fm_band - Various Frequency band supported.
+ *
+ * @CG2900_FM_BAND_US_EU: European / US Band.
+ * @CG2900_FM_BAND_JAPAN: Japan Band.
+ * @CG2900_FM_BAND_CHINA: China Band.
+ * @CG2900_FM_BAND_CUSTOM: Custom Band.
+ *
+ * Various Frequency band supported.
+ */
+enum cg2900_fm_band {
+	CG2900_FM_BAND_US_EU,
+	CG2900_FM_BAND_JAPAN,
+	CG2900_FM_BAND_CHINA,
+	CG2900_FM_BAND_CUSTOM
+};
+
+/**
+ * enum cg2900_fm_grid - Various Frequency grids supported.
+ *
+ * @CG2900_FM_GRID_50: 50 kHz spacing.
+ * @CG2900_FM_GRID_100: 100 kHz spacing.
+ * @CG2900_FM_GRID_200: 200 kHz spacing.
+ *
+ * Various Frequency grids supported.
+ */
+enum cg2900_fm_grid {
+	CG2900_FM_GRID_50,
+	CG2900_FM_GRID_100,
+	CG2900_FM_GRID_200
+};
+
+/**
+ * enum cg2900_fm_event - Various Events reported by FM API layer.
+ *
+ * @CG2900_EVENT_NO_EVENT: No Event.
+ * @CG2900_EVENT_SEARCH_CHANNEL_FOUND: Seek operation is completed.
+ * @CG2900_EVENT_SCAN_CHANNELS_FOUND: Band Scan is completed.
+ * @CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND: Block Scan is completed.
+ * @CG2900_EVENT_SCAN_CANCELLED: Scan/Seek is cancelled.
+ *
+ * Various Events reported by FM API layer.
+ */
+enum cg2900_fm_event {
+	CG2900_EVENT_NO_EVENT,
+	CG2900_EVENT_SEARCH_CHANNEL_FOUND,
+	CG2900_EVENT_SCAN_CHANNELS_FOUND,
+	CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND,
+	CG2900_EVENT_SCAN_CANCELLED
+};
+
+/**
+ * enum cg2900_fm_direction - Directions used while seek.
+ *
+ * @CG2900_DIR_DOWN: Search in downwards direction.
+ * @CG2900_DIR_UP: Search in upwards direction.
+ *
+ * Directions used while seek.
+ */
+enum cg2900_fm_direction {
+	CG2900_DIR_DOWN,
+	CG2900_DIR_UP
+};
+
+/**
+ * enum cg2900_fm_stereo_mode - Stereo Modes.
+ *
+ * @CG2900_MODE_MONO: Mono Mode.
+ * @CG2900_MODE_STEREO: Stereo Mode.
+ *
+ * Stereo Modes.
+ */
+enum cg2900_fm_stereo_mode {
+	CG2900_MODE_MONO,
+	CG2900_MODE_STEREO
+};
+
+#define CG2900_FM_DEFAULT_RSSI_THRESHOLD			100
+#define MAX_RDS_BUFFER					10
+#define MAX_RDS_GROUPS					22
+#define MIN_ANALOG_VOLUME				0
+#define MAX_ANALOG_VOLUME				20
+#define NUM_OF_RDS_BLOCKS				4
+#define RDS_BLOCK_MASK					0x1C
+#define RDS_ERROR_STATUS_MASK				0x03
+#define RDS_UPTO_TWO_BITS_CORRECTED			0x01
+#define RDS_UPTO_FIVE_BITS_CORRECTED			0x02
+#define MAX_RT_SIZE					65
+#define MAX_PSN_SIZE					9
+#define DEFAULT_CHANNELS_TO_SCAN			32
+#define MAX_CHANNELS_TO_SCAN				99
+#define MAX_CHANNELS_FOR_BLOCK_SCAN			198
+
+extern u8 fm_event;
+extern struct cg2900_fm_rds_buf fm_rds_buf[MAX_RDS_BUFFER][MAX_RDS_GROUPS];
+extern struct cg2900_fm_rds_info fm_rds_info;
+
+/**
+ * cg2900_fm_init()- Initializes FM Radio.
+ *
+ * Initializes the Variables and structures required for FM Driver.
+ * It also registers the callback to receive the events for command
+ * completion, etc
+ *
+ * Returns:
+ *   0,  if Initialization successful
+ *   -EINVAL, otherwise.
+ */
+int cg2900_fm_init(void);
+
+/**
+ * cg2900_fm_deinit()- De-initializes FM Radio.
+ *
+ * De-initializes the Variables and structures required for FM Driver.
+ *
+ * Returns:
+ *	 0, if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_deinit(void);
+
+/**
+ * cg2900_fm_switch_on()- Start up procedure of the FM radio.
+ *
+ * @device: Character device requesting the operation.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_switch_on(
+			struct device *device
+			);
+
+/**
+ * cg2900_fm_switch_off()- Switches off FM radio
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_switch_off(void);
+
+/**
+ * cg2900_fm_standby()- Makes the FM Radio Go in Standby mode.
+ *
+ * The FM Radio memorizes the the last state, i.e. Volume, last
+ * tuned station, etc that helps in resuming quickly to previous state.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_standby(void);
+
+/**
+ * cg2900_fm_power_up_from_standby()- Power Up FM Radio from Standby mode.
+ *
+ * It retruns the FM radio to the same state as it was before
+ * going to Standby.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_power_up_from_standby(void);
+
+/**
+ * cg2900_fm_set_rx_default_settings()- Loads FM Rx Default Settings.
+ *
+ * @freq: Frequency in Hz to be set on the FM Radio.
+ * @band: Band To be Set.
+ * (0: US/EU, 1: Japan, 2: China, 3: Custom)
+ * @grid: Grid specifying Spacing.
+ * (0: 50 KHz, 1: 100 KHz, 2: 200 Khz)
+ * @enable_rds: Flag indicating enable or disable rds transmission.
+ * @enable_stereo: Flag indicating enable or disable stereo mode.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_rx_default_settings(
+			u32 freq,
+			u8 band,
+			u8 grid,
+			bool enable_rds,
+			bool enable_stereo
+			);
+
+/**
+ * cg2900_fm_set_tx_default_settings()- Loads FM Tx Default Settings.
+ *
+ * @freq: Frequency in Hz to be set on the FM Radio.
+ * @band: Band To be Set.
+ * (0: US/EU, 1: Japan, 2: China, 3: Custom)
+ * @grid: Grid specifying Spacing.
+ * (0: 50 KHz, 1: 100 KHz, 2: 200 Khz)
+ * @enable_rds: Flag indicating enable or disable rds transmission.
+ * @enable_stereo: Flag indicating enable or disable stereo mode.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_tx_default_settings(
+			u32 freq,
+			u8 band,
+			u8 grid,
+			bool enable_rds,
+			bool enable_stereo
+			);
+
+/**
+ * cg2900_fm_set_grid()- Sets the Grid on the FM Radio.
+ *
+ * @grid: Grid specifying Spacing.
+ * (0: 50 KHz,1: 100 KHz,2: 200 Khz)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_grid(
+			u8 grid
+			);
+
+/**
+ * cg2900_fm_set_band()- Sets the Band on the FM Radio.
+ *
+ * @band: Band specifying Region.
+ * (0: US_EU,1: Japan,2: China,3: Custom)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_band(
+			u8 band
+			);
+
+/**
+ * cg2900_fm_search_up_freq()- seek Up.
+ *
+ * Searches the next available station in Upward Direction
+ * starting from the Current freq.
+ *
+ * If the operation is started successfully, the chip will generate the
+ * irpt_OperationSucced. interrupt when the operation is completed
+ * and will tune to the next available frequency.
+ * If no station is found, the chip is still tuned to the original station
+ * before starting the search
+ * Till the interrupt is received, no more API's should be called
+ * except cg2900_fm_stop_scan
+ *
+ * Returns:
+ *	 0,  if operation started successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_search_up_freq(void);
+
+/**
+ * cg2900_fm_search_down_freq()- seek Down.
+ *
+ * Searches the next available station in Downward Direction
+ * starting from the Current freq.
+ *
+ * If the operation is started successfully, the chip will generate
+ * the irpt_OperationSucced. interrupt when the operation is completed.
+ * and will tune to the next available frequency. If no station is found,
+ * the chip is still tuned to the original station before starting the search.
+ * Till the interrupt is received, no more API's should be called
+ * except cg2900_fm_stop_scan.
+ *
+ * Returns:
+ *	 0,  if operation started successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_search_down_freq(void);
+
+/**
+ * cg2900_fm_start_band_scan()- Band Scan.
+ *
+ * Searches for available Stations in the entire Band starting from
+ * current freq.
+ * If the operation is started successfully, the chip will generate
+ * the irpt_OperationSucced. interrupt when the operation is completed.
+ * After completion the chip will still be tuned the original station before
+ * starting the Scan. on reception of interrupt, the host should call the AP
+ * cg2900_fm_get_scan_result() to retrieve the Stations and corresponding
+ * RSSI of stations found in the Band.
+ * Till the interrupt is received, no more API's should be called
+ * except cg2900_fm_stop_scan, cg2900_fm_switch_off, cg2900_fm_standby and
+ * cg2900_fm_get_frequency.
+ *
+ * Returns:
+ *	 0,  if operation started successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_start_band_scan(void);
+
+/**
+ * cg2900_fm_stop_scan()- Stops an active ongoing seek or Band Scan.
+ *
+ * If the operation is started successfully, the chip will generate the
+ * irpt_OperationSucced interrupt when the operation is completed.
+ * Till the interrupt is received, no more API's should be called.
+ *
+ * Returns:
+ *	 0,  if operation started successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_stop_scan(void);
+
+/**
+ * cg2900_fm_get_scan_result()- Retreives Band Scan Result
+ *
+ * Retrieves the Scan Band Results of the stations found and
+ * the corressponding RSSI values of the stations.
+ * @num_of_scanfreq: (out) Number of Stations found
+ * during Scanning.
+ * @scan_freq: (out) Frequency of Stations in Hz
+ * found during Scanning.
+ * @scan_freq_rssi_level: (out) RSSI level of Stations
+ * found during Scanning.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_scan_result(
+			u16 *num_of_scanfreq,
+			u32 *scan_freq,
+			u32 *scan_freq_rssi_level
+			);
+
+/**
+ * cg2900_fm_start_block_scan()- Block Scan.
+ *
+ * Searches for RSSI level of all the channels between the start and stop
+ * channels. If the operation is started successfully, the chip will generate
+ * the irpt_OperationSucced interrupt when the operation is completed.
+ * After completion the chip will still be tuned the original station before
+ * starting the Scan. On reception of interrupt, the host should call the AP
+ * cg2900_fm_get_block_scan_result() to retrieve the RSSI of channels.
+ * Till the interrupt is received, no more API's should be called from Host
+ * except cg2900_fm_stop_scan, cg2900_fm_switch_off, cg2900_fm_standby and
+ * cg2900_fm_get_frequency.
+ * @start_freq: Start channel block scan Frequency.
+ * @end_freq:  End channel block scan Frequency
+ *
+ * Returns:
+ *	 0,  if operation started successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_start_block_scan(
+			u32 start_freq,
+			u32 end_freq
+			);
+
+/**
+ * cg2900_fm_get_scan_result()- Retreives Band Scan Result
+ *
+ * Retrieves the Scan Band Results of the stations found and
+ * the corressponding RSSI values of the stations.
+ * @num_of_scanchan: (out) Number of Stations found
+ * during Scanning.
+ * @scan_freq_rssi_level: (out) RSSI level of Stations
+ * found during Scanning.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_block_scan_result(
+			u16 *num_of_scanchan,
+			u16 *scan_freq_rssi_level
+			);
+
+/**
+ * cg2900_fm_tx_get_rds_deviation()- Gets RDS Deviation.
+ *
+ * Retrieves the RDS Deviation level set for FM Tx.
+ * @deviation: (out) Rds Deviation.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_get_rds_deviation(
+			u16 *deviation
+			);
+
+/**
+ * cg2900_fm_tx_set_rds_deviation()- Sets RDS Deviation.
+ *
+ * Sets the RDS Deviation level on FM Tx.
+ * @deviation: Rds Deviation to set on FM Tx.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_rds_deviation(
+			u16 deviation
+			);
+
+/**
+ * cg2900_fm_tx_set_pi_code()- Sets PI code for RDS Transmission.
+ *
+ * Sets the Program Identification code to be transmitted.
+ * @pi_code: PI code to be transmitted.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_pi_code(
+			u16 pi_code
+			);
+
+/**
+ * cg2900_fm_tx_set_pty_code()- Sets PTY code for RDS Transmission.
+ *
+ * Sets the Program Type code to be transmitted.
+ * @pty_code: PTY code to be transmitted.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_pty_code(
+			u16 pty_code
+			);
+
+/**
+ * cg2900_fm_tx_set_program_station_name()- Sets PSN for RDS Transmission.
+ *
+ * Sets the Program Station Name to be transmitted.
+ * @psn: Program Station Name to be transmitted.
+ * @len: Length of Program Station Name to be transmitted.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_program_station_name(
+			char *psn,
+			u8 len
+			);
+
+/**
+ * cg2900_fm_tx_set_radio_text()- Sets RT for RDS Transmission.
+ *
+ * Sets the radio text to be transmitted.
+ * @rt: Radio Text to be transmitted.
+ * @len: Length of Radio Text to be transmitted.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_radio_text(
+			char *rt,
+			u8 len
+			);
+
+/**
+ * cg2900_fm_tx_get_rds_deviation()- Gets Pilot Tone status
+ *
+ * Gets the current status of pilot tone for FM Tx.
+ * @enable: (out) Flag indicating Pilot Tone is enabled or disabled.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_get_pilot_tone_status(
+			bool *enable
+			);
+
+/**
+ * cg2900_fm_tx_set_pilot_tone_status()- Enables/Disables Pilot Tone.
+ *
+ * Enables or disables the pilot tone for FM Tx.
+ * @enable: Flag indicating enabling or disabling Pilot Tone.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_pilot_tone_status(
+			bool enable
+			);
+
+/**
+ * cg2900_fm_tx_get_pilot_deviation()- Gets Pilot Deviation.
+ *
+ * Retrieves the Pilot Tone Deviation level set for FM Tx.
+ * @deviation: (out) Pilot Tone Deviation.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_get_pilot_deviation(
+			u16 *deviation
+			);
+
+/**
+ * cg2900_fm_tx_set_pilot_deviation()- Sets Pilot Deviation.
+ *
+ * Sets the Pilot Tone Deviation level on FM Tx.
+ * @deviation: Pilot Tone Deviation to set.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_pilot_deviation(
+			u16 deviation
+			);
+
+/**
+ * cg2900_fm_tx_get_preemphasis()- Gets Pre-emhasis level.
+ *
+ * Retrieves the Preemphasis level set for FM Tx.
+ * @preemphasis: (out) Preemphasis level.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_get_preemphasis(
+			u8 *preemphasis
+			);
+
+/**
+ * cg2900_fm_tx_set_preemphasis()- Sets Pre-emhasis level.
+ *
+ * Sets the Preemphasis level on FM Tx.
+ * @preemphasis: Preemphasis level.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_preemphasis(
+			u8 preemphasis
+			);
+
+/**
+ * cg2900_fm_tx_get_power_level()- Gets Power level.
+ *
+ * Retrieves the Power level set for FM Tx.
+ * @power_level: (out) Power level.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_get_power_level(
+			u16 *power_level
+			);
+
+/**
+ * cg2900_fm_tx_set_power_level()- Sets Power level.
+ *
+ * Sets the Power level for FM Tx.
+ * @power_level: Power level.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_power_level(
+			u16 power_level
+			);
+
+/**
+ * cg2900_fm_tx_rds()- Enable or disable Tx RDS.
+ *
+ * Enable or disable RDS transmission.
+ * @enable_rds: Flag indicating enabling or disabling RDS.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_rds(
+			bool enable_rds
+			);
+
+/**
+ * cg2900_fm_set_audio_balance()- Sets Audio Balance.
+ *
+ * @balance: Audio Balnce to be Set in Percentage.
+ * (-100: Right Mute.... 0: Both on.... 100: Left Mute)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_audio_balance(
+			s8 balance
+			);
+
+/**
+ * cg2900_fm_set_volume()- Sets the Analog Out Gain of FM Chip.
+ *
+ * @vol_level: Volume Level to be set on Tuner (0-20).
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_volume(
+			u8 vol_level
+			);
+
+/**
+ * cg2900_fm_get_volume()- Gets the currently set Analog Out Gain of FM Chip.
+ *
+ * @vol_level: (out)Volume Level set on Tuner (0-20).
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_volume(
+			u8 *vol_level
+			);
+
+/**
+ * cg2900_fm_rds_off()- Disables the RDS decoding algorithm in FM chip
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_rds_off(void);
+
+/**
+ * cg2900_fm_rds_on()- Enables the RDS decoding algorithm in FM chip
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_rds_on(void);
+
+/**
+ * cg2900_fm_get_rds_status()- Retrieves the status whether RDS is enabled or not
+ *
+ * @rds_status: (out) Status of RDS
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_rds_status(
+			bool *rds_status
+			);
+
+/**
+ * cg2900_fm_mute()- Mutes the Audio output from FM Chip
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_mute(void);
+
+/**
+ * cg2900_fm_unmute()- Unmutes the Audio output from FM Chip
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_unmute(void);
+
+/**
+ * cg2900_fm_get_frequency()- Gets the Curently tuned Frequency on FM Radio
+ *
+ * @freq: (out) Frequency in Hz set on the FM Radio.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_frequency(
+			u32 *freq
+			);
+
+/**
+ * cg2900_fm_set_frequency()- Sets the frequency on FM Radio
+ *
+ * @new_freq: Frequency in Hz to be set on the FM Radio.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_frequency(
+			u32 new_freq
+			);
+
+/**
+ * cg2900_fm_get_signal_strength()- Gets the RSSI level.
+ *
+ * @signal_strength: (out) RSSI level of the currently
+ * tuned frequency.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_signal_strength(
+			u16 *signal_strength
+			);
+
+/**
+ * cg2900_fm_get_af_updat()- Retrives results of AF Update
+ *
+ * @af_update_rssi: (out) RSSI level of the Alternative frequency.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_af_update_get_result(
+			u16 *af_update_rssi
+			);
+
+
+/**
+ * cg2900_fm_af_update_start()- PErforms AF Update.
+ *
+ * @af_freq: AF frequency in Hz whose RSSI is to be retrived.
+ * tuned frequency.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+
+int cg2900_fm_af_update_start(
+			u32 af_freq
+			);
+
+/**
+ * cg2900_fm_af_switch_get_result()- Retrives the AF switch result.
+ *
+ * @af_switch_conclusion: (out) Conclusion of the AF Switch.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_af_switch_get_result(
+			u16 *af_switch_conclusion
+			);
+
+/**
+ * cg2900_fm_af_switch_start()- PErforms AF switch.
+ *
+ * @af_switch_freq: Alternate Frequency in Hz to be switched.
+ * @af_switch_pi: picode of the Alternative frequency.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_af_switch_start(
+			u32 af_switch_freq,
+			u16 af_switch_pi
+			);
+
+/**
+ * cg2900_fm_get_mode()- Gets the mode of the Radio tuner.
+ *
+ * @cur_mode: (out) Current mode set on FM Radio
+ * (0: Stereo, 1: Mono, 2: Blending, 3: Switching).
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_mode(
+			u8 *cur_mode
+			);
+
+/**
+ * cg2900_fm_set_mode()- Sets the mode on the Radio tuner.
+ *
+ * @mode: mode to be set on FM Radio
+ * (0: Stereo, 1: Mono, 2: Blending, 3: Switching.)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_mode(
+			u8 mode
+			);
+
+/**
+ * cg2900_fm_select_antenna()- Selects the Antenna of the Radio tuner.
+ *
+ * @antenna: (0: Embedded, 1: Wired.)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_select_antenna(
+			u8 antenna
+			);
+
+/**
+ * cg2900_fm_get_antenna()- Retreives the currently selected antenna.
+ *
+ * @antenna: out (0: Embedded, 1: Wired.)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_antenna(
+			u8 *antenna
+			);
+
+/**
+ * cg2900_fm_get_rssi_threshold()- Gets the rssi threshold currently
+ *
+ * set on FM radio.
+ * @rssi_thresold: (out) Current rssi threshold set.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_rssi_threshold(
+			u16 *rssi_thresold
+			);
+
+/**
+ * cg2900_fm_set_rssi_threshold()- Sets the rssi threshold to be used during
+ *
+ * Band Scan and seek Stations
+ * @rssi_thresold: rssi threshold to be set.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_rssi_threshold(
+			u16 rssi_thresold
+			);
+
+/**
+ * cg2900_handle_device_reset()- Handle The reset of Device
+ */
+void cg2900_handle_device_reset(void);
+
+/**
+ * wake_up_poll_queue()- Wakes up the Task waiting on Poll Queue.
+ * This function is called when Scan Band or seek has completed.
+ */
+void wake_up_poll_queue(void);
+
+/**
+ * void wake_up_read_queue()- Wakes up the Task waiting on Read Queue.
+ * This function is called when RDS data is available for reading by
+ * application.
+ */
+void wake_up_read_queue(void);
+
+/**
+ * void cg2900_fm_set_chip_version()- Sets the Version of the Controller.
+ *
+ * This function is used to update the Chip Version information at time
+ * of intitialization of FM driver.
+ * @revision:	Revision of the controller, e.g. to indicate that it is
+ *			a CG2900 controller.
+ * @sub_version:	Subversion of the controller, e.g. to indicate a certain
+ *			tape-out of the controller.
+ */
+void cg2900_fm_set_chip_version(
+			u16 revision,
+			u16 sub_version
+			);
+
+/**
+ * cg2900_fm_rx_set_deemphasis()- Sets de-emhasis level.
+ *
+ * Sets the Deemphasis level on FM Rx.
+ * @deemphasis: Deemphasis level.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_rx_set_deemphasis(
+			u8 deemphasis
+			);
+
+/**
+ * cg2900_fm_set_test_tone_generator()- Sets the Test Tone Generator.
+ *
+ * This function is used to enable/disable the Internal Tone Generator of
+ * CG2900.
+ * @test_tone_status: Status of tone generator.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_test_tone_generator(
+			u8 test_tone_status
+			);
+
+
+/**
+ * cg2900_fm_test_tone_connect()- Connect Audio outputs/inputs.
+ *
+ * This function connects the audio outputs/inputs of the Internal Tone
+ * Generator of CG2900.
+ * @left_audio_mode: Left Audio Output Mode.
+ * @right_audio_mode: Right Audio Output Mode.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_test_tone_connect(
+			u8 left_audio_mode,
+			u8 right_audio_mode
+			);
+
+/**
+ * cg2900_fm_test_tone_set_params()- Sets the Test Tone Parameters.
+ *
+ * This function is used to set the parameters of the Internal Tone Generator of
+ * CG2900.
+ * @tone_gen: Tone to be configured (Tone 1 or Tone 2)
+ * @frequency: Frequency of the tone.
+ * @volume: Volume of the tone.
+ * @phase_offset: Phase offset of the tone.
+ * @dc: DC to add to tone.
+ * @waveform: Waveform to generate, sine or pulse.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_test_tone_set_params(
+			u8 tone_gen,
+			u16 frequency,
+			u16 volume,
+			u16 phase_offset,
+			u16 dc,
+			u8 waveform
+			);
+
+#endif /* CG2900_FM_API_H */
diff --git a/drivers/media/radio/CG2900/cg2900_fm_driver.c b/drivers/media/radio/CG2900/cg2900_fm_driver.c
new file mode 100644
index 00000000000..6ac380409c1
--- /dev/null
+++ b/drivers/media/radio/CG2900/cg2900_fm_driver.c
@@ -0,0 +1,4829 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Linux FM Driver for CG2900 FM Chip
+ *
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#include <linux/device.h>
+#include <linux/time.h>
+#include <linux/mfd/cg2900.h>
+#include <linux/module.h>
+#include <linux/skbuff.h>
+#include <asm-generic/errno-base.h>
+#include "cg2900_fm_driver.h"
+
+/*
+ * Macro for printing the HCI Packet received from Protocol Driver
+ * to FM Driver.
+ */
+#define CG2900_HEX_READ_PACKET_DUMP \
+	if (cg2900_fm_debug_level == FM_HCI_PACKET_LOGS) \
+		fmd_hexdump('<', skb->data, skb->len);
+
+/* Macro for printing the HCI Packet sent to Protocol Driver from FM Driver */
+#define CG2900_HEX_WRITE_PACKET_DUMP \
+	if (cg2900_fm_debug_level == FM_HCI_PACKET_LOGS) \
+		fmd_hexdump('>', send_buffer, num_bytes);
+
+/* Converts the given value to ASCII format*/
+#define ASCVAL(x)(((x) <= 9) ? (x) + '0' : (x) - 10 + 'a')
+
+/* The receive Packet's 1st byte indicates the packet length */
+#define FM_GET_PKT_LEN(__data) __data[0]
+
+/* The receive Packet's following bytes are the actual data. */
+#define FM_GET_RSP_PKT_ADDR(__data) (&__data[1])
+
+/*
+ * LSB consists of shifting the command Id by 3 bits
+ * to left and ORING with the number of parameters of the
+ * command.
+ */
+#define FM_CMD_GET_LSB(__cmd_id, __num_param) \
+		((u8)(((__cmd_id << 3) & 0x00FF) | __num_param))
+
+/* MSB consists of shifting the command Id by 5 bits to right. */
+#define FM_CMD_GET_MSB(__cmd_id) \
+		((u8)(__cmd_id >> 5))
+
+/*
+ * Command id is mapped as shifting the MSB 5 bits to left and
+ * ORING with LSB shifted 3 bits to right.
+ */
+#define FM_GET_CMD_ID(__data) \
+		((u16)((__data[2] << 5) | __data[1] >> 3))
+
+/*
+ * Number of parameters in the response packet are the last 3 bits
+ * of the 1st byte of the received packet.
+ */
+#define FM_GET_NUM_PARAMS(__data) \
+		((u16)((__data[1] & 0x07)))
+
+/* Function Id is mapped to the 1st byte of the received packet */
+#define FM_GET_FUNCTION_ID(__data) __data[0]
+
+/*
+ * Block Id of the FM Firmware downloaded is mapped to the
+ *  2nd byte of the received packet.
+ */
+#define FM_GET_BLOCK_ID(__data) __data[1]
+
+/* Status of the received packet is mapped to the 4th byte. */
+#define FM_GET_STATUS(__data) __data[3]
+
+/*
+ * For PG1 of CG2900, the FM Interrupt is mapped
+ * to the 3rd and 4th byte of the received packet.
+ */
+#define FM_GET_PGI_INTERRUPT(__data) \
+		((u16)(__data[3] << 8 | __data[2]))
+
+/*
+ * For PG2 of CG2900, the FM Interrupt is mapped
+ * to the 5th and 6th byte of the received packet.
+ */
+#define FM_GET_PG2_INTERRUPT(__data) \
+		((u16)(__data[5] << 8 | __data[4]))
+
+#define FM_GET_NUM_RDS_GRPS(__data) __data[0]
+
+/* Response buffer starts from the 4th byte if the response buffer */
+#define FM_GET_RSP_BUFFER_ADDR(__data) (&__data[3])
+
+/* FM Function buffer starts from the 5th byte if the response buffer */
+#define FM_GET_FUNCTION_ADDR(__data) (&__data[4])
+
+/*
+ * Maximum time for chip to respond including the Command
+ * Competion interrupts for some commands. This time has been
+ * adjusted to cater to increased communication time with chip
+ * when debug level is set to 4.
+ */
+#define MAX_RESPONSE_TIME_IN_MS	5000
+
+/*
+ * enum fmd_gocmd_t - FM Driver Command state.
+ *
+ * @FMD_STATE_NONE: FM Driver in Idle state
+ * @FMD_STATE_MODE: FM Driver in setmode state
+ * @FMD_STATE_FREQUENCY: FM Driver in Set frequency state.
+ * @FMD_STATE_PA: FM Driver in SetPA state.
+ * @FMD_STATE_PA_LEVEL: FM Driver in Setpalevl state.
+ * @FMD_STATE_ANTENNA: FM Driver in Setantenna state
+ * @FMD_STATE_MUTE: FM Driver in Setmute state
+ * @FMD_STATE_SEEK: FM Driver in seek mode
+ * @FMD_STATE_SEEK_STOP: FM Driver in seek stop level state.
+ * @FMD_STATE_SCAN_BAND: FM Driver in Scanband mode
+ * @FMD_STATE_TX_SET_CTRL: FM Driver in RDS control state
+ * @FMD_STATE_TX_SET_THRSHLD: FM Driver in RDS threshld state
+ * @FMD_STATE_GEN_POWERUP: FM Driver in Power UP state.
+ * @FMD_STATE_SELECT_REF_CLK: FM Driver in Select Reference clock state.
+ * @FMD_STATE_SET_REF_CLK_PLL: FM Driver in Set Reference Freq state.
+ * @FMD_STATE_BLOCK_SCAN: FM Driver in Block Scan state.
+ * @FMD_STATE_AF_UPDATE: FM Driver in AF Update State.
+ * @FMD_STATE_AF_SWITCH: FM Driver in AF Switch State.
+ * @FMD_STATE_LAST: Last State of FM Driver
+ *
+ * Various states of the FM driver.
+ */
+enum fmd_gocmd {
+	FMD_STATE_NONE,
+	FMD_STATE_MODE,
+	FMD_STATE_FREQUENCY,
+	FMD_STATE_PA,
+	FMD_STATE_PA_LEVEL,
+	FMD_STATE_ANTENNA,
+	FMD_STATE_MUTE,
+	FMD_STATE_SEEK,
+	FMD_STATE_SEEK_STOP,
+	FMD_STATE_SCAN_BAND,
+	FMD_STATE_TX_SET_CTRL,
+	FMD_STATE_TX_SET_THRSHLD,
+	FMD_STATE_GEN_POWERUP,
+	FMD_STATE_SELECT_REF_CLK,
+	FMD_STATE_SET_REF_CLK_PLL,
+	FMD_STATE_BLOCK_SCAN,
+	FMD_STATE_AF_UPDATE,
+	FMD_STATE_AF_SWITCH,
+	FMD_STATE_LAST
+};
+
+/**
+ * struct fmd_rdsgroup_t - Rds group structure.
+ *
+ * @block: Array for RDS Block(s) received.
+ * @status: Array of Status of corresponding RDS block(s).
+ *
+ * It stores the value and status of a particular RDS group
+ * received.
+ */
+struct fmd_rds_group {
+	u16 block[NUM_OF_RDS_BLOCKS];
+	u8  status[NUM_OF_RDS_BLOCKS];
+};
+
+/**
+ * struct fmd_states_info - Main FM state info structure.
+ *
+ * @fmd_initialized: Flag indicating FM Driver is initialized or not
+ * @rx_freq_range: Receiver freq range
+ * @rx_volume:  Receiver volume level
+ * @rx_antenna: Receiver Antenna
+ * @rx_seek_stop_level:  RDS seek stop Level
+ * @rx_rds_on:  Receiver RDS ON
+ * @rx_stereo_mode: Receiver Stereo mode
+ * @max_channels_to_scan: Maximum Number of channels to Scan.
+ * @tx_freq_range: Transmitter freq Range
+ * @tx_preemphasis: Transmitter Pre emphiasis level
+ * @tx_stereo_mode: Transmitter stero mode
+ * @tx_rds_on:  Enable RDS
+ * @tx_pilot_dev: PIlot freq deviation
+ * @tx_rds_dev:  RDS deviation
+ * @tx_strength:  TX Signal Stregnth
+ * @irq_index:  Index where last interrupt is added to Interrupt queue
+ * @interrupt_available_for_processing:  Flag indicating if interrupt is
+ * available for processing or not.
+ * @interrupt_queue: Circular Queue to store the received interrupt from chip.
+ * @gocmd:  Command which is in progress.
+ * @rds_group:  Array of RDS group Buffer
+ * @callback: Callback registered by upper layers.
+ */
+struct fmd_states_info {
+	bool		fmd_initialized;
+	u8			rx_freq_range;
+	u8			rx_volume;
+	u8			rx_antenna;
+	u16			rx_seek_stop_level;
+	bool		rx_rds_on;
+	u8			rx_stereo_mode;
+	u8			tx_freq_range;
+	u8			tx_preemphasis;
+	bool		tx_stereo_mode;
+	u8			max_channels_to_scan;
+	bool		tx_rds_on;
+	u16			tx_pilot_dev;
+	u16			tx_rds_dev;
+	u16			tx_strength;
+	u8			irq_index;
+	bool		interrupt_available_for_processing;
+	u16			interrupt_queue[MAX_COUNT_OF_IRQS];
+	enum fmd_gocmd gocmd;
+	struct fmd_rds_group	rds_group[MAX_RDS_GROUPS];
+	fmd_radio_cb callback;
+};
+
+/**
+ * struct fmd_data - Main structure for FM data exchange.
+ *
+ * @cmd_id: Command Id of the command being exchanged.
+ * @num_parameters: Number of parameters
+ * @parameters: FM data parameters.
+ */
+struct fmd_data {
+	u32 cmd_id;
+	u16 num_parameters;
+	u8 *parameters;
+};
+
+static struct fmd_states_info fmd_state_info;
+static struct fmd_data fmd_data;
+static struct semaphore cmd_sem;
+static struct semaphore rds_sem;
+static struct semaphore interrupt_sem;
+static struct task_struct *rds_thread_task;
+static struct task_struct *irq_thread_task;
+static struct device *cg2900_fm_dev;
+static struct mutex write_mutex;
+static struct mutex send_cmd_mutex;
+static spinlock_t fmd_spinlock;
+static spinlock_t fmd_spinlock_read;
+
+/* Debug Level
+ * 1: Only Error Logs
+ * 2: Info Logs
+ * 3: Debug Logs
+ * 4: HCI Logs
+ */
+unsigned short cg2900_fm_debug_level = FM_ERROR_LOGS;
+EXPORT_SYMBOL(cg2900_fm_debug_level);
+
+static cg2900_fm_rds_cb cb_rds_func;
+static bool rds_thread_required;
+static bool irq_thread_required;
+
+static char event_name[FMD_EVENT_LAST_ELEMENT][MAX_NAME_SIZE] = {
+	"FMD_EVENT_OPERATION_COMPLETED",
+	"FMD_EVENT_ANTENNA_STATUS_CHANGED",
+	"FMD_EVENT_FREQUENCY_CHANGED",
+	"FMD_EVENT_SEEK_COMPLETED",
+	"FMD_EVENT_SCAN_BAND_COMPLETED",
+	"FMD_EVENT_BLOCK_SCAN_COMPLETED",
+	"FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE",
+	"FMD_EVENT_MONO_STEREO_TRANSITION_COMPLETE",
+	"FMD_EVENT_SEEK_STOPPED",
+	"FMD_EVENT_GEN_POWERUP",
+	"FMD_EVENT_RDSGROUP_RCVD",
+};
+
+static char interrupt_name[MAX_COUNT_OF_IRQS][MAX_NAME_SIZE] = {
+	"IRPT_OPERATION_SUCCEEDED",
+	"IRPT_OPERATION_FAILED",
+	"IRPT_NOT_DEFINED",
+	"IRPT_RX_BUFFER_FULL_OR_TX_BUFFER_EMPTY",
+	"IRPT_RX_SIGNAL_QUALITY_LOW_OR_TX_MUTE_STATUS_CHANGED",
+	"IRPT_MONO_STEREO_TRANSITION",
+	"IRPT_RX_RDS_SYNC_FOUND_OR_TX_INPUT_OVERDRIVE",
+	"IRPT_RDS_SYNC_LOST",
+	"IRPT_PI_CODE_CHANGED",
+	"IRPT_REQUESTED_BLOCK_AVAILABLE",
+	"IRPT_NOT_DEFINED",
+	"IRPT_NOT_DEFINED",
+	"IRPT_NOT_DEFINED",
+	"IRPT_NOT_DEFINED",
+	"IRPT_WARM_BOOT_READY",
+	"IRPT_COLD_BOOT_READY",
+};
+
+
+static void fmd_hexdump(
+			char prompt,
+			u8 *buffer,
+			int num_bytes
+			);
+static u8 fmd_get_event(
+			enum fmd_gocmd	gocmd
+			);
+static void fmd_event_name(
+			u8 event,
+			char *event_name
+			);
+static char *fmd_get_fm_function_name(
+			u8 fm_function
+			);
+static void fmd_interrupt_name(
+			u16 interrupt,
+			char *interrupt_name
+			);
+static void fmd_add_interrupt_to_queue(
+			u16 interrupt
+			);
+static void fmd_process_interrupt(
+			u16 interrupt
+			);
+static void fmd_callback(
+			u8 event,
+			bool event_successful
+			);
+static int fmd_rx_frequency_to_channel(
+			u32 freq,
+			u16 *channel
+			);
+static int fmd_rx_channel_to_frequency(
+			u16 channel_number,
+			u32 *frequency
+			);
+static int fmd_tx_frequency_to_channel(
+			u32 freq,
+			u16 *channel
+			);
+static int fmd_tx_channel_to_frequency(
+			u16 channel_number,
+			u32 *frequency
+			);
+static bool fmd_go_cmd_busy(void);
+static int fmd_send_cmd_and_read_resp(
+			const u16 cmd_id,
+			const u16 num_parameters,
+			const u16 *parameters,
+			u16 *resp_num_parameters,
+			u16 *resp_parameters
+			);
+static int fmd_send_cmd(
+			const u16 cmd_id,
+			const u16 num_parameters,
+			const u16 *parameters
+			);
+static int fmd_read_resp(
+			u16 *cmd_id,
+			u16 *num_parameters,
+			u16 *parameters
+			);
+static void fmd_process_fm_function(
+			u8 *packet_buffer
+			);
+static int fmd_write_file_block(
+			u32 file_block_id,
+			u8 *file_block,
+			u16 file_block_length
+			);
+static void fmd_receive_data(
+			u16 packet_length,
+			u8 *packet_buffer
+			);
+static int fmd_rds_thread(
+			void *data
+			);
+static void fmd_start_irq_thread(void);
+static void fmd_stop_irq_thread(void);
+static int fmd_irq_thread(
+			void *data
+			);
+static int fmd_send_packet(
+			u16 num_bytes,
+			u8 *send_buffer
+			);
+static int fmd_get_cmd_sem(void);
+static void fmd_set_cmd_sem(void);
+static void fmd_get_interrupt_sem(void);
+static void fmd_set_interrupt_sem(void);
+static bool fmd_driver_init(void);
+static void fmd_driver_exit(void);
+
+/* structure declared in time.h */
+struct timespec time_spec;
+
+
+/**
+ * fmd_hexdump() - Displays the HCI Data Bytes exchanged with FM Chip.
+ *
+ * @prompt: Prompt signifying the direction '<' for Rx '>' for Tx
+ * @buffer: Buffer to be displayed.
+ * @num_bytes: Number of bytes of the buffer.
+ */
+ static void fmd_hexdump(
+			char prompt,
+			u8 *buffer,
+			int num_bytes
+			)
+{
+	int i;
+	u8 tmp_val;
+	struct timespec time;
+	static u8 pkt_write[MAX_BUFFER_SIZE], *pkt_ptr;
+
+	getnstimeofday(&time);
+	sprintf(pkt_write, "\n[%08x:%08x] [%04x] %c",
+		(unsigned int)time.tv_sec,
+		(unsigned int)time.tv_nsec,
+		num_bytes, prompt);
+
+	pkt_ptr = pkt_write + strlen(pkt_write);
+	if (buffer == NULL)
+		return;
+
+	/* Copy the buffer only if the input buffer is not NULL */
+	for (i = 0; i < num_bytes; i++) {
+		*pkt_ptr++ = ' ';
+		tmp_val = buffer[i] >> 4;
+		*pkt_ptr++ = ASCVAL(tmp_val);
+		tmp_val = buffer[i] & 0x0F;
+		*pkt_ptr++ = ASCVAL(tmp_val);
+		if (i > 20) {
+			/* Print only 20 bytes at max */
+			break;
+		}
+	}
+	*pkt_ptr++ = '\0';
+	FM_HEX_REPORT("%s", pkt_write);
+}
+
+/**
+ * fmd_get_event() - Returns the Event based on FM Driver State.
+ *
+ * @gocmd: Pending FM Command
+ *
+ * Returns: Corresponding Event
+ */
+static u8 fmd_get_event(
+			enum fmd_gocmd gocmd
+			)
+{
+	u8 event = FMD_EVENT_OPERATION_COMPLETED;
+	switch (gocmd) {
+	case FMD_STATE_ANTENNA:
+		event = FMD_EVENT_ANTENNA_STATUS_CHANGED;
+		break;
+	case FMD_STATE_FREQUENCY:
+		event = FMD_EVENT_FREQUENCY_CHANGED;
+		break;
+	case FMD_STATE_SEEK:
+		event = FMD_EVENT_SEEK_COMPLETED;
+		break;
+	case FMD_STATE_SCAN_BAND:
+		event = FMD_EVENT_SCAN_BAND_COMPLETED;
+		break;
+	case FMD_STATE_BLOCK_SCAN:
+		event = FMD_EVENT_BLOCK_SCAN_COMPLETED;
+		break;
+	case FMD_STATE_AF_UPDATE:
+	/* Drop Down */
+	case FMD_STATE_AF_SWITCH:
+		event = FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE;
+		break;
+	case FMD_STATE_SEEK_STOP:
+		event = FMD_EVENT_SEEK_STOPPED;
+		break;
+	default:
+		event = FMD_EVENT_OPERATION_COMPLETED;
+		break;
+	}
+	return event;
+}
+
+/**
+ * fmd_event_name() - Converts the event to a displayable string.
+ *
+ * @event: Event that has occurred.
+ * @eventname: (out) Buffer to store event name.
+ */
+static void fmd_event_name(
+			u8 event,
+			char *eventname
+			)
+{
+	if (eventname == NULL) {
+		FM_ERR_REPORT("fmd_event_name: Output Buffer is NULL");
+		return;
+	}
+	if (event < FMD_EVENT_LAST_ELEMENT)
+		strcpy(eventname, event_name[event]);
+	else
+		strcpy(eventname, "FMD_EVENT_UNKNOWN");
+}
+
+/**
+ * fmd_get_fm_function_name() - Returns the FM Fucntion name.
+ *
+ * @fm_function: Function whose name is to be retrieved.
+ *
+ * Returns FM Function Name.
+ */
+static char *fmd_get_fm_function_name(
+			u8 fm_function
+			)
+{
+	switch (fm_function) {
+	case FM_FUNCTION_ENABLE:
+		return "FM_FUNCTION_ENABLE";
+		break;
+	case FM_FUNCTION_DISABLE:
+		return "FM_FUNCTION_DISABLE";
+		break;
+	case FM_FUNCTION_RESET:
+		return "FM_FUNCTION_RESET";
+		break;
+	case FM_FUNCTION_WRITE_COMMAND:
+		return "FM_FUNCTION_WRITE_COMMAND";
+		break;
+	case FM_FUNCTION_SET_INT_MASK_ALL:
+		return "FM_FUNCTION_SET_INT_MASK_ALL";
+		break;
+	case FM_FUNCTION_GET_INT_MASK_ALL:
+		return "FM_FUNCTION_GET_INT_MASK_ALL";
+		break;
+	case FM_FUNCTION_SET_INT_MASK:
+		return "FM_FUNCTION_SET_INT_MASK";
+		break;
+	case FM_FUNCTION_GET_INT_MASK:
+		return "FM_FUNCTION_GET_INT_MASK";
+		break;
+	case FM_FUNCTION_FIRMWARE_DOWNLOAD:
+		return "FM_FUNCTION_FIRMWARE_DOWNLOAD";
+		break;
+	default:
+		return "FM_FUNCTION_UNKNOWN";
+		break;
+	}
+}
+
+/**
+ * fmd_interrupt_name() - Converts the interrupt to a displayable string.
+ *
+ * @interrupt: interrupt received from FM Chip
+ * @interruptname: (out) Buffer to store interrupt name.
+ */
+static void fmd_interrupt_name(
+			u16 interrupt,
+			char *interruptname
+			)
+{
+	int index;
+
+	if (interruptname == NULL) {
+		FM_ERR_REPORT("fmd_interrupt_name: Output Buffer is NULL!!!");
+		return;
+	}
+	/* Convert Interrupt to Bit */
+	for (index = 0; index < MAX_COUNT_OF_IRQS; index++) {
+		if (interrupt & (1 << index)) {
+			/* Match found, break the loop */
+			break;
+		}
+	}
+	if (index < MAX_COUNT_OF_IRQS)
+		strcpy(interruptname, interrupt_name[index]);
+	else
+		strcpy(interruptname, "IRPT_UNKNOWN");
+}
+
+/**
+ * fmd_add_interrupt_to_queue() - Add interrupt to IRQ Queue.
+ *
+ * @interrupt: interrupt received from FM Chip
+ */
+static void fmd_add_interrupt_to_queue(
+			u16 interrupt
+			)
+{
+	FM_DEBUG_REPORT("fmd_add_interrupt_to_queue : "
+			"Interrupt Received = %04x", (u16) interrupt);
+
+	/* Reset the index if it reaches the array limit */
+	if (fmd_state_info.irq_index > MAX_COUNT_OF_IRQS - 1) {
+		spin_lock(&fmd_spinlock);
+		fmd_state_info.irq_index = 0;
+		spin_unlock(&fmd_spinlock);
+	}
+
+	spin_lock(&fmd_spinlock);
+	fmd_state_info.interrupt_queue[fmd_state_info.irq_index] = interrupt;
+	fmd_state_info.irq_index++;
+	spin_unlock(&fmd_spinlock);
+	if (!fmd_state_info.interrupt_available_for_processing) {
+		spin_lock(&fmd_spinlock);
+		fmd_state_info.interrupt_available_for_processing = true;
+		spin_unlock(&fmd_spinlock);
+		fmd_set_interrupt_sem();
+	}
+}
+
+/**
+ * fmd_process_interrupt() - Processes the Interrupt.
+ *
+ * This function processes the interrupt received from FM Chip
+ * and calls the corresponding callback registered by upper layers with
+ * proper parameters.
+ * @interrupt: interrupt received from FM Chip
+ */
+static void fmd_process_interrupt(
+			u16 interrupt
+			)
+{
+	char irpt_name[MAX_NAME_SIZE];
+
+	fmd_interrupt_name(interrupt, irpt_name);
+	FM_DEBUG_REPORT("%s", irpt_name);
+	if ((interrupt & IRPT_OPERATION_SUCCEEDED) |
+		(interrupt & IRPT_OPERATION_FAILED)) {
+		bool event_status = (interrupt & IRPT_OPERATION_SUCCEEDED);
+		u8 event = fmd_get_event(fmd_state_info.gocmd);
+
+		switch (fmd_state_info.gocmd) {
+		case FMD_STATE_MODE:
+			/* Mode has been changed. */
+		case FMD_STATE_MUTE:
+			/* FM radio is Muter or Unmuted */
+		case FMD_STATE_PA:
+			/* Power Amplifier has been enabled/disabled */
+		case FMD_STATE_PA_LEVEL:
+			/* Power Amplifier Level has been changed. */
+		case FMD_STATE_SELECT_REF_CLK:
+			/* Reference Clock has been selected. */
+		case FMD_STATE_SET_REF_CLK_PLL:
+			/* Reference Clock frequency has been changed. */
+		case FMD_STATE_TX_SET_CTRL:
+			/* Tx Control has been set. */
+		case FMD_STATE_TX_SET_THRSHLD:
+			/* Tx Threashold has been set. */
+			/* Set State to None and set the waiting semaphore. */
+			fmd_state_info.gocmd = FMD_STATE_NONE;
+			fmd_set_cmd_sem();
+			break;
+		case FMD_STATE_ANTENNA:
+			/* Antenna status has been changed. */
+		case FMD_STATE_SEEK_STOP:
+			/* Band scan, seek or block scan has completed. */
+		case FMD_STATE_AF_UPDATE:
+			/* AF Update has completed. */
+		case FMD_STATE_AF_SWITCH:
+			/* AF Switch has completed. */
+		case FMD_STATE_FREQUENCY:
+			/* Frequency has been changed. */
+			/*
+			 * Set State to None, set the waiting semaphore,
+			 * and inform upper layer.
+			 */
+			fmd_state_info.gocmd = FMD_STATE_NONE;
+			fmd_set_cmd_sem();
+			fmd_callback(
+				event,
+				event_status);
+			break;
+		case FMD_STATE_SEEK:
+			/* Seek has completed. */
+		case FMD_STATE_SCAN_BAND:
+			/* Band scan has completed. */
+		case FMD_STATE_BLOCK_SCAN:
+			/* Block scan has completed. */
+			/*
+			 * Set State to None. No need to set the
+			 * semaphore since this is an asyncronous event.
+			 */
+			fmd_state_info.gocmd = FMD_STATE_NONE;
+			/* Inform Upper layer. */
+			fmd_callback(event,	event_status);
+			break;
+		default:
+			/* Do Nothing */
+			FM_ERR_REPORT("Default %s case of "\
+				"interrupt processing", event_status ? \
+				"Success" : "Failed");
+			break;
+		}
+	}
+
+	if (interrupt & IRPT_RX_BUFFERFULL_TX_BUFFEREMPTY) {
+		/*
+		 * RDS Buffer Full or RDS Buffer Empty
+		 * interrupt received from chip, indicating
+		 * that RDS data is available if chip
+		 * is in Rx mode or RDS data can be send
+		 * to chip in case of Tx mode. Inform the
+		 * upper layers about this interrupt.
+		 */
+		fmd_callback(
+		     FMD_EVENT_RDSGROUP_RCVD,
+		     true);
+	}
+
+	if (interrupt & IRPT_RX_MONO_STEREO_TRANSITION) {
+		/*
+		 * Mono Stereo Transition interrupt
+		 * received from chip, inform the
+		 * upper layers about it.
+		 */
+		fmd_callback(
+		     FMD_EVENT_MONO_STEREO_TRANSITION_COMPLETE,
+		     true);
+	}
+
+	if ((interrupt & IRPT_COLD_BOOT_READY) |
+		(interrupt &  IRPT_WARM_BOOT_READY)) {
+		switch (fmd_state_info.gocmd) {
+		case FMD_STATE_GEN_POWERUP:
+			/*
+			 * Cold Boot/ Warm Boot Interrupt received from
+			 * chip, indicating transition from
+			 * power off/standby state to active state.
+			 * Inform the upper layers about it.
+			 */
+			fmd_callback(
+				FMD_EVENT_GEN_POWERUP,
+				true);
+			/* Set State to None and set the waiting semaphore. */
+			fmd_state_info.gocmd = FMD_STATE_NONE;
+			fmd_set_cmd_sem();
+			break;
+		default:
+			/* Do Nothing */
+			break;
+		}
+	}
+}
+
+/**
+ * fmd_callback() - Callback function for upper layers.
+ *
+ * Callback function that calls the registered callback of upper
+ * layers with proper parameters.
+ * @event: event for which the callback function was called
+ * from FM Driver.
+ * @event_successful: Signifying whether the event is called from FM
+ * Driver on receiving irpt_Operation_Succeeded or irpt_Operation_Failed.
+ */
+static void fmd_callback(
+			u8 event,
+			bool event_successful
+			)
+{
+	char event_name_string[MAX_NAME_SIZE];
+
+	fmd_event_name(event, event_name_string);
+
+	FM_DEBUG_REPORT("%s %x, %d", event_name_string,
+		(unsigned int)event , (unsigned int)event_successful);
+
+	if (fmd_state_info.callback)
+		fmd_state_info.callback(
+				event,
+				event_successful);
+}
+
+/**
+ * fmd_rx_frequency_to_channel() - Converts Rx frequency to channel number.
+ *
+ * Converts the Frequency in kHz to corresponding Channel number.
+ * This is used for FM Rx.
+ * @freq: Frequency in kHz.
+ * @channel: Channel Number corresponding to the given Frequency.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameters are not valid.
+ *
+ */
+static int fmd_rx_frequency_to_channel(
+			u32 freq,
+			u16 *channel
+			)
+{
+	u8	range;
+	int	result;
+	u32	min_freq;
+	u32	max_freq;
+
+	if (channel == NULL) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_get_freq_range(
+			&range);
+
+	if (result != 0)
+		goto error;
+
+	result = fmd_get_freq_range_properties(
+			range,
+			&min_freq,
+			&max_freq);
+
+	if (result != 0)
+		goto error;
+
+	if (freq > max_freq)
+		freq = max_freq;
+	else if (freq < min_freq)
+		freq = min_freq;
+
+	/*
+	 * Frequency in kHz needs to be divided with 50 kHz to get
+	 * channel number for all FM Bands
+	 */
+	*channel = (u16)((freq - min_freq) / CHANNEL_FREQ_CONVERTER_MHZ);
+	result = 0;
+error:
+	return result;
+}
+
+/**
+ * fmd_rx_channel_to_frequency() - Converts Rx Channel number to frequency.
+ *
+ * Converts the Channel Number to corresponding Frequency in kHz.
+ * This is used for FM Rx.
+ * @channel_number: Channel Number to be converted.
+ * @frequency: Frequency corresponding to the corresponding channel in kHz.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameters are not valid.
+ *
+ */
+static int fmd_rx_channel_to_frequency(
+			u16 channel_number,
+			u32 *frequency
+			)
+{
+	u8	range;
+	int	result;
+	u32	min_freq;
+	u32	max_freq;
+
+	if (frequency == NULL) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_get_freq_range(
+			&range);
+
+	if (result != 0)
+		goto error;
+
+	result = fmd_get_freq_range_properties(
+			range,
+			&min_freq,
+			&max_freq);
+
+	if (result != 0)
+		goto error;
+
+	/*
+	 * Channel Number needs to be multiplied with 50 kHz to get
+	 * frequency in kHz for all FM Bands
+	 */
+	*frequency = min_freq + (channel_number * CHANNEL_FREQ_CONVERTER_MHZ);
+
+	if (*frequency > max_freq)
+		*frequency = max_freq;
+	else if (*frequency < min_freq)
+		*frequency = min_freq;
+
+error:
+	return result;
+}
+
+/**
+ * fmd_tx_frequency_to_channel() - Converts Tx frequency to channel number.
+ *
+ * Converts the Frequency in kHz to corresponding Channel number.
+ * This is used for FM Tx.
+ * @freq: Frequency in kHz.
+ * @channel: (out)Channel Number corresponding to the given Frequency.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameters are not valid.
+ */
+static int fmd_tx_frequency_to_channel(
+			u32 freq,
+			u16 *channel
+			)
+{
+	u8	range;
+	int	result;
+	u32	min_freq;
+	u32	max_freq;
+
+	if (channel == NULL) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_tx_get_freq_range(
+			&range);
+
+	if (result != 0)
+		goto error;
+
+	result = fmd_get_freq_range_properties(
+			range,
+			&min_freq,
+			&max_freq);
+
+	if (result != 0)
+		goto error;
+
+	if (freq > max_freq)
+		freq = max_freq;
+	else if (freq < min_freq)
+		freq = min_freq;
+
+	/*
+	 * Frequency in kHz needs to be divided with 50 kHz to get
+	 * channel number for all FM Bands
+	 */
+	*channel = (u16)((freq - min_freq) / CHANNEL_FREQ_CONVERTER_MHZ);
+	result = 0;
+error:
+	return result;
+}
+
+/**
+ * fmd_tx_channel_to_frequency() - Converts Tx Channel number to frequency.
+ *
+ * Converts the Channel Number to corresponding Frequency in kHz.
+ * This is used for FM Tx.
+ * @channel_number: Channel Number to be converted.
+ * @frequency: Frequency corresponding to the corresponding channel
+ * in kHz.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameters are not valid.
+ */
+static int fmd_tx_channel_to_frequency(
+			u16 channel_number,
+			u32 *frequency
+			)
+{
+	u8	range;
+	int	result;
+	u32	min_freq;
+	u32	max_freq;
+
+	if (frequency == NULL) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_tx_get_freq_range(
+			&range);
+
+	if (result != 0)
+		goto error;
+
+	result = fmd_get_freq_range_properties(
+			range,
+			&min_freq,
+			&max_freq);
+
+	if (result != 0)
+		goto error;
+
+	/*
+	 * Channel Number needs to be multiplied with 50 kHz to get
+	 * frequency in kHz for all FM Bands
+	 */
+	*frequency = min_freq + (channel_number * CHANNEL_FREQ_CONVERTER_MHZ);
+
+	if (*frequency > max_freq)
+		*frequency = max_freq;
+	else if (*frequency < min_freq)
+		*frequency = min_freq;
+
+	result = 0;
+error:
+	return result;
+}
+
+/**
+ * fmd_go_cmd_busy() - Function to check if FM Driver is busy or idle
+ *
+ * Returns:
+ *   false if FM Driver is Idle
+ *   true otherwise
+ */
+static bool fmd_go_cmd_busy(void)
+{
+	return (fmd_state_info.gocmd != FMD_STATE_NONE);
+}
+
+/**
+ * fmd_read_cb() - Handle Received Data
+ *
+ * This function handles data received from connectivity protocol driver.
+ * @dev: Device receiving data.
+ * @skb: Buffer with data coming form device.
+ */
+static void fmd_read_cb(
+			struct cg2900_user_data *dev,
+			struct sk_buff *skb
+			)
+{
+	FM_INFO_REPORT("fmd_read_cb");
+
+	if (skb->data == NULL || skb->len == 0)
+		goto error;
+
+	spin_lock(&fmd_spinlock_read);
+	CG2900_HEX_READ_PACKET_DUMP;
+	/*
+	 * The first byte is length of bytes following bytes
+	 * Rest of the bytes are the actual data
+	 */
+	fmd_receive_data(
+					FM_GET_PKT_LEN(skb->data),
+					FM_GET_RSP_PKT_ADDR(skb->data));
+
+error:
+	kfree_skb(skb);
+	spin_unlock(&fmd_spinlock_read);
+}
+
+/**
+ * fmd_receive_data() - Processes the FM data received from device.
+ *
+ * @packet_length: Length of received Data Packet
+ * @packet_buffer: Received Data buffer.
+ */
+static void fmd_receive_data(
+			u16 packet_length,
+			u8 *packet_buffer
+			)
+{
+	if (packet_buffer == NULL) {
+		FM_ERR_REPORT("fmd_receive_data: Buffer = NULL");
+		return;
+	}
+
+	if (packet_length == FM_PG1_INTERRUPT_EVENT_LEN &&
+		packet_buffer[0] == FM_CATENA_OPCODE &&
+		packet_buffer[1] == FM_EVENT_ID) {
+		/* PG 1.0 interrupt Handling */
+		u16 interrupt = FM_GET_PGI_INTERRUPT(packet_buffer);
+		FM_DEBUG_REPORT("interrupt = %04x",
+			(unsigned int)interrupt);
+		fmd_add_interrupt_to_queue(interrupt);
+	} else if (packet_length == FM_PG2_INTERRUPT_EVENT_LEN &&
+		packet_buffer[0] == FM_SUCCESS_STATUS &&
+		packet_buffer[1] == FM_CATENA_OPCODE &&
+		packet_buffer[2] == FM_EVENT &&
+		packet_buffer[3] == FM_EVENT_ID) {
+		/* PG 2.0 interrupt Handling */
+		u16 interrupt = FM_GET_PG2_INTERRUPT(packet_buffer);
+		FM_DEBUG_REPORT("interrupt = %04x",
+			(unsigned int)interrupt);
+		fmd_add_interrupt_to_queue(interrupt);
+	} else if (packet_buffer[0] == FM_SUCCESS_STATUS &&
+		packet_buffer[1] == FM_CATENA_OPCODE &&
+		packet_buffer[2] == FM_WRITE) {
+		/* Command Complete or RDS Data Handling */
+		u8 fm_status = FM_GET_STATUS(packet_buffer);;
+		switch (fm_status) {
+		case FM_CMD_STATUS_CMD_SUCCESS:
+			fmd_process_fm_function(
+				FM_GET_FUNCTION_ADDR(packet_buffer));
+			break;
+		case FM_CMD_STATUS_HCI_ERR_HW_FAILURE:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_HCI_ERR_HW_FAILURE");
+			break;
+		case FM_CMD_STATUS_HCI_ERR_INVALID_PARAMETERS:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_HCI_ERR_INVALID_PARAMETERS");
+			break;
+		case FM_CMD_STATUS_IP_UNINIT:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_IP_UNINIT");
+			break;
+		case FM_CMD_STATUS_HCI_ERR_UNSPECIFIED_ERROR:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_HCI_ERR_UNSPECIFIED_ERROR");
+			break;
+		case FM_CMD_STATUS_HCI_ERR_CMD_DISALLOWED:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_HCI_ERR_CMD_DISALLOWED");
+			break;
+		case FM_CMD_STATUS_WRONG_SEQ_NUM:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_WRONG_SEQ_NUM");
+			break;
+		case FM_CMD_STATUS_UNKNOWN_FILE_TYPE:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_UNKNOWN_FILE_TYPE");
+			break;
+		case FM_CMD_STATUS_FILE_VERSION_MISMATCH:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_FILE_VERSION_MISMATCH");
+			break;
+		default:
+			FM_DEBUG_REPORT(
+				"Unknown Status = %02x", fm_status);
+			break;
+		}
+	}
+}
+
+/**
+ * fmd_reset_cb() - Reset callback fuction.
+ *
+ * @dev: CPD device reseting.
+ */
+static void fmd_reset_cb(struct cg2900_user_data *dev)
+{
+	FM_INFO_REPORT("fmd_reset_cb: Device Reset");
+	spin_lock(&fmd_spinlock_read);
+	cg2900_handle_device_reset();
+	spin_unlock(&fmd_spinlock_read);
+}
+
+/**
+ * fmd_rds_thread() - Thread for receiving RDS data from Chip.
+ *
+ * @data: Data beng passed as parameter on starting the thread.
+ */
+static int fmd_rds_thread(
+			void *data
+			)
+{
+	FM_INFO_REPORT("fmd_rds_thread Created Successfully");
+	while (rds_thread_required) {
+		if (cb_rds_func)
+			cb_rds_func();
+		/* Give 100 ms for context switching */
+		schedule_timeout_interruptible(msecs_to_jiffies(100));
+	}
+	FM_DEBUG_REPORT("fmd_rds_thread Exiting!!!");
+	return 0;
+}
+
+/**
+ * fmd_start_irq_thread() - Function for starting Interrupt Thread.
+ */
+static void fmd_start_irq_thread(void)
+{
+	FM_INFO_REPORT("fmd_start_irq_thread");
+	irq_thread_task = kthread_create(fmd_irq_thread, NULL, "irq_thread");
+	if (IS_ERR(irq_thread_task)) {
+		FM_ERR_REPORT("fmd_start_irq_thread: "
+			"Unable to Create irq_thread");
+		irq_thread_task = NULL;
+		return;
+	}
+	wake_up_process(irq_thread_task);
+}
+
+/**
+ * fmd_stop_irq_thread() - Function for stopping Interrupt Thread.
+ */
+static void fmd_stop_irq_thread(void)
+{
+	FM_INFO_REPORT("fmd_stop_irq_thread");
+	kthread_stop(irq_thread_task);
+	irq_thread_task = NULL;
+	FM_DEBUG_REPORT("-fmd_stop_irq_thread");
+}
+
+/**
+ * fmd_irq_thread() - Thread for processing Interrupts received from Chip.
+ *
+ * @data: Data being passed as parameter on starting the thread.
+ */
+
+static int fmd_irq_thread(
+			void *data
+			)
+{
+	int index;
+
+	FM_INFO_REPORT("fmd_irq_thread Created Successfully");
+
+	while (irq_thread_required) {
+		if (!fmd_state_info.interrupt_available_for_processing) {
+			FM_DEBUG_REPORT("fmd_irq_thread: Waiting on irq sem "
+				"interrupt_available_for_processing = %d "
+				"fmd_state_info.fmd_initialized = %d",
+			fmd_state_info.interrupt_available_for_processing,
+			fmd_state_info.fmd_initialized);
+			fmd_get_interrupt_sem();
+			FM_DEBUG_REPORT("fmd_irq_thread: Waiting on irq sem "
+				"interrupt_available_for_processing = %d "
+				"fmd_state_info.fmd_initialized = %d",
+			fmd_state_info.interrupt_available_for_processing,
+			fmd_state_info.fmd_initialized);
+		}
+		index = 0;
+
+		if (fmd_state_info.interrupt_available_for_processing) {
+			while (index < MAX_COUNT_OF_IRQS) {
+				if (fmd_state_info.interrupt_queue[index]
+				    != IRPT_INVALID) {
+					FM_DEBUG_REPORT("fmd_process_interrupt "
+						"Interrupt = %04x",
+						fmd_state_info.
+						interrupt_queue[index]);
+					fmd_process_interrupt(
+					fmd_state_info.interrupt_queue[index]);
+					fmd_state_info.interrupt_queue[index]
+					    = IRPT_INVALID;
+				}
+				index++;
+			}
+		}
+		fmd_state_info.interrupt_available_for_processing = false;
+		schedule_timeout_interruptible(msecs_to_jiffies(100));
+	}
+	FM_DEBUG_REPORT("fmd_irq_thread Exiting!!!");
+	return 0;
+}
+
+/**
+ * fmd_send_packet() - Sends the FM HCI Packet to the CG2900 Protocol Driver.
+ *
+ * @num_bytes: Number of bytes of Data to be sent including
+ * Channel Identifier (08)
+ * @send_buffer: Buffer containing the Data to be sent to Chip.
+ *
+ * Returns:
+ * 0, If packet was sent successfully to
+ * CG2900 Protocol Driver, otherwise the corresponding error.
+ * -EINVAL If parameters are not valid.
+ * -EIO If there is an Input/Output Error.
+ */
+static int fmd_send_packet(
+			u16 num_bytes,
+			u8 *send_buffer
+			)
+{
+	int err;
+	struct sk_buff *skb;
+	struct cg2900_user_data *pf_data;
+
+	FM_INFO_REPORT("fmd_send_packet");
+
+	if (send_buffer == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (!cg2900_fm_dev) {
+		FM_ERR_REPORT("fmd_send_packet: No FM device registered");
+		err = -EIO;
+		goto error;
+	}
+
+	pf_data = dev_get_platdata(cg2900_fm_dev);
+	if (!pf_data->opened) {
+		FM_ERR_REPORT("fmd_send_packet: FM channel is not opened");
+		err = -EIO;
+		goto error;
+	}
+
+	mutex_lock(&write_mutex);
+	CG2900_HEX_WRITE_PACKET_DUMP;
+
+	skb = pf_data->alloc_skb(num_bytes, GFP_KERNEL);
+	if (!skb) {
+		FM_ERR_REPORT("fmd_send_packet:Couldn't " \
+			"allocate sk_buff with length %d", num_bytes);
+		err = -EIO;
+		goto error;
+	}
+
+	/*
+	 * Copy the buffer removing the FM Header as this
+	 * would be done by Protocol Driver
+	 */
+	memcpy(skb_put(skb, num_bytes), send_buffer, num_bytes);
+
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		FM_ERR_REPORT("fmd_send_packet: "
+		"Failed to send(%d) bytes using "
+		"cg2900_write, err = %d",
+		num_bytes, err);
+		kfree(skb);
+		err = -EIO;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	mutex_unlock(&write_mutex);
+	FM_DEBUG_REPORT("fmd_send_packet returning %d", err);
+	return err;
+}
+
+/**
+ * fmd_get_cmd_sem() - Block on Command Semaphore.
+ *
+ * This is required to ensure Flow Control in FM Driver.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ETIME if timeout occurs.
+ */
+static int fmd_get_cmd_sem(void)
+{
+	int ret_val;
+
+	FM_INFO_REPORT("fmd_get_cmd_sem");
+
+	ret_val = down_timeout(&cmd_sem,
+		msecs_to_jiffies(MAX_RESPONSE_TIME_IN_MS));
+
+	if (ret_val)
+		FM_ERR_REPORT("fmd_get_cmd_sem: down_timeout "
+			      "returned error = %d", ret_val);
+
+	return ret_val;
+}
+
+/**
+ * fmd_set_cmd_sem() - Unblock on Command Semaphore.
+ *
+ * This is required to ensure Flow Control in FM Driver.
+ */
+static void fmd_set_cmd_sem(void)
+{
+	FM_DEBUG_REPORT("fmd_set_cmd_sem");
+
+	up(&cmd_sem);
+}
+
+/**
+ * fmd_get_interrupt_sem() - Block on Interrupt Semaphore.
+ *
+ * Till Interrupt is received, Interrupt Task is blocked.
+ */
+static void fmd_get_interrupt_sem(void)
+{
+	int ret_val;
+
+	FM_DEBUG_REPORT("fmd_get_interrupt_sem");
+
+	ret_val = down_killable(&interrupt_sem);
+
+	if (ret_val)
+		FM_ERR_REPORT("fmd_get_interrupt_sem: down_killable "
+			"returned error = %d", ret_val);
+}
+
+/**
+ * fmd_set_interrupt_sem() - Unblock on Interrupt Semaphore.
+ *
+ * on receiving  Interrupt, Interrupt Task is un-blocked.
+ */
+static void fmd_set_interrupt_sem(void)
+{
+	FM_DEBUG_REPORT("fmd_set_interrupt_sem");
+	up(&interrupt_sem);
+}
+
+/**
+ * fmd_driver_init()- Initializes the Mutex, Semaphore, etc for FM Driver.
+ *
+ * It also registers FM Driver with the Protocol Driver.
+ *
+ * Returns:
+ *   true if initialization is successful
+ *   false if initiialization fails.
+ */
+static bool fmd_driver_init(void)
+{
+	bool ret_val;
+	struct cg2900_rev_data rev_data;
+	struct cg2900_user_data *pf_data;
+	int err;
+
+	FM_INFO_REPORT("fmd_driver_init");
+
+	if (!cg2900_fm_dev) {
+		FM_ERR_REPORT("No device registered");
+		ret_val = false;
+		goto error;
+	}
+
+	/* Initialize the semaphores */
+	sema_init(&cmd_sem, 0);
+	sema_init(&rds_sem, 0);
+	sema_init(&interrupt_sem, 0);
+	cb_rds_func = NULL;
+	rds_thread_required = false;
+	irq_thread_required = true;
+
+	pf_data = dev_get_platdata(cg2900_fm_dev);
+
+	/* Create Mutex For Reading and Writing */
+	spin_lock_init(&fmd_spinlock_read);
+	mutex_init(&write_mutex);
+	mutex_init(&send_cmd_mutex);
+	spin_lock_init(&fmd_spinlock);
+	fmd_start_irq_thread();
+
+	/* Open the FM channel */
+	err = pf_data->open(pf_data);
+	if (err) {
+		FM_ERR_REPORT("fmd_driver_init: "
+			"Couldn't open FM channel. Either chip is not connected"
+			" or Protocol Driver is not initialized");
+		ret_val = false;
+		goto error;
+	}
+
+	if (!pf_data->get_local_revision(pf_data, &rev_data)) {
+		FM_DEBUG_REPORT("No revision data available");
+		ret_val = false;
+		goto error;
+	}
+
+	FM_DEBUG_REPORT("Read revision data revision %04x "
+		   "sub_version %04x",
+		   rev_data.revision, rev_data.sub_version);
+	cg2900_fm_set_chip_version(rev_data.revision, rev_data.sub_version);
+	ret_val = true;
+
+error:
+	FM_DEBUG_REPORT("fmd_driver_init: Returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * fmd_driver_exit() - Deinitializes the mutex, semaphores, etc.
+ *
+ * It also deregisters FM Driver with the Protocol Driver.
+ *
+ */
+static void fmd_driver_exit(void)
+{
+	struct cg2900_user_data *pf_data;
+
+	FM_INFO_REPORT("fmd_driver_exit");
+	irq_thread_required = false;
+	mutex_destroy(&write_mutex);
+	mutex_destroy(&send_cmd_mutex);
+	fmd_stop_irq_thread();
+	/* Close the FM channel */
+	pf_data = dev_get_platdata(cg2900_fm_dev);
+	if (pf_data->opened)
+		pf_data->close(pf_data);
+}
+
+/**
+ * fmd_send_cmd_and_read_resp() - Send command and read response.
+ *
+ * This function sends the HCI Command to Protocol Driver and
+ * Reads back the Response Packet.
+ * @cmd_id: Command Id to be sent to FM Chip.
+ * @num_parameters: Number of parameters of the command sent.
+ * @parameters: Buffer containing the Buffer to be sent.
+ * @resp_num_parameters: (out) Number of paramters of the response packet.
+ * @resp_parameters: (out) Buffer of the response packet.
+ *
+ * Returns:
+ *   0: If the command is sent successfully and the
+ *   response received is also correct.
+ *   -EINVAL: If the received response is not correct.
+ *   -EIO: If there is an input/output error.
+ *   -EINVAL: If parameters are not valid.
+ */
+static int fmd_send_cmd_and_read_resp(
+			const u16 cmd_id,
+			const u16 num_parameters,
+			const u16 *parameters,
+			u16 *resp_num_parameters,
+			u16 *resp_parameters
+			)
+{
+	int result;
+	u16 read_cmd_id = CMD_ID_NONE;
+
+	FM_INFO_REPORT("fmd_send_cmd_and_read_resp");
+
+	mutex_lock(&send_cmd_mutex);
+	result = fmd_send_cmd(
+			cmd_id,
+			num_parameters,
+			parameters);
+
+	if (result != 0)
+		goto error;
+
+	result = fmd_read_resp(
+			&read_cmd_id,
+			resp_num_parameters,
+			resp_parameters);
+
+	if (result != 0)
+		goto error;
+
+	/*
+	 * Check that the response belongs to the sent command
+	 */
+	if (read_cmd_id != cmd_id)
+		result = -EINVAL;
+
+error:
+	mutex_unlock(&send_cmd_mutex);
+	FM_DEBUG_REPORT("fmd_send_cmd_and_read_resp: "
+		"returning %d", result);
+	return result;
+}
+
+/**
+ * fmd_send_cmd() - This function sends the HCI Command
+ * to Protocol Driver.
+ *
+ * @cmd_id: Command Id to be sent to FM Chip.
+ * @num_parameters: Number of parameters of the command sent.
+ * @parameters: Buffer containing the Buffer to be sent.
+ *
+ * Returns:
+ *   0: If the command is sent successfully to Lower Layers.
+ *   -EIO: If there is an input/output error.
+ *   -EINVAL: If parameters are not valid.
+ */
+static int fmd_send_cmd(
+			const u16 cmd_id ,
+			const u16 num_parameters,
+			const u16 *parameters
+			)
+{
+	/*
+	 * Total Length includes 6 bytes HCI Header
+	 * and remaining bytes depending on number of paramters.
+	 */
+	u16 total_length = num_parameters * sizeof(u16) + FM_HCI_CMD_HEADER_LEN;
+	/*
+	 * Parameter Length includes 5 bytes HCI Header
+	 * and remaining bytes depending on number of paramters.
+	 */
+	u16 param_length = num_parameters * sizeof(u16) + FM_HCI_CMD_PARAM_LEN;
+	u8 *fm_data = kmalloc(total_length, GFP_KERNEL);
+	int err = -EINVAL;
+
+	FM_INFO_REPORT("fmd_send_cmd");
+
+	if (fm_data == NULL) {
+		err = -EIO;
+		goto error;
+	}
+
+	if (num_parameters && parameters == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	/* HCI encapsulation */
+	fm_data[0] = param_length;
+	fm_data[1] = FM_CATENA_OPCODE;
+	fm_data[2] = FM_WRITE;
+	fm_data[3] = FM_FUNCTION_WRITE_COMMAND;
+	fm_data[4] = FM_CMD_GET_LSB(cmd_id, num_parameters);
+	fm_data[5] = FM_CMD_GET_MSB(cmd_id);
+
+	memcpy(
+			(fm_data + FM_HCI_CMD_HEADER_LEN),
+			(void *)parameters,
+			num_parameters * sizeof(u16));
+
+	/* Send the Packet */
+	err = fmd_send_packet(total_length , fm_data);
+
+error:
+	kfree(fm_data);
+	FM_DEBUG_REPORT("fmd_send_cmd: "
+		"returning %d", err);
+	return err;
+}
+
+/**
+ * fmd_read_resp() - This function reads the response packet of the previous
+ * command sent to FM Chip and copies it to the buffer provided as parameter.
+ *
+ * @cmd_id: (out) Command Id received from FM Chip.
+ * @num_parameters: (out) Number of paramters of the response packet.
+ * @parameters: (out) Buffer of the response packet.
+ *
+ * Returns:
+ *   0: If the response buffer is copied successfully.
+ *   -EINVAL: If parameters are not valid.
+ *   -ETIME: Otherwise
+ */
+static int fmd_read_resp(
+			u16 *cmd_id,
+			u16 *num_parameters,
+			u16 *parameters
+			)
+{
+	int err;
+	FM_INFO_REPORT("fmd_read_resp");
+
+	/* Wait till response of the command is received */
+	if (fmd_get_cmd_sem()) {
+		err = -ETIME;
+		goto error;
+	}
+
+	/* Check if the parameters are valid */
+	if (cmd_id == NULL || (fmd_data.num_parameters &&
+		(num_parameters == NULL || parameters == NULL))) {
+		err =  -EINVAL;
+		goto error;
+	}
+
+	/* Fill the arguments */
+	*cmd_id = fmd_data.cmd_id;
+	if (fmd_data.num_parameters) {
+		*num_parameters = fmd_data.num_parameters;
+		memcpy(
+			parameters,
+			fmd_data.parameters,
+			(*num_parameters * sizeof(u16)));
+	}
+
+	err = 0;
+
+error:
+	FM_DEBUG_REPORT("fmd_read_resp: "
+		"returning %d", err);
+	return err;
+}
+
+/**
+ * fmd_process_fm_function() - Process FM Function.
+ *
+ * This function processes the Response buffer received
+ * from lower layers for the FM function and performs the necessary action to
+ * parse the same.
+ * @packet_buffer: Received Buffer.
+ */
+static void fmd_process_fm_function(
+			u8 *packet_buffer
+			)
+{
+	u8 fm_function_id;
+	u8 block_id;
+
+	if (packet_buffer == NULL)
+		return;
+
+	fm_function_id = FM_GET_FUNCTION_ID(packet_buffer);
+	switch (fm_function_id) {
+	case FM_FUNCTION_ENABLE:
+	case FM_FUNCTION_DISABLE:
+	case FM_FUNCTION_RESET:
+		FM_DEBUG_REPORT(
+			"fmd_process_fm_function: "
+			"command success received for %s",
+			fmd_get_fm_function_name(fm_function_id));
+		/* Release the semaphore since response is received */
+		fmd_set_cmd_sem();
+		break;
+	case FM_FUNCTION_WRITE_COMMAND:
+		FM_DEBUG_REPORT(
+			"fmd_process_fm_function: "
+			"command success received for %s",
+			fmd_get_fm_function_name(fm_function_id));
+
+		fmd_data.cmd_id = FM_GET_CMD_ID(packet_buffer);
+		fmd_data.num_parameters =
+			FM_GET_NUM_PARAMS(packet_buffer);
+
+		FM_DEBUG_REPORT(
+			"fmd_process_fm_function: "
+			"Cmd Id = 0x%04x, Num Of Parms = %02x",
+			fmd_data.cmd_id, fmd_data.num_parameters);
+
+		if (fmd_data.num_parameters) {
+			fmd_data.parameters =
+				FM_GET_RSP_BUFFER_ADDR(packet_buffer);
+			memcpy(fmd_data.parameters,
+				FM_GET_RSP_BUFFER_ADDR(packet_buffer),
+				fmd_data.num_parameters * sizeof(u16));
+		}
+		/* Release the semaphore since response is received */
+		fmd_set_cmd_sem();
+		break;
+	case FM_FUNCTION_FIRMWARE_DOWNLOAD:
+		block_id = FM_GET_BLOCK_ID(packet_buffer);
+		FM_DEBUG_REPORT(
+			"fmd_process_fm_function: "
+			"command success received for %s"
+			"block id = %02x",
+			fmd_get_fm_function_name(fm_function_id),
+			block_id);
+		/* Release the semaphore since response is received */
+		fmd_set_cmd_sem();
+		break;
+	default:
+		FM_ERR_REPORT(
+			"fmd_process_fm_function: "
+			"default case: command success received for %s",
+			fmd_get_fm_function_name(fm_function_id));
+		break;
+	}
+}
+
+/**
+ * fmd_write_file_block() - download firmware.
+ *
+ * This Function adds the header for downloading
+ * the firmware and coeffecient files and sends it to Protocol Driver.
+ * @file_block_id: Block ID of the F/W to be transmitted to FM Chip
+ * @file_block: Buffer containing the bytes to be sent.
+ * @file_block_length: Size of the Firmware buffer.
+ *
+ * Returns:
+ *   0: If there is no error.
+ *   -EINVAL: If parameters are not valid.
+ *   -ETIME: Otherwise
+ */
+static int fmd_write_file_block(
+			u32 file_block_id,
+			u8 *file_block,
+			u16 file_block_length
+			)
+{
+	int err;
+
+	FM_INFO_REPORT("fmd_write_file_block");
+	if (file_block == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	mutex_lock(&send_cmd_mutex);
+	file_block[0] = file_block_length + FM_HCI_WRITE_FILE_BLK_PARAM_LEN;
+	file_block[1] = FM_CATENA_OPCODE;
+	file_block[2] = FM_WRITE;
+	file_block[3] = FM_FUNCTION_FIRMWARE_DOWNLOAD;
+	file_block[4] = file_block_id;
+	/* Send the Packet */
+	err = fmd_send_packet(
+				file_block_length +
+				FM_HCI_WRITE_FILE_BLK_HEADER_LEN,
+				file_block);
+
+	/* wait till response comes */
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+
+error:
+	mutex_unlock(&send_cmd_mutex);
+	FM_DEBUG_REPORT("fmd_write_file_block: "
+		"returning %d", err);
+	return err;
+}
+
+int fmd_init(void)
+{
+	int err;
+
+	if (!fmd_driver_init()) {
+		err = -EIO;
+		goto error;
+	}
+
+	memset(&fmd_state_info, 0, sizeof(fmd_state_info));
+	fmd_state_info.fmd_initialized = true;
+	fmd_state_info.gocmd = FMD_STATE_NONE;
+	fmd_state_info.callback = NULL;
+	fmd_state_info.rx_freq_range = FMD_FREQRANGE_EUROAMERICA;
+	fmd_state_info.rx_stereo_mode = FMD_STEREOMODE_BLENDING;
+	fmd_state_info.rx_volume = MAX_ANALOG_VOLUME;
+	fmd_state_info.rx_antenna = FMD_ANTENNA_EMBEDDED;
+	fmd_state_info.rx_rds_on = false;
+	fmd_state_info.rx_seek_stop_level = DEFAULT_RSSI_THRESHOLD;
+	fmd_state_info.tx_freq_range = FMD_FREQRANGE_EUROAMERICA;
+	fmd_state_info.tx_preemphasis = FMD_EMPHASIS_75US;
+	fmd_state_info.tx_pilot_dev = DEFAULT_PILOT_DEVIATION;
+	fmd_state_info.tx_rds_dev = DEFAULT_RDS_DEVIATION;
+	fmd_state_info.tx_strength = MAX_POWER_LEVEL;
+	fmd_state_info.max_channels_to_scan = DEFAULT_CHANNELS_TO_SCAN;
+	fmd_state_info.tx_stereo_mode = true;
+	fmd_state_info.irq_index = 0;
+	spin_lock_init(&fmd_spinlock);
+	err = 0;
+
+error:
+	FM_DEBUG_REPORT("fmd_init returning = %d", err);
+	return err;
+}
+
+void fmd_exit(void)
+{
+	fmd_set_interrupt_sem();
+	fmd_driver_exit();
+	memset(&fmd_state_info, 0, sizeof(fmd_state_info));
+}
+
+int fmd_register_callback(
+			fmd_radio_cb callback
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	fmd_state_info.callback = callback;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_get_version(
+			u16 *version
+			)
+{
+	int err;
+	int io_result;
+	u16  response_count;
+	u16  response_data[CMD_GET_VERSION_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (version == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_GET_VERSION,
+			CMD_GET_VERSION_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	memcpy(version,
+		response_data,
+		sizeof(u16) * CMD_GET_VERSION_RSP_PARAM_LEN);
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_mode(
+			u8 mode
+			)
+{
+	int err;
+	u16 parameters[CMD_GOTO_MODE_PARAM_LEN];
+    int io_result;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (mode > FMD_MODE_TX) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = mode;
+
+	fmd_state_info.gocmd = FMD_STATE_MODE;
+	FM_ERR_REPORT("Sending Set Mode");
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_GOTO_MODE,
+			CMD_GOTO_MODE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_get_freq_range_properties(
+			u8 range,
+			u32 *min_freq,
+			u32 *max_freq
+			)
+{
+	int err;
+
+	if (min_freq == NULL || max_freq == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	switch (range) {
+	case FMD_FREQRANGE_EUROAMERICA:
+		*min_freq =  FMD_EU_US_MIN_FREQ_IN_KHZ;
+		*max_freq = FMD_EU_US_MAX_FREQ_IN_KHZ;
+		break;
+	case FMD_FREQRANGE_JAPAN:
+		*min_freq = FMD_JAPAN_MIN_FREQ_IN_KHZ;
+		*max_freq = FMD_JAPAN_MAX_FREQ_IN_KHZ;
+		break;
+	case FMD_FREQRANGE_CHINA:
+		*min_freq =  FMD_CHINA_MIN_FREQ_IN_KHZ;
+		*max_freq = FMD_CHINA_MAX_FREQ_IN_KHZ;
+		break;
+	default:
+		*min_freq =  FMD_EU_US_MIN_FREQ_IN_KHZ;
+		*max_freq = FMD_EU_US_MAX_FREQ_IN_KHZ;
+		break;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_antenna(
+			u8 antenna
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SET_ANTENNA_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (antenna > FMD_ANTENNA_WIRED) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = antenna;
+
+	fmd_state_info.gocmd = FMD_STATE_ANTENNA;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SET_ANTENNA,
+			CMD_SET_ANTENNA_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.rx_antenna = antenna;
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_get_antenna(
+			u8 *antenna
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	*antenna = fmd_state_info.rx_antenna;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_freq_range(
+			u8 range
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TN_SET_BAND_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	parameters[0] = range;
+	parameters[1] = FMD_MIN_CHANNEL_NUMBER;
+	parameters[2] = FMD_MAX_CHANNEL_NUMBER;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_TN_SET_BAND,
+			CMD_TN_SET_BAND_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+	fmd_state_info.rx_freq_range = range;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_get_freq_range(
+			u8 *range
+			)
+{
+	int err;
+
+	if (range == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	*range = fmd_state_info.rx_freq_range;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_grid(
+			u8 grid
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TN_SET_GRID_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (grid > FMD_GRID_200KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = grid;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_TN_SET_GRID,
+			CMD_TN_SET_GRID_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_frequency(
+			u32 freq
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_TUNE_SET_CHANNEL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (freq > FMD_EU_US_MAX_FREQ_IN_KHZ ||
+		freq < FMD_CHINA_MIN_FREQ_IN_KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_rx_frequency_to_channel(
+			freq,
+			&parameters[0]);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.gocmd = FMD_STATE_FREQUENCY;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_TUNE_SET_CHANNEL,
+			CMD_SP_TUNE_SET_CHANNEL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_frequency(
+			u32 *freq
+			)
+{
+	int err;
+	int io_result;
+	u16 response_count;
+	u16 response_data[CMD_SP_TUNE_GET_CHANNEL_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (freq == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+				CMD_FMR_SP_TUNE_GET_CHANNEL,
+				CMD_SP_TUNE_GET_CHANNEL_PARAM_LEN,
+				NULL,
+				&response_count,
+				response_data);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	io_result = fmd_rx_channel_to_frequency(
+			response_data[0], /* 1st byte is the Frequency */
+			freq);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_stereo_mode(
+			u8 mode
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_RP_STEREO_SET_MODE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (mode > FMD_STEREOMODE_BLENDING) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = mode;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_RP_STEREO_SET_MODE,
+			CMD_RP_STEREO_SET_MODE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.rx_stereo_mode = mode;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_stereo_mode(
+			u8 *mode
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (mode == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*mode = fmd_state_info.rx_stereo_mode;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_signal_strength(
+			u16 *strength
+			)
+{
+	int err;
+	int io_result;
+	u16 response_count;
+	u16 response_data[CMD_RP_GET_RSSI_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (strength == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_RP_GET_RSSI,
+			CMD_RP_GET_RSSI_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	*strength = response_data[0]; /* 1st byte is the signal strength */
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_stop_level(
+			u16 stoplevel
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	fmd_state_info.rx_seek_stop_level = stoplevel;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_stop_level(
+			u16 *stop_level
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (stop_level == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*stop_level = fmd_state_info.rx_seek_stop_level;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_seek(
+			bool upwards
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_SEARCH_START_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (upwards)
+		parameters[0] = 0x0000;
+	else
+		parameters[0] = 0x0001;
+	parameters[1] = fmd_state_info.rx_seek_stop_level;
+	parameters[2] = DEFAULT_PEAK_NOISE_VALUE;
+	parameters[3] = DEFAULT_AVERAGE_NOISE_MAX_VALUE;
+	fmd_state_info.gocmd = FMD_STATE_SEEK;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_SEARCH_START,
+			CMD_SP_SEARCH_START_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_scan_band(
+			u8 max_channels_to_scan
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_SCAN_START_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (max_channels_to_scan > MAX_CHANNELS_TO_SCAN) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = max_channels_to_scan;
+	parameters[1] = fmd_state_info.rx_seek_stop_level;
+	parameters[2] = DEFAULT_PEAK_NOISE_VALUE;
+	parameters[3] = DEFAULT_AVERAGE_NOISE_MAX_VALUE;
+
+	fmd_state_info.gocmd = FMD_STATE_SCAN_BAND;
+	fmd_state_info.max_channels_to_scan = max_channels_to_scan;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_SCAN_START,
+			CMD_SP_SCAN_START_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_max_channels_to_scan(
+			u8 *max_channels_to_scan
+			)
+{
+	int err;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (max_channels_to_scan == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*max_channels_to_scan = fmd_state_info.max_channels_to_scan;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_scan_band_info(
+			u32 index,
+			u16 *num_channels,
+			u16 *channels,
+			u16 *rssi
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_SCAN_GET_RESULT_PARAM_LEN];
+	u16 response_count;
+	u16 response_data[CMD_SP_SCAN_GET_RESULT_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (num_channels == NULL || rssi == NULL || channels == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = index;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_SCAN_GET_RESULT,
+			CMD_SP_SCAN_GET_RESULT_PARAM_LEN,
+			parameters,
+			&response_count,
+			response_data);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	/* 1st byte indicates number of channels found */
+	*num_channels = response_data[0];
+	/* 2nd byte indicates 1st channel number */
+	channels[0] = response_data[1];
+	/* 3rd byte indicates RSSI of corresponding channel */
+	rssi[0] = response_data[2];
+	/* 4th byte indicates 2nd channel number */
+	channels[1] = response_data[3];
+	/* 5th byte indicates RSSI of corresponding channel */
+	rssi[1] = response_data[4];
+	/* 6th byte indicates 3rd channel number */
+	channels[2] = response_data[5];
+	/* 7th byte indicates RSSI of corresponding channel */
+	rssi[2] = response_data[6];
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_block_scan(
+			u32 start_freq,
+			u32 stop_freq,
+			u8 antenna
+			)
+{
+	u16 start_channel;
+	u16 stop_channel;
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_BLOCK_SCAN_START_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (antenna > FMD_ANTENNA_WIRED) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (start_freq > FMD_EU_US_MAX_FREQ_IN_KHZ ||
+		start_freq < FMD_CHINA_MIN_FREQ_IN_KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (stop_freq > FMD_EU_US_MAX_FREQ_IN_KHZ ||
+		stop_freq < FMD_CHINA_MIN_FREQ_IN_KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	/* Convert the start frequency to corresponsing channel */
+	io_result = fmd_rx_frequency_to_channel(
+				start_freq,
+				&start_channel);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	/* Convert the end frequency to corresponsing channel */
+	io_result = fmd_rx_frequency_to_channel(
+				stop_freq,
+				&stop_channel);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	parameters[0] = start_channel;
+	parameters[1] = stop_channel;
+	parameters[2] = antenna;
+
+	fmd_state_info.gocmd = FMD_STATE_BLOCK_SCAN;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_BLOCK_SCAN_START,
+			CMD_SP_BLOCK_SCAN_START_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_block_scan_result(
+			u32 index,
+			u16 *num_channels,
+			u16 *rssi
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_BLOCK_SCAN_GET_RESULT_PARAM_LEN];
+	u16 response_count;
+	u16 response_data[CMD_SP_BLOCK_SCAN_GET_RESULT_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (num_channels == NULL || rssi == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = index;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_BLOCK_SCAN_GET_RESULT,
+			CMD_SP_BLOCK_SCAN_GET_RESULT_PARAM_LEN,
+			parameters,
+			&response_count,
+			response_data);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	/*
+	 * Response packet has 1st byte as the number
+	 * of channels, and the remaining 6 bytes as
+	 * rssi values of the channels.
+	 */
+	*num_channels = response_data[0];
+	rssi[0] = response_data[1];
+	rssi[1] = response_data[2];
+	rssi[2] = response_data[3];
+	rssi[3] = response_data[4];
+	rssi[4] = response_data[5];
+	rssi[5] = response_data[6];
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_stop_seeking(void)
+{
+	int err;
+	int io_result;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (!(fmd_state_info.gocmd == FMD_STATE_SEEK ||
+		fmd_state_info.gocmd == FMD_STATE_SCAN_BAND ||
+		fmd_state_info.gocmd == FMD_STATE_BLOCK_SCAN)) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	fmd_state_info.gocmd = FMD_STATE_SEEK_STOP;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_STOP,
+			CMD_SP_STOP_PARAM_LEN,
+			NULL,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_af_update_start(
+			u32 freq
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_AF_UPDATE_START_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	io_result = fmd_rx_frequency_to_channel(
+			freq,
+			&parameters[0]);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.gocmd = FMD_STATE_AF_UPDATE;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_AF_UPDATE_START,
+			CMD_SP_AF_UPDATE_START_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_af_update_result(
+			u16 *af_level
+			)
+{
+	int err;
+	int io_result;
+	u16 response_count;
+	u16 response_data[CMD_SP_AF_UPDATE_GET_RESULT_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (af_level == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_AF_UPDATE_GET_RESULT,
+			CMD_SP_AF_UPDATE_GET_RESULT_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+
+	if (io_result != 0)	{
+		err = io_result;
+		goto error;
+	}
+
+	/*
+	 * 1st byte of response packet is the
+	 * RSSI of the AF Frequency.
+	 */
+	*af_level = response_data[0];
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_af_switch_start(
+			u32 freq,
+			u16 picode
+			)
+{
+
+	int err;
+	int io_result;
+	u16  parameters[CMD_SP_AF_SWITCH_START_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	io_result = fmd_rx_frequency_to_channel(
+			freq,
+			&parameters[0]);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	parameters[1] = picode;
+	parameters[2] = 0xFFFF; /* PI Mask */
+	parameters[3] = fmd_state_info.rx_seek_stop_level;
+	parameters[4] = 0x0000; /* Unmute when AF's PI matches expected PI */
+
+	fmd_state_info.gocmd = FMD_STATE_AF_SWITCH;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_AF_SWITCH_START,
+			CMD_SP_AF_SWITCH_START_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_af_switch_results(
+			u16 *afs_conclusion,
+			u16 *afs_level,
+			u16 *afs_pi
+			)
+{
+	int err;
+	int io_result;
+	u16 response_count;
+	u16 response_data[CMD_SP_AF_SWITCH_GET_RESULT_RWSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (afs_conclusion == NULL ||
+		afs_level == NULL ||
+		afs_pi == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_AF_SWITCH_GET_RESULT,
+			CMD_SP_AF_SWITCH_GET_RESULT_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+
+	if (io_result != 0)	{
+		err = io_result;
+		goto error;
+	}
+
+	*afs_conclusion = response_data[0];
+	*afs_level = response_data[1];
+	*afs_pi = response_data[2];
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_rds(
+			bool *on
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (on == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*on = fmd_state_info.rx_rds_on;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_buffer_set_size(
+			u8 size
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_DP_BUFFER_SET_SIZE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (size > MAX_RDS_GROUPS) {
+		err = -EIO;
+		goto error;
+	}
+
+	parameters[0] = size;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_DP_BUFFER_SET_SIZE,
+			CMD_DP_BUFFER_SET_SIZE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_buffer_set_threshold(
+			u8 threshold
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_DP_BUFFER_SET_THRESHOLD_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (threshold > MAX_RDS_GROUPS) {
+		err = -EIO;
+		goto error;
+	}
+
+	parameters[0] = threshold;
+
+	io_result = fmd_send_cmd_and_read_resp(
+		CMD_FMR_DP_BUFFER_SET_THRESHOLD,
+		CMD_DP_BUFFER_SET_THRESHOLD_PARAM_LEN,
+		parameters,
+		NULL,
+		NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_rds(
+			u8 on_off_state
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_DP_SET_CONTROL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	switch (on_off_state) {
+	case FMD_SWITCH_ON_RDS_SIMULATOR:
+		parameters[0] = 0xFFFF;
+		break;
+	case FMD_SWITCH_OFF_RDS:
+	default:
+		parameters[0] = 0x0000;
+		fmd_state_info.rx_rds_on = false;
+		break;
+	case FMD_SWITCH_ON_RDS:
+		parameters[0] = 0x0001;
+		fmd_state_info.rx_rds_on = true;
+		break;
+	case FMD_SWITCH_ON_RDS_ENHANCED_MODE:
+		parameters[0] = 0x0002;
+		fmd_state_info.rx_rds_on = true;
+		break;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_DP_SET_CONTROL,
+			CMD_DP_SET_CONTROL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_low_level_rds_groups(
+			u8 index,
+			u16 *block1,
+			u16 *block2,
+			u16 *block3,
+			u16 *block4,
+			u8 *status1,
+			u8 *status2,
+			u8 *status3,
+			u8 *status4
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (block1 == NULL ||
+		block2 == NULL ||
+		block3 == NULL ||
+		block4 == NULL ||
+		status1 == NULL ||
+		status2 == NULL ||
+		status3 == NULL ||
+		status4 == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*block1  = fmd_state_info.rds_group[index].block[0];
+	*block2  = fmd_state_info.rds_group[index].block[1];
+	*block3  = fmd_state_info.rds_group[index].block[2];
+	*block4  = fmd_state_info.rds_group[index].block[3];
+	*status1 = fmd_state_info.rds_group[index].status[0];
+	*status2 = fmd_state_info.rds_group[index].status[1];
+	*status3 = fmd_state_info.rds_group[index].status[2];
+	*status4 = fmd_state_info.rds_group[index].status[3];
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_deemphasis(
+			u8 deemphasis
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_RP_SET_DEEMPHASIS_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	switch (deemphasis)	{
+	case FMD_EMPHASIS_50US:
+		parameters[0] = FMD_EMPHASIS_50US;
+		break;
+
+	case FMD_EMPHASIS_75US:
+		parameters[0] = FMD_EMPHASIS_75US;
+		break;
+
+	case FMD_EMPHASIS_NONE:
+	default:
+		parameters[0] = FMD_EMPHASIS_NONE;
+		break;
+
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_RP_SET_DEEMPHASIS,
+			CMD_RP_SET_DEEMPHASIS_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_pa(
+			bool on
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_PA_SET_MODE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (on)
+		parameters[0] = 0x0001;
+	else
+		parameters[0] = 0x0000;
+
+	fmd_state_info.gocmd = FMD_STATE_PA;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_PA_SET_MODE,
+			CMD_PA_SET_MODE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_signal_strength(
+			u16 strength
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_PA_SET_CONTROL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if ((strength > MAX_POWER_LEVEL)
+		|| (strength < MIN_POWER_LEVEL)) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = strength;
+
+	fmd_state_info.gocmd = FMD_STATE_PA_LEVEL;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_PA_SET_CONTROL,
+			CMD_PA_SET_CONTROL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_strength = strength;
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_signal_strength(
+			u16 *strength
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (strength == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*strength = fmd_state_info.tx_strength;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_freq_range(
+			u8 range
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TN_SET_BAND_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (range > FMD_FREQRANGE_CHINA) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = range;
+	parameters[1] = FMD_MIN_CHANNEL_NUMBER;
+	parameters[2] = FMD_MAX_CHANNEL_NUMBER;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_TN_SET_BAND,
+			CMD_TN_SET_BAND_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_freq_range = range;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_freq_range(
+			u8 *range
+			)
+{
+	int err;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (range == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*range = fmd_state_info.tx_freq_range;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_grid(
+			u8 grid
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TN_SET_GRID_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (grid > FMD_GRID_200KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = grid;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_TN_SET_GRID,
+			CMD_TN_SET_GRID_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_preemphasis(
+			u8 preemphasis
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_RP_SET_PREEMPHASIS_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	switch (preemphasis)	{
+	case FMD_EMPHASIS_50US:
+		parameters[0] = FMD_EMPHASIS_50US;
+		break;
+	case FMD_EMPHASIS_75US:
+	default:
+		parameters[0] = FMD_EMPHASIS_75US;
+		break;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_RP_SET_PREEMPHASIS,
+			CMD_RP_SET_PREEMPHASIS_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_preemphasis = preemphasis;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_preemphasis(
+			u8 *preemphasis
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (preemphasis == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*preemphasis = fmd_state_info.tx_preemphasis;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_frequency(
+			u32 freq
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_SP_TUNE_SET_CHANNEL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (freq > FMD_EU_US_MAX_FREQ_IN_KHZ ||
+		freq < FMD_CHINA_MIN_FREQ_IN_KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_tx_frequency_to_channel(
+			freq,
+			&parameters[0]);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.gocmd = FMD_STATE_FREQUENCY;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_SP_TUNE_SET_CHANNEL,
+			CMD_SP_TUNE_SET_CHANNEL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_frequency(
+			u32 *freq
+			)
+{
+	int err;
+	int io_result;
+	u16 response_count;
+	u16  response_data[CMD_SP_TUNE_GET_CHANNEL_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (freq == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_SP_TUNE_GET_CHANNEL,
+			CMD_SP_TUNE_GET_CHANNEL_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	io_result = fmd_tx_channel_to_frequency(
+			response_data[0], /* 1st byte is the Frequency */
+			freq);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_enable_stereo_mode(
+			bool enable_stereo_mode
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_RP_STEREO_SET_MODE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	parameters[0] = enable_stereo_mode;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_RP_STEREO_SET_MODE,
+			CMD_RP_STEREO_SET_MODE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_stereo_mode = enable_stereo_mode;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_stereo_mode(
+			bool *stereo_mode
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (stereo_mode == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*stereo_mode = fmd_state_info.tx_stereo_mode;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_pilot_deviation(
+			u16 deviation
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_RP_SET_PILOT_DEVIATION_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (deviation > MAX_PILOT_DEVIATION) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = deviation;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_RP_SET_PILOT_DEVIATION,
+			CMD_RP_SET_PILOT_DEVIATION_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_pilot_dev = deviation;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_pilot_deviation(
+			u16 *deviation
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (deviation == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*deviation = fmd_state_info.tx_pilot_dev;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_rds_deviation(
+			u16 deviation
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_RP_SET_RDS_DEVIATION_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (deviation > MAX_RDS_DEVIATION) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = deviation;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_RP_SET_RDS_DEVIATION,
+			CMD_RP_SET_RDS_DEVIATION_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_rds_dev = deviation;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_rds_deviation(
+			u16 *deviation
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (deviation == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*deviation = fmd_state_info.tx_rds_dev;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_rds(
+			bool on
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_DP_SET_CONTROL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (on)
+		parameters[0] = 0x0001;
+	else
+		parameters[0] = 0x0000;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_DP_SET_CONTROL,
+			CMD_DP_SET_CONTROL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_rds_on = on;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_group(
+			u16 position,
+			u8  *block1,
+			u8  *block2,
+			u8  *block3,
+			u8  *block4
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_DP_BUFFER_SET_GROUP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (block1 == NULL ||
+		block2 == NULL ||
+		block3 == NULL ||
+		block4 == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = position;
+	memcpy(&parameters[1], block1, sizeof(u16));
+	memcpy(&parameters[2], block2, sizeof(u16));
+	memcpy(&parameters[3], block3, sizeof(u16));
+	memcpy(&parameters[4], block4, sizeof(u16));
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_DP_BUFFER_SET_GROUP,
+			CMD_DP_BUFFER_SET_GROUP_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_buffer_set_size(
+			u16 buffer_size
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_DP_BUFFER_SET_SIZE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	parameters[0] = buffer_size;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_DP_BUFFER_SET_SIZE,
+			CMD_DP_BUFFER_SET_SIZE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+
+}
+
+int fmd_tx_get_rds(
+			bool *on
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (on == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*on = fmd_state_info.tx_rds_on;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_balance(
+			s8 balance
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SET_BALANCE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	/* Convert balance from percentage to chip number */
+	parameters[0] = (((s16)balance) * FMD_MAX_BALANCE) / 100;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_AUP_SET_BALANCE,
+			CMD_SET_BALANCE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_volume(
+			u8 volume
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SET_VOLUME_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	/* Convert volume from percentage to chip number */
+	parameters[0] = (((u16)volume) * FMD_MAX_VOLUME) / 100;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_AUP_SET_VOLUME,
+			CMD_SET_VOLUME_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.rx_volume = volume;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_get_volume(
+			u8 *volume
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (volume == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*volume = fmd_state_info.rx_volume;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_mute(
+			bool mute_on
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SET_MUTE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (!mute_on)
+		parameters[0] = 0x0000;
+	else
+		parameters[0] = 0x0001;
+	parameters[1] = 0x0001;
+
+	fmd_state_info.gocmd = FMD_STATE_MUTE;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_AUP_SET_MUTE,
+			CMD_SET_MUTE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_ext_set_mute(
+			bool mute_on
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_EXT_SET_MUTE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (!mute_on)
+		parameters[0] = 0x0000;
+	else
+		parameters[0] = 0x0001;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_AUP_EXT_SET_MUTE,
+			CMD_EXT_SET_MUTE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_power_up(void)
+{
+	int err;
+	int io_result;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	fmd_state_info.gocmd = FMD_STATE_GEN_POWERUP;
+	FM_ERR_REPORT("Sending Gen Power Up");
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_POWERUP,
+			CMD_POWERUP_PARAM_LEN,
+			NULL,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_goto_standby(void)
+{
+	int err;
+	int io_result;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_GOTO_STANDBY,
+			CMD_GOTO_STANDBY_PARAM_LEN,
+			NULL,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_goto_power_down(void)
+{
+	int err;
+	int io_result;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_GOTO_POWERDOWN,
+			CMD_GOTO_POWERDOWN_PARAM_LEN,
+			NULL,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_select_ref_clk(
+			u16 ref_clk
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SELECT_REFERENCE_CLOCK_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	parameters[0] = ref_clk;
+
+	fmd_state_info.gocmd = FMD_STATE_SELECT_REF_CLK;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_SELECT_REFERENCE_CLOCK,
+			CMD_SELECT_REFERENCE_CLOCK_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_ref_clk_pll(
+			u16 freq
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SET_REFERENCE_CLOCK_PLL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	parameters[0] = freq;
+
+	fmd_state_info.gocmd = FMD_STATE_SET_REF_CLK_PLL;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_SET_REFERENCE_CLOCK_PLL,
+			CMD_SET_REFERENCE_CLOCK_PLL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_send_fm_ip_enable(void)
+{
+	int err;
+	u8 fm_ip_enable_cmd[CMD_IP_ENABLE_CMD_LEN];
+
+	mutex_lock(&send_cmd_mutex);
+	fm_ip_enable_cmd[0] = CMD_IP_ENABLE_PARAM_LEN;
+	fm_ip_enable_cmd[1] = FM_CATENA_OPCODE;
+	fm_ip_enable_cmd[2] = FM_WRITE ;
+	fm_ip_enable_cmd[3] = FM_FUNCTION_ENABLE;
+
+	/* Send the Packet */
+	err = fmd_send_packet(
+				CMD_IP_ENABLE_CMD_LEN,
+				fm_ip_enable_cmd);
+
+	/* Check the ErrorCode */
+	if (err != 0)
+		goto error;
+
+	/* wait till response comes */
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+
+error:
+	mutex_unlock(&send_cmd_mutex);
+	return err;
+}
+
+int fmd_send_fm_ip_disable(void)
+{
+	int err;
+	u8 fm_ip_disable_cmd[CMD_IP_DISABLE_CMD_LEN];
+
+	mutex_lock(&send_cmd_mutex);
+	fm_ip_disable_cmd[0] = CMD_IP_DISABLE_PARAM_LEN;
+	fm_ip_disable_cmd[1] = FM_CATENA_OPCODE;
+	fm_ip_disable_cmd[2] = FM_WRITE ;
+	fm_ip_disable_cmd[3] = FM_FUNCTION_DISABLE;
+
+	/* Send the Packet */
+	err = fmd_send_packet(
+				CMD_IP_DISABLE_CMD_LEN,
+				fm_ip_disable_cmd);
+
+	/* Check the ErrorCode */
+	if (err != 0)
+		goto error;
+
+	/* wait till response comes */
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+
+error:
+	mutex_unlock(&send_cmd_mutex);
+	return err;
+}
+
+int fmd_send_fm_firmware(
+			u8 *fw_buffer,
+			u16 fw_size
+			)
+{
+	int err;
+	u16 bytes_to_write = ST_WRITE_FILE_BLK_SIZE -
+				FM_HCI_WRITE_FILE_BLK_PARAM_LEN;
+	u16 bytes_remaining = fw_size;
+	u8 fm_firmware_data[ST_WRITE_FILE_BLK_SIZE + FM_HCI_CMD_HEADER_LEN];
+	u32 block_id = 0;
+
+	if (fw_buffer == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	while (bytes_remaining > 0) {
+		if (bytes_remaining <
+			(ST_WRITE_FILE_BLK_SIZE -
+			 FM_HCI_WRITE_FILE_BLK_PARAM_LEN))
+			bytes_to_write = bytes_remaining;
+
+		/*
+		 * Five bytes of HCI Header for FM Firmware
+		 * so shift the firmware data by 5 bytes
+		 */
+		memcpy(
+			fm_firmware_data + FM_HCI_WRITE_FILE_BLK_HEADER_LEN,
+			fw_buffer, bytes_to_write);
+		err = fmd_write_file_block(
+				block_id,
+				fm_firmware_data,
+				bytes_to_write);
+		if (err) {
+			FM_DEBUG_REPORT("fmd_send_fm_firmware: "
+				"Failed to download %d Block "
+				"error = %d", (unsigned int)block_id, err);
+			goto error;
+		}
+		/*
+		 * Increment the Block Id by 1, since one
+		 * block is successfully transmitted
+		 * to the chip.
+		 */
+		block_id++;
+		/*
+		 * Increment the next firmware buffer equal
+		 * to the number of bytes transmitted.
+		 */
+		fw_buffer += bytes_to_write;
+		/*
+		 * Decrement the number of bytes remaining
+		 * equal to number of bytes transmitted successfully.
+		 */
+		bytes_remaining -= bytes_to_write;
+
+		if (block_id == ST_MAX_NUMBER_OF_FILE_BLOCKS)
+			block_id = 0;
+	}
+
+error:
+	return err;
+}
+
+int fmd_int_bufferfull(
+			u16 *number_of_rds_groups
+			)
+{
+	u16 response_count;
+	u16 response_data[CMD_DP_BUFFER_GET_GROUP_COUNT_PARAM_LEN];
+	u16 index = 0;
+	u16 rds_group_count;
+	u8 result = -ENOEXEC;
+	struct fmd_rds_group rds_group;
+
+	if (!fmd_state_info.rx_rds_on)
+		goto error;
+
+	/* get group count*/
+	result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_DP_BUFFER_GET_GROUP_COUNT,
+			CMD_DP_BUFFER_GET_GROUP_COUNT_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+
+	if (result != 0)
+		goto error;
+
+	/* read RDS groups */
+	rds_group_count = FM_GET_NUM_RDS_GRPS(response_data);
+	if (rds_group_count > MAX_RDS_GROUPS)
+		rds_group_count = MAX_RDS_GROUPS;
+
+	*number_of_rds_groups = rds_group_count;
+
+	if (rds_group_count) {
+		FM_DEBUG_REPORT("rds_group_count = %d", rds_group_count);
+		while (rds_group_count-- && fmd_state_info.rx_rds_on) {
+			result = fmd_send_cmd_and_read_resp(
+					CMD_FMR_DP_BUFFER_GET_GROUP,
+					CMD_DP_BUFFER_GET_GROUP_PARAM_LEN,
+					NULL,
+					&response_count,
+					(u16 *)&rds_group);
+
+			if (result != 0)
+				goto error;
+
+			if (fmd_state_info.rx_rds_on)
+				fmd_state_info.rds_group[index++] = rds_group;
+		}
+	}
+error:
+	return result;
+}
+
+void fmd_start_rds_thread(
+			cg2900_fm_rds_cb cb_func
+			)
+{
+	FM_INFO_REPORT("fmd_start_rds_thread");
+	cb_rds_func = cb_func;
+	rds_thread_required = true;
+	rds_thread_task = kthread_create(fmd_rds_thread, NULL, "rds_thread");
+	if (IS_ERR(rds_thread_task)) {
+		FM_ERR_REPORT("fmd_start_rds_thread: "
+			      "Unable to Create rds_thread");
+		rds_thread_task = NULL;
+		rds_thread_required = false;
+		return;
+	}
+	wake_up_process(rds_thread_task);
+}
+
+void fmd_stop_rds_thread(void)
+{
+	FM_INFO_REPORT("fmd_stop_rds_thread");
+	/* In case thread is waiting, set the rds sem */
+	fmd_set_rds_sem();
+	/* Re-initialize RDS Semaphore to zero */
+	sema_init(&rds_sem, 0);
+	cb_rds_func = NULL;
+	rds_thread_required = false;
+	if (rds_thread_task) {
+		kthread_stop(rds_thread_task);
+		rds_thread_task = NULL;
+		return;
+	}
+}
+
+void fmd_get_rds_sem(void)
+{
+	int ret_val;
+
+	FM_DEBUG_REPORT("fmd_get_rds_sem");
+	ret_val = down_killable(&rds_sem);
+
+	if (ret_val)
+		FM_ERR_REPORT("fmd_get_rds_sem: down_killable "
+			"returned error = %d", ret_val);
+}
+
+void fmd_set_rds_sem(void)
+{
+	FM_DEBUG_REPORT("fmd_set_rds_sem");
+	up(&rds_sem);
+}
+
+int fmd_set_dev(struct device *dev)
+{
+	struct cg2900_user_data *pf_data;
+
+	FM_DEBUG_REPORT("fmd_set_dev");
+
+	if (dev && cg2900_fm_dev) {
+		FM_ERR_REPORT("Only one FM device supported");
+		return -EACCES;
+	}
+
+	cg2900_fm_dev = dev;
+
+	if (!dev)
+		return 0;
+
+	pf_data = dev_get_platdata(dev);
+	pf_data->dev = dev;
+	pf_data->read_cb = fmd_read_cb;
+	pf_data->reset_cb = fmd_reset_cb;
+
+	return 0;
+}
+
+int fmd_set_test_tone_generator_status(
+			u8 test_tone_status
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TST_TONE_ENABLE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (test_tone_status > FMD_TST_TONE_ON_WO_SRC) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = test_tone_status;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_TST_TONE_ENABLE,
+			CMD_TST_TONE_ENABLE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_test_tone_connect(
+			u8 left_audio_mode,
+			u8 right_audio_mode
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TST_TONE_CONNECT_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (left_audio_mode > FMD_TST_TONE_AUDIO_TONE_SUM ||
+		right_audio_mode > FMD_TST_TONE_AUDIO_TONE_SUM) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = left_audio_mode;
+	parameters[1] = right_audio_mode;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_TST_TONE_CONNECT,
+			CMD_TST_TONE_CONNECT_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_test_tone_set_params(
+			u8 tone_gen,
+			u16 frequency,
+			u16 volume,
+			u16 phase_offset,
+			u16 dc,
+			u8 waveform
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TST_TONE_SET_PARAMS_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (tone_gen > FMD_TST_TONE_2 ||
+		waveform > FMD_TST_TONE_PULSE ||
+		frequency > 0x7FFF ||
+		volume > 0x7FFF) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = tone_gen;
+	parameters[1] = frequency;
+	parameters[2] = volume;
+	parameters[3] = phase_offset;
+	parameters[4] = dc;
+	parameters[5] = waveform;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_TST_TONE_SET_PARAMS,
+			CMD_TST_TONE_SET_PARAMS_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+MODULE_AUTHOR("Hemant Gupta");
+MODULE_LICENSE("GPL v2");
+
+module_param(cg2900_fm_debug_level, ushort, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(cg2900_fm_debug_level, "cg2900_fm_debug_level: "
+		" *1: Only Error Logs* "
+		" 2: Info Logs "
+		" 3: Debug Logs "
+		" 4: HCI Logs");
+
diff --git a/drivers/media/radio/CG2900/cg2900_fm_driver.h b/drivers/media/radio/CG2900/cg2900_fm_driver.h
new file mode 100755
index 00000000000..313824a1e00
--- /dev/null
+++ b/drivers/media/radio/CG2900/cg2900_fm_driver.h
@@ -0,0 +1,1761 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Linux FM Driver for CG2900 FM Chip
+ *
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#ifndef _FMDRIVER_H_
+#define _FMDRIVER_H_
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/uaccess.h>
+#include <linux/smp_lock.h>
+#include <linux/semaphore.h>
+#include <linux/version.h>
+#include <linux/kthread.h>
+#include <linux/kernel.h>
+#include <linux/timer.h>
+#include <linux/mutex.h>
+#include "cg2900_fm_api.h"
+
+/* structure declared in cg2900_fm_driver.c */
+extern struct timespec time_spec;
+
+/* module_param declared in cg2900_fm_driver.c */
+extern unsigned short cg2900_fm_debug_level;
+
+/**
+ * enum fmd_debug_levels - FM Driver Debug Levels.
+ *
+ * @FM_NO_LOGS: No Logs are displayed.
+ * @FM_ERROR_LOGS: Only Error Logs are displayed.
+ * @FM_INFO_LOGS: Function Entry logs are displayed.
+ * @FM_DEBUG_LOGS: Full debugging support.
+ * @FM_HCI_PACKET_LOGS: HCI Packet Sent/received to/by
+ * FM Driver are displayed.
+ *
+ * Various debug levels for FM Driver.
+ */
+enum fmd_debug_levels {
+	FM_NO_LOGS,
+	FM_ERROR_LOGS,
+	FM_INFO_LOGS,
+	FM_DEBUG_LOGS,
+	FM_HCI_PACKET_LOGS
+};
+
+#define FM_HEX_REPORT(fmt, arg...) \
+	if (cg2900_fm_debug_level == FM_HCI_PACKET_LOGS) { \
+		printk(KERN_INFO fmt "\r\n" , ## arg); \
+	}
+
+#define FM_DEBUG_REPORT(fmt, arg...) \
+	if (cg2900_fm_debug_level > FM_INFO_LOGS && \
+			cg2900_fm_debug_level < FM_HCI_PACKET_LOGS) { \
+		getnstimeofday(&time_spec); \
+		printk(KERN_INFO "\n[%08x:%08x] " \
+		"CG2900_FM_Driver: " fmt "\r\n" , \
+		(unsigned int)time_spec.tv_sec, \
+		(unsigned int)time_spec.tv_nsec, ## arg); \
+	}
+
+#define FM_INFO_REPORT(fmt, arg...)    \
+	if (cg2900_fm_debug_level > FM_ERROR_LOGS && \
+			cg2900_fm_debug_level < FM_HCI_PACKET_LOGS) { \
+		getnstimeofday(&time_spec); \
+		printk(KERN_INFO "\n[%08x:%08x] " \
+		"CG2900_FM_Driver: " fmt "\r\n" , \
+		(unsigned int)time_spec.tv_sec, \
+		(unsigned int)time_spec.tv_nsec, ## arg); \
+	}
+
+#define FM_ERR_REPORT(fmt, arg...)               \
+	if (cg2900_fm_debug_level >= FM_ERROR_LOGS) { \
+		getnstimeofday(&time_spec); \
+		printk(KERN_ERR "\n[%08x:%08x] " \
+		"CG2900_FM_Driver: " fmt "\r\n" , \
+		(unsigned int)time_spec.tv_sec, \
+		(unsigned int)time_spec.tv_nsec, ## arg); \
+	}
+
+#define MAX_COUNT_OF_IRQS				16
+#define MAX_BUFFER_SIZE					512
+#define MAX_NAME_SIZE					100
+/* Minimum Power level for CG2900. The value is in units of dBuV */
+#define MIN_POWER_LEVEL					88
+/* Maximum Power level for CG2900. The value is in units of dBuV */
+#define MAX_POWER_LEVEL					123
+/* Minimum RDS Deviation value for CG2900. The value is in units of 10 Hz */
+#define MIN_RDS_DEVIATION				0
+/* Default RDS Deviation value for CG2900. The value is in units of 10 Hz */
+#define DEFAULT_RDS_DEVIATION				200
+/* Maximum RDS Deviation value for CG2900. The value is in units of 10 Hz */
+#define MAX_RDS_DEVIATION				750
+#define FMD_EU_US_MIN_FREQ_IN_KHZ			87500
+#define FMD_EU_US_MAX_FREQ_IN_KHZ			108000
+#define FMD_JAPAN_MIN_FREQ_IN_KHZ			76000
+#define FMD_JAPAN_MAX_FREQ_IN_KHZ			90000
+#define FMD_CHINA_MIN_FREQ_IN_KHZ			70000
+#define FMD_CHINA_MAX_FREQ_IN_KHZ			108000
+#define FMD_MIN_CHANNEL_NUMBER				0
+#define FMD_MAX_CHANNEL_NUMBER				760
+/*
+ * Maximum supported balance for CG2900. This is just a hexadecimal number
+ * with no units.
+ */
+#define FMD_MAX_BALANCE					0x7FFF
+/*
+ * Maximum supported volume for CG2900. This is just a hexadecimal number
+ * with no units.
+ */
+#define FMD_MAX_VOLUME					0x7FFF
+/* Minimum Program Identification value as per RDS specification */
+#define MIN_PI_VALUE					0x0000
+/* Maximum Program Identification value as per RDS specification */
+#define MAX_PI_VALUE					0xFFFF
+/* Minimum Program Type code value as per RDS specification */
+#define MIN_PTY_VALUE					0
+/* Maximum Program Type code value as per RDS specification */
+#define MAX_PTY_VALUE					31
+/* Minimum Pilot Deviation value for CG2900. The value is in units of 10 Hz */
+#define MIN_PILOT_DEVIATION				0
+/* Default Pilot Deviation value for CG2900. The value is in units of 10 Hz */
+#define DEFAULT_PILOT_DEVIATION				675
+/* Maximum Pilot Deviation value for CG2900. The value is in units of 10 Hz */
+#define MAX_PILOT_DEVIATION				1000
+/*
+ * Default RSSI Threshold for a channel to be considered valid for CG2900.
+ * This is just a hexadecimal number with no units.
+ */
+#define DEFAULT_RSSI_THRESHOLD				0x0100
+/*
+ * Default Peak Noise level for a channel to be considered valid for CG2900.
+ * This is just a hexadecimal number with no units.
+ */
+#define DEFAULT_PEAK_NOISE_VALUE			0x0035
+/*
+ * Default Average Noise level for a channel to be considered valid for CG2900.
+ * This is just a hexadecimal number with no units.
+ */
+#define DEFAULT_AVERAGE_NOISE_MAX_VALUE			0x0030
+#define FREQUENCY_CONVERTOR_KHZ_HZ			1000
+#define CHANNEL_FREQ_CONVERTER_MHZ			50
+/* Interrupt(s) for CG2900 */
+#define IRPT_INVALID					0x0000
+#define IRPT_OPERATION_SUCCEEDED			0x0001
+#define IRPT_OPERATION_FAILED				0x0002
+#define IRPT_RX_BUFFERFULL_TX_BUFFEREMPTY		0x0008
+#define IRPT_RX_SIGNAL_QUALITYLOW_MUTE_STATUS_CHANGED	0x0010
+#define IRPT_RX_MONO_STEREO_TRANSITION			0x0020
+#define IRPT_TX_OVERMODULATION				0x0030
+#define IRPT_RX_RDS_SYNCFOUND_TX_OVERDRIVE		0x0040
+#define IRPT_RDS_SYNC_LOST				0x0080
+#define IRPT_PI_CODE_CHANGED				0x0100
+#define IRPT_REQUESTED_BLOCK_AVAILABLE			0x0200
+#define IRPT_BUFFER_CLEARED				0x2000
+#define IRPT_WARM_BOOT_READY				0x4000
+#define IRPT_COLD_BOOT_READY				0x8000
+/* FM Commands Id */
+#define CMD_ID_NONE					0x0000
+#define CMD_AUP_EXT_SET_MUTE				0x01E2
+#define CMD_AUP_SET_BALANCE				0x0042
+#define CMD_AUP_SET_MUTE				0x0062
+#define CMD_AUP_SET_VOLUME				0x0022
+#define CMD_FMR_DP_BUFFER_GET_GROUP			0x0303
+#define CMD_FMR_DP_BUFFER_GET_GROUP_COUNT		0x0323
+#define CMD_FMR_DP_BUFFER_SET_SIZE			0x0343
+#define CMD_FMR_DP_BUFFER_SET_THRESHOLD			0x06C3
+#define CMD_FMR_DP_SET_CONTROL				0x02A3
+#define CMD_FMR_RP_GET_RSSI				0x0083
+#define CMD_FMR_RP_STEREO_SET_MODE			0x0123
+#define CMD_FMR_SET_ANTENNA				0x0663
+#define CMD_FMR_SP_AF_SWITCH_GET_RESULT			0x0603
+#define CMD_FMR_SP_AF_SWITCH_START			0x04A3
+#define CMD_FMR_SP_AF_UPDATE_GET_RESULT			0x0483
+#define CMD_FMR_SP_AF_UPDATE_START			0x0463
+#define CMD_FMR_SP_BLOCK_SCAN_GET_RESULT		0x06A3
+#define CMD_FMR_SP_BLOCK_SCAN_START			0x0683
+#define CMD_FMR_SP_SCAN_GET_RESULT			0x0423
+#define CMD_FMR_SP_SCAN_START				0x0403
+#define CMD_FMR_SP_SEARCH_START				0x03E3
+#define CMD_FMR_SP_STOP					0x0383
+#define CMD_FMR_SP_TUNE_GET_CHANNEL			0x03A3
+#define CMD_FMR_SP_TUNE_SET_CHANNEL			0x03C3
+#define CMD_FMR_TN_SET_BAND				0x0023
+#define CMD_FMR_TN_SET_GRID				0x0043
+#define CMD_FMR_RP_SET_DEEMPHASIS			0x00C3
+#define CMD_FMT_DP_BUFFER_GET_POSITION			0x0204
+#define CMD_FMT_DP_BUFFER_SET_GROUP			0x0244
+#define CMD_FMT_DP_BUFFER_SET_SIZE			0x0224
+#define CMD_FMT_DP_BUFFER_SET_THRESHOLD			0x0284
+#define CMD_FMT_DP_SET_CONTROL				0x0264
+#define CMD_FMT_PA_SET_CONTROL				0x01A4
+#define CMD_FMT_PA_SET_MODE				0x01E4
+#define CMD_FMT_RP_SET_PILOT_DEVIATION			0x02A4
+#define CMD_FMT_RP_SET_PREEMPHASIS			0x00C4
+#define CMD_FMT_RP_SET_RDS_DEVIATION			0x0344
+#define CMD_FMT_RP_STEREO_SET_MODE			0x0164
+#define CMD_FMT_SP_TUNE_GET_CHANNEL			0x0184
+#define CMD_FMT_SP_TUNE_SET_CHANNEL			0x0064
+#define CMD_FMT_TN_SET_BAND				0x0024
+#define CMD_FMT_TN_SET_GRID				0x0044
+#define CMD_GEN_GET_MODE				0x0021
+#define CMD_GEN_GET_REGISTER_VALUE			0x00E1
+#define CMD_GEN_GET_VERSION				0x00C1
+#define CMD_GEN_GOTO_MODE				0x0041
+#define CMD_GEN_GOTO_POWERDOWN				0x0081
+#define CMD_GEN_GOTO_STANDBY				0x0061
+#define CMD_GEN_POWERUP					0x0141
+#define CMD_GEN_SELECT_REFERENCE_CLOCK			0x0201
+#define CMD_GEN_SET_REFERENCE_CLOCK			0x0161
+#define CMD_GEN_SET_REFERENCE_CLOCK_PLL			0x01A1
+#define CMD_GEN_SET_REGISTER_VALUE			0x0101
+#define CMD_TST_TONE_ENABLE				0x0027
+#define CMD_TST_TONE_CONNECT				0x0047
+#define CMD_TST_TONE_SET_PARAMS			0x0067
+
+/* FM Command Id Parameter Length */
+#define CMD_GET_VERSION_PARAM_LEN			0
+#define CMD_GET_VERSION_RSP_PARAM_LEN			7
+#define CMD_GOTO_MODE_PARAM_LEN				1
+#define CMD_SET_ANTENNA_PARAM_LEN			1
+#define CMD_TN_SET_BAND_PARAM_LEN			3
+#define CMD_TN_SET_GRID_PARAM_LEN			1
+#define CMD_SP_TUNE_SET_CHANNEL_PARAM_LEN		1
+#define CMD_SP_TUNE_GET_CHANNEL_PARAM_LEN		0
+#define CMD_SP_TUNE_GET_CHANNEL_RSP_PARAM_LEN		1
+#define CMD_RP_STEREO_SET_MODE_PARAM_LEN		1
+#define CMD_RP_GET_RSSI_PARAM_LEN			0
+#define CMD_RP_GET_RSSI_RSP_PARAM_LEN			1
+#define CMD_SP_SEARCH_START_PARAM_LEN			4
+#define CMD_SP_SCAN_START_PARAM_LEN			4
+#define CMD_SP_SCAN_GET_RESULT_PARAM_LEN		1
+#define CMD_SP_SCAN_GET_RESULT_RSP_PARAM_LEN		7
+#define CMD_SP_BLOCK_SCAN_START_PARAM_LEN		3
+#define CMD_SP_BLOCK_SCAN_GET_RESULT_PARAM_LEN		1
+#define CMD_SP_BLOCK_SCAN_GET_RESULT_RSP_PARAM_LEN	7
+#define CMD_SP_STOP_PARAM_LEN				0
+#define CMD_SP_AF_UPDATE_START_PARAM_LEN		1
+#define CMD_SP_AF_UPDATE_GET_RESULT_PARAM_LEN		0
+#define CMD_SP_AF_UPDATE_GET_RESULT_RSP_PARAM_LEN	1
+#define CMD_SP_AF_SWITCH_START_PARAM_LEN		5
+#define CMD_SP_AF_SWITCH_GET_RESULT_PARAM_LEN		0
+#define CMD_SP_AF_SWITCH_GET_RESULT_RWSP_PARAM_LEN	3
+#define CMD_DP_BUFFER_SET_SIZE_PARAM_LEN		1
+#define CMD_DP_BUFFER_SET_THRESHOLD_PARAM_LEN		1
+#define CMD_DP_SET_CONTROL_PARAM_LEN			1
+#define CMD_PA_SET_MODE_PARAM_LEN			1
+#define CMD_PA_SET_CONTROL_PARAM_LEN			1
+#define CMD_RP_SET_PREEMPHASIS_PARAM_LEN		1
+#define CMD_RP_SET_DEEMPHASIS_PARAM_LEN			1
+#define CMD_RP_SET_PILOT_DEVIATION_PARAM_LEN		1
+#define CMD_RP_SET_RDS_DEVIATION_PARAM_LEN		1
+#define CMD_DP_BUFFER_SET_GROUP_PARAM_LEN		5
+#define CMD_SET_BALANCE_PARAM_LEN			1
+#define CMD_SET_VOLUME_PARAM_LEN			1
+#define CMD_SET_MUTE_PARAM_LEN				2
+#define CMD_EXT_SET_MUTE_PARAM_LEN			1
+#define CMD_POWERUP_PARAM_LEN				0
+#define CMD_GOTO_STANDBY_PARAM_LEN			0
+#define CMD_GOTO_POWERDOWN_PARAM_LEN			0
+#define CMD_SELECT_REFERENCE_CLOCK_PARAM_LEN		1
+#define CMD_SET_REFERENCE_CLOCK_PLL_PARAM_LEN		1
+#define CMD_DP_BUFFER_GET_GROUP_COUNT_PARAM_LEN		0
+#define CMD_DP_BUFFER_GET_GROUP_PARAM_LEN		0
+#define CMD_IP_ENABLE_CMD_LEN				4
+#define CMD_IP_ENABLE_PARAM_LEN				3
+#define CMD_IP_DISABLE_CMD_LEN				4
+#define CMD_IP_DISABLE_PARAM_LEN			3
+#define CMD_TST_TONE_ENABLE_PARAM_LEN			1
+#define CMD_TST_TONE_CONNECT_PARAM_LEN			2
+#define CMD_TST_TONE_SET_PARAMS_PARAM_LEN		6
+
+/* FM HCI Command and event specific */
+#define FM_WRITE					0x00
+#define FM_READ						0x01
+#define FM_CATENA_OPCODE				0xFE
+#define HCI_CMD_FM					0xFD50
+#define HCI_CMD_VS_WRITE_FILE_BLOCK			0xFC2E
+#define FM_EVENT_ID					0x15
+#define FM_SUCCESS_STATUS				0x00
+#define FM_EVENT					0x01
+#define HCI_COMMAND_COMPLETE_EVENT			0x0E
+#define HCI_VS_DBG_EVENT				0xFF
+#define ST_WRITE_FILE_BLK_SIZE				254
+#define ST_MAX_NUMBER_OF_FILE_BLOCKS			256
+#define FM_PG1_INTERRUPT_EVENT_LEN			0x04
+#define FM_PG2_INTERRUPT_EVENT_LEN			0x06
+#define FM_HCI_CMD_HEADER_LEN				6
+#define FM_HCI_CMD_PARAM_LEN				5
+#define FM_HCI_WRITE_FILE_BLK_HEADER_LEN		5
+#define FM_HCI_WRITE_FILE_BLK_PARAM_LEN			4
+#define HCI_PACKET_INDICATOR_CMD			0x01
+#define HCI_PACKET_INDICATOR_EVENT			0x04
+#define HCI_PACKET_INDICATOR_FM_CMD_EVT			0x08
+/* FM Functions specific to CG2900 */
+#define FM_FUNCTION_ENABLE				0x00
+#define FM_FUNCTION_DISABLE				0x01
+#define FM_FUNCTION_RESET				0x02
+#define FM_FUNCTION_WRITE_COMMAND			0x10
+#define FM_FUNCTION_SET_INT_MASK_ALL			0x20
+#define FM_FUNCTION_GET_INT_MASK_ALL			0x21
+#define FM_FUNCTION_SET_INT_MASK			0x22
+#define FM_FUNCTION_GET_INT_MASK			0x23
+#define FM_FUNCTION_FIRMWARE_DOWNLOAD			0x30
+/* Command succeeded */
+#define FM_CMD_STATUS_CMD_SUCCESS			0x00
+/* HCI_ERR_HW_FAILURE when no response from the IP */
+#define FM_CMD_STATUS_HCI_ERR_HW_FAILURE		0x03
+/* HCI_ERR_INVALID_PARAMETERS. */
+#define FM_CMD_STATUS_HCI_ERR_INVALID_PARAMETERS	0x12
+/* When the host tries to send a command to an IP that hasn't been
+ * initialized.
+ */
+#define FM_CMD_STATUS_IP_UNINIT				0x15
+/* HCI_ERR_UNSPECIFIED_ERROR: any other error */
+#define FM_CMD_STATUS_HCI_ERR_UNSPECIFIED_ERROR		0x1F
+/* HCI_ERR_CMD_DISALLOWED when the host asks for an unauthorized operation
+ * (FM state transition for instance)
+ */
+#define FM_CMD_STATUS_HCI_ERR_CMD_DISALLOWED		0x0C
+/* Wrong sequence number for FM FW download command */
+#define FM_CMD_STATUS_WRONG_SEQ_NUM			0xF1
+/* Unknown file type for FM FW download command */
+#define FM_CMD_STATUS_UNKNOWN_FILE_TYPE			0xF2
+/* File version mismatch for FM FW download command */
+#define FM_CMD_STATUS_FILE_VERSION_MISMATCH		0xF3
+
+
+/**
+ * enum fmd_event - Events received.
+ *
+ * @FMD_EVENT_OPERATION_COMPLETED: Previous operation has been completed.
+ * @FMD_EVENT_ANTENNA_STATUS_CHANGED: Antenna has been changed.
+ * @FMD_EVENT_FREQUENCY_CHANGED: Frequency has been changed.
+ * @FMD_EVENT_SEEK_COMPLETED: Seek operation has completed.
+ * @FMD_EVENT_SCAN_BAND_COMPLETED: Band Scan completed.
+ * @FMD_EVENT_BLOCK_SCAN_COMPLETED: Block Scan completed.
+ * @FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE: Af Update or AF Switch is complete.
+ * @FMD_EVENT_MONO_STEREO_TRANSITION_COMPLETE: Mono stereo transition is
+ * completed.
+ * @FMD_EVENT_SEEK_STOPPED: Previous Seek/Band Scan/ Block Scan operation is
+ * stopped.
+ * @FMD_EVENT_GEN_POWERUP: FM IP Powerup has been powered up.
+ * @FMD_EVENT_RDSGROUP_RCVD: RDS Groups Full interrupt.
+ * @FMD_EVENT_LAST_ELEMENT: Last event, used for keeping count of
+ * number of events.
+ *
+ * Various events received from FM driver for Upper Layer(s) processing.
+ */
+enum fmd_event {
+	FMD_EVENT_OPERATION_COMPLETED,
+	FMD_EVENT_ANTENNA_STATUS_CHANGED,
+	FMD_EVENT_FREQUENCY_CHANGED,
+	FMD_EVENT_SEEK_COMPLETED,
+	FMD_EVENT_SCAN_BAND_COMPLETED,
+	FMD_EVENT_BLOCK_SCAN_COMPLETED,
+	FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE,
+	FMD_EVENT_MONO_STEREO_TRANSITION_COMPLETE,
+	FMD_EVENT_SEEK_STOPPED,
+	FMD_EVENT_GEN_POWERUP,
+	FMD_EVENT_RDSGROUP_RCVD,
+	FMD_EVENT_LAST_ELEMENT
+};
+
+/**
+ * enum fmd_mode - FM Driver Modes.
+ *
+ * @FMD_MODE_IDLE: FM Driver in Idle mode.
+ * @FMD_MODE_RX: FM Driver in Rx mode.
+ * @FMD_MODE_TX: FM Driver in Tx mode.
+ *
+ * Various Modes of FM Radio.
+ */
+enum fmd_mode {
+	FMD_MODE_IDLE,
+	FMD_MODE_RX,
+	FMD_MODE_TX
+};
+
+/**
+ * enum fmd_antenna - Antenna selection.
+ *
+ * @FMD_ANTENNA_EMBEDDED: Embedded Antenna.
+ * @FMD_ANTENNA_WIRED: Wired Antenna.
+ *
+ * Antenna to be used for FM Radio.
+ */
+enum fmd_antenna {
+	FMD_ANTENNA_EMBEDDED,
+	FMD_ANTENNA_WIRED
+};
+
+/**
+ * enum fmd_grid - Grid used on FM Radio.
+ *
+ * @FMD_GRID_50KHZ: 50  kHz grid spacing.
+ * @FMD_GRID_100KHZ: 100  kHz grid spacing.
+ * @FMD_GRID_200KHZ: 200  kHz grid spacing.
+ *
+ * Spacing used on FM Radio.
+ */
+enum fmd_grid {
+	FMD_GRID_50KHZ,
+	FMD_GRID_100KHZ,
+	FMD_GRID_200KHZ
+};
+
+/**
+ * enum fmd_emphasis - De-emphasis/Pre-emphasis level.
+ *
+ * @FMD_EMPHASIS_NONE: De-emphasis Disabled.
+ * @FMD_EMPHASIS_50US: 50 us de-emphasis/pre-emphasis level.
+ * @FMD_EMPHASIS_75US: 75 us de-emphasis/pre-emphasis level.
+ *
+ * De-emphasis/Pre-emphasis level used on FM Radio.
+ */
+enum fmd_emphasis {
+	FMD_EMPHASIS_NONE = 0,
+	FMD_EMPHASIS_50US = 1,
+	FMD_EMPHASIS_75US = 2
+};
+
+/**
+ * enum fmd_freq_range - Frequency range.
+ *
+ * @FMD_FREQRANGE_EUROAMERICA: EU/US Range (87.5 - 108 MHz).
+ * @FMD_FREQRANGE_JAPAN: Japan Range (76 - 90 MHz).
+ * @FMD_FREQRANGE_CHINA: China Range (70 - 108 MHz).
+ *
+ * Various Frequency range(s) supported by FM Radio.
+ */
+enum fmd_freq_range {
+	FMD_FREQRANGE_EUROAMERICA,
+	FMD_FREQRANGE_JAPAN,
+	FMD_FREQRANGE_CHINA
+};
+
+/**
+ * enum fmd_stereo_mode - FM Driver Stereo Modes.
+ *
+ * @FMD_STEREOMODE_OFF: Streo Blending Off.
+ * @FMD_STEREOMODE_MONO: Mono Mode.
+ * @FMD_STEREOMODE_BLENDING: Blending Mode.
+ *
+ * Various Stereo Modes of FM Radio.
+ */
+enum fmd_stereo_mode {
+	FMD_STEREOMODE_OFF,
+	FMD_STEREOMODE_MONO,
+	FMD_STEREOMODE_BLENDING
+};
+
+/**
+ * enum fmd_pilot_tone - Pilot Tone Selection
+ *
+ * @FMD_PILOT_TONE_DISABLED: Pilot Tone to be disabled.
+ * @FMD_PILOT_TONE_ENABLED: Pilot Tone to be enabled.
+ *
+ * Pilot Tone to be enabled or disabled.
+ */
+enum fmd_pilot_tone {
+	FMD_PILOT_TONE_DISABLED,
+	FMD_PILOT_TONE_ENABLED
+};
+
+/**
+ * enum fmd_output - Output of Sample Rate Converter.
+ *
+ * @FMD_OUTPUT_DISABLED: Sample Rate converter in disabled.
+ * @FMD_OUTPUT_I2S: I2S Output from Sample rate converter.
+ * @FMD_OUTPUT_PARALLEL: Parallel output from sample rate converter.
+ *
+ * Sample Rate Converter's output to be set on Connectivity Controller.
+ */
+enum fmd_output {
+	FMD_OUTPUT_DISABLED,
+	FMD_OUTPUT_I2S,
+	FMD_OUTPUT_PARALLEL
+};
+
+/**
+ * enum fmd_input - Audio Input to Sample Rate Converter.
+ *
+ * @FMD_INPUT_ANALOG: Selects the ADC's as audio source
+ * @FMD_INPUT_DIGITAL: Selects Digital Input as audio source.
+ *
+ * Audio Input source for Sample Rate Converter.
+ */
+enum fmd_input {
+	FMD_INPUT_ANALOG,
+	FMD_INPUT_DIGITAL
+};
+
+/**
+ * enum fmd_rds_mode - RDS Mode to be selected for FM Rx.
+ *
+ * @FMD_SWITCH_OFF_RDS: RDS Decoding disabled in FM Chip.
+ * @FMD_SWITCH_ON_RDS: RDS Decoding enabled in FM Chip.
+ * @FMD_SWITCH_ON_RDS_ENHANCED_MODE: Enhanced RDS Mode.
+ * @FMD_SWITCH_ON_RDS_SIMULATOR: RDS Simulator switched on in FM Chip.
+ *
+ * RDS Mode to be selected for FM Rx.
+ */
+enum fmd_rds_mode {
+	FMD_SWITCH_OFF_RDS,
+	FMD_SWITCH_ON_RDS,
+	FMD_SWITCH_ON_RDS_ENHANCED_MODE,
+	FMD_SWITCH_ON_RDS_SIMULATOR
+};
+
+/**
+ * enum fmd_tst_tone_status - Test Tone Generator Status.
+ *
+ * @FMD_TST_TONE_OFF: Test Tone Generator is off.
+ * @FMD_TST_TONE_ON_W_SRC: Test Tone Gen. is on with Sample Rate Conversion.
+ * @FMD_TST_TONE_ON_WO_SRC: Test Tone Gen. is on without Sample Rate Conversion.
+ *
+ * Test Tone Generator status to be set.
+ */
+enum fmd_tst_tone_status {
+	FMD_TST_TONE_OFF,
+	FMD_TST_TONE_ON_W_SRC,
+	FMD_TST_TONE_ON_WO_SRC
+};
+
+/**
+ * enum fmd_tst_tone_audio_mode - Test Tone Generator Audio Output/Input Mode.
+ *
+ * @FMD_TST_TONE_AUDIO_NORMAL: Normal Audio.
+ * @FMD_TST_TONE_AUDIO_ZERO: Zero.
+ * @FMD_TST_TONE_AUDIO_TONE_1: Tone 1.
+ * @FMD_TST_TONE_AUDIO_TONE_2: Tone 2.
+ * @FMD_TST_TONE_AUDIO_TONE_SUM: Sum of Tone 1 and Tone 2.
+ *
+ * Test Tone Generator Audio Output/Input Modes.
+ */
+enum fmd_tst_tone_audio_mode {
+	FMD_TST_TONE_AUDIO_NORMAL,
+	FMD_TST_TONE_AUDIO_ZERO,
+	FMD_TST_TONE_AUDIO_TONE_1,
+	FMD_TST_TONE_AUDIO_TONE_2,
+	FMD_TST_TONE_AUDIO_TONE_SUM
+};
+
+/**
+ * enum fmd_tst_tone - Test Tone of Internal Tone Generator.
+ *
+ * @FMD_TST_TONE_1: Test Tone 1
+ * @FMD_TST_TONE_2: Test Tone 2
+ *
+ * Test Tone.
+ */
+enum fmd_tst_tone {
+	FMD_TST_TONE_1,
+	FMD_TST_TONE_2
+};
+
+/**
+ * enum fmd_tst_tone_waveform - Test Tone Waveform of Internal Tone Generator.
+ *
+ * @FMD_TST_TONE_SINE: Sine wave
+ * @FMD_TST_TONE_PULSE: Pulse wave
+ *
+ * Test Tone waveform.
+ */
+enum fmd_tst_tone_waveform {
+	FMD_TST_TONE_SINE,
+	FMD_TST_TONE_PULSE
+};
+
+/* Callback function to receive radio events. */
+typedef void(*fmd_radio_cb)(
+		u8 event,
+		bool event_successful
+		);
+
+/**
+ * fmd_init() - Initialize the FM Driver internal structures.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -EIO, if there is an error.
+ */
+int fmd_init(void);
+
+/**
+ * fmd_exit() - De-initialize the FM Driver.
+ */
+void fmd_exit(void);
+
+/**
+ * fmd_register_callback() - Function to register callback function.
+ *
+ * This function registers the callback function provided by upper layers.
+ * @callback: Fmradio call back Function pointer
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_register_callback(
+			fmd_radio_cb callback
+			);
+
+/**
+ * fmd_get_version() - Retrieves the FM HW and FW version.
+ *
+ * @version: (out) Version Array
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameters are not valid.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_get_version(
+			u16 *version
+			);
+
+/**
+ * fmd_set_mode() - Starts a transition to the given mode.
+ *
+ * @mode: Transition mode
+ *
+ *  Returns:
+ *	 0,  if set mode done successfully.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_mode(
+			u8 mode
+			);
+
+/**
+ * fmd_get_freq_range_properties() - Retrieves Freq Range Properties.
+ *
+ * @range: range of freq
+ * @min_freq: (out) Minimum Frequency of the Band in kHz.
+ * @max_freq: (out) Maximum Frequency of the Band in kHz
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameter is invalid.
+ */
+int fmd_get_freq_range_properties(
+			u8 range,
+			u32  *min_freq,
+			u32  *max_freq
+			);
+
+/**
+ * fmd_set_antenna() - Selects the antenna to be used in receive mode.
+ *
+ * embedded - Selects the embedded antenna, wired- Selects the wired antenna.
+ * @antenna: Antenna Type
+ *
+ *  Returns:
+ *	 0,  if set antenna done successfully.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_antenna(
+			u8 antenna
+			);
+
+/**
+ * fmd_get_antenna() - Retrieves the currently used antenna type.
+ *
+ * @antenna: (out) Antenna Selected on FM Radio.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_get_antenna(
+			u8 *antenna
+			);
+
+/**
+ * fmd_set_freq_range() - Sets the FM band.
+ *
+ * @range: freq range
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_freq_range(
+			u8 range
+			);
+
+/**
+ * fmd_get_freq_range() - Gets the FM band currently in use.
+ *
+ * @range: (out) Frequency Range set on FM Radio.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ */
+int fmd_get_freq_range(
+			u8 *range
+			);
+
+/**
+ * fmd_rx_set_grid() - Sets the tuning grid.
+ *
+ * @grid: Tuning grid size
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_set_grid(
+			u8 grid
+			);
+
+/**
+ * fmd_rx_set_frequency() - Sets the FM Channel.
+ *
+ * @freq: Frequency to Set in Khz
+ *
+ *  Returns:
+ *	 0,  if set frequency done successfully.
+ *	 -EINVAL,  if parameters are invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_set_frequency(
+			u32 freq
+			);
+
+/**
+ * fmd_rx_get_frequency() - Gets the currently used FM Channel.
+ *
+ * @freq: (out) Current Frequency set on FM Radio.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL,  if parameters are invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_get_frequency(
+			u32  *freq
+			);
+
+/**
+ * fmd_rx_set_stereo_mode() - Sets the stereomode functionality.
+ *
+ * @mode: FMD_STEREOMODE_MONO, FMD_STEREOMODE_STEREO and
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_set_stereo_mode(
+			u8 mode
+			);
+
+/**
+ * fmd_rx_get_stereo_mode() - Gets the currently used FM mode.
+ *
+ * FMD_STEREOMODE_MONO, FMD_STEREOMODE_STEREO and
+ * FMD_STEREOMODE_AUTO.
+ * @mode: (out) Mode set on FM Radio, stereo or mono.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_rx_get_stereo_mode(
+			u8 *mode
+			);
+
+/**
+ * fmd_rx_get_signal_strength() - Gets the RSSI level of current frequency.
+ *
+ * @strength: (out) RSSI level of current channel.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_get_signal_strength(
+			u16  *strength
+			);
+
+/**
+ * fmd_rx_set_stop_level() - Sets the FM Rx Seek stop level.
+ *
+ * @stoplevel: seek stop level
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_rx_set_stop_level(
+			u16 stoplevel
+			);
+
+/**
+ * fmd_rx_get_stop_level() - Gets the current FM Rx Seek stop level.
+ *
+ * @stoplevel: (out) RSSI Threshold set on FM Radio.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_rx_get_stop_level(
+			u16  *stoplevel
+			);
+
+/**
+ * fmd_rx_seek() - Perform FM Seek.
+ *
+ * Starts searching relative to the actual channel with
+ * a specific direction, stop.
+ *  level and optional noise levels
+ * @upwards: scan up
+ *
+ *  Returns:
+ *	 0,  if seek started successfully.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_seek(
+			bool upwards
+			);
+
+/**
+ * fmd_rx_stop_seeking() - Stops a currently active seek or scan band.
+ *
+ *  Returns:
+ *	 0,  if stop seek done successfully.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -ENOEXEC, if FM Driver is
+ * not currently in Seek or Scan State..
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_stop_seeking(void);
+
+/**
+ * fmd_rx_af_update_start() - Perform AF update.
+ *
+ * This is used to switch to a shortly tune to a AF freq,
+ * measure its RSSI and tune back to the original frequency.
+ * @freq: Alternative frequncy in KHz to be set for AF updation.
+ *
+ *  Returns:
+ * -EBUSY, if FM Driver is not in idle state.
+ * 0,  if no error.
+ * -ENOEXEC, if preconditions are violated.
+ */
+int fmd_rx_af_update_start(
+			 u32 freq
+			 );
+
+/**
+ * fmd_rx_get_af_update_result() - Retrive result of AF update.
+ *
+ * Retrive the RSSI level of the Alternative frequency.
+ * @af_level: RSSI level of the Alternative frequency.
+ *
+ * Returns:
+ * -EBUSY, if FM Driver is not in idle state.
+ * 0,  if no error.
+ * -EINVAL, if parameter is invalid.
+ * -ENOEXEC, if preconditions are violated.
+ */
+int fmd_rx_get_af_update_result(
+			 u16 *af_level
+			 );
+
+/**
+ * fmd_af_switch_start() -Performs AF switch.
+ *
+ * @freq: Frequency to Set in Khz.
+ * @picode:programable id,unique for each station.
+ *
+ *  Returns:
+ * -EBUSY, if FM Driver is not in idle state.
+ * 0,  if no error and if AF switch started successfully.
+ * -ENOEXEC, if preconditions are violated.
+ */
+int fmd_rx_af_switch_start(
+			u32 freq,
+			u16  picode
+			);
+
+/**
+ * fmd_rx_get_af_switch_results() -Retrieves the results of AF Switch.
+ *
+ * @afs_conclusion: Conclusion of AF switch.
+ * @afs_level: RSSI level of the Alternative frequnecy.
+ * @afs_pi: PI code of the alternative channel (if found).
+ *
+ * Returns:
+ * -EBUSY, if FM Driver is not in idle state.
+ * 0,  if no error.
+ * -EINVAL, if parameter is invalid.
+ * -ENOEXEC, if preconditions are violated.
+ */
+int fmd_rx_get_af_switch_results(
+			 u16 *afs_conclusion,
+			 u16 *afs_level,
+			 u16 *afs_pi
+			 );
+
+/**
+ * fmd_rx_scan_band() - Starts Band Scan.
+ *
+ * Starts scanning the active band for the strongest
+ * channels above a threshold.
+ * @max_channels_to_scan: Maximum number of channels to scan.
+ *
+ * Returns:
+ *   0,  if scan band started successfully.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_scan_band(
+			u8 max_channels_to_scan
+			);
+
+/**
+ * fmd_rx_get_max_channels_to_scan() - Retreives the maximum channels.
+ *
+ * Retrieves the maximum number of channels that can be found during
+ * band scann.
+ * @max_channels_to_scan: (out) Maximum number of channels to scan.
+ *
+ * Returns:
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if parameter is invalid.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_get_max_channels_to_scan(
+			u8 *max_channels_to_scan
+			);
+
+/**
+ * fmd_rx_get_scan_band_info() - Retrieves Channels found during scanning.
+ *
+ * Retrieves the scanned active band
+ * for the strongest channels above a threshold.
+ * @index: (out) Index value to retrieve the channels.
+ * @numchannels: (out) Number of channels found during Band Scan.
+ * @channels: (out) Channels found during band scan.
+ * @rssi: (out) Rssi of channels found during Band scan.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_get_scan_band_info(
+			u32 index,
+			u16  *numchannels,
+			u16  *channels,
+			u16  *rssi
+			);
+
+/**
+ * fmd_rx_block_scan() - Starts Block Scan.
+ *
+ * Starts block scan for retriving the RSSI level of channels
+ * in the given block.
+ * @start_freq: Starting frequency of the block from where scanning has
+ * to be started.
+ * @stop_freq: End frequency of the block to be scanned.
+ * @antenna: Antenna to be used during scanning.
+ *
+ * Returns:
+ *   0,  if scan band started successfully.
+ *   -EINVAL,  if parameters are invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_block_scan(
+			u32 start_freq,
+			u32 stop_freq,
+			u8 antenna
+			);
+
+/**
+ * fmd_rx_get_block_scan_result() - Retrieves RSSI Level of channels.
+ *
+ * Retrieves the RSSI level of the channels in the block.
+ * @index: (out) Index value to retrieve the channels.
+ * @numchannels: (out) Number of channels found during Band Scan.
+ * @rssi: (out) Rssi of channels found during Band scan.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_get_block_scan_result(
+			u32 index,
+			u16 *numchannels,
+			u16 *rssi
+			);
+
+/**
+ * fmd_rx_get_rds() - Gets the current status of RDS transmission.
+ *
+ * @on: (out) RDS status
+ *
+ *  Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_rx_get_rds(
+			bool  *on
+			);
+
+/**
+ * fmd_rx_buffer_set_size() - Sets the number of groups that the data buffer.
+ * can contain and clears the buffer.
+ *
+ * @size: buffer size
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_buffer_set_size(
+			u8 size
+			);
+
+/**
+ * fmd_rx_buffer_set_threshold() - RDS Buffer Threshold level in FM Chip.
+ *
+ * Sets the group number at which the RDS buffer full interrupt must be
+ * generated. The interrupt will be set after reception of the group.
+ * @threshold: threshold level.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_buffer_set_threshold(
+			u8 threshold
+			);
+
+/**
+ * fmd_rx_set_rds() - Enables or disables demodulation of RDS data.
+ *
+ * @on_off_state : Rx Set ON /OFF control
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_set_rds(
+			u8 on_off_state
+			);
+
+/**
+ * fmd_rx_get_low_level_rds_groups() - Gets Low level RDS group data.
+ *
+ * @index: RDS group index
+ * @block1: (out) RDS Block 1
+ * @block2: (out) RDS Block 2
+ * @block3: (out) RDS Block 3
+ * @block4: (out) RDS Block 4
+ * @status1: (out) RDS data status 1
+ * @status2: (out) RDS data status 2
+ * @status3: (out) RDS data status 3
+ * @status4: (out) RDS data status 4
+ *
+ *  Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_rx_get_low_level_rds_groups(
+			u8 index,
+			u16  *block1,
+			u16  *block2,
+			u16  *block3,
+			u16  *block4,
+			u8  *status1,
+			u8  *status2,
+			u8  *status3,
+			u8  *status4
+			);
+
+/**
+ * fmd_tx_set_pa() - Enables or disables the Power Amplifier.
+ *
+ * @on: Power Amplifier current state to set
+ *
+ * Returns:
+ *   0,  if set Power Amplifier done successfully.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_pa(
+			bool on
+			);
+
+/**
+ * fmd_tx_set_signal_strength() - Sets the RF-level of the output FM signal.
+ *
+ * @strength: Signal strength to be set for FM Tx in dBuV.
+ *
+ * Returns:
+ *   0,  if set RSSI Level done successfully.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_signal_strength(
+			u16 strength
+			);
+
+/**
+ * fmd_tx_get_signal_strength() - Retrieves current RSSI of FM Tx.
+ *
+ * @strength: (out) Strength of signal being transmitted in dBuV.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -EINVAL, if parameter is invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_signal_strength(
+			u16  *strength
+			);
+
+/**
+ * fmd_tx_set_freq_range() - Sets the FM band and specifies the custom band.
+ *
+ * @range: Freq range to set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -EINVAL, if parameter is invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_freq_range(
+			u8 range
+			);
+
+/**
+ * fmd_tx_get_freq_range() - Gets the FM band currently in use.
+ *
+ * @range: (out) Frequency Range set on Fm Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ */
+int fmd_tx_get_freq_range(
+			u8 *range
+			);
+
+/**
+ * fmd_tx_set_grid() - Sets the tuning grid size.
+ *
+ * @grid: FM Grid (50 Khz, 100 Khz, 200 Khz) to be set for FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -EINVAL, if parameter is invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_grid(
+			u8 grid
+			);
+
+/**
+ * fmd_tx_get_grid() - Gets the current tuning grid size.
+ *
+ * @grid: (out) FM Grid (50 Khz, 100 Khz, 200 Khz) currently set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_grid(
+			u8 *grid
+			);
+
+/**
+ * fmd_tx_set_preemphasis() - Sets the Preemphasis characteristic of the Tx.
+ *
+ * @preemphasis: Pre-emphasis level to be set for FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_preemphasis(
+			u8 preemphasis
+			);
+
+/**
+ * fmd_tx_get_preemphasis() - Gets the currently used Preemphasis char of th FM Tx.
+ *
+ * @preemphasis: (out) Preemphasis Level used for FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_preemphasis(
+			u8 *preemphasis
+			);
+
+/**
+ * fmd_tx_set_frequency() - Sets the FM Channel for Tx.
+ *
+ * @freq: Freq to be set for transmission.
+ *
+ * Returns:
+ *   0,  if set frequency done successfully.
+ *   -EINVAL,  if parameters are invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_frequency(
+			u32 freq
+			);
+
+/**
+ * fmd_rx_get_frequency() - Gets the currently used Channel for Tx.
+ *
+ * @freq: (out) Frequency set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -EINVAL,  if parameters are invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_get_frequency(
+			u32  *freq
+			);
+
+/**
+ * fmd_tx_enable_stereo_mode() - Sets Stereo mode state for TX.
+ *
+ * @enable_stereo_mode: Flag indicating enabling or disabling Stereo mode.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_enable_stereo_mode(
+			bool enable_stereo_mode
+			);
+
+/**
+ * fmd_tx_get_stereo_mode() - Gets the currently used FM Tx stereo mode.
+ *
+ * @stereo_mode: (out) Stereo Mode state set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -EINVAL, if parameter is invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_stereo_mode(
+			bool  *stereo_mode
+			);
+
+/**
+ * fmd_tx_set_pilot_deviation() - Sets pilot deviation in HZ
+ *
+ * @deviation: Pilot deviation in HZ to set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_pilot_deviation(
+			u16 deviation
+			);
+
+/**
+ * fmd_tx_get_pilot_deviation() - Retrieves the current pilot deviation.
+ *
+ * @deviation: (out) Pilot deviation set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_pilot_deviation(
+			u16  *deviation
+			);
+
+/**
+ * fmd_tx_set_rds_deviation() - Sets Rds deviation in HZ.
+ *
+ * @deviation: RDS deviation in HZ.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_rds_deviation(
+			u16 deviation
+			);
+
+/**
+ * fmd_tx_get_rds_deviation() - Retrieves the current Rds deviation.
+ *
+ * @deviation: (out) RDS deviation currently set.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_rds_deviation(
+			u16  *deviation
+			);
+
+/**
+ * fmd_tx_set_rds() - Enables or disables RDS transmission for Tx.
+ *
+ * @on: Boolean - RDS ON
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_rds(
+			bool on
+			);
+
+/**
+ * fmd_rx_get_rds() - Gets the current status of RDS transmission for FM Tx.
+ *
+ * @on: (out) Rds enabled or disabled.
+ *
+ *Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_rds(
+			bool  *on
+			);
+
+/**
+ * fmd_tx_set_group() - Programs a grp on a certain position in the RDS buffer.
+ *
+ * @position: RDS group position
+ * @block1: Data to be transmitted in Block 1
+ * @block2: Data to be transmitted in Block 2
+ * @block3: Data to be transmitted in Block 3
+ * @block4: Data to be transmitted in Block 4
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if parameters are invalid.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_group(
+			u16 position,
+			u8  *block1,
+			u8  *block2,
+			u8  *block3,
+			u8  *block4
+			);
+
+/**
+ * fmd_tx_buffer_set_size() - Controls the size of the RDS buffer in groups.
+ *
+ * @buffer_size: RDS buffer size.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_buffer_set_size(
+			u16 buffer_size
+			);
+
+/**
+ * fmd_set_volume() - Sets the receive audio volume.
+ *
+ * @volume: Audio volume level
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_volume(
+			u8 volume
+			);
+
+/**
+ * fmd_get_volume() - Retrives the current audio volume.
+ *
+ * @volume: Analog Volume level.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_get_volume(
+			u8 *volume
+			);
+
+/**
+ * fmd_set_balance() - Controls the receiver audio balance.
+ *
+ * @balance: Audio balance level
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_balance(
+			s8 balance
+			);
+
+/**
+ * fmd_set_mute() - Enables or disables muting of the analog audio(DAC).
+ *
+ * @mute_on: bool of mute on
+ *
+ * Returns:
+ *   0,  if mute done successfully.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_mute(
+			bool mute_on
+			);
+
+/**
+ * fmd_ext_set_mute() - Enables or disables muting of the audio channel.
+ *
+ * @mute_on: bool to Mute
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_ext_set_mute(
+			bool mute_on
+			);
+
+/**
+ * fmd_power_up() - Puts the system in Powerup state.
+ *
+ * Returns:
+ *   0,  if power up command sent successfully to chip.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_power_up(void);
+
+/**
+ * fmd_goto_standby() - Puts the system in standby mode.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_goto_standby(void);
+
+/**
+ * fmd_goto_power_down() - Puts the system in Powerdown mode.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_goto_power_down(void);
+
+/**
+ * fmd_select_ref_clk() - Selects the FM reference clock.
+ *
+ * @ref_clk: Ref Clock.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_select_ref_clk(
+			u16 ref_clk
+			);
+
+/**
+ * fmd_set_ref_clk_pll() - Sets the freq of Referece Clock.
+ *
+ * Sets frequency and offset correction properties of the external
+ * reference clock of the PLL
+ * @freq: PLL Frequency/ 2 in kHz.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_ref_clk_pll(
+			u16 freq
+			);
+
+/**
+ * fmd_send_fm_ip_enable()- Enables the FM IP.
+ *
+ * Returns:
+ *	 0: If there is no error.
+ *	 -ETIME: Otherwise
+ */
+int fmd_send_fm_ip_enable(void);
+
+/**
+ * fmd_send_fm_ip_disable()- Disables the FM IP.
+ *
+ * Returns:
+ *	 0, If there is no error.
+ *	 -ETIME: Otherwise
+ */
+int fmd_send_fm_ip_disable(void);
+
+/**
+ * fmd_send_fm_firmware() - Send the FM Firmware File to Device.
+ *
+ * @fw_buffer: Firmware to be downloaded.
+ * @fw_size: Size of firmware to be downloaded.
+ *
+ * Returns:
+ *	 0, If there is no error.
+ *	 -ETIME: Otherwise
+ */
+int fmd_send_fm_firmware(
+			u8 *fw_buffer,
+			u16 fw_size
+			);
+
+/**
+ * fmd_int_bufferfull() - RDS Groups availabe for reading by Host.
+ *
+ * Gets the number of groups that are available in the
+ * buffer. This function is called in RX mode to read RDS groups.
+ * @number_of_rds_groups: Number of RDS groups ready to
+ * be read from the Host.
+ *
+ * Returns:
+ *	 0, If there is no error.
+ *	 corresponding error Otherwise
+ */
+int fmd_int_bufferfull(
+			u16 *number_of_rds_groups
+			);
+
+/**
+ * fmd_start_rds_thread() - Starts the RDS Thread for receiving RDS Data.
+ *
+ * This is started by Application when it wants to receive RDS Data.
+ * @cb_func: Callback function for receiving RDS Data
+ */
+void fmd_start_rds_thread(
+			cg2900_fm_rds_cb cb_func
+			);
+/**
+ * fmd_stop_rds_thread() - Stops the RDS Thread when Application does not
+ * want to receive RDS.
+ */
+void fmd_stop_rds_thread(void);
+
+/**
+ * fmd_get_rds_sem() - Block on RDS Semaphore.
+ * Till irpt_BufferFull is received, RDS Task is blocked.
+ */
+void fmd_get_rds_sem(void);
+
+/**
+ * fmd_set_rds_sem() - Unblock on RDS Semaphore.
+ * on receiving  irpt_BufferFull, RDS Task is un-blocked.
+ */
+void fmd_set_rds_sem(void);
+
+/**
+ * fmd_set_dev() - Set FM device.
+ *
+ * @dev: FM Device
+ *
+ * Returns:
+ *	 0, If there is no error.
+ *	 corresponding error Otherwise
+ */
+int fmd_set_dev(
+			struct device *dev
+			);
+
+/**
+ * fmd_set_test_tone_generator_status()- Sets the Test Tone Generator.
+ *
+ * This function is used to enable/disable the Internal Tone Generator of
+ * CG2900.
+ * @test_tone_status: Status of tone generator.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int fmd_set_test_tone_generator_status(
+			u8 test_tone_status
+			);
+
+/**
+ * fmd_test_tone_connect()- Connect Audio outputs/inputs.
+ *
+ * This function connects the audio outputs/inputs of the
+ * Internal Tone Generator of CG2900.
+ * @left_audio_mode: Left Audio Output Mode.
+ * @right_audio_mode: Right Audio Output Mode.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int fmd_test_tone_connect(
+			u8 left_audio_mode,
+			u8 right_audio_mode
+			);
+
+/**
+ * fmd_test_tone_set_params()- Sets the Test Tone Parameters.
+ *
+ * This function is used to set the parameters of
+ * the Internal Tone Generator of CG2900.
+ * @tone_gen: Tone to be configured (Tone 1 or Tone 2)
+ * @frequency: Frequency of the tone.
+ * @volume: Volume of the tone.
+ * @phase_offset: Phase offset of the tone.
+ * @dc: DC to add to tone.
+ * @waveform: Waveform to generate, sine or pulse.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int fmd_test_tone_set_params(
+			u8 tone_gen,
+			u16 frequency,
+			u16 volume,
+			u16 phase_offset,
+			u16 dc,
+			u8 waveform
+			);
+
+/**
+ * fmd_rx_set_deemphasis()- Connect Audio outputs/inputs.
+ *
+ * This function sets the de-emphasis filter to the
+ * specified de-empahsis level.
+ * @deemphasis: De-emphasis level to set.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int fmd_rx_set_deemphasis(
+			u8 deemphasis
+			);
+#endif /* _FMDRIVER_H_  */
diff --git a/drivers/media/radio/CG2900/radio-cg2900.c b/drivers/media/radio/CG2900/radio-cg2900.c
new file mode 100644
index 00000000000..4fe3e6ffd5c
--- /dev/null
+++ b/drivers/media/radio/CG2900/radio-cg2900.c
@@ -0,0 +1,2855 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Linux Wrapper for V4l2 FM Driver for CG2900.
+ *
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#include<linux/init.h>
+#include<linux/videodev2.h>
+#include"linux/videodev.h"
+#include<media/v4l2-ioctl.h>
+#include<media/v4l2-common.h>
+#include<linux/module.h>
+#include <linux/platform_device.h>
+#include<linux/string.h>
+#include<linux/wait.h>
+#include"cg2900_fm_driver.h"
+
+#define RADIO_CG2900_VERSION KERNEL_VERSION(1, 1, 0)
+#define BANNER "ST-Ericsson FM Radio Card driver v1.1.0"
+
+#define FMR_HZ_TO_MHZ_CONVERTER				1000000
+#define FMR_EU_US_LOW_FREQ_IN_MHZ			87.5
+#define FMR_EU_US_HIGH_FREQ_IN_MHZ			108
+#define FMR_JAPAN_LOW_FREQ_IN_MHZ			76
+#define FMR_JAPAN_HIGH_FREQ_IN_MHZ			90
+#define FMR_CHINA_LOW_FREQ_IN_MHZ			70
+#define FMR_CHINA_HIGH_FREQ_IN_MHZ			108
+#define FMR_MAX_BLOCK_SCAN_CHANNELS			198
+#define FMR_CHINA_GRID_IN_HZ				50000
+#define FMR_EUROPE_GRID_IN_HZ				100000
+#define FMR_USA_GRID_IN_HZ				200000
+#define FMR_AF_SWITCH_DATA_SIZE				2
+#define FMR_BLOCK_SCAN_DATA_SIZE			2
+#define FMR_TEST_TONE_CONNECT_DATA_SIZE			2
+#define FMR_TEST_TONE_SET_PARAMS_DATA_SIZE		6
+
+/* freq in Hz to V4l2 freq (units of 62.5Hz) */
+#define HZ_TO_V4L2(X)  (2*(X)/125)
+/* V4l2 freq (units of 62.5Hz) to freq in Hz */
+#define V4L2_TO_HZ(X)  (((X)*125)/(2))
+
+static int cg2900_open(
+			struct file *file
+			);
+static int cg2900_release(
+			struct file *file
+			);
+static ssize_t cg2900_read(
+			struct file *file,
+			char __user *data,
+			size_t count,
+			loff_t *pos
+			);
+static unsigned int cg2900_poll(
+			struct file *file,
+			struct poll_table_struct *wait
+			);
+static int vidioc_querycap(
+			struct file *file,
+			void *priv,
+			struct v4l2_capability *query_caps
+			);
+static int vidioc_get_tuner(
+			struct file *file,
+			void *priv,
+			struct v4l2_tuner *tuner
+			);
+static int vidioc_set_tuner(
+			struct file *file,
+			void *priv,
+			struct v4l2_tuner *tuner
+			);
+static int vidioc_get_modulator(
+			struct file *file,
+			void *priv,
+			struct v4l2_modulator *modulator
+			);
+static int vidioc_set_modulator(
+			struct file *file,
+			void *priv,
+			struct v4l2_modulator *modulator
+			);
+static int vidioc_get_frequency(
+			struct file *file,
+			void *priv,
+			struct v4l2_frequency *freq
+			);
+static int vidioc_set_frequency(
+			struct file *file,
+			void *priv,
+			struct v4l2_frequency *freq
+			);
+static int vidioc_query_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_queryctrl *query_ctrl
+			);
+static int vidioc_get_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_control *ctrl
+			);
+static int vidioc_set_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_control *ctrl
+			);
+static int vidioc_get_ext_ctrls(
+			struct file *file,
+			void *priv,
+			struct v4l2_ext_controls *ext_ctrl
+			);
+static int vidioc_set_ext_ctrls(
+			struct file *file,
+			void *priv,
+			struct v4l2_ext_controls *ext_ctrl
+			);
+static int vidioc_set_hw_freq_seek(
+			struct file *file,
+			void *priv,
+			struct v4l2_hw_freq_seek *freq_seek
+			);
+static int vidioc_get_audio(
+			struct file *file,
+			void *priv,
+			struct v4l2_audio *audio
+			);
+static int vidioc_set_audio(
+			struct file *file,
+			void *priv,
+			struct v4l2_audio *audio
+			);
+static int vidioc_get_input(
+			struct file *filp,
+			void *priv,
+			unsigned int *input
+			);
+static int vidioc_set_input(
+			struct file *filp,
+			void *priv,
+			unsigned int input
+			);
+static void cg2900_convert_err_to_v4l2(
+			char status_byte,
+			char *out_byte
+			);
+
+static u32 freq_low;
+static u32 freq_high;
+
+/* Module Parameters */
+static int radio_nr = -1;
+static int grid;
+static int band;
+
+/* cg2900_poll_queue - Main Wait Queue for polling (Scan/Seek) */
+static wait_queue_head_t cg2900_poll_queue;
+
+/* cg2900_read_queue - Main Wait Queue for receiving RDS Data */
+static DECLARE_WAIT_QUEUE_HEAD(cg2900_read_queue);
+
+/**
+ * enum fm_seek_status - Seek status of FM Radio.
+ *
+ * @FMR_SEEK_NONE: No seek in progress.
+ * @FMR_SEEK_IN_PROGRESS: Seek is in progress.
+ *
+ * Seek status of FM Radio.
+ */
+enum fm_seek_status {
+	FMR_SEEK_NONE,
+	FMR_SEEK_IN_PROGRESS
+};
+
+/**
+ * enum fm_power_state - Power states of FM Radio.
+ *
+ * @FMR_SWITCH_OFF: FM Radio is switched off.
+ * @FMR_SWITCH_ON: FM Radio is switched on.
+ * @FMR_STANDBY: FM Radio in standby state.
+ *
+ * Power states of FM Radio.
+ */
+enum fm_power_state {
+	FMR_SWITCH_OFF,
+	FMR_SWITCH_ON,
+	FMR_STANDBY
+};
+
+/**
+ * struct cg2900_device - Stores FM Device Info.
+ *
+ * @state: state of FM Radio
+ * @muted: FM Radio Mute/Unmute status
+ * @seekstatus: seek status
+ * @rx_rds_enabled: Rds enable/disable status for FM Rx
+ * @tx_rds_enabled: Rds enable/disable status for FM Tx
+ * @rx_stereo_status: Stereo Mode status for FM Rx
+ * @tx_stereo_status: Stereo Mode status for FM Tx
+ * @volume: Analog Volume Gain of FM Radio
+ * @rssi_threshold: rssi Thresold set on FM Radio
+ * @frequency: Frequency tuned on FM Radio in V4L2 Format
+ * @audiopath: Audio Balance
+ * @wait_on_read_queue: Flag for waiting on read queue.
+ * @fm_mode: Enum for storing the current FM Mode.
+ *
+ * FM Driver Information Structure.
+ */
+struct cg2900_device {
+	u8 state;
+	u8 muted;
+	u8 seekstatus;
+	bool rx_rds_enabled;
+	bool tx_rds_enabled;
+	bool rx_stereo_status;
+	bool tx_stereo_status;
+	int volume;
+	u16 rssi_threshold;
+	u32 frequency;
+	u32 audiopath;
+	bool wait_on_read_queue;
+	enum cg2900_fm_mode fm_mode;
+};
+
+/* Global Structure to store the maintain FM Driver device info */
+static struct cg2900_device cg2900_device;
+
+/* V4l2 File Operation Structure */
+static const struct v4l2_file_operations cg2900_fops = {
+	.owner = THIS_MODULE,
+	.open = cg2900_open,
+	.release = cg2900_release,
+	.read = cg2900_read,
+	.poll = cg2900_poll,
+	.ioctl = video_ioctl2,
+};
+
+/* V4L2 IOCTL Operation Structure */
+static const struct v4l2_ioctl_ops cg2900_ioctl_ops = {
+	.vidioc_querycap = vidioc_querycap,
+	.vidioc_g_tuner = vidioc_get_tuner,
+	.vidioc_s_tuner = vidioc_set_tuner,
+	.vidioc_g_modulator = vidioc_get_modulator,
+	.vidioc_s_modulator = vidioc_set_modulator,
+	.vidioc_g_frequency = vidioc_get_frequency,
+	.vidioc_s_frequency = vidioc_set_frequency,
+	.vidioc_queryctrl = vidioc_query_ctrl,
+	.vidioc_g_ctrl = vidioc_get_ctrl,
+	.vidioc_s_ctrl = vidioc_set_ctrl,
+	.vidioc_g_ext_ctrls = vidioc_get_ext_ctrls,
+	.vidioc_s_ext_ctrls = vidioc_set_ext_ctrls,
+	.vidioc_s_hw_freq_seek = vidioc_set_hw_freq_seek,
+	.vidioc_g_audio = vidioc_get_audio,
+	.vidioc_s_audio = vidioc_set_audio,
+	.vidioc_g_input = vidioc_get_input,
+	.vidioc_s_input = vidioc_set_input,
+};
+
+/* V4L2 Video Device Structure */
+static struct video_device cg2900_video_device = {
+	.name = "STE CG2900 FM Rx/Tx Radio",
+	.vfl_type = VID_TYPE_TUNER | VID_TYPE_CAPTURE,
+	.fops = &cg2900_fops,
+	.ioctl_ops = &cg2900_ioctl_ops,
+	.release = video_device_release_empty,
+};
+
+static u16 no_of_scan_freq;
+static u16 no_of_block_scan_freq;
+static u32 scanfreq_rssi_level[MAX_CHANNELS_TO_SCAN];
+static u16 block_scan_rssi_level[MAX_CHANNELS_FOR_BLOCK_SCAN];
+static u32 scanfreq[MAX_CHANNELS_TO_SCAN];
+static struct mutex fm_mutex;
+static spinlock_t fm_spinlock;
+static int users;
+
+/**
+ * vidioc_querycap()- Query FM Driver Capabilities.
+ *
+ * This function is used to query the capabilities of the
+ * FM Driver. This function is called when the application issues the IOCTL
+ * VIDIOC_QUERYCAP.
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @query_caps: v4l2_capability structure.
+ *
+ * Returns: 0
+ */
+static int vidioc_querycap(
+			struct file *file,
+			void *priv,
+			struct v4l2_capability *query_caps
+			)
+{
+	FM_INFO_REPORT("vidioc_querycap");
+	memset(
+			query_caps,
+			0,
+			sizeof(*query_caps)
+			);
+	strlcpy(
+			query_caps->driver,
+			"CG2900 Driver",
+			sizeof(query_caps->driver)
+			);
+	strlcpy(
+			query_caps->card,
+			"CG2900 FM Radio",
+			sizeof(query_caps->card)
+			);
+	strcpy(
+			query_caps->bus_info,
+			"platform"
+			);
+	query_caps->version = RADIO_CG2900_VERSION;
+	query_caps->capabilities =
+			V4L2_CAP_TUNER |
+			V4L2_CAP_MODULATOR |
+			V4L2_CAP_RADIO  |
+			V4L2_CAP_READWRITE |
+			V4L2_CAP_RDS_CAPTURE |
+			V4L2_CAP_HW_FREQ_SEEK |
+			V4L2_CAP_RDS_OUTPUT;
+	FM_DEBUG_REPORT("vidioc_querycap returning 0");
+	return 0;
+}
+
+/**
+ * vidioc_get_tuner()- Get FM Tuner Features.
+ *
+ * This function is used to get the tuner features.
+ * This function is called when the application issues the IOCTL
+ * VIDIOC_G_TUNER
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @tuner: v4l2_tuner structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_tuner(
+			struct file *file,
+			void *priv,
+			struct v4l2_tuner *tuner
+			)
+{
+	int status = 0;
+	u8 mode;
+	bool rds_enabled;
+	u16 rssi;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_get_tuner");
+
+	if (tuner->index > 0) {
+		FM_ERR_REPORT("vidioc_get_tuner: Only 1 tuner supported");
+		goto error;
+	}
+
+	memset(tuner, 0, sizeof(*tuner));
+	strcpy(tuner->name, "CG2900 FM Receiver");
+	tuner->type = V4L2_TUNER_RADIO;
+	tuner->rangelow = HZ_TO_V4L2(freq_low);
+	tuner->rangehigh = HZ_TO_V4L2(freq_high);
+	tuner->capability =
+			V4L2_TUNER_CAP_LOW /* Frequency steps = 1/16 kHz */
+			| V4L2_TUNER_CAP_STEREO	/* Can receive stereo */
+			| V4L2_TUNER_CAP_RDS;	/* Supports RDS Capture  */
+
+	if (cg2900_device.fm_mode == CG2900_FM_RX_MODE) {
+
+		status = cg2900_fm_get_mode(&mode);
+
+		FM_DEBUG_REPORT("vidioc_get_tuner: mode = %d, ", mode);
+
+		if (0 != status) {
+			/* Get mode API failed, set mode to mono */
+			tuner->audmode = V4L2_TUNER_MODE_MONO;
+			tuner->rxsubchans = V4L2_TUNER_SUB_MONO;
+			goto error;
+		}
+
+		switch (mode) {
+		case CG2900_MODE_STEREO:
+			tuner->audmode = V4L2_TUNER_MODE_STEREO;
+			tuner->rxsubchans = V4L2_TUNER_SUB_STEREO;
+			break;
+		case CG2900_MODE_MONO:
+		default:
+			tuner->audmode = V4L2_TUNER_MODE_MONO;
+			tuner->rxsubchans = V4L2_TUNER_SUB_MONO;
+			break;
+		}
+
+		status = cg2900_fm_get_rds_status(&rds_enabled);
+
+		if (0 != status) {
+			tuner->rxsubchans &= ~V4L2_TUNER_SUB_RDS;
+			goto error;
+		}
+
+		if (rds_enabled)
+			tuner->rxsubchans |= V4L2_TUNER_SUB_RDS;
+		else
+			tuner->rxsubchans &= ~V4L2_TUNER_SUB_RDS;
+	} else {
+		tuner->audmode = V4L2_TUNER_MODE_MONO;
+		tuner->rxsubchans = V4L2_TUNER_SUB_MONO;
+	}
+
+	if (cg2900_device.fm_mode == CG2900_FM_RX_MODE) {
+		status = cg2900_fm_get_signal_strength(&rssi);
+
+		if (0 != status) {
+			tuner->signal = 0;
+			goto error;
+		}
+		tuner->signal = rssi;
+	} else {
+		tuner->signal = 0;
+	}
+
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_get_tuner: returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_set_tuner()- Set FM Tuner Features.
+ *
+ * This function is used to set the tuner features.
+ * It also sets the default FM Rx settings.
+ * This function is called when the application issues the IOCTL
+ * VIDIOC_S_TUNER
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @tuner: v4l2_tuner structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_tuner(
+			struct file *file,
+			void *priv,
+			struct v4l2_tuner *tuner
+			)
+{
+	bool rds_status = false;
+	bool stereo_status = false;
+	int status = 0;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_set_tuner");
+	if (tuner->index != 0) {
+		FM_ERR_REPORT("vidioc_set_tuner: Only 1 tuner supported");
+		goto error;
+	}
+
+	if (cg2900_device.fm_mode != CG2900_FM_RX_MODE) {
+		/*
+		 * FM Rx mode should be configured
+		 * as earlier mode was not FM Rx
+		 */
+		if (CG2900_FM_BAND_US_EU == band) {
+			freq_low = FMR_EU_US_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_EU_US_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		} else if (CG2900_FM_BAND_JAPAN == band) {
+			freq_low = FMR_JAPAN_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_JAPAN_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		} else if (CG2900_FM_BAND_CHINA == band) {
+			freq_low = FMR_CHINA_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_CHINA_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		}
+		cg2900_device.fm_mode = CG2900_FM_RX_MODE;
+		cg2900_device.rx_rds_enabled =
+		    (tuner->rxsubchans & V4L2_TUNER_SUB_RDS) ?
+		    true : false;
+		if (tuner->rxsubchans & V4L2_TUNER_SUB_STEREO)
+			stereo_status = true;
+		else if (tuner->rxsubchans & V4L2_TUNER_SUB_MONO)
+			stereo_status = false;
+		cg2900_device.rx_stereo_status = stereo_status;
+		status = cg2900_fm_set_rx_default_settings(freq_low,
+				band,
+				grid,
+				cg2900_device.rx_rds_enabled,
+				cg2900_device.rx_stereo_status);
+
+		if (0 != status) {
+			FM_ERR_REPORT("vidioc_set_tuner: "
+				"cg2900_fm_set_rx_default_settings returned "
+				" %d", status);
+			goto error;
+		}
+		status = cg2900_fm_set_rssi_threshold(
+				cg2900_device.rssi_threshold);
+		if (0 != status) {
+			FM_ERR_REPORT("vidioc_set_tuner: "
+					"cg2900_fm_set_rssi_threshold returned "
+					" %d", status);
+			goto error;
+		}
+	} else {
+		/*
+		 * Mode was FM Rx only, change the RDS settings or stereo mode
+		 * if they are changed by application
+		 */
+		rds_status = (tuner->rxsubchans & V4L2_TUNER_SUB_RDS) ?
+		    true : false;
+		if (tuner->rxsubchans & V4L2_TUNER_SUB_STEREO)
+			stereo_status = true;
+		else if (tuner->rxsubchans & V4L2_TUNER_SUB_MONO)
+			stereo_status = false;
+		if (stereo_status != cg2900_device.rx_stereo_status) {
+			cg2900_device.rx_stereo_status = stereo_status;
+			if (stereo_status)
+				status =
+				    cg2900_fm_set_mode(
+						FMD_STEREOMODE_BLENDING);
+			else
+				status = cg2900_fm_set_mode(
+						FMD_STEREOMODE_MONO);
+
+			if (0 != status) {
+				FM_ERR_REPORT("vidioc_set_tuner: "
+						"cg2900_fm_set_mode returned "
+						" %d", status);
+				goto error;
+			}
+		}
+		if (rds_status != cg2900_device.rx_rds_enabled) {
+			cg2900_device.rx_rds_enabled = rds_status;
+			if (rds_status)
+				status = cg2900_fm_rds_on();
+			else
+				status = cg2900_fm_rds_off();
+
+			if (0 != status) {
+				FM_ERR_REPORT("vidioc_set_tuner: "
+						"cg2900_fm_rds returned "
+						" %d", status);
+				goto error;
+			}
+		}
+	}
+
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_set_tuner: returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_get_modulator()- Get FM Modulator Features.
+ *
+ * This function is used to get the modulator features.
+ * This function is called when the application issues the IOCTL
+ * VIDIOC_G_MODULATOR
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @modulator: v4l2_modulator structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_modulator(
+			struct file *file,
+			void *priv,
+			struct v4l2_modulator *modulator
+			)
+{
+	int status = 0;
+	bool rds_enabled;
+	u8 mode;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_get_modulator");
+
+	if (modulator->index > 0) {
+		FM_ERR_REPORT("vidioc_get_modulator: Only 1 "
+			      "modulator supported");
+		goto error;
+	}
+
+	memset(modulator, 0, sizeof(*modulator));
+	strcpy(modulator->name, "CG2900 FM Transmitter");
+	modulator->rangelow = freq_low;
+	modulator->rangehigh = freq_high;
+	modulator->capability = V4L2_TUNER_CAP_NORM /* Freq steps = 1/16 kHz */
+	    | V4L2_TUNER_CAP_STEREO	/* Can receive stereo */
+	    | V4L2_TUNER_CAP_RDS;	/* Supports RDS Capture  */
+
+	if (cg2900_device.fm_mode == CG2900_FM_TX_MODE) {
+		status = cg2900_fm_get_mode(&mode);
+		FM_DEBUG_REPORT("vidioc_get_modulator: mode = %d", mode);
+		if (0 != status) {
+			/* Get mode API failed, set mode to mono */
+			modulator->txsubchans = V4L2_TUNER_SUB_MONO;
+			goto error;
+		}
+		switch (mode) {
+			/* Stereo */
+		case CG2900_MODE_STEREO:
+			modulator->txsubchans = V4L2_TUNER_SUB_STEREO;
+			break;
+			/* Mono */
+		case CG2900_MODE_MONO:
+			modulator->txsubchans = V4L2_TUNER_SUB_MONO;
+			break;
+			/* Switching or Blending, set mode as Stereo */
+		default:
+			modulator->txsubchans = V4L2_TUNER_SUB_STEREO;
+		}
+		status = cg2900_fm_get_rds_status(&rds_enabled);
+		if (0 != status) {
+			modulator->txsubchans &= ~V4L2_TUNER_SUB_RDS;
+			goto error;
+		}
+		if (rds_enabled)
+			modulator->txsubchans |= V4L2_TUNER_SUB_RDS;
+		else
+			modulator->txsubchans &= ~V4L2_TUNER_SUB_RDS;
+	} else
+		modulator->txsubchans = V4L2_TUNER_SUB_MONO;
+
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_get_modulator: returning %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_set_modulator()- Set FM Modulator Features.
+ *
+ * This function is used to set the Modulaotr features.
+ * It also sets the default FM Tx settings.
+ * This function is called when the application issues the IOCTL
+ * VIDIOC_S_MODULATOR
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @modulator: v4l2_modulator structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_modulator(
+			struct file *file,
+			void *priv,
+			struct v4l2_modulator *modulator
+			)
+{
+	bool rds_status = false;
+	bool stereo_status = false;
+	int status = 0;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_set_modulator");
+	if (modulator->index != 0) {
+		FM_ERR_REPORT("vidioc_set_modulator: Only 1 "
+			      "modulator supported");
+		goto error;
+	}
+
+	if (cg2900_device.fm_mode != CG2900_FM_TX_MODE) {
+		/*
+		 * FM Tx mode should be configured as
+		 * earlier mode was not FM Tx
+		 */
+		if (band == CG2900_FM_BAND_US_EU) {
+			freq_low = FMR_EU_US_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_EU_US_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		} else if (band == CG2900_FM_BAND_JAPAN) {
+			freq_low = FMR_JAPAN_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_JAPAN_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		} else if (band == CG2900_FM_BAND_CHINA) {
+			freq_low = FMR_CHINA_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_CHINA_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		}
+		cg2900_device.fm_mode = CG2900_FM_TX_MODE;
+		cg2900_device.rx_rds_enabled = false;
+		cg2900_device.tx_rds_enabled =
+		    (modulator->txsubchans & V4L2_TUNER_SUB_RDS) ?
+		    true : false;
+		if (modulator->txsubchans & V4L2_TUNER_SUB_STEREO)
+			stereo_status = true;
+		else if (modulator->txsubchans & V4L2_TUNER_SUB_MONO)
+			stereo_status = false;
+		cg2900_device.tx_stereo_status = stereo_status;
+
+		status = cg2900_fm_set_tx_default_settings(freq_low,
+					       band,
+					       grid,
+					       cg2900_device.tx_rds_enabled,
+					       cg2900_device.
+					       tx_stereo_status);
+
+		if (0 != status) {
+			FM_ERR_REPORT("vidioc_set_modulator: "
+				"cg2900_fm_set_tx_default_settings returned "
+				" %d", status);
+			goto error;
+		}
+	} else {
+		/*
+		 * Mode was FM Tx only, change the RDS settings or stereo mode
+		 * if they are changed by application
+		 */
+		rds_status = (modulator->txsubchans & V4L2_TUNER_SUB_RDS) ?
+		    true : false;
+		if (modulator->txsubchans & V4L2_TUNER_SUB_STEREO)
+			stereo_status = true;
+		else if (modulator->txsubchans & V4L2_TUNER_SUB_MONO)
+			stereo_status = false;
+		if (stereo_status != cg2900_device.tx_stereo_status) {
+			cg2900_device.tx_stereo_status = stereo_status;
+			status = cg2900_fm_set_mode(stereo_status);
+			if (0 != status) {
+				FM_ERR_REPORT("vidioc_set_modulator: "
+						"cg2900_fm_set_mode returned "
+						" %d", status);
+				goto error;
+			}
+		}
+		if (rds_status != cg2900_device.tx_rds_enabled) {
+			cg2900_device.tx_rds_enabled = rds_status;
+			status = cg2900_fm_tx_rds(rds_status);
+			if (0 != status) {
+				FM_ERR_REPORT("vidioc_set_modulator: "
+						"cg2900_fm_tx_rds returned "
+						" %d", status);
+				goto error;
+			}
+		}
+	}
+
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_set_modulator: returning %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_get_frequency()- Get the Current FM Frequnecy.
+ *
+ * This function is used to get the currently tuned
+ * frequency on FM Radio. This function is called when the application
+ * issues the IOCTL VIDIOC_G_FREQUENCY
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @freq: v4l2_frequency structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_frequency(
+			struct file *file,
+			void *priv,
+			struct v4l2_frequency *freq
+			)
+{
+	int status;
+	u32 frequency;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_get_frequency: Status = %d",
+			cg2900_device.seekstatus);
+
+	status = cg2900_fm_get_frequency(&frequency);
+
+	if (0 != status) {
+		freq->frequency = cg2900_device.frequency;
+		goto error;
+	}
+
+	if (cg2900_device.seekstatus == FMR_SEEK_IN_PROGRESS) {
+		/*  Check if seek is finished or not */
+		if (CG2900_EVENT_SEARCH_CHANNEL_FOUND == fm_event) {
+			/* seek is finished */
+			spin_lock(&fm_spinlock);
+			cg2900_device.frequency = HZ_TO_V4L2(frequency);
+			freq->frequency = cg2900_device.frequency;
+			cg2900_device.seekstatus = FMR_SEEK_NONE;
+			fm_event = CG2900_EVENT_NO_EVENT;
+			spin_unlock(&fm_spinlock);
+		}
+	} else {
+		spin_lock(&fm_spinlock);
+		cg2900_device.frequency = HZ_TO_V4L2(frequency);
+		freq->frequency = cg2900_device.frequency;
+		spin_unlock(&fm_spinlock);
+	}
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_get_frequency: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_set_frequency()- Set the FM Frequnecy.
+ *
+ * This function is used to set the frequency
+ * on FM Radio. This function is called when the application
+ * issues the IOCTL VIDIOC_S_FREQUENCY
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @freq: v4l2_frequency structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_frequency(
+			struct file *file,
+			void *priv,
+			struct v4l2_frequency *freq
+			)
+{
+	u32 frequency = freq->frequency;
+	u32 freq_low, freq_high;
+	int status;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_set_frequency: Frequency = "
+			"%d ", V4L2_TO_HZ(frequency));
+
+	/*  Check which band is set currently */
+	switch (band) {
+	case CG2900_FM_BAND_US_EU:
+		freq_low = FMR_EU_US_LOW_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		freq_high = FMR_EU_US_HIGH_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		break;
+
+	case CG2900_FM_BAND_CHINA:
+		freq_low = FMR_CHINA_LOW_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		freq_high = FMR_CHINA_HIGH_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		break;
+
+	case CG2900_FM_BAND_JAPAN:
+		freq_low = FMR_JAPAN_LOW_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		freq_high = FMR_JAPAN_HIGH_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		break;
+
+	default:
+		/* Set to US_MAX and CHINA_MIN band */
+		freq_low = FMR_CHINA_LOW_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		freq_high = FMR_EU_US_HIGH_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+	}
+
+	/* Check if the frequency set is out of current band */
+	if ((V4L2_TO_HZ(frequency) < freq_low) ||
+		(V4L2_TO_HZ(frequency) > freq_high))
+		goto error;
+
+	spin_lock(&fm_spinlock);
+	fm_event = CG2900_EVENT_NO_EVENT;
+	no_of_scan_freq = 0;
+	spin_unlock(&fm_spinlock);
+
+	cg2900_device.seekstatus = FMR_SEEK_NONE;
+	cg2900_device.frequency = frequency;
+	status = cg2900_fm_set_frequency(V4L2_TO_HZ(frequency));
+
+	if (0 != status)
+		goto error;
+
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_set_frequency: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_query_ctrl()- Query the FM Driver control features.
+ *
+ * This function is used to query the control features on FM Radio.
+ * This function is called when the application
+ * issues the IOCTL VIDIOC_QUERYCTRL
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @query_ctrl: v4l2_queryctrl structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_query_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_queryctrl *query_ctrl
+			)
+{
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_query_ctrl");
+	/* Check which control is requested */
+	switch (query_ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		FM_DEBUG_REPORT("vidioc_query_ctrl:  V4L2_CID_AUDIO_MUTE");
+		query_ctrl->type = V4L2_CTRL_TYPE_BOOLEAN;
+		query_ctrl->minimum = 0;
+		query_ctrl->maximum = 1;
+		query_ctrl->step = 1;
+		query_ctrl->default_value = 0;
+		query_ctrl->flags = 0;
+		strncpy(query_ctrl->name, "CG2900 Mute", 32);
+		ret_val = 0;
+		break;
+
+	case V4L2_CID_AUDIO_VOLUME:
+		FM_DEBUG_REPORT("vidioc_query_ctrl: V4L2_CID_AUDIO_VOLUME");
+
+		strncpy(query_ctrl->name, "CG2900 Volume", 32);
+		query_ctrl->minimum = MIN_ANALOG_VOLUME;
+		query_ctrl->maximum = MAX_ANALOG_VOLUME;
+		query_ctrl->step = 1;
+		query_ctrl->default_value = MAX_ANALOG_VOLUME;
+		query_ctrl->flags = 0;
+		query_ctrl->type = V4L2_CTRL_TYPE_INTEGER;
+		ret_val = 0;
+		break;
+
+	case V4L2_CID_AUDIO_BALANCE:
+		FM_DEBUG_REPORT("vidioc_query_ctrl:   V4L2_CID_AUDIO_BALANCE ");
+		strncpy(query_ctrl->name, "CG2900 Audio Balance", 32);
+		query_ctrl->type = V4L2_CTRL_TYPE_INTEGER;
+		query_ctrl->minimum = 0x0000;
+		query_ctrl->maximum = 0xFFFF;
+		query_ctrl->step = 0x0001;
+		query_ctrl->default_value = 0x0000;
+		query_ctrl->flags = 0;
+		ret_val = 0;
+		break;
+
+	case V4L2_CID_AUDIO_BASS:
+		FM_DEBUG_REPORT("vidioc_query_ctrl: "
+				"V4L2_CID_AUDIO_BASS (unsupported)");
+		break;
+
+	case V4L2_CID_AUDIO_TREBLE:
+		FM_DEBUG_REPORT("vidioc_query_ctrl: "
+				"V4L2_CID_AUDIO_TREBLE (unsupported)");
+		break;
+
+	default:
+		FM_DEBUG_REPORT("vidioc_query_ctrl: "
+				"--> unsupported id = %d", (int)query_ctrl->id);
+		break;
+	}
+
+	FM_DEBUG_REPORT("vidioc_query_ctrl: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_get_ctrl()- Get the value of a particular Control.
+ *
+ * This function is used to get the value of a
+ * particular control from the FM Driver. This function is called
+ * when the application issues the IOCTL VIDIOC_G_CTRL
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @ctrl: v4l2_control structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_control *ctrl
+			)
+{
+	int status;
+	u8 value;
+	u16 rssi;
+	u8 antenna;
+	u16 conclusion;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_get_ctrl");
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_VOLUME:
+		status = cg2900_fm_get_volume(&value);
+		if (0 == status) {
+			ctrl->value = value;
+			cg2900_device.volume = value;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_AUDIO_MUTE:
+		ctrl->value = cg2900_device.muted;
+		ret_val = 0;
+		break;
+	case V4L2_CID_AUDIO_BALANCE:
+		ctrl->value = cg2900_device.audiopath;
+		ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_RSSI_THRESHOLD:
+		ctrl->value = cg2900_device.rssi_threshold;
+		ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_SELECT_ANTENNA:
+		status = cg2900_fm_get_antenna(&antenna);
+		FM_DEBUG_REPORT("vidioc_get_ctrl: Antenna = %d", antenna);
+		if (0 == status) {
+			ctrl->value = antenna;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_GET_RESULT:
+		status = cg2900_fm_af_update_get_result(&rssi);
+		FM_DEBUG_REPORT("vidioc_get_ctrl: AF RSSI Level = %d", rssi);
+		if (0 == status) {
+			ctrl->value = rssi;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_GET_RESULT:
+		status = cg2900_fm_af_switch_get_result(&conclusion);
+		FM_DEBUG_REPORT("vidioc_get_ctrl: AF Switch conclusion = %d",
+				conclusion);
+		if (0 != status)
+			break;
+		if (conclusion == 0) {
+			ctrl->value = conclusion;
+			FM_DEBUG_REPORT("vidioc_get_ctrl: "
+					"AF Switch conclusion = %d",
+					ctrl->value);
+			ret_val = 0;
+		} else {
+			/*
+			 * Convert positive error code returned by chip
+			 * into negative error codes to be in line with linux.
+			 */
+			ctrl->value = -conclusion;
+			FM_ERR_REPORT("vidioc_get_ctrl: "
+			"AF-Switch failed with value %d", (__s32)ctrl->value);
+			ret_val = 0;
+		}
+		break;
+	default:
+		FM_DEBUG_REPORT("vidioc_get_ctrl: "
+				"unsupported (id = %d)", (int)ctrl->id);
+		ret_val = -EINVAL;
+	}
+	FM_DEBUG_REPORT("vidioc_get_ctrl: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_set_ctrl()- Set the value of a particular Control.
+ *
+ * This function is used to set the value of a
+ * particular control from the FM Driver. This function is called when the
+ * application issues the IOCTL VIDIOC_S_CTRL
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @ctrl: v4l2_control structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -ERANGE when the parameter is out of range.
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_control *ctrl
+			)
+{
+	int status;
+	int ret_val = -EINVAL;
+	FM_INFO_REPORT("vidioc_set_ctrl");
+	/* Check which control is requested */
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_AUDIO_MUTE, "
+				"value = %d", ctrl->value);
+		if (ctrl->value > 1 && ctrl->value < 0) {
+			ret_val =  -ERANGE;
+			break;
+		}
+
+		if (ctrl->value) {
+			FM_DEBUG_REPORT("vidioc_set_ctrl: Ctrl_Id = "
+					"V4L2_CID_AUDIO_MUTE, "
+					"Muting the Radio");
+			status = cg2900_fm_mute();
+		} else {
+			FM_DEBUG_REPORT("vidioc_set_ctrl: "
+					"Ctrl_Id = V4L2_CID_AUDIO_MUTE, "
+					"UnMuting the Radio");
+			status = cg2900_fm_unmute();
+		}
+		if (0 == status) {
+			cg2900_device.muted = ctrl->value;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_AUDIO_VOLUME:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_AUDIO_VOLUME, "
+				"value = %d", ctrl->value);
+		if (ctrl->value > MAX_ANALOG_VOLUME &&
+				ctrl->value < MIN_ANALOG_VOLUME) {
+			ret_val = -ERANGE;
+			break;
+		}
+		status = cg2900_fm_set_volume(ctrl->value);
+		if (0 == status) {
+			cg2900_device.volume = ctrl->value;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_AUDIO_BALANCE:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_AUDIO_BALANCE, "
+				"value = %d", ctrl->value);
+		status = cg2900_fm_set_audio_balance(ctrl->value);
+		if (0 == status) {
+			cg2900_device.audiopath = ctrl->value;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_CG2900_RADIO_CHIP_STATE:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_CG2900_RADIO_CHIP_STATE, "
+				"value = %d", ctrl->value);
+		if (V4L2_CG2900_RADIO_STANDBY == ctrl->value)
+			status = cg2900_fm_standby();
+		else if (V4L2_CG2900_RADIO_POWERUP == ctrl->value)
+			status = cg2900_fm_power_up_from_standby();
+		else
+			break;
+		if (0 != status)
+			break;
+		if (V4L2_CG2900_RADIO_STANDBY == ctrl->value)
+			cg2900_device.state = FMR_STANDBY;
+		else if (V4L2_CG2900_RADIO_POWERUP == ctrl->value)
+			cg2900_device.state = FMR_SWITCH_ON;
+		ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_SELECT_ANTENNA:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_CG2900_RADIO_SELECT_ANTENNA, "
+				"value = %d", ctrl->value);
+		status = cg2900_fm_select_antenna(ctrl->value);
+		if (0 == status)
+			ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_BANDSCAN:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_CG2900_RADIO_BANDSCAN, "
+				"value = %d", ctrl->value);
+		if (V4L2_CG2900_RADIO_BANDSCAN_START == ctrl->value) {
+			cg2900_device.seekstatus = FMR_SEEK_IN_PROGRESS;
+			no_of_scan_freq = 0;
+			status = cg2900_fm_start_band_scan();
+		} else if (V4L2_CG2900_RADIO_BANDSCAN_STOP == ctrl->value) {
+			status = cg2900_fm_stop_scan();
+			cg2900_device.seekstatus = FMR_SEEK_NONE;
+		} else
+			break;
+		if (0 == status)
+			ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_RSSI_THRESHOLD:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_CG2900_RADIO_RSSI_THRESHOLD "
+				"= %d", ctrl->value);
+		status = cg2900_fm_set_rssi_threshold(ctrl->value);
+		if (0 == status) {
+			cg2900_device.rssi_threshold = ctrl->value;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_START:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_START "
+				"freq = %d Hz", ctrl->value);
+		status = cg2900_fm_af_update_start(ctrl->value);
+		if (0 == status)
+			ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_TEST_TONE_GENERATOR_SET_STATUS:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+			"V4L2_CID_CG2900_RADIO_TEST_TONE_GENERATOR_SET_STATUS "
+			"state = %d ", ctrl->value);
+		if (ctrl->value < V4L2_CG2900_RADIO_TEST_TONE_GEN_OFF ||
+			ctrl->value > V4L2_CG2900_RADIO_TEST_TONE_GENERATOR_ON_WO_SRC) {
+			FM_ERR_REPORT("Invalid parameter = %d", ctrl->value);
+			break;
+		}
+		status = cg2900_fm_set_test_tone_generator(ctrl->value);
+		if (0 == status)
+			ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_TUNE_DEEMPHASIS:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+			"V4L2_CID_CG2900_RADIO_TUNE_DEEMPHASIS, "
+			"Value = %d",
+			ctrl->value);
+
+		if ((V4L2_CG2900_RADIO_DEEMPHASIS_DISABLED >
+			ctrl->value) ||
+			(V4L2_CG2900_RADIO_DEEMPHASIS_75_uS <
+			ctrl->value)) {
+			FM_ERR_REPORT("Unsupported deemphasis = %d",
+			ctrl->value);
+			break;
+		}
+
+		switch (ctrl->value) {
+		case V4L2_CG2900_RADIO_DEEMPHASIS_50_uS:
+			ctrl->value = FMD_EMPHASIS_50US;
+			break;
+		case V4L2_CG2900_RADIO_DEEMPHASIS_75_uS:
+			ctrl->value = FMD_EMPHASIS_75US;
+			break;
+		case V4L2_CG2900_RADIO_DEEMPHASIS_DISABLED:
+			/* Drop Down */
+		default:
+			ctrl->value = FMD_EMPHASIS_NONE;
+			break;
+		}
+		status = cg2900_fm_rx_set_deemphasis(ctrl->value);
+
+		if (0 == status)
+			ret_val = 0;
+		break;
+	default:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"unsupported (id = %d)", (int)ctrl->id);
+	}
+	FM_DEBUG_REPORT("vidioc_set_ctrl: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_get_ext_ctrls()- Get the values of a particular control.
+ *
+ * This function is used to get the value of a
+ * particular control from the FM Driver. This is used when the data to
+ * be received is more than 1 paramter. This function is called when the
+ * application issues the IOCTL VIDIOC_G_EXT_CTRLS
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @ext_ctrl: v4l2_ext_controls structure.
+ *
+ * Returns:
+ *   0 when no error
+ *  -ENOSPC: when there is no space to copy the data into the buffer provided
+ * by application.
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_ext_ctrls(
+			struct file *file,
+			void *priv,
+			struct v4l2_ext_controls *ext_ctrl
+			)
+{
+	u32 *dest_buffer;
+	int index = 0;
+	int count = 0;
+	int ret_val = -EINVAL;
+	int status;
+
+	FM_INFO_REPORT("vidioc_get_ext_ctrls: Id = %04x,"
+			"ext_ctrl->ctrl_class = %04x",
+			ext_ctrl->controls->id,
+			ext_ctrl->ctrl_class);
+
+	if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_FM_TX &&
+	    ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+		FM_ERR_REPORT("vidioc_get_ext_ctrls: Unsupported "
+			      "ctrl_class = %04x", ext_ctrl->ctrl_class);
+		goto error;
+	}
+
+	switch (ext_ctrl->controls->id) {
+	case V4L2_CID_CG2900_RADIO_BANDSCAN_GET_RESULTS:
+		{
+			if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+				FM_ERR_REPORT("vidioc_get_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_BANDSCAN_GET_RESULTS "
+				"Unsupported ctrl_class = %04x",
+				ext_ctrl->ctrl_class);
+				break;
+			}
+			if (cg2900_device.seekstatus ==
+				FMR_SEEK_IN_PROGRESS) {
+				if (fm_event ==
+				    CG2900_EVENT_SCAN_CHANNELS_FOUND) {
+					/* Check to get Scan Result */
+					status =
+					    cg2900_fm_get_scan_result
+					    (&no_of_scan_freq, scanfreq,
+					     scanfreq_rssi_level);
+					if (0 != status) {
+						FM_ERR_REPORT
+						    ("vidioc_get_ext_ctrls: "
+						     "cg2900_fm_get_scan_"
+						     "result: returned %d",
+						     status);
+						break;
+					}
+				}
+			}
+			FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+					"SeekStatus = %d, GlobalEvent = %d, "
+					"numchannels = %d",
+					cg2900_device.seekstatus,
+					fm_event, no_of_scan_freq);
+
+			if (ext_ctrl->controls->size == 0 &&
+			    ext_ctrl->controls->string == NULL) {
+				if (cg2900_device.seekstatus ==
+				    FMR_SEEK_IN_PROGRESS &&
+				    CG2900_EVENT_SCAN_CHANNELS_FOUND
+				    == fm_event) {
+					spin_lock(&fm_spinlock);
+					ext_ctrl->controls->size =
+					    no_of_scan_freq;
+					cg2900_device.seekstatus
+					    = FMR_SEEK_NONE;
+					fm_event =
+					    CG2900_EVENT_NO_EVENT;
+					spin_unlock(&fm_spinlock);
+					return -ENOSPC;
+				}
+			} else if (ext_ctrl->controls->string != NULL) {
+				dest_buffer =
+				    (u32 *) ext_ctrl->controls->string;
+				while (index < no_of_scan_freq) {
+					*(dest_buffer + count + 0) =
+					    HZ_TO_V4L2(scanfreq[index]);
+					*(dest_buffer + count + 1) =
+					    scanfreq_rssi_level[index];
+					count += 2;
+					index++;
+				}
+				ret_val = 0;
+			}
+			break;
+		}
+	case V4L2_CID_CG2900_RADIO_BLOCKSCAN_GET_RESULTS:
+		{
+			if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+				FM_ERR_REPORT("vidioc_get_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_BLOCKSCAN_GET_RESULTS "
+				"Unsupported ctrl_class = %04x",
+				ext_ctrl->ctrl_class);
+				break;
+			}
+			if (cg2900_device.seekstatus ==
+				FMR_SEEK_IN_PROGRESS) {
+				if (fm_event ==
+				    CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND) {
+					/* Check to get BlockScan Result */
+					status =
+					    cg2900_fm_get_block_scan_result
+					    (&no_of_block_scan_freq,
+					     block_scan_rssi_level);
+					if (0 != status) {
+						FM_ERR_REPORT
+						    ("vidioc_get_ext_ctrls: "
+						     "cg2900_fm_get_block_scan_"
+						     "result: " "returned %d",
+						     status);
+						return ret_val;
+					}
+				}
+			}
+			FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+					"SeekStatus = %d, GlobalEvent = %d, "
+					"numchannels = %d",
+					cg2900_device.seekstatus,
+					fm_event, no_of_block_scan_freq);
+			if (ext_ctrl->controls->size == 0 &&
+			    ext_ctrl->controls->string == NULL) {
+				if (cg2900_device.seekstatus ==
+				    FMR_SEEK_IN_PROGRESS &&
+				    CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND
+				    == fm_event) {
+					spin_lock(&fm_spinlock);
+					ext_ctrl->controls->size =
+					    no_of_block_scan_freq;
+					cg2900_device.seekstatus
+					    = FMR_SEEK_NONE;
+					fm_event =
+					    CG2900_EVENT_NO_EVENT;
+					spin_unlock(&fm_spinlock);
+					return -ENOSPC;
+				}
+			} else if (ext_ctrl->controls->size >=
+				   no_of_block_scan_freq &&
+				   ext_ctrl->controls->string != NULL) {
+				dest_buffer =
+				    (u32 *) ext_ctrl->controls->string;
+				while (index < no_of_block_scan_freq) {
+					*(dest_buffer + index) =
+					    block_scan_rssi_level
+					    [index];
+					index++;
+				}
+				ret_val = 0;
+				return ret_val;
+			}
+			break;
+		}
+	case V4L2_CID_RDS_TX_DEVIATION:
+		{
+			FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+					"V4L2_CID_RDS_TX_DEVIATION");
+			if (V4L2_CTRL_CLASS_FM_TX != ext_ctrl->ctrl_class) {
+				FM_ERR_REPORT("Invalid Ctrl Class = %d",
+					      ext_ctrl->ctrl_class);
+				break;
+			}
+			status = cg2900_fm_tx_get_rds_deviation((u16 *) &
+							     ext_ctrl->
+							     controls->value);
+			if (status == 0)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_PILOT_TONE_ENABLED:
+		{
+			FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+					"V4L2_CID_PILOT_TONE_ENABLED");
+			if (V4L2_CTRL_CLASS_FM_TX != ext_ctrl->ctrl_class) {
+				FM_ERR_REPORT("Invalid Ctrl Class = %d",
+					      ext_ctrl->ctrl_class);
+				break;
+			}
+			status = cg2900_fm_tx_get_pilot_tone_status(
+					(bool *)&ext_ctrl->controls->value);
+			if (status == 0)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_PILOT_TONE_DEVIATION:
+		{
+			FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+					"V4L2_CID_PILOT_TONE_DEVIATION");
+			if (V4L2_CTRL_CLASS_FM_TX != ext_ctrl->ctrl_class) {
+				FM_ERR_REPORT("Invalid Ctrl Class = %d",
+					      ext_ctrl->ctrl_class);
+				break;
+			}
+			status = cg2900_fm_tx_get_pilot_deviation(
+					(u16 *)&ext_ctrl->controls->value);
+			if (status == 0)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_TUNE_PREEMPHASIS:
+		{
+			FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+					"V4L2_CID_TUNE_PREEMPHASIS");
+			if (V4L2_CTRL_CLASS_FM_TX != ext_ctrl->ctrl_class) {
+				FM_ERR_REPORT("Invalid Ctrl Class = %d",
+					      ext_ctrl->ctrl_class);
+				break;
+			}
+			status = cg2900_fm_tx_get_preemphasis(
+					(u8 *)&ext_ctrl->controls->value);
+			if (status == 0)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_TUNE_POWER_LEVEL:
+		{
+			FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+					"V4L2_CID_TUNE_POWER_LEVEL");
+			if (V4L2_CTRL_CLASS_FM_TX != ext_ctrl->ctrl_class) {
+				FM_ERR_REPORT("Invalid Ctrl Class = %d",
+					      ext_ctrl->ctrl_class);
+				break;
+			}
+			status = cg2900_fm_tx_get_power_level(
+					(u16 *)&ext_ctrl->controls->value);
+			if (status == 0)
+				ret_val = 0;
+			break;
+		}
+	default:
+		{
+			FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+					"unsupported (id = %d)",
+					(int)ext_ctrl->controls->id);
+		}
+	}
+
+error:
+	FM_DEBUG_REPORT("vidioc_get_ext_ctrls: returning = %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_set_ext_ctrls()- Set the values of a particular control.
+ *
+ * This function is used to set the value of a
+ * particular control on the FM Driver. This is used when the data to
+ * be set is more than 1 paramter. This function is called when the
+ * application issues the IOCTL VIDIOC_S_EXT_CTRLS
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @ext_ctrl: v4l2_ext_controls structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -ENOSPC: when there is no space to copy the data into the buffer provided
+ * by application.
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_ext_ctrls(
+			struct file *file,
+			void *priv,
+			struct v4l2_ext_controls *ext_ctrl
+			)
+{
+	int ret_val = -EINVAL;
+	int status;
+
+	FM_INFO_REPORT("vidioc_set_ext_ctrls: Id = %04x, ctrl_class = %04x",
+			ext_ctrl->controls->id, ext_ctrl->ctrl_class);
+
+	if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_FM_TX &&
+	    ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+		FM_ERR_REPORT("vidioc_set_ext_ctrls: Unsupported "
+			      "ctrl_class = %04x", ext_ctrl->ctrl_class);
+		goto error;
+	}
+
+	switch (ext_ctrl->controls->id) {
+	case V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START:
+		{
+			u32 af_switch_freq;
+			u16 af_switch_pi;
+			u32 *af_switch_buf;
+
+			if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START "
+				"Unsupported ctrl_class = %04x",
+				ext_ctrl->ctrl_class);
+				break;
+			}
+
+			if (ext_ctrl->controls->size !=
+				FMR_AF_SWITCH_DATA_SIZE ||
+				ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START "
+				"Unsupported ctrl_class = %04x",
+				ext_ctrl->ctrl_class);
+				break;
+			}
+
+			af_switch_buf = (u32 *) ext_ctrl->controls->string;
+			af_switch_freq = V4L2_TO_HZ(*af_switch_buf);
+			af_switch_pi = *(af_switch_buf + 1);
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+				"V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START: "
+				"AF Switch Freq =%d Hz AF Switch PI = %04x",
+				af_switch_freq,
+				af_switch_pi);
+
+			if (af_switch_freq < (FMR_CHINA_LOW_FREQ_IN_MHZ
+				* FMR_HZ_TO_MHZ_CONVERTER) ||
+				af_switch_freq > (FMR_CHINA_HIGH_FREQ_IN_MHZ
+				* FMR_HZ_TO_MHZ_CONVERTER)) {
+				FM_ERR_REPORT("Invalid Freq = %04x",
+					af_switch_freq);
+				break;
+			}
+
+			status = cg2900_fm_af_switch_start(
+					af_switch_freq,
+					af_switch_pi);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_RDS_TX_DEVIATION:
+		{
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_RDS_TX_DEVIATION, "
+					"Value = %d",
+					ext_ctrl->controls->value);
+
+			if (ext_ctrl->controls->value <= MIN_RDS_DEVIATION &&
+			    ext_ctrl->controls->value > MAX_RDS_DEVIATION) {
+				FM_ERR_REPORT("Invalid RDS Deviation = %02x",
+					ext_ctrl->controls->value);
+				break;
+			}
+
+			status = cg2900_fm_tx_set_rds_deviation(
+					ext_ctrl->controls->value);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_RDS_TX_PI:
+		{
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_RDS_TX_PI, PI = %04x",
+					ext_ctrl->controls->value);
+
+			if (ext_ctrl->controls->value <= MIN_PI_VALUE &&
+			    ext_ctrl->controls->value > MAX_PI_VALUE) {
+				FM_ERR_REPORT("Invalid PI = %04x",
+					ext_ctrl->controls->value);
+				break;
+			}
+
+			status = cg2900_fm_tx_set_pi_code(
+					ext_ctrl->controls->value);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_RDS_TX_PTY:
+		{
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_RDS_TX_PTY, PTY = %d",
+					ext_ctrl->controls->value);
+
+			if (ext_ctrl->controls->value < MIN_PTY_VALUE &&
+			    ext_ctrl->controls->value > MAX_PTY_VALUE) {
+				FM_ERR_REPORT("Invalid PTY = %02x",
+					      ext_ctrl->controls->value);
+				break;
+			}
+
+			status = cg2900_fm_tx_set_pty_code(
+					ext_ctrl->controls->value);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_RDS_TX_PS_NAME:
+		{
+			if (ext_ctrl->controls->size > MAX_PSN_SIZE
+			    || ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("Invalid PSN");
+				break;
+			}
+
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_RDS_TX_PS_NAME, "
+					"PSN = %s, Len = %d",
+					ext_ctrl->controls->string,
+					ext_ctrl->controls->size);
+
+			status = cg2900_fm_tx_set_program_station_name(
+					ext_ctrl->controls->string,
+					ext_ctrl->controls->size);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_RDS_TX_RADIO_TEXT:
+		{
+			if (ext_ctrl->controls->size >= MAX_RT_SIZE
+			    || ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("Invalid RT");
+				break;
+			}
+
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_RDS_TX_RADIO_TEXT, "
+					"RT = %s, Len = %d",
+					ext_ctrl->controls->string,
+					ext_ctrl->controls->size);
+
+			status = cg2900_fm_tx_set_radio_text(
+					ext_ctrl->controls->string,
+					ext_ctrl->controls->size);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_PILOT_TONE_ENABLED:
+		{
+			bool enable;
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_PILOT_TONE_ENABLED, "
+					"Value = %d",
+					ext_ctrl->controls->value);
+
+			if (FMD_PILOT_TONE_ENABLED ==
+				ext_ctrl->controls->value)
+				enable = true;
+			else if (FMD_PILOT_TONE_DISABLED ==
+				ext_ctrl->controls->value)
+				enable = false;
+			else {
+				FM_ERR_REPORT("Unsupported Value = %d",
+					      ext_ctrl->controls->value);
+				break;
+			}
+			status = cg2900_fm_tx_set_pilot_tone_status(enable);
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_PILOT_TONE_DEVIATION:
+		{
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_PILOT_TONE_DEVIATION, "
+					"Value = %d",
+					ext_ctrl->controls->value);
+
+			if (ext_ctrl->controls->value <= MIN_PILOT_DEVIATION &&
+			    ext_ctrl->controls->value > MAX_PILOT_DEVIATION) {
+				FM_ERR_REPORT("Invalid Pilot Deviation = %02x",
+					      ext_ctrl->controls->value);
+				break;
+			}
+
+			status = cg2900_fm_tx_set_pilot_deviation(
+					ext_ctrl->controls->value);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_TUNE_PREEMPHASIS:
+		{
+			u8 preemphasis;
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_TUNE_PREEMPHASIS, "
+					"Value = %d",
+					ext_ctrl->controls->value);
+
+			if ((V4L2_PREEMPHASIS_50_uS >
+					ext_ctrl->controls->value) ||
+					(V4L2_PREEMPHASIS_75_uS <
+					ext_ctrl->controls->value)) {
+				FM_ERR_REPORT("Unsupported Preemphasis = %d",
+					      ext_ctrl->controls->value);
+				break;
+			}
+
+			if (V4L2_PREEMPHASIS_50_uS ==
+				ext_ctrl->controls->value) {
+				preemphasis = FMD_EMPHASIS_50US;
+			} else if (V4L2_PREEMPHASIS_75_uS ==
+				ext_ctrl->controls->value) {
+				preemphasis = FMD_EMPHASIS_75US;
+			}
+
+			status = cg2900_fm_tx_set_preemphasis(preemphasis);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_TUNE_POWER_LEVEL:
+		{
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_TUNE_POWER_LEVEL, "
+					"Value = %d",
+					ext_ctrl->controls->value);
+			if (ext_ctrl->controls->value < MIN_POWER_LEVEL &&
+			    ext_ctrl->controls->value > MAX_POWER_LEVEL) {
+				FM_ERR_REPORT("Invalid Power Level = %02x",
+					      ext_ctrl->controls->value);
+				break;
+			}
+
+			status = cg2900_fm_tx_set_power_level(
+					ext_ctrl->controls->value);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_CG2900_RADIO_BLOCKSCAN_START:
+		{
+			u32 start_freq;
+			u32 end_freq;
+			u32 *block_scan_buf;
+			u32 current_grid;
+			u32 low_freq;
+			u32 high_freq;
+			u32 result_freq;
+			u8  no_of_block_scan_channels;
+
+			/* V4L2 Initial check */
+			if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_BLOCKSCAN_START "
+				"Unsupported ctrl_class = %04x",
+				ext_ctrl->ctrl_class);
+				break;
+			}
+
+			if (ext_ctrl->controls->size !=
+				FMR_BLOCK_SCAN_DATA_SIZE ||
+				ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_BLOCKSCAN_START "
+				"Invalid Parameters");
+				break;
+			}
+
+			/* Check for current grid */
+			if (grid == CG2900_FM_GRID_50)
+				current_grid = FMR_CHINA_GRID_IN_HZ;
+			else if (grid == CG2900_FM_GRID_100)
+				current_grid = FMR_EUROPE_GRID_IN_HZ;
+			else
+				current_grid = FMR_USA_GRID_IN_HZ;
+
+			/* Check for current band */
+			if (band == CG2900_FM_BAND_US_EU) {
+				low_freq = FMR_EU_US_LOW_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+				high_freq = FMR_EU_US_HIGH_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+
+			} else if (band == CG2900_FM_BAND_JAPAN) {
+				low_freq = FMR_JAPAN_LOW_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+				high_freq = FMR_JAPAN_HIGH_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+
+			} else {
+				low_freq = FMR_CHINA_LOW_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+				high_freq = FMR_CHINA_HIGH_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+			}
+
+			/* V4L2 Extended control */
+
+			block_scan_buf = (u32 *)ext_ctrl->controls->string;
+			start_freq = V4L2_TO_HZ(*block_scan_buf);
+			end_freq = V4L2_TO_HZ(*(block_scan_buf + 1));
+
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_CG2900_RADIO_"
+					"BLOCKSCAN_START: "
+					"Start Freq = %d Hz "
+					"End Freq = %d Hz",
+					start_freq,
+					end_freq);
+
+			result_freq = end_freq - start_freq;
+			no_of_block_scan_channels =
+				(u8)(result_freq / current_grid);
+
+			/* Frequency Check */
+			if (end_freq < start_freq) {
+				FM_ERR_REPORT("Start Freq (%d Hz) "
+					" > End Freq (%d Hz)",
+					start_freq,
+					end_freq);
+				break;
+			}
+
+			if ((start_freq < low_freq) ||
+				(start_freq > high_freq)) {
+				FM_ERR_REPORT("Out of Band Freq: "
+					"Start Freq = %d Hz",
+					start_freq);
+				break;
+			}
+
+			if ((end_freq < low_freq) ||
+				(end_freq > high_freq)) {
+				FM_ERR_REPORT("Out of Band Freq: "
+					"End Freq = %d Hz",
+					end_freq);
+				break;
+			}
+
+			/* Maximum allowed block scan range */
+			if (FMR_MAX_BLOCK_SCAN_CHANNELS <
+				no_of_block_scan_channels) {
+				FM_ERR_REPORT("No of channels (%d)"
+					"exceeds Max Block Scan (%d)",
+					no_of_block_scan_channels,
+					FMR_MAX_BLOCK_SCAN_CHANNELS);
+				break;
+			}
+
+			status = cg2900_fm_start_block_scan(
+					start_freq,
+					end_freq);
+			if (0 == status) {
+				cg2900_device.seekstatus =
+					FMR_SEEK_IN_PROGRESS;
+				ret_val = 0;
+			}
+			break;
+		}
+	case V4L2_CID_CG2900_RADIO_TEST_TONE_CONNECT:
+		{
+			u8 left_audio_mode;
+			u8 right_audio_mode;
+			u8 *test_tone_connect_buf;
+
+			/* V4L2 Initial check */
+			if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+					"V4L2_CID_CG2900_RADIO_"
+					"TEST_TONE_CONNECT "
+					"Unsupported ctrl_class = %04x",
+					ext_ctrl->ctrl_class);
+				break;
+			}
+
+			if (ext_ctrl->controls->size !=
+				FMR_TEST_TONE_CONNECT_DATA_SIZE ||
+				ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+					"V4L2_CID_CG2900_RADIO_TEST"
+					"_TONE_CONNECT "
+					"Invalid Parameters");
+				break;
+			}
+
+			/* V4L2 Extended control */
+			test_tone_connect_buf =
+				(u8 *)ext_ctrl->controls->string;
+			left_audio_mode = *test_tone_connect_buf;
+			right_audio_mode = *(test_tone_connect_buf + 1);
+
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+				"V4L2_CID_CG2900_RADIO_TEST_TONE_CONNECT"
+				"left_audio_mode Freq = %02x"
+				"right_audio_modeFreq = %02x",
+				left_audio_mode,
+				right_audio_mode);
+
+			/* Range Check */
+			if (left_audio_mode > \
+			V4L2_CG2900_RADIO_TEST_TONE_TONE_SUM) {
+				FM_ERR_REPORT("Invalid Value of "
+					"left_audio_mode (%02x) ",
+					left_audio_mode);
+				break;
+			}
+
+			if (right_audio_mode > \
+			V4L2_CG2900_RADIO_TEST_TONE_TONE_SUM) {
+				FM_ERR_REPORT("Invalid Value of "
+					"right_audio_mode (%02x) ",
+					left_audio_mode);
+				break;
+			}
+
+			status = cg2900_fm_test_tone_connect(
+					left_audio_mode,
+					right_audio_mode);
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_CG2900_RADIO_TEST_TONE_SET_PARAMS:
+		{
+			u8 tone_gen;
+			u16 frequency;
+			u16 volume;
+			u16 phase_offset;
+			u16 dc;
+			u8 waveform;
+			u16 *test_tone_set_params_buf;
+
+			/* V4L2 Initial check */
+			if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_TEST_TONE_SET_PARAMS "
+				"Unsupported ctrl_class = %04x",
+				ext_ctrl->ctrl_class);
+				break;
+			}
+
+			if (ext_ctrl->controls->size !=
+				FMR_TEST_TONE_SET_PARAMS_DATA_SIZE ||
+				ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"FMR_TEST_TONE_SET_PARAMS_DATA_SIZE "
+				"Invalid Parameters");
+				break;
+			}
+
+			/* V4L2 Extended control */
+			test_tone_set_params_buf = \
+			(u16 *)ext_ctrl->controls->string;
+
+			tone_gen = (u8)(*test_tone_set_params_buf);
+			frequency = *(test_tone_set_params_buf + 1);
+			volume = *(test_tone_set_params_buf + 2);
+			phase_offset = *(test_tone_set_params_buf + 3);
+			dc = *(test_tone_set_params_buf + 4);
+			waveform = (u8)(*(test_tone_set_params_buf + 5));
+
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+				"V4L2_CID_CG2900_RADIO_TEST_TONE_SET_PARAMS"
+				"tone_gen = %02x frequency = %04x"
+				"volume = %04x phase_offset = %04x"
+				"dc = %04x waveform = %02x",
+				tone_gen, frequency,
+				volume, phase_offset,
+				dc, waveform);
+
+			/* Range Check */
+			if (tone_gen > FMD_TST_TONE_2) {
+				FM_ERR_REPORT("Invalid Value of "
+					"tone_gen (%02x) ",
+					tone_gen);
+				break;
+			}
+
+			if (waveform > FMD_TST_TONE_PULSE) {
+				FM_ERR_REPORT("Invalid Value of "
+					"waveform (%02x) ",
+					waveform);
+				break;
+			}
+
+			if (frequency > 0x7FFF) {
+				FM_ERR_REPORT("Invalid Value of "
+					"frequency (%04x) ",
+					frequency);
+				break;
+			}
+
+			if (volume > 0x7FFF) {
+				FM_ERR_REPORT("Invalid Value of "
+					"volume (%04x) ",
+					volume);
+				break;
+			}
+
+			status = cg2900_fm_test_tone_set_params(
+					tone_gen,
+					frequency,
+					volume,
+					phase_offset,
+					dc,
+					waveform);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	default:
+		{
+			FM_ERR_REPORT("vidioc_set_ext_ctrls: "
+				      "Unsupported Id = %04x",
+				      ext_ctrl->controls->id);
+		}
+	}
+error:
+	return ret_val;
+}
+
+/**
+ * vidioc_set_hw_freq_seek()- seek Up/Down Frequency.
+ *
+ * This function is used to start seek
+ * on the FM Radio. Direction if seek is as inicated by the parameter
+ * inside the v4l2_hw_freq_seek structure. This function is called when the
+ * application issues the IOCTL VIDIOC_S_HW_FREQ_SEEK
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @freq_seek: v4l2_hw_freq_seek structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_hw_freq_seek(
+			struct file *file,
+			void *priv,
+			struct v4l2_hw_freq_seek *freq_seek
+			)
+{
+	int status;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_set_hw_freq_seek");
+
+	FM_DEBUG_REPORT("vidioc_set_hw_freq_seek: Status = %d, "
+			"Upwards = %d, Wrap Around = %d",
+			cg2900_device.seekstatus,
+			freq_seek->seek_upward, freq_seek->wrap_around);
+
+	if (cg2900_device.seekstatus == FMR_SEEK_IN_PROGRESS) {
+		FM_ERR_REPORT("vidioc_set_hw_freq_seek: "
+				"VIDIOC_S_HW_FREQ_SEEK, "
+				"freq_seek in progress");
+		goto error;
+	}
+
+	spin_lock(&fm_spinlock);
+	fm_event = CG2900_EVENT_NO_EVENT;
+	no_of_scan_freq = 0;
+	spin_unlock(&fm_spinlock);
+
+	if (CG2900_DIR_UP == freq_seek->seek_upward)
+		status = cg2900_fm_search_up_freq();
+	else if (CG2900_DIR_DOWN == freq_seek->seek_upward)
+		status = cg2900_fm_search_down_freq();
+	else
+		goto error;
+
+	if (0 != status)
+		goto error;
+
+	cg2900_device.seekstatus = FMR_SEEK_IN_PROGRESS;
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_set_hw_freq_seek: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_get_audio()- Get Audio features of FM Driver.
+ *
+ * This function is used to get the audio features of FM Driver.
+ * This function is imlemented as a dumy function.
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @audio: (out) v4l2_audio structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_audio(
+			struct file *file,
+			void *priv,
+			struct v4l2_audio *audio
+			)
+{
+	FM_INFO_REPORT("vidioc_get_audio");
+	strcpy(audio->name, "");
+	audio->capability = 0;
+	audio->mode = 0;
+	return 0;
+}
+
+/**
+ * vidioc_set_audio()- Set Audio features of FM Driver.
+ *
+ * This function is used to set the audio features of FM Driver.
+ * This function is imlemented as a dumy function.
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @audio: v4l2_audio structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_audio(
+			struct file *file,
+			void *priv,
+			struct v4l2_audio *audio
+			)
+{
+	FM_INFO_REPORT("vidioc_set_audio");
+	if (audio->index != 0)
+		return -EINVAL;
+	return 0;
+}
+
+/**
+ * vidioc_get_input()- Get the Input Value
+ *
+ * This function is used to get the Input.
+ * This function is imlemented as a dumy function.
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @input: (out) Value to be stored.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_input(
+			struct file *file,
+			void *priv,
+			unsigned int *input
+			)
+{
+	FM_INFO_REPORT("vidioc_get_input");
+	*input = 0;
+	return 0;
+}
+
+/**
+ * vidioc_set_input()- Set the input value.
+ *
+ * This function is used to set input.
+ * This function is imlemented as a dumy function.
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @input: Value to set
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_input(
+			struct file *file,
+			void *priv,
+			unsigned int input
+			)
+{
+	FM_INFO_REPORT("vidioc_set_input");
+	if (input != 0)
+		return -EINVAL;
+	return 0;
+}
+
+/**
+ * cg2900_convert_err_to_v4l2()- Convert Error Bits to V4L2 RDS format.
+ *
+ * This function converts the error bits in RDS Block
+ * as received from Chip into V4L2 RDS data specification.
+ *
+ * @status_byte: The status byte as received in RDS Group for
+ * particular RDS Block
+ * @out_byte: byte to store the modified byte with the err bits
+ * alligned as per V4L2 RDS Specifications.
+ */
+static void cg2900_convert_err_to_v4l2(
+			char status_byte,
+			char *out_byte
+			)
+{
+	if ((status_byte & RDS_ERROR_STATUS_MASK) == RDS_ERROR_STATUS_MASK) {
+		/* Uncorrectable Block */
+		*out_byte = (*out_byte | V4L2_RDS_BLOCK_ERROR);
+	} else if (((status_byte & RDS_UPTO_TWO_BITS_CORRECTED)
+		    == RDS_UPTO_TWO_BITS_CORRECTED) ||
+		   ((status_byte & RDS_UPTO_FIVE_BITS_CORRECTED)
+		    == RDS_UPTO_FIVE_BITS_CORRECTED)) {
+		/* Corrected Bits in Block */
+		*out_byte = (*out_byte | V4L2_RDS_BLOCK_CORRECTED);
+	}
+}
+
+/**
+ * cg2900_open()- This function nitializes and switches on FM.
+ *
+ * This is called when the application opens the character device.
+ *
+ * @file: File structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int cg2900_open(
+			struct file *file
+			)
+{
+	int status;
+	int ret_val = -EINVAL;
+	struct video_device *vdev = video_devdata(file);
+
+	lock_kernel();
+	users++;
+	FM_INFO_REPORT("cg2900_open: users = %d", users);
+
+	if (users > 1) {
+		FM_INFO_REPORT("cg2900_open: FM already switched on!!!");
+		ret_val = 0;
+		/*
+		 * No need to perform the initialization and switch on FM
+		 * since it is already done during the first open call to
+		 * this driver.
+		 */
+		goto done;
+	}
+
+	status = cg2900_fm_init();
+	if (0 != status)
+		goto init_error;
+
+	FM_DEBUG_REPORT("cg2900_open: Switching on FM");
+	status = cg2900_fm_switch_on(&(vdev->dev));
+	if (0 != status)
+		goto switch_on_error;
+
+	cg2900_device.state = FMR_SWITCH_ON;
+	cg2900_device.frequency = HZ_TO_V4L2(freq_low);
+	cg2900_device.rx_rds_enabled = false;
+	cg2900_device.muted = false;
+	cg2900_device.audiopath = 0;
+	cg2900_device.seekstatus = FMR_SEEK_NONE;
+	cg2900_device.rssi_threshold = CG2900_FM_DEFAULT_RSSI_THRESHOLD;
+	fm_event = CG2900_EVENT_NO_EVENT;
+	no_of_scan_freq = 0;
+	cg2900_device.fm_mode = CG2900_FM_IDLE_MODE;
+	ret_val = 0;
+	goto done;
+
+switch_on_error:
+	cg2900_fm_deinit();
+init_error:
+	users--;
+done:
+	unlock_kernel();
+	FM_DEBUG_REPORT("cg2900_open: returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * cg2900_release()- This function switches off FM.
+ *
+ * This function switches off FM and releases the resources.
+ * This is called when the application closes the character
+ * device.
+ *
+ * @file: File structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int cg2900_release(
+			struct file *file
+			)
+{
+	int status;
+	int ret_val = -EINVAL;
+
+	mutex_lock(&fm_mutex);
+
+	FM_INFO_REPORT("cg2900_release");
+	if (users <= 0) {
+		FM_ERR_REPORT("cg2900_release: No users registered "
+			      "with FM Driver");
+		goto done;
+	}
+
+	users--;
+	FM_INFO_REPORT("cg2900_release: users = %d", users);
+
+	if (0 == users) {
+		FM_DEBUG_REPORT("cg2900_release: Switching Off FM");
+		status = cg2900_fm_switch_off();
+		status = cg2900_fm_deinit();
+		if (0 != status)
+			goto done;
+
+		cg2900_device.state = FMR_SWITCH_OFF;
+		cg2900_device.frequency = 0;
+		cg2900_device.rx_rds_enabled = false;
+		cg2900_device.muted = false;
+		cg2900_device.seekstatus = FMR_SEEK_NONE;
+		fm_event = CG2900_EVENT_NO_EVENT;
+		no_of_scan_freq = 0;
+	}
+	ret_val = 0;
+
+done:
+	mutex_unlock(&fm_mutex);
+	FM_DEBUG_REPORT("cg2900_release: returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * cg2900_read()- This function is invoked when the application
+ * calls read() to receive RDS Data.
+ *
+ * @file: File structure.
+ * @data: buffer provided by application for receving the data.
+ * @count: Number of bytes that application wants to read from driver
+ * @pos: offset
+ *
+ * Returns:
+ *   Number of bytes copied to the user buffer
+ *   -EFAULT: If there is problem in copying data to buffer supplied
+ *    by application
+ *   -EIO: If the number of bytes to be read are not a multiple of
+ *    struct  v4l2_rds_data.
+ *   -EAGAIN: More than 22 blocks requested to be read or read
+ *    was called in non blocking mode and no data was available for reading.
+ *   -EINTR: If read was interrupted by a signal before data was avaialble.
+ *   0 when no data available for reading.
+ */
+static ssize_t cg2900_read(
+			struct file *file,
+			char __user *data,
+			size_t count, loff_t *pos
+			)
+{
+	int current_rds_grp;
+	int index = 0;
+	int blocks_to_read;
+	int ret;
+	struct v4l2_rds_data rdsbuf[MAX_RDS_GROUPS * NUM_OF_RDS_BLOCKS];
+	struct v4l2_rds_data *rdslocalbuf = rdsbuf;
+
+	FM_INFO_REPORT("cg2900_read");
+
+	blocks_to_read = (count / sizeof(struct v4l2_rds_data));
+
+	if (!cg2900_device.rx_rds_enabled) {
+		FM_INFO_REPORT("cg2900_read: returning 0");
+		return 0;
+	}
+
+	if (count % sizeof(struct v4l2_rds_data) != 0) {
+		FM_ERR_REPORT("cg2900_read: Invalid Number of bytes %d "
+			      "requested to read", count);
+		return -EIO;
+	}
+
+	if (blocks_to_read > MAX_RDS_GROUPS * NUM_OF_RDS_BLOCKS) {
+		FM_ERR_REPORT("cg2900_read: Too many blocks(%d) "
+			      "requested to be read", blocks_to_read);
+		return -EAGAIN;
+	}
+
+	if (file->f_flags & O_NONBLOCK) {
+		/* Non blocking mode selected by application */
+		if (fm_rds_info.rds_head == fm_rds_info.rds_tail) {
+			FM_DEBUG_REPORT("cg2900_read: Non Blocking mode "
+				"selected by application, returning as "
+				"no RDS data is available");
+			return -EAGAIN;
+		}
+	}
+
+	if (fm_rds_info.rds_head == fm_rds_info.rds_tail) {
+		/*
+		 * Blocking mode selected by application
+		 * if data is not available block on read queue
+		 */
+		FM_DEBUG_REPORT("cg2900_read: Blocking mode "
+				"selected by application, waiting till "
+				"rds data is available");
+		cg2900_device.wait_on_read_queue = true;
+		ret = wait_event_interruptible(cg2900_read_queue,
+					       (fm_rds_info.rds_head !=
+						fm_rds_info.rds_tail));
+		cg2900_device.wait_on_read_queue = false;
+		FM_DEBUG_REPORT("cg2900_read: "
+				"wait_event_interruptible returned = %d", ret);
+		if (ret == -ERESTARTSYS)
+			return -EINTR;
+	}
+
+	/*
+	 * Check again that in the meantime RDS is not disabled
+	 * by the user, If yes, return.
+	 */
+	if (!cg2900_device.rx_rds_enabled) {
+		FM_INFO_REPORT("cg2900_read: returning 0");
+		return 0;
+	}
+
+	current_rds_grp = fm_rds_info.rds_group_sent;
+
+	if ((fm_rds_info.rds_head == fm_rds_info.rds_tail) ||
+	    (fm_rds_buf[fm_rds_info.rds_tail]
+	     [current_rds_grp].block1 == 0x0000)) {
+		FM_INFO_REPORT("cg2900_read: returning 0");
+		return 0;
+	}
+
+	spin_lock(&fm_spinlock);
+	while (index < blocks_to_read) {
+		/* Check which Block needs to be transferred next */
+		switch (fm_rds_info.rds_block_sent % NUM_OF_RDS_BLOCKS) {
+		case 0:
+			(rdslocalbuf + index)->lsb =
+			fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block1;
+			(rdslocalbuf + index)->msb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block1 >> 8;
+			(rdslocalbuf + index)->block =
+			    (fm_rds_buf[fm_rds_info.rds_tail]
+			     [current_rds_grp].status1
+			     & RDS_BLOCK_MASK) >> 2;
+			cg2900_convert_err_to_v4l2(
+				fm_rds_buf[fm_rds_info.rds_tail]
+				[current_rds_grp].status1,
+				&(rdslocalbuf + index)->block);
+			break;
+		case 1:
+			(rdslocalbuf + index)->lsb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block2;
+			(rdslocalbuf + index)->msb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block2 >> 8;
+			(rdslocalbuf + index)->block =
+			    (fm_rds_buf[fm_rds_info.rds_tail]
+			     [current_rds_grp].status2
+			     & RDS_BLOCK_MASK) >> 2;
+			cg2900_convert_err_to_v4l2(
+				fm_rds_buf[fm_rds_info.rds_tail]
+				[current_rds_grp].status2,
+				&(rdslocalbuf + index)->block);
+			break;
+		case 2:
+			(rdslocalbuf + index)->lsb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block3;
+			(rdslocalbuf + index)->msb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block3 >> 8;
+			(rdslocalbuf + index)->block =
+			    (fm_rds_buf[fm_rds_info.rds_tail]
+			     [current_rds_grp].status3
+			     & RDS_BLOCK_MASK) >> 2;
+			cg2900_convert_err_to_v4l2(
+				fm_rds_buf[fm_rds_info.rds_tail]
+				[current_rds_grp].status3,
+				&(rdslocalbuf + index)->block);
+			break;
+		case 3:
+			(rdslocalbuf + index)->lsb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block4;
+			(rdslocalbuf + index)->msb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block4 >> 8;
+			(rdslocalbuf + index)->block =
+			    (fm_rds_buf[fm_rds_info.rds_tail]
+			     [current_rds_grp].status4
+			     & RDS_BLOCK_MASK) >> 2;
+			cg2900_convert_err_to_v4l2(
+				fm_rds_buf[fm_rds_info.rds_tail]
+				[current_rds_grp].status4,
+				&(rdslocalbuf + index)->block);
+			current_rds_grp++;
+			if (current_rds_grp == MAX_RDS_GROUPS) {
+				fm_rds_info.rds_tail++;
+				current_rds_grp = 0;
+			}
+			break;
+		default:
+			FM_ERR_REPORT("Invalid RDS Group!!!");
+			spin_unlock(&fm_spinlock);
+			return 0;
+		}
+		index++;
+		fm_rds_info.rds_block_sent++;
+		if (fm_rds_info.rds_block_sent == NUM_OF_RDS_BLOCKS)
+			fm_rds_info.rds_block_sent = 0;
+
+		if (!cg2900_device.rx_rds_enabled) {
+			FM_INFO_REPORT("cg2900_read: returning 0");
+			spin_unlock(&fm_spinlock);
+			return 0;
+		}
+	}
+	/* Update the RDS Group Count Sent to Application */
+	fm_rds_info.rds_group_sent = current_rds_grp;
+	if (fm_rds_info.rds_tail == MAX_RDS_BUFFER)
+		fm_rds_info.rds_tail = 0;
+
+	if (copy_to_user(data, rdslocalbuf, count)) {
+		spin_unlock(&fm_spinlock);
+		FM_ERR_REPORT("cg2900_read: Error "
+			      "in copying, returning");
+		return -EFAULT;
+	}
+	spin_unlock(&fm_spinlock);
+	return count;
+}
+
+/**
+ * cg2900_poll()- Check if the operation is complete or not.
+ *
+ * This function is invoked by application on calling poll() and is used to
+ * wait till the desired operation seek/Band Scan are complete.
+ * Driver blocks till the seek/BandScan Operation is complete. The application
+ * decides to read the results of seek/Band Scan based on the returned value of
+ * this function.
+ *
+ * @file: File structure.
+ * @wait: poll table
+ *
+ * Returns:
+ *   POLLRDNORM when Scan Band/Block Scan/Seek station is complete.
+ *   POLLHUP when seek station/Band Scan is stopped by user using Stop Scan.
+ */
+static unsigned int cg2900_poll(
+			struct file *file,
+			struct poll_table_struct *wait
+			)
+{
+	int ret_val = 0;
+
+	FM_INFO_REPORT("cg2900_poll");
+
+	poll_wait(file, &cg2900_poll_queue, wait);
+
+	if (cg2900_device.seekstatus == FMR_SEEK_IN_PROGRESS &&
+		((fm_event == CG2900_EVENT_SCAN_CHANNELS_FOUND) ||
+	    (fm_event == CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND) ||
+	    (fm_event == CG2900_EVENT_SEARCH_CHANNEL_FOUND))) {
+		/* Scan Completed, send event to application */
+		FM_DEBUG_REPORT("poll_wait returning POLLRDNORM");
+		ret_val = POLLRDNORM;
+		goto done;
+	}
+
+	if (fm_event == CG2900_EVENT_SCAN_CANCELLED) {
+		/* Scan/Search cancelled by User */
+		spin_lock(&fm_spinlock);
+		no_of_scan_freq = 0;
+		no_of_block_scan_freq = 0;
+		spin_unlock(&fm_spinlock);
+		cg2900_device.seekstatus = FMR_SEEK_NONE;
+		fm_event = CG2900_EVENT_NO_EVENT;
+		FM_DEBUG_REPORT("cg2900_poll: Cancel operation "
+				"returning POLLHUP");
+		FM_DEBUG_REPORT("poll_wait returning POLLHUP");
+		ret_val = POLLHUP;
+		goto done;
+	}
+
+done:
+	FM_DEBUG_REPORT("poll_wait returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * radio_cg2900_probe()- This function registers FM Driver with V4L2 Driver.
+ *
+ * This function is called whenever the driver is probed by the device system,
+ * i.e. when a CG2900 controller has connected. It registers the FM Driver with
+ * Video4Linux as a character device.
+ *
+ * @pdev: Platform device.
+ *
+ * Returns:
+ *   0 on success
+ * -EINVAL on error
+ */
+static int __devinit radio_cg2900_probe(
+			struct platform_device *pdev
+			)
+{
+	int err;
+
+	FM_INFO_REPORT(BANNER);
+
+	err = fmd_set_dev(&pdev->dev);
+	if (err) {
+		FM_ERR_REPORT("Could not set device %s", pdev->name);
+		return err;
+	}
+
+	radio_nr = 0;
+	grid = CG2900_FM_GRID_100;
+	band = CG2900_FM_BAND_US_EU;
+	FM_INFO_REPORT("radio_cg2900_probe: radio_nr= %d.", radio_nr);
+
+	/* Initialize the parameters */
+	if (video_register_device(
+				&cg2900_video_device,
+				VFL_TYPE_RADIO,
+				radio_nr) == -1) {
+		FM_ERR_REPORT("radio_cg2900_probe: video_register_device err");
+		return -EINVAL;
+	}
+	mutex_init(&fm_mutex);
+	spin_lock_init(&fm_spinlock);
+	init_waitqueue_head(&cg2900_poll_queue);
+	users = 0;
+	return 0;
+}
+
+/**
+ * radio_cg2900_remove()- This function removes the FM Driver.
+ *
+ * This function is called whenever the driver is removed by the device system,
+ * i.e. when a CG2900 controller has disconnected. It unregisters the FM Driver
+ * from Video4Linux.
+ *
+ * @pdev: Platform device.
+ *
+ * Returns: 0 on success
+ */
+static int __devexit radio_cg2900_remove(
+			struct platform_device *pdev
+			)
+{
+	FM_INFO_REPORT("radio_cg2900_remove");
+
+	/* Try to Switch Off FM in case it is still switched on */
+	cg2900_fm_switch_off();
+	cg2900_fm_deinit();
+	mutex_destroy(&fm_mutex);
+	video_unregister_device(&cg2900_video_device);
+	fmd_set_dev(NULL);
+	return 0;
+}
+
+static struct platform_driver radio_cg2900_driver = {
+	.driver = {
+		.name	= "cg2900-fm",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= radio_cg2900_probe,
+	.remove	= __devexit_p(radio_cg2900_remove),
+};
+
+/**
+ * radio_cg2900_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init radio_cg2900_init(void)
+{
+	FM_INFO_REPORT("radio_cg2900_init");
+	return platform_driver_register(&radio_cg2900_driver);
+}
+
+/**
+ * radio_cg2900_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit radio_cg2900_exit(void)
+{
+	FM_INFO_REPORT("radio_cg2900_exit");
+	platform_driver_unregister(&radio_cg2900_driver);
+}
+
+void wake_up_poll_queue(void)
+{
+	FM_INFO_REPORT("wake_up_poll_queue");
+	wake_up_interruptible(&cg2900_poll_queue);
+}
+
+void wake_up_read_queue(void)
+{
+	FM_INFO_REPORT("wake_up_read_queue");
+	if (cg2900_device.wait_on_read_queue)
+		wake_up_interruptible(&cg2900_read_queue);
+}
+
+void cg2900_handle_device_reset(void)
+{
+	FM_INFO_REPORT("cg2900_handle_device_reset");
+	users = 0;
+	cg2900_device.state = FMR_SWITCH_OFF;
+	cg2900_device.frequency = 0;
+	cg2900_device.rx_rds_enabled = false;
+	cg2900_device.muted = false;
+	cg2900_device.seekstatus = FMR_SEEK_NONE;
+	fm_event = CG2900_EVENT_NO_EVENT;
+	no_of_scan_freq = 0;
+}
+
+module_init(radio_cg2900_init);
+module_exit(radio_cg2900_exit);
+MODULE_AUTHOR("Hemant Gupta");
+MODULE_LICENSE("GPL v2");
+
+module_param(radio_nr, int, S_IRUGO);
+
+module_param(grid, int, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(grid, "Grid:"
+			"0=50 kHz"
+			"*1=100 kHz*"
+			"2=200 kHz");
+
+module_param(band, int, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(band, "Band:"
+			"*0=87.5-108 MHz*"
+			"1=76-90 MHz"
+			"2=70-108 MHz");
+
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 6ca938a6bf9..2031085ab1b 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -295,6 +295,27 @@ config MFD_TC6393XB
 	help
 	  Support for Toshiba Mobile IO Controller TC6393XB
 
+config AB5500_CORE
+	bool "ST-Ericsson AB5500 Mixed Signal Circuit core functions"
+	select MFD_CORE
+	depends on GENERIC_HARDIRQS && ABX500_CORE
+	help
+	  Select this to enable the AB5500 Mixed Signal IC core
+	  functionality. This connects to a AB5500 chip on the I2C bus via
+	  the Power and Reset Management Unit (PRCMU). It exposes a number
+	  of symbols needed for dependent devices to read and write
+	  registers and subscribe to events from this multi-functional IC.
+	  This is needed to use other features of the AB5500 such as
+	  battery-backed RTC, charging control, Regulators, LEDs, vibrator,
+	  system power and temperature, power management and ALSA sound.
+
+config AB5500_GPADC
+	bool "AB5500 GPADC driver"
+	depends on AB5500_CORE
+	default y
+	help
+	  AB5500 GPADC driver used to convert battery/usb voltage.
+
 config PMIC_DA903X
 	bool "Dialog Semiconductor DA9030/DA9034 PMIC Support"
 	depends on I2C=y
@@ -560,6 +581,14 @@ config AB8500_I2C_CORE
 	  the I2C bus is connected to the Power Reset
 	  and Mangagement Unit, PRCMU.
 
+config AB8500_DENC
+	bool "AB8500_DENC driver support(CVBS)"
+	depends on AB8500_CORE
+	help
+	  Select this option to add driver support for analog TV out through
+	  AB8500.
+
+
 config AB8500_DEBUG
        bool "Enable debug info via debugfs"
        depends on AB8500_CORE && DEBUG_FS
@@ -570,10 +599,10 @@ config AB8500_DEBUG
 
 config AB8500_GPADC
 	bool "AB8500 GPADC driver"
-	depends on AB8500_CORE && REGULATOR_AB8500
+	depends on AB8500_CORE
 	default y
 	help
-	  AB8500 GPADC driver used to convert Acc and battery/ac/usb voltage
+	  AB8500 GPADC driver used to convert Acc and battery/ac/usb voltage.
 
 config AB3550_CORE
         bool "ST-Ericsson AB3550 Mixed Signal Circuit core functions"
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index d7d47d2a4c7..593cf29405c 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -2,6 +2,7 @@
 # Makefile for multifunction miscellaneous devices
 #
 
+obj-$(CONFIG_AB5500_CORE)	+= ab5500-core.o ab5500-power.o
 88pm860x-objs			:= 88pm860x-core.o 88pm860x-i2c.o
 obj-$(CONFIG_MFD_88PM860X)	+= 88pm860x.o
 obj-$(CONFIG_MFD_SM501)		+= sm501.o
@@ -75,11 +76,13 @@ obj-$(CONFIG_AB3100_OTP)	+= ab3100-otp.o
 obj-$(CONFIG_AB3550_CORE)	+= ab3550-core.o
 obj-$(CONFIG_AB8500_CORE)	+= ab8500-core.o ab8500-sysctrl.o
 obj-$(CONFIG_AB8500_DEBUG)	+= ab8500-debugfs.o
+obj-$(CONFIG_AB8500_DENC)	+= ab8500-denc.o
 obj-$(CONFIG_AB8500_GPADC)	+= ab8500-gpadc.o
 obj-$(CONFIG_MFD_DB8500_PRCMU)	+= db8500-prcmu.o
 # ab8500-i2c need to come after db8500-prcmu (which provides the channel)
 obj-$(CONFIG_AB8500_I2C_CORE)	+= ab8500-i2c.o
 obj-$(CONFIG_MFD_DB5500_PRCMU)	+= db5500-prcmu.o
+obj-$(CONFIG_AB5500_GPADC)	+= ab5500-gpadc.o
 obj-$(CONFIG_MFD_TIMBERDALE)    += timberdale.o
 obj-$(CONFIG_PMIC_ADP5520)	+= adp5520.o
 obj-$(CONFIG_LPC_SCH)		+= lpc_sch.o
diff --git a/drivers/mfd/ab5500-core.c b/drivers/mfd/ab5500-core.c
new file mode 100755
index 00000000000..189f4b66bfa
--- /dev/null
+++ b/drivers/mfd/ab5500-core.c
@@ -0,0 +1,2817 @@
+/*
+ * Copyright (C) 2007-2011 ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ * Low-level core for exclusive access to the AB5500 IC on the I2C bus
+ * and some basic chip-configuration.
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com>
+ * Author: Mattias Nilsson <mattias.i.nilsson@stericsson.com>
+ * Author: Mattias Wallin <mattias.wallin@stericsson.com>
+ * Author: Rickard Andersson <rickard.andersson@stericsson.com>
+ * Author: Karl Komierowski  <karl.komierowski@stericsson.com>
+ * Author: Bibek Basu <bibek.basu@stericsson.com>
+ */
+
+#include <linux/mutex.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/random.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <linux/mfd/abx500.h>
+#include <linux/list.h>
+#include <linux/bitops.h>
+#include <linux/spinlock.h>
+#include <linux/mfd/core.h>
+#include <linux/version.h>
+#include <linux/io.h>
+#include <mach/prcmu.h>
+
+#define AB5500_NAME_STRING "ab5500"
+#define AB5500_ID_FORMAT_STRING "AB5500 %s"
+#define AB5500_NUM_EVENT_REG 23
+#define AB5500_IT_LATCH0_REG		0x40
+#define AB5500_IT_MASK0_REG		0x60
+/* These are the only registers inside AB5500 used in this main file */
+
+/* Read/write operation values. */
+#define AB5500_PERM_RD (0x01)
+#define AB5500_PERM_WR (0x02)
+
+/* Read/write permissions. */
+#define AB5500_PERM_RO (AB5500_PERM_RD)
+#define AB5500_PERM_RW (AB5500_PERM_RD | AB5500_PERM_WR)
+
+#define AB5500_MASK_BASE (0x60)
+#define AB5500_MASK_END (0x79)
+#define AB5500_CHIP_ID (0x20)
+#define AB5500_INTERRUPTS 0x007FFFFF
+/**
+ * struct ab5500_bank
+ * @slave_addr: I2C slave_addr found in AB5500 specification
+ * @name: Documentation name of the bank. For reference
+ */
+struct ab5500_bank {
+	u8 slave_addr;
+	const char *name;
+};
+
+static const struct ab5500_bank bankinfo[AB5500_NUM_BANKS] = {
+	[AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP] = {
+		AB5500_ADDR_VIT_IO_I2C_CLK_TST_OTP, "VIT_IO_I2C_CLK_TST_OTP"},
+	[AB5500_BANK_VDDDIG_IO_I2C_CLK_TST] = {
+		AB5500_ADDR_VDDDIG_IO_I2C_CLK_TST, "VDDDIG_IO_I2C_CLK_TST"},
+	[AB5500_BANK_VDENC] = {AB5500_ADDR_VDENC, "VDENC"},
+	[AB5500_BANK_SIM_USBSIM] = {AB5500_ADDR_SIM_USBSIM, "SIM_USBSIM"},
+	[AB5500_BANK_LED] = {AB5500_ADDR_LED, "LED"},
+	[AB5500_BANK_ADC] = {AB5500_ADDR_ADC, "ADC"},
+	[AB5500_BANK_RTC] = {AB5500_ADDR_RTC, "RTC"},
+	[AB5500_BANK_STARTUP] = {AB5500_ADDR_STARTUP, "STARTUP"},
+	[AB5500_BANK_DBI_ECI] = {AB5500_ADDR_DBI_ECI, "DBI-ECI"},
+	[AB5500_BANK_CHG] = {AB5500_ADDR_CHG, "CHG"},
+	[AB5500_BANK_FG_BATTCOM_ACC] = {
+		AB5500_ADDR_FG_BATTCOM_ACC, "FG_BATCOM_ACC"},
+	[AB5500_BANK_USB] = {AB5500_ADDR_USB, "USB"},
+	[AB5500_BANK_IT] = {AB5500_ADDR_IT, "IT"},
+	[AB5500_BANK_VIBRA] = {AB5500_ADDR_VIBRA, "VIBRA"},
+	[AB5500_BANK_AUDIO_HEADSETUSB] = {
+		AB5500_ADDR_AUDIO_HEADSETUSB, "AUDIO_HEADSETUSB"},
+};
+
+
+/**
+ * struct ab5500_reg_range
+ * @first: the first address of the range
+ * @last: the last address of the range
+ * @perm: access permissions for the range
+ */
+struct ab5500_reg_range {
+	u8 first;
+	u8 last;
+	u8 perm;
+};
+
+/**
+ * struct ab5500_i2c_ranges
+ * @count: the number of ranges in the list
+ * @range: the list of register ranges
+ */
+struct ab5500_i2c_ranges {
+	u8 nranges;
+	u8 bankid;
+	const struct ab5500_reg_range *range;
+};
+
+/**
+ * struct ab5500_i2c_banks
+ * @count: the number of ranges in the list
+ * @range: the list of register ranges
+ */
+struct ab5500_i2c_banks {
+	u8 nbanks;
+	const struct ab5500_i2c_ranges *bank;
+};
+
+/*
+ * Permissible register ranges for reading and writing per device and bank.
+ *
+ * The ranges must be listed in increasing address order, and no overlaps are
+ * allowed. It is assumed that write permission implies read permission
+ * (i.e. only RO and RW permissions should be used).  Ranges with write
+ * permission must not be split up.
+ */
+
+#define NO_RANGE {.count = 0, .range = NULL,}
+static struct ab5500_i2c_banks ab5500_bank_ranges[AB5500_NUM_DEVICES] = {
+	[AB5500_DEVID_USB] =  {
+		.nbanks = 1,
+		.bank = (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_USB,
+				.nranges = 12,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x01,
+						.last = 0x01,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x80,
+						.last = 0x83,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x87,
+						.last = 0x8A,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x8B,
+						.last = 0x8B,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0x91,
+						.last = 0x92,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0x93,
+						.last = 0x93,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x94,
+						.last = 0x94,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0xA8,
+						.last = 0xB0,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0xB2,
+						.last = 0xB2,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0xB4,
+						.last = 0xBC,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0xBF,
+						.last = 0xBF,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0xC1,
+						.last = 0xC5,
+						.perm = AB5500_PERM_RO,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_ADC] =  {
+		.nbanks = 1,
+		.bank = (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_ADC,
+				.nranges = 6,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x1F,
+						.last = 0x22,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x23,
+						.last = 0x24,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x26,
+						.last = 0x2D,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0x2F,
+						.last = 0x34,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x37,
+						.last = 0x57,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x58,
+						.last = 0x58,
+						.perm = AB5500_PERM_RO,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_LEDS] =  {
+		.nbanks = 1,
+		.bank = (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_LED,
+				.nranges = 1,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x00,
+						.last = 0x0C,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_VIDEO] =   {
+		.nbanks = 1,
+		.bank = (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_VDENC,
+				.nranges = 12,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x00,
+						.last = 0x08,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x09,
+						.last = 0x09,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0x0A,
+						.last = 0x12,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x15,
+						.last = 0x19,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x1B,
+						.last = 0x21,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x27,
+						.last = 0x2C,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x41,
+						.last = 0x41,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x45,
+						.last = 0x5B,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x5D,
+						.last = 0x5D,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x69,
+						.last = 0x69,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x6C,
+						.last = 0x6D,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x80,
+						.last = 0x81,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_REGULATORS] =   {
+		.nbanks = 2,
+		.bank =  (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_STARTUP,
+				.nranges = 12,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x00,
+						.last = 0x01,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x1F,
+						.last = 0x1F,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x2E,
+						.last = 0x2E,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0x2F,
+						.last = 0x30,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x50,
+						.last = 0x51,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x60,
+						.last = 0x61,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x66,
+						.last = 0x8A,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x8C,
+						.last = 0x96,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xAA,
+						.last = 0xB4,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xB7,
+						.last = 0xBF,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xC1,
+						.last = 0xCA,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xD3,
+						.last = 0xE0,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+			{
+				.bankid = AB5500_BANK_SIM_USBSIM,
+				.nranges = 1,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x13,
+						.last = 0x19,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_SIM] =   {
+		.nbanks = 1,
+		.bank = (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_SIM_USBSIM,
+				.nranges = 1,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x13,
+						.last = 0x19,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_RTC] =   {
+		.nbanks = 1,
+		.bank = (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_RTC,
+				.nranges = 2,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x00,
+						.last = 0x04,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x06,
+						.last = 0x0C,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_CHARGER] =   {
+		.nbanks = 4,
+		.bank = (struct ab5500_i2c_ranges[]) {
+			{
+				.bankid = AB5500_BANK_CHG,
+				.nranges = 2,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x11,
+						.last = 0x12,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0x12,
+						.last = 0x1B,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+			{
+				.bankid = AB5500_BANK_USB,
+				.nranges = 13,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x01,
+						.last = 0x01,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x80,
+						.last = 0x83,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x87,
+						.last = 0x8B,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0x91,
+						.last = 0x94,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xA8,
+						.last = 0xB0,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xB2,
+						.last = 0xB2,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xB4,
+						.last = 0xBC,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xBF,
+						.last = 0xBF,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xC1,
+						.last = 0xC6,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xCD,
+						.last = 0xCD,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xD6,
+						.last = 0xDA,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xDC,
+						.last = 0xDC,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xE0,
+						.last = 0xE4,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+			{
+				.bankid = AB5500_BANK_IT,
+				.nranges = 2,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x00,
+						.last = 0x02,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0x20,
+						.last = 0x36,
+						.perm = AB5500_PERM_RO,
+					},
+				},
+			},
+			{
+				.bankid = AB5500_BANK_STARTUP,
+				.nranges = 1,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x2F,
+						.last = 0x30,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_BTEMP] =   {
+		.nbanks = 1,
+		.bank = (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_FG_BATTCOM_ACC,
+				.nranges = 2,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x00,
+						.last = 0x0B,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0x0C,
+						.last = 0x24,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_FG] =   {
+		.nbanks = 1,
+		.bank = (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_FG_BATTCOM_ACC,
+				.nranges = 2,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x00,
+						.last = 0x0B,
+						.perm = AB5500_PERM_RO,
+					},
+					{
+						.first = 0x0C,
+						.last = 0x24,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_VIBRATOR] =   {
+		.nbanks = 1,
+		.bank = (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_VIBRA,
+				.nranges = 2,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x10,
+						.last = 0x13,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xFE,
+						.last = 0xFE,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_CODEC] =   {
+		.nbanks = 1,
+		.bank = (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_AUDIO_HEADSETUSB,
+				.nranges = 2,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x00,
+						.last = 0x48,
+						.perm = AB5500_PERM_RW,
+					},
+					{
+						.first = 0xEB,
+						.last = 0xFB,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+		},
+	},
+	[AB5500_DEVID_POWER] = {
+		.nbanks	= 2,
+		.bank	= (struct ab5500_i2c_ranges []) {
+			{
+				.bankid = AB5500_BANK_STARTUP,
+				.nranges = 1,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x30,
+						.last = 0x30,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+			{
+				.bankid = AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP,
+				.nranges = 1,
+				.range = (struct ab5500_reg_range[]) {
+					{
+						.first = 0x01,
+						.last = 0x01,
+						.perm = AB5500_PERM_RW,
+					},
+				},
+			},
+		},
+	},
+};
+
+#define AB5500_IRQ(bank, bit)	((bank) * 8 + (bit))
+
+/* I appologize for the resource names beeing a mix of upper case
+ * and lower case but I want them to be exact as the documentation */
+static struct mfd_cell ab5500_devs[AB5500_NUM_DEVICES] = {
+	[AB5500_DEVID_LEDS] = {
+		.name = "ab5500-leds",
+		.id = AB5500_DEVID_LEDS,
+	},
+	[AB5500_DEVID_POWER] = {
+		.name = "ab5500-power",
+		.id = AB5500_DEVID_POWER,
+	},
+	[AB5500_DEVID_REGULATORS] = {
+		.name = "ab5500-regulator",
+		.id = AB5500_DEVID_REGULATORS,
+	},
+	[AB5500_DEVID_SIM] = {
+		.name = "ab5500-sim",
+		.id = AB5500_DEVID_SIM,
+		.num_resources = 1,
+		.resources = (struct resource[]) {
+			{
+				.name = "SIMOFF",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(2, 0), /*rising*/
+				.end = AB5500_IRQ(2, 1), /*falling*/
+			},
+		},
+	},
+	[AB5500_DEVID_RTC] = {
+		.name = "ab5500-rtc",
+		.id = AB5500_DEVID_RTC,
+		.num_resources = 1,
+		.resources = (struct resource[]) {
+			{
+				.name	= "RTC_Alarm",
+				.flags	= IORESOURCE_IRQ,
+				.start	= AB5500_IRQ(1, 7),
+				.end	= AB5500_IRQ(1, 7),
+			}
+		},
+	},
+	[AB5500_DEVID_CHARGER] = {
+		.name = "ab5500-charger",
+		.id = AB5500_DEVID_CHARGER,
+		.num_resources = 29,
+		.resources = (struct resource[]) {
+			{
+				.name = "VBAT_INSERT",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(2, 4),
+				.end = AB5500_IRQ(2, 4),
+			},
+			{
+				.name = "TEMP_ASIC_ALARM",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(2, 2),
+				.end = AB5500_IRQ(2, 2),
+			},
+			{
+				.name = "BATT_REMOVAL",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(7, 6),
+				.end = AB5500_IRQ(7, 6),
+			},
+			{
+				.name = "BATT_ATTACH",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(7, 5),
+				.end = AB5500_IRQ(7, 5),
+			},
+			{
+				.name = "CGSTATE_10_PCVBUS_CHG",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 7),
+				.end = AB5500_IRQ(8, 7),
+			},
+			{
+				.name = "VBUS_FALLING",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 6),
+				.end = AB5500_IRQ(8, 6),
+			},
+			{
+				.name = "VBUS_RISING",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 5),
+				.end = AB5500_IRQ(8, 5),
+			},
+			{
+				.name = "UART_RDY_TX",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 2),
+				.end = AB5500_IRQ(8, 2),
+			},
+			{
+				.name = "UART_RDY_RX",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 1),
+				.end = AB5500_IRQ(8, 1),
+			},
+			{
+				.name = "UART_OVERRUN",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 0),
+				.end = AB5500_IRQ(8, 0),
+			},
+			{
+				.name = "VBUS_IMEAS_MAX_CHANGE_RISING",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(9, 7),
+				.end = AB5500_IRQ(9, 7),
+			},
+			{
+				.name = "USB_SUSPEND",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(9, 3),
+				.end = AB5500_IRQ(9, 3),
+			},
+			{
+				.name = "USB_CHAR_DET_DONE",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(9, 1),
+				.end = AB5500_IRQ(9, 1),
+			},
+			{
+				.name = "VBUS_IMEAS_MAX_CHANGE_FALLING",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(10, 0),
+				.end = AB5500_IRQ(10, 0),
+			},
+			{
+				.name = "OVV",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(14, 5),
+				.end = AB5500_IRQ(14, 5),
+			},
+			{
+				.name = "BSI_INDICATOR",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(14, 4),
+				.end = AB5500_IRQ(14, 4),
+			},
+			{
+				.name = "USB_CH_TH_PROTECTION",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(15, 4),
+				.end = AB5500_IRQ(15, 4),
+			},
+			{
+				.name = "USB_CH_NOT_OK",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(15, 3),
+				.end = AB5500_IRQ(15, 3),
+			},
+			{
+				.name = "CHAR_TEMP_WINDOW_OK_RISING",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(17, 7),
+				.end = AB5500_IRQ(17, 7),
+			},
+			{
+				.name = "CHARGING_STOPPED_BY_TEMP",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(18, 6),
+				.end = AB5500_IRQ(18, 6),
+			},
+			{
+				.name = "VBUS_DROP_FALLING",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(18, 4),
+				.end = AB5500_IRQ(18, 4),
+			},
+			{
+				.name = "VBUS_DROP_RISING",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(18, 3),
+				.end = AB5500_IRQ(18, 3),
+			},
+			{
+				.name = "CHAR_TEMP_WINDOW_OK_FALLING",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(18, 0),
+				.end = AB5500_IRQ(18, 0),
+			},
+			{
+				.name = "CHG_STATE_13_COMP_VBUS",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(21, 3),
+				.end = AB5500_IRQ(21, 3),
+			},
+			{
+				.name = "CHG_STATE_12_COMP_VBUS",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(21, 2),
+				.end = AB5500_IRQ(21, 2),
+			},
+			{
+				.name = "CHG_STATE_11_SAFE_MODE_VBUS",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(21, 1),
+				.end = AB5500_IRQ(21, 1),
+			},
+			{
+				.name = "USB_LINK_UPDATE",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(22, 1),
+				.end = AB5500_IRQ(22, 1),
+			},
+			{
+				.name = "CHG_SW_TIMER_OUT",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(23, 7),
+				.end = AB5500_IRQ(23, 7),
+			},
+			{
+				.name = "CHG_HW_TIMER_OUT",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(23, 6),
+				.end = AB5500_IRQ(23, 6),
+			},
+		},
+	},
+	[AB5500_DEVID_CHARGALG] = {
+		.name = "abx500-chargalg",
+		.id = AB5500_DEVID_CHARGALG,
+	},
+	[AB5500_DEVID_BTEMP] = {
+		.name = "ab5500-btemp",
+		.id = AB5500_DEVID_BTEMP,
+		.num_resources = 2,
+		.resources = (struct resource[]) {
+			{
+				.name = "BATT_ATTACH",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(7, 5),
+				.end = AB5500_IRQ(7, 5),
+			},
+			{
+				.name = "BATT_REMOVAL",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(7, 6),
+				.end = AB5500_IRQ(7, 6),
+			},
+		},
+	},
+	[AB5500_DEVID_ADC] = {
+		.name = "ab5500-adc",
+		.id = AB5500_DEVID_ADC,
+		.num_resources = 10,
+		.resources = (struct resource[]) {
+			{
+				.name = "TRIGGER-0",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(0, 0),
+				.end = AB5500_IRQ(0, 0),
+			},
+			{
+				.name = "TRIGGER-1",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(0, 1),
+				.end = AB5500_IRQ(0, 1),
+			},
+			{
+				.name = "TRIGGER-2",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(0, 2),
+				.end = AB5500_IRQ(0, 2),
+			},
+			{
+				.name = "TRIGGER-3",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(0, 3),
+				.end = AB5500_IRQ(0, 3),
+			},
+			{
+				.name = "TRIGGER-4",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(0, 4),
+				.end = AB5500_IRQ(0, 4),
+			},
+			{
+				.name = "TRIGGER-5",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(0, 5),
+				.end = AB5500_IRQ(0, 5),
+			},
+			{
+				.name = "TRIGGER-6",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(0, 6),
+				.end = AB5500_IRQ(0, 6),
+			},
+			{
+				.name = "TRIGGER-7",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(0, 7),
+				.end = AB5500_IRQ(0, 7),
+			},
+			{
+				.name = "TRIGGER-VBAT",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(0, 8),
+				.end = AB5500_IRQ(0, 8),
+			},
+			{
+				.name = "TRIGGER-VBAT-TXON",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(0, 9),
+				.end = AB5500_IRQ(0, 9),
+			},
+		},
+	},
+	[AB5500_DEVID_FG] = {
+		.name = "ab5500-fg",
+		.id = AB5500_DEVID_FG,
+		.num_resources = 6,
+		.resources = (struct resource[]) {
+			{
+				.name = "Batt_attach",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(7, 5),
+				.end = AB5500_IRQ(7, 5),
+			},
+			{
+				.name = "Batt_removal",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(7, 6),
+				.end = AB5500_IRQ(7, 6),
+			},
+			{
+				.name = "UART_framing",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(7, 7),
+				.end = AB5500_IRQ(7, 7),
+			},
+			{
+				.name = "UART_overrun",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 0),
+				.end = AB5500_IRQ(8, 0),
+			},
+			{
+				.name = "UART_Rdy_RX",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 1),
+				.end = AB5500_IRQ(8, 1),
+			},
+			{
+				.name = "UART_Rdy_TX",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 2),
+				.end = AB5500_IRQ(8, 2),
+			},
+		},
+	},
+	[AB5500_DEVID_VIBRATOR] = {
+		.name = "ab5500-vibrator",
+		.id = AB5500_DEVID_VIBRATOR,
+	},
+	[AB5500_DEVID_CODEC] = {
+		.name = "ab5500-codec",
+		.id = AB5500_DEVID_CODEC,
+		.num_resources = 3,
+		.resources = (struct resource[]) {
+			{
+				.name = "audio_spkr1_ovc",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(9, 5),
+				.end = AB5500_IRQ(9, 5),
+			},
+			{
+				.name = "audio_plllocked",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(9, 6),
+				.end = AB5500_IRQ(9, 6),
+			},
+			{
+				.name = "audio_spkr2_ovc",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(17, 4),
+				.end = AB5500_IRQ(17, 4),
+			},
+		},
+	},
+	[AB5500_DEVID_USB] = {
+		.name = "ab5500-usb",
+		.id = AB5500_DEVID_USB,
+		.num_resources = 36,
+		.resources = (struct resource[]) {
+			{
+				.name = "Link_Update",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(22, 1),
+				.end = AB5500_IRQ(22, 1),
+			},
+			{
+				.name = "DCIO",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 3),
+				.end = AB5500_IRQ(8, 4),
+			},
+			{
+				.name = "VBUS_R",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 5),
+				.end = AB5500_IRQ(8, 5),
+			},
+			{
+				.name = "VBUS_F",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 6),
+				.end = AB5500_IRQ(8, 6),
+			},
+			{
+				.name = "CHGstate_10_PCVBUSchg",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(8, 7),
+				.end = AB5500_IRQ(8, 7),
+			},
+			{
+				.name = "DCIOreverse_ovc",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(9, 0),
+				.end = AB5500_IRQ(9, 0),
+			},
+			{
+				.name = "USBCharDetDone",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(9, 1),
+				.end = AB5500_IRQ(9, 1),
+			},
+			{
+				.name = "DCIO_no_limit",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(9, 2),
+				.end = AB5500_IRQ(9, 2),
+			},
+			{
+				.name = "USB_suspend",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(9, 3),
+				.end = AB5500_IRQ(9, 3),
+			},
+			{
+				.name = "DCIOreverse_fwdcurrent",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(9, 4),
+				.end = AB5500_IRQ(9, 4),
+			},
+			{
+				.name = "Vbus_Imeasmax_change",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(9, 5),
+				.end = AB5500_IRQ(9, 6),
+			},
+			{
+				.name = "OVV",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(14, 5),
+				.end = AB5500_IRQ(14, 5),
+			},
+			{
+				.name = "USBcharging_NOTok",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(15, 3),
+				.end = AB5500_IRQ(15, 3),
+			},
+			{
+				.name = "usb_adp_sensoroff",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(15, 6),
+				.end = AB5500_IRQ(15, 6),
+			},
+			{
+				.name = "usb_adp_probeplug",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(15, 7),
+				.end = AB5500_IRQ(15, 7),
+			},
+			{
+				.name = "usb_adp_sinkerror",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(16, 0),
+				.end = AB5500_IRQ(16, 6),
+			},
+			{
+				.name = "usb_adp_sourceerror",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(16, 1),
+				.end = AB5500_IRQ(16, 1),
+			},
+			{
+				.name = "usb_idgnd_r",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(16, 2),
+				.end = AB5500_IRQ(16, 2),
+			},
+			{
+				.name = "usb_idgnd_f",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(16, 3),
+				.end = AB5500_IRQ(16, 3),
+			},
+			{
+				.name = "usb_iddetR1",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(16, 4),
+				.end = AB5500_IRQ(16, 5),
+			},
+			{
+				.name = "usb_iddetR2",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(16, 6),
+				.end = AB5500_IRQ(16, 7),
+			},
+			{
+				.name = "usb_iddetR3",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(17, 0),
+				.end = AB5500_IRQ(17, 1),
+			},
+			{
+				.name = "usb_iddetR4",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(17, 2),
+				.end = AB5500_IRQ(17, 3),
+			},
+			{
+				.name = "CharTempWindowOk",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(17, 7),
+				.end = AB5500_IRQ(18, 0),
+			},
+			{
+				.name = "USB_SprDetect",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(18, 1),
+				.end = AB5500_IRQ(18, 1),
+			},
+			{
+				.name = "usb_adp_probe_unplug",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(18, 2),
+				.end = AB5500_IRQ(18, 2),
+			},
+			{
+				.name = "VBUSChDrop",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(18, 3),
+				.end = AB5500_IRQ(18, 4),
+			},
+			{
+				.name = "dcio_char_rec_done",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(18, 5),
+				.end = AB5500_IRQ(18, 5),
+			},
+			{
+				.name = "Charging_stopped_by_temp",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(18, 6),
+				.end = AB5500_IRQ(18, 6),
+			},
+			{
+				.name = "CHGstate_11_SafeModeVBUS",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(21, 1),
+				.end = AB5500_IRQ(21, 2),
+			},
+			{
+				.name = "CHGstate_12_comletedVBUS",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(21, 2),
+				.end = AB5500_IRQ(21, 2),
+			},
+			{
+				.name = "CHGstate_13_completedVBUS",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(21, 3),
+				.end = AB5500_IRQ(21, 3),
+			},
+			{
+				.name = "CHGstate_14_FullChgDCIO",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(21, 4),
+				.end = AB5500_IRQ(21, 4),
+			},
+			{
+				.name = "CHGstate_15_SafeModeDCIO",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(21, 5),
+				.end = AB5500_IRQ(21, 5),
+			},
+			{
+				.name = "CHGstate_16_OFFsuspendDCIO",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(21, 6),
+				.end = AB5500_IRQ(21, 6),
+			},
+			{
+				.name = "CHGstate_17_completedDCIO",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(21, 7),
+				.end = AB5500_IRQ(21, 7),
+			},
+		},
+	},
+	[AB5500_DEVID_OTP] = {
+		.name = "ab5500-otp",
+		.id = AB5500_DEVID_OTP,
+	},
+	[AB5500_DEVID_VIDEO] = {
+		.name = "ab5500-video",
+		.id = AB5500_DEVID_VIDEO,
+		.num_resources = 1,
+		.resources = (struct resource[]) {
+			{
+				.name = "plugTVdet",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(22, 2),
+				.end = AB5500_IRQ(22, 2),
+			},
+		},
+	},
+	[AB5500_DEVID_DBIECI] = {
+		.name = "ab5500-dbieci",
+		.id = AB5500_DEVID_DBIECI,
+		.num_resources = 10,
+		.resources = (struct resource[]) {
+			{
+				.name = "COLL",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(14, 0),
+				.end = AB5500_IRQ(14, 0),
+			},
+			{
+				.name = "RESERR",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(14, 1),
+				.end = AB5500_IRQ(14, 1),
+			},
+			{
+				.name = "FRAERR",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(14, 2),
+				.end = AB5500_IRQ(14, 2),
+			},
+			{
+				.name = "COMERR",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(14, 3),
+				.end = AB5500_IRQ(14, 3),
+			},
+			{
+				.name = "BSI_indicator",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(14, 4),
+				.end = AB5500_IRQ(14, 4),
+			},
+			{
+				.name = "SPDSET",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(14, 6),
+				.end = AB5500_IRQ(14, 6),
+			},
+			{
+				.name = "DSENT",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(14, 7),
+				.end = AB5500_IRQ(14, 7),
+			},
+			{
+				.name = "DREC",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(15, 0),
+				.end = AB5500_IRQ(15, 0),
+			},
+			{
+				.name = "ACCINT",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(15, 1),
+				.end = AB5500_IRQ(15, 1),
+			},
+			{
+				.name = "NOPINT",
+				.flags = IORESOURCE_IRQ,
+				.start = AB5500_IRQ(15, 2),
+				.end = AB5500_IRQ(15, 2),
+			},
+		},
+	},
+	[AB5500_DEVID_ONSWA] = {
+		.name = "ab5500-onswa",
+		.id = AB5500_DEVID_ONSWA,
+		.num_resources = 2,
+		.resources = (struct resource[]) {
+			{
+				.name	= "ONSWAn_rising",
+				.flags	= IORESOURCE_IRQ,
+				.start	= AB5500_IRQ(1, 3),
+				.end	= AB5500_IRQ(1, 3),
+			},
+			{
+				.name	= "ONSWAn_falling",
+				.flags	= IORESOURCE_IRQ,
+				.start	= AB5500_IRQ(1, 4),
+				.end	= AB5500_IRQ(1, 4),
+			},
+		},
+	},
+};
+
+/*
+ * Functionality for getting/setting register values.
+ */
+static int get_register_interruptible(struct ab5500 *ab, u8 bank, u8 reg,
+	u8 *value)
+{
+	int err;
+
+	if (bank >= AB5500_NUM_BANKS)
+		return -EINVAL;
+
+	err = mutex_lock_interruptible(&ab->access_mutex);
+	if (err)
+		return err;
+	err = db5500_prcmu_abb_read(bankinfo[bank].slave_addr, reg, value, 1);
+
+	mutex_unlock(&ab->access_mutex);
+	return err;
+}
+
+static int get_register_page_interruptible(struct ab5500 *ab, u8 bank,
+	u8 first_reg, u8 *regvals, u8 numregs)
+{
+	int err;
+
+	if (bank >= AB5500_NUM_BANKS)
+		return -EINVAL;
+
+	err = mutex_lock_interruptible(&ab->access_mutex);
+	if (err)
+		return err;
+
+	while (numregs) {
+		/* The hardware limit for get page is 4 */
+		u8 curnum = min_t(u8, numregs, 4u);
+
+		err = db5500_prcmu_abb_read(bankinfo[bank].slave_addr,
+					    first_reg, regvals, curnum);
+		if (err)
+			goto out;
+
+		numregs -= curnum;
+		first_reg += curnum;
+		regvals += curnum;
+	}
+
+out:
+	mutex_unlock(&ab->access_mutex);
+	return err;
+}
+
+static int mask_and_set_register_interruptible(struct ab5500 *ab, u8 bank,
+	u8 reg, u8 bitmask, u8 bitvalues)
+{
+	int err = 0;
+
+	if (bank >= AB5500_NUM_BANKS)
+		return -EINVAL;
+
+	if (bitmask) {
+		u8 buf;
+
+		err = mutex_lock_interruptible(&ab->access_mutex);
+		if (err)
+			return err;
+
+		if (bitmask == 0xFF) /* No need to read in this case. */
+			buf = bitvalues;
+		else { /* Read and modify the register value. */
+			err = db5500_prcmu_abb_read(bankinfo[bank].slave_addr,
+				reg, &buf, 1);
+			if (err)
+				return err;
+
+			buf = ((~bitmask & buf) | (bitmask & bitvalues));
+		}
+		/* Write the new value. */
+		err = db5500_prcmu_abb_write(bankinfo[bank].slave_addr, reg,
+					     &buf, 1);
+
+		mutex_unlock(&ab->access_mutex);
+	}
+	return err;
+}
+
+static int
+set_register_interruptible(struct ab5500 *ab, u8 bank, u8 reg, u8 value)
+{
+	return mask_and_set_register_interruptible(ab, bank, reg, 0xff, value);
+}
+
+/*
+ * Read/write permission checking functions.
+ */
+static const struct ab5500_i2c_ranges *get_bankref(u8 devid, u8 bank)
+{
+	u8 i;
+
+	if (devid < AB5500_NUM_DEVICES) {
+		for (i = 0; i < ab5500_bank_ranges[devid].nbanks; i++) {
+			if (ab5500_bank_ranges[devid].bank[i].bankid == bank)
+				return &ab5500_bank_ranges[devid].bank[i];
+		}
+	}
+	return NULL;
+}
+
+static bool page_write_allowed(u8 devid, u8 bank, u8 first_reg, u8 last_reg)
+{
+	u8 i; /* range loop index */
+	const struct ab5500_i2c_ranges *bankref;
+
+	bankref = get_bankref(devid, bank);
+	if (bankref == NULL || last_reg < first_reg)
+		return false;
+
+	for (i = 0; i < bankref->nranges; i++) {
+		if (first_reg < bankref->range[i].first)
+			break;
+		if ((last_reg <= bankref->range[i].last) &&
+			(bankref->range[i].perm & AB5500_PERM_WR))
+			return true;
+	}
+	return false;
+}
+
+static bool reg_write_allowed(u8 devid, u8 bank, u8 reg)
+{
+	return page_write_allowed(devid, bank, reg, reg);
+}
+
+static bool page_read_allowed(u8 devid, u8 bank, u8 first_reg, u8 last_reg)
+{
+	u8 i;
+	const struct ab5500_i2c_ranges *bankref;
+
+	bankref = get_bankref(devid, bank);
+	if (bankref == NULL || last_reg < first_reg)
+		return false;
+
+
+	/* Find the range (if it exists in the list) that includes first_reg. */
+	for (i = 0; i < bankref->nranges; i++) {
+		if (first_reg < bankref->range[i].first)
+			return false;
+		if (first_reg <= bankref->range[i].last)
+			break;
+	}
+	/* Make sure that the entire range up to and including last_reg is
+	 * readable. This may span several of the ranges in the list.
+	 */
+	while ((i < bankref->nranges) &&
+		(bankref->range[i].perm & AB5500_PERM_RD)) {
+		if (last_reg <= bankref->range[i].last)
+			return true;
+		if ((++i >= bankref->nranges) ||
+			(bankref->range[i].first !=
+				(bankref->range[i - 1].last + 1))) {
+			break;
+		}
+	}
+	return false;
+}
+
+static bool reg_read_allowed(u8 devid, u8 bank, u8 reg)
+{
+	return page_read_allowed(devid, bank, reg, reg);
+}
+
+
+/*
+ * The exported register access functionality.
+ */
+static int ab5500_get_chip_id(struct device *dev)
+{
+	struct ab5500 *ab = dev_get_drvdata(dev->parent);
+
+	return (int)ab->chip_id;
+}
+
+static int ab5500_mask_and_set_register_interruptible(struct device *dev,
+		u8 bank, u8 reg, u8 bitmask, u8 bitvalues)
+{
+	struct ab5500 *ab;
+	struct platform_device *pdev = to_platform_device(dev);
+
+	if ((AB5500_NUM_BANKS <= bank) ||
+		!reg_write_allowed(pdev->id, bank, reg))
+		return -EINVAL;
+
+	ab = dev_get_drvdata(dev->parent);
+	return mask_and_set_register_interruptible(ab, bank, reg,
+		bitmask, bitvalues);
+}
+
+static int ab5500_set_register_interruptible(struct device *dev, u8 bank,
+	u8 reg, u8 value)
+{
+	return ab5500_mask_and_set_register_interruptible(dev, bank, reg, 0xFF,
+		value);
+}
+
+static int ab5500_get_register_interruptible(struct device *dev, u8 bank,
+		u8 reg, u8 *value)
+{
+	struct ab5500 *ab;
+	struct platform_device *pdev = to_platform_device(dev);
+
+	if ((AB5500_NUM_BANKS <= bank) ||
+		!reg_read_allowed(pdev->id, bank, reg))
+		return -EINVAL;
+
+	ab = dev_get_drvdata(dev->parent);
+	return get_register_interruptible(ab, bank, reg, value);
+}
+
+static int ab5500_get_register_page_interruptible(struct device *dev, u8 bank,
+		u8 first_reg, u8 *regvals, u8 numregs)
+{
+	struct ab5500 *ab;
+	struct platform_device *pdev = to_platform_device(dev);
+
+	if ((AB5500_NUM_BANKS <= bank) ||
+		!page_read_allowed(pdev->id, bank,
+			first_reg, (first_reg + numregs - 1)))
+		return -EINVAL;
+
+	ab = dev_get_drvdata(dev->parent);
+	return get_register_page_interruptible(ab, bank, first_reg, regvals,
+		numregs);
+}
+
+static int
+ab5500_event_registers_startup_state_get(struct device *dev, u8 *event)
+{
+	struct ab5500 *ab;
+
+	ab = dev_get_drvdata(dev->parent);
+	if (!ab->startup_events_read)
+		return -EAGAIN; /* Try again later */
+
+	memcpy(event, ab->startup_events, AB5500_NUM_EVENT_REG);
+	return 0;
+}
+
+static int ab5500_startup_irq_enabled(struct device *dev, unsigned int irq)
+{
+	struct ab5500 *ab;
+	bool val;
+
+	ab = irq_get_chip_data(irq);
+	irq -= ab->irq_base;
+	val = ((ab->startup_events[irq / 8] & BIT(irq % 8)) != 0);
+
+	return val;
+}
+
+static struct abx500_ops ab5500_ops = {
+	.get_chip_id = ab5500_get_chip_id,
+	.get_register = ab5500_get_register_interruptible,
+	.set_register = ab5500_set_register_interruptible,
+	.get_register_page = ab5500_get_register_page_interruptible,
+	.set_register_page = NULL,
+	.mask_and_set_register = ab5500_mask_and_set_register_interruptible,
+	.event_registers_startup_state_get =
+		ab5500_event_registers_startup_state_get,
+	.startup_irq_enabled = ab5500_startup_irq_enabled,
+};
+
+static irqreturn_t ab5500_irq(int irq, void *data)
+{
+	struct ab5500 *ab = data;
+	u8 i;
+	u8 *pvalue;
+	u8 value;
+
+	prcmu_get_abb_event_buffer((void **)&pvalue);
+	if (unlikely(pvalue == NULL)) {
+		dev_err(ab->dev, "PRCMU not enabled!!!\n");
+		goto error_irq;
+	}
+	for (i = 0; i < AB5500_NUM_EVENT_REG; i++) {
+		value = readb(pvalue);
+		if (value == 0) {
+			pvalue++;
+			continue;
+		}
+
+		do {
+			int bit = __ffs(value);
+			int line = i * 8 + bit;
+
+			handle_nested_irq(ab->irq_base + line);
+			value &= ~(1 << bit);
+		} while (value);
+		pvalue++;
+	}
+
+	return IRQ_HANDLED;
+error_irq:
+	return IRQ_NONE;
+}
+
+#ifdef CONFIG_DEBUG_FS
+static struct ab5500_i2c_ranges ab5500_reg_ranges[AB5500_NUM_BANKS] = {
+	[AB5500_BANK_LED] = {
+		.bankid = AB5500_BANK_LED,
+		.nranges = 1,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x0C,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+	[AB5500_BANK_ADC] = {
+		.bankid = AB5500_BANK_ADC,
+		.nranges = 6,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x1F,
+				.last = 0x22,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x23,
+				.last = 0x24,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x26,
+				.last = 0x2D,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0x2F,
+				.last = 0x34,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x37,
+				.last = 0x57,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x58,
+				.last = 0x58,
+				.perm = AB5500_PERM_RO,
+			},
+		},
+	},
+	[AB5500_BANK_RTC] = {
+		.bankid = AB5500_BANK_RTC,
+		.nranges = 2,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x04,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x06,
+				.last = 0x0C,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+	[AB5500_BANK_STARTUP] = {
+		.bankid = AB5500_BANK_STARTUP,
+		.nranges = 12,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x01,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x1F,
+				.last = 0x1F,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x2E,
+				.last = 0x2E,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0x2F,
+				.last = 0x30,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x50,
+				.last = 0x51,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x60,
+				.last = 0x61,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x66,
+				.last = 0x8A,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x8C,
+				.last = 0x96,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0xAA,
+				.last = 0xB4,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0xB7,
+				.last = 0xBF,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0xC1,
+				.last = 0xCA,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0xD3,
+				.last = 0xE0,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+	[AB5500_BANK_DBI_ECI] = {
+		.bankid = AB5500_BANK_DBI_ECI,
+		.nranges = 3,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x07,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x10,
+				.last = 0x10,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x13,
+				.last = 0x13,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+	[AB5500_BANK_CHG] = {
+		.bankid = AB5500_BANK_CHG,
+		.nranges = 2,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x11,
+				.last = 0x11,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0x12,
+				.last = 0x1B,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+	[AB5500_BANK_FG_BATTCOM_ACC] = {
+		.bankid = AB5500_BANK_FG_BATTCOM_ACC,
+		.nranges = 2,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x0B,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0x0C,
+				.last = 0x24,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+	[AB5500_BANK_USB] = {
+		.bankid = AB5500_BANK_USB,
+		.nranges = 12,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x01,
+				.last = 0x01,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x80,
+				.last = 0x83,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x87,
+				.last = 0x8A,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x8B,
+				.last = 0x8B,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0x91,
+				.last = 0x92,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0x93,
+				.last = 0x93,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x94,
+				.last = 0x94,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0xA8,
+				.last = 0xB0,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0xB2,
+				.last = 0xB2,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0xB4,
+				.last = 0xBC,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0xBF,
+				.last = 0xBF,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0xC1,
+				.last = 0xC5,
+				.perm = AB5500_PERM_RO,
+			},
+		},
+	},
+	[AB5500_BANK_IT] = {
+		.bankid = AB5500_BANK_IT,
+		.nranges = 4,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x02,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0x20,
+				.last = 0x36,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0x40,
+				.last = 0x56,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0x60,
+				.last = 0x76,
+				.perm = AB5500_PERM_RO,
+			},
+		},
+	},
+	[AB5500_BANK_VDDDIG_IO_I2C_CLK_TST] = {
+		.bankid = AB5500_BANK_VDDDIG_IO_I2C_CLK_TST,
+		.nranges = 7,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x02,
+				.last = 0x02,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x12,
+				.last = 0x12,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x30,
+				.last = 0x34,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x40,
+				.last = 0x44,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x50,
+				.last = 0x54,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x60,
+				.last = 0x64,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x70,
+				.last = 0x74,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+	[AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP] = {
+		.bankid = AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP,
+		.nranges = 13,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x01,
+				.last = 0x01,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x02,
+				.last = 0x02,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0x0D,
+				.last = 0x0F,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x1C,
+				.last = 0x1C,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x1E,
+				.last = 0x1E,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x20,
+				.last = 0x21,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x25,
+				.last = 0x25,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x28,
+				.last = 0x2A,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x30,
+				.last = 0x33,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x40,
+				.last = 0x43,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x50,
+				.last = 0x53,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x60,
+				.last = 0x63,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x70,
+				.last = 0x73,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+	[AB5500_BANK_VIBRA] = {
+		.bankid = AB5500_BANK_VIBRA,
+		.nranges = 2,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x10,
+				.last = 0x13,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0xFE,
+				.last = 0xFE,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+	[AB5500_BANK_AUDIO_HEADSETUSB] = {
+		.bankid = AB5500_BANK_AUDIO_HEADSETUSB,
+		.nranges = 2,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x48,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0xEB,
+				.last = 0xFB,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+	[AB5500_BANK_SIM_USBSIM] = {
+		.bankid = AB5500_BANK_SIM_USBSIM,
+		.nranges = 1,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x13,
+				.last = 0x19,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+	[AB5500_BANK_VDENC] = {
+		.bankid = AB5500_BANK_VDENC,
+		.nranges = 12,
+		.range = (struct ab5500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x08,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x09,
+				.last = 0x09,
+				.perm = AB5500_PERM_RO,
+			},
+			{
+				.first = 0x0A,
+				.last = 0x12,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x15,
+				.last = 0x19,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x1B,
+				.last = 0x21,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x27,
+				.last = 0x2C,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x41,
+				.last = 0x41,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x45,
+				.last = 0x5B,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x5D,
+				.last = 0x5D,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x69,
+				.last = 0x69,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x6C,
+				.last = 0x6D,
+				.perm = AB5500_PERM_RW,
+			},
+			{
+				.first = 0x80,
+				.last = 0x81,
+				.perm = AB5500_PERM_RW,
+			},
+		},
+	},
+};
+static int ab5500_registers_print(struct seq_file *s, void *p)
+{
+	struct ab5500 *ab = s->private;
+	unsigned int i;
+	u8 bank = (u8)ab->debug_bank;
+
+	seq_printf(s, AB5500_NAME_STRING " register values:\n");
+	for (bank = 0; bank < AB5500_NUM_BANKS; bank++) {
+		seq_printf(s, " bank %u, %s (0x%x):\n", bank,
+				bankinfo[bank].name,
+				bankinfo[bank].slave_addr);
+		for (i = 0; i < ab5500_reg_ranges[bank].nranges; i++) {
+			u8 reg;
+			int err;
+
+			for (reg = ab5500_reg_ranges[bank].range[i].first;
+				reg <= ab5500_reg_ranges[bank].range[i].last;
+				reg++) {
+				u8 value;
+
+				err = get_register_interruptible(ab, bank, reg,
+						&value);
+				if (err < 0) {
+					dev_err(ab->dev, "get_reg failed %d"
+						"bank 0x%x reg 0x%x\n",
+						err, bank, reg);
+					return err;
+				}
+
+				err = seq_printf(s, "[%d/0x%02X]: 0x%02X\n",
+						bank, reg, value);
+				if (err < 0) {
+					dev_err(ab->dev,
+						"seq_printf overflow\n");
+					/*
+					 * Error is not returned here since
+					 * the output is wanted in any case
+					 */
+					return 0;
+				}
+			}
+		}
+	}
+	return 0;
+}
+
+static int ab5500_registers_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab5500_registers_print, inode->i_private);
+}
+
+static const struct file_operations ab5500_registers_fops = {
+	.open = ab5500_registers_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab5500_bank_print(struct seq_file *s, void *p)
+{
+	struct ab5500 *ab = s->private;
+
+	seq_printf(s, "%d\n", ab->debug_bank);
+	return 0;
+}
+
+static int ab5500_bank_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab5500_bank_print, inode->i_private);
+}
+
+static ssize_t ab5500_bank_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct ab5500 *ab = ((struct seq_file *)(file->private_data))->private;
+	char buf[32];
+	int buf_size;
+	unsigned long user_bank;
+	int err;
+
+	/* Get userspace string and assure termination */
+	buf_size = min(count, (sizeof(buf) - 1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	err = strict_strtoul(buf, 0, &user_bank);
+	if (err)
+		return -EINVAL;
+
+	if (user_bank >= AB5500_NUM_BANKS) {
+		dev_err(ab->dev,
+			"debugfs error input > number of banks\n");
+		return -EINVAL;
+	}
+
+	ab->debug_bank = user_bank;
+
+	return buf_size;
+}
+
+static int ab5500_address_print(struct seq_file *s, void *p)
+{
+	struct ab5500 *ab = s->private;
+
+	seq_printf(s, "0x%02X\n", ab->debug_address);
+	return 0;
+}
+
+static int ab5500_address_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab5500_address_print, inode->i_private);
+}
+
+static ssize_t ab5500_address_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct ab5500 *ab = ((struct seq_file *)(file->private_data))->private;
+	char buf[32];
+	int buf_size;
+	unsigned long user_address;
+	int err;
+
+	/* Get userspace string and assure termination */
+	buf_size = min(count, (sizeof(buf) - 1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	err = strict_strtoul(buf, 0, &user_address);
+	if (err)
+		return -EINVAL;
+	if (user_address > 0xff) {
+		dev_err(ab->dev,
+			"debugfs error input > 0xff\n");
+		return -EINVAL;
+	}
+	ab->debug_address = user_address;
+	return buf_size;
+}
+
+static int ab5500_val_print(struct seq_file *s, void *p)
+{
+	struct ab5500 *ab = s->private;
+	int err;
+	u8 regvalue;
+
+	err = get_register_interruptible(ab, (u8)ab->debug_bank,
+		(u8)ab->debug_address, &regvalue);
+	if (err) {
+		dev_err(ab->dev, "get_reg failed %d, bank 0x%x"
+			", reg 0x%x\n", err, ab->debug_bank,
+			ab->debug_address);
+		return -EINVAL;
+	}
+	seq_printf(s, "0x%02X\n", regvalue);
+
+	return 0;
+}
+
+static int ab5500_val_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab5500_val_print, inode->i_private);
+}
+
+static ssize_t ab5500_val_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct ab5500 *ab = ((struct seq_file *)(file->private_data))->private;
+	char buf[32];
+	int buf_size;
+	unsigned long user_val;
+	int err;
+	u8 regvalue;
+
+	/* Get userspace string and assure termination */
+	buf_size = min(count, (sizeof(buf)-1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	err = strict_strtoul(buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	if (user_val > 0xff) {
+		dev_err(ab->dev,
+			"debugfs error input > 0xff\n");
+		return -EINVAL;
+	}
+	err = mask_and_set_register_interruptible(
+		ab, (u8)ab->debug_bank,
+		(u8)ab->debug_address, 0xFF, (u8)user_val);
+	if (err)
+		return -EINVAL;
+
+	get_register_interruptible(ab, (u8)ab->debug_bank,
+		(u8)ab->debug_address, &regvalue);
+	if (err)
+		return -EINVAL;
+
+	return buf_size;
+}
+
+static const struct file_operations ab5500_bank_fops = {
+	.open = ab5500_bank_open,
+	.write = ab5500_bank_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ab5500_address_fops = {
+	.open = ab5500_address_open,
+	.write = ab5500_address_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ab5500_val_fops = {
+	.open = ab5500_val_open,
+	.write = ab5500_val_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static struct dentry *ab5500_dir;
+static struct dentry *ab5500_reg_file;
+static struct dentry *ab5500_bank_file;
+static struct dentry *ab5500_address_file;
+static struct dentry *ab5500_val_file;
+
+static inline void ab5500_setup_debugfs(struct ab5500 *ab)
+{
+	ab->debug_bank = AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP;
+	ab->debug_address = AB5500_CHIP_ID;
+
+	ab5500_dir = debugfs_create_dir(AB5500_NAME_STRING, NULL);
+	if (!ab5500_dir)
+		goto exit_no_debugfs;
+
+	ab5500_reg_file = debugfs_create_file("all-bank-registers",
+		S_IRUGO, ab5500_dir, ab, &ab5500_registers_fops);
+	if (!ab5500_reg_file)
+		goto exit_destroy_dir;
+
+	ab5500_bank_file = debugfs_create_file("register-bank",
+		(S_IRUGO | S_IWUGO), ab5500_dir, ab, &ab5500_bank_fops);
+	if (!ab5500_bank_file)
+		goto exit_destroy_reg;
+
+	ab5500_address_file = debugfs_create_file("register-address",
+		(S_IRUGO | S_IWUGO), ab5500_dir, ab, &ab5500_address_fops);
+	if (!ab5500_address_file)
+		goto exit_destroy_bank;
+
+	ab5500_val_file = debugfs_create_file("register-value",
+		(S_IRUGO | S_IWUGO), ab5500_dir, ab, &ab5500_val_fops);
+	if (!ab5500_val_file)
+		goto exit_destroy_address;
+
+	return;
+
+exit_destroy_address:
+	debugfs_remove(ab5500_address_file);
+exit_destroy_bank:
+	debugfs_remove(ab5500_bank_file);
+exit_destroy_reg:
+	debugfs_remove(ab5500_reg_file);
+exit_destroy_dir:
+	debugfs_remove(ab5500_dir);
+exit_no_debugfs:
+	dev_err(ab->dev, "failed to create debugfs entries.\n");
+	return;
+}
+
+static inline void ab5500_remove_debugfs(void)
+{
+	debugfs_remove(ab5500_val_file);
+	debugfs_remove(ab5500_address_file);
+	debugfs_remove(ab5500_bank_file);
+	debugfs_remove(ab5500_reg_file);
+	debugfs_remove(ab5500_dir);
+}
+
+#else /* !CONFIG_DEBUG_FS */
+static inline void ab5500_setup_debugfs(struct ab5500 *ab)
+{
+}
+static inline void ab5500_remove_debugfs(void)
+{
+}
+#endif
+
+/*
+ * ab5500_setup : Basic set-up, datastructure creation/destruction
+ *		  and I2C interface.This sets up a default config
+ *		  in the AB5500 chip so that it will work as expected.
+ * @ab :	  Pointer to ab5500 structure
+ * @settings :    Pointer to struct abx500_init_settings
+ * @size :        Size of init data
+ */
+static int __init ab5500_setup(struct ab5500 *ab,
+	struct abx500_init_settings *settings, unsigned int size)
+{
+	int err = 0;
+	int i;
+
+	for (i = 0; i < size; i++) {
+		err = mask_and_set_register_interruptible(ab,
+			settings[i].bank,
+			settings[i].reg,
+			0xFF, settings[i].setting);
+		if (err)
+			goto exit_no_setup;
+
+		/* If event mask register update the event mask in ab5500 */
+		if ((settings[i].bank == AB5500_BANK_IT) &&
+			(AB5500_MASK_BASE <= settings[i].reg) &&
+			(settings[i].reg <= AB5500_MASK_END)) {
+			ab->mask[settings[i].reg - AB5500_MASK_BASE] =
+				settings[i].setting;
+		}
+	}
+exit_no_setup:
+	return err;
+}
+
+static void ab5500_irq_mask(struct irq_data *data)
+{
+	struct ab5500 *ab = irq_data_get_irq_chip_data(data);
+	int offset = data->irq - ab->irq_base;
+	int index = offset / 8;
+	int mask = BIT(offset % 8);
+
+	ab->mask[index] |= mask;
+}
+
+static void ab5500_irq_unmask(struct irq_data *data)
+{
+	struct ab5500 *ab = irq_data_get_irq_chip_data(data);
+	int offset = data->irq - ab->irq_base;
+	int index = offset / 8;
+	int mask = BIT(offset % 8);
+
+	ab->mask[index] &= ~mask;
+}
+
+static void ab5500_irq_lock(struct irq_data *data)
+{
+	struct ab5500 *ab = irq_data_get_irq_chip_data(data);
+
+	mutex_lock(&ab->irq_lock);
+}
+
+static void ab5500_irq_sync_unlock(struct irq_data *data)
+{
+	struct ab5500 *ab = irq_data_get_irq_chip_data(data);
+	int i;
+
+	for (i = 0; i < AB5500_NUM_IRQ_REGS; i++) {
+		u8 old = ab->oldmask[i];
+		u8 new = ab->mask[i];
+		int reg;
+
+		if (new == old)
+			continue;
+
+		ab->oldmask[i] = new;
+
+		reg = AB5500_IT_MASK0_REG + i;
+		set_register_interruptible(ab, AB5500_BANK_IT, reg, new);
+	}
+
+	mutex_unlock(&ab->irq_lock);
+}
+
+static struct irq_chip ab5500_irq_chip = {
+	.name			= "ab5500",
+	.irq_mask		= ab5500_irq_mask,
+	.irq_unmask		= ab5500_irq_unmask,
+	.irq_bus_lock		= ab5500_irq_lock,
+	.irq_bus_sync_unlock	= ab5500_irq_sync_unlock,
+};
+
+struct ab_family_id {
+	u8	id;
+	char	*name;
+};
+
+static const struct ab_family_id ids[] __initdata = {
+	/* AB5500 */
+	{
+		.id = AB5500_1_0,
+		.name = "1.0"
+	},
+	{
+		.id = AB5500_1_1,
+		.name = "1.1"
+	},
+	/* Terminator */
+	{
+		.id = 0x00,
+	}
+};
+
+static int ab5500_irq_init(struct ab5500 *ab)
+{
+	struct ab5500_platform_data *ab5500_plf_data =
+				dev_get_platdata(ab->dev);
+	int i;
+	unsigned int irq;
+
+	for (i = 0; i < ab5500_plf_data->irq.count; i++) {
+
+		irq = ab5500_plf_data->irq.base + i;
+		irq_set_chip_data(irq, ab);
+		irq_set_chip_and_handler(irq, &ab5500_irq_chip,
+			handle_simple_irq);
+		irq_set_nested_thread(irq, 1);
+#ifdef CONFIG_ARM
+		set_irq_flags(irq, IRQF_VALID);
+#else
+		set_irq_noprobe(irq);
+#endif
+	}
+	return 0;
+}
+
+static void ab5500_irq_remove(struct ab5500 *ab)
+{
+	struct ab5500_platform_data *ab5500_plf_data =
+				dev_get_platdata(ab->dev);
+	int i;
+	unsigned int irq;
+
+	for (i = 0; i < ab5500_plf_data->irq.count; i++) {
+		irq = ab5500_plf_data->irq.base + i;
+#ifdef CONFIG_ARM
+		set_irq_flags(irq, 0);
+#endif
+		irq_set_chip_and_handler(irq, NULL, NULL);
+		irq_set_chip_data(irq, NULL);
+	}
+}
+
+static int __init ab5500_probe(struct platform_device *pdev)
+{
+	struct ab5500 *ab;
+	struct ab5500_platform_data *ab5500_plf_data =
+		pdev->dev.platform_data;
+	struct resource *res;
+	int err;
+	int i;
+
+	ab = kzalloc(sizeof(struct ab5500), GFP_KERNEL);
+	if (!ab) {
+		dev_err(&pdev->dev,
+			"could not allocate " AB5500_NAME_STRING " device\n");
+		return -ENOMEM;
+	}
+
+	/* Initialize data structure */
+	mutex_init(&ab->access_mutex);
+	mutex_init(&ab->irq_lock);
+	ab->dev = &pdev->dev;
+	ab->irq_base = ab5500_plf_data->irq.base;
+
+	platform_set_drvdata(pdev, ab);
+
+	/* Read chip ID register */
+	err = get_register_interruptible(ab, AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP,
+		AB5500_CHIP_ID, &ab->chip_id);
+	if (err) {
+		dev_err(&pdev->dev, "could not communicate with the analog "
+			"baseband chip\n");
+		goto exit_no_detect;
+	}
+
+	for (i = 0; ids[i].id != 0x0; i++) {
+		if (ids[i].id == ab->chip_id) {
+			snprintf(&ab->chip_name[0], sizeof(ab->chip_name) - 1,
+				AB5500_ID_FORMAT_STRING, ids[i].name);
+			break;
+		}
+	}
+
+	if (ids[i].id == 0x0) {
+		dev_err(&pdev->dev, "unknown analog baseband chip id: 0x%x\n",
+			ab->chip_id);
+		dev_err(&pdev->dev, "driver not started!\n");
+		goto exit_no_detect;
+	}
+
+	dev_info(&pdev->dev, "detected AB chip: %s\n", &ab->chip_name[0]);
+
+	/* Readout ab->starup_events when prcmu driver is in place */
+	ab->startup_events[0] = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "ab5500_platform_get_resource error\n");
+		goto exit_no_detect;
+	}
+	ab->ab5500_irq = res->start;
+
+	/* Clear and mask all interrupts */
+	for (i = 0; i < AB5500_NUM_IRQ_REGS; i++) {
+		u8 latchreg = AB5500_IT_LATCH0_REG + i;
+		u8 maskreg = AB5500_IT_MASK0_REG + i;
+		u8 val;
+
+		get_register_interruptible(ab, AB5500_BANK_IT, latchreg, &val);
+		set_register_interruptible(ab, AB5500_BANK_IT, maskreg, 0xff);
+		ab->mask[i] = ab->oldmask[i] = 0xff;
+	}
+
+	if (ab->irq_base) {
+		err = ab5500_irq_init(ab);
+		if (err)
+			return err;
+
+	err = request_threaded_irq(res->start, NULL, ab5500_irq,
+				   IRQF_NO_SUSPEND | IRQF_ONESHOT,
+				   "ab5500-core", ab);
+		if (err)
+			goto exit_remove_irq;
+
+	}
+	prcmu_config_abb_event_readout(AB5500_INTERRUPTS);
+	/* This real unpredictable IRQ is of course sampled for entropy */
+	rand_initialize_irq(res->start);
+
+	err = abx500_register_ops(&pdev->dev, &ab5500_ops);
+	if (err) {
+		dev_err(&pdev->dev, "ab5500_register ops error\n");
+		goto exit_no_irq;
+	}
+
+	/* Set up and register the platform devices. */
+	for (i = 0; i < AB5500_NUM_DEVICES; i++) {
+		ab5500_devs[i].platform_data = ab5500_plf_data->dev_data[i];
+	}
+
+	err = mfd_add_devices(&pdev->dev, 0, ab5500_devs,
+		ARRAY_SIZE(ab5500_devs), NULL,
+		ab5500_plf_data->irq.base);
+	if (err) {
+		dev_err(&pdev->dev, "ab5500_mfd_add_device error\n");
+		goto exit_no_irq;
+	}
+	err = ab5500_setup(ab, ab5500_plf_data->init_settings,
+		ab5500_plf_data->init_settings_sz);
+	if (err) {
+		dev_err(&pdev->dev, "ab5500_setup error\n");
+		goto exit_no_irq;
+	}
+	ab5500_setup_debugfs(ab);
+	return 0;
+
+exit_no_irq:
+	if (ab->irq_base) {
+		free_irq(ab->ab5500_irq, ab);
+exit_remove_irq:
+	ab5500_irq_remove(ab);
+	}
+exit_no_detect:
+	kfree(ab);
+	return err;
+}
+
+static int __exit ab5500_remove(struct platform_device *pdev)
+{
+	struct ab5500 *ab = platform_get_drvdata(pdev);
+	struct resource *res;
+
+	/*
+	 * At this point, all subscribers should have unregistered
+	 * their notifiers so deactivate IRQ
+	 */
+	ab5500_remove_debugfs();
+	mfd_remove_devices(&pdev->dev);
+	if (ab->irq_base) {
+		free_irq(ab->ab5500_irq, ab);
+		ab5500_irq_remove(ab);
+	}
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	kfree(ab);
+	return 0;
+}
+
+static struct platform_driver ab5500_driver = {
+	.driver = {
+		.name = "ab5500-core",
+		.owner = THIS_MODULE,
+	},
+	.remove  = __exit_p(ab5500_remove),
+};
+
+static int __init ab5500_core_init(void)
+{
+	return platform_driver_probe(&ab5500_driver, ab5500_probe);
+}
+
+static void __exit ab5500_core_exit(void)
+{
+	platform_driver_unregister(&ab5500_driver);
+}
+
+subsys_initcall(ab5500_core_init);
+module_exit(ab5500_core_exit);
+
+MODULE_AUTHOR("Mattias Wallin <mattias.wallin@stericsson.com>");
+MODULE_DESCRIPTION("AB5500 core driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/ab5500-gpadc.c b/drivers/mfd/ab5500-gpadc.c
new file mode 100644
index 00000000000..dbfc9c30e38
--- /dev/null
+++ b/drivers/mfd/ab5500-gpadc.c
@@ -0,0 +1,1179 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Vijaya Kumar K <vijay.kilari@stericsson.com>
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/spinlock.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500-gpadc.h>
+
+/*
+ * Manual mode ADC registers
+ */
+#define AB5500_GPADC_MANUAL_STAT_REG		0x1F
+#define AB5500_GPADC_MANDATAL_REG		0x21
+#define AB5500_GPADC_MANDATAH_REG		0x20
+#define AB5500_GPADC_MANUAL_MUX_CTRL		0x22
+#define AB5500_GPADC_MANUAL_MODE_CTRL		0x23
+#define AB5500_GPADC_MANUAL_MODE_CTRL2		0x24
+/*
+ * Auto/Polling mode ADC registers
+ */
+#define AB5500_GPADC_AUTO_VBAT_MAX		0x26
+#define AB5500_GPADC_AUTO_VBAT_MIN_TXON		0x27
+#define AB5500_GPADC_AUTO_VBAT_MIN_NOTX		0x28
+#define AB5500_GPADC_AUTO_VBAT_AVGH		0x29
+#define AB5500_GPADC_AUTO_VBAT_AVGL		0x2A
+#define AB5500_GPADC_AUTO_ICHAR_MAX		0x2B
+#define AB5500_GPADC_AUTO_ICHAR_MIN		0x2C
+#define AB5500_GPADC_AUTO_ICHAR_AVG		0x2D
+#define AB5500_GPADC_AUTO_CTRL2			0x2F
+#define AB5500_GPADC_AUTO_CTRL1			0x30
+#define AB5500_GPADC_AUTO_PWR_CTRL		0x31
+#define AB5500_GPADC_AUTO_TRIG_VBAT_MIN_TXON	0x32
+#define AB5500_GPADC_AUTO_TRIG_VBAT_MIN_NOTX	0x33
+#define AB5500_GPADC_AUTO_TRIG_ADOUT0_CTRL	0x34
+#define AB5500_GPADC_AUTO_TRIG_ADOUT1_CTRL	0x35
+#define AB5500_GPADC_AUTO_TRIG0_MUX_CTRL	0x37
+#define AB5500_GPADC_AUTO_XTALTEMP_CTRL		0x57
+#define AB5500_GPADC_KELVIN_CTRL		0xFE
+
+/* gpadc constants */
+#define AB5500_INT_ADC_TRIG0		0x0
+#define AB5500_INT_ADC_TRIG1		0x1
+#define AB5500_INT_ADC_TRIG2		0x2
+#define AB5500_INT_ADC_TRIG3		0x3
+#define AB5500_INT_ADC_TRIG4		0x4
+#define AB5500_INT_ADC_TRIG5		0x5
+#define AB5500_INT_ADC_TRIG6		0x6
+#define AB5500_INT_ADC_TRIG7		0x7
+
+#define AB5500_GPADC_AUTO_TRIG_INDEX	AB5500_GPADC_AUTO_TRIG0_MUX_CTRL
+#define GPADC_MANUAL_READY		0x01
+#define GPADC_MANUAL_ADOUT0_MASK	0x30
+#define GPADC_MANUAL_ADOUT1_MASK	0xC0
+#define GPADC_MANUAL_ADOUT0_ON		0x10
+#define GPADC_MANUAL_ADOUT1_ON		0x40
+#define MUX_SCALE_VBAT_MASK		0x02
+#define MUX_SCALE_45			0x02
+#define MUX_SCALE_BDATA_MASK		0x01
+#define MUX_SCALE_BDATA27		0x00
+#define MUX_SCALE_BDATA18		0x01
+#define MUX_SCALE_ACCDET2_MASK		0x01
+#define MUX_SCALE_ACCDET3_MASK		0x02
+#define ACCDET2_SCALE_VOL27		0x00
+#define ACCDET3_SCALE_VOL27		0x00
+#define TRIGX_FREQ_MASK			0x07
+#define AUTO_VBAT_MASK			0x10
+#define AUTO_VBAT_ON			0x10
+#define TRIG_VBAT_TXON_ARM_MASK		0x08
+#define TRIG_VBAT_NOTX_ARM_MASK		0x04
+#define TRIGX_ARM_MASK			0x20
+#define TRIGX_ARM			0x20
+#define TRIGX_MUX_SELECT		0x1F
+#define ADC_CAL_OFF_MASK		0x04
+#define ADC_ON_MODE_MASK		0x03
+#define ADC_CAL_ON			0x00
+#define ADC_FULLPWR			0x03
+#define ADC_XTAL_FORCE_MASK		0x80
+#define ADC_XTAL_FORCE_EN		0x80
+#define ADC_XTAL_FORCE_DI		0x00
+#define ADOUT0				0x01
+#define ADOUT1				0x02
+#define MIN_INDEX			0x02
+#define MAX_INDEX			0x03
+#define CTRL_INDEX			0x01
+
+/* GPADC constants from AB5500 spec  */
+#define BTEMP_MIN		0
+#define BTEMP_MAX		1800
+#define BDATA_MIN		0
+#define BDATA_MAX		2750
+#define PCBTEMP_MIN		0
+#define PCBTEMP_MAX		1800
+#define XTALTEMP_MIN		0
+#define XTALTEMP_MAX		1800
+#define DIETEMP_MIN		0
+#define DIETEMP_MAX		1800
+#define VBUS_I_MIN		0
+#define VBUS_I_MAX		1600
+#define VBUS_V_MIN		0
+#define VBUS_V_MAX		20000
+#define ACCDET2_MIN		0
+#define ACCDET2_MAX		2500
+#define ACCDET3_MIN		0
+#define ACCDET3_MAX		2500
+#define VBAT_MIN		2300
+#define VBAT_MAX		4500
+#define BKBAT_MIN		0
+#define BKBAT_MAX		2750
+#define USBID_MIN		0
+#define USBID_MAX		1800
+#define KELVIN_MIN		0
+#define KELVIN_MAX		4500
+
+/* This is used for calibration */
+#define ADC_RESOLUTION			1023
+#define AUTO_ADC_RESOLUTION		255
+
+enum adc_auto_channels {
+	ADC_INPUT_TRIG0 = 0,
+	ADC_INPUT_TRIG1,
+	ADC_INPUT_TRIG2,
+	ADC_INPUT_TRIG3,
+	ADC_INPUT_TRIG4,
+	ADC_INPUT_TRIG5,
+	ADC_INPUT_TRIG6,
+	ADC_INPUT_TRIG7,
+	ADC_INPUT_VBAT_TXOFF,
+	ADC_INPUT_VBAT_TXON,
+	N_AUTO_TRIGGER
+};
+
+/**
+ * struct adc_auto_trigger - AB5500 GPADC auto trigger
+ * @adc_mux			Mux input
+ * @flag			Status of trigger
+ * @freq			Frequency of conversion
+ * @adout			Adout to pull
+ * @trig_min			trigger minimum value
+ * @trig_max			trigger maximum value
+ * @auto_adc_callback		notification callback
+ */
+struct adc_auto_trigger {
+	u8 auto_mux;
+	u8 flag;
+	u8 freq;
+	u8 adout;
+	u8 trig_min;
+	u8 trig_max;
+	int (*auto_callb)(int mux);
+};
+
+/**
+ * struct ab5500_btemp_interrupts - ab5500 interrupts
+ * @name:	name of the interrupt
+ * @isr		function pointer to the isr
+ */
+struct ab5500_adc_interrupts {
+	char *name;
+	irqreturn_t (*isr)(int irq, void *data);
+};
+
+/**
+ * struct ab5500_gpadc - AB5500 GPADC device information
+ * @chip_id			ABB chip id
+ * @dev:			pointer to the struct device
+ * @node:			a list of AB5500 GPADCs, hence prepared for
+				reentrance
+ * @ab5500_gpadc_complete:	pointer to the struct completion, to indicate
+ *				the completion of gpadc conversion
+ * @ab5500_gpadc_lock:		structure of type mutex
+ * @regu:			pointer to the struct regulator
+ * @irq:			interrupt number that is used by gpadc
+ * @cal_data			array of ADC calibration data structs
+ * @auto_trig			auto trigger channel
+ * @gpadc_trigX_work		work items for trigger channels
+ */
+struct ab5500_gpadc {
+	u8 chip_id;
+	struct device *dev;
+	struct list_head node;
+	struct mutex ab5500_gpadc_lock;
+	struct regulator *regu;
+	int irq;
+	int prev_bdata;
+	spinlock_t gpadc_auto_lock;
+	struct adc_auto_trigger adc_trig[N_AUTO_TRIGGER];
+	struct workqueue_struct *gpadc_wq;
+	struct work_struct gpadc_trig0_work;
+	struct work_struct gpadc_trig1_work;
+	struct work_struct gpadc_trig2_work;
+	struct work_struct gpadc_trig3_work;
+	struct work_struct gpadc_trig4_work;
+	struct work_struct gpadc_trig5_work;
+	struct work_struct gpadc_trig6_work;
+	struct work_struct gpadc_trig7_work;
+	struct work_struct gpadc_trig_vbat_txon_work;
+	struct work_struct gpadc_trig_vbat_txoff_work;
+};
+
+static LIST_HEAD(ab5500_gpadc_list);
+
+struct adc_data {
+	u8 mux;
+	int min;
+	int max;
+	int adout;
+};
+
+#define ADC_DATA(_id, _mux, _min, _max, _adout)	\
+	[_id] = {				\
+		.mux = _mux,		\
+		.min = _min,		\
+		.max = _max,		\
+		.adout = _adout		\
+	}
+
+struct adc_data adc_tab[] = {
+	ADC_DATA(BTEMP_BALL, 0x0D, BTEMP_MIN, BTEMP_MAX, ADOUT0),
+	ADC_DATA(BAT_CTRL, 0x0D, BDATA_MIN, BDATA_MAX, 0),
+	ADC_DATA(MAIN_BAT_V, 0x0C, VBAT_MIN, VBAT_MAX, 0),
+	ADC_DATA(MAIN_BAT_V_TXON, 0x0C, VBAT_MIN, VBAT_MAX, 0),
+	ADC_DATA(VBUS_V, 0x10, VBUS_V_MIN, VBUS_V_MAX, 0),
+	ADC_DATA(USB_CHARGER_C,	0x0A, VBUS_I_MIN, VBUS_I_MAX, 0),
+	ADC_DATA(BK_BAT_V, 0x07, BKBAT_MIN, BKBAT_MAX, 0),
+	ADC_DATA(DIE_TEMP, 0x0F, DIETEMP_MIN, DIETEMP_MAX, ADOUT0),
+	ADC_DATA(PCB_TEMP, 0x13, PCBTEMP_MIN, PCBTEMP_MAX, ADOUT0),
+	ADC_DATA(XTAL_TEMP, 0x06, XTALTEMP_MIN, XTALTEMP_MAX, ADOUT0),
+	ADC_DATA(USB_ID, 0x1A, USBID_MIN, USBID_MAX, 0),
+	ADC_DATA(ACC_DETECT2, 0x18, ACCDET2_MIN, ACCDET2_MAX, 0),
+	ADC_DATA(ACC_DETECT3, 0x19, ACCDET3_MIN, ACCDET3_MAX, 0),
+	ADC_DATA(MAIN_BAT_V_TRIG_MIN, 0x0C, VBAT_MIN, VBAT_MAX, 0),
+	ADC_DATA(MAIN_BAT_V_TXON_TRIG_MIN, 0x0C, VBAT_MIN, VBAT_MAX, 0),
+};
+/**
+ * ab5500_gpadc_get() - returns a reference to the primary AB5500 GPADC
+ * (i.e. the first GPADC in the instance list)
+ */
+struct ab5500_gpadc *ab5500_gpadc_get(const char *name)
+{
+	struct ab5500_gpadc *gpadc;
+	list_for_each_entry(gpadc, &ab5500_gpadc_list, node) {
+		if (!strcmp(name, dev_name(gpadc->dev)))
+			return gpadc;
+	}
+
+	return ERR_PTR(-ENOENT);
+}
+EXPORT_SYMBOL(ab5500_gpadc_get);
+
+#define CONV(min, max, x)\
+	((min) + ((((max)-(min))*(x))/ADC_RESOLUTION))
+
+static int ab5500_gpadc_ad_to_voltage(struct ab5500_gpadc *gpadc,
+					u8 in, u16 ad_val)
+{
+	int res;
+
+	switch (in) {
+	case PCB_TEMP:
+	case BTEMP_BALL:
+	case MAIN_BAT_V:
+	case MAIN_BAT_V_TXON:
+	case ACC_DETECT2:
+	case ACC_DETECT3:
+	case VBUS_V:
+	case USB_CHARGER_C:
+	case BK_BAT_V:
+	case XTAL_TEMP:
+	case USB_ID:
+	case BAT_CTRL:
+		res = CONV(adc_tab[in].min, adc_tab[in].max, ad_val);
+		break;
+	case DIE_TEMP:
+		/*
+		 * From the AB5500 product specification
+		 * T(deg cel) = 27 - ((ADCode - 709)/2.4213)
+		 * 27 + 709/2.4213 - ADCode/2.4123
+		 * (320 - ADCode)/2.4213
+		 */
+		res = 320 - (((unsigned long)ad_val * 1000000) / 242130) / 10;
+		break;
+	default:
+		dev_err(gpadc->dev,
+			"unknown channel, not possible to convert\n");
+		res = -EINVAL;
+		break;
+	}
+	return res;
+}
+
+/**
+ * ab5500_gpadc_convert() - gpadc conversion
+ * @input:	analog input to be converted to digital data
+ *
+ * This function converts the selected analog i/p to digital
+ * data.
+ */
+int ab5500_gpadc_convert(struct ab5500_gpadc *gpadc, u8 input)
+{
+	int result, ret = -EINVAL;
+	u16 data = 0;
+	u8 looplimit = 0;
+	u8 status = 0;
+	u8 low_data, high_data, adout_mask, adout_val;
+
+	if (!gpadc)
+		return -ENODEV;
+
+	mutex_lock(&gpadc->ab5500_gpadc_lock);
+
+	switch (input) {
+	case MAIN_BAT_V:
+	case MAIN_BAT_V_TXON:
+		/*
+		 * The value of mux scale volatage depends
+		 * on the type of battery
+		 * for LI-ion use MUX_SCALE_35 => 2.3-3.5V
+		 * for LiFePo4 use MUX_SCALE_45 => 2.3-4.5V
+		 * Check type of battery from platform data TODO ???
+		 */
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+			MUX_SCALE_VBAT_MASK, MUX_SCALE_45);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: failed to read status\n");
+			goto out;
+		}
+		break;
+	case BTEMP_BALL:
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+				MUX_SCALE_BDATA_MASK, MUX_SCALE_BDATA27);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set mux scale\n");
+			goto out;
+		}
+		break;
+	case BAT_CTRL:
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+				MUX_SCALE_BDATA_MASK, MUX_SCALE_BDATA27);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set mux scale\n");
+			goto out;
+		}
+		break;
+	case XTAL_TEMP:
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC, AB5500_GPADC_AUTO_XTALTEMP_CTRL,
+			ADC_XTAL_FORCE_MASK, ADC_XTAL_FORCE_EN);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set xtaltemp\n");
+			goto out;
+		}
+		break;
+	case ACC_DETECT2:
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL2,
+				MUX_SCALE_ACCDET2_MASK, ACCDET2_SCALE_VOL27);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set accdet2\n");
+			goto out;
+		}
+		break;
+	case ACC_DETECT3:
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL2,
+				MUX_SCALE_ACCDET3_MASK, ACCDET3_SCALE_VOL27);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set accdet3\n");
+			goto out;
+		}
+		break;
+	case USB_CHARGER_C:
+	case VBUS_V:
+	case BK_BAT_V:
+	case USB_ID:
+	case PCB_TEMP:
+	case DIE_TEMP:
+		break;
+	default:
+		dev_err(gpadc->dev, "gpadc: Wrong adc\n");
+		goto out;
+		break;
+	}
+	if (adc_tab[input].adout) {
+		adout_mask = adc_tab[input].adout == ADOUT0 ?
+			GPADC_MANUAL_ADOUT0_MASK : GPADC_MANUAL_ADOUT1_MASK;
+		adout_val = adc_tab[input].adout == ADOUT0 ?
+			GPADC_MANUAL_ADOUT0_ON : GPADC_MANUAL_ADOUT1_ON;
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+				adout_mask, adout_val);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set ADOUT\n");
+			goto out;
+		}
+	}
+	ret = abx500_set_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			AB5500_GPADC_MANUAL_MUX_CTRL, adc_tab[input].mux);
+	if (ret < 0) {
+		dev_err(gpadc->dev,
+			"gpadc: fail to trigger manual conv\n");
+		goto out;
+	}
+	/* wait for completion of conversion */
+	looplimit = 0;
+	do {
+		msleep(1);
+		ret = abx500_get_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC, AB5500_GPADC_MANUAL_STAT_REG,
+			&status);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: failed to read status\n");
+			goto out;
+		}
+		if (status & GPADC_MANUAL_READY)
+			break;
+	} while (++looplimit < 2);
+	if (looplimit >= 2) {
+		dev_err(gpadc->dev, "timeout:failed to complete conversion\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/*
+	 * Disable ADOUT for measurement
+	 */
+	if (adc_tab[input].adout) {
+		adout_mask = adc_tab[input].adout == ADOUT0 ?
+			GPADC_MANUAL_ADOUT0_MASK : GPADC_MANUAL_ADOUT1_MASK;
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+				adout_mask, 0x0);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to disable ADOUT\n");
+			goto out;
+		}
+	}
+	/*
+	 * Disable XTAL TEMP
+	 */
+	if (input == XTAL_TEMP) {
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC, AB5500_GPADC_AUTO_XTALTEMP_CTRL,
+			ADC_XTAL_FORCE_MASK, ADC_XTAL_FORCE_DI);
+		if (ret < 0) {
+			dev_err(gpadc->dev,
+				"gpadc: fail to disable xtaltemp\n");
+			goto out;
+		}
+	}
+	/* Read the converted RAW data */
+	ret = abx500_get_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			AB5500_GPADC_MANDATAL_REG, &low_data);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: read low data failed\n");
+		goto out;
+	}
+
+	ret = abx500_get_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			AB5500_GPADC_MANDATAH_REG, &high_data);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: read high data failed\n");
+		goto out;
+	}
+
+	data = (high_data << 2) | (low_data >> 6);
+	if (input == BAT_CTRL || input == BTEMP_BALL) {
+		/*
+		 * TODO: Re-check with h/w team
+		 * discard null or value < 5, as there is some error
+		 * in conversion
+		 */
+		if (data < 5)
+			data = gpadc->prev_bdata;
+		else
+			gpadc->prev_bdata = data;
+	}
+	result = ab5500_gpadc_ad_to_voltage(gpadc, input, data);
+
+	mutex_unlock(&gpadc->ab5500_gpadc_lock);
+	return result;
+
+out:
+	mutex_unlock(&gpadc->ab5500_gpadc_lock);
+	dev_err(gpadc->dev,
+		"gpadc: Failed to AD convert channel %d\n", input);
+	return ret;
+}
+EXPORT_SYMBOL(ab5500_gpadc_convert);
+
+/**
+ * ab5500_gpadc_program_auto() - gpadc conversion auto conversion
+ * @trig_index:	Generic trigger channel for conversion
+ *
+ * This function program the auto trigger channel
+ */
+static int ab5500_gpadc_program_auto(struct ab5500_gpadc *gpadc, int trig)
+{
+	int ret;
+	u8 adout;
+#define MIN_INDEX	0x02
+#define MAX_INDEX	0x03
+#define CTRL_INDEX	0x01
+	ret = abx500_set_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			AB5500_GPADC_AUTO_TRIG_INDEX + (trig << 2) + MIN_INDEX,
+			gpadc->adc_trig[trig].trig_min);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: fail to program min\n");
+		return ret;
+	}
+	ret = abx500_set_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			AB5500_GPADC_AUTO_TRIG_INDEX + (trig << 2) + MAX_INDEX,
+			gpadc->adc_trig[trig].trig_max);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: fail to program max\n");
+		return ret;
+	}
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+		AB5500_BANK_ADC, AB5500_GPADC_AUTO_TRIG_INDEX + (trig << 2),
+		TRIGX_MUX_SELECT, gpadc->adc_trig[trig].auto_mux);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: fail to select mux\n");
+		return ret;
+	}
+	if (gpadc->adc_trig[trig].adout) {
+		adout = gpadc->adc_trig[trig].adout == ADOUT0 ?
+				gpadc->adc_trig[trig].adout << 6 :
+				gpadc->adc_trig[trig].adout << 5;
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC,
+			AB5500_GPADC_AUTO_TRIG_INDEX + (trig << 2) + CTRL_INDEX,
+			adout, adout);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to program adout\n");
+			return ret;
+		}
+	}
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC,
+			AB5500_GPADC_AUTO_TRIG_INDEX + (trig << 2) + CTRL_INDEX,
+			TRIGX_FREQ_MASK, gpadc->adc_trig[trig].freq);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: fail to program freq\n");
+		return ret;
+	}
+	return ret;
+
+}
+
+#define TRIG_V(trigval, min, max) \
+	((((trigval) - (min)) * AUTO_ADC_RESOLUTION) / ((max) - (min)))
+
+static int ab5500_gpadc_vbat_auto_conf(struct ab5500_gpadc *gpadc,
+					struct adc_auto_input *in)
+{
+	int trig_min, ret;
+	u8 trig_reg, trig_arm;
+
+	/* Scale mux voltage */
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC,
+			AB5500_GPADC_MANUAL_MODE_CTRL,
+			MUX_SCALE_VBAT_MASK, MUX_SCALE_45);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: failed to set vbat scale\n");
+		return ret;
+	}
+
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC,
+			AB5500_GPADC_AUTO_CTRL1,
+			AUTO_VBAT_MASK, AUTO_VBAT_ON);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: failed to set vbat on\n");
+		return ret;
+	}
+
+	trig_min = TRIG_V(in->min, adc_tab[in->mux].min, adc_tab[in->mux].max);
+
+	if (in->mux == MAIN_BAT_V_TRIG_MIN) {
+		trig_reg = AB5500_GPADC_AUTO_TRIG_VBAT_MIN_NOTX;
+		trig_arm = TRIG_VBAT_NOTX_ARM_MASK;
+	} else {
+		trig_reg = AB5500_GPADC_AUTO_TRIG_VBAT_MIN_TXON;
+		trig_arm = TRIG_VBAT_TXON_ARM_MASK;
+	}
+	ret = abx500_set_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			trig_reg, trig_min);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: fail to program vbat min\n");
+		return ret;
+	}
+	/*
+	 * arm the trigger
+	 */
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+		AB5500_BANK_ADC, AB5500_GPADC_AUTO_CTRL1, trig_arm, trig_arm);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: failed to trig vbat\n");
+		return ret;
+	}
+	return ret;
+}
+/**
+ * ab5500_gpadc_convert_auto() - gpadc conversion
+ * @auto_input:	input trigger for conversion
+ *
+ * This function converts the selected channel from
+ * analog to digital data in auto mode
+ */
+
+int ab5500_gpadc_convert_auto(struct ab5500_gpadc *gpadc,
+				struct adc_auto_input *in)
+{
+	int ret, trig;
+	unsigned long flags;
+
+	if (!gpadc)
+		return -ENODEV;
+	mutex_lock(&gpadc->ab5500_gpadc_lock);
+
+	if (in->mux == MAIN_BAT_V_TXON_TRIG_MIN) {
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		if (gpadc->adc_trig[ADC_INPUT_VBAT_TXON].flag == true) {
+			spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+			ret = -EBUSY;
+			dev_err(gpadc->dev, "gpadc: Auto vbat txon busy");
+			goto out;
+		}
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+
+		ret = ab5500_gpadc_vbat_auto_conf(gpadc, in);
+		if (ret < 0)
+			goto out;
+
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXON].auto_mux = in->mux;
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXON].auto_callb =
+				in->auto_adc_callback;
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXON].flag = true;
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+	} else if (in->mux == MAIN_BAT_V_TRIG_MIN) {
+
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		if (gpadc->adc_trig[ADC_INPUT_VBAT_TXOFF].flag == true) {
+			spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+			ret = -EBUSY;
+			dev_err(gpadc->dev, "gpadc: Auto vbat busy");
+			goto out;
+		}
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+
+		ret = ab5500_gpadc_vbat_auto_conf(gpadc, in);
+		if (ret < 0)
+			goto out;
+
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXOFF].auto_mux = in->mux;
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXOFF].auto_callb =
+				in->auto_adc_callback;
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXOFF].flag = true;
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+	} else {
+		/*
+		 * check if free trigger is available
+		 */
+		trig = ADC_INPUT_TRIG0;
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		while (gpadc->adc_trig[trig].flag == true &&
+					trig <= ADC_INPUT_TRIG7)
+			trig++;
+
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+		if (trig > ADC_INPUT_TRIG7) {
+			ret = -EBUSY;
+			dev_err(gpadc->dev, "gpadc: no free channel\n");
+			goto out;
+		}
+		switch (in->mux) {
+		case BTEMP_BALL:
+		case MAIN_BAT_V:
+			/*
+			 * The value of mux scale volatage depends
+			 * on the type of battery
+			 * for LI-ion use MUX_SCALE_35 => 2.3-3.5V
+			 * for LiFePo4 use MUX_SCALE_45 => 2.3-4.5V
+			 * Check type of battery from platform data TODO ???
+			 */
+			ret = abx500_mask_and_set_register_interruptible(
+				gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+				MUX_SCALE_VBAT_MASK, MUX_SCALE_45);
+			if (ret < 0) {
+				dev_err(gpadc->dev,
+					"gpadc: failed to read status\n");
+				goto out;
+			}
+		case ACC_DETECT2:
+		case ACC_DETECT3:
+		case VBUS_V:
+		case USB_CHARGER_C:
+		case BK_BAT_V:
+		case PCB_TEMP:
+		case USB_ID:
+		case BAT_CTRL:
+			gpadc->adc_trig[trig].trig_min =
+			(u8)TRIG_V(in->min, adc_tab[in->mux].min,
+					adc_tab[in->mux].max);
+			gpadc->adc_trig[trig].trig_max =
+			(u8)TRIG_V(in->max, adc_tab[in->mux].min,
+					adc_tab[in->mux].max);
+			gpadc->adc_trig[trig].adout =
+				adc_tab[in->mux].adout;
+			break;
+		case DIE_TEMP:
+			/*
+			 * From the AB5500 product specification
+			 * T(deg_cel) = 27 -(ADCode - 709)/2.4123)
+			 * adc min and max values are based on the above formula.
+			 */
+			gpadc->adc_trig[trig].trig_min =
+				709 - (22413 * (in->min - 27))/10000;
+			gpadc->adc_trig[trig].trig_max =
+				709 - (22413 * (in->max - 27))/10000;
+			gpadc->adc_trig[trig].adout =
+				adc_tab[in->mux].adout;
+			break;
+		default:
+			dev_err(gpadc->dev, "Unknow GPADC request\n");
+			break;
+		}
+		gpadc->adc_trig[trig].freq = in->freq;
+		gpadc->adc_trig[trig].auto_mux =
+			adc_tab[in->mux].mux;
+		gpadc->adc_trig[trig].auto_callb = in->auto_adc_callback;
+
+		ret = ab5500_gpadc_program_auto(gpadc, trig);
+		if (ret < 0) {
+			dev_err(gpadc->dev,
+					"gpadc: fail to program auto ch\n");
+			goto out;
+		}
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC,
+				AB5500_GPADC_AUTO_TRIG_INDEX + (trig * 4),
+				TRIGX_ARM_MASK, TRIGX_ARM);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to trigger\n");
+			goto out;
+		}
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		gpadc->adc_trig[trig].flag = true;
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+	}
+out:
+	mutex_unlock(&gpadc->ab5500_gpadc_lock);
+	return ret;
+
+}
+EXPORT_SYMBOL(ab5500_gpadc_convert_auto);
+
+static void ab5500_gpadc_trigx_work(struct ab5500_gpadc *gp, int trig)
+{
+	unsigned long flags;
+	if (gp->adc_trig[trig].auto_callb != NULL) {
+		gp->adc_trig[trig].auto_callb(gp->adc_trig[trig].auto_mux);
+		spin_lock_irqsave(&gp->gpadc_auto_lock, flags);
+		gp->adc_trig[trig].flag = false;
+		spin_unlock_irqrestore(&gp->gpadc_auto_lock, flags);
+	} else {
+		dev_err(gp->dev, "Unknown trig for %d\n", trig);
+	}
+}
+/**
+ * ab5500_gpadc_trig0_work() - work item for trig0 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 0 auto conversion.
+ */
+static void ab5500_gpadc_trig0_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig0_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG0);
+}
+
+/**
+ * ab5500_gpadc_trig1_work() - work item for trig1 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig1 auto conversion.
+ */
+static void ab5500_gpadc_trig1_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig1_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG1);
+}
+
+/**
+ * ab5500_gpadc_trig2_work() - work item for trig2 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 2 auto conversion.
+ */
+static void ab5500_gpadc_trig2_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig2_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG2);
+}
+
+/**
+ * ab5500_gpadc_trig3_work() - work item for trig3 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 3 auto conversion.
+ */
+static void ab5500_gpadc_trig3_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig3_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG3);
+}
+
+/**
+ * ab5500_gpadc_trig4_work() - work item for trig4 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 4 auto conversion.
+ */
+static void ab5500_gpadc_trig4_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig4_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG4);
+}
+
+/**
+ * ab5500_gpadc_trig5_work() - work item for trig5 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 5 auto conversion.
+ */
+static void ab5500_gpadc_trig5_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig5_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG5);
+}
+
+/**
+ * ab5500_gpadc_trig6_work() - work item for trig6 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 6 auto conversion.
+ */
+static void ab5500_gpadc_trig6_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig6_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG6);
+}
+
+/**
+ * ab5500_gpadc_trig7_work() - work item for trig7 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 7 auto conversion.
+ */
+static void ab5500_gpadc_trig7_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig7_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG7);
+}
+
+/**
+ * ab5500_gpadc_vbat_txon_work() - work item for vbat_txon trigger auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for vbat_txon trigger auto adc.
+ */
+static void ab5500_gpadc_vbat_txon_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig_vbat_txon_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_VBAT_TXON);
+}
+
+/**
+ * ab5500_gpadc_vbat_txoff_work() - work item for vbat_txoff trigger auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for vbat_txoff trigger auto adc.
+ */
+static void ab5500_gpadc_vbat_txoff_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig_vbat_txoff_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_VBAT_TXOFF);
+}
+
+/**
+ * ab5500_adc_trigx_handler() - isr for auto gpadc conversion trigger
+ * @irq:	irq number
+ * @data:	pointer to the data passed during request irq
+ *
+ * This is a interrupt service routine for auto gpadc conversion.
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_adc_trigx_handler(int irq, void *_gpadc)
+{
+	struct ab5500_platform_data *plat;
+	struct ab5500_gpadc *gpadc = _gpadc;
+	int dev_irq;
+
+	plat = dev_get_platdata(gpadc->dev->parent);
+	dev_irq = irq - plat->irq.base;
+
+	switch (dev_irq) {
+	case AB5500_INT_ADC_TRIG0:
+		dev_dbg(gpadc->dev, "Trigger 0 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig0_work);
+		break;
+	case AB5500_INT_ADC_TRIG1:
+		dev_dbg(gpadc->dev, "Trigger 1 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig1_work);
+		break;
+	case AB5500_INT_ADC_TRIG2:
+		dev_dbg(gpadc->dev, "Trigger 2 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig2_work);
+		break;
+	case AB5500_INT_ADC_TRIG3:
+		dev_dbg(gpadc->dev, "Trigger 3 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig3_work);
+		break;
+	case AB5500_INT_ADC_TRIG4:
+		dev_dbg(gpadc->dev, "Trigger 4 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig4_work);
+		break;
+	case AB5500_INT_ADC_TRIG5:
+		dev_dbg(gpadc->dev, "Trigger 5 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig5_work);
+		break;
+	case AB5500_INT_ADC_TRIG6:
+		dev_dbg(gpadc->dev, "Trigger 6 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig6_work);
+		break;
+	case AB5500_INT_ADC_TRIG7:
+		dev_dbg(gpadc->dev, "Trigger 7 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig7_work);
+		break;
+	default:
+		dev_dbg(gpadc->dev, "unknown trigx handler input\n");
+		break;
+	}
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_adc_vbat_txon_handler() - isr for auto vbat_txon conversion trigger
+ * @irq:	irq number
+ * @data:	pointer to the data passed during request irq
+ *
+ * This is a interrupt service routine for auto vbat_txon conversion
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_adc_vbat_txon_handler(int irq, void *_gpadc)
+{
+	struct ab5500_gpadc *gpadc = _gpadc;
+
+	queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig_vbat_txon_work);
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_adc_vbat_txoff_handler() - isr for auto vbat_txoff conversion trigger
+ * @irq:	irq number
+ * @data:	pointer to the data passed during request irq
+ *
+ * This is a interrupt service routine for auto vbat_txoff conversion
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_adc_vbat_txoff_handler(int irq, void *_gpadc)
+{
+	struct ab5500_gpadc *gpadc = _gpadc;
+
+	queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig_vbat_txoff_work);
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_gpadc_configuration() - function for gpadc conversion
+ * @irq:	irq number
+ * @data:	pointer to the data passed during request irq
+ *
+ * This function configures the gpadc
+ */
+static int ab5500_gpadc_configuration(struct ab5500_gpadc *gpadc)
+{
+	int ret;
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC, AB5500_GPADC_AUTO_CTRL2,
+			ADC_CAL_OFF_MASK | ADC_ON_MODE_MASK,
+			ADC_CAL_ON | ADC_FULLPWR);
+	return ret;
+}
+
+/* ab5500 btemp driver interrupts and their respective isr */
+static struct ab5500_adc_interrupts ab5500_adc_irq[] = {
+	{"TRIGGER-0", ab5500_adc_trigx_handler},
+	{"TRIGGER-1", ab5500_adc_trigx_handler},
+	{"TRIGGER-2", ab5500_adc_trigx_handler},
+	{"TRIGGER-3", ab5500_adc_trigx_handler},
+	{"TRIGGER-4", ab5500_adc_trigx_handler},
+	{"TRIGGER-5", ab5500_adc_trigx_handler},
+	{"TRIGGER-6", ab5500_adc_trigx_handler},
+	{"TRIGGER-7", ab5500_adc_trigx_handler},
+	{"TRIGGER-VBAT-TXON", ab5500_adc_vbat_txon_handler},
+	{"TRIGGER-VBAT", ab5500_adc_vbat_txoff_handler},
+};
+
+static int __devinit ab5500_gpadc_probe(struct platform_device *pdev)
+{
+	int ret, irq, i, j;
+	struct ab5500_gpadc *gpadc;
+
+	gpadc = kzalloc(sizeof(struct ab5500_gpadc), GFP_KERNEL);
+	if (!gpadc) {
+		dev_err(&pdev->dev, "Error: No memory\n");
+		return -ENOMEM;
+	}
+	gpadc->dev = &pdev->dev;
+	mutex_init(&gpadc->ab5500_gpadc_lock);
+	spin_lock_init(&gpadc->gpadc_auto_lock);
+
+	/* Register interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_adc_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_adc_irq[i].name);
+		ret = request_threaded_irq(irq, NULL, ab5500_adc_irq[i].isr,
+			IRQF_NO_SUSPEND,
+			ab5500_adc_irq[i].name, gpadc);
+
+		if (ret) {
+			dev_err(gpadc->dev, "failed to request %s IRQ %d: %d\n"
+				, ab5500_adc_irq[i].name, irq, ret);
+			goto fail_irq;
+		}
+		dev_dbg(gpadc->dev, "Requested %s IRQ %d: %d\n",
+			ab5500_adc_irq[i].name, irq, ret);
+	}
+
+	/* Get Chip ID of the ABB ASIC  */
+	ret = abx500_get_chip_id(gpadc->dev);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "failed to get chip ID\n");
+		goto fail_irq;
+	}
+	gpadc->chip_id = (u8) ret;
+
+	/* Create a work queue for gpadc auto */
+	gpadc->gpadc_wq =
+		create_singlethread_workqueue("ab5500_gpadc_wq");
+	if (gpadc->gpadc_wq == NULL) {
+		dev_err(gpadc->dev, "failed to create work queue\n");
+		goto fail_irq;
+	}
+
+	INIT_WORK(&gpadc->gpadc_trig0_work, ab5500_gpadc_trig0_work);
+	INIT_WORK(&gpadc->gpadc_trig1_work, ab5500_gpadc_trig1_work);
+	INIT_WORK(&gpadc->gpadc_trig2_work, ab5500_gpadc_trig2_work);
+	INIT_WORK(&gpadc->gpadc_trig3_work, ab5500_gpadc_trig3_work);
+	INIT_WORK(&gpadc->gpadc_trig4_work, ab5500_gpadc_trig4_work);
+	INIT_WORK(&gpadc->gpadc_trig5_work, ab5500_gpadc_trig5_work);
+	INIT_WORK(&gpadc->gpadc_trig6_work, ab5500_gpadc_trig6_work);
+	INIT_WORK(&gpadc->gpadc_trig7_work, ab5500_gpadc_trig7_work);
+	INIT_WORK(&gpadc->gpadc_trig_vbat_txon_work,
+				ab5500_gpadc_vbat_txon_work);
+	INIT_WORK(&gpadc->gpadc_trig_vbat_txoff_work,
+				ab5500_gpadc_vbat_txoff_work);
+
+	for (j = 0; j < N_AUTO_TRIGGER; j++)
+		gpadc->adc_trig[j].flag = false;
+
+	ret = ab5500_gpadc_configuration(gpadc);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: configuration failed\n");
+		goto free_wq;
+	}
+	list_add_tail(&gpadc->node, &ab5500_gpadc_list);
+
+	return 0;
+free_wq:
+	destroy_workqueue(gpadc->gpadc_wq);
+fail_irq:
+	for (i = i - 1; i >= 0; i--) {
+		irq = platform_get_irq_byname(pdev, ab5500_adc_irq[i].name);
+		free_irq(irq, gpadc);
+	}
+	kfree(gpadc);
+	gpadc = NULL;
+	return ret;
+}
+
+static int __devexit ab5500_gpadc_remove(struct platform_device *pdev)
+{
+	int i, irq;
+	struct ab5500_gpadc *gpadc = platform_get_drvdata(pdev);
+
+	/* remove this gpadc entry from the list */
+	list_del(&gpadc->node);
+	/* Disable interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_adc_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_adc_irq[i].name);
+		free_irq(irq, gpadc);
+	}
+	/* Flush work */
+	flush_workqueue(gpadc->gpadc_wq);
+
+	/* Delete the work queue */
+	destroy_workqueue(gpadc->gpadc_wq);
+
+	kfree(gpadc);
+	gpadc = NULL;
+	return 0;
+}
+
+static struct platform_driver ab5500_gpadc_driver = {
+	.probe = ab5500_gpadc_probe,
+	.remove = __devexit_p(ab5500_gpadc_remove),
+	.driver = {
+		.name = "ab5500-adc",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab5500_gpadc_init(void)
+{
+	return platform_driver_register(&ab5500_gpadc_driver);
+}
+
+static void __exit ab5500_gpadc_exit(void)
+{
+	platform_driver_unregister(&ab5500_gpadc_driver);
+}
+
+subsys_initcall_sync(ab5500_gpadc_init);
+module_exit(ab5500_gpadc_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Vijaya Kumar K");
+MODULE_ALIAS("platform:ab5500_adc");
+MODULE_DESCRIPTION("AB5500 GPADC driver");
diff --git a/drivers/mfd/ab5500-power.c b/drivers/mfd/ab5500-power.c
new file mode 100644
index 00000000000..d23960bc976
--- /dev/null
+++ b/drivers/mfd/ab5500-power.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/signal.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+
+#include <linux/mfd/abx500.h>
+
+static struct device *dev;
+
+/* STARTUP */
+#define AB5500_SYSPOR_CONTROL	0x30
+
+/* VINT IO I2C CLOCK */
+#define AB5500_RTC_VINT		0x01
+
+int ab5500_clock_rtc_enable(int num, bool enable)
+{
+	/* RTC_CLK{0,1,2} are bits {4,3,2}, active low */
+	u8 mask = BIT(4 - num);
+	u8 value = enable ? 0 : mask;
+
+	/* Don't allow RTC_CLK0 to be controlled. */
+	if (num < 1 || num > 2)
+		return -EINVAL;
+
+	if (!dev)
+		return -EAGAIN;
+
+	return abx500_mask_and_set(dev, AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP,
+				   AB5500_RTC_VINT, mask, value);
+}
+
+static void ab5500_power_off(void)
+{
+	sigset_t old;
+	sigset_t all;
+
+	sigfillset(&all);
+
+	if (!sigprocmask(SIG_BLOCK, &all, &old)) {
+		/* Clear dbb_on */
+		int ret = abx500_set(dev, AB5500_BANK_STARTUP,
+				     AB5500_SYSPOR_CONTROL, 0);
+		WARN_ON(ret);
+	}
+}
+
+static int __devinit ab5500_power_probe(struct platform_device *pdev)
+{
+	struct ab5500_platform_data *plat = dev_get_platdata(pdev->dev.parent);
+
+	dev = &pdev->dev;
+
+	if (plat->pm_power_off)
+		pm_power_off = ab5500_power_off;
+
+	return 0;
+}
+
+static int __devexit ab5500_power_remove(struct platform_device *pdev)
+{
+	struct ab5500_platform_data *plat = dev_get_platdata(pdev->dev.parent);
+
+	if (plat->pm_power_off)
+		pm_power_off = NULL;
+	dev = NULL;
+
+	return 0;
+}
+
+static struct platform_driver ab5500_power_driver = {
+	.driver = {
+		.name = "ab5500-power",
+		.owner = THIS_MODULE,
+	},
+	.probe = ab5500_power_probe,
+	.remove = __devexit_p(ab5500_power_remove),
+};
+
+static int __init ab8500_sysctrl_init(void)
+{
+	return platform_driver_register(&ab5500_power_driver);
+}
+
+subsys_initcall(ab8500_sysctrl_init);
+
+MODULE_DESCRIPTION("AB5500 power driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index fc0c1af1566..03345a87b39 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -264,7 +264,6 @@ static void ab8500_irq_sync_unlock(struct irq_data *data)
 		reg = AB8500_IT_MASK1_REG + ab8500_irq_regoffset[i];
 		set_register_interruptible(ab8500, AB8500_INTERRUPT, reg, new);
 	}
-
 	mutex_unlock(&ab8500->irq_lock);
 }
 
@@ -293,6 +292,7 @@ static struct irq_chip ab8500_irq_chip = {
 	.irq_bus_lock		= ab8500_irq_lock,
 	.irq_bus_sync_unlock	= ab8500_irq_sync_unlock,
 	.irq_mask		= ab8500_irq_mask,
+	.irq_disable		= ab8500_irq_mask,
 	.irq_unmask		= ab8500_irq_unmask,
 };
 
@@ -325,7 +325,6 @@ static irqreturn_t ab8500_irq(int irq, void *dev)
 			value &= ~(1 << bit);
 		} while (value);
 	}
-
 	return IRQ_HANDLED;
 }
 
@@ -363,7 +362,7 @@ static void ab8500_irq_remove(struct ab8500 *ab8500)
 	}
 }
 
-static struct resource ab8500_gpio_resources[] = {
+static struct resource __devinitdata ab8500_gpio_resources[] = {
 	{
 		.name	= "GPIO_INT6",
 		.start	= AB8500_INT_GPIO6R,
@@ -372,7 +371,7 @@ static struct resource ab8500_gpio_resources[] = {
 	}
 };
 
-static struct resource ab8500_gpadc_resources[] = {
+static struct resource __devinitdata ab8500_gpadc_resources[] = {
 	{
 		.name	= "HW_CONV_END",
 		.start	= AB8500_INT_GP_HW_ADC_CONV_END,
@@ -387,7 +386,7 @@ static struct resource ab8500_gpadc_resources[] = {
 	},
 };
 
-static struct resource ab8500_rtc_resources[] = {
+static struct resource __devinitdata ab8500_rtc_resources[] = {
 	{
 		.name	= "60S",
 		.start	= AB8500_INT_RTC_60S,
@@ -402,7 +401,7 @@ static struct resource ab8500_rtc_resources[] = {
 	},
 };
 
-static struct resource ab8500_poweronkey_db_resources[] = {
+static struct resource __devinitdata ab8500_poweronkey_db_resources[] = {
 	{
 		.name	= "ONKEY_DBF",
 		.start	= AB8500_INT_PON_KEY1DB_F,
@@ -417,20 +416,47 @@ static struct resource ab8500_poweronkey_db_resources[] = {
 	},
 };
 
-static struct resource ab8500_bm_resources[] = {
+static struct resource __devinitdata ab8500_av_acc_detect_resources[] = {
 	{
-		.name = "MAIN_EXT_CH_NOT_OK",
-		.start = AB8500_INT_MAIN_EXT_CH_NOT_OK,
-		.end = AB8500_INT_MAIN_EXT_CH_NOT_OK,
-		.flags = IORESOURCE_IRQ,
+	       .name = "ACC_DETECT_1DB_F",
+	       .start = AB8500_INT_ACC_DETECT_1DB_F,
+	       .end = AB8500_INT_ACC_DETECT_1DB_F,
+	       .flags = IORESOURCE_IRQ,
 	},
 	{
-		.name = "BATT_OVV",
-		.start = AB8500_INT_BATT_OVV,
-		.end = AB8500_INT_BATT_OVV,
-		.flags = IORESOURCE_IRQ,
+	       .name = "ACC_DETECT_1DB_R",
+	       .start = AB8500_INT_ACC_DETECT_1DB_R,
+	       .end = AB8500_INT_ACC_DETECT_1DB_R,
+	       .flags = IORESOURCE_IRQ,
 	},
 	{
+	       .name = "ACC_DETECT_21DB_F",
+	       .start = AB8500_INT_ACC_DETECT_21DB_F,
+	       .end = AB8500_INT_ACC_DETECT_21DB_F,
+	       .flags = IORESOURCE_IRQ,
+	},
+	{
+	       .name = "ACC_DETECT_21DB_R",
+	       .start = AB8500_INT_ACC_DETECT_21DB_R,
+	       .end = AB8500_INT_ACC_DETECT_21DB_R,
+	       .flags = IORESOURCE_IRQ,
+	},
+	{
+	       .name = "ACC_DETECT_22DB_F",
+	       .start = AB8500_INT_ACC_DETECT_22DB_F,
+	       .end = AB8500_INT_ACC_DETECT_22DB_F,
+	       .flags = IORESOURCE_IRQ,
+	},
+	{
+	       .name = "ACC_DETECT_22DB_R",
+	       .start = AB8500_INT_ACC_DETECT_22DB_R,
+	       .end = AB8500_INT_ACC_DETECT_22DB_R,
+	       .flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct resource __devinitdata ab8500_charger_resources[] = {
+	{
 		.name = "MAIN_CH_UNPLUG_DET",
 		.start = AB8500_INT_MAIN_CH_UNPLUG_DET,
 		.end = AB8500_INT_MAIN_CH_UNPLUG_DET,
@@ -443,27 +469,27 @@ static struct resource ab8500_bm_resources[] = {
 		.flags = IORESOURCE_IRQ,
 	},
 	{
-		.name = "VBUS_DET_F",
-		.start = AB8500_INT_VBUS_DET_F,
-		.end = AB8500_INT_VBUS_DET_F,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
 		.name = "VBUS_DET_R",
 		.start = AB8500_INT_VBUS_DET_R,
 		.end = AB8500_INT_VBUS_DET_R,
 		.flags = IORESOURCE_IRQ,
 	},
 	{
-		.name = "BAT_CTRL_INDB",
-		.start = AB8500_INT_BAT_CTRL_INDB,
-		.end = AB8500_INT_BAT_CTRL_INDB,
+		.name = "VBUS_DET_F",
+		.start = AB8500_INT_VBUS_DET_F,
+		.end = AB8500_INT_VBUS_DET_F,
 		.flags = IORESOURCE_IRQ,
 	},
 	{
-		.name = "CH_WD_EXP",
-		.start = AB8500_INT_CH_WD_EXP,
-		.end = AB8500_INT_CH_WD_EXP,
+		.name = "USB_LINK_STATUS",
+		.start = AB8500_INT_USB_LINK_STATUS,
+		.end = AB8500_INT_USB_LINK_STATUS,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "USB_CHARGE_DET_DONE",
+		.start = AB8500_INT_USB_CHG_DET_DONE,
+		.end = AB8500_INT_USB_CHG_DET_DONE,
 		.flags = IORESOURCE_IRQ,
 	},
 	{
@@ -473,21 +499,60 @@ static struct resource ab8500_bm_resources[] = {
 		.flags = IORESOURCE_IRQ,
 	},
 	{
-		.name = "NCONV_ACCU",
-		.start = AB8500_INT_CCN_CONV_ACC,
-		.end = AB8500_INT_CCN_CONV_ACC,
+		.name = "USB_CH_TH_PROT_R",
+		.start = AB8500_INT_USB_CH_TH_PROT_R,
+		.end = AB8500_INT_USB_CH_TH_PROT_R,
 		.flags = IORESOURCE_IRQ,
 	},
 	{
-		.name = "LOW_BAT_F",
-		.start = AB8500_INT_LOW_BAT_F,
-		.end = AB8500_INT_LOW_BAT_F,
+		.name = "USB_CH_TH_PROT_F",
+		.start = AB8500_INT_USB_CH_TH_PROT_F,
+		.end = AB8500_INT_USB_CH_TH_PROT_F,
 		.flags = IORESOURCE_IRQ,
 	},
 	{
-		.name = "LOW_BAT_R",
-		.start = AB8500_INT_LOW_BAT_R,
-		.end = AB8500_INT_LOW_BAT_R,
+		.name = "MAIN_EXT_CH_NOT_OK",
+		.start = AB8500_INT_MAIN_EXT_CH_NOT_OK,
+		.end = AB8500_INT_MAIN_EXT_CH_NOT_OK,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "MAIN_CH_TH_PROT_R",
+		.start = AB8500_INT_MAIN_CH_TH_PROT_R,
+		.end = AB8500_INT_MAIN_CH_TH_PROT_R,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "MAIN_CH_TH_PROT_F",
+		.start = AB8500_INT_MAIN_CH_TH_PROT_F,
+		.end = AB8500_INT_MAIN_CH_TH_PROT_F,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "USB_CHARGER_NOT_OKR",
+		.start = AB8500_INT_USB_CHARGER_NOT_OK,
+		.end = AB8500_INT_USB_CHARGER_NOT_OK,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "USB_CHARGER_NOT_OKF",
+		.start = AB8500_INT_USB_CHARGER_NOT_OKF,
+		.end = AB8500_INT_USB_CHARGER_NOT_OKF,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "CH_WD_EXP",
+		.start = AB8500_INT_CH_WD_EXP,
+		.end = AB8500_INT_CH_WD_EXP,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct resource __devinitdata ab8500_btemp_resources[] = {
+	{
+		.name = "BAT_CTRL_INDB",
+		.start = AB8500_INT_BAT_CTRL_INDB,
+		.end = AB8500_INT_BAT_CTRL_INDB,
 		.flags = IORESOURCE_IRQ,
 	},
 	{
@@ -503,38 +568,55 @@ static struct resource ab8500_bm_resources[] = {
 		.flags = IORESOURCE_IRQ,
 	},
 	{
-		.name = "USB_CHARGER_NOT_OKR",
-		.start = AB8500_INT_USB_CHARGER_NOT_OK,
-		.end = AB8500_INT_USB_CHARGER_NOT_OK,
+		.name = "BTEMP_LOW_MEDIUM",
+		.start = AB8500_INT_BTEMP_LOW_MEDIUM,
+		.end = AB8500_INT_BTEMP_LOW_MEDIUM,
 		.flags = IORESOURCE_IRQ,
 	},
 	{
-		.name = "USB_CHARGE_DET_DONE",
-		.start = AB8500_INT_USB_CHG_DET_DONE,
-		.end = AB8500_INT_USB_CHG_DET_DONE,
+		.name = "BTEMP_MEDIUM_HIGH",
+		.start = AB8500_INT_BTEMP_MEDIUM_HIGH,
+		.end = AB8500_INT_BTEMP_MEDIUM_HIGH,
 		.flags = IORESOURCE_IRQ,
 	},
+};
+
+static struct resource __devinitdata ab8500_fg_resources[] = {
 	{
-		.name = "USB_CH_TH_PROT_R",
-		.start = AB8500_INT_USB_CH_TH_PROT_R,
-		.end = AB8500_INT_USB_CH_TH_PROT_R,
+		.name = "NCONV_ACCU",
+		.start = AB8500_INT_CCN_CONV_ACC,
+		.end = AB8500_INT_CCN_CONV_ACC,
 		.flags = IORESOURCE_IRQ,
 	},
 	{
-		.name = "MAIN_CH_TH_PROT_R",
-		.start = AB8500_INT_MAIN_CH_TH_PROT_R,
-		.end = AB8500_INT_MAIN_CH_TH_PROT_R,
+		.name = "BATT_OVV",
+		.start = AB8500_INT_BATT_OVV,
+		.end = AB8500_INT_BATT_OVV,
 		.flags = IORESOURCE_IRQ,
 	},
 	{
-		.name = "USB_CHARGER_NOT_OKF",
-		.start = AB8500_INT_USB_CHARGER_NOT_OKF,
-		.end = AB8500_INT_USB_CHARGER_NOT_OKF,
+		.name = "LOW_BAT_F",
+		.start = AB8500_INT_LOW_BAT_F,
+		.end = AB8500_INT_LOW_BAT_F,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "LOW_BAT_R",
+		.start = AB8500_INT_LOW_BAT_R,
+		.end = AB8500_INT_LOW_BAT_R,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "CC_INT_CALIB",
+		.start = AB8500_INT_CC_INT_CALIB,
+		.end = AB8500_INT_CC_INT_CALIB,
 		.flags = IORESOURCE_IRQ,
 	},
 };
 
-static struct resource ab8500_debug_resources[] = {
+static struct resource __devinitdata ab8500_chargalg_resources[] = {};
+
+static struct resource __devinitdata ab8500_debug_resources[] = {
 	{
 		.name	= "IRQ_FIRST",
 		.start	= AB8500_INT_MAIN_EXT_CH_NOT_OK,
@@ -549,7 +631,7 @@ static struct resource ab8500_debug_resources[] = {
 	},
 };
 
-static struct resource ab8500_usb_resources[] = {
+static struct resource __devinitdata ab8500_usb_resources[] = {
 	{
 		.name = "ID_WAKEUP_R",
 		.start = AB8500_INT_ID_WAKEUP_R,
@@ -580,9 +662,21 @@ static struct resource ab8500_usb_resources[] = {
 		.end = AB8500_INT_USB_LINK_STATUS,
 		.flags = IORESOURCE_IRQ,
 	},
+	{
+		.name = "USB_ADP_PROBE_PLUG",
+		.start = AB8500_INT_ADP_PROBE_PLUG,
+		.end = AB8500_INT_ADP_PROBE_PLUG,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "USB_ADP_PROBE_UNPLUG",
+		.start = AB8500_INT_ADP_PROBE_UNPLUG,
+		.end = AB8500_INT_ADP_PROBE_UNPLUG,
+		.flags = IORESOURCE_IRQ,
+	},
 };
 
-static struct resource ab8500_temp_resources[] = {
+static struct resource __devinitdata ab8500_temp_resources[] = {
 	{
 		.name  = "AB8500_TEMP_WARM",
 		.start = AB8500_INT_TEMP_WARM,
@@ -591,7 +685,7 @@ static struct resource ab8500_temp_resources[] = {
 	},
 };
 
-static struct mfd_cell ab8500_devs[] = {
+static struct mfd_cell __devinitdata ab8500_devs[] = {
 #ifdef CONFIG_DEBUG_FS
 	{
 		.name = "ab8500-debug",
@@ -606,6 +700,9 @@ static struct mfd_cell ab8500_devs[] = {
 		.name = "ab8500-regulator",
 	},
 	{
+		.name = "ab8500-regulator-debug",
+	},
+	{
 		.name = "ab8500-gpio",
 		.num_resources = ARRAY_SIZE(ab8500_gpio_resources),
 		.resources = ab8500_gpio_resources,
@@ -621,11 +718,33 @@ static struct mfd_cell ab8500_devs[] = {
 		.resources = ab8500_rtc_resources,
 	},
 	{
-		.name = "ab8500-bm",
-		.num_resources = ARRAY_SIZE(ab8500_bm_resources),
-		.resources = ab8500_bm_resources,
+		.name = "ab8500-charger",
+		.num_resources = ARRAY_SIZE(ab8500_charger_resources),
+		.resources = ab8500_charger_resources,
+	},
+	{
+		.name = "ab8500-btemp",
+		.num_resources = ARRAY_SIZE(ab8500_btemp_resources),
+		.resources = ab8500_btemp_resources,
+	},
+	{
+		.name = "ab8500-fg",
+		.num_resources = ARRAY_SIZE(ab8500_fg_resources),
+		.resources = ab8500_fg_resources,
+	},
+	{
+		.name = "ab8500-chargalg",
+		.num_resources = ARRAY_SIZE(ab8500_chargalg_resources),
+		.resources = ab8500_chargalg_resources,
+	},
+	{
+		.name = "ab8500-acc-det",
+		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
+		.resources = ab8500_av_acc_detect_resources,
+	},
+	{
+		.name = "ab8500-codec",
 	},
-	{ .name = "ab8500-codec", },
 	{
 		.name = "ab8500-usb",
 		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index 64748e42ac0..e177f3e49d9 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -4,19 +4,101 @@
  * Author: Mattias Wallin <mattias.wallin@stericsson.com> for ST-Ericsson.
  * License Terms: GNU General Public License v2
  */
+/*
+ * AB8500 register access
+ * ======================
+ *
+ * read:
+ * # echo BANK  >  <debugfs>/ab8500/register-bank
+ * # echo ADDR  >  <debugfs>/ab8500/register-address
+ * # cat <debugfs>/ab8500/register-value
+ *
+ * write:
+ * # echo BANK  >  <debugfs>/ab8500/register-bank
+ * # echo ADDR  >  <debugfs>/ab8500/register-address
+ * # echo VALUE >  <debugfs>/ab8500/register-value
+ *
+ * read all registers from a bank:
+ * # echo BANK  >  <debugfs>/ab8500/register-bank
+ * # cat <debugfs>/ab8500/all-bank-register
+ *
+ * BANK   target AB8500 register bank
+ * ADDR   target AB8500 register address
+ * VALUE  decimal or 0x-prefixed hexadecimal
+ *
+ *
+ * User Space notification on AB8500 IRQ
+ * =====================================
+ *
+ * Allows user space entity to be notified when target AB8500 IRQ occurs.
+ * When subscribed, a sysfs entry is created in ab8500.i2c platform device.
+ * One can pool this file to get target IRQ occurence information.
+ *
+ * subscribe to an AB8500 IRQ:
+ * # echo IRQ  >  <debugfs>/ab8500/irq-subscribe
+ *
+ * unsubscribe from an AB8500 IRQ:
+ * # echo IRQ  >  <debugfs>/ab8500/irq-unsubscribe
+ *
+ *
+ * AB8500 register formated read/write access
+ * ==========================================
+ *
+ * Read:  read data, data>>SHIFT, data&=MASK, output data
+ *        [0xABCDEF98] shift=12 mask=0xFFF => 0x00000CDE
+ * Write: read data, data &= ~(MASK<<SHIFT), data |= (VALUE<<SHIFT), write data
+ *        [0xABCDEF98] shift=12 mask=0xFFF value=0x123 => [0xAB123F98]
+ *
+ * Usage:
+ * # echo "CMD [OPTIONS] BANK ADRESS [VALUE]" > $debugfs/ab8500/hwreg
+ *
+ * CMD      read      read access
+ *          write     write access
+ *
+ * BANK     target reg bank
+ * ADDRESS  target reg address
+ * VALUE    (write) value to be updated
+ *
+ * OPTIONS
+ *  -d|-dec            (read) output in decimal
+ *  -h|-hexa           (read) output in 0x-hexa (default)
+ *  -l|-w|-b           32bit (default), 16bit or 8bit reg access
+ *  -m|-mask MASK      0x-hexa mask (default 0xFFFFFFFF)
+ *  -s|-shift SHIFT    bit shift value (read:left, write:right)
+ *  -o|-offset OFFSET  address offset to add to ADDRESS value
+ *
+ * Warning: bit shift operation is applied to bit-mask.
+ * Warning: bit shift direction depends on read or right command.
+ */
 
 #include <linux/seq_file.h>
 #include <linux/uaccess.h>
 #include <linux/fs.h>
 #include <linux/debugfs.h>
 #include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/kobject.h>
+#include <linux/slab.h>
 
 #include <linux/mfd/abx500.h>
 #include <linux/mfd/ab8500.h>
+#include <linux/mfd/ab8500/gpadc.h>
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/string.h>
+#include <linux/ctype.h>
+#endif
 
 static u32 debug_bank;
 static u32 debug_address;
 
+static int irq_first;
+static int irq_last;
+static u32 irq_count[AB8500_NR_IRQS];
+
+static struct device_attribute *dev_attr[AB8500_NR_IRQS];
+static char *event_name[AB8500_NR_IRQS];
+
 /**
  * struct ab8500_reg_range
  * @first: the first address of the range
@@ -41,7 +123,27 @@ struct ab8500_i2c_ranges {
 	const struct ab8500_reg_range *range;
 };
 
+/* hwreg- "mask" and "shift" entries ressources */
+struct hwreg_cfg {
+	u32  bank;      /* target bank */
+	u32  addr;      /* target address */
+	uint fmt;       /* format */
+	uint mask;      /* read/write mask, applied before any bit shift */
+	int  shift;     /* bit shift (read:right shift, write:left shift */
+};
+/* fmt bit #0: 0=hexa, 1=dec */
+#define REG_FMT_DEC(c) ((c)->fmt & 0x1)
+#define REG_FMT_HEX(c) (!REG_FMT_DEC(c))
+
+static struct hwreg_cfg hwreg_cfg = {
+	.addr = 0,			/* default: invalid phys addr */
+	.fmt = 0,			/* default: 32bit access, hex output */
+	.mask = 0xFFFFFFFF,	/* default: no mask */
+	.shift = 0,			/* default: no bit shift */
+};
+
 #define AB8500_NAME_STRING "ab8500"
+#define AB8500_ADC_NAME_STRING "gpadc"
 #define AB8500_NUM_BANKS 22
 
 #define AB8500_REV_REG 0x80
@@ -49,7 +151,7 @@ struct ab8500_i2c_ranges {
 static struct ab8500_i2c_ranges debug_ranges[AB8500_NUM_BANKS] = {
 	[0x0] = {
 		.num_ranges = 0,
-		.range = 0,
+		.range = NULL,
 	},
 	[AB8500_SYS_CTRL1_BLOCK] = {
 		.num_ranges = 3,
@@ -353,6 +455,24 @@ static struct ab8500_i2c_ranges debug_ranges[AB8500_NUM_BANKS] = {
 	},
 };
 
+static irqreturn_t ab8500_debug_handler(int irq, void *data)
+{
+	char buf[16];
+	struct kobject *kobj = (struct kobject *)data;
+	unsigned int irq_abb = irq - irq_first;
+
+	if (irq_abb < AB8500_NR_IRQS)
+		irq_count[irq_abb]++;
+	/*
+	 * This makes it possible to use poll for events (POLLPRI | POLLERR)
+	 * from userspace on sysfs file named <irq-nr>
+	 */
+	sprintf(buf, "%d", irq);
+	sysfs_notify(kobj, NULL, buf);
+
+	return IRQ_HANDLED;
+}
+
 static int ab8500_registers_print(struct seq_file *s, void *p)
 {
 	struct device *dev = s->private;
@@ -533,10 +653,732 @@ static ssize_t ab8500_val_write(struct file *file,
 		printk(KERN_ERR "abx500_set_reg failed %d, %d", err, __LINE__);
 		return -EINVAL;
 	}
+	return buf_size;
+}
+
+/*
+ * - HWREG DB8500 formated routines
+ */
+static int ab8500_hwreg_print(struct seq_file *s, void *d)
+{
+	struct device *dev = s->private;
+	int ret;
+	u8 regvalue;
+
+	ret = abx500_get_register_interruptible(dev,
+		(u8)hwreg_cfg.bank, (u8)hwreg_cfg.addr, &regvalue);
+	if (ret < 0) {
+		dev_err(dev, "abx500_get_reg fail %d, %d\n",
+			ret, __LINE__);
+		return -EINVAL;
+	}
+
+	if (hwreg_cfg.shift >= 0)
+		regvalue >>= hwreg_cfg.shift;
+	else
+		regvalue <<= -hwreg_cfg.shift;
+	regvalue &= hwreg_cfg.mask;
+
+	if (REG_FMT_DEC(&hwreg_cfg))
+		seq_printf(s, "%d\n", regvalue);
+	else
+		seq_printf(s, "0x%02X\n", regvalue);
+	return 0;
+}
+
+static int ab8500_hwreg_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_hwreg_print, inode->i_private);
+}
+
+static int ab8500_gpadc_bat_ctrl_print(struct seq_file *s, void *p)
+{
+	int bat_ctrl_raw;
+	int bat_ctrl_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	bat_ctrl_raw = ab8500_gpadc_read_raw(gpadc, BAT_CTRL);
+	bat_ctrl_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+			BAT_CTRL, bat_ctrl_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			bat_ctrl_convert, bat_ctrl_raw);
+}
+
+static int ab8500_gpadc_bat_ctrl_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_bat_ctrl_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_bat_ctrl_fops = {
+	.open = ab8500_gpadc_bat_ctrl_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_btemp_ball_print(struct seq_file *s, void *p)
+{
+	int btemp_ball_raw;
+	int btemp_ball_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	btemp_ball_raw = ab8500_gpadc_read_raw(gpadc, BTEMP_BALL);
+	btemp_ball_convert = ab8500_gpadc_ad_to_voltage(gpadc, BTEMP_BALL,
+			btemp_ball_raw);
+
+	return seq_printf(s,
+			"%d,0x%X\n", btemp_ball_convert, btemp_ball_raw);
+}
+
+static int ab8500_gpadc_btemp_ball_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_btemp_ball_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_btemp_ball_fops = {
+	.open = ab8500_gpadc_btemp_ball_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_main_charger_v_print(struct seq_file *s, void *p)
+{
+	int main_charger_v_raw;
+	int main_charger_v_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	main_charger_v_raw = ab8500_gpadc_read_raw(gpadc, MAIN_CHARGER_V);
+	main_charger_v_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+			MAIN_CHARGER_V, main_charger_v_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			main_charger_v_convert, main_charger_v_raw);
+}
+
+static int ab8500_gpadc_main_charger_v_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_main_charger_v_print,
+			inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_main_charger_v_fops = {
+	.open = ab8500_gpadc_main_charger_v_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_acc_detect1_print(struct seq_file *s, void *p)
+{
+	int acc_detect1_raw;
+	int acc_detect1_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	acc_detect1_raw = ab8500_gpadc_read_raw(gpadc, ACC_DETECT1);
+	acc_detect1_convert = ab8500_gpadc_ad_to_voltage(gpadc, ACC_DETECT1,
+			acc_detect1_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			acc_detect1_convert, acc_detect1_raw);
+}
+
+static int ab8500_gpadc_acc_detect1_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_acc_detect1_print,
+			inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_acc_detect1_fops = {
+	.open = ab8500_gpadc_acc_detect1_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_acc_detect2_print(struct seq_file *s, void *p)
+{
+	int acc_detect2_raw;
+	int acc_detect2_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	acc_detect2_raw = ab8500_gpadc_read_raw(gpadc, ACC_DETECT2);
+	acc_detect2_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+	    ACC_DETECT2, acc_detect2_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			acc_detect2_convert, acc_detect2_raw);
+}
+
+static int ab8500_gpadc_acc_detect2_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_acc_detect2_print,
+	    inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_acc_detect2_fops = {
+	.open = ab8500_gpadc_acc_detect2_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_aux1_print(struct seq_file *s, void *p)
+{
+	int aux1_raw;
+	int aux1_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	aux1_raw = ab8500_gpadc_read_raw(gpadc, ADC_AUX1);
+	aux1_convert = ab8500_gpadc_ad_to_voltage(gpadc, ADC_AUX1,
+			aux1_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			aux1_convert, aux1_raw);
+}
+
+static int ab8500_gpadc_aux1_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_aux1_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_aux1_fops = {
+	.open = ab8500_gpadc_aux1_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_aux2_print(struct seq_file *s, void *p)
+{
+	int aux2_raw;
+	int aux2_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	aux2_raw = ab8500_gpadc_read_raw(gpadc, ADC_AUX2);
+	aux2_convert = ab8500_gpadc_ad_to_voltage(gpadc, ADC_AUX2,
+			aux2_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			aux2_convert, aux2_raw);
+}
+
+static int ab8500_gpadc_aux2_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_aux2_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_aux2_fops = {
+	.open = ab8500_gpadc_aux2_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_main_bat_v_print(struct seq_file *s, void *p)
+{
+	int main_bat_v_raw;
+	int main_bat_v_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	main_bat_v_raw = ab8500_gpadc_read_raw(gpadc, MAIN_BAT_V);
+	main_bat_v_convert = ab8500_gpadc_ad_to_voltage(gpadc, MAIN_BAT_V,
+			main_bat_v_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			main_bat_v_convert, main_bat_v_raw);
+}
+
+static int ab8500_gpadc_main_bat_v_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_main_bat_v_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_main_bat_v_fops = {
+	.open = ab8500_gpadc_main_bat_v_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_vbus_v_print(struct seq_file *s, void *p)
+{
+	int vbus_v_raw;
+	int vbus_v_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	vbus_v_raw = ab8500_gpadc_read_raw(gpadc, VBUS_V);
+	vbus_v_convert = ab8500_gpadc_ad_to_voltage(gpadc, VBUS_V,
+			vbus_v_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			vbus_v_convert, vbus_v_raw);
+}
+
+static int ab8500_gpadc_vbus_v_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_vbus_v_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_vbus_v_fops = {
+	.open = ab8500_gpadc_vbus_v_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_main_charger_c_print(struct seq_file *s, void *p)
+{
+	int main_charger_c_raw;
+	int main_charger_c_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	main_charger_c_raw = ab8500_gpadc_read_raw(gpadc, MAIN_CHARGER_C);
+	main_charger_c_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+			MAIN_CHARGER_C, main_charger_c_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			main_charger_c_convert, main_charger_c_raw);
+}
+
+static int ab8500_gpadc_main_charger_c_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_main_charger_c_print,
+			inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_main_charger_c_fops = {
+	.open = ab8500_gpadc_main_charger_c_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_usb_charger_c_print(struct seq_file *s, void *p)
+{
+	int usb_charger_c_raw;
+	int usb_charger_c_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	usb_charger_c_raw = ab8500_gpadc_read_raw(gpadc, USB_CHARGER_C);
+	usb_charger_c_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+	    USB_CHARGER_C, usb_charger_c_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			usb_charger_c_convert, usb_charger_c_raw);
+}
+
+static int ab8500_gpadc_usb_charger_c_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_usb_charger_c_print,
+	    inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_usb_charger_c_fops = {
+	.open = ab8500_gpadc_usb_charger_c_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_bk_bat_v_print(struct seq_file *s, void *p)
+{
+	int bk_bat_v_raw;
+	int bk_bat_v_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	bk_bat_v_raw = ab8500_gpadc_read_raw(gpadc, BK_BAT_V);
+	bk_bat_v_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+			BK_BAT_V, bk_bat_v_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			bk_bat_v_convert, bk_bat_v_raw);
+}
+
+static int ab8500_gpadc_bk_bat_v_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_bk_bat_v_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_bk_bat_v_fops = {
+	.open = ab8500_gpadc_bk_bat_v_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_die_temp_print(struct seq_file *s, void *p)
+{
+	int die_temp_raw;
+	int die_temp_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get();
+	die_temp_raw = ab8500_gpadc_read_raw(gpadc, DIE_TEMP);
+	die_temp_convert = ab8500_gpadc_ad_to_voltage(gpadc, DIE_TEMP,
+			die_temp_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			die_temp_convert, die_temp_raw);
+}
+
+static int ab8500_gpadc_die_temp_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_die_temp_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_die_temp_fops = {
+	.open = ab8500_gpadc_die_temp_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+/*
+ * return length of an ASCII numerical value, 0 is string is not a
+ * numerical value.
+ * string shall start at value 1st char.
+ * string can be tailed with \0 or space or newline chars only.
+ * value can be decimal or hexadecimal (prefixed 0x or 0X).
+ */
+static int strval_len(char *b)
+{
+	char *s = b;
+	if ((*s == '0') && ((*(s+1) == 'x') || (*(s+1) == 'X'))) {
+		s += 2;
+		for (; *s && (*s != ' ') && (*s != '\n'); s++) {
+			if (!isxdigit(*s))
+				return 0;
+		}
+	} else {
+		if (*s == '-')
+			s++;
+		for (; *s && (*s != ' ') && (*s != '\n'); s++) {
+			if (!isdigit(*s))
+				return 0;
+		}
+	}
+	return (int) (s-b);
+}
+
+/*
+ * parse hwreg input data.
+ * update global hwreg_cfg only if input data syntax is ok.
+ */
+static ssize_t hwreg_common_write(char *b, struct hwreg_cfg *cfg,
+		struct device *dev)
+{
+	uint write, val = 0;
+	struct hwreg_cfg loc = {
+		.bank = 0,          /* default: invalid phys addr */
+		.addr = 0,          /* default: invalid phys addr */
+		.fmt = 0,           /* default: 32bit access, hex output */
+		.mask = 0xFFFFFFFF, /* default: no mask */
+		.shift = 0,         /* default: no bit shift */
+	};
+
+	/* read or write ? */
+	if (!strncmp(b, "read ", 5)) {
+		write = 0;
+		b += 5;
+	} else if (!strncmp(b, "write ", 6)) {
+		write = 1;
+		b += 6;
+	} else
+		return -EINVAL;
+
+	/* OPTIONS -l|-w|-b -s -m -o */
+	while ((*b == ' ') || (*b == '-')) {
+		if (*(b-1) != ' ') {
+			b++;
+			continue;
+		}
+		if ((!strncmp(b, "-d ", 3)) ||
+				(!strncmp(b, "-dec ", 5))) {
+			b += (*(b+2) == ' ') ? 3 : 5;
+			loc.fmt |= (1<<0);
+		} else if ((!strncmp(b, "-h ", 3)) ||
+				(!strncmp(b, "-hex ", 5))) {
+			b += (*(b+2) == ' ') ? 3 : 5;
+			loc.fmt &= ~(1<<0);
+		} else if ((!strncmp(b, "-m ", 3)) ||
+				(!strncmp(b, "-mask ", 6))) {
+			b += (*(b+2) == ' ') ? 3 : 6;
+			if (strval_len(b) == 0)
+				return -EINVAL;
+			loc.mask = simple_strtoul(b, &b, 0);
+		} else if ((!strncmp(b, "-s ", 3)) ||
+				(!strncmp(b, "-shift ", 7))) {
+			b += (*(b+2) == ' ') ? 3 : 7;
+			if (strval_len(b) == 0)
+				return -EINVAL;
+			loc.shift = simple_strtol(b, &b, 0);
+		} else {
+			return -EINVAL;
+		}
+	}
+	/* get arg BANK and ADDRESS */
+	if (strval_len(b) == 0)
+		return -EINVAL;
+	loc.bank = simple_strtoul(b, &b, 0);
+	while (*b == ' ')
+		b++;
+	if (strval_len(b) == 0)
+		return -EINVAL;
+	loc.addr = simple_strtoul(b, &b, 0);
+
+	if (write) {
+		while (*b == ' ')
+			b++;
+		if (strval_len(b) == 0)
+			return -EINVAL;
+		val = simple_strtoul(b, &b, 0);
+	}
+
+	/* args are ok, update target cfg (mainly for read) */
+	*cfg = loc;
+
+#ifdef ABB_HWREG_DEBUG
+	pr_warn("HWREG request: %s, %s, addr=0x%08X, mask=0x%X, shift=%d"
+			"value=0x%X\n", (write) ? "write" : "read",
+			REG_FMT_DEC(cfg) ? "decimal" : "hexa",
+			cfg->addr, cfg->mask, cfg->shift, val);
+#endif
+
+	if (write) {
+		u8  regvalue;
+		int ret = abx500_get_register_interruptible(dev,
+			(u8)cfg->bank, (u8)cfg->addr, &regvalue);
+		if (ret < 0) {
+			dev_err(dev, "abx500_get_reg fail %d, %d\n",
+				ret, __LINE__);
+			return -EINVAL;
+		}
+
+		if (cfg->shift >= 0) {
+			regvalue &= ~(cfg->mask << (cfg->shift));
+			val = (val & cfg->mask) << (cfg->shift);
+		} else {
+			regvalue &= ~(cfg->mask >> (-cfg->shift));
+			val = (val & cfg->mask) >> (-cfg->shift);
+		}
+		val = val | regvalue;
+
+		ret = abx500_set_register_interruptible(dev,
+			(u8)cfg->bank, (u8)cfg->addr, (u8)val);
+		if (ret < 0) {
+			pr_err("abx500_set_reg failed %d, %d", ret, __LINE__);
+			return -EINVAL;
+		}
+
+	}
+	return 0;
+}
+
+static ssize_t ab8500_hwreg_write(struct file *file,
+	const char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct device *dev = ((struct seq_file *)(file->private_data))->private;
+	char buf[128];
+	int buf_size, ret;
+
+	/* Get userspace string and assure termination */
+	buf_size = min(count, (sizeof(buf)-1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	/* get args and process */
+	ret = hwreg_common_write(buf, &hwreg_cfg, dev);
+	return (ret) ? ret : buf_size;
+}
+
+/*
+ * - irq subscribe/unsubscribe stuff
+ */
+static int ab8500_subscribe_unsubscribe_print(struct seq_file *s, void *p)
+{
+	seq_printf(s, "%d\n", irq_first);
+
+	return 0;
+}
+
+static int ab8500_subscribe_unsubscribe_open(struct inode *inode,
+	struct file *file)
+{
+	return single_open(file, ab8500_subscribe_unsubscribe_print,
+		inode->i_private);
+}
+
+/*
+ * Userspace should use poll() on this file. When an event occur
+ * the blocking poll will be released.
+ */
+static ssize_t show_irq(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	unsigned long name;
+	unsigned int irq_index;
+	int err;
+
+	err = strict_strtoul(attr->attr.name, 0, &name);
+	if (err)
+		return err;
+
+	irq_index = name - irq_first;
+	if (irq_index >= AB8500_NR_IRQS)
+		return -EINVAL;
+	else
+		return sprintf(buf, "%u\n", irq_count[irq_index]);
+}
+
+static ssize_t ab8500_subscribe_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct device *dev = ((struct seq_file *)(file->private_data))->private;
+	char buf[32];
+	int buf_size;
+	unsigned long user_val;
+	int err;
+	unsigned int irq_index;
+
+	/* Get userspace string and assure termination */
+	buf_size = min(count, (sizeof(buf)-1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	err = strict_strtoul(buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	if (user_val < irq_first) {
+		dev_err(dev, "debugfs error input < %d\n", irq_first);
+		return -EINVAL;
+	}
+	if (user_val > irq_last) {
+		dev_err(dev, "debugfs error input > %d\n", irq_last);
+		return -EINVAL;
+	}
+
+	irq_index = user_val - irq_first;
+	if (irq_index >= AB8500_NR_IRQS)
+		return -EINVAL;
+
+	/*
+	 * This will create a sysfs file named <irq-nr> which userspace can
+	 * use to select or poll and get the AB8500 events
+	 */
+	dev_attr[irq_index] = kmalloc(sizeof(struct device_attribute),
+		GFP_KERNEL);
+	event_name[irq_index] = kmalloc(buf_size, GFP_KERNEL);
+	sprintf(event_name[irq_index], "%lu", user_val);
+	dev_attr[irq_index]->show = show_irq;
+	dev_attr[irq_index]->store = NULL;
+	dev_attr[irq_index]->attr.name = event_name[irq_index];
+	dev_attr[irq_index]->attr.mode = S_IRUGO;
+	err = sysfs_create_file(&dev->kobj, &dev_attr[irq_index]->attr);
+	if (err < 0) {
+		printk(KERN_ERR "sysfs_create_file failed %d\n", err);
+		return err;
+	}
+
+	err = request_threaded_irq(user_val, NULL, ab8500_debug_handler,
+		IRQF_SHARED | IRQF_NO_SUSPEND, "ab8500-debug", &dev->kobj);
+	if (err < 0) {
+		printk(KERN_ERR "request_threaded_irq failed %d, %lu\n",
+			err, user_val);
+		sysfs_remove_file(&dev->kobj, &dev_attr[irq_index]->attr);
+		return err;
+	}
+
+	return buf_size;
+}
+
+static ssize_t ab8500_unsubscribe_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct device *dev = ((struct seq_file *)(file->private_data))->private;
+	char buf[32];
+	int buf_size;
+	unsigned long user_val;
+	int err;
+	unsigned int irq_index;
+
+	/* Get userspace string and assure termination */
+	buf_size = min(count, (sizeof(buf)-1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	err = strict_strtoul(buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	if (user_val < irq_first) {
+		dev_err(dev, "debugfs error input < %d\n", irq_first);
+		return -EINVAL;
+	}
+	if (user_val > irq_last) {
+		dev_err(dev, "debugfs error input > %d\n", irq_last);
+		return -EINVAL;
+	}
+
+	irq_index = user_val - irq_first;
+	if (irq_index >= AB8500_NR_IRQS)
+		return -EINVAL;
+
+	/* Set irq count to 0 when unsubscribe */
+	irq_count[irq_index] = 0;
+
+	if (dev_attr[irq_index])
+		sysfs_remove_file(&dev->kobj, &dev_attr[irq_index]->attr);
+
+
+	free_irq(user_val, &dev->kobj);
+	kfree(event_name[irq_index]);
+	kfree(dev_attr[irq_index]);
 
 	return buf_size;
 }
 
+/*
+ * - several deubgfs nodes fops
+ */
+
 static const struct file_operations ab8500_bank_fops = {
 	.open = ab8500_bank_open,
 	.write = ab8500_bank_write,
@@ -564,65 +1406,177 @@ static const struct file_operations ab8500_val_fops = {
 	.owner = THIS_MODULE,
 };
 
+static const struct file_operations ab8500_subscribe_fops = {
+	.open = ab8500_subscribe_unsubscribe_open,
+	.write = ab8500_subscribe_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ab8500_unsubscribe_fops = {
+	.open = ab8500_subscribe_unsubscribe_open,
+	.write = ab8500_unsubscribe_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ab8500_hwreg_fops = {
+	.open = ab8500_hwreg_open,
+	.write = ab8500_hwreg_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
 static struct dentry *ab8500_dir;
-static struct dentry *ab8500_reg_file;
-static struct dentry *ab8500_bank_file;
-static struct dentry *ab8500_address_file;
-static struct dentry *ab8500_val_file;
+static struct dentry *ab8500_gpadc_dir;
 
 static int __devinit ab8500_debug_probe(struct platform_device *plf)
 {
+	struct dentry *file;
 	debug_bank = AB8500_MISC;
 	debug_address = AB8500_REV_REG & 0x00FF;
 
+	irq_first = platform_get_irq_byname(plf, "IRQ_FIRST");
+	if (irq_first < 0) {
+		dev_err(&plf->dev, "First irq not found, err %d\n",
+				irq_first);
+		return irq_first;
+	}
+
+	irq_last = platform_get_irq_byname(plf, "IRQ_LAST");
+	if (irq_last < 0) {
+		dev_err(&plf->dev, "Last irq not found, err %d\n",
+				irq_last);
+		return irq_last;
+	}
+
 	ab8500_dir = debugfs_create_dir(AB8500_NAME_STRING, NULL);
 	if (!ab8500_dir)
-		goto exit_no_debugfs;
+		goto err;
+
+	ab8500_gpadc_dir = debugfs_create_dir(AB8500_ADC_NAME_STRING,
+	    ab8500_dir);
+	if (!ab8500_gpadc_dir)
+		goto err;
+
+	file = debugfs_create_file("all-bank-registers", S_IRUGO,
+	    ab8500_dir, &plf->dev, &ab8500_registers_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("register-bank", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_bank_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("register-address", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_address_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("register-value", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_val_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("irq-subscribe", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_subscribe_fops);
+	if (!file)
+		goto err;
 
-	ab8500_reg_file = debugfs_create_file("all-bank-registers",
-		S_IRUGO, ab8500_dir, &plf->dev, &ab8500_registers_fops);
-	if (!ab8500_reg_file)
-		goto exit_destroy_dir;
+	file = debugfs_create_file("irq-unsubscribe", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_unsubscribe_fops);
+	if (!file)
+		goto err;
 
-	ab8500_bank_file = debugfs_create_file("register-bank",
-		(S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev, &ab8500_bank_fops);
-	if (!ab8500_bank_file)
-		goto exit_destroy_reg;
+	file = debugfs_create_file("hwreg", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_hwreg_fops);
+	if (!file)
+		goto err;
 
-	ab8500_address_file = debugfs_create_file("register-address",
-		(S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev,
-		&ab8500_address_fops);
-	if (!ab8500_address_file)
-		goto exit_destroy_bank;
+	file = debugfs_create_file("bat_ctrl", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_bat_ctrl_fops);
+	if (!file)
+		goto err;
 
-	ab8500_val_file = debugfs_create_file("register-value",
-		(S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev, &ab8500_val_fops);
-	if (!ab8500_val_file)
-		goto exit_destroy_address;
+	file = debugfs_create_file("btemp_ball", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_btemp_ball_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("main_charger_v", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_charger_v_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("acc_detect1", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_acc_detect1_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("acc_detect2", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_acc_detect2_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("adc_aux1", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_aux1_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("adc_aux2", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_aux2_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("main_bat_v", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_bat_v_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("vbus_v", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_vbus_v_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("main_charger_c", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_charger_c_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("usb_charger_c", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_usb_charger_c_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("bk_bat_v", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_bk_bat_v_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("die_temp", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_die_temp_fops);
+	if (!file)
+		goto err;
 
 	return 0;
 
-exit_destroy_address:
-	debugfs_remove(ab8500_address_file);
-exit_destroy_bank:
-	debugfs_remove(ab8500_bank_file);
-exit_destroy_reg:
-	debugfs_remove(ab8500_reg_file);
-exit_destroy_dir:
-	debugfs_remove(ab8500_dir);
-exit_no_debugfs:
+err:
+	if (ab8500_dir)
+		debugfs_remove_recursive(ab8500_dir);
 	dev_err(&plf->dev, "failed to create debugfs entries.\n");
 	return -ENOMEM;
 }
 
 static int __devexit ab8500_debug_remove(struct platform_device *plf)
 {
-	debugfs_remove(ab8500_val_file);
-	debugfs_remove(ab8500_address_file);
-	debugfs_remove(ab8500_bank_file);
-	debugfs_remove(ab8500_reg_file);
-	debugfs_remove(ab8500_dir);
-
+	debugfs_remove_recursive(ab8500_dir);
 	return 0;
 }
 
diff --git a/drivers/mfd/ab8500-denc.c b/drivers/mfd/ab8500-denc.c
new file mode 100644
index 00000000000..1bf61e41bcd
--- /dev/null
+++ b/drivers/mfd/ab8500-denc.c
@@ -0,0 +1,538 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson AB8500 DENC base driver
+ *
+ * Author: Marcel Tunnissen <marcel.tuennissen@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/debugfs.h>
+#include <linux/list.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/err.h>
+#include <linux/uaccess.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/ab8500/denc-regs.h>
+#include <linux/mfd/ab8500/denc.h>
+
+#define AB8500_NAME      "ab8500"
+#define AB8500_DENC_NAME "ab8500_denc"
+
+struct device_usage {
+	struct list_head list;
+	struct platform_device *pdev;
+	bool taken;
+};
+static LIST_HEAD(device_list);
+
+/* To get rid of the extra bank parameter: */
+#define AB8500_REG_BANK_NR(__reg)	((0xff00 & (__reg)) >> 8)
+static inline u8 ab8500_rreg(struct device *dev, u32 reg)
+{
+	u8 val;
+	if (abx500_get_register_interruptible(dev, AB8500_REG_BANK_NR(reg),
+								reg, &val) < 0)
+		return 0;
+	else
+		return val;
+}
+
+static inline int ab8500_wreg(struct device *dev, u32 reg, u8 val)
+{
+	return abx500_set_register_interruptible(dev, AB8500_REG_BANK_NR(reg),
+								reg, val);
+}
+
+/* Only use in the macro below: */
+static inline int _ab8500_wreg_fld(struct device *dev, u32 reg, u8 val,
+							u8 mask, u8 shift)
+{
+	int ret;
+	u8 org_val;
+
+	ret = abx500_get_register_interruptible(dev, AB8500_REG_BANK_NR(reg),
+								reg, &org_val);
+	if (ret < 0)
+		return ret;
+	else
+		ab8500_wreg(dev, reg,
+				(org_val & ~mask) | ((val << shift) & mask));
+	return 0;
+}
+
+#define ab8500_wr_fld(__d, __reg, __fld, __val)				\
+	_ab8500_wreg_fld(__d, __reg, __val, __reg##_##__fld##_MASK,	\
+						__reg##_##__fld##_SHIFT)
+
+#define ab8500_set_fld(__cur_val, __reg, __fld, __val)			\
+	(((__cur_val) & ~__reg##_##__fld##_MASK) |			\
+	(((__val) << __reg##_##__fld##_SHIFT) & __reg##_##__fld##_MASK))
+
+#define AB8500_DENC_TRACE(__pd)	dev_dbg(&(__pd)->dev, "%s\n", __func__)
+
+#ifdef CONFIG_DEBUG_FS
+static struct dentry *debugfs_ab8500_denc_dir;
+static struct dentry *debugfs_ab8500_dump_regs_file;
+static void ab8500_denc_conf_ddr(struct platform_device *pdev);
+static int debugfs_ab8500_open_file(struct inode *inode, struct file *file);
+static ssize_t debugfs_ab8500_dump_regs(struct file *file, char __user *buf,
+						size_t count, loff_t *f_pos);
+
+static const struct file_operations debugfs_ab8500_dump_regs_fops = {
+	.owner = THIS_MODULE,
+	.open  = debugfs_ab8500_open_file,
+	.read  = debugfs_ab8500_dump_regs,
+};
+#endif /* CONFIG_DEBUG_FS */
+
+static int __devinit ab8500_denc_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct ab8500_platform_data *ab8500_pdata =
+		dev_get_platdata(pdev->dev.parent);
+	struct ab8500_denc_platform_data *pdata;
+	struct device_usage *device_data;
+
+	AB8500_DENC_TRACE(pdev);
+
+	if (ab8500_pdata == NULL) {
+		dev_err(&pdev->dev, "AB8500 platform data missing\n");
+		return -EINVAL;
+	}
+
+	pdata = ab8500_pdata->denc;
+	if (pdata == NULL) {
+		dev_err(&pdev->dev, "Denc platform data missing\n");
+		return -EINVAL;
+	}
+
+	device_data = kzalloc(sizeof(struct device_usage), GFP_KERNEL);
+	if (!device_data) {
+		dev_err(&pdev->dev, "Failed to allocate device data\n");
+		return -ENOMEM;
+	}
+	device_data->pdev = pdev;
+	list_add_tail(&device_data->list, &device_list);
+
+#ifdef CONFIG_DEBUG_FS
+	debugfs_ab8500_denc_dir = debugfs_create_dir(pdev->name, NULL);
+	debugfs_ab8500_dump_regs_file = debugfs_create_file(
+			"dumpregs", S_IRUGO,
+			debugfs_ab8500_denc_dir, &pdev->dev,
+			&debugfs_ab8500_dump_regs_fops
+		);
+#endif /* CONFIG_DEBUG_FS */
+	return ret;
+}
+
+static int __devexit ab8500_denc_remove(struct platform_device *pdev)
+{
+	struct list_head *element;
+	struct device_usage *device_data;
+
+	AB8500_DENC_TRACE(pdev);
+
+#ifdef CONFIG_DEBUG_FS
+	debugfs_remove(debugfs_ab8500_dump_regs_file);
+	debugfs_remove(debugfs_ab8500_denc_dir);
+#endif /* CONFIG_DEBUG_FS */
+
+	list_for_each(element, &device_list) {
+		device_data = list_entry(element, struct device_usage, list);
+		if (device_data->pdev == pdev) {
+			list_del(element);
+			kzfree(device_data);
+		}
+	}
+
+	return 0;
+}
+
+static struct platform_driver ab8500_denc_driver = {
+	.probe	= ab8500_denc_probe,
+	.remove = ab8500_denc_remove,
+	.driver = {
+		.name	= "ab8500-denc",
+	},
+};
+
+static void setup_27mhz(struct platform_device *pdev, bool enable)
+{
+	u8 data = ab8500_rreg(&pdev->dev, AB8500_SYS_ULP_CLK_CONF);
+
+	AB8500_DENC_TRACE(pdev);
+	/* TODO: check if this field needs to be set */
+	data = ab8500_set_fld(data, AB8500_SYS_ULP_CLK_CONF, CLK_27MHZ_PD_ENA,
+									true);
+	data = ab8500_set_fld(data, AB8500_SYS_ULP_CLK_CONF, CLK_27MHZ_BUF_ENA,
+									enable);
+	data = ab8500_set_fld(data, AB8500_SYS_ULP_CLK_CONF, TVOUT_CLK_INV,
+									false);
+	data = ab8500_set_fld(data, AB8500_SYS_ULP_CLK_CONF, TVOUT_CLK_DE_IN,
+									false);
+	data = ab8500_set_fld(data, AB8500_SYS_ULP_CLK_CONF, CLK_27MHZ_STRE,
+									1);
+	ab8500_wreg(&pdev->dev, AB8500_SYS_ULP_CLK_CONF, data);
+
+	data = ab8500_rreg(&pdev->dev, AB8500_SYS_CLK_CTRL);
+	data = ab8500_set_fld(data, AB8500_SYS_CLK_CTRL, TVOUT_CLK_VALID,
+									enable);
+	data = ab8500_set_fld(data, AB8500_SYS_CLK_CTRL, TVOUT_PLL_ENA,
+									enable);
+	ab8500_wreg(&pdev->dev, AB8500_SYS_CLK_CTRL, data);
+}
+
+static u32 map_tv_std(enum ab8500_denc_TV_std std)
+{
+	switch (std) {
+	case TV_STD_PAL_BDGHI:
+		return AB8500_DENC_CONF0_STD_PAL_BDGHI;
+	case TV_STD_PAL_N:
+		return AB8500_DENC_CONF0_STD_PAL_N;
+	case TV_STD_PAL_M:
+		return AB8500_DENC_CONF0_STD_PAL_M;
+	case TV_STD_NTSC_M:
+		return AB8500_DENC_CONF0_STD_NTSC_M;
+	default:
+		return 0;
+	}
+}
+
+static u32 map_cr_filter(enum ab8500_denc_cr_filter_bandwidth bw)
+{
+	switch (bw) {
+	case TV_CR_NTSC_LOW_DEF_FILTER:
+		return AB8500_DENC_CONF1_FLT_1_1MHZ;
+	case TV_CR_PAL_LOW_DEF_FILTER:
+		return AB8500_DENC_CONF1_FLT_1_3MHZ;
+	case TV_CR_NTSC_HIGH_DEF_FILTER:
+		return AB8500_DENC_CONF1_FLT_1_6MHZ;
+	case TV_CR_PAL_HIGH_DEF_FILTER:
+		return AB8500_DENC_CONF1_FLT_1_9MHZ;
+	default:
+		return 0;
+	}
+}
+
+static u32 map_phase_rst_mode(enum ab8500_denc_phase_reset_mode mode)
+{
+	switch (mode) {
+	case TV_PHASE_RST_MOD_DISABLE:
+		return AB8500_DENC_CONF8_PH_RST_MODE_DISABLED;
+	case TV_PHASE_RST_MOD_FROM_PHASE_BUF:
+		return AB8500_DENC_CONF8_PH_RST_MODE_UPDATE_FROM_PHASE_BUF;
+	case TV_PHASE_RST_MOD_FROM_INC_DFS:
+		return AB8500_DENC_CONF8_PH_RST_MODE_UPDATE_FROM_INC_DFS;
+	case TV_PHASE_RST_MOD_RST:
+		return AB8500_DENC_CONF8_PH_RST_MODE_RESET;
+	default:
+		return 0;
+	}
+}
+
+static u32 map_plug_time(enum ab8500_denc_plug_time time)
+{
+	switch (time) {
+	case TV_PLUG_TIME_0_5S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_0_5S;
+	case TV_PLUG_TIME_1S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_1S;
+	case TV_PLUG_TIME_1_5S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_1_5S;
+	case TV_PLUG_TIME_2S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_2S;
+	case TV_PLUG_TIME_2_5S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_2_5S;
+	case TV_PLUG_TIME_3S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_3S;
+	default:
+		return 0;
+	}
+}
+
+struct platform_device *ab8500_denc_get_device(void)
+{
+	struct list_head *element;
+	struct device_usage *device_data;
+
+	pr_debug("%s\n", __func__);
+	list_for_each(element, &device_list) {
+		device_data = list_entry(element, struct device_usage, list);
+		if (!device_data->taken) {
+			device_data->taken = true;
+			return device_data->pdev;
+		}
+	}
+	return NULL;
+}
+EXPORT_SYMBOL(ab8500_denc_get_device);
+
+void ab8500_denc_put_device(struct platform_device *pdev)
+{
+	struct list_head *element;
+	struct device_usage *device_data;
+
+	AB8500_DENC_TRACE(pdev);
+	list_for_each(element, &device_list) {
+		device_data = list_entry(element, struct device_usage, list);
+		if (device_data->pdev == pdev)
+			device_data->taken = false;
+	}
+}
+EXPORT_SYMBOL(ab8500_denc_put_device);
+
+void ab8500_denc_reset(struct platform_device *pdev, bool hard)
+{
+	AB8500_DENC_TRACE(pdev);
+	if (hard) {
+		u8 data = ab8500_rreg(&pdev->dev, AB8500_CTRL3);
+		/* reset start */
+		ab8500_wreg(&pdev->dev, AB8500_CTRL3,
+			ab8500_set_fld(data, AB8500_CTRL3, RESET_DENC_N, 0)
+		);
+		/* reset done */
+		ab8500_wreg(&pdev->dev, AB8500_CTRL3,
+			ab8500_set_fld(data, AB8500_CTRL3, RESET_DENC_N, 1)
+		);
+	} else {
+		ab8500_wr_fld(&pdev->dev, AB8500_DENC_CONF6, SOFT_RESET, 1);
+		mdelay(10);
+	}
+}
+EXPORT_SYMBOL(ab8500_denc_reset);
+
+void ab8500_denc_power_up(struct platform_device *pdev)
+{
+	setup_27mhz(pdev, true);
+}
+EXPORT_SYMBOL(ab8500_denc_power_up);
+
+void ab8500_denc_power_down(struct platform_device *pdev)
+{
+	setup_27mhz(pdev, false);
+}
+EXPORT_SYMBOL(ab8500_denc_power_down);
+
+void ab8500_denc_conf(struct platform_device *pdev,
+						struct ab8500_denc_conf *conf)
+{
+	u8 data;
+
+	AB8500_DENC_TRACE(pdev);
+
+	ab8500_wreg(&pdev->dev, AB8500_DENC_CONF0,
+		AB8500_VAL2REG(AB8500_DENC_CONF0, STD, map_tv_std(conf->TV_std))
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF0, SYNC,
+			conf->test_pattern ? AB8500_DENC_CONF0_SYNC_AUTO_TEST :
+					AB8500_DENC_CONF0_SYNC_F_BASED_SLAVE
+		)
+	);
+	ab8500_wreg(&pdev->dev, AB8500_DENC_CONF1,
+		AB8500_VAL2REG(AB8500_DENC_CONF1, BLK_LI,
+						!conf->partial_blanking)
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF1, FLT,
+						map_cr_filter(conf->cr_filter))
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF1, CO_KI, conf->suppress_col)
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF1, SETUP_MAIN,
+						conf->black_level_setup)
+		/* TODO: handle cc field: set to 0 now */
+	);
+
+	data = ab8500_rreg(&pdev->dev, AB8500_DENC_CONF2);
+	data = ab8500_set_fld(data, AB8500_DENC_CONF2, N_INTRL,
+						conf->progressive);
+	ab8500_wreg(&pdev->dev, AB8500_DENC_CONF2, data);
+
+	ab8500_wreg(&pdev->dev, AB8500_DENC_CONF8,
+		AB8500_VAL2REG(AB8500_DENC_CONF8, PH_RST_MODE,
+				map_phase_rst_mode(conf->phase_reset_mode))
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF8, VAL_422_MUX,
+						conf->act_output)
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF8, BLK_ALL,
+						conf->blank_all)
+	);
+	data = ab8500_rreg(&pdev->dev, AB8500_TVOUT_CTRL);
+	data = ab8500_set_fld(data, AB8500_TVOUT_CTRL, DAC_CTRL0,
+							conf->dac_enable);
+	data = ab8500_set_fld(data, AB8500_TVOUT_CTRL, DAC_CTRL1,
+							conf->act_dc_output);
+	ab8500_wreg(&pdev->dev, AB8500_TVOUT_CTRL, data);
+
+	/* no support for DDR in early versions */
+	if (AB8500_REG2VAL(AB8500_REV, FULL_MASK,
+				ab8500_rreg(&pdev->dev, AB8500_REV)) > 0)
+		ab8500_denc_conf_ddr(pdev);
+}
+EXPORT_SYMBOL(ab8500_denc_conf);
+
+void ab8500_denc_conf_plug_detect(struct platform_device *pdev,
+						bool enable, bool load_RC,
+						enum ab8500_denc_plug_time time)
+{
+	u8 data;
+
+	AB8500_DENC_TRACE(pdev);
+	data = ab8500_rreg(&pdev->dev, AB8500_TVOUT_CTRL);
+	data = ab8500_set_fld(data, AB8500_TVOUT_CTRL, TV_PLUG_ON,   enable);
+	data = ab8500_set_fld(data, AB8500_TVOUT_CTRL, TV_LOAD_RC,   load_RC);
+	data = ab8500_set_fld(data, AB8500_TVOUT_CTRL, PLUG_TV_TIME,
+							map_plug_time(time));
+	ab8500_wreg(&pdev->dev, AB8500_TVOUT_CTRL, data);
+}
+EXPORT_SYMBOL(ab8500_denc_conf_plug_detect);
+
+void ab8500_denc_mask_int_plug_det(struct platform_device *pdev, bool plug,
+								bool unplug)
+{
+	u8 data = ab8500_rreg(&pdev->dev, AB8500_IT_MASK1);
+
+	AB8500_DENC_TRACE(pdev);
+	data = ab8500_set_fld(data, AB8500_IT_MASK1, PLUG_TV_DET,   plug);
+	data = ab8500_set_fld(data, AB8500_IT_MASK1, UNPLUG_TV_DET, unplug);
+	ab8500_wreg(&pdev->dev, AB8500_IT_MASK1, data);
+}
+EXPORT_SYMBOL(ab8500_denc_mask_int_plug_det);
+
+static void ab8500_denc_conf_ddr(struct platform_device *pdev)
+{
+	struct ab8500_platform_data *core_pdata;
+	struct ab8500_denc_platform_data *denc_pdata;
+
+	AB8500_DENC_TRACE(pdev);
+	core_pdata = dev_get_platdata(pdev->dev.parent);
+	denc_pdata = core_pdata->denc;
+	ab8500_wreg(&pdev->dev, AB8500_TVOUT_CTRL2,
+		AB8500_VAL2REG(AB8500_TVOUT_CTRL2,
+					DENC_DDR, denc_pdata->ddr_enable) |
+		AB8500_VAL2REG(AB8500_TVOUT_CTRL2, SWAP_DDR_DATA_IN,
+					denc_pdata->ddr_little_endian));
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int debugfs_ab8500_open_file(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+#define DEBUG_BUF_SIZE 900
+
+#define AB8500_GPIO_DIR5                0x1014
+#define AB8500_GPIO_DIR5_35_SHIFT       2
+#define AB8500_GPIO_DIR5_35_MASK        (1 << AB8500_GPIO_DIR5_35_SHIFT)
+#define AB8500_GPIO_OUT5                0x1024
+#define AB8500_GPIO_OUT5_35_SHIFT       2
+#define AB8500_GPIO_OUT5_35_MASK        (1 << AB8500_GPIO_OUT5_35_SHIFT)
+#define AB8500_GPIO_OUT5_35_VIDEO       0
+#define AB8500_GPIO_OUT5_35_AUDIO       1
+#define AB8500_GPIO_NPUD5                0x1034
+#define AB8500_GPIO_NPUD5_35_SHIFT       2
+#define AB8500_GPIO_NPUD5_35_MASK        (1 << AB8500_GPIO_NPUD5_35_SHIFT)
+#define AB8500_GPIO_NPUD5_35_ACTIVE      0
+#define AB8500_GPIO_NPUD5_35_INACTIVE    1
+
+static ssize_t debugfs_ab8500_dump_regs(struct file *file, char __user *buf,
+						size_t count, loff_t *f_pos)
+{
+	int ret = 0;
+	size_t data_size = 0;
+	char   buffer[DEBUG_BUF_SIZE];
+	struct device *dev = file->private_data;
+
+	data_size += sprintf(buffer + data_size,
+		"AB8500 DENC registers:\n"
+		"------Regulators etc ----------\n"
+		"CTRL3         : 0x%04x = 0x%02x\n"
+		"SYSULPCLK_CONF: 0x%04x = 0x%02x\n"
+		"SYSCLK_CTRL   : 0x%04x = 0x%02x\n"
+		"REGU_MISC1    : 0x%04x = 0x%02x\n"
+		"VAUX12_REGU   : 0x%04x = 0x%02x\n"
+		"VAUX1_SEL1    : 0x%04x = 0x%02x\n"
+		"------TVout only --------------\n"
+		"DENC_CONF0    : 0x%04x = 0x%02x\n"
+		"DENC_CONF1    : 0x%04x = 0x%02x\n"
+		"DENC_CONF2    : 0x%04x = 0x%02x\n"
+		"DENC_CONF6    : 0x%04x = 0x%02x\n"
+		"DENC_CONF8    : 0x%04x = 0x%02x\n"
+		"TVOUT_CTRL    : 0x%04x = 0x%02x\n"
+		"TVOUT_CTRL2   : 0x%04x = 0x%02x\n"
+		"IT_MASK1      : 0x%04x = 0x%02x\n"
+		"------AV connector-------------\n"
+		"GPIO_DIR5     : 0x%04x = 0x%02x\n"
+		"GPIO_OUT5     : 0x%04x = 0x%02x\n"
+		"GPIO_NPUD5    : 0x%04x = 0x%02x\n"
+		,
+		AB8500_CTRL3,            ab8500_rreg(dev, AB8500_CTRL3),
+		AB8500_SYS_ULP_CLK_CONF, ab8500_rreg(dev,
+						AB8500_SYS_ULP_CLK_CONF),
+		AB8500_SYS_CLK_CTRL,     ab8500_rreg(dev, AB8500_SYS_CLK_CTRL),
+		AB8500_REGU_MISC1,       ab8500_rreg(dev, AB8500_REGU_MISC1),
+		AB8500_VAUX12_REGU,      ab8500_rreg(dev, AB8500_VAUX12_REGU),
+		AB8500_VAUX1_SEL,        ab8500_rreg(dev, AB8500_VAUX1_SEL),
+		AB8500_DENC_CONF0,       ab8500_rreg(dev, AB8500_DENC_CONF0),
+		AB8500_DENC_CONF1,       ab8500_rreg(dev, AB8500_DENC_CONF1),
+		AB8500_DENC_CONF2,       ab8500_rreg(dev, AB8500_DENC_CONF2),
+		AB8500_DENC_CONF6,       ab8500_rreg(dev, AB8500_DENC_CONF6),
+		AB8500_DENC_CONF8,       ab8500_rreg(dev, AB8500_DENC_CONF8),
+		AB8500_TVOUT_CTRL,       ab8500_rreg(dev, AB8500_TVOUT_CTRL),
+		AB8500_TVOUT_CTRL2,      ab8500_rreg(dev, AB8500_TVOUT_CTRL2),
+		AB8500_IT_MASK1,         ab8500_rreg(dev, AB8500_IT_MASK1),
+		AB8500_GPIO_DIR5,        ab8500_rreg(dev, AB8500_GPIO_DIR5),
+		AB8500_GPIO_OUT5,        ab8500_rreg(dev, AB8500_GPIO_OUT5),
+		AB8500_GPIO_NPUD5,       ab8500_rreg(dev, AB8500_GPIO_NPUD5)
+	);
+	if (data_size >= DEBUG_BUF_SIZE) {
+		printk(KERN_EMERG "AB8500 DENC: Buffer overrun\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* check if read done */
+	if (*f_pos > data_size)
+		goto out;
+
+	if (*f_pos + count > data_size)
+		count = data_size - *f_pos;
+
+	if (copy_to_user(buf, buffer + *f_pos, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+out:
+	return ret;
+}
+#endif /* CONFIG_DEBUG_FS */
+
+/* Module init */
+static int __init ab8500_denc_init(void)
+{
+	return platform_driver_register(&ab8500_denc_driver);
+}
+module_init(ab8500_denc_init);
+
+static void __exit ab8500_denc_exit(void)
+{
+	platform_driver_unregister(&ab8500_denc_driver);
+}
+module_exit(ab8500_denc_exit);
+
+MODULE_AUTHOR("Marcel Tunnissen <marcel.tuennissen@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson AB8500 DENC driver");
diff --git a/drivers/mfd/ab8500-gpadc.c b/drivers/mfd/ab8500-gpadc.c
index f16afb234ff..4eeac415d84 100644
--- a/drivers/mfd/ab8500-gpadc.c
+++ b/drivers/mfd/ab8500-gpadc.c
@@ -130,25 +130,24 @@ static LIST_HEAD(ab8500_gpadc_list);
  * ab8500_gpadc_get() - returns a reference to the primary AB8500 GPADC
  * (i.e. the first GPADC in the instance list)
  */
-struct ab8500_gpadc *ab8500_gpadc_get(char *name)
+struct ab8500_gpadc *ab8500_gpadc_get(void)
 {
 	struct ab8500_gpadc *gpadc;
+	gpadc = list_first_entry(&ab8500_gpadc_list, struct ab8500_gpadc, node);
 
-	list_for_each_entry(gpadc, &ab8500_gpadc_list, node) {
-		if (!strcmp(name, dev_name(gpadc->dev)))
-		    return gpadc;
-	}
-
-	return ERR_PTR(-ENOENT);
+	return gpadc;
 }
 EXPORT_SYMBOL(ab8500_gpadc_get);
 
-static int ab8500_gpadc_ad_to_voltage(struct ab8500_gpadc *gpadc, u8 input,
+/**
+ * ab8500_gpadc_ad_to_voltage() - Convert a raw ADC value to a voltage
+ */
+int ab8500_gpadc_ad_to_voltage(struct ab8500_gpadc *gpadc, u8 channel,
 	int ad_value)
 {
 	int res;
 
-	switch (input) {
+	switch (channel) {
 	case MAIN_CHARGER_V:
 		/* For some reason we don't have calibrated data */
 		if (!gpadc->cal_data[ADC_INPUT_VMAIN].gain) {
@@ -235,15 +234,41 @@ static int ab8500_gpadc_ad_to_voltage(struct ab8500_gpadc *gpadc, u8 input,
 
 /**
  * ab8500_gpadc_convert() - gpadc conversion
- * @input:	analog input to be converted to digital data
+ * @channel:	analog channel to be converted to digital data
  *
  * This function converts the selected analog i/p to digital
  * data.
  */
-int ab8500_gpadc_convert(struct ab8500_gpadc *gpadc, u8 input)
+int ab8500_gpadc_convert(struct ab8500_gpadc *gpadc, u8 channel)
+{
+	int ad_value;
+	int voltage;
+
+	ad_value = ab8500_gpadc_read_raw(gpadc, channel);
+	if (ad_value < 0) {
+		dev_err(gpadc->dev, "GPADC raw value failed ch: %d\n", channel);
+		return ad_value;
+	}
+
+	voltage = ab8500_gpadc_ad_to_voltage(gpadc, channel, ad_value);
+
+	if (voltage < 0)
+		dev_err(gpadc->dev, "GPADC to voltage conversion failed ch:"
+			" %d AD: 0x%x\n", channel, ad_value);
+
+	return voltage;
+}
+
+/**
+ * ab8500_gpadc_read_raw() - gpadc read
+ * @channel:	analog channel to be read
+ *
+ * This function obtains the raw ADC value, this then needs
+ * to be converted by calling ab8500_gpadc_ad_to_voltage()
+ */
+int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel)
 {
 	int ret;
-	u16 data = 0;
 	int looplimit = 0;
 	u8 val, low_data, high_data;
 
@@ -278,9 +303,9 @@ int ab8500_gpadc_convert(struct ab8500_gpadc *gpadc, u8 input)
 		goto out;
 	}
 
-	/* Select the input source and set average samples to 16 */
+	/* Select the channel source and set average samples to 16 */
 	ret = abx500_set_register_interruptible(gpadc->dev, AB8500_GPADC,
-		AB8500_GPADC_CTRL2_REG, (input | SW_AVG_16));
+		AB8500_GPADC_CTRL2_REG, (channel | SW_AVG_16));
 	if (ret < 0) {
 		dev_err(gpadc->dev,
 			"gpadc_conversion: set avg samples failed\n");
@@ -292,7 +317,7 @@ int ab8500_gpadc_convert(struct ab8500_gpadc *gpadc, u8 input)
 	 * charging current sense if it needed, ABB 3.0 needs some special
 	 * treatment too.
 	 */
-	switch (input) {
+	switch (channel) {
 	case MAIN_CHARGER_C:
 	case USB_CHARGER_C:
 		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
@@ -359,7 +384,6 @@ int ab8500_gpadc_convert(struct ab8500_gpadc *gpadc, u8 input)
 		goto out;
 	}
 
-	data = (high_data << 8) | low_data;
 	/* Disable GPADC */
 	ret = abx500_set_register_interruptible(gpadc->dev, AB8500_GPADC,
 		AB8500_GPADC_CTRL1_REG, DIS_GPADC);
@@ -370,8 +394,8 @@ int ab8500_gpadc_convert(struct ab8500_gpadc *gpadc, u8 input)
 	/* Disable VTVout LDO this is required for GPADC */
 	regulator_disable(gpadc->regu);
 	mutex_unlock(&gpadc->ab8500_gpadc_lock);
-	ret = ab8500_gpadc_ad_to_voltage(gpadc, input, data);
-	return ret;
+
+	return (high_data << 8) | low_data;
 
 out:
 	/*
@@ -385,10 +409,9 @@ out:
 	regulator_disable(gpadc->regu);
 	mutex_unlock(&gpadc->ab8500_gpadc_lock);
 	dev_err(gpadc->dev,
-		"gpadc_conversion: Failed to AD convert channel %d\n", input);
+		"gpadc_conversion: Failed to AD convert channel %d\n", channel);
 	return ret;
 }
-EXPORT_SYMBOL(ab8500_gpadc_convert);
 
 /**
  * ab8500_bm_gpswadcconvend_handler() - isr for s/w gpadc conversion completion
diff --git a/drivers/mfd/ab8500-i2c.c b/drivers/mfd/ab8500-i2c.c
index 9be541c6b00..dda0745a471 100644
--- a/drivers/mfd/ab8500-i2c.c
+++ b/drivers/mfd/ab8500-i2c.c
@@ -11,7 +11,8 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/mfd/ab8500.h>
-#include <linux/mfd/db8500-prcmu.h>
+#include <mach/prcmu.h>
+
 
 static int ab8500_i2c_write(struct ab8500 *ab8500, u16 addr, u8 data)
 {
diff --git a/drivers/mfd/ab8500-sysctrl.c b/drivers/mfd/ab8500-sysctrl.c
index 392185965b3..e6de6a279a5 100644
--- a/drivers/mfd/ab8500-sysctrl.c
+++ b/drivers/mfd/ab8500-sysctrl.c
@@ -6,12 +6,29 @@
 
 #include <linux/err.h>
 #include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/signal.h>
 #include <linux/mfd/ab8500.h>
 #include <linux/mfd/abx500.h>
 #include <linux/mfd/ab8500/sysctrl.h>
 
 static struct device *sysctrl_dev;
 
+void ab8500_power_off(void)
+{
+	sigset_t old;
+	sigset_t all;
+
+	sigfillset(&all);
+
+	if (!sigprocmask(SIG_BLOCK, &all, &old)) {
+		(void)ab8500_sysctrl_set(AB8500_STW4500CTRL1,
+					 AB8500_STW4500CTRL1_SWOFF |
+					 AB8500_STW4500CTRL1_SWRESET4500N);
+		(void)sigprocmask(SIG_SETMASK, &old, NULL);
+	}
+}
+
 static inline bool valid_bank(u8 bank)
 {
 	return ((bank == AB8500_SYS_CTRL1_BLOCK) ||
@@ -50,7 +67,12 @@ int ab8500_sysctrl_write(u16 reg, u8 mask, u8 value)
 
 static int __devinit ab8500_sysctrl_probe(struct platform_device *pdev)
 {
+	struct ab8500_platform_data *plat;
+
 	sysctrl_dev = &pdev->dev;
+	plat = dev_get_platdata(pdev->dev.parent);
+	if (plat->pm_power_off)
+		pm_power_off = ab8500_power_off;
 	return 0;
 }
 
diff --git a/drivers/mfd/db5500-prcmu.c b/drivers/mfd/db5500-prcmu.c
index 9dbb3cab4a6..f5774493e7e 100644
--- a/drivers/mfd/db5500-prcmu.c
+++ b/drivers/mfd/db5500-prcmu.c
@@ -19,12 +19,19 @@
 #include <linux/irq.h>
 #include <linux/jiffies.h>
 #include <linux/bitops.h>
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/mfd/core.h>
+#include <linux/regulator/db5500-prcmu.h>
+#include <linux/regulator/machine.h>
 #include <linux/interrupt.h>
-#include <linux/mfd/db5500-prcmu.h>
 #include <mach/hardware.h>
 #include <mach/irqs.h>
+#include <mach/prcmu.h>
 #include <mach/db5500-regs.h>
-#include "db5500-prcmu-regs.h"
+#include <mach/prcmu-regs.h>
+
+#define PRCM_SW_RST_REASON (tcdm_base + 0xFF8) /* 2 bytes */
 
 #define _PRCM_MB_HEADER (tcdm_base + 0xFE8)
 #define PRCM_REQ_MB0_HEADER (_PRCM_MB_HEADER + 0x0)
@@ -64,6 +71,47 @@
 #define PRCM_ACK_MB6 (tcdm_base + 0xF0C)
 #define PRCM_ACK_MB7 (tcdm_base + 0xF08)
 
+/* Mailbox 0 REQs */
+#define PRCM_REQ_MB0_AP_POWER_STATE    (PRCM_REQ_MB0 + 0x0)
+#define PRCM_REQ_MB0_ULP_CLOCK_STATE   (PRCM_REQ_MB0 + 0x1)
+#define PRCM_REQ_MB0_AP_PLL_STATE      (PRCM_REQ_MB0 + 0x2)
+#define PRCM_REQ_MB0_DDR_STATE         (PRCM_REQ_MB0 + 0x3)
+#define PRCM_REQ_MB0_ESRAM0_STATE      (PRCM_REQ_MB0 + 0x4)
+#define PRCM_REQ_MB0_WAKEUP_DBB        (PRCM_REQ_MB0 + 0x8)
+#define PRCM_REQ_MB0_WAKEUP_ABB        (PRCM_REQ_MB0 + 0xC)
+
+/* Mailbox 0 ACKs */
+#define PRCM_ACK_MB0_AP_PWRSTTR_STATUS (PRCM_ACK_MB0 + 0x0)
+#define PRCM_ACK_MB0_READ_POINTER      (PRCM_ACK_MB0 + 0x1)
+#define PRCM_ACK_MB0_WAKEUP_0_DBB      (PRCM_ACK_MB0 + 0x4)
+#define PRCM_ACK_MB0_WAKEUP_0_ABB      (PRCM_ACK_MB0 + 0x8)
+#define PRCM_ACK_MB0_WAKEUP_1_DBB      (PRCM_ACK_MB0 + 0x28)
+#define PRCM_ACK_MB0_WAKEUP_1_ABB      (PRCM_ACK_MB0 + 0x2C)
+#define PRCM_ACK_MB0_EVENT_ABB_NUMBERS 20
+
+/* Request mailbox 1 fields. */
+#define PRCM_REQ_MB1_ARM_OPP			(PRCM_REQ_MB1 + 0x0)
+#define PRCM_REQ_MB1_APE_OPP			(PRCM_REQ_MB1 + 0x1)
+
+/* Mailbox 1 ACKs */
+#define PRCM_ACK_MB1_CURRENT_ARM_OPP	(PRCM_ACK_MB1 + 0x0)
+#define PRCM_ACK_MB1_CURRENT_APE_OPP	(PRCM_ACK_MB1 + 0x1)
+#define PRCM_ACK_MB1_ARM_VOLT_STATUS	(PRCM_ACK_MB1 + 0x2)
+#define PRCM_ACK_MB1_APE_VOLT_STATUS	(PRCM_ACK_MB1 + 0x3)
+
+/* Mailbox 2 REQs */
+#define PRCM_REQ_MB2_EPOD_CLIENT (PRCM_REQ_MB2 + 0x0)
+#define PRCM_REQ_MB2_EPOD_STATE  (PRCM_REQ_MB2 + 0x1)
+#define PRCM_REQ_MB2_CLK_CLIENT  (PRCM_REQ_MB2 + 0x2)
+#define PRCM_REQ_MB2_CLK_STATE   (PRCM_REQ_MB2 + 0x3)
+#define PRCM_REQ_MB2_PLL_CLIENT  (PRCM_REQ_MB2 + 0x4)
+#define PRCM_REQ_MB2_PLL_STATE   (PRCM_REQ_MB2 + 0x5)
+
+/* Mailbox 2 ACKs */
+#define PRCM_ACK_MB2_EPOD_STATUS (PRCM_ACK_MB2 + 0x2)
+#define PRCM_ACK_MB2_CLK_STATUS  (PRCM_ACK_MB2 + 0x6)
+#define PRCM_ACK_MB2_PLL_STATUS  (PRCM_ACK_MB2 + 0xA)
+
 enum mb_return_code {
 	RC_SUCCESS,
 	RC_FAIL,
@@ -71,12 +119,36 @@ enum mb_return_code {
 
 /* Mailbox 0 headers. */
 enum mb0_header {
-	/* request */
-	RMB0H_PWR_STATE_TRANS = 1,
-	RMB0H_WAKE_UP_CFG,
-	RMB0H_RD_WAKE_UP_ACK,
 	/* acknowledge */
-	AMB0H_WAKE_UP = 1,
+	MB0H_WAKE_UP = 0,
+	/* request */
+	MB0H_PWR_STATE_TRANS,
+	MB0H_WAKE_UP_CFG,
+	MB0H_RD_WAKE_UP_ACK,
+};
+
+/* Mailbox 1 headers.*/
+enum mb1_header {
+	MB1H_ARM_OPP = 1,
+	MB1H_APE_OPP,
+	MB1H_ARM_APE_OPP,
+};
+
+/* Mailbox 2 headers. */
+enum mb2_header {
+	MB2H_EPOD_REQUEST = 1,
+	MB2H_CLK_REQUEST,
+	MB2H_PLL_REQUEST,
+};
+
+/* Mailbox 3 headers. */
+enum mb3_header {
+	MB3H_REFCLK_REQUEST = 1,
+};
+
+enum sysclk_state {
+	SYSCLK_OFF,
+	SYSCLK_ON,
 };
 
 /* Mailbox 5 headers. */
@@ -85,6 +157,46 @@ enum mb5_header {
 	MB5H_I2C_READ,
 };
 
+enum db5500_arm_opp {
+	DB5500_ARM_100_OPP = 1,
+	DB5500_ARM_50_OPP,
+	DB5500_ARM_EXT_OPP,
+};
+
+enum epod_state {
+	EPOD_OFF,
+	EPOD_ON,
+};
+enum epod_onoffret_state {
+	EPOD_OOR_OFF,
+	EPOD_OOR_RET,
+	EPOD_OOR_ON,
+};
+enum db5500_prcmu_pll {
+	DB5500_PLL_SOC0,
+	DB5500_PLL_SOC1,
+	DB5500_PLL_DDR,
+	DB5500_NUM_PLL_ID,
+};
+
+enum on_off_ret {
+	OFF_ST,
+	RET_ST,
+	ON_ST,
+};
+
+enum db5500_ap_pwr_state {
+	DB5500_AP_SLEEP = 2,
+	DB5500_AP_DEEP_SLEEP,
+	DB5500_AP_IDLE,
+};
+
+/* Request mailbox 3 fields */
+#define PRCM_REQ_MB3_REFCLK_MGT		(PRCM_REQ_MB3 + 0x0)
+
+/* Ack. mailbox 3 fields */
+#define PRCM_ACK_MB3_REFCLK_REQ		(PRCM_ACK_MB3 + 0x0)
+
 /* Request mailbox 5 fields. */
 #define PRCM_REQ_MB5_I2C_SLAVE (PRCM_REQ_MB5 + 0)
 #define PRCM_REQ_MB5_I2C_REG (PRCM_REQ_MB5 + 1)
@@ -109,26 +221,173 @@ enum mb5_header {
 #define PRCMU_DSI_CLOCK_SETTING			0x00000128
 /* TVCLK_MGT PLLSW=001 (PLLSOC0) PLLDIV=0x13, = 19.05 MHZ */
 #define PRCMU_DSI_LP_CLOCK_SETTING		0x00000135
-#define PRCMU_PLLDSI_FREQ_SETTING		0x0004013C
+#define PRCMU_PLLDSI_FREQ_SETTING		0x00020121
 #define PRCMU_DSI_PLLOUT_SEL_SETTING		0x00000002
-#define PRCMU_ENABLE_ESCAPE_CLOCK_DIV		0x03000101
+#define PRCMU_ENABLE_ESCAPE_CLOCK_DIV		0x03000201
 #define PRCMU_DISABLE_ESCAPE_CLOCK_DIV		0x00000101
 
 #define PRCMU_ENABLE_PLLDSI			0x00000001
 #define PRCMU_DISABLE_PLLDSI			0x00000000
 
 #define PRCMU_DSI_RESET_SW			0x00000003
+#define PRCMU_RESOUTN0_PIN			0x00000001
+#define PRCMU_RESOUTN1_PIN			0x00000002
+#define PRCMU_RESOUTN2_PIN			0x00000004
 
 #define PRCMU_PLLDSI_LOCKP_LOCKED		0x3
 
 /*
+ * Wakeups/IRQs
+ */
+
+#define WAKEUP_BIT_RTC BIT(0)
+#define WAKEUP_BIT_RTT0 BIT(1)
+#define WAKEUP_BIT_RTT1 BIT(2)
+#define WAKEUP_BIT_CD_IRQ BIT(3)
+#define WAKEUP_BIT_SRP_TIM BIT(4)
+#define WAKEUP_BIT_APE_REQ BIT(5)
+#define WAKEUP_BIT_USB BIT(6)
+#define WAKEUP_BIT_ABB BIT(7)
+#define WAKEUP_BIT_LOW_POWER_AUDIO BIT(8)
+#define WAKEUP_BIT_TEMP_SENSOR BIT(9)
+#define WAKEUP_BIT_ARM BIT(10)
+#define WAKEUP_BIT_AC_WAKE_ACK BIT(11)
+#define WAKEUP_BIT_MODEM_SW_RESET_REQ BIT(20)
+#define WAKEUP_BIT_GPIO0 BIT(23)
+#define WAKEUP_BIT_GPIO1 BIT(24)
+#define WAKEUP_BIT_GPIO2 BIT(25)
+#define WAKEUP_BIT_GPIO3 BIT(26)
+#define WAKEUP_BIT_GPIO4 BIT(27)
+#define WAKEUP_BIT_GPIO5 BIT(28)
+#define WAKEUP_BIT_GPIO6 BIT(29)
+#define WAKEUP_BIT_GPIO7 BIT(30)
+#define WAKEUP_BIT_AC_REL_ACK BIT(30)
+
+/*
+ * This vector maps irq numbers to the bits in the bit field used in
+ * communication with the PRCMU firmware.
+ *
+ * The reason for having this is to keep the irq numbers contiguous even though
+ * the bits in the bit field are not. (The bits also have a tendency to move
+ * around, to further complicate matters.)
+ */
+#define IRQ_INDEX(_name) ((IRQ_DB5500_PRCMU_##_name) - IRQ_DB5500_PRCMU_BASE)
+#define IRQ_ENTRY(_name)[IRQ_INDEX(_name)] = (WAKEUP_BIT_##_name)
+static u32 prcmu_irq_bit[NUM_DB5500_PRCMU_WAKEUPS] = {
+	IRQ_ENTRY(RTC),
+	IRQ_ENTRY(RTT0),
+	IRQ_ENTRY(RTT1),
+	IRQ_ENTRY(CD_IRQ),
+	IRQ_ENTRY(SRP_TIM),
+	IRQ_ENTRY(APE_REQ),
+	IRQ_ENTRY(USB),
+	IRQ_ENTRY(ABB),
+	IRQ_ENTRY(LOW_POWER_AUDIO),
+	IRQ_ENTRY(TEMP_SENSOR),
+	IRQ_ENTRY(ARM),
+	IRQ_ENTRY(AC_WAKE_ACK),
+	IRQ_ENTRY(MODEM_SW_RESET_REQ),
+	IRQ_ENTRY(GPIO0),
+	IRQ_ENTRY(GPIO1),
+	IRQ_ENTRY(GPIO2),
+	IRQ_ENTRY(GPIO3),
+	IRQ_ENTRY(GPIO4),
+	IRQ_ENTRY(GPIO5),
+	IRQ_ENTRY(GPIO6),
+	IRQ_ENTRY(GPIO7),
+	IRQ_ENTRY(AC_REL_ACK),
+};
+
+#define VALID_WAKEUPS (BIT(NUM_PRCMU_WAKEUP_INDICES) - 1)
+#define WAKEUP_ENTRY(_name)[PRCMU_WAKEUP_INDEX_##_name] = (WAKEUP_BIT_##_name)
+static u32 prcmu_wakeup_bit[NUM_PRCMU_WAKEUP_INDICES] = {
+	WAKEUP_ENTRY(RTC),
+	WAKEUP_ENTRY(RTT0),
+	WAKEUP_ENTRY(RTT1),
+	WAKEUP_ENTRY(CD_IRQ),
+	WAKEUP_ENTRY(USB),
+	WAKEUP_ENTRY(ABB),
+	WAKEUP_ENTRY(ARM)
+};
+
+/*
  * mb0_transfer - state needed for mailbox 0 communication.
- * @lock:		The transaction lock.
+ * @lock                The transaction lock.
+ * @dbb_irqs_lock       lock used for (un)masking DBB wakeup interrupts
+ * @mask_work:          Work structure used for (un)masking wakeup interrupts.
+ * @req:                Request data that need to persist between requests.
  */
 static struct {
 	spinlock_t lock;
+	spinlock_t dbb_irqs_lock;
+	struct work_struct mask_work;
+	struct {
+		u32 dbb_irqs;
+		u32 dbb_wakeups;
+		u32 abb_events;
+	} req;
 } mb0_transfer;
 
+
+/*
+ * mb1_transfer - state needed for mailbox 1 communication.
+ * @lock:	The transaction lock.
+ * @work:	The transaction completion structure.
+ * @req_arm_opp Requested arm opp
+ * @req_ape_opp Requested ape opp
+ * @ack:	Reply ("acknowledge") data.
+ */
+static struct {
+	struct mutex lock;
+	struct completion work;
+	u8 req_arm_opp;
+	u8 req_ape_opp;
+	struct {
+		u8 header;
+		u8 arm_opp;
+		u8 ape_opp;
+		u8 arm_voltage_st;
+		u8 ape_voltage_st;
+	} ack;
+} mb1_transfer;
+
+/*
+ * mb2_transfer - state needed for mailbox 2 communication.
+ * @lock:      The transaction lock.
+ * @work:      The transaction completion structure.
+ * @req:       Request data that need to persist between requests.
+ * @ack:       Reply ("acknowledge") data.
+ */
+static struct {
+	struct mutex lock;
+	struct completion work;
+	struct {
+		u8 epod_st[DB5500_NUM_EPOD_ID];
+		u8 pll_st[DB5500_NUM_PLL_ID];
+	} req;
+	struct {
+		u8 header;
+		u8 status;
+	} ack;
+} mb2_transfer;
+
+/*
+ * mb3_transfer - state needed for mailbox 3 communication.
+ * @sysclk_lock:	A lock used to handle concurrent sysclk requests.
+ * @sysclk_work:	Work structure used for sysclk requests.
+ * @req_st:		Requested clock state.
+ * @ack:		Acknowledgement data
+ */
+static struct {
+	struct mutex sysclk_lock;
+	struct completion sysclk_work;
+	enum sysclk_state req_st;
+	struct {
+		u8 header;
+		u8 status;
+	} ack;
+} mb3_transfer;
+
 /*
  * mb5_transfer - state needed for mailbox 5 communication.
  * @lock:	The transaction lock.
@@ -148,6 +407,337 @@ static struct {
 /* PRCMU TCDM base IO address. */
 static __iomem void *tcdm_base;
 
+struct clk_mgt {
+	unsigned int offset;
+	u32 pllsw;
+};
+
+static DEFINE_SPINLOCK(clk_mgt_lock);
+
+#define CLK_MGT_ENTRY(_name)[PRCMU_##_name] = {	\
+	(PRCM_##_name##_MGT_OFF), 0			\
+}
+static struct clk_mgt clk_mgt[PRCMU_NUM_REG_CLOCKS] = {
+	CLK_MGT_ENTRY(SGACLK),
+	CLK_MGT_ENTRY(UARTCLK),
+	CLK_MGT_ENTRY(MSP02CLK),
+	CLK_MGT_ENTRY(I2CCLK),
+	CLK_MGT_ENTRY(SDMMCCLK),
+	CLK_MGT_ENTRY(PER1CLK),
+	CLK_MGT_ENTRY(PER2CLK),
+	CLK_MGT_ENTRY(PER3CLK),
+	CLK_MGT_ENTRY(PER5CLK),
+	CLK_MGT_ENTRY(PER6CLK),
+	CLK_MGT_ENTRY(PWMCLK),
+	CLK_MGT_ENTRY(IRDACLK),
+	CLK_MGT_ENTRY(IRRCCLK),
+	CLK_MGT_ENTRY(HDMICLK),
+	CLK_MGT_ENTRY(APEATCLK),
+	CLK_MGT_ENTRY(APETRACECLK),
+	CLK_MGT_ENTRY(MCDECLK),
+	CLK_MGT_ENTRY(DSIALTCLK),
+	CLK_MGT_ENTRY(DMACLK),
+	CLK_MGT_ENTRY(B2R2CLK),
+	CLK_MGT_ENTRY(TVCLK),
+	CLK_MGT_ENTRY(RNGCLK),
+	CLK_MGT_ENTRY(SIACLK),
+	CLK_MGT_ENTRY(SVACLK),
+};
+
+bool db5500_prcmu_is_ac_wake_requested(void)
+{
+	return false;
+}
+
+static int request_sysclk(bool enable)
+{
+	int r;
+
+	r = 0;
+	mutex_lock(&mb3_transfer.sysclk_lock);
+
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(3))
+		cpu_relax();
+
+	if (enable)
+		mb3_transfer.req_st = SYSCLK_ON;
+	else
+		mb3_transfer.req_st = SYSCLK_OFF;
+
+	writeb(mb3_transfer.req_st, (PRCM_REQ_MB3_REFCLK_MGT));
+
+	writeb(MB3H_REFCLK_REQUEST, (PRCM_REQ_MB3_HEADER));
+	writel(MBOX_BIT(3), PRCM_MBOX_CPU_SET);
+
+	/*
+	 * The firmware only sends an ACK if we want to enable the
+	 * SysClk, and it succeeds.
+	 */
+	if (!wait_for_completion_timeout(&mb3_transfer.sysclk_work,
+			msecs_to_jiffies(20000))) {
+		pr_err("prcmu: %s timed out (20 s) waiting for a reply.\n",
+			__func__);
+		r = -EIO;
+		WARN(1, "Failed to set sysclk");
+		goto unlock_and_return;
+	}
+
+	if ((mb3_transfer.ack.header != MB3H_REFCLK_REQUEST) ||
+			(mb3_transfer.ack.status != mb3_transfer.req_st)) {
+		r = -EIO;
+	}
+
+unlock_and_return:
+	mutex_unlock(&mb3_transfer.sysclk_lock);
+
+	return r;
+}
+
+static int request_timclk(bool enable)
+{
+	u32 val = (PRCM_TCR_DOZE_MODE | PRCM_TCR_TENSEL_MASK);
+
+	if (!enable)
+		val |= PRCM_TCR_STOP_TIMERS;
+	writel(val, PRCM_TCR);
+
+	return 0;
+}
+
+static int request_reg_clock(u8 clock, bool enable)
+{
+	u32 val;
+	unsigned long flags;
+
+	WARN_ON(!clk_mgt[clock].offset);
+
+	spin_lock_irqsave(&clk_mgt_lock, flags);
+
+	/* Grab the HW semaphore. */
+	while ((readl(PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
+		cpu_relax();
+
+	val = readl(_PRCMU_BASE + clk_mgt[clock].offset);
+	if (enable) {
+		val |= (PRCM_CLK_MGT_CLKEN | clk_mgt[clock].pllsw);
+	} else {
+		clk_mgt[clock].pllsw = (val & PRCM_CLK_MGT_CLKPLLSW_MASK);
+		val &= ~(PRCM_CLK_MGT_CLKEN | PRCM_CLK_MGT_CLKPLLSW_MASK);
+	}
+	writel(val, (_PRCMU_BASE + clk_mgt[clock].offset));
+
+	/* Release the HW semaphore. */
+	writel(0, PRCM_SEM);
+
+	spin_unlock_irqrestore(&clk_mgt_lock, flags);
+
+	return 0;
+}
+
+/*
+ * request_pll() - Request for a pll to be enabled or disabled.
+ * @pll:        The pll for which the request is made.
+ * @enable:     Whether the clock should be enabled (true) or disabled (false).
+ *
+ * This function should only be used by the clock implementation.
+ * Do not use it from any other place!
+ */
+static int request_pll(u8 pll, bool enable)
+{
+	int r = 0;
+
+	BUG_ON(pll >= DB5500_NUM_PLL_ID);
+	mutex_lock(&mb2_transfer.lock);
+
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(2))
+		cpu_relax();
+
+	mb2_transfer.req.pll_st[pll] = enable;
+
+	/* fill in mailbox */
+	writeb(pll, PRCM_REQ_MB2_PLL_CLIENT);
+	writeb(mb2_transfer.req.pll_st[pll], PRCM_REQ_MB2_PLL_STATE);
+
+	writeb(MB2H_PLL_REQUEST, PRCM_REQ_MB2_HEADER);
+
+	writel(MBOX_BIT(2), PRCM_MBOX_CPU_SET);
+	if (!wait_for_completion_timeout(&mb2_transfer.work,
+		msecs_to_jiffies(500))) {
+		pr_err("prcmu: set_pll() failed.\n"
+			"prcmu: Please check your firmware version.\n");
+		r = -EIO;
+		WARN(1, "Failed to set pll");
+		goto unlock_and_return;
+	}
+	if (mb2_transfer.ack.status != RC_SUCCESS ||
+		mb2_transfer.ack.header != MB2H_PLL_REQUEST)
+		r = -EIO;
+
+unlock_and_return:
+	mutex_unlock(&mb2_transfer.lock);
+
+	return r;
+}
+
+/**
+ * db5500_prcmu_request_clock() - Request for a clock to be enabled or disabled.
+ * @clock:      The clock for which the request is made.
+ * @enable:     Whether the clock should be enabled (true) or disabled (false).
+ *
+ * This function should only be used by the clock implementation.
+ * Do not use it from any other place!
+ */
+int db5500_prcmu_request_clock(u8 clock, bool enable)
+{
+	if (clock < PRCMU_NUM_REG_CLOCKS)
+		return request_reg_clock(clock, enable);
+	else if (clock == PRCMU_TIMCLK)
+		return request_timclk(enable);
+	else if (clock == PRCMU_PLLSOC0)
+		return request_pll(DB5500_PLL_SOC0, enable);
+	else if (clock == PRCMU_PLLSOC1)
+		return request_pll(DB5500_PLL_SOC1, enable);
+	else if (clock == PRCMU_PLLDDR)
+		return request_pll(DB5500_PLL_DDR, enable);
+	else if (clock == PRCMU_SYSCLK)
+		return request_sysclk(enable);
+	else
+		return -EINVAL;
+}
+
+/* This function should only be called while mb0_transfer.lock is held. */
+static void config_wakeups(void)
+{
+	static u32 last_dbb_events;
+	static u32 last_abb_events;
+	u32 dbb_events;
+	u32 abb_events;
+
+	dbb_events = mb0_transfer.req.dbb_irqs | mb0_transfer.req.dbb_wakeups;
+
+	abb_events = mb0_transfer.req.abb_events;
+
+	if ((dbb_events == last_dbb_events) && (abb_events == last_abb_events))
+		return;
+
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
+		cpu_relax();
+
+	writel(dbb_events, PRCM_REQ_MB0_WAKEUP_DBB);
+	writel(abb_events, PRCM_REQ_MB0_WAKEUP_ABB);
+	writeb(MB0H_WAKE_UP_CFG, PRCM_REQ_MB0_HEADER);
+	writel(MBOX_BIT(0), PRCM_MBOX_CPU_SET);
+
+	last_dbb_events = dbb_events;
+	last_abb_events = abb_events;
+}
+
+int db5500_prcmu_config_esram0_deep_sleep(u8 state)
+{
+	unsigned long flags;
+
+	if ((state > ESRAM0_DEEP_SLEEP_STATE_RET) ||
+	    (state < ESRAM0_DEEP_SLEEP_STATE_OFF))
+		return -EINVAL;
+
+	spin_lock_irqsave(&mb0_transfer.lock, flags);
+
+	if (state == ESRAM0_DEEP_SLEEP_STATE_RET)
+		writeb(RET_ST, PRCM_REQ_MB0_ESRAM0_STATE);
+	else
+		writeb(OFF_ST, PRCM_REQ_MB0_ESRAM0_STATE);
+
+	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
+
+	return 0;
+}
+
+int db5500_prcmu_set_power_state(u8 state, bool keep_ulp_clk, bool keep_ap_pll)
+{
+	int r = 0;
+	unsigned long flags;
+
+	/* Deep Idle is not supported in DB5500 */
+	BUG_ON((state < PRCMU_AP_SLEEP) || (state >= PRCMU_AP_DEEP_IDLE));
+
+	spin_lock_irqsave(&mb0_transfer.lock, flags);
+
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
+		cpu_relax();
+
+	switch (state) {
+	case PRCMU_AP_IDLE:
+		writeb(DB5500_AP_IDLE, PRCM_REQ_MB0_AP_POWER_STATE);
+		/* TODO: Can be high latency */
+		writeb(DDR_PWR_STATE_UNCHANGED, PRCM_REQ_MB0_DDR_STATE);
+		break;
+	case PRCMU_AP_SLEEP:
+		writeb(DB5500_AP_SLEEP, PRCM_REQ_MB0_AP_POWER_STATE);
+		break;
+	case PRCMU_AP_DEEP_SLEEP:
+		writeb(DB5500_AP_DEEP_SLEEP, PRCM_REQ_MB0_AP_POWER_STATE);
+		break;
+	default:
+		r = -EINVAL;
+		goto unlock_return;
+	}
+	writeb((keep_ap_pll ? 1 : 0), PRCM_REQ_MB0_AP_PLL_STATE);
+	writeb((keep_ulp_clk ? 1 : 0), PRCM_REQ_MB0_ULP_CLOCK_STATE);
+
+	writeb(MB0H_PWR_STATE_TRANS, PRCM_REQ_MB0_HEADER);
+	writel(MBOX_BIT(0), PRCM_MBOX_CPU_SET);
+
+unlock_return:
+	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
+
+	return r;
+}
+
+void db5500_prcmu_enable_wakeups(u32 wakeups)
+{
+	unsigned long flags;
+	u32 bits;
+	int i;
+
+	BUG_ON(wakeups != (wakeups & VALID_WAKEUPS));
+
+	for (i = 0, bits = 0; i < NUM_PRCMU_WAKEUP_INDICES; i++) {
+		if (wakeups & BIT(i)) {
+			if (prcmu_wakeup_bit[i] == 0)
+				WARN(1, "WAKEUP NOT SUPPORTED");
+			else
+				bits |= prcmu_wakeup_bit[i];
+		}
+	}
+
+	spin_lock_irqsave(&mb0_transfer.lock, flags);
+
+	mb0_transfer.req.dbb_wakeups = bits;
+	config_wakeups();
+
+	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
+}
+
+void db5500_prcmu_config_abb_event_readout(u32 abb_events)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mb0_transfer.lock, flags);
+
+	mb0_transfer.req.abb_events = abb_events;
+	config_wakeups();
+
+	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
+}
+
+void db5500_prcmu_get_abb_event_buffer(void __iomem **buf)
+{
+	if (readb(PRCM_ACK_MB0_READ_POINTER) & 1)
+		*buf = (PRCM_ACK_MB0_WAKEUP_1_ABB);
+	else
+		*buf = (PRCM_ACK_MB0_WAKEUP_0_ABB);
+}
+
 /**
  * db5500_prcmu_abb_read() - Read register value(s) from the ABB.
  * @slave:	The I2C slave address.
@@ -230,6 +820,100 @@ int db5500_prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size)
 	return r;
 }
 
+/**
+ * db5500_prcmu_set_arm_opp - set the appropriate ARM OPP
+ * @opp: The new ARM operating point to which transition is to be made
+ * Returns: 0 on success, non-zero on failure
+ *
+ * This function sets the the operating point of the ARM.
+ */
+int db5500_prcmu_set_arm_opp(u8 opp)
+{
+	int r;
+	u8 db5500_opp;
+
+	r = 0;
+
+	switch (opp) {
+	case ARM_EXTCLK:
+		db5500_opp = DB5500_ARM_EXT_OPP;
+		break;
+	case ARM_50_OPP:
+		db5500_opp = DB5500_ARM_50_OPP;
+		break;
+	case ARM_100_OPP:
+		db5500_opp = DB5500_ARM_100_OPP;
+		break;
+	default:
+		pr_err("prcmu: %s() received wrong opp value: %d\n",
+				__func__, opp);
+		r = -EINVAL;
+		goto bailout;
+	}
+
+	mutex_lock(&mb1_transfer.lock);
+
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
+		cpu_relax();
+
+	writeb(MB1H_ARM_OPP, PRCM_REQ_MB1_HEADER);
+
+	writeb(db5500_opp, PRCM_REQ_MB1_ARM_OPP);
+	writel(MBOX_BIT(1), PRCM_MBOX_CPU_SET);
+
+	if (!wait_for_completion_timeout(&mb1_transfer.work,
+		msecs_to_jiffies(500))) {
+		r = -EIO;
+		WARN(1, "prcmu: failed to set arm opp");
+		goto unlock_and_return;
+	}
+
+	if (mb1_transfer.ack.header != MB1H_ARM_OPP ||
+		(mb1_transfer.ack.arm_opp != db5500_opp) ||
+		(mb1_transfer.ack.arm_voltage_st != RC_SUCCESS))
+		r = -EIO;
+
+unlock_and_return:
+	mutex_unlock(&mb1_transfer.lock);
+bailout:
+	return r;
+}
+
+/**
+ * db5500_prcmu_get_arm_opp - get the current ARM OPP
+ *
+ * Returns: the current ARM OPP
+ */
+int db5500_prcmu_get_arm_opp(void)
+{
+	return readb(PRCM_ACK_MB1_CURRENT_ARM_OPP);
+}
+
+int prcmu_resetout(u8 resoutn, u8 state)
+{
+	int pin = -1;
+
+	switch (resoutn) {
+	case 0:
+		pin = PRCMU_RESOUTN0_PIN;
+		break;
+	case 1:
+		pin = PRCMU_RESOUTN1_PIN;
+		break;
+	case 2:
+		pin = PRCMU_RESOUTN2_PIN;
+	default:
+		break;
+	}
+
+	if (pin > 0)
+		writel(pin, state > 0 ? PRCM_RESOUTN_SET : PRCM_RESOUTN_CLR);
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
 int db5500_prcmu_enable_dsipll(void)
 {
 	int i;
@@ -279,6 +963,29 @@ int db5500_prcmu_set_display_clocks(void)
 	return 0;
 }
 
+/**
+ * db5500_prcmu_system_reset - System reset
+ *
+ * Saves the reset reason code and then sets the APE_SOFTRST register which
+ * fires an interrupt to fw
+ */
+void db5500_prcmu_system_reset(u16 reset_code)
+{
+	writew(reset_code, PRCM_SW_RST_REASON);
+	writel(1, PRCM_APE_SOFTRST);
+}
+
+/**
+ * db5500_prcmu_get_reset_code - Retrieve SW reset reason code
+ *
+ * Retrieves the reset reason code stored by prcmu_system_reset() before
+ * last restart.
+ */
+u16 db5500_prcmu_get_reset_code(void)
+{
+	return readw(PRCM_SW_RST_REASON);
+}
+
 static void ack_dbb_wakeup(void)
 {
 	unsigned long flags;
@@ -288,12 +995,94 @@ static void ack_dbb_wakeup(void)
 	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
 		cpu_relax();
 
-	writeb(RMB0H_RD_WAKE_UP_ACK, PRCM_REQ_MB0_HEADER);
+	writeb(MB0H_RD_WAKE_UP_ACK, PRCM_REQ_MB0_HEADER);
 	writel(MBOX_BIT(0), PRCM_MBOX_CPU_SET);
 
 	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
 }
 
+int db5500_prcmu_set_epod(u16 epod, u8 epod_state)
+{
+	int r = 0;
+	bool ram_retention = false;
+
+	/* check argument */
+	BUG_ON(epod < DB5500_EPOD_ID_BASE);
+	BUG_ON(epod_state > EPOD_STATE_ON);
+	BUG_ON((epod - DB5500_EPOD_ID_BASE) >= DB5500_NUM_EPOD_ID);
+
+	if (epod == DB5500_EPOD_ID_ESRAM12)
+		ram_retention = true;
+
+	/* check argument */
+	BUG_ON(epod_state == EPOD_STATE_RAMRET && !ram_retention);
+
+	/* get lock */
+	mutex_lock(&mb2_transfer.lock);
+
+	/* wait for mailbox */
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(2))
+		cpu_relax();
+
+	/* Retention is allowed only for ESRAM12 */
+	if (epod  == DB5500_EPOD_ID_ESRAM12) {
+		switch (epod_state) {
+		case EPOD_STATE_ON:
+			mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE] =
+				EPOD_OOR_ON;
+			break;
+		case EPOD_STATE_OFF:
+			mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE] =
+				EPOD_OOR_OFF;
+			break;
+		case EPOD_STATE_RAMRET:
+			mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE] =
+				EPOD_OOR_RET;
+			break;
+		default:
+			r = -EINVAL;
+			goto unlock_and_return;
+			break;
+		}
+	} else {
+		if (epod_state == EPOD_STATE_ON)
+			mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE] =
+				EPOD_ON;
+		else if (epod_state == EPOD_STATE_OFF)
+			mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE] =
+				EPOD_OFF;
+		else {
+			r = -EINVAL;
+			goto unlock_and_return;
+		}
+	}
+	/* fill in mailbox */
+	writeb((epod - DB5500_EPOD_ID_BASE), PRCM_REQ_MB2_EPOD_CLIENT);
+	writeb(mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE],
+		PRCM_REQ_MB2_EPOD_STATE);
+
+	writeb(MB2H_EPOD_REQUEST, PRCM_REQ_MB2_HEADER);
+
+	writel(MBOX_BIT(2), PRCM_MBOX_CPU_SET);
+
+	if (!wait_for_completion_timeout(&mb2_transfer.work,
+		msecs_to_jiffies(500))) {
+		pr_err("prcmu: set_epod() failed.\n"
+			"prcmu: Please check your firmware version.\n");
+		r = -EIO;
+		WARN(1, "Failed to set epod");
+		goto unlock_and_return;
+	}
+
+	if (mb2_transfer.ack.status != RC_SUCCESS ||
+		mb2_transfer.ack.header != MB2H_EPOD_REQUEST)
+		r = -EIO;
+
+unlock_and_return:
+	mutex_unlock(&mb2_transfer.lock);
+	return r;
+}
+
 static inline void print_unknown_header_warning(u8 n, u8 header)
 {
 	pr_warning("prcmu: Unknown message header (%d) in mailbox %d.\n",
@@ -303,11 +1092,25 @@ static inline void print_unknown_header_warning(u8 n, u8 header)
 static bool read_mailbox_0(void)
 {
 	bool r;
+	u32 ev;
+	unsigned int n;
+
 	u8 header;
 
 	header = readb(PRCM_ACK_MB0_HEADER);
 	switch (header) {
-	case AMB0H_WAKE_UP:
+	case MB0H_WAKE_UP:
+		if (readb(PRCM_ACK_MB0_READ_POINTER) & 1)
+			ev = readl(PRCM_ACK_MB0_WAKEUP_1_DBB);
+		else
+			ev = readl(PRCM_ACK_MB0_WAKEUP_0_DBB);
+
+		ev &= mb0_transfer.req.dbb_irqs;
+
+		for (n = 0; n < NUM_DB5500_PRCMU_WAKEUPS; n++) {
+			if (ev & prcmu_irq_bit[n])
+				generic_handle_irq(IRQ_DB5500_PRCMU_BASE + n);
+		}
 		r = true;
 		break;
 	default:
@@ -315,31 +1118,93 @@ static bool read_mailbox_0(void)
 		r = false;
 		break;
 	}
-	writel(MBOX_BIT(0), PRCM_ARM_IT1_CLEAR);
+	writel(MBOX_BIT(0), PRCM_ARM_IT1_CLR);
 	return r;
 }
 
 static bool read_mailbox_1(void)
 {
-	writel(MBOX_BIT(1), PRCM_ARM_IT1_CLEAR);
+	u8 header;
+	bool do_complete = true;
+
+	header = mb1_transfer.ack.header = readb(PRCM_ACK_MB1_HEADER);
+
+	switch (header) {
+	case MB1H_ARM_OPP:
+		mb1_transfer.ack.arm_opp = readb(PRCM_ACK_MB1_CURRENT_ARM_OPP);
+		mb1_transfer.ack.arm_voltage_st =
+			readb(PRCM_ACK_MB1_ARM_VOLT_STATUS);
+		break;
+	case MB1H_ARM_APE_OPP:
+		mb1_transfer.ack.ape_opp = readb(PRCM_ACK_MB1_CURRENT_APE_OPP);
+		mb1_transfer.ack.ape_voltage_st =
+			readb(PRCM_ACK_MB1_APE_VOLT_STATUS);
+		break;
+	default:
+		print_unknown_header_warning(1, header);
+		do_complete = false;
+		break;
+	}
+
+	writel(MBOX_BIT(1), PRCM_ARM_IT1_CLR);
+
+	if (do_complete)
+		complete(&mb1_transfer.work);
+
 	return false;
 }
 
 static bool read_mailbox_2(void)
 {
-	writel(MBOX_BIT(2), PRCM_ARM_IT1_CLEAR);
+	u8 header;
+
+	header = readb(PRCM_ACK_MB2_HEADER);
+	mb2_transfer.ack.header = header;
+	switch (header) {
+	case MB2H_EPOD_REQUEST:
+		mb2_transfer.ack.status = readb(PRCM_ACK_MB2_EPOD_STATUS);
+		break;
+	case MB2H_CLK_REQUEST:
+		mb2_transfer.ack.status = readb(PRCM_ACK_MB2_CLK_STATUS);
+		break;
+	case MB2H_PLL_REQUEST:
+		mb2_transfer.ack.status = readb(PRCM_ACK_MB2_PLL_STATUS);
+		break;
+	default:
+		writel(MBOX_BIT(2), PRCM_ARM_IT1_CLR);
+		pr_err("prcmu: Wrong ACK received for MB2 request \n");
+		return false;
+		break;
+	}
+	writel(MBOX_BIT(2), PRCM_ARM_IT1_CLR);
+	complete(&mb2_transfer.work);
 	return false;
 }
 
 static bool read_mailbox_3(void)
 {
-	writel(MBOX_BIT(3), PRCM_ARM_IT1_CLEAR);
+	u8 header;
+
+	header = readb(PRCM_ACK_MB3_HEADER);
+	mb3_transfer.ack.header = header;
+	switch (header) {
+	case MB3H_REFCLK_REQUEST:
+		mb3_transfer.ack.status = readb(PRCM_ACK_MB3_REFCLK_REQ);
+		writel(MBOX_BIT(3), PRCM_ARM_IT1_CLR);
+		complete(&mb3_transfer.sysclk_work);
+		break;
+	default:
+		writel(MBOX_BIT(3), PRCM_ARM_IT1_CLR);
+		pr_err("prcmu: wrong MB3 header\n");
+		break;
+	}
+
 	return false;
 }
 
 static bool read_mailbox_4(void)
 {
-	writel(MBOX_BIT(4), PRCM_ARM_IT1_CLEAR);
+	writel(MBOX_BIT(4), PRCM_ARM_IT1_CLR);
 	return false;
 }
 
@@ -360,19 +1225,19 @@ static bool read_mailbox_5(void)
 		print_unknown_header_warning(5, header);
 		break;
 	}
-	writel(MBOX_BIT(5), PRCM_ARM_IT1_CLEAR);
+	writel(MBOX_BIT(5), PRCM_ARM_IT1_CLR);
 	return false;
 }
 
 static bool read_mailbox_6(void)
 {
-	writel(MBOX_BIT(6), PRCM_ARM_IT1_CLEAR);
+	writel(MBOX_BIT(6), PRCM_ARM_IT1_CLR);
 	return false;
 }
 
 static bool read_mailbox_7(void)
 {
-	writel(MBOX_BIT(7), PRCM_ARM_IT1_CLEAR);
+	writel(MBOX_BIT(7), PRCM_ARM_IT1_CLR);
 	return false;
 }
 
@@ -414,35 +1279,210 @@ static irqreturn_t prcmu_irq_thread_fn(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+static void prcmu_mask_work(struct work_struct *work)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mb0_transfer.lock, flags);
+
+	config_wakeups();
+
+	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
+}
+
+static void prcmu_irq_mask(struct irq_data *d)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mb0_transfer.dbb_irqs_lock, flags);
+
+	mb0_transfer.req.dbb_irqs &= ~prcmu_irq_bit[d->irq - IRQ_DB5500_PRCMU_BASE];
+
+	spin_unlock_irqrestore(&mb0_transfer.dbb_irqs_lock, flags);
+	schedule_work(&mb0_transfer.mask_work);
+}
+
+static void prcmu_irq_unmask(struct irq_data *d)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mb0_transfer.dbb_irqs_lock, flags);
+
+	mb0_transfer.req.dbb_irqs |= prcmu_irq_bit[d->irq - IRQ_DB5500_PRCMU_BASE];
+
+	spin_unlock_irqrestore(&mb0_transfer.dbb_irqs_lock, flags);
+	schedule_work(&mb0_transfer.mask_work);
+}
+
+static void noop(struct irq_data *d)
+{
+}
+
+static struct irq_chip prcmu_irq_chip = {
+	.name           = "prcmu",
+	.irq_disable    = prcmu_irq_mask,
+	.irq_ack        = noop,
+	.irq_mask       = prcmu_irq_mask,
+	.irq_unmask     = prcmu_irq_unmask,
+};
+
 void __init db5500_prcmu_early_init(void)
 {
+	unsigned int i;
+
 	tcdm_base = __io_address(U5500_PRCMU_TCDM_BASE);
 	spin_lock_init(&mb0_transfer.lock);
+	spin_lock_init(&mb0_transfer.dbb_irqs_lock);
+	mutex_init(&mb1_transfer.lock);
+	init_completion(&mb1_transfer.work);
+	mutex_init(&mb2_transfer.lock);
+	init_completion(&mb2_transfer.work);
+	mutex_init(&mb3_transfer.sysclk_lock);
+	init_completion(&mb3_transfer.sysclk_work);
 	mutex_init(&mb5_transfer.lock);
 	init_completion(&mb5_transfer.work);
+
+	INIT_WORK(&mb0_transfer.mask_work, prcmu_mask_work);
+
+	/* Initalize irqs. */
+	for (i = 0; i < NUM_DB5500_PRCMU_WAKEUPS; i++) {
+		unsigned int irq;
+
+		irq = IRQ_DB5500_PRCMU_BASE + i;
+		irq_set_chip_and_handler(irq, &prcmu_irq_chip,
+					 handle_simple_irq);
+		set_irq_flags(irq, IRQF_VALID);
+	}
 }
 
+/*
+ * Power domain switches (ePODs) modeled as regulators for the DB5500 SoC
+ */
+static struct regulator_consumer_supply db5500_vape_consumers[] = {
+	REGULATOR_SUPPLY("v-ape", NULL),
+	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.0"),
+	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.1"),
+	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.2"),
+	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.3"),
+	REGULATOR_SUPPLY("v-mmc", "sdi0"),
+	REGULATOR_SUPPLY("v-mmc", "sdi1"),
+	REGULATOR_SUPPLY("v-mmc", "sdi2"),
+	REGULATOR_SUPPLY("v-mmc", "sdi3"),
+	REGULATOR_SUPPLY("v-mmc", "sdi4"),
+	REGULATOR_SUPPLY("v-uart", "uart0"),
+	REGULATOR_SUPPLY("v-uart", "uart1"),
+	REGULATOR_SUPPLY("v-uart", "uart2"),
+	REGULATOR_SUPPLY("v-uart", "uart3"),
+};
+
+static struct regulator_consumer_supply db5500_sga_consumers[] = {
+	REGULATOR_SUPPLY("debug", "reg-virt-consumer.0"),
+	REGULATOR_SUPPLY("v-mali", NULL),
+};
+
+static struct regulator_consumer_supply db5500_hva_consumers[] = {
+	REGULATOR_SUPPLY("debug", "reg-virt-consumer.1"),
+	REGULATOR_SUPPLY("v-hva", NULL),
+};
+
+static struct regulator_consumer_supply db5500_sia_consumers[] = {
+	REGULATOR_SUPPLY("debug", "reg-virt-consumer.2"),
+	REGULATOR_SUPPLY("v-sia", "mmio_camera"),
+};
+
+static struct regulator_consumer_supply db5500_disp_consumers[] = {
+	REGULATOR_SUPPLY("debug", "reg-virt-consumer.3"),
+	REGULATOR_SUPPLY("vsupply", "b2r2_bus"),
+	REGULATOR_SUPPLY("vsupply", "mcde"),
+};
+
+static struct regulator_consumer_supply db5500_esram12_consumers[] = {
+	REGULATOR_SUPPLY("debug", "reg-virt-consumer.4"),
+	REGULATOR_SUPPLY("v-esram12", "mcde"),
+	REGULATOR_SUPPLY("esram12", "cm_control"),
+};
+
+#define DB5500_REGULATOR_SWITCH(lower, upper)                           \
+[DB5500_REGULATOR_SWITCH_##upper] = {                                   \
+	.constraints = {                                                \
+		.valid_ops_mask = REGULATOR_CHANGE_STATUS,              \
+	},                                                              \
+	.consumer_supplies      = db5500_##lower##_consumers,            \
+	.num_consumer_supplies  = ARRAY_SIZE(db5500_##lower##_consumers),\
+}
+
+static struct regulator_init_data db5500_regulators[DB5500_NUM_REGULATORS] = {
+	[DB5500_REGULATOR_VAPE] = {
+		.constraints = {
+			.valid_ops_mask	= REGULATOR_CHANGE_STATUS,
+		},
+		.consumer_supplies	= db5500_vape_consumers,
+		.num_consumer_supplies	= ARRAY_SIZE(db5500_vape_consumers),
+	},
+	DB5500_REGULATOR_SWITCH(sga, SGA),
+	DB5500_REGULATOR_SWITCH(hva, HVA),
+	DB5500_REGULATOR_SWITCH(sia, SIA),
+	DB5500_REGULATOR_SWITCH(disp, DISP),
+	DB5500_REGULATOR_SWITCH(esram12, ESRAM12),
+};
+
+static struct mfd_cell db5500_prcmu_devs[] = {
+	{
+		.name = "db5500-prcmu-regulators",
+		.platform_data = &db5500_regulators,
+		.pdata_size = sizeof(db5500_regulators),
+	},
+	{
+		.name = "cpufreq-u5500",
+	},
+};
+
 /**
  * prcmu_fw_init - arch init call for the Linux PRCMU fw init logic
  *
  */
-int __init db5500_prcmu_init(void)
+static int __init db5500_prcmu_probe(struct platform_device *pdev)
 {
-	int r = 0;
+	int err = 0;
 
 	if (ux500_is_svp() || !cpu_is_u5500())
 		return -ENODEV;
 
 	/* Clean up the mailbox interrupts after pre-kernel code. */
-	writel(ALL_MBOX_BITS, PRCM_ARM_IT1_CLEAR);
+	writel(ALL_MBOX_BITS, PRCM_ARM_IT1_CLR);
 
-	r = request_threaded_irq(IRQ_DB5500_PRCMU1, prcmu_irq_handler,
-		prcmu_irq_thread_fn, 0, "prcmu", NULL);
-	if (r < 0) {
+	err = request_threaded_irq(IRQ_DB5500_PRCMU1, prcmu_irq_handler,
+		prcmu_irq_thread_fn, IRQF_NO_SUSPEND, "prcmu", NULL);
+	if (err < 0) {
 		pr_err("prcmu: Failed to allocate IRQ_DB5500_PRCMU1.\n");
-		return -EBUSY;
+		err = -EBUSY;
+		goto no_irq_return;
 	}
-	return 0;
+
+	err = mfd_add_devices(&pdev->dev, 0, db5500_prcmu_devs,
+			      ARRAY_SIZE(db5500_prcmu_devs), NULL,
+			      0);
+
+	if (err)
+		pr_err("prcmu: Failed to add subdevices\n");
+	else
+		pr_info("DB5500 PRCMU initialized\n");
+
+no_irq_return:
+	return err;
+
+}
+
+static struct platform_driver db5500_prcmu_driver = {
+	.driver = {
+		.name = "db5500-prcmu",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init db5500_prcmu_init(void)
+{
+	return platform_driver_probe(&db5500_prcmu_driver, db5500_prcmu_probe);
 }
 
 arch_initcall(db5500_prcmu_init);
diff --git a/drivers/mfd/db8500-prcmu-regs.h b/drivers/mfd/db8500-prcmu-regs.h
deleted file mode 100644
index 3bbf04d5804..00000000000
--- a/drivers/mfd/db8500-prcmu-regs.h
+++ /dev/null
@@ -1,166 +0,0 @@
-/*
- * Copyright (C) STMicroelectronics 2009
- * Copyright (C) ST-Ericsson SA 2010
- *
- * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com>
- * Author: Sundar Iyer <sundar.iyer@stericsson.com>
- *
- * License Terms: GNU General Public License v2
- *
- * PRCM Unit registers
- */
-#ifndef __DB8500_PRCMU_REGS_H
-#define __DB8500_PRCMU_REGS_H
-
-#include <linux/bitops.h>
-#include <mach/hardware.h>
-
-#define BITS(_start, _end) ((BIT(_end) - BIT(_start)) + BIT(_end))
-
-#define PRCM_ARM_PLLDIVPS 0x118
-#define PRCM_ARM_PLLDIVPS_ARM_BRM_RATE	BITS(0, 5)
-#define PRCM_ARM_PLLDIVPS_MAX_MASK	0xF
-
-#define PRCM_PLLARM_LOCKP 0x0A8
-#define PRCM_PLLARM_LOCKP_PRCM_PLLARM_LOCKP3 BIT(1)
-
-#define PRCM_ARM_CHGCLKREQ 0x114
-#define PRCM_ARM_CHGCLKREQ_PRCM_ARM_CHGCLKREQ BIT(0)
-
-#define PRCM_PLLARM_ENABLE 0x98
-#define PRCM_PLLARM_ENABLE_PRCM_PLLARM_ENABLE	BIT(0)
-#define PRCM_PLLARM_ENABLE_PRCM_PLLARM_COUNTON	BIT(8)
-
-#define PRCM_ARMCLKFIX_MGT	0x0
-#define PRCM_A9_RESETN_CLR	0x1f4
-#define PRCM_A9_RESETN_SET	0x1f0
-#define PRCM_ARM_LS_CLAMP	0x30C
-#define PRCM_SRAM_A9		0x308
-
-/* ARM WFI Standby signal register */
-#define PRCM_ARM_WFI_STANDBY	0x130
-#define PRCM_IOCR		0x310
-#define PRCM_IOCR_IOFORCE BIT(0)
-
-/* CPU mailbox registers */
-#define PRCM_MBOX_CPU_VAL 0x0FC
-#define PRCM_MBOX_CPU_SET 0x100
-
-/* Dual A9 core interrupt management unit registers */
-#define PRCM_A9_MASK_REQ 0x328
-#define PRCM_A9_MASK_REQ_PRCM_A9_MASK_REQ BIT(0)
-
-#define PRCM_A9_MASK_ACK	0x32C
-#define PRCM_ARMITMSK31TO0	0x11C
-#define PRCM_ARMITMSK63TO32	0x120
-#define PRCM_ARMITMSK95TO64	0x124
-#define PRCM_ARMITMSK127TO96	0x128
-#define PRCM_POWER_STATE_VAL	0x25C
-#define PRCM_ARMITVAL31TO0	0x260
-#define PRCM_ARMITVAL63TO32	0x264
-#define PRCM_ARMITVAL95TO64	0x268
-#define PRCM_ARMITVAL127TO96	0x26C
-
-#define PRCM_HOSTACCESS_REQ 0x334
-#define PRCM_HOSTACCESS_REQ_HOSTACCESS_REQ BIT(0)
-
-#define PRCM_ARM_IT1_CLR 0x48C
-#define PRCM_ARM_IT1_VAL 0x494
-
-#define PRCM_ITSTATUS0		0x148
-#define PRCM_ITSTATUS1		0x150
-#define PRCM_ITSTATUS2		0x158
-#define PRCM_ITSTATUS3		0x160
-#define PRCM_ITSTATUS4		0x168
-#define PRCM_ITSTATUS5		0x484
-#define PRCM_ITCLEAR5		0x488
-#define PRCM_ARMIT_MASKXP70_IT	0x1018
-
-/* System reset register */
-#define PRCM_APE_SOFTRST 0x228
-
-/* Level shifter and clamp control registers */
-#define PRCM_MMIP_LS_CLAMP_SET 0x420
-#define PRCM_MMIP_LS_CLAMP_CLR 0x424
-
-/* PRCMU HW semaphore */
-#define PRCM_SEM 0x400
-#define PRCM_SEM_PRCM_SEM BIT(0)
-
-/* PRCMU clock/PLL/reset registers */
-#define PRCM_PLLDSI_FREQ	0x500
-#define PRCM_PLLDSI_ENABLE	0x504
-#define PRCM_PLLDSI_LOCKP	0x508
-#define PRCM_DSI_PLLOUT_SEL	0x530
-#define PRCM_DSITVCLK_DIV	0x52C
-#define PRCM_APE_RESETN_SET	0x1E4
-#define PRCM_APE_RESETN_CLR	0x1E8
-
-#define PRCM_TCR		0x1C8
-#define PRCM_TCR_TENSEL_MASK	BITS(0, 7)
-#define PRCM_TCR_STOP_TIMERS	BIT(16)
-#define PRCM_TCR_DOZE_MODE	BIT(17)
-
-#define PRCM_CLKOCR			0x1CC
-#define PRCM_CLKOCR_CLKODIV0_SHIFT	0
-#define PRCM_CLKOCR_CLKODIV0_MASK	BITS(0, 5)
-#define PRCM_CLKOCR_CLKOSEL0_SHIFT	6
-#define PRCM_CLKOCR_CLKOSEL0_MASK	BITS(6, 8)
-#define PRCM_CLKOCR_CLKODIV1_SHIFT	16
-#define PRCM_CLKOCR_CLKODIV1_MASK	BITS(16, 21)
-#define PRCM_CLKOCR_CLKOSEL1_SHIFT	22
-#define PRCM_CLKOCR_CLKOSEL1_MASK	BITS(22, 24)
-#define PRCM_CLKOCR_CLK1TYPE		BIT(28)
-
-#define PRCM_SGACLK_MGT		0x014
-#define PRCM_UARTCLK_MGT	0x018
-#define PRCM_MSP02CLK_MGT	0x01C
-#define PRCM_MSP1CLK_MGT	0x288
-#define PRCM_I2CCLK_MGT		0x020
-#define PRCM_SDMMCCLK_MGT	0x024
-#define PRCM_SLIMCLK_MGT	0x028
-#define PRCM_PER1CLK_MGT	0x02C
-#define PRCM_PER2CLK_MGT	0x030
-#define PRCM_PER3CLK_MGT	0x034
-#define PRCM_PER5CLK_MGT	0x038
-#define PRCM_PER6CLK_MGT	0x03C
-#define PRCM_PER7CLK_MGT	0x040
-#define PRCM_LCDCLK_MGT		0x044
-#define PRCM_BMLCLK_MGT		0x04C
-#define PRCM_HSITXCLK_MGT	0x050
-#define PRCM_HSIRXCLK_MGT	0x054
-#define PRCM_HDMICLK_MGT	0x058
-#define PRCM_APEATCLK_MGT	0x05C
-#define PRCM_APETRACECLK_MGT	0x060
-#define PRCM_MCDECLK_MGT	0x064
-#define PRCM_IPI2CCLK_MGT	0x068
-#define PRCM_DSIALTCLK_MGT	0x06C
-#define PRCM_DMACLK_MGT		0x074
-#define PRCM_B2R2CLK_MGT	0x078
-#define PRCM_TVCLK_MGT		0x07C
-#define PRCM_UNIPROCLK_MGT	0x278
-#define PRCM_SSPCLK_MGT		0x280
-#define PRCM_RNGCLK_MGT		0x284
-#define PRCM_UICCCLK_MGT	0x27C
-
-#define PRCM_CLK_MGT_CLKPLLDIV_MASK	BITS(0, 4)
-#define PRCM_CLK_MGT_CLKPLLSW_MASK	BITS(5, 7)
-#define PRCM_CLK_MGT_CLKEN		BIT(8)
-
-/* ePOD and memory power signal control registers */
-#define PRCM_EPOD_C_SET		0x410
-#define PRCM_SRAM_LS_SLEEP	0x304
-
-/* Debug power control unit registers */
-#define PRCM_POWER_STATE_SET 0x254
-
-/* Miscellaneous unit registers */
-#define PRCM_DSI_SW_RESET 0x324
-#define PRCM_GPIOCR		0x138
-
-/* GPIOCR register */
-#define PRCM_GPIOCR_SPI2_SELECT BIT(23)
-
-#define PRCM_DDR_SUBSYS_APE_MINBW  0x438
-
-#endif /* __DB8500_PRCMU_REGS_H */
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index 02a15d7cb3b..50c2c608803 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -27,14 +27,13 @@
 #include <linux/platform_device.h>
 #include <linux/uaccess.h>
 #include <linux/mfd/core.h>
-#include <linux/mfd/db8500-prcmu.h>
 #include <linux/regulator/db8500-prcmu.h>
 #include <linux/regulator/machine.h>
 #include <mach/hardware.h>
 #include <mach/irqs.h>
-#include <mach/db8500-regs.h>
+#include <mach/prcmu-regs.h>
 #include <mach/id.h>
-#include "db8500-prcmu-regs.h"
+#include <mach/prcmu.h>
 
 /* Offset for the firmware version within the TCPM */
 #define PRCMU_FW_VERSION_OFFSET 0xA4
@@ -131,12 +130,14 @@
 #define MB1H_REQUEST_APE_OPP_100_VOLT 0x3
 #define MB1H_RELEASE_APE_OPP_100_VOLT 0x4
 #define MB1H_RELEASE_USB_WAKEUP 0x5
+#define MB1H_PLL_ON_OFF 0x6
 
 /* Mailbox 1 Requests */
 #define PRCM_REQ_MB1_ARM_OPP			(PRCM_REQ_MB1 + 0x0)
 #define PRCM_REQ_MB1_APE_OPP			(PRCM_REQ_MB1 + 0x1)
-#define PRCM_REQ_MB1_APE_OPP_100_RESTORE	(PRCM_REQ_MB1 + 0x4)
-#define PRCM_REQ_MB1_ARM_OPP_100_RESTORE	(PRCM_REQ_MB1 + 0x8)
+#define PRCM_REQ_MB1_PLL_ON_OFF			(PRCM_REQ_MB1 + 0x4)
+#define PLL_SOC1_OFF	0x4
+#define PLL_SOC1_ON	0x8
 
 /* Mailbox 1 ACKs */
 #define PRCM_ACK_MB1_CURRENT_ARM_OPP	(PRCM_ACK_MB1 + 0x0)
@@ -184,6 +185,11 @@
 #define MB4H_HOTDOG	0x12
 #define MB4H_HOTMON	0x13
 #define MB4H_HOT_PERIOD	0x14
+#define MB4H_A9WDOG_CONF 0x16
+#define MB4H_A9WDOG_EN   0x17
+#define MB4H_A9WDOG_DIS  0x18
+#define MB4H_A9WDOG_LOAD 0x19
+#define MB4H_A9WDOG_KICK 0x20
 
 /* Mailbox 4 Requests */
 #define PRCM_REQ_MB4_DDR_ST_AP_SLEEP_IDLE	(PRCM_REQ_MB4 + 0x0)
@@ -196,6 +202,13 @@
 #define PRCM_REQ_MB4_HOT_PERIOD			(PRCM_REQ_MB4 + 0x0)
 #define HOTMON_CONFIG_LOW			BIT(0)
 #define HOTMON_CONFIG_HIGH			BIT(1)
+#define PRCM_REQ_MB4_A9WDOG_0			(PRCM_REQ_MB4 + 0x0)
+#define PRCM_REQ_MB4_A9WDOG_1			(PRCM_REQ_MB4 + 0x1)
+#define PRCM_REQ_MB4_A9WDOG_2			(PRCM_REQ_MB4 + 0x2)
+#define PRCM_REQ_MB4_A9WDOG_3			(PRCM_REQ_MB4 + 0x3)
+#define A9WDOG_AUTO_OFF_EN			BIT(7)
+#define A9WDOG_AUTO_OFF_DIS			0
+#define A9WDOG_ID_MASK				0xf
 
 /* Mailbox 5 Requests */
 #define PRCM_REQ_MB5_I2C_SLAVE_OP	(PRCM_REQ_MB5 + 0x0)
@@ -327,11 +340,13 @@ static struct {
  * mb1_transfer - state needed for mailbox 1 communication.
  * @lock:	The transaction lock.
  * @work:	The transaction completion structure.
+ * @ape_opp:	The current APE OPP.
  * @ack:	Reply ("acknowledge") data.
  */
 static struct {
 	struct mutex lock;
 	struct completion work;
+	u8 ape_opp;
 	struct {
 		u8 header;
 		u8 arm_opp;
@@ -412,7 +427,7 @@ struct clk_mgt {
 
 static DEFINE_SPINLOCK(clk_mgt_lock);
 
-#define CLK_MGT_ENTRY(_name)[PRCMU_##_name] = { (PRCM_##_name##_MGT), 0 }
+#define CLK_MGT_ENTRY(_name)[PRCMU_##_name] = { (PRCM_##_name##_MGT_OFF), 0 }
 struct clk_mgt clk_mgt[PRCMU_NUM_REG_CLOCKS] = {
 	CLK_MGT_ENTRY(SGACLK),
 	CLK_MGT_ENTRY(UARTCLK),
@@ -446,6 +461,39 @@ struct clk_mgt clk_mgt[PRCMU_NUM_REG_CLOCKS] = {
 };
 
 /*
+ * NOTE! Temporary until all users of set_hwacc() are using the regulator
+ * framework API
+ */
+static struct regulator *hwacc_regulator[NUM_HW_ACC];
+static struct regulator *hwacc_ret_regulator[NUM_HW_ACC];
+
+static bool hwacc_enabled[NUM_HW_ACC];
+static bool hwacc_ret_enabled[NUM_HW_ACC];
+
+static const char *hwacc_regulator_name[NUM_HW_ACC] = {
+	[HW_ACC_SVAMMDSP]	= "hwacc-sva-mmdsp",
+	[HW_ACC_SVAPIPE]	= "hwacc-sva-pipe",
+	[HW_ACC_SIAMMDSP]	= "hwacc-sia-mmdsp",
+	[HW_ACC_SIAPIPE]	= "hwacc-sia-pipe",
+	[HW_ACC_SGA]		= "hwacc-sga",
+	[HW_ACC_B2R2]		= "hwacc-b2r2",
+	[HW_ACC_MCDE]		= "hwacc-mcde",
+	[HW_ACC_ESRAM1]		= "hwacc-esram1",
+	[HW_ACC_ESRAM2]		= "hwacc-esram2",
+	[HW_ACC_ESRAM3]		= "hwacc-esram3",
+	[HW_ACC_ESRAM4]		= "hwacc-esram4",
+};
+
+static const char *hwacc_ret_regulator_name[NUM_HW_ACC] = {
+	[HW_ACC_SVAMMDSP]	= "hwacc-sva-mmdsp-ret",
+	[HW_ACC_SIAMMDSP]	= "hwacc-sia-mmdsp-ret",
+	[HW_ACC_ESRAM1]		= "hwacc-esram1-ret",
+	[HW_ACC_ESRAM2]		= "hwacc-esram2-ret",
+	[HW_ACC_ESRAM3]		= "hwacc-esram3-ret",
+	[HW_ACC_ESRAM4]		= "hwacc-esram4-ret",
+};
+
+/*
 * Used by MCDE to setup all necessary PRCMU registers
 */
 #define PRCMU_RESET_DSIPLL		0x00004000
@@ -493,55 +541,51 @@ static struct {
 } prcmu_version;
 
 
-int prcmu_enable_dsipll(void)
+int db8500_prcmu_enable_dsipll(void)
 {
 	int i;
 	unsigned int plldsifreq;
 
 	/* Clear DSIPLL_RESETN */
-	writel(PRCMU_RESET_DSIPLL, (_PRCMU_BASE + PRCM_APE_RESETN_CLR));
+	writel(PRCMU_RESET_DSIPLL, PRCM_APE_RESETN_CLR);
 	/* Unclamp DSIPLL in/out */
-	writel(PRCMU_UNCLAMP_DSIPLL, (_PRCMU_BASE + PRCM_MMIP_LS_CLAMP_CLR));
+	writel(PRCMU_UNCLAMP_DSIPLL, PRCM_MMIP_LS_CLAMP_CLR);
 
 	if (prcmu_is_u8400())
 		plldsifreq = PRCMU_PLLDSI_FREQ_SETTING_U8400;
 	else
 		plldsifreq = PRCMU_PLLDSI_FREQ_SETTING;
 	/* Set DSI PLL FREQ */
-	writel(plldsifreq, (_PRCMU_BASE + PRCM_PLLDSI_FREQ));
-	writel(PRCMU_DSI_PLLOUT_SEL_SETTING,
-		(_PRCMU_BASE + PRCM_DSI_PLLOUT_SEL));
+	writel(plldsifreq, PRCM_PLLDSI_FREQ);
+	writel(PRCMU_DSI_PLLOUT_SEL_SETTING, PRCM_DSI_PLLOUT_SEL);
 	/* Enable Escape clocks */
-	writel(PRCMU_ENABLE_ESCAPE_CLOCK_DIV,
-					(_PRCMU_BASE + PRCM_DSITVCLK_DIV));
+	writel(PRCMU_ENABLE_ESCAPE_CLOCK_DIV, PRCM_DSITVCLK_DIV);
 
 	/* Start DSI PLL */
-	writel(PRCMU_ENABLE_PLLDSI, (_PRCMU_BASE + PRCM_PLLDSI_ENABLE));
+	writel(PRCMU_ENABLE_PLLDSI, PRCM_PLLDSI_ENABLE);
 	/* Reset DSI PLL */
-	writel(PRCMU_DSI_RESET_SW, (_PRCMU_BASE + PRCM_DSI_SW_RESET));
+	writel(PRCMU_DSI_RESET_SW, PRCM_DSI_SW_RESET);
 	for (i = 0; i < 10; i++) {
-		if ((readl(_PRCMU_BASE + PRCM_PLLDSI_LOCKP) &
-			PRCMU_PLLDSI_LOCKP_LOCKED)
+		if ((readl(PRCM_PLLDSI_LOCKP) & PRCMU_PLLDSI_LOCKP_LOCKED)
 					== PRCMU_PLLDSI_LOCKP_LOCKED)
 			break;
 		udelay(100);
 	}
 	/* Set DSIPLL_RESETN */
-	writel(PRCMU_RESET_DSIPLL, (_PRCMU_BASE + PRCM_APE_RESETN_SET));
+	writel(PRCMU_RESET_DSIPLL, PRCM_APE_RESETN_SET);
 	return 0;
 }
 
-int prcmu_disable_dsipll(void)
+int db8500_prcmu_disable_dsipll(void)
 {
 	/* Disable dsi pll */
-	writel(PRCMU_DISABLE_PLLDSI, (_PRCMU_BASE + PRCM_PLLDSI_ENABLE));
+	writel(PRCMU_DISABLE_PLLDSI, PRCM_PLLDSI_ENABLE);
 	/* Disable  escapeclock */
-	writel(PRCMU_DISABLE_ESCAPE_CLOCK_DIV,
-					(_PRCMU_BASE + PRCM_DSITVCLK_DIV));
+	writel(PRCMU_DISABLE_ESCAPE_CLOCK_DIV, PRCM_DSITVCLK_DIV);
 	return 0;
 }
 
-int prcmu_set_display_clocks(void)
+int db8500_prcmu_set_display_clocks(void)
 {
 	unsigned long flags;
 	unsigned int dsiclk;
@@ -554,15 +598,15 @@ int prcmu_set_display_clocks(void)
 	spin_lock_irqsave(&clk_mgt_lock, flags);
 
 	/* Grab the HW semaphore. */
-	while ((readl(_PRCMU_BASE + PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
+	while ((readl(PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
 		cpu_relax();
 
-	writel(dsiclk, (_PRCMU_BASE + PRCM_HDMICLK_MGT));
-	writel(PRCMU_DSI_LP_CLOCK_SETTING, (_PRCMU_BASE + PRCM_TVCLK_MGT));
-	writel(PRCMU_DPI_CLOCK_SETTING, (_PRCMU_BASE + PRCM_LCDCLK_MGT));
+	writel(dsiclk, PRCM_HDMICLK_MGT);
+	writel(PRCMU_DSI_LP_CLOCK_SETTING, PRCM_TVCLK_MGT);
+	writel(PRCMU_DPI_CLOCK_SETTING, PRCM_LCDCLK_MGT);
 
 	/* Release the HW semaphore. */
-	writel(0, (_PRCMU_BASE + PRCM_SEM));
+	writel(0, PRCM_SEM);
 
 	spin_unlock_irqrestore(&clk_mgt_lock, flags);
 
@@ -578,8 +622,8 @@ void prcmu_enable_spi2(void)
 	unsigned long flags;
 
 	spin_lock_irqsave(&gpiocr_lock, flags);
-	reg = readl(_PRCMU_BASE + PRCM_GPIOCR);
-	writel(reg | PRCM_GPIOCR_SPI2_SELECT, _PRCMU_BASE + PRCM_GPIOCR);
+	reg = readl(PRCM_GPIOCR);
+	writel(reg | PRCM_GPIOCR_SPI2_SELECT, PRCM_GPIOCR);
 	spin_unlock_irqrestore(&gpiocr_lock, flags);
 }
 
@@ -592,8 +636,8 @@ void prcmu_disable_spi2(void)
 	unsigned long flags;
 
 	spin_lock_irqsave(&gpiocr_lock, flags);
-	reg = readl(_PRCMU_BASE + PRCM_GPIOCR);
-	writel(reg & ~PRCM_GPIOCR_SPI2_SELECT, _PRCMU_BASE + PRCM_GPIOCR);
+	reg = readl(PRCM_GPIOCR);
+	writel(reg & ~PRCM_GPIOCR_SPI2_SELECT, PRCM_GPIOCR);
 	spin_unlock_irqrestore(&gpiocr_lock, flags);
 }
 
@@ -701,7 +745,7 @@ int prcmu_config_clkout(u8 clkout, u8 source, u8 div)
 
 	spin_lock_irqsave(&clkout_lock, flags);
 
-	val = readl(_PRCMU_BASE + PRCM_CLKOCR);
+	val = readl(PRCM_CLKOCR);
 	if (val & div_mask) {
 		if (div) {
 			if ((val & mask) != bits) {
@@ -715,7 +759,7 @@ int prcmu_config_clkout(u8 clkout, u8 source, u8 div)
 			}
 		}
 	}
-	writel((bits | (val & ~mask)), (_PRCMU_BASE + PRCM_CLKOCR));
+	writel((bits | (val & ~mask)), PRCM_CLKOCR);
 	requests[clkout] += (div ? 1 : -1);
 
 unlock_and_return:
@@ -724,7 +768,7 @@ unlock_and_return:
 	return r;
 }
 
-int prcmu_set_power_state(u8 state, bool keep_ulp_clk, bool keep_ap_pll)
+int db8500_prcmu_set_power_state(u8 state, bool keep_ulp_clk, bool keep_ap_pll)
 {
 	unsigned long flags;
 
@@ -732,7 +776,7 @@ int prcmu_set_power_state(u8 state, bool keep_ulp_clk, bool keep_ap_pll)
 
 	spin_lock_irqsave(&mb0_transfer.lock, flags);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
 		cpu_relax();
 
 	writeb(MB0H_POWER_STATE_TRANS, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB0));
@@ -741,7 +785,7 @@ int prcmu_set_power_state(u8 state, bool keep_ulp_clk, bool keep_ap_pll)
 	writeb((keep_ulp_clk ? 1 : 0),
 		(tcdm_base + PRCM_REQ_MB0_ULP_CLOCK_STATE));
 	writeb(0, (tcdm_base + PRCM_REQ_MB0_DO_NOT_WFI));
-	writel(MBOX_BIT(0), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(0), PRCM_MBOX_CPU_SET);
 
 	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
 
@@ -770,18 +814,18 @@ static void config_wakeups(void)
 		return;
 
 	for (i = 0; i < 2; i++) {
-		while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
+		while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
 			cpu_relax();
 		writel(dbb_events, (tcdm_base + PRCM_REQ_MB0_WAKEUP_8500));
 		writel(abb_events, (tcdm_base + PRCM_REQ_MB0_WAKEUP_4500));
 		writeb(header[i], (tcdm_base + PRCM_MBOX_HEADER_REQ_MB0));
-		writel(MBOX_BIT(0), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+		writel(MBOX_BIT(0), PRCM_MBOX_CPU_SET);
 	}
 	last_dbb_events = dbb_events;
 	last_abb_events = abb_events;
 }
 
-void prcmu_enable_wakeups(u32 wakeups)
+void db8500_prcmu_enable_wakeups(u32 wakeups)
 {
 	unsigned long flags;
 	u32 bits;
@@ -802,7 +846,7 @@ void prcmu_enable_wakeups(u32 wakeups)
 	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
 }
 
-void prcmu_config_abb_event_readout(u32 abb_events)
+void db8500_prcmu_config_abb_event_readout(u32 abb_events)
 {
 	unsigned long flags;
 
@@ -814,7 +858,7 @@ void prcmu_config_abb_event_readout(u32 abb_events)
 	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
 }
 
-void prcmu_get_abb_event_buffer(void __iomem **buf)
+void db8500_prcmu_get_abb_event_buffer(void __iomem **buf)
 {
 	if (readb(tcdm_base + PRCM_ACK_MB0_READ_POINTER) & 1)
 		*buf = (tcdm_base + PRCM_ACK_MB0_WAKEUP_1_4500);
@@ -823,13 +867,13 @@ void prcmu_get_abb_event_buffer(void __iomem **buf)
 }
 
 /**
- * prcmu_set_arm_opp - set the appropriate ARM OPP
+ * db8500_prcmu_set_arm_opp - set the appropriate ARM OPP
  * @opp: The new ARM operating point to which transition is to be made
  * Returns: 0 on success, non-zero on failure
  *
  * This function sets the the operating point of the ARM.
  */
-int prcmu_set_arm_opp(u8 opp)
+int db8500_prcmu_set_arm_opp(u8 opp)
 {
 	int r;
 
@@ -840,14 +884,14 @@ int prcmu_set_arm_opp(u8 opp)
 
 	mutex_lock(&mb1_transfer.lock);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
 		cpu_relax();
 
 	writeb(MB1H_ARM_APE_OPP, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB1));
 	writeb(opp, (tcdm_base + PRCM_REQ_MB1_ARM_OPP));
 	writeb(APE_NO_CHANGE, (tcdm_base + PRCM_REQ_MB1_APE_OPP));
 
-	writel(MBOX_BIT(1), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(1), PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb1_transfer.work);
 
 	if ((mb1_transfer.ack.header != MB1H_ARM_APE_OPP) ||
@@ -860,11 +904,11 @@ int prcmu_set_arm_opp(u8 opp)
 }
 
 /**
- * prcmu_get_arm_opp - get the current ARM OPP
+ * db8500_prcmu_get_arm_opp - get the current ARM OPP
  *
  * Returns: the current ARM OPP
  */
-int prcmu_get_arm_opp(void)
+int db8500_prcmu_get_arm_opp(void)
 {
 	return readb(tcdm_base + PRCM_ACK_MB1_CURRENT_ARM_OPP);
 }
@@ -876,7 +920,7 @@ int prcmu_get_arm_opp(void)
  */
 int prcmu_get_ddr_opp(void)
 {
-	return readb(_PRCMU_BASE + PRCM_DDR_SUBSYS_APE_MINBW);
+	return readb(PRCM_DDR_SUBSYS_APE_MINBW);
 }
 
 /**
@@ -892,10 +936,57 @@ int prcmu_set_ddr_opp(u8 opp)
 		return -EINVAL;
 	/* Changing the DDR OPP can hang the hardware pre-v21 */
 	if (cpu_is_u8500v20_or_later() && !cpu_is_u8500v20())
-		writeb(opp, (_PRCMU_BASE + PRCM_DDR_SUBSYS_APE_MINBW));
+		writeb(opp, PRCM_DDR_SUBSYS_APE_MINBW);
 
 	return 0;
 }
+
+/* Divide the frequency of certain clocks by 2 for APE_50_PARTLY_25_OPP. */
+static void request_even_slower_clocks(bool enable)
+{
+	const u8 clock_reg_offset[] = {
+		PRCM_ACLK_MGT_OFF,
+		PRCM_DMACLK_MGT_OFF
+	};
+	unsigned long flags;
+	unsigned int i;
+
+	spin_lock_irqsave(&clk_mgt_lock, flags);
+
+	/* Grab the HW semaphore. */
+	while ((readl(PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
+		cpu_relax();
+
+	for (i = 0; i < ARRAY_SIZE(clock_reg_offset); i++) {
+		u32 val;
+		u32 div;
+
+		val = readl(_PRCMU_BASE + clock_reg_offset[i]);
+		div = (val & PRCM_CLK_MGT_CLKPLLDIV_MASK);
+		if (enable) {
+			if ((div <= 1) || (div > 15)) {
+				pr_err("prcmu: Bad clock divider %d in %s\n",
+					div, __func__);
+				goto unlock_and_return;
+			}
+			div <<= 1;
+		} else {
+			if (div <= 2)
+				goto unlock_and_return;
+			div >>= 1;
+		}
+		val = ((val & ~PRCM_CLK_MGT_CLKPLLDIV_MASK) |
+			(div & PRCM_CLK_MGT_CLKPLLDIV_MASK));
+		writel(val, (_PRCMU_BASE + clock_reg_offset[i]));
+	}
+
+unlock_and_return:
+	/* Release the HW semaphore. */
+	writel(0, PRCM_SEM);
+
+	spin_unlock_irqrestore(&clk_mgt_lock, flags);
+}
+
 /**
  * set_ape_opp - set the appropriate APE OPP
  * @opp: The new APE operating point to which transition is to be made
@@ -907,22 +998,39 @@ int prcmu_set_ape_opp(u8 opp)
 {
 	int r = 0;
 
+	if (opp == mb1_transfer.ape_opp)
+		return 0;
+
 	mutex_lock(&mb1_transfer.lock);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
+	if (mb1_transfer.ape_opp == APE_50_PARTLY_25_OPP)
+		request_even_slower_clocks(false);
+
+	if ((opp != APE_100_OPP) && (mb1_transfer.ape_opp != APE_100_OPP))
+		goto skip_message;
+
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
 		cpu_relax();
 
 	writeb(MB1H_ARM_APE_OPP, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB1));
 	writeb(ARM_NO_CHANGE, (tcdm_base + PRCM_REQ_MB1_ARM_OPP));
-	writeb(opp, (tcdm_base + PRCM_REQ_MB1_APE_OPP));
+	writeb(((opp == APE_50_PARTLY_25_OPP) ? APE_50_OPP : opp),
+		(tcdm_base + PRCM_REQ_MB1_APE_OPP));
 
-	writel(MBOX_BIT(1), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(1), PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb1_transfer.work);
 
 	if ((mb1_transfer.ack.header != MB1H_ARM_APE_OPP) ||
 		(mb1_transfer.ack.ape_opp != opp))
 		r = -EIO;
 
+skip_message:
+	if ((!r && (opp == APE_50_PARTLY_25_OPP)) ||
+		(r && (mb1_transfer.ape_opp == APE_50_PARTLY_25_OPP)))
+		request_even_slower_clocks(true);
+	if (!r)
+		mb1_transfer.ape_opp = opp;
+
 	mutex_unlock(&mb1_transfer.lock);
 
 	return r;
@@ -966,12 +1074,12 @@ int prcmu_request_ape_opp_100_voltage(bool enable)
 		header = MB1H_RELEASE_APE_OPP_100_VOLT;
 	}
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
 		cpu_relax();
 
 	writeb(header, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB1));
 
-	writel(MBOX_BIT(1), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(1), PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb1_transfer.work);
 
 	if ((mb1_transfer.ack.header != header) ||
@@ -995,13 +1103,13 @@ int prcmu_release_usb_wakeup_state(void)
 
 	mutex_lock(&mb1_transfer.lock);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
 		cpu_relax();
 
 	writeb(MB1H_RELEASE_USB_WAKEUP,
 		(tcdm_base + PRCM_MBOX_HEADER_REQ_MB1));
 
-	writel(MBOX_BIT(1), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(1), PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb1_transfer.work);
 
 	if ((mb1_transfer.ack.header != MB1H_RELEASE_USB_WAKEUP) ||
@@ -1013,15 +1121,169 @@ int prcmu_release_usb_wakeup_state(void)
 	return r;
 }
 
+static int request_pll(u8 clock, bool enable)
+{
+	int r = 0;
+
+	if (clock == PRCMU_PLLSOC1)
+		clock = (enable ? PLL_SOC1_ON : PLL_SOC1_OFF);
+	else
+		return -EINVAL;
+
+	mutex_lock(&mb1_transfer.lock);
+
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
+		cpu_relax();
+
+	writeb(MB1H_PLL_ON_OFF, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB1));
+	writeb(clock, (tcdm_base + PRCM_REQ_MB1_PLL_ON_OFF));
+
+	writel(MBOX_BIT(1), PRCM_MBOX_CPU_SET);
+	wait_for_completion(&mb1_transfer.work);
+
+	if (mb1_transfer.ack.header != MB1H_PLL_ON_OFF)
+		r = -EIO;
+
+	mutex_unlock(&mb1_transfer.lock);
+
+	return r;
+}
+
+/**
+ * prcmu_set_hwacc - set the power state of a h/w accelerator
+ * @hwacc_dev: The hardware accelerator (enum hw_acc_dev).
+ * @state: The new power state (enum hw_acc_state).
+ *
+ * This function sets the power state of a hardware accelerator.
+ * This function should not be called from interrupt context.
+ *
+ * NOTE! Deprecated, to be removed when all users switched over to use the
+ * regulator framework API.
+ */
+int prcmu_set_hwacc(u16 hwacc_dev, u8 state)
+{
+	int r = 0;
+	bool ram_retention = false;
+	bool enable, enable_ret;
+
+	/* check argument */
+	BUG_ON(hwacc_dev >= NUM_HW_ACC);
+
+	/* get state of switches */
+	enable = hwacc_enabled[hwacc_dev];
+	enable_ret = hwacc_ret_enabled[hwacc_dev];
+
+	/* set flag if retention is possible */
+	switch (hwacc_dev) {
+	case HW_ACC_SVAMMDSP:
+	case HW_ACC_SIAMMDSP:
+	case HW_ACC_ESRAM1:
+	case HW_ACC_ESRAM2:
+	case HW_ACC_ESRAM3:
+	case HW_ACC_ESRAM4:
+		ram_retention = true;
+		break;
+	}
+
+	/* check argument */
+	BUG_ON(state > HW_ON);
+	BUG_ON(state == HW_OFF_RAMRET && !ram_retention);
+
+	/* modify enable flags */
+	switch (state) {
+	case HW_OFF:
+		enable_ret = false;
+		enable = false;
+		break;
+	case HW_ON:
+		enable = true;
+		break;
+	case HW_OFF_RAMRET:
+		enable_ret = true;
+		enable = false;
+		break;
+	}
+
+	/* get regulator (lazy) */
+	if (hwacc_regulator[hwacc_dev] == NULL) {
+		hwacc_regulator[hwacc_dev] = regulator_get(NULL,
+			hwacc_regulator_name[hwacc_dev]);
+		if (IS_ERR(hwacc_regulator[hwacc_dev])) {
+			pr_err("prcmu: failed to get supply %s\n",
+				hwacc_regulator_name[hwacc_dev]);
+			r = PTR_ERR(hwacc_regulator[hwacc_dev]);
+			goto out;
+		}
+	}
+
+	if (ram_retention) {
+		if (hwacc_ret_regulator[hwacc_dev] == NULL) {
+			hwacc_ret_regulator[hwacc_dev] = regulator_get(NULL,
+				hwacc_ret_regulator_name[hwacc_dev]);
+			if (IS_ERR(hwacc_ret_regulator[hwacc_dev])) {
+				pr_err("prcmu: failed to get supply %s\n",
+					hwacc_ret_regulator_name[hwacc_dev]);
+				r = PTR_ERR(hwacc_ret_regulator[hwacc_dev]);
+				goto out;
+			}
+		}
+	}
+
+	/* set regulators */
+	if (ram_retention) {
+		if (enable_ret && !hwacc_ret_enabled[hwacc_dev]) {
+			r = regulator_enable(hwacc_ret_regulator[hwacc_dev]);
+			if (r < 0) {
+				pr_err("prcmu_set_hwacc: ret enable failed\n");
+				goto out;
+			}
+			hwacc_ret_enabled[hwacc_dev] = true;
+		}
+	}
+
+	if (enable && !hwacc_enabled[hwacc_dev]) {
+		r = regulator_enable(hwacc_regulator[hwacc_dev]);
+		if (r < 0) {
+			pr_err("prcmu_set_hwacc: enable failed\n");
+			goto out;
+		}
+		hwacc_enabled[hwacc_dev] = true;
+	}
+
+	if (!enable && hwacc_enabled[hwacc_dev]) {
+		r = regulator_disable(hwacc_regulator[hwacc_dev]);
+		if (r < 0) {
+			pr_err("prcmu_set_hwacc: disable failed\n");
+			goto out;
+		}
+		hwacc_enabled[hwacc_dev] = false;
+	}
+
+	if (ram_retention) {
+		if (!enable_ret && hwacc_ret_enabled[hwacc_dev]) {
+			r = regulator_disable(hwacc_ret_regulator[hwacc_dev]);
+			if (r < 0) {
+				pr_err("prcmu_set_hwacc: ret disable failed\n");
+				goto out;
+			}
+			hwacc_ret_enabled[hwacc_dev] = false;
+		}
+	}
+
+out:
+	return r;
+}
+EXPORT_SYMBOL(prcmu_set_hwacc);
+
 /**
- * prcmu_set_epod - set the state of a EPOD (power domain)
+ * db8500_prcmu_set_epod - set the state of a EPOD (power domain)
  * @epod_id: The EPOD to set
  * @epod_state: The new EPOD state
  *
  * This function sets the state of a EPOD (power domain). It may not be called
  * from interrupt context.
  */
-int prcmu_set_epod(u16 epod_id, u8 epod_state)
+int db8500_prcmu_set_epod(u16 epod_id, u8 epod_state)
 {
 	int r = 0;
 	bool ram_retention = false;
@@ -1048,7 +1310,7 @@ int prcmu_set_epod(u16 epod_id, u8 epod_state)
 	mutex_lock(&mb2_transfer.lock);
 
 	/* wait for mailbox */
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(2))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(2))
 		cpu_relax();
 
 	/* fill in mailbox */
@@ -1058,7 +1320,7 @@ int prcmu_set_epod(u16 epod_id, u8 epod_state)
 
 	writeb(MB2H_DPS, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB2));
 
-	writel(MBOX_BIT(2), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(2), PRCM_MBOX_CPU_SET);
 
 	/*
 	 * The current firmware version does not handle errors correctly,
@@ -1145,13 +1407,13 @@ static int request_sysclk(bool enable)
 
 	spin_lock_irqsave(&mb3_transfer.lock, flags);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(3))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(3))
 		cpu_relax();
 
 	writeb((enable ? ON : OFF), (tcdm_base + PRCM_REQ_MB3_SYSCLK_MGT));
 
 	writeb(MB3H_SYSCLK, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB3));
-	writel(MBOX_BIT(3), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(3), PRCM_MBOX_CPU_SET);
 
 	spin_unlock_irqrestore(&mb3_transfer.lock, flags);
 
@@ -1177,7 +1439,7 @@ static int request_timclk(bool enable)
 
 	if (!enable)
 		val |= PRCM_TCR_STOP_TIMERS;
-	writel(val, (_PRCMU_BASE + PRCM_TCR));
+	writel(val, PRCM_TCR);
 
 	return 0;
 }
@@ -1190,7 +1452,7 @@ static int request_reg_clock(u8 clock, bool enable)
 	spin_lock_irqsave(&clk_mgt_lock, flags);
 
 	/* Grab the HW semaphore. */
-	while ((readl(_PRCMU_BASE + PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
+	while ((readl(PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
 		cpu_relax();
 
 	val = readl(_PRCMU_BASE + clk_mgt[clock].offset);
@@ -1203,34 +1465,58 @@ static int request_reg_clock(u8 clock, bool enable)
 	writel(val, (_PRCMU_BASE + clk_mgt[clock].offset));
 
 	/* Release the HW semaphore. */
-	writel(0, (_PRCMU_BASE + PRCM_SEM));
+	writel(0, PRCM_SEM);
 
 	spin_unlock_irqrestore(&clk_mgt_lock, flags);
 
 	return 0;
 }
 
+static int request_sga_clock(u8 clock, bool enable)
+{
+	u32 val;
+	int ret;
+
+	if (enable) {
+		val = readl(PRCM_CGATING_BYPASS);
+		writel(val | PRCM_CGATING_BYPASS_ICN2, PRCM_CGATING_BYPASS);
+	}
+
+	ret = request_reg_clock(clock, enable);
+
+	if (!ret && !enable) {
+		val = readl(PRCM_CGATING_BYPASS);
+		writel(val & ~PRCM_CGATING_BYPASS_ICN2, PRCM_CGATING_BYPASS);
+	}
+
+	return ret;
+}
+
 /**
- * prcmu_request_clock() - Request for a clock to be enabled or disabled.
+ * db8500_prcmu_request_clock() - Request for a clock to be enabled or disabled.
  * @clock:      The clock for which the request is made.
  * @enable:     Whether the clock should be enabled (true) or disabled (false).
  *
  * This function should only be used by the clock implementation.
  * Do not use it from any other place!
  */
-int prcmu_request_clock(u8 clock, bool enable)
+int db8500_prcmu_request_clock(u8 clock, bool enable)
 {
-	if (clock < PRCMU_NUM_REG_CLOCKS)
+	if (clock == PRCMU_SGACLK)
+		return request_sga_clock(clock, enable);
+	else if (clock < PRCMU_NUM_REG_CLOCKS)
 		return request_reg_clock(clock, enable);
 	else if (clock == PRCMU_TIMCLK)
 		return request_timclk(enable);
 	else if (clock == PRCMU_SYSCLK)
 		return request_sysclk(enable);
+	else if (clock == PRCMU_PLLSOC1)
+		return request_pll(clock, enable);
 	else
 		return -EINVAL;
 }
 
-int prcmu_config_esram0_deep_sleep(u8 state)
+int db8500_prcmu_config_esram0_deep_sleep(u8 state)
 {
 	if ((state > ESRAM0_DEEP_SLEEP_STATE_RET) ||
 	    (state < ESRAM0_DEEP_SLEEP_STATE_OFF))
@@ -1238,7 +1524,7 @@ int prcmu_config_esram0_deep_sleep(u8 state)
 
 	mutex_lock(&mb4_transfer.lock);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
 		cpu_relax();
 
 	writeb(MB4H_MEM_ST, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB4));
@@ -1248,7 +1534,7 @@ int prcmu_config_esram0_deep_sleep(u8 state)
 	       (tcdm_base + PRCM_REQ_MB4_DDR_ST_AP_DEEP_IDLE));
 	writeb(state, (tcdm_base + PRCM_REQ_MB4_ESRAM0_ST));
 
-	writel(MBOX_BIT(4), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(4), PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb4_transfer.work);
 
 	mutex_unlock(&mb4_transfer.lock);
@@ -1260,13 +1546,13 @@ int prcmu_config_hotdog(u8 threshold)
 {
 	mutex_lock(&mb4_transfer.lock);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
 		cpu_relax();
 
 	writeb(threshold, (tcdm_base + PRCM_REQ_MB4_HOTDOG_THRESHOLD));
 	writeb(MB4H_HOTDOG, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB4));
 
-	writel(MBOX_BIT(4), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(4), PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb4_transfer.work);
 
 	mutex_unlock(&mb4_transfer.lock);
@@ -1278,7 +1564,7 @@ int prcmu_config_hotmon(u8 low, u8 high)
 {
 	mutex_lock(&mb4_transfer.lock);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
 		cpu_relax();
 
 	writeb(low, (tcdm_base + PRCM_REQ_MB4_HOTMON_LOW));
@@ -1287,7 +1573,7 @@ int prcmu_config_hotmon(u8 low, u8 high)
 		(tcdm_base + PRCM_REQ_MB4_HOTMON_CONFIG));
 	writeb(MB4H_HOTMON, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB4));
 
-	writel(MBOX_BIT(4), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(4), PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb4_transfer.work);
 
 	mutex_unlock(&mb4_transfer.lock);
@@ -1299,13 +1585,13 @@ static int config_hot_period(u16 val)
 {
 	mutex_lock(&mb4_transfer.lock);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
 		cpu_relax();
 
 	writew(val, (tcdm_base + PRCM_REQ_MB4_HOT_PERIOD));
 	writeb(MB4H_HOT_PERIOD, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB4));
 
-	writel(MBOX_BIT(4), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(4), PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb4_transfer.work);
 
 	mutex_unlock(&mb4_transfer.lock);
@@ -1326,6 +1612,78 @@ int prcmu_stop_temp_sense(void)
 	return config_hot_period(0xFFFF);
 }
 
+static int prcmu_a9wdog(u8 cmd, u8 d0, u8 d1, u8 d2, u8 d3)
+{
+
+	mutex_lock(&mb4_transfer.lock);
+
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
+		cpu_relax();
+
+	writeb(d0, (tcdm_base + PRCM_REQ_MB4_A9WDOG_0));
+	writeb(d1, (tcdm_base + PRCM_REQ_MB4_A9WDOG_1));
+	writeb(d2, (tcdm_base + PRCM_REQ_MB4_A9WDOG_2));
+	writeb(d3, (tcdm_base + PRCM_REQ_MB4_A9WDOG_3));
+
+	writeb(cmd, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB4));
+
+	writel(MBOX_BIT(4), PRCM_MBOX_CPU_SET);
+	wait_for_completion(&mb4_transfer.work);
+
+	mutex_unlock(&mb4_transfer.lock);
+
+	return 0;
+
+}
+
+int prcmu_config_a9wdog(u8 num, bool sleep_auto_off)
+{
+	BUG_ON(num == 0 || num > 0xf);
+	return prcmu_a9wdog(MB4H_A9WDOG_CONF, num, 0, 0,
+			    sleep_auto_off ? A9WDOG_AUTO_OFF_EN :
+			    A9WDOG_AUTO_OFF_DIS);
+}
+
+int prcmu_enable_a9wdog(u8 id)
+{
+	return prcmu_a9wdog(MB4H_A9WDOG_EN, id, 0, 0, 0);
+}
+
+int prcmu_disable_a9wdog(u8 id)
+{
+	return prcmu_a9wdog(MB4H_A9WDOG_DIS, id, 0, 0, 0);
+}
+
+int prcmu_kick_a9wdog(u8 id)
+{
+	return prcmu_a9wdog(MB4H_A9WDOG_KICK, id, 0, 0, 0);
+}
+
+/*
+ * timeout is 28 bit, in ms.
+ */
+#define MAX_WATCHDOG_TIMEOUT 131000
+int prcmu_load_a9wdog(u8 id, u32 timeout)
+{
+	if (timeout > MAX_WATCHDOG_TIMEOUT)
+		/*
+		 * Due to calculation bug in prcmu fw, timeouts
+		 * can't be bigger than 131 seconds.
+		 */
+		return -EINVAL;
+
+	return prcmu_a9wdog(MB4H_A9WDOG_LOAD,
+			    (id & A9WDOG_ID_MASK) |
+			    /*
+			     * Put the lowest 28 bits of timeout at
+			     * offset 4. Four first bits are used for id.
+			     */
+			    (u8)((timeout << 4) & 0xf0),
+			    (u8)((timeout >> 4) & 0xff),
+			    (u8)((timeout >> 12) & 0xff),
+			    (u8)((timeout >> 20) & 0xff));
+}
+
 /**
  * prcmu_set_clock_divider() - Configure the clock divider.
  * @clock:	The clock for which the request is made.
@@ -1345,7 +1703,7 @@ int prcmu_set_clock_divider(u8 clock, u8 divider)
 	spin_lock_irqsave(&clk_mgt_lock, flags);
 
 	/* Grab the HW semaphore. */
-	while ((readl(_PRCMU_BASE + PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
+	while ((readl(PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
 		cpu_relax();
 
 	val = readl(_PRCMU_BASE + clk_mgt[clock].offset);
@@ -1354,7 +1712,7 @@ int prcmu_set_clock_divider(u8 clock, u8 divider)
 	writel(val, (_PRCMU_BASE + clk_mgt[clock].offset));
 
 	/* Release the HW semaphore. */
-	writel(0, (_PRCMU_BASE + PRCM_SEM));
+	writel(0, PRCM_SEM);
 
 	spin_unlock_irqrestore(&clk_mgt_lock, flags);
 
@@ -1380,7 +1738,7 @@ int prcmu_abb_read(u8 slave, u8 reg, u8 *value, u8 size)
 
 	mutex_lock(&mb5_transfer.lock);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(5))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(5))
 		cpu_relax();
 
 	writeb(PRCMU_I2C_READ(slave), (tcdm_base + PRCM_REQ_MB5_I2C_SLAVE_OP));
@@ -1388,7 +1746,7 @@ int prcmu_abb_read(u8 slave, u8 reg, u8 *value, u8 size)
 	writeb(reg, (tcdm_base + PRCM_REQ_MB5_I2C_REG));
 	writeb(0, (tcdm_base + PRCM_REQ_MB5_I2C_VAL));
 
-	writel(MBOX_BIT(5), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(5), PRCM_MBOX_CPU_SET);
 
 	if (!wait_for_completion_timeout(&mb5_transfer.work,
 				msecs_to_jiffies(20000))) {
@@ -1426,7 +1784,7 @@ int prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size)
 
 	mutex_lock(&mb5_transfer.lock);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(5))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(5))
 		cpu_relax();
 
 	writeb(PRCMU_I2C_WRITE(slave), (tcdm_base + PRCM_REQ_MB5_I2C_SLAVE_OP));
@@ -1434,7 +1792,7 @@ int prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size)
 	writeb(reg, (tcdm_base + PRCM_REQ_MB5_I2C_REG));
 	writeb(*value, (tcdm_base + PRCM_REQ_MB5_I2C_VAL));
 
-	writel(MBOX_BIT(5), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(5), PRCM_MBOX_CPU_SET);
 
 	if (!wait_for_completion_timeout(&mb5_transfer.work,
 				msecs_to_jiffies(20000))) {
@@ -1456,21 +1814,44 @@ int prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size)
 void prcmu_ac_wake_req(void)
 {
 	u32 val;
+	u32 status;
 
 	mutex_lock(&mb0_transfer.ac_wake_lock);
 
-	val = readl(_PRCMU_BASE + PRCM_HOSTACCESS_REQ);
+	val = readl(PRCM_HOSTACCESS_REQ);
 	if (val & PRCM_HOSTACCESS_REQ_HOSTACCESS_REQ)
 		goto unlock_and_return;
 
 	atomic_set(&ac_wake_req_state, 1);
 
-	writel((val | PRCM_HOSTACCESS_REQ_HOSTACCESS_REQ),
-		(_PRCMU_BASE + PRCM_HOSTACCESS_REQ));
+retry:
+	writel((val | PRCM_HOSTACCESS_REQ_HOSTACCESS_REQ), PRCM_HOSTACCESS_REQ);
 
 	if (!wait_for_completion_timeout(&mb0_transfer.ac_wake_work,
-			msecs_to_jiffies(20000))) {
-		pr_err("prcmu: %s timed out (20 s) waiting for a reply.\n",
+			msecs_to_jiffies(5000))) {
+		panic("prcmu: %s timed out (5 s) waiting for a reply.\n",
+			__func__);
+		goto unlock_and_return;
+	}
+
+	/*
+	 * The modem can generate an AC_WAKE_ACK, and then still go to sleep.
+	 * As a workaround, we wait, and then check that the modem is indeed
+	 * awake (in terms of the value of the PRCM_MOD_AWAKE_STATUS
+	 * register, which may not be the whole truth).
+	 */
+	udelay(400);
+	status = (readl(PRCM_MOD_AWAKE_STATUS) & BITS(0, 2));
+	if (status != (PRCM_MOD_AWAKE_STATUS_PRCM_MOD_AAPD_AWAKE |
+			PRCM_MOD_AWAKE_STATUS_PRCM_MOD_COREPD_AWAKE)) {
+		pr_err("prcmu: %s received ack, but modem not awake (0x%X).\n",
+			__func__, status);
+		udelay(1200);
+		writel(val, PRCM_HOSTACCESS_REQ);
+		if (wait_for_completion_timeout(&mb0_transfer.ac_wake_work,
+				msecs_to_jiffies(5000)))
+			goto retry;
+		panic("prcmu: %s timed out (5 s) waiting for AC_SLEEP_ACK.\n",
 			__func__);
 	}
 
@@ -1487,16 +1868,16 @@ void prcmu_ac_sleep_req()
 
 	mutex_lock(&mb0_transfer.ac_wake_lock);
 
-	val = readl(_PRCMU_BASE + PRCM_HOSTACCESS_REQ);
+	val = readl(PRCM_HOSTACCESS_REQ);
 	if (!(val & PRCM_HOSTACCESS_REQ_HOSTACCESS_REQ))
 		goto unlock_and_return;
 
 	writel((val & ~PRCM_HOSTACCESS_REQ_HOSTACCESS_REQ),
-		(_PRCMU_BASE + PRCM_HOSTACCESS_REQ));
+		PRCM_HOSTACCESS_REQ);
 
 	if (!wait_for_completion_timeout(&mb0_transfer.ac_wake_work,
-			msecs_to_jiffies(20000))) {
-		pr_err("prcmu: %s timed out (20 s) waiting for a reply.\n",
+			msecs_to_jiffies(5000))) {
+		panic("prcmu: %s timed out (5 s) waiting for a reply.\n",
 			__func__);
 	}
 
@@ -1506,21 +1887,32 @@ unlock_and_return:
 	mutex_unlock(&mb0_transfer.ac_wake_lock);
 }
 
-bool prcmu_is_ac_wake_requested(void)
+bool db8500_prcmu_is_ac_wake_requested(void)
 {
 	return (atomic_read(&ac_wake_req_state) != 0);
 }
 
 /**
- * prcmu_system_reset - System reset
+ * db8500_prcmu_system_reset - System reset
  *
- * Saves the reset reason code and then sets the APE_SOFRST register which
+ * Saves the reset reason code and then sets the APE_SOFTRST register which
  * fires interrupt to fw
  */
-void prcmu_system_reset(u16 reset_code)
+void db8500_prcmu_system_reset(u16 reset_code)
 {
 	writew(reset_code, (tcdm_base + PRCM_SW_RST_REASON));
-	writel(1, (_PRCMU_BASE + PRCM_APE_SOFTRST));
+	writel(1, PRCM_APE_SOFTRST);
+}
+
+/**
+ * db8500_prcmu_get_reset_code - Retrieve SW reset reason code
+ *
+ * Retrieves the reset reason code stored by prcmu_system_reset() before
+ * last restart.
+ */
+u16 db8500_prcmu_get_reset_code(void)
+{
+	return readw(tcdm_base + PRCM_SW_RST_REASON);
 }
 
 /**
@@ -1530,11 +1922,11 @@ void prcmu_modem_reset(void)
 {
 	mutex_lock(&mb1_transfer.lock);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
 		cpu_relax();
 
 	writeb(MB1H_RESET_MODEM, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB1));
-	writel(MBOX_BIT(1), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(1), PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb1_transfer.work);
 
 	/*
@@ -1551,11 +1943,11 @@ static void ack_dbb_wakeup(void)
 
 	spin_lock_irqsave(&mb0_transfer.lock, flags);
 
-	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
+	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
 		cpu_relax();
 
 	writeb(MB0H_READ_WAKEUP_ACK, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB0));
-	writel(MBOX_BIT(0), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+	writel(MBOX_BIT(0), PRCM_MBOX_CPU_SET);
 
 	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
 }
@@ -1600,7 +1992,7 @@ static bool read_mailbox_0(void)
 		r = false;
 		break;
 	}
-	writel(MBOX_BIT(0), (_PRCMU_BASE + PRCM_ARM_IT1_CLR));
+	writel(MBOX_BIT(0), PRCM_ARM_IT1_CLR);
 	return r;
 }
 
@@ -1613,7 +2005,7 @@ static bool read_mailbox_1(void)
 		PRCM_ACK_MB1_CURRENT_APE_OPP);
 	mb1_transfer.ack.ape_voltage_status = readb(tcdm_base +
 		PRCM_ACK_MB1_APE_VOLTAGE_STATUS);
-	writel(MBOX_BIT(1), (_PRCMU_BASE + PRCM_ARM_IT1_CLR));
+	writel(MBOX_BIT(1), PRCM_ARM_IT1_CLR);
 	complete(&mb1_transfer.work);
 	return false;
 }
@@ -1621,14 +2013,14 @@ static bool read_mailbox_1(void)
 static bool read_mailbox_2(void)
 {
 	mb2_transfer.ack.status = readb(tcdm_base + PRCM_ACK_MB2_DPS_STATUS);
-	writel(MBOX_BIT(2), (_PRCMU_BASE + PRCM_ARM_IT1_CLR));
+	writel(MBOX_BIT(2), PRCM_ARM_IT1_CLR);
 	complete(&mb2_transfer.work);
 	return false;
 }
 
 static bool read_mailbox_3(void)
 {
-	writel(MBOX_BIT(3), (_PRCMU_BASE + PRCM_ARM_IT1_CLR));
+	writel(MBOX_BIT(3), PRCM_ARM_IT1_CLR);
 	return false;
 }
 
@@ -1643,6 +2035,11 @@ static bool read_mailbox_4(void)
 	case MB4H_HOTDOG:
 	case MB4H_HOTMON:
 	case MB4H_HOT_PERIOD:
+	case MB4H_A9WDOG_CONF:
+	case MB4H_A9WDOG_EN:
+	case MB4H_A9WDOG_DIS:
+	case MB4H_A9WDOG_LOAD:
+	case MB4H_A9WDOG_KICK:
 		break;
 	default:
 		print_unknown_header_warning(4, header);
@@ -1650,7 +2047,7 @@ static bool read_mailbox_4(void)
 		break;
 	}
 
-	writel(MBOX_BIT(4), (_PRCMU_BASE + PRCM_ARM_IT1_CLR));
+	writel(MBOX_BIT(4), PRCM_ARM_IT1_CLR);
 
 	if (do_complete)
 		complete(&mb4_transfer.work);
@@ -1662,20 +2059,20 @@ static bool read_mailbox_5(void)
 {
 	mb5_transfer.ack.status = readb(tcdm_base + PRCM_ACK_MB5_I2C_STATUS);
 	mb5_transfer.ack.value = readb(tcdm_base + PRCM_ACK_MB5_I2C_VAL);
-	writel(MBOX_BIT(5), (_PRCMU_BASE + PRCM_ARM_IT1_CLR));
+	writel(MBOX_BIT(5), PRCM_ARM_IT1_CLR);
 	complete(&mb5_transfer.work);
 	return false;
 }
 
 static bool read_mailbox_6(void)
 {
-	writel(MBOX_BIT(6), (_PRCMU_BASE + PRCM_ARM_IT1_CLR));
+	writel(MBOX_BIT(6), PRCM_ARM_IT1_CLR);
 	return false;
 }
 
 static bool read_mailbox_7(void)
 {
-	writel(MBOX_BIT(7), (_PRCMU_BASE + PRCM_ARM_IT1_CLR));
+	writel(MBOX_BIT(7), PRCM_ARM_IT1_CLR);
 	return false;
 }
 
@@ -1696,7 +2093,7 @@ static irqreturn_t prcmu_irq_handler(int irq, void *data)
 	u8 n;
 	irqreturn_t r;
 
-	bits = (readl(_PRCMU_BASE + PRCM_ARM_IT1_VAL) & ALL_MBOX_BITS);
+	bits = (readl(PRCM_ARM_IT1_VAL) & ALL_MBOX_BITS);
 	if (unlikely(!bits))
 		return IRQ_NONE;
 
@@ -1768,40 +2165,34 @@ static struct irq_chip prcmu_irq_chip = {
 	.irq_unmask	= prcmu_irq_unmask,
 };
 
-void __init prcmu_early_init(void)
+void __init db8500_prcmu_early_init(void)
 {
 	unsigned int i;
 
-	if (cpu_is_u8500v1()) {
-		tcdm_base = __io_address(U8500_PRCMU_TCDM_BASE_V1);
-	} else if (cpu_is_u8500v2()) {
-		void *tcpm_base = ioremap_nocache(U8500_PRCMU_TCPM_BASE, SZ_4K);
-
-		if (tcpm_base != NULL) {
-			int version;
-			version = readl(tcpm_base + PRCMU_FW_VERSION_OFFSET);
-			prcmu_version.project_number = version & 0xFF;
-			prcmu_version.api_version = (version >> 8) & 0xFF;
-			prcmu_version.func_version = (version >> 16) & 0xFF;
-			prcmu_version.errata = (version >> 24) & 0xFF;
-			pr_info("PRCMU firmware version %d.%d.%d\n",
-				(version >> 8) & 0xFF, (version >> 16) & 0xFF,
-				(version >> 24) & 0xFF);
-			iounmap(tcpm_base);
-		}
-
-		tcdm_base = __io_address(U8500_PRCMU_TCDM_BASE);
-	} else {
-		pr_err("prcmu: Unsupported chip version\n");
-		BUG();
+	void *tcpm_base = ioremap_nocache(U8500_PRCMU_TCPM_BASE, SZ_4K);
+
+	if (tcpm_base != NULL) {
+		int version;
+		version = readl(tcpm_base + PRCMU_FW_VERSION_OFFSET);
+		prcmu_version.project_number = version & 0xFF;
+		prcmu_version.api_version = (version >> 8) & 0xFF;
+		prcmu_version.func_version = (version >> 16) & 0xFF;
+		prcmu_version.errata = (version >> 24) & 0xFF;
+		pr_info("PRCMU firmware version %d.%d.%d\n",
+			(version >> 8) & 0xFF, (version >> 16) & 0xFF,
+			(version >> 24) & 0xFF);
+		iounmap(tcpm_base);
 	}
 
+	tcdm_base = __io_address(U8500_PRCMU_TCDM_BASE);
+
 	spin_lock_init(&mb0_transfer.lock);
 	spin_lock_init(&mb0_transfer.dbb_irqs_lock);
 	mutex_init(&mb0_transfer.ac_wake_lock);
 	init_completion(&mb0_transfer.ac_wake_work);
 	mutex_init(&mb1_transfer.lock);
 	init_completion(&mb1_transfer.work);
+	mb1_transfer.ape_opp = APE_NO_CHANGE;
 	mutex_init(&mb2_transfer.lock);
 	init_completion(&mb2_transfer.work);
 	spin_lock_init(&mb2_transfer.auto_pm_lock);
@@ -1826,6 +2217,16 @@ void __init prcmu_early_init(void)
 	}
 }
 
+static void __init init_prcm_registers(void)
+{
+	u32 val;
+
+	val = readl(PRCM_A9PL_FORCE_CLKEN);
+	val &= ~(PRCM_A9PL_FORCE_CLKEN_PRCM_A9PL_FORCE_CLKEN |
+		PRCM_A9PL_FORCE_CLKEN_PRCM_A9AXI_FORCE_CLKEN);
+	writel(val, (PRCM_A9PL_FORCE_CLKEN));
+}
+
 /*
  * Power domain switches (ePODs) modeled as regulators for the DB8500 SoC
  */
@@ -1852,8 +2253,6 @@ static struct regulator_consumer_supply db8500_vape_consumers[] = {
 
 static struct regulator_consumer_supply db8500_vsmps2_consumers[] = {
 	/* CG2900 and CW1200 power to off-chip peripherals */
-	REGULATOR_SUPPLY("gbf_1v8", "cg2900-uart.0"),
-	REGULATOR_SUPPLY("wlan_1v8", "cw1200.0"),
 	REGULATOR_SUPPLY("musb_1v8", "ab8500-usb.0"),
 	/* AV8100 regulator */
 	REGULATOR_SUPPLY("hdmi_1v8", "0-0070"),
@@ -1861,7 +2260,42 @@ static struct regulator_consumer_supply db8500_vsmps2_consumers[] = {
 
 static struct regulator_consumer_supply db8500_b2r2_mcde_consumers[] = {
 	REGULATOR_SUPPLY("vsupply", "b2r2.0"),
-	REGULATOR_SUPPLY("vsupply", "mcde.0"),
+	REGULATOR_SUPPLY("vsupply", "mcde"),
+};
+
+/* SVA MMDSP regulator switch */
+static struct regulator_consumer_supply db8500_svammdsp_consumers[] = {
+	REGULATOR_SUPPLY("sva-mmdsp", "cm_control"),
+};
+
+/* SVA pipe regulator switch */
+static struct regulator_consumer_supply db8500_svapipe_consumers[] = {
+	REGULATOR_SUPPLY("sva-pipe", "cm_control"),
+};
+
+/* SIA MMDSP regulator switch */
+static struct regulator_consumer_supply db8500_siammdsp_consumers[] = {
+	REGULATOR_SUPPLY("sia-mmdsp", "cm_control"),
+};
+
+/* SIA pipe regulator switch */
+static struct regulator_consumer_supply db8500_siapipe_consumers[] = {
+	REGULATOR_SUPPLY("sia-pipe", "cm_control"),
+};
+
+static struct regulator_consumer_supply db8500_sga_consumers[] = {
+	REGULATOR_SUPPLY("v-mali", NULL),
+};
+
+/* ESRAM1 and 2 regulator switch */
+static struct regulator_consumer_supply db8500_esram12_consumers[] = {
+	REGULATOR_SUPPLY("esram12", "cm_control"),
+};
+
+/* ESRAM3 and 4 regulator switch */
+static struct regulator_consumer_supply db8500_esram34_consumers[] = {
+	REGULATOR_SUPPLY("v-esram34", "mcde"),
+	REGULATOR_SUPPLY("esram34", "cm_control"),
 };
 
 static struct regulator_init_data db8500_regulators[DB8500_NUM_REGULATORS] = {
@@ -1923,6 +2357,8 @@ static struct regulator_init_data db8500_regulators[DB8500_NUM_REGULATORS] = {
 			.name = "db8500-sva-mmdsp",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
+		.consumer_supplies = db8500_svammdsp_consumers,
+		.num_consumer_supplies = ARRAY_SIZE(db8500_svammdsp_consumers),
 	},
 	[DB8500_REGULATOR_SWITCH_SVAMMDSPRET] = {
 		.constraints = {
@@ -1937,6 +2373,8 @@ static struct regulator_init_data db8500_regulators[DB8500_NUM_REGULATORS] = {
 			.name = "db8500-sva-pipe",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
+		.consumer_supplies = db8500_svapipe_consumers,
+		.num_consumer_supplies = ARRAY_SIZE(db8500_svapipe_consumers),
 	},
 	[DB8500_REGULATOR_SWITCH_SIAMMDSP] = {
 		.supply_regulator = "db8500-vape",
@@ -1944,6 +2382,8 @@ static struct regulator_init_data db8500_regulators[DB8500_NUM_REGULATORS] = {
 			.name = "db8500-sia-mmdsp",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
+		.consumer_supplies = db8500_siammdsp_consumers,
+		.num_consumer_supplies = ARRAY_SIZE(db8500_siammdsp_consumers),
 	},
 	[DB8500_REGULATOR_SWITCH_SIAMMDSPRET] = {
 		.constraints = {
@@ -1957,6 +2397,8 @@ static struct regulator_init_data db8500_regulators[DB8500_NUM_REGULATORS] = {
 			.name = "db8500-sia-pipe",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
+		.consumer_supplies = db8500_siapipe_consumers,
+		.num_consumer_supplies = ARRAY_SIZE(db8500_siapipe_consumers),
 	},
 	[DB8500_REGULATOR_SWITCH_SGA] = {
 		.supply_regulator = "db8500-vape",
@@ -1964,6 +2406,9 @@ static struct regulator_init_data db8500_regulators[DB8500_NUM_REGULATORS] = {
 			.name = "db8500-sga",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
+		.consumer_supplies = db8500_sga_consumers,
+		.num_consumer_supplies = ARRAY_SIZE(db8500_sga_consumers),
+
 	},
 	[DB8500_REGULATOR_SWITCH_B2R2_MCDE] = {
 		.supply_regulator = "db8500-vape",
@@ -1980,6 +2425,8 @@ static struct regulator_init_data db8500_regulators[DB8500_NUM_REGULATORS] = {
 			.name = "db8500-esram12",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
+		.consumer_supplies = db8500_esram12_consumers,
+		.num_consumer_supplies = ARRAY_SIZE(db8500_esram12_consumers),
 	},
 	[DB8500_REGULATOR_SWITCH_ESRAM12RET] = {
 		.constraints = {
@@ -1993,6 +2440,8 @@ static struct regulator_init_data db8500_regulators[DB8500_NUM_REGULATORS] = {
 			.name = "db8500-esram34",
 			.valid_ops_mask = REGULATOR_CHANGE_STATUS,
 		},
+		.consumer_supplies = db8500_esram34_consumers,
+		.num_consumer_supplies = ARRAY_SIZE(db8500_esram34_consumers),
 	},
 	[DB8500_REGULATOR_SWITCH_ESRAM34RET] = {
 		.constraints = {
@@ -2024,8 +2473,10 @@ static int __init db8500_prcmu_probe(struct platform_device *pdev)
 	if (ux500_is_svp())
 		return -ENODEV;
 
+	init_prcm_registers();
+
 	/* Clean up the mailbox interrupts after pre-kernel code. */
-	writel(ALL_MBOX_BITS, (_PRCMU_BASE + PRCM_ARM_IT1_CLR));
+	writel(ALL_MBOX_BITS, PRCM_ARM_IT1_CLR);
 
 	err = request_threaded_irq(IRQ_DB8500_PRCMU1, prcmu_irq_handler,
 		prcmu_irq_thread_fn, IRQF_NO_SUSPEND, "prcmu", NULL);
diff --git a/drivers/mfd/stmpe.c b/drivers/mfd/stmpe.c
index 7ab7746631d..f9abf20975b 100644
--- a/drivers/mfd/stmpe.c
+++ b/drivers/mfd/stmpe.c
@@ -228,7 +228,7 @@ int stmpe_block_write(struct stmpe *stmpe, u8 reg, u8 length,
 EXPORT_SYMBOL_GPL(stmpe_block_write);
 
 /**
- * stmpe_set_altfunc: set the alternate function for STMPE pins
+ * stmpe_set_altfunc()- set the alternate function for STMPE pins
  * @stmpe:	Device to configure
  * @pins:	Bitmask of pins to affect
  * @block:	block to enable alternate functions for
@@ -674,7 +674,7 @@ static irqreturn_t stmpe_irq(int irq, void *data)
 	ret = stmpe_block_read(stmpe, israddr, num, isr);
 	if (ret < 0)
 		return IRQ_NONE;
-
+back:
 	for (i = 0; i < num; i++) {
 		int bank = num - i - 1;
 		u8 status = isr[i];
@@ -696,6 +696,22 @@ static irqreturn_t stmpe_irq(int irq, void *data)
 		stmpe_reg_write(stmpe, israddr + i, clear);
 	}
 
+	/*
+	   It may happen that on the first status read interrupt
+	   sources may not showup, so read one more time.
+	 */
+	ret = stmpe_block_read(stmpe, israddr, num, isr);
+	if (ret >= 0) {
+		for (i = 0; i < num; i++) {
+			int bank = num - i - 1;
+			u8 status = isr[i];
+
+			status &= stmpe->ier[bank];
+			if (status)
+				goto back;
+		}
+	}
+
 	return IRQ_HANDLED;
 }
 
diff --git a/drivers/mfd/stmpe.h b/drivers/mfd/stmpe.h
index 0dbdc4e8cd7..e4ee3895658 100644
--- a/drivers/mfd/stmpe.h
+++ b/drivers/mfd/stmpe.h
@@ -42,6 +42,7 @@ struct stmpe_variant_block {
  * @id_mask:	bits valid in CHIPID register for comparison with id_val
  * @num_gpios:	number of GPIOS
  * @af_bits:	number of bits used to specify the alternate function
+ * @regs: variant specific registers.
  * @blocks:	list of blocks present on this device
  * @num_blocks:	number of blocks present on this device
  * @num_irqs:	number of internal IRQs available on this device
diff --git a/drivers/mfd/tps6105x.c b/drivers/mfd/tps6105x.c
index a293b978e27..d7b9e0c60ea 100644
--- a/drivers/mfd/tps6105x.c
+++ b/drivers/mfd/tps6105x.c
@@ -195,6 +195,7 @@ static int __devinit tps6105x_probe(struct i2c_client *client,
 	return 0;
 
 fail:
+	i2c_set_clientdata(client, NULL);
 	kfree(tps6105x);
 	return ret;
 }
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 68f367184ab..03d034ad3bc 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -483,6 +483,20 @@ config ARM_CHARLCD
 	  line and the Linux version on the second line, but that's
 	  still useful.
 
+config STE_TRACE_MODEM
+	tristate "DB8500 trace Modem"
+	depends on ARCH_U8500
+	default n
+	help
+	 Select this option to enable modem tracing by APE
+
+config DBX500_MLOADER
+	tristate "Modem firmware loader for db8500"
+	default n
+	depends on UX500_SOC_DB8500 || UX500_SOC_DB5500
+	help
+	  Provides a user interface to load modem firmware on dbx500 SOCs
+
 config BMP085
 	tristate "BMP085 digital pressure sensor"
 	depends on I2C && SYSFS
@@ -535,6 +549,31 @@ config APANIC_PLABEL
 	 If your platform uses a different flash partition label for storing
  	 crashdumps, enter it here.
 
+config HWMEM
+	bool "Hardware memory driver"
+	default n
+	help
+	  This driver provides a way to allocate contiguous system memory which
+	  can be used by hardware. It also enables accessing hwmem allocated
+	  memory buffers through a secure id which can be shared across processes.
+
+config U5500_MBOX
+	bool "Mailbox support"
+	depends on (UX500_SOC_DB5500 && U5500_MODEM_IRQ)
+	default y
+	help
+	  Add support for U5500 mailbox communication with modem side
+
+config U8500_SIM_DETECT
+	bool "Sim hot swap detection support"
+	depends on (MODEM && UX500_SOC_DB8500)
+	default n
+	help
+	  Add support for sim hot swap detection support in U8500.Driver
+	  basically wakes up the modem if its sleeping when sim hot plug
+	  in/out has happened.
+
+source "drivers/misc/Kconfig.stm"
 source "drivers/misc/c2port/Kconfig"
 source "drivers/misc/eeprom/Kconfig"
 source "drivers/misc/cb710/Kconfig"
@@ -542,5 +581,7 @@ source "drivers/misc/iwmc3200top/Kconfig"
 source "drivers/misc/ti-st/Kconfig"
 source "drivers/misc/lis3lv02d/Kconfig"
 source "drivers/misc/carma/Kconfig"
+source "drivers/misc/i2s/Kconfig"
+source "drivers/misc/audio_io_dev/Kconfig"
 
 endif # MISC_DEVICES
diff --git a/drivers/misc/Kconfig.stm b/drivers/misc/Kconfig.stm
new file mode 100644
index 00000000000..ef2b94683a3
--- /dev/null
+++ b/drivers/misc/Kconfig.stm
@@ -0,0 +1,114 @@
+menuconfig STM_TRACE
+	bool "STM MIPI Trace driver"
+	depends on ARCH_U8500
+	help
+	  Simple System Trace Module driver. It allows to use and configure the
+	  STM, either from kernel space, or from user space.
+
+if STM_TRACE
+
+config STM_NUMBER_OF_CHANNEL
+	int
+	default 512 if ARCH_U8500
+	default 256
+	help
+	  Number Max of channels always a multiple of 256
+
+config STM_PRINTK
+	bool "printk support"
+	depends on STM_TRACE
+	help
+	  Duplicate printk output on STM printk channel & activate stm_printk
+
+config STM_PRINTK_CHANNEL
+	int "printk channel"
+	range 0 255
+	depends on STM_PRINTK
+	default 255
+	help
+	  STM printk channel number
+
+config STM_FTRACE
+	bool "functions tracing"
+	depends on FTRACE
+	default y
+	help
+	  Output function tracing on STM dedicated channel
+
+config STM_FTRACE_CHANNEL
+	int "ftrace channel"
+	range 0 255
+	depends on STM_FTRACE
+	default 254
+	help
+	  STM ftrace channel number
+
+config STM_CTX_SWITCH
+	bool "Context switch tracing"
+	depends on CONTEXT_SWITCH_TRACER
+	default y
+	help
+	  Output scheduler context switch on STM dedicated channel
+
+config STM_CTX_SWITCH_CHANNEL
+	int "Context switch channel"
+	range 0 255
+	depends on STM_CTX_SWITCH
+	default 253
+	help
+	  STM Context switch channel number
+
+config STM_WAKEUP
+	bool "Scheduler wakeup tracing"
+	depends on CONTEXT_SWITCH_TRACER
+	default y
+	help
+	  Output scheduler wakeup on STM dedicated channel
+
+config STM_WAKEUP_CHANNEL
+	int "Wakeup channel"
+	range 0 255
+	depends on STM_WAKEUP
+	default 252
+	help
+	  STM scheduler wakeup channel number
+
+config STM_STACK_TRACE
+	bool "Stack tracing"
+	depends on STACKTRACE
+	default y
+	help
+	  Output stack tracing on STM dedicated channel
+
+config STM_STACK_TRACE_CHANNEL
+	int "Stack trace channel"
+	range 0 255
+	depends on STM_STACK_TRACE
+	default 251
+	help
+	  STM stack trace channel number
+
+config STM_TRACE_PRINTK
+	bool "trace printk & binary printk support"
+	depends on TRACING
+	default y
+	help
+	  Duplicate trace printk output on STM printk channel
+
+config STM_TRACE_PRINTK_CHANNEL
+	int "trace_printk channel"
+	range 0 255
+	depends on TRACING
+	default 250
+	help
+	  STM trace_printk channel number
+
+config STM_TRACE_BPRINTK_CHANNEL
+	int "trace_bprintk channel"
+	range 0 255
+	depends on TRACING
+	default 249
+	help
+	  STM trace binary printk channel number
+
+endif
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 2d430484519..5b13b84e222 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -52,3 +52,11 @@ obj-y				+= carma/
 obj-$(CONFIG_WL127X_RFKILL)	+= wl127x-rfkill.o
 obj-$(CONFIG_APANIC)		+= apanic.o
 obj-$(CONFIG_SENSORS_AK8975)	+= akm8975.o
+obj-$(CONFIG_STM_TRACE) 	+= stm.o
+obj-$(CONFIG_STE_TRACE_MODEM) 	+= db8500-modem-trace.o
+obj-$(CONFIG_DBX500_MLOADER) 	+= dbx500-mloader.o
+obj-$(CONFIG_STM_I2S)           += i2s/
+obj-$(CONFIG_STE_AUDIO_IO_DEV)	+= audio_io_dev/
+obj-$(CONFIG_HWMEM)             += hwmem/
+obj-$(CONFIG_U5500_MBOX)	+= mbox.o mbox_channels-db5500.o
+obj-$(CONFIG_U8500_SIM_DETECT)	+= sim_detect.o
diff --git a/drivers/misc/ab8500-pwm.c b/drivers/misc/ab8500-pwm.c
index 54e3d05b63c..e56d9c993da 100644
--- a/drivers/misc/ab8500-pwm.c
+++ b/drivers/misc/ab8500-pwm.c
@@ -8,6 +8,7 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/pwm.h>
+#include <linux/clk.h>
 #include <linux/mfd/ab8500.h>
 #include <linux/mfd/abx500.h>
 
@@ -26,8 +27,10 @@
 struct pwm_device {
 	struct device *dev;
 	struct list_head node;
+	struct clk *clk;
 	const char *label;
 	unsigned int pwm_id;
+	bool clk_enabled;
 };
 
 static LIST_HEAD(pwm_list);
@@ -66,9 +69,17 @@ int pwm_enable(struct pwm_device *pwm)
 {
 	int ret;
 
+	if (!pwm->clk_enabled) {
+		ret = clk_enable(pwm->clk);
+		if (ret < 0) {
+			dev_err(pwm->dev, "failed to enable clock\n");
+			return ret;
+		}
+		pwm->clk_enabled = true;
+	}
 	ret = abx500_mask_and_set_register_interruptible(pwm->dev,
 				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
-				1 << (pwm->pwm_id-1), ENABLE_PWM);
+				1 << (pwm->pwm_id-1), 1 << (pwm->pwm_id-1));
 	if (ret < 0)
 		dev_err(pwm->dev, "%s: Failed to disable PWM, Error %d\n",
 							pwm->label, ret);
@@ -83,9 +94,27 @@ void pwm_disable(struct pwm_device *pwm)
 	ret = abx500_mask_and_set_register_interruptible(pwm->dev,
 				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
 				1 << (pwm->pwm_id-1), DISABLE_PWM);
+	/*
+	 * Workaround to set PWM in disable.
+	 * If enable bit is not toggled the PWM might output 50/50 duty cycle
+	 * even though it should be disabled
+	 */
+	ret &= abx500_mask_and_set_register_interruptible(pwm->dev,
+				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
+				1 << (pwm->pwm_id-1),
+				ENABLE_PWM << (pwm->pwm_id-1));
+	ret &= abx500_mask_and_set_register_interruptible(pwm->dev,
+				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
+				1 << (pwm->pwm_id-1), DISABLE_PWM);
+
 	if (ret < 0)
 		dev_err(pwm->dev, "%s: Failed to disable PWM, Error %d\n",
 							pwm->label, ret);
+	if (pwm->clk_enabled) {
+		clk_disable(pwm->clk);
+		pwm->clk_enabled = false;
+	}
+
 	return;
 }
 EXPORT_SYMBOL(pwm_disable);
@@ -115,6 +144,8 @@ EXPORT_SYMBOL(pwm_free);
 static int __devinit ab8500_pwm_probe(struct platform_device *pdev)
 {
 	struct pwm_device *pwm;
+	int ret = 0;
+
 	/*
 	 * Nothing to be done in probe, this is required to get the
 	 * device which is required for ab8500 read and write
@@ -128,14 +159,24 @@ static int __devinit ab8500_pwm_probe(struct platform_device *pdev)
 	pwm->pwm_id = pdev->id;
 	list_add_tail(&pwm->node, &pwm_list);
 	platform_set_drvdata(pdev, pwm);
+
+	pwm->clk = clk_get(pwm->dev, NULL);
+	if (IS_ERR(pwm->clk)) {
+		dev_err(pwm->dev, "clock request failed\n");
+		ret = PTR_ERR(pwm->clk);
+		kfree(pwm);
+		return ret;
+	}
+	pwm->clk_enabled = false;
 	dev_dbg(pwm->dev, "pwm probe successful\n");
-	return 0;
+	return ret;
 }
 
 static int __devexit ab8500_pwm_remove(struct platform_device *pdev)
 {
 	struct pwm_device *pwm = platform_get_drvdata(pdev);
 	list_del(&pwm->node);
+	clk_put(pwm->clk);
 	dev_dbg(&pdev->dev, "pwm driver removed\n");
 	kfree(pwm);
 	return 0;
diff --git a/drivers/misc/audio_io_dev/Kconfig b/drivers/misc/audio_io_dev/Kconfig
new file mode 100644
index 00000000000..57bb77172f7
--- /dev/null
+++ b/drivers/misc/audio_io_dev/Kconfig
@@ -0,0 +1,11 @@
+#
+# AB8500 Audio IO Device Driver configuration
+#
+config STE_AUDIO_IO_DEV
+	bool "AB8500 Audio IO device driver"
+	depends on ARCH_U8500 && AB8500_CORE && STM_MSP_I2S
+	default y
+	---help---
+	  If you say Y here, you will enable the AB8500 Audio IO device driver.
+
+	  If unsure, say N.
diff --git a/drivers/misc/audio_io_dev/Makefile b/drivers/misc/audio_io_dev/Makefile
new file mode 100644
index 00000000000..44b21fcc573
--- /dev/null
+++ b/drivers/misc/audio_io_dev/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for AB8500 device drivers
+#
+obj-$(CONFIG_STE_AUDIO_IO_DEV)	+= ste_audio_io.o
+ste_audio_io-objs := ste_audio_io_dev.o\
+			ste_audio_io_core.o\
+			ste_audio_io_func.o\
+			ste_audio_io_hwctrl_common.o
+
diff --git a/drivers/misc/audio_io_dev/ste_audio_io_ab8500_reg_defs.h b/drivers/misc/audio_io_dev/ste_audio_io_ab8500_reg_defs.h
new file mode 100644
index 00000000000..1436430f7de
--- /dev/null
+++ b/drivers/misc/audio_io_dev/ste_audio_io_ab8500_reg_defs.h
@@ -0,0 +1,349 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Deepak KARDA/ deepak.karda@stericsson.com for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+
+#ifndef _AUDIOIO_REG_DEFS_H_
+#define _AUDIOIO_REG_DEFS_H_
+
+
+ /* Registers */
+#define POWER_UP_CONTROL_REG			0x0D00
+#define SOFTWARE_RESET_REG			0x0D01
+#define DIGITAL_AD_CHANNELS_ENABLE_REG		0x0D02
+#define DIGITAL_DA_CHANNELS_ENABLE_REG		0x0D03
+#define LOW_POWER_HS_EAR_CONF_REG		0x0D04
+#define LINE_IN_MIC_CONF_REG			0x0D05
+#define DMIC_ENABLE_REG				0x0D06
+#define ADC_DAC_ENABLE_REG			0x0D07
+#define ANALOG_OUTPUT_ENABLE_REG		0x0D08
+#define DIGITAL_OUTPUT_ENABLE_REG		0x0D09
+#define MUTE_HS_EAR_REG				0x0D0A
+#define SHORT_CIRCUIT_DISABLE_REG		0x0D0B
+#define NCP_ENABLE_HS_AUTOSTART_REG		0x0D0C
+#define ENVELOPE_THRESHOLD_REG			0x0D0D
+#define ENVELOPE_DECAY_TIME_REG			0x0D0E
+#define VIB_DRIVER_CONF_REG			0x0D0F
+#define PWM_VIBNL_CONF_REG			0x0D10
+#define PWM_VIBPL_CONF_REG			0x0D11
+#define PWM_VIBNR_CONF_REG			0x0D12
+#define PWM_VIBPR_CONF_REG			0x0D13
+#define ANALOG_MIC1_GAIN_REG			0x0D14
+#define ANALOG_MIC2_GAIN_REG			0x0D15
+#define ANALOG_HS_GAIN_REG			0x0D16
+#define ANALOG_LINE_IN_GAIN_REG			0x0D17
+#define LINE_IN_TO_HSL_GAIN_REG			0x0D18
+#define LINE_IN_TO_HSR_GAIN_REG			0x0D19
+#define AD_FILTER_CONF_REG			0x0D1A
+#define IF0_IF1_MASTER_CONF_REG			0x0D1B
+#define IF0_CONF_REG				0x0D1C
+#define TDM_IF_BYPASS_B_FIFO_REG		0x0D1D
+#define IF1_CONF_REG				0x0D1E
+#define AD_ALLOCATION_TO_SLOT0_1_REG		0x0D1F
+#define AD_ALLOCATION_TO_SLOT2_3_REG		0x0D20
+#define AD_ALLOCATION_TO_SLOT4_5_REG		0x0D21
+#define AD_ALLOCATION_TO_SLOT6_7_REG		0x0D22
+#define AD_ALLOCATION_TO_SLOT8_9_REG		0x0D23
+#define AD_ALLOCATION_TO_SLOT10_11_REG		0x0D24
+#define AD_ALLOCATION_TO_SLOT12_13_REG		0x0D25
+#define AD_ALLOCATION_TO_SLOT14_15_REG		0x0D26
+#define AD_ALLOCATION_TO_SLOT16_17_REG		0x0D27
+#define AD_ALLOCATION_TO_SLOT18_19_REG		0x0D28
+#define AD_ALLOCATION_TO_SLOT20_21_REG		0x0D29
+#define AD_ALLOCATION_TO_SLOT22_23_REG		0x0D2A
+#define AD_ALLOCATION_TO_SLOT24_25_REG		0x0D2B
+#define AD_ALLOCATION_TO_SLOT26_27_REG		0x0D2C
+#define AD_ALLOCATION_TO_SLOT28_29_REG		0x0D2D
+#define AD_ALLOCATION_TO_SLOT30_31_REG		0x0D2E
+#define AD_SLOT_0_TO_7_TRISTATE_REG		0x0D2F
+#define AD_SLOT_8_TO_15_TRISTATE_REG		0x0D30
+#define AD_SLOT_16_TO_23_TRISTATE_REG		0x0D31
+#define AD_SLOT_24_TO_31_TRISTATE_REG		0x0D32
+#define SLOT_SELECTION_TO_DA1_REG		0x0D33
+#define SLOT_SELECTION_TO_DA2_REG		0x0D34
+#define SLOT_SELECTION_TO_DA3_REG		0x0D35
+#define SLOT_SELECTION_TO_DA4_REG		0x0D36
+#define SLOT_SELECTION_TO_DA5_REG		0x0D37
+#define SLOT_SELECTION_TO_DA6_REG		0x0D38
+#define SLOT_SELECTION_TO_DA7_REG		0x0D39
+#define SLOT_SELECTION_TO_DA8_REG		0x0D3A
+#define CLASS_D_EMI_PARALLEL_CONF_REG		0x0D3B
+#define CLASS_D_PATH_CONTROL_REG		0x0D3C
+#define CLASS_D_DITHER_CONTROL_REG		0x0D3D
+#define DMIC_DECIMATOR_FILTER_REG		0x0D3E
+#define DIGITAL_MUXES_REG1			0x0D3F
+#define DIGITAL_MUXES_REG2			0x0D40
+#define AD1_DIGITAL_GAIN_REG			0x0D41
+#define AD2_DIGITAL_GAIN_REG			0x0D42
+#define AD3_DIGITAL_GAIN_REG			0x0D43
+#define AD4_DIGITAL_GAIN_REG			0x0D44
+#define AD5_DIGITAL_GAIN_REG			0x0D45
+#define AD6_DIGITAL_GAIN_REG			0x0D46
+#define DA1_DIGITAL_GAIN_REG			0x0D47
+#define DA2_DIGITAL_GAIN_REG			0x0D48
+#define DA3_DIGITAL_GAIN_REG			0x0D49
+#define DA4_DIGITAL_GAIN_REG			0x0D4A
+#define DA5_DIGITAL_GAIN_REG			0x0D4B
+#define DA6_DIGITAL_GAIN_REG			0x0D4C
+#define AD1_TO_HFL_DIGITAL_GAIN_REG		0x0D4D
+#define AD2_TO_HFR_DIGITAL_GAIN_REG		0x0D4E
+#define HSL_EAR_DIGITAL_GAIN_REG		0x0D4F
+#define HSR_DIGITAL_GAIN_REG			0x0D50
+#define SIDETONE_FIR1_GAIN_REG			0x0D51
+#define SIDETONE_FIR2_GAIN_REG			0x0D52
+#define ANC_FILTER_CONTROL_REG			0x0D53
+#define ANC_WARPED_GAIN_REG			0x0D54
+#define ANC_FIR_OUTPUT_GAIN_REG			0x0D55
+#define ANC_IIR_OUTPUT_GAIN_REG			0x0D56
+#define ANC_FIR_COEFF_MSB_REG			0x0D57
+#define ANC_FIR_COEFF_LSB_REG			0x0D58
+#define ANC_IIR_COEFF_MSB_REG			0x0D59
+#define ANC_IIR_COEFF_LSB_REG			0x0D5A
+#define ANC_WARP_DELAY_MSB_REG			0x0D5B
+#define ANC_WARP_DELAY_LSB_REG			0x0D5C
+#define ANC_FIR_PEAK_MSB_REG			0x0D5D
+#define ANC_FIR_PEAK_LSB_REG			0x0D5E
+#define ANC_IIR_PEAK_MSB_REG			0x0D5F
+#define ANC_IIR_PEAK_LSB_REG			0x0D60
+#define SIDETONE_FIR_ADDR_REG			0x0D61
+#define SIDETONE_FIR_COEFF_MSB_REG		0x0D62
+#define SIDETONE_FIR_COEFF_LSB_REG		0x0D63
+#define FILTERS_CONTROL_REG			0x0D64
+#define IRQ_MASK_LSB_REG			0x0D65
+#define IRQ_STATUS_LSB_REG			0x0D66
+#define IRQ_MASK_MSB_REG			0x0D67
+#define IRQ_STATUS_MSB_REG			0x0D68
+#define BURST_FIFO_INT_CONTROL_REG		0x0D69
+#define BURST_FIFO_LENGTH_REG			0x0D6A
+#define BURST_FIFO_CONTROL_REG			0x0D6B
+#define BURST_FIFO_SWITCH_FRAME_REG		0x0D6C
+#define BURST_FIFO_WAKE_UP_DELAY_REG		0x0D6D
+#define BURST_FIFO_SAMPLES_REG			0x0D6E
+#define REVISION_REG				0x0D6F
+
+/* POWER_UP_CONTROL_REG Masks */
+#define DEVICE_POWER_UP				0x80
+#define ANALOG_PARTS_POWER_UP			0x08
+
+/* SOFTWARE_RESET_REG Masks */
+#define SW_RESET				0x80
+
+/* DIGITAL_AD_CHANNELS_ENABLE_REG Masks */
+#define EN_AD1					0x80
+#define EN_AD2					0x80
+#define EN_AD3					0x20
+#define EN_AD4					0x20
+#define EN_AD5					0x08
+#define EN_AD6					0x04
+
+/* DIGITAL_DA_CHANNELS_ENABLE_REG Masks */
+#define EN_DA1					0x80
+#define EN_DA2					0x40
+#define EN_DA3					0x20
+#define EN_DA4					0x10
+#define EN_DA5					0x08
+#define EN_DA6					0x04
+
+/* LOW_POWER_HS_EAR_CONF_REG Masks */
+#define LOW_POWER_HS				0x80
+#define HS_DAC_DRIVER_LP			0x40
+#define HS_DAC_LP				0x20
+#define EAR_DAC_LP				0x10
+
+/* LINE_IN_MIC_CONF_REG Masks */
+#define EN_MIC1					0x80
+#define EN_MIC2					0x40
+#define EN_LIN_IN_L				0x20
+#define EN_LIN_IN_R				0x10
+#define MUT_MIC1				0x08
+#define MUT_MIC2				0x04
+#define MUT_LIN_IN_L				0x02
+#define MUT_LIN_IN_R				0x01
+
+/* DMIC_ENABLE_REG Masks */
+#define EN_DMIC1				0x80
+#define EN_DMIC2				0x40
+#define EN_DMIC3				0x20
+#define EN_DMIC4				0x10
+#define EN_DMIC5				0x08
+#define EN_DMIC6				0x04
+
+/* ADC_DAC_ENABLE_REG Masks */
+#define SEL_MIC1B_CLR_MIC1A			0x80
+#define SEL_LINR_CLR_MIC2			0x40
+#define POWER_UP_HSL_DAC			0x20
+#define POWER_UP_HSR_DAC			0x10
+#define POWER_UP_ADC1				0x04
+#define POWER_UP_ADC3				0x02
+#define POWER_UP_ADC2				0x01
+
+/* ANALOG_OUTPUT_ENABLE_REG and DIGITAL_OUTPUT_ENABLE_REG and
+					MUTE_HS_EAR_REG Masks */
+#define EN_EAR_DAC_MASK				0x04
+#define EN_HSL_DAC_MASK				0x02
+#define EN_HSR_DAC_MASK				0x01
+#define EN_EAR_MASK				0x40
+#define EN_HSL_MASK				0x20
+#define EN_HSR_MASK				0x10
+#define EN_HFL_MASK				0x08
+#define EN_HFR_MASK				0x04
+#define EN_VIBL_MASK				0x02
+#define EN_VIBR_MASK				0x01
+
+/* SHORT_CIRCUIT_DISABLE_REG Masks */
+#define HS_SHORT_DIS				0x20
+#define HS_PULL_DOWN_EN				0x10
+#define HS_OSC_EN				0x04
+#define DIS_HS_FAD				0x02
+#define HS_ZCD_DIS				0x01
+
+/* NCP_ENABLE_HS_AUTOSTART_REG Masks */
+#define EN_NEG_CP				0x80
+#define HS_AUTO_EN				0x01
+
+/* ANALOG_MIC1_GAIN_REG and ANALOG_MIC1_GAIN_REG Masks */
+#define MIC_ANALOG_GAIN_MASK			0x1F
+
+/*ANALOG_HS_GAIN_REG and ANALOG_LINE_IN_GAIN_REG Masks*/
+#define L_ANALOG_GAIN_MASK			0xF0
+#define R_ANALOG_GAIN_MASK			0x0F
+
+/* IF0_IF1_MASTER_CONF_REG Masks */
+#define EN_MASTGEN				0x80
+#define BITCLK_OSR_N_64				0x02
+#define BITCLK_OSR_N_128			0x04
+#define BITCLK_OSR_N_256			0x06
+#define EN_FSYNC_BITCLK				0x01
+#define EN_FSYNC_BITCLK1			0x10
+
+/* IF0_CONF_REG and IF1_CONF_REG Masks */
+#define FSYNC_FALLING_EDGE			0x40
+#define BITCLK_FALLING_EDGE			0x20
+#define IF_DELAYED				0x10
+#define I2S_LEFT_ALIGNED_FORMAT			0x08
+#define TDM_FORMAT				0x04
+#define WORD_LENGTH_32				0x03
+#define WORD_LENGTH_24				0x02
+#define WORD_LENGTH_20				0x01
+#define WORD_LENGTH_16				0x00
+
+/* TDM_IF_BYPASS_B_FIFO_REG Masks */
+#define IF0_BFifoEn				0x01
+#define IF0_MASTER				0x02
+
+#define IF1_MASTER				0x20
+/*
+ * AD_ALLOCATION_TO_SLOT0_1_REG and AD_ALLOCATION_TO_SLOT2_3_REG and
+ * AD_ALLOCATION_TO_SLOT4_5_REG and AD_ALLOCATION_TO_SLOT6_7_REG Masks
+ */
+#define DATA_FROM_AD_OUT1			0x00
+#define DATA_FROM_AD_OUT2			0x01
+#define DATA_FROM_AD_OUT3			0x02
+#define DATA_FROM_AD_OUT4			0x03
+#define DATA_FROM_AD_OUT5			0x04
+#define DATA_FROM_AD_OUT6			0x05
+#define DATA_FROM_AD_OUT7			0x06
+#define DATA_FROM_AD_OUT8			0x07
+#define TRISTATE				0x0C
+
+/*
+ * SLOT_SELECTION_TO_DA1_REG and SLOT_SELECTION_TO_DA2_REG and
+ * SLOT_SELECTION_TO_DA3_REG and SLOT_SELECTION_TO_DA4_REG Masks
+ * SLOT_SELECTION_TO_DA5_REG and SLOT_SELECTION_TO_DA6_REG  Masks
+ */
+#define SLOT08_FOR_DA_PATH			0x08
+#define SLOT09_FOR_DA_PATH			0x09
+#define SLOT10_FOR_DA_PATH			0x0A
+#define SLOT11_FOR_DA_PATH			0x0B
+#define SLOT12_FOR_DA_PATH			0x0C
+#define SLOT13_FOR_DA_PATH			0x0D
+#define SLOT14_FOR_DA_PATH			0x0E
+#define SLOT15_FOR_DA_PATH			0x0F
+
+/* DIGITAL_MUXES_REG1 Masks */
+#define DA1_TO_HSL				0x80
+#define DA2_TO_HSR				0x40
+#define SEL_DMIC1_FOR_AD_OUT1			0x20
+#define SEL_DMIC2_FOR_AD_OUT2			0x10
+#define SEL_DMIC3_FOR_AD_OUT3			0x08
+/*#define SEL_DMIC5_FOR_AD_OUT5			0x04*/
+/*#define SEL_DMIC6_FOR_AD_OUT6			0x02*/
+/*#define SEL_DMIC1_FOR_AD_OUT1			0x01*/
+
+/*
+ * AD1_DIGITAL_GAIN_REG and AD2_DIGITAL_GAIN_REG & AD3_DIGITAL_GAIN_REG Masks
+ * AD4_DIGITAL_GAIN_REG and AD5_DIGITAL_GAIN_REG & AD6_DIGITAL_GAIN_REG Masks
+ * DA1_DIGITAL_GAIN_REG and DA2_DIGITAL_GAIN_REG & DA3_DIGITAL_GAIN_REG Masks
+ * DA4_DIGITAL_GAIN_REG and DA5_DIGITAL_GAIN_REG & DA6_DIGITAL_GAIN_REG Masks
+ */
+#define DIS_FADING				0x40
+#define DIGITAL_GAIN_MASK			0x3F
+
+/*
+ * HSL_EAR_DIGITAL_GAIN_REG and HSR_DIGITAL_GAIN_REG Masks
+ */
+#define FADE_SPEED_MASK				0xC0
+#define DIS_DIG_GAIN_FADING			0x10
+#define HS_DIGITAL_GAIN_MASK			0x0F
+
+/* FMRx/FMTx Masks */
+#define SLOT24_FOR_DA_PATH			0x18
+#define SEL_AD_OUT8_FROM_DAIN7			0x20
+#define SLOT25_FOR_DA_PATH			0x19
+#define SEL_AD_OUT6_FROM_DAIN8			0x20
+#define SEL_IF8_FROM_AD_OUT7			0x60
+#define SEL_IF17_FROM_AD_OUT7			0x60
+#define SEL_IF16_FROM_AD_OUT8			0x07
+
+#define SEL_IF6_FROM_AD_OUT5			0x04
+#define SEL_IF7_FROM_AD_OUT6			0x50
+#define SEL_IF17_FROM_AD_OUT6			0x50
+#define SEL_AD_OUT5_FROM_DAIN7			0x20
+
+/* Burst FIFO Control Masks */
+#define WAKEUP_SIGNAL_SAMPLE_COUNT		0x1B
+#define BURST_FIFO_TRANSFER_LENGTH		0xC0
+#define BURST_FIFO_INF_RUNNING			0x01
+#define BURST_FIFO_INF_IN_MASTER_MODE		0x02
+#define PRE_BIT_CLK0_COUNT			0x1C
+#define BURST_FIFO_WAKUP_DEALAY			0x70
+
+/* Filter Control Masks */
+/* SideTone Masks */
+#define SIDETONE_DIGITAL_GAIN_MASK		0x1F
+#define FIR1_FROMAD1				0x0C
+#define FIR1_FROMAD2				0x03
+#define FIR1_FROMAD3				0x08
+#define FIR1_DAIN1				0x0C
+
+#define FIR2_FROMAD2				0x00
+#define FIR2_FROMAD3				0x01
+#define FIR2_FROMAD4				0x02
+#define FIR2_DAIN2				0x03
+
+#define FIR2_ANDFIR1AD3				0x09
+#define FIR_FILTERCONTROL			0x04
+#define APPLY_FIR_COEFFS_MASK			0x80
+
+/* IRQ status masks */
+#define NCP_READY_MASK				0x80
+
+/* AB8500 power control Masks */
+#define AB8500_VER_1_0				0x10
+#define AB8500_VER_1_1				0x11
+#define CLK_32K_OUT2_DISABLE			0x01
+#define INACTIVE_RESET_AUDIO			0x02
+#define AB8500_REQ_SYS_CLK			0x08
+#define ENABLE_AUDIO_CLK_TO_AUDIO_BLK		0x10
+#define ENABLE_VINTCORE12_SUPPLY		0x04
+#define VAMIC2_ENABLE				0x10
+#define VAMIC1_ENABLE				0x08
+#define VDMIC_ENABLE				0x04
+#define VAUDIO_ENABLE				0x02
+#define GPIO27_DIR_OUTPUT			0x04
+#define GPIO29_DIR_OUTPUT			0x10
+#define GPIO31_DIR_OUTPUT			0x40
+#define GPIO35_DIR_OUTPUT			0X04
+#endif
diff --git a/drivers/misc/audio_io_dev/ste_audio_io_core.c b/drivers/misc/audio_io_dev/ste_audio_io_core.c
new file mode 100644
index 00000000000..7f3636b9291
--- /dev/null
+++ b/drivers/misc/audio_io_dev/ste_audio_io_core.c
@@ -0,0 +1,1589 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Deepak KARDA/ deepak.karda@stericsson.com for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/regulator/consumer.h>
+#include <mach/ste_audio_io_vibrator.h>
+#include <mach/ste_audio.h>
+
+#include "ste_audio_io_core.h"
+#include "ste_audio_io_hwctrl_common.h"
+#include "ste_audio_io_ab8500_reg_defs.h"
+
+static struct audiocodec_context_t *ptr_audio_codec_cnxt;
+
+static struct clk *clk_ptr_msp1;
+static struct clk *clk_ptr_msp3;
+static struct clk *clk_ptr_audioclk;
+static struct clk *clk_ptr_sysclk;
+static struct clk *clk_ptr_ulpclk;
+
+static struct regulator *regulator_vdmic;
+static struct regulator *regulator_vaudio;
+static struct regulator *regulator_vamic1;
+static struct regulator *regulator_vamic2;
+struct regulator *regulator_avsource;
+
+static void ste_audio_io_init_transducer_cnxt(void);
+static int ste_audio_io_core_init_regulators(void);
+static int ste_audio_io_core_init_clocks(void);
+static int ste_audio_io_core_free_regulators(void);
+static int ste_audio_io_core_free_clocks(void);
+static int ste_audio_io_enable_audio_clock(void);
+static int ste_audio_io_disable_audio_clock(void);
+
+static struct transducer_context_t transducer_headset = {
+	.pwr_up_func = ste_audio_io_power_up_headset,
+	.pwr_down_func = ste_audio_io_power_down_headset,
+	.set_gain_func = ste_audio_io_set_headset_gain,
+	.get_gain_func = ste_audio_io_get_headset_gain,
+	.mute_func = ste_audio_io_mute_headset,
+	.unmute_func = ste_audio_io_unmute_headset,
+	.enable_fade_func = ste_audio_io_enable_fade_headset,
+	.disable_fade_func = ste_audio_io_disable_fade_headset,
+	.switch_to_burst_func = ste_audio_io_switch_to_burst_mode_headset,
+	.switch_to_normal_func = ste_audio_io_switch_to_normal_mode_headset
+};
+
+static struct transducer_context_t transducer_earpiece = {
+	.pwr_up_func = ste_audio_io_power_up_earpiece,
+	.pwr_down_func = ste_audio_io_power_down_earpiece,
+	.set_gain_func = ste_audio_io_set_earpiece_gain,
+	.get_gain_func = ste_audio_io_get_earpiece_gain,
+	.mute_func = ste_audio_io_mute_earpiece,
+	.unmute_func = ste_audio_io_unmute_earpiece,
+	.enable_fade_func = ste_audio_io_enable_fade_earpiece,
+	.disable_fade_func = ste_audio_io_disable_fade_earpiece
+};
+
+static struct transducer_context_t transducer_ihf = {
+	.pwr_up_func = ste_audio_io_power_up_ihf,
+	.pwr_down_func = ste_audio_io_power_down_ihf,
+	.set_gain_func = ste_audio_io_set_ihf_gain,
+	.get_gain_func = ste_audio_io_get_ihf_gain,
+	.mute_func = ste_audio_io_mute_ihf,
+	.unmute_func = ste_audio_io_unmute_ihf,
+	.enable_fade_func = ste_audio_io_enable_fade_ihf,
+	.disable_fade_func = ste_audio_io_disable_fade_ihf
+
+};
+
+static struct transducer_context_t transducer_vibl = {
+	.pwr_up_func = ste_audio_io_power_up_vibl,
+	.pwr_down_func = ste_audio_io_power_down_vibl,
+	.set_gain_func = ste_audio_io_set_vibl_gain,
+	.get_gain_func = ste_audio_io_get_vibl_gain,
+	.mute_func = ste_audio_io_mute_vibl,
+	.unmute_func = ste_audio_io_unmute_vibl,
+	.enable_fade_func = ste_audio_io_enable_fade_vibl,
+	.disable_fade_func = ste_audio_io_disable_fade_vibl
+};
+
+static struct transducer_context_t transducer_vibr = {
+	.pwr_up_func = ste_audio_io_power_up_vibr,
+	.pwr_down_func = ste_audio_io_power_down_vibr,
+	.set_gain_func = ste_audio_io_set_vibr_gain,
+	.get_gain_func = ste_audio_io_get_vibr_gain,
+	.mute_func = ste_audio_io_mute_vibr,
+	.unmute_func = ste_audio_io_unmute_vibr,
+	.enable_fade_func = ste_audio_io_enable_fade_vibr,
+	.disable_fade_func = ste_audio_io_disable_fade_vibr
+};
+
+static struct transducer_context_t transducer_mic1a = {
+	.pwr_up_func = ste_audio_io_power_up_mic1a,
+	.pwr_down_func = ste_audio_io_power_down_mic1a,
+	.set_gain_func = ste_audio_io_set_mic1a_gain,
+	.get_gain_func = ste_audio_io_get_mic1a_gain,
+	.mute_func = ste_audio_io_mute_mic1a,
+	.unmute_func = ste_audio_io_unmute_mic1a,
+	.enable_fade_func = ste_audio_io_enable_fade_mic1a,
+	.disable_fade_func = ste_audio_io_disable_fade_mic1a
+};
+
+static struct transducer_context_t transducer_mic1b = {
+	.pwr_up_func = ste_audio_io_power_up_mic1b,
+	.pwr_down_func = ste_audio_io_power_down_mic1b,
+	.set_gain_func = ste_audio_io_set_mic1a_gain,
+	.get_gain_func = ste_audio_io_get_mic1a_gain,
+	.mute_func = ste_audio_io_mute_mic1a,
+	.unmute_func = ste_audio_io_unmute_mic1a,
+	.enable_fade_func = ste_audio_io_enable_fade_mic1a,
+	.disable_fade_func = ste_audio_io_disable_fade_mic1a,
+	.enable_loop = ste_audio_io_enable_loop_mic1b,
+	.disable_loop = ste_audio_io_disable_loop_mic1b
+};
+
+static struct transducer_context_t transducer_mic2 = {
+	.pwr_up_func = ste_audio_io_power_up_mic2,
+	.pwr_down_func = ste_audio_io_power_down_mic2,
+	.set_gain_func = ste_audio_io_set_mic2_gain,
+	.get_gain_func = ste_audio_io_get_mic2_gain,
+	.mute_func = ste_audio_io_mute_mic2,
+	.unmute_func = ste_audio_io_unmute_mic2,
+	.enable_fade_func = ste_audio_io_enable_fade_mic2,
+	.disable_fade_func = ste_audio_io_disable_fade_mic2
+};
+
+static struct transducer_context_t transducer_lin = {
+	.pwr_up_func = ste_audio_io_power_up_lin,
+	.pwr_down_func = ste_audio_io_power_down_lin,
+	.set_gain_func = ste_audio_io_set_lin_gain,
+	.get_gain_func = ste_audio_io_get_lin_gain,
+	.mute_func = ste_audio_io_mute_lin,
+	.unmute_func = ste_audio_io_unmute_lin,
+	.enable_fade_func = ste_audio_io_enable_fade_lin,
+	.disable_fade_func = ste_audio_io_disable_fade_lin
+};
+
+static struct transducer_context_t transducer_dmic12 = {
+	.pwr_up_func = ste_audio_io_power_up_dmic12,
+	.pwr_down_func = ste_audio_io_power_down_dmic12,
+	.set_gain_func = ste_audio_io_set_dmic12_gain,
+	.get_gain_func = ste_audio_io_get_dmic12_gain,
+	.mute_func = ste_audio_io_mute_dmic12,
+	.unmute_func = ste_audio_io_unmute_dmic12,
+	.enable_fade_func = ste_audio_io_enable_fade_dmic12,
+	.disable_fade_func = ste_audio_io_disable_fade_dmic12,
+	.enable_loop = ste_audio_io_enable_loop_dmic12,
+	.disable_loop = ste_audio_io_disable_loop_dmic12
+};
+
+static struct transducer_context_t transducer_dmic34 = {
+	.pwr_up_func = ste_audio_io_power_up_dmic34,
+	.pwr_down_func = ste_audio_io_power_down_dmic34,
+	.set_gain_func = ste_audio_io_set_dmic34_gain,
+	.get_gain_func = ste_audio_io_get_dmic34_gain,
+	.mute_func = ste_audio_io_mute_dmic34,
+	.unmute_func = ste_audio_io_unmute_dmic34,
+	.enable_fade_func = ste_audio_io_enable_fade_dmic34,
+	.disable_fade_func = ste_audio_io_disable_fade_dmic34
+};
+
+static struct transducer_context_t transducer_dmic56 = {
+	.pwr_up_func = ste_audio_io_power_up_dmic56,
+	.pwr_down_func = ste_audio_io_power_down_dmic56,
+	.set_gain_func = ste_audio_io_set_dmic56_gain,
+	.get_gain_func = ste_audio_io_get_dmic56_gain,
+	.mute_func = ste_audio_io_mute_dmic56,
+	.unmute_func = ste_audio_io_unmute_dmic56,
+	.enable_fade_func = ste_audio_io_enable_fade_dmic56,
+	.disable_fade_func = ste_audio_io_disable_fade_dmic56,
+};
+
+static struct transducer_context_t transducer_fmrx = {
+	.pwr_up_func = ste_audio_io_power_up_fmrx,
+	.pwr_down_func = ste_audio_io_power_down_fmrx,
+};
+
+static struct transducer_context_t transducer_fmtx = {
+	.pwr_up_func = ste_audio_io_power_up_fmtx,
+	.pwr_down_func = ste_audio_io_power_down_fmtx,
+};
+
+static struct transducer_context_t transducer_bluetooth = {
+	.pwr_up_func = ste_audio_io_power_up_bluetooth,
+	.pwr_down_func = ste_audio_io_power_down_bluetooth,
+};
+
+bool ste_audio_io_core_is_ready_for_suspend()
+{
+	bool err = false;
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+	if ((!ptr_audio_codec_cnxt->power_client) &&
+			(!ptr_audio_codec_cnxt->audio_codec_powerup))
+		err = true;
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+	return err;
+}
+
+static int ste_audio_io_core_init_regulators()
+{
+	int error = 0;
+	regulator_vdmic = regulator_get(NULL, "v-dmic");
+	if (IS_ERR(regulator_vdmic)) {
+		error = PTR_ERR(regulator_vdmic);
+		dev_err(ptr_audio_codec_cnxt->dev,
+				"Register error for v-dmic=%d", error);
+		return error;
+	}
+	regulator_vamic1 = regulator_get(NULL, "v-amic1");
+	if (IS_ERR(regulator_vamic1)) {
+		error = PTR_ERR(regulator_vamic1);
+		dev_err(ptr_audio_codec_cnxt->dev,
+				"Register error for v-amic1=%d", error);
+		goto free_regulator_vdmic;
+	}
+	regulator_vamic2 = regulator_get(NULL, "v-amic2");
+	if (IS_ERR(regulator_vamic2)) {
+		error = PTR_ERR(regulator_vamic2);
+		dev_err(ptr_audio_codec_cnxt->dev,
+				"Register error for v-amic2=%d", error);
+		goto free_regulator_vdmic_vamic1;
+	}
+	regulator_vaudio = regulator_get(NULL, "v-audio");
+	if (IS_ERR(regulator_vaudio)) {
+		error = PTR_ERR(regulator_vaudio);
+		dev_err(ptr_audio_codec_cnxt->dev,
+				"Register error for v-audio=%d", error);
+		goto free_regulator_vdmic_vamic1_vamic2;
+	}
+	regulator_avsource = regulator_get(ptr_audio_codec_cnxt->dev,
+								"vcc-avswitch");
+	if (IS_ERR(regulator_avsource)) {
+		error = PTR_ERR(regulator_avsource);
+		dev_err(ptr_audio_codec_cnxt->dev,
+				"Register error for vcc-avswitch =%d", error);
+		goto free_regulator_vdmic_vamic1_vamic2_vaudio;
+	}
+	return error;
+free_regulator_vdmic_vamic1_vamic2_vaudio:
+	regulator_put(regulator_vaudio);
+free_regulator_vdmic_vamic1_vamic2:
+	regulator_put(regulator_vamic2);
+free_regulator_vdmic_vamic1:
+	regulator_put(regulator_vamic1);
+free_regulator_vdmic:
+	regulator_put(regulator_vdmic);
+	return error;
+}
+
+static int ste_audio_io_core_free_regulators()
+{
+	regulator_put(regulator_vdmic);
+	regulator_put(regulator_vamic1);
+	regulator_put(regulator_vamic2);
+	regulator_put(regulator_vaudio);
+	regulator_put(regulator_avsource);
+	return 0;
+}
+
+static int ste_audio_io_core_init_clocks()
+{
+	int error = 0;
+	clk_ptr_sysclk = clk_get(ptr_audio_codec_cnxt->dev, "sysclk");
+	if (IS_ERR(clk_ptr_sysclk)) {
+		error = -EFAULT;
+		dev_err(ptr_audio_codec_cnxt->dev,
+			"Sysclk get failed error = %d", error);
+		return error;
+	}
+	clk_ptr_ulpclk = clk_get(ptr_audio_codec_cnxt->dev, "ulpclk");
+	if (IS_ERR(clk_ptr_ulpclk)) {
+		error = -EFAULT;
+		dev_err(ptr_audio_codec_cnxt->dev,
+			"Ulpclk get failed error = %d", error);
+		goto free_sysclk;
+	}
+	clk_ptr_audioclk = clk_get(ptr_audio_codec_cnxt->dev, "audioclk");
+	if (IS_ERR(clk_ptr_audioclk)) {
+		error = -EFAULT;
+		dev_err(ptr_audio_codec_cnxt->dev,
+			"Audioclk get failed error = %d", error);
+		goto free_ulpclk;
+	}
+	return error;
+free_ulpclk:
+	clk_put(clk_ptr_ulpclk);
+free_sysclk:
+	clk_put(clk_ptr_sysclk);
+	return error;
+}
+
+static int ste_audio_io_core_free_clocks()
+{
+	clk_put(clk_ptr_audioclk);
+	clk_put(clk_ptr_ulpclk);
+	clk_put(clk_ptr_sysclk);
+	return 0;
+}
+
+int ste_audio_io_core_api_init_data(struct platform_device *pdev)
+{
+	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
+	struct ab8500_platform_data *pdata = dev_get_platdata(ab8500->dev);
+	int error = 0;
+	ptr_audio_codec_cnxt = kmalloc(sizeof(struct audiocodec_context_t),
+						GFP_KERNEL);
+	if (!ptr_audio_codec_cnxt)
+		return -ENOMEM;
+
+	memset(ptr_audio_codec_cnxt, 0, sizeof(*ptr_audio_codec_cnxt));
+	ptr_audio_codec_cnxt->dev = &pdev->dev;
+	ptr_audio_codec_cnxt->clk_type = AUDIOIO_ULP_CLK;
+	mutex_init(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	if (pdata) {
+		if (pdata->audio) {
+			ptr_audio_codec_cnxt->gpio_altf_init =
+				pdata->audio->ste_gpio_altf_init;
+			ptr_audio_codec_cnxt->gpio_altf_exit =
+				pdata->audio->ste_gpio_altf_exit;
+		}
+	}
+
+	error = ste_audio_io_core_init_regulators();
+	if (error)
+		goto free_audio_codec_cnxt;
+	error = ste_audio_io_core_init_clocks();
+	if (error)
+		goto free_audio_codec_cnxt_regulators;
+	ste_audio_io_init_transducer_cnxt();
+	return error;
+
+free_audio_codec_cnxt_regulators:
+	ste_audio_io_core_free_regulators();
+free_audio_codec_cnxt:
+	kfree(ptr_audio_codec_cnxt);
+	return error;
+}
+
+static int ste_audio_io_enable_audio_clock()
+{
+	int error = 0;
+	if (ptr_audio_codec_cnxt->is_audio_clk_enabled)
+		return 0;
+
+	if (AUDIOIO_ULP_CLK == ptr_audio_codec_cnxt->clk_type) {
+		error = clk_set_parent(clk_ptr_audioclk, clk_ptr_ulpclk);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"Setting Ulpclk as parent failed error = %d", error);
+			return error;
+		}
+	} else {
+		error = clk_set_parent(clk_ptr_audioclk, clk_ptr_sysclk);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"Setting Sysclk as parent failed error = %d", error);
+			return error;
+		}
+	}
+	error = clk_enable(clk_ptr_audioclk);
+	if (error) {
+		dev_err(ptr_audio_codec_cnxt->dev,
+			"Audioclk enable failed error = %d", error);
+		return error;
+	}
+	ptr_audio_codec_cnxt->is_audio_clk_enabled = 1;
+	return error;
+}
+
+static int ste_audio_io_disable_audio_clock()
+{
+	if (!ptr_audio_codec_cnxt->is_audio_clk_enabled)
+		return 0;
+	clk_disable(clk_ptr_audioclk);
+	ptr_audio_codec_cnxt->is_audio_clk_enabled = 0;
+	return 0;
+}
+
+static void ste_audio_io_init_transducer_cnxt(void)
+{
+	ptr_audio_codec_cnxt->transducer[HS_CH] = &transducer_headset;
+	ptr_audio_codec_cnxt->transducer[EAR_CH] = &transducer_earpiece;
+	ptr_audio_codec_cnxt->transducer[IHF_CH] = &transducer_ihf;
+	ptr_audio_codec_cnxt->transducer[VIBL_CH] = &transducer_vibl;
+	ptr_audio_codec_cnxt->transducer[VIBR_CH] = &transducer_vibr;
+	ptr_audio_codec_cnxt->transducer[MIC1A_CH] = &transducer_mic1a;
+	ptr_audio_codec_cnxt->transducer[MIC1B_CH] = &transducer_mic1b;
+	ptr_audio_codec_cnxt->transducer[MIC2_CH] = &transducer_mic2;
+	ptr_audio_codec_cnxt->transducer[LIN_CH] = &transducer_lin;
+	ptr_audio_codec_cnxt->transducer[DMIC12_CH] = &transducer_dmic12;
+	ptr_audio_codec_cnxt->transducer[DMIC34_CH] = &transducer_dmic34;
+	ptr_audio_codec_cnxt->transducer[DMIC56_CH] = &transducer_dmic56;
+	ptr_audio_codec_cnxt->transducer[FMRX_CH] = &transducer_fmrx;
+	ptr_audio_codec_cnxt->transducer[FMTX_CH] = &transducer_fmtx;
+	ptr_audio_codec_cnxt->transducer[BLUETOOTH_CH] = &transducer_bluetooth;
+}
+
+void ste_audio_io_core_api_free_data(void)
+{
+	ste_audio_io_core_free_regulators();
+	ste_audio_io_core_free_clocks();
+	kfree(ptr_audio_codec_cnxt);
+}
+
+static int ste_audio_io_core_api_enable_regulators(int channel_type)
+{
+	int error = 0;
+
+	switch (channel_type) {
+	case EAR_CH:
+	case HS_CH:
+	case IHF_CH:
+	case VIBL_CH:
+	case VIBR_CH:
+	case LIN_CH:
+	case FMRX_CH:
+	case FMTX_CH:
+	case BLUETOOTH_CH:
+		/* vaduio already enabled
+		   no additional regualtor required */
+	break;
+
+	case MIC1A_CH:
+	case MIC1B_CH:
+		error = regulator_enable(regulator_vamic1);
+		if (error)
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"unable to enable regulator vamic1 error = %d", error);
+	break;
+
+	case MIC2_CH:
+		error = regulator_enable(regulator_vamic2);
+		if (error)
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"unable to enable regulator vamic2 error = %d", error);
+	break;
+
+	case DMIC12_CH:
+	case DMIC34_CH:
+	case DMIC56_CH:
+	case MULTI_MIC_CH:
+		error = regulator_enable(regulator_vdmic);
+		if (error)
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"unable to enable regulator vdmic error = %d", error);
+	}
+	return error;
+}
+
+static int ste_audio_io_core_api_disable_regulators(int channel_type)
+{
+	int error = 0;
+
+	switch (channel_type) {
+	case EAR_CH:
+	case HS_CH:
+	case IHF_CH:
+	case VIBL_CH:
+	case VIBR_CH:
+	case LIN_CH:
+	case FMRX_CH:
+	case FMTX_CH:
+	case BLUETOOTH_CH:
+		/* no need to disable separately*/
+	break;
+
+	case MIC1A_CH:
+	case MIC1B_CH:
+		error = regulator_disable(regulator_vamic1);
+		if (error)
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"unable to disable regulator vamic1 error = %d", error);
+	break;
+
+	case MIC2_CH:
+		error = regulator_disable(regulator_vamic2);
+		if (error)
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"unable to disable regulator vamic2 error = %d", error);
+	break;
+
+	case DMIC12_CH:
+	case DMIC34_CH:
+	case DMIC56_CH:
+	case MULTI_MIC_CH:
+		error = regulator_disable(regulator_vdmic);
+		if (error)
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"unable to disable regulator vdmic error = %d", error);
+	}
+	return error;
+}
+
+int ste_audio_io_core_api_powerup_audiocodec(int power_client)
+{
+	int error = 0;
+	int acodec_device_id;
+	__u8 data, old_data;
+	/* aquire mutex */
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	acodec_device_id = abx500_get_chip_id(&ste_audio_io_device->dev);
+
+	/*
+	 * If there is no power client registered, power up
+	 * common audio blocks for audio and vibrator
+	 */
+	if (!ptr_audio_codec_cnxt->power_client) {
+		error = ste_audio_io_enable_audio_clock();
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"Unable to enable audio clock = %d", error);
+			goto err_cleanup;
+		}
+		old_data = HW_REG_READ(AB8500_CTRL3_REG);
+
+		/* Enable 32 Khz clock signal on Clk32KOut2 ball */
+		data = (~CLK_32K_OUT2_DISABLE) & old_data;
+		error = HW_REG_WRITE(AB8500_CTRL3_REG, data);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+				"enabling 32KHz clock error = %d", error);
+			goto err_cleanup;
+		}
+		data = INACTIVE_RESET_AUDIO | old_data;
+		error = HW_REG_WRITE(AB8500_CTRL3_REG, data);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"deactivate audio codec reset error = %d", error);
+			goto err_cleanup;
+		}
+		regulator_enable(regulator_vaudio);
+
+		old_data = HW_REG_READ(AB8500_GPIO_DIR4_REG);
+		data = (GPIO27_DIR_OUTPUT | GPIO29_DIR_OUTPUT |
+			GPIO31_DIR_OUTPUT) | old_data;
+		error = HW_REG_WRITE(AB8500_GPIO_DIR4_REG, data);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+				"setting gpio dir4 error = %d", error);
+			goto err_cleanup;
+		}
+		error = HW_REG_WRITE(SOFTWARE_RESET_REG, SW_RESET);
+		if (error != 0) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+				"Software reset error=%d", error);
+			goto err_cleanup;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(POWER_UP_CONTROL_REG,
+				(DEVICE_POWER_UP|ANALOG_PARTS_POWER_UP), 0);
+		if (error != 0) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+				"Device Power Up, error=%d", error);
+			goto err_cleanup;
+		}
+	}
+	/* Save information that given client already powered up audio block */
+	ptr_audio_codec_cnxt->power_client |= power_client;
+
+	/* If audio block requested power up, turn on additional audio blocks */
+	if (power_client == STE_AUDIOIO_POWER_AUDIO) {
+		if (!ptr_audio_codec_cnxt->audio_codec_powerup) {
+			clk_ptr_msp1 = clk_get_sys("msp1", NULL);
+			if (!IS_ERR(clk_ptr_msp1)) {
+				error = clk_enable(clk_ptr_msp1);
+				if (error)
+					goto err_cleanup;
+			} else {
+				error = -EFAULT;
+				goto err_cleanup;
+			}
+
+			if (AB8500_REV_20 <= acodec_device_id) {
+				clk_ptr_msp3 = clk_get_sys("msp3", NULL);
+				if (!IS_ERR(clk_ptr_msp3)) {
+					error = clk_enable(clk_ptr_msp3);
+					if (error)
+						goto err_cleanup;
+				} else {
+					error = -EFAULT;
+					goto err_cleanup;
+				}
+			}
+
+			if (ptr_audio_codec_cnxt->gpio_altf_init) {
+				error = ptr_audio_codec_cnxt->gpio_altf_init();
+				if (error)
+					goto err_cleanup;
+			}
+
+			error = HW_ACODEC_MODIFY_WRITE(IF0_IF1_MASTER_CONF_REG,
+							EN_MASTGEN, 0);
+			if (error != 0) {
+				dev_err(ptr_audio_codec_cnxt->dev,
+				"Enable Master Generator, error=%d", error);
+				goto err_cleanup;
+			}
+
+			error = HW_ACODEC_MODIFY_WRITE(TDM_IF_BYPASS_B_FIFO_REG,
+							IF0_MASTER, 0);
+			if (error != 0) {
+				dev_err(ptr_audio_codec_cnxt->dev,
+					"IF0: Master Mode, error=%d", error);
+				goto err_cleanup;
+			}
+
+			/* Configuring IF0 */
+
+			error = HW_ACODEC_MODIFY_WRITE(IF0_IF1_MASTER_CONF_REG,
+							BITCLK_OSR_N_256, 0);
+			if (error != 0) {
+				dev_err(ptr_audio_codec_cnxt->dev,
+				"IF0: Enable FsBitClk & FSync error=%d", error);
+				goto err_cleanup;
+			}
+
+			error = HW_REG_WRITE(IF0_CONF_REG, IF_DELAYED
+						| TDM_FORMAT | WORD_LENGTH_20);
+			if (error != 0) {
+				dev_err(ptr_audio_codec_cnxt->dev,
+				"IF0: TDM Format 16 Bits word length, error=%d",
+					error);
+				goto err_cleanup;
+			}
+		}
+		ptr_audio_codec_cnxt->audio_codec_powerup++;
+	}
+err_cleanup:
+	/* release mutex */
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+	return error;
+}
+
+int ste_audio_io_core_api_powerdown_audiocodec(int power_client)
+{
+	int error = 0;
+	/* aquire mutex */
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	/* Update power client status */
+	if (power_client == STE_AUDIOIO_POWER_AUDIO) {
+		ptr_audio_codec_cnxt->audio_codec_powerup--;
+		if (!ptr_audio_codec_cnxt->audio_codec_powerup) {
+			ptr_audio_codec_cnxt->power_client &= ~power_client;
+			ste_audio_io_disable_audio_clock();
+			clk_disable(clk_ptr_msp1);
+			clk_put(clk_ptr_msp1);
+			if (AB8500_REV_20 <=
+				abx500_get_chip_id(&ste_audio_io_device->dev)) {
+				clk_disable(clk_ptr_msp3);
+				clk_put(clk_ptr_msp3);
+			}
+
+			if (ptr_audio_codec_cnxt->gpio_altf_exit) {
+				error = ptr_audio_codec_cnxt->gpio_altf_exit();
+				if (error)
+					goto err_cleanup;
+			}
+		}
+	} else
+		ptr_audio_codec_cnxt->power_client &= ~power_client;
+
+	/* If no power client registered, power down audio block */
+	if (!ptr_audio_codec_cnxt->power_client) {
+		regulator_disable(regulator_vaudio);
+		ste_audio_io_disable_audio_clock();
+		if (error != 0) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+		"Device Power Down and Analog Parts Power Down error = %d ",
+							error);
+			goto err_cleanup;
+		}
+	}
+
+err_cleanup:
+	/* release mutex */
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+	return error;
+}
+/**
+ * @brief Read from AB8500 device
+ * @dev_data Pointer to the structure __audioio_data
+ * @return 0
+ */
+
+int ste_audio_io_core_api_access_read(struct audioio_data_t *dev_data)
+{
+	int reg;
+	if (NULL == dev_data)
+		return -EFAULT;
+	reg = (dev_data->block<<8)|(dev_data->addr&0xff);
+	dev_data->data = HW_REG_READ(reg);
+	return 0;
+}
+/**
+ * @brief Write on AB8500 device
+ * @dev_data Pointer to the structure __audioio_data
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_core_api_access_write(struct audioio_data_t *dev_data)
+{
+	int retval, reg;
+	if (NULL == dev_data)
+		return -EFAULT;
+
+	reg = (dev_data->block<<8)|(dev_data->addr&0xff);
+	retval = HW_REG_WRITE(reg, dev_data->data);
+
+	return retval;
+}
+/**
+ * @brief Store the power and mute status of transducer
+ * @channel_index Channel-index of transducer
+ * @ptr Array storing the status
+ * @value status being stored
+ * @return 0 on success otherwise negative error code
+ */
+
+void ste_audio_io_core_api_store_data(enum AUDIOIO_CH_INDEX channel_index,
+							int *ptr, int value)
+{
+	if (channel_index & e_CHANNEL_1)
+		ptr[0] = value;
+
+	if (channel_index & e_CHANNEL_2)
+		ptr[1] = value;
+
+	if (channel_index & e_CHANNEL_3)
+		ptr[2] = value;
+
+	if (channel_index & e_CHANNEL_4)
+		ptr[3] = value;
+}
+/**
+ * @brief Get power or mute status on a specific channel
+ * @channel_index Channel-index of the transducer
+ * @ptr Pointer to is_power_up array or is_muted array
+ * @return status of control switch
+ */
+enum AUDIOIO_COMMON_SWITCH ste_audio_io_core_api_get_status(
+				enum AUDIOIO_CH_INDEX channel_index, int *ptr)
+{
+	if (channel_index & e_CHANNEL_1) {
+		if (AUDIOIO_TRUE == ptr[0])
+			return AUDIOIO_COMMON_ON;
+		else
+			return AUDIOIO_COMMON_OFF;
+	}
+
+	if (channel_index & e_CHANNEL_2) {
+		if (AUDIOIO_TRUE == ptr[1])
+			return AUDIOIO_COMMON_ON;
+		else
+			return AUDIOIO_COMMON_OFF;
+	}
+
+	if (channel_index & e_CHANNEL_3) {
+		if (AUDIOIO_TRUE == ptr[2])
+			return AUDIOIO_COMMON_ON;
+		else
+			return AUDIOIO_COMMON_OFF;
+	}
+
+	if (channel_index & e_CHANNEL_4) {
+		if (AUDIOIO_TRUE == ptr[3])
+			return AUDIOIO_COMMON_ON;
+		else
+			return AUDIOIO_COMMON_OFF;
+	}
+	return 0;
+}
+
+int ste_audio_io_core_api_acodec_power_control(struct audioio_acodec_pwr_ctrl_t
+							*audio_acodec_pwr_ctrl)
+{
+	int error = 0;
+	if (audio_acodec_pwr_ctrl->ctrl_switch == AUDIOIO_COMMON_ON)
+		error = ste_audio_io_core_api_powerup_audiocodec(
+				STE_AUDIOIO_POWER_AUDIO);
+	else
+		error = ste_audio_io_core_api_powerdown_audiocodec(
+				STE_AUDIOIO_POWER_AUDIO);
+
+	return error;
+}
+/**
+ * @brief Control for powering on/off HW components on a specific channel
+ * @pwr_ctrl Pointer to the structure __audioio_pwr_ctrl
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_core_api_power_control_transducer(
+					struct audioio_pwr_ctrl_t *pwr_ctrl)
+{
+	int error = 0;
+	struct transducer_context_t *ptr = NULL;
+	enum AUDIOIO_CH_INDEX channel_index;
+
+	channel_index = pwr_ctrl->channel_index;
+
+	if ((pwr_ctrl->channel_type < FIRST_CH)
+			|| (pwr_ctrl->channel_type > LAST_CH))
+		return -EINVAL;
+
+	ptr = ptr_audio_codec_cnxt->transducer[pwr_ctrl->channel_type];
+
+	/* aquire mutex */
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	if (AUDIOIO_COMMON_ON == pwr_ctrl->ctrl_switch) {
+		if (ptr->pwr_up_func) {
+			error = ste_audio_io_core_api_enable_regulators(
+							pwr_ctrl->channel_type);
+			if (error)
+				goto free_mutex;
+
+			error = ptr->pwr_up_func(pwr_ctrl->channel_index,
+						ptr_audio_codec_cnxt->dev);
+			if (0 == error) {
+				ste_audio_io_core_api_store_data(channel_index,
+						ptr->is_power_up, AUDIOIO_TRUE);
+			}
+		}
+	} else {
+		if (ptr->pwr_down_func) {
+			error = ptr->pwr_down_func(pwr_ctrl->channel_index,
+						ptr_audio_codec_cnxt->dev);
+			if (0 == error) {
+				ste_audio_io_core_api_store_data(channel_index,
+					ptr->is_power_up, AUDIOIO_FALSE);
+			}
+			error = ste_audio_io_core_api_disable_regulators(
+							pwr_ctrl->channel_type);
+		}
+	}
+
+free_mutex:
+	/* release mutex */
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	return error;
+}
+/**
+ * @brief Query power state of HW path on specified channel
+ * @pwr_ctrl Pointer to the structure __audioio_pwr_ctrl
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_core_api_power_status_transducer(
+					struct audioio_pwr_ctrl_t *pwr_ctrl)
+{
+
+	struct transducer_context_t *ptr = NULL;
+	enum AUDIOIO_CH_INDEX channel_index;
+
+	channel_index = pwr_ctrl->channel_index;
+
+	if ((pwr_ctrl->channel_type < FIRST_CH)
+			|| (pwr_ctrl->channel_type > LAST_CH))
+		return -EINVAL;
+
+	ptr = ptr_audio_codec_cnxt->transducer[pwr_ctrl->channel_type];
+
+
+	/* aquire mutex */
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+
+	pwr_ctrl->ctrl_switch = ste_audio_io_core_api_get_status(channel_index,
+							ptr->is_power_up);
+
+	/* release mutex */
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	return 0;
+
+}
+
+int ste_audio_io_core_api_loop_control(struct audioio_loop_ctrl_t *loop_ctrl)
+{
+	int error = 0;
+	struct transducer_context_t *ptr = NULL;
+
+	if ((loop_ctrl->channel_type < FIRST_CH)
+			|| (loop_ctrl->channel_type > LAST_CH))
+		return -EINVAL;
+
+	ptr = ptr_audio_codec_cnxt->transducer[loop_ctrl->channel_type];
+
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	if (AUDIOIO_COMMON_ON == loop_ctrl->ctrl_switch) {
+		if (ptr->enable_loop) {
+			error = ptr->enable_loop(loop_ctrl->channel_index,
+					loop_ctrl->hw_loop,
+					loop_ctrl->loop_gain,
+					ptr_audio_codec_cnxt->dev,
+					ptr_audio_codec_cnxt->transducer);
+			if (error)
+				dev_err(ptr_audio_codec_cnxt->dev,
+			"Loop enable failed for hw loop = %d,  error = %d ",
+					(int)loop_ctrl->hw_loop, error);
+		} else {
+			error = -EFAULT;
+			dev_err(ptr_audio_codec_cnxt->dev,
+		"Hw Loop enable does not exist for channel= %d,  error = %d ",
+					(int)loop_ctrl->channel_type, error);
+		}
+	} else {
+		if (ptr->disable_loop) {
+			error = ptr->disable_loop(loop_ctrl->channel_index,
+					loop_ctrl->hw_loop,
+					ptr_audio_codec_cnxt->dev,
+					ptr_audio_codec_cnxt->transducer);
+			if (error)
+				dev_err(ptr_audio_codec_cnxt->dev,
+			"Loop disable failed for hw loop = %d,  error = %d ",
+					(int)loop_ctrl->hw_loop, error);
+		} else {
+			error = -EFAULT;
+				dev_err(ptr_audio_codec_cnxt->dev,
+		"Hw Loop disable does not exist for channel= %d,  error = %d ",
+					(int)loop_ctrl->channel_type, error);
+		}
+	}
+
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	return error;
+}
+
+int ste_audio_io_core_api_loop_status(struct audioio_loop_ctrl_t *loop_ctrl)
+{
+	return 0;
+}
+
+int ste_audio_io_core_api_get_transducer_gain_capability(
+					struct audioio_get_gain_t *get_gain)
+{
+	return 0;
+}
+
+int ste_audio_io_core_api_gain_capabilities_loop(
+					struct audioio_gain_loop_t *gain_loop)
+{
+	if ((gain_loop->channel_type < FIRST_CH)
+			|| (gain_loop->channel_type > LAST_CH))
+		return -EINVAL;
+
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	gain_loop->num_loop =
+		transducer_max_no_Of_supported_loops[gain_loop->channel_type];
+	gain_loop->max_gains = max_no_of_loop_gains[gain_loop->channel_type];
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+	return 0;
+}
+
+int ste_audio_io_core_api_supported_loops(
+				struct audioio_support_loop_t *support_loop)
+{
+	if ((support_loop->channel_type < FIRST_CH)
+			|| (support_loop->channel_type > LAST_CH))
+		return -EINVAL;
+
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+	support_loop->spprtd_loop_index =
+	transducer_no_Of_supported_loop_indexes[support_loop->channel_type];
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+	return 0;
+}
+
+int ste_audio_io_core_api_gain_descriptor_transducer(
+				struct audioio_gain_desc_trnsdr_t *gdesc_trnsdr)
+{
+	return 0;
+}
+/**
+ * @brief Control for muting a specific channel in HW
+ * @mute_trnsdr Pointer to the structure __audioio_mute_trnsdr
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_core_api_mute_control_transducer(
+				struct audioio_mute_trnsdr_t *mute_trnsdr)
+{
+	int error = 0;
+	struct transducer_context_t *ptr = NULL;
+	enum AUDIOIO_CH_INDEX channel_index;
+
+	channel_index = mute_trnsdr->channel_index;
+
+	if ((mute_trnsdr->channel_type < FIRST_CH)
+			|| (mute_trnsdr->channel_type > LAST_CH))
+		return -EINVAL;
+
+	ptr = ptr_audio_codec_cnxt->transducer[mute_trnsdr->channel_type];
+
+	/* aquire mutex */
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	if (AUDIOIO_COMMON_ON == mute_trnsdr->ctrl_switch) {
+		if (ptr->mute_func) {
+			error = ptr->mute_func(mute_trnsdr->channel_index,
+						ptr_audio_codec_cnxt->dev);
+		    if (0 == error) {
+				ste_audio_io_core_api_store_data(channel_index ,
+						ptr->is_muted, AUDIOIO_TRUE);
+			}
+		}
+	} else {
+		if (ptr->unmute_func) {
+			if (0 == ptr->unmute_func(channel_index, ptr->gain,
+						ptr_audio_codec_cnxt->dev)) {
+				ste_audio_io_core_api_store_data(channel_index,
+						ptr->is_muted, AUDIOIO_FALSE);
+			}
+		}
+	}
+
+	/* release mutex */
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	return error;
+}
+/**
+ * @brief Query state of mute on specified channel
+ * @mute_trnsdr Pointer to the structure __audioio_mute_trnsdr
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_core_api_mute_status_transducer(
+				struct audioio_mute_trnsdr_t *mute_trnsdr)
+{
+	struct transducer_context_t *ptr = NULL;
+	enum AUDIOIO_CH_INDEX channel_index;
+
+	channel_index = mute_trnsdr->channel_index;
+
+	if ((mute_trnsdr->channel_type < FIRST_CH)
+			|| (mute_trnsdr->channel_type > LAST_CH))
+		return -EINVAL;
+
+	ptr = ptr_audio_codec_cnxt->transducer[mute_trnsdr->channel_type];
+
+	/* aquire mutex */
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	mute_trnsdr->ctrl_switch = ste_audio_io_core_api_get_status(
+				channel_index, ptr->is_muted);
+	/* release mutex */
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	return 0;
+}
+/**
+ * @brief control the fading on the transducer called on.
+ * @fade_ctrl Pointer to the structure __audioio_fade_ctrl
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_core_api_fading_control(struct audioio_fade_ctrl_t *fade_ctrl)
+{
+	int error = 0;
+	struct transducer_context_t *ptr = NULL;
+
+	if ((fade_ctrl->channel_type < FIRST_CH)
+			|| (fade_ctrl->channel_type > LAST_CH))
+		return -EINVAL;
+	ptr = ptr_audio_codec_cnxt->transducer[fade_ctrl->channel_type];
+
+	/* aquire mutex */
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	if (AUDIOIO_COMMON_ON == fade_ctrl->ctrl_switch)
+		error = ptr->enable_fade_func(ptr_audio_codec_cnxt->dev);
+
+	else
+		error = ptr->disable_fade_func(ptr_audio_codec_cnxt->dev);
+
+
+	/* release mutex */
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	return error;
+}
+/**
+ * @brief control the low power mode of headset.
+ * @burst_ctrl Pointer to the structure __audioio_burst_ctrl
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_core_api_burstmode_control(
+					struct audioio_burst_ctrl_t *burst_ctrl)
+{
+	int error = 0;
+	struct transducer_context_t *ptr = NULL;
+	int burst_fifo_switch_frame;
+
+	burst_fifo_switch_frame = burst_ctrl->burst_fifo_switch_frame;
+
+	if ((burst_ctrl->channel_type < FIRST_CH)
+			|| (burst_ctrl->channel_type > LAST_CH))
+		return -EINVAL;
+	ptr = ptr_audio_codec_cnxt->transducer[burst_ctrl->channel_type];
+
+	/* aquire mutex */
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	if (AUDIOIO_COMMON_ON == burst_ctrl->ctrl_switch) {
+		if (ptr->switch_to_burst_func)
+			error = ptr->switch_to_burst_func(
+					burst_fifo_switch_frame,
+						ptr_audio_codec_cnxt->dev);
+	} else
+			if (ptr->switch_to_normal_func)
+				error = ptr->switch_to_normal_func(
+						ptr_audio_codec_cnxt->dev);
+	/* release mutex */
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	return error;
+}
+/**
+ * @brief Convert channel index to array index
+ * @channel_index Channel Index of transducer
+ * @return Array index corresponding to the specified channel index
+ */
+
+int convert_channel_index_to_array_index(enum AUDIOIO_CH_INDEX channel_index)
+{
+	if (channel_index & e_CHANNEL_1)
+		return 0;
+	else if (channel_index & e_CHANNEL_2)
+		return 1;
+	else if (channel_index & e_CHANNEL_3)
+		return 2;
+	else
+		return 3;
+}
+
+/**
+ * @brief Set individual gain along the HW path of a specified channel
+ * @gctrl_trnsdr Pointer to the structure __audioio_gain_ctrl_trnsdr
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_core_api_gain_control_transducer(
+				struct audioio_gain_ctrl_trnsdr_t *gctrl_trnsdr)
+{
+	struct transducer_context_t *ptr = NULL;
+	enum AUDIOIO_CH_INDEX channel_index;
+	int ch_array_index;
+	u16 gain_index;
+	int gain_value;
+	u32 linear;
+	int channel_type;
+	int error;
+	int min_gain, max_gain, gain;
+
+	if ((gctrl_trnsdr->channel_type < FIRST_CH)
+			|| (gctrl_trnsdr->channel_type > LAST_CH))
+		return -EINVAL;
+
+	if (gctrl_trnsdr->gain_index >= MAX_NO_GAINS)
+		return -EINVAL;
+
+	ptr = ptr_audio_codec_cnxt->transducer[gctrl_trnsdr->channel_type];
+	channel_index = gctrl_trnsdr->channel_index;
+	gain_index = gctrl_trnsdr->gain_index;
+	gain_value = gctrl_trnsdr->gain_value;
+	linear = gctrl_trnsdr->linear;
+	channel_type = gctrl_trnsdr->channel_type;
+
+	ch_array_index = convert_channel_index_to_array_index(channel_index);
+	if (linear) {	/* Gain is in the range 0 to 100 */
+		min_gain = gain_descriptor[channel_type]\
+				[ch_array_index][gain_index].min_gain;
+		max_gain = gain_descriptor[channel_type]\
+				[ch_array_index][gain_index].max_gain;
+
+		gain = ((gain_value * (max_gain - min_gain))/100) + min_gain;
+	} else
+		/* Convert to db */
+		gain = gain_value/100;
+
+	gain_value = gain;
+
+#if 1
+	if (gain_index >= transducer_no_of_gains[channel_type])
+		return -EINVAL;
+
+	if (gain_value < gain_descriptor[channel_type]\
+				[ch_array_index][gain_index].min_gain)
+		return -EINVAL;
+
+	if (gain_value > gain_descriptor[channel_type]\
+				[ch_array_index][gain_index].max_gain)
+		return -EINVAL;
+
+#endif
+
+	/* aquire mutex */
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	error = ptr->set_gain_func(channel_index,
+					gain_index, gain_value, linear,
+					ptr_audio_codec_cnxt->dev);
+	if (0 == error)
+		ste_audio_io_core_api_store_data(channel_index ,
+							ptr->gain, gain_value);
+
+
+	/* release mutex */
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	return error;
+}
+/**
+ * @brief Get individual gain along the HW path of a specified channel
+ * @gctrl_trnsdr Pointer to the structure __audioio_gain_ctrl_trnsdr
+ * @return 0 on success otherwise negative error code
+ */
+
+
+int ste_audio_io_core_api_gain_query_transducer(
+				struct audioio_gain_ctrl_trnsdr_t *gctrl_trnsdr)
+{
+	struct transducer_context_t *ptr = NULL;
+	enum AUDIOIO_CH_INDEX channel_index;
+	u16 gain_index;
+	u32 linear;
+	int left_volume, right_volume;
+	int max_gain, min_gain;
+	int ch_array_index;
+
+	if ((gctrl_trnsdr->channel_type < FIRST_CH)
+			|| (gctrl_trnsdr->channel_type > LAST_CH))
+		return -EINVAL;
+
+	if (gctrl_trnsdr->gain_index >= MAX_NO_GAINS)
+		return -EINVAL;
+
+	ptr = ptr_audio_codec_cnxt->transducer[gctrl_trnsdr->channel_type];
+
+	channel_index = gctrl_trnsdr->channel_index;
+	gain_index = gctrl_trnsdr->gain_index;
+	linear = gctrl_trnsdr->linear;
+
+	ptr->get_gain_func(&left_volume, &right_volume, gain_index,
+						ptr_audio_codec_cnxt->dev);
+
+	ch_array_index = convert_channel_index_to_array_index(channel_index);
+	max_gain = gain_descriptor[gctrl_trnsdr->channel_type]\
+				[ch_array_index][gain_index].max_gain;
+	min_gain = gain_descriptor[gctrl_trnsdr->channel_type]\
+				[ch_array_index][gain_index].min_gain;
+
+	switch (channel_index) {
+	case e_CHANNEL_1:
+		gctrl_trnsdr->gain_value = linear ? \
+		min_gain+left_volume*(max_gain-min_gain)/100 : left_volume;
+		break;
+	case e_CHANNEL_2:
+		gctrl_trnsdr->gain_value = linear ? \
+		min_gain+right_volume*(max_gain-min_gain)/100 : right_volume;
+		break;
+	case e_CHANNEL_3:
+		break;
+	case e_CHANNEL_4:
+		break;
+	case e_CHANNEL_ALL:
+		if (left_volume == right_volume) {
+			if (linear)
+				gctrl_trnsdr->gain_value =
+				min_gain+right_volume*(max_gain-min_gain)/100;
+			else
+				gctrl_trnsdr->gain_value  = right_volume;
+			}
+	}
+
+	return 0;
+}
+
+
+int ste_audio_io_core_api_fsbitclk_control(
+			struct audioio_fsbitclk_ctrl_t *fsbitclk_ctrl)
+{
+	int error = 0;
+
+	if (AUDIOIO_COMMON_ON == fsbitclk_ctrl->ctrl_switch)
+		error = HW_ACODEC_MODIFY_WRITE(IF0_IF1_MASTER_CONF_REG,
+							EN_FSYNC_BITCLK, 0);
+	else
+		error = HW_ACODEC_MODIFY_WRITE(IF0_IF1_MASTER_CONF_REG, 0,
+							EN_FSYNC_BITCLK);
+
+	return error;
+}
+int ste_audio_io_core_api_pseudoburst_control(
+			struct audioio_pseudoburst_ctrl_t *pseudoburst_ctrl)
+{
+	int error = 0;
+
+	return error;
+}
+int ste_audio_io_core_debug(int x)
+{
+	debug_audioio(x);
+
+return 0;
+}
+
+/**
+ * ste_audioio_vibrator_alloc()
+ * @client:		Client id which allocates vibrator
+ * @mask:		Mask against which vibrator usage is checked
+ *
+ * This function allocates vibrator.
+ * Mask is added here as audioio driver controls left and right vibrator
+ * separately (can work independently). In case when audioio has allocated
+ * one of its channels (left or right) it should be still able to allocate
+ * the other channel.
+ *
+ * Returns:
+ *	0 - Success
+ *	-EBUSY - other client already registered
+ **/
+int ste_audioio_vibrator_alloc(int client, int mask)
+{
+	int error = 0;
+
+	/* Check if other client is already using vibrator */
+	if (ptr_audio_codec_cnxt->vibra_client & ~mask)
+		error = -EBUSY;
+	else
+		ptr_audio_codec_cnxt->vibra_client |= client;
+
+	return error;
+}
+
+/**
+ * ste_audioio_vibrator_release()
+ * @client:	     Client id which releases vibrator
+ *
+ * This function releases vibrator
+ **/
+void ste_audioio_vibrator_release(int client)
+{
+	ptr_audio_codec_cnxt->vibra_client &= ~client;
+}
+
+/**
+ * ste_audioio_vibrator_pwm_control()
+ * @client:		Client id which will use vibrator
+ * @left_speed:		Left vibrator speed
+ * @right_speed:	Right vibrator speed
+ *
+ * This function controls vibrator using PWM source
+ *
+ * Returns:
+ *      0 - success
+ *	-EBUSY - Vibrator already used
+ **/
+int ste_audioio_vibrator_pwm_control(
+				int client,
+				struct ste_vibra_speed left_speed,
+				struct ste_vibra_speed right_speed)
+{
+	int error = 0;
+
+	mutex_lock(&ptr_audio_codec_cnxt->audio_io_mutex);
+
+	/* Try to allocate vibrator for given client */
+	error = ste_audioio_vibrator_alloc(client, client);
+
+	mutex_unlock(&ptr_audio_codec_cnxt->audio_io_mutex);
+
+	if (error)
+		return error;
+
+	/* Duty cycle supported by vibrator's PWM is 0-100 */
+	if (left_speed.positive > STE_AUDIOIO_VIBRATOR_MAX_SPEED)
+		left_speed.positive = STE_AUDIOIO_VIBRATOR_MAX_SPEED;
+
+	if (right_speed.positive > STE_AUDIOIO_VIBRATOR_MAX_SPEED)
+		right_speed.positive = STE_AUDIOIO_VIBRATOR_MAX_SPEED;
+
+	if (left_speed.negative > STE_AUDIOIO_VIBRATOR_MAX_SPEED)
+		left_speed.negative = STE_AUDIOIO_VIBRATOR_MAX_SPEED;
+
+	if (right_speed.negative > STE_AUDIOIO_VIBRATOR_MAX_SPEED)
+		right_speed.negative = STE_AUDIOIO_VIBRATOR_MAX_SPEED;
+
+	if (left_speed.negative || right_speed.negative ||
+		left_speed.positive || right_speed.positive) {
+		/* Power up audio block for vibrator */
+		error = ste_audio_io_core_api_powerup_audiocodec(
+						STE_AUDIOIO_POWER_VIBRA);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+				"Audio power up failed %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG,
+					(EN_VIBL_MASK|EN_VIBR_MASK), 0);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+				"Powerup Vibrator Class-D driver %d",
+				error);
+			return error;
+		}
+
+		error = HW_REG_WRITE(VIB_DRIVER_CONF_REG, 0xff);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+				"Enable Vibrator PWM generator %d",
+				error);
+			return error;
+		}
+	}
+
+	error = HW_REG_WRITE(PWM_VIBNL_CONF_REG, left_speed.negative);
+	if (error) {
+		dev_err(ptr_audio_codec_cnxt->dev,
+			"Write Left Vibrator negative PWM %d", error);
+		goto err_cleanup;
+	}
+
+	error = HW_REG_WRITE(PWM_VIBPL_CONF_REG, left_speed.positive);
+	if (error) {
+		dev_err(ptr_audio_codec_cnxt->dev,
+			"Write Left Vibrator positive PWM %d", error);
+		goto err_cleanup;
+	}
+
+	error = HW_REG_WRITE(PWM_VIBNR_CONF_REG, right_speed.negative);
+	if (error) {
+		dev_err(ptr_audio_codec_cnxt->dev,
+			"Write Right Vibrator negative PWM %d", error);
+		goto err_cleanup;
+	}
+
+	error = HW_REG_WRITE(PWM_VIBPR_CONF_REG, right_speed.positive);
+	if (error) {
+		dev_err(ptr_audio_codec_cnxt->dev,
+			"Write Right Vibrator positive PWM %d", error);
+		goto err_cleanup;
+	}
+
+	if (!left_speed.negative && !right_speed.negative &&
+		!left_speed.positive && !right_speed.positive) {
+		error = HW_REG_WRITE(VIB_DRIVER_CONF_REG, 0);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+				"Disable PWM Vibrator generator %d",
+				error);
+			goto err_cleanup;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG,
+					0, (EN_VIBL_MASK|EN_VIBR_MASK));
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+				"Power down Vibrator Class-D driver %d",
+				error);
+			goto err_cleanup;
+		}
+
+		/* Power down audio block */
+		error = ste_audio_io_core_api_powerdown_audiocodec(
+				STE_AUDIOIO_POWER_VIBRA);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+				"Audio power down failed %d", error);
+			goto err_cleanup;
+		}
+	}
+
+err_cleanup:
+	/* Release client */
+	if (!left_speed.negative && !right_speed.negative &&
+		!left_speed.positive && !right_speed.positive) {
+		mutex_lock(&ptr_audio_codec_cnxt->audio_io_mutex);
+		ste_audioio_vibrator_release(client);
+		mutex_unlock(&ptr_audio_codec_cnxt->audio_io_mutex);
+	}
+	return error;
+}
+EXPORT_SYMBOL(ste_audioio_vibrator_pwm_control);
+
+/**
+ * @brief This function sets FIR coefficients
+ * @fir_coeffs: pointer to structure audioio_fir_coefficients_t
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_core_api_fir_coeffs_control(struct audioio_fir_coefficients_t
+						*fir_coeffs)
+{
+	unsigned char coefficient;
+	int i, error;
+
+	if (fir_coeffs->start_addr >= STE_AUDIOIO_MAX_COEFFICIENTS)
+		return -EINVAL;
+
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+
+	error = HW_REG_WRITE(SIDETONE_FIR_ADDR_REG, fir_coeffs->start_addr);
+	if (error) {
+		dev_err(ptr_audio_codec_cnxt->dev,
+			"FIR start address write failed %d", error);
+			goto err_cleanup;
+	}
+
+	for (i = fir_coeffs->start_addr;
+				i < STE_AUDIOIO_MAX_COEFFICIENTS; i++) {
+
+		coefficient = (fir_coeffs->coefficients[i]>>8) & 0xff;
+		error = HW_REG_WRITE(SIDETONE_FIR_COEFF_MSB_REG, coefficient);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"FIR coefficient [%d] msb write failed %d", i, error);
+			goto err_cleanup;
+		}
+
+		coefficient = fir_coeffs->coefficients[i] & 0xff;
+		error = HW_REG_WRITE(SIDETONE_FIR_COEFF_LSB_REG, coefficient);
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"FIR coefficient [%d] lsb write failed %d", i, error);
+			goto err_cleanup;
+		}
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(SIDETONE_FIR_ADDR_REG,
+						APPLY_FIR_COEFFS_MASK, 0);
+	if (error) {
+		dev_err(ptr_audio_codec_cnxt->dev,
+			"FIR coefficients activation failed %d", error);
+			goto err_cleanup;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(FILTERS_CONTROL_REG,
+						FIR_FILTERCONTROL, 0);
+	if (error) {
+		dev_err(ptr_audio_codec_cnxt->dev,
+			"ST FIR Filters enable failed %d", error);
+			goto err_cleanup;
+	}
+
+err_cleanup:
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+	return error;
+}
+
+/**
+ * @brief This function sets and enable clock
+ * @clk_type: pointer to structure audioio_clk_select_t
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_core_clk_select_control(struct audioio_clk_select_t
+						*clk_type)
+{
+	int error = 0;
+	mutex_lock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+	if (ptr_audio_codec_cnxt->clk_type != clk_type->required_clk) {
+		/* disable running clk*/
+		ste_audio_io_disable_audio_clock();
+		/* assign required clk*/
+		ptr_audio_codec_cnxt->clk_type = clk_type->required_clk;
+		/* enable required clk*/
+		error = ste_audio_io_enable_audio_clock();
+		if (error) {
+			dev_err(ptr_audio_codec_cnxt->dev,
+			"Clock enabled failed = %d", error);
+			goto err_cleanup;
+		}
+	}
+err_cleanup:
+	mutex_unlock(&(ptr_audio_codec_cnxt->audio_io_mutex));
+	return error;
+}
+
diff --git a/drivers/misc/audio_io_dev/ste_audio_io_core.h b/drivers/misc/audio_io_dev/ste_audio_io_core.h
new file mode 100644
index 00000000000..44849f1d5e8
--- /dev/null
+++ b/drivers/misc/audio_io_dev/ste_audio_io_core.h
@@ -0,0 +1,138 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Deepak KARDA/ deepak.karda@stericsson.com for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#ifndef _AUDIOIO_CORE_H_
+#define _AUDIOIO_CORE_H_
+
+#include <mach/ste_audio_io_ioctl.h>
+#include "ste_audio_io_func.h"
+#include "ste_audio_io_hwctrl_common.h"
+
+#define MAX_NO_CHANNELS			4
+
+#define STE_AUDIOIO_POWER_AUDIO		1
+#define STE_AUDIOIO_POWER_VIBRA		2
+
+struct transducer_context_t {
+	/* public variables */
+	int gain[MAX_NO_CHANNELS];
+	int is_muted[MAX_NO_CHANNELS];
+	int is_power_up[MAX_NO_CHANNELS];
+	/* public funcs */
+	int (*pwr_up_func)(enum AUDIOIO_CH_INDEX, struct device *);
+	int (*pwr_down_func)(enum AUDIOIO_CH_INDEX, struct device *);
+	int (*pwr_state_func)(struct device *);
+	int (*set_gain_func)(enum AUDIOIO_CH_INDEX, u16, int, u32,
+						struct device *);
+	int (*get_gain_func)(int *, int *, u16, struct device *);
+	int (*mute_func)(enum AUDIOIO_CH_INDEX, struct device *);
+	int (*unmute_func)(enum AUDIOIO_CH_INDEX, int *, struct device *);
+	int (*mute_state_func)(struct device *);
+	int (*enable_fade_func)(struct device *);
+	int (*disable_fade_func)(struct device *);
+	int (*switch_to_burst_func)(int, struct device *);
+	int (*switch_to_normal_func)(struct device *);
+	int (*enable_loop)(enum AUDIOIO_CH_INDEX, enum AUDIOIO_HAL_HW_LOOPS,
+				int, struct device *, void *);
+	int (*disable_loop)(enum AUDIOIO_CH_INDEX, enum AUDIOIO_HAL_HW_LOOPS,
+						struct device *, void *);
+};
+
+struct audiocodec_context_t {
+	int	audio_codec_powerup;
+	int	is_audio_clk_enabled;
+	enum	AUDIOIO_CLK_TYPE clk_type;
+	int	power_client;
+	int	vibra_client;
+	struct mutex audio_io_mutex;
+	struct mutex vibrator_mutex;
+	struct transducer_context_t *transducer[MAX_NO_TRANSDUCERS];
+	struct device *dev;
+	int (*gpio_altf_init) (void);
+	int (*gpio_altf_exit) (void);
+};
+
+
+int ste_audio_io_core_api_access_read(struct audioio_data_t *dev_data);
+
+int ste_audio_io_core_api_access_write(struct audioio_data_t *dev_data);
+
+int ste_audio_io_core_api_power_control_transducer(
+			struct audioio_pwr_ctrl_t *pwr_ctrl);
+
+int ste_audio_io_core_api_power_status_transducer(
+					struct audioio_pwr_ctrl_t *pwr_ctrl);
+
+int ste_audio_io_core_api_loop_control(struct audioio_loop_ctrl_t *loop_ctrl);
+
+int ste_audio_io_core_api_loop_status(struct audioio_loop_ctrl_t *loop_ctrl);
+
+int ste_audio_io_core_api_get_transducer_gain_capability(
+		struct audioio_get_gain_t *get_gain);
+
+int ste_audio_io_core_api_gain_capabilities_loop(
+				struct audioio_gain_loop_t *gain_loop);
+
+int ste_audio_io_core_api_supported_loops(
+				struct audioio_support_loop_t *support_loop);
+
+int ste_audio_io_core_api_gain_descriptor_transducer(
+	struct audioio_gain_desc_trnsdr_t *gdesc_trnsdr);
+
+int ste_audio_io_core_api_gain_control_transducer(
+	struct audioio_gain_ctrl_trnsdr_t *gctrl_trnsdr);
+
+int ste_audio_io_core_api_gain_query_transducer(
+	struct audioio_gain_ctrl_trnsdr_t *gctrl_trnsdr);
+
+int ste_audio_io_core_api_mute_control_transducer(
+	struct audioio_mute_trnsdr_t *mute_trnsdr);
+
+int ste_audio_io_core_api_mute_status_transducer(
+	struct audioio_mute_trnsdr_t *mute_trnsdr);
+
+int ste_audio_io_core_api_fading_control(struct audioio_fade_ctrl_t *fade_ctrl);
+
+int ste_audio_io_core_api_burstmode_control(
+				struct audioio_burst_ctrl_t *burst_ctrl);
+
+int ste_audio_io_core_api_powerup_audiocodec(int power_client);
+
+int ste_audio_io_core_api_powerdown_audiocodec(int power_client);
+
+int ste_audio_io_core_api_init_data(struct platform_device *pdev);
+
+bool ste_audio_io_core_is_ready_for_suspend(void);
+void ste_audio_io_core_api_free_data(void);
+
+int ste_audio_io_core_api_fsbitclk_control(
+				struct audioio_fsbitclk_ctrl_t *fsbitclk_ctrl);
+int ste_audio_io_core_api_pseudoburst_control(
+		struct audioio_pseudoburst_ctrl_t *pseudoburst_ctrl);
+
+void ste_audio_io_core_api_store_data(enum AUDIOIO_CH_INDEX channel_index,
+							int *ptr, int value);
+
+int ste_audioio_vibrator_alloc(int client, int mask);
+
+void ste_audioio_vibrator_release(int client);
+
+enum AUDIOIO_COMMON_SWITCH ste_audio_io_core_api_get_status(
+				enum AUDIOIO_CH_INDEX channel_index, int *ptr);
+
+int ste_audio_io_core_api_acodec_power_control(struct audioio_acodec_pwr_ctrl_t
+							*audio_acodec_pwr_ctrl);
+
+int ste_audio_io_core_api_fir_coeffs_control(struct audioio_fir_coefficients_t
+						*fir_coeffs);
+
+int ste_audio_io_core_clk_select_control(struct audioio_clk_select_t
+						*clk_type);
+
+int ste_audio_io_core_debug(int x);
+
+#endif /* _AUDIOIO_CORE_H_ */
+
diff --git a/drivers/misc/audio_io_dev/ste_audio_io_dev.c b/drivers/misc/audio_io_dev/ste_audio_io_dev.c
new file mode 100644
index 00000000000..edea31e4315
--- /dev/null
+++ b/drivers/misc/audio_io_dev/ste_audio_io_dev.c
@@ -0,0 +1,759 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Deepak KARDA/ deepak.karda@stericsson.com for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/ioctl.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/cdev.h>
+#include <linux/uaccess.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/miscdevice.h>
+#include "ste_audio_io_dev.h"
+
+#define STR_DEBUG_ON		"debug on"
+#define AUDIOIO_DEVNAME		"ab8500-codec"
+
+static int ste_audio_io_open(struct inode *inode, struct file *filp);
+static int ste_audio_io_release(struct inode *inode, struct file *filp);
+static long ste_audio_io_ioctl(struct file *file, unsigned int cmd,
+							unsigned long arg);
+static int ste_audio_io_cmd_parser(unsigned int cmd, unsigned long arg);
+static ssize_t ste_audio_io_write(struct file *filp,
+		const char __user *buf, size_t count, loff_t *f_pos);
+
+
+/**
+ * @brief Check IOCTL type, command no and access direction
+ * @ inode  value corresponding to the file descriptor
+ * @file	 value corresponding to the file descriptor
+ * @cmd	IOCTL command code
+ * @arg	Command argument
+ * @return 0 on success otherwise negative error code
+ */
+static long ste_audio_io_ioctl(struct file *file, unsigned int cmd,
+							unsigned long arg)
+{
+	int retval = 0;
+	int err = 0;
+
+	/* Check type and command number */
+	if (_IOC_TYPE(cmd) != AUDIOIO_IOC_MAGIC)
+		return -ENOTTY;
+
+	/* IOC_DIR is from the user perspective, while access_ok is
+	 * from the kernel perspective; so they look reversed.
+	 */
+	if (_IOC_DIR(cmd) & _IOC_READ)
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg,
+							_IOC_SIZE(cmd));
+	if (err == 0 && _IOC_DIR(cmd) & _IOC_WRITE)
+		err = !access_ok(VERIFY_READ, (void __user *)arg,
+							_IOC_SIZE(cmd));
+	if (err)
+		return -EFAULT;
+
+	retval = ste_audio_io_cmd_parser(cmd, arg);
+
+	return retval;
+}
+/**
+ * @brief IOCTL call to read the value from AB8500 device
+ * @cmd IOCTL command code
+ * @arg  Command argument
+ * @return 0 on success otherwise negative error code
+ */
+
+static int process_read_register_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_data_t *audio_dev_data;
+
+	audio_dev_data = (struct audioio_data_t *)&cmd_data;
+
+	if (copy_from_user(audio_dev_data, (void __user *)arg,
+						sizeof(struct audioio_data_t)))
+		return -EFAULT;
+
+	retval =  ste_audio_io_core_api_access_read(audio_dev_data);
+	if (0 != retval)
+		return retval;
+
+	if (copy_to_user((void __user *)arg, audio_dev_data,
+						sizeof(struct audioio_data_t)))
+		return -EFAULT;
+	return 0;
+}
+/**
+ * @brief IOCTL call to write the given value to the AB8500 device
+ * @cmd IOCTL command code
+ * @arg  Command argument
+ * @return 0 on success otherwise negative error code
+ */
+
+static int process_write_register_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_data_t	*audio_dev_data;
+
+	audio_dev_data = (struct audioio_data_t *)&cmd_data;
+
+	if (copy_from_user(audio_dev_data, (void __user *)arg,
+						sizeof(struct audioio_data_t)))
+		return  -EFAULT;
+
+	retval = ste_audio_io_core_api_access_write(audio_dev_data);
+
+	return retval;
+}
+/**
+ * @brief IOCTL call to control the power on/off of hardware components
+ * @cmd IOCTL command code
+ * @arg  Command argument
+ * @return 0 on success otherwise negative error code
+ */
+
+static int process_pwr_ctrl_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_pwr_ctrl_t *audio_pwr_ctrl;
+
+	audio_pwr_ctrl = (struct audioio_pwr_ctrl_t *)&cmd_data;
+
+	if (copy_from_user(audio_pwr_ctrl, (void __user *)arg,
+					sizeof(struct audioio_pwr_ctrl_t)))
+		return -EFAULT;
+
+	retval = ste_audio_io_core_api_power_control_transducer(audio_pwr_ctrl);
+
+	return retval;
+}
+
+static int process_pwr_sts_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_pwr_ctrl_t *audio_pwr_sts;
+
+	audio_pwr_sts = (struct audioio_pwr_ctrl_t *)&cmd_data;
+
+	if (copy_from_user(audio_pwr_sts, (void __user *)arg,
+					sizeof(struct audioio_pwr_ctrl_t)))
+		return  -EFAULT;
+
+	retval = ste_audio_io_core_api_power_status_transducer(audio_pwr_sts);
+	if (0 != retval)
+		return retval;
+
+	if (copy_to_user((void __user *)arg, audio_pwr_sts,
+					sizeof(struct audioio_pwr_ctrl_t)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int process_lp_ctrl_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_loop_ctrl_t *audio_lp_ctrl;
+
+	audio_lp_ctrl = (struct audioio_loop_ctrl_t *)&cmd_data;
+
+	if (copy_from_user(audio_lp_ctrl, (void __user *)arg,
+					sizeof(struct audioio_loop_ctrl_t)))
+		return -EFAULT;
+
+	retval =  ste_audio_io_core_api_loop_control(audio_lp_ctrl);
+
+	return retval;
+}
+
+static int process_lp_sts_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_loop_ctrl_t *audio_lp_sts;
+
+	audio_lp_sts = (struct audioio_loop_ctrl_t *)&cmd_data;
+
+
+	if (copy_from_user(audio_lp_sts, (void __user *)arg,
+					sizeof(struct audioio_loop_ctrl_t)))
+		return  -EFAULT;
+
+	retval = ste_audio_io_core_api_loop_status(audio_lp_sts);
+	if (0 != retval)
+		return retval;
+
+	if (copy_to_user((void __user *)arg, audio_lp_sts,
+					sizeof(struct audioio_loop_ctrl_t)))
+		return -EFAULT;
+	return 0;
+}
+
+static int process_get_trnsdr_gain_capability_cmd(unsigned int cmd,
+							unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_get_gain_t *audio_trnsdr_gain;
+
+	audio_trnsdr_gain = (struct audioio_get_gain_t *)&cmd_data;
+
+	if (copy_from_user(audio_trnsdr_gain, (void __user *)arg,
+					sizeof(struct audioio_get_gain_t)))
+		return -EFAULT;
+
+	retval = ste_audio_io_core_api_get_transducer_gain_capability(
+							audio_trnsdr_gain);
+	if (0 != retval)
+		return retval;
+
+	if (copy_to_user((void __user *)arg, audio_trnsdr_gain,
+					sizeof(struct audioio_get_gain_t)))
+		return -EFAULT;
+	return 0;
+}
+
+static int process_gain_cap_loop_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_gain_loop_t *audio_gain_loop;
+
+	audio_gain_loop = (struct audioio_gain_loop_t *)&cmd_data;
+
+	if (copy_from_user(audio_gain_loop, (void __user *)arg,
+					sizeof(struct audioio_gain_loop_t)))
+		return  -EFAULT;
+
+	retval = ste_audio_io_core_api_gain_capabilities_loop(audio_gain_loop);
+	if (0 != retval)
+		return retval;
+
+	if (copy_to_user((void __user *)arg, audio_gain_loop,
+					sizeof(struct audioio_gain_loop_t)))
+		return -EFAULT;
+	return 0;
+}
+
+
+static int process_support_loop_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_support_loop_t *audio_spprt_loop;
+
+	audio_spprt_loop = (struct audioio_support_loop_t *)&cmd_data;
+
+	if (copy_from_user(audio_spprt_loop, (void __user *)arg,
+					sizeof(struct audioio_support_loop_t)))
+		return  -EFAULT;
+
+	retval = ste_audio_io_core_api_supported_loops(audio_spprt_loop);
+	if (0 != retval)
+		return retval;
+
+	if (copy_to_user((void __user *)arg, audio_spprt_loop,
+					sizeof(struct audioio_support_loop_t)))
+		return -EFAULT;
+	return 0;
+}
+
+
+static int process_gain_desc_trnsdr_cmd(unsigned int cmd, unsigned long arg)
+
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_gain_desc_trnsdr_t *audio_gain_desc;
+
+	audio_gain_desc = (struct audioio_gain_desc_trnsdr_t *)&cmd_data;
+
+	if (copy_from_user(audio_gain_desc, (void __user *)arg,
+				sizeof(struct audioio_gain_desc_trnsdr_t)))
+		return  -EFAULT;
+
+	retval = ste_audio_io_core_api_gain_descriptor_transducer(
+					audio_gain_desc);
+	if (0 != retval)
+		return retval;
+
+	if (copy_to_user((void __user *)arg, audio_gain_desc,
+				sizeof(struct audioio_gain_desc_trnsdr_t)))
+		return -EFAULT;
+	return 0;
+}
+
+
+static int process_gain_ctrl_trnsdr_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_gain_ctrl_trnsdr_t *audio_gain_ctrl;
+
+	audio_gain_ctrl = (struct audioio_gain_ctrl_trnsdr_t *)&cmd_data;
+
+	if (copy_from_user(audio_gain_ctrl, (void __user *)arg,
+				sizeof(struct audioio_gain_ctrl_trnsdr_t)))
+		return -EFAULT;
+
+	retval = ste_audio_io_core_api_gain_control_transducer(
+					audio_gain_ctrl);
+
+	return retval;
+}
+
+static int process_gain_query_trnsdr_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_gain_ctrl_trnsdr_t *audio_gain_query;
+
+	audio_gain_query = (struct audioio_gain_ctrl_trnsdr_t *)&cmd_data;
+
+	if (copy_from_user(audio_gain_query, (void __user *)arg,
+				sizeof(struct audioio_gain_ctrl_trnsdr_t)))
+		return -EFAULT;
+
+	retval = ste_audio_io_core_api_gain_query_transducer(audio_gain_query);
+	if (0 != retval)
+		return retval;
+
+	if (copy_to_user((void __user *)arg, audio_gain_query,
+				sizeof(struct audioio_gain_ctrl_trnsdr_t)))
+		return -EFAULT;
+	return 0;
+}
+
+static int process_mute_ctrl_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_mute_trnsdr_t *audio_mute_ctrl;
+
+	audio_mute_ctrl = (struct audioio_mute_trnsdr_t *)&cmd_data;
+	if (copy_from_user(audio_mute_ctrl , (void __user *)arg,
+					sizeof(struct audioio_mute_trnsdr_t)))
+		return -EFAULT;
+
+	retval = ste_audio_io_core_api_mute_control_transducer(
+						audio_mute_ctrl);
+
+	return retval;
+}
+
+static int process_mute_sts_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_mute_trnsdr_t *audio_mute_sts;
+
+	audio_mute_sts = (struct audioio_mute_trnsdr_t *)&cmd_data;
+
+	if (copy_from_user(audio_mute_sts, (void __user *)arg,
+				sizeof(struct audioio_mute_trnsdr_t)))
+		return -EFAULT;
+
+	retval = ste_audio_io_core_api_mute_status_transducer(audio_mute_sts);
+	if (0 != retval)
+		return retval;
+
+	if (copy_to_user((void __user *)arg, audio_mute_sts,
+				sizeof(struct audioio_mute_trnsdr_t)))
+		return -EFAULT;
+	return 0;
+}
+
+static int process_fade_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_fade_ctrl_t *audio_fade;
+	audio_fade = (struct audioio_fade_ctrl_t *)&cmd_data;
+
+	if (copy_from_user(audio_fade , (void __user *)arg,
+					sizeof(struct audioio_fade_ctrl_t)))
+		return -EFAULT;
+
+	retval =  ste_audio_io_core_api_fading_control(audio_fade);
+
+	return retval;
+}
+
+static int process_burst_ctrl_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_burst_ctrl_t *audio_burst;
+
+	audio_burst = (struct audioio_burst_ctrl_t  *)&cmd_data;
+	if (copy_from_user(audio_burst , (void __user *)arg,
+					sizeof(struct audioio_burst_ctrl_t)))
+		return -EFAULT;
+
+	retval = ste_audio_io_core_api_burstmode_control(audio_burst);
+
+	return retval;
+
+	return 0;
+}
+
+static int process_fsbitclk_ctrl_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_fsbitclk_ctrl_t *audio_fsbitclk;
+
+	audio_fsbitclk = (struct audioio_fsbitclk_ctrl_t  *)&cmd_data;
+
+	if (copy_from_user(audio_fsbitclk , (void __user *)arg,
+				sizeof(struct audioio_fsbitclk_ctrl_t)))
+		return -EFAULT;
+
+	retval = ste_audio_io_core_api_fsbitclk_control(audio_fsbitclk);
+
+	return retval;
+
+	return 0;
+
+}
+
+static int process_pseudoburst_ctrl_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_pseudoburst_ctrl_t *audio_pseudoburst;
+
+	audio_pseudoburst = (struct audioio_pseudoburst_ctrl_t  *)&cmd_data;
+
+	if (copy_from_user(audio_pseudoburst , (void __user *)arg,
+				sizeof(struct audioio_pseudoburst_ctrl_t)))
+		return -EFAULT;
+
+	retval = ste_audio_io_core_api_pseudoburst_control(audio_pseudoburst);
+
+	return retval;
+
+	return 0;
+
+}
+static int process_audiocodec_pwr_ctrl_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	union audioio_cmd_data_t cmd_data;
+	struct audioio_acodec_pwr_ctrl_t *audio_acodec_pwr_ctrl;
+	audio_acodec_pwr_ctrl = (struct audioio_acodec_pwr_ctrl_t *)&cmd_data;
+	if (copy_from_user(audio_acodec_pwr_ctrl, (void __user *)arg,
+				sizeof(struct audioio_acodec_pwr_ctrl_t)))
+		return -EFAULT;
+	retval = ste_audio_io_core_api_acodec_power_control(
+							audio_acodec_pwr_ctrl);
+	return retval;
+}
+
+static int process_fir_coeffs_ctrl_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval;
+	struct audioio_fir_coefficients_t *cmd_data;
+	cmd_data = kmalloc(sizeof(struct audioio_fir_coefficients_t),
+								GFP_KERNEL);
+	if (!cmd_data)
+		return -ENOMEM;
+	if (copy_from_user(cmd_data, (void __user *)arg,
+				sizeof(struct audioio_fir_coefficients_t))) {
+		kfree(cmd_data);
+		return -EFAULT;
+	}
+	retval = ste_audio_io_core_api_fir_coeffs_control(cmd_data);
+	kfree(cmd_data);
+	return retval;
+}
+
+static int process_clk_select_cmd(unsigned int cmd, unsigned long arg)
+{
+	int retval;
+	struct audioio_clk_select_t *cmd_data;
+	cmd_data = kmalloc(sizeof(struct audioio_clk_select_t),
+								GFP_KERNEL);
+	if (!cmd_data)
+		return -ENOMEM;
+	if (copy_from_user(cmd_data, (void __user *)arg,
+				sizeof(struct audioio_clk_select_t))) {
+		kfree(cmd_data);
+		return -EFAULT;
+	}
+	retval = ste_audio_io_core_clk_select_control(cmd_data);
+	kfree(cmd_data);
+	return retval;
+}
+
+static int ste_audio_io_cmd_parser(unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+
+	switch (cmd) {
+	case AUDIOIO_READ_REGISTER:
+		retval = process_read_register_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_WRITE_REGISTER:
+		retval = process_write_register_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_PWR_CTRL_TRNSDR:
+		retval = process_pwr_ctrl_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_PWR_STS_TRNSDR:
+		retval = process_pwr_sts_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_LOOP_CTRL:
+		 retval = process_lp_ctrl_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_LOOP_STS:
+		 retval = process_lp_sts_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_GET_TRNSDR_GAIN_CAPABILITY:
+		retval = process_get_trnsdr_gain_capability_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_GAIN_CAP_LOOP:
+		retval = process_gain_cap_loop_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_SUPPORT_LOOP:
+		retval = process_support_loop_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_GAIN_DESC_TRNSDR:
+		retval = process_gain_desc_trnsdr_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_GAIN_CTRL_TRNSDR:
+		retval = process_gain_ctrl_trnsdr_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_GAIN_QUERY_TRNSDR:
+		retval = process_gain_query_trnsdr_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_MUTE_CTRL_TRNSDR:
+		retval = process_mute_ctrl_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_MUTE_STS_TRNSDR:
+		retval = process_mute_sts_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_FADE_CTRL:
+		retval = process_fade_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_BURST_CTRL:
+		retval = process_burst_ctrl_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_FSBITCLK_CTRL:
+		retval = process_fsbitclk_ctrl_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_PSEUDOBURST_CTRL:
+		retval = process_pseudoburst_ctrl_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_AUDIOCODEC_PWR_CTRL:
+		retval = process_audiocodec_pwr_ctrl_cmd(cmd, arg);
+	break;
+
+	case AUDIOIO_FIR_COEFFS_CTRL:
+		retval = process_fir_coeffs_ctrl_cmd(cmd, arg);
+		break;
+	case AUDIOIO_CLK_SELECT_CTRL:
+		retval = process_clk_select_cmd(cmd, arg);
+		break;
+	}
+	return retval;
+}
+
+static int ste_audio_io_open(struct inode *inode, struct file *filp)
+{
+	if (!try_module_get(THIS_MODULE))
+		return -ENODEV;
+	return 0;
+}
+
+static int ste_audio_io_release(struct inode *inode, struct file *filp)
+{
+	module_put(THIS_MODULE);
+	return 0;
+}
+
+static ssize_t ste_audio_io_write(struct file *filp,
+		const char __user *buf, size_t count, loff_t *f_pos)
+{
+	char *x = kmalloc(count, GFP_KERNEL);
+	int debug_flag = 0;
+
+	if (copy_from_user(x, buf, count))
+		return  -EFAULT;
+
+	if (count >= strlen(STR_DEBUG_ON)) {
+
+		if (!strncmp(STR_DEBUG_ON, x, strlen(STR_DEBUG_ON)))
+			debug_flag = 1;
+		else
+			debug_flag = 0;
+	}
+
+	ste_audio_io_core_debug(debug_flag);
+
+	kfree(x);
+
+	return count;
+}
+
+static const struct file_operations ste_audio_io_fops = {
+	.owner   = THIS_MODULE,
+	.unlocked_ioctl   = ste_audio_io_ioctl,
+	.open    = ste_audio_io_open,
+	.release = ste_audio_io_release,
+	.write   = ste_audio_io_write,
+};
+
+/**
+ * audio_io_misc_dev - Misc device config for audio_io
+ */
+static struct miscdevice audio_io_misc_dev = {
+	MISC_DYNAMIC_MINOR,
+	"audioio",
+	&ste_audio_io_fops
+};
+
+/**
+ * ste_audio_io_probe() - probe the device
+ * @pdev:	pointer to the platform device structure
+ *
+ * This funtion is called after the driver is registered to platform
+ * device framework. It does allocate the memory for the internal
+ * data structure and intialized core APIs.
+ */
+static int ste_audio_io_drv_probe(struct platform_device *pdev)
+{
+	int error;
+
+	ste_audio_io_device = pdev;
+
+	dev_dbg(&ste_audio_io_device->dev, "ste_audio_io device probe\n");
+
+	error = misc_register(&audio_io_misc_dev);
+	if (error) {
+		printk(KERN_WARNING "%s: registering misc device failed\n",
+		       __func__);
+		return error;
+	}
+
+	error = ste_audio_io_core_api_init_data(ste_audio_io_device);
+	if (error < 0) {
+		dev_err(&ste_audio_io_device->dev,
+			"ste_audioio_core_api_init_data failed err = %d",
+			error);
+		goto ste_audio_io_misc_deregister;
+	}
+	return 0;
+
+ste_audio_io_misc_deregister:
+	misc_deregister(&audio_io_misc_dev);
+	return error;
+}
+
+/**
+ * ste_audio_io_remove() - Removes the device
+ * @pdev:	pointer to the platform_device structure
+ *
+ * This function is called when this mnodule is removed using rmmod
+ */
+static int ste_audio_io_drv_remove(struct platform_device *pdev)
+{
+	ste_audio_io_core_api_free_data();
+	misc_deregister(&audio_io_misc_dev);
+	return 0;
+}
+
+/**
+ * ste_audio_io_drv_suspend - suspend audio_io
+ * @pdev:       platform data
+ * @state:	power down level
+ */
+static int ste_audio_io_drv_suspend(struct platform_device *pdev,
+				    pm_message_t state)
+{
+	if (ste_audio_io_core_is_ready_for_suspend())
+		return 0;
+	else
+		return -EINVAL;
+}
+
+/**
+ * ste_audio_io_drv_resume - put back audio_io in the normal state
+ * @pdev:       platform data
+ */
+static int ste_audio_io_drv_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+
+/**
+ * struct audio_io_driver: audio_io platform structure
+ * @probe:	The probe funtion to be called
+ * @remove:	The remove funtion to be called
+ * @resume:	The resume function to be called
+ * @suspend:	The suspend function to be called
+ * @driver:	The driver data
+ */
+static struct platform_driver ste_audio_io_driver = {
+	.probe = ste_audio_io_drv_probe,
+	.remove = ste_audio_io_drv_remove,
+	.driver = {
+		.name = AUDIOIO_DEVNAME,
+		.owner = THIS_MODULE,
+	},
+	.suspend = ste_audio_io_drv_suspend,
+	.resume = ste_audio_io_drv_resume,
+};
+
+/** Pointer to platform device needed to access abx500 core functions */
+struct platform_device *ste_audio_io_device;
+
+static int __init ste_audio_io_init(void)
+{
+	return  platform_driver_register(&ste_audio_io_driver);
+}
+module_init(ste_audio_io_init);
+
+static void __exit ste_audio_io_exit(void)
+{
+	platform_driver_unregister(&ste_audio_io_driver);
+}
+module_exit(ste_audio_io_exit);
+
+MODULE_AUTHOR("Deepak KARDA <deepak.karda@stericsson.com>");
+MODULE_DESCRIPTION("STE_AUDIO_IO");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/misc/audio_io_dev/ste_audio_io_dev.h b/drivers/misc/audio_io_dev/ste_audio_io_dev.h
new file mode 100644
index 00000000000..bcb9dce3ad2
--- /dev/null
+++ b/drivers/misc/audio_io_dev/ste_audio_io_dev.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Deepak KARDA/ deepak.karda@stericsson.com for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#ifndef _AUDIOIO_DEV_H_
+#define _AUDIOIO_DEV_H_
+
+#include <mach/ste_audio_io_ioctl.h>
+#include "ste_audio_io_core.h"
+
+union audioio_cmd_data_t {
+	struct audioio_burst_ctrl_t  audioio_burst_ctrl;
+	struct audioio_fade_ctrl_t  audioio_fade_ctrl;
+	struct audioio_mute_trnsdr_t  audioio_mute_trnsdr;
+	struct audioio_gain_ctrl_trnsdr_t  audioio_gain_ctrl_trnsdr;
+	struct audioio_gain_desc_trnsdr_t  audioio_gain_desc_trnsdr;
+	struct audioio_support_loop_t  audioio_support_loop;
+	struct audioio_gain_loop_t  audioio_gain_loop;
+	struct audioio_get_gain_t  audioio_get_gain;
+	struct audioio_loop_ctrl_t  audioio_loop_ctrl;
+	struct audioio_pwr_ctrl_t  audioio_pwr_ctrl;
+	struct audioio_data_t  audioio_data;
+	struct audioio_fsbitclk_ctrl_t audioio_fsbitclk_ctrl;
+	struct audioio_acodec_pwr_ctrl_t audioio_acodec_pwr_ctrl;
+	struct audioio_pseudoburst_ctrl_t audioio_pseudoburst_ctrl;
+};
+
+
+#endif /* _AUDIOIO_DEV_H_ */
+
diff --git a/drivers/misc/audio_io_dev/ste_audio_io_func.c b/drivers/misc/audio_io_dev/ste_audio_io_func.c
new file mode 100644
index 00000000000..7238085938e
--- /dev/null
+++ b/drivers/misc/audio_io_dev/ste_audio_io_func.c
@@ -0,0 +1,4371 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Deepak KARDA/ deepak.karda@stericsson.com for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#include <linux/gpio.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/jiffies.h>
+#include <mach/ste_audio_io_vibrator.h>
+#include <mach/ste_audio.h>
+#include "ste_audio_io_func.h"
+#include "ste_audio_io_core.h"
+#include "ste_audio_io_ab8500_reg_defs.h"
+#include "ste_audio_io_hwctrl_common.h"
+
+static struct clk *clk_ptr_msp0;
+static int bluetooth_power_up_count;
+static int  acodec_reg_dump;
+
+#define NCP_TIMEOUT		200 /* 200 ms */
+/*
+ * TODO: Use proper register defines instead of home-made generic ones.
+ */
+#define SHIFT_QUARTET0  0
+#define SHIFT_QUARTET1  4
+#define MASK_QUARTET    (0xFUL)
+#define MASK_QUARTET1   (MASK_QUARTET << SHIFT_QUARTET1)
+#define MASK_QUARTET0   (MASK_QUARTET << SHIFT_QUARTET0)
+
+/**
+ * @brief Modify the specified register
+ * @reg Register
+ * @mask_set Bit to be set
+ * @mask_clear Bit to be cleared
+ * @return 0 on success otherwise negative error code
+ */
+
+unsigned int ab8500_acodec_modify_write(unsigned int reg, u8 mask_set,
+								u8 mask_clear)
+{
+	u8 value8, retval = 0;
+	value8 = HW_REG_READ(reg);
+	/* clear the specified bit */
+	value8 &= ~mask_clear;
+	/* set the asked bit */
+	value8 |= mask_set;
+	retval = HW_REG_WRITE(reg, value8);
+	return retval;
+}
+
+/**
+ * @brief Power up headset on a specific channel
+ * @channel_index Channel-index of headset
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_power_up_headset(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_DA = 0;
+	unsigned long end_time;
+
+	/* Check if HS PowerUp request is mono or Stereo channel */
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "HS should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	ste_audio_io_mute_headset(channel_index, dev);
+
+	error = HW_ACODEC_MODIFY_WRITE(NCP_ENABLE_HS_AUTOSTART_REG,
+								HS_AUTO_EN, 0);
+	if (0 != error) {
+		dev_err(dev, "NCP fully controlled with EnCpHs bit %d", error);
+		return error;
+	}
+	error = HW_ACODEC_MODIFY_WRITE(NCP_ENABLE_HS_AUTOSTART_REG,
+					(EN_NEG_CP|HS_AUTO_EN), 0);
+	if (0 != error) {
+		dev_err(dev, "Enable Negative Charge Pump %d", error);
+		return error;
+	}
+
+	/* Wait for negative charge pump to start */
+	end_time = jiffies + msecs_to_jiffies(NCP_TIMEOUT);
+	while (!(HW_REG_READ(IRQ_STATUS_MSB_REG) & NCP_READY_MASK)
+				&& time_after_eq(end_time, jiffies)) {
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_timeout(1);
+	}
+
+	if (!(HW_REG_READ(IRQ_STATUS_MSB_REG) & NCP_READY_MASK)) {
+		error = -EFAULT;
+		dev_err(dev, "Negative Charge Pump start error % d", error);
+		return error;
+	}
+
+	/* Enable DA1 for HSL */
+	if (channel_index & e_CHANNEL_1) {
+
+		/* Power Up HSL driver */
+		error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG,
+							EN_HSL_MASK, 0);
+		if (0 != error) {
+			dev_err(dev, "Power Up HSL Driver %d", error);
+			return error;
+		}
+
+		initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+
+		if (EN_DA1 & initialVal_DA)
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA1_REG,
+							SLOT08_FOR_DA_PATH, 0);
+		if (0 != error) {
+			dev_err(dev, "Data sent to DA1 from Slot 08 %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1,
+								DA1_TO_HSL, 0);
+		if (0 != error) {
+			dev_err(dev,
+				"DA_IN1 path mixed with sidetone FIR %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+								EN_DA1, 0);
+		if (0 != error) {
+			dev_err(dev, "Power up HSL %d ", error);
+			return error;
+		}
+
+		/* Power Up HSL DAC driver */
+		error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG,
+							POWER_UP_HSL_DAC, 0);
+		if (0 != error) {
+			dev_err(dev, "Power Up HSL DAC driver %d", error);
+			return error;
+		}
+
+		/* Power up HSL DAC and digital path */
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG,
+							EN_HSL_MASK, 0);
+		if (0 != error) {
+			dev_err(dev,
+				"Power up HSL DAC and digital path %d",
+									error);
+			return error;
+		}
+
+		/*
+		* Disable short detection. Pull Down output to ground,
+		* Use local oscillator, Gain change without zero cross control
+		*/
+		error = HW_ACODEC_MODIFY_WRITE(SHORT_CIRCUIT_DISABLE_REG,
+			HS_SHORT_DIS|HS_PULL_DOWN_EN|HS_OSC_EN|HS_ZCD_DIS, 0);
+		if (0 != error) {
+			dev_err(dev, "Disable short detection."
+			"Pull Down output to ground,Use local oscillator,Gain"
+			"change without zero cross control %d", error);
+			return error;
+		}
+	}
+
+	/* Enable DA2 for HSR */
+	if (channel_index & e_CHANNEL_2) {
+
+		/* Power Up HSR driver */
+		error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG,
+							EN_HSR_MASK, 0);
+		if (0 != error) {
+			dev_err(dev, "Power Up HSR Driver %d", error);
+			return error;
+		}
+
+		initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+		if (EN_DA2 & initialVal_DA)
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA2_REG,
+							SLOT09_FOR_DA_PATH, 0);
+		if (0 != error) {
+			dev_err(dev,
+				"Data sent to DA2 from Slot 09 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1, DA2_TO_HSR,
+									0);
+		if (0 != error) {
+			dev_err(dev,
+				"DA_IN2 path mixed with sidetone FIR %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+								EN_DA2, 0);
+		if (0 != error) {
+			dev_err(dev, "Power up HSR %d ", error);
+			return error;
+		}
+
+					/* Power Up HSR DAC driver */
+		error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG,
+							POWER_UP_HSR_DAC, 0);
+		if (0 != error) {
+			dev_err(dev, "Power Up HSR DAC driver %d", error);
+			return error;
+		}
+
+			/* Power up HSR DAC and digital path */
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG,
+							EN_HSR_MASK, 0);
+		if (0 != error) {
+			dev_err(dev,
+				"Power up HSR DAC and digital path %d",
+									error);
+			return error;
+		}
+
+	/*
+	* TEST START .havent cleared the bits in power down.Disable short
+	* detection. Pull Down output to ground, Use local oscillator,
+	* Gain change without zero cross control
+	*/
+
+		error = HW_ACODEC_MODIFY_WRITE(SHORT_CIRCUIT_DISABLE_REG,
+			HS_SHORT_DIS|HS_PULL_DOWN_EN|HS_OSC_EN|HS_ZCD_DIS, 0);
+		if (0 != error) {
+			dev_err(dev, "Disable short detection."
+			"Pull Down output to ground, Use local oscillator,"
+			"Gain change without zero cross control %d", error);
+			return error;
+		}
+	/* TEST END */
+	}
+	ste_audio_io_unmute_headset(channel_index, 0, dev);
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+
+/**
+ * @brief Power down headset on a specific channel
+ * @channel_index Channel-index of headset
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_power_down_headset(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_DA = 0;
+	unsigned long end_time;
+
+		/* Check if HS Power Down request is mono or Stereo channel */
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "HS should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	/* Disable Negative Charge Pump */
+	error = HW_ACODEC_MODIFY_WRITE(NCP_ENABLE_HS_AUTOSTART_REG,
+						(EN_NEG_CP|HS_AUTO_EN), 0);
+	if (0 != error) {
+		dev_err(dev, "NCP not fully controlled with EnCpHs bit %d",
+								error);
+		return error;
+	}
+	error = HW_ACODEC_MODIFY_WRITE(NCP_ENABLE_HS_AUTOSTART_REG, 0,
+						EN_NEG_CP);
+	if (0 != error) {
+		dev_err(dev, "Disable Negative Charge Pump %d", error);
+		return error;
+	}
+
+	/* Wait for negative charge pump to stop */
+	end_time = jiffies + msecs_to_jiffies(NCP_TIMEOUT);
+	while ((HW_REG_READ(IRQ_STATUS_MSB_REG) & NCP_READY_MASK)
+				&& time_after_eq(end_time, jiffies)) {
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_timeout(1);
+	}
+
+	if (HW_REG_READ(IRQ_STATUS_MSB_REG) & NCP_READY_MASK) {
+		error = -EFAULT;
+		dev_err(dev, "Negative Charge Pump stop error % d", error);
+		return error;
+	}
+
+	if (channel_index & e_CHANNEL_1) {
+		initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+		if (!(initialVal_DA & EN_DA1))
+			return 0;
+
+		/* Power Down HSL driver */
+		error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG, 0,
+								EN_HSL_MASK);
+		if (0 != error) {
+			dev_err(dev, "Power down HSL Driver %d", error);
+			return error;
+		}
+
+		/* Power Down HSL DAC driver */
+		error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, 0,
+							POWER_UP_HSL_DAC);
+		if (0 != error) {
+			dev_err(dev, "Power Up HSL DAC Driver %d", error);
+			return error;
+		}
+
+		/* Power Down HSL DAC and digital path */
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG, 0,
+								EN_HSL_MASK);
+		if (0 != error) {
+			dev_err(dev,
+				"Power down HSL DAC and digital path %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+						0, EN_DA1);
+		if (0 != error) {
+			dev_err(dev, "Disable DA1 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1,
+					0, DA1_TO_HSL);
+		if (0 != error) {
+			dev_err(dev,
+			"Clear DA_IN1 path mixed with sidetone FIR %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA1_REG, 0,
+							SLOT08_FOR_DA_PATH);
+		if (0 != error) {
+			dev_err(dev,
+				"Data sent to DA1 cleared from Slot 08 %d",
+									error);
+			return error;
+		}
+
+
+	}
+	/* Enable DA2 for HSR */
+
+	if (channel_index & e_CHANNEL_2) {
+		initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+		if (!(initialVal_DA & EN_DA2))
+			return 0;
+
+		/* Power Down HSR driver */
+		error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG, 0,
+								EN_HSR_MASK);
+		if (0 != error) {
+			dev_err(dev, "Power down HSR Driver %d", error);
+			return error;
+		}
+
+		/* Power Down HSR DAC driver */
+		error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, 0,
+							POWER_UP_HSR_DAC);
+		if (0 != error) {
+			dev_err(dev, "Power down HSR DAC Driver %d", error);
+			return error;
+		}
+
+			/* Power Down HSR DAC and digital path */
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG, 0,
+								EN_HSR_MASK);
+		if (0 != error) {
+			dev_err(dev,
+				"Power down HSR DAC and digital path %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+								0, EN_DA2);
+		if (0 != error) {
+			dev_err(dev, "Disable DA2 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1, 0,
+								DA2_TO_HSR);
+		if (0 != error) {
+			dev_err(dev,
+			"Clear DA_IN2 path mixed with sidetone FIR %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA2_REG, 0,
+							SLOT09_FOR_DA_PATH);
+		if (0 != error) {
+			dev_err(dev,
+				"Data sent to DA2 cleared from Slot 09 %d",
+									error);
+			return error;
+		}
+
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+
+/**
+ * @brief Mute headset on a specific channel
+ * @channel_index Headeset channel-index
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_mute_headset(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+		/* Check if HS Mute request is mono or Stereo channel */
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "HS should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	if (channel_index & e_CHANNEL_1) {
+		/* Mute HSL */
+		error = HW_ACODEC_MODIFY_WRITE(MUTE_HS_EAR_REG,
+					       EN_HSL_MASK | EN_HSL_DAC_MASK,
+					       0);
+		if (0 != error) {
+			dev_err(dev, "Mute HSL %d", error);
+			return error;
+		}
+	}
+
+	if (channel_index & e_CHANNEL_2) {
+		/* Mute HSR */
+		error = HW_ACODEC_MODIFY_WRITE(MUTE_HS_EAR_REG,
+					       EN_HSR_MASK | EN_HSR_DAC_MASK,
+					       0);
+		if (0 != error) {
+			dev_err(dev, "Mute HSR %d", error);
+			return error;
+		}
+	}
+
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+
+/**
+ * @brief Unmute headset on a specific channel
+ * @channel_index Headeset channel-index
+ * @gain Gain index of headset
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_unmute_headset(enum AUDIOIO_CH_INDEX channel_index, int *gain,
+					struct device *dev)
+{
+	int error = 0;
+
+	/* Check if HS UnMute request is mono or Stereo channel */
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "HS should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	if (channel_index & e_CHANNEL_1) {
+		/* UnMute HSL */
+		error = HW_ACODEC_MODIFY_WRITE(MUTE_HS_EAR_REG, 0,
+					       EN_HSL_MASK | EN_HSL_DAC_MASK);
+		if (0 != error) {
+			dev_err(dev, "UnMute HSL %d", error);
+			return error;
+		}
+	}
+
+	if (channel_index & e_CHANNEL_2) {
+		/* UnMute HSR */
+		error = HW_ACODEC_MODIFY_WRITE(MUTE_HS_EAR_REG, 0,
+					       EN_HSR_MASK | EN_HSR_DAC_MASK);
+		if (0 != error) {
+			dev_err(dev, "UnMute HSR %d", error);
+			return error;
+		}
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+
+/**
+ * @brief Enables fading of headset on a specific channel
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_enable_fade_headset(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(SHORT_CIRCUIT_DISABLE_REG,
+						0, DIS_HS_FAD);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for HS %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DA1_DIGITAL_GAIN_REG, 0, DIS_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for HSL %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(HSL_EAR_DIGITAL_GAIN_REG, 0,
+							DIS_DIG_GAIN_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for Digital Gain of HSL %d",
+									error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DA2_DIGITAL_GAIN_REG, 0, DIS_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for HSR %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(HSR_DIGITAL_GAIN_REG, 0,
+							DIS_DIG_GAIN_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for Digital Gain of HSR %d",
+									error);
+		return error;
+	}
+
+	return error;
+}
+/**
+ * @brief Disables fading of headset on a specific channel
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_disable_fade_headset(struct device *dev)
+{
+	int error = 0;
+	error = HW_ACODEC_MODIFY_WRITE(SHORT_CIRCUIT_DISABLE_REG,
+						DIS_HS_FAD, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for HS %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DA1_DIGITAL_GAIN_REG, DIS_FADING, 0);
+	if  (0 != error) {
+		dev_err(dev, "Disable fading for HSL %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(HSL_EAR_DIGITAL_GAIN_REG,
+						DIS_DIG_GAIN_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for Digital Gain of HSL %d",
+									error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DA2_DIGITAL_GAIN_REG, DIS_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for HSR %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(HSR_DIGITAL_GAIN_REG,
+						DIS_DIG_GAIN_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for Digital Gain of HSR %d",
+									error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Power up earpiece
+ * @channel_index Channel-index
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_power_up_earpiece(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_DA = 0;
+
+	/* Check if Earpiece PowerUp request is mono channel */
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "EARPIECE should have mono channel");
+		return -EINVAL;
+	}
+
+	initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+
+	/* Check if Earpiece is already powered up or DA1 being used by HS */
+	if (EN_DA1 & initialVal_DA)
+		return 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1,
+							DA1_TO_HSL, 0);
+	if (0 != error) {
+		dev_err(dev,
+			"DA_IN1 path mixed with sidetone FIR %d", error);
+		return error;
+	}
+
+	/* Enable DA1 */
+	error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA1_REG,
+							SLOT08_FOR_DA_PATH, 0);
+	if (0 != error) {
+		dev_err(dev, "Data sent to DA1 from Slot 08 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+				EN_DA1, 0);
+	if (0 != error) {
+		dev_err(dev, "Enable DA1 %d", error);
+		return error;
+	}
+
+	/* Power Up EAR class-AB driver */
+	error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG,
+					EN_EAR_MASK, 0);
+	if (0 != error) {
+		dev_err(dev, "Power Up EAR class-AB driver %d", error);
+		return error;
+	}
+
+	/* Power up EAR DAC and digital path */
+	error = HW_ACODEC_MODIFY_WRITE(
+				DIGITAL_OUTPUT_ENABLE_REG, EN_EAR_MASK, 0);
+	if (0 != error) {
+		dev_err(dev, "Power up EAR DAC and digital path %d", error);
+		return error;
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+/**
+ * @brief Power down earpiece
+ * @channel_index Channel-index
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_power_down_earpiece(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_DA = 0;
+
+	/* Check if Earpiece PowerDown request is mono channel */
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "EARPIECE should have mono channel");
+		return -EINVAL;
+	}
+
+	/* Check if Earpiece is already powered down or DA1 being used by HS */
+	initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+	if (!(initialVal_DA & EN_DA1))
+		return 0;
+
+	/* Power Down EAR class-AB driver */
+	error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG,
+						0, EN_EAR_MASK);
+	if (0 != error) {
+		dev_err(dev, "Power Down EAR class-AB driver %d", error);
+		return error;
+	}
+
+		/* Power Down EAR DAC and digital path */
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG,
+					0, EN_EAR_MASK);
+	if (0 != error) {
+		dev_err(dev,
+			"Power Down EAR DAC and digital path %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1, 0, DA1_TO_HSL);
+	if (0 != error) {
+		dev_err(dev,
+			"Clear DA_IN1 path mixed with sidetone FIR %d",
+									error);
+		return error;
+	}
+
+	/* Disable DA1 */
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+						0, EN_DA1);
+	if (0 != error) {
+		dev_err(dev, "Disable DA1 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA1_REG, 0,
+							SLOT08_FOR_DA_PATH);
+	if (0 != error) {
+		dev_err(dev,
+			"Data sent to DA1 cleared from Slot 08 %d", error);
+		return error;
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+/**
+ * @brief Mute earpiece
+ * @channel_index Channel-index
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_mute_earpiece(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+
+	/* Check if Earpiece Mute request is mono channel */
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "EARPIECE should have mono channel");
+		return -EINVAL;
+	}
+
+	/* Mute Earpiece */
+	error = HW_ACODEC_MODIFY_WRITE(MUTE_HS_EAR_REG,
+				       EN_EAR_MASK | EN_EAR_DAC_MASK, 0);
+	if (0 != error) {
+		dev_err(dev, "Mute Earpiece %d", error);
+		return error;
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+/**
+ * @brief Unmute earpiece
+ * @channel_index Channel-index
+ * @gain Gain index of earpiece
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_unmute_earpiece(enum AUDIOIO_CH_INDEX channel_index, int *gain,
+					struct device *dev)
+{
+	int error = 0;
+
+	/* Check if Earpiece UnMute request is mono channel */
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "EARPIECE should have mono channel");
+		return -EINVAL;
+	}
+
+	/* UnMute Earpiece */
+	error = HW_ACODEC_MODIFY_WRITE(MUTE_HS_EAR_REG, 0,
+				       EN_EAR_MASK | EN_EAR_DAC_MASK);
+	if (0 != error) {
+		dev_err(dev, "UnMute Earpiece %d", error);
+		return error;
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+/**
+ * @brief Enables fading of earpiece
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_enable_fade_earpiece(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(DA1_DIGITAL_GAIN_REG, 0, DIS_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for Ear %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(HSL_EAR_DIGITAL_GAIN_REG, 0,
+							DIS_DIG_GAIN_FADING);
+	if (0 != error) {
+		dev_err(dev,
+			"Enable fading for Digital Gain of Ear %d", error);
+		return error;
+	}
+
+	return error;
+}
+/**
+ * @brief Disables fading of earpiece
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_disable_fade_earpiece(struct device *dev)
+{
+	int error = 0;
+	error = HW_ACODEC_MODIFY_WRITE(DA1_DIGITAL_GAIN_REG, DIS_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for Ear %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Power up IHF on a specific channel
+ * @channel_index Channel-index
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_power_up_ihf(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_DA = 0;
+
+	/* Check if IHF PowerUp request is mono or Stereo channel */
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "IHF should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	if (channel_index & e_CHANNEL_1) {
+		initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+		if (EN_DA3 & initialVal_DA)
+			return 0;
+
+		/* Enable DA3 for IHFL */
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA3_REG,
+							SLOT10_FOR_DA_PATH, 0);
+		if (0 != error) {
+			dev_err(dev, "Data sent to DA3 from Slot 10 %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+								EN_DA3, 0);
+		if (0 != error) {
+			dev_err(dev, "Power up IHFL %d", error);
+			return error;
+		}
+
+		/* Power Up HFL Class-D driver */
+		error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG,
+								EN_HFL_MASK, 0);
+		if (0 != error) {
+			dev_err(dev, "Power Up HFL Class-D Driver %d", error);
+			return error;
+		}
+
+		/* Power up HFL Class D driver and digital path */
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG,
+								EN_HFL_MASK, 0);
+		if (0 != error) {
+			dev_err(dev,
+			"Power up HFL Class D driver & digital path %d",
+									error);
+			return error;
+		}
+	}
+
+	/* Enable DA4 for IHFR */
+	if (channel_index & e_CHANNEL_2) {
+		initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+		if (EN_DA4 & initialVal_DA)
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA4_REG,
+							SLOT11_FOR_DA_PATH, 0);
+		if (0 != error) {
+			dev_err(dev, "Data sent to DA4 from Slot 11 %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+								EN_DA4, 0);
+		if (0 != error) {
+			dev_err(dev, "Enable DA4 %d", error);
+			return error;
+		}
+
+		/* Power Up HFR Class-D driver */
+		error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG,
+								EN_HFR_MASK, 0);
+		if (0 != error) {
+			dev_err(dev, "Power Up HFR Class-D Driver %d", error);
+			return error;
+		}
+
+		/* Power up HFR Class D driver and digital path */
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG,
+							EN_HFR_MASK, 0);
+		if (0 != error) {
+			dev_err(dev,
+			"Power up HFR Class D driver and digital path %d",
+									error);
+			return error;
+		}
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+/**
+ * @brief Power down IHF on a specific channel
+ * @channel_index Channel-index
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_power_down_ihf(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_DA = 0;
+
+		/* Check if IHF Power Down request is mono or Stereo channel */
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "IHF should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	if (channel_index & e_CHANNEL_1) {
+		initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+		if (!(initialVal_DA & EN_DA3))
+			return 0;
+
+		/* Power Down HFL Class-D driver */
+		error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG, 0,
+								EN_HFL_MASK);
+		if (0 != error) {
+			dev_err(dev, "Power Down HFL Class-D Driver %d",
+									error);
+			return error;
+		}
+
+		/* Power Down HFL Class D driver and digital path */
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG, 0,
+								EN_HFL_MASK);
+		if (0 != error) {
+			dev_err(dev,
+			"Power Down HFL Class D driver & digital path %d",
+									error);
+			return error;
+		}
+
+		/* Disable DA3 for IHFL */
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+								0, EN_DA3);
+		if (0 != error) {
+			dev_err(dev, "Disable DA3 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA3_REG, 0,
+							SLOT10_FOR_DA_PATH);
+		if (0 != error) {
+			dev_err(dev,
+				"Data sent to DA3 cleared from Slot 10 %d",
+									error);
+			return error;
+		}
+	}
+
+	if (channel_index & e_CHANNEL_2) {
+		initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+
+		/* Check if IHF is already powered Down */
+		if (!(initialVal_DA & EN_DA4))
+			return 0;
+
+		/* Power Down HFR Class-D Driver */
+		error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG, 0,
+								EN_HFR_MASK);
+		if (0 != error) {
+			dev_err(dev, "Power Down HFR Class-D Driver %d",
+									error);
+			return error;
+		}
+
+		/* Power Down HFR Class D driver and digital path */
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG, 0,
+								EN_HFR_MASK);
+		if (0 != error) {
+			dev_err(dev,
+			"Power Down HFR Class D driver & digital path %d",
+									error);
+			return error;
+		}
+
+			/* Disable DA4 for IHFR */
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+								0, EN_DA4);
+		if (0 != error) {
+			dev_err(dev, "Disable DA4 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA4_REG, 0,
+							SLOT11_FOR_DA_PATH);
+		if (0 != error) {
+			dev_err(dev,
+				"Data sent to DA4 cleared from Slot 11 %d",
+									error);
+			return error;
+		}
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+/**
+ * @brief Mute IHF on a specific channel
+ * @channel_index Channel-index
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_mute_ihf(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+
+	if ((channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		error = ste_audio_io_set_ihf_gain(channel_index, 0, -63,
+								0, dev);
+		if (0 != error) {
+			dev_err(dev, "Mute ihf %d", error);
+			return error;
+		}
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+/**
+ * @brief Unmute IHF on a specific channel
+ * @channel_index Channel-index
+ * @gain Gain index of IHF
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_unmute_ihf(enum AUDIOIO_CH_INDEX channel_index, int *gain,
+					struct device *dev)
+{
+	int error = 0;
+
+	if (channel_index & e_CHANNEL_1) {
+		error = ste_audio_io_set_ihf_gain(channel_index, 0, gain[0],
+									0, dev);
+		if (0 != error) {
+			dev_err(dev, "UnMute ihf %d", error);
+			return error;
+		}
+		}
+
+	if (channel_index & e_CHANNEL_2) {
+		error = ste_audio_io_set_ihf_gain(channel_index, 0, gain[1],
+									0, dev);
+		if (0 != error) {
+			dev_err(dev, "UnMute ihf %d", error);
+			return error;
+		}
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+/**
+ * @brief Enable fading of IHF
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_enable_fade_ihf(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(DA3_DIGITAL_GAIN_REG, 0, DIS_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for HFL %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DA4_DIGITAL_GAIN_REG, 0, DIS_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for HFR %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Disable fading of IHF
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_disable_fade_ihf(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(DA3_DIGITAL_GAIN_REG, DIS_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for HFL %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DA4_DIGITAL_GAIN_REG, DIS_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for HFR %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Power up VIBL
+ * @channel_index Channel-index of VIBL
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_power_up_vibl(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_DA = 0;
+
+	/* Check if VibL PowerUp request is mono channel */
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "VibL should have mono channel");
+		return -EINVAL;
+	}
+
+	/* Try to allocate vibrator for audio left channel */
+	error = ste_audioio_vibrator_alloc(STE_AUDIOIO_CLIENT_AUDIO_L,
+		STE_AUDIOIO_CLIENT_AUDIO_R | STE_AUDIOIO_CLIENT_AUDIO_L);
+	if (error) {
+		dev_err(dev, "  Couldn't allocate vibrator %d, client %d",
+			error, STE_AUDIOIO_CLIENT_AUDIO_L);
+		return error;
+	}
+
+	initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+
+	/* Check if VibL is already powered up */
+	if (initialVal_DA & EN_DA5)
+		return 0;
+
+	/* Enable DA5 for vibl */
+	error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA5_REG,
+							SLOT12_FOR_DA_PATH, 0);
+	if (0 != error) {
+		dev_err(dev, "Data sent to DA5 from Slot 12 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+					EN_DA5, 0);
+	if (0 != error) {
+		dev_err(dev, "Enable DA5 for VibL %d", error);
+		return error;
+	}
+
+	/* Power Up VibL Class-D driver */
+	error = HW_ACODEC_MODIFY_WRITE(
+				ANALOG_OUTPUT_ENABLE_REG, EN_VIBL_MASK, 0);
+	if (0 != error) {
+		dev_err(dev, "Power Up VibL Class-D Driver %d", error);
+		return error;
+	}
+
+	/* Power up VibL Class D driver and digital path */
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG,
+						EN_VIBL_MASK, 0);
+	if (0 != error) {
+		dev_err(dev,
+			"Power up VibL Class D driver and digital path %d",
+									error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Power down VIBL
+ * @channel_index Channel-index of VIBL
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_power_down_vibl(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_DA = 0;
+
+	/* Check if VibL Power Down request is mono channel */
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "VibL should have mono channel");
+		return -EINVAL;
+	}
+
+	initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+
+	/* Check if VibL is already powered down */
+	if (!(initialVal_DA & EN_DA5))
+		return 0;
+
+
+	/* Power Down VibL Class-D driver */
+	error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG,
+					0, EN_VIBL_MASK);
+	if (0 != error)	{
+		dev_err(dev, "Power Down VibL Class-D Driver %d", error);
+		return error;
+	}
+
+	/* Power Down VibL Class D driver and digital path */
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG, 0,
+								EN_VIBL_MASK);
+	if (0 != error) {
+		dev_err(dev,
+			"Power Down VibL Class D driver & digital path %d",
+									error);
+		return error;
+	}
+
+	/* Disable DA5 for VibL */
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+					0, EN_DA5);
+	if (0 != error) {
+		dev_err(dev, "Disable DA5 for VibL %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA5_REG, 0,
+							SLOT12_FOR_DA_PATH);
+	if (0 != error) {
+		dev_err(dev,
+			"Data sent to DA5 cleared from Slot 12 %d", error);
+		return error;
+	}
+
+	/* Release vibrator */
+	ste_audioio_vibrator_release(STE_AUDIOIO_CLIENT_AUDIO_L);
+
+	return error;
+}
+/**
+ * @brief Enable fading of VIBL
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_enable_fade_vibl(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(DA5_DIGITAL_GAIN_REG, 0, DIS_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for VibL %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Disable fading of VIBL
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_disable_fade_vibl(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(DA5_DIGITAL_GAIN_REG, DIS_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for VibL %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Power up VIBR
+ * @channel_index Channel-index of VIBR
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_power_up_vibr(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_DA = 0;
+
+	/* Check if VibR PowerUp request is mono channel */
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "VibR should have mono channel");
+		return -EINVAL;
+	}
+
+	/* Try to allocate vibrator for audio right channel */
+	error = ste_audioio_vibrator_alloc(STE_AUDIOIO_CLIENT_AUDIO_R,
+		STE_AUDIOIO_CLIENT_AUDIO_R | STE_AUDIOIO_CLIENT_AUDIO_L);
+	if (error) {
+		dev_err(dev, "  Couldn't allocate vibrator %d, client %d",
+			error, STE_AUDIOIO_CLIENT_AUDIO_R);
+		return error;
+	}
+
+	initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+
+	/* Check if VibR is already powered up */
+	if (initialVal_DA & EN_DA6)
+		return 0;
+
+	/* Enable DA6 for vibr */
+	error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA6_REG,
+							SLOT13_FOR_DA_PATH, 0);
+	if (0 != error) {
+		dev_err(dev, "Data sent to DA5 from Slot 13 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(
+				DIGITAL_DA_CHANNELS_ENABLE_REG, EN_DA6, 0);
+	if (0 != error) {
+		dev_err(dev, "Enable DA6 for VibR %d", error);
+		return error;
+	}
+
+	/* Power Up VibR Class-D driver */
+	error = HW_ACODEC_MODIFY_WRITE(
+				ANALOG_OUTPUT_ENABLE_REG, EN_VIBR_MASK, 0);
+	if (0 != error) {
+		dev_err(dev, "Power Up VibR Class-D Driver %d", error);
+		return error;
+	}
+
+	/* Power up VibR Class D driver and digital path */
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG,
+							EN_VIBR_MASK, 0);
+	if (0 != error) {
+		dev_err(dev,
+			"Power up VibR Class D driver & digital path %d",
+									error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Power down VIBR
+ * @channel_index Channel-index of VIBR
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_power_down_vibr(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_DA = 0;
+
+	/* Check if VibR PowerDown request is mono channel */
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "VibR should have mono channel");
+		return -EINVAL;
+	}
+
+	initialVal_DA = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+
+	/* Check if VibR is already powered down */
+	if (!(initialVal_DA & EN_DA6))
+		return 0;
+
+
+	/* Power Down VibR Class-D driver */
+	error = HW_ACODEC_MODIFY_WRITE(ANALOG_OUTPUT_ENABLE_REG, 0,
+							EN_VIBR_MASK);
+	if (0 != error) {
+		dev_err(dev, "Power Down VibR Class-D Driver %d", error);
+		return error;
+	}
+
+	/* Power Down VibR Class D driver and digital path */
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_OUTPUT_ENABLE_REG, 0,
+								EN_VIBR_MASK);
+	if (0 != error) {
+		dev_err(dev,
+			"Power Down VibR Class D driver & digital path %d",
+									error);
+		return error;
+	}
+
+	/* Disable DA6 for VibR */
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_DA_CHANNELS_ENABLE_REG,
+					0, EN_DA6);
+	if (0 != error) {
+		dev_err(dev, "Disable DA6 for VibR %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA6_REG, 0,
+							SLOT13_FOR_DA_PATH);
+	if (0 != error) {
+		dev_err(dev, "Data sent to DA5 cleared from Slot 13 %d",
+									error);
+		return error;
+	}
+
+	/* Release vibrator */
+	ste_audioio_vibrator_release(STE_AUDIOIO_CLIENT_AUDIO_R);
+
+	return error;
+}
+/**
+ * @brief Enable fading of VIBR
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_enable_fade_vibr(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(DA6_DIGITAL_GAIN_REG, 0, DIS_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for VibR %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Disable fading of VIBR
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_disable_fade_vibr(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(DA6_DIGITAL_GAIN_REG, DIS_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for VibR %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Power up MIC1A
+ * @channel_index Channel-index of MIC1A
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_power_up_mic1a(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_AD = 0;
+
+	/* Check if Mic1 PowerUp request is mono channel */
+
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "MIC1 should have mono channel");
+		return -EINVAL;
+	}
+
+	initialVal_AD = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+	/* Check if Mic1 is already powered up or used by Dmic3 */
+	if (EN_AD3 & initialVal_AD)
+		return 0;
+
+	error = HW_REG_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG, DATA_FROM_AD_OUT3);
+	if (0 != error) {
+		dev_err(dev, "Slot 02 outputs data from AD_OUT3 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+						EN_AD3, 0);
+	if (0 != error) {
+		dev_err(dev, "Enable AD3 for Mic1 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1, 0,
+							SEL_DMIC3_FOR_AD_OUT3);
+	if (0 != error) {
+		dev_err(dev, "Select ADC1 for AD_OUT3 %d", error);
+		return error;
+	}
+
+	/* Select MIC1A */
+	error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, 0,
+							SEL_MIC1B_CLR_MIC1A);
+	if (0 != error) {
+		dev_err(dev, "Select MIC1A %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, EN_MIC1, 0);
+	if (0 != error) {
+		dev_err(dev, "Power up Mic1 %d", error);
+		return error;
+	}
+
+	/* Power Up ADC1 */
+	error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, POWER_UP_ADC1, 0);
+	if (0 != error) {
+		dev_err(dev, "Power Up ADC1 %d", error);
+		return error;
+	}
+
+return error;
+}
+/**
+ * @brief Power down MIC1A
+ * @channel_index Channel-index of MIC1A
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_power_down_mic1a(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_AD = 0;
+
+	/* Check if Mic1 PowerDown request is mono channel */
+
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "Mic1 should have mono channel");
+		return -EINVAL;
+	}
+
+	initialVal_AD = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+	/* Check if Mic1 is already powered down or used by Dmic3 */
+	if (!(initialVal_AD & EN_AD3))
+		return 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, 0, EN_MIC1);
+	if (0 != error) {
+		dev_err(dev, "Power Down Mic1 %d", error);
+		return error;
+	}
+
+		/* Power Down ADC1 */
+	error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, 0, POWER_UP_ADC1);
+	if (0 != error) {
+		dev_err(dev, "Power Down ADC1 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+						0, EN_AD3);
+	if (0 != error) {
+		dev_err(dev, "Disable AD3 for Mic1 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT2_3_REG, 0,
+							DATA_FROM_AD_OUT3);
+	if (0 != error) {
+		dev_err(dev, "Slot 02 outputs data cleared from AD_OUT3 %d",
+									error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Mute MIC1A
+ * @channel_index Channel-index of MIC1A
+ * @return 0 on success otherwise negative error code
+ */
+
+
+int ste_audio_io_mute_mic1a(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "MIC1 should have mono channel");
+		return -EINVAL;
+	}
+
+	/* Mute mic1 */
+	error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, MUT_MIC1, 0);
+	if (0 != error) {
+		dev_err(dev, "Mute Mic1 %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Unmute MIC1A
+ * @channel_index Channel-index of MIC1A
+ * @gain Gain index of MIC1A
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_unmute_mic1a(enum AUDIOIO_CH_INDEX channel_index, int *gain,
+					struct device *dev)
+{
+	int error = 0;
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "Mic1 should have mono channel");
+		return -EINVAL;
+	}
+	/* UnMute mic1 */
+	error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, 0, MUT_MIC1);
+	if (0 != error) {
+		dev_err(dev, "UnMute Mic1 %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Enable fading of MIC1A
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_enable_fade_mic1a(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(AD3_DIGITAL_GAIN_REG, 0, DIS_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for Mic1 %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Disable fading of MIC1A
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_disable_fade_mic1a(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(AD3_DIGITAL_GAIN_REG, DIS_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for Mic1 %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Power up MIC1B
+ * @channel_index Channel-index of MIC1B
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_power_up_mic1b(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error;
+	unsigned char initialVal_AD = 0;
+
+	error = regulator_enable(regulator_avsource);
+	if (0 != error) {
+		dev_err(dev, "regulator avsource enable failed = %d", error);
+		return error;
+	}
+	/* GPIO35 settings to enable MIC 1B input instead of TVOUT */
+	error = HW_ACODEC_MODIFY_WRITE(AB8500_GPIO_DIR5_REG,
+						GPIO35_DIR_OUTPUT, 0);
+	if (0 != error) {
+		dev_err(dev, "setting AB8500_GPIO_DIR5_REG reg %d", error);
+		return error;
+	}
+	error = HW_ACODEC_MODIFY_WRITE(AB8500_GPIO_OUT5_REG,
+						GPIO35_DIR_OUTPUT, 0);
+	if (0 != error) {
+		dev_err(dev, "setting AB8500_GPIO_OUT5_REG reg %d", error);
+		return error;
+	}
+
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "Mic1 should have mono channel");
+		return -EINVAL;
+	}
+
+	initialVal_AD = HW_REG_READ(DIGITAL_AD_CHANNELS_ENABLE_REG);
+	/* Check if Mic1 is already powered up or used by Dmic3 */
+	if (EN_AD3 & initialVal_AD)
+		return 0;
+
+	error = HW_REG_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG, DATA_FROM_AD_OUT3);
+	if (0 != error) {
+		dev_err(dev, "Slot 02 outputs data from AD_OUT3 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+						EN_AD3, 0);
+	if (0 != error) {
+		dev_err(dev, "Enable AD3 for Mic1 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1, 0,
+							SEL_DMIC3_FOR_AD_OUT3);
+	if (0 != error) {
+		dev_err(dev, "Select ADC1 for AD_OUT3 %d", error);
+		return error;
+	}
+
+	/* Select MIC1B */
+	error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, SEL_MIC1B_CLR_MIC1A,
+									0);
+	if (0 != error) {
+		dev_err(dev, "Select MIC1B %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, EN_MIC1, 0);
+	if (0 != error) {
+		dev_err(dev, "Power up Mic1 %d", error);
+		return error;
+	}
+
+	/* Power Up ADC1 */
+	error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, POWER_UP_ADC1, 0);
+	if (0 != error) {
+		dev_err(dev, "Power Up ADC1 %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Power down MIC1B
+ * @channel_index Channel-index of MIC1B
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_power_down_mic1b(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error;
+	unsigned char initialVal_AD = 0;
+
+	/* Check if Mic1 PowerDown request is mono channel */
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "Mic1 should have mono channel");
+		return -EINVAL;
+	}
+
+	initialVal_AD = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+
+	/* Check if Mic1 is already powered down or used by Dmic3 */
+	if (!(initialVal_AD & EN_AD3))
+		return 0;
+
+
+	error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, 0, EN_MIC1);
+	if (0 != error) {
+		dev_err(dev, "Power Down Mic1 %d", error);
+		return error;
+	}
+
+	/* Power Down ADC1 */
+	error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, 0, POWER_UP_ADC1);
+	if (0 != error) {
+		dev_err(dev, "Power Down ADC1 %d", error);
+		return error;
+	}
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG, 0,
+			EN_AD3);
+	if (0 != error) {
+		dev_err(dev, "Disable AD3 for Mic1 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT2_3_REG, 0,
+							DATA_FROM_AD_OUT3);
+	if (0 != error) {
+		dev_err(dev, "Slot 02 outputs data cleared from AD_OUT3 %d",
+									error);
+		return error;
+	}
+
+	/* undo GPIO35 settings */
+	error = HW_ACODEC_MODIFY_WRITE(AB8500_GPIO_DIR5_REG,
+						0, GPIO35_DIR_OUTPUT);
+	if (0 != error) {
+		dev_err(dev, "resetting AB8500_GPIO_DIR5_REG reg %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(AB8500_GPIO_OUT5_REG,
+						0, GPIO35_DIR_OUTPUT);
+	if (0 != error) {
+		dev_err(dev, "resetting AB8500_GPIO_OUT5_REG reg %d", error);
+		return error;
+	}
+
+	error = regulator_disable(regulator_avsource);
+	if (0 != error) {
+		dev_err(dev, "regulator avsource disable failed = %d", error);
+		return error;
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+
+/**
+ * @brief enable hardware loop of mic1b
+ * @chnl_index Channel-index of MIC1B
+ * @hw_loop type of hardware loop
+ * @loop_gain gain value to be used in hardware loop
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_enable_loop_mic1b(enum AUDIOIO_CH_INDEX chnl_index,
+					enum AUDIOIO_HAL_HW_LOOPS hw_loop,
+					int loop_gain, struct device *dev,
+					void *cookie)
+{
+	int error;
+	struct transducer_context_t *trnsdr;
+	trnsdr = (struct transducer_context_t *)cookie;
+
+	switch (hw_loop) {
+		/* Check if HSL is active */
+	case AUDIOIO_SIDETONE_LOOP:
+		if (!(trnsdr[HS_CH].is_power_up[e_CHANNEL_1])
+			&& !(trnsdr[EAR_CH].is_power_up[e_CHANNEL_1])) {
+			error = -EFAULT;
+			dev_err(dev,
+			"HS or Earpiece not powered up error = %d",
+					error);
+			return error;
+		}
+
+		/* For ch1, Power On STFIR1, data comes from AD3*/
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG2,
+						FIR1_FROMAD3, 0);
+		if (error)
+			dev_err(dev, "FIR1 data comes from AD_OUT3 %d",
+								error);
+		error = HW_REG_WRITE(SIDETONE_FIR1_GAIN_REG, loop_gain);
+			if (error) {
+				dev_err(dev,
+					"Set FIR1 Gain index = %d",
+							error);
+				return error;
+			}
+		break;
+	default:
+		error = -EINVAL;
+		dev_err(dev, "loop not supported %d", error);
+	}
+	return error;
+}
+
+/**
+ * @brief disable hardware loop of mic1b
+ * @chnl_index Channel-index of MIC1B
+ * @hw_loop type of hardware loop
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_disable_loop_mic1b(enum AUDIOIO_CH_INDEX chnl_index,
+					enum AUDIOIO_HAL_HW_LOOPS hw_loop,
+					struct device *dev, void *cookie)
+{
+	int error;
+	struct transducer_context_t *trnsdr;
+	trnsdr = (struct transducer_context_t *)cookie;
+
+	switch (hw_loop) {
+	/* Check if HSL is active */
+	case AUDIOIO_SIDETONE_LOOP:
+		if (!trnsdr[HS_CH].is_power_up[e_CHANNEL_1]
+			&& !trnsdr[EAR_CH].is_power_up[e_CHANNEL_1]) {
+			error = -EFAULT;
+			dev_err(dev, "HS or Earpiece not powered up, err = %d",
+					error);
+			return error;
+		}
+
+		/* For ch1, Power down STFIR1, data comes from AD3*/
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG2,
+						0, FIR1_FROMAD3);
+		if (error) {
+			dev_err(dev, "FIR1 data comes from AD_OUT3, err = %d",
+								error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(FILTERS_CONTROL_REG,
+						0, FIR_FILTERCONTROL);
+		if (error) {
+			dev_err(dev,
+				"ST FIR Filters disable failed %d", error);
+				return error;
+		}
+		break;
+	default:
+		error = -EINVAL;
+		dev_err(dev, "loop not supported %d", error);
+	}
+	return error;
+}
+
+/**
+ * @brief Power up MIC2
+ * @channel_index Channel-index of MIC2
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_power_up_mic2(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_AD = 0;
+
+	/* Check if Mic2 PowerUp request is mono channel */
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "Mic2 should have mono channel");
+		return -EINVAL;
+	}
+
+	initialVal_AD = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+
+	/* Check if Mic2 is already powered up or used by LINR or Dmic2 */
+	if (EN_AD2 & initialVal_AD)
+		return 0;
+
+
+	error = HW_REG_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG, DATA_FROM_AD_OUT2);
+	if (0 != error) {
+		dev_err(dev, "Slot 01 outputs data from AD_OUT2 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG, EN_AD2,
+									0);
+	if (0 != error) {
+		dev_err(dev, "Enable AD2 for Mic2 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1, 0,
+							SEL_DMIC2_FOR_AD_OUT2);
+	if (0 != error) {
+		dev_err(dev, "Select ADC2 for AD_OUT2 %d", error);
+		return error;
+	}
+
+		/* Select mic2 */
+	error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, 0,
+							SEL_LINR_CLR_MIC2);
+	if (0 != error) {
+		dev_err(dev, "Select MIC2 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, EN_MIC2, 0);
+	if (0 != error) {
+		dev_err(dev, "Power up Mic2 %d", error);
+		return error;
+	}
+
+	/* Power Up ADC1 */
+	error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, POWER_UP_ADC2, 0);
+	if (0 != error) {
+		dev_err(dev, "Power Up ADC2 %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Power down MIC2
+ * @channel_index Channel-index of MIC2
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_power_down_mic2(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_AD = 0;
+
+	/* Check if Mic2 PowerDown request is mono channel */
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "Mic2 should have mono channel");
+		return -EINVAL;
+	}
+
+	initialVal_AD = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+
+	/* Check if Mic2 is already powered down or used by LINR or Dmic2 */
+	if (!(initialVal_AD & EN_AD2))
+		return 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, 0, EN_MIC2);
+	if (0 != error) {
+		dev_err(dev, "Power Down Mic2 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+					0, EN_AD2);
+	if (0 != error) {
+		dev_err(dev, "Disable AD2 for Mic2 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG, 0,
+						(DATA_FROM_AD_OUT2<<4));
+	if (0 != error) {
+		dev_err(dev, "Slot 01 outputs data cleared from AD_OUT2 %d",
+									error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Mute MIC2
+ * @channel_index Channel-index of MIC2
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_mute_mic2(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "Mic2 should have mono channel");
+		return -EINVAL;
+	}
+
+	/* Mute mic2 */
+	error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, MUT_MIC2, 0);
+	if (0 != error) {
+		dev_err(dev, "Mute Mic2 %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Unmute MIC2
+ * @channel_index Channel-index of MIC2
+ * @gain Gain index of MIC2
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_unmute_mic2(enum AUDIOIO_CH_INDEX channel_index, int *gain,
+					struct device *dev)
+{
+	int error = 0;
+	if (!(channel_index & e_CHANNEL_1)) {
+		dev_err(dev, "Mic2 should have mono channel");
+		return -EINVAL;
+	}
+	/* UnMute mic2 */
+	error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, 0, MUT_MIC2);
+	if (0 != error) {
+		dev_err(dev, "UnMute Mic2 %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Enable fading of MIC2
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_enable_fade_mic2(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(AD2_DIGITAL_GAIN_REG, 0, DIS_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for Mic2 %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Disable fading of MIC2
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_disable_fade_mic2(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(AD2_DIGITAL_GAIN_REG, DIS_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for Mic2 %d", error);
+		return error;
+	}
+
+	return error;
+}
+/**
+ * @brief Power up LinIn
+ * @channel_index Channel-index of LinIn
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_power_up_lin(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_AD = 0;
+
+	/* Check if LinIn PowerUp request is mono or Stereo channel */
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "LinIn should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	/* Enable AD1 for LinInL */
+	if (channel_index & e_CHANNEL_1) {
+		initialVal_AD = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+		if (initialVal_AD & EN_AD1)
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG,
+							DATA_FROM_AD_OUT1, 0);
+		if (0 != error) {
+			dev_err(dev, "Slot 00 outputs data from AD_OUT1 %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+								EN_AD1, 0);
+		if (0 != error) {
+			dev_err(dev, "Enable AD1 for LinInL %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1, 0,
+							SEL_DMIC1_FOR_AD_OUT1);
+		if (0 != error) {
+			dev_err(dev, "Select ADC3 for AD_OUT1  %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(
+				LINE_IN_MIC_CONF_REG, EN_LIN_IN_L, 0);
+		if (0 != error) {
+			dev_err(dev, "Power up LinInL %d", error);
+			return error;
+		}
+
+		/* Power Up ADC3 */
+		error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG,
+					POWER_UP_ADC3, 0);
+		if (0 != error) {
+			dev_err(dev, "Power Up ADC3 %d", error);
+			return error;
+		}
+	}
+	/* Enable AD2 for LinInR */
+
+	if (channel_index & e_CHANNEL_2) {
+		initialVal_AD = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+		if (EN_AD2 & initialVal_AD)
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG,
+						(DATA_FROM_AD_OUT2<<4), 0);
+		if (0 != error) {
+			dev_err(dev, "Slot 01 outputs data from AD_OUT2 %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+								EN_AD2, 0);
+		if (0 != error) {
+			dev_err(dev, "Enable AD2 LinInR %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1, 0,
+							SEL_DMIC2_FOR_AD_OUT2);
+		if (0 != error) {
+			dev_err(dev, "Select ADC2 for AD_OUT2 %d", error);
+			return error;
+		}
+
+		/* Select LinInR */
+		error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG,
+							SEL_LINR_CLR_MIC2, 0);
+		if (0 != error) {
+			dev_err(dev, "Select LinInR %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG,
+						EN_LIN_IN_R, 0);
+		if (0 != error) {
+			dev_err(dev, "Power up LinInR %d", error);
+			return error;
+		}
+
+		/* Power Up ADC2 */
+		error = HW_ACODEC_MODIFY_WRITE(
+				ADC_DAC_ENABLE_REG, POWER_UP_ADC2, 0);
+		if (0 != error) {
+			dev_err(dev, "Power Up ADC2 %d", error);
+			return error;
+		}
+	}
+	return error;
+}
+/**
+ * @brief Power down LinIn
+ * @channel_index Channel-index of LinIn
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_power_down_lin(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_AD = 0;
+
+	/* Check if LinIn PowerDown request is mono or Stereo channel */
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "LinIn should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	/* Enable AD1 for LinInL */
+	if (channel_index & e_CHANNEL_1) {
+		initialVal_AD = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+		if (!(initialVal_AD & EN_AD1))
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, 0,
+								EN_LIN_IN_L);
+		if (0 != error) {
+			dev_err(dev, "Power Down LinInL %d", error);
+			return error;
+		}
+
+			/* Power Down ADC3 */
+		error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, 0,
+								POWER_UP_ADC3);
+		if (0 != error) {
+			dev_err(dev, "Power Down ADC3 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+						0, EN_AD1);
+		if (0 != error) {
+			dev_err(dev, "Disable AD1 for LinInL %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG, 0,
+							DATA_FROM_AD_OUT1);
+		if (0 != error) {
+			dev_err(dev,
+			"Slot 00 outputs data cleared from AD_OUT1 %d",
+								error);
+			return error;
+		}
+	}
+
+	/* Enable AD2 for LinInR */
+	if (channel_index & e_CHANNEL_2) {
+		initialVal_AD = HW_REG_READ(DIGITAL_DA_CHANNELS_ENABLE_REG);
+		if (!(initialVal_AD & EN_AD2))
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, 0,
+								EN_LIN_IN_R);
+		if (0 != error) {
+			dev_err(dev, "Power Down LinInR %d", error);
+			return error;
+		}
+
+		/* Power Down ADC2 */
+		error = HW_ACODEC_MODIFY_WRITE(ADC_DAC_ENABLE_REG, 0,
+								POWER_UP_ADC2);
+		if (0 != error) {
+			dev_err(dev, "Power Down ADC2 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+							0, EN_AD2);
+		if (0 != error) {
+			dev_err(dev, "Disable AD2 LinInR %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG, 0,
+						(DATA_FROM_AD_OUT2<<4));
+		if (0 != error) {
+			dev_err(dev,
+				"Slot01 outputs data cleared from AD_OUT2 %d",
+									error);
+			return error;
+		}
+	}
+	return error;
+}
+/**
+ * @brief Mute LinIn
+ * @channel_index Channel-index of LinIn
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_mute_lin(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "LinIn should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	if (channel_index & e_CHANNEL_1) {
+		/* Mute LinInL */
+		error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG,
+							MUT_LIN_IN_L, 0);
+		if (0 != error) {
+			dev_err(dev, "Mute LinInL %d", error);
+			return error;
+		}
+	}
+
+	if (channel_index & e_CHANNEL_2) {
+		/* Mute LinInR */
+		error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG,
+								MUT_LIN_IN_R,
+								0);
+		if (0 != error) {
+			dev_err(dev, "Mute LinInR %d", error);
+			return error;
+		}
+	}
+	return error;
+}
+/**
+ * @brief Unmute LinIn
+ * @channel_index Channel-index of LinIn
+ * @gain Gain index of LinIn
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_unmute_lin(enum AUDIOIO_CH_INDEX channel_index, int *gain,
+					struct device *dev)
+{
+	int error = 0;
+
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "LinIn should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	if (channel_index & e_CHANNEL_1) {
+		/* UnMute LinInL */
+		error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, 0,
+								MUT_LIN_IN_L);
+		if (0 != error) {
+			dev_err(dev, "UnMute LinInL %d", error);
+			return error;
+		}
+	}
+
+	if (channel_index & e_CHANNEL_2) {
+		/* UnMute LinInR */
+		error = HW_ACODEC_MODIFY_WRITE(LINE_IN_MIC_CONF_REG, 0,
+								MUT_LIN_IN_R);
+		if (0 != error) {
+			dev_err(dev, "UnMute LinInR %d", error);
+			return error;
+		}
+	}
+	return error;
+}
+/**
+ * @brief Enables fading of LinIn
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_enable_fade_lin(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(AD1_DIGITAL_GAIN_REG, 0, DIS_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for LinInL %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(AD2_DIGITAL_GAIN_REG, 0, DIS_FADING);
+	if (0 != error) {
+		dev_err(dev, "Enable fading for LinInR %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Disables fading of LinIn
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_disable_fade_lin(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(AD1_DIGITAL_GAIN_REG, DIS_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for LinInL %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(AD2_DIGITAL_GAIN_REG, DIS_FADING, 0);
+	if (0 != error) {
+		dev_err(dev, "Disable fading for LinInR %d", error);
+		return error;
+	}
+	return error;
+}
+/**
+ * @brief Power Up DMIC12 LinIn
+ * @channel_index Channel-index of DMIC12
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_power_up_dmic12(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_AD = 0;
+
+	/* Check if DMic12 request is mono or Stereo */
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "DMic12 does not support more than 2 channels");
+
+		return -EINVAL;
+	}
+
+	/* Setting Direction for GPIO pins on AB8500 */
+	error = HW_REG_WRITE(AB8500_GPIO_DIR4_REG, GPIO27_DIR_OUTPUT);
+	if (0 != error) {
+		dev_err(dev, "Setting Direction for GPIO pins on AB8500 %d",
+									error);
+		return error;
+	}
+
+	/* Enable AD1 for Dmic1 */
+	if (channel_index & e_CHANNEL_1) {
+		/* Check if DMIC1 is already powered up or used by LinInL */
+		initialVal_AD = HW_REG_READ(DIGITAL_AD_CHANNELS_ENABLE_REG);
+		if (initialVal_AD & EN_AD1)
+			return 0;
+
+		error = HW_REG_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG,
+							DATA_FROM_AD_OUT1);
+		if (0 != error) {
+			dev_err(dev, "Slot 00 outputs data from AD_OUT1 %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+								EN_AD1, 0);
+		if (0 != error) {
+			dev_err(dev, "Enable AD1 for DMIC1 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1,
+						SEL_DMIC1_FOR_AD_OUT1, 0);
+		if (0 != error) {
+			dev_err(dev, "Select DMIC1 for AD_OUT1 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DMIC_ENABLE_REG, EN_DMIC1, 0);
+		if (0 != error) {
+			dev_err(dev, "Enable DMIC1 %d", error);
+			return error;
+		}
+	}
+	/* Enable AD2 for Dmic2 */
+
+	if (channel_index & e_CHANNEL_2) {
+		/* Check if DMIC2 is already powered up
+		or used by Mic2 or LinInR */
+		initialVal_AD = HW_REG_READ(DIGITAL_AD_CHANNELS_ENABLE_REG);
+		if (initialVal_AD & EN_AD2)
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG,
+						(DATA_FROM_AD_OUT2<<4), 0);
+		if (0 != error) {
+			dev_err(dev, "Slot 01 outputs data from AD_OUT2 %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+								EN_AD2, 0);
+		if (0 != error) {
+			dev_err(dev, "Enable AD2 for DMIC2 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1,
+						SEL_DMIC2_FOR_AD_OUT2, 0);
+		if (0 != error) {
+			dev_err(dev, "Select DMIC2 for AD_OUT2 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DMIC_ENABLE_REG, EN_DMIC2, 0);
+		if (0 != error) {
+			dev_err(dev, "Enable DMIC2 %d", error);
+			return error;
+		}
+	}
+
+	return error;
+}
+/**
+ * @brief Power down DMIC12 LinIn
+ * @channel_index Channel-index of DMIC12
+ * @return 0 on success otherwise negative error code
+ */
+
+
+int ste_audio_io_power_down_dmic12(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_AD = 0;
+
+	/* Check if DMic12 request is mono or Stereo or multi channel */
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "DMic12 does not support more than 2 channels");
+
+		return -EINVAL;
+	}
+
+	/* Setting Direction for GPIO pins on AB8500 */
+	error = HW_ACODEC_MODIFY_WRITE(AB8500_GPIO_DIR4_REG, 0,
+							GPIO27_DIR_OUTPUT);
+	if (0 != error) {
+		dev_err(dev, "Clearing Direction for GPIO pins on AB8500 %d",
+									error);
+		return error;
+	}
+	/* Enable AD1 for Dmic1 */
+	if (channel_index & e_CHANNEL_1) {
+		/* Check if DMIC1 is already powered Down or used by LinInL */
+		initialVal_AD = HW_REG_READ(DIGITAL_AD_CHANNELS_ENABLE_REG);
+		if (!(initialVal_AD & EN_AD1))
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(DMIC_ENABLE_REG, 0, EN_DMIC1);
+		if (0 != error) {
+			dev_err(dev, "Enable DMIC1 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+							0, EN_AD1);
+		if (0 != error) {
+			dev_err(dev, "Disable AD1 for DMIC1 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG, 0,
+							DATA_FROM_AD_OUT1);
+		if (0 != error) {
+			dev_err(dev,
+			"Slot 00 outputs data cleared from AD_OUT1 %d",
+								error);
+			return error;
+		}
+	}
+
+	/* Enable AD2 for Dmic2 */
+	if (channel_index & e_CHANNEL_2) {
+		/* MIC2 is already powered Down or used by Mic2 or LinInR */
+		initialVal_AD = HW_REG_READ(DIGITAL_AD_CHANNELS_ENABLE_REG);
+		if (!(initialVal_AD & EN_AD2))
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(DMIC_ENABLE_REG, 0, EN_DMIC2);
+		if (0 != error) {
+			dev_err(dev, "Enable DMIC2 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+							0, EN_AD2);
+		if (0 != error) {
+			dev_err(dev, "Disable AD2 for DMIC2 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT0_1_REG, 0,
+						(DATA_FROM_AD_OUT2<<4));
+		if (0 != error) {
+			dev_err(dev,
+			"Slot 01 outputs data cleared from AD_OUT2 %d",
+								error);
+			return error;
+		}
+	}
+	return error;
+}
+/**
+ * @brief  Get headset gain
+ * @left_volume
+ * @right_volume
+ * @return 0 on success otherwise negative error code
+ */
+
+
+int ste_audio_io_get_headset_gain(int *left_volume, int *right_volume,
+			u16 gain_index, struct device *dev)
+{
+	int i = 0;
+	if (gain_index == 0) {
+
+		*left_volume = 0 - HW_REG_READ(DA1_DIGITAL_GAIN_REG);
+		*right_volume = 0 - HW_REG_READ(DA2_DIGITAL_GAIN_REG);
+
+	}
+
+	if (gain_index == 1) {
+		*left_volume = 8 - HW_REG_READ(HSL_EAR_DIGITAL_GAIN_REG);
+		*right_volume = 8 - HW_REG_READ(HSR_DIGITAL_GAIN_REG);
+	}
+
+	if (gain_index == 2) {
+		i = (HW_REG_READ(ANALOG_HS_GAIN_REG)>>4);
+		*left_volume = hs_analog_gain_table[i];
+		i = (HW_REG_READ(ANALOG_HS_GAIN_REG) & MASK_QUARTET0);
+		*right_volume = hs_analog_gain_table[i];
+	}
+	return 0;
+}
+/**
+ * @brief  Get earpiece gain
+ * @left_volume
+ * @right_volume
+ * @return 0 on success otherwise negative error code
+ */
+
+
+int ste_audio_io_get_earpiece_gain(int *left_volume, int *right_volume,
+			u16 gain_index, struct device *dev)
+{
+	if (0 == gain_index)
+		*left_volume = 0 - HW_REG_READ(DA1_DIGITAL_GAIN_REG);
+	if (1 == gain_index)
+		*left_volume = 8 - HW_REG_READ(HSL_EAR_DIGITAL_GAIN_REG);
+	return 0;
+}
+/**
+ * @brief  Get ihf gain
+ * @left_volume
+ * @right_volume
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_get_ihf_gain(int *left_volume, int *right_volume,
+			u16 gain_index, struct device *dev)
+{
+
+	*left_volume = 0 - HW_REG_READ(DA3_DIGITAL_GAIN_REG);
+	*right_volume = 0 - HW_REG_READ(DA4_DIGITAL_GAIN_REG);
+	return 0;
+}
+/**
+ * @brief  Get vibl gain
+ * @left_volume
+ * @right_volume
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_get_vibl_gain(int *left_volume, int *right_volume,
+			u16 gain_index, struct device *dev)
+{
+
+	*left_volume = 0 - HW_REG_READ(DA5_DIGITAL_GAIN_REG);
+
+	return 0;
+}
+/**
+ * @brief  Get vibr gain
+ * @left_volume
+ * @right_volume
+ * @return 0 on success otherwise negative error code
+ */
+
+
+int ste_audio_io_get_vibr_gain(int *left_volume, int *right_volume,
+			u16 gain_index, struct device *dev)
+{
+
+	*right_volume = 0 - HW_REG_READ(DA6_DIGITAL_GAIN_REG);
+	return 0;
+}
+/**
+ * @brief  Get MIC1A & MIC2A gain
+ * @left_volume
+ * @right_volume
+ * @return 0 on success otherwise negative error code
+ */
+
+
+int ste_audio_io_get_mic1a_gain(int *left_volume, int *right_volume,
+			u16 gain_index, struct device *dev)
+{
+	if (gain_index == 0)
+		*left_volume = 31 - HW_REG_READ(AD3_DIGITAL_GAIN_REG);
+	if (gain_index == 1)
+		*left_volume = HW_REG_READ(ANALOG_MIC1_GAIN_REG);
+
+	return 0;
+}
+/**
+ * @brief  Get MIC2 gain
+ * @left_volume
+ * @right_volume
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_get_mic2_gain(int *left_volume, int *right_volume,
+			u16 gain_index, struct device *dev)
+{
+	if (gain_index == 0)
+		*left_volume = 31 - HW_REG_READ(AD2_DIGITAL_GAIN_REG);
+	if (gain_index == 1)
+		*left_volume = HW_REG_READ(ANALOG_MIC2_GAIN_REG);
+
+	return 0;
+}
+/**
+ * @brief  Get Lin IN gain
+ * @left_volume
+ * @right_volume
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_get_lin_gain(int *left_volume, int *right_volume,
+			u16 gain_index, struct device *dev)
+{
+	if (gain_index == 0) {
+		*left_volume = 31 - HW_REG_READ(AD1_DIGITAL_GAIN_REG);
+		*right_volume = 31 - HW_REG_READ(AD2_DIGITAL_GAIN_REG);
+	}
+
+	if (gain_index == 0) {
+		*left_volume = 2 * ((HW_REG_READ(ANALOG_HS_GAIN_REG)>>4) - 5);
+		*right_volume = 2 * (HW_REG_READ(ANALOG_LINE_IN_GAIN_REG) - 5);
+	}
+
+	return 0;
+}
+/**
+ * @brief  Get DMIC12 gain
+ * @left_volume
+ * @right_volume
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_get_dmic12_gain(int *left_volume, int *right_volume,
+			u16 gain_index, struct device *dev)
+{
+
+	*left_volume = HW_REG_READ(AD1_DIGITAL_GAIN_REG);
+
+	*right_volume = HW_REG_READ(AD2_DIGITAL_GAIN_REG);
+
+	return 0;
+}
+/**
+ * @brief  Get DMIC34 gain
+ * @left_volume
+ * @right_volume
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_get_dmic34_gain(int *left_volume, int *right_volume,
+			u16 gain_index, struct device *dev)
+{
+	*left_volume = HW_REG_READ(AD3_DIGITAL_GAIN_REG);
+	*right_volume = HW_REG_READ(AD4_DIGITAL_GAIN_REG);
+
+	return 0;
+}
+/**
+ * @brief  Get DMIC56 gain
+ * @left_volume
+ * @right_volume
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_get_dmic56_gain(int *left_volume, int *right_volume,
+			u16 gain_index, struct device *dev)
+{
+	*left_volume = HW_REG_READ(AD5_DIGITAL_GAIN_REG);
+
+	*right_volume = HW_REG_READ(AD6_DIGITAL_GAIN_REG);
+	return 0;
+}
+/**
+ * @brief Set gain of headset along a specified channel
+ * @channel_index Channel-index of headset
+ * @gain_index Gain index of headset
+ * @gain_value Gain value of headset
+ * @linear
+ * @return 0 on success otherwise negative error code
+ */
+
+
+int ste_audio_io_set_headset_gain(enum AUDIOIO_CH_INDEX channel_index,
+		u16 gain_index, int gain_value, u32 linear,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initial_val = 0;
+	int i = 0;
+	int acodec_device_id;
+
+	acodec_device_id = abx500_get_chip_id(dev);
+
+	if (channel_index & e_CHANNEL_1) {
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 0 - gain_value;
+
+			initial_val = HW_REG_READ(DA1_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(DA1_DIGITAL_GAIN_REG,
+						((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain HSL gainindex = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+
+		if (gain_index == 1) {
+			int gain = 0;
+			gain = 8 - gain_value;
+
+			initial_val = HW_REG_READ(HSL_EAR_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(HSL_EAR_DIGITAL_GAIN_REG,
+			((initial_val & (~HS_DIGITAL_GAIN_MASK)) | (gain &
+						HS_DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain HSL gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+
+		if (gain_index == 2) {
+			/* Set Analog gain */
+			int gain = -1;
+
+			if (gain_value % 2) {
+				gain_value -= 1;
+				dev_err(dev,
+		"Odd Gain received.Fixing it to 2dB step gain_value = %d",
+								gain_value);
+			}
+			/* Fix for 4dB step gains. Select one lower value */
+			if (gain_value == -22)
+				gain_value = -24;
+
+			if (gain_value == -26)
+				gain_value = -28;
+
+			if (gain_value == -30)
+				gain_value = -32;
+
+			for (i = 0 ; i < 16; i++) {
+				if (hs_analog_gain_table[i] == gain_value) {
+						gain = i<<4;
+						break;
+					}
+			}
+			if (gain == -1)
+				return -1;
+
+			if ((AB8500_REV_10 == acodec_device_id) ||
+					(AB8500_REV_11 == acodec_device_id)) {
+				if (!gain)
+					gain = 0x10;
+				gain = 0xF0 - gain;
+			}
+			initial_val = HW_REG_READ(ANALOG_HS_GAIN_REG);
+
+			/* Write gain */
+			error = HW_REG_WRITE(ANALOG_HS_GAIN_REG, ((initial_val &
+			(~L_ANALOG_GAIN_MASK)) | (gain & L_ANALOG_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain HSL gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+
+	/* for HSR */
+	if (channel_index & e_CHANNEL_2) {
+		/* Set Gain HSR */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 0 - gain_value;
+
+			initial_val = HW_REG_READ(DA2_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(DA2_DIGITAL_GAIN_REG, ((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain HSR gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+
+		if (gain_index == 1) {
+			int gain = 0;
+			gain = 8 - gain_value;
+
+			initial_val = HW_REG_READ(HSR_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(HSR_DIGITAL_GAIN_REG, ((initial_val
+			& (~HS_DIGITAL_GAIN_MASK)) | (gain &
+							HS_DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain HSR gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+
+		}
+
+		if (gain_index == 2) {
+			/* Set Analog gain */
+			int gain = -1;
+
+			if (gain_value % 2) {
+				gain_value -= 1;
+				dev_err(dev,
+		"Odd Gain received.Fixing it to 2dB step gain_value = %d",
+								gain_value);
+			}
+			/* Fix for 4dB step gains. Select one lower value */
+			if (gain_value == -22)
+				gain_value = -24;
+
+			if (gain_value == -26)
+				gain_value = -28;
+
+			if (gain_value == -30)
+				gain_value = -32;
+
+			for (i = 0 ; i < 16 ; i++) {
+				if (hs_analog_gain_table[i] == gain_value) {
+						gain = i;
+						break;
+					}
+			}
+			if (gain == -1)
+				return -1;
+
+			if ((AB8500_REV_10 == acodec_device_id) ||
+					(AB8500_REV_11 == acodec_device_id)) {
+				if (!gain)
+					gain = 1;
+				gain = 0x0F - gain;
+			}
+			initial_val = HW_REG_READ(ANALOG_HS_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(ANALOG_HS_GAIN_REG, ((initial_val &
+			(~R_ANALOG_GAIN_MASK)) | (gain & R_ANALOG_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain HSR gainindex = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+/**
+ * @brief Set gain of earpiece
+ * @channel_index Channel-index of earpiece
+ * @gain_index Gain index of earpiece
+ * @gain_value Gain value of earpiece
+ * @linear
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_set_earpiece_gain(enum AUDIOIO_CH_INDEX channel_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initial_val = 0;
+	if (channel_index & e_CHANNEL_1) {
+		if (0 == gain_index) {
+			int gain = 0;
+			gain = 0 - gain_value;
+
+			initial_val = HW_REG_READ(DA1_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(DA1_DIGITAL_GAIN_REG, ((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain Ear gainindex = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+
+		if (gain_index == 1) {
+			int gain = 0;
+			gain = 8 - gain_value;
+
+			initial_val = HW_REG_READ(HSL_EAR_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(HSL_EAR_DIGITAL_GAIN_REG,
+			((initial_val & (~HS_DIGITAL_GAIN_MASK)) | (gain &
+							HS_DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain Ear gainindex = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+/**
+ * @brief Set gain of vibl
+ * @channel_index Channel-index of vibl
+ * @gain_index Gain index of vibl
+ * @gain_value Gain value of vibl
+ * @linear
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_set_vibl_gain(enum AUDIOIO_CH_INDEX channel_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev)
+{
+
+	int error = 0;
+	unsigned char initial_val = 0;
+
+	if (channel_index & e_CHANNEL_1) {
+		/* Set Gain vibl */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 0 - gain_value;
+
+			initial_val = HW_REG_READ(DA5_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(DA5_DIGITAL_GAIN_REG, ((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain VibL gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+	return error;
+}
+/**
+ * @brief Set gain of vibr
+ * @channel_index Channel-index of vibr
+ * @gain_index Gain index of vibr
+ * @gain_value Gain value of vibr
+ * @linear
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_set_vibr_gain(enum AUDIOIO_CH_INDEX channel_index,
+				u16 gain_index, int gain_value,
+				u32 linear,
+					struct device *dev)
+{
+
+	int error = 0;
+	unsigned char initial_val = 0;
+
+	if (channel_index & e_CHANNEL_1) {
+		/* Set Gain vibr */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 0 - gain_value;
+
+			initial_val = HW_REG_READ(DA6_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(DA6_DIGITAL_GAIN_REG,
+					((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain VibR gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+	return error;
+}
+/**
+ * @brief Set gain of ihf along a specified channel
+ * @channel_index Channel-index of ihf
+ * @gain_index Gain index of ihf
+ * @gain_value Gain value of ihf
+ * @linear
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_set_ihf_gain(enum AUDIOIO_CH_INDEX channel_index,
+				u16 gain_index, int gain_value, u32 linear,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initial_val = 0;
+
+	if (channel_index & e_CHANNEL_1) {
+		/* Set Gain IHFL */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 0 - gain_value;
+
+			initial_val = HW_REG_READ(DA3_DIGITAL_GAIN_REG);
+			error = HW_REG_WRITE(DA3_DIGITAL_GAIN_REG, ((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain IHFL gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+
+		}
+	}
+	if (channel_index & e_CHANNEL_2) {
+		/* Set Gain IHFR */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 0 - gain_value;
+
+			initial_val = HW_REG_READ(DA4_DIGITAL_GAIN_REG);
+			error = HW_REG_WRITE(DA4_DIGITAL_GAIN_REG, ((initial_val
+			&  (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain IHFR gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+
+	return error;
+}
+/**
+ * @brief Set gain of MIC1A & MIC1B
+ * @channel_index Channel-index of MIC1
+ * @gain_index Gain index of MIC1
+ * @gain_value Gain value of MIC1
+ * @linear
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_set_mic1a_gain(enum AUDIOIO_CH_INDEX channel_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initial_val = 0;
+
+	if (channel_index & e_CHANNEL_1) {
+		/* Set Gain mic1 */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 31 - gain_value;
+
+			initial_val = HW_REG_READ(AD3_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(AD3_DIGITAL_GAIN_REG, ((initial_val
+			&  (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain Mic1 gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+
+		}
+
+		if (gain_index == 1) {
+			/* Set Analog gain */
+			initial_val = HW_REG_READ(ANALOG_MIC1_GAIN_REG);
+
+			/* Write gain */
+			error = HW_REG_WRITE(ANALOG_MIC1_GAIN_REG, ((initial_val
+			& (~MIC_ANALOG_GAIN_MASK)) | (gain_value &
+							MIC_ANALOG_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain Mic1 gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+	return error;
+}
+/**
+ * @brief Set gain of MIC2
+ * @channel_index Channel-index of MIC2
+ * @gain_index Gain index of MIC2
+ * @gain_value Gain value of MIC2
+ * @linear
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_set_mic2_gain(enum AUDIOIO_CH_INDEX channel_index,
+				u16 gain_index, int gain_value,
+				u32 linear,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initial_val = 0;
+
+	if (channel_index & e_CHANNEL_1) {
+		/* Set Gain mic2 */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 31 - gain_value;
+
+		    initial_val = HW_REG_READ(AD2_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(AD2_DIGITAL_GAIN_REG, ((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain Mic2 gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+
+		}
+
+		if (gain_index == 1) {
+			/* Set Analog gain */
+			initial_val = HW_REG_READ(ANALOG_MIC2_GAIN_REG);
+
+			/* Write gain */
+			error = HW_REG_WRITE(ANALOG_MIC2_GAIN_REG, ((initial_val
+			& (~MIC_ANALOG_GAIN_MASK)) | (gain_value &
+							MIC_ANALOG_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain Mic2 gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+	return error;
+}
+/**
+ * @brief Set gain of Lin IN along a specified channel
+ * @channel_index Channel-index of Lin In
+ * @gain_index Gain index of Lin In
+ * @gain_value Gain value of Lin In
+ * @linear
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_set_lin_gain(enum AUDIOIO_CH_INDEX channel_index,
+				u16 gain_index, int gain_value, u32 linear,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initial_val = 0;
+
+	if (channel_index & e_CHANNEL_1) {
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 31 - gain_value;
+
+			initial_val = HW_REG_READ(AD1_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(AD1_DIGITAL_GAIN_REG,
+							((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain LinInL gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+
+		}
+
+		if (gain_index == 1) {
+			int gain = 0;
+			/*
+			 * Converting -10 to 20 range into 0 - 15
+			 * & shifting it left by 4 bits
+			 */
+			gain = ((gain_value/2) + 5)<<4;
+
+			initial_val = HW_REG_READ(ANALOG_LINE_IN_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(ANALOG_LINE_IN_GAIN_REG,
+			((initial_val & (~L_ANALOG_GAIN_MASK)) | (gain &
+							L_ANALOG_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain LinInL gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+
+	if (channel_index & e_CHANNEL_2) {
+		/* Set Gain LinInR */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 31 - gain_value;
+
+			initial_val = HW_REG_READ(AD2_DIGITAL_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(AD2_DIGITAL_GAIN_REG,
+							((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain LinInR gain index = %d%d",
+							gain_index, error);
+				return error;
+			}
+		}
+		if (gain_index == 1) {
+			int gain = 0;
+			/* Converting -10 to 20 range into 0 - 15 */
+			gain = ((gain_value/2) + 5);
+
+			initial_val = HW_REG_READ(ANALOG_LINE_IN_GAIN_REG);
+			/* Write gain */
+			error = HW_REG_WRITE(ANALOG_LINE_IN_GAIN_REG,
+			((initial_val & (~R_ANALOG_GAIN_MASK)) | (gain &
+							R_ANALOG_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain LinInR gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+
+	return error;
+}
+/**
+ * @brief Set gain of DMIC12 along a specified channel
+ * @channel_index Channel-index of DMIC12
+ * @gain_index Gain index of DMIC12
+ * @gain_value Gain value of DMIC12
+ * @linear
+ * @return 0 on success otherwise negative error code
+ */
+
+int ste_audio_io_set_dmic12_gain(enum AUDIOIO_CH_INDEX channel_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initial_val = 0;
+
+	if (channel_index & e_CHANNEL_1) {
+		/* Set Gain Dmic1 */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 31 - gain_value;
+
+			initial_val = HW_REG_READ(AD1_DIGITAL_GAIN_REG);
+			error = HW_REG_WRITE(AD1_DIGITAL_GAIN_REG,
+				((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain DMic1 gain index = %d %d",
+							gain_index, error);
+				return error;
+			 }
+		 }
+	 }
+	if (channel_index & e_CHANNEL_2) {
+		/* Set Gain Dmic2 */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 31 - gain_value;
+
+			initial_val = HW_REG_READ(AD2_DIGITAL_GAIN_REG);
+			error = HW_REG_WRITE(AD2_DIGITAL_GAIN_REG,
+					((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain DMic2 gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+	return error;
+}
+
+int ste_audio_io_switch_to_burst_mode_headset(int burst_fifo_switch_frame,
+					struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(BURST_FIFO_INT_CONTROL_REG,
+						WAKEUP_SIGNAL_SAMPLE_COUNT, 0);
+	if (0 != error)
+		return error;
+
+	error = HW_ACODEC_MODIFY_WRITE(BURST_FIFO_LENGTH_REG,
+					BURST_FIFO_TRANSFER_LENGTH, 0);
+	if (0 != error)
+		return error;
+
+	error = HW_ACODEC_MODIFY_WRITE(BURST_FIFO_CONTROL_REG,
+			(BURST_FIFO_INF_RUNNING | BURST_FIFO_INF_IN_MASTER_MODE
+						|PRE_BIT_CLK0_COUNT), 0);
+	if (0 != error)
+		return error;
+
+	error = HW_ACODEC_MODIFY_WRITE(BURST_FIFO_WAKE_UP_DELAY_REG,
+						BURST_FIFO_WAKUP_DEALAY, 0);
+	if (0 != error)
+		return error;
+
+	error = HW_REG_WRITE(BURST_FIFO_SWITCH_FRAME_REG,
+						burst_fifo_switch_frame);
+	if (0 != error)
+		return error;
+
+	error = HW_ACODEC_MODIFY_WRITE(TDM_IF_BYPASS_B_FIFO_REG,
+							IF0_BFifoEn, 0);
+	if (0 != error)
+		return error;
+
+	return error;
+}
+int ste_audio_io_switch_to_normal_mode_headset(
+					struct device *dev)
+{
+	int error = 0;
+
+	error = HW_ACODEC_MODIFY_WRITE(TDM_IF_BYPASS_B_FIFO_REG, 0,
+								IF0_BFifoEn);
+	if (0 != error)
+		return error;
+
+	error = HW_ACODEC_MODIFY_WRITE(BURST_FIFO_INT_CONTROL_REG,
+						0, WAKEUP_SIGNAL_SAMPLE_COUNT);
+	if (0 != error)
+		return error;
+
+	error = HW_ACODEC_MODIFY_WRITE(BURST_FIFO_LENGTH_REG,
+						0, BURST_FIFO_TRANSFER_LENGTH);
+	if (0 != error)
+		return error;
+
+	error = HW_ACODEC_MODIFY_WRITE(BURST_FIFO_CONTROL_REG, 0,
+			(BURST_FIFO_INF_RUNNING | BURST_FIFO_INF_IN_MASTER_MODE
+						|PRE_BIT_CLK0_COUNT));
+	if (0 != error)
+		return error;
+
+	error = HW_ACODEC_MODIFY_WRITE(BURST_FIFO_WAKE_UP_DELAY_REG,
+						0, BURST_FIFO_WAKUP_DEALAY);
+	if (0 != error)
+		return error;
+
+	return error;
+}
+
+
+int ste_audio_io_mute_vibl(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	return 0;
+}
+
+int ste_audio_io_unmute_vibl(enum AUDIOIO_CH_INDEX channel_index, int *gain,
+					struct device *dev)
+{
+	return 0;
+}
+
+int ste_audio_io_mute_vibr(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	return 0;
+}
+int ste_audio_io_unmute_vibr(enum AUDIOIO_CH_INDEX channel_index, int *gain,
+					struct device *dev)
+{
+	return 0;
+}
+
+int ste_audio_io_mute_dmic12(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	if ((channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		error = ste_audio_io_set_dmic12_gain(channel_index, 0, -32,
+									0, dev);
+		if (0 != error) {
+			dev_err(dev, "Mute dmic12 %d", error);
+			return error;
+		}
+	}
+
+	return error;
+
+}
+
+int ste_audio_io_unmute_dmic12(enum AUDIOIO_CH_INDEX channel_index, int *gain,
+					struct device *dev)
+{
+	int error = 0;
+	if ((channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		error = ste_audio_io_set_dmic12_gain(channel_index,
+					0, gain[0], 0, dev);
+		if (0 != error) {
+			dev_err(dev, "UnMute dmic12 %d", error);
+			return error;
+		}
+	}
+	return error;
+}
+int ste_audio_io_enable_fade_dmic12(struct device *dev)
+{
+	return 0;
+}
+
+int ste_audio_io_disable_fade_dmic12(struct device *dev)
+{
+	return 0;
+}
+
+/**
+ * @brief enable hardware loop of dmic12
+ * @chnl_index Channel-index of dmic12
+ * @hw_loop type of hardware loop
+ * @loop_gain gain value to be used in hardware loop
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_enable_loop_dmic12(enum AUDIOIO_CH_INDEX chnl_index,
+					enum AUDIOIO_HAL_HW_LOOPS hw_loop,
+					int loop_gain, struct device *dev,
+					void *cookie)
+{
+	int error = 0;
+	struct transducer_context_t *trnsdr;
+	trnsdr = (struct transducer_context_t *)cookie;
+
+	switch (hw_loop) {
+	/* Check if HSL is active */
+	case AUDIOIO_SIDETONE_LOOP:
+		if (!trnsdr[HS_CH].is_power_up[e_CHANNEL_1]
+			&& !trnsdr[EAR_CH].is_power_up[e_CHANNEL_1]) {
+			error = -EFAULT;
+			dev_err(dev,
+		"Sidetone enable needs HS or Earpiece powered up, err = %d",
+								error);
+			return error;
+		}
+
+		if (chnl_index & e_CHANNEL_1) {
+			/* For ch1, Power On STFIR1, data comes from AD1*/
+			error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG2,
+						0, FIR1_FROMAD1);
+			if (error) {
+				dev_err(dev, "FIR1 data comes from AD_OUT1 %d",
+								error);
+				return error;
+			}
+
+			error = HW_REG_WRITE(SIDETONE_FIR1_GAIN_REG, loop_gain);
+			if (error) {
+				dev_err(dev,
+					"Set FIR1 Gain index = %d", error);
+				return error;
+			}
+		}
+
+		if (chnl_index & e_CHANNEL_2) {
+			/* For ch2, Power On STFIR1, data comes from AD2*/
+			error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG2,
+							0, FIR1_FROMAD2);
+			if (error) {
+				dev_err(dev, "FIR1 data comes from AD_OUT2 %d",
+									error);
+				return error;
+			}
+			error = HW_REG_WRITE(SIDETONE_FIR2_GAIN_REG, loop_gain);
+			if (error) {
+				dev_err(dev,
+					"Set FIR2 Gain error = %d", error);
+				return error;
+			}
+		}
+		break;
+	default:
+		error = -EINVAL;
+		dev_err(dev, "loop not supported %d", error);
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+
+/**
+ * @brief disable hardware loop of dmic12
+ * @chnl_index Channel-index of dmic12
+ * @hw_loop type of hardware loop
+ * @return 0 on success otherwise negative error code
+ */
+int ste_audio_io_disable_loop_dmic12(enum AUDIOIO_CH_INDEX chnl_index,
+					enum AUDIOIO_HAL_HW_LOOPS hw_loop,
+					struct device *dev, void *cookie)
+{
+	int error = -EINVAL;
+	struct transducer_context_t *trnsdr;
+	trnsdr = (struct transducer_context_t *)cookie;
+
+	switch (hw_loop) {
+	/* Check if HSL is active */
+	case AUDIOIO_SIDETONE_LOOP:
+		if (!trnsdr[HS_CH].is_power_up[e_CHANNEL_1]
+			&& !trnsdr[EAR_CH].is_power_up[e_CHANNEL_1]) {
+			error = -EFAULT;
+			dev_err(dev,
+		"Sidetone disable needs HS or Earpiece powered up, err = %d",
+					error);
+			return error;
+		}
+
+		if (chnl_index & e_CHANNEL_1) {
+			/* For ch1, Power On STFIR1, data comes from AD1*/
+			error =
+			HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG2,
+						0, FIR1_FROMAD1);
+			if (error)
+				dev_err(dev, "FIR1 data comes from AD_OUT1 %d",
+								error);
+		}
+
+		if (chnl_index & e_CHANNEL_2) {
+			/* For ch2, Power On STFIR1, data comes from AD2*/
+			error =
+			HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG2,
+						0, FIR1_FROMAD2);
+			if (error)
+				dev_err(dev, "FIR1 data comes from AD_OUT2 %d",
+							error);
+		}
+		error = HW_ACODEC_MODIFY_WRITE(FILTERS_CONTROL_REG,
+						0, FIR_FILTERCONTROL);
+		if (error) {
+			dev_err(dev,
+				"ST FIR Filters disable failed %d", error);
+				return error;
+		}
+		break;
+	default:
+		dev_err(dev, "loop not supported %d", error);
+	}
+	dump_acodec_registers(__func__, dev);
+	return error;
+}
+
+int ste_audio_io_power_up_dmic34(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_AD = 0;
+
+	/* Check if DMic34 request is mono or Stereo */
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "DMic34 does not support more than 2 channels");
+		return -EINVAL;
+	}
+
+	/* Setting Direction for GPIO pins on AB8500 */
+	error = HW_REG_WRITE(AB8500_GPIO_DIR4_REG, GPIO29_DIR_OUTPUT);
+	if (0 != error) {
+		dev_err(dev, "Setting Direction for GPIO pins on AB8500 %d",
+									error);
+		return error;
+	}
+
+	if (channel_index & e_CHANNEL_1) {
+		/* Check if DMIC3 is already powered up or used by Mic1A
+		or Mic1B */
+		initialVal_AD = HW_REG_READ(DIGITAL_AD_CHANNELS_ENABLE_REG);
+
+	if (initialVal_AD & (EN_AD3))
+		return 0;
+
+
+	error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT2_3_REG,
+							DATA_FROM_AD_OUT3, 0);
+	if (0 != error) {
+		dev_err(dev, "Slot 02 outputs data from AD_OUT3 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG, EN_AD3,
+									0);
+	if (0 != error) {
+		dev_err(dev, "Enable AD3 for DMIC3 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DIGITAL_MUXES_REG1,
+					SEL_DMIC3_FOR_AD_OUT3,
+									0);
+	if (0 != error) {
+		dev_err(dev, "Select DMIC3 for AD_OUT3 %d", error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(DMIC_ENABLE_REG, EN_DMIC3, 0);
+	if (0 != error) {
+		dev_err(dev, "Enable DMIC3 %d", error);
+		return error;
+	}
+}
+
+	/* Enable AD4 for Dmic4 */
+	if (channel_index & e_CHANNEL_2) {
+		/* Check if DMIC4 is already powered up */
+		if (initialVal_AD & (EN_AD4))
+			return 0;
+
+
+		error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT2_3_REG,
+						(DATA_FROM_AD_OUT4<<4), 0);
+		if (0 != error) {
+			dev_err(dev, "Slot 03 outputs data from AD_OUT4 %d",
+									error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+								EN_AD4, 0);
+		if (0 != error) {
+			dev_err(dev, "Enable AD4 for DMIC4 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DMIC_ENABLE_REG, EN_DMIC4, 0);
+		if (0 != error) {
+			dev_err(dev, "Enable DMIC4 %d", error);
+			return error;
+		}
+	}
+	return error;
+}
+
+int ste_audio_io_power_down_dmic34(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_AD = 0;
+
+
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "DMic34 does not support more than 2 channels");
+		return -EINVAL;
+	}
+
+	/* Setting Direction for GPIO pins on AB8500 */
+	error = HW_ACODEC_MODIFY_WRITE(AB8500_GPIO_DIR4_REG, 0,
+							GPIO29_DIR_OUTPUT);
+	if (0 != error) {
+		dev_err(dev, "Clearing Direction for GPIO pins on AB8500 %d",
+									error);
+		return error;
+	}
+
+	/* Enable AD1 for Dmic1 */
+	if (channel_index & e_CHANNEL_1) {
+		/* Check if DMIC3 is already powered Down or used by Mic1A
+							or Mic1B */
+		initialVal_AD = HW_REG_READ(DIGITAL_AD_CHANNELS_ENABLE_REG);
+		if (!(initialVal_AD & EN_AD3))
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(DMIC_ENABLE_REG, 0, EN_DMIC3);
+		if (0 != error) {
+			dev_err(dev, "Enable DMIC3 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+								0,
+								EN_AD3);
+		if (0 != error) {
+			dev_err(dev, "Disable AD3 for DMIC3 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT2_3_REG, 0,
+							DATA_FROM_AD_OUT3);
+		if (0 != error) {
+			dev_err(dev,
+			"Slot 02 outputs data cleared from AD_OUT3 %d",
+									error);
+			return error;
+		}
+	}
+
+	/* Enable AD4 for Dmic4 */
+	if (channel_index & e_CHANNEL_2) {
+		/* Check if DMIC4 is already powered down */
+		initialVal_AD = HW_REG_READ(DIGITAL_AD_CHANNELS_ENABLE_REG);
+		if (!(initialVal_AD & EN_AD4))
+			return 0;
+
+		error = HW_ACODEC_MODIFY_WRITE(DMIC_ENABLE_REG, 0, EN_DMIC4);
+		if (0 != error) {
+			dev_err(dev, "Enable DMIC4 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(DIGITAL_AD_CHANNELS_ENABLE_REG,
+							0, EN_AD4);
+		if (0 != error) {
+			dev_err(dev, "Disable AD4 for DMIC4 %d", error);
+			return error;
+		}
+
+		error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT2_3_REG, 0,
+						(DATA_FROM_AD_OUT4<<4));
+		if (0 != error) {
+			dev_err(dev,
+			"Slot 03 outputs data cleared from AD_OUT4 %d",
+									error);
+			return error;
+		}
+	}
+	return error;
+}
+int ste_audio_io_set_dmic34_gain(enum AUDIOIO_CH_INDEX channel_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initial_val = 0;
+
+	if (channel_index & e_CHANNEL_1) {
+		/* Set Gain Dmic3 */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 31 - gain_value;
+
+			initial_val = HW_REG_READ(AD3_DIGITAL_GAIN_REG);
+			error = HW_REG_WRITE(AD3_DIGITAL_GAIN_REG,
+						((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain DMic3 gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+
+	if (channel_index & e_CHANNEL_2) {
+		/* Set Gain Dmic4 */
+		if (gain_index == 0) {
+			int gain = 0;
+			gain = 31 - gain_value;
+
+			initial_val = HW_REG_READ(AD4_DIGITAL_GAIN_REG);
+			error = HW_REG_WRITE(AD4_DIGITAL_GAIN_REG,
+						((initial_val
+			& (~DIGITAL_GAIN_MASK)) | (gain & DIGITAL_GAIN_MASK)));
+
+			if (0 != error) {
+				dev_err(dev,
+					"Set Gain DMic4 gain index = %d %d",
+							gain_index, error);
+				return error;
+			}
+		}
+	}
+
+	return error;
+}
+int ste_audio_io_mute_dmic34(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	if ((channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		error = ste_audio_io_set_dmic34_gain(channel_index, 0, -32,
+									0, dev);
+		if (0 != error) {
+			dev_err(dev, "Mute dmic34 %d", error);
+			return error;
+		}
+	}
+	return error;
+}
+int ste_audio_io_unmute_dmic34(enum AUDIOIO_CH_INDEX channel_index, int *gain,
+					struct device *dev)
+{
+	int error = 0;
+	if ((channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		error = ste_audio_io_set_dmic34_gain(channel_index,
+					0, gain[0], 0, dev);
+		if (0 != error) {
+			dev_err(dev, "UnMute dmic34 %d", error);
+			return error;
+		}
+	}
+	return error;
+}
+int ste_audio_io_enable_fade_dmic34(struct device *dev)
+{
+	return 0;
+}
+
+int ste_audio_io_disable_fade_dmic34(struct device *dev)
+{
+	return 0;
+}
+
+int ste_audio_io_power_up_dmic56(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	return 0;
+}
+int ste_audio_io_power_down_dmic56(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	return 0;
+}
+int ste_audio_io_set_dmic56_gain(enum AUDIOIO_CH_INDEX channel_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev)
+{
+	return 0;
+}
+int ste_audio_io_mute_dmic56(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	return 0;
+}
+int ste_audio_io_unmute_dmic56(enum AUDIOIO_CH_INDEX channel_index, int *gain,
+					struct device *dev)
+{
+	return 0;
+}
+int ste_audio_io_enable_fade_dmic56(struct device *dev)
+{
+	return 0;
+}
+
+int ste_audio_io_disable_fade_dmic56(struct device *dev)
+{
+	return 0;
+}
+
+int ste_audio_io_configure_if1(struct device *dev)
+{
+	int error = 0;
+
+	error = HW_REG_WRITE(IF1_CONF_REG, IF_DELAYED |
+			I2S_LEFT_ALIGNED_FORMAT | WORD_LENGTH_16);
+	if (error != 0) {
+		dev_err(dev,
+		"Configure IF1: I2S Format 16 Bits word length error = %d",
+							error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(TDM_IF_BYPASS_B_FIFO_REG, IF1_MASTER, 0);
+	if (error != 0) {
+		dev_err(dev,
+			"Configure IF1: IF1 master error = %d",
+								error);
+		return error;
+	}
+
+	error = HW_ACODEC_MODIFY_WRITE(IF0_IF1_MASTER_CONF_REG,
+							EN_FSYNC_BITCLK1, 0);
+	if (error != 0) {
+		dev_err(dev,
+	"ConfigIF1 bitclk is 32x48KHz, enable Fsync1 and Bitclk1 error = %d",
+								error);
+		return error;
+	}
+	return error;
+}
+
+int ste_audio_io_power_up_fmrx(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal = 0;
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "FMRX should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	ste_audio_io_configure_if1(dev);
+
+	if (channel_index & e_CHANNEL_1) {
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA7_REG,
+							SLOT24_FOR_DA_PATH, 0);
+		if (0 != error) {
+			dev_err(dev, "Data sent to DA_IN7 from Slot 24 %d",
+									error);
+			return error;
+		}
+		/* DA_IN7 to AD_OUT8 path */
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA5_REG,
+						SEL_AD_OUT8_FROM_DAIN7, 0);
+		if (0 != error) {
+			dev_err(dev, "Data sent to AD_OUT5 from DA_IN7 %d",
+									error);
+			return error;
+		}
+
+		initialVal = HW_REG_READ(AD_ALLOCATION_TO_SLOT6_7_REG);
+		error = HW_REG_WRITE(AD_ALLOCATION_TO_SLOT6_7_REG,
+			((initialVal & MASK_QUARTET1)|SEL_IF6_FROM_AD_OUT5));
+		if (0 != error) {
+			dev_err(dev, "Data sent to IF slot 6 from AD_OUT5 %d",
+									error);
+			return error;
+		}
+	}
+
+	if (channel_index & e_CHANNEL_2) {
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA8_REG,
+							SLOT25_FOR_DA_PATH, 0);
+		if (0 != error) {
+			dev_err(dev, "Data sent to DA_IN8 from Slot 25 %d",
+									error);
+			return error;
+		}
+
+		/* DA_IN7 to AD_OUT8 path */
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA6_REG,
+						SEL_AD_OUT6_FROM_DAIN8, 0);
+		if (0 != error) {
+			dev_err(dev, "Data sent to AD_OUT6 from DA_IN8 %d",
+									error);
+			return error;
+		}
+
+		initialVal = HW_REG_READ(AD_ALLOCATION_TO_SLOT6_7_REG);
+
+		error = HW_REG_WRITE(AD_ALLOCATION_TO_SLOT6_7_REG,
+		(initialVal & MASK_QUARTET0)|SEL_IF7_FROM_AD_OUT6);
+							/* 5x is written */
+		if (0 != error) {
+			dev_err(dev, "Data sent to IF7 from AD_OUT6  %d",
+									error);
+			return error;
+		}
+	}
+	return error;
+}
+int ste_audio_io_power_down_fmrx(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "FMRX should have mono or stereo channels");
+		return -EINVAL;
+	}
+	if (channel_index & e_CHANNEL_1) {
+		/* data sent to DA7 input of DA filter form IF1 */
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA7_REG, 0,
+							SLOT24_FOR_DA_PATH);
+	if (0 != error) {
+		dev_err(dev, "Clearing Data sent to DA_IN7 from Slot 24 %d",
+									error);
+		return error;
+	}
+	/* DA_IN7 to AD_OUT8 path */
+	error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA5_REG, 0,
+						SEL_AD_OUT8_FROM_DAIN7);
+	if (0 != error) {
+		dev_err(dev, "Clearing Data sent to AD_OUT5 from DA_IN7 %d",
+									error);
+		return error;
+	}
+	error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT6_7_REG, 0,
+						SEL_IF6_FROM_AD_OUT5);
+	if (0 != error) {
+		dev_err(dev,
+			"Clearing Data sent to IF slot 6 from AD_OUT5 %d",
+									error);
+		return error;
+	}
+}
+
+	if (channel_index & e_CHANNEL_2) {
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA8_REG, 0,
+						SLOT25_FOR_DA_PATH);
+		if (0 != error) {
+			dev_err(dev,
+			"Clearing Data sent to DA_IN8 from Slot 25 %d",
+									error);
+			return error;
+		}
+
+		/* DA_IN7 to AD_OUT8 path */
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA6_REG, 0,
+						SEL_AD_OUT6_FROM_DAIN8);
+		if (0 != error) {
+			dev_err(dev,
+			"Clearing Data sent to AD_OUT6 from DA_IN8 %d",
+									error);
+			return error;
+			}
+		error = HW_ACODEC_MODIFY_WRITE(AD_ALLOCATION_TO_SLOT6_7_REG, 0,
+							SEL_IF7_FROM_AD_OUT6);
+		if (0 != error) {
+			dev_err(dev,
+				"Clearing Data sent to IF7 from AD_OUT6 %d",
+									error);
+			return error;
+		}
+	}
+	return error;
+}
+
+int ste_audio_io_power_up_fmtx(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal = 0;
+
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "FMTX should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	ste_audio_io_configure_if1(dev);
+
+	if (channel_index & e_CHANNEL_1) {
+		/* data sent to DA7 input of DA filter form IF1 14 slot */
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA7_REG,
+							SLOT14_FOR_DA_PATH, 0);
+		if (0 != error) {
+			dev_err(dev,
+				"Data sent to DA_IN7 from Slot 14 %d", error);
+			return error;
+		}
+		/* DA_IN7 to AD_OUT5 path */
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA5_REG,
+						SEL_AD_OUT5_FROM_DAIN7, 0);
+		if (0 != error) {
+			dev_err(dev, "Data sent to AD_OUT5 from DA_IN7 %d",
+									error);
+			return error;
+		}
+
+		initialVal = HW_REG_READ(AD_ALLOCATION_TO_SLOT16_17_REG);
+		error = HW_REG_WRITE(AD_ALLOCATION_TO_SLOT16_17_REG,
+			(initialVal & MASK_QUARTET1)|SEL_IF6_FROM_AD_OUT5);
+		if (0 != error) {
+			dev_err(dev, "Data sent to IF16 from AD_OUT5  %d",
+									error);
+			return error;
+		}
+	}
+
+	if (channel_index & e_CHANNEL_2) {
+		/* data sent to DA8 input of DA filter */
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA8_REG,
+							SLOT15_FOR_DA_PATH, 0);
+		if (0 != error) {
+			dev_err(dev, "Data sent to DA_IN8 from  Slot 15 %d",
+									error);
+			return error;
+		}
+
+		/* DA_IN8 to AD_OUT6 path */
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA6_REG,
+						SEL_AD_OUT6_FROM_DAIN8, 0);
+		if (0 != error) {
+			dev_err(dev, "Data sent to AD_OUT6 from DA_IN8 %d",
+									error);
+			return error;
+		}
+
+		initialVal = HW_REG_READ(AD_ALLOCATION_TO_SLOT16_17_REG);
+		error = HW_REG_WRITE(AD_ALLOCATION_TO_SLOT16_17_REG,
+			(initialVal & MASK_QUARTET0)|SEL_IF17_FROM_AD_OUT6);
+		if (0 != error) {
+			dev_err(dev, "Data sent to IF17 from AD_OUT6  %d",
+								error);
+			return error;
+		}
+	}
+	return error;
+}
+
+int ste_audio_io_power_down_fmtx(enum AUDIOIO_CH_INDEX channel_index,
+					struct device *dev)
+{
+	int error = 0;
+	unsigned char initialVal_AD = 0;
+
+	if (!(channel_index & (e_CHANNEL_1 | e_CHANNEL_2))) {
+		dev_err(dev, "FMTX should have mono or stereo channels");
+		return -EINVAL;
+	}
+
+	if (channel_index & e_CHANNEL_1) {
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA7_REG, 0,
+							SLOT14_FOR_DA_PATH);
+		if (0 != error) {
+			dev_err(dev,
+			"Clearing Data sent to DA_IN7 from Slot 14 %d",
+									error);
+			return error;
+		}
+		/* DA_IN7 to AD_OUT8 path */
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA5_REG, 0,
+						SEL_AD_OUT5_FROM_DAIN7);
+		if (0 != error) {
+			dev_err(dev,
+			"Clearing Data sent to AD_OUT5 from DA_IN7 %d",
+								error);
+			return error;
+		}
+		error = HW_REG_WRITE(AD_ALLOCATION_TO_SLOT16_17_REG,
+							SEL_IF6_FROM_AD_OUT5);
+		if (0 != error) {
+			dev_err(dev,
+				"Clearing Data sent to IF16 from AD_OUT8 %d",
+								error);
+			return error;
+		}
+	}
+
+	 if (channel_index & e_CHANNEL_2) {
+		/* data sent to DA8 input of DA filter */
+		initialVal_AD = HW_REG_READ(SLOT_SELECTION_TO_DA8_REG);
+
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA8_REG, 0,
+							SLOT15_FOR_DA_PATH);
+		if (0 != error) {
+			dev_err(dev,
+			"Clearing Data sent to DA_IN8 from Slot 15 %d",
+									error);
+			return error;
+		}
+
+		/* DA_IN7 to AD_OUT8 path */
+		error = HW_ACODEC_MODIFY_WRITE(SLOT_SELECTION_TO_DA6_REG, 0,
+						SEL_AD_OUT6_FROM_DAIN8);
+		if (0 != error) {
+			dev_err(dev,
+			"Clearing Data sent to AD_OUT6 from DA_IN8 %d",
+									error);
+			return error;
+		}
+		error = HW_REG_WRITE(AD_ALLOCATION_TO_SLOT16_17_REG,
+							SEL_IF17_FROM_AD_OUT6);
+		if (0 != error) {
+			dev_err(dev,
+				"Clearing Data sent to IF17 from AD_OUT6 %d",
+									error);
+			return error;
+		}
+	}
+	return error;
+}
+int ste_audio_io_power_up_bluetooth(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev)
+{
+	int error = 0;
+	struct ab8500 *ab8500 = dev_get_drvdata(dev->parent);
+	struct ab8500_platform_data *pdata = dev_get_platdata(ab8500->dev);
+	if (bluetooth_power_up_count++)
+		return error;
+
+	if (pdata) {
+		if (pdata->audio) {
+			error = pdata->audio->ste_gpio_altf_init();
+			if (error == 0) {
+				clk_ptr_msp0 = clk_get_sys("msp0", NULL);
+				if (!IS_ERR(clk_ptr_msp0)) {
+					error = clk_enable(clk_ptr_msp0);
+					return error;
+				} else
+					return -EFAULT;
+			}
+		}
+	}
+	return error;
+}
+
+int ste_audio_io_power_down_bluetooth(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev)
+{
+	int error = 0;
+	struct ab8500 *ab8500 = dev_get_drvdata(dev->parent);
+	struct ab8500_platform_data *pdata = dev_get_platdata(ab8500->dev);
+
+	if (--bluetooth_power_up_count)
+		return error;
+
+	if (pdata) {
+		if (pdata->audio) {
+			error = pdata->audio->ste_gpio_altf_exit();
+			if (error == 0) {
+				clk_disable(clk_ptr_msp0);
+				clk_put(clk_ptr_msp0);
+			}
+		}
+	}
+	return error;
+}
+
+int dump_acodec_registers(const char *str, struct device *dev)
+{
+	int reg_count = REVISION_REG & 0xff;
+	if (1 == acodec_reg_dump) {
+		u8 i = 0;
+		dev_info(dev, "\n func  : %s\n", str);
+		for (i = 0; i <= reg_count; i++)
+			dev_info(dev,
+				"block = 0x0D, adr = %x = %x\n",
+				i, HW_REG_READ((AB8500_AUDIO << 8) | i));
+	}
+	str = str; /* keep compiler happy */
+	return 0;
+}
+
+int debug_audioio(int x)
+{
+
+	if (1 == x)
+		acodec_reg_dump = 1;
+	else
+		acodec_reg_dump = 0;
+	return 0;
+}
+
+
+
+
+
diff --git a/drivers/misc/audio_io_dev/ste_audio_io_func.h b/drivers/misc/audio_io_dev/ste_audio_io_func.h
new file mode 100644
index 00000000000..65b777f5436
--- /dev/null
+++ b/drivers/misc/audio_io_dev/ste_audio_io_func.h
@@ -0,0 +1,358 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Deepak KARDA/ deepak.karda@stericsson.com for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#ifndef _AUDIOIO_FUNC_H_
+#define _AUDIOIO_FUNC_H_
+
+#include <linux/string.h>
+#include <linux/platform_device.h>
+#include <mach/ste_audio_io_ioctl.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+
+#define AB8500_REV_10		0x10
+#define AB8500_REV_11		0x11
+#define AB8500_REV_20		0x20
+
+#define AB8500_CTRL3_REG		0x00000200
+#define AB8500_GPIO_DIR4_REG		0x00001013
+#define AB8500_GPIO_DIR5_REG		0x00001014
+#define AB8500_GPIO_OUT5_REG		0x00001024
+
+extern struct platform_device *ste_audio_io_device;
+extern struct regulator *regulator_avsource;
+
+int dump_acodec_registers(const char *, struct device *dev);
+int debug_audioio(int  x);
+
+#define AB8500_BLOCK_ADDR(address) ((address >> 8) & 0xff)
+#define AB8500_OFFSET_ADDR(address) (address & 0xff)
+
+static inline unsigned char HW_REG_READ(unsigned short reg)
+{
+	unsigned char ret;
+	int err;
+
+	err = abx500_get_register_interruptible(&ste_audio_io_device->dev,
+					  AB8500_BLOCK_ADDR(reg),
+					  AB8500_OFFSET_ADDR(reg),
+					  &ret);
+	if (err < 0)
+		return err;
+	else
+		return ret;
+}
+
+static inline int HW_REG_WRITE(unsigned short reg, unsigned char data)
+{
+	return abx500_set_register_interruptible(&ste_audio_io_device->dev,
+						 AB8500_BLOCK_ADDR(reg),
+						 AB8500_OFFSET_ADDR(reg),
+						 data);
+}
+
+unsigned int ab8500_acodec_modify_write(unsigned int reg, u8 mask_set,
+					u8 mask_clear);
+
+#define HW_ACODEC_MODIFY_WRITE(reg, mask_set, mask_clear)\
+	ab8500_acodec_modify_write(reg, mask_set, mask_clear)
+
+unsigned int ab8500_modify_write(unsigned int reg, u8 mask_set, u8 mask_clear);
+
+int ste_audio_io_power_up_headset(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_headset(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_headset_query(struct device *dev);
+int ste_audio_io_set_headset_gain(enum AUDIOIO_CH_INDEX chnl_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_headset_gain(int *, int *, u16,
+					struct device *dev);
+int ste_audio_io_mute_headset(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_headset(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_headset_state(struct device *dev);
+int ste_audio_io_enable_fade_headset(struct device *dev);
+int ste_audio_io_disable_fade_headset(struct device *dev);
+int ste_audio_io_switch_to_burst_mode_headset(int burst_fifo_switch_frame,
+					struct device *dev);
+int ste_audio_io_switch_to_normal_mode_headset(
+					struct device *dev);
+
+int ste_audio_io_power_up_earpiece(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_earpiece(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_earpiece_query(struct device *dev);
+int ste_audio_io_set_earpiece_gain(enum AUDIOIO_CH_INDEX chnl_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_earpiece_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_earpiece(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_earpiece(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_earpiece_state(struct device *dev);
+int ste_audio_io_enable_fade_earpiece(struct device *dev);
+int ste_audio_io_disable_fade_earpiece(struct device *dev);
+
+int ste_audio_io_power_up_ihf(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_ihf(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_ihf_query(struct device *dev);
+int ste_audio_io_set_ihf_gain(enum AUDIOIO_CH_INDEX chnl_index, u16 gain_index,
+					int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_ihf_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_ihf(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_ihf(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_ihf_state(struct device *dev);
+int ste_audio_io_enable_fade_ihf(struct device *dev);
+int ste_audio_io_disable_fade_ihf(struct device *dev);
+
+int ste_audio_io_power_up_vibl(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_vibl(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_vibl_query(struct device *dev);
+int ste_audio_io_set_vibl_gain(enum AUDIOIO_CH_INDEX chnl_index, u16 gain_index,
+					int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_vibl_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_vibl(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_vibl(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_vibl_state(struct device *dev);
+int ste_audio_io_enable_fade_vibl(struct device *dev);
+int ste_audio_io_disable_fade_vibl(struct device *dev);
+
+int ste_audio_io_power_up_vibr(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_vibr(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_vibr_query(struct device *dev);
+int ste_audio_io_set_vibr_gain(enum AUDIOIO_CH_INDEX chnl_index, u16 gain_index,
+					int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_vibr_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_vibr(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_vibr(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_vibr_state(struct device *dev);
+int ste_audio_io_enable_fade_vibr(struct device *dev);
+int ste_audio_io_disable_fade_vibr(struct device *dev);
+
+int ste_audio_io_power_up_mic1a(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_mic1a(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_mic1a_query(struct device *dev);
+int ste_audio_io_set_mic1a_gain(enum AUDIOIO_CH_INDEX chnl_index,
+	u16 gain_index, int gain_value, u32 linear, struct device *dev);
+int ste_audio_io_get_mic1a_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_mic1a(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_mic1a(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_mic1a_state(struct device *dev);
+int ste_audio_io_enable_fade_mic1a(struct device *dev);
+int ste_audio_io_disable_fade_mic1a(struct device *dev);
+
+/*
+ *** Mic1b ***
+ */
+int ste_audio_io_power_up_mic1b(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_mic1b(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_mic1b_query(struct device *dev);
+int ste_audio_io_set_mic1b_gain(enum AUDIOIO_CH_INDEX chnl_index,
+		u16 gain_index, int gain_value, u32 linear, struct device *dev);
+int ste_audio_io_get_mic1b_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_mic1b(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_mic1b(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_mic1b_state(struct device *dev);
+int ste_audio_io_enable_fade_mic1b(struct device *dev);
+int ste_audio_io_disable_fade_mic1b(struct device *dev);
+int ste_audio_io_enable_loop_mic1b(enum AUDIOIO_CH_INDEX chnl_index,
+					enum AUDIOIO_HAL_HW_LOOPS,
+					int loop_gain, struct device *dev,
+					void *cookie);
+int ste_audio_io_disable_loop_mic1b(enum AUDIOIO_CH_INDEX chnl_index,
+					enum AUDIOIO_HAL_HW_LOOPS hw_loop,
+					struct device *dev, void *cookie);
+/*
+ *** Mic2 ***
+ */
+int ste_audio_io_power_up_mic2(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_mic2(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_mic2_query(struct device *dev);
+int ste_audio_io_set_mic2_gain(enum AUDIOIO_CH_INDEX chnl_index, u16 gain_index,
+					int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_mic2_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_mic2(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_mic2(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_mic2_state(struct device *dev);
+int ste_audio_io_enable_fade_mic2(struct device *dev);
+int ste_audio_io_disable_fade_mic2(struct device *dev);
+
+int ste_audio_io_power_up_lin(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_lin(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_lin_query(struct device *dev);
+int ste_audio_io_set_lin_gain(enum AUDIOIO_CH_INDEX chnl_index, u16 gain_index,
+					int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_lin_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_lin(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_lin(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_lin_state(struct device *dev);
+int ste_audio_io_enable_fade_lin(struct device *dev);
+int ste_audio_io_disable_fade_lin(struct device *dev);
+
+int ste_audio_io_power_up_dmic12(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_dmic12(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_dmic12_query(struct device *dev);
+int ste_audio_io_set_dmic12_gain(enum AUDIOIO_CH_INDEX chnl_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_dmic12_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_dmic12(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_dmic12(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_dmic12_state(struct device *dev);
+int ste_audio_io_enable_fade_dmic12(struct device *dev);
+int ste_audio_io_disable_fade_dmic12(struct device *dev);
+int ste_audio_io_enable_loop_dmic12(enum AUDIOIO_CH_INDEX chnl_index,
+					enum AUDIOIO_HAL_HW_LOOPS,
+					int loop_gain, struct device *dev,
+					void *cookie);
+int ste_audio_io_disable_loop_dmic12(enum AUDIOIO_CH_INDEX chnl_index,
+					enum AUDIOIO_HAL_HW_LOOPS hw_loop,
+					struct device *dev, void *cookie);
+
+int ste_audio_io_power_up_dmic34(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_dmic34(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_dmic34_query(struct device *dev);
+int ste_audio_io_set_dmic34_gain(enum AUDIOIO_CH_INDEX chnl_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_dmic34_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_dmic34(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_dmic34(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_dmic34_state(struct device *dev);
+int ste_audio_io_enable_fade_dmic34(struct device *dev);
+int ste_audio_io_disable_fade_dmic34(struct device *dev);
+
+int ste_audio_io_power_up_dmic56(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_dmic56(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_dmic56_query(struct device *dev);
+int ste_audio_io_set_dmic56_gain(enum AUDIOIO_CH_INDEX chnl_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_dmic56_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_dmic56(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_dmic56(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_dmic56_state(struct device *dev);
+int ste_audio_io_enable_fade_dmic56(struct device *dev);
+int ste_audio_io_disable_fade_dmic56(struct device *dev);
+
+int ste_audio_io_power_up_fmrx(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_fmrx(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_fmrx_query(struct device *dev);
+int ste_audio_io_set_fmrx_gain(enum AUDIOIO_CH_INDEX chnl_index, u16 gain_index,
+					int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_fmrx_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_fmrx(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_fmrx(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_fmrx_state(struct device *dev);
+int ste_audio_io_enable_fade_fmrx(struct device *dev);
+int ste_audio_io_disable_fade_fmrx(struct device *dev);
+
+int ste_audio_io_power_up_fmtx(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_fmtx(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_fmtx_query(struct device *dev);
+int ste_audio_io_set_fmtx_gain(enum AUDIOIO_CH_INDEX chnl_index, u16 gain_index,
+					int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_fmtx_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_fmtx(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_fmtx(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_fmtx_state(struct device *dev);
+int ste_audio_io_enable_fade_fmtx(struct device *dev);
+int ste_audio_io_disable_fade_fmtx(struct device *dev);
+
+int ste_audio_io_power_up_bluetooth(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_down_bluetooth(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_power_state_bluetooth_query(struct device *dev);
+int ste_audio_io_set_bluetooth_gain(enum AUDIOIO_CH_INDEX chnl_index,
+			u16 gain_index, int gain_value, u32 linear,
+					struct device *dev);
+int ste_audio_io_get_bluetooth_gain(int*, int*, u16,
+					struct device *dev);
+int ste_audio_io_mute_bluetooth(enum AUDIOIO_CH_INDEX chnl_index,
+					struct device *dev);
+int ste_audio_io_unmute_bluetooth(enum AUDIOIO_CH_INDEX chnl_index, int *gain,
+					struct device *dev);
+int ste_audio_io_mute_bluetooth_state(struct device *dev);
+int ste_audio_io_enable_fade_bluetooth(struct device *dev);
+int ste_audio_io_disable_fade_bluetooth(struct device *dev);
+
+
+#endif
+
diff --git a/drivers/misc/audio_io_dev/ste_audio_io_hwctrl_common.c b/drivers/misc/audio_io_dev/ste_audio_io_hwctrl_common.c
new file mode 100644
index 00000000000..c2409f849ae
--- /dev/null
+++ b/drivers/misc/audio_io_dev/ste_audio_io_hwctrl_common.c
@@ -0,0 +1,189 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Deepak KARDA/ deepak.karda@stericsson.com for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+
+#include <linux/types.h>
+#include "ste_audio_io_hwctrl_common.h"
+
+/* Number of channels for each transducer */
+const uint transducer_no_of_channels[MAX_NO_TRANSDUCERS] = {
+	1, /* Earpiece */
+	2, /* HS */
+	2, /* IHF */
+	1, /* VibL */
+	1, /* VibR */
+	1, /* Mic1A */
+	1, /* Mic1B */
+	1, /* Mic2 */
+	2, /* LinIn */
+	2, /* DMIC12 */
+	2, /* DMIC34 */
+	2, /* /DMIC56 */
+	4	/* MultiMic */
+	};
+
+/* Maximum number of gains in each transducer path
+ (all channels of a specific transducer have same max no of gains) */
+const uint	transducer_no_of_gains[MAX_NO_TRANSDUCERS] = {
+	2, /* Ear		g3 and g4 */
+	3, /* HS		g3 and g4 and analog */
+	1, /* IHF		g3 */
+	1, /* VibL		g3 */
+	1, /* VibR		g3 */
+	2, /* Mic1A		g1 and analog */
+	2, /* Mic1A		g1 and analog */
+	2, /* Mic2		g1 and analog */
+	2, /* LinIn		g1 and analog */
+	1, /* DMIC12	g1 */
+	1, /* DMIC34	g1 */
+	1, /* DMIC56	g1 */
+	1	/* MultiMic	g1 */
+	};
+
+const uint transducer_no_Of_supported_loop_indexes[MAX_NO_TRANSDUCERS] = {
+	0x09,/* Ear0x01|0x08*/
+	0x38770,/*{0x01|0x10|0x20|0x40|0x100*/
+	/*|0x200|0x400|0x8000|0x10000|0x20000}, HS*/
+	0x86,/*IHF*/
+	0x0,/*VibL*/
+	0x0,/*VibR*/
+	0x0,/*Mic1A*/
+	0x01,/*Mic1B    Sidetone is controlled*/
+	0x0,/*Mic2*/
+	0x0,/*LinIn*/
+	0x0,/*DMIC12*/
+	0x0,/*DMIC34*/
+	0x0,/*DMIC56*/
+	0x01,/*MultiMic Sidetone is controlled*/
+	0x0,/*FMRx*/
+	0x0/*FMTx*/
+	};
+
+const uint transducer_max_no_Of_supported_loops[MAX_NO_TRANSDUCERS] = {
+	0,/*Ear		Sidetone*/
+	2,/*HS SideTone LININ_HS LININR_HSR  LININ_HSL*/
+	1,/*IHF        TO BE DEFINED*/
+	0,/*VibL      TO BE DEFINED*/
+	0,/*VibR      TO BE DEFINED*/
+	1,/*Mic1A   TO BE DEFINED*/
+	1,/*Mic1B    SIDETONE TO BE DEFINED*/
+	1,/*Mic2    TO BE DEFINED*/
+	0, /* LinIn */
+	1,/*DMIC12-ANC*/
+	0,/*DMIC34-ANC*/
+	0, /* DMIC56 */
+	1,/*MultiMic-SIDETONE ANC*/
+	0,/*FMRx*/
+	0/*FMTx*/
+	};
+
+const uint max_no_of_loop_gains[MAX_NO_TRANSDUCERS] = {
+	0,/*Earpiece*/
+	2,/*HS*/
+	0,
+	0,
+	0,
+	0,
+	2,/*Mic1B-Sidetone 2 gains*/
+	0,
+	0,
+	2,/*DMIC12*/
+	0,
+	0,
+	2,/*Multimic, Sidetone max no gains = 2*/
+	0,
+	0
+	};
+
+
+struct gain_descriptor_t gain_descriptor[MAX_NO_TRANSDUCERS]\
+		[MAX_NO_CHANNELS][MAX_NO_GAINS] = {
+	/* gainIndex=0	1			2
+	EDestinationEar */
+	{{{-63, 0, 1}, {-1, 8, 1}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,/* channelIndex=3 */
+
+	/* EDestinationHS */
+	{{{-63, 0, 1}, {-1, 8, 1}, {-32, 4, 2} } , /* channelIndex=0 */
+	{{-63, 0, 1}, {-1, 8, 1}, {-32, 4, 2} } , /* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,   /* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,   /* channelIndex=3 */
+
+	/* EDestinationIHF */
+	{{{-63, 0, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{-63, 0, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,/* channelIndex=3 */
+
+	/* EDestinationVibL */
+	{{{-63, 0, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,/* channelIndex=3 */
+
+	/* EDestinationVibR */
+	{{{-63, 0, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,/* channelIndex=3 */
+
+	/* ESourceMic1A */
+	{{{-32, 31, 1}, {0, 31, 1}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,/* channelIndex=3 */
+
+	/* ESourceMic1B */
+	{{{-32, 31, 1}, {0, 31, 1}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,/* channelIndex=3 */
+
+	/* ESourceMic2 */
+	{{{-32, 31, 1}, {0, 31, 1}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,/* channelIndex=3 */
+
+	/* ESourceLin */
+	{{{-32, 31, 1}, {-10, 20, 2}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{-32, 31, 1}, {-10, 20, 2}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,/* channelIndex=3 */
+
+	/* ESourceDMic12 */
+	{{{-32, 31, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{-32, 31, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,/* channelIndex=3 */
+
+	/* ESourceDMic34 */
+	{{{-32, 31, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{-32, 31, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,/* channelIndex=3 */
+
+	/* ESourceDMic56 */
+	{{{-32, 31, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{-32, 31, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=2 */
+	{{0, 0, 0}, {0, 0, 0}, {0, 0, 0} } } ,/* channelIndex=3 */
+
+	/* ESourceMultiMic */
+	{{{-32, 31, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=0 */
+	{{-32, 31, 1}, {0, 0, 0}, {0, 0, 0} } ,/* channelIndex=1 */
+	{{-32, 31, 1}, {0, 0, 0}, {0, 0, 0} },/* channelIndex=2 */
+	{{-32, 31, 1}, {0, 0, 0}, {0, 0, 0} } } /* channelIndex=3 */
+};
+
+
+const int hs_analog_gain_table[16] = {4, 2, 0, -2, -4, -6, -8, -10,
+				-12, -14, -16, -18, -20, -24, -28, -32};
+
+
+
diff --git a/drivers/misc/audio_io_dev/ste_audio_io_hwctrl_common.h b/drivers/misc/audio_io_dev/ste_audio_io_hwctrl_common.h
new file mode 100644
index 00000000000..cc2bfe21d81
--- /dev/null
+++ b/drivers/misc/audio_io_dev/ste_audio_io_hwctrl_common.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Deepak KARDA/ deepak.karda@stericsson.com for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#ifndef __AUDIOIO_HWCTRL_COMMON_H__
+#define __AUDIOIO_HWCTRL_COMMON_H__
+
+#include <linux/types.h>
+#include <mach/ste_audio_io_ioctl.h>
+/*
+ * Defines
+ */
+
+#define MAX_GAIN				100
+#define MIN_GAIN				0
+#define	MAX_NO_CHANNELS				4
+#define	MAX_NO_GAINS				3
+#define	MAX_NO_LOOPS				1
+#define	MAX_NO_LOOP_GAINS			1
+
+struct gain_descriptor_t {
+	int min_gain;
+	int max_gain;
+	uint gain_step;
+};
+
+
+/* Number of channels for each transducer */
+extern const uint transducer_no_of_channels[MAX_NO_TRANSDUCERS];
+
+/*
+ * Maximum number of gains in each transducer path
+ * all channels of a specific transducer have same max no of gains
+ */
+extern const uint transducer_no_of_gains[MAX_NO_TRANSDUCERS];
+
+/* Maximum number of supported loops for each transducer */
+extern const uint transducer_no_Of_supported_loop_indexes[MAX_NO_TRANSDUCERS];
+extern const uint transducer_max_no_Of_supported_loops[MAX_NO_TRANSDUCERS];
+extern const uint max_no_of_loop_gains[MAX_NO_TRANSDUCERS];
+extern const int hs_analog_gain_table[16] ;
+
+extern struct gain_descriptor_t gain_descriptor[MAX_NO_TRANSDUCERS]\
+				[MAX_NO_CHANNELS][MAX_NO_GAINS];
+
+#endif
+
+/* End of audio_io_hwctrl_common.h */
diff --git a/drivers/misc/bh1780gli.c b/drivers/misc/bh1780gli.c
index 82fe2d06782..0a731371fb5 100644
--- a/drivers/misc/bh1780gli.c
+++ b/drivers/misc/bh1780gli.c
@@ -18,10 +18,12 @@
  * this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include <linux/i2c.h>
+#include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/mutex.h>
 #include <linux/platform_device.h>
 #include <linux/delay.h>
+#include <linux/regulator/consumer.h>
 
 #define BH1780_REG_CONTROL	0x80
 #define BH1780_REG_PARTID	0x8A
@@ -39,6 +41,7 @@
 
 struct bh1780_data {
 	struct i2c_client *client;
+	struct regulator *regulator;
 	int power_state;
 	/* lock for sysfs operations */
 	struct mutex lock;
@@ -71,6 +74,9 @@ static ssize_t bh1780_show_lux(struct device *dev,
 	struct bh1780_data *ddata = platform_get_drvdata(pdev);
 	int lsb, msb;
 
+	if (ddata->power_state == BH1780_POFF)
+		return -EINVAL;
+
 	lsb = bh1780_read(ddata, BH1780_REG_DLOW, "DLOW");
 	if (lsb < 0)
 		return lsb;
@@ -88,13 +94,9 @@ static ssize_t bh1780_show_power_state(struct device *dev,
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct bh1780_data *ddata = platform_get_drvdata(pdev);
-	int state;
-
-	state = bh1780_read(ddata, BH1780_REG_CONTROL, "CONTROL");
-	if (state < 0)
-		return state;
 
-	return sprintf(buf, "%d\n", state & BH1780_POWMASK);
+	/* we already maintain a sw state */
+	return sprintf(buf, "%d\n", ddata->power_state);
 }
 
 static ssize_t bh1780_store_power_state(struct device *dev,
@@ -103,7 +105,7 @@ static ssize_t bh1780_store_power_state(struct device *dev,
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct bh1780_data *ddata = platform_get_drvdata(pdev);
-	unsigned long val;
+	long val;
 	int error;
 
 	error = strict_strtoul(buf, 0, &val);
@@ -113,14 +115,24 @@ static ssize_t bh1780_store_power_state(struct device *dev,
 	if (val < BH1780_POFF || val > BH1780_PON)
 		return -EINVAL;
 
+	if (ddata->power_state == val)
+		return count;
+
 	mutex_lock(&ddata->lock);
 
+	if (ddata->power_state == BH1780_POFF)
+		regulator_enable(ddata->regulator);
+
 	error = bh1780_write(ddata, BH1780_REG_CONTROL, val, "CONTROL");
 	if (error < 0) {
 		mutex_unlock(&ddata->lock);
+		regulator_disable(ddata->regulator);
 		return error;
 	}
 
+	if (val == BH1780_POFF)
+		regulator_disable(ddata->regulator);
+
 	msleep(BH1780_PON_DELAY);
 	ddata->power_state = val;
 	mutex_unlock(&ddata->lock);
@@ -130,7 +142,7 @@ static ssize_t bh1780_store_power_state(struct device *dev,
 
 static DEVICE_ATTR(lux, S_IRUGO, bh1780_show_lux, NULL);
 
-static DEVICE_ATTR(power_state, S_IWUSR | S_IRUGO,
+static DEVICE_ATTR(power_state, S_IWUGO | S_IRUGO,
 		bh1780_show_power_state, bh1780_store_power_state);
 
 static struct attribute *bh1780_attributes[] = {
@@ -152,21 +164,36 @@ static int __devinit bh1780_probe(struct i2c_client *client,
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE)) {
 		ret = -EIO;
-		goto err_op_failed;
+		return ret;
 	}
 
 	ddata = kzalloc(sizeof(struct bh1780_data), GFP_KERNEL);
 	if (ddata == NULL) {
+		dev_err(&client->dev, "failed to alloc ddata\n");
 		ret = -ENOMEM;
-		goto err_op_failed;
+		return ret;
 	}
 
 	ddata->client = client;
 	i2c_set_clientdata(client, ddata);
 
+	ddata->regulator = regulator_get(&client->dev, "vcc");
+	if (IS_ERR(ddata->regulator)) {
+		dev_err(&client->dev, "failed to get regulator\n");
+		ret = PTR_ERR(ddata->regulator);
+		goto free_ddata;
+	}
+
+	regulator_enable(ddata->regulator);
+
 	ret = bh1780_read(ddata, BH1780_REG_PARTID, "PART ID");
-	if (ret < 0)
-		goto err_op_failed;
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to read part ID\n");
+		goto put_regulator;
+	}
+
+	regulator_disable(ddata->regulator);
+	ddata->power_state = BH1780_POFF;
 
 	dev_info(&client->dev, "Ambient Light Sensor, Rev : %d\n",
 			(ret & BH1780_REVMASK));
@@ -175,11 +202,14 @@ static int __devinit bh1780_probe(struct i2c_client *client,
 
 	ret = sysfs_create_group(&client->dev.kobj, &bh1780_attr_group);
 	if (ret)
-		goto err_op_failed;
+		goto put_regulator;
 
 	return 0;
 
-err_op_failed:
+put_regulator:
+	regulator_disable(ddata->regulator);
+	regulator_put(ddata->regulator);
+free_ddata:
 	kfree(ddata);
 	return ret;
 }
@@ -215,6 +245,8 @@ static int bh1780_suspend(struct device *dev)
 	if (ret < 0)
 		return ret;
 
+	regulator_disable(ddata->regulator);
+
 	return 0;
 }
 
@@ -229,11 +261,16 @@ static int bh1780_resume(struct device *dev)
 	ret = bh1780_write(ddata, BH1780_REG_CONTROL, state,
 				"CONTROL");
 
+	regulator_enable(ddata->regulator);
+
+	ret = bh1780_write(ddata, BH1780_REG_CONTROL, ddata->power_state,
+			"CONTROL");
 	if (ret < 0)
 		return ret;
 
 	return 0;
 }
+
 static SIMPLE_DEV_PM_OPS(bh1780_pm, bh1780_suspend, bh1780_resume);
 #define BH1780_PMOPS (&bh1780_pm)
 #else
@@ -252,7 +289,7 @@ static struct i2c_driver bh1780_driver = {
 	.driver = {
 		.name = "bh1780",
 		.pm	= BH1780_PMOPS,
-},
+	},
 };
 
 static int __init bh1780_init(void)
diff --git a/drivers/misc/db8500-modem-trace.c b/drivers/misc/db8500-modem-trace.c
new file mode 100644
index 00000000000..0d739fb4694
--- /dev/null
+++ b/drivers/misc/db8500-modem-trace.c
@@ -0,0 +1,273 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors: Michel JAOUEN <michel.jaouen@stericsson.com>
+ *          Maxime COQUELIN <maxime.coquelin-nonst@stericsson.com>
+ *				for ST-Ericsson
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/err.h>
+#include <linux/spinlock.h>
+#include <linux/device.h>
+#include <linux/cdev.h>
+#include <linux/poll.h>
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/uaccess.h>
+#include <linux/mman.h>
+#include <linux/db8500-modem-trace.h>
+
+#include <mach/hardware.h>
+
+#define DEVICE_NAME "db8500-modem-trace"
+
+/* activation of this flag triggers an initialization of 2 buffers
+ * 4kbytes , id 0xdeadbeef
+ * and 16Kbytes id 0xfadafada
+ * we assume that platform provides minimum 20Kbytes. */
+
+struct trace {
+	u32 start;
+	u32 end;
+	u32 mdm_base;
+	u32 ape_base;
+	void __iomem *area;
+	/* this spinlock to forbid concurrent access on the same trace buffer */
+	spinlock_t lock;
+	struct device *dev;
+	struct miscdevice misc_dev;
+};
+
+struct trace_modem {
+	u32 phys_addr;
+	u8 filler;
+};
+
+static struct trace *trace_priv;
+
+
+/*  all this definition are linked to modem interface */
+#define MODEM_MARKER 0x88
+/*  free marker is also written on filler */
+#define FREE_MARKER 0xa5
+#define FREE_MARKER_2 0xa5a5
+#define READ_MARKER 0x5a
+
+struct buffer_header {
+	u8 pattern;
+	u8 filler;
+	u16 head_size;
+};
+
+
+static int trace_read(unsigned long arg)
+{
+	struct modem_trace_req req;
+	struct buffer_header *pt;
+	char tmp_char;
+
+	if (copy_from_user(&req, (struct modem_trace_req *)arg,
+				sizeof(struct modem_trace_req)))
+		return -EFAULT;
+
+	/* compute Modem physical address to APE physical address range  */
+	if (req.phys_addr < trace_priv->mdm_base) {
+		dev_err(trace_priv->dev, "MODEM ADDR uncorrect\n");
+		return -EINVAL;
+	}
+	req.phys_addr += trace_priv->ape_base - trace_priv->mdm_base;
+
+	/* check request is in the range and aligned */
+	if ((req.phys_addr % 4 != 0)
+			|| (req.phys_addr < trace_priv->start)
+			|| (req.phys_addr + req.size) >= trace_priv->end) {
+		dev_err(trace_priv->dev, "req out of range %x %x\n",
+			req.phys_addr, req.size);
+		return -EINVAL;
+	}
+
+	/* perform access to memory area  */
+	pt = (struct buffer_header *)((u32)trace_priv->area +
+			req.phys_addr - trace_priv->start);
+
+	/* in case of several request coming on same trace buffer take a
+	 * spinlock */
+	spin_lock(&trace_priv->lock);
+	if (pt->pattern != MODEM_MARKER) {
+		/* pattern and size not matching */
+		dev_err(trace_priv->dev, "req not matching filler %x/%x \
+				or/and pattern %x\n", req.filler, pt->filler,
+				pt->pattern);
+		spin_unlock(&trace_priv->lock);
+		return -EINVAL;
+	}
+	/* mark pattern as read and unlock spin */
+	pt->pattern = READ_MARKER;
+	spin_unlock(&trace_priv->lock);
+
+	req.size -= copy_to_user(req.buff, pt, req.size);
+
+	pt->pattern = FREE_MARKER;
+	pt->filler = FREE_MARKER;
+	tmp_char = MODEM_MARKER;
+
+	/*  Update marker for trace tool */
+	if (copy_to_user(req.buff, &tmp_char, 1))
+		return -EFAULT;
+
+	/*  Update effective written size */
+	if (copy_to_user((struct modem_trace_req *)arg, &req,
+				sizeof(struct modem_trace_req)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int trace_mmapdump(struct file *file, struct vm_area_struct *vma)
+{
+	unsigned long vma_start = vma->vm_start;
+
+	if (vma->vm_flags & VM_WRITE)
+		return -EPERM;
+
+	if ((vma->vm_end - vma->vm_start) <
+			(trace_priv->end - trace_priv->start))
+		return -EINVAL;
+	if (remap_pfn_range(vma,
+				vma_start,
+				trace_priv->start >> PAGE_SHIFT,
+				trace_priv->end - trace_priv->start,
+				vma->vm_page_prot))
+		return -EAGAIN;
+	return 0;
+}
+
+static long trace_ioctl(struct file *filp, unsigned int cmd,
+		unsigned long arg)
+{
+	long ret = 0;
+	void __user *argp = (void __user *)arg;
+	unsigned long size = trace_priv->end-trace_priv->start;
+
+	switch (cmd) {
+	case TM_GET_DUMPINFO:
+		ret = put_user(size, (unsigned long *)argp);
+		break;
+	case TM_TRACE_REQ:
+		ret = trace_read(arg);
+		break;
+
+	default:
+		ret = -EPERM;
+		break;
+	}
+	return ret;
+}
+
+static const struct file_operations trace_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = trace_ioctl,
+	.mmap = trace_mmapdump
+};
+
+static int trace_probe(struct platform_device *pdev)
+{
+	int rv = 0;
+	struct db8500_trace_platform_data *pdata = pdev->dev.platform_data;
+	/* retrieve area descriptor from platform device ressource */
+	struct resource *mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if ((mem->start == 0) && (mem->end == 0)) {
+		rv = -EINVAL;
+		goto out;
+	}
+
+	if ((pdata->ape_base == 0) || (pdata->modem_base == 0)) {
+		rv = -EINVAL;
+		goto out;
+	}
+
+	trace_priv = kzalloc(sizeof(*trace_priv), GFP_ATOMIC);
+	if (!trace_priv) {
+		rv = -ENOMEM;
+		goto out;
+	}
+
+	trace_priv->dev = &pdev->dev;
+	trace_priv->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	trace_priv->misc_dev.name = DEVICE_NAME;
+	trace_priv->misc_dev.fops = &trace_fops;
+	trace_priv->area = (void __iomem *)ioremap_nocache(mem->start,
+						mem->end - mem->start);
+	if (!trace_priv->area) {
+		rv = -ENOMEM;
+		goto outfree;
+	}
+
+	trace_priv->start = mem->start;
+	trace_priv->end = mem->end;
+
+	trace_priv->mdm_base = pdata->modem_base;
+	trace_priv->ape_base = pdata->ape_base;
+
+	/*  spin allowing smp access for reading/writing trace buffer header  */
+	spin_lock_init(&trace_priv->lock);
+
+	rv = misc_register(&trace_priv->misc_dev);
+	if (rv) {
+		dev_err(&pdev->dev, "can't misc_register\n");
+		goto outunmap;
+	}
+
+	return rv;
+
+outunmap:
+	iounmap(trace_priv->area);
+outfree:
+	kfree(trace_priv);
+out:
+	return rv;
+
+}
+
+static int trace_remove(struct platform_device *pdev)
+{
+	int rv = 0;
+
+	if (trace_priv) {
+		rv = misc_deregister(&trace_priv->misc_dev);
+		iounmap(trace_priv->area);
+		kfree(trace_priv);
+	}
+
+	return rv;
+}
+
+static struct platform_driver trace_driver = {
+	.probe = trace_probe,
+	.remove = trace_remove,
+	.driver = {
+		.name = "db8500-modem-trace",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int trace_init(void)
+{
+	platform_driver_register(&trace_driver);
+	return 0;
+}
+static void trace_exit(void)
+{
+	platform_driver_unregister(&trace_driver);
+}
+module_init(trace_init);
+module_exit(trace_exit);
+
+MODULE_AUTHOR("ST-Ericsson");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/dbx500-mloader.c b/drivers/misc/dbx500-mloader.c
new file mode 100644
index 00000000000..c3ec8b67983
--- /dev/null
+++ b/drivers/misc/dbx500-mloader.c
@@ -0,0 +1,269 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Ludovic Barre <ludovic.barre@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/firmware.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/uaccess.h>
+#include <linux/mman.h>
+#include <linux/io.h>
+
+#include <mach/mloader-dbx500.h>
+#include <linux/mloader.h>
+
+#define DEVICE_NAME "dbx500_mloader_fw"
+
+struct mloader_priv {
+	struct platform_device *pdev;
+	struct dbx500_mloader_pdata *pdata;
+	struct miscdevice misc_dev;
+	u32 aeras_size;
+};
+
+static struct mloader_priv *mloader_priv;
+
+static int mloader_fw_send(struct dbx500_ml_fw *fw_info)
+{
+	const struct firmware *fw;
+	unsigned long size;
+	unsigned long phys_start;
+	void *fw_data;
+	void *vaddr;
+	void __iomem *ioaddr;
+	int ret;
+
+	ret = request_firmware(&fw, fw_info->name, &mloader_priv->pdev->dev);
+	if (ret) {
+		dev_err(&mloader_priv->pdev->dev, "request firmware failed\n");
+		goto out;
+	}
+
+	if (fw->size > (fw_info->area->size - fw_info->offset)) {
+		dev_err(&mloader_priv->pdev->dev,
+				"fw:%s is too big for:%s\n",
+				fw_info->name, fw_info->area->name);
+		ret = -EINVAL;
+		goto err_fw;
+	}
+
+	size = PAGE_ALIGN(fw->size);
+	phys_start = fw_info->area->start + fw_info->offset;
+	phys_start &= PAGE_MASK;
+	ioaddr = ioremap(phys_start, size);
+	if (!ioaddr) {
+		dev_err(&mloader_priv->pdev->dev,
+				"failed remap memory region.\n");
+		ret = -EINVAL;
+		goto err_fw;
+	}
+
+	vaddr = ioaddr + (fw_info->offset & ~PAGE_MASK);
+	fw_data = (void *)fw->data;
+	memcpy_toio(vaddr, fw_data, fw->size);
+	iounmap(ioaddr);
+
+err_fw:
+	release_firmware(fw);
+out:
+	return ret;
+}
+
+static int mloader_fw_upload(void)
+{
+	int i, ret;
+	struct dbx500_mloader_pdata *pdata = mloader_priv->pdata;
+
+	for (i = 0; i < pdata->nr_fws; i++) {
+		ret = mloader_fw_send(&pdata->fws[i]);
+		if (ret)
+			goto err;
+	}
+
+	return 0;
+err:
+	dev_err(&mloader_priv->pdev->dev,
+				"Failed to upload %s firmware", pdata->fws[i].name);
+	return ret;
+}
+
+static int mloader_fw_mmapdump(struct file *file, struct vm_area_struct *vma)
+{
+	int i;
+	unsigned long dump_size = 0;
+	unsigned long vma_start = vma->vm_start;
+
+	if (vma->vm_flags & VM_WRITE)
+		return -EPERM;
+
+	for (i = 0 ; i < mloader_priv->pdata->nr_areas ; i++)
+		dump_size += mloader_priv->pdata->areas[i].size;
+
+	if ((vma->vm_end - vma->vm_start) < dump_size)
+		return -EINVAL;
+
+	for (i = 0 ; i < mloader_priv->pdata->nr_areas ; i++) {
+		if (remap_pfn_range(vma,
+			    vma_start,
+			    mloader_priv->pdata->areas[i].start >> PAGE_SHIFT,
+			    mloader_priv->pdata->areas[i].size,
+			    vma->vm_page_prot))
+			return -EAGAIN;
+		vma_start += mloader_priv->pdata->areas[i].size;
+	}
+	return 0;
+}
+
+static void mloader_fw_dumpinfo(struct dump_image *images)
+{
+	u32 offset = 0;
+	int i;
+
+	for (i = 0 ; i < mloader_priv->pdata->nr_areas ; i++) {
+		strncpy(images[i].name,
+				mloader_priv->pdata->areas[i].name, MAX_NAME);
+		images[i].name[MAX_NAME-1] = 0;
+		images[i].offset = offset;
+		images[i].size = mloader_priv->pdata->areas[i].size;
+		offset += mloader_priv->pdata->areas[i].size;
+	}
+}
+
+static long mloader_fw_ioctl(struct file *filp, unsigned int cmd,
+		unsigned long arg)
+{
+	long ret = 0;
+	void __user *argp = (void __user *)arg;
+
+	switch (cmd) {
+	case ML_UPLOAD:
+		ret = mloader_fw_upload();
+		break;
+	case ML_GET_NBIMAGES:
+		ret = put_user(mloader_priv->pdata->nr_areas,
+				(unsigned long __user *)argp);
+		break;
+	case ML_GET_DUMPINFO: {
+		struct dump_image *dump_images;
+		dump_images = kzalloc(mloader_priv->pdata->nr_areas
+				* sizeof(struct dump_image), GFP_ATOMIC);
+		mloader_fw_dumpinfo(dump_images);
+		ret = copy_to_user(argp, (void *) dump_images,
+				mloader_priv->pdata->nr_areas
+				* sizeof(struct dump_image)) ? -EFAULT : 0;
+		kfree(dump_images);
+		break;
+	}
+	default:
+		ret = -EPERM;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct file_operations modem_fw_fops = {
+	.owner			= THIS_MODULE,
+	.unlocked_ioctl		= mloader_fw_ioctl,
+	.mmap			= mloader_fw_mmapdump,
+};
+
+static int __devinit mloader_fw_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	int i;
+
+	mloader_priv = kzalloc(sizeof(*mloader_priv), GFP_ATOMIC);
+	if (!mloader_priv) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	mloader_priv->pdev = pdev;
+	mloader_priv->pdata = pdev->dev.platform_data;
+
+	mloader_priv->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	mloader_priv->misc_dev.name = DEVICE_NAME;
+	mloader_priv->misc_dev.fops = &modem_fw_fops;
+	ret = misc_register(&mloader_priv->misc_dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can't misc_register\n");
+		goto err_free_priv;
+	}
+
+	dev_info(&mloader_priv->pdev->dev, "mloader device register\n");
+
+	for (i = 0 ; i < mloader_priv->pdata->nr_areas ; i++) {
+		dev_dbg(&mloader_priv->pdev->dev,
+				"Area:%d (name:%s start:%x size:%x)\n",
+				i, mloader_priv->pdata->areas[i].name,
+				mloader_priv->pdata->areas[i].start,
+				mloader_priv->pdata->areas[i].size);
+	}
+
+	for (i = 0 ; i < mloader_priv->pdata->nr_fws ; i++) {
+		dev_dbg(&mloader_priv->pdev->dev,
+				"Firmware:%d (name:%s offset:%x "
+				"area_name:%s area_start:%x area_size:%x)\n",
+				i, mloader_priv->pdata->fws[i].name,
+				mloader_priv->pdata->fws[i].offset,
+				mloader_priv->pdata->fws[i].area->name,
+				mloader_priv->pdata->fws[i].area->start,
+				mloader_priv->pdata->fws[i].area->size);
+	}
+
+	return ret;
+
+err_free_priv:
+	kfree(mloader_priv);
+out:
+	return ret;
+}
+
+static int __devexit mloader_fw_remove(struct platform_device *pdev)
+{
+	int err;
+
+	err = misc_register(&mloader_priv->misc_dev);
+	if (err < 0)
+		dev_err(&pdev->dev, "can't misc_deregister, %d\n", err);
+
+	kfree(mloader_priv);
+
+	return err;
+}
+
+static struct platform_driver mloader_fw_driver = {
+	.driver.name	= DEVICE_NAME,
+	.driver.owner	= THIS_MODULE,
+	.probe		= mloader_fw_probe,
+	.remove		= __devexit_p(mloader_fw_remove),
+};
+
+static int __init mloader_fw_init(void)
+{
+	return platform_driver_register(&mloader_fw_driver);
+}
+
+static void __exit mloader_fw_exit(void)
+{
+	kfree(mloader_priv);
+	platform_driver_unregister(&mloader_fw_driver);
+}
+
+module_init(mloader_fw_init);
+module_exit(mloader_fw_exit);
+MODULE_DESCRIPTION("ST-Ericsson modem loader firmware");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Ludovic Barre <ludovic.barre@stericsson.com>");
diff --git a/drivers/misc/hwmem/Makefile b/drivers/misc/hwmem/Makefile
new file mode 100644
index 00000000000..c307616a181
--- /dev/null
+++ b/drivers/misc/hwmem/Makefile
@@ -0,0 +1,3 @@
+hwmem-objs := hwmem-main.o hwmem-ioctl.o cache_handler.o contig_alloc.o
+
+obj-$(CONFIG_HWMEM) += hwmem.o
diff --git a/drivers/misc/hwmem/cache_handler.c b/drivers/misc/hwmem/cache_handler.c
new file mode 100644
index 00000000000..e0ab4ee6cf8
--- /dev/null
+++ b/drivers/misc/hwmem/cache_handler.c
@@ -0,0 +1,510 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Cache handler
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/hwmem.h>
+
+#include <asm/pgtable.h>
+
+#include <mach/dcache.h>
+
+#include "cache_handler.h"
+
+#define U32_MAX (~(u32)0)
+
+enum hwmem_alloc_flags cachi_get_cache_settings(
+			enum hwmem_alloc_flags requested_cache_settings);
+void cachi_set_pgprot_cache_options(enum hwmem_alloc_flags cache_settings,
+							pgprot_t *pgprot);
+
+static void sync_buf_pre_cpu(struct cach_buf *buf, enum hwmem_access access,
+						struct hwmem_region *region);
+static void sync_buf_post_cpu(struct cach_buf *buf,
+	enum hwmem_access next_access, struct hwmem_region *next_region);
+
+static void invalidate_cpu_cache(struct cach_buf *buf,
+					struct cach_range *range_2b_used);
+static void clean_cpu_cache(struct cach_buf *buf,
+					struct cach_range *range_2b_used);
+static void flush_cpu_cache(struct cach_buf *buf,
+					struct cach_range *range_2b_used);
+
+static void null_range(struct cach_range *range);
+static void expand_range(struct cach_range *range,
+					struct cach_range *range_2_add);
+/*
+ * Expands range to one of enclosing_range's two edges. The function will
+ * choose which of enclosing_range's edges to expand range to in such a
+ * way that the size of range is minimized. range must be located inside
+ * enclosing_range.
+ */
+static void expand_range_2_edge(struct cach_range *range,
+					struct cach_range *enclosing_range);
+static void shrink_range(struct cach_range *range,
+					struct cach_range *range_2_remove);
+static bool is_non_empty_range(struct cach_range *range);
+static void intersect_range(struct cach_range *range_1,
+		struct cach_range *range_2, struct cach_range *intersection);
+/* Align_up restrictions apply here to */
+static void align_range_up(struct cach_range *range, u32 alignment);
+static u32 range_length(struct cach_range *range);
+static void region_2_range(struct hwmem_region *region, u32 buffer_size,
+						struct cach_range *range);
+
+static void *offset_2_vaddr(struct cach_buf *buf, u32 offset);
+static u32 offset_2_paddr(struct cach_buf *buf, u32 offset);
+
+/* Saturates, might return unaligned values when that happens */
+static u32 align_up(u32 value, u32 alignment);
+static u32 align_down(u32 value, u32 alignment);
+
+/*
+ * Exported functions
+ */
+
+void cach_init_buf(struct cach_buf *buf, enum hwmem_alloc_flags cache_settings,
+								u32 size)
+{
+	buf->vstart = NULL;
+	buf->pstart = 0;
+	buf->size = size;
+
+	buf->cache_settings = cachi_get_cache_settings(cache_settings);
+}
+
+void cach_set_buf_addrs(struct cach_buf *buf, void* vaddr, u32 paddr)
+{
+	bool tmp;
+
+	buf->vstart = vaddr;
+	buf->pstart = paddr;
+
+	if (buf->cache_settings & HWMEM_ALLOC_HINT_CACHED) {
+		/*
+		 * Keep whatever is in the cache. This way we avoid an
+		 * unnecessary synch if CPU is the first user.
+		 */
+		buf->range_in_cpu_cache.start = 0;
+		buf->range_in_cpu_cache.end = buf->size;
+		align_range_up(&buf->range_in_cpu_cache,
+						get_dcache_granularity());
+		buf->range_dirty_in_cpu_cache.start = 0;
+		buf->range_dirty_in_cpu_cache.end = buf->size;
+		align_range_up(&buf->range_dirty_in_cpu_cache,
+						get_dcache_granularity());
+	} else {
+		flush_cpu_dcache(buf->vstart, buf->pstart, buf->size, false,
+									&tmp);
+		drain_cpu_write_buf();
+
+		null_range(&buf->range_in_cpu_cache);
+		null_range(&buf->range_dirty_in_cpu_cache);
+	}
+	null_range(&buf->range_invalid_in_cpu_cache);
+}
+
+void cach_set_pgprot_cache_options(struct cach_buf *buf, pgprot_t *pgprot)
+{
+	cachi_set_pgprot_cache_options(buf->cache_settings, pgprot);
+}
+
+void cach_set_domain(struct cach_buf *buf, enum hwmem_access access,
+			enum hwmem_domain domain, struct hwmem_region *region)
+{
+	struct hwmem_region *__region;
+	struct hwmem_region full_region;
+
+	if (region != NULL) {
+		__region = region;
+	} else {
+		full_region.offset = 0;
+		full_region.count = 1;
+		full_region.start = 0;
+		full_region.end = buf->size;
+		full_region.size = buf->size;
+
+		__region = &full_region;
+	}
+
+	switch (domain) {
+	case HWMEM_DOMAIN_SYNC:
+		sync_buf_post_cpu(buf, access, __region);
+
+		break;
+
+	case HWMEM_DOMAIN_CPU:
+		sync_buf_pre_cpu(buf, access, __region);
+
+		break;
+	}
+}
+
+/*
+ * Local functions
+ */
+
+enum hwmem_alloc_flags __attribute__((weak)) cachi_get_cache_settings(
+			enum hwmem_alloc_flags requested_cache_settings)
+{
+	static const u32 CACHE_ON_FLAGS_MASK = HWMEM_ALLOC_HINT_CACHED |
+		HWMEM_ALLOC_HINT_CACHE_WB | HWMEM_ALLOC_HINT_CACHE_WT |
+		HWMEM_ALLOC_HINT_CACHE_NAOW | HWMEM_ALLOC_HINT_CACHE_AOW |
+				HWMEM_ALLOC_HINT_INNER_AND_OUTER_CACHE |
+					HWMEM_ALLOC_HINT_INNER_CACHE_ONLY;
+	/* We don't know the cache setting so we assume worst case. */
+	static const u32 CACHE_SETTING = HWMEM_ALLOC_HINT_WRITE_COMBINE |
+			HWMEM_ALLOC_HINT_CACHED | HWMEM_ALLOC_HINT_CACHE_WB |
+						HWMEM_ALLOC_HINT_CACHE_AOW |
+					HWMEM_ALLOC_HINT_INNER_AND_OUTER_CACHE;
+
+	if (requested_cache_settings & CACHE_ON_FLAGS_MASK)
+		return CACHE_SETTING;
+	else if (requested_cache_settings & HWMEM_ALLOC_HINT_WRITE_COMBINE ||
+		(requested_cache_settings & HWMEM_ALLOC_HINT_UNCACHED &&
+		 !(requested_cache_settings &
+					HWMEM_ALLOC_HINT_NO_WRITE_COMBINE)))
+		return HWMEM_ALLOC_HINT_WRITE_COMBINE;
+	else if (requested_cache_settings &
+					(HWMEM_ALLOC_HINT_NO_WRITE_COMBINE |
+						HWMEM_ALLOC_HINT_UNCACHED))
+		return 0;
+	else
+		/* Nothing specified, use cached */
+		return CACHE_SETTING;
+}
+
+void __attribute__((weak)) cachi_set_pgprot_cache_options(
+		enum hwmem_alloc_flags cache_settings, pgprot_t *pgprot)
+{
+	if (cache_settings & HWMEM_ALLOC_HINT_CACHED)
+		*pgprot = *pgprot; /* To silence compiler and checkpatch */
+	else if (cache_settings & HWMEM_ALLOC_HINT_WRITE_COMBINE)
+		*pgprot = pgprot_writecombine(*pgprot);
+	else
+		*pgprot = pgprot_noncached(*pgprot);
+}
+
+bool __attribute__((weak)) speculative_data_prefetch(void)
+{
+	/* We don't know so we go with the safe alternative */
+	return true;
+}
+
+static void sync_buf_pre_cpu(struct cach_buf *buf, enum hwmem_access access,
+						struct hwmem_region *region)
+{
+	bool write = access & HWMEM_ACCESS_WRITE;
+	bool read = access & HWMEM_ACCESS_READ;
+
+	if (!write && !read)
+		return;
+
+	if (buf->cache_settings & HWMEM_ALLOC_HINT_CACHED) {
+		struct cach_range region_range;
+
+		region_2_range(region, buf->size, &region_range);
+
+		if (read || (write && buf->cache_settings &
+						HWMEM_ALLOC_HINT_CACHE_WB))
+			/* Perform defered invalidates */
+			invalidate_cpu_cache(buf, &region_range);
+		if (read || (write && buf->cache_settings &
+						HWMEM_ALLOC_HINT_CACHE_AOW))
+			expand_range(&buf->range_in_cpu_cache, &region_range);
+		if (write && buf->cache_settings & HWMEM_ALLOC_HINT_CACHE_WB) {
+			struct cach_range dirty_range_addition;
+
+			if (buf->cache_settings & HWMEM_ALLOC_HINT_CACHE_AOW)
+				dirty_range_addition = region_range;
+			else
+				intersect_range(&buf->range_in_cpu_cache,
+					&region_range, &dirty_range_addition);
+
+			expand_range(&buf->range_dirty_in_cpu_cache,
+							&dirty_range_addition);
+		}
+	}
+	if (buf->cache_settings & HWMEM_ALLOC_HINT_WRITE_COMBINE) {
+		if (write)
+			buf->in_cpu_write_buf = true;
+	}
+}
+
+static void sync_buf_post_cpu(struct cach_buf *buf,
+	enum hwmem_access next_access, struct hwmem_region *next_region)
+{
+	bool write = next_access & HWMEM_ACCESS_WRITE;
+	bool read = next_access & HWMEM_ACCESS_READ;
+	struct cach_range region_range;
+
+	if (!write && !read)
+		return;
+
+	region_2_range(next_region, buf->size, &region_range);
+
+	if (write) {
+		if (speculative_data_prefetch()) {
+			/* Defer invalidate */
+			struct cach_range intersection;
+
+			intersect_range(&buf->range_in_cpu_cache,
+						&region_range, &intersection);
+
+			expand_range(&buf->range_invalid_in_cpu_cache,
+								&intersection);
+
+			clean_cpu_cache(buf, &region_range);
+		} else {
+			flush_cpu_cache(buf, &region_range);
+		}
+	}
+	if (read)
+		clean_cpu_cache(buf, &region_range);
+
+	if (buf->in_cpu_write_buf) {
+		drain_cpu_write_buf();
+
+		buf->in_cpu_write_buf = false;
+	}
+}
+
+static void invalidate_cpu_cache(struct cach_buf *buf, struct cach_range *range)
+{
+	struct cach_range intersection;
+
+	intersect_range(&buf->range_invalid_in_cpu_cache, range,
+								&intersection);
+	if (is_non_empty_range(&intersection)) {
+		bool flushed_everything;
+
+		expand_range_2_edge(&intersection,
+					&buf->range_invalid_in_cpu_cache);
+
+		/*
+		 * Cache handler never uses invalidate to discard data in the
+		 * cache so we can use flush instead which is considerably
+		 * faster for large buffers.
+		 */
+		flush_cpu_dcache(
+				offset_2_vaddr(buf, intersection.start),
+				offset_2_paddr(buf, intersection.start),
+				range_length(&intersection),
+				buf->cache_settings &
+					HWMEM_ALLOC_HINT_INNER_CACHE_ONLY,
+							&flushed_everything);
+
+		if (flushed_everything) {
+			null_range(&buf->range_invalid_in_cpu_cache);
+			null_range(&buf->range_dirty_in_cpu_cache);
+		} else {
+			/*
+			 * No need to shrink range_in_cpu_cache as invalidate
+			 * is only used when we can't keep track of what's in
+			 * the CPU cache.
+			 */
+			shrink_range(&buf->range_invalid_in_cpu_cache,
+								&intersection);
+		}
+	}
+}
+
+static void clean_cpu_cache(struct cach_buf *buf, struct cach_range *range)
+{
+	struct cach_range intersection;
+
+	intersect_range(&buf->range_dirty_in_cpu_cache, range, &intersection);
+	if (is_non_empty_range(&intersection)) {
+		bool cleaned_everything;
+
+		expand_range_2_edge(&intersection,
+					&buf->range_dirty_in_cpu_cache);
+
+		clean_cpu_dcache(
+				offset_2_vaddr(buf, intersection.start),
+				offset_2_paddr(buf, intersection.start),
+				range_length(&intersection),
+				buf->cache_settings &
+					HWMEM_ALLOC_HINT_INNER_CACHE_ONLY,
+							&cleaned_everything);
+
+		if (cleaned_everything)
+			null_range(&buf->range_dirty_in_cpu_cache);
+		else
+			shrink_range(&buf->range_dirty_in_cpu_cache,
+								&intersection);
+	}
+}
+
+static void flush_cpu_cache(struct cach_buf *buf, struct cach_range *range)
+{
+	struct cach_range intersection;
+
+	intersect_range(&buf->range_in_cpu_cache, range, &intersection);
+	if (is_non_empty_range(&intersection)) {
+		bool flushed_everything;
+
+		expand_range_2_edge(&intersection, &buf->range_in_cpu_cache);
+
+		flush_cpu_dcache(
+				offset_2_vaddr(buf, intersection.start),
+				offset_2_paddr(buf, intersection.start),
+				range_length(&intersection),
+				buf->cache_settings &
+					HWMEM_ALLOC_HINT_INNER_CACHE_ONLY,
+							&flushed_everything);
+
+		if (flushed_everything) {
+			if (!speculative_data_prefetch())
+				null_range(&buf->range_in_cpu_cache);
+			null_range(&buf->range_dirty_in_cpu_cache);
+			null_range(&buf->range_invalid_in_cpu_cache);
+		} else {
+			if (!speculative_data_prefetch())
+				shrink_range(&buf->range_in_cpu_cache,
+							 &intersection);
+			shrink_range(&buf->range_dirty_in_cpu_cache,
+								&intersection);
+			shrink_range(&buf->range_invalid_in_cpu_cache,
+								&intersection);
+		}
+	}
+}
+
+static void null_range(struct cach_range *range)
+{
+	range->start = U32_MAX;
+	range->end = 0;
+}
+
+static void expand_range(struct cach_range *range,
+						struct cach_range *range_2_add)
+{
+	range->start = min(range->start, range_2_add->start);
+	range->end = max(range->end, range_2_add->end);
+}
+
+/*
+ * Expands range to one of enclosing_range's two edges. The function will
+ * choose which of enclosing_range's edges to expand range to in such a
+ * way that the size of range is minimized. range must be located inside
+ * enclosing_range.
+ */
+static void expand_range_2_edge(struct cach_range *range,
+					struct cach_range *enclosing_range)
+{
+	u32 space_on_low_side = range->start - enclosing_range->start;
+	u32 space_on_high_side = enclosing_range->end - range->end;
+
+	if (space_on_low_side < space_on_high_side)
+		range->start = enclosing_range->start;
+	else
+		range->end = enclosing_range->end;
+}
+
+static void shrink_range(struct cach_range *range,
+					struct cach_range *range_2_remove)
+{
+	if (range_2_remove->start > range->start)
+		range->end = min(range->end, range_2_remove->start);
+	else
+		range->start = max(range->start, range_2_remove->end);
+
+	if (range->start >= range->end)
+		null_range(range);
+}
+
+static bool is_non_empty_range(struct cach_range *range)
+{
+	return range->end > range->start;
+}
+
+static void intersect_range(struct cach_range *range_1,
+		struct cach_range *range_2, struct cach_range *intersection)
+{
+	intersection->start = max(range_1->start, range_2->start);
+	intersection->end = min(range_1->end, range_2->end);
+
+	if (intersection->start >= intersection->end)
+		null_range(intersection);
+}
+
+/* Align_up restrictions apply here to */
+static void align_range_up(struct cach_range *range, u32 alignment)
+{
+	if (!is_non_empty_range(range))
+		return;
+
+	range->start = align_down(range->start, alignment);
+	range->end = align_up(range->end, alignment);
+}
+
+static u32 range_length(struct cach_range *range)
+{
+	if (is_non_empty_range(range))
+		return range->end - range->start;
+	else
+		return 0;
+}
+
+static void region_2_range(struct hwmem_region *region, u32 buffer_size,
+						struct cach_range *range)
+{
+	/*
+	 * We don't care about invalid regions, instead we limit the region's
+	 * range to the buffer's range. This should work good enough, worst
+	 * case we synch the entire buffer when we get an invalid region which
+	 * is acceptable.
+	 */
+	range->start = region->offset + region->start;
+	range->end = min(region->offset + (region->count * region->size) -
+				(region->size - region->end), buffer_size);
+	if (range->start >= range->end) {
+		null_range(range);
+		return;
+	}
+
+	align_range_up(range, get_dcache_granularity());
+}
+
+static void *offset_2_vaddr(struct cach_buf *buf, u32 offset)
+{
+	return (void *)((u32)buf->vstart + offset);
+}
+
+static u32 offset_2_paddr(struct cach_buf *buf, u32 offset)
+{
+	return buf->pstart + offset;
+}
+
+/* Saturates, might return unaligned values when that happens */
+static u32 align_up(u32 value, u32 alignment)
+{
+	u32 remainder = value % alignment;
+	u32 value_2_add;
+
+	if (remainder == 0)
+		return value;
+
+	value_2_add = alignment - remainder;
+
+	if (value_2_add > U32_MAX - value) /* Will overflow */
+		return U32_MAX;
+
+	return value + value_2_add;
+}
+
+static u32 align_down(u32 value, u32 alignment)
+{
+	u32 remainder = value % alignment;
+	if (remainder == 0)
+		return value;
+
+	return value - remainder;
+}
diff --git a/drivers/misc/hwmem/cache_handler.h b/drivers/misc/hwmem/cache_handler.h
new file mode 100644
index 00000000000..792105196fa
--- /dev/null
+++ b/drivers/misc/hwmem/cache_handler.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Cache handler
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/*
+ * Cache handler can not handle simultaneous execution! The caller has to
+ * ensure such a situation does not occur.
+ */
+
+#ifndef _CACHE_HANDLER_H_
+#define _CACHE_HANDLER_H_
+
+#include <linux/types.h>
+#include <linux/hwmem.h>
+
+/*
+ * To not have to double all datatypes we've used hwmem datatypes. If someone
+ * want's to use cache handler but not hwmem then we'll have to define our own
+ * datatypes.
+ */
+
+struct cach_range {
+	u32 start; /* Inclusive */
+	u32 end; /* Exclusive */
+};
+
+/*
+ * Internal, do not touch!
+ */
+struct cach_buf {
+	void *vstart;
+	u32 pstart;
+	u32 size;
+
+	/* Remaining hints are active */
+	enum hwmem_alloc_flags cache_settings;
+
+	bool in_cpu_write_buf;
+	struct cach_range range_in_cpu_cache;
+	struct cach_range range_dirty_in_cpu_cache;
+	struct cach_range range_invalid_in_cpu_cache;
+};
+
+void cach_init_buf(struct cach_buf *buf,
+			enum hwmem_alloc_flags cache_settings, u32 size);
+
+void cach_set_buf_addrs(struct cach_buf *buf, void* vaddr, u32 paddr);
+
+void cach_set_pgprot_cache_options(struct cach_buf *buf, pgprot_t *pgprot);
+
+void cach_set_domain(struct cach_buf *buf, enum hwmem_access access,
+			enum hwmem_domain domain, struct hwmem_region *region);
+
+#endif /* _CACHE_HANDLER_H_ */
diff --git a/drivers/misc/hwmem/contig_alloc.c b/drivers/misc/hwmem/contig_alloc.c
new file mode 100644
index 00000000000..bc71ca08f0f
--- /dev/null
+++ b/drivers/misc/hwmem/contig_alloc.c
@@ -0,0 +1,468 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Contiguous memory allocator
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>,
+ * Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+#include <linux/module.h>
+#include <linux/vmalloc.h>
+#include <asm/sizes.h>
+
+#define MAX_INSTANCE_NAME_LENGTH 31
+
+struct alloc {
+	struct list_head list;
+
+	bool in_use;
+	phys_addr_t paddr;
+	size_t size;
+};
+
+struct instance {
+	struct list_head list;
+
+	char name[MAX_INSTANCE_NAME_LENGTH + 1];
+
+	phys_addr_t region_paddr;
+	void *region_kaddr;
+	size_t region_size;
+
+	struct list_head alloc_list;
+
+#ifdef CONFIG_DEBUG_FS
+	struct inode *debugfs_inode;
+#endif /* #ifdef CONFIG_DEBUG_FS */
+};
+
+static LIST_HEAD(instance_list);
+
+static DEFINE_MUTEX(lock);
+
+void *cona_create(const char *name, phys_addr_t region_paddr,
+							size_t region_size);
+void *cona_alloc(void *instance, size_t size);
+void cona_free(void *instance, void *alloc);
+phys_addr_t cona_get_alloc_paddr(void *alloc);
+void *cona_get_alloc_kaddr(void *instance, void *alloc);
+size_t cona_get_alloc_size(void *alloc);
+
+static int init_alloc_list(struct instance *instance);
+static void clean_alloc_list(struct instance *instance);
+static struct alloc *find_free_alloc_bestfit(struct instance *instance,
+								size_t size);
+static struct alloc *split_allocation(struct alloc *alloc,
+							size_t new_alloc_size);
+static phys_addr_t get_alloc_offset(struct instance *instance,
+							struct alloc *alloc);
+
+void *cona_create(const char *name, phys_addr_t region_paddr,
+							size_t region_size)
+{
+	int ret;
+	struct instance *instance;
+	struct vm_struct *vm_area;
+
+	if (region_size == 0)
+		return ERR_PTR(-EINVAL);
+
+	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
+	if (instance == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	memcpy(instance->name, name, MAX_INSTANCE_NAME_LENGTH + 1);
+	/* Truncate name if necessary */
+	instance->name[MAX_INSTANCE_NAME_LENGTH] = '\0';
+	instance->region_paddr = region_paddr;
+	instance->region_size = region_size;
+
+	vm_area = get_vm_area(region_size, VM_IOREMAP);
+	if (vm_area == NULL) {
+		printk(KERN_WARNING "CONA: Failed to allocate %u bytes"
+					" kernel virtual memory", region_size);
+		ret = -ENOMSG;
+		goto vmem_alloc_failed;
+	}
+	instance->region_kaddr = vm_area->addr;
+
+	INIT_LIST_HEAD(&instance->alloc_list);
+	ret = init_alloc_list(instance);
+	if (ret < 0)
+		goto init_alloc_list_failed;
+
+	mutex_lock(&lock);
+	list_add_tail(&instance->list, &instance_list);
+	mutex_unlock(&lock);
+
+	return instance;
+
+init_alloc_list_failed:
+	vm_area = remove_vm_area(instance->region_kaddr);
+	if (vm_area == NULL)
+		printk(KERN_ERR "CONA: Failed to free kernel virtual memory,"
+							" resource leak!\n");
+
+	kfree(vm_area);
+vmem_alloc_failed:
+	kfree(instance);
+
+	return ERR_PTR(ret);
+}
+
+void *cona_alloc(void *instance, size_t size)
+{
+	struct instance *instance_l = (struct instance *)instance;
+	struct alloc *alloc;
+
+	if (size == 0)
+		return ERR_PTR(-EINVAL);
+
+	mutex_lock(&lock);
+
+	alloc = find_free_alloc_bestfit(instance_l, size);
+	if (IS_ERR(alloc))
+		goto out;
+	if (size < alloc->size) {
+		alloc = split_allocation(alloc, size);
+		if (IS_ERR(alloc))
+			goto out;
+	} else {
+		alloc->in_use = true;
+	}
+
+out:
+	mutex_unlock(&lock);
+
+	return alloc;
+}
+
+void cona_free(void *instance, void *alloc)
+{
+	struct instance *instance_l = (struct instance *)instance;
+	struct alloc *alloc_l = (struct alloc *)alloc;
+	struct alloc *other;
+
+	mutex_lock(&lock);
+
+	alloc_l->in_use = false;
+
+	other = list_entry(alloc_l->list.prev, struct alloc, list);
+	if ((alloc_l->list.prev != &instance_l->alloc_list) &&
+							!other->in_use) {
+		other->size += alloc_l->size;
+		list_del(&alloc_l->list);
+		kfree(alloc_l);
+		alloc_l = other;
+	}
+	other = list_entry(alloc_l->list.next, struct alloc, list);
+	if ((alloc_l->list.next != &instance_l->alloc_list) &&
+							!other->in_use) {
+		alloc_l->size += other->size;
+		list_del(&other->list);
+		kfree(other);
+	}
+
+	mutex_unlock(&lock);
+}
+
+phys_addr_t cona_get_alloc_paddr(void *alloc)
+{
+	return ((struct alloc *)alloc)->paddr;
+}
+
+void *cona_get_alloc_kaddr(void *instance, void *alloc)
+{
+	struct instance *instance_l = (struct instance *)instance;
+
+	return instance_l->region_kaddr + get_alloc_offset(instance_l,
+							(struct alloc *)alloc);
+}
+
+size_t cona_get_alloc_size(void *alloc)
+{
+	return ((struct alloc *)alloc)->size;
+}
+
+static int init_alloc_list(struct instance *instance)
+{
+	/*
+	 * Hack to not get any allocs that cross a 64MiB boundary as B2R2 can't
+	 * handle that.
+	 */
+	int ret;
+	u32 curr_pos = instance->region_paddr;
+	u32 region_end = instance->region_paddr + instance->region_size;
+	u32 next_64mib_boundary = (curr_pos + SZ_64M) & ~(SZ_64M - 1);
+	struct alloc *alloc;
+
+	if (PAGE_SIZE >= SZ_64M) {
+		printk(KERN_WARNING "CONA: PAGE_SIZE >= 64MiB\n");
+		return -ENOMSG;
+	}
+
+	while (next_64mib_boundary < region_end) {
+		if (next_64mib_boundary - curr_pos > PAGE_SIZE) {
+			alloc = kzalloc(sizeof(struct alloc), GFP_KERNEL);
+			if (alloc == NULL) {
+				ret = -ENOMEM;
+				goto error;
+			}
+			alloc->paddr = curr_pos;
+			alloc->size = next_64mib_boundary - curr_pos -
+								PAGE_SIZE;
+			alloc->in_use = false;
+			list_add_tail(&alloc->list, &instance->alloc_list);
+			curr_pos = alloc->paddr + alloc->size;
+		}
+
+		alloc = kzalloc(sizeof(struct alloc), GFP_KERNEL);
+		if (alloc == NULL) {
+			ret = -ENOMEM;
+			goto error;
+		}
+		alloc->paddr = curr_pos;
+		alloc->size = PAGE_SIZE;
+		alloc->in_use = true;
+		list_add_tail(&alloc->list, &instance->alloc_list);
+		curr_pos = alloc->paddr + alloc->size;
+
+		next_64mib_boundary += SZ_64M;
+	}
+
+	alloc = kzalloc(sizeof(struct alloc), GFP_KERNEL);
+	if (alloc == NULL) {
+		ret = -ENOMEM;
+		goto error;
+	}
+	alloc->paddr = curr_pos;
+	alloc->size = region_end - curr_pos;
+	alloc->in_use = false;
+	list_add_tail(&alloc->list, &instance->alloc_list);
+
+	return 0;
+
+error:
+	clean_alloc_list(instance);
+
+	return ret;
+}
+
+static void clean_alloc_list(struct instance *instance)
+{
+	while (list_empty(&instance->alloc_list) == 0) {
+		struct alloc *i = list_first_entry(&instance->alloc_list,
+							struct alloc, list);
+
+		list_del(&i->list);
+
+		kfree(i);
+	}
+}
+
+static struct alloc *find_free_alloc_bestfit(struct instance *instance,
+								size_t size)
+{
+	size_t best_diff = ~(size_t)0;
+	struct alloc *alloc = NULL, *i;
+
+	list_for_each_entry(i, &instance->alloc_list, list) {
+		size_t diff = i->size - size;
+		if (i->in_use || i->size < size)
+			continue;
+		if (diff < best_diff) {
+			alloc = i;
+			best_diff = diff;
+		}
+	}
+
+	return alloc != NULL ? alloc : ERR_PTR(-ENOMEM);
+}
+
+static struct alloc *split_allocation(struct alloc *alloc,
+							size_t new_alloc_size)
+{
+	struct alloc *new_alloc;
+
+	new_alloc = kzalloc(sizeof(struct alloc), GFP_KERNEL);
+	if (new_alloc == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	new_alloc->in_use = true;
+	new_alloc->paddr = alloc->paddr;
+	new_alloc->size = new_alloc_size;
+	alloc->size -= new_alloc_size;
+	alloc->paddr += new_alloc_size;
+
+	list_add_tail(&new_alloc->list, &alloc->list);
+
+	return new_alloc;
+}
+
+static phys_addr_t get_alloc_offset(struct instance *instance,
+							struct alloc *alloc)
+{
+	return alloc->paddr - instance->region_paddr;
+}
+
+/* Debug */
+
+#ifdef CONFIG_DEBUG_FS
+
+static int print_alloc(struct alloc *alloc, char **buf, size_t buf_size);
+static struct instance *get_instance_from_file(struct file *file);
+static int debugfs_allocs_read(struct file *filp, char __user *buf,
+						size_t count, loff_t *f_pos);
+
+static const struct file_operations debugfs_allocs_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_allocs_read,
+};
+
+static int print_alloc(struct alloc *alloc, char **buf, size_t buf_size)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		size_t buf_size_l;
+		if (i == 0)
+			buf_size_l = 0;
+		else
+			buf_size_l = buf_size;
+
+		ret = snprintf(*buf, buf_size_l, "paddr: %10x\tsize: %10u\t"
+				"in use: %1u\n", alloc->paddr, alloc->size,
+								alloc->in_use);
+		if (ret < 0)
+			return -ENOMSG;
+		else if (ret + 1 > buf_size)
+			return -EINVAL;
+	}
+
+	*buf += ret;
+
+	return 0;
+}
+
+static struct instance *get_instance_from_file(struct file *file)
+{
+	struct instance *curr_instance;
+
+	list_for_each_entry(curr_instance, &instance_list, list) {
+		if (file->f_dentry->d_inode == curr_instance->debugfs_inode)
+			return curr_instance;
+	}
+
+	return ERR_PTR(-ENOENT);
+}
+
+static int debugfs_allocs_read(struct file *file, char __user *buf,
+						size_t count, loff_t *f_pos)
+{
+	/*
+	 * We assume the supplied buffer and PAGE_SIZE is large enough to hold
+	 * information about at least one alloc, if not no data will be
+	 * returned.
+	 */
+
+	int ret;
+	struct instance *instance;
+	struct alloc *curr_alloc;
+	char *local_buf = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	char *local_buf_pos = local_buf;
+	size_t available_space = min((size_t)PAGE_SIZE, count);
+	/* private_data is intialized to NULL in open which I assume is 0. */
+	void **curr_pos = &file->private_data;
+	size_t bytes_read;
+
+	if (local_buf == NULL)
+		return -ENOMEM;
+
+	mutex_lock(&lock);
+
+	instance = get_instance_from_file(file);
+	if (IS_ERR(instance)) {
+		ret = PTR_ERR(instance);
+		goto out;
+	}
+
+	list_for_each_entry(curr_alloc, &instance->alloc_list, list) {
+		phys_addr_t alloc_offset = get_alloc_offset(instance,
+								curr_alloc);
+		if (alloc_offset < (phys_addr_t)*curr_pos)
+			continue;
+
+		ret = print_alloc(curr_alloc, &local_buf_pos, available_space -
+					(size_t)(local_buf_pos - local_buf));
+		if (ret == -EINVAL) /* No more room */
+			break;
+		else if (ret < 0)
+			goto out;
+
+		/*
+		 * There could be an overflow issue here in the unlikely case
+		 * where the region is placed at the end of the address range
+		 * and the last alloc is 1 byte large. Since this is debug code
+		 * and that case most likely never will happen I've chosen to
+		 * defer fixing it till it happens.
+		 */
+		*curr_pos = (void *)(alloc_offset + 1);
+	}
+
+	bytes_read = (size_t)(local_buf_pos - local_buf);
+
+	ret = copy_to_user(buf, local_buf, bytes_read);
+	if (ret < 0)
+		goto out;
+
+	ret = bytes_read;
+
+out:
+	kfree(local_buf);
+
+	mutex_unlock(&lock);
+
+	return ret;
+}
+
+static int __init init_debugfs(void)
+{
+	struct instance *curr_instance;
+	struct dentry *debugfs_root_dir = debugfs_create_dir("cona", NULL);
+
+	mutex_lock(&lock);
+
+	list_for_each_entry(curr_instance, &instance_list, list) {
+		struct dentry *file_dentry;
+		char tmp_str[MAX_INSTANCE_NAME_LENGTH + 7 + 1];
+		tmp_str[0] = '\0';
+		strcat(tmp_str, curr_instance->name);
+		strcat(tmp_str, "_allocs");
+		file_dentry = debugfs_create_file(tmp_str, 0444,
+				debugfs_root_dir, 0, &debugfs_allocs_fops);
+		if (file_dentry != NULL)
+			curr_instance->debugfs_inode = file_dentry->d_inode;
+	}
+
+	mutex_unlock(&lock);
+
+	return 0;
+}
+/*
+ * Must be executed after all instances have been created, hence the
+ * late_initcall.
+ */
+late_initcall(init_debugfs);
+
+#endif /* #ifdef CONFIG_DEBUG_FS */
diff --git a/drivers/misc/hwmem/hwmem-ioctl.c b/drivers/misc/hwmem/hwmem-ioctl.c
new file mode 100644
index 00000000000..e9e50de78bd
--- /dev/null
+++ b/drivers/misc/hwmem/hwmem-ioctl.c
@@ -0,0 +1,532 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Hardware memory driver, hwmem
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/idr.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/mm_types.h>
+#include <linux/hwmem.h>
+#include <linux/device.h>
+#include <linux/sched.h>
+
+static int hwmem_open(struct inode *inode, struct file *file);
+static int hwmem_ioctl_mmap(struct file *file, struct vm_area_struct *vma);
+static int hwmem_release_fop(struct inode *inode, struct file *file);
+static long hwmem_ioctl(struct file *file, unsigned int cmd,
+	unsigned long arg);
+static unsigned long hwmem_get_unmapped_area(struct file *file,
+	unsigned long addr, unsigned long len, unsigned long pgoff,
+	unsigned long flags);
+
+static const struct file_operations hwmem_fops = {
+	.open = hwmem_open,
+	.mmap = hwmem_ioctl_mmap,
+	.unlocked_ioctl = hwmem_ioctl,
+	.release = hwmem_release_fop,
+	.get_unmapped_area = hwmem_get_unmapped_area,
+};
+
+static struct miscdevice hwmem_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "hwmem",
+	.fops = &hwmem_fops,
+};
+
+struct hwmem_file {
+	struct mutex lock;
+	struct idr idr; /* id -> struct hwmem_alloc*, ref counted */
+	struct hwmem_alloc *fd_alloc; /* Ref counted */
+};
+
+static s32 create_id(struct hwmem_file *hwfile, struct hwmem_alloc *alloc)
+{
+	int id, ret;
+
+	while (true) {
+		if (idr_pre_get(&hwfile->idr, GFP_KERNEL) == 0)
+			return -ENOMEM;
+
+		ret = idr_get_new_above(&hwfile->idr, alloc, 1, &id);
+		if (ret == 0)
+			break;
+		else if (ret != -EAGAIN)
+			return -ENOMEM;
+	}
+
+	/*
+	 * IDR always returns the lowest free id so there is no wrapping issue
+	 * because of this.
+	 */
+	if (id >= (s32)1 << (31 - PAGE_SHIFT)) {
+		dev_err(hwmem_device.this_device, "Out of IDs!\n");
+		idr_remove(&hwfile->idr, id);
+		return -ENOMSG;
+	}
+
+	return (s32)id << PAGE_SHIFT;
+}
+
+static void remove_id(struct hwmem_file *hwfile, s32 id)
+{
+	idr_remove(&hwfile->idr, id >> PAGE_SHIFT);
+}
+
+static struct hwmem_alloc *resolve_id(struct hwmem_file *hwfile, s32 id)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = id ? idr_find(&hwfile->idr, id >> PAGE_SHIFT) :
+							hwfile->fd_alloc;
+	if (alloc == NULL)
+		alloc = ERR_PTR(-EINVAL);
+
+	return alloc;
+}
+
+static s32 alloc(struct hwmem_file *hwfile, struct hwmem_alloc_request *req)
+{
+	s32 ret = 0;
+	struct hwmem_alloc *alloc;
+
+	alloc = hwmem_alloc(req->size, req->flags, req->default_access,
+								req->mem_type);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	ret = create_id(hwfile, alloc);
+	if (ret < 0)
+		hwmem_release(alloc);
+
+	return ret;
+}
+
+static int alloc_fd(struct hwmem_file *hwfile, struct hwmem_alloc_request *req)
+{
+	struct hwmem_alloc *alloc;
+
+	if (hwfile->fd_alloc)
+		return -EINVAL;
+
+	alloc = hwmem_alloc(req->size, req->flags, req->default_access,
+								req->mem_type);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	hwfile->fd_alloc = alloc;
+
+	return 0;
+}
+
+static int release(struct hwmem_file *hwfile, s32 id)
+{
+	struct hwmem_alloc *alloc;
+
+	if (id == 0)
+		return -EINVAL;
+
+	alloc = resolve_id(hwfile, id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	remove_id(hwfile, id);
+	hwmem_release(alloc);
+
+	return 0;
+}
+
+static int set_cpu_domain(struct hwmem_file *hwfile,
+					struct hwmem_set_domain_request *req)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = resolve_id(hwfile, req->id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	return hwmem_set_domain(alloc, req->access, HWMEM_DOMAIN_CPU,
+					(struct hwmem_region *)&req->region);
+}
+
+static int set_sync_domain(struct hwmem_file *hwfile,
+					struct hwmem_set_domain_request *req)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = resolve_id(hwfile, req->id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	return hwmem_set_domain(alloc, req->access, HWMEM_DOMAIN_SYNC,
+					(struct hwmem_region *)&req->region);
+}
+
+static int pin(struct hwmem_file *hwfile, struct hwmem_pin_request *req)
+{
+	int ret;
+	struct hwmem_alloc *alloc;
+	enum hwmem_mem_type mem_type;
+	struct hwmem_mem_chunk mem_chunk;
+	size_t mem_chunk_length = 1;
+
+	alloc = resolve_id(hwfile, req->id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	hwmem_get_info(alloc, NULL, &mem_type, NULL);
+	if (mem_type != HWMEM_MEM_CONTIGUOUS_SYS)
+		return -EINVAL;
+
+	ret = hwmem_pin(alloc, &mem_chunk, &mem_chunk_length);
+	if (ret < 0)
+		return ret;
+
+	req->phys_addr = mem_chunk.paddr;
+
+	return 0;
+}
+
+static int unpin(struct hwmem_file *hwfile, s32 id)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = resolve_id(hwfile, id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	hwmem_unpin(alloc);
+
+	return 0;
+}
+
+static int set_access(struct hwmem_file *hwfile,
+		struct hwmem_set_access_request *req)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = resolve_id(hwfile, req->id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	return hwmem_set_access(alloc, req->access, req->pid);
+}
+
+static int get_info(struct hwmem_file *hwfile,
+		struct hwmem_get_info_request *req)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = resolve_id(hwfile, req->id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	hwmem_get_info(alloc, &req->size, &req->mem_type, &req->access);
+
+	return 0;
+}
+
+static s32 export(struct hwmem_file *hwfile, s32 id)
+{
+	s32 ret;
+	struct hwmem_alloc *alloc;
+	enum hwmem_access access;
+
+	alloc = resolve_id(hwfile, id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	/*
+	 * The user could be about to send the buffer to a driver but
+	 * there is a chance the current thread group don't have import rights
+	 * if it gained access to the buffer via a inter-process fd transfer
+	 * (fork, Android binder), if this is the case the driver will not be
+	 * able to resolve the buffer name. To avoid this situation we give the
+	 * current thread group import rights. This will not breach the
+	 * security as the process already has access to the buffer (otherwise
+	 * it would not be able to get here).
+	 */
+	hwmem_get_info(alloc, NULL, NULL, &access);
+
+	ret = hwmem_set_access(alloc, (access | HWMEM_ACCESS_IMPORT),
+							task_tgid_nr(current));
+	if (ret < 0)
+		return ret;
+
+	return hwmem_get_name(alloc);
+}
+
+static s32 import(struct hwmem_file *hwfile, s32 name)
+{
+	s32 ret = 0;
+	struct hwmem_alloc *alloc;
+	enum hwmem_access access;
+
+	alloc = hwmem_resolve_by_name(name);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	/* Check access permissions for process */
+	hwmem_get_info(alloc, NULL, NULL, &access);
+	if (!(access & HWMEM_ACCESS_IMPORT)) {
+		ret = -EPERM;
+		goto error;
+	}
+
+	ret = create_id(hwfile, alloc);
+	if (ret < 0)
+		goto error;
+
+	return ret;
+
+error:
+	hwmem_release(alloc);
+
+	return ret;
+}
+
+static int import_fd(struct hwmem_file *hwfile, s32 name)
+{
+	int ret;
+	struct hwmem_alloc *alloc;
+	enum hwmem_access access;
+
+	if (hwfile->fd_alloc)
+		return -EINVAL;
+
+	alloc = hwmem_resolve_by_name(name);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	/* Check access permissions for process */
+	hwmem_get_info(alloc, NULL, NULL, &access);
+	if (!(access & HWMEM_ACCESS_IMPORT)) {
+		ret = -EPERM;
+		goto error;
+	}
+
+	hwfile->fd_alloc = alloc;
+
+	return 0;
+
+error:
+	hwmem_release(alloc);
+
+	return ret;
+}
+
+static int hwmem_open(struct inode *inode, struct file *file)
+{
+	struct hwmem_file *hwfile;
+
+	hwfile = kzalloc(sizeof(struct hwmem_file), GFP_KERNEL);
+	if (hwfile == NULL)
+		return -ENOMEM;
+
+	idr_init(&hwfile->idr);
+	mutex_init(&hwfile->lock);
+	file->private_data = hwfile;
+
+	return 0;
+}
+
+static int hwmem_ioctl_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	int ret;
+	struct hwmem_file *hwfile = (struct hwmem_file *)file->private_data;
+	struct hwmem_alloc *alloc;
+
+	mutex_lock(&hwfile->lock);
+
+	alloc = resolve_id(hwfile, (s32)vma->vm_pgoff << PAGE_SHIFT);
+	if (IS_ERR(alloc)) {
+		ret = PTR_ERR(alloc);
+		goto out;
+	}
+
+	ret = hwmem_mmap(alloc, vma);
+
+out:
+	mutex_unlock(&hwfile->lock);
+
+	return ret;
+}
+
+static int hwmem_release_idr_for_each_wrapper(int id, void *ptr, void *data)
+{
+	hwmem_release((struct hwmem_alloc *)ptr);
+
+	return 0;
+}
+
+static int hwmem_release_fop(struct inode *inode, struct file *file)
+{
+	struct hwmem_file *hwfile = (struct hwmem_file *)file->private_data;
+
+	idr_for_each(&hwfile->idr, hwmem_release_idr_for_each_wrapper, NULL);
+	idr_remove_all(&hwfile->idr);
+	idr_destroy(&hwfile->idr);
+
+	if (hwfile->fd_alloc)
+		hwmem_release(hwfile->fd_alloc);
+
+	mutex_destroy(&hwfile->lock);
+
+	kfree(hwfile);
+
+	return 0;
+}
+
+static long hwmem_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	int ret = -ENOSYS;
+	struct hwmem_file *hwfile = (struct hwmem_file *)file->private_data;
+
+	mutex_lock(&hwfile->lock);
+
+	switch (cmd) {
+	case HWMEM_ALLOC_IOC:
+		{
+			struct hwmem_alloc_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+					sizeof(struct hwmem_alloc_request)))
+				ret = -EFAULT;
+			else
+				ret = alloc(hwfile, &req);
+		}
+		break;
+	case HWMEM_ALLOC_FD_IOC:
+		{
+			struct hwmem_alloc_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+					sizeof(struct hwmem_alloc_request)))
+				ret = -EFAULT;
+			else
+				ret = alloc_fd(hwfile, &req);
+		}
+		break;
+	case HWMEM_RELEASE_IOC:
+		ret = release(hwfile, (s32)arg);
+		break;
+	case HWMEM_SET_CPU_DOMAIN_IOC:
+		{
+			struct hwmem_set_domain_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+				sizeof(struct hwmem_set_domain_request)))
+				ret = -EFAULT;
+			else
+				ret = set_cpu_domain(hwfile, &req);
+		}
+		break;
+	case HWMEM_SET_SYNC_DOMAIN_IOC:
+		{
+			struct hwmem_set_domain_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+				sizeof(struct hwmem_set_domain_request)))
+				ret = -EFAULT;
+			else
+				ret = set_sync_domain(hwfile, &req);
+		}
+		break;
+	case HWMEM_PIN_IOC:
+		{
+			struct hwmem_pin_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+				sizeof(struct hwmem_pin_request)))
+				ret = -EFAULT;
+			else
+				ret = pin(hwfile, &req);
+			if (ret == 0 && copy_to_user((void __user *)arg, &req,
+					sizeof(struct hwmem_pin_request)))
+				ret = -EFAULT;
+		}
+		break;
+	case HWMEM_UNPIN_IOC:
+		ret = unpin(hwfile, (s32)arg);
+		break;
+	case HWMEM_SET_ACCESS_IOC:
+		{
+			struct hwmem_set_access_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+				sizeof(struct hwmem_set_access_request)))
+				ret = -EFAULT;
+			else
+				ret = set_access(hwfile, &req);
+		}
+		break;
+	case HWMEM_GET_INFO_IOC:
+		{
+			struct hwmem_get_info_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+				sizeof(struct hwmem_get_info_request)))
+				ret = -EFAULT;
+			else
+				ret = get_info(hwfile, &req);
+			if (ret == 0 && copy_to_user((void __user *)arg, &req,
+					sizeof(struct hwmem_get_info_request)))
+				ret = -EFAULT;
+		}
+		break;
+	case HWMEM_EXPORT_IOC:
+		ret = export(hwfile, (s32)arg);
+		break;
+	case HWMEM_IMPORT_IOC:
+		ret = import(hwfile, (s32)arg);
+		break;
+	case HWMEM_IMPORT_FD_IOC:
+		ret = import_fd(hwfile, (s32)arg);
+		break;
+	}
+
+	mutex_unlock(&hwfile->lock);
+
+	return ret;
+}
+
+static unsigned long hwmem_get_unmapped_area(struct file *file,
+	unsigned long addr, unsigned long len, unsigned long pgoff,
+	unsigned long flags)
+{
+	/*
+	 * pgoff will not be valid as it contains a buffer id (right shifted
+	 * PAGE_SHIFT bits). To not confuse get_unmapped_area we'll not pass
+	 * on file or pgoff.
+	 */
+	return current->mm->get_unmapped_area(NULL, addr, len, 0, flags);
+}
+
+int __init hwmem_ioctl_init(void)
+{
+	if (PAGE_SHIFT < 1 || PAGE_SHIFT > 30 || sizeof(size_t) != 4 ||
+		sizeof(int) > 4 || sizeof(enum hwmem_alloc_flags) != 4 ||
+					sizeof(enum hwmem_access) != 4 ||
+					 sizeof(enum hwmem_mem_type) != 4) {
+		dev_err(hwmem_device.this_device, "PAGE_SHIFT < 1 || PAGE_SHIFT"
+			" > 30 || sizeof(size_t) != 4 || sizeof(int) > 4 ||"
+			" sizeof(enum hwmem_alloc_flags) != 4 || sizeof(enum"
+			" hwmem_access) != 4 || sizeof(enum hwmem_mem_type)"
+								" != 4\n");
+		return -ENOMSG;
+	}
+	if (PAGE_SHIFT > 15)
+		dev_warn(hwmem_device.this_device, "Due to the page size only"
+				" %u id:s per file instance are available\n",
+					((u32)1 << (31 - PAGE_SHIFT)) - 1);
+
+	return misc_register(&hwmem_device);
+}
+
+void __exit hwmem_ioctl_exit(void)
+{
+	misc_deregister(&hwmem_device);
+}
diff --git a/drivers/misc/hwmem/hwmem-main.c b/drivers/misc/hwmem/hwmem-main.c
new file mode 100644
index 00000000000..b91d99bc2be
--- /dev/null
+++ b/drivers/misc/hwmem/hwmem-main.c
@@ -0,0 +1,726 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Hardware memory driver, hwmem
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>,
+ * Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/idr.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/pid.h>
+#include <linux/list.h>
+#include <linux/hwmem.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/kallsyms.h>
+#include <linux/vmalloc.h>
+#include "cache_handler.h"
+
+#define S32_MAX 2147483647
+
+struct hwmem_alloc_threadg_info {
+	struct list_head list;
+
+	struct pid *threadg_pid; /* Ref counted */
+
+	enum hwmem_access access;
+};
+
+struct hwmem_alloc {
+	struct list_head list;
+
+	atomic_t ref_cnt;
+
+	enum hwmem_alloc_flags flags;
+	struct hwmem_mem_type_struct *mem_type;
+
+	void *allocator_hndl;
+	phys_addr_t paddr;
+	void *kaddr;
+	size_t size;
+	s32 name;
+
+	/* Access control */
+	enum hwmem_access default_access;
+	struct list_head threadg_info_list;
+
+	/* Cache handling */
+	struct cach_buf cach_buf;
+
+#ifdef CONFIG_DEBUG_FS
+	/* Debug */
+	void *creator;
+	pid_t creator_tgid;
+#endif /* #ifdef CONFIG_DEBUG_FS */
+};
+
+static struct platform_device *hwdev;
+
+static LIST_HEAD(alloc_list);
+static DEFINE_IDR(global_idr);
+static DEFINE_MUTEX(lock);
+
+static void vm_open(struct vm_area_struct *vma);
+static void vm_close(struct vm_area_struct *vma);
+static struct vm_operations_struct vm_ops = {
+	.open = vm_open,
+	.close = vm_close,
+};
+
+static void kunmap_alloc(struct hwmem_alloc *alloc);
+
+/* Helpers */
+
+static void destroy_alloc_threadg_info(
+		struct hwmem_alloc_threadg_info *info)
+{
+	if (info->threadg_pid)
+		put_pid(info->threadg_pid);
+
+	kfree(info);
+}
+
+static void clean_alloc_threadg_info_list(struct hwmem_alloc *alloc)
+{
+	struct hwmem_alloc_threadg_info *info;
+	struct hwmem_alloc_threadg_info *tmp;
+
+	list_for_each_entry_safe(info, tmp, &(alloc->threadg_info_list),
+									list) {
+		list_del(&info->list);
+		destroy_alloc_threadg_info(info);
+	}
+}
+
+static enum hwmem_access get_access(struct hwmem_alloc *alloc)
+{
+	struct hwmem_alloc_threadg_info *info;
+	struct pid *my_pid;
+	bool found = false;
+
+	my_pid = find_get_pid(task_tgid_nr(current));
+	if (!my_pid)
+		return 0;
+
+	list_for_each_entry(info, &(alloc->threadg_info_list), list) {
+		if (info->threadg_pid == my_pid) {
+			found = true;
+			break;
+		}
+	}
+
+	put_pid(my_pid);
+
+	if (found)
+		return info->access;
+	else
+		return alloc->default_access;
+}
+
+static void clear_alloc_mem(struct hwmem_alloc *alloc)
+{
+	cach_set_domain(&alloc->cach_buf, HWMEM_ACCESS_WRITE,
+						HWMEM_DOMAIN_CPU, NULL);
+
+	memset(alloc->kaddr, 0, alloc->size);
+}
+
+static void destroy_alloc(struct hwmem_alloc *alloc)
+{
+	list_del(&alloc->list);
+
+	if (alloc->name != 0) {
+		idr_remove(&global_idr, alloc->name);
+		alloc->name = 0;
+	}
+
+	clean_alloc_threadg_info_list(alloc);
+
+	kunmap_alloc(alloc);
+
+	if (!IS_ERR_OR_NULL(alloc->allocator_hndl))
+		alloc->mem_type->allocator_api.free(
+					alloc->mem_type->allocator_instance,
+							alloc->allocator_hndl);
+
+	kfree(alloc);
+}
+
+static int kmap_alloc(struct hwmem_alloc *alloc)
+{
+	int ret;
+	pgprot_t pgprot;
+	void *alloc_kaddr;
+
+	alloc_kaddr = alloc->mem_type->allocator_api.get_alloc_kaddr(
+		alloc->mem_type->allocator_instance, alloc->allocator_hndl);
+	if (IS_ERR(alloc_kaddr))
+		return PTR_ERR(alloc_kaddr);
+
+	pgprot = PAGE_KERNEL;
+	cach_set_pgprot_cache_options(&alloc->cach_buf, &pgprot);
+
+	ret = ioremap_page_range((unsigned long)alloc_kaddr,
+		(unsigned long)alloc_kaddr + alloc->size, alloc->paddr, pgprot);
+	if (ret < 0) {
+		dev_warn(&hwdev->dev, "Failed to map %#x - %#x", alloc->paddr,
+						alloc->paddr + alloc->size);
+		return ret;
+	}
+
+	alloc->kaddr = alloc_kaddr;
+
+	return 0;
+}
+
+static void kunmap_alloc(struct hwmem_alloc *alloc)
+{
+	if (alloc->kaddr == NULL)
+		return;
+
+	unmap_kernel_range((unsigned long)alloc->kaddr, alloc->size);
+
+	alloc->kaddr = NULL;
+}
+
+static struct hwmem_mem_type_struct *resolve_mem_type(
+						enum hwmem_mem_type mem_type)
+{
+	unsigned int i;
+	for (i = 0; i < hwmem_num_mem_types; i++) {
+		if (hwmem_mem_types[i].id == mem_type)
+			return &hwmem_mem_types[i];
+	}
+
+	return ERR_PTR(-ENOENT);
+}
+
+/* HWMEM API */
+
+struct hwmem_alloc *hwmem_alloc(size_t size, enum hwmem_alloc_flags flags,
+		enum hwmem_access def_access, enum hwmem_mem_type mem_type)
+{
+	int ret;
+	struct hwmem_alloc *alloc;
+
+	if (hwdev == NULL) {
+		printk(KERN_ERR "HWMEM: Badly configured\n");
+		return ERR_PTR(-ENOMSG);
+	}
+
+	if (size == 0)
+		return ERR_PTR(-EINVAL);
+
+	mutex_lock(&lock);
+
+	size = PAGE_ALIGN(size);
+
+	alloc = kzalloc(sizeof(struct hwmem_alloc), GFP_KERNEL);
+	if (alloc == NULL) {
+		ret = -ENOMEM;
+		goto alloc_alloc_failed;
+	}
+
+	INIT_LIST_HEAD(&alloc->list);
+	atomic_inc(&alloc->ref_cnt);
+	alloc->flags = flags;
+	alloc->default_access = def_access;
+	INIT_LIST_HEAD(&alloc->threadg_info_list);
+#ifdef CONFIG_DEBUG_FS
+	alloc->creator = __builtin_return_address(0);
+	alloc->creator_tgid = task_tgid_nr(current);
+#endif
+	alloc->mem_type = resolve_mem_type(mem_type);
+	if (IS_ERR(alloc->mem_type)) {
+		ret = PTR_ERR(alloc->mem_type);
+		goto resolve_mem_type_failed;
+	}
+
+	alloc->allocator_hndl = alloc->mem_type->allocator_api.alloc(
+				alloc->mem_type->allocator_instance, size);
+	if (IS_ERR(alloc->allocator_hndl)) {
+		ret = PTR_ERR(alloc->allocator_hndl);
+		goto allocator_failed;
+	}
+
+	alloc->paddr = alloc->mem_type->allocator_api.get_alloc_paddr(
+							alloc->allocator_hndl);
+	alloc->size = alloc->mem_type->allocator_api.get_alloc_size(
+							alloc->allocator_hndl);
+
+	cach_init_buf(&alloc->cach_buf, alloc->flags, alloc->size);
+	ret = kmap_alloc(alloc);
+	if (ret < 0)
+		goto kmap_alloc_failed;
+	cach_set_buf_addrs(&alloc->cach_buf, alloc->kaddr, alloc->paddr);
+
+	list_add_tail(&alloc->list, &alloc_list);
+
+	clear_alloc_mem(alloc);
+
+	goto out;
+
+kmap_alloc_failed:
+allocator_failed:
+resolve_mem_type_failed:
+	destroy_alloc(alloc);
+alloc_alloc_failed:
+	alloc = ERR_PTR(ret);
+
+out:
+	mutex_unlock(&lock);
+
+	return alloc;
+}
+EXPORT_SYMBOL(hwmem_alloc);
+
+void hwmem_release(struct hwmem_alloc *alloc)
+{
+	mutex_lock(&lock);
+
+	if (atomic_dec_and_test(&alloc->ref_cnt))
+		destroy_alloc(alloc);
+
+	mutex_unlock(&lock);
+}
+EXPORT_SYMBOL(hwmem_release);
+
+int hwmem_set_domain(struct hwmem_alloc *alloc, enum hwmem_access access,
+		enum hwmem_domain domain, struct hwmem_region *region)
+{
+	mutex_lock(&lock);
+
+	cach_set_domain(&alloc->cach_buf, access, domain, region);
+
+	mutex_unlock(&lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(hwmem_set_domain);
+
+int hwmem_pin(struct hwmem_alloc *alloc, struct hwmem_mem_chunk *mem_chunks,
+							u32 *mem_chunks_length)
+{
+	if (*mem_chunks_length < 1) {
+		*mem_chunks_length = 1;
+		return -ENOSPC;
+	}
+
+	mutex_lock(&lock);
+
+	mem_chunks[0].paddr = alloc->paddr;
+	mem_chunks[0].size = alloc->size;
+	*mem_chunks_length = 1;
+
+	mutex_unlock(&lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(hwmem_pin);
+
+void hwmem_unpin(struct hwmem_alloc *alloc)
+{
+}
+EXPORT_SYMBOL(hwmem_unpin);
+
+static void vm_open(struct vm_area_struct *vma)
+{
+	atomic_inc(&((struct hwmem_alloc *)vma->vm_private_data)->ref_cnt);
+}
+
+static void vm_close(struct vm_area_struct *vma)
+{
+	hwmem_release((struct hwmem_alloc *)vma->vm_private_data);
+}
+
+int hwmem_mmap(struct hwmem_alloc *alloc, struct vm_area_struct *vma)
+{
+	int ret = 0;
+	unsigned long vma_size = vma->vm_end - vma->vm_start;
+	enum hwmem_access access;
+	mutex_lock(&lock);
+
+	access = get_access(alloc);
+
+	/* Check permissions */
+	if ((!(access & HWMEM_ACCESS_WRITE) &&
+				(vma->vm_flags & VM_WRITE)) ||
+			(!(access & HWMEM_ACCESS_READ) &&
+				(vma->vm_flags & VM_READ))) {
+		ret = -EPERM;
+		goto illegal_access;
+	}
+
+	if (vma_size > alloc->size) {
+		ret = -EINVAL;
+		goto illegal_size;
+	}
+
+	/*
+	 * We don't want Linux to do anything (merging etc) with our VMAs as
+	 * the offset is not necessarily valid
+	 */
+	vma->vm_flags |= VM_SPECIAL;
+	cach_set_pgprot_cache_options(&alloc->cach_buf, &vma->vm_page_prot);
+	vma->vm_private_data = (void *)alloc;
+	atomic_inc(&alloc->ref_cnt);
+	vma->vm_ops = &vm_ops;
+
+	ret = remap_pfn_range(vma, vma->vm_start, alloc->paddr >> PAGE_SHIFT,
+		min(vma_size, (unsigned long)alloc->size), vma->vm_page_prot);
+	if (ret < 0)
+		goto map_failed;
+
+	goto out;
+
+map_failed:
+	atomic_dec(&alloc->ref_cnt);
+illegal_size:
+illegal_access:
+
+out:
+	mutex_unlock(&lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(hwmem_mmap);
+
+void *hwmem_kmap(struct hwmem_alloc *alloc)
+{
+	void *ret;
+
+	mutex_lock(&lock);
+
+	ret = alloc->kaddr;
+
+	mutex_unlock(&lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(hwmem_kmap);
+
+void hwmem_kunmap(struct hwmem_alloc *alloc)
+{
+}
+EXPORT_SYMBOL(hwmem_kunmap);
+
+int hwmem_set_access(struct hwmem_alloc *alloc,
+		enum hwmem_access access, pid_t pid_nr)
+{
+	int ret;
+	struct hwmem_alloc_threadg_info *info;
+	struct pid *pid;
+	bool found = false;
+
+	pid = find_get_pid(pid_nr);
+	if (!pid) {
+		ret = -EINVAL;
+		goto error_get_pid;
+	}
+
+	list_for_each_entry(info, &(alloc->threadg_info_list), list) {
+		if (info->threadg_pid == pid) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		info = kmalloc(sizeof(*info), GFP_KERNEL);
+		if (!info) {
+			ret = -ENOMEM;
+			goto error_alloc_info;
+		}
+
+		info->threadg_pid = pid;
+		info->access = access;
+
+		list_add_tail(&(info->list), &(alloc->threadg_info_list));
+	} else {
+		info->access = access;
+	}
+
+	return 0;
+
+error_alloc_info:
+	put_pid(pid);
+error_get_pid:
+	return ret;
+}
+EXPORT_SYMBOL(hwmem_set_access);
+
+void hwmem_get_info(struct hwmem_alloc *alloc, u32 *size,
+	enum hwmem_mem_type *mem_type, enum hwmem_access *access)
+{
+	mutex_lock(&lock);
+
+	if (size != NULL)
+		*size = alloc->size;
+	if (mem_type != NULL)
+		*mem_type = alloc->mem_type->id;
+	if (access != NULL)
+		*access = get_access(alloc);
+
+	mutex_unlock(&lock);
+}
+EXPORT_SYMBOL(hwmem_get_info);
+
+s32 hwmem_get_name(struct hwmem_alloc *alloc)
+{
+	int ret = 0, name;
+
+	mutex_lock(&lock);
+
+	if (alloc->name != 0) {
+		ret = alloc->name;
+		goto out;
+	}
+
+	while (true) {
+		if (idr_pre_get(&global_idr, GFP_KERNEL) == 0) {
+			ret = -ENOMEM;
+			goto pre_get_id_failed;
+		}
+
+		ret = idr_get_new_above(&global_idr, alloc, 1, &name);
+		if (ret == 0)
+			break;
+		else if (ret != -EAGAIN)
+			goto get_id_failed;
+	}
+
+	if (name > S32_MAX) {
+		ret = -ENOMSG;
+		goto overflow;
+	}
+
+	alloc->name = name;
+
+	ret = name;
+	goto out;
+
+overflow:
+	idr_remove(&global_idr, name);
+get_id_failed:
+pre_get_id_failed:
+
+out:
+	mutex_unlock(&lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(hwmem_get_name);
+
+struct hwmem_alloc *hwmem_resolve_by_name(s32 name)
+{
+	struct hwmem_alloc *alloc;
+
+	mutex_lock(&lock);
+
+	alloc = idr_find(&global_idr, name);
+	if (alloc == NULL) {
+		alloc = ERR_PTR(-EINVAL);
+		goto find_failed;
+	}
+	atomic_inc(&alloc->ref_cnt);
+
+	goto out;
+
+find_failed:
+
+out:
+	mutex_unlock(&lock);
+
+	return alloc;
+}
+EXPORT_SYMBOL(hwmem_resolve_by_name);
+
+/* Debug */
+
+#ifdef CONFIG_DEBUG_FS
+
+static int debugfs_allocs_read(struct file *filp, char __user *buf,
+						size_t count, loff_t *f_pos);
+
+static const struct file_operations debugfs_allocs_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_allocs_read,
+};
+
+static int print_alloc(struct hwmem_alloc *alloc, char **buf, size_t buf_size)
+{
+	int ret;
+	char creator[KSYM_SYMBOL_LEN];
+	int i;
+
+	if (sprint_symbol(creator, (unsigned long)alloc->creator) < 0)
+		creator[0] = '\0';
+
+	for (i = 0; i < 2; i++) {
+		size_t buf_size_l;
+		if (i == 0)
+			buf_size_l = 0;
+		else
+			buf_size_l = buf_size;
+
+		ret = snprintf(*buf, buf_size_l,
+			"%#x\n"
+				"\tSize: %u\n"
+				"\tMemory type: %u\n"
+				"\tName: %#x\n"
+				"\tReference count: %i\n"
+				"\tAllocation flags: %#x\n"
+				"\t$ settings: %#x\n"
+				"\tDefault access: %#x\n"
+				"\tPhysical address: %#x\n"
+				"\tKernel virtual address: %#x\n"
+				"\tCreator: %s\n"
+				"\tCreator thread group id: %u\n",
+			(unsigned int)alloc, alloc->size, alloc->mem_type->id,
+			alloc->name, atomic_read(&alloc->ref_cnt),
+			alloc->flags, alloc->cach_buf.cache_settings,
+			alloc->default_access, alloc->paddr,
+			(unsigned int)alloc->kaddr, creator,
+			alloc->creator_tgid);
+		if (ret < 0)
+			return -ENOMSG;
+		else if (ret + 1 > buf_size)
+			return -EINVAL;
+	}
+
+	*buf += ret;
+
+	return 0;
+}
+
+static int debugfs_allocs_read(struct file *file, char __user *buf,
+						size_t count, loff_t *f_pos)
+{
+	/*
+	 * We assume the supplied buffer and PAGE_SIZE is large enough to hold
+	 * information about at least one alloc, if not no data will be
+	 * returned.
+	 */
+
+	int ret;
+	size_t i = 0;
+	struct hwmem_alloc *curr_alloc;
+	char *local_buf = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	char *local_buf_pos = local_buf;
+	size_t available_space = min((size_t)PAGE_SIZE, count);
+	/* private_data is intialized to NULL in open which I assume is 0. */
+	void **curr_pos = &file->private_data;
+	size_t bytes_read;
+
+	if (local_buf == NULL)
+		return -ENOMEM;
+
+	mutex_lock(&lock);
+
+	list_for_each_entry(curr_alloc, &alloc_list, list) {
+		if (i++ < (size_t)*curr_pos)
+			continue;
+
+		ret = print_alloc(curr_alloc, &local_buf_pos, available_space -
+					(size_t)(local_buf_pos - local_buf));
+		if (ret == -EINVAL) /* No more room */
+			break;
+		else if (ret < 0)
+			goto out;
+
+		*curr_pos = (void *)i;
+	}
+
+	bytes_read = (size_t)(local_buf_pos - local_buf);
+
+	ret = copy_to_user(buf, local_buf, bytes_read);
+	if (ret < 0)
+		goto out;
+
+	ret = bytes_read;
+
+out:
+	kfree(local_buf);
+
+	mutex_unlock(&lock);
+
+	return ret;
+}
+
+static void init_debugfs(void)
+{
+	/* Hwmem is never unloaded so dropping the dentrys is ok. */
+	struct dentry *debugfs_root_dir = debugfs_create_dir("hwmem", NULL);
+	(void)debugfs_create_file("allocs", 0444, debugfs_root_dir, 0,
+							&debugfs_allocs_fops);
+}
+
+#endif /* #ifdef CONFIG_DEBUG_FS */
+
+/* Module */
+
+extern int hwmem_ioctl_init(void);
+
+static int __devinit hwmem_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	if (hwdev) {
+		dev_err(&pdev->dev, "Probed multiple times\n");
+		return -EINVAL;
+	}
+
+	hwdev = pdev;
+
+	/*
+	 * No need to flush the caches here. If we can keep track of the cache
+	 * content then none of our memory will be in the caches, if we can't
+	 * keep track of the cache content we always assume all our memory is
+	 * in the caches.
+	 */
+
+	ret = hwmem_ioctl_init();
+	if (ret < 0)
+		dev_warn(&pdev->dev, "Failed to start hwmem-ioctl, continuing"
+								" anyway\n");
+
+#ifdef CONFIG_DEBUG_FS
+	init_debugfs();
+#endif
+
+	dev_info(&pdev->dev, "Probed OK\n");
+
+	return 0;
+}
+
+static struct platform_driver hwmem_driver = {
+	.probe	= hwmem_probe,
+	.driver = {
+		.name	= "hwmem",
+	},
+};
+
+static int __init hwmem_init(void)
+{
+	return platform_driver_register(&hwmem_driver);
+}
+subsys_initcall(hwmem_init);
+
+MODULE_AUTHOR("Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Hardware memory driver");
+
diff --git a/drivers/misc/i2s/Kconfig b/drivers/misc/i2s/Kconfig
new file mode 100644
index 00000000000..569818caa5d
--- /dev/null
+++ b/drivers/misc/i2s/Kconfig
@@ -0,0 +1,19 @@
+#
+# U8500 I2S HW kernel configuration
+#
+config STM_I2S
+	bool "U8500 I2S hardware driver"
+	depends on ARCH_U8500 && STE_DMA40
+	default y
+	---help---
+	  If you say Y here, you will enable the U8500 I2S hardware driver.
+
+	  If unsure, say N.
+config STM_MSP_I2S
+	tristate "U8500 MSP_I2S hardware driver"
+	depends on ARCH_U8500 && STE_DMA40 && STM_I2S
+	default y
+	---help---
+	  If you say Y here, you will enable the U8500 MSP_I2S hardware driver.
+
+	  If unsure, say N.
diff --git a/drivers/misc/i2s/Makefile b/drivers/misc/i2s/Makefile
new file mode 100644
index 00000000000..06cb51d7b30
--- /dev/null
+++ b/drivers/misc/i2s/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for I2S drivers
+#
+
+nmdk_i2s-objs := i2s.o
+obj-$(CONFIG_STM_I2S)	+= nmdk_i2s.o
+obj-$(CONFIG_STM_MSP_I2S)	+= msp_i2s.o
diff --git a/drivers/misc/i2s/i2s.c b/drivers/misc/i2s/i2s.c
new file mode 100644
index 00000000000..8ee7492a4fa
--- /dev/null
+++ b/drivers/misc/i2s/i2s.c
@@ -0,0 +1,631 @@
+/*----------------------------------------------------------------------------*/
+/*  copyright STMicroelectronics, 2007.                                       */
+/*                                                                            */
+/* 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.1 of the License, or (at your option)*/
+/* any later version.                                                         */
+/*                                                                            */
+/* This program is distributed in the hope that it will be useful, but        */
+/* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY */
+/* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License    */
+/* for more details.                                                          */
+/*                                                                            */
+/* You should have received a copy of the GNU General Public License          */
+/* along with this program. If not, see <http://www.gnu.org/licenses/>.       */
+/*----------------------------------------------------------------------------*/
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/cache.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/idr.h>
+#include <linux/i2s/i2s.h>
+#include <linux/platform_device.h>
+
+/*******************************************************************************/
+static DEFINE_MUTEX(core_lock);
+
+static void i2sdev_release(struct device *dev)
+{
+	struct i2s_device *i2s = to_i2s_device(dev);
+
+	if (i2s->controller)
+		put_device(&(i2s->controller->dev));
+	kfree(dev);
+}
+static ssize_t
+modalias_show(struct device *dev, struct device_attribute *a, char *buf)
+{
+	const struct i2s_device *i2s = to_i2s_device(dev);
+	return sprintf(buf, "%s\n", i2s->modalias);
+}
+
+static struct device_attribute i2s_dev_attrs[] = {
+	__ATTR_RO(modalias),
+	__ATTR_NULL,
+};
+
+/* modalias support makes "modprobe $MODALIAS" new-style hotplug work,
+ * and the sysfs version makes coldplug work too.
+ */
+static const struct i2s_device_id *i2s_match_id(const struct i2s_device_id *id,
+						const struct i2s_device *device)
+{
+	while (id->name[0]) {
+		if (strcmp(device->modalias, id->name) == 0)
+			return id;
+		id++;
+	}
+	return NULL;
+}
+
+static int i2s_match_device(struct device *dev, struct device_driver *drv)
+{
+	const struct i2s_device *device = to_i2s_device(dev);
+	struct i2s_driver *driver = to_i2s_driver(drv);
+	if (driver->id_table)
+		return i2s_match_id(driver->id_table, device) != NULL;
+	return 0;
+}
+
+static int i2s_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	const struct i2s_device *i2s = to_i2s_device(dev);
+
+	add_uevent_var(env, "MODALIAS=%s", i2s->modalias);
+	return 0;
+}
+
+#ifdef	CONFIG_PM
+static int i2s_suspend(struct device *dev, pm_message_t message)
+{
+	int value = 0;
+	struct i2s_driver *drv = to_i2s_driver(dev->driver);
+
+	/* suspend will stop irqs and dma; no more i/o */
+	if (drv) {
+		if (drv->suspend)
+			value = drv->suspend(to_i2s_device(dev), message);
+		else
+			dev_dbg(dev, "... can't suspend\n");
+	}
+	return value;
+}
+
+static int i2s_resume(struct device *dev)
+{
+	int value = 0;
+	struct i2s_driver *drv = to_i2s_driver(dev->driver);
+
+	/* resume may restart the i/o queue */
+	if (drv) {
+		if (drv->resume)
+			value = drv->resume(to_i2s_device(dev));
+		else
+			dev_dbg(dev, "... can't resume\n");
+	}
+	return value;
+}
+
+#else
+#define i2s_suspend	NULL
+#define i2s_resume	NULL
+#endif
+
+/*This bus is designed to handle various protocols supported by the MSP- ARM Primecell IP
+ * such as
+ * I2s, PCM, AC97, TDM .... (refer to the data sheet for the complete list.
+ * Current MSP driver has the above ones coded.
+ * */
+struct bus_type i2s_bus_type = {
+	.name = "i2s",
+	.dev_attrs = i2s_dev_attrs,
+	.match = i2s_match_device,
+	.uevent = i2s_uevent,
+	.suspend = i2s_suspend,
+	.resume = i2s_resume,
+};
+
+EXPORT_SYMBOL_GPL(i2s_bus_type);
+
+static int i2s_drv_probe(struct device *dev)
+{
+	const struct i2s_driver *sdrv = to_i2s_driver(dev->driver);
+
+	return sdrv->probe(to_i2s_device(dev));
+}
+
+static int i2s_drv_remove(struct device *dev)
+{
+	const struct i2s_driver *sdrv = to_i2s_driver(dev->driver);
+
+	return sdrv->remove(to_i2s_device(dev));
+}
+
+static void i2s_drv_shutdown(struct device *dev)
+{
+	const struct i2s_driver *sdrv = to_i2s_driver(dev->driver);
+
+	sdrv->shutdown(to_i2s_device(dev));
+}
+
+/**
+ * i2s_register_driver - register a I2S driver
+ * @sdrv: the driver to register
+ * Context: can sleep
+ */
+int i2s_register_driver(struct i2s_driver *sdrv)
+{
+	sdrv->driver.bus = &i2s_bus_type;
+	if (sdrv->probe)
+		sdrv->driver.probe = i2s_drv_probe;
+	if (sdrv->remove)
+		sdrv->driver.remove = i2s_drv_remove;
+	if (sdrv->shutdown)
+		sdrv->driver.shutdown = i2s_drv_shutdown;
+	return driver_register(&sdrv->driver);
+}
+
+EXPORT_SYMBOL_GPL(i2s_register_driver);
+
+/******************************************************************************/
+struct board_i2s_combined_info {
+	struct i2s_board_info board_info;
+	struct i2s_device *i2s_dev_p;
+};
+struct boardinfo {
+	struct list_head list;
+	unsigned n_board_info;
+	struct board_i2s_combined_info board_i2s_info[0];
+};
+
+static LIST_HEAD(board_list);
+static DEFINE_MUTEX(board_lock);
+
+/*
+ * Get an i2s device. Used in MSP LTP tests.
+ */
+struct i2s_device *i2s_get_device_from_boardinfo(int chip_select)
+{
+	struct boardinfo *bi;
+	struct i2s_device *i2s_dev_p = NULL;
+
+	mutex_lock(&board_lock);
+	list_for_each_entry(bi, &board_list, list) {
+		struct board_i2s_combined_info *chip = bi->board_i2s_info;
+		unsigned n;
+
+		for (n = bi->n_board_info; n > 0; n--, chip++)
+			if (chip->board_info.chip_select == chip_select) {
+				i2s_dev_p = chip->i2s_dev_p;
+				break;
+			}
+		if (i2s_dev_p != NULL)
+			break;
+	}
+	mutex_unlock(&board_lock);
+
+	return i2s_dev_p;
+}
+
+EXPORT_SYMBOL_GPL(i2s_get_device_from_boardinfo);
+
+/* I2S devices should normally not be created by I2S device drivers; that
+ * would make them board-specific.  Similarly with I2S master drivers.
+ * Device registration normally goes into like arch/.../mach.../board-YYY.c
+ * with other readonly (flashable) information about mainboard devices.
+ */
+struct i2s_device *i2s_alloc_device(struct device *device)
+{
+	struct i2s_device *i2s;
+	struct device *dev = device->parent;
+
+	get_device(device);
+	i2s = kzalloc(sizeof *i2s, GFP_KERNEL);
+	if (!i2s) {
+		dev_err(dev, "cannot alloc i2s_device\n");
+		return NULL;
+	}
+
+	i2s->dev.parent = dev;
+	i2s->dev.bus = &i2s_bus_type;
+	i2s->dev.release = i2sdev_release;
+	device_initialize(&i2s->dev);
+	return i2s;
+}
+
+EXPORT_SYMBOL_GPL(i2s_alloc_device);
+
+/**
+ * i2s_add_device - Add i2s_device allocated with i2s_alloc_device
+ * @i2s: i2s_device to register
+ *
+ * Companion function to i2s_alloc_device.  Devices allocated with
+ * i2s_alloc_device can be added onto the i2s bus with this function.
+ *
+ * Returns 0 on success; negative errno on failure
+ */
+int i2s_add_device(struct i2s_device *i2s)
+{
+	static DEFINE_MUTEX(i2s_add_lock);
+	struct device *dev = i2s->dev.parent;
+	int status;
+
+	dev_set_name(&i2s->dev, "%s.%u", "i2s", i2s->chip_select);
+
+	mutex_lock(&i2s_add_lock);
+
+	if (bus_find_device_by_name(&i2s_bus_type, NULL, dev_name(&i2s->dev))
+	    != NULL) {
+		dev_err(dev, "chipselect %d already in use\n",
+			i2s->chip_select);
+		status = -EBUSY;
+		goto done;
+	}
+
+	/* Device may be bound to an active driver when this returns */
+	status = device_add(&i2s->dev);
+	if (status < 0)
+		dev_err(dev, "can't %s %s, status %d\n",
+			"add", dev_name(&i2s->dev), status);
+	else
+		dev_dbg(dev, "registered child %s\n", dev_name(&i2s->dev));
+
+	  done:
+	mutex_unlock(&i2s_add_lock);
+	return status;
+}
+
+EXPORT_SYMBOL_GPL(i2s_add_device);
+
+/**
+ * i2s_new_device - instantiate one new I2S device
+ * @i2s_cont: Controller to which device is connected
+ * @chip: Describes the I2S device
+ * Context: can sleep
+ *
+ * On typical mainboards, this is purely internal; and it's not needed
+ * after board init creates the hard-wired devices.  Some development
+ * platforms may not be able to use i2s_register_board_info though, and
+ * this is exported so that driver could add devices (which it would
+ * learn about out-of-band).
+ *
+ * Returns the new device, or NULL.
+ */
+struct i2s_device *i2s_new_device(struct i2s_controller *i2s_cont,
+				  struct i2s_board_info *chip)
+{
+	struct i2s_device *proxy;
+	int status;
+
+	/* NOTE:  caller did any chip->bus_num checks necessary.
+	 *
+	 * Also, unless we change the return value convention to use
+	 * error-or-pointer (not NULL-or-pointer), troubleshootability
+	 * suggests syslogged diagnostics are best here (ugh).
+	 */
+
+	proxy = i2s_alloc_device(&i2s_cont->dev);
+	if (!proxy)
+		return NULL;
+
+	WARN_ON(strlen(chip->modalias) >= sizeof(proxy->modalias));
+
+	proxy->chip_select = chip->chip_select;
+	strlcpy(proxy->modalias, chip->modalias, sizeof(proxy->modalias));
+	proxy->dev.platform_data = (void *)chip->platform_data;
+	proxy->controller = i2s_cont;
+
+	status = i2s_add_device(proxy);
+	if (status < 0) {
+		kfree(proxy);
+		return NULL;
+	}
+
+	return proxy;
+}
+
+EXPORT_SYMBOL_GPL(i2s_new_device);
+
+/**
+ * i2s_register_board_info - register I2S devices for a given board
+ * @info: array of chip descriptors
+ * @n: how many descriptors are provided
+ * Context: can sleep
+ *
+ * Board-specific early init code calls this (probably during arch_initcall)
+ * with segments of the I2S device table.  Any device nodes are created later,
+ * after the relevant parent I2S controller (id) is defined.  We keep
+ * this table of devices forever, so that reloading a controller driver will
+ * not make Linux forget about these hard-wired devices.
+ *
+ */
+int __init
+i2s_register_board_info(struct i2s_board_info const *info, unsigned n)
+{
+	int i;
+	struct boardinfo *bi;
+
+	bi = kmalloc(sizeof(*bi) + (n * sizeof(struct board_i2s_combined_info)), GFP_KERNEL);
+	if (!bi)
+		return -ENOMEM;
+	bi->n_board_info = n;
+
+	for (i = 0; i < n; i++)
+		memcpy(&bi->board_i2s_info[i].board_info, &info[i], sizeof *info);
+
+	mutex_lock(&board_lock);
+	list_add_tail(&bi->list, &board_list);
+	mutex_unlock(&board_lock);
+	return 0;
+}
+
+/**
+ * scan_boardinfo - Scan, creates and registered new i2s device structure.
+ * @i2s_cont: i2s controller structure
+ * Context: process
+ *
+ * It will scan the device list that may be registered statically using
+ * register_board_info func in arch specific directory and call
+ * i2s_new_device to create and registered i2s device over i2s bus. It is
+ * called by i2s_add_controller function.
+ *
+ * Returns void.
+ */
+static void scan_boardinfo(struct i2s_controller *i2s_cont)
+{
+	struct boardinfo *bi;
+
+	mutex_lock(&board_lock);
+	list_for_each_entry(bi, &board_list, list) {
+		struct board_i2s_combined_info *chip = bi->board_i2s_info;
+		unsigned n;
+
+		for (n = bi->n_board_info; n > 0; n--, chip++) {
+			if (chip->board_info.chip_select != i2s_cont->id)
+				continue;
+			/* NOTE: this relies on i2s_new_device to
+			 * issue diagnostics when given bogus inputs
+			 */
+			chip->i2s_dev_p = i2s_new_device(i2s_cont, &chip->board_info);
+		}
+	}
+	mutex_unlock(&board_lock);
+}
+
+/******************************************************************************/
+/**I2S Controller inittialization*/
+static void i2s_controller_dev_release(struct device *dev)
+{
+	struct i2s_controller *i2s_cont;
+	i2s_cont = container_of(dev, struct i2s_controller, dev);
+	kfree(i2s_cont);
+}
+
+static ssize_t
+show_controller_name(struct device *dev, struct device_attribute *attr,
+		     char *buf)
+{
+	struct i2s_controller *cont = to_i2s_controller(dev);
+	return sprintf(buf, "%s\n", cont->name);
+}
+
+static struct device_attribute i2s_controller_attrs[] = {
+	__ATTR(name, S_IRUGO, show_controller_name, NULL),
+	{},
+};
+
+static struct class i2s_controller_class = {
+	.owner = THIS_MODULE,
+	.name = "i2s-controller",
+	.dev_attrs = i2s_controller_attrs,
+};
+
+static int i2s_register_controller(struct i2s_controller *cont)
+{
+	int res = 0;
+	mutex_init(&cont->bus_lock);
+
+	mutex_lock(&core_lock);
+
+	/* Add the controller to the driver core.
+	 * If the parent pointer is not set up,
+	 * we add this controller to the host bus.
+	 */
+	if (cont->dev.parent == NULL) {
+		cont->dev.parent = &platform_bus;
+		pr_debug("I2S controller driver [%s] forgot to specify "
+			 "physical device\n", cont->name);
+	}
+	dev_set_name(&cont->dev, "I2Scrlr-%d", cont->id);
+	cont->dev.release = &i2s_controller_dev_release;
+	cont->dev.class = &i2s_controller_class;
+	res = device_register(&cont->dev);
+	if (res)
+		goto out_unlock;
+
+	dev_dbg(&cont->dev, "controller [%s] registered\n", cont->name);
+	scan_boardinfo(cont);
+	  out_unlock:
+	mutex_unlock(&core_lock);
+	return res;
+}
+
+/**
+ * i2s_add_controller - declare i2s controller, use dynamic bus number
+ * @controller: the controller to add
+ * Context: can sleep
+ *
+ */
+int i2s_add_controller(struct i2s_controller *controller)
+{
+	return i2s_register_controller(controller);
+}
+
+EXPORT_SYMBOL(i2s_add_controller);
+
+static int __unregister(struct device *dev, void *controller_dev)
+{
+	/* note: before about 2.6.14-rc1 this would corrupt memory: */
+	if (dev != controller_dev)
+		i2s_unregister_device(to_i2s_device(dev));
+	return 0;
+}
+
+/**
+ * i2s_del_controller - unregister I2S controller
+ * @cont: the controller being unregistered
+ * Context: can sleep
+ *
+ * This unregisters an I2S controller which was previously registered
+ * by @i2s_add_controller.
+ */
+int i2s_del_controller(struct i2s_controller *cont)
+{
+	int res = 0;
+	int dummy;
+	mutex_lock(&core_lock);
+
+	dummy = device_for_each_child(cont->dev.parent, &cont->dev,
+				      __unregister);
+	device_unregister(&cont->dev);
+	mutex_unlock(&core_lock);
+	return res;
+}
+
+EXPORT_SYMBOL(i2s_del_controller);
+
+/******************************************************************************/
+/*I2S interface apis*/
+
+/**
+ * i2s_transfer - Main i2s transfer function.
+ * @i2s_cont: i2s controller structure passed by client driver.
+ * @message: i2s message structure contains transceive info.
+ * Context: process or interrupt.
+ *
+ * This API is called by client i2s driver as i2s_xfer funtion. It will handle
+ * main i2s transfer over i2s bus. The controller should registered its own
+ * functions using i2s algorithm structure.
+ *
+ * Returns error(-1) in case of failure or success(0).
+ */
+int i2s_transfer(struct i2s_controller *i2s_cont, struct i2s_message *message)
+{
+	return i2s_cont->algo->cont_transfer(i2s_cont, message);
+
+}
+
+EXPORT_SYMBOL(i2s_transfer);
+
+/**
+ * i2s_cleanup - Close the current i2s connection btw controller and client.
+ * @i2s_cont: i2s controller structure
+ * @flag: It indicates the functionality that needs to be disabled.
+ * Context: process
+ *
+ * This API will disable and reset the controller's configuration. Reset the
+ * controller so that i2s client driver can reconfigure with new configuration.
+ * Controller should release all the necessary resources which was acquired
+ * during setup.
+ *
+ * Returns error(-1) in case of failure or success(0).
+ */
+int i2s_cleanup(struct i2s_controller *i2s_cont, i2s_flag flag)
+{
+	int status = 0;
+	status = i2s_cont->algo->cont_cleanup(i2s_cont, flag);
+	if (status)
+		return -1;
+	else
+		return 0;
+}
+
+EXPORT_SYMBOL(i2s_cleanup);
+
+/**
+ * i2s_setup - configures and enables the I2S controller.
+ * @i2s_cont: i2s controller sent by i2s device.
+ * @config: specifies the configuration parameters.
+ *
+ * This function configures the I2S controller with the client configuration.
+ * Controller was already registered on I2S bus by some master controller
+ * driver.
+ *
+ * Returns error(-1) in case of failure else success(0)
+ */
+int i2s_setup(struct i2s_controller *i2s_cont, void *config)
+{
+	return i2s_cont->algo->cont_setup(i2s_cont, config);
+}
+
+EXPORT_SYMBOL(i2s_setup);
+
+/**
+ * i2s_hw_status - Get the current hw status for the i2s controller.
+ * @i2s_cont: i2s controller structure passed by client driver.
+ * Context: process or interrupt.
+ *
+ * This API is called by client i2s driver to find out current hw status.
+ * The controller should registered its own functions using i2s algorithm structure.
+ *
+ * Returns current hw status register.
+ */
+int i2s_hw_status(struct i2s_controller *i2s_cont)
+{
+	return i2s_cont->algo->cont_hw_status(i2s_cont);
+}
+
+/**
+ * i2s_get_pointer - Get the current dma_addr_t for the i2s controller.
+ * @i2s_cont: i2s controller structure passed by client driver.
+ * @i2s_direction: Specifies TX or RX direction.
+ * Context: process or interrupt.
+ *
+ * This API is called by client i2s driver to return a dma_addr_t corresponding
+ * to the position of the DMA-controller.
+ * The controller should registered its own functions using i2s algorithm structure.
+ *
+ * Returns current hw status register.
+ */
+dma_addr_t i2s_get_pointer(struct i2s_controller *i2s_cont,
+			enum i2s_direction_t i2s_direction)
+{
+	return i2s_cont->algo->cont_get_pointer(i2s_cont, i2s_direction);
+}
+
+/******************************************************************************/
+
+static int __init i2s_init(void)
+{
+	int status;
+
+	status = bus_register(&i2s_bus_type);
+	if (status < 0)
+		goto err0;
+
+	status = class_register(&i2s_controller_class);
+	if (status < 0)
+		goto err1;
+	return 0;
+
+	  err1:
+	bus_unregister(&i2s_bus_type);
+	  err0:
+	return status;
+}
+
+static void __exit i2s_exit(void)
+{
+	class_unregister(&i2s_controller_class);
+	bus_unregister(&i2s_bus_type);
+}
+
+subsys_initcall(i2s_init);
+module_exit(i2s_exit);
+
+MODULE_AUTHOR("Sandeep Kaushik, <sandeep-mmc.kaushik@st.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/i2s/msp_i2s.c b/drivers/misc/i2s/msp_i2s.c
new file mode 100644
index 00000000000..343a502c68e
--- /dev/null
+++ b/drivers/misc/i2s/msp_i2s.c
@@ -0,0 +1,2034 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License terms:
+ *
+ * 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 <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/pfn.h>
+
+#include <linux/regulator/consumer.h>
+#include <mach/prcmu.h>
+#include <mach/hardware.h>
+#include <asm/io.h>
+#include <asm/delay.h>
+#include <asm/irq.h>
+#include <linux/dmaengine.h>
+
+#include <mach/hardware.h>
+#include <mach/irqs.h>
+#include <linux/i2s/i2s.h>
+#include <mach/msp.h>
+#include <linux/dma-mapping.h>
+
+struct regulator *msp_vape_supply;
+
+#define STM_MSP_NAME "STM_MSP"
+#define MSP_NAME "msp"
+#define DRIVER_DEBUG_PFX "MSP"
+#define DRIVER_DEBUG CONFIG_STM_MSP_DEBUG
+#define DRIVER_DBG "MSP"
+#define NMDK_DBG /* message level */
+
+extern struct driver_debug_st DBG_ST;
+ /* Protocol desciptors */
+static const struct msp_protocol_desc protocol_desc_tab[] = {
+	I2S_PROTOCOL_DESC,
+	PCM_PROTOCOL_DESC,
+	PCM_COMPAND_PROTOCOL_DESC,
+	AC97_PROTOCOL_DESC,
+	SPI_MASTER_PROTOCOL_DESC,
+	SPI_SLAVE_PROTOCOL_DESC,
+};
+
+/* Local static functions */
+static int msp_dma_xfer(struct msp *msp, struct i2s_message *msg);
+static int msp_polling_xfer(struct msp *msp, struct i2s_message *msg);
+static int msp_interrupt_xfer(struct msp *msp, struct i2s_message *msg);
+static int msp_start_dma(struct msp *msp, int transmit, dma_addr_t data,
+			 size_t bytes);
+static int configure_protocol(struct msp *msp,
+			      struct msp_config *config);
+static int configure_clock(struct msp *msp,
+			   struct msp_config *config);
+static int configure_multichannel(struct msp *msp,
+				  struct msp_config *config);
+static int stm_msp_configure_enable(struct i2s_controller *i2s_cont,
+				    void *configuration);
+static int stm_msp_transceive_data(struct i2s_controller *i2s_cont,
+				   struct i2s_message *message);
+
+static int stm_msp_disable(struct msp *msp, int direction,
+			   i2s_flag flag);
+static int stm_msp_close(struct i2s_controller *i2s_cont, i2s_flag flag);
+static int stm_msp_hw_status(struct i2s_controller *i2s_cont);
+
+#define I2S_DEVICE "i2s_device"
+static struct i2s_algorithm i2s_algo = {
+	.cont_setup		= stm_msp_configure_enable,
+	.cont_transfer		= stm_msp_transceive_data,
+	.cont_cleanup		= stm_msp_close,
+	.cont_hw_status		= stm_msp_hw_status,
+};
+
+/**
+ * stm_msp_write - writel a value to specified register
+ * @value: value
+ * @reg: pointer to register' address
+ * Context: atomic(can be both process and interrupt)
+ * Returns void.
+ */
+static inline void stm_msp_write(u32 value, void __iomem *reg)
+{
+	writel(value, reg);
+}
+
+/**
+ * stm_msp_read - readl a value to specified register
+ * @reg: pointer to register' address
+ * Context: atomic(can be both process and interrupt)
+ * Returns u32 register's value.
+ */
+static inline u32 stm_msp_read(void __iomem *reg)
+{
+	return readl(reg);
+}
+
+static void u8_msp_read(struct trans_data *xfer_data)
+{
+	struct i2s_message *message = &xfer_data->message;
+	while ((message->rx_offset < message->rxbytes) &&
+	       !((stm_msp_read(xfer_data->msp->registers + MSP_FLR)) &
+		 RX_FIFO_EMPTY)) {
+		message->rx_offset += 1;
+		*(u8 *) message->rxdata =
+		    (u8) stm_msp_read(xfer_data->msp->registers + MSP_DR);
+		message->rxdata += 1;
+	}
+}
+
+static void u16_msp_read(struct trans_data *xfer_data)
+{
+	struct i2s_message *message = &xfer_data->message;
+	while ((message->rx_offset < message->rxbytes) &&
+	       !((stm_msp_read(xfer_data->msp->registers + MSP_FLR)) &
+		 RX_FIFO_EMPTY)) {
+		message->rx_offset += 2;
+		*(u16 *) message->rxdata =
+		    (u16) stm_msp_read(xfer_data->msp->registers + MSP_DR);
+		message->rxdata += 2;
+	}
+}
+
+/**
+ * u32_msp_read - Msp 32bit read function.
+ * @xfer_data: transfer data structure.
+ *
+ * It reads 32bit data from msp receive fifo until it gets empty.
+ *
+ * Returns void.
+ */
+static void u32_msp_read(struct trans_data *xfer_data)
+{
+	struct i2s_message *message = &xfer_data->message;
+	while ((message->rx_offset < message->rxbytes) &&
+	       !((stm_msp_read(xfer_data->msp->registers + MSP_FLR)) &
+		 RX_FIFO_EMPTY)) {
+		*(u32 *) message->rxdata =
+		    (u32) stm_msp_read(xfer_data->msp->registers + MSP_DR);
+		message->rx_offset += 4;
+		message->rxdata += 4;
+	}
+}
+static void u8_msp_write(struct trans_data *xfer_data)
+{
+	struct i2s_message *message = &xfer_data->message;
+	while ((message->tx_offset < message->txbytes) &&
+	       !((stm_msp_read(xfer_data->msp->registers + MSP_FLR)) &
+		 TX_FIFO_FULL)) {
+		message->tx_offset += 1;
+		stm_msp_write(*(u8 *) message->txdata,
+			      xfer_data->msp->registers + MSP_DR);
+		message->txdata += 1;
+	}
+}
+
+static void u16_msp_write(struct trans_data *xfer_data)
+{
+	struct i2s_message *message = &xfer_data->message;
+	while ((message->tx_offset < message->txbytes) &&
+	       !((stm_msp_read(xfer_data->msp->registers + MSP_FLR)) &
+		 TX_FIFO_FULL)) {
+		message->tx_offset += 2;
+		stm_msp_write(*(u16 *) message->txdata,
+			      xfer_data->msp->registers + MSP_DR);
+		message->txdata += 2;
+	}
+}
+
+/**
+ * u32_msp_write - Msp 32bit write function.
+ * @xfer_data: transfer data structure.
+ *
+ * It writes 32bit data to msp transmit fifo until it gets full.
+ *
+ * Returns void.
+ */
+static void u32_msp_write(struct trans_data *xfer_data)
+{
+	struct i2s_message *message = &xfer_data->message;
+	while ((message->tx_offset < message->txbytes) &&
+	       !((stm_msp_read(xfer_data->msp->registers + MSP_FLR)) &
+		 TX_FIFO_FULL)) {
+		message->tx_offset += 4;
+		stm_msp_write(*(u32 *) message->txdata,
+			      xfer_data->msp->registers + MSP_DR);
+		message->txdata += 4;
+	}
+}
+
+/**
+ * set_transmit_protocol_descriptor - Set the Transmit Configuration register.
+ * @msp: main msp controller structure.
+ * @protocol_desc: pointer to protocol descriptor structure.
+ * @data_size: Run time configurable element length.
+ *
+ * It will setup transmit configuration register of msp.
+ * Various values related to a particular protocol can be set like, elemnet
+ * length, frame length, endianess etc.
+ *
+ * Returns void.
+ */
+static void set_transmit_protocol_descriptor(struct msp *msp,
+						struct msp_protocol_desc
+							*protocol_desc,
+						enum msp_data_size data_size)
+{
+	u32 temp_reg = 0;
+
+	temp_reg |= MSP_P2_ENABLE_BIT(protocol_desc->tx_phase_mode);
+	temp_reg |= MSP_P2_START_MODE_BIT(protocol_desc->tx_phase2_start_mode);
+	temp_reg |= MSP_P1_FRAME_LEN_BITS(protocol_desc->tx_frame_length_1);
+	temp_reg |= MSP_P2_FRAME_LEN_BITS(protocol_desc->tx_frame_length_2);
+	if (msp->def_elem_len) {
+		temp_reg |=
+		    MSP_P1_ELEM_LEN_BITS(protocol_desc->tx_element_length_1);
+		temp_reg |=
+		    MSP_P2_ELEM_LEN_BITS(protocol_desc->tx_element_length_2);
+		if (protocol_desc->tx_element_length_1 ==
+		    protocol_desc->tx_element_length_2) {
+			msp->actual_data_size =
+			    protocol_desc->tx_element_length_1;
+		} else {
+			msp->actual_data_size = data_size;
+		}
+	} else {
+		temp_reg |= MSP_P1_ELEM_LEN_BITS(data_size);
+		temp_reg |= MSP_P2_ELEM_LEN_BITS(data_size);
+		msp->actual_data_size = data_size;
+	}
+	temp_reg |= MSP_DATA_DELAY_BITS(protocol_desc->tx_data_delay);
+	temp_reg |=
+	    MSP_SET_ENDIANNES_BIT(protocol_desc->tx_bit_transfer_format);
+	temp_reg |= MSP_FRAME_SYNC_POL(protocol_desc->tx_frame_sync_pol);
+	temp_reg |= MSP_DATA_WORD_SWAP(protocol_desc->tx_half_word_swap);
+	temp_reg |= MSP_SET_COMPANDING_MODE(protocol_desc->compression_mode);
+	temp_reg |= MSP_SET_FRAME_SYNC_IGNORE(protocol_desc->frame_sync_ignore);
+
+	stm_msp_write(temp_reg, msp->registers + MSP_TCF);
+}
+
+/**
+ * set_receive_protocol_descriptor - Set the Receive Configuration register.
+ * @msp: main msp controller structure.
+ * @protocol_desc: pointer to protocol descriptor structure.
+ * @data_size: Run time configurable element length.
+ *
+ * It will setup receive configuration register of msp.
+ * Various values related to a particular protocol can be set like, elemnet
+ * length, frame length, endianess etc.
+ *
+ * Returns void.
+ */
+static void set_receive_protocol_descriptor(struct msp *msp,
+						struct msp_protocol_desc
+							*protocol_desc,
+							enum msp_data_size
+								data_size)
+{
+	u32 temp_reg = 0;
+
+	temp_reg |= MSP_P2_ENABLE_BIT(protocol_desc->rx_phase_mode);
+	temp_reg |= MSP_P2_START_MODE_BIT(protocol_desc->rx_phase2_start_mode);
+	temp_reg |= MSP_P1_FRAME_LEN_BITS(protocol_desc->rx_frame_length_1);
+	temp_reg |= MSP_P2_FRAME_LEN_BITS(protocol_desc->rx_frame_length_2);
+	if (msp->def_elem_len) {
+		temp_reg |=
+		    MSP_P1_ELEM_LEN_BITS(protocol_desc->rx_element_length_1);
+		temp_reg |=
+		    MSP_P2_ELEM_LEN_BITS(protocol_desc->rx_element_length_2);
+		if (protocol_desc->rx_element_length_1 ==
+		    protocol_desc->rx_element_length_2) {
+			msp->actual_data_size =
+			    protocol_desc->rx_element_length_1;
+		} else {
+			msp->actual_data_size = data_size;
+		}
+	} else {
+		temp_reg |= MSP_P1_ELEM_LEN_BITS(data_size);
+		temp_reg |= MSP_P2_ELEM_LEN_BITS(data_size);
+		msp->actual_data_size = data_size;
+	}
+
+	temp_reg |= MSP_DATA_DELAY_BITS(protocol_desc->rx_data_delay);
+	temp_reg |=
+	    MSP_SET_ENDIANNES_BIT(protocol_desc->rx_bit_transfer_format);
+	temp_reg |= MSP_FRAME_SYNC_POL(protocol_desc->rx_frame_sync_pol);
+	temp_reg |= MSP_DATA_WORD_SWAP(protocol_desc->rx_half_word_swap);
+	temp_reg |= MSP_SET_COMPANDING_MODE(protocol_desc->expansion_mode);
+	temp_reg |= MSP_SET_FRAME_SYNC_IGNORE(protocol_desc->frame_sync_ignore);
+
+	stm_msp_write(temp_reg, msp->registers + MSP_RCF);
+
+}
+
+/**
+ * configure_protocol - Configures transmit and receive protocol.
+ * @msp: main msp controller structure.
+ * @config: configuration structure passed by client driver
+ *
+ * This will configure transmit and receive protocol decriptors.
+ *
+ * Returns error(-1) on failure else success(0).
+ */
+static int configure_protocol(struct msp *msp,
+			      struct msp_config *config)
+{
+	int direction;
+	struct msp_protocol_desc *protocol_desc;
+	enum msp_data_size data_size;
+	u32 temp_reg = 0;
+
+	data_size = config->data_size;
+	msp->def_elem_len = config->def_elem_len;
+	direction = config->direction;
+	if (config->default_protocol_desc == 1) {
+		if (config->protocol >= MSP_INVALID_PROTOCOL) {
+			printk(KERN_ERR
+			       "invalid protocol in configure_protocol()\n");
+			return -EINVAL;
+		}
+		protocol_desc =
+		    (struct msp_protocol_desc *)&protocol_desc_tab[config->
+								   protocol];
+	} else {
+		protocol_desc =
+		    (struct msp_protocol_desc *)&config->protocol_desc;
+	}
+
+	if (data_size < MSP_DATA_BITS_DEFAULT
+	    || data_size > MSP_DATA_BITS_32) {
+		printk(KERN_ERR
+		       "invalid data size requested in configure_protocol()\n");
+		return -EINVAL;
+	}
+
+	switch (direction) {
+	case MSP_TRANSMIT_MODE:
+		set_transmit_protocol_descriptor(msp, protocol_desc, data_size);
+		break;
+	case MSP_RECEIVE_MODE:
+		set_receive_protocol_descriptor(msp, protocol_desc, data_size);
+		break;
+	case MSP_BOTH_T_R_MODE:
+		set_transmit_protocol_descriptor(msp, protocol_desc, data_size);
+		set_receive_protocol_descriptor(msp, protocol_desc, data_size);
+		break;
+	default:
+		printk(KERN_ERR "Invalid direction given\n");
+		return -EINVAL;
+	}
+	/* The below code is needed for both Rx and Tx path can't separate
+	 * them.
+	 */
+	temp_reg = stm_msp_read(msp->registers + MSP_GCR) & ~TX_CLK_POL_RISING;
+	temp_reg |= MSP_TX_CLKPOL_BIT(protocol_desc->tx_clock_pol);
+	stm_msp_write(temp_reg, msp->registers + MSP_GCR);
+	temp_reg = stm_msp_read(msp->registers + MSP_GCR) & ~RX_CLK_POL_RISING;
+	temp_reg |= MSP_RX_CLKPOL_BIT(protocol_desc->rx_clock_pol);
+	stm_msp_write(temp_reg, msp->registers + MSP_GCR);
+
+	return 0;
+}
+
+/**
+ * configure_clock - Set clock in sample rate generator.
+ * @msp: main msp controller structure.
+ * @config: configuration structure passed by client driver
+ *
+ * This will set the frame width and period. Also enable sample rate generator
+ *
+ * Returns error(-1) on failure else success(0).
+ */
+static int configure_clock(struct msp *msp,
+			   struct msp_config *config)
+{
+
+	u32 dummy;
+	u32 frame_per = 0;
+	u32 sck_div = 0;
+	u32 frame_width = 0;
+	u32 temp_reg = 0;
+	u32 bit_clock = 0;
+	struct msp_protocol_desc *protocol_desc = NULL;
+	stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) &
+		       (~(SRG_ENABLE))), msp->registers + MSP_GCR);
+
+	if (config->default_protocol_desc) {
+		protocol_desc =
+		    (struct msp_protocol_desc *)&protocol_desc_tab[config->
+								   protocol];
+	} else {
+		protocol_desc =
+		    (struct msp_protocol_desc *)&config->protocol_desc;
+	}
+
+	switch (config->protocol) {
+	case MSP_PCM_PROTOCOL:
+	case MSP_PCM_COMPAND_PROTOCOL:
+		frame_width = protocol_desc->frame_width;
+		sck_div =
+		    config->input_clock_freq / (config->frame_freq *
+						(protocol_desc->
+						 total_clocks_for_one_frame));
+		frame_per = protocol_desc->frame_period;
+		break;
+	case MSP_I2S_PROTOCOL:
+		frame_width = protocol_desc->frame_width;
+		sck_div =
+		    config->input_clock_freq / (config->frame_freq *
+						(protocol_desc->
+						 total_clocks_for_one_frame));
+		frame_per = protocol_desc->frame_period;
+
+		break;
+	case MSP_AC97_PROTOCOL:
+		/* Not supported */
+		printk(KERN_WARNING "AC97 protocol not supported\n");
+		return -ENOSYS;
+	default:
+		printk(KERN_ERR "Invalid mode attempted for setting clocks\n");
+		return -EINVAL;
+	}
+
+	temp_reg = (sck_div - 1) & SCK_DIV_MASK;
+	temp_reg |= FRAME_WIDTH_BITS(frame_width);
+	temp_reg |= FRAME_PERIOD_BITS(frame_per);
+	stm_msp_write(temp_reg, msp->registers + MSP_SRG);
+
+	/* Input clock frequency value configured is in MHz/1000 */
+	bit_clock = (config->input_clock_freq * 1000)/(sck_div + 1);
+
+	/* If the bit clock is higher than 19.2MHz, Vape should be run in 100% OPP */
+	/* Only consider OPP 100% when bit-clock is used, i.e. MSP master mode */
+	if ((bit_clock > 19200000) && ((config->tx_clock_sel != 0) || (config->rx_clock_sel != 0))) {
+		prcmu_qos_update_requirement(PRCMU_QOS_APE_OPP, "msp_i2s", 100);
+		msp->vape_opp_constraint = 1;
+	} else {
+		prcmu_qos_update_requirement(PRCMU_QOS_APE_OPP, "msp_i2s", 50);
+		msp->vape_opp_constraint = 0;
+	}
+
+	/* Wait a bit */
+	dummy = ((stm_msp_read(msp->registers + MSP_SRG)) >> FRWID_SHIFT) & 0x0000003F;
+
+	/* Enable clock */
+	stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) | ((SRG_ENABLE))),
+		      msp->registers + MSP_GCR);
+
+	/* Another wait */
+	dummy =
+	    ((stm_msp_read(msp->registers + MSP_SRG)) >> FRWID_SHIFT) &
+	    0x0000003F;
+	return 0;
+}
+
+/**
+ * configure_multichannel - Enable multichannel support for transmit & receive.
+ * @msp: main msp controller structure.
+ * @config: configuration structure passed by client driver
+ *
+ * This will enable multichannel support for transmit and receive.
+ * It will set Receive comparator also if configured.
+ *
+ * Returns error(-1) on failure else success(0).
+ */
+static int configure_multichannel(struct msp *msp,
+				  struct msp_config *config)
+{
+	struct msp_protocol_desc *protocol_desc;
+	struct msp_multichannel_config *mult_config;
+	if (config->default_protocol_desc == 1) {
+		if (config->protocol >= MSP_INVALID_PROTOCOL) {
+			printk(KERN_ERR
+			       "invalid protocol in configure_protocol()\n");
+			return -EINVAL;
+		}
+		protocol_desc =
+		    (struct msp_protocol_desc *)&protocol_desc_tab[config->
+								   protocol];
+	} else {
+		protocol_desc =
+		    (struct msp_protocol_desc *)&config->protocol_desc;
+	}
+	mult_config = &config->multichannel_config;
+	if (true == mult_config->tx_multichannel_enable) {
+		if (MSP_SINGLE_PHASE == protocol_desc->tx_phase_mode) {
+			stm_msp_write((stm_msp_read(msp->registers + MSP_MCR) |
+				       ((mult_config->
+					 tx_multichannel_enable << TMCEN_BIT) &
+					(0x0000020))),
+				      msp->registers + MSP_MCR);
+			stm_msp_write(mult_config->tx_channel_0_enable,
+				      msp->registers + MSP_TCE0);
+			stm_msp_write(mult_config->tx_channel_1_enable,
+				      msp->registers + MSP_TCE1);
+			stm_msp_write(mult_config->tx_channel_2_enable,
+				      msp->registers + MSP_TCE2);
+			stm_msp_write(mult_config->tx_channel_3_enable,
+				      msp->registers + MSP_TCE3);
+		} else {
+			printk(KERN_ERR "Not in authorised mode\n");
+			return -1;
+		}
+	}
+	if (true == mult_config->rx_multichannel_enable) {
+		if (MSP_SINGLE_PHASE == protocol_desc->rx_phase_mode) {
+			stm_msp_write((stm_msp_read(msp->registers + MSP_MCR) |
+				       ((mult_config->
+					 rx_multichannel_enable << RMCEN_BIT) &
+					(0x0000001))),
+				      msp->registers + MSP_MCR);
+			stm_msp_write(mult_config->rx_channel_0_enable,
+				      msp->registers + MSP_RCE0);
+			stm_msp_write(mult_config->rx_channel_1_enable,
+				      msp->registers + MSP_RCE1);
+			stm_msp_write(mult_config->rx_channel_2_enable,
+				      msp->registers + MSP_RCE2);
+			stm_msp_write(mult_config->rx_channel_3_enable,
+				      msp->registers + MSP_RCE3);
+		} else {
+			printk(KERN_ERR "Not in authorised mode\n");
+			return -1;
+		}
+		if (mult_config->rx_comparison_enable_mode) {
+			stm_msp_write((stm_msp_read(msp->registers + MSP_MCR) |
+				       ((mult_config->
+					 rx_comparison_enable_mode << RCMPM_BIT)
+					& (0x0000018))),
+				      msp->registers + MSP_MCR);
+
+			stm_msp_write(mult_config->comparison_mask,
+				      msp->registers + MSP_RCM);
+			stm_msp_write(mult_config->comparison_value,
+				      msp->registers + MSP_RCV);
+
+		}
+	}
+	return 0;
+
+}
+
+/**
+ * configure_dma - configure dma channel for transmit or receive.
+ * @msp: msp structure
+ * @config: configuration structure.
+ * Context: process
+ *
+ * It will configure dma channels and request them in Logical mode for both
+ * transmit and recevie modes.It also register the respective callback handlers
+ * for DMA.
+ *
+ * Returns void.
+ */
+void configure_dma(struct msp *msp, struct msp_config *config)
+{
+	struct stedma40_chan_cfg *rx_dma_info = msp->dma_cfg_rx;
+	struct stedma40_chan_cfg *tx_dma_info = msp->dma_cfg_tx;
+	dma_cap_mask_t mask;
+
+	if (config->direction == MSP_TRANSMIT_MODE
+	    || config->direction == MSP_BOTH_T_R_MODE) {
+
+		if (msp->tx_pipeid != NULL) {
+			dma_release_channel(msp->tx_pipeid);
+			msp->tx_pipeid = NULL;
+		}
+
+		if (config->data_size == MSP_DATA_BITS_32)
+			tx_dma_info->src_info.data_width = STEDMA40_WORD_WIDTH;
+		else if (config->data_size == MSP_DATA_BITS_16)
+			tx_dma_info->src_info.data_width
+				= STEDMA40_HALFWORD_WIDTH;
+		else if (config->data_size == MSP_DATA_BITS_8)
+			tx_dma_info->src_info.data_width
+				= STEDMA40_BYTE_WIDTH;
+		else
+			printk(KERN_ERR "Wrong data size\n");
+
+		if (config->data_size == MSP_DATA_BITS_32)
+			tx_dma_info->dst_info.data_width = STEDMA40_WORD_WIDTH;
+		else if (config->data_size == MSP_DATA_BITS_16)
+			tx_dma_info->dst_info.data_width
+				= STEDMA40_HALFWORD_WIDTH;
+		else if (config->data_size == MSP_DATA_BITS_8)
+			tx_dma_info->dst_info.data_width
+				= STEDMA40_BYTE_WIDTH;
+		else
+			printk(KERN_ERR "Wrong data size\n");
+
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+		msp->tx_pipeid = dma_request_channel(mask, stedma40_filter,
+						     tx_dma_info);
+	}
+	if (config->direction == MSP_RECEIVE_MODE
+	    || config->direction == MSP_BOTH_T_R_MODE) {
+
+		if (msp->rx_pipeid != NULL) {
+			dma_release_channel(msp->rx_pipeid);
+			msp->rx_pipeid = NULL;
+		}
+
+		if (config->data_size == MSP_DATA_BITS_32)
+			rx_dma_info->src_info.data_width = STEDMA40_WORD_WIDTH;
+		else if (config->data_size == MSP_DATA_BITS_16)
+			rx_dma_info->src_info.data_width
+				= STEDMA40_HALFWORD_WIDTH;
+		else if (config->data_size == MSP_DATA_BITS_8)
+			rx_dma_info->src_info.data_width = STEDMA40_BYTE_WIDTH;
+		else
+			printk(KERN_ERR "Wrong data size\n");
+
+		if (config->data_size == MSP_DATA_BITS_32)
+			rx_dma_info->dst_info.data_width = STEDMA40_WORD_WIDTH;
+		else if (config->data_size == MSP_DATA_BITS_16)
+			rx_dma_info->dst_info.data_width
+				= STEDMA40_HALFWORD_WIDTH;
+		else if (config->data_size == MSP_DATA_BITS_8)
+			rx_dma_info->dst_info.data_width = STEDMA40_BYTE_WIDTH;
+		else
+			printk(KERN_ERR "Wrong data size\n");
+
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+		msp->rx_pipeid = dma_request_channel(mask, stedma40_filter,
+						     rx_dma_info);
+	}
+
+}
+
+/**
+ * msp_enable - Setup the msp configuration.
+ * @msp: msp data contains main msp structure.
+ * @config: configuration structure sent by i2s client driver.
+ * Context: process
+ *
+ * Main msp configuring functions to configure msp in accordance with msp
+ * protocol descriptor, configuring msp clock,setup transfer mode selected by
+ * user like DMA, interrupt or polling and in the end enable RX and Tx path.
+ *
+ * Returns error(-1) in case of failure or success(0).
+ */
+static int msp_enable(struct msp *msp, struct msp_config *config)
+{
+	int status = 0;
+	int state;
+
+	/* Check msp state whether in RUN or CONFIGURED Mode */
+	state = msp->msp_state;
+	if (state == MSP_STATE_IDLE) {
+		if (msp->plat_init) {
+			status = msp->plat_init();
+			if (status) {
+				printk(KERN_ERR "Error in msp_i2s_init,"
+					" status is %d\n", status);
+				return status;
+			}
+		}
+	}
+
+	/* Configure msp with protocol dependent settings */
+	configure_protocol(msp, config);
+	configure_clock(msp, config);
+	if (config->multichannel_configured == 1) {
+		status = configure_multichannel(msp, config);
+		if (status)
+			printk(KERN_ERR "multichannel can't be configured\n");
+	}
+	msp->work_mode = config->work_mode;
+
+	if (msp->work_mode == MSP_DMA_MODE && !msp->dma_cfg_rx) {
+		switch (config->direction)  {
+		case MSP_RECEIVE_MODE:
+		case MSP_BOTH_T_R_MODE:
+			dev_err(&msp->i2s_cont->dev, "RX DMA not available");
+			return -EINVAL;
+		}
+	}
+
+	if (msp->work_mode == MSP_DMA_MODE && !msp->dma_cfg_tx) {
+		switch (config->direction)  {
+		case MSP_TRANSMIT_MODE:
+		case MSP_BOTH_T_R_MODE:
+			dev_err(&msp->i2s_cont->dev, "TX DMA not available");
+			return -EINVAL;
+		}
+	}
+
+	switch (config->direction) {
+	case MSP_TRANSMIT_MODE:
+		/*Currently they are ignored
+		   stm_msp_write((stm_msp_read(msp->registers + MSP_IMSC) |
+		   TRANSMIT_UNDERRUN_ERR_INT |
+		   TRANSMIT_FRAME_SYNC_ERR_INT),
+		   msp->registers + MSP_IMSC); */
+		if (config->work_mode == MSP_DMA_MODE) {
+			stm_msp_write(stm_msp_read(msp->registers + MSP_DMACR) |
+				      TX_DMA_ENABLE,
+				      msp->registers + MSP_DMACR);
+
+			msp->xfer_data.tx_handler = config->handler;
+			msp->xfer_data.tx_callback_data =
+			    config->tx_callback_data;
+			configure_dma(msp, config);
+		}
+		if (config->work_mode == MSP_POLLING_MODE) {
+			stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+				       (TX_ENABLE)), msp->registers + MSP_GCR);
+		}
+		if (msp->work_mode != MSP_DMA_MODE) {
+			switch (msp->actual_data_size) {
+			case MSP_DATA_BITS_8:
+				msp->write = u8_msp_write;
+				break;
+			case MSP_DATA_BITS_10:
+			case MSP_DATA_BITS_12:
+			case MSP_DATA_BITS_14:
+			case MSP_DATA_BITS_16:
+				msp->write = u16_msp_write;
+				break;
+			case MSP_DATA_BITS_20:
+			case MSP_DATA_BITS_24:
+			case MSP_DATA_BITS_32:
+			default:
+				msp->write = u32_msp_write;
+				break;
+			}
+			msp->xfer_data.tx_handler = config->handler;
+			msp->xfer_data.tx_callback_data =
+			    config->tx_callback_data;
+			msp->xfer_data.rx_callback_data =
+			    config->rx_callback_data;
+			msp->xfer_data.msp = msp;
+		}
+		break;
+	case MSP_RECEIVE_MODE:
+		/*Currently they are ignored
+		   stm_msp_write(stm_msp_read(msp->registers + MSP_IMSC) |
+		   RECEIVE_OVERRUN_ERROR_INT | RECEIVE_FRAME_SYNC_ERR_INT,
+		   msp->registers + MSP_IMSC); */
+		if (config->work_mode == MSP_DMA_MODE) {
+			stm_msp_write(stm_msp_read(msp->registers + MSP_DMACR) |
+				      RX_DMA_ENABLE,
+				      msp->registers + MSP_DMACR);
+
+			msp->xfer_data.rx_handler = config->handler;
+			msp->xfer_data.rx_callback_data =
+			    config->rx_callback_data;
+
+			configure_dma(msp, config);
+		}
+		if (config->work_mode == MSP_POLLING_MODE) {
+			stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+				       (RX_ENABLE)), msp->registers + MSP_GCR);
+		}
+		if (msp->work_mode != MSP_DMA_MODE) {
+			switch (msp->actual_data_size) {
+			case MSP_DATA_BITS_8:
+				msp->read = u8_msp_read;
+				break;
+			case MSP_DATA_BITS_10:
+			case MSP_DATA_BITS_12:
+			case MSP_DATA_BITS_14:
+			case MSP_DATA_BITS_16:
+				msp->read = u16_msp_read;
+				break;
+			case MSP_DATA_BITS_20:
+			case MSP_DATA_BITS_24:
+			case MSP_DATA_BITS_32:
+			default:
+				msp->read = u32_msp_read;
+				break;
+			}
+			msp->xfer_data.rx_handler = config->handler;
+			msp->xfer_data.tx_callback_data =
+			    config->tx_callback_data;
+			msp->xfer_data.rx_callback_data =
+			    config->rx_callback_data;
+			msp->xfer_data.msp = msp;
+		}
+
+		break;
+	case MSP_BOTH_T_R_MODE:
+		/*Currently they are ignored
+		   stm_msp_write(stm_msp_read(msp->registers + MSP_IMSC) |
+		   RECEIVE_OVERRUN_ERROR_INT | RECEIVE_FRAME_SYNC_ERR_INT |
+		   TRANSMIT_UNDERRUN_ERR_INT | TRANSMIT_FRAME_SYNC_ERR_INT ,
+		   msp->registers + MSP_IMSC); */
+		if (config->work_mode == MSP_DMA_MODE) {
+			stm_msp_write(stm_msp_read(msp->registers + MSP_DMACR) |
+				      RX_DMA_ENABLE | TX_DMA_ENABLE,
+				      msp->registers + MSP_DMACR);
+
+			msp->xfer_data.tx_handler = config->handler;
+			msp->xfer_data.rx_handler = config->handler;
+			msp->xfer_data.tx_callback_data =
+			    config->tx_callback_data;
+			msp->xfer_data.rx_callback_data =
+			    config->rx_callback_data;
+
+			configure_dma(msp, config);
+		}
+		if (config->work_mode == MSP_POLLING_MODE) {
+			stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+				       (TX_ENABLE)), msp->registers + MSP_GCR);
+			stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+				       (RX_ENABLE)), msp->registers + MSP_GCR);
+		}
+		if (msp->work_mode != MSP_DMA_MODE) {
+			switch (msp->actual_data_size) {
+			case MSP_DATA_BITS_8:
+				msp->read = u8_msp_read;
+				msp->write = u8_msp_write;
+				break;
+			case MSP_DATA_BITS_10:
+			case MSP_DATA_BITS_12:
+			case MSP_DATA_BITS_14:
+			case MSP_DATA_BITS_16:
+				msp->read = u16_msp_read;
+				msp->write = u16_msp_write;
+				break;
+			case MSP_DATA_BITS_20:
+			case MSP_DATA_BITS_24:
+			case MSP_DATA_BITS_32:
+			default:
+				msp->read = u32_msp_read;
+				msp->write = u32_msp_write;
+				break;
+			}
+			msp->xfer_data.tx_handler = config->handler;
+			msp->xfer_data.rx_handler = config->handler;
+			msp->xfer_data.tx_callback_data =
+			    config->tx_callback_data;
+			msp->xfer_data.rx_callback_data =
+			    config->rx_callback_data;
+			msp->xfer_data.msp = msp;
+		}
+
+		break;
+	default:
+		printk(KERN_ERR "Invalid direction parameter\n");
+		if (msp->plat_exit)
+			msp->plat_exit();
+		status = -EINVAL;
+		return status;
+	}
+
+	switch (config->work_mode) {
+	case MSP_DMA_MODE:
+		msp->transfer = msp_dma_xfer;
+		break;
+	case MSP_POLLING_MODE:
+		msp->transfer = msp_polling_xfer;
+		break;
+	case MSP_INTERRUPT_MODE:
+		msp->transfer = msp_interrupt_xfer;
+		break;
+	default:
+		msp->transfer = NULL;
+	}
+
+	stm_msp_write(config->iodelay, msp->registers + MSP_IODLY);
+
+	/* enable frame generation logic */
+	stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+		       (FRAME_GEN_ENABLE)), msp->registers + MSP_GCR);
+
+	return status;
+}
+
+/**
+ * flush_rx_fifo - Flush Rx fifo MSP controller.
+ * @msp: msp structure.
+ *
+ * This function flush the rx fifo of msp controller.
+ *
+ * Returns error(-1) in case of failure else success(0)
+ */
+static void flush_rx_fifo(struct msp *msp)
+{
+	u32 dummy = 0;
+	u32 limit = 32;
+	u32 cur = stm_msp_read(msp->registers + MSP_GCR);
+	stm_msp_write(cur | RX_ENABLE, msp->registers + MSP_GCR);
+	while (!(stm_msp_read(msp->registers + MSP_FLR) & RX_FIFO_EMPTY)
+	       && limit--) {
+		dummy = stm_msp_read(msp->registers + MSP_DR);
+	}
+	stm_msp_write(cur, msp->registers + MSP_GCR);
+}
+
+/**
+ * flush_tx_fifo - Flush Tx fifo MSP controller.
+ * @msp: msp structure.
+ *
+ * This function flush the tx fifo using test intergration register to read data
+ * from tx fifo directly.
+ *
+ * Returns error(-1) in case of failure else success(0)
+ */
+static void flush_tx_fifo(struct msp *msp)
+{
+	u32 dummy = 0;
+	u32 limit = 32;
+	u32 cur = stm_msp_read(msp->registers + MSP_GCR);
+	stm_msp_write(cur | TX_ENABLE, msp->registers + MSP_GCR);
+	stm_msp_write(0x3, msp->registers + MSP_ITCR);
+	while (!(stm_msp_read(msp->registers + MSP_FLR) & TX_FIFO_EMPTY)
+	       && limit--) {
+		dummy = stm_msp_read(msp->registers + MSP_TSTDR);
+	}
+	stm_msp_write(0x0, msp->registers + MSP_ITCR);
+	stm_msp_write(cur, msp->registers + MSP_GCR);
+}
+
+/**
+ * stm_msp_configure_enable - configures and enables the MSP controller.
+ * @i2s_cont: i2s controller sent by i2s device.
+ * @configuration: specifies the configuration parameters.
+ *
+ * This function configures the msp controller with the client configuration.
+ *
+ * Returns error(-1) in case of failure else success(0)
+ */
+static int stm_msp_configure_enable(struct i2s_controller *i2s_cont,
+				    void *configuration)
+{
+	u32 old_reg;
+	u32 new_reg;
+	u32 mask;
+	int res;
+	struct msp_config *config =
+	    (struct msp_config *)configuration;
+	struct msp *msp = (struct msp *)i2s_cont->data;
+
+	if (in_interrupt()) {
+		printk(KERN_ERR
+		       "can't call configure_enable in interrupt context\n");
+		return -1;
+	}
+
+	/* Two simultanous configuring msp is avoidable */
+	down(&msp->lock);
+
+	/* Don't enable regulator if its msp1 or msp3 */
+	if (!(msp->reg_enabled) && msp->id != MSP_1_I2S_CONTROLLER
+				&& msp->id != MSP_3_I2S_CONTROLLER) {
+		res = regulator_enable(msp_vape_supply);
+		if (res != 0) {
+			dev_err(&msp->i2s_cont->dev,
+					"Failed to enable regulator\n");
+			up(&msp->lock);
+			return res;
+		}
+		msp->reg_enabled = 1;
+	}
+
+	switch (msp->users) {
+	case 0:
+		clk_enable(msp->clk);
+		msp->direction = config->direction;
+		break;
+	case 1:
+		if (msp->direction == MSP_BOTH_T_R_MODE ||
+		    config->direction == msp->direction ||
+		    config->direction == MSP_BOTH_T_R_MODE) {
+			dev_notice(&i2s_cont->dev, "%s: MSP in use in the "
+				   "desired direction.\n", __func__);
+			up(&msp->lock);
+			return -EBUSY;
+		}
+		msp->direction = MSP_BOTH_T_R_MODE;
+		break;
+	default:
+		dev_notice(&i2s_cont->dev, "%s: MSP in use in both "
+			   "directions.\n", __func__);
+		up(&msp->lock);
+		return -EBUSY;
+	}
+	msp->users++;
+
+	/* First do the global config register */
+	mask =
+	    RX_CLK_SEL_MASK | TX_CLK_SEL_MASK | RX_FRAME_SYNC_MASK |
+	    TX_FRAME_SYNC_MASK | RX_SYNC_SEL_MASK | TX_SYNC_SEL_MASK |
+	    RX_FIFO_ENABLE_MASK | TX_FIFO_ENABLE_MASK | SRG_CLK_SEL_MASK |
+	    LOOPBACK_MASK | TX_EXTRA_DELAY_MASK;
+
+	new_reg =
+	    (config->tx_clock_sel | config->rx_clock_sel | config->
+	     rx_frame_sync_pol | config->tx_frame_sync_pol | config->
+	     rx_frame_sync_sel | config->tx_frame_sync_sel | config->
+	     rx_fifo_config | config->tx_fifo_config | config->
+	     srg_clock_sel | config->loopback_enable | config->tx_data_enable);
+
+	old_reg = stm_msp_read(msp->registers + MSP_GCR);
+	old_reg &= ~mask;
+	new_reg |= old_reg;
+	stm_msp_write(new_reg, msp->registers + MSP_GCR);
+
+	if (msp_enable(msp, config) != 0) {
+		printk(KERN_ERR "error enabling MSP\n");
+		return -EBUSY;
+	}
+	if (config->loopback_enable & 0x80)
+		msp->loopback_enable = 1;
+	/*Sometimes FIFO doesn't gets empty hence limit is provided */
+	flush_tx_fifo(msp);
+	/*This has been added in order to fix fifo flush problem
+	   When last xfer occurs some data remains in fifo. In order to
+	   flush that data delay is needed */
+	msleep(10);
+	/* wait for fifo to flush */
+	flush_rx_fifo(msp);
+
+	/* RX_BUSY take a while to clear */
+	msleep(10);
+
+	msp->msp_state = MSP_STATE_CONFIGURED;
+	up(&msp->lock);
+	return 0;
+}
+
+static int msp_start_dma(struct msp *msp, int transmit, dma_addr_t data,
+			 size_t bytes)
+{
+	struct dma_async_tx_descriptor *desc;
+	struct scatterlist sg;
+
+	sg_init_table(&sg, 1);
+	sg_set_page(&sg, pfn_to_page(PFN_DOWN(data)), bytes,
+		    offset_in_page(data));
+	sg_dma_address(&sg) = data;
+	sg_dma_len(&sg) = bytes;
+
+	if (transmit) {
+		if (!msp->tx_pipeid)
+			return -EINVAL;
+		desc = msp->tx_pipeid->device->
+				device_prep_slave_sg(msp->tx_pipeid,
+						&sg, 1, DMA_TO_DEVICE,
+						DMA_PREP_INTERRUPT
+						| DMA_CTRL_ACK);
+		if (!desc)
+			return -ENOMEM;
+
+		desc->callback = msp->xfer_data.tx_handler;
+		desc->callback_param = msp->xfer_data.tx_callback_data;
+		desc->tx_submit(desc);
+		dma_async_issue_pending(msp->tx_pipeid);
+	} else {
+		if (!msp->rx_pipeid)
+			return -EINVAL;
+
+		desc = msp->rx_pipeid->device->
+				device_prep_slave_sg(msp->rx_pipeid,
+						&sg, 1, DMA_FROM_DEVICE,
+						DMA_PREP_INTERRUPT
+						| DMA_CTRL_ACK);
+		if (!desc)
+			return -EBUSY;
+
+		desc->callback = msp->xfer_data.rx_handler;
+		desc->callback_param = msp->xfer_data.rx_callback_data;
+		desc->tx_submit(desc);
+		dma_async_issue_pending(msp->rx_pipeid);
+	}
+
+	return 0;
+}
+
+static int msp_single_dma_tx(struct msp *msp, dma_addr_t data, size_t bytes)
+{
+	int status;
+	status = msp_start_dma(msp, 1, data, bytes);
+	stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) | (TX_ENABLE)),
+		msp->registers + MSP_GCR);
+	return status;
+}
+
+static int msp_single_dma_rx(struct msp *msp, dma_addr_t data, size_t bytes)
+{
+	int status;
+	status = msp_start_dma(msp, 0, data, bytes);
+	stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) | (RX_ENABLE)),
+		msp->registers + MSP_GCR);
+	return status;
+}
+
+static void msp_cyclic_dma_start(struct msp *msp,
+				dma_addr_t buf_addr,
+				size_t buf_len,
+				size_t period_len,
+				enum dma_data_direction direction)
+{
+	int ret;
+	struct dma_async_tx_descriptor *cdesc;
+	struct dma_chan *pipeid = (direction == DMA_TO_DEVICE) ?
+				msp->tx_pipeid :
+				msp->rx_pipeid;
+
+	pr_debug("%s: buf_addr = %p\n", __func__, (void *) buf_addr);
+	pr_debug("%s: buf_len = %d\n", __func__, buf_len);
+	pr_debug("%s: perios_len = %d\n", __func__, period_len);
+
+	/* setup the cyclic description */
+	cdesc = pipeid->device->device_prep_dma_cyclic(pipeid,
+			buf_addr, /* reuse the sq list for the moment */
+			buf_len,
+			period_len,
+			direction);
+
+	if (IS_ERR(cdesc)) {
+		pr_err("%s: Error: device_prep_dma_cyclic failed (%ld)!\n",
+			__func__,
+			PTR_ERR(cdesc));
+		return;
+	}
+
+	cdesc->callback = (direction == DMA_TO_DEVICE) ?
+				msp->xfer_data.tx_handler :
+				msp->xfer_data.rx_handler;
+	cdesc->callback_param = (direction == DMA_TO_DEVICE) ?
+				msp->xfer_data.tx_callback_data :
+				msp->xfer_data.rx_callback_data;
+
+	/* submit to the dma */
+	ret = dmaengine_submit(cdesc);
+
+	/* start the dma */
+	dma_async_issue_pending(pipeid);
+
+	return;
+}
+
+/* Legacy function. Used by HATS driver. */
+static void msp_loopback_inf_start_dma(struct msp *msp,
+				dma_addr_t data,
+				size_t bytes)
+{
+#if 0
+	struct stedma40_cyclic_desc *rxcdesc;
+	struct stedma40_cyclic_desc *txcdesc;
+	struct scatterlist rxsg[2];
+	struct scatterlist txsg[2];
+	size_t len = bytes >> 1;
+	int ret;
+
+	sg_init_table(rxsg, ARRAY_SIZE(rxsg));
+	sg_init_table(txsg, ARRAY_SIZE(txsg));
+
+	sg_dma_len(&rxsg[0]) = len;
+	sg_dma_len(&rxsg[1]) = len;
+	sg_dma_len(&txsg[0]) = len;
+	sg_dma_len(&txsg[1]) = len;
+
+	sg_dma_address(&rxsg[0]) = data;
+	sg_dma_address(&rxsg[1]) = data + len;
+
+	sg_dma_address(&txsg[0]) = data + len;
+	sg_dma_address(&txsg[1]) = data;
+
+	rxcdesc = stedma40_cyclic_prep_sg(msp->rx_pipeid,
+					  rxsg, ARRAY_SIZE(rxsg),
+					  DMA_FROM_DEVICE, 0);
+	if (IS_ERR(rxcdesc))
+		return;
+
+	txcdesc = stedma40_cyclic_prep_sg(msp->tx_pipeid,
+					  txsg, ARRAY_SIZE(txsg),
+					  DMA_TO_DEVICE, 0);
+	if (IS_ERR(txcdesc))
+		goto free_rx;
+
+	ret = stedma40_cyclic_start(msp->rx_pipeid);
+	if (ret)
+		goto free_tx;
+
+	ret = stedma40_cyclic_start(msp->tx_pipeid);
+	if (ret)
+		goto stop_rx;
+
+	msp->infinite = true;
+
+	return;
+
+stop_rx:
+	stedma40_cyclic_stop(msp->rx_pipeid);
+free_tx:
+	stedma40_cyclic_free(msp->tx_pipeid);
+free_rx:
+	stedma40_cyclic_free(msp->rx_pipeid);
+#endif
+  return;
+}
+
+/**
+ * msp_dma_xfer - Handles DMA transfers over i2s bus.
+ * @msp: main msp structure.
+ * @msg: i2s_message contains info about transmit and receive data.
+ * Context: process
+ *
+ * This will first check whether data buffer is dmaable or not.
+ * Call dma_map_single apis etc to make it dmaable dma. Starts the dma transfer
+ * for TX and RX parallely and wait for it to get completed.
+ *
+ * Returns error(-1) in case of failure or success(0).
+ */
+static int msp_dma_xfer(struct msp *msp, struct i2s_message *msg)
+{
+	int status = 0;
+
+	switch (msg->i2s_transfer_mode) {
+	default:
+	case I2S_TRANSFER_MODE_SINGLE_DMA:
+		if (msg->i2s_direction == I2S_DIRECTION_RX ||
+				msg->i2s_direction == I2S_DIRECTION_BOTH)
+			if (msg->rxdata && (msg->rxbytes > 0)) {
+				if (!msg->dma_flag)
+					msg->rxdata =
+						(void *)dma_map_single(NULL,
+								msg->rxdata,
+								msg->rxbytes,
+								DMA_FROM_DEVICE
+								);
+				status = msp_single_dma_rx(msp,
+						(dma_addr_t)msg->rxdata,
+						msg->rxbytes);
+			}
+		if (msg->i2s_direction == I2S_DIRECTION_TX ||
+				msg->i2s_direction == I2S_DIRECTION_BOTH)
+			if (msg->txdata && (msg->txbytes > 0)) {
+				if (!msg->dma_flag)
+					msg->txdata =
+						(void *)dma_map_single(NULL,
+								msg->txdata,
+								msg->txbytes,
+								DMA_TO_DEVICE);
+				status = msp_single_dma_tx(msp,
+						(dma_addr_t)msg->txdata,
+						msg->txbytes);
+			}
+		break;
+
+	case I2S_TRANSFER_MODE_CYCLIC_DMA:
+		if (msg->i2s_direction == I2S_DIRECTION_TX) {
+			msp_cyclic_dma_start(msp,
+					msg->buf_addr,
+					msg->buf_len,
+					msg->period_len,
+					DMA_TO_DEVICE);
+			stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+				(TX_ENABLE)),
+				msp->registers + MSP_GCR);
+		} else {
+			msp_cyclic_dma_start(msp,
+					msg->buf_addr,
+					msg->buf_len,
+					msg->period_len,
+					DMA_FROM_DEVICE);
+			stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+				(RX_ENABLE)),
+				msp->registers + MSP_GCR);
+		}
+		break;
+
+	case I2S_TRANSFER_MODE_INF_LOOPBACK:
+		msp_loopback_inf_start_dma(msp,
+					(dma_addr_t)msg->rxdata,
+					msg->rxbytes);
+		stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+			       (RX_ENABLE)),
+				msp->registers + MSP_GCR);
+		stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+			       (TX_ENABLE)),
+				msp->registers + MSP_GCR);
+		break;
+	}
+
+	return status;
+}
+
+#if 0
+/**
+ * msp_handle_irq - Interrupt handler routine.
+ * @irq: irq no.
+ * @dev_id: device structure registered in request irq.
+ *
+ * Returns error(-1) on failure else success(0).
+ */
+static irqreturn_t msp_handle_irq(int irq, void *dev_id)
+{
+	u32 irq_status;
+	struct msp *msp = (struct msp *)dev_id;
+	struct i2s_message *message = &msp->xfer_data.message;
+	u32 irq_mask = 0;
+	irq_status = stm_msp_read(msp->registers + MSP_MIS);
+	irq_mask = stm_msp_read(msp->registers + MSP_IMSC);
+/* Disable the interrupt to prevent immediate recurrence */
+	stm_msp_write(stm_msp_read(msp->registers + MSP_IMSC) & ~irq_status,
+		      msp->registers + MSP_IMSC);
+
+/* Clear the interrupt */
+	stm_msp_write(irq_status, msp->registers + MSP_ICR);
+/* Check for an error condition */
+	msp->msp_io_error = irq_status & (RECEIVE_OVERRUN_ERROR_INT |
+					  RECEIVE_FRAME_SYNC_ERR_INT |
+					  TRANSMIT_UNDERRUN_ERR_INT |
+					  TRANSMIT_FRAME_SYNC_ERR_INT);
+
+	 /*Currently they are ignored */
+	if (irq_status & RECEIVE_OVERRUN_ERROR_INT)
+		;
+	if (irq_status & TRANSMIT_UNDERRUN_ERR_INT)
+		;
+
+	/* This code has been added basically to support loopback mode
+	 * Basically Transmit interrupt is not disabled even after its
+	 * completion so that receive fifo gets an additional interrupt
+	 */
+	if (irq_mask & (RECEIVE_SERVICE_INT)
+	    && (irq_mask & (TRANSMIT_SERVICE_INT)) && (msp->loopback_enable)) {
+		if (msp->read)
+			msp->read(&msp->xfer_data);
+		if (msp->write)
+			msp->write(&msp->xfer_data);
+		if (message->rx_offset >= message->rxbytes) {
+			if (msp->xfer_data.rx_handler)
+				msp->xfer_data.rx_handler(msp->
+							  xfer_data.
+							  rx_callback_data,
+							  message->rx_offset);
+			msp->xfer_data.rx_handler = NULL;
+			return IRQ_HANDLED;
+		}
+
+		if (message->tx_offset >= message->txbytes) {
+			if (msp->xfer_data.tx_handler)
+				msp->xfer_data.tx_handler(msp->xfer_data.
+							  tx_callback_data,
+							  message->tx_offset);
+			msp->xfer_data.tx_handler = NULL;
+		}
+		stm_msp_write(irq_mask, msp->registers + MSP_IMSC);
+		return IRQ_HANDLED;
+	}
+
+	if (irq_status & RECEIVE_SERVICE_INT) {
+		if (msp->read)
+			msp->read(&msp->xfer_data);
+		if (message->rx_offset >= message->rxbytes) {
+			irq_mask &= ~RECEIVE_SERVICE_INT;
+			stm_msp_write(irq_mask, msp->registers + MSP_IMSC);
+			if (msp->xfer_data.rx_handler)
+				msp->xfer_data.rx_handler(msp->
+							  xfer_data.
+							  rx_callback_data,
+							  message->rx_offset);
+			if (!(irq_status & TRANSMIT_SERVICE_INT))
+				return IRQ_HANDLED;
+		}
+	}
+	if (irq_status & TRANSMIT_SERVICE_INT) {
+		if (msp->write)
+			msp->write(&msp->xfer_data);
+		if (message->tx_offset >= message->txbytes) {
+			irq_mask &= ~TRANSMIT_SERVICE_INT;
+			stm_msp_write(irq_mask, msp->registers + MSP_IMSC);
+			if (msp->xfer_data.tx_handler)
+				msp->xfer_data.tx_handler(msp->xfer_data.
+							  tx_callback_data,
+							  message->tx_offset);
+			return IRQ_HANDLED;
+		}
+	}
+	stm_msp_write(irq_mask, msp->registers + MSP_IMSC);
+	return IRQ_HANDLED;
+
+}
+#endif
+
+/**
+ * msp_interrupt_xfer - Handles Interrupt transfers over i2s bus.
+ * @msp: main msp structure.
+ * @msg: i2s_message contains info about transmit and receive data.
+ * Context: Process or interrupt.
+ *
+ * This implements transfer and receive functions used in interrupt mode.
+ * This can be used in interrupt context if a callback handler is registered
+ * by client driver. This has been to improve performance in interrupt mode.
+ * Hence can't use sleep in this function.
+ *
+ * Returns error(-1) in case of failure or success(0).
+ */
+static int msp_interrupt_xfer(struct msp *msp, struct i2s_message *msg)
+{
+	struct i2s_message *message;
+	u32 irq_mask = 0;
+
+	if (msg->i2s_transfer_mode != I2S_TRANSFER_MODE_NON_DMA)
+		return -EINVAL;
+
+	if (msg->txbytes) {
+		msp->xfer_data.message.txbytes = msg->txbytes;
+		msp->xfer_data.message.txdata = msg->txdata;
+		msp->xfer_data.message.tx_offset = 0;
+	}
+	if (msg->rxbytes) {
+		msp->xfer_data.message.rxbytes = msg->rxbytes;
+		msp->xfer_data.message.rxdata = msg->rxdata;
+		msp->xfer_data.message.rx_offset = 0;
+	}
+	message = &msp->xfer_data.message;
+	if ((message->txdata == NULL || message->txbytes == 0)
+	    && (message->rxdata == NULL || message->rxbytes == 0)) {
+		printk(KERN_ERR
+		       "transmit_receive_data is NULL with bytes > 0\n");
+		return -EINVAL;
+	}
+
+	msp->msp_io_error = 0;
+
+	if (message->tx_offset < message->txbytes) {
+		irq_mask |= TRANSMIT_SERVICE_INT;
+		stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+			       (TX_ENABLE)), msp->registers + MSP_GCR);
+	}
+	if (message->rx_offset < message->rxbytes) {
+		irq_mask |= RECEIVE_SERVICE_INT;
+		stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+			       (RX_ENABLE)), msp->registers + MSP_GCR);
+	}
+	stm_msp_write((stm_msp_read(msp->registers + MSP_IMSC) |
+		       irq_mask), msp->registers + MSP_IMSC);
+	return 0;
+}
+
+/**
+ * func_notify_timer - Handles Polling hang issue over i2s bus.
+ * @data: main msp data address
+ * Context: Interrupt.
+ *
+ * This is  used to handle error condition in transfer and receive function used
+ * in polling mode.
+ * Sometimes due to passing wrong protocol desc , polling transfer may hang.
+ * To prevent this, timer is added.
+ *
+ * Returns void.
+ */
+static void func_notify_timer(unsigned long data)
+{
+	struct msp *msp = (struct msp *)data;
+	if (msp->polling_flag) {
+		msp->msp_io_error = 1;
+		printk(KERN_ERR
+		       "Polling is taking two much time, may be it got hang\n");
+		del_timer(&msp->notify_timer);
+	}
+}
+
+/**
+ * msp_polling_xfer - Handles Polling transfers over i2s bus.
+ * @msp: main msp structure.
+ * @msg: i2s_message contains info about transmit and receive data.
+ * Context: Process.
+ *
+ * This implements transfer and receive functions used in polling mode. This is
+ * blocking fucntion.
+ * It is recommended to use interrupt or dma mode for better performance rather
+ * than the polling mode.
+ *
+ * Returns error(-1) in case of failure or success(0).
+ */
+static int msp_polling_xfer(struct msp *msp, struct i2s_message *msg)
+{
+	struct i2s_message *message;
+	u32 time_expire = 0;
+	u32 tr_ex = 0, rr_ex = 0;
+	u32 msec_jiffies = 0;
+
+	if (msg->i2s_transfer_mode != I2S_TRANSFER_MODE_NON_DMA)
+		return -EINVAL;
+
+	if (msg->txbytes) {
+		msp->xfer_data.message.txbytes = msg->txbytes;
+		msp->xfer_data.message.txdata = msg->txdata;
+		msp->xfer_data.message.tx_offset = 0;
+		tr_ex = msg->txbytes;
+	}
+	if (msg->rxbytes) {
+		msp->xfer_data.message.rxbytes = msg->rxbytes;
+		msp->xfer_data.message.rxdata = msg->rxdata;
+		msp->xfer_data.message.rx_offset = 0;
+		rr_ex = msg->rxbytes;
+	}
+	message = &msp->xfer_data.message;
+	time_expire = (tr_ex + rr_ex) / 1024;
+	if (!time_expire)
+		msec_jiffies = 500;
+	else
+		msec_jiffies = time_expire * 500;
+	msp->notify_timer.expires = jiffies + msecs_to_jiffies(msec_jiffies);
+	down(&msp->lock);
+	if (message->txdata == NULL && message->txbytes > 0) {
+		printk(KERN_ERR
+		       "transmit_receive_data is NULL with bytes > 0\n");
+		return -EINVAL;
+	}
+
+	if (message->rxdata == NULL && message->rxbytes > 0) {
+		printk(KERN_ERR
+		       "transmit_receive_data is NULL with bytes > 0\n");
+		return -EINVAL;
+	}
+	msp->msp_io_error = 0;
+	msp->polling_flag = 1;
+	add_timer(&msp->notify_timer);
+	while (message->tx_offset < message->txbytes
+	       || message->rx_offset < message->rxbytes) {
+		if (msp->msp_io_error)
+			break;
+		if (msp->read)
+			msp->read(&msp->xfer_data);
+		if (msp->write)
+			msp->write(&msp->xfer_data);
+	}
+	msp->polling_flag = 0;
+	del_timer(&msp->notify_timer);
+	up(&msp->lock);
+	return message->txbytes + message->rxbytes;
+}
+
+/**
+ * stm_msp_transceive_data - Main i2s transfer function.
+ * @i2s_cont: i2s controller structure passed by client driver.
+ * @message: i2s message structure contains transceive info.
+ * Context: process or interrupt.
+ *
+ * This function is registered over i2s_xfer funtions. It will handle main i2s
+ * transfer over i2s bus in various modes.It call msp transfer function on which
+ * suitable transfer function is already registered i.e dma ,interrupt or
+ * polling function.
+ *
+ * Returns error(-1) in case of failure or success(0).
+ */
+static int stm_msp_transceive_data(struct i2s_controller *i2s_cont,
+				   struct i2s_message *message)
+{
+	int status = 0;
+	struct msp *msp = (struct msp *)i2s_cont->data;
+
+	if (!message || (msp->msp_state == MSP_STATE_IDLE)) {
+		printk(KERN_ERR "Message is NULL\n");
+		return -EPERM;
+	}
+
+	msp->msp_state = MSP_STATE_RUN;
+	if (msp->transfer)
+		status = msp->transfer(msp, message);
+
+	if (msp->msp_state == MSP_STATE_RUN)
+		msp->msp_state = MSP_STATE_CONFIGURED;
+
+	return status;
+}
+
+/**
+ * msp_disable_receive - Disable receive functionality.
+ * @msp: main msp structure.
+ * Context: process.
+ *
+ * This function will disable msp controller's receive functionality like dma,
+ * interrupt receive data buffer all are disabled.
+ *
+ * Returns void.
+ */
+static void msp_disable_receive(struct msp *msp)
+{
+	stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) &
+		       (~RX_ENABLE)), msp->registers + MSP_GCR);
+	stm_msp_write((stm_msp_read(msp->registers + MSP_DMACR) &
+		       (~RX_DMA_ENABLE)), msp->registers + MSP_DMACR);
+	stm_msp_write((stm_msp_read(msp->registers + MSP_IMSC) &
+		       (~
+			(RECEIVE_SERVICE_INT |
+			 RECEIVE_OVERRUN_ERROR_INT))),
+		      msp->registers + MSP_IMSC);
+	msp->xfer_data.message.rxbytes = 0;
+	msp->xfer_data.message.rx_offset = 0;
+	msp->xfer_data.message.rxdata = NULL;
+	msp->read = NULL;
+
+}
+
+/**
+ * msp_disable_transmit - Disable transmit functionality.
+ * @msp: main msp structure.
+ * Context: process.
+ *
+ * This function will disable msp controller's transmit functionality like dma,
+ * interrupt transmit data buffer all are disabled.
+ *
+ * Returns void.
+ */
+static void msp_disable_transmit(struct msp *msp)
+{
+
+	stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) &
+		       (~TX_ENABLE)), msp->registers + MSP_GCR);
+	stm_msp_write((stm_msp_read(msp->registers + MSP_DMACR) &
+		       (~TX_DMA_ENABLE)), msp->registers + MSP_DMACR);
+	stm_msp_write((stm_msp_read(msp->registers + MSP_IMSC) &
+		       (~
+			(TRANSMIT_SERVICE_INT |
+			 TRANSMIT_UNDERRUN_ERR_INT))),
+		      msp->registers + MSP_IMSC);
+	msp->xfer_data.message.txbytes = 0;
+	msp->xfer_data.message.tx_offset = 0;
+	msp->xfer_data.message.txdata = NULL;
+	msp->write = NULL;
+
+}
+
+/**
+ * stm_msp_disable - disable the given msp controller
+ * @msp: specifies the msp contoller data
+ * @direction: specifies the transmit/receive direction
+ * @flag: It indicates the functionality that needs to be disabled.
+ *
+ * Returns error(-1) in case of failure else success(0)
+ */
+static int stm_msp_disable(struct msp *msp, int direction, i2s_flag flag)
+{
+	int limit = 32;
+	u32 dummy = 0;
+	int status = 0;
+	if (!
+	    (stm_msp_read(msp->registers + MSP_GCR) &
+	     ((TX_ENABLE | RX_ENABLE)))) {
+		return 0;
+	}
+	if (msp->work_mode == MSP_DMA_MODE) {
+		if (flag == DISABLE_ALL || flag == DISABLE_TRANSMIT) {
+			if (msp->tx_pipeid != NULL) {
+				dmaengine_terminate_all(msp->tx_pipeid);
+				dma_release_channel(msp->tx_pipeid);
+				msp->tx_pipeid = NULL;
+			}
+		}
+		if ((flag == DISABLE_ALL || flag == DISABLE_RECEIVE)) {
+			if (msp->rx_pipeid != NULL) {
+				dmaengine_terminate_all(msp->rx_pipeid);
+				dma_release_channel(msp->rx_pipeid);
+				msp->rx_pipeid = NULL;
+			}
+		}
+	}
+	if (flag == DISABLE_TRANSMIT)
+		msp_disable_transmit(msp);
+	else if (flag == DISABLE_RECEIVE)
+		msp_disable_receive(msp);
+	else {
+		stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) |
+			       (LOOPBACK_MASK)), msp->registers + MSP_GCR);
+		/* Flush Tx fifo */
+		while ((!
+			(stm_msp_read(msp->registers + MSP_FLR) &
+			 TX_FIFO_EMPTY)) && limit--)
+			dummy = stm_msp_read(msp->registers + MSP_DR);
+
+		/* Disable Transmit channel */
+		stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) &
+			       (~TX_ENABLE)), msp->registers + MSP_GCR);
+		limit = 32;
+		/* Flush Rx Fifo */
+		while ((!
+			(stm_msp_read(msp->registers + MSP_FLR) &
+			 RX_FIFO_EMPTY)) && limit--)
+			dummy = stm_msp_read(msp->registers + MSP_DR);
+		/* Disable Loopback and Receive channel */
+		stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) &
+			       (~(RX_ENABLE | LOOPBACK_MASK))),
+			      msp->registers + MSP_GCR);
+		/*This has been added in order to fix fifo flush problem
+		   When last xfer occurs some data remains in fifo. In order to
+		   flush that data delay is needed */
+		msleep(10);
+		msp_disable_transmit(msp);
+		msp_disable_receive(msp);
+
+	}
+
+	/* disable sample rate and frame generators */
+	if (flag == DISABLE_ALL) {
+		msp->msp_state = MSP_STATE_IDLE;
+		stm_msp_write((stm_msp_read(msp->registers + MSP_GCR) &
+			       (~(FRAME_GEN_ENABLE | SRG_ENABLE))),
+			      msp->registers + MSP_GCR);
+		memset(&msp->xfer_data, 0, sizeof(struct trans_data));
+		if (msp->plat_exit)
+			status = msp->plat_exit();
+			if (status)
+				printk(KERN_ERR "Error in msp_i2s_exit\n");
+		if (msp->work_mode == MSP_POLLING_MODE
+		    && msp->msp_state == MSP_STATE_RUN) {
+			up(&msp->lock);
+		}
+		msp->transfer = NULL;
+		stm_msp_write(0, msp->registers + MSP_GCR);
+		stm_msp_write(0, msp->registers + MSP_TCF);
+		stm_msp_write(0, msp->registers + MSP_RCF);
+		stm_msp_write(0, msp->registers + MSP_DMACR);
+		stm_msp_write(0, msp->registers + MSP_SRG);
+		stm_msp_write(0, msp->registers + MSP_MCR);
+		stm_msp_write(0, msp->registers + MSP_RCM);
+		stm_msp_write(0, msp->registers + MSP_RCV);
+		stm_msp_write(0, msp->registers + MSP_TCE0);
+		stm_msp_write(0, msp->registers + MSP_TCE1);
+		stm_msp_write(0, msp->registers + MSP_TCE2);
+		stm_msp_write(0, msp->registers + MSP_TCE3);
+		stm_msp_write(0, msp->registers + MSP_RCE0);
+		stm_msp_write(0, msp->registers + MSP_RCE1);
+		stm_msp_write(0, msp->registers + MSP_RCE2);
+		stm_msp_write(0, msp->registers + MSP_RCE3);
+	}
+	return status;
+}
+
+/**
+ * stm_msp_close - Close the current i2s connection btw controller and client.
+ * @i2s_cont: i2s controller structure
+ * @flag: It indicates the functionality that needs to be disabled.
+ * Context: process
+ *
+ * It will call msp_disable and reset the msp configuration. Disables Rx and Tx
+ * channels, free gpio irqs and interrupt pins.
+ * Called by i2s client driver to indicate the completion of use of i2s bus.
+ * It is registered on i2s_close function.
+ *
+ * Returns error(-1) in case of failure or success(0).
+ */
+static int stm_msp_close(struct i2s_controller *i2s_cont, i2s_flag flag)
+{
+	int status = 0;
+	struct msp *msp = (struct msp *)i2s_cont->data;
+	down(&msp->lock);
+	if (msp->users == 0) {
+		pr_err("MSP already closed!\n");
+		status = -EINVAL;
+		goto end;
+	}
+	dev_dbg(&i2s_cont->dev, "%s: users = %d, flag = %d.\n",
+	       __func__, msp->users, flag);
+	/* We need to call it twice for DISABLE_ALL*/
+	msp->users = flag == DISABLE_ALL ? 0 : msp->users - 1;
+	if (msp->users)
+		status = stm_msp_disable(msp, MSP_BOTH_T_R_MODE, flag);
+	else {
+		status = stm_msp_disable(msp, MSP_BOTH_T_R_MODE, DISABLE_ALL);
+		clk_disable(msp->clk);
+		if (msp->reg_enabled) {
+			status = regulator_disable(msp_vape_supply);
+			msp->reg_enabled = 0;
+		}
+		if (status != 0) {
+			dev_err(&msp->i2s_cont->dev,
+				"Failed to disable regulator\n");
+			clk_enable(msp->clk);
+			goto end;
+		}
+	}
+	if (status)
+		goto end;
+	if (msp->users)
+		msp->direction = flag == DISABLE_TRANSMIT ?
+			MSP_RECEIVE_MODE : MSP_TRANSMIT_MODE;
+
+	if (msp->vape_opp_constraint == 1) {
+		prcmu_qos_update_requirement(PRCMU_QOS_APE_OPP, "msp_i2s", 50);
+		msp->vape_opp_constraint = 0;
+	}
+end:
+	up(&msp->lock);
+	return status;
+
+}
+
+static int stm_msp_hw_status(struct i2s_controller *i2s_cont)
+{
+	struct msp *msp = (struct msp *)i2s_cont->data;
+
+	int status = stm_msp_read(msp->registers + MSP_RIS) & 0xee;
+	if (status)
+		stm_msp_write(status, msp->registers + MSP_ICR);
+
+	return status;
+}
+
+		/*Platform driver's functions */
+/**
+ * msp_probe - Probe function
+ * @pdev: platform device structure.
+ * Context: process
+ *
+ * Probe function of msp platform driver.Handles allocation of memory and irq
+ * resource. It creates i2s_controller and one i2s_device per msp controller.
+ *
+ * Returns error(-1) in case of failure or success(0).
+ */
+int msp_probe(struct platform_device *pdev)
+{
+	int status = 0;
+	struct device *dev;
+	s16 platform_num = 0;
+	struct resource *res = NULL;
+	int irq;
+	struct i2s_controller *i2s_cont;
+	struct msp_i2s_platform_data *platform_data;
+	struct msp *msp;
+
+	if (!pdev)
+		return -EPERM;
+	msp = kzalloc(sizeof(*msp), GFP_KERNEL);
+
+	platform_data = (struct msp_i2s_platform_data *)pdev->dev.platform_data;
+
+	msp->id = platform_data->id;
+	msp->plat_init = platform_data->msp_i2s_init;
+	msp->plat_exit = platform_data->msp_i2s_exit;
+
+	msp->dma_cfg_rx = platform_data->msp_i2s_dma_rx;
+	msp->dma_cfg_tx = platform_data->msp_i2s_dma_tx;
+
+	dev = &pdev->dev;
+	platform_num = msp->id - 1;
+
+	sema_init(&msp->lock, 1);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "probe - MEM resources not defined\n");
+		status = -EINVAL;
+		goto free_msp;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		status = -EINVAL;
+		goto free_msp;
+	}
+	msp->irq = irq;
+
+	msp->registers = ioremap(res->start, (res->end - res->start + 1));
+	if (msp->registers == NULL) {
+		status = -EINVAL;
+		goto free_msp;
+	}
+
+	msp_vape_supply = regulator_get(NULL, "v-ape");
+	if (IS_ERR(msp_vape_supply)) {
+		status = PTR_ERR(msp_vape_supply);
+		printk(KERN_WARNING "msp i2s : failed to get v-ape supply\n");
+		goto free_irq;
+	}
+	prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP, "msp_i2s", 50);
+	msp->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(msp->clk)) {
+		status = PTR_ERR(msp->clk);
+		goto free_irq;
+	}
+
+	init_timer(&msp->notify_timer);
+	msp->notify_timer.expires = jiffies + msecs_to_jiffies(1000);
+	msp->notify_timer.function = func_notify_timer;
+	msp->notify_timer.data = (unsigned long)msp;
+
+	msp->rx_pipeid = NULL;
+	msp->tx_pipeid = NULL;
+	msp->read = NULL;
+	msp->write = NULL;
+	msp->transfer = NULL;
+	msp->msp_state = MSP_STATE_IDLE;
+	msp->loopback_enable = 0;
+
+	dev_set_drvdata(&pdev->dev, msp);
+	/* I2S Controller is allocated and added in I2S controller class. */
+	i2s_cont =
+	    (struct i2s_controller *)kzalloc(sizeof(*i2s_cont), GFP_KERNEL);
+	if (!i2s_cont) {
+		dev_err(&pdev->dev, "i2s controller alloc failed \n");
+		status = -EINVAL;
+		goto del_timer;
+	}
+	i2s_cont->dev.parent = dev;
+	i2s_cont->algo = &i2s_algo;
+	i2s_cont->data = (void *)msp;
+	i2s_cont->id = platform_num;
+	snprintf(i2s_cont->name, sizeof(i2s_cont->name),
+		 "MSP_I2S.%04x", platform_num);
+
+	status = i2s_add_controller(i2s_cont);
+	if (status) {
+		dev_err(&pdev->dev, "i2s add controller failed (%d)\n", status);
+		goto free_cont;
+	}
+	msp->i2s_cont = i2s_cont;
+	return status;
+free_cont:
+	kfree(msp->i2s_cont);
+del_timer:
+	del_timer_sync(&msp->notify_timer);
+	clk_put(msp->clk);
+free_irq:
+	iounmap(msp->registers);
+free_msp:
+	kfree(msp);
+	return status;
+}
+
+/**
+ * msp_remove - remove function
+ * @pdev: platform device structure.
+ * Context: process
+ *
+ * remove function of msp platform driver.Handles dellocation of memory and irq
+ * resource. It deletes i2s_controller and one i2s_device per msp controller
+ * created in msp_probe.
+ *
+ * Returns error(-1) in case of failure or success(0).
+ */
+static int msp_remove(struct platform_device *pdev)
+{
+	struct msp *msp =
+	    (struct msp *)dev_get_drvdata(&pdev->dev);
+	int status = 0;
+	i2s_del_controller(msp->i2s_cont);
+	del_timer_sync(&msp->notify_timer);
+	clk_put(msp->clk);
+	iounmap(msp->registers);
+	prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP, "msp_i2s");
+	regulator_put(msp_vape_supply);
+	kfree(msp);
+	return status;
+}
+#ifdef CONFIG_PM
+/**
+ * msp_suspend - MSP suspend function registered with PM framework.
+ * @pdev: Reference to platform device structure of the device
+ * @state: power mgmt state.
+ *
+ * This function is invoked when the system is going into sleep, called
+ * by the power management framework of the linux kernel.
+ * Nothing is required as controller is configured with every transfer.
+ * It is assumed that no active tranfer is in progress at this time.
+ * Client driver should make sure of this.
+ *
+ */
+
+int msp_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct msp *msp =
+		(struct msp *)dev_get_drvdata(&pdev->dev);
+
+	down(&msp->lock);
+	if (msp->users > 0) {
+		up(&msp->lock);
+		return -EBUSY;
+	}
+	up(&msp->lock);
+
+	return 0;
+}
+/**
+ * msp_resume - MSP Resume function registered with PM framework.
+ * @pdev: Reference to platform device structure of the device
+ *
+ * This function is invoked when the system is coming out of sleep, called
+ * by the power management framework of the linux kernel.
+ * Nothing is required.
+ *
+ */
+
+int msp_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+#else
+#define msp_suspend NULL
+#define msp_resume NULL
+#endif
+
+static struct platform_driver msp_i2s_driver = {
+	.probe = msp_probe,
+	.remove = msp_remove,
+	.suspend = msp_suspend,
+	.resume = msp_resume,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "MSP_I2S",
+		   },
+};
+
+static int __init stm_msp_mod_init(void)
+{
+	return platform_driver_register(&msp_i2s_driver);
+}
+
+static void __exit stm_msp_exit(void)
+{
+	platform_driver_unregister(&msp_i2s_driver);
+	return;
+}
+
+module_init(stm_msp_mod_init);
+module_exit(stm_msp_exit);
+
+MODULE_AUTHOR("Sandeep Kaushik");
+MODULE_DESCRIPTION("STM MSP driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/i2s/msp_i2s.h b/drivers/misc/i2s/msp_i2s.h
new file mode 100644
index 00000000000..2ac86e7700e
--- /dev/null
+++ b/drivers/misc/i2s/msp_i2s.h
@@ -0,0 +1,362 @@
+/*----------------------------------------------------------------------------------*/
+/*  copyright STMicroelectronics, 2007.                                            */
+/*                                                                                  */
+/* 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.1 of the License, or (at your option)      */
+/* any later version.                                                               */
+/*                                                                                  */
+/* This program is distributed in the hope that it will be useful, but WITHOUT      */
+/* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS    */
+/* FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.   */
+/*                                                                                  */
+/* You should have received a copy of the GNU General Public License                */
+/* along with this program. If not, see <http://www.gnu.org/licenses/>.             */
+/*----------------------------------------------------------------------------------*/
+
+
+#ifndef STM_MSP_HEADER
+#define STM_MSP_HEADER
+
+#define MSP_DR 0x00
+#define MSP_GCR		0x04
+#define MSP_TCF		0x08
+#define MSP_RCF		0x0c
+#define MSP_SRG		0x10
+#define MSP_FLR		0x14
+#define MSP_DMACR	0x18
+
+#define MSP_IMSC	0x20
+#define MSP_RI 0x24
+#define MSP_MIS 0x28
+#define MSP_ICR		0x2c
+#define MSP_MCR		0x30
+#define MSP_RCV 0x34
+#define MSP_RCM 0x38
+
+#define MSP_TCE0	0x40
+#define MSP_TCE1	0x44
+#define MSP_TCE2 0x48
+#define MSP_TCE3 0x4c
+
+#define MSP_RCE0	0x60
+#define MSP_RCE1	0x64
+#define MSP_RCE2	0x68
+#define MSP_RCE3 0x6c
+
+#define MSP_ITCR	0x80
+#define MSP_ITIP	0x84
+#define MSP_ITOP	0x88
+#define MSP_TSTDR	0x8c
+
+#define MSP_PID0	0xfe0
+#define MSP_PID1	0xfe4
+#define MSP_PID2 0xfe8
+#define MSP_PID3	0xfec
+
+#define MSP_CID0	0xff0
+#define MSP_CID1 0xff4
+#define MSP_CID2	0xff8
+#define MSP_CID3 0xffc
+
+
+/* Single or dual phase mode */
+enum
+{
+	MSP_SINGLE_PHASE,
+	MSP_DUAL_PHASE
+};
+
+
+/* Transmit/Receive shifter status
+-----------------------------------*/
+enum
+{
+	    MSP_SxHIFTER_IDLE   = 0,
+	    MSP_SHIFTER_WORKING = 1
+};
+
+
+/* Transmit/Receive FIFO status
+---------------------------------*/
+enum
+{
+	MSP_FIFO_FULL,
+	MSP_FIFO_PART_FILLED,
+	MSP_FIFO_EMPTY
+};
+
+
+/* Frame length
+------------------*/
+enum
+{
+	MSP_FRAME_LENGTH_1		= 0,
+	MSP_FRAME_LENGTH_2		= 1,
+	MSP_FRAME_LENGTH_4		= 3,
+	MSP_FRAME_LENGTH_8		= 7,
+	MSP_FRAME_LENGTH_12		= 11,
+	MSP_FRAME_LENGTH_16		= 15,
+	MSP_FRAME_LENGTH_20		= 19,
+	MSP_FRAME_LENGTH_32		= 31,
+	MSP_FRAME_LENGTH_48		= 47,
+	MSP_FRAME_LENGTH_64		= 63
+};
+
+/* Element length */
+enum
+{
+	MSP_ELEM_LENGTH_8		= 0,
+	MSP_ELEM_LENGTH_10		= 1,
+	MSP_ELEM_LENGTH_12		= 2,
+	MSP_ELEM_LENGTH_14		= 3,
+	MSP_ELEM_LENGTH_16		= 4,
+	MSP_ELEM_LENGTH_20		= 5,
+	MSP_ELEM_LENGTH_24		= 6,
+	MSP_ELEM_LENGTH_32		= 7
+};
+
+
+/* Data delay (in bit clock cycles)
+---------------------------------------*/
+enum
+{
+	MSP_DELAY_0 = 0,
+	MSP_DELAY_1 = 1,
+	MSP_DELAY_2 = 2,
+	MSP_DELAY_3 = 3
+};
+
+
+/* Configurations of clocks (transmit, receive or sample rate generator)
+-------------------------------------------------------------------------*/
+enum
+{
+	MSP_RISING_EDGE			= 0,
+	MSP_FALLING_EDGE		= 1
+};
+
+/* Protocol dependant parameters list */
+struct msp_protocol_desc
+{
+	    u32 phase_mode;
+	    u32 frame_len_1;
+	    u32 frame_len_2;
+	    u32 element_len_1;
+	    u32 element_len_2;
+	    u32 data_delay;
+	    u32 tx_clock_edge;
+	    u32 rx_clock_edge;
+};
+#define RX_ENABLE_MASK         0x00000001
+#define RX_FIFO_ENABLE_MASK    0x00000002
+#define RX_FRAME_SYNC_MASK     0x00000004
+#define DIRECT_COMPANDING_MASK 0x00000008
+#define RX_SYNC_SEL_MASK       0x00000010
+#define RX_CLK_POL_MASK        0x00000020
+#define RX_CLK_SEL_MASK        0x00000040
+#define LOOPBACK_MASK          0x00000080
+#define TX_ENABLE_MASK         0x00000100
+#define TX_FIFO_ENABLE_MASK    0x00000200
+#define TX_FRAME_SYNC_MASK     0x00000400
+#define TX_MSP_TDR_TSR         0x00000800
+#define TX_SYNC_SEL_MASK       0x00001800
+#define TX_CLK_POL_MASK        0x00002000
+#define TX_CLK_SEL_MASK        0x00004000
+#define TX_EXTRA_DELAY_MASK    0x00008000
+#define SRG_ENABLE_MASK        0x00010000
+#define SRG_CLK_POL_MASK       0x00020000
+#define SRG_CLK_SEL_MASK       0x000C0000
+#define FRAME_GEN_EN_MASK      0x00100000
+#define SPI_CLK_MODE_MASK      0x00600000
+#define SPI_BURST_MODE_MASK    0x00800000
+
+#define RXEN_BIT		0
+#define RFFEN_BIT		1
+#define RFSPOL_BIT		2
+#define DCM_BIT		3
+#define RFSSEL_BIT		4
+#define RCKPOL_BIT		5
+#define RCKSEL_BIT		6
+#define LBM_BIT		7
+#define TXEN_BIT		8
+#define TFFEN_BIT		9
+#define TFSPOL_BIT		10
+#define TFSSEL_BIT		11
+#define TCKPOL_BIT		13
+#define TCKSEL_BIT		14
+#define TXDDL_BIT		15
+#define SGEN_BIT		16
+#define SCKPOL_BIT		17
+#define SCKSEL_BIT		18
+#define FGEN_BIT		20
+#define SPICKM_BIT		21
+
+#define msp_rx_clkpol_bit(n)     ((n & 1) << RCKPOL_BIT)
+#define msp_tx_clkpol_bit(n)     ((n & 1) << TCKPOL_BIT)
+#define msp_spi_clk_mode_bits(n) ((n & 3) << SPICKM_BIT)
+
+
+/* Use this to clear the clock mode bits to non-spi */
+#define MSP_NON_SPI_CLK_MASK 0x00600000
+
+#define P1ELEN_BIT		0
+#define P1FLEN_BIT		3
+#define DTYP_BIT		10
+#define ENDN_BIT		12
+#define DDLY_BIT		13
+#define FSIG_BIT		15
+#define P2ELEN_BIT		16
+#define P2FLEN_BIT		19
+#define P2SM_BIT		26
+#define P2EN_BIT		27
+
+#define msp_p1_elem_len_bits(n) (n & 0x00000007)
+#define msp_p2_elem_len_bits(n)  (((n) << P2ELEN_BIT) & 0x00070000)
+#define msp_p1_frame_len_bits(n) (((n) << P1FLEN_BIT) & 0x00000378)
+#define msp_p2_frame_len_bits(n) (((n) << P2FLEN_BIT) & 0x03780000)
+#define msp_data_delay_bits(n)   (((n) << DDLY_BIT) & 0x00003000)
+#define msp_data_type_bits(n)    (((n) << DTYP_BIT) & 0x00000600)
+#define msp_p2_start_mode_bit(n) (n << P2SM_BIT)
+#define msp_p2_enable_bit(n)     (n << P2EN_BIT)
+
+/* Flag register
+--------------------*/
+#define RX_BUSY       0x00000001
+#define RX_FIFO_EMPTY 0x00000002
+#define RX_FIFO_FULL  0x00000004
+#define TX_BUSY       0x00000008
+#define TX_FIFO_EMPTY 0x00000010
+#define TX_FIFO_FULL  0x00000020
+
+#define RBUSY_BIT		0
+#define RFE_BIT			1
+#define RFU_BIT			2
+#define TBUSY_BIT		3
+#define TFE_BIT			4
+#define TFU_BIT			5
+
+/* Multichannel control register
+---------------------------------*/
+#define RMCEN_BIT		0
+#define RMCSF_BIT		1
+#define RCMPM_BIT		3
+#define TMCEN_BIT		5
+#define TNCSF_BIT		6
+
+/* Sample rate generator register
+------------------------------------*/
+#define SCKDIV_BIT		0
+#define FRWID_BIT		10
+#define FRPER_BIT		16
+
+#define SCK_DIV_MASK 0x0000003FF
+#define frame_width_bits(n) (((n) << FRWID_BIT)  &0x0000FC00)
+#define frame_period_bits(n) (((n) << FRPER_BIT) &0x1FFF0000)
+
+
+/* DMA controller register
+---------------------------*/
+#define RX_DMA_ENABLE 0x00000001
+#define TX_DMA_ENABLE 0x00000002
+
+#define RDMAE_BIT		0
+#define TDMAE_BIT		1
+
+/*Interrupt Register
+-----------------------------------------*/
+#define RECEIVE_SERVICE_INT         0x00000001
+#define RECEIVE_OVERRUN_ERROR_INT   0x00000002
+#define RECEIVE_FRAME_SYNC_ERR_INT  0x00000004
+#define RECEIVE_FRAME_SYNC_INT      0x00000008
+#define TRANSMIT_SERVICE_INT        0x00000010
+#define TRANSMIT_UNDERRUN_ERR_INT   0x00000020
+#define TRANSMIT_FRAME_SYNC_ERR_INT 0x00000040
+#define TRANSMIT_FRAME_SYNC_INT     0x00000080
+#define ALL_INT                     0x000000ff
+
+/* Protocol configuration values
+* I2S: Single phase, 16 bits, 2 words per frame
+-----------------------------------------------*/
+#define I2S_PROTOCOL_DESC			\
+{									\
+	MSP_SINGLE_PHASE,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_ELEM_LENGTH_32,				\
+	MSP_ELEM_LENGTH_32,				\
+	MSP_DELAY_1,					\
+	MSP_FALLING_EDGE,				\
+	MSP_FALLING_EDGE			\
+}
+
+#define PCM_PROTOCOL_DESC			\
+{									\
+	MSP_SINGLE_PHASE,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_ELEM_LENGTH_16,				\
+	MSP_ELEM_LENGTH_16,				\
+	MSP_DATA_DELAY,					\
+	MSP_TX_CLOCK_EDGE,				\
+	MSP_RX_CLOCK_EDGE			\
+}
+
+/* Companded PCM: Single phase, 8 bits, 1 word per frame
+--------------------------------------------------------*/
+#define PCM_COMPAND_PROTOCOL_DESC	\
+{									\
+	MSP_SINGLE_PHASE,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_DELAY_0,					\
+	MSP_RISING_EDGE,					\
+	MSP_FALLING_EDGE					\
+}
+
+/* AC97: Double phase, 1 element of 16 bits during first phase,
+* 12 elements of 20 bits in second phase.
+--------------------------------------------------------------*/
+#define AC97_PROTOCOL_DESC			\
+{									\
+	MSP_DUAL_PHASE,					\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_12,			\
+	MSP_ELEM_LENGTH_16,				\
+	MSP_ELEM_LENGTH_20,				\
+	MSP_DELAY_1,					\
+	MSP_RISING_EDGE,					\
+	MSP_FALLING_EDGE					\
+}
+
+#define SPI_MASTER_PROTOCOL_DESC	\
+{									\
+	MSP_SINGLE_PHASE,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_DELAY_1,					\
+	MSP_FALLING_EDGE,				\
+	MSP_RISING_EDGE				\
+}
+#define SPI_SLAVE_PROTOCOL_DESC		\
+{									\
+	MSP_SINGLE_PHASE,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_FRAME_LENGTH_1,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_ELEM_LENGTH_8,				\
+	MSP_DELAY_1,					\
+	MSP_FALLING_EDGE,				\
+	MSP_RISING_EDGE				\
+}
+
+#define MSP_FRAME_PERIOD_IN_MONO_MODE 256
+#define MSP_FRAME_PERIOD_IN_STEREO_MODE 32
+#define MSP_FRAME_WIDTH_IN_STEREO_MODE 16
+
+#endif
+
diff --git a/arch/arm/mach-ux500/mbox-db5500.c b/drivers/misc/mbox.c
index 2b2d51caf9d..2b2d51caf9d 100644
--- a/arch/arm/mach-ux500/mbox-db5500.c
+++ b/drivers/misc/mbox.c
diff --git a/drivers/misc/mbox_channels-db5500.c b/drivers/misc/mbox_channels-db5500.c
new file mode 100644
index 00000000000..1fb883f3296
--- /dev/null
+++ b/drivers/misc/mbox_channels-db5500.c
@@ -0,0 +1,1171 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Mailbox Logical Driver
+ *
+ * Author: Marcin Mielczarczyk <marcin.mielczarczyk@tieto.com> for ST-Ericsson.
+ *         Bibek Basu ,bibek.basu@stericsson.com>
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <asm/mach-types.h>
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <mach/mbox-db5500.h>
+#include <mach/mbox_channels-db5500.h>
+#include <linux/io.h>
+
+/* Defines start sequence number for given mailbox channel */
+#define CHANNEL_START_SEQUENCE_NUMBER	0x80
+
+/* Defines number of channels per mailbox unit */
+#define CHANNELS_PER_MBOX_UNIT		256
+
+/*
+ * This macro builds mbox channel PDU header with following format:
+ * ---------------------------------------------------------------------------
+ * |                |         |          |                                    |
+ * | Sequence nmbr  |  Type   |  Length  | Destination logical channel number |
+ * |                |         |          |                                    |
+ * ---------------------------------------------------------------------------
+ * 31               24        20         16                    0
+ *
+ */
+#define BUILD_HEADER(chan, len, type, seq_no) \
+			((chan) | (((len) & 0xf) << 16) | \
+			(((type) & 0xf) << 20) | ((seq_no) << 24))
+
+/* Returns type from mbox message header */
+#define GET_TYPE(mbox_msg)			(((mbox_msg) >> 20) & 0xf)
+
+/* Returns channel number from mbox message header */
+#define GET_CHANNEL(mbox_msg)			((mbox_msg) & 0xffff)
+
+/* Returns length of payload from mbox message header */
+#define GET_LENGTH(mbox_msg)			(((mbox_msg) >> 16) & 0xf)
+
+/* Returns sequence number from mbox message header */
+#define GET_SEQ_NUMBER(mbox_msg)		(((mbox_msg) >> 24)
+
+/* Number of buffers */
+#define NUM_DSP_BUFFER	3
+
+/* circular buffer indicator */
+static int buffer_index;
+
+enum mbox_msg{
+	MBOX_CLOSE,
+	MBOX_OPEN,
+	MBOX_SEND,
+	MBOX_CAST,
+	MBOX_ACK,
+	MBOX_NAK,
+};
+
+enum mbox_dir {
+	MBOX_TX,
+	MBOX_RX,
+};
+
+struct mbox_channel_mapping {
+	u16		chan_base;
+	u8		mbox_id;
+	enum mbox_dir	direction;
+};
+
+/* This table maps mbox logical channel to mbox id and direction */
+static struct mbox_channel_mapping channel_mappings[] = {
+	{0x500, 2, MBOX_RX}, /* channel 5 maps to mbox 0.1, dsp->app (unsec) */
+	{0x900, 2, MBOX_TX}, /* channel 9 maps to mbox 0.0, app->dsp (unsec) */
+};
+
+/* This table specifies mailbox ids which mbox channels module will use */
+static u8 mbox_ids[] = {
+	2,		/* app <-> dsp (unsec) */
+};
+
+/**
+ * struct mbox_unit_status - current status of mbox unit
+ * @mbox_id :		holds mbox unit identification number
+ * @mbox :		holds mbox pointer after mbox_register() call
+ * @tx_chans :		holds list of open tx mbox channels
+ * @tx_lock:		lock for tx channel
+ * @rx_chans :		holds list of open rx mbox channels
+ * @rx_lock:		lock for rx channel
+ */
+struct mbox_unit_status {
+	u8			mbox_id;
+	struct mbox		*mbox;
+	struct list_head	tx_chans;
+	spinlock_t		tx_lock;
+	struct list_head	rx_chans;
+	spinlock_t		rx_lock;
+};
+
+static struct {
+	struct platform_device	*pdev;
+	struct mbox_unit_status	mbox_unit[ARRAY_SIZE(mbox_ids)];
+} channels;
+
+/* This structure describes pending element for mbox tx channel */
+struct pending_elem {
+	struct list_head	list;
+	u32			*data;
+	u8			length;
+};
+
+struct rx_pending_elem {
+	u32			buffer[MAILBOX_NR_OF_DATAWORDS];
+	u8			length;
+	void			*priv;
+};
+
+struct rx_pending_elem rx_pending[NUM_DSP_BUFFER + 1];
+
+/* This structure holds list of pending elements for mbox tx channel */
+struct tx_channel {
+	struct list_head	pending;
+};
+
+/* Specific status for mbox rx channel */
+struct rx_channel {
+	struct list_head	pending;
+	spinlock_t		lock;
+	u32			buffer[MAILBOX_NR_OF_DATAWORDS];
+	u8			index;
+	u8			length;
+};
+
+/* This structure holds status of mbox channel - common for tx and rx */
+struct channel_status {
+	struct list_head	list;
+	u16			channel;
+	int			state;
+	mbox_channel_cb_t	*cb;
+	void			*priv;
+	u8			seq_number;
+	bool			with_ack;
+	struct rx_channel	rx;
+	struct tx_channel	tx;
+	struct work_struct	receive_msg;
+	struct work_struct	open_msg;
+	struct work_struct	cast_msg;
+	struct mutex		lock;
+};
+
+/* Checks if provided channel number is valid */
+static bool check_channel(u16 channel, enum mbox_dir direction)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(channel_mappings); i++) {
+		if ((channel >= channel_mappings[i].chan_base) &&
+		    (channel < channel_mappings[i].chan_base +
+		    CHANNELS_PER_MBOX_UNIT)) {
+			/* Check if direction of given channel is correct*/
+			if (channel_mappings[i].direction == direction)
+				return true;
+			else
+				break;
+		}
+	}
+	return false;
+}
+
+/* get the tx channel corresponding to the given rx channel */
+static u16 get_tx_channel(u16 channel)
+{
+	int i;
+	int relative_chan = 0;
+	int mbox_id = 0xFF;
+	u16 tx_channel = 0xFF;
+
+	for (i = 0; i < ARRAY_SIZE(channel_mappings); i++) {
+		if ((channel >= channel_mappings[i].chan_base) &&
+				(channel < channel_mappings[i].chan_base +
+				 CHANNELS_PER_MBOX_UNIT)) {
+			/* Check if direction of given channel is correct*/
+			relative_chan = channel - channel_mappings[i].chan_base;
+			mbox_id = channel_mappings[i].mbox_id;
+
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(channel_mappings); i++) {
+		if ((mbox_id == channel_mappings[i].mbox_id) &&
+				(channel_mappings[i].direction == MBOX_TX))
+			tx_channel = channel_mappings[i].chan_base +
+				relative_chan;
+	}
+	return tx_channel;
+}
+
+/* Returns mbox unit id for given mbox channel */
+static int get_mbox_id(u16 channel)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(channel_mappings); i++) {
+		if ((channel >= channel_mappings[i].chan_base) &&
+		    (channel < channel_mappings[i].chan_base +
+		    CHANNELS_PER_MBOX_UNIT)) {
+			return channel_mappings[i].mbox_id;
+		}
+	}
+	/* There is no mbox unit registered for given channel */
+	return -EINVAL;
+}
+
+/* Returns mbox structure saved after mbox_register() call */
+static struct mbox *get_mbox(u16 channel)
+{
+	int i;
+	int mbox_id = get_mbox_id(channel);
+
+	if (mbox_id < 0) {
+		dev_err(&channels.pdev->dev, "couldn't get mbox id\n");
+		return NULL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(channels.mbox_unit); i++) {
+		if (channels.mbox_unit[i].mbox_id == mbox_id)
+			return channels.mbox_unit[i].mbox;
+	}
+	return NULL;
+}
+
+/* Returns pointer to rx mbox channels list for given mbox unit */
+static struct list_head *get_rx_list(u8 mbox_id)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mbox_ids); i++) {
+		if (channels.mbox_unit[i].mbox_id == mbox_id)
+			return &channels.mbox_unit[i].rx_chans;
+	}
+	return NULL;
+}
+
+/* Returns pointer to tx mbox channels list for given mbox unit */
+static struct list_head *get_tx_list(u8 mbox_id)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mbox_ids); i++) {
+		if (channels.mbox_unit[i].mbox_id == mbox_id)
+			return &channels.mbox_unit[i].tx_chans;
+	}
+	return NULL;
+}
+
+static int send_pdu(struct channel_status *chan_status, int command,
+		u16 channel)
+{
+	struct mbox *mbox;
+	u32 header = 0;
+	int ret = 0;
+
+	/* SEND PDU is not supported */
+	if (command == MBOX_SEND) {
+		dev_err(&channels.pdev->dev, "SEND command not implemented\n");
+		ret = -EINVAL;
+		goto exit;
+	}
+	mbox = get_mbox(chan_status->channel);
+	if (mbox == NULL) {
+		dev_err(&channels.pdev->dev, "couldn't get mailbox\n");
+		ret = -ENOSYS;
+		goto exit;
+	}
+	/* For CAST type send all pending messages */
+	if (command == MBOX_CAST) {
+		struct list_head *pos, *n;
+
+		/* Send all pending messages from TX channel */
+		list_for_each_safe(pos, n, &chan_status->tx.pending) {
+			struct pending_elem *pending =
+				list_entry(pos, struct pending_elem, list);
+			int i;
+
+			header = BUILD_HEADER(channel,
+					pending->length,
+					command,
+					chan_status->seq_number);
+
+			ret = mbox_send(mbox, header, true);
+			if (ret < 0) {
+				dev_err(&channels.pdev->dev,
+					"failed to send header, err=%d\n", ret);
+				goto exit;
+			}
+
+			for (i = 0; i < pending->length; i++) {
+				ret = mbox_send(mbox, pending->data[i], true);
+				if (ret < 0) {
+					dev_err(&channels.pdev->dev,
+					"failed to send header, err=%d\n", ret);
+					goto exit;
+				}
+			}
+
+			/* Call client's callback that data is already sent */
+			if (chan_status->cb)
+				chan_status->cb(pending->data, pending->length,
+						chan_status->priv);
+			else
+				dev_err(&channels.pdev->dev,
+					"%s no callback provided\n", __func__);
+
+			/* Increment sequence number */
+			chan_status->seq_number++;
+
+			/* Remove and free element from the list */
+			list_del(&pending->list);
+			kfree(pending);
+		}
+	} else {
+		header = BUILD_HEADER(channel, 0,
+				command, chan_status->seq_number);
+
+		ret = mbox_send(mbox, header, true);
+		if (ret < 0)
+			dev_err(&channels.pdev->dev, "failed to send header\n");
+		/* Increment sequence number */
+		chan_status->seq_number++;
+	}
+
+exit:
+	return ret;
+}
+
+void mbox_handle_receive_msg(struct work_struct *work)
+{
+	struct channel_status *rx_chan = container_of(work,
+			struct channel_status,
+			receive_msg);
+
+	/* Call client's callback and reset state */
+	if (rx_chan->cb) {
+		static int rx_pending_count;
+
+		if (rx_pending_count == NUM_DSP_BUFFER)
+			rx_pending_count = 0;
+		else
+			rx_pending_count++;
+	rx_chan->cb(rx_pending[rx_pending_count].buffer,
+			rx_pending[rx_pending_count].length,
+			rx_pending[rx_pending_count].priv);
+		buffer_index--;
+	} else {
+		dev_err(&channels.pdev->dev,
+				"%s no callback provided\n", __func__);
+	}
+
+}
+
+void mbox_handle_open_msg(struct work_struct *work)
+{
+	struct channel_status *tx_chan = container_of(work,
+							struct channel_status,
+							 open_msg);
+	/* Change channel state to OPEN */
+	tx_chan->state = MBOX_OPEN;
+	/* If pending list not empty, start sending data */
+	if (!list_empty(&tx_chan->tx.pending))
+		send_pdu(tx_chan, MBOX_CAST, tx_chan->channel);
+}
+
+void mbox_handle_cast_msg(struct work_struct *work)
+{
+	struct channel_status *rx_chan = container_of(work,
+							struct channel_status,
+							 cast_msg);
+	/* Check if channel is opened */
+	if (rx_chan->state == MBOX_CLOSE) {
+		/* Peer sent message to closed channel */
+		dev_err(&channels.pdev->dev,
+			"channel in wrong state\n");
+	}
+}
+
+static bool handle_receive_msg(u32 mbox_msg, struct list_head *rx_list)
+{
+	struct list_head *pos;
+	struct channel_status *tmp;
+	int i;
+	static int rx_pending_count;
+	struct channel_status *rx_chan = NULL;
+	struct mbox_unit_status *mbox_unit = container_of(rx_list,
+			struct mbox_unit_status,
+			rx_chans);
+	spin_lock(&mbox_unit->rx_lock);
+	list_for_each(pos, rx_list) {
+		tmp = list_entry(pos, struct channel_status, list);
+		if (tmp->state == MBOX_SEND ||
+				tmp->state == MBOX_CAST)
+			/* Received message is payload */
+			rx_chan = tmp;
+	}
+	/* Received message is header */
+	spin_unlock(&mbox_unit->rx_lock);
+	if (rx_chan) {
+		/* Store received data in RX channel buffer */
+		rx_chan->rx.buffer[rx_chan->rx.index++] = mbox_msg;
+		/* Check if it's last data of PDU */
+		if (rx_chan->rx.index == rx_chan->rx.length) {
+			if (rx_pending_count == NUM_DSP_BUFFER)
+				rx_pending_count = 0;
+			else
+				rx_pending_count++;
+			for (i = 0; i < MAILBOX_NR_OF_DATAWORDS; i++) {
+				rx_pending[rx_pending_count].buffer[i] =
+					rx_chan->rx.buffer[i];
+			}
+			rx_pending[rx_pending_count].length =
+				rx_chan->rx.length;
+			rx_pending[rx_pending_count].priv = rx_chan->priv;
+			rx_chan->rx.index = 0;
+			rx_chan->rx.length = 0;
+			rx_chan->state = MBOX_OPEN;
+			rx_chan->seq_number++;
+			buffer_index++;
+			if (buffer_index >= NUM_DSP_BUFFER)
+				dev_err(&channels.pdev->dev,
+					"rxbuf overflow%d\n", buffer_index);
+			schedule_work(&rx_chan->receive_msg);
+		}
+		dev_dbg(&channels.pdev->dev, "%s OK\n", __func__);
+		return true;
+	}
+	return false;
+}
+
+static void handle_open_msg(u16 channel, u8 mbox_id)
+{
+	struct list_head *tx_list, *pos;
+	struct channel_status *tmp;
+	struct channel_status *tx_chan = NULL;
+	struct mbox_unit_status *mbox_unit;
+	channel = get_tx_channel(channel);
+	dev_dbg(&channels.pdev->dev, "%s mbox_id %d\tchannel %x\n",
+			__func__, mbox_id, channel);
+	/* Get TX channesenx for given mbox unit */
+	tx_list = get_tx_list(mbox_id);
+	if (tx_list == NULL) {
+		dev_err(&channels.pdev->dev, "given mbox id is not valid %d\n",
+			mbox_id);
+		return;
+	}
+	mbox_unit = container_of(tx_list, struct mbox_unit_status, tx_chans);
+	/* Search for channel in tx list */
+	spin_lock(&mbox_unit->tx_lock);
+	list_for_each(pos, tx_list) {
+		tmp = list_entry(pos, struct channel_status, list);
+		dev_dbg(&channels.pdev->dev, "tmp->channel=%d\n",
+				tmp->channel);
+		if (tmp->channel == channel)
+			tx_chan = tmp;
+	}
+	spin_unlock(&mbox_unit->tx_lock);
+	if (tx_chan) {
+		schedule_work(&tx_chan->open_msg);
+	} else {
+		/* No tx channel found on the list, allocate new element */
+		tx_chan = kzalloc(sizeof(*tx_chan), GFP_ATOMIC);
+		if (tx_chan == NULL) {
+			dev_err(&channels.pdev->dev,
+				"failed to allocate memory\n");
+			return;
+		}
+
+		/* Fill initial data and add this element to tx list */
+		tx_chan->channel = get_tx_channel(channel);
+		tx_chan->state = MBOX_OPEN;
+		tx_chan->seq_number = CHANNEL_START_SEQUENCE_NUMBER;
+		INIT_LIST_HEAD(&tx_chan->tx.pending);
+		INIT_WORK(&tx_chan->open_msg, mbox_handle_open_msg);
+		INIT_WORK(&tx_chan->cast_msg, mbox_handle_cast_msg);
+		INIT_WORK(&tx_chan->receive_msg, mbox_handle_receive_msg);
+		mutex_init(&tx_chan->lock);
+		spin_lock(&mbox_unit->tx_lock);
+		list_add_tail(&tx_chan->list, tx_list);
+		spin_unlock(&mbox_unit->tx_lock);
+	}
+}
+
+static void handle_cast_msg(u16 channel, struct list_head *rx_list,
+						u32 mbox_msg, bool send)
+{
+	struct list_head *pos;
+	struct channel_status *tmp;
+	struct channel_status *rx_chan = NULL;
+	struct mbox_unit_status *mbox_unit = container_of(rx_list,
+						struct mbox_unit_status,
+							 rx_chans);
+	dev_dbg(&channels.pdev->dev, " %s\n", __func__);
+	/* Search for channel in rx list */
+	spin_lock(&mbox_unit->rx_lock);
+	list_for_each(pos, rx_list) {
+		tmp = list_entry(pos, struct channel_status, list);
+		if (tmp->channel == channel)
+			rx_chan = tmp;
+	}
+	spin_unlock(&mbox_unit->rx_lock);
+
+	if (rx_chan) {
+		rx_chan->rx.buffer[0] = mbox_msg;
+		rx_chan->with_ack = send;
+		rx_chan->rx.length = GET_LENGTH(rx_chan->rx.buffer[0]);
+		if (rx_chan->rx.length <= MAILBOX_NR_OF_DATAWORDS &&
+				rx_chan->rx.length > 0) {
+			rx_chan->rx.index = 0;
+			rx_chan->state = MBOX_CAST;
+		}
+		schedule_work(&rx_chan->cast_msg);
+	} else {
+		/* Channel not found, peer sent wrong message */
+		dev_err(&channels.pdev->dev, "channel %d doesn't exist\n",
+			channel);
+	}
+}
+
+/*
+ * This callback is called whenever mbox unit receives data.
+ * priv parameter holds mbox unit id.
+ */
+static void mbox_cb(u32 mbox_msg, void *priv)
+{
+	u8 mbox_id = *(u8 *)priv;
+	struct list_head *rx_list;
+	u8 type = GET_TYPE(mbox_msg);
+	u16 channel = GET_CHANNEL(mbox_msg);
+
+	dev_dbg(&channels.pdev->dev, "%s type %d\t, mbox_msg %x\n",
+		       __func__, type, mbox_msg);
+	/* Get RX channels list for given mbox unit */
+	rx_list = get_rx_list(mbox_id);
+	if (rx_list == NULL) {
+		dev_err(&channels.pdev->dev, "given mbox id is not valid %d\n",
+				mbox_id);
+		return;
+	}
+
+	/* If received message is payload this function will take care of it */
+	if (handle_receive_msg(mbox_msg, rx_list))
+		return;
+
+	/* Received message is header as no RX channel is in SEND/CAST state */
+	switch (type) {
+	case MBOX_CLOSE:
+		/* Not implemented */
+		break;
+	case MBOX_OPEN:
+		handle_open_msg(channel, mbox_id);
+		break;
+	case MBOX_SEND:
+		handle_cast_msg(channel, rx_list, mbox_msg, true);
+		break;
+	case MBOX_CAST:
+		handle_cast_msg(channel, rx_list, mbox_msg, false);
+		break;
+	case MBOX_ACK:
+	case MBOX_NAK:
+		/* Not implemented */
+		break;
+	}
+}
+
+/**
+ * mbox_channel_register() - Registers for a channel
+ * @channel:	Channel Number.
+ * @cb:		Pointer to function pointer mbox_channel_cb_t
+ * @priv:	Pointer to private data
+ *
+ * This routine is used to register for a logical channel.
+ * It first does sanity check on the requested channel availability
+ * and parameters. Then it prepares internal entry for the channel.
+ * And send a OPEN request for that channel.
+ */
+int mbox_channel_register(u16 channel, mbox_channel_cb_t *cb, void *priv)
+{
+	struct channel_status *rx_chan;
+	struct list_head *pos, *rx_list;
+	int res = 0;
+	struct mbox_unit_status *mbox_unit;
+
+	dev_dbg(&channels.pdev->dev, " %s channel = %d\n", __func__, channel);
+	/* Closing of channels is not implemented */
+	if (cb == NULL) {
+		dev_err(&channels.pdev->dev,
+			"channel close is not implemented\n");
+		res = -EINVAL;
+		goto exit;
+	}
+
+	/* Check if provided channel number is valid */
+	if (!check_channel(channel, MBOX_RX)) {
+		dev_err(&channels.pdev->dev, "wrong mbox channel number %d\n",
+			channel);
+		res = -EINVAL;
+		goto exit;
+	}
+
+	rx_list = get_rx_list(get_mbox_id(channel));
+	if (rx_list == NULL) {
+		dev_err(&channels.pdev->dev, "given mbox id is not valid\n");
+		res = -EINVAL;
+		goto exit;
+	}
+
+	mbox_unit = container_of(rx_list, struct mbox_unit_status, rx_chans);
+
+	/* Check if channel is already registered */
+	spin_lock(&mbox_unit->rx_lock);
+	list_for_each(pos, rx_list) {
+		rx_chan = list_entry(pos, struct channel_status, list);
+
+		if (rx_chan->channel == channel) {
+			dev_dbg(&channels.pdev->dev,
+				"channel already registered\n");
+			rx_chan->cb = cb;
+			rx_chan->priv = priv;
+			spin_unlock(&mbox_unit->rx_lock);
+			goto exit;
+		}
+	}
+	spin_unlock(&mbox_unit->rx_lock);
+
+	rx_chan = kzalloc(sizeof(*rx_chan), GFP_KERNEL);
+	if (rx_chan == NULL) {
+		dev_err(&channels.pdev->dev,
+			"couldn't allocate channel status\n");
+		res = -ENOMEM;
+		goto exit;
+	}
+
+	/* Fill out newly allocated element and add it to rx list */
+	rx_chan->channel = channel;
+	rx_chan->cb = cb;
+	rx_chan->priv = priv;
+	rx_chan->seq_number = CHANNEL_START_SEQUENCE_NUMBER;
+	mutex_init(&rx_chan->lock);
+	INIT_LIST_HEAD(&rx_chan->rx.pending);
+	INIT_WORK(&rx_chan->open_msg, mbox_handle_open_msg);
+	INIT_WORK(&rx_chan->cast_msg, mbox_handle_cast_msg);
+	INIT_WORK(&rx_chan->receive_msg, mbox_handle_receive_msg);
+	spin_lock(&mbox_unit->rx_lock);
+	list_add_tail(&rx_chan->list, rx_list);
+	spin_unlock(&mbox_unit->rx_lock);
+
+	mutex_lock(&rx_chan->lock);
+	res = send_pdu(rx_chan, MBOX_OPEN, get_tx_channel(rx_chan->channel));
+	if (res) {
+		dev_err(&channels.pdev->dev, "failed to send OPEN command\n");
+		spin_lock(&mbox_unit->rx_lock);
+		list_del(&rx_chan->list);
+		spin_unlock(&mbox_unit->rx_lock);
+		kfree(rx_chan);
+	} else {
+		rx_chan->seq_number++;
+		rx_chan->state = MBOX_OPEN;
+	}
+	mutex_unlock(&rx_chan->lock);
+exit:
+	return res;
+}
+EXPORT_SYMBOL(mbox_channel_register);
+
+/**
+ * mbox_channel_send() - Send messages
+ * @msg:	Pointer to mbox_channel_msg data structure.
+ *
+ * This routine is used to send messages over the registered logical
+ * TX channel. It first does sanity check on the message paramenters.
+ * It registered channel is not found then it just registers for that
+ * channel. If channel found, it puts the message to the pending list.
+ * If channel is OPEN, it then pushes the message to the mailbox in
+ * FIFO manner from the pending list.
+ */
+int mbox_channel_send(struct mbox_channel_msg *msg)
+{
+	struct list_head *pos, *tx_list;
+	struct channel_status *tmp = NULL;
+	struct channel_status *tx_chan = NULL;
+	struct pending_elem *pending;
+	struct mbox_unit_status *mbox_unit;
+
+	if (msg->length > MAILBOX_NR_OF_DATAWORDS || msg->length == 0) {
+		dev_err(&channels.pdev->dev, "data length incorrect\n");
+		return -EINVAL;
+	}
+
+	if (!check_channel(msg->channel, MBOX_TX)) {
+		dev_err(&channels.pdev->dev, "wrong channel number %d\n",
+			msg->channel);
+		return -EINVAL;
+	}
+
+	tx_list = get_tx_list(get_mbox_id(msg->channel));
+	if (tx_list == NULL) {
+		dev_err(&channels.pdev->dev, "given mbox id is not valid\n");
+		return -EINVAL;
+	}
+
+	mbox_unit = container_of(tx_list, struct mbox_unit_status, tx_chans);
+
+	spin_lock(&mbox_unit->tx_lock);
+	dev_dbg(&channels.pdev->dev, "send:tx_list=%x\tmbox_unit=%x\n",
+			(u32)tx_list, (u32)mbox_unit);
+	list_for_each(pos, tx_list) {
+		tmp = list_entry(pos, struct channel_status, list);
+		if (tmp->channel == msg->channel)
+			tx_chan = tmp;
+	}
+	spin_unlock(&mbox_unit->tx_lock);
+	/* Allocate pending element and add it to the list */
+	pending = kzalloc(sizeof(*pending), GFP_KERNEL);
+	if (pending == NULL) {
+		dev_err(&channels.pdev->dev,
+			"couldn't allocate memory for pending\n");
+		return -ENOMEM;
+	}
+	pending->data = msg->data;
+	pending->length = msg->length;
+
+	if (tx_chan) {
+		mutex_lock(&tx_chan->lock);
+		list_add_tail(&pending->list, &tx_chan->tx.pending);
+		tx_chan->cb = msg->cb;
+		tx_chan->priv = msg->priv;
+		/* If channel is already opened start sending data */
+		if (tx_chan->state == MBOX_OPEN)
+			send_pdu(tx_chan, MBOX_CAST, tx_chan->channel);
+		/* Stop processing here */
+		mutex_unlock(&tx_chan->lock);
+	} else {
+		/* No channel found on the list, allocate new element */
+		tx_chan = kzalloc(sizeof(*tx_chan), GFP_KERNEL);
+		if (tx_chan == NULL) {
+			dev_err(&channels.pdev->dev,
+					"couldn't allocate memory for \
+					tx_chan\n");
+			return -ENOMEM;
+		}
+		tx_chan->channel = msg->channel;
+		tx_chan->cb = msg->cb;
+		tx_chan->priv = msg->priv;
+		tx_chan->state = MBOX_CLOSE;
+		tx_chan->seq_number = CHANNEL_START_SEQUENCE_NUMBER;
+		INIT_LIST_HEAD(&tx_chan->tx.pending);
+		INIT_WORK(&tx_chan->open_msg, mbox_handle_open_msg);
+		INIT_WORK(&tx_chan->cast_msg, mbox_handle_cast_msg);
+		INIT_WORK(&tx_chan->receive_msg, mbox_handle_receive_msg);
+		mutex_init(&tx_chan->lock);
+		spin_lock(&mbox_unit->tx_lock);
+		list_add_tail(&tx_chan->list, tx_list);
+		spin_unlock(&mbox_unit->tx_lock);
+		mutex_lock(&tx_chan->lock);
+		list_add_tail(&pending->list, &tx_chan->tx.pending);
+		mutex_unlock(&tx_chan->lock);
+	}
+	return 0;
+}
+EXPORT_SYMBOL(mbox_channel_send);
+
+static void revoke_pending_msgs(struct channel_status *tx_chan)
+{
+	struct list_head *pos, *n;
+	struct pending_elem *pending;
+
+	list_for_each_safe(pos, n, &tx_chan->tx.pending) {
+		pending = list_entry(pos, struct pending_elem, list);
+
+		if (tx_chan->cb)
+			tx_chan->cb(pending->data, pending->length,
+							tx_chan->priv);
+		else
+			dev_err(&channels.pdev->dev,
+				"%s no callback provided\n", __func__);
+		list_del(&pending->list);
+		kfree(pending);
+	}
+}
+
+/**
+ * mbox_channel_revoke_messages() - Revoke pending messages
+ * @channel:	Channel on which action to be taken.
+ *
+ * This routine Clear all pending messages from TX channel
+ * It searches for the channel.Checks if there is pending
+ * messages.Calls if tehre is any registered function. And
+ * deletes the messages for the pending list.
+ */
+int mbox_channel_revoke_messages(u16 channel)
+{
+	struct list_head *pos, *tx_list;
+	struct channel_status *tmp;
+	struct channel_status *tx_chan = NULL;
+	struct mbox_unit_status *mbox_unit;
+	int res = 0;
+
+	if (!check_channel(channel, MBOX_TX)) {
+		dev_err(&channels.pdev->dev,
+			"wrong channel number %d\n", channel);
+		return -EINVAL;
+	}
+
+	tx_list = get_tx_list(get_mbox_id(channel));
+	if (tx_list == NULL) {
+		dev_err(&channels.pdev->dev, "given mbox id is not valid\n");
+		return -EINVAL;
+	}
+
+	mbox_unit = container_of(tx_list, struct mbox_unit_status, tx_chans);
+
+	spin_lock(&mbox_unit->tx_lock);
+	list_for_each(pos, tx_list) {
+		tmp = list_entry(pos, struct channel_status, list);
+		if (tmp->channel == channel)
+			tx_chan = tmp;
+	}
+	spin_unlock(&mbox_unit->tx_lock);
+
+	if (tx_chan) {
+		mutex_lock(&tx_chan->lock);
+		revoke_pending_msgs(tx_chan);
+		mutex_unlock(&tx_chan->lock);
+		dev_dbg(&channels.pdev->dev, "channel %d cleared\n",
+			channel);
+	} else {
+		dev_err(&channels.pdev->dev, "no channel found\n");
+		res = -EINVAL;
+	}
+
+	dev_dbg(&channels.pdev->dev, "%s exiting %d\n", __func__, res);
+	return res;
+}
+EXPORT_SYMBOL(mbox_channel_revoke_messages);
+
+#if defined(CONFIG_DEBUG_FS)
+#define MBOXTEST_DEBUG 1
+#ifdef MBOXTEST_DEBUG
+#define DBG_TEST(x)  x
+#else
+#define DBG_TEST(x)
+#endif
+
+#define MBOX_TEST_MAX_WORDS	3
+#define MBOX_RX_CHAN		0x500
+#define MBOX_TX_RX_CHANNEL_DIFF 0x400
+#define MBOX_MAX_NUM_TRANSFER	30000
+static int registration_done;
+/**
+ * struct mboxtest_data - mbox test via debugfs information
+ * @rx_buff:		Buffer for incomming data
+ * @rx_pointer:		Ptr to actual RX data buff
+ * @tx_buff:		Buffer for outgoing data
+ * @tx_pointer:		Ptr to actual TX data buff
+ * @tx_done:		TX Transfer done indicator
+ * @rx_done:		RX Transfer done indicator
+ * @received:		Received words
+ * @xfer_words:		Num of bytes in actual trf
+ * @xfers:		Number of transfers
+ * @words:		Number of total words
+ * @channel:		Channel test number
+ */
+struct mboxtest_data {
+	unsigned int		*rx_buff;
+	unsigned int		*rx_pointer;
+	unsigned int		*tx_buff;
+	unsigned int		*tx_pointer;
+	struct completion	tx_done;
+	struct completion	rx_done;
+	int			received;
+	int			xfer_words;
+	int			xfers;
+	int			words;
+	int			channel;
+};
+
+static void mboxtest_receive_cb(u32 *data, u32 len, void *arg)
+{
+	struct mboxtest_data *mboxtest = (struct mboxtest_data *) arg;
+	int i;
+
+	printk(KERN_INFO "receive_cb.. data.= 0x%X, len = %d\n",
+								*data, len);
+
+	for (i = 0; i < len; i++)
+		*(mboxtest->rx_pointer++) = *(data++);
+
+	mboxtest->received += len;
+
+	printk(KERN_INFO "received = %d, words = %d\n",
+				mboxtest->received, mboxtest->words);
+	if (mboxtest->received >= mboxtest->words)
+		complete(&mboxtest->rx_done);
+	dev_dbg(&channels.pdev->dev, "%s exiting\n", __func__);
+}
+
+static void mboxtest_send_cb(u32 *data, u32 len, void *arg)
+{
+	struct mboxtest_data *mboxtest = (struct mboxtest_data *) arg;
+
+	printk(KERN_INFO "send_cb.. data.= 0x%X, len = %d\n",
+							*data, len);
+
+	complete(&mboxtest->tx_done);
+	dev_dbg(&channels.pdev->dev, "kernel:mboxtest_send_cb exiting\n");
+}
+
+static int mboxtest_transmit(struct mboxtest_data *mboxtest)
+{
+	int status = 0;
+	struct mbox_channel_msg msg;
+
+	dev_dbg(&channels.pdev->dev, "%s entering\n", __func__);
+	init_completion(&mboxtest->tx_done);
+
+	msg.channel = mboxtest->channel;
+	msg.data = mboxtest->tx_pointer;
+	msg.length = mboxtest->words;
+	msg.cb = mboxtest_send_cb;
+	msg.priv = mboxtest;
+
+	status = mbox_channel_send(&msg);
+	if (!status) {
+		mboxtest->tx_pointer += mboxtest->xfer_words;
+		wait_for_completion(&mboxtest->tx_done);
+	}
+
+	dev_dbg(&channels.pdev->dev, "%s exiting %d\n",
+			__func__, status);
+	return status;
+}
+
+static int transfer_test(struct mboxtest_data *mboxtest)
+{
+	int status = 0;
+	int len = 0;
+	int i;
+
+	len = mboxtest->words;
+
+	dev_dbg(&channels.pdev->dev, "%s enterring\n", __func__);
+	/* Allocate buffers */
+	mboxtest->rx_buff = kzalloc(sizeof(unsigned int) * len, GFP_KERNEL);
+	if (!mboxtest->rx_buff) {
+		DBG_TEST(printk(KERN_INFO
+			"Cannot allocate mbox rx memory\n"));
+		status = -ENOMEM;
+		goto err1;
+	}
+	memset(mboxtest->rx_buff, '\0', sizeof(unsigned int) * len);
+
+	mboxtest->tx_buff = kzalloc(sizeof(unsigned int) * len, GFP_KERNEL);
+	if (!mboxtest->tx_buff) {
+		DBG_TEST(printk(KERN_INFO
+			"Cannot allocate mbox tx memory\n"));
+		status = -ENOMEM;
+		goto err2;
+	}
+	memset(mboxtest->tx_buff, '\0', sizeof(unsigned int) * len);
+
+	/* Generate data */
+		get_random_bytes((unsigned char *)mboxtest->tx_buff,
+				sizeof(unsigned int) * len);
+	/* Set pointers */
+	mboxtest->tx_pointer = mboxtest->tx_buff;
+	mboxtest->rx_pointer = mboxtest->rx_buff;
+	mboxtest->received = 0;
+	init_completion(&mboxtest->rx_done);
+
+	/* Start tx transfer test transfer */
+	status = mboxtest_transmit(mboxtest);
+	DBG_TEST(printk(KERN_INFO "xfer_words=%d\n",
+				mboxtest->xfer_words));
+	if (!status)
+		wait_for_completion(&mboxtest->rx_done);
+	for (i = 0; i < len; i++)
+		DBG_TEST(printk(KERN_INFO "%d -> TX:0x%X, RX:0x%X\n", i,
+			mboxtest->tx_buff[i], mboxtest->rx_buff[i]));
+
+	dev_dbg(&channels.pdev->dev, "%s exiting %d\n", __func__, status);
+	return status;
+err2:
+	kfree(mboxtest->rx_buff);
+err1:
+	return status;
+}
+
+static int mboxtest_prepare(struct mboxtest_data *mboxtest)
+{
+	int err = 0;
+
+	mboxtest->xfers = MBOX_MAX_NUM_TRANSFER;
+	/* Calculate number of bytes in each transfer */
+	mboxtest->xfer_words = mboxtest->words / mboxtest->xfers;
+
+	/* Trim to maxiumum data words per transfer */
+	if (mboxtest->xfer_words > MBOX_TEST_MAX_WORDS) {
+		DBG_TEST(printk(KERN_INFO "Recalculating xfers ...\n"));
+		mboxtest->xfer_words = MBOX_TEST_MAX_WORDS;
+		if (mboxtest->words % mboxtest->xfer_words)
+			mboxtest->xfers = (mboxtest->words /
+						mboxtest->xfer_words) + 1;
+		else
+			mboxtest->xfers = (mboxtest->words /
+						mboxtest->xfer_words);
+	}
+
+	DBG_TEST(printk(KERN_INFO "Params: chan=0x%X words=%d, xfers=%d\n",
+			mboxtest->channel, mboxtest->words,
+						mboxtest->xfers));
+
+	if (mbox_channel_register(mboxtest->channel,
+			mboxtest_receive_cb, mboxtest)) {
+		DBG_TEST(printk(KERN_INFO "Cannot register mbox channel\n"));
+		err = -ENOMEM;
+		goto err;
+	}
+
+	registration_done = true;
+	return 0;
+err:
+	kfree(mboxtest);
+	return err;
+}
+
+struct mboxtest_data mboxtest;
+/*
+ * Expected input: <nbr_channel> <nbr_word>
+ * Example: "echo 500 2"
+ */
+static ssize_t mbox_write_channel(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf,
+			       size_t count)
+{
+	unsigned long nbr_channel;
+	unsigned long nbr_word;
+	char int_buf[16];
+	char *token;
+	char *val;
+
+	strncpy((char *) &int_buf, buf, sizeof(int_buf));
+	token = (char *) &int_buf;
+
+	/* Parse message */
+	val = strsep(&token, " ");
+	if ((val == NULL) || (strict_strtoul(val, 16, &nbr_channel) != 0))
+		nbr_channel = MBOX_RX_CHAN;
+
+	val = strsep(&token, " ");
+	if ((val == NULL) || (strict_strtoul(val, 16, &nbr_word) != 0))
+		nbr_word = 2;
+
+	dev_dbg(dev, "Will setup logical channel %ld\n", nbr_channel);
+	mboxtest.channel = nbr_channel;
+	mboxtest.words = nbr_word;
+
+	if (!registration_done)
+		mboxtest_prepare(&mboxtest);
+	else
+		dev_dbg(&channels.pdev->dev, "already registration done\n");
+
+	return count;
+}
+
+static ssize_t mbox_read_channel(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+
+	unsigned long i;
+	static bool config_done;
+
+	if (!config_done) {
+		config_done = true;
+		mboxtest.channel += MBOX_TX_RX_CHANNEL_DIFF;
+	}
+	dev_dbg(dev, "Will transfer %d words %d times at channel 0x%x\n",
+			mboxtest.words, mboxtest.xfers, mboxtest.channel);
+	for (i = 0; i < mboxtest.xfers; i++)
+		transfer_test(&mboxtest);
+
+	return 1;
+}
+static DEVICE_ATTR(channel, S_IWUGO | S_IRUGO, mbox_read_channel,
+		mbox_write_channel);
+
+#endif
+
+static int __init mbox_channel_probe(struct platform_device *pdev)
+{
+	int i, ret = 0;
+	struct mbox *mbox;
+
+	dev_dbg(&(pdev->dev), "Probing mailbox (pdev = 0x%X)...\n", (u32)pdev);
+
+	/* Register to given mailbox units (ids) */
+	for (i = 0; i < ARRAY_SIZE(mbox_ids); i++) {
+		mbox = mbox_setup(mbox_ids[i], mbox_cb, &mbox_ids[i]);
+		if (mbox == NULL) {
+			dev_err(&(pdev->dev), "Unable to setup mailbox %d\n",
+				mbox_ids[i]);
+			ret = -EBUSY;
+			goto exit;
+		}
+		channels.mbox_unit[i].mbox_id = mbox_ids[i];
+		channels.mbox_unit[i].mbox = mbox;
+		INIT_LIST_HEAD(&channels.mbox_unit[i].rx_chans);
+		INIT_LIST_HEAD(&channels.mbox_unit[i].tx_chans);
+		spin_lock_init(&channels.mbox_unit[i].rx_lock);
+		spin_lock_init(&channels.mbox_unit[i].tx_lock);
+	}
+
+	channels.pdev = pdev;
+
+	dev_dbg(&(pdev->dev), "Mailbox channel driver loaded\n");
+#if defined(CONFIG_DEBUG_FS)
+	ret = device_create_file(&(pdev->dev), &dev_attr_channel);
+	if (ret != 0)
+		dev_warn(&(pdev->dev),
+			 "Unable to create mbox_channel sysfs entry");
+
+
+#endif
+exit:
+	return ret;
+}
+
+static struct platform_driver mbox_channel_driver = {
+	.driver = {
+		.name = "mbox_channel",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init mbox_channel_init(void)
+{
+	if (!machine_is_u5500())
+		return 0;
+
+	platform_device_register_simple("mbox_channel", 0, NULL, 0);
+
+	return platform_driver_probe(&mbox_channel_driver, mbox_channel_probe);
+}
+module_init(mbox_channel_init);
+
+static void __exit mbox_channel_exit(void)
+{
+	platform_driver_unregister(&mbox_channel_driver);
+}
+module_exit(mbox_channel_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MBOX channels driver");
diff --git a/drivers/misc/sim_detect.c b/drivers/misc/sim_detect.c
new file mode 100644
index 00000000000..c267fe95ee3
--- /dev/null
+++ b/drivers/misc/sim_detect.c
@@ -0,0 +1,164 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: BIBEK BASU <bibek.basu@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+#include <linux/workqueue.h>
+#include <linux/modem/modem_client.h>
+#include <mach/sim_detect.h>
+
+/* time in millisec */
+#define TIMER_DELAY	10
+struct sim_detect{
+	struct work_struct	timer_expired;
+	struct device	*dev;
+	struct modem *modem;
+	struct hrtimer timer;
+};
+
+static void inform_modem_release(struct work_struct *work)
+{
+	struct sim_detect *sim_detect =
+		container_of(work, struct sim_detect, timer_expired);
+
+	/* call Modem Access Framework api to release modem */
+	modem_release(sim_detect->modem);
+}
+
+static enum hrtimer_restart timer_callback(struct hrtimer *timer)
+{
+	struct sim_detect *sim_detect =
+		container_of(timer, struct sim_detect, timer);
+
+	schedule_work(&sim_detect->timer_expired);
+	return HRTIMER_NORESTART;
+}
+
+static irqreturn_t sim_activity_irq(int irq, void *dev)
+{
+	struct sim_detect *sim_detect = dev;
+
+	/* call Modem Access Framework api to acquire modem */
+	modem_request(sim_detect->modem);
+	/* start the timer for 10ms */
+	hrtimer_start(&sim_detect->timer,
+			ktime_set(0, TIMER_DELAY*NSEC_PER_MSEC),
+			HRTIMER_MODE_REL);
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_PM
+/**
+ * sim_detect_suspend() - This routine puts the Sim detect in to sustend state.
+ * @dev:	pointer to device structure.
+ *
+ * This routine checks the current ongoing communication with Modem by
+ * examining the modem_get_usage and work_pending state.
+ * accordingly prevents suspend if modem communication
+ * is on-going.
+ */
+int sim_detect_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct sim_detect *sim_detect = platform_get_drvdata(pdev);
+
+	dev_dbg(&pdev->dev, "%s called...\n", __func__);
+	/* if modem is accessed, event system suspend */
+	if (modem_get_usage(sim_detect->modem)
+			|| work_pending(&sim_detect->timer_expired))
+		return -EBUSY;
+	else
+		return 0;
+}
+
+static const struct dev_pm_ops sim_detect_dev_pm_ops = {
+	.suspend = sim_detect_suspend,
+};
+#endif
+
+static int __devinit sim_detect_probe(struct platform_device *pdev)
+{
+	struct sim_detect_platform_data *plat = dev_get_platdata(&pdev->dev);
+	struct sim_detect *sim_detect;
+	int ret;
+
+	sim_detect = kzalloc(sizeof(struct sim_detect), GFP_KERNEL);
+	if (sim_detect == NULL) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+	sim_detect->dev = &pdev->dev;
+	INIT_WORK(&sim_detect->timer_expired, inform_modem_release);
+	hrtimer_init(&sim_detect->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	sim_detect->timer.function = timer_callback;
+	sim_detect->modem = modem_get(sim_detect->dev, "u8500-shrm-modem");
+	if (IS_ERR(sim_detect->modem)) {
+		ret = PTR_ERR(sim_detect->modem);
+		dev_err(sim_detect->dev, "Could not retrieve the modem\n");
+		goto out_free;
+	}
+	platform_set_drvdata(pdev, sim_detect);
+	ret = request_threaded_irq(plat->irq_num,
+		       NULL, sim_activity_irq,
+		       IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+		       "sim activity", sim_detect);
+	if (ret < 0)
+		goto out_free_irq;
+	return 0;
+out_free_irq:
+	modem_put(sim_detect->modem);
+	platform_set_drvdata(pdev, NULL);
+out_free:
+	kfree(sim_detect);
+	return ret;
+}
+
+static int __devexit sim_detect_remove(struct platform_device *pdev)
+{
+	struct sim_detect *sim_detect = platform_get_drvdata(pdev);
+
+	modem_put(sim_detect->modem);
+	platform_set_drvdata(pdev, NULL);
+	kfree(sim_detect);
+	return 0;
+}
+
+static struct platform_driver sim_detect_driver = {
+	.driver = {
+		.name = "sim_detect",
+		.owner = THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm = &sim_detect_dev_pm_ops,
+#endif
+	},
+	.probe = sim_detect_probe,
+	.remove = __devexit_p(sim_detect_remove),
+};
+
+static int __init sim_detect_init(void)
+{
+	return platform_driver_register(&sim_detect_driver);
+}
+module_init(sim_detect_init);
+
+static void __exit sim_detect_exit(void)
+{
+	platform_driver_unregister(&sim_detect_driver);
+}
+module_exit(sim_detect_exit);
+
+MODULE_AUTHOR("BIBEK BASU <bibek.basu@stericsson.com>");
+MODULE_DESCRIPTION("Detects SIM Hot Swap and wakes modem");
+MODULE_ALIAS("SIM DETECT INTERRUPT driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/misc/stm.c b/drivers/misc/stm.c
new file mode 100644
index 00000000000..a05b2d14573
--- /dev/null
+++ b/drivers/misc/stm.c
@@ -0,0 +1,797 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ *         Philippe Langlais <philippe.Langlais@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/cdev.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/debugfs.h>
+#include <trace/stm.h>
+
+/* STM Registers */
+#define STM_CR          (stm.virtbase)
+#define STM_MMC         (stm.virtbase + 0x008)
+#define STM_TER         (stm.virtbase + 0x010)
+#define STMPERIPHID0    (stm.virtbase + 0xFC0)
+#define STMPERIPHID1    (stm.virtbase + 0xFC8)
+#define STMPERIPHID2    (stm.virtbase + 0xFD0)
+#define STMPERIPHID3    (stm.virtbase + 0xFD8)
+#define STMPCELLID0     (stm.virtbase + 0xFE0)
+#define STMPCELLID1     (stm.virtbase + 0xFE8)
+#define STMPCELLID2     (stm.virtbase + 0xFF0)
+#define STMPCELLID3     (stm.virtbase + 0xFF8)
+
+#define STM_CLOCK_SHIFT 6
+#define STM_CLOCK_MASK  0x1C0
+
+/* Hardware mode for all sources */
+#define STM_MMC_DEFAULT            0xFFFFFFFF
+
+/* Max number of channels (multiple of 256) */
+#define STM_NUMBER_OF_CHANNEL      CONFIG_STM_NUMBER_OF_CHANNEL
+
+/* # dynamically allocated channel with stm_trace_buffer */
+#define NB_KERNEL_DYNAMIC_CHANNEL  128
+
+static struct stm_device {
+	const struct stm_platform_data *pdata;
+	void __iomem *virtbase;
+	/* Used to register the allocated channels */
+	DECLARE_BITMAP(ch_bitmap, STM_NUMBER_OF_CHANNEL);
+} stm;
+
+volatile struct stm_channel __iomem *stm_channels;
+
+static struct cdev cdev;
+static struct class *stm_class;
+static int stm_major;
+
+static DEFINE_SPINLOCK(lock);
+
+/* Middle value for clock divisor */
+static enum clock_div stm_clockdiv = STM_CLOCK_DIV8;
+
+/* Default value for STM output connection */
+static enum stm_connection_type stm_connection = STM_DEFAULT_CONNECTION;
+
+#define STM_BUFSIZE	256
+struct channel_data {
+	DECLARE_BITMAP(bitmap, STM_NUMBER_OF_CHANNEL);
+	int numero;
+	spinlock_t lock;
+	u8 data_buffer[STM_BUFSIZE];
+};
+
+static u64 stm_printk_buf[1024/sizeof(u64)];
+static arch_spinlock_t stm_buf_lock =
+		(arch_spinlock_t)__ARCH_SPIN_LOCK_UNLOCKED;
+
+int stm_alloc_channel(int offset)
+{
+	int channel;
+
+	/* Look for a free channel from offset */
+	do {
+		channel = find_next_zero_bit(stm.ch_bitmap,
+				STM_NUMBER_OF_CHANNEL, offset);
+	} while ((channel < STM_NUMBER_OF_CHANNEL)
+	       && test_and_set_bit(channel, stm.ch_bitmap));
+	return channel;
+}
+EXPORT_SYMBOL(stm_alloc_channel);
+
+void stm_free_channel(int channel)
+{
+	clear_bit(channel, stm.ch_bitmap);
+}
+EXPORT_SYMBOL(stm_free_channel);
+
+static int stm_get_channel(struct channel_data *ch_data, int __user *arg)
+{
+	int channel, err;
+
+	channel = stm_alloc_channel(0);
+	if (channel < STM_NUMBER_OF_CHANNEL) {
+		/* One free found ! */
+		err = put_user(channel, arg);
+		if (err)
+			stm_free_channel(channel);
+		else
+			/* Register it in the context of the file */
+			set_bit(channel, ch_data->bitmap);
+	} else
+		err = -ENOMEM;
+	return err;
+}
+
+static int stm_release_channel(struct channel_data *ch_data, int channel)
+{
+	if ((channel < 0) || (channel >= STM_NUMBER_OF_CHANNEL))
+		return -EINVAL;
+	stm_free_channel(channel);
+	clear_bit(channel, ch_data->bitmap);
+	return 0;
+}
+
+/*
+ * Trace a buffer on a given channel
+ * with auto time stamping on last byte(s) only
+ */
+int stm_trace_buffer_onchannel(int channel,
+			       const void *data, size_t length)
+{
+	int i, mod64;
+	volatile struct stm_channel __iomem *pch;
+
+	if (channel >= STM_NUMBER_OF_CHANNEL || !stm_channels)
+		return 0;
+
+	pch = &stm_channels[channel];
+
+	/*  Align data pointer to u64 & time stamp last byte(s) */
+	mod64 = (int)data & 7;
+	i = length - 8 + mod64;
+	switch (mod64) {
+	case 0:
+		if (i)
+			pch->no_stamp64 = *(u64 *)data;
+		else {
+			pch->stamp64 = *(u64 *)data;
+			return length;
+		}
+		data += 8;
+		break;
+	case 1:
+		pch->no_stamp8 = *(u8 *)data;
+		pch->no_stamp16 = *(u16 *)(data+1);
+		if (i)
+			pch->no_stamp32 = *(u32 *)(data+3);
+		else {
+			pch->stamp32 = *(u32 *)(data+3);
+			return length;
+		}
+		data += 7;
+		break;
+	case 2:
+		pch->no_stamp16 = *(u16 *)data;
+		if (i)
+			pch->no_stamp32 = *(u32 *)(data+2);
+		else {
+			pch->stamp32 = *(u32 *)(data+2);
+			return length;
+		}
+		data += 6;
+		break;
+	case 3:
+		pch->no_stamp8 = *(u8 *)data;
+		if (i)
+			pch->no_stamp32 = *(u32 *)(data+1);
+		else {
+			pch->stamp32 = *(u32 *)(data+1);
+			return length;
+		}
+		data += 5;
+		break;
+	case 4:
+		if (i)
+			pch->no_stamp32 = *(u32 *)data;
+		else {
+			pch->stamp32 = *(u32 *)data;
+			return length;
+		}
+		data += 4;
+		break;
+	case 5:
+		pch->no_stamp8 = *(u8 *)data;
+		if (i)
+			pch->no_stamp16 = *(u16 *)(data+1);
+		else {
+			pch->stamp16 = *(u16 *)(data+1);
+			return length;
+		}
+		data += 3;
+		break;
+	case 6:
+		if (i)
+			pch->no_stamp16 = *(u16 *)data;
+		else {
+			pch->stamp16 = *(u16 *)data;
+			return length;
+		}
+		data += 2;
+		break;
+	case 7:
+		if (i)
+			pch->no_stamp8 = *(u8 *)data;
+		else {
+			pch->stamp8 = *(u8 *)data;
+			return length;
+		}
+		data++;
+		break;
+	}
+	for (;;) {
+		if (i > 8) {
+			pch->no_stamp64 = *(u64 *)data;
+			data += 8;
+			i -= 8;
+		} else if (i == 8) {
+			pch->stamp64 = *(u64 *)data;
+			break;
+		} else if (i > 4) {
+			pch->no_stamp32 = *(u32 *)data;
+			data += 4;
+			i -= 4;
+		} else if (i == 4) {
+			pch->stamp32 = *(u32 *)data;
+			break;
+		} else if (i > 2) {
+			pch->no_stamp16 = *(u16 *)data;
+			data += 2;
+			i -= 2;
+		} else if (i == 2) {
+			pch->stamp16 = *(u16 *)data;
+			break;
+		} else {
+			pch->stamp8 = *(u8 *)data;
+			break;
+		}
+	}
+	return length;
+}
+EXPORT_SYMBOL(stm_trace_buffer_onchannel);
+
+static int stm_open(struct inode *inode, struct file *file)
+{
+	struct channel_data *channel_data;
+
+	channel_data = kzalloc(sizeof(struct channel_data), GFP_KERNEL);
+	if (channel_data == NULL)
+		return -ENOMEM;
+
+	spin_lock_init(&channel_data->lock);
+	channel_data->numero = -1;   /*  Channel not yet allocated */
+	file->private_data = channel_data;
+
+	return 0;
+}
+
+static int stm_release(struct inode *inode, struct file *file)
+{
+	struct channel_data *channel;
+
+	channel = (struct channel_data *)file->private_data;
+
+	/* Free allocated channel if necessary */
+	if (channel->numero != -1)
+		stm_free_channel(channel->numero);
+
+	bitmap_andnot(stm.ch_bitmap, stm.ch_bitmap,
+			channel->bitmap, STM_NUMBER_OF_CHANNEL);
+
+	kfree(channel);
+	return 0;
+}
+
+static ssize_t stm_write(struct file *file, const char __user *buf,
+		  size_t size, loff_t *off)
+{
+	struct channel_data *channel = file->private_data;
+
+	/*  Alloc channel at first write */
+	if (channel->numero == -1) {
+		channel->numero = stm_alloc_channel(0);
+		if (channel->numero > STM_NUMBER_OF_CHANNEL)
+			return -ENOMEM;
+	}
+
+	if (size > STM_BUFSIZE)
+		size = STM_BUFSIZE;
+
+	spin_lock(&channel->lock);
+
+	if (copy_from_user
+	    (channel->data_buffer, (void __user *) buf, size)) {
+		spin_unlock(&channel->lock);
+		return -EFAULT;
+	}
+	size = stm_trace_buffer_onchannel(channel->numero,
+			channel->data_buffer, size);
+
+	spin_unlock(&channel->lock);
+
+	return size;
+}
+
+static int stm_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	/*
+	 * Don't allow a mapping that covers more than the STM channels
+	 */
+	if ((vma->vm_end - vma->vm_start) >
+	    STM_NUMBER_OF_CHANNEL*sizeof(struct stm_channel))
+		return -EINVAL;
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	if (io_remap_pfn_range(vma, vma->vm_start,
+			stm.pdata->channels_phys_base>>PAGE_SHIFT,
+			STM_NUMBER_OF_CHANNEL*sizeof(struct stm_channel),
+			vma->vm_page_prot))
+		return -EAGAIN;
+
+	return 0;
+}
+
+/* Enable the trace for given sources (bitfield) */
+static void stm_enable_src(unsigned int v)
+{
+	unsigned int cr_val;
+	spin_lock(&lock);
+	cr_val = readl(STM_CR);
+	cr_val &= ~STM_CLOCK_MASK;
+	writel(cr_val|(stm_clockdiv<<STM_CLOCK_SHIFT), STM_CR);
+	writel(v, STM_TER);
+	spin_unlock(&lock);
+}
+
+/* Disable all sources */
+static void stm_disable_src(void)
+{
+	writel(0x0, STM_CR);   /* stop clock */
+	writel(0x0, STM_TER);  /* Disable cores */
+}
+
+/* Set clock speed */
+static int stm_set_ckdiv(enum clock_div v)
+{
+	unsigned int val;
+
+	spin_lock(&lock);
+	val = readl(STM_CR);
+	val &= ~STM_CLOCK_MASK;
+	writel(val | ((v << STM_CLOCK_SHIFT) & STM_CLOCK_MASK), STM_CR);
+	spin_unlock(&lock);
+	stm_clockdiv = v;
+
+	return 0;
+}
+
+/* Return the control register */
+static inline unsigned int stm_get_cr(void)
+{
+	return readl(STM_CR);
+}
+
+/*
+ * Set Trace MODE lossless/lossy (Software/Hardware)
+ * each bit represent the corresponding mode of this source
+ */
+static inline void stm_set_modes(unsigned int modes)
+{
+	writel(modes, STM_MMC);
+}
+
+/* Get Trace MODE lossless/lossy (Software/Hardware)
+ * each bit represent the corresponding mode of this source */
+static inline unsigned int stm_get_modes(void)
+{
+	return readl(STM_MMC);
+}
+
+/* Count # of free channels */
+static int stm_nb_free_channels(void)
+{
+	int nb_channels, offset;
+
+	nb_channels = 0;
+	offset = 0;
+	for (;;) {
+		offset = find_next_zero_bit(stm.ch_bitmap,
+				STM_NUMBER_OF_CHANNEL, offset);
+		if (offset == STM_NUMBER_OF_CHANNEL)
+			break;
+		offset++;
+		nb_channels++;
+	}
+	return nb_channels;
+}
+
+static long stm_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	int err = 0;
+	struct channel_data *channel = file->private_data;
+
+	switch (cmd) {
+
+	case STM_CONNECTION:
+		if (stm.pdata->stm_connection)
+			stm.pdata->stm_connection(arg);
+		stm_connection = arg;
+		break;
+
+	case STM_DISABLE:
+		stm_disable_src();
+		break;
+
+	case STM_GET_NB_MAX_CHANNELS:
+		err = put_user(STM_NUMBER_OF_CHANNEL, (unsigned int *)arg);
+		break;
+
+	case STM_GET_NB_FREE_CHANNELS:
+		err = put_user(stm_nb_free_channels(), (unsigned int *)arg);
+		break;
+
+	case STM_GET_CHANNEL_NO:
+		err = put_user(channel->numero, (unsigned int *)arg);
+		break;
+
+	case STM_SET_CLOCK_DIV:
+		err = stm_set_ckdiv((enum clock_div) arg);
+		break;
+
+	case STM_SET_MODE:
+		stm_set_modes(arg);
+		break;
+
+	case STM_GET_MODE:
+		err = put_user(stm_get_modes(), (unsigned int *)arg);
+		break;
+
+	case STM_GET_CTRL_REG:
+		err = put_user(stm_get_cr(), (unsigned int *)arg);
+		break;
+
+	case STM_ENABLE_SRC:
+		stm_enable_src(arg);
+		break;
+
+	case STM_GET_FREE_CHANNEL:
+		err = stm_get_channel(channel, (int *)arg);
+		break;
+
+	case STM_RELEASE_CHANNEL:
+		err = stm_release_channel(channel, arg);
+		break;
+
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	return err;
+}
+
+/*
+ * Trace a buffer on a dynamically allocated channel
+ * with auto time stamping on the first byte(s) only
+ * Dynamic channel number >=
+ *     STM_NUMBER_OF_CHANNEL - NB_KERNEL_DYNAMIC_CHANNEL
+ */
+int stm_trace_buffer(const void *data, size_t length)
+{
+	int channel;
+
+	channel = stm_alloc_channel(STM_NUMBER_OF_CHANNEL
+			- NB_KERNEL_DYNAMIC_CHANNEL);
+	if (channel < STM_NUMBER_OF_CHANNEL) {
+		length = stm_trace_buffer_onchannel(channel, data, length);
+		stm_free_channel(channel);
+		return length;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(stm_trace_buffer);
+
+static const struct file_operations stm_fops = {
+	.owner =		THIS_MODULE,
+	.unlocked_ioctl =	stm_ioctl,
+	.open =			stm_open,
+	.llseek =		no_llseek,
+	.write =		stm_write,
+	.release =		stm_release,
+	.mmap =			stm_mmap,
+};
+
+/*
+ * Init and deinit driver
+ */
+
+static int __devinit stm_probe(struct platform_device *pdev)
+{
+	int retval = 0;
+
+	if (!pdev || !pdev->dev.platform_data)  {
+		pr_alert("No device/platform_data found on STM driver\n");
+		return -ENODEV;
+	}
+
+	stm.pdata = pdev->dev.platform_data;
+
+	cdev_init(&cdev, &stm_fops);
+	cdev.owner = THIS_MODULE;
+
+	stm_channels =
+		ioremap_nocache(stm.pdata->channels_phys_base,
+			STM_NUMBER_OF_CHANNEL*sizeof(*stm_channels));
+	if (stm_channels == NULL) {
+		dev_err(&pdev->dev, "could not remap STM Msg register\n");
+		return -ENODEV;
+	}
+
+	stm.virtbase = ioremap_nocache(stm.pdata->regs_phys_base, SZ_4K);
+	if (stm.virtbase == NULL) {
+		retval = -EIO;
+		dev_err(&pdev->dev, "could not remap STM Register\n");
+		goto err_channels;
+	}
+
+	retval = cdev_add(&cdev, MKDEV(stm_major, 0), 1);
+	if (retval) {
+		dev_err(&pdev->dev, "chardev registration failed\n");
+		goto err_channels;
+	}
+
+	if (IS_ERR(device_create(stm_class, &pdev->dev,
+				 MKDEV(stm_major, 0), NULL, STM_DEV_NAME)))
+		dev_err(&pdev->dev, "can't create device\n");
+
+	/* Check chip IDs if necessary */
+	if (stm.pdata->id_mask) {
+		u32 periph_id, cell_id;
+
+		periph_id = (readb(STMPERIPHID3)<<24) +
+				 (readb(STMPERIPHID2)<<16) +
+				 (readb(STMPERIPHID1)<<8) +
+				 readb(STMPERIPHID0);
+		cell_id = (readb(STMPCELLID3)<<24) +
+				 (readb(STMPCELLID2)<<16) +
+				 (readb(STMPCELLID1)<<8) +
+				 readb(STMPCELLID0);
+		/* Only warns if it isn't a ST-Ericsson supported one */
+		if ((periph_id & stm.pdata->id_mask) != 0x00080dec ||
+		    cell_id != 0xb105f00d) {
+			dev_warn(&pdev->dev, "STM-Trace IC not compatible\n");
+			dev_warn(&pdev->dev, "periph_id=%x\n", periph_id);
+			dev_warn(&pdev->dev, "pcell_id=%x\n", cell_id);
+		}
+	}
+
+	/* Reserve channels if necessary */
+	if (stm.pdata->channels_reserved_sz) {
+		int i;
+
+		for (i = 0; i < stm.pdata->channels_reserved_sz; i++) {
+			set_bit(stm.pdata->channels_reserved[i],
+					stm.ch_bitmap);
+		}
+	}
+	/* Reserve kernel trace channels on demand */
+#ifdef CONFIG_STM_PRINTK
+	set_bit(CONFIG_STM_PRINTK_CHANNEL, stm.ch_bitmap);
+#endif
+#ifdef CONFIG_STM_FTRACE
+	set_bit(CONFIG_STM_FTRACE_CHANNEL, stm.ch_bitmap);
+#endif
+#ifdef CONFIG_STM_CTX_SWITCH
+	set_bit(CONFIG_STM_CTX_SWITCH_CHANNEL, stm.ch_bitmap);
+#endif
+#ifdef CONFIG_STM_WAKEUP
+	set_bit(CONFIG_STM_WAKEUP_CHANNEL, stm.ch_bitmap);
+#endif
+#ifdef CONFIG_STM_STACK_TRACE
+	set_bit(CONFIG_STM_STACK_TRACE_CHANNEL, stm.ch_bitmap);
+#endif
+#ifdef CONFIG_STM_TRACE_PRINTK
+	set_bit(CONFIG_STM_TRACE_PRINTK_CHANNEL, stm.ch_bitmap);
+	set_bit(CONFIG_STM_TRACE_BPRINTK_CHANNEL, stm.ch_bitmap);
+#endif
+
+	if (stm.pdata->stm_connection) {
+		retval = stm.pdata->stm_connection(stm_connection);
+		if (retval) {
+			dev_err(&pdev->dev, "failed to connect STM output\n");
+			goto err_channels;
+		}
+	}
+
+	/* Enable STM Masters given in pdata */
+	if (stm.pdata->masters_enabled)
+		stm_enable_src(stm.pdata->masters_enabled);
+
+	stm_set_modes(STM_MMC_DEFAULT); /* Set all sources in HW mode */
+
+	dev_info(&pdev->dev, "STM-Trace driver probed successfully\n");
+	stm_printk("STM-Trace driver initialized\n");
+	return 0;
+
+err_channels:
+	iounmap(stm_channels);
+	return retval;
+}
+
+static int __devexit stm_remove(struct platform_device *pdev)
+{
+	device_destroy(stm_class, MKDEV(stm_major, 0));
+	cdev_del(&cdev);
+
+	if (stm.pdata->stm_connection)
+		(void) stm.pdata->stm_connection(STM_DISCONNECT);
+
+	stm_disable_src();
+	iounmap(stm.virtbase);
+	iounmap(stm_channels);
+
+	return 0;
+}
+
+int stm_printk(const char *fmt, ...)
+{
+	int ret;
+	size_t size;
+	va_list args;
+
+	va_start(args, fmt);
+	arch_spin_lock(&stm_buf_lock);
+	size = vscnprintf((char *)stm_printk_buf,
+			sizeof(stm_printk_buf), fmt, args);
+	ret = stm_trace_buffer(stm_printk_buf, size);
+	arch_spin_unlock(&stm_buf_lock);
+	va_end(args);
+	return ret;
+}
+EXPORT_SYMBOL(stm_printk);
+
+/*
+ * Debugfs interface
+ */
+
+static int stm_connection_show(void *data, u64 *val)
+{
+	*val = stm_connection;
+	return 0;
+}
+
+static int stm_connection_set(void *data, u64 val)
+{
+	if (stm.pdata->stm_connection) {
+		stm_connection = val;
+		stm.pdata->stm_connection(val);
+	}
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(stm_connection_fops, stm_connection_show,
+		stm_connection_set, "%llu\n");
+
+static int stm_clockdiv_show(void *data, u64 *val)
+{
+	*val = stm_clockdiv;
+	return 0;
+}
+
+static int stm_clockdiv_set(void *data, u64 val)
+{
+	stm_set_ckdiv(val);
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(stm_clockdiv_fops, stm_clockdiv_show,
+		stm_clockdiv_set, "%llu\n");
+
+static int stm_masters_enable_show(void *data, u64 *val)
+{
+	*val = readl(STM_TER);
+	return 0;
+}
+
+static int stm_masters_enable_set(void *data, u64 val)
+{
+	stm_enable_src(val);
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(stm_masters_enable_fops, stm_masters_enable_show,
+		stm_masters_enable_set, "%08llx\n");
+
+static int stm_masters_modes_show(void *data, u64 *val)
+{
+	*val = stm_get_modes();
+	return 0;
+}
+
+static int stm_masters_modes_set(void *data, u64 val)
+{
+	stm_set_modes(val);
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(stm_masters_modes_fops, stm_masters_modes_show,
+		stm_masters_modes_set, "%08llx\n");
+
+/* Count # of free channels */
+static int stm_free_channels_show(void *data, u64 *val)
+{
+	*val = stm_nb_free_channels();
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(stm_free_channels_fops, stm_free_channels_show,
+		NULL, "%lld\n");
+
+static __init int stm_init_debugfs(void)
+{
+	struct dentry *d_stm;
+
+	d_stm =  debugfs_create_dir(STM_DEV_NAME, NULL);
+	if (!d_stm)
+		return -ENOMEM;
+
+	(void) debugfs_create_file("connection",  S_IRUGO | S_IWUGO, d_stm,
+				NULL, &stm_connection_fops);
+	(void) debugfs_create_file("clockdiv", S_IRUGO | S_IWUGO, d_stm,
+				NULL, &stm_clockdiv_fops);
+	(void) debugfs_create_file("masters_enable", S_IRUGO | S_IWUGO, d_stm,
+				NULL, &stm_masters_enable_fops);
+	(void) debugfs_create_file("masters_modes", S_IRUGO | S_IWUGO, d_stm,
+				NULL, &stm_masters_modes_fops);
+	(void) debugfs_create_file("free_channels", S_IRUGO, d_stm,
+				NULL, &stm_free_channels_fops);
+	return 0;
+}
+fs_initcall(stm_init_debugfs);
+
+static struct platform_driver stm_driver = {
+	.probe = stm_probe,
+	.remove = __devexit_p(stm_remove),
+	.driver = {
+		.name = STM_DEV_NAME,
+		.owner = THIS_MODULE,
+	}
+};
+
+static int __init stm_init(void)
+{
+	int retval;
+	dev_t dev;
+
+	stm_class = class_create(THIS_MODULE, STM_DEV_NAME);
+	if (IS_ERR(stm_class)) {
+		pr_err("stm: can't register stm class\n");
+		return PTR_ERR(stm_class);
+	}
+
+	retval = alloc_chrdev_region(&dev, 0, 1, STM_DEV_NAME);
+	if (retval) {
+		pr_err("stm: can't register character device\n");
+		class_destroy(stm_class);
+		return retval;
+	}
+	stm_major = MAJOR(dev);
+	return platform_driver_register(&stm_driver);
+}
+
+static void __exit stm_exit(void)
+{
+	platform_driver_unregister(&stm_driver);
+	unregister_chrdev_region(MKDEV(stm_major, 0), 1);
+	class_destroy(stm_class);
+}
+
+arch_initcall(stm_init);   /* STM init ASAP need to wait GPIO init */
+module_exit(stm_exit);
+
+MODULE_AUTHOR("Paul Ghaleb - ST Microelectronics");
+MODULE_AUTHOR("Pierre Peiffer - ST-Ericsson");
+MODULE_AUTHOR("Philippe Langlais - ST-Ericsson");
+MODULE_DESCRIPTION("System Trace Module driver");
+MODULE_ALIAS("stm");
+MODULE_ALIAS("stm-trace");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index c779503e757..09e85160f05 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -521,7 +521,10 @@ static u32 mmc_sd_num_wr_blocks(struct mmc_card *card)
 	return result;
 }
 
-static u32 get_card_status(struct mmc_card *card, struct request *req)
+static int get_card_status(struct mmc_card *card,
+				struct request *req,
+				u32 *status,
+				int retries)
 {
 	struct mmc_command cmd = {0};
 	int err;
@@ -530,11 +533,38 @@ static u32 get_card_status(struct mmc_card *card, struct request *req)
 	if (!mmc_host_is_spi(card->host))
 		cmd.arg = card->rca << 16;
 	cmd.flags = MMC_RSP_SPI_R2 | MMC_RSP_R1 | MMC_CMD_AC;
-	err = mmc_wait_for_cmd(card->host, &cmd, 0);
+	err = mmc_wait_for_cmd(card->host, &cmd, retries);
 	if (err)
 		printk(KERN_ERR "%s: error %d sending status command",
 		       req->rq_disk->disk_name, err);
-	return cmd.resp[0];
+	else
+		*status = cmd.resp[0];
+
+	return err;
+}
+
+static int wait_for_ready_state(struct mmc_card *card, struct request *req)
+{
+	u32 status;
+	int err = 0;
+
+	if (!mmc_host_is_spi(card->host) && rq_data_dir(req) != READ) {
+
+		do {
+			err = get_card_status(card, req, &status, 5);
+			if (err)
+				break;
+
+			/*
+			 * Some cards mishandle the status bits,
+			 * so make sure to check both the busy
+			 * indication and the card state.
+			 */
+		} while (!(status & R1_READY_FOR_DATA) ||
+			(R1_CURRENT_STATE(status) == 7));
+	}
+
+	return err;
 }
 
 static int mmc_blk_issue_discard_rq(struct mmc_queue *mq, struct request *req)
@@ -804,6 +834,15 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *req)
 		 * until later as we need to wait for the card to leave
 		 * programming mode even when things go wrong.
 		 */
+		if (!brq.cmd.error && !brq.stop.error &&
+			brq.data.error == -EAGAIN) {
+			printk(KERN_WARNING "%s: retrying transfer\n",
+					req->rq_disk->disk_name);
+			if (wait_for_ready_state(card, req))
+				goto cmd_err;
+			continue;
+		}
+
 		if (brq.sbc.error || brq.cmd.error ||
 		    brq.data.error || brq.stop.error) {
 			if (brq.data.blocks > 1 && rq_data_dir(req) == READ) {
@@ -813,7 +852,7 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *req)
 				disable_multi = 1;
 				continue;
 			}
-			status = get_card_status(card, req);
+			get_card_status(card, req, &status, 0);
 		} else if (disable_multi == 1) {
 			disable_multi = 0;
 		}
@@ -850,35 +889,8 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *req)
 			       brq.stop.resp[0], status);
 		}
 
-		if (!mmc_host_is_spi(card->host) && rq_data_dir(req) != READ) {
-			do {
-				int err;
-
-				cmd.opcode = MMC_SEND_STATUS;
-				cmd.arg = card->rca << 16;
-				cmd.flags = MMC_RSP_R1 | MMC_CMD_AC;
-				err = mmc_wait_for_cmd(card->host, &cmd, 5);
-				if (err) {
-					printk(KERN_ERR "%s: error %d requesting status\n",
-					       req->rq_disk->disk_name, err);
-					goto cmd_err;
-				}
-				/*
-				 * Some cards mishandle the status bits,
-				 * so make sure to check both the busy
-				 * indication and the card state.
-				 */
-			} while (!(cmd.resp[0] & R1_READY_FOR_DATA) ||
-				(R1_CURRENT_STATE(cmd.resp[0]) == 7));
-
-#if 0
-			if (cmd.resp[0] & ~0x00000900)
-				printk(KERN_ERR "%s: status = %08x\n",
-				       req->rq_disk->disk_name, cmd.resp[0]);
-			if (mmc_decode_status(cmd.resp))
-				goto cmd_err;
-#endif
-		}
+		if (wait_for_ready_state(card, req))
+			goto cmd_err;
 
 		if (brq.cmd.error || brq.stop.error || brq.data.error) {
 			if (rq_data_dir(req) == READ) {
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 7c3444a3707..f4e3a8d1535 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -392,6 +392,8 @@ int mmc_host_enable(struct mmc_host *host)
 	if (host->ops->enable) {
 		int err;
 
+		wake_lock(&host->wakelock);
+
 		host->en_dis_recurs = 1;
 		err = host->ops->enable(host);
 		host->en_dis_recurs = 0;
@@ -399,6 +401,7 @@ int mmc_host_enable(struct mmc_host *host)
 		if (err) {
 			pr_debug("%s: enable error %d\n",
 				 mmc_hostname(host), err);
+			wake_unlock(&host->wakelock);
 			return err;
 		}
 	}
@@ -426,6 +429,8 @@ static int mmc_host_do_disable(struct mmc_host *host, int lazy)
 
 			mmc_schedule_delayed_work(&host->disable, delay);
 		}
+		if (err == 0)
+			wake_unlock(&host->wakelock);
 	}
 	host->enabled = 0;
 	return 0;
@@ -1156,8 +1161,7 @@ void mmc_attach_bus(struct mmc_host *host, const struct mmc_bus_ops *ops)
 }
 
 /*
- * Remove the current bus handler from a host. Assumes that there are
- * no interesting cards left, so the bus is powered down.
+ * Remove the current bus handler from a host.
  */
 void mmc_detach_bus(struct mmc_host *host)
 {
@@ -1174,8 +1178,6 @@ void mmc_detach_bus(struct mmc_host *host)
 
 	spin_unlock_irqrestore(&host->lock, flags);
 
-	mmc_power_off(host);
-
 	mmc_bus_put(host);
 }
 
@@ -1616,8 +1618,21 @@ void mmc_rescan(struct work_struct *work)
 	 * still present
 	 */
 	if (host->bus_ops && host->bus_ops->detect && !host->bus_dead
-	    && !(host->caps & MMC_CAP_NONREMOVABLE))
-		host->bus_ops->detect(host);
+	    && !(host->caps & MMC_CAP_NONREMOVABLE)) {
+
+		if (host->bus_ops->detect(host)) {
+			/* power off if the card was not re-detected */
+			mmc_claim_host(host);
+			mmc_power_off(host);
+			mmc_release_host(host);
+		}
+	}
+
+	/* If the card was removed the bus will be marked
+	 * as dead - extend the wakelock so userspace
+	 * can respond */
+	if (host->bus_dead)
+		extend_wakelock = 1;
 
 	/* If the card was removed the bus will be marked
 	 * as dead - extend the wakelock so userspace
@@ -1663,6 +1678,7 @@ void mmc_rescan(struct work_struct *work)
 		wake_lock_timeout(&mmc_delayed_work_wake_lock, HZ / 2);
 	else
 		wake_unlock(&mmc_delayed_work_wake_lock);
+
 	if (host->caps & MMC_CAP_NEEDS_POLL)
 		mmc_schedule_delayed_work(&host->detect, HZ);
 }
@@ -1697,6 +1713,7 @@ void mmc_stop_host(struct mmc_host *host)
 
 		mmc_claim_host(host);
 		mmc_detach_bus(host);
+		mmc_power_off(host);
 		mmc_release_host(host);
 		mmc_bus_put(host);
 		return;
@@ -1918,6 +1935,7 @@ int mmc_pm_notify(struct notifier_block *notify_block,
 			host->bus_ops->remove(host);
 
 		mmc_detach_bus(host);
+		mmc_power_off(host);
 		mmc_release_host(host);
 		host->pm_flags = 0;
 		break;
diff --git a/drivers/mmc/core/core.h b/drivers/mmc/core/core.h
index d9411ed2a39..49ef1fc7c83 100644
--- a/drivers/mmc/core/core.h
+++ b/drivers/mmc/core/core.h
@@ -19,7 +19,7 @@ struct mmc_bus_ops {
 	int (*awake)(struct mmc_host *);
 	int (*sleep)(struct mmc_host *);
 	void (*remove)(struct mmc_host *);
-	void (*detect)(struct mmc_host *);
+	int (*detect)(struct mmc_host *);
 	int (*suspend)(struct mmc_host *);
 	int (*resume)(struct mmc_host *);
 	int (*power_save)(struct mmc_host *);
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index 84694a9410d..e36032cde44 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -94,7 +94,7 @@ static void mmc_host_clk_gate_delayed(struct mmc_host *host)
 		spin_unlock_irqrestore(&host->clk_lock, flags);
 		return;
 	}
-	mutex_lock(&host->clk_gate_mutex);
+	mmc_claim_host(host);
 	spin_lock_irqsave(&host->clk_lock, flags);
 	if (!host->clk_requests) {
 		spin_unlock_irqrestore(&host->clk_lock, flags);
@@ -104,7 +104,7 @@ static void mmc_host_clk_gate_delayed(struct mmc_host *host)
 		pr_debug("%s: gated MCI clock\n", mmc_hostname(host));
 	}
 	spin_unlock_irqrestore(&host->clk_lock, flags);
-	mutex_unlock(&host->clk_gate_mutex);
+	mmc_release_host(host);
 }
 
 /*
@@ -130,7 +130,7 @@ void mmc_host_clk_ungate(struct mmc_host *host)
 {
 	unsigned long flags;
 
-	mutex_lock(&host->clk_gate_mutex);
+	mmc_claim_host(host);
 	spin_lock_irqsave(&host->clk_lock, flags);
 	if (host->clk_gated) {
 		spin_unlock_irqrestore(&host->clk_lock, flags);
@@ -140,7 +140,7 @@ void mmc_host_clk_ungate(struct mmc_host *host)
 	}
 	host->clk_requests++;
 	spin_unlock_irqrestore(&host->clk_lock, flags);
-	mutex_unlock(&host->clk_gate_mutex);
+	mmc_release_host(host);
 }
 
 /**
@@ -215,7 +215,6 @@ static inline void mmc_host_clk_init(struct mmc_host *host)
 	host->clk_gated = false;
 	INIT_WORK(&host->clk_gate_work, mmc_host_clk_gate_work);
 	spin_lock_init(&host->clk_lock);
-	mutex_init(&host->clk_gate_mutex);
 }
 
 /**
@@ -290,6 +289,10 @@ struct mmc_host *mmc_alloc_host(int extra, struct device *dev)
 	host->pm_notify.notifier_call = mmc_pm_notify;
 #endif
 
+	snprintf(host->wakelock_name, sizeof(host->wakelock_name),
+			"mmc%d_delay_work", host->index);
+	wake_lock_init(&host->wakelock, WAKE_LOCK_SUSPEND, host->wakelock_name);
+
 	/*
 	 * By default, hosts do not support SGIO or large requests.
 	 * They have to set these according to their abilities.
@@ -380,6 +383,7 @@ EXPORT_SYMBOL(mmc_remove_host);
  */
 void mmc_free_host(struct mmc_host *host)
 {
+	wake_lock_destroy(&host->wakelock);
 	spin_lock(&mmc_host_lock);
 	idr_remove(&mmc_host_idr, host->index);
 	spin_unlock(&mmc_host_lock);
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index aa7d1d79b8c..f7fb800f3f9 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -870,7 +870,7 @@ static void mmc_remove(struct mmc_host *host)
 /*
  * Card detection callback from host.
  */
-static void mmc_detect(struct mmc_host *host)
+static int mmc_detect(struct mmc_host *host)
 {
 	int err;
 
@@ -893,6 +893,8 @@ static void mmc_detect(struct mmc_host *host)
 		mmc_detach_bus(host);
 		mmc_release_host(host);
 	}
+
+	return err;
 }
 
 /*
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index 5decf4972bc..99a4acc87ea 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -1009,7 +1009,7 @@ static void mmc_sd_remove(struct mmc_host *host)
 /*
  * Card detection callback from host.
  */
-static void mmc_sd_detect(struct mmc_host *host)
+static int mmc_sd_detect(struct mmc_host *host)
 {
 	int err = 0;
 #ifdef CONFIG_MMC_PARANOID_SD_INIT
@@ -1050,6 +1050,8 @@ static void mmc_sd_detect(struct mmc_host *host)
 		mmc_detach_bus(host);
 		mmc_release_host(host);
 	}
+
+	return err;
 }
 
 /*
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index 7da522e958a..fa5d223a968 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -574,7 +574,7 @@ static void mmc_sdio_remove(struct mmc_host *host)
 /*
  * Card detection callback from host.
  */
-static void mmc_sdio_detect(struct mmc_host *host)
+static int mmc_sdio_detect(struct mmc_host *host)
 {
 	int err;
 
@@ -619,6 +619,8 @@ out:
 		mmc_detach_bus(host);
 		mmc_release_host(host);
 	}
+
+	return err;
 }
 
 /*
diff --git a/drivers/mmc/core/sdio_irq.c b/drivers/mmc/core/sdio_irq.c
index 03ead028d2c..8fda29797a1 100644
--- a/drivers/mmc/core/sdio_irq.c
+++ b/drivers/mmc/core/sdio_irq.c
@@ -27,6 +27,8 @@
 
 #include "sdio_ops.h"
 
+#define SDIO_IDLE_POLL_PERIOD 10
+
 static int process_sdio_pending_irqs(struct mmc_card *card)
 {
 	int i, ret, count;
@@ -43,6 +45,22 @@ static int process_sdio_pending_irqs(struct mmc_card *card)
 		return 1;
 	}
 
+	/*
+	 * Optimization, if there is only 1 function registered
+	 * and IRQ:s are supported and currently enabled, then
+	 * we can assume that this actually is an IRQ and we can
+	 * call the registered IRQ handler directly without
+	 * checking the CCCR registers.
+	 */
+	if ((card->host->caps & MMC_CAP_SDIO_IRQ) &&
+	    card->host->sdio_irqs && (card->sdio_funcs == 1)) {
+		struct sdio_func *func = card->sdio_func[0];
+		if (func && func->irq_handler) {
+			func->irq_handler(func);
+			return 1;
+		}
+	}
+
 	ret = mmc_io_rw_direct(card, 0, 0, SDIO_CCCR_INTx, 0, &pending);
 	if (ret) {
 		printk(KERN_DEBUG "%s: error %d reading SDIO_CCCR_INTx\n",
@@ -76,89 +94,56 @@ static int process_sdio_pending_irqs(struct mmc_card *card)
 	return ret;
 }
 
-static int sdio_irq_thread(void *_host)
+static void sdio_irq_work_func(struct work_struct *work)
 {
-	struct mmc_host *host = _host;
-	struct sched_param param = { .sched_priority = 1 };
-	unsigned long period, idle_period;
+	struct mmc_host *host =	(struct mmc_host *)
+		container_of(work, struct mmc_host, sdio_irq_work.work);
 	int ret;
 
-	sched_setscheduler(current, SCHED_FIFO, &param);
-
 	/*
-	 * We want to allow for SDIO cards to work even on non SDIO
-	 * aware hosts.  One thing that non SDIO host cannot do is
-	 * asynchronous notification of pending SDIO card interrupts
-	 * hence we poll for them in that case.
+	 * We claim the host here on drivers behalf for a couple
+	 * reasons:
+	 *
+	 * 1) it is already needed to retrieve the CCCR_INTx;
+	 * 2) we want the driver(s) to clear the IRQ condition ASAP;
+	 * 3) we need to control the abort condition locally.
+	 *
+	 * Just like traditional hard IRQ handlers, we expect SDIO
+	 * IRQ handlers to be quick and to the point, so that the
+	 * holding of the host lock does not cover too much work
+	 * that doesn't require that lock to be held.
 	 */
-	idle_period = msecs_to_jiffies(10);
-	period = (host->caps & MMC_CAP_SDIO_IRQ) ?
-		MAX_SCHEDULE_TIMEOUT : idle_period;
-
-	pr_debug("%s: IRQ thread started (poll period = %lu jiffies)\n",
-		 mmc_hostname(host), period);
+	mmc_claim_host(host);
 
-	do {
-		/*
-		 * We claim the host here on drivers behalf for a couple
-		 * reasons:
-		 *
-		 * 1) it is already needed to retrieve the CCCR_INTx;
-		 * 2) we want the driver(s) to clear the IRQ condition ASAP;
-		 * 3) we need to control the abort condition locally.
-		 *
-		 * Just like traditional hard IRQ handlers, we expect SDIO
-		 * IRQ handlers to be quick and to the point, so that the
-		 * holding of the host lock does not cover too much work
-		 * that doesn't require that lock to be held.
-		 */
-		ret = __mmc_claim_host(host, &host->sdio_irq_thread_abort);
-		if (ret)
-			break;
-		ret = process_sdio_pending_irqs(host->card);
+	/* Check if there are any subscribers to IRQ:s */
+	if (!host->sdio_irqs) {
 		mmc_release_host(host);
+		return;
+	}
 
-		/*
-		 * Give other threads a chance to run in the presence of
-		 * errors.
-		 */
-		if (ret < 0) {
-			set_current_state(TASK_INTERRUPTIBLE);
-			if (!kthread_should_stop())
-				schedule_timeout(HZ);
-			set_current_state(TASK_RUNNING);
-		}
+	ret = process_sdio_pending_irqs(host->card);
 
+	if (host->caps & MMC_CAP_SDIO_IRQ)
+		host->ops->enable_sdio_irq(host, true);
+	else {
 		/*
 		 * Adaptive polling frequency based on the assumption
 		 * that an interrupt will be closely followed by more.
 		 * This has a substantial benefit for network devices.
 		 */
-		if (!(host->caps & MMC_CAP_SDIO_IRQ)) {
-			if (ret > 0)
-				period /= 2;
-			else {
-				period++;
-				if (period > idle_period)
-					period = idle_period;
-			}
+		if (ret > 0)
+			host->sdio_poll_period /= 2;
+		else {
+			host->sdio_poll_period++;
+			if (host->sdio_poll_period > SDIO_IDLE_POLL_PERIOD)
+				host->sdio_poll_period = SDIO_IDLE_POLL_PERIOD;
 		}
+		queue_delayed_work(host->sdio_irq_workqueue,
+				   &host->sdio_irq_work,
+				   msecs_to_jiffies(host->sdio_poll_period));
+	}
 
-		set_current_state(TASK_INTERRUPTIBLE);
-		if (host->caps & MMC_CAP_SDIO_IRQ)
-			host->ops->enable_sdio_irq(host, 1);
-		if (!kthread_should_stop())
-			schedule_timeout(period);
-		set_current_state(TASK_RUNNING);
-	} while (!kthread_should_stop());
-
-	if (host->caps & MMC_CAP_SDIO_IRQ)
-		host->ops->enable_sdio_irq(host, 0);
-
-	pr_debug("%s: IRQ thread exiting with code %d\n",
-		 mmc_hostname(host), ret);
-
-	return ret;
+	mmc_release_host(host);
 }
 
 static int sdio_card_irq_get(struct mmc_card *card)
@@ -168,14 +153,28 @@ static int sdio_card_irq_get(struct mmc_card *card)
 	WARN_ON(!host->claimed);
 
 	if (!host->sdio_irqs++) {
-		atomic_set(&host->sdio_irq_thread_abort, 0);
-		host->sdio_irq_thread =
-			kthread_run(sdio_irq_thread, host, "ksdioirqd/%s",
-				mmc_hostname(host));
-		if (IS_ERR(host->sdio_irq_thread)) {
-			int err = PTR_ERR(host->sdio_irq_thread);
+		host->sdio_irq_workqueue =
+			create_singlethread_workqueue("sdio_irq_workqueue");
+		if (host->sdio_irq_workqueue == NULL) {
 			host->sdio_irqs--;
-			return err;
+			return -ENOMEM;
+		}
+
+		INIT_DELAYED_WORK(&host->sdio_irq_work, sdio_irq_work_func);
+
+		/*
+		 * We want to allow for SDIO cards to work even on non SDIO
+		 * aware hosts.  One thing that non SDIO host cannot do is
+		 * asynchronous notification of pending SDIO card interrupts
+		 * hence we poll for them in that case.
+		 */
+		if (host->caps & MMC_CAP_SDIO_IRQ)
+			host->ops->enable_sdio_irq(host, true);
+		else {
+			host->sdio_poll_period = SDIO_IDLE_POLL_PERIOD;
+			queue_delayed_work(host->sdio_irq_workqueue,
+				&host->sdio_irq_work,
+				msecs_to_jiffies(host->sdio_poll_period));
 		}
 	}
 
@@ -190,8 +189,26 @@ static int sdio_card_irq_put(struct mmc_card *card)
 	BUG_ON(host->sdio_irqs < 1);
 
 	if (!--host->sdio_irqs) {
-		atomic_set(&host->sdio_irq_thread_abort, 1);
-		kthread_stop(host->sdio_irq_thread);
+		host->ops->enable_sdio_irq(host, false);
+
+		/*
+		 * Temporarily release the host in order to complete
+		 * any pending work before destroying the work queue.
+		 *
+		 * There is a theroetical chance of messing up here,
+		 * if a calling driver is waiting to claim the host
+		 * in order to claim an SDIO IRQ, and that call falls through
+		 * while releasing the IRQ:s here, there is no guarantee
+		 * that that IRQ:s will be reliably turned on or off.
+		 * This will be fixed in a coming patch, but this solution
+		 * is deemed good enough for now since it fixes an obvious
+		 * error and the failing case deemed not likely to happen.
+		 */
+		mmc_release_host(card->host);
+		cancel_delayed_work_sync(&host->sdio_irq_work);
+		destroy_workqueue(host->sdio_irq_workqueue);
+		host->sdio_irq_workqueue = NULL;
+		mmc_claim_host(card->host);
 	}
 
 	return 0;
diff --git a/drivers/mmc/core/sdio_ops.c b/drivers/mmc/core/sdio_ops.c
index f087d876c57..a29f99a30c4 100644
--- a/drivers/mmc/core/sdio_ops.c
+++ b/drivers/mmc/core/sdio_ops.c
@@ -144,10 +144,15 @@ int mmc_io_rw_extended(struct mmc_card *card, int write, unsigned fn,
 	cmd.arg |= fn << 28;
 	cmd.arg |= incr_addr ? 0x04000000 : 0x00000000;
 	cmd.arg |= addr << 9;
-	if (blocks == 1 && blksz <= 512)
-		cmd.arg |= (blksz == 512) ? 0 : blksz;	/* byte mode */
+
+	if (blocks == 1 && blksz < 512)
+		cmd.arg |= blksz;			/* byte mode */
+	else if (blocks == 1 && blksz == 512 &&
+		 !(card->host->caps & MMC_CAP_BROKEN_SDIO_CMD53))
+		cmd.arg |= 0;				/* byte mode, 0=512 */
 	else
 		cmd.arg |= 0x08000000 | blocks;		/* block mode */
+
 	cmd.flags = MMC_RSP_SPI_R5 | MMC_RSP_R5 | MMC_CMD_ADTC;
 
 	data.blksz = blksz;
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 50f4f77ed20..13ee09eb3e2 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -34,6 +34,11 @@
 #include <asm/io.h>
 #include <asm/sizes.h>
 
+#ifdef CONFIG_ARCH_U8500
+/* To be withdrawn when cpu_is_u8500v20_or_later() call will disapear */
+#include <mach/hardware.h>
+#endif
+
 #include "mmci.h"
 
 #define DRIVER_NAME "mmci-pl18x"
@@ -52,6 +57,10 @@ static unsigned int fmax = 515633;
  * @sdio: variant supports SDIO
  * @st_clkdiv: true if using a ST-specific clock divider algorithm
  * @blksz_datactrl16: true if Block size is at b16..b30 position in datactrl register
+ * @pwrreg_powerup: power up value for MMCIPOWER register
+ * @non_power_of_2_blksize: variant supports block sizes that are not
+ *		a power of two.
+ * @blksz_datactrl16: true if Block size is at b16..b30 position in datactrl register
  */
 struct variant_data {
 	unsigned int		clkreg;
@@ -62,12 +71,15 @@ struct variant_data {
 	bool			sdio;
 	bool			st_clkdiv;
 	bool			blksz_datactrl16;
+	unsigned int		pwrreg_powerup;
+	bool			non_power_of_2_blksize;
 };
 
 static struct variant_data variant_arm = {
 	.fifosize		= 16 * 4,
 	.fifohalfsize		= 8 * 4,
 	.datalength_bits	= 16,
+	.pwrreg_powerup		= MCI_PWR_UP,
 };
 
 static struct variant_data variant_arm_extended_fifo = {
@@ -82,6 +94,7 @@ static struct variant_data variant_u300 = {
 	.clkreg_enable		= MCI_ST_U300_HWFCEN,
 	.datalength_bits	= 16,
 	.sdio			= true,
+	.pwrreg_powerup		= MCI_PWR_ON,
 };
 
 static struct variant_data variant_ux500 = {
@@ -92,8 +105,108 @@ static struct variant_data variant_ux500 = {
 	.datalength_bits	= 24,
 	.sdio			= true,
 	.st_clkdiv		= true,
+	.pwrreg_powerup		= MCI_PWR_ON,
+	.non_power_of_2_blksize	= true,
+};
+/*
+ * Debugfs
+ */
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+static int mmci_regs_show(struct seq_file *seq, void *v)
+{
+	struct mmci_host *host = seq->private;
+	unsigned long iflags;
+	u32 pwr, clk, arg, cmd, rspcmd, r0, r1, r2, r3;
+	u32 dtimer, dlength, dctrl, dcnt;
+	u32 sta, clear, mask0, mask1, fifocnt, fifo;
+
+	mmc_host_enable(host->mmc);
+	spin_lock_irqsave(&host->lock, iflags);
+
+	pwr = readl(host->base + MMCIPOWER);
+	clk = readl(host->base + MMCICLOCK);
+	arg = readl(host->base + MMCIARGUMENT);
+	cmd = readl(host->base + MMCICOMMAND);
+	rspcmd = readl(host->base + MMCIRESPCMD);
+	r0 = readl(host->base + MMCIRESPONSE0);
+	r1 = readl(host->base + MMCIRESPONSE1);
+	r2 = readl(host->base + MMCIRESPONSE2);
+	r3 = readl(host->base + MMCIRESPONSE3);
+	dtimer = readl(host->base + MMCIDATATIMER);
+	dlength = readl(host->base + MMCIDATALENGTH);
+	dctrl = readl(host->base + MMCIDATACTRL);
+	dcnt = readl(host->base + MMCIDATACNT);
+	sta = readl(host->base + MMCISTATUS);
+	clear = readl(host->base + MMCICLEAR);
+	mask0 = readl(host->base + MMCIMASK0);
+	mask1 = readl(host->base + MMCIMASK1);
+	fifocnt = readl(host->base + MMCIFIFOCNT);
+	fifo = readl(host->base + MMCIFIFO);
+
+	spin_unlock_irqrestore(&host->lock, iflags);
+	mmc_host_disable(host->mmc);
+
+	seq_printf(seq, "\033[1;34mMMCI registers\033[0m\n");
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_power", pwr);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_clock", clk);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_arg", arg);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_cmd", cmd);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_respcmd", rspcmd);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_resp0", r0);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_resp1", r1);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_resp2", r2);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_resp3", r3);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_datatimer", dtimer);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_datalen", dlength);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_datactrl", dctrl);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_datacnt", dcnt);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_status", sta);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_iclear", clear);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_imask0", mask0);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_imask1", mask1);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_fifocnt", fifocnt);
+	seq_printf(seq, "%-20s:0x%x\n", "mmci_fifo", fifo);
+
+	return 0;
+}
+
+static int mmci_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, mmci_regs_show, inode->i_private);
+}
+
+static const struct file_operations mmci_fops_regs = {
+	.owner		= THIS_MODULE,
+	.open		= mmci_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
 };
 
+static void mmci_debugfs_create(struct mmci_host *host)
+{
+	host->debug_regs = debugfs_create_file("regs", S_IRUGO,
+					       host->mmc->debugfs_root, host,
+					       &mmci_fops_regs);
+
+	if (IS_ERR(host->debug_regs))
+		dev_err(mmc_dev(host->mmc),
+				"failed to create debug regs file\n");
+}
+
+static void mmci_debugfs_remove(struct mmci_host *host)
+{
+	debugfs_remove(host->debug_regs);
+}
+
+#else
+static inline void mmci_debugfs_create(struct mmci_host *host) { }
+static inline void mmci_debugfs_remove(struct mmci_host *host) { }
+#endif
+
 static struct variant_data variant_ux500v2 = {
 	.fifosize		= 30 * 4,
 	.fifohalfsize		= 8 * 4,
@@ -102,6 +215,8 @@ static struct variant_data variant_ux500v2 = {
 	.datalength_bits	= 24,
 	.sdio			= true,
 	.st_clkdiv		= true,
+	.pwrreg_powerup		= MCI_PWR_ON,
+	.non_power_of_2_blksize	= true,
 	.blksz_datactrl16	= true,
 };
 
@@ -192,8 +307,31 @@ static void mmci_set_mask1(struct mmci_host *host, unsigned int mask)
 
 static void mmci_stop_data(struct mmci_host *host)
 {
-	writel(0, host->base + MMCIDATACTRL);
+	u32 clk;
+	unsigned int datactrl = 0;
+
+	/*
+	 * The ST Micro variants has a special bit
+	 * to enable SDIO mode. This bit must remain set even when not
+	 * doing data transfers, otherwise no SDIO interrupts can be
+	 * received.
+	 */
+	if (host->variant->sdio &&
+		host->mmc->card &&
+		mmc_card_sdio(host->mmc->card))
+		datactrl |= MCI_ST_DPSM_SDIOEN;
+
+	writel(datactrl, host->base + MMCIDATACTRL);
 	mmci_set_mask1(host, 0);
+
+	/* Needed for DDR */
+	if (host->mmc->card && mmc_card_ddr_mode(host->mmc->card)) {
+		clk = readl(host->base + MMCICLOCK);
+		clk &= ~MCI_ST_UX500_NEG_EDGE;
+
+		writel(clk, (host->base + MMCICLOCK));
+	}
+
 	host->data = NULL;
 }
 
@@ -370,6 +508,10 @@ static int mmci_dma_start_data(struct mmci_host *host, unsigned int datactrl)
 	struct dma_async_tx_descriptor *desc;
 	int nr_sg;
 
+	/* If less than or equal to the fifo size, don't bother with DMA */
+	if (host->size <= variant->fifosize)
+		return -EINVAL;
+
 	host->dma_current = NULL;
 
 	if (data->flags & MMC_DATA_READ) {
@@ -458,6 +600,7 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 	unsigned long long clks;
 	void __iomem *base;
 	int blksz_bits;
+	u32 clk;
 
 	dev_dbg(mmc_dev(host->mmc), "blksz %04x blks %04x flags %08x\n",
 		data->blksz, data->blocks, data->flags);
@@ -465,6 +608,8 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 	host->data = data;
 	host->size = data->blksz * data->blocks;
 	data->bytes_xfered = 0;
+	host->cache_len = 0;
+	host->cache = 0;
 
 	clks = (unsigned long long)data->timeout_ns * host->cclk;
 	do_div(clks, 1000000000UL);
@@ -476,7 +621,6 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 	writel(host->size, base + MMCIDATALENGTH);
 
 	blksz_bits = ffs(data->blksz) - 1;
-	BUG_ON(1 << blksz_bits != data->blksz);
 
 	if (variant->blksz_datactrl16)
 		datactrl = MCI_DPSM_ENABLE | (data->blksz << 16);
@@ -486,6 +630,48 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 	if (data->flags & MMC_DATA_READ)
 		datactrl |= MCI_DPSM_DIRECTION;
 
+	if (host->mmc->card && mmc_card_ddr_mode(host->mmc->card)) {
+		datactrl |= MCI_ST_DPSM_DDRMODE;
+
+		/* Needed for DDR */
+		clk = readl(base + MMCICLOCK);
+		clk |= MCI_ST_UX500_NEG_EDGE;
+
+		writel(clk, (base + MMCICLOCK));
+	}
+
+	if (variant->sdio &&
+		host->mmc->card &&
+		mmc_card_sdio(host->mmc->card)) {
+			/*
+			 * The ST Micro variants has a special bit
+			 * to enable SDIO mode. This bit is set the first time
+			 * a SDIO data transfer is done and must remain set
+			 * after the data transfer is completed. The reason is
+			 * because of otherwise no SDIO interrupts can be
+			 * received.
+			 */
+			datactrl |= MCI_ST_DPSM_SDIOEN;
+
+			/*
+			 * The ST Micro variant for SDIO transfer sizes
+			 * less than or equal to 8 bytes needs to have clock
+			 * H/W flow control disabled. Since flow control is
+			 * not really needed for anything that fits in the
+			 * FIFO, we can disable it for any write smaller
+			 * than the FIFO size.
+			 */
+			if ((host->size <= variant->fifosize) &&
+			    (data->flags & MMC_DATA_WRITE))
+				writel(readl(host->base + MMCICLOCK) &
+					~variant->clkreg_enable,
+					host->base + MMCICLOCK);
+			else
+				writel(readl(host->base + MMCICLOCK) |
+					variant->clkreg_enable,
+					host->base + MMCICLOCK);
+	}
+
 	/*
 	 * Attempt to use DMA operation mode, if this
 	 * should fail, fall back to PIO mode
@@ -514,11 +700,6 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 		irqmask = MCI_TXFIFOHALFEMPTYMASK;
 	}
 
-	/* The ST Micro variants has a special bit to enable SDIO */
-	if (variant->sdio && host->mmc->card)
-		if (mmc_card_sdio(host->mmc->card))
-			datactrl |= MCI_ST_DPSM_SDIOEN;
-
 	writel(datactrl, base + MMCIDATACTRL);
 	writel(readl(base + MMCIMASK0) & ~MCI_DATAENDMASK, base + MMCIMASK0);
 	mmci_set_mask1(host, irqmask);
@@ -644,14 +825,14 @@ mmci_cmd_irq(struct mmci_host *host, struct mmc_command *cmd,
 	}
 }
 
-static int mmci_pio_read(struct mmci_host *host, char *buffer, unsigned int remain)
+static int mmci_pio_read(struct mmci_host *host, char *buffer,
+			 unsigned int remain, u32 status)
 {
 	void __iomem *base = host->base;
 	char *ptr = buffer;
-	u32 status;
 	int host_remain = host->size;
 
-	do {
+	while (status & MCI_RXDATAAVLBL) {
 		int count = host_remain - (readl(base + MMCIFIFOCNT) << 2);
 
 		if (count > remain)
@@ -670,59 +851,107 @@ static int mmci_pio_read(struct mmci_host *host, char *buffer, unsigned int rema
 			break;
 
 		status = readl(base + MMCISTATUS);
-	} while (status & MCI_RXDATAAVLBL);
+	}
 
 	return ptr - buffer;
 }
 
-static int mmci_pio_write(struct mmci_host *host, char *buffer, unsigned int remain, u32 status)
+static int mmci_pio_write(struct mmci_host *host, char *buffer,
+			  unsigned int remain, u32 status)
 {
 	struct variant_data *variant = host->variant;
 	void __iomem *base = host->base;
+
+	unsigned int data_left = host->size;
+	unsigned int maxcnt;
 	char *ptr = buffer;
 
-	do {
-		unsigned int count, maxcnt;
+	while (status & MCI_TXFIFOHALFEMPTY) {
+		unsigned int count;
+		char *cache_ptr;
+		int i;
 
-		maxcnt = status & MCI_TXFIFOEMPTY ?
-			 variant->fifosize : variant->fifohalfsize;
-		count = min(remain, maxcnt);
+		count = min(remain, variant->fifohalfsize);
 
 		/*
-		 * The ST Micro variant for SDIO transfer sizes
-		 * less then 8 bytes should have clock H/W flow
-		 * control disabled.
+		 * A write to the FIFO must always be done of 4 bytes aligned
+		 * data. If the buffer is not 4 bytes aligned we must pad the
+		 * data, but this must only be done for the final write for the
+		 * entire data transfer, otherwise we will corrupt the data.
+		 * Thus a buffer cache of four bytes is needed to temporary
+		 * store data.
 		 */
-		if (variant->sdio &&
-		    mmc_card_sdio(host->mmc->card)) {
-			if (count < 8)
-				writel(readl(host->base + MMCICLOCK) &
-					~variant->clkreg_enable,
-					host->base + MMCICLOCK);
-			else
-				writel(readl(host->base + MMCICLOCK) |
-					variant->clkreg_enable,
-					host->base + MMCICLOCK);
+
+		if (host->cache_len) {
+			cache_ptr = (char *)&host->cache;
+			cache_ptr = cache_ptr + host->cache_len;
+			data_left += host->cache_len;
+
+			while ((host->cache_len < 4) && (remain > 0)) {
+				*cache_ptr = *ptr;
+				cache_ptr++;
+				ptr++;
+				host->cache_len++;
+				remain--;
+			}
+
+			if ((host->cache_len == 4) ||
+				(data_left == host->cache_len)) {
+
+				writesl(base + MMCIFIFO, &host->cache, 1);
+				if (data_left == host->cache_len)
+					break;
+
+				host->cache = 0;
+				host->cache_len = 0;
+				maxcnt -= 4;
+				data_left -= 4;
+			}
+
+			if (remain == 0)
+				break;
 		}
 
-		/*
-		 * SDIO especially may want to send something that is
-		 * not divisible by 4 (as opposed to card sectors
-		 * etc), and the FIFO only accept full 32-bit writes.
-		 * So compensate by adding +3 on the count, a single
-		 * byte become a 32bit write, 7 bytes will be two
-		 * 32bit writes etc.
-		 */
-		writesl(base + MMCIFIFO, ptr, (count + 3) >> 2);
 
-		ptr += count;
-		remain -= count;
+		if (!(count % 4) || (data_left == count)) {
+			/*
+			 * The data is either 4-bytes aligned or it is the
+			 * last data to write. It is thus fine to potentially
+			 * pad the data if needed.
+			 */
+			writesl(base + MMCIFIFO, ptr, (count + 3) >> 2);
+			ptr += count;
+			remain -= count;
+			data_left -= count;
+
+		} else {
+
+			host->cache_len = count % 4;
+			count = (count >> 2) << 2;
+
+			if (count)
+				writesl(base + MMCIFIFO, ptr, count >> 2);
+
+			ptr += count;
+			remain -= count;
+			data_left -= count;
+
+			i = 0;
+			cache_ptr = (char *)&host->cache;
+			while (i < host->cache_len) {
+				*cache_ptr = *ptr;
+				cache_ptr++;
+				ptr++;
+				remain--;
+				i++;
+			}
+		}
 
 		if (remain == 0)
 			break;
 
 		status = readl(base + MMCISTATUS);
-	} while (status & MCI_TXFIFOHALFEMPTY);
+	};
 
 	return ptr - buffer;
 }
@@ -767,11 +996,14 @@ static irqreturn_t mmci_pio_irq(int irq, void *dev_id)
 
 		len = 0;
 		if (status & MCI_RXACTIVE)
-			len = mmci_pio_read(host, buffer, remain);
+			len = mmci_pio_read(host, buffer, remain, status);
 		if (status & MCI_TXACTIVE)
 			len = mmci_pio_write(host, buffer, remain, status);
 
-		sg_miter->consumed = len;
+		if (len > sg_miter->consumed)
+			len = sg_miter->consumed;
+		else
+			sg_miter->consumed = len;
 
 		host->size -= len;
 		remain -= len;
@@ -814,6 +1046,7 @@ static irqreturn_t mmci_irq(int irq, void *dev_id)
 {
 	struct mmci_host *host = dev_id;
 	u32 status;
+	int sdio_irq = 0;
 	int ret = 0;
 
 	spin_lock(&host->lock);
@@ -836,6 +1069,9 @@ static irqreturn_t mmci_irq(int irq, void *dev_id)
 
 		dev_dbg(mmc_dev(host->mmc), "irq0 (data+cmd) %08x\n", status);
 
+		if (status & MCI_ST_SDIOIT)
+			sdio_irq = 1;
+
 		data = host->data;
 		if (status & (MCI_DATACRCFAIL|MCI_DATATIMEOUT|MCI_TXUNDERRUN|
 			      MCI_RXOVERRUN|MCI_DATAEND|MCI_DATABLOCKEND) && data)
@@ -850,17 +1086,27 @@ static irqreturn_t mmci_irq(int irq, void *dev_id)
 
 	spin_unlock(&host->lock);
 
+	if (sdio_irq)
+		mmc_signal_sdio_irq(host->mmc);
+
 	return IRQ_RETVAL(ret);
 }
 
 static void mmci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 {
 	struct mmci_host *host = mmc_priv(mmc);
+	struct variant_data *variant = host->variant;
 	unsigned long flags;
 
 	WARN_ON(host->mrq != NULL);
 
-	if (mrq->data && !is_power_of_2(mrq->data->blksz)) {
+	if (mrq->data &&
+	    (!variant->non_power_of_2_blksize ||
+#ifdef CONFIG_ARCH_U8500
+	     !cpu_is_u8500v20_or_later() ||
+#endif
+	    (mmc->card && mmc_card_ddr_mode(mmc->card))) &&
+	    !is_power_of_2(mrq->data->blksz)) {
 		dev_err(mmc_dev(mmc), "unsupported block size (%d bytes)\n",
 			mrq->data->blksz);
 		mrq->cmd->error = -EINVAL;
@@ -883,6 +1129,7 @@ static void mmci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 static void mmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 {
 	struct mmci_host *host = mmc_priv(mmc);
+	struct variant_data *variant = host->variant;
 	u32 pwr = 0;
 	unsigned long flags;
 	int ret;
@@ -909,11 +1156,15 @@ static void mmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		if (host->plat->vdd_handler)
 			pwr |= host->plat->vdd_handler(mmc_dev(mmc), ios->vdd,
 						       ios->power_mode);
-		/* The ST version does not have this, fall through to POWER_ON */
-		if (host->hw_designer != AMBA_VENDOR_ST) {
-			pwr |= MCI_PWR_UP;
-			break;
-		}
+
+		/*
+		 * The ST Micro variant doesn't have the PL180s MCI_PWR_UP
+		 * and instead uses MCI_PWR_ON so apply whatever value is
+		 * configured in the variant data.
+		 */
+		pwr |= variant->pwrreg_powerup;
+
+		break;
 	case MMC_POWER_ON:
 		pwr |= MCI_PWR_ON;
 		break;
@@ -984,11 +1235,30 @@ static irqreturn_t mmci_cd_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static void mmci_enable_sdio_irq(struct mmc_host *mmc, int enable)
+{
+	unsigned long flags;
+	unsigned int mask0;
+	struct mmci_host *host = mmc_priv(mmc);
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	mask0 = readl(host->base + MMCIMASK0);
+	if (enable)
+		mask0 |= MCI_ST_SDIOIT;
+	else
+		mask0 &= ~MCI_ST_SDIOIT;
+	writel(mask0, host->base + MMCIMASK0);
+
+	spin_unlock_irqrestore(&host->lock, flags);
+}
+
 static const struct mmc_host_ops mmci_ops = {
 	.request	= mmci_request,
 	.set_ios	= mmci_set_ios,
 	.get_ro		= mmci_get_ro,
 	.get_cd		= mmci_get_cd,
+	.enable_sdio_irq = mmci_enable_sdio_irq,
 };
 
 static int __devinit mmci_probe(struct amba_device *dev,
@@ -1063,15 +1333,10 @@ static int __devinit mmci_probe(struct amba_device *dev,
 	}
 
 	mmc->ops = &mmci_ops;
-	/*
-	 * The ARM and ST versions of the block have slightly different
-	 * clock divider equations which means that the minimum divider
-	 * differs too.
-	 */
 	if (variant->st_clkdiv)
-		mmc->f_min = DIV_ROUND_UP(host->mclk, 257);
+		mmc->f_min = host->mclk / 257;
 	else
-		mmc->f_min = DIV_ROUND_UP(host->mclk, 512);
+		mmc->f_min = (host->mclk + 511) / 512;
 	/*
 	 * If the platform data supplies a maximum operating
 	 * frequency, this takes precedence. Else, we fall back
@@ -1208,6 +1473,8 @@ static int __devinit mmci_probe(struct amba_device *dev,
 
 	mmc_add_host(mmc);
 
+	mmci_debugfs_create(host);
+
 	return 0;
 
  irq0_free:
@@ -1243,6 +1510,7 @@ static int __devexit mmci_remove(struct amba_device *dev)
 	if (mmc) {
 		struct mmci_host *host = mmc_priv(mmc);
 
+		mmci_debugfs_remove(host);
 		mmc_remove_host(mmc);
 
 		writel(0, host->base + MMCIMASK0);
diff --git a/drivers/mmc/host/mmci.h b/drivers/mmc/host/mmci.h
index 2164e8c6476..52b06d43c7d 100644
--- a/drivers/mmc/host/mmci.h
+++ b/drivers/mmc/host/mmci.h
@@ -196,6 +196,9 @@ struct mmci_host {
 	/* pio stuff */
 	struct sg_mapping_iter	sg_miter;
 	unsigned int		size;
+	unsigned int		cache;
+	unsigned int		cache_len;
+
 	struct regulator	*vcc;
 
 #ifdef CONFIG_DMA_ENGINE
@@ -208,5 +211,9 @@ struct mmci_host {
 #else
 #define dma_inprogress(host)	(0)
 #endif
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry		*debug_regs;
+#endif
 };
 
diff --git a/drivers/modem/Kconfig b/drivers/modem/Kconfig
new file mode 100644
index 00000000000..95d4307775b
--- /dev/null
+++ b/drivers/modem/Kconfig
@@ -0,0 +1,22 @@
+config MODEM
+        bool "Modem Access Framework"
+        default n
+        help
+         Add support for Modem Access Framework. It allows different
+	 platform specific drivers to register modem access mechanisms
+	 and allows transparent access to modem to the client drivers.
+
+	 If unsure, say N.
+
+config MODEM_U8500
+	bool "Modem Access driver for STE U8500 platform"
+	depends on MODEM
+	default n
+	help
+	 Add support for Modem Access driver on STE U8500 platform which
+	 uses Shared Memroy as IPC mechanism between Modem processor and
+	 Application processor.
+
+	 If unsure, say N.
+
+source "drivers/modem/shrm/Kconfig"
diff --git a/drivers/modem/Makefile b/drivers/modem/Makefile
new file mode 100644
index 00000000000..e1b3f376cce
--- /dev/null
+++ b/drivers/modem/Makefile
@@ -0,0 +1,4 @@
+obj-$(CONFIG_MODEM)       := modem_access.o
+obj-$(CONFIG_MODEM_U8500) += modem_u8500.o
+obj-$(CONFIG_U8500_SHRM)  += shrm/
+
diff --git a/drivers/modem/modem_access.c b/drivers/modem/modem_access.c
new file mode 100644
index 00000000000..7337029c251
--- /dev/null
+++ b/drivers/modem/modem_access.c
@@ -0,0 +1,416 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com>
+ *
+ * Heavily adapted from Regulator framework.
+ * Provides mechanisms for registering platform specific access
+ * mechanisms for modem.
+ * Also, exposes APIs for gettng/releasing the access and even
+ * query the access status, and the modem usage status.
+ */
+#include <linux/modem/modem.h>
+#include <linux/modem/modem_client.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+
+static DEFINE_MUTEX(modem_list_mutex);
+static LIST_HEAD(modem_list);
+
+struct modem {
+	struct device *dev;
+	struct list_head list;
+	char *modem_name;
+	struct device_attribute dev_attr;
+	struct modem_dev *mdev;
+	atomic_t use;
+};
+
+static const char *mdev_get_name(struct modem_dev *mdev)
+{
+	if (mdev->desc->name)
+		return mdev->desc->name;
+	else
+		return "";
+}
+
+static int _modem_is_requested(struct modem_dev *mdev)
+{
+	/* If we don't know then assume that the modem is always on */
+	if (!mdev->desc->ops->is_requested)
+		return 0;
+
+	return mdev->desc->ops->is_requested(mdev);
+}
+
+/**
+ * modem_is_requested - check if modem access is requested
+ * @modem: modem device
+ *
+ * Checks whether modem is accessed or not by querying
+ * the underlying platform specific modem access
+ * implementation.
+ */
+int modem_is_requested(struct modem *modem)
+{
+	int ret;
+
+	mutex_lock(&modem->mdev->mutex);
+	ret = _modem_is_requested(modem->mdev);
+	mutex_unlock(&modem->mdev->mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL(modem_is_requested);
+
+static int _modem_request(struct modem_dev *mdev)
+{
+	int ret;
+
+	if (++mdev->use_count == 1) {
+		ret = _modem_is_requested(mdev);
+		if (ret == 0)
+			mdev->desc->ops->request(mdev);
+	}
+
+	return 0;
+}
+
+/**
+ * modem_request - Request access the modem
+ * @modem: modem device
+ *
+ * API to access the modem. It keeps a client
+ * specific check on whether the particular modem
+ * requested is accessed or not.
+ */
+void modem_request(struct modem *modem)
+{
+	struct modem_dev *mdev = modem->mdev;
+	int ret = 0;
+
+
+	mutex_lock(&mdev->mutex);
+	if (atomic_read(&modem->use) == 1) {
+		mutex_unlock(&mdev->mutex);
+		return;
+	}
+	ret = _modem_request(mdev);
+	if (ret == 0)
+		atomic_set(&modem->use, 1);
+	mutex_unlock(&mdev->mutex);
+}
+EXPORT_SYMBOL(modem_request);
+
+static int _modem_release(struct modem_dev *mdev)
+{
+	if (WARN(mdev->use_count <= 0,
+				"unbalanced releases for %s\n",
+				mdev_get_name(mdev)))
+		return -EIO;
+
+	if (--mdev->use_count == 0)
+		mdev->desc->ops->release(mdev);
+
+	return 0;
+}
+
+/**
+ * modem_release - Release access to modem
+ * @modem: modem device
+ *
+ * Releases accesss to the modem. It keeps a client
+ * specific check on whether a particular modem
+ * is released or not.
+ */
+void modem_release(struct modem *modem)
+{
+	struct modem_dev *mdev = modem->mdev;
+	int ret = 0;
+
+	mutex_lock(&mdev->mutex);
+	if (atomic_read(&modem->use) == 0) {
+		mutex_unlock(&mdev->mutex);
+		return;
+	}
+	ret = _modem_release(mdev);
+	if (ret == 0)
+		atomic_set(&modem->use, 0);
+	mutex_unlock(&mdev->mutex);
+}
+EXPORT_SYMBOL(modem_release);
+
+/**
+ * modem_get_usage - Check if particular client is using modem
+ * @modem: modem device
+ *
+ * Checks whether the particular client is using access to modem.
+ * This API could be used by client drivers in making their
+ * suspend decisions.
+ */
+int modem_get_usage(struct modem *modem)
+{
+	return atomic_read(&modem->use);
+}
+EXPORT_SYMBOL(modem_get_usage);
+
+static struct modem *create_modem(struct modem_dev *mdev,
+		struct device *dev,
+		const char *id)
+{
+	struct modem *modem;
+
+	modem = kzalloc(sizeof(*modem), GFP_KERNEL);
+	if (modem == NULL)
+		return NULL;
+
+	mutex_lock(&mdev->mutex);
+	modem->mdev = mdev;
+	modem->dev = dev;
+	list_add(&modem->list, &mdev->client_list);
+
+	mutex_unlock(&mdev->mutex);
+	return modem;
+
+}
+
+static struct modem *_modem_get(struct device *dev, const char *id,
+		int exclusive)
+{
+	struct modem_dev *mdev_ptr;
+	struct modem *modem = ERR_PTR(-ENODEV);
+	int ret;
+
+	if (id == NULL) {
+		pr_err("modem_get with no identifier\n");
+		return modem;
+	}
+
+	mutex_lock(&modem_list_mutex);
+	list_for_each_entry(mdev_ptr, &modem_list, modem_list) {
+		if (strcmp(mdev_get_name(mdev_ptr), id) == 0)
+			goto found;
+	}
+
+	return ERR_PTR(-ENODEV);
+
+found:
+	if (!try_module_get(mdev_ptr->owner))
+		goto out;
+
+	modem = create_modem(mdev_ptr, dev, id);
+	if (modem == NULL) {
+		modem = ERR_PTR(-ENOMEM);
+		module_put(mdev_ptr->owner);
+	}
+
+	mdev_ptr->open_count++;
+	ret = _modem_is_requested(mdev_ptr);
+	if (ret)
+		mdev_ptr->use_count = 1;
+	else
+		mdev_ptr->use_count = 0;
+
+out:
+	mutex_unlock(&modem_list_mutex);
+	return modem;
+
+}
+
+/**
+ * modem_get - Get reference to a particular platform specific modem
+ * @dev: device
+ * @id: modem device name
+ *
+ * Get reference to a particular modem device.
+ */
+struct modem *modem_get(struct device *dev, const char *id)
+{
+	return _modem_get(dev, id, 0);
+}
+EXPORT_SYMBOL(modem_get);
+
+/**
+ * modem_put - Release reference to a modem device
+ * @modem: modem device
+ *
+ * Release reference to a modem device.
+ */
+void modem_put(struct modem *modem)
+{
+	struct modem_dev *mdev;
+
+	if (modem == NULL || IS_ERR(modem))
+		return;
+
+	mutex_lock(&modem_list_mutex);
+	mdev = modem->mdev;
+
+	list_del(&modem->list);
+	kfree(modem);
+
+	mdev->open_count--;
+
+	module_put(mdev->owner);
+	mutex_unlock(&modem_list_mutex);
+}
+EXPORT_SYMBOL(modem_put);
+
+static ssize_t modem_print_state(char *buf, int state)
+{
+	if (state > 0)
+		return sprintf(buf, "accessed\n");
+	else if (state == 0)
+		return sprintf(buf, "released\n");
+	else
+		return sprintf(buf, "unknown\n");
+}
+
+static ssize_t modem_state_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct modem_dev *mdev = dev_get_drvdata(dev);
+	ssize_t ret;
+
+	mutex_lock(&mdev->mutex);
+	ret = modem_print_state(buf, _modem_is_requested(mdev));
+	mutex_unlock(&mdev->mutex);
+
+	return ret;
+}
+static DEVICE_ATTR(state, 0444, modem_state_show, NULL);
+
+static ssize_t modem_use_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct modem_dev *mdev = dev_get_drvdata(dev);
+	struct modem *mod;
+	size_t size = 0;
+
+	list_for_each_entry(mod, &mdev->client_list, list) {
+		if (mod->dev != NULL)
+			size += sprintf((buf + size), "%s (%d)\n",
+				dev_name(mod->dev), atomic_read(&mod->use));
+		else
+			size += sprintf((buf + size), "unknown (%d)\n",
+				atomic_read(&mod->use));
+	}
+	size += sprintf((buf + size), "\n");
+
+	return size;
+}
+static DEVICE_ATTR(use, 0444, modem_use_show, NULL);
+
+static ssize_t modem_name_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct modem_dev *mdev = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%s\n", mdev_get_name(mdev));
+}
+static DEVICE_ATTR(name, 0444, modem_name_show, NULL);
+
+static ssize_t modem_num_active_users_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct modem_dev *mdev = dev_get_drvdata(dev);
+	return sprintf(buf, "%d\n", mdev->use_count);
+}
+static DEVICE_ATTR(num_active_users, 0444, modem_num_active_users_show, NULL);
+
+static int add_modem_attributes(struct modem_dev *mdev)
+{
+	struct device      *dev = &mdev->dev;
+	struct modem_ops   *ops = mdev->desc->ops;
+	int                status = 0;
+
+	status = device_create_file(dev, &dev_attr_use);
+	if (status < 0)
+		return status;
+
+	status = device_create_file(dev, &dev_attr_name);
+	if (status < 0)
+		return status;
+
+	status = device_create_file(dev, &dev_attr_num_active_users);
+	if (status < 0)
+		return status;
+
+	if (ops->is_requested) {
+		status = device_create_file(dev, &dev_attr_state);
+		if (status < 0)
+			return status;
+	}
+
+	return 0;
+}
+
+/**
+ * modem_register - register a modem
+ * @modem_desc: - description for modem
+ * @dev:        - device
+ * @driver_data:- driver specific data
+ *
+ * Register a modem with the modem access framework, so that
+ * it could be used by client drivers for accessing the
+ * modem.
+ */
+struct modem_dev *modem_register(struct modem_desc *modem_desc,
+		struct device *dev,
+		void *driver_data)
+{
+	static atomic_t modem_no = ATOMIC_INIT(0);
+	struct modem_dev *mdev;
+	int ret;
+
+	if (modem_desc == NULL)
+		return ERR_PTR(-EINVAL);
+
+	if (modem_desc->name == NULL || modem_desc->ops == NULL)
+		return ERR_PTR(-EINVAL);
+
+	mdev = kzalloc(sizeof(struct modem_dev), GFP_KERNEL);
+	if (mdev == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_lock(&modem_list_mutex);
+
+	mutex_init(&mdev->mutex);
+	mdev->modem_data = driver_data;
+	mdev->owner = modem_desc->owner;
+	mdev->desc = modem_desc;
+	INIT_LIST_HEAD(&mdev->client_list);
+	INIT_LIST_HEAD(&mdev->modem_list);
+	BLOCKING_INIT_NOTIFIER_HEAD(&mdev->notifier);
+
+	/* mdev->dev.class = &modem_class;*/
+	mdev->dev.parent = dev;
+	dev_set_name(&mdev->dev, "modem.%d", atomic_inc_return(&modem_no) - 1);
+	ret = device_register(&mdev->dev);
+	if (ret != 0)
+		goto clean;
+
+	dev_set_drvdata(&mdev->dev, mdev);
+
+	ret = add_modem_attributes(mdev);
+	if (ret < 0)
+		goto backoff;
+
+	list_add(&mdev->modem_list, &modem_list);
+
+out:
+	mutex_unlock(&modem_list_mutex);
+	return mdev;
+
+backoff:
+	device_unregister(&mdev->dev);
+	mdev = ERR_PTR(ret);
+	goto out;
+
+clean:
+	kfree(mdev);
+	mdev = ERR_PTR(ret);
+	goto out;
+}
+EXPORT_SYMBOL(modem_register);
diff --git a/drivers/modem/modem_u8500.c b/drivers/modem/modem_u8500.c
new file mode 100644
index 00000000000..20b5fe78ef7
--- /dev/null
+++ b/drivers/modem/modem_u8500.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com>
+ *
+ * Platform driver implementing access mechanisms to modem
+ * on U8500 which uses Shared Memroy as IPC between Application
+ * Processor and Modem processor.
+ */
+#include <linux/modem/modem.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <mach/prcmu.h>
+
+static void u8500_modem_request(struct modem_dev *mdev)
+{
+	prcmu_ac_wake_req();
+}
+
+static void u8500_modem_release(struct modem_dev *mdev)
+{
+	prcmu_ac_sleep_req();
+}
+
+static int u8500_modem_is_requested(struct modem_dev *mdev)
+{
+	return prcmu_is_ac_wake_requested();
+}
+
+static struct modem_ops u8500_modem_ops = {
+	.request = u8500_modem_request,
+	.release = u8500_modem_release,
+	.is_requested = u8500_modem_is_requested,
+};
+
+static struct modem_desc u8500_modem_desc = {
+	.name   = "u8500-shrm-modem",
+	.id     = 0,
+	.ops    = &u8500_modem_ops,
+	.owner  = THIS_MODULE,
+};
+
+
+static int __devinit u8500_modem_probe(struct platform_device *pdev)
+{
+	struct modem_dev *mdev;
+	int err;
+
+	mdev = modem_register(&u8500_modem_desc, &pdev->dev,
+			NULL);
+	if (IS_ERR(mdev)) {
+		err = PTR_ERR(mdev);
+		pr_err("failed to register %s: err %i\n",
+				u8500_modem_desc.name, err);
+	}
+
+	return 0;
+}
+
+static int __devexit u8500_modem_remove(struct platform_device *pdev)
+{
+
+	return 0;
+}
+
+static struct platform_driver u8500_modem_driver = {
+	.driver = {
+		.name = "u8500-modem",
+		.owner = THIS_MODULE,
+	},
+	.probe = u8500_modem_probe,
+	.remove = __devexit_p(u8500_modem_remove),
+};
+
+static int __init u8500_modem_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&u8500_modem_driver);
+	if (ret < 0) {
+		printk(KERN_ERR "u8500_modem: platform driver reg failed\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void __exit u8500_modem_exit(void)
+{
+	platform_driver_unregister(&u8500_modem_driver);
+}
+
+arch_initcall(u8500_modem_init);
diff --git a/drivers/modem/shrm/Kconfig b/drivers/modem/shrm/Kconfig
new file mode 100644
index 00000000000..465c8bb10a1
--- /dev/null
+++ b/drivers/modem/shrm/Kconfig
@@ -0,0 +1,43 @@
+#
+# SHM HW kernel configuration
+#
+config U8500_SHRM
+	bool "U8500 SHRM hardware driver"
+	depends on ARCH_U8500 && PHONET && MODEM_U8500
+	default Y
+	---help---
+	  If you say Y here, you will enable the STN8500 SHM hardware driver.
+
+	  If unsure, say N.
+choice
+	prompt "Modem Image Version"
+	depends on U8500_SHRM
+	default SHRM_V1_UPDATES_VERSION
+
+	config SHRM_V1_UPDATES_VERSION
+	depends on U8500_SHRM
+	bool "SHRM V1 UPDATES"
+	help
+	 Modem Images with V1 Updates
+
+endchoice
+
+config U8500_SHRM_LOOP_BACK
+	bool "U8500 SHRM loopback"
+	depends on U8500_SHRM
+	default n
+	---help---
+	  If you say Y here, you will enable the shm loopback
+
+	  If unsure, say N.
+
+config U8500_SHRM_MODEM_SILENT_RESET
+	bool "U8500 SHRM Modem Silent Reset"
+	depends on U8500_SHRM
+	default n
+	---help---
+	  If you say Y here, you will enable the modem silent reset feature
+
+	  If unsure, say N.
+
+
diff --git a/drivers/modem/shrm/Makefile b/drivers/modem/shrm/Makefile
new file mode 100644
index 00000000000..8115c24920b
--- /dev/null
+++ b/drivers/modem/shrm/Makefile
@@ -0,0 +1,11 @@
+#
+# Makefile for SHRM drivers
+#
+
+ifdef CONFIG_PHONET
+u8500_shrm-objs := 	modem_shrm_driver.o shrm_fifo.o shrm_protocol.o
+else
+u8500_shrm-objs := 	shrm_driver.o shrm_fifo.o shrm_protocol.o
+endif
+
+obj-$(CONFIG_U8500_SHRM)	+= u8500_shrm.o
diff --git a/drivers/modem/shrm/modem_shrm_driver.c b/drivers/modem/shrm/modem_shrm_driver.c
new file mode 100644
index 00000000000..5e986ab9a2c
--- /dev/null
+++ b/drivers/modem/shrm/modem_shrm_driver.c
@@ -0,0 +1,669 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/uaccess.h>
+#include <asm/atomic.h>
+#include <linux/io.h>
+#include <linux/skbuff.h>
+#ifdef CONFIG_HIGH_RES_TIMERS
+#include <linux/hrtimer.h>
+static struct hrtimer timer;
+#endif
+#include <linux/if_ether.h>
+#include <linux/netdevice.h>
+#include <linux/phonet.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_config.h>
+#include <linux/modem/shrm/shrm_net.h>
+#include <linux/modem/shrm/shrm.h>
+
+#include <mach/isa_ioctl.h>
+/* debug functionality */
+#define ISA_DEBUG 0
+
+#define PHONET_TASKLET
+#define MAX_RCV_LEN	2048
+
+static void do_phonet_rcv_tasklet(unsigned long unused);
+struct tasklet_struct phonet_rcv_tasklet;
+
+/**
+ * audio_receive() - Receive audio channel completion callback
+ * @shrm:	pointer to shrm device information structure
+ * @data:	message pointer
+ * @n_bytes:	message size
+ * @l2_header:	L2 header/device ID 2->audio, 5->audio_loopback
+ *
+ * This fucntion is called from the audio receive handler. Copies the audio
+ * message from the FIFO to the AUDIO queue. The message is later copied from
+ * this queue to the user buffer through the char or net interface read
+ * operation.
+ */
+static int audio_receive(struct shrm_dev *shrm, void *data,
+					u32 n_bytes, u8 l2_header)
+{
+	u32 size = 0;
+	int ret = 0;
+	int idx;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *audiodev;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	idx = shrm_get_cdev_index(l2_header);
+	if (idx < 0) {
+		dev_err(shrm->dev, "failed to get index\n");
+		return idx;
+	}
+	audiodev = &shrm->isa_context->isadev[idx];
+	q = &audiodev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+	ret = add_msg_to_queue(q, n_bytes);
+	spin_unlock(&q->update_lock);
+	if (ret < 0)
+		dev_err(shrm->dev, "Adding a msg to message queue failed");
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * common_receive() - Receive common channel completion callback
+ * @shrm:	pointer to the shrm device information structure
+ * @data:	message pointer
+ * @n_bytes:	message size
+ * @l2_header:	L2 header / device ID
+ *
+ * This function is called from the receive handler to copy the respective
+ * ISI, RPC, SECURITY message to its respective queue. The message is then
+ * copied from queue to the user buffer on char net interface read operation.
+ */
+static int common_receive(struct shrm_dev *shrm, void *data,
+					u32 n_bytes, u8 l2_header)
+{
+	u32 size = 0;
+	int ret = 0;
+	int idx;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *isa_dev;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	idx = shrm_get_cdev_index(l2_header);
+	if (idx < 0) {
+		dev_err(shrm->dev, "failed to get index\n");
+		return idx;
+	}
+	isa_dev = &shrm->isa_context->isadev[idx];
+	q = &isa_dev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		dev_dbg(shrm->dev, "Inside Loop Back\n");
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+	ret = add_msg_to_queue(q, n_bytes);
+	spin_unlock(&q->update_lock);
+	if (ret < 0) {
+		dev_err(shrm->dev, "Adding a msg to message queue failed");
+		return ret;
+	}
+
+
+	if (l2_header == ISI_MESSAGING) {
+		if (shrm->netdev_flag_up) {
+			dev_dbg(shrm->dev,
+				"scheduling the phonet tasklet from %s!\n",
+				__func__);
+			tasklet_schedule(&phonet_rcv_tasklet);
+		}
+		dev_dbg(shrm->dev,
+				"Out of phonet tasklet %s!!!\n", __func__);
+	}
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * rx_common_l2msg_handler() - common channel receive handler
+ * @l2_header:		L2 header
+ * @msg:		pointer to the receive buffer
+ * @length:		length of the msg to read
+ * @shrm:		pointer to shrm device information structure
+ *
+ * This function is called to receive the message from CaMsgPendingNotification
+ * interrupt handler.
+ */
+static void rx_common_l2msg_handler(u8 l2_header,
+				 void *msg, u32 length,
+				 struct shrm_dev *shrm)
+{
+	int ret = 0;
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	ret = common_receive(shrm, msg, length, l2_header);
+	if (ret < 0)
+		dev_err(shrm->dev,
+			"common receive with l2 header %d failed\n", l2_header);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+/**
+ * rx_audio_l2msg_handler() - audio channel receive handler
+ * @l2_header:		L2 header
+ * @msg:		pointer to the receive buffer
+ * @length:		length of the msg to read
+ * @shrm:		pointer to shrm device information structure
+ *
+ * This function is called to receive the message from CaMsgPendingNotification
+ * interrupt handler.
+ */
+static void rx_audio_l2msg_handler(u8 l2_header,
+				void *msg, u32 length,
+				struct shrm_dev *shrm)
+{
+	int ret = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	ret = audio_receive(shrm, msg, length, l2_header);
+	if (ret < 0)
+		dev_err(shrm->dev, "audio receive failed\n");
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+static int __init shm_initialise_irq(struct shrm_dev *shrm)
+{
+	int err = 0;
+
+	err  = shrm_protocol_init(shrm,
+			rx_common_l2msg_handler, rx_audio_l2msg_handler);
+	if (err < 0) {
+		dev_err(shrm->dev, "SHM Protocol Init Failure\n");
+		return err;
+	}
+
+	err = request_irq(shrm->ca_wake_irq,
+			ca_wake_irq_handler, IRQF_TRIGGER_RISING,
+				 "ca_wake-up", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"Unable to allocate shm tx interrupt line\n");
+		free_irq(shrm->ca_wake_irq, shrm);
+		return err;
+	}
+
+	err = request_irq(shrm->ac_read_notif_0_irq,
+		ac_read_notif_0_irq_handler, 0,
+		"ac_read_notif_0", shrm);
+
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error ac_read_notif_0_irq interrupt line\n");
+		goto irq_err1;
+	}
+
+	err = request_irq(shrm->ac_read_notif_1_irq,
+		ac_read_notif_1_irq_handler, 0,
+		"ac_read_notif_1", shrm);
+
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error ac_read_notif_1_irq interrupt line\n");
+		goto irq_err2;
+	}
+
+	err = request_irq(shrm->ca_msg_pending_notif_0_irq,
+		 ca_msg_pending_notif_0_irq_handler, 0,
+		"ca_msg_pending_notif_0", shrm);
+
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error ca_msg_pending_notif_0_irq line\n");
+		goto irq_err3;
+	}
+
+	err = request_irq(shrm->ca_msg_pending_notif_1_irq,
+		 ca_msg_pending_notif_1_irq_handler, 0,
+		"ca_msg_pending_notif_1", shrm);
+
+	if (err < 0) {
+		dev_err(shrm->dev,
+			"error ca_msg_pending_notif_1_irq interrupt line\n");
+		goto irq_err4;
+	}
+	return err;
+irq_err4:
+	free_irq(shrm->ca_msg_pending_notif_0_irq, shrm);
+irq_err3:
+	free_irq(shrm->ac_read_notif_1_irq, shrm);
+irq_err2:
+	free_irq(shrm->ac_read_notif_0_irq, shrm);
+irq_err1:
+	free_irq(shrm->ca_wake_irq, shrm);
+	return err;
+}
+
+static void free_shm_irq(struct shrm_dev *shrm)
+{
+	free_irq(shrm->ca_wake_irq, shrm);
+	free_irq(shrm->ac_read_notif_0_irq, shrm);
+	free_irq(shrm->ac_read_notif_1_irq, shrm);
+	free_irq(shrm->ca_msg_pending_notif_0_irq, shrm);
+	free_irq(shrm->ca_msg_pending_notif_1_irq, shrm);
+}
+
+
+
+#ifdef CONFIG_HIGH_RES_TIMERS
+static enum hrtimer_restart callback(struct hrtimer *timer)
+{
+	return HRTIMER_NORESTART;
+}
+#endif
+
+void do_phonet_rcv_tasklet(unsigned long unused)
+{
+	ssize_t ret;
+	struct shrm_dev *shrm = (struct shrm_dev *)unused;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	for (;;) {
+		ret = shrm_net_receive(shrm->ndev);
+		if (ret == 0) {
+			dev_dbg(shrm->dev, "len is zero, queue empty\n");
+			break;
+		}
+		if (ret < 0) {
+			dev_err(shrm->dev, "len < 0 !!! error!!!\n");
+			break;
+		}
+	}
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+static int shrm_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	struct resource *res;
+	struct shrm_dev *shrm = NULL;
+
+	shrm = kzalloc(sizeof(struct shrm_dev), GFP_KERNEL);
+	if (shrm == NULL) {
+		dev_err(&pdev->dev,
+			"Could not allocate memory for struct shm_dev\n");
+		return -ENOMEM;
+	}
+
+	shrm->dev = &pdev->dev;
+	shrm->modem = modem_get(shrm->dev, "u8500-shrm-modem");
+	if (shrm->modem == NULL) {
+		dev_err(shrm->dev, " Could not retrieve the modem.\n");
+		return -ENODEV;
+	}
+
+	/* initialise the SHM */
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(shrm->dev,
+				"Unable to map Ca Wake up interrupt\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_wake_irq = res->start;
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
+
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map APE_Read_notif_common IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ac_read_notif_0_irq = res->start;
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 2);
+
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map APE_Read_notif_audio IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ac_read_notif_1_irq = res->start;
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 3);
+
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map Cmt_msg_pending_notif_common IRQbase\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_msg_pending_notif_0_irq = res->start;
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 4);
+
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map Cmt_msg_pending_notif_audio IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_msg_pending_notif_1_irq = res->start;
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if (!res) {
+		dev_err(shrm->dev,
+				"Could not get SHM IO memory information\n");
+		err = -ENODEV;
+		goto rollback_intr;
+	}
+	shrm->intr_base = (void __iomem *)ioremap_nocache(res->start,
+					res->end - res->start + 1);
+	if (!(shrm->intr_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ape_common_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_APE_COMMON_BASE;
+	shrm->ape_common_fifo_base =
+		(void __iomem *)ioremap_nocache(
+					U8500_SHM_FIFO_APE_COMMON_BASE,
+					SHM_FIFO_0_SIZE);
+	shrm->ape_common_fifo_size = (SHM_FIFO_0_SIZE)/4;
+
+	if (!(shrm->ape_common_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ape_common_fifo_base;
+	}
+	shrm->cmt_common_fifo_base_phy =
+		(u32 *)U8500_SHM_FIFO_CMT_COMMON_BASE;
+	shrm->cmt_common_fifo_base =
+		(void __iomem *)ioremap_nocache(
+			U8500_SHM_FIFO_CMT_COMMON_BASE, SHM_FIFO_0_SIZE);
+	shrm->cmt_common_fifo_size = (SHM_FIFO_0_SIZE)/4;
+
+	if (!(shrm->cmt_common_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_cmt_common_fifo_base;
+	}
+	shrm->ape_audio_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_APE_AUDIO_BASE;
+	shrm->ape_audio_fifo_base =
+		(void __iomem *)ioremap_nocache(U8500_SHM_FIFO_APE_AUDIO_BASE,
+							SHM_FIFO_1_SIZE);
+	shrm->ape_audio_fifo_size = (SHM_FIFO_1_SIZE)/4;
+
+	if (!(shrm->ape_audio_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ape_audio_fifo_base;
+	}
+	shrm->cmt_audio_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_CMT_AUDIO_BASE;
+	shrm->cmt_audio_fifo_base =
+		(void __iomem *)ioremap_nocache(U8500_SHM_FIFO_CMT_AUDIO_BASE,
+							SHM_FIFO_1_SIZE);
+	shrm->cmt_audio_fifo_size = (SHM_FIFO_1_SIZE)/4;
+
+	if (!(shrm->cmt_audio_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_cmt_audio_fifo_base;
+	}
+	shrm->ac_common_shared_wptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_0_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_common_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ac_common_shared_wptr;
+	}
+	shrm->ac_common_shared_rptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_0_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_common_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ca_common_shared_wptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_0_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_common_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ca_common_shared_rptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_0_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_common_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ac_audio_shared_wptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_1_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_audio_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ac_audio_shared_rptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_1_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_audio_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ca_audio_shared_wptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_1_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_audio_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ca_audio_shared_rptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_1_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_audio_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+	if (isa_init(shrm) != 0) {
+		dev_err(shrm->dev, "Driver Initialization Error\n");
+		err = -EBUSY;
+	}
+	/* install handlers and tasklets */
+	if (shm_initialise_irq(shrm)) {
+		dev_err(shrm->dev,
+				"shm error in interrupt registration\n");
+		goto rollback_irq;
+	}
+#ifdef CONFIG_HIGH_RES_TIMERS
+	hrtimer_init(&timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	timer.function = callback;
+	hrtimer_start(&timer, ktime_set(0, 2*NSEC_PER_MSEC), HRTIMER_MODE_REL);
+#endif
+	err = shrm_register_netdev(shrm);
+	if (err < 0)
+		goto rollback_irq;
+
+	tasklet_init(&phonet_rcv_tasklet, do_phonet_rcv_tasklet, 0);
+	phonet_rcv_tasklet.data = (unsigned long)shrm;
+
+	platform_set_drvdata(pdev, shrm);
+
+	return err;
+rollback_irq:
+	free_shm_irq(shrm);
+rollback_map:
+	iounmap(shrm->ac_common_shared_wptr);
+	iounmap(shrm->ac_common_shared_rptr);
+	iounmap(shrm->ca_common_shared_wptr);
+	iounmap(shrm->ca_common_shared_rptr);
+	iounmap(shrm->ac_audio_shared_wptr);
+	iounmap(shrm->ac_audio_shared_rptr);
+	iounmap(shrm->ca_audio_shared_wptr);
+	iounmap(shrm->ca_audio_shared_rptr);
+rollback_ac_common_shared_wptr:
+	iounmap(shrm->cmt_audio_fifo_base);
+rollback_cmt_audio_fifo_base:
+	iounmap(shrm->ape_audio_fifo_base);
+rollback_ape_audio_fifo_base:
+	iounmap(shrm->cmt_common_fifo_base);
+rollback_cmt_common_fifo_base:
+	iounmap(shrm->ape_common_fifo_base);
+rollback_ape_common_fifo_base:
+	iounmap(shrm->intr_base);
+rollback_intr:
+	kfree(shrm);
+	return err;
+}
+
+static int __exit shrm_remove(struct platform_device *pdev)
+{
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+
+	free_shm_irq(shrm);
+	iounmap(shrm->intr_base);
+	iounmap(shrm->ape_common_fifo_base);
+	iounmap(shrm->cmt_common_fifo_base);
+	iounmap(shrm->ape_audio_fifo_base);
+	iounmap(shrm->cmt_audio_fifo_base);
+	iounmap(shrm->ac_common_shared_wptr);
+	iounmap(shrm->ac_common_shared_rptr);
+	iounmap(shrm->ca_common_shared_wptr);
+	iounmap(shrm->ca_common_shared_rptr);
+	iounmap(shrm->ac_audio_shared_wptr);
+	iounmap(shrm->ac_audio_shared_rptr);
+	iounmap(shrm->ca_audio_shared_wptr);
+	iounmap(shrm->ca_audio_shared_rptr);
+	shrm_unregister_netdev(shrm);
+	isa_exit(shrm);
+	kfree(shrm);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/**
+ * u8500_shrm_suspend() - This routine puts the SHRM in to sustend state.
+ * @dev:	pointer to device structure.
+ *
+ * This routine checks the current ongoing communication with Modem by
+ * examining the ca_wake state and prevents suspend if modem communication
+ * is on-going.
+ * If ca_wake = 1 (high), modem comm. is on-going; don't suspend
+ * If ca_wake = 0 (low), no comm. with modem on-going.Allow suspend
+ */
+int u8500_shrm_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+	int err;
+
+	dev_dbg(&pdev->dev, "%s called...\n", __func__);
+	dev_dbg(&pdev->dev, "ca_wake_req_state = %x\n",
+						get_ca_wake_req_state());
+
+	/* if ca_wake_req is high, prevent system suspend */
+	if (!get_ca_wake_req_state()) {
+		err = shrm_suspend_netdev(shrm->ndev);
+		return err;
+	} else
+		return -EBUSY;
+}
+
+/**
+ * u8500_shrm_resume() - This routine resumes the SHRM from suspend state.
+ * @dev:	pointer to device structure
+ *
+ * This routine restore back the current state of the SHRM
+ */
+int u8500_shrm_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+	int err;
+
+	dev_dbg(&pdev->dev, "%s called...\n", __func__);
+	err = shrm_resume_netdev(shrm->ndev);
+
+	return err;
+}
+
+static const struct dev_pm_ops shrm_dev_pm_ops = {
+	.suspend_noirq = u8500_shrm_suspend,
+	.resume_noirq = u8500_shrm_resume,
+};
+#endif
+
+static struct platform_driver shrm_driver = {
+	.remove = __exit_p(shrm_remove),
+	.driver = {
+		.name = "u8500_shrm",
+		.owner = THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm = &shrm_dev_pm_ops,
+#endif
+	},
+};
+
+static int __init shrm_driver_init(void)
+{
+	return platform_driver_probe(&shrm_driver, shrm_probe);
+}
+
+static void __exit shrm_driver_exit(void)
+{
+	platform_driver_unregister(&shrm_driver);
+}
+
+module_init(shrm_driver_init);
+module_exit(shrm_driver_exit);
+
+MODULE_AUTHOR("Biju Das, Kumar Sanghvi, Arun Murthy");
+MODULE_DESCRIPTION("Shared Memory Modem Driver Interface");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/modem/shrm/shrm_driver.c b/drivers/modem/shrm/shrm_driver.c
new file mode 100644
index 00000000000..11540831f95
--- /dev/null
+++ b/drivers/modem/shrm/shrm_driver.c
@@ -0,0 +1,1439 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#define DEBUG
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/smp_lock.h>
+#include <linux/poll.h>
+#include <linux/mutex.h>
+#include <linux/uaccess.h>
+#include <asm/atomic.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_config.h>
+#include <linux/modem/shrm/shrm.h>
+
+#include <mach/isa_ioctl.h>
+
+
+#ifdef CONFIG_HIGH_RES_TIMERS
+#include <linux/hrtimer.h>
+static struct hrtimer timer;
+#endif
+
+
+#define NAME "IPC_ISA"
+#define ISA_DEVICES 4
+/**debug functionality*/
+#define ISA_DEBUG 0
+
+#define ISI_MESSAGING (0)
+#define RPC_MESSAGING (1)
+#define AUDIO_MESSAGING (2)
+#define SECURITY_MESSAGING (3)
+
+#define SIZE_OF_FIFO (512*1024)
+
+static u8 message_fifo[4][SIZE_OF_FIFO];
+
+static u8 wr_isi_msg[10*1024];
+static u8 wr_rpc_msg[10*1024];
+static u8 wr_sec_msg[10*1024];
+static u8 wr_audio_msg[10*1024];
+
+/* global data */
+/*
+ * int major:This variable is exported to user as module_param to specify
+ * major number at load time
+ */
+static int major;
+module_param(major, int, 0);
+MODULE_PARM_DESC(major, "Major device number");
+/* global fops mutex */
+static DEFINE_MUTEX(isa_lock);
+rx_cb common_rx;
+rx_cb audio_rx;
+
+
+static int isi_receive(struct shrm_dev *shrm, void *data, u32 n_bytes);
+static int rpc_receive(struct shrm_dev *shrm, void *data, u32 n_bytes);
+static int audio_receive(struct shrm_dev *shrm, void *data, u32 n_bytes);
+static int security_receive(struct shrm_dev *shrm,
+						void *data, u32 n_bytes);
+
+static void rx_common_l2msg_handler(u8 l2_header,
+				 void *msg, u32 length,
+				 struct shrm_dev *shrm)
+{
+	int ret = 0;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+	u8 *pdata;
+#endif
+     dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	switch (l2_header) {
+	case ISI_MESSAGING:
+		ret = isi_receive(shrm, msg, length);
+		if (ret < 0)
+			dev_err(shrm->dev, "isi receive failed\n");
+		break;
+	case RPC_MESSAGING:
+		ret  = rpc_receive(shrm, msg, length);
+		if (ret < 0)
+			dev_err(shrm->dev, "rpc receive failed\n");
+		break;
+	case SECURITY_MESSAGING:
+		ret = security_receive(shrm, msg, length);
+		if (ret < 0)
+			dev_err(shrm->dev,
+					"security receive failed\n");
+		break;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+	case COMMMON_LOOPBACK_MESSAGING:
+		pdata = (u8 *)msg;
+		if ((*pdata == 0x50) || (*pdata == 0xAF)) {
+			ret = isi_receive(shrm, msg, length);
+			if (ret < 0)
+				dev_err(shrm->dev, "isi receive failed\n");
+		} else if ((*pdata == 0x0A) || (*pdata == 0xF5)) {
+			ret = rpc_receive(shrm, msg, length);
+			if (ret < 0)
+				dev_err(shrm->dev, "rpc receive failed\n");
+		} else if ((*pdata == 0xFF) || (*pdata == 0x00)) {
+			ret = security_receive(shrm, msg, length);
+			if (ret < 0)
+				dev_err(shrm->dev,
+						"security receive failed\n");
+		}
+		break;
+#endif
+	default:
+		break;
+	}
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+static void rx_audio_l2msg_handler(u8 l2_header,
+				void *msg, u32 length,
+				struct shrm_dev *shrm)
+{
+	int ret = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	audio_receive(shrm, msg, length);
+	if (ret < 0)
+		dev_err(shrm->dev, "audio receive failed\n");
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+static int __init shm_initialise_irq(struct shrm_dev *shrm)
+{
+	int err = 0;
+
+	shrm_protocol_init(shrm,
+			rx_common_l2msg_handler, rx_audio_l2msg_handler);
+
+	err = request_irq(shrm->ca_wake_irq,
+			ca_wake_irq_handler, IRQF_TRIGGER_RISING,
+				 "ca_wake-up", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"Unable to allocate shm tx interrupt line\n");
+		return err;
+	}
+
+	err = request_irq(shrm->ac_read_notif_0_irq,
+			ac_read_notif_0_irq_handler, 0,
+			"ac_read_notif_0", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error ac_read_notif_0_irq interrupt line\n");
+		goto irq_err1;
+	}
+
+	err = request_irq(shrm->ac_read_notif_1_irq,
+			ac_read_notif_1_irq_handler, 0,
+			"ac_read_notif_1", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error ac_read_notif_1_irq interrupt line\n");
+		goto irq_err2;
+	}
+
+	err = request_irq(shrm->ca_msg_pending_notif_0_irq,
+			 ca_msg_pending_notif_0_irq_handler, 0,
+			"ca_msg_pending_notif_0", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error  ca_msg_pending_notif_0_irq line\n");
+		goto irq_err3;
+	}
+
+	err = request_irq(shrm->ca_msg_pending_notif_1_irq,
+			 ca_msg_pending_notif_1_irq_handler, 0,
+			"ca_msg_pending_notif_1", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+			"error ca_msg_pending_notif_1_irq interrupt line\n");
+		goto irq_err4;
+	}
+
+	return err;
+
+irq_err4:
+	free_irq(shrm->ca_msg_pending_notif_0_irq, shrm);
+irq_err3:
+	free_irq(shrm->ac_read_notif_1_irq, shrm);
+irq_err2:
+	free_irq(shrm->ac_read_notif_0_irq, shrm);
+irq_err1:
+	free_irq(shrm->ca_wake_irq, shrm);
+	return err;
+}
+
+static void free_shm_irq(struct shrm_dev *shrm)
+{
+	free_irq(shrm->ca_wake_irq, shrm);
+	free_irq(shrm->ac_read_notif_0_irq, shrm);
+	free_irq(shrm->ac_read_notif_1_irq, shrm);
+	free_irq(shrm->ca_msg_pending_notif_0_irq, shrm);
+	free_irq(shrm->ca_msg_pending_notif_1_irq, shrm);
+}
+
+/**
+ * create_queue() - To create FIFO for Tx and Rx message buffering.
+ * @q: message queue.
+ * @devicetype: device type 0-isi,1-rpc,2-audio,3-security.
+ *
+ * This function creates a FIFO buffer of n_bytes size using
+ * dma_alloc_coherent(). It also initializes all queue handling
+ * locks, queue management pointers. It also initializes message list
+ * which occupies this queue.
+ *
+ * It return -ENOMEM in case of no memory.
+ */
+static int create_queue(struct message_queue *q, u32 devicetype,
+					struct shrm_dev *shrm)
+{
+	q->fifo_base = (u8 *)&message_fifo[devicetype];
+	q->size = SIZE_OF_FIFO;
+	q->readptr = 0;
+	q->writeptr = 0;
+	q->no = 0;
+	q->shrm = shrm;
+	spin_lock_init(&q->update_lock);
+	INIT_LIST_HEAD(&q->msg_list);
+	init_waitqueue_head(&q->wq_readable);
+	atomic_set(&q->q_rp, 0);
+
+	return 0;
+}
+/**
+ * delete_queue() - To delete FIFO and assiciated memory.
+ * @q: message queue
+ *
+ * This function deletes FIFO created using create_queue() function.
+ * It resets queue management pointers.
+ */
+static void delete_queue(struct message_queue *q)
+{
+	q->size = 0;
+	q->readptr = 0;
+	q->writeptr = 0;
+}
+
+/**
+ * add_msg_to_queue() - Add a message inside inside queue
+ *
+ * @q: message queue
+ * @size: size in bytes
+ *
+ * This function tries to allocate n_bytes of size in FIFO q.
+ * It returns negative number when no memory can be allocated
+ * currently.
+ */
+int add_msg_to_queue(struct message_queue *q, u32 size)
+{
+	struct queue_element *new_msg = NULL;
+	struct shrm_dev *shrm = q->shrm;
+
+	dev_dbg(shrm->dev, "%s IN q->writeptr=%d\n",
+						__func__, q->writeptr);
+	new_msg = kmalloc(sizeof(struct queue_element),
+						 GFP_KERNEL|GFP_ATOMIC);
+
+	if (new_msg == NULL) {
+		dev_err(shrm->dev, "memory overflow inside while(1)\n");
+		return -ENOMEM;
+	}
+	new_msg->offset = q->writeptr;
+	new_msg->size = size;
+	new_msg->no = q->no++;
+
+	/* check for overflow condition */
+	if (q->readptr <= q->writeptr) {
+		if (((q->writeptr-q->readptr) + size) >= q->size) {
+			dev_err(shrm->dev, "Buffer overflow !!\n");
+			BUG_ON(((q->writeptr-q->readptr) + size) >= q->size);
+		}
+	} else {
+		if ((q->writeptr + size) >= q->readptr) {
+			dev_err(shrm->dev, "Buffer overflow !!\n");
+			BUG_ON((q->writeptr + size) >= q->readptr);
+		}
+	}
+	q->writeptr = (q->writeptr + size) % q->size;
+	if (list_empty(&q->msg_list)) {
+		list_add_tail(&new_msg->entry, &q->msg_list);
+		/* There can be 2 blocking calls read  and another select */
+
+		atomic_set(&q->q_rp, 1);
+		wake_up_interruptible(&q->wq_readable);
+	} else
+		list_add_tail(&new_msg->entry, &q->msg_list);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return 0;
+}
+
+/**
+ * remove_msg_from_queue() - To remove a message from the msg queue.
+ *
+ * @q: message queue
+ *
+ * This function delets a message from the message list associated with message
+ * queue q and also updates read ptr.
+ * If the message list is empty, then, event is set to block the select and
+ * read calls of the paricular queue.
+ *
+ * The message list is FIFO style and message is always added to tail and
+ * removed from head.
+ */
+
+int remove_msg_from_queue(struct message_queue *q)
+{
+	struct queue_element *old_msg = NULL;
+	struct shrm_dev *shrm = q->shrm;
+	struct list_head *msg;
+
+	dev_dbg(shrm->dev, "%s IN q->readptr %d\n",
+					__func__, q->readptr);
+
+	list_for_each(msg, &q->msg_list) {
+		old_msg = list_entry(msg, struct queue_element, entry);
+		if (old_msg == NULL) {
+			dev_err(shrm->dev, ":no message found\n");
+			return -EFAULT;
+		}
+		break;
+	}
+	list_del(msg);
+	q->readptr = (q->readptr + old_msg->size) % q->size;
+	if (list_empty(&q->msg_list)) {
+		dev_dbg(shrm->dev, "List is empty setting RP= 0\n");
+		atomic_set(&q->q_rp, 0);
+	}
+	kfree(old_msg);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return 0;
+}
+
+/**
+ * get_size_of_new_msg() - retrieve new message from message list
+ *
+ * @q: message queue
+ *
+ * This function will retrieve most recent message from the corresponding
+ * queue list. New message is always retrieved from head side.
+ * It returns new message no, offset if FIFO and size.
+ */
+int get_size_of_new_msg(struct message_queue *q)
+{
+	struct queue_element *new_msg = NULL;
+	struct list_head *msg_list;
+	struct shrm_dev *shrm = q->shrm;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	spin_lock_bh(&q->update_lock);
+	list_for_each(msg_list, &q->msg_list) {
+		new_msg = list_entry(msg_list, struct queue_element, entry);
+		if (new_msg == NULL) {
+			spin_unlock_bh(&q->update_lock);
+			dev_err(shrm->dev, "no message found\n");
+			return -1;
+		}
+		break;
+	}
+	spin_unlock_bh(&q->update_lock);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return new_msg->size;
+}
+
+/**
+ * isi_receive() - Rx Completion callback
+ *
+ * @data:message pointer
+ * @n_bytes:message size
+ *
+ * This function is a callback to indicate ISI message reception is complete.
+ * It updates Writeptr of the Fifo
+ */
+static int isi_receive(struct shrm_dev *shrm,
+					void *data, u32 n_bytes)
+{
+	u32 size = 0;
+	int ret = 0;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *isidev = &shrm->isa_context->isadev[0];
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	q = &isidev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		dev_dbg(shrm->dev, "Inside Loop Back\n");
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+	ret = add_msg_to_queue(q, n_bytes);
+	if (ret < 0)
+		dev_err(shrm->dev, "Adding msg to message queue failed\n");
+	spin_unlock(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * rpc_receive() - Rx Completion callback
+ *
+ * @data:message pointer
+ * @n_bytes:message size
+ *
+ * This function is a callback to indicate RPC message reception is complete.
+ * It updates Writeptr of the Fifo
+ */
+static int rpc_receive(struct shrm_dev *shrm,
+					void *data, u32 n_bytes)
+{
+	u32 size = 0;
+	int ret = 0;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *rpcdev = &shrm->isa_context->isadev[1];
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	q = &rpcdev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+
+	ret = add_msg_to_queue(q, n_bytes);
+	if (ret < 0)
+		dev_err(shrm->dev, "Adding msg to message queue failed\n");
+	spin_unlock(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * audio_receive() - Rx Completion callback
+ *
+ * @data:message pointer
+ * @n_bytes:message size
+ *
+ * This function is a callback to indicate audio message reception is complete.
+ * It updates Writeptr of the Fifo
+ */
+static int audio_receive(struct shrm_dev *shrm,
+					void *data, u32 n_bytes)
+{
+	u32 size = 0;
+	int ret = 0;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *audiodev = &shrm->isa_context->isadev[2];
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	q = &audiodev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+	ret = add_msg_to_queue(q, n_bytes);
+	if (ret < 0)
+		dev_err(shrm->dev, "Adding msg to message queue failed\n");
+	spin_unlock(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * security_receive() - Rx Completion callback
+ *
+ * @data:message pointer
+ * @n_bytes: message size
+ *
+ * This function is a callback to indicate security message reception
+ * is complete.It updates Writeptr of the Fifo
+ */
+static int security_receive(struct shrm_dev *shrm,
+					void *data, u32 n_bytes)
+{
+	u32 size = 0;
+	int ret = 0;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *secdev = &shrm->isa_context->isadev[3];
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	q = &secdev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+	ret = add_msg_to_queue(q, n_bytes);
+	if (ret < 0)
+		dev_err(shrm->dev, "Adding msg to message queue failed\n");
+	spin_unlock(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+
+/**
+ * isa_select() - Select Interface
+ *
+ * @filp:file descriptor pointer
+ * @wait:poll_table_struct pointer
+ *
+ * This function is used to perform non-blocking read operations. It allows
+ * a process to determine whether it can read from one or more open files
+ * without blocking. These calls can also block a process until any of a
+ * given set of file descriptors becomes available for reading.
+ * If a file is ready to read, POLLIN | POLLRDNORM bitmask is returned.
+ * The driver method is called whenever the user-space program performs a select
+ * system call involving a file descriptor associated with the driver.
+ */
+static u32 isa_select(struct file *filp,
+				struct poll_table_struct *wait)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	u32 mask = 0;
+	u32 m = iminor(filp->f_path.dentry->d_inode);
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (isadev->device_id != m)
+			return -1;
+	q = &isadev->dl_queue;
+	poll_wait(filp, &q->wq_readable, wait);
+	if (atomic_read(&q->q_rp) == 1)
+		mask = POLLIN | POLLRDNORM;
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return mask;
+}
+
+/**
+ * isa_read() - Read from device
+ *
+ * @filp:file descriptor
+ * @buf:user buffer pointer
+ * @len:size of requested data transfer
+ * @ppos:not used
+ *
+ * This function is called whenever user calls read() system call.
+ * It reads a oldest message from queue and copies it into user buffer and
+ * returns its size.
+ * If there is no message present in queue, then it blocks until new data is
+ * available.
+ */
+ssize_t isa_read(struct file *filp, char __user *buf,
+				size_t len, loff_t *ppos)
+{
+	struct isadev_context *isadev = (struct isadev_context *)
+							filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	char *psrc;
+	u32 msgsize;
+	u32 size = 0;
+	int ret = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (len <= 0)
+		return -EFAULT;
+	q = &isadev->dl_queue;
+
+	spin_lock_bh(&q->update_lock);
+	if (list_empty(&q->msg_list)) {
+		spin_unlock_bh(&q->update_lock);
+		if (wait_event_interruptible(q->wq_readable,
+				atomic_read(&q->q_rp) == 1)) {
+			return -ERESTARTSYS;
+		}
+	} else
+		spin_unlock_bh(&q->update_lock);
+
+	msgsize = get_size_of_new_msg(q);
+	if ((q->readptr+msgsize) >= q->size) {
+		dev_dbg(shrm->dev, "Inside Loop Back\n");
+		psrc = (char *)buf;
+		size = (q->size-q->readptr);
+		/* Copy First Part of msg */
+		if (copy_to_user(psrc,
+				(u8 *)(q->fifo_base+q->readptr),
+				size)) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		if (copy_to_user(psrc,
+				(u8 *)(q->fifo_base),
+				(msgsize-size))) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+	} else {
+		if (copy_to_user(buf,
+				(u8 *)(q->fifo_base+q->readptr),
+				msgsize)) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+	}
+
+	spin_lock_bh(&q->update_lock);
+	ret = remove_msg_from_queue(q);
+	if (ret < 0) {
+		dev_err(shrm->dev,
+				"Removing msg from message queue failed\n");
+		msgsize = ret;
+	}
+	spin_unlock_bh(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return msgsize;
+}
+/**
+ * isa_write() - Write to device
+ *
+ * @filp:file descriptor
+ * @buf:user buffer pointer
+ * @len:size of requested data transfer
+ * @ppos:not used
+ *
+ * This function is called whenever user calls write() system call.
+ * It checks if there is space available in queue, and copies the message
+ * inside queue. If there is no space, it blocks until space becomes available.
+ * It also schedules transfer thread to transmit the newly added message.
+ */
+static ssize_t isa_write(struct file *filp, const char __user *buf,
+				 size_t len, loff_t *ppos)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	int err, ret;
+	void *addr = 0;
+	u8 l2_header = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	if (len <= 0)
+		return -EFAULT;
+	q = &isadev->dl_queue;
+
+	switch (isadev->device_id) {
+	case ISI_MESSAGING:
+		dev_dbg(shrm->dev, "ISI\n");
+		addr = (void *)wr_isi_msg;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+		dev_dbg(shrm->dev, "Loopback\n");
+		l2_header = COMMON_LOOPBACK_MESSAGING;
+#else
+		l2_header = isadev->device_id;
+#endif
+		break;
+	case RPC_MESSAGING:
+		dev_dbg(shrm->dev, "RPC\n");
+		addr = (void *)wr_rpc_msg;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+		l2_header = COMMON_LOOPBACK_MESSAGING;
+#else
+		l2_header = isadev->device_id;
+#endif
+		break;
+	case AUDIO_MESSAGING:
+		dev_dbg(shrm->dev, "Audio\n");
+		addr = (void *)wr_audio_msg;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+		l2_header = AUDIO_LOOPBACK_MESSAGING;
+#else
+		l2_header = isadev->device_id;
+#endif
+
+		break;
+	case SECURITY_MESSAGING:
+		dev_dbg(shrm->dev, "Security\n");
+		addr = (void *)wr_sec_msg;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+		l2_header = COMMON_LOOPBACK_MESSAGING;
+#else
+		l2_header = isadev->device_id;
+#endif
+		break;
+	default:
+		dev_dbg(shrm->dev, "Wrong device\n");
+		return -EFAULT;
+	}
+
+	if (copy_from_user(addr, buf, len)) {
+		dev_err(shrm->dev, "copy_from_user failed\n");
+		return -EFAULT;
+	}
+
+	/* Write msg to Fifo */
+	if (isadev->device_id == 2) {
+		mutex_lock(&shrm->isa_context->tx_audio_mutex);
+		err = shm_write_msg(shrm, l2_header, addr, len);
+		if (!err)
+			ret = len;
+		else
+			ret = err;
+		mutex_unlock(&shrm->isa_context->tx_audio_mutex);
+	} else {
+		spin_lock_bh(&shrm->isa_context->common_tx);
+		err = shm_write_msg(shrm, l2_header, addr, len);
+		if (!err)
+			ret = len;
+		else
+			ret = err;
+		spin_unlock_bh(&shrm->isa_context->common_tx);
+	}
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * isa_ioctl() - To handle different ioctl commands supported by driver.
+ *
+ * @inode: structure is used by the kernel internally to represent files
+ * @filp:file descriptor pointer
+ * @cmd:ioctl command
+ * @arg:input param
+ *
+ * Following ioctls are supported by this driver.
+ * DLP_IOCTL_ALLOCATE_BUFFER - To allocate buffer for new uplink message.
+ * This ioctl is called with required message size. It returns offset for
+ * the allocates space in the queue. DLP_IOCTL_PUT_MESSAGE -  To indicate
+ * new uplink message available in queuq for  transmission. Message is copied
+ * from offset location returned by previous ioctl before calling this ioctl.
+ * DLP_IOCTL_GET_MESSAGE - To check if any downlink message is available in
+ * queue. It returns  offset for new message inside queue.
+ * DLP_IOCTL_DEALLOCATE_BUFFER - To deallocate any buffer allocate for
+ * downlink message once the message is copied. Message is copied from offset
+ * location returned by previous ioctl before calling this ioctl.
+ */
+static int isa_ioctl(struct inode *inode, struct file *filp,
+				unsigned cmd, unsigned long arg)
+{
+	int err = 0;
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	u32 m = iminor(inode);
+
+	if (isadev->device_id != m)
+		return -1;
+
+	switch (cmd) {
+	case DLP_IOC_ALLOCATE_BUFFER:
+		dev_dbg(shrm->dev, "DLP_IOC_ALLOCATE_BUFFER\n");
+		break;
+	case DLP_IOC_PUT_MESSAGE:
+		dev_dbg(shrm->dev, "DLP_IOC_PUT_MESSAGE\n");
+		break;
+	case DLP_IOC_GET_MESSAGE:
+		dev_dbg(shrm->dev, "DLP_IOC_GET_MESSAGE\n");
+		break;
+	case DLP_IOC_DEALLOCATE_BUFFER:
+		dev_dbg(shrm->dev, "DLP_IOC_DEALLOCATE_BUFFER\n");
+		break;
+	default:
+		dev_dbg(shrm->dev, "Unknown IOCTL\n");
+		err = -1;
+		break;
+	}
+	return err;
+}
+/**
+ * isa_mmap() - Maps kernel queue memory to user space.
+ *
+ * @filp:file descriptor pointer
+ * @vma:virtual area memory structure.
+ *
+ * This function maps kernel FIFO into user space. This function
+ * shall be called twice to map both uplink and downlink buffers.
+ */
+static int isa_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+
+	u32	m = iminor(filp->f_path.dentry->d_inode);
+	dev_dbg(shrm->dev, "%s %dIN\n", __func__, m);
+
+	isadev = (struct isadev_context *)filp->private_data;
+	return 0;
+}
+
+/**
+ * isa_close() - Close device file
+ *
+ * @inode:structure is used by the kernel internally to represent files
+ * @filp:device file descriptor
+ *
+ * This function deletes structues associated with this file, deletes
+ * queues, flushes and destroys workqueus and closes this file.
+ * It also unregisters itself from l2mux driver.
+ */
+static int isa_close(struct inode *inode, struct file *filp)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct isa_driver_context *isa_context = shrm->isa_context;
+	u8 m;
+
+	mutex_lock(&isa_lock);
+	m = iminor(filp->f_path.dentry->d_inode);
+	dev_dbg(shrm->dev, "%s IN %d", __func__, m);
+
+	if (atomic_dec_and_test(&isa_context->is_open[m])) {
+		atomic_inc(&isa_context->is_open[m]);
+		dev_err(shrm->dev, "Device not opened yet\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	}
+	atomic_set(&isa_context->is_open[m], 1);
+
+	dev_dbg(shrm->dev, "isadev->device_id %d", isadev->device_id);
+	dev_dbg(shrm->dev, "Closed %d device\n", m);
+
+	if (m == ISI_MESSAGING)
+		dev_dbg(shrm->dev, "Closed ISI_MESSAGING Device\n");
+	else if (m == RPC_MESSAGING)
+		dev_dbg(shrm->dev, "Closed RPC_MESSAGING Device\n");
+	else if (m == AUDIO_MESSAGING)
+		dev_dbg(shrm->dev, "Closed AUDIO_MESSAGING Device\n");
+	else if (m == SECURITY_MESSAGING)
+		dev_dbg(shrm->dev, "Closed SECURITY_MESSAGING Device\n");
+	else
+		dev_dbg(shrm->dev, NAME ":No such device present\n");
+
+	mutex_unlock(&isa_lock);
+	return 0;
+}
+/**
+ * isa_open() -  Open device file
+ *
+ * @inode: structure is used by the kernel internally to represent files
+ * @filp: device file descriptor
+ *
+ * This function performs initialization tasks needed to open SHM channel.
+ * Following tasks are performed.
+ * -return if device is already opened
+ * -create uplink FIFO
+ * -create downlink FIFO
+ * -init delayed workqueue thread
+ * -register to l2mux driver
+ */
+static int isa_open(struct inode *inode, struct file *filp)
+{
+	int err = 0;
+	u8 m;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context = container_of(
+			inode->i_cdev,
+			struct isa_driver_context,
+			cdev);
+	struct shrm_dev *shrm = isa_context->isadev->dl_queue.shrm;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (get_boot_state() != BOOT_DONE) {
+		dev_err(shrm->dev, "Boot is not done\n");
+		return -EBUSY;
+	}
+	mutex_lock(&isa_lock);
+	m = iminor(inode);
+
+	if ((m != ISI_MESSAGING) && (m != RPC_MESSAGING) &&
+		(m != AUDIO_MESSAGING) && (m != SECURITY_MESSAGING)) {
+		dev_err(shrm->dev, "No such device present\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	}
+	if (!atomic_dec_and_test(&isa_context->is_open[m])) {
+		atomic_inc(&isa_context->is_open[m]);
+		dev_err(shrm->dev, "Device already opened\n");
+		mutex_unlock(&isa_lock);
+		return -EBUSY;
+	}
+
+	if (m == ISI_MESSAGING)
+		dev_dbg(shrm->dev, "Open ISI_MESSAGING Device\n");
+	else if (m == RPC_MESSAGING)
+		dev_dbg(shrm->dev, "Open RPC_MESSAGING Device\n");
+	else if (m == AUDIO_MESSAGING)
+		dev_dbg(shrm->dev, "Open AUDIO_MESSAGING Device\n");
+	else if (m == SECURITY_MESSAGING)
+		dev_dbg(shrm->dev, "Open SECURITY_MESSAGING Device\n");
+	else
+		dev_dbg(shrm->dev, ":No such device present\n");
+
+	isadev = &isa_context->isadev[m];
+	if (filp != NULL)
+		filp->private_data = isadev;
+
+	mutex_unlock(&isa_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return err;
+}
+
+const struct file_operations isa_fops = {
+	.owner	 = THIS_MODULE,
+	.open	 = isa_open,
+	.release = isa_close,
+	.ioctl	 = isa_ioctl,
+	.mmap	 = isa_mmap,
+	.read	 = isa_read,
+	.write	 = isa_write,
+	.poll	 = isa_select,
+};
+
+/**
+ * isa_init() - module insertion function
+ *
+ * This function registers module as a character driver using
+ * register_chrdev_region() or alloc_chrdev_region. It adds this
+ * driver to system using cdev_add() call. Major number is dynamically
+ * allocated using alloc_chrdev_region() by default or left to user to specify
+ * it during load time. For this variable major is used as module_param
+ * Nodes to be created using
+ * mknod /dev/isi c $major 0
+ * mknod /dev/rpc c $major 1
+ * mknod /dev/audio c $major 2
+ * mknod /dev/sec c $major 3
+ */
+int isa_init(struct shrm_dev *shrm)
+{
+	dev_t	dev_id;
+	int	retval, no_dev;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context;
+
+	isa_context = kzalloc(sizeof(struct isa_driver_context),
+							GFP_KERNEL);
+	shrm->isa_context = isa_context;
+	if (isa_context == NULL) {
+		dev_err(shrm->dev, "Failed to alloc memory\n");
+		return -ENOMEM;
+	}
+
+	if (major) {
+		dev_id = MKDEV(major, 0);
+		retval = register_chrdev_region(dev_id, ISA_DEVICES, NAME);
+	} else {
+		retval = alloc_chrdev_region(&dev_id, 0, ISA_DEVICES, NAME);
+		major = MAJOR(dev_id);
+	}
+
+	dev_dbg(shrm->dev, "major %d\n", major);
+
+	cdev_init(&isa_context->cdev, &isa_fops);
+	isa_context->cdev.owner = THIS_MODULE;
+	retval = cdev_add(&isa_context->cdev, dev_id, ISA_DEVICES);
+	if (retval) {
+		dev_err(shrm->dev, "Failed to add char device\n");
+		return retval;
+	}
+
+	for (no_dev = 0; no_dev < ISA_DEVICES; no_dev++)
+		atomic_set(&isa_context->is_open[no_dev], 1);
+
+	isa_context->isadev = kzalloc(sizeof
+					(struct isadev_context)*ISA_DEVICES,
+					GFP_KERNEL);
+	if (isa_context->isadev == NULL) {
+		dev_err(shrm->dev, "Failed to alloc memory\n");
+		return -ENOMEM;
+	}
+	for (no_dev = 0; no_dev < ISA_DEVICES; no_dev++) {
+		isadev = &isa_context->isadev[no_dev];
+		isadev->device_id = no_dev;
+		retval = create_queue(&isadev->dl_queue,
+					isadev->device_id, shrm);
+		if (retval < 0) {
+			dev_err(shrm->dev, "create dl_queue failed\n");
+			delete_queue(&isadev->dl_queue);
+			kfree(isadev);
+			return retval;
+		}
+	}
+	mutex_init(&isa_context->tx_audio_mutex);
+	spin_lock_init(&isa_context->common_tx);
+
+	dev_err(shrm->dev, "SHRM char driver added\n");
+
+	return retval;
+}
+
+void isa_exit(struct shrm_dev *shrm)
+{
+	int	no_dev;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context = shrm->isa_context;
+	dev_t dev_id = MKDEV(major, 0);
+
+	for (no_dev = 0; no_dev < ISA_DEVICES; no_dev++) {
+		isadev = &isa_context->isadev[no_dev];
+		delete_queue(&isadev->dl_queue);
+		kfree(isadev);
+	}
+
+	cdev_del(&isa_context->cdev);
+	unregister_chrdev_region(dev_id, ISA_DEVICES);
+	kfree(isa_context);
+
+	dev_err(shrm->dev, "SHRM char driver removed\n");
+}
+
+#ifdef CONFIG_HIGH_RES_TIMERS
+static enum hrtimer_restart callback(struct hrtimer *timer)
+{
+	return HRTIMER_NORESTART;
+}
+#endif
+
+
+static int __init shrm_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	struct resource *res;
+	struct shrm_dev *shrm = NULL;
+
+	if (pdev == NULL)  {
+		dev_err(shrm->dev,
+			"No device/platform_data found on shm device\n");
+		return -ENODEV;
+	}
+
+
+	shrm = kzalloc(sizeof(struct shrm_dev), GFP_KERNEL);
+	if (shrm == NULL) {
+		dev_err(shrm->dev,
+			"Could not allocate memory for struct shm_dev\n");
+		return -ENOMEM;
+	}
+	shrm->dev = &pdev->dev;
+
+	/* initialise the SHM */
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(shrm->dev, "Unable to map Ca Wake up interrupt\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_wake_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map APE_Read_notif_common IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ac_read_notif_0_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 2);
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map APE_Read_notif_audio IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ac_read_notif_1_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 3);
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map Cmt_msg_pending_notif_common IRQbase\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_msg_pending_notif_0_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 4);
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map Cmt_msg_pending_notif_audio IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_msg_pending_notif_1_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(shrm->dev,
+				"Could not get SHM IO memory information\n");
+		err = -ENODEV;
+		goto rollback_intr;
+	}
+
+	shrm->intr_base = (void __iomem *)ioremap_nocache(res->start,
+					res->end - res->start + 1);
+
+	if (!(shrm->intr_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+
+	shrm->ape_common_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_APE_COMMON_BASE;
+	shrm->ape_common_fifo_base =
+		(void __iomem *)ioremap_nocache(
+					U8500_SHM_FIFO_APE_COMMON_BASE,
+					SHM_FIFO_0_SIZE);
+	shrm->ape_common_fifo_size = (SHM_FIFO_0_SIZE)/4;
+
+	if (!(shrm->ape_common_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ape_common_fifo_base;
+	}
+
+	shrm->cmt_common_fifo_base_phy =
+		(u32 *)U8500_SHM_FIFO_CMT_COMMON_BASE;
+
+	shrm->cmt_common_fifo_base =
+		(void __iomem *)ioremap_nocache(
+			U8500_SHM_FIFO_CMT_COMMON_BASE, SHM_FIFO_0_SIZE);
+	shrm->cmt_common_fifo_size = (SHM_FIFO_0_SIZE)/4;
+
+	if (!(shrm->cmt_common_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_cmt_common_fifo_base;
+	}
+
+	shrm->ape_audio_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_APE_AUDIO_BASE;
+	shrm->ape_audio_fifo_base =
+		(void __iomem *)ioremap_nocache(U8500_SHM_FIFO_APE_AUDIO_BASE,
+							SHM_FIFO_1_SIZE);
+	shrm->ape_audio_fifo_size = (SHM_FIFO_1_SIZE)/4;
+
+	if (!(shrm->ape_audio_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ape_audio_fifo_base;
+	}
+
+	shrm->cmt_audio_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_CMT_AUDIO_BASE;
+	shrm->cmt_audio_fifo_base =
+		(void __iomem *)ioremap_nocache(U8500_SHM_FIFO_CMT_AUDIO_BASE,
+							SHM_FIFO_1_SIZE);
+	shrm->cmt_audio_fifo_size = (SHM_FIFO_1_SIZE)/4;
+
+	if (!(shrm->cmt_audio_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_cmt_audio_fifo_base;
+	}
+
+	shrm->ac_common_shared_wptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_0_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_common_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ac_common_shared_wptr;
+	}
+
+	shrm->ac_common_shared_rptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_0_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_common_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	shrm->ca_common_shared_wptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_0_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_common_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+	shrm->ca_common_shared_rptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_0_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_common_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	shrm->ac_audio_shared_wptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_1_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_audio_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	shrm->ac_audio_shared_rptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_1_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_audio_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	shrm->ca_audio_shared_wptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_1_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_audio_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	shrm->ca_audio_shared_rptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_1_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_audio_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	if (isa_init(shrm) != 0) {
+		dev_err(shrm->dev, "Driver Initialization Error\n");
+		err = -EBUSY;
+	}
+	/* install handlers and tasklets */
+	if (shm_initialise_irq(shrm)) {
+		dev_err(shrm->dev, "shm error in interrupt registration\n");
+		goto rollback_irq;
+	}
+
+#ifdef CONFIG_HIGH_RES_TIMERS
+	hrtimer_init(&timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	timer.function = callback;
+
+	hrtimer_start(&timer, ktime_set(0, 2*NSEC_PER_MSEC), HRTIMER_MODE_REL);
+#endif
+
+	return err;
+
+rollback_irq:
+	free_shm_irq(shrm);
+rollback_map:
+	iounmap(shrm->ac_common_shared_wptr);
+	iounmap(shrm->ac_common_shared_rptr);
+	iounmap(shrm->ca_common_shared_wptr);
+	iounmap(shrm->ca_common_shared_rptr);
+	iounmap(shrm->ac_audio_shared_wptr);
+	iounmap(shrm->ac_audio_shared_rptr);
+	iounmap(shrm->ca_audio_shared_wptr);
+	iounmap(shrm->ca_audio_shared_rptr);
+rollback_ac_common_shared_wptr:
+	iounmap(shrm->cmt_audio_fifo_base);
+rollback_cmt_audio_fifo_base:
+	iounmap(shrm->ape_audio_fifo_base);
+rollback_ape_audio_fifo_base:
+	iounmap(shrm->cmt_common_fifo_base);
+rollback_cmt_common_fifo_base:
+	iounmap(shrm->ape_common_fifo_base);
+rollback_ape_common_fifo_base:
+	iounmap(shrm->intr_base);
+rollback_intr:
+	kfree(shrm);
+	return err;
+}
+
+static int __exit shrm_remove(struct platform_device *pdev)
+{
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+
+	free_shm_irq(shrm);
+	iounmap(shrm->intr_base);
+	iounmap(shrm->ape_common_fifo_base);
+	iounmap(shrm->cmt_common_fifo_base);
+	iounmap(shrm->ape_audio_fifo_base);
+	iounmap(shrm->cmt_audio_fifo_base);
+	iounmap(shrm->ac_common_shared_wptr);
+	iounmap(shrm->ac_common_shared_rptr);
+	iounmap(shrm->ca_common_shared_wptr);
+	iounmap(shrm->ca_common_shared_rptr);
+	iounmap(shrm->ac_audio_shared_wptr);
+	iounmap(shrm->ac_audio_shared_rptr);
+	iounmap(shrm->ca_audio_shared_wptr);
+	iounmap(shrm->ca_audio_shared_rptr);
+	kfree(shrm);
+	isa_exit(shrm);
+
+	return 0;
+}
+#ifdef CONFIG_PM
+
+/**
+ * u8500_shrm_suspend() - This routine puts the SHRM in to sustend state.
+ * @pdev: platform device.
+ *
+ * This routine checks the current ongoing communication with Modem by
+ * examining the ca_wake state and prevents suspend if modem communication
+ * is on-going.
+ * If ca_wake = 1 (high), modem comm. is on-going; don't suspend
+ * If ca_wake = 0 (low), no comm. with modem on-going.Allow suspend
+ */
+int u8500_shrm_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+
+	dev_dbg(shrm->dev, "%s called...\n", __func__);
+	dev_dbg(shrm->dev, "\n ca_wake_req_state = %x\n",
+					get_ca_wake_req_state());
+	/* if ca_wake_req is high, prevent system suspend */
+	if (get_ca_wake_req_state())
+		return -EBUSY;
+	else
+		return 0;
+}
+
+/**
+ * u8500_shrm_resume() - This routine resumes the SHRM from sustend state.
+ * @pdev: platform device.
+ *
+ * This routine restore back the current state of the SHRM
+ */
+int u8500_shrm_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+
+	dev_dbg(shrm->dev, "%s called...\n", __func__);
+	/* TODO:
+	 * As of now, no state save takes place in suspend.
+	 * So, nothing to restore in resume.
+	 * Simply return as of now.
+	 * State saved in suspend should be restored here.
+	 */
+
+	return 0;
+}
+
+static const struct dev_pm_ops shrm_dev_pm_ops = {
+	.suspend = u8500_shrm_suspend,
+	.resume = u8500_shrm_resume,
+};
+#endif
+
+static struct platform_driver shrm_driver = {
+	.remove = __exit_p(shrm_remove),
+	.driver = {
+		.name = "u8500_shrm",
+		.owner = THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm = &shrm_dev_pm_ops,
+#endif
+	},
+};
+
+static int __init shrm_driver_init(void)
+{
+	return platform_driver_probe(&shrm_driver, shrm_probe);
+}
+
+static void __exit shrm_driver_exit(void)
+{
+	platform_driver_unregister(&shrm_driver);
+}
+
+module_init(shrm_driver_init);
+module_exit(shrm_driver_exit);
+
+MODULE_AUTHOR("Biju Das");
+MODULE_DESCRIPTION("Shared Memory Modem Driver Interface");
+MODULE_LICENSE("GPL");
diff --git a/drivers/modem/shrm/shrm_fifo.c b/drivers/modem/shrm/shrm_fifo.c
new file mode 100644
index 00000000000..883966d4ab5
--- /dev/null
+++ b/drivers/modem/shrm/shrm_fifo.c
@@ -0,0 +1,827 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghavi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/modem/shrm/shrm.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_net.h>
+
+#define L1_BOOT_INFO_REQ	1
+#define L1_BOOT_INFO_RESP	2
+#define L1_NORMAL_MSG		3
+#define L1_HEADER_MASK		28
+#define L1_MAPID_MASK		0xF0000000
+#define CONFIG_OFFSET		8
+#define COUNTER_OFFSET		20
+#define L2_HEADER_SIZE		4
+#define L2_HEADER_OFFSET	24
+#define MASK_0_15_BIT		0xFF
+#define MASK_16_31_BIT		0xFF00
+#define MASK_16_27_BIT		0xFFF0000
+#define MASK_0_39_BIT		0xFFFFF
+#define MASK_40_55_BIT		0xFF00000
+#define MASK_8_16_BIT           0x0000FF00
+#define MSG_LEN_OFFSET          16
+#define SHRM_VER                2
+#define ca_ist_inactivity_timer 100 /*100ms */
+#define ca_csc_inactivity_timer 100 /*100ms */
+
+static u8 msg_audio_counter;
+static u8 msg_common_counter;
+
+struct fifo_write_params ape_shm_fifo_0;
+struct fifo_write_params ape_shm_fifo_1;
+struct fifo_read_params cmt_shm_fifo_0;
+struct fifo_read_params cmt_shm_fifo_1;
+
+
+static u8 cmt_read_notif_0_send;
+static u8 cmt_read_notif_1_send;
+
+void shm_fifo_init(struct shrm_dev *shrm)
+{
+	ape_shm_fifo_0.writer_local_wptr	= 0;
+	ape_shm_fifo_0.writer_local_rptr	= 0;
+	*((u32 *)shrm->ac_common_shared_wptr) = 0;
+	*((u32 *)shrm->ac_common_shared_rptr) = 0;
+	ape_shm_fifo_0.shared_wptr		= 0;
+	ape_shm_fifo_0.shared_rptr		= 0;
+	ape_shm_fifo_0.availablesize = shrm->ape_common_fifo_size;
+	ape_shm_fifo_0.end_addr_fifo    = shrm->ape_common_fifo_size;
+	ape_shm_fifo_0.fifo_virtual_addr = shrm->ape_common_fifo_base;
+	spin_lock_init(&ape_shm_fifo_0.fifo_update_lock);
+
+
+	cmt_shm_fifo_0.reader_local_rptr	= 0;
+	cmt_shm_fifo_0.reader_local_wptr	= 0;
+	cmt_shm_fifo_0.shared_wptr	=
+			*((u32 *)shrm->ca_common_shared_wptr);
+	cmt_shm_fifo_0.shared_rptr	=
+			*((u32 *)shrm->ca_common_shared_rptr);
+	cmt_shm_fifo_0.availablesize	= shrm->cmt_common_fifo_size;
+	cmt_shm_fifo_0.end_addr_fifo	= shrm->cmt_common_fifo_size;
+	cmt_shm_fifo_0.fifo_virtual_addr = shrm->cmt_common_fifo_base;
+
+	ape_shm_fifo_1.writer_local_wptr	= 0;
+	ape_shm_fifo_1.writer_local_rptr	= 0;
+	ape_shm_fifo_1.shared_wptr		= 0;
+	ape_shm_fifo_1.shared_rptr		= 0;
+	*((u32 *)shrm->ac_audio_shared_wptr) = 0;
+	*((u32 *)shrm->ac_audio_shared_rptr) = 0;
+	ape_shm_fifo_1.availablesize = shrm->ape_audio_fifo_size;
+	ape_shm_fifo_1.end_addr_fifo    = shrm->ape_audio_fifo_size;
+	ape_shm_fifo_1.fifo_virtual_addr = shrm->ape_audio_fifo_base;
+	spin_lock_init(&ape_shm_fifo_1.fifo_update_lock);
+
+	cmt_shm_fifo_1.reader_local_rptr	= 0;
+	cmt_shm_fifo_1.reader_local_wptr	= 0;
+	cmt_shm_fifo_1.shared_wptr		=
+			*((u32 *)shrm->ca_audio_shared_wptr);
+	cmt_shm_fifo_1.shared_rptr		=
+			*((u32 *)shrm->ca_audio_shared_rptr);
+	cmt_shm_fifo_1.availablesize	= shrm->cmt_audio_fifo_size;
+	cmt_shm_fifo_1.end_addr_fifo	= shrm->cmt_audio_fifo_size;
+	cmt_shm_fifo_1.fifo_virtual_addr = shrm->cmt_audio_fifo_base;
+	msg_audio_counter = 0;
+	msg_common_counter = 0;
+}
+
+u8 read_boot_info_req(struct shrm_dev *shrm,
+				u32 *config,
+				u32 *version)
+{
+	struct fifo_read_params *fifo = &cmt_shm_fifo_0;
+	u32 *msg;
+	u32 header = 0;
+	u8 msgtype;
+
+	/* Read L1 header read content of reader_local_rptr */
+	msg = (u32 *)
+		(fifo->reader_local_rptr + fifo->fifo_virtual_addr);
+	header = *msg;
+	msgtype = (header & L1_MAPID_MASK) >> L1_MSG_MAPID_OFFSET;
+	if (msgtype != L1_BOOT_INFO_REQ) {
+		dev_err(shrm->dev, "Read_Boot_Info_Req Fatal ERROR\n");
+		BUG();
+	}
+	*config = (header >> CONFIG_OFFSET) & MASK_0_15_BIT;
+	*version = header & MASK_0_15_BIT;
+	fifo->reader_local_rptr += 1;
+
+	return 1;
+}
+
+void write_boot_info_resp(struct shrm_dev *shrm, u32 config,
+							u32 version)
+{
+	struct fifo_write_params *fifo = &ape_shm_fifo_0;
+	u32 *msg;
+	u8 msg_length;
+	version = SHRM_VER;
+
+	spin_lock_bh(&fifo->fifo_update_lock);
+	/* Read L1 header read content of reader_local_rptr */
+	msg = (u32 *)
+		(fifo->writer_local_wptr+fifo->fifo_virtual_addr);
+	if (version < 1)	{
+		*msg = ((L1_BOOT_INFO_RESP << L1_MSG_MAPID_OFFSET) |
+				((config << CONFIG_OFFSET) & MASK_16_31_BIT)
+				| (version & MASK_0_15_BIT));
+		msg_length = 1;
+	} else {
+		*msg = ((L1_BOOT_INFO_RESP << L1_MSG_MAPID_OFFSET) |
+			((0x8 << MSG_LEN_OFFSET) & MASK_16_27_BIT) |
+			((config << CONFIG_OFFSET) & MASK_8_16_BIT)|
+			version);
+		msg++;
+		*msg = ca_ist_inactivity_timer;
+		msg++;
+		*msg = ca_csc_inactivity_timer;
+		msg_length = L1_NORMAL_MSG;
+	}
+	fifo->writer_local_wptr += msg_length;
+	fifo->availablesize -= msg_length;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+/**
+ * shm_write_msg_to_fifo() - write message to FIFO
+ * @shrm:	pointer to shrm device information structure
+ * @channel:	audio or common channel
+ * @l2header:	L2 header or device ID
+ * @addr:	pointer to write buffer address
+ * @length:	length of mst to write
+ *
+ * Function Which Writes the data into Fifo in IPC zone
+ * It is called from shm_write_msg. This function will copy the msg
+ * from the kernel buffer to FIFO. There are 4 kernel buffers from where
+ * the data is to copied to FIFO one for each of the messages ISI, RPC,
+ * AUDIO and SECURITY. ISI, RPC and SECURITY messages are pushed to FIFO
+ * in commmon channel and AUDIO message is pushed onto audio channel FIFO.
+ */
+int shm_write_msg_to_fifo(struct shrm_dev *shrm, u8 channel,
+				u8 l2header, void *addr, u32 length)
+{
+	struct fifo_write_params *fifo = NULL;
+	u32 l1_header = 0, l2_header = 0;
+	u32 requiredsize;
+	u32 size = 0;
+	u32 *msg;
+	u8 *src;
+
+	if (channel == COMMON_CHANNEL)
+		fifo = &ape_shm_fifo_0;
+	else if (channel == AUDIO_CHANNEL)
+		fifo = &ape_shm_fifo_1;
+	else {
+		dev_err(shrm->dev, "invalid channel\n");
+		return -EINVAL;
+	}
+
+	/* L2 size in 32b */
+	requiredsize = ((length + 3) / 4);
+	/* Add size of L1 & L2 header */
+	requiredsize += 2;
+
+	/* if availablesize = or < requiredsize then error */
+	if (fifo->availablesize <= requiredsize) {
+		/* Fatal ERROR - should never happens */
+		dev_dbg(shrm->dev, "wr_wptr= %x\n",
+					fifo->writer_local_wptr);
+		dev_dbg(shrm->dev, "wr_rptr= %x\n",
+					fifo->writer_local_rptr);
+		dev_dbg(shrm->dev, "shared_wptr= %x\n",
+						fifo->shared_wptr);
+		dev_dbg(shrm->dev, "shared_rptr= %x\n",
+						fifo->shared_rptr);
+		dev_dbg(shrm->dev, "availsize= %x\n",
+						fifo->availablesize);
+		dev_dbg(shrm->dev, "end__fifo= %x\n",
+				fifo->end_addr_fifo);
+		dev_warn(shrm->dev, "Modem is busy, please wait."
+				" c_cnt = %d; a_cnt = %d\n", msg_common_counter,
+				msg_audio_counter);
+		if (channel == COMMON_CHANNEL) {
+			dev_warn(shrm->dev,
+					"Modem is lagging behind in reading."
+					"Stopping n/w dev queue\n");
+			shrm_stop_netdev(shrm->ndev);
+		}
+
+		return -EAGAIN;
+	}
+
+	if (channel == COMMON_CHANNEL) {
+		/* build L1 header */
+		l1_header = ((L1_NORMAL_MSG << L1_MSG_MAPID_OFFSET) |
+				(((msg_common_counter++) << COUNTER_OFFSET)
+				 & MASK_40_55_BIT) |
+				((length + L2_HEADER_SIZE) & MASK_0_39_BIT));
+	} else if (channel == AUDIO_CHANNEL) {
+		/* build L1 header */
+		l1_header = ((L1_NORMAL_MSG << L1_MSG_MAPID_OFFSET) |
+				(((msg_audio_counter++) << COUNTER_OFFSET)
+				 & MASK_40_55_BIT) |
+				((length + L2_HEADER_SIZE) & MASK_0_39_BIT));
+	}
+
+	/*
+	 * Need to take care race condition for fifo->availablesize
+	 * & fifo->writer_local_rptr with Ac_Read_notification interrupt.
+	 * One option could be use stack variable for LocalRptr and recompute
+	 * fifo->availablesize,based on flag enabled in the
+	 * Ac_read_notification
+	 */
+	l2_header = ((l2header << L2_HEADER_OFFSET) |
+					((length) & MASK_0_39_BIT));
+	spin_lock_bh(&fifo->fifo_update_lock);
+	/* Check Local Rptr is less than or equal to Local WPtr */
+	if (fifo->writer_local_rptr <= fifo->writer_local_wptr) {
+		msg = (u32 *)
+			(fifo->fifo_virtual_addr+fifo->writer_local_wptr);
+
+		/* check enough place bewteen writer_local_wptr & end of FIFO */
+		if ((fifo->end_addr_fifo-fifo->writer_local_wptr) >=
+							requiredsize) {
+			/* Add L1 header and L2 header */
+			*msg = l1_header;
+			msg++;
+			*msg = l2_header;
+			msg++;
+
+			/* copy the l2 message in 1 memcpy */
+			memcpy((void *)msg, addr, length);
+			/* UpdateWptr */
+			fifo->writer_local_wptr += requiredsize;
+			fifo->availablesize -= requiredsize;
+			fifo->writer_local_wptr %= fifo->end_addr_fifo;
+		} else {
+			/*
+			 * message is split between and of FIFO and beg of FIFO
+			 * copy first part from writer_local_wptr to end of FIFO
+			 */
+			size = fifo->end_addr_fifo-fifo->writer_local_wptr;
+
+			if (size == 1) {
+				/* Add L1 header */
+				*msg = l1_header;
+				msg++;
+				/* UpdateWptr */
+				fifo->writer_local_wptr = 0;
+				fifo->availablesize -= size;
+				/*
+				 * copy second part from beg of FIFO
+				 * with remaining part of msg
+				 */
+				msg =	(u32 *)
+						fifo->fifo_virtual_addr;
+				*msg = l2_header;
+				msg++;
+
+				/* copy the l3 message in 1 memcpy */
+				memcpy((void *)msg, addr, length);
+				/* UpdateWptr */
+				fifo->writer_local_wptr +=
+							requiredsize-size;
+				fifo->availablesize -=
+							(requiredsize-size);
+			} else if (size == 2) {
+				/* Add L1 header and L2 header */
+				*msg = l1_header;
+				msg++;
+				*msg = l2_header;
+				msg++;
+
+				/* UpdateWptr */
+				fifo->writer_local_wptr = 0;
+				fifo->availablesize -= size;
+
+				/*
+				 * copy second part from beg of FIFO
+				 * with remaining part of msg
+				 */
+				msg =	(u32 *)
+						fifo->fifo_virtual_addr;
+				/* copy the l3 message in 1 memcpy */
+				memcpy((void *)msg, addr, length);
+
+				/* UpdateWptr */
+				fifo->writer_local_wptr +=
+							requiredsize-size;
+				fifo->availablesize -=
+							(requiredsize-size);
+			} else {
+				/* Add L1 header and L2 header */
+				*msg = l1_header;
+				msg++;
+				*msg = l2_header;
+				msg++;
+
+				/* copy the l2 message in 1 memcpy */
+				memcpy((void *)msg, addr, (size-2)*4);
+
+
+				/* UpdateWptr */
+				fifo->writer_local_wptr = 0;
+				fifo->availablesize -= size;
+
+				/*
+				 * copy second part from beg of FIFO
+				 * with remaining part of msg
+				 */
+				msg = (u32 *)fifo->fifo_virtual_addr;
+				src = (u8 *)addr+((size - 2) * 4);
+				memcpy((void *)msg, src,
+						(length-((size - 2) * 4)));
+
+				/* UpdateWptr */
+				fifo->writer_local_wptr +=
+							requiredsize-size;
+				fifo->availablesize -=
+							(requiredsize-size);
+			}
+
+		}
+	} else {
+		/* writer_local_rptr > writer_local_wptr */
+		msg = (u32 *)
+			(fifo->fifo_virtual_addr+fifo->writer_local_wptr);
+		/* Add L1 header and L2 header */
+		*msg = l1_header;
+		msg++;
+		*msg = l2_header;
+		msg++;
+		/*
+		 * copy message possbile between writer_local_wptr up
+		 * to writer_local_rptr copy the l3 message in 1 memcpy
+		 */
+		memcpy((void *)msg, addr, length);
+
+		/* UpdateWptr */
+		fifo->writer_local_wptr += requiredsize;
+		fifo->availablesize -= requiredsize;
+
+	}
+	spin_unlock_bh(&fifo->fifo_update_lock);
+	return length;
+}
+
+/**
+ * read_one_l2msg_common() - read message from common channel
+ * @shrm:	pointer to shrm device information structure
+ * @l2_msg:	pointer to the read L2 message buffer
+ * @len:	message length
+ *
+ * This function read one message from the FIFO  and returns l2 header type
+ */
+u8 read_one_l2msg_common(struct shrm_dev *shrm,
+			u8 *l2_msg, u32 *len)
+{
+	struct fifo_read_params *fifo = &cmt_shm_fifo_0;
+
+	u32 *msg;
+	u32 l1_header = 0;
+	u32 l2_header = 0;
+	u32 length;
+	u8 msgtype;
+	u32 msg_size;
+	u32 size = 0;
+
+	/* Read L1 header read content of reader_local_rptr */
+	msg = (u32 *)
+		(fifo->reader_local_rptr+fifo->fifo_virtual_addr);
+	l1_header = *msg++;
+	msgtype = (l1_header & 0xF0000000) >> L1_HEADER_MASK;
+
+	if (msgtype != L1_NORMAL_MSG) {
+		/* Fatal ERROR - should never happens */
+		dev_dbg(shrm->dev, "wr_wptr= %x\n",
+						fifo->reader_local_wptr);
+		dev_dbg(shrm->dev, "wr_rptr= %x\n",
+						fifo->reader_local_rptr);
+		dev_dbg(shrm->dev, "shared_wptr= %x\n",
+						fifo->shared_wptr);
+		dev_dbg(shrm->dev, "shared_rptr= %x\n",
+						fifo->shared_rptr);
+		dev_dbg(shrm->dev, "availsize= %x\n",
+						fifo->availablesize);
+		dev_dbg(shrm->dev, "end_fifo= %x\n",
+						fifo->end_addr_fifo);
+		/* Fatal ERROR - should never happens */
+		dev_crit(shrm->dev, "Fatal ERROR - should never happen\n");
+		BUG();
+	 }
+	if (fifo->reader_local_rptr == (fifo->end_addr_fifo-1)) {
+		l2_header = (*((u32 *)fifo->fifo_virtual_addr));
+		length = l2_header & MASK_0_39_BIT;
+	} else {
+		/* Read L2 header,Msg size & content of reader_local_rptr */
+		l2_header = *msg;
+		length = l2_header & MASK_0_39_BIT;
+	}
+
+	*len = length;
+	msg_size = ((length + 3) / 4);
+	msg_size += 2;
+
+	if (fifo->reader_local_rptr + msg_size <=
+						fifo->end_addr_fifo) {
+		/* Skip L2 header */
+		msg++;
+
+		/* read msg between reader_local_rptr and end of FIFO */
+		memcpy((void *)l2_msg, (void *)msg, length);
+		/* UpdateLocalRptr */
+		fifo->reader_local_rptr += msg_size;
+		fifo->reader_local_rptr %= fifo->end_addr_fifo;
+	} else {
+		/*
+		 * msg split between end of FIFO and beg copy first
+		 * part of msg read msg between reader_local_rptr
+		 * and end of FIFO
+		 */
+		size = fifo->end_addr_fifo-fifo->reader_local_rptr;
+		if (size == 1) {
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			/* Skip L2 header */
+			msg++;
+			memcpy((void *)l2_msg, (void *)(msg), length);
+		} else if (size == 2) {
+			/* Skip L2 header */
+			msg++;
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			memcpy((void *)l2_msg,
+						(void *)(msg), length);
+		} else {
+			/* Skip L2 header */
+			msg++;
+			memcpy((void *)l2_msg, (void *)msg, ((size - 2) * 4));
+			/* copy second part of msg */
+			l2_msg += ((size - 2) * 4);
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			memcpy((void *)l2_msg, (void *)(msg),
+						(length-((size - 2) * 4)));
+		}
+		fifo->reader_local_rptr =
+			(fifo->reader_local_rptr+msg_size) %
+				fifo->end_addr_fifo;
+	}
+	return (l2_header>>L2_HEADER_OFFSET) & MASK_0_15_BIT;
+ }
+
+u8 read_remaining_messages_common()
+{
+	struct fifo_read_params *fifo = &cmt_shm_fifo_0;
+	/*
+	 * There won't be any Race condition reader_local_rptr &
+	 * fifo->reader_local_wptr with CaMsgpending Notification Interrupt
+	 */
+	return ((fifo->reader_local_rptr != fifo->reader_local_wptr) ? 1 : 0);
+}
+
+u8 read_one_l2msg_audio(struct shrm_dev *shrm,
+				u8 *l2_msg, u32 *len)
+{
+	struct fifo_read_params *fifo = &cmt_shm_fifo_1;
+
+	u32 *msg;
+	u32 l1_header = 0;
+	u32 l2_header = 0;
+	u32 length;
+	u8 msgtype;
+	u32 msg_size;
+	u32 size = 0;
+
+	/* Read L1 header read content of reader_local_rptr */
+	 msg = (u32 *)
+			(fifo->reader_local_rptr+fifo->fifo_virtual_addr);
+	 l1_header = *msg++;
+	 msgtype = (l1_header & 0xF0000000) >> L1_HEADER_MASK;
+
+	if (msgtype != L1_NORMAL_MSG) {
+		/* Fatal ERROR - should never happens */
+		dev_dbg(shrm->dev, "wr_local_wptr= %x\n",
+						fifo->reader_local_wptr);
+		dev_dbg(shrm->dev, "wr_local_rptr= %x\n",
+						fifo->reader_local_rptr);
+		dev_dbg(shrm->dev, "shared_wptr= %x\n",
+						fifo->shared_wptr);
+		dev_dbg(shrm->dev, "shared_rptr= %x\n",
+						fifo->shared_rptr);
+		dev_dbg(shrm->dev, "availsize=%x\n",
+						fifo->availablesize);
+		dev_dbg(shrm->dev, "end_fifo= %x\n",
+						fifo->end_addr_fifo);
+		/* Fatal ERROR - should never happens */
+		dev_crit(shrm->dev, "Fatal ERROR - should never happen\n");
+		BUG();
+	 }
+	if (fifo->reader_local_rptr == (fifo->end_addr_fifo-1)) {
+		l2_header = (*((u32 *)fifo->fifo_virtual_addr));
+		length = l2_header & MASK_0_39_BIT;
+	} else {
+		/* Read L2 header,Msg size & content of reader_local_rptr */
+		l2_header = *msg;
+		length = l2_header & MASK_0_39_BIT;
+	}
+
+	*len = length;
+	msg_size = ((length + 3) / 4);
+	msg_size += 2;
+
+	if (fifo->reader_local_rptr + msg_size <=
+						fifo->end_addr_fifo) {
+		/* Skip L2 header */
+		msg++;
+		/* read msg between reader_local_rptr and end of FIFO */
+		memcpy((void *)l2_msg, (void *)msg, length);
+		/* UpdateLocalRptr */
+		fifo->reader_local_rptr += msg_size;
+		fifo->reader_local_rptr %= fifo->end_addr_fifo;
+	} else {
+
+		/*
+		 * msg split between end of FIFO and beg
+		 * copy first part of msg
+		 * read msg between reader_local_rptr and end of FIFO
+		 */
+		size = fifo->end_addr_fifo-fifo->reader_local_rptr;
+		if (size == 1) {
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			/* Skip L2 header */
+			msg++;
+			memcpy((void *)l2_msg, (void *)(msg), length);
+		} else if (size == 2) {
+			/* Skip L2 header */
+			msg++;
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			memcpy((void *)l2_msg, (void *)(msg), length);
+		} else {
+			/* Skip L2 header */
+			msg++;
+			memcpy((void *)l2_msg, (void *)msg, ((size - 2) * 4));
+			/* copy second part of msg */
+			l2_msg += ((size - 2) * 4);
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			memcpy((void *)l2_msg, (void *)(msg),
+						(length-((size - 2) * 4)));
+		}
+		fifo->reader_local_rptr =
+			(fifo->reader_local_rptr+msg_size) %
+			fifo->end_addr_fifo;
+
+	}
+	return (l2_header>>L2_HEADER_OFFSET) & MASK_0_15_BIT;
+ }
+
+u8 read_remaining_messages_audio()
+{
+	struct fifo_read_params *fifo = &cmt_shm_fifo_1;
+
+	return ((fifo->reader_local_rptr != fifo->reader_local_wptr) ?
+									1 : 0);
+}
+
+u8 is_the_only_one_unread_message(struct shrm_dev *shrm,
+						u8 channel, u32 length)
+{
+	struct fifo_write_params *fifo = NULL;
+	u32 messagesize = 0;
+	u8 is_only_one_unread_msg = 0;
+
+	if (channel == COMMON_CHANNEL)
+		fifo = &ape_shm_fifo_0;
+	else /* channel = AUDIO_CHANNEL */
+		fifo = &ape_shm_fifo_1;
+
+	/* L3 size in 32b */
+	messagesize = ((length + 3) / 4);
+	/* Add size of L1 & L2 header */
+	messagesize += 2;
+	/*
+	 * possibility of race condition with Ac Read notification interrupt.
+	 * need to check ?
+	 */
+	if (fifo->writer_local_wptr > fifo->writer_local_rptr)
+		is_only_one_unread_msg =
+			((fifo->writer_local_rptr + messagesize) ==
+			fifo->writer_local_wptr) ? 1 : 0;
+	else
+		/* Msg split between end of fifo and starting of Fifo */
+		is_only_one_unread_msg =
+			(((fifo->writer_local_rptr + messagesize) %
+			fifo->end_addr_fifo) == fifo->writer_local_wptr) ?
+									1 : 0;
+
+	return is_only_one_unread_msg;
+}
+
+void update_ca_common_local_wptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update CA common reader local write pointer with the
+	 * shared write pointer
+	 */
+	struct fifo_read_params *fifo = &cmt_shm_fifo_0;
+
+	fifo->shared_wptr =
+		(*((u32 *)shrm->ca_common_shared_wptr));
+	fifo->reader_local_wptr = fifo->shared_wptr;
+}
+
+void update_ca_audio_local_wptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update CA audio reader local write pointer with the
+	 * shared write pointer
+	 */
+	struct fifo_read_params *fifo = &cmt_shm_fifo_1;
+
+	fifo->shared_wptr =
+		(*((u32 *)shrm->ca_audio_shared_wptr));
+	fifo->reader_local_wptr = fifo->shared_wptr;
+}
+
+void update_ac_common_local_rptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update AC common writer local read pointer with the
+	 * shared read pointer
+	 */
+	struct fifo_write_params *fifo;
+	u32 free_space = 0;
+
+	fifo = &ape_shm_fifo_0;
+
+	spin_lock_bh(&fifo->fifo_update_lock);
+	fifo->shared_rptr =
+		(*((u32 *)shrm->ac_common_shared_rptr));
+
+	if (fifo->shared_rptr >= fifo->writer_local_rptr)
+		free_space =
+			(fifo->shared_rptr-fifo->writer_local_rptr);
+	else {
+		free_space =
+			(fifo->end_addr_fifo-fifo->writer_local_rptr);
+		free_space += fifo->shared_rptr;
+	}
+
+	/* Chance of race condition of below variables with write_msg */
+	fifo->availablesize += free_space;
+	fifo->writer_local_rptr = fifo->shared_rptr;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+void update_ac_audio_local_rptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update AC audio writer local read pointer with the
+	 * shared read pointer
+	 */
+	struct fifo_write_params *fifo;
+	u32 free_space = 0;
+
+	fifo = &ape_shm_fifo_1;
+	spin_lock_bh(&fifo->fifo_update_lock);
+	fifo->shared_rptr =
+		(*((u32 *)shrm->ac_audio_shared_rptr));
+
+	if (fifo->shared_rptr >= fifo->writer_local_rptr)
+		free_space =
+			(fifo->shared_rptr-fifo->writer_local_rptr);
+	else {
+		free_space =
+			(fifo->end_addr_fifo-fifo->writer_local_rptr);
+		free_space += fifo->shared_rptr;
+	}
+
+	/* Chance of race condition of below variables with write_msg */
+	fifo->availablesize += free_space;
+	fifo->writer_local_rptr = fifo->shared_rptr;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+void update_ac_common_shared_wptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update AC common shared write pointer with the
+	 * local write pointer
+	 */
+	struct fifo_write_params *fifo;
+
+	fifo = &ape_shm_fifo_0;
+	spin_lock_bh(&fifo->fifo_update_lock);
+	/* Update shared pointer fifo offset of the IPC zone */
+	(*((u32 *)shrm->ac_common_shared_wptr)) =
+						fifo->writer_local_wptr;
+
+	fifo->shared_wptr = fifo->writer_local_wptr;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+void update_ac_audio_shared_wptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update AC audio shared write pointer with the
+	 * local write pointer
+	 */
+	struct fifo_write_params *fifo;
+
+	fifo = &ape_shm_fifo_1;
+	spin_lock_bh(&fifo->fifo_update_lock);
+	/* Update shared pointer fifo offset of the IPC zone */
+	(*((u32 *)shrm->ac_audio_shared_wptr)) =
+						fifo->writer_local_wptr;
+	fifo->shared_wptr = fifo->writer_local_wptr;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+void update_ca_common_shared_rptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update CA common shared read pointer with the
+	 * local read pointer
+	 */
+	struct fifo_read_params *fifo;
+
+	fifo = &cmt_shm_fifo_0;
+
+	/* Update shared pointer fifo offset of the IPC zone */
+	(*((u32 *)shrm->ca_common_shared_rptr)) =
+						fifo->reader_local_rptr;
+	fifo->shared_rptr = fifo->reader_local_rptr;
+}
+
+void update_ca_audio_shared_rptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update CA audio shared read pointer with the
+	 * local read pointer
+	 */
+	struct fifo_read_params *fifo;
+
+	fifo = &cmt_shm_fifo_1;
+
+	/* Update shared pointer fifo offset of the IPC zone */
+	(*((u32 *)shrm->ca_audio_shared_rptr)) =
+						fifo->reader_local_rptr;
+	fifo->shared_rptr = fifo->reader_local_rptr;
+}
+
+void get_reader_pointers(u8 channel_type, u32 *reader_local_rptr,
+				u32 *reader_local_wptr, u32 *shared_rptr)
+{
+	struct fifo_read_params *fifo = NULL;
+
+	if (channel_type == COMMON_CHANNEL)
+		fifo = &cmt_shm_fifo_0;
+	else /* channel_type = AUDIO_CHANNEL */
+		fifo = &cmt_shm_fifo_1;
+
+	*reader_local_rptr = fifo->reader_local_rptr;
+	*reader_local_wptr = fifo->reader_local_wptr;
+	*shared_rptr = fifo->shared_rptr;
+}
+
+void get_writer_pointers(u8 channel_type, u32 *writer_local_rptr,
+			 u32 *writer_local_wptr, u32 *shared_wptr)
+{
+	struct fifo_write_params *fifo = NULL;
+
+	if (channel_type == COMMON_CHANNEL)
+		fifo = &ape_shm_fifo_0;
+	else /* channel_type = AUDIO_CHANNEL */
+		fifo = &ape_shm_fifo_1;
+
+	spin_lock_bh(&fifo->fifo_update_lock);
+	*writer_local_rptr = fifo->writer_local_rptr;
+	*writer_local_wptr = fifo->writer_local_wptr;
+	*shared_wptr = fifo->shared_wptr;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+void set_ca_msg_0_read_notif_send(u8 val)
+{
+	cmt_read_notif_0_send = val;
+}
+
+u8 get_ca_msg_0_read_notif_send(void)
+{
+	return cmt_read_notif_0_send;
+}
+
+void set_ca_msg_1_read_notif_send(u8 val)
+{
+	cmt_read_notif_1_send = val;
+}
+
+u8 get_ca_msg_1_read_notif_send(void)
+{
+	return cmt_read_notif_1_send;
+}
diff --git a/drivers/modem/shrm/shrm_protocol.c b/drivers/modem/shrm/shrm_protocol.c
new file mode 100644
index 00000000000..14fe28ca267
--- /dev/null
+++ b/drivers/modem/shrm/shrm_protocol.c
@@ -0,0 +1,1193 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+#include <linux/netlink.h>
+#include <linux/workqueue.h>
+#include <linux/modem/shrm/shrm.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_net.h>
+#include <linux/modem/modem_client.h>
+#include <mach/prcmu.h>
+#include <mach/prcmu-regs.h>
+#include <mach/suspend.h>
+#include <mach/reboot_reasons.h>
+
+#define L2_HEADER_ISI		0x0
+#define L2_HEADER_RPC		0x1
+#define L2_HEADER_AUDIO		0x2
+#define L2_HEADER_SECURITY	0x3
+#define L2_HEADER_COMMON_SIMPLE_LOOPBACK	0xC0
+#define L2_HEADER_COMMON_ADVANCED_LOOPBACK	0xC1
+#define L2_HEADER_AUDIO_SIMPLE_LOOPBACK		0x80
+#define L2_HEADER_AUDIO_ADVANCED_LOOPBACK	0x81
+#define L2_HEADER_CIQ		0xC3
+#define MAX_PAYLOAD 1024
+
+static u8 boot_state = BOOT_INIT;
+static u8 recieve_common_msg[8*1024];
+static u8 recieve_audio_msg[8*1024];
+static received_msg_handler rx_common_handler;
+static received_msg_handler rx_audio_handler;
+static struct hrtimer timer;
+struct sock *shrm_nl_sk;
+
+static char shrm_common_tx_state = SHRM_SLEEP_STATE;
+static char shrm_common_rx_state = SHRM_SLEEP_STATE;
+static char shrm_audio_tx_state = SHRM_SLEEP_STATE;
+static char shrm_audio_rx_state = SHRM_SLEEP_STATE;
+
+static atomic_t ac_sleep_disable_count = ATOMIC_INIT(0);
+static struct shrm_dev *shm_dev;
+
+/* Spin lock and tasklet declaration */
+DECLARE_TASKLET(shm_ca_0_tasklet, shm_ca_msgpending_0_tasklet, 0);
+DECLARE_TASKLET(shm_ca_1_tasklet, shm_ca_msgpending_1_tasklet, 0);
+DECLARE_TASKLET(shm_ac_read_0_tasklet, shm_ac_read_notif_0_tasklet, 0);
+DECLARE_TASKLET(shm_ac_read_1_tasklet, shm_ac_read_notif_1_tasklet, 0);
+
+static DEFINE_MUTEX(ac_state_mutex);
+
+static DEFINE_SPINLOCK(ca_common_lock);
+static DEFINE_SPINLOCK(ca_audio_lock);
+static DEFINE_SPINLOCK(ca_wake_req_lock);
+static DEFINE_SPINLOCK(boot_lock);
+
+enum shrm_nl {
+	SHRM_NL_MOD_RESET = 1,
+	SHRM_NL_MOD_QUERY_STATE,
+	SHRM_NL_USER_MOD_RESET,
+	SHRM_NL_STATUS_MOD_ONLINE,
+	SHRM_NL_STATUS_MOD_OFFLINE,
+};
+
+static void shm_ac_sleep_req_work(struct work_struct *work)
+{
+	mutex_lock(&ac_state_mutex);
+	if (atomic_read(&ac_sleep_disable_count) == 0)
+		modem_release(shm_dev->modem);
+	mutex_unlock(&ac_state_mutex);
+}
+
+static void shm_ac_wake_req_work(struct work_struct *work)
+{
+	mutex_lock(&ac_state_mutex);
+	modem_request(shm_dev->modem);
+	mutex_unlock(&ac_state_mutex);
+}
+
+static u32 get_host_accessport_val(void)
+{
+	u32 prcm_hostaccess;
+
+	prcm_hostaccess = readl(PRCM_HOSTACCESS_REQ);
+	wmb();
+	prcm_hostaccess = prcm_hostaccess & 0x01;
+
+	return prcm_hostaccess;
+}
+static enum hrtimer_restart callback(struct hrtimer *timer)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ca_wake_req_lock, flags);
+	if (((shrm_common_rx_state == SHRM_IDLE) ||
+				(shrm_common_rx_state == SHRM_SLEEP_STATE))
+			&& ((shrm_common_tx_state == SHRM_IDLE) ||
+				(shrm_common_tx_state == SHRM_SLEEP_STATE))
+			&& ((shrm_audio_rx_state == SHRM_IDLE)  ||
+				(shrm_audio_rx_state == SHRM_SLEEP_STATE))
+			&& ((shrm_audio_tx_state == SHRM_IDLE)  ||
+				(shrm_audio_tx_state == SHRM_SLEEP_STATE))) {
+
+		shrm_common_rx_state = SHRM_SLEEP_STATE;
+		shrm_audio_rx_state = SHRM_SLEEP_STATE;
+		shrm_common_tx_state = SHRM_SLEEP_STATE;
+		shrm_audio_tx_state = SHRM_SLEEP_STATE;
+
+		queue_work(shm_dev->shm_ac_sleep_wq,
+				&shm_dev->shm_ac_sleep_req);
+
+	}
+	spin_unlock_irqrestore(&ca_wake_req_lock, flags);
+
+	return HRTIMER_NORESTART;
+}
+
+int nl_send_multicast_message(int msg, gfp_t gfp_mask)
+{
+	struct sk_buff *skb = NULL;
+	struct nlmsghdr *nlh = NULL;
+	int err;
+
+	/* prepare netlink message */
+	skb = alloc_skb(NLMSG_SPACE(MAX_PAYLOAD), gfp_mask);
+	if (!skb) {
+		dev_err(shm_dev->dev, "%s:alloc_skb failed\n", __func__);
+		err = -ENOMEM;
+		goto out;
+	}
+
+	nlh = (struct nlmsghdr *)skb->data;
+	nlh->nlmsg_len = NLMSG_SPACE(MAX_PAYLOAD);
+	dev_dbg(shm_dev->dev, "nlh->nlmsg_len = %d\n", nlh->nlmsg_len);
+
+	nlh->nlmsg_pid = 0;  /* from kernel */
+	nlh->nlmsg_flags = 0;
+	*(int *)NLMSG_DATA(nlh) = msg;
+	skb_put(skb, MAX_PAYLOAD);
+	/* sender is in group 1<<0 */
+	NETLINK_CB(skb).pid = 0;  /* from kernel */
+	/* to mcast group 1<<0 */
+	NETLINK_CB(skb).dst_group = 1;
+
+	/*multicast the message to all listening processes*/
+	err = netlink_broadcast(shrm_nl_sk, skb, 0, 1, gfp_mask);
+	dev_dbg(shm_dev->dev, "ret val from nl-multicast = %d\n", err);
+
+out:
+	return err;
+}
+
+static void nl_send_unicast_message(int dst_pid)
+{
+	struct sk_buff *skb = NULL;
+	struct nlmsghdr *nlh = NULL;
+	int err;
+	int bt_state;
+	unsigned long flags;
+
+	dev_info(shm_dev->dev, "Sending unicast message\n");
+
+	/* prepare the NL message for unicast */
+	skb = alloc_skb(NLMSG_SPACE(MAX_PAYLOAD), GFP_KERNEL);
+	if (!skb) {
+		dev_err(shm_dev->dev, "%s:alloc_skb failed\n", __func__);
+		return;
+	}
+
+	nlh = (struct nlmsghdr *)skb->data;
+	nlh->nlmsg_len = NLMSG_SPACE(MAX_PAYLOAD);
+	dev_dbg(shm_dev->dev, "nlh->nlmsg_len = %d\n", nlh->nlmsg_len);
+
+	nlh->nlmsg_pid = 0;  /* from kernel */
+	nlh->nlmsg_flags = 0;
+
+	spin_lock_irqsave(&boot_lock, flags);
+	bt_state = boot_state;
+	spin_unlock_irqrestore(&boot_lock, flags);
+
+	if (bt_state == BOOT_DONE)
+		*(int *)NLMSG_DATA(nlh) = SHRM_NL_STATUS_MOD_ONLINE;
+	else
+		*(int *)NLMSG_DATA(nlh) = SHRM_NL_STATUS_MOD_OFFLINE;
+
+	skb_put(skb, MAX_PAYLOAD);
+	/* sender is in group 1<<0 */
+	NETLINK_CB(skb).pid = 0;  /* from kernel */
+	NETLINK_CB(skb).dst_group = 0;
+
+	/*unicast the message to the querying processes*/
+	err = netlink_unicast(shrm_nl_sk, skb, dst_pid, MSG_DONTWAIT);
+	dev_dbg(shm_dev->dev, "ret val from nl-unicast = %d\n", err);
+}
+
+
+static int check_modem_in_reset(void)
+{
+	u8 bt_state;
+	unsigned long flags;
+
+	spin_lock_irqsave(&boot_lock, flags);
+	bt_state = boot_state;
+	spin_unlock_irqrestore(&boot_lock, flags);
+
+#ifdef CONFIG_U8500_SHRM_MODEM_SILENT_RESET
+	if (bt_state != BOOT_UNKNOWN)
+		return 0;
+	else
+		return -ENODEV;
+#else
+	/*
+	 * this check won't be applicable and won't work correctly
+	 * if modem-silent-feature is not enabled
+	 * so, simply return 0
+	 */
+	return 0;
+#endif
+}
+
+void shm_ca_msgpending_0_tasklet(unsigned long tasklet_data)
+{
+	struct shrm_dev *shrm = (struct shrm_dev *)tasklet_data;
+	u32 reader_local_rptr;
+	u32 reader_local_wptr;
+	u32 shared_rptr;
+	u32 config = 0, version = 0;
+	unsigned long flags;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	/* Interprocess locking */
+	spin_lock(&ca_common_lock);
+
+	/* Update_reader_local_wptr with shared_wptr */
+	update_ca_common_local_wptr(shrm);
+	get_reader_pointers(COMMON_CHANNEL, &reader_local_rptr,
+				&reader_local_wptr, &shared_rptr);
+
+	set_ca_msg_0_read_notif_send(0);
+
+	if (boot_state == BOOT_DONE) {
+		shrm_common_rx_state = SHRM_PTR_FREE;
+
+		if (reader_local_rptr != shared_rptr)
+			ca_msg_read_notification_0(shrm);
+		if (reader_local_rptr != reader_local_wptr)
+			receive_messages_common(shrm);
+		get_reader_pointers(COMMON_CHANNEL, &reader_local_rptr,
+				&reader_local_wptr, &shared_rptr);
+		if (reader_local_rptr == reader_local_wptr)
+			shrm_common_rx_state = SHRM_IDLE;
+	} else {
+		/* BOOT phase.only a BOOT_RESP should be in FIFO */
+		if (boot_state != BOOT_INFO_SYNC) {
+			if (!read_boot_info_req(shrm, &config, &version)) {
+				dev_err(shrm->dev,
+						"Unable to read boot state\n");
+				BUG();
+			}
+			/* SendReadNotification */
+			ca_msg_read_notification_0(shrm);
+			/*
+			 * Check the version number before
+			 * sending Boot info response
+			 */
+
+			/* send MsgPending notification */
+			write_boot_info_resp(shrm, config, version);
+			spin_lock_irqsave(&boot_lock, flags);
+			boot_state = BOOT_INFO_SYNC;
+			spin_unlock_irqrestore(&boot_lock, flags);
+			dev_info(shrm->dev, "BOOT_INFO_SYNC\n");
+			queue_work(shrm->shm_common_ch_wr_wq,
+					&shrm->send_ac_msg_pend_notify_0);
+		} else {
+			ca_msg_read_notification_0(shrm);
+			dev_info(shrm->dev,
+				"BOOT_INFO_SYNC\n");
+		}
+	}
+	/* Interprocess locking */
+	spin_unlock(&ca_common_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void shm_ca_msgpending_1_tasklet(unsigned long tasklet_data)
+{
+	struct shrm_dev *shrm = (struct shrm_dev *)tasklet_data;
+	u32 reader_local_rptr;
+	u32 reader_local_wptr;
+	u32 shared_rptr;
+
+	/*
+	 * This function is called when CaMsgPendingNotification Trigerred
+	 * by CMU. It means that CMU has wrote a message into Ca Audio FIFO
+	 */
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown\n",
+				__func__);
+		return;
+	}
+
+	/* Interprocess locking */
+	spin_lock(&ca_audio_lock);
+
+	/* Update_reader_local_wptr(with shared_wptr) */
+	update_ca_audio_local_wptr(shrm);
+	get_reader_pointers(AUDIO_CHANNEL, &reader_local_rptr,
+					&reader_local_wptr, &shared_rptr);
+
+	set_ca_msg_1_read_notif_send(0);
+
+	if (boot_state != BOOT_DONE) {
+		dev_err(shrm->dev, "Boot Error\n");
+		return;
+	}
+	shrm_audio_rx_state = SHRM_PTR_FREE;
+	/* Check we already read the message */
+	if (reader_local_rptr != shared_rptr)
+		ca_msg_read_notification_1(shrm);
+	if (reader_local_rptr != reader_local_wptr)
+		receive_messages_audio(shrm);
+
+	get_reader_pointers(AUDIO_CHANNEL, &reader_local_rptr,
+			&reader_local_wptr, &shared_rptr);
+	if (reader_local_rptr == reader_local_wptr)
+		shrm_audio_rx_state = SHRM_IDLE;
+
+	 /* Interprocess locking */
+	spin_unlock(&ca_audio_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void shm_ac_read_notif_0_tasklet(unsigned long tasklet_data)
+{
+	struct shrm_dev *shrm = (struct shrm_dev *)tasklet_data;
+	u32 writer_local_rptr;
+	u32 writer_local_wptr;
+	u32 shared_wptr;
+	unsigned long flags;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	/* Update writer_local_rptrwith shared_rptr */
+	update_ac_common_local_rptr(shrm);
+	get_writer_pointers(COMMON_CHANNEL, &writer_local_rptr,
+				&writer_local_wptr, &shared_wptr);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown\n",
+				__func__);
+		return;
+	}
+
+	if (boot_state == BOOT_INFO_SYNC) {
+		/* BOOT_RESP sent by APE has been received by CMT */
+		spin_lock_irqsave(&boot_lock, flags);
+		boot_state = BOOT_DONE;
+		spin_unlock_irqrestore(&boot_lock, flags);
+		dev_info(shrm->dev, "IPC_ISA BOOT_DONE\n");
+
+		if (shrm->msr_flag) {
+			shrm_start_netdev(shrm->ndev);
+			shrm->msr_flag = 0;
+
+			/* multicast that modem is online */
+			nl_send_multicast_message(SHRM_NL_STATUS_MOD_ONLINE,
+					GFP_ATOMIC);
+		}
+
+	} else if (boot_state == BOOT_DONE) {
+		if (writer_local_rptr != writer_local_wptr) {
+			shrm_common_tx_state = SHRM_PTR_FREE;
+			queue_work(shrm->shm_common_ch_wr_wq,
+					&shrm->send_ac_msg_pend_notify_0);
+		} else {
+			shrm_common_tx_state = SHRM_IDLE;
+			shrm_restart_netdev(shrm->ndev);
+		}
+	} else {
+		dev_err(shrm->dev, "Invalid boot state\n");
+	}
+	/* start timer here */
+	hrtimer_start(&timer, ktime_set(0, 10*NSEC_PER_MSEC),
+			HRTIMER_MODE_REL);
+	atomic_dec(&ac_sleep_disable_count);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void shm_ac_read_notif_1_tasklet(unsigned long tasklet_data)
+{
+	struct shrm_dev *shrm = (struct shrm_dev *)tasklet_data;
+	u32 writer_local_rptr;
+	u32 writer_local_wptr;
+	u32 shared_wptr;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown\n",
+				__func__);
+		return;
+	}
+
+	/* Update writer_local_rptr(with shared_rptr) */
+	update_ac_audio_local_rptr(shrm);
+	get_writer_pointers(AUDIO_CHANNEL, &writer_local_rptr,
+				&writer_local_wptr, &shared_wptr);
+	if (boot_state != BOOT_DONE) {
+		dev_err(shrm->dev, "Error Case in boot state\n");
+		return;
+	}
+	if (writer_local_rptr != writer_local_wptr) {
+		shrm_audio_tx_state = SHRM_PTR_FREE;
+		queue_work(shrm->shm_audio_ch_wr_wq,
+				&shrm->send_ac_msg_pend_notify_1);
+	} else {
+		shrm_audio_tx_state = SHRM_IDLE;
+	}
+	/* start timer here */
+	hrtimer_start(&timer, ktime_set(0, 10*NSEC_PER_MSEC),
+			HRTIMER_MODE_REL);
+	atomic_dec(&ac_sleep_disable_count);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void shm_ca_sleep_req_work(struct work_struct *work)
+{
+	dev_dbg(shm_dev->dev, "%s:IRQ_PRCMU_CA_SLEEP\n", __func__);
+
+	shrm_common_rx_state = SHRM_IDLE;
+	shrm_audio_rx_state =  SHRM_IDLE;
+
+	writel((1<<GOP_CA_WAKE_ACK_BIT),
+		shm_dev->intr_base + GOP_SET_REGISTER_BASE);
+
+	hrtimer_start(&timer, ktime_set(0, 10*NSEC_PER_MSEC),
+			HRTIMER_MODE_REL);
+#ifdef CONFIG_UX500_SUSPEND
+	suspend_unblock_sleep();
+#endif
+	atomic_dec(&ac_sleep_disable_count);
+}
+
+void shm_ca_wake_req_work(struct work_struct *work)
+{
+	struct shrm_dev *shrm = container_of(work,
+			struct shrm_dev, shm_ca_wake_req);
+
+	/* initialize the FIFO Variables */
+	if (boot_state == BOOT_INIT)
+		shm_fifo_init(shrm);
+
+	mutex_lock(&ac_state_mutex);
+	modem_request(shrm->modem);
+	mutex_unlock(&ac_state_mutex);
+
+	/* send ca_wake_ack_interrupt to CMU */
+	if (!get_host_accessport_val())
+		BUG();
+	writel((1<<GOP_CA_WAKE_ACK_BIT),
+			shm_dev->intr_base + GOP_SET_REGISTER_BASE);
+}
+#ifdef CONFIG_U8500_SHRM_MODEM_SILENT_RESET
+static int shrm_modem_reset_sequence(void)
+{
+	int err;
+	unsigned long flags;
+
+	/*
+	 * disable irqs
+	 * very much needed for user-space initiated
+	 * modem-reset
+	 */
+	disable_irq_nosync(shm_dev->ac_read_notif_0_irq);
+	disable_irq_nosync(shm_dev->ac_read_notif_1_irq);
+	disable_irq_nosync(shm_dev->ca_msg_pending_notif_0_irq);
+	disable_irq_nosync(shm_dev->ca_msg_pending_notif_1_irq);
+	disable_irq_nosync(IRQ_PRCMU_CA_WAKE);
+	disable_irq_nosync(IRQ_PRCMU_CA_SLEEP);
+
+
+	/* update the boot_state */
+	spin_lock_irqsave(&boot_lock, flags);
+	boot_state = BOOT_UNKNOWN;
+
+	/*
+	 * put a barrier over here to make sure boot_state is updated
+	 * else, it is seen that some of already executing modem
+	 * irqs or tasklets fail the protocol checks and will ultimately
+	 * try to acces the modem causing system to hang.
+	 * This is particularly seen with user-space initiated modem reset
+	 */
+	wmb();
+	spin_unlock_irqrestore(&boot_lock, flags);
+
+	hrtimer_cancel(&timer);
+
+	/*
+	 * keep the count to 0 so that we can bring down the line
+	 * for normal ac-wake and ac-sleep logic
+	 */
+	atomic_set(&ac_sleep_disable_count, 0);
+
+	/* workaround for MSR */
+	queue_work(shm_dev->shm_ac_wake_wq,
+			&shm_dev->shm_ac_wake_req);
+
+	/* stop network queue */
+	shrm_stop_netdev(shm_dev->ndev);
+
+	/* reset char device queues */
+	shrm_char_reset_queues(shm_dev);
+
+	/* reset protocol states */
+	shrm_common_tx_state = SHRM_SLEEP_STATE;
+	shrm_common_rx_state = SHRM_SLEEP_STATE;
+	shrm_audio_tx_state = SHRM_SLEEP_STATE;
+	shrm_audio_rx_state = SHRM_SLEEP_STATE;
+
+	/* set the msr flag */
+	shm_dev->msr_flag = 1;
+
+	/* multicast that modem is going to reset */
+	err = nl_send_multicast_message(SHRM_NL_MOD_RESET, GFP_ATOMIC);
+
+	/* reset the boot state */
+	spin_lock_irqsave(&boot_lock, flags);
+	boot_state = BOOT_INIT;
+	spin_unlock_irqrestore(&boot_lock, flags);
+
+	/* re-enable irqs */
+	enable_irq(shm_dev->ac_read_notif_0_irq);
+	enable_irq(shm_dev->ac_read_notif_1_irq);
+	enable_irq(shm_dev->ca_msg_pending_notif_0_irq);
+	enable_irq(shm_dev->ca_msg_pending_notif_1_irq);
+	enable_irq(IRQ_PRCMU_CA_WAKE);
+	enable_irq(IRQ_PRCMU_CA_SLEEP);
+
+	return err;
+}
+#endif
+
+static void shrm_modem_reset_callback(unsigned long irq)
+{
+	dev_err(shm_dev->dev, "Received mod_reset_req interrupt\n");
+
+#ifdef CONFIG_U8500_SHRM_MODEM_SILENT_RESET
+	{
+		int err;
+		dev_info(shm_dev->dev, "Initiating Modem silent reset\n");
+
+		err = shrm_modem_reset_sequence();
+		if (err)
+			dev_err(shm_dev->dev,
+				"Failed multicast of modem reset\n");
+	}
+#else
+	dev_info(shm_dev->dev, "Modem in reset loop, doing System reset\n");
+
+	/* Call the PRCMU reset API */
+	prcmu_system_reset(SW_RESET_NO_ARGUMENT);
+#endif
+}
+
+DECLARE_TASKLET(shrm_sw_reset_callback, shrm_modem_reset_callback,
+		IRQ_PRCMU_MODEM_SW_RESET_REQ);
+
+static irqreturn_t shrm_prcmu_irq_handler(int irq, void *data)
+{
+	struct shrm_dev *shrm = data;
+
+	switch (irq) {
+	case IRQ_PRCMU_CA_WAKE:
+#ifdef CONFIG_UX500_SUSPEND
+		suspend_block_sleep();
+#endif
+		if (shrm->msr_flag)
+			atomic_set(&ac_sleep_disable_count, 0);
+		atomic_inc(&ac_sleep_disable_count);
+		queue_work(shrm->shm_ca_wake_wq, &shrm->shm_ca_wake_req);
+		break;
+	case IRQ_PRCMU_CA_SLEEP:
+		queue_work(shrm->shm_ca_wake_wq, &shrm->shm_ca_sleep_req);
+		break;
+	case IRQ_PRCMU_MODEM_SW_RESET_REQ:
+		tasklet_schedule(&shrm_sw_reset_callback);
+		break;
+	default:
+		dev_err(shrm->dev, "%s: => IRQ %d\n", __func__, irq);
+		return IRQ_NONE;
+	}
+	return IRQ_HANDLED;
+}
+
+static void send_ac_msg_pend_notify_0_work(struct work_struct *work)
+{
+	struct shrm_dev *shrm = container_of(work, struct shrm_dev,
+			send_ac_msg_pend_notify_0);
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	update_ac_common_shared_wptr(shrm);
+
+	mutex_lock(&ac_state_mutex);
+	atomic_inc(&ac_sleep_disable_count);
+	modem_request(shrm->modem);
+	mutex_unlock(&ac_state_mutex);
+
+	if (!get_host_accessport_val())
+		BUG();
+
+	/* Trigger AcMsgPendingNotification to CMU */
+	writel((1<<GOP_COMMON_AC_MSG_PENDING_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_SET_REGISTER_BASE);
+
+	if (shrm_common_tx_state == SHRM_PTR_FREE)
+		shrm_common_tx_state = SHRM_PTR_BUSY;
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+static void send_ac_msg_pend_notify_1_work(struct work_struct *work)
+{
+	struct shrm_dev *shrm = container_of(work, struct shrm_dev,
+			send_ac_msg_pend_notify_1);
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	/* Update shared_wptr with writer_local_wptr) */
+	update_ac_audio_shared_wptr(shrm);
+
+	mutex_lock(&ac_state_mutex);
+	atomic_inc(&ac_sleep_disable_count);
+	modem_request(shrm->modem);
+	mutex_unlock(&ac_state_mutex);
+
+	if (!get_host_accessport_val())
+		BUG();
+
+	/* Trigger AcMsgPendingNotification to CMU */
+	writel((1<<GOP_AUDIO_AC_MSG_PENDING_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_SET_REGISTER_BASE);
+
+	if (shrm_audio_tx_state == SHRM_PTR_FREE)
+		shrm_audio_tx_state = SHRM_PTR_BUSY;
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void shm_nl_receive(struct sk_buff *skb)
+{
+	struct nlmsghdr *nlh = NULL;
+	int msg;
+
+	dev_dbg(shm_dev->dev, "Received NL msg from user-space\n");
+
+	nlh = (struct nlmsghdr *)skb->data;
+	msg = *((int *)(NLMSG_DATA(nlh)));
+	switch (msg) {
+	case SHRM_NL_MOD_QUERY_STATE:
+		dev_info(shm_dev->dev, "mod-query-state from user-space\n");
+		nl_send_unicast_message(nlh->nlmsg_pid);
+		break;
+
+	case SHRM_NL_USER_MOD_RESET:
+		dev_info(shm_dev->dev, "user-space inited mod-reset-req\n");
+		dev_info(shm_dev->dev, "PCRMU resets modem\n");
+		prcmu_modem_reset();
+		break;
+
+	default:
+		dev_err(shm_dev->dev, "Invalid NL msg from user-space\n");
+		break;
+	};
+}
+
+int shrm_protocol_init(struct shrm_dev *shrm,
+			received_msg_handler common_rx_handler,
+			received_msg_handler audio_rx_handler)
+{
+	int err;
+
+	shm_dev = shrm;
+	boot_state = BOOT_INIT;
+	dev_info(shrm->dev, "IPC_ISA BOOT_INIT\n");
+	rx_common_handler = common_rx_handler;
+	rx_audio_handler = audio_rx_handler;
+	atomic_set(&ac_sleep_disable_count, 0);
+
+	hrtimer_init(&timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	timer.function = callback;
+
+	shrm->shm_common_ch_wr_wq = create_singlethread_workqueue
+		("shm_common_channel_irq");
+	if (!shrm->shm_common_ch_wr_wq) {
+		dev_err(shrm->dev, "failed to create work queue\n");
+		return -ENOMEM;
+	}
+	shrm->shm_audio_ch_wr_wq = create_singlethread_workqueue
+		("shm_audio_channel_irq");
+	if (!shrm->shm_audio_ch_wr_wq) {
+		dev_err(shrm->dev, "failed to create work queue\n");
+		err = -ENOMEM;
+		goto free_wq1;
+	}
+	shrm->shm_ac_wake_wq = create_singlethread_workqueue("shm_ac_wake_req");
+	if (!shrm->shm_ac_wake_wq) {
+		dev_err(shrm->dev, "failed to create work queue\n");
+		err = -ENOMEM;
+		goto free_wq2;
+	}
+	shrm->shm_ca_wake_wq = create_singlethread_workqueue("shm_ca_wake_req");
+	if (!shrm->shm_ac_wake_wq) {
+		dev_err(shrm->dev, "failed to create work queue\n");
+		err = -ENOMEM;
+		goto free_wq3;
+	}
+	shrm->shm_ac_sleep_wq = create_singlethread_workqueue
+						("shm_ac_sleep_req");
+	if (!shrm->shm_ac_sleep_wq) {
+		dev_err(shrm->dev, "failed to create work queue\n");
+		err = -ENOMEM;
+		goto free_wq4;
+	}
+	INIT_WORK(&shrm->send_ac_msg_pend_notify_0,
+			send_ac_msg_pend_notify_0_work);
+	INIT_WORK(&shrm->send_ac_msg_pend_notify_1,
+			send_ac_msg_pend_notify_1_work);
+	INIT_WORK(&shrm->shm_ca_wake_req, shm_ca_wake_req_work);
+	INIT_WORK(&shrm->shm_ca_sleep_req, shm_ca_sleep_req_work);
+	INIT_WORK(&shrm->shm_ac_sleep_req, shm_ac_sleep_req_work);
+	INIT_WORK(&shrm->shm_ac_wake_req, shm_ac_wake_req_work);
+
+	/* set tasklet data */
+	shm_ca_0_tasklet.data = (unsigned long)shrm;
+	shm_ca_1_tasklet.data = (unsigned long)shrm;
+
+	err = request_irq(IRQ_PRCMU_CA_SLEEP, shrm_prcmu_irq_handler,
+			IRQF_NO_SUSPEND, "ca-sleep", shrm);
+	if (err < 0) {
+		dev_err(shm_dev->dev, "Failed alloc IRQ_PRCMU_CA_SLEEP.\n");
+		goto free_wq5;
+	}
+
+	err = request_irq(IRQ_PRCMU_CA_WAKE, shrm_prcmu_irq_handler,
+		IRQF_NO_SUSPEND, "ca-wake", shrm);
+	if (err < 0) {
+		dev_err(shm_dev->dev, "Failed alloc IRQ_PRCMU_CA_WAKE.\n");
+		goto drop2;
+	}
+
+	err = request_irq(IRQ_PRCMU_MODEM_SW_RESET_REQ, shrm_prcmu_irq_handler,
+			IRQF_NO_SUSPEND, "modem-sw-reset-req", shrm);
+	if (err < 0) {
+		dev_err(shm_dev->dev,
+				"Failed alloc IRQ_PRCMU_MODEM_SW_RESET_REQ.\n");
+		goto drop1;
+	}
+
+#ifdef CONFIG_U8500_SHRM_MODEM_SILENT_RESET
+	/* init netlink socket for user-space communication */
+	shrm_nl_sk = netlink_kernel_create(NULL, NETLINK_SHRM, 1,
+			shm_nl_receive, NULL, THIS_MODULE);
+
+	if (!shrm_nl_sk) {
+		dev_err(shm_dev->dev, "netlink socket creation failed\n");
+		goto drop;
+	}
+#endif
+	return 0;
+
+#ifdef CONFIG_U8500_SHRM_MODEM_SILENT_RESET
+drop:
+	free_irq(IRQ_PRCMU_MODEM_SW_RESET_REQ, NULL);
+#endif
+drop1:
+	free_irq(IRQ_PRCMU_CA_WAKE, NULL);
+drop2:
+	free_irq(IRQ_PRCMU_CA_SLEEP, NULL);
+free_wq5:
+	destroy_workqueue(shrm->shm_ac_sleep_wq);
+free_wq4:
+	destroy_workqueue(shrm->shm_ca_wake_wq);
+free_wq3:
+	destroy_workqueue(shrm->shm_ac_wake_wq);
+free_wq2:
+	destroy_workqueue(shrm->shm_audio_ch_wr_wq);
+free_wq1:
+	destroy_workqueue(shrm->shm_common_ch_wr_wq);
+	return err;
+}
+
+void shrm_protocol_deinit(struct shrm_dev *shrm)
+{
+	free_irq(IRQ_PRCMU_CA_SLEEP, NULL);
+	free_irq(IRQ_PRCMU_CA_WAKE, NULL);
+	free_irq(IRQ_PRCMU_MODEM_SW_RESET_REQ, NULL);
+	flush_scheduled_work();
+	destroy_workqueue(shrm->shm_common_ch_wr_wq);
+	destroy_workqueue(shrm->shm_audio_ch_wr_wq);
+	destroy_workqueue(shrm->shm_ac_wake_wq);
+	destroy_workqueue(shrm->shm_ca_wake_wq);
+	destroy_workqueue(shrm->shm_ac_sleep_wq);
+	modem_put(shrm->modem);
+}
+
+int get_ca_wake_req_state(void)
+{
+	return ((atomic_read(&ac_sleep_disable_count) > 0) ||
+			modem_get_usage(shm_dev->modem));
+}
+
+irqreturn_t ca_wake_irq_handler(int irq, void *ctrlr)
+{
+	struct shrm_dev *shrm = ctrlr;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	/* initialize the FIFO Variables */
+	if (boot_state == BOOT_INIT)
+		shm_fifo_init(shrm);
+
+	dev_dbg(shrm->dev, "Inside ca_wake_irq_handler\n");
+
+	/* Clear the interrupt */
+	writel((1 << GOP_CA_WAKE_REQ_BIT),
+				shrm->intr_base + GOP_CLEAR_REGISTER_BASE);
+
+	/* send ca_wake_ack_interrupt to CMU */
+	writel((1 << GOP_CA_WAKE_ACK_BIT),
+		shrm->intr_base + GOP_SET_REGISTER_BASE);
+
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return IRQ_HANDLED;
+}
+
+
+irqreturn_t ac_read_notif_0_irq_handler(int irq, void *ctrlr)
+{
+	struct shrm_dev *shrm = ctrlr;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	shm_ac_read_0_tasklet.data = (unsigned long)shrm;
+	tasklet_schedule(&shm_ac_read_0_tasklet);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	/* Clear the interrupt */
+	writel((1 << GOP_COMMON_AC_READ_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_CLEAR_REGISTER_BASE);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return IRQ_HANDLED;
+}
+
+irqreturn_t ac_read_notif_1_irq_handler(int irq, void *ctrlr)
+{
+	struct shrm_dev *shrm = ctrlr;
+
+	dev_dbg(shrm->dev, "%s IN+\n", __func__);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	shm_ac_read_1_tasklet.data = (unsigned long)shrm;
+	tasklet_schedule(&shm_ac_read_1_tasklet);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	/* Clear the interrupt */
+	writel((1 << GOP_AUDIO_AC_READ_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_CLEAR_REGISTER_BASE);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return IRQ_HANDLED;
+}
+
+irqreturn_t ca_msg_pending_notif_0_irq_handler(int irq, void *ctrlr)
+{
+	struct shrm_dev *shrm = ctrlr;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	tasklet_schedule(&shm_ca_0_tasklet);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	/* Clear the interrupt */
+	writel((1 << GOP_COMMON_CA_MSG_PENDING_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_CLEAR_REGISTER_BASE);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return IRQ_HANDLED;
+}
+
+irqreturn_t ca_msg_pending_notif_1_irq_handler(int irq, void *ctrlr)
+{
+	struct shrm_dev *shrm = ctrlr;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	tasklet_schedule(&shm_ca_1_tasklet);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	/* Clear the interrupt */
+	writel((1<<GOP_AUDIO_CA_MSG_PENDING_NOTIFICATION_BIT),
+			shrm->intr_base+GOP_CLEAR_REGISTER_BASE);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return IRQ_HANDLED;
+
+}
+
+/**
+ * shm_write_msg() - write message to shared memory
+ * @shrm:	pointer to the shrm device information structure
+ * @l2_header:	L2 header
+ * @addr:	pointer to the message
+ * @length:	length of the message to be written
+ *
+ * This function is called from net or char interface driver write operation.
+ * Prior to calling this function the message is copied from the user space
+ * buffer to the kernel buffer. This function based on the l2 header routes
+ * the message to the respective channel and FIFO. Then makes a call to the
+ * fifo write function where the message is written to the physical device.
+ */
+int shm_write_msg(struct shrm_dev *shrm, u8 l2_header,
+					void *addr, u32 length)
+{
+	u8 channel = 0;
+	int ret;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (boot_state != BOOT_DONE) {
+		dev_err(shrm->dev,
+				"error after boot done  call this fn\n");
+		ret = -ENODEV;
+		goto out;
+	}
+
+	if ((l2_header == L2_HEADER_ISI) ||
+			(l2_header == L2_HEADER_RPC) ||
+			(l2_header == L2_HEADER_SECURITY) ||
+			(l2_header == L2_HEADER_COMMON_SIMPLE_LOOPBACK) ||
+			(l2_header == L2_HEADER_COMMON_ADVANCED_LOOPBACK) ||
+			(l2_header == L2_HEADER_CIQ)) {
+		channel = 0;
+		if (shrm_common_tx_state == SHRM_SLEEP_STATE)
+			shrm_common_tx_state = SHRM_PTR_FREE;
+		else if (shrm_common_tx_state == SHRM_IDLE)
+			shrm_common_tx_state = SHRM_PTR_FREE;
+
+	} else if ((l2_header == L2_HEADER_AUDIO) ||
+			(l2_header == L2_HEADER_AUDIO_SIMPLE_LOOPBACK) ||
+			(l2_header == L2_HEADER_AUDIO_ADVANCED_LOOPBACK)) {
+		if (shrm_audio_tx_state == SHRM_SLEEP_STATE)
+			shrm_audio_tx_state = SHRM_PTR_FREE;
+		else if (shrm_audio_tx_state == SHRM_IDLE)
+			shrm_audio_tx_state = SHRM_PTR_FREE;
+
+		channel = 1;
+	} else {
+		ret = -ENODEV;
+		goto out;
+	}
+	ret = shm_write_msg_to_fifo(shrm, channel, l2_header, addr, length);
+	if (ret < 0) {
+		dev_err(shrm->dev, "write message to fifo failed\n");
+		return ret;
+	}
+	/*
+	 * notify only if new msg copied is the only unread one
+	 * otherwise it means that reading process is ongoing
+	 */
+	if (is_the_only_one_unread_message(shrm, channel, length)) {
+
+		/* Send Message Pending Noitication to CMT */
+		if (channel == 0)
+			queue_work(shrm->shm_common_ch_wr_wq,
+					&shrm->send_ac_msg_pend_notify_0);
+		else
+			queue_work(shrm->shm_audio_ch_wr_wq,
+					&shrm->send_ac_msg_pend_notify_1);
+
+	}
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return 0;
+
+out:
+	return ret;
+}
+
+void ca_msg_read_notification_0(struct shrm_dev *shrm)
+{
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (get_ca_msg_0_read_notif_send() == 0) {
+		update_ca_common_shared_rptr(shrm);
+
+		if (check_modem_in_reset()) {
+			dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+					__func__);
+			return;
+		}
+
+		/* Trigger CaMsgReadNotification to CMU */
+		writel((1 << GOP_COMMON_CA_READ_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_SET_REGISTER_BASE);
+		set_ca_msg_0_read_notif_send(1);
+		shrm_common_rx_state = SHRM_PTR_BUSY;
+	}
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void ca_msg_read_notification_1(struct shrm_dev *shrm)
+{
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (get_ca_msg_1_read_notif_send() == 0) {
+		update_ca_audio_shared_rptr(shrm);
+
+		if (check_modem_in_reset()) {
+			dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+					__func__);
+			return;
+		}
+
+		/* Trigger CaMsgReadNotification to CMU */
+		writel((1<<GOP_AUDIO_CA_READ_NOTIFICATION_BIT),
+			shrm->intr_base+GOP_SET_REGISTER_BASE);
+		set_ca_msg_1_read_notif_send(1);
+		shrm_audio_rx_state = SHRM_PTR_BUSY;
+	}
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+/**
+ * receive_messages_common - receive common channnel msg from
+ * CMT(Cellular Mobile Terminal)
+ * @shrm:	pointer to shrm device information structure
+ *
+ * The messages sent from CMT to APE are written to the respective FIFO
+ * and an interrupt is triggered by the CMT. This ca message pending
+ * interrupt calls this function. This function sends a read notification
+ * acknowledgement to the CMT and calls the common channel receive handler
+ * where the messsage is copied to the respective(ISI, RPC, SECURIT) queue
+ * based on the message l2 header.
+ */
+void receive_messages_common(struct shrm_dev *shrm)
+{
+	u8 l2_header;
+	u32 len;
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return;
+	}
+
+	l2_header = read_one_l2msg_common(shrm, recieve_common_msg, &len);
+	/* Send Recieve_Call_back to Upper Layer */
+	if (!rx_common_handler) {
+		dev_err(shrm->dev, "common_rx_handler is Null\n");
+		BUG();
+	}
+	(*rx_common_handler)(l2_header, &recieve_common_msg, len,
+					shrm);
+	/* SendReadNotification */
+	ca_msg_read_notification_0(shrm);
+
+	while (read_remaining_messages_common()) {
+		if (check_modem_in_reset()) {
+			dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+					__func__);
+			return;
+		}
+
+		l2_header = read_one_l2msg_common(shrm, recieve_common_msg,
+								&len);
+		/* Send Recieve_Call_back to Upper Layer */
+		(*rx_common_handler)(l2_header,
+					&recieve_common_msg, len,
+					shrm);
+	}
+}
+
+/**
+ * receive_messages_audio() - receive audio message from CMT
+ * @shrm:	pointer to shrm device information structure
+ *
+ * The messages sent from CMT to APE are written to the respective FIFO
+ * and an interrupt is triggered by the CMT. This ca message pending
+ * interrupt calls this function. This function sends a read notification
+ * acknowledgement to the CMT and calls the common channel receive handler
+ * where the messsage is copied to the audio queue.
+ */
+void receive_messages_audio(struct shrm_dev *shrm)
+{
+	u8 l2_header;
+	u32 len;
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return;
+	}
+
+	l2_header = read_one_l2msg_audio(shrm, recieve_audio_msg, &len);
+	/* Send Recieve_Call_back to Upper Layer */
+
+	if (!rx_audio_handler) {
+		dev_crit(shrm->dev, "audio_rx_handler is Null\n");
+		BUG();
+	}
+	(*rx_audio_handler)(l2_header, &recieve_audio_msg,
+					len, shrm);
+
+	/* SendReadNotification */
+	ca_msg_read_notification_1(shrm);
+	while (read_remaining_messages_audio())	{
+		if (check_modem_in_reset()) {
+			dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+					__func__);
+			return;
+		}
+
+		l2_header = read_one_l2msg_audio(shrm,
+						recieve_audio_msg, &len);
+		/* Send Recieve_Call_back to Upper Layer */
+		(*rx_audio_handler)(l2_header,
+					&recieve_audio_msg, len,
+					shrm);
+	}
+}
+
+u8 get_boot_state()
+{
+	return boot_state;
+}
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index 366624f7ab4..6667410a268 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -309,3 +309,7 @@ obj-$(CONFIG_CAIF) += caif/
 obj-$(CONFIG_OCTEON_MGMT_ETHERNET) += octeon/
 obj-$(CONFIG_PCH_GBE) += pch_gbe/
 obj-$(CONFIG_TILE_NET) += tile/
+
+ifdef CONFIG_PHONET
+obj-$(CONFIG_U8500_SHRM) += u8500_shrm.o
+endif
diff --git a/drivers/net/caif/Kconfig b/drivers/net/caif/Kconfig
index 09ed3f42d67..0d4427ef723 100644
--- a/drivers/net/caif/Kconfig
+++ b/drivers/net/caif/Kconfig
@@ -38,3 +38,12 @@ config CAIF_SHM
 	default n
 	---help---
 	The CAIF shared memory protocol driver for the STE UX5500 platform.
+
+config CAIF_HSI
+	tristate "CAIF HSI transport driver"
+	depends on CAIF
+	default n
+	---help---
+	The caif low level driver for CAIF over HSI.
+	Be aware that if you enable this then you also need to
+	enable a low-level HSI driver.
diff --git a/drivers/net/caif/Makefile b/drivers/net/caif/Makefile
index 9560b9d624b..91dff861560 100644
--- a/drivers/net/caif/Makefile
+++ b/drivers/net/caif/Makefile
@@ -10,3 +10,6 @@ obj-$(CONFIG_CAIF_SPI_SLAVE) += cfspi_slave.o
 # Shared memory
 caif_shm-objs := caif_shmcore.o caif_shm_u5500.o
 obj-$(CONFIG_CAIF_SHM) += caif_shm.o
+
+# HSI interface
+obj-$(CONFIG_CAIF_HSI) += caif_hsi.o
diff --git a/drivers/net/caif/caif_hsi.c b/drivers/net/caif/caif_hsi.c
new file mode 100644
index 00000000000..73f78623cfe
--- /dev/null
+++ b/drivers/net/caif/caif_hsi.c
@@ -0,0 +1,623 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Contact: Sjur Brendeland <sjur.brandeland@stericsson.com>
+ * Author:  Daniel Martensson <Daniel.Martensson@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/list.h>
+#include <linux/if_arp.h>
+#include <net/caif/caif_hsi.h>
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Daniel Martensson<daniel.martensson@stericsson.com>");
+MODULE_DESCRIPTION("CAIF HSI driver");
+
+/* Returns the number of padding bytes for alignment. */
+#define PAD_POW2(x, pow) ((((x)&((pow)-1)) == 0) ? 0 : \
+				(((pow)-((x)&((pow)-1)))))
+
+/*
+ * HSI padding options.
+ * Warning: must be a base of 2 (& operation used) and can not be zero !
+ */
+static int hsi_head_align = 4;
+module_param(hsi_head_align, int, S_IRUGO);
+MODULE_PARM_DESC(hsi_head_align, "HSI head alignment.");
+
+static int hsi_tail_align = 4;
+module_param(hsi_tail_align, int, S_IRUGO);
+MODULE_PARM_DESC(hsi_tail_align, "HSI tail alignment.");
+
+/*
+ * HSI link layer flowcontrol thresholds.
+ * Warning: A high threshold value migth increase throughput but it will at
+ * the same time prevent channel prioritization and increase the risk of
+ * flooding the modem. The high threshold should be above the low.
+ */
+static int hsi_high_threshold = 100;
+module_param(hsi_high_threshold, int, S_IRUGO);
+MODULE_PARM_DESC(hsi_high_threshold, "HSI high threshold (FLOW OFF).");
+
+static int hsi_low_threshold = 50;
+module_param(hsi_low_threshold, int, S_IRUGO);
+MODULE_PARM_DESC(hsi_low_threshold, "HSI high threshold (FLOW ON).");
+
+#define ON 1
+#define OFF 0
+
+/*
+ * Threshold values for the HSI packet queue. Flowcontrol will be asserted
+ * when the number of packets exceeds HIGH_WATER_MARK. It will not be
+ * de-asserted before the number of packets drops below LOW_WATER_MARK.
+ */
+#define LOW_WATER_MARK   hsi_low_threshold
+#define HIGH_WATER_MARK  hsi_high_threshold
+
+static LIST_HEAD(cfhsi_list);
+static spinlock_t cfhsi_list_lock;
+
+static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
+{
+	int nfrms = 0;
+	int pld_len = 0;
+	struct sk_buff *skb;
+	u8 *pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
+
+	skb = skb_peek(&cfhsi->qhead);
+	if (!skb)
+		return 0;
+
+	/* Check if we can embed a CAIF frame. */
+	if (skb->len < CFHSI_MAX_EMB_FRM_SZ) {
+		struct caif_payload_info *info;
+		int hpad = 0;
+		int tpad = 0;
+
+		/* Calculate needed head alignment and tail alignment. */
+		info = (struct caif_payload_info *)&skb->cb;
+
+		hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align);
+		tpad = PAD_POW2((skb->len + hpad), hsi_tail_align);
+
+		/* Check if frame still fits with added alignment. */
+		if ((skb->len + hpad + tpad) <= CFHSI_MAX_EMB_FRM_SZ) {
+			u8 *pemb = desc->emb_frm;
+			skb = skb_dequeue(&cfhsi->qhead);
+			desc->offset = CFHSI_DESC_SHORT_SZ;
+			*pemb = (u8)(hpad - 1);
+			pemb += hpad;
+
+			/* Copy in embedded CAIF frame. */
+			skb_copy_bits(skb, 0, pemb, skb->len);
+			kfree_skb(skb);
+		}
+	} else {
+		/* Clear offset. */
+		desc->offset = 0;
+	}
+
+
+	/* Create payload CAIF frames. */
+	pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
+	while (skb_peek(&cfhsi->qhead) && nfrms < CFHSI_MAX_PKTS) {
+		struct caif_payload_info *info;
+		int hpad = 0;
+		int tpad = 0;
+
+		skb = skb_dequeue(&cfhsi->qhead);
+
+		/* Calculate needed head alignment and tail alignment. */
+		info = (struct caif_payload_info *)&skb->cb;
+
+		hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align);
+		tpad = PAD_POW2((skb->len + hpad), hsi_tail_align);
+
+		/* Fill in CAIF frame length in descriptor. */
+		desc->cffrm_len[nfrms] = hpad + skb->len + tpad;
+
+		/* Fill head padding information. */
+		*pfrm = (u8)(hpad - 1);
+		pfrm += hpad;
+
+		/* Copy in CAIF frame. */
+		skb_copy_bits(skb, 0, pfrm, skb->len);
+		kfree_skb(skb);
+
+		/* Update payload length. */
+		pld_len += desc->cffrm_len[nfrms];
+
+		/* Update frame pointer. */
+		pfrm += skb->len + tpad;
+
+		/* Update number of frames. */
+		nfrms++;
+	}
+
+	/* Unused length fields should be zero-filled (according to SPEC). */
+	while (nfrms < CFHSI_MAX_PKTS) {
+		desc->cffrm_len[nfrms] = 0x0000;
+		nfrms++;
+	}
+
+	/* Check if we can piggy-back another descriptor. */
+	skb = skb_peek(&cfhsi->qhead);
+	if (skb)
+		desc->header |= CFHSI_PIGGY_DESC;
+	else
+		desc->header &= ~CFHSI_PIGGY_DESC;
+
+	return CFHSI_DESC_SZ + pld_len;
+}
+
+static void cfhsi_tx_done_cb(struct cfhsi_drv *drv)
+{
+	struct cfhsi *cfhsi = NULL;
+	struct cfhsi_desc *desc = NULL;
+	struct sk_buff *skb;
+	unsigned long flags;
+	int len = 0;
+
+	cfhsi = container_of(drv, struct cfhsi, drv);
+	desc = (struct cfhsi_desc *)cfhsi->tx_buf;
+
+	spin_lock_irqsave(&cfhsi->lock, flags);
+
+	/*
+	 * Send flow on if flow off has been previously signalled
+	 * and number of packets is below low water mark.
+	 */
+	if (cfhsi->flow_off_sent && cfhsi->qhead.qlen <= cfhsi->q_low_mark &&
+		cfhsi->cfdev.flowctrl) {
+		cfhsi->flow_off_sent = 0;
+		cfhsi->cfdev.flowctrl(cfhsi->ndev, ON);
+	}
+
+	skb = skb_peek(&cfhsi->qhead);
+	if (!skb) {
+		cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
+		spin_unlock_irqrestore(&cfhsi->lock, flags);
+		return;
+	}
+
+
+	/* Create HSI frame. */
+	len = cfhsi_tx_frm(desc, cfhsi);
+	BUG_ON(!len);
+	spin_unlock_irqrestore(&cfhsi->lock, flags);
+
+	/* Set up new transfer. */
+	cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
+}
+
+static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
+{
+	int xfer_sz = 0;
+	int nfrms = 0;
+	u16 *plen = NULL;
+	u8 *pfrm = NULL;
+
+	/* Sanity check header and offset. */
+	BUG_ON(desc->header & ~CFHSI_PIGGY_DESC);
+	BUG_ON(desc->offset > CFHSI_MAX_EMB_FRM_SZ);
+
+	/* Check for embedded CAIF frame. */
+	if (desc->offset) {
+		struct sk_buff *skb = NULL;
+		u8 *dst = NULL;
+		int len = 0;
+		pfrm = ((u8 *)desc) + desc->offset;
+
+		/* Remove offset padding. */
+		pfrm += *pfrm + 1;
+
+		/* Read length of CAIF frame (little endian). */
+		len = *pfrm;
+		len |= ((*(pfrm+1)) << 8) & 0xFF00;
+		/* Add FCS fields. */
+		len += 2;
+
+		/* Allocate SKB (OK even in IRQ context). */
+		skb = netdev_alloc_skb(cfhsi->ndev, len + 1);
+		if (skb == NULL)
+			goto err;
+
+		dst = skb_put(skb, len);
+		memcpy(dst, pfrm, len);
+
+		skb->protocol = htons(ETH_P_CAIF);
+		skb_reset_mac_header(skb);
+		skb->dev = cfhsi->ndev;
+
+		/*
+		 * We are called from a arch specific platform device.
+		 * Unfortunately we don't know what context we're
+		 * running in. HSI might well run in a work queue as
+		 * the HSI protocol might require the driver to sleep.
+		 */
+		if (in_interrupt())
+			(void)netif_rx(skb);
+		else
+			(void)netif_rx_ni(skb);
+
+		/* Update statistics. */
+		cfhsi->ndev->stats.rx_packets++;
+		cfhsi->ndev->stats.rx_bytes += len;
+	}
+
+	/* Calculate transfer length. */
+	plen = desc->cffrm_len;
+	while (nfrms < CFHSI_MAX_PKTS && *plen) {
+		xfer_sz += *plen;
+		plen++;
+		nfrms++;
+	}
+
+	/* Check for piggy-backed descriptor. */
+	if (desc->header & CFHSI_PIGGY_DESC)
+		xfer_sz += CFHSI_DESC_SZ;
+
+err:
+	return xfer_sz;
+}
+
+static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
+{
+	int rx_sz = 0;
+	int nfrms = 0;
+	u16 *plen = NULL;
+	u8 *pfrm = NULL;
+
+	/* Sanity check header and offset. */
+	BUG_ON(desc->header & ~CFHSI_PIGGY_DESC);
+	BUG_ON(desc->offset > CFHSI_MAX_EMB_FRM_SZ);
+
+	/* Set frame pointer to start of payload. */
+	pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
+	plen = desc->cffrm_len;
+	while (nfrms < CFHSI_MAX_PKTS && *plen) {
+		struct sk_buff *skb;
+		u8 *dst = NULL;
+		u8 *pcffrm = NULL;
+		int len = 0;
+
+		BUG_ON(desc->cffrm_len[nfrms] > CFHSI_MAX_PAYLOAD_SZ);
+
+		/* CAIF frame starts after head padding. */
+		pcffrm = pfrm + *pfrm + 1;
+
+		/* Read length of CAIF frame (little endian). */
+		len = *pcffrm;
+		len |= ((*(pcffrm + 1)) << 8) & 0xFF00;
+		/* Add FCS fields. */
+		len += 2;
+
+		/* Allocate SKB (OK even in IRQ context). */
+		skb = netdev_alloc_skb(cfhsi->ndev, len + 1);
+		if (skb == NULL)
+			goto err;
+
+		dst = skb_put(skb, len);
+		memcpy(dst, pcffrm, len);
+
+		skb->protocol = htons(ETH_P_CAIF);
+		skb_reset_mac_header(skb);
+		skb->dev = cfhsi->ndev;
+
+		/*
+		 * As explained above we're called from a platform
+		 * device, and don't know the context we're running in.
+		 */
+		if (in_interrupt())
+			(void)netif_rx(skb);
+		else
+			(void)netif_rx_ni(skb);
+
+		/* Update statistics. */
+		cfhsi->ndev->stats.rx_packets++;
+		cfhsi->ndev->stats.rx_bytes += len;
+
+		pfrm += *plen;
+		rx_sz += *plen;
+		plen++;
+		nfrms++;
+	}
+
+err:
+	return rx_sz;
+}
+
+static void cfhsi_rx_done_cb(struct cfhsi_drv *drv)
+{
+	struct cfhsi *cfhsi = NULL;
+	struct cfhsi_desc *desc = NULL;
+	int len = 0;
+	u8 *ptr = NULL;
+
+	cfhsi = container_of(drv, struct cfhsi, drv);
+	desc = (struct cfhsi_desc *)cfhsi->rx_buf;
+
+	if (cfhsi->rx_state == CFHSI_RX_STATE_DESC)
+		len = cfhsi_rx_desc(desc, cfhsi);
+	else {
+		int pld_len = cfhsi_rx_pld(desc, cfhsi);
+
+		if (desc->header & CFHSI_PIGGY_DESC) {
+			struct cfhsi_desc *piggy_desc;
+			piggy_desc = (struct cfhsi_desc *)(desc->emb_frm +
+						CFHSI_MAX_EMB_FRM_SZ + pld_len);
+
+			/* Extract piggy-backed descriptor. */
+			len = cfhsi_rx_desc(piggy_desc, cfhsi);
+
+			/*
+			 * Copy needed information from the piggy-backed
+			 * descriptor to the descriptor in the start.
+			 */
+			memcpy((u8 *)desc, (u8 *)piggy_desc,
+					CFHSI_DESC_SHORT_SZ);
+		}
+	}
+
+	if (len) {
+		cfhsi->rx_state = CFHSI_RX_STATE_PAYLOAD;
+		ptr = cfhsi->rx_buf + CFHSI_DESC_SZ;
+	} else {
+		len = CFHSI_DESC_SZ;
+		cfhsi->rx_state = CFHSI_RX_STATE_DESC;
+		ptr = cfhsi->rx_buf;
+	}
+
+	/* Set up new transfer. */
+	cfhsi->dev->cfhsi_rx(ptr, len, cfhsi->dev);
+}
+
+static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct cfhsi *cfhsi = NULL;
+	unsigned long flags;
+	int start_xfer = 0;
+
+	if (!dev)
+		return -EINVAL;
+
+	cfhsi = netdev_priv(dev);
+
+	spin_lock_irqsave(&cfhsi->lock, flags);
+
+	skb_queue_tail(&cfhsi->qhead, skb);
+
+	/* Send flow off if number of packets is above high water mark. */
+	if (!cfhsi->flow_off_sent &&
+		cfhsi->qhead.qlen > cfhsi->q_high_mark &&
+		cfhsi->cfdev.flowctrl) {
+		cfhsi->flow_off_sent = 1;
+		cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF);
+	}
+
+	if (cfhsi->tx_state == CFHSI_TX_STATE_IDLE) {
+		cfhsi->tx_state = CFHSI_TX_STATE_XFER;
+		start_xfer = 1;
+	}
+
+	if (start_xfer) {
+		struct cfhsi_desc *desc = (struct cfhsi_desc *) cfhsi->tx_buf;
+		int len;
+
+		/* Create HSI frame. */
+		len = cfhsi_tx_frm(desc, cfhsi);
+		BUG_ON(!len);
+
+		spin_unlock_irqrestore(&cfhsi->lock, flags);
+
+		/* Set up new transfer. */
+		cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
+		return 0;
+	}
+
+	spin_unlock_irqrestore(&cfhsi->lock, flags);
+	return 0;
+}
+
+static int cfhsi_open(struct net_device *dev)
+{
+	netif_wake_queue(dev);
+
+	return 0;
+}
+
+static int cfhsi_close(struct net_device *dev)
+{
+	netif_stop_queue(dev);
+
+	return 0;
+}
+static const struct net_device_ops cfhsi_ops = {
+	.ndo_open = cfhsi_open,
+	.ndo_stop = cfhsi_close,
+	.ndo_start_xmit = cfhsi_xmit
+};
+
+static void cfhsi_setup(struct net_device *dev)
+{
+	struct cfhsi *cfhsi = netdev_priv(dev);
+	dev->features = 0;
+	dev->netdev_ops = &cfhsi_ops;
+	dev->type = ARPHRD_CAIF;
+	dev->flags = IFF_POINTOPOINT | IFF_NOARP;
+	dev->mtu = CFHSI_MAX_PAYLOAD_SZ;
+	dev->tx_queue_len = 0;
+	dev->destructor = free_netdev;
+	skb_queue_head_init(&cfhsi->qhead);
+	cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW;
+	cfhsi->cfdev.use_frag = false;
+	cfhsi->cfdev.use_stx = false;
+	cfhsi->cfdev.use_fcs = false;
+	cfhsi->ndev = dev;
+}
+
+int cfhsi_probe(struct platform_device *pdev)
+{
+	struct cfhsi *cfhsi = NULL;
+	struct net_device *ndev;
+	struct cfhsi_dev *dev;
+	int res;
+
+	ndev = alloc_netdev(sizeof(struct cfhsi), "cfhsi%d", cfhsi_setup);
+	if (!ndev) {
+		pr_info("cfhsi_probe: alloc_netdev failed.\n");
+		return -ENODEV;
+	}
+
+	cfhsi = netdev_priv(ndev);
+	netif_stop_queue(ndev);
+	cfhsi->ndev = ndev;
+	cfhsi->pdev = pdev;
+
+	/* Initialize state vaiables. */
+	cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
+	cfhsi->rx_state = CFHSI_RX_STATE_DESC;
+
+	/* Set flow info */
+	cfhsi->flow_off_sent = 0;
+	cfhsi->q_low_mark = LOW_WATER_MARK;
+	cfhsi->q_high_mark = HIGH_WATER_MARK;
+
+	/* Assign the HSI device. */
+	dev = (struct cfhsi_dev *)pdev->dev.platform_data;
+	cfhsi->dev = dev;
+
+	/* Assign the driver to this HSI device. */
+	dev->drv = &cfhsi->drv;
+
+	/*
+	 * Allocate a TX buffer with the size of a HSI packet descriptors
+	 * and the necessary room for CAIF payload frames.
+	 */
+	cfhsi->tx_buf = kzalloc(CFHSI_BUF_SZ_TX, GFP_KERNEL);
+	if (!cfhsi->tx_buf) {
+		netdev_err(cfhsi->ndev, "cfhsi: failed to allocate TX buffer.\n");
+		res = -ENODEV;
+		goto err_alloc_tx;
+	}
+
+	/*
+	 * Allocate a RX buffer with the size of two HSI packet descriptors and
+	 * the necessary room for CAIF payload frames.
+	 */
+	cfhsi->rx_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL);
+	if (!cfhsi->rx_buf) {
+		netdev_err(cfhsi->ndev, "cfhsi: failed to allocate RX buffer.\n");
+		res = -ENODEV;
+		goto err_alloc_rx;
+	}
+
+	/* Initialize spin locks. */
+	spin_lock_init(&cfhsi->lock);
+
+	/* Set up the driver. */
+	cfhsi->drv.tx_done_cb = cfhsi_tx_done_cb;
+	cfhsi->drv.rx_done_cb = cfhsi_rx_done_cb;
+
+	/* Add CAIF HSI device to list. */
+	spin_lock(&cfhsi_list_lock);
+	list_add_tail(&cfhsi->list, &cfhsi_list);
+	spin_unlock(&cfhsi_list_lock);
+
+	/* Register network device. */
+	res = register_netdev(ndev);
+	if (res) {
+		netdev_err(cfhsi->ndev, "cfhsi: Reg. error: %d.\n", res);
+		goto err_net_reg;
+	}
+
+	/* Start an initial read operation. */
+	cfhsi->dev->cfhsi_rx(cfhsi->rx_buf, CFHSI_DESC_SZ, cfhsi->dev);
+
+	return res;
+
+err_net_reg:
+	kfree(cfhsi->rx_buf);
+err_alloc_rx:
+	kfree(cfhsi->tx_buf);
+err_alloc_tx:
+	free_netdev(ndev);
+
+	return res;
+}
+
+int cfhsi_remove(struct platform_device *pdev)
+{
+	struct list_head *list_node;
+	struct list_head *n;
+	struct cfhsi *cfhsi = NULL;
+	struct cfhsi_dev *dev;
+
+	dev = (struct cfhsi_dev *)pdev->dev.platform_data;
+	spin_lock(&cfhsi_list_lock);
+	list_for_each_safe(list_node, n, &cfhsi_list) {
+		cfhsi = list_entry(list_node, struct cfhsi, list);
+		/* Find the corresponding device. */
+		if (cfhsi->dev == dev) {
+			/* Remove from list. */
+			list_del(list_node);
+			/* Free buffers. */
+			kfree(cfhsi->tx_buf);
+			kfree(cfhsi->rx_buf);
+			unregister_netdev(cfhsi->ndev);
+			spin_unlock(&cfhsi_list_lock);
+			return 0;
+		}
+	}
+	spin_unlock(&cfhsi_list_lock);
+	return -ENODEV;
+}
+
+struct platform_driver cfhsi_plat_drv = {
+	.probe = cfhsi_probe,
+	.remove = cfhsi_remove,
+	.driver = {
+		   .name = "cfhsi",
+		   .owner = THIS_MODULE,
+		   },
+};
+
+static void __exit cfhsi_exit_module(void)
+{
+	struct list_head *list_node;
+	struct list_head *n;
+	struct cfhsi *cfhsi = NULL;
+
+	list_for_each_safe(list_node, n, &cfhsi_list) {
+		cfhsi = list_entry(list_node, struct cfhsi, list);
+		platform_device_unregister(cfhsi->pdev);
+	}
+
+	/* Unregister platform driver. */
+	platform_driver_unregister(&cfhsi_plat_drv);
+}
+
+static int __init cfhsi_init_module(void)
+{
+	int result;
+
+	/* Initialize spin lock. */
+	spin_lock_init(&cfhsi_list_lock);
+
+	/* Register platform driver. */
+	result = platform_driver_register(&cfhsi_plat_drv);
+	if (result) {
+		pr_warning("Could not register platform HSI driver.\n");
+		goto err_dev_register;
+	}
+
+	return result;
+
+err_dev_register:
+	return result;
+}
+
+module_init(cfhsi_init_module);
+module_exit(cfhsi_exit_module);
diff --git a/drivers/net/caif/caif_serial.c b/drivers/net/caif/caif_serial.c
index 3df0c0f8b8b..318821b14fe 100644
--- a/drivers/net/caif/caif_serial.c
+++ b/drivers/net/caif/caif_serial.c
@@ -182,6 +182,7 @@ static void ldisc_receive(struct tty_struct *tty, const u8 *data,
 	 * This is not yet handled.
 	 */
 
+	BUG_ON(ser->dev == NULL);
 
 	/*
 	 * Workaround for garbage at start of transmission,
diff --git a/drivers/net/caif/caif_spi.c b/drivers/net/caif/caif_spi.c
index 57e63937381..6fb7865c94b 100644
--- a/drivers/net/caif/caif_spi.c
+++ b/drivers/net/caif/caif_spi.c
@@ -227,7 +227,7 @@ static ssize_t dbgfs_frame(struct file *file, char __user *user_buf,
 			"Tx data (Len: %d):\n", cfspi->tx_cpck_len);
 
 	len += print_frame((buf + len), (DEBUGFS_BUF_SIZE - len),
-			   cfspi->xfer.va_tx,
+			   cfspi->xfer.va_tx[0],
 			   (cfspi->tx_cpck_len + SPI_CMD_SZ), 100);
 
 	len += snprintf((buf + len), (DEBUGFS_BUF_SIZE - len),
@@ -600,48 +600,11 @@ static int cfspi_close(struct net_device *dev)
 	netif_stop_queue(dev);
 	return 0;
 }
-static const struct net_device_ops cfspi_ops = {
-	.ndo_open = cfspi_open,
-	.ndo_stop = cfspi_close,
-	.ndo_start_xmit = cfspi_xmit
-};
 
-static void cfspi_setup(struct net_device *dev)
+static int cfspi_init(struct net_device *dev)
 {
+	int res = 0;
 	struct cfspi *cfspi = netdev_priv(dev);
-	dev->features = 0;
-	dev->netdev_ops = &cfspi_ops;
-	dev->type = ARPHRD_CAIF;
-	dev->flags = IFF_NOARP | IFF_POINTOPOINT;
-	dev->tx_queue_len = 0;
-	dev->mtu = SPI_MAX_PAYLOAD_SIZE;
-	dev->destructor = free_netdev;
-	skb_queue_head_init(&cfspi->qhead);
-	skb_queue_head_init(&cfspi->chead);
-	cfspi->cfdev.link_select = CAIF_LINK_HIGH_BANDW;
-	cfspi->cfdev.use_frag = false;
-	cfspi->cfdev.use_stx = false;
-	cfspi->cfdev.use_fcs = false;
-	cfspi->ndev = dev;
-}
-
-int cfspi_spi_probe(struct platform_device *pdev)
-{
-	struct cfspi *cfspi = NULL;
-	struct net_device *ndev;
-	struct cfspi_dev *dev;
-	int res;
-	dev = (struct cfspi_dev *)pdev->dev.platform_data;
-
-	ndev = alloc_netdev(sizeof(struct cfspi),
-			"cfspi%d", cfspi_setup);
-	if (!ndev)
-		return -ENOMEM;
-
-	cfspi = netdev_priv(ndev);
-	netif_stop_queue(ndev);
-	cfspi->ndev = ndev;
-	cfspi->pdev = pdev;
 
 	/* Set flow info. */
 	cfspi->flow_off_sent = 0;
@@ -657,18 +620,13 @@ int cfspi_spi_probe(struct platform_device *pdev)
 		cfspi->slave_talked = false;
 	}
 
-	/* Assign the SPI device. */
-	cfspi->dev = dev;
-	/* Assign the device ifc to this SPI interface. */
-	dev->ifc = &cfspi->ifc;
-
 	/* Allocate DMA buffers. */
-	cfspi->xfer.va_tx = dma_alloc(&cfspi->xfer.pa_tx);
-	if (!cfspi->xfer.va_tx) {
+	cfspi->xfer.va_tx[0] = dma_alloc(&cfspi->xfer.pa_tx[0]);
+	if (!cfspi->xfer.va_tx[0]) {
 		printk(KERN_WARNING
 		       "CFSPI: failed to allocate dma TX buffer.\n");
 		res = -ENODEV;
-		goto err_dma_alloc_tx;
+		goto err_dma_alloc_tx_0;
 	}
 
 	cfspi->xfer.va_rx = dma_alloc(&cfspi->xfer.pa_rx);
@@ -719,6 +677,87 @@ int cfspi_spi_probe(struct platform_device *pdev)
 	/* Schedule the work queue. */
 	queue_work(cfspi->wq, &cfspi->work);
 
+	return 0;
+
+ err_create_wq:
+	dma_free(cfspi->xfer.va_rx, cfspi->xfer.pa_rx);
+ err_dma_alloc_rx:
+	dma_free(cfspi->xfer.va_tx[0], cfspi->xfer.pa_tx[0]);
+ err_dma_alloc_tx_0:
+	return res;
+}
+
+static void cfspi_uninit(struct net_device *dev)
+{
+	struct cfspi *cfspi = netdev_priv(dev);
+
+	/* Remove from list. */
+	spin_lock(&cfspi_list_lock);
+	list_del(&cfspi->list);
+	spin_unlock(&cfspi_list_lock);
+
+	cfspi->ndev = NULL;
+	/* Free DMA buffers. */
+	dma_free(cfspi->xfer.va_rx, cfspi->xfer.pa_rx);
+	dma_free(cfspi->xfer.va_tx[0], cfspi->xfer.pa_tx[0]);
+	set_bit(SPI_TERMINATE, &cfspi->state);
+	wake_up_interruptible(&cfspi->wait);
+	destroy_workqueue(cfspi->wq);
+	/* Destroy debugfs directory and files. */
+	dev_debugfs_rem(cfspi);
+	return;
+}
+
+static const struct net_device_ops cfspi_ops = {
+	.ndo_open = cfspi_open,
+	.ndo_stop = cfspi_close,
+	.ndo_init = cfspi_init,
+	.ndo_uninit = cfspi_uninit,
+	.ndo_start_xmit = cfspi_xmit
+};
+
+static void cfspi_setup(struct net_device *dev)
+{
+	struct cfspi *cfspi = netdev_priv(dev);
+	dev->features = 0;
+	dev->netdev_ops = &cfspi_ops;
+	dev->type = ARPHRD_CAIF;
+	dev->flags = IFF_NOARP | IFF_POINTOPOINT;
+	dev->tx_queue_len = 0;
+	dev->mtu = SPI_MAX_PAYLOAD_SIZE;
+	dev->destructor = free_netdev;
+	skb_queue_head_init(&cfspi->qhead);
+	skb_queue_head_init(&cfspi->chead);
+	cfspi->cfdev.link_select = CAIF_LINK_HIGH_BANDW;
+	cfspi->cfdev.use_frag = false;
+	cfspi->cfdev.use_stx = false;
+	cfspi->cfdev.use_fcs = false;
+	cfspi->ndev = dev;
+}
+
+int cfspi_spi_probe(struct platform_device *pdev)
+{
+	struct cfspi *cfspi = NULL;
+	struct net_device *ndev;
+	struct cfspi_dev *dev;
+	int res;
+	dev = (struct cfspi_dev *)pdev->dev.platform_data;
+
+	ndev = alloc_netdev(sizeof(struct cfspi),
+			"cfspi%d", cfspi_setup);
+	if (!dev)
+		return -ENODEV;
+
+	cfspi = netdev_priv(ndev);
+	netif_stop_queue(ndev);
+	cfspi->ndev = ndev;
+	cfspi->pdev = pdev;
+
+	/* Assign the SPI device. */
+	cfspi->dev = dev;
+	/* Assign the device ifc to this SPI interface. */
+	dev->ifc = &cfspi->ifc;
+
 	/* Register network device. */
 	res = register_netdev(ndev);
 	if (res) {
@@ -728,15 +767,6 @@ int cfspi_spi_probe(struct platform_device *pdev)
 	return res;
 
  err_net_reg:
-	dev_debugfs_rem(cfspi);
-	set_bit(SPI_TERMINATE, &cfspi->state);
-	wake_up_interruptible(&cfspi->wait);
-	destroy_workqueue(cfspi->wq);
- err_create_wq:
-	dma_free(cfspi->xfer.va_rx, cfspi->xfer.pa_rx);
- err_dma_alloc_rx:
-	dma_free(cfspi->xfer.va_tx, cfspi->xfer.pa_tx);
- err_dma_alloc_tx:
 	free_netdev(ndev);
 
 	return res;
@@ -744,34 +774,8 @@ int cfspi_spi_probe(struct platform_device *pdev)
 
 int cfspi_spi_remove(struct platform_device *pdev)
 {
-	struct list_head *list_node;
-	struct list_head *n;
-	struct cfspi *cfspi = NULL;
-	struct cfspi_dev *dev;
-
-	dev = (struct cfspi_dev *)pdev->dev.platform_data;
-	spin_lock(&cfspi_list_lock);
-	list_for_each_safe(list_node, n, &cfspi_list) {
-		cfspi = list_entry(list_node, struct cfspi, list);
-		/* Find the corresponding device. */
-		if (cfspi->dev == dev) {
-			/* Remove from list. */
-			list_del(list_node);
-			/* Free DMA buffers. */
-			dma_free(cfspi->xfer.va_rx, cfspi->xfer.pa_rx);
-			dma_free(cfspi->xfer.va_tx, cfspi->xfer.pa_tx);
-			set_bit(SPI_TERMINATE, &cfspi->state);
-			wake_up_interruptible(&cfspi->wait);
-			destroy_workqueue(cfspi->wq);
-			/* Destroy debugfs directory and files. */
-			dev_debugfs_rem(cfspi);
-			unregister_netdev(cfspi->ndev);
-			spin_unlock(&cfspi_list_lock);
-			return 0;
-		}
-	}
-	spin_unlock(&cfspi_list_lock);
-	return -ENODEV;
+	/* Everything is done in cfspi_uninit(). */
+	return 0;
 }
 
 static void __exit cfspi_exit_module(void)
@@ -782,7 +786,7 @@ static void __exit cfspi_exit_module(void)
 
 	list_for_each_safe(list_node, n, &cfspi_list) {
 		cfspi = list_entry(list_node, struct cfspi, list);
-		platform_device_unregister(cfspi->pdev);
+		unregister_netdev(cfspi->ndev);
 	}
 
 	/* Destroy sysfs files. */
diff --git a/drivers/net/caif/caif_spi_slave.c b/drivers/net/caif/caif_spi_slave.c
index b009e03cda9..1ed2942952e 100644
--- a/drivers/net/caif/caif_spi_slave.c
+++ b/drivers/net/caif/caif_spi_slave.c
@@ -86,13 +86,13 @@ void cfspi_xfer(struct work_struct *work)
 
 #if CFSPI_DBG_PREFILL
 		/* Prefill buffers for easier debugging. */
-		memset(cfspi->xfer.va_tx, 0xFF, SPI_DMA_BUF_LEN);
+		memset(cfspi->xfer.va_tx[0], 0xFF, SPI_DMA_BUF_LEN);
 		memset(cfspi->xfer.va_rx, 0xFF, SPI_DMA_BUF_LEN);
 #endif	/* CFSPI_DBG_PREFILL */
 
 		cfspi_dbg_state(cfspi, CFSPI_STATE_AWAKE);
 
-	/* Check whether we have a committed frame. */
+		/* Check whether we have a committed frame. */
 		if (cfspi->tx_cpck_len) {
 			int len;
 
@@ -103,7 +103,7 @@ void cfspi_xfer(struct work_struct *work)
 			ptr += SPI_IND_SZ;
 			len = cfspi_xmitfrm(cfspi, ptr, cfspi->tx_cpck_len);
 			WARN_ON(len != cfspi->tx_cpck_len);
-	}
+		}
 
 		cfspi_dbg_state(cfspi, CFSPI_STATE_GET_NEXT);
 
@@ -116,7 +116,7 @@ void cfspi_xfer(struct work_struct *work)
 		 * Add indication and length at the beginning of the frame,
 		 * using little endian.
 		 */
-		ptr = (u8 *) cfspi->xfer.va_tx;
+		ptr = (u8 *) cfspi->xfer.va_tx[0];
 		*ptr++ = SPI_CMD_IND;
 		*ptr++ = (SPI_CMD_IND  & 0xFF00) >> 8;
 		*ptr++ = cfspi->tx_npck_len & 0x00FF;
diff --git a/drivers/net/smsc911x.c b/drivers/net/smsc911x.c
index c6d47d10590..d9a16744e2f 100644
--- a/drivers/net/smsc911x.c
+++ b/drivers/net/smsc911x.c
@@ -139,12 +139,15 @@ struct smsc911x_data {
 
 static inline u32 __smsc911x_reg_read(struct smsc911x_data *pdata, u32 reg)
 {
+	void __iomem *addr = pdata->ioaddr;
+	int shift = pdata->config.shift;
+
 	if (pdata->config.flags & SMSC911X_USE_32BIT)
-		return readl(pdata->ioaddr + reg);
+		return readl(addr + (reg << shift));
 
 	if (pdata->config.flags & SMSC911X_USE_16BIT)
-		return ((readw(pdata->ioaddr + reg) & 0xFFFF) |
-			((readw(pdata->ioaddr + reg + 2) & 0xFFFF) << 16));
+		return ((readw(addr + (reg << shift)) & 0xFFFF) |
+			((readw(addr + ((reg + 2) << shift)) & 0xFFFF) << 16));
 
 	BUG();
 	return 0;
@@ -181,14 +184,17 @@ static inline u32 smsc911x_reg_read(struct smsc911x_data *pdata, u32 reg)
 static inline void __smsc911x_reg_write(struct smsc911x_data *pdata, u32 reg,
 					u32 val)
 {
+	void __iomem *addr = pdata->ioaddr;
+	int shift = pdata->config.shift;
+
 	if (pdata->config.flags & SMSC911X_USE_32BIT) {
-		writel(val, pdata->ioaddr + reg);
+		writel(val, addr + (reg << shift));
 		return;
 	}
 
 	if (pdata->config.flags & SMSC911X_USE_16BIT) {
-		writew(val & 0xFFFF, pdata->ioaddr + reg);
-		writew((val >> 16) & 0xFFFF, pdata->ioaddr + reg + 2);
+		writew(val & 0xFFFF, addr + (reg << shift));
+		writew((val >> 16) & 0xFFFF, addr + ((reg + 2) << shift));
 		return;
 	}
 
@@ -241,7 +247,8 @@ smsc911x_tx_writefifo(struct smsc911x_data *pdata, unsigned int *buf,
 	}
 
 	if (pdata->config.flags & SMSC911X_USE_32BIT) {
-		writesl(pdata->ioaddr + TX_DATA_FIFO, buf, wordcount);
+		writesl(pdata->ioaddr + (TX_DATA_FIFO << pdata->config.shift),
+			buf, wordcount);
 		goto out;
 	}
 
@@ -307,7 +314,8 @@ smsc911x_rx_readfifo(struct smsc911x_data *pdata, unsigned int *buf,
 	}
 
 	if (pdata->config.flags & SMSC911X_USE_32BIT) {
-		readsl(pdata->ioaddr + RX_DATA_FIFO, buf, wordcount);
+		readsl(pdata->ioaddr + (RX_DATA_FIFO << pdata->config.shift),
+					buf, wordcount);
 		goto out;
 	}
 
@@ -2148,7 +2156,8 @@ static int __devinit smsc911x_drv_probe(struct platform_device *pdev)
 
 	dev->irq = irq_res->start;
 	irq_flags = irq_res->flags & IRQF_TRIGGER_MASK;
-	pdata->ioaddr = ioremap_nocache(res->start, res_size);
+	pdata->ioaddr = ioremap_nocache(res->start,
+					(res_size << config->shift));
 
 	/* copy config parameters across to pdata */
 	memcpy(&pdata->config, config, sizeof(pdata->config));
diff --git a/drivers/net/u8500_shrm.c b/drivers/net/u8500_shrm.c
new file mode 100644
index 00000000000..0e813bbb3cc
--- /dev/null
+++ b/drivers/net/u8500_shrm.c
@@ -0,0 +1,318 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2009
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/if_ether.h>
+#include <linux/netdevice.h>
+#include <linux/phonet.h>
+#include <linux/if_phonet.h>
+#include <linux/if_arp.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_config.h>
+#include <linux/modem/shrm/shrm_net.h>
+#include <linux/modem/shrm/shrm.h>
+#include <net/sock.h>
+#include <net/phonet/phonet.h>
+#include <net/phonet/pep.h>
+
+
+/**
+ * shrm_net_receive() - receive data and copy to user space buffer
+ * @dev:	pointer to the network device structure
+ *
+ * Copy data from ISI queue to the user space buffer.
+ */
+int shrm_net_receive(struct net_device *dev)
+{
+	struct sk_buff *skb;
+	struct isadev_context *isadev;
+	struct message_queue *q;
+	u32 msgsize;
+	u32 size = 0;
+	struct shrm_net_iface_priv *net_iface_priv =
+		(struct shrm_net_iface_priv *)netdev_priv(dev);
+	struct shrm_dev *shrm = net_iface_priv->shrm_device;
+
+	isadev = &shrm->isa_context->isadev[ISI_MESSAGING];
+	q = &isadev->dl_queue;
+
+	spin_lock_bh(&q->update_lock);
+	if (list_empty(&q->msg_list)) {
+		spin_unlock_bh(&q->update_lock);
+		dev_dbg(shrm->dev, "Empty Shrm queue\n");
+		return 0;
+	}
+	spin_unlock_bh(&q->update_lock);
+
+	msgsize = get_size_of_new_msg(q);
+	if (msgsize <= 0)
+		return msgsize;
+
+	/*
+	 * The packet has been retrieved from the transmission
+	 * medium. Build an skb around it, so upper layers can handle it
+	 */
+	skb = dev_alloc_skb(msgsize);
+	if (!skb) {
+		if (printk_ratelimit())
+			dev_notice(shrm->dev,
+			"isa rx: low on mem - packet dropped\n");
+		dev->stats.rx_dropped++;
+		goto out;
+	}
+
+	if ((q->readptr+msgsize) >= q->size) {
+		size = (q->size-q->readptr);
+		/*Copy First Part of msg*/
+		skb_copy_to_linear_data(skb,
+				(u8 *)(q->fifo_base + q->readptr), size);
+		skb_put(skb, size);
+
+		/*Copy Second Part of msg at the top of fifo*/
+		skb_copy_to_linear_data_offset(skb, size,
+				(u8 *)(q->fifo_base), (msgsize - size));
+		skb_put(skb, msgsize-size);
+
+	} else {
+		skb_copy_to_linear_data(skb,
+				(u8 *)(q->fifo_base+q->readptr), msgsize);
+		skb_put(skb, msgsize);
+	}
+
+	spin_lock_bh(&q->update_lock);
+	remove_msg_from_queue(q);
+	spin_unlock_bh(&q->update_lock);
+
+	skb_reset_mac_header(skb);
+	__skb_pull(skb, dev->hard_header_len);
+	/*Write metadata, and then pass to the receive level*/
+	skb->dev = dev;/*kmalloc(sizeof(struct net_device), GFP_ATOMIC);*/
+	skb->protocol = htons(ETH_P_PHONET);
+	skb->priority = 0;
+	skb->ip_summed = CHECKSUM_UNNECESSARY; /* don't check it */
+	if (likely(netif_rx_ni(skb) == NET_RX_SUCCESS)) {
+		dev->stats.rx_packets++;
+		dev->stats.rx_bytes += msgsize;
+	} else
+		dev->stats.rx_dropped++;
+
+	return msgsize;
+out:
+	return -ENOMEM;
+}
+
+static int netdev_isa_open(struct net_device *dev)
+{
+	struct shrm_net_iface_priv *net_iface_priv =
+			(struct shrm_net_iface_priv *)netdev_priv(dev);
+	struct shrm_dev *shrm = net_iface_priv->shrm_device;
+
+	shrm->netdev_flag_up = 1;
+	if (!netif_carrier_ok(dev))
+		netif_carrier_on(dev);
+	netif_wake_queue(dev);
+	return 0;
+}
+
+static int netdev_isa_close(struct net_device *dev)
+{
+	struct shrm_net_iface_priv *net_iface_priv =
+			(struct shrm_net_iface_priv *)netdev_priv(dev);
+	struct shrm_dev *shrm = net_iface_priv->shrm_device;
+
+	shrm->netdev_flag_up = 0;
+	netif_stop_queue(dev);
+	netif_carrier_off(dev);
+	return 0;
+}
+
+static int netdev_isa_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	struct if_phonet_req *req = (struct if_phonet_req *)ifr;
+
+	switch (cmd) {
+	case SIOCPNGAUTOCONF:
+		req->ifr_phonet_autoconf.device = PN_DEV_HOST;
+		return 0;
+	}
+	return -ENOIOCTLCMD;
+}
+
+static struct net_device_stats *netdev_isa_stats(struct net_device *dev)
+{
+	return &dev->stats;
+}
+
+/**
+ * netdev_isa_write() - write through the net interface
+ * @skb:	pointer to the socket buffer
+ * @dev:	pointer to the network device structure
+ *
+ * Copies data(ISI message) from the user buffer to the kernel buffer and
+ * schedule transfer thread to transmit the message to the modem via FIFO.
+ */
+static netdev_tx_t netdev_isa_write(struct sk_buff *skb, struct net_device *dev)
+{
+	int err;
+	int retval = 0;
+	struct shrm_net_iface_priv *net_iface_priv =
+			(struct shrm_net_iface_priv *)netdev_priv(dev);
+	struct shrm_dev *shrm = net_iface_priv->shrm_device;
+
+	/*
+	 * FIXME:
+	 * U8500 modem requires that Pipe created/enabled Indication should
+	 * be sent from the port corresponding to GPRS socket.
+	 * Also, the U8500 modem does not implement Pipe controller
+	 * which takes care of port manipulations for GPRS traffic.
+	 *
+	 * Now, APE has GPRS socket and the socket for sending
+	 * Indication msgs bound to different ports.
+	 * Phonet stack does not allow an indication msg to be sent
+	 * from GPRS socket, since Phonet stack assumes the presence
+	 * of Pipe controller in modem.
+	 *
+	 * So, due to lack of Pipe controller implementation in the
+	 * U8500 modem, carry out the port manipulation related to
+	 * GPRS traffic here.
+	 * Ideally, it should be done either by Pipe controller in
+	 * modem OR some implementation of Pipe controller on APE side
+	 */
+	if (skb->data[RESOURCE_ID_INDEX] == PN_PIPE) {
+		if ((skb->data[MSG_ID_INDEX] == PNS_PIPE_CREATED_IND) ||
+			(skb->data[MSG_ID_INDEX] == PNS_PIPE_ENABLED_IND) ||
+			(skb->data[MSG_ID_INDEX] == PNS_PIPE_DISABLED_IND))
+			skb->data[SRC_OBJ_INDEX] = skb->data[PIPE_HDL_INDEX];
+	}
+
+	spin_lock_bh(&shrm->isa_context->common_tx);
+	err = shm_write_msg(shrm, ISI_MESSAGING, skb->data,
+			skb->len);
+	if (!err) {
+		dev->stats.tx_packets++;
+		dev->stats.tx_bytes += skb->len;
+		retval = NETDEV_TX_OK;
+		dev_kfree_skb(skb);
+	} else {
+		dev->stats.tx_dropped++;
+		retval = NETDEV_TX_BUSY;
+	}
+	spin_unlock_bh(&shrm->isa_context->common_tx);
+
+	return retval;
+}
+
+static const struct net_device_ops shrm_netdev_ops = {
+	.ndo_open = netdev_isa_open,
+	.ndo_stop = netdev_isa_close,
+	.ndo_do_ioctl = netdev_isa_ioctl,
+	.ndo_start_xmit = netdev_isa_write,
+	.ndo_get_stats = netdev_isa_stats,
+};
+
+static void shm_net_init(struct net_device *dev)
+{
+	struct shrm_net_iface_priv *net_iface_priv;
+
+	dev->netdev_ops = &shrm_netdev_ops;
+	dev->header_ops = &phonet_header_ops;
+	dev->type = ARPHRD_PHONET;
+	dev->flags = IFF_POINTOPOINT | IFF_NOARP;
+	dev->mtu = PHONET_MAX_MTU;
+	dev->hard_header_len = SHRM_HLEN;
+	dev->addr_len = PHONET_ALEN;
+	dev->tx_queue_len = PN_TX_QUEUE_LEN;
+	dev->destructor = free_netdev;
+	dev->dev_addr[0] = PN_LINK_ADDR;
+	net_iface_priv = netdev_priv(dev);
+	memset(net_iface_priv, 0 , sizeof(struct shrm_net_iface_priv));
+}
+
+int shrm_register_netdev(struct shrm_dev *shrm)
+{
+	struct net_device *nw_device;
+	struct shrm_net_iface_priv *net_iface_priv;
+	char *devname = "shrm%d";
+	int err;
+
+	/* allocate the net device */
+	nw_device = shrm->ndev = alloc_netdev(
+			sizeof(struct shrm_net_iface_priv),
+			devname, shm_net_init);
+	if (nw_device == NULL) {
+		dev_err(shrm->dev, "Failed to allocate SHRM Netdev\n");
+		return -ENOMEM;
+	}
+	err = register_netdev(shrm->ndev);
+	if (err) {
+		dev_err(shrm->dev, "Err %i in reg shrm-netdev\n", err);
+		free_netdev(shrm->ndev);
+		return -ENODEV;
+	}
+	dev_info(shrm->dev, "Registered shrm netdev\n");
+
+	net_iface_priv = (struct shrm_net_iface_priv *)netdev_priv(nw_device);
+	net_iface_priv->shrm_device = shrm;
+	net_iface_priv->iface_num = 0;
+
+	return err;
+}
+
+int shrm_stop_netdev(struct net_device *dev)
+{
+	netif_stop_queue(dev);
+	return 0;
+}
+
+int shrm_restart_netdev(struct net_device *dev)
+{
+	if (netif_queue_stopped(dev))
+		netif_wake_queue(dev);
+	return 0;
+}
+
+int shrm_start_netdev(struct net_device *dev)
+{
+	struct shrm_net_iface_priv *net_iface_priv =
+			(struct shrm_net_iface_priv *)netdev_priv(dev);
+	struct shrm_dev *shrm = net_iface_priv->shrm_device;
+
+	if (!netif_carrier_ok(dev))
+		netif_carrier_on(dev);
+	netif_start_queue(dev);
+	shrm->netdev_flag_up = 1;
+	return 0;
+}
+
+int shrm_suspend_netdev(struct net_device *dev)
+{
+	if (netif_running(dev)) {
+		netif_stop_queue(dev);
+		netif_carrier_off(dev);
+	}
+	netif_device_detach(dev);
+
+	return 0;
+}
+
+int shrm_resume_netdev(struct net_device *dev)
+{
+	netif_device_attach(dev);
+	if (netif_running(dev)) {
+		netif_carrier_on(dev);
+		netif_wake_queue(dev);
+	}
+
+	return 0;
+}
+
+void shrm_unregister_netdev(struct shrm_dev *shrm)
+{
+	unregister_netdev(shrm->ndev);
+}
diff --git a/drivers/net/wireless/bcm4329/bcmspibrcm.c b/drivers/net/wireless/bcm4329/bcmspibrcm.c
new file mode 100644
index 00000000000..0f131a40f4b
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmspibrcm.c
@@ -0,0 +1,1726 @@
+/*
+ * Broadcom BCMSDH to gSPI Protocol Conversion Layer
+ *
+ * Copyright (C) 2010, Broadcom Corporation
+ * All Rights Reserved.
+ * 
+ * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Broadcom Corporation;
+ * the contents of this file may not be disclosed to third parties, copied
+ * or duplicated in any form, in whole or in part, without the prior
+ * written permission of Broadcom Corporation.
+ *
+ * $Id: bcmspibrcm.c,v 1.11.2.10.2.9.6.11 2009/05/21 13:21:57 Exp $
+ */
+
+#define HSMODE
+
+#include <typedefs.h>
+
+#include <bcmdevs.h>
+#include <bcmendian.h>
+#include <bcmutils.h>
+#include <osl.h>
+#include <hndsoc.h>
+#include <siutils.h>
+#include <sbchipc.h>
+#include <sbsdio.h>
+#include <spid.h>
+
+#include <bcmsdbus.h>	/* bcmsdh to/from specific controller APIs */
+#include <sdiovar.h>	/* ioctl/iovars */
+#include <sdio.h>
+
+#include <pcicfg.h>
+
+
+#include <bcmspibrcm.h>
+#include <bcmspi.h>
+
+#define F0_RESPONSE_DELAY	16
+#define F1_RESPONSE_DELAY	16
+#define F2_RESPONSE_DELAY	F0_RESPONSE_DELAY
+
+#define CMDLEN		4
+
+#define DWORDMODE_ON (sd->chip == BCM4329_CHIP_ID) && (sd->chiprev == 2) && (sd->dwordmode == TRUE)
+
+/* Globals */
+uint sd_msglevel = 0;
+
+uint sd_hiok = FALSE;		/* Use hi-speed mode if available? */
+uint sd_sdmode = SDIOH_MODE_SPI;		/* Use SD4 mode by default */
+uint sd_f2_blocksize = 64;		/* Default blocksize */
+
+
+uint sd_divisor = 2;
+uint sd_power = 1;		/* Default to SD Slot powered ON */
+uint sd_clock = 1;		/* Default to SD Clock turned ON */
+uint sd_crc = 0;		/* Default to SPI CRC Check turned OFF */
+
+uint8	spi_outbuf[SPI_MAX_PKT_LEN];
+uint8	spi_inbuf[SPI_MAX_PKT_LEN];
+
+/* 128bytes buffer is enough to clear data-not-available and program response-delay F0 bits
+ * assuming we will not exceed F0 response delay > 100 bytes at 48MHz.
+ */
+#define BUF2_PKT_LEN	128
+uint8	spi_outbuf2[BUF2_PKT_LEN];
+uint8	spi_inbuf2[BUF2_PKT_LEN];
+
+/* Prototypes */
+static bool bcmspi_test_card(sdioh_info_t *sd);
+static bool bcmspi_host_device_init_adapt(sdioh_info_t *sd);
+static int bcmspi_set_highspeed_mode(sdioh_info_t *sd, bool hsmode);
+static int bcmspi_cmd_issue(sdioh_info_t *sd, bool use_dma, uint32 cmd_arg,
+                           uint32 *data, uint32 datalen);
+static int bcmspi_card_regread(sdioh_info_t *sd, int func, uint32 regaddr,
+                              int regsize, uint32 *data);
+static int bcmspi_card_regwrite(sdioh_info_t *sd, int func, uint32 regaddr,
+                               int regsize, uint32 data);
+static int bcmspi_card_bytewrite(sdioh_info_t *sd, int func, uint32 regaddr,
+                               uint8 *data);
+static int bcmspi_driver_init(sdioh_info_t *sd);
+static int bcmspi_card_buf(sdioh_info_t *sd, int rw, int func, bool fifo,
+                          uint32 addr, int nbytes, uint32 *data);
+static int bcmspi_card_regread_fixedaddr(sdioh_info_t *sd, int func, uint32 regaddr, int regsize,
+                                 uint32 *data);
+static void bcmspi_cmd_getdstatus(sdioh_info_t *sd, uint32 *dstatus_buffer);
+static int bcmspi_update_stats(sdioh_info_t *sd, uint32 cmd_arg);
+
+/*
+ *  Public entry points & extern's
+ */
+extern sdioh_info_t *
+sdioh_attach(osl_t *osh, void *bar0, uint irq)
+{
+	sdioh_info_t *sd;
+
+	sd_trace(("%s\n", __FUNCTION__));
+	if ((sd = (sdioh_info_t *)MALLOC(osh, sizeof(sdioh_info_t))) == NULL) {
+		sd_err(("%s: out of memory, malloced %d bytes\n", __FUNCTION__, MALLOCED(osh)));
+		return NULL;
+	}
+	bzero((char *)sd, sizeof(sdioh_info_t));
+	sd->osh = osh;
+	if (spi_osinit(sd) != 0) {
+		sd_err(("%s: spi_osinit() failed\n", __FUNCTION__));
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return NULL;
+	}
+
+	sd->bar0 = bar0;
+	sd->irq = irq;
+	sd->intr_handler = NULL;
+	sd->intr_handler_arg = NULL;
+	sd->intr_handler_valid = FALSE;
+
+	/* Set defaults */
+	sd->use_client_ints = TRUE;
+	sd->sd_use_dma = FALSE;	/* DMA Not supported */
+
+	/* Spi device default is 16bit mode, change to 4 when device is changed to 32bit
+	 * mode
+	 */
+	sd->wordlen = 2;
+
+	if (!spi_hw_attach(sd)) {
+		sd_err(("%s: spi_hw_attach() failed\n", __FUNCTION__));
+		spi_osfree(sd);
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return (NULL);
+	}
+
+	if (bcmspi_driver_init(sd) != SUCCESS) {
+		sd_err(("%s: bcmspi_driver_init() failed()\n", __FUNCTION__));
+		spi_hw_detach(sd);
+		spi_osfree(sd);
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return (NULL);
+	}
+
+	if (spi_register_irq(sd, irq) != SUCCESS) {
+		sd_err(("%s: spi_register_irq() failed for irq = %d\n", __FUNCTION__, irq));
+		spi_hw_detach(sd);
+		spi_osfree(sd);
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return (NULL);
+	}
+
+	sd_trace(("%s: Done\n", __FUNCTION__));
+
+	return sd;
+}
+
+extern SDIOH_API_RC
+sdioh_detach(osl_t *osh, sdioh_info_t *sd)
+{
+	sd_trace(("%s\n", __FUNCTION__));
+	if (sd) {
+		sd_err(("%s: detaching from hardware\n", __FUNCTION__));
+		spi_free_irq(sd->irq, sd);
+		spi_hw_detach(sd);
+		spi_osfree(sd);
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+	}
+	return SDIOH_API_RC_SUCCESS;
+}
+
+/* Configure callback to client when we recieve client interrupt */
+extern SDIOH_API_RC
+sdioh_interrupt_register(sdioh_info_t *sd, sdioh_cb_fn_t fn, void *argh)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+	sd->intr_handler = fn;
+	sd->intr_handler_arg = argh;
+	sd->intr_handler_valid = TRUE;
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_interrupt_deregister(sdioh_info_t *sd)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+	sd->intr_handler_valid = FALSE;
+	sd->intr_handler = NULL;
+	sd->intr_handler_arg = NULL;
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_interrupt_query(sdioh_info_t *sd, bool *onoff)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+	*onoff = sd->client_intr_enabled;
+	return SDIOH_API_RC_SUCCESS;
+}
+
+#if defined(DHD_DEBUG)
+extern bool
+sdioh_interrupt_pending(sdioh_info_t *sd)
+{
+	return 0;
+}
+#endif
+
+extern SDIOH_API_RC
+sdioh_query_device(sdioh_info_t *sd)
+{
+	/* Return a BRCM ID appropriate to the dongle class */
+	return (sd->num_funcs > 1) ? BCM4329_D11NDUAL_ID : BCM4318_D11G_ID;
+}
+
+/* Provide dstatus bits of spi-transaction for dhd layers. */
+extern uint32
+sdioh_get_dstatus(sdioh_info_t *sd)
+{
+	return sd->card_dstatus;
+}
+
+extern void
+sdioh_chipinfo(sdioh_info_t *sd, uint32 chip, uint32 chiprev)
+{
+	sd->chip = chip;
+	sd->chiprev = chiprev;
+}
+
+extern void
+sdioh_dwordmode(sdioh_info_t *sd, bool set)
+{
+	uint8 reg = 0;
+	int status;
+
+	if ((status = sdioh_request_byte(sd, SDIOH_READ, SPI_FUNC_0, SPID_STATUS_ENABLE, &reg)) !=
+	     SUCCESS) {
+		sd_err(("%s: Failed to set dwordmode in gSPI\n", __FUNCTION__));
+		return;
+	}
+
+	if (set) {
+		reg |= DWORD_PKT_LEN_EN;
+		sd->dwordmode = TRUE;
+		sd->client_block_size[SPI_FUNC_2] = 4096; /* h2spi's limit is 4KB, we support 8KB */
+	} else {
+		reg &= ~DWORD_PKT_LEN_EN;
+		sd->dwordmode = FALSE;
+		sd->client_block_size[SPI_FUNC_2] = 2048;
+	}
+
+	if ((status = sdioh_request_byte(sd, SDIOH_WRITE, SPI_FUNC_0, SPID_STATUS_ENABLE, &reg)) !=
+	     SUCCESS) {
+		sd_err(("%s: Failed to set dwordmode in gSPI\n", __FUNCTION__));
+		return;
+	}
+}
+
+
+uint
+sdioh_query_iofnum(sdioh_info_t *sd)
+{
+	return sd->num_funcs;
+}
+
+/* IOVar table */
+enum {
+	IOV_MSGLEVEL = 1,
+	IOV_BLOCKMODE,
+	IOV_BLOCKSIZE,
+	IOV_DMA,
+	IOV_USEINTS,
+	IOV_NUMINTS,
+	IOV_NUMLOCALINTS,
+	IOV_HOSTREG,
+	IOV_DEVREG,
+	IOV_DIVISOR,
+	IOV_SDMODE,
+	IOV_HISPEED,
+	IOV_HCIREGS,
+	IOV_POWER,
+	IOV_CLOCK,
+	IOV_SPIERRSTATS,
+	IOV_RESP_DELAY_ALL
+};
+
+const bcm_iovar_t sdioh_iovars[] = {
+	{"sd_msglevel",	IOV_MSGLEVEL, 	0,	IOVT_UINT32,	0 },
+	{"sd_blocksize", IOV_BLOCKSIZE, 0,	IOVT_UINT32,	0 }, /* ((fn << 16) | size) */
+	{"sd_dma",	IOV_DMA,	0,	IOVT_BOOL,	0 },
+	{"sd_ints",	IOV_USEINTS,	0,	IOVT_BOOL,	0 },
+	{"sd_numints",	IOV_NUMINTS,	0,	IOVT_UINT32,	0 },
+	{"sd_numlocalints", IOV_NUMLOCALINTS, 0, IOVT_UINT32,	0 },
+	{"sd_hostreg",	IOV_HOSTREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
+	{"sd_devreg",	IOV_DEVREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t)	},
+	{"sd_divisor",	IOV_DIVISOR,	0,	IOVT_UINT32,	0 },
+	{"sd_power",	IOV_POWER,	0,	IOVT_UINT32,	0 },
+	{"sd_clock",	IOV_CLOCK,	0,	IOVT_UINT32,	0 },
+	{"sd_mode",	IOV_SDMODE,	0,	IOVT_UINT32,	100},
+	{"sd_highspeed",	IOV_HISPEED,	0,	IOVT_UINT32,	0},
+	{"spi_errstats", IOV_SPIERRSTATS, 0, IOVT_BUFFER, sizeof(struct spierrstats_t) },
+	{"spi_respdelay",	IOV_RESP_DELAY_ALL,	0,	IOVT_BOOL,	0 },
+	{NULL, 0, 0, 0, 0 }
+};
+
+int
+sdioh_iovar_op(sdioh_info_t *si, const char *name,
+               void *params, int plen, void *arg, int len, bool set)
+{
+	const bcm_iovar_t *vi = NULL;
+	int bcmerror = 0;
+	int val_size;
+	int32 int_val = 0;
+	bool bool_val;
+	uint32 actionid;
+/*
+	sdioh_regs_t *regs;
+*/
+
+	ASSERT(name);
+	ASSERT(len >= 0);
+
+	/* Get must have return space; Set does not take qualifiers */
+	ASSERT(set || (arg && len));
+	ASSERT(!set || (!params && !plen));
+
+	sd_trace(("%s: Enter (%s %s)\n", __FUNCTION__, (set ? "set" : "get"), name));
+
+	if ((vi = bcm_iovar_lookup(sdioh_iovars, name)) == NULL) {
+		bcmerror = BCME_UNSUPPORTED;
+		goto exit;
+	}
+
+	if ((bcmerror = bcm_iovar_lencheck(vi, arg, len, set)) != 0)
+		goto exit;
+
+	/* Set up params so get and set can share the convenience variables */
+	if (params == NULL) {
+		params = arg;
+		plen = len;
+	}
+
+	if (vi->type == IOVT_VOID)
+		val_size = 0;
+	else if (vi->type == IOVT_BUFFER)
+		val_size = len;
+	else
+		val_size = sizeof(int);
+
+	if (plen >= (int)sizeof(int_val))
+		bcopy(params, &int_val, sizeof(int_val));
+
+	bool_val = (int_val != 0) ? TRUE : FALSE;
+
+	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
+	switch (actionid) {
+	case IOV_GVAL(IOV_MSGLEVEL):
+		int_val = (int32)sd_msglevel;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_MSGLEVEL):
+		sd_msglevel = int_val;
+		break;
+
+	case IOV_GVAL(IOV_BLOCKSIZE):
+		if ((uint32)int_val > si->num_funcs) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+		int_val = (int32)si->client_block_size[int_val];
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_DMA):
+		int_val = (int32)si->sd_use_dma;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DMA):
+		si->sd_use_dma = (bool)int_val;
+		break;
+
+	case IOV_GVAL(IOV_USEINTS):
+		int_val = (int32)si->use_client_ints;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_USEINTS):
+		break;
+
+	case IOV_GVAL(IOV_DIVISOR):
+		int_val = (uint32)sd_divisor;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DIVISOR):
+		sd_divisor = int_val;
+		if (!spi_start_clock(si, (uint16)sd_divisor)) {
+			sd_err(("%s: set clock failed\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+		}
+		break;
+
+	case IOV_GVAL(IOV_POWER):
+		int_val = (uint32)sd_power;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_POWER):
+		sd_power = int_val;
+		break;
+
+	case IOV_GVAL(IOV_CLOCK):
+		int_val = (uint32)sd_clock;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_CLOCK):
+		sd_clock = int_val;
+		break;
+
+	case IOV_GVAL(IOV_SDMODE):
+		int_val = (uint32)sd_sdmode;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SDMODE):
+		sd_sdmode = int_val;
+		break;
+
+	case IOV_GVAL(IOV_HISPEED):
+		int_val = (uint32)sd_hiok;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_HISPEED):
+		sd_hiok = int_val;
+
+		if (!bcmspi_set_highspeed_mode(si, (bool)sd_hiok)) {
+			sd_err(("%s: Failed changing highspeed mode to %d.\n",
+			        __FUNCTION__, sd_hiok));
+			bcmerror = BCME_ERROR;
+			return ERROR;
+		}
+		break;
+
+	case IOV_GVAL(IOV_NUMINTS):
+		int_val = (int32)si->intrcount;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_NUMLOCALINTS):
+		int_val = (int32)si->local_intrcount;
+		bcopy(&int_val, arg, val_size);
+		break;
+	case IOV_GVAL(IOV_DEVREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+		uint8 data;
+
+		if (sdioh_cfg_read(si, sd_ptr->func, sd_ptr->offset, &data)) {
+			bcmerror = BCME_SDIO_ERROR;
+			break;
+		}
+
+		int_val = (int)data;
+		bcopy(&int_val, arg, sizeof(int_val));
+		break;
+	}
+
+	case IOV_SVAL(IOV_DEVREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+		uint8 data = (uint8)sd_ptr->value;
+
+		if (sdioh_cfg_write(si, sd_ptr->func, sd_ptr->offset, &data)) {
+			bcmerror = BCME_SDIO_ERROR;
+			break;
+		}
+		break;
+	}
+
+
+	case IOV_GVAL(IOV_SPIERRSTATS):
+	{
+		bcopy(&si->spierrstats, arg, sizeof(struct spierrstats_t));
+		break;
+	}
+
+	case IOV_SVAL(IOV_SPIERRSTATS):
+	{
+		bzero(&si->spierrstats, sizeof(struct spierrstats_t));
+		break;
+	}
+
+	case IOV_GVAL(IOV_RESP_DELAY_ALL):
+		int_val = (int32)si->resp_delay_all;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_RESP_DELAY_ALL):
+		si->resp_delay_all = (bool)int_val;
+		int_val = STATUS_ENABLE|INTR_WITH_STATUS;
+		if (si->resp_delay_all)
+			int_val |= RESP_DELAY_ALL;
+		else {
+			if (bcmspi_card_regwrite(si, SPI_FUNC_0, SPID_RESPONSE_DELAY, 1,
+			     F1_RESPONSE_DELAY) != SUCCESS) {
+				sd_err(("%s: Unable to set response delay.\n", __FUNCTION__));
+				bcmerror = BCME_SDIO_ERROR;
+				break;
+			}
+		}
+
+		if (bcmspi_card_regwrite(si, SPI_FUNC_0, SPID_STATUS_ENABLE, 1, int_val)
+		     != SUCCESS) {
+			sd_err(("%s: Unable to set response delay.\n", __FUNCTION__));
+			bcmerror = BCME_SDIO_ERROR;
+			break;
+		}
+		break;
+
+	default:
+		bcmerror = BCME_UNSUPPORTED;
+		break;
+	}
+exit:
+
+	return bcmerror;
+}
+
+extern SDIOH_API_RC
+sdioh_cfg_read(sdioh_info_t *sd, uint fnc_num, uint32 addr, uint8 *data)
+{
+	SDIOH_API_RC status;
+	/* No lock needed since sdioh_request_byte does locking */
+	status = sdioh_request_byte(sd, SDIOH_READ, fnc_num, addr, data);
+	return status;
+}
+
+extern SDIOH_API_RC
+sdioh_cfg_write(sdioh_info_t *sd, uint fnc_num, uint32 addr, uint8 *data)
+{
+	/* No lock needed since sdioh_request_byte does locking */
+	SDIOH_API_RC status;
+
+	if ((fnc_num == SPI_FUNC_1) && (addr == SBSDIO_FUNC1_FRAMECTRL)) {
+		uint8 dummy_data;
+		status = sdioh_cfg_read(sd, fnc_num, addr, &dummy_data);
+		if (status) {
+			sd_err(("sdioh_cfg_read() failed.\n"));
+			return status;
+		}
+	}
+
+	status = sdioh_request_byte(sd, SDIOH_WRITE, fnc_num, addr, data);
+	return status;
+}
+
+extern SDIOH_API_RC
+sdioh_cis_read(sdioh_info_t *sd, uint func, uint8 *cisd, uint32 length)
+{
+	uint32 count;
+	int offset;
+	uint32 cis_byte;
+	uint16 *cis = (uint16 *)cisd;
+	uint bar0 = SI_ENUM_BASE;
+	int status;
+	uint8 data;
+
+	sd_trace(("%s: Func %d\n", __FUNCTION__, func));
+
+	spi_lock(sd);
+
+	/* Set sb window address to 0x18000000 */
+	data = (bar0 >> 8) & SBSDIO_SBADDRLOW_MASK;
+	status = bcmspi_card_bytewrite(sd, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW, &data);
+	if (status == SUCCESS) {
+		data = (bar0 >> 16) & SBSDIO_SBADDRMID_MASK;
+		status = bcmspi_card_bytewrite(sd, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRMID, &data);
+	} else {
+		sd_err(("%s: Unable to set sb-addr-windows\n", __FUNCTION__));
+		spi_unlock(sd);
+		return (BCME_ERROR);
+	}
+	if (status == SUCCESS) {
+		data = (bar0 >> 24) & SBSDIO_SBADDRHIGH_MASK;
+		status = bcmspi_card_bytewrite(sd, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRHIGH, &data);
+	} else {
+		sd_err(("%s: Unable to set sb-addr-windows\n", __FUNCTION__));
+		spi_unlock(sd);
+		return (BCME_ERROR);
+	}
+
+	offset =  CC_OTP; /* OTP offset in chipcommon. */
+	for (count = 0; count < length/2; count++) {
+		if (bcmspi_card_regread (sd, SDIO_FUNC_1, offset, 2, &cis_byte) < 0) {
+			sd_err(("%s: regread failed: Can't read CIS\n", __FUNCTION__));
+			spi_unlock(sd);
+			return (BCME_ERROR);
+		}
+
+		*cis = (uint16)cis_byte;
+		cis++;
+		offset += 2;
+	}
+
+	spi_unlock(sd);
+
+	return (BCME_OK);
+}
+
+extern SDIOH_API_RC
+sdioh_request_byte(sdioh_info_t *sd, uint rw, uint func, uint regaddr, uint8 *byte)
+{
+	int status;
+	uint32 cmd_arg;
+	uint32 dstatus;
+	uint32 data = (uint32)(*byte);
+
+	spi_lock(sd);
+
+	cmd_arg = 0;
+	cmd_arg = SFIELD(cmd_arg, SPI_FUNCTION, func);
+	cmd_arg = SFIELD(cmd_arg, SPI_ACCESS, 1);	/* Incremental access */
+	cmd_arg = SFIELD(cmd_arg, SPI_REG_ADDR, regaddr);
+	cmd_arg = SFIELD(cmd_arg, SPI_RW_FLAG, rw == SDIOH_READ ? 0 : 1);
+	cmd_arg = SFIELD(cmd_arg, SPI_LEN, 1);
+
+	sd_trace(("%s cmd_arg = 0x%x\n", __FUNCTION__, cmd_arg));
+	sd_trace(("%s: rw=%d, func=%d, regaddr=0x%08x, data=0x%x\n", __FUNCTION__, rw, func,
+	         regaddr, data));
+
+	if ((status = bcmspi_cmd_issue(sd, sd->sd_use_dma,
+	                              cmd_arg, &data, 1)) != SUCCESS) {
+		spi_unlock(sd);
+		return status;
+	}
+
+	if (rw == SDIOH_READ)
+		*byte = (uint8)data;
+
+	bcmspi_cmd_getdstatus(sd, &dstatus);
+	if (dstatus)
+		sd_trace(("dstatus =0x%x\n", dstatus));
+
+	spi_unlock(sd);
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_request_word(sdioh_info_t *sd, uint cmd_type, uint rw, uint func, uint addr,
+                   uint32 *word, uint nbytes)
+{
+	int status;
+
+	spi_lock(sd);
+
+	if (rw == SDIOH_READ)
+		status = bcmspi_card_regread(sd, func, addr, nbytes, word);
+	else
+		status = bcmspi_card_regwrite(sd, func, addr, nbytes, *word);
+
+	spi_unlock(sd);
+	return (status == SUCCESS ?  SDIOH_API_RC_SUCCESS : SDIOH_API_RC_FAIL);
+}
+
+extern SDIOH_API_RC
+sdioh_request_buffer(sdioh_info_t *sd, uint pio_dma, uint fix_inc, uint rw, uint func,
+                     uint addr, uint reg_width, uint buflen_u, uint8 *buffer, void *pkt)
+{
+	int len;
+	int buflen = (int)buflen_u;
+	bool fifo = (fix_inc == SDIOH_DATA_FIX);
+
+	spi_lock(sd);
+
+	ASSERT(reg_width == 4);
+	ASSERT(buflen_u < (1 << 30));
+	ASSERT(sd->client_block_size[func]);
+
+	sd_data(("%s: %c len %d r_cnt %d t_cnt %d, pkt @0x%p\n",
+	         __FUNCTION__, rw == SDIOH_READ ? 'R' : 'W',
+	         buflen_u, sd->r_cnt, sd->t_cnt, pkt));
+
+	/* Break buffer down into blocksize chunks. */
+	while (buflen > 0) {
+		len = MIN(sd->client_block_size[func], buflen);
+		if (bcmspi_card_buf(sd, rw, func, fifo, addr, len, (uint32 *)buffer) != SUCCESS) {
+			sd_err(("%s: bcmspi_card_buf %s failed\n",
+				__FUNCTION__, rw == SDIOH_READ ? "Read" : "Write"));
+			spi_unlock(sd);
+			return SDIOH_API_RC_FAIL;
+		}
+		buffer += len;
+		buflen -= len;
+		if (!fifo)
+			addr += len;
+	}
+	spi_unlock(sd);
+	return SDIOH_API_RC_SUCCESS;
+}
+
+/* This function allows write to gspi bus when another rd/wr function is deep down the call stack.
+ * Its main aim is to have simpler spi writes rather than recursive writes.
+ * e.g. When there is a need to program response delay on the fly after detecting the SPI-func
+ * this call will allow to program the response delay.
+ */
+static int
+bcmspi_card_byterewrite(sdioh_info_t *sd, int func, uint32 regaddr, uint8 byte)
+{
+	uint32 cmd_arg;
+	uint32 datalen = 1;
+	uint32 hostlen;
+
+	cmd_arg = 0;
+
+	cmd_arg = SFIELD(cmd_arg, SPI_RW_FLAG, 1);
+	cmd_arg = SFIELD(cmd_arg, SPI_ACCESS, 1);	/* Incremental access */
+	cmd_arg = SFIELD(cmd_arg, SPI_FUNCTION, func);
+	cmd_arg = SFIELD(cmd_arg, SPI_REG_ADDR, regaddr);
+	cmd_arg = SFIELD(cmd_arg, SPI_LEN, datalen);
+
+	sd_trace(("%s cmd_arg = 0x%x\n", __FUNCTION__, cmd_arg));
+
+
+	/* Set up and issue the SPI command.  MSByte goes out on bus first.  Increase datalen
+	 * according to the wordlen mode(16/32bit) the device is in.
+	 */
+	ASSERT(sd->wordlen == 4 || sd->wordlen == 2);
+	datalen = ROUNDUP(datalen, sd->wordlen);
+
+	/* Start by copying command in the spi-outbuffer */
+	if (sd->wordlen == 4) { /* 32bit spid */
+		*(uint32 *)spi_outbuf2 = bcmswap32(cmd_arg);
+		if (datalen & 0x3)
+			datalen += (4 - (datalen & 0x3));
+	} else if (sd->wordlen == 2) { /* 16bit spid */
+		*(uint16 *)spi_outbuf2 = bcmswap16(cmd_arg & 0xffff);
+		*(uint16 *)&spi_outbuf2[2] = bcmswap16((cmd_arg & 0xffff0000) >> 16);
+		if (datalen & 0x1)
+			datalen++;
+	} else {
+		sd_err(("%s: Host is %d bit spid, could not create SPI command.\n",
+		        __FUNCTION__, 8 * sd->wordlen));
+		return ERROR;
+	}
+
+	/* for Write, put the data into the output buffer  */
+	if (datalen != 0) {
+			if (sd->wordlen == 4) { /* 32bit spid */
+				*(uint32 *)&spi_outbuf2[CMDLEN] = bcmswap32(byte);
+			} else if (sd->wordlen == 2) { /* 16bit spid */
+				*(uint16 *)&spi_outbuf2[CMDLEN] = bcmswap16(byte & 0xffff);
+				*(uint16 *)&spi_outbuf2[CMDLEN + 2] =
+					bcmswap16((byte & 0xffff0000) >> 16);
+			}
+	}
+
+	/* +4 for cmd, +4 for dstatus */
+	hostlen = datalen + 8;
+	hostlen += (4 - (hostlen & 0x3));
+	spi_sendrecv(sd, spi_outbuf2, spi_inbuf2, hostlen);
+
+	/* Last 4bytes are dstatus.  Device is configured to return status bits. */
+	if (sd->wordlen == 4) { /* 32bit spid */
+		sd->card_dstatus = bcmswap32(*(uint32 *)&spi_inbuf2[datalen + CMDLEN ]);
+	} else if (sd->wordlen == 2) { /* 16bit spid */
+		sd->card_dstatus = (bcmswap16(*(uint16 *)&spi_inbuf2[datalen + CMDLEN ]) |
+		                   (bcmswap16(*(uint16 *)&spi_inbuf2[datalen + CMDLEN + 2]) << 16));
+	} else {
+		sd_err(("%s: Host is %d bit machine, could not read SPI dstatus.\n",
+		        __FUNCTION__, 8 * sd->wordlen));
+		return ERROR;
+	}
+
+	if (sd->card_dstatus)
+		sd_trace(("dstatus after byte rewrite = 0x%x\n", sd->card_dstatus));
+
+	return (BCME_OK);
+}
+
+/* Program the response delay corresponding to the spi function */
+static int
+bcmspi_prog_resp_delay(sdioh_info_t *sd, int func, uint8 resp_delay)
+{
+	if (sd->resp_delay_all == FALSE)
+		return (BCME_OK);
+
+	if (sd->prev_fun == func)
+		return (BCME_OK);
+
+	if (F0_RESPONSE_DELAY == F1_RESPONSE_DELAY)
+		return (BCME_OK);
+
+	bcmspi_card_byterewrite(sd, SPI_FUNC_0, SPID_RESPONSE_DELAY, resp_delay);
+
+	/* Remember function for which to avoid reprogramming resp-delay in next iteration */
+	sd->prev_fun = func;
+
+	return (BCME_OK);
+
+}
+
+#define GSPI_RESYNC_PATTERN	0x0
+
+/* A resync pattern is a 32bit MOSI line with all zeros. Its a special command in gSPI.
+ * It resets the spi-bkplane logic so that all F1 related ping-pong buffer logic is
+ * synchronised and all queued resuests are cancelled.
+ */
+static int
+bcmspi_resync_f1(sdioh_info_t *sd)
+{
+	uint32 cmd_arg = GSPI_RESYNC_PATTERN, data = 0, datalen = 0;
+
+
+	/* Set up and issue the SPI command.  MSByte goes out on bus first.  Increase datalen
+	 * according to the wordlen mode(16/32bit) the device is in.
+	 */
+	ASSERT(sd->wordlen == 4 || sd->wordlen == 2);
+	datalen = ROUNDUP(datalen, sd->wordlen);
+
+	/* Start by copying command in the spi-outbuffer */
+	*(uint32 *)spi_outbuf2 = cmd_arg;
+
+	/* for Write, put the data into the output buffer  */
+	*(uint32 *)&spi_outbuf2[CMDLEN] = data;
+
+	/* +4 for cmd, +4 for dstatus */
+	spi_sendrecv(sd, spi_outbuf2, spi_inbuf2, datalen + 8);
+
+	/* Last 4bytes are dstatus.  Device is configured to return status bits. */
+	if (sd->wordlen == 4) { /* 32bit spid */
+		sd->card_dstatus = bcmswap32(*(uint32 *)&spi_inbuf2[datalen + CMDLEN ]);
+	} else if (sd->wordlen == 2) { /* 16bit spid */
+		sd->card_dstatus = (bcmswap16(*(uint16 *)&spi_inbuf2[datalen + CMDLEN ]) |
+		                   (bcmswap16(*(uint16 *)&spi_inbuf2[datalen + CMDLEN + 2]) << 16));
+	} else {
+		sd_err(("%s: Host is %d bit machine, could not read SPI dstatus.\n",
+		        __FUNCTION__, 8 * sd->wordlen));
+		return ERROR;
+	}
+
+	if (sd->card_dstatus)
+		sd_trace(("dstatus after resync pattern write = 0x%x\n", sd->card_dstatus));
+
+	return (BCME_OK);
+}
+
+uint32 dstatus_count = 0;
+
+static int
+bcmspi_update_stats(sdioh_info_t *sd, uint32 cmd_arg)
+{
+	uint32 dstatus = sd->card_dstatus;
+	struct spierrstats_t *spierrstats = &sd->spierrstats;
+	int err = SUCCESS;
+
+	sd_trace(("cmd = 0x%x, dstatus = 0x%x\n", cmd_arg, dstatus));
+
+	/* Store dstatus of last few gSPI transactions */
+	spierrstats->dstatus[dstatus_count % NUM_PREV_TRANSACTIONS] = dstatus;
+	spierrstats->spicmd[dstatus_count % NUM_PREV_TRANSACTIONS] = cmd_arg;
+	dstatus_count++;
+
+	if (sd->card_init_done == FALSE)
+		return err;
+
+	if (dstatus & STATUS_DATA_NOT_AVAILABLE) {
+		spierrstats->dna++;
+		sd_trace(("Read data not available on F1 addr = 0x%x\n",
+		        GFIELD(cmd_arg, SPI_REG_ADDR)));
+		/* Clear dna bit */
+		bcmspi_card_byterewrite(sd, SPI_FUNC_0, SPID_INTR_REG, DATA_UNAVAILABLE);
+	}
+
+	if (dstatus & STATUS_UNDERFLOW) {
+		spierrstats->rdunderflow++;
+		sd_err(("FIFO underflow happened due to current F2 read command.\n"));
+	}
+
+	if (dstatus & STATUS_OVERFLOW) {
+		spierrstats->wroverflow++;
+		sd_err(("FIFO overflow happened due to current (F1/F2) write command.\n"));
+		if ((sd->chip == BCM4329_CHIP_ID) && (sd->chiprev == 0)) {
+			bcmspi_card_byterewrite(sd, SPI_FUNC_0, SPID_INTR_REG, F1_OVERFLOW);
+			bcmspi_resync_f1(sd);
+			sd_err(("Recovering from F1 FIFO overflow.\n"));
+		} else {
+			err = ERROR_OF;
+		}
+	}
+
+	if (dstatus & STATUS_F2_INTR) {
+		spierrstats->f2interrupt++;
+		sd_trace(("Interrupt from F2.  SW should clear corresponding IntStatus bits\n"));
+	}
+
+	if (dstatus & STATUS_F3_INTR) {
+		spierrstats->f3interrupt++;
+		sd_err(("Interrupt from F3.  SW should clear corresponding IntStatus bits\n"));
+	}
+
+	if (dstatus & STATUS_HOST_CMD_DATA_ERR) {
+		spierrstats->hostcmddataerr++;
+		sd_err(("Error in CMD or Host data, detected by CRC/Checksum (optional)\n"));
+	}
+
+	if (dstatus & STATUS_F2_PKT_AVAILABLE) {
+		spierrstats->f2pktavailable++;
+		sd_trace(("Packet is available/ready in F2 TX FIFO\n"));
+		sd_trace(("Packet length = %d\n", sd->dwordmode ?
+		         ((dstatus & STATUS_F2_PKT_LEN_MASK) >> (STATUS_F2_PKT_LEN_SHIFT - 2)) :
+		         ((dstatus & STATUS_F2_PKT_LEN_MASK) >> STATUS_F2_PKT_LEN_SHIFT)));
+	}
+
+	if (dstatus & STATUS_F3_PKT_AVAILABLE) {
+		spierrstats->f3pktavailable++;
+		sd_err(("Packet is available/ready in F3 TX FIFO\n"));
+		sd_err(("Packet length = %d\n",
+		        (dstatus & STATUS_F3_PKT_LEN_MASK) >> STATUS_F3_PKT_LEN_SHIFT));
+	}
+
+	return err;
+}
+
+extern int
+sdioh_abort(sdioh_info_t *sd, uint func)
+{
+	return 0;
+}
+
+int
+sdioh_start(sdioh_info_t *sd, int stage)
+{
+	return SUCCESS;
+}
+
+int
+sdioh_stop(sdioh_info_t *sd)
+{
+	return SUCCESS;
+}
+
+
+
+/*
+ * Private/Static work routines
+ */
+static int
+bcmspi_host_init(sdioh_info_t *sd)
+{
+
+	/* Default power on mode */
+	sd->sd_mode = SDIOH_MODE_SPI;
+	sd->polled_mode = TRUE;
+	sd->host_init_done = TRUE;
+	sd->card_init_done = FALSE;
+	sd->adapter_slot = 1;
+
+	return (SUCCESS);
+}
+
+static int
+get_client_blocksize(sdioh_info_t *sd)
+{
+	uint32 regdata[2];
+	int status;
+
+	/* Find F1/F2/F3 max packet size */
+	if ((status = bcmspi_card_regread(sd, 0, SPID_F1_INFO_REG,
+	                                 8, regdata)) != SUCCESS) {
+		return status;
+	}
+
+	sd_trace(("pkt_size regdata[0] = 0x%x, regdata[1] = 0x%x\n",
+	        regdata[0], regdata[1]));
+
+	sd->client_block_size[1] = (regdata[0] & F1_MAX_PKT_SIZE) >> 2;
+	sd_trace(("Func1 blocksize = %d\n", sd->client_block_size[1]));
+	ASSERT(sd->client_block_size[1] == BLOCK_SIZE_F1);
+
+	sd->client_block_size[2] = ((regdata[0] >> 16) & F2_MAX_PKT_SIZE) >> 2;
+	sd_trace(("Func2 blocksize = %d\n", sd->client_block_size[2]));
+	ASSERT(sd->client_block_size[2] == BLOCK_SIZE_F2);
+
+	sd->client_block_size[3] = (regdata[1] & F3_MAX_PKT_SIZE) >> 2;
+	sd_trace(("Func3 blocksize = %d\n", sd->client_block_size[3]));
+	ASSERT(sd->client_block_size[3] == BLOCK_SIZE_F3);
+
+	return 0;
+}
+
+static int
+bcmspi_client_init(sdioh_info_t *sd)
+{
+	uint32	status_en_reg = 0;
+	sd_trace(("%s: Powering up slot %d\n", __FUNCTION__, sd->adapter_slot));
+
+#ifdef HSMODE
+	if (!spi_start_clock(sd, (uint16)sd_divisor)) {
+		sd_err(("spi_start_clock failed\n"));
+		return ERROR;
+	}
+#else
+	/* Start at ~400KHz clock rate for initialization */
+	if (!spi_start_clock(sd, 128)) {
+		sd_err(("spi_start_clock failed\n"));
+		return ERROR;
+	}
+#endif /* HSMODE */
+
+	if (!bcmspi_host_device_init_adapt(sd)) {
+		sd_err(("bcmspi_host_device_init_adapt failed\n"));
+		return ERROR;
+	}
+
+	if (!bcmspi_test_card(sd)) {
+		sd_err(("bcmspi_test_card failed\n"));
+		return ERROR;
+	}
+
+	sd->num_funcs = SPI_MAX_IOFUNCS;
+
+	get_client_blocksize(sd);
+
+	/* Apply resync pattern cmd with all zeros to reset spi-bkplane F1 logic */
+	bcmspi_resync_f1(sd);
+
+	sd->dwordmode = FALSE;
+
+	bcmspi_card_regread(sd, 0, SPID_STATUS_ENABLE, 1, &status_en_reg);
+
+	sd_trace(("%s: Enabling interrupt with dstatus \n", __FUNCTION__));
+	status_en_reg |= INTR_WITH_STATUS;
+
+
+	if (bcmspi_card_regwrite(sd, SPI_FUNC_0, SPID_STATUS_ENABLE, 1,
+	    status_en_reg & 0xff) != SUCCESS) {
+		sd_err(("%s: Unable to set response delay for all fun's.\n", __FUNCTION__));
+		return ERROR;
+	}
+
+
+#ifndef HSMODE
+	/* After configuring for High-Speed mode, set the desired clock rate. */
+	if (!spi_start_clock(sd, 4)) {
+		sd_err(("spi_start_clock failed\n"));
+		return ERROR;
+	}
+#endif /* HSMODE */
+
+	sd->card_init_done = TRUE;
+
+
+	return SUCCESS;
+}
+
+static int
+bcmspi_set_highspeed_mode(sdioh_info_t *sd, bool hsmode)
+{
+	uint32 regdata;
+	int status;
+
+	if ((status = bcmspi_card_regread(sd, 0, SPID_CONFIG,
+	                                 4, &regdata)) != SUCCESS)
+		return status;
+
+	sd_trace(("In %s spih-ctrl = 0x%x \n", __FUNCTION__, regdata));
+
+
+	if (hsmode == TRUE) {
+		sd_trace(("Attempting to enable High-Speed mode.\n"));
+
+		if (regdata & HIGH_SPEED_MODE) {
+			sd_trace(("Device is already in High-Speed mode.\n"));
+			return status;
+		} else {
+			regdata |= HIGH_SPEED_MODE;
+			sd_trace(("Writing %08x to device at %08x\n", regdata, SPID_CONFIG));
+			if ((status = bcmspi_card_regwrite(sd, 0, SPID_CONFIG,
+			                                  4, regdata)) != SUCCESS) {
+				return status;
+			}
+		}
+	} else {
+		sd_trace(("Attempting to disable High-Speed mode.\n"));
+
+		if (regdata & HIGH_SPEED_MODE) {
+			regdata &= ~HIGH_SPEED_MODE;
+			sd_trace(("Writing %08x to device at %08x\n", regdata, SPID_CONFIG));
+			if ((status = bcmspi_card_regwrite(sd, 0, SPID_CONFIG,
+			                                  4, regdata)) != SUCCESS)
+				return status;
+		}
+		 else {
+			sd_trace(("Device is already in Low-Speed mode.\n"));
+			return status;
+		}
+	}
+
+	spi_controller_highspeed_mode(sd, hsmode);
+
+	return TRUE;
+}
+
+#define bcmspi_find_curr_mode(sd) { \
+	sd->wordlen = 2; \
+	status = bcmspi_card_regread_fixedaddr(sd, 0, SPID_TEST_READ, 4, &regdata); \
+	regdata &= 0xff; \
+	if ((regdata == 0xad) || (regdata == 0x5b) || \
+	    (regdata == 0x5d) || (regdata == 0x5a)) \
+		break; \
+	sd->wordlen = 4; \
+	status = bcmspi_card_regread_fixedaddr(sd, 0, SPID_TEST_READ, 4, &regdata); \
+	regdata &= 0xff; \
+	if ((regdata == 0xad) || (regdata == 0x5b) || \
+	    (regdata == 0x5d) || (regdata == 0x5a)) \
+		break; \
+	sd_trace(("Silicon testability issue: regdata = 0x%x." \
+	          " Expected 0xad, 0x5a, 0x5b or 0x5d.\n", regdata));	\
+	OSL_DELAY(100000); \
+}
+
+#define INIT_ADAPT_LOOP		100
+
+/* Adapt clock-phase-speed-bitwidth between host and device */
+static bool
+bcmspi_host_device_init_adapt(sdioh_info_t *sd)
+{
+	uint32 wrregdata, regdata = 0;
+	int status;
+	int i;
+
+	/* Due to a silicon testability issue, the first command from the Host
+	 * to the device will get corrupted (first bit will be lost). So the
+	 * Host should poll the device with a safe read request. ie: The Host
+	 * should try to read F0 addr 0x14 using the Fixed address mode
+	 * (This will prevent a unintended write command to be detected by device)
+	 */
+	for (i = 0; i < INIT_ADAPT_LOOP; i++) {
+		/* If device was not power-cycled it will stay in 32bit mode with
+		 * response-delay-all bit set.  Alternate the iteration so that
+		 * read either with or without response-delay for F0 to succeed.
+		 */
+		bcmspi_find_curr_mode(sd);
+		sd->resp_delay_all = (i & 0x1) ? TRUE : FALSE;
+
+		bcmspi_find_curr_mode(sd);
+		sd->dwordmode = TRUE;
+
+		bcmspi_find_curr_mode(sd);
+		sd->dwordmode = FALSE;
+	}
+
+	/* Bail out, device not detected */
+	if (i == INIT_ADAPT_LOOP)
+		return FALSE;
+
+	/* Softreset the spid logic */
+	if ((sd->dwordmode) || (sd->wordlen == 4)) {
+		bcmspi_card_regwrite(sd, 0, SPID_RESET_BP, 1, RESET_ON_WLAN_BP_RESET|RESET_SPI);
+		bcmspi_card_regread(sd, 0, SPID_RESET_BP, 1, &regdata);
+		sd_trace(("reset reg read = 0x%x\n", regdata));
+		sd_trace(("dwordmode = %d, wordlen = %d, resp_delay_all = %d\n", sd->dwordmode,
+		       sd->wordlen, sd->resp_delay_all));
+		/* Restore default state after softreset */
+		sd->wordlen = 2;
+		sd->dwordmode = FALSE;
+	}
+
+	if (sd->wordlen == 4) {
+		if ((status = bcmspi_card_regread(sd, 0, SPID_TEST_READ, 4, &regdata)) !=
+		     SUCCESS)
+				return FALSE;
+		if (regdata == TEST_RO_DATA_32BIT_LE) {
+			sd_trace(("Spid is already in 32bit LE mode. Value read = 0x%x\n",
+			          regdata));
+			sd_trace(("Spid power was left on.\n"));
+		} else {
+			sd_err(("Spid power was left on but signature read failed."
+			        " Value read = 0x%x\n", regdata));
+			return FALSE;
+		}
+	} else {
+		sd->wordlen = 2;
+
+#define CTRL_REG_DEFAULT	0x00010430 /* according to the host m/c */
+
+		wrregdata = (CTRL_REG_DEFAULT);
+		sd->resp_delay_all = TRUE;
+		if (sd->resp_delay_all == TRUE) {
+			/* Enable response delay for all */
+			wrregdata |= (RESP_DELAY_ALL << 16);
+			/* Program response delay value */
+			wrregdata &= 0xffff00ff;
+			wrregdata |= (F1_RESPONSE_DELAY << 8);
+			sd->prev_fun = SPI_FUNC_1;
+			bcmspi_card_regwrite(sd, 0, SPID_CONFIG, 4, wrregdata);
+		}
+
+		if ((status = bcmspi_card_regread(sd, 0, SPID_TEST_READ, 4, &regdata)) != SUCCESS)
+			return FALSE;
+		sd_trace(("(we are still in 16bit mode) 32bit READ LE regdata = 0x%x\n", regdata));
+
+#ifndef HSMODE
+		wrregdata |= (CLOCK_PHASE | CLOCK_POLARITY);
+		wrregdata &= ~HIGH_SPEED_MODE;
+		bcmspi_card_regwrite(sd, 0, SPID_CONFIG, 4, wrregdata);
+#endif /* HSMODE */
+
+		for (i = 0; i < INIT_ADAPT_LOOP; i++) {
+			if ((regdata == 0xfdda7d5b) || (regdata == 0xfdda7d5a)) {
+				sd_trace(("0xfeedbead was leftshifted by 1-bit.\n"));
+				if ((status = bcmspi_card_regread(sd, 0, SPID_TEST_READ, 4,
+				     &regdata)) != SUCCESS)
+					return FALSE;
+			}
+			OSL_DELAY(1000);
+		}
+
+
+		/* Change to host controller intr-polarity of active-low */
+		wrregdata &= ~INTR_POLARITY;
+		sd_trace(("(we are still in 16bit mode) 32bit Write LE reg-ctrl-data = 0x%x\n",
+		        wrregdata));
+		/* Change to 32bit mode */
+		wrregdata |= WORD_LENGTH_32;
+		bcmspi_card_regwrite(sd, 0, SPID_CONFIG, 4, wrregdata);
+
+		/* Change command/data packaging in 32bit LE mode */
+		sd->wordlen = 4;
+
+		if ((status = bcmspi_card_regread(sd, 0, SPID_TEST_READ, 4, &regdata)) != SUCCESS)
+			return FALSE;
+
+		if (regdata == TEST_RO_DATA_32BIT_LE) {
+			sd_trace(("Read spid passed. Value read = 0x%x\n", regdata));
+			sd_trace(("Spid had power-on cycle OR spi was soft-resetted \n"));
+		} else {
+			sd_err(("Stale spid reg values read as it was kept powered. Value read ="
+			  "0x%x\n", regdata));
+			return FALSE;
+		}
+	}
+
+
+	return TRUE;
+}
+
+static bool
+bcmspi_test_card(sdioh_info_t *sd)
+{
+	uint32 regdata;
+	int status;
+
+	if ((status = bcmspi_card_regread(sd, 0, SPID_TEST_READ, 4, &regdata)) != SUCCESS)
+		return FALSE;
+
+	if (regdata == (TEST_RO_DATA_32BIT_LE))
+		sd_trace(("32bit LE regdata = 0x%x\n", regdata));
+	else {
+		sd_trace(("Incorrect 32bit LE regdata = 0x%x\n", regdata));
+		return FALSE;
+	}
+
+
+#define RW_PATTERN1	0xA0A1A2A3
+#define RW_PATTERN2	0x4B5B6B7B
+
+	regdata = RW_PATTERN1;
+	if ((status = bcmspi_card_regwrite(sd, 0, SPID_TEST_RW, 4, regdata)) != SUCCESS)
+		return FALSE;
+	regdata = 0;
+	if ((status = bcmspi_card_regread(sd, 0, SPID_TEST_RW, 4, &regdata)) != SUCCESS)
+		return FALSE;
+	if (regdata != RW_PATTERN1) {
+		sd_err(("Write-Read spid failed. Value wrote = 0x%x, Value read = 0x%x\n",
+			RW_PATTERN1, regdata));
+		return FALSE;
+	} else
+		sd_trace(("R/W spid passed. Value read = 0x%x\n", regdata));
+
+	regdata = RW_PATTERN2;
+	if ((status = bcmspi_card_regwrite(sd, 0, SPID_TEST_RW, 4, regdata)) != SUCCESS)
+		return FALSE;
+	regdata = 0;
+	if ((status = bcmspi_card_regread(sd, 0, SPID_TEST_RW, 4, &regdata)) != SUCCESS)
+		return FALSE;
+	if (regdata != RW_PATTERN2) {
+		sd_err(("Write-Read spid failed. Value wrote = 0x%x, Value read = 0x%x\n",
+			RW_PATTERN2, regdata));
+		return FALSE;
+	} else
+		sd_trace(("R/W spid passed. Value read = 0x%x\n", regdata));
+
+	return TRUE;
+}
+
+static int
+bcmspi_driver_init(sdioh_info_t *sd)
+{
+	sd_trace(("%s\n", __FUNCTION__));
+	if ((bcmspi_host_init(sd)) != SUCCESS) {
+		return ERROR;
+	}
+
+	if (bcmspi_client_init(sd) != SUCCESS) {
+		return ERROR;
+	}
+
+	return SUCCESS;
+}
+
+/* Read device reg */
+static int
+bcmspi_card_regread(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 *data)
+{
+	int status;
+	uint32 cmd_arg, dstatus;
+
+	ASSERT(regsize);
+
+	if (func == 2)
+		sd_trace(("Reg access on F2 will generate error indication in dstatus bits.\n"));
+
+	cmd_arg = 0;
+	cmd_arg = SFIELD(cmd_arg, SPI_RW_FLAG, 0);
+	cmd_arg = SFIELD(cmd_arg, SPI_ACCESS, 1);	/* Incremental access */
+	cmd_arg = SFIELD(cmd_arg, SPI_FUNCTION, func);
+	cmd_arg = SFIELD(cmd_arg, SPI_REG_ADDR, regaddr);
+	cmd_arg = SFIELD(cmd_arg, SPI_LEN, regsize == BLOCK_SIZE_F2 ? 0 : regsize);
+
+	sd_trace(("%s cmd_arg = 0x%x\n", __FUNCTION__, cmd_arg));
+	sd_trace(("%s: rw=%d, func=%d, regaddr=0x%08x, data=0x%x\n", __FUNCTION__, 0, func,
+	         regaddr, *data));
+
+	if ((status = bcmspi_cmd_issue(sd, sd->sd_use_dma, cmd_arg, data, regsize))
+	    != SUCCESS)
+		return status;
+
+	bcmspi_cmd_getdstatus(sd, &dstatus);
+	if (dstatus)
+		sd_trace(("dstatus =0x%x\n", dstatus));
+
+	return SUCCESS;
+}
+
+static int
+bcmspi_card_regread_fixedaddr(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 *data)
+{
+
+	int status;
+	uint32 cmd_arg;
+	uint32 dstatus;
+
+	ASSERT(regsize);
+
+	if (func == 2)
+		sd_trace(("Reg access on F2 will generate error indication in dstatus bits.\n"));
+
+	cmd_arg = 0;
+	cmd_arg = SFIELD(cmd_arg, SPI_RW_FLAG, 0);
+	cmd_arg = SFIELD(cmd_arg, SPI_ACCESS, 0);	/* Fixed access */
+	cmd_arg = SFIELD(cmd_arg, SPI_FUNCTION, func);
+	cmd_arg = SFIELD(cmd_arg, SPI_REG_ADDR, regaddr);
+	cmd_arg = SFIELD(cmd_arg, SPI_LEN, regsize);
+
+	sd_trace(("%s cmd_arg = 0x%x\n", __FUNCTION__, cmd_arg));
+
+	if ((status = bcmspi_cmd_issue(sd, sd->sd_use_dma, cmd_arg, data, regsize))
+	    != SUCCESS)
+		return status;
+
+	sd_trace(("%s: rw=%d, func=%d, regaddr=0x%08x, data=0x%x\n", __FUNCTION__, 0, func,
+	         regaddr, *data));
+
+	bcmspi_cmd_getdstatus(sd, &dstatus);
+	sd_trace(("dstatus =0x%x\n", dstatus));
+	return SUCCESS;
+}
+
+/* write a device register */
+static int
+bcmspi_card_regwrite(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 data)
+{
+	int status;
+	uint32 cmd_arg, dstatus;
+
+	ASSERT(regsize);
+
+	cmd_arg = 0;
+
+	cmd_arg = SFIELD(cmd_arg, SPI_RW_FLAG, 1);
+	cmd_arg = SFIELD(cmd_arg, SPI_ACCESS, 1);	/* Incremental access */
+	cmd_arg = SFIELD(cmd_arg, SPI_FUNCTION, func);
+	cmd_arg = SFIELD(cmd_arg, SPI_REG_ADDR, regaddr);
+	cmd_arg = SFIELD(cmd_arg, SPI_LEN, regsize == BLOCK_SIZE_F2 ? 0 : regsize);
+
+	sd_trace(("%s cmd_arg = 0x%x\n", __FUNCTION__, cmd_arg));
+	sd_trace(("%s: rw=%d, func=%d, regaddr=0x%08x, data=0x%x\n", __FUNCTION__, 1, func,
+	         regaddr, data));
+
+
+	if ((status = bcmspi_cmd_issue(sd, sd->sd_use_dma, cmd_arg, &data, regsize))
+	    != SUCCESS)
+		return status;
+
+	bcmspi_cmd_getdstatus(sd, &dstatus);
+	if (dstatus)
+		sd_trace(("dstatus =0x%x\n", dstatus));
+
+	return SUCCESS;
+}
+
+/* write a device register - 1 byte */
+static int
+bcmspi_card_bytewrite(sdioh_info_t *sd, int func, uint32 regaddr, uint8 *byte)
+{
+	int status;
+	uint32 cmd_arg;
+	uint32 dstatus;
+	uint32 data = (uint32)(*byte);
+
+	cmd_arg = 0;
+	cmd_arg = SFIELD(cmd_arg, SPI_FUNCTION, func);
+	cmd_arg = SFIELD(cmd_arg, SPI_ACCESS, 1);	/* Incremental access */
+	cmd_arg = SFIELD(cmd_arg, SPI_REG_ADDR, regaddr);
+	cmd_arg = SFIELD(cmd_arg, SPI_RW_FLAG, 1);
+	cmd_arg = SFIELD(cmd_arg, SPI_LEN, 1);
+
+	sd_trace(("%s cmd_arg = 0x%x\n", __FUNCTION__, cmd_arg));
+	sd_trace(("%s: func=%d, regaddr=0x%08x, data=0x%x\n", __FUNCTION__, func,
+	         regaddr, data));
+
+	if ((status = bcmspi_cmd_issue(sd, sd->sd_use_dma,
+	                              cmd_arg, &data, 1)) != SUCCESS) {
+		return status;
+	}
+
+	bcmspi_cmd_getdstatus(sd, &dstatus);
+	if (dstatus)
+		sd_trace(("dstatus =0x%x\n", dstatus));
+
+	return SUCCESS;
+}
+
+void
+bcmspi_cmd_getdstatus(sdioh_info_t *sd, uint32 *dstatus_buffer)
+{
+	*dstatus_buffer = sd->card_dstatus;
+}
+
+/* 'data' is of type uint32 whereas other buffers are of type uint8 */
+static int
+bcmspi_cmd_issue(sdioh_info_t *sd, bool use_dma, uint32 cmd_arg,
+                uint32 *data, uint32 datalen)
+{
+	uint32	i, j;
+	uint8	resp_delay = 0;
+	int	err = SUCCESS;
+	uint32	hostlen;
+	uint32 spilen = 0;
+	uint32 dstatus_idx = 0;
+	uint16 templen, buslen, len, *ptr = NULL;
+
+	sd_trace(("spi cmd = 0x%x\n", cmd_arg));
+
+	if (DWORDMODE_ON) {
+		spilen = GFIELD(cmd_arg, SPI_LEN);
+		if ((GFIELD(cmd_arg, SPI_FUNCTION) == SPI_FUNC_0) ||
+		    (GFIELD(cmd_arg, SPI_FUNCTION) == SPI_FUNC_1))
+			dstatus_idx = spilen * 3;
+
+		if ((GFIELD(cmd_arg, SPI_FUNCTION) == SPI_FUNC_2) &&
+		    (GFIELD(cmd_arg, SPI_RW_FLAG) == 1)) {
+			spilen = spilen << 2;
+			dstatus_idx = (spilen % 16) ? (16 - (spilen % 16)) : 0;
+			/* convert len to mod16 size */
+			spilen = ROUNDUP(spilen, 16);
+			cmd_arg = SFIELD(cmd_arg, SPI_LEN, (spilen >> 2));
+		}
+	}
+
+	/* Set up and issue the SPI command.  MSByte goes out on bus first.  Increase datalen
+	 * according to the wordlen mode(16/32bit) the device is in.
+	 */
+	if (sd->wordlen == 4) { /* 32bit spid */
+		*(uint32 *)spi_outbuf = bcmswap32(cmd_arg);
+		if (datalen & 0x3)
+			datalen += (4 - (datalen & 0x3));
+	} else if (sd->wordlen == 2) { /* 16bit spid */
+		*(uint16 *)spi_outbuf = bcmswap16(cmd_arg & 0xffff);
+		*(uint16 *)&spi_outbuf[2] = bcmswap16((cmd_arg & 0xffff0000) >> 16);
+		if (datalen & 0x1)
+			datalen++;
+		if (datalen < 4)
+			datalen = ROUNDUP(datalen, 4);
+	} else {
+		sd_err(("Host is %d bit spid, could not create SPI command.\n",
+			8 * sd->wordlen));
+		return ERROR;
+	}
+
+	/* for Write, put the data into the output buffer */
+	if (GFIELD(cmd_arg, SPI_RW_FLAG) == 1) {
+		/* We send len field of hw-header always a mod16 size, both from host and dongle */
+		if (DWORDMODE_ON) {
+			if (GFIELD(cmd_arg, SPI_FUNCTION) == SPI_FUNC_2) {
+				ptr = (uint16 *)&data[0];
+				templen = *ptr;
+				/* ASSERT(*ptr == ~*(ptr + 1)); */
+				templen = ROUNDUP(templen, 16);
+				*ptr = templen;
+				sd_trace(("actual tx len = %d\n", (uint16)(~*(ptr+1))));
+			}
+		}
+
+		if (datalen != 0) {
+			for (i = 0; i < datalen/4; i++) {
+				if (sd->wordlen == 4) { /* 32bit spid */
+					*(uint32 *)&spi_outbuf[i * 4 + CMDLEN] =
+						bcmswap32(data[i]);
+				} else if (sd->wordlen == 2) { /* 16bit spid */
+					*(uint16 *)&spi_outbuf[i * 4 + CMDLEN] =
+						bcmswap16(data[i] & 0xffff);
+					*(uint16 *)&spi_outbuf[i * 4 + CMDLEN + 2] =
+						bcmswap16((data[i] & 0xffff0000) >> 16);
+				}
+			}
+		}
+	}
+
+	/* Append resp-delay number of bytes and clock them out for F0/1/2 reads. */
+	if (GFIELD(cmd_arg, SPI_RW_FLAG) == 0) {
+		int func = GFIELD(cmd_arg, SPI_FUNCTION);
+		switch (func) {
+			case 0:
+				resp_delay = sd->resp_delay_all ? F0_RESPONSE_DELAY : 0;
+				break;
+			case 1:
+				resp_delay = F1_RESPONSE_DELAY;
+				break;
+			case 2:
+				resp_delay = sd->resp_delay_all ? F2_RESPONSE_DELAY : 0;
+				break;
+			default:
+				ASSERT(0);
+				break;
+		}
+		/* Program response delay */
+	        bcmspi_prog_resp_delay(sd, func, resp_delay);
+	}
+
+	/* +4 for cmd and +4 for dstatus */
+	hostlen = datalen + 8 + resp_delay;
+	hostlen += dstatus_idx;
+	hostlen += (4 - (hostlen & 0x3));
+	spi_sendrecv(sd, spi_outbuf, spi_inbuf, hostlen);
+
+	/* for Read, get the data into the input buffer */
+	if (datalen != 0) {
+		if (GFIELD(cmd_arg, SPI_RW_FLAG) == 0) { /* if read cmd */
+			for (j = 0; j < datalen/4; j++) {
+				if (sd->wordlen == 4) { /* 32bit spid */
+					data[j] = bcmswap32(*(uint32 *)&spi_inbuf[j * 4 +
+					            CMDLEN + resp_delay]);
+				} else if (sd->wordlen == 2) { /* 16bit spid */
+					data[j] = (bcmswap16(*(uint16 *)&spi_inbuf[j * 4 +
+					            CMDLEN + resp_delay])) |
+					         ((bcmswap16(*(uint16 *)&spi_inbuf[j * 4 +
+					            CMDLEN + resp_delay + 2])) << 16);
+				}
+			}
+
+			if ((DWORDMODE_ON) && (GFIELD(cmd_arg, SPI_FUNCTION) == SPI_FUNC_2)) {
+				ptr = (uint16 *)&data[0];
+				templen = *ptr;
+				buslen = len = ~(*(ptr + 1));
+				buslen = ROUNDUP(buslen, 16);
+				/* populate actual len in hw-header */
+				if (templen == buslen)
+					*ptr = len;
+			}
+		}
+	}
+
+	/* Restore back the len field of the hw header */
+	if (DWORDMODE_ON) {
+		if ((GFIELD(cmd_arg, SPI_FUNCTION) == SPI_FUNC_2) &&
+		    (GFIELD(cmd_arg, SPI_RW_FLAG) == 1)) {
+			ptr = (uint16 *)&data[0];
+			*ptr = (uint16)(~*(ptr+1));
+		}
+	}
+
+	dstatus_idx += (datalen + CMDLEN + resp_delay);
+	/* Last 4bytes are dstatus.  Device is configured to return status bits. */
+	if (sd->wordlen == 4) { /* 32bit spid */
+		sd->card_dstatus = bcmswap32(*(uint32 *)&spi_inbuf[dstatus_idx]);
+	} else if (sd->wordlen == 2) { /* 16bit spid */
+		sd->card_dstatus = (bcmswap16(*(uint16 *)&spi_inbuf[dstatus_idx]) |
+		                   (bcmswap16(*(uint16 *)&spi_inbuf[dstatus_idx + 2]) << 16));
+	} else {
+		sd_err(("Host is %d bit machine, could not read SPI dstatus.\n",
+			8 * sd->wordlen));
+		return ERROR;
+	}
+	if (sd->card_dstatus == 0xffffffff) {
+		sd_err(("looks like not a GSPI device or device is not powered.\n"));
+	}
+
+	err = bcmspi_update_stats(sd, cmd_arg);
+
+	return err;
+
+}
+
+static int
+bcmspi_card_buf(sdioh_info_t *sd, int rw, int func, bool fifo,
+                uint32 addr, int nbytes, uint32 *data)
+{
+	int status;
+	uint32 cmd_arg;
+	bool write = rw == SDIOH_READ ? 0 : 1;
+	uint retries = 0;
+
+	bool enable;
+	uint32	spilen;
+
+	cmd_arg = 0;
+
+	ASSERT(nbytes);
+	ASSERT(nbytes <= sd->client_block_size[func]);
+
+	if (write) sd->t_cnt++; else sd->r_cnt++;
+
+	if (func == 2) {
+		/* Frame len check limited by gSPI. */
+		if ((nbytes > 2000) && write) {
+			sd_trace((">2KB write: F2 wr of %d bytes\n", nbytes));
+		}
+		/* ASSERT(nbytes <= 2048); Fix bigger len gspi issue and uncomment. */
+		/* If F2 fifo on device is not ready to receive data, don't do F2 transfer */
+		if (write) {
+			uint32 dstatus;
+			/* check F2 ready with cached one */
+			bcmspi_cmd_getdstatus(sd, &dstatus);
+			if ((dstatus & STATUS_F2_RX_READY) == 0) {
+				retries = WAIT_F2RXFIFORDY;
+				enable = 0;
+				while (retries-- && !enable) {
+					OSL_DELAY(WAIT_F2RXFIFORDY_DELAY * 1000);
+					bcmspi_card_regread(sd, SPI_FUNC_0, SPID_STATUS_REG, 4,
+					                   &dstatus);
+					if (dstatus & STATUS_F2_RX_READY)
+						enable = TRUE;
+				}
+				if (!enable) {
+					struct spierrstats_t *spierrstats = &sd->spierrstats;
+					spierrstats->f2rxnotready++;
+					sd_err(("F2 FIFO is not ready to receive data.\n"));
+					return ERROR;
+				}
+				sd_trace(("No of retries on F2 ready %d\n",
+					(WAIT_F2RXFIFORDY - retries)));
+			}
+		}
+	}
+
+	/* F2 transfers happen on 0 addr */
+	addr = (func == 2) ? 0 : addr;
+
+	/* In pio mode buffer is read using fixed address fifo in func 1 */
+	if ((func == 1) && (fifo))
+		cmd_arg = SFIELD(cmd_arg, SPI_ACCESS, 0);
+	else
+		cmd_arg = SFIELD(cmd_arg, SPI_ACCESS, 1);
+
+	cmd_arg = SFIELD(cmd_arg, SPI_FUNCTION, func);
+	cmd_arg = SFIELD(cmd_arg, SPI_REG_ADDR, addr);
+	cmd_arg = SFIELD(cmd_arg, SPI_RW_FLAG, write);
+	spilen = sd->data_xfer_count = MIN(sd->client_block_size[func], nbytes);
+	if ((sd->dwordmode == TRUE) && (GFIELD(cmd_arg, SPI_FUNCTION) == SPI_FUNC_2)) {
+		/* convert len to mod4 size */
+		spilen = spilen + ((spilen & 0x3) ? (4 - (spilen & 0x3)): 0);
+		cmd_arg = SFIELD(cmd_arg, SPI_LEN, (spilen >> 2));
+	} else
+		cmd_arg = SFIELD(cmd_arg, SPI_LEN, spilen);
+
+	if ((func == 2) && (fifo == 1)) {
+		sd_data(("%s: %s func %d, %s, addr 0x%x, len %d bytes, r_cnt %d t_cnt %d\n",
+		          __FUNCTION__, write ? "Wr" : "Rd", func, "INCR",
+		          addr, nbytes, sd->r_cnt, sd->t_cnt));
+	}
+
+	sd_trace(("%s cmd_arg = 0x%x\n", __FUNCTION__, cmd_arg));
+	sd_data(("%s: %s func %d, %s, addr 0x%x, len %d bytes, r_cnt %d t_cnt %d\n",
+	         __FUNCTION__, write ? "Wd" : "Rd", func, "INCR",
+	         addr, nbytes, sd->r_cnt, sd->t_cnt));
+
+
+	if ((status = bcmspi_cmd_issue(sd, sd->sd_use_dma, cmd_arg,
+	     data, nbytes)) != SUCCESS) {
+		sd_err(("%s: cmd_issue failed for %s\n", __FUNCTION__,
+			(write ? "write" : "read")));
+		return status;
+	}
+
+	/* gSPI expects that hw-header-len is equal to spi-command-len */
+	if ((func == 2) && (rw == SDIOH_WRITE) && (sd->dwordmode == FALSE)) {
+		ASSERT((uint16)sd->data_xfer_count == (uint16)(*data & 0xffff));
+		ASSERT((uint16)sd->data_xfer_count == (uint16)(~((*data & 0xffff0000) >> 16)));
+	}
+
+	if ((nbytes > 2000) && !write) {
+		sd_trace((">2KB read: F2 rd of %d bytes\n", nbytes));
+	}
+
+	return SUCCESS;
+}
+
+/* Reset and re-initialize the device */
+int
+sdioh_sdio_reset(sdioh_info_t *si)
+{
+	si->card_init_done = FALSE;
+	return bcmspi_client_init(si);
+}
diff --git a/drivers/net/wireless/bcmdhd/Makefile b/drivers/net/wireless/bcmdhd/Makefile
deleted file mode 100644
index e82c9856f00..00000000000
--- a/drivers/net/wireless/bcmdhd/Makefile
+++ /dev/null
@@ -1,31 +0,0 @@
-# bcmdhd
-DHDCFLAGS = -Wall -Wstrict-prototypes -Dlinux -DBCMDRIVER                     \
-	-DBCMDONGLEHOST -DUNRELEASEDCHIP -DBCMDMA32 -DWLBTAMP -DBCMFILEIMAGE  \
-	-DDHDTHREAD -DDHD_GPL -DDHD_SCHED -DDHD_DEBUG -DSDTEST -DBDC -DTOE    \
-	-DDHD_BCMEVENTS -DSHOW_EVENTS -DDONGLEOVERLAYS -DBCMDBG               \
-	-DCUSTOMER_HW2 -DCUSTOM_OOB_GPIO_NUM=2 -DOOB_INTR_ONLY -DHW_OOB       \
-	-DMMC_SDIO_ABORT -DBCMSDIO -DBCMLXSDMMC -DBCMPLATFORM_BUS -DWLP2P     \
-	-DNEW_COMPAT_WIRELESS -DWIFI_ACT_FRAME -DARP_OFFLOAD_SUPPORT          \
-	-DKEEP_ALIVE -DCSCAN -DGET_CUSTOM_MAC_ENABLE -DPKT_FILTER_SUPPORT     \
-	-DEMBEDDED_PLATFORM -DENABLE_INSMOD_NO_FW_LOAD -DPNO_SUPPORT          \
-	-Idrivers/net/wireless/bcmdhd -Idrivers/net/wireless/bcmdhd/include
-
-DHDOFILES = aiutils.o bcmsdh_sdmmc_linux.o dhd_linux.o siutils.o bcmutils.o   \
-	dhd_linux_sched.o bcmwifi.o dhd_sdio.o bcmevent.o dhd_bta.o hndpmu.o  \
-	bcmsdh.o dhd_cdc.o bcmsdh_linux.o dhd_common.o linux_osl.o            \
-	bcmsdh_sdmmc.o dhd_custom_gpio.o sbutils.o wldev_common.o wl_android.o
-
-obj-$(CONFIG_BCMDHD) += bcmdhd.o
-bcmdhd-objs += $(DHDOFILES)
-ifneq ($(CONFIG_WIRELESS_EXT),)
-bcmdhd-objs += wl_iw.o
-DHDCFLAGS += -DSOFTAP
-endif
-ifneq ($(CONFIG_CFG80211),)
-bcmdhd-objs += wl_cfg80211.o wl_cfgp2p.o dhd_linux_mon.o
-DHDCFLAGS += -DWL_CFG80211
-endif
-EXTRA_CFLAGS = $(DHDCFLAGS)
-ifeq ($(CONFIG_BCMDHD),m)
-EXTRA_LDFLAGS += --strip-debug
-endif
diff --git a/drivers/net/wireless/bcmdhd/bcmevent.c b/drivers/net/wireless/bcmdhd/bcmevent.c
deleted file mode 100644
index 24581ddd353..00000000000
--- a/drivers/net/wireless/bcmdhd/bcmevent.c
+++ /dev/null
@@ -1,125 +0,0 @@
-/*
- * bcmevent read-only data shared by kernel or app layers
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- * $Id: bcmevent.c,v 1.8.2.7 2011-02-01 06:23:39 Exp $
- */
-
-#include <typedefs.h>
-#include <bcmutils.h>
-#include <proto/ethernet.h>
-#include <proto/bcmeth.h>
-#include <proto/bcmevent.h>
-
-#if WLC_E_LAST != 85
-#error "You need to add an entry to bcmevent_names[] for the new event"
-#endif
-
-const bcmevent_name_t bcmevent_names[] = {
-	{ WLC_E_SET_SSID, "SET_SSID" },
-	{ WLC_E_JOIN, "JOIN" },
-	{ WLC_E_START, "START" },
-	{ WLC_E_AUTH, "AUTH" },
-	{ WLC_E_AUTH_IND, "AUTH_IND" },
-	{ WLC_E_DEAUTH, "DEAUTH" },
-	{ WLC_E_DEAUTH_IND, "DEAUTH_IND" },
-	{ WLC_E_ASSOC, "ASSOC" },
-	{ WLC_E_ASSOC_IND, "ASSOC_IND" },
-	{ WLC_E_REASSOC, "REASSOC" },
-	{ WLC_E_REASSOC_IND, "REASSOC_IND" },
-	{ WLC_E_DISASSOC, "DISASSOC" },
-	{ WLC_E_DISASSOC_IND, "DISASSOC_IND" },
-	{ WLC_E_QUIET_START, "START_QUIET" },
-	{ WLC_E_QUIET_END, "END_QUIET" },
-	{ WLC_E_BEACON_RX, "BEACON_RX" },
-	{ WLC_E_LINK, "LINK" },
-	{ WLC_E_MIC_ERROR, "MIC_ERROR" },
-	{ WLC_E_NDIS_LINK, "NDIS_LINK" },
-	{ WLC_E_ROAM, "ROAM" },
-	{ WLC_E_TXFAIL, "TXFAIL" },
-	{ WLC_E_PMKID_CACHE, "PMKID_CACHE" },
-	{ WLC_E_RETROGRADE_TSF, "RETROGRADE_TSF" },
-	{ WLC_E_PRUNE, "PRUNE" },
-	{ WLC_E_AUTOAUTH, "AUTOAUTH" },
-	{ WLC_E_EAPOL_MSG, "EAPOL_MSG" },
-	{ WLC_E_SCAN_COMPLETE, "SCAN_COMPLETE" },
-	{ WLC_E_ADDTS_IND, "ADDTS_IND" },
-	{ WLC_E_DELTS_IND, "DELTS_IND" },
-	{ WLC_E_BCNSENT_IND, "BCNSENT_IND" },
-	{ WLC_E_BCNRX_MSG, "BCNRX_MSG" },
-	{ WLC_E_BCNLOST_MSG, "BCNLOST_IND" },
-	{ WLC_E_ROAM_PREP, "ROAM_PREP" },
-	{ WLC_E_PFN_NET_FOUND, "PFNFOUND_IND" },
-	{ WLC_E_PFN_NET_LOST, "PFNLOST_IND" },
-#if defined(IBSS_PEER_DISCOVERY_EVENT)
-	{ WLC_E_IBSS_ASSOC, "IBSS_ASSOC" },
-#endif /* defined(IBSS_PEER_DISCOVERY_EVENT) */
-	{ WLC_E_RADIO, "RADIO" },
-	{ WLC_E_PSM_WATCHDOG, "PSM_WATCHDOG" },
-	{ WLC_E_PROBREQ_MSG, "PROBE_REQ_MSG" },
-	{ WLC_E_SCAN_CONFIRM_IND, "SCAN_CONFIRM_IND" },
-	{ WLC_E_PSK_SUP, "PSK_SUP" },
-	{ WLC_E_COUNTRY_CODE_CHANGED, "CNTRYCODE_IND" },
-	{ WLC_E_EXCEEDED_MEDIUM_TIME, "EXCEEDED_MEDIUM_TIME" },
-	{ WLC_E_ICV_ERROR, "ICV_ERROR" },
-	{ WLC_E_UNICAST_DECODE_ERROR, "UNICAST_DECODE_ERROR" },
-	{ WLC_E_MULTICAST_DECODE_ERROR, "MULTICAST_DECODE_ERROR" },
-	{ WLC_E_TRACE, "TRACE" },
-	{ WLC_E_BTA_HCI_EVENT, "BTA_HCI_EVENT" },
-	{ WLC_E_IF, "IF" },
-#ifdef WLP2P
-	{ WLC_E_P2P_DISC_LISTEN_COMPLETE, "WLC_E_P2P_DISC_LISTEN_COMPLETE" },
-#endif
-	{ WLC_E_RSSI, "RSSI" },
-	{ WLC_E_PFN_SCAN_COMPLETE, "SCAN_COMPLETE" },
-	{ WLC_E_EXTLOG_MSG, "EXTERNAL LOG MESSAGE" },
-#ifdef WIFI_ACT_FRAME
-	{ WLC_E_ACTION_FRAME, "ACTION_FRAME" },
-	{ WLC_E_ACTION_FRAME_RX, "ACTION_FRAME_RX" },
-	{ WLC_E_ACTION_FRAME_COMPLETE, "ACTION_FRAME_COMPLETE" },
-#endif
-	{ WLC_E_ESCAN_RESULT, "WLC_E_ESCAN_RESULT" },
-	{ WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE, "WLC_E_AF_OFF_CHAN_COMPLETE" },
-#ifdef WLP2P
-	{ WLC_E_PROBRESP_MSG, "PROBE_RESP_MSG" },
-	{ WLC_E_P2P_PROBREQ_MSG, "P2P PROBE_REQ_MSG" },
-#endif
-#ifdef PROP_TXSTATUS
-	{ WLC_E_FIFO_CREDIT_MAP, "FIFO_CREDIT_MAP" },
-#endif
-	{ WLC_E_WAKE_EVENT, "WAKE_EVENT" },
-	{ WLC_E_DCS_REQUEST, "DCS_REQUEST" },
-	{ WLC_E_RM_COMPLETE, "RM_COMPLETE" },
-#ifdef WLMEDIA_HTSF
-	{ WLC_E_HTSFSYNC, "HTSF_SYNC_EVENT" },
-#endif
-	{ WLC_E_OVERLAY_REQ, "OVERLAY_REQ_EVENT" },
-	{ WLC_E_CSA_COMPLETE_IND, "WLC_E_CSA_COMPLETE_IND" },
-	{ WLC_E_EXCESS_PM_WAKE_EVENT, "EXCESS_PM_WAKE_EVENT" },
-	{ WLC_E_PFN_SCAN_NONE, "PFN_SCAN_NONE" },
-	{ WLC_E_PFN_SCAN_ALLGONE, "PFN_SCAN_ALLGONE" },
-#ifdef SOFTAP
-	{ WLC_E_GTK_PLUMBED, "GTK_PLUMBED" }
-#endif
-};
-
-
-const int bcmevent_names_size = ARRAYSIZE(bcmevent_names);
diff --git a/drivers/net/wireless/bcmdhd/bcmsdh_linux.c b/drivers/net/wireless/bcmdhd/bcmsdh_linux.c
deleted file mode 100644
index a4dc6ff4042..00000000000
--- a/drivers/net/wireless/bcmdhd/bcmsdh_linux.c
+++ /dev/null
@@ -1,731 +0,0 @@
-/*
- * SDIO access interface for drivers - linux specific (pci only)
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: bcmsdh_linux.c,v 1.72.6.5 2010-12-23 01:13:15 Exp $
- */
-
-/**
- * @file bcmsdh_linux.c
- */
-
-#define __UNDEF_NO_VERSION__
-
-#include <typedefs.h>
-#include <linuxver.h>
-
-#include <linux/pci.h>
-#include <linux/completion.h>
-
-#include <osl.h>
-#include <pcicfg.h>
-#include <bcmdefs.h>
-#include <bcmdevs.h>
-
-#if defined(OOB_INTR_ONLY)
-#include <linux/irq.h>
-extern void dhdsdio_isr(void * args);
-#include <bcmutils.h>
-#include <dngl_stats.h>
-#include <dhd.h>
-#endif /* defined(OOB_INTR_ONLY) */
-
-/**
- * SDIO Host Controller info
- */
-typedef struct bcmsdh_hc bcmsdh_hc_t;
-
-struct bcmsdh_hc {
-	bcmsdh_hc_t *next;
-#ifdef BCMPLATFORM_BUS
-	struct device *dev;			/* platform device handle */
-#else
-	struct pci_dev *dev;		/* pci device handle */
-#endif /* BCMPLATFORM_BUS */
-	osl_t *osh;
-	void *regs;			/* SDIO Host Controller address */
-	bcmsdh_info_t *sdh;		/* SDIO Host Controller handle */
-	void *ch;
-	unsigned int oob_irq;
-	unsigned long oob_flags; /* OOB Host specifiction as edge and etc */
-	bool oob_irq_registered;
-	bool oob_irq_enable_flag;
-#if defined(OOB_INTR_ONLY)
-	spinlock_t irq_lock;
-#endif
-};
-static bcmsdh_hc_t *sdhcinfo = NULL;
-
-/* driver info, initialized when bcmsdh_register is called */
-static bcmsdh_driver_t drvinfo = {NULL, NULL};
-
-/* debugging macros */
-#define SDLX_MSG(x)
-
-/**
- * Checks to see if vendor and device IDs match a supported SDIO Host Controller.
- */
-bool
-bcmsdh_chipmatch(uint16 vendor, uint16 device)
-{
-	/* Add other vendors and devices as required */
-
-#ifdef BCMSDIOH_STD
-	/* Check for Arasan host controller */
-	if (vendor == VENDOR_SI_IMAGE) {
-		return (TRUE);
-	}
-	/* Check for BRCM 27XX Standard host controller */
-	if (device == BCM27XX_SDIOH_ID && vendor == VENDOR_BROADCOM) {
-		return (TRUE);
-	}
-	/* Check for BRCM Standard host controller */
-	if (device == SDIOH_FPGA_ID && vendor == VENDOR_BROADCOM) {
-		return (TRUE);
-	}
-	/* Check for TI PCIxx21 Standard host controller */
-	if (device == PCIXX21_SDIOH_ID && vendor == VENDOR_TI) {
-		return (TRUE);
-	}
-	if (device == PCIXX21_SDIOH0_ID && vendor == VENDOR_TI) {
-		return (TRUE);
-	}
-	/* Ricoh R5C822 Standard SDIO Host */
-	if (device == R5C822_SDIOH_ID && vendor == VENDOR_RICOH) {
-		return (TRUE);
-	}
-	/* JMicron Standard SDIO Host */
-	if (device == JMICRON_SDIOH_ID && vendor == VENDOR_JMICRON) {
-		return (TRUE);
-	}
-
-#endif /* BCMSDIOH_STD */
-#ifdef BCMSDIOH_SPI
-	/* This is the PciSpiHost. */
-	if (device == SPIH_FPGA_ID && vendor == VENDOR_BROADCOM) {
-		printf("Found PCI SPI Host Controller\n");
-		return (TRUE);
-	}
-
-#endif /* BCMSDIOH_SPI */
-
-	return (FALSE);
-}
-
-#if defined(BCMPLATFORM_BUS)
-#if defined(BCMLXSDMMC)
-/* forward declarations */
-int bcmsdh_probe(struct device *dev);
-int bcmsdh_remove(struct device *dev);
-
-EXPORT_SYMBOL(bcmsdh_probe);
-EXPORT_SYMBOL(bcmsdh_remove);
-
-#else
-/* forward declarations */
-static int __devinit bcmsdh_probe(struct device *dev);
-static int __devexit bcmsdh_remove(struct device *dev);
-#endif /* BCMLXSDMMC */
-
-#ifndef BCMLXSDMMC
-static struct device_driver bcmsdh_driver = {
-	.name		= "pxa2xx-mci",
-	.bus		= &platform_bus_type,
-	.probe		= bcmsdh_probe,
-	.remove		= bcmsdh_remove,
-	.suspend	= NULL,
-	.resume		= NULL,
-	};
-#endif /* BCMLXSDMMC */
-
-#ifndef BCMLXSDMMC
-static
-#endif /* BCMLXSDMMC */
-int bcmsdh_probe(struct device *dev)
-{
-	osl_t *osh = NULL;
-	bcmsdh_hc_t *sdhc = NULL;
-	ulong regs = 0;
-	bcmsdh_info_t *sdh = NULL;
-#if !defined(BCMLXSDMMC) && defined(BCMPLATFORM_BUS)
-	struct platform_device *pdev;
-	struct resource *r;
-#endif /* BCMLXSDMMC */
-	int irq = 0;
-	uint32 vendevid;
-	unsigned long irq_flags = 0;
-
-#if !defined(BCMLXSDMMC) && defined(BCMPLATFORM_BUS)
-	pdev = to_platform_device(dev);
-	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	irq = platform_get_irq(pdev, 0);
-	if (!r || irq == NO_IRQ)
-		return -ENXIO;
-#endif /* BCMLXSDMMC */
-
-#if defined(OOB_INTR_ONLY)
-#ifdef HW_OOB
-	irq_flags =
-		IORESOURCE_IRQ | IORESOURCE_IRQ_HIGHLEVEL | IORESOURCE_IRQ_SHAREABLE;
-#else
-	 irq_flags = IRQF_TRIGGER_FALLING;
-#endif /* HW_OOB */
-
-	/* Get customer specific OOB IRQ parametres: IRQ number as IRQ type */
-	irq = dhd_customer_oob_irq_map(&irq_flags);
-	if  (irq < 0) {
-		SDLX_MSG(("%s: Host irq is not defined\n", __FUNCTION__));
-		return 1;
-	}
-#endif /* defined(OOB_INTR_ONLY) */
-	/* allocate SDIO Host Controller state info */
-	if (!(osh = osl_attach(dev, PCI_BUS, FALSE))) {
-		SDLX_MSG(("%s: osl_attach failed\n", __FUNCTION__));
-		goto err;
-	}
-	if (!(sdhc = MALLOC(osh, sizeof(bcmsdh_hc_t)))) {
-		SDLX_MSG(("%s: out of memory, allocated %d bytes\n",
-			__FUNCTION__,
-			MALLOCED(osh)));
-		goto err;
-	}
-	bzero(sdhc, sizeof(bcmsdh_hc_t));
-	sdhc->osh = osh;
-
-	sdhc->dev = (void *)dev;
-
-#ifdef BCMLXSDMMC
-	if (!(sdh = bcmsdh_attach(osh, (void *)0,
-	                          (void **)&regs, irq))) {
-		SDLX_MSG(("%s: bcmsdh_attach failed\n", __FUNCTION__));
-		goto err;
-	}
-#else
-	if (!(sdh = bcmsdh_attach(osh, (void *)r->start,
-	                          (void **)&regs, irq))) {
-		SDLX_MSG(("%s: bcmsdh_attach failed\n", __FUNCTION__));
-		goto err;
-	}
-#endif /* BCMLXSDMMC */
-	sdhc->sdh = sdh;
-	sdhc->oob_irq = irq;
-	sdhc->oob_flags = irq_flags;
-	sdhc->oob_irq_registered = FALSE;	/* to make sure.. */
-	sdhc->oob_irq_enable_flag = FALSE;
-#if defined(OOB_INTR_ONLY)
-	spin_lock_init(&sdhc->irq_lock);
-#endif
-
-	/* chain SDIO Host Controller info together */
-	sdhc->next = sdhcinfo;
-	sdhcinfo = sdhc;
-	/* Read the vendor/device ID from the CIS */
-	vendevid = bcmsdh_query_device(sdh);
-
-	/* try to attach to the target device */
-	if (!(sdhc->ch = drvinfo.attach((vendevid >> 16),
-	                                 (vendevid & 0xFFFF), 0, 0, 0, 0,
-	                                (void *)regs, NULL, sdh))) {
-		SDLX_MSG(("%s: device attach failed\n", __FUNCTION__));
-		goto err;
-	}
-
-	return 0;
-
-	/* error handling */
-err:
-	if (sdhc) {
-		if (sdhc->sdh)
-			bcmsdh_detach(sdhc->osh, sdhc->sdh);
-		MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
-	}
-	if (osh)
-		osl_detach(osh);
-	return -ENODEV;
-}
-
-#ifndef BCMLXSDMMC
-static
-#endif /* BCMLXSDMMC */
-int bcmsdh_remove(struct device *dev)
-{
-	bcmsdh_hc_t *sdhc, *prev;
-	osl_t *osh;
-
-	sdhc = sdhcinfo;
-	drvinfo.detach(sdhc->ch);
-	bcmsdh_detach(sdhc->osh, sdhc->sdh);
-	/* find the SDIO Host Controller state for this pdev and take it out from the list */
-	for (sdhc = sdhcinfo, prev = NULL; sdhc; sdhc = sdhc->next) {
-		if (sdhc->dev == (void *)dev) {
-			if (prev)
-				prev->next = sdhc->next;
-			else
-				sdhcinfo = NULL;
-			break;
-		}
-		prev = sdhc;
-	}
-	if (!sdhc) {
-		SDLX_MSG(("%s: failed\n", __FUNCTION__));
-		return 0;
-	}
-
-
-	/* release SDIO Host Controller info */
-	osh = sdhc->osh;
-	MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
-	osl_detach(osh);
-
-#if !defined(BCMLXSDMMC) || defined(OOB_INTR_ONLY)
-	dev_set_drvdata(dev, NULL);
-#endif /* !defined(BCMLXSDMMC) || defined(OOB_INTR_ONLY) */
-
-	return 0;
-}
-
-#else /* BCMPLATFORM_BUS */
-
-#if !defined(BCMLXSDMMC)
-/* forward declarations for PCI probe and remove functions. */
-static int __devinit bcmsdh_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent);
-static void __devexit bcmsdh_pci_remove(struct pci_dev *pdev);
-
-/**
- * pci id table
- */
-static struct pci_device_id bcmsdh_pci_devid[] __devinitdata = {
-	{ vendor: PCI_ANY_ID,
-	device: PCI_ANY_ID,
-	subvendor: PCI_ANY_ID,
-	subdevice: PCI_ANY_ID,
-	class: 0,
-	class_mask: 0,
-	driver_data: 0,
-	},
-	{ 0, }
-};
-MODULE_DEVICE_TABLE(pci, bcmsdh_pci_devid);
-
-/**
- * SDIO Host Controller pci driver info
- */
-static struct pci_driver bcmsdh_pci_driver = {
-	node:		{},
-	name:		"bcmsdh",
-	id_table:	bcmsdh_pci_devid,
-	probe:		bcmsdh_pci_probe,
-	remove:		bcmsdh_pci_remove,
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
-	save_state:	NULL,
-#endif
-	suspend:	NULL,
-	resume:		NULL,
-	};
-
-
-extern uint sd_pci_slot;	/* Force detection to a particular PCI */
-							/* slot only . Allows for having multiple */
-							/* WL devices at once in a PC */
-							/* Only one instance of dhd will be */
-							/* usable at a time */
-							/* Upper word is bus number, */
-							/* lower word is slot number */
-							/* Default value of 0xffffffff turns this */
-							/* off */
-module_param(sd_pci_slot, uint, 0);
-
-
-/**
- * Detect supported SDIO Host Controller and attach if found.
- *
- * Determine if the device described by pdev is a supported SDIO Host
- * Controller.  If so, attach to it and attach to the target device.
- */
-static int __devinit
-bcmsdh_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
-{
-	osl_t *osh = NULL;
-	bcmsdh_hc_t *sdhc = NULL;
-	ulong regs;
-	bcmsdh_info_t *sdh = NULL;
-	int rc;
-
-	if (sd_pci_slot != 0xFFFFffff) {
-		if (pdev->bus->number != (sd_pci_slot>>16) ||
-			PCI_SLOT(pdev->devfn) != (sd_pci_slot&0xffff)) {
-			SDLX_MSG(("%s: %s: bus %X, slot %X, vend %X, dev %X\n",
-				__FUNCTION__,
-				bcmsdh_chipmatch(pdev->vendor, pdev->device)
-				?"Found compatible SDIOHC"
-				:"Probing unknown device",
-				pdev->bus->number, PCI_SLOT(pdev->devfn), pdev->vendor,
-				pdev->device));
-			return -ENODEV;
-		}
-		SDLX_MSG(("%s: %s: bus %X, slot %X, vendor %X, device %X (good PCI location)\n",
-			__FUNCTION__,
-			bcmsdh_chipmatch(pdev->vendor, pdev->device)
-			?"Using compatible SDIOHC"
-			:"WARNING, forced use of unkown device",
-			pdev->bus->number, PCI_SLOT(pdev->devfn), pdev->vendor, pdev->device));
-	}
-
-	if ((pdev->vendor == VENDOR_TI) && ((pdev->device == PCIXX21_FLASHMEDIA_ID) ||
-	    (pdev->device == PCIXX21_FLASHMEDIA0_ID))) {
-		uint32 config_reg;
-
-		SDLX_MSG(("%s: Disabling TI FlashMedia Controller.\n", __FUNCTION__));
-		if (!(osh = osl_attach(pdev, PCI_BUS, FALSE))) {
-			SDLX_MSG(("%s: osl_attach failed\n", __FUNCTION__));
-			goto err;
-		}
-
-		config_reg = OSL_PCI_READ_CONFIG(osh, 0x4c, 4);
-
-		/*
-		 * Set MMC_SD_DIS bit in FlashMedia Controller.
-		 * Disbling the SD/MMC Controller in the FlashMedia Controller
-		 * allows the Standard SD Host Controller to take over control
-		 * of the SD Slot.
-		 */
-		config_reg |= 0x02;
-		OSL_PCI_WRITE_CONFIG(osh, 0x4c, 4, config_reg);
-		osl_detach(osh);
-	}
-	/* match this pci device with what we support */
-	/* we can't solely rely on this to believe it is our SDIO Host Controller! */
-	if (!bcmsdh_chipmatch(pdev->vendor, pdev->device)) {
-		return -ENODEV;
-	}
-
-	/* this is a pci device we might support */
-	SDLX_MSG(("%s: Found possible SDIO Host Controller: bus %d slot %d func %d irq %d\n",
-		__FUNCTION__,
-		pdev->bus->number, PCI_SLOT(pdev->devfn),
-		PCI_FUNC(pdev->devfn), pdev->irq));
-
-	/* use bcmsdh_query_device() to get the vendor ID of the target device so
-	 * it will eventually appear in the Broadcom string on the console
-	 */
-
-	/* allocate SDIO Host Controller state info */
-	if (!(osh = osl_attach(pdev, PCI_BUS, FALSE))) {
-		SDLX_MSG(("%s: osl_attach failed\n", __FUNCTION__));
-		goto err;
-	}
-	if (!(sdhc = MALLOC(osh, sizeof(bcmsdh_hc_t)))) {
-		SDLX_MSG(("%s: out of memory, allocated %d bytes\n",
-			__FUNCTION__,
-			MALLOCED(osh)));
-		goto err;
-	}
-	bzero(sdhc, sizeof(bcmsdh_hc_t));
-	sdhc->osh = osh;
-
-	sdhc->dev = pdev;
-
-	/* map to address where host can access */
-	pci_set_master(pdev);
-	rc = pci_enable_device(pdev);
-	if (rc) {
-		SDLX_MSG(("%s: Cannot enable PCI device\n", __FUNCTION__));
-		goto err;
-	}
-	if (!(sdh = bcmsdh_attach(osh, (void *)(uintptr)pci_resource_start(pdev, 0),
-	                          (void **)&regs, pdev->irq))) {
-		SDLX_MSG(("%s: bcmsdh_attach failed\n", __FUNCTION__));
-		goto err;
-	}
-
-	sdhc->sdh = sdh;
-
-	/* try to attach to the target device */
-	if (!(sdhc->ch = drvinfo.attach(VENDOR_BROADCOM, /* pdev->vendor, */
-	                                bcmsdh_query_device(sdh) & 0xFFFF, 0, 0, 0, 0,
-	                                (void *)regs, NULL, sdh))) {
-		SDLX_MSG(("%s: device attach failed\n", __FUNCTION__));
-		goto err;
-	}
-
-	/* chain SDIO Host Controller info together */
-	sdhc->next = sdhcinfo;
-	sdhcinfo = sdhc;
-
-	return 0;
-
-	/* error handling */
-err:
-	if (sdhc) {
-		if (sdhc->sdh)
-			bcmsdh_detach(sdhc->osh, sdhc->sdh);
-		MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
-	}
-	if (osh)
-		osl_detach(osh);
-	return -ENODEV;
-}
-
-
-/**
- * Detach from target devices and SDIO Host Controller
- */
-static void __devexit
-bcmsdh_pci_remove(struct pci_dev *pdev)
-{
-	bcmsdh_hc_t *sdhc, *prev;
-	osl_t *osh;
-
-	/* find the SDIO Host Controller state for this pdev and take it out from the list */
-	for (sdhc = sdhcinfo, prev = NULL; sdhc; sdhc = sdhc->next) {
-		if (sdhc->dev == pdev) {
-			if (prev)
-				prev->next = sdhc->next;
-			else
-				sdhcinfo = NULL;
-			break;
-		}
-		prev = sdhc;
-	}
-	if (!sdhc)
-		return;
-
-	drvinfo.detach(sdhc->ch);
-
-	bcmsdh_detach(sdhc->osh, sdhc->sdh);
-
-	/* release SDIO Host Controller info */
-	osh = sdhc->osh;
-	MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
-	osl_detach(osh);
-}
-#endif /* BCMLXSDMMC */
-#endif /* BCMPLATFORM_BUS */
-
-extern int sdio_function_init(void);
-
-int
-bcmsdh_register(bcmsdh_driver_t *driver)
-{
-	int error = 0;
-
-	drvinfo = *driver;
-
-#if defined(BCMPLATFORM_BUS)
-#if defined(BCMLXSDMMC)
-	SDLX_MSG(("Linux Kernel SDIO/MMC Driver\n"));
-	error = sdio_function_init();
-#else
-	SDLX_MSG(("Intel PXA270 SDIO Driver\n"));
-	error = driver_register(&bcmsdh_driver);
-#endif /* defined(BCMLXSDMMC) */
-	return error;
-#endif /* defined(BCMPLATFORM_BUS) */
-
-#if !defined(BCMPLATFORM_BUS) && !defined(BCMLXSDMMC)
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
-	if (!(error = pci_module_init(&bcmsdh_pci_driver)))
-		return 0;
-#else
-	if (!(error = pci_register_driver(&bcmsdh_pci_driver)))
-		return 0;
-#endif
-
-	SDLX_MSG(("%s: pci_module_init failed 0x%x\n", __FUNCTION__, error));
-#endif /* BCMPLATFORM_BUS */
-
-	return error;
-}
-
-extern void sdio_function_cleanup(void);
-
-void
-bcmsdh_unregister(void)
-{
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
-	if (bcmsdh_pci_driver.node.next)
-#endif
-
-#if defined(BCMPLATFORM_BUS) && !defined(BCMLXSDMMC)
-		driver_unregister(&bcmsdh_driver);
-#endif
-#if defined(BCMLXSDMMC)
-	sdio_function_cleanup();
-#endif /* BCMLXSDMMC */
-#if !defined(BCMPLATFORM_BUS) && !defined(BCMLXSDMMC)
-		pci_unregister_driver(&bcmsdh_pci_driver);
-#endif /* BCMPLATFORM_BUS */
-}
-
-#if defined(OOB_INTR_ONLY)
-void bcmsdh_oob_intr_set(bool enable)
-{
-	static bool curstate = 1;
-	unsigned long flags;
-
-	spin_lock_irqsave(&sdhcinfo->irq_lock, flags);
-	if (curstate != enable) {
-		if (enable)
-			enable_irq(sdhcinfo->oob_irq);
-		else
-			disable_irq_nosync(sdhcinfo->oob_irq);
-		curstate = enable;
-	}
-	spin_unlock_irqrestore(&sdhcinfo->irq_lock, flags);
-}
-
-static irqreturn_t wlan_oob_irq(int irq, void *dev_id)
-{
-	dhd_pub_t *dhdp;
-
-	dhdp = (dhd_pub_t *)dev_get_drvdata(sdhcinfo->dev);
-
-	bcmsdh_oob_intr_set(0);
-
-	if (dhdp == NULL) {
-		SDLX_MSG(("Out of band GPIO interrupt fired way too early\n"));
-		return IRQ_HANDLED;
-	}
-
-	dhdsdio_isr((void *)dhdp->bus);
-
-	return IRQ_HANDLED;
-}
-
-int bcmsdh_register_oob_intr(void * dhdp)
-{
-	int error = 0;
-
-	SDLX_MSG(("%s Enter \n", __FUNCTION__));
-
-	/* IORESOURCE_IRQ | IORESOURCE_IRQ_HIGHLEVEL | IORESOURCE_IRQ_SHAREABLE; */
-
-	dev_set_drvdata(sdhcinfo->dev, dhdp);
-
-	if (!sdhcinfo->oob_irq_registered) {
-		SDLX_MSG(("%s IRQ=%d Type=%X \n", __FUNCTION__,
-			(int)sdhcinfo->oob_irq, (int)sdhcinfo->oob_flags));
-		/* Refer to customer Host IRQ docs about proper irqflags definition */
-		error = request_irq(sdhcinfo->oob_irq, wlan_oob_irq, sdhcinfo->oob_flags,
-			"bcmsdh_sdmmc", NULL);
-		if (error)
-			return -ENODEV;
-
-		enable_irq_wake(sdhcinfo->oob_irq);
-		sdhcinfo->oob_irq_registered = TRUE;
-		sdhcinfo->oob_irq_enable_flag = TRUE;
-	}
-
-	return 0;
-}
-
-void bcmsdh_set_irq(int flag)
-{
-	if (sdhcinfo->oob_irq_registered && sdhcinfo->oob_irq_enable_flag != flag) {
-		SDLX_MSG(("%s Flag = %d", __FUNCTION__, flag));
-		sdhcinfo->oob_irq_enable_flag = flag;
-		if (flag) {
-			enable_irq(sdhcinfo->oob_irq);
-			enable_irq_wake(sdhcinfo->oob_irq);
-		} else {
-			disable_irq_wake(sdhcinfo->oob_irq);
-			disable_irq(sdhcinfo->oob_irq);
-		}
-	}
-}
-
-void bcmsdh_unregister_oob_intr(void)
-{
-	SDLX_MSG(("%s: Enter\n", __FUNCTION__));
-
-	if (sdhcinfo->oob_irq_registered == TRUE) {
-		bcmsdh_set_irq(FALSE);
-		free_irq(sdhcinfo->oob_irq, NULL);
-		sdhcinfo->oob_irq_registered = FALSE;
-	}
-}
-#endif /* defined(OOB_INTR_ONLY) */
-/* Module parameters specific to each host-controller driver */
-
-extern uint sd_msglevel;	/* Debug message level */
-module_param(sd_msglevel, uint, 0);
-
-extern uint sd_power;	/* 0 = SD Power OFF, 1 = SD Power ON. */
-module_param(sd_power, uint, 0);
-
-extern uint sd_clock;	/* SD Clock Control, 0 = SD Clock OFF, 1 = SD Clock ON */
-module_param(sd_clock, uint, 0);
-
-extern uint sd_divisor;	/* Divisor (-1 means external clock) */
-module_param(sd_divisor, uint, 0);
-
-extern uint sd_sdmode;	/* Default is SD4, 0=SPI, 1=SD1, 2=SD4 */
-module_param(sd_sdmode, uint, 0);
-
-extern uint sd_hiok;	/* Ok to use hi-speed mode */
-module_param(sd_hiok, uint, 0);
-
-extern uint sd_f2_blocksize;
-module_param(sd_f2_blocksize, int, 0);
-
-
-#ifdef BCMSDH_MODULE
-EXPORT_SYMBOL(bcmsdh_attach);
-EXPORT_SYMBOL(bcmsdh_detach);
-EXPORT_SYMBOL(bcmsdh_intr_query);
-EXPORT_SYMBOL(bcmsdh_intr_enable);
-EXPORT_SYMBOL(bcmsdh_intr_disable);
-EXPORT_SYMBOL(bcmsdh_intr_reg);
-EXPORT_SYMBOL(bcmsdh_intr_dereg);
-
-#if defined(DHD_DEBUG)
-EXPORT_SYMBOL(bcmsdh_intr_pending);
-#endif
-
-EXPORT_SYMBOL(bcmsdh_devremove_reg);
-EXPORT_SYMBOL(bcmsdh_cfg_read);
-EXPORT_SYMBOL(bcmsdh_cfg_write);
-EXPORT_SYMBOL(bcmsdh_cis_read);
-EXPORT_SYMBOL(bcmsdh_reg_read);
-EXPORT_SYMBOL(bcmsdh_reg_write);
-EXPORT_SYMBOL(bcmsdh_regfail);
-EXPORT_SYMBOL(bcmsdh_send_buf);
-EXPORT_SYMBOL(bcmsdh_recv_buf);
-
-EXPORT_SYMBOL(bcmsdh_rwdata);
-EXPORT_SYMBOL(bcmsdh_abort);
-EXPORT_SYMBOL(bcmsdh_query_device);
-EXPORT_SYMBOL(bcmsdh_query_iofnum);
-EXPORT_SYMBOL(bcmsdh_iovar_op);
-EXPORT_SYMBOL(bcmsdh_register);
-EXPORT_SYMBOL(bcmsdh_unregister);
-EXPORT_SYMBOL(bcmsdh_chipmatch);
-EXPORT_SYMBOL(bcmsdh_reset);
-EXPORT_SYMBOL(bcmsdh_waitlockfree);
-
-EXPORT_SYMBOL(bcmsdh_get_dstatus);
-EXPORT_SYMBOL(bcmsdh_cfg_read_word);
-EXPORT_SYMBOL(bcmsdh_cfg_write_word);
-EXPORT_SYMBOL(bcmsdh_cur_sbwad);
-EXPORT_SYMBOL(bcmsdh_chipinfo);
-
-#endif /* BCMSDH_MODULE */
diff --git a/drivers/net/wireless/bcmdhd/dhd.h b/drivers/net/wireless/bcmdhd/dhd.h
deleted file mode 100644
index 281e047af85..00000000000
--- a/drivers/net/wireless/bcmdhd/dhd.h
+++ /dev/null
@@ -1,700 +0,0 @@
-/*
- * Header file describing the internal (inter-module) DHD interfaces.
- *
- * Provides type definitions and function prototypes used to link the
- * DHD OS, bus, and protocol modules.
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: dhd.h,v 1.60.4.17 2011-01-09 08:11:56 Exp $
- */
-
-/****************
- * Common types *
- */
-
-#ifndef _dhd_h_
-#define _dhd_h_
-
-#if defined(CHROMIUMOS_COMPAT_WIRELESS)
-#include <linux/sched.h>
-#endif
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/skbuff.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/random.h>
-#include <linux/spinlock.h>
-#include <linux/ethtool.h>
-#include <asm/uaccess.h>
-#include <asm/unaligned.h>
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_HAS_WAKELOCK)
-#include <linux/wakelock.h>
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined (CONFIG_HAS_WAKELOCK) */
-/* The kernel threading is sdio-specific */
-
-#define ALL_INTERFACES	0xff
-
-#include <wlioctl.h>
-
-
-/* Forward decls */
-struct dhd_bus;
-struct dhd_prot;
-struct dhd_info;
-struct dhd_cmn;
-
-/* The level of bus communication with the dongle */
-enum dhd_bus_state {
-	DHD_BUS_DOWN,		/* Not ready for frame transfers */
-	DHD_BUS_LOAD,		/* Download access only (CPU reset) */
-	DHD_BUS_DATA		/* Ready for frame transfers */
-};
-
-/* Firmware requested operation mode */
-#define STA_MASK			0x0001
-#define HOSTAPD_MASK			0x0002
-#define WFD_MASK			0x0004
-#define SOFTAP_FW_MASK			0x0008
-
-enum dhd_bus_wake_state {
-	WAKE_LOCK_OFF,
-	WAKE_LOCK_PRIV,
-	WAKE_LOCK_DPC,
-	WAKE_LOCK_IOCTL,
-	WAKE_LOCK_DOWNLOAD,
-	WAKE_LOCK_TMOUT,
-	WAKE_LOCK_WATCHDOG,
-	WAKE_LOCK_LINK_DOWN_TMOUT,
-	WAKE_LOCK_PNO_FIND_TMOUT,
-	WAKE_LOCK_SOFTAP_SET,
-	WAKE_LOCK_SOFTAP_STOP,
-	WAKE_LOCK_SOFTAP_START,
-	WAKE_LOCK_SOFTAP_THREAD,
-	WAKE_LOCK_MAX
-};
-enum dhd_prealloc_index {
-	DHD_PREALLOC_PROT = 0,
-	DHD_PREALLOC_RXBUF,
-	DHD_PREALLOC_DATABUF,
-	DHD_PREALLOC_OSL_BUF
-};
-
-#if defined(DHD_USE_STATIC_BUF)
-
-uint8* dhd_os_prealloc(void *osh, int section, uint size);
-void dhd_os_prefree(void *osh, void *addr, uint size);
-#define DHD_OS_PREALLOC(osh, section, size) dhd_os_prealloc(osh, section, size)
-#define DHD_OS_PREFREE(osh, addr, size) dhd_os_prefree(osh, addr, size)
-
-#else
-
-#define DHD_OS_PREALLOC(osh, section, size) MALLOC(osh, size)
-#define DHD_OS_PREFREE(osh, addr, size) MFREE(osh, addr, size)
-
-#endif /* defined(DHD_USE_STATIC_BUF) */
-
-/* Common structure for module and instance linkage */
-typedef struct dhd_pub {
-	/* Linkage ponters */
-	osl_t *osh;		/* OSL handle */
-	struct dhd_bus *bus;	/* Bus module handle */
-	struct dhd_prot *prot;	/* Protocol module handle */
-	struct dhd_info  *info; /* Info module handle */
-	struct dhd_cmn	*cmn;	/* dhd_common module handle */
-
-	/* Internal dhd items */
-	bool up;		/* Driver up/down (to OS) */
-	bool txoff;		/* Transmit flow-controlled */
-	bool dongle_reset;  /* TRUE = DEVRESET put dongle into reset */
-	enum dhd_bus_state busstate;
-	uint hdrlen;		/* Total DHD header length (proto + bus) */
-	uint maxctl;		/* Max size rxctl request from proto to bus */
-	uint rxsz;		/* Rx buffer size bus module should use */
-	uint8 wme_dp;	/* wme discard priority */
-
-	/* Dongle media info */
-	bool iswl;		/* Dongle-resident driver is wl */
-	ulong drv_version;	/* Version of dongle-resident driver */
-	struct ether_addr mac;	/* MAC address obtained from dongle */
-	dngl_stats_t dstats;	/* Stats for dongle-based data */
-
-	/* Additional stats for the bus level */
-	ulong tx_packets;	/* Data packets sent to dongle */
-	ulong tx_multicast;	/* Multicast data packets sent to dongle */
-	ulong tx_errors;	/* Errors in sending data to dongle */
-	ulong tx_ctlpkts;	/* Control packets sent to dongle */
-	ulong tx_ctlerrs;	/* Errors sending control frames to dongle */
-	ulong rx_packets;	/* Packets sent up the network interface */
-	ulong rx_multicast;	/* Multicast packets sent up the network interface */
-	ulong rx_errors;	/* Errors processing rx data packets */
-	ulong rx_ctlpkts;	/* Control frames processed from dongle */
-	ulong rx_ctlerrs;	/* Errors in processing rx control frames */
-	ulong rx_dropped;	/* Packets dropped locally (no memory) */
-	ulong rx_flushed;  /* Packets flushed due to unscheduled sendup thread */
-	ulong wd_dpc_sched;   /* Number of times dhd dpc scheduled by watchdog timer */
-
-	ulong rx_readahead_cnt;	/* Number of packets where header read-ahead was used. */
-	ulong tx_realloc;	/* Number of tx packets we had to realloc for headroom */
-	ulong fc_packets;       /* Number of flow control pkts recvd */
-
-	/* Last error return */
-	int bcmerror;
-	uint tickcnt;
-
-	/* Last error from dongle */
-	int dongle_error;
-
-	/* Suspend disable flag and "in suspend" flag */
-	int suspend_disable_flag; /* "1" to disable all extra powersaving during suspend */
-	int in_suspend;			/* flag set to 1 when early suspend called */
-#ifdef PNO_SUPPORT
-	int pno_enable;                 /* pno status : "1" is pno enable */
-#endif /* PNO_SUPPORT */
-	int dtim_skip;         /* dtim skip , default 0 means wake each dtim */
-
-	/* Pkt filter defination */
-	char * pktfilter[100];
-	int pktfilter_count;
-
-	wl_country_t dhd_cspec;		/* Current Locale info */
-	char eventmask[WL_EVENTING_MASK_LEN];
-	int	op_mode;				/* STA, HostAPD, WFD, SoftAP */
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_HAS_WAKELOCK)
-	struct wake_lock 	wakelock[WAKE_LOCK_MAX];
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined (CONFIG_HAS_WAKELOCK) */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
-	struct mutex 	wl_start_stop_lock; /* lock/unlock for Android start/stop */
-	struct mutex 	wl_softap_lock;		 /* lock/unlock for any SoftAP/STA settings */
-#endif 
-
-	uint16	maxdatablks;
-#ifdef PROP_TXSTATUS
-	int   wlfc_enabled;
-	void* wlfc_state;
-#endif
-	bool	dongle_isolation;
-
-#ifdef WLMEDIA_HTSF
-	uint8 htsfdlystat_sz; /* Size of delay stats, max 255B */
-#endif
-} dhd_pub_t;
-
-typedef struct dhd_cmn {
-	osl_t *osh;		/* OSL handle */
-	dhd_pub_t *dhd;
-} dhd_cmn_t;
-
-
-	#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
-
-	#define DHD_PM_RESUME_WAIT_INIT(a) DECLARE_WAIT_QUEUE_HEAD(a);
-	#define _DHD_PM_RESUME_WAIT(a, b) do {\
-			int retry = 0; \
-			smp_mb(); \
-			while (dhd_mmc_suspend && retry++ != b) { \
-				wait_event_interruptible_timeout(a, FALSE, HZ/100); \
-			} \
-		} 	while (0)
-	#define DHD_PM_RESUME_WAIT(a) 		_DHD_PM_RESUME_WAIT(a, 200)
-	#define DHD_PM_RESUME_WAIT_FOREVER(a) 	_DHD_PM_RESUME_WAIT(a, ~0)
-	#define DHD_PM_RESUME_RETURN_ERROR(a)	do { if (dhd_mmc_suspend) return a; } while (0)
-	#define DHD_PM_RESUME_RETURN		do { if (dhd_mmc_suspend) return; } while (0)
-
-	#define DHD_SPINWAIT_SLEEP_INIT(a) DECLARE_WAIT_QUEUE_HEAD(a);
-	#define SPINWAIT_SLEEP(a, exp, us) do { \
-		uint countdown = (us) + 9999; \
-		while ((exp) && (countdown >= 10000)) { \
-			wait_event_interruptible_timeout(a, FALSE, HZ/100); \
-			countdown -= 10000; \
-		} \
-	} while (0)
-
-	#else
-
-	#define DHD_PM_RESUME_WAIT_INIT(a)
-	#define DHD_PM_RESUME_WAIT(a)
-	#define DHD_PM_RESUME_WAIT_FOREVER(a)
-	#define DHD_PM_RESUME_RETURN_ERROR(a)
-	#define DHD_PM_RESUME_RETURN
-
-	#define DHD_SPINWAIT_SLEEP_INIT(a)
-	#define SPINWAIT_SLEEP(a, exp, us)  do { \
-		uint countdown = (us) + 9; \
-		while ((exp) && (countdown >= 10)) { \
-			OSL_DELAY(10);  \
-			countdown -= 10;  \
-		} \
-	} while (0)
-
-	#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
-#ifndef DHDTHREAD
-#undef	SPINWAIT_SLEEP
-#define SPINWAIT_SLEEP(a, exp, us) SPINWAIT(exp, us)
-#endif /* DHDTHREAD */
-#define DHD_IF_VIF	0x01	/* Virtual IF (Hidden from user) */
-
-unsigned long dhd_os_spin_lock(dhd_pub_t *pub);
-void dhd_os_spin_unlock(dhd_pub_t *pub, unsigned long flags);
-
-/*  Wakelock Functions */
-extern int dhd_os_wake_lock(dhd_pub_t *pub);
-extern int dhd_os_wake_unlock(dhd_pub_t *pub);
-extern int dhd_os_wake_lock_timeout(dhd_pub_t *pub);
-extern int dhd_os_wake_lock_timeout_enable(dhd_pub_t *pub);
-
-inline static void MUTEX_LOCK_SOFTAP_SET_INIT(dhd_pub_t * dhdp)
-{
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
-	mutex_init(&dhdp->wl_softap_lock);
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
-}
-
-inline static void MUTEX_LOCK_SOFTAP_SET(dhd_pub_t * dhdp)
-{
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
-	mutex_lock(&dhdp->wl_softap_lock);
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
-}
-
-inline static void MUTEX_UNLOCK_SOFTAP_SET(dhd_pub_t * dhdp)
-{
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
-	mutex_unlock(&dhdp->wl_softap_lock);
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
-}
-
-#define DHD_OS_WAKE_LOCK(pub) 			dhd_os_wake_lock(pub)
-#define DHD_OS_WAKE_UNLOCK(pub) 		dhd_os_wake_unlock(pub)
-#define DHD_OS_WAKE_LOCK_TIMEOUT(pub)		dhd_os_wake_lock_timeout(pub)
-#define DHD_OS_WAKE_LOCK_TIMEOUT_ENABLE(pub)	dhd_os_wake_lock_timeout_enable(pub)
-
-
-/* interface operations (register, remove) should be atomic, use this lock to prevent race
- * condition among wifi on/off and interface operation functions
- */
-void dhd_net_if_lock(struct net_device *dev);
-void dhd_net_if_unlock(struct net_device *dev);
-
-typedef struct dhd_if_event {
-	uint8 ifidx;
-	uint8 action;
-	uint8 flags;
-	uint8 bssidx;
-	uint8 is_AP;
-} dhd_if_event_t;
-
-typedef enum dhd_attach_states
-{
-	DHD_ATTACH_STATE_INIT = 0x0,
-	DHD_ATTACH_STATE_NET_ALLOC = 0x1,
-	DHD_ATTACH_STATE_DHD_ALLOC = 0x2,
-	DHD_ATTACH_STATE_ADD_IF = 0x4,
-	DHD_ATTACH_STATE_PROT_ATTACH = 0x8,
-	DHD_ATTACH_STATE_WL_ATTACH = 0x10,
-	DHD_ATTACH_STATE_THREADS_CREATED = 0x20,
-	DHD_ATTACH_STATE_WAKELOCKS_INIT = 0x40,
-	DHD_ATTACH_STATE_CFG80211 = 0x80,
-	DHD_ATTACH_STATE_EARLYSUSPEND_DONE = 0x100,
-	DHD_ATTACH_STATE_DONE = 0x200
-} dhd_attach_states_t;
-
-/* Value -1 means we are unsuccessful in creating the kthread. */
-#define DHD_PID_KT_INVALID 	-1
-/* Value -2 means we are unsuccessful in both creating the kthread and tasklet */
-#define DHD_PID_KT_TL_INVALID	-2
-
-/*
- * Exported from dhd OS modules (dhd_linux/dhd_ndis)
- */
-
-/* To allow osl_attach/detach calls from os-independent modules */
-osl_t *dhd_osl_attach(void *pdev, uint bustype);
-void dhd_osl_detach(osl_t *osh);
-
-/* Indication from bus module regarding presence/insertion of dongle.
- * Return dhd_pub_t pointer, used as handle to OS module in later calls.
- * Returned structure should have bus and prot pointers filled in.
- * bus_hdrlen specifies required headroom for bus module header.
- */
-extern dhd_pub_t *dhd_attach(osl_t *osh, struct dhd_bus *bus, uint bus_hdrlen);
-extern int dhd_net_attach(dhd_pub_t *dhdp, int idx);
-
-/* Indication from bus module regarding removal/absence of dongle */
-extern void dhd_detach(dhd_pub_t *dhdp);
-extern void dhd_free(dhd_pub_t *dhdp);
-
-/* Indication from bus module to change flow-control state */
-extern void dhd_txflowcontrol(dhd_pub_t *dhdp, int ifidx, bool on);
-
-extern bool dhd_prec_enq(dhd_pub_t *dhdp, struct pktq *q, void *pkt, int prec);
-
-/* Receive frame for delivery to OS.  Callee disposes of rxp. */
-extern void dhd_rx_frame(dhd_pub_t *dhdp, int ifidx, void *rxp, int numpkt, uint8 chan);
-
-/* Return pointer to interface name */
-extern char *dhd_ifname(dhd_pub_t *dhdp, int idx);
-
-/* Request scheduling of the bus dpc */
-extern void dhd_sched_dpc(dhd_pub_t *dhdp);
-
-/* Notify tx completion */
-extern void dhd_txcomplete(dhd_pub_t *dhdp, void *txp, bool success);
-
-/* OS independent layer functions */
-extern int dhd_os_proto_block(dhd_pub_t * pub);
-extern int dhd_os_proto_unblock(dhd_pub_t * pub);
-extern int dhd_os_ioctl_resp_wait(dhd_pub_t * pub, uint * condition, bool * pending);
-extern int dhd_os_ioctl_resp_wake(dhd_pub_t * pub);
-extern unsigned int dhd_os_get_ioctl_resp_timeout(void);
-extern void dhd_os_set_ioctl_resp_timeout(unsigned int timeout_msec);
-extern void * dhd_os_open_image(char * filename);
-extern int dhd_os_get_image_block(char * buf, int len, void * image);
-extern void dhd_os_close_image(void * image);
-extern void dhd_os_wd_timer(void *bus, uint wdtick);
-extern void dhd_os_sdlock(dhd_pub_t * pub);
-extern void dhd_os_sdunlock(dhd_pub_t * pub);
-extern void dhd_os_sdlock_txq(dhd_pub_t * pub);
-extern void dhd_os_sdunlock_txq(dhd_pub_t * pub);
-extern void dhd_os_sdlock_rxq(dhd_pub_t * pub);
-extern void dhd_os_sdunlock_rxq(dhd_pub_t * pub);
-extern void dhd_os_sdlock_sndup_rxq(dhd_pub_t * pub);
-extern void dhd_customer_gpio_wlan_ctrl(int onoff);
-extern int  dhd_custom_get_mac_address(unsigned char *buf);
-extern void dhd_os_sdunlock_sndup_rxq(dhd_pub_t * pub);
-extern void dhd_os_sdlock_eventq(dhd_pub_t * pub);
-extern void dhd_os_sdunlock_eventq(dhd_pub_t * pub);
-extern int dhd_pno_enable(dhd_pub_t *dhd, int pfn_enabled);
-extern int dhd_pno_clean(dhd_pub_t *dhd);
-extern int dhd_pno_set(dhd_pub_t *dhd, wlc_ssid_t* ssids_local, int nssid,
-                       ushort  scan_fr, int pno_repeat, int pno_freq_expo_max);
-extern int dhd_pno_get_status(dhd_pub_t *dhd);
-extern int dhd_dev_pno_reset(struct net_device *dev);
-extern int dhd_dev_pno_set(struct net_device *dev, wlc_ssid_t* ssids_local,
-                           int nssid, ushort  scan_fr, int pno_repeat, int pno_freq_expo_max);
-extern int dhd_dev_pno_enable(struct net_device *dev,  int pfn_enabled);
-extern int dhd_dev_get_pno_status(struct net_device *dev);
-extern int dhd_get_dtim_skip(dhd_pub_t *dhd);
-
-#define DHD_UNICAST_FILTER_NUM		0
-#define DHD_BROADCAST_FILTER_NUM	1
-#define DHD_MULTICAST4_FILTER_NUM	2
-#define DHD_MULTICAST6_FILTER_NUM	3
-extern int net_os_set_packet_filter(struct net_device *dev, int val);
-extern int net_os_rxfilter_add_remove(struct net_device *dev, int val, int num);
-
-#ifdef DHD_DEBUG
-extern int write_to_file(dhd_pub_t *dhd, uint8 *buf, int size);
-#endif /* DHD_DEBUG */
-#if defined(OOB_INTR_ONLY)
-extern int dhd_customer_oob_irq_map(unsigned long *irq_flags_ptr);
-#endif /* defined(OOB_INTR_ONLY) */
-extern void dhd_os_sdtxlock(dhd_pub_t * pub);
-extern void dhd_os_sdtxunlock(dhd_pub_t * pub);
-
-#if defined(DHDTHREAD)
-struct task_struct;
-struct sched_param;
-int setScheduler(struct task_struct *p, int policy, struct sched_param *param);
-#endif /* DHDTHREAD && DHD_GPL */
-
-typedef struct {
-	uint32 limit;		/* Expiration time (usec) */
-	uint32 increment;	/* Current expiration increment (usec) */
-	uint32 elapsed;		/* Current elapsed time (usec) */
-	uint32 tick;		/* O/S tick time (usec) */
-} dhd_timeout_t;
-
-extern void dhd_timeout_start(dhd_timeout_t *tmo, uint usec);
-extern int dhd_timeout_expired(dhd_timeout_t *tmo);
-
-extern int dhd_ifname2idx(struct dhd_info *dhd, char *name);
-extern int dhd_net2idx(struct dhd_info *dhd, struct net_device *net);
-extern struct net_device * dhd_idx2net(struct dhd_pub *dhd_pub, int ifidx);
-extern int wl_host_event(dhd_pub_t *dhd_pub, int *idx, void *pktdata,
-                         wl_event_msg_t *, void **data_ptr);
-extern void wl_event_to_host_order(wl_event_msg_t * evt);
-
-extern int dhd_wl_ioctl(dhd_pub_t *dhd_pub, int ifindex, wl_ioctl_t *ioc, void *buf, int len);
-extern int dhd_wl_ioctl_cmd(dhd_pub_t *dhd_pub, int cmd, void *arg, int len, uint8 set,
-                            int ifindex);
-
-extern struct dhd_cmn *dhd_common_init(osl_t *osh);
-extern void dhd_common_deinit(dhd_pub_t *dhd_pub, dhd_cmn_t *sa_cmn);
-
-extern int dhd_add_if(struct dhd_info *dhd, int ifidx, void *handle,
-	char *name, uint8 *mac_addr, uint32 flags, uint8 bssidx);
-extern void dhd_del_if(struct dhd_info *dhd, int ifidx);
-
-extern void dhd_vif_add(struct dhd_info *dhd, int ifidx, char * name);
-extern void dhd_vif_del(struct dhd_info *dhd, int ifidx);
-
-extern void dhd_event(struct dhd_info *dhd, char *evpkt, int evlen, int ifidx);
-extern void dhd_vif_sendup(struct dhd_info *dhd, int ifidx, uchar *cp, int len);
-
-
-/* Send packet to dongle via data channel */
-extern int dhd_sendpkt(dhd_pub_t *dhdp, int ifidx, void *pkt);
-
-/* send up locally generated event */
-extern void dhd_sendup_event_common(dhd_pub_t *dhdp, wl_event_msg_t *event, void *data);
-/* Send event to host */
-extern void dhd_sendup_event(dhd_pub_t *dhdp, wl_event_msg_t *event, void *data);
-extern int dhd_bus_devreset(dhd_pub_t *dhdp, uint8 flag);
-extern uint dhd_bus_status(dhd_pub_t *dhdp);
-extern int  dhd_bus_start(dhd_pub_t *dhdp);
-extern int dhd_bus_membytes(dhd_pub_t *dhdp, bool set, uint32 address, uint8 *data, uint size);
-extern void dhd_print_buf(void *pbuf, int len, int bytes_per_line);
-
-
-typedef enum cust_gpio_modes {
-	WLAN_RESET_ON,
-	WLAN_RESET_OFF,
-	WLAN_POWER_ON,
-	WLAN_POWER_OFF
-} cust_gpio_modes_t;
-
-extern int wl_iw_iscan_set_scan_broadcast_prep(struct net_device *dev, uint flag);
-extern int wl_iw_send_priv_event(struct net_device *dev, char *flag);
-/*
- * Insmod parameters for debug/test
- */
-
-/* Watchdog timer interval */
-extern uint dhd_watchdog_ms;
-
-#if defined(DHD_DEBUG)
-/* Console output poll interval */
-extern uint dhd_console_ms;
-extern uint wl_msg_level;
-#endif /* defined(DHD_DEBUG) */
-
-/* Use interrupts */
-extern uint dhd_intr;
-
-/* Use polling */
-extern uint dhd_poll;
-
-/* ARP offload agent mode */
-extern uint dhd_arp_mode;
-
-/* ARP offload enable */
-extern uint dhd_arp_enable;
-
-/* Pkt filte enable control */
-extern uint dhd_pkt_filter_enable;
-
-/*  Pkt filter init setup */
-extern uint dhd_pkt_filter_init;
-
-/* Pkt filter mode control */
-extern uint dhd_master_mode;
-
-/* Roaming mode control */
-extern uint dhd_roam_disable;
-
-/* Roaming mode control */
-extern uint dhd_radio_up;
-
-/* Initial idletime ticks (may be -1 for immediate idle, 0 for no idle) */
-extern int dhd_idletime;
-#define DHD_IDLETIME_TICKS 1
-
-/* SDIO Drive Strength */
-extern uint dhd_sdiod_drive_strength;
-
-/* Override to force tx queueing all the time */
-extern uint dhd_force_tx_queueing;
-/* Default KEEP_ALIVE Period is 55 sec to prevent AP from sending Keep Alive probe frame */
-#define KEEP_ALIVE_PERIOD 55000
-#define NULL_PKT_STR	"null_pkt"
-
-#ifdef SDTEST
-/* Echo packet generator (SDIO), pkts/s */
-extern uint dhd_pktgen;
-
-/* Echo packet len (0 => sawtooth, max 1800) */
-extern uint dhd_pktgen_len;
-#define MAX_PKTGEN_LEN 1800
-#endif
-
-
-/* optionally set by a module_param_string() */
-#define MOD_PARAM_PATHLEN	2048
-extern char fw_path[MOD_PARAM_PATHLEN];
-extern char nv_path[MOD_PARAM_PATHLEN];
-
-#ifdef SOFTAP
-extern char fw_path2[MOD_PARAM_PATHLEN];
-#endif
-
-/* Flag to indicate if we should download firmware on driver load */
-extern uint dhd_download_fw_on_driverload;
-
-/* For supporting multiple interfaces */
-#define DHD_MAX_IFS	16
-#define DHD_DEL_IF	-0xe
-#define DHD_BAD_IF	-0xf
-
-#ifdef PROP_TXSTATUS
-/* Please be mindful that total pkttag space is 32 octets only */
-typedef struct dhd_pkttag {
-	/*
-	b[11 ] - 1 = this packet was sent in response to one time packet request,
-	do not increment credit on status for this one. [WLFC_CTL_TYPE_MAC_REQUEST_PACKET].
-	b[10 ] - 1 = signal-only-packet to firmware [i.e. nothing to piggyback on]
-	b[9  ] - 1 = packet is host->firmware (transmit direction)
-	       - 0 = packet received from firmware (firmware->host)
-	b[8  ] - 1 = packet was sent due to credit_request (pspoll),
-	             packet does not count against FIFO credit.
-	       - 0 = normal transaction, packet counts against FIFO credit
-	b[7  ] - 1 = AP, 0 = STA
-	b[6:4] - AC FIFO number
-	b[3:0] - interface index
-	*/
-	uint16	if_flags;
-	/* destination MAC address for this packet so that not every
-	module needs to open the packet to find this
-	*/
-	uint8	dstn_ether[ETHER_ADDR_LEN];
-	/*
-	This 32-bit goes from host to device for every packet.
-	*/
-	uint32	htod_tag;
-	/* bus specific stuff */
-	union {
-		struct {
-			void* stuff;
-			uint32 thing1;
-			uint32 thing2;
-		} sd;
-		struct {
-			void* bus;
-			void* urb;
-		} usb;
-	} bus_specific;
-} dhd_pkttag_t;
-
-#define DHD_PKTTAG_SET_H2DTAG(tag, h2dvalue)	((dhd_pkttag_t*)(tag))->htod_tag = (h2dvalue)
-#define DHD_PKTTAG_H2DTAG(tag)					(((dhd_pkttag_t*)(tag))->htod_tag)
-
-#define DHD_PKTTAG_IFMASK		0xf
-#define DHD_PKTTAG_IFTYPE_MASK	0x1
-#define DHD_PKTTAG_IFTYPE_SHIFT	7
-#define DHD_PKTTAG_FIFO_MASK	0x7
-#define DHD_PKTTAG_FIFO_SHIFT	4
-
-#define DHD_PKTTAG_SIGNALONLY_MASK			0x1
-#define DHD_PKTTAG_SIGNALONLY_SHIFT			10
-
-#define DHD_PKTTAG_ONETIMEPKTRQST_MASK		0x1
-#define DHD_PKTTAG_ONETIMEPKTRQST_SHIFT		11
-
-#define DHD_PKTTAG_PKTDIR_MASK			0x1
-#define DHD_PKTTAG_PKTDIR_SHIFT			9
-
-#define DHD_PKTTAG_CREDITCHECK_MASK		0x1
-#define DHD_PKTTAG_CREDITCHECK_SHIFT	8
-
-#define DHD_PKTTAG_INVALID_FIFOID 0x7
-
-#define DHD_PKTTAG_SETFIFO(tag, fifo)	((dhd_pkttag_t*)(tag))->if_flags = \
-	(((dhd_pkttag_t*)(tag))->if_flags & ~(DHD_PKTTAG_FIFO_MASK << DHD_PKTTAG_FIFO_SHIFT)) | \
-	(((fifo) & DHD_PKTTAG_FIFO_MASK) << DHD_PKTTAG_FIFO_SHIFT)
-#define DHD_PKTTAG_FIFO(tag)			((((dhd_pkttag_t*)(tag))->if_flags >> \
-	DHD_PKTTAG_FIFO_SHIFT) & DHD_PKTTAG_FIFO_MASK)
-
-#define DHD_PKTTAG_SETIF(tag, if)	((dhd_pkttag_t*)(tag))->if_flags = \
-	(((dhd_pkttag_t*)(tag))->if_flags & ~DHD_PKTTAG_IFMASK) | ((if) & DHD_PKTTAG_IFMASK)
-#define DHD_PKTTAG_IF(tag)	(((dhd_pkttag_t*)(tag))->if_flags & DHD_PKTTAG_IFMASK)
-
-#define DHD_PKTTAG_SETIFTYPE(tag, isAP)	((dhd_pkttag_t*)(tag))->if_flags = \
-	(((dhd_pkttag_t*)(tag))->if_flags & \
-	~(DHD_PKTTAG_IFTYPE_MASK << DHD_PKTTAG_IFTYPE_SHIFT)) | \
-	(((isAP) & DHD_PKTTAG_IFTYPE_MASK) << DHD_PKTTAG_IFTYPE_SHIFT)
-#define DHD_PKTTAG_IFTYPE(tag)	((((dhd_pkttag_t*)(tag))->if_flags >> \
-	DHD_PKTTAG_IFTYPE_SHIFT) & DHD_PKTTAG_IFTYPE_MASK)
-
-#define DHD_PKTTAG_SETCREDITCHECK(tag, check)	((dhd_pkttag_t*)(tag))->if_flags = \
-	(((dhd_pkttag_t*)(tag))->if_flags & \
-	~(DHD_PKTTAG_CREDITCHECK_MASK << DHD_PKTTAG_CREDITCHECK_SHIFT)) | \
-	(((check) & DHD_PKTTAG_CREDITCHECK_MASK) << DHD_PKTTAG_CREDITCHECK_SHIFT)
-#define DHD_PKTTAG_CREDITCHECK(tag)	((((dhd_pkttag_t*)(tag))->if_flags >> \
-	DHD_PKTTAG_CREDITCHECK_SHIFT) & DHD_PKTTAG_CREDITCHECK_MASK)
-
-#define DHD_PKTTAG_SETPKTDIR(tag, dir)	((dhd_pkttag_t*)(tag))->if_flags = \
-	(((dhd_pkttag_t*)(tag))->if_flags & \
-	~(DHD_PKTTAG_PKTDIR_MASK << DHD_PKTTAG_PKTDIR_SHIFT)) | \
-	(((dir) & DHD_PKTTAG_PKTDIR_MASK) << DHD_PKTTAG_PKTDIR_SHIFT)
-#define DHD_PKTTAG_PKTDIR(tag)	((((dhd_pkttag_t*)(tag))->if_flags >> \
-	DHD_PKTTAG_PKTDIR_SHIFT) & DHD_PKTTAG_PKTDIR_MASK)
-
-#define DHD_PKTTAG_SETSIGNALONLY(tag, signalonly)	((dhd_pkttag_t*)(tag))->if_flags = \
-	(((dhd_pkttag_t*)(tag))->if_flags & \
-	~(DHD_PKTTAG_SIGNALONLY_MASK << DHD_PKTTAG_SIGNALONLY_SHIFT)) | \
-	(((signalonly) & DHD_PKTTAG_SIGNALONLY_MASK) << DHD_PKTTAG_SIGNALONLY_SHIFT)
-#define DHD_PKTTAG_SIGNALONLY(tag)	((((dhd_pkttag_t*)(tag))->if_flags >> \
-	DHD_PKTTAG_SIGNALONLY_SHIFT) & DHD_PKTTAG_SIGNALONLY_MASK)
-
-#define DHD_PKTTAG_SETONETIMEPKTRQST(tag)	((dhd_pkttag_t*)(tag))->if_flags = \
-	(((dhd_pkttag_t*)(tag))->if_flags & \
-	~(DHD_PKTTAG_ONETIMEPKTRQST_MASK << DHD_PKTTAG_ONETIMEPKTRQST_SHIFT)) | \
-	(1 << DHD_PKTTAG_ONETIMEPKTRQST_SHIFT)
-#define DHD_PKTTAG_ONETIMEPKTRQST(tag)	((((dhd_pkttag_t*)(tag))->if_flags >> \
-	DHD_PKTTAG_ONETIMEPKTRQST_SHIFT) & DHD_PKTTAG_ONETIMEPKTRQST_MASK)
-
-#define DHD_PKTTAG_SETDSTN(tag, dstn_MAC_ea)	memcpy(((dhd_pkttag_t*)((tag)))->dstn_ether, \
-	(dstn_MAC_ea), ETHER_ADDR_LEN)
-#define DHD_PKTTAG_DSTN(tag)	((dhd_pkttag_t*)(tag))->dstn_ether
-
-typedef int (*f_commitpkt_t)(void* ctx, void* p);
-int dhd_wlfc_enable(dhd_pub_t *dhd);
-int dhd_wlfc_interface_event(struct dhd_info *, uint8 action, uint8 ifid, uint8 iftype, uint8* ea);
-int dhd_wlfc_FIFOcreditmap_event(struct dhd_info *dhd, uint8* event_data);
-int dhd_wlfc_event(struct dhd_info *dhd);
-int dhd_os_wlfc_block(dhd_pub_t *pub);
-int dhd_os_wlfc_unblock(dhd_pub_t *pub);
-
-#ifdef PROP_TXSTATUS_DEBUG
-#define DHD_WLFC_CTRINC_MAC_CLOSE(entry)	do { (entry)->closed_ct++; } while (0)
-#define DHD_WLFC_CTRINC_MAC_OPEN(entry)		do { (entry)->opened_ct++; } while (0)
-#else
-#define DHD_WLFC_CTRINC_MAC_CLOSE(entry)	do {} while (0)
-#define DHD_WLFC_CTRINC_MAC_OPEN(entry)		do {} while (0)
-#endif
-
-#endif /* PROP_TXSTATUS */
-
-extern void dhd_wait_for_event(dhd_pub_t *dhd, bool *lockvar);
-extern void dhd_wait_event_wakeup(dhd_pub_t*dhd);
-
-#ifdef ARP_OFFLOAD_SUPPORT
-#define MAX_IPV4_ENTRIES	8
-/* dhd_commn arp offload wrapers */
-void dhd_aoe_hostip_clr(dhd_pub_t *dhd);
-void dhd_aoe_arp_clr(dhd_pub_t *dhd);
-int dhd_arp_get_arp_hostip_table(dhd_pub_t *dhd, void *buf, int buflen);
-void dhd_arp_offload_add_ip(dhd_pub_t *dhd, uint32 ipaddr);
-#endif /* ARP_OFFLOAD_SUPPORT */
-#endif /* _dhd_h_ */
diff --git a/drivers/net/wireless/bcmdhd/dhd_bus.h b/drivers/net/wireless/bcmdhd/dhd_bus.h
deleted file mode 100644
index bccb8b6603f..00000000000
--- a/drivers/net/wireless/bcmdhd/dhd_bus.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * Header file describing the internal (inter-module) DHD interfaces.
- *
- * Provides type definitions and function prototypes used to link the
- * DHD OS, bus, and protocol modules.
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: dhd_bus.h,v 1.14.28.1 2010-12-23 01:13:17 Exp $
- */
-
-#ifndef _dhd_bus_h_
-#define _dhd_bus_h_
-
-/*
- * Exported from dhd bus module (dhd_usb, dhd_sdio)
- */
-
-/* Indicate (dis)interest in finding dongles. */
-extern int dhd_bus_register(void);
-extern void dhd_bus_unregister(void);
-
-/* Download firmware image and nvram image */
-extern bool dhd_bus_download_firmware(struct dhd_bus *bus, osl_t *osh,
-	char *fw_path, char *nv_path);
-
-/* Stop bus module: clear pending frames, disable data flow */
-extern void dhd_bus_stop(struct dhd_bus *bus, bool enforce_mutex);
-
-/* Initialize bus module: prepare for communication w/dongle */
-extern int dhd_bus_init(dhd_pub_t *dhdp, bool enforce_mutex);
-
-/* Get the Bus Idle Time */
-extern void dhd_bus_getidletime(dhd_pub_t *dhdp, int *idletime);
-
-/* Set the Bus Idle Time*/
-extern void dhd_bus_setidletime(dhd_pub_t *dhdp, int idle_time);
-/* Send a data frame to the dongle.  Callee disposes of txp. */
-extern int dhd_bus_txdata(struct dhd_bus *bus, void *txp);
-
-/* Send/receive a control message to/from the dongle.
- * Expects caller to enforce a single outstanding transaction.
- */
-extern int dhd_bus_txctl(struct dhd_bus *bus, uchar *msg, uint msglen);
-extern int dhd_bus_rxctl(struct dhd_bus *bus, uchar *msg, uint msglen);
-
-/* Watchdog timer function */
-extern bool dhd_bus_watchdog(dhd_pub_t *dhd);
-extern void dhd_disable_intr(dhd_pub_t *dhd);
-
-#if defined(DHD_DEBUG)
-/* Device console input function */
-extern int dhd_bus_console_in(dhd_pub_t *dhd, uchar *msg, uint msglen);
-#endif /* defined(DHD_DEBUG) */
-
-/* Deferred processing for the bus, return TRUE requests reschedule */
-extern bool dhd_bus_dpc(struct dhd_bus *bus);
-extern void dhd_bus_isr(bool * InterruptRecognized, bool * QueueMiniportHandleInterrupt, void *arg);
-
-
-/* Check for and handle local prot-specific iovar commands */
-extern int dhd_bus_iovar_op(dhd_pub_t *dhdp, const char *name,
-                            void *params, int plen, void *arg, int len, bool set);
-
-/* Add bus dump output to a buffer */
-extern void dhd_bus_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf);
-
-/* Clear any bus counters */
-extern void dhd_bus_clearcounts(dhd_pub_t *dhdp);
-
-/* return the dongle chipid */
-extern uint dhd_bus_chip(struct dhd_bus *bus);
-
-/* Set user-specified nvram parameters. */
-extern void dhd_bus_set_nvram_params(struct dhd_bus * bus, const char *nvram_params);
-
-extern void *dhd_bus_pub(struct dhd_bus *bus);
-extern void *dhd_bus_txq(struct dhd_bus *bus);
-extern uint dhd_bus_hdrlen(struct dhd_bus *bus);
-
-#endif /* _dhd_bus_h_ */
diff --git a/drivers/net/wireless/bcmdhd/dhd_cdc.c b/drivers/net/wireless/bcmdhd/dhd_cdc.c
deleted file mode 100644
index 0f9893a1537..00000000000
--- a/drivers/net/wireless/bcmdhd/dhd_cdc.c
+++ /dev/null
@@ -1,2278 +0,0 @@
-/*
- * DHD Protocol Module for CDC and BDC.
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: dhd_cdc.c,v 1.51.6.31 2011-02-09 14:31:43 Exp $
- *
- * BDC is like CDC, except it includes a header for data packets to convey
- * packet priority over the bus, and flags (e.g. to indicate checksum status
- * for dongle offload.)
- */
-
-#include <typedefs.h>
-#include <osl.h>
-
-#include <bcmutils.h>
-#include <bcmcdc.h>
-#include <bcmendian.h>
-
-#include <dngl_stats.h>
-#include <dhd.h>
-#include <dhd_proto.h>
-#include <dhd_bus.h>
-#include <dhd_dbg.h>
-
-
-#ifdef PROP_TXSTATUS
-#include <wlfc_proto.h>
-#include <dhd_wlfc.h>
-#endif
-
-/* Packet alignment for most efficient SDIO (can change based on platform) */
-#ifndef DHD_SDALIGN
-#define DHD_SDALIGN	32
-#endif
-#if !ISPOWEROF2(DHD_SDALIGN)
-#error DHD_SDALIGN is not a power of 2!
-#endif
-
-#define RETRIES 2		/* # of retries to retrieve matching ioctl response */
-#define BUS_HEADER_LEN	(16+DHD_SDALIGN)	/* Must be at least SDPCM_RESERVE
-				 * defined in dhd_sdio.c (amount of header tha might be added)
-				 * plus any space that might be needed for alignment padding.
-				 */
-#define ROUND_UP_MARGIN	2048 	/* Biggest SDIO block size possible for
-				 * round off at the end of buffer
-				 */
-
-typedef struct dhd_prot {
-	uint16 reqid;
-	uint8 pending;
-	uint32 lastcmd;
-	uint8 bus_header[BUS_HEADER_LEN];
-	cdc_ioctl_t msg;
-	unsigned char buf[WLC_IOCTL_MAXLEN + ROUND_UP_MARGIN];
-} dhd_prot_t;
-
-static int
-dhdcdc_msg(dhd_pub_t *dhd)
-{
-	int err = 0;
-	dhd_prot_t *prot = dhd->prot;
-	int len = ltoh32(prot->msg.len) + sizeof(cdc_ioctl_t);
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	DHD_OS_WAKE_LOCK(dhd);
-
-	/* NOTE : cdc->msg.len holds the desired length of the buffer to be
-	 *        returned. Only up to CDC_MAX_MSG_SIZE of this buffer area
-	 *	  is actually sent to the dongle
-	 */
-	if (len > CDC_MAX_MSG_SIZE)
-		len = CDC_MAX_MSG_SIZE;
-
-	/* Send request */
-	err = dhd_bus_txctl(dhd->bus, (uchar*)&prot->msg, len);
-
-	DHD_OS_WAKE_UNLOCK(dhd);
-	return err;
-}
-
-static int
-dhdcdc_cmplt(dhd_pub_t *dhd, uint32 id, uint32 len)
-{
-	int ret;
-	int cdc_len = len+sizeof(cdc_ioctl_t);
-	dhd_prot_t *prot = dhd->prot;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	do {
-		ret = dhd_bus_rxctl(dhd->bus, (uchar*)&prot->msg, cdc_len);
-		if (ret < 0)
-			break;
-	} while (CDC_IOC_ID(ltoh32(prot->msg.flags)) != id);
-
-	return ret;
-}
-
-static int
-dhdcdc_query_ioctl(dhd_pub_t *dhd, int ifidx, uint cmd, void *buf, uint len, uint8 action)
-{
-	dhd_prot_t *prot = dhd->prot;
-	cdc_ioctl_t *msg = &prot->msg;
-	void *info;
-	int ret = 0, retries = 0;
-	uint32 id, flags = 0;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-	DHD_CTL(("%s: cmd %d len %d\n", __FUNCTION__, cmd, len));
-
-
-	/* Respond "bcmerror" and "bcmerrorstr" with local cache */
-	if (cmd == WLC_GET_VAR && buf)
-	{
-		if (!strcmp((char *)buf, "bcmerrorstr"))
-		{
-			strncpy((char *)buf, bcmerrorstr(dhd->dongle_error), BCME_STRLEN);
-			goto done;
-		}
-		else if (!strcmp((char *)buf, "bcmerror"))
-		{
-			*(int *)buf = dhd->dongle_error;
-			goto done;
-		}
-	}
-
-	memset(msg, 0, sizeof(cdc_ioctl_t));
-
-	msg->cmd = htol32(cmd);
-	msg->len = htol32(len);
-	msg->flags = (++prot->reqid << CDCF_IOC_ID_SHIFT);
-	CDC_SET_IF_IDX(msg, ifidx);
-	/* add additional action bits */
-	action &= WL_IOCTL_ACTION_MASK;
-	msg->flags |= (action << CDCF_IOC_ACTION_SHIFT);
-	msg->flags = htol32(msg->flags);
-
-	if (buf)
-		memcpy(prot->buf, buf, len);
-
-	if ((ret = dhdcdc_msg(dhd)) < 0) {
-		DHD_ERROR(("dhdcdc_query_ioctl: dhdcdc_msg failed w/status %d\n", ret));
-		goto done;
-	}
-
-retry:
-	/* wait for interrupt and get first fragment */
-	if ((ret = dhdcdc_cmplt(dhd, prot->reqid, len)) < 0)
-		goto done;
-
-	flags = ltoh32(msg->flags);
-	id = (flags & CDCF_IOC_ID_MASK) >> CDCF_IOC_ID_SHIFT;
-
-	if ((id < prot->reqid) && (++retries < RETRIES))
-		goto retry;
-	if (id != prot->reqid) {
-		DHD_ERROR(("%s: %s: unexpected request id %d (expected %d)\n",
-		           dhd_ifname(dhd, ifidx), __FUNCTION__, id, prot->reqid));
-		ret = -EINVAL;
-		goto done;
-	}
-
-	/* Check info buffer */
-	info = (void*)&msg[1];
-
-	/* Copy info buffer */
-	if (buf)
-	{
-		if (ret < (int)len)
-			len = ret;
-		memcpy(buf, info, len);
-	}
-
-	/* Check the ERROR flag */
-	if (flags & CDCF_IOC_ERROR)
-	{
-		ret = ltoh32(msg->status);
-		/* Cache error from dongle */
-		dhd->dongle_error = ret;
-	}
-
-done:
-	return ret;
-}
-
-static int
-dhdcdc_set_ioctl(dhd_pub_t *dhd, int ifidx, uint cmd, void *buf, uint len, uint8 action)
-{
-	dhd_prot_t *prot = dhd->prot;
-	cdc_ioctl_t *msg = &prot->msg;
-	int ret = 0;
-	uint32 flags, id;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-	DHD_CTL(("%s: cmd %d len %d\n", __FUNCTION__, cmd, len));
-
-	memset(msg, 0, sizeof(cdc_ioctl_t));
-
-	msg->cmd = htol32(cmd);
-	msg->len = htol32(len);
-	msg->flags = (++prot->reqid << CDCF_IOC_ID_SHIFT);
-	CDC_SET_IF_IDX(msg, ifidx);
-	/* add additional action bits */
-	action &= WL_IOCTL_ACTION_MASK;
-	msg->flags |= (action << CDCF_IOC_ACTION_SHIFT) | CDCF_IOC_SET;
-	msg->flags = htol32(msg->flags);
-
-	if (buf)
-		memcpy(prot->buf, buf, len);
-
-	if ((ret = dhdcdc_msg(dhd)) < 0) {
-		DHD_ERROR(("%s: dhdcdc_msg failed w/status %d\n", __FUNCTION__, ret));
-		goto done;
-	}
-
-	if ((ret = dhdcdc_cmplt(dhd, prot->reqid, len)) < 0)
-		goto done;
-
-	flags = ltoh32(msg->flags);
-	id = (flags & CDCF_IOC_ID_MASK) >> CDCF_IOC_ID_SHIFT;
-
-	if (id != prot->reqid) {
-		DHD_ERROR(("%s: %s: unexpected request id %d (expected %d)\n",
-		           dhd_ifname(dhd, ifidx), __FUNCTION__, id, prot->reqid));
-		ret = -EINVAL;
-		goto done;
-	}
-
-	/* Check the ERROR flag */
-	if (flags & CDCF_IOC_ERROR)
-	{
-		ret = ltoh32(msg->status);
-		/* Cache error from dongle */
-		dhd->dongle_error = ret;
-	}
-
-done:
-	return ret;
-}
-
-
-int
-dhd_prot_ioctl(dhd_pub_t *dhd, int ifidx, wl_ioctl_t * ioc, void * buf, int len)
-{
-	dhd_prot_t *prot = dhd->prot;
-	int ret = -1;
-	uint8 action;
-
-	if (dhd->busstate == DHD_BUS_DOWN) {
-		DHD_ERROR(("%s : bus is down. we have nothing to do\n", __FUNCTION__));
-		goto done;
-	}
-
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	ASSERT(len <= WLC_IOCTL_MAXLEN);
-
-	if (len > WLC_IOCTL_MAXLEN)
-		goto done;
-
-	if (prot->pending == TRUE) {
-		DHD_ERROR(("CDC packet is pending!!!! cmd=0x%x (%lu) lastcmd=0x%x (%lu)\n",
-			ioc->cmd, (unsigned long)ioc->cmd, prot->lastcmd,
-			(unsigned long)prot->lastcmd));
-		if ((ioc->cmd == WLC_SET_VAR) || (ioc->cmd == WLC_GET_VAR)) {
-			DHD_TRACE(("iovar cmd=%s\n", (char*)buf));
-		}
-		goto done;
-	}
-
-	prot->pending = TRUE;
-	prot->lastcmd = ioc->cmd;
-	action = ioc->set;
-	if (action & WL_IOCTL_ACTION_SET)
-		ret = dhdcdc_set_ioctl(dhd, ifidx, ioc->cmd, buf, len, action);
-	else {
-		ret = dhdcdc_query_ioctl(dhd, ifidx, ioc->cmd, buf, len, action);
-		if (ret > 0)
-			ioc->used = ret - sizeof(cdc_ioctl_t);
-	}
-
-	/* Too many programs assume ioctl() returns 0 on success */
-	if (ret >= 0)
-		ret = 0;
-	else {
-		cdc_ioctl_t *msg = &prot->msg;
-		ioc->needed = ltoh32(msg->len); /* len == needed when set/query fails from dongle */
-	}
-
-	/* Intercept the wme_dp ioctl here */
-	if ((!ret) && (ioc->cmd == WLC_SET_VAR) && (!strcmp(buf, "wme_dp"))) {
-		int slen, val = 0;
-
-		slen = strlen("wme_dp") + 1;
-		if (len >= (int)(slen + sizeof(int)))
-			bcopy(((char *)buf + slen), &val, sizeof(int));
-		dhd->wme_dp = (uint8) ltoh32(val);
-	}
-
-	prot->pending = FALSE;
-
-done:
-	return ret;
-}
-
-int
-dhd_prot_iovar_op(dhd_pub_t *dhdp, const char *name,
-                  void *params, int plen, void *arg, int len, bool set)
-{
-	return BCME_UNSUPPORTED;
-}
-
-#ifdef PROP_TXSTATUS
-void
-dhd_wlfc_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf)
-{
-	int i;
-	uint8* ea;
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhdp->wlfc_state;
-	wlfc_hanger_t* h;
-	wlfc_mac_descriptor_t* mac_table;
-	wlfc_mac_descriptor_t* interfaces;
-	char* iftypes[] = {"STA", "AP", "WDS", "p2pGO", "p2pCL"};
-
-	if (wlfc == NULL) {
-		bcm_bprintf(strbuf, "wlfc not initialized yet\n");
-		return;
-	}
-	h = (wlfc_hanger_t*)wlfc->hanger;
-	if (h == NULL) {
-		bcm_bprintf(strbuf, "wlfc-hanger not initialized yet\n");
-	}
-
-	mac_table = wlfc->destination_entries.nodes;
-	interfaces = wlfc->destination_entries.interfaces;
-	bcm_bprintf(strbuf, "---- wlfc stats ----\n");
-	if (h) {
-		bcm_bprintf(strbuf, "wlfc hanger (pushed,popped,f_push,"
-			"f_pop,f_slot, pending) = (%d,%d,%d,%d,%d,%d)\n",
-			h->pushed,
-			h->popped,
-			h->failed_to_push,
-			h->failed_to_pop,
-			h->failed_slotfind,
-			(h->pushed - h->popped));
-	}
-
-	bcm_bprintf(strbuf, "wlfc fail(tlv,credit_rqst,mac_update,psmode_update), "
-		"(dq_full,sendq_full, rollback_fail) = (%d,%d,%d,%d), (%d,%d,%d)\n",
-		wlfc->stats.tlv_parse_failed,
-		wlfc->stats.credit_request_failed,
-		wlfc->stats.mac_update_failed,
-		wlfc->stats.psmode_update_failed,
-		wlfc->stats.delayq_full_error,
-		wlfc->stats.sendq_full_error,
-		wlfc->stats.rollback_failed);
-
-	bcm_bprintf(strbuf, "SENDQ (len,credit,sent) "
-		"(AC0[%d,%d,%d],AC1[%d,%d,%d],AC2[%d,%d,%d],AC3[%d,%d,%d],BC_MC[%d,%d,%d])\n",
-		wlfc->SENDQ.q[0].len, wlfc->FIFO_credit[0], wlfc->stats.sendq_pkts[0],
-		wlfc->SENDQ.q[1].len, wlfc->FIFO_credit[1], wlfc->stats.sendq_pkts[1],
-		wlfc->SENDQ.q[2].len, wlfc->FIFO_credit[2], wlfc->stats.sendq_pkts[2],
-		wlfc->SENDQ.q[3].len, wlfc->FIFO_credit[3], wlfc->stats.sendq_pkts[3],
-		wlfc->SENDQ.q[4].len, wlfc->FIFO_credit[4], wlfc->stats.sendq_pkts[4]);
-
-#ifdef PROP_TXSTATUS_DEBUG
-	bcm_bprintf(strbuf, "SENDQ dropped: AC[0-3]:(%d,%d,%d,%d), (bcmc,atim):(%d,%d)\n",
-		wlfc->stats.dropped_qfull[0], wlfc->stats.dropped_qfull[1],
-		wlfc->stats.dropped_qfull[2], wlfc->stats.dropped_qfull[3],
-		wlfc->stats.dropped_qfull[4], wlfc->stats.dropped_qfull[5]);
-#endif
-
-	bcm_bprintf(strbuf, "\n");
-	for (i = 0; i < WLFC_MAX_IFNUM; i++) {
-		if (interfaces[i].occupied) {
-			char* iftype_desc;
-
-			if (interfaces[i].iftype > WLC_E_IF_ROLE_P2P_CLIENT)
-				iftype_desc = "<Unknown";
-			else
-				iftype_desc = iftypes[interfaces[i].iftype];
-
-			ea = interfaces[i].ea;
-			bcm_bprintf(strbuf, "INTERFACE[%d].ea = "
-				"[%02x:%02x:%02x:%02x:%02x:%02x], if:%d, type: %s\n", i,
-				ea[0], ea[1], ea[2], ea[3], ea[4], ea[5],
-				interfaces[i].interface_id,
-				iftype_desc);
-
-			bcm_bprintf(strbuf, "INTERFACE[%d].DELAYQ(len,state,credit)"
-				"= (%d,%s,%d)\n",
-				i,
-				interfaces[i].psq.len,
-				((interfaces[i].state ==
-				WLFC_STATE_OPEN) ? " OPEN":"CLOSE"),
-				interfaces[i].requested_credit);
-
-			bcm_bprintf(strbuf, "INTERFACE[%d].DELAYQ"
-				"(sup,ac0),(sup,ac1),(sup,ac2),(sup,ac3) = "
-				"(%d,%d),(%d,%d),(%d,%d),(%d,%d)\n",
-				i,
-				interfaces[i].psq.q[0].len,
-				interfaces[i].psq.q[1].len,
-				interfaces[i].psq.q[2].len,
-				interfaces[i].psq.q[3].len,
-				interfaces[i].psq.q[4].len,
-				interfaces[i].psq.q[5].len,
-				interfaces[i].psq.q[6].len,
-				interfaces[i].psq.q[7].len);
-		}
-	}
-
-	bcm_bprintf(strbuf, "\n");
-	for (i = 0; i < WLFC_MAC_DESC_TABLE_SIZE; i++) {
-		if (mac_table[i].occupied) {
-			ea = mac_table[i].ea;
-			bcm_bprintf(strbuf, "MAC_table[%d].ea = "
-				"[%02x:%02x:%02x:%02x:%02x:%02x], if:%d\n", i,
-				ea[0], ea[1], ea[2], ea[3], ea[4], ea[5],
-				mac_table[i].interface_id);
-
-			bcm_bprintf(strbuf, "MAC_table[%d].DELAYQ(len,state,credit)"
-				"= (%d,%s,%d)\n",
-				i,
-				mac_table[i].psq.len,
-				((mac_table[i].state ==
-				WLFC_STATE_OPEN) ? " OPEN":"CLOSE"),
-				mac_table[i].requested_credit);
-#ifdef PROP_TXSTATUS_DEBUG
-			bcm_bprintf(strbuf, "MAC_table[%d]: (opened, closed) = (%d, %d)\n",
-				i, mac_table[i].opened_ct, mac_table[i].closed_ct);
-#endif
-			bcm_bprintf(strbuf, "MAC_table[%d].DELAYQ"
-				"(sup,ac0),(sup,ac1),(sup,ac2),(sup,ac3) = "
-				"(%d,%d),(%d,%d),(%d,%d),(%d,%d)\n",
-				i,
-				mac_table[i].psq.q[0].len,
-				mac_table[i].psq.q[1].len,
-				mac_table[i].psq.q[2].len,
-				mac_table[i].psq.q[3].len,
-				mac_table[i].psq.q[4].len,
-				mac_table[i].psq.q[5].len,
-				mac_table[i].psq.q[6].len,
-				mac_table[i].psq.q[7].len);
-		}
-	}
-
-#ifdef PROP_TXSTATUS_DEBUG
-	{
-		int avg;
-		int moving_avg = 0;
-		int moving_samples;
-
-		if (wlfc->stats.latency_sample_count) {
-			moving_samples = sizeof(wlfc->stats.deltas)/sizeof(uint32);
-
-			for (i = 0; i < moving_samples; i++)
-				moving_avg += wlfc->stats.deltas[i];
-			moving_avg /= moving_samples;
-
-			avg = (100 * wlfc->stats.total_status_latency) /
-				wlfc->stats.latency_sample_count;
-			bcm_bprintf(strbuf, "txstatus latency (average, last, moving[%d]) = "
-				"(%d.%d, %03d, %03d)\n",
-				moving_samples, avg/100, (avg - (avg/100)*100),
-				wlfc->stats.latency_most_recent,
-				moving_avg);
-		}
-	}
-
-	bcm_bprintf(strbuf, "wlfc- fifo[0-5] credit stats: sent = (%d,%d,%d,%d,%d,%d), "
-		"back = (%d,%d,%d,%d,%d,%d)\n",
-		wlfc->stats.fifo_credits_sent[0],
-		wlfc->stats.fifo_credits_sent[1],
-		wlfc->stats.fifo_credits_sent[2],
-		wlfc->stats.fifo_credits_sent[3],
-		wlfc->stats.fifo_credits_sent[4],
-		wlfc->stats.fifo_credits_sent[5],
-
-		wlfc->stats.fifo_credits_back[0],
-		wlfc->stats.fifo_credits_back[1],
-		wlfc->stats.fifo_credits_back[2],
-		wlfc->stats.fifo_credits_back[3],
-		wlfc->stats.fifo_credits_back[4],
-		wlfc->stats.fifo_credits_back[5]);
-	{
-		uint32 fifo_cr_sent = 0;
-		uint32 fifo_cr_acked = 0;
-		uint32 request_cr_sent = 0;
-		uint32 request_cr_ack = 0;
-		uint32 bc_mc_cr_ack = 0;
-
-		for (i = 0; i < sizeof(wlfc->stats.fifo_credits_sent)/sizeof(uint32); i++) {
-			fifo_cr_sent += wlfc->stats.fifo_credits_sent[i];
-		}
-
-		for (i = 0; i < sizeof(wlfc->stats.fifo_credits_back)/sizeof(uint32); i++) {
-			fifo_cr_acked += wlfc->stats.fifo_credits_back[i];
-		}
-
-		for (i = 0; i < WLFC_MAC_DESC_TABLE_SIZE; i++) {
-			if (wlfc->destination_entries.nodes[i].occupied) {
-				request_cr_sent +=
-					wlfc->destination_entries.nodes[i].dstncredit_sent_packets;
-			}
-		}
-		for (i = 0; i < WLFC_MAX_IFNUM; i++) {
-			if (wlfc->destination_entries.interfaces[i].occupied) {
-				request_cr_sent +=
-				wlfc->destination_entries.interfaces[i].dstncredit_sent_packets;
-			}
-		}
-		for (i = 0; i < WLFC_MAC_DESC_TABLE_SIZE; i++) {
-			if (wlfc->destination_entries.nodes[i].occupied) {
-				request_cr_ack +=
-					wlfc->destination_entries.nodes[i].dstncredit_acks;
-			}
-		}
-		for (i = 0; i < WLFC_MAX_IFNUM; i++) {
-			if (wlfc->destination_entries.interfaces[i].occupied) {
-				request_cr_ack +=
-					wlfc->destination_entries.interfaces[i].dstncredit_acks;
-			}
-		}
-		bcm_bprintf(strbuf, "wlfc- (sent, status) => pq(%d,%d), vq(%d,%d),"
-			"other:%d, bc_mc:%d, signal-only, (sent,freed): (%d,%d)",
-			fifo_cr_sent, fifo_cr_acked,
-			request_cr_sent, request_cr_ack,
-			wlfc->destination_entries.other.dstncredit_acks,
-			bc_mc_cr_ack,
-			wlfc->stats.signal_only_pkts_sent, wlfc->stats.signal_only_pkts_freed);
-	}
-#endif /* PROP_TXSTATUS_DEBUG */
-	bcm_bprintf(strbuf, "\n");
-	bcm_bprintf(strbuf, "wlfc- pkt((in,2bus,txstats,hdrpull),(dropped,hdr_only,wlc_tossed)"
-		"(freed,free_err,rollback)) = "
-		"((%d,%d,%d,%d),(%d,%d,%d),(%d,%d,%d))\n",
-		wlfc->stats.pktin,
-		wlfc->stats.pkt2bus,
-		wlfc->stats.txstatus_in,
-		wlfc->stats.dhd_hdrpulls,
-
-		wlfc->stats.pktdropped,
-		wlfc->stats.wlfc_header_only_pkt,
-		wlfc->stats.wlc_tossed_pkts,
-
-		wlfc->stats.pkt_freed,
-		wlfc->stats.pkt_free_err, wlfc->stats.rollback);
-
-	bcm_bprintf(strbuf, "wlfc- suppress((d11,wlc,err),enq(d11,wl,hq,mac?),retx(d11,wlc,hq)) = "
-		"((%d,%d,%d),(%d,%d,%d,%d),(%d,%d,%d))\n",
-
-		wlfc->stats.d11_suppress,
-		wlfc->stats.wl_suppress,
-		wlfc->stats.bad_suppress,
-
-		wlfc->stats.psq_d11sup_enq,
-		wlfc->stats.psq_wlsup_enq,
-		wlfc->stats.psq_hostq_enq,
-		wlfc->stats.mac_handle_notfound,
-
-		wlfc->stats.psq_d11sup_retx,
-		wlfc->stats.psq_wlsup_retx,
-		wlfc->stats.psq_hostq_retx);
-	return;
-}
-
-/* Create a place to store all packet pointers submitted to the firmware until 
-	a status comes back, suppress or otherwise.
-
-	hang-er: noun, a contrivance on which things are hung, as a hook.
-*/
-static void*
-dhd_wlfc_hanger_create(osl_t *osh, int max_items)
-{
-	int i;
-	wlfc_hanger_t* hanger;
-
-	/* allow only up to a specific size for now */
-	ASSERT(max_items == WLFC_HANGER_MAXITEMS);
-
-	if ((hanger = (wlfc_hanger_t*)MALLOC(osh, WLFC_HANGER_SIZE(max_items))) == NULL)
-		return NULL;
-
-	memset(hanger, 0, WLFC_HANGER_SIZE(max_items));
-	hanger->max_items = max_items;
-
-	for (i = 0; i < hanger->max_items; i++) {
-		hanger->items[i].state = WLFC_HANGER_ITEM_STATE_FREE;
-	}
-	return hanger;
-}
-
-static int
-dhd_wlfc_hanger_delete(osl_t *osh, void* hanger)
-{
-	wlfc_hanger_t* h = (wlfc_hanger_t*)hanger;
-
-	if (h) {
-		MFREE(osh, h, WLFC_HANGER_SIZE(h->max_items));
-		return BCME_OK;
-	}
-	return BCME_BADARG;
-}
-
-static uint16
-dhd_wlfc_hanger_get_free_slot(void* hanger)
-{
-	int i;
-	wlfc_hanger_t* h = (wlfc_hanger_t*)hanger;
-
-	if (h) {
-		for (i = 0; i < h->max_items; i++) {
-			if (h->items[i].state == WLFC_HANGER_ITEM_STATE_FREE)
-				return (uint16)i;
-		}
-	}
-	h->failed_slotfind++;
-	return WLFC_HANGER_MAXITEMS;
-}
-
-static int
-dhd_wlfc_hanger_pushpkt(void* hanger, void* pkt, uint32 slot_id)
-{
-	int rc = BCME_OK;
-	wlfc_hanger_t* h = (wlfc_hanger_t*)hanger;
-
-	if (h && (slot_id < WLFC_HANGER_MAXITEMS)) {
-		if (h->items[slot_id].state == WLFC_HANGER_ITEM_STATE_FREE) {
-			h->items[slot_id].state = WLFC_HANGER_ITEM_STATE_INUSE;
-			h->items[slot_id].pkt = pkt;
-			h->items[slot_id].identifier = slot_id;
-			h->pushed++;
-		}
-		else {
-			h->failed_to_push++;
-			rc = BCME_NOTFOUND;
-		}
-	}
-	else
-		rc = BCME_BADARG;
-	return rc;
-}
-
-static int
-dhd_wlfc_hanger_poppkt(void* hanger, uint32 slot_id, void** pktout, int remove_from_hanger)
-{
-	int rc = BCME_OK;
-	wlfc_hanger_t* h = (wlfc_hanger_t*)hanger;
-
-	/* this packet was not pushed at the time it went to the firmware */
-	if (slot_id == WLFC_HANGER_MAXITEMS)
-		return BCME_NOTFOUND;
-
-	if (h) {
-		if (h->items[slot_id].state == WLFC_HANGER_ITEM_STATE_INUSE) {
-			*pktout = h->items[slot_id].pkt;
-			if (remove_from_hanger) {
-				h->items[slot_id].state =
-					WLFC_HANGER_ITEM_STATE_FREE;
-				h->items[slot_id].pkt = NULL;
-				h->items[slot_id].identifier = 0;
-				h->popped++;
-			}
-		}
-		else {
-			h->failed_to_pop++;
-			rc = BCME_NOTFOUND;
-		}
-	}
-	else
-		rc = BCME_BADARG;
-	return rc;
-}
-
-static int
-_dhd_wlfc_pushheader(athost_wl_status_info_t* ctx, void* p, bool tim_signal,
-	uint8 tim_bmp, uint8 mac_handle, uint32 htodtag)
-{
-	uint32 wl_pktinfo = 0;
-	uint8* wlh;
-	uint8 dataOffset;
-	uint8 fillers;
-	uint8 tim_signal_len = 0;
-
-	struct bdc_header *h;
-
-	if (tim_signal) {
-		tim_signal_len = 1 + 1 + WLFC_CTL_VALUE_LEN_PENDING_TRAFFIC_BMP;
-	}
-
-	/* +2 is for Type[1] and Len[1] in TLV, plus TIM signal */
-	dataOffset = WLFC_CTL_VALUE_LEN_PKTTAG + 2 + tim_signal_len;
-	fillers = ROUNDUP(dataOffset, 4) - dataOffset;
-	dataOffset += fillers;
-
-	PKTPUSH(ctx->osh, p, dataOffset);
-	wlh = (uint8*) PKTDATA(ctx->osh, p);
-
-	wl_pktinfo = htol32(htodtag);
-
-	wlh[0] = WLFC_CTL_TYPE_PKTTAG;
-	wlh[1] = WLFC_CTL_VALUE_LEN_PKTTAG;
-	memcpy(&wlh[2], &wl_pktinfo, sizeof(uint32));
-
-	if (tim_signal_len) {
-		wlh[dataOffset - fillers - tim_signal_len ] =
-			WLFC_CTL_TYPE_PENDING_TRAFFIC_BMP;
-		wlh[dataOffset - fillers - tim_signal_len + 1] =
-			WLFC_CTL_VALUE_LEN_PENDING_TRAFFIC_BMP;
-		wlh[dataOffset - fillers - tim_signal_len + 2] = mac_handle;
-		wlh[dataOffset - fillers - tim_signal_len + 3] = tim_bmp;
-	}
-	if (fillers)
-		memset(&wlh[dataOffset - fillers], WLFC_CTL_TYPE_FILLER, fillers);
-
-	PKTPUSH(ctx->osh, p, BDC_HEADER_LEN);
-	h = (struct bdc_header *)PKTDATA(ctx->osh, p);
-	h->flags = (BDC_PROTO_VER << BDC_FLAG_VER_SHIFT);
-	if (PKTSUMNEEDED(p))
-		h->flags |= BDC_FLAG_SUM_NEEDED;
-
-
-	h->priority = (PKTPRIO(p) & BDC_PRIORITY_MASK);
-	h->flags2 = 0;
-	h->dataOffset = dataOffset >> 2;
-	BDC_SET_IF_IDX(h, DHD_PKTTAG_IF(PKTTAG(p)));
-	return BCME_OK;
-}
-
-static int
-_dhd_wlfc_pullheader(athost_wl_status_info_t* ctx, void* pktbuf)
-{
-	struct bdc_header *h;
-
-	if (PKTLEN(ctx->osh, pktbuf) < BDC_HEADER_LEN) {
-		WLFC_DBGMESG(("%s: rx data too short (%d < %d)\n", __FUNCTION__,
-		           PKTLEN(ctx->osh, pktbuf), BDC_HEADER_LEN));
-		return BCME_ERROR;
-	}
-	h = (struct bdc_header *)PKTDATA(ctx->osh, pktbuf);
-
-	/* pull BDC header */
-	PKTPULL(ctx->osh, pktbuf, BDC_HEADER_LEN);
-	/* pull wl-header */
-	PKTPULL(ctx->osh, pktbuf, (h->dataOffset << 2));
-	return BCME_OK;
-}
-
-static wlfc_mac_descriptor_t*
-_dhd_wlfc_find_table_entry(athost_wl_status_info_t* ctx, void* p)
-{
-	int i;
-	wlfc_mac_descriptor_t* table = ctx->destination_entries.nodes;
-	uint8 ifid = DHD_PKTTAG_IF(PKTTAG(p));
-	uint8* dstn = DHD_PKTTAG_DSTN(PKTTAG(p));
-
-	/* no lookup necessary, only if this packet belongs to STA interface */
-	if (((ctx->destination_entries.interfaces[ifid].iftype == WLC_E_IF_ROLE_STA) ||
-		ETHER_ISMULTI(dstn) ||
-		(ctx->destination_entries.interfaces[ifid].iftype == WLC_E_IF_ROLE_P2P_CLIENT)) &&
-		(ctx->destination_entries.interfaces[ifid].occupied)) {
-			return &ctx->destination_entries.interfaces[ifid];
-	}
-
-	for (i = 0; i < WLFC_MAC_DESC_TABLE_SIZE; i++) {
-		if (table[i].occupied) {
-			if (table[i].interface_id == ifid) {
-				if (!memcmp(table[i].ea, dstn, ETHER_ADDR_LEN))
-					return &table[i];
-			}
-		}
-	}
-	return &ctx->destination_entries.other;
-}
-
-static int
-_dhd_wlfc_rollback_packet_toq(athost_wl_status_info_t* ctx,
-	void* p, ewlfc_packet_state_t pkt_type, uint32 hslot)
-{
-	/*
-	put the packet back to the head of queue
-
-	- a packet from send-q will need to go back to send-q and not delay-q
-	since that will change the order of packets.
-	- suppressed packet goes back to suppress sub-queue
-	- pull out the header, if new or delayed packet
-
-	Note: hslot is used only when header removal is done.
-	*/
-	wlfc_mac_descriptor_t* entry;
-	void* pktout;
-	int rc = BCME_OK;
-	int prec;
-
-	entry = _dhd_wlfc_find_table_entry(ctx, p);
-	prec = DHD_PKTTAG_FIFO(PKTTAG(p));
-	if (entry != NULL) {
-		if (pkt_type == eWLFC_PKTTYPE_SUPPRESSED) {
-			/* wl-header is saved for suppressed packets */
-			if (WLFC_PKTQ_PENQ_HEAD(&entry->psq, ((prec << 1) + 1), p) == NULL) {
-				WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
-				rc = BCME_ERROR;
-			}
-		}
-		else {
-			/* remove header first */
-			_dhd_wlfc_pullheader(ctx, p);
-
-			if (pkt_type == eWLFC_PKTTYPE_DELAYED) {
-				/* delay-q packets are going to delay-q */
-				if (WLFC_PKTQ_PENQ_HEAD(&entry->psq, (prec << 1), p) == NULL) {
-					WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
-					rc = BCME_ERROR;
-				}
-			}
-			else {
-				/* these are going to SENDQ */
-				if (WLFC_PKTQ_PENQ_HEAD(&ctx->SENDQ, prec, p) == NULL) {
-					WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
-					rc = BCME_ERROR;
-				}
-			}
-			/* free the hanger slot */
-			dhd_wlfc_hanger_poppkt(ctx->hanger, hslot, &pktout, 1);
-
-			/* decrement sequence count */
-			WLFC_DECR_SEQCOUNT(entry, prec);
-		}
-		/*
-		if this packet did not count against FIFO credit, it must have
-		taken a requested_credit from the firmware (for pspoll etc.)
-		*/
-		if (!DHD_PKTTAG_CREDITCHECK(PKTTAG(p))) {
-			entry->requested_credit++;
-		}
-	}
-	else {
-		WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
-		rc = BCME_ERROR;
-	}
-	if (rc != BCME_OK)
-		ctx->stats.rollback_failed++;
-	else
-		ctx->stats.rollback++;
-
-	return rc;
-}
-
-static void
-_dhd_wlfc_flow_control_check(athost_wl_status_info_t* ctx, struct pktq* pq, uint8 if_id)
-{
-	if ((pq->len <= WLFC_FLOWCONTROL_LOWATER) && (ctx->hostif_flow_state[if_id] == ON)) {
-		/* start traffic */
-		ctx->hostif_flow_state[if_id] = OFF;
-		/*
-		WLFC_DBGMESG(("qlen:%02d, if:%02d, ->OFF, start traffic %s()\n",
-		pq->len, if_id, __FUNCTION__));
-		*/
-		WLFC_DBGMESG(("F"));
-		/* dhd_txflowcontrol(ctx->dhdp, if_id, OFF); */
-		ctx->toggle_host_if = 0;
-	}
-	if ((pq->len >= WLFC_FLOWCONTROL_HIWATER) && (ctx->hostif_flow_state[if_id] == OFF)) {
-		/* stop traffic */
-		ctx->hostif_flow_state[if_id] = ON;
-		/*
-		WLFC_DBGMESG(("qlen:%02d, if:%02d, ->ON, stop traffic   %s()\n",
-		pq->len, if_id, __FUNCTION__));
-		*/
-		WLFC_DBGMESG(("N"));
-		/* dhd_txflowcontrol(ctx->dhdp, if_id, ON); */
-		ctx->host_ifidx = if_id;
-		ctx->toggle_host_if = 1;
-	}
-	return;
-}
-
-static int
-_dhd_wlfc_send_signalonly_packet(athost_wl_status_info_t* ctx, wlfc_mac_descriptor_t* entry,
-	uint8 ta_bmp)
-{
-	int rc = BCME_OK;
-	void* p = NULL;
-	int dummylen = ((dhd_pub_t *)ctx->dhdp)->hdrlen+ 12;
-
-	/* allocate a dummy packet */
-	p = PKTGET(ctx->osh, dummylen, TRUE);
-	if (p) {
-		PKTPULL(ctx->osh, p, dummylen);
-		DHD_PKTTAG_SET_H2DTAG(PKTTAG(p), 0);
-		_dhd_wlfc_pushheader(ctx, p, TRUE, ta_bmp, entry->mac_handle, 0);
-		DHD_PKTTAG_SETSIGNALONLY(PKTTAG(p), 1);
-#ifdef PROP_TXSTATUS_DEBUG
-		ctx->stats.signal_only_pkts_sent++;
-#endif
-		rc = dhd_bus_txdata(((dhd_pub_t *)ctx->dhdp)->bus, p);
-		if (rc != BCME_OK) {
-			PKTFREE(ctx->osh, p, TRUE);
-		}
-	}
-	else {
-		DHD_ERROR(("%s: couldn't allocate new %d-byte packet\n",
-		           __FUNCTION__, dummylen));
-		rc = BCME_NOMEM;
-	}
-	return rc;
-}
-
-/* Return TRUE if traffic availability changed */
-static bool
-_dhd_wlfc_traffic_pending_check(athost_wl_status_info_t* ctx, wlfc_mac_descriptor_t* entry,
-	int prec)
-{
-	bool rc = FALSE;
-
-	if (entry->state == WLFC_STATE_CLOSE) {
-		if ((pktq_plen(&entry->psq, (prec << 1)) == 0) &&
-			(pktq_plen(&entry->psq, ((prec << 1) + 1)) == 0)) {
-
-			if (entry->traffic_pending_bmp & NBITVAL(prec)) {
-				rc = TRUE;
-				entry->traffic_pending_bmp =
-					entry->traffic_pending_bmp & ~ NBITVAL(prec);
-			}
-		}
-		else {
-			if (!(entry->traffic_pending_bmp & NBITVAL(prec))) {
-				rc = TRUE;
-				entry->traffic_pending_bmp =
-					entry->traffic_pending_bmp | NBITVAL(prec);
-			}
-		}
-	}
-	if (rc) {
-		/* request a TIM update to firmware at the next piggyback opportunity */
-		if (entry->traffic_lastreported_bmp != entry->traffic_pending_bmp) {
-			entry->send_tim_signal = 1;
-			_dhd_wlfc_send_signalonly_packet(ctx, entry, entry->traffic_pending_bmp);
-			entry->traffic_lastreported_bmp = entry->traffic_pending_bmp;
-			entry->send_tim_signal = 0;
-		}
-		else {
-			rc = FALSE;
-		}
-	}
-	return rc;
-}
-
-static int
-_dhd_wlfc_enque_suppressed(athost_wl_status_info_t* ctx, int prec, void* p)
-{
-	wlfc_mac_descriptor_t* entry;
-
-	entry = _dhd_wlfc_find_table_entry(ctx, p);
-	if (entry == NULL) {
-		WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
-		return BCME_NOTFOUND;
-	}
-	/*
-	- suppressed packets go to sub_queue[2*prec + 1] AND
-	- delayed packets go to sub_queue[2*prec + 0] to ensure
-	order of delivery.
-	*/
-	if (WLFC_PKTQ_PENQ(&entry->psq, ((prec << 1) + 1), p) == NULL) {
-		ctx->stats.delayq_full_error++;
-		/* WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__)); */
-		WLFC_DBGMESG(("s"));
-		return BCME_ERROR;
-	}
-	/* A packet has been pushed, update traffic availability bitmap, if applicable */
-	_dhd_wlfc_traffic_pending_check(ctx, entry, prec);
-	_dhd_wlfc_flow_control_check(ctx, &entry->psq, DHD_PKTTAG_IF(PKTTAG(p)));
-	return BCME_OK;
-}
-
-static int
-_dhd_wlfc_pretx_pktprocess(athost_wl_status_info_t* ctx,
-	wlfc_mac_descriptor_t* entry, void* p, int header_needed, uint32* slot)
-{
-	int rc = BCME_OK;
-	int hslot = WLFC_HANGER_MAXITEMS;
-	bool send_tim_update = FALSE;
-	uint32 htod = 0;
-	uint8 free_ctr;
-
-	*slot = hslot;
-
-	if (entry == NULL) {
-		entry = _dhd_wlfc_find_table_entry(ctx, p);
-	}
-
-	if (entry == NULL) {
-		WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
-		return BCME_ERROR;
-	}
-	if (entry->send_tim_signal) {
-		send_tim_update = TRUE;
-		entry->send_tim_signal = 0;
-		entry->traffic_lastreported_bmp = entry->traffic_pending_bmp;
-	}
-	if (header_needed) {
-		hslot = dhd_wlfc_hanger_get_free_slot(ctx->hanger);
-		free_ctr = WLFC_SEQCOUNT(entry, DHD_PKTTAG_FIFO(PKTTAG(p)));
-		DHD_PKTTAG_SET_H2DTAG(PKTTAG(p), htod);
-	}
-	else {
-		hslot = WLFC_PKTID_HSLOT_GET(DHD_PKTTAG_H2DTAG(PKTTAG(p)));
-		free_ctr = WLFC_PKTID_FREERUNCTR_GET(DHD_PKTTAG_H2DTAG(PKTTAG(p)));
-	}
-	WLFC_PKTID_HSLOT_SET(htod, hslot);
-	WLFC_PKTID_FREERUNCTR_SET(htod, free_ctr);
-	DHD_PKTTAG_SETPKTDIR(PKTTAG(p), 1);
-	WL_TXSTATUS_SET_FLAGS(htod, WLFC_PKTFLAG_PKTFROMHOST);
-	WL_TXSTATUS_SET_FIFO(htod, DHD_PKTTAG_FIFO(PKTTAG(p)));
-	WLFC_PKTFLAG_SET_GENERATION(htod, entry->generation);
-
-	if (!DHD_PKTTAG_CREDITCHECK(PKTTAG(p))) {
-		/*
-		Indicate that this packet is being sent in response to an
-		explicit request from the firmware side.
-		*/
-		WLFC_PKTFLAG_SET_PKTREQUESTED(htod);
-	}
-	else {
-		WLFC_PKTFLAG_CLR_PKTREQUESTED(htod);
-	}
-	if (header_needed) {
-		rc = _dhd_wlfc_pushheader(ctx, p, send_tim_update,
-			entry->traffic_lastreported_bmp, entry->mac_handle, htod);
-		if (rc == BCME_OK) {
-			DHD_PKTTAG_SET_H2DTAG(PKTTAG(p), htod);
-			/*
-			a new header was created for this packet.
-			push to hanger slot and scrub q. Since bus
-			send succeeded, increment seq number as well.
-			*/
-			rc = dhd_wlfc_hanger_pushpkt(ctx->hanger, p, hslot);
-			if (rc == BCME_OK) {
-				/* increment free running sequence count */
-				WLFC_INCR_SEQCOUNT(entry, DHD_PKTTAG_FIFO(PKTTAG(p)));
-#ifdef PROP_TXSTATUS_DEBUG
-				((wlfc_hanger_t*)(ctx->hanger))->items[hslot].push_time =
-					OSL_SYSUPTIME();
-#endif
-			}
-			else {
-				WLFC_DBGMESG(("%s() hanger_pushpkt() failed, rc: %d\n",
-					__FUNCTION__, rc));
-			}
-		}
-	}
-	else {
-		/* remove old header */
-		_dhd_wlfc_pullheader(ctx, p);
-
-		hslot = WLFC_PKTID_HSLOT_GET(DHD_PKTTAG_H2DTAG(PKTTAG(p)));
-		free_ctr = WLFC_PKTID_FREERUNCTR_GET(DHD_PKTTAG_H2DTAG(PKTTAG(p)));
-		/* push new header */
-		_dhd_wlfc_pushheader(ctx, p, send_tim_update,
-			entry->traffic_lastreported_bmp, entry->mac_handle, htod);
-	}
-	*slot = hslot;
-	return rc;
-}
-
-static int
-_dhd_wlfc_is_destination_closed(athost_wl_status_info_t* ctx,
-	wlfc_mac_descriptor_t* entry, int prec)
-{
-	if (ctx->destination_entries.interfaces[entry->interface_id].iftype ==
-		WLC_E_IF_ROLE_P2P_GO) {
-		/* - destination interface is of type p2p GO.
-		For a p2pGO interface, if the destination is OPEN but the interface is
-		CLOSEd, do not send traffic. But if the dstn is CLOSEd while there is
-		destination-specific-credit left send packets. This is because the
-		firmware storing the destination-specific-requested packet in queue.
-		*/
-		if ((entry->state == WLFC_STATE_CLOSE) && (entry->requested_credit == 0) &&
-			(entry->requested_packet == 0))
-			return 1;
-	}
-	/* AP, p2p_go -> unicast desc entry, STA/p2p_cl -> interface desc. entry */
-	if (((entry->state == WLFC_STATE_CLOSE) && (entry->requested_credit == 0) &&
-		(entry->requested_packet == 0)) ||
-		(!(entry->ac_bitmap & (1 << prec))))
-		return 1;
-
-	return 0;
-}
-
-static void*
-_dhd_wlfc_deque_delayedq(athost_wl_status_info_t* ctx,
-	int prec, uint8* ac_credit_spent, uint8* needs_hdr, wlfc_mac_descriptor_t** entry_out)
-{
-	wlfc_mac_descriptor_t* entry;
-	wlfc_mac_descriptor_t* table;
-	uint8 token_pos;
-	int total_entries;
-	void* p = NULL;
-	int pout;
-	int i;
-
-	*entry_out = NULL;
-	token_pos = ctx->token_pos[prec];
-	/* most cases a packet will count against FIFO credit */
-	*ac_credit_spent = 1;
-	*needs_hdr = 1;
-
-	/* search all entries, include nodes as well as interfaces */
-	table = (wlfc_mac_descriptor_t*)&ctx->destination_entries;
-	total_entries = sizeof(ctx->destination_entries)/sizeof(wlfc_mac_descriptor_t);
-
-	for (i = 0; i < total_entries; i++) {
-		entry = &table[(token_pos + i) % total_entries];
-		if (entry->occupied) {
-			if (!_dhd_wlfc_is_destination_closed(ctx, entry, prec)) {
-				p = pktq_mdeq(&entry->psq,
-					/* higher precedence will be picked up first,
-					i.e. suppressed packets before delayed ones
-					*/
-					(NBITVAL((prec << 1) + 1) | NBITVAL((prec << 1))),
-					&pout);
-				if (p != NULL) {
-					/* did the packet come from suppress sub-queue? */
-					if (pout == ((prec << 1) + 1)) {
-						/*
-						this packet was suppressed and was sent on the bus
-						previously; this already has a header
-						*/
-						*needs_hdr = 0;
-					}
-					if (entry->requested_credit > 0) {
-						entry->requested_credit--;
-#ifdef PROP_TXSTATUS_DEBUG
-						entry->dstncredit_sent_packets++;
-#endif
-						/*
-						if the packet was pulled out while destination is in
-						closed state but had a non-zero packets requested,
-						then this should not count against the FIFO credit.
-						That is due to the fact that the firmware will
-						most likely hold onto this packet until a suitable
-						time later to push it to the appropriate  AC FIFO.
-						*/
-						if (entry->state == WLFC_STATE_CLOSE)
-							*ac_credit_spent = 0;
-					}
-					else if (entry->requested_packet > 0) {
-						entry->requested_packet--;
-						DHD_PKTTAG_SETONETIMEPKTRQST(PKTTAG(p));
-						if (entry->state == WLFC_STATE_CLOSE)
-							*ac_credit_spent = 0;
-					}
-					/* move token to ensure fair round-robin */
-					ctx->token_pos[prec] =
-						(token_pos + i + 1) % total_entries;
-					*entry_out = entry;
-					_dhd_wlfc_flow_control_check(ctx, &entry->psq,
-						DHD_PKTTAG_IF(PKTTAG(p)));
-					/*
-					A packet has been picked up, update traffic
-					availability bitmap, if applicable
-					*/
-					_dhd_wlfc_traffic_pending_check(ctx, entry, prec);
-					return p;
-				}
-			}
-		}
-	}
-	return NULL;
-}
-
-static void*
-_dhd_wlfc_deque_sendq(athost_wl_status_info_t* ctx, int prec, uint8* ac_credit_spent)
-{
-	wlfc_mac_descriptor_t* entry;
-	void* p;
-
-	/* most cases a packet will count against FIFO credit */
-	*ac_credit_spent = 1;
-
-	p = pktq_pdeq(&ctx->SENDQ, prec);
-	if (p != NULL) {
-		if (ETHER_ISMULTI(DHD_PKTTAG_DSTN(PKTTAG(p))))
-			/* bc/mc packets do not have a delay queue */
-			return p;
-
-		entry = _dhd_wlfc_find_table_entry(ctx, p);
-
-		if (entry == NULL) {
-			WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
-			return p;
-		}
-
-		while ((p != NULL) && _dhd_wlfc_is_destination_closed(ctx, entry, prec)) {
-			/*
-			- suppressed packets go to sub_queue[2*prec + 1] AND
-			- delayed packets go to sub_queue[2*prec + 0] to ensure
-			order of delivery.
-			*/
-			if (WLFC_PKTQ_PENQ(&entry->psq, (prec << 1), p) == NULL) {
-				WLFC_DBGMESG(("D"));
-				/* dhd_txcomplete(ctx->dhdp, p, FALSE); */
-				PKTFREE(ctx->osh, p, TRUE);
-				ctx->stats.delayq_full_error++;
-			}
-			/*
-			A packet has been pushed, update traffic availability bitmap,
-			if applicable
-			*/
-			_dhd_wlfc_traffic_pending_check(ctx, entry, prec);
-			_dhd_wlfc_flow_control_check(ctx, &entry->psq, DHD_PKTTAG_IF(PKTTAG(p)));
-			p = pktq_pdeq(&ctx->SENDQ, prec);
-			if (p == NULL)
-				break;
-
-			entry = _dhd_wlfc_find_table_entry(ctx, p);
-
-			if ((entry == NULL) || (ETHER_ISMULTI(DHD_PKTTAG_DSTN(PKTTAG(p))))) {
-				return p;
-			}
-		}
-		if (p) {
-			if (entry->requested_packet == 0) {
-				if (entry->requested_credit > 0)
-					entry->requested_credit--;
-			}
-			else {
-				entry->requested_packet--;
-				DHD_PKTTAG_SETONETIMEPKTRQST(PKTTAG(p));
-			}
-			if (entry->state == WLFC_STATE_CLOSE)
-				*ac_credit_spent = 0;
-#ifdef PROP_TXSTATUS_DEBUG
-			entry->dstncredit_sent_packets++;
-#endif
-		}
-		if (p)
-			_dhd_wlfc_flow_control_check(ctx, &ctx->SENDQ, DHD_PKTTAG_IF(PKTTAG(p)));
-	}
-	return p;
-}
-
-static int
-_dhd_wlfc_mac_entry_update(athost_wl_status_info_t* ctx, wlfc_mac_descriptor_t* entry,
-	ewlfc_mac_entry_action_t action, uint8 ifid, uint8 iftype, uint8* ea)
-{
-	int rc = BCME_OK;
-
-	if (action == eWLFC_MAC_ENTRY_ACTION_ADD) {
-		entry->occupied = 1;
-		entry->state = WLFC_STATE_OPEN;
-		entry->requested_credit = 0;
-		entry->interface_id = ifid;
-		entry->iftype = iftype;
-		entry->ac_bitmap = 0xff; /* update this when handling APSD */
-		/* for an interface entry we may not care about the MAC address */
-		if (ea != NULL)
-			memcpy(&entry->ea[0], ea, ETHER_ADDR_LEN);
-		pktq_init(&entry->psq, WLFC_PSQ_PREC_COUNT, WLFC_PSQ_LEN);
-	}
-	else if (action == eWLFC_MAC_ENTRY_ACTION_DEL) {
-		entry->occupied = 0;
-		entry->state = WLFC_STATE_CLOSE;
-		entry->requested_credit = 0;
-		/* enable after packets are queued-deqeued properly.
-		pktq_flush(dhd->osh, &entry->psq, FALSE, NULL, 0);
-		*/
-	}
-	return rc;
-}
-
-int
-dhd_wlfc_interface_entry_update(void* state,
-	ewlfc_mac_entry_action_t action, uint8 ifid, uint8 iftype, uint8* ea)
-{
-	athost_wl_status_info_t* ctx = (athost_wl_status_info_t*)state;
-	wlfc_mac_descriptor_t* entry;
-
-	if (ifid >= WLFC_MAX_IFNUM)
-		return BCME_BADARG;
-
-	entry = &ctx->destination_entries.interfaces[ifid];
-	return _dhd_wlfc_mac_entry_update(ctx, entry, action, ifid, iftype, ea);
-}
-
-int
-dhd_wlfc_FIFOcreditmap_update(void* state, uint8* credits)
-{
-	athost_wl_status_info_t* ctx = (athost_wl_status_info_t*)state;
-
-	/* update the AC FIFO credit map */
-	ctx->FIFO_credit[0] = credits[0];
-	ctx->FIFO_credit[1] = credits[1];
-	ctx->FIFO_credit[2] = credits[2];
-	ctx->FIFO_credit[3] = credits[3];
-	/* credit for bc/mc packets */
-	ctx->FIFO_credit[4] = credits[4];
-	/* credit for ATIM FIFO is not used yet. */
-	return BCME_OK;
-}
-
-int
-dhd_wlfc_enque_sendq(void* state, int prec, void* p)
-{
-	athost_wl_status_info_t* ctx = (athost_wl_status_info_t*)state;
-
-	if ((state == NULL) ||
-		/* prec = AC_COUNT is used for bc/mc queue */
-		(prec > AC_COUNT) ||
-		(p == NULL)) {
-		WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
-		return BCME_BADARG;
-	}
-	if (FALSE == dhd_prec_enq(ctx->dhdp, &ctx->SENDQ, p, prec)) {
-		ctx->stats.sendq_full_error++;
-		/*
-		WLFC_DBGMESG(("Error: %s():%d, qlen:%d\n",
-		__FUNCTION__, __LINE__, ctx->SENDQ.len));
-		*/
-		WLFC_HOST_FIFO_DROPPEDCTR_INC(ctx, prec);
-		WLFC_DBGMESG(("Q"));
-		PKTFREE(ctx->osh, p, TRUE);
-		return BCME_ERROR;
-	}
-	ctx->stats.pktin++;
-	/* _dhd_wlfc_flow_control_check(ctx, &ctx->SENDQ, DHD_PKTTAG_IF(PKTTAG(p))); */
-	return BCME_OK;
-}
-
-int
-dhd_wlfc_commit_packets(void* state, f_commitpkt_t fcommit, void* commit_ctx)
-{
-	int ac;
-	int credit;
-	uint8 ac_fifo_credit_spent;
-	uint8 needs_hdr;
-	uint32 hslot;
-	void* p;
-	int rc;
-	athost_wl_status_info_t* ctx = (athost_wl_status_info_t*)state;
-	wlfc_mac_descriptor_t* mac_entry;
-
-	if ((state == NULL) ||
-		(fcommit == NULL)) {
-		WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
-		return BCME_BADARG;
-	}
-
-	/* 
-	Commit packets for regular AC traffic. Higher priority first.
-
-	-NOTE:
-	If the bus between the host and firmware is overwhelmed by the
-	traffic from host, it is possible that higher priority traffic
-	starves the lower priority queue. If that occurs often, we may
-	have to employ weighted round-robin or ucode scheme to avoid
-	low priority packet starvation.
-	*/
-	for (ac = AC_COUNT; ac >= 0; ac--) {
-		for (credit = 0; credit < ctx->FIFO_credit[ac];) {
-			p = _dhd_wlfc_deque_delayedq(ctx, ac, &ac_fifo_credit_spent, &needs_hdr,
-				&mac_entry);
-			if (p == NULL)
-				break;
-			/*
-			if ac_fifo_credit_spent = 0
-
-			This packet will not count against the FIFO credit.
-			To ensure the txstatus corresponding to this packet
-			does not provide an implied credit (default behavior)
-			mark the packet accordingly.
-
-			if ac_fifo_credit_spent = 1
-
-			This is a normal packet and it counts against the FIFO
-			credit count.
-			*/
-			DHD_PKTTAG_SETCREDITCHECK(PKTTAG(p), ac_fifo_credit_spent);
-			rc = _dhd_wlfc_pretx_pktprocess(ctx, mac_entry, p, needs_hdr, &hslot);
-
-			if (rc == BCME_OK)
-				rc = fcommit(commit_ctx, p);
-			else
-				ctx->stats.generic_error++;
-
-			if (rc == BCME_OK) {
-				ctx->stats.pkt2bus++;
-				if (ac_fifo_credit_spent) {
-					ctx->stats.sendq_pkts[ac]++;
-					WLFC_HOST_FIFO_CREDIT_INC_SENTCTRS(ctx, ac);
-					/*
-					1 FIFO credit has been spent by sending this packet
-					to the device.
-					*/
-					credit++;
-				}
-			}
-			else {
-				/* bus commit has failed, rollback. */
-				rc = _dhd_wlfc_rollback_packet_toq(ctx,
-					p,
-					/*
-					- remove wl-header for a delayed packet
-					- save wl-header header for suppressed packets
-					*/
-					(needs_hdr ? eWLFC_PKTTYPE_DELAYED :
-					eWLFC_PKTTYPE_SUPPRESSED),
-					hslot);
-				if (rc != BCME_OK)
-					ctx->stats.rollback_failed++;
-			}
-		}
-		ctx->FIFO_credit[ac] -= credit;
-		/* packets from SENDQ are fresh and they'd need header */
-		needs_hdr = 1;
-		for (credit = 0; credit < ctx->FIFO_credit[ac];) {
-			p = _dhd_wlfc_deque_sendq(ctx, ac, &ac_fifo_credit_spent);
-			if (p == NULL)
-				break;
-
-			DHD_PKTTAG_SETCREDITCHECK(PKTTAG(p), ac_fifo_credit_spent);
-			rc = _dhd_wlfc_pretx_pktprocess(ctx, NULL, p, needs_hdr, &hslot);
-			if (rc == BCME_OK)
-				rc = fcommit(commit_ctx, p);
-			else
-				ctx->stats.generic_error++;
-
-			if (rc == BCME_OK) {
-				ctx->stats.pkt2bus++;
-				if (ac_fifo_credit_spent) {
-					WLFC_HOST_FIFO_CREDIT_INC_SENTCTRS(ctx, ac);
-					ctx->stats.sendq_pkts[ac]++;
-					credit++;
-				}
-			}
-			else {
-				/* bus commit has failed, rollback. */
-				rc = _dhd_wlfc_rollback_packet_toq(ctx,
-					p,
-					/* remove wl-header while rolling back */
-					eWLFC_PKTTYPE_NEW,
-					hslot);
-				if (rc != BCME_OK)
-					ctx->stats.rollback_failed++;
-			}
-		}
-		ctx->FIFO_credit[ac] -= credit;
-	}
-	return BCME_OK;
-}
-
-static uint8
-dhd_wlfc_find_mac_desc_id_from_mac(dhd_pub_t *dhdp, uint8* ea)
-{
-	wlfc_mac_descriptor_t* table =
-		((athost_wl_status_info_t*)dhdp->wlfc_state)->destination_entries.nodes;
-	uint8 table_index;
-
-	if (ea != NULL) {
-		for (table_index = 0; table_index < WLFC_MAC_DESC_TABLE_SIZE; table_index++) {
-			if ((memcmp(ea, &table[table_index].ea[0], ETHER_ADDR_LEN) == 0) &&
-				table[table_index].occupied)
-				return table_index;
-		}
-	}
-	return WLFC_MAC_DESC_ID_INVALID;
-}
-
-void
-dhd_wlfc_txcomplete(dhd_pub_t *dhd, void *txp, bool success)
-{
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhd->wlfc_state;
-	void* p;
-
-	if (DHD_PKTTAG_SIGNALONLY(PKTTAG(txp))) {
-#ifdef PROP_TXSTATUS_DEBUG
-		wlfc->stats.signal_only_pkts_freed++;
-#endif
-		/* is this a signal-only packet? */
-		PKTFREE(wlfc->osh, txp, TRUE);
-		return;
-	}
-	if (!success) {
-		WLFC_DBGMESG(("At: %s():%d, bus_complete() failure for %p, htod_tag:0x%08x\n",
-			__FUNCTION__, __LINE__, txp, DHD_PKTTAG_H2DTAG(PKTTAG(txp))));
-		dhd_wlfc_hanger_poppkt(wlfc->hanger, WLFC_PKTID_HSLOT_GET(DHD_PKTTAG_H2DTAG
-			(PKTTAG(txp))), &p, 1);
-
-		/* indicate failure and free the packet */
-		dhd_txcomplete(dhd, txp, FALSE);
-		PKTFREE(wlfc->osh, txp, TRUE);
-
-		/* return the credit, if necessary */
-		if (DHD_PKTTAG_CREDITCHECK(PKTTAG(txp)))
-			wlfc->FIFO_credit[DHD_PKTTAG_FIFO(PKTTAG(txp))]++;
-	}
-	return;
-}
-
-/* Handle discard or suppress indication */
-static int
-dhd_wlfc_txstatus_update(dhd_pub_t *dhd, uint8* pkt_info)
-{
-	uint8 	status_flag;
-	uint32	status;
-	int		ret;
-	int		remove_from_hanger = 1;
-	void*	pktbuf;
-	uint8	fifo_id;
-	wlfc_mac_descriptor_t* entry = NULL;
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhd->wlfc_state;
-
-	memcpy(&status, pkt_info, sizeof(uint32));
-	status_flag = WL_TXSTATUS_GET_FLAGS(status);
-	wlfc->stats.txstatus_in++;
-
-	if (status_flag == WLFC_CTL_PKTFLAG_DISCARD) {
-		wlfc->stats.pkt_freed++;
-	}
-
-	else if (status_flag == WLFC_CTL_PKTFLAG_D11SUPPRESS) {
-		wlfc->stats.d11_suppress++;
-		remove_from_hanger = 0;
-	}
-
-	else if (status_flag == WLFC_CTL_PKTFLAG_WLSUPPRESS) {
-		wlfc->stats.wl_suppress++;
-		remove_from_hanger = 0;
-	}
-
-	else if (status_flag == WLFC_CTL_PKTFLAG_TOSSED_BYWLC) {
-		wlfc->stats.wlc_tossed_pkts++;
-	}
-
-	ret = dhd_wlfc_hanger_poppkt(wlfc->hanger,
-		WLFC_PKTID_HSLOT_GET(status), &pktbuf, remove_from_hanger);
-	if (ret != BCME_OK) {
-		/* do something */
-		return ret;
-	}
-
-	if (!remove_from_hanger) {
-		/* this packet was suppressed */
-
-		entry = _dhd_wlfc_find_table_entry(wlfc, pktbuf);
-		entry->generation = WLFC_PKTID_GEN(status);
-	}
-
-#ifdef PROP_TXSTATUS_DEBUG
-	{
-		uint32 new_t = OSL_SYSUPTIME();
-		uint32 old_t;
-		uint32 delta;
-		old_t = ((wlfc_hanger_t*)(wlfc->hanger))->items[
-			WLFC_PKTID_HSLOT_GET(status)].push_time;
-
-
-		wlfc->stats.latency_sample_count++;
-		if (new_t > old_t)
-			delta = new_t - old_t;
-		else
-			delta = 0xffffffff + new_t - old_t;
-		wlfc->stats.total_status_latency += delta;
-		wlfc->stats.latency_most_recent = delta;
-
-		wlfc->stats.deltas[wlfc->stats.idx_delta++] = delta;
-		if (wlfc->stats.idx_delta == sizeof(wlfc->stats.deltas)/sizeof(uint32))
-			wlfc->stats.idx_delta = 0;
-	}
-#endif /* PROP_TXSTATUS_DEBUG */
-
-	fifo_id = DHD_PKTTAG_FIFO(PKTTAG(pktbuf));
-
-	/* pick up the implicit credit from this packet */
-	if (DHD_PKTTAG_CREDITCHECK(PKTTAG(pktbuf))) {
-		if (wlfc->proptxstatus_mode == WLFC_FCMODE_IMPLIED_CREDIT) {
-			wlfc->FIFO_credit[fifo_id]++;
-		}
-	}
-	else {
-		/*
-		if this packet did not count against FIFO credit, it must have
-		taken a requested_credit from the destination entry (for pspoll etc.)
-		*/
-		if (!entry) {
-
-			entry = _dhd_wlfc_find_table_entry(wlfc, pktbuf);
-		}
-		if (!DHD_PKTTAG_ONETIMEPKTRQST(PKTTAG(pktbuf)))
-			entry->requested_credit++;
-#ifdef PROP_TXSTATUS_DEBUG
-		entry->dstncredit_acks++;
-#endif
-	}
-	if ((status_flag == WLFC_CTL_PKTFLAG_D11SUPPRESS) ||
-		(status_flag == WLFC_CTL_PKTFLAG_WLSUPPRESS)) {
-		ret = _dhd_wlfc_enque_suppressed(wlfc, fifo_id, pktbuf);
-		if (ret != BCME_OK) {
-			/* delay q is full, drop this packet */
-			dhd_wlfc_hanger_poppkt(wlfc->hanger, WLFC_PKTID_HSLOT_GET(status),
-			&pktbuf, 1);
-
-			/* indicate failure and free the packet */
-			dhd_txcomplete(dhd, pktbuf, FALSE);
-			PKTFREE(wlfc->osh, pktbuf, TRUE);
-		}
-	}
-	else {
-		dhd_txcomplete(dhd, pktbuf, TRUE);
-		/* free the packet */
-		PKTFREE(wlfc->osh, pktbuf, TRUE);
-	}
-	return BCME_OK;
-}
-
-static int
-dhd_wlfc_fifocreditback_indicate(dhd_pub_t *dhd, uint8* credits)
-{
-	int i;
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhd->wlfc_state;
-	for (i = 0; i < WLFC_CTL_VALUE_LEN_FIFO_CREDITBACK; i++) {
-#ifdef PROP_TXSTATUS_DEBUG
-		wlfc->stats.fifo_credits_back[i] += credits[i];
-#endif
-		/* update FIFO credits */
-		if (wlfc->proptxstatus_mode == WLFC_FCMODE_EXPLICIT_CREDIT)
-			wlfc->FIFO_credit[i] += credits[i];
-	}
-	return BCME_OK;
-}
-
-static int
-dhd_wlfc_rssi_indicate(dhd_pub_t *dhd, uint8* rssi)
-{
-	(void)dhd;
-	(void)rssi;
-	return BCME_OK;
-}
-
-static int
-dhd_wlfc_mac_table_update(dhd_pub_t *dhd, uint8* value, uint8 type)
-{
-	int rc;
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhd->wlfc_state;
-	wlfc_mac_descriptor_t* table;
-	uint8 existing_index;
-	uint8 table_index;
-	uint8 ifid;
-	uint8* ea;
-
-	WLFC_DBGMESG(("%s(), mac [%02x:%02x:%02x:%02x:%02x:%02x],%s,idx:%d,id:0x%02x\n",
-		__FUNCTION__, value[2], value[3], value[4], value[5], value[6], value[7],
-		((type == WLFC_CTL_TYPE_MACDESC_ADD) ? "ADD":"DEL"),
-		WLFC_MAC_DESC_GET_LOOKUP_INDEX(value[0]), value[0]));
-
-	table = wlfc->destination_entries.nodes;
-	table_index = WLFC_MAC_DESC_GET_LOOKUP_INDEX(value[0]);
-	ifid = value[1];
-	ea = &value[2];
-
-	if (type == WLFC_CTL_TYPE_MACDESC_ADD) {
-		existing_index = dhd_wlfc_find_mac_desc_id_from_mac(dhd, &value[2]);
-		if (existing_index == WLFC_MAC_DESC_ID_INVALID) {
-			/* this MAC entry does not exist, create one */
-			if (!table[table_index].occupied) {
-				table[table_index].mac_handle = value[0];
-				rc = _dhd_wlfc_mac_entry_update(wlfc, &table[table_index],
-				eWLFC_MAC_ENTRY_ACTION_ADD, ifid,
-				wlfc->destination_entries.interfaces[ifid].iftype,
-				ea);
-			}
-			else {
-				/* the space should have been empty, but it's not */
-				wlfc->stats.mac_update_failed++;
-			}
-		}
-		else {
-			/*
-			there is an existing entry, move it to new index
-			if necessary.
-			*/
-			if (existing_index != table_index) {
-				/* if we already have an entry, free the old one */
-				table[existing_index].occupied = 0;
-				table[existing_index].state = WLFC_STATE_CLOSE;
-				table[existing_index].requested_credit = 0;
-				table[existing_index].interface_id = 0;
-			}
-		}
-	}
-	if (type == WLFC_CTL_TYPE_MACDESC_DEL) {
-		if (table[table_index].occupied) {
-				rc = _dhd_wlfc_mac_entry_update(wlfc, &table[table_index],
-					eWLFC_MAC_ENTRY_ACTION_DEL, ifid,
-					wlfc->destination_entries.interfaces[ifid].iftype,
-					ea);
-		}
-		else {
-			/* the space should have been occupied, but it's not */
-			wlfc->stats.mac_update_failed++;
-		}
-	}
-	return BCME_OK;
-}
-
-static int
-dhd_wlfc_psmode_update(dhd_pub_t *dhd, uint8* value, uint8 type)
-{
-	/* Handle PS on/off indication */
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhd->wlfc_state;
-	wlfc_mac_descriptor_t* table;
-	wlfc_mac_descriptor_t* desc;
-	uint8 mac_handle = value[0];
-	int i;
-
-	table = wlfc->destination_entries.nodes;
-	desc = &table[WLFC_MAC_DESC_GET_LOOKUP_INDEX(mac_handle)];
-	if (desc->occupied) {
-		/* a fresh PS mode should wipe old ps credits? */
-		desc->requested_credit = 0;
-		if (type == WLFC_CTL_TYPE_MAC_OPEN) {
-			desc->state = WLFC_STATE_OPEN;
-			DHD_WLFC_CTRINC_MAC_OPEN(desc);
-		}
-		else {
-			desc->state = WLFC_STATE_CLOSE;
-			DHD_WLFC_CTRINC_MAC_CLOSE(desc);
-			/*
-			Indicate to firmware if there is any traffic pending.
-			*/
-			for (i = AC_BE; i < AC_COUNT; i++) {
-				_dhd_wlfc_traffic_pending_check(wlfc, desc, i);
-			}
-		}
-	}
-	else {
-		wlfc->stats.psmode_update_failed++;
-	}
-	return BCME_OK;
-}
-
-static int
-dhd_wlfc_interface_update(dhd_pub_t *dhd, uint8* value, uint8 type)
-{
-	/* Handle PS on/off indication */
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhd->wlfc_state;
-	wlfc_mac_descriptor_t* table;
-	uint8 if_id = value[0];
-
-	if (if_id < WLFC_MAX_IFNUM) {
-		table = wlfc->destination_entries.interfaces;
-		if (table[if_id].occupied) {
-			if (type == WLFC_CTL_TYPE_INTERFACE_OPEN) {
-				table[if_id].state = WLFC_STATE_OPEN;
-				/* WLFC_DBGMESG(("INTERFACE[%d] OPEN\n", if_id)); */
-			}
-			else {
-				table[if_id].state = WLFC_STATE_CLOSE;
-				/* WLFC_DBGMESG(("INTERFACE[%d] CLOSE\n", if_id)); */
-			}
-			return BCME_OK;
-		}
-	}
-	wlfc->stats.interface_update_failed++;
-
-	return BCME_OK;
-}
-
-static int
-dhd_wlfc_credit_request(dhd_pub_t *dhd, uint8* value)
-{
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhd->wlfc_state;
-	wlfc_mac_descriptor_t* table;
-	wlfc_mac_descriptor_t* desc;
-	uint8 mac_handle;
-	uint8 credit;
-
-	table = wlfc->destination_entries.nodes;
-	mac_handle = value[1];
-	credit = value[0];
-
-	desc = &table[WLFC_MAC_DESC_GET_LOOKUP_INDEX(mac_handle)];
-	if (desc->occupied) {
-		desc->requested_credit = credit;
-
-		desc->ac_bitmap = value[2];
-	}
-	else {
-		wlfc->stats.credit_request_failed++;
-	}
-	return BCME_OK;
-}
-
-static int
-dhd_wlfc_packet_request(dhd_pub_t *dhd, uint8* value)
-{
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhd->wlfc_state;
-	wlfc_mac_descriptor_t* table;
-	wlfc_mac_descriptor_t* desc;
-	uint8 mac_handle;
-	uint8 packet_count;
-
-	table = wlfc->destination_entries.nodes;
-	mac_handle = value[1];
-	packet_count = value[0];
-
-	desc = &table[WLFC_MAC_DESC_GET_LOOKUP_INDEX(mac_handle)];
-	if (desc->occupied) {
-		desc->requested_packet = packet_count;
-
-		desc->ac_bitmap = value[2];
-	}
-	else {
-		wlfc->stats.packet_request_failed++;
-	}
-	return BCME_OK;
-}
-
-static int
-dhd_wlfc_parse_header_info(dhd_pub_t *dhd, void* pktbuf, int tlv_hdr_len)
-{
-	uint8 type, len;
-	uint8* value;
-	uint8* tmpbuf;
-	uint16 remainder = tlv_hdr_len;
-	uint16 processed = 0;
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhd->wlfc_state;
-	tmpbuf = (uint8*)PKTDATA(dhd->osh, pktbuf);
-	if (remainder) {
-		while ((processed < (WLFC_MAX_PENDING_DATALEN * 2)) && (remainder > 0)) {
-			type = tmpbuf[processed];
-			if (type == WLFC_CTL_TYPE_FILLER) {
-				remainder -= 1;
-				processed += 1;
-				continue;
-			}
-
-			len  = tmpbuf[processed + 1];
-			value = &tmpbuf[processed + 2];
-
-			if (remainder < (2 + len))
-				break;
-
-			remainder -= 2 + len;
-			processed += 2 + len;
-			if (type == WLFC_CTL_TYPE_TXSTATUS)
-				dhd_wlfc_txstatus_update(dhd, value);
-
-			else if (type == WLFC_CTL_TYPE_FIFO_CREDITBACK)
-				dhd_wlfc_fifocreditback_indicate(dhd, value);
-
-			else if (type == WLFC_CTL_TYPE_RSSI)
-				dhd_wlfc_rssi_indicate(dhd, value);
-
-			else if (type == WLFC_CTL_TYPE_MAC_REQUEST_CREDIT)
-				dhd_wlfc_credit_request(dhd, value);
-
-			else if (type == WLFC_CTL_TYPE_MAC_REQUEST_PACKET)
-				dhd_wlfc_packet_request(dhd, value);
-
-			else if ((type == WLFC_CTL_TYPE_MAC_OPEN) ||
-				(type == WLFC_CTL_TYPE_MAC_CLOSE))
-				dhd_wlfc_psmode_update(dhd, value, type);
-
-			else if ((type == WLFC_CTL_TYPE_MACDESC_ADD) ||
-				(type == WLFC_CTL_TYPE_MACDESC_DEL))
-				dhd_wlfc_mac_table_update(dhd, value, type);
-
-			else if ((type == WLFC_CTL_TYPE_INTERFACE_OPEN) ||
-				(type == WLFC_CTL_TYPE_INTERFACE_CLOSE)) {
-				dhd_wlfc_interface_update(dhd, value, type);
-			}
-		}
-		if (remainder != 0) {
-			/* trouble..., something is not right */
-			wlfc->stats.tlv_parse_failed++;
-		}
-	}
-	return BCME_OK;
-}
-
-int
-dhd_wlfc_init(dhd_pub_t *dhd)
-{
-	char iovbuf[12]; /* Room for "tlv" + '\0' + parameter */
-	/* enable all signals & indicate host proptxstatus logic is active */
-	uint32 tlv = dhd->wlfc_enabled?
-		WLFC_FLAGS_RSSI_SIGNALS |
-		WLFC_FLAGS_XONXOFF_SIGNALS |
-		WLFC_FLAGS_CREDIT_STATUS_SIGNALS |
-		WLFC_FLAGS_HOST_PROPTXSTATUS_ACTIVE : 0;
-
-
-	/*
-	try to enable/disable signaling by sending "tlv" iovar. if that fails,
-	fallback to no flow control? Print a message for now.
-	*/
-
-	/* enable proptxtstatus signaling by default */
-	bcm_mkiovar("tlv", (char *)&tlv, 4, iovbuf, sizeof(iovbuf));
-	if (dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0) < 0) {
-		DHD_ERROR(("dhd_wlfc_init(): failed to enable/disable bdcv2 tlv signaling\n"));
-	}
-	else {
-		/*
-		Leaving the message for now, it should be removed after a while; once
-		the tlv situation is stable.
-		*/
-		DHD_ERROR(("dhd_wlfc_init(): successfully %s bdcv2 tlv signaling, %d\n",
-			dhd->wlfc_enabled?"enabled":"disabled", tlv));
-	}
-	return BCME_OK;
-}
-
-int
-dhd_wlfc_enable(dhd_pub_t *dhd)
-{
-	int i;
-	athost_wl_status_info_t* wlfc;
-
-	if (!dhd->wlfc_enabled || dhd->wlfc_state)
-		return BCME_OK;
-
-	/* allocate space to track txstatus propagated from firmware */
-	dhd->wlfc_state = MALLOC(dhd->osh, sizeof(athost_wl_status_info_t));
-	if (dhd->wlfc_state == NULL)
-		return BCME_NOMEM;
-
-	/* initialize state space */
-	wlfc = (athost_wl_status_info_t*)dhd->wlfc_state;
-	memset(wlfc, 0, sizeof(athost_wl_status_info_t));
-
-	/* remember osh & dhdp */
-	wlfc->osh = dhd->osh;
-	wlfc->dhdp = dhd;
-
-	wlfc->hanger =
-		dhd_wlfc_hanger_create(dhd->osh, WLFC_HANGER_MAXITEMS);
-	if (wlfc->hanger == NULL) {
-		MFREE(dhd->osh, dhd->wlfc_state, sizeof(athost_wl_status_info_t));
-		dhd->wlfc_state = NULL;
-		return BCME_NOMEM;
-	}
-
-	/* initialize all interfaces to accept traffic */
-	for (i = 0; i < WLFC_MAX_IFNUM; i++) {
-		wlfc->hostif_flow_state[i] = OFF;
-	}
-
-	/* 
-	create the SENDQ containing
-	sub-queues for all AC precedences + 1 for bc/mc traffic
-	*/
-	pktq_init(&wlfc->SENDQ, (AC_COUNT + 1), WLFC_SENDQ_LEN);
-
-	wlfc->destination_entries.other.state = WLFC_STATE_OPEN;
-	/* bc/mc FIFO is always open [credit aside], i.e. b[5] */
-	wlfc->destination_entries.other.ac_bitmap = 0x1f;
-	wlfc->destination_entries.other.interface_id = 0;
-
-	wlfc->proptxstatus_mode = WLFC_FCMODE_EXPLICIT_CREDIT;
-
-	return BCME_OK;
-}
-
-/* release all packet resources */
-void
-dhd_wlfc_cleanup(dhd_pub_t *dhd)
-{
-	int i;
-	int total_entries;
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhd->wlfc_state;
-	wlfc_mac_descriptor_t* table;
-	wlfc_hanger_t* h;
-
-	if (dhd->wlfc_state == NULL)
-		return;
-
-	total_entries = sizeof(wlfc->destination_entries)/sizeof(wlfc_mac_descriptor_t);
-	/* search all entries, include nodes as well as interfaces */
-	table = (wlfc_mac_descriptor_t*)&wlfc->destination_entries;
-
-	for (i = 0; i < total_entries; i++) {
-		if (table[i].occupied) {
-			if (table[i].psq.len) {
-				WLFC_DBGMESG(("%s(): DELAYQ[%d].len = %d\n",
-					__FUNCTION__, i, table[i].psq.len));
-				/* release packets held in DELAYQ */
-				pktq_flush(wlfc->osh, &table[i].psq, TRUE, NULL, 0);
-			}
-			table[i].occupied = 0;
-		}
-	}
-	/* release packets held in SENDQ */
-	if (wlfc->SENDQ.len)
-		pktq_flush(wlfc->osh, &wlfc->SENDQ, TRUE, NULL, 0);
-	/* any in the hanger? */
-	h = (wlfc_hanger_t*)wlfc->hanger;
-	for (i = 0; i < h->max_items; i++) {
-		if (h->items[i].state == WLFC_HANGER_ITEM_STATE_INUSE) {
-			PKTFREE(wlfc->osh, h->items[i].pkt, TRUE);
-		}
-	}
-	return;
-}
-
-void
-dhd_wlfc_deinit(dhd_pub_t *dhd)
-{
-	/* cleanup all psq related resources */
-	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
-		dhd->wlfc_state;
-
-	if (dhd->wlfc_state == NULL)
-		return;
-
-#ifdef PROP_TXSTATUS_DEBUG
-	{
-		int i;
-		wlfc_hanger_t* h = (wlfc_hanger_t*)wlfc->hanger;
-		for (i = 0; i < h->max_items; i++) {
-			if (h->items[i].state == WLFC_HANGER_ITEM_STATE_INUSE) {
-				WLFC_DBGMESG(("%s() pkt[%d] = 0x%p, FIFO_credit_used:%d\n",
-					__FUNCTION__, i, h->items[i].pkt,
-					DHD_PKTTAG_CREDITCHECK(PKTTAG(h->items[i].pkt))));
-			}
-		}
-	}
-#endif
-	/* delete hanger */
-	dhd_wlfc_hanger_delete(dhd->osh, wlfc->hanger);
-
-	/* free top structure */
-	MFREE(dhd->osh, dhd->wlfc_state, sizeof(athost_wl_status_info_t));
-	dhd->wlfc_state = NULL;
-	return;
-}
-#endif /* PROP_TXSTATUS */
-
-void
-dhd_prot_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf)
-{
-	bcm_bprintf(strbuf, "Protocol CDC: reqid %d\n", dhdp->prot->reqid);
-#ifdef PROP_TXSTATUS
-	if (dhdp->wlfc_state)
-		dhd_wlfc_dump(dhdp, strbuf);
-#endif
-}
-
-void
-dhd_prot_hdrpush(dhd_pub_t *dhd, int ifidx, void *pktbuf)
-{
-#ifdef BDC
-	struct bdc_header *h;
-#endif /* BDC */
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-#ifdef BDC
-	/* Push BDC header used to convey priority for buses that don't */
-
-	PKTPUSH(dhd->osh, pktbuf, BDC_HEADER_LEN);
-
-	h = (struct bdc_header *)PKTDATA(dhd->osh, pktbuf);
-
-	h->flags = (BDC_PROTO_VER << BDC_FLAG_VER_SHIFT);
-	if (PKTSUMNEEDED(pktbuf))
-		h->flags |= BDC_FLAG_SUM_NEEDED;
-
-
-	h->priority = (PKTPRIO(pktbuf) & BDC_PRIORITY_MASK);
-	h->flags2 = 0;
-	h->dataOffset = 0;
-#endif /* BDC */
-	BDC_SET_IF_IDX(h, ifidx);
-}
-
-int
-dhd_prot_hdrpull(dhd_pub_t *dhd, int *ifidx, void *pktbuf)
-{
-#ifdef BDC
-	struct bdc_header *h;
-#endif
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-#ifdef BDC
-	/* Pop BDC header used to convey priority for buses that don't */
-
-	if (PKTLEN(dhd->osh, pktbuf) < BDC_HEADER_LEN) {
-		DHD_ERROR(("%s: rx data too short (%d < %d)\n", __FUNCTION__,
-		           PKTLEN(dhd->osh, pktbuf), BDC_HEADER_LEN));
-		return BCME_ERROR;
-	}
-
-	h = (struct bdc_header *)PKTDATA(dhd->osh, pktbuf);
-
-	if ((*ifidx = BDC_GET_IF_IDX(h)) >= DHD_MAX_IFS) {
-		DHD_ERROR(("%s: rx data ifnum out of range (%d)\n",
-		           __FUNCTION__, *ifidx));
-		return BCME_ERROR;
-	}
-
-	if (((h->flags & BDC_FLAG_VER_MASK) >> BDC_FLAG_VER_SHIFT) != BDC_PROTO_VER) {
-		DHD_ERROR(("%s: non-BDC packet received, flags = 0x%x\n",
-		           dhd_ifname(dhd, *ifidx), h->flags));
-		if (((h->flags & BDC_FLAG_VER_MASK) >> BDC_FLAG_VER_SHIFT) == BDC_PROTO_VER_1)
-			h->dataOffset = 0;
-		else
-			return BCME_ERROR;
-	}
-
-	if (h->flags & BDC_FLAG_SUM_GOOD) {
-		DHD_INFO(("%s: BDC packet received with good rx-csum, flags 0x%x\n",
-		          dhd_ifname(dhd, *ifidx), h->flags));
-		PKTSETSUMGOOD(pktbuf, TRUE);
-	}
-
-	PKTSETPRIO(pktbuf, (h->priority & BDC_PRIORITY_MASK));
-	PKTPULL(dhd->osh, pktbuf, BDC_HEADER_LEN);
-#endif /* BDC */
-
-	if (PKTLEN(dhd->osh, pktbuf) < (uint32) (h->dataOffset << 2)) {
-		DHD_ERROR(("%s: rx data too short (%d < %d)\n", __FUNCTION__,
-		           PKTLEN(dhd->osh, pktbuf), (h->dataOffset * 4)));
-		return BCME_ERROR;
-	}
-
-#ifdef PROP_TXSTATUS
-	if (dhd->wlfc_state &&
-		((athost_wl_status_info_t*)dhd->wlfc_state)->proptxstatus_mode
-		!= WLFC_FCMODE_NONE &&
-		(!DHD_PKTTAG_PKTDIR(PKTTAG(pktbuf)))) {
-		/*
-		- parse txstatus only for packets that came from the firmware
-		*/
-		dhd_os_wlfc_block(dhd);
-		dhd_wlfc_parse_header_info(dhd, pktbuf, (h->dataOffset << 2));
-		((athost_wl_status_info_t*)dhd->wlfc_state)->stats.dhd_hdrpulls++;
-		dhd_wlfc_commit_packets(dhd->wlfc_state, (f_commitpkt_t)dhd_bus_txdata,
-			dhd->bus);
-		dhd_os_wlfc_unblock(dhd);
-	}
-#endif /* PROP_TXSTATUS */
-	PKTPULL(dhd->osh, pktbuf, (h->dataOffset << 2));
-	return 0;
-}
-
-int
-dhd_prot_attach(dhd_pub_t *dhd)
-{
-	dhd_prot_t *cdc;
-
-	if (!(cdc = (dhd_prot_t *)DHD_OS_PREALLOC(dhd->osh, DHD_PREALLOC_PROT,
-		sizeof(dhd_prot_t)))) {
-		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
-		goto fail;
-	}
-	memset(cdc, 0, sizeof(dhd_prot_t));
-
-	/* ensure that the msg buf directly follows the cdc msg struct */
-	if ((uintptr)(&cdc->msg + 1) != (uintptr)cdc->buf) {
-		DHD_ERROR(("dhd_prot_t is not correctly defined\n"));
-		goto fail;
-	}
-
-	dhd->prot = cdc;
-#ifdef BDC
-	dhd->hdrlen += BDC_HEADER_LEN;
-#endif
-	dhd->maxctl = WLC_IOCTL_MAXLEN + sizeof(cdc_ioctl_t) + ROUND_UP_MARGIN;
-	return 0;
-
-fail:
-#ifndef DHD_USE_STATIC_BUF
-	if (cdc != NULL)
-		MFREE(dhd->osh, cdc, sizeof(dhd_prot_t));
-#endif
-	return BCME_NOMEM;
-}
-
-/* ~NOTE~ What if another thread is waiting on the semaphore?  Holding it? */
-void
-dhd_prot_detach(dhd_pub_t *dhd)
-{
-#ifdef PROP_TXSTATUS
-	dhd_wlfc_deinit(dhd);
-#endif
-#ifndef DHD_USE_STATIC_BUF
-	MFREE(dhd->osh, dhd->prot, sizeof(dhd_prot_t));
-#endif
-	dhd->prot = NULL;
-}
-
-void
-dhd_prot_dstats(dhd_pub_t *dhd)
-{
-	/* No stats from dongle added yet, copy bus stats */
-	dhd->dstats.tx_packets = dhd->tx_packets;
-	dhd->dstats.tx_errors = dhd->tx_errors;
-	dhd->dstats.rx_packets = dhd->rx_packets;
-	dhd->dstats.rx_errors = dhd->rx_errors;
-	dhd->dstats.rx_dropped = dhd->rx_dropped;
-	dhd->dstats.multicast = dhd->rx_multicast;
-	return;
-}
-
-
-int
-dhd_prot_init(dhd_pub_t *dhd)
-{
-	int ret = 0;
-	wlc_rev_info_t revinfo;
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-
-
-	/* Get the device rev info */
-	memset(&revinfo, 0, sizeof(revinfo));
-	ret = dhd_wl_ioctl_cmd(dhd, WLC_GET_REVINFO, &revinfo, sizeof(revinfo), FALSE, 0);
-	if (ret < 0)
-		goto done;
-
-
-#ifdef PROP_TXSTATUS
-	ret = dhd_wlfc_init(dhd);
-#endif
-
-#ifndef WL_CFG80211
-	ret = dhd_preinit_ioctls(dhd);
-#endif /* WL_CFG80211 */
-
-	/* Always assumes wl for now */
-	dhd->iswl = TRUE;
-
-done:
-	return ret;
-}
-
-void
-dhd_prot_stop(dhd_pub_t *dhd)
-{
-	/* Nothing to do for CDC */
-}
diff --git a/drivers/net/wireless/bcmdhd/dhd_common.c b/drivers/net/wireless/bcmdhd/dhd_common.c
deleted file mode 100644
index f6bb8e5bb56..00000000000
--- a/drivers/net/wireless/bcmdhd/dhd_common.c
+++ /dev/null
@@ -1,2606 +0,0 @@
-/*
- * Broadcom Dongle Host Driver (DHD), common DHD core.
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: dhd_common.c,v 1.57.2.22 2011-02-01 18:38:37 Exp $
- */
-#include <typedefs.h>
-#include <osl.h>
-
-#include <epivers.h>
-#include <bcmutils.h>
-
-#include <bcmendian.h>
-#include <dngl_stats.h>
-#include <wlioctl.h>
-#include <dhd.h>
-
-#include <proto/bcmevent.h>
-
-#include <dhd_bus.h>
-#include <dhd_proto.h>
-#include <dhd_dbg.h>
-#include <msgtrace.h>
-
-#ifdef WL_CFG80211
-#include <wl_cfg80211.h>
-#endif
-#include <proto/bt_amp_hci.h>
-#include <dhd_bta.h>
-#ifdef SET_RANDOM_MAC_SOFTAP
-#include <linux/random.h>
-#include <linux/jiffies.h>
-#endif
-
-#ifdef PROP_TXSTATUS
-#include <wlfc_proto.h>
-#include <dhd_wlfc.h>
-#endif
-
-
-#ifdef WLMEDIA_HTSF
-extern void htsf_update(struct dhd_info *dhd, void *data);
-#endif
-int dhd_msg_level = DHD_ERROR_VAL;
-
-
-#include <wl_iw.h>
-
-char fw_path[MOD_PARAM_PATHLEN];
-char nv_path[MOD_PARAM_PATHLEN];
-
-#ifdef SOFTAP
-char fw_path2[MOD_PARAM_PATHLEN];
-extern bool softap_enabled;
-#endif
-
-/* Last connection success/failure status */
-uint32 dhd_conn_event;
-uint32 dhd_conn_status;
-uint32 dhd_conn_reason;
-
-#define htod32(i) i
-#define htod16(i) i
-#define dtoh32(i) i
-#define dtoh16(i) i
-extern int dhd_iscan_request(void * dhdp, uint16 action);
-extern void dhd_ind_scan_confirm(void *h, bool status);
-extern int dhd_iscan_in_progress(void *h);
-void dhd_iscan_lock(void);
-void dhd_iscan_unlock(void);
-extern int dhd_change_mtu(dhd_pub_t *dhd, int new_mtu, int ifidx);
-
-bool ap_cfg_running = FALSE;
-bool ap_fw_loaded = FALSE;
-
-#if defined(KEEP_ALIVE)
-int dhd_keep_alive_onoff(dhd_pub_t *dhd);
-#endif /* KEEP_ALIVE */
-
-/* Packet alignment for most efficient SDIO (can change based on platform) */
-#ifndef DHD_SDALIGN
-#define DHD_SDALIGN	32
-#endif
-#if !ISPOWEROF2(DHD_SDALIGN)
-#error DHD_SDALIGN is not a power of 2!
-#endif
-
-#ifdef DHD_DEBUG
-const char dhd_version[] = "Dongle Host Driver, version " EPI_VERSION_STR "\nCompiled on "
-	__DATE__ " at " __TIME__;
-#else
-const char dhd_version[] = "Dongle Host Driver, version " EPI_VERSION_STR;
-#endif
-
-void dhd_set_timer(void *bus, uint wdtick);
-
-
-
-/* IOVar table */
-enum {
-	IOV_VERSION = 1,
-	IOV_MSGLEVEL,
-	IOV_BCMERRORSTR,
-	IOV_BCMERROR,
-	IOV_WDTICK,
-	IOV_DUMP,
-	IOV_CLEARCOUNTS,
-	IOV_LOGDUMP,
-	IOV_LOGCAL,
-	IOV_LOGSTAMP,
-	IOV_GPIOOB,
-	IOV_IOCTLTIMEOUT,
-	IOV_HCI_CMD,		/* HCI command */
-	IOV_HCI_ACL_DATA,	/* HCI data packet */
-#if defined(DHD_DEBUG)
-	IOV_CONS,
-	IOV_DCONSOLE_POLL,
-#endif /* defined(DHD_DEBUG) */
-#ifdef PROP_TXSTATUS
-	IOV_PROPTXSTATUS_ENABLE,
-	IOV_PROPTXSTATUS_MODE,
-#endif
-	IOV_BUS_TYPE,
-#ifdef WLMEDIA_HTSF
-	IOV_WLPKTDLYSTAT_SZ,
-#endif
-	IOV_CHANGEMTU,
-	IOV_LAST
-};
-
-const bcm_iovar_t dhd_iovars[] = {
-	{"version", 	IOV_VERSION,	0,	IOVT_BUFFER,	sizeof(dhd_version) },
-#ifdef DHD_DEBUG
-	{"msglevel",	IOV_MSGLEVEL,	0,	IOVT_UINT32,	0 },
-#endif /* DHD_DEBUG */
-	{"bcmerrorstr", IOV_BCMERRORSTR, 0, IOVT_BUFFER,	BCME_STRLEN },
-	{"bcmerror",	IOV_BCMERROR,	0,	IOVT_INT8,	0 },
-	{"wdtick",	IOV_WDTICK, 0,	IOVT_UINT32,	0 },
-	{"dump",	IOV_DUMP,	0,	IOVT_BUFFER,	DHD_IOCTL_MAXLEN },
-#ifdef DHD_DEBUG
-	{"cons",	IOV_CONS,	0,	IOVT_BUFFER,	0 },
-	{"dconpoll",	IOV_DCONSOLE_POLL, 0,	IOVT_UINT32,	0 },
-#endif
-	{"clearcounts", IOV_CLEARCOUNTS, 0, IOVT_VOID,	0 },
-	{"gpioob",	IOV_GPIOOB,	0,	IOVT_UINT32,	0 },
-	{"ioctl_timeout",	IOV_IOCTLTIMEOUT,	0,	IOVT_UINT32,	0 },
-	{"HCI_cmd",	IOV_HCI_CMD,	0,	IOVT_BUFFER,	0},
-	{"HCI_ACL_data", IOV_HCI_ACL_DATA, 0,	IOVT_BUFFER,	0},
-#ifdef PROP_TXSTATUS
-	{"proptx",	IOV_PROPTXSTATUS_ENABLE,	0,	IOVT_UINT32,	0 },
-	/*
-	set the proptxtstatus operation mode:
-	0 - Do not do any proptxtstatus flow control
-	1 - Use implied credit from a packet status
-	2 - Use explicit credit
-	*/
-	{"ptxmode",	IOV_PROPTXSTATUS_MODE,	0,	IOVT_UINT32,	0 },
-#endif
-	{"bustype", IOV_BUS_TYPE, 0, IOVT_UINT32, 0},
-#ifdef WLMEDIA_HTSF
-	{"pktdlystatsz", IOV_WLPKTDLYSTAT_SZ, 0, IOVT_UINT8, 0 },
-#endif
-	{"changemtu", IOV_CHANGEMTU, 0, IOVT_UINT32, 0 },
-	{NULL, 0, 0, 0, 0 }
-};
-
-struct dhd_cmn *
-dhd_common_init(osl_t *osh)
-{
-	dhd_cmn_t *cmn;
-
-	/* Init global variables at run-time, not as part of the declaration.
-	 * This is required to support init/de-init of the driver. Initialization
-	 * of globals as part of the declaration results in non-deterministic
-	 * behavior since the value of the globals may be different on the
-	 * first time that the driver is initialized vs subsequent initializations.
-	 */
-	/* Allocate private bus interface state */
-	if (!(cmn = MALLOC(osh, sizeof(dhd_cmn_t)))) {
-		DHD_ERROR(("%s: MALLOC failed\n", __FUNCTION__));
-		return NULL;
-	}
-	memset(cmn, 0, sizeof(dhd_cmn_t));
-	cmn->osh = osh;
-
-#ifdef CONFIG_BCMDHD_FW_PATH
-	bcm_strncpy_s(fw_path, sizeof(fw_path), CONFIG_BCMDHD_FW_PATH, MOD_PARAM_PATHLEN-1);
-#else /* CONFIG_BCMDHD_FW_PATH */
-	fw_path[0] = '\0';
-#endif /* CONFIG_BCMDHD_FW_PATH */
-#ifdef CONFIG_BCMDHD_NVRAM_PATH
-	bcm_strncpy_s(nv_path, sizeof(nv_path), CONFIG_BCMDHD_NVRAM_PATH, MOD_PARAM_PATHLEN-1);
-#else /* CONFIG_BCMDHD_NVRAM_PATH */
-	nv_path[0] = '\0';
-#endif /* CONFIG_BCMDHD_NVRAM_PATH */
-#ifdef SOFTAP
-	fw_path2[0] = '\0';
-#endif
-	return cmn;
-}
-
-void
-dhd_common_deinit(dhd_pub_t *dhd_pub, dhd_cmn_t *sa_cmn)
-{
-	osl_t *osh;
-	dhd_cmn_t *cmn;
-
-	if (dhd_pub != NULL)
-		cmn = dhd_pub->cmn;
-	else
-		cmn = sa_cmn;
-
-	if (!cmn)
-		return;
-
-	osh = cmn->osh;
-
-	if (dhd_pub != NULL)
-	dhd_pub->cmn = NULL;
-	MFREE(osh, cmn, sizeof(dhd_cmn_t));
-}
-
-static int
-dhd_dump(dhd_pub_t *dhdp, char *buf, int buflen)
-{
-	char eabuf[ETHER_ADDR_STR_LEN];
-
-	struct bcmstrbuf b;
-	struct bcmstrbuf *strbuf = &b;
-
-	bcm_binit(strbuf, buf, buflen);
-
-	/* Base DHD info */
-	bcm_bprintf(strbuf, "%s\n", dhd_version);
-	bcm_bprintf(strbuf, "\n");
-	bcm_bprintf(strbuf, "pub.up %d pub.txoff %d pub.busstate %d\n",
-	            dhdp->up, dhdp->txoff, dhdp->busstate);
-	bcm_bprintf(strbuf, "pub.hdrlen %d pub.maxctl %d pub.rxsz %d\n",
-	            dhdp->hdrlen, dhdp->maxctl, dhdp->rxsz);
-	bcm_bprintf(strbuf, "pub.iswl %d pub.drv_version %ld pub.mac %s\n",
-	            dhdp->iswl, dhdp->drv_version, bcm_ether_ntoa(&dhdp->mac, eabuf));
-	bcm_bprintf(strbuf, "pub.bcmerror %d tickcnt %d\n", dhdp->bcmerror, dhdp->tickcnt);
-
-	bcm_bprintf(strbuf, "dongle stats:\n");
-	bcm_bprintf(strbuf, "tx_packets %ld tx_bytes %ld tx_errors %ld tx_dropped %ld\n",
-	            dhdp->dstats.tx_packets, dhdp->dstats.tx_bytes,
-	            dhdp->dstats.tx_errors, dhdp->dstats.tx_dropped);
-	bcm_bprintf(strbuf, "rx_packets %ld rx_bytes %ld rx_errors %ld rx_dropped %ld\n",
-	            dhdp->dstats.rx_packets, dhdp->dstats.rx_bytes,
-	            dhdp->dstats.rx_errors, dhdp->dstats.rx_dropped);
-	bcm_bprintf(strbuf, "multicast %ld\n", dhdp->dstats.multicast);
-
-	bcm_bprintf(strbuf, "bus stats:\n");
-	bcm_bprintf(strbuf, "tx_packets %ld tx_multicast %ld tx_errors %ld\n",
-	            dhdp->tx_packets, dhdp->tx_multicast, dhdp->tx_errors);
-	bcm_bprintf(strbuf, "tx_ctlpkts %ld tx_ctlerrs %ld\n",
-	            dhdp->tx_ctlpkts, dhdp->tx_ctlerrs);
-	bcm_bprintf(strbuf, "rx_packets %ld rx_multicast %ld rx_errors %ld \n",
-	            dhdp->rx_packets, dhdp->rx_multicast, dhdp->rx_errors);
-	bcm_bprintf(strbuf, "rx_ctlpkts %ld rx_ctlerrs %ld rx_dropped %ld\n",
-	            dhdp->rx_ctlpkts, dhdp->rx_ctlerrs, dhdp->rx_dropped);
-	bcm_bprintf(strbuf, "rx_readahead_cnt %ld tx_realloc %ld\n",
-	            dhdp->rx_readahead_cnt, dhdp->tx_realloc);
-	bcm_bprintf(strbuf, "\n");
-
-	/* Add any prot info */
-	dhd_prot_dump(dhdp, strbuf);
-	bcm_bprintf(strbuf, "\n");
-
-	/* Add any bus info */
-	dhd_bus_dump(dhdp, strbuf);
-
-	return (!strbuf->size ? BCME_BUFTOOSHORT : 0);
-}
-
-int
-dhd_wl_ioctl_cmd(dhd_pub_t *dhd_pub, int cmd, void *arg, int len, uint8 set, int ifindex)
-{
-	wl_ioctl_t ioc;
-
-	ioc.cmd = cmd;
-	ioc.buf = arg;
-	ioc.len = len;
-	ioc.set = set;
-
-	return dhd_wl_ioctl(dhd_pub, ifindex, &ioc, arg, len);
-}
-
-
-int
-dhd_wl_ioctl(dhd_pub_t *dhd_pub, int ifindex, wl_ioctl_t *ioc, void *buf, int len)
-{
-	int ret;
-
-	dhd_os_proto_block(dhd_pub);
-
-	ret = dhd_prot_ioctl(dhd_pub, ifindex, ioc, buf, len);
-
-
-	dhd_os_proto_unblock(dhd_pub);
-	return ret;
-}
-
-static int
-dhd_doiovar(dhd_pub_t *dhd_pub, const bcm_iovar_t *vi, uint32 actionid, const char *name,
-            void *params, int plen, void *arg, int len, int val_size)
-{
-	int bcmerror = 0;
-	int32 int_val = 0;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-	DHD_TRACE(("%s: actionid = %d; name %s\n", __FUNCTION__, actionid, name));
-
-	if ((bcmerror = bcm_iovar_lencheck(vi, arg, len, IOV_ISSET(actionid))) != 0)
-		goto exit;
-
-	if (plen >= (int)sizeof(int_val))
-		bcopy(params, &int_val, sizeof(int_val));
-
-	switch (actionid) {
-	case IOV_GVAL(IOV_VERSION):
-		/* Need to have checked buffer length */
-		bcm_strncpy_s((char*)arg, len, dhd_version, len);
-		break;
-
-	case IOV_GVAL(IOV_MSGLEVEL):
-		int_val = (int32)dhd_msg_level;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_MSGLEVEL):
-		dhd_msg_level = int_val;
-		break;
-	case IOV_GVAL(IOV_BCMERRORSTR):
-		bcm_strncpy_s((char *)arg, len, bcmerrorstr(dhd_pub->bcmerror), BCME_STRLEN);
-		((char *)arg)[BCME_STRLEN - 1] = 0x00;
-		break;
-
-	case IOV_GVAL(IOV_BCMERROR):
-		int_val = (int32)dhd_pub->bcmerror;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_GVAL(IOV_WDTICK):
-		int_val = (int32)dhd_watchdog_ms;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_WDTICK):
-		if (!dhd_pub->up) {
-			bcmerror = BCME_NOTUP;
-			break;
-		}
-		dhd_os_wd_timer(dhd_pub, (uint)int_val);
-		break;
-
-	case IOV_GVAL(IOV_DUMP):
-		bcmerror = dhd_dump(dhd_pub, arg, len);
-		break;
-
-#ifdef DHD_DEBUG
-	case IOV_GVAL(IOV_DCONSOLE_POLL):
-		int_val = (int32)dhd_console_ms;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_DCONSOLE_POLL):
-		dhd_console_ms = (uint)int_val;
-		break;
-
-	case IOV_SVAL(IOV_CONS):
-		if (len > 0)
-			bcmerror = dhd_bus_console_in(dhd_pub, arg, len - 1);
-		break;
-#endif /* DHD_DEBUG */
-
-	case IOV_SVAL(IOV_CLEARCOUNTS):
-		dhd_pub->tx_packets = dhd_pub->rx_packets = 0;
-		dhd_pub->tx_errors = dhd_pub->rx_errors = 0;
-		dhd_pub->tx_ctlpkts = dhd_pub->rx_ctlpkts = 0;
-		dhd_pub->tx_ctlerrs = dhd_pub->rx_ctlerrs = 0;
-		dhd_pub->rx_dropped = 0;
-		dhd_pub->rx_readahead_cnt = 0;
-		dhd_pub->tx_realloc = 0;
-		dhd_pub->wd_dpc_sched = 0;
-		memset(&dhd_pub->dstats, 0, sizeof(dhd_pub->dstats));
-		dhd_bus_clearcounts(dhd_pub);
-#ifdef PROP_TXSTATUS
-		/* clear proptxstatus related counters */
-		if (dhd_pub->wlfc_state) {
-			athost_wl_status_info_t *wlfc =
-			        (athost_wl_status_info_t*)dhd_pub->wlfc_state;
-			wlfc_hanger_t* hanger;
-
-			memset(&wlfc->stats, 0, sizeof(athost_wl_stat_counters_t));
-
-			hanger = (wlfc_hanger_t*)wlfc->hanger;
-			hanger->pushed = 0;
-			hanger->popped = 0;
-			hanger->failed_slotfind = 0;
-			hanger->failed_to_pop = 0;
-			hanger->failed_to_push = 0;
-		}
-#endif /* PROP_TXSTATUS */
-		break;
-
-
-	case IOV_GVAL(IOV_IOCTLTIMEOUT): {
-		int_val = (int32)dhd_os_get_ioctl_resp_timeout();
-		bcopy(&int_val, arg, sizeof(int_val));
-		break;
-	}
-
-	case IOV_SVAL(IOV_IOCTLTIMEOUT): {
-		if (int_val <= 0)
-			bcmerror = BCME_BADARG;
-		else
-			dhd_os_set_ioctl_resp_timeout((unsigned int)int_val);
-		break;
-	}
-
-	case IOV_SVAL(IOV_HCI_CMD): {
-		amp_hci_cmd_t *cmd = (amp_hci_cmd_t *)arg;
-
-		/* sanity check: command preamble present */
-		if (len < HCI_CMD_PREAMBLE_SIZE)
-			return BCME_BUFTOOSHORT;
-
-		/* sanity check: command parameters are present */
-		if (len < (int)(HCI_CMD_PREAMBLE_SIZE + cmd->plen))
-			return BCME_BUFTOOSHORT;
-
-		dhd_bta_docmd(dhd_pub, cmd, len);
-		break;
-	}
-
-	case IOV_SVAL(IOV_HCI_ACL_DATA): {
-		amp_hci_ACL_data_t *ACL_data = (amp_hci_ACL_data_t *)arg;
-
-		/* sanity check: HCI header present */
-		if (len < HCI_ACL_DATA_PREAMBLE_SIZE)
-			return BCME_BUFTOOSHORT;
-
-		/* sanity check: ACL data is present */
-		if (len < (int)(HCI_ACL_DATA_PREAMBLE_SIZE + ACL_data->dlen))
-			return BCME_BUFTOOSHORT;
-
-		dhd_bta_tx_hcidata(dhd_pub, ACL_data, len);
-		break;
-	}
-
-#ifdef PROP_TXSTATUS
-	case IOV_GVAL(IOV_PROPTXSTATUS_ENABLE):
-		int_val = dhd_pub->wlfc_enabled? 1 : 0;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_PROPTXSTATUS_ENABLE):
-		dhd_pub->wlfc_enabled = int_val? 1 : 0;
-		break;
-
-	case IOV_GVAL(IOV_PROPTXSTATUS_MODE): {
-		athost_wl_status_info_t *wlfc =
-		        (athost_wl_status_info_t*)dhd_pub->wlfc_state;
-		int_val = dhd_pub->wlfc_state ? (int32)wlfc->proptxstatus_mode : 0;
-		bcopy(&int_val, arg, val_size);
-		break;
-	}
-
-	case IOV_SVAL(IOV_PROPTXSTATUS_MODE):
-		if (dhd_pub->wlfc_state) {
-			athost_wl_status_info_t *wlfc =
-			        (athost_wl_status_info_t*)dhd_pub->wlfc_state;
-			wlfc->proptxstatus_mode = int_val & 0xff;
-		}
-		break;
-#endif /* PROP_TXSTATUS */
-
-	case IOV_GVAL(IOV_BUS_TYPE):
-	/* The dhd application query the driver to check if its usb or sdio.  */
-#ifdef BCMDHDUSB
-		int_val = BUS_TYPE_USB;
-#endif
-		int_val = BUS_TYPE_SDIO;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-
-#ifdef WLMEDIA_HTSF
-	case IOV_GVAL(IOV_WLPKTDLYSTAT_SZ):
-		int_val = dhd_pub->htsfdlystat_sz;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_WLPKTDLYSTAT_SZ):
-		dhd_pub->htsfdlystat_sz = int_val & 0xff;
-		printf("Setting tsfdlystat_sz:%d\n", dhd_pub->htsfdlystat_sz);
-		break;
-#endif
-	case IOV_SVAL(IOV_CHANGEMTU):
-		int_val &= 0xffff;
-		bcmerror = dhd_change_mtu(dhd_pub, int_val, 0);
-		break;
-
-	default:
-		bcmerror = BCME_UNSUPPORTED;
-		break;
-	}
-
-exit:
-	DHD_TRACE(("%s: actionid %d, bcmerror %d\n", __FUNCTION__, actionid, bcmerror));
-	return bcmerror;
-}
-
-/* Store the status of a connection attempt for later retrieval by an iovar */
-void
-dhd_store_conn_status(uint32 event, uint32 status, uint32 reason)
-{
-	/* Do not overwrite a WLC_E_PRUNE with a WLC_E_SET_SSID
-	 * because an encryption/rsn mismatch results in both events, and
-	 * the important information is in the WLC_E_PRUNE.
-	 */
-	if (!(event == WLC_E_SET_SSID && status == WLC_E_STATUS_FAIL &&
-	      dhd_conn_event == WLC_E_PRUNE)) {
-		dhd_conn_event = event;
-		dhd_conn_status = status;
-		dhd_conn_reason = reason;
-	}
-}
-
-bool
-dhd_prec_enq(dhd_pub_t *dhdp, struct pktq *q, void *pkt, int prec)
-{
-	void *p;
-	int eprec = -1;		/* precedence to evict from */
-	bool discard_oldest;
-
-	/* Fast case, precedence queue is not full and we are also not
-	 * exceeding total queue length
-	 */
-	if (!pktq_pfull(q, prec) && !pktq_full(q)) {
-		pktq_penq(q, prec, pkt);
-		return TRUE;
-	}
-
-	/* Determine precedence from which to evict packet, if any */
-	if (pktq_pfull(q, prec))
-		eprec = prec;
-	else if (pktq_full(q)) {
-		p = pktq_peek_tail(q, &eprec);
-		ASSERT(p);
-		if (eprec > prec || eprec < 0)
-			return FALSE;
-	}
-
-	/* Evict if needed */
-	if (eprec >= 0) {
-		/* Detect queueing to unconfigured precedence */
-		ASSERT(!pktq_pempty(q, eprec));
-		discard_oldest = AC_BITMAP_TST(dhdp->wme_dp, eprec);
-		if (eprec == prec && !discard_oldest)
-			return FALSE;		/* refuse newer (incoming) packet */
-		/* Evict packet according to discard policy */
-		p = discard_oldest ? pktq_pdeq(q, eprec) : pktq_pdeq_tail(q, eprec);
-		ASSERT(p);
-
-		PKTFREE(dhdp->osh, p, TRUE);
-	}
-
-	/* Enqueue */
-	p = pktq_penq(q, prec, pkt);
-	ASSERT(p);
-
-	return TRUE;
-}
-
-static int
-dhd_iovar_op(dhd_pub_t *dhd_pub, const char *name,
-	void *params, int plen, void *arg, int len, bool set)
-{
-	int bcmerror = 0;
-	int val_size;
-	const bcm_iovar_t *vi = NULL;
-	uint32 actionid;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	ASSERT(name);
-	ASSERT(len >= 0);
-
-	/* Get MUST have return space */
-	ASSERT(set || (arg && len));
-
-	/* Set does NOT take qualifiers */
-	ASSERT(!set || (!params && !plen));
-
-	if ((vi = bcm_iovar_lookup(dhd_iovars, name)) == NULL) {
-		bcmerror = BCME_UNSUPPORTED;
-		goto exit;
-	}
-
-	DHD_CTL(("%s: %s %s, len %d plen %d\n", __FUNCTION__,
-		name, (set ? "set" : "get"), len, plen));
-
-	/* set up 'params' pointer in case this is a set command so that
-	 * the convenience int and bool code can be common to set and get
-	 */
-	if (params == NULL) {
-		params = arg;
-		plen = len;
-	}
-
-	if (vi->type == IOVT_VOID)
-		val_size = 0;
-	else if (vi->type == IOVT_BUFFER)
-		val_size = len;
-	else
-		/* all other types are integer sized */
-		val_size = sizeof(int);
-
-	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
-
-	bcmerror = dhd_doiovar(dhd_pub, vi, actionid, name, params, plen, arg, len, val_size);
-
-exit:
-	return bcmerror;
-}
-
-int
-dhd_ioctl(dhd_pub_t * dhd_pub, dhd_ioctl_t *ioc, void * buf, uint buflen)
-{
-	int bcmerror = 0;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (!buf) {
-		return BCME_BADARG;
-	}
-
-	switch (ioc->cmd) {
-	case DHD_GET_MAGIC:
-		if (buflen < sizeof(int))
-			bcmerror = BCME_BUFTOOSHORT;
-		else
-			*(int*)buf = DHD_IOCTL_MAGIC;
-		break;
-
-	case DHD_GET_VERSION:
-		if (buflen < sizeof(int))
-			bcmerror = -BCME_BUFTOOSHORT;
-		else
-			*(int*)buf = DHD_IOCTL_VERSION;
-		break;
-
-	case DHD_GET_VAR:
-	case DHD_SET_VAR: {
-		char *arg;
-		uint arglen;
-
-		/* scan past the name to any arguments */
-		for (arg = buf, arglen = buflen; *arg && arglen; arg++, arglen--)
-			;
-
-		if (*arg) {
-			bcmerror = BCME_BUFTOOSHORT;
-			break;
-		}
-
-		/* account for the NUL terminator */
-		arg++, arglen--;
-
-		/* call with the appropriate arguments */
-		if (ioc->cmd == DHD_GET_VAR)
-			bcmerror = dhd_iovar_op(dhd_pub, buf, arg, arglen,
-			buf, buflen, IOV_GET);
-		else
-			bcmerror = dhd_iovar_op(dhd_pub, buf, NULL, 0, arg, arglen, IOV_SET);
-		if (bcmerror != BCME_UNSUPPORTED)
-			break;
-
-		/* not in generic table, try protocol module */
-		if (ioc->cmd == DHD_GET_VAR)
-			bcmerror = dhd_prot_iovar_op(dhd_pub, buf, arg,
-				arglen, buf, buflen, IOV_GET);
-		else
-			bcmerror = dhd_prot_iovar_op(dhd_pub, buf,
-				NULL, 0, arg, arglen, IOV_SET);
-		if (bcmerror != BCME_UNSUPPORTED)
-			break;
-
-		/* if still not found, try bus module */
-		if (ioc->cmd == DHD_GET_VAR) {
-			bcmerror = dhd_bus_iovar_op(dhd_pub, buf,
-				arg, arglen, buf, buflen, IOV_GET);
-		} else {
-			bcmerror = dhd_bus_iovar_op(dhd_pub, buf,
-				NULL, 0, arg, arglen, IOV_SET);
-		}
-
-		break;
-	}
-
-	default:
-		bcmerror = BCME_UNSUPPORTED;
-	}
-
-	return bcmerror;
-}
-
-#ifdef SHOW_EVENTS
-static void
-wl_show_host_event(wl_event_msg_t *event, void *event_data)
-{
-	uint i, status, reason;
-	bool group = FALSE, flush_txq = FALSE, link = FALSE;
-	const char *auth_str;
-	const char *event_name;
-	uchar *buf;
-	char err_msg[256], eabuf[ETHER_ADDR_STR_LEN];
-	uint event_type, flags, auth_type, datalen;
-
-	event_type = ntoh32(event->event_type);
-	flags = ntoh16(event->flags);
-	status = ntoh32(event->status);
-	reason = ntoh32(event->reason);
-	auth_type = ntoh32(event->auth_type);
-	datalen = ntoh32(event->datalen);
-
-	/* debug dump of event messages */
-	sprintf(eabuf, "%02x:%02x:%02x:%02x:%02x:%02x",
-	        (uchar)event->addr.octet[0]&0xff,
-	        (uchar)event->addr.octet[1]&0xff,
-	        (uchar)event->addr.octet[2]&0xff,
-	        (uchar)event->addr.octet[3]&0xff,
-	        (uchar)event->addr.octet[4]&0xff,
-	        (uchar)event->addr.octet[5]&0xff);
-
-	event_name = "UNKNOWN";
-	for (i = 0; i < (uint)bcmevent_names_size; i++)
-		if (bcmevent_names[i].event == event_type)
-			event_name = bcmevent_names[i].name;
-
-	if (flags & WLC_EVENT_MSG_LINK)
-		link = TRUE;
-	if (flags & WLC_EVENT_MSG_GROUP)
-		group = TRUE;
-	if (flags & WLC_EVENT_MSG_FLUSHTXQ)
-		flush_txq = TRUE;
-
-	switch (event_type) {
-	case WLC_E_START:
-	case WLC_E_DEAUTH:
-	case WLC_E_DISASSOC:
-		DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
-		break;
-
-	case WLC_E_ASSOC_IND:
-	case WLC_E_REASSOC_IND:
-
-		DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
-		break;
-
-	case WLC_E_ASSOC:
-	case WLC_E_REASSOC:
-		if (status == WLC_E_STATUS_SUCCESS) {
-			DHD_EVENT(("MACEVENT: %s, MAC %s, SUCCESS\n", event_name, eabuf));
-		} else if (status == WLC_E_STATUS_TIMEOUT) {
-			DHD_EVENT(("MACEVENT: %s, MAC %s, TIMEOUT\n", event_name, eabuf));
-		} else if (status == WLC_E_STATUS_FAIL) {
-			DHD_EVENT(("MACEVENT: %s, MAC %s, FAILURE, reason %d\n",
-			       event_name, eabuf, (int)reason));
-		} else {
-			DHD_EVENT(("MACEVENT: %s, MAC %s, unexpected status %d\n",
-			       event_name, eabuf, (int)status));
-		}
-		break;
-
-	case WLC_E_DEAUTH_IND:
-	case WLC_E_DISASSOC_IND:
-		DHD_EVENT(("MACEVENT: %s, MAC %s, reason %d\n", event_name, eabuf, (int)reason));
-		break;
-
-	case WLC_E_AUTH:
-	case WLC_E_AUTH_IND:
-		if (auth_type == DOT11_OPEN_SYSTEM)
-			auth_str = "Open System";
-		else if (auth_type == DOT11_SHARED_KEY)
-			auth_str = "Shared Key";
-		else {
-			sprintf(err_msg, "AUTH unknown: %d", (int)auth_type);
-			auth_str = err_msg;
-		}
-		if (event_type == WLC_E_AUTH_IND) {
-			DHD_EVENT(("MACEVENT: %s, MAC %s, %s\n", event_name, eabuf, auth_str));
-		} else if (status == WLC_E_STATUS_SUCCESS) {
-			DHD_EVENT(("MACEVENT: %s, MAC %s, %s, SUCCESS\n",
-				event_name, eabuf, auth_str));
-		} else if (status == WLC_E_STATUS_TIMEOUT) {
-			DHD_EVENT(("MACEVENT: %s, MAC %s, %s, TIMEOUT\n",
-				event_name, eabuf, auth_str));
-		} else if (status == WLC_E_STATUS_FAIL) {
-			DHD_EVENT(("MACEVENT: %s, MAC %s, %s, FAILURE, reason %d\n",
-			       event_name, eabuf, auth_str, (int)reason));
-		}
-
-		break;
-
-	case WLC_E_JOIN:
-	case WLC_E_ROAM:
-	case WLC_E_SET_SSID:
-		if (status == WLC_E_STATUS_SUCCESS) {
-			DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
-		} else if (status == WLC_E_STATUS_FAIL) {
-			DHD_EVENT(("MACEVENT: %s, failed\n", event_name));
-		} else if (status == WLC_E_STATUS_NO_NETWORKS) {
-			DHD_EVENT(("MACEVENT: %s, no networks found\n", event_name));
-		} else {
-			DHD_EVENT(("MACEVENT: %s, unexpected status %d\n",
-				event_name, (int)status));
-		}
-		break;
-
-	case WLC_E_BEACON_RX:
-		if (status == WLC_E_STATUS_SUCCESS) {
-			DHD_EVENT(("MACEVENT: %s, SUCCESS\n", event_name));
-		} else if (status == WLC_E_STATUS_FAIL) {
-			DHD_EVENT(("MACEVENT: %s, FAIL\n", event_name));
-		} else {
-			DHD_EVENT(("MACEVENT: %s, status %d\n", event_name, status));
-		}
-		break;
-
-	case WLC_E_LINK:
-		DHD_EVENT(("MACEVENT: %s %s\n", event_name, link?"UP":"DOWN"));
-		break;
-
-	case WLC_E_MIC_ERROR:
-		DHD_EVENT(("MACEVENT: %s, MAC %s, Group %d, Flush %d\n",
-		       event_name, eabuf, group, flush_txq));
-		break;
-
-	case WLC_E_ICV_ERROR:
-	case WLC_E_UNICAST_DECODE_ERROR:
-	case WLC_E_MULTICAST_DECODE_ERROR:
-		DHD_EVENT(("MACEVENT: %s, MAC %s\n",
-		       event_name, eabuf));
-		break;
-
-	case WLC_E_TXFAIL:
-		DHD_EVENT(("MACEVENT: %s, RA %s\n", event_name, eabuf));
-		break;
-
-	case WLC_E_SCAN_COMPLETE:
-	case WLC_E_PMKID_CACHE:
-		DHD_EVENT(("MACEVENT: %s\n", event_name));
-		break;
-
-	case WLC_E_PFN_NET_FOUND:
-	case WLC_E_PFN_NET_LOST:
-	case WLC_E_PFN_SCAN_COMPLETE:
-	case WLC_E_PFN_SCAN_NONE:
-	case WLC_E_PFN_SCAN_ALLGONE:
-		DHD_EVENT(("PNOEVENT: %s\n", event_name));
-		break;
-
-	case WLC_E_PSK_SUP:
-	case WLC_E_PRUNE:
-		DHD_EVENT(("MACEVENT: %s, status %d, reason %d\n",
-		           event_name, (int)status, (int)reason));
-		break;
-
-#ifdef WIFI_ACT_FRAME
-	case WLC_E_ACTION_FRAME:
-		DHD_TRACE(("MACEVENT: %s Bssid %s\n", event_name, eabuf));
-		break;
-#endif /* WIFI_ACT_FRAME */
-
-	case WLC_E_TRACE: {
-		static uint32 seqnum_prev = 0;
-		msgtrace_hdr_t hdr;
-		uint32 nblost;
-		char *s, *p;
-
-		buf = (uchar *) event_data;
-		memcpy(&hdr, buf, MSGTRACE_HDRLEN);
-
-		if (hdr.version != MSGTRACE_VERSION) {
-			printf("\nMACEVENT: %s [unsupported version --> "
-			       "dhd version:%d dongle version:%d]\n",
-			       event_name, MSGTRACE_VERSION, hdr.version);
-			/* Reset datalen to avoid display below */
-			datalen = 0;
-			break;
-		}
-
-		/* There are 2 bytes available at the end of data */
-		buf[MSGTRACE_HDRLEN + ntoh16(hdr.len)] = '\0';
-
-		if (ntoh32(hdr.discarded_bytes) || ntoh32(hdr.discarded_printf)) {
-			printf("\nWLC_E_TRACE: [Discarded traces in dongle -->"
-			       "discarded_bytes %d discarded_printf %d]\n",
-			       ntoh32(hdr.discarded_bytes), ntoh32(hdr.discarded_printf));
-		}
-
-		nblost = ntoh32(hdr.seqnum) - seqnum_prev - 1;
-		if (nblost > 0) {
-			printf("\nWLC_E_TRACE: [Event lost --> seqnum %d nblost %d\n",
-			       ntoh32(hdr.seqnum), nblost);
-		}
-		seqnum_prev = ntoh32(hdr.seqnum);
-
-		/* Display the trace buffer. Advance from \n to \n to avoid display big
-		 * printf (issue with Linux printk )
-		 */
-		p = (char *)&buf[MSGTRACE_HDRLEN];
-		while ((s = strstr(p, "\n")) != NULL) {
-			*s = '\0';
-			printf("%s\n", p);
-			p = s+1;
-		}
-		printf("%s\n", p);
-
-		/* Reset datalen to avoid display below */
-		datalen = 0;
-		break;
-	}
-
-
-	case WLC_E_RSSI:
-		DHD_EVENT(("MACEVENT: %s %d\n", event_name, ntoh32(*((int *)event_data))));
-		break;
-
-	default:
-		DHD_EVENT(("MACEVENT: %s %d, MAC %s, status %d, reason %d, auth %d\n",
-		       event_name, event_type, eabuf, (int)status, (int)reason,
-		       (int)auth_type));
-		break;
-	}
-
-	/* show any appended data */
-	if (datalen) {
-		buf = (uchar *) event_data;
-		DHD_EVENT((" data (%d) : ", datalen));
-		for (i = 0; i < datalen; i++)
-			DHD_EVENT((" 0x%02x ", *buf++));
-		DHD_EVENT(("\n"));
-	}
-}
-#endif /* SHOW_EVENTS */
-
-int
-wl_host_event(dhd_pub_t *dhd_pub, int *ifidx, void *pktdata,
-              wl_event_msg_t *event, void **data_ptr)
-{
-	/* check whether packet is a BRCM event pkt */
-	bcm_event_t *pvt_data = (bcm_event_t *)pktdata;
-	char *event_data;
-	uint32 type, status, reason, datalen;
-	uint16 flags;
-	int evlen;
-
-	if (bcmp(BRCM_OUI, &pvt_data->bcm_hdr.oui[0], DOT11_OUI_LEN)) {
-		DHD_ERROR(("%s: mismatched OUI, bailing\n", __FUNCTION__));
-		return (BCME_ERROR);
-	}
-
-	/* BRCM event pkt may be unaligned - use xxx_ua to load user_subtype. */
-	if (ntoh16_ua((void *)&pvt_data->bcm_hdr.usr_subtype) != BCMILCP_BCM_SUBTYPE_EVENT) {
-		DHD_ERROR(("%s: mismatched subtype, bailing\n", __FUNCTION__));
-		return (BCME_ERROR);
-	}
-
-	*data_ptr = &pvt_data[1];
-	event_data = *data_ptr;
-
-	/* memcpy since BRCM event pkt may be unaligned. */
-	memcpy(event, &pvt_data->event, sizeof(wl_event_msg_t));
-
-	type = ntoh32_ua((void *)&event->event_type);
-	flags = ntoh16_ua((void *)&event->flags);
-	status = ntoh32_ua((void *)&event->status);
-	reason = ntoh32_ua((void *)&event->reason);
-	datalen = ntoh32_ua((void *)&event->datalen);
-	evlen = datalen + sizeof(bcm_event_t);
-
-	switch (type) {
-#ifdef PROP_TXSTATUS
-	case WLC_E_FIFO_CREDIT_MAP:
-		dhd_wlfc_event(dhd_pub->info);
-		dhd_wlfc_FIFOcreditmap_event(dhd_pub->info, event_data);
-		WLFC_DBGMESG(("WLC_E_FIFO_CREDIT_MAP:(AC0,AC1,AC2,AC3),(BC_MC),(OTHER): "
-			"(%d,%d,%d,%d),(%d),(%d)\n", event_data[0], event_data[1],
-			event_data[2],
-			event_data[3], event_data[4], event_data[5]));
-		break;
-#endif
-
-	case WLC_E_IF:
-		{
-		dhd_if_event_t *ifevent = (dhd_if_event_t *)event_data;
-#ifdef PROP_TXSTATUS
-{
-		uint8* ea = pvt_data->eth.ether_dhost;
-		WLFC_DBGMESG(("WLC_E_IF: idx:%d, action:%s, iftype:%s, "
-		              "[%02x:%02x:%02x:%02x:%02x:%02x]\n",
-		              ifevent->ifidx,
-		              ((ifevent->action == WLC_E_IF_ADD) ? "ADD":"DEL"),
-		              ((ifevent->is_AP == 0) ? "STA":"AP "),
-		              ea[0], ea[1], ea[2], ea[3], ea[4], ea[5]));
-		(void)ea;
-
-		dhd_wlfc_interface_event(dhd_pub->info,
-		                         ((ifevent->action == WLC_E_IF_ADD) ?
-		                          eWLFC_MAC_ENTRY_ACTION_ADD : eWLFC_MAC_ENTRY_ACTION_DEL),
-		                         ifevent->ifidx, ifevent->is_AP, ea);
-
-		/* dhd already has created an interface by default, for 0 */
-		if (ifevent->ifidx == 0)
-			break;
-}
-#endif /* PROP_TXSTATUS */
-
-#ifdef WL_CFG80211
-			if (wl_cfg80211_is_progress_ifchange()) {
-				DHD_ERROR(("%s:  ifidx %d for %s action %d\n",
-					__FUNCTION__, ifevent->ifidx,
-					event->ifname, ifevent->action));
-			if (ifevent->action == WLC_E_IF_ADD)
-					wl_cfg80211_notify_ifchange();
-				return (BCME_OK);
-			}
-#endif /* WL_CFG80211 */
-				if (ifevent->ifidx > 0 && ifevent->ifidx < DHD_MAX_IFS) {
-					if (ifevent->action == WLC_E_IF_ADD) {
-						if (dhd_add_if(dhd_pub->info, ifevent->ifidx,
-							NULL, event->ifname,
-							event->addr.octet,
-							ifevent->flags, ifevent->bssidx)) {
-							DHD_ERROR(("%s: dhd_add_if failed!!"
-									" ifidx: %d for %s\n",
-									__FUNCTION__,
-									ifevent->ifidx,
-									event->ifname));
-							return (BCME_ERROR);
-						}
-					}
-			else
-				dhd_del_if(dhd_pub->info, ifevent->ifidx);
-		} else {
-#ifndef PROP_TXSTATUS
-			DHD_ERROR(("%s: Invalid ifidx %d for %s\n",
-			           __FUNCTION__, ifevent->ifidx, event->ifname));
-#endif /* !PROP_TXSTATUS */
-		}
-			}
-			/* send up the if event: btamp user needs it */
-			*ifidx = dhd_ifname2idx(dhd_pub->info, event->ifname);
-			/* push up to external supp/auth */
-			dhd_event(dhd_pub->info, (char *)pvt_data, evlen, *ifidx);
-			break;
-
-
-#ifdef WLMEDIA_HTSF
-	case WLC_E_HTSFSYNC:
-		htsf_update(dhd_pub->info, event_data);
-		break;
-#endif /* WLMEDIA_HTSF */
-	case WLC_E_NDIS_LINK: {
-		uint32 temp = hton32(WLC_E_LINK);
-
-		memcpy((void *)(&pvt_data->event.event_type), &temp,
-		       sizeof(pvt_data->event.event_type));
-	}
-		/* These are what external supplicant/authenticator wants */
-		/* fall through */
-	case WLC_E_LINK:
-	case WLC_E_DEAUTH:
-	case WLC_E_DEAUTH_IND:
-	case WLC_E_DISASSOC:
-	case WLC_E_DISASSOC_IND:
-		DHD_EVENT(("%s: Link event %d, flags %x, status %x\n",
-		           __FUNCTION__, type, flags, status));
-		/* fall through */
-	default:
-		*ifidx = dhd_ifname2idx(dhd_pub->info, event->ifname);
-		/* push up to external supp/auth */
-		dhd_event(dhd_pub->info, (char *)pvt_data, evlen, *ifidx);
-		DHD_TRACE(("%s: MAC event %d, flags %x, status %x\n",
-		           __FUNCTION__, type, flags, status));
-
-		/* put it back to WLC_E_NDIS_LINK */
-		if (type == WLC_E_NDIS_LINK) {
-			uint32 temp;
-
-			temp = ntoh32_ua((void *)&event->event_type);
-			DHD_TRACE(("Converted to WLC_E_LINK type %d\n", temp));
-
-			temp = ntoh32(WLC_E_NDIS_LINK);
-			memcpy((void *)(&pvt_data->event.event_type), &temp,
-			       sizeof(pvt_data->event.event_type));
-		}
-		break;
-	}
-
-#ifdef SHOW_EVENTS
-	wl_show_host_event(event, (void *)event_data);
-#endif /* SHOW_EVENTS */
-
-	return (BCME_OK);
-}
-
-void
-wl_event_to_host_order(wl_event_msg_t * evt)
-{
-	/* Event struct members passed from dongle to host are stored in network
-	 * byte order. Convert all members to host-order.
-	 */
-	evt->event_type = ntoh32(evt->event_type);
-	evt->flags = ntoh16(evt->flags);
-	evt->status = ntoh32(evt->status);
-	evt->reason = ntoh32(evt->reason);
-	evt->auth_type = ntoh32(evt->auth_type);
-	evt->datalen = ntoh32(evt->datalen);
-	evt->version = ntoh16(evt->version);
-}
-
-void
-dhd_print_buf(void *pbuf, int len, int bytes_per_line)
-{
-#ifdef DHD_DEBUG
-	int i, j = 0;
-	unsigned char *buf = pbuf;
-
-	if (bytes_per_line == 0) {
-		bytes_per_line = len;
-	}
-
-	for (i = 0; i < len; i++) {
-		printf("%2.2x", *buf++);
-		j++;
-		if (j == bytes_per_line) {
-			printf("\n");
-			j = 0;
-		} else {
-			printf(":");
-		}
-	}
-	printf("\n");
-#endif /* DHD_DEBUG */
-}
-
-#define strtoul(nptr, endptr, base) bcm_strtoul((nptr), (endptr), (base))
-
-/* Convert user's input in hex pattern to byte-size mask */
-static int
-wl_pattern_atoh(char *src, char *dst)
-{
-	int i;
-	if (strncmp(src, "0x", 2) != 0 &&
-	    strncmp(src, "0X", 2) != 0) {
-		DHD_ERROR(("Mask invalid format. Needs to start with 0x\n"));
-		return -1;
-	}
-	src = src + 2; /* Skip past 0x */
-	if (strlen(src) % 2 != 0) {
-		DHD_ERROR(("Mask invalid format. Needs to be of even length\n"));
-		return -1;
-	}
-	for (i = 0; *src != '\0'; i++) {
-		char num[3];
-		bcm_strncpy_s(num, sizeof(num), src, 2);
-		num[2] = '\0';
-		dst[i] = (uint8)strtoul(num, NULL, 16);
-		src += 2;
-	}
-	return i;
-}
-
-void
-dhd_pktfilter_offload_enable(dhd_pub_t * dhd, char *arg, int enable, int master_mode)
-{
-	char				*argv[8];
-	int					i = 0;
-	const char 			*str;
-	int					buf_len;
-	int					str_len;
-	char				*arg_save = 0, *arg_org = 0;
-	int					rc;
-	char				buf[128];
-	wl_pkt_filter_enable_t	enable_parm;
-	wl_pkt_filter_enable_t	* pkt_filterp;
-
-	if (!arg)
-		return;
-
-	if (!(arg_save = MALLOC(dhd->osh, strlen(arg) + 1))) {
-		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
-		goto fail;
-	}
-	arg_org = arg_save;
-	memcpy(arg_save, arg, strlen(arg) + 1);
-
-	argv[i] = bcmstrtok(&arg_save, " ", 0);
-
-	i = 0;
-	if (argv[i] == NULL) {
-		DHD_ERROR(("No args provided\n"));
-		goto fail;
-	}
-
-	str = "pkt_filter_enable";
-	str_len = strlen(str);
-	bcm_strncpy_s(buf, sizeof(buf), str, str_len);
-	buf[str_len] = '\0';
-	buf_len = str_len + 1;
-
-	pkt_filterp = (wl_pkt_filter_enable_t *)(buf + str_len + 1);
-
-	/* Parse packet filter id. */
-	enable_parm.id = htod32(strtoul(argv[i], NULL, 0));
-
-	/* Parse enable/disable value. */
-	enable_parm.enable = htod32(enable);
-
-	buf_len += sizeof(enable_parm);
-	memcpy((char *)pkt_filterp,
-	       &enable_parm,
-	       sizeof(enable_parm));
-
-	/* Enable/disable the specified filter. */
-	rc = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, buf_len, TRUE, 0);
-	rc = rc >= 0 ? 0 : rc;
-	if (rc)
-		DHD_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
-		__FUNCTION__, arg, rc));
-	else
-		DHD_TRACE(("%s: successfully added pktfilter %s\n",
-		__FUNCTION__, arg));
-
-	/* Contorl the master mode */
-	bcm_mkiovar("pkt_filter_mode", (char *)&master_mode, 4, buf, sizeof(buf));
-	rc = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, sizeof(buf), TRUE, 0);
-	rc = rc >= 0 ? 0 : rc;
-	if (rc)
-		DHD_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
-		__FUNCTION__, arg, rc));
-
-fail:
-	if (arg_org)
-		MFREE(dhd->osh, arg_org, strlen(arg) + 1);
-}
-
-void
-dhd_pktfilter_offload_set(dhd_pub_t * dhd, char *arg)
-{
-	const char 			*str;
-	wl_pkt_filter_t		pkt_filter;
-	wl_pkt_filter_t		*pkt_filterp;
-	int					buf_len;
-	int					str_len;
-	int 				rc;
-	uint32				mask_size;
-	uint32				pattern_size;
-	char				*argv[8], * buf = 0;
-	int					i = 0;
-	char				*arg_save = 0, *arg_org = 0;
-#define BUF_SIZE		2048
-
-	if (!arg)
-		return;
-
-	if (!(arg_save = MALLOC(dhd->osh, strlen(arg) + 1))) {
-		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
-		goto fail;
-	}
-
-	arg_org = arg_save;
-
-	if (!(buf = MALLOC(dhd->osh, BUF_SIZE))) {
-		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
-		goto fail;
-	}
-
-	memcpy(arg_save, arg, strlen(arg) + 1);
-
-	if (strlen(arg) > BUF_SIZE) {
-		DHD_ERROR(("Not enough buffer %d < %d\n", (int)strlen(arg), (int)sizeof(buf)));
-		goto fail;
-	}
-
-	argv[i] = bcmstrtok(&arg_save, " ", 0);
-	while (argv[i++])
-		argv[i] = bcmstrtok(&arg_save, " ", 0);
-
-	i = 0;
-	if (argv[i] == NULL) {
-		DHD_ERROR(("No args provided\n"));
-		goto fail;
-	}
-
-	str = "pkt_filter_add";
-	str_len = strlen(str);
-	bcm_strncpy_s(buf, BUF_SIZE, str, str_len);
-	buf[ str_len ] = '\0';
-	buf_len = str_len + 1;
-
-	pkt_filterp = (wl_pkt_filter_t *) (buf + str_len + 1);
-
-	/* Parse packet filter id. */
-	pkt_filter.id = htod32(strtoul(argv[i], NULL, 0));
-
-	if (argv[++i] == NULL) {
-		DHD_ERROR(("Polarity not provided\n"));
-		goto fail;
-	}
-
-	/* Parse filter polarity. */
-	pkt_filter.negate_match = htod32(strtoul(argv[i], NULL, 0));
-
-	if (argv[++i] == NULL) {
-		DHD_ERROR(("Filter type not provided\n"));
-		goto fail;
-	}
-
-	/* Parse filter type. */
-	pkt_filter.type = htod32(strtoul(argv[i], NULL, 0));
-
-	if (argv[++i] == NULL) {
-		DHD_ERROR(("Offset not provided\n"));
-		goto fail;
-	}
-
-	/* Parse pattern filter offset. */
-	pkt_filter.u.pattern.offset = htod32(strtoul(argv[i], NULL, 0));
-
-	if (argv[++i] == NULL) {
-		DHD_ERROR(("Bitmask not provided\n"));
-		goto fail;
-	}
-
-	/* Parse pattern filter mask. */
-	mask_size =
-		htod32(wl_pattern_atoh(argv[i], (char *) pkt_filterp->u.pattern.mask_and_pattern));
-
-	if (argv[++i] == NULL) {
-		DHD_ERROR(("Pattern not provided\n"));
-		goto fail;
-	}
-
-	/* Parse pattern filter pattern. */
-	pattern_size =
-		htod32(wl_pattern_atoh(argv[i],
-	         (char *) &pkt_filterp->u.pattern.mask_and_pattern[mask_size]));
-
-	if (mask_size != pattern_size) {
-		DHD_ERROR(("Mask and pattern not the same size\n"));
-		goto fail;
-	}
-
-	pkt_filter.u.pattern.size_bytes = mask_size;
-	buf_len += WL_PKT_FILTER_FIXED_LEN;
-	buf_len += (WL_PKT_FILTER_PATTERN_FIXED_LEN + 2 * mask_size);
-
-	/* Keep-alive attributes are set in local	variable (keep_alive_pkt), and
-	** then memcpy'ed into buffer (keep_alive_pktp) since there is no
-	** guarantee that the buffer is properly aligned.
-	*/
-	memcpy((char *)pkt_filterp,
-	       &pkt_filter,
-	       WL_PKT_FILTER_FIXED_LEN + WL_PKT_FILTER_PATTERN_FIXED_LEN);
-
-	rc = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, buf_len, TRUE, 0);
-	rc = rc >= 0 ? 0 : rc;
-
-	if (rc)
-		DHD_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
-		__FUNCTION__, arg, rc));
-	else
-		DHD_TRACE(("%s: successfully added pktfilter %s\n",
-		__FUNCTION__, arg));
-
-fail:
-	if (arg_org)
-		MFREE(dhd->osh, arg_org, strlen(arg) + 1);
-
-	if (buf)
-		MFREE(dhd->osh, buf, BUF_SIZE);
-}
-
-/* ========================== */
-/* ==== ARP OFFLOAD SUPPORT = */
-/* ========================== */
-#ifdef ARP_OFFLOAD_SUPPORT
-void
-dhd_arp_offload_set(dhd_pub_t * dhd, int arp_mode)
-{
-	char iovbuf[32];
-	int retcode;
-
-	bcm_mkiovar("arp_ol", (char *)&arp_mode, 4, iovbuf, sizeof(iovbuf));
-	retcode = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-	retcode = retcode >= 0 ? 0 : retcode;
-	if (retcode)
-		DHD_TRACE(("%s: failed to set ARP offload mode to 0x%x, retcode = %d\n",
-		__FUNCTION__, arp_mode, retcode));
-	else
-		DHD_TRACE(("%s: successfully set ARP offload mode to 0x%x\n",
-		__FUNCTION__, arp_mode));
-}
-
-void
-dhd_arp_offload_enable(dhd_pub_t * dhd, int arp_enable)
-{
-	char iovbuf[32];
-	int retcode;
-
-	bcm_mkiovar("arpoe", (char *)&arp_enable, 4, iovbuf, sizeof(iovbuf));
-	retcode = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-	retcode = retcode >= 0 ? 0 : retcode;
-	if (retcode)
-		DHD_TRACE(("%s: failed to enabe ARP offload to %d, retcode = %d\n",
-		__FUNCTION__, arp_enable, retcode));
-	else
-		DHD_TRACE(("%s: successfully enabed ARP offload to %d\n",
-		__FUNCTION__, arp_enable));
-}
-
-void dhd_aoe_arp_clr(dhd_pub_t *dhd)
-{
-	int ret = 0;
-	int iov_len = 0;
-	char iovbuf[128];
-
-	if (dhd == NULL) return;
-
-	iov_len = bcm_mkiovar("arp_table_clear", 0, 0, iovbuf, sizeof(iovbuf));
-	if ((ret  = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, iov_len, TRUE, 0) < 0))
-		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
-}
-
-void dhd_aoe_hostip_clr(dhd_pub_t *dhd)
-{
-	int ret = 0;
-	int iov_len = 0;
-	char iovbuf[128];
-
-	if (dhd == NULL) return;
-
-	iov_len = bcm_mkiovar("arp_hostip_clear", 0, 0, iovbuf, sizeof(iovbuf));
-	if ((ret  = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, iov_len, TRUE, 0)) < 0)
-		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
-}
-
-void dhd_arp_offload_add_ip(dhd_pub_t *dhd, uint32 ipaddr)
-{
-	int iov_len = 0;
-	char iovbuf[32];
-	int retcode;
-
-	iov_len = bcm_mkiovar("arp_hostip", (char *)&ipaddr, 4, iovbuf, sizeof(iovbuf));
-	retcode = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, iov_len, TRUE, 0);
-
-	if (retcode)
-		DHD_TRACE(("%s: ARP ip addr add failed, retcode = %d\n",
-		__FUNCTION__, retcode));
-	else
-		DHD_TRACE(("%s: sARP H ipaddr entry added \n",
-		__FUNCTION__));
-}
-
-int dhd_arp_get_arp_hostip_table(dhd_pub_t *dhd, void *buf, int buflen)
-{
-	int retcode, i;
-	int iov_len = 0;
-	uint32 *ptr32 = buf;
-	bool clr_bottom = FALSE;
-
-	if (!buf)
-		return -1;
-
-	iov_len = bcm_mkiovar("arp_hostip", 0, 0, buf, buflen);
-	retcode = dhd_wl_ioctl_cmd(dhd, WLC_GET_VAR, buf, buflen, TRUE, 0);
-
-	if (retcode) {
-		DHD_TRACE(("%s: ioctl WLC_GET_VAR error %d\n",
-		__FUNCTION__, retcode));
-
-		return -1;
-	}
-
-	/* clean up the buf, ascii reminder */
-	for (i = 0; i < MAX_IPV4_ENTRIES; i++) {
-		if (!clr_bottom) {
-			if (*ptr32 == 0)
-				clr_bottom = TRUE;
-		} else {
-			*ptr32 = 0;
-		}
-		ptr32++;
-	}
-
-	return 0;
-}
-#endif /* ARP_OFFLOAD_SUPPORT  */
-
-int
-dhd_preinit_ioctls(dhd_pub_t *dhd)
-{
-	int ret = 0;
-	char eventmask[WL_EVENTING_MASK_LEN];
-	char iovbuf[WL_EVENTING_MASK_LEN + 12];	/*  Room for "event_msgs" + '\0' + bitvec  */
-
-	uint up = 0;
-	uint power_mode = PM_FAST;
-	uint32 dongle_align = DHD_SDALIGN;
-	uint32 glom = 0;
-	uint bcn_timeout = 4;
-	uint retry_max = 3;
-#if defined(ARP_OFFLOAD_SUPPORT)
-	int arpoe = 1;
-#endif
-	int scan_assoc_time = 40;
-	int scan_unassoc_time = 40;
-	const char 				*str;
-	wl_pkt_filter_t		pkt_filter;
-	wl_pkt_filter_t		*pkt_filterp;
-	int						buf_len;
-	int						str_len;
-	uint32					mask_size;
-	uint32					pattern_size;
-	char buf[WLC_IOCTL_SMLEN];
-	char *ptr;
-	uint filter_mode = 1;
-	uint32 listen_interval = LISTEN_INTERVAL; /* Default Listen Interval in Beacons */
-#if defined(SOFTAP)
-	uint dtim = 1;
-#endif
-#if (defined(AP) && !defined(WLP2P)) || (!defined(AP) && defined(WL_CFG80211))
-	uint32 mpc = 0; /* Turn MPC off for AP/APSTA mode */
-#endif /* AP */
-#if defined(AP) || defined(WLP2P)
-	uint32 apsta = 1; /* Enable APSTA mode */
-#endif /* defined(AP) || defined(WLP2P) */
-#ifdef GET_CUSTOM_MAC_ENABLE
-	struct ether_addr ea_addr;
-#endif /* GET_CUSTOM_MAC_ENABLE */
-
-	dhd->op_mode = 0;
-#ifdef GET_CUSTOM_MAC_ENABLE
-	ret = dhd_custom_get_mac_address(ea_addr.octet);
-	if (!ret) {
-		memset(buf, 0, sizeof(buf));
-		bcm_mkiovar("cur_etheraddr", (void *)&ea_addr, ETHER_ADDR_LEN, buf, sizeof(buf));
-		ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, sizeof(buf), TRUE, 0);
-		if (ret < 0) {
-			DHD_ERROR(("%s: can't set MAC address , error=%d\n", __FUNCTION__, ret));
-			return BCME_NOTUP;
-		}
-	} else {
-#endif /* GET_CUSTOM_MAC_ENABLE */
-		/* Get the default device MAC address directly from firmware */
-		memset(buf, 0, sizeof(buf));
-		bcm_mkiovar("cur_etheraddr", 0, 0, buf, sizeof(buf));
-		if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_GET_VAR, buf, sizeof(buf),
-			FALSE, 0)) < 0) {
-			DHD_ERROR(("%s: can't get MAC address , error=%d\n", __FUNCTION__, ret));
-			return BCME_NOTUP;
-		}
-		/* Update public MAC address after reading from Firmware */
-		memcpy(dhd->mac.octet, buf, ETHER_ADDR_LEN);
-#ifdef GET_CUSTOM_MAC_ENABLE
-	}
-#endif /* GET_CUSTOM_MAC_ENABLE */
-
-#ifdef SET_RANDOM_MAC_SOFTAP
-	if (strstr(fw_path, "_apsta") != NULL) {
-		uint rand_mac;
-
-		srandom32((uint)jiffies);
-		rand_mac = random32();
-		iovbuf[0] = 0x02;              /* locally administered bit */
-		iovbuf[1] = 0x1A;
-		iovbuf[2] = 0x11;
-		iovbuf[3] = (unsigned char)(rand_mac & 0x0F) | 0xF0;
-		iovbuf[4] = (unsigned char)(rand_mac >> 8);
-		iovbuf[5] = (unsigned char)(rand_mac >> 16);
-
-		bcm_mkiovar("cur_etheraddr", (void *)iovbuf, ETHER_ADDR_LEN, buf, sizeof(buf));
-		ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, sizeof(buf), TRUE, 0);
-		if (ret < 0) {
-			DHD_ERROR(("%s: can't set MAC address , error=%d\n", __FUNCTION__, ret));
-		} else
-			memcpy(dhd->mac.octet, iovbuf, ETHER_ADDR_LEN);
-	}
-#endif /* SET_RANDOM_MAC_SOFTAP */
-
-	DHD_TRACE(("Firmware = %s\n", fw_path));
-#if !defined(AP) && defined(WLP2P)
-	/* Check if firmware with WFD support used */
-	if (strstr(fw_path, "_p2p") != NULL) {
-		bcm_mkiovar("apsta", (char *)&apsta, 4, iovbuf, sizeof(iovbuf));
-		if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR,
-			iovbuf, sizeof(iovbuf), TRUE, 0)) < 0) {
-			DHD_ERROR(("%s APSTA for WFD failed ret= %d\n", __FUNCTION__, ret));
-		} else {
-			dhd->op_mode |= WFD_MASK;
-#if defined(ARP_OFFLOAD_SUPPORT)
-			arpoe = 0;
-#endif /* (ARP_OFFLOAD_SUPPORT) */
-			dhd_pkt_filter_enable = FALSE;
-		}
-	}
-#endif /* !defined(AP) && defined(WLP2P) */
-
-#if !defined(AP) && defined(WL_CFG80211)
-	/* Check if firmware with HostAPD support used */
-	if (strstr(fw_path, "_apsta") != NULL) {
-			/* Turn off MPC in AP mode */
-			bcm_mkiovar("mpc", (char *)&mpc, 4, iovbuf, sizeof(iovbuf));
-			if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf,
-				sizeof(iovbuf), TRUE, 0)) < 0) {
-				DHD_ERROR(("%s mpc for HostAPD failed  %d\n", __FUNCTION__, ret));
-			} else {
-				dhd->op_mode |= HOSTAPD_MASK;
-#if defined(ARP_OFFLOAD_SUPPORT)
-				arpoe = 0;
-#endif /* (ARP_OFFLOAD_SUPPORT) */
-				dhd_pkt_filter_enable = FALSE;
-			}
-	}
-#endif /* !defined(AP) && defined(WL_CFG80211) */
-
-	if ((dhd->op_mode != WFD_MASK) && (dhd->op_mode != HOSTAPD_MASK)) {
-		/* STA only operation mode */
-		dhd->op_mode |= STA_MASK;
-		dhd_pkt_filter_enable = TRUE;
-	}
-	DHD_ERROR(("Firmware up: op_mode=%d, "
-			"Broadcom Dongle Host Driver mac=%.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
-			dhd->op_mode,
-			dhd->mac.octet[0], dhd->mac.octet[1], dhd->mac.octet[2],
-			dhd->mac.octet[3], dhd->mac.octet[4], dhd->mac.octet[5]));
-
-	/* Set Country code  */
-	if (dhd->dhd_cspec.ccode[0] != 0) {
-		bcm_mkiovar("country", (char *)&dhd->dhd_cspec,
-			sizeof(wl_country_t), iovbuf, sizeof(iovbuf));
-		if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0)) < 0)
-			DHD_ERROR(("%s: country code setting failed\n", __FUNCTION__));
-	}
-
-	/* Set Listen Interval */
-	bcm_mkiovar("assoc_listen", (char *)&listen_interval, 4, iovbuf, sizeof(iovbuf));
-	if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0)) < 0)
-		DHD_ERROR(("%s assoc_listen failed %d\n", __FUNCTION__, ret));
-
-	/* query for 'ver' to get version info from firmware */
-	memset(buf, 0, sizeof(buf));
-	ptr = buf;
-	bcm_mkiovar("ver", (char *)&buf, 4, buf, sizeof(buf));
-	if ((ret  = dhd_wl_ioctl_cmd(dhd, WLC_GET_VAR, buf, sizeof(buf), FALSE, 0)) < 0)
-		DHD_ERROR(("%s failed %d\n", __FUNCTION__, ret));
-	else {
-		bcmstrtok(&ptr, "\n", 0);
-		/* Print fw version info */
-		DHD_ERROR(("Firmware version = %s\n", buf));
-	}
-	/* Set PowerSave mode */
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_PM, (char *)&power_mode, sizeof(power_mode), TRUE, 0);
-
-	/* Match Host and Dongle rx alignment */
-	bcm_mkiovar("bus:txglomalign", (char *)&dongle_align, 4, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-
-	/* disable glom option per default */
-	bcm_mkiovar("bus:txglom", (char *)&glom, 4, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-
-	/* Setup timeout if Beacons are lost and roam is off to report link down */
-	bcm_mkiovar("bcn_timeout", (char *)&bcn_timeout, 4, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-	/* Setup assoc_retry_max count to reconnect target AP in dongle */
-	bcm_mkiovar("assoc_retry_max", (char *)&retry_max, 4, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-#if defined(AP) && !defined(WLP2P)
-	/* Turn off MPC in AP mode */
-	bcm_mkiovar("mpc", (char *)&mpc, 4, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-	bcm_mkiovar("apsta", (char *)&apsta, 4, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-#endif /* defined(AP) && !defined(WLP2P) */
-#if defined(SOFTAP)
-	if (ap_fw_loaded == TRUE) {
-		dhd_wl_ioctl_cmd(dhd, WLC_SET_DTIMPRD, (char *)&dtim, sizeof(dtim), TRUE, 0);
-	}
-#endif
-
-#if defined(KEEP_ALIVE)
-	{
-	/* Set Keep Alive : be sure to use FW with -keepalive */
-	int res;
-
-#if defined(SOFTAP)
-	if (ap_fw_loaded == FALSE)
-#endif
-		if ((res = dhd_keep_alive_onoff(dhd)) < 0)
-			DHD_ERROR(("%s set keeplive failed %d\n",
-			__FUNCTION__, res));
-	}
-#endif /* defined(KEEP_ALIVE) */
-
-	/* Force STA UP */
-	ret = dhd_wl_ioctl_cmd(dhd, WLC_UP, (char *)&up, sizeof(up), TRUE, 0);
-	if (ret < 0)
-		goto done;
-
-	/* Setup event_msgs */
-	bcm_mkiovar("event_msgs", eventmask, WL_EVENTING_MASK_LEN, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhd, WLC_GET_VAR, iovbuf, sizeof(iovbuf), FALSE, 0);
-	if (ret < 0)
-		goto done;
-	bcopy(iovbuf, eventmask, WL_EVENTING_MASK_LEN);
-
-	/* Setup event_msgs */
-	setbit(eventmask, WLC_E_SET_SSID);
-	setbit(eventmask, WLC_E_PRUNE);
-	setbit(eventmask, WLC_E_AUTH);
-	setbit(eventmask, WLC_E_REASSOC);
-	setbit(eventmask, WLC_E_REASSOC_IND);
-	setbit(eventmask, WLC_E_DEAUTH_IND);
-	setbit(eventmask, WLC_E_DISASSOC_IND);
-	setbit(eventmask, WLC_E_DISASSOC);
-	setbit(eventmask, WLC_E_JOIN);
-	setbit(eventmask, WLC_E_ASSOC_IND);
-	setbit(eventmask, WLC_E_PSK_SUP);
-	setbit(eventmask, WLC_E_LINK);
-	setbit(eventmask, WLC_E_NDIS_LINK);
-	setbit(eventmask, WLC_E_MIC_ERROR);
-	setbit(eventmask, WLC_E_PMKID_CACHE);
-	setbit(eventmask, WLC_E_TXFAIL);
-	setbit(eventmask, WLC_E_JOIN_START);
-	setbit(eventmask, WLC_E_SCAN_COMPLETE);
-#ifdef WLMEDIA_HTSF
-	setbit(eventmask, WLC_E_HTSFSYNC);
-#endif
-	bcm_mkiovar("event_msgs", eventmask, WL_EVENTING_MASK_LEN, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_SCAN_CHANNEL_TIME, (char *)&scan_assoc_time,
-		sizeof(scan_assoc_time), TRUE, 0);
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_SCAN_UNASSOC_TIME, (char *)&scan_unassoc_time,
-		sizeof(scan_unassoc_time), TRUE, 0);
-
-	/* add a default packet filter pattern */
-	str = "pkt_filter_add";
-	str_len = strlen(str);
-	strncpy(buf, str, str_len);
-	buf[ str_len ] = '\0';
-	buf_len = str_len + 1;
-
-	pkt_filterp = (wl_pkt_filter_t *) (buf + str_len + 1);
-
-	/* Parse packet filter id. */
-	pkt_filter.id = htod32(100);
-
-	/* Parse filter polarity. */
-	pkt_filter.negate_match = htod32(0);
-
-	/* Parse filter type. */
-	pkt_filter.type = htod32(0);
-
-	/* Parse pattern filter offset. */
-	pkt_filter.u.pattern.offset = htod32(0);
-
-	/* Parse pattern filter mask. */
-	mask_size =	htod32(wl_pattern_atoh("0x01",
-		(char *) pkt_filterp->u.pattern.mask_and_pattern));
-
-	/* Parse pattern filter pattern. */
-	pattern_size = htod32(wl_pattern_atoh("0x00",
-		(char *) &pkt_filterp->u.pattern.mask_and_pattern[mask_size]));
-
-	if (mask_size != pattern_size) {
-		DHD_ERROR(("Mask and pattern not the same size\n"));
-		return -EINVAL;
-	}
-
-	pkt_filter.u.pattern.size_bytes = mask_size;
-	buf_len += WL_PKT_FILTER_FIXED_LEN;
-	buf_len += (WL_PKT_FILTER_PATTERN_FIXED_LEN + 2 * mask_size);
-
-	/* Keep-alive attributes are set in local	variable (keep_alive_pkt), and
-	** then memcpy'ed into buffer (keep_alive_pktp) since there is no
-	** guarantee that the buffer is properly aligned.
-	*/
-	memcpy((char *)pkt_filterp, &pkt_filter,
-		WL_PKT_FILTER_FIXED_LEN + WL_PKT_FILTER_PATTERN_FIXED_LEN);
-
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, buf_len, TRUE, 0);
-
-	/* set mode to allow pattern */
-	bcm_mkiovar("pkt_filter_mode", (char *)&filter_mode, 4, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-
-#ifdef ARP_OFFLOAD_SUPPORT
-	/* Set and enable ARP offload feature for STA only  */
-	if (arpoe && !ap_fw_loaded) {
-		dhd_arp_offload_set(dhd, dhd_arp_mode);
-		dhd_arp_offload_enable(dhd, arpoe);
-	} else {
-		dhd_arp_offload_set(dhd, 0);
-		dhd_arp_offload_enable(dhd, FALSE);
-	}
-#endif /* ARP_OFFLOAD_SUPPORT */
-
-#ifdef PKT_FILTER_SUPPORT
-	if (ap_fw_loaded) {
-		int i;
-		for (i = 0; i < dhd->pktfilter_count; i++) {
-			dhd_pktfilter_offload_enable(dhd, dhd->pktfilter[i],
-				0, dhd_master_mode);
-		}
-	}
-#endif /* PKT_FILTER_SUPPORT */
-
-
-done:
-	return ret;
-}
-
-/* send up locally generated event */
-void
-dhd_sendup_event_common(dhd_pub_t *dhdp, wl_event_msg_t *event, void *data)
-{
-	switch (ntoh32(event->event_type)) {
-	case WLC_E_BTA_HCI_EVENT:
-		break;
-	default:
-		break;
-	}
-
-	/* Call per-port handler. */
-	dhd_sendup_event(dhdp, event, data);
-}
-
-
-
-#ifdef SIMPLE_ISCAN
-
-uint iscan_thread_id = 0;
-iscan_buf_t * iscan_chain = 0;
-
-iscan_buf_t *
-dhd_iscan_allocate_buf(dhd_pub_t *dhd, iscan_buf_t **iscanbuf)
-{
-	iscan_buf_t *iscanbuf_alloc = 0;
-	iscan_buf_t *iscanbuf_head;
-
-	DHD_ISCAN(("%s: Entered\n", __FUNCTION__));
-	dhd_iscan_lock();
-
-	iscanbuf_alloc = (iscan_buf_t*)MALLOC(dhd->osh, sizeof(iscan_buf_t));
-	if (iscanbuf_alloc == NULL)
-		goto fail;
-
-	iscanbuf_alloc->next = NULL;
-	iscanbuf_head = *iscanbuf;
-
-	DHD_ISCAN(("%s: addr of allocated node = 0x%X"
-		   "addr of iscanbuf_head = 0x%X dhd = 0x%X\n",
-		   __FUNCTION__, iscanbuf_alloc, iscanbuf_head, dhd));
-
-	if (iscanbuf_head == NULL) {
-		*iscanbuf = iscanbuf_alloc;
-		DHD_ISCAN(("%s: Head is allocated\n", __FUNCTION__));
-		goto fail;
-	}
-
-	while (iscanbuf_head->next)
-		iscanbuf_head = iscanbuf_head->next;
-
-	iscanbuf_head->next = iscanbuf_alloc;
-
-fail:
-	dhd_iscan_unlock();
-	return iscanbuf_alloc;
-}
-
-void
-dhd_iscan_free_buf(void *dhdp, iscan_buf_t *iscan_delete)
-{
-	iscan_buf_t *iscanbuf_free = 0;
-	iscan_buf_t *iscanbuf_prv = 0;
-	iscan_buf_t *iscanbuf_cur;
-	dhd_pub_t *dhd = dhd_bus_pub(dhdp);
-	DHD_ISCAN(("%s: Entered\n", __FUNCTION__));
-
-	dhd_iscan_lock();
-
-	iscanbuf_cur = iscan_chain;
-
-	/* If iscan_delete is null then delete the entire
-	 * chain or else delete specific one provided
-	 */
-	if (!iscan_delete) {
-		while (iscanbuf_cur) {
-			iscanbuf_free = iscanbuf_cur;
-			iscanbuf_cur = iscanbuf_cur->next;
-			iscanbuf_free->next = 0;
-			MFREE(dhd->osh, iscanbuf_free, sizeof(iscan_buf_t));
-		}
-		iscan_chain = 0;
-	} else {
-		while (iscanbuf_cur) {
-			if (iscanbuf_cur == iscan_delete)
-				break;
-			iscanbuf_prv = iscanbuf_cur;
-			iscanbuf_cur = iscanbuf_cur->next;
-		}
-		if (iscanbuf_prv)
-			iscanbuf_prv->next = iscan_delete->next;
-
-		iscan_delete->next = 0;
-		MFREE(dhd->osh, iscan_delete, sizeof(iscan_buf_t));
-
-		if (!iscanbuf_prv)
-			iscan_chain = 0;
-	}
-	dhd_iscan_unlock();
-}
-
-iscan_buf_t *
-dhd_iscan_result_buf(void)
-{
-	return iscan_chain;
-}
-
-int
-dhd_iscan_issue_request(void * dhdp, wl_iscan_params_t *pParams, uint32 size)
-{
-	int rc = -1;
-	dhd_pub_t *dhd = dhd_bus_pub(dhdp);
-	char *buf;
-	char iovar[] = "iscan";
-	uint32 allocSize = 0;
-	wl_ioctl_t ioctl;
-
-	if (pParams) {
-		allocSize = (size + strlen(iovar) + 1);
-		if ((allocSize < size) || (allocSize < strlen(iovar)))
-		{
-			DHD_ERROR(("%s: overflow - allocation size too large %d < %d + %d!\n",
-				__FUNCTION__, allocSize, size, strlen(iovar)));
-			goto cleanUp;
-		}
-		buf = MALLOC(dhd->osh, allocSize);
-
-		if (buf == NULL)
-			{
-			DHD_ERROR(("%s: malloc of size %d failed!\n", __FUNCTION__, allocSize));
-			goto cleanUp;
-			}
-		ioctl.cmd = WLC_SET_VAR;
-		bcm_mkiovar(iovar, (char *)pParams, size, buf, allocSize);
-		rc = dhd_wl_ioctl(dhd, 0, &ioctl, buf, allocSize);
-	}
-
-cleanUp:
-	if (buf) {
-		MFREE(dhd->osh, buf, allocSize);
-	}
-
-	return rc;
-}
-
-static int
-dhd_iscan_get_partial_result(void *dhdp, uint *scan_count)
-{
-	wl_iscan_results_t *list_buf;
-	wl_iscan_results_t list;
-	wl_scan_results_t *results;
-	iscan_buf_t *iscan_cur;
-	int status = -1;
-	dhd_pub_t *dhd = dhd_bus_pub(dhdp);
-	int rc;
-	wl_ioctl_t ioctl;
-
-	DHD_ISCAN(("%s: Enter\n", __FUNCTION__));
-
-	iscan_cur = dhd_iscan_allocate_buf(dhd, &iscan_chain);
-	if (!iscan_cur) {
-		DHD_ERROR(("%s: Failed to allocate node\n", __FUNCTION__));
-		dhd_iscan_free_buf(dhdp, 0);
-		dhd_iscan_request(dhdp, WL_SCAN_ACTION_ABORT);
-		dhd_ind_scan_confirm(dhdp, FALSE);
-		goto fail;
-	}
-
-	dhd_iscan_lock();
-
-	memset(iscan_cur->iscan_buf, 0, WLC_IW_ISCAN_MAXLEN);
-	list_buf = (wl_iscan_results_t*)iscan_cur->iscan_buf;
-	results = &list_buf->results;
-	results->buflen = WL_ISCAN_RESULTS_FIXED_SIZE;
-	results->version = 0;
-	results->count = 0;
-
-	memset(&list, 0, sizeof(list));
-	list.results.buflen = htod32(WLC_IW_ISCAN_MAXLEN);
-	bcm_mkiovar("iscanresults", (char *)&list, WL_ISCAN_RESULTS_FIXED_SIZE,
-		iscan_cur->iscan_buf, WLC_IW_ISCAN_MAXLEN);
-	ioctl.cmd = WLC_GET_VAR;
-	ioctl.set = FALSE;
-	rc = dhd_wl_ioctl(dhd, 0, &ioctl, iscan_cur->iscan_buf, WLC_IW_ISCAN_MAXLEN);
-
-	results->buflen = dtoh32(results->buflen);
-	results->version = dtoh32(results->version);
-	*scan_count = results->count = dtoh32(results->count);
-	status = dtoh32(list_buf->status);
-	DHD_ISCAN(("%s: Got %d resuls status = (%x)\n", __FUNCTION__, results->count, status));
-
-	dhd_iscan_unlock();
-
-	if (!(*scan_count)) {
-		 /* TODO: race condition when FLUSH already called */
-		dhd_iscan_free_buf(dhdp, 0);
-	}
-fail:
-	return status;
-}
-
-#endif /* SIMPLE_ISCAN */
-
-/* Function to estimate possible DTIM_SKIP value */
-int
-dhd_get_dtim_skip(dhd_pub_t *dhd)
-{
-	int bcn_li_dtim;
-	int ret;
-	uint8 bssid[6];
-	int dtim_assoc = 0;
-
-	if ((dhd->dtim_skip == 0) || (dhd->dtim_skip == 1))
-		bcn_li_dtim = 3;
-	else
-		bcn_li_dtim = dhd->dtim_skip;
-
-	/* Check if associated */
-	if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_GET_BSSID,
-		(char *)&bssid, ETHER_ADDR_LEN, FALSE, 0)) == BCME_NOTASSOCIATED) {
-		DHD_TRACE(("%s NOT assoc ret %d\n", __FUNCTION__, ret));
-		goto exit;
-	}
-
-	/* if assoc grab ap's dtim value */
-	if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_GET_DTIMPRD,
-		&dtim_assoc, sizeof(dtim_assoc), FALSE, 0)) < 0) {
-		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
-		goto exit;
-	}
-
-	DHD_ERROR(("%s bcn_li_dtim=%d DTIM=%d Listen=%d\n",
-		__FUNCTION__, bcn_li_dtim, dtim_assoc, LISTEN_INTERVAL));
-
-	/* if not assocated just eixt */
-	if (dtim_assoc == 0) {
-		goto exit;
-	}
-
-	/* check if sta listen interval fits into AP dtim */
-	if (dtim_assoc > LISTEN_INTERVAL) {
-		/* AP DTIM to big for our Listen Interval : no dtim skiping */
-		bcn_li_dtim = 1;
-		DHD_ERROR(("%s DTIM=%d > Listen=%d : too big ...\n",
-			__FUNCTION__, dtim_assoc, LISTEN_INTERVAL));
-		goto exit;
-	}
-
-	if ((bcn_li_dtim * dtim_assoc) > LISTEN_INTERVAL) {
-		/* Round up dtim_skip to fit into STAs Listen Interval */
-		bcn_li_dtim = (int)(LISTEN_INTERVAL / dtim_assoc);
-		DHD_TRACE(("%s agjust dtim_skip as %d\n", __FUNCTION__, bcn_li_dtim));
-	}
-
-exit:
-	return bcn_li_dtim;
-}
-
-/* Check if HostAPD or WFD mode setup */
-bool dhd_check_ap_wfd_mode_set(dhd_pub_t *dhd)
-{
-#ifdef  WL_CFG80211
-	if (((dhd->op_mode & HOSTAPD_MASK) == HOSTAPD_MASK) ||
-		((dhd->op_mode & WFD_MASK) == WFD_MASK))
-		return TRUE;
-	else
-#endif /* WL_CFG80211 */
-		return FALSE;
-}
-
-#ifdef PNO_SUPPORT
-int
-dhd_pno_clean(dhd_pub_t *dhd)
-{
-	char iovbuf[128];
-	int pfn_enabled = 0;
-	int iov_len = 0;
-	int ret;
-
-	/* Disable pfn */
-	iov_len = bcm_mkiovar("pfn", (char *)&pfn_enabled, 4, iovbuf, sizeof(iovbuf));
-	if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0)) >= 0) {
-		/* clear pfn */
-		iov_len = bcm_mkiovar("pfnclear", 0, 0, iovbuf, sizeof(iovbuf));
-		if (iov_len) {
-			if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf,
-			                            iov_len, TRUE, 0)) < 0) {
-				DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
-			}
-		}
-		else {
-			ret = -1;
-			DHD_ERROR(("%s failed code %d\n", __FUNCTION__, iov_len));
-		}
-	}
-	else
-		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
-
-	return ret;
-}
-
-int
-dhd_pno_enable(dhd_pub_t *dhd, int pfn_enabled)
-{
-	char iovbuf[128];
-	uint8 bssid[6];
-	int ret = -1;
-
-	if ((!dhd) && ((pfn_enabled != 0) || (pfn_enabled != 1))) {
-		DHD_ERROR(("%s error exit\n", __FUNCTION__));
-		return ret;
-	}
-
-	if (dhd_check_ap_wfd_mode_set(dhd) == TRUE)
-		return (ret);
-	memset(iovbuf, 0, sizeof(iovbuf));
-	/* Check if disassoc to enable pno */
-	if ((pfn_enabled) &&
-		((ret = dhd_wl_ioctl_cmd(dhd, WLC_GET_BSSID,
-		(char *)&bssid, ETHER_ADDR_LEN, TRUE, 0)) == BCME_NOTASSOCIATED)) {
-			DHD_TRACE(("%s pno enable called in disassoc mode\n", __FUNCTION__));
-	}
-	else if (pfn_enabled) {
-			DHD_ERROR(("%s pno enable called in assoc mode ret=%d\n",
-				__FUNCTION__, ret));
-			return ret;
-	}
-	/* Enable/disable PNO */
-	if ((ret = bcm_mkiovar("pfn", (char *)&pfn_enabled, 4, iovbuf, sizeof(iovbuf))) > 0) {
-		if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf,
-		                            sizeof(iovbuf), TRUE, 0)) < 0) {
-			DHD_ERROR(("%s failed for error=%d\n", __FUNCTION__, ret));
-			return ret;
-		}
-		else {
-			dhd->pno_enable = pfn_enabled;
-			DHD_TRACE(("%s set pno as %d\n", __FUNCTION__, dhd->pno_enable));
-		}
-	}
-	else DHD_ERROR(("%s failed err=%d\n", __FUNCTION__, ret));
-
-	return ret;
-}
-
-/* Function to execute combined scan */
-int
-dhd_pno_set(dhd_pub_t *dhd, wlc_ssid_t* ssids_local, int nssid, ushort scan_fr,
-	int pno_repeat, int pno_freq_expo_max)
-{
-	int err = -1;
-	char iovbuf[128];
-	int k, i;
-	wl_pfn_param_t pfn_param;
-	wl_pfn_t	pfn_element;
-	uint len = 0;
-
-	DHD_TRACE(("%s nssid=%d nchan=%d\n", __FUNCTION__, nssid, scan_fr));
-
-	if ((!dhd) && (!ssids_local)) {
-		DHD_ERROR(("%s error exit\n", __FUNCTION__));
-		err = -1;
-	}
-	if (dhd_check_ap_wfd_mode_set(dhd) == TRUE)
-		return (err);
-
-	/* Check for broadcast ssid */
-	for (k = 0; k < nssid; k++) {
-		if (!ssids_local[k].SSID_len) {
-			DHD_ERROR(("%d: Broadcast SSID is ilegal for PNO setting\n", k));
-			return err;
-		}
-	}
-/* #define  PNO_DUMP 1 */
-#ifdef PNO_DUMP
-	{
-		int j;
-		for (j = 0; j < nssid; j++) {
-			DHD_ERROR(("%d: scan  for  %s size =%d\n", j,
-				ssids_local[j].SSID, ssids_local[j].SSID_len));
-		}
-	}
-#endif /* PNO_DUMP */
-
-	/* clean up everything */
-	if  ((err = dhd_pno_clean(dhd)) < 0) {
-		DHD_ERROR(("%s failed error=%d\n", __FUNCTION__, err));
-		return err;
-	}
-	memset(iovbuf, 0, sizeof(iovbuf));
-	memset(&pfn_param, 0, sizeof(pfn_param));
-	memset(&pfn_element, 0, sizeof(pfn_element));
-
-	/* set pfn parameters */
-	pfn_param.version = htod32(PFN_VERSION);
-	pfn_param.flags = htod16((PFN_LIST_ORDER << SORT_CRITERIA_BIT));
-
-	/* check and set extra pno params */
-	if ((pno_repeat != 0) || (pno_freq_expo_max != 0)) {
-		pfn_param.flags |= htod16(ENABLE << ENABLE_ADAPTSCAN_BIT);
-		pfn_param.repeat = (uchar) (pno_repeat);
-		pfn_param.exp = (uchar) (pno_freq_expo_max);
-	}
-	/* set up pno scan fr */
-	if (scan_fr  != 0)
-		pfn_param.scan_freq = htod32(scan_fr);
-
-	if (pfn_param.scan_freq > PNO_SCAN_MAX_FW_SEC) {
-		DHD_ERROR(("%s pno freq above %d sec\n", __FUNCTION__, PNO_SCAN_MAX_FW_SEC));
-		return err;
-	}
-	if (pfn_param.scan_freq < PNO_SCAN_MIN_FW_SEC) {
-		DHD_ERROR(("%s pno freq less %d sec\n", __FUNCTION__, PNO_SCAN_MIN_FW_SEC));
-		return err;
-	}
-
-	len = bcm_mkiovar("pfn_set", (char *)&pfn_param, sizeof(pfn_param), iovbuf, sizeof(iovbuf));
-	if ((err = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, len, TRUE, 0)) < 0) {
-				DHD_ERROR(("%s pfn_set failed for error=%d\n",
-					__FUNCTION__, err));
-				return err;
-	}
-
-	/* set all pfn ssid */
-	for (i = 0; i < nssid; i++) {
-
-		pfn_element.infra = htod32(DOT11_BSSTYPE_INFRASTRUCTURE);
-		pfn_element.auth = (DOT11_OPEN_SYSTEM);
-		pfn_element.wpa_auth = htod32(WPA_AUTH_PFN_ANY);
-		pfn_element.wsec = htod32(0);
-		pfn_element.infra = htod32(1);
-
-		memcpy((char *)pfn_element.ssid.SSID, ssids_local[i].SSID, ssids_local[i].SSID_len);
-		pfn_element.ssid.SSID_len = ssids_local[i].SSID_len;
-
-		if ((len =
-		bcm_mkiovar("pfn_add", (char *)&pfn_element,
-			sizeof(pfn_element), iovbuf, sizeof(iovbuf))) > 0) {
-			if ((err =
-			dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, len, TRUE, 0)) < 0) {
-				DHD_ERROR(("%s failed for i=%d error=%d\n",
-					__FUNCTION__, i, err));
-				return err;
-			}
-			else
-				DHD_ERROR(("%s set OK with PNO time=%d repeat=%d max_adjust=%d\n",
-					__FUNCTION__, pfn_param.scan_freq,
-					pfn_param.repeat, pfn_param.exp));
-		}
-		else DHD_ERROR(("%s failed err=%d\n", __FUNCTION__, err));
-	}
-
-	/* Enable PNO */
-	/* dhd_pno_enable(dhd, 1); */
-	return err;
-}
-
-int
-dhd_pno_get_status(dhd_pub_t *dhd)
-{
-	int ret = -1;
-
-	if (!dhd)
-		return ret;
-	else
-		return (dhd->pno_enable);
-}
-
-#endif /* PNO_SUPPORT */
-
-#if defined(KEEP_ALIVE)
-int dhd_keep_alive_onoff(dhd_pub_t *dhd)
-{
-	char			buf[256];
-	const char		*str;
-	wl_mkeep_alive_pkt_t	mkeep_alive_pkt;
-	wl_mkeep_alive_pkt_t	*mkeep_alive_pktp;
-	int			buf_len;
-	int			str_len;
-	int res			= -1;
-
-	if (dhd_check_ap_wfd_mode_set(dhd) == TRUE)
-		return (res);
-
-	DHD_TRACE(("%s execution\n", __FUNCTION__));
-
-	str = "mkeep_alive";
-	str_len = strlen(str);
-	strncpy(buf, str, str_len);
-	buf[ str_len ] = '\0';
-	mkeep_alive_pktp = (wl_mkeep_alive_pkt_t *) (buf + str_len + 1);
-	mkeep_alive_pkt.period_msec = KEEP_ALIVE_PERIOD;
-	buf_len = str_len + 1;
-	mkeep_alive_pkt.version = htod16(WL_MKEEP_ALIVE_VERSION);
-	mkeep_alive_pkt.length = htod16(WL_MKEEP_ALIVE_FIXED_LEN);
-	/* Setup keep alive zero for null packet generation */
-	mkeep_alive_pkt.keep_alive_id = 0;
-	mkeep_alive_pkt.len_bytes = 0;
-	buf_len += WL_MKEEP_ALIVE_FIXED_LEN;
-	/* Keep-alive attributes are set in local variable (mkeep_alive_pkt), and
-	 * then memcpy'ed into buffer (mkeep_alive_pktp) since there is no
-	 * guarantee that the buffer is properly aligned.
-	 */
-	memcpy((char *)mkeep_alive_pktp, &mkeep_alive_pkt, WL_MKEEP_ALIVE_FIXED_LEN);
-
-	res = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, buf_len, TRUE, 0);
-
-	return res;
-}
-#endif /* defined(KEEP_ALIVE) */
-/* Android ComboSCAN support */
-
-/*
- *  data parsing from ComboScan tlv list
-*/
-int
-wl_iw_parse_data_tlv(char** list_str, void *dst, int dst_size, const char token,
-                     int input_size, int *bytes_left)
-{
-	char* str = *list_str;
-	uint16 short_temp;
-	uint32 int_temp;
-
-	if ((list_str == NULL) || (*list_str == NULL) ||(bytes_left == NULL) || (*bytes_left < 0)) {
-		DHD_ERROR(("%s error paramters\n", __FUNCTION__));
-		return -1;
-	}
-
-	/* Clean all dest bytes */
-	memset(dst, 0, dst_size);
-	while (*bytes_left > 0) {
-
-		if (str[0] != token) {
-			DHD_TRACE(("%s NOT Type=%d get=%d left_parse=%d \n",
-				__FUNCTION__, token, str[0], *bytes_left));
-			return -1;
-		}
-
-		*bytes_left -= 1;
-		str += 1;
-
-		if (input_size == 1) {
-			memcpy(dst, str, input_size);
-		}
-		else if (input_size == 2) {
-			memcpy(dst, (char *)htod16(memcpy(&short_temp, str, input_size)),
-				input_size);
-		}
-		else if (input_size == 4) {
-			memcpy(dst, (char *)htod32(memcpy(&int_temp, str, input_size)),
-				input_size);
-		}
-
-		*bytes_left -= input_size;
-		str += input_size;
-		*list_str = str;
-		return 1;
-	}
-	return 1;
-}
-
-/*
- *  channel list parsing from cscan tlv list
-*/
-int
-wl_iw_parse_channel_list_tlv(char** list_str, uint16* channel_list,
-                             int channel_num, int *bytes_left)
-{
-	char* str = *list_str;
-	int idx = 0;
-
-	if ((list_str == NULL) || (*list_str == NULL) ||(bytes_left == NULL) || (*bytes_left < 0)) {
-		DHD_ERROR(("%s error paramters\n", __FUNCTION__));
-		return -1;
-	}
-
-	while (*bytes_left > 0) {
-
-		if (str[0] != CSCAN_TLV_TYPE_CHANNEL_IE) {
-			*list_str = str;
-			DHD_TRACE(("End channel=%d left_parse=%d %d\n", idx, *bytes_left, str[0]));
-			return idx;
-		}
-		/* Get proper CSCAN_TLV_TYPE_CHANNEL_IE */
-		*bytes_left -= 1;
-		str += 1;
-
-		if (str[0] == 0) {
-			/* All channels */
-			channel_list[idx] = 0x0;
-		}
-		else {
-			channel_list[idx] = (uint16)str[0];
-			DHD_TRACE(("%s channel=%d \n", __FUNCTION__,  channel_list[idx]));
-		}
-		*bytes_left -= 1;
-		str += 1;
-
-		if (idx++ > 255) {
-			DHD_ERROR(("%s Too many channels \n", __FUNCTION__));
-			return -1;
-		}
-	}
-
-	*list_str = str;
-	return idx;
-}
-
-/*
- *  SSIDs list parsing from cscan tlv list
- */
-int
-wl_iw_parse_ssid_list_tlv(char** list_str, wlc_ssid_t* ssid, int max, int *bytes_left)
-{
-	char* str =  *list_str;
-	int idx = 0;
-
-	if ((list_str == NULL) || (*list_str == NULL) || (*bytes_left < 0)) {
-		DHD_ERROR(("%s error paramters\n", __FUNCTION__));
-		return -1;
-	}
-
-	while (*bytes_left > 0) {
-
-		if (str[0] != CSCAN_TLV_TYPE_SSID_IE) {
-			*list_str = str;
-			DHD_TRACE(("nssid=%d left_parse=%d %d\n", idx, *bytes_left, str[0]));
-			return idx;
-		}
-
-		/* Get proper CSCAN_TLV_TYPE_SSID_IE */
-		*bytes_left -= 1;
-		str += 1;
-
-		if (str[0] == 0) {
-			/* Broadcast SSID */
-			ssid[idx].SSID_len = 0;
-			memset((char*)ssid[idx].SSID, 0x0, DOT11_MAX_SSID_LEN);
-			*bytes_left -= 1;
-			str += 1;
-
-			DHD_TRACE(("BROADCAST SCAN  left=%d\n", *bytes_left));
-		}
-		else if (str[0] <= DOT11_MAX_SSID_LEN) {
-			/* Get proper SSID size */
-			ssid[idx].SSID_len = str[0];
-			*bytes_left -= 1;
-			str += 1;
-
-			/* Get SSID */
-			if (ssid[idx].SSID_len > *bytes_left) {
-				DHD_ERROR(("%s out of memory range len=%d but left=%d\n",
-				__FUNCTION__, ssid[idx].SSID_len, *bytes_left));
-				return -1;
-			}
-
-			memcpy((char*)ssid[idx].SSID, str, ssid[idx].SSID_len);
-
-			*bytes_left -= ssid[idx].SSID_len;
-			str += ssid[idx].SSID_len;
-
-			DHD_TRACE(("%s :size=%d left=%d\n",
-				(char*)ssid[idx].SSID, ssid[idx].SSID_len, *bytes_left));
-		}
-		else {
-			DHD_ERROR(("### SSID size more that %d\n", str[0]));
-			return -1;
-		}
-
-		if (idx++ >  max) {
-			DHD_ERROR(("%s number of SSIDs more that %d\n", __FUNCTION__, idx));
-			return -1;
-		}
-	}
-
-	*list_str = str;
-	return idx;
-}
-
-/* Parse a comma-separated list from list_str into ssid array, starting
- * at index idx.  Max specifies size of the ssid array.  Parses ssids
- * and returns updated idx; if idx >= max not all fit, the excess have
- * not been copied.  Returns -1 on empty string, or on ssid too long.
- */
-int
-wl_iw_parse_ssid_list(char** list_str, wlc_ssid_t* ssid, int idx, int max)
-{
-	char* str, *ptr;
-
-	if ((list_str == NULL) || (*list_str == NULL))
-		return -1;
-
-	for (str = *list_str; str != NULL; str = ptr) {
-
-		/* check for next TAG */
-		if (!strncmp(str, GET_CHANNEL, strlen(GET_CHANNEL))) {
-			*list_str	 = str + strlen(GET_CHANNEL);
-			return idx;
-		}
-
-		if ((ptr = strchr(str, ',')) != NULL) {
-			*ptr++ = '\0';
-		}
-
-		if (strlen(str) > DOT11_MAX_SSID_LEN) {
-			DHD_ERROR(("ssid <%s> exceeds %d\n", str, DOT11_MAX_SSID_LEN));
-			return -1;
-		}
-
-		if (strlen(str) == 0)
-			ssid[idx].SSID_len = 0;
-
-		if (idx < max) {
-			bcm_strcpy_s((char*)ssid[idx].SSID, sizeof(ssid[idx].SSID), str);
-			ssid[idx].SSID_len = strlen(str);
-		}
-		idx++;
-	}
-	return idx;
-}
-
-/*
- * Parse channel list from iwpriv CSCAN
- */
-int
-wl_iw_parse_channel_list(char** list_str, uint16* channel_list, int channel_num)
-{
-	int num;
-	int val;
-	char* str;
-	char* endptr = NULL;
-
-	if ((list_str == NULL)||(*list_str == NULL))
-		return -1;
-
-	str = *list_str;
-	num = 0;
-	while (strncmp(str, GET_NPROBE, strlen(GET_NPROBE))) {
-		val = (int)strtoul(str, &endptr, 0);
-		if (endptr == str) {
-			printf("could not parse channel number starting at"
-				" substring \"%s\" in list:\n%s\n",
-				str, *list_str);
-			return -1;
-		}
-		str = endptr + strspn(endptr, " ,");
-
-		if (num == channel_num) {
-			DHD_ERROR(("too many channels (more than %d) in channel list:\n%s\n",
-				channel_num, *list_str));
-			return -1;
-		}
-
-		channel_list[num++] = (uint16)val;
-	}
-	*list_str = str;
-	return num;
-}
diff --git a/drivers/net/wireless/bcmdhd/dhd_linux.c b/drivers/net/wireless/bcmdhd/dhd_linux.c
deleted file mode 100644
index 03ba34a2c82..00000000000
--- a/drivers/net/wireless/bcmdhd/dhd_linux.c
+++ /dev/null
@@ -1,4660 +0,0 @@
-/*
- * Broadcom Dongle Host Driver (DHD), Linux-specific network interface
- * Basically selected code segments from usb-cdc.c and usb-rndis.c
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: dhd_linux.c,v 1.131.2.55 2011-02-09 05:31:56 Exp $
- */
-
-#include <typedefs.h>
-#include <linuxver.h>
-#include <osl.h>
-
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/skbuff.h>
-#include <linux/netdevice.h>
-#include <linux/inetdevice.h>
-#include <linux/rtnetlink.h>
-#include <linux/etherdevice.h>
-#include <linux/random.h>
-#include <linux/spinlock.h>
-#include <linux/ethtool.h>
-#include <linux/fcntl.h>
-#include <linux/fs.h>
-
-#include <asm/uaccess.h>
-#include <asm/unaligned.h>
-
-#include <epivers.h>
-#include <bcmutils.h>
-#include <bcmendian.h>
-
-#include <proto/ethernet.h>
-#include <dngl_stats.h>
-#include <dhd.h>
-#include <dhd_bus.h>
-#include <dhd_proto.h>
-#include <dhd_dbg.h>
-#ifdef CONFIG_HAS_WAKELOCK
-#include <linux/wakelock.h>
-#endif
-#ifdef WL_CFG80211
-#include <wl_cfg80211.h>
-#endif
-
-#include <proto/802.11_bta.h>
-#include <proto/bt_amp_hci.h>
-#include <dhd_bta.h>
-
-#ifdef WLMEDIA_HTSF
-#include <linux/time.h>
-#include <htsf.h>
-
-#define HTSF_MINLEN 200    /* min. packet length to timestamp */
-#define HTSF_BUS_DELAY 150 /* assume a fix propagation in us  */
-#define TSMAX  1000        /* max no. of timing record kept   */
-#define NUMBIN 34
-
-static uint32 tsidx = 0;
-static uint32 htsf_seqnum = 0;
-uint32 tsfsync;
-struct timeval tsync;
-static uint32 tsport = 5010;
-
-typedef struct histo_ {
-	uint32 bin[NUMBIN];
-} histo_t;
-
-static histo_t vi_d1, vi_d2, vi_d3, vi_d4;
-#endif /* WLMEDIA_HTSF */
-
-#if defined(SOFTAP)
-extern bool ap_cfg_running;
-extern bool ap_fw_loaded;
-#endif
-
-/* enable HOSTIP cache update from the host side when an eth0:N is up */
-#define AOE_IP_ALIAS_SUPPORT 1
-
-#ifdef PROP_TXSTATUS
-#include <wlfc_proto.h>
-#include <dhd_wlfc.h>
-#endif
-
-#include <wl_android.h>
-
-#ifdef ARP_OFFLOAD_SUPPORT
-static int dhd_device_event(struct notifier_block *this,
-	unsigned long event,
-	void *ptr);
-
-static struct notifier_block dhd_notifier = {
-	.notifier_call = dhd_device_event
-};
-#endif /* ARP_OFFLOAD_SUPPORT */
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
-#include <linux/suspend.h>
-volatile bool dhd_mmc_suspend = FALSE;
-DECLARE_WAIT_QUEUE_HEAD(dhd_dpc_wait);
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
-
-#if defined(OOB_INTR_ONLY)
-extern void dhd_enable_oob_intr(struct dhd_bus *bus, bool enable);
-#endif /* defined(OOB_INTR_ONLY) */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
-MODULE_LICENSE("GPL v2");
-#endif /* LinuxVer */
-
-#include <dhd_bus.h>
-
-#define DBUS_RX_BUFFER_SIZE_DHD(net)	(net->mtu + net->hard_header_len + dhd->pub.hdrlen)
-
-#if LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 15)
-const char *
-print_tainted()
-{
-	return "";
-}
-#endif	/* LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 15) */
-
-/* Linux wireless extension support */
-#if defined(CONFIG_WIRELESS_EXT)
-#include <wl_iw.h>
-extern wl_iw_extra_params_t  g_wl_iw_params;
-#endif /* defined(CONFIG_WIRELESS_EXT) */
-
-#if defined(CONFIG_HAS_EARLYSUSPEND)
-#include <linux/earlysuspend.h>
-extern int dhdcdc_set_ioctl(dhd_pub_t *dhd, int ifidx, uint cmd, void *buf, uint len);
-extern int dhd_get_dtim_skip(dhd_pub_t *dhd);
-#endif /* defined(CONFIG_HAS_EARLYSUSPEND) */
-
-#ifdef PKT_FILTER_SUPPORT
-extern void dhd_pktfilter_offload_set(dhd_pub_t * dhd, char *arg);
-extern void dhd_pktfilter_offload_enable(dhd_pub_t * dhd, char *arg, int enable, int master_mode);
-#endif
-
-/* Interface control information */
-typedef struct dhd_if {
-	struct dhd_info *info;			/* back pointer to dhd_info */
-	/* OS/stack specifics */
-	struct net_device *net;
-	struct net_device_stats stats;
-	int 			idx;			/* iface idx in dongle */
-	int 			state;			/* interface state */
-	uint 			subunit;		/* subunit */
-	uint8			mac_addr[ETHER_ADDR_LEN];	/* assigned MAC address */
-	bool			attached;		/* Delayed attachment when unset */
-	bool			txflowcontrol;	/* Per interface flow control indicator */
-	char			name[IFNAMSIZ+1]; /* linux interface name */
-	uint8			bssidx;			/* bsscfg index for the interface */
-} dhd_if_t;
-
-#ifdef WLMEDIA_HTSF
-typedef struct {
-	uint32 low;
-	uint32 high;
-} tsf_t;
-
-typedef struct {
-	uint32 last_cycle;
-	uint32 last_sec;
-	uint32 last_tsf;
-	uint32 coef;     /* scaling factor */
-	uint32 coefdec1; /* first decimal  */
-	uint32 coefdec2; /* second decimal */
-} htsf_t;
-
-typedef struct {
-	uint32 t1;
-	uint32 t2;
-	uint32 t3;
-	uint32 t4;
-} tstamp_t;
-
-static tstamp_t ts[TSMAX];
-static tstamp_t maxdelayts;
-static uint32 maxdelay = 0, tspktcnt = 0, maxdelaypktno = 0;
-
-#endif  /* WLMEDIA_HTSF */
-
-/* Local private structure (extension of pub) */
-typedef struct dhd_info {
-#if defined(CONFIG_WIRELESS_EXT)
-	wl_iw_t		iw;		/* wireless extensions state (must be first) */
-#endif /* defined(CONFIG_WIRELESS_EXT) */
-
-	dhd_pub_t pub;
-
-	/* For supporting multiple interfaces */
-	dhd_if_t *iflist[DHD_MAX_IFS];
-
-	struct semaphore proto_sem;
-#ifdef PROP_TXSTATUS
-	spinlock_t	wlfc_spinlock;
-#endif
-#ifdef WLMEDIA_HTSF
-	htsf_t  htsf;
-#endif
-	wait_queue_head_t ioctl_resp_wait;
-	struct timer_list timer;
-	bool wd_timer_valid;
-	struct tasklet_struct tasklet;
-	spinlock_t	sdlock;
-	spinlock_t	txqlock;
-	spinlock_t	dhd_lock;
-#ifdef DHDTHREAD
-	/* Thread based operation */
-	bool threads_only;
-	struct semaphore sdsem;
-
-	tsk_ctl_t	thr_dpc_ctl;
-	tsk_ctl_t	thr_wdt_ctl;
-
-#else
-	bool dhd_tasklet_create;
-#endif /* DHDTHREAD */
-	tsk_ctl_t	thr_sysioc_ctl;
-
-	/* Wakelocks */
-#if defined(CONFIG_HAS_WAKELOCK) && (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-	struct wake_lock wl_wifi;   /* Wifi wakelock */
-	struct wake_lock wl_rxwake; /* Wifi rx wakelock */
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
-	/* net_device interface lock, prevent race conditions among net_dev interface
-	 * calls and wifi_on or wifi_off
-	 */
-	struct mutex dhd_net_if_mutex;
-#endif
-	spinlock_t wakelock_spinlock;
-	int wakelock_counter;
-	int wakelock_timeout_enable;
-
-	int hang_was_sent;
-
-	/* Thread to issue ioctl for multicast */
-	bool set_multicast;
-	bool set_macaddress;
-	struct ether_addr macvalue;
-	wait_queue_head_t ctrl_wait;
-	atomic_t pend_8021x_cnt;
-	dhd_attach_states_t dhd_state;
-
-#ifdef CONFIG_HAS_EARLYSUSPEND
-	struct early_suspend early_suspend;
-#endif /* CONFIG_HAS_EARLYSUSPEND */
-} dhd_info_t;
-
-/* Definitions to provide path to the firmware and nvram
- * example nvram_path[MOD_PARAM_PATHLEN]="/projects/wlan/nvram.txt"
- */
-char firmware_path[MOD_PARAM_PATHLEN];
-char nvram_path[MOD_PARAM_PATHLEN];
-
-extern int wl_control_wl_start(struct net_device *dev);
-extern int net_os_send_hang_message(struct net_device *dev);
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-struct semaphore dhd_registration_sem;
-#define DHD_REGISTRATION_TIMEOUT  12000  /* msec : allowed time to finished dhd registration */
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
-
-/* Spawn a thread for system ioctls (set mac, set mcast) */
-uint dhd_sysioc = TRUE;
-module_param(dhd_sysioc, uint, 0);
-
-/* Error bits */
-module_param(dhd_msg_level, int, 0);
-
-/* load firmware and/or nvram values from the filesystem */
-module_param_string(firmware_path, firmware_path, MOD_PARAM_PATHLEN, 0660);
-module_param_string(nvram_path, nvram_path, MOD_PARAM_PATHLEN, 0);
-
-/* Watchdog interval */
-uint dhd_watchdog_ms = 10;
-module_param(dhd_watchdog_ms, uint, 0);
-
-#if defined(DHD_DEBUG)
-/* Console poll interval */
-uint dhd_console_ms = 0;
-module_param(dhd_console_ms, uint, 0);
-#endif /* defined(DHD_DEBUG) */
-
-/* ARP offload agent mode : Enable ARP Host Auto-Reply and ARP Peer Auto-Reply */
-uint dhd_arp_mode = 0xb;
-module_param(dhd_arp_mode, uint, 0);
-
-/* ARP offload enable */
-uint dhd_arp_enable = TRUE;
-module_param(dhd_arp_enable, uint, 0);
-
-/* Global Pkt filter enable control */
-uint dhd_pkt_filter_enable = TRUE;
-module_param(dhd_pkt_filter_enable, uint, 0);
-
-/*  Pkt filter init setup */
-uint dhd_pkt_filter_init = 0;
-module_param(dhd_pkt_filter_init, uint, 0);
-
-/* Pkt filter mode control */
-uint dhd_master_mode = TRUE;
-module_param(dhd_master_mode, uint, 1);
-
-#ifdef DHDTHREAD
-/* Watchdog thread priority, -1 to use kernel timer */
-int dhd_watchdog_prio = 97;
-module_param(dhd_watchdog_prio, int, 0);
-
-/* DPC thread priority, -1 to use tasklet */
-int dhd_dpc_prio = 98;
-module_param(dhd_dpc_prio, int, 0);
-
-/* DPC thread priority, -1 to use tasklet */
-extern int dhd_dongle_memsize;
-module_param(dhd_dongle_memsize, int, 0);
-#endif /* DHDTHREAD */
-/* Control fw roaming */
-uint dhd_roam_disable = 0;
-
-/* Control radio state */
-uint dhd_radio_up = 1;
-
-/* Network inteface name */
-char iface_name[IFNAMSIZ];
-module_param_string(iface_name, iface_name, IFNAMSIZ, 0);
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
-#define DAEMONIZE(a) daemonize(a); \
-	allow_signal(SIGKILL); \
-	allow_signal(SIGTERM);
-#else /* Linux 2.4 (w/o preemption patch) */
-#define RAISE_RX_SOFTIRQ() \
-	cpu_raise_softirq(smp_processor_id(), NET_RX_SOFTIRQ)
-#define DAEMONIZE(a) daemonize(); \
-	do { if (a) \
-		strncpy(current->comm, a, MIN(sizeof(current->comm), (strlen(a) + 1))); \
-	} while (0);
-#endif /* LINUX_VERSION_CODE  */
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
-#define BLOCKABLE()	(!in_atomic())
-#else
-#define BLOCKABLE()	(!in_interrupt())
-#endif
-
-/* The following are specific to the SDIO dongle */
-
-/* IOCTL response timeout */
-int dhd_ioctl_timeout_msec = IOCTL_RESP_TIMEOUT;
-
-/* Idle timeout for backplane clock */
-int dhd_idletime = DHD_IDLETIME_TICKS;
-module_param(dhd_idletime, int, 0);
-
-/* Use polling */
-uint dhd_poll = FALSE;
-module_param(dhd_poll, uint, 0);
-
-/* Use interrupts */
-uint dhd_intr = TRUE;
-module_param(dhd_intr, uint, 0);
-
-/* SDIO Drive Strength (in milliamps) */
-uint dhd_sdiod_drive_strength = 6;
-module_param(dhd_sdiod_drive_strength, uint, 0);
-
-/* Tx/Rx bounds */
-extern uint dhd_txbound;
-extern uint dhd_rxbound;
-module_param(dhd_txbound, uint, 0);
-module_param(dhd_rxbound, uint, 0);
-
-/* Deferred transmits */
-extern uint dhd_deferred_tx;
-module_param(dhd_deferred_tx, uint, 0);
-
-#ifdef BCMDBGFS
-extern void dhd_dbg_init(dhd_pub_t *dhdp);
-extern void dhd_dbg_remove(void);
-#endif /* BCMDBGFS */
-
-
-
-#ifdef SDTEST
-/* Echo packet generator (pkts/s) */
-uint dhd_pktgen = 0;
-module_param(dhd_pktgen, uint, 0);
-
-/* Echo packet len (0 => sawtooth, max 2040) */
-uint dhd_pktgen_len = 0;
-module_param(dhd_pktgen_len, uint, 0);
-#endif /* SDTEST */
-
-/* Version string to report */
-#ifdef DHD_DEBUG
-#ifndef SRCBASE
-#define SRCBASE        "drivers/net/wireless/bcmdhd"
-#endif
-#define DHD_COMPILED "\nCompiled in " SRCBASE
-#else
-#define DHD_COMPILED
-#endif /* DHD_DEBUG */
-
-static char dhd_version[] = "Dongle Host Driver, version " EPI_VERSION_STR
-#ifdef DHD_DEBUG
-"\nCompiled in " SRCBASE " on " __DATE__ " at " __TIME__
-#endif
-;
-static void dhd_net_if_lock_local(dhd_info_t *dhd);
-static void dhd_net_if_unlock_local(dhd_info_t *dhd);
-
-#ifdef WLMEDIA_HTSF
-void htsf_update(dhd_info_t *dhd, void *data);
-tsf_t prev_tsf, cur_tsf;
-
-uint32 dhd_get_htsf(dhd_info_t *dhd, int ifidx);
-static int dhd_ioctl_htsf_get(dhd_info_t *dhd, int ifidx);
-static void dhd_dump_latency(void);
-static void dhd_htsf_addtxts(dhd_pub_t *dhdp, void *pktbuf);
-static void dhd_htsf_addrxts(dhd_pub_t *dhdp, void *pktbuf);
-static void dhd_dump_htsfhisto(histo_t *his, char *s);
-#endif /* WLMEDIA_HTSF */
-
-/* Monitor interface */
-int dhd_monitor_init(void *dhd_pub);
-int dhd_monitor_uninit(void);
-
-
-#if defined(CONFIG_WIRELESS_EXT)
-struct iw_statistics *dhd_get_wireless_stats(struct net_device *dev);
-#endif /* defined(CONFIG_WIRELESS_EXT) */
-
-static void dhd_dpc(ulong data);
-/* forward decl */
-extern int dhd_wait_pend8021x(struct net_device *dev);
-
-#ifdef TOE
-#ifndef BDC
-#error TOE requires BDC
-#endif /* !BDC */
-static int dhd_toe_get(dhd_info_t *dhd, int idx, uint32 *toe_ol);
-static int dhd_toe_set(dhd_info_t *dhd, int idx, uint32 toe_ol);
-#endif /* TOE */
-
-static int dhd_wl_host_event(dhd_info_t *dhd, int *ifidx, void *pktdata,
-                             wl_event_msg_t *event_ptr, void **data_ptr);
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) && 1
-static int dhd_sleep_pm_callback(struct notifier_block *nfb, unsigned long action, void *ignored)
-{
-	int ret = NOTIFY_DONE;
-
-	switch (action)	{
-		case PM_HIBERNATION_PREPARE:
-		case PM_SUSPEND_PREPARE:
-			dhd_mmc_suspend = TRUE;
-		ret = NOTIFY_OK;
-		break;
-		case PM_POST_HIBERNATION:
-		case PM_POST_SUSPEND:
-			dhd_mmc_suspend = FALSE;
-		ret = NOTIFY_OK;
-		break;
-	}
-	smp_mb();
-	return ret;
-}
-
-static struct notifier_block dhd_sleep_pm_notifier = {
-	.notifier_call = dhd_sleep_pm_callback,
-	.priority = 0
-};
-extern int register_pm_notifier(struct notifier_block *nb);
-extern int unregister_pm_notifier(struct notifier_block *nb);
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
-	/* && defined(DHD_GPL) */
-
-static void dhd_set_packet_filter(int value, dhd_pub_t *dhd)
-{
-#ifdef PKT_FILTER_SUPPORT
-	DHD_TRACE(("%s: %d\n", __FUNCTION__, value));
-	/* 1 - Enable packet filter, only allow unicast packet to send up */
-	/* 0 - Disable packet filter */
-	if (dhd_pkt_filter_enable) {
-		int i;
-
-		for (i = 0; i < dhd->pktfilter_count; i++) {
-			dhd_pktfilter_offload_set(dhd, dhd->pktfilter[i]);
-			dhd_pktfilter_offload_enable(dhd, dhd->pktfilter[i],
-				value, dhd_master_mode);
-		}
-	}
-#endif
-}
-
-#if defined(CONFIG_HAS_EARLYSUSPEND)
-static int dhd_set_suspend(int value, dhd_pub_t *dhd)
-{
-	int power_mode = PM_MAX;
-	/* wl_pkt_filter_enable_t	enable_parm; */
-	char iovbuf[32];
-	int bcn_li_dtim = 3;
-	uint roamvar = 1;
-
-	DHD_TRACE(("%s: enter, value = %d in_suspend=%d\n",
-		__FUNCTION__, value, dhd->in_suspend));
-
-	if (dhd && dhd->up) {
-		if (value && dhd->in_suspend) {
-
-				/* Kernel suspended */
-				DHD_ERROR(("%s: force extra Suspend setting \n", __FUNCTION__));
-
-				dhd_wl_ioctl_cmd(dhd, WLC_SET_PM, (char *)&power_mode,
-				                 sizeof(power_mode), TRUE, 0);
-
-				/* Enable packet filter, only allow unicast packet to send up */
-				dhd_set_packet_filter(1, dhd);
-
-				/* If DTIM skip is set up as default, force it to wake
-				 * each third DTIM for better power savings.  Note that
-				 * one side effect is a chance to miss BC/MC packet.
-				 */
-				bcn_li_dtim = dhd_get_dtim_skip(dhd);
-				bcm_mkiovar("bcn_li_dtim", (char *)&bcn_li_dtim,
-					4, iovbuf, sizeof(iovbuf));
-				dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-
-				/* Disable firmware roaming during suspend */
-				bcm_mkiovar("roam_off", (char *)&roamvar, 4,
-					iovbuf, sizeof(iovbuf));
-				dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-			} else {
-
-				/* Kernel resumed  */
-				DHD_TRACE(("%s: Remove extra suspend setting \n", __FUNCTION__));
-
-				power_mode = PM_FAST;
-				dhd_wl_ioctl_cmd(dhd, WLC_SET_PM, (char *)&power_mode,
-				                 sizeof(power_mode), TRUE, 0);
-
-				/* disable pkt filter */
-				dhd_set_packet_filter(0, dhd);
-
-				/* restore pre-suspend setting for dtim_skip */
-				bcm_mkiovar("bcn_li_dtim", (char *)&dhd->dtim_skip,
-					4, iovbuf, sizeof(iovbuf));
-
-				dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-				roamvar = dhd_roam_disable;
-				bcm_mkiovar("roam_off", (char *)&roamvar, 4, iovbuf,
-					sizeof(iovbuf));
-				dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-			}
-	}
-
-	return 0;
-}
-
-static void dhd_suspend_resume_helper(struct dhd_info *dhd, int val)
-{
-	dhd_pub_t *dhdp = &dhd->pub;
-
-	DHD_OS_WAKE_LOCK(dhdp);
-	/* Set flag when early suspend was called */
-	dhdp->in_suspend = val;
-	if (!dhdp->suspend_disable_flag)
-		dhd_set_suspend(val, dhdp);
-	DHD_OS_WAKE_UNLOCK(dhdp);
-}
-
-static void dhd_early_suspend(struct early_suspend *h)
-{
-	struct dhd_info *dhd = container_of(h, struct dhd_info, early_suspend);
-
-	DHD_TRACE(("%s: enter\n", __FUNCTION__));
-
-	if (dhd)
-		dhd_suspend_resume_helper(dhd, 1);
-}
-
-static void dhd_late_resume(struct early_suspend *h)
-{
-	struct dhd_info *dhd = container_of(h, struct dhd_info, early_suspend);
-
-	DHD_TRACE(("%s: enter\n", __FUNCTION__));
-
-	if (dhd)
-		dhd_suspend_resume_helper(dhd, 0);
-}
-#endif /* defined(CONFIG_HAS_EARLYSUSPEND) */
-
-/*
- * Generalized timeout mechanism.  Uses spin sleep with exponential back-off until
- * the sleep time reaches one jiffy, then switches over to task delay.  Usage:
- *
- *      dhd_timeout_start(&tmo, usec);
- *      while (!dhd_timeout_expired(&tmo))
- *              if (poll_something())
- *                      break;
- *      if (dhd_timeout_expired(&tmo))
- *              fatal();
- */
-
-void
-dhd_timeout_start(dhd_timeout_t *tmo, uint usec)
-{
-	tmo->limit = usec;
-	tmo->increment = 0;
-	tmo->elapsed = 0;
-	tmo->tick = 1000000 / HZ;
-}
-
-int
-dhd_timeout_expired(dhd_timeout_t *tmo)
-{
-	/* Does nothing the first call */
-	if (tmo->increment == 0) {
-		tmo->increment = 1;
-		return 0;
-	}
-
-	if (tmo->elapsed >= tmo->limit)
-		return 1;
-
-	/* Add the delay that's about to take place */
-	tmo->elapsed += tmo->increment;
-
-	if (tmo->increment < tmo->tick) {
-		OSL_DELAY(tmo->increment);
-		tmo->increment *= 2;
-		if (tmo->increment > tmo->tick)
-			tmo->increment = tmo->tick;
-	} else {
-		wait_queue_head_t delay_wait;
-		DECLARE_WAITQUEUE(wait, current);
-		int pending;
-		init_waitqueue_head(&delay_wait);
-		add_wait_queue(&delay_wait, &wait);
-		set_current_state(TASK_INTERRUPTIBLE);
-		schedule_timeout(1);
-		pending = signal_pending(current);
-		remove_wait_queue(&delay_wait, &wait);
-		set_current_state(TASK_RUNNING);
-		if (pending)
-			return 1;	/* Interrupted */
-	}
-
-	return 0;
-}
-
-int
-dhd_net2idx(dhd_info_t *dhd, struct net_device *net)
-{
-	int i = 0;
-
-	ASSERT(dhd);
-	while (i < DHD_MAX_IFS) {
-		if (dhd->iflist[i] && (dhd->iflist[i]->net == net))
-			return i;
-		i++;
-	}
-
-	return DHD_BAD_IF;
-}
-
-struct net_device * dhd_idx2net(struct dhd_pub *dhd_pub, int ifidx)
-{
-	struct dhd_info *dhd_info;
-
-	if (!dhd_pub || ifidx < 0 || ifidx >= DHD_MAX_IFS)
-		return NULL;
-	dhd_info = dhd_pub->info;
-	if (dhd_info && dhd_info->iflist[ifidx])
-		return dhd_info->iflist[ifidx]->net;
-	return NULL;
-}
-
-int
-dhd_ifname2idx(dhd_info_t *dhd, char *name)
-{
-	int i = DHD_MAX_IFS;
-
-	ASSERT(dhd);
-
-	if (name == NULL || *name == '\0')
-		return 0;
-
-	while (--i > 0)
-		if (dhd->iflist[i] && !strncmp(dhd->iflist[i]->name, name, IFNAMSIZ))
-				break;
-
-	DHD_TRACE(("%s: return idx %d for \"%s\"\n", __FUNCTION__, i, name));
-
-	return i;	/* default - the primary interface */
-}
-
-char *
-dhd_ifname(dhd_pub_t *dhdp, int ifidx)
-{
-	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
-
-	ASSERT(dhd);
-
-	if (ifidx < 0 || ifidx >= DHD_MAX_IFS) {
-		DHD_ERROR(("%s: ifidx %d out of range\n", __FUNCTION__, ifidx));
-		return "<if_bad>";
-	}
-
-	if (dhd->iflist[ifidx] == NULL) {
-		DHD_ERROR(("%s: null i/f %d\n", __FUNCTION__, ifidx));
-		return "<if_null>";
-	}
-
-	if (dhd->iflist[ifidx]->net)
-		return dhd->iflist[ifidx]->net->name;
-
-	return "<if_none>";
-}
-
-uint8 *
-dhd_bssidx2bssid(dhd_pub_t *dhdp, int idx)
-{
-	int i;
-	dhd_info_t *dhd = (dhd_info_t *)dhdp;
-
-	ASSERT(dhd);
-	for (i = 0; i < DHD_MAX_IFS; i++)
-	if (dhd->iflist[i] && dhd->iflist[i]->bssidx == idx)
-		return dhd->iflist[i]->mac_addr;
-
-	return NULL;
-}
-
-
-static void
-_dhd_set_multicast_list(dhd_info_t *dhd, int ifidx)
-{
-	struct net_device *dev;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35)
-	struct netdev_hw_addr *ha;
-#else
-	struct dev_mc_list *mclist;
-#endif
-	uint32 allmulti, cnt;
-
-	wl_ioctl_t ioc;
-	char *buf, *bufp;
-	uint buflen;
-	int ret;
-
-	ASSERT(dhd && dhd->iflist[ifidx]);
-	dev = dhd->iflist[ifidx]->net;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
-	netif_addr_lock_bh(dev);
-#endif
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35)
-	cnt = netdev_mc_count(dev);
-#else
-	cnt = dev->mc_count;
-#endif
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
-	netif_addr_unlock_bh(dev);
-#endif
-
-	/* Determine initial value of allmulti flag */
-	allmulti = (dev->flags & IFF_ALLMULTI) ? TRUE : FALSE;
-
-	/* Send down the multicast list first. */
-
-
-	buflen = sizeof("mcast_list") + sizeof(cnt) + (cnt * ETHER_ADDR_LEN);
-	if (!(bufp = buf = MALLOC(dhd->pub.osh, buflen))) {
-		DHD_ERROR(("%s: out of memory for mcast_list, cnt %d\n",
-		           dhd_ifname(&dhd->pub, ifidx), cnt));
-		return;
-	}
-
-	strcpy(bufp, "mcast_list");
-	bufp += strlen("mcast_list") + 1;
-
-	cnt = htol32(cnt);
-	memcpy(bufp, &cnt, sizeof(cnt));
-	bufp += sizeof(cnt);
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
-	netif_addr_lock_bh(dev);
-#endif
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35)
-	netdev_for_each_mc_addr(ha, dev) {
-		if (!cnt)
-			break;
-		memcpy(bufp, ha->addr, ETHER_ADDR_LEN);
-		bufp += ETHER_ADDR_LEN;
-		cnt--;
-	}
-#else
-	for (mclist = dev->mc_list; (mclist && (cnt > 0)); cnt--, mclist = mclist->next) {
-		memcpy(bufp, (void *)mclist->dmi_addr, ETHER_ADDR_LEN);
-		bufp += ETHER_ADDR_LEN;
-	}
-#endif
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
-	netif_addr_unlock_bh(dev);
-#endif
-
-	memset(&ioc, 0, sizeof(ioc));
-	ioc.cmd = WLC_SET_VAR;
-	ioc.buf = buf;
-	ioc.len = buflen;
-	ioc.set = TRUE;
-
-	ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
-	if (ret < 0) {
-		DHD_ERROR(("%s: set mcast_list failed, cnt %d\n",
-			dhd_ifname(&dhd->pub, ifidx), cnt));
-		allmulti = cnt ? TRUE : allmulti;
-	}
-
-	MFREE(dhd->pub.osh, buf, buflen);
-
-	/* Now send the allmulti setting.  This is based on the setting in the
-	 * net_device flags, but might be modified above to be turned on if we
-	 * were trying to set some addresses and dongle rejected it...
-	 */
-
-	buflen = sizeof("allmulti") + sizeof(allmulti);
-	if (!(buf = MALLOC(dhd->pub.osh, buflen))) {
-		DHD_ERROR(("%s: out of memory for allmulti\n", dhd_ifname(&dhd->pub, ifidx)));
-		return;
-	}
-	allmulti = htol32(allmulti);
-
-	if (!bcm_mkiovar("allmulti", (void*)&allmulti, sizeof(allmulti), buf, buflen)) {
-		DHD_ERROR(("%s: mkiovar failed for allmulti, datalen %d buflen %u\n",
-		           dhd_ifname(&dhd->pub, ifidx), (int)sizeof(allmulti), buflen));
-		MFREE(dhd->pub.osh, buf, buflen);
-		return;
-	}
-
-
-	memset(&ioc, 0, sizeof(ioc));
-	ioc.cmd = WLC_SET_VAR;
-	ioc.buf = buf;
-	ioc.len = buflen;
-	ioc.set = TRUE;
-
-	ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
-	if (ret < 0) {
-		DHD_ERROR(("%s: set allmulti %d failed\n",
-		           dhd_ifname(&dhd->pub, ifidx), ltoh32(allmulti)));
-	}
-
-	MFREE(dhd->pub.osh, buf, buflen);
-
-	/* Finally, pick up the PROMISC flag as well, like the NIC driver does */
-
-	allmulti = (dev->flags & IFF_PROMISC) ? TRUE : FALSE;
-	allmulti = htol32(allmulti);
-
-	memset(&ioc, 0, sizeof(ioc));
-	ioc.cmd = WLC_SET_PROMISC;
-	ioc.buf = &allmulti;
-	ioc.len = sizeof(allmulti);
-	ioc.set = TRUE;
-
-	ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
-	if (ret < 0) {
-		DHD_ERROR(("%s: set promisc %d failed\n",
-		           dhd_ifname(&dhd->pub, ifidx), ltoh32(allmulti)));
-	}
-}
-
-static int
-_dhd_set_mac_address(dhd_info_t *dhd, int ifidx, struct ether_addr *addr)
-{
-	char buf[32];
-	wl_ioctl_t ioc;
-	int ret;
-
-	if (!bcm_mkiovar("cur_etheraddr", (char*)addr, ETHER_ADDR_LEN, buf, 32)) {
-		DHD_ERROR(("%s: mkiovar failed for cur_etheraddr\n", dhd_ifname(&dhd->pub, ifidx)));
-		return -1;
-	}
-	memset(&ioc, 0, sizeof(ioc));
-	ioc.cmd = WLC_SET_VAR;
-	ioc.buf = buf;
-	ioc.len = 32;
-	ioc.set = TRUE;
-
-	ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
-	if (ret < 0) {
-		DHD_ERROR(("%s: set cur_etheraddr failed\n", dhd_ifname(&dhd->pub, ifidx)));
-	} else {
-		memcpy(dhd->iflist[ifidx]->net->dev_addr, addr, ETHER_ADDR_LEN);
-	}
-
-	return ret;
-}
-
-#ifdef SOFTAP
-extern struct net_device *ap_net_dev;
-extern tsk_ctl_t ap_eth_ctl; /* ap netdev heper thread ctl */
-#endif
-
-static void
-dhd_op_if(dhd_if_t *ifp)
-{
-	dhd_info_t *dhd;
-	int ret = 0, err = 0;
-#ifdef SOFTAP
-	unsigned long flags;
-#endif
-
-	ASSERT(ifp && ifp->info && ifp->idx);	/* Virtual interfaces only */
-
-	dhd = ifp->info;
-
-	DHD_TRACE(("%s: idx %d, state %d\n", __FUNCTION__, ifp->idx, ifp->state));
-
-#ifdef WL_CFG80211
-	if (wl_cfg80211_is_progress_ifchange())
-			return;
-
-#endif
-	switch (ifp->state) {
-	case WLC_E_IF_ADD:
-		/*
-		 * Delete the existing interface before overwriting it
-		 * in case we missed the WLC_E_IF_DEL event.
-		 */
-		if (ifp->net != NULL) {
-			DHD_ERROR(("%s: ERROR: netdev:%s already exists, try free & unregister \n",
-			 __FUNCTION__, ifp->net->name));
-			netif_stop_queue(ifp->net);
-			unregister_netdev(ifp->net);
-			free_netdev(ifp->net);
-		}
-		/* Allocate etherdev, including space for private structure */
-		if (!(ifp->net = alloc_etherdev(sizeof(dhd)))) {
-			DHD_ERROR(("%s: OOM - alloc_etherdev\n", __FUNCTION__));
-			ret = -ENOMEM;
-		}
-		if (ret == 0) {
-			strncpy(ifp->net->name, ifp->name, IFNAMSIZ);
-			ifp->net->name[IFNAMSIZ - 1] = '\0';
-			memcpy(netdev_priv(ifp->net), &dhd, sizeof(dhd));
-#ifdef WL_CFG80211
-			if (dhd->dhd_state & DHD_ATTACH_STATE_CFG80211)
-				if (!wl_cfg80211_notify_ifadd(ifp->net, ifp->idx, ifp->bssidx,
-					dhd_net_attach)) {
-					ifp->state = 0;
-					return;
-			}
-
-#endif
-
-			if ((err = dhd_net_attach(&dhd->pub, ifp->idx)) != 0) {
-				DHD_ERROR(("%s: dhd_net_attach failed, err %d\n",
-					__FUNCTION__, err));
-				ret = -EOPNOTSUPP;
-			} else {
-#if defined(SOFTAP)
-		if (ap_fw_loaded && !(dhd->dhd_state & DHD_ATTACH_STATE_CFG80211)) {
-				 /* semaphore that the soft AP CODE waits on */
-				flags = dhd_os_spin_lock(&dhd->pub);
-
-				/* save ptr to wl0.1 netdev for use in wl_iw.c  */
-				ap_net_dev = ifp->net;
-				 /* signal to the SOFTAP 'sleeper' thread, wl0.1 is ready */
-				up(&ap_eth_ctl.sema);
-				dhd_os_spin_unlock(&dhd->pub, flags);
-		}
-#endif
-				DHD_TRACE(("\n ==== pid:%x, net_device for if:%s created ===\n\n",
-					current->pid, ifp->net->name));
-				ifp->state = 0;
-			}
-		}
-		break;
-	case WLC_E_IF_DEL:
-		if (ifp->net != NULL) {
-			DHD_TRACE(("\n%s: got 'WLC_E_IF_DEL' state\n", __FUNCTION__));
-#ifdef WL_CFG80211
-			wl_cfg80211_ifdel_ops(ifp->net);
-#endif
-			netif_stop_queue(ifp->net);
-			unregister_netdev(ifp->net);
-			ret = DHD_DEL_IF;	/* Make sure the free_netdev() is called */
-		}
-		break;
-	default:
-		DHD_ERROR(("%s: bad op %d\n", __FUNCTION__, ifp->state));
-		ASSERT(!ifp->state);
-		break;
-	}
-
-	if (ret < 0) {
-		if (ifp->net) {
-			free_netdev(ifp->net);
-		}
-		dhd->iflist[ifp->idx] = NULL;
-#ifdef WL_CFG80211
-		if (dhd->dhd_state & DHD_ATTACH_STATE_CFG80211) {
-			wl_cfg80211_notify_ifdel(ifp->net);
-		}
-#endif
-#ifdef SOFTAP
-		flags = dhd_os_spin_lock(&dhd->pub);
-		if (ifp->net == ap_net_dev)
-			ap_net_dev = NULL;   /*  NULL  SOFTAP global wl0.1 as well */
-		dhd_os_spin_unlock(&dhd->pub, flags);
-#endif /*  SOFTAP */
-	MFREE(dhd->pub.osh, ifp, sizeof(*ifp));
-	}
-}
-
-static int
-_dhd_sysioc_thread(void *data)
-{
-	tsk_ctl_t *tsk = (tsk_ctl_t *)data;
-	dhd_info_t *dhd = (dhd_info_t *)tsk->parent;
-
-
-	int i;
-#ifdef SOFTAP
-	bool in_ap = FALSE;
-	unsigned long flags;
-#endif
-
-	DAEMONIZE("dhd_sysioc");
-
-	complete(&tsk->completed);
-
-	while (down_interruptible(&tsk->sema) == 0) {
-
-		SMP_RD_BARRIER_DEPENDS();
-		if (tsk->terminated) {
-			break;
-		}
-
-		dhd_net_if_lock_local(dhd);
-		DHD_OS_WAKE_LOCK(&dhd->pub);
-
-		for (i = 0; i < DHD_MAX_IFS; i++) {
-			if (dhd->iflist[i]) {
-				DHD_TRACE(("%s: interface %d\n", __FUNCTION__, i));
-#ifdef SOFTAP
-				flags = dhd_os_spin_lock(&dhd->pub);
-				in_ap = (ap_net_dev != NULL);
-				dhd_os_spin_unlock(&dhd->pub, flags);
-#endif /* SOFTAP */
-				if (dhd->iflist[i]->state)
-					dhd_op_if(dhd->iflist[i]);
-#ifdef SOFTAP
-				if (dhd->iflist[i] == NULL) {
-					DHD_TRACE(("\n\n %s: interface %d just been removed,"
-						"!\n\n", __FUNCTION__, i));
-					continue;
-				}
-
-				if (in_ap && dhd->set_macaddress)  {
-					DHD_TRACE(("attempt to set MAC for %s in AP Mode,"
-						"blocked. \n", dhd->iflist[i]->net->name));
-					dhd->set_macaddress = FALSE;
-					continue;
-				}
-
-				if (in_ap && dhd->set_multicast)  {
-					DHD_TRACE(("attempt to set MULTICAST list for %s"
-					 "in AP Mode, blocked. \n", dhd->iflist[i]->net->name));
-					dhd->set_multicast = FALSE;
-					continue;
-				}
-#endif /* SOFTAP */
-				if (dhd->set_multicast) {
-					dhd->set_multicast = FALSE;
-					_dhd_set_multicast_list(dhd, i);
-				}
-				if (dhd->set_macaddress) {
-					dhd->set_macaddress = FALSE;
-					_dhd_set_mac_address(dhd, i, &dhd->macvalue);
-				}
-			}
-		}
-
-		DHD_OS_WAKE_UNLOCK(&dhd->pub);
-		dhd_net_if_unlock_local(dhd);
-	}
-	DHD_TRACE(("%s: stopped\n", __FUNCTION__));
-	complete_and_exit(&tsk->completed, 0);
-}
-
-static int
-dhd_set_mac_address(struct net_device *dev, void *addr)
-{
-	int ret = 0;
-
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	struct sockaddr *sa = (struct sockaddr *)addr;
-	int ifidx;
-
-	ifidx = dhd_net2idx(dhd, dev);
-	if (ifidx == DHD_BAD_IF)
-		return -1;
-
-	ASSERT(&dhd->thr_sysioc_ctl.thr_pid >= 0);
-	memcpy(&dhd->macvalue, sa->sa_data, ETHER_ADDR_LEN);
-	dhd->set_macaddress = TRUE;
-	up(&dhd->thr_sysioc_ctl.sema);
-
-	return ret;
-}
-
-static void
-dhd_set_multicast_list(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	int ifidx;
-
-	ifidx = dhd_net2idx(dhd, dev);
-	if (ifidx == DHD_BAD_IF)
-		return;
-
-	ASSERT(&dhd->thr_sysioc_ctl.thr_pid >= 0);
-	dhd->set_multicast = TRUE;
-	up(&dhd->thr_sysioc_ctl.sema);
-}
-
-#ifdef PROP_TXSTATUS
-int
-dhd_os_wlfc_block(dhd_pub_t *pub)
-{
-	dhd_info_t *di = (dhd_info_t *)(pub->info);
-	ASSERT(di != NULL);
-	spin_lock_bh(&di->wlfc_spinlock);
-	return 1;
-}
-
-int
-dhd_os_wlfc_unblock(dhd_pub_t *pub)
-{
-	dhd_info_t *di = (dhd_info_t *)(pub->info);
-	ASSERT(di != NULL);
-	spin_unlock_bh(&di->wlfc_spinlock);
-	return 1;
-}
-
-const uint8 wme_fifo2ac[] = { 0, 1, 2, 3, 1, 1 };
-uint8 prio2fifo[8] = { 1, 0, 0, 1, 2, 2, 3, 3 };
-#define WME_PRIO2AC(prio)	wme_fifo2ac[prio2fifo[(prio)]]
-
-#endif /* PROP_TXSTATUS */
-int
-dhd_sendpkt(dhd_pub_t *dhdp, int ifidx, void *pktbuf)
-{
-	int ret;
-	dhd_info_t *dhd = (dhd_info_t *)(dhdp->info);
-	struct ether_header *eh = NULL;
-
-	/* Reject if down */
-	if (!dhdp->up || (dhdp->busstate == DHD_BUS_DOWN)) {
-		/* free the packet here since the caller won't */
-		PKTFREE(dhdp->osh, pktbuf, TRUE);
-		return -ENODEV;
-	}
-
-	/* Update multicast statistic */
-	if (PKTLEN(dhdp->osh, pktbuf) >= ETHER_ADDR_LEN) {
-		uint8 *pktdata = (uint8 *)PKTDATA(dhdp->osh, pktbuf);
-		eh = (struct ether_header *)pktdata;
-
-		if (ETHER_ISMULTI(eh->ether_dhost))
-			dhdp->tx_multicast++;
-		if (ntoh16(eh->ether_type) == ETHER_TYPE_802_1X)
-			atomic_inc(&dhd->pend_8021x_cnt);
-	}
-
-	/* Look into the packet and update the packet priority */
-	if (PKTPRIO(pktbuf) == 0)
-		pktsetprio(pktbuf, FALSE);
-
-#ifdef PROP_TXSTATUS
-	if (dhdp->wlfc_state) {
-		/* store the interface ID */
-		DHD_PKTTAG_SETIF(PKTTAG(pktbuf), ifidx);
-
-		/* store destination MAC in the tag as well */
-		DHD_PKTTAG_SETDSTN(PKTTAG(pktbuf), eh->ether_dhost);
-
-		/* decide which FIFO this packet belongs to */
-		if (ETHER_ISMULTI(eh->ether_dhost))
-			/* one additional queue index (highest AC + 1) is used for bc/mc queue */
-			DHD_PKTTAG_SETFIFO(PKTTAG(pktbuf), AC_COUNT);
-		else
-			DHD_PKTTAG_SETFIFO(PKTTAG(pktbuf), WME_PRIO2AC(PKTPRIO(pktbuf)));
-	} else
-#endif /* PROP_TXSTATUS */
-	/* If the protocol uses a data header, apply it */
-	dhd_prot_hdrpush(dhdp, ifidx, pktbuf);
-
-	/* Use bus module to send data frame */
-#ifdef WLMEDIA_HTSF
-	dhd_htsf_addtxts(dhdp, pktbuf);
-#endif
-#ifdef PROP_TXSTATUS
-	if (dhdp->wlfc_state && ((athost_wl_status_info_t*)dhdp->wlfc_state)->proptxstatus_mode
-			!= WLFC_FCMODE_NONE) {
-		dhd_os_wlfc_block(dhdp);
-		ret = dhd_wlfc_enque_sendq(dhdp->wlfc_state, DHD_PKTTAG_FIFO(PKTTAG(pktbuf)),
-			pktbuf);
-		dhd_wlfc_commit_packets(dhdp->wlfc_state,  (f_commitpkt_t)dhd_bus_txdata,
-			dhdp->bus);
-		if (((athost_wl_status_info_t*)dhdp->wlfc_state)->toggle_host_if) {
-			((athost_wl_status_info_t*)dhdp->wlfc_state)->toggle_host_if = 0;
-		}
-		dhd_os_wlfc_unblock(dhdp);
-	}
-	else
-		/* non-proptxstatus way */
-	ret = dhd_bus_txdata(dhdp->bus, pktbuf);
-#else
-	ret = dhd_bus_txdata(dhdp->bus, pktbuf);
-#endif /* PROP_TXSTATUS */
-
-
-	return ret;
-}
-
-int
-dhd_start_xmit(struct sk_buff *skb, struct net_device *net)
-{
-	int ret;
-	void *pktbuf;
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
-	int ifidx;
-#ifdef WLMEDIA_HTSF
-	uint8 htsfdlystat_sz = dhd->pub.htsfdlystat_sz;
-#else
-	uint8 htsfdlystat_sz = 0;
-#endif
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	DHD_OS_WAKE_LOCK(&dhd->pub);
-
-	/* Reject if down */
-	if (!dhd->pub.up || (dhd->pub.busstate == DHD_BUS_DOWN)) {
-		DHD_ERROR(("%s: xmit rejected pub.up=%d busstate=%d \n",
-			__FUNCTION__, dhd->pub.up, dhd->pub.busstate));
-		netif_stop_queue(net);
-		/* Send Event when bus down detected during data session */
-		if (dhd->pub.busstate == DHD_BUS_DOWN)  {
-			DHD_ERROR(("%s: Event HANG sent up\n", __FUNCTION__));
-			net_os_send_hang_message(net);
-		}
-		DHD_OS_WAKE_UNLOCK(&dhd->pub);
-		return -ENODEV;
-	}
-
-	ifidx = dhd_net2idx(dhd, net);
-	if (ifidx == DHD_BAD_IF) {
-		DHD_ERROR(("%s: bad ifidx %d\n", __FUNCTION__, ifidx));
-		netif_stop_queue(net);
-		DHD_OS_WAKE_UNLOCK(&dhd->pub);
-		return -ENODEV;
-	}
-
-	/* Make sure there's enough room for any header */
-
-	if (skb_headroom(skb) < dhd->pub.hdrlen + htsfdlystat_sz) {
-		struct sk_buff *skb2;
-
-		DHD_INFO(("%s: insufficient headroom\n",
-		          dhd_ifname(&dhd->pub, ifidx)));
-		dhd->pub.tx_realloc++;
-
-		skb2 = skb_realloc_headroom(skb, dhd->pub.hdrlen + htsfdlystat_sz);
-
-		dev_kfree_skb(skb);
-		if ((skb = skb2) == NULL) {
-			DHD_ERROR(("%s: skb_realloc_headroom failed\n",
-			           dhd_ifname(&dhd->pub, ifidx)));
-			ret = -ENOMEM;
-			goto done;
-		}
-	}
-
-	/* Convert to packet */
-	if (!(pktbuf = PKTFRMNATIVE(dhd->pub.osh, skb))) {
-		DHD_ERROR(("%s: PKTFRMNATIVE failed\n",
-		           dhd_ifname(&dhd->pub, ifidx)));
-		dev_kfree_skb_any(skb);
-		ret = -ENOMEM;
-		goto done;
-	}
-#ifdef WLMEDIA_HTSF
-	if (htsfdlystat_sz && PKTLEN(dhd->pub.osh, pktbuf) >= ETHER_ADDR_LEN) {
-		uint8 *pktdata = (uint8 *)PKTDATA(dhd->pub.osh, pktbuf);
-		struct ether_header *eh = (struct ether_header *)pktdata;
-
-		if (!ETHER_ISMULTI(eh->ether_dhost) &&
-			(ntoh16(eh->ether_type) == ETHER_TYPE_IP)) {
-			eh->ether_type = hton16(ETHER_TYPE_BRCM_PKTDLYSTATS);
-		}
-	}
-#endif
-
-	ret = dhd_sendpkt(&dhd->pub, ifidx, pktbuf);
-
-
-done:
-	if (ret)
-		dhd->pub.dstats.tx_dropped++;
-	else
-		dhd->pub.tx_packets++;
-
-	DHD_OS_WAKE_UNLOCK(&dhd->pub);
-
-	/* Return ok: we always eat the packet */
-	return 0;
-}
-
-void
-dhd_txflowcontrol(dhd_pub_t *dhdp, int ifidx, bool state)
-{
-	struct net_device *net;
-	dhd_info_t *dhd = dhdp->info;
-	int i;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	dhdp->txoff = state;
-	ASSERT(dhd);
-
-	if (ifidx == ALL_INTERFACES) {
-		/* Flow control on all active interfaces */
-		for (i = 0; i < DHD_MAX_IFS; i++) {
-			if (dhd->iflist[i]) {
-				net = dhd->iflist[i]->net;
-				if (state == ON)
-					netif_stop_queue(net);
-				else
-					netif_wake_queue(net);
-			}
-		}
-	}
-	else {
-		if (dhd->iflist[ifidx]) {
-			net = dhd->iflist[ifidx]->net;
-			if (state == ON)
-				netif_stop_queue(net);
-			else
-				netif_wake_queue(net);
-		}
-	}
-}
-
-void
-dhd_rx_frame(dhd_pub_t *dhdp, int ifidx, void *pktbuf, int numpkt, uint8 chan)
-{
-	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
-	struct sk_buff *skb;
-	uchar *eth;
-	uint len;
-	void *data, *pnext = NULL, *save_pktbuf;
-	int i;
-	dhd_if_t *ifp;
-	wl_event_msg_t event;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	save_pktbuf = pktbuf;
-
-	for (i = 0; pktbuf && i < numpkt; i++, pktbuf = pnext) {
-		struct ether_header *eh;
-		struct dot11_llc_snap_header *lsh;
-
-		ifp = dhd->iflist[ifidx];
-
-		/* Dropping packets before registering net device to avoid kernel panic */
-		if (!ifp->net || ifp->net->reg_state != NETREG_REGISTERED) {
-			DHD_ERROR(("%s: net device is NOT registered yet. drop packet\n",
-			__FUNCTION__));
-			PKTFREE(dhdp->osh, pktbuf, TRUE);
-			continue;
-		}
-
-		pnext = PKTNEXT(dhdp->osh, pktbuf);
-		PKTSETNEXT(wl->sh.osh, pktbuf, NULL);
-
-		eh = (struct ether_header *)PKTDATA(wl->sh.osh, pktbuf);
-		lsh = (struct dot11_llc_snap_header *)&eh[1];
-
-		if ((ntoh16(eh->ether_type) < ETHER_TYPE_MIN) &&
-		    (PKTLEN(wl->sh.osh, pktbuf) >= RFC1042_HDR_LEN) &&
-		    bcmp(lsh, BT_SIG_SNAP_MPROT, DOT11_LLC_SNAP_HDR_LEN - 2) == 0 &&
-		    lsh->type == HTON16(BTA_PROT_L2CAP)) {
-			amp_hci_ACL_data_t *ACL_data = (amp_hci_ACL_data_t *)
-			        ((uint8 *)eh + RFC1042_HDR_LEN);
-			ACL_data = NULL;
-		}
-
-#ifdef PROP_TXSTATUS
-		if (dhdp->wlfc_state && PKTLEN(wl->sh.osh, pktbuf) == 0) {
-			/* WLFC may send header only packet when
-			there is an urgent message but no packet to
-			piggy-back on
-			*/
-			((athost_wl_status_info_t*)dhdp->wlfc_state)->stats.wlfc_header_only_pkt++;
-			PKTFREE(dhdp->osh, pktbuf, TRUE);
-			continue;
-		}
-#endif
-
-		skb = PKTTONATIVE(dhdp->osh, pktbuf);
-
-		/* Get the protocol, maintain skb around eth_type_trans()
-		 * The main reason for this hack is for the limitation of
-		 * Linux 2.4 where 'eth_type_trans' uses the 'net->hard_header_len'
-		 * to perform skb_pull inside vs ETH_HLEN. Since to avoid
-		 * coping of the packet coming from the network stack to add
-		 * BDC, Hardware header etc, during network interface registration
-		 * we set the 'net->hard_header_len' to ETH_HLEN + extra space required
-		 * for BDC, Hardware header etc. and not just the ETH_HLEN
-		 */
-		eth = skb->data;
-		len = skb->len;
-
-		ifp = dhd->iflist[ifidx];
-		if (ifp == NULL)
-			ifp = dhd->iflist[0];
-
-		ASSERT(ifp);
-		skb->dev = ifp->net;
-		skb->protocol = eth_type_trans(skb, skb->dev);
-
-		if (skb->pkt_type == PACKET_MULTICAST) {
-			dhd->pub.rx_multicast++;
-		}
-
-		skb->data = eth;
-		skb->len = len;
-
-#ifdef WLMEDIA_HTSF
-	dhd_htsf_addrxts(dhdp, pktbuf);
-#endif
-		/* Strip header, count, deliver upward */
-		skb_pull(skb, ETH_HLEN);
-
-		/* Process special event packets and then discard them */
-		if (ntoh16(skb->protocol) == ETHER_TYPE_BRCM) {
-			dhd_wl_host_event(dhd, &ifidx,
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 22)
-			skb->mac_header,
-#else
-			skb->mac.raw,
-#endif
-			&event,
-			&data);
-
-			wl_event_to_host_order(&event);
-			if (event.event_type == WLC_E_BTA_HCI_EVENT) {
-				dhd_bta_doevt(dhdp, data, event.datalen);
-			}
-		}
-
-		ASSERT(ifidx < DHD_MAX_IFS && dhd->iflist[ifidx]);
-		if (dhd->iflist[ifidx] && !dhd->iflist[ifidx]->state)
-			ifp = dhd->iflist[ifidx];
-
-		if (ifp->net)
-			ifp->net->last_rx = jiffies;
-
-		dhdp->dstats.rx_bytes += skb->len;
-		dhdp->rx_packets++; /* Local count */
-
-		if (in_interrupt()) {
-			netif_rx(skb);
-		} else {
-			/* If the receive is not processed inside an ISR,
-			 * the softirqd must be woken explicitly to service
-			 * the NET_RX_SOFTIRQ.  In 2.6 kernels, this is handled
-			 * by netif_rx_ni(), but in earlier kernels, we need
-			 * to do it manually.
-			 */
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0)
-			netif_rx_ni(skb);
-#else
-			ulong flags;
-			netif_rx(skb);
-			local_irq_save(flags);
-			RAISE_RX_SOFTIRQ();
-			local_irq_restore(flags);
-#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0) */
-		}
-	}
-	DHD_OS_WAKE_LOCK_TIMEOUT_ENABLE(dhdp);
-}
-
-void
-dhd_event(struct dhd_info *dhd, char *evpkt, int evlen, int ifidx)
-{
-	/* Linux version has nothing to do */
-	return;
-}
-
-void
-dhd_txcomplete(dhd_pub_t *dhdp, void *txp, bool success)
-{
-	uint ifidx;
-	dhd_info_t *dhd = (dhd_info_t *)(dhdp->info);
-	struct ether_header *eh;
-	uint16 type;
-	uint len;
-
-	dhd_prot_hdrpull(dhdp, &ifidx, txp);
-
-	eh = (struct ether_header *)PKTDATA(dhdp->osh, txp);
-	type  = ntoh16(eh->ether_type);
-
-	if (type == ETHER_TYPE_802_1X)
-		atomic_dec(&dhd->pend_8021x_cnt);
-
-	/* Crack open the packet and check to see if it is BT HCI ACL data packet.
-	 * If yes generate packet completion event.
-	 */
-	len = PKTLEN(dhdp->osh, txp);
-
-	/* Generate ACL data tx completion event locally to avoid SDIO bus transaction */
-	if ((type < ETHER_TYPE_MIN) && (len >= RFC1042_HDR_LEN)) {
-		struct dot11_llc_snap_header *lsh = (struct dot11_llc_snap_header *)&eh[1];
-
-		if (bcmp(lsh, BT_SIG_SNAP_MPROT, DOT11_LLC_SNAP_HDR_LEN - 2) == 0 &&
-		    ntoh16(lsh->type) == BTA_PROT_L2CAP) {
-
-			dhd_bta_tx_hcidata_complete(dhdp, txp, success);
-		}
-	}
-}
-
-static struct net_device_stats *
-dhd_get_stats(struct net_device *net)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
-	dhd_if_t *ifp;
-	int ifidx;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	ifidx = dhd_net2idx(dhd, net);
-	if (ifidx == DHD_BAD_IF)
-		return NULL;
-
-	ifp = dhd->iflist[ifidx];
-	ASSERT(dhd && ifp);
-
-	if (dhd->pub.up) {
-		/* Use the protocol to get dongle stats */
-		dhd_prot_dstats(&dhd->pub);
-	}
-
-	/* Copy dongle stats to net device stats */
-	ifp->stats.rx_packets = dhd->pub.dstats.rx_packets;
-	ifp->stats.tx_packets = dhd->pub.dstats.tx_packets;
-	ifp->stats.rx_bytes = dhd->pub.dstats.rx_bytes;
-	ifp->stats.tx_bytes = dhd->pub.dstats.tx_bytes;
-	ifp->stats.rx_errors = dhd->pub.dstats.rx_errors;
-	ifp->stats.tx_errors = dhd->pub.dstats.tx_errors;
-	ifp->stats.rx_dropped = dhd->pub.dstats.rx_dropped;
-	ifp->stats.tx_dropped = dhd->pub.dstats.tx_dropped;
-	ifp->stats.multicast = dhd->pub.dstats.multicast;
-
-	return &ifp->stats;
-}
-
-#ifdef DHDTHREAD
-static int
-dhd_watchdog_thread(void *data)
-{
-	tsk_ctl_t *tsk = (tsk_ctl_t *)data;
-	dhd_info_t *dhd = (dhd_info_t *)tsk->parent;
-	/* This thread doesn't need any user-level access,
-	 * so get rid of all our resources
-	 */
-#ifdef DHD_SCHED
-	if (dhd_watchdog_prio > 0) {
-		struct sched_param param;
-		param.sched_priority = (dhd_watchdog_prio < MAX_RT_PRIO)?
-			dhd_watchdog_prio:(MAX_RT_PRIO-1);
-		setScheduler(current, SCHED_FIFO, &param);
-	}
-#endif /* DHD_SCHED */
-
-	DAEMONIZE("dhd_watchdog");
-
-	/* Run until signal received */
-	complete(&tsk->completed);
-
-	while (1)
-		if (down_interruptible (&tsk->sema) == 0) {
-			unsigned long flags;
-
-			SMP_RD_BARRIER_DEPENDS();
-			if (tsk->terminated) {
-				break;
-			}
-
-			dhd_os_sdlock(&dhd->pub);
-			if (dhd->pub.dongle_reset == FALSE) {
-				DHD_TIMER(("%s:\n", __FUNCTION__));
-
-				/* Call the bus module watchdog */
-				dhd_bus_watchdog(&dhd->pub);
-
-				flags = dhd_os_spin_lock(&dhd->pub);
-				/* Count the tick for reference */
-				dhd->pub.tickcnt++;
-				/* Reschedule the watchdog */
-				if (dhd->wd_timer_valid)
-					mod_timer(&dhd->timer,
-					jiffies + dhd_watchdog_ms * HZ / 1000);
-				dhd_os_spin_unlock(&dhd->pub, flags);
-			}
-			dhd_os_sdunlock(&dhd->pub);
-			DHD_OS_WAKE_UNLOCK(&dhd->pub);
-		} else {
-			break;
-		}
-
-	complete_and_exit(&tsk->completed, 0);
-}
-#endif /* DHDTHREAD */
-
-static void dhd_watchdog(ulong data)
-{
-	dhd_info_t *dhd = (dhd_info_t *)data;
-	unsigned long flags;
-
-	DHD_OS_WAKE_LOCK(&dhd->pub);
-	if (dhd->pub.dongle_reset) {
-		DHD_OS_WAKE_UNLOCK(&dhd->pub);
-		return;
-	}
-
-#ifdef DHDTHREAD
-	if (dhd->thr_wdt_ctl.thr_pid >= 0) {
-		up(&dhd->thr_wdt_ctl.sema);
-		return;
-	}
-#endif /* DHDTHREAD */
-
-	dhd_os_sdlock(&dhd->pub);
-	/* Call the bus module watchdog */
-	dhd_bus_watchdog(&dhd->pub);
-
-	flags = dhd_os_spin_lock(&dhd->pub);
-	/* Count the tick for reference */
-	dhd->pub.tickcnt++;
-
-	/* Reschedule the watchdog */
-	if (dhd->wd_timer_valid)
-		mod_timer(&dhd->timer, jiffies + dhd_watchdog_ms * HZ / 1000);
-	dhd_os_spin_unlock(&dhd->pub, flags);
-	dhd_os_sdunlock(&dhd->pub);
-	DHD_OS_WAKE_UNLOCK(&dhd->pub);
-}
-
-#ifdef DHDTHREAD
-static int
-dhd_dpc_thread(void *data)
-{
-	tsk_ctl_t *tsk = (tsk_ctl_t *)data;
-	dhd_info_t *dhd = (dhd_info_t *)tsk->parent;
-
-	/* This thread doesn't need any user-level access,
-	 * so get rid of all our resources
-	 */
-#ifdef DHD_SCHED
-	if (dhd_dpc_prio > 0)
-	{
-		struct sched_param param;
-		param.sched_priority = (dhd_dpc_prio < MAX_RT_PRIO)?dhd_dpc_prio:(MAX_RT_PRIO-1);
-		setScheduler(current, SCHED_FIFO, &param);
-	}
-#endif /* DHD_SCHED */
-
-	DAEMONIZE("dhd_dpc");
-	/* DHD_OS_WAKE_LOCK is called in dhd_sched_dpc[dhd_linux.c] down below  */
-
-	/*  signal: thread has started */
-	complete(&tsk->completed);
-
-	/* Run until signal received */
-	while (1) {
-		if (down_interruptible(&tsk->sema) == 0) {
-
-			SMP_RD_BARRIER_DEPENDS();
-			if (tsk->terminated) {
-				break;
-			}
-
-			/* Call bus dpc unless it indicated down (then clean stop) */
-			if (dhd->pub.busstate != DHD_BUS_DOWN) {
-				if (dhd_bus_dpc(dhd->pub.bus)) {
-					up(&tsk->sema);
-				}
-				else {
-					DHD_OS_WAKE_UNLOCK(&dhd->pub);
-				}
-			} else {
-				dhd_bus_stop(dhd->pub.bus, TRUE);
-				DHD_OS_WAKE_UNLOCK(&dhd->pub);
-			}
-		}
-		else
-			break;
-	}
-
-	complete_and_exit(&tsk->completed, 0);
-}
-#endif /* DHDTHREAD */
-
-static void
-dhd_dpc(ulong data)
-{
-	dhd_info_t *dhd;
-
-	dhd = (dhd_info_t *)data;
-
-	/* this (tasklet) can be scheduled in dhd_sched_dpc[dhd_linux.c]
-	 * down below , wake lock is set,
-	 * the tasklet is initialized in dhd_attach()
-	 */
-	/* Call bus dpc unless it indicated down (then clean stop) */
-	if (dhd->pub.busstate != DHD_BUS_DOWN) {
-		if (dhd_bus_dpc(dhd->pub.bus))
-			tasklet_schedule(&dhd->tasklet);
-		else
-			DHD_OS_WAKE_UNLOCK(&dhd->pub);
-	} else {
-		dhd_bus_stop(dhd->pub.bus, TRUE);
-		DHD_OS_WAKE_UNLOCK(&dhd->pub);
-	}
-}
-
-void
-dhd_sched_dpc(dhd_pub_t *dhdp)
-{
-	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
-
-	DHD_OS_WAKE_LOCK(dhdp);
-#ifdef DHDTHREAD
-	if (dhd->thr_dpc_ctl.thr_pid >= 0) {
-		up(&dhd->thr_dpc_ctl.sema);
-		return;
-	}
-#endif /* DHDTHREAD */
-
-	tasklet_schedule(&dhd->tasklet);
-}
-
-#ifdef TOE
-/* Retrieve current toe component enables, which are kept as a bitmap in toe_ol iovar */
-static int
-dhd_toe_get(dhd_info_t *dhd, int ifidx, uint32 *toe_ol)
-{
-	wl_ioctl_t ioc;
-	char buf[32];
-	int ret;
-
-	memset(&ioc, 0, sizeof(ioc));
-
-	ioc.cmd = WLC_GET_VAR;
-	ioc.buf = buf;
-	ioc.len = (uint)sizeof(buf);
-	ioc.set = FALSE;
-
-	strcpy(buf, "toe_ol");
-	if ((ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len)) < 0) {
-		/* Check for older dongle image that doesn't support toe_ol */
-		if (ret == -EIO) {
-			DHD_ERROR(("%s: toe not supported by device\n",
-				dhd_ifname(&dhd->pub, ifidx)));
-			return -EOPNOTSUPP;
-		}
-
-		DHD_INFO(("%s: could not get toe_ol: ret=%d\n", dhd_ifname(&dhd->pub, ifidx), ret));
-		return ret;
-	}
-
-	memcpy(toe_ol, buf, sizeof(uint32));
-	return 0;
-}
-
-/* Set current toe component enables in toe_ol iovar, and set toe global enable iovar */
-static int
-dhd_toe_set(dhd_info_t *dhd, int ifidx, uint32 toe_ol)
-{
-	wl_ioctl_t ioc;
-	char buf[32];
-	int toe, ret;
-
-	memset(&ioc, 0, sizeof(ioc));
-
-	ioc.cmd = WLC_SET_VAR;
-	ioc.buf = buf;
-	ioc.len = (uint)sizeof(buf);
-	ioc.set = TRUE;
-
-	/* Set toe_ol as requested */
-
-	strcpy(buf, "toe_ol");
-	memcpy(&buf[sizeof("toe_ol")], &toe_ol, sizeof(uint32));
-
-	if ((ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len)) < 0) {
-		DHD_ERROR(("%s: could not set toe_ol: ret=%d\n",
-			dhd_ifname(&dhd->pub, ifidx), ret));
-		return ret;
-	}
-
-	/* Enable toe globally only if any components are enabled. */
-
-	toe = (toe_ol != 0);
-
-	strcpy(buf, "toe");
-	memcpy(&buf[sizeof("toe")], &toe, sizeof(uint32));
-
-	if ((ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len)) < 0) {
-		DHD_ERROR(("%s: could not set toe: ret=%d\n", dhd_ifname(&dhd->pub, ifidx), ret));
-		return ret;
-	}
-
-	return 0;
-}
-#endif /* TOE */
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)
-static void
-dhd_ethtool_get_drvinfo(struct net_device *net, struct ethtool_drvinfo *info)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
-
-	sprintf(info->driver, "wl");
-	sprintf(info->version, "%lu", dhd->pub.drv_version);
-}
-
-struct ethtool_ops dhd_ethtool_ops = {
-	.get_drvinfo = dhd_ethtool_get_drvinfo
-};
-#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24) */
-
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2)
-static int
-dhd_ethtool(dhd_info_t *dhd, void *uaddr)
-{
-	struct ethtool_drvinfo info;
-	char drvname[sizeof(info.driver)];
-	uint32 cmd;
-#ifdef TOE
-	struct ethtool_value edata;
-	uint32 toe_cmpnt, csum_dir;
-	int ret;
-#endif
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	/* all ethtool calls start with a cmd word */
-	if (copy_from_user(&cmd, uaddr, sizeof (uint32)))
-		return -EFAULT;
-
-	switch (cmd) {
-	case ETHTOOL_GDRVINFO:
-		/* Copy out any request driver name */
-		if (copy_from_user(&info, uaddr, sizeof(info)))
-			return -EFAULT;
-		strncpy(drvname, info.driver, sizeof(info.driver));
-		drvname[sizeof(info.driver)-1] = '\0';
-
-		/* clear struct for return */
-		memset(&info, 0, sizeof(info));
-		info.cmd = cmd;
-
-		/* if dhd requested, identify ourselves */
-		if (strcmp(drvname, "?dhd") == 0) {
-			sprintf(info.driver, "dhd");
-			strcpy(info.version, EPI_VERSION_STR);
-		}
-
-		/* otherwise, require dongle to be up */
-		else if (!dhd->pub.up) {
-			DHD_ERROR(("%s: dongle is not up\n", __FUNCTION__));
-			return -ENODEV;
-		}
-
-		/* finally, report dongle driver type */
-		else if (dhd->pub.iswl)
-			sprintf(info.driver, "wl");
-		else
-			sprintf(info.driver, "xx");
-
-		sprintf(info.version, "%lu", dhd->pub.drv_version);
-		if (copy_to_user(uaddr, &info, sizeof(info)))
-			return -EFAULT;
-		DHD_CTL(("%s: given %*s, returning %s\n", __FUNCTION__,
-		         (int)sizeof(drvname), drvname, info.driver));
-		break;
-
-#ifdef TOE
-	/* Get toe offload components from dongle */
-	case ETHTOOL_GRXCSUM:
-	case ETHTOOL_GTXCSUM:
-		if ((ret = dhd_toe_get(dhd, 0, &toe_cmpnt)) < 0)
-			return ret;
-
-		csum_dir = (cmd == ETHTOOL_GTXCSUM) ? TOE_TX_CSUM_OL : TOE_RX_CSUM_OL;
-
-		edata.cmd = cmd;
-		edata.data = (toe_cmpnt & csum_dir) ? 1 : 0;
-
-		if (copy_to_user(uaddr, &edata, sizeof(edata)))
-			return -EFAULT;
-		break;
-
-	/* Set toe offload components in dongle */
-	case ETHTOOL_SRXCSUM:
-	case ETHTOOL_STXCSUM:
-		if (copy_from_user(&edata, uaddr, sizeof(edata)))
-			return -EFAULT;
-
-		/* Read the current settings, update and write back */
-		if ((ret = dhd_toe_get(dhd, 0, &toe_cmpnt)) < 0)
-			return ret;
-
-		csum_dir = (cmd == ETHTOOL_STXCSUM) ? TOE_TX_CSUM_OL : TOE_RX_CSUM_OL;
-
-		if (edata.data != 0)
-			toe_cmpnt |= csum_dir;
-		else
-			toe_cmpnt &= ~csum_dir;
-
-		if ((ret = dhd_toe_set(dhd, 0, toe_cmpnt)) < 0)
-			return ret;
-
-		/* If setting TX checksum mode, tell Linux the new mode */
-		if (cmd == ETHTOOL_STXCSUM) {
-			if (edata.data)
-				dhd->iflist[0]->net->features |= NETIF_F_IP_CSUM;
-			else
-				dhd->iflist[0]->net->features &= ~NETIF_F_IP_CSUM;
-		}
-
-		break;
-#endif /* TOE */
-
-	default:
-		return -EOPNOTSUPP;
-	}
-
-	return 0;
-}
-#endif /* LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2) */
-
-static int
-dhd_ioctl_entry(struct net_device *net, struct ifreq *ifr, int cmd)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
-	dhd_ioctl_t ioc;
-	int bcmerror = 0;
-	int buflen = 0;
-	void *buf = NULL;
-	uint driver = 0;
-	int ifidx;
-	int ret;
-
-	DHD_OS_WAKE_LOCK(&dhd->pub);
-
-	ifidx = dhd_net2idx(dhd, net);
-	DHD_TRACE(("%s: ifidx %d, cmd 0x%04x\n", __FUNCTION__, ifidx, cmd));
-
-	if (ifidx == DHD_BAD_IF) {
-		DHD_OS_WAKE_UNLOCK(&dhd->pub);
-		return -1;
-	}
-
-#if defined(CONFIG_WIRELESS_EXT)
-	/* linux wireless extensions */
-	if ((cmd >= SIOCIWFIRST) && (cmd <= SIOCIWLAST)) {
-		/* may recurse, do NOT lock */
-		ret = wl_iw_ioctl(net, ifr, cmd);
-		DHD_OS_WAKE_UNLOCK(&dhd->pub);
-		return ret;
-	}
-#endif /* defined(CONFIG_WIRELESS_EXT) */
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2)
-	if (cmd == SIOCETHTOOL) {
-		ret = dhd_ethtool(dhd, (void*)ifr->ifr_data);
-		DHD_OS_WAKE_UNLOCK(&dhd->pub);
-		return ret;
-	}
-#endif /* LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2) */
-
-	if (cmd == SIOCDEVPRIVATE+1) {
-		ret = wl_android_priv_cmd(net, ifr, cmd);
-		DHD_OS_WAKE_UNLOCK(&dhd->pub);
-		return ret;
-	}
-
-	if (cmd != SIOCDEVPRIVATE) {
-		DHD_OS_WAKE_UNLOCK(&dhd->pub);
-		return -EOPNOTSUPP;
-	}
-
-	memset(&ioc, 0, sizeof(ioc));
-
-	/* Copy the ioc control structure part of ioctl request */
-	if (copy_from_user(&ioc, ifr->ifr_data, sizeof(wl_ioctl_t))) {
-		bcmerror = -BCME_BADADDR;
-		goto done;
-	}
-
-	/* Copy out any buffer passed */
-	if (ioc.buf) {
-		buflen = MIN(ioc.len, DHD_IOCTL_MAXLEN);
-		/* optimization for direct ioctl calls from kernel */
-		/*
-		if (segment_eq(get_fs(), KERNEL_DS)) {
-			buf = ioc.buf;
-		} else {
-		*/
-		{
-			if (!(buf = (char*)MALLOC(dhd->pub.osh, buflen))) {
-				bcmerror = -BCME_NOMEM;
-				goto done;
-			}
-			if (copy_from_user(buf, ioc.buf, buflen)) {
-				bcmerror = -BCME_BADADDR;
-				goto done;
-			}
-		}
-	}
-
-	/* To differentiate between wl and dhd read 4 more byes */
-	if ((copy_from_user(&driver, (char *)ifr->ifr_data + sizeof(wl_ioctl_t),
-		sizeof(uint)) != 0)) {
-		bcmerror = -BCME_BADADDR;
-		goto done;
-	}
-
-	if (!capable(CAP_NET_ADMIN)) {
-		bcmerror = -BCME_EPERM;
-		goto done;
-	}
-
-	/* check for local dhd ioctl and handle it */
-	if (driver == DHD_IOCTL_MAGIC) {
-		bcmerror = dhd_ioctl((void *)&dhd->pub, &ioc, buf, buflen);
-		if (bcmerror)
-			dhd->pub.bcmerror = bcmerror;
-		goto done;
-	}
-
-	/* send to dongle (must be up, and wl). */
-	if (dhd->pub.busstate != DHD_BUS_DATA) {
-		bcmerror = BCME_DONGLE_DOWN;
-		goto done;
-	}
-
-	if (!dhd->pub.iswl) {
-		bcmerror = BCME_DONGLE_DOWN;
-		goto done;
-	}
-
-	/*
-	 * Flush the TX queue if required for proper message serialization:
-	 * Intercept WLC_SET_KEY IOCTL - serialize M4 send and set key IOCTL to
-	 * prevent M4 encryption and
-	 * intercept WLC_DISASSOC IOCTL - serialize WPS-DONE and WLC_DISASSOC IOCTL to
-	 * prevent disassoc frame being sent before WPS-DONE frame.
-	 */
-	if (ioc.cmd == WLC_SET_KEY ||
-	    (ioc.cmd == WLC_SET_VAR && ioc.buf != NULL &&
-	     strncmp("wsec_key", ioc.buf, 9) == 0) ||
-	    (ioc.cmd == WLC_SET_VAR && ioc.buf != NULL &&
-	     strncmp("bsscfg:wsec_key", ioc.buf, 15) == 0) ||
-	    ioc.cmd == WLC_DISASSOC)
-		dhd_wait_pend8021x(net);
-
-#ifdef WLMEDIA_HTSF
-	if (ioc.buf) {
-		/*  short cut wl ioctl calls here  */
-		if (strcmp("htsf", ioc.buf) == 0) {
-			dhd_ioctl_htsf_get(dhd, 0);
-			return BCME_OK;
-		}
-
-		if (strcmp("htsflate", ioc.buf) == 0) {
-			if (ioc.set) {
-				memset(ts, 0, sizeof(tstamp_t)*TSMAX);
-				memset(&maxdelayts, 0, sizeof(tstamp_t));
-				maxdelay = 0;
-				tspktcnt = 0;
-				maxdelaypktno = 0;
-				memset(&vi_d1.bin, 0, sizeof(uint32)*NUMBIN);
-				memset(&vi_d2.bin, 0, sizeof(uint32)*NUMBIN);
-				memset(&vi_d3.bin, 0, sizeof(uint32)*NUMBIN);
-				memset(&vi_d4.bin, 0, sizeof(uint32)*NUMBIN);
-			} else {
-				dhd_dump_latency();
-			}
-			return BCME_OK;
-		}
-		if (strcmp("htsfclear", ioc.buf) == 0) {
-			memset(&vi_d1.bin, 0, sizeof(uint32)*NUMBIN);
-			memset(&vi_d2.bin, 0, sizeof(uint32)*NUMBIN);
-			memset(&vi_d3.bin, 0, sizeof(uint32)*NUMBIN);
-			memset(&vi_d4.bin, 0, sizeof(uint32)*NUMBIN);
-			htsf_seqnum = 0;
-			return BCME_OK;
-		}
-		if (strcmp("htsfhis", ioc.buf) == 0) {
-			dhd_dump_htsfhisto(&vi_d1, "H to D");
-			dhd_dump_htsfhisto(&vi_d2, "D to D");
-			dhd_dump_htsfhisto(&vi_d3, "D to H");
-			dhd_dump_htsfhisto(&vi_d4, "H to H");
-			return BCME_OK;
-		}
-		if (strcmp("tsport", ioc.buf) == 0) {
-			if (ioc.set) {
-				memcpy(&tsport, ioc.buf + 7, 4);
-			} else {
-				DHD_ERROR(("current timestamp port: %d \n", tsport));
-			}
-			return BCME_OK;
-		}
-	}
-#endif /* WLMEDIA_HTSF */
-
-	bcmerror = dhd_wl_ioctl(&dhd->pub, ifidx, (wl_ioctl_t *)&ioc, buf, buflen);
-
-done:
-	if ((bcmerror == -ETIMEDOUT) || ((dhd->pub.busstate == DHD_BUS_DOWN) &&
-		(!dhd->pub.dongle_reset))) {
-		DHD_ERROR(("%s: Event HANG send up\n", __FUNCTION__));
-		net_os_send_hang_message(net);
-	}
-
-	if (!bcmerror && buf && ioc.buf) {
-		if (copy_to_user(ioc.buf, buf, buflen))
-			bcmerror = -EFAULT;
-	}
-
-	if (buf)
-		MFREE(dhd->pub.osh, buf, buflen);
-
-	DHD_OS_WAKE_UNLOCK(&dhd->pub);
-
-	return OSL_ERROR(bcmerror);
-}
-
-static int
-dhd_stop(struct net_device *net)
-{
-	int ifidx;
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-	if (dhd->pub.up == 0) {
-		return 0;
-	}
-	ifidx = dhd_net2idx(dhd, net);
-
-#ifdef WL_CFG80211
-	if (ifidx == 0)
-		wl_cfg80211_down();
-#endif
-
-#ifdef PROP_TXSTATUS
-	dhd_wlfc_cleanup(&dhd->pub);
-#endif
-	/* Set state and stop OS transmissions */
-	dhd->pub.up = 0;
-	netif_stop_queue(net);
-
-	/* Stop the protocol module */
-	dhd_prot_stop(&dhd->pub);
-
-#if defined(WL_CFG80211)
-	if (ifidx == 0)
-		wl_android_wifi_off(net);
-#endif
-
-	OLD_MOD_DEC_USE_COUNT;
-	return 0;
-}
-
-static int
-dhd_open(struct net_device *net)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
-
-#ifdef TOE
-	uint32 toe_ol;
-#endif
-	int ifidx;
-	int32 ret = 0;
-
-	/* Update FW path if it was changed */
-	if ((firmware_path != NULL) && (firmware_path[0] != '\0')) {
-		if (firmware_path[strlen(firmware_path)-1] == '\n')
-			firmware_path[strlen(firmware_path)-1] = '\0';
-		strcpy(fw_path, firmware_path);
-		firmware_path[0] = '\0';
-	}
-#if !defined(WL_CFG80211)
-	/** Force start if ifconfig_up gets called before START command
-	 *  We keep WEXT's wl_control_wl_start to provide backward compatibility
-	 *  This should be removed in the future
-	 */
-	wl_control_wl_start(net);
-#endif
-
-	ifidx = dhd_net2idx(dhd, net);
-	DHD_TRACE(("%s: ifidx %d\n", __FUNCTION__, ifidx));
-
-	if (ifidx < 0) {
-		DHD_ERROR(("%s: Error: called with invalid IF\n", __FUNCTION__));
-		return -1;
-	}
-
-	if (!dhd->iflist[ifidx] || dhd->iflist[ifidx]->state == WLC_E_IF_DEL) {
-		DHD_ERROR(("%s: Error: called when IF already deleted\n", __FUNCTION__));
-		return -1;
-	}
-
-	if (ifidx == 0) {
-		atomic_set(&dhd->pend_8021x_cnt, 0);
-#if defined(WL_CFG80211)
-		wl_android_wifi_on(net);
-#endif
-
-		if (dhd->pub.busstate != DHD_BUS_DATA) {
-			int ret;
-
-			/* try to bring up bus */
-			if ((ret = dhd_bus_start(&dhd->pub)) != 0) {
-				DHD_ERROR(("%s: failed with code %d\n", __FUNCTION__, ret));
-				return -1;
-			}
-
-		}
-
-		/* dhd_prot_init has been called in dhd_bus_start or wl_android_wifi_on */
-		memcpy(net->dev_addr, dhd->pub.mac.octet, ETHER_ADDR_LEN);
-
-#ifdef TOE
-		/* Get current TOE mode from dongle */
-		if (dhd_toe_get(dhd, ifidx, &toe_ol) >= 0 && (toe_ol & TOE_TX_CSUM_OL) != 0)
-			dhd->iflist[ifidx]->net->features |= NETIF_F_IP_CSUM;
-		else
-			dhd->iflist[ifidx]->net->features &= ~NETIF_F_IP_CSUM;
-#endif /* TOE */
-
-#if defined(WL_CFG80211)
-		if (unlikely(wl_cfg80211_up())) {
-			DHD_ERROR(("%s: failed to bring up cfg80211\n", __FUNCTION__));
-			return -1;
-		}
-#endif /* WL_CFG80211 */
-	}
-
-	/* Allow transmit calls */
-	netif_start_queue(net);
-	dhd->pub.up = 1;
-
-#ifdef BCMDBGFS
-	dhd_dbg_init(&dhd->pub);
-#endif
-
-	OLD_MOD_INC_USE_COUNT;
-	return ret;
-}
-
-osl_t *
-dhd_osl_attach(void *pdev, uint bustype)
-{
-	return osl_attach(pdev, bustype, TRUE);
-}
-
-void
-dhd_osl_detach(osl_t *osh)
-{
-	if (MALLOCED(osh)) {
-		DHD_ERROR(("%s: MEMORY LEAK %d bytes\n", __FUNCTION__, MALLOCED(osh)));
-	}
-	osl_detach(osh);
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && 1
-	up(&dhd_registration_sem);
-#endif
-}
-
-int
-dhd_add_if(dhd_info_t *dhd, int ifidx, void *handle, char *name,
-	uint8 *mac_addr, uint32 flags, uint8 bssidx)
-{
-	dhd_if_t *ifp;
-
-	DHD_TRACE(("%s: idx %d, handle->%p\n", __FUNCTION__, ifidx, handle));
-
-	ASSERT(dhd && (ifidx < DHD_MAX_IFS));
-
-	ifp = dhd->iflist[ifidx];
-	if (ifp != NULL) {
-		if (ifp->net != NULL) {
-			netif_stop_queue(ifp->net);
-			unregister_netdev(ifp->net);
-			free_netdev(ifp->net);
-		}
-	} else
-		if ((ifp = MALLOC(dhd->pub.osh, sizeof(dhd_if_t))) == NULL) {
-			DHD_ERROR(("%s: OOM - dhd_if_t\n", __FUNCTION__));
-			return -ENOMEM;
-		}
-
-	memset(ifp, 0, sizeof(dhd_if_t));
-	ifp->info = dhd;
-	dhd->iflist[ifidx] = ifp;
-	strncpy(ifp->name, name, IFNAMSIZ);
-	ifp->name[IFNAMSIZ] = '\0';
-	if (mac_addr != NULL)
-		memcpy(&ifp->mac_addr, mac_addr, ETHER_ADDR_LEN);
-
-	if (handle == NULL) {
-		ifp->state = WLC_E_IF_ADD;
-		ifp->idx = ifidx;
-		ifp->bssidx = bssidx;
-		ASSERT(&dhd->thr_sysioc_ctl.thr_pid >= 0);
-		up(&dhd->thr_sysioc_ctl.sema);
-	} else
-		ifp->net = (struct net_device *)handle;
-
-	return 0;
-}
-
-void
-dhd_del_if(dhd_info_t *dhd, int ifidx)
-{
-	dhd_if_t *ifp;
-
-	DHD_TRACE(("%s: idx %d\n", __FUNCTION__, ifidx));
-
-	ASSERT(dhd && ifidx && (ifidx < DHD_MAX_IFS));
-	ifp = dhd->iflist[ifidx];
-	if (!ifp) {
-		DHD_ERROR(("%s: Null interface\n", __FUNCTION__));
-		return;
-	}
-
-	ifp->state = WLC_E_IF_DEL;
-	ifp->idx = ifidx;
-	ASSERT(&dhd->thr_sysioc_ctl.thr_pid >= 0);
-	up(&dhd->thr_sysioc_ctl.sema);
-}
-
-dhd_pub_t *
-dhd_attach(osl_t *osh, struct dhd_bus *bus, uint bus_hdrlen)
-{
-	dhd_info_t *dhd = NULL;
-	struct net_device *net = NULL;
-
-	dhd_attach_states_t dhd_state = DHD_ATTACH_STATE_INIT;
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	/* updates firmware nvram path if it was provided as module parameters */
-	if ((firmware_path != NULL) && (firmware_path[0] != '\0'))
-		strcpy(fw_path, firmware_path);
-	if ((nvram_path != NULL) && (nvram_path[0] != '\0'))
-		strcpy(nv_path, nvram_path);
-
-	/* Allocate etherdev, including space for private structure */
-	if (!(net = alloc_etherdev(sizeof(dhd)))) {
-		DHD_ERROR(("%s: OOM - alloc_etherdev\n", __FUNCTION__));
-		goto fail;
-	}
-	dhd_state |= DHD_ATTACH_STATE_NET_ALLOC;
-
-	/* Allocate primary dhd_info */
-	if (!(dhd = MALLOC(osh, sizeof(dhd_info_t)))) {
-		DHD_ERROR(("%s: OOM - alloc dhd_info\n", __FUNCTION__));
-		goto fail;
-	}
-	memset(dhd, 0, sizeof(dhd_info_t));
-
-#ifdef DHDTHREAD
-	dhd->thr_dpc_ctl.thr_pid = DHD_PID_KT_TL_INVALID;
-	dhd->thr_wdt_ctl.thr_pid = DHD_PID_KT_INVALID;
-#else
-	dhd->dhd_tasklet_create = FALSE;
-#endif /* DHDTHREAD */
-	dhd->thr_sysioc_ctl.thr_pid = DHD_PID_KT_INVALID;
-	dhd_state |= DHD_ATTACH_STATE_DHD_ALLOC;
-
-	/*
-	 * Save the dhd_info into the priv
-	 */
-	memcpy((void *)netdev_priv(net), &dhd, sizeof(dhd));
-	dhd->pub.osh = osh;
-
-	/* Link to info module */
-	dhd->pub.info = dhd;
-	/* Link to bus module */
-	dhd->pub.bus = bus;
-	dhd->pub.hdrlen = bus_hdrlen;
-
-	/* Set network interface name if it was provided as module parameter */
-	if (iface_name[0]) {
-		int len;
-		char ch;
-		strncpy(net->name, iface_name, IFNAMSIZ);
-		net->name[IFNAMSIZ - 1] = 0;
-		len = strlen(net->name);
-		ch = net->name[len - 1];
-		if ((ch > '9' || ch < '0') && (len < IFNAMSIZ - 2))
-			strcat(net->name, "%d");
-	}
-
-	if (dhd_add_if(dhd, 0, (void *)net, net->name, NULL, 0, 0) == DHD_BAD_IF)
-		goto fail;
-	dhd_state |= DHD_ATTACH_STATE_ADD_IF;
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31))
-	net->open = NULL;
-#else
-	net->netdev_ops = NULL;
-#endif
-
-	sema_init(&dhd->proto_sem, 1);
-
-#ifdef PROP_TXSTATUS
-	spin_lock_init(&dhd->wlfc_spinlock);
-	dhd->pub.wlfc_enabled = TRUE;
-#endif /* PROP_TXSTATUS */
-
-	/* Initialize other structure content */
-	init_waitqueue_head(&dhd->ioctl_resp_wait);
-	init_waitqueue_head(&dhd->ctrl_wait);
-
-	/* Initialize the spinlocks */
-	spin_lock_init(&dhd->sdlock);
-	spin_lock_init(&dhd->txqlock);
-	spin_lock_init(&dhd->dhd_lock);
-
-	/* Initialize Wakelock stuff */
-	spin_lock_init(&dhd->wakelock_spinlock);
-	dhd->wakelock_counter = 0;
-	dhd->wakelock_timeout_enable = 0;
-#ifdef CONFIG_HAS_WAKELOCK
-	wake_lock_init(&dhd->wl_wifi, WAKE_LOCK_SUSPEND, "wlan_wake");
-	wake_lock_init(&dhd->wl_rxwake, WAKE_LOCK_SUSPEND, "wlan_rx_wake");
-#endif
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
-	mutex_init(&dhd->dhd_net_if_mutex);
-#endif
-	dhd_state |= DHD_ATTACH_STATE_WAKELOCKS_INIT;
-
-	/* Attach and link in the protocol */
-	if (dhd_prot_attach(&dhd->pub) != 0) {
-		DHD_ERROR(("dhd_prot_attach failed\n"));
-		goto fail;
-	}
-	dhd_state |= DHD_ATTACH_STATE_PROT_ATTACH;
-
-#ifdef WL_CFG80211
-	/* Attach and link in the cfg80211 */
-	if (unlikely(wl_cfg80211_attach(net, &dhd->pub))) {
-		DHD_ERROR(("wl_cfg80211_attach failed\n"));
-		goto fail;
-	}
-
-	dhd_monitor_init(&dhd->pub);
-	dhd_state |= DHD_ATTACH_STATE_CFG80211;
-#endif
-#if defined(CONFIG_WIRELESS_EXT)
-	/* Attach and link in the iw */
-	if (!(dhd_state &  DHD_ATTACH_STATE_CFG80211)) {
-		if (wl_iw_attach(net, (void *)&dhd->pub) != 0) {
-			DHD_ERROR(("wl_iw_attach failed\n"));
-			goto fail;
-		}
-	dhd_state |= DHD_ATTACH_STATE_WL_ATTACH;
-	}
-#endif /* defined(CONFIG_WIRELESS_EXT) */
-
-
-	/* Set up the watchdog timer */
-	init_timer(&dhd->timer);
-	dhd->timer.data = (ulong)dhd;
-	dhd->timer.function = dhd_watchdog;
-
-#ifdef DHDTHREAD
-	/* Initialize thread based operation and lock */
-	sema_init(&dhd->sdsem, 1);
-	if ((dhd_watchdog_prio >= 0) && (dhd_dpc_prio >= 0)) {
-		dhd->threads_only = TRUE;
-	}
-	else {
-		dhd->threads_only = FALSE;
-	}
-
-	if (dhd_dpc_prio >= 0) {
-		/* Initialize watchdog thread */
-		PROC_START(dhd_watchdog_thread, dhd, &dhd->thr_wdt_ctl, 0);
-	} else {
-		dhd->thr_wdt_ctl.thr_pid = -1;
-	}
-
-	/* Set up the bottom half handler */
-	if (dhd_dpc_prio >= 0) {
-		/* Initialize DPC thread */
-		PROC_START(dhd_dpc_thread, dhd, &dhd->thr_dpc_ctl, 0);
-	} else {
-		/*  use tasklet for dpc */
-		tasklet_init(&dhd->tasklet, dhd_dpc, (ulong)dhd);
-		dhd->thr_dpc_ctl.thr_pid = -1;
-	}
-#else
-	/* Set up the bottom half handler */
-	tasklet_init(&dhd->tasklet, dhd_dpc, (ulong)dhd);
-	dhd->dhd_tasklet_create = TRUE;
-#endif /* DHDTHREAD */
-
-	if (dhd_sysioc) {
-		PROC_START(_dhd_sysioc_thread, dhd, &dhd->thr_sysioc_ctl, 0);
-	} else {
-		dhd->thr_sysioc_ctl.thr_pid = -1;
-	}
-	dhd_state |= DHD_ATTACH_STATE_THREADS_CREATED;
-
-	/*
-	 * Save the dhd_info into the priv
-	 */
-	memcpy(netdev_priv(net), &dhd, sizeof(dhd));
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) && 1
-	register_pm_notifier(&dhd_sleep_pm_notifier);
-#endif /*  (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
-
-#ifdef CONFIG_HAS_EARLYSUSPEND
-	dhd->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 20;
-	dhd->early_suspend.suspend = dhd_early_suspend;
-	dhd->early_suspend.resume = dhd_late_resume;
-	register_early_suspend(&dhd->early_suspend);
-	dhd_state |= DHD_ATTACH_STATE_EARLYSUSPEND_DONE;
-#endif
-
-#ifdef ARP_OFFLOAD_SUPPORT
-	register_inetaddr_notifier(&dhd_notifier);
-#endif /* ARP_OFFLOAD_SUPPORT */
-
-	dhd_state |= DHD_ATTACH_STATE_DONE;
-	dhd->dhd_state = dhd_state;
-	return &dhd->pub;
-
-fail:
-	if (dhd_state < DHD_ATTACH_STATE_DHD_ALLOC) {
-		if (net) free_netdev(net);
-	} else {
-		DHD_TRACE(("%s: Calling dhd_detach dhd_state 0x%x &dhd->pub %p\n",
-			__FUNCTION__, dhd_state, &dhd->pub));
-		dhd->dhd_state = dhd_state;
-		dhd_detach(&dhd->pub);
-		dhd_free(&dhd->pub);
-	}
-
-	return NULL;
-}
-
-int
-dhd_bus_start(dhd_pub_t *dhdp)
-{
-	int ret = -1;
-	dhd_info_t *dhd = (dhd_info_t*)dhdp->info;
-	unsigned long flags;
-#ifdef EMBEDDED_PLATFORM
-	char iovbuf[WL_EVENTING_MASK_LEN + 12];	/*  Room for "event_msgs" + '\0' + bitvec  */
-#endif /* EMBEDDED_PLATFORM */
-
-	ASSERT(dhd);
-
-	DHD_TRACE(("%s: \n", __FUNCTION__));
-
-	dhd_os_sdlock(dhdp);
-
-	/* try to download image and nvram to the dongle */
-	if  ((dhd->pub.busstate == DHD_BUS_DOWN) &&
-		(fw_path != NULL) && (fw_path[0] != '\0') &&
-		(nv_path != NULL) && (nv_path[0] != '\0')) {
-		/* wake lock moved to dhdsdio_download_firmware */
-		if (!(dhd_bus_download_firmware(dhd->pub.bus, dhd->pub.osh,
-		                                fw_path, nv_path))) {
-			DHD_ERROR(("%s: dhdsdio_probe_download failed. firmware = %s nvram = %s\n",
-			           __FUNCTION__, fw_path, nv_path));
-			dhd_os_sdunlock(dhdp);
-			return -1;
-		}
-	}
-	if (dhd->pub.busstate != DHD_BUS_LOAD) {
-		dhd_os_sdunlock(dhdp);
-		return -ENETDOWN;
-	}
-
-	/* Start the watchdog timer */
-	dhd->pub.tickcnt = 0;
-	dhd_os_wd_timer(&dhd->pub, dhd_watchdog_ms);
-
-	/* Bring up the bus */
-	if ((ret = dhd_bus_init(&dhd->pub, FALSE)) != 0) {
-		DHD_ERROR(("%s, dhd_bus_init failed %d\n", __FUNCTION__, ret));
-		dhd_os_sdunlock(dhdp);
-		return ret;
-	}
-#if defined(OOB_INTR_ONLY)
-	/* Host registration for OOB interrupt */
-	if (bcmsdh_register_oob_intr(dhdp)) {
-		/* deactivate timer and wait for the handler to finish */
-		flags = dhd_os_spin_lock(&dhd->pub);
-		dhd->wd_timer_valid = FALSE;
-		dhd_os_spin_unlock(&dhd->pub, flags);
-		del_timer_sync(&dhd->timer);
-
-		DHD_ERROR(("%s Host failed to register for OOB\n", __FUNCTION__));
-		dhd_os_sdunlock(dhdp);
-		return -ENODEV;
-	}
-
-	/* Enable oob at firmware */
-	dhd_enable_oob_intr(dhd->pub.bus, TRUE);
-#endif /* defined(OOB_INTR_ONLY) */
-
-	/* If bus is not ready, can't come up */
-	if (dhd->pub.busstate != DHD_BUS_DATA) {
-		flags = dhd_os_spin_lock(&dhd->pub);
-		dhd->wd_timer_valid = FALSE;
-		dhd_os_spin_unlock(&dhd->pub, flags);
-		del_timer_sync(&dhd->timer);
-		DHD_ERROR(("%s failed bus is not ready\n", __FUNCTION__));
-		dhd_os_sdunlock(dhdp);
-		return -ENODEV;
-	}
-
-	dhd_os_sdunlock(dhdp);
-
-#ifdef EMBEDDED_PLATFORM
-	bcm_mkiovar("event_msgs", dhdp->eventmask, WL_EVENTING_MASK_LEN, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhdp, WLC_GET_VAR, iovbuf, sizeof(iovbuf), FALSE, 0);
-	bcopy(iovbuf, dhdp->eventmask, WL_EVENTING_MASK_LEN);
-
-	setbit(dhdp->eventmask, WLC_E_SET_SSID);
-	setbit(dhdp->eventmask, WLC_E_PRUNE);
-	setbit(dhdp->eventmask, WLC_E_AUTH);
-	setbit(dhdp->eventmask, WLC_E_REASSOC);
-	setbit(dhdp->eventmask, WLC_E_REASSOC_IND);
-	setbit(dhdp->eventmask, WLC_E_DEAUTH_IND);
-	setbit(dhdp->eventmask, WLC_E_DISASSOC_IND);
-	setbit(dhdp->eventmask, WLC_E_DISASSOC);
-	setbit(dhdp->eventmask, WLC_E_JOIN);
-	setbit(dhdp->eventmask, WLC_E_ASSOC_IND);
-	setbit(dhdp->eventmask, WLC_E_PSK_SUP);
-	setbit(dhdp->eventmask, WLC_E_LINK);
-	setbit(dhdp->eventmask, WLC_E_NDIS_LINK);
-	setbit(dhdp->eventmask, WLC_E_MIC_ERROR);
-	setbit(dhdp->eventmask, WLC_E_PMKID_CACHE);
-	setbit(dhdp->eventmask, WLC_E_TXFAIL);
-	setbit(dhdp->eventmask, WLC_E_JOIN_START);
-	setbit(dhdp->eventmask, WLC_E_SCAN_COMPLETE);
-	setbit(dhdp->eventmask, WLC_E_ACTION_FRAME_RX);
-	setbit(dhdp->eventmask, WLC_E_ACTION_FRAME_COMPLETE);
-#if defined(WLP2P)
-	setbit(dhdp->eventmask, WLC_E_P2P_PROBREQ_MSG);
-#endif /* WLP2P */
-#ifdef PNO_SUPPORT
-	setbit(dhdp->eventmask, WLC_E_PFN_NET_FOUND);
-#endif /* PNO_SUPPORT */
-
-/* enable dongle roaming event */
-	setbit(dhdp->eventmask, WLC_E_ROAM);
-
-
-	dhdp->pktfilter_count = 4;
-	/* Setup filter to allow only unicast */
-	dhdp->pktfilter[0] = "100 0 0 0 0x01 0x00";
-	dhdp->pktfilter[1] = NULL;
-	dhdp->pktfilter[2] = NULL;
-	dhdp->pktfilter[3] = NULL;
-#endif /* EMBEDDED_PLATFORM */
-
-#ifdef READ_MACADDR
-	dhd_read_macaddr(dhd);
-#endif
-
-	/* Bus is ready, do any protocol initialization */
-	if ((ret = dhd_prot_init(&dhd->pub)) < 0)
-		return ret;
-
-#ifdef WRITE_MACADDR
-	dhd_write_macaddr(dhd->pub.mac.octet);
-#endif
-
-	return 0;
-}
-
-int
-dhd_iovar(dhd_pub_t *pub, int ifidx, char *name, char *cmd_buf, uint cmd_len, int set)
-{
-	char buf[strlen(name) + 1 + cmd_len];
-	int len = sizeof(buf);
-	wl_ioctl_t ioc;
-	int ret;
-
-	len = bcm_mkiovar(name, cmd_buf, cmd_len, buf, len);
-
-	memset(&ioc, 0, sizeof(ioc));
-
-	ioc.cmd = set? WLC_SET_VAR : WLC_GET_VAR;
-	ioc.buf = buf;
-	ioc.len = len;
-	ioc.set = TRUE;
-
-	ret = dhd_wl_ioctl(pub, ifidx, &ioc, ioc.buf, ioc.len);
-	if (!set && ret >= 0)
-		memcpy(cmd_buf, buf, cmd_len);
-
-	return ret;
-}
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 31))
-static struct net_device_ops dhd_ops_pri = {
-	.ndo_open = dhd_open,
-	.ndo_stop = dhd_stop,
-	.ndo_get_stats = dhd_get_stats,
-	.ndo_do_ioctl = dhd_ioctl_entry,
-	.ndo_start_xmit = dhd_start_xmit,
-	.ndo_set_mac_address = dhd_set_mac_address,
-	.ndo_set_multicast_list = dhd_set_multicast_list,
-};
-
-static struct net_device_ops dhd_ops_virt = {
-	.ndo_get_stats = dhd_get_stats,
-	.ndo_do_ioctl = dhd_ioctl_entry,
-	.ndo_start_xmit = dhd_start_xmit,
-	.ndo_set_mac_address = dhd_set_mac_address,
-	.ndo_set_multicast_list = dhd_set_multicast_list,
-};
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 31)) */
-
-int dhd_change_mtu(dhd_pub_t *dhdp, int new_mtu, int ifidx)
-{
-	struct dhd_info *dhd = dhdp->info;
-	struct net_device *dev = NULL;
-
-	ASSERT(dhd && dhd->iflist[ifidx]);
-	dev = dhd->iflist[ifidx]->net;
-	ASSERT(dev);
-
-	if (netif_running(dev)) {
-		DHD_ERROR(("%s: Must be down to change its MTU", dev->name));
-		return BCME_NOTDOWN;
-	}
-
-#define DHD_MIN_MTU 1500
-#define DHD_MAX_MTU 1752
-
-	if ((new_mtu < DHD_MIN_MTU) || (new_mtu > DHD_MAX_MTU)) {
-		DHD_ERROR(("%s: MTU size %d is invalid.\n", __FUNCTION__, new_mtu));
-		return BCME_BADARG;
-	}
-
-	dev->mtu = new_mtu;
-	return 0;
-}
-
-#ifdef ARP_OFFLOAD_SUPPORT
-/* add or remove AOE host ip(s) (up to 8 IPs on the interface)  */
-void aoe_update_host_ipv4_table(dhd_pub_t *dhd_pub, u32 ipa, bool add)
-{
-	u32 ipv4_buf[MAX_IPV4_ENTRIES]; /* temp save for AOE host_ip table */
-	int i;
-	int ret;
-
-	bzero(ipv4_buf, sizeof(ipv4_buf));
-
-	/* display what we've got */
-	ret = dhd_arp_get_arp_hostip_table(dhd_pub, ipv4_buf, sizeof(ipv4_buf));
-	DHD_ARPOE(("%s: hostip table read from Dongle:\n", __FUNCTION__));
-#ifdef AOE_DBG
-	dhd_print_buf(ipv4_buf, 32, 4); /* max 8 IPs 4b each */
-#endif
-	/* now we saved hoste_ip table, clr it in the dongle AOE */
-	dhd_aoe_hostip_clr(dhd_pub);
-
-	if (ret) {
-		DHD_ERROR(("%s failed\n", __FUNCTION__));
-		return;
-	}
-
-	for (i = 0; i < MAX_IPV4_ENTRIES; i++) {
-
-		if (add && (ipv4_buf[i] == 0)) {
-
-				ipv4_buf[i]	= ipa;
-				add = FALSE; /* added ipa to local table  */
-				DHD_ARPOE(("%s: Saved new IP in temp arp_hostip[%d]\n",
-				__FUNCTION__, i));
-
-		} else if (ipv4_buf[i] == ipa) {
-			ipv4_buf[i]	= 0;
-			DHD_ARPOE(("%s: removed IP:%x from temp table %d\n",
-				__FUNCTION__, ipa, i));
-		}
-
-		if (ipv4_buf[i] != 0) {
-			/* add back host_ip entries from our local cache */
-			dhd_arp_offload_add_ip(dhd_pub, ipv4_buf[i]);
-			DHD_ARPOE(("%s: added IP:%x to dongle arp_hostip[%d]\n\n",
-				__FUNCTION__, ipv4_buf[i], i));
-		}
-	}
-#ifdef AOE_DBG
-	/* see the resulting hostip table */
-	dhd_arp_get_arp_hostip_table(dhd_pub, ipv4_buf, sizeof(ipv4_buf));
-	DHD_ARPOE(("%s: read back arp_hostip table:\n", __FUNCTION__));
-	dhd_print_buf(ipv4_buf, 32, 4); /* max 8 IPs 4b each */
-#endif
-}
-
-static int dhd_device_event(struct notifier_block *this,
-	unsigned long event,
-	void *ptr)
-{
-	struct in_ifaddr *ifa = (struct in_ifaddr *)ptr;
-
-	dhd_info_t *dhd;
-	dhd_pub_t *dhd_pub;
-
-	if (!ifa)
-		return NOTIFY_DONE;
-
-	dhd = *(dhd_info_t **)netdev_priv(ifa->ifa_dev->dev);
-	dhd_pub = &dhd->pub;
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 31))
-	if (ifa->ifa_dev->dev->netdev_ops == &dhd_ops_pri) {
-#else
-	if (ifa->ifa_dev->dev) {
-#endif
-		switch (event) {
-		case NETDEV_UP:
-			DHD_ARPOE(("%s: [%s] Up IP: 0x%x\n",
-				__FUNCTION__, ifa->ifa_label, ifa->ifa_address));
-
-#ifdef AOE_IP_ALIAS_SUPPORT
-			if (ifa->ifa_label[strlen(ifa->ifa_label)-2] == 0x3a) {
-				DHD_ARPOE(("%s:add aliased IP to AOE hostip cache\n",
-					__FUNCTION__));
-				aoe_update_host_ipv4_table(dhd_pub, ifa->ifa_address, TRUE);
-			}
-			else
-				aoe_update_host_ipv4_table(dhd_pub, ifa->ifa_address, TRUE);
-#endif
-			break;
-
-		case NETDEV_DOWN:
-			DHD_ARPOE(("%s: [%s] Down IP: 0x%x\n",
-				__FUNCTION__, ifa->ifa_label, ifa->ifa_address));
-
-#ifdef AOE_IP_ALIAS_SUPPORT
-		if (!(ifa->ifa_label[strlen(ifa->ifa_label)-2] == 0x3a)) {
-				DHD_ARPOE(("%s: primary interface is down, AOE clr all\n",
-					__FUNCTION__));
-				dhd_aoe_hostip_clr(&dhd->pub);
-				dhd_aoe_arp_clr(&dhd->pub);
-		} else
-			aoe_update_host_ipv4_table(dhd_pub, ifa->ifa_address, FALSE);
-#else
-	dhd_aoe_hostip_clr(&dhd->pub);
-	dhd_aoe_arp_clr(&dhd->pub);
-#endif
-			break;
-
-		default:
-			DHD_ARPOE(("%s: do noting for [%s] Event: %lu\n",
-				__func__, ifa->ifa_label, event));
-			break;
-		}
-	}
-	return NOTIFY_DONE;
-}
-#endif /* ARP_OFFLOAD_SUPPORT */
-
-int
-dhd_net_attach(dhd_pub_t *dhdp, int ifidx)
-{
-	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
-	struct net_device *net = NULL;
-	int err = 0;
-	uint8 temp_addr[ETHER_ADDR_LEN] = { 0x00, 0x90, 0x4c, 0x11, 0x22, 0x33 };
-
-	DHD_TRACE(("%s: ifidx %d\n", __FUNCTION__, ifidx));
-
-	ASSERT(dhd && dhd->iflist[ifidx]);
-
-	net = dhd->iflist[ifidx]->net;
-	ASSERT(net);
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31))
-	ASSERT(!net->open);
-	net->get_stats = dhd_get_stats;
-	net->do_ioctl = dhd_ioctl_entry;
-	net->hard_start_xmit = dhd_start_xmit;
-	net->set_mac_address = dhd_set_mac_address;
-	net->set_multicast_list = dhd_set_multicast_list;
-	net->open = net->stop = NULL;
-#else
-	ASSERT(!net->netdev_ops);
-	net->netdev_ops = &dhd_ops_virt;
-#endif
-
-	/* Ok, link into the network layer... */
-	if (ifidx == 0) {
-		/*
-		 * device functions for the primary interface only
-		 */
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31))
-		net->open = dhd_open;
-		net->stop = dhd_stop;
-#else
-		net->netdev_ops = &dhd_ops_pri;
-#endif
-	} else {
-	/*
-	 * We have to use the primary MAC for virtual interfaces
-	 */
-		memcpy(temp_addr, dhd->iflist[ifidx]->mac_addr, ETHER_ADDR_LEN);
-		/*
-		 * Android sets the locally administered bit to indicate that this is a
-		 * portable hotspot.  This will not work in simultaneous AP/STA mode,
-		 * nor with P2P.  Need to set the Donlge's MAC address, and then use that.
-		 */
-		if (ifidx > 0) {
-			DHD_ERROR(("%s interface [%s]: set locally administered bit in MAC\n",
-			__func__, net->name));
-			temp_addr[0] |= 0x02;
-		}
-	}
-
-	net->hard_header_len = ETH_HLEN + dhd->pub.hdrlen;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)
-	net->ethtool_ops = &dhd_ethtool_ops;
-#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24) */
-
-#if defined(CONFIG_WIRELESS_EXT)
-#if WIRELESS_EXT < 19
-	net->get_wireless_stats = dhd_get_wireless_stats;
-#endif /* WIRELESS_EXT < 19 */
-#if WIRELESS_EXT > 12
-	net->wireless_handlers = (struct iw_handler_def *)&wl_iw_handler_def;
-#endif /* WIRELESS_EXT > 12 */
-#endif /* defined(CONFIG_WIRELESS_EXT) */
-
-	dhd->pub.rxsz = DBUS_RX_BUFFER_SIZE_DHD(net);
-
-	memcpy(net->dev_addr, temp_addr, ETHER_ADDR_LEN);
-
-	if ((err = register_netdev(net)) != 0) {
-		DHD_ERROR(("couldn't register the net device, err %d\n", err));
-		goto fail;
-	}
-	printf("Broadcom Dongle Host Driver: register interface [%s]"
-		" MAC: %.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
-		net->name,
-		net->dev_addr[0], net->dev_addr[1], net->dev_addr[2],
-		net->dev_addr[3], net->dev_addr[4], net->dev_addr[5]);
-
-#if defined(SOFTAP) && defined(CONFIG_WIRELESS_EXT) && !defined(WL_CFG80211)
-		wl_iw_iscan_set_scan_broadcast_prep(net, 1);
-#endif
-
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-	if (ifidx == 0) {
-		up(&dhd_registration_sem);
-	}
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
-	return 0;
-
-fail:
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31)
-	net->open = NULL;
-#else
-	net->netdev_ops = NULL;
-#endif
-	return err;
-}
-
-void
-dhd_bus_detach(dhd_pub_t *dhdp)
-{
-	dhd_info_t *dhd;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (dhdp) {
-		dhd = (dhd_info_t *)dhdp->info;
-		if (dhd) {
-
-			/** In case of Android cfg80211 driver, the bus is down in dhd_stop,
-			 *  calling stop again will cuase SD read/write errors.
-			 */
-			if (dhd->pub.busstate != DHD_BUS_DOWN) {
-				/* Stop the protocol module */
-				dhd_prot_stop(&dhd->pub);
-
-				/* Stop the bus module */
-				dhd_bus_stop(dhd->pub.bus, TRUE);
-			}
-
-#if defined(OOB_INTR_ONLY)
-			bcmsdh_unregister_oob_intr();
-#endif /* defined(OOB_INTR_ONLY) */
-		}
-	}
-}
-
-
-void dhd_detach(dhd_pub_t *dhdp)
-{
-	dhd_info_t *dhd;
-	unsigned long flags;
-	int timer_valid = FALSE;
-
-	if (!dhdp)
-		return;
-
-	dhd = (dhd_info_t *)dhdp->info;
-	if (!dhd)
-		return;
-
-	DHD_TRACE(("%s: Enter state 0x%x\n", __FUNCTION__, dhd->dhd_state));
-
-	if (!(dhd->dhd_state & DHD_ATTACH_STATE_DONE)) {
-		/* Give sufficient time for threads to start running in case
-		 * dhd_attach() has failed
-		 */
-		osl_delay(1000*100);
-	}
-
-#ifdef ARP_OFFLOAD_SUPPORT
-	unregister_inetaddr_notifier(&dhd_notifier);
-#endif /* ARP_OFFLOAD_SUPPORT */
-
-#if defined(CONFIG_HAS_EARLYSUSPEND)
-	if (dhd->dhd_state & DHD_ATTACH_STATE_EARLYSUSPEND_DONE) {
-		if (dhd->early_suspend.suspend)
-			unregister_early_suspend(&dhd->early_suspend);
-	}
-#endif /* defined(CONFIG_HAS_EARLYSUSPEND) */
-
-#if defined(CONFIG_WIRELESS_EXT)
-	if (dhd->dhd_state & DHD_ATTACH_STATE_WL_ATTACH) {
-		/* Detatch and unlink in the iw */
-		wl_iw_detach();
-	}
-#endif /* defined(CONFIG_WIRELESS_EXT) */
-
-	if (&dhd->thr_sysioc_ctl.thr_pid >= 0) {
-		PROC_STOP(&dhd->thr_sysioc_ctl);
-	}
-
-	/* delete all interfaces, start with virtual  */
-	if (dhd->dhd_state & DHD_ATTACH_STATE_ADD_IF) {
-		int i = 1;
-		dhd_if_t *ifp;
-
-		for (i = 1; i < DHD_MAX_IFS; i++)
-			if (dhd->iflist[i]) {
-				dhd->iflist[i]->state = WLC_E_IF_DEL;
-				dhd->iflist[i]->idx = i;
-				dhd_op_if(dhd->iflist[i]);
-			}
-
-		/*  delete primary interface 0 */
-		ifp = dhd->iflist[0];
-		ASSERT(ifp);
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31))
-		if (ifp->net->open)
-#else
-		if (ifp->net->netdev_ops == &dhd_ops_pri)
-#endif
-		{
-			if (ifp->net) {
-				unregister_netdev(ifp->net);
-				free_netdev(ifp->net);
-				ifp->net = NULL;
-			}
-			MFREE(dhd->pub.osh, ifp, sizeof(*ifp));
-			dhd->iflist[0] = NULL;
-		}
-	}
-
-	/* Clear the watchdog timer */
-	flags = dhd_os_spin_lock(&dhd->pub);
-	timer_valid = dhd->wd_timer_valid;
-	dhd->wd_timer_valid = FALSE;
-	dhd_os_spin_unlock(&dhd->pub, flags);
-	if (timer_valid)
-		del_timer_sync(&dhd->timer);
-
-	if (dhd->dhd_state & DHD_ATTACH_STATE_THREADS_CREATED) {
-#ifdef DHDTHREAD
-		if (dhd->thr_wdt_ctl.thr_pid >= 0) {
-			PROC_STOP(&dhd->thr_wdt_ctl);
-		}
-
-		if (dhd->thr_dpc_ctl.thr_pid >= 0) {
-			PROC_STOP(&dhd->thr_dpc_ctl);
-		}
-		else
-#endif /* DHDTHREAD */
-		tasklet_kill(&dhd->tasklet);
-	}
-	if (dhd->dhd_state & DHD_ATTACH_STATE_PROT_ATTACH) {
-		dhd_bus_detach(dhdp);
-
-		if (dhdp->prot)
-			dhd_prot_detach(dhdp);
-	}
-
-#ifdef WL_CFG80211
-	if (dhd->dhd_state & DHD_ATTACH_STATE_CFG80211) {
-		wl_cfg80211_detach();
-		dhd_monitor_uninit();
-	}
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
-	unregister_pm_notifier(&dhd_sleep_pm_notifier);
-#endif
-
-	if (dhd->dhd_state & DHD_ATTACH_STATE_WAKELOCKS_INIT) {
-#ifdef CONFIG_HAS_WAKELOCK
-		wake_lock_destroy(&dhd->wl_wifi);
-		wake_lock_destroy(&dhd->wl_rxwake);
-#endif
-	}
-
-}
-
-
-void
-dhd_free(dhd_pub_t *dhdp)
-{
-	dhd_info_t *dhd;
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (dhdp) {
-		dhd = (dhd_info_t *)dhdp->info;
-		if (dhd)
-			MFREE(dhd->pub.osh, dhd, sizeof(*dhd));
-	}
-}
-
-static void __exit
-dhd_module_cleanup(void)
-{
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	dhd_bus_unregister();
-
-#if defined(CONFIG_WIFI_CONTROL_FUNC)
-	wl_android_wifictrl_func_del();
-#endif /* CONFIG_WIFI_CONTROL_FUNC */
-	wl_android_exit();
-
-	/* Call customer gpio to turn off power with WL_REG_ON signal */
-	dhd_customer_gpio_wlan_ctrl(WLAN_POWER_OFF);
-}
-
-
-static int __init
-dhd_module_init(void)
-{
-	int error = 0;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	wl_android_init();
-
-#ifdef DHDTHREAD
-	/* Sanity check on the module parameters */
-	do {
-		/* Both watchdog and DPC as tasklets are ok */
-		if ((dhd_watchdog_prio < 0) && (dhd_dpc_prio < 0))
-			break;
-
-		/* If both watchdog and DPC are threads, TX must be deferred */
-		if ((dhd_watchdog_prio >= 0) && (dhd_dpc_prio >= 0) && dhd_deferred_tx)
-			break;
-
-		DHD_ERROR(("Invalid module parameters.\n"));
-		return -EINVAL;
-	} while (0);
-#endif /* DHDTHREAD */
-
-	/* Call customer gpio to turn on power with WL_REG_ON signal */
-	dhd_customer_gpio_wlan_ctrl(WLAN_POWER_ON);
-
-#if defined(CONFIG_WIFI_CONTROL_FUNC)
-	if (wl_android_wifictrl_func_add() < 0)
-		goto fail_1;
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-		sema_init(&dhd_registration_sem, 0);
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
-	error = dhd_bus_register();
-
-
-	if (!error)
-		printf("\n%s\n", dhd_version);
-	else {
-		DHD_ERROR(("%s: sdio_register_driver failed\n", __FUNCTION__));
-		goto fail_1;
-	}
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-		/*
-		 * Wait till MMC sdio_register_driver callback called and made driver attach.
-		 * It's needed to make sync up exit from dhd insmod  and
-		 * Kernel MMC sdio device callback registration
-		 */
-	if (down_timeout(&dhd_registration_sem,  msecs_to_jiffies(DHD_REGISTRATION_TIMEOUT)) != 0) {
-		error = -EINVAL;
-		DHD_ERROR(("%s: sdio_register_driver timeout\n", __FUNCTION__));
-		goto fail_2;
-		}
-#endif
-#if defined(WL_CFG80211)
-	error = wl_android_post_init();
-#endif
-
-	return error;
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && 1
-fail_2:
-	dhd_bus_unregister();
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
-fail_1:
-#if defined(CONFIG_WIFI_CONTROL_FUNC)
-	wl_android_wifictrl_func_del();
-#endif 
-
-	/* Call customer gpio to turn off power with WL_REG_ON signal */
-	dhd_customer_gpio_wlan_ctrl(WLAN_POWER_OFF);
-
-	return error;
-}
-
-late_initcall(dhd_module_init);
-module_exit(dhd_module_cleanup);
-
-/*
- * OS specific functions required to implement DHD driver in OS independent way
- */
-int
-dhd_os_proto_block(dhd_pub_t *pub)
-{
-	dhd_info_t * dhd = (dhd_info_t *)(pub->info);
-
-	if (dhd) {
-		down(&dhd->proto_sem);
-		return 1;
-	}
-
-	return 0;
-}
-
-int
-dhd_os_proto_unblock(dhd_pub_t *pub)
-{
-	dhd_info_t * dhd = (dhd_info_t *)(pub->info);
-
-	if (dhd) {
-		up(&dhd->proto_sem);
-		return 1;
-	}
-
-	return 0;
-}
-
-unsigned int
-dhd_os_get_ioctl_resp_timeout(void)
-{
-	return ((unsigned int)dhd_ioctl_timeout_msec);
-}
-
-void
-dhd_os_set_ioctl_resp_timeout(unsigned int timeout_msec)
-{
-	dhd_ioctl_timeout_msec = (int)timeout_msec;
-}
-
-int
-dhd_os_ioctl_resp_wait(dhd_pub_t *pub, uint *condition, bool *pending)
-{
-	dhd_info_t * dhd = (dhd_info_t *)(pub->info);
-	DECLARE_WAITQUEUE(wait, current);
-	int timeout = dhd_ioctl_timeout_msec;
-
-	/* Convert timeout in millsecond to jiffies */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-	timeout = msecs_to_jiffies(timeout);
-#else
-	timeout = timeout * HZ / 1000;
-#endif
-
-	/* Wait until control frame is available */
-	add_wait_queue(&dhd->ioctl_resp_wait, &wait);
-	set_current_state(TASK_INTERRUPTIBLE);
-
-	/* Memory barrier to support multi-processing
-	 * As the variable "condition", which points to dhd->rxlen (dhd_bus_rxctl[dhd_sdio.c])
-	 * Can be changed by another processor.
-	 */
-	smp_mb();
-	while (!(*condition) && (!signal_pending(current) && timeout)) {
-		timeout = schedule_timeout(timeout);
-		smp_mb();
-	}
-
-	if (signal_pending(current))
-		*pending = TRUE;
-
-	set_current_state(TASK_RUNNING);
-	remove_wait_queue(&dhd->ioctl_resp_wait, &wait);
-
-	return timeout;
-}
-
-int
-dhd_os_ioctl_resp_wake(dhd_pub_t *pub)
-{
-	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
-
-	if (waitqueue_active(&dhd->ioctl_resp_wait)) {
-		wake_up_interruptible(&dhd->ioctl_resp_wait);
-	}
-
-	return 0;
-}
-
-void
-dhd_os_wd_timer(void *bus, uint wdtick)
-{
-	dhd_pub_t *pub = bus;
-	dhd_info_t *dhd = (dhd_info_t *)pub->info;
-	unsigned long flags;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	flags = dhd_os_spin_lock(pub);
-
-	/* don't start the wd until fw is loaded */
-	if (pub->busstate == DHD_BUS_DOWN) {
-		dhd_os_spin_unlock(pub, flags);
-		return;
-	}
-
-	/* Totally stop the timer */
-	if (!wdtick && dhd->wd_timer_valid == TRUE) {
-		dhd->wd_timer_valid = FALSE;
-		dhd_os_spin_unlock(pub, flags);
-#ifdef DHDTHREAD
-		del_timer_sync(&dhd->timer);
-#else
-		del_timer(&dhd->timer);
-#endif /* DHDTHREAD */
-		return;
-	}
-
-	if (wdtick) {
-		dhd_watchdog_ms = (uint)wdtick;
-		/* Re arm the timer, at last watchdog period */
-		mod_timer(&dhd->timer, jiffies + dhd_watchdog_ms * HZ / 1000);
-		dhd->wd_timer_valid = TRUE;
-	}
-	dhd_os_spin_unlock(pub, flags);
-}
-
-void *
-dhd_os_open_image(char *filename)
-{
-	struct file *fp;
-
-	/* wl_cfg80211_request_fw(filename); */
-
-	fp = filp_open(filename, O_RDONLY, 0);
-	/*
-	 * 2.6.11 (FC4) supports filp_open() but later revs don't?
-	 * Alternative:
-	 * fp = open_namei(AT_FDCWD, filename, O_RD, 0);
-	 * ???
-	 */
-	 if (IS_ERR(fp))
-		 fp = NULL;
-
-	 return fp;
-}
-
-int
-dhd_os_get_image_block(char *buf, int len, void *image)
-{
-	struct file *fp = (struct file *)image;
-	int rdlen;
-
-	/* wl_cfg80211_read_fw(buf, len); */
-
-	if (!image)
-		return 0;
-
-	rdlen = kernel_read(fp, fp->f_pos, buf, len);
-	if (rdlen > 0)
-		fp->f_pos += rdlen;
-
-	return rdlen;
-}
-
-void
-dhd_os_close_image(void *image)
-{
-	/* wl_cfg80211_release_fw(); */
-
-	if (image)
-		filp_close((struct file *)image, NULL);
-}
-
-
-void
-dhd_os_sdlock(dhd_pub_t *pub)
-{
-	dhd_info_t *dhd;
-
-	dhd = (dhd_info_t *)(pub->info);
-
-#ifdef DHDTHREAD
-	if (dhd->threads_only)
-		down(&dhd->sdsem);
-	else
-#endif /* DHDTHREAD */
-	spin_lock_bh(&dhd->sdlock);
-}
-
-void
-dhd_os_sdunlock(dhd_pub_t *pub)
-{
-	dhd_info_t *dhd;
-
-	dhd = (dhd_info_t *)(pub->info);
-
-#ifdef DHDTHREAD
-	if (dhd->threads_only)
-		up(&dhd->sdsem);
-	else
-#endif /* DHDTHREAD */
-	spin_unlock_bh(&dhd->sdlock);
-}
-
-void
-dhd_os_sdlock_txq(dhd_pub_t *pub)
-{
-	dhd_info_t *dhd;
-
-	dhd = (dhd_info_t *)(pub->info);
-	spin_lock_bh(&dhd->txqlock);
-}
-
-void
-dhd_os_sdunlock_txq(dhd_pub_t *pub)
-{
-	dhd_info_t *dhd;
-
-	dhd = (dhd_info_t *)(pub->info);
-	spin_unlock_bh(&dhd->txqlock);
-}
-void
-dhd_os_sdlock_rxq(dhd_pub_t *pub)
-{
-}
-void
-dhd_os_sdunlock_rxq(dhd_pub_t *pub)
-{
-}
-
-void
-dhd_os_sdtxlock(dhd_pub_t *pub)
-{
-	dhd_os_sdlock(pub);
-}
-
-void
-dhd_os_sdtxunlock(dhd_pub_t *pub)
-{
-	dhd_os_sdunlock(pub);
-}
-
-#if defined(DHD_USE_STATIC_BUF)
-uint8* dhd_os_prealloc(void *osh, int section, uint size)
-{
-	return (uint8*)wl_android_prealloc(section, size);
-}
-
-void dhd_os_prefree(void *osh, void *addr, uint size)
-{
-}
-#endif /* defined(CONFIG_WIFI_CONTROL_FUNC) */
-
-#if defined(CONFIG_WIRELESS_EXT)
-struct iw_statistics *
-dhd_get_wireless_stats(struct net_device *dev)
-{
-	int res = 0;
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-
-	if (!dhd->pub.up) {
-		return NULL;
-	}
-
-	res = wl_iw_get_wireless_stats(dev, &dhd->iw.wstats);
-
-	if (res == 0)
-		return &dhd->iw.wstats;
-	else
-		return NULL;
-}
-#endif /* defined(CONFIG_WIRELESS_EXT) */
-
-static int
-dhd_wl_host_event(dhd_info_t *dhd, int *ifidx, void *pktdata,
-	wl_event_msg_t *event, void **data)
-{
-	int bcmerror = 0;
-	ASSERT(dhd != NULL);
-
-	bcmerror = wl_host_event(&dhd->pub, ifidx, pktdata, event, data);
-	if (bcmerror != BCME_OK)
-		return (bcmerror);
-
-#if defined(CONFIG_WIRELESS_EXT)
-	if (event->bsscfgidx == 0) {
-		/*
-		 * Wireless ext is on primary interface only
-		 */
-
-		ASSERT(dhd->iflist[*ifidx] != NULL);
-		ASSERT(dhd->iflist[*ifidx]->net != NULL);
-
-		if (dhd->iflist[*ifidx]->net) {
-			wl_iw_event(dhd->iflist[*ifidx]->net, event, *data);
-		}
-	}
-#endif /* defined(CONFIG_WIRELESS_EXT)  */
-
-#ifdef WL_CFG80211
-
-	if ((wl_cfg80211_is_progress_ifchange() ||
-		wl_cfg80211_is_progress_ifadd()) && (*ifidx != 0)) {
-		/*
-		 * If IF_ADD/CHANGE operation is going on,
-		 *  discard any event received on the virtual I/F
-		 */
-		return (BCME_OK);
-	}
-
-	ASSERT(dhd->iflist[*ifidx] != NULL);
-	ASSERT(dhd->iflist[*ifidx]->net != NULL);
-	if (dhd->iflist[*ifidx]->net) {
-		wl_cfg80211_event(dhd->iflist[*ifidx]->net, event, *data);
-	}
-#endif /* defined(WL_CFG80211) */
-
-	return (bcmerror);
-}
-
-/* send up locally generated event */
-void
-dhd_sendup_event(dhd_pub_t *dhdp, wl_event_msg_t *event, void *data)
-{
-	switch (ntoh32(event->event_type)) {
-	/* Send up locally generated AMP HCI Events */
-	case WLC_E_BTA_HCI_EVENT: {
-		struct sk_buff *p, *skb;
-		bcm_event_t *msg;
-		wl_event_msg_t *p_bcm_event;
-		char *ptr;
-		uint32 len;
-		uint32 pktlen;
-		dhd_if_t *ifp;
-		dhd_info_t *dhd;
-		uchar *eth;
-		int ifidx;
-
-		len = ntoh32(event->datalen);
-		pktlen = sizeof(bcm_event_t) + len + 2;
-		dhd = dhdp->info;
-		ifidx = dhd_ifname2idx(dhd, event->ifname);
-
-		if ((p = PKTGET(dhdp->osh, pktlen, FALSE))) {
-			ASSERT(ISALIGNED((uintptr)PKTDATA(dhdp->osh, p), sizeof(uint32)));
-
-			msg = (bcm_event_t *) PKTDATA(dhdp->osh, p);
-
-			bcopy(&dhdp->mac, &msg->eth.ether_dhost, ETHER_ADDR_LEN);
-			bcopy(&dhdp->mac, &msg->eth.ether_shost, ETHER_ADDR_LEN);
-			ETHER_TOGGLE_LOCALADDR(&msg->eth.ether_shost);
-
-			msg->eth.ether_type = hton16(ETHER_TYPE_BRCM);
-
-			/* BCM Vendor specific header... */
-			msg->bcm_hdr.subtype = hton16(BCMILCP_SUBTYPE_VENDOR_LONG);
-			msg->bcm_hdr.version = BCMILCP_BCM_SUBTYPEHDR_VERSION;
-			bcopy(BRCM_OUI, &msg->bcm_hdr.oui[0], DOT11_OUI_LEN);
-
-			/* vendor spec header length + pvt data length (private indication
-			 *  hdr + actual message itself)
-			 */
-			msg->bcm_hdr.length = hton16(BCMILCP_BCM_SUBTYPEHDR_MINLENGTH +
-				BCM_MSG_LEN + sizeof(wl_event_msg_t) + (uint16)len);
-			msg->bcm_hdr.usr_subtype = hton16(BCMILCP_BCM_SUBTYPE_EVENT);
-
-			PKTSETLEN(dhdp->osh, p, (sizeof(bcm_event_t) + len + 2));
-
-			/* copy  wl_event_msg_t into sk_buf */
-
-			/* pointer to wl_event_msg_t in sk_buf */
-			p_bcm_event = &msg->event;
-			bcopy(event, p_bcm_event, sizeof(wl_event_msg_t));
-
-			/* copy hci event into sk_buf */
-			bcopy(data, (p_bcm_event + 1), len);
-
-			msg->bcm_hdr.length  = hton16(sizeof(wl_event_msg_t) +
-				ntoh16(msg->bcm_hdr.length));
-			PKTSETLEN(dhdp->osh, p, (sizeof(bcm_event_t) + len + 2));
-
-			ptr = (char *)(msg + 1);
-			/* Last 2 bytes of the message are 0x00 0x00 to signal that there
-			 * are no ethertypes which are following this
-			 */
-			ptr[len+0] = 0x00;
-			ptr[len+1] = 0x00;
-
-			skb = PKTTONATIVE(dhdp->osh, p);
-			eth = skb->data;
-			len = skb->len;
-
-			ifp = dhd->iflist[ifidx];
-			if (ifp == NULL)
-			     ifp = dhd->iflist[0];
-
-			ASSERT(ifp);
-			skb->dev = ifp->net;
-			skb->protocol = eth_type_trans(skb, skb->dev);
-
-			skb->data = eth;
-			skb->len = len;
-
-			/* Strip header, count, deliver upward */
-			skb_pull(skb, ETH_HLEN);
-
-			/* Send the packet */
-			if (in_interrupt()) {
-				netif_rx(skb);
-			} else {
-				netif_rx_ni(skb);
-			}
-		}
-		else {
-			/* Could not allocate a sk_buf */
-			DHD_ERROR(("%s: unable to alloc sk_buf", __FUNCTION__));
-		}
-		break;
-	} /* case WLC_E_BTA_HCI_EVENT */
-
-	default:
-		break;
-	}
-}
-
-void dhd_wait_for_event(dhd_pub_t *dhd, bool *lockvar)
-{
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
-	struct dhd_info *dhdinfo =  dhd->info;
-	dhd_os_sdunlock(dhd);
-	wait_event_interruptible_timeout(dhdinfo->ctrl_wait, (*lockvar == FALSE), HZ * 2);
-	dhd_os_sdlock(dhd);
-#endif
-	return;
-}
-
-void dhd_wait_event_wakeup(dhd_pub_t *dhd)
-{
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
-	struct dhd_info *dhdinfo =  dhd->info;
-	if (waitqueue_active(&dhdinfo->ctrl_wait))
-		wake_up_interruptible(&dhdinfo->ctrl_wait);
-#endif
-	return;
-}
-
-int
-dhd_dev_reset(struct net_device *dev, uint8 flag)
-{
-	int ret;
-
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-
-	ret = dhd_bus_devreset(&dhd->pub, flag);
-	if (ret) {
-		DHD_ERROR(("%s: dhd_bus_devreset: %d\n", __FUNCTION__, ret));
-		return ret;
-	}
-
-	return ret;
-}
-
-int net_os_set_suspend_disable(struct net_device *dev, int val)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	int ret = 0;
-
-	if (dhd) {
-		ret = dhd->pub.suspend_disable_flag;
-		dhd->pub.suspend_disable_flag = val;
-	}
-	return ret;
-}
-
-int net_os_set_suspend(struct net_device *dev, int val)
-{
-	int ret = 0;
-#if defined(CONFIG_HAS_EARLYSUSPEND)
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-
-	if (dhd) {
-		ret = dhd_set_suspend(val, &dhd->pub);
-	}
-#endif /* defined(CONFIG_HAS_EARLYSUSPEND) */
-	return ret;
-}
-
-int net_os_set_dtim_skip(struct net_device *dev, int val)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-
-	if (dhd)
-		dhd->pub.dtim_skip = val;
-
-	return 0;
-}
-
-int net_os_rxfilter_add_remove(struct net_device *dev, int add_remove, int num)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	char *filterp = NULL;
-	int ret = 0;
-
-	if (!dhd || (num == DHD_UNICAST_FILTER_NUM))
-		return ret;
-	if (num >= dhd->pub.pktfilter_count)
-		return -EINVAL;
-	if (add_remove) {
-		switch (num) {
-		case DHD_BROADCAST_FILTER_NUM:
-			filterp = "101 0 0 0 0xFFFFFFFFFFFF 0xFFFFFFFFFFFF";
-			break;
-		case DHD_MULTICAST4_FILTER_NUM:
-			filterp = "102 0 0 0 0xFFFFFF 0x01005E";
-			break;
-		case DHD_MULTICAST6_FILTER_NUM:
-			filterp = "103 0 0 0 0xFFFF 0x3333";
-			break;
-		default:
-			return -EINVAL;
-		}
-	}
-	dhd->pub.pktfilter[num] = filterp;
-	return ret;
-}
-
-int net_os_set_packet_filter(struct net_device *dev, int val)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	int ret = 0;
-
-	/* Packet filtering is set only if we still in early-suspend and
-	 * we need either to turn it ON or turn it OFF
-	 * We can always turn it OFF in case of early-suspend, but we turn it
-	 * back ON only if suspend_disable_flag was not set
-	*/
-	if (dhd && dhd->pub.up) {
-		if (dhd->pub.in_suspend) {
-			if (!val || (val && !dhd->pub.suspend_disable_flag))
-				dhd_set_packet_filter(val, &dhd->pub);
-		}
-	}
-	return ret;
-}
-
-
-void
-dhd_dev_init_ioctl(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-
-	dhd_preinit_ioctls(&dhd->pub);
-}
-
-#ifdef PNO_SUPPORT
-/* Linux wrapper to call common dhd_pno_clean */
-int
-dhd_dev_pno_reset(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-
-	return (dhd_pno_clean(&dhd->pub));
-}
-
-
-/* Linux wrapper to call common dhd_pno_enable */
-int
-dhd_dev_pno_enable(struct net_device *dev,  int pfn_enabled)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-
-	return (dhd_pno_enable(&dhd->pub, pfn_enabled));
-}
-
-
-/* Linux wrapper to call common dhd_pno_set */
-int
-dhd_dev_pno_set(struct net_device *dev, wlc_ssid_t* ssids_local, int nssid,
-	ushort  scan_fr, int pno_repeat, int pno_freq_expo_max)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-
-	return (dhd_pno_set(&dhd->pub, ssids_local, nssid, scan_fr, pno_repeat, pno_freq_expo_max));
-}
-
-/* Linux wrapper to get  pno status */
-int
-dhd_dev_get_pno_status(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-
-	return (dhd_pno_get_status(&dhd->pub));
-}
-
-#endif /* PNO_SUPPORT */
-
-int net_os_send_hang_message(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	int ret = 0;
-
-	if (dhd) {
-		if (!dhd->hang_was_sent) {
-			dhd->hang_was_sent = 1;
-#if defined(CONFIG_WIRELESS_EXT)
-			ret = wl_iw_send_priv_event(dev, "HANG");
-#endif
-		}
-	}
-	return ret;
-}
-
-void dhd_bus_country_set(struct net_device *dev, wl_country_t *cspec)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-
-	if (dhd && dhd->pub.up)
-			memcpy(&dhd->pub.dhd_cspec, cspec, sizeof(wl_country_t));
-}
-
-
-void dhd_net_if_lock(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	dhd_net_if_lock_local(dhd);
-}
-
-void dhd_net_if_unlock(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	dhd_net_if_unlock_local(dhd);
-}
-
-static void dhd_net_if_lock_local(dhd_info_t *dhd)
-{
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
-	if (dhd)
-		mutex_lock(&dhd->dhd_net_if_mutex);
-#endif
-}
-
-static void dhd_net_if_unlock_local(dhd_info_t *dhd)
-{
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
-	if (dhd)
-		mutex_unlock(&dhd->dhd_net_if_mutex);
-#endif
-}
-
-unsigned long dhd_os_spin_lock(dhd_pub_t *pub)
-{
-	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
-	unsigned long flags = 0;
-
-	if (dhd)
-		spin_lock_irqsave(&dhd->dhd_lock, flags);
-
-	return flags;
-}
-
-void dhd_os_spin_unlock(dhd_pub_t *pub, unsigned long flags)
-{
-	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
-
-	if (dhd)
-		spin_unlock_irqrestore(&dhd->dhd_lock, flags);
-}
-
-static int
-dhd_get_pend_8021x_cnt(dhd_info_t *dhd)
-{
-	return (atomic_read(&dhd->pend_8021x_cnt));
-}
-
-#define MAX_WAIT_FOR_8021X_TX	10
-
-int
-dhd_wait_pend8021x(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	int timeout = 10 * HZ / 1000;
-	int ntimes = MAX_WAIT_FOR_8021X_TX;
-	int pend = dhd_get_pend_8021x_cnt(dhd);
-
-	while (ntimes && pend) {
-		if (pend) {
-			set_current_state(TASK_INTERRUPTIBLE);
-			schedule_timeout(timeout);
-			set_current_state(TASK_RUNNING);
-			ntimes--;
-		}
-		pend = dhd_get_pend_8021x_cnt(dhd);
-	}
-	return pend;
-}
-
-#ifdef DHD_DEBUG
-int
-write_to_file(dhd_pub_t *dhd, uint8 *buf, int size)
-{
-	int ret = 0;
-	struct file *fp;
-	mm_segment_t old_fs;
-	loff_t pos = 0;
-
-	/* change to KERNEL_DS address limit */
-	old_fs = get_fs();
-	set_fs(KERNEL_DS);
-
-	/* open file to write */
-	fp = filp_open("/tmp/mem_dump", O_WRONLY|O_CREAT, 0640);
-	if (!fp) {
-		printf("%s: open file error\n", __FUNCTION__);
-		ret = -1;
-		goto exit;
-	}
-
-	/* Write buf to file */
-	fp->f_op->write(fp, buf, size, &pos);
-
-exit:
-	/* free buf before return */
-	MFREE(dhd->osh, buf, size);
-	/* close file before return */
-	if (fp)
-		filp_close(fp, current->files);
-	/* restore previous address limit */
-	set_fs(old_fs);
-
-	return ret;
-}
-#endif /* DHD_DEBUG */
-
-int dhd_os_wake_lock_timeout(dhd_pub_t *pub)
-{
-	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
-	unsigned long flags;
-	int ret = 0;
-
-	if (dhd) {
-		spin_lock_irqsave(&dhd->wakelock_spinlock, flags);
-		ret = dhd->wakelock_timeout_enable;
-#ifdef CONFIG_HAS_WAKELOCK
-		if (dhd->wakelock_timeout_enable)
-			wake_lock_timeout(&dhd->wl_rxwake, HZ);
-#endif
-		dhd->wakelock_timeout_enable = 0;
-		spin_unlock_irqrestore(&dhd->wakelock_spinlock, flags);
-	}
-	return ret;
-}
-
-int net_os_wake_lock_timeout(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	int ret = 0;
-
-	if (dhd)
-		ret = dhd_os_wake_lock_timeout(&dhd->pub);
-	return ret;
-}
-
-int dhd_os_wake_lock_timeout_enable(dhd_pub_t *pub)
-{
-	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
-	unsigned long flags;
-
-	if (dhd) {
-		spin_lock_irqsave(&dhd->wakelock_spinlock, flags);
-		dhd->wakelock_timeout_enable = 1;
-		spin_unlock_irqrestore(&dhd->wakelock_spinlock, flags);
-	}
-	return 0;
-}
-
-int net_os_wake_lock_timeout_enable(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	int ret = 0;
-
-	if (dhd)
-		ret = dhd_os_wake_lock_timeout_enable(&dhd->pub);
-	return ret;
-}
-
-int dhd_os_wake_lock(dhd_pub_t *pub)
-{
-	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
-	unsigned long flags;
-	int ret = 0;
-
-	if (dhd) {
-		spin_lock_irqsave(&dhd->wakelock_spinlock, flags);
-#ifdef CONFIG_HAS_WAKELOCK
-		if (!dhd->wakelock_counter)
-			wake_lock(&dhd->wl_wifi);
-#endif
-		dhd->wakelock_counter++;
-		ret = dhd->wakelock_counter;
-		spin_unlock_irqrestore(&dhd->wakelock_spinlock, flags);
-	}
-	return ret;
-}
-
-int net_os_wake_lock(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	int ret = 0;
-
-	if (dhd)
-		ret = dhd_os_wake_lock(&dhd->pub);
-	return ret;
-}
-
-int dhd_os_wake_unlock(dhd_pub_t *pub)
-{
-	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
-	unsigned long flags;
-	int ret = 0;
-
-	dhd_os_wake_lock_timeout(pub);
-	if (dhd) {
-		spin_lock_irqsave(&dhd->wakelock_spinlock, flags);
-		if (dhd->wakelock_counter) {
-			dhd->wakelock_counter--;
-#ifdef CONFIG_HAS_WAKELOCK
-			if (!dhd->wakelock_counter)
-				wake_unlock(&dhd->wl_wifi);
-#endif
-			ret = dhd->wakelock_counter;
-		}
-		spin_unlock_irqrestore(&dhd->wakelock_spinlock, flags);
-	}
-	return ret;
-}
-
-int net_os_wake_unlock(struct net_device *dev)
-{
-	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
-	int ret = 0;
-
-	if (dhd)
-		ret = dhd_os_wake_unlock(&dhd->pub);
-	return ret;
-}
-
-int dhd_ioctl_entry_local(struct net_device *net, wl_ioctl_t *ioc, int cmd)
-{
-	int ifidx;
-	int ret = 0;
-	dhd_info_t *dhd = NULL;
-
-	if (!net || !netdev_priv(net)) {
-		DHD_ERROR(("%s invalid parameter\n", __FUNCTION__));
-		return -EINVAL;
-	}
-
-	dhd = *(dhd_info_t **)netdev_priv(net);
-	ifidx = dhd_net2idx(dhd, net);
-	if (ifidx == DHD_BAD_IF) {
-		DHD_ERROR(("%s bad ifidx\n", __FUNCTION__));
-		return -ENODEV;
-	}
-
-	DHD_OS_WAKE_LOCK(&dhd->pub);
-	ret = dhd_wl_ioctl(&dhd->pub, ifidx, ioc, ioc->buf, ioc->len);
-	DHD_OS_WAKE_UNLOCK(&dhd->pub);
-
-	return ret;
-}
-
-#ifdef PROP_TXSTATUS
-extern int dhd_wlfc_interface_entry_update(void* state,	ewlfc_mac_entry_action_t action, uint8 ifid,
-	uint8 iftype, uint8* ea);
-extern int dhd_wlfc_FIFOcreditmap_update(void* state, uint8* credits);
-
-int dhd_wlfc_interface_event(struct dhd_info *dhd, uint8 action, uint8 ifid, uint8 iftype,
-	uint8* ea)
-{
-	if (dhd->pub.wlfc_state == NULL)
-		return BCME_OK;
-
-	return dhd_wlfc_interface_entry_update(dhd->pub.wlfc_state, action, ifid, iftype, ea);
-}
-
-int dhd_wlfc_FIFOcreditmap_event(struct dhd_info *dhd, uint8* event_data)
-{
-	if (dhd->pub.wlfc_state == NULL)
-		return BCME_OK;
-
-	return dhd_wlfc_FIFOcreditmap_update(dhd->pub.wlfc_state, event_data);
-}
-
-int dhd_wlfc_event(struct dhd_info *dhd)
-{
-	return dhd_wlfc_enable(&dhd->pub);
-}
-#endif /* PROP_TXSTATUS */
-
-#ifdef BCMDBGFS
-
-#include <linux/debugfs.h>
-
-extern uint32 dhd_readregl(void *bp, uint32 addr);
-extern uint32 dhd_writeregl(void *bp, uint32 addr, uint32 data);
-
-typedef struct dhd_dbgfs {
-	struct dentry	*debugfs_dir;
-	struct dentry	*debugfs_mem;
-	dhd_pub_t 	*dhdp;
-	uint32 		size;
-} dhd_dbgfs_t;
-
-dhd_dbgfs_t g_dbgfs;
-
-static int
-dhd_dbg_state_open(struct inode *inode, struct file *file)
-{
-	file->private_data = inode->i_private;
-	return 0;
-}
-
-static ssize_t
-dhd_dbg_state_read(struct file *file, char __user *ubuf,
-                       size_t count, loff_t *ppos)
-{
-	ssize_t rval;
-	uint32 tmp;
-	loff_t pos = *ppos;
-	size_t ret;
-
-	if (pos < 0)
-		return -EINVAL;
-	if (pos >= g_dbgfs.size || !count)
-		return 0;
-	if (count > g_dbgfs.size - pos)
-		count = g_dbgfs.size - pos;
-
-	/* Basically enforce aligned 4 byte reads. It's up to the user to work out the details */
-	tmp = dhd_readregl(g_dbgfs.dhdp->bus, file->f_pos & (~3));
-
-	ret = copy_to_user(ubuf, &tmp, 4);
-	if (ret == count)
-		return -EFAULT;
-
-	count -= ret;
-	*ppos = pos + count;
-	rval = count;
-
-	return rval;
-}
-
-
-static ssize_t
-dhd_debugfs_write(struct file *file, const char __user *ubuf, size_t count, loff_t *ppos)
-{
-	loff_t pos = *ppos;
-	size_t ret;
-	uint32 buf;
-
-	if (pos < 0)
-		return -EINVAL;
-	if (pos >= g_dbgfs.size || !count)
-		return 0;
-	if (count > g_dbgfs.size - pos)
-		count = g_dbgfs.size - pos;
-
-	ret = copy_from_user(&buf, ubuf, sizeof(uint32));
-	if (ret == count)
-		return -EFAULT;
-
-	/* Basically enforce aligned 4 byte writes. It's up to the user to work out the details */
-	dhd_writeregl(g_dbgfs.dhdp->bus, file->f_pos & (~3), buf);
-
-	return count;
-}
-
-
-loff_t
-dhd_debugfs_lseek(struct file *file, loff_t off, int whence)
-{
-	loff_t pos = -1;
-
-	switch (whence) {
-		case 0:
-			pos = off;
-			break;
-		case 1:
-			pos = file->f_pos + off;
-			break;
-		case 2:
-			pos = g_dbgfs.size - off;
-	}
-	return (pos < 0 || pos > g_dbgfs.size) ? -EINVAL : (file->f_pos = pos);
-}
-
-static const struct file_operations dhd_dbg_state_ops = {
-	.read   = dhd_dbg_state_read,
-	.write	= dhd_debugfs_write,
-	.open   = dhd_dbg_state_open,
-	.llseek	= dhd_debugfs_lseek
-};
-
-static void dhd_dbg_create(void)
-{
-	if (g_dbgfs.debugfs_dir) {
-		g_dbgfs.debugfs_mem = debugfs_create_file("mem", 0644, g_dbgfs.debugfs_dir,
-			NULL, &dhd_dbg_state_ops);
-	}
-}
-
-void dhd_dbg_init(dhd_pub_t *dhdp)
-{
-	int err;
-
-	g_dbgfs.dhdp = dhdp;
-	g_dbgfs.size = 0x20000000; /* Allow access to various cores regs */
-
-	g_dbgfs.debugfs_dir = debugfs_create_dir("dhd", 0);
-	if (IS_ERR(g_dbgfs.debugfs_dir)) {
-		err = PTR_ERR(g_dbgfs.debugfs_dir);
-		g_dbgfs.debugfs_dir = NULL;
-		return;
-	}
-
-	dhd_dbg_create();
-
-	return;
-}
-
-void dhd_dbg_remove(void)
-{
-	debugfs_remove(g_dbgfs.debugfs_mem);
-	debugfs_remove(g_dbgfs.debugfs_dir);
-
-	bzero((unsigned char *) &g_dbgfs, sizeof(g_dbgfs));
-
-}
-#endif /* ifdef BCMDBGFS */
-
-#ifdef WLMEDIA_HTSF
-
-static
-void dhd_htsf_addtxts(dhd_pub_t *dhdp, void *pktbuf)
-{
-	dhd_info_t *dhd = (dhd_info_t *)(dhdp->info);
-	struct sk_buff *skb;
-	uint32 htsf = 0;
-	uint16 dport = 0, oldmagic = 0xACAC;
-	char *p1;
-	htsfts_t ts;
-
-	/*  timestamp packet  */
-
-	p1 = (char*) PKTDATA(dhdp->osh, pktbuf);
-
-	if (PKTLEN(dhdp->osh, pktbuf) > HTSF_MINLEN) {
-/*		memcpy(&proto, p1+26, 4);  	*/
-		memcpy(&dport, p1+40, 2);
-/* 	proto = ((ntoh32(proto))>> 16) & 0xFF;  */
-		dport = ntoh16(dport);
-	}
-
-	/* timestamp only if  icmp or udb iperf with port 5555 */
-/*	if (proto == 17 && dport == tsport) { */
-	if (dport >= tsport && dport <= tsport + 20) {
-
-		skb = (struct sk_buff *) pktbuf;
-
-		htsf = dhd_get_htsf(dhd, 0);
-		memset(skb->data + 44, 0, 2); /* clear checksum */
-		memcpy(skb->data+82, &oldmagic, 2);
-		memcpy(skb->data+84, &htsf, 4);
-
-		memset(&ts, 0, sizeof(htsfts_t));
-		ts.magic  = HTSFMAGIC;
-		ts.prio   = PKTPRIO(pktbuf);
-		ts.seqnum = htsf_seqnum++;
-		ts.c10    = get_cycles();
-		ts.t10    = htsf;
-		ts.endmagic = HTSFENDMAGIC;
-
-		memcpy(skb->data + HTSF_HOSTOFFSET, &ts, sizeof(ts));
-	}
-}
-
-static void dhd_dump_htsfhisto(histo_t *his, char *s)
-{
-	int pktcnt = 0, curval = 0, i;
-	for (i = 0; i < (NUMBIN-2); i++) {
-		curval += 500;
-		printf("%d ",  his->bin[i]);
-		pktcnt += his->bin[i];
-	}
-	printf(" max: %d TotPkt: %d neg: %d [%s]\n", his->bin[NUMBIN-2], pktcnt,
-		his->bin[NUMBIN-1], s);
-}
-
-static
-void sorttobin(int value, histo_t *histo)
-{
-	int i, binval = 0;
-
-	if (value < 0) {
-		histo->bin[NUMBIN-1]++;
-		return;
-	}
-	if (value > histo->bin[NUMBIN-2])  /* store the max value  */
-		histo->bin[NUMBIN-2] = value;
-
-	for (i = 0; i < (NUMBIN-2); i++) {
-		binval += 500; /* 500m s bins */
-		if (value <= binval) {
-			histo->bin[i]++;
-			return;
-		}
-	}
-	histo->bin[NUMBIN-3]++;
-}
-
-static
-void dhd_htsf_addrxts(dhd_pub_t *dhdp, void *pktbuf)
-{
-	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
-	struct sk_buff *skb;
-	char *p1;
-	uint16 old_magic;
-	int d1, d2, d3, end2end;
-	htsfts_t *htsf_ts;
-	uint32 htsf;
-
-	skb = PKTTONATIVE(dhdp->osh, pktbuf);
-	p1 = (char*)PKTDATA(dhdp->osh, pktbuf);
-
-	if (PKTLEN(osh, pktbuf) > HTSF_MINLEN) {
-		memcpy(&old_magic, p1+78, 2);
-		htsf_ts = (htsfts_t*) (p1 + HTSF_HOSTOFFSET - 4);
-	}
-	else
-		return;
-
-	if (htsf_ts->magic == HTSFMAGIC) {
-		htsf_ts->tE0 = dhd_get_htsf(dhd, 0);
-		htsf_ts->cE0 = get_cycles();
-	}
-
-	if (old_magic == 0xACAC) {
-
-		tspktcnt++;
-		htsf = dhd_get_htsf(dhd, 0);
-		memcpy(skb->data+92, &htsf, sizeof(uint32));
-
-		memcpy(&ts[tsidx].t1, skb->data+80, 16);
-
-		d1 = ts[tsidx].t2 - ts[tsidx].t1;
-		d2 = ts[tsidx].t3 - ts[tsidx].t2;
-		d3 = ts[tsidx].t4 - ts[tsidx].t3;
-		end2end = ts[tsidx].t4 - ts[tsidx].t1;
-
-		sorttobin(d1, &vi_d1);
-		sorttobin(d2, &vi_d2);
-		sorttobin(d3, &vi_d3);
-		sorttobin(end2end, &vi_d4);
-
-		if (end2end > 0 && end2end >  maxdelay) {
-			maxdelay = end2end;
-			maxdelaypktno = tspktcnt;
-			memcpy(&maxdelayts, &ts[tsidx], 16);
-		}
-		if (++tsidx >= TSMAX)
-			tsidx = 0;
-	}
-}
-
-uint32 dhd_get_htsf(dhd_info_t *dhd, int ifidx)
-{
-	uint32 htsf = 0, cur_cycle, delta, delta_us;
-	uint32    factor, baseval, baseval2;
-	cycles_t t;
-
-	t = get_cycles();
-	cur_cycle = t;
-
-	if (cur_cycle >  dhd->htsf.last_cycle)
-		delta = cur_cycle -  dhd->htsf.last_cycle;
-	else {
-		delta = cur_cycle + (0xFFFFFFFF -  dhd->htsf.last_cycle);
-	}
-
-	delta = delta >> 4;
-
-	if (dhd->htsf.coef) {
-		/* times ten to get the first digit */
-	        factor = (dhd->htsf.coef*10 + dhd->htsf.coefdec1);
-		baseval  = (delta*10)/factor;
-		baseval2 = (delta*10)/(factor+1);
-		delta_us  = (baseval -  (((baseval - baseval2) * dhd->htsf.coefdec2)) / 10);
-		htsf = (delta_us << 4) +  dhd->htsf.last_tsf + HTSF_BUS_DELAY;
-	}
-	else {
-		DHD_ERROR(("-------dhd->htsf.coef = 0 -------\n"));
-	}
-
-	return htsf;
-}
-
-static void dhd_dump_latency(void)
-{
-	int i, max = 0;
-	int d1, d2, d3, d4, d5;
-
-	printf("T1       T2       T3       T4           d1  d2   t4-t1     i    \n");
-	for (i = 0; i < TSMAX; i++) {
-		d1 = ts[i].t2 - ts[i].t1;
-		d2 = ts[i].t3 - ts[i].t2;
-		d3 = ts[i].t4 - ts[i].t3;
-		d4 = ts[i].t4 - ts[i].t1;
-		d5 = ts[max].t4-ts[max].t1;
-		if (d4 > d5 && d4 > 0)  {
-			max = i;
-		}
-		printf("%08X %08X %08X %08X \t%d %d %d   %d i=%d\n",
-			ts[i].t1, ts[i].t2, ts[i].t3, ts[i].t4,
-			d1, d2, d3, d4, i);
-	}
-
-	printf("current idx = %d \n", tsidx);
-
-	printf("Highest latency %d pkt no.%d total=%d\n", maxdelay, maxdelaypktno, tspktcnt);
-	printf("%08X %08X %08X %08X \t%d %d %d   %d\n",
-	maxdelayts.t1, maxdelayts.t2, maxdelayts.t3, maxdelayts.t4,
-	maxdelayts.t2 - maxdelayts.t1,
-	maxdelayts.t3 - maxdelayts.t2,
-	maxdelayts.t4 - maxdelayts.t3,
-	maxdelayts.t4 - maxdelayts.t1);
-}
-
-
-static int
-dhd_ioctl_htsf_get(dhd_info_t *dhd, int ifidx)
-{
-	wl_ioctl_t ioc;
-	char buf[32];
-	int ret;
-	uint32 s1, s2;
-
-	struct tsf {
-		uint32 low;
-		uint32 high;
-	} tsf_buf;
-
-	memset(&ioc, 0, sizeof(ioc));
-	memset(&tsf_buf, 0, sizeof(tsf_buf));
-
-	ioc.cmd = WLC_GET_VAR;
-	ioc.buf = buf;
-	ioc.len = (uint)sizeof(buf);
-	ioc.set = FALSE;
-
-	strcpy(buf, "tsf");
-	s1 = dhd_get_htsf(dhd, 0);
-	if ((ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len)) < 0) {
-		if (ret == -EIO) {
-			DHD_ERROR(("%s: tsf is not supported by device\n",
-				dhd_ifname(&dhd->pub, ifidx)));
-			return -EOPNOTSUPP;
-		}
-		return ret;
-	}
-	s2 = dhd_get_htsf(dhd, 0);
-
-	memcpy(&tsf_buf, buf, sizeof(tsf_buf));
-	printf(" TSF_h=%04X lo=%08X Calc:htsf=%08X, coef=%d.%d%d delta=%d ",
-		tsf_buf.high, tsf_buf.low, s2, dhd->htsf.coef, dhd->htsf.coefdec1,
-		dhd->htsf.coefdec2, s2-tsf_buf.low);
-	printf("lasttsf=%08X lastcycle=%08X\n", dhd->htsf.last_tsf, dhd->htsf.last_cycle);
-	return 0;
-}
-
-void htsf_update(dhd_info_t *dhd, void *data)
-{
-	static ulong  cur_cycle = 0, prev_cycle = 0;
-	uint32 htsf, tsf_delta = 0;
-	uint32 hfactor = 0, cyc_delta, dec1 = 0, dec2, dec3, tmp;
-	ulong b, a;
-	cycles_t t;
-
-	/* cycles_t in inlcude/mips/timex.h */
-
-	t = get_cycles();
-
-	prev_cycle = cur_cycle;
-	cur_cycle = t;
-
-	if (cur_cycle > prev_cycle)
-		cyc_delta = cur_cycle - prev_cycle;
-	else {
-		b = cur_cycle;
-		a = prev_cycle;
-		cyc_delta = cur_cycle + (0xFFFFFFFF - prev_cycle);
-	}
-
-	if (data == NULL)
-		printf(" tsf update ata point er is null \n");
-
-	memcpy(&prev_tsf, &cur_tsf, sizeof(tsf_t));
-	memcpy(&cur_tsf, data, sizeof(tsf_t));
-
-	if (cur_tsf.low == 0) {
-		DHD_INFO((" ---- 0 TSF, do not update, return\n"));
-		return;
-	}
-
-	if (cur_tsf.low > prev_tsf.low)
-		tsf_delta = (cur_tsf.low - prev_tsf.low);
-	else {
-		DHD_INFO((" ---- tsf low is smaller cur_tsf= %08X, prev_tsf=%08X, \n",
-		 cur_tsf.low, prev_tsf.low));
-		if (cur_tsf.high > prev_tsf.high) {
-			tsf_delta = cur_tsf.low + (0xFFFFFFFF - prev_tsf.low);
-			DHD_INFO((" ---- Wrap around tsf coutner  adjusted TSF=%08X\n", tsf_delta));
-		}
-		else
-			return; /* do not update */
-	}
-
-	if (tsf_delta)  {
-		hfactor = cyc_delta / tsf_delta;
-		tmp  = 	(cyc_delta - (hfactor * tsf_delta))*10;
-		dec1 =  tmp/tsf_delta;
-		dec2 =  ((tmp - dec1*tsf_delta)*10) / tsf_delta;
-		tmp  = 	(tmp   - (dec1*tsf_delta))*10;
-		dec3 =  ((tmp - dec2*tsf_delta)*10) / tsf_delta;
-
-		if (dec3 > 4) {
-			if (dec2 == 9) {
-				dec2 = 0;
-				if (dec1 == 9) {
-					dec1 = 0;
-					hfactor++;
-				}
-				else {
-					dec1++;
-				}
-			}
-			else
-				dec2++;
-		}
-	}
-
-	if (hfactor) {
-		htsf = ((cyc_delta * 10)  / (hfactor*10+dec1)) + prev_tsf.low;
-		dhd->htsf.coef = hfactor;
-		dhd->htsf.last_cycle = cur_cycle;
-		dhd->htsf.last_tsf = cur_tsf.low;
-		dhd->htsf.coefdec1 = dec1;
-		dhd->htsf.coefdec2 = dec2;
-	}
-	else {
-		htsf = prev_tsf.low;
-	}
-}
-
-#endif /* WLMEDIA_HTSF */
diff --git a/drivers/net/wireless/bcmdhd/dhd_linux_mon.c b/drivers/net/wireless/bcmdhd/dhd_linux_mon.c
deleted file mode 100644
index dd9c71f75be..00000000000
--- a/drivers/net/wireless/bcmdhd/dhd_linux_mon.c
+++ /dev/null
@@ -1,393 +0,0 @@
-/*
- * Broadcom Dongle Host Driver (DHD), Linux monitor network interface
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- *
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- *
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- *
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: dhd_linux_mon.c,v 1.131.2.55 2011-02-09 05:31:56 Exp $
- */
-
-#include <linux/string.h>
-#include <linux/module.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/if_arp.h>
-#include <linux/ieee80211.h>
-#include <linux/rtnetlink.h>
-#include <net/ieee80211_radiotap.h>
-
-#include <wlioctl.h>
-#include <bcmutils.h>
-#include <linux_osl.h>
-#include <dhd_dbg.h>
-#include <dngl_stats.h>
-#include <dhd.h>
-
-typedef enum monitor_states
-{
-	MONITOR_STATE_DEINIT = 0x0,
-	MONITOR_STATE_INIT = 0x1,
-	MONITOR_STATE_INTERFACE_ADDED = 0x2,
-	MONITOR_STATE_INTERFACE_DELETED = 0x4
-} monitor_states_t;
-extern int dhd_start_xmit(struct sk_buff *skb, struct net_device *net);
-
-/**
- * Local declarations and defintions (not exposed)
- */
-#define MON_PRINT(format, ...) printk("DHD-MON: %s " format, __func__, ##__VA_ARGS__)
-#define MON_TRACE MON_PRINT
-
-typedef struct monitor_interface {
-	int radiotap_enabled;
-	struct net_device* real_ndev;	/* The real interface that the monitor is on */
-	struct net_device* mon_ndev;
-} monitor_interface;
-
-typedef struct dhd_linux_monitor {
-	void *dhd_pub;
-	monitor_states_t monitor_state;
-	monitor_interface mon_if[DHD_MAX_IFS];
-	struct mutex lock;		/* lock to protect mon_if */
-} dhd_linux_monitor_t;
-
-static dhd_linux_monitor_t g_monitor;
-
-static struct net_device* lookup_real_netdev(char *name);
-static monitor_interface* ndev_to_monif(struct net_device *ndev);
-static int dhd_mon_if_open(struct net_device *ndev);
-static int dhd_mon_if_stop(struct net_device *ndev);
-static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev);
-static void dhd_mon_if_set_multicast_list(struct net_device *ndev);
-static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr);
-
-static const struct net_device_ops dhd_mon_if_ops = {
-	.ndo_open		= dhd_mon_if_open,
-	.ndo_stop		= dhd_mon_if_stop,
-	.ndo_start_xmit		= dhd_mon_if_subif_start_xmit,
-	.ndo_set_multicast_list = dhd_mon_if_set_multicast_list,
-	.ndo_set_mac_address 	= dhd_mon_if_change_mac,
-};
-
-/**
- * Local static function defintions
- */
-
-/* Look up dhd's net device table to find a match (e.g. interface "eth0" is a match for "mon.eth0"
- * "p2p-eth0-0" is a match for "mon.p2p-eth0-0")
- */
-static struct net_device* lookup_real_netdev(char *name)
-{
-	int i;
-	int last_name_len = 0;
-	struct net_device *ndev;
-	struct net_device *ndev_found = NULL;
-
-	/* We want to find interface "p2p-eth0-0" for monitor interface "mon.p2p-eth0-0", so
-	 * we skip "eth0" even if "mon.p2p-eth0-0" contains "eth0"
-	 */
-	for (i = 0; i < DHD_MAX_IFS; i++) {
-		ndev = dhd_idx2net(g_monitor.dhd_pub, i);
-		if (ndev && strstr(name, ndev->name)) {
-			if (strlen(ndev->name) > last_name_len) {
-				ndev_found = ndev;
-				last_name_len = strlen(ndev->name);
-			}
-		}
-	}
-
-	return ndev_found;
-}
-
-static monitor_interface* ndev_to_monif(struct net_device *ndev)
-{
-	int i;
-
-	for (i = 0; i < DHD_MAX_IFS; i++) {
-		if (g_monitor.mon_if[i].mon_ndev == ndev)
-			return &g_monitor.mon_if[i];
-	}
-
-	return NULL;
-}
-
-static int dhd_mon_if_open(struct net_device *ndev)
-{
-	int ret = 0;
-
-	MON_PRINT("enter\n");
-	return ret;
-}
-
-static int dhd_mon_if_stop(struct net_device *ndev)
-{
-	int ret = 0;
-
-	MON_PRINT("enter\n");
-	return ret;
-}
-
-static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev)
-{
-	int ret = 0;
-	int rtap_len;
-	int qos_len = 0;
-	int dot11_hdr_len = 24;
-	int snap_len = 6;
-	unsigned char *pdata;
-	unsigned short frame_ctl;
-	unsigned char src_mac_addr[6];
-	unsigned char dst_mac_addr[6];
-	struct ieee80211_hdr *dot11_hdr;
-	struct ieee80211_radiotap_header *rtap_hdr;
-	monitor_interface* mon_if;
-
-	MON_PRINT("enter\n");
-
-	mon_if = ndev_to_monif(ndev);
-	if (mon_if == NULL || mon_if->real_ndev == NULL) {
-		MON_PRINT(" cannot find matched net dev, skip the packet\n");
-		goto fail;
-	}
-
-	if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
-		goto fail;
-
-	rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
-	if (unlikely(rtap_hdr->it_version))
-		goto fail;
-
-	rtap_len = ieee80211_get_radiotap_len(skb->data);
-	if (unlikely(skb->len < rtap_len))
-		goto fail;
-
-	MON_PRINT("radiotap len (should be 14): %d\n", rtap_len);
-
-	/* Skip the ratio tap header */
-	skb_pull(skb, rtap_len);
-
-	dot11_hdr = (struct ieee80211_hdr *)skb->data;
-	frame_ctl = le16_to_cpu(dot11_hdr->frame_control);
-	/* Check if the QoS bit is set */
-	if ((frame_ctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) {
-		/* Check if this ia a Wireless Distribution System (WDS) frame
-		 * which has 4 MAC addresses
-		 */
-		if (dot11_hdr->frame_control & 0x0080)
-			qos_len = 2;
-		if ((dot11_hdr->frame_control & 0x0300) == 0x0300)
-			dot11_hdr_len += 6;
-
-		memcpy(dst_mac_addr, dot11_hdr->addr1, sizeof(dst_mac_addr));
-		memcpy(src_mac_addr, dot11_hdr->addr2, sizeof(src_mac_addr));
-
-		/* Skip the 802.11 header, QoS (if any) and SNAP, but leave spaces for
-		 * for two MAC addresses
-		 */
-		skb_pull(skb, dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
-		pdata = (unsigned char*)skb->data;
-		memcpy(pdata, dst_mac_addr, sizeof(dst_mac_addr));
-		memcpy(pdata + sizeof(dst_mac_addr), src_mac_addr, sizeof(src_mac_addr));
-
-		MON_PRINT("if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
-
-		/* Use the real net device to transmit the packet */
-		ret = dhd_start_xmit(skb, mon_if->real_ndev);
-
-		return ret;
-	}
-fail:
-	dev_kfree_skb(skb);
-	return 0;
-}
-
-static void dhd_mon_if_set_multicast_list(struct net_device *ndev)
-{
-	monitor_interface* mon_if;
-
-	mon_if = ndev_to_monif(ndev);
-	if (mon_if == NULL || mon_if->real_ndev == NULL) {
-		MON_PRINT(" cannot find matched net dev, skip the packet\n");
-	}
-
-	MON_PRINT("enter, if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
-}
-
-static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr)
-{
-	int ret = 0;
-	monitor_interface* mon_if;
-
-	mon_if = ndev_to_monif(ndev);
-	if (mon_if == NULL || mon_if->real_ndev == NULL) {
-		MON_PRINT(" cannot find matched net dev, skip the packet\n");
-	}
-
-	MON_PRINT("enter, if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
-	return ret;
-}
-
-/**
- * Global function definitions (declared in dhd_linux_mon.h)
- */
-
-int dhd_add_monitor(char *name, struct net_device **new_ndev)
-{
-	int i;
-	int idx = -1;
-	int ret = 0;
-	struct net_device* ndev = NULL;
-	dhd_linux_monitor_t **dhd_mon;
-
-	mutex_lock(&g_monitor.lock);
-
-	MON_TRACE("enter, if name: %s\n", name);
-	if (!name || !new_ndev) {
-		MON_PRINT("invalid parameters\n");
-		ret = -EINVAL;
-		goto out;
-	}
-
-	/*
-	 * Find a vacancy
-	 */
-	for (i = 0; i < DHD_MAX_IFS; i++)
-		if (g_monitor.mon_if[i].mon_ndev == NULL) {
-			idx = i;
-			break;
-		}
-	if (idx == -1) {
-		MON_PRINT("exceeds maximum interfaces\n");
-		ret = -EFAULT;
-		goto out;
-	}
-
-	ndev = alloc_etherdev(sizeof(dhd_linux_monitor_t*));
-	if (!ndev) {
-		MON_PRINT("failed to allocate memory\n");
-		ret = -ENOMEM;
-		goto out;
-	}
-
-	ndev->type = ARPHRD_IEEE80211_RADIOTAP;
-	strncpy(ndev->name, name, IFNAMSIZ);
-	ndev->name[IFNAMSIZ - 1] = 0;
-	ndev->netdev_ops = &dhd_mon_if_ops;
-
-	ret = register_netdevice(ndev);
-	if (ret) {
-		MON_PRINT(" register_netdevice failed (%d)\n", ret);
-		goto out;
-	}
-
-	*new_ndev = ndev;
-	g_monitor.mon_if[idx].radiotap_enabled = TRUE;
-	g_monitor.mon_if[idx].mon_ndev = ndev;
-	g_monitor.mon_if[idx].real_ndev = lookup_real_netdev(name);
-	dhd_mon = (dhd_linux_monitor_t **)netdev_priv(ndev);
-	*dhd_mon = &g_monitor;
-	g_monitor.monitor_state = MONITOR_STATE_INTERFACE_ADDED;
-	MON_PRINT("net device returned: 0x%p\n", ndev);
-	MON_PRINT("found a matched net device, name %s\n", g_monitor.mon_if[idx].real_ndev->name);
-
-out:
-	if (ret && ndev)
-		free_netdev(ndev);
-
-	mutex_unlock(&g_monitor.lock);
-	return ret;
-
-}
-
-int dhd_del_monitor(struct net_device *ndev)
-{
-	int i;
-	bool rollback_lock = false;
-	if (!ndev)
-		return -EINVAL;
-	mutex_lock(&g_monitor.lock);
-	for (i = 0; i < DHD_MAX_IFS; i++) {
-		if (g_monitor.mon_if[i].mon_ndev == ndev ||
-			g_monitor.mon_if[i].real_ndev == ndev) {
-			g_monitor.mon_if[i].real_ndev = NULL;
-			if (rtnl_is_locked()) {
-				rtnl_unlock();
-				rollback_lock = true;
-			}
-			unregister_netdev(g_monitor.mon_if[i].mon_ndev);
-			free_netdev(g_monitor.mon_if[i].mon_ndev);
-			g_monitor.mon_if[i].mon_ndev = NULL;
-			g_monitor.monitor_state = MONITOR_STATE_INTERFACE_DELETED;
-			break;
-		}
-	}
-	if (rollback_lock) {
-		rtnl_lock();
-		rollback_lock = false;
-	}
-
-	if (g_monitor.monitor_state !=
-	MONITOR_STATE_INTERFACE_DELETED)
-		MON_PRINT("interface not found in monitor IF array, is this a monitor IF? 0x%p\n",
-			ndev);
-	mutex_unlock(&g_monitor.lock);
-
-	return 0;
-}
-
-int dhd_monitor_init(void *dhd_pub)
-{
-	if (g_monitor.monitor_state == MONITOR_STATE_DEINIT) {
-		g_monitor.dhd_pub = dhd_pub;
-		mutex_init(&g_monitor.lock);
-		g_monitor.monitor_state = MONITOR_STATE_INIT;
-	}
-	return 0;
-}
-
-int dhd_monitor_uninit(void)
-{
-	int i;
-	struct net_device *ndev;
-	bool rollback_lock = false;
-	mutex_lock(&g_monitor.lock);
-	if (g_monitor.monitor_state != MONITOR_STATE_DEINIT) {
-		for (i = 0; i < DHD_MAX_IFS; i++) {
-			ndev = g_monitor.mon_if[i].mon_ndev;
-			if (ndev) {
-				if (rtnl_is_locked()) {
-					rtnl_unlock();
-					rollback_lock = true;
-				}
-				unregister_netdev(ndev);
-				free_netdev(ndev);
-				g_monitor.mon_if[i].real_ndev = NULL;
-				g_monitor.mon_if[i].mon_ndev = NULL;
-				if (rollback_lock) {
-					rtnl_lock();
-					rollback_lock = false;
-				}
-			}
-		}
-		g_monitor.monitor_state = MONITOR_STATE_DEINIT;
-	}
-	mutex_unlock(&g_monitor.lock);
-	return 0;
-}
diff --git a/drivers/net/wireless/bcmdhd/dhd_sdio.c b/drivers/net/wireless/bcmdhd/dhd_sdio.c
deleted file mode 100644
index 6d89f6b984a..00000000000
--- a/drivers/net/wireless/bcmdhd/dhd_sdio.c
+++ /dev/null
@@ -1,6242 +0,0 @@
-/*
- * DHD Bus Module for SDIO
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: dhd_sdio.c,v 1.274.2.40 2011-02-09 22:42:44 Exp $
- */
-
-#include <typedefs.h>
-#include <osl.h>
-#include <bcmsdh.h>
-
-#ifdef BCMEMBEDIMAGE
-#include BCMEMBEDIMAGE
-#endif /* BCMEMBEDIMAGE */
-
-#include <bcmdefs.h>
-#include <bcmutils.h>
-#include <bcmendian.h>
-#include <bcmdevs.h>
-
-#include <siutils.h>
-#include <hndpmu.h>
-#include <hndsoc.h>
-#include <bcmsdpcm.h>
-#if defined(DHD_DEBUG)
-#include <hndrte_armtrap.h>
-#include <hndrte_cons.h>
-#endif /* defined(DHD_DEBUG) */
-#include <sbchipc.h>
-#include <sbhnddma.h>
-
-#include <sdio.h>
-#include <sbsdio.h>
-#include <sbsdpcmdev.h>
-#include <bcmsdpcm.h>
-#include <bcmsdbus.h>
-
-#include <proto/ethernet.h>
-#include <proto/802.1d.h>
-#include <proto/802.11.h>
-
-#include <dngl_stats.h>
-#include <dhd.h>
-#include <dhd_bus.h>
-#include <dhd_proto.h>
-#include <dhd_dbg.h>
-#include <dhdioctl.h>
-#include <sdiovar.h>
-
-#ifndef DHDSDIO_MEM_DUMP_FNAME
-#define DHDSDIO_MEM_DUMP_FNAME         "mem_dump"
-#endif
-
-#define QLEN		256	/* bulk rx and tx queue lengths */
-#define FCHI		(QLEN - 10)
-#define FCLOW		(FCHI / 2)
-#define PRIOMASK	7
-
-#define TXRETRIES	2	/* # of retries for tx frames */
-
-#define DHD_RXBOUND	50	/* Default for max rx frames in one scheduling */
-
-#define DHD_TXBOUND	20	/* Default for max tx frames in one scheduling */
-
-#define DHD_TXMINMAX	1	/* Max tx frames if rx still pending */
-
-#define MEMBLOCK	2048		/* Block size used for downloading of dongle image */
-#define MAX_NVRAMBUF_SIZE	4096	/* max nvram buf size */
-#define MAX_DATA_BUF	(32 * 1024)	/* Must be large enough to hold biggest possible glom */
-
-/* Packet alignment for most efficient SDIO (can change based on platform) */
-#ifndef DHD_SDALIGN
-#define DHD_SDALIGN	32
-#endif
-#if !ISPOWEROF2(DHD_SDALIGN)
-#error DHD_SDALIGN is not a power of 2!
-#endif
-
-#ifndef DHD_FIRSTREAD
-#define DHD_FIRSTREAD   32
-#endif
-#if !ISPOWEROF2(DHD_FIRSTREAD)
-#error DHD_FIRSTREAD is not a power of 2!
-#endif
-
-/* Total length of frame header for dongle protocol */
-#define SDPCM_HDRLEN	(SDPCM_FRAMETAG_LEN + SDPCM_SWHEADER_LEN)
-#ifdef SDTEST
-#define SDPCM_RESERVE	(SDPCM_HDRLEN + SDPCM_TEST_HDRLEN + DHD_SDALIGN)
-#else
-#define SDPCM_RESERVE	(SDPCM_HDRLEN + DHD_SDALIGN)
-#endif
-
-/* Space for header read, limit for data packets */
-#ifndef MAX_HDR_READ
-#define MAX_HDR_READ	32
-#endif
-#if !ISPOWEROF2(MAX_HDR_READ)
-#error MAX_HDR_READ is not a power of 2!
-#endif
-
-#define MAX_RX_DATASZ	2048
-
-/* Maximum milliseconds to wait for F2 to come up */
-#define DHD_WAIT_F2RDY	3000
-
-/* Bump up limit on waiting for HT to account for first startup;
- * if the image is doing a CRC calculation before programming the PMU
- * for HT availability, it could take a couple hundred ms more, so
- * max out at a 1 second (1000000us).
- */
-#if (PMU_MAX_TRANSITION_DLY <= 1000000)
-#undef PMU_MAX_TRANSITION_DLY
-#define PMU_MAX_TRANSITION_DLY 1000000
-#endif
-
-/* Value for ChipClockCSR during initial setup */
-#define DHD_INIT_CLKCTL1	(SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_ALP_AVAIL_REQ)
-#define DHD_INIT_CLKCTL2	(SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_FORCE_ALP)
-
-/* Flags for SDH calls */
-#define F2SYNC	(SDIO_REQ_4BYTE | SDIO_REQ_FIXED)
-
-/* Packet free applicable unconditionally for sdio and sdspi.  Conditional if
- * bufpool was present for gspi bus.
- */
-#define PKTFREE2()		if ((bus->bus != SPI_BUS) || bus->usebufpool) \
-					PKTFREE(bus->dhd->osh, pkt, FALSE);
-DHD_SPINWAIT_SLEEP_INIT(sdioh_spinwait_sleep);
-#if defined(OOB_INTR_ONLY)
-extern void bcmsdh_set_irq(int flag);
-#endif /* defined(OOB_INTR_ONLY) */
-#ifdef PROP_TXSTATUS
-extern void dhd_wlfc_txcomplete(dhd_pub_t *dhd, void *txp, bool success);
-#endif
-
-#ifdef DHD_DEBUG
-/* Device console log buffer state */
-typedef struct dhd_console {
-	uint		count;			/* Poll interval msec counter */
-	uint		log_addr;		/* Log struct address (fixed) */
-	hndrte_log_t	log;			/* Log struct (host copy) */
-	uint		bufsize;		/* Size of log buffer */
-	uint8		*buf;			/* Log buffer (host copy) */
-	uint		last;			/* Last buffer read index */
-} dhd_console_t;
-#endif /* DHD_DEBUG */
-
-/* Private data for SDIO bus interaction */
-typedef struct dhd_bus {
-	dhd_pub_t	*dhd;
-
-	bcmsdh_info_t	*sdh;			/* Handle for BCMSDH calls */
-	si_t		*sih;			/* Handle for SI calls */
-	char		*vars;			/* Variables (from CIS and/or other) */
-	uint		varsz;			/* Size of variables buffer */
-	uint32		sbaddr;			/* Current SB window pointer (-1, invalid) */
-
-	sdpcmd_regs_t	*regs;			/* Registers for SDIO core */
-	uint		sdpcmrev;		/* SDIO core revision */
-	uint		armrev;			/* CPU core revision */
-	uint		ramrev;			/* SOCRAM core revision */
-	uint32		ramsize;		/* Size of RAM in SOCRAM (bytes) */
-	uint32		orig_ramsize;		/* Size of RAM in SOCRAM (bytes) */
-
-	uint32		bus;			/* gSPI or SDIO bus */
-	uint32		hostintmask;		/* Copy of Host Interrupt Mask */
-	uint32		intstatus;		/* Intstatus bits (events) pending */
-	bool		dpc_sched;		/* Indicates DPC schedule (intrpt rcvd) */
-	bool		fcstate;		/* State of dongle flow-control */
-
-	uint16		cl_devid;		/* cached devid for dhdsdio_probe_attach() */
-	char		*fw_path; /* module_param: path to firmware image */
-	char		*nv_path; /* module_param: path to nvram vars file */
-	const char      *nvram_params;		/* user specified nvram params. */
-
-	uint		blocksize;		/* Block size of SDIO transfers */
-	uint		roundup;		/* Max roundup limit */
-
-	struct pktq	txq;			/* Queue length used for flow-control */
-	uint8		flowcontrol;		/* per prio flow control bitmask */
-	uint8		tx_seq;			/* Transmit sequence number (next) */
-	uint8		tx_max;			/* Maximum transmit sequence allowed */
-
-	uint8		hdrbuf[MAX_HDR_READ + DHD_SDALIGN];
-	uint8		*rxhdr;			/* Header of current rx frame (in hdrbuf) */
-	uint16		nextlen;		/* Next Read Len from last header */
-	uint8		rx_seq;			/* Receive sequence number (expected) */
-	bool		rxskip;			/* Skip receive (awaiting NAK ACK) */
-
-	void		*glomd;			/* Packet containing glomming descriptor */
-	void		*glom;			/* Packet chain for glommed superframe */
-	uint		glomerr;		/* Glom packet read errors */
-
-	uint8		*rxbuf;			/* Buffer for receiving control packets */
-	uint		rxblen;			/* Allocated length of rxbuf */
-	uint8		*rxctl;			/* Aligned pointer into rxbuf */
-	uint8		*databuf;		/* Buffer for receiving big glom packet */
-	uint8		*dataptr;		/* Aligned pointer into databuf */
-	uint		rxlen;			/* Length of valid data in buffer */
-
-	uint8		sdpcm_ver;		/* Bus protocol reported by dongle */
-
-	bool		intr;			/* Use interrupts */
-	bool		poll;			/* Use polling */
-	bool		ipend;			/* Device interrupt is pending */
-	bool		intdis;			/* Interrupts disabled by isr */
-	uint 		intrcount;		/* Count of device interrupt callbacks */
-	uint		lastintrs;		/* Count as of last watchdog timer */
-	uint		spurious;		/* Count of spurious interrupts */
-	uint		pollrate;		/* Ticks between device polls */
-	uint		polltick;		/* Tick counter */
-	uint		pollcnt;		/* Count of active polls */
-
-#ifdef DHD_DEBUG
-	dhd_console_t	console;		/* Console output polling support */
-	uint		console_addr;		/* Console address from shared struct */
-#endif /* DHD_DEBUG */
-
-	uint		regfails;		/* Count of R_REG/W_REG failures */
-
-	uint		clkstate;		/* State of sd and backplane clock(s) */
-	bool		activity;		/* Activity flag for clock down */
-	int32		idletime;		/* Control for activity timeout */
-	int32		idlecount;		/* Activity timeout counter */
-	int32		idleclock;		/* How to set bus driver when idle */
-	int32		sd_divisor;		/* Speed control to bus driver */
-	int32		sd_mode;		/* Mode control to bus driver */
-	int32		sd_rxchain;		/* If bcmsdh api accepts PKT chains */
-	bool		use_rxchain;		/* If dhd should use PKT chains */
-	bool		sleeping;		/* Is SDIO bus sleeping? */
-	bool		rxflow_mode;	/* Rx flow control mode */
-	bool		rxflow;			/* Is rx flow control on */
-	uint		prev_rxlim_hit;		/* Is prev rx limit exceeded (per dpc schedule) */
-	bool		alp_only;		/* Don't use HT clock (ALP only) */
-	/* Field to decide if rx of control frames happen in rxbuf or lb-pool */
-	bool		usebufpool;
-
-#ifdef SDTEST
-	/* external loopback */
-	bool		ext_loop;
-	uint8		loopid;
-
-	/* pktgen configuration */
-	uint		pktgen_freq;		/* Ticks between bursts */
-	uint		pktgen_count;		/* Packets to send each burst */
-	uint		pktgen_print;		/* Bursts between count displays */
-	uint		pktgen_total;		/* Stop after this many */
-	uint		pktgen_minlen;		/* Minimum packet data len */
-	uint		pktgen_maxlen;		/* Maximum packet data len */
-	uint		pktgen_mode;		/* Configured mode: tx, rx, or echo */
-	uint		pktgen_stop;		/* Number of tx failures causing stop */
-
-	/* active pktgen fields */
-	uint		pktgen_tick;		/* Tick counter for bursts */
-	uint		pktgen_ptick;		/* Burst counter for printing */
-	uint		pktgen_sent;		/* Number of test packets generated */
-	uint		pktgen_rcvd;		/* Number of test packets received */
-	uint		pktgen_fail;		/* Number of failed send attempts */
-	uint16		pktgen_len;		/* Length of next packet to send */
-#define PKTGEN_RCV_IDLE     (0)
-#define PKTGEN_RCV_ONGOING  (1)
-	uint16		pktgen_rcv_state;		/* receive state */
-	uint		pktgen_rcvd_rcvsession;	/* test pkts rcvd per rcv session. */
-#endif /* SDTEST */
-
-	/* Some additional counters */
-	uint		tx_sderrs;		/* Count of tx attempts with sd errors */
-	uint		fcqueued;		/* Tx packets that got queued */
-	uint		rxrtx;			/* Count of rtx requests (NAK to dongle) */
-	uint		rx_toolong;		/* Receive frames too long to receive */
-	uint		rxc_errors;		/* SDIO errors when reading control frames */
-	uint		rx_hdrfail;		/* SDIO errors on header reads */
-	uint		rx_badhdr;		/* Bad received headers (roosync?) */
-	uint		rx_badseq;		/* Mismatched rx sequence number */
-	uint		fc_rcvd;		/* Number of flow-control events received */
-	uint		fc_xoff;		/* Number which turned on flow-control */
-	uint		fc_xon;			/* Number which turned off flow-control */
-	uint		rxglomfail;		/* Failed deglom attempts */
-	uint		rxglomframes;		/* Number of glom frames (superframes) */
-	uint		rxglompkts;		/* Number of packets from glom frames */
-	uint		f2rxhdrs;		/* Number of header reads */
-	uint		f2rxdata;		/* Number of frame data reads */
-	uint		f2txdata;		/* Number of f2 frame writes */
-	uint		f1regdata;		/* Number of f1 register accesses */
-
-	uint8		*ctrl_frame_buf;
-	uint32		ctrl_frame_len;
-	bool		ctrl_frame_stat;
-	uint32		rxint_mode;	/* rx interrupt mode */
-} dhd_bus_t;
-
-/* clkstate */
-#define CLK_NONE	0
-#define CLK_SDONLY	1
-#define CLK_PENDING	2	/* Not used yet */
-#define CLK_AVAIL	3
-
-#define DHD_NOPMU(dhd)	(FALSE)
-
-#ifdef DHD_DEBUG
-static int qcount[NUMPRIO];
-static int tx_packets[NUMPRIO];
-#endif /* DHD_DEBUG */
-
-/* Deferred transmit */
-const uint dhd_deferred_tx = 1;
-
-extern uint dhd_watchdog_ms;
-extern void dhd_os_wd_timer(void *bus, uint wdtick);
-
-/* Tx/Rx bounds */
-uint dhd_txbound;
-uint dhd_rxbound;
-uint dhd_txminmax = DHD_TXMINMAX;
-
-/* override the RAM size if possible */
-#define DONGLE_MIN_MEMSIZE (128 *1024)
-int dhd_dongle_memsize;
-
-static bool dhd_doflow;
-static bool dhd_alignctl;
-
-static bool sd1idle;
-
-static bool retrydata;
-#define RETRYCHAN(chan) (((chan) == SDPCM_EVENT_CHANNEL) || retrydata)
-
-static const uint watermark = 8;
-static const uint firstread = DHD_FIRSTREAD;
-
-#define HDATLEN (firstread - (SDPCM_HDRLEN))
-
-/* Retry count for register access failures */
-static const uint retry_limit = 2;
-
-/* Force even SD lengths (some host controllers mess up on odd bytes) */
-static bool forcealign;
-
-/* Flag to indicate if we should download firmware on driver load */
-uint dhd_download_fw_on_driverload = FALSE;
-
-#define ALIGNMENT  4
-
-#if defined(OOB_INTR_ONLY) && defined(HW_OOB)
-extern void bcmsdh_enable_hw_oob_intr(void *sdh, bool enable);
-#endif
-
-#if defined(OOB_INTR_ONLY) && defined(SDIO_ISR_THREAD)
-#error OOB_INTR_ONLY is NOT working with SDIO_ISR_THREAD
-#endif /* defined(OOB_INTR_ONLY) && defined(SDIO_ISR_THREAD) */
-#define PKTALIGN(osh, p, len, align)					\
-	do {								\
-		uint datalign;						\
-		datalign = (uintptr)PKTDATA((osh), (p));		\
-		datalign = ROUNDUP(datalign, (align)) - datalign;	\
-		ASSERT(datalign < (align));				\
-		ASSERT(PKTLEN((osh), (p)) >= ((len) + datalign));	\
-		if (datalign)						\
-			PKTPULL((osh), (p), datalign);			\
-		PKTSETLEN((osh), (p), (len));				\
-	} while (0)
-
-/* Limit on rounding up frames */
-static const uint max_roundup = 512;
-
-/* Try doing readahead */
-static bool dhd_readahead;
-
-
-/* To check if there's window offered */
-#define DATAOK(bus) \
-	(((uint8)(bus->tx_max - bus->tx_seq) > 1) && \
-	(((uint8)(bus->tx_max - bus->tx_seq) & 0x80) == 0))
-
-/* To check if there's window offered for ctrl frame */
-#define TXCTLOK(bus) \
-	(((uint8)(bus->tx_max - bus->tx_seq) != 0) && \
-	(((uint8)(bus->tx_max - bus->tx_seq) & 0x80) == 0))
-
-/* Macros to get register read/write status */
-/* NOTE: these assume a local dhdsdio_bus_t *bus! */
-#define R_SDREG(regvar, regaddr, retryvar) \
-do { \
-	retryvar = 0; \
-	do { \
-		regvar = R_REG(bus->dhd->osh, regaddr); \
-	} while (bcmsdh_regfail(bus->sdh) && (++retryvar <= retry_limit)); \
-	if (retryvar) { \
-		bus->regfails += (retryvar-1); \
-		if (retryvar > retry_limit) { \
-			DHD_ERROR(("%s: FAILED" #regvar "READ, LINE %d\n", \
-			           __FUNCTION__, __LINE__)); \
-			regvar = 0; \
-		} \
-	} \
-} while (0)
-
-#define W_SDREG(regval, regaddr, retryvar) \
-do { \
-	retryvar = 0; \
-	do { \
-		W_REG(bus->dhd->osh, regaddr, regval); \
-	} while (bcmsdh_regfail(bus->sdh) && (++retryvar <= retry_limit)); \
-	if (retryvar) { \
-		bus->regfails += (retryvar-1); \
-		if (retryvar > retry_limit) \
-			DHD_ERROR(("%s: FAILED REGISTER WRITE, LINE %d\n", \
-			           __FUNCTION__, __LINE__)); \
-	} \
-} while (0)
-
-
-/*
- * pktavail interrupts from dongle to host can be managed in 3 different ways
- * whenever there is a packet available in dongle to transmit to host.
- *
- * Mode 0:	Dongle writes the software host mailbox and host is interrupted.
- * Mode 1:	(sdiod core rev >= 4)
- *		Device sets a new bit in the intstatus whenever there is a packet
- *		available in fifo.  Host can't clear this specific status bit until all the 
- *		packets are read from the FIFO.  No need to ack dongle intstatus.
- * Mode 2:	(sdiod core rev >= 4)
- *		Device sets a bit in the intstatus, and host acks this by writing
- *		one to this bit.  Dongle won't generate anymore packet interrupts
- *		until host reads all the packets from the dongle and reads a zero to
- *		figure that there are no more packets.  No need to disable host ints.
- *		Need to ack the intstatus.
- */
-
-#define SDIO_DEVICE_HMB_RXINT		0	/* default old way */
-#define SDIO_DEVICE_RXDATAINT_MODE_0	1	/* from sdiod rev 4 */
-#define SDIO_DEVICE_RXDATAINT_MODE_1	2	/* from sdiod rev 4 */
-
-
-#define FRAME_AVAIL_MASK(bus) 	\
-	((bus->rxint_mode == SDIO_DEVICE_HMB_RXINT) ? I_HMB_FRAME_IND : I_XMTDATA_AVAIL)
-
-#define DHD_BUS			SDIO_BUS
-
-#define PKT_AVAILABLE(bus, intstatus)	((intstatus) & (FRAME_AVAIL_MASK(bus)))
-
-#define HOSTINTMASK		(I_HMB_SW_MASK | I_CHIPACTIVE)
-
-#define GSPI_PR55150_BAILOUT
-
-
-#ifdef SDTEST
-static void dhdsdio_testrcv(dhd_bus_t *bus, void *pkt, uint seq);
-static void dhdsdio_sdtest_set(dhd_bus_t *bus, uint8 count);
-#endif
-
-#ifdef DHD_DEBUG
-static int dhdsdio_checkdied(dhd_bus_t *bus, uint8 *data, uint size);
-static int dhdsdio_mem_dump(dhd_bus_t *bus);
-static int dhd_serialconsole(dhd_bus_t *bus, bool get, bool enable, int *bcmerror);
-#endif /* DHD_DEBUG */
-static int dhdsdio_download_state(dhd_bus_t *bus, bool enter);
-
-static void dhdsdio_release(dhd_bus_t *bus, osl_t *osh);
-static void dhdsdio_release_malloc(dhd_bus_t *bus, osl_t *osh);
-static void dhdsdio_disconnect(void *ptr);
-static bool dhdsdio_chipmatch(uint16 chipid);
-static bool dhdsdio_probe_attach(dhd_bus_t *bus, osl_t *osh, void *sdh,
-                                 void * regsva, uint16  devid);
-static bool dhdsdio_probe_malloc(dhd_bus_t *bus, osl_t *osh, void *sdh);
-static bool dhdsdio_probe_init(dhd_bus_t *bus, osl_t *osh, void *sdh);
-static void dhdsdio_release_dongle(dhd_bus_t *bus, osl_t *osh, bool dongle_isolation,
-	bool reset_flag);
-
-static void dhd_dongle_setmemsize(struct dhd_bus *bus, int mem_size);
-static int dhd_bcmsdh_recv_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags,
-	uint8 *buf, uint nbytes,
-	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle);
-static int dhd_bcmsdh_send_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags,
-	uint8 *buf, uint nbytes,
-	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle);
-
-static bool dhdsdio_download_firmware(dhd_bus_t *bus, osl_t *osh, void *sdh);
-static int _dhdsdio_download_firmware(dhd_bus_t *bus);
-
-static int dhdsdio_download_code_file(dhd_bus_t *bus, char *image_path);
-static int dhdsdio_download_nvram(dhd_bus_t *bus);
-#ifdef BCMEMBEDIMAGE
-static int dhdsdio_download_code_array(dhd_bus_t *bus);
-#endif
-
-#ifdef WLMEDIA_HTSF
-#include <htsf.h>
-extern uint32 dhd_get_htsf(void *dhd, int ifidx);
-#endif /* WLMEDIA_HTSF */
-
-static void
-dhd_dongle_setmemsize(struct dhd_bus *bus, int mem_size)
-{
-	int32 min_size =  DONGLE_MIN_MEMSIZE;
-	/* Restrict the memsize to user specified limit */
-	DHD_ERROR(("user: Restrict the dongle ram size to %d, min accepted %d\n",
-		dhd_dongle_memsize, min_size));
-	if ((dhd_dongle_memsize > min_size) &&
-		(dhd_dongle_memsize < (int32)bus->orig_ramsize))
-		bus->ramsize = dhd_dongle_memsize;
-}
-
-static int
-dhdsdio_set_siaddr_window(dhd_bus_t *bus, uint32 address)
-{
-	int err = 0;
-	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW,
-	                 (address >> 8) & SBSDIO_SBADDRLOW_MASK, &err);
-	if (!err)
-		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRMID,
-		                 (address >> 16) & SBSDIO_SBADDRMID_MASK, &err);
-	if (!err)
-		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRHIGH,
-		                 (address >> 24) & SBSDIO_SBADDRHIGH_MASK, &err);
-	return err;
-}
-
-
-/* Turn backplane clock on or off */
-static int
-dhdsdio_htclk(dhd_bus_t *bus, bool on, bool pendok)
-{
-	int err;
-	uint8 clkctl, clkreq, devctl;
-	bcmsdh_info_t *sdh;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-#if defined(OOB_INTR_ONLY)
-	pendok = FALSE;
-#endif
-	clkctl = 0;
-	sdh = bus->sdh;
-
-
-	if (on) {
-		/* Request HT Avail */
-		clkreq = bus->alp_only ? SBSDIO_ALP_AVAIL_REQ : SBSDIO_HT_AVAIL_REQ;
-
-
-
-
-		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
-		if (err) {
-			DHD_ERROR(("%s: HT Avail request error: %d\n", __FUNCTION__, err));
-			return BCME_ERROR;
-		}
-
-		if (pendok &&
-		    ((bus->sih->buscoretype == PCMCIA_CORE_ID) && (bus->sih->buscorerev == 9))) {
-			uint32 dummy, retries;
-			R_SDREG(dummy, &bus->regs->clockctlstatus, retries);
-		}
-
-		/* Check current status */
-		clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
-		if (err) {
-			DHD_ERROR(("%s: HT Avail read error: %d\n", __FUNCTION__, err));
-			return BCME_ERROR;
-		}
-
-		/* Go to pending and await interrupt if appropriate */
-		if (!SBSDIO_CLKAV(clkctl, bus->alp_only) && pendok) {
-			/* Allow only clock-available interrupt */
-			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
-			if (err) {
-				DHD_ERROR(("%s: Devctl access error setting CA: %d\n",
-				           __FUNCTION__, err));
-				return BCME_ERROR;
-			}
-
-			devctl |= SBSDIO_DEVCTL_CA_INT_ONLY;
-			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
-			DHD_INFO(("CLKCTL: set PENDING\n"));
-			bus->clkstate = CLK_PENDING;
-			return BCME_OK;
-		} else if (bus->clkstate == CLK_PENDING) {
-			/* Cancel CA-only interrupt filter */
-			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
-			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
-		}
-
-		/* Otherwise, wait here (polling) for HT Avail */
-		if (!SBSDIO_CLKAV(clkctl, bus->alp_only)) {
-			SPINWAIT_SLEEP(sdioh_spinwait_sleep,
-				((clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
-			                                    SBSDIO_FUNC1_CHIPCLKCSR, &err)),
-			          !SBSDIO_CLKAV(clkctl, bus->alp_only)), PMU_MAX_TRANSITION_DLY);
-		}
-		if (err) {
-			DHD_ERROR(("%s: HT Avail request error: %d\n", __FUNCTION__, err));
-			return BCME_ERROR;
-		}
-		if (!SBSDIO_CLKAV(clkctl, bus->alp_only)) {
-			DHD_ERROR(("%s: HT Avail timeout (%d): clkctl 0x%02x\n",
-			           __FUNCTION__, PMU_MAX_TRANSITION_DLY, clkctl));
-			return BCME_ERROR;
-		}
-
-
-		/* Mark clock available */
-		bus->clkstate = CLK_AVAIL;
-		DHD_INFO(("CLKCTL: turned ON\n"));
-
-#if defined(DHD_DEBUG)
-		if (bus->alp_only == TRUE) {
-#if !defined(BCMLXSDMMC)
-			if (!SBSDIO_ALPONLY(clkctl)) {
-				DHD_ERROR(("%s: HT Clock, when ALP Only\n", __FUNCTION__));
-			}
-#endif /* !defined(BCMLXSDMMC) */
-		} else {
-			if (SBSDIO_ALPONLY(clkctl)) {
-				DHD_ERROR(("%s: HT Clock should be on.\n", __FUNCTION__));
-			}
-		}
-#endif /* defined (DHD_DEBUG) */
-
-		bus->activity = TRUE;
-	} else {
-		clkreq = 0;
-
-		if (bus->clkstate == CLK_PENDING) {
-			/* Cancel CA-only interrupt filter */
-			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
-			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
-		}
-
-		bus->clkstate = CLK_SDONLY;
-		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
-		DHD_INFO(("CLKCTL: turned OFF\n"));
-		if (err) {
-			DHD_ERROR(("%s: Failed access turning clock off: %d\n",
-			           __FUNCTION__, err));
-			return BCME_ERROR;
-		}
-	}
-	return BCME_OK;
-}
-
-/* Change idle/active SD state */
-static int
-dhdsdio_sdclk(dhd_bus_t *bus, bool on)
-{
-	int err;
-	int32 iovalue;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (on) {
-		if (bus->idleclock == DHD_IDLE_STOP) {
-			/* Turn on clock and restore mode */
-			iovalue = 1;
-			err = bcmsdh_iovar_op(bus->sdh, "sd_clock", NULL, 0,
-			                      &iovalue, sizeof(iovalue), TRUE);
-			if (err) {
-				DHD_ERROR(("%s: error enabling sd_clock: %d\n",
-				           __FUNCTION__, err));
-				return BCME_ERROR;
-			}
-
-			iovalue = bus->sd_mode;
-			err = bcmsdh_iovar_op(bus->sdh, "sd_mode", NULL, 0,
-			                      &iovalue, sizeof(iovalue), TRUE);
-			if (err) {
-				DHD_ERROR(("%s: error changing sd_mode: %d\n",
-				           __FUNCTION__, err));
-				return BCME_ERROR;
-			}
-		} else if (bus->idleclock != DHD_IDLE_ACTIVE) {
-			/* Restore clock speed */
-			iovalue = bus->sd_divisor;
-			err = bcmsdh_iovar_op(bus->sdh, "sd_divisor", NULL, 0,
-			                      &iovalue, sizeof(iovalue), TRUE);
-			if (err) {
-				DHD_ERROR(("%s: error restoring sd_divisor: %d\n",
-				           __FUNCTION__, err));
-				return BCME_ERROR;
-			}
-		}
-		bus->clkstate = CLK_SDONLY;
-	} else {
-		/* Stop or slow the SD clock itself */
-		if ((bus->sd_divisor == -1) || (bus->sd_mode == -1)) {
-			DHD_TRACE(("%s: can't idle clock, divisor %d mode %d\n",
-			           __FUNCTION__, bus->sd_divisor, bus->sd_mode));
-			return BCME_ERROR;
-		}
-		if (bus->idleclock == DHD_IDLE_STOP) {
-			if (sd1idle) {
-				/* Change to SD1 mode and turn off clock */
-				iovalue = 1;
-				err = bcmsdh_iovar_op(bus->sdh, "sd_mode", NULL, 0,
-				                      &iovalue, sizeof(iovalue), TRUE);
-				if (err) {
-					DHD_ERROR(("%s: error changing sd_clock: %d\n",
-					           __FUNCTION__, err));
-					return BCME_ERROR;
-				}
-			}
-
-			iovalue = 0;
-			err = bcmsdh_iovar_op(bus->sdh, "sd_clock", NULL, 0,
-			                      &iovalue, sizeof(iovalue), TRUE);
-			if (err) {
-				DHD_ERROR(("%s: error disabling sd_clock: %d\n",
-				           __FUNCTION__, err));
-				return BCME_ERROR;
-			}
-		} else if (bus->idleclock != DHD_IDLE_ACTIVE) {
-			/* Set divisor to idle value */
-			iovalue = bus->idleclock;
-			err = bcmsdh_iovar_op(bus->sdh, "sd_divisor", NULL, 0,
-			                      &iovalue, sizeof(iovalue), TRUE);
-			if (err) {
-				DHD_ERROR(("%s: error changing sd_divisor: %d\n",
-				           __FUNCTION__, err));
-				return BCME_ERROR;
-			}
-		}
-		bus->clkstate = CLK_NONE;
-	}
-
-	return BCME_OK;
-}
-
-/* Transition SD and backplane clock readiness */
-static int
-dhdsdio_clkctl(dhd_bus_t *bus, uint target, bool pendok)
-{
-	int ret = BCME_OK;
-#ifdef DHD_DEBUG
-	uint oldstate = bus->clkstate;
-#endif /* DHD_DEBUG */
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	/* Early exit if we're already there */
-	if (bus->clkstate == target) {
-		if (target == CLK_AVAIL) {
-			dhd_os_wd_timer(bus->dhd, dhd_watchdog_ms);
-			bus->activity = TRUE;
-		}
-		return ret;
-	}
-
-	switch (target) {
-	case CLK_AVAIL:
-		/* Make sure SD clock is available */
-		if (bus->clkstate == CLK_NONE)
-			dhdsdio_sdclk(bus, TRUE);
-		/* Now request HT Avail on the backplane */
-		ret = dhdsdio_htclk(bus, TRUE, pendok);
-		if (ret == BCME_OK) {
-			dhd_os_wd_timer(bus->dhd, dhd_watchdog_ms);
-			bus->activity = TRUE;
-		}
-		break;
-
-	case CLK_SDONLY:
-		/* Remove HT request, or bring up SD clock */
-		if (bus->clkstate == CLK_NONE)
-			ret = dhdsdio_sdclk(bus, TRUE);
-		else if (bus->clkstate == CLK_AVAIL)
-			ret = dhdsdio_htclk(bus, FALSE, FALSE);
-		else
-			DHD_ERROR(("dhdsdio_clkctl: request for %d -> %d\n",
-			           bus->clkstate, target));
-		if (ret == BCME_OK) {
-			dhd_os_wd_timer(bus->dhd, dhd_watchdog_ms);
-		}
-		break;
-
-	case CLK_NONE:
-		/* Make sure to remove HT request */
-		if (bus->clkstate == CLK_AVAIL)
-			ret = dhdsdio_htclk(bus, FALSE, FALSE);
-		/* Now remove the SD clock */
-		ret = dhdsdio_sdclk(bus, FALSE);
-#ifdef DHD_DEBUG
-		if (dhd_console_ms == 0)
-#endif /* DHD_DEBUG */
-		dhd_os_wd_timer(bus->dhd, 0);
-		break;
-	}
-#ifdef DHD_DEBUG
-	DHD_INFO(("dhdsdio_clkctl: %d -> %d\n", oldstate, bus->clkstate));
-#endif /* DHD_DEBUG */
-
-	return ret;
-}
-
-static int
-dhdsdio_bussleep(dhd_bus_t *bus, bool sleep)
-{
-	bcmsdh_info_t *sdh = bus->sdh;
-	sdpcmd_regs_t *regs = bus->regs;
-	uint retries = 0;
-
-	DHD_INFO(("dhdsdio_bussleep: request %s (currently %s)\n",
-	          (sleep ? "SLEEP" : "WAKE"),
-	          (bus->sleeping ? "SLEEP" : "WAKE")));
-
-	/* Done if we're already in the requested state */
-	if (sleep == bus->sleeping)
-		return BCME_OK;
-
-	/* Going to sleep: set the alarm and turn off the lights... */
-	if (sleep) {
-		/* Don't sleep if something is pending */
-		if (bus->dpc_sched || bus->rxskip || pktq_len(&bus->txq))
-			return BCME_BUSY;
-
-
-		/* Disable SDIO interrupts (no longer interested) */
-		bcmsdh_intr_disable(bus->sdh);
-
-		/* Make sure the controller has the bus up */
-		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-
-		/* Tell device to start using OOB wakeup */
-		W_SDREG(SMB_USE_OOB, &regs->tosbmailbox, retries);
-		if (retries > retry_limit)
-			DHD_ERROR(("CANNOT SIGNAL CHIP, WILL NOT WAKE UP!!\n"));
-
-		/* Turn off our contribution to the HT clock request */
-		dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
-
-		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
-		                 SBSDIO_FORCE_HW_CLKREQ_OFF, NULL);
-
-		/* Isolate the bus */
-		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL,
-		                 SBSDIO_DEVCTL_PADS_ISO, NULL);
-
-		/* Change state */
-		bus->sleeping = TRUE;
-
-	} else {
-		/* Waking up: bus power up is ok, set local state */
-
-		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
-		                 0, NULL);
-
-		/* Force pad isolation off if possible (in case power never toggled) */
-		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, 0, NULL);
-
-
-		/* Make sure the controller has the bus up */
-		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-
-		/* Send misc interrupt to indicate OOB not needed */
-		W_SDREG(0, &regs->tosbmailboxdata, retries);
-		if (retries <= retry_limit)
-			W_SDREG(SMB_DEV_INT, &regs->tosbmailbox, retries);
-
-		if (retries > retry_limit)
-			DHD_ERROR(("CANNOT SIGNAL CHIP TO CLEAR OOB!!\n"));
-
-		/* Make sure we have SD bus access */
-		dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
-
-		/* Change state */
-		bus->sleeping = FALSE;
-
-		/* Enable interrupts again */
-		if (bus->intr && (bus->dhd->busstate == DHD_BUS_DATA)) {
-			bus->intdis = FALSE;
-			bcmsdh_intr_enable(bus->sdh);
-		}
-	}
-
-	return BCME_OK;
-}
-
-#if defined(OOB_INTR_ONLY)
-void
-dhd_enable_oob_intr(struct dhd_bus *bus, bool enable)
-{
-#if defined(HW_OOB)
-	bcmsdh_enable_hw_oob_intr(bus->sdh, enable);
-#else
-	sdpcmd_regs_t *regs = bus->regs;
-	uint retries = 0;
-
-	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-	if (enable == TRUE) {
-
-		/* Tell device to start using OOB wakeup */
-		W_SDREG(SMB_USE_OOB, &regs->tosbmailbox, retries);
-		if (retries > retry_limit)
-			DHD_ERROR(("CANNOT SIGNAL CHIP, WILL NOT WAKE UP!!\n"));
-
-	} else {
-		/* Send misc interrupt to indicate OOB not needed */
-		W_SDREG(0, &regs->tosbmailboxdata, retries);
-		if (retries <= retry_limit)
-			W_SDREG(SMB_DEV_INT, &regs->tosbmailbox, retries);
-	}
-
-	/* Turn off our contribution to the HT clock request */
-	dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
-#endif /* !defined(HW_OOB) */
-}
-#endif /* defined(OOB_INTR_ONLY) */
-
-#define BUS_WAKE(bus) \
-	do { \
-		if ((bus)->sleeping) \
-			dhdsdio_bussleep((bus), FALSE); \
-	} while (0);
-
-
-/* Writes a HW/SW header into the packet and sends it. */
-/* Assumes: (a) header space already there, (b) caller holds lock */
-static int
-dhdsdio_txpkt(dhd_bus_t *bus, void *pkt, uint chan, bool free_pkt)
-{
-	int ret;
-	osl_t *osh;
-	uint8 *frame;
-	uint16 len, pad1 = 0;
-	uint32 swheader;
-	uint retries = 0;
-	bcmsdh_info_t *sdh;
-	void *new;
-	int i;
-#ifdef WLMEDIA_HTSF
-	char *p;
-	htsfts_t *htsf_ts;
-#endif
-
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	sdh = bus->sdh;
-	osh = bus->dhd->osh;
-
-	if (bus->dhd->dongle_reset) {
-		ret = BCME_NOTREADY;
-		goto done;
-	}
-
-	frame = (uint8*)PKTDATA(osh, pkt);
-
-#ifdef WLMEDIA_HTSF
-	if (PKTLEN(osh, pkt) >= 100) {
-		p = PKTDATA(osh, pkt);
-		htsf_ts = (htsfts_t*) (p + HTSF_HOSTOFFSET + 12);
-		if (htsf_ts->magic == HTSFMAGIC) {
-			htsf_ts->c20 = get_cycles();
-			htsf_ts->t20 = dhd_get_htsf(bus->dhd->info, 0);
-		}
-	}
-#endif /* WLMEDIA_HTSF */
-
-	/* Add alignment padding, allocate new packet if needed */
-	if ((pad1 = ((uintptr)frame % DHD_SDALIGN))) {
-		if (PKTHEADROOM(osh, pkt) < pad1) {
-			DHD_INFO(("%s: insufficient headroom %d for %d pad1\n",
-			          __FUNCTION__, (int)PKTHEADROOM(osh, pkt), pad1));
-			bus->dhd->tx_realloc++;
-			new = PKTGET(osh, (PKTLEN(osh, pkt) + DHD_SDALIGN), TRUE);
-			if (!new) {
-				DHD_ERROR(("%s: couldn't allocate new %d-byte packet\n",
-				           __FUNCTION__, PKTLEN(osh, pkt) + DHD_SDALIGN));
-				ret = BCME_NOMEM;
-				goto done;
-			}
-
-			PKTALIGN(osh, new, PKTLEN(osh, pkt), DHD_SDALIGN);
-			bcopy(PKTDATA(osh, pkt), PKTDATA(osh, new), PKTLEN(osh, pkt));
-			if (free_pkt)
-				PKTFREE(osh, pkt, TRUE);
-			/* free the pkt if canned one is not used */
-			free_pkt = TRUE;
-			pkt = new;
-			frame = (uint8*)PKTDATA(osh, pkt);
-			ASSERT(((uintptr)frame % DHD_SDALIGN) == 0);
-			pad1 = 0;
-		} else {
-			PKTPUSH(osh, pkt, pad1);
-			frame = (uint8*)PKTDATA(osh, pkt);
-
-			ASSERT((pad1 + SDPCM_HDRLEN) <= (int) PKTLEN(osh, pkt));
-			bzero(frame, pad1 + SDPCM_HDRLEN);
-		}
-	}
-	ASSERT(pad1 < DHD_SDALIGN);
-
-	/* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
-	len = (uint16)PKTLEN(osh, pkt);
-	*(uint16*)frame = htol16(len);
-	*(((uint16*)frame) + 1) = htol16(~len);
-
-	/* Software tag: channel, sequence number, data offset */
-	swheader = ((chan << SDPCM_CHANNEL_SHIFT) & SDPCM_CHANNEL_MASK) | bus->tx_seq |
-	        (((pad1 + SDPCM_HDRLEN) << SDPCM_DOFFSET_SHIFT) & SDPCM_DOFFSET_MASK);
-	htol32_ua_store(swheader, frame + SDPCM_FRAMETAG_LEN);
-	htol32_ua_store(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
-
-#ifdef DHD_DEBUG
-	if (PKTPRIO(pkt) < ARRAYSIZE(tx_packets)) {
-		tx_packets[PKTPRIO(pkt)]++;
-	}
-	if (DHD_BYTES_ON() &&
-	    (((DHD_CTL_ON() && (chan == SDPCM_CONTROL_CHANNEL)) ||
-	      (DHD_DATA_ON() && (chan != SDPCM_CONTROL_CHANNEL))))) {
-		prhex("Tx Frame", frame, len);
-	} else if (DHD_HDRS_ON()) {
-		prhex("TxHdr", frame, MIN(len, 16));
-	}
-#endif
-
-	/* Raise len to next SDIO block to eliminate tail command */
-	if (bus->roundup && bus->blocksize && (len > bus->blocksize)) {
-		uint16 pad2 = bus->blocksize - (len % bus->blocksize);
-		if ((pad2 <= bus->roundup) && (pad2 < bus->blocksize))
-#ifdef NOTUSED
-			if (pad2 <= PKTTAILROOM(osh, pkt))
-#endif /* NOTUSED */
-				len += pad2;
-	} else if (len % DHD_SDALIGN) {
-		len += DHD_SDALIGN - (len % DHD_SDALIGN);
-	}
-
-	/* Some controllers have trouble with odd bytes -- round to even */
-	if (forcealign && (len & (ALIGNMENT - 1))) {
-#ifdef NOTUSED
-		if (PKTTAILROOM(osh, pkt))
-#endif
-			len = ROUNDUP(len, ALIGNMENT);
-#ifdef NOTUSED
-		else
-			DHD_ERROR(("%s: sending unrounded %d-byte packet\n", __FUNCTION__, len));
-#endif
-	}
-
-	do {
-		ret = dhd_bcmsdh_send_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
-		                          frame, len, pkt, NULL, NULL);
-		bus->f2txdata++;
-		ASSERT(ret != BCME_PENDING);
-
-		if (ret < 0) {
-			/* On failure, abort the command and terminate the frame */
-			DHD_INFO(("%s: sdio error %d, abort command and terminate frame.\n",
-			          __FUNCTION__, ret));
-			bus->tx_sderrs++;
-
-			bcmsdh_abort(sdh, SDIO_FUNC_2);
-			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL,
-			                 SFC_WF_TERM, NULL);
-			bus->f1regdata++;
-
-			for (i = 0; i < 3; i++) {
-				uint8 hi, lo;
-				hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
-				                     SBSDIO_FUNC1_WFRAMEBCHI, NULL);
-				lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
-				                     SBSDIO_FUNC1_WFRAMEBCLO, NULL);
-				bus->f1regdata += 2;
-				if ((hi == 0) && (lo == 0))
-					break;
-			}
-
-		}
-		if (ret == 0) {
-			bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
-		}
-	} while ((ret < 0) && retrydata && retries++ < TXRETRIES);
-
-done:
-	/* restore pkt buffer pointer before calling tx complete routine */
-	PKTPULL(osh, pkt, SDPCM_HDRLEN + pad1);
-#ifdef PROP_TXSTATUS
-	if (bus->dhd->wlfc_state) {
-		dhd_os_sdunlock(bus->dhd);
-		dhd_wlfc_txcomplete(bus->dhd, pkt, ret == 0);
-		dhd_os_sdlock(bus->dhd);
-	} else {
-#endif /* PROP_TXSTATUS */
-	dhd_txcomplete(bus->dhd, pkt, ret != 0);
-	if (free_pkt)
-		PKTFREE(osh, pkt, TRUE);
-
-#ifdef PROP_TXSTATUS
-	}
-#endif
-	return ret;
-}
-
-int
-dhd_bus_txdata(struct dhd_bus *bus, void *pkt)
-{
-	int ret = BCME_ERROR;
-	osl_t *osh;
-	uint datalen, prec;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	osh = bus->dhd->osh;
-	datalen = PKTLEN(osh, pkt);
-
-#ifdef SDTEST
-	/* Push the test header if doing loopback */
-	if (bus->ext_loop) {
-		uint8* data;
-		PKTPUSH(osh, pkt, SDPCM_TEST_HDRLEN);
-		data = PKTDATA(osh, pkt);
-		*data++ = SDPCM_TEST_ECHOREQ;
-		*data++ = (uint8)bus->loopid++;
-		*data++ = (datalen >> 0);
-		*data++ = (datalen >> 8);
-		datalen += SDPCM_TEST_HDRLEN;
-	}
-#endif /* SDTEST */
-
-	/* Add space for the header */
-	PKTPUSH(osh, pkt, SDPCM_HDRLEN);
-	ASSERT(ISALIGNED((uintptr)PKTDATA(osh, pkt), 2));
-
-	prec = PRIO2PREC((PKTPRIO(pkt) & PRIOMASK));
-#ifndef DHDTHREAD
-	/* Lock: we're about to use shared data/code (and SDIO) */
-	dhd_os_sdlock(bus->dhd);
-#endif /* DHDTHREAD */
-
-	/* Check for existing queue, current flow-control, pending event, or pending clock */
-	if (dhd_deferred_tx || bus->fcstate || pktq_len(&bus->txq) || bus->dpc_sched ||
-	    (!DATAOK(bus)) || (bus->flowcontrol & NBITVAL(prec)) ||
-	    (bus->clkstate != CLK_AVAIL)) {
-		DHD_TRACE(("%s: deferring pktq len %d\n", __FUNCTION__,
-			pktq_len(&bus->txq)));
-		bus->fcqueued++;
-
-		/* Priority based enq */
-		dhd_os_sdlock_txq(bus->dhd);
-		if (dhd_prec_enq(bus->dhd, &bus->txq, pkt, prec) == FALSE) {
-			PKTPULL(osh, pkt, SDPCM_HDRLEN);
-#ifndef DHDTHREAD
-			/* Need to also release txqlock before releasing sdlock.
-			 * This thread still has txqlock and releases sdlock.
-			 * Deadlock happens when dpc() grabs sdlock first then
-			 * attempts to grab txqlock.
-			 */
-			dhd_os_sdunlock_txq(bus->dhd);
-			dhd_os_sdunlock(bus->dhd);
-#endif
-#ifdef PROP_TXSTATUS
-			if (bus->dhd->wlfc_state)
-				dhd_wlfc_txcomplete(bus->dhd, pkt, FALSE);
-			else
-#endif
-			dhd_txcomplete(bus->dhd, pkt, FALSE);
-#ifndef DHDTHREAD
-			dhd_os_sdlock(bus->dhd);
-			dhd_os_sdlock_txq(bus->dhd);
-#endif
-#ifdef PROP_TXSTATUS
-			/* let the caller decide whether to free the packet */
-		if (!bus->dhd->wlfc_state)
-#endif
-			PKTFREE(osh, pkt, TRUE);
-			ret = BCME_NORESOURCE;
-		}
-		else
-			ret = BCME_OK;
-		dhd_os_sdunlock_txq(bus->dhd);
-
-		if ((pktq_len(&bus->txq) >= FCHI) && dhd_doflow)
-			dhd_txflowcontrol(bus->dhd, ALL_INTERFACES, ON);
-
-#ifdef DHD_DEBUG
-		if (pktq_plen(&bus->txq, prec) > qcount[prec])
-			qcount[prec] = pktq_plen(&bus->txq, prec);
-#endif
-		/* Schedule DPC if needed to send queued packet(s) */
-		if (dhd_deferred_tx && !bus->dpc_sched) {
-			bus->dpc_sched = TRUE;
-			dhd_sched_dpc(bus->dhd);
-		}
-	} else {
-#ifdef DHDTHREAD
-		/* Lock: we're about to use shared data/code (and SDIO) */
-		dhd_os_sdlock(bus->dhd);
-#endif /* DHDTHREAD */
-
-		/* Otherwise, send it now */
-		BUS_WAKE(bus);
-		/* Make sure back plane ht clk is on, no pending allowed */
-		dhdsdio_clkctl(bus, CLK_AVAIL, TRUE);
-#ifndef SDTEST
-		ret = dhdsdio_txpkt(bus, pkt, SDPCM_DATA_CHANNEL, TRUE);
-#else
-		ret = dhdsdio_txpkt(bus, pkt,
-		        (bus->ext_loop ? SDPCM_TEST_CHANNEL : SDPCM_DATA_CHANNEL), TRUE);
-#endif
-		if (ret)
-			bus->dhd->tx_errors++;
-		else
-			bus->dhd->dstats.tx_bytes += datalen;
-
-		if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
-			bus->activity = FALSE;
-			dhdsdio_clkctl(bus, CLK_NONE, TRUE);
-		}
-
-#ifdef DHDTHREAD
-		dhd_os_sdunlock(bus->dhd);
-#endif /* DHDTHREAD */
-	}
-
-#ifndef DHDTHREAD
-	dhd_os_sdunlock(bus->dhd);
-#endif /* DHDTHREAD */
-
-	return ret;
-}
-
-static uint
-dhdsdio_sendfromq(dhd_bus_t *bus, uint maxframes)
-{
-	void *pkt;
-	uint32 intstatus = 0;
-	uint retries = 0;
-	int ret = 0, prec_out;
-	uint cnt = 0;
-	uint datalen;
-	uint8 tx_prec_map;
-
-	dhd_pub_t *dhd = bus->dhd;
-	sdpcmd_regs_t *regs = bus->regs;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	tx_prec_map = ~bus->flowcontrol;
-
-	/* Send frames until the limit or some other event */
-	for (cnt = 0; (cnt < maxframes) && DATAOK(bus); cnt++) {
-		dhd_os_sdlock_txq(bus->dhd);
-		if ((pkt = pktq_mdeq(&bus->txq, tx_prec_map, &prec_out)) == NULL) {
-			dhd_os_sdunlock_txq(bus->dhd);
-			break;
-		}
-		dhd_os_sdunlock_txq(bus->dhd);
-		datalen = PKTLEN(bus->dhd->osh, pkt) - SDPCM_HDRLEN;
-
-#ifndef SDTEST
-		ret = dhdsdio_txpkt(bus, pkt, SDPCM_DATA_CHANNEL, TRUE);
-#else
-		ret = dhdsdio_txpkt(bus, pkt,
-		        (bus->ext_loop ? SDPCM_TEST_CHANNEL : SDPCM_DATA_CHANNEL), TRUE);
-#endif
-		if (ret)
-			bus->dhd->tx_errors++;
-		else
-			bus->dhd->dstats.tx_bytes += datalen;
-
-		/* In poll mode, need to check for other events */
-		if (!bus->intr && cnt)
-		{
-			/* Check device status, signal pending interrupt */
-			R_SDREG(intstatus, &regs->intstatus, retries);
-			bus->f2txdata++;
-			if (bcmsdh_regfail(bus->sdh))
-				break;
-			if (intstatus & bus->hostintmask)
-				bus->ipend = TRUE;
-		}
-	}
-
-	/* Deflow-control stack if needed */
-	if (dhd_doflow && dhd->up && (dhd->busstate == DHD_BUS_DATA) &&
-	    dhd->txoff && (pktq_len(&bus->txq) < FCLOW))
-		dhd_txflowcontrol(dhd, ALL_INTERFACES, OFF);
-
-	return cnt;
-}
-
-int
-dhd_bus_txctl(struct dhd_bus *bus, uchar *msg, uint msglen)
-{
-	uint8 *frame;
-	uint16 len;
-	uint32 swheader;
-	uint retries = 0;
-	bcmsdh_info_t *sdh = bus->sdh;
-	uint8 doff = 0;
-	int ret = -1;
-	int i;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (bus->dhd->dongle_reset)
-		return -EIO;
-
-	/* Back the pointer to make a room for bus header */
-	frame = msg - SDPCM_HDRLEN;
-	len = (msglen += SDPCM_HDRLEN);
-
-	/* Add alignment padding (optional for ctl frames) */
-	if (dhd_alignctl) {
-		if ((doff = ((uintptr)frame % DHD_SDALIGN))) {
-			frame -= doff;
-			len += doff;
-			msglen += doff;
-			bzero(frame, doff + SDPCM_HDRLEN);
-		}
-		ASSERT(doff < DHD_SDALIGN);
-	}
-	doff += SDPCM_HDRLEN;
-
-	/* Round send length to next SDIO block */
-	if (bus->roundup && bus->blocksize && (len > bus->blocksize)) {
-		uint16 pad = bus->blocksize - (len % bus->blocksize);
-		if ((pad <= bus->roundup) && (pad < bus->blocksize))
-			len += pad;
-	} else if (len % DHD_SDALIGN) {
-		len += DHD_SDALIGN - (len % DHD_SDALIGN);
-	}
-
-	/* Satisfy length-alignment requirements */
-	if (forcealign && (len & (ALIGNMENT - 1)))
-		len = ROUNDUP(len, ALIGNMENT);
-
-	ASSERT(ISALIGNED((uintptr)frame, 2));
-
-
-	/* Need to lock here to protect txseq and SDIO tx calls */
-	dhd_os_sdlock(bus->dhd);
-
-	BUS_WAKE(bus);
-
-	/* Make sure backplane clock is on */
-	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-
-	/* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
-	*(uint16*)frame = htol16((uint16)msglen);
-	*(((uint16*)frame) + 1) = htol16(~msglen);
-
-	/* Software tag: channel, sequence number, data offset */
-	swheader = ((SDPCM_CONTROL_CHANNEL << SDPCM_CHANNEL_SHIFT) & SDPCM_CHANNEL_MASK)
-	        | bus->tx_seq | ((doff << SDPCM_DOFFSET_SHIFT) & SDPCM_DOFFSET_MASK);
-	htol32_ua_store(swheader, frame + SDPCM_FRAMETAG_LEN);
-	htol32_ua_store(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
-
-	if (!TXCTLOK(bus)) {
-		DHD_INFO(("%s: No bus credit bus->tx_max %d, bus->tx_seq %d\n",
-			__FUNCTION__, bus->tx_max, bus->tx_seq));
-		bus->ctrl_frame_stat = TRUE;
-		/* Send from dpc */
-		bus->ctrl_frame_buf = frame;
-		bus->ctrl_frame_len = len;
-
-		dhd_wait_for_event(bus->dhd, &bus->ctrl_frame_stat);
-
-		if (bus->ctrl_frame_stat == FALSE) {
-			DHD_INFO(("%s: ctrl_frame_stat == FALSE\n", __FUNCTION__));
-			ret = 0;
-		} else {
-			DHD_ERROR(("%s: ctrl_frame_stat == TRUE\n", __FUNCTION__));
-			ret = -1;
-			bus->ctrl_frame_stat = FALSE;
-			goto done;
-		}
-	}
-
-	if (ret == -1) {
-#ifdef DHD_DEBUG
-		if (DHD_BYTES_ON() && DHD_CTL_ON()) {
-			prhex("Tx Frame", frame, len);
-		} else if (DHD_HDRS_ON()) {
-			prhex("TxHdr", frame, MIN(len, 16));
-		}
-#endif
-
-		do {
-			ret = dhd_bcmsdh_send_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
-			                          frame, len, NULL, NULL, NULL);
-			ASSERT(ret != BCME_PENDING);
-
-			if (ret < 0) {
-			/* On failure, abort the command and terminate the frame */
-				DHD_INFO(("%s: sdio error %d, abort command and terminate frame.\n",
-				          __FUNCTION__, ret));
-				bus->tx_sderrs++;
-
-				bcmsdh_abort(sdh, SDIO_FUNC_2);
-
-				bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL,
-				                 SFC_WF_TERM, NULL);
-				bus->f1regdata++;
-
-				for (i = 0; i < 3; i++) {
-					uint8 hi, lo;
-					hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
-					                     SBSDIO_FUNC1_WFRAMEBCHI, NULL);
-					lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
-					                     SBSDIO_FUNC1_WFRAMEBCLO, NULL);
-					bus->f1regdata += 2;
-					if ((hi == 0) && (lo == 0))
-						break;
-				}
-
-			}
-			if (ret == 0) {
-				bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
-			}
-		} while ((ret < 0) && retries++ < TXRETRIES);
-	}
-
-done:
-	if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
-		bus->activity = FALSE;
-		dhdsdio_clkctl(bus, CLK_NONE, TRUE);
-	}
-
-	dhd_os_sdunlock(bus->dhd);
-
-	if (ret)
-		bus->dhd->tx_ctlerrs++;
-	else
-		bus->dhd->tx_ctlpkts++;
-
-	return ret ? -EIO : 0;
-}
-
-int
-dhd_bus_rxctl(struct dhd_bus *bus, uchar *msg, uint msglen)
-{
-	int timeleft;
-	uint rxlen = 0;
-	bool pending;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (bus->dhd->dongle_reset)
-		return -EIO;
-
-	/* Wait until control frame is available */
-	timeleft = dhd_os_ioctl_resp_wait(bus->dhd, &bus->rxlen, &pending);
-
-	dhd_os_sdlock(bus->dhd);
-	rxlen = bus->rxlen;
-	bcopy(bus->rxctl, msg, MIN(msglen, rxlen));
-	bus->rxlen = 0;
-	dhd_os_sdunlock(bus->dhd);
-
-	if (rxlen) {
-		DHD_CTL(("%s: resumed on rxctl frame, got %d expected %d\n",
-		         __FUNCTION__, rxlen, msglen));
-	} else if (timeleft == 0) {
-		DHD_ERROR(("%s: resumed on timeout\n", __FUNCTION__));
-#ifdef DHD_DEBUG
-		dhd_os_sdlock(bus->dhd);
-		dhdsdio_checkdied(bus, NULL, 0);
-		dhd_os_sdunlock(bus->dhd);
-#endif /* DHD_DEBUG */
-	} else if (pending == TRUE) {
-		DHD_CTL(("%s: canceled\n", __FUNCTION__));
-		return -ERESTARTSYS;
-	} else {
-		DHD_CTL(("%s: resumed for unknown reason?\n", __FUNCTION__));
-#ifdef DHD_DEBUG
-		dhd_os_sdlock(bus->dhd);
-		dhdsdio_checkdied(bus, NULL, 0);
-		dhd_os_sdunlock(bus->dhd);
-#endif /* DHD_DEBUG */
-	}
-
-	if (rxlen)
-		bus->dhd->rx_ctlpkts++;
-	else
-		bus->dhd->rx_ctlerrs++;
-
-	return rxlen ? (int)rxlen : -ETIMEDOUT;
-}
-
-/* IOVar table */
-enum {
-	IOV_INTR = 1,
-	IOV_POLLRATE,
-	IOV_SDREG,
-	IOV_SBREG,
-	IOV_SDCIS,
-	IOV_MEMBYTES,
-	IOV_MEMSIZE,
-#ifdef DHD_DEBUG
-	IOV_CHECKDIED,
-	IOV_SERIALCONS,
-#endif
-	IOV_DOWNLOAD,
-	IOV_SOCRAM_STATE,
-	IOV_FORCEEVEN,
-	IOV_SDIOD_DRIVE,
-	IOV_READAHEAD,
-	IOV_SDRXCHAIN,
-	IOV_ALIGNCTL,
-	IOV_SDALIGN,
-	IOV_DEVRESET,
-	IOV_CPU,
-#ifdef SDTEST
-	IOV_PKTGEN,
-	IOV_EXTLOOP,
-#endif /* SDTEST */
-	IOV_SPROM,
-	IOV_TXBOUND,
-	IOV_RXBOUND,
-	IOV_TXMINMAX,
-	IOV_IDLETIME,
-	IOV_IDLECLOCK,
-	IOV_SD1IDLE,
-	IOV_SLEEP,
-	IOV_DONGLEISOLATION,
-	IOV_VARS
-#ifdef SOFTAP
-    , IOV_FWPATH
-#endif
-};
-
-const bcm_iovar_t dhdsdio_iovars[] = {
-	{"intr",	IOV_INTR,	0,	IOVT_BOOL,	0 },
-	{"sleep",	IOV_SLEEP,	0,	IOVT_BOOL,	0 },
-	{"pollrate",	IOV_POLLRATE,	0,	IOVT_UINT32,	0 },
-	{"idletime",	IOV_IDLETIME,	0,	IOVT_INT32,	0 },
-	{"idleclock",	IOV_IDLECLOCK,	0,	IOVT_INT32,	0 },
-	{"sd1idle",	IOV_SD1IDLE,	0,	IOVT_BOOL,	0 },
-	{"membytes",	IOV_MEMBYTES,	0,	IOVT_BUFFER,	2 * sizeof(int) },
-	{"memsize",	IOV_MEMSIZE,	0,	IOVT_UINT32,	0 },
-	{"download",	IOV_DOWNLOAD,	0,	IOVT_BOOL,	0 },
-	{"socram_state",	IOV_SOCRAM_STATE,	0,	IOVT_BOOL,	0 },
-	{"vars",	IOV_VARS,	0,	IOVT_BUFFER,	0 },
-	{"sdiod_drive",	IOV_SDIOD_DRIVE, 0,	IOVT_UINT32,	0 },
-	{"readahead",	IOV_READAHEAD,	0,	IOVT_BOOL,	0 },
-	{"sdrxchain",	IOV_SDRXCHAIN,	0,	IOVT_BOOL,	0 },
-	{"alignctl",	IOV_ALIGNCTL,	0,	IOVT_BOOL,	0 },
-	{"sdalign",	IOV_SDALIGN,	0,	IOVT_BOOL,	0 },
-	{"devreset",	IOV_DEVRESET,	0,	IOVT_BOOL,	0 },
-#ifdef DHD_DEBUG
-	{"sdreg",	IOV_SDREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
-	{"sbreg",	IOV_SBREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
-	{"sd_cis",	IOV_SDCIS,	0,	IOVT_BUFFER,	DHD_IOCTL_MAXLEN },
-	{"forcealign",	IOV_FORCEEVEN,	0,	IOVT_BOOL,	0 },
-	{"txbound",	IOV_TXBOUND,	0,	IOVT_UINT32,	0 },
-	{"rxbound",	IOV_RXBOUND,	0,	IOVT_UINT32,	0 },
-	{"txminmax",	IOV_TXMINMAX,	0,	IOVT_UINT32,	0 },
-	{"cpu",		IOV_CPU,	0,	IOVT_BOOL,	0 },
-#ifdef DHD_DEBUG
-	{"checkdied",	IOV_CHECKDIED,	0,	IOVT_BUFFER,	0 },
-#endif /* DHD_DEBUG  */
-#endif /* DHD_DEBUG */
-#ifdef SDTEST
-	{"extloop",	IOV_EXTLOOP,	0,	IOVT_BOOL,	0 },
-	{"pktgen",	IOV_PKTGEN,	0,	IOVT_BUFFER,	sizeof(dhd_pktgen_t) },
-#endif /* SDTEST */
-	{"dngl_isolation", IOV_DONGLEISOLATION,	0,	IOVT_UINT32,	0 },
-#ifdef SOFTAP
-	{"fwpath", IOV_FWPATH, 0, IOVT_BUFFER, 0 },
-#endif
-	{NULL, 0, 0, 0, 0 }
-};
-
-static void
-dhd_dump_pct(struct bcmstrbuf *strbuf, char *desc, uint num, uint div)
-{
-	uint q1, q2;
-
-	if (!div) {
-		bcm_bprintf(strbuf, "%s N/A", desc);
-	} else {
-		q1 = num / div;
-		q2 = (100 * (num - (q1 * div))) / div;
-		bcm_bprintf(strbuf, "%s %d.%02d", desc, q1, q2);
-	}
-}
-
-void
-dhd_bus_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf)
-{
-	dhd_bus_t *bus = dhdp->bus;
-
-	bcm_bprintf(strbuf, "Bus SDIO structure:\n");
-	bcm_bprintf(strbuf, "hostintmask 0x%08x intstatus 0x%08x sdpcm_ver %d\n",
-	            bus->hostintmask, bus->intstatus, bus->sdpcm_ver);
-	bcm_bprintf(strbuf, "fcstate %d qlen %d tx_seq %d, max %d, rxskip %d rxlen %d rx_seq %d\n",
-	            bus->fcstate, pktq_len(&bus->txq), bus->tx_seq, bus->tx_max, bus->rxskip,
-	            bus->rxlen, bus->rx_seq);
-	bcm_bprintf(strbuf, "intr %d intrcount %d lastintrs %d spurious %d\n",
-	            bus->intr, bus->intrcount, bus->lastintrs, bus->spurious);
-	bcm_bprintf(strbuf, "pollrate %d pollcnt %d regfails %d\n",
-	            bus->pollrate, bus->pollcnt, bus->regfails);
-
-	bcm_bprintf(strbuf, "\nAdditional counters:\n");
-	bcm_bprintf(strbuf, "tx_sderrs %d fcqueued %d rxrtx %d rx_toolong %d rxc_errors %d\n",
-	            bus->tx_sderrs, bus->fcqueued, bus->rxrtx, bus->rx_toolong,
-	            bus->rxc_errors);
-	bcm_bprintf(strbuf, "rx_hdrfail %d badhdr %d badseq %d\n",
-	            bus->rx_hdrfail, bus->rx_badhdr, bus->rx_badseq);
-	bcm_bprintf(strbuf, "fc_rcvd %d, fc_xoff %d, fc_xon %d\n",
-	            bus->fc_rcvd, bus->fc_xoff, bus->fc_xon);
-	bcm_bprintf(strbuf, "rxglomfail %d, rxglomframes %d, rxglompkts %d\n",
-	            bus->rxglomfail, bus->rxglomframes, bus->rxglompkts);
-	bcm_bprintf(strbuf, "f2rx (hdrs/data) %d (%d/%d), f2tx %d f1regs %d\n",
-	            (bus->f2rxhdrs + bus->f2rxdata), bus->f2rxhdrs, bus->f2rxdata,
-	            bus->f2txdata, bus->f1regdata);
-	{
-		dhd_dump_pct(strbuf, "\nRx: pkts/f2rd", bus->dhd->rx_packets,
-		             (bus->f2rxhdrs + bus->f2rxdata));
-		dhd_dump_pct(strbuf, ", pkts/f1sd", bus->dhd->rx_packets, bus->f1regdata);
-		dhd_dump_pct(strbuf, ", pkts/sd", bus->dhd->rx_packets,
-		             (bus->f2rxhdrs + bus->f2rxdata + bus->f1regdata));
-		dhd_dump_pct(strbuf, ", pkts/int", bus->dhd->rx_packets, bus->intrcount);
-		bcm_bprintf(strbuf, "\n");
-
-		dhd_dump_pct(strbuf, "Rx: glom pct", (100 * bus->rxglompkts),
-		             bus->dhd->rx_packets);
-		dhd_dump_pct(strbuf, ", pkts/glom", bus->rxglompkts, bus->rxglomframes);
-		bcm_bprintf(strbuf, "\n");
-
-		dhd_dump_pct(strbuf, "Tx: pkts/f2wr", bus->dhd->tx_packets, bus->f2txdata);
-		dhd_dump_pct(strbuf, ", pkts/f1sd", bus->dhd->tx_packets, bus->f1regdata);
-		dhd_dump_pct(strbuf, ", pkts/sd", bus->dhd->tx_packets,
-		             (bus->f2txdata + bus->f1regdata));
-		dhd_dump_pct(strbuf, ", pkts/int", bus->dhd->tx_packets, bus->intrcount);
-		bcm_bprintf(strbuf, "\n");
-
-		dhd_dump_pct(strbuf, "Total: pkts/f2rw",
-		             (bus->dhd->tx_packets + bus->dhd->rx_packets),
-		             (bus->f2txdata + bus->f2rxhdrs + bus->f2rxdata));
-		dhd_dump_pct(strbuf, ", pkts/f1sd",
-		             (bus->dhd->tx_packets + bus->dhd->rx_packets), bus->f1regdata);
-		dhd_dump_pct(strbuf, ", pkts/sd",
-		             (bus->dhd->tx_packets + bus->dhd->rx_packets),
-		             (bus->f2txdata + bus->f2rxhdrs + bus->f2rxdata + bus->f1regdata));
-		dhd_dump_pct(strbuf, ", pkts/int",
-		             (bus->dhd->tx_packets + bus->dhd->rx_packets), bus->intrcount);
-		bcm_bprintf(strbuf, "\n\n");
-	}
-
-#ifdef SDTEST
-	if (bus->pktgen_count) {
-		bcm_bprintf(strbuf, "pktgen config and count:\n");
-		bcm_bprintf(strbuf, "freq %d count %d print %d total %d min %d len %d\n",
-		            bus->pktgen_freq, bus->pktgen_count, bus->pktgen_print,
-		            bus->pktgen_total, bus->pktgen_minlen, bus->pktgen_maxlen);
-		bcm_bprintf(strbuf, "send attempts %d rcvd %d fail %d\n",
-		            bus->pktgen_sent, bus->pktgen_rcvd, bus->pktgen_fail);
-	}
-#endif /* SDTEST */
-#ifdef DHD_DEBUG
-	bcm_bprintf(strbuf, "dpc_sched %d host interrupt%spending\n",
-	            bus->dpc_sched, (bcmsdh_intr_pending(bus->sdh) ? " " : " not "));
-	bcm_bprintf(strbuf, "blocksize %d roundup %d\n", bus->blocksize, bus->roundup);
-#endif /* DHD_DEBUG */
-	bcm_bprintf(strbuf, "clkstate %d activity %d idletime %d idlecount %d sleeping %d\n",
-	            bus->clkstate, bus->activity, bus->idletime, bus->idlecount, bus->sleeping);
-}
-
-void
-dhd_bus_clearcounts(dhd_pub_t *dhdp)
-{
-	dhd_bus_t *bus = (dhd_bus_t *)dhdp->bus;
-
-	bus->intrcount = bus->lastintrs = bus->spurious = bus->regfails = 0;
-	bus->rxrtx = bus->rx_toolong = bus->rxc_errors = 0;
-	bus->rx_hdrfail = bus->rx_badhdr = bus->rx_badseq = 0;
-	bus->tx_sderrs = bus->fc_rcvd = bus->fc_xoff = bus->fc_xon = 0;
-	bus->rxglomfail = bus->rxglomframes = bus->rxglompkts = 0;
-	bus->f2rxhdrs = bus->f2rxdata = bus->f2txdata = bus->f1regdata = 0;
-}
-
-#ifdef SDTEST
-static int
-dhdsdio_pktgen_get(dhd_bus_t *bus, uint8 *arg)
-{
-	dhd_pktgen_t pktgen;
-
-	pktgen.version = DHD_PKTGEN_VERSION;
-	pktgen.freq = bus->pktgen_freq;
-	pktgen.count = bus->pktgen_count;
-	pktgen.print = bus->pktgen_print;
-	pktgen.total = bus->pktgen_total;
-	pktgen.minlen = bus->pktgen_minlen;
-	pktgen.maxlen = bus->pktgen_maxlen;
-	pktgen.numsent = bus->pktgen_sent;
-	pktgen.numrcvd = bus->pktgen_rcvd;
-	pktgen.numfail = bus->pktgen_fail;
-	pktgen.mode = bus->pktgen_mode;
-	pktgen.stop = bus->pktgen_stop;
-
-	bcopy(&pktgen, arg, sizeof(pktgen));
-
-	return 0;
-}
-
-static int
-dhdsdio_pktgen_set(dhd_bus_t *bus, uint8 *arg)
-{
-	dhd_pktgen_t pktgen;
-	uint oldcnt, oldmode;
-
-	bcopy(arg, &pktgen, sizeof(pktgen));
-	if (pktgen.version != DHD_PKTGEN_VERSION)
-		return BCME_BADARG;
-
-	oldcnt = bus->pktgen_count;
-	oldmode = bus->pktgen_mode;
-
-	bus->pktgen_freq = pktgen.freq;
-	bus->pktgen_count = pktgen.count;
-	bus->pktgen_print = pktgen.print;
-	bus->pktgen_total = pktgen.total;
-	bus->pktgen_minlen = pktgen.minlen;
-	bus->pktgen_maxlen = pktgen.maxlen;
-	bus->pktgen_mode = pktgen.mode;
-	bus->pktgen_stop = pktgen.stop;
-
-	bus->pktgen_tick = bus->pktgen_ptick = 0;
-	bus->pktgen_len = MAX(bus->pktgen_len, bus->pktgen_minlen);
-	bus->pktgen_len = MIN(bus->pktgen_len, bus->pktgen_maxlen);
-
-	/* Clear counts for a new pktgen (mode change, or was stopped) */
-	if (bus->pktgen_count && (!oldcnt || oldmode != bus->pktgen_mode))
-		bus->pktgen_sent = bus->pktgen_rcvd = bus->pktgen_fail = 0;
-
-	return 0;
-}
-#endif /* SDTEST */
-
-static int
-dhdsdio_membytes(dhd_bus_t *bus, bool write, uint32 address, uint8 *data, uint size)
-{
-	int bcmerror = 0;
-	uint32 sdaddr;
-	uint dsize;
-
-	/* Determine initial transfer parameters */
-	sdaddr = address & SBSDIO_SB_OFT_ADDR_MASK;
-	if ((sdaddr + size) & SBSDIO_SBWINDOW_MASK)
-		dsize = (SBSDIO_SB_OFT_ADDR_LIMIT - sdaddr);
-	else
-		dsize = size;
-
-	/* Set the backplane window to include the start address */
-	if ((bcmerror = dhdsdio_set_siaddr_window(bus, address))) {
-		DHD_ERROR(("%s: window change failed\n", __FUNCTION__));
-		goto xfer_done;
-	}
-
-	/* Do the transfer(s) */
-	while (size) {
-		DHD_INFO(("%s: %s %d bytes at offset 0x%08x in window 0x%08x\n",
-		          __FUNCTION__, (write ? "write" : "read"), dsize, sdaddr,
-		          (address & SBSDIO_SBWINDOW_MASK)));
-		if ((bcmerror = bcmsdh_rwdata(bus->sdh, write, sdaddr, data, dsize))) {
-			DHD_ERROR(("%s: membytes transfer failed\n", __FUNCTION__));
-			break;
-		}
-
-		/* Adjust for next transfer (if any) */
-		if ((size -= dsize)) {
-			data += dsize;
-			address += dsize;
-			if ((bcmerror = dhdsdio_set_siaddr_window(bus, address))) {
-				DHD_ERROR(("%s: window change failed\n", __FUNCTION__));
-				break;
-			}
-			sdaddr = 0;
-			dsize = MIN(SBSDIO_SB_OFT_ADDR_LIMIT, size);
-		}
-
-	}
-
-xfer_done:
-	/* Return the window to backplane enumeration space for core access */
-	if (dhdsdio_set_siaddr_window(bus, bcmsdh_cur_sbwad(bus->sdh))) {
-		DHD_ERROR(("%s: FAILED to set window back to 0x%x\n", __FUNCTION__,
-			bcmsdh_cur_sbwad(bus->sdh)));
-	}
-
-	return bcmerror;
-}
-
-#ifdef DHD_DEBUG
-static int
-dhdsdio_readshared(dhd_bus_t *bus, sdpcm_shared_t *sh)
-{
-	uint32 addr;
-	int rv;
-
-	/* Read last word in memory to determine address of sdpcm_shared structure */
-	if ((rv = dhdsdio_membytes(bus, FALSE, bus->ramsize - 4, (uint8 *)&addr, 4)) < 0)
-		return rv;
-
-	addr = ltoh32(addr);
-
-	DHD_INFO(("sdpcm_shared address 0x%08X\n", addr));
-
-	/*
-	 * Check if addr is valid.
-	 * NVRAM length at the end of memory should have been overwritten.
-	 */
-	if (addr == 0 || ((~addr >> 16) & 0xffff) == (addr & 0xffff)) {
-		DHD_ERROR(("%s: address (0x%08x) of sdpcm_shared invalid\n", __FUNCTION__, addr));
-		return BCME_ERROR;
-	}
-
-	/* Read hndrte_shared structure */
-	if ((rv = dhdsdio_membytes(bus, FALSE, addr, (uint8 *)sh, sizeof(sdpcm_shared_t))) < 0)
-		return rv;
-
-	/* Endianness */
-	sh->flags = ltoh32(sh->flags);
-	sh->trap_addr = ltoh32(sh->trap_addr);
-	sh->assert_exp_addr = ltoh32(sh->assert_exp_addr);
-	sh->assert_file_addr = ltoh32(sh->assert_file_addr);
-	sh->assert_line = ltoh32(sh->assert_line);
-	sh->console_addr = ltoh32(sh->console_addr);
-	sh->msgtrace_addr = ltoh32(sh->msgtrace_addr);
-
-	if ((sh->flags & SDPCM_SHARED_VERSION_MASK) == 3 && SDPCM_SHARED_VERSION == 1)
-		return BCME_OK;
-	if ((sh->flags & SDPCM_SHARED_VERSION_MASK) != SDPCM_SHARED_VERSION) {
-		DHD_ERROR(("%s: sdpcm_shared version %d in dhd "
-		           "is different than sdpcm_shared version %d in dongle\n",
-		           __FUNCTION__, SDPCM_SHARED_VERSION,
-		           sh->flags & SDPCM_SHARED_VERSION_MASK));
-		return BCME_ERROR;
-	}
-
-	return BCME_OK;
-}
-
-#define CONSOLE_LINE_MAX	192
-
-static int
-dhdsdio_readconsole(dhd_bus_t *bus)
-{
-	dhd_console_t *c = &bus->console;
-	uint8 line[CONSOLE_LINE_MAX], ch;
-	uint32 n, idx, addr;
-	int rv;
-
-	/* Don't do anything until FWREADY updates console address */
-	if (bus->console_addr == 0)
-		return 0;
-
-	/* Read console log struct */
-	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, log);
-	if ((rv = dhdsdio_membytes(bus, FALSE, addr, (uint8 *)&c->log, sizeof(c->log))) < 0)
-		return rv;
-
-	/* Allocate console buffer (one time only) */
-	if (c->buf == NULL) {
-		c->bufsize = ltoh32(c->log.buf_size);
-		if ((c->buf = MALLOC(bus->dhd->osh, c->bufsize)) == NULL)
-			return BCME_NOMEM;
-	}
-
-	idx = ltoh32(c->log.idx);
-
-	/* Protect against corrupt value */
-	if (idx > c->bufsize)
-		return BCME_ERROR;
-
-	/* Skip reading the console buffer if the index pointer has not moved */
-	if (idx == c->last)
-		return BCME_OK;
-
-	/* Read the console buffer */
-	addr = ltoh32(c->log.buf);
-	if ((rv = dhdsdio_membytes(bus, FALSE, addr, c->buf, c->bufsize)) < 0)
-		return rv;
-
-	while (c->last != idx) {
-		for (n = 0; n < CONSOLE_LINE_MAX - 2; n++) {
-			if (c->last == idx) {
-				/* This would output a partial line.  Instead, back up
-				 * the buffer pointer and output this line next time around.
-				 */
-				if (c->last >= n)
-					c->last -= n;
-				else
-					c->last = c->bufsize - n;
-				goto break2;
-			}
-			ch = c->buf[c->last];
-			c->last = (c->last + 1) % c->bufsize;
-			if (ch == '\n')
-				break;
-			line[n] = ch;
-		}
-
-		if (n > 0) {
-			if (line[n - 1] == '\r')
-				n--;
-			line[n] = 0;
-			printf("CONSOLE: %s\n", line);
-		}
-	}
-break2:
-
-	return BCME_OK;
-}
-
-static int
-dhdsdio_checkdied(dhd_bus_t *bus, uint8 *data, uint size)
-{
-	int bcmerror = 0;
-	uint msize = 512;
-	char *mbuffer = NULL;
-	uint maxstrlen = 256;
-	char *str = NULL;
-	trap_t tr;
-	sdpcm_shared_t sdpcm_shared;
-	struct bcmstrbuf strbuf;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (data == NULL) {
-		/*
-		 * Called after a rx ctrl timeout. "data" is NULL.
-		 * allocate memory to trace the trap or assert.
-		 */
-		size = msize;
-		mbuffer = data = MALLOC(bus->dhd->osh, msize);
-		if (mbuffer == NULL) {
-			DHD_ERROR(("%s: MALLOC(%d) failed \n", __FUNCTION__, msize));
-			bcmerror = BCME_NOMEM;
-			goto done;
-		}
-	}
-
-	if ((str = MALLOC(bus->dhd->osh, maxstrlen)) == NULL) {
-		DHD_ERROR(("%s: MALLOC(%d) failed \n", __FUNCTION__, maxstrlen));
-		bcmerror = BCME_NOMEM;
-		goto done;
-	}
-
-	if ((bcmerror = dhdsdio_readshared(bus, &sdpcm_shared)) < 0)
-		goto done;
-
-	bcm_binit(&strbuf, data, size);
-
-	bcm_bprintf(&strbuf, "msgtrace address : 0x%08X\nconsole address  : 0x%08X\n",
-	            sdpcm_shared.msgtrace_addr, sdpcm_shared.console_addr);
-
-	if ((sdpcm_shared.flags & SDPCM_SHARED_ASSERT_BUILT) == 0) {
-		/* NOTE: Misspelled assert is intentional - DO NOT FIX.
-		 * (Avoids conflict with real asserts for programmatic parsing of output.)
-		 */
-		bcm_bprintf(&strbuf, "Assrt not built in dongle\n");
-	}
-
-	if ((sdpcm_shared.flags & (SDPCM_SHARED_ASSERT|SDPCM_SHARED_TRAP)) == 0) {
-		/* NOTE: Misspelled assert is intentional - DO NOT FIX.
-		 * (Avoids conflict with real asserts for programmatic parsing of output.)
-		 */
-		bcm_bprintf(&strbuf, "No trap%s in dongle",
-		          (sdpcm_shared.flags & SDPCM_SHARED_ASSERT_BUILT)
-		          ?"/assrt" :"");
-	} else {
-		if (sdpcm_shared.flags & SDPCM_SHARED_ASSERT) {
-			/* Download assert */
-			bcm_bprintf(&strbuf, "Dongle assert");
-			if (sdpcm_shared.assert_exp_addr != 0) {
-				str[0] = '\0';
-				if ((bcmerror = dhdsdio_membytes(bus, FALSE,
-				                                 sdpcm_shared.assert_exp_addr,
-				                                 (uint8 *)str, maxstrlen)) < 0)
-					goto done;
-
-				str[maxstrlen - 1] = '\0';
-				bcm_bprintf(&strbuf, " expr \"%s\"", str);
-			}
-
-			if (sdpcm_shared.assert_file_addr != 0) {
-				str[0] = '\0';
-				if ((bcmerror = dhdsdio_membytes(bus, FALSE,
-				                                 sdpcm_shared.assert_file_addr,
-				                                 (uint8 *)str, maxstrlen)) < 0)
-					goto done;
-
-				str[maxstrlen - 1] = '\0';
-				bcm_bprintf(&strbuf, " file \"%s\"", str);
-			}
-
-			bcm_bprintf(&strbuf, " line %d ", sdpcm_shared.assert_line);
-		}
-
-		if (sdpcm_shared.flags & SDPCM_SHARED_TRAP) {
-			if ((bcmerror = dhdsdio_membytes(bus, FALSE,
-			                                 sdpcm_shared.trap_addr,
-			                                 (uint8*)&tr, sizeof(trap_t))) < 0)
-				goto done;
-
-			bcm_bprintf(&strbuf,
-			"Dongle trap type 0x%x @ epc 0x%x, cpsr 0x%x, spsr 0x%x, sp 0x%x,"
-			"lp 0x%x, rpc 0x%x Trap offset 0x%x, "
-			"r0 0x%x, r1 0x%x, r2 0x%x, r3 0x%x, r4 0x%x, r5 0x%x, r6 0x%x, r7 0x%x\n",
-			ltoh32(tr.type), ltoh32(tr.epc), ltoh32(tr.cpsr), ltoh32(tr.spsr),
-			ltoh32(tr.r13), ltoh32(tr.r14), ltoh32(tr.pc),
-			ltoh32(sdpcm_shared.trap_addr),
-			ltoh32(tr.r0), ltoh32(tr.r1), ltoh32(tr.r2), ltoh32(tr.r3),
-			ltoh32(tr.r4), ltoh32(tr.r5), ltoh32(tr.r6), ltoh32(tr.r7));
-		}
-	}
-
-	if (sdpcm_shared.flags & (SDPCM_SHARED_ASSERT | SDPCM_SHARED_TRAP)) {
-		DHD_ERROR(("%s: %s\n", __FUNCTION__, strbuf.origbuf));
-	}
-
-#ifdef DHD_DEBUG
-	if (sdpcm_shared.flags & SDPCM_SHARED_TRAP) {
-		/* Mem dump to a file on device */
-		dhdsdio_mem_dump(bus);
-	}
-#endif /* DHD_DEBUG */
-
-done:
-	if (mbuffer)
-		MFREE(bus->dhd->osh, mbuffer, msize);
-	if (str)
-		MFREE(bus->dhd->osh, str, maxstrlen);
-
-	return bcmerror;
-}
-
-static int
-dhdsdio_mem_dump(dhd_bus_t *bus)
-{
-	int ret = 0;
-	int size; /* Full mem size */
-	int start = 0; /* Start address */
-	int read_size = 0; /* Read size of each iteration */
-	uint8 *buf = NULL, *databuf = NULL;
-
-	/* Get full mem size */
-	size = bus->ramsize;
-	buf = MALLOC(bus->dhd->osh, size);
-	if (!buf) {
-		printf("%s: Out of memory (%d bytes)\n", __FUNCTION__, size);
-		return -1;
-	}
-
-	/* Read mem content */
-	printf("Dump dongle memory");
-	databuf = buf;
-	while (size)
-	{
-		read_size = MIN(MEMBLOCK, size);
-		if ((ret = dhdsdio_membytes(bus, FALSE, start, databuf, read_size)))
-		{
-			printf("%s: Error membytes %d\n", __FUNCTION__, ret);
-			if (buf) {
-				MFREE(bus->dhd->osh, buf, size);
-			}
-			return -1;
-		}
-		printf(".");
-
-		/* Decrement size and increment start address */
-		size -= read_size;
-		start += read_size;
-		databuf += read_size;
-	}
-	printf("Done\n");
-
-	/* free buf before return !!! */
-	if (write_to_file(bus->dhd, buf, bus->ramsize))
-	{
-		printf("%s: Error writing to files\n", __FUNCTION__);
-		return -1;
-	}
-
-	/* buf free handled in write_to_file, not here */
-	return 0;
-}
-#endif /* defined(DHD_DEBUG) */
-
-int
-dhdsdio_downloadvars(dhd_bus_t *bus, void *arg, int len)
-{
-	int bcmerror = BCME_OK;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	/* Basic sanity checks */
-	if (bus->dhd->up) {
-		bcmerror = BCME_NOTDOWN;
-		goto err;
-	}
-	if (!len) {
-		bcmerror = BCME_BUFTOOSHORT;
-		goto err;
-	}
-
-	/* Free the old ones and replace with passed variables */
-	if (bus->vars)
-		MFREE(bus->dhd->osh, bus->vars, bus->varsz);
-
-	bus->vars = MALLOC(bus->dhd->osh, len);
-	bus->varsz = bus->vars ? len : 0;
-	if (bus->vars == NULL) {
-		bcmerror = BCME_NOMEM;
-		goto err;
-	}
-
-	/* Copy the passed variables, which should include the terminating double-null */
-	bcopy(arg, bus->vars, bus->varsz);
-err:
-	return bcmerror;
-}
-
-#ifdef DHD_DEBUG
-
-#define CC_PLL_CHIPCTRL_SERIAL_ENAB	(1  << 24)
-static int
-dhd_serialconsole(dhd_bus_t *bus, bool set, bool enable, int *bcmerror)
-{
-	int int_val;
-	uint32 addr, data;
-
-
-	addr = SI_ENUM_BASE + OFFSETOF(chipcregs_t, chipcontrol_addr);
-	data = SI_ENUM_BASE + OFFSETOF(chipcregs_t, chipcontrol_data);
-	*bcmerror = 0;
-
-	bcmsdh_reg_write(bus->sdh, addr, 4, 1);
-	if (bcmsdh_regfail(bus->sdh)) {
-		*bcmerror = BCME_SDIO_ERROR;
-		return -1;
-	}
-	int_val = bcmsdh_reg_read(bus->sdh, data, 4);
-	if (bcmsdh_regfail(bus->sdh)) {
-		*bcmerror = BCME_SDIO_ERROR;
-		return -1;
-	}
-	if (!set)
-		return (int_val & CC_PLL_CHIPCTRL_SERIAL_ENAB);
-	if (enable)
-		int_val |= CC_PLL_CHIPCTRL_SERIAL_ENAB;
-	else
-		int_val &= ~CC_PLL_CHIPCTRL_SERIAL_ENAB;
-	bcmsdh_reg_write(bus->sdh, data, 4, int_val);
-	if (bcmsdh_regfail(bus->sdh)) {
-		*bcmerror = BCME_SDIO_ERROR;
-		return -1;
-	}
-	if (bus->sih->chip == BCM4330_CHIP_ID) {
-		uint32 chipcontrol;
-		addr = SI_ENUM_BASE + OFFSETOF(chipcregs_t, chipcontrol);
-		chipcontrol = bcmsdh_reg_read(bus->sdh, addr, 4);
-		chipcontrol &= ~0x8;
-		if (enable) {
-			chipcontrol |=  0x8;
-			chipcontrol &= ~0x3;
-		}
-		bcmsdh_reg_write(bus->sdh, addr, 4, chipcontrol);
-	}
-
-	return (int_val & CC_PLL_CHIPCTRL_SERIAL_ENAB);
-}
-#endif 
-
-static int
-dhdsdio_doiovar(dhd_bus_t *bus, const bcm_iovar_t *vi, uint32 actionid, const char *name,
-                void *params, int plen, void *arg, int len, int val_size)
-{
-	int bcmerror = 0;
-	int32 int_val = 0;
-	bool bool_val = 0;
-
-	DHD_TRACE(("%s: Enter, action %d name %s params %p plen %d arg %p len %d val_size %d\n",
-	           __FUNCTION__, actionid, name, params, plen, arg, len, val_size));
-
-	if ((bcmerror = bcm_iovar_lencheck(vi, arg, len, IOV_ISSET(actionid))) != 0)
-		goto exit;
-
-	if (plen >= (int)sizeof(int_val))
-		bcopy(params, &int_val, sizeof(int_val));
-
-	bool_val = (int_val != 0) ? TRUE : FALSE;
-
-
-	/* Some ioctls use the bus */
-	dhd_os_sdlock(bus->dhd);
-
-	/* Check if dongle is in reset. If so, only allow DEVRESET iovars */
-	if (bus->dhd->dongle_reset && !(actionid == IOV_SVAL(IOV_DEVRESET) ||
-	                                actionid == IOV_GVAL(IOV_DEVRESET))) {
-		bcmerror = BCME_NOTREADY;
-		goto exit;
-	}
-
-	/* Handle sleep stuff before any clock mucking */
-	if (vi->varid == IOV_SLEEP) {
-		if (IOV_ISSET(actionid)) {
-			bcmerror = dhdsdio_bussleep(bus, bool_val);
-		} else {
-			int_val = (int32)bus->sleeping;
-			bcopy(&int_val, arg, val_size);
-		}
-		goto exit;
-	}
-
-	/* Request clock to allow SDIO accesses */
-	if (!bus->dhd->dongle_reset) {
-		BUS_WAKE(bus);
-		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-	}
-
-	switch (actionid) {
-	case IOV_GVAL(IOV_INTR):
-		int_val = (int32)bus->intr;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_INTR):
-		bus->intr = bool_val;
-		bus->intdis = FALSE;
-		if (bus->dhd->up) {
-			if (bus->intr) {
-				DHD_INTR(("%s: enable SDIO device interrupts\n", __FUNCTION__));
-				bcmsdh_intr_enable(bus->sdh);
-			} else {
-				DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
-				bcmsdh_intr_disable(bus->sdh);
-			}
-		}
-		break;
-
-	case IOV_GVAL(IOV_POLLRATE):
-		int_val = (int32)bus->pollrate;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_POLLRATE):
-		bus->pollrate = (uint)int_val;
-		bus->poll = (bus->pollrate != 0);
-		break;
-
-	case IOV_GVAL(IOV_IDLETIME):
-		int_val = bus->idletime;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_IDLETIME):
-		if ((int_val < 0) && (int_val != DHD_IDLE_IMMEDIATE)) {
-			bcmerror = BCME_BADARG;
-		} else {
-			bus->idletime = int_val;
-		}
-		break;
-
-	case IOV_GVAL(IOV_IDLECLOCK):
-		int_val = (int32)bus->idleclock;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_IDLECLOCK):
-		bus->idleclock = int_val;
-		break;
-
-	case IOV_GVAL(IOV_SD1IDLE):
-		int_val = (int32)sd1idle;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_SD1IDLE):
-		sd1idle = bool_val;
-		break;
-
-
-	case IOV_SVAL(IOV_MEMBYTES):
-	case IOV_GVAL(IOV_MEMBYTES):
-	{
-		uint32 address;
-		uint size, dsize;
-		uint8 *data;
-
-		bool set = (actionid == IOV_SVAL(IOV_MEMBYTES));
-
-		ASSERT(plen >= 2*sizeof(int));
-
-		address = (uint32)int_val;
-		bcopy((char *)params + sizeof(int_val), &int_val, sizeof(int_val));
-		size = (uint)int_val;
-
-		/* Do some validation */
-		dsize = set ? plen - (2 * sizeof(int)) : len;
-		if (dsize < size) {
-			DHD_ERROR(("%s: error on %s membytes, addr 0x%08x size %d dsize %d\n",
-			           __FUNCTION__, (set ? "set" : "get"), address, size, dsize));
-			bcmerror = BCME_BADARG;
-			break;
-		}
-
-		DHD_INFO(("%s: Request to %s %d bytes at address 0x%08x\n", __FUNCTION__,
-		          (set ? "write" : "read"), size, address));
-
-		/* If we know about SOCRAM, check for a fit */
-		if ((bus->orig_ramsize) &&
-		    ((address > bus->orig_ramsize) || (address + size > bus->orig_ramsize)))
-		{
-			uint8 enable, protect;
-			si_socdevram(bus->sih, FALSE, &enable, &protect);
-			if (!enable || protect) {
-				DHD_ERROR(("%s: ramsize 0x%08x doesn't have %d bytes at 0x%08x\n",
-					__FUNCTION__, bus->orig_ramsize, size, address));
-				DHD_ERROR(("%s: socram enable %d, protect %d\n",
-					__FUNCTION__, enable, protect));
-				bcmerror = BCME_BADARG;
-				break;
-			}
-			if (enable && (bus->sih->chip == BCM4330_CHIP_ID)) {
-				uint32 devramsize = si_socdevram_size(bus->sih);
-				if ((address < SOCDEVRAM_4330_ARM_ADDR) ||
-					(address + size > (SOCDEVRAM_4330_ARM_ADDR + devramsize))) {
-					DHD_ERROR(("%s: bad address 0x%08x, size 0x%08x\n",
-						__FUNCTION__, address, size));
-					DHD_ERROR(("%s: socram range 0x%08x,size 0x%08x\n",
-						__FUNCTION__, SOCDEVRAM_4330_ARM_ADDR, devramsize));
-					bcmerror = BCME_BADARG;
-					break;
-				}
-				/* move it such that address is real now */
-				address -= SOCDEVRAM_4330_ARM_ADDR;
-				address += SOCDEVRAM_4330_BP_ADDR;
-				DHD_INFO(("%s: Request to %s %d bytes @ Mapped address 0x%08x\n",
-					__FUNCTION__, (set ? "write" : "read"), size, address));
-			}
-		}
-
-		/* Generate the actual data pointer */
-		data = set ? (uint8*)params + 2 * sizeof(int): (uint8*)arg;
-
-		/* Call to do the transfer */
-		bcmerror = dhdsdio_membytes(bus, set, address, data, size);
-
-		break;
-	}
-
-	case IOV_GVAL(IOV_MEMSIZE):
-		int_val = (int32)bus->ramsize;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_GVAL(IOV_SDIOD_DRIVE):
-		int_val = (int32)dhd_sdiod_drive_strength;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_SDIOD_DRIVE):
-		dhd_sdiod_drive_strength = int_val;
-		si_sdiod_drive_strength_init(bus->sih, bus->dhd->osh, dhd_sdiod_drive_strength);
-		break;
-
-	case IOV_SVAL(IOV_DOWNLOAD):
-		bcmerror = dhdsdio_download_state(bus, bool_val);
-		break;
-
-	case IOV_SVAL(IOV_SOCRAM_STATE):
-		bcmerror = dhdsdio_download_state(bus, bool_val);
-		break;
-
-	case IOV_SVAL(IOV_VARS):
-		bcmerror = dhdsdio_downloadvars(bus, arg, len);
-		break;
-
-	case IOV_GVAL(IOV_READAHEAD):
-		int_val = (int32)dhd_readahead;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_READAHEAD):
-		if (bool_val && !dhd_readahead)
-			bus->nextlen = 0;
-		dhd_readahead = bool_val;
-		break;
-
-	case IOV_GVAL(IOV_SDRXCHAIN):
-		int_val = (int32)bus->use_rxchain;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_SDRXCHAIN):
-		if (bool_val && !bus->sd_rxchain)
-			bcmerror = BCME_UNSUPPORTED;
-		else
-			bus->use_rxchain = bool_val;
-		break;
-	case IOV_GVAL(IOV_ALIGNCTL):
-		int_val = (int32)dhd_alignctl;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_ALIGNCTL):
-		dhd_alignctl = bool_val;
-		break;
-
-	case IOV_GVAL(IOV_SDALIGN):
-		int_val = DHD_SDALIGN;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-#ifdef DHD_DEBUG
-	case IOV_GVAL(IOV_VARS):
-		if (bus->varsz < (uint)len)
-			bcopy(bus->vars, arg, bus->varsz);
-		else
-			bcmerror = BCME_BUFTOOSHORT;
-		break;
-#endif /* DHD_DEBUG */
-
-#ifdef DHD_DEBUG
-	case IOV_GVAL(IOV_SDREG):
-	{
-		sdreg_t *sd_ptr;
-		uint32 addr, size;
-
-		sd_ptr = (sdreg_t *)params;
-
-		addr = (uintptr)bus->regs + sd_ptr->offset;
-		size = sd_ptr->func;
-		int_val = (int32)bcmsdh_reg_read(bus->sdh, addr, size);
-		if (bcmsdh_regfail(bus->sdh))
-			bcmerror = BCME_SDIO_ERROR;
-		bcopy(&int_val, arg, sizeof(int32));
-		break;
-	}
-
-	case IOV_SVAL(IOV_SDREG):
-	{
-		sdreg_t *sd_ptr;
-		uint32 addr, size;
-
-		sd_ptr = (sdreg_t *)params;
-
-		addr = (uintptr)bus->regs + sd_ptr->offset;
-		size = sd_ptr->func;
-		bcmsdh_reg_write(bus->sdh, addr, size, sd_ptr->value);
-		if (bcmsdh_regfail(bus->sdh))
-			bcmerror = BCME_SDIO_ERROR;
-		break;
-	}
-
-	/* Same as above, but offset is not backplane (not SDIO core) */
-	case IOV_GVAL(IOV_SBREG):
-	{
-		sdreg_t sdreg;
-		uint32 addr, size;
-
-		bcopy(params, &sdreg, sizeof(sdreg));
-
-		addr = SI_ENUM_BASE + sdreg.offset;
-		size = sdreg.func;
-		int_val = (int32)bcmsdh_reg_read(bus->sdh, addr, size);
-		if (bcmsdh_regfail(bus->sdh))
-			bcmerror = BCME_SDIO_ERROR;
-		bcopy(&int_val, arg, sizeof(int32));
-		break;
-	}
-
-	case IOV_SVAL(IOV_SBREG):
-	{
-		sdreg_t sdreg;
-		uint32 addr, size;
-
-		bcopy(params, &sdreg, sizeof(sdreg));
-
-		addr = SI_ENUM_BASE + sdreg.offset;
-		size = sdreg.func;
-		bcmsdh_reg_write(bus->sdh, addr, size, sdreg.value);
-		if (bcmsdh_regfail(bus->sdh))
-			bcmerror = BCME_SDIO_ERROR;
-		break;
-	}
-
-	case IOV_GVAL(IOV_SDCIS):
-	{
-		*(char *)arg = 0;
-
-		bcmstrcat(arg, "\nFunc 0\n");
-		bcmsdh_cis_read(bus->sdh, 0x10, (uint8 *)arg + strlen(arg), SBSDIO_CIS_SIZE_LIMIT);
-		bcmstrcat(arg, "\nFunc 1\n");
-		bcmsdh_cis_read(bus->sdh, 0x11, (uint8 *)arg + strlen(arg), SBSDIO_CIS_SIZE_LIMIT);
-		bcmstrcat(arg, "\nFunc 2\n");
-		bcmsdh_cis_read(bus->sdh, 0x12, (uint8 *)arg + strlen(arg), SBSDIO_CIS_SIZE_LIMIT);
-		break;
-	}
-
-	case IOV_GVAL(IOV_FORCEEVEN):
-		int_val = (int32)forcealign;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_FORCEEVEN):
-		forcealign = bool_val;
-		break;
-
-	case IOV_GVAL(IOV_TXBOUND):
-		int_val = (int32)dhd_txbound;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_TXBOUND):
-		dhd_txbound = (uint)int_val;
-		break;
-
-	case IOV_GVAL(IOV_RXBOUND):
-		int_val = (int32)dhd_rxbound;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_RXBOUND):
-		dhd_rxbound = (uint)int_val;
-		break;
-
-	case IOV_GVAL(IOV_TXMINMAX):
-		int_val = (int32)dhd_txminmax;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_TXMINMAX):
-		dhd_txminmax = (uint)int_val;
-		break;
-
-	case IOV_GVAL(IOV_SERIALCONS):
-		int_val = dhd_serialconsole(bus, FALSE, 0, &bcmerror);
-		if (bcmerror != 0)
-			break;
-
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_SERIALCONS):
-		dhd_serialconsole(bus, TRUE, bool_val, &bcmerror);
-		break;
-
-
-
-#endif /* DHD_DEBUG */
-
-
-#ifdef SDTEST
-	case IOV_GVAL(IOV_EXTLOOP):
-		int_val = (int32)bus->ext_loop;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_EXTLOOP):
-		bus->ext_loop = bool_val;
-		break;
-
-	case IOV_GVAL(IOV_PKTGEN):
-		bcmerror = dhdsdio_pktgen_get(bus, arg);
-		break;
-
-	case IOV_SVAL(IOV_PKTGEN):
-		bcmerror = dhdsdio_pktgen_set(bus, arg);
-		break;
-#endif /* SDTEST */
-
-
-	case IOV_GVAL(IOV_DONGLEISOLATION):
-		int_val = bus->dhd->dongle_isolation;
-		bcopy(&int_val, arg, val_size);
-		break;
-
-	case IOV_SVAL(IOV_DONGLEISOLATION):
-		bus->dhd->dongle_isolation = bool_val;
-		break;
-
-	case IOV_SVAL(IOV_DEVRESET):
-		DHD_TRACE(("%s: Called set IOV_DEVRESET=%d dongle_reset=%d busstate=%d\n",
-		           __FUNCTION__, bool_val, bus->dhd->dongle_reset,
-		           bus->dhd->busstate));
-
-		ASSERT(bus->dhd->osh);
-		/* ASSERT(bus->cl_devid); */
-
-		dhd_bus_devreset(bus->dhd, (uint8)bool_val);
-
-		break;
-#ifdef SOFTAP
-	case IOV_GVAL(IOV_FWPATH):
-	{
-		uint32  fw_path_len;
-
-		fw_path_len = strlen(bus->fw_path);
-		DHD_INFO(("[softap] get fwpath, l=%d\n", len));
-
-		if (fw_path_len > len-1) {
-			bcmerror = BCME_BUFTOOSHORT;
-			break;
-		}
-
-		if (fw_path_len) {
-			bcopy(bus->fw_path, arg, fw_path_len);
-			((uchar*)arg)[fw_path_len] = 0;
-		}
-		break;
-	}
-
-	case IOV_SVAL(IOV_FWPATH):
-		DHD_INFO(("[softap] set fwpath, idx=%d\n", int_val));
-
-		switch (int_val) {
-		case 1:
-			bus->fw_path = fw_path; /* ordinary one */
-			break;
-		case 2:
-			bus->fw_path = fw_path2;
-			break;
-		default:
-			bcmerror = BCME_BADARG;
-			break;
-		}
-
-		DHD_INFO(("[softap] new fw path: %s\n", (bus->fw_path[0] ? bus->fw_path : "NULL")));
-		break;
-
-#endif /* SOFTAP */
-	case IOV_GVAL(IOV_DEVRESET):
-		DHD_TRACE(("%s: Called get IOV_DEVRESET\n", __FUNCTION__));
-
-		/* Get its status */
-		int_val = (bool) bus->dhd->dongle_reset;
-		bcopy(&int_val, arg, val_size);
-
-		break;
-
-	default:
-		bcmerror = BCME_UNSUPPORTED;
-		break;
-	}
-
-exit:
-	if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
-		bus->activity = FALSE;
-		dhdsdio_clkctl(bus, CLK_NONE, TRUE);
-	}
-
-	dhd_os_sdunlock(bus->dhd);
-
-	if (actionid == IOV_SVAL(IOV_DEVRESET) && bool_val == FALSE)
-		dhd_preinit_ioctls((dhd_pub_t *) bus->dhd);
-
-	return bcmerror;
-}
-
-static int
-dhdsdio_write_vars(dhd_bus_t *bus)
-{
-	int bcmerror = 0;
-	uint32 varsize;
-	uint32 varaddr;
-	uint8 *vbuffer;
-	uint32 varsizew;
-#ifdef DHD_DEBUG
-	uint8 *nvram_ularray;
-#endif /* DHD_DEBUG */
-
-	/* Even if there are no vars are to be written, we still need to set the ramsize. */
-	varsize = bus->varsz ? ROUNDUP(bus->varsz, 4) : 0;
-	varaddr = (bus->ramsize - 4) - varsize;
-
-	if (bus->vars) {
-		if ((bus->sih->buscoretype == SDIOD_CORE_ID) && (bus->sdpcmrev == 7)) {
-			if (((varaddr & 0x3C) == 0x3C) && (varsize > 4)) {
-				DHD_ERROR(("PR85623WAR in place\n"));
-				varsize += 4;
-				varaddr -= 4;
-			}
-		}
-
-		vbuffer = (uint8 *)MALLOC(bus->dhd->osh, varsize);
-		if (!vbuffer)
-			return BCME_NOMEM;
-
-		bzero(vbuffer, varsize);
-		bcopy(bus->vars, vbuffer, bus->varsz);
-
-		/* Write the vars list */
-		bcmerror = dhdsdio_membytes(bus, TRUE, varaddr, vbuffer, varsize);
-#ifdef DHD_DEBUG
-		/* Verify NVRAM bytes */
-		DHD_INFO(("Compare NVRAM dl & ul; varsize=%d\n", varsize));
-		nvram_ularray = (uint8*)MALLOC(bus->dhd->osh, varsize);
-		if (!nvram_ularray)
-			return BCME_NOMEM;
-
-		/* Upload image to verify downloaded contents. */
-		memset(nvram_ularray, 0xaa, varsize);
-
-		/* Read the vars list to temp buffer for comparison */
-		bcmerror = dhdsdio_membytes(bus, FALSE, varaddr, nvram_ularray, varsize);
-		if (bcmerror) {
-				DHD_ERROR(("%s: error %d on reading %d nvram bytes at 0x%08x\n",
-					__FUNCTION__, bcmerror, varsize, varaddr));
-		}
-		/* Compare the org NVRAM with the one read from RAM */
-		if (memcmp(vbuffer, nvram_ularray, varsize)) {
-			DHD_ERROR(("%s: Downloaded NVRAM image is corrupted.\n", __FUNCTION__));
-		} else
-			DHD_ERROR(("%s: Download, Upload and compare of NVRAM succeeded.\n",
-			__FUNCTION__));
-
-		MFREE(bus->dhd->osh, nvram_ularray, varsize);
-#endif /* DHD_DEBUG */
-
-		MFREE(bus->dhd->osh, vbuffer, varsize);
-	}
-
-	/* adjust to the user specified RAM */
-	DHD_INFO(("Physical memory size: %d, usable memory size: %d\n",
-		bus->orig_ramsize, bus->ramsize));
-	DHD_INFO(("Vars are at %d, orig varsize is %d\n",
-		varaddr, varsize));
-	varsize = ((bus->orig_ramsize - 4) - varaddr);
-
-	/*
-	 * Determine the length token:
-	 * Varsize, converted to words, in lower 16-bits, checksum in upper 16-bits.
-	 */
-	if (bcmerror) {
-		varsizew = 0;
-	} else {
-		varsizew = varsize / 4;
-		varsizew = (~varsizew << 16) | (varsizew & 0x0000FFFF);
-		varsizew = htol32(varsizew);
-	}
-
-	DHD_INFO(("New varsize is %d, length token=0x%08x\n", varsize, varsizew));
-
-	/* Write the length token to the last word */
-	bcmerror = dhdsdio_membytes(bus, TRUE, (bus->orig_ramsize - 4),
-		(uint8*)&varsizew, 4);
-
-	return bcmerror;
-}
-
-static int
-dhdsdio_download_state(dhd_bus_t *bus, bool enter)
-{
-	uint retries;
-	int bcmerror = 0;
-
-	/* To enter download state, disable ARM and reset SOCRAM.
-	 * To exit download state, simply reset ARM (default is RAM boot).
-	 */
-	if (enter) {
-		bus->alp_only = TRUE;
-
-		if (!(si_setcore(bus->sih, ARM7S_CORE_ID, 0)) &&
-		    !(si_setcore(bus->sih, ARMCM3_CORE_ID, 0))) {
-			DHD_ERROR(("%s: Failed to find ARM core!\n", __FUNCTION__));
-			bcmerror = BCME_ERROR;
-			goto fail;
-		}
-
-		si_core_disable(bus->sih, 0);
-		if (bcmsdh_regfail(bus->sdh)) {
-			bcmerror = BCME_SDIO_ERROR;
-			goto fail;
-		}
-
-		if (!(si_setcore(bus->sih, SOCRAM_CORE_ID, 0))) {
-			DHD_ERROR(("%s: Failed to find SOCRAM core!\n", __FUNCTION__));
-			bcmerror = BCME_ERROR;
-			goto fail;
-		}
-
-		si_core_reset(bus->sih, 0, 0);
-		if (bcmsdh_regfail(bus->sdh)) {
-			DHD_ERROR(("%s: Failure trying reset SOCRAM core?\n", __FUNCTION__));
-			bcmerror = BCME_SDIO_ERROR;
-			goto fail;
-		}
-
-		/* Clear the top bit of memory */
-		if (bus->ramsize) {
-			uint32 zeros = 0;
-			if (dhdsdio_membytes(bus, TRUE, bus->ramsize - 4, (uint8*)&zeros, 4) < 0) {
-				bcmerror = BCME_SDIO_ERROR;
-				goto fail;
-			}
-		}
-	} else {
-		if (!(si_setcore(bus->sih, SOCRAM_CORE_ID, 0))) {
-			DHD_ERROR(("%s: Failed to find SOCRAM core!\n", __FUNCTION__));
-			bcmerror = BCME_ERROR;
-			goto fail;
-		}
-
-		if (!si_iscoreup(bus->sih)) {
-			DHD_ERROR(("%s: SOCRAM core is down after reset?\n", __FUNCTION__));
-			bcmerror = BCME_ERROR;
-			goto fail;
-		}
-
-		if ((bcmerror = dhdsdio_write_vars(bus))) {
-			DHD_ERROR(("%s: could not write vars to RAM\n", __FUNCTION__));
-			goto fail;
-		}
-
-		if (!si_setcore(bus->sih, PCMCIA_CORE_ID, 0) &&
-		    !si_setcore(bus->sih, SDIOD_CORE_ID, 0)) {
-			DHD_ERROR(("%s: Can't change back to SDIO core?\n", __FUNCTION__));
-			bcmerror = BCME_ERROR;
-			goto fail;
-		}
-		W_SDREG(0xFFFFFFFF, &bus->regs->intstatus, retries);
-
-
-		if (!(si_setcore(bus->sih, ARM7S_CORE_ID, 0)) &&
-		    !(si_setcore(bus->sih, ARMCM3_CORE_ID, 0))) {
-			DHD_ERROR(("%s: Failed to find ARM core!\n", __FUNCTION__));
-			bcmerror = BCME_ERROR;
-			goto fail;
-		}
-
-		si_core_reset(bus->sih, 0, 0);
-		if (bcmsdh_regfail(bus->sdh)) {
-			DHD_ERROR(("%s: Failure trying to reset ARM core?\n", __FUNCTION__));
-			bcmerror = BCME_SDIO_ERROR;
-			goto fail;
-		}
-
-		/* Allow HT Clock now that the ARM is running. */
-		bus->alp_only = FALSE;
-
-		bus->dhd->busstate = DHD_BUS_LOAD;
-	}
-
-fail:
-	/* Always return to SDIOD core */
-	if (!si_setcore(bus->sih, PCMCIA_CORE_ID, 0))
-		si_setcore(bus->sih, SDIOD_CORE_ID, 0);
-
-	return bcmerror;
-}
-
-int
-dhd_bus_iovar_op(dhd_pub_t *dhdp, const char *name,
-                 void *params, int plen, void *arg, int len, bool set)
-{
-	dhd_bus_t *bus = dhdp->bus;
-	const bcm_iovar_t *vi = NULL;
-	int bcmerror = 0;
-	int val_size;
-	uint32 actionid;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	ASSERT(name);
-	ASSERT(len >= 0);
-
-	/* Get MUST have return space */
-	ASSERT(set || (arg && len));
-
-	/* Set does NOT take qualifiers */
-	ASSERT(!set || (!params && !plen));
-
-	/* Look up var locally; if not found pass to host driver */
-	if ((vi = bcm_iovar_lookup(dhdsdio_iovars, name)) == NULL) {
-		dhd_os_sdlock(bus->dhd);
-
-		BUS_WAKE(bus);
-
-		/* Turn on clock in case SD command needs backplane */
-		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-
-		bcmerror = bcmsdh_iovar_op(bus->sdh, name, params, plen, arg, len, set);
-
-		/* Check for bus configuration changes of interest */
-
-		/* If it was divisor change, read the new one */
-		if (set && strcmp(name, "sd_divisor") == 0) {
-			if (bcmsdh_iovar_op(bus->sdh, "sd_divisor", NULL, 0,
-			                    &bus->sd_divisor, sizeof(int32), FALSE) != BCME_OK) {
-				bus->sd_divisor = -1;
-				DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, name));
-			} else {
-				DHD_INFO(("%s: noted %s update, value now %d\n",
-				          __FUNCTION__, name, bus->sd_divisor));
-			}
-		}
-		/* If it was a mode change, read the new one */
-		if (set && strcmp(name, "sd_mode") == 0) {
-			if (bcmsdh_iovar_op(bus->sdh, "sd_mode", NULL, 0,
-			                    &bus->sd_mode, sizeof(int32), FALSE) != BCME_OK) {
-				bus->sd_mode = -1;
-				DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, name));
-			} else {
-				DHD_INFO(("%s: noted %s update, value now %d\n",
-				          __FUNCTION__, name, bus->sd_mode));
-			}
-		}
-		/* Similar check for blocksize change */
-		if (set && strcmp(name, "sd_blocksize") == 0) {
-			int32 fnum = 2;
-			if (bcmsdh_iovar_op(bus->sdh, "sd_blocksize", &fnum, sizeof(int32),
-			                    &bus->blocksize, sizeof(int32), FALSE) != BCME_OK) {
-				bus->blocksize = 0;
-				DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_blocksize"));
-			} else {
-				DHD_INFO(("%s: noted %s update, value now %d\n",
-				          __FUNCTION__, "sd_blocksize", bus->blocksize));
-			}
-		}
-		bus->roundup = MIN(max_roundup, bus->blocksize);
-
-		if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
-			bus->activity = FALSE;
-			dhdsdio_clkctl(bus, CLK_NONE, TRUE);
-		}
-
-		dhd_os_sdunlock(bus->dhd);
-		goto exit;
-	}
-
-	DHD_CTL(("%s: %s %s, len %d plen %d\n", __FUNCTION__,
-	         name, (set ? "set" : "get"), len, plen));
-
-	/* set up 'params' pointer in case this is a set command so that
-	 * the convenience int and bool code can be common to set and get
-	 */
-	if (params == NULL) {
-		params = arg;
-		plen = len;
-	}
-
-	if (vi->type == IOVT_VOID)
-		val_size = 0;
-	else if (vi->type == IOVT_BUFFER)
-		val_size = len;
-	else
-		/* all other types are integer sized */
-		val_size = sizeof(int);
-
-	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
-	bcmerror = dhdsdio_doiovar(bus, vi, actionid, name, params, plen, arg, len, val_size);
-
-exit:
-	return bcmerror;
-}
-
-void
-dhd_bus_stop(struct dhd_bus *bus, bool enforce_mutex)
-{
-	osl_t *osh;
-	uint32 local_hostintmask;
-	uint8 saveclk;
-	uint retries;
-	int err;
-	if (!bus->dhd)
-		return;
-
-	osh = bus->dhd->osh;
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	bcmsdh_waitlockfree(NULL);
-
-	if (enforce_mutex)
-		dhd_os_sdlock(bus->dhd);
-
-	BUS_WAKE(bus);
-
-	/* Change our idea of bus state */
-	bus->dhd->busstate = DHD_BUS_DOWN;
-
-	/* Enable clock for device interrupts */
-	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-
-	/* Disable and clear interrupts at the chip level also */
-	W_SDREG(0, &bus->regs->hostintmask, retries);
-	local_hostintmask = bus->hostintmask;
-	bus->hostintmask = 0;
-
-	/* Force clocks on backplane to be sure F2 interrupt propagates */
-	saveclk = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
-	if (!err) {
-		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
-		                 (saveclk | SBSDIO_FORCE_HT), &err);
-	}
-	if (err) {
-		DHD_ERROR(("%s: Failed to force clock for F2: err %d\n", __FUNCTION__, err));
-	}
-
-	/* Turn off the bus (F2), free any pending packets */
-	DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
-	bcmsdh_intr_disable(bus->sdh);
-	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, SDIO_FUNC_ENABLE_1, NULL);
-
-	/* Clear any pending interrupts now that F2 is disabled */
-	W_SDREG(local_hostintmask, &bus->regs->intstatus, retries);
-
-	/* Turn off the backplane clock (only) */
-	dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
-
-	/* Clear the data packet queues */
-	pktq_flush(osh, &bus->txq, TRUE, NULL, 0);
-
-	/* Clear any held glomming stuff */
-	if (bus->glomd)
-		PKTFREE(osh, bus->glomd, FALSE);
-
-	if (bus->glom)
-		PKTFREE(osh, bus->glom, FALSE);
-
-	bus->glom = bus->glomd = NULL;
-
-	/* Clear rx control and wake any waiters */
-	bus->rxlen = 0;
-	dhd_os_ioctl_resp_wake(bus->dhd);
-
-	/* Reset some F2 state stuff */
-	bus->rxskip = FALSE;
-	bus->tx_seq = bus->rx_seq = 0;
-
-	if (enforce_mutex)
-		dhd_os_sdunlock(bus->dhd);
-}
-
-int
-dhd_bus_init(dhd_pub_t *dhdp, bool enforce_mutex)
-{
-	dhd_bus_t *bus = dhdp->bus;
-	dhd_timeout_t tmo;
-	uint retries = 0;
-	uint8 ready, enable;
-	int err, ret = 0;
-	uint8 saveclk;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	ASSERT(bus->dhd);
-	if (!bus->dhd)
-		return 0;
-
-	if (enforce_mutex)
-		dhd_os_sdlock(bus->dhd);
-
-	/* Make sure backplane clock is on, needed to generate F2 interrupt */
-	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-	if (bus->clkstate != CLK_AVAIL) {
-		DHD_ERROR(("%s: clock state is wrong. state = %d\n", __FUNCTION__, bus->clkstate));
-		goto exit;
-	}
-
-
-	/* Force clocks on backplane to be sure F2 interrupt propagates */
-	saveclk = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
-	if (!err) {
-		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
-		                 (saveclk | SBSDIO_FORCE_HT), &err);
-	}
-	if (err) {
-		DHD_ERROR(("%s: Failed to force clock for F2: err %d\n", __FUNCTION__, err));
-		goto exit;
-	}
-
-	/* Enable function 2 (frame transfers) */
-	W_SDREG((SDPCM_PROT_VERSION << SMB_DATA_VERSION_SHIFT),
-	        &bus->regs->tosbmailboxdata, retries);
-	enable = (SDIO_FUNC_ENABLE_1 | SDIO_FUNC_ENABLE_2);
-
-	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, enable, NULL);
-
-	/* Give the dongle some time to do its thing and set IOR2 */
-	dhd_timeout_start(&tmo, DHD_WAIT_F2RDY * 1000);
-
-	ready = 0;
-	while (ready != enable && !dhd_timeout_expired(&tmo))
-	        ready = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IORDY, NULL);
-
-
-	DHD_INFO(("%s: enable 0x%02x, ready 0x%02x (waited %uus)\n",
-	          __FUNCTION__, enable, ready, tmo.elapsed));
-
-
-	/* If F2 successfully enabled, set core and enable interrupts */
-	if (ready == enable) {
-		/* Make sure we're talking to the core. */
-		if (!(bus->regs = si_setcore(bus->sih, PCMCIA_CORE_ID, 0)))
-			bus->regs = si_setcore(bus->sih, SDIOD_CORE_ID, 0);
-		ASSERT(bus->regs != NULL);
-
-		/* Set up the interrupt mask and enable interrupts */
-		bus->hostintmask = HOSTINTMASK;
-		/* corerev 4 could use the newer interrupt logic to detect the frames */
-		if ((bus->sih->buscoretype == SDIOD_CORE_ID) && (bus->sdpcmrev == 4) &&
-			(bus->rxint_mode != SDIO_DEVICE_HMB_RXINT)) {
-			bus->hostintmask &= ~I_HMB_FRAME_IND;
-			bus->hostintmask |= I_XMTDATA_AVAIL;
-		}
-		W_SDREG(bus->hostintmask, &bus->regs->hostintmask, retries);
-
-		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_WATERMARK, (uint8)watermark, &err);
-
-		/* Set bus state according to enable result */
-		dhdp->busstate = DHD_BUS_DATA;
-
-		/* bcmsdh_intr_unmask(bus->sdh); */
-
-		bus->intdis = FALSE;
-		if (bus->intr) {
-			DHD_INTR(("%s: enable SDIO device interrupts\n", __FUNCTION__));
-			bcmsdh_intr_enable(bus->sdh);
-		} else {
-			DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
-			bcmsdh_intr_disable(bus->sdh);
-		}
-
-	}
-
-
-	else {
-		/* Disable F2 again */
-		enable = SDIO_FUNC_ENABLE_1;
-		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, enable, NULL);
-	}
-
-	/* Restore previous clock setting */
-	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, saveclk, &err);
-
-
-	/* If we didn't come up, turn off backplane clock */
-	if (dhdp->busstate != DHD_BUS_DATA)
-		dhdsdio_clkctl(bus, CLK_NONE, FALSE);
-
-exit:
-	if (enforce_mutex)
-		dhd_os_sdunlock(bus->dhd);
-
-	return ret;
-}
-
-static void
-dhdsdio_rxfail(dhd_bus_t *bus, bool abort, bool rtx)
-{
-	bcmsdh_info_t *sdh = bus->sdh;
-	sdpcmd_regs_t *regs = bus->regs;
-	uint retries = 0;
-	uint16 lastrbc;
-	uint8 hi, lo;
-	int err;
-
-	DHD_ERROR(("%s: %sterminate frame%s\n", __FUNCTION__,
-	           (abort ? "abort command, " : ""), (rtx ? ", send NAK" : "")));
-
-	if (abort) {
-		bcmsdh_abort(sdh, SDIO_FUNC_2);
-	}
-
-	bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL, SFC_RF_TERM, &err);
-	bus->f1regdata++;
-
-	/* Wait until the packet has been flushed (device/FIFO stable) */
-	for (lastrbc = retries = 0xffff; retries > 0; retries--) {
-		hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_RFRAMEBCHI, NULL);
-		lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_RFRAMEBCLO, NULL);
-		bus->f1regdata += 2;
-
-		if ((hi == 0) && (lo == 0))
-			break;
-
-		if ((hi > (lastrbc >> 8)) && (lo > (lastrbc & 0x00ff))) {
-			DHD_ERROR(("%s: count growing: last 0x%04x now 0x%04x\n",
-			           __FUNCTION__, lastrbc, ((hi << 8) + lo)));
-		}
-		lastrbc = (hi << 8) + lo;
-	}
-
-	if (!retries) {
-		DHD_ERROR(("%s: count never zeroed: last 0x%04x\n", __FUNCTION__, lastrbc));
-	} else {
-		DHD_INFO(("%s: flush took %d iterations\n", __FUNCTION__, (0xffff - retries)));
-	}
-
-	if (rtx) {
-		bus->rxrtx++;
-		W_SDREG(SMB_NAK, &regs->tosbmailbox, retries);
-		bus->f1regdata++;
-		if (retries <= retry_limit) {
-			bus->rxskip = TRUE;
-		}
-	}
-
-	/* Clear partial in any case */
-	bus->nextlen = 0;
-
-	/* If we can't reach the device, signal failure */
-	if (err || bcmsdh_regfail(sdh))
-		bus->dhd->busstate = DHD_BUS_DOWN;
-}
-
-static void
-dhdsdio_read_control(dhd_bus_t *bus, uint8 *hdr, uint len, uint doff)
-{
-	bcmsdh_info_t *sdh = bus->sdh;
-	uint rdlen, pad;
-
-	int sdret;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	/* Control data already received in aligned rxctl */
-	if ((bus->bus == SPI_BUS) && (!bus->usebufpool))
-		goto gotpkt;
-
-	ASSERT(bus->rxbuf);
-	/* Set rxctl for frame (w/optional alignment) */
-	bus->rxctl = bus->rxbuf;
-	if (dhd_alignctl) {
-		bus->rxctl += firstread;
-		if ((pad = ((uintptr)bus->rxctl % DHD_SDALIGN)))
-			bus->rxctl += (DHD_SDALIGN - pad);
-		bus->rxctl -= firstread;
-	}
-	ASSERT(bus->rxctl >= bus->rxbuf);
-
-	/* Copy the already-read portion over */
-	bcopy(hdr, bus->rxctl, firstread);
-	if (len <= firstread)
-		goto gotpkt;
-
-	/* Copy the full data pkt in gSPI case and process ioctl. */
-	if (bus->bus == SPI_BUS) {
-		bcopy(hdr, bus->rxctl, len);
-		goto gotpkt;
-	}
-
-	/* Raise rdlen to next SDIO block to avoid tail command */
-	rdlen = len - firstread;
-	if (bus->roundup && bus->blocksize && (rdlen > bus->blocksize)) {
-		pad = bus->blocksize - (rdlen % bus->blocksize);
-		if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
-		    ((len + pad) < bus->dhd->maxctl))
-			rdlen += pad;
-	} else if (rdlen % DHD_SDALIGN) {
-		rdlen += DHD_SDALIGN - (rdlen % DHD_SDALIGN);
-	}
-
-	/* Satisfy length-alignment requirements */
-	if (forcealign && (rdlen & (ALIGNMENT - 1)))
-		rdlen = ROUNDUP(rdlen, ALIGNMENT);
-
-	/* Drop if the read is too big or it exceeds our maximum */
-	if ((rdlen + firstread) > bus->dhd->maxctl) {
-		DHD_ERROR(("%s: %d-byte control read exceeds %d-byte buffer\n",
-		           __FUNCTION__, rdlen, bus->dhd->maxctl));
-		bus->dhd->rx_errors++;
-		dhdsdio_rxfail(bus, FALSE, FALSE);
-		goto done;
-	}
-
-	if ((len - doff) > bus->dhd->maxctl) {
-		DHD_ERROR(("%s: %d-byte ctl frame (%d-byte ctl data) exceeds %d-byte limit\n",
-		           __FUNCTION__, len, (len - doff), bus->dhd->maxctl));
-		bus->dhd->rx_errors++; bus->rx_toolong++;
-		dhdsdio_rxfail(bus, FALSE, FALSE);
-		goto done;
-	}
-
-
-	/* Read remainder of frame body into the rxctl buffer */
-	sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
-	                            (bus->rxctl + firstread), rdlen, NULL, NULL, NULL);
-	bus->f2rxdata++;
-	ASSERT(sdret != BCME_PENDING);
-
-	/* Control frame failures need retransmission */
-	if (sdret < 0) {
-		DHD_ERROR(("%s: read %d control bytes failed: %d\n", __FUNCTION__, rdlen, sdret));
-		bus->rxc_errors++; /* dhd.rx_ctlerrs is higher level */
-		dhdsdio_rxfail(bus, TRUE, TRUE);
-		goto done;
-	}
-
-gotpkt:
-
-#ifdef DHD_DEBUG
-	if (DHD_BYTES_ON() && DHD_CTL_ON()) {
-		prhex("RxCtrl", bus->rxctl, len);
-	}
-#endif
-
-	/* Point to valid data and indicate its length */
-	bus->rxctl += doff;
-	bus->rxlen = len - doff;
-
-done:
-	/* Awake any waiters */
-	dhd_os_ioctl_resp_wake(bus->dhd);
-}
-
-static uint8
-dhdsdio_rxglom(dhd_bus_t *bus, uint8 rxseq)
-{
-	uint16 dlen, totlen;
-	uint8 *dptr, num = 0;
-
-	uint16 sublen, check;
-	void *pfirst, *plast, *pnext, *save_pfirst;
-	osl_t *osh = bus->dhd->osh;
-
-	int errcode;
-	uint8 chan, seq, doff, sfdoff;
-	uint8 txmax;
-
-	int ifidx = 0;
-	bool usechain = bus->use_rxchain;
-
-	/* If packets, issue read(s) and send up packet chain */
-	/* Return sequence numbers consumed? */
-
-	DHD_TRACE(("dhdsdio_rxglom: start: glomd %p glom %p\n", bus->glomd, bus->glom));
-
-	/* If there's a descriptor, generate the packet chain */
-	if (bus->glomd) {
-		dhd_os_sdlock_rxq(bus->dhd);
-
-		pfirst = plast = pnext = NULL;
-		dlen = (uint16)PKTLEN(osh, bus->glomd);
-		dptr = PKTDATA(osh, bus->glomd);
-		if (!dlen || (dlen & 1)) {
-			DHD_ERROR(("%s: bad glomd len (%d), ignore descriptor\n",
-			           __FUNCTION__, dlen));
-			dlen = 0;
-		}
-
-		for (totlen = num = 0; dlen; num++) {
-			/* Get (and move past) next length */
-			sublen = ltoh16_ua(dptr);
-			dlen -= sizeof(uint16);
-			dptr += sizeof(uint16);
-			if ((sublen < SDPCM_HDRLEN) ||
-			    ((num == 0) && (sublen < (2 * SDPCM_HDRLEN)))) {
-				DHD_ERROR(("%s: descriptor len %d bad: %d\n",
-				           __FUNCTION__, num, sublen));
-				pnext = NULL;
-				break;
-			}
-			if (sublen % DHD_SDALIGN) {
-				DHD_ERROR(("%s: sublen %d not a multiple of %d\n",
-				           __FUNCTION__, sublen, DHD_SDALIGN));
-				usechain = FALSE;
-			}
-			totlen += sublen;
-
-			/* For last frame, adjust read len so total is a block multiple */
-			if (!dlen) {
-				sublen += (ROUNDUP(totlen, bus->blocksize) - totlen);
-				totlen = ROUNDUP(totlen, bus->blocksize);
-			}
-
-			/* Allocate/chain packet for next subframe */
-			if ((pnext = PKTGET(osh, sublen + DHD_SDALIGN, FALSE)) == NULL) {
-				DHD_ERROR(("%s: PKTGET failed, num %d len %d\n",
-				           __FUNCTION__, num, sublen));
-				break;
-			}
-			ASSERT(!PKTLINK(pnext));
-			if (!pfirst) {
-				ASSERT(!plast);
-				pfirst = plast = pnext;
-			} else {
-				ASSERT(plast);
-				PKTSETNEXT(osh, plast, pnext);
-				plast = pnext;
-			}
-
-			/* Adhere to start alignment requirements */
-			PKTALIGN(osh, pnext, sublen, DHD_SDALIGN);
-		}
-
-		/* If all allocations succeeded, save packet chain in bus structure */
-		if (pnext) {
-			DHD_GLOM(("%s: allocated %d-byte packet chain for %d subframes\n",
-			          __FUNCTION__, totlen, num));
-			if (DHD_GLOM_ON() && bus->nextlen) {
-				if (totlen != bus->nextlen) {
-					DHD_GLOM(("%s: glomdesc mismatch: nextlen %d glomdesc %d "
-					          "rxseq %d\n", __FUNCTION__, bus->nextlen,
-					          totlen, rxseq));
-				}
-			}
-			bus->glom = pfirst;
-			pfirst = pnext = NULL;
-		} else {
-			if (pfirst)
-				PKTFREE(osh, pfirst, FALSE);
-			bus->glom = NULL;
-			num = 0;
-		}
-
-		/* Done with descriptor packet */
-		PKTFREE(osh, bus->glomd, FALSE);
-		bus->glomd = NULL;
-		bus->nextlen = 0;
-
-		dhd_os_sdunlock_rxq(bus->dhd);
-	}
-
-	/* Ok -- either we just generated a packet chain, or had one from before */
-	if (bus->glom) {
-		if (DHD_GLOM_ON()) {
-			DHD_GLOM(("%s: attempt superframe read, packet chain:\n", __FUNCTION__));
-			for (pnext = bus->glom; pnext; pnext = PKTNEXT(osh, pnext)) {
-				DHD_GLOM(("    %p: %p len 0x%04x (%d)\n",
-				          pnext, (uint8*)PKTDATA(osh, pnext),
-				          PKTLEN(osh, pnext), PKTLEN(osh, pnext)));
-			}
-		}
-
-		pfirst = bus->glom;
-		dlen = (uint16)pkttotlen(osh, pfirst);
-
-		/* Do an SDIO read for the superframe.  Configurable iovar to
-		 * read directly into the chained packet, or allocate a large
-		 * packet and and copy into the chain.
-		 */
-		if (usechain) {
-			errcode = dhd_bcmsdh_recv_buf(bus,
-			                              bcmsdh_cur_sbwad(bus->sdh), SDIO_FUNC_2,
-			                              F2SYNC, (uint8*)PKTDATA(osh, pfirst),
-			                              dlen, pfirst, NULL, NULL);
-		} else if (bus->dataptr) {
-			errcode = dhd_bcmsdh_recv_buf(bus,
-			                              bcmsdh_cur_sbwad(bus->sdh), SDIO_FUNC_2,
-			                              F2SYNC, bus->dataptr,
-			                              dlen, NULL, NULL, NULL);
-			sublen = (uint16)pktfrombuf(osh, pfirst, 0, dlen, bus->dataptr);
-			if (sublen != dlen) {
-				DHD_ERROR(("%s: FAILED TO COPY, dlen %d sublen %d\n",
-				           __FUNCTION__, dlen, sublen));
-				errcode = -1;
-			}
-			pnext = NULL;
-		} else {
-			DHD_ERROR(("COULDN'T ALLOC %d-BYTE GLOM, FORCE FAILURE\n", dlen));
-			errcode = -1;
-		}
-		bus->f2rxdata++;
-		ASSERT(errcode != BCME_PENDING);
-
-		/* On failure, kill the superframe, allow a couple retries */
-		if (errcode < 0) {
-			DHD_ERROR(("%s: glom read of %d bytes failed: %d\n",
-			           __FUNCTION__, dlen, errcode));
-			bus->dhd->rx_errors++;
-
-			if (bus->glomerr++ < 3) {
-				dhdsdio_rxfail(bus, TRUE, TRUE);
-			} else {
-				bus->glomerr = 0;
-				dhdsdio_rxfail(bus, TRUE, FALSE);
-				dhd_os_sdlock_rxq(bus->dhd);
-				PKTFREE(osh, bus->glom, FALSE);
-				dhd_os_sdunlock_rxq(bus->dhd);
-				bus->rxglomfail++;
-				bus->glom = NULL;
-			}
-			return 0;
-		}
-
-#ifdef DHD_DEBUG
-		if (DHD_GLOM_ON()) {
-			prhex("SUPERFRAME", PKTDATA(osh, pfirst),
-			      MIN(PKTLEN(osh, pfirst), 48));
-		}
-#endif
-
-
-		/* Validate the superframe header */
-		dptr = (uint8 *)PKTDATA(osh, pfirst);
-		sublen = ltoh16_ua(dptr);
-		check = ltoh16_ua(dptr + sizeof(uint16));
-
-		chan = SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]);
-		seq = SDPCM_PACKET_SEQUENCE(&dptr[SDPCM_FRAMETAG_LEN]);
-		bus->nextlen = dptr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
-		if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
-			DHD_INFO(("%s: got frame w/nextlen too large (%d) seq %d\n",
-			          __FUNCTION__, bus->nextlen, seq));
-			bus->nextlen = 0;
-		}
-		doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
-		txmax = SDPCM_WINDOW_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
-
-		errcode = 0;
-		if ((uint16)~(sublen^check)) {
-			DHD_ERROR(("%s (superframe): HW hdr error: len/check 0x%04x/0x%04x\n",
-			           __FUNCTION__, sublen, check));
-			errcode = -1;
-		} else if (ROUNDUP(sublen, bus->blocksize) != dlen) {
-			DHD_ERROR(("%s (superframe): len 0x%04x, rounded 0x%04x, expect 0x%04x\n",
-			           __FUNCTION__, sublen, ROUNDUP(sublen, bus->blocksize), dlen));
-			errcode = -1;
-		} else if (SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]) != SDPCM_GLOM_CHANNEL) {
-			DHD_ERROR(("%s (superframe): bad channel %d\n", __FUNCTION__,
-			           SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN])));
-			errcode = -1;
-		} else if (SDPCM_GLOMDESC(&dptr[SDPCM_FRAMETAG_LEN])) {
-			DHD_ERROR(("%s (superframe): got second descriptor?\n", __FUNCTION__));
-			errcode = -1;
-		} else if ((doff < SDPCM_HDRLEN) ||
-		           (doff > (PKTLEN(osh, pfirst) - SDPCM_HDRLEN))) {
-			DHD_ERROR(("%s (superframe): Bad data offset %d: HW %d pkt %d min %d\n",
-			           __FUNCTION__, doff, sublen, PKTLEN(osh, pfirst), SDPCM_HDRLEN));
-			errcode = -1;
-		}
-
-		/* Check sequence number of superframe SW header */
-		if (rxseq != seq) {
-			DHD_INFO(("%s: (superframe) rx_seq %d, expected %d\n",
-			          __FUNCTION__, seq, rxseq));
-			bus->rx_badseq++;
-			rxseq = seq;
-		}
-
-		/* Check window for sanity */
-		if ((uint8)(txmax - bus->tx_seq) > 0x40) {
-			DHD_ERROR(("%s: got unlikely tx max %d with tx_seq %d\n",
-			           __FUNCTION__, txmax, bus->tx_seq));
-			txmax = bus->tx_seq + 2;
-		}
-		bus->tx_max = txmax;
-
-		/* Remove superframe header, remember offset */
-		PKTPULL(osh, pfirst, doff);
-		sfdoff = doff;
-
-		/* Validate all the subframe headers */
-		for (num = 0, pnext = pfirst; pnext && !errcode;
-		     num++, pnext = PKTNEXT(osh, pnext)) {
-			dptr = (uint8 *)PKTDATA(osh, pnext);
-			dlen = (uint16)PKTLEN(osh, pnext);
-			sublen = ltoh16_ua(dptr);
-			check = ltoh16_ua(dptr + sizeof(uint16));
-			chan = SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]);
-			doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
-#ifdef DHD_DEBUG
-			if (DHD_GLOM_ON()) {
-				prhex("subframe", dptr, 32);
-			}
-#endif
-
-			if ((uint16)~(sublen^check)) {
-				DHD_ERROR(("%s (subframe %d): HW hdr error: "
-				           "len/check 0x%04x/0x%04x\n",
-				           __FUNCTION__, num, sublen, check));
-				errcode = -1;
-			} else if ((sublen > dlen) || (sublen < SDPCM_HDRLEN)) {
-				DHD_ERROR(("%s (subframe %d): length mismatch: "
-				           "len 0x%04x, expect 0x%04x\n",
-				           __FUNCTION__, num, sublen, dlen));
-				errcode = -1;
-			} else if ((chan != SDPCM_DATA_CHANNEL) &&
-			           (chan != SDPCM_EVENT_CHANNEL)) {
-				DHD_ERROR(("%s (subframe %d): bad channel %d\n",
-				           __FUNCTION__, num, chan));
-				errcode = -1;
-			} else if ((doff < SDPCM_HDRLEN) || (doff > sublen)) {
-				DHD_ERROR(("%s (subframe %d): Bad data offset %d: HW %d min %d\n",
-				           __FUNCTION__, num, doff, sublen, SDPCM_HDRLEN));
-				errcode = -1;
-			}
-		}
-
-		if (errcode) {
-			/* Terminate frame on error, request a couple retries */
-			if (bus->glomerr++ < 3) {
-				/* Restore superframe header space */
-				PKTPUSH(osh, pfirst, sfdoff);
-				dhdsdio_rxfail(bus, TRUE, TRUE);
-			} else {
-				bus->glomerr = 0;
-				dhdsdio_rxfail(bus, TRUE, FALSE);
-				dhd_os_sdlock_rxq(bus->dhd);
-				PKTFREE(osh, bus->glom, FALSE);
-				dhd_os_sdunlock_rxq(bus->dhd);
-				bus->rxglomfail++;
-				bus->glom = NULL;
-			}
-			bus->nextlen = 0;
-			return 0;
-		}
-
-		/* Basic SD framing looks ok - process each packet (header) */
-		save_pfirst = pfirst;
-		bus->glom = NULL;
-		plast = NULL;
-
-		dhd_os_sdlock_rxq(bus->dhd);
-		for (num = 0; pfirst; rxseq++, pfirst = pnext) {
-			pnext = PKTNEXT(osh, pfirst);
-			PKTSETNEXT(osh, pfirst, NULL);
-
-			dptr = (uint8 *)PKTDATA(osh, pfirst);
-			sublen = ltoh16_ua(dptr);
-			chan = SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]);
-			seq = SDPCM_PACKET_SEQUENCE(&dptr[SDPCM_FRAMETAG_LEN]);
-			doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
-
-			DHD_GLOM(("%s: Get subframe %d, %p(%p/%d), sublen %d chan %d seq %d\n",
-			          __FUNCTION__, num, pfirst, PKTDATA(osh, pfirst),
-			          PKTLEN(osh, pfirst), sublen, chan, seq));
-
-			ASSERT((chan == SDPCM_DATA_CHANNEL) || (chan == SDPCM_EVENT_CHANNEL));
-
-			if (rxseq != seq) {
-				DHD_GLOM(("%s: rx_seq %d, expected %d\n",
-				          __FUNCTION__, seq, rxseq));
-				bus->rx_badseq++;
-				rxseq = seq;
-			}
-
-#ifdef DHD_DEBUG
-			if (DHD_BYTES_ON() && DHD_DATA_ON()) {
-				prhex("Rx Subframe Data", dptr, dlen);
-			}
-#endif
-
-			PKTSETLEN(osh, pfirst, sublen);
-			PKTPULL(osh, pfirst, doff);
-
-			if (PKTLEN(osh, pfirst) == 0) {
-				PKTFREE(bus->dhd->osh, pfirst, FALSE);
-				if (plast) {
-					PKTSETNEXT(osh, plast, pnext);
-				} else {
-					ASSERT(save_pfirst == pfirst);
-					save_pfirst = pnext;
-				}
-				continue;
-			} else if (dhd_prot_hdrpull(bus->dhd, &ifidx, pfirst) != 0) {
-				DHD_ERROR(("%s: rx protocol error\n", __FUNCTION__));
-				bus->dhd->rx_errors++;
-				PKTFREE(osh, pfirst, FALSE);
-				if (plast) {
-					PKTSETNEXT(osh, plast, pnext);
-				} else {
-					ASSERT(save_pfirst == pfirst);
-					save_pfirst = pnext;
-				}
-				continue;
-			}
-
-			/* this packet will go up, link back into chain and count it */
-			PKTSETNEXT(osh, pfirst, pnext);
-			plast = pfirst;
-			num++;
-
-#ifdef DHD_DEBUG
-			if (DHD_GLOM_ON()) {
-				DHD_GLOM(("%s subframe %d to stack, %p(%p/%d) nxt/lnk %p/%p\n",
-				          __FUNCTION__, num, pfirst,
-				          PKTDATA(osh, pfirst), PKTLEN(osh, pfirst),
-				          PKTNEXT(osh, pfirst), PKTLINK(pfirst)));
-				prhex("", (uint8 *)PKTDATA(osh, pfirst),
-				      MIN(PKTLEN(osh, pfirst), 32));
-			}
-#endif /* DHD_DEBUG */
-		}
-		dhd_os_sdunlock_rxq(bus->dhd);
-		if (num) {
-			dhd_os_sdunlock(bus->dhd);
-			dhd_rx_frame(bus->dhd, ifidx, save_pfirst, num, 0);
-			dhd_os_sdlock(bus->dhd);
-		}
-
-		bus->rxglomframes++;
-		bus->rxglompkts += num;
-	}
-	return num;
-}
-
-/* Return TRUE if there may be more frames to read */
-static uint
-dhdsdio_readframes(dhd_bus_t *bus, uint maxframes, bool *finished)
-{
-	osl_t *osh = bus->dhd->osh;
-	bcmsdh_info_t *sdh = bus->sdh;
-
-	uint16 len, check;	/* Extracted hardware header fields */
-	uint8 chan, seq, doff;	/* Extracted software header fields */
-	uint8 fcbits;		/* Extracted fcbits from software header */
-	uint8 delta;
-
-	void *pkt;	/* Packet for event or data frames */
-	uint16 pad;	/* Number of pad bytes to read */
-	uint16 rdlen;	/* Total number of bytes to read */
-	uint8 rxseq;	/* Next sequence number to expect */
-	uint rxleft = 0;	/* Remaining number of frames allowed */
-	int sdret;	/* Return code from bcmsdh calls */
-	uint8 txmax;	/* Maximum tx sequence offered */
-	bool len_consistent; /* Result of comparing readahead len and len from hw-hdr */
-	uint8 *rxbuf;
-	int ifidx = 0;
-	uint rxcount = 0; /* Total frames read */
-
-#if defined(DHD_DEBUG) || defined(SDTEST)
-	bool sdtest = FALSE;	/* To limit message spew from test mode */
-#endif
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	ASSERT(maxframes);
-
-#ifdef SDTEST
-	/* Allow pktgen to override maxframes */
-	if (bus->pktgen_count && (bus->pktgen_mode == DHD_PKTGEN_RECV)) {
-		maxframes = bus->pktgen_count;
-		sdtest = TRUE;
-	}
-#endif
-
-	/* Not finished unless we encounter no more frames indication */
-	*finished = FALSE;
-
-
-	for (rxseq = bus->rx_seq, rxleft = maxframes;
-	     !bus->rxskip && rxleft && bus->dhd->busstate != DHD_BUS_DOWN;
-	     rxseq++, rxleft--) {
-
-		/* Handle glomming separately */
-		if (bus->glom || bus->glomd) {
-			uint8 cnt;
-			DHD_GLOM(("%s: calling rxglom: glomd %p, glom %p\n",
-			          __FUNCTION__, bus->glomd, bus->glom));
-			cnt = dhdsdio_rxglom(bus, rxseq);
-			DHD_GLOM(("%s: rxglom returned %d\n", __FUNCTION__, cnt));
-			rxseq += cnt - 1;
-			rxleft = (rxleft > cnt) ? (rxleft - cnt) : 1;
-			continue;
-		}
-
-		/* Try doing single read if we can */
-		if (dhd_readahead && bus->nextlen) {
-			uint16 nextlen = bus->nextlen;
-			bus->nextlen = 0;
-
-			if (bus->bus == SPI_BUS) {
-				rdlen = len = nextlen;
-			}
-			else {
-				rdlen = len = nextlen << 4;
-
-				/* Pad read to blocksize for efficiency */
-				if (bus->roundup && bus->blocksize && (rdlen > bus->blocksize)) {
-					pad = bus->blocksize - (rdlen % bus->blocksize);
-					if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
-						((rdlen + pad + firstread) < MAX_RX_DATASZ))
-						rdlen += pad;
-				} else if (rdlen % DHD_SDALIGN) {
-					rdlen += DHD_SDALIGN - (rdlen % DHD_SDALIGN);
-				}
-			}
-
-			/* We use bus->rxctl buffer in WinXP for initial control pkt receives.
-			 * Later we use buffer-poll for data as well as control packets.
-			 * This is required because dhd receives full frame in gSPI unlike SDIO.
-			 * After the frame is received we have to distinguish whether it is data
-			 * or non-data frame.
-			 */
-			/* Allocate a packet buffer */
-			dhd_os_sdlock_rxq(bus->dhd);
-			if (!(pkt = PKTGET(osh, rdlen + DHD_SDALIGN, FALSE))) {
-				if (bus->bus == SPI_BUS) {
-					bus->usebufpool = FALSE;
-					bus->rxctl = bus->rxbuf;
-					if (dhd_alignctl) {
-						bus->rxctl += firstread;
-						if ((pad = ((uintptr)bus->rxctl % DHD_SDALIGN)))
-							bus->rxctl += (DHD_SDALIGN - pad);
-						bus->rxctl -= firstread;
-					}
-					ASSERT(bus->rxctl >= bus->rxbuf);
-					rxbuf = bus->rxctl;
-					/* Read the entire frame */
-					sdret = dhd_bcmsdh_recv_buf(bus,
-					                            bcmsdh_cur_sbwad(sdh),
-					                            SDIO_FUNC_2,
-					                            F2SYNC, rxbuf, rdlen,
-					                            NULL, NULL, NULL);
-					bus->f2rxdata++;
-					ASSERT(sdret != BCME_PENDING);
-
-
-					/* Control frame failures need retransmission */
-					if (sdret < 0) {
-						DHD_ERROR(("%s: read %d control bytes failed: %d\n",
-						   __FUNCTION__, rdlen, sdret));
-						/* dhd.rx_ctlerrs is higher level */
-						bus->rxc_errors++;
-						dhd_os_sdunlock_rxq(bus->dhd);
-						dhdsdio_rxfail(bus, TRUE,
-						    (bus->bus == SPI_BUS) ? FALSE : TRUE);
-						continue;
-					}
-				} else {
-					/* Give up on data, request rtx of events */
-					DHD_ERROR(("%s (nextlen): PKTGET failed: len %d rdlen %d "
-					           "expected rxseq %d\n",
-					           __FUNCTION__, len, rdlen, rxseq));
-					/* Just go try again w/normal header read */
-					dhd_os_sdunlock_rxq(bus->dhd);
-					continue;
-				}
-			} else {
-				if (bus->bus == SPI_BUS)
-					bus->usebufpool = TRUE;
-
-				ASSERT(!PKTLINK(pkt));
-				PKTALIGN(osh, pkt, rdlen, DHD_SDALIGN);
-				rxbuf = (uint8 *)PKTDATA(osh, pkt);
-				/* Read the entire frame */
-				sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh),
-				                            SDIO_FUNC_2,
-				                            F2SYNC, rxbuf, rdlen,
-				                            pkt, NULL, NULL);
-				bus->f2rxdata++;
-				ASSERT(sdret != BCME_PENDING);
-
-				if (sdret < 0) {
-					DHD_ERROR(("%s (nextlen): read %d bytes failed: %d\n",
-					   __FUNCTION__, rdlen, sdret));
-					PKTFREE(bus->dhd->osh, pkt, FALSE);
-					bus->dhd->rx_errors++;
-					dhd_os_sdunlock_rxq(bus->dhd);
-					/* Force retry w/normal header read.  Don't attempt NAK for
-					 * gSPI
-					 */
-					dhdsdio_rxfail(bus, TRUE,
-					      (bus->bus == SPI_BUS) ? FALSE : TRUE);
-					continue;
-				}
-			}
-			dhd_os_sdunlock_rxq(bus->dhd);
-
-			/* Now check the header */
-			bcopy(rxbuf, bus->rxhdr, SDPCM_HDRLEN);
-
-			/* Extract hardware header fields */
-			len = ltoh16_ua(bus->rxhdr);
-			check = ltoh16_ua(bus->rxhdr + sizeof(uint16));
-
-			/* All zeros means readahead info was bad */
-			if (!(len|check)) {
-				DHD_INFO(("%s (nextlen): read zeros in HW header???\n",
-				           __FUNCTION__));
-				dhd_os_sdlock_rxq(bus->dhd);
-				PKTFREE2();
-				dhd_os_sdunlock_rxq(bus->dhd);
-				GSPI_PR55150_BAILOUT;
-				continue;
-			}
-
-			/* Validate check bytes */
-			if ((uint16)~(len^check)) {
-				DHD_ERROR(("%s (nextlen): HW hdr error: nextlen/len/check"
-				           " 0x%04x/0x%04x/0x%04x\n", __FUNCTION__, nextlen,
-				           len, check));
-				dhd_os_sdlock_rxq(bus->dhd);
-				PKTFREE2();
-				dhd_os_sdunlock_rxq(bus->dhd);
-				bus->rx_badhdr++;
-				dhdsdio_rxfail(bus, FALSE, FALSE);
-				GSPI_PR55150_BAILOUT;
-				continue;
-			}
-
-			/* Validate frame length */
-			if (len < SDPCM_HDRLEN) {
-				DHD_ERROR(("%s (nextlen): HW hdr length invalid: %d\n",
-				           __FUNCTION__, len));
-				dhd_os_sdlock_rxq(bus->dhd);
-				PKTFREE2();
-				dhd_os_sdunlock_rxq(bus->dhd);
-				GSPI_PR55150_BAILOUT;
-				continue;
-			}
-
-			/* Check for consistency with readahead info */
-				len_consistent = (nextlen != (ROUNDUP(len, 16) >> 4));
-			if (len_consistent) {
-				/* Mismatch, force retry w/normal header (may be >4K) */
-				DHD_ERROR(("%s (nextlen): mismatch, nextlen %d len %d rnd %d; "
-				           "expected rxseq %d\n",
-				           __FUNCTION__, nextlen, len, ROUNDUP(len, 16), rxseq));
-				dhd_os_sdlock_rxq(bus->dhd);
-				PKTFREE2();
-				dhd_os_sdunlock_rxq(bus->dhd);
-				dhdsdio_rxfail(bus, TRUE, (bus->bus == SPI_BUS) ? FALSE : TRUE);
-				GSPI_PR55150_BAILOUT;
-				continue;
-			}
-
-
-			/* Extract software header fields */
-			chan = SDPCM_PACKET_CHANNEL(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
-			seq = SDPCM_PACKET_SEQUENCE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
-			doff = SDPCM_DOFFSET_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
-			txmax = SDPCM_WINDOW_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
-
-				bus->nextlen =
-				         bus->rxhdr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
-				if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
-					DHD_INFO(("%s (nextlen): got frame w/nextlen too large"
-					          " (%d), seq %d\n", __FUNCTION__, bus->nextlen,
-					          seq));
-					bus->nextlen = 0;
-				}
-
-				bus->dhd->rx_readahead_cnt ++;
-			/* Handle Flow Control */
-			fcbits = SDPCM_FCMASK_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
-
-			delta = 0;
-			if (~bus->flowcontrol & fcbits) {
-				bus->fc_xoff++;
-				delta = 1;
-			}
-			if (bus->flowcontrol & ~fcbits) {
-				bus->fc_xon++;
-				delta = 1;
-			}
-
-			if (delta) {
-				bus->fc_rcvd++;
-				bus->flowcontrol = fcbits;
-			}
-
-			/* Check and update sequence number */
-			if (rxseq != seq) {
-				DHD_INFO(("%s (nextlen): rx_seq %d, expected %d\n",
-				          __FUNCTION__, seq, rxseq));
-				bus->rx_badseq++;
-				rxseq = seq;
-			}
-
-			/* Check window for sanity */
-			if ((uint8)(txmax - bus->tx_seq) > 0x40) {
-					DHD_ERROR(("%s: got unlikely tx max %d with tx_seq %d\n",
-						__FUNCTION__, txmax, bus->tx_seq));
-					txmax = bus->tx_seq + 2;
-			}
-			bus->tx_max = txmax;
-
-#ifdef DHD_DEBUG
-			if (DHD_BYTES_ON() && DHD_DATA_ON()) {
-				prhex("Rx Data", rxbuf, len);
-			} else if (DHD_HDRS_ON()) {
-				prhex("RxHdr", bus->rxhdr, SDPCM_HDRLEN);
-			}
-#endif
-
-			if (chan == SDPCM_CONTROL_CHANNEL) {
-				if (bus->bus == SPI_BUS) {
-					dhdsdio_read_control(bus, rxbuf, len, doff);
-					if (bus->usebufpool) {
-						dhd_os_sdlock_rxq(bus->dhd);
-						PKTFREE(bus->dhd->osh, pkt, FALSE);
-						dhd_os_sdunlock_rxq(bus->dhd);
-					}
-					continue;
-				} else {
-					DHD_ERROR(("%s (nextlen): readahead on control"
-					           " packet %d?\n", __FUNCTION__, seq));
-					/* Force retry w/normal header read */
-					bus->nextlen = 0;
-					dhdsdio_rxfail(bus, FALSE, TRUE);
-					dhd_os_sdlock_rxq(bus->dhd);
-					PKTFREE2();
-					dhd_os_sdunlock_rxq(bus->dhd);
-					continue;
-				}
-			}
-
-			if ((bus->bus == SPI_BUS) && !bus->usebufpool) {
-				DHD_ERROR(("Received %d bytes on %d channel. Running out of "
-				           "rx pktbuf's or not yet malloced.\n", len, chan));
-				continue;
-			}
-
-			/* Validate data offset */
-			if ((doff < SDPCM_HDRLEN) || (doff > len)) {
-				DHD_ERROR(("%s (nextlen): bad data offset %d: HW len %d min %d\n",
-				           __FUNCTION__, doff, len, SDPCM_HDRLEN));
-				dhd_os_sdlock_rxq(bus->dhd);
-				PKTFREE2();
-				dhd_os_sdunlock_rxq(bus->dhd);
-				ASSERT(0);
-				dhdsdio_rxfail(bus, FALSE, FALSE);
-				continue;
-			}
-
-			/* All done with this one -- now deliver the packet */
-			goto deliver;
-		}
-		/* gSPI frames should not be handled in fractions */
-		if (bus->bus == SPI_BUS) {
-			break;
-		}
-
-		/* Read frame header (hardware and software) */
-		sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
-		                            bus->rxhdr, firstread, NULL, NULL, NULL);
-		bus->f2rxhdrs++;
-		ASSERT(sdret != BCME_PENDING);
-
-		if (sdret < 0) {
-			DHD_ERROR(("%s: RXHEADER FAILED: %d\n", __FUNCTION__, sdret));
-			bus->rx_hdrfail++;
-			dhdsdio_rxfail(bus, TRUE, TRUE);
-			continue;
-		}
-
-#ifdef DHD_DEBUG
-		if (DHD_BYTES_ON() || DHD_HDRS_ON()) {
-			prhex("RxHdr", bus->rxhdr, SDPCM_HDRLEN);
-		}
-#endif
-
-		/* Extract hardware header fields */
-		len = ltoh16_ua(bus->rxhdr);
-		check = ltoh16_ua(bus->rxhdr + sizeof(uint16));
-
-		/* All zeros means no more frames */
-		if (!(len|check)) {
-			*finished = TRUE;
-			break;
-		}
-
-		/* Validate check bytes */
-		if ((uint16)~(len^check)) {
-			DHD_ERROR(("%s: HW hdr error: len/check 0x%04x/0x%04x\n",
-			           __FUNCTION__, len, check));
-			bus->rx_badhdr++;
-			dhdsdio_rxfail(bus, FALSE, FALSE);
-			continue;
-		}
-
-		/* Validate frame length */
-		if (len < SDPCM_HDRLEN) {
-			DHD_ERROR(("%s: HW hdr length invalid: %d\n", __FUNCTION__, len));
-			continue;
-		}
-
-		/* Extract software header fields */
-		chan = SDPCM_PACKET_CHANNEL(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
-		seq = SDPCM_PACKET_SEQUENCE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
-		doff = SDPCM_DOFFSET_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
-		txmax = SDPCM_WINDOW_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
-
-		/* Validate data offset */
-		if ((doff < SDPCM_HDRLEN) || (doff > len)) {
-			DHD_ERROR(("%s: Bad data offset %d: HW len %d, min %d seq %d\n",
-			           __FUNCTION__, doff, len, SDPCM_HDRLEN, seq));
-			bus->rx_badhdr++;
-			ASSERT(0);
-			dhdsdio_rxfail(bus, FALSE, FALSE);
-			continue;
-		}
-
-		/* Save the readahead length if there is one */
-		bus->nextlen = bus->rxhdr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
-		if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
-			DHD_INFO(("%s (nextlen): got frame w/nextlen too large (%d), seq %d\n",
-			          __FUNCTION__, bus->nextlen, seq));
-			bus->nextlen = 0;
-		}
-
-		/* Handle Flow Control */
-		fcbits = SDPCM_FCMASK_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
-
-		delta = 0;
-		if (~bus->flowcontrol & fcbits) {
-			bus->fc_xoff++;
-			delta = 1;
-		}
-		if (bus->flowcontrol & ~fcbits) {
-			bus->fc_xon++;
-			delta = 1;
-		}
-
-		if (delta) {
-			bus->fc_rcvd++;
-			bus->flowcontrol = fcbits;
-		}
-
-		/* Check and update sequence number */
-		if (rxseq != seq) {
-			DHD_INFO(("%s: rx_seq %d, expected %d\n", __FUNCTION__, seq, rxseq));
-			bus->rx_badseq++;
-			rxseq = seq;
-		}
-
-		/* Check window for sanity */
-		if ((uint8)(txmax - bus->tx_seq) > 0x40) {
-			DHD_ERROR(("%s: got unlikely tx max %d with tx_seq %d\n",
-			           __FUNCTION__, txmax, bus->tx_seq));
-			txmax = bus->tx_seq + 2;
-		}
-		bus->tx_max = txmax;
-
-		/* Call a separate function for control frames */
-		if (chan == SDPCM_CONTROL_CHANNEL) {
-			dhdsdio_read_control(bus, bus->rxhdr, len, doff);
-			continue;
-		}
-
-		ASSERT((chan == SDPCM_DATA_CHANNEL) || (chan == SDPCM_EVENT_CHANNEL) ||
-		       (chan == SDPCM_TEST_CHANNEL) || (chan == SDPCM_GLOM_CHANNEL));
-
-		/* Length to read */
-		rdlen = (len > firstread) ? (len - firstread) : 0;
-
-		/* May pad read to blocksize for efficiency */
-		if (bus->roundup && bus->blocksize && (rdlen > bus->blocksize)) {
-			pad = bus->blocksize - (rdlen % bus->blocksize);
-			if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
-			    ((rdlen + pad + firstread) < MAX_RX_DATASZ))
-				rdlen += pad;
-		} else if (rdlen % DHD_SDALIGN) {
-			rdlen += DHD_SDALIGN - (rdlen % DHD_SDALIGN);
-		}
-
-		/* Satisfy length-alignment requirements */
-		if (forcealign && (rdlen & (ALIGNMENT - 1)))
-			rdlen = ROUNDUP(rdlen, ALIGNMENT);
-
-		if ((rdlen + firstread) > MAX_RX_DATASZ) {
-			/* Too long -- skip this frame */
-			DHD_ERROR(("%s: too long: len %d rdlen %d\n", __FUNCTION__, len, rdlen));
-			bus->dhd->rx_errors++; bus->rx_toolong++;
-			dhdsdio_rxfail(bus, FALSE, FALSE);
-			continue;
-		}
-
-		dhd_os_sdlock_rxq(bus->dhd);
-		if (!(pkt = PKTGET(osh, (rdlen + firstread + DHD_SDALIGN), FALSE))) {
-			/* Give up on data, request rtx of events */
-			DHD_ERROR(("%s: PKTGET failed: rdlen %d chan %d\n",
-			           __FUNCTION__, rdlen, chan));
-			bus->dhd->rx_dropped++;
-			dhd_os_sdunlock_rxq(bus->dhd);
-			dhdsdio_rxfail(bus, FALSE, RETRYCHAN(chan));
-			continue;
-		}
-		dhd_os_sdunlock_rxq(bus->dhd);
-
-		ASSERT(!PKTLINK(pkt));
-
-		/* Leave room for what we already read, and align remainder */
-		ASSERT(firstread < (PKTLEN(osh, pkt)));
-		PKTPULL(osh, pkt, firstread);
-		PKTALIGN(osh, pkt, rdlen, DHD_SDALIGN);
-
-		/* Read the remaining frame data */
-		sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
-		                            ((uint8 *)PKTDATA(osh, pkt)), rdlen, pkt, NULL, NULL);
-		bus->f2rxdata++;
-		ASSERT(sdret != BCME_PENDING);
-
-		if (sdret < 0) {
-			DHD_ERROR(("%s: read %d %s bytes failed: %d\n", __FUNCTION__, rdlen,
-			           ((chan == SDPCM_EVENT_CHANNEL) ? "event" :
-			            ((chan == SDPCM_DATA_CHANNEL) ? "data" : "test")), sdret));
-			dhd_os_sdlock_rxq(bus->dhd);
-			PKTFREE(bus->dhd->osh, pkt, FALSE);
-			dhd_os_sdunlock_rxq(bus->dhd);
-			bus->dhd->rx_errors++;
-			dhdsdio_rxfail(bus, TRUE, RETRYCHAN(chan));
-			continue;
-		}
-
-		/* Copy the already-read portion */
-		PKTPUSH(osh, pkt, firstread);
-		bcopy(bus->rxhdr, PKTDATA(osh, pkt), firstread);
-
-#ifdef DHD_DEBUG
-		if (DHD_BYTES_ON() && DHD_DATA_ON()) {
-			prhex("Rx Data", PKTDATA(osh, pkt), len);
-		}
-#endif
-
-deliver:
-		/* Save superframe descriptor and allocate packet frame */
-		if (chan == SDPCM_GLOM_CHANNEL) {
-			if (SDPCM_GLOMDESC(&bus->rxhdr[SDPCM_FRAMETAG_LEN])) {
-				DHD_GLOM(("%s: got glom descriptor, %d bytes:\n",
-				          __FUNCTION__, len));
-#ifdef DHD_DEBUG
-				if (DHD_GLOM_ON()) {
-					prhex("Glom Data", PKTDATA(osh, pkt), len);
-				}
-#endif
-				PKTSETLEN(osh, pkt, len);
-				ASSERT(doff == SDPCM_HDRLEN);
-				PKTPULL(osh, pkt, SDPCM_HDRLEN);
-				bus->glomd = pkt;
-			} else {
-				DHD_ERROR(("%s: glom superframe w/o descriptor!\n", __FUNCTION__));
-				dhdsdio_rxfail(bus, FALSE, FALSE);
-			}
-			continue;
-		}
-
-		/* Fill in packet len and prio, deliver upward */
-		PKTSETLEN(osh, pkt, len);
-		PKTPULL(osh, pkt, doff);
-
-#ifdef SDTEST
-		/* Test channel packets are processed separately */
-		if (chan == SDPCM_TEST_CHANNEL) {
-			dhdsdio_testrcv(bus, pkt, seq);
-			continue;
-		}
-#endif /* SDTEST */
-
-		if (PKTLEN(osh, pkt) == 0) {
-			dhd_os_sdlock_rxq(bus->dhd);
-			PKTFREE(bus->dhd->osh, pkt, FALSE);
-			dhd_os_sdunlock_rxq(bus->dhd);
-			continue;
-		} else if (dhd_prot_hdrpull(bus->dhd, &ifidx, pkt) != 0) {
-			DHD_ERROR(("%s: rx protocol error\n", __FUNCTION__));
-			dhd_os_sdlock_rxq(bus->dhd);
-			PKTFREE(bus->dhd->osh, pkt, FALSE);
-			dhd_os_sdunlock_rxq(bus->dhd);
-			bus->dhd->rx_errors++;
-			continue;
-		}
-
-
-		/* Unlock during rx call */
-		dhd_os_sdunlock(bus->dhd);
-		dhd_rx_frame(bus->dhd, ifidx, pkt, 1, chan);
-		dhd_os_sdlock(bus->dhd);
-	}
-	rxcount = maxframes - rxleft;
-#ifdef DHD_DEBUG
-	/* Message if we hit the limit */
-	if (!rxleft && !sdtest)
-		DHD_DATA(("%s: hit rx limit of %d frames\n", __FUNCTION__, maxframes));
-	else
-#endif /* DHD_DEBUG */
-	DHD_DATA(("%s: processed %d frames\n", __FUNCTION__, rxcount));
-	/* Back off rxseq if awaiting rtx, update rx_seq */
-	if (bus->rxskip)
-		rxseq--;
-	bus->rx_seq = rxseq;
-
-	return rxcount;
-}
-
-static uint32
-dhdsdio_hostmail(dhd_bus_t *bus)
-{
-	sdpcmd_regs_t *regs = bus->regs;
-	uint32 intstatus = 0;
-	uint32 hmb_data;
-	uint8 fcbits;
-	uint retries = 0;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	/* Read mailbox data and ack that we did so */
-	R_SDREG(hmb_data, &regs->tohostmailboxdata, retries);
-	if (retries <= retry_limit)
-		W_SDREG(SMB_INT_ACK, &regs->tosbmailbox, retries);
-	bus->f1regdata += 2;
-
-	/* Dongle recomposed rx frames, accept them again */
-	if (hmb_data & HMB_DATA_NAKHANDLED) {
-		DHD_INFO(("Dongle reports NAK handled, expect rtx of %d\n", bus->rx_seq));
-		if (!bus->rxskip) {
-			DHD_ERROR(("%s: unexpected NAKHANDLED!\n", __FUNCTION__));
-		}
-		bus->rxskip = FALSE;
-		intstatus |= FRAME_AVAIL_MASK(bus);
-	}
-
-	/*
-	 * DEVREADY does not occur with gSPI.
-	 */
-	if (hmb_data & (HMB_DATA_DEVREADY | HMB_DATA_FWREADY)) {
-		bus->sdpcm_ver = (hmb_data & HMB_DATA_VERSION_MASK) >> HMB_DATA_VERSION_SHIFT;
-		if (bus->sdpcm_ver != SDPCM_PROT_VERSION)
-			DHD_ERROR(("Version mismatch, dongle reports %d, expecting %d\n",
-			           bus->sdpcm_ver, SDPCM_PROT_VERSION));
-		else
-			DHD_INFO(("Dongle ready, protocol version %d\n", bus->sdpcm_ver));
-		/* make sure for the SDIO_DEVICE_RXDATAINT_MODE_1 corecontrol is proper */
-		if ((bus->sih->buscoretype == SDIOD_CORE_ID) && (bus->sdpcmrev >= 4) &&
-		    (bus->rxint_mode  == SDIO_DEVICE_RXDATAINT_MODE_1)) {
-			uint32 val;
-
-			val = R_REG(bus->dhd->osh, &bus->regs->corecontrol);
-			val &= ~CC_XMTDATAAVAIL_MODE;
-			val |= CC_XMTDATAAVAIL_CTRL;
-			W_REG(bus->dhd->osh, &bus->regs->corecontrol, val);
-
-			val = R_REG(bus->dhd->osh, &bus->regs->corecontrol);
-		}
-
-#ifdef DHD_DEBUG
-		/* Retrieve console state address now that firmware should have updated it */
-		{
-			sdpcm_shared_t shared;
-			if (dhdsdio_readshared(bus, &shared) == 0)
-				bus->console_addr = shared.console_addr;
-		}
-#endif /* DHD_DEBUG */
-	}
-
-	/*
-	 * Flow Control has been moved into the RX headers and this out of band
-	 * method isn't used any more.  Leave this here for possibly remaining backward
-	 * compatible with older dongles
-	 */
-	if (hmb_data & HMB_DATA_FC) {
-		fcbits = (hmb_data & HMB_DATA_FCDATA_MASK) >> HMB_DATA_FCDATA_SHIFT;
-
-		if (fcbits & ~bus->flowcontrol)
-			bus->fc_xoff++;
-		if (bus->flowcontrol & ~fcbits)
-			bus->fc_xon++;
-
-		bus->fc_rcvd++;
-		bus->flowcontrol = fcbits;
-	}
-
-
-	/* Shouldn't be any others */
-	if (hmb_data & ~(HMB_DATA_DEVREADY |
-	                 HMB_DATA_FWHALT |
-	                 HMB_DATA_NAKHANDLED |
-	                 HMB_DATA_FC |
-	                 HMB_DATA_FWREADY |
-	                 HMB_DATA_FCDATA_MASK |
-	                 HMB_DATA_VERSION_MASK)) {
-		DHD_ERROR(("Unknown mailbox data content: 0x%02x\n", hmb_data));
-	}
-
-	return intstatus;
-}
-
-static bool
-dhdsdio_dpc(dhd_bus_t *bus)
-{
-	bcmsdh_info_t *sdh = bus->sdh;
-	sdpcmd_regs_t *regs = bus->regs;
-	uint32 intstatus, newstatus = 0;
-	uint retries = 0;
-	uint rxlimit = dhd_rxbound; /* Rx frames to read before resched */
-	uint txlimit = dhd_txbound; /* Tx frames to send before resched */
-	uint framecnt = 0;		  /* Temporary counter of tx/rx frames */
-	bool rxdone = TRUE;		  /* Flag for no more read data */
-	bool resched = FALSE;	  /* Flag indicating resched wanted */
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (bus->dhd->busstate == DHD_BUS_DOWN) {
-		DHD_ERROR(("%s: Bus down, ret\n", __FUNCTION__));
-		bus->intstatus = 0;
-		return 0;
-	}
-
-	/* Start with leftover status bits */
-	intstatus = bus->intstatus;
-
-	dhd_os_sdlock(bus->dhd);
-
-	/* If waiting for HTAVAIL, check status */
-	if (bus->clkstate == CLK_PENDING) {
-		int err;
-		uint8 clkctl, devctl = 0;
-
-#ifdef DHD_DEBUG
-		/* Check for inconsistent device control */
-		devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
-		if (err) {
-			DHD_ERROR(("%s: error reading DEVCTL: %d\n", __FUNCTION__, err));
-			bus->dhd->busstate = DHD_BUS_DOWN;
-		} else {
-			ASSERT(devctl & SBSDIO_DEVCTL_CA_INT_ONLY);
-		}
-#endif /* DHD_DEBUG */
-
-		/* Read CSR, if clock on switch to AVAIL, else ignore */
-		clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
-		if (err) {
-			DHD_ERROR(("%s: error reading CSR: %d\n", __FUNCTION__, err));
-			bus->dhd->busstate = DHD_BUS_DOWN;
-		}
-
-		DHD_INFO(("DPC: PENDING, devctl 0x%02x clkctl 0x%02x\n", devctl, clkctl));
-
-		if (SBSDIO_HTAV(clkctl)) {
-			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
-			if (err) {
-				DHD_ERROR(("%s: error reading DEVCTL: %d\n",
-				           __FUNCTION__, err));
-				bus->dhd->busstate = DHD_BUS_DOWN;
-			}
-			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
-			if (err) {
-				DHD_ERROR(("%s: error writing DEVCTL: %d\n",
-				           __FUNCTION__, err));
-				bus->dhd->busstate = DHD_BUS_DOWN;
-			}
-			bus->clkstate = CLK_AVAIL;
-		} else {
-			goto clkwait;
-		}
-	}
-
-	BUS_WAKE(bus);
-
-	/* Make sure backplane clock is on */
-	dhdsdio_clkctl(bus, CLK_AVAIL, TRUE);
-	if (bus->clkstate != CLK_AVAIL)
-		goto clkwait;
-
-	/* Pending interrupt indicates new device status */
-	if (bus->ipend) {
-		bus->ipend = FALSE;
-		R_SDREG(newstatus, &regs->intstatus, retries);
-		bus->f1regdata++;
-		if (bcmsdh_regfail(bus->sdh))
-			newstatus = 0;
-		newstatus &= bus->hostintmask;
-		bus->fcstate = !!(newstatus & I_HMB_FC_STATE);
-		if (newstatus) {
-			bus->f1regdata++;
-			if ((bus->rxint_mode == SDIO_DEVICE_RXDATAINT_MODE_0) &&
-				(newstatus == I_XMTDATA_AVAIL)) {
-			}
-			else
-				W_SDREG(newstatus, &regs->intstatus, retries);
-		}
-	}
-
-	/* Merge new bits with previous */
-	intstatus |= newstatus;
-	bus->intstatus = 0;
-
-	/* Handle flow-control change: read new state in case our ack
-	 * crossed another change interrupt.  If change still set, assume
-	 * FC ON for safety, let next loop through do the debounce.
-	 */
-	if (intstatus & I_HMB_FC_CHANGE) {
-		intstatus &= ~I_HMB_FC_CHANGE;
-		W_SDREG(I_HMB_FC_CHANGE, &regs->intstatus, retries);
-		R_SDREG(newstatus, &regs->intstatus, retries);
-		bus->f1regdata += 2;
-		bus->fcstate = !!(newstatus & (I_HMB_FC_STATE | I_HMB_FC_CHANGE));
-		intstatus |= (newstatus & bus->hostintmask);
-	}
-
-	/* Just being here means nothing more to do for chipactive */
-	if (intstatus & I_CHIPACTIVE) {
-		/* ASSERT(bus->clkstate == CLK_AVAIL); */
-		intstatus &= ~I_CHIPACTIVE;
-	}
-
-	/* Handle host mailbox indication */
-	if (intstatus & I_HMB_HOST_INT) {
-		intstatus &= ~I_HMB_HOST_INT;
-		intstatus |= dhdsdio_hostmail(bus);
-	}
-
-	/* Generally don't ask for these, can get CRC errors... */
-	if (intstatus & I_WR_OOSYNC) {
-		DHD_ERROR(("Dongle reports WR_OOSYNC\n"));
-		intstatus &= ~I_WR_OOSYNC;
-	}
-
-	if (intstatus & I_RD_OOSYNC) {
-		DHD_ERROR(("Dongle reports RD_OOSYNC\n"));
-		intstatus &= ~I_RD_OOSYNC;
-	}
-
-	if (intstatus & I_SBINT) {
-		DHD_ERROR(("Dongle reports SBINT\n"));
-		intstatus &= ~I_SBINT;
-	}
-
-	/* Would be active due to wake-wlan in gSPI */
-	if (intstatus & I_CHIPACTIVE) {
-		DHD_INFO(("Dongle reports CHIPACTIVE\n"));
-		intstatus &= ~I_CHIPACTIVE;
-	}
-
-	/* Ignore frame indications if rxskip is set */
-	if (bus->rxskip) {
-		intstatus &= ~FRAME_AVAIL_MASK(bus);
-	}
-
-	/* On frame indication, read available frames */
-	if (PKT_AVAILABLE(bus, intstatus)) {
-		framecnt = dhdsdio_readframes(bus, rxlimit, &rxdone);
-		if (rxdone || bus->rxskip)
-			intstatus  &= ~FRAME_AVAIL_MASK(bus);
-		rxlimit -= MIN(framecnt, rxlimit);
-	}
-
-	/* Keep still-pending events for next scheduling */
-	bus->intstatus = intstatus;
-
-clkwait:
-	/* Re-enable interrupts to detect new device events (mailbox, rx frame)
-	 * or clock availability.  (Allows tx loop to check ipend if desired.)
-	 * (Unless register access seems hosed, as we may not be able to ACK...)
-	 */
-	if (bus->intr && bus->intdis && !bcmsdh_regfail(sdh)) {
-		DHD_INTR(("%s: enable SDIO interrupts, rxdone %d framecnt %d\n",
-		          __FUNCTION__, rxdone, framecnt));
-		bus->intdis = FALSE;
-#if defined(OOB_INTR_ONLY)
-	bcmsdh_oob_intr_set(1);
-#endif /* (OOB_INTR_ONLY) */
-		bcmsdh_intr_enable(sdh);
-	}
-
-	if (TXCTLOK(bus) && bus->ctrl_frame_stat && (bus->clkstate == CLK_AVAIL))  {
-		int ret, i;
-
-		ret = dhd_bcmsdh_send_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
-		                      (uint8 *)bus->ctrl_frame_buf, (uint32)bus->ctrl_frame_len,
-			NULL, NULL, NULL);
-		ASSERT(ret != BCME_PENDING);
-
-		if (ret < 0) {
-			/* On failure, abort the command and terminate the frame */
-			DHD_INFO(("%s: sdio error %d, abort command and terminate frame.\n",
-			          __FUNCTION__, ret));
-			bus->tx_sderrs++;
-
-			bcmsdh_abort(sdh, SDIO_FUNC_2);
-
-			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL,
-			                 SFC_WF_TERM, NULL);
-			bus->f1regdata++;
-
-			for (i = 0; i < 3; i++) {
-				uint8 hi, lo;
-				hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
-				                     SBSDIO_FUNC1_WFRAMEBCHI, NULL);
-				lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
-				                     SBSDIO_FUNC1_WFRAMEBCLO, NULL);
-				bus->f1regdata += 2;
-				if ((hi == 0) && (lo == 0))
-					break;
-			}
-		}
-		if (ret == 0) {
-			bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
-		}
-
-		bus->ctrl_frame_stat = FALSE;
-		dhd_wait_event_wakeup(bus->dhd);
-	}
-	/* Send queued frames (limit 1 if rx may still be pending) */
-	else if ((bus->clkstate == CLK_AVAIL) && !bus->fcstate &&
-	    pktq_mlen(&bus->txq, ~bus->flowcontrol) && txlimit && DATAOK(bus)) {
-		framecnt = rxdone ? txlimit : MIN(txlimit, dhd_txminmax);
-		framecnt = dhdsdio_sendfromq(bus, framecnt);
-		txlimit -= framecnt;
-	}
-	/* Resched the DPC if ctrl cmd is pending on bus credit */
-	if (bus->ctrl_frame_stat)
-		resched = TRUE;
-
-	/* Resched if events or tx frames are pending, else await next interrupt */
-	/* On failed register access, all bets are off: no resched or interrupts */
-	if ((bus->dhd->busstate == DHD_BUS_DOWN) || bcmsdh_regfail(sdh)) {
-		DHD_ERROR(("%s: failed backplane access over SDIO, halting operation %d \n",
-		           __FUNCTION__, bcmsdh_regfail(sdh)));
-		bus->dhd->busstate = DHD_BUS_DOWN;
-		bus->intstatus = 0;
-	} else if (bus->clkstate == CLK_PENDING) {
-		/* Awaiting I_CHIPACTIVE; don't resched */
-	} else if (bus->intstatus || bus->ipend ||
-	           (!bus->fcstate && pktq_mlen(&bus->txq, ~bus->flowcontrol) && DATAOK(bus)) ||
-			PKT_AVAILABLE(bus, bus->intstatus)) {  /* Read multiple frames */
-		resched = TRUE;
-	}
-
-	bus->dpc_sched = resched;
-
-	/* If we're done for now, turn off clock request. */
-	if ((bus->idletime == DHD_IDLE_IMMEDIATE) && (bus->clkstate != CLK_PENDING)) {
-		bus->activity = FALSE;
-		dhdsdio_clkctl(bus, CLK_NONE, FALSE);
-	}
-
-	dhd_os_sdunlock(bus->dhd);
-	return resched;
-}
-
-bool
-dhd_bus_dpc(struct dhd_bus *bus)
-{
-	bool resched;
-
-	/* Call the DPC directly. */
-	DHD_TRACE(("Calling dhdsdio_dpc() from %s\n", __FUNCTION__));
-	resched = dhdsdio_dpc(bus);
-
-	return resched;
-}
-
-void
-dhdsdio_isr(void *arg)
-{
-	dhd_bus_t *bus = (dhd_bus_t*)arg;
-	bcmsdh_info_t *sdh;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (!bus) {
-		DHD_ERROR(("%s : bus is null pointer , exit \n", __FUNCTION__));
-		return;
-	}
-	sdh = bus->sdh;
-
-	if (bus->dhd->busstate == DHD_BUS_DOWN) {
-		DHD_ERROR(("%s : bus is down. we have nothing to do\n", __FUNCTION__));
-		return;
-	}
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	/* Count the interrupt call */
-	bus->intrcount++;
-	bus->ipend = TRUE;
-
-	/* Shouldn't get this interrupt if we're sleeping? */
-	if (bus->sleeping) {
-		DHD_ERROR(("INTERRUPT WHILE SLEEPING??\n"));
-		return;
-	}
-
-	/* Disable additional interrupts (is this needed now)? */
-	if (bus->intr) {
-		DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
-	} else {
-		DHD_ERROR(("dhdsdio_isr() w/o interrupt configured!\n"));
-	}
-
-	bcmsdh_intr_disable(sdh);
-	bus->intdis = TRUE;
-
-#if defined(SDIO_ISR_THREAD)
-	DHD_TRACE(("Calling dhdsdio_dpc() from %s\n", __FUNCTION__));
-	DHD_OS_WAKE_LOCK(bus->dhd);
-	while (dhdsdio_dpc(bus));
-	DHD_OS_WAKE_UNLOCK(bus->dhd);
-#else
-	bus->dpc_sched = TRUE;
-	dhd_sched_dpc(bus->dhd);
-#endif 
-
-}
-
-#ifdef SDTEST
-static void
-dhdsdio_pktgen_init(dhd_bus_t *bus)
-{
-	/* Default to specified length, or full range */
-	if (dhd_pktgen_len) {
-		bus->pktgen_maxlen = MIN(dhd_pktgen_len, MAX_PKTGEN_LEN);
-		bus->pktgen_minlen = bus->pktgen_maxlen;
-	} else {
-		bus->pktgen_maxlen = MAX_PKTGEN_LEN;
-		bus->pktgen_minlen = 0;
-	}
-	bus->pktgen_len = (uint16)bus->pktgen_minlen;
-
-	/* Default to per-watchdog burst with 10s print time */
-	bus->pktgen_freq = 1;
-	bus->pktgen_print = 10000 / dhd_watchdog_ms;
-	bus->pktgen_count = (dhd_pktgen * dhd_watchdog_ms + 999) / 1000;
-
-	/* Default to echo mode */
-	bus->pktgen_mode = DHD_PKTGEN_ECHO;
-	bus->pktgen_stop = 1;
-}
-
-static void
-dhdsdio_pktgen(dhd_bus_t *bus)
-{
-	void *pkt;
-	uint8 *data;
-	uint pktcount;
-	uint fillbyte;
-	osl_t *osh = bus->dhd->osh;
-	uint16 len;
-
-	/* Display current count if appropriate */
-	if (bus->pktgen_print && (++bus->pktgen_ptick >= bus->pktgen_print)) {
-		bus->pktgen_ptick = 0;
-		printf("%s: send attempts %d rcvd %d\n",
-		       __FUNCTION__, bus->pktgen_sent, bus->pktgen_rcvd);
-	}
-
-	/* For recv mode, just make sure dongle has started sending */
-	if (bus->pktgen_mode == DHD_PKTGEN_RECV) {
-		if (bus->pktgen_rcv_state == PKTGEN_RCV_IDLE) {
-			bus->pktgen_rcv_state = PKTGEN_RCV_ONGOING;
-			dhdsdio_sdtest_set(bus, (uint8)bus->pktgen_total);
-		}
-		return;
-	}
-
-	/* Otherwise, generate or request the specified number of packets */
-	for (pktcount = 0; pktcount < bus->pktgen_count; pktcount++) {
-		/* Stop if total has been reached */
-		if (bus->pktgen_total && (bus->pktgen_sent >= bus->pktgen_total)) {
-			bus->pktgen_count = 0;
-			break;
-		}
-
-		/* Allocate an appropriate-sized packet */
-		len = bus->pktgen_len;
-		if (!(pkt = PKTGET(osh, (len + SDPCM_HDRLEN + SDPCM_TEST_HDRLEN + DHD_SDALIGN),
-		                   TRUE))) {;
-			DHD_ERROR(("%s: PKTGET failed!\n", __FUNCTION__));
-			break;
-		}
-		PKTALIGN(osh, pkt, (len + SDPCM_HDRLEN + SDPCM_TEST_HDRLEN), DHD_SDALIGN);
-		data = (uint8*)PKTDATA(osh, pkt) + SDPCM_HDRLEN;
-
-		/* Write test header cmd and extra based on mode */
-		switch (bus->pktgen_mode) {
-		case DHD_PKTGEN_ECHO:
-			*data++ = SDPCM_TEST_ECHOREQ;
-			*data++ = (uint8)bus->pktgen_sent;
-			break;
-
-		case DHD_PKTGEN_SEND:
-			*data++ = SDPCM_TEST_DISCARD;
-			*data++ = (uint8)bus->pktgen_sent;
-			break;
-
-		case DHD_PKTGEN_RXBURST:
-			*data++ = SDPCM_TEST_BURST;
-			*data++ = (uint8)bus->pktgen_count;
-			break;
-
-		default:
-			DHD_ERROR(("Unrecognized pktgen mode %d\n", bus->pktgen_mode));
-			PKTFREE(osh, pkt, TRUE);
-			bus->pktgen_count = 0;
-			return;
-		}
-
-		/* Write test header length field */
-		*data++ = (len >> 0);
-		*data++ = (len >> 8);
-
-		/* Then fill in the remainder -- N/A for burst, but who cares... */
-		for (fillbyte = 0; fillbyte < len; fillbyte++)
-			*data++ = SDPCM_TEST_FILL(fillbyte, (uint8)bus->pktgen_sent);
-
-#ifdef DHD_DEBUG
-		if (DHD_BYTES_ON() && DHD_DATA_ON()) {
-			data = (uint8*)PKTDATA(osh, pkt) + SDPCM_HDRLEN;
-			prhex("dhdsdio_pktgen: Tx Data", data, PKTLEN(osh, pkt) - SDPCM_HDRLEN);
-		}
-#endif
-
-		/* Send it */
-		if (dhdsdio_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, TRUE)) {
-			bus->pktgen_fail++;
-			if (bus->pktgen_stop && bus->pktgen_stop == bus->pktgen_fail)
-				bus->pktgen_count = 0;
-		}
-		bus->pktgen_sent++;
-
-		/* Bump length if not fixed, wrap at max */
-		if (++bus->pktgen_len > bus->pktgen_maxlen)
-			bus->pktgen_len = (uint16)bus->pktgen_minlen;
-
-		/* Special case for burst mode: just send one request! */
-		if (bus->pktgen_mode == DHD_PKTGEN_RXBURST)
-			break;
-	}
-}
-
-static void
-dhdsdio_sdtest_set(dhd_bus_t *bus, uint8 count)
-{
-	void *pkt;
-	uint8 *data;
-	osl_t *osh = bus->dhd->osh;
-
-	/* Allocate the packet */
-	if (!(pkt = PKTGET(osh, SDPCM_HDRLEN + SDPCM_TEST_HDRLEN + DHD_SDALIGN, TRUE))) {
-		DHD_ERROR(("%s: PKTGET failed!\n", __FUNCTION__));
-		return;
-	}
-	PKTALIGN(osh, pkt, (SDPCM_HDRLEN + SDPCM_TEST_HDRLEN), DHD_SDALIGN);
-	data = (uint8*)PKTDATA(osh, pkt) + SDPCM_HDRLEN;
-
-	/* Fill in the test header */
-	*data++ = SDPCM_TEST_SEND;
-	*data++ = count;
-	*data++ = (bus->pktgen_maxlen >> 0);
-	*data++ = (bus->pktgen_maxlen >> 8);
-
-	/* Send it */
-	if (dhdsdio_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, TRUE))
-		bus->pktgen_fail++;
-}
-
-
-static void
-dhdsdio_testrcv(dhd_bus_t *bus, void *pkt, uint seq)
-{
-	osl_t *osh = bus->dhd->osh;
-	uint8 *data;
-	uint pktlen;
-
-	uint8 cmd;
-	uint8 extra;
-	uint16 len;
-	uint16 offset;
-
-	/* Check for min length */
-	if ((pktlen = PKTLEN(osh, pkt)) < SDPCM_TEST_HDRLEN) {
-		DHD_ERROR(("dhdsdio_restrcv: toss runt frame, pktlen %d\n", pktlen));
-		PKTFREE(osh, pkt, FALSE);
-		return;
-	}
-
-	/* Extract header fields */
-	data = PKTDATA(osh, pkt);
-	cmd = *data++;
-	extra = *data++;
-	len = *data++; len += *data++ << 8;
-	DHD_TRACE(("%s:cmd:%d, xtra:%d,len:%d\n", __FUNCTION__, cmd, extra, len));
-	/* Check length for relevant commands */
-	if (cmd == SDPCM_TEST_DISCARD || cmd == SDPCM_TEST_ECHOREQ || cmd == SDPCM_TEST_ECHORSP) {
-		if (pktlen != len + SDPCM_TEST_HDRLEN) {
-			DHD_ERROR(("dhdsdio_testrcv: frame length mismatch, pktlen %d seq %d"
-			           " cmd %d extra %d len %d\n", pktlen, seq, cmd, extra, len));
-			PKTFREE(osh, pkt, FALSE);
-			return;
-		}
-	}
-
-	/* Process as per command */
-	switch (cmd) {
-	case SDPCM_TEST_ECHOREQ:
-		/* Rx->Tx turnaround ok (even on NDIS w/current implementation) */
-		*(uint8 *)(PKTDATA(osh, pkt)) = SDPCM_TEST_ECHORSP;
-		if (dhdsdio_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, TRUE) == 0) {
-			bus->pktgen_sent++;
-		} else {
-			bus->pktgen_fail++;
-			PKTFREE(osh, pkt, FALSE);
-		}
-		bus->pktgen_rcvd++;
-		break;
-
-	case SDPCM_TEST_ECHORSP:
-		if (bus->ext_loop) {
-			PKTFREE(osh, pkt, FALSE);
-			bus->pktgen_rcvd++;
-			break;
-		}
-
-		for (offset = 0; offset < len; offset++, data++) {
-			if (*data != SDPCM_TEST_FILL(offset, extra)) {
-				DHD_ERROR(("dhdsdio_testrcv: echo data mismatch: "
-				           "offset %d (len %d) expect 0x%02x rcvd 0x%02x\n",
-				           offset, len, SDPCM_TEST_FILL(offset, extra), *data));
-				break;
-			}
-		}
-		PKTFREE(osh, pkt, FALSE);
-		bus->pktgen_rcvd++;
-		break;
-
-	case SDPCM_TEST_DISCARD:
-		{
-			int i = 0;
-			uint8 *prn = data;
-			uint8 testval = extra;
-			for (i = 0; i < len; i++) {
-				if (*prn != testval) {
-					DHD_ERROR(("DIErr@Pkt#:%d,Ix:%d, expected:0x%x, got:0x%x\n",
-						i, bus->pktgen_rcvd_rcvsession, testval, *prn));
-					prn++; testval++;
-				}
-			}
-		}
-		PKTFREE(osh, pkt, FALSE);
-		bus->pktgen_rcvd++;
-		break;
-
-	case SDPCM_TEST_BURST:
-	case SDPCM_TEST_SEND:
-	default:
-		DHD_INFO(("dhdsdio_testrcv: unsupported or unknown command, pktlen %d seq %d"
-		          " cmd %d extra %d len %d\n", pktlen, seq, cmd, extra, len));
-		PKTFREE(osh, pkt, FALSE);
-		break;
-	}
-
-	/* For recv mode, stop at limit (and tell dongle to stop sending) */
-	if (bus->pktgen_mode == DHD_PKTGEN_RECV) {
-		if (bus->pktgen_rcv_state != PKTGEN_RCV_IDLE) {
-			bus->pktgen_rcvd_rcvsession++;
-
-			if (bus->pktgen_total &&
-				(bus->pktgen_rcvd_rcvsession >= bus->pktgen_total)) {
-			bus->pktgen_count = 0;
-	            DHD_ERROR(("Pktgen:rcv test complete!\n"));
-	            bus->pktgen_rcv_state = PKTGEN_RCV_IDLE;
-			dhdsdio_sdtest_set(bus, FALSE);
-				bus->pktgen_rcvd_rcvsession = 0;
-			}
-		}
-	}
-}
-#endif /* SDTEST */
-
-extern void
-dhd_disable_intr(dhd_pub_t *dhdp)
-{
-	dhd_bus_t *bus;
-	bus = dhdp->bus;
-	bcmsdh_intr_disable(bus->sdh);
-}
-
-extern bool
-dhd_bus_watchdog(dhd_pub_t *dhdp)
-{
-	dhd_bus_t *bus;
-
-	DHD_TIMER(("%s: Enter\n", __FUNCTION__));
-
-	bus = dhdp->bus;
-
-	if (bus->dhd->dongle_reset)
-		return FALSE;
-
-	/* Ignore the timer if simulating bus down */
-	if (bus->sleeping)
-		return FALSE;
-
-	if (dhdp->busstate == DHD_BUS_DOWN)
-		return FALSE;
-
-	/* Poll period: check device if appropriate. */
-	if (bus->poll && (++bus->polltick >= bus->pollrate)) {
-		uint32 intstatus = 0;
-
-		/* Reset poll tick */
-		bus->polltick = 0;
-
-		/* Check device if no interrupts */
-		if (!bus->intr || (bus->intrcount == bus->lastintrs)) {
-
-			if (!bus->dpc_sched) {
-				uint8 devpend;
-				devpend = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_0,
-				                          SDIOD_CCCR_INTPEND, NULL);
-				intstatus = devpend & (INTR_STATUS_FUNC1 | INTR_STATUS_FUNC2);
-			}
-
-			/* If there is something, make like the ISR and schedule the DPC */
-			if (intstatus) {
-				bus->pollcnt++;
-				bus->ipend = TRUE;
-				if (bus->intr) {
-					bcmsdh_intr_disable(bus->sdh);
-				}
-				bus->dpc_sched = TRUE;
-				dhd_sched_dpc(bus->dhd);
-
-			}
-		}
-
-		/* Update interrupt tracking */
-		bus->lastintrs = bus->intrcount;
-	}
-
-#ifdef DHD_DEBUG
-	/* Poll for console output periodically */
-	if (dhdp->busstate == DHD_BUS_DATA && dhd_console_ms != 0) {
-		bus->console.count += dhd_watchdog_ms;
-		if (bus->console.count >= dhd_console_ms) {
-			bus->console.count -= dhd_console_ms;
-			/* Make sure backplane clock is on */
-			dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-			if (dhdsdio_readconsole(bus) < 0)
-				dhd_console_ms = 0;	/* On error, stop trying */
-		}
-	}
-#endif /* DHD_DEBUG */
-
-#ifdef SDTEST
-	/* Generate packets if configured */
-	if (bus->pktgen_count && (++bus->pktgen_tick >= bus->pktgen_freq)) {
-		/* Make sure backplane clock is on */
-		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-		bus->pktgen_tick = 0;
-		dhdsdio_pktgen(bus);
-	}
-#endif
-
-	/* On idle timeout clear activity flag and/or turn off clock */
-	if ((bus->idletime > 0) && (bus->clkstate == CLK_AVAIL)) {
-		if (++bus->idlecount >= bus->idletime) {
-			bus->idlecount = 0;
-			if (bus->activity) {
-				bus->activity = FALSE;
-				dhdsdio_clkctl(bus, CLK_NONE, FALSE);
-			}
-		}
-	}
-
-	return bus->ipend;
-}
-
-#ifdef DHD_DEBUG
-extern int
-dhd_bus_console_in(dhd_pub_t *dhdp, uchar *msg, uint msglen)
-{
-	dhd_bus_t *bus = dhdp->bus;
-	uint32 addr, val;
-	int rv;
-	void *pkt;
-
-	/* Address could be zero if CONSOLE := 0 in dongle Makefile */
-	if (bus->console_addr == 0)
-		return BCME_UNSUPPORTED;
-
-	/* Exclusive bus access */
-	dhd_os_sdlock(bus->dhd);
-
-	/* Don't allow input if dongle is in reset */
-	if (bus->dhd->dongle_reset) {
-		dhd_os_sdunlock(bus->dhd);
-		return BCME_NOTREADY;
-	}
-
-	/* Request clock to allow SDIO accesses */
-	BUS_WAKE(bus);
-	/* No pend allowed since txpkt is called later, ht clk has to be on */
-	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-
-	/* Zero cbuf_index */
-	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, cbuf_idx);
-	val = htol32(0);
-	if ((rv = dhdsdio_membytes(bus, TRUE, addr, (uint8 *)&val, sizeof(val))) < 0)
-		goto done;
-
-	/* Write message into cbuf */
-	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, cbuf);
-	if ((rv = dhdsdio_membytes(bus, TRUE, addr, (uint8 *)msg, msglen)) < 0)
-		goto done;
-
-	/* Write length into vcons_in */
-	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, vcons_in);
-	val = htol32(msglen);
-	if ((rv = dhdsdio_membytes(bus, TRUE, addr, (uint8 *)&val, sizeof(val))) < 0)
-		goto done;
-
-	/* Bump dongle by sending an empty packet on the event channel.
-	 * sdpcm_sendup (RX) checks for virtual console input.
-	 */
-	if ((pkt = PKTGET(bus->dhd->osh, 4 + SDPCM_RESERVE, TRUE)) != NULL)
-		dhdsdio_txpkt(bus, pkt, SDPCM_EVENT_CHANNEL, TRUE);
-
-done:
-	if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
-		bus->activity = FALSE;
-		dhdsdio_clkctl(bus, CLK_NONE, TRUE);
-	}
-
-	dhd_os_sdunlock(bus->dhd);
-
-	return rv;
-}
-#endif /* DHD_DEBUG */
-
-#ifdef DHD_DEBUG
-static void
-dhd_dump_cis(uint fn, uint8 *cis)
-{
-	uint byte, tag, tdata;
-	DHD_INFO(("Function %d CIS:\n", fn));
-
-	for (tdata = byte = 0; byte < SBSDIO_CIS_SIZE_LIMIT; byte++) {
-		if ((byte % 16) == 0)
-			DHD_INFO(("    "));
-		DHD_INFO(("%02x ", cis[byte]));
-		if ((byte % 16) == 15)
-			DHD_INFO(("\n"));
-		if (!tdata--) {
-			tag = cis[byte];
-			if (tag == 0xff)
-				break;
-			else if (!tag)
-				tdata = 0;
-			else if ((byte + 1) < SBSDIO_CIS_SIZE_LIMIT)
-				tdata = cis[byte + 1] + 1;
-			else
-				DHD_INFO(("]"));
-		}
-	}
-	if ((byte % 16) != 15)
-		DHD_INFO(("\n"));
-}
-#endif /* DHD_DEBUG */
-
-static bool
-dhdsdio_chipmatch(uint16 chipid)
-{
-	if (chipid == BCM4325_CHIP_ID)
-		return TRUE;
-	if (chipid == BCM4329_CHIP_ID)
-		return TRUE;
-	if (chipid == BCM4315_CHIP_ID)
-		return TRUE;
-	if (chipid == BCM4319_CHIP_ID)
-		return TRUE;
-	if (chipid == BCM4336_CHIP_ID)
-		return TRUE;
-	if (chipid == BCM4330_CHIP_ID)
-		return TRUE;
-	if (chipid == BCM43237_CHIP_ID)
-		return TRUE;
-	if (chipid == BCM43362_CHIP_ID)
-		return TRUE;
-	if (chipid == BCM43239_CHIP_ID)
-		return TRUE;
-	return FALSE;
-}
-
-static void *
-dhdsdio_probe(uint16 venid, uint16 devid, uint16 bus_no, uint16 slot,
-	uint16 func, uint bustype, void *regsva, osl_t * osh, void *sdh)
-{
-	int ret;
-	dhd_bus_t *bus;
-	dhd_cmn_t *cmn;
-#ifdef GET_CUSTOM_MAC_ENABLE
-	struct ether_addr ea_addr;
-#endif /* GET_CUSTOM_MAC_ENABLE */
-
-	/* Init global variables at run-time, not as part of the declaration.
-	 * This is required to support init/de-init of the driver. Initialization
-	 * of globals as part of the declaration results in non-deterministic
-	 * behavior since the value of the globals may be different on the
-	 * first time that the driver is initialized vs subsequent initializations.
-	 */
-	dhd_txbound = DHD_TXBOUND;
-	dhd_rxbound = DHD_RXBOUND;
-	dhd_alignctl = TRUE;
-	sd1idle = TRUE;
-	dhd_readahead = TRUE;
-	retrydata = FALSE;
-	dhd_doflow = FALSE;
-	dhd_dongle_memsize = 0;
-	dhd_txminmax = DHD_TXMINMAX;
-
-	forcealign = TRUE;
-
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-	DHD_INFO(("%s: venid 0x%04x devid 0x%04x\n", __FUNCTION__, venid, devid));
-
-	/* We make assumptions about address window mappings */
-	ASSERT((uintptr)regsva == SI_ENUM_BASE);
-
-	/* BCMSDH passes venid and devid based on CIS parsing -- but low-power start
-	 * means early parse could fail, so here we should get either an ID
-	 * we recognize OR (-1) indicating we must request power first.
-	 */
-	/* Check the Vendor ID */
-	switch (venid) {
-		case 0x0000:
-		case VENDOR_BROADCOM:
-			break;
-		default:
-			DHD_ERROR(("%s: unknown vendor: 0x%04x\n",
-			           __FUNCTION__, venid));
-			return NULL;
-	}
-
-	/* Check the Device ID and make sure it's one that we support */
-	switch (devid) {
-		case BCM4325_D11DUAL_ID:		/* 4325 802.11a/g id */
-		case BCM4325_D11G_ID:			/* 4325 802.11g 2.4Ghz band id */
-		case BCM4325_D11A_ID:			/* 4325 802.11a 5Ghz band id */
-			DHD_INFO(("%s: found 4325 Dongle\n", __FUNCTION__));
-			break;
-		case BCM4329_D11N_ID:		/* 4329 802.11n dualband device */
-		case BCM4329_D11N2G_ID:		/* 4329 802.11n 2.4G device */
-		case BCM4329_D11N5G_ID:		/* 4329 802.11n 5G device */
-		case 0x4329:
-			DHD_INFO(("%s: found 4329 Dongle\n", __FUNCTION__));
-			break;
-		case BCM4315_D11DUAL_ID:		/* 4315 802.11a/g id */
-		case BCM4315_D11G_ID:			/* 4315 802.11g id */
-		case BCM4315_D11A_ID:			/* 4315 802.11a id */
-			DHD_INFO(("%s: found 4315 Dongle\n", __FUNCTION__));
-			break;
-		case BCM4319_D11N_ID:			/* 4319 802.11n id */
-		case BCM4319_D11N2G_ID:			/* 4319 802.11n2g id */
-		case BCM4319_D11N5G_ID:			/* 4319 802.11n5g id */
-			DHD_INFO(("%s: found 4319 Dongle\n", __FUNCTION__));
-			break;
-		case 0:
-			DHD_INFO(("%s: allow device id 0, will check chip internals\n",
-			          __FUNCTION__));
-			break;
-
-		default:
-			DHD_ERROR(("%s: skipping 0x%04x/0x%04x, not a dongle\n",
-			           __FUNCTION__, venid, devid));
-			return NULL;
-	}
-
-	if (osh == NULL) {
-		/* Ask the OS interface part for an OSL handle */
-		if (!(osh = dhd_osl_attach(sdh, DHD_BUS))) {
-			DHD_ERROR(("%s: osl_attach failed!\n", __FUNCTION__));
-			return NULL;
-		}
-	}
-
-	/* Allocate private bus interface state */
-	if (!(bus = MALLOC(osh, sizeof(dhd_bus_t)))) {
-		DHD_ERROR(("%s: MALLOC of dhd_bus_t failed\n", __FUNCTION__));
-		goto fail;
-	}
-	bzero(bus, sizeof(dhd_bus_t));
-	bus->sdh = sdh;
-	bus->cl_devid = (uint16)devid;
-	bus->bus = DHD_BUS;
-	bus->tx_seq = SDPCM_SEQUENCE_WRAP - 1;
-	bus->usebufpool = FALSE; /* Use bufpool if allocated, else use locally malloced rxbuf */
-
-	/* attach the common module */
-	if (!(cmn = dhd_common_init(osh))) {
-		DHD_ERROR(("%s: dhd_common_init failed\n", __FUNCTION__));
-		goto fail;
-	}
-
-	/* attempt to attach to the dongle */
-	if (!(dhdsdio_probe_attach(bus, osh, sdh, regsva, devid))) {
-		DHD_ERROR(("%s: dhdsdio_probe_attach failed\n", __FUNCTION__));
-		dhd_common_deinit(NULL, cmn);
-		goto fail;
-	}
-
-	/* Attach to the dhd/OS/network interface */
-	if (!(bus->dhd = dhd_attach(osh, bus, SDPCM_RESERVE))) {
-		DHD_ERROR(("%s: dhd_attach failed\n", __FUNCTION__));
-		goto fail;
-	}
-
-	bus->dhd->cmn = cmn;
-	cmn->dhd = bus->dhd;
-
-	/* Allocate buffers */
-	if (!(dhdsdio_probe_malloc(bus, osh, sdh))) {
-		DHD_ERROR(("%s: dhdsdio_probe_malloc failed\n", __FUNCTION__));
-		goto fail;
-	}
-
-	if (!(dhdsdio_probe_init(bus, osh, sdh))) {
-		DHD_ERROR(("%s: dhdsdio_probe_init failed\n", __FUNCTION__));
-		goto fail;
-	}
-
-	if (bus->intr) {
-		/* Register interrupt callback, but mask it (not operational yet). */
-		DHD_INTR(("%s: disable SDIO interrupts (not interested yet)\n", __FUNCTION__));
-		bcmsdh_intr_disable(sdh);
-		if ((ret = bcmsdh_intr_reg(sdh, dhdsdio_isr, bus)) != 0) {
-			DHD_ERROR(("%s: FAILED: bcmsdh_intr_reg returned %d\n",
-			           __FUNCTION__, ret));
-			goto fail;
-		}
-		DHD_INTR(("%s: registered SDIO interrupt function ok\n", __FUNCTION__));
-	} else {
-		DHD_INFO(("%s: SDIO interrupt function is NOT registered due to polling mode\n",
-		           __FUNCTION__));
-	}
-
-	DHD_INFO(("%s: completed!!\n", __FUNCTION__));
-
-#ifdef GET_CUSTOM_MAC_ENABLE
-	/* Read MAC address from external customer place 	*/
-	memset(&ea_addr, 0, sizeof(ea_addr));
-	ret = dhd_custom_get_mac_address(ea_addr.octet);
-	if (!ret) {
-		memcpy(bus->dhd->mac.octet, (void *)&ea_addr, ETHER_ADDR_LEN);
-	}
-#endif /* GET_CUSTOM_MAC_ENABLE */
-
-	/* if firmware path present try to download and bring up bus */
-	if (dhd_download_fw_on_driverload && (ret = dhd_bus_start(bus->dhd)) != 0) {
-		DHD_ERROR(("%s: dhd_bus_start failed\n", __FUNCTION__));
-		if (ret == BCME_NOTUP)
-			goto fail;
-	}
-	/* Ok, have the per-port tell the stack we're open for business */
-	if (dhd_net_attach(bus->dhd, 0) != 0) {
-		DHD_ERROR(("%s: Net attach failed!!\n", __FUNCTION__));
-		goto fail;
-	}
-
-	return bus;
-
-fail:
-	dhdsdio_release(bus, osh);
-	return NULL;
-}
-
-static bool
-dhdsdio_probe_attach(struct dhd_bus *bus, osl_t *osh, void *sdh, void *regsva,
-                     uint16 devid)
-{
-	int err = 0;
-	uint8 clkctl = 0;
-
-	bus->alp_only = TRUE;
-
-	/* Return the window to backplane enumeration space for core access */
-	if (dhdsdio_set_siaddr_window(bus, SI_ENUM_BASE)) {
-		DHD_ERROR(("%s: FAILED to return to SI_ENUM_BASE\n", __FUNCTION__));
-	}
-
-#ifdef DHD_DEBUG
-	DHD_ERROR(("F1 signature read @0x18000000=0x%4x\n",
-	       bcmsdh_reg_read(bus->sdh, SI_ENUM_BASE, 4)));
-
-#endif /* DHD_DEBUG */
-
-
-	/* Force PLL off until si_attach() programs PLL control regs */
-
-
-
-	bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, DHD_INIT_CLKCTL1, &err);
-	if (!err)
-		clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
-
-	if (err || ((clkctl & ~SBSDIO_AVBITS) != DHD_INIT_CLKCTL1)) {
-		DHD_ERROR(("dhdsdio_probe: ChipClkCSR access: err %d wrote 0x%02x read 0x%02x\n",
-		           err, DHD_INIT_CLKCTL1, clkctl));
-		goto fail;
-	}
-
-
-#ifdef DHD_DEBUG
-	if (DHD_INFO_ON()) {
-		uint fn, numfn;
-		uint8 *cis[SDIOD_MAX_IOFUNCS];
-		int err = 0;
-
-		numfn = bcmsdh_query_iofnum(sdh);
-		ASSERT(numfn <= SDIOD_MAX_IOFUNCS);
-
-		/* Make sure ALP is available before trying to read CIS */
-		SPINWAIT(((clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
-		                                    SBSDIO_FUNC1_CHIPCLKCSR, NULL)),
-		          !SBSDIO_ALPAV(clkctl)), PMU_MAX_TRANSITION_DLY);
-
-		/* Now request ALP be put on the bus */
-		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
-		                 DHD_INIT_CLKCTL2, &err);
-		OSL_DELAY(65);
-
-		for (fn = 0; fn <= numfn; fn++) {
-			if (!(cis[fn] = MALLOC(osh, SBSDIO_CIS_SIZE_LIMIT))) {
-				DHD_INFO(("dhdsdio_probe: fn %d cis malloc failed\n", fn));
-				break;
-			}
-			bzero(cis[fn], SBSDIO_CIS_SIZE_LIMIT);
-
-			if ((err = bcmsdh_cis_read(sdh, fn, cis[fn], SBSDIO_CIS_SIZE_LIMIT))) {
-				DHD_INFO(("dhdsdio_probe: fn %d cis read err %d\n", fn, err));
-				MFREE(osh, cis[fn], SBSDIO_CIS_SIZE_LIMIT);
-				break;
-			}
-			dhd_dump_cis(fn, cis[fn]);
-		}
-
-		while (fn-- > 0) {
-			ASSERT(cis[fn]);
-			MFREE(osh, cis[fn], SBSDIO_CIS_SIZE_LIMIT);
-		}
-
-		if (err) {
-			DHD_ERROR(("dhdsdio_probe: failure reading or parsing CIS\n"));
-			goto fail;
-		}
-	}
-#endif /* DHD_DEBUG */
-
-	/* si_attach() will provide an SI handle and scan the backplane */
-	if (!(bus->sih = si_attach((uint)devid, osh, regsva, DHD_BUS, sdh,
-	                           &bus->vars, &bus->varsz))) {
-		DHD_ERROR(("%s: si_attach failed!\n", __FUNCTION__));
-		goto fail;
-	}
-
-	bcmsdh_chipinfo(sdh, bus->sih->chip, bus->sih->chiprev);
-
-	if (!dhdsdio_chipmatch((uint16)bus->sih->chip)) {
-		DHD_ERROR(("%s: unsupported chip: 0x%04x\n",
-		           __FUNCTION__, bus->sih->chip));
-		goto fail;
-	}
-
-
-	si_sdiod_drive_strength_init(bus->sih, osh, dhd_sdiod_drive_strength);
-
-
-	/* Get info on the ARM and SOCRAM cores... */
-	if (!DHD_NOPMU(bus)) {
-		if ((si_setcore(bus->sih, ARM7S_CORE_ID, 0)) ||
-		    (si_setcore(bus->sih, ARMCM3_CORE_ID, 0))) {
-			bus->armrev = si_corerev(bus->sih);
-		} else {
-			DHD_ERROR(("%s: failed to find ARM core!\n", __FUNCTION__));
-			goto fail;
-		}
-		if (!(bus->orig_ramsize = si_socram_size(bus->sih))) {
-			DHD_ERROR(("%s: failed to find SOCRAM memory!\n", __FUNCTION__));
-			goto fail;
-		}
-		bus->ramsize = bus->orig_ramsize;
-		if (dhd_dongle_memsize)
-			dhd_dongle_setmemsize(bus, dhd_dongle_memsize);
-
-		DHD_ERROR(("DHD: dongle ram size is set to %d(orig %d)\n",
-			bus->ramsize, bus->orig_ramsize));
-	}
-
-	/* ...but normally deal with the SDPCMDEV core */
-	if (!(bus->regs = si_setcore(bus->sih, PCMCIA_CORE_ID, 0)) &&
-	    !(bus->regs = si_setcore(bus->sih, SDIOD_CORE_ID, 0))) {
-		DHD_ERROR(("%s: failed to find SDIODEV core!\n", __FUNCTION__));
-		goto fail;
-	}
-	bus->sdpcmrev = si_corerev(bus->sih);
-
-	/* Set core control so an SDIO reset does a backplane reset */
-	OR_REG(osh, &bus->regs->corecontrol, CC_BPRESEN);
-	bus->rxint_mode = SDIO_DEVICE_HMB_RXINT;
-
-	if ((bus->sih->buscoretype == SDIOD_CORE_ID) && (bus->sdpcmrev >= 4) &&
-		(bus->rxint_mode  == SDIO_DEVICE_RXDATAINT_MODE_1))
-	{
-		uint32 val;
-
-		val = R_REG(osh, &bus->regs->corecontrol);
-		val &= ~CC_XMTDATAAVAIL_MODE;
-		val |= CC_XMTDATAAVAIL_CTRL;
-		W_REG(osh, &bus->regs->corecontrol, val);
-	}
-
-
-	pktq_init(&bus->txq, (PRIOMASK + 1), QLEN);
-
-	/* Locate an appropriately-aligned portion of hdrbuf */
-	bus->rxhdr = (uint8 *)ROUNDUP((uintptr)&bus->hdrbuf[0], DHD_SDALIGN);
-
-	/* Set the poll and/or interrupt flags */
-	bus->intr = (bool)dhd_intr;
-	if ((bus->poll = (bool)dhd_poll))
-		bus->pollrate = 1;
-
-	return TRUE;
-
-fail:
-	if (bus->sih != NULL)
-		si_detach(bus->sih);
-	return FALSE;
-}
-
-static bool
-dhdsdio_probe_malloc(dhd_bus_t *bus, osl_t *osh, void *sdh)
-{
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (bus->dhd->maxctl) {
-		bus->rxblen = ROUNDUP((bus->dhd->maxctl + SDPCM_HDRLEN), ALIGNMENT) + DHD_SDALIGN;
-		if (!(bus->rxbuf = DHD_OS_PREALLOC(osh, DHD_PREALLOC_RXBUF, bus->rxblen))) {
-			DHD_ERROR(("%s: MALLOC of %d-byte rxbuf failed\n",
-			           __FUNCTION__, bus->rxblen));
-			goto fail;
-		}
-	}
-	/* Allocate buffer to receive glomed packet */
-	if (!(bus->databuf = DHD_OS_PREALLOC(osh, DHD_PREALLOC_DATABUF, MAX_DATA_BUF))) {
-		DHD_ERROR(("%s: MALLOC of %d-byte databuf failed\n",
-			__FUNCTION__, MAX_DATA_BUF));
-		/* release rxbuf which was already located as above */
-		if (!bus->rxblen)
-			DHD_OS_PREFREE(osh, bus->rxbuf, bus->rxblen);
-		goto fail;
-	}
-
-	/* Align the buffer */
-	if ((uintptr)bus->databuf % DHD_SDALIGN)
-		bus->dataptr = bus->databuf + (DHD_SDALIGN - ((uintptr)bus->databuf % DHD_SDALIGN));
-	else
-		bus->dataptr = bus->databuf;
-
-	return TRUE;
-
-fail:
-	return FALSE;
-}
-
-static bool
-dhdsdio_probe_init(dhd_bus_t *bus, osl_t *osh, void *sdh)
-{
-	int32 fnum;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-#ifdef SDTEST
-	dhdsdio_pktgen_init(bus);
-#endif /* SDTEST */
-
-	/* Disable F2 to clear any intermediate frame state on the dongle */
-	bcmsdh_cfg_write(sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, SDIO_FUNC_ENABLE_1, NULL);
-
-	bus->dhd->busstate = DHD_BUS_DOWN;
-	bus->sleeping = FALSE;
-	bus->rxflow = FALSE;
-	bus->prev_rxlim_hit = 0;
-
-
-	/* Done with backplane-dependent accesses, can drop clock... */
-	bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
-
-	/* ...and initialize clock/power states */
-	bus->clkstate = CLK_SDONLY;
-	bus->idletime = (int32)dhd_idletime;
-	bus->idleclock = DHD_IDLE_ACTIVE;
-
-	/* Query the SD clock speed */
-	if (bcmsdh_iovar_op(sdh, "sd_divisor", NULL, 0,
-	                    &bus->sd_divisor, sizeof(int32), FALSE) != BCME_OK) {
-		DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_divisor"));
-		bus->sd_divisor = -1;
-	} else {
-		DHD_INFO(("%s: Initial value for %s is %d\n",
-		          __FUNCTION__, "sd_divisor", bus->sd_divisor));
-	}
-
-	/* Query the SD bus mode */
-	if (bcmsdh_iovar_op(sdh, "sd_mode", NULL, 0,
-	                    &bus->sd_mode, sizeof(int32), FALSE) != BCME_OK) {
-		DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_mode"));
-		bus->sd_mode = -1;
-	} else {
-		DHD_INFO(("%s: Initial value for %s is %d\n",
-		          __FUNCTION__, "sd_mode", bus->sd_mode));
-	}
-
-	/* Query the F2 block size, set roundup accordingly */
-	fnum = 2;
-	if (bcmsdh_iovar_op(sdh, "sd_blocksize", &fnum, sizeof(int32),
-	                    &bus->blocksize, sizeof(int32), FALSE) != BCME_OK) {
-		bus->blocksize = 0;
-		DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_blocksize"));
-	} else {
-		DHD_INFO(("%s: Initial value for %s is %d\n",
-		          __FUNCTION__, "sd_blocksize", bus->blocksize));
-	}
-	bus->roundup = MIN(max_roundup, bus->blocksize);
-
-	/* Query if bus module supports packet chaining, default to use if supported */
-	if (bcmsdh_iovar_op(sdh, "sd_rxchain", NULL, 0,
-	                    &bus->sd_rxchain, sizeof(int32), FALSE) != BCME_OK) {
-		bus->sd_rxchain = FALSE;
-	} else {
-		DHD_INFO(("%s: bus module (through bcmsdh API) %s chaining\n",
-		          __FUNCTION__, (bus->sd_rxchain ? "supports" : "does not support")));
-	}
-	bus->use_rxchain = (bool)bus->sd_rxchain;
-
-	return TRUE;
-}
-
-bool
-dhd_bus_download_firmware(struct dhd_bus *bus, osl_t *osh,
-                          char *pfw_path, char *pnv_path)
-{
-	bool ret;
-	bus->fw_path = pfw_path;
-	bus->nv_path = pnv_path;
-
-	ret = dhdsdio_download_firmware(bus, osh, bus->sdh);
-
-
-	return ret;
-}
-
-static bool
-dhdsdio_download_firmware(struct dhd_bus *bus, osl_t *osh, void *sdh)
-{
-	bool ret;
-
-	/* Download the firmware */
-	DHD_OS_WAKE_LOCK(bus->dhd);
-	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-
-	ret = _dhdsdio_download_firmware(bus) == 0;
-
-	dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
-	DHD_OS_WAKE_UNLOCK(bus->dhd);
-	return ret;
-}
-
-/* Detach and free everything */
-static void
-dhdsdio_release(dhd_bus_t *bus, osl_t *osh)
-{
-	bool dongle_isolation = FALSE;
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (bus) {
-		ASSERT(osh);
-
-		/* De-register interrupt handler */
-		bcmsdh_intr_disable(bus->sdh);
-		bcmsdh_intr_dereg(bus->sdh);
-
-		if (bus->dhd) {
-			dhd_common_deinit(bus->dhd, NULL);
-			dongle_isolation = bus->dhd->dongle_isolation;
-			dhd_detach(bus->dhd);
-			dhdsdio_release_dongle(bus, osh, dongle_isolation, TRUE);
-			dhd_free(bus->dhd);
-			bus->dhd = NULL;
-		}
-
-		dhdsdio_release_malloc(bus, osh);
-
-#ifdef DHD_DEBUG
-		if (bus->console.buf != NULL)
-			MFREE(osh, bus->console.buf, bus->console.bufsize);
-#endif
-
-		MFREE(osh, bus, sizeof(dhd_bus_t));
-	}
-
-	if (osh)
-		dhd_osl_detach(osh);
-
-	DHD_TRACE(("%s: Disconnected\n", __FUNCTION__));
-}
-
-static void
-dhdsdio_release_malloc(dhd_bus_t *bus, osl_t *osh)
-{
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (bus->dhd && bus->dhd->dongle_reset)
-		return;
-
-	if (bus->rxbuf) {
-#ifndef DHD_USE_STATIC_BUF
-		MFREE(osh, bus->rxbuf, bus->rxblen);
-#endif
-		bus->rxctl = bus->rxbuf = NULL;
-		bus->rxlen = 0;
-	}
-
-	if (bus->databuf) {
-#ifndef DHD_USE_STATIC_BUF
-		MFREE(osh, bus->databuf, MAX_DATA_BUF);
-#endif
-		bus->databuf = NULL;
-	}
-
-	if (bus->vars && bus->varsz) {
-		MFREE(osh, bus->vars, bus->varsz);
-		bus->vars = NULL;
-	}
-
-}
-
-
-static void
-dhdsdio_release_dongle(dhd_bus_t *bus, osl_t *osh, bool dongle_isolation, bool reset_flag)
-{
-	DHD_TRACE(("%s: Enter bus->dhd %p bus->dhd->dongle_reset %d \n", __FUNCTION__,
-		bus->dhd, bus->dhd->dongle_reset));
-
-	if ((bus->dhd && bus->dhd->dongle_reset) && reset_flag)
-		return;
-
-	if (bus->sih) {
-		if (bus->dhd) {
-			dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
-		}
-#if !defined(BCMLXSDMMC)
-		if (dongle_isolation == FALSE)
-			si_watchdog(bus->sih, 4);
-#endif /* !defined(BCMLXSDMMC) */
-		if (bus->dhd) {
-			dhdsdio_clkctl(bus, CLK_NONE, FALSE);
-		}
-		si_detach(bus->sih);
-		if (bus->vars && bus->varsz)
-			MFREE(osh, bus->vars, bus->varsz);
-		bus->vars = NULL;
-	}
-
-	DHD_TRACE(("%s: Disconnected\n", __FUNCTION__));
-}
-
-static void
-dhdsdio_disconnect(void *ptr)
-{
-	dhd_bus_t *bus = (dhd_bus_t *)ptr;
-
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	if (bus) {
-		ASSERT(bus->dhd);
-		dhdsdio_release(bus, bus->dhd->osh);
-	}
-
-	DHD_TRACE(("%s: Disconnected\n", __FUNCTION__));
-}
-
-
-/* Register/Unregister functions are called by the main DHD entry
- * point (e.g. module insertion) to link with the bus driver, in
- * order to look for or await the device.
- */
-
-static bcmsdh_driver_t dhd_sdio = {
-	dhdsdio_probe,
-	dhdsdio_disconnect
-};
-
-int
-dhd_bus_register(void)
-{
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	return bcmsdh_register(&dhd_sdio);
-}
-
-void
-dhd_bus_unregister(void)
-{
-	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
-
-	bcmsdh_unregister();
-}
-
-#ifdef BCMEMBEDIMAGE
-static int
-dhdsdio_download_code_array(struct dhd_bus *bus)
-{
-	int bcmerror = -1;
-	int offset = 0;
-	unsigned char *ularray = NULL;
-
-	DHD_INFO(("%s: download embedded firmware...\n", __FUNCTION__));
-
-	/* Download image */
-	while ((offset + MEMBLOCK) < sizeof(dlarray)) {
-		bcmerror = dhdsdio_membytes(bus, TRUE, offset,
-			(uint8 *) (dlarray + offset), MEMBLOCK);
-		if (bcmerror) {
-			DHD_ERROR(("%s: error %d on writing %d membytes at 0x%08x\n",
-			        __FUNCTION__, bcmerror, MEMBLOCK, offset));
-			goto err;
-		}
-
-		offset += MEMBLOCK;
-	}
-
-	if (offset < sizeof(dlarray)) {
-		bcmerror = dhdsdio_membytes(bus, TRUE, offset,
-			(uint8 *) (dlarray + offset), sizeof(dlarray) - offset);
-		if (bcmerror) {
-			DHD_ERROR(("%s: error %d on writing %d membytes at 0x%08x\n",
-			        __FUNCTION__, bcmerror, sizeof(dlarray) - offset, offset));
-			goto err;
-		}
-	}
-
-#ifdef DHD_DEBUG
-	/* Upload and compare the downloaded code */
-	{
-		ularray = MALLOC(bus->dhd->osh, bus->ramsize);
-		/* Upload image to verify downloaded contents. */
-		offset = 0;
-		memset(ularray, 0xaa, bus->ramsize);
-		while ((offset + MEMBLOCK) < sizeof(dlarray)) {
-			bcmerror = dhdsdio_membytes(bus, FALSE, offset, ularray + offset, MEMBLOCK);
-			if (bcmerror) {
-				DHD_ERROR(("%s: error %d on reading %d membytes at 0x%08x\n",
-					__FUNCTION__, bcmerror, MEMBLOCK, offset));
-				goto err;
-			}
-
-			offset += MEMBLOCK;
-		}
-
-		if (offset < sizeof(dlarray)) {
-			bcmerror = dhdsdio_membytes(bus, FALSE, offset,
-				ularray + offset, sizeof(dlarray) - offset);
-			if (bcmerror) {
-				DHD_ERROR(("%s: error %d on reading %d membytes at 0x%08x\n",
-					__FUNCTION__, bcmerror, sizeof(dlarray) - offset, offset));
-				goto err;
-			}
-		}
-
-		if (memcmp(dlarray, ularray, sizeof(dlarray))) {
-			DHD_ERROR(("%s: Downloaded image is corrupted (%s, %s, %s).\n",
-			           __FUNCTION__, dlimagename, dlimagever, dlimagedate));
-			goto err;
-		} else
-			DHD_ERROR(("%s: Download, Upload and compare succeeded (%s, %s, %s).\n",
-			           __FUNCTION__, dlimagename, dlimagever, dlimagedate));
-
-	}
-#endif /* DHD_DEBUG */
-
-err:
-	if (ularray)
-		MFREE(bus->dhd->osh, ularray, bus->ramsize);
-	return bcmerror;
-}
-#endif /* BCMEMBEDIMAGE */
-
-static int
-dhdsdio_download_code_file(struct dhd_bus *bus, char *pfw_path)
-{
-	int bcmerror = -1;
-	int offset = 0;
-	uint len;
-	void *image = NULL;
-	uint8 *memblock = NULL, *memptr;
-
-	DHD_INFO(("%s: download firmware %s\n", __FUNCTION__, pfw_path));
-
-	image = dhd_os_open_image(pfw_path);
-	if (image == NULL)
-		goto err;
-
-	memptr = memblock = MALLOC(bus->dhd->osh, MEMBLOCK + DHD_SDALIGN);
-	if (memblock == NULL) {
-		DHD_ERROR(("%s: Failed to allocate memory %d bytes\n", __FUNCTION__, MEMBLOCK));
-		goto err;
-	}
-	if ((uint32)(uintptr)memblock % DHD_SDALIGN)
-		memptr += (DHD_SDALIGN - ((uint32)(uintptr)memblock % DHD_SDALIGN));
-
-	/* Download image */
-	while ((len = dhd_os_get_image_block((char*)memptr, MEMBLOCK, image))) {
-		bcmerror = dhdsdio_membytes(bus, TRUE, offset, memptr, len);
-		if (bcmerror) {
-			DHD_ERROR(("%s: error %d on writing %d membytes at 0x%08x\n",
-			        __FUNCTION__, bcmerror, MEMBLOCK, offset));
-			goto err;
-		}
-
-		offset += MEMBLOCK;
-	}
-
-err:
-	if (memblock)
-		MFREE(bus->dhd->osh, memblock, MEMBLOCK + DHD_SDALIGN);
-
-	if (image)
-		dhd_os_close_image(image);
-
-	return bcmerror;
-}
-
-/* 
-	EXAMPLE: nvram_array
-	nvram_arry format:
-	name=value
-	Use carriage return at the end of each assignment, and an empty string with
-	carriage return at the end of array.
-
-	For example:
-	unsigned char  nvram_array[] = {"name1=value1\n", "name2=value2\n", "\n"};
-	Hex values start with 0x, and mac addr format: xx:xx:xx:xx:xx:xx.
-
-	Search "EXAMPLE: nvram_array" to see how the array is activated.
-*/
-
-void
-dhd_bus_set_nvram_params(struct dhd_bus * bus, const char *nvram_params)
-{
-	bus->nvram_params = nvram_params;
-}
-
-static int
-dhdsdio_download_nvram(struct dhd_bus *bus)
-{
-	int bcmerror = -1;
-	uint len;
-	void * image = NULL;
-	char * memblock = NULL;
-	char *bufp;
-	char *pnv_path;
-	bool nvram_file_exists;
-
-	pnv_path = bus->nv_path;
-
-	nvram_file_exists = ((pnv_path != NULL) && (pnv_path[0] != '\0'));
-	if (!nvram_file_exists && (bus->nvram_params == NULL))
-		return (0);
-
-	if (nvram_file_exists) {
-		image = dhd_os_open_image(pnv_path);
-		if (image == NULL)
-			goto err;
-	}
-
-	memblock = MALLOC(bus->dhd->osh, MAX_NVRAMBUF_SIZE);
-	if (memblock == NULL) {
-		DHD_ERROR(("%s: Failed to allocate memory %d bytes\n",
-		           __FUNCTION__, MAX_NVRAMBUF_SIZE));
-		goto err;
-	}
-
-	/* Download variables */
-	if (nvram_file_exists) {
-		len = dhd_os_get_image_block(memblock, MAX_NVRAMBUF_SIZE, image);
-	}
-	else {
-		len = strlen(bus->nvram_params);
-		ASSERT(len <= MAX_NVRAMBUF_SIZE);
-		memcpy(memblock, bus->nvram_params, len);
-	}
-	if (len > 0 && len < MAX_NVRAMBUF_SIZE) {
-		bufp = (char *)memblock;
-		bufp[len] = 0;
-		len = process_nvram_vars(bufp, len);
-		if (len % 4) {
-			len += 4 - (len % 4);
-		}
-		bufp += len;
-		*bufp++ = 0;
-		if (len)
-			bcmerror = dhdsdio_downloadvars(bus, memblock, len + 1);
-		if (bcmerror) {
-			DHD_ERROR(("%s: error downloading vars: %d\n",
-			           __FUNCTION__, bcmerror));
-		}
-	}
-	else {
-		DHD_ERROR(("%s: error reading nvram file: %d\n",
-		           __FUNCTION__, len));
-		bcmerror = BCME_SDIO_ERROR;
-	}
-
-err:
-	if (memblock)
-		MFREE(bus->dhd->osh, memblock, MAX_NVRAMBUF_SIZE);
-
-	if (image)
-		dhd_os_close_image(image);
-
-	return bcmerror;
-}
-
-static int
-_dhdsdio_download_firmware(struct dhd_bus *bus)
-{
-	int bcmerror = -1;
-
-	bool embed = FALSE;	/* download embedded firmware */
-	bool dlok = FALSE;	/* download firmware succeeded */
-
-	/* Out immediately if no image to download */
-	if ((bus->fw_path == NULL) || (bus->fw_path[0] == '\0')) {
-#ifdef BCMEMBEDIMAGE
-		embed = TRUE;
-#else
-		return 0;
-#endif
-	}
-
-	/* Keep arm in reset */
-	if (dhdsdio_download_state(bus, TRUE)) {
-		DHD_ERROR(("%s: error placing ARM core in reset\n", __FUNCTION__));
-		goto err;
-	}
-
-	/* External image takes precedence if specified */
-	if ((bus->fw_path != NULL) && (bus->fw_path[0] != '\0')) {
-		if (dhdsdio_download_code_file(bus, bus->fw_path)) {
-			DHD_ERROR(("%s: dongle image file download failed\n", __FUNCTION__));
-#ifdef BCMEMBEDIMAGE
-			embed = TRUE;
-#else
-			goto err;
-#endif
-		}
-		else {
-			embed = FALSE;
-			dlok = TRUE;
-		}
-	}
-#ifdef BCMEMBEDIMAGE
-	if (embed) {
-		if (dhdsdio_download_code_array(bus)) {
-			DHD_ERROR(("%s: dongle image array download failed\n", __FUNCTION__));
-			goto err;
-		}
-		else {
-			dlok = TRUE;
-		}
-	}
-#endif
-	if (!dlok) {
-		DHD_ERROR(("%s: dongle image download failed\n", __FUNCTION__));
-		goto err;
-	}
-
-	/* EXAMPLE: nvram_array */
-	/* If a valid nvram_arry is specified as above, it can be passed down to dongle */
-	/* dhd_bus_set_nvram_params(bus, (char *)&nvram_array); */
-
-	/* External nvram takes precedence if specified */
-	if (dhdsdio_download_nvram(bus)) {
-		DHD_ERROR(("%s: dongle nvram file download failed\n", __FUNCTION__));
-		goto err;
-	}
-
-	/* Take arm out of reset */
-	if (dhdsdio_download_state(bus, FALSE)) {
-		DHD_ERROR(("%s: error getting out of ARM core reset\n", __FUNCTION__));
-		goto err;
-	}
-
-	bcmerror = 0;
-
-err:
-	return bcmerror;
-}
-
-static int
-dhd_bcmsdh_recv_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags, uint8 *buf, uint nbytes,
-	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle)
-{
-	int status;
-
-	status = bcmsdh_recv_buf(bus->sdh, addr, fn, flags, buf, nbytes, pkt, complete, handle);
-
-	return status;
-}
-
-static int
-dhd_bcmsdh_send_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags, uint8 *buf, uint nbytes,
-	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle)
-{
-	return (bcmsdh_send_buf(bus->sdh, addr, fn, flags, buf, nbytes, pkt, complete, handle));
-}
-
-uint
-dhd_bus_chip(struct dhd_bus *bus)
-{
-	ASSERT(bus->sih != NULL);
-	return bus->sih->chip;
-}
-
-void *
-dhd_bus_pub(struct dhd_bus *bus)
-{
-	return bus->dhd;
-}
-
-void *
-dhd_bus_txq(struct dhd_bus *bus)
-{
-	return &bus->txq;
-}
-
-uint
-dhd_bus_hdrlen(struct dhd_bus *bus)
-{
-	return SDPCM_HDRLEN;
-}
-
-int
-dhd_bus_devreset(dhd_pub_t *dhdp, uint8 flag)
-{
-	int bcmerror = 0;
-	dhd_bus_t *bus;
-
-	bus = dhdp->bus;
-
-	if (flag == TRUE) {
-		if (!bus->dhd->dongle_reset) {
-			dhd_os_sdlock(dhdp);
-			dhd_os_wd_timer(dhdp, 0);
-#if !defined(IGNORE_ETH0_DOWN)
-			/* Force flow control as protection when stop come before ifconfig_down */
-			dhd_txflowcontrol(bus->dhd, ALL_INTERFACES, ON);
-#endif /* !defined(IGNORE_ETH0_DOWN) */
-			/* Expect app to have torn down any connection before calling */
-			/* Stop the bus, disable F2 */
-			dhd_bus_stop(bus, FALSE);
-
-#if defined(OOB_INTR_ONLY)
-			/* Clean up any pending IRQ */
-			bcmsdh_set_irq(FALSE);
-#endif /* defined(OOB_INTR_ONLY) */
-
-			/* Clean tx/rx buffer pointers, detach from the dongle */
-			dhdsdio_release_dongle(bus, bus->dhd->osh, TRUE, TRUE);
-
-			bus->dhd->dongle_reset = TRUE;
-			bus->dhd->up = FALSE;
-			dhd_os_sdunlock(dhdp);
-
-			DHD_TRACE(("%s:  WLAN OFF DONE\n", __FUNCTION__));
-			/* App can now remove power from device */
-		} else
-			bcmerror = BCME_SDIO_ERROR;
-	} else {
-		/* App must have restored power to device before calling */
-
-		DHD_TRACE(("\n\n%s: == WLAN ON ==\n", __FUNCTION__));
-
-		if (bus->dhd->dongle_reset) {
-			/* Turn on WLAN */
-			dhd_os_sdlock(dhdp);
-
-			/* Reset SD client */
-			bcmsdh_reset(bus->sdh);
-
-			/* Attempt to re-attach & download */
-			if (dhdsdio_probe_attach(bus, bus->dhd->osh, bus->sdh,
-			                        (uint32 *)SI_ENUM_BASE,
-			                        bus->cl_devid)) {
-				/* Attempt to download binary to the dongle */
-				if (dhdsdio_probe_init(bus, bus->dhd->osh, bus->sdh) &&
-					dhdsdio_download_firmware(bus, bus->dhd->osh, bus->sdh)) {
-
-					/* Re-init bus, enable F2 transfer */
-					bcmerror = dhd_bus_init((dhd_pub_t *) bus->dhd, FALSE);
-					if (bcmerror == BCME_OK) {
-#if defined(OOB_INTR_ONLY)
-						bcmsdh_set_irq(TRUE);
-						dhd_enable_oob_intr(bus, TRUE);
-#endif /* defined(OOB_INTR_ONLY) */
-
-					bus->dhd->dongle_reset = FALSE;
-					bus->dhd->up = TRUE;
-
-#if !defined(IGNORE_ETH0_DOWN)
-					/* Restore flow control  */
-					dhd_txflowcontrol(bus->dhd, ALL_INTERFACES, OFF);
-#endif 
-					dhd_os_wd_timer(dhdp, dhd_watchdog_ms);
-
-					DHD_TRACE(("%s: WLAN ON DONE\n", __FUNCTION__));
-					} else {
-						dhd_bus_stop(bus, FALSE);
-						dhdsdio_release_dongle(bus, bus->dhd->osh,
-							TRUE, FALSE);
-					}
-				} else
-					bcmerror = BCME_SDIO_ERROR;
-			} else
-				bcmerror = BCME_SDIO_ERROR;
-
-			dhd_os_sdunlock(dhdp);
-		} else {
-			DHD_INFO(("%s called when dongle is not in reset\n",
-				__FUNCTION__));
-			DHD_INFO(("Will call dhd_bus_start instead\n"));
-			sdioh_start(NULL, 1);
-			if ((bcmerror = dhd_bus_start(dhdp)) != 0)
-				DHD_ERROR(("%s: dhd_bus_start fail with %d\n",
-					__FUNCTION__, bcmerror));
-		}
-	}
-	return bcmerror;
-}
-
-int
-dhd_bus_membytes(dhd_pub_t *dhdp, bool set, uint32 address, uint8 *data, uint size)
-{
-	dhd_bus_t *bus;
-
-	bus = dhdp->bus;
-	return dhdsdio_membytes(bus, set, address, data, size);
-}
diff --git a/drivers/net/wireless/bcmdhd/include/bcmutils.h b/drivers/net/wireless/bcmdhd/include/bcmutils.h
deleted file mode 100644
index 530036f0ba7..00000000000
--- a/drivers/net/wireless/bcmdhd/include/bcmutils.h
+++ /dev/null
@@ -1,708 +0,0 @@
-/*
- * Misc useful os-independent macros and functions.
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: bcmutils.h,v 13.236.2.16 2011-01-26 00:45:06 Exp $
- */
-
-
-#ifndef	_bcmutils_h_
-#define	_bcmutils_h_
-
-#define bcm_strcpy_s(dst, noOfElements, src)            strcpy((dst), (src))
-#define bcm_strncpy_s(dst, noOfElements, src, count)    strncpy((dst), (src), (count))
-#define bcm_strcat_s(dst, noOfElements, src)            strcat((dst), (src))
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-#define _BCM_U	0x01	
-#define _BCM_L	0x02	
-#define _BCM_D	0x04	
-#define _BCM_C	0x08	
-#define _BCM_P	0x10	
-#define _BCM_S	0x20	
-#define _BCM_X	0x40	
-#define _BCM_SP	0x80	
-
-extern const unsigned char bcm_ctype[];
-#define bcm_ismask(x)	(bcm_ctype[(int)(unsigned char)(x)])
-
-#define bcm_isalnum(c)	((bcm_ismask(c)&(_BCM_U|_BCM_L|_BCM_D)) != 0)
-#define bcm_isalpha(c)	((bcm_ismask(c)&(_BCM_U|_BCM_L)) != 0)
-#define bcm_iscntrl(c)	((bcm_ismask(c)&(_BCM_C)) != 0)
-#define bcm_isdigit(c)	((bcm_ismask(c)&(_BCM_D)) != 0)
-#define bcm_isgraph(c)	((bcm_ismask(c)&(_BCM_P|_BCM_U|_BCM_L|_BCM_D)) != 0)
-#define bcm_islower(c)	((bcm_ismask(c)&(_BCM_L)) != 0)
-#define bcm_isprint(c)	((bcm_ismask(c)&(_BCM_P|_BCM_U|_BCM_L|_BCM_D|_BCM_SP)) != 0)
-#define bcm_ispunct(c)	((bcm_ismask(c)&(_BCM_P)) != 0)
-#define bcm_isspace(c)	((bcm_ismask(c)&(_BCM_S)) != 0)
-#define bcm_isupper(c)	((bcm_ismask(c)&(_BCM_U)) != 0)
-#define bcm_isxdigit(c)	((bcm_ismask(c)&(_BCM_D|_BCM_X)) != 0)
-#define bcm_tolower(c)	(bcm_isupper((c)) ? ((c) + 'a' - 'A') : (c))
-#define bcm_toupper(c)	(bcm_islower((c)) ? ((c) + 'A' - 'a') : (c))
-
-
-
-struct bcmstrbuf {
-	char *buf;	
-	unsigned int size;	
-	char *origbuf;	
-	unsigned int origsize;	
-};
-
-
-#ifdef BCMDRIVER
-#include <osl.h>
-
-#define GPIO_PIN_NOTDEFINED 	0x20	
-
-
-#define SPINWAIT(exp, us) { \
-	uint countdown = (us) + 9; \
-	while ((exp) && (countdown >= 10)) {\
-		OSL_DELAY(10); \
-		countdown -= 10; \
-	} \
-}
-
-
-#ifndef PKTQ_LEN_DEFAULT
-#define PKTQ_LEN_DEFAULT        128	
-#endif
-#ifndef PKTQ_MAX_PREC
-#define PKTQ_MAX_PREC           16	
-#endif
-
-typedef struct pktq_prec {
-	void *head;     
-	void *tail;     
-	uint16 len;     
-	uint16 max;     
-} pktq_prec_t;
-
-
-
-struct pktq {
-	uint16 num_prec;        
-	uint16 hi_prec;         
-	uint16 max;             
-	uint16 len;             
-	
-	struct pktq_prec q[PKTQ_MAX_PREC];
-};
-
-
-struct spktq {
-	uint16 num_prec;        
-	uint16 hi_prec;         
-	uint16 max;             
-	uint16 len;             
-	
-	struct pktq_prec q[1];
-};
-
-#define PKTQ_PREC_ITER(pq, prec)        for (prec = (pq)->num_prec - 1; prec >= 0; prec--)
-
-
-typedef bool (*ifpkt_cb_t)(void*, int);
-
-#ifdef BCMPKTPOOL
-#define POOL_ENAB(pool)		((pool) && (pool)->inited)
-#if defined(BCM4329C0)
-#define SHARED_POOL		(pktpool_shared_ptr)
-#else
-#define SHARED_POOL		(pktpool_shared)
-#endif 
-#else 
-#define POOL_ENAB(bus)		0
-#define SHARED_POOL		((struct pktpool *)NULL)
-#endif 
-
-#ifndef PKTPOOL_LEN_MAX
-#define PKTPOOL_LEN_MAX		40
-#endif
-#define PKTPOOL_CB_MAX		3
-
-struct pktpool;
-typedef void (*pktpool_cb_t)(struct pktpool *pool, void *arg);
-typedef struct {
-	pktpool_cb_t cb;
-	void *arg;
-} pktpool_cbinfo_t;
-
-#ifdef BCMDBG_POOL
-
-#define POOL_IDLE	0
-#define POOL_RXFILL	1
-#define POOL_RXDH	2
-#define POOL_RXD11	3
-#define POOL_TXDH	4
-#define POOL_TXD11	5
-#define POOL_AMPDU	6
-#define POOL_TXENQ	7
-
-typedef struct {
-	void *p;
-	uint32 cycles;
-	uint32 dur;
-} pktpool_dbg_t;
-
-typedef struct {
-	uint8 txdh;	
-	uint8 txd11;	
-	uint8 enq;	
-	uint8 rxdh;	
-	uint8 rxd11;	
-	uint8 rxfill;	
-	uint8 idle;	
-} pktpool_stats_t;
-#endif 
-
-typedef struct pktpool {
-	bool inited;
-	uint16 r;
-	uint16 w;
-	uint16 len;
-	uint16 maxlen;
-	uint16 plen;
-	bool istx;
-	bool empty;
-	uint8 cbtoggle;
-	uint8 cbcnt;
-	uint8 ecbcnt;
-	bool emptycb_disable;
-	pktpool_cbinfo_t cbs[PKTPOOL_CB_MAX];
-	pktpool_cbinfo_t ecbs[PKTPOOL_CB_MAX];
-	void *q[PKTPOOL_LEN_MAX + 1];
-
-#ifdef BCMDBG_POOL
-	uint8 dbg_cbcnt;
-	pktpool_cbinfo_t dbg_cbs[PKTPOOL_CB_MAX];
-	uint16 dbg_qlen;
-	pktpool_dbg_t dbg_q[PKTPOOL_LEN_MAX + 1];
-#endif
-} pktpool_t;
-
-#if defined(BCM4329C0)
-extern pktpool_t *pktpool_shared_ptr;
-#else
-extern pktpool_t *pktpool_shared;
-#endif 
-
-extern int pktpool_init(osl_t *osh, pktpool_t *pktp, int *pktplen, int plen, bool istx);
-extern int pktpool_deinit(osl_t *osh, pktpool_t *pktp);
-extern int pktpool_fill(osl_t *osh, pktpool_t *pktp, bool minimal);
-extern void* pktpool_get(pktpool_t *pktp);
-extern void pktpool_free(pktpool_t *pktp, void *p);
-extern int pktpool_add(pktpool_t *pktp, void *p);
-extern uint16 pktpool_avail(pktpool_t *pktp);
-extern int pktpool_avail_register(pktpool_t *pktp, pktpool_cb_t cb, void *arg);
-extern int pktpool_empty_register(pktpool_t *pktp, pktpool_cb_t cb, void *arg);
-extern int pktpool_setmaxlen(pktpool_t *pktp, uint16 maxlen);
-extern void pktpool_emptycb_disable(pktpool_t *pktp, bool disable);
-
-#define POOLPTR(pp)			((pktpool_t *)(pp))
-#define pktpool_len(pp)			(POOLPTR(pp)->len - 1)
-#define pktpool_plen(pp)		(POOLPTR(pp)->plen)
-#define pktpool_maxlen(pp)		(POOLPTR(pp)->maxlen)
-
-#ifdef BCMDBG_POOL
-extern int pktpool_dbg_register(pktpool_t *pktp, pktpool_cb_t cb, void *arg);
-extern int pktpool_start_trigger(pktpool_t *pktp, void *p);
-extern int pktpool_dbg_dump(pktpool_t *pktp);
-extern int pktpool_dbg_notify(pktpool_t *pktp);
-extern int pktpool_stats_dump(pktpool_t *pktp, pktpool_stats_t *stats);
-#endif 
-
-
-
-struct ether_addr;
-
-extern int ether_isbcast(const void *ea);
-extern int ether_isnulladdr(const void *ea);
-
-
-
-#define pktq_psetmax(pq, prec, _max)    ((pq)->q[prec].max = (_max))
-#define pktq_plen(pq, prec)             ((pq)->q[prec].len)
-#define pktq_pavail(pq, prec)           ((pq)->q[prec].max - (pq)->q[prec].len)
-#define pktq_pfull(pq, prec)            ((pq)->q[prec].len >= (pq)->q[prec].max)
-#define pktq_pempty(pq, prec)           ((pq)->q[prec].len == 0)
-
-#define pktq_ppeek(pq, prec)            ((pq)->q[prec].head)
-#define pktq_ppeek_tail(pq, prec)       ((pq)->q[prec].tail)
-
-extern void *pktq_penq(struct pktq *pq, int prec, void *p);
-extern void *pktq_penq_head(struct pktq *pq, int prec, void *p);
-extern void *pktq_pdeq(struct pktq *pq, int prec);
-extern void *pktq_pdeq_tail(struct pktq *pq, int prec);
-
-extern void pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir,
-	ifpkt_cb_t fn, int arg);
-
-extern bool pktq_pdel(struct pktq *pq, void *p, int prec);
-
-
-
-extern int pktq_mlen(struct pktq *pq, uint prec_bmp);
-extern void *pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out);
-
-
-
-#define pktq_len(pq)                    ((int)(pq)->len)
-#define pktq_max(pq)                    ((int)(pq)->max)
-#define pktq_avail(pq)                  ((int)((pq)->max - (pq)->len))
-#define pktq_full(pq)                   ((pq)->len >= (pq)->max)
-#define pktq_empty(pq)                  ((pq)->len == 0)
-
-
-#define pktenq(pq, p)		pktq_penq(((struct pktq *)pq), 0, (p))
-#define pktenq_head(pq, p)	pktq_penq_head(((struct pktq *)pq), 0, (p))
-#define pktdeq(pq)		pktq_pdeq(((struct pktq *)pq), 0)
-#define pktdeq_tail(pq)		pktq_pdeq_tail(((struct pktq *)pq), 0)
-#define pktqinit(pq, len) pktq_init(((struct pktq *)pq), 1, len)
-
-extern void pktq_init(struct pktq *pq, int num_prec, int max_len);
-
-extern void *pktq_deq(struct pktq *pq, int *prec_out);
-extern void *pktq_deq_tail(struct pktq *pq, int *prec_out);
-extern void *pktq_peek(struct pktq *pq, int *prec_out);
-extern void *pktq_peek_tail(struct pktq *pq, int *prec_out);
-extern void pktq_flush(osl_t *osh, struct pktq *pq, bool dir, ifpkt_cb_t fn, int arg);
-
-
-
-extern uint pktcopy(osl_t *osh, void *p, uint offset, int len, uchar *buf);
-extern uint pktfrombuf(osl_t *osh, void *p, uint offset, int len, uchar *buf);
-extern uint pkttotlen(osl_t *osh, void *p);
-extern void *pktlast(osl_t *osh, void *p);
-extern uint pktsegcnt(osl_t *osh, void *p);
-
-
-extern uint pktsetprio(void *pkt, bool update_vtag);
-#define	PKTPRIO_VDSCP	0x100		
-#define	PKTPRIO_VLAN	0x200		
-#define	PKTPRIO_UPD	0x400		
-#define	PKTPRIO_DSCP	0x800		
-
-
-extern int bcm_atoi(char *s);
-extern ulong bcm_strtoul(char *cp, char **endp, uint base);
-extern char *bcmstrstr(char *haystack, char *needle);
-extern char *bcmstrcat(char *dest, const char *src);
-extern char *bcmstrncat(char *dest, const char *src, uint size);
-extern ulong wchar2ascii(char *abuf, ushort *wbuf, ushort wbuflen, ulong abuflen);
-char* bcmstrtok(char **string, const char *delimiters, char *tokdelim);
-int bcmstricmp(const char *s1, const char *s2);
-int bcmstrnicmp(const char* s1, const char* s2, int cnt);
-
-
-
-extern char *bcm_ether_ntoa(const struct ether_addr *ea, char *buf);
-extern int bcm_ether_atoe(char *p, struct ether_addr *ea);
-
-
-struct ipv4_addr;
-extern char *bcm_ip_ntoa(struct ipv4_addr *ia, char *buf);
-
-
-extern void bcm_mdelay(uint ms);
-
-extern char *getvar(char *vars, const char *name);
-extern int getintvar(char *vars, const char *name);
-extern int getintvararray(char *vars, const char *name, int index);
-extern int getintvararraysize(char *vars, const char *name);
-extern uint getgpiopin(char *vars, char *pin_name, uint def_pin);
-#define bcm_perf_enable()
-#define bcmstats(fmt)
-#define	bcmlog(fmt, a1, a2)
-#define	bcmdumplog(buf, size)	*buf = '\0'
-#define	bcmdumplogent(buf, idx)	-1
-
-#define bcmtslog(tstamp, fmt, a1, a2)
-#define bcmprinttslogs()
-#define bcmprinttstamp(us)
-
-extern char *bcm_nvram_vars(uint *length);
-extern int bcm_nvram_cache(void *sih);
-
-
-
-
-typedef struct bcm_iovar {
-	const char *name;	
-	uint16 varid;		
-	uint16 flags;		
-	uint16 type;		
-	uint16 minlen;		
-} bcm_iovar_t;
-
-
-
-
-#define IOV_GET 0 
-#define IOV_SET 1 
-
-
-#define IOV_GVAL(id)		((id)*2)
-#define IOV_SVAL(id)		(((id)*2)+IOV_SET)
-#define IOV_ISSET(actionid)	((actionid & IOV_SET) == IOV_SET)
-#define IOV_ID(actionid)	(actionid >> 1)
-
-
-
-extern const bcm_iovar_t *bcm_iovar_lookup(const bcm_iovar_t *table, const char *name);
-extern int bcm_iovar_lencheck(const bcm_iovar_t *table, void *arg, int len, bool set);
-#if defined(WLTINYDUMP) || defined(WLMSG_INFORM) || defined(WLMSG_ASSOC) || \
-	defined(WLMSG_PRPKT) || defined(WLMSG_WSEC)
-extern int bcm_format_ssid(char* buf, const uchar ssid[], uint ssid_len);
-#endif 
-#endif	
-
-
-#define IOVT_VOID	0	
-#define IOVT_BOOL	1	
-#define IOVT_INT8	2	
-#define IOVT_UINT8	3	
-#define IOVT_INT16	4	
-#define IOVT_UINT16	5	
-#define IOVT_INT32	6	
-#define IOVT_UINT32	7	
-#define IOVT_BUFFER	8	
-#define BCM_IOVT_VALID(type) (((unsigned int)(type)) <= IOVT_BUFFER)
-
-
-#define BCM_IOV_TYPE_INIT { \
-	"void", \
-	"bool", \
-	"int8", \
-	"uint8", \
-	"int16", \
-	"uint16", \
-	"int32", \
-	"uint32", \
-	"buffer", \
-	"" }
-
-#define BCM_IOVT_IS_INT(type) (\
-	(type == IOVT_BOOL) || \
-	(type == IOVT_INT8) || \
-	(type == IOVT_UINT8) || \
-	(type == IOVT_INT16) || \
-	(type == IOVT_UINT16) || \
-	(type == IOVT_INT32) || \
-	(type == IOVT_UINT32))
-
-
-
-#define BCME_STRLEN 		64	
-#define VALID_BCMERROR(e)  ((e <= 0) && (e >= BCME_LAST))
-
-
-
-
-#define BCME_OK				0	
-#define BCME_ERROR			-1	
-#define BCME_BADARG			-2	
-#define BCME_BADOPTION			-3	
-#define BCME_NOTUP			-4	
-#define BCME_NOTDOWN			-5	
-#define BCME_NOTAP			-6	
-#define BCME_NOTSTA			-7	
-#define BCME_BADKEYIDX			-8	
-#define BCME_RADIOOFF 			-9	
-#define BCME_NOTBANDLOCKED		-10	
-#define BCME_NOCLK			-11	
-#define BCME_BADRATESET			-12	
-#define BCME_BADBAND			-13	
-#define BCME_BUFTOOSHORT		-14	
-#define BCME_BUFTOOLONG			-15	
-#define BCME_BUSY			-16	
-#define BCME_NOTASSOCIATED		-17	
-#define BCME_BADSSIDLEN			-18	
-#define BCME_OUTOFRANGECHAN		-19	
-#define BCME_BADCHAN			-20	
-#define BCME_BADADDR			-21	
-#define BCME_NORESOURCE			-22	
-#define BCME_UNSUPPORTED		-23	
-#define BCME_BADLEN			-24	
-#define BCME_NOTREADY			-25	
-#define BCME_EPERM			-26	
-#define BCME_NOMEM			-27	
-#define BCME_ASSOCIATED			-28	
-#define BCME_RANGE			-29	
-#define BCME_NOTFOUND			-30	
-#define BCME_WME_NOT_ENABLED		-31	
-#define BCME_TSPEC_NOTFOUND		-32	
-#define BCME_ACM_NOTSUPPORTED		-33	
-#define BCME_NOT_WME_ASSOCIATION	-34	
-#define BCME_SDIO_ERROR			-35	
-#define BCME_DONGLE_DOWN		-36	
-#define BCME_VERSION			-37 	
-#define BCME_TXFAIL			-38 	
-#define BCME_RXFAIL			-39	
-#define BCME_NODEVICE			-40 	
-#define BCME_NMODE_DISABLED		-41 	
-#define BCME_NONRESIDENT		-42 
-#define BCME_LAST			BCME_NONRESIDENT
-
-
-#define BCMERRSTRINGTABLE {		\
-	"OK",				\
-	"Undefined error",		\
-	"Bad Argument",			\
-	"Bad Option",			\
-	"Not up",			\
-	"Not down",			\
-	"Not AP",			\
-	"Not STA",			\
-	"Bad Key Index",		\
-	"Radio Off",			\
-	"Not band locked",		\
-	"No clock",			\
-	"Bad Rate valueset",		\
-	"Bad Band",			\
-	"Buffer too short",		\
-	"Buffer too long",		\
-	"Busy",				\
-	"Not Associated",		\
-	"Bad SSID len",			\
-	"Out of Range Channel",		\
-	"Bad Channel",			\
-	"Bad Address",			\
-	"Not Enough Resources",		\
-	"Unsupported",			\
-	"Bad length",			\
-	"Not Ready",			\
-	"Not Permitted",		\
-	"No Memory",			\
-	"Associated",			\
-	"Not In Range",			\
-	"Not Found",			\
-	"WME Not Enabled",		\
-	"TSPEC Not Found",		\
-	"ACM Not Supported",		\
-	"Not WME Association",		\
-	"SDIO Bus Error",		\
-	"Dongle Not Accessible",	\
-	"Incorrect version",		\
-	"TX Failure",			\
-	"RX Failure",			\
-	"Device Not Present",		\
-	"NMODE Disabled",		\
-	"Nonresident overlay access", \
-}
-
-#ifndef ABS
-#define	ABS(a)			(((a) < 0)?-(a):(a))
-#endif 
-
-#ifndef MIN
-#define	MIN(a, b)		(((a) < (b))?(a):(b))
-#endif 
-
-#ifndef MAX
-#define	MAX(a, b)		(((a) > (b))?(a):(b))
-#endif 
-
-#define CEIL(x, y)		(((x) + ((y)-1)) / (y))
-#define	ROUNDUP(x, y)		((((x)+((y)-1))/(y))*(y))
-#define	ISALIGNED(a, x)		(((uintptr)(a) & ((x)-1)) == 0)
-#define ALIGN_ADDR(addr, boundary) (void *)(((uintptr)(addr) + (boundary) - 1) \
-	                                         & ~((boundary) - 1))
-#define	ISPOWEROF2(x)		((((x)-1)&(x)) == 0)
-#define VALID_MASK(mask)	!((mask) & ((mask) + 1))
-#ifndef OFFSETOF
-#define	OFFSETOF(type, member)	((uint)(uintptr)&((type *)0)->member)
-#endif 
-#ifndef ARRAYSIZE
-#define ARRAYSIZE(a)		(sizeof(a)/sizeof(a[0]))
-#endif
-
-
-extern void *_bcmutils_dummy_fn;
-#define REFERENCE_FUNCTION(f)	(_bcmutils_dummy_fn = (void *)(f))
-
-
-#ifndef setbit
-#ifndef NBBY		      
-#define	NBBY	8	
-#endif 
-#define	setbit(a, i)	(((uint8 *)a)[(i)/NBBY] |= 1<<((i)%NBBY))
-#define	clrbit(a, i)	(((uint8 *)a)[(i)/NBBY] &= ~(1<<((i)%NBBY)))
-#define	isset(a, i)	(((const uint8 *)a)[(i)/NBBY] & (1<<((i)%NBBY)))
-#define	isclr(a, i)	((((const uint8 *)a)[(i)/NBBY] & (1<<((i)%NBBY))) == 0)
-#endif 
-
-#define	NBITS(type)	(sizeof(type) * 8)
-#define NBITVAL(nbits)	(1 << (nbits))
-#define MAXBITVAL(nbits)	((1 << (nbits)) - 1)
-#define	NBITMASK(nbits)	MAXBITVAL(nbits)
-#define MAXNBVAL(nbyte)	MAXBITVAL((nbyte) * 8)
-
-
-#define MUX(pred, true, false) ((pred) ? (true) : (false))
-
-
-#define MODDEC(x, bound) MUX((x) == 0, (bound) - 1, (x) - 1)
-#define MODINC(x, bound) MUX((x) == (bound) - 1, 0, (x) + 1)
-
-
-#define MODDEC_POW2(x, bound) (((x) - 1) & ((bound) - 1))
-#define MODINC_POW2(x, bound) (((x) + 1) & ((bound) - 1))
-
-
-#define MODADD(x, y, bound) \
-    MUX((x) + (y) >= (bound), (x) + (y) - (bound), (x) + (y))
-#define MODSUB(x, y, bound) \
-    MUX(((int)(x)) - ((int)(y)) < 0, (x) - (y) + (bound), (x) - (y))
-
-
-#define MODADD_POW2(x, y, bound) (((x) + (y)) & ((bound) - 1))
-#define MODSUB_POW2(x, y, bound) (((x) - (y)) & ((bound) - 1))
-
-
-#define CRC8_INIT_VALUE  0xff		
-#define CRC8_GOOD_VALUE  0x9f		
-#define CRC16_INIT_VALUE 0xffff		
-#define CRC16_GOOD_VALUE 0xf0b8		
-#define CRC32_INIT_VALUE 0xffffffff	
-#define CRC32_GOOD_VALUE 0xdebb20e3	
-
-
-typedef struct bcm_bit_desc {
-	uint32	bit;
-	const char* name;
-} bcm_bit_desc_t;
-
-
-typedef struct bcm_tlv {
-	uint8	id;
-	uint8	len;
-	uint8	data[1];
-} bcm_tlv_t;
-
-
-#define bcm_valid_tlv(elt, buflen) ((buflen) >= 2 && (int)(buflen) >= (int)(2 + (elt)->len))
-
-
-#define ETHER_ADDR_STR_LEN	18	
-
-
-
-static INLINE void
-xor_128bit_block(const uint8 *src1, const uint8 *src2, uint8 *dst)
-{
-	if (
-#ifdef __i386__
-	    1 ||
-#endif
-	    (((uintptr)src1 | (uintptr)src2 | (uintptr)dst) & 3) == 0) {
-		
-		
-		((uint32 *)dst)[0] = ((const uint32 *)src1)[0] ^ ((const uint32 *)src2)[0];
-		((uint32 *)dst)[1] = ((const uint32 *)src1)[1] ^ ((const uint32 *)src2)[1];
-		((uint32 *)dst)[2] = ((const uint32 *)src1)[2] ^ ((const uint32 *)src2)[2];
-		((uint32 *)dst)[3] = ((const uint32 *)src1)[3] ^ ((const uint32 *)src2)[3];
-	} else {
-		
-		int k;
-		for (k = 0; k < 16; k++)
-			dst[k] = src1[k] ^ src2[k];
-	}
-}
-
-
-
-extern uint8 hndcrc8(uint8 *p, uint nbytes, uint8 crc);
-extern uint16 hndcrc16(uint8 *p, uint nbytes, uint16 crc);
-extern uint32 hndcrc32(uint8 *p, uint nbytes, uint32 crc);
-
-#if defined(DHD_DEBUG) || defined(WLMSG_PRHDRS) || defined(WLMSG_PRPKT) || \
-	defined(WLMSG_ASSOC)
-extern int bcm_format_flags(const bcm_bit_desc_t *bd, uint32 flags, char* buf, int len);
-#endif
-
-#if defined(DHD_DEBUG) || defined(WLMSG_PRHDRS) || defined(WLMSG_PRPKT) || \
-	defined(WLMSG_ASSOC) || defined(WLMEDIA_PEAKRATE)
-extern int bcm_format_hex(char *str, const void *bytes, int len);
-#endif
-
-extern const char *bcm_crypto_algo_name(uint algo);
-extern char *bcm_chipname(uint chipid, char *buf, uint len);
-extern char *bcm_brev_str(uint32 brev, char *buf);
-extern void printbig(char *buf);
-extern void prhex(const char *msg, uchar *buf, uint len);
-
-
-extern bcm_tlv_t *bcm_next_tlv(bcm_tlv_t *elt, int *buflen);
-extern bcm_tlv_t *bcm_parse_tlvs(void *buf, int buflen, uint key);
-extern bcm_tlv_t *bcm_parse_ordered_tlvs(void *buf, int buflen, uint key);
-
-
-extern const char *bcmerrorstr(int bcmerror);
-
-
-typedef uint32 mbool;
-#define mboolset(mb, bit)		((mb) |= (bit))		
-#define mboolclr(mb, bit)		((mb) &= ~(bit))	
-#define mboolisset(mb, bit)		(((mb) & (bit)) != 0)	
-#define	mboolmaskset(mb, mask, val)	((mb) = (((mb) & ~(mask)) | (val)))
-
-
-extern uint16 bcm_qdbm_to_mw(uint8 qdbm);
-extern uint8 bcm_mw_to_qdbm(uint16 mw);
-
-
-struct fielddesc {
-	const char *nameandfmt;
-	uint32 	offset;
-	uint32 	len;
-};
-
-extern void bcm_binit(struct bcmstrbuf *b, char *buf, uint size);
-extern int bcm_bprintf(struct bcmstrbuf *b, const char *fmt, ...);
-extern void bcm_inc_bytes(uchar *num, int num_bytes, uint8 amount);
-extern int bcm_cmp_bytes(uchar *arg1, uchar *arg2, uint8 nbytes);
-extern void bcm_print_bytes(char *name, const uchar *cdata, int len);
-
-typedef  uint32 (*bcmutl_rdreg_rtn)(void *arg0, uint arg1, uint32 offset);
-extern uint bcmdumpfields(bcmutl_rdreg_rtn func_ptr, void *arg0, uint arg1, struct fielddesc *str,
-                          char *buf, uint32 bufsize);
-
-extern uint bcm_mkiovar(char *name, char *data, uint datalen, char *buf, uint len);
-extern uint bcm_bitcount(uint8 *bitmap, uint bytelength);
-
-
-
-#define SSID_FMT_BUF_LEN	((4 * DOT11_MAX_SSID_LEN) + 1)
-
-unsigned int process_nvram_vars(char *varbuf, unsigned int len);
-
-#ifdef __cplusplus
-	}
-#endif
-
-#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/epivers.h b/drivers/net/wireless/bcmdhd/include/epivers.h
deleted file mode 100644
index f474dfa482a..00000000000
--- a/drivers/net/wireless/bcmdhd/include/epivers.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: epivers.h.in,v 13.32.4.1 2010-09-17 00:39:18 Exp $
- *
-*/
-
-
-#ifndef _epivers_h_
-#define _epivers_h_
-
-#define	EPI_MAJOR_VERSION	5
-
-#define	EPI_MINOR_VERSION	90
-
-#define	EPI_RC_NUMBER		125
-
-#define	EPI_INCREMENTAL_NUMBER	69
-
-#define	EPI_BUILD_NUMBER	0
-
-#define	EPI_VERSION		5, 90, 125, 69
-
-#define	EPI_VERSION_NUM		0x055a7d45
-
-#define EPI_VERSION_DEV		5.90.125
-
-
-#define	EPI_VERSION_STR		"5.90.125.69"
-
-#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/linuxver.h b/drivers/net/wireless/bcmdhd/include/linuxver.h
deleted file mode 100644
index e1c62b73f15..00000000000
--- a/drivers/net/wireless/bcmdhd/include/linuxver.h
+++ /dev/null
@@ -1,591 +0,0 @@
-/*
- * Linux-specific abstractions to gain some independence from linux kernel versions.
- * Pave over some 2.2 versus 2.4 versus 2.6 kernel differences.
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: linuxver.h,v 13.53.2.2 2010-12-22 23:47:26 Exp $
- */
-
-
-#ifndef _linuxver_h_
-#define _linuxver_h_
-
-#include <linux/version.h>
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
-#include <linux/config.h>
-#else
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 33))
-#include <generated/autoconf.h>
-#else
-#include <linux/autoconf.h>
-#endif
-#endif 
-#include <linux/module.h>
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 0))
-
-#ifdef __UNDEF_NO_VERSION__
-#undef __NO_VERSION__
-#else
-#define __NO_VERSION__
-#endif
-#endif	
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 5, 0)
-#define module_param(_name_, _type_, _perm_)	MODULE_PARM(_name_, "i")
-#define module_param_string(_name_, _string_, _size_, _perm_) \
-		MODULE_PARM(_string_, "c" __MODULE_STRING(_size_))
-#endif
-
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 9))
-#include <linux/malloc.h>
-#else
-#include <linux/slab.h>
-#endif
-
-#include <linux/types.h>
-#include <linux/init.h>
-#include <linux/mm.h>
-#include <linux/string.h>
-#include <linux/pci.h>
-#include <linux/interrupt.h>
-#include <linux/netdevice.h>
-#include <linux/semaphore.h>
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 28))
-#undef IP_TOS
-#endif 
-#include <asm/io.h>
-
-#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 5, 41))
-#include <linux/workqueue.h>
-#else
-#include <linux/tqueue.h>
-#ifndef work_struct
-#define work_struct tq_struct
-#endif
-#ifndef INIT_WORK
-#define INIT_WORK(_work, _func, _data) INIT_TQUEUE((_work), (_func), (_data))
-#endif
-#ifndef schedule_work
-#define schedule_work(_work) schedule_task((_work))
-#endif
-#ifndef flush_scheduled_work
-#define flush_scheduled_work() flush_scheduled_tasks()
-#endif
-#endif	
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
-#define	MY_INIT_WORK(_work, _func)	INIT_WORK(_work, _func)
-#else
-#define	MY_INIT_WORK(_work, _func)	INIT_WORK(_work, _func, _work)
-typedef void (*work_func_t)(void *work);
-#endif	
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
-
-#ifndef IRQ_NONE
-typedef void irqreturn_t;
-#define IRQ_NONE
-#define IRQ_HANDLED
-#define IRQ_RETVAL(x)
-#endif
-#else
-typedef irqreturn_t(*FN_ISR) (int irq, void *dev_id, struct pt_regs *ptregs);
-#endif	
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18)
-#define IRQF_SHARED	SA_SHIRQ
-#endif 
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 17)
-#ifdef	CONFIG_NET_RADIO
-#define	CONFIG_WIRELESS_EXT
-#endif
-#endif	
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 5, 67)
-#define MOD_INC_USE_COUNT
-#define MOD_DEC_USE_COUNT
-#endif 
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 32)
-#include <linux/sched.h>
-#endif
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)
-#include <net/lib80211.h>
-#endif
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)
-#include <linux/ieee80211.h>
-#else
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 14)
-#include <net/ieee80211.h>
-#endif
-#endif 
-
-
-#ifndef __exit
-#define __exit
-#endif
-#ifndef __devexit
-#define __devexit
-#endif
-#ifndef __devinit
-#define __devinit	__init
-#endif
-#ifndef __devinitdata
-#define __devinitdata
-#endif
-#ifndef __devexit_p
-#define __devexit_p(x)	x
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 0))
-
-#define pci_get_drvdata(dev)		(dev)->sysdata
-#define pci_set_drvdata(dev, value)	(dev)->sysdata = (value)
-
-
-
-struct pci_device_id {
-	unsigned int vendor, device;		
-	unsigned int subvendor, subdevice;	
-	unsigned int class, class_mask;		
-	unsigned long driver_data;		
-};
-
-struct pci_driver {
-	struct list_head node;
-	char *name;
-	const struct pci_device_id *id_table;	
-	int (*probe)(struct pci_dev *dev,
-	             const struct pci_device_id *id); 
-	void (*remove)(struct pci_dev *dev);	
-	void (*suspend)(struct pci_dev *dev);	
-	void (*resume)(struct pci_dev *dev);	
-};
-
-#define MODULE_DEVICE_TABLE(type, name)
-#define PCI_ANY_ID (~0)
-
-
-#define pci_module_init pci_register_driver
-extern int pci_register_driver(struct pci_driver *drv);
-extern void pci_unregister_driver(struct pci_driver *drv);
-
-#endif 
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 18))
-#define pci_module_init pci_register_driver
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 2, 18))
-#ifdef MODULE
-#define module_init(x) int init_module(void) { return x(); }
-#define module_exit(x) void cleanup_module(void) { x(); }
-#else
-#define module_init(x)	__initcall(x);
-#define module_exit(x)	__exitcall(x);
-#endif
-#endif	
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 31)
-#define WL_USE_NETDEV_OPS
-#else
-#undef WL_USE_NETDEV_OPS
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31)) && defined(CONFIG_RFKILL_INPUT)
-#define WL_CONFIG_RFKILL_INPUT
-#else
-#undef WL_CONFIG_RFKILL_INPUT
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 48))
-#define list_for_each(pos, head) \
-	for (pos = (head)->next; pos != (head); pos = pos->next)
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 13))
-#define pci_resource_start(dev, bar)	((dev)->base_address[(bar)])
-#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 44))
-#define pci_resource_start(dev, bar)	((dev)->resource[(bar)].start)
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 23))
-#define pci_enable_device(dev) do { } while (0)
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 14))
-#define net_device device
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 42))
-
-
-
-#ifndef PCI_DMA_TODEVICE
-#define	PCI_DMA_TODEVICE	1
-#define	PCI_DMA_FROMDEVICE	2
-#endif
-
-typedef u32 dma_addr_t;
-
-
-static inline int get_order(unsigned long size)
-{
-	int order;
-
-	size = (size-1) >> (PAGE_SHIFT-1);
-	order = -1;
-	do {
-		size >>= 1;
-		order++;
-	} while (size);
-	return order;
-}
-
-static inline void *pci_alloc_consistent(struct pci_dev *hwdev, size_t size,
-                                         dma_addr_t *dma_handle)
-{
-	void *ret;
-	int gfp = GFP_ATOMIC | GFP_DMA;
-
-	ret = (void *)__get_free_pages(gfp, get_order(size));
-
-	if (ret != NULL) {
-		memset(ret, 0, size);
-		*dma_handle = virt_to_bus(ret);
-	}
-	return ret;
-}
-static inline void pci_free_consistent(struct pci_dev *hwdev, size_t size,
-                                       void *vaddr, dma_addr_t dma_handle)
-{
-	free_pages((unsigned long)vaddr, get_order(size));
-}
-#define pci_map_single(cookie, address, size, dir)	virt_to_bus(address)
-#define pci_unmap_single(cookie, address, size, dir)
-
-#endif 
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 43))
-
-#define dev_kfree_skb_any(a)		dev_kfree_skb(a)
-#define netif_down(dev)			do { (dev)->start = 0; } while (0)
-
-
-#ifndef _COMPAT_NETDEVICE_H
-
-
-
-#define dev_kfree_skb_irq(a)	dev_kfree_skb(a)
-#define netif_wake_queue(dev) \
-		do { clear_bit(0, &(dev)->tbusy); mark_bh(NET_BH); } while (0)
-#define netif_stop_queue(dev)	set_bit(0, &(dev)->tbusy)
-
-static inline void netif_start_queue(struct net_device *dev)
-{
-	dev->tbusy = 0;
-	dev->interrupt = 0;
-	dev->start = 1;
-}
-
-#define netif_queue_stopped(dev)	(dev)->tbusy
-#define netif_running(dev)		(dev)->start
-
-#endif 
-
-#define netif_device_attach(dev)	netif_start_queue(dev)
-#define netif_device_detach(dev)	netif_stop_queue(dev)
-
-
-#define tasklet_struct				tq_struct
-static inline void tasklet_schedule(struct tasklet_struct *tasklet)
-{
-	queue_task(tasklet, &tq_immediate);
-	mark_bh(IMMEDIATE_BH);
-}
-
-static inline void tasklet_init(struct tasklet_struct *tasklet,
-                                void (*func)(unsigned long),
-                                unsigned long data)
-{
-	tasklet->next = NULL;
-	tasklet->sync = 0;
-	tasklet->routine = (void (*)(void *))func;
-	tasklet->data = (void *)data;
-}
-#define tasklet_kill(tasklet)	{ do {} while (0); }
-
-
-#define del_timer_sync(timer) del_timer(timer)
-
-#else
-
-#define netif_down(dev)
-
-#endif 
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 3))
-
-
-#define PREPARE_TQUEUE(_tq, _routine, _data)			\
-	do {							\
-		(_tq)->routine = _routine;			\
-		(_tq)->data = _data;				\
-	} while (0)
-
-
-#define INIT_TQUEUE(_tq, _routine, _data)			\
-	do {							\
-		INIT_LIST_HEAD(&(_tq)->list);			\
-		(_tq)->sync = 0;				\
-		PREPARE_TQUEUE((_tq), (_routine), (_data));	\
-	} while (0)
-
-#endif	
-
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 9)
-#define	PCI_SAVE_STATE(a, b)	pci_save_state(a)
-#define	PCI_RESTORE_STATE(a, b)	pci_restore_state(a)
-#else
-#define	PCI_SAVE_STATE(a, b)	pci_save_state(a, b)
-#define	PCI_RESTORE_STATE(a, b)	pci_restore_state(a, b)
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 6))
-static inline int
-pci_save_state(struct pci_dev *dev, u32 *buffer)
-{
-	int i;
-	if (buffer) {
-		for (i = 0; i < 16; i++)
-			pci_read_config_dword(dev, i * 4, &buffer[i]);
-	}
-	return 0;
-}
-
-static inline int
-pci_restore_state(struct pci_dev *dev, u32 *buffer)
-{
-	int i;
-
-	if (buffer) {
-		for (i = 0; i < 16; i++)
-			pci_write_config_dword(dev, i * 4, buffer[i]);
-	}
-	
-	else {
-		for (i = 0; i < 6; i ++)
-			pci_write_config_dword(dev,
-			                       PCI_BASE_ADDRESS_0 + (i * 4),
-			                       pci_resource_start(dev, i));
-		pci_write_config_byte(dev, PCI_INTERRUPT_LINE, dev->irq);
-	}
-	return 0;
-}
-#endif 
-
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 19))
-#define read_c0_count() read_32bit_cp0_register(CP0_COUNT)
-#endif
-
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 24))
-#ifndef SET_MODULE_OWNER
-#define SET_MODULE_OWNER(dev)		do {} while (0)
-#define OLD_MOD_INC_USE_COUNT		MOD_INC_USE_COUNT
-#define OLD_MOD_DEC_USE_COUNT		MOD_DEC_USE_COUNT
-#else
-#define OLD_MOD_INC_USE_COUNT		do {} while (0)
-#define OLD_MOD_DEC_USE_COUNT		do {} while (0)
-#endif
-#else 
-#ifndef SET_MODULE_OWNER
-#define SET_MODULE_OWNER(dev)		do {} while (0)
-#endif
-#ifndef MOD_INC_USE_COUNT
-#define MOD_INC_USE_COUNT			do {} while (0)
-#endif
-#ifndef MOD_DEC_USE_COUNT
-#define MOD_DEC_USE_COUNT			do {} while (0)
-#endif
-#define OLD_MOD_INC_USE_COUNT		MOD_INC_USE_COUNT
-#define OLD_MOD_DEC_USE_COUNT		MOD_DEC_USE_COUNT
-#endif 
-
-#ifndef SET_NETDEV_DEV
-#define SET_NETDEV_DEV(net, pdev)	do {} while (0)
-#endif
-
-#ifndef HAVE_FREE_NETDEV
-#define free_netdev(dev)		kfree(dev)
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
-
-#define af_packet_priv			data
-#endif
-
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 11)
-#define DRV_SUSPEND_STATE_TYPE pm_message_t
-#else
-#define DRV_SUSPEND_STATE_TYPE uint32
-#endif
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
-#define CHECKSUM_HW	CHECKSUM_PARTIAL
-#endif
-
-typedef struct {
-	void 	*parent;  
-	struct	task_struct *p_task;
-	long 	thr_pid;
-	int 	prio; 
-	struct	semaphore sema;
-	bool	terminated;
-	struct	completion completed;
-} tsk_ctl_t;
-
-
-
-
-#ifdef DHD_DEBUG
-#define DBG_THR(x) printk x
-#else
-#define DBG_THR(x)
-#endif
-
-#define SMP_RD_BARRIER_DEPENDS(x) smp_rmb(x)
-
-
-#define PROC_START(thread_func, owner, tsk_ctl, flags) \
-{ \
-	sema_init(&((tsk_ctl)->sema), 0); \
-	init_completion(&((tsk_ctl)->completed)); \
-	(tsk_ctl)->parent = owner; \
-	(tsk_ctl)->terminated = FALSE; \
-	(tsk_ctl)->thr_pid = kernel_thread(thread_func, tsk_ctl, flags); \
-	if ((tsk_ctl)->thr_pid > 0) \
-		wait_for_completion(&((tsk_ctl)->completed)); \
-	DBG_THR(("%s thr:%lx started\n", __FUNCTION__, (tsk_ctl)->thr_pid)); \
-}
-
-#define PROC_STOP(tsk_ctl) \
-{ \
-	(tsk_ctl)->terminated = TRUE; \
-	smp_wmb(); \
-	up(&((tsk_ctl)->sema));	\
-	wait_for_completion(&((tsk_ctl)->completed)); \
-	DBG_THR(("%s thr:%lx terminated OK\n", __FUNCTION__, (tsk_ctl)->thr_pid)); \
-	(tsk_ctl)->thr_pid = -1; \
-}
-
-
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 31))
-#define KILL_PROC(nr, sig) \
-{ \
-struct task_struct *tsk; \
-struct pid *pid;    \
-pid = find_get_pid((pid_t)nr);    \
-tsk = pid_task(pid, PIDTYPE_PID);    \
-if (tsk) send_sig(sig, tsk, 1); \
-}
-#else
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && (LINUX_VERSION_CODE <= \
-	KERNEL_VERSION(2, 6, 30))
-#define KILL_PROC(pid, sig) \
-{ \
-	struct task_struct *tsk; \
-	tsk = find_task_by_vpid(pid); \
-	if (tsk) send_sig(sig, tsk, 1); \
-}
-#else
-#define KILL_PROC(pid, sig) \
-{ \
-	kill_proc(pid, sig, 1); \
-}
-#endif
-#endif 
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
-#include <linux/time.h>
-#include <linux/wait.h>
-#else
-#include <linux/sched.h>
-
-#define __wait_event_interruptible_timeout(wq, condition, ret)		\
-do {									\
-	wait_queue_t __wait;						\
-	init_waitqueue_entry(&__wait, current);				\
-									\
-	add_wait_queue(&wq, &__wait);					\
-	for (;;) {							\
-		set_current_state(TASK_INTERRUPTIBLE);			\
-		if (condition)						\
-			break;						\
-		if (!signal_pending(current)) {				\
-			ret = schedule_timeout(ret);			\
-			if (!ret)					\
-				break;					\
-			continue;					\
-		}							\
-		ret = -ERESTARTSYS;					\
-		break;							\
-	}								\
-	current->state = TASK_RUNNING;					\
-	remove_wait_queue(&wq, &__wait);				\
-} while (0)
-
-#define wait_event_interruptible_timeout(wq, condition, timeout)	\
-({									\
-	long __ret = timeout;						\
-	if (!(condition))						\
-		__wait_event_interruptible_timeout(wq, condition, __ret); \
-	__ret;								\
-})
-
-#endif 
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)
-#define WL_DEV_IF(dev)          ((wl_if_t*)netdev_priv(dev))
-#else
-#define WL_DEV_IF(dev)          ((wl_if_t*)(dev)->priv)
-#endif
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 20)
-#define WL_ISR(i, d, p)         wl_isr((i), (d))
-#else
-#define WL_ISR(i, d, p)         wl_isr((i), (d), (p))
-#endif  
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
-#define netdev_priv(dev) dev->priv
-#endif 
-
-#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/proto/bcmevent.h b/drivers/net/wireless/bcmdhd/include/proto/bcmevent.h
deleted file mode 100644
index 30ec848c40a..00000000000
--- a/drivers/net/wireless/bcmdhd/include/proto/bcmevent.h
+++ /dev/null
@@ -1,312 +0,0 @@
-/*
- * Broadcom Event  protocol definitions
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * Dependencies: proto/bcmeth.h
- *
- * $Id: bcmevent.h,v 9.64.2.9 2011-02-01 06:24:21 Exp $
- *
- */
-
-
-
-
-#ifndef _BCMEVENT_H_
-#define _BCMEVENT_H_
-
-#ifndef _TYPEDEFS_H_
-#include <typedefs.h>
-#endif
-
-
-#include <packed_section_start.h>
-
-#define BCM_EVENT_MSG_VERSION		2	
-#define BCM_MSG_IFNAME_MAX		16	
-
-
-#define WLC_EVENT_MSG_LINK		0x01	
-#define WLC_EVENT_MSG_FLUSHTXQ		0x02	
-#define WLC_EVENT_MSG_GROUP		0x04	
-#define WLC_EVENT_MSG_UNKBSS		0x08	
-#define WLC_EVENT_MSG_UNKIF		0x10	
-
-
-
-
-typedef BWL_PRE_PACKED_STRUCT struct
-{
-	uint16	version;
-	uint16	flags;			
-	uint32	event_type;		
-	uint32	status;			
-	uint32	reason;			
-	uint32	auth_type;		
-	uint32	datalen;		
-	struct ether_addr	addr;	
-	char	ifname[BCM_MSG_IFNAME_MAX]; 
-} BWL_POST_PACKED_STRUCT wl_event_msg_v1_t;
-
-
-typedef BWL_PRE_PACKED_STRUCT struct
-{
-	uint16	version;
-	uint16	flags;			
-	uint32	event_type;		
-	uint32	status;			
-	uint32	reason;			
-	uint32	auth_type;		
-	uint32	datalen;		
-	struct ether_addr	addr;	
-	char	ifname[BCM_MSG_IFNAME_MAX]; 
-	uint8	ifidx;			
-	uint8	bsscfgidx;		
-} BWL_POST_PACKED_STRUCT wl_event_msg_t;
-
-
-typedef BWL_PRE_PACKED_STRUCT struct bcm_event {
-	struct ether_header eth;
-	bcmeth_hdr_t		bcm_hdr;
-	wl_event_msg_t		event;
-	
-} BWL_POST_PACKED_STRUCT bcm_event_t;
-
-#define BCM_MSG_LEN	(sizeof(bcm_event_t) - sizeof(bcmeth_hdr_t) - sizeof(struct ether_header))
-
-
-#define WLC_E_SET_SSID		0	
-#define WLC_E_JOIN		1	
-#define WLC_E_START		2	
-#define WLC_E_AUTH		3	
-#define WLC_E_AUTH_IND		4	
-#define WLC_E_DEAUTH		5	
-#define WLC_E_DEAUTH_IND	6	
-#define WLC_E_ASSOC		7	
-#define WLC_E_ASSOC_IND		8	
-#define WLC_E_REASSOC		9	
-#define WLC_E_REASSOC_IND	10	
-#define WLC_E_DISASSOC		11	
-#define WLC_E_DISASSOC_IND	12	
-#define WLC_E_QUIET_START	13	
-#define WLC_E_QUIET_END		14	
-#define WLC_E_BEACON_RX		15	
-#define WLC_E_LINK		16	
-#define WLC_E_MIC_ERROR		17	
-#define WLC_E_NDIS_LINK		18	
-#define WLC_E_ROAM		19	
-#define WLC_E_TXFAIL		20	
-#define WLC_E_PMKID_CACHE	21	
-#define WLC_E_RETROGRADE_TSF	22	
-#define WLC_E_PRUNE		23	
-#define WLC_E_AUTOAUTH		24	
-#define WLC_E_EAPOL_MSG		25	
-#define WLC_E_SCAN_COMPLETE	26	
-#define WLC_E_ADDTS_IND		27	
-#define WLC_E_DELTS_IND		28	
-#define WLC_E_BCNSENT_IND	29	
-#define WLC_E_BCNRX_MSG		30	
-#define WLC_E_BCNLOST_MSG	31	
-#define WLC_E_ROAM_PREP		32	
-#define WLC_E_PFN_NET_FOUND	33	
-#define WLC_E_PFN_NET_LOST	34	
-#define WLC_E_RESET_COMPLETE	35
-#define WLC_E_JOIN_START	36
-#define WLC_E_ROAM_START	37
-#define WLC_E_ASSOC_START	38
-#define WLC_E_IBSS_ASSOC	39
-#define WLC_E_RADIO		40
-#define WLC_E_PSM_WATCHDOG	41	
-#define WLC_E_PROBREQ_MSG       44      
-#define WLC_E_SCAN_CONFIRM_IND  45
-#define WLC_E_PSK_SUP		46	
-#define WLC_E_COUNTRY_CODE_CHANGED 47
-#define	WLC_E_EXCEEDED_MEDIUM_TIME 48	
-#define WLC_E_ICV_ERROR		49	
-#define WLC_E_UNICAST_DECODE_ERROR 50	
-#define WLC_E_MULTICAST_DECODE_ERROR 51 
-#define WLC_E_TRACE		52
-#define WLC_E_BTA_HCI_EVENT	53	
-#define WLC_E_IF		54	
-#ifdef WLP2P
-#define WLC_E_P2P_DISC_LISTEN_COMPLETE 	55	
-#endif
-#define WLC_E_RSSI		56	
-#define WLC_E_PFN_SCAN_COMPLETE	57	
-#define WLC_E_EXTLOG_MSG	58
-#define WLC_E_ACTION_FRAME      59 	
-#define WLC_E_ACTION_FRAME_COMPLETE 60	
-#define WLC_E_PRE_ASSOC_IND	61	
-#define WLC_E_PRE_REASSOC_IND	62	
-#define WLC_E_CHANNEL_ADOPTED	63
-#define WLC_E_AP_STARTED	64	
-#define WLC_E_DFS_AP_STOP	65	
-#define WLC_E_DFS_AP_RESUME	66	
-#define WLC_E_WAI_STA_EVENT	67	
-#define WLC_E_WAI_MSG 		68	
-#define WLC_E_ESCAN_RESULT 	69	
-#define WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE 	70	
-#if defined(WLP2P)
-#define WLC_E_PROBRESP_MSG	71	
-#define WLC_E_P2P_PROBREQ_MSG	72	
-#endif
-#define WLC_E_DCS_REQUEST 73
-
-#define WLC_E_FIFO_CREDIT_MAP	74 
-
-#define WLC_E_ACTION_FRAME_RX	75	
-#define WLC_E_WAKE_EVENT	76	
-#define WLC_E_RM_COMPLETE	77	
-#define WLC_E_HTSFSYNC		78	
-#define WLC_E_OVERLAY_REQ	79	
-#define WLC_E_CSA_COMPLETE_IND  80
-#define WLC_E_EXCESS_PM_WAKE_EVENT	81	
-#define WLC_E_PFN_SCAN_NONE		82	
-#define WLC_E_PFN_SCAN_ALLGONE	83	
-#define WLC_E_GTK_PLUMBED 84
-#define WLC_E_LAST	85
-
-
-typedef struct {
-	uint event;
-	const char *name;
-} bcmevent_name_t;
-
-extern const bcmevent_name_t	bcmevent_names[];
-extern const int		bcmevent_names_size;
-
-
-#define WLC_E_STATUS_SUCCESS		0	
-#define WLC_E_STATUS_FAIL		1	
-#define WLC_E_STATUS_TIMEOUT		2	
-#define WLC_E_STATUS_NO_NETWORKS	3	
-#define WLC_E_STATUS_ABORT		4	
-#define WLC_E_STATUS_NO_ACK		5	
-#define WLC_E_STATUS_UNSOLICITED	6	
-#define WLC_E_STATUS_ATTEMPT		7	
-#define WLC_E_STATUS_PARTIAL		8	
-#define WLC_E_STATUS_NEWSCAN		9	
-#define WLC_E_STATUS_NEWASSOC		10	
-#define WLC_E_STATUS_11HQUIET		11	
-#define WLC_E_STATUS_SUPPRESS		12	
-#define WLC_E_STATUS_NOCHANS		13	
-#define WLC_E_STATUS_CS_ABORT		15	
-#define WLC_E_STATUS_ERROR		16	
-
-
-#define WLC_E_REASON_INITIAL_ASSOC	0	
-#define WLC_E_REASON_LOW_RSSI		1	
-#define WLC_E_REASON_DEAUTH		2	
-#define WLC_E_REASON_DISASSOC		3	
-#define WLC_E_REASON_BCNS_LOST		4	
-#define WLC_E_REASON_MINTXRATE		9	
-#define WLC_E_REASON_TXFAIL		10	
-
-
-#define WLC_E_REASON_FAST_ROAM_FAILED	5	
-#define WLC_E_REASON_DIRECTED_ROAM	6	
-#define WLC_E_REASON_TSPEC_REJECTED	7	
-#define WLC_E_REASON_BETTER_AP		8	
-
-
-#define WLC_E_PRUNE_ENCR_MISMATCH	1	
-#define WLC_E_PRUNE_BCAST_BSSID		2	
-#define WLC_E_PRUNE_MAC_DENY		3	
-#define WLC_E_PRUNE_MAC_NA		4	
-#define WLC_E_PRUNE_REG_PASSV		5	
-#define WLC_E_PRUNE_SPCT_MGMT		6	
-#define WLC_E_PRUNE_RADAR		7	
-#define WLC_E_RSN_MISMATCH		8	
-#define WLC_E_PRUNE_NO_COMMON_RATES	9	
-#define WLC_E_PRUNE_BASIC_RATES		10	
-#define WLC_E_PRUNE_CIPHER_NA		12	
-#define WLC_E_PRUNE_KNOWN_STA		13	
-#define WLC_E_PRUNE_WDS_PEER		15	
-#define WLC_E_PRUNE_QBSS_LOAD		16	
-#define WLC_E_PRUNE_HOME_AP		17	
-
-
-#define WLC_E_SUP_OTHER			0	
-#define WLC_E_SUP_DECRYPT_KEY_DATA	1	
-#define WLC_E_SUP_BAD_UCAST_WEP128	2	
-#define WLC_E_SUP_BAD_UCAST_WEP40	3	
-#define WLC_E_SUP_UNSUP_KEY_LEN		4	
-#define WLC_E_SUP_PW_KEY_CIPHER		5	
-#define WLC_E_SUP_MSG3_TOO_MANY_IE	6	
-#define WLC_E_SUP_MSG3_IE_MISMATCH	7	
-#define WLC_E_SUP_NO_INSTALL_FLAG	8	
-#define WLC_E_SUP_MSG3_NO_GTK		9	
-#define WLC_E_SUP_GRP_KEY_CIPHER	10	
-#define WLC_E_SUP_GRP_MSG1_NO_GTK	11	
-#define WLC_E_SUP_GTK_DECRYPT_FAIL	12	
-#define WLC_E_SUP_SEND_FAIL		13	
-#define WLC_E_SUP_DEAUTH		14	
-#define WLC_E_SUP_WPA_PSK_TMO		15	
-
-
-
-typedef BWL_PRE_PACKED_STRUCT struct wl_event_rx_frame_data {
-	uint16	version;
-	uint16	channel;	
-	int32	rssi;
-	uint32	mactime;
-	uint32	rate;
-} BWL_POST_PACKED_STRUCT wl_event_rx_frame_data_t;
-
-#define BCM_RX_FRAME_DATA_VERSION 1
-
-
-typedef struct wl_event_data_if {
-	uint8 ifidx;
-	uint8 opcode;		
-	uint8 reserved;
-	uint8 bssidx;		
-	uint8 role;		
-} wl_event_data_if_t;
-
-
-#define WLC_E_IF_ADD		1	
-#define WLC_E_IF_DEL		2	
-#define WLC_E_IF_CHANGE		3	
-
-
-#define WLC_E_IF_ROLE_STA		0	
-#define WLC_E_IF_ROLE_AP		1	
-#define WLC_E_IF_ROLE_WDS		2	
-#define WLC_E_IF_ROLE_P2P_GO		3	
-#define WLC_E_IF_ROLE_P2P_CLIENT	4	
-#define WLC_E_IF_ROLE_BTA_CREATOR	5	
-#define WLC_E_IF_ROLE_BTA_ACCEPTOR	6	
-
-
-#define WLC_E_LINK_BCN_LOSS	1	
-#define WLC_E_LINK_DISASSOC	2	
-#define WLC_E_LINK_ASSOC_REC	3	
-#define WLC_E_LINK_BSSCFG_DIS	4	
-
-
-#define WLC_E_OVL_DOWNLOAD	0	
-#define WLC_E_OVL_UPDATE_IND	1	
-
-
-#include <packed_section_end.h>
-
-#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/sdio.h b/drivers/net/wireless/bcmdhd/include/sdio.h
deleted file mode 100644
index ca932266a1b..00000000000
--- a/drivers/net/wireless/bcmdhd/include/sdio.h
+++ /dev/null
@@ -1,611 +0,0 @@
-/*
- * SDIO spec header file
- * Protocol and standard (common) device definitions
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: sdio.h,v 13.27.14.1 2010-09-07 13:37:45 Exp $
- */
-
-#ifndef	_SDIO_H
-#define	_SDIO_H
-
-
-/* CCCR structure for function 0 */
-typedef volatile struct {
-	uint8	cccr_sdio_rev;		/* RO, cccr and sdio revision */
-	uint8	sd_rev;			/* RO, sd spec revision */
-	uint8	io_en;			/* I/O enable */
-	uint8	io_rdy;			/* I/O ready reg */
-	uint8	intr_ctl;		/* Master and per function interrupt enable control */
-	uint8	intr_status;		/* RO, interrupt pending status */
-	uint8	io_abort;		/* read/write abort or reset all functions */
-	uint8	bus_inter;		/* bus interface control */
-	uint8	capability;		/* RO, card capability */
-
-	uint8	cis_base_low;		/* 0x9 RO, common CIS base address, LSB */
-	uint8	cis_base_mid;
-	uint8	cis_base_high;		/* 0xB RO, common CIS base address, MSB */
-
-	/* suspend/resume registers */
-	uint8	bus_suspend;		/* 0xC */
-	uint8	func_select;		/* 0xD */
-	uint8	exec_flag;		/* 0xE */
-	uint8	ready_flag;		/* 0xF */
-
-	uint8	fn0_blk_size[2];	/* 0x10(LSB), 0x11(MSB) */
-
-	uint8	power_control;		/* 0x12 (SDIO version 1.10) */
-
-	uint8	speed_control;		/* 0x13 */
-} sdio_regs_t;
-
-/* SDIO Device CCCR offsets */
-#define SDIOD_CCCR_REV			0x00
-#define SDIOD_CCCR_SDREV		0x01
-#define SDIOD_CCCR_IOEN			0x02
-#define SDIOD_CCCR_IORDY		0x03
-#define SDIOD_CCCR_INTEN		0x04
-#define SDIOD_CCCR_INTPEND		0x05
-#define SDIOD_CCCR_IOABORT		0x06
-#define SDIOD_CCCR_BICTRL		0x07
-#define SDIOD_CCCR_CAPABLITIES		0x08
-#define SDIOD_CCCR_CISPTR_0		0x09
-#define SDIOD_CCCR_CISPTR_1		0x0A
-#define SDIOD_CCCR_CISPTR_2		0x0B
-#define SDIOD_CCCR_BUSSUSP		0x0C
-#define SDIOD_CCCR_FUNCSEL		0x0D
-#define SDIOD_CCCR_EXECFLAGS		0x0E
-#define SDIOD_CCCR_RDYFLAGS		0x0F
-#define SDIOD_CCCR_BLKSIZE_0		0x10
-#define SDIOD_CCCR_BLKSIZE_1		0x11
-#define SDIOD_CCCR_POWER_CONTROL	0x12
-#define SDIOD_CCCR_SPEED_CONTROL	0x13
-#define SDIOD_CCCR_UHSI_SUPPORT		0x14
-#define SDIOD_CCCR_DRIVER_STRENGTH	0x15
-#define SDIOD_CCCR_INTR_EXTN		0x16
-
-/* Broadcom extensions (corerev >= 1) */
-#define SDIOD_CCCR_BRCM_SEPINT		0xf2
-
-/* cccr_sdio_rev */
-#define SDIO_REV_SDIOID_MASK	0xf0	/* SDIO spec revision number */
-#define SDIO_REV_CCCRID_MASK	0x0f	/* CCCR format version number */
-
-/* sd_rev */
-#define SD_REV_PHY_MASK		0x0f	/* SD format version number */
-
-/* io_en */
-#define SDIO_FUNC_ENABLE_1	0x02	/* function 1 I/O enable */
-#define SDIO_FUNC_ENABLE_2	0x04	/* function 2 I/O enable */
-
-/* io_rdys */
-#define SDIO_FUNC_READY_1	0x02	/* function 1 I/O ready */
-#define SDIO_FUNC_READY_2	0x04	/* function 2 I/O ready */
-
-/* intr_ctl */
-#define INTR_CTL_MASTER_EN	0x1	/* interrupt enable master */
-#define INTR_CTL_FUNC1_EN	0x2	/* interrupt enable for function 1 */
-#define INTR_CTL_FUNC2_EN	0x4	/* interrupt enable for function 2 */
-
-/* intr_status */
-#define INTR_STATUS_FUNC1	0x2	/* interrupt pending for function 1 */
-#define INTR_STATUS_FUNC2	0x4	/* interrupt pending for function 2 */
-
-/* io_abort */
-#define IO_ABORT_RESET_ALL	0x08	/* I/O card reset */
-#define IO_ABORT_FUNC_MASK	0x07	/* abort selction: function x */
-
-/* bus_inter */
-#define BUS_CARD_DETECT_DIS	0x80	/* Card Detect disable */
-#define BUS_SPI_CONT_INTR_CAP	0x40	/* support continuous SPI interrupt */
-#define BUS_SPI_CONT_INTR_EN	0x20	/* continuous SPI interrupt enable */
-#define BUS_SD_DATA_WIDTH_MASK	0x03	/* bus width mask */
-#define BUS_SD_DATA_WIDTH_4BIT	0x02	/* bus width 4-bit mode */
-#define BUS_SD_DATA_WIDTH_1BIT	0x00	/* bus width 1-bit mode */
-
-/* capability */
-#define SDIO_CAP_4BLS		0x80	/* 4-bit support for low speed card */
-#define SDIO_CAP_LSC		0x40	/* low speed card */
-#define SDIO_CAP_E4MI		0x20	/* enable interrupt between block of data in 4-bit mode */
-#define SDIO_CAP_S4MI		0x10	/* support interrupt between block of data in 4-bit mode */
-#define SDIO_CAP_SBS		0x08	/* support suspend/resume */
-#define SDIO_CAP_SRW		0x04	/* support read wait */
-#define SDIO_CAP_SMB		0x02	/* support multi-block transfer */
-#define SDIO_CAP_SDC		0x01	/* Support Direct commands during multi-byte transfer */
-
-/* power_control */
-#define SDIO_POWER_SMPC		0x01	/* supports master power control (RO) */
-#define SDIO_POWER_EMPC		0x02	/* enable master power control (allow > 200mA) (RW) */
-
-/* speed_control (control device entry into high-speed clocking mode) */
-#define SDIO_SPEED_SHS		0x01	/* supports high-speed [clocking] mode (RO) */
-#define SDIO_SPEED_EHS		0x02	/* enable high-speed [clocking] mode (RW) */
-
-/* for setting bus speed in card: 0x13h */
-#define SDIO_BUS_SPEED_UHSISEL_M	BITFIELD_MASK(3)
-#define SDIO_BUS_SPEED_UHSISEL_S	1
-
-/* for getting bus speed cap in card: 0x14h */
-#define SDIO_BUS_SPEED_UHSICAP_M	BITFIELD_MASK(3)
-#define SDIO_BUS_SPEED_UHSICAP_S	0
-
-/* for getting driver type CAP in card: 0x15h */
-#define SDIO_BUS_DRVR_TYPE_CAP_M	BITFIELD_MASK(3)
-#define SDIO_BUS_DRVR_TYPE_CAP_S	0
-
-/* for setting driver type selection in card: 0x15h */
-#define SDIO_BUS_DRVR_TYPE_SEL_M	BITFIELD_MASK(2)
-#define SDIO_BUS_DRVR_TYPE_SEL_S	4
-
-/* for getting async int support in card: 0x16h */
-#define SDIO_BUS_ASYNCINT_CAP_M	BITFIELD_MASK(1)
-#define SDIO_BUS_ASYNCINT_CAP_S	0
-
-/* for setting async int selection in card: 0x16h */
-#define SDIO_BUS_ASYNCINT_SEL_M	BITFIELD_MASK(1)
-#define SDIO_BUS_ASYNCINT_SEL_S	1
-
-/* brcm sepint */
-#define SDIO_SEPINT_MASK	0x01	/* route sdpcmdev intr onto separate pad (chip-specific) */
-#define SDIO_SEPINT_OE		0x02	/* 1 asserts output enable for above pad */
-#define SDIO_SEPINT_ACT_HI	0x04	/* use active high interrupt level instead of active low */
-
-/* FBR structure for function 1-7, FBR addresses and register offsets */
-typedef volatile struct {
-	uint8	devctr;			/* device interface, CSA control */
-	uint8	ext_dev;		/* extended standard I/O device type code */
-	uint8	pwr_sel;		/* power selection support */
-	uint8	PAD[6];			/* reserved */
-
-	uint8	cis_low;		/* CIS LSB */
-	uint8	cis_mid;
-	uint8	cis_high;		/* CIS MSB */
-	uint8	csa_low;		/* code storage area, LSB */
-	uint8	csa_mid;
-	uint8	csa_high;		/* code storage area, MSB */
-	uint8	csa_dat_win;		/* data access window to function */
-
-	uint8	fnx_blk_size[2];	/* block size, little endian */
-} sdio_fbr_t;
-
-/* Maximum number of I/O funcs */
-#define SDIOD_MAX_IOFUNCS		7
-
-/* SDIO Device FBR Start Address  */
-#define SDIOD_FBR_STARTADDR		0x100
-
-/* SDIO Device FBR Size */
-#define SDIOD_FBR_SIZE			0x100
-
-/* Macro to calculate FBR register base */
-#define SDIOD_FBR_BASE(n)		((n) * 0x100)
-
-/* Function register offsets */
-#define SDIOD_FBR_DEVCTR		0x00	/* basic info for function */
-#define SDIOD_FBR_EXT_DEV		0x01	/* extended I/O device code */
-#define SDIOD_FBR_PWR_SEL		0x02	/* power selection bits */
-
-/* SDIO Function CIS ptr offset */
-#define SDIOD_FBR_CISPTR_0		0x09
-#define SDIOD_FBR_CISPTR_1		0x0A
-#define SDIOD_FBR_CISPTR_2		0x0B
-
-/* Code Storage Area pointer */
-#define SDIOD_FBR_CSA_ADDR_0		0x0C
-#define SDIOD_FBR_CSA_ADDR_1		0x0D
-#define SDIOD_FBR_CSA_ADDR_2		0x0E
-#define SDIOD_FBR_CSA_DATA		0x0F
-
-/* SDIO Function I/O Block Size */
-#define SDIOD_FBR_BLKSIZE_0		0x10
-#define SDIOD_FBR_BLKSIZE_1		0x11
-
-/* devctr */
-#define SDIOD_FBR_DEVCTR_DIC	0x0f	/* device interface code */
-#define SDIOD_FBR_DECVTR_CSA	0x40	/* CSA support flag */
-#define SDIOD_FBR_DEVCTR_CSA_EN	0x80	/* CSA enabled */
-/* interface codes */
-#define SDIOD_DIC_NONE		0	/* SDIO standard interface is not supported */
-#define SDIOD_DIC_UART		1
-#define SDIOD_DIC_BLUETOOTH_A	2
-#define SDIOD_DIC_BLUETOOTH_B	3
-#define SDIOD_DIC_GPS		4
-#define SDIOD_DIC_CAMERA	5
-#define SDIOD_DIC_PHS		6
-#define SDIOD_DIC_WLAN		7
-#define SDIOD_DIC_EXT		0xf	/* extended device interface, read ext_dev register */
-
-/* pwr_sel */
-#define SDIOD_PWR_SEL_SPS	0x01	/* supports power selection */
-#define SDIOD_PWR_SEL_EPS	0x02	/* enable power selection (low-current mode) */
-
-/* misc defines */
-#define SDIO_FUNC_0		0
-#define SDIO_FUNC_1		1
-#define SDIO_FUNC_2		2
-#define SDIO_FUNC_3		3
-#define SDIO_FUNC_4		4
-#define SDIO_FUNC_5		5
-#define SDIO_FUNC_6		6
-#define SDIO_FUNC_7		7
-
-#define SD_CARD_TYPE_UNKNOWN	0	/* bad type or unrecognized */
-#define SD_CARD_TYPE_IO		1	/* IO only card */
-#define SD_CARD_TYPE_MEMORY	2	/* memory only card */
-#define SD_CARD_TYPE_COMBO	3	/* IO and memory combo card */
-
-#define SDIO_MAX_BLOCK_SIZE	2048	/* maximum block size for block mode operation */
-#define SDIO_MIN_BLOCK_SIZE	1	/* minimum block size for block mode operation */
-
-/* Card registers: status bit position */
-#define CARDREG_STATUS_BIT_OUTOFRANGE		31
-#define CARDREG_STATUS_BIT_COMCRCERROR		23
-#define CARDREG_STATUS_BIT_ILLEGALCOMMAND	22
-#define CARDREG_STATUS_BIT_ERROR		19
-#define CARDREG_STATUS_BIT_IOCURRENTSTATE3	12
-#define CARDREG_STATUS_BIT_IOCURRENTSTATE2	11
-#define CARDREG_STATUS_BIT_IOCURRENTSTATE1	10
-#define CARDREG_STATUS_BIT_IOCURRENTSTATE0	9
-#define CARDREG_STATUS_BIT_FUN_NUM_ERROR	4
-
-
-
-#define SD_CMD_GO_IDLE_STATE		0	/* mandatory for SDIO */
-#define SD_CMD_SEND_OPCOND		1
-#define SD_CMD_MMC_SET_RCA		3
-#define SD_CMD_IO_SEND_OP_COND		5	/* mandatory for SDIO */
-#define SD_CMD_SELECT_DESELECT_CARD	7
-#define SD_CMD_SEND_CSD			9
-#define SD_CMD_SEND_CID			10
-#define SD_CMD_STOP_TRANSMISSION	12
-#define SD_CMD_SEND_STATUS		13
-#define SD_CMD_GO_INACTIVE_STATE	15
-#define SD_CMD_SET_BLOCKLEN		16
-#define SD_CMD_READ_SINGLE_BLOCK	17
-#define SD_CMD_READ_MULTIPLE_BLOCK	18
-#define SD_CMD_WRITE_BLOCK		24
-#define SD_CMD_WRITE_MULTIPLE_BLOCK	25
-#define SD_CMD_PROGRAM_CSD		27
-#define SD_CMD_SET_WRITE_PROT		28
-#define SD_CMD_CLR_WRITE_PROT		29
-#define SD_CMD_SEND_WRITE_PROT		30
-#define SD_CMD_ERASE_WR_BLK_START	32
-#define SD_CMD_ERASE_WR_BLK_END		33
-#define SD_CMD_ERASE			38
-#define SD_CMD_LOCK_UNLOCK		42
-#define SD_CMD_IO_RW_DIRECT		52	/* mandatory for SDIO */
-#define SD_CMD_IO_RW_EXTENDED		53	/* mandatory for SDIO */
-#define SD_CMD_APP_CMD			55
-#define SD_CMD_GEN_CMD			56
-#define SD_CMD_READ_OCR			58
-#define SD_CMD_CRC_ON_OFF		59	/* mandatory for SDIO */
-#define SD_ACMD_SD_STATUS		13
-#define SD_ACMD_SEND_NUM_WR_BLOCKS	22
-#define SD_ACMD_SET_WR_BLOCK_ERASE_CNT	23
-#define SD_ACMD_SD_SEND_OP_COND		41
-#define SD_ACMD_SET_CLR_CARD_DETECT	42
-#define SD_ACMD_SEND_SCR		51
-
-/* argument for SD_CMD_IO_RW_DIRECT and SD_CMD_IO_RW_EXTENDED */
-#define SD_IO_OP_READ		0   /* Read_Write: Read */
-#define SD_IO_OP_WRITE		1   /* Read_Write: Write */
-#define SD_IO_RW_NORMAL		0   /* no RAW */
-#define SD_IO_RW_RAW		1   /* RAW */
-#define SD_IO_BYTE_MODE		0   /* Byte Mode */
-#define SD_IO_BLOCK_MODE	1   /* BlockMode */
-#define SD_IO_FIXED_ADDRESS	0   /* fix Address */
-#define SD_IO_INCREMENT_ADDRESS	1   /* IncrementAddress */
-
-/* build SD_CMD_IO_RW_DIRECT Argument */
-#define SDIO_IO_RW_DIRECT_ARG(rw, raw, func, addr, data) \
-	((((rw) & 1) << 31) | (((func) & 0x7) << 28) | (((raw) & 1) << 27) | \
-	 (((addr) & 0x1FFFF) << 9) | ((data) & 0xFF))
-
-/* build SD_CMD_IO_RW_EXTENDED Argument */
-#define SDIO_IO_RW_EXTENDED_ARG(rw, blk, func, addr, inc_addr, count) \
-	((((rw) & 1) << 31) | (((func) & 0x7) << 28) | (((blk) & 1) << 27) | \
-	 (((inc_addr) & 1) << 26) | (((addr) & 0x1FFFF) << 9) | ((count) & 0x1FF))
-
-/* SDIO response parameters */
-#define SD_RSP_NO_NONE			0
-#define SD_RSP_NO_1			1
-#define SD_RSP_NO_2			2
-#define SD_RSP_NO_3			3
-#define SD_RSP_NO_4			4
-#define SD_RSP_NO_5			5
-#define SD_RSP_NO_6			6
-
-	/* Modified R6 response (to CMD3) */
-#define SD_RSP_MR6_COM_CRC_ERROR	0x8000
-#define SD_RSP_MR6_ILLEGAL_COMMAND	0x4000
-#define SD_RSP_MR6_ERROR		0x2000
-
-	/* Modified R1 in R4 Response (to CMD5) */
-#define SD_RSP_MR1_SBIT			0x80
-#define SD_RSP_MR1_PARAMETER_ERROR	0x40
-#define SD_RSP_MR1_RFU5			0x20
-#define SD_RSP_MR1_FUNC_NUM_ERROR	0x10
-#define SD_RSP_MR1_COM_CRC_ERROR	0x08
-#define SD_RSP_MR1_ILLEGAL_COMMAND	0x04
-#define SD_RSP_MR1_RFU1			0x02
-#define SD_RSP_MR1_IDLE_STATE		0x01
-
-	/* R5 response (to CMD52 and CMD53) */
-#define SD_RSP_R5_COM_CRC_ERROR		0x80
-#define SD_RSP_R5_ILLEGAL_COMMAND	0x40
-#define SD_RSP_R5_IO_CURRENTSTATE1	0x20
-#define SD_RSP_R5_IO_CURRENTSTATE0	0x10
-#define SD_RSP_R5_ERROR			0x08
-#define SD_RSP_R5_RFU			0x04
-#define SD_RSP_R5_FUNC_NUM_ERROR	0x02
-#define SD_RSP_R5_OUT_OF_RANGE		0x01
-
-#define SD_RSP_R5_ERRBITS		0xCB
-
-
-/* ------------------------------------------------
- *  SDIO Commands and responses
- *
- *  I/O only commands are:
- *      CMD0, CMD3, CMD5, CMD7, CMD15, CMD52, CMD53
- * ------------------------------------------------
- */
-
-/* SDIO Commands */
-#define SDIOH_CMD_0		0
-#define SDIOH_CMD_3		3
-#define SDIOH_CMD_5		5
-#define SDIOH_CMD_7		7
-#define SDIOH_CMD_11		11
-#define SDIOH_CMD_14		14
-#define SDIOH_CMD_15		15
-#define SDIOH_CMD_19		19
-#define SDIOH_CMD_52		52
-#define SDIOH_CMD_53		53
-#define SDIOH_CMD_59		59
-
-/* SDIO Command Responses */
-#define SDIOH_RSP_NONE		0
-#define SDIOH_RSP_R1		1
-#define SDIOH_RSP_R2		2
-#define SDIOH_RSP_R3		3
-#define SDIOH_RSP_R4		4
-#define SDIOH_RSP_R5		5
-#define SDIOH_RSP_R6		6
-
-/*
- *  SDIO Response Error flags
- */
-#define SDIOH_RSP5_ERROR_FLAGS	0xCB
-
-/* ------------------------------------------------
- * SDIO Command structures. I/O only commands are:
- *
- * 	CMD0, CMD3, CMD5, CMD7, CMD15, CMD52, CMD53
- * ------------------------------------------------
- */
-
-#define CMD5_OCR_M		BITFIELD_MASK(24)
-#define CMD5_OCR_S		0
-
-#define CMD5_S18R_M		BITFIELD_MASK(1)
-#define CMD5_S18R_S		24
-
-#define CMD7_RCA_M		BITFIELD_MASK(16)
-#define CMD7_RCA_S		16
-#define CMD14_RCA_M		BITFIELD_MASK(16)
-#define CMD14_RCA_S		16
-#define CMD14_SLEEP_M		BITFIELD_MASK(1)
-#define CMD14_SLEEP_S		15
-
-#define CMD_15_RCA_M		BITFIELD_MASK(16)
-#define CMD_15_RCA_S		16
-
-#define CMD52_DATA_M		BITFIELD_MASK(8)  /* Bits [7:0]    - Write Data/Stuff bits of CMD52
-						   */
-#define CMD52_DATA_S		0
-#define CMD52_REG_ADDR_M	BITFIELD_MASK(17) /* Bits [25:9]   - register address */
-#define CMD52_REG_ADDR_S	9
-#define CMD52_RAW_M		BITFIELD_MASK(1)  /* Bit  27       - Read after Write flag */
-#define CMD52_RAW_S		27
-#define CMD52_FUNCTION_M	BITFIELD_MASK(3)  /* Bits [30:28]  - Function number */
-#define CMD52_FUNCTION_S	28
-#define CMD52_RW_FLAG_M		BITFIELD_MASK(1)  /* Bit  31       - R/W flag */
-#define CMD52_RW_FLAG_S		31
-
-
-#define CMD53_BYTE_BLK_CNT_M	BITFIELD_MASK(9) /* Bits [8:0]     - Byte/Block Count of CMD53 */
-#define CMD53_BYTE_BLK_CNT_S	0
-#define CMD53_REG_ADDR_M	BITFIELD_MASK(17) /* Bits [25:9]   - register address */
-#define CMD53_REG_ADDR_S	9
-#define CMD53_OP_CODE_M		BITFIELD_MASK(1)  /* Bit  26       - R/W Operation Code */
-#define CMD53_OP_CODE_S		26
-#define CMD53_BLK_MODE_M	BITFIELD_MASK(1)  /* Bit  27       - Block Mode */
-#define CMD53_BLK_MODE_S	27
-#define CMD53_FUNCTION_M	BITFIELD_MASK(3)  /* Bits [30:28]  - Function number */
-#define CMD53_FUNCTION_S	28
-#define CMD53_RW_FLAG_M		BITFIELD_MASK(1)  /* Bit  31       - R/W flag */
-#define CMD53_RW_FLAG_S		31
-
-/* ------------------------------------------------------
- * SDIO Command Response structures for SD1 and SD4 modes
- *  -----------------------------------------------------
- */
-#define RSP4_IO_OCR_M		BITFIELD_MASK(24) /* Bits [23:0]  - Card's OCR Bits [23:0] */
-#define RSP4_IO_OCR_S		0
-
-#define RSP4_S18A_M			BITFIELD_MASK(1) /* Bits [23:0]  - Card's OCR Bits [23:0] */
-#define RSP4_S18A_S			24
-
-#define RSP4_STUFF_M		BITFIELD_MASK(3)  /* Bits [26:24] - Stuff bits */
-#define RSP4_STUFF_S		24
-#define RSP4_MEM_PRESENT_M	BITFIELD_MASK(1)  /* Bit  27      - Memory present */
-#define RSP4_MEM_PRESENT_S	27
-#define RSP4_NUM_FUNCS_M	BITFIELD_MASK(3)  /* Bits [30:28] - Number of I/O funcs */
-#define RSP4_NUM_FUNCS_S	28
-#define RSP4_CARD_READY_M	BITFIELD_MASK(1)  /* Bit  31      - SDIO card ready */
-#define RSP4_CARD_READY_S	31
-
-#define RSP6_STATUS_M		BITFIELD_MASK(16) /* Bits [15:0]  - Card status bits [19,22,23,12:0]
-						   */
-#define RSP6_STATUS_S		0
-#define RSP6_IO_RCA_M		BITFIELD_MASK(16) /* Bits [31:16] - RCA bits[31-16] */
-#define RSP6_IO_RCA_S		16
-
-#define RSP1_AKE_SEQ_ERROR_M	BITFIELD_MASK(1)  /* Bit 3       - Authentication seq error */
-#define RSP1_AKE_SEQ_ERROR_S	3
-#define RSP1_APP_CMD_M		BITFIELD_MASK(1)  /* Bit 5       - Card expects ACMD */
-#define RSP1_APP_CMD_S		5
-#define RSP1_READY_FOR_DATA_M	BITFIELD_MASK(1)  /* Bit 8       - Ready for data (buff empty) */
-#define RSP1_READY_FOR_DATA_S	8
-#define RSP1_CURR_STATE_M	BITFIELD_MASK(4)  /* Bits [12:9] - State of card
-						   * when Cmd was received
-						   */
-#define RSP1_CURR_STATE_S	9
-#define RSP1_EARSE_RESET_M	BITFIELD_MASK(1)  /* Bit 13   - Erase seq cleared */
-#define RSP1_EARSE_RESET_S	13
-#define RSP1_CARD_ECC_DISABLE_M	BITFIELD_MASK(1)  /* Bit 14   - Card ECC disabled */
-#define RSP1_CARD_ECC_DISABLE_S	14
-#define RSP1_WP_ERASE_SKIP_M	BITFIELD_MASK(1)  /* Bit 15   - Partial blocks erased due to W/P */
-#define RSP1_WP_ERASE_SKIP_S	15
-#define RSP1_CID_CSD_OVERW_M	BITFIELD_MASK(1)  /* Bit 16   - Illegal write to CID or R/O bits
-						   * of CSD
-						   */
-#define RSP1_CID_CSD_OVERW_S	16
-#define RSP1_ERROR_M		BITFIELD_MASK(1)  /* Bit 19   - General/Unknown error */
-#define RSP1_ERROR_S		19
-#define RSP1_CC_ERROR_M		BITFIELD_MASK(1)  /* Bit 20   - Internal Card Control error */
-#define RSP1_CC_ERROR_S		20
-#define RSP1_CARD_ECC_FAILED_M	BITFIELD_MASK(1)  /* Bit 21   - Card internal ECC failed
-						   * to correct data
-						   */
-#define RSP1_CARD_ECC_FAILED_S	21
-#define RSP1_ILLEGAL_CMD_M	BITFIELD_MASK(1)  /* Bit 22   - Cmd not legal for the card state */
-#define RSP1_ILLEGAL_CMD_S	22
-#define RSP1_COM_CRC_ERROR_M	BITFIELD_MASK(1)  /* Bit 23   - CRC check of previous command failed
-						   */
-#define RSP1_COM_CRC_ERROR_S	23
-#define RSP1_LOCK_UNLOCK_FAIL_M	BITFIELD_MASK(1)  /* Bit 24   - Card lock-unlock Cmd Seq error */
-#define RSP1_LOCK_UNLOCK_FAIL_S	24
-#define RSP1_CARD_LOCKED_M	BITFIELD_MASK(1)  /* Bit 25   - Card locked by the host */
-#define RSP1_CARD_LOCKED_S	25
-#define RSP1_WP_VIOLATION_M	BITFIELD_MASK(1)  /* Bit 26   - Attempt to program
-						   * write-protected blocks
-						   */
-#define RSP1_WP_VIOLATION_S	26
-#define RSP1_ERASE_PARAM_M	BITFIELD_MASK(1)  /* Bit 27   - Invalid erase blocks */
-#define RSP1_ERASE_PARAM_S	27
-#define RSP1_ERASE_SEQ_ERR_M	BITFIELD_MASK(1)  /* Bit 28   - Erase Cmd seq error */
-#define RSP1_ERASE_SEQ_ERR_S	28
-#define RSP1_BLK_LEN_ERR_M	BITFIELD_MASK(1)  /* Bit 29   - Block length error */
-#define RSP1_BLK_LEN_ERR_S	29
-#define RSP1_ADDR_ERR_M		BITFIELD_MASK(1)  /* Bit 30   - Misaligned address */
-#define RSP1_ADDR_ERR_S		30
-#define RSP1_OUT_OF_RANGE_M	BITFIELD_MASK(1)  /* Bit 31   - Cmd arg was out of range */
-#define RSP1_OUT_OF_RANGE_S	31
-
-
-#define RSP5_DATA_M		BITFIELD_MASK(8)  /* Bits [0:7]   - data */
-#define RSP5_DATA_S		0
-#define RSP5_FLAGS_M		BITFIELD_MASK(8)  /* Bit  [15:8]  - Rsp flags */
-#define RSP5_FLAGS_S		8
-#define RSP5_STUFF_M		BITFIELD_MASK(16) /* Bits [31:16] - Stuff bits */
-#define RSP5_STUFF_S		16
-
-/* ----------------------------------------------
- * SDIO Command Response structures for SPI mode
- * ----------------------------------------------
- */
-#define SPIRSP4_IO_OCR_M	BITFIELD_MASK(16) /* Bits [15:0]    - Card's OCR Bits [23:8] */
-#define SPIRSP4_IO_OCR_S	0
-#define SPIRSP4_STUFF_M		BITFIELD_MASK(3)  /* Bits [18:16]   - Stuff bits */
-#define SPIRSP4_STUFF_S		16
-#define SPIRSP4_MEM_PRESENT_M	BITFIELD_MASK(1)  /* Bit  19        - Memory present */
-#define SPIRSP4_MEM_PRESENT_S	19
-#define SPIRSP4_NUM_FUNCS_M	BITFIELD_MASK(3)  /* Bits [22:20]   - Number of I/O funcs */
-#define SPIRSP4_NUM_FUNCS_S	20
-#define SPIRSP4_CARD_READY_M	BITFIELD_MASK(1)  /* Bit  23        - SDIO card ready */
-#define SPIRSP4_CARD_READY_S	23
-#define SPIRSP4_IDLE_STATE_M	BITFIELD_MASK(1)  /* Bit  24        - idle state */
-#define SPIRSP4_IDLE_STATE_S	24
-#define SPIRSP4_ILLEGAL_CMD_M	BITFIELD_MASK(1)  /* Bit  26        - Illegal Cmd error */
-#define SPIRSP4_ILLEGAL_CMD_S	26
-#define SPIRSP4_COM_CRC_ERROR_M	BITFIELD_MASK(1)  /* Bit  27        - COM CRC error */
-#define SPIRSP4_COM_CRC_ERROR_S	27
-#define SPIRSP4_FUNC_NUM_ERROR_M	BITFIELD_MASK(1)  /* Bit  28        - Function number error
-							   */
-#define SPIRSP4_FUNC_NUM_ERROR_S	28
-#define SPIRSP4_PARAM_ERROR_M	BITFIELD_MASK(1)  /* Bit  30        - Parameter Error Bit */
-#define SPIRSP4_PARAM_ERROR_S	30
-#define SPIRSP4_START_BIT_M	BITFIELD_MASK(1)  /* Bit  31        - Start Bit */
-#define SPIRSP4_START_BIT_S	31
-
-#define SPIRSP5_DATA_M			BITFIELD_MASK(8)  /* Bits [23:16]   - R/W Data */
-#define SPIRSP5_DATA_S			16
-#define SPIRSP5_IDLE_STATE_M		BITFIELD_MASK(1)  /* Bit  24        - Idle state */
-#define SPIRSP5_IDLE_STATE_S		24
-#define SPIRSP5_ILLEGAL_CMD_M		BITFIELD_MASK(1)  /* Bit  26        - Illegal Cmd error */
-#define SPIRSP5_ILLEGAL_CMD_S		26
-#define SPIRSP5_COM_CRC_ERROR_M		BITFIELD_MASK(1)  /* Bit  27        - COM CRC error */
-#define SPIRSP5_COM_CRC_ERROR_S		27
-#define SPIRSP5_FUNC_NUM_ERROR_M	BITFIELD_MASK(1)  /* Bit  28        - Function number error
-							   */
-#define SPIRSP5_FUNC_NUM_ERROR_S	28
-#define SPIRSP5_PARAM_ERROR_M		BITFIELD_MASK(1)  /* Bit  30        - Parameter Error Bit */
-#define SPIRSP5_PARAM_ERROR_S		30
-#define SPIRSP5_START_BIT_M		BITFIELD_MASK(1)  /* Bit  31        - Start Bit */
-#define SPIRSP5_START_BIT_S		31
-
-/* RSP6 card status format; Pg 68 Physical Layer spec v 1.10 */
-#define RSP6STAT_AKE_SEQ_ERROR_M	BITFIELD_MASK(1)  /* Bit 3	- Authentication seq error
-							   */
-#define RSP6STAT_AKE_SEQ_ERROR_S	3
-#define RSP6STAT_APP_CMD_M		BITFIELD_MASK(1)  /* Bit 5	- Card expects ACMD */
-#define RSP6STAT_APP_CMD_S		5
-#define RSP6STAT_READY_FOR_DATA_M	BITFIELD_MASK(1)  /* Bit 8	- Ready for data
-							   * (buff empty)
-							   */
-#define RSP6STAT_READY_FOR_DATA_S	8
-#define RSP6STAT_CURR_STATE_M		BITFIELD_MASK(4)  /* Bits [12:9] - Card state at
-							   * Cmd reception
-							   */
-#define RSP6STAT_CURR_STATE_S		9
-#define RSP6STAT_ERROR_M		BITFIELD_MASK(1)  /* Bit 13  - General/Unknown error Bit 19
-							   */
-#define RSP6STAT_ERROR_S		13
-#define RSP6STAT_ILLEGAL_CMD_M		BITFIELD_MASK(1)  /* Bit 14  - Illegal cmd for
-							   * card state Bit 22
-							   */
-#define RSP6STAT_ILLEGAL_CMD_S		14
-#define RSP6STAT_COM_CRC_ERROR_M	BITFIELD_MASK(1)  /* Bit 15  - CRC previous command
-							   * failed Bit 23
-							   */
-#define RSP6STAT_COM_CRC_ERROR_S	15
-
-#define SDIOH_XFER_TYPE_READ    SD_IO_OP_READ
-#define SDIOH_XFER_TYPE_WRITE   SD_IO_OP_WRITE
-
-/* command issue options */
-#define CMD_OPTION_DEFAULT	0
-#define CMD_OPTION_TUNING	1
-
-#endif /* _SDIO_H */
diff --git a/drivers/net/wireless/bcmdhd/include/wlioctl.h b/drivers/net/wireless/bcmdhd/include/wlioctl.h
deleted file mode 100644
index 1059de147ef..00000000000
--- a/drivers/net/wireless/bcmdhd/include/wlioctl.h
+++ /dev/null
@@ -1,2714 +0,0 @@
-/*
- * Custom OID/ioctl definitions for
- * Broadcom 802.11abg Networking Device Driver
- *
- * Definitions subject to change without notice.
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: wlioctl.h,v 1.767.2.38 2011-02-01 23:04:28 Exp $
- */
-
-
-#ifndef _wlioctl_h_
-#define _wlioctl_h_
-
-#include <typedefs.h>
-#include <proto/ethernet.h>
-#include <proto/bcmeth.h>
-#include <proto/bcmevent.h>
-#include <proto/802.11.h>
-#include <bcmwifi.h>
-
-#include <bcmcdc.h>
-
-#ifndef INTF_NAME_SIZ
-#define INTF_NAME_SIZ   16
-#endif
-
-
-typedef struct remote_ioctl {
-	cdc_ioctl_t     msg;
-	uint        data_len;
-	char            intf_name[INTF_NAME_SIZ];
-} rem_ioctl_t;
-#define REMOTE_SIZE sizeof(rem_ioctl_t)
-
-#define ACTION_FRAME_SIZE 1040
-
-typedef struct wl_action_frame {
-	struct ether_addr   da;
-	uint16          len;
-	uint32          packetId;
-	uint8           data[ACTION_FRAME_SIZE];
-} wl_action_frame_t;
-
-#define WL_WIFI_ACTION_FRAME_SIZE sizeof(struct wl_action_frame)
-
-typedef struct ssid_info
-{
-	uint8		ssid_len;
-	uint8		ssid[32];
-} ssid_info_t;
-
-typedef struct wl_af_params {
-	uint32          channel;
-	int32           dwell_time;
-	struct ether_addr   BSSID;
-	wl_action_frame_t   action_frame;
-} wl_af_params_t;
-
-#define WL_WIFI_AF_PARAMS_SIZE sizeof(struct wl_af_params)
-
-
-#define BWL_DEFAULT_PACKING
-#include <packed_section_start.h>
-
-
-
-
-
-#define LEGACY2_WL_BSS_INFO_VERSION 108     
-
-
-typedef struct wl_bss_info_108 {
-	uint32      version;        
-	uint32      length;         
-	struct ether_addr BSSID;
-	uint16      beacon_period;      
-	uint16      capability;     
-	uint8       SSID_len;
-	uint8       SSID[32];
-	struct {
-		uint    count;          
-		uint8   rates[16];      
-	} rateset;              
-	chanspec_t  chanspec;       
-	uint16      atim_window;        
-	uint8       dtim_period;        
-	int16       RSSI;           
-	int8        phy_noise;      
-
-	uint8       n_cap;          
-	uint32      nbss_cap;       
-	uint8       ctl_ch;         
-	uint32      reserved32[1];      
-	uint8       flags;          
-	uint8       reserved[3];        
-	uint8       basic_mcs[MCSSET_LEN];  
-
-	uint16      ie_offset;      
-	uint32      ie_length;      
-	
-	
-} wl_bss_info_108_t;
-
-#define WL_BSS_INFO_VERSION 109     
-
-
-typedef struct wl_bss_info {
-	uint32      version;        
-	uint32      length;         
-	struct ether_addr BSSID;
-	uint16      beacon_period;      
-	uint16      capability;     
-	uint8       SSID_len;
-	uint8       SSID[32];
-	struct {
-		uint    count;          
-		uint8   rates[16];      
-	} rateset;              
-	chanspec_t  chanspec;       
-	uint16      atim_window;        
-	uint8       dtim_period;        
-	int16       RSSI;           
-	int8        phy_noise;      
-
-	uint8       n_cap;          
-	uint32      nbss_cap;       
-	uint8       ctl_ch;         
-	uint32      reserved32[1];      
-	uint8       flags;          
-	uint8       reserved[3];        
-	uint8       basic_mcs[MCSSET_LEN];  
-
-	uint16      ie_offset;      
-	uint32      ie_length;      
-	int16       SNR;            
-	
-	
-} wl_bss_info_t;
-
-typedef struct wl_bsscfg {
-	uint32  wsec;
-	uint32  WPA_auth;
-	uint32  wsec_index;
-	uint32  associated;
-	uint32  BSS;
-	uint32  phytest_on;
-	struct ether_addr   prev_BSSID;
-	struct ether_addr   BSSID;
-} wl_bsscfg_t;
-
-typedef struct wl_bss_config {
-	uint32  atim_window;
-	uint32  beacon_period;
-	uint32  chanspec;
-} wl_bss_config_t;
-
-
-typedef struct wlc_ssid {
-	uint32      SSID_len;
-	uchar       SSID[32];
-} wlc_ssid_t;
-
-
-#define WL_BSSTYPE_INFRA 1
-#define WL_BSSTYPE_INDEP 0
-#define WL_BSSTYPE_ANY   2
-
-
-#define WL_SCANFLAGS_PASSIVE 0x01       
-#define WL_SCANFLAGS_RESERVED 0x02      
-#define WL_SCANFLAGS_PROHIBITED 0x04    
-
-#define WL_SCAN_PARAMS_SSID_MAX 	10
-typedef struct wl_scan_params {
-	wlc_ssid_t ssid;        
-	struct ether_addr bssid;    
-	int8 bss_type;          
-	uint8 scan_type;        
-	int32 nprobes;          
-	int32 active_time;      
-	int32 passive_time;     
-	int32 home_time;        
-	int32 channel_num;      
-	uint16 channel_list[1];     
-} wl_scan_params_t;
-
-
-#define WL_SCAN_PARAMS_FIXED_SIZE 64
-
-
-#define WL_SCAN_PARAMS_COUNT_MASK 0x0000ffff
-#define WL_SCAN_PARAMS_NSSID_SHIFT 16
-
-#define WL_SCAN_ACTION_START      1
-#define WL_SCAN_ACTION_CONTINUE   2
-#define WL_SCAN_ACTION_ABORT      3
-
-#define ISCAN_REQ_VERSION 1
-
-
-typedef struct wl_iscan_params {
-	uint32 version;
-	uint16 action;
-	uint16 scan_duration;
-	wl_scan_params_t params;
-} wl_iscan_params_t;
-
-
-#define WL_ISCAN_PARAMS_FIXED_SIZE (OFFSETOF(wl_iscan_params_t, params) + sizeof(wlc_ssid_t))
-
-typedef struct wl_scan_results {
-	uint32 buflen;
-	uint32 version;
-	uint32 count;
-	wl_bss_info_t bss_info[1];
-} wl_scan_results_t;
-
-
-#define WL_SCAN_RESULTS_FIXED_SIZE (sizeof(wl_scan_results_t) - sizeof(wl_bss_info_t))
-
-
-#define WL_SCAN_RESULTS_SUCCESS 0
-#define WL_SCAN_RESULTS_PARTIAL 1
-#define WL_SCAN_RESULTS_PENDING 2
-#define WL_SCAN_RESULTS_ABORTED 3
-#define WL_SCAN_RESULTS_NO_MEM  4
-
-
-#define DNGL_RXCTXT_SIZE	45
-
-#if defined(SIMPLE_ISCAN)
-#define ISCAN_RETRY_CNT   5
-#define ISCAN_STATE_IDLE   0
-#define ISCAN_STATE_SCANING 1
-#define ISCAN_STATE_PENDING 2
-
-
-#define WLC_IW_ISCAN_MAXLEN   2048
-typedef struct iscan_buf {
-	struct iscan_buf * next;
-	char   iscan_buf[WLC_IW_ISCAN_MAXLEN];
-} iscan_buf_t;
-#endif 
-
-#define ESCAN_REQ_VERSION 1
-
-typedef struct wl_escan_params {
-	uint32 version;
-	uint16 action;
-	uint16 sync_id;
-	wl_scan_params_t params;
-} wl_escan_params_t;
-
-#define WL_ESCAN_PARAMS_FIXED_SIZE (OFFSETOF(wl_escan_params_t, params) + sizeof(wlc_ssid_t))
-
-typedef struct wl_escan_result {
-	uint32 buflen;
-	uint32 version;
-	uint16 sync_id;
-	uint16 bss_count;
-	wl_bss_info_t bss_info[1];
-} wl_escan_result_t;
-
-#define WL_ESCAN_RESULTS_FIXED_SIZE (sizeof(wl_escan_result_t) - sizeof(wl_bss_info_t))
-
-
-typedef struct wl_iscan_results {
-	uint32 status;
-	wl_scan_results_t results;
-} wl_iscan_results_t;
-
-
-#define WL_ISCAN_RESULTS_FIXED_SIZE \
-	(WL_SCAN_RESULTS_FIXED_SIZE + OFFSETOF(wl_iscan_results_t, results))
-
-typedef struct wl_probe_params {
-	wlc_ssid_t ssid;
-	struct ether_addr bssid;
-	struct ether_addr mac;
-} wl_probe_params_t;
-
-#define WL_NUMRATES     16  
-typedef struct wl_rateset {
-	uint32  count;          
-	uint8   rates[WL_NUMRATES]; 
-} wl_rateset_t;
-
-typedef struct wl_rateset_args {
-	uint32  count;          
-	uint8   rates[WL_NUMRATES]; 
-	uint8   mcs[MCSSET_LEN];        
-} wl_rateset_args_t;
-
-
-typedef struct wl_uint32_list {
-	
-	uint32 count;
-	
-	uint32 element[1];
-} wl_uint32_list_t;
-
-
-typedef struct wl_assoc_params {
-	struct ether_addr bssid;    
-	uint16 bssid_cnt;       
-	int32 chanspec_num;     
-	chanspec_t chanspec_list[1];    
-} wl_assoc_params_t;
-#define WL_ASSOC_PARAMS_FIXED_SIZE  (sizeof(wl_assoc_params_t) - sizeof(chanspec_t))
-
-
-typedef wl_assoc_params_t wl_reassoc_params_t;
-#define WL_REASSOC_PARAMS_FIXED_SIZE    WL_ASSOC_PARAMS_FIXED_SIZE
-
-
-typedef wl_assoc_params_t wl_join_assoc_params_t;
-#define WL_JOIN_ASSOC_PARAMS_FIXED_SIZE        WL_ASSOC_PARAMS_FIXED_SIZE
-
-
-typedef struct wl_join_params {
-	wlc_ssid_t ssid;
-	wl_assoc_params_t params;   
-} wl_join_params_t;
-#define WL_JOIN_PARAMS_FIXED_SIZE   (sizeof(wl_join_params_t) - sizeof(chanspec_t))
-
-
-typedef struct wl_join_scan_params {
-	uint8 scan_type;                
-	int32 nprobes;                  
-	int32 active_time;              
-	int32 passive_time;             
-	int32 home_time;                
-} wl_join_scan_params_t;
-
-
-typedef struct wl_extjoin_params {
-	wlc_ssid_t ssid;                
-	wl_join_scan_params_t scan;
-	wl_join_assoc_params_t assoc;   
-} wl_extjoin_params_t;
-#define WL_EXTJOIN_PARAMS_FIXED_SIZE    (sizeof(wl_extjoin_params_t) - sizeof(chanspec_t))
-
-typedef struct {
-	uint32 num;
-	chanspec_t list[1];
-} chanspec_list_t;
-
-
-#define NRATE_MCS_INUSE 0x00000080  
-#define NRATE_RATE_MASK 0x0000007f  
-#define NRATE_STF_MASK  0x0000ff00  
-#define NRATE_STF_SHIFT 8       
-#define NRATE_OVERRIDE  0x80000000  
-#define NRATE_OVERRIDE_MCS_ONLY 0x40000000 
-#define NRATE_SGI_MASK  0x00800000      
-#define NRATE_SGI_SHIFT 23              
-#define NRATE_LDPC_CODING 0x00400000    
-#define NRATE_LDPC_SHIFT 22             
-
-#define NRATE_STF_SISO  0       
-#define NRATE_STF_CDD   1       
-#define NRATE_STF_STBC  2       
-#define NRATE_STF_SDM   3       
-
-#define ANTENNA_NUM_1   1       
-#define ANTENNA_NUM_2   2
-#define ANTENNA_NUM_3   3
-#define ANTENNA_NUM_4   4
-
-#define ANT_SELCFG_AUTO     0x80    
-#define ANT_SELCFG_MASK     0x33    
-#define ANT_SELCFG_MAX      4   
-#define ANT_SELCFG_TX_UNICAST   0   
-#define ANT_SELCFG_RX_UNICAST   1   
-#define ANT_SELCFG_TX_DEF   2   
-#define ANT_SELCFG_RX_DEF   3   
-
-#define MAX_STREAMS_SUPPORTED   4   
-
-typedef struct {
-	uint8 ant_config[ANT_SELCFG_MAX];   
-	uint8 num_antcfg;   
-} wlc_antselcfg_t;
-
-#define HIGHEST_SINGLE_STREAM_MCS   7 
-
-#define MAX_CCA_CHANNELS 38 
-#define MAX_CCA_SECS     60 
-
-#define IBSS_MED        15  
-#define IBSS_HI         25  
-#define OBSS_MED        12
-#define OBSS_HI         25
-#define INTERFER_MED    5
-#define INTERFER_HI     10
-
-#define  CCA_FLAG_2G_ONLY       0x01    
-#define  CCA_FLAG_5G_ONLY       0x02    
-#define  CCA_FLAG_IGNORE_DURATION   0x04    
-#define  CCA_FLAGS_PREFER_1_6_11    0x10
-#define  CCA_FLAG_IGNORE_INTERFER   0x20 
-
-#define CCA_ERRNO_BAND      1   
-#define CCA_ERRNO_DURATION  2   
-#define CCA_ERRNO_PREF_CHAN 3   
-#define CCA_ERRNO_INTERFER  4   
-#define CCA_ERRNO_TOO_FEW   5   
-
-typedef struct {
-	uint32 duration;    
-	uint32 congest_ibss;    
-				
-	uint32 congest_obss;    
-	uint32 interference;    
-	uint32 timestamp;   
-} cca_congest_t;
-
-typedef struct {
-	chanspec_t chanspec;    
-	uint8 num_secs;     
-	cca_congest_t  secs[1]; 
-} cca_congest_channel_req_t;
-
-#define WLC_CNTRY_BUF_SZ    4       
-
-typedef struct wl_country {
-	char country_abbrev[WLC_CNTRY_BUF_SZ];  
-	int32 rev;              
-	char ccode[WLC_CNTRY_BUF_SZ];       
-} wl_country_t;
-
-typedef struct wl_channels_in_country {
-	uint32 buflen;
-	uint32 band;
-	char country_abbrev[WLC_CNTRY_BUF_SZ];
-	uint32 count;
-	uint32 channel[1];
-} wl_channels_in_country_t;
-
-typedef struct wl_country_list {
-	uint32 buflen;
-	uint32 band_set;
-	uint32 band;
-	uint32 count;
-	char country_abbrev[1];
-} wl_country_list_t;
-
-#define WL_NUM_RPI_BINS     8
-#define WL_RM_TYPE_BASIC    1
-#define WL_RM_TYPE_CCA      2
-#define WL_RM_TYPE_RPI      3
-
-#define WL_RM_FLAG_PARALLEL (1<<0)
-
-#define WL_RM_FLAG_LATE     (1<<1)
-#define WL_RM_FLAG_INCAPABLE    (1<<2)
-#define WL_RM_FLAG_REFUSED  (1<<3)
-
-typedef struct wl_rm_req_elt {
-	int8    type;
-	int8    flags;
-	chanspec_t  chanspec;
-	uint32  token;      
-	uint32  tsf_h;      
-	uint32  tsf_l;      
-	uint32  dur;        
-} wl_rm_req_elt_t;
-
-typedef struct wl_rm_req {
-	uint32  token;      
-	uint32  count;      
-	void    *cb;        
-	void    *cb_arg;    
-	wl_rm_req_elt_t req[1]; 
-} wl_rm_req_t;
-#define WL_RM_REQ_FIXED_LEN OFFSETOF(wl_rm_req_t, req)
-
-typedef struct wl_rm_rep_elt {
-	int8    type;
-	int8    flags;
-	chanspec_t  chanspec;
-	uint32  token;      
-	uint32  tsf_h;      
-	uint32  tsf_l;      
-	uint32  dur;        
-	uint32  len;        
-	uint8   data[1];    
-} wl_rm_rep_elt_t;
-#define WL_RM_REP_ELT_FIXED_LEN 24  
-
-#define WL_RPI_REP_BIN_NUM 8
-typedef struct wl_rm_rpi_rep {
-	uint8   rpi[WL_RPI_REP_BIN_NUM];
-	int8    rpi_max[WL_RPI_REP_BIN_NUM];
-} wl_rm_rpi_rep_t;
-
-typedef struct wl_rm_rep {
-	uint32  token;      
-	uint32  len;        
-	wl_rm_rep_elt_t rep[1]; 
-} wl_rm_rep_t;
-#define WL_RM_REP_FIXED_LEN 8
-
-
-typedef enum sup_auth_status {
-	
-	WLC_SUP_DISCONNECTED = 0,
-	WLC_SUP_CONNECTING,
-	WLC_SUP_IDREQUIRED,
-	WLC_SUP_AUTHENTICATING,
-	WLC_SUP_AUTHENTICATED,
-	WLC_SUP_KEYXCHANGE,
-	WLC_SUP_KEYED,
-	WLC_SUP_TIMEOUT,
-	WLC_SUP_LAST_BASIC_STATE,
-
-	
-	
-	WLC_SUP_KEYXCHANGE_WAIT_M1 = WLC_SUP_AUTHENTICATED,
-	
-	WLC_SUP_KEYXCHANGE_PREP_M2 = WLC_SUP_KEYXCHANGE,
-	
-	WLC_SUP_KEYXCHANGE_WAIT_M3 = WLC_SUP_LAST_BASIC_STATE,
-	WLC_SUP_KEYXCHANGE_PREP_M4, 
-	WLC_SUP_KEYXCHANGE_WAIT_G1, 
-	WLC_SUP_KEYXCHANGE_PREP_G2  
-} sup_auth_status_t;
-
-
-#define CRYPTO_ALGO_OFF         0
-#define CRYPTO_ALGO_WEP1        1
-#define CRYPTO_ALGO_TKIP        2
-#define CRYPTO_ALGO_WEP128      3
-#define CRYPTO_ALGO_AES_CCM     4
-#define CRYPTO_ALGO_AES_OCB_MSDU    5
-#define CRYPTO_ALGO_AES_OCB_MPDU    6
-#define CRYPTO_ALGO_NALG        7
-#define CRYPTO_ALGO_PMK			12
-
-#define WSEC_GEN_MIC_ERROR  0x0001
-#define WSEC_GEN_REPLAY     0x0002
-#define WSEC_GEN_ICV_ERROR  0x0004
-
-#define WL_SOFT_KEY (1 << 0)    
-#define WL_PRIMARY_KEY  (1 << 1)    
-#define WL_KF_RES_4 (1 << 4)    
-#define WL_KF_RES_5 (1 << 5)    
-#define WL_IBSS_PEER_GROUP_KEY  (1 << 6)    
-
-typedef struct wl_wsec_key {
-	uint32      index;      
-	uint32      len;        
-	uint8       data[DOT11_MAX_KEY_SIZE];   
-	uint32      pad_1[18];
-	uint32      algo;       
-	uint32      flags;      
-	uint32      pad_2[2];
-	int     pad_3;
-	int     iv_initialized; 
-	int     pad_4;
-	
-	struct {
-		uint32  hi;     
-		uint16  lo;     
-	} rxiv;
-	uint32      pad_5[2];
-	struct ether_addr ea;       
-} wl_wsec_key_t;
-
-#define WSEC_MIN_PSK_LEN    8
-#define WSEC_MAX_PSK_LEN    64
-
-
-#define WSEC_PASSPHRASE     (1<<0)
-
-
-typedef struct {
-	ushort  key_len;        
-	ushort  flags;          
-	uint8   key[WSEC_MAX_PSK_LEN];  
-} wsec_pmk_t;
-
-
-#define WEP_ENABLED     0x0001
-#define TKIP_ENABLED        0x0002
-#define AES_ENABLED     0x0004
-#define WSEC_SWFLAG     0x0008
-#define SES_OW_ENABLED      0x0040  
-
-
-#define WPA_AUTH_DISABLED   0x0000  
-#define WPA_AUTH_NONE       0x0001  
-#define WPA_AUTH_UNSPECIFIED    0x0002  
-#define WPA_AUTH_PSK        0x0004  
- 
-#define WPA2_AUTH_UNSPECIFIED   0x0040  
-#define WPA2_AUTH_PSK       0x0080  
-#define BRCM_AUTH_PSK           0x0100  
-#define BRCM_AUTH_DPT       0x0200  
-#define WPA2_AUTH_MFP           0x1000
-#define WPA2_AUTH_TPK		0x2000
-#define WPA2_AUTH_FT		0x4000
-
-
-#define MAXPMKID        16
-
-typedef struct _pmkid {
-	struct ether_addr   BSSID;
-	uint8           PMKID[WPA2_PMKID_LEN];
-} pmkid_t;
-
-typedef struct _pmkid_list {
-	uint32  npmkid;
-	pmkid_t pmkid[1];
-} pmkid_list_t;
-
-typedef struct _pmkid_cand {
-	struct ether_addr   BSSID;
-	uint8           preauth;
-} pmkid_cand_t;
-
-typedef struct _pmkid_cand_list {
-	uint32  npmkid_cand;
-	pmkid_cand_t    pmkid_cand[1];
-} pmkid_cand_list_t;
-
-typedef struct wl_assoc_info {
-	uint32      req_len;
-	uint32      resp_len;
-	uint32      flags;
-	struct dot11_assoc_req req;
-	struct ether_addr reassoc_bssid;
-	struct dot11_assoc_resp resp;
-} wl_assoc_info_t;
-
-
-#define WLC_ASSOC_REQ_IS_REASSOC 0x01
-
-
-
-#define WLC_TXFILTER_OVERRIDE_DISABLED  0
-#define WLC_TXFILTER_OVERRIDE_ENABLED   1
-
-
-typedef struct {
-	uint32  val;
-	struct ether_addr ea;
-} scb_val_t;
-
-
-typedef struct {
-	uint32 code;
-	scb_val_t ioctl_args;
-} authops_t;
-
-
-typedef struct channel_info {
-	int hw_channel;
-	int target_channel;
-	int scan_channel;
-} channel_info_t;
-
-
-struct maclist {
-	uint count;         
-	struct ether_addr ea[1];    
-};
-
-
-typedef struct get_pktcnt {
-	uint rx_good_pkt;
-	uint rx_bad_pkt;
-	uint tx_good_pkt;
-	uint tx_bad_pkt;
-	uint rx_ocast_good_pkt; 
-} get_pktcnt_t;
-
-#define WL_IOCTL_ACTION_GET				0x0
-#define WL_IOCTL_ACTION_SET				0x1
-#define WL_IOCTL_ACTION_OVL_IDX_MASK	0x1e
-#define WL_IOCTL_ACTION_OVL_RSV			0x20
-#define WL_IOCTL_ACTION_OVL				0x40
-#define WL_IOCTL_ACTION_MASK			0x7e
-#define WL_IOCTL_ACTION_OVL_SHIFT		1
-
-
-typedef struct wl_ioctl {
-	uint cmd;   
-	void *buf;  
-	uint len;   
-	uint8 set;		
-	uint used;  
-	uint needed;    
-} wl_ioctl_t;
-
-
-#define ioctl_subtype   set     
-#define ioctl_pid   used        
-#define ioctl_status    needed      
-
-
-typedef struct wlc_rev_info {
-	uint        vendorid;   
-	uint        deviceid;   
-	uint        radiorev;   
-	uint        chiprev;    
-	uint        corerev;    
-	uint        boardid;    
-	uint        boardvendor;    
-	uint        boardrev;   
-	uint        driverrev;  
-	uint        ucoderev;   
-	uint        bus;        
-	uint        chipnum;    
-	uint        phytype;    
-	uint        phyrev;     
-	uint        anarev;     
-	uint        chippkg;    
-} wlc_rev_info_t;
-
-#define WL_REV_INFO_LEGACY_LENGTH   48
-
-#define WL_BRAND_MAX 10
-typedef struct wl_instance_info {
-	uint instance;
-	char brand[WL_BRAND_MAX];
-} wl_instance_info_t;
-
-
-typedef struct wl_txfifo_sz {
-	uint16  magic;
-	uint16  fifo;
-	uint16  size;
-} wl_txfifo_sz_t;
-
-#define WL_TXFIFO_SZ_MAGIC  0xa5a5
-
-
-
-#define WLC_IOV_NAME_LEN 30
-typedef struct wlc_iov_trx_s {
-	uint8 module;
-	uint8 type;
-	char name[WLC_IOV_NAME_LEN];
-} wlc_iov_trx_t;
-
-
-#define WLC_IOCTL_MAGIC     0x14e46c77
-
-
-#define WLC_IOCTL_VERSION   1
-
-#define WLC_IOCTL_MAXLEN        8192    
-#define WLC_IOCTL_SMLEN         256 
-#define WLC_IOCTL_MEDLEN        1536    
-#ifdef WLC_HIGH_ONLY
-#define WLC_SAMPLECOLLECT_MAXLEN    1024    
-#else
-#define WLC_SAMPLECOLLECT_MAXLEN    10240   
-#endif 
-
-
-#define WLC_GET_MAGIC               0
-#define WLC_GET_VERSION             1
-#define WLC_UP                  2
-#define WLC_DOWN                3
-#define WLC_GET_LOOP                4
-#define WLC_SET_LOOP                5
-#define WLC_DUMP                6
-#define WLC_GET_MSGLEVEL            7
-#define WLC_SET_MSGLEVEL            8
-#define WLC_GET_PROMISC             9
-#define WLC_SET_PROMISC             10
-#define WLC_OVERLAY_IOCTL           11
-#define WLC_GET_RATE                12
- 
-#define WLC_GET_INSTANCE            14
- 
- 
- 
- 
-#define WLC_GET_INFRA               19
-#define WLC_SET_INFRA               20
-#define WLC_GET_AUTH                21
-#define WLC_SET_AUTH                22
-#define WLC_GET_BSSID               23
-#define WLC_SET_BSSID               24
-#define WLC_GET_SSID                25
-#define WLC_SET_SSID                26
-#define WLC_RESTART             27
- 
-#define WLC_GET_CHANNEL             29
-#define WLC_SET_CHANNEL             30
-#define WLC_GET_SRL             31
-#define WLC_SET_SRL             32
-#define WLC_GET_LRL             33
-#define WLC_SET_LRL             34
-#define WLC_GET_PLCPHDR             35
-#define WLC_SET_PLCPHDR             36
-#define WLC_GET_RADIO               37
-#define WLC_SET_RADIO               38
-#define WLC_GET_PHYTYPE             39
-#define WLC_DUMP_RATE               40
-#define WLC_SET_RATE_PARAMS         41
-#define WLC_GET_FIXRATE             42
-#define WLC_SET_FIXRATE             43
- 
- 
-#define WLC_GET_KEY             44
-#define WLC_SET_KEY             45
-#define WLC_GET_REGULATORY          46
-#define WLC_SET_REGULATORY          47
-#define WLC_GET_PASSIVE_SCAN            48
-#define WLC_SET_PASSIVE_SCAN            49
-#define WLC_SCAN                50
-#define WLC_SCAN_RESULTS            51
-#define WLC_DISASSOC                52
-#define WLC_REASSOC             53
-#define WLC_GET_ROAM_TRIGGER            54
-#define WLC_SET_ROAM_TRIGGER            55
-#define WLC_GET_ROAM_DELTA          56
-#define WLC_SET_ROAM_DELTA          57
-#define WLC_GET_ROAM_SCAN_PERIOD        58
-#define WLC_SET_ROAM_SCAN_PERIOD        59
-#define WLC_EVM                 60  
-#define WLC_GET_TXANT               61
-#define WLC_SET_TXANT               62
-#define WLC_GET_ANTDIV              63
-#define WLC_SET_ANTDIV              64
- 
- 
-#define WLC_GET_CLOSED              67
-#define WLC_SET_CLOSED              68
-#define WLC_GET_MACLIST             69
-#define WLC_SET_MACLIST             70
-#define WLC_GET_RATESET             71
-#define WLC_SET_RATESET             72
- 
-#define WLC_LONGTRAIN               74
-#define WLC_GET_BCNPRD              75
-#define WLC_SET_BCNPRD              76
-#define WLC_GET_DTIMPRD             77
-#define WLC_SET_DTIMPRD             78
-#define WLC_GET_SROM                79
-#define WLC_SET_SROM                80
-#define WLC_GET_WEP_RESTRICT            81
-#define WLC_SET_WEP_RESTRICT            82
-#define WLC_GET_COUNTRY             83
-#define WLC_SET_COUNTRY             84
-#define WLC_GET_PM              85
-#define WLC_SET_PM              86
-#define WLC_GET_WAKE                87
-#define WLC_SET_WAKE                88
- 
-#define WLC_GET_FORCELINK           90  
-#define WLC_SET_FORCELINK           91  
-#define WLC_FREQ_ACCURACY           92  
-#define WLC_CARRIER_SUPPRESS            93  
-#define WLC_GET_PHYREG              94
-#define WLC_SET_PHYREG              95
-#define WLC_GET_RADIOREG            96
-#define WLC_SET_RADIOREG            97
-#define WLC_GET_REVINFO             98
-#define WLC_GET_UCANTDIV            99
-#define WLC_SET_UCANTDIV            100
-#define WLC_R_REG               101
-#define WLC_W_REG               102
-
- 
-#define WLC_GET_MACMODE             105
-#define WLC_SET_MACMODE             106
-#define WLC_GET_MONITOR             107
-#define WLC_SET_MONITOR             108
-#define WLC_GET_GMODE               109
-#define WLC_SET_GMODE               110
-#define WLC_GET_LEGACY_ERP          111
-#define WLC_SET_LEGACY_ERP          112
-#define WLC_GET_RX_ANT              113
-#define WLC_GET_CURR_RATESET            114 
-#define WLC_GET_SCANSUPPRESS            115
-#define WLC_SET_SCANSUPPRESS            116
-#define WLC_GET_AP              117
-#define WLC_SET_AP              118
-#define WLC_GET_EAP_RESTRICT            119
-#define WLC_SET_EAP_RESTRICT            120
-#define WLC_SCB_AUTHORIZE           121
-#define WLC_SCB_DEAUTHORIZE         122
-#define WLC_GET_WDSLIST             123
-#define WLC_SET_WDSLIST             124
-#define WLC_GET_ATIM                125
-#define WLC_SET_ATIM                126
-#define WLC_GET_RSSI                127
-#define WLC_GET_PHYANTDIV           128
-#define WLC_SET_PHYANTDIV           129
-#define WLC_AP_RX_ONLY              130
-#define WLC_GET_TX_PATH_PWR         131
-#define WLC_SET_TX_PATH_PWR         132
-#define WLC_GET_WSEC                133
-#define WLC_SET_WSEC                134
-#define WLC_GET_PHY_NOISE           135
-#define WLC_GET_BSS_INFO            136
-#define WLC_GET_PKTCNTS             137
-#define WLC_GET_LAZYWDS             138
-#define WLC_SET_LAZYWDS             139
-#define WLC_GET_BANDLIST            140
-#define WLC_GET_BAND                141
-#define WLC_SET_BAND                142
-#define WLC_SCB_DEAUTHENTICATE          143
-#define WLC_GET_SHORTSLOT           144
-#define WLC_GET_SHORTSLOT_OVERRIDE      145
-#define WLC_SET_SHORTSLOT_OVERRIDE      146
-#define WLC_GET_SHORTSLOT_RESTRICT      147
-#define WLC_SET_SHORTSLOT_RESTRICT      148
-#define WLC_GET_GMODE_PROTECTION        149
-#define WLC_GET_GMODE_PROTECTION_OVERRIDE   150
-#define WLC_SET_GMODE_PROTECTION_OVERRIDE   151
-#define WLC_UPGRADE             152
- 
- 
-#define WLC_GET_IGNORE_BCNS         155
-#define WLC_SET_IGNORE_BCNS         156
-#define WLC_GET_SCB_TIMEOUT         157
-#define WLC_SET_SCB_TIMEOUT         158
-#define WLC_GET_ASSOCLIST           159
-#define WLC_GET_CLK             160
-#define WLC_SET_CLK             161
-#define WLC_GET_UP              162
-#define WLC_OUT                 163
-#define WLC_GET_WPA_AUTH            164
-#define WLC_SET_WPA_AUTH            165
-#define WLC_GET_UCFLAGS             166
-#define WLC_SET_UCFLAGS             167
-#define WLC_GET_PWRIDX              168
-#define WLC_SET_PWRIDX              169
-#define WLC_GET_TSSI                170
-#define WLC_GET_SUP_RATESET_OVERRIDE        171
-#define WLC_SET_SUP_RATESET_OVERRIDE        172
- 
- 
- 
- 
- 
-#define WLC_GET_PROTECTION_CONTROL      178
-#define WLC_SET_PROTECTION_CONTROL      179
-#define WLC_GET_PHYLIST             180
-#define WLC_ENCRYPT_STRENGTH            181 
-#define WLC_DECRYPT_STATUS          182 
-#define WLC_GET_KEY_SEQ             183
-#define WLC_GET_SCAN_CHANNEL_TIME       184
-#define WLC_SET_SCAN_CHANNEL_TIME       185
-#define WLC_GET_SCAN_UNASSOC_TIME       186
-#define WLC_SET_SCAN_UNASSOC_TIME       187
-#define WLC_GET_SCAN_HOME_TIME          188
-#define WLC_SET_SCAN_HOME_TIME          189
-#define WLC_GET_SCAN_NPROBES            190
-#define WLC_SET_SCAN_NPROBES            191
-#define WLC_GET_PRB_RESP_TIMEOUT        192
-#define WLC_SET_PRB_RESP_TIMEOUT        193
-#define WLC_GET_ATTEN               194
-#define WLC_SET_ATTEN               195
-#define WLC_GET_SHMEM               196 
-#define WLC_SET_SHMEM               197 
- 
- 
-#define WLC_SET_WSEC_TEST           200
-#define WLC_SCB_DEAUTHENTICATE_FOR_REASON   201
-#define WLC_TKIP_COUNTERMEASURES        202
-#define WLC_GET_PIOMODE             203
-#define WLC_SET_PIOMODE             204
-#define WLC_SET_ASSOC_PREFER            205
-#define WLC_GET_ASSOC_PREFER            206
-#define WLC_SET_ROAM_PREFER         207
-#define WLC_GET_ROAM_PREFER         208
-#define WLC_SET_LED             209
-#define WLC_GET_LED             210
-#define WLC_GET_INTERFERENCE_MODE       211
-#define WLC_SET_INTERFERENCE_MODE       212
-#define WLC_GET_CHANNEL_QA          213
-#define WLC_START_CHANNEL_QA            214
-#define WLC_GET_CHANNEL_SEL         215
-#define WLC_START_CHANNEL_SEL           216
-#define WLC_GET_VALID_CHANNELS          217
-#define WLC_GET_FAKEFRAG            218
-#define WLC_SET_FAKEFRAG            219
-#define WLC_GET_PWROUT_PERCENTAGE       220
-#define WLC_SET_PWROUT_PERCENTAGE       221
-#define WLC_SET_BAD_FRAME_PREEMPT       222
-#define WLC_GET_BAD_FRAME_PREEMPT       223
-#define WLC_SET_LEAP_LIST           224
-#define WLC_GET_LEAP_LIST           225
-#define WLC_GET_CWMIN               226
-#define WLC_SET_CWMIN               227
-#define WLC_GET_CWMAX               228
-#define WLC_SET_CWMAX               229
-#define WLC_GET_WET             230
-#define WLC_SET_WET             231
-#define WLC_GET_PUB             232
- 
- 
-#define WLC_GET_KEY_PRIMARY         235
-#define WLC_SET_KEY_PRIMARY         236
- 
-#define WLC_GET_ACI_ARGS            238
-#define WLC_SET_ACI_ARGS            239
-#define WLC_UNSET_CALLBACK          240
-#define WLC_SET_CALLBACK            241
-#define WLC_GET_RADAR               242
-#define WLC_SET_RADAR               243
-#define WLC_SET_SPECT_MANAGMENT         244
-#define WLC_GET_SPECT_MANAGMENT         245
-#define WLC_WDS_GET_REMOTE_HWADDR       246 
-#define WLC_WDS_GET_WPA_SUP         247
-#define WLC_SET_CS_SCAN_TIMER           248
-#define WLC_GET_CS_SCAN_TIMER           249
-#define WLC_MEASURE_REQUEST         250
-#define WLC_INIT                251
-#define WLC_SEND_QUIET              252
-#define WLC_KEEPALIVE           253
-#define WLC_SEND_PWR_CONSTRAINT         254
-#define WLC_UPGRADE_STATUS          255
-#define WLC_CURRENT_PWR             256
-#define WLC_GET_SCAN_PASSIVE_TIME       257
-#define WLC_SET_SCAN_PASSIVE_TIME       258
-#define WLC_LEGACY_LINK_BEHAVIOR        259
-#define WLC_GET_CHANNELS_IN_COUNTRY     260
-#define WLC_GET_COUNTRY_LIST            261
-#define WLC_GET_VAR             262 
-#define WLC_SET_VAR             263 
-#define WLC_NVRAM_GET               264 
-#define WLC_NVRAM_SET               265
-#define WLC_NVRAM_DUMP              266
-#define WLC_REBOOT              267
-#define WLC_SET_WSEC_PMK            268
-#define WLC_GET_AUTH_MODE           269
-#define WLC_SET_AUTH_MODE           270
-#define WLC_GET_WAKEENTRY           271
-#define WLC_SET_WAKEENTRY           272
-#define WLC_NDCONFIG_ITEM           273 
-#define WLC_NVOTPW              274
-#define WLC_OTPW                275
-#define WLC_IOV_BLOCK_GET           276
-#define WLC_IOV_MODULES_GET         277
-#define WLC_SOFT_RESET              278
-#define WLC_GET_ALLOW_MODE          279
-#define WLC_SET_ALLOW_MODE          280
-#define WLC_GET_DESIRED_BSSID           281
-#define WLC_SET_DESIRED_BSSID           282
-#define WLC_DISASSOC_MYAP           283
-#define WLC_GET_NBANDS              284 
-#define WLC_GET_BANDSTATES          285 
-#define WLC_GET_WLC_BSS_INFO            286 
-#define WLC_GET_ASSOC_INFO          287 
-#define WLC_GET_OID_PHY             288 
-#define WLC_SET_OID_PHY             289 
-#define WLC_SET_ASSOC_TIME          290 
-#define WLC_GET_DESIRED_SSID            291 
-#define WLC_GET_CHANSPEC            292 
-#define WLC_GET_ASSOC_STATE         293 
-#define WLC_SET_PHY_STATE           294 
-#define WLC_GET_SCAN_PENDING            295 
-#define WLC_GET_SCANREQ_PENDING         296 
-#define WLC_GET_PREV_ROAM_REASON        297 
-#define WLC_SET_PREV_ROAM_REASON        298 
-#define WLC_GET_BANDSTATES_PI           299 
-#define WLC_GET_PHY_STATE           300 
-#define WLC_GET_BSS_WPA_RSN         301 
-#define WLC_GET_BSS_WPA2_RSN            302 
-#define WLC_GET_BSS_BCN_TS          303 
-#define WLC_GET_INT_DISASSOC            304 
-#define WLC_SET_NUM_PEERS           305     
-#define WLC_GET_NUM_BSS             306 
-#define WLC_NPHY_SAMPLE_COLLECT         307 
-#define WLC_UM_PRIV             308 
-#define WLC_GET_CMD             309
-  
-#define WLC_SET_INTERFERENCE_OVERRIDE_MODE  311 
-#define WLC_GET_INTERFERENCE_OVERRIDE_MODE  312 
-#define WLC_GET_WAI_RESTRICT            313 
-#define WLC_SET_WAI_RESTRICT            314 
-#define WLC_SET_WAI_REKEY           315 
-#define WLC_SET_PEAKRATE            316 
-#define WLC_GET_PEAKRATE            317
-#define WLC_LAST                318
-
-#ifndef EPICTRL_COOKIE
-#define EPICTRL_COOKIE      0xABADCEDE
-#endif
-
-
-#define CMN_IOCTL_OFF 0x180
-
-
-
-
-#define WL_OID_BASE     0xFFE41420
-
-
-#define OID_WL_GETINSTANCE  (WL_OID_BASE + WLC_GET_INSTANCE)
-#define OID_WL_GET_FORCELINK    (WL_OID_BASE + WLC_GET_FORCELINK)
-#define OID_WL_SET_FORCELINK    (WL_OID_BASE + WLC_SET_FORCELINK)
-#define OID_WL_ENCRYPT_STRENGTH (WL_OID_BASE + WLC_ENCRYPT_STRENGTH)
-#define OID_WL_DECRYPT_STATUS   (WL_OID_BASE + WLC_DECRYPT_STATUS)
-#define OID_LEGACY_LINK_BEHAVIOR (WL_OID_BASE + WLC_LEGACY_LINK_BEHAVIOR)
-#define OID_WL_NDCONFIG_ITEM    (WL_OID_BASE + WLC_NDCONFIG_ITEM)
-
-
-#define OID_STA_CHANSPEC    (WL_OID_BASE + WLC_GET_CHANSPEC)
-#define OID_STA_NBANDS      (WL_OID_BASE + WLC_GET_NBANDS)
-#define OID_STA_GET_PHY     (WL_OID_BASE + WLC_GET_OID_PHY)
-#define OID_STA_SET_PHY     (WL_OID_BASE + WLC_SET_OID_PHY)
-#define OID_STA_ASSOC_TIME  (WL_OID_BASE + WLC_SET_ASSOC_TIME)
-#define OID_STA_DESIRED_SSID    (WL_OID_BASE + WLC_GET_DESIRED_SSID)
-#define OID_STA_SET_PHY_STATE   (WL_OID_BASE + WLC_SET_PHY_STATE)
-#define OID_STA_SCAN_PENDING    (WL_OID_BASE + WLC_GET_SCAN_PENDING)
-#define OID_STA_SCANREQ_PENDING (WL_OID_BASE + WLC_GET_SCANREQ_PENDING)
-#define OID_STA_GET_ROAM_REASON (WL_OID_BASE + WLC_GET_PREV_ROAM_REASON)
-#define OID_STA_SET_ROAM_REASON (WL_OID_BASE + WLC_SET_PREV_ROAM_REASON)
-#define OID_STA_GET_PHY_STATE   (WL_OID_BASE + WLC_GET_PHY_STATE)
-#define OID_STA_INT_DISASSOC    (WL_OID_BASE + WLC_GET_INT_DISASSOC)
-#define OID_STA_SET_NUM_PEERS   (WL_OID_BASE + WLC_SET_NUM_PEERS)
-#define OID_STA_GET_NUM_BSS (WL_OID_BASE + WLC_GET_NUM_BSS)
-
-#define WL_DECRYPT_STATUS_SUCCESS   1
-#define WL_DECRYPT_STATUS_FAILURE   2
-#define WL_DECRYPT_STATUS_UNKNOWN   3
-
-
-#define WLC_UPGRADE_SUCCESS         0
-#define WLC_UPGRADE_PENDING         1
-
-#ifdef CONFIG_USBRNDIS_RETAIL
-
-typedef struct {
-	char *name;
-	void *param;
-} ndconfig_item_t;
-#endif
-
-
-
-#define WL_AUTH_OPEN_SYSTEM     0   
-#define WL_AUTH_SHARED_KEY      1   
-#define WL_AUTH_OPEN_SHARED     2   
-
-
-#define WL_RADIO_SW_DISABLE     (1<<0)
-#define WL_RADIO_HW_DISABLE     (1<<1)
-#define WL_RADIO_MPC_DISABLE        (1<<2)
-#define WL_RADIO_COUNTRY_DISABLE    (1<<3)  
-
-#define WL_SPURAVOID_OFF    0
-#define WL_SPURAVOID_ON1    1
-#define WL_SPURAVOID_ON2    2
-
-
-#define WL_TXPWR_OVERRIDE   (1U<<31)
-#define WL_TXPWR_NEG   (1U<<30)
-
-#define WL_PHY_PAVARS_LEN   6   
-
-#define WL_PHY_PAVARS2_NUM	3	
-#define WL_PHY_PAVAR_VER	1	
-typedef struct wl_pavars2 {
-	uint16 ver;		
-	uint16 len;		
-	uint16 inuse;		
-	uint16 phy_type;	
-	uint16 bandrange;
-	uint16 chain;
-	uint16 inpa[WL_PHY_PAVARS2_NUM];	
-} wl_pavars2_t;
-
-typedef struct wl_po {
-	uint16  phy_type;   
-	uint16  band;
-	uint16  cckpo;
-	uint32  ofdmpo;
-	uint16  mcspo[8];
-} wl_po_t;
-
-
-#define WLC_TXPWR_MAX       (127)   
-
-
-#define WL_DIAG_INTERRUPT           1   
-#define WL_DIAG_LOOPBACK            2   
-#define WL_DIAG_MEMORY              3   
-#define WL_DIAG_LED             4   
-#define WL_DIAG_REG             5   
-#define WL_DIAG_SROM                6   
-#define WL_DIAG_DMA             7   
-
-#define WL_DIAGERR_SUCCESS          0
-#define WL_DIAGERR_FAIL_TO_RUN          1   
-#define WL_DIAGERR_NOT_SUPPORTED        2   
-#define WL_DIAGERR_INTERRUPT_FAIL       3   
-#define WL_DIAGERR_LOOPBACK_FAIL        4   
-#define WL_DIAGERR_SROM_FAIL            5   
-#define WL_DIAGERR_SROM_BADCRC          6   
-#define WL_DIAGERR_REG_FAIL         7   
-#define WL_DIAGERR_MEMORY_FAIL          8   
-#define WL_DIAGERR_NOMEM            9   
-#define WL_DIAGERR_DMA_FAIL         10  
-
-#define WL_DIAGERR_MEMORY_TIMEOUT       11  
-#define WL_DIAGERR_MEMORY_BADPATTERN        12  
-
-
-#define WLC_BAND_AUTO       0   
-#define WLC_BAND_5G     1   
-#define WLC_BAND_2G     2   
-#define WLC_BAND_ALL        3   
-
-
-#define WL_CHAN_FREQ_RANGE_2G      0
-#define WL_CHAN_FREQ_RANGE_5GL     1
-#define WL_CHAN_FREQ_RANGE_5GM     2
-#define WL_CHAN_FREQ_RANGE_5GH     3
-
-#define WL_CHAN_FREQ_RANGE_5GLL_VER2    4
-#define WL_CHAN_FREQ_RANGE_5GLH_VER2    5
-#define WL_CHAN_FREQ_RANGE_5GML_VER2    6
-#define WL_CHAN_FREQ_RANGE_5GMH_VER2    7
-#define WL_CHAN_FREQ_RANGE_5GH_VER2     8
-
-#define WL_CHAN_FREQ_RANGE_5GLL_5BAND    4
-#define WL_CHAN_FREQ_RANGE_5GLH_5BAND    5
-#define WL_CHAN_FREQ_RANGE_5GML_5BAND    6
-#define WL_CHAN_FREQ_RANGE_5GMH_5BAND    7
-#define WL_CHAN_FREQ_RANGE_5GH_5BAND     8
-
-
-#define WLC_PHY_TYPE_A      0
-#define WLC_PHY_TYPE_B      1
-#define WLC_PHY_TYPE_G      2
-#define WLC_PHY_TYPE_N      4
-#define WLC_PHY_TYPE_LP     5
-#define WLC_PHY_TYPE_SSN    6
-#define WLC_PHY_TYPE_HT     7
-#define WLC_PHY_TYPE_LCN    8
-#define WLC_PHY_TYPE_NULL   0xf
-
-
-#define WLC_MACMODE_DISABLED    0   
-#define WLC_MACMODE_DENY    1   
-#define WLC_MACMODE_ALLOW   2   
-
-
-#define GMODE_LEGACY_B      0
-#define GMODE_AUTO      1
-#define GMODE_ONLY      2
-#define GMODE_B_DEFERRED    3
-#define GMODE_PERFORMANCE   4
-#define GMODE_LRS       5
-#define GMODE_MAX       6
-
-
-#define WLC_PLCP_AUTO   -1
-#define WLC_PLCP_SHORT  0
-#define WLC_PLCP_LONG   1
-
-
-#define WLC_PROTECTION_AUTO     -1
-#define WLC_PROTECTION_OFF      0
-#define WLC_PROTECTION_ON       1
-#define WLC_PROTECTION_MMHDR_ONLY   2
-#define WLC_PROTECTION_CTS_ONLY     3
-
-
-#define WLC_PROTECTION_CTL_OFF      0
-#define WLC_PROTECTION_CTL_LOCAL    1
-#define WLC_PROTECTION_CTL_OVERLAP  2
-
-
-#define WLC_N_PROTECTION_OFF        0
-#define WLC_N_PROTECTION_OPTIONAL   1
-#define WLC_N_PROTECTION_20IN40     2
-#define WLC_N_PROTECTION_MIXEDMODE  3
-
-
-#define WLC_N_PREAMBLE_MIXEDMODE    0
-#define WLC_N_PREAMBLE_GF       1
-#define WLC_N_PREAMBLE_GF_BRCM          2
-
-
-#define WLC_N_BW_20ALL          0
-#define WLC_N_BW_40ALL          1
-#define WLC_N_BW_20IN2G_40IN5G      2
-
-
-#define WLC_N_TXRX_CHAIN0       0
-#define WLC_N_TXRX_CHAIN1       1
-
-
-#define WLC_N_SGI_20            0x01
-#define WLC_N_SGI_40            0x02
-
-
-#define PM_OFF  0
-#define PM_MAX  1
-#define PM_FAST 2
-
-#define LISTEN_INTERVAL		10
-
-#define INTERFERE_OVRRIDE_OFF   -1  
-#define INTERFERE_NONE  0   
-#define NON_WLAN    1   
-#define WLAN_MANUAL 2   
-#define WLAN_AUTO   3   
-#define WLAN_AUTO_W_NOISE   4   
-#define AUTO_ACTIVE (1 << 7) 
-
-typedef struct wl_aci_args {
-	int enter_aci_thresh; 
-	int exit_aci_thresh; 
-	int usec_spin; 
-	int glitch_delay; 
-	uint16 nphy_adcpwr_enter_thresh;    
-	uint16 nphy_adcpwr_exit_thresh; 
-	uint16 nphy_repeat_ctr;     
-	uint16 nphy_num_samples;    
-	uint16 nphy_undetect_window_sz; 
-	uint16 nphy_b_energy_lo_aci;    
-	uint16 nphy_b_energy_md_aci;    
-	uint16 nphy_b_energy_hi_aci;    
-	uint16 nphy_noise_noassoc_glitch_th_up; 
-	uint16 nphy_noise_noassoc_glitch_th_dn;
-	uint16 nphy_noise_assoc_glitch_th_up;
-	uint16 nphy_noise_assoc_glitch_th_dn;
-	uint16 nphy_noise_assoc_aci_glitch_th_up;
-	uint16 nphy_noise_assoc_aci_glitch_th_dn;
-	uint16 nphy_noise_assoc_enter_th;
-	uint16 nphy_noise_noassoc_enter_th;
-	uint16 nphy_noise_assoc_rx_glitch_badplcp_enter_th;
-	uint16 nphy_noise_noassoc_crsidx_incr;
-	uint16 nphy_noise_assoc_crsidx_incr;
-	uint16 nphy_noise_crsidx_decr;
-} wl_aci_args_t;
-
-#define TRIGGER_NOW             0
-#define TRIGGER_CRS             0x01
-#define TRIGGER_CRSDEASSERT         0x02
-#define TRIGGER_GOODFCS             0x04
-#define TRIGGER_BADFCS              0x08
-#define TRIGGER_BADPLCP             0x10
-#define TRIGGER_CRSGLITCH           0x20
-#define WL_ACI_ARGS_LEGACY_LENGTH   16  
-#define WL_SAMPLECOLLECT_T_VERSION  1   
-typedef struct wl_samplecollect_args {
-	
-	uint8 coll_us;
-	int cores;
-	
-	uint16 version;     
-	uint16 length;      
-	uint8 trigger;
-	uint16 timeout;
-	uint16 mode;
-	uint32 pre_dur;
-	uint32 post_dur;
-	uint8 gpio_sel;
-	bool downsamp;
-	bool be_deaf;
-	bool agc;       
-	bool filter;        
-} wl_samplecollect_args_t;
-
-#define WL_SAMPLEDATA_HEADER_TYPE   1
-#define WL_SAMPLEDATA_HEADER_SIZE   80  
-#define WL_SAMPLEDATA_TYPE      2
-#define WL_SAMPLEDATA_SEQ       0xff    
-#define WL_SAMPLEDATA_MORE_DATA     0x100   
-#define WL_SAMPLEDATA_T_VERSION     1   
-
-#define WL_SAMPLEDATA_T_VERSION_SPEC_AN 2
-
-typedef struct wl_sampledata {
-	uint16 version; 
-	uint16 size;    
-	uint16 tag; 
-	uint16 length;  
-	uint32 flag;    
-} wl_sampledata_t;
-
-
-
-#define WL_ERROR_VAL        0x00000001
-#define WL_TRACE_VAL        0x00000002
-#define WL_PRHDRS_VAL       0x00000004
-#define WL_PRPKT_VAL        0x00000008
-#define WL_INFORM_VAL       0x00000010
-#define WL_TMP_VAL      0x00000020
-#define WL_OID_VAL      0x00000040
-#define WL_RATE_VAL     0x00000080
-#define WL_ASSOC_VAL        0x00000100
-#define WL_PRUSR_VAL        0x00000200
-#define WL_PS_VAL       0x00000400
-#define WL_TXPWR_VAL        0x00000800  
-#define WL_PORT_VAL     0x00001000
-#define WL_DUAL_VAL     0x00002000
-#define WL_WSEC_VAL     0x00004000
-#define WL_WSEC_DUMP_VAL    0x00008000
-#define WL_LOG_VAL      0x00010000
-#define WL_NRSSI_VAL        0x00020000  
-#define WL_LOFT_VAL     0x00040000  
-#define WL_REGULATORY_VAL   0x00080000
-#define WL_PHYCAL_VAL       0x00100000  
-#define WL_RADAR_VAL        0x00200000  
-#define WL_MPC_VAL      0x00400000
-#define WL_APSTA_VAL        0x00800000
-#define WL_DFS_VAL      0x01000000
-#define WL_BA_VAL       0x02000000
-#define WL_ACI_VAL      0x04000000
-#define WL_MBSS_VAL     0x04000000
-#define WL_CAC_VAL      0x08000000
-#define WL_AMSDU_VAL        0x10000000
-#define WL_AMPDU_VAL        0x20000000
-#define WL_FFPLD_VAL        0x40000000
-
-
-#define WL_DPT_VAL      0x00000001
-#define WL_SCAN_VAL     0x00000002
-#define WL_WOWL_VAL     0x00000004
-#define WL_COEX_VAL     0x00000008
-#define WL_RTDC_VAL     0x00000010
-#define WL_PROTO_VAL        0x00000020
-#define WL_BTA_VAL      0x00000040
-#define WL_CHANINT_VAL      0x00000080
-#define WL_THERMAL_VAL      0x00000100  
-#define WL_P2P_VAL      0x00000200
-#define WL_TXRX_VAL		0x00000400
-#define WL_MCHAN_VAL            0x00000800
-
-
-#define WL_LED_NUMGPIO      16  
-
-
-#define WL_LED_OFF      0       
-#define WL_LED_ON       1       
-#define WL_LED_ACTIVITY     2       
-#define WL_LED_RADIO        3       
-#define WL_LED_ARADIO       4       
-#define WL_LED_BRADIO       5       
-#define WL_LED_BGMODE       6       
-#define WL_LED_WI1      7
-#define WL_LED_WI2      8
-#define WL_LED_WI3      9
-#define WL_LED_ASSOC        10      
-#define WL_LED_INACTIVE     11      
-#define WL_LED_ASSOCACT     12      
-#define WL_LED_WI4      13
-#define WL_LED_WI5      14
-#define WL_LED_BLINKSLOW    15      
-#define WL_LED_BLINKMED     16      
-#define WL_LED_BLINKFAST    17      
-#define WL_LED_BLINKCUSTOM  18      
-#define WL_LED_BLINKPERIODIC    19      
-#define WL_LED_ASSOC_WITH_SEC   20      
-						
-#define WL_LED_START_OFF    21      
-#define WL_LED_NUMBEHAVIOR  22
-
-
-#define WL_LED_BEH_MASK     0x7f        
-#define WL_LED_AL_MASK      0x80        
-
-
-#define WL_NUMCHANNELS      64
-#define WL_NUMCHANSPECS     100
-
-
-#define WL_WDS_WPA_ROLE_AUTH    0   
-#define WL_WDS_WPA_ROLE_SUP 1   
-#define WL_WDS_WPA_ROLE_AUTO    255 
-
-
-#define WL_EVENTING_MASK_LEN    16
-
-
-
-
-#define WL_JOIN_PREF_RSSI   1   
-#define WL_JOIN_PREF_WPA    2   
-#define WL_JOIN_PREF_BAND   3   
-#define WL_JOIN_PREF_RSSI_DELTA 4   
-#define WL_JOIN_PREF_TRANS_PREF	5
-
-
-#define WLJP_BAND_ASSOC_PREF    255 
-
-
-#define WL_WPA_ACP_MCS_ANY  "\x00\x00\x00\x00"
-
-struct tsinfo_arg {
-	uint8 octets[3];
-};
-
-#define NFIFO           6   
-
-#define WL_CNT_T_VERSION    6   
-
-typedef struct {
-	uint16  version;    
-	uint16  length;     
-
-	
-	uint32  txframe;    
-	uint32  txbyte;     
-	uint32  txretrans;  
-	uint32  txerror;    
-	uint32  txctl;      
-	uint32  txprshort;  
-	uint32  txserr;     
-	uint32  txnobuf;    
-	uint32  txnoassoc;  
-	uint32  txrunt;     
-	uint32  txchit;     
-	uint32  txcmiss;    
-
-	
-	uint32  txuflo;     
-	uint32  txphyerr;   
-	uint32  txphycrs;
-
-	
-	uint32  rxframe;    
-	uint32  rxbyte;     
-	uint32  rxerror;    
-	uint32  rxctl;      
-	uint32  rxnobuf;    
-	uint32  rxnondata;  
-	uint32  rxbadds;    
-	uint32  rxbadcm;    
-	uint32  rxfragerr;  
-	uint32  rxrunt;     
-	uint32  rxgiant;    
-	uint32  rxnoscb;    
-	uint32  rxbadproto; 
-	uint32  rxbadsrcmac;    
-	uint32  rxbadda;    
-	uint32  rxfilter;   
-
-	
-	uint32  rxoflo;     
-	uint32  rxuflo[NFIFO];  
-
-	uint32  d11cnt_txrts_off;   
-	uint32  d11cnt_rxcrc_off;   
-	uint32  d11cnt_txnocts_off; 
-
-	
-	uint32  dmade;      
-	uint32  dmada;      
-	uint32  dmape;      
-	uint32  reset;      
-	uint32  tbtt;       
-	uint32  txdmawar;
-	uint32  pkt_callback_reg_fail;  
-
-	
-	uint32  txallfrm;   
-	uint32  txrtsfrm;   
-	uint32  txctsfrm;   
-	uint32  txackfrm;   
-	uint32  txdnlfrm;   
-	uint32  txbcnfrm;   
-	uint32  txfunfl[8]; 
-	uint32  txtplunfl;  
-	uint32  txphyerror; 
-	uint32  rxfrmtoolong;   
-	uint32  rxfrmtooshrt;   
-	uint32  rxinvmachdr;    
-	uint32  rxbadfcs;   
-	uint32  rxbadplcp;  
-	uint32  rxcrsglitch;    
-	uint32  rxstrt;     
-	uint32  rxdfrmucastmbss; 
-	uint32  rxmfrmucastmbss; 
-	uint32  rxcfrmucast;    
-	uint32  rxrtsucast; 
-	uint32  rxctsucast; 
-	uint32  rxackucast; 
-	uint32  rxdfrmocast;    
-	uint32  rxmfrmocast;    
-	uint32  rxcfrmocast;    
-	uint32  rxrtsocast; 
-	uint32  rxctsocast; 
-	uint32  rxdfrmmcast;    
-	uint32  rxmfrmmcast;    
-	uint32  rxcfrmmcast;    
-	uint32  rxbeaconmbss;   
-	uint32  rxdfrmucastobss; 
-	uint32  rxbeaconobss;   
-	uint32  rxrsptmout; 
-	uint32  bcntxcancl; 
-	uint32  rxf0ovfl;   
-	uint32  rxf1ovfl;   
-	uint32  rxf2ovfl;   
-	uint32  txsfovfl;   
-	uint32  pmqovfl;    
-	uint32  rxcgprqfrm; 
-	uint32  rxcgprsqovfl;   
-	uint32  txcgprsfail;    
-	uint32  txcgprssuc; 
-	uint32  prs_timeout;    
-	uint32  rxnack;
-	uint32  frmscons;
-	uint32  txnack;
-	uint32  txglitch_nack;  
-	uint32  txburst;    
-
-	
-	uint32  txfrag;     
-	uint32  txmulti;    
-	uint32  txfail;     
-	uint32  txretry;    
-	uint32  txretrie;   
-	uint32  rxdup;      
-	uint32  txrts;      
-	uint32  txnocts;    
-	uint32  txnoack;    
-	uint32  rxfrag;     
-	uint32  rxmulti;    
-	uint32  rxcrc;      
-	uint32  txfrmsnt;   
-	uint32  rxundec;    
-
-	
-	uint32  tkipmicfaill;   
-	uint32  tkipcntrmsr;    
-	uint32  tkipreplay; 
-	uint32  ccmpfmterr; 
-	uint32  ccmpreplay; 
-	uint32  ccmpundec;  
-	uint32  fourwayfail;    
-	uint32  wepundec;   
-	uint32  wepicverr;  
-	uint32  decsuccess; 
-	uint32  tkipicverr; 
-	uint32  wepexcluded;    
-
-	uint32  rxundec_mcst;   
-
-	
-	uint32  tkipmicfaill_mcst;  
-	uint32  tkipcntrmsr_mcst;   
-	uint32  tkipreplay_mcst;    
-	uint32  ccmpfmterr_mcst;    
-	uint32  ccmpreplay_mcst;    
-	uint32  ccmpundec_mcst; 
-	uint32  fourwayfail_mcst;   
-	uint32  wepundec_mcst;  
-	uint32  wepicverr_mcst; 
-	uint32  decsuccess_mcst;    
-	uint32  tkipicverr_mcst;    
-	uint32  wepexcluded_mcst;   
-
-	uint32  txchanrej;  
-	uint32  txexptime;  
-	uint32  psmwds;     
-	uint32  phywatchdog;    
-
-	
-	uint32  prq_entries_handled;    
-	uint32  prq_undirected_entries; 
-	uint32  prq_bad_entries;    
-	uint32  atim_suppress_count;    
-	uint32  bcn_template_not_ready; 
-	uint32  bcn_template_not_ready_done; 
-	uint32  late_tbtt_dpc;  
-
-	
-	uint32  rx1mbps;    
-	uint32  rx2mbps;    
-	uint32  rx5mbps5;   
-	uint32  rx6mbps;    
-	uint32  rx9mbps;    
-	uint32  rx11mbps;   
-	uint32  rx12mbps;   
-	uint32  rx18mbps;   
-	uint32  rx24mbps;   
-	uint32  rx36mbps;   
-	uint32  rx48mbps;   
-	uint32  rx54mbps;   
-	uint32  rx108mbps;  
-	uint32  rx162mbps;  
-	uint32  rx216mbps;  
-	uint32  rx270mbps;  
-	uint32  rx324mbps;  
-	uint32  rx378mbps;  
-	uint32  rx432mbps;  
-	uint32  rx486mbps;  
-	uint32  rx540mbps;  
-
-	
-	uint32  pktengrxducast; 
-	uint32  pktengrxdmcast; 
-
-	uint32  rfdisable;  
-	uint32  bphy_rxcrsglitch;   
-
-	uint32  txmpdu_sgi; 
-	uint32  rxmpdu_sgi; 
-	uint32  txmpdu_stbc;    
-	uint32  rxmpdu_stbc;    
-} wl_cnt_t;
-
-
-#define WL_WME_CNT_VERSION  1   
-
-typedef struct {
-	uint32 packets;
-	uint32 bytes;
-} wl_traffic_stats_t;
-
-typedef struct {
-	uint16  version;    
-	uint16  length;     
-
-	wl_traffic_stats_t tx[AC_COUNT];    
-	wl_traffic_stats_t tx_failed[AC_COUNT]; 
-	wl_traffic_stats_t rx[AC_COUNT];    
-	wl_traffic_stats_t rx_failed[AC_COUNT]; 
-
-	wl_traffic_stats_t forward[AC_COUNT];   
-
-	wl_traffic_stats_t tx_expired[AC_COUNT];    
-
-} wl_wme_cnt_t;
-
-struct wl_msglevel2 {
-	uint32 low;
-	uint32 high;
-};
-
-typedef struct wl_mkeep_alive_pkt {
-	uint16	version;
-	uint16	length;
-	uint32	period_msec;
-	uint16	len_bytes;
-	uint8	keep_alive_id;
-	uint8	data[1];
-} wl_mkeep_alive_pkt_t;
-
-#define WL_MKEEP_ALIVE_VERSION          1
-#define WL_MKEEP_ALIVE_FIXED_LEN        OFFSETOF(wl_mkeep_alive_pkt_t, data)
-#define WL_MKEEP_ALIVE_PRECISION        500
-
-#define WLC_ROAM_TRIGGER_DEFAULT    0 
-#define WLC_ROAM_TRIGGER_BANDWIDTH  1 
-#define WLC_ROAM_TRIGGER_DISTANCE   2 
-#define WLC_ROAM_TRIGGER_AUTO       3 
-#define WLC_ROAM_TRIGGER_MAX_VALUE  3 
-
-
-#define WPA_AUTH_PFN_ANY	0xffffffff	
-
-enum {
-	PFN_LIST_ORDER,
-	PFN_RSSI
-};
-
-enum {
-	DISABLE,
-	ENABLE
-};
-
-enum {
-	OFF_ADAPT,
-	SMART_ADAPT,
-	STRICT_ADAPT,
-	SLOW_ADAPT
-};
-
-#define SORT_CRITERIA_BIT		0
-#define AUTO_NET_SWITCH_BIT		1
-#define ENABLE_BKGRD_SCAN_BIT	2
-#define IMMEDIATE_SCAN_BIT		3
-#define	AUTO_CONNECT_BIT		4
-#define	ENABLE_BD_SCAN_BIT		5
-#define ENABLE_ADAPTSCAN_BIT	6
-#define IMMEDIATE_EVENT_BIT		8
-
-#define SORT_CRITERIA_MASK		0x0001
-#define AUTO_NET_SWITCH_MASK	0x0002
-#define ENABLE_BKGRD_SCAN_MASK	0x0004
-#define IMMEDIATE_SCAN_MASK		0x0008
-#define	AUTO_CONNECT_MASK		0x0010
-#define ENABLE_BD_SCAN_MASK		0x0020
-#define ENABLE_ADAPTSCAN_MASK	0x00c0
-#define IMMEDIATE_EVENT_MASK	0x0100
-
-#define PFN_VERSION				2
-#define PFN_SCANRESULT_VERSION	1
-#define MAX_PFN_LIST_COUNT	16
-
-#define PFN_COMPLETE			1
-#define PFN_INCOMPLETE			0
-
-#define DEFAULT_BESTN			2
-#define DEFAULT_MSCAN			0
-#define DEFAULT_REPEAT			10
-#define DEFAULT_EXP				2
-
-
-typedef struct wl_pfn_subnet_info {
-	struct ether_addr BSSID;
-	uint8	channel; 
-	uint8	SSID_len;
-	uint8	SSID[32];
-} wl_pfn_subnet_info_t;
-
-typedef struct wl_pfn_net_info {
-	wl_pfn_subnet_info_t pfnsubnet;
-	int16	RSSI; 
-	uint16	timestamp; 
-} wl_pfn_net_info_t;
-
-typedef struct wl_pfn_scanresults {
-	uint32 version;
-	uint32 status;
-	uint32 count;
-	wl_pfn_net_info_t netinfo[1];
-} wl_pfn_scanresults_t;
-
-
-typedef struct wl_pfn_param {
-	int32 version;			
-	int32 scan_freq;		
-	int32 lost_network_timeout;	
-	int16 flags;			
-	int16 rssi_margin;		
-	uint8 bestn; 
-	uint8 mscan; 
-	uint8 repeat; 
-	uint8 exp; 
-	int32 slow_freq;
-} wl_pfn_param_t;
-
-typedef struct wl_pfn_bssid {
-	struct ether_addr 	macaddr;
-	
-	uint16				flags;
-} wl_pfn_bssid_t;
-#define WL_PFN_SUPPRESSFOUND_MASK	0x08
-#define WL_PFN_SUPPRESSLOST_MASK	0x10
-
-typedef struct wl_pfn_cfg {
-	uint32				reporttype;
-	int32				channel_num;
-	uint16				channel_list[WL_NUMCHANNELS];
-} wl_pfn_cfg_t;
-#define WL_PFN_REPORT_ALLNET 	0
-#define WL_PFN_REPORT_SSIDNET 	1
-#define WL_PFN_REPORT_BSSIDNET 	2
-
-typedef struct wl_pfn {
-	wlc_ssid_t		ssid;			
-	int32			flags;			
-	int32			infra;			
-	int32			auth;			
-	int32			wpa_auth;		
-	int32			wsec;			
-} wl_pfn_t;
-#define WL_PFN_HIDDEN_BIT		2
-#define PNO_SCAN_MAX_FW		508*1000	
-#define PNO_SCAN_MAX_FW_SEC	PNO_SCAN_MAX_FW/1000 
-#define PNO_SCAN_MIN_FW_SEC	10			
-#define WL_PFN_HIDDEN_MASK		0x4
-
-
-#define TOE_TX_CSUM_OL      0x00000001
-#define TOE_RX_CSUM_OL      0x00000002
-
-
-#define TOE_ERRTEST_TX_CSUM 0x00000001
-#define TOE_ERRTEST_RX_CSUM 0x00000002
-#define TOE_ERRTEST_RX_CSUM2    0x00000004
-
-struct toe_ol_stats_t {
-	
-	uint32 tx_summed;
-
-	
-	uint32 tx_iph_fill;
-	uint32 tx_tcp_fill;
-	uint32 tx_udp_fill;
-	uint32 tx_icmp_fill;
-
-	
-	uint32 rx_iph_good;
-	uint32 rx_iph_bad;
-	uint32 rx_tcp_good;
-	uint32 rx_tcp_bad;
-	uint32 rx_udp_good;
-	uint32 rx_udp_bad;
-	uint32 rx_icmp_good;
-	uint32 rx_icmp_bad;
-
-	
-	uint32 tx_tcp_errinj;
-	uint32 tx_udp_errinj;
-	uint32 tx_icmp_errinj;
-
-	
-	uint32 rx_tcp_errinj;
-	uint32 rx_udp_errinj;
-	uint32 rx_icmp_errinj;
-};
-
-
-#define ARP_OL_AGENT        0x00000001
-#define ARP_OL_SNOOP        0x00000002
-#define ARP_OL_HOST_AUTO_REPLY  0x00000004
-#define ARP_OL_PEER_AUTO_REPLY  0x00000008
-
-
-#define ARP_ERRTEST_REPLY_PEER  0x1
-#define ARP_ERRTEST_REPLY_HOST  0x2
-
-#define ARP_MULTIHOMING_MAX 8   
-
-
-struct arp_ol_stats_t {
-	uint32  host_ip_entries;    
-	uint32  host_ip_overflow;   
-
-	uint32  arp_table_entries;  
-	uint32  arp_table_overflow; 
-
-	uint32  host_request;       
-	uint32  host_reply;     
-	uint32  host_service;       
-
-	uint32  peer_request;       
-	uint32  peer_request_drop;  
-	uint32  peer_reply;     
-	uint32  peer_reply_drop;    
-	uint32  peer_service;       
-};
-
-
-
-
-typedef struct wl_keep_alive_pkt {
-	uint32  period_msec;    
-	uint16  len_bytes;  
-	uint8   data[1];    
-} wl_keep_alive_pkt_t;
-
-#define WL_KEEP_ALIVE_FIXED_LEN     OFFSETOF(wl_keep_alive_pkt_t, data)
-
-
-
-
-typedef enum wl_pkt_filter_type {
-	WL_PKT_FILTER_TYPE_PATTERN_MATCH    
-} wl_pkt_filter_type_t;
-
-#define WL_PKT_FILTER_TYPE wl_pkt_filter_type_t
-
-
-typedef struct wl_pkt_filter_pattern {
-	uint32  offset;     
-	uint32  size_bytes; 
-	uint8   mask_and_pattern[1]; 
-} wl_pkt_filter_pattern_t;
-
-
-typedef struct wl_pkt_filter {
-	uint32  id;     
-	uint32  type;       
-	uint32  negate_match;   
-	union {         
-		wl_pkt_filter_pattern_t pattern;    
-	} u;
-} wl_pkt_filter_t;
-
-#define WL_PKT_FILTER_FIXED_LEN       OFFSETOF(wl_pkt_filter_t, u)
-#define WL_PKT_FILTER_PATTERN_FIXED_LEN   OFFSETOF(wl_pkt_filter_pattern_t, mask_and_pattern)
-
-
-typedef struct wl_pkt_filter_enable {
-	uint32  id;     
-	uint32  enable;     
-} wl_pkt_filter_enable_t;
-
-
-typedef struct wl_pkt_filter_list {
-	uint32  num;        
-	wl_pkt_filter_t filter[1];  
-} wl_pkt_filter_list_t;
-
-#define WL_PKT_FILTER_LIST_FIXED_LEN      OFFSETOF(wl_pkt_filter_list_t, filter)
-
-
-typedef struct wl_pkt_filter_stats {
-	uint32  num_pkts_matched;   
-	uint32  num_pkts_forwarded; 
-	uint32  num_pkts_discarded; 
-} wl_pkt_filter_stats_t;
-
-
-typedef struct wl_seq_cmd_ioctl {
-	uint32 cmd;     
-	uint32 len;     
-} wl_seq_cmd_ioctl_t;
-
-#define WL_SEQ_CMD_ALIGN_BYTES  4
-
-
-#define WL_SEQ_CMDS_GET_IOCTL_FILTER(cmd) \
-	(((cmd) == WLC_GET_MAGIC) || \
-	 ((cmd) == WLC_GET_VERSION) || \
-	 ((cmd) == WLC_GET_AP) || \
-	 ((cmd) == WLC_GET_INSTANCE))
-
-
-
-#define WL_PKTENG_PER_TX_START          0x01
-#define WL_PKTENG_PER_TX_STOP           0x02
-#define WL_PKTENG_PER_RX_START          0x04
-#define WL_PKTENG_PER_RX_WITH_ACK_START     0x05
-#define WL_PKTENG_PER_TX_WITH_ACK_START     0x06
-#define WL_PKTENG_PER_RX_STOP           0x08
-#define WL_PKTENG_PER_MASK          0xff
-
-#define WL_PKTENG_SYNCHRONOUS           0x100   
-
-typedef struct wl_pkteng {
-	uint32 flags;
-	uint32 delay;           
-	uint32 nframes;         
-	uint32 length;          
-	uint8  seqno;           
-	struct ether_addr dest;     
-	struct ether_addr src;      
-} wl_pkteng_t;
-
-#define NUM_80211b_RATES    4
-#define NUM_80211ag_RATES   8
-#define NUM_80211n_RATES    32
-#define NUM_80211_RATES     (NUM_80211b_RATES+NUM_80211ag_RATES+NUM_80211n_RATES)
-typedef struct wl_pkteng_stats {
-	uint32 lostfrmcnt;      
-	int32 rssi;         
-	int32 snr;          
-	uint16 rxpktcnt[NUM_80211_RATES+1];
-} wl_pkteng_stats_t;
-
-
-#define WL_WOWL_MAGIC   (1 << 0)    
-#define WL_WOWL_NET (1 << 1)    
-#define WL_WOWL_DIS (1 << 2)    
-#define WL_WOWL_RETR    (1 << 3)    
-#define WL_WOWL_BCN (1 << 4)    
-#define WL_WOWL_TST (1 << 5)    
-#define WL_WOWL_M1      (1 << 6)        
-#define WL_WOWL_EAPID   (1 << 7)        
-#define WL_WOWL_KEYROT  (1 << 14)       
-#define WL_WOWL_BCAST   (1 << 15)   
-
-#define MAGIC_PKT_MINLEN 102    
-
-typedef struct {
-	uint masksize;      
-	uint offset;        
-	uint patternoffset; 
-	uint patternsize;   
-	ulong id;       
-	
-	
-} wl_wowl_pattern_t;
-
-typedef struct {
-	uint            count;
-	wl_wowl_pattern_t   pattern[1];
-} wl_wowl_pattern_list_t;
-
-typedef struct {
-	uint8   pci_wakeind;    
-	uint16  ucode_wakeind;  
-} wl_wowl_wakeind_t;
-
-
-typedef struct wl_txrate_class {
-	uint8       init_rate;
-	uint8       min_rate;
-	uint8       max_rate;
-} wl_txrate_class_t;
-
-
-
-
-#define WLC_OBSS_SCAN_PASSIVE_DWELL_DEFAULT     20  
-#define WLC_OBSS_SCAN_PASSIVE_DWELL_MIN         5   
-#define WLC_OBSS_SCAN_PASSIVE_DWELL_MAX         1000    
-#define WLC_OBSS_SCAN_ACTIVE_DWELL_DEFAULT      10  
-#define WLC_OBSS_SCAN_ACTIVE_DWELL_MIN          10  
-#define WLC_OBSS_SCAN_ACTIVE_DWELL_MAX          1000    
-#define WLC_OBSS_SCAN_WIDTHSCAN_INTERVAL_DEFAULT    300 
-#define WLC_OBSS_SCAN_WIDTHSCAN_INTERVAL_MIN        10  
-#define WLC_OBSS_SCAN_WIDTHSCAN_INTERVAL_MAX        900 
-#define WLC_OBSS_SCAN_CHANWIDTH_TRANSITION_DLY_DEFAULT  5
-#define WLC_OBSS_SCAN_CHANWIDTH_TRANSITION_DLY_MIN  5
-#define WLC_OBSS_SCAN_CHANWIDTH_TRANSITION_DLY_MAX  100
-#define WLC_OBSS_SCAN_PASSIVE_TOTAL_PER_CHANNEL_DEFAULT 200 
-#define WLC_OBSS_SCAN_PASSIVE_TOTAL_PER_CHANNEL_MIN 200 
-#define WLC_OBSS_SCAN_PASSIVE_TOTAL_PER_CHANNEL_MAX 10000   
-#define WLC_OBSS_SCAN_ACTIVE_TOTAL_PER_CHANNEL_DEFAULT  20  
-#define WLC_OBSS_SCAN_ACTIVE_TOTAL_PER_CHANNEL_MIN  20  
-#define WLC_OBSS_SCAN_ACTIVE_TOTAL_PER_CHANNEL_MAX  10000   
-#define WLC_OBSS_SCAN_ACTIVITY_THRESHOLD_DEFAULT    25  
-#define WLC_OBSS_SCAN_ACTIVITY_THRESHOLD_MIN        0   
-#define WLC_OBSS_SCAN_ACTIVITY_THRESHOLD_MAX        100 
-
-
-typedef struct wl_obss_scan_arg {
-	int16   passive_dwell;
-	int16   active_dwell;
-	int16   bss_widthscan_interval;
-	int16   passive_total;
-	int16   active_total;
-	int16   chanwidth_transition_delay;
-	int16   activity_threshold;
-} wl_obss_scan_arg_t;
-
-#define WL_OBSS_SCAN_PARAM_LEN  sizeof(wl_obss_scan_arg_t)
-#define WL_MIN_NUM_OBSS_SCAN_ARG 7  
-
-#define WL_COEX_INFO_MASK       0x07
-#define WL_COEX_INFO_REQ        0x01
-#define WL_COEX_40MHZ_INTOLERANT    0x02
-#define WL_COEX_WIDTH20         0x04
-
-#define WLC_RSSI_INVALID     0  
-
-#define MAX_RSSI_LEVELS 8
-
-
-typedef struct wl_rssi_event {
-	uint32 rate_limit_msec;     
-	uint8 num_rssi_levels;      
-	int8 rssi_levels[MAX_RSSI_LEVELS];  
-} wl_rssi_event_t;
-
-typedef struct wl_action_obss_coex_req {
-	uint8 info;
-	uint8 num;
-	uint8 ch_list[1];
-} wl_action_obss_coex_req_t;
-
-
-#define EXTLOG_CUR_VER      0x0100
-
-#define MAX_ARGSTR_LEN      18 
-
-
-#define LOG_MODULE_COMMON   0x0001
-#define LOG_MODULE_ASSOC    0x0002
-#define LOG_MODULE_EVENT    0x0004
-#define LOG_MODULE_MAX      3           
-
-
-#define WL_LOG_LEVEL_DISABLE    0
-#define WL_LOG_LEVEL_ERR    1
-#define WL_LOG_LEVEL_WARN   2
-#define WL_LOG_LEVEL_INFO   3
-#define WL_LOG_LEVEL_MAX    WL_LOG_LEVEL_INFO   
-
-
-#define LOG_FLAG_EVENT      1
-
-
-#define LOG_ARGTYPE_NULL    0
-#define LOG_ARGTYPE_STR     1   
-#define LOG_ARGTYPE_INT     2   
-#define LOG_ARGTYPE_INT_STR 3   
-#define LOG_ARGTYPE_STR_INT 4   
-
-typedef struct wlc_extlog_cfg {
-	int max_number;
-	uint16 module;  
-	uint8 level;
-	uint8 flag;
-	uint16 version;
-} wlc_extlog_cfg_t;
-
-typedef struct log_record {
-	uint32 time;
-	uint16 module;
-	uint16 id;
-	uint8 level;
-	uint8 sub_unit;
-	uint8 seq_num;
-	int32 arg;
-	char str[MAX_ARGSTR_LEN];
-} log_record_t;
-
-typedef struct wlc_extlog_req {
-	uint32 from_last;
-	uint32 num;
-} wlc_extlog_req_t;
-
-typedef struct wlc_extlog_results {
-	uint16 version;
-	uint16 record_len;
-	uint32 num;
-	log_record_t logs[1];
-} wlc_extlog_results_t;
-
-typedef struct log_idstr {
-	uint16  id;
-	uint16  flag;
-	uint8   arg_type;
-	const char  *fmt_str;
-} log_idstr_t;
-
-#define FMTSTRF_USER        1
-
-
-typedef enum {
-	FMTSTR_DRIVER_UP_ID = 0,
-	FMTSTR_DRIVER_DOWN_ID = 1,
-	FMTSTR_SUSPEND_MAC_FAIL_ID = 2,
-	FMTSTR_NO_PROGRESS_ID = 3,
-	FMTSTR_RFDISABLE_ID = 4,
-	FMTSTR_REG_PRINT_ID = 5,
-	FMTSTR_EXPTIME_ID = 6,
-	FMTSTR_JOIN_START_ID = 7,
-	FMTSTR_JOIN_COMPLETE_ID = 8,
-	FMTSTR_NO_NETWORKS_ID = 9,
-	FMTSTR_SECURITY_MISMATCH_ID = 10,
-	FMTSTR_RATE_MISMATCH_ID = 11,
-	FMTSTR_AP_PRUNED_ID = 12,
-	FMTSTR_KEY_INSERTED_ID = 13,
-	FMTSTR_DEAUTH_ID = 14,
-	FMTSTR_DISASSOC_ID = 15,
-	FMTSTR_LINK_UP_ID = 16,
-	FMTSTR_LINK_DOWN_ID = 17,
-	FMTSTR_RADIO_HW_OFF_ID = 18,
-	FMTSTR_RADIO_HW_ON_ID = 19,
-	FMTSTR_EVENT_DESC_ID = 20,
-	FMTSTR_PNP_SET_POWER_ID = 21,
-	FMTSTR_RADIO_SW_OFF_ID = 22,
-	FMTSTR_RADIO_SW_ON_ID = 23,
-	FMTSTR_PWD_MISMATCH_ID = 24,
-	FMTSTR_FATAL_ERROR_ID = 25,
-	FMTSTR_AUTH_FAIL_ID = 26,
-	FMTSTR_ASSOC_FAIL_ID = 27,
-	FMTSTR_IBSS_FAIL_ID = 28,
-	FMTSTR_EXTAP_FAIL_ID = 29,
-	FMTSTR_MAX_ID
-} log_fmtstr_id_t;
-
-#ifdef DONGLEOVERLAYS
-typedef struct {
-	uint32 flags_idx;	
-	uint32 offset;		
-	uint32 len;			
-	
-} wl_ioctl_overlay_t;
-
-#define OVERLAY_IDX_MASK		0x000000ff
-#define OVERLAY_IDX_SHIFT		0
-#define OVERLAY_FLAGS_MASK		0xffffff00
-#define OVERLAY_FLAGS_SHIFT		8
-
-#define OVERLAY_FLAG_POSTLOAD	0x100
-
-#define OVERLAY_FLAG_DEFER_DL	0x200
-
-#define OVERLAY_FLAG_PRESLEEP	0x400
-
-#define OVERLAY_DOWNLOAD_CHUNKSIZE	1024
-#endif 
-
-
-#include <packed_section_end.h>
-
-
-#include <packed_section_start.h>
-
-#define VNDR_IE_CMD_LEN     4   
-
-
-#define VNDR_IE_BEACON_FLAG 0x1
-#define VNDR_IE_PRBRSP_FLAG 0x2
-#define VNDR_IE_ASSOCRSP_FLAG   0x4
-#define VNDR_IE_AUTHRSP_FLAG    0x8
-#define VNDR_IE_PRBREQ_FLAG 0x10
-#define VNDR_IE_ASSOCREQ_FLAG   0x20
-#define VNDR_IE_CUSTOM_FLAG 0x100 
-
-#define VNDR_IE_INFO_HDR_LEN    (sizeof(uint32))
-
-typedef BWL_PRE_PACKED_STRUCT struct {
-	uint32 pktflag;         
-	vndr_ie_t vndr_ie_data;     
-} BWL_POST_PACKED_STRUCT vndr_ie_info_t;
-
-typedef BWL_PRE_PACKED_STRUCT struct {
-	int iecount;            
-	vndr_ie_info_t vndr_ie_list[1]; 
-} BWL_POST_PACKED_STRUCT vndr_ie_buf_t;
-
-typedef BWL_PRE_PACKED_STRUCT struct {
-	char cmd[VNDR_IE_CMD_LEN];  
-	vndr_ie_buf_t vndr_ie_buffer;   
-} BWL_POST_PACKED_STRUCT vndr_ie_setbuf_t;
-
-
-
-typedef BWL_PRE_PACKED_STRUCT struct sta_prbreq_wps_ie_hdr {
-	struct ether_addr staAddr;
-	uint16 ieLen;
-} BWL_POST_PACKED_STRUCT sta_prbreq_wps_ie_hdr_t;
-
-typedef BWL_PRE_PACKED_STRUCT struct sta_prbreq_wps_ie_data {
-	sta_prbreq_wps_ie_hdr_t hdr;
-	uint8 ieData[1];
-} BWL_POST_PACKED_STRUCT sta_prbreq_wps_ie_data_t;
-
-typedef BWL_PRE_PACKED_STRUCT struct sta_prbreq_wps_ie_list {
-	uint32 totLen;
-	uint8 ieDataList[1];
-} BWL_POST_PACKED_STRUCT sta_prbreq_wps_ie_list_t;
-
-
-#ifdef WLMEDIA_TXFAILEVENT
-typedef BWL_PRE_PACKED_STRUCT struct {
-	char   dest[ETHER_ADDR_LEN]; 
-	uint8  prio;            
-	uint8  flags;           
-	uint32 tsf_l;           
-	uint32 tsf_h;           
-	uint16 rates;           
-	uint16 txstatus;        
-} BWL_POST_PACKED_STRUCT txfailinfo_t;
-#endif 
-
-#include <packed_section_end.h>
-
-
-#define ASSERTLOG_CUR_VER   0x0100
-#define MAX_ASSRTSTR_LEN    64
-
-typedef struct assert_record {
-	uint32 time;
-	uint8 seq_num;
-	char str[MAX_ASSRTSTR_LEN];
-} assert_record_t;
-
-typedef struct assertlog_results {
-	uint16 version;
-	uint16 record_len;
-	uint32 num;
-	assert_record_t logs[1];
-} assertlog_results_t;
-
-#define LOGRRC_FIX_LEN  8
-#define IOBUF_ALLOWED_NUM_OF_LOGREC(type, len) ((len - LOGRRC_FIX_LEN)/sizeof(type))
-
-
-
-
-
-#define CHANIM_DISABLE  0   
-#define CHANIM_DETECT   1   
-#define CHANIM_ACT  2   
-#define CHANIM_MODE_MAX 2
-
-
-#define APCS_IOCTL      1
-#define APCS_CHANIM     2
-#define APCS_CSTIMER    3
-#define APCS_BTA        4
-
-
-#define CHANIM_ACS_RECORD           10
-
-
-typedef struct {
-	bool valid;
-	uint8 trigger;
-	chanspec_t selected_chspc;
-	uint32 glitch_cnt;
-	uint8 ccastats;
-	uint timestamp;
-} chanim_acs_record_t;
-
-typedef struct {
-	chanim_acs_record_t acs_record[CHANIM_ACS_RECORD];
-	uint8 count;
-	uint timestamp;
-} wl_acs_record_t;
-
-
-
-#define SMFS_VERSION 1
-
-typedef struct wl_smfs_elem {
-	uint32 count;
-	uint16 code;  
-} wl_smfs_elem_t;
-
-typedef struct wl_smf_stats {
-	uint32 version;
-	uint16 length;  
-	uint8 type;
-	uint8 codetype;
-	uint32 ignored_cnt;
-	uint32 malformed_cnt;
-	uint32 count_total; 
-	wl_smfs_elem_t elem[1];
-} wl_smf_stats_t;
-
-#define WL_SMFSTATS_FIXED_LEN OFFSETOF(wl_smf_stats_t, elem);
-
-enum {
-	SMFS_CODETYPE_SC,
-	SMFS_CODETYPE_RC
-};
-
-
-#define SMFS_CODE_MALFORMED 0xFFFE
-#define SMFS_CODE_IGNORED   0xFFFD
-
-typedef enum smfs_type {
-	SMFS_TYPE_AUTH,
-	SMFS_TYPE_ASSOC,
-	SMFS_TYPE_REASSOC,
-	SMFS_TYPE_DISASSOC_TX,
-	SMFS_TYPE_DISASSOC_RX,
-	SMFS_TYPE_DEAUTH_TX,
-	SMFS_TYPE_DEAUTH_RX,
-	SMFS_TYPE_MAX
-} smfs_type_t;
-
-#ifdef PHYMON
-
-#define PHYMON_VERSION 1
-
-typedef struct wl_phycal_core_state {
-	
-	int16 tx_iqlocal_a;
-	int16 tx_iqlocal_b;
-	int8 tx_iqlocal_ci;
-	int8 tx_iqlocal_cq;
-	int8 tx_iqlocal_di;
-	int8 tx_iqlocal_dq;
-	int8 tx_iqlocal_ei;
-	int8 tx_iqlocal_eq;
-	int8 tx_iqlocal_fi;
-	int8 tx_iqlocal_fq;
-
-	
-	int16 rx_iqcal_a;
-	int16 rx_iqcal_b;
-
-	uint8 tx_iqlocal_pwridx; 
-	uint32 papd_epsilon_table[64]; 
-	int16 papd_epsilon_offset; 
-	uint8 curr_tx_pwrindex; 
-	int8 idle_tssi; 
-	int8 est_tx_pwr; 
-	int8 est_rx_pwr; 
-	uint16 rx_gaininfo; 
-	uint16 init_gaincode; 
-	int8 estirr_tx;
-	int8 estirr_rx;
-
-} wl_phycal_core_state_t;
-
-typedef struct wl_phycal_state {
-	int version;
-	int8 num_phy_cores; 
-	int8 curr_temperature; 
-	chanspec_t chspec; 
-	bool aci_state; 
-	uint16 crsminpower; 
-	uint16 crsminpowerl; 
-	uint16 crsminpoweru; 
-	wl_phycal_core_state_t phycal_core[1];
-} wl_phycal_state_t;
-
-#define WL_PHYCAL_STAT_FIXED_LEN OFFSETOF(wl_phycal_state_t, phycal_core)
-#endif 
-
-#ifdef WLP2P
-
-typedef struct wl_p2p_disc_st {
-	uint8 state;    
-	chanspec_t chspec;  
-	uint16 dwell;   
-} wl_p2p_disc_st_t;
-
-
-#define WL_P2P_DISC_ST_SCAN 0
-#define WL_P2P_DISC_ST_LISTEN   1
-#define WL_P2P_DISC_ST_SEARCH   2
-
-
-typedef struct wl_p2p_scan {
-	uint8 type;     
-	uint8 reserved[3];
-	
-} wl_p2p_scan_t;
-
-
-typedef struct wl_p2p_if {
-	struct ether_addr addr;
-	uint8 type; 
-	chanspec_t chspec;  
-} wl_p2p_if_t;
-
-
-#define WL_P2P_IF_CLIENT    0
-#define WL_P2P_IF_GO        1
-#define WL_P2P_IF_DYNBCN_GO     2
-#define WL_P2P_IF_DEV       3
-
-
-typedef struct wl_p2p_ifq {
-	uint bsscfgidx;
-	char ifname[BCM_MSG_IFNAME_MAX];
-} wl_p2p_ifq_t;
-
-
-typedef struct wl_p2p_ops {
-	uint8 ops;  
-	uint8 ctw;  
-} wl_p2p_ops_t;
-
-
-typedef struct wl_p2p_sched_desc {
-	uint32 start;
-	uint32 interval;
-	uint32 duration;
-	uint32 count;   
-} wl_p2p_sched_desc_t;
-
-
-#define WL_P2P_SCHED_RSVD   0
-#define WL_P2P_SCHED_REPEAT 255 
-
-typedef struct wl_p2p_sched {
-	uint8 type; 
-	uint8 action;   
-	uint8 option;   
-	wl_p2p_sched_desc_t desc[1];
-} wl_p2p_sched_t;
-#define WL_P2P_SCHED_FIXED_LEN      3
-
-
-#define WL_P2P_SCHED_TYPE_ABS       0   
-#define WL_P2P_SCHED_TYPE_REQ_ABS   1   
-
-
-#define WL_P2P_SCHED_ACTION_NONE    0   
-#define WL_P2P_SCHED_ACTION_DOZE    1   
-
-#define WL_P2P_SCHED_ACTION_GOOFF   2   
-
-#define WL_P2P_SCHED_ACTION_RESET   255 
-
-
-#define WL_P2P_SCHED_OPTION_NORMAL  0   
-#define WL_P2P_SCHED_OPTION_BCNPCT  1   
-
-#define WL_P2P_SCHED_OPTION_TSFOFS  2   
-
-
-#define WL_P2P_FEAT_GO_CSA      (1 << 0)        
-#define WL_P2P_FEAT_GO_NOLEGACY (1 << 1)        
-#define WL_P2P_FEAT_RESTRICT_DEV_RESP (1 << 2)  
-#endif 
-
-
-#define BCM_ACTION_RFAWARE      0x77
-#define BCM_ACTION_RFAWARE_DCS  0x01
-
-
-
-#define WL_11N_2x2          1
-#define WL_11N_3x3          3
-#define WL_11N_4x4          4
-
-
-#define WLFEATURE_DISABLE_11N       0x00000001
-#define WLFEATURE_DISABLE_11N_STBC_TX   0x00000002
-#define WLFEATURE_DISABLE_11N_STBC_RX   0x00000004
-#define WLFEATURE_DISABLE_11N_SGI_TX    0x00000008
-#define WLFEATURE_DISABLE_11N_SGI_RX    0x00000010
-#define WLFEATURE_DISABLE_11N_AMPDU_TX  0x00000020
-#define WLFEATURE_DISABLE_11N_AMPDU_RX  0x00000040
-#define WLFEATURE_DISABLE_11N_GF    0x00000080
-
-
-#define LQ_IDX_LAST             3
-#define MCS_INDEX_SIZE          33
-
-#define LQ_IDX_MIN              0
-#define LQ_IDX_MAX              1
-#define LQ_IDX_AVG              2
-#define LQ_IDX_SUM              2
-#define LQ_IDX_LAST             3
-#define LQ_STOP_MONITOR         0
-#define LQ_START_MONITOR        1
-
-#define LINKQUAL_V1     0x01
-
-struct  wl_lq {
-	int32 enable;
-	int32 rssi[LQ_IDX_LAST];    
-	int32 rssicnt;
-	int32 snr[LQ_IDX_LAST];     
-	uint32 nsamples;            
-	uint8 isvalid;              
-	uint8 version;
-}; 
-
-typedef struct wl_lq wl_lq_t;
-typedef struct wl_lq  wl_lq_stats_t;
-
-typedef struct {
-	struct  ether_addr ea;  
-	uint8   ac_cat; 
-	uint8   num_pkts;   
-} wl_mac_ratehisto_cmd_t;   
-
-
-typedef struct {
-	uint32  rate[WLC_MAXRATE + 1];  
-	uint32  mcs_index[MCS_INDEX_SIZE];  
-	uint32  tsf_timer[2][2];    
-} wl_mac_ratehisto_res_t;   
-
-#ifdef PROP_TXSTATUS
-
-
-#define WLFC_FLAGS_RSSI_SIGNALS                         1
-
-
-#define WLFC_FLAGS_XONXOFF_SIGNALS                      2
-
-
-#define WLFC_FLAGS_CREDIT_STATUS_SIGNALS        4
-
-#define WLFC_FLAGS_HOST_PROPTXSTATUS_ACTIVE     8
-#define WLFC_FLAGS_PSQ_GENERATIONFSM_ENABLE     16
-#define WLFC_FLAGS_PSQ_ZERO_BUFFER_ENABLE	32
-#endif 
-
-#define BTA_STATE_LOG_SZ    64
-
-
-enum {
-	HCIReset = 1,
-	HCIReadLocalAMPInfo,
-	HCIReadLocalAMPASSOC,
-	HCIWriteRemoteAMPASSOC,
-	HCICreatePhysicalLink,
-	HCIAcceptPhysicalLinkRequest,
-	HCIDisconnectPhysicalLink,
-	HCICreateLogicalLink,
-	HCIAcceptLogicalLink,
-	HCIDisconnectLogicalLink,
-	HCILogicalLinkCancel,
-	HCIAmpStateChange,
-	HCIWriteLogicalLinkAcceptTimeout
-};
-
-typedef struct flush_txfifo {
-	uint32 txfifobmp;
-	uint32 hwtxfifoflush;
-	struct ether_addr ea;
-} flush_txfifo_t;
-
-#define CHANNEL_5G_LOW_START    36      
-#define CHANNEL_5G_MID_START    52      
-#define CHANNEL_5G_HIGH_START   100     
-#define CHANNEL_5G_UPPER_START  149     
-
-enum {
-	SPATIAL_MODE_2G_IDX = 0,
-	SPATIAL_MODE_5G_LOW_IDX,
-	SPATIAL_MODE_5G_MID_IDX,
-	SPATIAL_MODE_5G_HIGH_IDX,
-	SPATIAL_MODE_5G_UPPER_IDX,
-	SPATIAL_MODE_MAX_IDX
-};
-
-#endif 
diff --git a/drivers/net/wireless/bcmdhd/wl_android.c b/drivers/net/wireless/bcmdhd/wl_android.c
deleted file mode 100644
index d6471d99106..00000000000
--- a/drivers/net/wireless/bcmdhd/wl_android.c
+++ /dev/null
@@ -1,749 +0,0 @@
-/*
- * Linux cfg80211 driver - Android related functions
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- *
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- *
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- *
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: wl_android.c,v 1.1.4.1.2.14 2011/02/09 01:40:07 Exp $
- */
-
-#include <linux/module.h>
-#include <linux/netdevice.h>
-
-#include <wl_android.h>
-#include <wldev_common.h>
-#include <wlioctl.h>
-#include <bcmutils.h>
-#include <linux_osl.h>
-#include <dhd_dbg.h>
-#include <dngl_stats.h>
-#include <dhd.h>
-#include <bcmsdbus.h>
-
-#if defined(CONFIG_WIFI_CONTROL_FUNC)
-#include <linux/platform_device.h>
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35))
-#include <linux/wlan_plat.h>
-#else
-#include <linux/wifi_tiwlan.h>
-#endif
-#endif /* CONFIG_WIFI_CONTROL_FUNC */
-
-/*
- * Android private command strings, PLEASE define new private commands here
- * so they can be updated easily in the future (if needed)
- */
-
-#define CMD_START		"START"
-#define CMD_STOP		"STOP"
-#define	CMD_SCAN_ACTIVE		"SCAN-ACTIVE"
-#define	CMD_SCAN_PASSIVE	"SCAN-PASSIVE"
-#define CMD_RSSI		"RSSI"
-#define CMD_LINKSPEED		"LINKSPEED"
-#define CMD_RXFILTER_START	"RXFILTER-START"
-#define CMD_RXFILTER_STOP	"RXFILTER-STOP"
-#define CMD_RXFILTER_ADD	"RXFILTER-ADD"
-#define CMD_RXFILTER_REMOVE	"RXFILTER-REMOVE"
-#define CMD_BTCOEXSCAN_START	"BTCOEXSCAN-START"
-#define CMD_BTCOEXSCAN_STOP	"BTCOEXSCAN-STOP"
-#define CMD_BTCOEXMODE		"BTCOEXMODE"
-#define CMD_SETSUSPENDOPT	"SETSUSPENDOPT"
-#define CMD_P2P_DEV_ADDR	"P2P_DEV_ADDR"
-#define CMD_SETFWPATH		"SETFWPATH"
-#define CMD_SETBAND		"SETBAND"
-#define CMD_GETBAND		"GETBAND"
-#define CMD_COUNTRY		"COUNTRY"
-
-#ifdef PNO_SUPPORT
-#define CMD_PNOSSIDCLR_SET	"PNOSSIDCLR"
-#define CMD_PNOSETUP_SET	"PNOSETUP "
-#define CMD_PNOENABLE_SET	"PNOFORCE"
-#define CMD_PNODEBUG_SET	"PNODEBUG"
-
-#define PNO_TLV_PREFIX			'S'
-#define PNO_TLV_VERSION			'1'
-#define PNO_TLV_SUBVERSION 		'2'
-#define PNO_TLV_RESERVED		'0'
-#define PNO_TLV_TYPE_SSID_IE		'S'
-#define PNO_TLV_TYPE_TIME		'T'
-#define PNO_TLV_FREQ_REPEAT		'R'
-#define PNO_TLV_FREQ_EXPO_MAX		'M'
-
-typedef struct cmd_tlv {
-	char prefix;
-	char version;
-	char subver;
-	char reserved;
-} cmd_tlv_t;
-#endif /* PNO_SUPPORT */
-
-typedef struct android_wifi_priv_cmd {
-	char *buf;
-	int used_len;
-	int total_len;
-} android_wifi_priv_cmd;
-
-/**
- * Extern function declarations (TODO: move them to dhd_linux.h)
- */
-void dhd_customer_gpio_wlan_ctrl(int onoff);
-uint dhd_dev_reset(struct net_device *dev, uint8 flag);
-void dhd_dev_init_ioctl(struct net_device *dev);
-#ifdef WL_CFG80211
-int wl_cfg80211_get_p2p_dev_addr(struct net_device *net, struct ether_addr *p2pdev_addr);
-#else
-int wl_cfg80211_get_p2p_dev_addr(struct net_device *net, struct ether_addr *p2pdev_addr) { return 0; }
-#endif
-
-extern bool ap_fw_loaded;
-#ifdef CUSTOMER_HW2
-extern char iface_name[IFNAMSIZ];
-#endif
-
-/**
- * Local (static) functions and variables
- */
-
-/* Initialize g_wifi_on to 1 so dhd_bus_start will be called for the first
- * time (only) in dhd_open, subsequential wifi on will be handled by
- * wl_android_wifi_on
- */
-static int g_wifi_on = TRUE;
-
-/**
- * Local (static) function definitions
- */
-static int wl_android_get_link_speed(struct net_device *net, char *command, int total_len)
-{
-	int link_speed;
-	int bytes_written;
-	int error;
-
-	error = wldev_get_link_speed(net, &link_speed);
-	if (error)
-		return -1;
-
-	/* Convert Kbps to Android Mbps */
-	link_speed = link_speed / 1000;
-	bytes_written = snprintf(command, total_len, "LinkSpeed %d", link_speed);
-	DHD_INFO(("%s: command result is %s\n", __FUNCTION__, command));
-	return bytes_written;
-}
-
-static int wl_android_get_rssi(struct net_device *net, char *command, int total_len)
-{
-	wlc_ssid_t ssid = {0};
-	int rssi;
-	int bytes_written = 0;
-	int error;
-
-	error = wldev_get_rssi(net, &rssi);
-	if (error)
-		return -1;
-	error = wldev_get_ssid(net, &ssid);
-	if (error)
-		return -1;
-	if ((ssid.SSID_len == 0) || (ssid.SSID_len > DOT11_MAX_SSID_LEN)) {
-		DHD_ERROR(("%s: wldev_get_ssid failed\n", __FUNCTION__));
-	} else {
-		memcpy(command, ssid.SSID, ssid.SSID_len);
-		bytes_written = ssid.SSID_len;
-	}
-	bytes_written += snprintf(&command[bytes_written], total_len, " rssi %d", rssi);
-	DHD_INFO(("%s: command result is %s (%d)\n", __FUNCTION__, command, bytes_written));
-	return bytes_written;
-}
-
-static int wl_android_set_suspendopt(struct net_device *dev, char *command, int total_len)
-{
-	int suspend_flag;
-	int ret_now;
-	int ret = 0;
-
-	suspend_flag = *(command + strlen(CMD_SETSUSPENDOPT) + 1) - '0';
-
-	if (suspend_flag != 0)
-		suspend_flag = 1;
-	ret_now = net_os_set_suspend_disable(dev, suspend_flag);
-
-	if (ret_now != suspend_flag) {
-		if (!(ret = net_os_set_suspend(dev, ret_now)))
-			DHD_INFO(("%s: Suspend Flag %d -> %d\n",
-					__FUNCTION__, ret_now, suspend_flag));
-		else
-			DHD_ERROR(("%s: failed %d\n", __FUNCTION__, ret));
-	}
-	return ret;
-}
-
-static int wl_android_get_band(struct net_device *dev, char *command, int total_len)
-{
-	uint band;
-	int bytes_written;
-	int error;
-
-	error = wldev_get_band(dev, &band);
-	if (error)
-		return -1;
-	bytes_written = snprintf(command, total_len, "Band %d", band);
-	return bytes_written;
-}
-
-#ifdef PNO_SUPPORT
-static int wl_android_set_pno_setup(struct net_device *dev, char *command, int total_len)
-{
-	wlc_ssid_t ssids_local[MAX_PFN_LIST_COUNT];
-	int res = -1;
-	int nssid = 0;
-	cmd_tlv_t *cmd_tlv_temp;
-	char *str_ptr;
-	int tlv_size_left;
-	int pno_time = 0;
-	int pno_repeat = 0;
-	int pno_freq_expo_max = 0;
-
-	DHD_INFO(("%s: command=%s, len=%d\n", __FUNCTION__, command, total_len));
-
-	if (total_len < (strlen(CMD_PNOSETUP_SET) + sizeof(cmd_tlv_t))) {
-		DHD_ERROR(("%s argument=%d less min size\n", __FUNCTION__, total_len));
-		goto exit_proc;
-	}
-
-	str_ptr = command + strlen(CMD_PNOSETUP_SET);
-	tlv_size_left = total_len - strlen(CMD_PNOSETUP_SET);
-
-	cmd_tlv_temp = (cmd_tlv_t *)str_ptr;
-	memset(ssids_local, 0, sizeof(ssids_local));
-
-	if ((cmd_tlv_temp->prefix == PNO_TLV_PREFIX) &&
-		(cmd_tlv_temp->version == PNO_TLV_VERSION) &&
-		(cmd_tlv_temp->subver == PNO_TLV_SUBVERSION)) {
-
-		str_ptr += sizeof(cmd_tlv_t);
-		tlv_size_left -= sizeof(cmd_tlv_t);
-
-		if ((nssid = wl_iw_parse_ssid_list_tlv(&str_ptr, ssids_local,
-			MAX_PFN_LIST_COUNT, &tlv_size_left)) <= 0) {
-			DHD_ERROR(("SSID is not presented or corrupted ret=%d\n", nssid));
-			goto exit_proc;
-		} else {
-			if ((str_ptr[0] != PNO_TLV_TYPE_TIME) || (tlv_size_left <= 1)) {
-				DHD_ERROR(("%s scan duration corrupted field size %d\n",
-					__FUNCTION__, tlv_size_left));
-				goto exit_proc;
-			}
-			str_ptr++;
-			pno_time = simple_strtoul(str_ptr, &str_ptr, 16);
-			DHD_INFO(("%s: pno_time=%d\n", __FUNCTION__, pno_time));
-
-			if (str_ptr[0] != 0) {
-				if ((str_ptr[0] != PNO_TLV_FREQ_REPEAT)) {
-					DHD_ERROR(("%s pno repeat : corrupted field\n",
-						__FUNCTION__));
-					goto exit_proc;
-				}
-				str_ptr++;
-				pno_repeat = simple_strtoul(str_ptr, &str_ptr, 16);
-				DHD_INFO(("%s :got pno_repeat=%d\n", __FUNCTION__, pno_repeat));
-				if (str_ptr[0] != PNO_TLV_FREQ_EXPO_MAX) {
-					DHD_ERROR(("%s FREQ_EXPO_MAX corrupted field size\n",
-						__FUNCTION__));
-					goto exit_proc;
-				}
-				str_ptr++;
-				pno_freq_expo_max = simple_strtoul(str_ptr, &str_ptr, 16);
-				DHD_INFO(("%s: pno_freq_expo_max=%d\n",
-					__FUNCTION__, pno_freq_expo_max));
-			}
-		}
-	} else {
-		DHD_ERROR(("%s get wrong TLV command\n", __FUNCTION__));
-		goto exit_proc;
-	}
-
-	res = dhd_dev_pno_set(dev, ssids_local, nssid, pno_time, pno_repeat, pno_freq_expo_max);
-
-exit_proc:
-	return res;
-}
-#endif /* PNO_SUPPORT */
-
-static int wl_android_get_p2p_dev_addr(struct net_device *ndev, char *command, int total_len)
-{
-	int ret;
-	int bytes_written = 0;
-
-	ret = wl_cfg80211_get_p2p_dev_addr(ndev, (struct ether_addr*)command);
-	if (ret)
-		return 0;
-	bytes_written = sizeof(struct ether_addr);
-	return bytes_written;
-}
-
-/**
- * Global function definitions (declared in wl_android.h)
- */
-
-int wl_android_wifi_on(struct net_device *dev)
-{
-	int ret = 0;
-
-	printk("%s in\n", __FUNCTION__);
-	if (!dev) {
-		DHD_ERROR(("%s: dev is null\n", __FUNCTION__));
-		return -EINVAL;
-	}
-
-	dhd_net_if_lock(dev);
-	if (!g_wifi_on) {
-		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_ON);
-		sdioh_start(NULL, 0);
-		ret = dhd_dev_reset(dev, FALSE);
-		sdioh_start(NULL, 1);
-		dhd_dev_init_ioctl(dev);
-		g_wifi_on = 1;
-	}
-	dhd_net_if_unlock(dev);
-
-	return ret;
-}
-
-int wl_android_wifi_off(struct net_device *dev)
-{
-	int ret = 0;
-
-	printk("%s in\n", __FUNCTION__);
-	if (!dev) {
-		DHD_TRACE(("%s: dev is null\n", __FUNCTION__));
-		return -EINVAL;
-	}
-
-	dhd_net_if_lock(dev);
-	if (g_wifi_on) {
-		dhd_dev_reset(dev, 1);
-		sdioh_stop(NULL);
-		/* clean up dtim_skip setting */
-		net_os_set_dtim_skip(dev, TRUE);
-		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_OFF);
-		g_wifi_on = 0;
-	}
-	dhd_net_if_unlock(dev);
-
-	return ret;
-}
-
-static int wl_android_set_fwpath(struct net_device *net, char *command, int total_len)
-{
-	if ((strlen(command) - strlen(CMD_SETFWPATH)) > MOD_PARAM_PATHLEN)
-		return -1;
-	bcm_strncpy_s(fw_path, sizeof(fw_path),
-		command + strlen(CMD_SETFWPATH) + 1, MOD_PARAM_PATHLEN - 1);
-	if (strstr(fw_path, "apsta") != NULL) {
-		DHD_INFO(("GOT APSTA FIRMWARE\n"));
-		ap_fw_loaded = TRUE;
-	} else {
-		DHD_INFO(("GOT STA FIRMWARE\n"));
-		ap_fw_loaded = FALSE;
-	}
-	return 0;
-}
-
-int wl_android_priv_cmd(struct net_device *net, struct ifreq *ifr, int cmd)
-{
-	int ret = 0;
-	char *command = NULL;
-	int bytes_written = 0;
-	android_wifi_priv_cmd priv_cmd;
-
-	net_os_wake_lock(net);
-
-	if (!ifr->ifr_data) {
-		ret = -EINVAL;
-		goto exit;
-	}
-	if (copy_from_user(&priv_cmd, ifr->ifr_data, sizeof(android_wifi_priv_cmd))) {
-		ret = -EFAULT;
-		goto exit;
-	}
-	command = kmalloc(priv_cmd.total_len, GFP_KERNEL);
-	if (!command)
-	{
-		DHD_ERROR(("%s: failed to allocate memory\n", __FUNCTION__));
-		ret = -ENOMEM;
-		goto exit;
-	}
-	if (copy_from_user(command, priv_cmd.buf, priv_cmd.total_len)) {
-		ret = -EFAULT;
-		goto exit;
-	}
-
-	DHD_INFO(("%s: Android private cmd \"%s\" on %s\n", __FUNCTION__, command, ifr->ifr_name));
-
-	if (strnicmp(command, CMD_START, strlen(CMD_START)) == 0) {
-		DHD_INFO(("%s, Received regular START command\n", __FUNCTION__));
-		bytes_written = wl_android_wifi_on(net);
-	}
-	else if (strnicmp(command, CMD_SETFWPATH, strlen(CMD_SETFWPATH)) == 0) {
-		bytes_written = wl_android_set_fwpath(net, command, priv_cmd.total_len);
-	}
-
-	if (!g_wifi_on) {
-		DHD_ERROR(("%s: Ignore private cmd \"%s\" - iface %s is down\n",
-			__FUNCTION__, command, ifr->ifr_name));
-		ret = 0;
-		goto exit;
-	}
-
-	if (strnicmp(command, CMD_STOP, strlen(CMD_STOP)) == 0) {
-		bytes_written = wl_android_wifi_off(net);
-	}
-	else if (strnicmp(command, CMD_SCAN_ACTIVE, strlen(CMD_SCAN_ACTIVE)) == 0) {
-		/* TBD: SCAN-ACTIVE */
-	}
-	else if (strnicmp(command, CMD_SCAN_PASSIVE, strlen(CMD_SCAN_PASSIVE)) == 0) {
-		/* TBD: SCAN-PASSIVE */
-	}
-	else if (strnicmp(command, CMD_RSSI, strlen(CMD_RSSI)) == 0) {
-		bytes_written = wl_android_get_rssi(net, command, priv_cmd.total_len);
-	}
-	else if (strnicmp(command, CMD_LINKSPEED, strlen(CMD_LINKSPEED)) == 0) {
-		bytes_written = wl_android_get_link_speed(net, command, priv_cmd.total_len);
-	}
-	else if (strnicmp(command, CMD_RXFILTER_START, strlen(CMD_RXFILTER_START)) == 0) {
-		bytes_written = net_os_set_packet_filter(net, 1);
-	}
-	else if (strnicmp(command, CMD_RXFILTER_STOP, strlen(CMD_RXFILTER_STOP)) == 0) {
-		bytes_written = net_os_set_packet_filter(net, 0);
-	}
-	else if (strnicmp(command, CMD_RXFILTER_ADD, strlen(CMD_RXFILTER_ADD)) == 0) {
-		int filter_num = *(command + strlen(CMD_RXFILTER_ADD) + 1) - '0';
-		bytes_written = net_os_rxfilter_add_remove(net, TRUE, filter_num);
-	}
-	else if (strnicmp(command, CMD_RXFILTER_REMOVE, strlen(CMD_RXFILTER_REMOVE)) == 0) {
-		int filter_num = *(command + strlen(CMD_RXFILTER_REMOVE) + 1) - '0';
-		bytes_written = net_os_rxfilter_add_remove(net, FALSE, filter_num);
-	}
-	else if (strnicmp(command, CMD_BTCOEXSCAN_START, strlen(CMD_BTCOEXSCAN_START)) == 0) {
-		/* TBD: BTCOEXSCAN-START */
-	}
-	else if (strnicmp(command, CMD_BTCOEXSCAN_STOP, strlen(CMD_BTCOEXSCAN_STOP)) == 0) {
-		/* TBD: BTCOEXSCAN-STOP */
-	}
-	else if (strnicmp(command, CMD_BTCOEXMODE, strlen(CMD_BTCOEXMODE)) == 0) {
-		/* TBD: BTCOEXMODE */
-	}
-	else if (strnicmp(command, CMD_SETSUSPENDOPT, strlen(CMD_SETSUSPENDOPT)) == 0) {
-		bytes_written = wl_android_set_suspendopt(net, command, priv_cmd.total_len);
-	}
-	else if (strnicmp(command, CMD_SETBAND, strlen(CMD_SETBAND)) == 0) {
-		uint band = *(command + strlen(CMD_SETBAND) + 1) - '0';
-		bytes_written = wldev_set_band(net, band);
-	}
-	else if (strnicmp(command, CMD_GETBAND, strlen(CMD_GETBAND)) == 0) {
-		bytes_written = wl_android_get_band(net, command, priv_cmd.total_len);
-	}
-	else if (strnicmp(command, CMD_COUNTRY, strlen(CMD_COUNTRY)) == 0) {
-		char *country_code = command + strlen(CMD_COUNTRY) + 1;
-		bytes_written = wldev_set_country(net, country_code);
-	}
-#ifdef PNO_SUPPORT
-	else if (strnicmp(command, CMD_PNOSSIDCLR_SET, strlen(CMD_PNOSSIDCLR_SET)) == 0) {
-		bytes_written = dhd_dev_pno_reset(net);
-	}
-	else if (strnicmp(command, CMD_PNOSETUP_SET, strlen(CMD_PNOSETUP_SET)) == 0) {
-		bytes_written = wl_android_set_pno_setup(net, command, priv_cmd.total_len);
-	}
-	else if (strnicmp(command, CMD_PNOENABLE_SET, strlen(CMD_PNOENABLE_SET)) == 0) {
-		uint pfn_enabled = *(command + strlen(CMD_PNOENABLE_SET) + 1) - '0';
-		bytes_written = dhd_dev_pno_enable(net, pfn_enabled);
-	}
-#endif
-	else if (strnicmp(command, CMD_P2P_DEV_ADDR, strlen(CMD_P2P_DEV_ADDR)) == 0) {
-		bytes_written = wl_android_get_p2p_dev_addr(net, command, priv_cmd.total_len);
-	} else {
-		DHD_ERROR(("Unknown PRIVATE command %s - ignored\n", command));
-		snprintf(command, 3, "OK");
-		bytes_written = strlen("OK");
-	}
-
-	if (bytes_written > 0) {
-		if (bytes_written > priv_cmd.total_len) {
-			DHD_ERROR(("%s: bytes_written = %d\n", __FUNCTION__, bytes_written));
-			bytes_written = priv_cmd.total_len;
-		} else {
-			bytes_written++;
-		}
-		priv_cmd.used_len = bytes_written;
-		if (copy_to_user(priv_cmd.buf, command, bytes_written)) {
-			DHD_ERROR(("%s: failed to copy data to user buffer\n", __FUNCTION__));
-			ret = -EFAULT;
-		}
-	} else {
-		ret = bytes_written;
-	}
-
-exit:
-	net_os_wake_unlock(net);
-	if (command) {
-		kfree(command);
-	}
-
-	return ret;
-}
-
-int wl_android_init(void)
-{
-	int ret = 0;
-
-	dhd_msg_level = DHD_ERROR_VAL;
-#ifdef ENABLE_INSMOD_NO_FW_LOAD
-	dhd_download_fw_on_driverload = FALSE;
-#endif /* ENABLE_INSMOD_NO_FW_LOAD */
-#ifdef CUSTOMER_HW2
-	if (!iface_name[0])
-		bcm_strncpy_s(iface_name, IFNAMSIZ, "wlan", IFNAMSIZ);
-#endif /* CUSTOMER_HW2 */
-	return ret;
-}
-
-int wl_android_exit(void)
-{
-	int ret = 0;
-
-	return ret;
-}
-
-int wl_android_post_init(void)
-{
-	int ret = 0;
-	if (!dhd_download_fw_on_driverload) {
-		/* Call customer gpio to turn off power with WL_REG_ON signal */
-		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_OFF);
-		g_wifi_on = 0;
-
-	}
-	return ret;
-}
-
-/**
- * Functions for Android WiFi card detection
- */
-#if defined(CONFIG_WIFI_CONTROL_FUNC)
-
-static int g_wifidev_registered = 0;
-static struct semaphore wifi_control_sem;
-static struct wifi_platform_data *wifi_control_data = NULL;
-static struct resource *wifi_irqres = NULL;
-
-static int wifi_add_dev(void);
-static void wifi_del_dev(void);
-
-int wl_android_wifictrl_func_add(void)
-{
-	int ret = 0;
-	sema_init(&wifi_control_sem, 0);
-
-	ret = wifi_add_dev();
-	if (ret) {
-		DHD_ERROR(("%s: platform_driver_register failed\n", __FUNCTION__));
-		return ret;
-	}
-	g_wifidev_registered = 1;
-
-	/* Waiting callback after platform_driver_register is done or exit with error */
-	if (down_timeout(&wifi_control_sem,  msecs_to_jiffies(1000)) != 0) {
-		ret = -EINVAL;
-		DHD_ERROR(("%s: platform_driver_register timeout\n", __FUNCTION__));
-	}
-
-	return ret;
-}
-
-void wl_android_wifictrl_func_del(void)
-{
-	if (g_wifidev_registered)
-	{
-		wifi_del_dev();
-		g_wifidev_registered = 0;
-	}
-}
-
-void* wl_android_prealloc(int section, unsigned long size)
-{
-	void *alloc_ptr = NULL;
-	if (wifi_control_data && wifi_control_data->mem_prealloc) {
-		alloc_ptr = wifi_control_data->mem_prealloc(section, size);
-		if (alloc_ptr) {
-			DHD_INFO(("success alloc section %d\n", section));
-			bzero(alloc_ptr, size);
-			return alloc_ptr;
-		}
-	}
-
-	DHD_ERROR(("can't alloc section %d\n", section));
-	return 0;
-}
-
-int wifi_get_irq_number(unsigned long *irq_flags_ptr)
-{
-	if (wifi_irqres) {
-		*irq_flags_ptr = wifi_irqres->flags & IRQF_TRIGGER_MASK;
-		return (int)wifi_irqres->start;
-	}
-#ifdef CUSTOM_OOB_GPIO_NUM
-	return CUSTOM_OOB_GPIO_NUM;
-#else
-	return -1;
-#endif
-}
-
-int wifi_set_power(int on, unsigned long msec)
-{
-	DHD_ERROR(("%s = %d\n", __FUNCTION__, on));
-	if (wifi_control_data && wifi_control_data->set_power) {
-		wifi_control_data->set_power(on);
-	}
-	if (msec)
-		mdelay(msec);
-	return 0;
-}
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35))
-int wifi_get_mac_addr(unsigned char *buf)
-{
-	DHD_ERROR(("%s\n", __FUNCTION__));
-	if (!buf)
-		return -EINVAL;
-	if (wifi_control_data && wifi_control_data->get_mac_addr) {
-		return wifi_control_data->get_mac_addr(buf);
-	}
-	return -EOPNOTSUPP;
-}
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35)) */
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 39))
-void *wifi_get_country_code(char *ccode)
-{
-	DHD_TRACE(("%s\n", __FUNCTION__));
-	if (!ccode)
-		return NULL;
-	if (wifi_control_data && wifi_control_data->get_country_code) {
-		return wifi_control_data->get_country_code(ccode);
-	}
-	return NULL;
-}
-#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 39)) */
-
-static int wifi_set_carddetect(int on)
-{
-	DHD_ERROR(("%s = %d\n", __FUNCTION__, on));
-	if (wifi_control_data && wifi_control_data->set_carddetect) {
-		wifi_control_data->set_carddetect(on);
-	}
-	return 0;
-}
-
-static int wifi_probe(struct platform_device *pdev)
-{
-	struct wifi_platform_data *wifi_ctrl =
-		(struct wifi_platform_data *)(pdev->dev.platform_data);
-
-	DHD_ERROR(("## %s\n", __FUNCTION__));
-	wifi_irqres = platform_get_resource_byname(pdev, IORESOURCE_IRQ, "bcmdhd_wlan_irq");
-	if (wifi_irqres == NULL)
-		wifi_irqres = platform_get_resource_byname(pdev,
-			IORESOURCE_IRQ, "bcm4329_wlan_irq");
-	wifi_control_data = wifi_ctrl;
-
-	wifi_set_power(1, 0);	/* Power On */
-	wifi_set_carddetect(1);	/* CardDetect (0->1) */
-
-	up(&wifi_control_sem);
-	return 0;
-}
-
-static int wifi_remove(struct platform_device *pdev)
-{
-	struct wifi_platform_data *wifi_ctrl =
-		(struct wifi_platform_data *)(pdev->dev.platform_data);
-
-	DHD_ERROR(("## %s\n", __FUNCTION__));
-	wifi_control_data = wifi_ctrl;
-
-	wifi_set_power(0, 0);	/* Power Off */
-	wifi_set_carddetect(0);	/* CardDetect (1->0) */
-
-	up(&wifi_control_sem);
-	return 0;
-}
-
-static int wifi_suspend(struct platform_device *pdev, pm_message_t state)
-{
-	DHD_TRACE(("##> %s\n", __FUNCTION__));
-#if defined(OOB_INTR_ONLY)
-	bcmsdh_oob_intr_set(0);
-#endif /* (OOB_INTR_ONLY) */
-	return 0;
-}
-
-static int wifi_resume(struct platform_device *pdev)
-{
-	DHD_TRACE(("##> %s\n", __FUNCTION__));
-#if defined(OOB_INTR_ONLY)
-	bcmsdh_oob_intr_set(1);
-#endif /* (OOB_INTR_ONLY) */
-	return 0;
-}
-
-static struct platform_driver wifi_device = {
-	.probe          = wifi_probe,
-	.remove         = wifi_remove,
-	.suspend        = wifi_suspend,
-	.resume         = wifi_resume,
-	.driver         = {
-	.name   = "bcmdhd_wlan",
-	}
-};
-
-static struct platform_driver wifi_device_legacy = {
-	.probe          = wifi_probe,
-	.remove         = wifi_remove,
-	.suspend        = wifi_suspend,
-	.resume         = wifi_resume,
-	.driver         = {
-	.name   = "bcm4329_wlan",
-	}
-};
-
-static int wifi_add_dev(void)
-{
-	DHD_TRACE(("## Calling platform_driver_register\n"));
-	platform_driver_register(&wifi_device);
-	platform_driver_register(&wifi_device_legacy);
-	return 0;
-}
-
-static void wifi_del_dev(void)
-{
-	DHD_TRACE(("## Unregister platform_driver_register\n"));
-	platform_driver_unregister(&wifi_device);
-	platform_driver_unregister(&wifi_device_legacy);
-}
-#endif /* defined(CONFIG_WIFI_CONTROL_FUNC) */
diff --git a/drivers/net/wireless/bcmdhd/wl_android.h b/drivers/net/wireless/bcmdhd/wl_android.h
deleted file mode 100644
index 17373b7f6d5..00000000000
--- a/drivers/net/wireless/bcmdhd/wl_android.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * Linux cfg80211 driver - Android related functions
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- *
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- *
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- *
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: wl_android.c,v 1.1.4.1.2.14 2011/02/09 01:40:07 Exp $
- */
-
-#include <linux/module.h>
-#include <linux/netdevice.h>
-#include <wldev_common.h>
-
-/**
- * Android platform dependent functions, feel free to add Android specific functions here
- * (save the macros in dhd). Please do NOT declare functions that are NOT exposed to dhd
- * or cfg, define them as static in wl_android.c
- */
-
-/**
- * wl_android_init will be called from module init function (dhd_module_init now), similarly
- * wl_android_exit will be called from module exit function (dhd_module_cleanup now)
- */
-int wl_android_init(void);
-int wl_android_exit(void);
-int wl_android_post_init(void);
-int wl_android_wifi_on(struct net_device *dev);
-int wl_android_wifi_off(struct net_device *dev);
-int wl_android_priv_cmd(struct net_device *net, struct ifreq *ifr, int cmd);
-
-#if defined(CONFIG_WIFI_CONTROL_FUNC)
-int wl_android_wifictrl_func_add(void);
-void wl_android_wifictrl_func_del(void);
-void* wl_android_prealloc(int section, unsigned long size);
-
-int wifi_get_irq_number(unsigned long *irq_flags_ptr);
-int wifi_set_power(int on, unsigned long msec);
-int wifi_get_mac_addr(unsigned char *buf);
-void *wifi_get_country_code(char *ccode);
-#endif /* CONFIG_WIFI_CONTROL_FUNC */
diff --git a/drivers/net/wireless/bcmdhd/wl_cfg80211.c b/drivers/net/wireless/bcmdhd/wl_cfg80211.c
deleted file mode 100644
index 09529136787..00000000000
--- a/drivers/net/wireless/bcmdhd/wl_cfg80211.c
+++ /dev/null
@@ -1,6584 +0,0 @@
-/*
- * Linux cfg80211 driver
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- *
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- *
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: wl_cfg80211.c,v 1.1.4.1.2.14 2011/02/09 01:40:07 Exp $
- */
-
-#include <typedefs.h>
-#include <linuxver.h>
-#include <osl.h>
-#include <linux/kernel.h>
-
-/*
- * sys proc file will be REMOVED in next release
- */
-#if defined(DHD_P2P_DEV_ADDR_FROM_SYSFS) && defined(CONFIG_SYSCTL)
-#include <linux/sysctl.h>
-#endif
-
-#include <bcmutils.h>
-#include <bcmwifi.h>
-#include <bcmendian.h>
-#include <proto/ethernet.h>
-#include <proto/802.11.h>
-#include <linux/if_arp.h>
-#include <asm/uaccess.h>
-
-#include <dngl_stats.h>
-#include <dhd.h>
-#include <dhdioctl.h>
-#include <wlioctl.h>
-
-#include <proto/ethernet.h>
-#include <dngl_stats.h>
-#include <dhd.h>
-
-#include <linux/kernel.h>
-#include <linux/kthread.h>
-#include <linux/netdevice.h>
-#include <linux/sched.h>
-#include <linux/etherdevice.h>
-#include <linux/wireless.h>
-#include <linux/ieee80211.h>
-#include <linux/wait.h>
-#include <net/cfg80211.h>
-
-#include <net/rtnetlink.h>
-#include <linux/mmc/sdio_func.h>
-#include <linux/firmware.h>
-#include <bcmsdbus.h>
-
-#include <wlioctl.h>
-#include <wldev_common.h>
-#include <wl_cfg80211.h>
-#include <wl_cfgp2p.h>
-
-static struct sdio_func *cfg80211_sdio_func;
-static struct wl_priv *wlcfg_drv_priv;
-
-u32 wl_dbg_level = WL_DBG_ERR;
-
-#define WL_4329_FW_FILE "brcm/bcm4329-fullmac-4-218-248-5.bin"
-#define WL_4329_NVRAM_FILE "brcm/bcm4329-fullmac-4-218-248-5.txt"
-#define WL_TRACE(a) printk("%s ", __FUNCTION__); printk a
-#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
-#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
-#define MAX_WAIT_TIME 1500
-static s8 ioctlbuf[WLC_IOCTL_MAXLEN];
-
-#if defined(DHD_P2P_DEV_ADDR_FROM_SYSFS) && defined(CONFIG_SYSCTL)
-#define MAC_STRING_LEN (sizeof(u8) * 17)
-u8 wl_sysctl_macstring[2][MAC_STRING_LEN];
-
-static ctl_table wl_sysctl_child[] = {
-	{
-	.procname = "p2p_dev_addr",
-	.data = &wl_sysctl_macstring[0],
-	.maxlen = MAC_STRING_LEN,
-	.mode =  0444,
-	.child = NULL,
-	.proc_handler = proc_dostring,
-	},
-	{
-	.procname = "p2p_int_addr",
-	.data = &wl_sysctl_macstring[1],
-	.maxlen = MAC_STRING_LEN,
-	.mode =  0444,
-	.child = NULL,
-	.proc_handler = proc_dostring,
-	},
-	{0}
-};
-static ctl_table wl_sysctl_table[] = {
-	{
-	.procname = "wifi",
-	.data = NULL,
-	.maxlen = 0,
-	.mode =  0555,
-	.child = wl_sysctl_child,
-	.proc_handler = NULL,
-	},
-	{0}
-};
-static struct ctl_table_header *wl_sysctl_hdr;
-#endif /* CONFIG_SYSCTL */
-
-/* This is to override regulatory domains defined in cfg80211 module (reg.c)
- * By default world regulatory domain defined in reg.c puts the flags NL80211_RRF_PASSIVE_SCAN
- * and NL80211_RRF_NO_IBSS for 5GHz channels (for 36..48 and 149..165).
- * With respect to these flags, wpa_supplicant doesn't start p2p operations on 5GHz channels.
- * All the chnages in world regulatory domain are to be done here.
- */
-static const struct ieee80211_regdomain brcm_regdom = {
-	.n_reg_rules = 5,
-	.alpha2 =  "99",
-	.reg_rules = {
-		/* IEEE 802.11b/g, channels 1..11 */
-		REG_RULE(2412-10, 2462+10, 40, 6, 20, 0),
-		/* IEEE 802.11b/g, channels 12..13. No HT40
-		 * channel fits here.
-		 */
-		REG_RULE(2467-10, 2472+10, 20, 6, 20,
-		NL80211_RRF_PASSIVE_SCAN |
-		NL80211_RRF_NO_IBSS),
-		/* IEEE 802.11 channel 14 - Only JP enables
-		 * this and for 802.11b only
-		 */
-		REG_RULE(2484-10, 2484+10, 20, 6, 20,
-		NL80211_RRF_PASSIVE_SCAN |
-		NL80211_RRF_NO_IBSS |
-		NL80211_RRF_NO_OFDM),
-		/* IEEE 802.11a, channel 36..64 */
-		REG_RULE(5150-10, 5350+10, 40, 6, 20, 0),
-		/* IEEE 802.11a, channel 100..165 */
-		REG_RULE(5470-10, 5850+10, 40, 6, 20, 0), }
-};
-
-
-/* Data Element Definitions */
-#define WPS_ID_CONFIG_METHODS     0x1008
-#define WPS_ID_REQ_TYPE           0x103A
-#define WPS_ID_DEVICE_NAME        0x1011
-#define WPS_ID_VERSION            0x104A
-#define WPS_ID_DEVICE_PWD_ID      0x1012
-#define WPS_ID_REQ_DEV_TYPE       0x106A
-#define WPS_ID_SELECTED_REGISTRAR_CONFIG_METHODS 0x1053
-#define WPS_ID_PRIM_DEV_TYPE      0x1054
-
-/* Device Password ID */
-#define DEV_PW_DEFAULT 0x0000
-#define DEV_PW_USER_SPECIFIED 0x0001,
-#define DEV_PW_MACHINE_SPECIFIED 0x0002
-#define DEV_PW_REKEY 0x0003
-#define DEV_PW_PUSHBUTTON 0x0004
-#define DEV_PW_REGISTRAR_SPECIFIED 0x0005
-
-/* Config Methods */
-#define WPS_CONFIG_USBA 0x0001
-#define WPS_CONFIG_ETHERNET 0x0002
-#define WPS_CONFIG_LABEL 0x0004
-#define WPS_CONFIG_DISPLAY 0x0008
-#define WPS_CONFIG_EXT_NFC_TOKEN 0x0010
-#define WPS_CONFIG_INT_NFC_TOKEN 0x0020
-#define WPS_CONFIG_NFC_INTERFACE 0x0040
-#define WPS_CONFIG_PUSHBUTTON 0x0080
-#define WPS_CONFIG_KEYPAD 0x0100
-#define WPS_CONFIG_VIRT_PUSHBUTTON 0x0280
-#define WPS_CONFIG_PHY_PUSHBUTTON 0x0480
-#define WPS_CONFIG_VIRT_DISPLAY 0x2008
-#define WPS_CONFIG_PHY_DISPLAY 0x4008
-
-/*
- * cfg80211_ops api/callback list
- */
-static s32 wl_frame_get_mgmt(u16 fc, const struct ether_addr *da,
-	const struct ether_addr *sa, const struct ether_addr *bssid,
-	u8 **pheader, u32 *body_len, u8 *pbody);
-static s32 __wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
-	struct cfg80211_scan_request *request,
-	struct cfg80211_ssid *this_ssid);
-static s32 wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
-	struct cfg80211_scan_request *request);
-static s32 wl_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed);
-static s32 wl_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
-	struct cfg80211_ibss_params *params);
-static s32 wl_cfg80211_leave_ibss(struct wiphy *wiphy,
-	struct net_device *dev);
-static s32 wl_cfg80211_get_station(struct wiphy *wiphy,
-	struct net_device *dev, u8 *mac,
-	struct station_info *sinfo);
-static s32 wl_cfg80211_set_power_mgmt(struct wiphy *wiphy,
-	struct net_device *dev, bool enabled,
-	s32 timeout);
-static int wl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
-	struct cfg80211_connect_params *sme);
-static s32 wl_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
-	u16 reason_code);
-static s32 wl_cfg80211_set_tx_power(struct wiphy *wiphy,
-	enum nl80211_tx_power_setting type,
-	s32 dbm);
-static s32 wl_cfg80211_get_tx_power(struct wiphy *wiphy, s32 *dbm);
-static s32 wl_cfg80211_config_default_key(struct wiphy *wiphy,
-	struct net_device *dev,
-	u8 key_idx, bool unicast, bool multicast);
-static s32 wl_cfg80211_add_key(struct wiphy *wiphy, struct net_device *dev,
-	u8 key_idx, bool pairwise, const u8 *mac_addr,
-	struct key_params *params);
-static s32 wl_cfg80211_del_key(struct wiphy *wiphy, struct net_device *dev,
-	u8 key_idx, bool pairwise, const u8 *mac_addr);
-static s32 wl_cfg80211_get_key(struct wiphy *wiphy, struct net_device *dev,
-	u8 key_idx, bool pairwise, const u8 *mac_addr,
-	void *cookie, void (*callback) (void *cookie,
-	struct key_params *params));
-static s32 wl_cfg80211_config_default_mgmt_key(struct wiphy *wiphy,
-	struct net_device *dev,	u8 key_idx);
-static s32 wl_cfg80211_resume(struct wiphy *wiphy);
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 39)
-static s32 wl_cfg80211_suspend(struct wiphy *wiphy, struct cfg80211_wowlan *wow);
-#else
-static s32 wl_cfg80211_suspend(struct wiphy *wiphy);
-#endif
-static s32 wl_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *dev,
-	struct cfg80211_pmksa *pmksa);
-static s32 wl_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *dev,
-	struct cfg80211_pmksa *pmksa);
-static s32 wl_cfg80211_flush_pmksa(struct wiphy *wiphy,
-	struct net_device *dev);
-static void wl_notify_escan_complete(struct wl_priv *wl, bool aborted);
-/*
- * event & event Q handlers for cfg80211 interfaces
- */
-static s32 wl_create_event_handler(struct wl_priv *wl);
-static void wl_destroy_event_handler(struct wl_priv *wl);
-static s32 wl_event_handler(void *data);
-static void wl_init_eq(struct wl_priv *wl);
-static void wl_flush_eq(struct wl_priv *wl);
-static void wl_lock_eq(struct wl_priv *wl);
-static void wl_unlock_eq(struct wl_priv *wl);
-static void wl_init_eq_lock(struct wl_priv *wl);
-static void wl_init_event_handler(struct wl_priv *wl);
-static struct wl_event_q *wl_deq_event(struct wl_priv *wl);
-static s32 wl_enq_event(struct wl_priv *wl, struct net_device *ndev, u32 type,
-	const wl_event_msg_t *msg, void *data);
-static void wl_put_event(struct wl_event_q *e);
-static void wl_wakeup_event(struct wl_priv *wl);
-static s32 wl_notify_connect_status(struct wl_priv *wl,
-	struct net_device *ndev,
-	const wl_event_msg_t *e, void *data);
-static s32 wl_notify_roaming_status(struct wl_priv *wl,
-	struct net_device *ndev,
-	const wl_event_msg_t *e, void *data);
-static s32 wl_notify_scan_status(struct wl_priv *wl, struct net_device *ndev,
-	const wl_event_msg_t *e, void *data);
-static s32 wl_bss_connect_done(struct wl_priv *wl, struct net_device *ndev,
-	const wl_event_msg_t *e, void *data, bool completed);
-static s32 wl_bss_roaming_done(struct wl_priv *wl, struct net_device *ndev,
-	const wl_event_msg_t *e, void *data);
-static s32 wl_notify_mic_status(struct wl_priv *wl, struct net_device *ndev,
-	const wl_event_msg_t *e, void *data);
-/*
- * register/deregister sdio function
- */
-struct sdio_func *wl_cfg80211_get_sdio_func(void);
-static void wl_clear_sdio_func(void);
-
-/*
- * ioctl utilites
- */
-static s32 wl_dev_bufvar_get(struct net_device *dev, s8 *name, s8 *buf,
-	s32 buf_len);
-static __used s32 wl_dev_bufvar_set(struct net_device *dev, s8 *name,
-	s8 *buf, s32 len);
-static s32 wl_dev_intvar_set(struct net_device *dev, s8 *name, s32 val);
-static s32 wl_dev_intvar_get(struct net_device *dev, s8 *name,
-	s32 *retval);
-
-/*
- * cfg80211 set_wiphy_params utilities
- */
-static s32 wl_set_frag(struct net_device *dev, u32 frag_threshold);
-static s32 wl_set_rts(struct net_device *dev, u32 frag_threshold);
-static s32 wl_set_retry(struct net_device *dev, u32 retry, bool l);
-
-/*
- * wl profile utilities
- */
-static s32 wl_update_prof(struct wl_priv *wl, const wl_event_msg_t *e,
-	void *data, s32 item);
-static void *wl_read_prof(struct wl_priv *wl, s32 item);
-static void wl_init_prof(struct wl_profile *prof);
-
-/*
- * cfg80211 connect utilites
- */
-static s32 wl_set_wpa_version(struct net_device *dev,
-	struct cfg80211_connect_params *sme);
-static s32 wl_set_auth_type(struct net_device *dev,
-	struct cfg80211_connect_params *sme);
-static s32 wl_set_set_cipher(struct net_device *dev,
-	struct cfg80211_connect_params *sme);
-static s32 wl_set_key_mgmt(struct net_device *dev,
-	struct cfg80211_connect_params *sme);
-static s32 wl_set_set_sharedkey(struct net_device *dev,
-	struct cfg80211_connect_params *sme);
-static s32 wl_get_assoc_ies(struct wl_priv *wl, struct net_device *ndev);
-static void wl_ch_to_chanspec(int ch,
-	struct wl_join_params *join_params, size_t *join_params_size);
-
-/*
- * information element utilities
- */
-static void wl_rst_ie(struct wl_priv *wl);
-static __used s32 wl_add_ie(struct wl_priv *wl, u8 t, u8 l, u8 *v);
-static s32 wl_mrg_ie(struct wl_priv *wl, u8 *ie_stream, u16 ie_size);
-static s32 wl_cp_ie(struct wl_priv *wl, u8 *dst, u16 dst_size);
-static u32 wl_get_ielen(struct wl_priv *wl);
-
-static s32 wl_mode_to_nl80211_iftype(s32 mode);
-
-static struct wireless_dev *wl_alloc_wdev(struct device *sdiofunc_dev);
-static void wl_free_wdev(struct wl_priv *wl);
-
-static s32 wl_inform_bss(struct wl_priv *wl);
-static s32 wl_inform_single_bss(struct wl_priv *wl, struct wl_bss_info *bi);
-static s32 wl_update_bss_info(struct wl_priv *wl, struct net_device *ndev);
-
-static s32 wl_add_keyext(struct wiphy *wiphy, struct net_device *dev,
-	u8 key_idx, const u8 *mac_addr,
-	struct key_params *params);
-/*
- * key indianess swap utilities
- */
-static void swap_key_from_BE(struct wl_wsec_key *key);
-static void swap_key_to_BE(struct wl_wsec_key *key);
-
-/*
- * wl_priv memory init/deinit utilities
- */
-static s32 wl_init_priv_mem(struct wl_priv *wl);
-static void wl_deinit_priv_mem(struct wl_priv *wl);
-
-static void wl_delay(u32 ms);
-
-/*
- * ibss mode utilities
- */
-static bool wl_is_ibssmode(struct wl_priv *wl, struct net_device *ndev);
-static __used bool wl_is_ibssstarter(struct wl_priv *wl);
-
-/*
- * dongle up/down , default configuration utilities
- */
-static bool wl_is_linkdown(struct wl_priv *wl, const wl_event_msg_t *e);
-static bool wl_is_linkup(struct wl_priv *wl, const wl_event_msg_t *e, struct net_device *ndev);
-static bool wl_is_nonetwork(struct wl_priv *wl, const wl_event_msg_t *e);
-static void wl_link_up(struct wl_priv *wl);
-static void wl_link_down(struct wl_priv *wl);
-static s32 wl_dongle_mode(struct wl_priv *wl, struct net_device *ndev, s32 iftype);
-static s32 __wl_cfg80211_up(struct wl_priv *wl);
-static s32 __wl_cfg80211_down(struct wl_priv *wl);
-static s32 wl_dongle_probecap(struct wl_priv *wl);
-static void wl_init_conf(struct wl_conf *conf);
-static s32 wl_dongle_add_remove_eventmsg(struct net_device *ndev, u16 event, bool add);
-static s32 wl_dongle_eventmsg(struct net_device *ndev);
-
-/*
- * dongle configuration utilities
- */
-#ifndef EMBEDDED_PLATFORM
-static s32 wl_dongle_country(struct net_device *ndev, u8 ccode);
-static s32 wl_dongle_up(struct net_device *ndev, u32 up);
-static s32 wl_dongle_power(struct net_device *ndev, u32 power_mode);
-static s32 wl_dongle_glom(struct net_device *ndev, u32 glom,
-	u32 dongle_align);
-static s32 wl_dongle_roam(struct net_device *ndev, u32 roamvar,
-	u32 bcn_timeout);
-static s32 wl_dongle_eventmsg(struct net_device *ndev);
-static s32 wl_dongle_scantime(struct net_device *ndev, s32 scan_assoc_time,
-	s32 scan_unassoc_time);
-static s32 wl_dongle_offload(struct net_device *ndev, s32 arpoe,
-	s32 arp_ol);
-static s32 wl_pattern_atoh(s8 *src, s8 *dst);
-static s32 wl_dongle_filter(struct net_device *ndev, u32 filter_mode);
-static s32 wl_update_wiphybands(struct wl_priv *wl);
-#endif				/* !EMBEDDED_PLATFORM */
-static __used void wl_dongle_poweron(struct wl_priv *wl);
-static __used void wl_dongle_poweroff(struct wl_priv *wl);
-static s32 wl_config_dongle(struct wl_priv *wl, bool need_lock);
-
-/*
- * iscan handler
- */
-static void wl_iscan_timer(unsigned long data);
-static void wl_term_iscan(struct wl_priv *wl);
-static s32 wl_init_scan(struct wl_priv *wl);
-static s32 wl_iscan_thread(void *data);
-static s32 wl_run_iscan(struct wl_iscan_ctrl *iscan, struct cfg80211_scan_request *request,
-	u16 action);
-static s32 wl_do_iscan(struct wl_priv *wl,  struct cfg80211_scan_request *request);
-static s32 wl_wakeup_iscan(struct wl_iscan_ctrl *iscan);
-static s32 wl_invoke_iscan(struct wl_priv *wl);
-static s32 wl_get_iscan_results(struct wl_iscan_ctrl *iscan, u32 *status,
-	struct wl_scan_results **bss_list);
-static void wl_notify_iscan_complete(struct wl_iscan_ctrl *iscan, bool aborted);
-static void wl_init_iscan_handler(struct wl_iscan_ctrl *iscan);
-static s32 wl_iscan_done(struct wl_priv *wl);
-static s32 wl_iscan_pending(struct wl_priv *wl);
-static s32 wl_iscan_inprogress(struct wl_priv *wl);
-static s32 wl_iscan_aborted(struct wl_priv *wl);
-
-/*
- * fw/nvram downloading handler
- */
-static void wl_init_fw(struct wl_fw_ctrl *fw);
-
-/*
- * find most significant bit set
- */
-static __used u32 wl_find_msb(u16 bit16);
-
-/*
- * update pmklist to dongle
- */
-static __used s32 wl_update_pmklist(struct net_device *dev,
-	struct wl_pmk_list *pmk_list, s32 err);
-
-/*
- * debufs support
- */
-static int wl_debugfs_add_netdev_params(struct wl_priv *wl);
-static void wl_debugfs_remove_netdev(struct wl_priv *wl);
-
-/*
- * rfkill support
- */
-static int wl_setup_rfkill(struct wl_priv *wl, bool setup);
-static int wl_rfkill_set(void *data, bool blocked);
-
-/*
- * Some external functions, TODO: move them to dhd_linux.h
- */
-int dhd_add_monitor(char *name, struct net_device **new_ndev);
-int dhd_del_monitor(struct net_device *ndev);
-int dhd_monitor_init(void *dhd_pub);
-int dhd_monitor_uninit(void);
-int dhd_start_xmit(struct sk_buff *skb, struct net_device *net);
-
-#define CHECK_SYS_UP(wlpriv)							\
-do {									\
-	if (unlikely(!wl_get_drv_status(wlpriv, READY))) {	\
-		WL_INFO(("device is not ready : status (%d)\n",		\
-			(int)wlpriv->status));			\
-		return -EIO;						\
-	}								\
-} while (0)
-
-
-#define IS_WPA_AKM(akm) ((akm) == RSN_AKM_NONE || \
-				 (akm) == RSN_AKM_UNSPECIFIED || \
-				 (akm) == RSN_AKM_PSK)
-
-
-extern int dhd_wait_pend8021x(struct net_device *dev);
-
-#if (WL_DBG_LEVEL > 0)
-#define WL_DBG_ESTR_MAX	50
-static s8 wl_dbg_estr[][WL_DBG_ESTR_MAX] = {
-	"SET_SSID", "JOIN", "START", "AUTH", "AUTH_IND",
-	"DEAUTH", "DEAUTH_IND", "ASSOC", "ASSOC_IND", "REASSOC",
-	"REASSOC_IND", "DISASSOC", "DISASSOC_IND", "QUIET_START", "QUIET_END",
-	"BEACON_RX", "LINK", "MIC_ERROR", "NDIS_LINK", "ROAM",
-	"TXFAIL", "PMKID_CACHE", "RETROGRADE_TSF", "PRUNE", "AUTOAUTH",
-	"EAPOL_MSG", "SCAN_COMPLETE", "ADDTS_IND", "DELTS_IND", "BCNSENT_IND",
-	"BCNRX_MSG", "BCNLOST_MSG", "ROAM_PREP", "PFN_NET_FOUND",
-	"PFN_NET_LOST",
-	"RESET_COMPLETE", "JOIN_START", "ROAM_START", "ASSOC_START",
-	"IBSS_ASSOC",
-	"RADIO", "PSM_WATCHDOG", "WLC_E_CCX_ASSOC_START", "WLC_E_CCX_ASSOC_ABORT",
-	"PROBREQ_MSG",
-	"SCAN_CONFIRM_IND", "PSK_SUP", "COUNTRY_CODE_CHANGED",
-	"EXCEEDED_MEDIUM_TIME", "ICV_ERROR",
-	"UNICAST_DECODE_ERROR", "MULTICAST_DECODE_ERROR", "TRACE",
-	"WLC_E_BTA_HCI_EVENT", "IF", "WLC_E_P2P_DISC_LISTEN_COMPLETE",
-	"RSSI", "PFN_SCAN_COMPLETE", "WLC_E_EXTLOG_MSG",
-	"ACTION_FRAME", "ACTION_FRAME_COMPLETE", "WLC_E_PRE_ASSOC_IND",
-	"WLC_E_PRE_REASSOC_IND", "WLC_E_CHANNEL_ADOPTED", "WLC_E_AP_STARTED",
-	"WLC_E_DFS_AP_STOP", "WLC_E_DFS_AP_RESUME", "WLC_E_WAI_STA_EVENT",
-	"WLC_E_WAI_MSG", "WLC_E_ESCAN_RESULT", "WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE",
-	"WLC_E_PROBRESP_MSG", "WLC_E_P2P_PROBREQ_MSG", "WLC_E_DCS_REQUEST", "WLC_E_FIFO_CREDIT_MAP",
-	"WLC_E_ACTION_FRAME_RX", "WLC_E_WAKE_EVENT", "WLC_E_RM_COMPLETE"
-};
-#endif				/* WL_DBG_LEVEL */
-
-#define CHAN2G(_channel, _freq, _flags) {			\
-	.band			= IEEE80211_BAND_2GHZ,		\
-	.center_freq		= (_freq),			\
-	.hw_value		= (_channel),			\
-	.flags			= (_flags),			\
-	.max_antenna_gain	= 0,				\
-	.max_power		= 30,				\
-}
-
-#define CHAN5G(_channel, _flags) {				\
-	.band			= IEEE80211_BAND_5GHZ,		\
-	.center_freq		= 5000 + (5 * (_channel)),	\
-	.hw_value		= (_channel),			\
-	.flags			= (_flags),			\
-	.max_antenna_gain	= 0,				\
-	.max_power		= 30,				\
-}
-
-#define RATE_TO_BASE100KBPS(rate)   (((rate) * 10) / 2)
-#define RATETAB_ENT(_rateid, _flags) \
-	{								\
-		.bitrate	= RATE_TO_BASE100KBPS(_rateid),     \
-		.hw_value	= (_rateid),			    \
-		.flags	  = (_flags),			     \
-	}
-
-static struct ieee80211_rate __wl_rates[] = {
-	RATETAB_ENT(WLC_RATE_1M, 0),
-	RATETAB_ENT(WLC_RATE_2M, IEEE80211_RATE_SHORT_PREAMBLE),
-	RATETAB_ENT(WLC_RATE_5M5, IEEE80211_RATE_SHORT_PREAMBLE),
-	RATETAB_ENT(WLC_RATE_11M, IEEE80211_RATE_SHORT_PREAMBLE),
-	RATETAB_ENT(WLC_RATE_6M, 0),
-	RATETAB_ENT(WLC_RATE_9M, 0),
-	RATETAB_ENT(WLC_RATE_12M, 0),
-	RATETAB_ENT(WLC_RATE_18M, 0),
-	RATETAB_ENT(WLC_RATE_24M, 0),
-	RATETAB_ENT(WLC_RATE_36M, 0),
-	RATETAB_ENT(WLC_RATE_48M, 0),
-	RATETAB_ENT(WLC_RATE_54M, 0)
-};
-
-#define wl_a_rates		(__wl_rates + 4)
-#define wl_a_rates_size	8
-#define wl_g_rates		(__wl_rates + 0)
-#define wl_g_rates_size	12
-
-static struct ieee80211_channel __wl_2ghz_channels[] = {
-	CHAN2G(1, 2412, 0),
-	CHAN2G(2, 2417, 0),
-	CHAN2G(3, 2422, 0),
-	CHAN2G(4, 2427, 0),
-	CHAN2G(5, 2432, 0),
-	CHAN2G(6, 2437, 0),
-	CHAN2G(7, 2442, 0),
-	CHAN2G(8, 2447, 0),
-	CHAN2G(9, 2452, 0),
-	CHAN2G(10, 2457, 0),
-	CHAN2G(11, 2462, 0),
-	CHAN2G(12, 2467, 0),
-	CHAN2G(13, 2472, 0),
-	CHAN2G(14, 2484, 0)
-};
-
-static struct ieee80211_channel __wl_5ghz_a_channels[] = {
-	CHAN5G(34, 0), CHAN5G(36, 0),
-	CHAN5G(38, 0), CHAN5G(40, 0),
-	CHAN5G(42, 0), CHAN5G(44, 0),
-	CHAN5G(46, 0), CHAN5G(48, 0),
-	CHAN5G(52, 0), CHAN5G(56, 0),
-	CHAN5G(60, 0), CHAN5G(64, 0),
-	CHAN5G(100, 0), CHAN5G(104, 0),
-	CHAN5G(108, 0), CHAN5G(112, 0),
-	CHAN5G(116, 0), CHAN5G(120, 0),
-	CHAN5G(124, 0), CHAN5G(128, 0),
-	CHAN5G(132, 0), CHAN5G(136, 0),
-	CHAN5G(140, 0), CHAN5G(149, 0),
-	CHAN5G(153, 0), CHAN5G(157, 0),
-	CHAN5G(161, 0), CHAN5G(165, 0)
-};
-
-static struct ieee80211_supported_band __wl_band_2ghz = {
-	.band = IEEE80211_BAND_2GHZ,
-	.channels = __wl_2ghz_channels,
-	.n_channels = ARRAY_SIZE(__wl_2ghz_channels),
-	.bitrates = wl_g_rates,
-	.n_bitrates = wl_g_rates_size
-};
-
-static struct ieee80211_supported_band __wl_band_5ghz_a = {
-	.band = IEEE80211_BAND_5GHZ,
-	.channels = __wl_5ghz_a_channels,
-	.n_channels = ARRAY_SIZE(__wl_5ghz_a_channels),
-	.bitrates = wl_a_rates,
-	.n_bitrates = wl_a_rates_size
-};
-
-static const u32 __wl_cipher_suites[] = {
-	WLAN_CIPHER_SUITE_WEP40,
-	WLAN_CIPHER_SUITE_WEP104,
-	WLAN_CIPHER_SUITE_TKIP,
-	WLAN_CIPHER_SUITE_CCMP,
-	WLAN_CIPHER_SUITE_AES_CMAC
-};
-
-/* There isn't a lot of sense in it, but you can transmit anything you like */
-static const struct ieee80211_txrx_stypes
-wl_cfg80211_default_mgmt_stypes[NUM_NL80211_IFTYPES] = {
-	[NL80211_IFTYPE_ADHOC] = {
-		.tx = 0xffff,
-		.rx = BIT(IEEE80211_STYPE_ACTION >> 4)
-	},
-	[NL80211_IFTYPE_STATION] = {
-		.tx = 0xffff,
-		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
-		BIT(IEEE80211_STYPE_PROBE_REQ >> 4)
-	},
-	[NL80211_IFTYPE_AP] = {
-		.tx = 0xffff,
-		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
-		BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
-		BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
-		BIT(IEEE80211_STYPE_DISASSOC >> 4) |
-		BIT(IEEE80211_STYPE_AUTH >> 4) |
-		BIT(IEEE80211_STYPE_DEAUTH >> 4) |
-		BIT(IEEE80211_STYPE_ACTION >> 4)
-	},
-	[NL80211_IFTYPE_AP_VLAN] = {
-		/* copy AP */
-		.tx = 0xffff,
-		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
-		BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
-		BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
-		BIT(IEEE80211_STYPE_DISASSOC >> 4) |
-		BIT(IEEE80211_STYPE_AUTH >> 4) |
-		BIT(IEEE80211_STYPE_DEAUTH >> 4) |
-		BIT(IEEE80211_STYPE_ACTION >> 4)
-	},
-	[NL80211_IFTYPE_P2P_CLIENT] = {
-		.tx = 0xffff,
-		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
-		BIT(IEEE80211_STYPE_PROBE_REQ >> 4)
-	},
-	[NL80211_IFTYPE_P2P_GO] = {
-		.tx = 0xffff,
-		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
-		BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
-		BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
-		BIT(IEEE80211_STYPE_DISASSOC >> 4) |
-		BIT(IEEE80211_STYPE_AUTH >> 4) |
-		BIT(IEEE80211_STYPE_DEAUTH >> 4) |
-		BIT(IEEE80211_STYPE_ACTION >> 4)
-	}
-};
-
-static void swap_key_from_BE(struct wl_wsec_key *key)
-{
-	key->index = htod32(key->index);
-	key->len = htod32(key->len);
-	key->algo = htod32(key->algo);
-	key->flags = htod32(key->flags);
-	key->rxiv.hi = htod32(key->rxiv.hi);
-	key->rxiv.lo = htod16(key->rxiv.lo);
-	key->iv_initialized = htod32(key->iv_initialized);
-}
-
-static void swap_key_to_BE(struct wl_wsec_key *key)
-{
-	key->index = dtoh32(key->index);
-	key->len = dtoh32(key->len);
-	key->algo = dtoh32(key->algo);
-	key->flags = dtoh32(key->flags);
-	key->rxiv.hi = dtoh32(key->rxiv.hi);
-	key->rxiv.lo = dtoh16(key->rxiv.lo);
-	key->iv_initialized = dtoh32(key->iv_initialized);
-}
-
-/* For debug: Dump the contents of the encoded wps ie buffe */
-static void
-wl_validate_wps_ie(char *wps_ie, bool *pbc)
-{
-	#define WPS_IE_FIXED_LEN 6
-	u16 len = (u16) wps_ie[TLV_LEN_OFF];
-	u8 *subel = wps_ie+  WPS_IE_FIXED_LEN;
-	u16 subelt_id;
-	u16 subelt_len;
-	u16 val;
-	u8 *valptr = (uint8*) &val;
-
-	WL_DBG(("wps_ie len=%d\n", len));
-
-	len -= 4;	/* for the WPS IE's OUI, oui_type fields */
-
-	while (len >= 4) {		/* must have attr id, attr len fields */
-		valptr[0] = *subel++;
-		valptr[1] = *subel++;
-		subelt_id = HTON16(val);
-
-		valptr[0] = *subel++;
-		valptr[1] = *subel++;
-		subelt_len = HTON16(val);
-
-		len -= 4;			/* for the attr id, attr len fields */
-		len -= subelt_len;	/* for the remaining fields in this attribute */
-		WL_DBG((" subel=%p, subelt_id=0x%x subelt_len=%u\n",
-			subel, subelt_id, subelt_len));
-
-		if (subelt_id == WPS_ID_VERSION) {
-			WL_DBG(("  attr WPS_ID_VERSION: %u\n", *subel));
-		} else if (subelt_id == WPS_ID_REQ_TYPE) {
-			WL_DBG(("  attr WPS_ID_REQ_TYPE: %u\n", *subel));
-		} else if (subelt_id == WPS_ID_CONFIG_METHODS) {
-			valptr[0] = *subel;
-			valptr[1] = *(subel + 1);
-			WL_DBG(("  attr WPS_ID_CONFIG_METHODS: %x\n", HTON16(val)));
-		} else if (subelt_id == WPS_ID_DEVICE_NAME) {
-			char devname[100];
-			memcpy(devname, subel, subelt_len);
-			devname[subelt_len] = '\0';
-			WL_DBG(("  attr WPS_ID_DEVICE_NAME: %s (len %u)\n",
-				devname, subelt_len));
-		} else if (subelt_id == WPS_ID_DEVICE_PWD_ID) {
-			valptr[0] = *subel;
-			valptr[1] = *(subel + 1);
-			WL_DBG(("  attr WPS_ID_DEVICE_PWD_ID: %u\n", HTON16(val)));
-			*pbc = (HTON16(val) == DEV_PW_PUSHBUTTON) ? true : false;
-		} else if (subelt_id == WPS_ID_PRIM_DEV_TYPE) {
-			valptr[0] = *subel;
-			valptr[1] = *(subel + 1);
-			WL_DBG(("  attr WPS_ID_PRIM_DEV_TYPE: cat=%u \n", HTON16(val)));
-			valptr[0] = *(subel + 6);
-			valptr[1] = *(subel + 7);
-			WL_DBG(("  attr WPS_ID_PRIM_DEV_TYPE: subcat=%u\n", HTON16(val)));
-		} else if (subelt_id == WPS_ID_REQ_DEV_TYPE) {
-			valptr[0] = *subel;
-			valptr[1] = *(subel + 1);
-			WL_DBG(("  attr WPS_ID_REQ_DEV_TYPE: cat=%u\n", HTON16(val)));
-			valptr[0] = *(subel + 6);
-			valptr[1] = *(subel + 7);
-			WL_DBG(("  attr WPS_ID_REQ_DEV_TYPE: subcat=%u\n", HTON16(val)));
-		} else if (subelt_id == WPS_ID_SELECTED_REGISTRAR_CONFIG_METHODS) {
-			valptr[0] = *subel;
-			valptr[1] = *(subel + 1);
-			WL_DBG(("  attr WPS_ID_SELECTED_REGISTRAR_CONFIG_METHODS"
-				": cat=%u\n", HTON16(val)));
-		} else {
-			WL_DBG(("  unknown attr 0x%x\n", subelt_id));
-		}
-
-		subel += subelt_len;
-	}
-}
-
-static struct net_device* wl_cfg80211_add_monitor_if(char *name)
-{
-	int ret = 0;
-	struct net_device* ndev = NULL;
-
-	ret = dhd_add_monitor(name, &ndev);
-	WL_INFO(("wl_cfg80211_add_monitor_if net device returned: 0x%p\n", ndev));
-	return ndev;
-}
-
-static struct net_device *
-wl_cfg80211_add_virtual_iface(struct wiphy *wiphy, char *name,
-	enum nl80211_iftype type, u32 *flags,
-	struct vif_params *params)
-{
-	s32 err;
-	s32 timeout = -1;
-	s32 wlif_type = -1;
-	s32 index = 0;
-	s32 mode = 0;
-	chanspec_t chspec;
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	struct net_device *_ndev;
-	dhd_pub_t *dhd = (dhd_pub_t *)(wl->pub);
-	int (*net_attach)(dhd_pub_t *dhdp, int ifidx);
-	WL_DBG(("if name: %s, type: %d\n", name, type));
-	switch (type) {
-	case NL80211_IFTYPE_ADHOC:
-	case NL80211_IFTYPE_AP_VLAN:
-	case NL80211_IFTYPE_WDS:
-	case NL80211_IFTYPE_MESH_POINT:
-		WL_ERR(("Unsupported interface type\n"));
-		mode = WL_MODE_IBSS;
-		return NULL;
-	case NL80211_IFTYPE_MONITOR:
-		return wl_cfg80211_add_monitor_if(name);
-	case NL80211_IFTYPE_P2P_CLIENT:
-	case NL80211_IFTYPE_STATION:
-		wlif_type = WL_P2P_IF_CLIENT;
-		mode = WL_MODE_BSS;
-		break;
-	case NL80211_IFTYPE_P2P_GO:
-	case NL80211_IFTYPE_AP:
-		wlif_type = WL_P2P_IF_GO;
-		mode = WL_MODE_AP;
-		break;
-	default:
-		WL_ERR(("Unsupported interface type\n"));
-		return NULL;
-		break;
-	}
-
-	if (!name) {
-		WL_ERR(("name is NULL\n"));
-		return NULL;
-	}
-	if (wl->p2p_supported && (wlif_type != -1)) {
-		if (wl_get_p2p_status(wl, IF_DELETING) == 1) {
-			/* wait till IF_DEL is complete
-			 * release the lock for the unregister to proceed
-			 */
-			rtnl_unlock();
-			WL_INFO(("%s: Released the lock and wait till IF_DEL is complete\n",
-				__func__));
-			timeout = wait_event_interruptible_timeout(wl->dongle_event_wait,
-				(wl_get_p2p_status(wl, IF_DELETING) == false),
-				msecs_to_jiffies(MAX_WAIT_TIME));
-
-			/* put back the rtnl_lock again */
-			rtnl_lock();
-			if (timeout > 0) {
-				WL_ERR(("IF DEL is Success\n"));
-
-			} else {
-				WL_ERR(("%s: timeount < 0, return -EAGAIN\n", __func__));
-				return ERR_PTR(-EAGAIN);
-			}
-		}
-		if (!p2p_on(wl) && strstr(name, WL_P2P_INTERFACE_PREFIX)) {
-			p2p_on(wl) = true;
-			wl_cfgp2p_set_firm_p2p(wl);
-			wl_cfgp2p_init_discovery(wl);
-		}
-
-		memset(wl->p2p->vir_ifname, 0, IFNAMSIZ);
-		strncpy(wl->p2p->vir_ifname, name, IFNAMSIZ - 1);
-		wl_cfgp2p_generate_bss_mac(&dhd->mac, &wl->p2p->dev_addr, &wl->p2p->int_addr);
-
-		/* Temporary use channel 11, in case GO will be changed with set_channel API  */
-		chspec = wf_chspec_aton(WL_P2P_TEMP_CHAN);
-
-		/* For P2P mode, use P2P-specific driver features to create the
-		 * bss: "wl p2p_ifadd"
-		 */
-		wl_set_p2p_status(wl, IF_ADD);
-		err = wl_cfgp2p_ifadd(wl, &wl->p2p->int_addr, htod32(wlif_type), chspec);
-
-		if (unlikely(err))
-			return ERR_PTR(-ENOMEM);
-
-		timeout = wait_event_interruptible_timeout(wl->dongle_event_wait,
-			(wl_get_p2p_status(wl, IF_ADD) == false),
-			msecs_to_jiffies(MAX_WAIT_TIME));
-		if (timeout > 0 && (!wl_get_p2p_status(wl, IF_ADD))) {
-
-			struct wireless_dev *vwdev;
-			vwdev = kzalloc(sizeof(*vwdev), GFP_KERNEL);
-			if (unlikely(!vwdev)) {
-				WL_ERR(("Could not allocate wireless device\n"));
-				return ERR_PTR(-ENOMEM);
-			}
-			vwdev->wiphy = wl->wdev->wiphy;
-			WL_INFO((" virtual interface(%s) is created memalloc done \n",
-			wl->p2p->vir_ifname));
-			index = alloc_idx_vwdev(wl);
-			wl->vwdev[index] = vwdev;
-			vwdev->iftype =
-				(wlif_type == WL_P2P_IF_CLIENT) ? NL80211_IFTYPE_STATION
-				: NL80211_IFTYPE_AP;
-			_ndev =  wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION);
-			_ndev->ieee80211_ptr = vwdev;
-			SET_NETDEV_DEV(_ndev, wiphy_dev(vwdev->wiphy));
-			vwdev->netdev = _ndev;
-			wl_set_drv_status(wl, READY);
-			wl->p2p->vif_created = true;
-			set_mode_by_netdev(wl, _ndev, mode);
-			net_attach =  wl_to_p2p_bss_private(wl, P2PAPI_BSSCFG_CONNECTION);
-			rtnl_unlock();
-			if (net_attach && !net_attach(dhd, _ndev->ifindex))
-				WL_DBG((" virtual interface(%s) is "
-					"created net attach done\n", wl->p2p->vir_ifname));
-			else {
-				rtnl_lock();
-				goto fail;
-			}
-			rtnl_lock();
-			return _ndev;
-
-		} else {
-			wl_clr_p2p_status(wl, IF_ADD);
-			WL_ERR((" virtual interface(%s) is not created \n", wl->p2p->vir_ifname));
-			memset(wl->p2p->vir_ifname, '\0', IFNAMSIZ);
-			wl->p2p->vif_created = false;
-		}
-	}
-fail:
-	return ERR_PTR(-ENODEV);
-}
-
-
-static s32
-wl_cfg80211_del_virtual_iface(struct wiphy *wiphy, struct net_device *dev)
-{
-	struct ether_addr p2p_mac;
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	s32 timeout = -1;
-	s32 ret = 0;
-	WL_DBG(("Enter\n"));
-	if (wl->p2p_supported) {
-		memcpy(p2p_mac.octet, wl->p2p->int_addr.octet, ETHER_ADDR_LEN);
-		if (wl->p2p->vif_created) {
-			if (wl_get_drv_status(wl, SCANNING)) {
-				wl_cfg80211_scan_abort(wl, dev);
-			}
-
-			ret = wl_cfgp2p_ifdel(wl, &p2p_mac);
-			wl_set_p2p_status(wl, IF_DELETING);
-			if (ret) {
-				/* Firmware could not delete the interface so we will not get WLC_E_IF event for cleaning the dhd virtual nw interace
-				 * So lets do it here. Failures from fw will ensure the application to do ifconfig <inter> down and up sequnce, which will reload the fw
-				* however we should cleanup the linux network virtual interfaces
-				*/
-				dhd_pub_t *dhd = (dhd_pub_t *)(wl->pub);
-				WL_ERR(("Firmware returned an error from p2p_ifdel, try to remove linux virtual network interface dev->name %s\n", dev->name));
-				dhd_del_if(dhd->info, dhd_net2idx(dhd->info, dev));
-			}
-
-			/* Wait for any pending scan req to get aborted from the sysioc context */
-			timeout = wait_event_interruptible_timeout(wl->dongle_event_wait,
-				(wl->scan_request == false),
-				msecs_to_jiffies(MAX_WAIT_TIME));
-
-			if (timeout > 0 && (!wl->scan_request)) {
-				WL_DBG(("IFDEL Operations Done"));
-			} else {
-				WL_ERR(("IFDEL didn't complete properly"));
-			}
-			ret = dhd_del_monitor(dev);
-		}
-	}
-	return ret;
-}
-
-static s32
-wl_cfg80211_change_virtual_iface(struct wiphy *wiphy, struct net_device *ndev,
-	enum nl80211_iftype type, u32 *flags,
-	struct vif_params *params)
-{
-	s32 ap = 0;
-	s32 infra = 0;
-	s32 err = BCME_OK;
-	s32 timeout = -1;
-	s32 wlif_type;
-	s32 mode = 0;
-	chanspec_t chspec;
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	WL_DBG(("Enter \n"));
-	switch (type) {
-	case NL80211_IFTYPE_MONITOR:
-	case NL80211_IFTYPE_WDS:
-	case NL80211_IFTYPE_MESH_POINT:
-		ap = 1;
-		WL_ERR(("type (%d) : currently we do not support this type\n",
-			type));
-		break;
-	case NL80211_IFTYPE_ADHOC:
-		mode = WL_MODE_IBSS;
-		break;
-	case NL80211_IFTYPE_STATION:
-	case NL80211_IFTYPE_P2P_CLIENT:
-		mode = WL_MODE_BSS;
-		infra = 1;
-		break;
-	case NL80211_IFTYPE_AP:
-	case NL80211_IFTYPE_AP_VLAN:
-	case NL80211_IFTYPE_P2P_GO:
-		mode = WL_MODE_AP;
-		ap = 1;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-
-	if (ap) {
-		set_mode_by_netdev(wl, ndev, mode);
-		if (wl->p2p_supported && wl->p2p->vif_created) {
-			WL_DBG(("p2p_vif_created (%d) p2p_on (%d)\n", wl->p2p->vif_created,
-			p2p_on(wl)));
-			chspec = wf_chspec_aton(WL_P2P_TEMP_CHAN);
-			wlif_type = ap ? WL_P2P_IF_GO : WL_P2P_IF_CLIENT;
-			WL_ERR(("%s : ap (%d), infra (%d), iftype: (%d)\n",
-				ndev->name, ap, infra, type));
-			wl_set_p2p_status(wl, IF_CHANGING);
-			wl_clr_p2p_status(wl, IF_CHANGED);
-			err = wl_cfgp2p_ifchange(wl, &wl->p2p->int_addr, htod32(wlif_type), chspec);
-			timeout = wait_event_interruptible_timeout(wl->dongle_event_wait,
-				(wl_get_p2p_status(wl, IF_CHANGED) == true),
-				msecs_to_jiffies(MAX_WAIT_TIME));
-			set_mode_by_netdev(wl, ndev, mode);
-			wl_clr_p2p_status(wl, IF_CHANGING);
-			wl_clr_p2p_status(wl, IF_CHANGED);
-		} else if (ndev == wl_to_prmry_ndev(wl) &&
-			!wl_get_drv_status(wl, AP_CREATED)) {
-			wl_set_drv_status(wl, AP_CREATING);
-			if (!wl->ap_info &&
-				!(wl->ap_info = kzalloc(sizeof(struct ap_info), GFP_KERNEL))) {
-				WL_ERR(("struct ap_saved_ie allocation failed\n"));
-				return -ENOMEM;
-			}
-		} else {
-			WL_ERR(("Cannot change the interface for GO or SOFTAP\n"));
-			return -EINVAL;
-		}
-	}
-
-	ndev->ieee80211_ptr->iftype = type;
-	return 0;
-}
-
-s32
-wl_cfg80211_notify_ifadd(struct net_device *net, s32 idx, s32 bssidx,
-int (*_net_attach)(dhd_pub_t *dhdp, int ifidx))
-{
-	struct wl_priv *wl = wlcfg_drv_priv;
-	s32 ret = BCME_OK;
-	if (!net) {
-		WL_ERR(("net is NULL\n"));
-		return 0;
-	}
-	if (wl->p2p_supported) {
-		WL_DBG(("IF_ADD event called from dongle, old interface name: %s,"
-			"new name: %s\n", net->name, wl->p2p->vir_ifname));
-		/* Assign the net device to CONNECT BSSCFG */
-		strncpy(net->name, wl->p2p->vir_ifname, IFNAMSIZ - 1);
-		wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION) = net;
-		wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_CONNECTION) = bssidx;
-		wl_to_p2p_bss_private(wl, P2PAPI_BSSCFG_CONNECTION) = _net_attach;
-		net->ifindex = idx;
-		wl_clr_p2p_status(wl, IF_ADD);
-
-		wake_up_interruptible(&wl->dongle_event_wait);
-	}
-	return ret;
-}
-
-s32
-wl_cfg80211_ifdel_ops(struct net_device *net)
-{
-	struct wl_priv *wl = wlcfg_drv_priv;
-
-	if (!net || !net->name) {
-		WL_DBG(("net is NULL\n"));
-		return 0;
-	}
-
-	if ((wl->p2p->vif_created) && (wl->scan_request)) {
-
-		/* Abort any pending scan requests */
-		wl->escan_info.escan_state = WL_ESCAN_STATE_IDLE;
-		rtnl_lock();
-		WL_INFO(("ESCAN COMPLETED\n"));
-		wl_notify_escan_complete(wl, true);
-		rtnl_unlock();
-	}
-
-	/* Wake up any waiting thread */
-	wake_up_interruptible(&wl->dongle_event_wait);
-
-	return 0;
-}
-
-s32
-wl_cfg80211_notify_ifdel(struct net_device *net)
-{
-	struct wl_priv *wl = wlcfg_drv_priv;
-
-
-	if (wl->p2p->vif_created) {
-		s32 index = 0;
-
-		WL_DBG(("IF_DEL event called from dongle, net %x, vif name: %s\n",
-			(unsigned int)net, wl->p2p->vir_ifname));
-
-		memset(wl->p2p->vir_ifname, '\0', IFNAMSIZ);
-		index = wl_cfgp2p_find_idx(wl, net);
-		wl_to_p2p_bss_ndev(wl, index) = NULL;
-		wl_to_p2p_bss_bssidx(wl, index) = 0;
-		wl->p2p->vif_created = false;
-		set_mode_by_netdev(wl, net, -1);
-		wl_cfgp2p_clear_management_ie(wl,
-			index);
-		index = get_idx_vwdev_by_netdev(wl, net);
-		WL_DBG(("index : %d\n", index));
-		if (index >= 0) {
-				free_vwdev_by_index(wl, index);
-		}
-	}
-
-	wl_clr_p2p_status(wl, IF_DELETING);
-
-	/* Wake up any waiting thread */
-	wake_up_interruptible(&wl->dongle_event_wait);
-
-	return 0;
-}
-
-s32
-wl_cfg80211_is_progress_ifadd(void)
-{
-	s32 is_progress = 0;
-	struct wl_priv *wl = wlcfg_drv_priv;
-	if (wl_get_p2p_status(wl, IF_ADD))
-		is_progress = 1;
-	return is_progress;
-}
-
-s32
-wl_cfg80211_is_progress_ifchange(void)
-{
-	s32 is_progress = 0;
-	struct wl_priv *wl = wlcfg_drv_priv;
-	if (wl_get_p2p_status(wl, IF_CHANGING))
-		is_progress = 1;
-	return is_progress;
-}
-
-
-s32
-wl_cfg80211_notify_ifchange(void)
-{
-	struct wl_priv *wl = wlcfg_drv_priv;
-	if (wl_get_p2p_status(wl, IF_CHANGING)) {
-		wl_set_p2p_status(wl, IF_CHANGED);
-		wake_up_interruptible(&wl->dongle_event_wait);
-	}
-	return 0;
-}
-
-static void wl_scan_prep(struct wl_scan_params *params, struct cfg80211_scan_request *request)
-{
-	u32 n_ssids = request->n_ssids;
-	u32 n_channels = request->n_channels;
-	u16 channel;
-	chanspec_t chanspec;
-	s32 i, offset;
-	char *ptr;
-	wlc_ssid_t ssid;
-
-	memcpy(&params->bssid, &ether_bcast, ETHER_ADDR_LEN);
-	params->bss_type = DOT11_BSSTYPE_ANY;
-	params->scan_type = 0;
-	params->nprobes = -1;
-	params->active_time = -1;
-	params->passive_time = -1;
-	params->home_time = -1;
-	params->channel_num = 0;
-	memset(&params->ssid, 0, sizeof(wlc_ssid_t));
-
-	WL_SCAN(("Preparing Scan request\n"));
-	WL_SCAN(("nprobes=%d\n", params->nprobes));
-	WL_SCAN(("active_time=%d\n", params->active_time));
-	WL_SCAN(("passive_time=%d\n", params->passive_time));
-	WL_SCAN(("home_time=%d\n", params->home_time));
-	WL_SCAN(("scan_type=%d\n", params->scan_type));
-
-	params->nprobes = htod32(params->nprobes);
-	params->active_time = htod32(params->active_time);
-	params->passive_time = htod32(params->passive_time);
-	params->home_time = htod32(params->home_time);
-
-	/* Copy channel array if applicable */
-	WL_SCAN(("### List of channelspecs to scan ###\n"));
-	if (n_channels > 0) {
-		for (i = 0; i < n_channels; i++) {
-			chanspec = 0;
-			channel = ieee80211_frequency_to_channel(request->channels[i]->center_freq);
-			if (request->channels[i]->band == IEEE80211_BAND_2GHZ)
-				chanspec |= WL_CHANSPEC_BAND_2G;
-			else
-				chanspec |= WL_CHANSPEC_BAND_5G;
-
-			if (request->channels[i]->flags & IEEE80211_CHAN_NO_HT40) {
-				chanspec |= WL_CHANSPEC_BW_20;
-				chanspec |= WL_CHANSPEC_CTL_SB_NONE;
-			} else {
-				chanspec |= WL_CHANSPEC_BW_40;
-				if (request->channels[i]->flags & IEEE80211_CHAN_NO_HT40PLUS)
-					chanspec |= WL_CHANSPEC_CTL_SB_LOWER;
-				else
-					chanspec |= WL_CHANSPEC_CTL_SB_UPPER;
-			}
-
-			params->channel_list[i] = channel;
-			params->channel_list[i] &= WL_CHANSPEC_CHAN_MASK;
-			params->channel_list[i] |= chanspec;
-			WL_SCAN(("Chan : %d, Channel spec: %x \n",
-			channel, params->channel_list[i]));
-			params->channel_list[i] = htod16(params->channel_list[i]);
-		}
-	} else {
-		WL_SCAN(("Scanning all channels\n"));
-	}
-
-	/* Copy ssid array if applicable */
-	WL_SCAN(("### List of SSIDs to scan ###\n"));
-	if (n_ssids > 0) {
-		offset = offsetof(wl_scan_params_t, channel_list) + n_channels * sizeof(u16);
-		offset = roundup(offset, sizeof(u32));
-		ptr = (char*)params + offset;
-		for (i = 0; i < n_ssids; i++) {
-			memset(&ssid, 0, sizeof(wlc_ssid_t));
-			ssid.SSID_len = request->ssids[i].ssid_len;
-			memcpy(ssid.SSID, request->ssids[i].ssid, ssid.SSID_len);
-			if (!ssid.SSID_len)
-				WL_SCAN(("%d: Broadcast scan\n", i));
-			else
-				WL_SCAN(("%d: scan  for  %s size =%d\n", i,
-				ssid.SSID, ssid.SSID_len));
-			memcpy(ptr, &ssid, sizeof(wlc_ssid_t));
-			ptr += sizeof(wlc_ssid_t);
-		}
-	} else {
-		WL_SCAN(("Broadcast scan\n"));
-	}
-	/* Adding mask to channel numbers */
-	params->channel_num =
-	        htod32((n_ssids << WL_SCAN_PARAMS_NSSID_SHIFT) |
-	               (n_channels & WL_SCAN_PARAMS_COUNT_MASK));
-}
-
-static s32
-wl_run_iscan(struct wl_iscan_ctrl *iscan, struct cfg80211_scan_request *request, u16 action)
-{
-	u32 n_channels;
-	u32 n_ssids;
-	s32 params_size =
-	    (WL_SCAN_PARAMS_FIXED_SIZE + offsetof(wl_iscan_params_t, params));
-	struct wl_iscan_params *params;
-	s32 err = 0;
-
-	if (request != NULL) {
-		n_channels = request->n_channels;
-		n_ssids = request->n_ssids;
-		/* Allocate space for populating ssids in wl_iscan_params struct */
-		if (n_channels % 2)
-			/* If n_channels is odd, add a padd of u16 */
-			params_size += sizeof(u16) * (n_channels + 1);
-		else
-			params_size += sizeof(u16) * n_channels;
-
-		/* Allocate space for populating ssids in wl_iscan_params struct */
-		params_size += sizeof(struct wlc_ssid) * n_ssids;
-	}
-	params = (struct wl_iscan_params *)kzalloc(params_size, GFP_KERNEL);
-	if (!params) {
-		err = -ENOMEM;
-		goto done;
-	}
-
-	if (request != NULL)
-		wl_scan_prep(&params->params, request);
-
-	params->version = htod32(ISCAN_REQ_VERSION);
-	params->action = htod16(action);
-	params->scan_duration = htod16(0);
-
-	if (params_size + sizeof("iscan") >= WLC_IOCTL_MEDLEN) {
-		WL_ERR(("ioctl buffer length is not sufficient\n"));
-		err = -ENOMEM;
-		goto done;
-	}
-	err = wldev_iovar_setbuf(iscan->dev, "iscan", params, params_size,
-		iscan->ioctl_buf, WLC_IOCTL_MEDLEN);
-	if (unlikely(err)) {
-		if (err == -EBUSY) {
-			WL_INFO(("system busy : iscan canceled\n"));
-		} else {
-			WL_ERR(("error (%d)\n", err));
-		}
-	}
-	kfree(params);
-done:
-	return err;
-}
-
-static s32 wl_do_iscan(struct wl_priv *wl, struct cfg80211_scan_request *request)
-{
-	struct wl_iscan_ctrl *iscan = wl_to_iscan(wl);
-	struct net_device *ndev = wl_to_prmry_ndev(wl);
-	s32 passive_scan;
-	s32 err = 0;
-
-	iscan->state = WL_ISCAN_STATE_SCANING;
-
-	passive_scan = wl->active_scan ? 0 : 1;
-	err = wldev_ioctl(ndev, WLC_SET_PASSIVE_SCAN,
-		&passive_scan, sizeof(passive_scan), false);
-	if (unlikely(err)) {
-		WL_DBG(("error (%d)\n", err));
-		return err;
-	}
-	wl->iscan_kickstart = true;
-	wl_run_iscan(iscan, request, WL_SCAN_ACTION_START);
-	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
-	iscan->timer_on = 1;
-
-	return err;
-}
-
-static s32
-wl_run_escan(struct wl_priv *wl, struct net_device *ndev,
-	struct cfg80211_scan_request *request, uint16 action)
-{
-	s32 err = BCME_OK;
-	u32 n_channels;
-	u32 n_ssids;
-	s32 params_size = (WL_SCAN_PARAMS_FIXED_SIZE + OFFSETOF(wl_escan_params_t, params));
-	wl_escan_params_t *params;
-	struct cfg80211_scan_request *scan_request = wl->scan_request;
-	u32 num_chans = 0;
-	s32 search_state = WL_P2P_DISC_ST_SCAN;
-	u32 i;
-	u16 *default_chan_list = NULL;
-	WL_DBG(("Enter \n"));
-
-
-	if (!wl->p2p_supported || ((ndev == wl_to_prmry_ndev(wl)) &&
-		!p2p_scan(wl))) {
-		/* LEGACY SCAN TRIGGER */
-		WL_SCAN((" LEGACY E-SCAN START\n"));
-
-		if (request != NULL) {
-			n_channels = request->n_channels;
-			n_ssids = request->n_ssids;
-			/* Allocate space for populating ssids in wl_iscan_params struct */
-			if (n_channels % 2)
-				/* If n_channels is odd, add a padd of u16 */
-				params_size += sizeof(u16) * (n_channels + 1);
-			else
-				params_size += sizeof(u16) * n_channels;
-
-			/* Allocate space for populating ssids in wl_iscan_params struct */
-			params_size += sizeof(struct wlc_ssid) * n_ssids;
-		}
-		params = (wl_escan_params_t *) kzalloc(params_size, GFP_KERNEL);
-		if (params == NULL) {
-			err = -ENOMEM;
-			goto exit;
-		}
-
-		if (request != NULL)
-			wl_scan_prep(&params->params, request);
-		params->version = htod32(ESCAN_REQ_VERSION);
-		params->action =  htod16(action);
-		params->sync_id = htod16(0x1234);
-		if (params_size + sizeof("escan") >= WLC_IOCTL_MEDLEN) {
-			WL_ERR(("ioctl buffer length not sufficient\n"));
-			err = -ENOMEM;
-			goto exit;
-		}
-		wldev_iovar_setbuf(ndev, "escan", params, params_size,
-			wl->escan_ioctl_buf, WLC_IOCTL_MEDLEN);
-		kfree(params);
-	}
-	else if (p2p_on(wl) && p2p_scan(wl)) {
-		/* P2P SCAN TRIGGER */
-		if (scan_request && scan_request->n_channels) {
-			num_chans = scan_request->n_channels;
-			WL_SCAN((" chann number : %d\n", num_chans));
-			default_chan_list = kzalloc(num_chans * sizeof(*default_chan_list),
-				GFP_KERNEL);
-			if (default_chan_list == NULL) {
-				WL_ERR(("channel list allocation failed \n"));
-				err = -ENOMEM;
-				goto exit;
-			}
-			for (i = 0; i < num_chans; i++)
-			{
-				default_chan_list[i] =
-				ieee80211_frequency_to_channel(
-					scan_request->channels[i]->center_freq);
-			}
-			if (num_chans == 3 && (
-						(default_chan_list[0] == SOCIAL_CHAN_1) &&
-						(default_chan_list[1] == SOCIAL_CHAN_2) &&
-						(default_chan_list[2] == SOCIAL_CHAN_3))) {
-				/* SOCIAL CHANNELS 1, 6, 11 */
-				search_state = WL_P2P_DISC_ST_SEARCH;
-				WL_INFO(("P2P SEARCH PHASE START \n"));
-			} else {
-				WL_INFO(("P2P SCAN STATE START \n"));
-			}
-
-		}
-		err = wl_cfgp2p_escan(wl, ndev, wl->active_scan, num_chans, default_chan_list,
-			search_state, action,
-			wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
-		kfree(default_chan_list);
-	}
-exit:
-	return err;
-}
-
-
-static s32
-wl_do_escan(struct wl_priv *wl, struct wiphy *wiphy, struct net_device *ndev,
-	struct cfg80211_scan_request *request)
-{
-	s32 err = BCME_OK;
-	s32 passive_scan;
-	wl_scan_results_t *results;
-	WL_SCAN(("Enter \n"));
-
-	wl->escan_info.wiphy = wiphy;
-	wl->escan_info.escan_state = WL_ESCAN_STATE_SCANING;
-	passive_scan = wl->active_scan ? 0 : 1;
-	err = wldev_ioctl(ndev, WLC_SET_PASSIVE_SCAN,
-		&passive_scan, sizeof(passive_scan), false);
-	if (unlikely(err)) {
-		WL_DBG(("error (%d)\n", err));
-		return err;
-	}
-	results = (wl_scan_results_t *) wl->escan_info.escan_buf;
-	results->version = 0;
-	results->count = 0;
-	results->buflen = WL_SCAN_RESULTS_FIXED_SIZE;
-
-	wl_run_escan(wl, ndev, request, WL_SCAN_ACTION_START);
-	return err;
-}
-
-static s32
-__wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
-	struct cfg80211_scan_request *request,
-	struct cfg80211_ssid *this_ssid)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	struct cfg80211_ssid *ssids;
-	struct wl_scan_req *sr = wl_to_sr(wl);
-	s32 passive_scan;
-	bool iscan_req;
-	bool escan_req;
-	bool spec_scan;
-	bool p2p_ssid;
-	s32 err = 0;
-	s32 i;
-
-	if (unlikely(wl_get_drv_status(wl, SCANNING))) {
-		WL_ERR(("Scanning already : status (%d)\n", (int)wl->status));
-		return -EAGAIN;
-	}
-	if (unlikely(wl_get_drv_status(wl, SCAN_ABORTING))) {
-		WL_ERR(("Scanning being aborted : status (%d)\n",
-			(int)wl->status));
-		return -EAGAIN;
-	}
-	if (request->n_ssids > WL_SCAN_PARAMS_SSID_MAX) {
-		WL_ERR(("n_ssids > WL_SCAN_PARAMS_SSID_MAX\n"));
-		return -EOPNOTSUPP;
-	}
-
-	WL_DBG(("wiphy (%p)\n", wiphy));
-
-	iscan_req = false;
-	spec_scan = false;
-	if (request) {		/* scan bss */
-		ssids = request->ssids;
-		if (wl->iscan_on && (!ssids || !ssids->ssid_len || request->n_ssids != 1)) {
-			iscan_req = true;
-		} else if (wl->escan_on) {
-			escan_req = true;
-			p2p_ssid = false;
-			for (i = 0; i < request->n_ssids; i++) {
-				if (ssids[i].ssid_len && IS_P2P_SSID(ssids[i].ssid)) {
-					p2p_ssid = true;
-					break;
-				}
-			}
-			if (p2p_ssid) {
-				if (wl->p2p_supported) {
-					/* p2p scan trigger */
-					if (p2p_on(wl) == false) {
-						/* p2p on at the first time */
-						p2p_on(wl) = true;
-						wl_cfgp2p_set_firm_p2p(wl);
-					}
-					p2p_scan(wl) = true;
-				}
-			} else {
-				/* legacy scan trigger
-				 * So, we have to disable p2p discovery if p2p discovery is on
-				 */
-				if (wl->p2p_supported) {
-					p2p_scan(wl) = false;
-					/* If Netdevice is not equals to primary and p2p is on
-					*  , we will do p2p scan using P2PAPI_BSSCFG_DEVICE.
-					*/
-					if (p2p_on(wl) && (ndev != wl_to_prmry_ndev(wl)))
-						p2p_scan(wl) = true;
-
-					if (p2p_scan(wl) == false) {
-						if (wl_get_p2p_status(wl, DISCOVERY_ON)) {
-							err = wl_cfgp2p_discover_enable_search(wl,
-							false);
-							if (unlikely(err)) {
-								goto scan_out;
-							}
-
-						}
-					}
-				}
-			}
-		}
-	} else {		/* scan in ibss */
-		/* we don't do iscan in ibss */
-		ssids = this_ssid;
-	}
-	wl->scan_request = request;
-	wl_set_drv_status(wl, SCANNING);
-	if (iscan_req) {
-		err = wl_do_iscan(wl, request);
-		if (likely(!err))
-			return err;
-		else
-			goto scan_out;
-	} else if (escan_req) {
-		if (wl->p2p_supported) {
-			if (p2p_on(wl) && p2p_scan(wl)) {
-
-				err = wl_cfgp2p_enable_discovery(wl, ndev,
-				request->ie, request->ie_len);
-
-				if (unlikely(err)) {
-					goto scan_out;
-				}
-			}
-		}
-		err = wl_do_escan(wl, wiphy, ndev, request);
-		if (likely(!err))
-			return err;
-		else
-			goto scan_out;
-
-
-	} else {
-		memset(&sr->ssid, 0, sizeof(sr->ssid));
-		sr->ssid.SSID_len =
-			min_t(u8, sizeof(sr->ssid.SSID), ssids->ssid_len);
-		if (sr->ssid.SSID_len) {
-			memcpy(sr->ssid.SSID, ssids->ssid, sr->ssid.SSID_len);
-			sr->ssid.SSID_len = htod32(sr->ssid.SSID_len);
-			WL_SCAN(("Specific scan ssid=\"%s\" len=%d\n",
-				sr->ssid.SSID, sr->ssid.SSID_len));
-			spec_scan = true;
-		} else {
-			WL_SCAN(("Broadcast scan\n"));
-		}
-		WL_SCAN(("sr->ssid.SSID_len (%d)\n", sr->ssid.SSID_len));
-		passive_scan = wl->active_scan ? 0 : 1;
-		err = wldev_ioctl(ndev, WLC_SET_PASSIVE_SCAN,
-			&passive_scan, sizeof(passive_scan), false);
-		if (unlikely(err)) {
-			WL_SCAN(("WLC_SET_PASSIVE_SCAN error (%d)\n", err));
-			goto scan_out;
-		}
-		err = wldev_ioctl(ndev, WLC_SCAN, &sr->ssid,
-			sizeof(sr->ssid), false);
-		if (err) {
-			if (err == -EBUSY) {
-				WL_INFO(("system busy : scan for \"%s\" "
-					"canceled\n", sr->ssid.SSID));
-			} else {
-				WL_ERR(("WLC_SCAN error (%d)\n", err));
-			}
-			goto scan_out;
-		}
-	}
-
-	return 0;
-
-scan_out:
-	wl_clr_drv_status(wl, SCANNING);
-	wl->scan_request = NULL;
-	return err;
-}
-
-static s32
-wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
-	struct cfg80211_scan_request *request)
-{
-	s32 err = 0;
-	struct wl_priv *wl = wiphy_priv(wiphy);
-
-	WL_DBG(("Enter \n"));
-	CHECK_SYS_UP(wl);
-	err = __wl_cfg80211_scan(wiphy, ndev, request, NULL);
-	if (unlikely(err)) {
-		WL_ERR(("scan error (%d)\n", err));
-		return err;
-	}
-
-	return err;
-}
-
-static s32 wl_dev_intvar_set(struct net_device *dev, s8 *name, s32 val)
-{
-	s8 buf[WLC_IOCTL_SMLEN];
-	u32 len;
-	s32 err = 0;
-
-	val = htod32(val);
-	len = bcm_mkiovar(name, (char *)(&val), sizeof(val), buf, sizeof(buf));
-	BUG_ON(unlikely(!len));
-
-	err = wldev_ioctl(dev, WLC_SET_VAR, buf, len, false);
-	if (unlikely(err)) {
-		WL_ERR(("error (%d)\n", err));
-	}
-
-	return err;
-}
-
-static s32
-wl_dev_intvar_get(struct net_device *dev, s8 *name, s32 *retval)
-{
-	union {
-		s8 buf[WLC_IOCTL_SMLEN];
-		s32 val;
-	} var;
-	u32 len;
-	u32 data_null;
-	s32 err = 0;
-
-	len = bcm_mkiovar(name, (char *)(&data_null), 0,
-		(char *)(&var), sizeof(var.buf));
-	BUG_ON(unlikely(!len));
-	err = wldev_ioctl(dev, WLC_GET_VAR, &var, len, false);
-	if (unlikely(err)) {
-		WL_ERR(("error (%d)\n", err));
-	}
-	*retval = dtoh32(var.val);
-
-	return err;
-}
-
-static s32 wl_set_rts(struct net_device *dev, u32 rts_threshold)
-{
-	s32 err = 0;
-
-	err = wl_dev_intvar_set(dev, "rtsthresh", rts_threshold);
-	if (unlikely(err)) {
-		WL_ERR(("Error (%d)\n", err));
-		return err;
-	}
-	return err;
-}
-
-static s32 wl_set_frag(struct net_device *dev, u32 frag_threshold)
-{
-	s32 err = 0;
-
-	err = wl_dev_intvar_set(dev, "fragthresh", frag_threshold);
-	if (unlikely(err)) {
-		WL_ERR(("Error (%d)\n", err));
-		return err;
-	}
-	return err;
-}
-
-static s32 wl_set_retry(struct net_device *dev, u32 retry, bool l)
-{
-	s32 err = 0;
-	u32 cmd = (l ? WLC_SET_LRL : WLC_SET_SRL);
-
-	retry = htod32(retry);
-	err = wldev_ioctl(dev, cmd, &retry, sizeof(retry), false);
-	if (unlikely(err)) {
-		WL_ERR(("cmd (%d) , error (%d)\n", cmd, err));
-		return err;
-	}
-	return err;
-}
-
-static s32 wl_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
-{
-	struct wl_priv *wl = (struct wl_priv *)wiphy_priv(wiphy);
-	struct net_device *ndev = wl_to_prmry_ndev(wl);
-	s32 err = 0;
-
-	CHECK_SYS_UP(wl);
-	if (changed & WIPHY_PARAM_RTS_THRESHOLD &&
-		(wl->conf->rts_threshold != wiphy->rts_threshold)) {
-		wl->conf->rts_threshold = wiphy->rts_threshold;
-		err = wl_set_rts(ndev, wl->conf->rts_threshold);
-		if (!err)
-			return err;
-	}
-	if (changed & WIPHY_PARAM_FRAG_THRESHOLD &&
-		(wl->conf->frag_threshold != wiphy->frag_threshold)) {
-		wl->conf->frag_threshold = wiphy->frag_threshold;
-		err = wl_set_frag(ndev, wl->conf->frag_threshold);
-		if (!err)
-			return err;
-	}
-	if (changed & WIPHY_PARAM_RETRY_LONG &&
-		(wl->conf->retry_long != wiphy->retry_long)) {
-		wl->conf->retry_long = wiphy->retry_long;
-		err = wl_set_retry(ndev, wl->conf->retry_long, true);
-		if (!err)
-			return err;
-	}
-	if (changed & WIPHY_PARAM_RETRY_SHORT &&
-		(wl->conf->retry_short != wiphy->retry_short)) {
-		wl->conf->retry_short = wiphy->retry_short;
-		err = wl_set_retry(ndev, wl->conf->retry_short, false);
-		if (!err) {
-			return err;
-		}
-	}
-
-	return err;
-}
-
-static s32
-wl_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
-	struct cfg80211_ibss_params *params)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	struct cfg80211_bss *bss;
-	struct ieee80211_channel *chan;
-	struct wl_join_params join_params;
-	struct cfg80211_ssid ssid;
-	s32 scan_retry = 0;
-	s32 err = 0;
-
-	WL_TRACE(("In\n"));
-	CHECK_SYS_UP(wl);
-	if (params->bssid) {
-		WL_ERR(("Invalid bssid\n"));
-		return -EOPNOTSUPP;
-	}
-	bss = cfg80211_get_ibss(wiphy, NULL, params->ssid, params->ssid_len);
-	if (!bss) {
-		memcpy(ssid.ssid, params->ssid, params->ssid_len);
-		ssid.ssid_len = params->ssid_len;
-		do {
-			if (unlikely
-				(__wl_cfg80211_scan(wiphy, dev, NULL, &ssid) ==
-				 -EBUSY)) {
-				wl_delay(150);
-			} else {
-				break;
-			}
-		} while (++scan_retry < WL_SCAN_RETRY_MAX);
-		/* to allow scan_inform to propagate to cfg80211 plane */
-		rtnl_unlock();
-
-		/* wait 4 secons till scan done.... */
-		schedule_timeout_interruptible(4 * HZ);
-		rtnl_lock();
-		bss = cfg80211_get_ibss(wiphy, NULL,
-			params->ssid, params->ssid_len);
-	}
-	if (bss) {
-		wl->ibss_starter = false;
-		WL_DBG(("Found IBSS\n"));
-	} else {
-		wl->ibss_starter = true;
-	}
-	chan = params->channel;
-	if (chan)
-		wl->channel = ieee80211_frequency_to_channel(chan->center_freq);
-	/*
-	 * Join with specific BSSID and cached SSID
-	 * If SSID is zero join based on BSSID only
-	 */
-	memset(&join_params, 0, sizeof(join_params));
-	memcpy((void *)join_params.ssid.SSID, (void *)params->ssid,
-		params->ssid_len);
-	join_params.ssid.SSID_len = htod32(params->ssid_len);
-	if (params->bssid)
-		memcpy(&join_params.params.bssid, params->bssid,
-			ETHER_ADDR_LEN);
-	else
-		memset(&join_params.params.bssid, 0, ETHER_ADDR_LEN);
-
-	err = wldev_ioctl(dev, WLC_SET_SSID, &join_params,
-		sizeof(join_params), false);
-	if (unlikely(err)) {
-		WL_ERR(("Error (%d)\n", err));
-		return err;
-	}
-	return err;
-}
-
-static s32 wl_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	s32 err = 0;
-
-	CHECK_SYS_UP(wl);
-	wl_link_down(wl);
-
-	return err;
-}
-
-static s32
-wl_set_wpa_version(struct net_device *dev, struct cfg80211_connect_params *sme)
-{
-	struct wl_priv *wl = wlcfg_drv_priv;
-	struct wl_security *sec;
-	s32 val = 0;
-	s32 err = 0;
-	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
-
-	if (sme->crypto.wpa_versions & NL80211_WPA_VERSION_1)
-		val = WPA_AUTH_PSK; /* | WPA_AUTH_UNSPECIFIED; */
-	else if (sme->crypto.wpa_versions & NL80211_WPA_VERSION_2)
-		val = WPA2_AUTH_PSK; /* | WPA2_AUTH_UNSPECIFIED ; */
-	else
-		val = WPA_AUTH_DISABLED;
-
-	if (is_wps_conn(sme))
-		val = WPA_AUTH_DISABLED;
-
-	WL_DBG(("setting wpa_auth to 0x%0x\n", val));
-	err = wldev_iovar_setint_bsscfg(dev, "wpa_auth", val, bssidx);
-	if (unlikely(err)) {
-		WL_ERR(("set wpa_auth failed (%d)\n", err));
-		return err;
-	}
-	sec = wl_read_prof(wl, WL_PROF_SEC);
-	sec->wpa_versions = sme->crypto.wpa_versions;
-	return err;
-}
-
-static s32
-wl_set_auth_type(struct net_device *dev, struct cfg80211_connect_params *sme)
-{
-	struct wl_priv *wl = wlcfg_drv_priv;
-	struct wl_security *sec;
-	s32 val = 0;
-	s32 err = 0;
-	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
-	switch (sme->auth_type) {
-	case NL80211_AUTHTYPE_OPEN_SYSTEM:
-		val = 0;
-		WL_DBG(("open system\n"));
-		break;
-	case NL80211_AUTHTYPE_SHARED_KEY:
-		val = 1;
-		WL_DBG(("shared key\n"));
-		break;
-	case NL80211_AUTHTYPE_AUTOMATIC:
-		val = 2;
-		WL_DBG(("automatic\n"));
-		break;
-	case NL80211_AUTHTYPE_NETWORK_EAP:
-		WL_DBG(("network eap\n"));
-	default:
-		val = 2;
-		WL_ERR(("invalid auth type (%d)\n", sme->auth_type));
-		break;
-	}
-
-	err = wldev_iovar_setint_bsscfg(dev, "auth", val, bssidx);
-	if (unlikely(err)) {
-		WL_ERR(("set auth failed (%d)\n", err));
-		return err;
-	}
-	sec = wl_read_prof(wl, WL_PROF_SEC);
-	sec->auth_type = sme->auth_type;
-	return err;
-}
-
-static s32
-wl_set_set_cipher(struct net_device *dev, struct cfg80211_connect_params *sme)
-{
-	struct wl_priv *wl = wlcfg_drv_priv;
-	struct wl_security *sec;
-	s32 pval = 0;
-	s32 gval = 0;
-	s32 err = 0;
-	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
-
-	if (sme->crypto.n_ciphers_pairwise) {
-		switch (sme->crypto.ciphers_pairwise[0]) {
-		case WLAN_CIPHER_SUITE_WEP40:
-		case WLAN_CIPHER_SUITE_WEP104:
-			pval = WEP_ENABLED;
-			break;
-		case WLAN_CIPHER_SUITE_TKIP:
-			pval = TKIP_ENABLED;
-			break;
-		case WLAN_CIPHER_SUITE_CCMP:
-			pval = AES_ENABLED;
-			break;
-		case WLAN_CIPHER_SUITE_AES_CMAC:
-			pval = AES_ENABLED;
-			break;
-		default:
-			WL_ERR(("invalid cipher pairwise (%d)\n",
-				sme->crypto.ciphers_pairwise[0]));
-			return -EINVAL;
-		}
-	}
-	if (sme->crypto.cipher_group) {
-		switch (sme->crypto.cipher_group) {
-		case WLAN_CIPHER_SUITE_WEP40:
-		case WLAN_CIPHER_SUITE_WEP104:
-			gval = WEP_ENABLED;
-			break;
-		case WLAN_CIPHER_SUITE_TKIP:
-			gval = TKIP_ENABLED;
-			break;
-		case WLAN_CIPHER_SUITE_CCMP:
-			gval = AES_ENABLED;
-			break;
-		case WLAN_CIPHER_SUITE_AES_CMAC:
-			gval = AES_ENABLED;
-			break;
-		default:
-			WL_ERR(("invalid cipher group (%d)\n",
-				sme->crypto.cipher_group));
-			return -EINVAL;
-		}
-	}
-
-	WL_DBG(("pval (%d) gval (%d)\n", pval, gval));
-
-	if (is_wps_conn(sme)) {
-		err = wldev_iovar_setint_bsscfg(dev, "wsec", 4, bssidx);
-	} else {
-		err = wldev_iovar_setint_bsscfg(dev, "wsec", pval | gval, bssidx);
-	}
-	if (unlikely(err)) {
-		WL_ERR(("error (%d)\n", err));
-		return err;
-	}
-
-	sec = wl_read_prof(wl, WL_PROF_SEC);
-	sec->cipher_pairwise = sme->crypto.ciphers_pairwise[0];
-	sec->cipher_group = sme->crypto.cipher_group;
-
-	return err;
-}
-
-static s32
-wl_set_key_mgmt(struct net_device *dev, struct cfg80211_connect_params *sme)
-{
-	struct wl_priv *wl = wlcfg_drv_priv;
-	struct wl_security *sec;
-	s32 val = 0;
-	s32 err = 0;
-	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
-
-	if (sme->crypto.n_akm_suites) {
-		err = wl_dev_intvar_get(dev, "wpa_auth", &val);
-		if (unlikely(err)) {
-			WL_ERR(("could not get wpa_auth (%d)\n", err));
-			return err;
-		}
-		if (val & (WPA_AUTH_PSK | WPA_AUTH_UNSPECIFIED)) {
-			switch (sme->crypto.akm_suites[0]) {
-			case WLAN_AKM_SUITE_8021X:
-				val = WPA_AUTH_UNSPECIFIED;
-				break;
-			case WLAN_AKM_SUITE_PSK:
-				val = WPA_AUTH_PSK;
-				break;
-			default:
-				WL_ERR(("invalid cipher group (%d)\n",
-					sme->crypto.cipher_group));
-				return -EINVAL;
-			}
-		} else if (val & (WPA2_AUTH_PSK | WPA2_AUTH_UNSPECIFIED)) {
-			switch (sme->crypto.akm_suites[0]) {
-			case WLAN_AKM_SUITE_8021X:
-				val = WPA2_AUTH_UNSPECIFIED;
-				break;
-			case WLAN_AKM_SUITE_PSK:
-				val = WPA2_AUTH_PSK;
-				break;
-			default:
-				WL_ERR(("invalid cipher group (%d)\n",
-					sme->crypto.cipher_group));
-				return -EINVAL;
-			}
-		}
-
-		WL_DBG(("setting wpa_auth to %d\n", val));
-
-		err = wldev_iovar_setint_bsscfg(dev, "wpa_auth", val, bssidx);
-		if (unlikely(err)) {
-			WL_ERR(("could not set wpa_auth (%d)\n", err));
-			return err;
-		}
-	}
-	sec = wl_read_prof(wl, WL_PROF_SEC);
-	sec->wpa_auth = sme->crypto.akm_suites[0];
-
-	return err;
-}
-
-static s32
-wl_set_set_sharedkey(struct net_device *dev,
-	struct cfg80211_connect_params *sme)
-{
-	struct wl_priv *wl = wlcfg_drv_priv;
-	struct wl_security *sec;
-	struct wl_wsec_key key;
-	s32 val;
-	s32 err = 0;
-	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
-
-	WL_DBG(("key len (%d)\n", sme->key_len));
-	if (sme->key_len) {
-		sec = wl_read_prof(wl, WL_PROF_SEC);
-		WL_DBG(("wpa_versions 0x%x cipher_pairwise 0x%x\n",
-			sec->wpa_versions, sec->cipher_pairwise));
-		if (!(sec->wpa_versions & (NL80211_WPA_VERSION_1 |
-			NL80211_WPA_VERSION_2)) &&
-			(sec->cipher_pairwise & (WLAN_CIPHER_SUITE_WEP40 |
-			WLAN_CIPHER_SUITE_WEP104))) {
-			memset(&key, 0, sizeof(key));
-			key.len = (u32) sme->key_len;
-			key.index = (u32) sme->key_idx;
-			if (unlikely(key.len > sizeof(key.data))) {
-				WL_ERR(("Too long key length (%u)\n", key.len));
-				return -EINVAL;
-			}
-			memcpy(key.data, sme->key, key.len);
-			key.flags = WL_PRIMARY_KEY;
-			switch (sec->cipher_pairwise) {
-			case WLAN_CIPHER_SUITE_WEP40:
-				key.algo = CRYPTO_ALGO_WEP1;
-				break;
-			case WLAN_CIPHER_SUITE_WEP104:
-				key.algo = CRYPTO_ALGO_WEP128;
-				break;
-			default:
-				WL_ERR(("Invalid algorithm (%d)\n",
-					sme->crypto.ciphers_pairwise[0]));
-				return -EINVAL;
-			}
-			/* Set the new key/index */
-			WL_DBG(("key length (%d) key index (%d) algo (%d)\n",
-				key.len, key.index, key.algo));
-			WL_DBG(("key \"%s\"\n", key.data));
-			swap_key_from_BE(&key);
-			err = wldev_iovar_setbuf_bsscfg(dev, "wsec_key", &key, sizeof(key),
-				ioctlbuf, sizeof(ioctlbuf), bssidx);
-			if (unlikely(err)) {
-				WL_ERR(("WLC_SET_KEY error (%d)\n", err));
-				return err;
-			}
-			if (sec->auth_type == NL80211_AUTHTYPE_OPEN_SYSTEM) {
-				WL_DBG(("set auth_type to shared key\n"));
-				val = 1;	/* shared key */
-				err = wldev_iovar_setint_bsscfg(dev, "auth", val, bssidx);
-				if (unlikely(err)) {
-					WL_ERR(("set auth failed (%d)\n", err));
-					return err;
-				}
-			}
-		}
-	}
-	return err;
-}
-
-static s32
-wl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
-	struct cfg80211_connect_params *sme)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	struct ieee80211_channel *chan = sme->channel;
-	struct wl_join_params join_params;
-	size_t join_params_size;
-	s32 err = 0;
-	wpa_ie_fixed_t *wpa_ie;
-	bcm_tlv_t *wpa2_ie;
-	u8* wpaie  = 0;
-	u32 wpaie_len = 0;
-	WL_DBG(("In\n"));
-	CHECK_SYS_UP(wl);
-
-	/*
-	 * Cancel ongoing scan to sync up with sme state machine of cfg80211.
-	 */
-	if (wl->scan_request) {
-		wl_cfg80211_scan_abort(wl, dev);
-	}
-
-	if (IS_P2P_SSID(sme->ssid) && (dev != wl_to_prmry_ndev(wl))) {
-		/* we only allow to connect using virtual interface in case of P2P */
-		if (p2p_on(wl) && is_wps_conn(sme)) {
-			WL_DBG(("p2p index : %d\n", wl_cfgp2p_find_idx(wl, dev)));
-			/* Have to apply WPS IE + P2P IE in assoc req frame */
-			wl_cfgp2p_set_management_ie(wl, dev,
-				wl_cfgp2p_find_idx(wl, dev), VNDR_IE_PRBREQ_FLAG,
-				wl_to_p2p_bss_saved_ie(wl, P2PAPI_BSSCFG_DEVICE).p2p_probe_req_ie,
-				wl_to_p2p_bss_saved_ie(wl,
-				P2PAPI_BSSCFG_DEVICE).p2p_probe_req_ie_len);
-			wl_cfgp2p_set_management_ie(wl, dev, wl_cfgp2p_find_idx(wl, dev),
-				VNDR_IE_ASSOCREQ_FLAG, sme->ie, sme->ie_len);
-		} else if (p2p_on(wl) && (sme->crypto.wpa_versions & NL80211_WPA_VERSION_2)) {
-			/* This is the connect req after WPS is done [credentials exchanged] 
-			 * currently identified with WPA_VERSION_2 .
-			 * Update the previously set IEs with
-			 * the newly received IEs from Supplicant. This will remove the WPS IE from
-			 * the Assoc Req.
-			 */
-			wl_cfgp2p_set_management_ie(wl, dev, wl_cfgp2p_find_idx(wl, dev),
-				VNDR_IE_ASSOCREQ_FLAG, sme->ie, sme->ie_len);
-		}
-
-	} else if (dev == wl_to_prmry_ndev(wl)) {
-			/* find the RSN_IE */
-			if ((wpa2_ie = bcm_parse_tlvs((u8 *)sme->ie, sme->ie_len,
-				DOT11_MNG_RSN_ID)) != NULL) {
-				WL_DBG((" WPA2 IE is found\n"));
-			}
-			/* find the WPA_IE */
-			if ((wpa_ie = wl_cfgp2p_find_wpaie((u8 *)sme->ie,
-				sme->ie_len)) != NULL) {
-				WL_DBG((" WPA IE is found\n"));
-			}
-			if (wpa_ie != NULL || wpa2_ie != NULL) {
-				wpaie = (wpa_ie != NULL) ? (u8 *)wpa_ie : (u8 *)wpa2_ie;
-				wpaie_len = (wpa_ie != NULL) ? wpa_ie->length : wpa2_ie->len;
-				wpaie_len += WPA_RSN_IE_TAG_FIXED_LEN;
-				wldev_iovar_setbuf(dev, "wpaie", wpaie, wpaie_len,
-				ioctlbuf, sizeof(ioctlbuf));
-			} else {
-				wldev_iovar_setbuf(dev, "wpaie", NULL, 0,
-				ioctlbuf, sizeof(ioctlbuf));
-			}
-
-	}
-
-	if (unlikely(!sme->ssid)) {
-		WL_ERR(("Invalid ssid\n"));
-		return -EOPNOTSUPP;
-	}
-	if (chan) {
-		wl->channel = ieee80211_frequency_to_channel(chan->center_freq);
-		WL_DBG(("channel (%d), center_req (%d)\n", wl->channel,
-			chan->center_freq));
-	} else
-		wl->channel = 0;
-	WL_DBG(("ie (%p), ie_len (%zd)\n", sme->ie, sme->ie_len));
-	err = wl_set_wpa_version(dev, sme);
-	if (unlikely(err))
-		return err;
-
-	err = wl_set_auth_type(dev, sme);
-	if (unlikely(err))
-		return err;
-
-	err = wl_set_set_cipher(dev, sme);
-	if (unlikely(err))
-		return err;
-
-	err = wl_set_key_mgmt(dev, sme);
-	if (unlikely(err))
-		return err;
-
-	err = wl_set_set_sharedkey(dev, sme);
-	if (unlikely(err))
-		return err;
-
-	wl_update_prof(wl, NULL, sme->bssid, WL_PROF_BSSID);
-	/*
-	 *  Join with specific BSSID and cached SSID
-	 *  If SSID is zero join based on BSSID only
-	 */
-	memset(&join_params, 0, sizeof(join_params));
-	join_params_size = sizeof(join_params.ssid);
-
-	join_params.ssid.SSID_len = min(sizeof(join_params.ssid.SSID), sme->ssid_len);
-	memcpy(&join_params.ssid.SSID, sme->ssid, join_params.ssid.SSID_len);
-	join_params.ssid.SSID_len = htod32(join_params.ssid.SSID_len);
-	wl_update_prof(wl, NULL, &join_params.ssid, WL_PROF_SSID);
-	if (sme->bssid)
-		memcpy(&join_params.params.bssid, sme->bssid, ETH_ALEN);
-	else
-		memcpy(&join_params.params.bssid, &ether_bcast, ETH_ALEN);
-
-	wl_ch_to_chanspec(wl->channel, &join_params, &join_params_size);
-	WL_DBG(("join_param_size %d\n", join_params_size));
-
-	if (join_params.ssid.SSID_len < IEEE80211_MAX_SSID_LEN) {
-		WL_INFO(("ssid \"%s\", len (%d)\n", join_params.ssid.SSID,
-			join_params.ssid.SSID_len));
-	}
-	wl_set_drv_status(wl, CONNECTING);
-	err = wldev_ioctl(dev, WLC_SET_SSID, &join_params, join_params_size, false);
-	if (unlikely(err)) {
-		WL_ERR(("error (%d)\n", err));
-		wl_clr_drv_status(wl, CONNECTING);
-		return err;
-	}
-	return err;
-}
-
-static s32
-wl_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
-	u16 reason_code)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	scb_val_t scbval;
-	bool act = false;
-	s32 err = 0;
-
-	WL_ERR(("Reason %d\n\n\n", reason_code));
-	CHECK_SYS_UP(wl);
-	act = *(bool *) wl_read_prof(wl, WL_PROF_ACT);
-	if (likely(act)) {
-		/*
-		* Cancel ongoing scan to sync up with sme state machine of cfg80211.
-		*/
-		if (wl->scan_request) {
-			wl_cfg80211_scan_abort(wl, dev);
-		}
-		wl_set_drv_status(wl, DISCONNECTING);
-		scbval.val = reason_code;
-		memcpy(&scbval.ea, &wl->bssid, ETHER_ADDR_LEN);
-		scbval.val = htod32(scbval.val);
-		err = wldev_ioctl(dev, WLC_DISASSOC, &scbval,
-			sizeof(scb_val_t), false);
-		if (unlikely(err)) {
-			wl_clr_drv_status(wl, DISCONNECTING);
-			WL_ERR(("error (%d)\n", err));
-			return err;
-		}
-	}
-
-	return err;
-}
-
-static s32
-wl_cfg80211_set_tx_power(struct wiphy *wiphy,
-	enum nl80211_tx_power_setting type, s32 dbm)
-{
-
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	struct net_device *ndev = wl_to_prmry_ndev(wl);
-	u16 txpwrmw;
-	s32 err = 0;
-	s32 disable = 0;
-
-	CHECK_SYS_UP(wl);
-	switch (type) {
-	case NL80211_TX_POWER_AUTOMATIC:
-		break;
-	case NL80211_TX_POWER_LIMITED:
-		if (dbm < 0) {
-			WL_ERR(("TX_POWER_LIMITTED - dbm is negative\n"));
-			return -EINVAL;
-		}
-		break;
-	case NL80211_TX_POWER_FIXED:
-		if (dbm < 0) {
-			WL_ERR(("TX_POWER_FIXED - dbm is negative..\n"));
-			return -EINVAL;
-		}
-		break;
-	}
-	/* Make sure radio is off or on as far as software is concerned */
-	disable = WL_RADIO_SW_DISABLE << 16;
-	disable = htod32(disable);
-	err = wldev_ioctl(ndev, WLC_SET_RADIO, &disable, sizeof(disable), false);
-	if (unlikely(err)) {
-		WL_ERR(("WLC_SET_RADIO error (%d)\n", err));
-		return err;
-	}
-
-	if (dbm > 0xffff)
-		txpwrmw = 0xffff;
-	else
-		txpwrmw = (u16) dbm;
-	err = wl_dev_intvar_set(ndev, "qtxpower",
-		(s32) (bcm_mw_to_qdbm(txpwrmw)));
-	if (unlikely(err)) {
-		WL_ERR(("qtxpower error (%d)\n", err));
-		return err;
-	}
-	wl->conf->tx_power = dbm;
-
-	return err;
-}
-
-static s32 wl_cfg80211_get_tx_power(struct wiphy *wiphy, s32 *dbm)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	struct net_device *ndev = wl_to_prmry_ndev(wl);
-	s32 txpwrdbm;
-	u8 result;
-	s32 err = 0;
-
-	CHECK_SYS_UP(wl);
-	err = wl_dev_intvar_get(ndev, "qtxpower", &txpwrdbm);
-	if (unlikely(err)) {
-		WL_ERR(("error (%d)\n", err));
-		return err;
-	}
-	result = (u8) (txpwrdbm & ~WL_TXPWR_OVERRIDE);
-	*dbm = (s32) bcm_qdbm_to_mw(result);
-
-	return err;
-}
-
-static s32
-wl_cfg80211_config_default_key(struct wiphy *wiphy, struct net_device *dev,
-	u8 key_idx, bool unicast, bool multicast)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	u32 index;
-	s32 wsec;
-	s32 err = 0;
-	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
-
-	WL_DBG(("key index (%d)\n", key_idx));
-	CHECK_SYS_UP(wl);
-	err = wldev_iovar_getint_bsscfg(dev, "wsec", &wsec, bssidx);
-	if (unlikely(err)) {
-		WL_ERR(("WLC_GET_WSEC error (%d)\n", err));
-		return err;
-	}
-	if (wsec & WEP_ENABLED) {
-		/* Just select a new current key */
-		index = (u32) key_idx;
-		index = htod32(index);
-		err = wldev_ioctl(dev, WLC_SET_KEY_PRIMARY, &index,
-			sizeof(index), false);
-		if (unlikely(err)) {
-			WL_ERR(("error (%d)\n", err));
-		}
-	}
-	return err;
-}
-
-static s32
-wl_add_keyext(struct wiphy *wiphy, struct net_device *dev,
-	u8 key_idx, const u8 *mac_addr, struct key_params *params)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	struct wl_wsec_key key;
-	s32 err = 0;
-	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
-	s32 mode = get_mode_by_netdev(wl, dev);
-	memset(&key, 0, sizeof(key));
-	key.index = (u32) key_idx;
-
-	if (!ETHER_ISMULTI(mac_addr))
-		memcpy((char *)&key.ea, (void *)mac_addr, ETHER_ADDR_LEN);
-	key.len = (u32) params->key_len;
-
-	/* check for key index change */
-	if (key.len == 0) {
-		/* key delete */
-		swap_key_from_BE(&key);
-		wldev_iovar_setbuf_bsscfg(dev, "wsec_key", &key, sizeof(key), ioctlbuf,
-			sizeof(ioctlbuf), bssidx);
-		if (unlikely(err)) {
-			WL_ERR(("key delete error (%d)\n", err));
-			return err;
-		}
-	} else {
-		if (key.len > sizeof(key.data)) {
-			WL_ERR(("Invalid key length (%d)\n", key.len));
-			return -EINVAL;
-		}
-		WL_DBG(("Setting the key index %d\n", key.index));
-		memcpy(key.data, params->key, key.len);
-
-		if ((mode == WL_MODE_BSS) &&
-			(params->cipher == WLAN_CIPHER_SUITE_TKIP)) {
-			u8 keybuf[8];
-			memcpy(keybuf, &key.data[24], sizeof(keybuf));
-			memcpy(&key.data[24], &key.data[16], sizeof(keybuf));
-			memcpy(&key.data[16], keybuf, sizeof(keybuf));
-		}
-
-		/* if IW_ENCODE_EXT_RX_SEQ_VALID set */
-		if (params->seq && params->seq_len == 6) {
-			/* rx iv */
-			u8 *ivptr;
-			ivptr = (u8 *) params->seq;
-			key.rxiv.hi = (ivptr[5] << 24) | (ivptr[4] << 16) |
-				(ivptr[3] << 8) | ivptr[2];
-			key.rxiv.lo = (ivptr[1] << 8) | ivptr[0];
-			key.iv_initialized = true;
-		}
-
-		switch (params->cipher) {
-		case WLAN_CIPHER_SUITE_WEP40:
-			key.algo = CRYPTO_ALGO_WEP1;
-			WL_DBG(("WLAN_CIPHER_SUITE_WEP40\n"));
-			break;
-		case WLAN_CIPHER_SUITE_WEP104:
-			key.algo = CRYPTO_ALGO_WEP128;
-			WL_DBG(("WLAN_CIPHER_SUITE_WEP104\n"));
-			break;
-		case WLAN_CIPHER_SUITE_TKIP:
-			key.algo = CRYPTO_ALGO_TKIP;
-			WL_DBG(("WLAN_CIPHER_SUITE_TKIP\n"));
-			break;
-		case WLAN_CIPHER_SUITE_AES_CMAC:
-			key.algo = CRYPTO_ALGO_AES_CCM;
-			WL_DBG(("WLAN_CIPHER_SUITE_AES_CMAC\n"));
-			break;
-		case WLAN_CIPHER_SUITE_CCMP:
-			key.algo = CRYPTO_ALGO_AES_CCM;
-			WL_DBG(("WLAN_CIPHER_SUITE_CCMP\n"));
-			break;
-		default:
-			WL_ERR(("Invalid cipher (0x%x)\n", params->cipher));
-			return -EINVAL;
-		}
-		swap_key_from_BE(&key);
-#ifdef CONFIG_WIRELESS_EXT
-		dhd_wait_pend8021x(dev);
-#endif
-		wldev_iovar_setbuf_bsscfg(dev, "wsec_key", &key, sizeof(key), ioctlbuf,
-			sizeof(ioctlbuf), bssidx);
-		if (unlikely(err)) {
-			WL_ERR(("WLC_SET_KEY error (%d)\n", err));
-			return err;
-		}
-	}
-	return err;
-}
-
-static s32
-wl_cfg80211_add_key(struct wiphy *wiphy, struct net_device *dev,
-	u8 key_idx, bool pairwise, const u8 *mac_addr,
-	struct key_params *params)
-{
-	struct wl_wsec_key key;
-	s32 val = 0;
-	s32 wsec = 0;
-	s32 err = 0;
-	u8 keybuf[8];
-	s32 bssidx = 0;
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	s32 mode = get_mode_by_netdev(wl, dev);
-	WL_DBG(("key index (%d)\n", key_idx));
-	CHECK_SYS_UP(wl);
-
-	bssidx = wl_cfgp2p_find_idx(wl, dev);
-
-	if (mac_addr) {
-		wl_add_keyext(wiphy, dev, key_idx, mac_addr, params);
-		goto exit;
-	}
-	memset(&key, 0, sizeof(key));
-
-	key.len = (u32) params->key_len;
-	key.index = (u32) key_idx;
-
-	if (unlikely(key.len > sizeof(key.data))) {
-		WL_ERR(("Too long key length (%u)\n", key.len));
-		return -EINVAL;
-	}
-	memcpy(key.data, params->key, key.len);
-
-	key.flags = WL_PRIMARY_KEY;
-	switch (params->cipher) {
-	case WLAN_CIPHER_SUITE_WEP40:
-		key.algo = CRYPTO_ALGO_WEP1;
-		val = WEP_ENABLED;
-		WL_DBG(("WLAN_CIPHER_SUITE_WEP40\n"));
-		break;
-	case WLAN_CIPHER_SUITE_WEP104:
-		key.algo = CRYPTO_ALGO_WEP128;
-		val = WEP_ENABLED;
-		WL_DBG(("WLAN_CIPHER_SUITE_WEP104\n"));
-		break;
-	case WLAN_CIPHER_SUITE_TKIP:
-		key.algo = CRYPTO_ALGO_TKIP;
-		val = TKIP_ENABLED;
-		/* wpa_supplicant switches the third and fourth quarters of the TKIP key */
-		if (mode == WL_MODE_BSS) {
-			bcopy(&key.data[24], keybuf, sizeof(keybuf));
-			bcopy(&key.data[16], &key.data[24], sizeof(keybuf));
-			bcopy(keybuf, &key.data[16], sizeof(keybuf));
-		}
-		WL_DBG(("WLAN_CIPHER_SUITE_TKIP\n"));
-		break;
-	case WLAN_CIPHER_SUITE_AES_CMAC:
-		key.algo = CRYPTO_ALGO_AES_CCM;
-		val = AES_ENABLED;
-		WL_DBG(("WLAN_CIPHER_SUITE_AES_CMAC\n"));
-		break;
-	case WLAN_CIPHER_SUITE_CCMP:
-		key.algo = CRYPTO_ALGO_AES_CCM;
-		val = AES_ENABLED;
-		WL_DBG(("WLAN_CIPHER_SUITE_CCMP\n"));
-		break;
-	default:
-		WL_ERR(("Invalid cipher (0x%x)\n", params->cipher));
-		return -EINVAL;
-	}
-
-	/* Set the new key/index */
-	swap_key_from_BE(&key);
-	err = wldev_iovar_setbuf_bsscfg(dev, "wsec_key", &key, sizeof(key), ioctlbuf,
-		sizeof(ioctlbuf), bssidx);
-	if (unlikely(err)) {
-		WL_ERR(("WLC_SET_KEY error (%d)\n", err));
-		return err;
-	}
-
-exit:
-	err = wldev_iovar_getint_bsscfg(dev, "wsec", &wsec, bssidx);
-	if (unlikely(err)) {
-		WL_ERR(("get wsec error (%d)\n", err));
-		return err;
-	}
-
-	wsec |= val;
-	err = wldev_iovar_setint_bsscfg(dev, "wsec", wsec, bssidx);
-	if (unlikely(err)) {
-		WL_ERR(("set wsec error (%d)\n", err));
-		return err;
-	}
-
-#ifdef NOT_YET
-	/* TODO: Removed in P2P, check later --lm */
-	val = 1;		/* assume shared key. otherwise 0 */
-	val = htod32(val);
-	err = wldev_ioctl(dev, WLC_SET_AUTH, &val, sizeof(val), false);
-	if (unlikely(err)) {
-		WL_ERR(("WLC_SET_AUTH error (%d)\n", err));
-		return err;
-	}
-#endif
-	return err;
-}
-
-static s32
-wl_cfg80211_del_key(struct wiphy *wiphy, struct net_device *dev,
-	u8 key_idx, bool pairwise, const u8 *mac_addr)
-{
-	struct wl_wsec_key key;
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	s32 err = 0;
-	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
-
-	WL_DBG(("Enter\n"));
-	CHECK_SYS_UP(wl);
-	memset(&key, 0, sizeof(key));
-
-	key.index = (u32) key_idx;
-	key.flags = WL_PRIMARY_KEY;
-	key.algo = CRYPTO_ALGO_OFF;
-
-	WL_DBG(("key index (%d)\n", key_idx));
-	/* Set the new key/index */
-	swap_key_from_BE(&key);
-	wldev_iovar_setbuf_bsscfg(dev, "wsec_key", &key, sizeof(key), ioctlbuf,
-		sizeof(ioctlbuf), bssidx);
-	if (unlikely(err)) {
-		if (err == -EINVAL) {
-			if (key.index >= DOT11_MAX_DEFAULT_KEYS) {
-				/* we ignore this key index in this case */
-				WL_DBG(("invalid key index (%d)\n", key_idx));
-			}
-		} else {
-			WL_ERR(("WLC_SET_KEY error (%d)\n", err));
-		}
-		return err;
-	}
-
-#ifdef NOT_YET
-	/* TODO: Removed in P2P twig, check later --lin */
-	val = 0;		/* assume open key. otherwise 1 */
-	val = htod32(val);
-	err = wldev_ioctl(dev, WLC_SET_AUTH, &val, sizeof(val), false);
-	if (unlikely(err)) {
-		WL_ERR(("WLC_SET_AUTH error (%d)\n", err));
-		return err;
-	}
-#endif
-	return err;
-}
-
-static s32
-wl_cfg80211_get_key(struct wiphy *wiphy, struct net_device *dev,
-	u8 key_idx, bool pairwise, const u8 *mac_addr, void *cookie,
-	void (*callback) (void *cookie, struct key_params * params))
-{
-	struct key_params params;
-	struct wl_wsec_key key;
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	struct wl_security *sec;
-	s32 wsec;
-	s32 err = 0;
-	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
-
-	WL_DBG(("key index (%d)\n", key_idx));
-	CHECK_SYS_UP(wl);
-	memset(&key, 0, sizeof(key));
-	key.index = key_idx;
-	swap_key_to_BE(&key);
-	memset(&params, 0, sizeof(params));
-	params.key_len = (u8) min_t(u8, DOT11_MAX_KEY_SIZE, key.len);
-	memcpy(params.key, key.data, params.key_len);
-
-	wldev_iovar_getint_bsscfg(dev, "wsec", &wsec, bssidx);
-	if (unlikely(err)) {
-		WL_ERR(("WLC_GET_WSEC error (%d)\n", err));
-		return err;
-	}
-	switch (wsec & ~SES_OW_ENABLED) {
-		case WEP_ENABLED:
-			sec = wl_read_prof(wl, WL_PROF_SEC);
-			if (sec->cipher_pairwise & WLAN_CIPHER_SUITE_WEP40) {
-				params.cipher = WLAN_CIPHER_SUITE_WEP40;
-				WL_DBG(("WLAN_CIPHER_SUITE_WEP40\n"));
-			} else if (sec->cipher_pairwise & WLAN_CIPHER_SUITE_WEP104) {
-				params.cipher = WLAN_CIPHER_SUITE_WEP104;
-				WL_DBG(("WLAN_CIPHER_SUITE_WEP104\n"));
-			}
-			break;
-		case TKIP_ENABLED:
-			params.cipher = WLAN_CIPHER_SUITE_TKIP;
-			WL_DBG(("WLAN_CIPHER_SUITE_TKIP\n"));
-			break;
-		case AES_ENABLED:
-			params.cipher = WLAN_CIPHER_SUITE_AES_CMAC;
-			WL_DBG(("WLAN_CIPHER_SUITE_AES_CMAC\n"));
-			break;
-		default:
-			WL_ERR(("Invalid algo (0x%x)\n", wsec));
-			return -EINVAL;
-	}
-
-	callback(cookie, &params);
-	return err;
-}
-
-static s32
-wl_cfg80211_config_default_mgmt_key(struct wiphy *wiphy,
-	struct net_device *dev, u8 key_idx)
-{
-	WL_INFO(("Not supported\n"));
-	return -EOPNOTSUPP;
-}
-
-static s32
-wl_cfg80211_get_station(struct wiphy *wiphy, struct net_device *dev,
-	u8 *mac, struct station_info *sinfo)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	scb_val_t scb_val;
-	int rssi;
-	s32 rate;
-	s32 err = 0;
-
-	CHECK_SYS_UP(wl);
-	if (unlikely
-		(memcmp(mac, wl_read_prof(wl, WL_PROF_BSSID), ETHER_ADDR_LEN))) {
-		WL_ERR(("Wrong Mac address\n"));
-		return -ENOENT;
-	}
-
-	/* Report the current tx rate */
-	err = wldev_ioctl(dev, WLC_GET_RATE, &rate, sizeof(rate), false);
-	if (err) {
-		WL_ERR(("Could not get rate (%d)\n", err));
-	} else {
-		rate = dtoh32(rate);
-		sinfo->filled |= STATION_INFO_TX_BITRATE;
-		sinfo->txrate.legacy = rate * 5;
-		WL_DBG(("Rate %d Mbps\n", (rate / 2)));
-	}
-
-	if (wl_get_drv_status(wl, CONNECTED)) {
-		memset(&scb_val, 0, sizeof(scb_val));
-		scb_val.val = 0;
-		err = wldev_ioctl(dev, WLC_GET_RSSI, &scb_val,
-			sizeof(scb_val_t), false);
-		if (unlikely(err)) {
-			WL_ERR(("Could not get rssi (%d)\n", err));
-			return err;
-		}
-		rssi = dtoh32(scb_val.val);
-		sinfo->filled |= STATION_INFO_SIGNAL;
-		sinfo->signal = rssi;
-		WL_DBG(("RSSI %d dBm\n", rssi));
-	}
-
-#if defined(ANDROID_WIRELESS_PATCH)
-	err = wldev_ioctl(dev, WLC_GET_RATE, &sinfo->link_speed, sizeof(sinfo->link_speed), false);
-	sinfo->link_speed = sinfo->link_speed / 2; /* Convert internal 500Kbps to Mpbs */
-	if (!err)
-		sinfo->filled |= STATION_LINK_SPEED;
-	else
-		WL_ERR(("WLC_GET_RATE failed\n"));
-#endif
-
-	return err;
-}
-
-static s32
-wl_cfg80211_set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
-	bool enabled, s32 timeout)
-{
-	s32 pm;
-	s32 err = 0;
-	struct wl_priv *wl = wiphy_priv(wiphy);
-
-	CHECK_SYS_UP(wl);
-	pm = enabled ? PM_FAST : PM_OFF;
-	/* Do not enable the power save after assoc if it is p2p interface */
-	if (wl->p2p && wl->p2p->vif_created) {
-		WL_DBG(("Do not enable the power save for p2p interfaces even after assoc\n"));
-		pm = PM_OFF;
-	}
-	pm = htod32(pm);
-	WL_DBG(("power save %s\n", (pm ? "enabled" : "disabled")));
-	err = wldev_ioctl(dev, WLC_SET_PM, &pm, sizeof(pm), false);
-	if (unlikely(err)) {
-		if (err == -ENODEV)
-			WL_DBG(("net_device is not ready yet\n"));
-		else
-			WL_ERR(("error (%d)\n", err));
-		return err;
-	}
-	return err;
-}
-
-static __used u32 wl_find_msb(u16 bit16)
-{
-	u32 ret = 0;
-
-	if (bit16 & 0xff00) {
-		ret += 8;
-		bit16 >>= 8;
-	}
-
-	if (bit16 & 0xf0) {
-		ret += 4;
-		bit16 >>= 4;
-	}
-
-	if (bit16 & 0xc) {
-		ret += 2;
-		bit16 >>= 2;
-	}
-
-	if (bit16 & 2)
-		ret += bit16 & 2;
-	else if (bit16)
-		ret += bit16;
-
-	return ret;
-}
-
-static s32 wl_cfg80211_resume(struct wiphy *wiphy)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	s32 err = 0;
-
-	if (unlikely(!wl_get_drv_status(wl, READY))) {
-		WL_INFO(("device is not ready : status (%d)\n",
-			(int)wl->status));
-		return 0;
-	}
-
-	wl_invoke_iscan(wl);
-
-	return err;
-}
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 39)
-static s32 wl_cfg80211_suspend(struct wiphy *wiphy, struct cfg80211_wowlan *wow)
-#else
-static s32 wl_cfg80211_suspend(struct wiphy *wiphy)
-#endif
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	s32 err = 0;
-	if (unlikely(!wl_get_drv_status(wl, READY))) {
-		WL_INFO(("device is not ready : status (%d)\n",
-			(int)wl->status));
-		return 0;
-	}
-
-	wl_set_drv_status(wl, SCAN_ABORTING);
-	wl_term_iscan(wl);
-	if (wl->scan_request) {
-		cfg80211_scan_done(wl->scan_request, true);
-		wl->scan_request = NULL;
-	}
-	wl_clr_drv_status(wl, SCANNING);
-	wl_clr_drv_status(wl, SCAN_ABORTING);
-
-	return err;
-}
-
-static __used s32
-wl_update_pmklist(struct net_device *dev, struct wl_pmk_list *pmk_list,
-	s32 err)
-{
-	int i, j;
-	struct wl_priv *wl = wlcfg_drv_priv;
-	struct net_device *primary_dev = wl_to_prmry_ndev(wl);
-
-	/* Firmware is supporting pmk list only for STA interface i.e. primary interface
-	  * Refer code wlc_bsscfg.c->wlc_bsscfg_sta_init
-	  * Do we really need to support PMK cache in P2P in firmware?
-	*/
-	if (primary_dev != dev) {
-		WL_INFO(("Not supporting Flushing pmklist on virtual"
-			" interfaces than primary interface\n"));
-		return err;
-	}
-
-	WL_DBG(("No of elements %d\n", pmk_list->pmkids.npmkid));
-	for (i = 0; i < pmk_list->pmkids.npmkid; i++) {
-		WL_DBG(("PMKID[%d]: %pM =\n", i,
-			&pmk_list->pmkids.pmkid[i].BSSID));
-		for (j = 0; j < WPA2_PMKID_LEN; j++) {
-			WL_DBG(("%02x\n", pmk_list->pmkids.pmkid[i].PMKID[j]));
-		}
-	}
-	if (likely(!err)) {
-		err = wl_dev_bufvar_set(dev, "pmkid_info", (char *)pmk_list,
-			sizeof(*pmk_list));
-	}
-
-	return err;
-}
-
-static s32
-wl_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *dev,
-	struct cfg80211_pmksa *pmksa)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	s32 err = 0;
-	int i;
-
-	CHECK_SYS_UP(wl);
-	for (i = 0; i < wl->pmk_list->pmkids.npmkid; i++)
-		if (!memcmp(pmksa->bssid, &wl->pmk_list->pmkids.pmkid[i].BSSID,
-			ETHER_ADDR_LEN))
-			break;
-	if (i < WL_NUM_PMKIDS_MAX) {
-		memcpy(&wl->pmk_list->pmkids.pmkid[i].BSSID, pmksa->bssid,
-			ETHER_ADDR_LEN);
-		memcpy(&wl->pmk_list->pmkids.pmkid[i].PMKID, pmksa->pmkid,
-			WPA2_PMKID_LEN);
-		if (i == wl->pmk_list->pmkids.npmkid)
-			wl->pmk_list->pmkids.npmkid++;
-	} else {
-		err = -EINVAL;
-	}
-	WL_DBG(("set_pmksa,IW_PMKSA_ADD - PMKID: %pM =\n",
-		&wl->pmk_list->pmkids.pmkid[wl->pmk_list->pmkids.npmkid - 1].BSSID));
-	for (i = 0; i < WPA2_PMKID_LEN; i++) {
-		WL_DBG(("%02x\n",
-			wl->pmk_list->pmkids.pmkid[wl->pmk_list->pmkids.npmkid - 1].
-			PMKID[i]));
-	}
-
-	err = wl_update_pmklist(dev, wl->pmk_list, err);
-
-	return err;
-}
-
-static s32
-wl_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *dev,
-	struct cfg80211_pmksa *pmksa)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	struct _pmkid_list pmkid;
-	s32 err = 0;
-	int i;
-
-	CHECK_SYS_UP(wl);
-	memcpy(&pmkid.pmkid[0].BSSID, pmksa->bssid, ETHER_ADDR_LEN);
-	memcpy(&pmkid.pmkid[0].PMKID, pmksa->pmkid, WPA2_PMKID_LEN);
-
-	WL_DBG(("del_pmksa,IW_PMKSA_REMOVE - PMKID: %pM =\n",
-		&pmkid.pmkid[0].BSSID));
-	for (i = 0; i < WPA2_PMKID_LEN; i++) {
-		WL_DBG(("%02x\n", pmkid.pmkid[0].PMKID[i]));
-	}
-
-	for (i = 0; i < wl->pmk_list->pmkids.npmkid; i++)
-		if (!memcmp
-		    (pmksa->bssid, &wl->pmk_list->pmkids.pmkid[i].BSSID,
-		     ETHER_ADDR_LEN))
-			break;
-
-	if ((wl->pmk_list->pmkids.npmkid > 0) &&
-		(i < wl->pmk_list->pmkids.npmkid)) {
-		memset(&wl->pmk_list->pmkids.pmkid[i], 0, sizeof(pmkid_t));
-		for (; i < (wl->pmk_list->pmkids.npmkid - 1); i++) {
-			memcpy(&wl->pmk_list->pmkids.pmkid[i].BSSID,
-				&wl->pmk_list->pmkids.pmkid[i + 1].BSSID,
-				ETHER_ADDR_LEN);
-			memcpy(&wl->pmk_list->pmkids.pmkid[i].PMKID,
-				&wl->pmk_list->pmkids.pmkid[i + 1].PMKID,
-				WPA2_PMKID_LEN);
-		}
-		wl->pmk_list->pmkids.npmkid--;
-	} else {
-		err = -EINVAL;
-	}
-
-	err = wl_update_pmklist(dev, wl->pmk_list, err);
-
-	return err;
-
-}
-
-static s32
-wl_cfg80211_flush_pmksa(struct wiphy *wiphy, struct net_device *dev)
-{
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	s32 err = 0;
-	CHECK_SYS_UP(wl);
-	memset(wl->pmk_list, 0, sizeof(*wl->pmk_list));
-	err = wl_update_pmklist(dev, wl->pmk_list, err);
-	return err;
-
-}
-
-wl_scan_params_t *
-wl_cfg80211_scan_alloc_params(int channel, int nprobes, int *out_params_size)
-{
-	wl_scan_params_t *params;
-	int params_size;
-	int num_chans;
-
-	*out_params_size = 0;
-
-	/* Our scan params only need space for 1 channel and 0 ssids */
-	params_size = WL_SCAN_PARAMS_FIXED_SIZE + 1 * sizeof(uint16);
-	params = (wl_scan_params_t*) kzalloc(params_size, GFP_KERNEL);
-	if (params == NULL) {
-		WL_ERR(("%s: mem alloc failed (%d bytes)\n", __func__, params_size));
-		return params;
-	}
-	memset(params, 0, params_size);
-	params->nprobes = nprobes;
-
-	num_chans = (channel == 0) ? 0 : 1;
-
-	memcpy(&params->bssid, &ether_bcast, ETHER_ADDR_LEN);
-	params->bss_type = DOT11_BSSTYPE_ANY;
-	params->scan_type = DOT11_SCANTYPE_ACTIVE;
-	params->nprobes = htod32(1);
-	params->active_time = htod32(-1);
-	params->passive_time = htod32(-1);
-	params->home_time = htod32(10);
-	params->channel_list[0] = htodchanspec(channel);
-
-	/* Our scan params have 1 channel and 0 ssids */
-	params->channel_num = htod32((0 << WL_SCAN_PARAMS_NSSID_SHIFT) |
-	(num_chans & WL_SCAN_PARAMS_COUNT_MASK));
-
-	*out_params_size = params_size;	/* rtn size to the caller */
-	return params;
-}
-s32
-wl_cfg80211_scan_abort(struct wl_priv *wl, struct net_device *ndev)
-{
-	wl_scan_params_t *params;
-	s32 params_size;
-	s32 err = BCME_OK;
-
-	WL_DBG(("Enter\n"));
-
-	/* Our scan params only need space for 1 channel and 0 ssids */
-	params = wl_cfg80211_scan_alloc_params(-1, 0, &params_size);
-	if (params == NULL) {
-		WL_ERR(("scan params allocation failed \n"));
-		err = -ENOMEM;
-	}
-	/* Do a scan abort to stop the driver's scan engine */
-	err = wldev_ioctl(ndev, WLC_SCAN, params, params_size, false);
-	if (err < 0) {
-		WL_ERR(("scan abort  failed \n"));
-	}
-
-	return err;
-}
-static s32
-wl_cfg80211_remain_on_channel(struct wiphy *wiphy, struct net_device *dev,
-	struct ieee80211_channel * channel,
-	enum nl80211_channel_type channel_type,
-	unsigned int duration, u64 *cookie)
-{
-	s32 target_channel;
-
-	s32 err = BCME_OK;
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	dhd_pub_t *dhd = (dhd_pub_t *)(wl->pub);
-	WL_DBG(("Enter, netdev_ifidx: %d \n", dev->ifindex));
-	if (likely(wl_get_drv_status(wl, SCANNING))) {
-		wl_cfg80211_scan_abort(wl, dev);
-	}
-
-	target_channel = ieee80211_frequency_to_channel(channel->center_freq);
-	memcpy(&wl->remain_on_chan, channel, sizeof(struct ieee80211_channel));
-	wl->remain_on_chan_type = channel_type;
-	wl->cache_cookie = *cookie;
-	cfg80211_ready_on_channel(dev, *cookie, channel,
-		channel_type, duration, GFP_KERNEL);
-	if (!p2p_on(wl)) {
-		wl_cfgp2p_generate_bss_mac(&dhd->mac, &wl->p2p->dev_addr, &wl->p2p->int_addr);
-
-		/* In case of p2p_listen command, supplicant send remain_on_channel
-		 * without turning on P2P
-		 */
-
-		p2p_on(wl) = true;
-		err = wl_cfgp2p_enable_discovery(wl, dev, NULL, 0);
-
-		if (unlikely(err)) {
-			goto exit;
-		}
-	}
-	if (p2p_on(wl))
-		wl_cfgp2p_discover_listen(wl, target_channel, duration);
-
-
-exit:
-	return err;
-}
-
-static s32
-wl_cfg80211_cancel_remain_on_channel(struct wiphy *wiphy, struct net_device *dev,
-	u64 cookie)
-{
-	s32 err = 0;
-	WL_DBG((" enter ) netdev_ifidx: %d \n", dev->ifindex));
-	return err;
-}
-
-static s32
-wl_cfg80211_mgmt_tx(struct wiphy *wiphy, struct net_device *dev,
-	struct ieee80211_channel *channel, bool offchan,
-	enum nl80211_channel_type channel_type,
-	bool channel_type_valid, unsigned int wait,
-	const u8* buf, size_t len, u64 *cookie)
-{
-	wl_action_frame_t *action_frame;
-	wl_af_params_t *af_params;
-	wifi_p2p_ie_t *p2p_ie;
-	wpa_ie_fixed_t *wps_ie;
-	const struct ieee80211_mgmt *mgmt;
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	dhd_pub_t *dhd = (dhd_pub_t *)(wl->pub);
-	s32 err = BCME_OK;
-	s32 bssidx = 0;
-	u32 p2pie_len = 0;
-	u32 wpsie_len = 0;
-	u16 fc;
-	bool ack = false;
-	WL_DBG(("Enter \n"));
-	/* find bssidx based on ndev */
-	bssidx = wl_cfgp2p_find_idx(wl, dev);
-	/* cookie generation */
-	*cookie = (unsigned long) buf;
-
-	if (bssidx == -1) {
-
-		WL_ERR(("Can not find the bssidx for dev( %p )\n", dev));
-		return -ENODEV;
-	}
-	if (wl->p2p_supported && p2p_on(wl)) {
-		wl_cfgp2p_generate_bss_mac(&dhd->mac, &wl->p2p->dev_addr, &wl->p2p->int_addr);
-		/* Suspend P2P discovery search-listen to prevent it from changing the
-		 * channel.
-		 */
-		if ((err = wl_cfgp2p_discover_enable_search(wl, false)) < 0) {
-			WL_ERR(("Can not disable discovery mode\n"));
-			return -EFAULT;
-		}
-	}
-
-	mgmt = (const struct ieee80211_mgmt *) buf;
-	fc = mgmt->frame_control;
-	if (fc != IEEE80211_STYPE_ACTION) {
-		if (fc == IEEE80211_STYPE_PROBE_RESP) {
-			s32 ie_offset =  DOT11_MGMT_HDR_LEN + DOT11_BCN_PRB_FIXED_LEN;
-			s32 ie_len = len - ie_offset;
-			if ((p2p_ie = wl_cfgp2p_find_p2pie((u8 *)(buf + ie_offset), ie_len))
-				!= NULL) {
-				/* Total length of P2P Information Element */
-				p2pie_len = p2p_ie->len + sizeof(p2p_ie->len) + sizeof(p2p_ie->id);
-				/* Have to change p2p device address in dev_info attribute
-				 * because Supplicant use primary eth0 address
-				 */
-			#ifdef ENABLE_DRIVER_CHANGE_IFADDR /* We are now doing this in supplicant */
-				wl_cfg80211_change_ifaddr((u8 *)p2p_ie,
-					&wl->p2p_dev_addr, P2P_SEID_DEV_INFO);
-			#endif
-			}
-			if ((wps_ie = wl_cfgp2p_find_wpsie((u8 *)(buf + ie_offset), ie_len))
-				!= NULL) {
-				/* Order of Vendor IE is 1) WPS IE +
-				 * 2) P2P IE created by supplicant
-				 *  So, it is ok to find start address of WPS IE
-				 *  to save IEs to firmware
-				 */
-				wpsie_len = wps_ie->length + sizeof(wps_ie->length) +
-					sizeof(wps_ie->tag);
-				wl_cfgp2p_set_management_ie(wl, dev, bssidx,
-					VNDR_IE_PRBRSP_FLAG,
-					(u8 *)wps_ie, wpsie_len + p2pie_len);
-				/* remove WLC_E_PROBREQ_MSG event to prevent HOSTAPD
-				 * from responding many probe request
-				 */
-			}
-		}
-		cfg80211_mgmt_tx_status(dev, *cookie, buf, len, true, GFP_KERNEL);
-		goto exit;
-	} else {
-	    /* Abort the dwell time of any previous off-channel action frame that may
-	     * be still in effect.  Sending off-channel action frames relies on the
-	     * driver's scan engine.  If a previous off-channel action frame tx is
-	     * still in progress (including the dwell time), then this new action
-	     * frame will not be sent out.
-	     */
-		wl_cfg80211_scan_abort(wl, dev);
-	}
-	af_params = (wl_af_params_t *) kzalloc(WL_WIFI_AF_PARAMS_SIZE, GFP_KERNEL);
-
-	if (af_params == NULL)
-	{
-		WL_ERR(("unable to allocate frame\n"));
-		return -ENOMEM;
-	}
-
-	action_frame = &af_params->action_frame;
-
-	/* Add the packet Id */
-	action_frame->packetId = (u32) action_frame;
-	WL_DBG(("action frame %d\n", action_frame->packetId));
-	/* Add BSSID */
-	memcpy(&action_frame->da, &mgmt->da[0], ETHER_ADDR_LEN);
-	memcpy(&af_params->BSSID, &mgmt->bssid[0], ETHER_ADDR_LEN);
-
-	/* Add the length exepted for 802.11 header  */
-	action_frame->len = len - DOT11_MGMT_HDR_LEN;
-	WL_DBG(("action_frame->len: %d\n", action_frame->len));
-
-	/* Add the channel */
-	af_params->channel =
-		ieee80211_frequency_to_channel(channel->center_freq);
-
-	/* Add the dwell time
-	 * Dwell time to stay off-channel to wait for a response action frame
-	 * after transmitting an GO Negotiation action frame
-	 */
-	af_params->dwell_time = WL_DWELL_TIME;
-
-	memcpy(action_frame->data, &buf[DOT11_MGMT_HDR_LEN], action_frame->len);
-
-	if (wl->p2p->vif_created) {
-		wifi_p2p_pub_act_frame_t *act_frm =
-			(wifi_p2p_pub_act_frame_t *) (action_frame->data);
-		/*
-		 *  Have to change intented address from GO REQ or GO RSP and INVITE REQ
-		 *  because wpa-supplicant use eth0 primary address
-		 */
-		if ((act_frm->subtype == P2P_PAF_GON_REQ)||
-		  (act_frm->subtype == P2P_PAF_GON_RSP)||
-		  (act_frm->subtype == P2P_PAF_GON_CONF)||
-		  (act_frm->subtype == P2P_PAF_INVITE_REQ)) {
-			p2p_ie = wl_cfgp2p_find_p2pie(act_frm->elts,
-				action_frame->len - P2P_PUB_AF_FIXED_LEN);
-		#ifdef ENABLE_DRIVER_CHANGE_IFADDR /* We are now doing this in supplicant */
-			wl_cfg80211_change_ifaddr((u8 *)p2p_ie, &wl->p2p.int_addr,
-				P2P_SEID_INTINTADDR);
-			wl_cfg80211_change_ifaddr((u8 *)p2p_ie, &wl->p2p.dev_addr,
-				P2P_SEID_DEV_INFO);
-			wl_cfg80211_change_ifaddr((u8 *)p2p_ie, &wl->p2p.dev_addr,
-				P2P_SEID_GROUP_ID);
-		#endif
-		}
-	}
-
-	ack = (wl_cfgp2p_tx_action_frame(wl, dev, af_params, bssidx)) ? false : true;
-	cfg80211_mgmt_tx_status(dev, *cookie, buf, len, ack, GFP_KERNEL);
-
-	kfree(af_params);
-exit:
-	return err;
-}
-
-
-static void
-wl_cfg80211_mgmt_frame_register(struct wiphy *wiphy, struct net_device *dev,
-	u16 frame_type, bool reg)
-{
-
-	WL_DBG(("%s: frame_type: %x, reg: %d\n", __func__, frame_type, reg));
-
-	if (frame_type != (IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_REQ))
-		return;
-
-	return;
-}
-
-
-static s32
-wl_cfg80211_change_bss(struct wiphy *wiphy,
-	struct net_device *dev,
-	struct bss_parameters *params)
-{
-	if (params->use_cts_prot >= 0) {
-	}
-
-	if (params->use_short_preamble >= 0) {
-	}
-
-	if (params->use_short_slot_time >= 0) {
-	}
-
-	if (params->basic_rates) {
-	}
-
-	if (params->ap_isolate >= 0) {
-	}
-
-	if (params->ht_opmode >= 0) {
-	}
-
-	return 0;
-}
-
-static s32
-wl_cfg80211_set_channel(struct wiphy *wiphy, struct net_device *dev,
-	struct ieee80211_channel *chan,
-	enum nl80211_channel_type channel_type)
-{
-	s32 channel;
-	s32 err = BCME_OK;
-
-	channel = ieee80211_frequency_to_channel(chan->center_freq);
-	WL_DBG(("netdev_ifidx(%d), chan_type(%d) target channel(%d) \n",
-		dev->ifindex, channel_type, channel));
-	wldev_ioctl(dev, WLC_SET_CHANNEL, &channel, sizeof(channel), false);
-
-	return err;
-}
-
-static s32
-wl_validate_wpa2ie(struct net_device *dev, bcm_tlv_t *wpa2ie, s32 bssidx)
-{
-	s32 len = 0;
-	s32 err = BCME_OK;
-	u16 auth = 0; /* d11 open authentication */
-	u16 count;
-	u32 wsec;
-	u32 pval = 0;
-	u32 gval = 0;
-	u32 wpa_auth = 0;
-	u8* tmp;
-	wpa_suite_mcast_t *mcast;
-	wpa_suite_ucast_t *ucast;
-	wpa_suite_auth_key_mgmt_t *mgmt;
-	if (wpa2ie == NULL)
-		goto exit;
-
-	WL_DBG(("Enter \n"));
-	len =  wpa2ie->len;
-	/* check the mcast cipher */
-	mcast = (wpa_suite_mcast_t *)&wpa2ie->data[WPA2_VERSION_LEN];
-	tmp = mcast->oui;
-	switch (tmp[DOT11_OUI_LEN]) {
-		case WPA_CIPHER_NONE:
-			gval = 0;
-			break;
-		case WPA_CIPHER_WEP_40:
-		case WPA_CIPHER_WEP_104:
-			gval = WEP_ENABLED;
-			break;
-		case WPA_CIPHER_TKIP:
-			gval = TKIP_ENABLED;
-			break;
-		case WPA_CIPHER_AES_CCM:
-			gval = AES_ENABLED;
-			break;
-		default:
-			WL_ERR(("No Security Info\n"));
-			break;
-	}
-	len -= WPA_SUITE_LEN;
-	/* check the unicast cipher */
-	ucast = (wpa_suite_ucast_t *)&mcast[1];
-	count = ltoh16_ua(&ucast->count);
-	tmp = ucast->list[0].oui;
-	switch (tmp[DOT11_OUI_LEN]) {
-		case WPA_CIPHER_NONE:
-			pval = 0;
-			break;
-		case WPA_CIPHER_WEP_40:
-		case WPA_CIPHER_WEP_104:
-			pval = WEP_ENABLED;
-			break;
-		case WPA_CIPHER_TKIP:
-			pval = TKIP_ENABLED;
-			break;
-		case WPA_CIPHER_AES_CCM:
-			pval = AES_ENABLED;
-			break;
-		default:
-			WL_ERR(("No Security Info\n"));
-	}
-	/* FOR WPS , set SEC_OW_ENABLED */
-	wsec = (pval | gval | SES_OW_ENABLED);
-	/* check the AKM */
-	mgmt = (wpa_suite_auth_key_mgmt_t *)&ucast->list[1];
-	count = ltoh16_ua(&mgmt->count);
-	tmp = (u8 *)&mgmt->list[0];
-	switch (tmp[DOT11_OUI_LEN]) {
-		case RSN_AKM_NONE:
-			wpa_auth = WPA_AUTH_NONE;
-			break;
-		case RSN_AKM_UNSPECIFIED:
-			wpa_auth = WPA2_AUTH_UNSPECIFIED;
-			break;
-		case RSN_AKM_PSK:
-			wpa_auth = WPA2_AUTH_PSK;
-			break;
-		default:
-			WL_ERR(("No Key Mgmt Info\n"));
-	}
-	/* set auth */
-	err = wldev_iovar_setint_bsscfg(dev, "auth", auth, bssidx);
-	if (err < 0) {
-		WL_ERR(("auth error %d\n", err));
-		return BCME_ERROR;
-	}
-	/* set wsec */
-	err = wldev_iovar_setint_bsscfg(dev, "wsec", wsec, bssidx);
-	if (err < 0) {
-		WL_ERR(("wsec error %d\n", err));
-		return BCME_ERROR;
-	}
-	/* set upper-layer auth */
-	err = wldev_iovar_setint_bsscfg(dev, "wpa_auth", wpa_auth, bssidx);
-	if (err < 0) {
-		WL_ERR(("wpa_auth error %d\n", err));
-		return BCME_ERROR;
-	}
-exit:
-	return 0;
-}
-
-static s32
-wl_validate_wpaie(struct net_device *dev, wpa_ie_fixed_t *wpaie, s32 bssidx)
-{
-	wpa_suite_mcast_t *mcast;
-	wpa_suite_ucast_t *ucast;
-	wpa_suite_auth_key_mgmt_t *mgmt;
-	u16 auth = 0; /* d11 open authentication */
-	u16 count;
-	s32 err = BCME_OK;
-	s32 len = 0;
-	u32 i;
-	u32 wsec;
-	u32 pval = 0;
-	u32 gval = 0;
-	u32 wpa_auth = 0;
-	u32 tmp = 0;
-
-	if (wpaie == NULL)
-		goto exit;
-	WL_DBG(("Enter \n"));
-	len = wpaie->length;    /* value length */
-	len -= WPA_IE_TAG_FIXED_LEN;
-	/* check for multicast cipher suite */
-	if (len < WPA_SUITE_LEN) {
-		WL_INFO(("no multicast cipher suite\n"));
-		goto exit;
-	}
-
-	/* pick up multicast cipher */
-	mcast = (wpa_suite_mcast_t *)&wpaie[1];
-	len -= WPA_SUITE_LEN;
-	if (!bcmp(mcast->oui, WPA_OUI, WPA_OUI_LEN)) {
-		if (IS_WPA_CIPHER(mcast->type)) {
-			tmp = 0;
-			switch (mcast->type) {
-				case WPA_CIPHER_NONE:
-					tmp = 0;
-					break;
-				case WPA_CIPHER_WEP_40:
-				case WPA_CIPHER_WEP_104:
-					tmp = WEP_ENABLED;
-					break;
-				case WPA_CIPHER_TKIP:
-					tmp = TKIP_ENABLED;
-					break;
-				case WPA_CIPHER_AES_CCM:
-					tmp = AES_ENABLED;
-					break;
-				default:
-					WL_ERR(("No Security Info\n"));
-			}
-			gval |= tmp;
-		}
-	}
-	/* Check for unicast suite(s) */
-	if (len < WPA_IE_SUITE_COUNT_LEN) {
-		WL_INFO(("no unicast suite\n"));
-		goto exit;
-	}
-	/* walk thru unicast cipher list and pick up what we recognize */
-	ucast = (wpa_suite_ucast_t *)&mcast[1];
-	count = ltoh16_ua(&ucast->count);
-	len -= WPA_IE_SUITE_COUNT_LEN;
-	for (i = 0; i < count && len >= WPA_SUITE_LEN;
-		i++, len -= WPA_SUITE_LEN) {
-		if (!bcmp(ucast->list[i].oui, WPA_OUI, WPA_OUI_LEN)) {
-			if (IS_WPA_CIPHER(ucast->list[i].type)) {
-				tmp = 0;
-				switch (ucast->list[i].type) {
-					case WPA_CIPHER_NONE:
-						tmp = 0;
-						break;
-					case WPA_CIPHER_WEP_40:
-					case WPA_CIPHER_WEP_104:
-						tmp = WEP_ENABLED;
-						break;
-					case WPA_CIPHER_TKIP:
-						tmp = TKIP_ENABLED;
-						break;
-					case WPA_CIPHER_AES_CCM:
-						tmp = AES_ENABLED;
-						break;
-					default:
-						WL_ERR(("No Security Info\n"));
-				}
-				pval |= tmp;
-			}
-		}
-	}
-	len -= (count - i) * WPA_SUITE_LEN;
-	/* Check for auth key management suite(s) */
-	if (len < WPA_IE_SUITE_COUNT_LEN) {
-		WL_INFO((" no auth key mgmt suite\n"));
-		goto exit;
-	}
-	/* walk thru auth management suite list and pick up what we recognize */
-	mgmt = (wpa_suite_auth_key_mgmt_t *)&ucast->list[count];
-	count = ltoh16_ua(&mgmt->count);
-	len -= WPA_IE_SUITE_COUNT_LEN;
-	for (i = 0; i < count && len >= WPA_SUITE_LEN;
-		i++, len -= WPA_SUITE_LEN) {
-		if (!bcmp(mgmt->list[i].oui, WPA_OUI, WPA_OUI_LEN)) {
-			if (IS_WPA_AKM(mgmt->list[i].type)) {
-				tmp = 0;
-				switch (mgmt->list[i].type) {
-					case RSN_AKM_NONE:
-						tmp = WPA_AUTH_NONE;
-						break;
-					case RSN_AKM_UNSPECIFIED:
-						tmp = WPA_AUTH_UNSPECIFIED;
-						break;
-					case RSN_AKM_PSK:
-						tmp = WPA_AUTH_PSK;
-						break;
-					default:
-						WL_ERR(("No Key Mgmt Info\n"));
-				}
-				wpa_auth |= tmp;
-			}
-		}
-
-	}
-	/* FOR WPS , set SEC_OW_ENABLED */
-	wsec = (pval | gval | SES_OW_ENABLED);
-	/* set auth */
-	err = wldev_iovar_setint_bsscfg(dev, "auth", auth, bssidx);
-	if (err < 0) {
-		WL_ERR(("auth error %d\n", err));
-		return BCME_ERROR;
-	}
-	/* set wsec */
-	err = wldev_iovar_setint_bsscfg(dev, "wsec", wsec, bssidx);
-	if (err < 0) {
-		WL_ERR(("wsec error %d\n", err));
-		return BCME_ERROR;
-	}
-	/* set upper-layer auth */
-	err = wldev_iovar_setint_bsscfg(dev, "wpa_auth", wpa_auth, bssidx);
-	if (err < 0) {
-		WL_ERR(("wpa_auth error %d\n", err));
-		return BCME_ERROR;
-	}
-exit:
-	return 0;
-}
-
-static s32
-wl_cfg80211_set_beacon(struct wiphy *wiphy, struct net_device *dev,
-	struct beacon_parameters *info)
-{
-	s32 err = BCME_OK;
-	bcm_tlv_t *ssid_ie;
-	wlc_ssid_t ssid;
-	struct wl_priv *wl = wiphy_priv(wiphy);
-	struct wl_join_params join_params;
-	wpa_ie_fixed_t *wps_ie;
-	wpa_ie_fixed_t *wpa_ie;
-	bcm_tlv_t *wpa2_ie;
-	wifi_p2p_ie_t *p2p_ie;
-	bool is_bssup = false;
-	bool update_bss = false;
-	bool pbc = false;
-	u16 wpsie_len = 0;
-	u16 p2pie_len = 0;
-	u8 beacon_ie[IE_MAX_LEN];
-	s32 ie_offset = 0;
-	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
-	s32 infra = 1;
-	s32 join_params_size = 0;
-	s32 ap = 0;
-	WL_DBG(("interval (%d) dtim_period (%d) head_len (%d) tail_len (%d)\n",
-		info->interval, info->dtim_period, info->head_len, info->tail_len));
-	if (wl->p2p_supported && p2p_on(wl) &&
-		(bssidx >= wl_to_p2p_bss_bssidx(wl,
-		P2PAPI_BSSCFG_CONNECTION))) {
-		memset(beacon_ie, 0, sizeof(beacon_ie));
-		/* We don't need to set beacon for P2P_GO,
-		 * but need to parse ssid from beacon_parameters
-		 * because there is no way to set ssid
-		 */
-		ie_offset = DOT11_MGMT_HDR_LEN + DOT11_BCN_PRB_FIXED_LEN;
-		/* find the SSID */
-		if ((ssid_ie = bcm_parse_tlvs((u8 *)&info->head[ie_offset],
-			info->head_len - ie_offset,
-			DOT11_MNG_SSID_ID)) != NULL) {
-			memcpy(wl->p2p->ssid.SSID, ssid_ie->data, ssid_ie->len);
-			wl->p2p->ssid.SSID_len = ssid_ie->len;
-			WL_DBG(("SSID (%s) in Head \n", ssid_ie->data));
-
-		} else {
-			WL_ERR(("No SSID in beacon \n"));
-		}
-
-		/* find the WPSIE */
-		if ((wps_ie = wl_cfgp2p_find_wpsie((u8 *)info->tail, info->tail_len)) != NULL) {
-			wpsie_len = wps_ie->length + WPA_RSN_IE_TAG_FIXED_LEN;
-			/*
-			 * Should be compared with saved ie before saving it
-			 */
-			wl_validate_wps_ie((char *) wps_ie, &pbc);
-			memcpy(beacon_ie, wps_ie, wpsie_len);
-		} else {
-			WL_ERR(("No WPSIE in beacon \n"));
-		}
-
-
-		/* find the P2PIE */
-		if ((p2p_ie = wl_cfgp2p_find_p2pie((u8 *)info->tail, info->tail_len)) != NULL) {
-			/* Total length of P2P Information Element */
-			p2pie_len = p2p_ie->len + sizeof(p2p_ie->len) + sizeof(p2p_ie->id);
-		#ifdef ENABLE_DRIVER_CHANGE_IFADDR /* We are now doing this in supplicant */
-			/* Have to change device address in dev_id attribute because Supplicant
-			 * use primary eth0 address
-			 */
-			wl_cfg80211_change_ifaddr((u8 *)p2p_ie, &wl->p2p_dev_addr, P2P_SEID_DEV_ID);
-		#endif
-			memcpy(&beacon_ie[wpsie_len], p2p_ie, p2pie_len);
-
-		} else {
-			WL_ERR(("No P2PIE in beacon \n"));
-		}
-		wl_cfgp2p_set_management_ie(wl, dev, bssidx, VNDR_IE_BEACON_FLAG,
-			beacon_ie, wpsie_len + p2pie_len);
-		wl_cfgp2p_set_management_ie(wl, dev, bssidx, VNDR_IE_ASSOCRSP_FLAG,
-			beacon_ie, wpsie_len + p2pie_len);
-
-		/* find the RSN_IE */
-		if ((wpa2_ie = bcm_parse_tlvs((u8 *)info->tail, info->tail_len,
-			DOT11_MNG_RSN_ID)) != NULL) {
-			WL_DBG((" WPA2 IE is found\n"));
-		}
-		is_bssup = wl_cfgp2p_bss_isup(dev, bssidx);
-
-		if (!is_bssup && (wpa2_ie != NULL)) {
-			if ((err = wl_validate_wpa2ie(dev, wpa2_ie, bssidx)) < 0) {
-					WL_ERR(("WPA2 IE parsing error"));
-					return BCME_ERROR;
-			}
-			err = wldev_ioctl(dev, WLC_SET_INFRA, &infra, sizeof(s32), false);
-			if (err < 0) {
-				WL_ERR(("SET INFRA error %d\n", err));
-				return err;
-			}
-			err = wldev_iovar_setbuf_bsscfg(dev, "ssid", &wl->p2p->ssid,
-				sizeof(wl->p2p->ssid), ioctlbuf, sizeof(ioctlbuf), bssidx);
-			if (err < 0) {
-				WL_ERR(("GO SSID setting error %d\n", err));
-				return err;
-			}
-			if ((err = wl_cfgp2p_bss(dev, bssidx, 1)) < 0) {
-				WL_ERR(("GO Bring up error %d\n", err));
-				return err;
-			}
-		}
-	} else if (wl_get_drv_status(wl, AP_CREATING)) {
-		ie_offset = DOT11_MGMT_HDR_LEN + DOT11_BCN_PRB_FIXED_LEN;
-		ap = 1;
-		/* find the SSID */
-		if ((ssid_ie = bcm_parse_tlvs((u8 *)&info->head[ie_offset],
-			info->head_len - ie_offset,
-			DOT11_MNG_SSID_ID)) != NULL) {
-			memset(&ssid, 0, sizeof(wlc_ssid_t));
-			memcpy(ssid.SSID, ssid_ie->data, ssid_ie->len);
-			WL_DBG(("SSID is (%s) in Head \n", ssid.SSID));
-			ssid.SSID_len = ssid_ie->len;
-			wldev_iovar_setint(dev, "mpc", 0);
-			wldev_ioctl(dev, WLC_DOWN, &ap, sizeof(s32), false);
-			wldev_ioctl(dev, WLC_SET_INFRA, &infra, sizeof(s32), false);
-			if ((err = wldev_ioctl(dev, WLC_SET_AP, &ap, sizeof(s32), false)) < 0) {
-				WL_ERR(("setting AP mode failed %d \n", err));
-				return err;
-			}
-			/* find the RSN_IE */
-			if ((wpa2_ie = bcm_parse_tlvs((u8 *)info->tail, info->tail_len,
-				DOT11_MNG_RSN_ID)) != NULL) {
-				WL_DBG((" WPA2 IE is found\n"));
-			}
-			/* find the WPA_IE */
-			if ((wpa_ie = wl_cfgp2p_find_wpaie((u8 *)info->tail,
-			info->tail_len)) != NULL) {
-				WL_DBG((" WPA IE is found\n"));
-			}
-			if ((wpa_ie != NULL || wpa2_ie != NULL)) {
-				if (wl_validate_wpa2ie(dev, wpa2_ie, bssidx)  < 0 ||
-					wl_validate_wpaie(dev, wpa_ie, bssidx) < 0) {
-					wl->ap_info->security_mode = false;
-					return BCME_ERROR;
-				}
-				wl->ap_info->security_mode = true;
-				kfree(wl->ap_info->rsn_ie);
-				kfree(wl->ap_info->wpa_ie);
-				kfree(wl->ap_info->wps_ie);
-				if (wpa_ie != NULL) {
-					/* WPAIE */
-					wl->ap_info->rsn_ie = NULL;
-					wl->ap_info->wpa_ie = kmemdup(wpa_ie,
-						wpa_ie->length + WPA_RSN_IE_TAG_FIXED_LEN,
-						GFP_KERNEL);
-				} else {
-					/* RSNIE */
-					wl->ap_info->wpa_ie = NULL;
-					wl->ap_info->rsn_ie = kmemdup(wpa2_ie,
-						wpa2_ie->len + WPA_RSN_IE_TAG_FIXED_LEN,
-						GFP_KERNEL);
-				}
-			} else
-				wl->ap_info->security_mode = false;
-			/* find the WPSIE */
-			if ((wps_ie = wl_cfgp2p_find_wpsie((u8 *)info->tail,
-				info->tail_len)) != NULL) {
-				wpsie_len = wps_ie->length +WPA_RSN_IE_TAG_FIXED_LEN;
-				/*
-				* Should be compared with saved ie before saving it
-				*/
-				wl_validate_wps_ie((char *) wps_ie, &pbc);
-				memcpy(beacon_ie, wps_ie, wpsie_len);
-				wl_cfgp2p_set_management_ie(wl, dev, bssidx, VNDR_IE_BEACON_FLAG,
-				beacon_ie, wpsie_len);
-				wl->ap_info->wps_ie = kmemdup(wps_ie, 	wpsie_len, GFP_KERNEL);
-				/* add WLC_E_PROBREQ_MSG event to respose probe_request from STA */
-				wl_dongle_add_remove_eventmsg(dev, WLC_E_PROBREQ_MSG, pbc);
-			} else {
-				WL_DBG(("No WPSIE in beacon \n"));
-			}
-			wldev_ioctl(dev, WLC_UP, &ap, sizeof(s32), false);
-
-			memset(&join_params, 0, sizeof(join_params));
-			/* join parameters starts with ssid */
-			join_params_size = sizeof(join_params.ssid);
-			memcpy(join_params.ssid.SSID, ssid.SSID, ssid.SSID_len);
-			join_params.ssid.SSID_len = htod32(ssid.SSID_len);
-			/* create softap */
-			if ((err = wldev_ioctl(dev, WLC_SET_SSID, &join_params,
-				join_params_size, false)) == 0) {
-				wl_clr_drv_status(wl, AP_CREATING);
-				wl_set_drv_status(wl, AP_CREATED);
-			}
-		}
-	} else if (wl_get_drv_status(wl, AP_CREATED)) {
-		ap = 1;
-		/* find the WPSIE */
-		if ((wps_ie = wl_cfgp2p_find_wpsie((u8 *)info->tail, info->tail_len)) != NULL) {
-			wpsie_len = wps_ie->length + WPA_RSN_IE_TAG_FIXED_LEN;
-			/*
-			 * Should be compared with saved ie before saving it
-			 */
-			wl_validate_wps_ie((char *) wps_ie, &pbc);
-			memcpy(beacon_ie, wps_ie, wpsie_len);
-			wl_cfgp2p_set_management_ie(wl, dev, bssidx, VNDR_IE_BEACON_FLAG,
-			beacon_ie, wpsie_len);
-			if (wl->ap_info->wps_ie &&
-				memcmp(wl->ap_info->wps_ie, wps_ie, wpsie_len)) {
-				WL_DBG((" WPS IE is changed\n"));
-				kfree(wl->ap_info->wps_ie);
-				wl->ap_info->wps_ie = kmemdup(wps_ie, 	wpsie_len, GFP_KERNEL);
-				/* add WLC_E_PROBREQ_MSG event to respose probe_request from STA */
-				wl_dongle_add_remove_eventmsg(dev, WLC_E_PROBREQ_MSG, pbc);
-			} else if (wl->ap_info->wps_ie == NULL) {
-				WL_DBG((" WPS IE is added\n"));
-				wl->ap_info->wps_ie = kmemdup(wps_ie, 	wpsie_len, GFP_KERNEL);
-				/* add WLC_E_PROBREQ_MSG event to respose probe_request from STA */
-				wl_dongle_add_remove_eventmsg(dev, WLC_E_PROBREQ_MSG, pbc);
-			}
-			/* find the RSN_IE */
-			if ((wpa2_ie = bcm_parse_tlvs((u8 *)info->tail, info->tail_len,
-				DOT11_MNG_RSN_ID)) != NULL) {
-				WL_DBG((" WPA2 IE is found\n"));
-			}
-			/* find the WPA_IE */
-			if ((wpa_ie = wl_cfgp2p_find_wpaie((u8 *)info->tail,
-				info->tail_len)) != NULL) {
-				WL_DBG((" WPA IE is found\n"));
-			}
-			if ((wpa_ie != NULL || wpa2_ie != NULL)) {
-				if (!wl->ap_info->security_mode) {
-					/* change from open mode to security mode */
-					update_bss = true;
-					if (wpa_ie != NULL) {
-						wl->ap_info->wpa_ie = kmemdup(wpa_ie,
-						wpa_ie->length + WPA_RSN_IE_TAG_FIXED_LEN,
-						GFP_KERNEL);
-					} else {
-						wl->ap_info->rsn_ie = kmemdup(wpa2_ie,
-						wpa2_ie->len + WPA_RSN_IE_TAG_FIXED_LEN,
-						GFP_KERNEL);
-					}
-				} else if (wl->ap_info->wpa_ie) {
-					/* change from WPA mode to WPA2 mode */
-					if (wpa2_ie != NULL) {
-						update_bss = true;
-						kfree(wl->ap_info->wpa_ie);
-						wl->ap_info->rsn_ie = kmemdup(wpa2_ie,
-						wpa2_ie->len + WPA_RSN_IE_TAG_FIXED_LEN,
-						GFP_KERNEL);
-						wl->ap_info->wpa_ie = NULL;
-					}
-					else if (memcmp(wl->ap_info->wpa_ie,
-						wpa_ie, wpa_ie->length +
-						WPA_RSN_IE_TAG_FIXED_LEN)) {
-						kfree(wl->ap_info->wpa_ie);
-						update_bss = true;
-						wl->ap_info->wpa_ie = kmemdup(wpa_ie,
-						wpa_ie->length + WPA_RSN_IE_TAG_FIXED_LEN,
-						GFP_KERNEL);
-						wl->ap_info->rsn_ie = NULL;
-					}
-				} else {
-					/* change from WPA2 mode to WPA mode */
-					if (wpa_ie != NULL) {
-						update_bss = true;
-						kfree(wl->ap_info->rsn_ie);
-						wl->ap_info->rsn_ie = NULL;
-						wl->ap_info->wpa_ie = kmemdup(wpa_ie,
-						wpa_ie->length + WPA_RSN_IE_TAG_FIXED_LEN,
-						GFP_KERNEL);
-					} else if (memcmp(wl->ap_info->rsn_ie,
-						wpa2_ie, wpa2_ie->len + WPA_RSN_IE_TAG_FIXED_LEN)) {
-						update_bss = true;
-						kfree(wl->ap_info->rsn_ie);
-						wl->ap_info->rsn_ie = kmemdup(wpa2_ie,
-						wpa2_ie->len + WPA_RSN_IE_TAG_FIXED_LEN,
-						GFP_KERNEL);
-						wl->ap_info->wpa_ie = NULL;
-					}
-				}
-				if (update_bss) {
-					wl->ap_info->security_mode = true;
-					wl_cfgp2p_bss(dev, bssidx, 0);
-					if (wl_validate_wpa2ie(dev, wpa2_ie, bssidx)  < 0 ||
-						wl_validate_wpaie(dev, wpa_ie, bssidx) < 0) {
-						return BCME_ERROR;
-					}
-					wl_cfgp2p_bss(dev, bssidx, 1);
-				}
-			}
-		} else {
-			WL_ERR(("No WPSIE in beacon \n"));
-		}
-	}
-	return 0;
-}
-
-#if defined(ANDROID_WIRELESS_PATCH)
-static s32
-wl_cfg80211_drv_start(struct wiphy *wiphy, struct net_device *dev)
-{
-	s32 err = 0;
-
-	printk("Android driver start command\n");
-	return err;
-}
-
-static s32
-wl_cfg80211_drv_stop(struct wiphy *wiphy, struct net_device *dev)
-{
-	s32 err = 0;
-
-	printk("Android driver stop command\n");
-	return err;
-}
-#endif /* defined(ANDROID_WIRELESS_PATCH) */
-
-static struct cfg80211_ops wl_cfg80211_ops = {
-	.add_virtual_intf = wl_cfg80211_add_virtual_iface,
-	.del_virtual_intf = wl_cfg80211_del_virtual_iface,
-	.change_virtual_intf = wl_cfg80211_change_virtual_iface,
-	.scan = wl_cfg80211_scan,
-	.set_wiphy_params = wl_cfg80211_set_wiphy_params,
-	.join_ibss = wl_cfg80211_join_ibss,
-	.leave_ibss = wl_cfg80211_leave_ibss,
-	.get_station = wl_cfg80211_get_station,
-	.set_tx_power = wl_cfg80211_set_tx_power,
-	.get_tx_power = wl_cfg80211_get_tx_power,
-	.add_key = wl_cfg80211_add_key,
-	.del_key = wl_cfg80211_del_key,
-	.get_key = wl_cfg80211_get_key,
-	.set_default_key = wl_cfg80211_config_default_key,
-	.set_default_mgmt_key = wl_cfg80211_config_default_mgmt_key,
-	.set_power_mgmt = wl_cfg80211_set_power_mgmt,
-	.connect = wl_cfg80211_connect,
-	.disconnect = wl_cfg80211_disconnect,
-	.suspend = wl_cfg80211_suspend,
-	.resume = wl_cfg80211_resume,
-	.set_pmksa = wl_cfg80211_set_pmksa,
-	.del_pmksa = wl_cfg80211_del_pmksa,
-	.flush_pmksa = wl_cfg80211_flush_pmksa,
-	.remain_on_channel = wl_cfg80211_remain_on_channel,
-	.cancel_remain_on_channel = wl_cfg80211_cancel_remain_on_channel,
-	.mgmt_tx = wl_cfg80211_mgmt_tx,
-	.mgmt_frame_register = wl_cfg80211_mgmt_frame_register,
-	.change_bss = wl_cfg80211_change_bss,
-	.set_channel = wl_cfg80211_set_channel,
-	.set_beacon = wl_cfg80211_set_beacon,
-#if defined(ANDROID_WIRELESS_PATCH)
-	.drv_start = wl_cfg80211_drv_start,
-	.drv_stop = wl_cfg80211_drv_stop
-#endif
-};
-
-static s32 wl_mode_to_nl80211_iftype(s32 mode)
-{
-	s32 err = 0;
-
-	switch (mode) {
-	case WL_MODE_BSS:
-		return NL80211_IFTYPE_STATION;
-	case WL_MODE_IBSS:
-		return NL80211_IFTYPE_ADHOC;
-	case WL_MODE_AP:
-		return NL80211_IFTYPE_AP;
-	default:
-		return NL80211_IFTYPE_UNSPECIFIED;
-	}
-
-	return err;
-}
-
-static struct wireless_dev *wl_alloc_wdev(struct device *sdiofunc_dev)
-{
-	struct wireless_dev *wdev;
-	s32 err = 0;
-	wdev = kzalloc(sizeof(*wdev), GFP_KERNEL);
-	if (unlikely(!wdev)) {
-		WL_ERR(("Could not allocate wireless device\n"));
-		return ERR_PTR(-ENOMEM);
-	}
-	wdev->wiphy =
-	    wiphy_new(&wl_cfg80211_ops, sizeof(struct wl_priv));
-	if (unlikely(!wdev->wiphy)) {
-		WL_ERR(("Couldn not allocate wiphy device\n"));
-		err = -ENOMEM;
-		goto wiphy_new_out;
-	}
-	set_wiphy_dev(wdev->wiphy, sdiofunc_dev);
-	wdev->wiphy->max_scan_ie_len = WL_SCAN_IE_LEN_MAX;
-	/* Report  how many SSIDs Driver can support per Scan request */
-	wdev->wiphy->max_scan_ssids = WL_SCAN_PARAMS_SSID_MAX;
-	wdev->wiphy->max_num_pmkids = WL_NUM_PMKIDS_MAX;
-	wdev->wiphy->interface_modes =
-	    BIT(NL80211_IFTYPE_STATION) | BIT(NL80211_IFTYPE_ADHOC)
-	    | BIT(NL80211_IFTYPE_AP) | BIT(NL80211_IFTYPE_MONITOR);
-
-	wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &__wl_band_2ghz;
-	wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = &__wl_band_5ghz_a;
-	wdev->wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
-	wdev->wiphy->cipher_suites = __wl_cipher_suites;
-	wdev->wiphy->n_cipher_suites = ARRAY_SIZE(__wl_cipher_suites);
-	wdev->wiphy->max_remain_on_channel_duration = 5000;
-	wdev->wiphy->mgmt_stypes = wl_cfg80211_default_mgmt_stypes;
-#ifndef WL_POWERSAVE_DISABLED
-	wdev->wiphy->flags |= WIPHY_FLAG_PS_ON_BY_DEFAULT;
-#else
-	wdev->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
-#endif				/* !WL_POWERSAVE_DISABLED */
-	wdev->wiphy->flags |= WIPHY_FLAG_NETNS_OK |
-		WIPHY_FLAG_4ADDR_AP |
-#if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 39)
-		WIPHY_FLAG_SUPPORTS_SEPARATE_DEFAULT_KEYS |
-#endif
-		WIPHY_FLAG_4ADDR_STATION;
-
-	WL_DBG(("Registering custom regulatory)\n"));
-	wdev->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY;
-	wiphy_apply_custom_regulatory(wdev->wiphy, &brcm_regdom);
-	/* Now we can register wiphy with cfg80211 module */
-	err = wiphy_register(wdev->wiphy);
-	if (unlikely(err < 0)) {
-		WL_ERR(("Couldn not register wiphy device (%d)\n", err));
-		goto wiphy_register_out;
-	}
-	return wdev;
-
-wiphy_register_out:
-	wiphy_free(wdev->wiphy);
-
-wiphy_new_out:
-	kfree(wdev);
-
-	return ERR_PTR(err);
-}
-
-static void wl_free_wdev(struct wl_priv *wl)
-{
-	int i;
-	struct wireless_dev *wdev = wl->wdev;
-
-	if (unlikely(!wdev)) {
-		WL_ERR(("wdev is invalid\n"));
-		return;
-	}
-
-	for (i = 0; i < VWDEV_CNT; i++) {
-		if ((wl->vwdev[i] != NULL)) {
-			kfree(wl->vwdev[i]);
-			wl->vwdev[i] = NULL;
-		}
-	}
-	wiphy_unregister(wdev->wiphy);
-	wdev->wiphy->dev.parent = NULL;
-	wiphy_free(wdev->wiphy);
-	kfree(wdev);
-}
-
-static s32 wl_inform_bss(struct wl_priv *wl)
-{
-	struct wl_scan_results *bss_list;
-	struct wl_bss_info *bi = NULL;	/* must be initialized */
-	s32 err = 0;
-	s32 i;
-
-	bss_list = wl->bss_list;
-	WL_DBG(("scanned AP count (%d)\n", bss_list->count));
-	bi = next_bss(bss_list, bi);
-	for_each_bss(bss_list, bi, i) {
-		err = wl_inform_single_bss(wl, bi);
-		if (unlikely(err))
-			break;
-	}
-	return err;
-}
-
-static s32 wl_inform_single_bss(struct wl_priv *wl, struct wl_bss_info *bi)
-{
-	struct wiphy *wiphy = wiphy_from_scan(wl);
-	struct ieee80211_mgmt *mgmt;
-	struct ieee80211_channel *channel;
-	struct ieee80211_supported_band *band;
-	struct wl_cfg80211_bss_info *notif_bss_info;
-	struct wl_scan_req *sr = wl_to_sr(wl);
-	struct beacon_proberesp *beacon_proberesp;
-	s32 mgmt_type;
-	u32 signal;
-	u32 freq;
-	s32 err = 0;
-
-	if (unlikely(dtoh32(bi->length) > WL_BSS_INFO_MAX)) {
-		WL_DBG(("Beacon is larger than buffer. Discarding\n"));
-		return err;
-	}
-	notif_bss_info = kzalloc(sizeof(*notif_bss_info) + sizeof(*mgmt)
-		- sizeof(u8) + WL_BSS_INFO_MAX, GFP_KERNEL);
-	if (unlikely(!notif_bss_info)) {
-		WL_ERR(("notif_bss_info alloc failed\n"));
-		return -ENOMEM;
-	}
-	mgmt = (struct ieee80211_mgmt *)notif_bss_info->frame_buf;
-	notif_bss_info->channel =
-		bi->ctl_ch ? bi->ctl_ch : CHSPEC_CHANNEL(bi->chanspec);
-
-	if (notif_bss_info->channel <= CH_MAX_2G_CHANNEL)
-		band = wiphy->bands[IEEE80211_BAND_2GHZ];
-	else
-		band = wiphy->bands[IEEE80211_BAND_5GHZ];
-	notif_bss_info->rssi = bi->RSSI;
-	memcpy(mgmt->bssid, &bi->BSSID, ETHER_ADDR_LEN);
-	mgmt_type = wl->active_scan ?
-		IEEE80211_STYPE_PROBE_RESP : IEEE80211_STYPE_BEACON;
-	if (!memcmp(bi->SSID, sr->ssid.SSID, bi->SSID_len)) {
-	    mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT | mgmt_type);
-	}
-	beacon_proberesp = wl->active_scan ?
-		(struct beacon_proberesp *)&mgmt->u.probe_resp :
-		(struct beacon_proberesp *)&mgmt->u.beacon;
-	beacon_proberesp->timestamp = 0;
-	beacon_proberesp->beacon_int = cpu_to_le16(bi->beacon_period);
-	beacon_proberesp->capab_info = cpu_to_le16(bi->capability);
-	wl_rst_ie(wl);
-	/*
-	* wl_add_ie is not necessary because it can only add duplicated
-	* SSID, rate information to frame_buf
-	*/
-	/*
-	* wl_add_ie(wl, WLAN_EID_SSID, bi->SSID_len, bi->SSID);
-	* wl_add_ie(wl, WLAN_EID_SUPP_RATES, bi->rateset.count,
-	* bi->rateset.rates);
-	*/
-	wl_mrg_ie(wl, ((u8 *) bi) + bi->ie_offset, bi->ie_length);
-	wl_cp_ie(wl, beacon_proberesp->variable, WL_BSS_INFO_MAX -
-		offsetof(struct wl_cfg80211_bss_info, frame_buf));
-	notif_bss_info->frame_len = offsetof(struct ieee80211_mgmt,
-		u.beacon.variable) + wl_get_ielen(wl);
-#if LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 38) && !defined(WL_COMPAT_WIRELESS)
-	freq = ieee80211_channel_to_frequency(notif_bss_info->channel);
-#else
-	freq = ieee80211_channel_to_frequency(notif_bss_info->channel, band->band);
-#endif
-	channel = ieee80211_get_channel(wiphy, freq);
-
-	WL_DBG(("SSID : \"%s\", rssi %d, channel %d, capability : 0x04%x, bssid %pM\n",
-		bi->SSID,
-		notif_bss_info->rssi, notif_bss_info->channel,
-		mgmt->u.beacon.capab_info, &bi->BSSID));
-
-	signal = notif_bss_info->rssi * 100;
-	if (unlikely(!cfg80211_inform_bss_frame(wiphy, channel, mgmt,
-		le16_to_cpu
-		(notif_bss_info->frame_len),
-		signal, GFP_KERNEL))) {
-		WL_ERR(("cfg80211_inform_bss_frame error\n"));
-		kfree(notif_bss_info);
-		return -EINVAL;
-	}
-	kfree(notif_bss_info);
-
-	return err;
-}
-
-static bool wl_is_linkup(struct wl_priv *wl, const wl_event_msg_t *e, struct net_device *ndev)
-{
-	u32 event = ntoh32(e->event_type);
-	u32 status =  ntoh32(e->status);
-	u16 flags = ntoh16(e->flags);
-
-	WL_DBG(("event %d, status %d\n", event, status));
-	if (event == WLC_E_SET_SSID) {
-		if (status == WLC_E_STATUS_SUCCESS) {
-			if (!wl_is_ibssmode(wl, ndev))
-				return true;
-		}
-	} else if (event == WLC_E_LINK) {
-		if (flags & WLC_EVENT_MSG_LINK)
-			return true;
-	}
-
-	WL_DBG(("wl_is_linkup false\n"));
-	return false;
-}
-
-static bool wl_is_linkdown(struct wl_priv *wl, const wl_event_msg_t *e)
-{
-	u32 event = ntoh32(e->event_type);
-	u16 flags = ntoh16(e->flags);
-
-	if (event == WLC_E_DEAUTH_IND ||
-	event == WLC_E_DISASSOC_IND ||
-	event == WLC_E_DISASSOC ||
-	event == WLC_E_DEAUTH) {
-		return true;
-	} else if (event == WLC_E_LINK) {
-		if (!(flags & WLC_EVENT_MSG_LINK))
-			return true;
-	}
-
-	return false;
-}
-
-static bool wl_is_nonetwork(struct wl_priv *wl, const wl_event_msg_t *e)
-{
-	u32 event = ntoh32(e->event_type);
-	u32 status = ntoh32(e->status);
-
-	if (event == WLC_E_LINK && status == WLC_E_STATUS_NO_NETWORKS)
-		return true;
-	if (event == WLC_E_SET_SSID && status != WLC_E_STATUS_SUCCESS)
-		return true;
-
-	return false;
-}
-
-static s32
-wl_notify_connect_status(struct wl_priv *wl, struct net_device *ndev,
-	const wl_event_msg_t *e, void *data)
-{
-	bool act;
-	bool isfree = false;
-	s32 err = 0;
-	s32 freq;
-	s32 channel;
-	u8 body[200];
-	u32 event = ntoh32(e->event_type);
-	u32 reason = ntoh32(e->reason);
-	u32 len = ntoh32(e->datalen);
-	u16 fc = 0;
-	u8 *mgmt_frame;
-	u8 bsscfgidx = e->bsscfgidx;
-	struct ieee80211_supported_band *band;
-	struct ether_addr da;
-	struct ether_addr bssid;
-	struct wiphy *wiphy = wl_to_wiphy(wl);
-	channel_info_t ci;
-
-	memset(body, 0, sizeof(body));
-	WL_DBG(("Enter \n"));
-
-	if (get_mode_by_netdev(wl, ndev) == WL_MODE_AP) {
-		memcpy(body, data, len);
-		wldev_iovar_getbuf_bsscfg(ndev, "cur_etheraddr",
-		&da, sizeof(struct ether_addr), ioctlbuf, sizeof(ioctlbuf), bsscfgidx);
-		wldev_ioctl(ndev, WLC_GET_BSSID, &bssid, ETHER_ADDR_LEN, false);
-		switch (event) {
-			case WLC_E_ASSOC_IND:
-				fc = FC_ASSOC_REQ;
-				break;
-			case WLC_E_REASSOC_IND:
-				fc = FC_REASSOC_REQ;
-				break;
-			case WLC_E_DISASSOC_IND:
-				fc = FC_DISASSOC;
-				break;
-			case WLC_E_DEAUTH_IND:
-				fc = FC_DEAUTH;
-				break;
-			case WLC_E_DEAUTH:
-				fc = FC_DEAUTH;
-				break;
-			default:
-				fc = 0;
-				goto exit;
-		}
-		if ((err = wldev_ioctl(ndev, WLC_GET_CHANNEL, &ci, sizeof(ci), false)))
-			return err;
-
-		channel = dtoh32(ci.hw_channel);
-		if (channel <= CH_MAX_2G_CHANNEL)
-			band = wiphy->bands[IEEE80211_BAND_2GHZ];
-		else
-			band = wiphy->bands[IEEE80211_BAND_5GHZ];
-
-#if LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 38) && !defined(WL_COMPAT_WIRELESS)
-		freq = ieee80211_channel_to_frequency(channel);
-#else
-		freq = ieee80211_channel_to_frequency(channel, band->band);
-#endif
-
-		err = wl_frame_get_mgmt(fc, &da, &e->addr, &bssid,
-		&mgmt_frame, &len, body);
-		if (err < 0)
-				goto exit;
-		isfree = true;
-
-		if (event == WLC_E_ASSOC_IND && reason == DOT11_SC_SUCCESS) {
-			cfg80211_send_rx_assoc(ndev, mgmt_frame, len);
-		} else if (event == WLC_E_DISASSOC_IND) {
-			cfg80211_send_disassoc(ndev, mgmt_frame, len);
-		} else if ((event == WLC_E_DEAUTH_IND) || (event == WLC_E_DEAUTH)) {
-			cfg80211_send_deauth(ndev, mgmt_frame, len);
-		}
-
-	} else {
-		WL_DBG(("wl_notify_connect_status : event %d status : %d \n",
-			ntoh32(e->event_type), ntoh32(e->status)));
-		if (wl_is_linkup(wl, e, ndev)) {
-			wl_link_up(wl);
-			act = true;
-			wl_update_prof(wl, e, &act, WL_PROF_ACT);
-			if (wl_is_ibssmode(wl, ndev)) {
-				printk("cfg80211_ibss_joined");
-				cfg80211_ibss_joined(ndev, (s8 *)&e->addr,
-					GFP_KERNEL);
-				WL_DBG(("joined in IBSS network\n"));
-			} else {
-				if (!wl_get_drv_status(wl, DISCONNECTING)) {
-					printk("wl_bss_connect_done succeeded");
-					wl_bss_connect_done(wl, ndev, e, data, true);
-					WL_DBG(("joined in BSS network \"%s\"\n",
-					((struct wlc_ssid *)
-					 wl_read_prof(wl, WL_PROF_SSID))->SSID));
-				}
-			}
-
-		} else if (wl_is_linkdown(wl, e)) {
-			if (wl->scan_request) {
-				if (wl->escan_on) {
-					wl_notify_escan_complete(wl, true);
-				} else
-					wl_iscan_aborted(wl);
-			}
-			if (wl_get_drv_status(wl, CONNECTED)) {
-				printk("link down, call cfg80211_disconnected ");
-				wl_clr_drv_status(wl, CONNECTED);
-				cfg80211_disconnected(ndev, 0, NULL, 0, GFP_KERNEL);
-				wl_link_down(wl);
-				wl_init_prof(wl->profile);
-			} else if (wl_get_drv_status(wl, CONNECTING)) {
-				printk("link down, during connecting");
-				wl_bss_connect_done(wl, ndev, e, data, false);
-			}
-			wl_clr_drv_status(wl, DISCONNECTING);
-
-		} else if (wl_is_nonetwork(wl, e)) {
-			printk("connect failed e->status 0x%x", (int)ntoh32(e->status));
-			if (wl_get_drv_status(wl, CONNECTING))
-				wl_bss_connect_done(wl, ndev, e, data, false);
-		} else {
-			printk("%s nothing\n", __FUNCTION__);
-		}
-		printk("\n");
-	}
-exit:
-	if (isfree)
-		kfree(mgmt_frame);
-	return err;
-}
-
-static s32
-wl_notify_roaming_status(struct wl_priv *wl, struct net_device *ndev,
-	const wl_event_msg_t *e, void *data)
-{
-	bool act;
-	s32 err = 0;
-	u32 event = be32_to_cpu(e->event_type);
-	u32 status = be32_to_cpu(e->status);
-	WL_DBG(("Enter \n"));
-	if (event == WLC_E_ROAM && status == WLC_E_STATUS_SUCCESS) {
-		if (wl_get_drv_status(wl, CONNECTED))
-			wl_bss_roaming_done(wl, ndev, e, data);
-		else
-			wl_bss_connect_done(wl, ndev, e, data, true);
-		act = true;
-		wl_update_prof(wl, e, &act, WL_PROF_ACT);
-	}
-	return err;
-}
-
-static __used s32
-wl_dev_bufvar_set(struct net_device *dev, s8 *name, s8 *buf, s32 len)
-{
-	struct wl_priv *wl = wlcfg_drv_priv;
-	u32 buflen;
-
-	buflen = bcm_mkiovar(name, buf, len, wl->ioctl_buf, WL_IOCTL_LEN_MAX);
-	BUG_ON(unlikely(!buflen));
-
-	return wldev_ioctl(dev, WLC_SET_VAR, wl->ioctl_buf, buflen, false);
-}
-
-static s32
-wl_dev_bufvar_get(struct net_device *dev, s8 *name, s8 *buf,
-	s32 buf_len)
-{
-	struct wl_priv *wl = wlcfg_drv_priv;
-	u32 len;
-	s32 err = 0;
-
-	len = bcm_mkiovar(name, NULL, 0, wl->ioctl_buf, WL_IOCTL_LEN_MAX);
-	BUG_ON(unlikely(!len));
-	err = wldev_ioctl(dev, WLC_GET_VAR, (void *)wl->ioctl_buf,
-		WL_IOCTL_LEN_MAX, false);
-	if (unlikely(err)) {
-		WL_ERR(("error (%d)\n", err));
-		return err;
-	}
-	memcpy(buf, wl->ioctl_buf, buf_len);
-
-	return err;
-}
-
-static s32 wl_get_assoc_ies(struct wl_priv *wl, struct net_device *ndev)
-{
-	wl_assoc_info_t assoc_info;
-	struct wl_connect_info *conn_info = wl_to_conn(wl);
-	s32 err = 0;
-
-	WL_DBG(("Enter \n"));
-	err = wl_dev_bufvar_get(ndev, "assoc_info", wl->extra_buf,
-		WL_ASSOC_INFO_MAX);
-	if (unlikely(err)) {
-		WL_ERR(("could not get assoc info (%d)\n", err));
-		return err;
-	}
-	memcpy(&assoc_info, wl->extra_buf, sizeof(wl_assoc_info_t));
-	assoc_info.req_len = htod32(assoc_info.req_len);
-	assoc_info.resp_len = htod32(assoc_info.resp_len);
-	assoc_info.flags = htod32(assoc_info.flags);
-	if (conn_info->req_ie_len) {
-		conn_info->req_ie_len = 0;
-		bzero(conn_info->req_ie, sizeof(conn_info->req_ie));
-	}
-	if (conn_info->resp_ie_len) {
-		conn_info->resp_ie_len = 0;
-		bzero(conn_info->resp_ie, sizeof(conn_info->resp_ie));
-	}
-	if (assoc_info.req_len) {
-		err = wl_dev_bufvar_get(ndev, "assoc_req_ies", wl->extra_buf,
-			WL_ASSOC_INFO_MAX);
-		if (unlikely(err)) {
-			WL_ERR(("could not get assoc req (%d)\n", err));
-			return err;
-		}
-		conn_info->req_ie_len = assoc_info.req_len - sizeof(struct dot11_assoc_req);
-		if (assoc_info.flags & WLC_ASSOC_REQ_IS_REASSOC) {
-			conn_info->req_ie_len -= ETHER_ADDR_LEN;
-		}
-		if (conn_info->req_ie_len <= MAX_REQ_LINE)
-			memcpy(conn_info->req_ie, wl->extra_buf, conn_info->req_ie_len);
-		else {
-			WL_ERR(("%s IE size %d above max %d size \n",
-				__FUNCTION__, conn_info->req_ie_len, MAX_REQ_LINE));
-			return err;
-		}
-	} else {
-		conn_info->req_ie_len = 0;
-	}
-	if (assoc_info.resp_len) {
-		err = wl_dev_bufvar_get(ndev, "assoc_resp_ies", wl->extra_buf,
-			WL_ASSOC_INFO_MAX);
-		if (unlikely(err)) {
-			WL_ERR(("could not get assoc resp (%d)\n", err));
-			return err;
-		}
-		conn_info->resp_ie_len = assoc_info.resp_len -sizeof(struct dot11_assoc_resp);
-		if (conn_info->resp_ie_len <= MAX_REQ_LINE)
-			memcpy(conn_info->resp_ie, wl->extra_buf, conn_info->resp_ie_len);
-		else {
-			WL_ERR(("%s IE size %d above max %d size \n",
-				__FUNCTION__, conn_info->resp_ie_len, MAX_REQ_LINE));
-			return err;
-		}
-	} else {
-		conn_info->resp_ie_len = 0;
-	}
-	WL_DBG(("req len (%d) resp len (%d)\n", conn_info->req_ie_len,
-		conn_info->resp_ie_len));
-
-	return err;
-}
-
-static void wl_ch_to_chanspec(int ch, struct wl_join_params *join_params,
-        size_t *join_params_size)
-{
-	chanspec_t chanspec = 0;
-
-	if (ch != 0) {
-		join_params->params.chanspec_num = 1;
-		join_params->params.chanspec_list[0] = ch;
-
-		if (join_params->params.chanspec_list[0])
-			chanspec |= WL_CHANSPEC_BAND_2G;
-		else
-			chanspec |= WL_CHANSPEC_BAND_5G;
-
-		chanspec |= WL_CHANSPEC_BW_20;
-		chanspec |= WL_CHANSPEC_CTL_SB_NONE;
-
-		*join_params_size += WL_ASSOC_PARAMS_FIXED_SIZE +
-			join_params->params.chanspec_num * sizeof(chanspec_t);
-
-		join_params->params.chanspec_list[0]  &= WL_CHANSPEC_CHAN_MASK;
-		join_params->params.chanspec_list[0] |= chanspec;
-		join_params->params.chanspec_list[0] =
-			htodchanspec(join_params->params.chanspec_list[0]);
-
-		join_params->params.chanspec_num =
-			htod32(join_params->params.chanspec_num);
-
-		WL_DBG(("%s  join_params->params.chanspec_list[0]= %X\n",
-			__FUNCTION__, join_params->params.chanspec_list[0]));
-
-	}
-}
-
-static s32 wl_update_bss_info(struct wl_priv *wl, struct net_device *ndev)
-{
-	struct cfg80211_bss *bss;
-	struct wl_bss_info *bi;
-	struct wlc_ssid *ssid;
-	struct bcm_tlv *tim;
-	u16 beacon_interval;
-	u8 dtim_period;
-	size_t ie_len;
-	u8 *ie;
-	s32 err = 0;
-	struct wiphy *wiphy;
-	wiphy = wl_to_wiphy(wl);
-
-	if (wl_is_ibssmode(wl, ndev))
-		return err;
-
-	ssid = (struct wlc_ssid *)wl_read_prof(wl, WL_PROF_SSID);
-	bss = cfg80211_get_bss(wiphy, NULL, (s8 *)&wl->bssid,
-		ssid->SSID, ssid->SSID_len, WLAN_CAPABILITY_ESS,
-		WLAN_CAPABILITY_ESS);
-
-	rtnl_lock();
-	if (unlikely(!bss)) {
-		WL_DBG(("Could not find the AP\n"));
-		*(u32 *) wl->extra_buf = htod32(WL_EXTRA_BUF_MAX);
-		err = wldev_ioctl(wl_to_prmry_ndev(wl), WLC_GET_BSS_INFO,
-			wl->extra_buf, WL_EXTRA_BUF_MAX, false);
-		if (unlikely(err)) {
-			WL_ERR(("Could not get bss info %d\n", err));
-			goto update_bss_info_out;
-		}
-		bi = (struct wl_bss_info *)(wl->extra_buf + 4);
-		if (unlikely(memcmp(&bi->BSSID, &wl->bssid, ETHER_ADDR_LEN))) {
-			err = -EIO;
-			goto update_bss_info_out;
-		}
-		err = wl_inform_single_bss(wl, bi);
-		if (unlikely(err))
-			goto update_bss_info_out;
-
-		ie = ((u8 *)bi) + bi->ie_offset;
-		ie_len = bi->ie_length;
-		beacon_interval = cpu_to_le16(bi->beacon_period);
-	} else {
-		WL_DBG(("Found the AP in the list - BSSID %pM\n", bss->bssid));
-		ie = bss->information_elements;
-		ie_len = bss->len_information_elements;
-		beacon_interval = bss->beacon_interval;
-		cfg80211_put_bss(bss);
-	}
-
-	tim = bcm_parse_tlvs(ie, ie_len, WLAN_EID_TIM);
-	if (tim) {
-		dtim_period = tim->data[1];
-	} else {
-		/*
-		* active scan was done so we could not get dtim
-		* information out of probe response.
-		* so we speficially query dtim information to dongle.
-		*/
-		err = wldev_ioctl(wl_to_prmry_ndev(wl), WLC_GET_DTIMPRD,
-			&dtim_period, sizeof(dtim_period), false);
-		if (unlikely(err)) {
-			WL_ERR(("WLC_GET_DTIMPRD error (%d)\n", err));
-			goto update_bss_info_out;
-		}
-	}
-
-	wl_update_prof(wl, NULL, &beacon_interval, WL_PROF_BEACONINT);
-	wl_update_prof(wl, NULL, &dtim_period, WL_PROF_DTIMPERIOD);
-
-update_bss_info_out:
-	rtnl_unlock();
-	return err;
-}
-
-static s32
-wl_bss_roaming_done(struct wl_priv *wl, struct net_device *ndev,
-	const wl_event_msg_t *e, void *data)
-{
-	struct wl_connect_info *conn_info = wl_to_conn(wl);
-	s32 err = 0;
-
-	wl_get_assoc_ies(wl, ndev);
-	memcpy(&wl->bssid, &e->addr, ETHER_ADDR_LEN);
-	wl_update_bss_info(wl, ndev);
-	wl_update_pmklist(ndev, wl->pmk_list, err);
-	cfg80211_roamed(ndev,
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 39)
-		NULL,
-#endif
-		(u8 *)&wl->bssid,
-		conn_info->req_ie, conn_info->req_ie_len,
-		conn_info->resp_ie, conn_info->resp_ie_len, GFP_KERNEL);
-	WL_DBG(("Report roaming result\n"));
-
-	wl_set_drv_status(wl, CONNECTED);
-
-	return err;
-}
-
-static s32
-wl_bss_connect_done(struct wl_priv *wl, struct net_device *ndev,
-	const wl_event_msg_t *e, void *data, bool completed)
-{
-	struct wl_connect_info *conn_info = wl_to_conn(wl);
-	s32 err = 0;
-
-	WL_DBG((" enter\n"));
-	if (wl->scan_request) {
-			wl_cfg80211_scan_abort(wl, ndev);
-	}
-	if (wl_get_drv_status(wl, CONNECTING)) {
-		wl_clr_drv_status(wl, CONNECTING);
-		if (completed) {
-			wl_get_assoc_ies(wl, ndev);
-			memcpy(&wl->bssid, &e->addr, ETHER_ADDR_LEN);
-			wl_update_bss_info(wl, ndev);
-			wl_update_pmklist(ndev, wl->pmk_list, err);
-			wl_set_drv_status(wl, CONNECTED);
-		}
-		cfg80211_connect_result(ndev,
-			(u8 *)&wl->bssid,
-			conn_info->req_ie,
-			conn_info->req_ie_len,
-			conn_info->resp_ie,
-			conn_info->resp_ie_len,
-			completed ? WLAN_STATUS_SUCCESS : WLAN_STATUS_AUTH_TIMEOUT,
-			GFP_KERNEL);
-		WL_DBG(("Report connect result - connection %s\n",
-			completed ? "succeeded" : "failed"));
-	}
-	return err;
-}
-
-static s32
-wl_notify_mic_status(struct wl_priv *wl, struct net_device *ndev,
-	const wl_event_msg_t *e, void *data)
-{
-	u16 flags = ntoh16(e->flags);
-	enum nl80211_key_type key_type;
-
-	rtnl_lock();
-	if (flags & WLC_EVENT_MSG_GROUP)
-		key_type = NL80211_KEYTYPE_GROUP;
-	else
-		key_type = NL80211_KEYTYPE_PAIRWISE;
-
-	cfg80211_michael_mic_failure(ndev, (u8 *)&e->addr, key_type, -1,
-		NULL, GFP_KERNEL);
-	rtnl_unlock();
-
-	return 0;
-}
-
-static s32
-wl_notify_scan_status(struct wl_priv *wl, struct net_device *ndev,
-	const wl_event_msg_t *e, void *data)
-{
-	struct channel_info channel_inform;
-	struct wl_scan_results *bss_list;
-	u32 len = WL_SCAN_BUF_MAX;
-	s32 err = 0;
-
-	WL_DBG(("Enter \n"));
-	if (wl->iscan_on && wl->iscan_kickstart)
-		return wl_wakeup_iscan(wl_to_iscan(wl));
-
-	if (unlikely(!wl_get_drv_status(wl, SCANNING))) {
-		wl_clr_drv_status(wl, SCANNING);
-		WL_DBG(("Scan complete while device not scanning\n"));
-		return -EINVAL;
-	}
-	wl_clr_drv_status(wl, SCANNING);
-	rtnl_lock();
-	err = wldev_ioctl(ndev, WLC_GET_CHANNEL, &channel_inform,
-		sizeof(channel_inform), false);
-	if (unlikely(err)) {
-		WL_ERR(("scan busy (%d)\n", err));
-		goto scan_done_out;
-	}
-	channel_inform.scan_channel = dtoh32(channel_inform.scan_channel);
-	if (unlikely(channel_inform.scan_channel)) {
-
-		WL_DBG(("channel_inform.scan_channel (%d)\n",
-			channel_inform.scan_channel));
-	}
-	wl->bss_list = wl->scan_results;
-	bss_list = wl->bss_list;
-	memset(bss_list, 0, len);
-	bss_list->buflen = htod32(len);
-	err = wldev_ioctl(ndev, WLC_SCAN_RESULTS, bss_list, len, false);
-	if (unlikely(err)) {
-		WL_ERR(("%s Scan_results error (%d)\n", ndev->name, err));
-		err = -EINVAL;
-		goto scan_done_out;
-	}
-	bss_list->buflen = dtoh32(bss_list->buflen);
-	bss_list->version = dtoh32(bss_list->version);
-	bss_list->count = dtoh32(bss_list->count);
-
-	err = wl_inform_bss(wl);
-	if (err)
-		goto scan_done_out;
-
-scan_done_out:
-	if (wl->scan_request) {
-		WL_DBG(("cfg80211_scan_done\n"));
-		cfg80211_scan_done(wl->scan_request, false);
-		wl->scan_request = NULL;
-	}
-	rtnl_unlock();
-	return err;
-}
-static s32
-wl_frame_get_mgmt(u16 fc, const struct ether_addr *da,
-	const struct ether_addr *sa, const struct ether_addr *bssid,
-	u8 **pheader, u32 *body_len, u8 *pbody)
-{
-	struct dot11_management_header *hdr;
-	u32 totlen = 0;
-	s32 err = 0;
-	u8 *offset;
-	u32 prebody_len = *body_len;
-	switch (fc) {
-		case FC_ASSOC_REQ:
-			/* capability , listen interval */
-			totlen = DOT11_ASSOC_REQ_FIXED_LEN;
-			*body_len += DOT11_ASSOC_REQ_FIXED_LEN;
-			break;
-
-		case FC_REASSOC_REQ:
-			/* capability, listen inteval, ap address */
-			totlen = DOT11_REASSOC_REQ_FIXED_LEN;
-			*body_len += DOT11_REASSOC_REQ_FIXED_LEN;
-			break;
-	}
-	totlen += DOT11_MGMT_HDR_LEN + prebody_len;
-	*pheader = kzalloc(totlen, GFP_KERNEL);
-	if (*pheader == NULL) {
-		WL_ERR(("memory alloc failed \n"));
-		return -ENOMEM;
-	}
-	hdr = (struct dot11_management_header *) (*pheader);
-	hdr->fc = htol16(fc);
-	hdr->durid = 0;
-	hdr->seq = 0;
-	offset = (u8*)(hdr + 1) + (totlen - DOT11_MGMT_HDR_LEN - prebody_len);
-	bcopy((const char*)da, (u8*)&hdr->da, ETHER_ADDR_LEN);
-	bcopy((const char*)sa, (u8*)&hdr->sa, ETHER_ADDR_LEN);
-	bcopy((const char*)bssid, (u8*)&hdr->bssid, ETHER_ADDR_LEN);
-	bcopy((const char*)pbody, offset, prebody_len);
-	*body_len = totlen;
-	return err;
-}
-static s32
-wl_notify_rx_mgmt_frame(struct wl_priv *wl, struct net_device *ndev,
-	const wl_event_msg_t *e, void *data)
-{
-	struct ieee80211_supported_band *band;
-	struct wiphy *wiphy = wl_to_wiphy(wl);
-	struct ether_addr da;
-	struct ether_addr bssid;
-	bool isfree = false;
-	s32 err = 0;
-	s32 freq;
-	wl_event_rx_frame_data_t *rxframe =
-		(wl_event_rx_frame_data_t*)data;
-	u32 event = ntoh32(e->event_type);
-	u8 *mgmt_frame;
-	u8 bsscfgidx = e->bsscfgidx;
-	u32 mgmt_frame_len = ntoh32(e->datalen) - sizeof(wl_event_rx_frame_data_t);
-	u16 channel = ((ntoh16(rxframe->channel) & WL_CHANSPEC_CHAN_MASK) & 0x0f);
-	if (channel <= CH_MAX_2G_CHANNEL)
-		band = wiphy->bands[IEEE80211_BAND_2GHZ];
-	else
-		band = wiphy->bands[IEEE80211_BAND_5GHZ];
-
-#if LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 38) && !defined(WL_COMPAT_WIRELESS)
-	freq = ieee80211_channel_to_frequency(channel);
-#else
-	freq = ieee80211_channel_to_frequency(channel, band->band);
-#endif
-	if (event == WLC_E_ACTION_FRAME_RX) {
-		wldev_iovar_getbuf_bsscfg(ndev, "cur_etheraddr",
-		&da, sizeof(struct ether_addr), ioctlbuf, sizeof(ioctlbuf), bsscfgidx);
-		wldev_ioctl(ndev, WLC_GET_BSSID, &bssid, ETHER_ADDR_LEN, false);
-		err = wl_frame_get_mgmt(FC_ACTION, &da, &e->addr, &bssid,
-			&mgmt_frame, &mgmt_frame_len,
-			(u8 *)((wl_event_rx_frame_data_t *)rxframe + 1));
-		if (err < 0) {
-			WL_ERR(("%s: Error in receiving action frame len %d channel %d freq %d\n",
-				__func__, mgmt_frame_len, channel, freq));
-			goto exit;
-		}
-		isfree = true;
-	} else {
-		mgmt_frame = (u8 *)((wl_event_rx_frame_data_t *)rxframe + 1);
-	}
-
-	cfg80211_rx_mgmt(ndev, freq, mgmt_frame, mgmt_frame_len, GFP_ATOMIC);
-
-	WL_DBG(("%s: mgmt_frame_len (%d) , e->datalen (%d), channel (%d), freq (%d)\n", __func__,
-		mgmt_frame_len, ntoh32(e->datalen), channel, freq));
-
-	if (isfree)
-		kfree(mgmt_frame);
-exit:
-	return 0;
-}
-
-static void wl_init_conf(struct wl_conf *conf)
-{
-	s32 i = 0;
-	WL_DBG(("Enter \n"));
-	for (i = 0; i <= VWDEV_CNT; i++) {
-		conf->mode[i].type = -1;
-		conf->mode[i].ndev = NULL;
-	}
-	conf->frag_threshold = (u32)-1;
-	conf->rts_threshold = (u32)-1;
-	conf->retry_short = (u32)-1;
-	conf->retry_long = (u32)-1;
-	conf->tx_power = -1;
-}
-
-static void wl_init_prof(struct wl_profile *prof)
-{
-	memset(prof, 0, sizeof(*prof));
-}
-
-static void wl_init_event_handler(struct wl_priv *wl)
-{
-	memset(wl->evt_handler, 0, sizeof(wl->evt_handler));
-
-	wl->evt_handler[WLC_E_SCAN_COMPLETE] = wl_notify_scan_status;
-	/* wl->evt_handler[WLC_E_JOIN] = wl_notify_connect_status; */
-	wl->evt_handler[WLC_E_LINK] = wl_notify_connect_status;
-	wl->evt_handler[WLC_E_DEAUTH_IND] = wl_notify_connect_status;
-	wl->evt_handler[WLC_E_DEAUTH] = wl_notify_connect_status;
-	wl->evt_handler[WLC_E_DISASSOC_IND] = wl_notify_connect_status;
-	wl->evt_handler[WLC_E_ASSOC_IND] = wl_notify_connect_status;
-	wl->evt_handler[WLC_E_REASSOC_IND] = wl_notify_connect_status;
-	wl->evt_handler[WLC_E_ROAM] = wl_notify_roaming_status;
-	wl->evt_handler[WLC_E_MIC_ERROR] = wl_notify_mic_status;
-	wl->evt_handler[WLC_E_SET_SSID] = wl_notify_connect_status;
-	wl->evt_handler[WLC_E_ACTION_FRAME_RX] = wl_notify_rx_mgmt_frame;
-	wl->evt_handler[WLC_E_PROBREQ_MSG] = wl_notify_rx_mgmt_frame;
-	wl->evt_handler[WLC_E_P2P_PROBREQ_MSG] = wl_notify_rx_mgmt_frame;
-	wl->evt_handler[WLC_E_P2P_DISC_LISTEN_COMPLETE] = wl_cfgp2p_listen_complete;
-	wl->evt_handler[WLC_E_ACTION_FRAME_COMPLETE] = wl_cfgp2p_action_tx_complete;
-	wl->evt_handler[WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE] = wl_cfgp2p_action_tx_complete;
-
-}
-
-static s32 wl_init_priv_mem(struct wl_priv *wl)
-{
-	WL_DBG(("Enter \n"));
-	wl->scan_results = (void *)kzalloc(WL_SCAN_BUF_MAX, GFP_KERNEL);
-	if (unlikely(!wl->scan_results)) {
-		WL_ERR(("Scan results alloc failed\n"));
-		goto init_priv_mem_out;
-	}
-	wl->conf = (void *)kzalloc(sizeof(*wl->conf), GFP_KERNEL);
-	if (unlikely(!wl->conf)) {
-		WL_ERR(("wl_conf alloc failed\n"));
-		goto init_priv_mem_out;
-	}
-	wl->profile = (void *)kzalloc(sizeof(*wl->profile), GFP_KERNEL);
-	if (unlikely(!wl->profile)) {
-		WL_ERR(("wl_profile alloc failed\n"));
-		goto init_priv_mem_out;
-	}
-	wl->bss_info = (void *)kzalloc(WL_BSS_INFO_MAX, GFP_KERNEL);
-	if (unlikely(!wl->bss_info)) {
-		WL_ERR(("Bss information alloc failed\n"));
-		goto init_priv_mem_out;
-	}
-	wl->scan_req_int =
-	    (void *)kzalloc(sizeof(*wl->scan_req_int), GFP_KERNEL);
-	if (unlikely(!wl->scan_req_int)) {
-		WL_ERR(("Scan req alloc failed\n"));
-		goto init_priv_mem_out;
-	}
-	wl->ioctl_buf = (void *)kzalloc(WL_IOCTL_LEN_MAX, GFP_KERNEL);
-	if (unlikely(!wl->ioctl_buf)) {
-		WL_ERR(("Ioctl buf alloc failed\n"));
-		goto init_priv_mem_out;
-	}
-	wl->escan_ioctl_buf = (void *)kzalloc(WLC_IOCTL_MAXLEN, GFP_KERNEL);
-	if (unlikely(!wl->escan_ioctl_buf)) {
-		WL_ERR(("Ioctl buf alloc failed\n"));
-		goto init_priv_mem_out;
-	}
-	wl->extra_buf = (void *)kzalloc(WL_EXTRA_BUF_MAX, GFP_KERNEL);
-	if (unlikely(!wl->extra_buf)) {
-		WL_ERR(("Extra buf alloc failed\n"));
-		goto init_priv_mem_out;
-	}
-	wl->iscan = (void *)kzalloc(sizeof(*wl->iscan), GFP_KERNEL);
-	if (unlikely(!wl->iscan)) {
-		WL_ERR(("Iscan buf alloc failed\n"));
-		goto init_priv_mem_out;
-	}
-	wl->fw = (void *)kzalloc(sizeof(*wl->fw), GFP_KERNEL);
-	if (unlikely(!wl->fw)) {
-		WL_ERR(("fw object alloc failed\n"));
-		goto init_priv_mem_out;
-	}
-	wl->pmk_list = (void *)kzalloc(sizeof(*wl->pmk_list), GFP_KERNEL);
-	if (unlikely(!wl->pmk_list)) {
-		WL_ERR(("pmk list alloc failed\n"));
-		goto init_priv_mem_out;
-	}
-
-	return 0;
-
-init_priv_mem_out:
-	wl_deinit_priv_mem(wl);
-
-	return -ENOMEM;
-}
-
-static void wl_deinit_priv_mem(struct wl_priv *wl)
-{
-	kfree(wl->scan_results);
-	wl->scan_results = NULL;
-	kfree(wl->bss_info);
-	wl->bss_info = NULL;
-	kfree(wl->conf);
-	wl->conf = NULL;
-	kfree(wl->profile);
-	wl->profile = NULL;
-	kfree(wl->scan_req_int);
-	wl->scan_req_int = NULL;
-	kfree(wl->ioctl_buf);
-	wl->ioctl_buf = NULL;
-	kfree(wl->escan_ioctl_buf);
-	wl->escan_ioctl_buf = NULL;
-	kfree(wl->extra_buf);
-	wl->extra_buf = NULL;
-	kfree(wl->iscan);
-	wl->iscan = NULL;
-	kfree(wl->fw);
-	wl->fw = NULL;
-	kfree(wl->pmk_list);
-	wl->pmk_list = NULL;
-	if (wl->ap_info) {
-		kfree(wl->ap_info->wpa_ie);
-		kfree(wl->ap_info->rsn_ie);
-		kfree(wl->ap_info->wps_ie);
-		kfree(wl->ap_info);
-		wl->ap_info = NULL;
-	}
-}
-
-static s32 wl_create_event_handler(struct wl_priv *wl)
-{
-	int ret = 0;
-	WL_DBG(("Enter \n"));
-
-	wl->event_tsk.thr_pid = DHD_PID_KT_INVALID;
-	PROC_START(wl_event_handler, wl, &wl->event_tsk, 0);
-	if (wl->event_tsk.thr_pid < 0)
-		ret = -ENOMEM;
-	return ret;
-}
-
-static void wl_destroy_event_handler(struct wl_priv *wl)
-{
-	if (wl->event_tsk.thr_pid >= 0)
-		PROC_STOP(&wl->event_tsk);
-}
-
-static void wl_term_iscan(struct wl_priv *wl)
-{
-	struct wl_iscan_ctrl *iscan = wl_to_iscan(wl);
-	WL_TRACE(("In\n"));
-	if (wl->iscan_on && iscan->tsk) {
-		iscan->state = WL_ISCAN_STATE_IDLE;
-		WL_INFO(("SIGTERM\n"));
-		send_sig(SIGTERM, iscan->tsk, 1);
-		WL_DBG(("kthread_stop\n"));
-		kthread_stop(iscan->tsk);
-		iscan->tsk = NULL;
-	}
-}
-
-static void wl_notify_iscan_complete(struct wl_iscan_ctrl *iscan, bool aborted)
-{
-	struct wl_priv *wl = iscan_to_wl(iscan);
-
-	WL_DBG(("Enter \n"));
-	if (unlikely(!wl_get_drv_status(wl, SCANNING))) {
-		wl_clr_drv_status(wl, SCANNING);
-		WL_ERR(("Scan complete while device not scanning\n"));
-		return;
-	}
-	wl_clr_drv_status(wl, SCANNING);
-	if (likely(wl->scan_request)) {
-		cfg80211_scan_done(wl->scan_request, aborted);
-		wl->scan_request = NULL;
-	}
-	wl->iscan_kickstart = false;
-}
-
-static s32 wl_wakeup_iscan(struct wl_iscan_ctrl *iscan)
-{
-	if (likely(iscan->state != WL_ISCAN_STATE_IDLE)) {
-		WL_DBG(("wake up iscan\n"));
-		up(&iscan->sync);
-		return 0;
-	}
-
-	return -EIO;
-}
-
-static s32
-wl_get_iscan_results(struct wl_iscan_ctrl *iscan, u32 *status,
-	struct wl_scan_results **bss_list)
-{
-	struct wl_iscan_results list;
-	struct wl_scan_results *results;
-	struct wl_iscan_results *list_buf;
-	s32 err = 0;
-
-	WL_DBG(("Enter \n"));
-	memset(iscan->scan_buf, 0, WL_ISCAN_BUF_MAX);
-	list_buf = (struct wl_iscan_results *)iscan->scan_buf;
-	results = &list_buf->results;
-	results->buflen = WL_ISCAN_RESULTS_FIXED_SIZE;
-	results->version = 0;
-	results->count = 0;
-
-	memset(&list, 0, sizeof(list));
-	list.results.buflen = htod32(WL_ISCAN_BUF_MAX);
-	err = wldev_iovar_getbuf(iscan->dev, "iscanresults", &list,
-		WL_ISCAN_RESULTS_FIXED_SIZE, iscan->scan_buf,
-		WL_ISCAN_BUF_MAX);
-	if (unlikely(err)) {
-		WL_ERR(("error (%d)\n", err));
-		return err;
-	}
-	results->buflen = dtoh32(results->buflen);
-	results->version = dtoh32(results->version);
-	results->count = dtoh32(results->count);
-	WL_DBG(("results->count = %d\n", results->count));
-	WL_DBG(("results->buflen = %d\n", results->buflen));
-	*status = dtoh32(list_buf->status);
-	*bss_list = results;
-
-	return err;
-}
-
-static s32 wl_iscan_done(struct wl_priv *wl)
-{
-	struct wl_iscan_ctrl *iscan = wl->iscan;
-	s32 err = 0;
-
-	iscan->state = WL_ISCAN_STATE_IDLE;
-	rtnl_lock();
-	wl_inform_bss(wl);
-	wl_notify_iscan_complete(iscan, false);
-	rtnl_unlock();
-
-	return err;
-}
-
-static s32 wl_iscan_pending(struct wl_priv *wl)
-{
-	struct wl_iscan_ctrl *iscan = wl->iscan;
-	s32 err = 0;
-
-	/* Reschedule the timer */
-	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
-	iscan->timer_on = 1;
-
-	return err;
-}
-
-static s32 wl_iscan_inprogress(struct wl_priv *wl)
-{
-	struct wl_iscan_ctrl *iscan = wl->iscan;
-	s32 err = 0;
-
-	rtnl_lock();
-	wl_inform_bss(wl);
-	wl_run_iscan(iscan, NULL, WL_SCAN_ACTION_CONTINUE);
-	rtnl_unlock();
-	/* Reschedule the timer */
-	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
-	iscan->timer_on = 1;
-
-	return err;
-}
-
-static s32 wl_iscan_aborted(struct wl_priv *wl)
-{
-	struct wl_iscan_ctrl *iscan = wl->iscan;
-	s32 err = 0;
-
-	iscan->state = WL_ISCAN_STATE_IDLE;
-	rtnl_lock();
-	wl_notify_iscan_complete(iscan, true);
-	rtnl_unlock();
-
-	return err;
-}
-
-static s32 wl_iscan_thread(void *data)
-{
-	struct sched_param param = {.sched_priority = MAX_RT_PRIO - 1 };
-	struct wl_iscan_ctrl *iscan = (struct wl_iscan_ctrl *)data;
-	struct wl_priv *wl = iscan_to_wl(iscan);
-	u32 status;
-	int err = 0;
-
-	sched_setscheduler(current, SCHED_FIFO, &param);
-	allow_signal(SIGTERM);
-	status = WL_SCAN_RESULTS_PARTIAL;
-	while (likely(!down_interruptible(&iscan->sync))) {
-		if (kthread_should_stop())
-			break;
-		if (iscan->timer_on) {
-			del_timer_sync(&iscan->timer);
-			iscan->timer_on = 0;
-		}
-		rtnl_lock();
-		err = wl_get_iscan_results(iscan, &status, &wl->bss_list);
-		if (unlikely(err)) {
-			status = WL_SCAN_RESULTS_ABORTED;
-			WL_ERR(("Abort iscan\n"));
-		}
-		rtnl_unlock();
-		iscan->iscan_handler[status] (wl);
-	}
-	if (iscan->timer_on) {
-		del_timer_sync(&iscan->timer);
-		iscan->timer_on = 0;
-	}
-	WL_DBG(("%s was terminated\n", __func__));
-
-	return 0;
-}
-
-static void wl_iscan_timer(unsigned long data)
-{
-	struct wl_iscan_ctrl *iscan = (struct wl_iscan_ctrl *)data;
-
-	if (iscan) {
-		iscan->timer_on = 0;
-		WL_DBG(("timer expired\n"));
-		wl_wakeup_iscan(iscan);
-	}
-}
-
-static s32 wl_invoke_iscan(struct wl_priv *wl)
-{
-	struct wl_iscan_ctrl *iscan = wl_to_iscan(wl);
-	int err = 0;
-
-	if (wl->iscan_on && !iscan->tsk) {
-		iscan->state = WL_ISCAN_STATE_IDLE;
-		sema_init(&iscan->sync, 0);
-		iscan->tsk = kthread_run(wl_iscan_thread, iscan, "wl_iscan");
-		if (IS_ERR(iscan->tsk)) {
-			WL_ERR(("Could not create iscan thread\n"));
-			iscan->tsk = NULL;
-			return -ENOMEM;
-		}
-	}
-
-	return err;
-}
-
-static void wl_init_iscan_handler(struct wl_iscan_ctrl *iscan)
-{
-	memset(iscan->iscan_handler, 0, sizeof(iscan->iscan_handler));
-	iscan->iscan_handler[WL_SCAN_RESULTS_SUCCESS] = wl_iscan_done;
-	iscan->iscan_handler[WL_SCAN_RESULTS_PARTIAL] = wl_iscan_inprogress;
-	iscan->iscan_handler[WL_SCAN_RESULTS_PENDING] = wl_iscan_pending;
-	iscan->iscan_handler[WL_SCAN_RESULTS_ABORTED] = wl_iscan_aborted;
-	iscan->iscan_handler[WL_SCAN_RESULTS_NO_MEM] = wl_iscan_aborted;
-}
-
-static void wl_notify_escan_complete(struct wl_priv *wl, bool aborted)
-{
-	WL_DBG(("Enter \n"));
-	if (unlikely(!wl_get_drv_status(wl, SCANNING))) {
-		wl_clr_drv_status(wl, SCANNING);
-		WL_ERR(("Scan complete while device not scanning\n"));
-		wl->scan_request = NULL;
-		return;
-	}
-	wl_clr_drv_status(wl, SCANNING);
-	if (wl->p2p_supported && p2p_on(wl))
-		wl_clr_p2p_status(wl, SCANNING);
-
-	if (likely(wl->scan_request)) {
-		cfg80211_scan_done(wl->scan_request, aborted);
-		wl->scan_request = NULL;
-	}
-}
-
-static s32 wl_escan_handler(struct wl_priv *wl,
-	struct net_device *ndev,
-	const wl_event_msg_t *e, void *data)
-{
-	s32 err = BCME_OK;
-	s32 status = ntoh32(e->status);
-	wl_bss_info_t *bi;
-	wl_escan_result_t *escan_result;
-	wl_bss_info_t *bss = NULL;
-	wl_scan_results_t *list;
-	u32 bi_length;
-	u32 i;
-	WL_DBG((" enter event type : %d, status : %d \n",
-		ntoh32(e->event_type), ntoh32(e->status)));
-	if (!wl->escan_on &&
-		!wl_get_drv_status(wl, SCANNING)) {
-		WL_ERR(("escan is not ready \n"));
-		return err;
-	}
-
-	if (status == WLC_E_STATUS_PARTIAL) {
-		WL_INFO(("WLC_E_STATUS_PARTIAL \n"));
-		escan_result = (wl_escan_result_t *) data;
-		if (!escan_result) {
-			WL_ERR(("Invalid escan result (NULL pointer)\n"));
-			goto exit;
-		}
-		if (dtoh16(escan_result->bss_count) != 1) {
-			WL_ERR(("Invalid bss_count %d: ignoring\n", escan_result->bss_count));
-			goto exit;
-		}
-		bi = escan_result->bss_info;
-		if (!bi) {
-			WL_ERR(("Invalid escan bss info (NULL pointer)\n"));
-			goto exit;
-		}
-		bi_length = dtoh32(bi->length);
-		if (bi_length != (dtoh32(escan_result->buflen) - WL_ESCAN_RESULTS_FIXED_SIZE)) {
-			WL_ERR(("Invalid bss_info length %d: ignoring\n", bi_length));
-			goto exit;
-		}
-		list = (wl_scan_results_t *)wl->escan_info.escan_buf;
-		if (bi_length > ESCAN_BUF_SIZE - list->buflen) {
-			WL_ERR(("Buffer is too small: ignoring\n"));
-			goto exit;
-		}
-#define WLC_BSS_RSSI_ON_CHANNEL 0x0002
-		for (i = 0; i < list->count; i++) {
-			bss = bss ? (wl_bss_info_t *)((uintptr)bss + dtoh32(bss->length))
-				: list->bss_info;
-
-			if (!bcmp(&bi->BSSID, &bss->BSSID, ETHER_ADDR_LEN) &&
-				CHSPEC_BAND(bi->chanspec) == CHSPEC_BAND(bss->chanspec) &&
-				bi->SSID_len == bss->SSID_len &&
-				!bcmp(bi->SSID, bss->SSID, bi->SSID_len)) {
-				if ((bss->flags & WLC_BSS_RSSI_ON_CHANNEL) ==
-					(bi->flags & WLC_BSS_RSSI_ON_CHANNEL)) {
-					/* preserve max RSSI if the measurements are
-					 * both on-channel or both off-channel
-					 */
-					bss->RSSI = MAX(bss->RSSI, bi->RSSI);
-				} else if ((bss->flags & WLC_BSS_RSSI_ON_CHANNEL) &&
-					(bi->flags & WLC_BSS_RSSI_ON_CHANNEL) == 0) {
-					/* preserve the on-channel rssi measurement
-					 * if the new measurement is off channel
-					 */
-					bss->RSSI = bi->RSSI;
-					bss->flags |= WLC_BSS_RSSI_ON_CHANNEL;
-				}
-
-				goto exit;
-			}
-		}
-		memcpy(&(wl->escan_info.escan_buf[list->buflen]), bi, bi_length);
-		list->version = dtoh32(bi->version);
-		list->buflen += bi_length;
-		list->count++;
-
-	}
-	else if (status == WLC_E_STATUS_SUCCESS) {
-		wl->escan_info.escan_state = WL_ESCAN_STATE_IDLE;
-		if (likely(wl->scan_request)) {
-			rtnl_lock();
-			WL_INFO(("ESCAN COMPLETED\n"));
-			wl->bss_list = (wl_scan_results_t *)wl->escan_info.escan_buf;
-			wl_inform_bss(wl);
-			wl_notify_escan_complete(wl, false);
-			rtnl_unlock();
-		}
-	}
-	else if (status == WLC_E_STATUS_ABORT) {
-		wl->escan_info.escan_state = WL_ESCAN_STATE_IDLE;
-		if (likely(wl->scan_request)) {
-			rtnl_lock();
-			WL_INFO(("ESCAN ABORTED\n"));
-			wl->bss_list = (wl_scan_results_t *)wl->escan_info.escan_buf;
-			wl_inform_bss(wl);
-			wl_notify_escan_complete(wl, true);
-			rtnl_unlock();
-		}
-	}
-exit:
-	return err;
-}
-
-static s32 wl_init_scan(struct wl_priv *wl)
-{
-	struct wl_iscan_ctrl *iscan = wl_to_iscan(wl);
-	int err = 0;
-
-	if (wl->iscan_on) {
-		iscan->dev = wl_to_prmry_ndev(wl);
-		iscan->state = WL_ISCAN_STATE_IDLE;
-		wl_init_iscan_handler(iscan);
-		iscan->timer_ms = WL_ISCAN_TIMER_INTERVAL_MS;
-		init_timer(&iscan->timer);
-		iscan->timer.data = (unsigned long) iscan;
-		iscan->timer.function = wl_iscan_timer;
-		sema_init(&iscan->sync, 0);
-		iscan->tsk = kthread_run(wl_iscan_thread, iscan, "wl_iscan");
-		if (IS_ERR(iscan->tsk)) {
-			WL_ERR(("Could not create iscan thread\n"));
-			iscan->tsk = NULL;
-			return -ENOMEM;
-		}
-		iscan->data = wl;
-	} else if (wl->escan_on) {
-		wl->evt_handler[WLC_E_ESCAN_RESULT] = wl_escan_handler;
-		wl->escan_info.escan_state = WL_ESCAN_STATE_IDLE;
-	}
-
-	return err;
-}
-
-static void wl_init_fw(struct wl_fw_ctrl *fw)
-{
-	fw->status = 0;
-}
-
-static s32 wl_init_priv(struct wl_priv *wl)
-{
-	struct wiphy *wiphy = wl_to_wiphy(wl);
-	s32 err = 0;
-	s32 i = 0;
-
-	wl->scan_request = NULL;
-	wl->pwr_save = !!(wiphy->flags & WIPHY_FLAG_PS_ON_BY_DEFAULT);
-	wl->iscan_on = false;
-	wl->escan_on = true;
-	wl->roam_on = false;
-	wl->iscan_kickstart = false;
-	wl->active_scan = true;
-	wl->dongle_up = false;
-	wl->rf_blocked = false;
-
-	for (i = 0; i < VWDEV_CNT; i++)
-		wl->vwdev[i] = NULL;
-
-	init_waitqueue_head(&wl->dongle_event_wait);
-	wl_init_eq(wl);
-	err = wl_init_priv_mem(wl);
-	if (unlikely(err))
-		return err;
-	if (unlikely(wl_create_event_handler(wl)))
-		return -ENOMEM;
-	wl_init_event_handler(wl);
-	mutex_init(&wl->usr_sync);
-	err = wl_init_scan(wl);
-	if (unlikely(err))
-		return err;
-	wl_init_fw(wl->fw);
-	wl_init_conf(wl->conf);
-	wl_init_prof(wl->profile);
-	wl_link_down(wl);
-
-	return err;
-}
-
-static void wl_deinit_priv(struct wl_priv *wl)
-{
-	wl_destroy_event_handler(wl);
-	wl->dongle_up = false;	/* dongle down */
-	wl_flush_eq(wl);
-	wl_link_down(wl);
-	wl_term_iscan(wl);
-	wl_deinit_priv_mem(wl);
-}
-
-#if defined(DHD_P2P_DEV_ADDR_FROM_SYSFS) && defined(CONFIG_SYSCTL)
-s32 wl_cfg80211_sysctl_export_devaddr(void *data)
-{
-	/* Export the p2p_dev_addr via sysctl interface
-	 * so that wpa_supplicant can access it
-	 */
-	dhd_pub_t *dhd = (dhd_pub_t *)data;
-	struct wl_priv *wl = wlcfg_drv_priv;
-
-	wl_cfgp2p_generate_bss_mac(&dhd->mac, &wl->p2p->dev_addr, &wl->p2p->int_addr);
-
-	sprintf((char *)&wl_sysctl_macstring[0], MACSTR, MAC2STR(wl->p2p->dev_addr.octet));
-	sprintf((char *)&wl_sysctl_macstring[1], MACSTR, MAC2STR(wl->p2p->int_addr.octet));
-
-	return 0;
-}
-#endif /* CONFIG_SYSCTL */
-
-s32 wl_cfg80211_attach_post(struct net_device *ndev)
-{
-	struct wl_priv * wl = NULL;
-	s32 err = 0;
-	WL_TRACE(("In\n"));
-	if (unlikely(!ndev)) {
-		WL_ERR(("ndev is invaild\n"));
-		return -ENODEV;
-	}
-	wl = wlcfg_drv_priv;
-	if (wl && !wl_get_drv_status(wl, READY)) {
-			if (wl->wdev &&
-				wl_cfgp2p_supported(wl, ndev)) {
-				wl->wdev->wiphy->interface_modes |=
-					(BIT(NL80211_IFTYPE_P2P_CLIENT)|
-					BIT(NL80211_IFTYPE_P2P_GO));
-				if ((err = wl_cfgp2p_init_priv(wl)) != 0)
-					goto fail;
-#if defined(DHD_P2P_DEV_ADDR_FROM_SYSFS) && defined(CONFIG_SYSCTL)
-				wl_cfg80211_sysctl_export_devaddr(wl->pub);
-#endif
-				wl->p2p_supported = true;
-			}
-	} else
-		return -ENODEV;
-
-	wl_set_drv_status(wl, READY);
-fail:
-	return err;
-}
-s32 wl_cfg80211_attach(struct net_device *ndev, void *data)
-{
-	struct wireless_dev *wdev;
-	struct wl_priv *wl;
-	s32 err = 0;
-
-	WL_TRACE(("In\n"));
-	if (unlikely(!ndev)) {
-		WL_ERR(("ndev is invaild\n"));
-		return -ENODEV;
-	}
-	WL_DBG(("func %p\n", wl_cfg80211_get_sdio_func()));
-	wdev = wl_alloc_wdev(&wl_cfg80211_get_sdio_func()->dev);
-	if (unlikely(IS_ERR(wdev)))
-		return -ENOMEM;
-
-	wdev->iftype = wl_mode_to_nl80211_iftype(WL_MODE_BSS);
-	wl = (struct wl_priv *)wiphy_priv(wdev->wiphy);
-	wl->wdev = wdev;
-	wl->pub = data;
-
-	ndev->ieee80211_ptr = wdev;
-	SET_NETDEV_DEV(ndev, wiphy_dev(wdev->wiphy));
-	wdev->netdev = ndev;
-
-	err = wl_init_priv(wl);
-	if (unlikely(err)) {
-		WL_ERR(("Failed to init iwm_priv (%d)\n", err));
-		goto cfg80211_attach_out;
-	}
-
-	err = wl_setup_rfkill(wl, TRUE);
-	if (unlikely(err)) {
-		WL_ERR(("Failed to setup rfkill %d\n", err));
-		goto cfg80211_attach_out;
-	}
-
-#if defined(DHD_P2P_DEV_ADDR_FROM_SYSFS) && defined(CONFIG_SYSCTL)
-	if (!(wl_sysctl_hdr = register_sysctl_table(wl_sysctl_table))) {
-		WL_ERR(("%s: sysctl register failed!! \n", __func__));
-		goto cfg80211_attach_out;
-	}
-#endif
-	wlcfg_drv_priv = wl;
-	return err;
-
-cfg80211_attach_out:
-	err = wl_setup_rfkill(wl, FALSE);
-	wl_free_wdev(wl);
-	return err;
-}
-
-void wl_cfg80211_detach(void)
-{
-	struct wl_priv *wl;
-
-	wl = wlcfg_drv_priv;
-
-	WL_TRACE(("In\n"));
-#if defined(DHD_P2P_DEV_ADDR_FROM_SYSFS) && defined(CONFIG_SYSCTL)
-	if (wl_sysctl_hdr)
-		unregister_sysctl_table(wl_sysctl_hdr);
-#endif
-	wl_setup_rfkill(wl, FALSE);
-	if (wl->p2p_supported)
-		wl_cfgp2p_deinit_priv(wl);
-	wl_deinit_priv(wl);
-	wlcfg_drv_priv = NULL;
-	wl_clear_sdio_func();
-	wl_free_wdev(wl);
-}
-
-static void wl_wakeup_event(struct wl_priv *wl)
-{
-	if (wl->event_tsk.thr_pid >= 0)
-		up(&wl->event_tsk.sema);
-}
-
-static s32 wl_event_handler(void *data)
-{
-	struct net_device *netdev;
-	struct wl_priv *wl = NULL;
-	struct wl_event_q *e;
-	tsk_ctl_t *tsk = (tsk_ctl_t *)data;
-
-	wl = (struct wl_priv *)tsk->parent;
-	complete(&tsk->completed);
-
-	while (down_interruptible (&tsk->sema) == 0) {
-		SMP_RD_BARRIER_DEPENDS();
-		if (tsk->terminated)
-			break;
-		e = wl_deq_event(wl);
-		if (unlikely(!e)) {
-			WL_ERR(("equeue empty..\n"));
-			return 0;
-		}
-		WL_DBG(("event type (%d), if idx: %d\n", e->etype, e->emsg.ifidx));
-		netdev = dhd_idx2net((struct dhd_pub *)(wl->pub), e->emsg.ifidx);
-		if (!netdev)
-			netdev = wl_to_prmry_ndev(wl);
-		if (e->etype < WLC_E_LAST && wl->evt_handler[e->etype]) {
-			wl->evt_handler[e->etype] (wl, netdev, &e->emsg, e->edata);
-		} else {
-			WL_DBG(("Unknown Event (%d): ignoring\n", e->etype));
-		}
-		wl_put_event(e);
-	}
-	WL_DBG(("%s was terminated\n", __func__));
-	complete_and_exit(&tsk->completed, 0);
-	return 0;
-}
-
-void
-wl_cfg80211_event(struct net_device *ndev, const wl_event_msg_t * e, void *data)
-{
-	u32 event_type = ntoh32(e->event_type);
-	struct wl_priv *wl = wlcfg_drv_priv;
-
-#if (WL_DBG_LEVEL > 0)
-	s8 *estr = (event_type <= sizeof(wl_dbg_estr) / WL_DBG_ESTR_MAX - 1) ?
-	    wl_dbg_estr[event_type] : (s8 *) "Unknown";
-	WL_DBG(("event_type (%d):" "WLC_E_" "%s\n", event_type, estr));
-#endif /* (WL_DBG_LEVEL > 0) */
-
-	if (likely(!wl_enq_event(wl, ndev, event_type, e, data)))
-		wl_wakeup_event(wl);
-}
-
-static void wl_init_eq(struct wl_priv *wl)
-{
-	wl_init_eq_lock(wl);
-	INIT_LIST_HEAD(&wl->eq_list);
-}
-
-static void wl_flush_eq(struct wl_priv *wl)
-{
-	struct wl_event_q *e;
-
-	wl_lock_eq(wl);
-	while (!list_empty(&wl->eq_list)) {
-		e = list_first_entry(&wl->eq_list, struct wl_event_q, eq_list);
-		list_del(&e->eq_list);
-		kfree(e);
-	}
-	wl_unlock_eq(wl);
-}
-
-/*
-* retrieve first queued event from head
-*/
-
-static struct wl_event_q *wl_deq_event(struct wl_priv *wl)
-{
-	struct wl_event_q *e = NULL;
-
-	wl_lock_eq(wl);
-	if (likely(!list_empty(&wl->eq_list))) {
-		e = list_first_entry(&wl->eq_list, struct wl_event_q, eq_list);
-		list_del(&e->eq_list);
-	}
-	wl_unlock_eq(wl);
-
-	return e;
-}
-
-/*
- * push event to tail of the queue
- */
-
-static s32
-wl_enq_event(struct wl_priv *wl, struct net_device *ndev, u32 event, const wl_event_msg_t *msg,
-	void *data)
-{
-	struct wl_event_q *e;
-	s32 err = 0;
-	uint32 evtq_size;
-	uint32 data_len;
-
-	data_len = 0;
-	if (data)
-		data_len = ntoh32(msg->datalen);
-	evtq_size = sizeof(struct wl_event_q) + data_len;
-	e = kzalloc(evtq_size, GFP_ATOMIC);
-	if (unlikely(!e)) {
-		WL_ERR(("event alloc failed\n"));
-		return -ENOMEM;
-	}
-	e->etype = event;
-	memcpy(&e->emsg, msg, sizeof(wl_event_msg_t));
-	if (data)
-		memcpy(e->edata, data, data_len);
-	wl_lock_eq(wl);
-	list_add_tail(&e->eq_list, &wl->eq_list);
-	wl_unlock_eq(wl);
-
-	return err;
-}
-
-static void wl_put_event(struct wl_event_q *e)
-{
-	kfree(e);
-}
-
-void wl_cfg80211_set_sdio_func(void *func)
-{
-	cfg80211_sdio_func = (struct sdio_func *)func;
-}
-
-static void wl_clear_sdio_func(void)
-{
-	cfg80211_sdio_func = NULL;
-}
-
-struct sdio_func *wl_cfg80211_get_sdio_func(void)
-{
-	return cfg80211_sdio_func;
-}
-
-static s32 wl_dongle_mode(struct wl_priv *wl, struct net_device *ndev, s32 iftype)
-{
-	s32 infra = 0;
-	s32 err = 0;
-	s32 mode = 0;
-	switch (iftype) {
-	case NL80211_IFTYPE_MONITOR:
-	case NL80211_IFTYPE_WDS:
-		WL_ERR(("type (%d) : currently we do not support this mode\n",
-			iftype));
-		err = -EINVAL;
-		return err;
-	case NL80211_IFTYPE_ADHOC:
-		mode = WL_MODE_IBSS;
-		break;
-	case NL80211_IFTYPE_STATION:
-	case NL80211_IFTYPE_P2P_CLIENT:
-		mode = WL_MODE_BSS;
-		infra = 1;
-		break;
-	case NL80211_IFTYPE_AP:
-	case NL80211_IFTYPE_P2P_GO:
-		mode = WL_MODE_AP;
-		infra = 1;
-		break;
-	default:
-		err = -EINVAL;
-		WL_ERR(("invalid type (%d)\n", iftype));
-		return err;
-	}
-	infra = htod32(infra);
-	err = wldev_ioctl(ndev, WLC_SET_INFRA, &infra, sizeof(infra), false);
-	if (unlikely(err)) {
-		WL_ERR(("WLC_SET_INFRA error (%d)\n", err));
-		return err;
-	}
-
-	set_mode_by_netdev(wl, ndev, mode);
-
-	return 0;
-}
-static s32 wl_dongle_add_remove_eventmsg(struct net_device *ndev, u16 event, bool add)
-{
-	s8 iovbuf[WL_EVENTING_MASK_LEN + 12];
-
-	s8 eventmask[WL_EVENTING_MASK_LEN];
-	s32 err = 0;
-
-	/* Setup event_msgs */
-	bcm_mkiovar("event_msgs", eventmask, WL_EVENTING_MASK_LEN, iovbuf,
-		sizeof(iovbuf));
-	err = wldev_ioctl(ndev, WLC_GET_VAR, iovbuf, sizeof(iovbuf), false);
-	if (unlikely(err)) {
-		WL_ERR(("Get event_msgs error (%d)\n", err));
-		goto dongle_eventmsg_out;
-	}
-	memcpy(eventmask, iovbuf, WL_EVENTING_MASK_LEN);
-	if (add) {
-		setbit(eventmask, event);
-	} else {
-		clrbit(eventmask, event);
-	}
-	bcm_mkiovar("event_msgs", eventmask, WL_EVENTING_MASK_LEN, iovbuf,
-		sizeof(iovbuf));
-	err = wldev_ioctl(ndev, WLC_SET_VAR, iovbuf, sizeof(iovbuf), false);
-	if (unlikely(err)) {
-		WL_ERR(("Set event_msgs error (%d)\n", err));
-		goto dongle_eventmsg_out;
-	}
-
-dongle_eventmsg_out:
-	return err;
-
-}
-
-static s32 wl_dongle_eventmsg(struct net_device *ndev)
-{
-	s8 iovbuf[WL_EVENTING_MASK_LEN + 12];
-
-	s8 eventmask[WL_EVENTING_MASK_LEN];
-	s32 err = 0;
-
-	/* Setup event_msgs */
-	bcm_mkiovar("event_msgs", eventmask, WL_EVENTING_MASK_LEN, iovbuf,
-		sizeof(iovbuf));
-	err = wldev_ioctl(ndev, WLC_GET_VAR, iovbuf, sizeof(iovbuf), false);
-	if (unlikely(err)) {
-		WL_ERR(("Get event_msgs error (%d)\n", err));
-		goto dongle_eventmsg_out;
-	}
-	memcpy(eventmask, iovbuf, WL_EVENTING_MASK_LEN);
-
-	setbit(eventmask, WLC_E_SET_SSID);
-	setbit(eventmask, WLC_E_PRUNE);
-	setbit(eventmask, WLC_E_AUTH);
-	setbit(eventmask, WLC_E_REASSOC);
-	setbit(eventmask, WLC_E_REASSOC_IND);
-	setbit(eventmask, WLC_E_DEAUTH_IND);
-	setbit(eventmask, WLC_E_DEAUTH);
-	setbit(eventmask, WLC_E_DISASSOC_IND);
-	setbit(eventmask, WLC_E_DISASSOC);
-	setbit(eventmask, WLC_E_JOIN);
-	setbit(eventmask, WLC_E_ROAM);
-	setbit(eventmask, WLC_E_ASSOC_IND);
-	setbit(eventmask, WLC_E_LINK);
-	setbit(eventmask, WLC_E_MIC_ERROR);
-	setbit(eventmask, WLC_E_PMKID_CACHE);
-	setbit(eventmask, WLC_E_TXFAIL);
-	setbit(eventmask, WLC_E_SCAN_COMPLETE);
-	setbit(eventmask, WLC_E_ACTION_FRAME_RX);
-	setbit(eventmask, WLC_E_ACTION_FRAME_COMPLETE);
-	setbit(eventmask, WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE);
-	setbit(eventmask, WLC_E_P2P_PROBREQ_MSG);
-	setbit(eventmask, WLC_E_P2P_DISC_LISTEN_COMPLETE);
-	setbit(eventmask, WLC_E_ESCAN_RESULT);
-	bcm_mkiovar("event_msgs", eventmask, WL_EVENTING_MASK_LEN, iovbuf,
-		sizeof(iovbuf));
-	err = wldev_ioctl(ndev, WLC_SET_VAR, iovbuf, sizeof(iovbuf), false);
-	if (unlikely(err)) {
-		WL_ERR(("Set event_msgs error (%d)\n", err));
-		goto dongle_eventmsg_out;
-	}
-
-dongle_eventmsg_out:
-	return err;
-}
-
-#ifndef EMBEDDED_PLATFORM
-static s32 wl_dongle_country(struct net_device *ndev, u8 ccode)
-{
-
-	s32 err = 0;
-
-	return err;
-}
-
-static s32 wl_dongle_up(struct net_device *ndev, u32 up)
-{
-	s32 err = 0;
-
-	err = wldev_ioctl(ndev, WLC_UP, &up, sizeof(up), false);
-	if (unlikely(err)) {
-		WL_ERR(("WLC_UP error (%d)\n", err));
-	}
-	return err;
-}
-
-static s32 wl_dongle_power(struct net_device *ndev, u32 power_mode)
-{
-	s32 err = 0;
-
-	WL_TRACE(("In\n"));
-	err = wldev_ioctl(ndev, WLC_SET_PM, &power_mode, sizeof(power_mode), false);
-	if (unlikely(err)) {
-		WL_ERR(("WLC_SET_PM error (%d)\n", err));
-	}
-	return err;
-}
-
-static s32
-wl_dongle_glom(struct net_device *ndev, u32 glom, u32 dongle_align)
-{
-	s8 iovbuf[WL_EVENTING_MASK_LEN + 12];
-
-	s32 err = 0;
-
-	/* Match Host and Dongle rx alignment */
-	bcm_mkiovar("bus:txglomalign", (char *)&dongle_align, 4, iovbuf,
-		sizeof(iovbuf));
-	err = wldev_ioctl(ndev, WLC_SET_VAR, iovbuf, sizeof(iovbuf), false);
-	if (unlikely(err)) {
-		WL_ERR(("txglomalign error (%d)\n", err));
-		goto dongle_glom_out;
-	}
-	/* disable glom option per default */
-	bcm_mkiovar("bus:txglom", (char *)&glom, 4, iovbuf, sizeof(iovbuf));
-	err = wldev_ioctl(ndev, WLC_SET_VAR, iovbuf, sizeof(iovbuf), false);
-	if (unlikely(err)) {
-		WL_ERR(("txglom error (%d)\n", err));
-		goto dongle_glom_out;
-	}
-dongle_glom_out:
-	return err;
-}
-
-static s32
-wl_dongle_roam(struct net_device *ndev, u32 roamvar, u32 bcn_timeout)
-{
-	s8 iovbuf[WL_EVENTING_MASK_LEN + 12];
-
-	s32 err = 0;
-
-	/* Setup timeout if Beacons are lost and roam is off to report link down */
-	if (roamvar) {
-		bcm_mkiovar("bcn_timeout", (char *)&bcn_timeout, 4, iovbuf,
-			sizeof(iovbuf));
-		err = wldev_ioctl(ndev, WLC_SET_VAR, iovbuf, sizeof(iovbuf), false);
-		if (unlikely(err)) {
-			WL_ERR(("bcn_timeout error (%d)\n", err));
-			goto dongle_rom_out;
-		}
-	}
-	/* Enable/Disable built-in roaming to allow supplicant to take care of roaming */
-	bcm_mkiovar("roam_off", (char *)&roamvar, 4, iovbuf, sizeof(iovbuf));
-	err = wldev_ioctl(ndev, WLC_SET_VAR, iovbuf, sizeof(iovbuf), false);
-	if (unlikely(err)) {
-		WL_ERR(("roam_off error (%d)\n", err));
-		goto dongle_rom_out;
-	}
-dongle_rom_out:
-	return err;
-}
-
-static s32
-wl_dongle_scantime(struct net_device *ndev, s32 scan_assoc_time,
-	s32 scan_unassoc_time)
-{
-	s32 err = 0;
-
-	err = wldev_ioctl(ndev, WLC_SET_SCAN_CHANNEL_TIME, &scan_assoc_time,
-		sizeof(scan_assoc_time), false);
-	if (err) {
-		if (err == -EOPNOTSUPP) {
-			WL_INFO(("Scan assoc time is not supported\n"));
-		} else {
-			WL_ERR(("Scan assoc time error (%d)\n", err));
-		}
-		goto dongle_scantime_out;
-	}
-	err = wldev_ioctl(ndev, WLC_SET_SCAN_UNASSOC_TIME, &scan_unassoc_time,
-		sizeof(scan_unassoc_time), false);
-	if (err) {
-		if (err == -EOPNOTSUPP) {
-			WL_INFO(("Scan unassoc time is not supported\n"));
-		} else {
-			WL_ERR(("Scan unassoc time error (%d)\n", err));
-		}
-		goto dongle_scantime_out;
-	}
-
-dongle_scantime_out:
-	return err;
-}
-
-static s32
-wl_dongle_offload(struct net_device *ndev, s32 arpoe, s32 arp_ol)
-{
-	/* Room for "event_msgs" + '\0' + bitvec */
-	s8 iovbuf[WL_EVENTING_MASK_LEN + 12];
-
-	s32 err = 0;
-
-	/* Set ARP offload */
-	bcm_mkiovar("arpoe", (char *)&arpoe, 4, iovbuf, sizeof(iovbuf));
-	err = wldev_ioctl(ndev, WLC_SET_VAR, iovbuf, sizeof(iovbuf), false);
-	if (err) {
-		if (err == -EOPNOTSUPP)
-			WL_INFO(("arpoe is not supported\n"));
-		else
-			WL_ERR(("arpoe error (%d)\n", err));
-
-		goto dongle_offload_out;
-	}
-	bcm_mkiovar("arp_ol", (char *)&arp_ol, 4, iovbuf, sizeof(iovbuf));
-	err = wldev_ioctl(ndev, WLC_SET_VAR, iovbuf, sizeof(iovbuf), false);
-	if (err) {
-		if (err == -EOPNOTSUPP)
-			WL_INFO(("arp_ol is not supported\n"));
-		else
-			WL_ERR(("arp_ol error (%d)\n", err));
-
-		goto dongle_offload_out;
-	}
-
-dongle_offload_out:
-	return err;
-}
-
-static s32 wl_pattern_atoh(s8 *src, s8 *dst)
-{
-	int i;
-	if (strncmp(src, "0x", 2) != 0 && strncmp(src, "0X", 2) != 0) {
-		WL_ERR(("Mask invalid format. Needs to start with 0x\n"));
-		return -1;
-	}
-	src = src + 2;		/* Skip past 0x */
-	if (strlen(src) % 2 != 0) {
-		WL_ERR(("Mask invalid format. Needs to be of even length\n"));
-		return -1;
-	}
-	for (i = 0; *src != '\0'; i++) {
-		char num[3];
-		strncpy(num, src, 2);
-		num[2] = '\0';
-		dst[i] = (u8) simple_strtoul(num, NULL, 16);
-		src += 2;
-	}
-	return i;
-}
-
-static s32 wl_dongle_filter(struct net_device *ndev, u32 filter_mode)
-{
-	/* Room for "event_msgs" + '\0' + bitvec */
-	s8 iovbuf[WL_EVENTING_MASK_LEN + 12];
-
-	const s8 *str;
-	struct wl_pkt_filter pkt_filter;
-	struct wl_pkt_filter *pkt_filterp;
-	s32 buf_len;
-	s32 str_len;
-	u32 mask_size;
-	u32 pattern_size;
-	s8 buf[256];
-	s32 err = 0;
-
-	/* add a default packet filter pattern */
-	str = "pkt_filter_add";
-	str_len = strlen(str);
-	strncpy(buf, str, str_len);
-	buf[str_len] = '\0';
-	buf_len = str_len + 1;
-
-	pkt_filterp = (struct wl_pkt_filter *)(buf + str_len + 1);
-
-	/* Parse packet filter id. */
-	pkt_filter.id = htod32(100);
-
-	/* Parse filter polarity. */
-	pkt_filter.negate_match = htod32(0);
-
-	/* Parse filter type. */
-	pkt_filter.type = htod32(0);
-
-	/* Parse pattern filter offset. */
-	pkt_filter.u.pattern.offset = htod32(0);
-
-	/* Parse pattern filter mask. */
-	mask_size = htod32(wl_pattern_atoh("0xff",
-		(char *)pkt_filterp->u.pattern.
-		    mask_and_pattern));
-
-	/* Parse pattern filter pattern. */
-	pattern_size = htod32(wl_pattern_atoh("0x00",
-		(char *)&pkt_filterp->u.pattern.mask_and_pattern[mask_size]));
-
-	if (mask_size != pattern_size) {
-		WL_ERR(("Mask and pattern not the same size\n"));
-		err = -EINVAL;
-		goto dongle_filter_out;
-	}
-
-	pkt_filter.u.pattern.size_bytes = mask_size;
-	buf_len += WL_PKT_FILTER_FIXED_LEN;
-	buf_len += (WL_PKT_FILTER_PATTERN_FIXED_LEN + 2 * mask_size);
-
-	/* Keep-alive attributes are set in local
-	 * variable (keep_alive_pkt), and
-	 * then memcpy'ed into buffer (keep_alive_pktp) since there is no
-	 * guarantee that the buffer is properly aligned.
-	 */
-	memcpy((char *)pkt_filterp, &pkt_filter,
-		WL_PKT_FILTER_FIXED_LEN + WL_PKT_FILTER_PATTERN_FIXED_LEN);
-
-	err = wldev_ioctl(ndev, WLC_SET_VAR, buf, buf_len, false);
-	if (err) {
-		if (err == -EOPNOTSUPP) {
-			WL_INFO(("filter not supported\n"));
-		} else {
-			WL_ERR(("filter (%d)\n", err));
-		}
-		goto dongle_filter_out;
-	}
-
-	/* set mode to allow pattern */
-	bcm_mkiovar("pkt_filter_mode", (char *)&filter_mode, 4, iovbuf,
-		sizeof(iovbuf));
-	err = wldev_ioctl(ndev, WLC_SET_VAR, iovbuf, sizeof(iovbuf), false);
-	if (err) {
-		if (err == -EOPNOTSUPP) {
-			WL_INFO(("filter_mode not supported\n"));
-		} else {
-			WL_ERR(("filter_mode (%d)\n", err));
-		}
-		goto dongle_filter_out;
-	}
-
-dongle_filter_out:
-	return err;
-}
-#endif				/* !EMBEDDED_PLATFORM */
-
-s32 wl_config_dongle(struct wl_priv *wl, bool need_lock)
-{
-#ifndef DHD_SDALIGN
-#define DHD_SDALIGN	32
-#endif
-	struct net_device *ndev;
-	struct wireless_dev *wdev;
-	s32 err = 0;
-
-	WL_TRACE(("In\n"));
-	if (wl->dongle_up) {
-		WL_ERR(("Dongle is already up\n"));
-		return err;
-	}
-
-	ndev = wl_to_prmry_ndev(wl);
-	wdev = ndev->ieee80211_ptr;
-	if (need_lock)
-		rtnl_lock();
-	err = wl_dongle_eventmsg(ndev);
-	if (unlikely(err)) {
-		WL_ERR(("wl_dongle_eventmsg failed\n"));
-		goto default_conf_out;
-	}
-#ifndef EMBEDDED_PLATFORM
-	err = wl_dongle_up(ndev, 0);
-	if (unlikely(err)) {
-		WL_ERR(("wl_dongle_up failed\n"));
-		goto default_conf_out;
-	}
-	err = wl_dongle_country(ndev, 0);
-	if (unlikely(err)) {
-		WL_ERR(("wl_dongle_country failed\n"));
-		goto default_conf_out;
-	}
-	err = wl_dongle_power(ndev, PM_FAST);
-	if (unlikely(err)) {
-		WL_ERR(("wl_dongle_power failed\n"));
-		goto default_conf_out;
-	}
-	err = wl_dongle_glom(ndev, 0, DHD_SDALIGN);
-	if (unlikely(err)) {
-		WL_ERR(("wl_dongle_glom failed\n"));
-		goto default_conf_out;
-	}
-	err = wl_dongle_roam(ndev, (wl->roam_on ? 0 : 1), 3);
-	if (unlikely(err)) {
-		WL_ERR(("wl_dongle_roam failed\n"));
-		goto default_conf_out;
-	}
-	err = wl_dongle_eventmsg(ndev);
-	if (unlikely(err)) {
-		WL_ERR(("wl_dongle_eventmsg failed\n"));
-		goto default_conf_out;
-	}
-
-	wl_dongle_scantime(ndev, 40, 80);
-	wl_dongle_offload(ndev, 1, 0xf);
-	wl_dongle_filter(ndev, 1);
-#endif				/* !EMBEDDED_PLATFORM */
-
-	err = wl_dongle_mode(wl, ndev, wdev->iftype);
-	if (unlikely(err && err != -EINPROGRESS)) {
-		WL_ERR(("wl_dongle_mode failed\n"));
-		goto default_conf_out;
-	}
-	err = wl_dongle_probecap(wl);
-	if (unlikely(err)) {
-		WL_ERR(("wl_dongle_probecap failed\n"));
-		goto default_conf_out;
-	}
-
-	/* -EINPROGRESS: Call commit handler */
-
-default_conf_out:
-	if (need_lock)
-		rtnl_unlock();
-
-	wl->dongle_up = true;
-
-	return err;
-
-}
-
-static s32 wl_update_wiphybands(struct wl_priv *wl)
-{
-	struct wiphy *wiphy;
-	s8 phylist_buf[128];
-	s8 *phy;
-	s32 err = 0;
-
-	err = wldev_ioctl(wl_to_prmry_ndev(wl), WLC_GET_PHYLIST, phylist_buf,
-		sizeof(phylist_buf), false);
-	if (unlikely(err)) {
-		WL_ERR(("error (%d)\n", err));
-		return err;
-	}
-	phy = phylist_buf;
-	for (; *phy; phy++) {
-		if (*phy == 'a' || *phy == 'n') {
-			wiphy = wl_to_wiphy(wl);
-			wiphy->bands[IEEE80211_BAND_5GHZ] =
-				&__wl_band_5ghz_a;
-		}
-	}
-	return err;
-}
-
-static s32 __wl_cfg80211_up(struct wl_priv *wl)
-{
-	s32 err = 0;
-
-	WL_TRACE(("In\n"));
-	wl_debugfs_add_netdev_params(wl);
-
-	err = wl_config_dongle(wl, false);
-	if (unlikely(err))
-		return err;
-	dhd_monitor_init(wl->pub);
-	wl_invoke_iscan(wl);
-	wl_set_drv_status(wl, READY);
-	return err;
-}
-
-static s32 __wl_cfg80211_down(struct wl_priv *wl)
-{
-	s32 err = 0;
-
-	WL_TRACE(("In\n"));
-	/* Check if cfg80211 interface is already down */
-	if (!wl_get_drv_status(wl, READY))
-		return err;	/* it is even not ready */
-
-	wl_set_drv_status(wl, SCAN_ABORTING);
-
-	wl_term_iscan(wl);
-	if (wl->scan_request) {
-		cfg80211_scan_done(wl->scan_request, true);
-		wl->scan_request = NULL;
-	}
-	wl_clr_drv_status(wl, READY);
-	wl_clr_drv_status(wl, SCANNING);
-	wl_clr_drv_status(wl, SCAN_ABORTING);
-	wl_clr_drv_status(wl, CONNECTING);
-	wl_clr_drv_status(wl, CONNECTED);
-	wl_clr_drv_status(wl, DISCONNECTING);
-	if (wl_get_drv_status(wl, AP_CREATED)) {
-		wl_clr_drv_status(wl, AP_CREATED);
-		wl_clr_drv_status(wl, AP_CREATING);
-		wl_to_prmry_ndev(wl)->ieee80211_ptr->iftype =
-		NL80211_IFTYPE_STATION;
-	}
-	wl->dongle_up = false;
-	wl_flush_eq(wl);
-	wl_link_down(wl);
-	if (wl->p2p_supported)
-		wl_cfgp2p_down(wl);
-	dhd_monitor_uninit();
-
-	wl_debugfs_remove_netdev(wl);
-
-	return err;
-}
-
-s32 wl_cfg80211_up(void)
-{
-	struct wl_priv *wl;
-	s32 err = 0;
-
-	WL_TRACE(("In\n"));
-	wl = wlcfg_drv_priv;
-	mutex_lock(&wl->usr_sync);
-	wl_cfg80211_attach_post(wl_to_prmry_ndev(wl));
-	err = __wl_cfg80211_up(wl);
-	if (err)
-		WL_ERR(("__wl_cfg80211_up failed\n"));
-	mutex_unlock(&wl->usr_sync);
-
-	return err;
-}
-
-s32 wl_cfg80211_down(void)
-{
-	struct wl_priv *wl;
-	s32 err = 0;
-
-	WL_TRACE(("In\n"));
-	wl = wlcfg_drv_priv;
-	mutex_lock(&wl->usr_sync);
-	err = __wl_cfg80211_down(wl);
-	mutex_unlock(&wl->usr_sync);
-
-	return err;
-}
-static s32 wl_dongle_probecap(struct wl_priv *wl)
-{
-	s32 err = 0;
-
-	err = wl_update_wiphybands(wl);
-	if (unlikely(err))
-		return err;
-
-	return err;
-}
-
-static void *wl_read_prof(struct wl_priv *wl, s32 item)
-{
-	switch (item) {
-	case WL_PROF_SEC:
-		return &wl->profile->sec;
-	case WL_PROF_ACT:
-		return &wl->profile->active;
-	case WL_PROF_BSSID:
-		return &wl->profile->bssid;
-	case WL_PROF_SSID:
-		return &wl->profile->ssid;
-	}
-	WL_ERR(("invalid item (%d)\n", item));
-	return NULL;
-}
-
-static s32
-wl_update_prof(struct wl_priv *wl, const wl_event_msg_t *e, void *data,
-	s32 item)
-{
-	s32 err = 0;
-	struct wlc_ssid *ssid;
-
-	switch (item) {
-	case WL_PROF_SSID:
-		ssid = (wlc_ssid_t *) data;
-		memset(wl->profile->ssid.SSID, 0,
-			sizeof(wl->profile->ssid.SSID));
-		memcpy(wl->profile->ssid.SSID, ssid->SSID, ssid->SSID_len);
-		wl->profile->ssid.SSID_len = ssid->SSID_len;
-		break;
-	case WL_PROF_BSSID:
-		if (data)
-			memcpy(wl->profile->bssid, data, ETHER_ADDR_LEN);
-		else
-			memset(wl->profile->bssid, 0, ETHER_ADDR_LEN);
-		break;
-	case WL_PROF_SEC:
-		memcpy(&wl->profile->sec, data, sizeof(wl->profile->sec));
-		break;
-	case WL_PROF_ACT:
-		wl->profile->active = *(bool *)data;
-		break;
-	case WL_PROF_BEACONINT:
-		wl->profile->beacon_interval = *(u16 *)data;
-		break;
-	case WL_PROF_DTIMPERIOD:
-		wl->profile->dtim_period = *(u8 *)data;
-		break;
-	default:
-		WL_ERR(("unsupported item (%d)\n", item));
-		err = -EOPNOTSUPP;
-		break;
-	}
-
-	return err;
-}
-
-void wl_cfg80211_dbg_level(u32 level)
-{
-	/*
-	* prohibit to change debug level
-	* by insmod parameter.
-	* eventually debug level will be configured
-	* in compile time by using CONFIG_XXX
-	*/
-	/* wl_dbg_level = level; */
-}
-
-static bool wl_is_ibssmode(struct wl_priv *wl, struct net_device *ndev)
-{
-	return get_mode_by_netdev(wl, ndev) == WL_MODE_IBSS;
-}
-
-static __used bool wl_is_ibssstarter(struct wl_priv *wl)
-{
-	return wl->ibss_starter;
-}
-
-static void wl_rst_ie(struct wl_priv *wl)
-{
-	struct wl_ie *ie = wl_to_ie(wl);
-
-	ie->offset = 0;
-}
-
-static __used s32 wl_add_ie(struct wl_priv *wl, u8 t, u8 l, u8 *v)
-{
-	struct wl_ie *ie = wl_to_ie(wl);
-	s32 err = 0;
-
-	if (unlikely(ie->offset + l + 2 > WL_TLV_INFO_MAX)) {
-		WL_ERR(("ei crosses buffer boundary\n"));
-		return -ENOSPC;
-	}
-	ie->buf[ie->offset] = t;
-	ie->buf[ie->offset + 1] = l;
-	memcpy(&ie->buf[ie->offset + 2], v, l);
-	ie->offset += l + 2;
-
-	return err;
-}
-
-static s32 wl_mrg_ie(struct wl_priv *wl, u8 *ie_stream, u16 ie_size)
-{
-	struct wl_ie *ie = wl_to_ie(wl);
-	s32 err = 0;
-
-	if (unlikely(ie->offset + ie_size > WL_TLV_INFO_MAX)) {
-		WL_ERR(("ei_stream crosses buffer boundary\n"));
-		return -ENOSPC;
-	}
-	memcpy(&ie->buf[ie->offset], ie_stream, ie_size);
-	ie->offset += ie_size;
-
-	return err;
-}
-
-static s32 wl_cp_ie(struct wl_priv *wl, u8 *dst, u16 dst_size)
-{
-	struct wl_ie *ie = wl_to_ie(wl);
-	s32 err = 0;
-
-	if (unlikely(ie->offset > dst_size)) {
-		WL_ERR(("dst_size is not enough\n"));
-		return -ENOSPC;
-	}
-	memcpy(dst, &ie->buf[0], ie->offset);
-
-	return err;
-}
-
-static u32 wl_get_ielen(struct wl_priv *wl)
-{
-	struct wl_ie *ie = wl_to_ie(wl);
-
-	return ie->offset;
-}
-
-static void wl_link_up(struct wl_priv *wl)
-{
-	wl->link_up = true;
-}
-
-static void wl_link_down(struct wl_priv *wl)
-{
-	struct wl_connect_info *conn_info = wl_to_conn(wl);
-
-	WL_DBG(("In\n"));
-	wl->link_up = false;
-	conn_info->req_ie_len = 0;
-	conn_info->resp_ie_len = 0;
-}
-
-static void wl_lock_eq(struct wl_priv *wl)
-{
-	spin_lock_irq(&wl->eq_lock);
-}
-
-static void wl_unlock_eq(struct wl_priv *wl)
-{
-	spin_unlock_irq(&wl->eq_lock);
-}
-
-static void wl_init_eq_lock(struct wl_priv *wl)
-{
-	spin_lock_init(&wl->eq_lock);
-}
-
-static void wl_delay(u32 ms)
-{
-	if (ms < 1000 / HZ) {
-		cond_resched();
-		mdelay(ms);
-	} else {
-		msleep(ms);
-	}
-}
-
-s32 wl_cfg80211_read_fw(s8 *buf, u32 size)
-{
-	const struct firmware *fw_entry;
-	struct wl_priv *wl;
-
-	wl = wlcfg_drv_priv;
-
-	fw_entry = wl->fw->fw_entry;
-
-	if (fw_entry->size < wl->fw->ptr + size)
-		size = fw_entry->size - wl->fw->ptr;
-
-	memcpy(buf, &fw_entry->data[wl->fw->ptr], size);
-	wl->fw->ptr += size;
-	return size;
-}
-
-void wl_cfg80211_release_fw(void)
-{
-	struct wl_priv *wl;
-
-	wl = wlcfg_drv_priv;
-	release_firmware(wl->fw->fw_entry);
-	wl->fw->ptr = 0;
-}
-
-void *wl_cfg80211_request_fw(s8 *file_name)
-{
-	struct wl_priv *wl;
-	const struct firmware *fw_entry = NULL;
-	s32 err = 0;
-
-	WL_TRACE(("In\n"));
-	WL_DBG(("file name : \"%s\"\n", file_name));
-	wl = wlcfg_drv_priv;
-
-	if (!test_bit(WL_FW_LOADING_DONE, &wl->fw->status)) {
-		err = request_firmware(&wl->fw->fw_entry, file_name,
-			&wl_cfg80211_get_sdio_func()->dev);
-		if (unlikely(err)) {
-			WL_ERR(("Could not download fw (%d)\n", err));
-			goto req_fw_out;
-		}
-		set_bit(WL_FW_LOADING_DONE, &wl->fw->status);
-		fw_entry = wl->fw->fw_entry;
-		if (fw_entry) {
-			WL_DBG(("fw size (%zd), data (%p)\n", fw_entry->size,
-				fw_entry->data));
-		}
-	} else if (!test_bit(WL_NVRAM_LOADING_DONE, &wl->fw->status)) {
-		err = request_firmware(&wl->fw->fw_entry, file_name,
-			&wl_cfg80211_get_sdio_func()->dev);
-		if (unlikely(err)) {
-			WL_ERR(("Could not download nvram (%d)\n", err));
-			goto req_fw_out;
-		}
-		set_bit(WL_NVRAM_LOADING_DONE, &wl->fw->status);
-		fw_entry = wl->fw->fw_entry;
-		if (fw_entry) {
-			WL_DBG(("nvram size (%zd), data (%p)\n", fw_entry->size,
-				fw_entry->data));
-		}
-	} else {
-		WL_DBG(("Downloading already done. Nothing to do more\n"));
-		err = -EPERM;
-	}
-
-req_fw_out:
-	if (unlikely(err)) {
-		return NULL;
-	}
-	wl->fw->ptr = 0;
-	return (void *)fw_entry->data;
-}
-
-s8 *wl_cfg80211_get_fwname(void)
-{
-	struct wl_priv *wl;
-
-	wl = wlcfg_drv_priv;
-	strcpy(wl->fw->fw_name, WL_4329_FW_FILE);
-	return wl->fw->fw_name;
-}
-
-s8 *wl_cfg80211_get_nvramname(void)
-{
-	struct wl_priv *wl;
-
-	wl = wlcfg_drv_priv;
-	strcpy(wl->fw->nvram_name, WL_4329_NVRAM_FILE);
-	return wl->fw->nvram_name;
-}
-
-s32 wl_cfg80211_get_p2p_dev_addr(struct net_device *net, struct ether_addr *p2pdev_addr)
-{
-	struct wl_priv *wl;
-	dhd_pub_t *dhd_pub;
-	struct ether_addr p2pif_addr;
-
-	wl = wlcfg_drv_priv;
-	dhd_pub = (dhd_pub_t *)wl->pub;
-	wl_cfgp2p_generate_bss_mac(&dhd_pub->mac, p2pdev_addr, &p2pif_addr);
-
-	return 0;
-}
-
-static __used void wl_dongle_poweron(struct wl_priv *wl)
-{
-
-	WL_DBG(("Enter \n"));
-	dhd_customer_gpio_wlan_ctrl(WLAN_RESET_ON);
-
-#if defined(BCMLXSDMMC)
-	sdioh_start(NULL, 0);
-#endif
-#if defined(BCMLXSDMMC)
-	sdioh_start(NULL, 1);
-#endif
-	wl_cfg80211_resume(wl_to_wiphy(wl));
-}
-
-static __used void wl_dongle_poweroff(struct wl_priv *wl)
-{
-
-
-	WL_DBG(("Enter \n"));
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 39)
-	wl_cfg80211_suspend(wl_to_wiphy(wl), NULL);
-#else
-	wl_cfg80211_suspend(wl_to_wiphy(wl));
-#endif
-
-#if defined(BCMLXSDMMC)
-	sdioh_stop(NULL);
-#endif
-	/* clean up dtim_skip setting */
-	dhd_customer_gpio_wlan_ctrl(WLAN_RESET_OFF);
-}
-static int wl_debugfs_add_netdev_params(struct wl_priv *wl)
-{
-	char buf[10+IFNAMSIZ];
-	struct dentry *fd;
-	s32 err = 0;
-
-	WL_TRACE(("In\n"));
-	sprintf(buf, "netdev:%s", wl_to_prmry_ndev(wl)->name);
-	wl->debugfsdir = debugfs_create_dir(buf, wl_to_wiphy(wl)->debugfsdir);
-
-	fd = debugfs_create_u16("beacon_int", S_IRUGO, wl->debugfsdir,
-		(u16 *)&wl->profile->beacon_interval);
-	if (!fd) {
-		err = -ENOMEM;
-		goto err_out;
-	}
-
-	fd = debugfs_create_u8("dtim_period", S_IRUGO, wl->debugfsdir,
-		(u8 *)&wl->profile->dtim_period);
-	if (!fd) {
-		err = -ENOMEM;
-		goto err_out;
-	}
-
-err_out:
-	return err;
-}
-
-static void wl_debugfs_remove_netdev(struct wl_priv *wl)
-{
-	WL_DBG(("Enter \n"));
-}
-
-static const struct rfkill_ops wl_rfkill_ops = {
-	.set_block = wl_rfkill_set
-};
-
-static int wl_rfkill_set(void *data, bool blocked)
-{
-	struct wl_priv *wl = (struct wl_priv *)data;
-
-	WL_DBG(("Enter \n"));
-	WL_DBG(("RF %s\n", blocked ? "blocked" : "unblocked"));
-
-	if (!wl)
-		return -EINVAL;
-
-	wl->rf_blocked = blocked;
-
-	return 0;
-}
-
-static int wl_setup_rfkill(struct wl_priv *wl, bool setup)
-{
-	s32 err = 0;
-
-	WL_DBG(("Enter \n"));
-	if (!wl)
-		return -EINVAL;
-	if (setup) {
-		wl->rfkill = rfkill_alloc("brcmfmac-wifi",
-			&wl_cfg80211_get_sdio_func()->dev,
-			RFKILL_TYPE_WLAN, &wl_rfkill_ops, (void *)wl);
-
-		if (!wl->rfkill) {
-			err = -ENOMEM;
-			goto err_out;
-		}
-
-		err = rfkill_register(wl->rfkill);
-
-		if (err)
-			rfkill_destroy(wl->rfkill);
-	} else {
-		if (!wl->rfkill) {
-			err = -ENOMEM;
-			goto err_out;
-		}
-
-		rfkill_unregister(wl->rfkill);
-		rfkill_destroy(wl->rfkill);
-	}
-
-err_out:
-	return err;
-}
diff --git a/drivers/net/wireless/bcmdhd/wl_cfg80211.h b/drivers/net/wireless/bcmdhd/wl_cfg80211.h
deleted file mode 100644
index a5637240c17..00000000000
--- a/drivers/net/wireless/bcmdhd/wl_cfg80211.h
+++ /dev/null
@@ -1,520 +0,0 @@
-/*
- * Linux cfg80211 driver
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: wl_cfg80211.h,v 1.1.4.1.2.8 2011/02/09 01:37:52 Exp $
- */
-
-#ifndef _wl_cfg80211_h_
-#define _wl_cfg80211_h_
-
-#include <linux/wireless.h>
-#include <typedefs.h>
-#include <proto/ethernet.h>
-#include <wlioctl.h>
-#include <linux/wireless.h>
-#include <net/cfg80211.h>
-#include <linux/rfkill.h>
-
-#include <wl_cfgp2p.h>
-
-struct wl_conf;
-struct wl_iface;
-struct wl_priv;
-struct wl_security;
-struct wl_ibss;
-
-
-#define htod32(i) i
-#define htod16(i) i
-#define dtoh32(i) i
-#define dtoh16(i) i
-#define htodchanspec(i) i
-#define dtohchanspec(i) i
-
-#define WL_DBG_NONE	0
-#define WL_DBG_SCAN 	(1 << 3)
-#define WL_DBG_DBG 	(1 << 2)
-#define WL_DBG_INFO	(1 << 1)
-#define WL_DBG_ERR	(1 << 0)
-#define WL_DBG_MASK ((WL_DBG_DBG | WL_DBG_INFO | WL_DBG_ERR) << 1)
-
-/* 0 invalidates all debug messages.  default is 1 */
-#define WL_DBG_LEVEL 0xFF
-
-#define	WL_ERR(args)									\
-do {										\
-	if (wl_dbg_level & WL_DBG_ERR) {				\
-			printk(KERN_ERR "CFG80211-ERROR) %s : ", __func__);	\
-			printk args;						\
-		} 								\
-} while (0)
-#define	WL_INFO(args)									\
-do {										\
-	if (wl_dbg_level & WL_DBG_INFO) {				\
-			printk(KERN_ERR "CFG80211-INFO) %s : ", __func__);	\
-			printk args;						\
-		}								\
-} while (0)
-#define	WL_SCAN(args)								\
-do {									\
-	if (wl_dbg_level & WL_DBG_SCAN) {			\
-		printk(KERN_ERR "CFG80211-SCAN) %s :", __func__);	\
-		printk args;							\
-	}									\
-} while (0)
-#if (WL_DBG_LEVEL > 0)
-#define	WL_DBG(args)								\
-do {									\
-	if (wl_dbg_level & WL_DBG_DBG) {			\
-		printk(KERN_ERR "CFG80211-DEBUG) %s :", __func__);	\
-		printk args;							\
-	}									\
-} while (0)
-#else				/* !(WL_DBG_LEVEL > 0) */
-#define	WL_DBG(args)
-#endif				/* (WL_DBG_LEVEL > 0) */
-
-#define WL_SCAN_RETRY_MAX	3	/* used for ibss scan */
-#define WL_NUM_PMKIDS_MAX	MAXPMKID	/* will be used
-						 * for 2.6.33 kernel
-						 * or later
-						 */
-#define WL_SCAN_BUF_MAX 		(1024 * 8)
-#define WL_TLV_INFO_MAX 		1024
-#define WL_SCAN_IE_LEN_MAX      2048
-#define WL_BSS_INFO_MAX			2048
-#define WL_ASSOC_INFO_MAX	512	/*
-				 * needs to grab assoc info from dongle to
-				 * report it to cfg80211 through "connect"
-				 * event
-				 */
-#define WL_IOCTL_LEN_MAX	1024
-#define WL_EXTRA_BUF_MAX	2048
-#define WL_ISCAN_BUF_MAX	2048	/*
-				 * the buf lengh can be WLC_IOCTL_MAXLEN (8K)
-				 * to reduce iteration
-				 */
-#define WL_ISCAN_TIMER_INTERVAL_MS	3000
-#define WL_SCAN_ERSULTS_LAST 	(WL_SCAN_RESULTS_NO_MEM+1)
-#define WL_AP_MAX	256	/* virtually unlimitted as long
-				 * as kernel memory allows
-				 */
-#define WL_FILE_NAME_MAX		256
-#define WL_DWELL_TIME 	200
-#define VWDEV_CNT 3
-/* dongle status */
-enum wl_status {
-	WL_STATUS_READY = 0,
-	WL_STATUS_SCANNING,
-	WL_STATUS_SCAN_ABORTING,
-	WL_STATUS_CONNECTING,
-	WL_STATUS_CONNECTED,
-	WL_STATUS_DISCONNECTING,
-	WL_STATUS_AP_CREATING,
-	WL_STATUS_AP_CREATED
-};
-
-/* wi-fi mode */
-enum wl_mode {
-	WL_MODE_BSS,
-	WL_MODE_IBSS,
-	WL_MODE_AP
-};
-
-/* dongle profile list */
-enum wl_prof_list {
-	WL_PROF_MODE,
-	WL_PROF_SSID,
-	WL_PROF_SEC,
-	WL_PROF_IBSS,
-	WL_PROF_BAND,
-	WL_PROF_BSSID,
-	WL_PROF_ACT,
-	WL_PROF_BEACONINT,
-	WL_PROF_DTIMPERIOD
-};
-
-/* dongle iscan state */
-enum wl_iscan_state {
-	WL_ISCAN_STATE_IDLE,
-	WL_ISCAN_STATE_SCANING
-};
-
-/* donlge escan state */
-enum wl_escan_state {
-    WL_ESCAN_STATE_IDLE,
-    WL_ESCAN_STATE_SCANING
-};
-/* fw downloading status */
-enum wl_fw_status {
-	WL_FW_LOADING_DONE,
-	WL_NVRAM_LOADING_DONE
-};
-
-/* beacon / probe_response */
-struct beacon_proberesp {
-	__le64 timestamp;
-	__le16 beacon_int;
-	__le16 capab_info;
-	u8 variable[0];
-} __attribute__ ((packed));
-
-/* dongle configuration */
-struct wl_conf {
-	struct net_mode {
-		struct net_device *ndev;
-		s32 type;
-	} mode [VWDEV_CNT + 1];		/* adhoc , infrastructure or ap */
-	u32 frag_threshold;
-	u32 rts_threshold;
-	u32 retry_short;
-	u32 retry_long;
-	s32 tx_power;
-	struct ieee80211_channel channel;
-};
-
-typedef s32(*EVENT_HANDLER) (struct wl_priv *wl,
-                            struct net_device *ndev, const wl_event_msg_t *e, void *data);
-
-
-/* bss inform structure for cfg80211 interface */
-struct wl_cfg80211_bss_info {
-	u16 band;
-	u16 channel;
-	s16 rssi;
-	u16 frame_len;
-	u8 frame_buf[1];
-};
-
-/* basic structure of scan request */
-struct wl_scan_req {
-	struct wlc_ssid ssid;
-};
-
-/* basic structure of information element */
-struct wl_ie {
-	u16 offset;
-	u8 buf[WL_TLV_INFO_MAX];
-};
-
-/* event queue for cfg80211 main event */
-struct wl_event_q {
-	struct list_head eq_list;
-	u32 etype;
-	wl_event_msg_t emsg;
-	s8 edata[1];
-};
-
-/* security information with currently associated ap */
-struct wl_security {
-	u32 wpa_versions;
-	u32 auth_type;
-	u32 cipher_pairwise;
-	u32 cipher_group;
-	u32 wpa_auth;
-};
-
-/* ibss information for currently joined ibss network */
-struct wl_ibss {
-	u8 beacon_interval;	/* in millisecond */
-	u8 atim;		/* in millisecond */
-	s8 join_only;
-	u8 band;
-	u8 channel;
-};
-
-/* dongle profile */
-struct wl_profile {
-	u32 mode;
-	struct wlc_ssid ssid;
-	u8 bssid[ETHER_ADDR_LEN];
-	u16 beacon_interval;
-	u8 dtim_period;
-	struct wl_security sec;
-	struct wl_ibss ibss;
-	s32 band;
-	bool active;
-};
-
-typedef s32(*ISCAN_HANDLER) (struct wl_priv *wl);
-
-/* dongle iscan controller */
-struct wl_iscan_ctrl {
-	struct net_device *dev;
-	struct timer_list timer;
-	u32 timer_ms;
-	u32 timer_on;
-	s32 state;
-	struct task_struct *tsk;
-	struct semaphore sync;
-	ISCAN_HANDLER iscan_handler[WL_SCAN_ERSULTS_LAST];
-	void *data;
-	s8 ioctl_buf[WLC_IOCTL_SMLEN];
-	s8 scan_buf[WL_ISCAN_BUF_MAX];
-};
-
-/* association inform */
-#define MAX_REQ_LINE 1024
-struct wl_connect_info {
-	u8 req_ie[MAX_REQ_LINE];
-	s32 req_ie_len;
-	u8 resp_ie[MAX_REQ_LINE];
-	s32 resp_ie_len;
-};
-
-/* firmware /nvram downloading controller */
-struct wl_fw_ctrl {
-	const struct firmware *fw_entry;
-	unsigned long status;
-	u32 ptr;
-	s8 fw_name[WL_FILE_NAME_MAX];
-	s8 nvram_name[WL_FILE_NAME_MAX];
-};
-
-/* assoc ie length */
-struct wl_assoc_ielen {
-	u32 req_len;
-	u32 resp_len;
-};
-
-/* wpa2 pmk list */
-struct wl_pmk_list {
-	pmkid_list_t pmkids;
-	pmkid_t foo[MAXPMKID - 1];
-};
-
-
-#define ESCAN_BUF_SIZE (64 * 1024)
-
-struct escan_info {
-    u32 escan_state;
-    u8 escan_buf[ESCAN_BUF_SIZE];
-    struct wiphy *wiphy;
-};
-
-struct ap_info {
-/* Structure to hold WPS, WPA IEs for a AP */
-	u8   probe_res_ie[IE_MAX_LEN];
-	u8   beacon_ie[IE_MAX_LEN];
-	u32 probe_res_ie_len;
-	u32 beacon_ie_len;
-	u8 *wpa_ie;
-	u8 *rsn_ie;
-	u8 *wps_ie;
-	bool security_mode;
-};
-/* dongle private data of cfg80211 interface */
-struct wl_priv {
-	struct wireless_dev *wdev;	/* representing wl cfg80211 device */
-	struct wireless_dev *vwdev[VWDEV_CNT];
-	struct wl_conf *conf;	/* dongle configuration */
-	struct cfg80211_scan_request *scan_request;	/* scan request object */
-	EVENT_HANDLER evt_handler[WLC_E_LAST];
-	struct list_head eq_list;	/* used for event queue */
-	spinlock_t eq_lock;	/* for event queue synchronization */
-	struct mutex usr_sync;	/* maily for dongle up/down synchronization */
-	struct wl_scan_results *bss_list;
-	struct wl_scan_results *scan_results;
-
-	/* scan request object for internal purpose */
-	struct wl_scan_req *scan_req_int;
-
-	/* bss information for cfg80211 layer */
-	struct wl_cfg80211_bss_info *bss_info;
-	/* information element object for internal purpose */
-	struct wl_ie ie;
-	u8 scan_ie_buf[2048];
-	int scan_ie_len;
-	struct ether_addr bssid;	/* bssid of currently engaged network */
-
-	/* for synchronization of main event thread */
-	struct wl_profile *profile;	/* holding dongle profile */
-	struct wl_iscan_ctrl *iscan;	/* iscan controller */
-
-	/* association information container */
-	struct wl_connect_info conn_info;
-
-	/* control firwmare and nvram paramter downloading */
-	struct wl_fw_ctrl *fw;
-	struct wl_pmk_list *pmk_list;	/* wpa2 pmk list */
-	tsk_ctl_t event_tsk;		/* task of main event handler thread */
-	unsigned long status;		/* current dongle status */
-	void *pub;
-	u32 channel;		/* current channel */
-	bool iscan_on;		/* iscan on/off switch */
-	bool iscan_kickstart;	/* indicate iscan already started */
-	bool escan_on;      /* escan on/off switch */
-	struct escan_info escan_info;   /* escan information */
-	bool active_scan;	/* current scan mode */
-	bool ibss_starter;	/* indicates this sta is ibss starter */
-	bool link_up;		/* link/connection up flag */
-
-	/* indicate whether dongle to support power save mode */
-	bool pwr_save;
-	bool dongle_up;		/* indicate whether dongle up or not */
-	bool roam_on;		/* on/off switch for dongle self-roaming */
-	bool scan_tried;	/* indicates if first scan attempted */
-	u8 *ioctl_buf;	/* ioctl buffer */
-	u8 *escan_ioctl_buf;
-	u8 *extra_buf;	/* maily to grab assoc information */
-	struct dentry *debugfsdir;
-	struct rfkill *rfkill;
-	bool rf_blocked;
-	struct ieee80211_channel remain_on_chan;
-	enum nl80211_channel_type remain_on_chan_type;
-	u64 cache_cookie;
-	wait_queue_head_t dongle_event_wait;
-	struct ap_info *ap_info;
-	struct p2p_info *p2p;
-	bool p2p_supported;
-	s8 last_eventmask[WL_EVENTING_MASK_LEN];
-};
-
-#define wl_to_wiphy(w) (w->wdev->wiphy)
-#define wl_to_prmry_ndev(w) (w->wdev->netdev)
-#define ndev_to_wl(n) (wdev_to_wl(n->ieee80211_ptr))
-#define wl_to_sr(w) (w->scan_req_int)
-#define wl_to_ie(w) (&w->ie)
-#define iscan_to_wl(i) ((struct wl_priv *)(i->data))
-#define wl_to_iscan(w) (w->iscan)
-#define wl_to_conn(w) (&w->conn_info)
-#define wiphy_from_scan(w) (w->escan_info.wiphy)
-#define wl_get_drv_status(wl, stat)   (test_bit(WL_STATUS_ ## stat, &(wl)->status))
-#define wl_set_drv_status(wl, stat)   (set_bit(WL_STATUS_ ## stat, &(wl)->status))
-#define wl_clr_drv_status(wl, stat)   (clear_bit(WL_STATUS_ ## stat, &(wl)->status))
-#define wl_chg_drv_status(wl, stat)   (change_bit(WL_STATUS_ ## stat, &(wl)->status))
-
-static inline struct wl_bss_info *next_bss(struct wl_scan_results *list, struct wl_bss_info *bss)
-{
-	return bss = bss ?
-		(struct wl_bss_info *)((uintptr) bss + dtoh32(bss->length)) : list->bss_info;
-}
-static inline s32 alloc_idx_vwdev(struct wl_priv *wl)
-{
-	s32 i = 0;
-	for (i = 0; i < VWDEV_CNT; i++) {
-		if (wl->vwdev[i] == NULL)
-				return i;
-	}
-	return -1;
-}
-
-static inline s32 get_idx_vwdev_by_netdev(struct wl_priv *wl, struct net_device *ndev)
-{
-	s32 i = 0;
-	for (i = 0; i < VWDEV_CNT; i++) {
-		if ((wl->vwdev[i] != NULL) && (wl->vwdev[i]->netdev == ndev))
-				return i;
-	}
-	return -1;
-}
-
-static inline s32 get_mode_by_netdev(struct wl_priv *wl, struct net_device *ndev)
-{
-	s32 i = 0;
-	for (i = 0; i <= VWDEV_CNT; i++) {
-		if (wl->conf->mode[i].ndev != NULL && (wl->conf->mode[i].ndev == ndev))
-			return wl->conf->mode[i].type;
-	}
-	return -1;
-}
-static inline void set_mode_by_netdev(struct wl_priv *wl, struct net_device *ndev, s32 type)
-{
-	s32 i = 0;
-	for (i = 0; i <= VWDEV_CNT; i++) {
-		if (type == -1) {
-			/* free the info of netdev */
-			if (wl->conf->mode[i].ndev == ndev) {
-				wl->conf->mode[i].ndev = NULL;
-				wl->conf->mode[i].type = -1;
-				break;
-			}
-
-		} else {
-			if ((wl->conf->mode[i].ndev != NULL)&&
-			(wl->conf->mode[i].ndev == ndev)) {
-				/* update type of ndev */
-				wl->conf->mode[i].type = type;
-				break;
-			}
-			else if ((wl->conf->mode[i].ndev == NULL)&&
-			(wl->conf->mode[i].type == -1)) {
-				wl->conf->mode[i].ndev = ndev;
-				wl->conf->mode[i].type = type;
-				break;
-			}
-		}
-	}
-}
-#define free_vwdev_by_index(wl, __i) do {      \
-						if (wl->vwdev[__i] != NULL) \
-							kfree(wl->vwdev[__i]); \
-						wl->vwdev[__i] = NULL; \
-					} while (0)
-
-#define for_each_bss(list, bss, __i)	\
-	for (__i = 0; __i < list->count && __i < WL_AP_MAX; __i++, bss = next_bss(list, bss))
-
-/* In case of WPS from wpa_supplicant, pairwise siute and group suite is 0.
- * In addtion to that, wpa_version is WPA_VERSION_1
- */
-#define is_wps_conn(_sme) \
-	((_sme->crypto.wpa_versions & NL80211_WPA_VERSION_1) && \
-	 (!_sme->crypto.n_ciphers_pairwise) && \
-	 (!_sme->crypto.cipher_group))
-extern s32 wl_cfg80211_attach(struct net_device *ndev, void *data);
-extern s32 wl_cfg80211_attach_post(struct net_device *ndev);
-extern void wl_cfg80211_detach(void);
-/* event handler from dongle */
-extern void wl_cfg80211_event(struct net_device *ndev, const wl_event_msg_t *e,
-            void *data);
-extern void wl_cfg80211_set_sdio_func(void *func);	/* set sdio function info */
-extern struct sdio_func *wl_cfg80211_get_sdio_func(void);	/* set sdio function info */
-extern s32 wl_cfg80211_up(void);	/* dongle up */
-extern s32 wl_cfg80211_down(void);	/* dongle down */
-extern s32 wl_cfg80211_notify_ifadd(struct net_device *net, s32 idx, s32 bssidx,
-int (*_net_attach)(dhd_pub_t *dhdp, int ifidx));
-extern s32 wl_cfg80211_ifdel_ops(struct net_device *net);
-extern s32 wl_cfg80211_notify_ifdel(struct net_device *net);
-extern s32 wl_cfg80211_is_progress_ifadd(void);
-extern s32 wl_cfg80211_is_progress_ifchange(void);
-extern s32 wl_cfg80211_is_progress_ifadd(void);
-extern s32 wl_cfg80211_notify_ifchange(void);
-extern void wl_cfg80211_dbg_level(u32 level);
-extern void *wl_cfg80211_request_fw(s8 *file_name);
-extern s32 wl_cfg80211_read_fw(s8 *buf, u32 size);
-extern void wl_cfg80211_release_fw(void);
-extern s8 *wl_cfg80211_get_fwname(void);
-extern s8 *wl_cfg80211_get_nvramname(void);
-extern s32 wl_cfg80211_get_p2p_dev_addr(struct net_device *net, struct ether_addr *p2pdev_addr);
-#ifdef CONFIG_SYSCTL
-extern s32 wl_cfg80211_sysctl_export_devaddr(void *data);
-#endif
-
-/* do scan abort */
-extern s32
-wl_cfg80211_scan_abort(struct wl_priv *wl, struct net_device *ndev);
-
-extern s32
-wl_cfg80211_if_is_group_owner(void);
-#endif				/* _wl_cfg80211_h_ */
diff --git a/drivers/net/wireless/bcmdhd/wl_cfgp2p.c b/drivers/net/wireless/bcmdhd/wl_cfgp2p.c
deleted file mode 100644
index 98271c28658..00000000000
--- a/drivers/net/wireless/bcmdhd/wl_cfgp2p.c
+++ /dev/null
@@ -1,1276 +0,0 @@
-/*
- * Linux cfgp2p driver
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: wl_cfgp2p.c,v 1.1.4.1.2.14 2011-02-09 01:40:07 Exp $
- *
- */
-#include <typedefs.h>
-#include <linuxver.h>
-#include <osl.h>
-#include <linux/kernel.h>
-#include <linux/kthread.h>
-#include <linux/netdevice.h>
-#include <linux/types.h>
-#include <linux/string.h>
-#include <linux/timer.h>
-#include <linux/if_arp.h>
-#include <asm/uaccess.h>
-
-#include <bcmutils.h>
-#include <bcmendian.h>
-#include <proto/ethernet.h>
-#include <dngl_stats.h>
-#include <dhd.h>
-#include <dhdioctl.h>
-#include <wlioctl.h>
-
-#include <wl_cfg80211.h>
-#include <wl_cfgp2p.h>
-#include <wldev_common.h>
-
-
-static s8 ioctlbuf[WLC_IOCTL_MAXLEN];
-static s8 scanparambuf[WLC_IOCTL_SMLEN];
-static s8 *smbuf = ioctlbuf;
-
-static bool
-wl_cfgp2p_has_ie(u8 *ie, u8 **tlvs, u32 *tlvs_len, const u8 *oui, u32 oui_len, u8 type);
-
-static s32
-wl_cfgp2p_vndr_ie(struct net_device *ndev, s32 bssidx, s32 pktflag,
-            s8 *oui, s32 ie_id, s8 *data, s32 data_len, s32 delete);
-/* 
- *  Initialize variables related to P2P
- *
- */
-s32
-wl_cfgp2p_init_priv(struct wl_priv *wl)
-{
-	if (!(wl->p2p = kzalloc(sizeof(struct p2p_info), GFP_KERNEL))) {
-		CFGP2P_ERR(("struct p2p_info allocation failed\n"));
-		return -ENOMEM;
-	}
-#define INIT_IE(IE_TYPE, BSS_TYPE)		\
-	do {							\
-		memset(wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie, 0, \
-		   sizeof(wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie)); \
-		wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie_len = 0; \
-	} while (0);
-
-	INIT_IE(probe_req, P2PAPI_BSSCFG_PRIMARY);
-	INIT_IE(probe_res, P2PAPI_BSSCFG_PRIMARY);
-	INIT_IE(assoc_req, P2PAPI_BSSCFG_PRIMARY);
-	INIT_IE(assoc_res, P2PAPI_BSSCFG_PRIMARY);
-	INIT_IE(beacon,    P2PAPI_BSSCFG_PRIMARY);
-	INIT_IE(probe_req, P2PAPI_BSSCFG_DEVICE);
-	INIT_IE(probe_res, P2PAPI_BSSCFG_DEVICE);
-	INIT_IE(assoc_req, P2PAPI_BSSCFG_DEVICE);
-	INIT_IE(assoc_res, P2PAPI_BSSCFG_DEVICE);
-	INIT_IE(beacon,    P2PAPI_BSSCFG_DEVICE);
-	INIT_IE(probe_req, P2PAPI_BSSCFG_CONNECTION);
-	INIT_IE(probe_res, P2PAPI_BSSCFG_CONNECTION);
-	INIT_IE(assoc_req, P2PAPI_BSSCFG_CONNECTION);
-	INIT_IE(assoc_res, P2PAPI_BSSCFG_CONNECTION);
-	INIT_IE(beacon,    P2PAPI_BSSCFG_CONNECTION);
-#undef INIT_IE
-	wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_PRIMARY) = wl_to_prmry_ndev(wl);
-	wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_PRIMARY) = 0;
-	wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE) = NULL;
-	wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) = 0;
-	wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION) = NULL;
-	wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_CONNECTION) = 0;
-
-	return BCME_OK;
-
-}
-/*
- *  Deinitialize variables related to P2P
- *
- */
-void
-wl_cfgp2p_deinit_priv(struct wl_priv *wl)
-{
-	if (wl->p2p) {
-		kfree(wl->p2p);
-	}
-	wl->p2p_supported = 0;
-}
-/*
- * Set P2P functions into firmware
- */
-s32
-wl_cfgp2p_set_firm_p2p(struct wl_priv *wl)
-{
-	struct net_device *ndev = wl_to_prmry_ndev(wl);
-	s32 ret = BCME_OK;
-	s32 val = 0;
-	/* Do we have to check whether APSTA is enabled or not ? */
-	wldev_iovar_getint(ndev, "apsta", &val);
-	if (val == 0) {
-		val = 1;
-		wldev_ioctl(ndev, WLC_DOWN, &val, sizeof(s32), false);
-		wldev_iovar_setint(ndev, "apsta", val);
-		wldev_ioctl(ndev, WLC_UP, &val, sizeof(s32), false);
-	}
-	return ret;
-}
-
-/* Create a new P2P BSS.
- * Parameters:
- * @mac      : MAC address of the BSS to create
- * @if_type  : interface type: WL_P2P_IF_GO or WL_P2P_IF_CLIENT
- * @chspec   : chspec to use if creating a GO BSS.
- * Returns 0 if success.
- */
-s32
-wl_cfgp2p_ifadd(struct wl_priv *wl, struct ether_addr *mac, u8 if_type,
-            chanspec_t chspec)
-{
-	wl_p2p_if_t ifreq;
-	s32 err;
-	struct net_device *ndev = wl_to_prmry_ndev(wl);
-
-	ifreq.type = if_type;
-	ifreq.chspec = chspec;
-	memcpy(ifreq.addr.octet, mac->octet, sizeof(ifreq.addr.octet));
-
-	CFGP2P_INFO(("---wl p2p_ifadd %02x:%02x:%02x:%02x:%02x:%02x %s %u\n",
-	    ifreq.addr.octet[0], ifreq.addr.octet[1], ifreq.addr.octet[2],
-		ifreq.addr.octet[3], ifreq.addr.octet[4], ifreq.addr.octet[5],
-		(if_type == WL_P2P_IF_GO) ? "go" : "client",
-	        (chspec & WL_CHANSPEC_CHAN_MASK) >> WL_CHANSPEC_CHAN_SHIFT));
-
-	err = wldev_iovar_setbuf(ndev, "p2p_ifadd", &ifreq, sizeof(ifreq),
-		ioctlbuf, sizeof(ioctlbuf));
-	return err;
-}
-
-/* Delete a P2P BSS.
- * Parameters:
- * @mac      : MAC address of the BSS to create
- * Returns 0 if success.
- */
-s32
-wl_cfgp2p_ifdel(struct wl_priv *wl, struct ether_addr *mac)
-{
-	s32 ret;
-	struct net_device *netdev = wl_to_prmry_ndev(wl);
-
-	CFGP2P_INFO(("------primary idx %d : wl p2p_ifdel %02x:%02x:%02x:%02x:%02x:%02x\n",
-	    netdev->ifindex, mac->octet[0], mac->octet[1], mac->octet[2],
-	    mac->octet[3], mac->octet[4], mac->octet[5]));
-
-	ret = wldev_iovar_setbuf(netdev, "p2p_ifdel", mac, sizeof(*mac),
-		ioctlbuf, sizeof(ioctlbuf));
-
-	if (unlikely(ret < 0)) {
-		printk("'wl p2p_ifdel' error %d\n", ret);
-	}
-	return ret;
-}
-
-/* Change a P2P Role.
- * Parameters:
- * @mac      : MAC address of the BSS to change a role
- * Returns 0 if success.
- */
-s32
-wl_cfgp2p_ifchange(struct wl_priv *wl, struct ether_addr *mac, u8 if_type,
-            chanspec_t chspec)
-{
-	wl_p2p_if_t ifreq;
-	s32 err;
-	struct net_device *netdev =  wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION);
-
-	ifreq.type = if_type;
-	ifreq.chspec = chspec;
-	memcpy(ifreq.addr.octet, mac->octet, sizeof(ifreq.addr.octet));
-
-	CFGP2P_INFO(("---wl p2p_ifchange %02x:%02x:%02x:%02x:%02x:%02x %s %u\n",
-	    ifreq.addr.octet[0], ifreq.addr.octet[1], ifreq.addr.octet[2],
-	    ifreq.addr.octet[3], ifreq.addr.octet[4], ifreq.addr.octet[5],
-	    (if_type == WL_P2P_IF_GO) ? "go" : "client",
-		(chspec & WL_CHANSPEC_CHAN_MASK) >> WL_CHANSPEC_CHAN_SHIFT));
-
-	err = wldev_iovar_setbuf(netdev, "p2p_ifupd", &ifreq, sizeof(ifreq),
-		ioctlbuf, sizeof(ioctlbuf));
-
-	if (unlikely(err < 0)) {
-		printk("'wl p2p_ifupd' error %d\n", err);
-	}
-	return err;
-}
-
-
-/* Get the index of a created P2P BSS.
- * Parameters:
- * @mac      : MAC address of the created BSS
- * @index    : output: index of created BSS
- * Returns 0 if success.
- */
-s32
-wl_cfgp2p_ifidx(struct wl_priv *wl, struct ether_addr *mac, s32 *index)
-{
-	s32 ret;
-	u8 getbuf[64];
-	struct net_device *dev = wl_to_prmry_ndev(wl);
-
-	CFGP2P_INFO(("---wl p2p_if %02x:%02x:%02x:%02x:%02x:%02x\n",
-	    mac->octet[0], mac->octet[1], mac->octet[2],
-	    mac->octet[3], mac->octet[4], mac->octet[5]));
-
-	ret = wldev_iovar_getbuf_bsscfg(dev, "p2p_if", mac, sizeof(*mac),
-	            getbuf, sizeof(getbuf), wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_PRIMARY));
-
-	if (ret == 0) {
-		memcpy(index, getbuf, sizeof(index));
-		CFGP2P_INFO(("---wl p2p_if   ==> %d\n", *index));
-	}
-
-	return ret;
-}
-
-s32
-wl_cfgp2p_set_discovery(struct wl_priv *wl, s32 on)
-{
-	s32 ret = BCME_OK;
-	struct net_device *ndev = wl_to_prmry_ndev(wl);
-	CFGP2P_DBG(("enter\n"));
-
-	ret = wldev_iovar_setint(ndev, "p2p_disc", on);
-
-	if (unlikely(ret < 0)) {
-		CFGP2P_ERR(("p2p_disc %d error %d\n", on, ret));
-	}
-
-	return ret;
-}
-
-/* Set the WL driver's P2P mode.
- * Parameters :
- * @mode      : is one of WL_P2P_DISC_ST_{SCAN,LISTEN,SEARCH}.
- * @channel   : the channel to listen
- * @listen_ms : the time (milli seconds) to wait
- * @bssidx    : bss index for BSSCFG
- * Returns 0 if success
- */
-
-s32
-wl_cfgp2p_set_p2p_mode(struct wl_priv *wl, u8 mode, u32 channel, u16 listen_ms, int bssidx)
-{
-	wl_p2p_disc_st_t discovery_mode;
-	s32 ret;
-	struct net_device *dev;
-	CFGP2P_DBG(("enter\n"));
-
-	if (unlikely(bssidx >= P2PAPI_BSSCFG_MAX)) {
-		CFGP2P_ERR((" %d index out of range\n", bssidx));
-		return -1;
-	}
-
-	dev = wl_to_p2p_bss_ndev(wl, bssidx);
-	if (unlikely(dev == NULL)) {
-		CFGP2P_ERR(("bssidx %d is not assigned\n", bssidx));
-		return BCME_NOTFOUND;
-	}
-
-	/* Put the WL driver into P2P Listen Mode to respond to P2P probe reqs */
-	discovery_mode.state = mode;
-	discovery_mode.chspec = CH20MHZ_CHSPEC(channel);
-	discovery_mode.dwell = listen_ms;
-	ret = wldev_iovar_setbuf_bsscfg(dev, "p2p_state", &discovery_mode,
-	            sizeof(discovery_mode), ioctlbuf, sizeof(ioctlbuf), bssidx);
-
-	return ret;
-}
-
-/* Get the index of the P2P Discovery BSS */
-s32
-wl_cfgp2p_get_disc_idx(struct wl_priv *wl, s32 *index)
-{
-	s32 ret;
-	struct net_device *dev = wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_PRIMARY);
-
-	ret = wldev_iovar_getint(dev, "p2p_dev", index);
-	CFGP2P_INFO(("p2p_dev bsscfg_idx=%d ret=%d\n", *index, ret));
-
-	if (unlikely(ret <  0)) {
-	    CFGP2P_ERR(("'p2p_dev' error %d\n", ret));
-		return ret;
-	}
-	return ret;
-}
-
-s32
-wl_cfgp2p_init_discovery(struct wl_priv *wl)
-{
-
-	s32 index = 0;
-	s32 ret = BCME_OK;
-
-	CFGP2P_DBG(("enter\n"));
-
-	if (wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) != 0) {
-		CFGP2P_ERR(("do nothing, already initialized\n"));
-		return ret;
-	}
-
-	ret = wl_cfgp2p_set_discovery(wl, 1);
-	if (ret < 0) {
-		CFGP2P_ERR(("set discover error\n"));
-		return ret;
-	}
-	/* Enable P2P Discovery in the WL Driver */
-	ret = wl_cfgp2p_get_disc_idx(wl, &index);
-
-	if (ret < 0) {
-		return ret;
-	}
-	wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE) =
-	    wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_PRIMARY);
-	wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) = index;
-
-	/* Set the initial discovery state to SCAN */
-	ret = wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SCAN, 0, 0,
-	        wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
-
-	if (unlikely(ret != 0)) {
-		CFGP2P_ERR(("unable to set WL_P2P_DISC_ST_SCAN\n"));
-		wl_cfgp2p_set_discovery(wl, 0);
-		wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) = 0;
-		wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE) = NULL;
-		return 0;
-	}
-	return ret;
-}
-
-/* Deinitialize P2P Discovery
- * Parameters :
- * @wl        : wl_private data
- * Returns 0 if succes
- */
-s32
-wl_cfgp2p_deinit_discovery(struct wl_priv *wl)
-{
-	s32 ret = BCME_OK;
-	CFGP2P_DBG(("enter\n"));
-
-	if (wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) == 0) {
-		CFGP2P_ERR(("do nothing, not initialized\n"));
-		return -1;
-	}
-	/* Set the discovery state to SCAN */
-	ret = wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SCAN, 0, 0,
-	            wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
-	/* Disable P2P discovery in the WL driver (deletes the discovery BSSCFG) */
-	ret = wl_cfgp2p_set_discovery(wl, 0);
-
-	/* Clear our saved WPS and P2P IEs for the discovery BSS.  The driver
-	 * deleted these IEs when wl_cfgp2p_set_discovery() deleted the discovery
-	 * BSS.
-	 */
-
-	/* Clear the saved bsscfg index of the discovery BSSCFG to indicate we
-	 * have no discovery BSS.
-	 */
-	wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) = 0;
-	wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE) = NULL;
-
-	return ret;
-
-}
-/* Enable P2P Discovery
- * Parameters:
- * @wl	: wl_private data
- * @ie  : probe request ie (WPS IE + P2P IE)
- * @ie_len   : probe request ie length
- * Returns 0 if success.
- */
-s32
-wl_cfgp2p_enable_discovery(struct wl_priv *wl, struct net_device *dev, const u8 *ie, u32 ie_len)
-{
-	s32 ret = BCME_OK;
-	if (wl_get_p2p_status(wl, DISCOVERY_ON)) {
-		CFGP2P_INFO((" DISCOVERY is already initialized, we have nothing to do\n"));
-		goto set_ie;
-	}
-
-	wl_set_p2p_status(wl, DISCOVERY_ON);
-
-	CFGP2P_DBG(("enter\n"));
-
-	ret = wl_cfgp2p_init_discovery(wl);
-	if (unlikely(ret < 0)) {
-		CFGP2P_ERR((" init discovery error %d\n", ret));
-		goto exit;
-	}
-	/* Set wsec to any non-zero value in the discovery bsscfg to ensure our
-	 * P2P probe responses have the privacy bit set in the 802.11 WPA IE.
-	 * Some peer devices may not initiate WPS with us if this bit is not set.
-	 */
-	ret = wldev_iovar_setint_bsscfg(wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE),
-			"wsec", AES_ENABLED, wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
-	if (unlikely(ret < 0)) {
-		CFGP2P_ERR((" wsec error %d\n", ret));
-	}
-set_ie:
-	ret = wl_cfgp2p_set_management_ie(wl, dev,
-	            wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE),
-	            VNDR_IE_PRBREQ_FLAG, ie, ie_len);
-
-	if (unlikely(ret < 0)) {
-		CFGP2P_ERR(("set probreq ie occurs error %d\n", ret));
-		goto exit;
-	}
-exit:
-	return ret;
-}
-
-/* Disable P2P Discovery
- * Parameters:
- * @wl       : wl_private_data
- * Returns 0 if success.
- */
-s32
-wl_cfgp2p_disable_discovery(struct wl_priv *wl)
-{
-	s32 ret = BCME_OK;
-	CFGP2P_DBG((" enter\n"));
-	wl_clr_p2p_status(wl, DISCOVERY_ON);
-
-	if (wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) == 0) {
-		CFGP2P_ERR((" do nothing, not initialized\n"));
-		goto exit;
-	}
-
-	ret = wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SCAN, 0, 0,
-	            wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
-
-	if (unlikely(ret < 0)) {
-
-		CFGP2P_ERR(("unable to set WL_P2P_DISC_ST_SCAN\n"));
-	}
-	/* Do a scan abort to stop the driver's scan engine in case it is still
-	 * waiting out an action frame tx dwell time.
-	 */
-#ifdef NOT_YET
-	if (wl_get_p2p_status(wl, SCANNING)) {
-		p2pwlu_scan_abort(hdl, FALSE);
-	}
-#endif
-	wl_clr_p2p_status(wl, DISCOVERY_ON);
-	ret = wl_cfgp2p_deinit_discovery(wl);
-
-exit:
-	return ret;
-}
-
-s32
-wl_cfgp2p_escan(struct wl_priv *wl, struct net_device *dev, u16 active,
-	u32 num_chans, u16 *channels,
-	s32 search_state, u16 action, u32 bssidx)
-{
-	s32 ret = BCME_OK;
-	s32 memsize;
-	s32 eparams_size;
-	u32 i;
-	s8 *memblk;
-	wl_p2p_scan_t *p2p_params;
-	wl_escan_params_t *eparams;
-	wlc_ssid_t ssid;
-	/* Scan parameters */
-#define P2PAPI_SCAN_NPROBES 1
-#define P2PAPI_SCAN_DWELL_TIME_MS 40
-#define P2PAPI_SCAN_HOME_TIME_MS 10
-
-	wl_set_p2p_status(wl, SCANNING);
-	/* Allocate scan params which need space for 3 channels and 0 ssids */
-	eparams_size = (WL_SCAN_PARAMS_FIXED_SIZE +
-	    OFFSETOF(wl_escan_params_t, params)) +
-		num_chans * sizeof(eparams->params.channel_list[0]);
-
-	memsize = sizeof(wl_p2p_scan_t) + eparams_size;
-	memblk = scanparambuf;
-	if (memsize > sizeof(scanparambuf)) {
-		CFGP2P_ERR((" scanpar buf too small (%u > %u)\n",
-		    memsize, sizeof(scanparambuf)));
-		return -1;
-	}
-	memset(memblk, 0, memsize);
-	memset(ioctlbuf, 0, sizeof(ioctlbuf));
-	if (search_state == WL_P2P_DISC_ST_SEARCH) {
-		/*
-		 * If we in SEARCH STATE, we don't need to set SSID explictly
-		 * because dongle use P2P WILDCARD internally by default
-		 */
-		wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SEARCH, 0, 0, bssidx);
-		ssid.SSID_len = htod32(0);
-
-	} else if (search_state == WL_P2P_DISC_ST_SCAN) {
-		/* SCAN STATE 802.11 SCAN
-		 * WFD Supplicant has p2p_find command with (type=progressive, type= full)
-		 * So if P2P_find command with type=progressive,
-		 * we have to set ssid to P2P WILDCARD because
-		 * we just do broadcast scan unless setting SSID
-		 */
-		strcpy(ssid.SSID, WL_P2P_WILDCARD_SSID);
-		ssid.SSID_len = htod32(WL_P2P_WILDCARD_SSID_LEN);
-		wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SCAN, 0, 0, bssidx);
-	}
-
-
-	/* Fill in the P2P scan structure at the start of the iovar param block */
-	p2p_params = (wl_p2p_scan_t*) memblk;
-	p2p_params->type = 'E';
-	/* Fill in the Scan structure that follows the P2P scan structure */
-	eparams = (wl_escan_params_t*) (p2p_params + 1);
-	eparams->params.bss_type = DOT11_BSSTYPE_ANY;
-	if (active)
-		eparams->params.scan_type = DOT11_SCANTYPE_ACTIVE;
-	else
-		eparams->params.scan_type = DOT11_SCANTYPE_PASSIVE;
-
-	memcpy(&eparams->params.bssid, &ether_bcast, ETHER_ADDR_LEN);
-	if (ssid.SSID_len)
-		memcpy(&eparams->params.ssid, &ssid, sizeof(wlc_ssid_t));
-
-	eparams->params.nprobes = htod32(P2PAPI_SCAN_NPROBES);
-	eparams->params.home_time = htod32(P2PAPI_SCAN_HOME_TIME_MS);
-	eparams->params.active_time = htod32(-1);
-	eparams->params.passive_time = htod32(-1);
-	eparams->params.channel_num = htod32((0 << WL_SCAN_PARAMS_NSSID_SHIFT) |
-	    (num_chans & WL_SCAN_PARAMS_COUNT_MASK));
-
-	for (i = 0; i < num_chans; i++) {
-		eparams->params.channel_list[i] = htodchanspec(channels[i]);
-	}
-	eparams->version = htod32(ESCAN_REQ_VERSION);
-	eparams->action =  htod16(action);
-	eparams->sync_id = htod16(0x1234);
-	CFGP2P_INFO(("SCAN CHANNELS : "));
-
-	for (i = 0; i < num_chans; i++) {
-		if (i == 0) CFGP2P_INFO(("%d", channels[i]));
-		else CFGP2P_INFO((",%d", channels[i]));
-	}
-
-	CFGP2P_INFO(("\n"));
-
-	ret = wldev_iovar_setbuf_bsscfg(dev, "p2p_scan",
-	            memblk, memsize, smbuf, sizeof(ioctlbuf), bssidx);
-	return ret;
-}
-/* Check whether pointed-to IE looks like WPA. */
-#define wl_cfgp2p_is_wpa_ie(ie, tlvs, len)	wl_cfgp2p_has_ie(ie, tlvs, len, \
-		(const uint8 *)WPS_OUI, WPS_OUI_LEN, WPA_OUI_TYPE)
-/* Check whether pointed-to IE looks like WPS. */
-#define wl_cfgp2p_is_wps_ie(ie, tlvs, len)	wl_cfgp2p_has_ie(ie, tlvs, len, \
-		(const uint8 *)WPS_OUI, WPS_OUI_LEN, WPS_OUI_TYPE)
-/* Check whether the given IE looks like WFA P2P IE. */
-#define wl_cfgp2p_is_p2p_ie(ie, tlvs, len)	wl_cfgp2p_has_ie(ie, tlvs, len, \
-		(const uint8 *)WFA_OUI, WFA_OUI_LEN, WFA_OUI_TYPE_P2P)
-/* Delete and Set a management vndr ie to firmware
- * Parameters:
- * @wl       : wl_private data
- * @ndev     : net device for bssidx
- * @bssidx   : bssidx for BSS
- * @pktflag  : packet flag for IE (VNDR_IE_PRBREQ_FLAG,VNDR_IE_PRBRSP_FLAG, VNDR_IE_ASSOCRSP_FLAG,
- *                                 VNDR_IE_ASSOCREQ_FLAG)
- * @ie       :  VNDR IE (such as P2P IE , WPS IE)
- * @ie_len   : VNDR IE Length
- * Returns 0 if success.
- */
-
-s32
-wl_cfgp2p_set_management_ie(struct wl_priv *wl, struct net_device *ndev, s32 bssidx,
-    s32 pktflag, const u8 *vndr_ie, u32 vndr_ie_len)
-{
-	/* Vendor-specific Information Element ID */
-#define VNDR_SPEC_ELEMENT_ID 0xdd
-	s32 ret = BCME_OK;
-	u32 pos;
-	u8  *ie_buf;
-	u8  *mgmt_ie_buf;
-	u32 mgmt_ie_buf_len;
-	u32 *mgmt_ie_len;
-	u8 ie_id, ie_len;
-	u8 delete = 0;
-#define IE_TYPE(type, bsstype) (wl_to_p2p_bss_saved_ie(wl, bsstype).p2p_ ## type ## _ie)
-#define IE_TYPE_LEN(type, bsstype) (wl_to_p2p_bss_saved_ie(wl, bsstype).p2p_ ## type ## _ie_len)
-	if (bssidx == -1)
-		return BCME_BADARG;
-	if (wl->p2p_supported && p2p_on(wl)) {
-		if (bssidx == P2PAPI_BSSCFG_PRIMARY)
-			bssidx =  wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE);
-		switch (pktflag) {
-			case VNDR_IE_PRBREQ_FLAG :
-				mgmt_ie_buf = IE_TYPE(probe_req, bssidx);
-				mgmt_ie_len = &IE_TYPE_LEN(probe_req, bssidx);
-				mgmt_ie_buf_len = sizeof(IE_TYPE(probe_req, bssidx));
-				break;
-			case VNDR_IE_PRBRSP_FLAG :
-				mgmt_ie_buf = IE_TYPE(probe_res, bssidx);
-				mgmt_ie_len = &IE_TYPE_LEN(probe_res, bssidx);
-				mgmt_ie_buf_len = sizeof(IE_TYPE(probe_res, bssidx));
-				break;
-			case VNDR_IE_ASSOCREQ_FLAG :
-				mgmt_ie_buf = IE_TYPE(assoc_req, bssidx);
-				mgmt_ie_len = &IE_TYPE_LEN(assoc_req, bssidx);
-				mgmt_ie_buf_len = sizeof(IE_TYPE(assoc_req, bssidx));
-				break;
-			case VNDR_IE_ASSOCRSP_FLAG :
-				mgmt_ie_buf = IE_TYPE(assoc_res, bssidx);
-				mgmt_ie_len = &IE_TYPE_LEN(assoc_res, bssidx);
-				mgmt_ie_buf_len = sizeof(IE_TYPE(assoc_res, bssidx));
-				break;
-			case VNDR_IE_BEACON_FLAG :
-				mgmt_ie_buf = IE_TYPE(beacon, bssidx);
-				mgmt_ie_len = &IE_TYPE_LEN(beacon, bssidx);
-				mgmt_ie_buf_len = sizeof(IE_TYPE(beacon, bssidx));
-				break;
-			default:
-				mgmt_ie_buf = NULL;
-				mgmt_ie_len = NULL;
-				CFGP2P_ERR(("not suitable type\n"));
-				return -1;
-		}
-	} else {
-		switch (pktflag) {
-			case VNDR_IE_PRBRSP_FLAG :
-				mgmt_ie_buf = wl->ap_info->probe_res_ie;
-				mgmt_ie_len = &wl->ap_info->probe_res_ie_len;
-				mgmt_ie_buf_len = sizeof(wl->ap_info->probe_res_ie);
-				break;
-			case VNDR_IE_BEACON_FLAG :
-				mgmt_ie_buf = wl->ap_info->beacon_ie;
-				mgmt_ie_len = &wl->ap_info->beacon_ie_len;
-				mgmt_ie_buf_len = sizeof(wl->ap_info->beacon_ie);
-				break;
-			default:
-				mgmt_ie_buf = NULL;
-				mgmt_ie_len = NULL;
-				CFGP2P_ERR(("not suitable type\n"));
-				return -1;
-		}
-	}
-	/* Add if there is any extra IE */
-	if (vndr_ie && vndr_ie_len) {
-		CFGP2P_INFO(("Request has extra IE"));
-		if (vndr_ie_len > mgmt_ie_buf_len) {
-			CFGP2P_ERR(("extra IE size too big\n"));
-			ret = -ENOMEM;
-		} else {
-			if (mgmt_ie_buf != NULL) {
-				if ((vndr_ie_len == *mgmt_ie_len) &&
-				     (memcmp(mgmt_ie_buf, vndr_ie, vndr_ie_len) == 0)) {
-					CFGP2P_INFO(("Previous mgmt IE is equals to current IE"));
-					goto exit;
-				}
-				pos = 0;
-				delete = 1;
-				ie_buf = (u8 *) mgmt_ie_buf;
-				while (pos < *mgmt_ie_len) {
-					ie_id = ie_buf[pos++];
-					ie_len = ie_buf[pos++];
-					CFGP2P_INFO(("DELELED ID(%d), Len(%d),"
-						"OUI(%02x:%02x:%02x)\n",
-						ie_id, ie_len, ie_buf[pos],
-						ie_buf[pos+1], ie_buf[pos+2]));
-					ret = wl_cfgp2p_vndr_ie(ndev, bssidx, pktflag,
-					    ie_buf+pos, VNDR_SPEC_ELEMENT_ID,
-						ie_buf+pos+3, ie_len-3, delete);
-					pos += ie_len;
-				}
-
-			}
-			/* save the current IE in wl struct */
-			memcpy(mgmt_ie_buf, vndr_ie, vndr_ie_len);
-			*mgmt_ie_len = vndr_ie_len;
-			pos = 0;
-			ie_buf = (u8 *) vndr_ie;
-			delete = 0;
-			while (pos < vndr_ie_len) {
-				ie_id = ie_buf[pos++];
-				ie_len = ie_buf[pos++];
-				if ((ie_id == DOT11_MNG_VS_ID) &&
-				   (wl_cfgp2p_is_wps_ie(&ie_buf[pos-2], NULL, 0) ||
-					wl_cfgp2p_is_p2p_ie(&ie_buf[pos-2], NULL, 0))) {
-					CFGP2P_INFO(("ADDED ID : %d, Len : %d , OUI :"
-						"%02x:%02x:%02x\n", ie_id, ie_len, ie_buf[pos],
-						ie_buf[pos+1], ie_buf[pos+2]));
-					ret = wl_cfgp2p_vndr_ie(ndev, bssidx, pktflag, ie_buf+pos,
-					    VNDR_SPEC_ELEMENT_ID, ie_buf+pos+3, ie_len-3, delete);
-				}
-				pos += ie_len;
-			}
-		}
-
-	}
-#undef IE_TYPE
-#undef IE_TYPE_LEN
-exit:
-	return ret;
-}
-
-/* Clear the manament IE buffer of BSSCFG
- * Parameters:
- * @wl       : wl_private data
- * @bssidx   : bssidx for BSS
- *
- * Returns 0 if success.
- */
-s32
-wl_cfgp2p_clear_management_ie(struct wl_priv *wl, s32 bssidx)
-{
-#define INIT_IE(IE_TYPE, BSS_TYPE)		\
-	do {							\
-		memset(wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie, 0, \
-		   sizeof(wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie)); \
-		wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie_len = 0; \
-	} while (0);
-	if (bssidx < 0) {
-		CFGP2P_ERR(("invalid bssidx\n"));
-		return BCME_BADARG;
-	}
-	INIT_IE(probe_req, bssidx);
-	INIT_IE(probe_res, bssidx);
-	INIT_IE(assoc_req, bssidx);
-	INIT_IE(assoc_res, bssidx);
-	INIT_IE(beacon, bssidx);
-	return BCME_OK;
-}
-
-
-/* Is any of the tlvs the expected entry? If
- * not update the tlvs buffer pointer/length.
- */
-static bool
-wl_cfgp2p_has_ie(u8 *ie, u8 **tlvs, u32 *tlvs_len, const u8 *oui, u32 oui_len, u8 type)
-{
-	/* If the contents match the OUI and the type */
-	if (ie[TLV_LEN_OFF] >= oui_len + 1 &&
-	        !bcmp(&ie[TLV_BODY_OFF], oui, oui_len) &&
-	        type == ie[TLV_BODY_OFF + oui_len]) {
-		return TRUE;
-	}
-
-	if (tlvs == NULL)
-		return FALSE;
-	/* point to the next ie */
-	ie += ie[TLV_LEN_OFF] + TLV_HDR_LEN;
-	/* calculate the length of the rest of the buffer */
-	*tlvs_len -= (int)(ie - *tlvs);
-	/* update the pointer to the start of the buffer */
-	*tlvs = ie;
-
-	return FALSE;
-}
-
-wpa_ie_fixed_t *
-wl_cfgp2p_find_wpaie(u8 *parse, u32 len)
-{
-	bcm_tlv_t *ie;
-
-	while ((ie = bcm_parse_tlvs(parse, (u32)len, DOT11_MNG_VS_ID))) {
-		if (wl_cfgp2p_is_wpa_ie((u8*)ie, &parse, &len)) {
-			return (wpa_ie_fixed_t *)ie;
-		}
-	}
-	return NULL;
-}
-
-wpa_ie_fixed_t *
-wl_cfgp2p_find_wpsie(u8 *parse, u32 len)
-{
-	bcm_tlv_t *ie;
-
-	while ((ie = bcm_parse_tlvs(parse, (u32)len, DOT11_MNG_VS_ID))) {
-		if (wl_cfgp2p_is_wps_ie((u8*)ie, &parse, &len)) {
-			return (wpa_ie_fixed_t *)ie;
-		}
-	}
-	return NULL;
-}
-
-wifi_p2p_ie_t *
-wl_cfgp2p_find_p2pie(u8 *parse, u32 len)
-{
-	bcm_tlv_t *ie;
-
-	while ((ie = bcm_parse_tlvs(parse, (int)len, DOT11_MNG_VS_ID))) {
-		if (wl_cfgp2p_is_p2p_ie((uint8*)ie, &parse, &len)) {
-			return (wifi_p2p_ie_t *)ie;
-		}
-	}
-	return NULL;
-}
-
-static s32
-wl_cfgp2p_vndr_ie(struct net_device *ndev, s32 bssidx, s32 pktflag,
-            s8 *oui, s32 ie_id, s8 *data, s32 data_len, s32 delete)
-{
-	s32 err = BCME_OK;
-	s32 buf_len;
-	s32 iecount;
-
-	vndr_ie_setbuf_t *ie_setbuf;
-
-	/* Validate the pktflag parameter */
-	if ((pktflag & ~(VNDR_IE_BEACON_FLAG | VNDR_IE_PRBRSP_FLAG |
-	            VNDR_IE_ASSOCRSP_FLAG | VNDR_IE_AUTHRSP_FLAG |
-	            VNDR_IE_PRBREQ_FLAG | VNDR_IE_ASSOCREQ_FLAG))) {
-		CFGP2P_ERR(("p2pwl_vndr_ie: Invalid packet flag 0x%x\n", pktflag));
-		return -1;
-	}
-
-	buf_len = sizeof(vndr_ie_setbuf_t) + data_len - 1;
-	ie_setbuf = (vndr_ie_setbuf_t *) kzalloc(buf_len, GFP_KERNEL);
-
-	CFGP2P_INFO((" ie_id : %02x, data length : %d\n", ie_id, data_len));
-	if (!ie_setbuf) {
-
-		CFGP2P_ERR(("Error allocating buffer for IE\n"));
-		return -ENOMEM;
-	}
-	if (delete)
-		strcpy(ie_setbuf->cmd, "del");
-	else
-		strcpy(ie_setbuf->cmd, "add");
-	/* Buffer contains only 1 IE */
-	iecount = htod32(1);
-	memcpy((void *)&ie_setbuf->vndr_ie_buffer.iecount, &iecount, sizeof(int));
-	pktflag = htod32(pktflag);
-	memcpy((void *)&ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].pktflag,
-	    &pktflag, sizeof(uint32));
-	ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.id = ie_id;
-	ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.len
-	        = (uchar)(data_len + VNDR_IE_MIN_LEN);
-	memcpy(ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.oui, oui, 3);
-	memcpy(ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.data, data, data_len);
-	err = wldev_iovar_setbuf_bsscfg(ndev, "vndr_ie", ie_setbuf, buf_len,
-		ioctlbuf, sizeof(ioctlbuf), bssidx);
-
-	CFGP2P_INFO(("vndr_ie iovar returns %d\n", err));
-	kfree(ie_setbuf);
-	return err;
-}
-
-/*
- * Search the bssidx based on dev argument
- * Parameters:
- * @wl       : wl_private data
- * @ndev     : net device to search bssidx
- *  Returns bssidx for ndev
- */
-s32
-wl_cfgp2p_find_idx(struct wl_priv *wl, struct net_device *ndev)
-{
-	u32 i;
-	s32 index = -1;
-
-	if (ndev == NULL) {
-		CFGP2P_ERR((" ndev is NULL\n"));
-		goto exit;
-	}
-	if (!wl->p2p_supported) {
-		return P2PAPI_BSSCFG_PRIMARY;
-	}
-	for (i = 0; i < P2PAPI_BSSCFG_MAX; i++) {
-		if (ndev == wl_to_p2p_bss_ndev(wl, i)) {
-			index = wl_to_p2p_bss_bssidx(wl, i);
-			break;
-		}
-	}
-	if (index == -1)
-		return P2PAPI_BSSCFG_PRIMARY;
-exit:
-	return index;
-}
-/*
- * Callback function for WLC_E_P2P_DISC_LISTEN_COMPLETE
- */
-s32
-wl_cfgp2p_listen_complete(struct wl_priv *wl, struct net_device *ndev,
-            const wl_event_msg_t *e, void *data)
-{
-	s32 ret = BCME_OK;
-
-	CFGP2P_DBG((" Enter\n"));
-	if (wl_get_p2p_status(wl, LISTEN_EXPIRED) == 0) {
-		wl_set_p2p_status(wl, LISTEN_EXPIRED);
-
-		if (wl->p2p->listen_timer)
-			del_timer_sync(wl->p2p->listen_timer);
-
-		cfg80211_remain_on_channel_expired(ndev, wl->cache_cookie, &wl->remain_on_chan,
-		    wl->remain_on_chan_type, GFP_KERNEL);
-	}
-
-	return ret;
-
-}
-
-/*
- *  Timer expire callback function for LISTEN
- *  We can't report cfg80211_remain_on_channel_expired from Timer ISR context, 
- *  so lets do it from thread context.
- */
-static void
-wl_cfgp2p_listen_expired(unsigned long data)
-{
-	wl_event_msg_t msg;
-	struct wl_priv *wl = (struct wl_priv *) data;
-
-	CFGP2P_DBG((" Enter\n"));
-	msg.event_type =  hton32(WLC_E_P2P_DISC_LISTEN_COMPLETE);
-	wl_cfg80211_event(wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE), &msg, NULL);
-}
-
-/* 
- * Do a P2P Listen on the given channel for the given duration.
- * A listen consists of sitting idle and responding to P2P probe requests
- * with a P2P probe response.
- *
- * This fn assumes dongle p2p device discovery is already enabled.
- * Parameters   :
- * @wl          : wl_private data
- * @channel     : channel to listen
- * @duration_ms : the time (milli seconds) to wait
- */
-s32
-wl_cfgp2p_discover_listen(struct wl_priv *wl, s32 channel, u32 duration_ms)
-{
-#define INIT_TIMER(timer, func, duration, extra_delay)	\
-	do {                   \
-		init_timer(timer); \
-		timer->function = func; \
-		timer->expires = jiffies + msecs_to_jiffies(duration + extra_delay); \
-		timer->data = (unsigned long) wl; \
-		add_timer(timer); \
-	} while (0);
-
-	s32 ret = BCME_OK;
-	CFGP2P_DBG((" Enter Channel : %d, Duration : %d\n", channel, duration_ms));
-	if (unlikely(wl_get_p2p_status(wl, DISCOVERY_ON) == 0)) {
-
-		CFGP2P_ERR((" Discovery is not set, so we have noting to do\n"));
-
-		ret = BCME_NOTREADY;
-		goto exit;
-	}
-
-	wl_clr_p2p_status(wl, LISTEN_EXPIRED);
-
-	wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_LISTEN, channel, (u16) duration_ms,
-	            wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
-
-	if (wl->p2p->listen_timer)
-		del_timer_sync(wl->p2p->listen_timer);
-
-	wl->p2p->listen_timer = kmalloc(sizeof(struct timer_list), GFP_KERNEL);
-
-	if (wl->p2p->listen_timer == NULL) {
-		CFGP2P_ERR(("listen_timer allocation failed\n"));
-		return -ENOMEM;
-	}
-
-	/*  We will wait to receive WLC_E_P2P_DISC_LISTEN_COMPLETE from dongle , 
-	 *  otherwise we will wait up to duration_ms + 200ms
-	 */
-	INIT_TIMER(wl->p2p->listen_timer, wl_cfgp2p_listen_expired, duration_ms, 200);
-
-#undef INIT_TIMER
-exit:
-	return ret;
-}
-
-
-s32
-wl_cfgp2p_discover_enable_search(struct wl_priv *wl, u8 enable)
-{
-	s32 ret = BCME_OK;
-	CFGP2P_DBG((" Enter\n"));
-	if (!wl_get_p2p_status(wl, DISCOVERY_ON)) {
-
-		CFGP2P_DBG((" do nothing, discovery is off\n"));
-		return ret;
-	}
-	if (wl_get_p2p_status(wl, SEARCH_ENABLED) == enable) {
-		CFGP2P_DBG(("already : %d\n", enable));
-		return ret;
-	}
-
-	wl_chg_p2p_status(wl, SEARCH_ENABLED);
-	/* When disabling Search, reset the WL driver's p2p discovery state to
-	 * WL_P2P_DISC_ST_SCAN.
-	 */
-	if (!enable) {
-		wl_clr_p2p_status(wl, SCANNING);
-		ret = wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SCAN, 0, 0,
-		            wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
-	}
-
-	return ret;
-}
-
-/*
- * Callback function for WLC_E_ACTION_FRAME_COMPLETE, WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE
- */
-s32
-wl_cfgp2p_action_tx_complete(struct wl_priv *wl, struct net_device *ndev,
-            const wl_event_msg_t *e, void *data)
-{
-	s32 ret = BCME_OK;
-	u32 event_type = ntoh32(e->event_type);
-	u32 status = ntoh32(e->status);
-	CFGP2P_DBG((" Enter\n"));
-	if (event_type == WLC_E_ACTION_FRAME_COMPLETE) {
-
-		CFGP2P_INFO((" WLC_E_ACTION_FRAME_COMPLETE is received : %d\n", status));
-		if (status == WLC_E_STATUS_SUCCESS) {
-			wl_set_p2p_status(wl, ACTION_TX_COMPLETED);
-		}
-		else {
-			wl_set_p2p_status(wl, ACTION_TX_NOACK);
-			CFGP2P_ERR(("WLC_E_ACTION_FRAME_COMPLETE : NO ACK\n"));
-		}
-		wake_up_interruptible(&wl->dongle_event_wait);
-	} else {
-		CFGP2P_INFO((" WLC_E_ACTION_FRAME_OFFCHAN_COMPLETE is received,"
-					"status : %d\n", status));
-
-	}
-
-	return ret;
-}
-/* Send an action frame immediately without doing channel synchronization.
- *
- * This function does not wait for a completion event before returning.
- * The WLC_E_ACTION_FRAME_COMPLETE event will be received when the action
- * frame is transmitted.
- * The WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE event will be received when an
- * 802.11 ack has been received for the sent action frame.
- */
-s32
-wl_cfgp2p_tx_action_frame(struct wl_priv *wl, struct net_device *dev,
-	wl_af_params_t *af_params, s32 bssidx)
-{
-	s32 ret = BCME_OK;
-	s32 timeout = 0;
-
-
-	CFGP2P_INFO(("\n"));
-	CFGP2P_INFO(("channel : %u , dwell time : %u\n",
-	    af_params->channel, af_params->dwell_time));
-
-	wl_clr_p2p_status(wl, ACTION_TX_COMPLETED);
-	wl_clr_p2p_status(wl, ACTION_TX_NOACK);
-#define MAX_WAIT_TIME 2000
-	if (bssidx == P2PAPI_BSSCFG_PRIMARY)
-		bssidx =  wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE);
-
-	ret = wldev_iovar_setbuf_bsscfg(dev, "actframe",
-	           af_params, sizeof(*af_params), ioctlbuf, sizeof(ioctlbuf), bssidx);
-
-	if (ret < 0) {
-
-		CFGP2P_ERR((" sending action frame is failed\n"));
-		goto exit;
-	}
-	timeout = wait_event_interruptible_timeout(wl->dongle_event_wait,
-	(wl_get_p2p_status(wl, ACTION_TX_COMPLETED) ||wl_get_p2p_status(wl, ACTION_TX_NOACK)),
-	                    msecs_to_jiffies(MAX_WAIT_TIME));
-
-	if (timeout > 0 && wl_get_p2p_status(wl, ACTION_TX_COMPLETED)) {
-		CFGP2P_INFO(("tx action frame operation is completed\n"));
-		ret = BCME_OK;
-	} else {
-		ret = BCME_ERROR;
-		CFGP2P_INFO(("tx action frame operation is failed\n"));
-	}
-exit:
-	CFGP2P_INFO((" via act frame iovar : status = %d\n", ret));
-#undef MAX_WAIT_TIME
-	return ret;
-}
-
-/* Generate our P2P Device Address and P2P Interface Address from our primary
- * MAC address.
- */
-void
-wl_cfgp2p_generate_bss_mac(struct ether_addr *primary_addr,
-            struct ether_addr *out_dev_addr, struct ether_addr *out_int_addr)
-{
-	memset(out_dev_addr, 0, sizeof(*out_dev_addr));
-	memset(out_int_addr, 0, sizeof(*out_int_addr));
-
-	/* Generate the P2P Device Address.  This consists of the device's
-	 * primary MAC address with the locally administered bit set.
-	 */
-	memcpy(out_dev_addr, primary_addr, sizeof(*out_dev_addr));
-	out_dev_addr->octet[0] |= 0x02;
-
-	/* Generate the P2P Interface Address.  If the discovery and connection
-	 * BSSCFGs need to simultaneously co-exist, then this address must be
-	 * different from the P2P Device Address.
-	 */
-	memcpy(out_int_addr, out_dev_addr, sizeof(*out_int_addr));
-	out_int_addr->octet[4] ^= 0x80;
-
-}
-
-/* P2P IF Address change to Virtual Interface MAC Address */
-void
-wl_cfg80211_change_ifaddr(u8* buf, struct ether_addr *p2p_int_addr, u8 element_id)
-{
-	wifi_p2p_ie_t *ie = (wifi_p2p_ie_t*) buf;
-	u16 len = ie->len;
-	u8 *subel;
-	u8 subelt_id;
-	u16 subelt_len;
-	CFGP2P_DBG((" Enter\n"));
-
-	/* Point subel to the P2P IE's subelt field.
-	 * Subtract the preceding fields (id, len, OUI, oui_type) from the length.
-	 */
-	subel = ie->subelts;
-	len -= 4;	/* exclude OUI + OUI_TYPE */
-
-	while (len >= 3) {
-	/* attribute id */
-		subelt_id = *subel;
-		subel += 1;
-		len -= 1;
-
-		/* 2-byte little endian */
-		subelt_len = *subel++;
-		subelt_len |= *subel++ << 8;
-
-		len -= 2;
-		len -= subelt_len;	/* for the remaining subelt fields */
-
-		if (subelt_id == element_id) {
-			if (subelt_id == P2P_SEID_INTINTADDR) {
-				memcpy(subel, p2p_int_addr->octet, ETHER_ADDR_LEN);
-				CFGP2P_INFO(("Intended P2P Interface Address ATTR FOUND\n"));
-			} else if (subelt_id == P2P_SEID_DEV_ID) {
-				memcpy(subel, p2p_int_addr->octet, ETHER_ADDR_LEN);
-				CFGP2P_INFO(("Device ID ATTR FOUND\n"));
-			} else if (subelt_id == P2P_SEID_DEV_INFO) {
-				memcpy(subel, p2p_int_addr->octet, ETHER_ADDR_LEN);
-				CFGP2P_INFO(("Device INFO ATTR FOUND\n"));
-			} else if (subelt_id == P2P_SEID_GROUP_ID) {
-				memcpy(subel, p2p_int_addr->octet, ETHER_ADDR_LEN);
-				CFGP2P_INFO(("GROUP ID ATTR FOUND\n"));
-			}			return;
-		} else {
-			CFGP2P_DBG(("OTHER id : %d\n", subelt_id));
-		}
-		subel += subelt_len;
-	}
-}
-/*
- * Check if a BSS is up.
- * This is a common implementation called by most OSL implementations of
- * p2posl_bss_isup().  DO NOT call this function directly from the
- * common code -- call p2posl_bss_isup() instead to allow the OSL to
- * override the common implementation if necessary.
- */
-bool
-wl_cfgp2p_bss_isup(struct net_device *ndev, int bsscfg_idx)
-{
-	s32 result, val;
-	bool isup = false;
-	s8 getbuf[64];
-
-	/* Check if the BSS is up */
-	*(int*)getbuf = -1;
-	result = wldev_iovar_getbuf_bsscfg(ndev, "bss", &bsscfg_idx,
-	                    sizeof(bsscfg_idx), getbuf, sizeof(getbuf), 0);
-	if (result != 0) {
-		CFGP2P_ERR(("'wl bss -C %d' failed: %d\n", bsscfg_idx, result));
-		CFGP2P_ERR(("NOTE: this ioctl error is normal "
-					"when the BSS has not been created yet.\n"));
-	} else {
-		val = *(int*)getbuf;
-		val = dtoh32(val);
-		CFGP2P_INFO(("---wl bss -C %d   ==> %d\n", bsscfg_idx, val));
-		isup = (val ? TRUE : FALSE);
-	}
-	return isup;
-}
-
-
-/* Bring up or down a BSS */
-s32
-wl_cfgp2p_bss(struct net_device *ndev, s32 bsscfg_idx, s32 up)
-{
-	s32 ret = BCME_OK;
-	s32 val = up ? 1 : 0;
-
-	struct {
-		s32 cfg;
-		s32 val;
-	} bss_setbuf;
-
-	bss_setbuf.cfg = htod32(bsscfg_idx);
-	bss_setbuf.val = htod32(val);
-	CFGP2P_INFO(("---wl bss -C %d %s\n", bsscfg_idx, up ? "up" : "down"));
-	ret = wldev_iovar_setbuf(ndev, "bss", &bss_setbuf, sizeof(bss_setbuf),
-		ioctlbuf, sizeof(ioctlbuf));
-
-	if (ret != 0) {
-		CFGP2P_ERR(("'bss %d' failed with %d\n", up, ret));
-	}
-
-	return ret;
-}
-
-/* Check if 'p2p' is supported in the driver */
-s32
-wl_cfgp2p_supported(struct wl_priv *wl, struct net_device *ndev)
-{
-	s32 ret = BCME_OK;
-	s32 p2p_supported = 0;
-	ret = wldev_iovar_getint(ndev, "p2p",
-	               &p2p_supported);
-	if (ret < 0) {
-		CFGP2P_ERR(("wl p2p error %d\n", ret));
-		return 0;
-	}
-	if (p2p_supported == 1) {
-		CFGP2P_INFO(("p2p is supported\n"));
-	} else {
-		CFGP2P_INFO(("p2p is unsupported\n"));
-		p2p_supported = 0;
-	}
-	return p2p_supported;
-}
-/* Cleanup P2P resources */
-s32
-wl_cfgp2p_down(struct wl_priv *wl)
-{
-	if (wl->p2p->listen_timer)
-		del_timer_sync(wl->p2p->listen_timer);
-	return 0;
-}
diff --git a/drivers/net/wireless/bcmdhd/wl_cfgp2p.h b/drivers/net/wireless/bcmdhd/wl_cfgp2p.h
deleted file mode 100644
index b08504d8f95..00000000000
--- a/drivers/net/wireless/bcmdhd/wl_cfgp2p.h
+++ /dev/null
@@ -1,235 +0,0 @@
-/*
- * Linux cfgp2p driver
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: wl_cfgp2p.h,v 1.1.4.1.2.8 2011/02/09 01:37:52 Exp $
- */
-#ifndef _wl_cfgp2p_h_
-#define _wl_cfgp2p_h_
-#include <proto/802.11.h>
-#include <proto/p2p.h>
-
-struct wl_priv;
-extern u32 wl_dbg_level;
-
-/* Enumeration of the usages of the BSSCFGs used by the P2P Library.  Do not
- * confuse this with a bsscfg index.  This value is an index into the
- * saved_ie[] array of structures which in turn contains a bsscfg index field.
- */
-typedef enum {
-	P2PAPI_BSSCFG_PRIMARY, /* maps to driver's primary bsscfg */
-	P2PAPI_BSSCFG_DEVICE, /* maps to driver's P2P device discovery bsscfg */
-	P2PAPI_BSSCFG_CONNECTION, /* maps to driver's P2P connection bsscfg */
-	P2PAPI_BSSCFG_MAX
-} p2p_bsscfg_type_t;
-
-#define IE_MAX_LEN 300
-/* Structure to hold all saved P2P and WPS IEs for a BSSCFG */
-struct p2p_saved_ie {
-	u8   p2p_probe_req_ie[IE_MAX_LEN];
-	u8   p2p_probe_res_ie[IE_MAX_LEN];
-	u8   p2p_assoc_req_ie[IE_MAX_LEN];
-	u8   p2p_assoc_res_ie[IE_MAX_LEN];
-	u8   p2p_beacon_ie[IE_MAX_LEN];
-	u32 p2p_probe_req_ie_len;
-	u32 p2p_probe_res_ie_len;
-	u32 p2p_assoc_req_ie_len;
-	u32 p2p_assoc_res_ie_len;
-	u32 p2p_beacon_ie_len;
-};
-
-struct p2p_bss {
-	u32 bssidx;
-	struct net_device *dev;
-	struct p2p_saved_ie saved_ie;
-	void *private_data;
-};
-
-struct p2p_info {
-	bool on;    /* p2p on/off switch */
-	bool scan;
-	bool vif_created;
-	s8 vir_ifname[IFNAMSIZ];
-	unsigned long status;
-	struct ether_addr dev_addr;
-	struct ether_addr int_addr;
-	struct p2p_bss bss_idx[P2PAPI_BSSCFG_MAX];
-	struct timer_list *listen_timer;
-	wlc_ssid_t ssid;
-};
-
-/* dongle status */
-enum wl_cfgp2p_status {
-	WLP2P_STATUS_DISCOVERY_ON = 0,
-	WLP2P_STATUS_SEARCH_ENABLED,
-	WLP2P_STATUS_IF_ADD,
-	WLP2P_STATUS_IF_DEL,
-	WLP2P_STATUS_IF_DELETING,
-	WLP2P_STATUS_IF_CHANGING,
-	WLP2P_STATUS_IF_CHANGED,
-	WLP2P_STATUS_LISTEN_EXPIRED,
-	WLP2P_STATUS_ACTION_TX_COMPLETED,
-	WLP2P_STATUS_ACTION_TX_NOACK,
-	WLP2P_STATUS_SCANNING
-};
-
-
-#define wl_to_p2p_bss_ndev(w, type) 	((wl)->p2p->bss_idx[type].dev)
-#define wl_to_p2p_bss_bssidx(w, type) 	((wl)->p2p->bss_idx[type].bssidx)
-#define wl_to_p2p_bss_saved_ie(w, type) 	((wl)->p2p->bss_idx[type].saved_ie)
-#define wl_to_p2p_bss_private(w, type)  ((wl)->p2p->bss_idx[type].private_data)
-#define wl_to_p2p_bss(wl, type) ((wl)->p2p->bss_idx[type])
-#define wl_get_p2p_status(wl, stat) ((!(wl)->p2p_supported) ? 0 : test_bit(WLP2P_STATUS_ ## stat, \
-									&(wl)->p2p->status))
-#define wl_set_p2p_status(wl, stat) ((!(wl)->p2p_supported) ? : set_bit(WLP2P_STATUS_ ## stat, \
-									&(wl)->p2p->status))
-#define wl_clr_p2p_status(wl, stat) ((!(wl)->p2p_supported) ? : clear_bit(WLP2P_STATUS_ ## stat, \
-									&(wl)->p2p->status))
-#define wl_chg_p2p_status(wl, stat) ((!(wl)->p2p_supported) ? : change_bit(WLP2P_STATUS_ ## stat, \
-									&(wl)->p2p->status))
-#define p2p_on(wl) ((wl)->p2p->on)
-#define p2p_scan(wl) ((wl)->p2p->scan)
-
-
-/* dword align allocation */
-#define WLC_IOCTL_MAXLEN 8192
-#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
-#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
-
-#define CFGP2P_ERR(args)									\
-	do {										\
-		if (wl_dbg_level & WL_DBG_ERR) {				\
-			printk(KERN_ERR "CFGP2P-ERROR) %s : ", __func__);	\
-			printk args;						\
-		}									\
-	} while (0)
-#define	CFGP2P_INFO(args)									\
-	do {										\
-		if (wl_dbg_level & WL_DBG_INFO) {				\
-			printk(KERN_ERR "CFGP2P-INFO) %s : ", __func__);	\
-			printk args;						\
-		}									\
-	} while (0)
-#define	CFGP2P_DBG(args)								\
-	do {									\
-		if (wl_dbg_level & WL_DBG_DBG) {			\
-			printk(KERN_ERR "CFGP2P-DEBUG) %s :", __func__);	\
-			printk args;							\
-		}									\
-	} while (0)
-
-
-extern s32
-wl_cfgp2p_init_priv(struct wl_priv *wl);
-extern void
-wl_cfgp2p_deinit_priv(struct wl_priv *wl);
-extern s32
-wl_cfgp2p_set_firm_p2p(struct wl_priv *wl);
-extern s32
-wl_cfgp2p_set_p2p_mode(struct wl_priv *wl, u8 mode,
-            u32 channel, u16 listen_ms, int bssidx);
-extern s32
-wl_cfgp2p_ifadd(struct wl_priv *wl, struct ether_addr *mac, u8 if_type,
-            chanspec_t chspec);
-extern s32
-wl_cfgp2p_ifdel(struct wl_priv *wl, struct ether_addr *mac);
-extern s32
-wl_cfgp2p_ifchange(struct wl_priv *wl, struct ether_addr *mac, u8 if_type, chanspec_t chspec);
-
-extern s32
-wl_cfgp2p_ifidx(struct wl_priv *wl, struct ether_addr *mac, s32 *index);
-
-extern s32
-wl_cfgp2p_init_discovery(struct wl_priv *wl);
-extern s32
-wl_cfgp2p_enable_discovery(struct wl_priv *wl, struct net_device *dev, const u8 *ie, u32 ie_len);
-extern s32
-wl_cfgp2p_disable_discovery(struct wl_priv *wl);
-extern s32
-wl_cfgp2p_escan(struct wl_priv *wl, struct net_device *dev, u16 active, u32 num_chans,
-	u16 *channels,
-	s32 search_state, u16 action, u32 bssidx);
-
-extern wpa_ie_fixed_t *
-wl_cfgp2p_find_wpaie(u8 *parse, u32 len);
-
-extern wpa_ie_fixed_t *
-wl_cfgp2p_find_wpsie(u8 *parse, u32 len);
-
-extern wifi_p2p_ie_t *
-wl_cfgp2p_find_p2pie(u8 *parse, u32 len);
-
-extern s32
-wl_cfgp2p_set_management_ie(struct wl_priv *wl, struct net_device *ndev, s32 bssidx,
-            s32 pktflag, const u8 *vndr_ie, u32 vndr_ie_len);
-extern s32
-wl_cfgp2p_clear_management_ie(struct wl_priv *wl, s32 bssidx);
-
-extern s32
-wl_cfgp2p_find_idx(struct wl_priv *wl, struct net_device *ndev);
-
-
-extern s32
-wl_cfgp2p_listen_complete(struct wl_priv *wl, struct net_device *ndev,
-            const wl_event_msg_t *e, void *data);
-extern s32
-wl_cfgp2p_discover_listen(struct wl_priv *wl, s32 channel, u32 duration_ms);
-
-extern s32
-wl_cfgp2p_discover_enable_search(struct wl_priv *wl, u8 enable);
-
-extern s32
-wl_cfgp2p_action_tx_complete(struct wl_priv *wl, struct net_device *ndev,
-            const wl_event_msg_t *e, void *data);
-extern s32
-wl_cfgp2p_tx_action_frame(struct wl_priv *wl, struct net_device *dev,
-	wl_af_params_t *af_params, s32 bssidx);
-
-extern void
-wl_cfgp2p_generate_bss_mac(struct ether_addr *primary_addr, struct ether_addr *out_dev_addr,
-            struct ether_addr *out_int_addr);
-
-extern void
-wl_cfg80211_change_ifaddr(u8* buf, struct ether_addr *p2p_int_addr, u8 element_id);
-extern bool
-wl_cfgp2p_bss_isup(struct net_device *ndev, int bsscfg_idx);
-
-extern s32
-wl_cfgp2p_bss(struct net_device *ndev, s32 bsscfg_idx, s32 up);
-
-
-extern s32
-wl_cfgp2p_supported(struct wl_priv *wl, struct net_device *ndev);
-
-extern s32
-wl_cfgp2p_down(struct wl_priv *wl);
-
-/* WiFi Direct */
-#define SOCIAL_CHAN_1 1
-#define SOCIAL_CHAN_2 6
-#define SOCIAL_CHAN_3 11
-#define WL_P2P_WILDCARD_SSID "DIRECT-"
-#define WL_P2P_WILDCARD_SSID_LEN 7
-#define WL_P2P_INTERFACE_PREFIX "p2p"
-#define WL_P2P_TEMP_CHAN "11"
-#define IS_P2P_SSID(ssid) (memcmp(ssid, WL_P2P_WILDCARD_SSID, WL_P2P_WILDCARD_SSID_LEN) == 0)
-#endif				/* _wl_cfgp2p_h_ */
diff --git a/drivers/net/wireless/bcmdhd/wl_iw.c b/drivers/net/wireless/bcmdhd/wl_iw.c
deleted file mode 100644
index 6d546fcd396..00000000000
--- a/drivers/net/wireless/bcmdhd/wl_iw.c
+++ /dev/null
@@ -1,8631 +0,0 @@
-/*
- * Linux Wireless Extensions support
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: wl_iw.c,v 1.132.2.18 2011-02-05 01:44:47 Exp $
- */
-
-#include <wlioctl.h>
-
-#include <typedefs.h>
-#include <linuxver.h>
-#include <osl.h>
-
-#include <bcmutils.h>
-#include <bcmendian.h>
-#include <proto/ethernet.h>
-
-#include <linux/if_arp.h>
-#include <asm/uaccess.h>
-
-#include <dngl_stats.h>
-#include <dhd.h>
-#include <dhdioctl.h>
-
-typedef void wlc_info_t;
-typedef void wl_info_t;
-typedef const struct si_pub  si_t;
-#include <wlioctl.h>
-
-#include <proto/ethernet.h>
-#include <dngl_stats.h>
-#include <dhd.h>
-#define WL_ERROR(x) printf x
-#define WL_TRACE(x)
-#define WL_ASSOC(x)
-#define WL_INFORM(x)
-#define WL_WSEC(x)
-#define WL_SCAN(x)
-
-
-#ifdef PNO_SET_DEBUG
-#define WL_PNO(x)	printf x
-#else
-#define WL_PNO(x)
-#endif
-
-
-#define JF2MS ((((jiffies / HZ) * 1000) + ((jiffies % HZ) * 1000) / HZ))
-
-#ifdef COEX_DBG       
-#define WL_TRACE_COEX(x) printf("TS:%lu ", JF2MS); \
-							printf x
-#else
-#define WL_TRACE_COEX(x)
-#endif
-
-#ifdef SCAN_DBG        
-#define WL_TRACE_SCAN(x) printf("TS:%lu ", JF2MS); \
-							printf x
-#else
-#define WL_TRACE_SCAN(x)
-#endif
-
-
-#include <wl_iw.h>
-
-
-
-
-#define IW_WSEC_ENABLED(wsec)	((wsec) & (WEP_ENABLED | TKIP_ENABLED | AES_ENABLED))
-
-#include <linux/rtnetlink.h>
-
-#define WL_IW_USE_ISCAN  1
-#define ENABLE_ACTIVE_PASSIVE_SCAN_SUPPRESS  1
-
-#ifdef OEM_CHROMIUMOS
-bool g_set_essid_before_scan = TRUE;
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
-	struct mutex  g_wl_ss_scan_lock; 
-#endif 
-
-#if defined(SOFTAP)
-#define WL_SOFTAP(x)
-static struct net_device *priv_dev;
-extern bool ap_cfg_running;
-extern bool ap_fw_loaded;
-struct net_device *ap_net_dev = NULL;
-tsk_ctl_t ap_eth_ctl;
-static int wl_iw_set_ap_security(struct net_device *dev, struct ap_profile *ap);
-static int wl_iw_softap_deassoc_stations(struct net_device *dev, u8 *mac);
-#endif 
-
-
-#define WL_IW_IOCTL_CALL(func_call) \
-	do {				\
-		func_call;		\
-	} while (0)
-
-#define RETURN_IF_EXTRA_NULL(extra) \
-	if (!extra) { \
-		WL_ERROR(("%s: error : extra is null pointer\n", __FUNCTION__)); \
-		return -EINVAL; \
-	}
-
-static int		g_onoff = G_WLAN_SET_ON;
-wl_iw_extra_params_t	g_wl_iw_params;
-
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
-
-static struct mutex	wl_cache_lock;
-static struct mutex	wl_softap_lock;
-
-#define DHD_OS_MUTEX_INIT(a) mutex_init(a)
-#define DHD_OS_MUTEX_LOCK(a) mutex_lock(a)
-#define DHD_OS_MUTEX_UNLOCK(a) mutex_unlock(a)
-
-#else
-
-#define DHD_OS_MUTEX_INIT(a)
-#define DHD_OS_MUTEX_LOCK(a)
-#define DHD_OS_MUTEX_UNLOCK(a)
-
-#endif 
-
-#include <bcmsdbus.h>
-extern void dhd_customer_gpio_wlan_ctrl(int onoff);
-extern uint dhd_dev_reset(struct net_device *dev, uint8 flag);
-extern void dhd_dev_init_ioctl(struct net_device *dev);
-
-uint wl_msg_level = WL_ERROR_VAL;
-
-#define MAX_WLIW_IOCTL_LEN 1024
-
-
-#define htod32(i) i
-#define htod16(i) i
-#define dtoh32(i) i
-#define dtoh16(i) i
-#define htodchanspec(i) i
-#define dtohchanspec(i) i
-
-#ifdef CONFIG_WIRELESS_EXT
-
-extern struct iw_statistics *dhd_get_wireless_stats(struct net_device *dev);
-extern int dhd_wait_pend8021x(struct net_device *dev);
-#endif 
-
-#if WIRELESS_EXT < 19
-#define IW_IOCTL_IDX(cmd)	((cmd) - SIOCIWFIRST)
-#define IW_EVENT_IDX(cmd)	((cmd) - IWEVFIRST)
-#endif 
-
-static void *g_scan = NULL;
-static volatile uint g_scan_specified_ssid;	
-static wlc_ssid_t g_specific_ssid;		
-
-static wlc_ssid_t g_ssid;
-
-bool btcoex_is_sco_active(struct net_device *dev);  
-static wl_iw_ss_cache_ctrl_t g_ss_cache_ctrl;	
-#if defined(CONFIG_FIRST_SCAN)
-static volatile uint g_first_broadcast_scan;	
-static volatile uint g_first_counter_scans;
-#define MAX_ALLOWED_BLOCK_SCAN_FROM_FIRST_SCAN 3
-#endif 
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
-#define DAEMONIZE(a) daemonize(a); \
-	allow_signal(SIGKILL); \
-	allow_signal(SIGTERM);
-#else 
-#define RAISE_RX_SOFTIRQ() \
-	cpu_raise_softirq(smp_processor_id(), NET_RX_SOFTIRQ)
-#define DAEMONIZE(a) daemonize(); \
-	do { if (a) \
-		strncpy(current->comm, a, MIN(sizeof(current->comm), (strlen(a) + 1))); \
-	} while (0);
-#endif 
-
-#if defined(WL_IW_USE_ISCAN)
-#if  !defined(CSCAN)
-static void wl_iw_free_ss_cache(void);
-static int   wl_iw_run_ss_cache_timer(int kick_off);
-#endif 
-#if defined(CONFIG_FIRST_SCAN)
-int  wl_iw_iscan_set_scan_broadcast_prep(struct net_device *dev, uint flag);
-#endif 
-static int dev_wlc_bufvar_set(struct net_device *dev, char *name, char *buf, int len);
-#define ISCAN_STATE_IDLE   0
-#define ISCAN_STATE_SCANING 1
-
-
-#define WLC_IW_ISCAN_MAXLEN   2048
-typedef struct iscan_buf {
-	struct iscan_buf * next;
-	char   iscan_buf[WLC_IW_ISCAN_MAXLEN];
-} iscan_buf_t;
-
-typedef struct iscan_info {
-	struct net_device *dev;
-	struct timer_list timer;
-	uint32 timer_ms;
-	uint32 timer_on;
-	int    iscan_state;
-	iscan_buf_t * list_hdr;
-	iscan_buf_t * list_cur;
-
-	
-	tsk_ctl_t tsk_ctl;
-
-	uint32 scan_flag;	
-#if defined CSCAN
-	char ioctlbuf[WLC_IOCTL_MEDLEN];
-#else
-	char ioctlbuf[WLC_IOCTL_SMLEN];
-#endif 
-	
-	wl_iscan_params_t *iscan_ex_params_p;
-	int iscan_ex_param_size;
-} iscan_info_t;
-
-
-
-#define  COEX_DHCP 1	
-#ifdef COEX_DHCP
-
-#define BT_DHCP_eSCO_FIX 
-#define BT_DHCP_USE_FLAGS  
-#define BT_DHCP_OPPORTUNITY_WINDOW_TIME	 2500 
-#define BT_DHCP_FLAG_FORCE_TIME 5500 
-
-
-
-static int wl_iw_set_btcoex_dhcp(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-);
-
-static void wl_iw_bt_flag_set(struct net_device *dev, bool set);
-static void wl_iw_bt_release(void);
-
-typedef enum bt_coex_status {
-	BT_DHCP_IDLE = 0,
-	BT_DHCP_START,
-	BT_DHCP_OPPORTUNITY_WINDOW,
-	BT_DHCP_FLAG_FORCE_TIMEOUT
-} coex_status_t;
-
-
-typedef struct bt_info {
-	struct net_device *dev;
-	struct timer_list timer;
-	uint32 timer_ms;
-	uint32 timer_on;
-	uint32 ts_dhcp_start; 
-	uint32 ts_dhcp_ok;    
-	bool	dhcp_done; 
-	int	bt_state;
-
-	
-	tsk_ctl_t tsk_ctl;
-
-} bt_info_t;
-
-bt_info_t *g_bt = NULL;
-static void wl_iw_bt_timerfunc(ulong data);
-#endif 
-iscan_info_t *g_iscan = NULL;
-void dhd_print_buf(void *pbuf, int len, int bytes_per_line);
-static void wl_iw_timerfunc(ulong data);
-static void wl_iw_set_event_mask(struct net_device *dev);
-static int
-wl_iw_iscan(iscan_info_t *iscan, wlc_ssid_t *ssid, uint16 action);
-#endif 
-
-static int
-wl_iw_set_scan(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-);
-
-#ifndef CSCAN
-static int
-wl_iw_get_scan(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-);
-
-static uint
-wl_iw_get_scan_prep(
-	wl_scan_results_t *list,
-	struct iw_request_info *info,
-	char *extra,
-	short max_size
-);
-#endif 
-
-static void
-swap_key_from_BE(
-	wl_wsec_key_t *key
-)
-{
-	key->index = htod32(key->index);
-	key->len = htod32(key->len);
-	key->algo = htod32(key->algo);
-	key->flags = htod32(key->flags);
-	key->rxiv.hi = htod32(key->rxiv.hi);
-	key->rxiv.lo = htod16(key->rxiv.lo);
-	key->iv_initialized = htod32(key->iv_initialized);
-}
-
-static void
-swap_key_to_BE(
-	wl_wsec_key_t *key
-)
-{
-	key->index = dtoh32(key->index);
-	key->len = dtoh32(key->len);
-	key->algo = dtoh32(key->algo);
-	key->flags = dtoh32(key->flags);
-	key->rxiv.hi = dtoh32(key->rxiv.hi);
-	key->rxiv.lo = dtoh16(key->rxiv.lo);
-	key->iv_initialized = dtoh32(key->iv_initialized);
-}
-
-static int
-dev_wlc_ioctl(
-	struct net_device *dev,
-	int cmd,
-	void *arg,
-	int len
-)
-{
-	struct ifreq ifr;
-	wl_ioctl_t ioc;
-	mm_segment_t fs;
-	int ret = -EINVAL;
-
-	if (!dev) {
-		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
-		return ret;
-	}
-
-	net_os_wake_lock(dev);
-
-	WL_INFORM(("%s, PID:%x: send Local IOCTL -> dhd: cmd:0x%x, buf:%p, len:%d ,\n",
-		__FUNCTION__, current->pid, cmd, arg, len));
-
-	if (g_onoff == G_WLAN_SET_ON) {
-		memset(&ioc, 0, sizeof(ioc));
-		ioc.cmd = cmd;
-		ioc.buf = arg;
-		ioc.len = len;
-
-		strcpy(ifr.ifr_name, dev->name);
-		ifr.ifr_data = (caddr_t) &ioc;
-
-		
-		ret = dev_open(dev);
-		if (ret) {
-			WL_ERROR(("%s: Error dev_open: %d\n", __func__, ret));
-			net_os_wake_unlock(dev);
-			return ret;
-		}
-
-		fs = get_fs();
-		set_fs(get_ds());
-#if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 31)
-		ret = dev->do_ioctl(dev, &ifr, SIOCDEVPRIVATE);
-#else
-		ret = dev->netdev_ops->ndo_do_ioctl(dev, &ifr, SIOCDEVPRIVATE);
-#endif 
-		set_fs(fs);
-	}
-	else {
-		WL_TRACE(("%s: call after driver stop : ignored\n", __FUNCTION__));
-	}
-
-	net_os_wake_unlock(dev);
-
-	return ret;
-}
-
-
-static int
-dev_wlc_intvar_get_reg(
-	struct net_device *dev,
-	char *name,
-	uint  reg,
-	int *retval)
-{
-	union {
-		char buf[WLC_IOCTL_SMLEN];
-		int val;
-	} var;
-	int error;
-
-	uint len;
-	len = bcm_mkiovar(name, (char *)(&reg), sizeof(reg), (char *)(&var), sizeof(var.buf));
-	ASSERT(len);
-	error = dev_wlc_ioctl(dev, WLC_GET_VAR, (void *)&var, len);
-
-	*retval = dtoh32(var.val);
-	return (error);
-}
-
-
-static int
-dev_wlc_intvar_set_reg(
-	struct net_device *dev,
-	char *name,
-	char *addr,
-	char * val)
-{
-	char reg_addr[8];
-
-	memset(reg_addr, 0, sizeof(reg_addr));
-	memcpy((char *)&reg_addr[0], (char *)addr, 4);
-	memcpy((char *)&reg_addr[4], (char *)val, 4);
-
-	return (dev_wlc_bufvar_set(dev, name,  (char *)&reg_addr[0], sizeof(reg_addr)));
-}
-
-
-
-
-static int
-dev_wlc_intvar_set(
-	struct net_device *dev,
-	char *name,
-	int val)
-{
-	char buf[WLC_IOCTL_SMLEN];
-	uint len;
-
-	val = htod32(val);
-	len = bcm_mkiovar(name, (char *)(&val), sizeof(val), buf, sizeof(buf));
-	ASSERT(len);
-
-	return (dev_wlc_ioctl(dev, WLC_SET_VAR, buf, len));
-}
-
-#if defined(WL_IW_USE_ISCAN)
-static int
-dev_iw_iovar_setbuf(
-	struct net_device *dev,
-	char *iovar,
-	void *param,
-	int paramlen,
-	void *bufptr,
-	int buflen)
-{
-	int iolen;
-
-	iolen = bcm_mkiovar(iovar, param, paramlen, bufptr, buflen);
-	ASSERT(iolen);
-
-	if (iolen == 0)
-		return 0;
-
-	return (dev_wlc_ioctl(dev, WLC_SET_VAR, bufptr, iolen));
-}
-
-static int
-dev_iw_iovar_getbuf(
-	struct net_device *dev,
-	char *iovar,
-	void *param,
-	int paramlen,
-	void *bufptr,
-	int buflen)
-{
-	int iolen;
-
-	iolen = bcm_mkiovar(iovar, param, paramlen, bufptr, buflen);
-	ASSERT(iolen);
-
-	return (dev_wlc_ioctl(dev, WLC_GET_VAR, bufptr, buflen));
-}
-#endif 
-
-
-#if WIRELESS_EXT > 17
-static int
-dev_wlc_bufvar_set(
-	struct net_device *dev,
-	char *name,
-	char *buf, int len)
-{
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31)
-	char ioctlbuf[MAX_WLIW_IOCTL_LEN];
-#else
-	static char ioctlbuf[MAX_WLIW_IOCTL_LEN];
-#endif 
-	uint buflen;
-
-	buflen = bcm_mkiovar(name, buf, len, ioctlbuf, sizeof(ioctlbuf));
-	ASSERT(buflen);
-
-	return (dev_wlc_ioctl(dev, WLC_SET_VAR, ioctlbuf, buflen));
-}
-#endif 
-
-
-static int
-dev_wlc_bufvar_get(
-	struct net_device *dev,
-	char *name,
-	char *buf, int buflen)
-{
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31)
-	char ioctlbuf[MAX_WLIW_IOCTL_LEN];
-#else
-	static char ioctlbuf[MAX_WLIW_IOCTL_LEN];
-#endif 
-	int error;
-	uint len;
-
-	len = bcm_mkiovar(name, NULL, 0, ioctlbuf, sizeof(ioctlbuf));
-	ASSERT(len);
-	error = dev_wlc_ioctl(dev, WLC_GET_VAR, (void *)ioctlbuf, MAX_WLIW_IOCTL_LEN);
-	if (!error)
-		bcopy(ioctlbuf, buf, buflen);
-
-	return (error);
-}
-
-
-
-static int
-dev_wlc_intvar_get(
-	struct net_device *dev,
-	char *name,
-	int *retval)
-{
-	union {
-		char buf[WLC_IOCTL_SMLEN];
-		int val;
-	} var;
-	int error;
-
-	uint len;
-	uint data_null;
-
-	len = bcm_mkiovar(name, (char *)(&data_null), 0, (char *)(&var), sizeof(var.buf));
-	ASSERT(len);
-	error = dev_wlc_ioctl(dev, WLC_GET_VAR, (void *)&var, len);
-
-	*retval = dtoh32(var.val);
-
-	return (error);
-}
-
-
-#if WIRELESS_EXT > 12
-static int
-wl_iw_set_active_scan(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int as = 0;
-	int error = 0;
-	char *p = extra;
-
-#if defined(WL_IW_USE_ISCAN)
-	if (g_iscan->iscan_state == ISCAN_STATE_IDLE)
-#endif 
-		error = dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &as, sizeof(as));
-#if defined(WL_IW_USE_ISCAN)
-	else
-		g_iscan->scan_flag = as;
-#endif 
-	p += snprintf(p, MAX_WX_STRING, "OK");
-
-	wrqu->data.length = p - extra + 1;
-	return error;
-}
-
-static int
-wl_iw_set_passive_scan(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int ps = 1;
-	int error = 0;
-	char *p = extra;
-
-#if defined(WL_IW_USE_ISCAN)
-	if (g_iscan->iscan_state == ISCAN_STATE_IDLE) {
-#endif 
-
-		 
-		if (g_scan_specified_ssid == 0) {
-			error = dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &ps, sizeof(ps));
-		}
-#if defined(WL_IW_USE_ISCAN)
-	}
-	else
-		g_iscan->scan_flag = ps;
-#endif 
-
-	p += snprintf(p, MAX_WX_STRING, "OK");
-
-	wrqu->data.length = p - extra + 1;
-	return error;
-}
-
-
-static int
-wl_iw_set_txpower(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error = 0;
-	char *p = extra;
-	int txpower = -1;
-
-	txpower = bcm_atoi(extra + strlen(TXPOWER_SET_CMD) + 1);
-	if ((txpower >= 0) && (txpower <= 127))
-	{
-		txpower |= WL_TXPWR_OVERRIDE;
-		txpower = htod32(txpower);
-
-		error = dev_wlc_intvar_set(dev, "qtxpower", txpower);
-		p += snprintf(p, MAX_WX_STRING, "OK");
-		WL_TRACE(("%s: set TXpower 0x%X is OK\n", __FUNCTION__, txpower));
-	} else {
-		WL_ERROR(("%s: set tx power failed\n", __FUNCTION__));
-		p += snprintf(p, MAX_WX_STRING, "FAIL");
-	}
-
-	wrqu->data.length = p - extra + 1;
-	return error;
-}
-
-static int
-wl_iw_get_macaddr(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error;
-	char buf[128];
-	struct ether_addr *id;
-	char *p = extra;
-
-	
-	strcpy(buf, "cur_etheraddr");
-	error = dev_wlc_ioctl(dev, WLC_GET_VAR, buf, sizeof(buf));
-	id = (struct ether_addr *) buf;
-	p += snprintf(p, MAX_WX_STRING, "Macaddr = %02X:%02X:%02X:%02X:%02X:%02X\n",
-		id->octet[0], id->octet[1], id->octet[2],
-		id->octet[3], id->octet[4], id->octet[5]);
-	wrqu->data.length = p - extra + 1;
-
-	return error;
-}
-
-
-
-static int
-wl_iw_set_country(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	char country_code[WLC_CNTRY_BUF_SZ];
-	int error = 0;
-	char *p = extra;
-	int country_offset;
-	int country_code_size;
-	wl_country_t cspec = {{0}, 0, {0}};
-	char smbuf[WLC_IOCTL_SMLEN];
-	scb_val_t scbval;
-
-	cspec.rev = -1;
-	memset(country_code, 0, sizeof(country_code));
-	memset(smbuf, 0, sizeof(smbuf));
-
-	
-	country_offset = strcspn(extra, " ");
-	country_code_size = strlen(extra) - country_offset;
-
-	
-	if (country_offset != 0) {
-		strncpy(country_code, extra + country_offset +1,
-			MIN(country_code_size, sizeof(country_code)));
-
-		
-		bzero(&scbval, sizeof(scb_val_t));
-		if ((error = dev_wlc_ioctl(dev, WLC_DISASSOC, &scbval, sizeof(scb_val_t)))) {
-			WL_ERROR(("%s: set country failed due to Disassoc error\n", __FUNCTION__));
-			goto exit_failed;
-		}
-
-		memcpy(cspec.country_abbrev, country_code, WLC_CNTRY_BUF_SZ);
-		memcpy(cspec.ccode, country_code, WLC_CNTRY_BUF_SZ);
-
-		get_customized_country_code((char *)&cspec.country_abbrev, &cspec);
-
-		
-		if ((error = dev_iw_iovar_setbuf(dev, "country", &cspec,
-			sizeof(cspec), smbuf, sizeof(smbuf))) >= 0) {
-			p += snprintf(p, MAX_WX_STRING, "OK");
-			WL_ERROR(("%s: set country for %s as %s rev %d is OK\n",
-				__FUNCTION__, country_code, cspec.ccode, cspec.rev));
-			dhd_bus_country_set(dev, &cspec);
-			goto exit;
-		}
-	}
-
-	WL_ERROR(("%s: set country for %s as %s rev %d failed\n",
-		__FUNCTION__, country_code, cspec.ccode, cspec.rev));
-
-exit_failed:
-	p += snprintf(p, MAX_WX_STRING, "FAIL");
-
-exit:
-	wrqu->data.length = p - extra + 1;
-	return error;
-}
-
-static int
-wl_iw_set_power_mode(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error = 0;
-	char *p = extra;
-	static int  pm = PM_FAST;
-	int  pm_local = PM_OFF;
-	char powermode_val = 0;
-
-	WL_TRACE_COEX(("%s: DHCP session cmd:%s\n", __FUNCTION__, extra));
-
-	strncpy((char *)&powermode_val, extra + strlen("POWERMODE") +1, 1);
-
-	if (strnicmp((char *)&powermode_val, "1", strlen("1")) == 0) {
-
-		WL_TRACE(("%s: DHCP session starts\n", __FUNCTION__));
-
-		dev_wlc_ioctl(dev, WLC_GET_PM, &pm, sizeof(pm));
-		dev_wlc_ioctl(dev, WLC_SET_PM, &pm_local, sizeof(pm_local));
-
-		
-		net_os_set_packet_filter(dev, 0);
-
-#ifdef COEX_DHCP
-		g_bt->ts_dhcp_start = JF2MS;
-		g_bt->dhcp_done = FALSE;
-		WL_TRACE_COEX(("%s: DHCP start, pm:%d changed to pm:%d\n",
-			__FUNCTION__, pm, pm_local));
-
-#endif 
-	} else if (strnicmp((char *)&powermode_val, "0", strlen("0")) == 0) {
-		
-
-		dev_wlc_ioctl(dev, WLC_SET_PM, &pm, sizeof(pm));
-
-		
-		net_os_set_packet_filter(dev, 1);
-
-#ifdef COEX_DHCP
-		g_bt->dhcp_done = TRUE;
-		g_bt->ts_dhcp_ok = JF2MS;
-		WL_TRACE_COEX(("%s: DHCP done for:%d ms, restored pm:%d\n",
-			__FUNCTION__, (g_bt->ts_dhcp_ok - g_bt->ts_dhcp_start), pm));
-#endif 
-
-	} else {
-		WL_ERROR(("%s Unkwown yet power setting, ignored\n",
-			__FUNCTION__));
-	}
-
-	p += snprintf(p, MAX_WX_STRING, "OK");
-
-	wrqu->data.length = p - extra + 1;
-
-	return error;
-}
-
-
-bool btcoex_is_sco_active(struct net_device *dev)
-{
-	int ioc_res = 0;
-	bool res = FALSE;
-	int sco_id_cnt = 0;
-	int param27;
-	int i;
-
-	for (i = 0; i < 12; i++) {
-
-		ioc_res = dev_wlc_intvar_get_reg(dev, "btc_params", 27, &param27);
-
-		WL_TRACE_COEX(("%s, sample[%d], btc params: 27:%x\n",
-			__FUNCTION__, i, param27));
-
-		if (ioc_res < 0) {
-			WL_ERROR(("%s ioc read btc params error\n", __FUNCTION__));
-			break;
-		}
-
-		if ((param27 & 0x6) == 2) { 
-			sco_id_cnt++;
-		}
-
-		if (sco_id_cnt > 2) {
-			WL_TRACE_COEX(("%s, sco/esco detected, pkt id_cnt:%d  samples:%d\n",
-				__FUNCTION__, sco_id_cnt, i));
-			res = TRUE;
-			break;
-		}
-
-		msleep(5);
-	}
-
-	return res;
-}
-
-#if defined(BT_DHCP_eSCO_FIX)
-
-static int set_btc_esco_params(struct net_device *dev, bool trump_sco)
-{
-	static bool saved_status = FALSE;
-
-	char buf_reg50va_dhcp_on[8] = { 50, 00, 00, 00, 0x22, 0x80, 0x00, 0x00 };
-	char buf_reg51va_dhcp_on[8] = { 51, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
-	char buf_reg64va_dhcp_on[8] = { 64, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
-	char buf_reg65va_dhcp_on[8] = { 65, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
-	char buf_reg71va_dhcp_on[8] = { 71, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
-
-	uint32 regaddr;
-	static uint32 saved_reg50;
-	static uint32 saved_reg51;
-	static uint32 saved_reg64;
-	static uint32 saved_reg65;
-	static uint32 saved_reg71;
-
-	if (trump_sco) { 
-
-		
-		WL_TRACE_COEX(("Do new SCO/eSCO coex algo {save & override} \n"));
-
-
-		if  ((!dev_wlc_intvar_get_reg(dev, "btc_params", 50,  &saved_reg50)) &&
-			(!dev_wlc_intvar_get_reg(dev, "btc_params", 51,  &saved_reg51)) &&
-			(!dev_wlc_intvar_get_reg(dev, "btc_params", 64,  &saved_reg64)) &&
-			(!dev_wlc_intvar_get_reg(dev, "btc_params", 65,  &saved_reg65)) &&
-			(!dev_wlc_intvar_get_reg(dev, "btc_params", 71,  &saved_reg71))) {
-
-			saved_status = TRUE;
-			WL_TRACE_COEX(("%s saved bt_params[50,51,64,65,71]:"
-				" 0x%x 0x%x 0x%x 0x%x 0x%x\n",
-				__FUNCTION__, saved_reg50, saved_reg51,
-				saved_reg64, saved_reg65, saved_reg71));
-
-		} else {
-			WL_ERROR((":%s: save btc_params failed\n",
-				__FUNCTION__));
-			saved_status = FALSE;
-			return -1;
-		}
-
-		WL_TRACE_COEX(("override with [50,51,64,65,71]:"
-			" 0x%x 0x%x 0x%x 0x%x 0x%x\n",
-			*(u32 *)(buf_reg50va_dhcp_on+4),
-			*(u32 *)(buf_reg51va_dhcp_on+4),
-			*(u32 *)(buf_reg64va_dhcp_on+4),
-			*(u32 *)(buf_reg65va_dhcp_on+4),
-			*(u32 *)(buf_reg71va_dhcp_on+4)));
-
-		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg50va_dhcp_on[0], 8);
-		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg51va_dhcp_on[0], 8);
-		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg64va_dhcp_on[0], 8);
-		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg65va_dhcp_on[0], 8);
-		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg71va_dhcp_on[0], 8);
-
-		saved_status = TRUE;
-
-	} else if (saved_status) {
-		
-		WL_TRACE_COEX(("Do new SCO/eSCO coex algo {save & override} \n"));
-
-		regaddr = 50;
-		dev_wlc_intvar_set_reg(dev, "btc_params",
-			(char *)&regaddr, (char *)&saved_reg50);
-		regaddr = 51;
-		dev_wlc_intvar_set_reg(dev, "btc_params",
-			(char *)&regaddr, (char *)&saved_reg51);
-		regaddr = 64;
-		dev_wlc_intvar_set_reg(dev, "btc_params",
-			(char *)&regaddr, (char *)&saved_reg64);
-		regaddr = 65;
-		dev_wlc_intvar_set_reg(dev, "btc_params",
-			(char *)&regaddr, (char *)&saved_reg65);
-		regaddr = 71;
-		dev_wlc_intvar_set_reg(dev, "btc_params",
-			(char *)&regaddr, (char *)&saved_reg71);
-
-		WL_TRACE_COEX(("restore bt_params[50,51,64,65,71]: 0x%x 0x%x 0x%x 0x%x 0x%x\n",
-			saved_reg50, saved_reg51, saved_reg64,
-			saved_reg65, saved_reg71));
-
-		saved_status = FALSE;
-	} else {
-		WL_ERROR((":%s att to restore not saved BTCOEX params\n",
-			__FUNCTION__));
-		return -1;
-	}
-	return 0;
-}
-#endif 
-
-
-static int
-wl_iw_get_power_mode(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error = 0;
-	int pm_local;
-	char *p = extra;
-
-	error = dev_wlc_ioctl(dev, WLC_GET_PM, &pm_local, sizeof(pm_local));
-	if (!error) {
-		WL_TRACE(("%s: Powermode = %d\n", __func__, pm_local));
-		if (pm_local == PM_OFF)
-			pm_local = 1; 
-		else
-			pm_local = 0; 
-		p += snprintf(p, MAX_WX_STRING, "powermode = %d", pm_local);
-	}
-	else {
-		WL_TRACE(("%s: Error = %d\n", __func__, error));
-		p += snprintf(p, MAX_WX_STRING, "FAIL");
-	}
-	wrqu->data.length = p - extra + 1;
-	return error;
-}
-
-static int
-wl_iw_set_btcoex_dhcp(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error = 0;
-	char *p = extra;
-	char powermode_val = 0;
-	char buf_reg66va_dhcp_on[8] = { 66, 00, 00, 00, 0x10, 0x27, 0x00, 0x00 };
-	char buf_reg41va_dhcp_on[8] = { 41, 00, 00, 00, 0x33, 0x00, 0x00, 0x00 };
-	char buf_reg68va_dhcp_on[8] = { 68, 00, 00, 00, 0x90, 0x01, 0x00, 0x00 };
-
-	uint32 regaddr;
-	static uint32 saved_reg66;
-	static uint32 saved_reg41;
-	static uint32 saved_reg68;
-	static bool saved_status = FALSE;
-
-#ifdef COEX_DHCP
-	char buf_flag7_default[8] =   { 7, 00, 00, 00, 0x0, 0x00, 0x00, 0x00};
-#endif 
-
-	
-	strncpy((char *)&powermode_val, extra + strlen("BTCOEXMODE") +1, 1);
-
-	if (strnicmp((char *)&powermode_val, "1", strlen("1")) == 0) {
-
-		WL_TRACE(("%s: DHCP session starts\n", __FUNCTION__));
-
-		
-		if ((saved_status == FALSE) &&
-			(!dev_wlc_intvar_get_reg(dev, "btc_params", 66,  &saved_reg66)) &&
-			(!dev_wlc_intvar_get_reg(dev, "btc_params", 41,  &saved_reg41)) &&
-			(!dev_wlc_intvar_get_reg(dev, "btc_params", 68,  &saved_reg68)))   {
-				saved_status = TRUE;
-				WL_TRACE(("Saved 0x%x 0x%x 0x%x\n",
-					saved_reg66, saved_reg41, saved_reg68));
-
-				
-
-				
-#ifdef COEX_DHCP
-				
-				if (btcoex_is_sco_active(dev)) {
-					
-					dev_wlc_bufvar_set(dev, "btc_params",
-						(char *)&buf_reg66va_dhcp_on[0],
-						sizeof(buf_reg66va_dhcp_on));
-					
-					dev_wlc_bufvar_set(dev, "btc_params",
-						(char *)&buf_reg41va_dhcp_on[0],
-						sizeof(buf_reg41va_dhcp_on));
-					
-					dev_wlc_bufvar_set(dev, "btc_params",
-						(char *)&buf_reg68va_dhcp_on[0],
-						sizeof(buf_reg68va_dhcp_on));
-					saved_status = TRUE;
-
-					g_bt->bt_state = BT_DHCP_START;
-					g_bt->timer_on = 1;
-					mod_timer(&g_bt->timer, g_bt->timer.expires);
-					WL_TRACE_COEX(("%s enable BT DHCP Timer\n",
-					__FUNCTION__));
-				}
-#endif 
-		}
-		else if (saved_status == TRUE) {
-			WL_ERROR(("%s was called w/o DHCP OFF. Continue\n", __FUNCTION__));
-		}
-	}
-	else if (strnicmp((char *)&powermode_val, "2", strlen("2")) == 0) {
-
-
-		
-
-#ifdef COEX_DHCP
-		
-		WL_TRACE(("%s disable BT DHCP Timer\n", __FUNCTION__));
-		if (g_bt->timer_on) {
-			g_bt->timer_on = 0;
-			del_timer_sync(&g_bt->timer);
-
-			if (g_bt->bt_state != BT_DHCP_IDLE) {
-			
-				WL_TRACE_COEX(("%s bt->bt_state:%d\n",
-					__FUNCTION__, g_bt->bt_state));
-				
-				up(&g_bt->tsk_ctl.sema);
-			}
-		}
-
-		
-		if (saved_status == TRUE)
-			dev_wlc_bufvar_set(dev, "btc_flags",
-				(char *)&buf_flag7_default[0], sizeof(buf_flag7_default));
-#endif 
-
-		
-		if (saved_status == TRUE) {
-			regaddr = 66;
-			dev_wlc_intvar_set_reg(dev, "btc_params",
-				(char *)&regaddr, (char *)&saved_reg66);
-			regaddr = 41;
-			dev_wlc_intvar_set_reg(dev, "btc_params",
-				(char *)&regaddr, (char *)&saved_reg41);
-			regaddr = 68;
-			dev_wlc_intvar_set_reg(dev, "btc_params",
-				(char *)&regaddr, (char *)&saved_reg68);
-
-			WL_TRACE_COEX(("restore regs {66,41,68} <- 0x%x 0x%x 0x%x\n",
-				saved_reg66, saved_reg41, saved_reg68));
-		}
-		saved_status = FALSE;
-
-	}
-	else {
-		WL_ERROR(("%s Unkwown yet power setting, ignored\n",
-			__FUNCTION__));
-	}
-
-	p += snprintf(p, MAX_WX_STRING, "OK");
-
-	wrqu->data.length = p - extra + 1;
-
-	return error;
-}
-
-static int
-wl_iw_set_suspend(
-struct net_device *dev,
-struct iw_request_info *info,
-union iwreq_data *wrqu,
-char *extra
-)
-{
-	int suspend_flag;
-	int ret_now;
-	int ret = 0;
-
-	suspend_flag = *(extra + strlen(SETSUSPEND_CMD) + 1) - '0';
-
-	if (suspend_flag != 0)
-		suspend_flag = 1;
-
-	ret_now = net_os_set_suspend_disable(dev, suspend_flag);
-
-	
-	if (ret_now != suspend_flag) {
-		if (!(ret = net_os_set_suspend(dev, ret_now)))
-			WL_ERROR(("%s: Suspend Flag %d -> %d\n",
-			          __FUNCTION__, ret_now, suspend_flag));
-		else
-			WL_ERROR(("%s: failed %d\n", __FUNCTION__, ret));
-	}
-
-	return ret;
-}
-
-static int
-wl_format_ssid(char* ssid_buf, uint8* ssid, int ssid_len)
-{
-	int i, c;
-	char *p = ssid_buf;
-
-	if (ssid_len > 32) ssid_len = 32;
-
-	for (i = 0; i < ssid_len; i++) {
-		c = (int)ssid[i];
-		if (c == '\\') {
-			*p++ = '\\';
-			*p++ = '\\';
-		} else if (isprint((uchar)c)) {
-			*p++ = (char)c;
-		} else {
-			p += sprintf(p, "\\x%02X", c);
-		}
-	}
-	*p = '\0';
-
-	return p - ssid_buf;
-}
-
-static int
-wl_iw_get_link_speed(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error = 0;
-	char *p = extra;
-	static int link_speed;
-
-	
-	net_os_wake_lock(dev);
-	if (g_onoff == G_WLAN_SET_ON) {
-		error = dev_wlc_ioctl(dev, WLC_GET_RATE, &link_speed, sizeof(link_speed));
-		link_speed *= 500000;
-	}
-
-	p += snprintf(p, MAX_WX_STRING, "LinkSpeed %d", link_speed/1000000);
-
-	wrqu->data.length = p - extra + 1;
-
-	net_os_wake_unlock(dev);
-	return error;
-}
-
-
-static int
-wl_iw_get_dtim_skip(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error = -1;
-	char *p = extra;
-	char iovbuf[32];
-
-	net_os_wake_lock(dev);
-	if (g_onoff == G_WLAN_SET_ON) {
-
-			memset(iovbuf, 0, sizeof(iovbuf));
-			strcpy(iovbuf, "bcn_li_dtim");
-
-			if ((error = dev_wlc_ioctl(dev, WLC_GET_VAR,
-				&iovbuf, sizeof(iovbuf))) >= 0) {
-
-				p += snprintf(p, MAX_WX_STRING, "Dtim_skip %d", iovbuf[0]);
-				WL_TRACE(("%s: get dtim_skip = %d\n", __FUNCTION__, iovbuf[0]));
-				wrqu->data.length = p - extra + 1;
-			}
-			else
-				WL_ERROR(("%s: get dtim_skip failed code %d\n",
-					__FUNCTION__, error));
-	}
-	net_os_wake_unlock(dev);
-	return error;
-}
-
-
-static int
-wl_iw_set_dtim_skip(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error = -1;
-	char *p = extra;
-	int bcn_li_dtim;
-	char iovbuf[32];
-
-	net_os_wake_lock(dev);
-	if (g_onoff == G_WLAN_SET_ON) {
-
-		bcn_li_dtim = htod32((uint)*(extra + strlen(DTIM_SKIP_SET_CMD) + 1) - '0');
-
-		if ((bcn_li_dtim >= 0) || ((bcn_li_dtim <= 5))) {
-
-			memset(iovbuf, 0, sizeof(iovbuf));
-			bcm_mkiovar("bcn_li_dtim", (char *)&bcn_li_dtim,
-				4, iovbuf, sizeof(iovbuf));
-
-			if ((error = dev_wlc_ioctl(dev, WLC_SET_VAR,
-				&iovbuf, sizeof(iovbuf))) >= 0) {
-				p += snprintf(p, MAX_WX_STRING, "OK");
-
-				net_os_set_dtim_skip(dev, bcn_li_dtim);
-
-				WL_TRACE(("%s: set dtim_skip %d OK\n", __FUNCTION__,
-					bcn_li_dtim));
-				goto exit;
-			}
-			else  WL_ERROR(("%s: set dtim_skip %d failed code %d\n",
-				__FUNCTION__, bcn_li_dtim, error));
-		}
-		else  WL_ERROR(("%s Incorrect dtim_skip setting %d, ignored\n",
-			__FUNCTION__, bcn_li_dtim));
-	}
-
-	p += snprintf(p, MAX_WX_STRING, "FAIL");
-
-exit:
-	wrqu->data.length = p - extra + 1;
-	net_os_wake_unlock(dev);
-	return error;
-}
-
-
-static int
-wl_iw_get_band(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error = -1;
-	char *p = extra;
-	static int band;
-
-	net_os_wake_lock(dev);
-
-	if (g_onoff == G_WLAN_SET_ON) {
-		error = dev_wlc_ioctl(dev, WLC_GET_BAND, &band, sizeof(band));
-
-		p += snprintf(p, MAX_WX_STRING, "Band %d", band);
-
-		wrqu->data.length = p - extra + 1;
-	}
-
-	net_os_wake_unlock(dev);
-	return error;
-}
-
-
-static int
-wl_iw_set_band(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error = -1;
-	char *p = extra;
-	uint band;
-
-	net_os_wake_lock(dev);
-
-	if (g_onoff == G_WLAN_SET_ON) {
-
-		band = htod32((uint)*(extra + strlen(BAND_SET_CMD) + 1) - '0');
-
-		if ((band == WLC_BAND_AUTO) || (band == WLC_BAND_5G) || (band == WLC_BAND_2G)) {
-			
-			if ((error = dev_wlc_ioctl(dev, WLC_SET_BAND,
-				&band, sizeof(band))) >= 0) {
-				p += snprintf(p, MAX_WX_STRING, "OK");
-				WL_TRACE(("%s: set band %d OK\n", __FUNCTION__, band));
-				goto exit;
-			} else {
-				WL_ERROR(("%s: set band %d failed code %d\n", __FUNCTION__,
-				          band, error));
-			}
-		} else {
-			WL_ERROR(("%s Incorrect band setting %d, ignored\n", __FUNCTION__, band));
-		}
-	}
-
-	p += snprintf(p, MAX_WX_STRING, "FAIL");
-
-exit:
-	wrqu->data.length = p - extra + 1;
-	net_os_wake_unlock(dev);
-	return error;
-}
-
-#ifdef PNO_SUPPORT
-
-static int
-wl_iw_set_pno_reset(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error = -1;
-	char *p = extra;
-
-	net_os_wake_lock(dev);
-	if ((g_onoff == G_WLAN_SET_ON) && (dev != NULL)) {
-
-		if ((error = dhd_dev_pno_reset(dev)) >= 0) {
-				p += snprintf(p, MAX_WX_STRING, "OK");
-				WL_TRACE(("%s: set OK\n", __FUNCTION__));
-				goto exit;
-		}
-		else  WL_ERROR(("%s: failed code %d\n", __FUNCTION__, error));
-	}
-
-	p += snprintf(p, MAX_WX_STRING, "FAIL");
-
-exit:
-	wrqu->data.length = p - extra + 1;
-	net_os_wake_unlock(dev);
-	return error;
-}
-
-
-
-static int
-wl_iw_set_pno_enable(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error = -1;
-	char *p = extra;
-	int pfn_enabled;
-
-	net_os_wake_lock(dev);
-	pfn_enabled = htod32((uint)*(extra + strlen(PNOENABLE_SET_CMD) + 1) - '0');
-
-	if ((g_onoff == G_WLAN_SET_ON) && (dev != NULL)) {
-
-		if ((error = dhd_dev_pno_enable(dev, pfn_enabled)) >= 0) {
-				p += snprintf(p, MAX_WX_STRING, "OK");
-				WL_TRACE(("%s: set OK\n", __FUNCTION__));
-				goto exit;
-		}
-		else  WL_ERROR(("%s: failed code %d\n", __FUNCTION__, error));
-	}
-
-	p += snprintf(p, MAX_WX_STRING, "FAIL");
-
-exit:
-	wrqu->data.length = p - extra + 1;
-	net_os_wake_unlock(dev);
-	return error;
-}
-
-
-
-static int
-wl_iw_set_pno_set(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int res = -1;
-	wlc_ssid_t ssids_local[MAX_PFN_LIST_COUNT];
-	int nssid = 0;
-	cmd_tlv_t *cmd_tlv_temp;
-	char *str_ptr;
-	int tlv_size_left;
-	int pno_time;
-	int pno_repeat;
-	int pno_freq_expo_max;
-#ifdef PNO_SET_DEBUG
-	int i;
-	char pno_in_example[] = {
-		'P', 'N', 'O', 'S', 'E', 'T', 'U', 'P', ' ',
-		'S', '1', '2', '0',
-		'S',
-		0x04,
-		'B', 'R', 'C', 'M',
-		'S',
-		0x04,
-		'G', 'O', 'O', 'G',
-		'T',
-		'1', 'E',
-		'R',
-		'2',
-		'M',
-		'2',
-		0x00
-		};
-#endif 
-
-	net_os_wake_lock(dev);
-	WL_ERROR(("\n### %s: info->cmd:%x, info->flags:%x, u.data=0x%p, u.len=%d\n",
-		__FUNCTION__, info->cmd, info->flags,
-		wrqu->data.pointer, wrqu->data.length));
-
-	if (g_onoff == G_WLAN_SET_OFF) {
-		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
-		goto exit_proc;
-	}
-
-	if (wrqu->data.length < (strlen(PNOSETUP_SET_CMD) + sizeof(cmd_tlv_t))) {
-		WL_ERROR(("%s argument=%d less %d\n", __FUNCTION__,
-		          wrqu->data.length, (int)(strlen(PNOSETUP_SET_CMD) + sizeof(cmd_tlv_t))));
-		goto exit_proc;
-	}
-
-#ifdef PNO_SET_DEBUG
-	if (!(extra = kmalloc(sizeof(pno_in_example) +100, GFP_KERNEL))) {
-		res = -ENOMEM;
-		goto exit_proc;
-	}
-	memcpy(extra, pno_in_example, sizeof(pno_in_example));
-	wrqu->data.length = sizeof(pno_in_example);
-	for (i = 0; i < wrqu->data.length; i++)
-		printf("%02X ", extra[i]);
-	printf("\n");
-#endif 
-
-	str_ptr = extra;
-#ifdef PNO_SET_DEBUG
-	str_ptr +=  strlen("PNOSETUP ");
-	tlv_size_left = wrqu->data.length - strlen("PNOSETUP ");
-#else
-	str_ptr +=  strlen(PNOSETUP_SET_CMD);
-	tlv_size_left = wrqu->data.length - strlen(PNOSETUP_SET_CMD);
-#endif
-
-	cmd_tlv_temp = (cmd_tlv_t *)str_ptr;
-	memset(ssids_local, 0, sizeof(ssids_local));
-	pno_repeat = pno_freq_expo_max = 0;
-	
-	if ((cmd_tlv_temp->prefix == PNO_TLV_PREFIX) &&
-		(cmd_tlv_temp->version == PNO_TLV_VERSION) &&
-		(cmd_tlv_temp->subver == PNO_TLV_SUBVERSION))
-	{
-		str_ptr += sizeof(cmd_tlv_t);
-		tlv_size_left  -= sizeof(cmd_tlv_t);
-
-		
-		if ((nssid = wl_iw_parse_ssid_list_tlv(&str_ptr, ssids_local,
-			MAX_PFN_LIST_COUNT,
-			&tlv_size_left)) <= 0) {
-			WL_ERROR(("SSID is not presented or corrupted ret=%d\n", nssid));
-			goto exit_proc;
-		}
-		else {
-			if ((str_ptr[0] != PNO_TLV_TYPE_TIME) || (tlv_size_left <= 1)) {
-				WL_ERROR(("%s scan duration corrupted field size %d\n",
-					__FUNCTION__, tlv_size_left));
-				goto exit_proc;
-			}
-			str_ptr++;
-			pno_time = simple_strtoul(str_ptr, &str_ptr, 16);
-			WL_PNO(("%s: pno_time=%d\n", __FUNCTION__, pno_time));
-
-			
-			if (str_ptr[0] != 0) {
-				if ((str_ptr[0] != PNO_TLV_FREQ_REPEAT)) {
-					WL_ERROR(("%s pno repeat : corrupted field\n",
-						__FUNCTION__));
-					goto exit_proc;
-				}
-				str_ptr++;
-				pno_repeat = simple_strtoul(str_ptr, &str_ptr, 16);
-				WL_PNO(("%s :got pno_repeat=%d\n", __FUNCTION__, pno_repeat));
-				if (str_ptr[0] != PNO_TLV_FREQ_EXPO_MAX) {
-					WL_ERROR(("%s FREQ_EXPO_MAX corrupted field size\n",
-						__FUNCTION__));
-					goto exit_proc;
-				}
-				str_ptr++;
-				pno_freq_expo_max = simple_strtoul(str_ptr, &str_ptr, 16);
-				WL_PNO(("%s: pno_freq_expo_max=%d\n",
-					__FUNCTION__, pno_freq_expo_max));
-			}
-		}
-	}
-	else {
-		WL_ERROR(("%s get wrong TLV command\n", __FUNCTION__));
-		goto exit_proc;
-	}
-
-	
-	res = dhd_dev_pno_set(dev, ssids_local, nssid, pno_time, pno_repeat, pno_freq_expo_max);
-
-exit_proc:
-	net_os_wake_unlock(dev);
-	return res;
-}
-#endif 
-
-static int
-wl_iw_get_rssi(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	static int rssi = 0;
-	static wlc_ssid_t ssid = {0};
-	int error = 0;
-	char *p = extra;
-	static char ssidbuf[SSID_FMT_BUF_LEN];
-	scb_val_t scb_val;
-
-	net_os_wake_lock(dev);
-
-	bzero(&scb_val, sizeof(scb_val_t));
-
-	if (g_onoff == G_WLAN_SET_ON) {
-		error = dev_wlc_ioctl(dev, WLC_GET_RSSI, &scb_val, sizeof(scb_val_t));
-		if (error) {
-			WL_ERROR(("%s: Fails %d\n", __FUNCTION__, error));
-			net_os_wake_unlock(dev);
-			return error;
-		}
-		rssi = dtoh32(scb_val.val);
-
-		error = dev_wlc_ioctl(dev, WLC_GET_SSID, &ssid, sizeof(ssid));
-		if (!error) {
-			ssid.SSID_len = dtoh32(ssid.SSID_len);
-			wl_format_ssid(ssidbuf, ssid.SSID, dtoh32(ssid.SSID_len));
-		}
-	}
-
-	p += snprintf(p, MAX_WX_STRING, "%s rssi %d ", ssidbuf, rssi);
-	wrqu->data.length = p - extra + 1;
-
-	net_os_wake_unlock(dev);
-	return error;
-}
-
-int
-wl_iw_send_priv_event(
-	struct net_device *dev,
-	char *flag
-)
-{
-	union iwreq_data wrqu;
-	char extra[IW_CUSTOM_MAX + 1];
-	int cmd;
-
-	cmd = IWEVCUSTOM;
-	memset(&wrqu, 0, sizeof(wrqu));
-	if (strlen(flag) > sizeof(extra))
-		return -1;
-
-	strcpy(extra, flag);
-	wrqu.data.length = strlen(extra);
-	wireless_send_event(dev, cmd, &wrqu, extra);
-	net_os_wake_lock_timeout_enable(dev);
-	WL_TRACE(("Send IWEVCUSTOM Event as %s\n", extra));
-
-	return 0;
-}
-
-
-int
-wl_control_wl_start(struct net_device *dev)
-{
-	wl_iw_t *iw;
-	int ret = 0;
-
-	WL_TRACE(("Enter %s \n", __FUNCTION__));
-
-	if (!dev) {
-		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
-		return -1;
-	}
-
-	iw = *(wl_iw_t **)netdev_priv(dev);
-
-	if (!iw) {
-		WL_ERROR(("%s: wl is null\n", __FUNCTION__));
-		return -1;
-	}
-
-	dhd_net_if_lock(dev);
-
-	if (g_onoff == G_WLAN_SET_OFF) {
-		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_ON);
-
-#if defined(BCMLXSDMMC)
-		sdioh_start(NULL, 0);
-#endif
-
-		ret = dhd_dev_reset(dev, 0);
-
-#if defined(BCMLXSDMMC)
-		sdioh_start(NULL, 1);
-#endif
-
-		dhd_dev_init_ioctl(dev);
-
-		g_onoff = G_WLAN_SET_ON;
-	}
-	WL_TRACE(("Exited %s\n", __FUNCTION__));
-
-	dhd_net_if_unlock(dev);
-	return ret;
-}
-
-
-static int
-wl_iw_control_wl_off(
-	struct net_device *dev,
-	struct iw_request_info *info
-)
-{
-	wl_iw_t *iw;
-	int ret = 0;
-
-	WL_TRACE(("Enter %s\n", __FUNCTION__));
-
-	if (!dev) {
-		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
-		return -1;
-	}
-
-	iw = *(wl_iw_t **)netdev_priv(dev);
-
-	if (!iw) {
-		WL_ERROR(("%s: wl is null\n", __FUNCTION__));
-		return -1;
-	}
-
-	dhd_net_if_lock(dev);
-
-#ifdef SOFTAP
-	ap_cfg_running = FALSE;
-#endif 
-
-	if (g_onoff == G_WLAN_SET_ON) {
-		g_onoff = G_WLAN_SET_OFF;
-
-#if defined(WL_IW_USE_ISCAN)
-		g_iscan->iscan_state = ISCAN_STATE_IDLE;
-#endif 
-
-		dhd_dev_reset(dev, 1);
-
-#if defined(WL_IW_USE_ISCAN)
-#if !defined(CSCAN)
-		
-		wl_iw_free_ss_cache();
-		wl_iw_run_ss_cache_timer(0);
-		
-		g_ss_cache_ctrl.m_link_down = 1;
-#endif 
-		memset(g_scan, 0, G_SCAN_RESULTS);
-		g_scan_specified_ssid = 0;
-#if defined(CONFIG_FIRST_SCAN)
-		
-		g_first_broadcast_scan = BROADCAST_SCAN_FIRST_IDLE;
-		g_first_counter_scans = 0;
-#endif 
-#endif 
-
-#if defined(BCMLXSDMMC)
-		sdioh_stop(NULL);
-#endif
-
-		net_os_set_dtim_skip(dev, 0);
-
-		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_OFF);
-
-		wl_iw_send_priv_event(dev, "STOP");
-	}
-
-	dhd_net_if_unlock(dev);
-
-	WL_TRACE(("Exited %s\n", __FUNCTION__));
-
-	return ret;
-}
-
-static int
-wl_iw_control_wl_on(
-	struct net_device *dev,
-	struct iw_request_info *info
-)
-{
-	int ret = 0;
-
-	WL_TRACE(("Enter %s \n", __FUNCTION__));
-
-	ret = wl_control_wl_start(dev);
-
-	wl_iw_send_priv_event(dev, "START");
-
-#ifdef SOFTAP
-	if (!ap_fw_loaded) {
-		wl_iw_iscan_set_scan_broadcast_prep(dev, 0);
-	}
-#else
-	wl_iw_iscan_set_scan_broadcast_prep(dev, 0);
-#endif
-
-	WL_TRACE(("Exited %s\n", __FUNCTION__));
-
-	return ret;
-}
-
-#ifdef SOFTAP
-static struct ap_profile my_ap;
-static int set_ap_cfg(struct net_device *dev, struct ap_profile *ap); 
-static int get_assoc_sta_list(struct net_device *dev, char *buf, int len);
-static int set_ap_mac_list(struct net_device *dev, void *buf);
-
-#define PTYPE_STRING 0
-#define PTYPE_INTDEC 1   
-#define PTYPE_INTHEX 2
-#define PTYPE_STR_HEX 3  
-
-static int get_parameter_from_string(
-	char **str_ptr, const char *token, int param_type, void  *dst, int param_max_len);
-
-static int
-hex2num(char c)
-{
-	if (c >= '0' && c <= '9')
-		return c - '0';
-	if (c >= 'a' && c <= 'f')
-		return c - 'a' + 10;
-	if (c >= 'A' && c <= 'F')
-		return c - 'A' + 10;
-	return -1;
-}
-
-
-
-static int
-hstr_2_buf(const char *txt, u8 *buf, int len)
-{
-	int i;
-
-	for (i = 0; i < len; i++) {
-		int a, b;
-
-		a = hex2num(*txt++);
-		if (a < 0)
-			return -1;
-		b = hex2num(*txt++);
-		if (b < 0)
-			return -1;
-		*buf++ = (a << 4) | b;
-	}
-
-	return 0;
-}
-
-
-
-static int
-init_ap_profile_from_string(char *param_str, struct ap_profile *ap_cfg)
-{
-	char *str_ptr = param_str;
-	char sub_cmd[16];
-	int ret = 0;
-
-	memset(sub_cmd, 0, sizeof(sub_cmd));
-	memset(ap_cfg, 0, sizeof(struct ap_profile));
-
-	
-	if (get_parameter_from_string(&str_ptr, "ASCII_CMD=",
-		PTYPE_STRING, sub_cmd, SSID_LEN) != 0) {
-	 return -1;
-	}
-	if (strncmp(sub_cmd, "AP_CFG", 6)) {
-	   WL_ERROR(("ERROR: sub_cmd:%s != 'AP_CFG'!\n", sub_cmd));
-		return -1;
-	}
-
-	
-	
-	ret = get_parameter_from_string(&str_ptr, "SSID=", PTYPE_STRING, ap_cfg->ssid, SSID_LEN);
-
-	ret |= get_parameter_from_string(&str_ptr, "SEC=", PTYPE_STRING,  ap_cfg->sec, SEC_LEN);
-
-	ret |= get_parameter_from_string(&str_ptr, "KEY=", PTYPE_STRING,  ap_cfg->key, KEY_LEN);
-
-	ret |= get_parameter_from_string(&str_ptr, "CHANNEL=", PTYPE_INTDEC, &ap_cfg->channel, 5);
-
-	
-	get_parameter_from_string(&str_ptr, "PREAMBLE=", PTYPE_INTDEC, &ap_cfg->preamble, 5);
-
-	
-	get_parameter_from_string(&str_ptr, "MAX_SCB=", PTYPE_INTDEC,  &ap_cfg->max_scb, 5);
-
-	
-	get_parameter_from_string(&str_ptr, "HIDDEN=",
-		PTYPE_INTDEC,  &ap_cfg->closednet, 5);
-
-	
-	get_parameter_from_string(&str_ptr, "COUNTRY=",
-		PTYPE_STRING,  &ap_cfg->country_code, 3);
-
-	return ret;
-}
-#endif 
-
-
-
-#ifdef SOFTAP
-static int
-iwpriv_set_ap_config(struct net_device *dev,
-            struct iw_request_info *info,
-            union iwreq_data *wrqu,
-            char *ext)
-{
-	int res = 0;
-	char  *extra = NULL;
-	struct ap_profile *ap_cfg = &my_ap;
-
-	WL_TRACE(("> Got IWPRIV SET_AP IOCTL: info->cmd:%x, info->flags:%x, u.data:%p, u.len:%d\n",
-		info->cmd, info->flags,
-		wrqu->data.pointer, wrqu->data.length));
-
-	if (!ap_fw_loaded) {
-		WL_ERROR(("Can't execute %s(), SOFTAP fw is not Loaded\n",
-			__FUNCTION__));
-		return -1;
-	}
-
-	if (wrqu->data.length != 0) {
-
-		char *str_ptr;
-
-		if (!(extra = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
-			return -ENOMEM;
-
-		if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
-			kfree(extra);
-			return -EFAULT;
-		}
-
-		extra[wrqu->data.length] = 0;
-		WL_SOFTAP((" Got str param in iw_point:\n %s\n", extra));
-
-		memset(ap_cfg, 0, sizeof(struct ap_profile));
-
-		
-
-		str_ptr = extra;
-
-		if ((res = init_ap_profile_from_string(extra, ap_cfg)) < 0) {
-			WL_ERROR(("%s failed to parse %d\n", __FUNCTION__, res));
-			kfree(extra);
-			return -1;
-		}
-
-	} else {
-	 
-	  WL_ERROR(("IWPRIV argument len = 0 \n"));
-	  return -1;
-	}
-
-	if ((res = set_ap_cfg(dev, ap_cfg)) < 0)
-		WL_ERROR(("%s failed to set_ap_cfg %d\n", __FUNCTION__, res));
-
-	kfree(extra);
-
-	return res;
-}
-#endif 
-
-
-
-#ifdef SOFTAP
-static int iwpriv_get_assoc_list(struct net_device *dev,
-        struct iw_request_info *info,
-        union iwreq_data *p_iwrq,
-        char *extra)
-{
-	int i, ret = 0;
-	char mac_buf[256];
-	struct maclist *sta_maclist = (struct maclist *)mac_buf;
-
-	char mac_lst[384];
-	char *p_mac_str;
-	char *p_mac_str_end;
-	wl_iw_t *iw;
-
-	if ((!dev) || (!extra)) {
-		
-		return -EINVAL;
-	}
-
-
-	iw = *(wl_iw_t **)netdev_priv(dev);
-
-	net_os_wake_lock(dev);
-	DHD_OS_MUTEX_LOCK(&wl_softap_lock);
-
-	WL_TRACE(("\n %s: IWPRIV IOCTL: cmd:%hx, flags:%hx, extra:%p, iwp.len:%d,"
-		"iwp.len:%p, iwp.flags:%x  \n", __FUNCTION__, info->cmd, info->flags,
-		extra, p_iwrq->data.length, p_iwrq->data.pointer, p_iwrq->data.flags));
-
-
-	memset(sta_maclist, 0, sizeof(mac_buf));
-
-	sta_maclist->count = 8;
-
-	WL_SOFTAP(("%s: net device:%s, buf_sz:%d\n",
-		__FUNCTION__, dev->name, sizeof(mac_buf)));
-
-	if ((ret = get_assoc_sta_list(dev, mac_buf, sizeof(mac_buf))) < 0) {
-		WL_ERROR(("%s: sta list ioctl error:%d\n",
-			__FUNCTION__, ret));
-		goto func_exit;
-	}
-
-	WL_SOFTAP(("%s: got %d stations\n", __FUNCTION__,
-		sta_maclist->count));
-
-
-	
-	memset(mac_lst, 0, sizeof(mac_lst));
-	p_mac_str = mac_lst;
-	p_mac_str_end = &mac_lst[sizeof(mac_lst)-1];
-
-	for (i = 0; i < 8; i++) { 
-		struct ether_addr * id = &sta_maclist->ea[i];
-		if (!ETHER_ISNULLADDR(id->octet)) {
-			scb_val_t scb_val;
-			int rssi = 0;
-			bzero(&scb_val, sizeof(scb_val_t));
-
-			
-			if ((p_mac_str_end - p_mac_str) <= 36) {
-				WL_ERROR(("%s: mac list buf is < 36 for item[%i] item\n",
-					__FUNCTION__, i));
-				break;
-			}
-
-			p_mac_str += snprintf(p_mac_str, MAX_WX_STRING,
-			"\nMac[%d]=%02X:%02X:%02X:%02X:%02X:%02X,", i,
-			id->octet[0], id->octet[1], id->octet[2],
-			id->octet[3], id->octet[4], id->octet[5]);
-
-			
-			bcopy(id->octet, &scb_val.ea, 6);
-			ret = dev_wlc_ioctl(dev, WLC_GET_RSSI, &scb_val, sizeof(scb_val_t));
-			if (ret  < 0) {
-				snprintf(p_mac_str, MAX_WX_STRING, "RSSI:ERR");
-				WL_ERROR(("%s: RSSI ioctl error:%d\n",
-					__FUNCTION__, ret));
-				break;
-			}
-
-			rssi = dtoh32(scb_val.val);
-			p_mac_str += snprintf(p_mac_str, MAX_WX_STRING,
-			"RSSI:%d", rssi);
-		}
-	}
-
-	p_iwrq->data.length = strlen(mac_lst)+1; 
-
-	WL_SOFTAP(("%s: data to user:\n%s\n usr_ptr:%p\n", __FUNCTION__,
-		mac_lst, p_iwrq->data.pointer));
-
-	if (p_iwrq->data.length) {
-		bcopy(mac_lst, extra, p_iwrq->data.length);
-	}
-
-func_exit:
-
-	DHD_OS_MUTEX_UNLOCK(&wl_softap_lock);
-	net_os_wake_unlock(dev);
-
-	WL_SOFTAP(("%s: Exited\n", __FUNCTION__));
-	return ret;
-}
-#endif 
-
-
-#ifdef SOFTAP
-
-#define MAC_FILT_MAX 8
-static int iwpriv_set_mac_filters(struct net_device *dev,
-        struct iw_request_info *info,
-        union iwreq_data *wrqu,
-        char *ext)
-{
-	int i, ret = -1;
-	char  * extra = NULL;
-	int mac_cnt = 0; 
-	int mac_mode = 0;
-	struct ether_addr *p_ea;
-	struct mac_list_set mflist_set; 
-
-	WL_SOFTAP((">>> Got IWPRIV SET_MAC_FILTER IOCTL:  info->cmd:%x,"
-			"info->flags:%x, u.data:%p, u.len:%d\n",
-			info->cmd, info->flags,
-			wrqu->data.pointer, wrqu->data.length));
-
-	if (wrqu->data.length != 0) {
-
-		char *str_ptr;
-
-		if (!(extra = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
-			return -ENOMEM;
-
-		if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
-			kfree(extra);
-			return -EFAULT;
-		}
-
-		extra[wrqu->data.length] = 0;
-		WL_SOFTAP((" Got parameter string in iw_point:\n %s \n", extra));
-
-		memset(&mflist_set, 0, sizeof(mflist_set));
-
-		
-		str_ptr = extra;
-
-
-		
-		if (get_parameter_from_string(&str_ptr, "MAC_MODE=",
-			PTYPE_INTDEC, &mac_mode, 4) != 0) {
-			WL_ERROR(("ERROR: 'MAC_MODE=' token is missing\n"));
-			goto exit_proc;
-		}
-
-		p_ea = &mflist_set.mac_list.ea[0];
-
-		if (get_parameter_from_string(&str_ptr, "MAC_CNT=",
-			PTYPE_INTDEC, &mac_cnt, 4) != 0) {
-			WL_ERROR(("ERROR: 'MAC_CNT=' token param is missing \n"));
-			goto exit_proc;
-		}
-
-		if (mac_cnt > MAC_FILT_MAX) {
-			WL_ERROR(("ERROR: number of MAC filters > MAX\n"));
-			goto exit_proc;
-		}
-
-		for (i=0; i< mac_cnt; i++)	
-		if (get_parameter_from_string(&str_ptr, "MAC=",
-			PTYPE_STR_HEX, &p_ea[i], 12) != 0) {
-			WL_ERROR(("ERROR: MAC_filter[%d] is missing !\n", i));
-			goto exit_proc;
-		}
-
-		WL_SOFTAP(("MAC_MODE=:%d, MAC_CNT=%d, MACs:..\n", mac_mode, mac_cnt));
-		for (i = 0; i < mac_cnt; i++) {
-		   WL_SOFTAP(("mac_filt[%d]:", i));
-		   dhd_print_buf(&p_ea[i], 6, 0);
-		}
-
-		
-		mflist_set.mode = mac_mode;
-		mflist_set.mac_list.count = mac_cnt;
-		set_ap_mac_list(dev, &mflist_set);
-
-		
-		wrqu->data.pointer = NULL;
-		wrqu->data.length = 0;
-		ret = 0;
-
-	} else {
-	 
-	  WL_ERROR(("IWPRIV argument len is 0\n"));
-	  return -1;
-	}
-
-	exit_proc:
-	kfree(extra);
-	return ret;
-}
-#endif 
-
-
-#ifdef SOFTAP
-
-static int iwpriv_set_ap_sta_disassoc(struct net_device *dev,
-        struct iw_request_info *info,
-        union iwreq_data *wrqu,
-        char *ext)
-{
-	int res = 0;
-	char sta_mac[6] = {0, 0, 0, 0, 0, 0};
-	char cmd_buf[256];
-	char *str_ptr = cmd_buf;
-
-	WL_SOFTAP((">>%s called\n args: info->cmd:%x,"
-		" info->flags:%x, u.data.p:%p, u.data.len:%d\n",
-		__FUNCTION__, info->cmd, info->flags,
-		wrqu->data.pointer, wrqu->data.length));
-
-	if (wrqu->data.length != 0) {
-
-		if (copy_from_user(cmd_buf, wrqu->data.pointer, wrqu->data.length)) {
-			return -EFAULT;
-		}
-
-		if (get_parameter_from_string(&str_ptr,
-			"MAC=", PTYPE_STR_HEX, sta_mac, 12) == 0) {
-			res = wl_iw_softap_deassoc_stations(dev, sta_mac);
-		} else  {
-			WL_ERROR(("ERROR: STA_MAC= token not found\n"));
-		}
-	}
-
-	return res;
-}
-#endif 
-
-#endif 
-
-#if WIRELESS_EXT < 13
-struct iw_request_info
-{
-	__u16		cmd;		
-	__u16		flags;		
-};
-
-typedef int (*iw_handler)(struct net_device *dev,
-                struct iw_request_info *info,
-                void *wrqu,
-                char *extra);
-#endif 
-
-static int
-wl_iw_config_commit(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	void *zwrq,
-	char *extra
-)
-{
-	wlc_ssid_t ssid;
-	int error;
-	struct sockaddr bssid;
-
-	WL_TRACE(("%s: SIOCSIWCOMMIT\n", dev->name));
-
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_SSID, &ssid, sizeof(ssid))))
-		return error;
-
-	ssid.SSID_len = dtoh32(ssid.SSID_len);
-
-	if (!ssid.SSID_len)
-		return 0;
-
-	bzero(&bssid, sizeof(struct sockaddr));
-	if ((error = dev_wlc_ioctl(dev, WLC_REASSOC, &bssid, ETHER_ADDR_LEN))) {
-		WL_ERROR(("%s: WLC_REASSOC to %s failed \n", __FUNCTION__, ssid.SSID));
-		return error;
-	}
-
-	return 0;
-}
-
-static int
-wl_iw_get_name(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	char *cwrq,
-	char *extra
-)
-{
-	WL_TRACE(("%s: SIOCGIWNAME\n", dev->name));
-
-	strcpy(cwrq, "IEEE 802.11-DS");
-
-	return 0;
-}
-
-static int
-wl_iw_set_freq(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_freq *fwrq,
-	char *extra
-)
-{
-	int error, chan;
-	uint sf = 0;
-
-	WL_TRACE(("%s %s: SIOCSIWFREQ\n", __FUNCTION__, dev->name));
-
-#if defined(SOFTAP)
-	if (ap_cfg_running) {
-		WL_TRACE(("%s:>> not executed, 'SOFT_AP is active' \n", __FUNCTION__));
-		return 0;
-	}
-#endif
-
-	
-	if (fwrq->e == 0 && fwrq->m < MAXCHANNEL) {
-		chan = fwrq->m;
-	}
-	
-	else {
-		
-		if (fwrq->e >= 6) {
-			fwrq->e -= 6;
-			while (fwrq->e--)
-				fwrq->m *= 10;
-		} else if (fwrq->e < 6) {
-			while (fwrq->e++ < 6)
-				fwrq->m /= 10;
-		}
-		
-		if (fwrq->m > 4000 && fwrq->m < 5000)
-			sf = WF_CHAN_FACTOR_4_G; 
-
-		chan = wf_mhz2channel(fwrq->m, sf);
-	}
-
-	chan = htod32(chan);
-
-	if ((error = dev_wlc_ioctl(dev, WLC_SET_CHANNEL, &chan, sizeof(chan))))
-		return error;
-
-	g_wl_iw_params.target_channel = chan;
-
-	
-	return -EINPROGRESS;
-}
-
-static int
-wl_iw_get_freq(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_freq *fwrq,
-	char *extra
-)
-{
-	channel_info_t ci;
-	int error;
-
-	WL_TRACE(("%s: SIOCGIWFREQ\n", dev->name));
-
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_CHANNEL, &ci, sizeof(ci))))
-		return error;
-
-	
-	fwrq->m = dtoh32(ci.hw_channel);
-	fwrq->e = dtoh32(0);
-	return 0;
-}
-
-static int
-wl_iw_set_mode(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	__u32 *uwrq,
-	char *extra
-)
-{
-	int infra = 0, ap = 0, error = 0;
-
-	WL_TRACE(("%s: SIOCSIWMODE\n", dev->name));
-
-	switch (*uwrq) {
-	case IW_MODE_MASTER:
-		infra = ap = 1;
-		break;
-	case IW_MODE_ADHOC:
-	case IW_MODE_AUTO:
-		break;
-	case IW_MODE_INFRA:
-		infra = 1;
-		break;
-	default:
-		return -EINVAL;
-	}
-	infra = htod32(infra);
-	ap = htod32(ap);
-
-	if ((error = dev_wlc_ioctl(dev, WLC_SET_INFRA, &infra, sizeof(infra))) ||
-	    (error = dev_wlc_ioctl(dev, WLC_SET_AP, &ap, sizeof(ap))))
-		return error;
-
-	
-	return -EINPROGRESS;
-}
-
-static int
-wl_iw_get_mode(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	__u32 *uwrq,
-	char *extra
-)
-{
-	int error, infra = 0, ap = 0;
-
-	WL_TRACE(("%s: SIOCGIWMODE\n", dev->name));
-
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_INFRA, &infra, sizeof(infra))) ||
-	    (error = dev_wlc_ioctl(dev, WLC_GET_AP, &ap, sizeof(ap))))
-		return error;
-
-	infra = dtoh32(infra);
-	ap = dtoh32(ap);
-	*uwrq = infra ? ap ? IW_MODE_MASTER : IW_MODE_INFRA : IW_MODE_ADHOC;
-
-	return 0;
-}
-
-static int
-wl_iw_get_range(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	struct iw_range *range = (struct iw_range *) extra;
-	wl_uint32_list_t *list;
-	wl_rateset_t rateset;
-	int8 *channels;
-	int error, i, k;
-	uint sf, ch;
-
-	int phytype;
-	int bw_cap = 0, sgi_tx = 0, nmode = 0;
-	channel_info_t ci;
-	uint8 nrate_list2copy = 0;
-	uint16 nrate_list[4][8] = { {13, 26, 39, 52, 78, 104, 117, 130},
-		{14, 29, 43, 58, 87, 116, 130, 144},
-		{27, 54, 81, 108, 162, 216, 243, 270},
-		{30, 60, 90, 120, 180, 240, 270, 300}};
-
-	WL_TRACE(("%s: SIOCGIWRANGE\n", dev->name));
-
-	if (!extra)
-		return -EINVAL;
-
-	channels = kmalloc((MAXCHANNEL+1)*4, GFP_KERNEL);
-	if (!channels) {
-		WL_ERROR(("Could not alloc channels\n"));
-		return -ENOMEM;
-	}
-	list = (wl_uint32_list_t *)channels;
-
-	dwrq->length = sizeof(struct iw_range);
-	memset(range, 0, sizeof(range));
-
-	
-	range->min_nwid = range->max_nwid = 0;
-
-	
-	list->count = htod32(MAXCHANNEL);
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_VALID_CHANNELS, channels, (MAXCHANNEL+1)*4))) {
-		kfree(channels);
-		return error;
-	}
-	for (i = 0; i < dtoh32(list->count) && i < IW_MAX_FREQUENCIES; i++) {
-		range->freq[i].i = dtoh32(list->element[i]);
-
-		ch = dtoh32(list->element[i]);
-		if (ch <= CH_MAX_2G_CHANNEL)
-			sf = WF_CHAN_FACTOR_2_4_G;
-		else
-			sf = WF_CHAN_FACTOR_5_G;
-
-		range->freq[i].m = wf_channel2mhz(ch, sf);
-		range->freq[i].e = 6;
-	}
-	range->num_frequency = range->num_channels = i;
-
-	
-	range->max_qual.qual = 5;
-	
-	range->max_qual.level = 0x100 - 200;	
-	
-	range->max_qual.noise = 0x100 - 200;	
-	
-	range->sensitivity = 65535;
-
-#if WIRELESS_EXT > 11
-	
-	range->avg_qual.qual = 3;
-	
-	range->avg_qual.level = 0x100 + WL_IW_RSSI_GOOD;
-	
-	range->avg_qual.noise = 0x100 - 75;	
-#endif 
-
-	
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_CURR_RATESET, &rateset, sizeof(rateset)))) {
-		kfree(channels);
-		return error;
-	}
-	rateset.count = dtoh32(rateset.count);
-	range->num_bitrates = rateset.count;
-	for (i = 0; i < rateset.count && i < IW_MAX_BITRATES; i++)
-		range->bitrate[i] = (rateset.rates[i]& 0x7f) * 500000; 
-	dev_wlc_intvar_get(dev, "nmode", &nmode);
-	dev_wlc_ioctl(dev, WLC_GET_PHYTYPE, &phytype, sizeof(phytype));
-
-	if (nmode == 1 && phytype == WLC_PHY_TYPE_SSN) {
-		dev_wlc_intvar_get(dev, "mimo_bw_cap", &bw_cap);
-		dev_wlc_intvar_get(dev, "sgi_tx", &sgi_tx);
-		dev_wlc_ioctl(dev, WLC_GET_CHANNEL, &ci, sizeof(channel_info_t));
-		ci.hw_channel = dtoh32(ci.hw_channel);
-
-		if (bw_cap == 0 ||
-			(bw_cap == 2 && ci.hw_channel <= 14)) {
-			if (sgi_tx == 0)
-				nrate_list2copy = 0;
-			else
-				nrate_list2copy = 1;
-		}
-		if (bw_cap == 1 ||
-			(bw_cap == 2 && ci.hw_channel >= 36)) {
-			if (sgi_tx == 0)
-				nrate_list2copy = 2;
-			else
-				nrate_list2copy = 3;
-		}
-		range->num_bitrates += 8;
-		for (k = 0; i < range->num_bitrates; k++, i++) {
-			
-			range->bitrate[i] = (nrate_list[nrate_list2copy][k]) * 500000;
-		}
-	}
-
-	
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_PHYTYPE, &i, sizeof(i)))) {
-		kfree(channels);
-		return error;
-	}
-	i = dtoh32(i);
-	if (i == WLC_PHY_TYPE_A)
-		range->throughput = 24000000;	
-	else
-		range->throughput = 1500000;	
-
-	
-	range->min_rts = 0;
-	range->max_rts = 2347;
-	range->min_frag = 256;
-	range->max_frag = 2346;
-
-	range->max_encoding_tokens = DOT11_MAX_DEFAULT_KEYS;
-	range->num_encoding_sizes = 4;
-	range->encoding_size[0] = WEP1_KEY_SIZE;
-	range->encoding_size[1] = WEP128_KEY_SIZE;
-#if WIRELESS_EXT > 17
-	range->encoding_size[2] = TKIP_KEY_SIZE;
-#else
-	range->encoding_size[2] = 0;
-#endif
-	range->encoding_size[3] = AES_KEY_SIZE;
-
-	
-	range->min_pmp = 0;
-	range->max_pmp = 0;
-	range->min_pmt = 0;
-	range->max_pmt = 0;
-	range->pmp_flags = 0;
-	range->pm_capa = 0;
-
-	
-	range->num_txpower = 2;
-	range->txpower[0] = 1;
-	range->txpower[1] = 255;
-	range->txpower_capa = IW_TXPOW_MWATT;
-
-#if WIRELESS_EXT > 10
-	range->we_version_compiled = WIRELESS_EXT;
-	range->we_version_source = 19;
-
-	
-	range->retry_capa = IW_RETRY_LIMIT;
-	range->retry_flags = IW_RETRY_LIMIT;
-	range->r_time_flags = 0;
-	
-	range->min_retry = 1;
-	range->max_retry = 255;
-	
-	range->min_r_time = 0;
-	range->max_r_time = 0;
-#endif 
-
-#if WIRELESS_EXT > 17
-	range->enc_capa = IW_ENC_CAPA_WPA;
-	range->enc_capa |= IW_ENC_CAPA_CIPHER_TKIP;
-	range->enc_capa |= IW_ENC_CAPA_CIPHER_CCMP;
-	range->enc_capa |= IW_ENC_CAPA_WPA2;
-
-	
-	IW_EVENT_CAPA_SET_KERNEL(range->event_capa);
-	
-	IW_EVENT_CAPA_SET(range->event_capa, SIOCGIWAP);
-	IW_EVENT_CAPA_SET(range->event_capa, SIOCGIWSCAN);
-	IW_EVENT_CAPA_SET(range->event_capa, IWEVTXDROP);
-	IW_EVENT_CAPA_SET(range->event_capa, IWEVMICHAELMICFAILURE);
-	IW_EVENT_CAPA_SET(range->event_capa, IWEVPMKIDCAND);
-#endif 
-
-	kfree(channels);
-
-	return 0;
-}
-
-static int
-rssi_to_qual(int rssi)
-{
-	if (rssi <= WL_IW_RSSI_NO_SIGNAL)
-		return 0;
-	else if (rssi <= WL_IW_RSSI_VERY_LOW)
-		return 1;
-	else if (rssi <= WL_IW_RSSI_LOW)
-		return 2;
-	else if (rssi <= WL_IW_RSSI_GOOD)
-		return 3;
-	else if (rssi <= WL_IW_RSSI_VERY_GOOD)
-		return 4;
-	else
-		return 5;
-}
-
-static int
-wl_iw_set_spy(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	wl_iw_t *iw = NETDEV_PRIV(dev);
-	struct sockaddr *addr = (struct sockaddr *) extra;
-	int i;
-
-	WL_TRACE(("%s: SIOCSIWSPY\n", dev->name));
-
-	if (!extra)
-		return -EINVAL;
-
-	iw->spy_num = MIN(ARRAYSIZE(iw->spy_addr), dwrq->length);
-	for (i = 0; i < iw->spy_num; i++)
-		memcpy(&iw->spy_addr[i], addr[i].sa_data, ETHER_ADDR_LEN);
-	memset(iw->spy_qual, 0, sizeof(iw->spy_qual));
-
-	return 0;
-}
-
-static int
-wl_iw_get_spy(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	wl_iw_t *iw = NETDEV_PRIV(dev);
-	struct sockaddr *addr = (struct sockaddr *) extra;
-	struct iw_quality *qual = (struct iw_quality *) &addr[iw->spy_num];
-	int i;
-
-	WL_TRACE(("%s: SIOCGIWSPY\n", dev->name));
-
-	if (!extra)
-		return -EINVAL;
-
-	dwrq->length = iw->spy_num;
-	for (i = 0; i < iw->spy_num; i++) {
-		memcpy(addr[i].sa_data, &iw->spy_addr[i], ETHER_ADDR_LEN);
-		addr[i].sa_family = AF_UNIX;
-		memcpy(&qual[i], &iw->spy_qual[i], sizeof(struct iw_quality));
-		iw->spy_qual[i].updated = 0;
-	}
-
-	return 0;
-}
-
-
-static int
-wl_iw_ch_to_chanspec(int ch, wl_join_params_t *join_params, int *join_params_size)
-{
-	chanspec_t chanspec = 0;
-
-	if (ch != 0) {
-		
-		join_params->params.chanspec_num = 1;
-		join_params->params.chanspec_list[0] = ch;
-
-		if (join_params->params.chanspec_list[0])
-			chanspec |= WL_CHANSPEC_BAND_2G;
-		else
-			chanspec |= WL_CHANSPEC_BAND_5G;
-
-		chanspec |= WL_CHANSPEC_BW_20;
-		chanspec |= WL_CHANSPEC_CTL_SB_NONE;
-
-		
-		*join_params_size += WL_ASSOC_PARAMS_FIXED_SIZE +
-			join_params->params.chanspec_num * sizeof(chanspec_t);
-
-		
-		join_params->params.chanspec_list[0]  &= WL_CHANSPEC_CHAN_MASK;
-		join_params->params.chanspec_list[0] |= chanspec;
-		join_params->params.chanspec_list[0] =
-		        htodchanspec(join_params->params.chanspec_list[0]);
-
-		join_params->params.chanspec_num = htod32(join_params->params.chanspec_num);
-
-		WL_TRACE(("%s  join_params->params.chanspec_list[0]= %X\n",
-			__FUNCTION__, join_params->params.chanspec_list[0]));
-	}
-	return 1;
-}
-
-static int
-wl_iw_set_wap(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct sockaddr *awrq,
-	char *extra
-)
-{
-	int error = -EINVAL;
-	wl_join_params_t join_params;
-	int join_params_size;
-
-	WL_TRACE(("%s: SIOCSIWAP\n", dev->name));
-
-	if (awrq->sa_family != ARPHRD_ETHER) {
-		WL_ERROR(("Invalid Header...sa_family\n"));
-		return -EINVAL;
-	}
-
-	
-	if (ETHER_ISBCAST(awrq->sa_data) || ETHER_ISNULLADDR(awrq->sa_data)) {
-		scb_val_t scbval;
-		
-		bzero(&scbval, sizeof(scb_val_t));
-		
-		(void) dev_wlc_ioctl(dev, WLC_DISASSOC, &scbval, sizeof(scb_val_t));
-		return 0;
-	}
-
-
-	
-	memset(&join_params, 0, sizeof(join_params));
-	join_params_size = sizeof(join_params.ssid);
-
-	memcpy(join_params.ssid.SSID, g_ssid.SSID, g_ssid.SSID_len);
-	join_params.ssid.SSID_len = htod32(g_ssid.SSID_len);
-	memcpy(&join_params.params.bssid, awrq->sa_data, ETHER_ADDR_LEN);
-
-	
-	
-	WL_TRACE(("%s  target_channel=%d\n", __FUNCTION__, g_wl_iw_params.target_channel));
-	wl_iw_ch_to_chanspec(g_wl_iw_params.target_channel, &join_params, &join_params_size);
-
-	if ((error = dev_wlc_ioctl(dev, WLC_SET_SSID, &join_params, join_params_size))) {
-		WL_ERROR(("%s Invalid ioctl data=%d\n", __FUNCTION__, error));
-		return error;
-	}
-
-	if (g_ssid.SSID_len) {
-		WL_TRACE(("%s: join SSID=%s BSSID="MACSTR" ch=%d\n", __FUNCTION__,
-			g_ssid.SSID, MAC2STR((u8 *)awrq->sa_data),
-			g_wl_iw_params.target_channel));
-	}
-
-	
-	memset(&g_ssid, 0, sizeof(g_ssid));
-	return 0;
-}
-
-static int
-wl_iw_get_wap(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct sockaddr *awrq,
-	char *extra
-)
-{
-	WL_TRACE(("%s: SIOCGIWAP\n", dev->name));
-
-	awrq->sa_family = ARPHRD_ETHER;
-	memset(awrq->sa_data, 0, ETHER_ADDR_LEN);
-
-	
-	(void) dev_wlc_ioctl(dev, WLC_GET_BSSID, awrq->sa_data, ETHER_ADDR_LEN);
-
-	return 0;
-}
-
-#if WIRELESS_EXT > 17
-static int
-wl_iw_mlme(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct sockaddr *awrq,
-	char *extra
-)
-{
-	struct iw_mlme *mlme;
-	scb_val_t scbval;
-	int error  = -EINVAL;
-
-	WL_TRACE(("%s: SIOCSIWMLME DISASSOC/DEAUTH\n", dev->name));
-
-	mlme = (struct iw_mlme *)extra;
-	if (mlme == NULL) {
-		WL_ERROR(("Invalid ioctl data.\n"));
-		return error;
-	}
-
-	scbval.val = mlme->reason_code;
-	bcopy(&mlme->addr.sa_data, &scbval.ea, ETHER_ADDR_LEN);
-
-	if (mlme->cmd == IW_MLME_DISASSOC) {
-		scbval.val = htod32(scbval.val);
-		error = dev_wlc_ioctl(dev, WLC_DISASSOC, &scbval, sizeof(scb_val_t));
-	}
-	else if (mlme->cmd == IW_MLME_DEAUTH) {
-		scbval.val = htod32(scbval.val);
-		error = dev_wlc_ioctl(dev, WLC_SCB_DEAUTHENTICATE_FOR_REASON, &scbval,
-			sizeof(scb_val_t));
-	}
-	else {
-		WL_ERROR(("Invalid ioctl data.\n"));
-		return error;
-	}
-
-	return error;
-}
-#endif 
-
-#ifndef WL_IW_USE_ISCAN
-static int
-wl_iw_get_aplist(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	wl_scan_results_t *list;
-	struct sockaddr *addr = (struct sockaddr *) extra;
-	struct iw_quality qual[IW_MAX_AP];
-	wl_bss_info_t *bi = NULL;
-	int error, i;
-	uint buflen = dwrq->length;
-
-	WL_TRACE(("%s: SIOCGIWAPLIST\n", dev->name));
-
-	if (!extra)
-		return -EINVAL;
-
-	
-	list = kmalloc(buflen, GFP_KERNEL);
-	if (!list)
-		return -ENOMEM;
-	memset(list, 0, buflen);
-	list->buflen = htod32(buflen);
-	if ((error = dev_wlc_ioctl(dev, WLC_SCAN_RESULTS, list, buflen))) {
-		WL_ERROR(("%d: Scan results error %d\n", __LINE__, error));
-		kfree(list);
-		return error;
-	}
-	list->buflen = dtoh32(list->buflen);
-	list->version = dtoh32(list->version);
-	list->count = dtoh32(list->count);
-	if (list->version != WL_BSS_INFO_VERSION) {
-		WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
-		          __FUNCTION__, list->version));
-		kfree(list);
-		return -EINVAL;
-	}
-
-	for (i = 0, dwrq->length = 0; i < list->count && dwrq->length < IW_MAX_AP; i++) {
-		bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) : list->bss_info;
-		ASSERT(((uintptr)bi + dtoh32(bi->length)) <= ((uintptr)list +
-			buflen));
-
-		
-		if (!(dtoh16(bi->capability) & DOT11_CAP_ESS))
-			continue;
-
-		
-		memcpy(addr[dwrq->length].sa_data, &bi->BSSID, ETHER_ADDR_LEN);
-		addr[dwrq->length].sa_family = ARPHRD_ETHER;
-		qual[dwrq->length].qual = rssi_to_qual(dtoh16(bi->RSSI));
-		qual[dwrq->length].level = 0x100 + dtoh16(bi->RSSI);
-		qual[dwrq->length].noise = 0x100 + bi->phy_noise;
-
-		
-#if WIRELESS_EXT > 18
-		qual[dwrq->length].updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
-#else
-		qual[dwrq->length].updated = 7;
-#endif 
-
-		dwrq->length++;
-	}
-
-	kfree(list);
-
-	if (dwrq->length) {
-		memcpy(&addr[dwrq->length], qual, sizeof(struct iw_quality) * dwrq->length);
-		
-		dwrq->flags = 1;
-	}
-
-	return 0;
-}
-#endif 
-
-#ifdef WL_IW_USE_ISCAN
-static int
-wl_iw_iscan_get_aplist(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	wl_scan_results_t *list;
-	iscan_buf_t * buf;
-	iscan_info_t *iscan = g_iscan;
-
-	struct sockaddr *addr = (struct sockaddr *) extra;
-	struct iw_quality qual[IW_MAX_AP];
-	wl_bss_info_t *bi = NULL;
-	int i;
-
-	WL_TRACE(("%s: SIOCGIWAPLIST\n", dev->name));
-
-	if (!extra)
-		return -EINVAL;
-
-	if ((!iscan) || (iscan->tsk_ctl.thr_pid < 0)) {
-		WL_ERROR(("%s error\n", __FUNCTION__));
-		return 0;
-	}
-
-	buf = iscan->list_hdr;
-	
-	while (buf) {
-		list = &((wl_iscan_results_t*)buf->iscan_buf)->results;
-		if (list->version != WL_BSS_INFO_VERSION) {
-			WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
-				__FUNCTION__, list->version));
-			return -EINVAL;
-		}
-
-		bi = NULL;
-		for (i = 0, dwrq->length = 0; i < list->count && dwrq->length < IW_MAX_AP; i++) {
-			bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length))
-			          : list->bss_info;
-			ASSERT(((uintptr)bi + dtoh32(bi->length)) <= ((uintptr)list +
-				WLC_IW_ISCAN_MAXLEN));
-
-			
-			if (!(dtoh16(bi->capability) & DOT11_CAP_ESS))
-				continue;
-
-			
-			memcpy(addr[dwrq->length].sa_data, &bi->BSSID, ETHER_ADDR_LEN);
-			addr[dwrq->length].sa_family = ARPHRD_ETHER;
-			qual[dwrq->length].qual = rssi_to_qual(dtoh16(bi->RSSI));
-			qual[dwrq->length].level = 0x100 + dtoh16(bi->RSSI);
-			qual[dwrq->length].noise = 0x100 + bi->phy_noise;
-
-			
-#if WIRELESS_EXT > 18
-			qual[dwrq->length].updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
-#else
-			qual[dwrq->length].updated = 7;
-#endif 
-
-			dwrq->length++;
-		}
-		buf = buf->next;
-	}
-	if (dwrq->length) {
-		memcpy(&addr[dwrq->length], qual, sizeof(struct iw_quality) * dwrq->length);
-		
-		dwrq->flags = 1;
-	}
-
-	return 0;
-}
-
-static int
-wl_iw_iscan_prep(wl_scan_params_t *params, wlc_ssid_t *ssid)
-{
-	int err = 0;
-
-	memcpy(&params->bssid, &ether_bcast, ETHER_ADDR_LEN);
-	params->bss_type = DOT11_BSSTYPE_ANY;
-	params->scan_type = 0;
-	params->nprobes = -1;
-	params->active_time = -1;
-	params->passive_time = -1;
-	params->home_time = -1;
-	params->channel_num = 0;
-
-#if defined(CONFIG_FIRST_SCAN)
-	
-	if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_STARTED)
-		params->passive_time = 30;
-#endif 
-	params->nprobes = htod32(params->nprobes);
-	params->active_time = htod32(params->active_time);
-	params->passive_time = htod32(params->passive_time);
-	params->home_time = htod32(params->home_time);
-	if (ssid && ssid->SSID_len)
-		memcpy(&params->ssid, ssid, sizeof(wlc_ssid_t));
-
-	return err;
-}
-
-static int
-wl_iw_iscan(iscan_info_t *iscan, wlc_ssid_t *ssid, uint16 action)
-{
-	int err = 0;
-
-	iscan->iscan_ex_params_p->version = htod32(ISCAN_REQ_VERSION);
-	iscan->iscan_ex_params_p->action = htod16(action);
-	iscan->iscan_ex_params_p->scan_duration = htod16(0);
-
-	WL_SCAN(("%s : nprobes=%d\n", __FUNCTION__, iscan->iscan_ex_params_p->params.nprobes));
-	WL_SCAN(("active_time=%d\n", iscan->iscan_ex_params_p->params.active_time));
-	WL_SCAN(("passive_time=%d\n", iscan->iscan_ex_params_p->params.passive_time));
-	WL_SCAN(("home_time=%d\n", iscan->iscan_ex_params_p->params.home_time));
-	WL_SCAN(("scan_type=%d\n", iscan->iscan_ex_params_p->params.scan_type));
-	WL_SCAN(("bss_type=%d\n", iscan->iscan_ex_params_p->params.bss_type));
-
-	if ((dev_iw_iovar_setbuf(iscan->dev, "iscan", iscan->iscan_ex_params_p,
-		iscan->iscan_ex_param_size, iscan->ioctlbuf, sizeof(iscan->ioctlbuf)))) {
-			WL_ERROR(("Set ISCAN for %s failed with %d\n", __FUNCTION__, err));
-			err = -1;
-	}
-
-	return err;
-}
-
-static void
-wl_iw_timerfunc(ulong data)
-{
-	iscan_info_t *iscan = (iscan_info_t *)data;
-	if (iscan) {
-		iscan->timer_on = 0;
-		if (iscan->iscan_state != ISCAN_STATE_IDLE) {
-			WL_TRACE(("timer trigger\n"));
-			up(&iscan->tsk_ctl.sema);
-		}
-	}
-}
-
-static void
-wl_iw_set_event_mask(struct net_device *dev)
-{
-	char eventmask[WL_EVENTING_MASK_LEN];
-	char iovbuf[WL_EVENTING_MASK_LEN + 12];	
-
-	dev_iw_iovar_getbuf(dev, "event_msgs", "", 0, iovbuf, sizeof(iovbuf));
-	bcopy(iovbuf, eventmask, WL_EVENTING_MASK_LEN);
-	setbit(eventmask, WLC_E_SCAN_COMPLETE);
-	dev_iw_iovar_setbuf(dev, "event_msgs", eventmask, WL_EVENTING_MASK_LEN,
-		iovbuf, sizeof(iovbuf));
-}
-
-static uint32
-wl_iw_iscan_get(iscan_info_t *iscan)
-{
-	iscan_buf_t * buf;
-	iscan_buf_t * ptr;
-	wl_iscan_results_t * list_buf;
-	wl_iscan_results_t list;
-	wl_scan_results_t *results;
-	uint32 status;
-	int res = 0;
-
-	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
-	if (iscan->list_cur) {
-		buf = iscan->list_cur;
-		iscan->list_cur = buf->next;
-	}
-	else {
-		buf = kmalloc(sizeof(iscan_buf_t), GFP_KERNEL);
-		if (!buf) {
-			WL_ERROR(("%s can't alloc iscan_buf_t : going to abort currect iscan\n",
-			          __FUNCTION__));
-			DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
-			return WL_SCAN_RESULTS_NO_MEM;
-		}
-		buf->next = NULL;
-		if (!iscan->list_hdr)
-			iscan->list_hdr = buf;
-		else {
-			ptr = iscan->list_hdr;
-			while (ptr->next) {
-				ptr = ptr->next;
-			}
-			ptr->next = buf;
-		}
-	}
-	memset(buf->iscan_buf, 0, WLC_IW_ISCAN_MAXLEN);
-	list_buf = (wl_iscan_results_t*)buf->iscan_buf;
-	results = &list_buf->results;
-	results->buflen = WL_ISCAN_RESULTS_FIXED_SIZE;
-	results->version = 0;
-	results->count = 0;
-
-	memset(&list, 0, sizeof(list));
-	list.results.buflen = htod32(WLC_IW_ISCAN_MAXLEN);
-	res = dev_iw_iovar_getbuf(
-		iscan->dev,
-		"iscanresults",
-		&list,
-		WL_ISCAN_RESULTS_FIXED_SIZE,
-		buf->iscan_buf,
-		WLC_IW_ISCAN_MAXLEN);
-	if (res == 0) {
-		results->buflen = dtoh32(results->buflen);
-		results->version = dtoh32(results->version);
-		results->count = dtoh32(results->count);
-		WL_TRACE(("results->count = %d\n", results->count));
-		WL_TRACE(("results->buflen = %d\n", results->buflen));
-		status = dtoh32(list_buf->status);
-	} else {
-		WL_ERROR(("%s returns error %d\n", __FUNCTION__, res));
-		
-		status = WL_SCAN_RESULTS_NO_MEM;
-	}
-	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
-	return status;
-}
-
-static void
-wl_iw_force_specific_scan(iscan_info_t *iscan)
-{
-	WL_TRACE(("%s force Specific SCAN for %s\n", __FUNCTION__, g_specific_ssid.SSID));
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-	rtnl_lock();
-#endif
-
-	(void) dev_wlc_ioctl(iscan->dev, WLC_SCAN, &g_specific_ssid, sizeof(g_specific_ssid));
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-	rtnl_unlock();
-#endif
-}
-
-static void
-wl_iw_send_scan_complete(iscan_info_t *iscan)
-{
-	union iwreq_data wrqu;
-
-	memset(&wrqu, 0, sizeof(wrqu));
-
-	
-	wireless_send_event(iscan->dev, SIOCGIWSCAN, &wrqu, NULL);
-#if defined(CONFIG_FIRST_SCAN)
-		if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_STARTED)
-			g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_READY;
-#endif 
-		WL_TRACE(("Send Event ISCAN complete\n"));
-}
-
-static int
-_iscan_sysioc_thread(void *data)
-{
-	uint32 status;
-
-	tsk_ctl_t *tsk_ctl = (tsk_ctl_t *)data;
-	iscan_info_t *iscan = (iscan_info_t *) tsk_ctl->parent;
-
-
-	static bool iscan_pass_abort = FALSE;
-
-	DAEMONIZE("iscan_sysioc");
-
-	status = WL_SCAN_RESULTS_PARTIAL;
-
-	
-	complete(&tsk_ctl->completed);
-
-	while (down_interruptible(&tsk_ctl->sema) == 0) {
-
-		SMP_RD_BARRIER_DEPENDS();
-		if (tsk_ctl->terminated) {
-			break;
-		}
-#if defined(SOFTAP)
-		
-		if (ap_cfg_running) {
-		 WL_TRACE(("%s skipping SCAN ops in AP mode !!!\n", __FUNCTION__));
-		 net_os_wake_unlock(iscan->dev);
-		 continue;
-		}
-#endif 
-
-		if (iscan->timer_on) {
-			
-			iscan->timer_on = 0;
-			del_timer_sync(&iscan->timer);
-		}
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-		rtnl_lock();
-#endif
-		status = wl_iw_iscan_get(iscan);
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-		rtnl_unlock();
-#endif
-
-	if  (g_scan_specified_ssid && (iscan_pass_abort == TRUE)) {
-		WL_TRACE(("%s Get results from specific scan status=%d\n", __FUNCTION__, status));
-			wl_iw_send_scan_complete(iscan);
-			iscan_pass_abort = FALSE;
-			status  = -1;
-		}
-
-		switch (status) {
-			case WL_SCAN_RESULTS_PARTIAL:
-				WL_TRACE(("iscanresults incomplete\n"));
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-				rtnl_lock();
-#endif
-				
-				wl_iw_iscan(iscan, NULL, WL_SCAN_ACTION_CONTINUE);
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-				rtnl_unlock();
-#endif
-				
-				mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
-				iscan->timer_on = 1;
-				break;
-			case WL_SCAN_RESULTS_SUCCESS:
-				WL_TRACE(("iscanresults complete\n"));
-				iscan->iscan_state = ISCAN_STATE_IDLE;
-				wl_iw_send_scan_complete(iscan);
-				break;
-			case WL_SCAN_RESULTS_PENDING:
-				WL_TRACE(("iscanresults pending\n"));
-				
-				mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
-				iscan->timer_on = 1;
-				break;
-			case WL_SCAN_RESULTS_ABORTED:
-				WL_TRACE(("iscanresults aborted\n"));
-				iscan->iscan_state = ISCAN_STATE_IDLE;
-				if (g_scan_specified_ssid == 0)
-					wl_iw_send_scan_complete(iscan);
-				else {
-					iscan_pass_abort = TRUE;
-					wl_iw_force_specific_scan(iscan);
-				}
-				break;
-			case WL_SCAN_RESULTS_NO_MEM:
-				WL_TRACE(("iscanresults can't alloc memory: skip\n"));
-				iscan->iscan_state = ISCAN_STATE_IDLE;
-				break;
-			default:
-				WL_TRACE(("iscanresults returned unknown status %d\n", status));
-				break;
-		 }
-
-		net_os_wake_unlock(iscan->dev);
-	}
-
-	if (iscan->timer_on) {
-		iscan->timer_on = 0;
-		del_timer_sync(&iscan->timer);
-	}
-	complete_and_exit(&tsk_ctl->completed, 0);
-}
-#endif 
-
-#if !defined(CSCAN)
-
-static void
-wl_iw_set_ss_cache_timer_flag(void)
-{
-	g_ss_cache_ctrl.m_timer_expired = 1;
-	WL_TRACE(("%s called\n", __FUNCTION__));
-}
-
-
-static int
-wl_iw_init_ss_cache_ctrl(void)
-{
-	WL_TRACE(("%s :\n", __FUNCTION__));
-	g_ss_cache_ctrl.m_prev_scan_mode = 0;
-	g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
-	g_ss_cache_ctrl.m_cache_head = NULL;
-	g_ss_cache_ctrl.m_link_down = 0;
-	g_ss_cache_ctrl.m_timer_expired = 0;
-	memset(g_ss_cache_ctrl.m_active_bssid, 0, ETHER_ADDR_LEN);
-
-	g_ss_cache_ctrl.m_timer = kmalloc(sizeof(struct timer_list), GFP_KERNEL);
-	if (!g_ss_cache_ctrl.m_timer) {
-		return -ENOMEM;
-	}
-	g_ss_cache_ctrl.m_timer->function = (void *)wl_iw_set_ss_cache_timer_flag;
-	init_timer(g_ss_cache_ctrl.m_timer);
-
-	return 0;
-}
-
-
-
-static void
-wl_iw_free_ss_cache(void)
-{
-	wl_iw_ss_cache_t *node, *cur;
-	wl_iw_ss_cache_t **spec_scan_head;
-
-	WL_TRACE(("%s called\n", __FUNCTION__));
-
-	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
-	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
-	node = *spec_scan_head;
-
-	for (;node;) {
-		WL_TRACE(("%s : SSID - %s\n", __FUNCTION__, node->bss_info->SSID));
-		cur = node;
-		node = cur->next;
-		kfree(cur);
-	}
-	*spec_scan_head = NULL;
-	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
-}
-
-
-
-static int
-wl_iw_run_ss_cache_timer(int kick_off)
-{
-	struct timer_list **timer;
-
-	timer = &g_ss_cache_ctrl.m_timer;
-
-	if (*timer) {
-		if (kick_off) {
-#ifdef CONFIG_PRESCANNED
-			(*timer)->expires = jiffies + 70000 * HZ / 1000;
-#else
-			(*timer)->expires = jiffies + 30000 * HZ / 1000;	
-#endif
-			add_timer(*timer);
-			WL_TRACE(("%s : timer starts \n", __FUNCTION__));
-		} else {
-			del_timer_sync(*timer);
-			WL_TRACE(("%s : timer stops \n", __FUNCTION__));
-		}
-	}
-
-	return 0;
-}
-
-
-static void
-wl_iw_release_ss_cache_ctrl(void)
-{
-	WL_TRACE(("%s :\n", __FUNCTION__));
-	wl_iw_free_ss_cache();
-	wl_iw_run_ss_cache_timer(0);
-	if (g_ss_cache_ctrl.m_timer) {
-		kfree(g_ss_cache_ctrl.m_timer);
-	}
-}
-
-
-
-static void
-wl_iw_reset_ss_cache(void)
-{
-	wl_iw_ss_cache_t *node, *prev, *cur;
-	wl_iw_ss_cache_t **spec_scan_head;
-
-	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
-	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
-	node = *spec_scan_head;
-	prev = node;
-
-	for (;node;) {
-		WL_TRACE(("%s : node SSID %s \n", __FUNCTION__, node->bss_info->SSID));
-		if (!node->dirty) {
-			cur = node;
-			if (cur == *spec_scan_head) {
-				*spec_scan_head = cur->next;
-				prev = *spec_scan_head;
-			}
-			else {
-				prev->next = cur->next;
-			}
-			node = cur->next;
-
-			WL_TRACE(("%s : Del node : SSID %s\n", __FUNCTION__, cur->bss_info->SSID));
-			kfree(cur);
-			continue;
-		}
-
-		node->dirty = 0;
-		prev = node;
-		node = node->next;
-	}
-	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
-}
-
-
-static int
-wl_iw_add_bss_to_ss_cache(wl_scan_results_t *ss_list)
-{
-
-	wl_iw_ss_cache_t *node, *prev, *leaf;
-	wl_iw_ss_cache_t **spec_scan_head;
-	wl_bss_info_t *bi = NULL;
-	int i;
-
-	
-	if (!ss_list->count) {
-		return 0;
-	}
-
-	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
-	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
-
-	for (i = 0; i < ss_list->count; i++) {
-
-		node = *spec_scan_head;
-		prev = node;
-
-		bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) : ss_list->bss_info;
-
-		WL_TRACE(("%s : find %d with specific SSID %s\n", __FUNCTION__, i, bi->SSID));
-		for (;node;) {
-			if (!memcmp(&node->bss_info->BSSID, &bi->BSSID, ETHER_ADDR_LEN)) {
-				
-				WL_TRACE(("dirty marked : SSID %s\n", bi->SSID));
-				node->dirty = 1;
-				break;
-			}
-			prev = node;
-			node = node->next;
-		}
-
-		if (node) {
-			continue;
-		}
-
-		leaf = kmalloc(bi->length + WLC_IW_SS_CACHE_CTRL_FIELD_MAXLEN, GFP_KERNEL);
-		if (!leaf) {
-			WL_ERROR(("Memory alloc failure %d\n",
-				bi->length + WLC_IW_SS_CACHE_CTRL_FIELD_MAXLEN));
-			DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
-			return -ENOMEM;
-		}
-
-		memcpy(leaf->bss_info, bi, bi->length);
-		leaf->next = NULL;
-		leaf->dirty = 1;
-		leaf->count = 1;
-		leaf->version = ss_list->version;
-
-		if (!prev) {
-			*spec_scan_head = leaf;
-		}
-		else {
-			prev->next = leaf;
-		}
-	}
-	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
-	return 0;
-}
-
-
-static int
-wl_iw_merge_scan_cache(struct iw_request_info *info, char *extra, uint buflen_from_user,
-__u16 *merged_len)
-{
-	wl_iw_ss_cache_t *node;
-	wl_scan_results_t *list_merge;
-
-	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
-	node = g_ss_cache_ctrl.m_cache_head;
-	for (;node;) {
-		list_merge = (wl_scan_results_t *)&node->buflen;
-		WL_TRACE(("%s: Cached Specific APs list=%d\n", __FUNCTION__, list_merge->count));
-		if (buflen_from_user - *merged_len > 0) {
-			*merged_len += (__u16) wl_iw_get_scan_prep(list_merge, info,
-				extra + *merged_len, buflen_from_user - *merged_len);
-		}
-		else {
-			WL_TRACE(("%s: exit with break\n", __FUNCTION__));
-			break;
-		}
-		node = node->next;
-	}
-	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
-	return 0;
-}
-
-
-static int
-wl_iw_delete_bss_from_ss_cache(void *addr)
-{
-
-	wl_iw_ss_cache_t *node, *prev;
-	wl_iw_ss_cache_t **spec_scan_head;
-
-	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
-	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
-	node = *spec_scan_head;
-	prev = node;
-	for (;node;) {
-		if (!memcmp(&node->bss_info->BSSID, addr, ETHER_ADDR_LEN)) {
-			if (node == *spec_scan_head) {
-				*spec_scan_head = node->next;
-			}
-			else {
-				prev->next = node->next;
-			}
-
-			WL_TRACE(("%s : Del node : %s\n", __FUNCTION__, node->bss_info->SSID));
-			kfree(node);
-			break;
-		}
-
-		prev = node;
-		node = node->next;
-	}
-
-	memset(addr, 0, ETHER_ADDR_LEN);
-	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
-	return 0;
-}
-
-#endif	
-
-static int
-wl_iw_set_scan(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int error;
-	WL_TRACE(("\n:%s dev:%s: SIOCSIWSCAN : SCAN\n", __FUNCTION__, dev->name));
-
-#ifdef OEM_CHROMIUMOS
-	g_set_essid_before_scan = FALSE;
-#endif
-
-#if defined(CSCAN)
-		WL_ERROR(("%s: Scan from SIOCGIWSCAN not supported\n", __FUNCTION__));
-		return -EINVAL;
-#endif 
-
-#if defined(SOFTAP)
-	
-	if (ap_cfg_running) {
-		WL_TRACE(("\n>%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
-		return 0;
-	}
-#endif 
-
-	
-	if (g_onoff == G_WLAN_SET_OFF)
-		return 0;
-
-	
-	memset(&g_specific_ssid, 0, sizeof(g_specific_ssid));
-#ifndef WL_IW_USE_ISCAN
-	
-	g_scan_specified_ssid = 0;
-#endif 
-
-#if WIRELESS_EXT > 17
-	
-	if (wrqu->data.length == sizeof(struct iw_scan_req)) {
-		if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
-			struct iw_scan_req *req = (struct iw_scan_req *)extra;
-#if defined(CONFIG_FIRST_SCAN)
-			if (g_first_broadcast_scan != BROADCAST_SCAN_FIRST_RESULT_CONSUMED) {
-				
-				WL_TRACE(("%s Ignoring SC %s first BC is not done = %d\n",
-				          __FUNCTION__, req->essid,
-				          g_first_broadcast_scan));
-				return -EBUSY;
-			}
-#endif	
-			if (g_scan_specified_ssid) {
-				WL_TRACE(("%s Specific SCAN is not done ignore scan for = %s \n",
-					__FUNCTION__, req->essid));
-				
-				return -EBUSY;
-			}
-			else {
-				g_specific_ssid.SSID_len = MIN(sizeof(g_specific_ssid.SSID),
-				                               req->essid_len);
-				memcpy(g_specific_ssid.SSID, req->essid, g_specific_ssid.SSID_len);
-				g_specific_ssid.SSID_len = htod32(g_specific_ssid.SSID_len);
-				g_scan_specified_ssid = 1;
-				WL_TRACE(("### Specific scan ssid=%s len=%d\n",
-				          g_specific_ssid.SSID, g_specific_ssid.SSID_len));
-			}
-		}
-	}
-#endif 
-	
-	if ((error = dev_wlc_ioctl(dev, WLC_SCAN, &g_specific_ssid, sizeof(g_specific_ssid)))) {
-		WL_TRACE(("#### Set SCAN for %s failed with %d\n", g_specific_ssid.SSID, error));
-		
-		g_scan_specified_ssid = 0;
-		return -EBUSY;
-	}
-
-	return 0;
-}
-
-#ifdef WL_IW_USE_ISCAN
-int
-wl_iw_iscan_set_scan_broadcast_prep(struct net_device *dev, uint flag)
-{
-	wlc_ssid_t ssid;
-	iscan_info_t *iscan = g_iscan;
-
-#if defined(CONFIG_FIRST_SCAN)
-	
-	if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_IDLE) {
-		g_first_broadcast_scan = BROADCAST_SCAN_FIRST_STARTED;
-		WL_TRACE(("%s: First Brodcast scan was forced\n", __FUNCTION__));
-	}
-	else if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_STARTED) {
-		WL_TRACE(("%s: ignore ISCAN request first BS is not done yet\n", __FUNCTION__));
-		return 0;
-	}
-#endif 
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-	if (flag)
-		rtnl_lock();
-#endif
-
-	dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &iscan->scan_flag, sizeof(iscan->scan_flag));
-	wl_iw_set_event_mask(dev);
-
-	WL_TRACE(("+++: Set Broadcast ISCAN\n"));
-	
-	memset(&ssid, 0, sizeof(ssid));
-
-	iscan->list_cur = iscan->list_hdr;
-	iscan->iscan_state = ISCAN_STATE_SCANING;
-
-	memset(&iscan->iscan_ex_params_p->params, 0, iscan->iscan_ex_param_size);
-	wl_iw_iscan_prep(&iscan->iscan_ex_params_p->params, &ssid);
-	wl_iw_iscan(iscan, &ssid, WL_SCAN_ACTION_START);
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-	if (flag)
-		rtnl_unlock();
-#endif
-
-	mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
-
-	iscan->timer_on = 1;
-
-	return 0;
-}
-
-static int
-wl_iw_iscan_set_scan(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	wlc_ssid_t ssid;
-	iscan_info_t *iscan = g_iscan;
-	int ret = 0;
-
-	WL_TRACE_SCAN(("%s: SIOCSIWSCAN : ISCAN\n", dev->name));
-
-#if defined(CSCAN)
-		WL_ERROR(("%s: Scan from SIOCGIWSCAN not supported\n", __FUNCTION__));
-		return -EINVAL;
-#endif 
-
-	net_os_wake_lock(dev);
-
-	
-#if defined(SOFTAP)
-	if (ap_cfg_running) {
-		WL_TRACE(("\n>%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
-		goto set_scan_end;
-	}
-#endif
-	
-	if (g_onoff == G_WLAN_SET_OFF) {
-		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
-		goto set_scan_end;
-	}
-
-#ifdef PNO_SUPPORT
-	
-	if  (dhd_dev_get_pno_status(dev)) {
-		WL_ERROR(("%s: Scan called when PNO is active\n", __FUNCTION__));
-	}
-#endif 
-
-	
-	if ((!iscan) || (iscan->tsk_ctl.thr_pid < 0)) {
-		WL_ERROR(("%s error \n",  __FUNCTION__));
-		goto set_scan_end;
-	}
-
-	if (g_scan_specified_ssid) {
-		WL_TRACE(("%s Specific SCAN already running ignoring BC scan\n",
-		          __FUNCTION__));
-		ret = EBUSY;
-		goto set_scan_end;
-	}
-
-	
-	memset(&ssid, 0, sizeof(ssid));
-
-#if WIRELESS_EXT > 17
-	
-	if (wrqu->data.length == sizeof(struct iw_scan_req)) {
-		if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
-			int as = 0;
-			struct iw_scan_req *req = (struct iw_scan_req *)extra;
-			
-			ssid.SSID_len = MIN(sizeof(ssid.SSID), req->essid_len);
-			memcpy(ssid.SSID, req->essid, ssid.SSID_len);
-			ssid.SSID_len = htod32(ssid.SSID_len);
-			dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &as, sizeof(as));
-			wl_iw_set_event_mask(dev);
-			ret = wl_iw_set_scan(dev, info, wrqu, extra);
-			goto set_scan_end;
-		}
-		else {
-			g_scan_specified_ssid = 0;
-
-			if (iscan->iscan_state == ISCAN_STATE_SCANING) {
-				WL_TRACE(("%s ISCAN already in progress \n", __FUNCTION__));
-				goto set_scan_end;
-			}
-		}
-	}
-#endif 
-
-#if defined(CONFIG_FIRST_SCAN) && !defined(CSCAN)
-	if (g_first_broadcast_scan < BROADCAST_SCAN_FIRST_RESULT_CONSUMED) {
-		if (++g_first_counter_scans == MAX_ALLOWED_BLOCK_SCAN_FROM_FIRST_SCAN) {
-
-			WL_ERROR(("%s Clean up First scan flag which is %d\n",
-			          __FUNCTION__, g_first_broadcast_scan));
-			g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_CONSUMED;
-		}
-		else {
-			WL_ERROR(("%s Ignoring Broadcast Scan:First Scan is not done yet %d\n",
-			          __FUNCTION__, g_first_counter_scans));
-			ret = -EBUSY;
-			goto set_scan_end;
-		}
-	}
-#endif
-
-	wl_iw_iscan_set_scan_broadcast_prep(dev, 0);
-
-set_scan_end:
-	net_os_wake_unlock(dev);
-	return ret;
-}
-#endif 
-
-#if WIRELESS_EXT > 17
-static bool
-ie_is_wpa_ie(uint8 **wpaie, uint8 **tlvs, int *tlvs_len)
-{
-
-
-	uint8 *ie = *wpaie;
-
-	
-	if ((ie[1] >= 6) &&
-		!bcmp((const void *)&ie[2], (const void *)(WPA_OUI "\x01"), 4)) {
-		return TRUE;
-	}
-
-	
-	ie += ie[1] + 2;
-	
-	*tlvs_len -= (int)(ie - *tlvs);
-	
-	*tlvs = ie;
-	return FALSE;
-}
-
-static bool
-ie_is_wps_ie(uint8 **wpsie, uint8 **tlvs, int *tlvs_len)
-{
-
-
-	uint8 *ie = *wpsie;
-
-	
-	if ((ie[1] >= 4) &&
-		!bcmp((const void *)&ie[2], (const void *)(WPA_OUI "\x04"), 4)) {
-		return TRUE;
-	}
-
-	
-	ie += ie[1] + 2;
-	
-	*tlvs_len -= (int)(ie - *tlvs);
-	
-	*tlvs = ie;
-	return FALSE;
-}
-#endif 
-
-
-static int
-wl_iw_handle_scanresults_ies(char **event_p, char *end,
-	struct iw_request_info *info, wl_bss_info_t *bi)
-{
-#if WIRELESS_EXT > 17
-	struct iw_event	iwe;
-	char *event;
-
-	event = *event_p;
-	if (bi->ie_length) {
-		
-		bcm_tlv_t *ie;
-		uint8 *ptr = ((uint8 *)bi) + sizeof(wl_bss_info_t);
-		int ptr_len = bi->ie_length;
-
-		if ((ie = bcm_parse_tlvs(ptr, ptr_len, DOT11_MNG_RSN_ID))) {
-			iwe.cmd = IWEVGENIE;
-			iwe.u.data.length = ie->len + 2;
-			event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)ie);
-		}
-		ptr = ((uint8 *)bi) + sizeof(wl_bss_info_t);
-
-		while ((ie = bcm_parse_tlvs(ptr, ptr_len, DOT11_MNG_WPA_ID))) {
-			
-			if (ie_is_wps_ie(((uint8 **)&ie), &ptr, &ptr_len)) {
-				iwe.cmd = IWEVGENIE;
-				iwe.u.data.length = ie->len + 2;
-				event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)ie);
-				break;
-			}
-		}
-
-		ptr = ((uint8 *)bi) + sizeof(wl_bss_info_t);
-		ptr_len = bi->ie_length;
-		while ((ie = bcm_parse_tlvs(ptr, ptr_len, DOT11_MNG_WPA_ID))) {
-			if (ie_is_wpa_ie(((uint8 **)&ie), &ptr, &ptr_len)) {
-				iwe.cmd = IWEVGENIE;
-				iwe.u.data.length = ie->len + 2;
-				event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)ie);
-				break;
-			}
-		}
-
-	*event_p = event;
-	}
-#endif 
-
-	return 0;
-}
-
-#ifndef CSCAN
-static uint
-wl_iw_get_scan_prep(
-	wl_scan_results_t *list,
-	struct iw_request_info *info,
-	char *extra,
-	short max_size)
-{
-	int  i, j;
-	struct iw_event  iwe;
-	wl_bss_info_t *bi = NULL;
-	char *event = extra, *end = extra + max_size - WE_ADD_EVENT_FIX, *value;
-	int	ret = 0;
-
-	if (!list) {
-		WL_ERROR(("%s: Null list pointer", __FUNCTION__));
-		return ret;
-	}
-
-	
-
-	for (i = 0; i < list->count && i < IW_MAX_AP; i++) {
-		if (list->version != WL_BSS_INFO_VERSION) {
-			WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
-			          __FUNCTION__, list->version));
-			return ret;
-		}
-
-		bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) : list->bss_info;
-
-		WL_TRACE(("%s : %s\n", __FUNCTION__, bi->SSID));
-
-		
-		iwe.cmd = SIOCGIWAP;
-		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
-		memcpy(iwe.u.ap_addr.sa_data, &bi->BSSID, ETHER_ADDR_LEN);
-		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_ADDR_LEN);
-		
-		iwe.u.data.length = dtoh32(bi->SSID_len);
-		iwe.cmd = SIOCGIWESSID;
-		iwe.u.data.flags = 1;
-		event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, bi->SSID);
-
-		
-		if (dtoh16(bi->capability) & (DOT11_CAP_ESS | DOT11_CAP_IBSS)) {
-			iwe.cmd = SIOCGIWMODE;
-			if (dtoh16(bi->capability) & DOT11_CAP_ESS)
-				iwe.u.mode = IW_MODE_INFRA;
-			else
-				iwe.u.mode = IW_MODE_ADHOC;
-			event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_UINT_LEN);
-		}
-
-		
-		iwe.cmd = SIOCGIWFREQ;
-		iwe.u.freq.m = wf_channel2mhz(CHSPEC_CHANNEL(bi->chanspec),
-			CHSPEC_CHANNEL(bi->chanspec) <= CH_MAX_2G_CHANNEL ?
-			WF_CHAN_FACTOR_2_4_G : WF_CHAN_FACTOR_5_G);
-		iwe.u.freq.e = 6;
-		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_FREQ_LEN);
-
-		
-		iwe.cmd = IWEVQUAL;
-		iwe.u.qual.qual = rssi_to_qual(dtoh16(bi->RSSI));
-		iwe.u.qual.level = 0x100 + dtoh16(bi->RSSI);
-		iwe.u.qual.noise = 0x100 + bi->phy_noise;
-		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_QUAL_LEN);
-
-		
-		 wl_iw_handle_scanresults_ies(&event, end, info, bi);
-
-		
-		iwe.cmd = SIOCGIWENCODE;
-		if (dtoh16(bi->capability) & DOT11_CAP_PRIVACY)
-			iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
-		else
-			iwe.u.data.flags = IW_ENCODE_DISABLED;
-		iwe.u.data.length = 0;
-		event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)event);
-
-		
-		if (bi->rateset.count) {
-			if (((event -extra) + IW_EV_LCP_LEN) <= (uintptr)end) {
-				value = event + IW_EV_LCP_LEN;
-				iwe.cmd = SIOCGIWRATE;
-				
-				iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
-				for (j = 0; j < bi->rateset.count && j < IW_MAX_BITRATES; j++) {
-					iwe.u.bitrate.value =
-						(bi->rateset.rates[j] & 0x7f) * 500000;
-					value = IWE_STREAM_ADD_VALUE(info, event, value, end, &iwe,
-						IW_EV_PARAM_LEN);
-				}
-				event = value;
-			}
-		}
-	}
-
-	if ((ret = (event - extra)) < 0) {
-		WL_ERROR(("==> Wrong size\n"));
-		ret = 0;
-	}
-
-	WL_TRACE(("%s: size=%d bytes prepared \n", __FUNCTION__, (unsigned int)(event - extra)));
-	return (uint)ret;
-}
-
-static int
-wl_iw_get_scan(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	channel_info_t ci;
-	wl_scan_results_t *list_merge;
-	wl_scan_results_t *list = (wl_scan_results_t *) g_scan;
-	int error;
-	uint buflen_from_user = dwrq->length;
-	uint len =  G_SCAN_RESULTS;
-	__u16 len_ret = 0;
-#if  !defined(CSCAN)
-	__u16 merged_len = 0;
-#endif
-#if defined(WL_IW_USE_ISCAN)
-	iscan_info_t *iscan = g_iscan;
-	iscan_buf_t * p_buf;
-#if  !defined(CSCAN)
-	uint32 counter = 0;
-#endif 
-#endif 
-
-	WL_TRACE(("%s: buflen_from_user %d: \n", dev->name, buflen_from_user));
-
-	if (!extra) {
-		WL_TRACE(("%s: wl_iw_get_scan return -EINVAL\n", dev->name));
-		return -EINVAL;
-	}
-
-	
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_CHANNEL, &ci, sizeof(ci))))
-		return error;
-	ci.scan_channel = dtoh32(ci.scan_channel);
-	if (ci.scan_channel)
-		return -EAGAIN;
-
-#if  !defined(CSCAN)
-	if (g_ss_cache_ctrl.m_timer_expired) {
-		wl_iw_free_ss_cache();
-		g_ss_cache_ctrl.m_timer_expired ^= 1;
-	}
-	if ((!g_scan_specified_ssid && g_ss_cache_ctrl.m_prev_scan_mode) ||
-		g_ss_cache_ctrl.m_cons_br_scan_cnt > 4) {
-		g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
-		
-		wl_iw_reset_ss_cache();
-	}
-	g_ss_cache_ctrl.m_prev_scan_mode = g_scan_specified_ssid;
-	if (g_scan_specified_ssid) {
-		g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
-	}
-	else {
-		g_ss_cache_ctrl.m_cons_br_scan_cnt++;
-	}
-#endif 
-
-
-	
-	if (g_scan_specified_ssid) {
-		
-		list = kmalloc(len, GFP_KERNEL);
-		if (!list) {
-			WL_TRACE(("%s: wl_iw_get_scan return -ENOMEM\n", dev->name));
-			g_scan_specified_ssid = 0;
-			return -ENOMEM;
-		}
-	}
-
-	memset(list, 0, len);
-	list->buflen = htod32(len);
-	if ((error = dev_wlc_ioctl(dev, WLC_SCAN_RESULTS, list, len))) {
-		WL_ERROR(("%s: %s : Scan_results ERROR %d\n", dev->name, __FUNCTION__, error));
-		dwrq->length = len;
-		if (g_scan_specified_ssid) {
-			g_scan_specified_ssid = 0;
-			kfree(list);
-		}
-		return 0;
-	}
-	list->buflen = dtoh32(list->buflen);
-	list->version = dtoh32(list->version);
-	list->count = dtoh32(list->count);
-
-	
-	if (list->version != WL_BSS_INFO_VERSION) {
-		WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
-		          __FUNCTION__, list->version));
-		if (g_scan_specified_ssid) {
-			g_scan_specified_ssid = 0;
-			kfree(list);
-		}
-		return -EINVAL;
-	}
-
-#if  !defined(CSCAN)
-	if (g_scan_specified_ssid) {
-		
-		wl_iw_add_bss_to_ss_cache(list);
-		kfree(list);
-	}
-#endif
-
-#if  !defined(CSCAN)
-	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
-#if defined(WL_IW_USE_ISCAN)
-	if (g_scan_specified_ssid)
-		WL_TRACE(("%s: Specified scan APs from scan=%d\n", __FUNCTION__, list->count));
-	p_buf = iscan->list_hdr;
-	
-	while (p_buf != iscan->list_cur) {
-		list_merge = &((wl_iscan_results_t*)p_buf->iscan_buf)->results;
-		WL_TRACE(("%s: Bcast APs list=%d\n", __FUNCTION__, list_merge->count));
-		counter += list_merge->count;
-		if (list_merge->count > 0)
-			len_ret += (__u16) wl_iw_get_scan_prep(list_merge, info,
-			    extra+len_ret, buflen_from_user -len_ret);
-		p_buf = p_buf->next;
-	}
-	WL_TRACE(("%s merged with total Bcast APs=%d\n", __FUNCTION__, counter));
-#else
-	list_merge = (wl_scan_results_t *) g_scan;
-	len_ret = (__u16) wl_iw_get_scan_prep(list_merge, info, extra, buflen_from_user);
-#endif 
-	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
-	if (g_ss_cache_ctrl.m_link_down) {
-		
-		wl_iw_delete_bss_from_ss_cache(g_ss_cache_ctrl.m_active_bssid);
-	}
-	
-	wl_iw_merge_scan_cache(info, extra+len_ret, buflen_from_user-len_ret, &merged_len);
-	len_ret += merged_len;
-	wl_iw_run_ss_cache_timer(0);
-	wl_iw_run_ss_cache_timer(1);
-#else	
-
-	
-	if (g_scan_specified_ssid) {
-		WL_TRACE(("%s: Specified scan APs in the list =%d\n", __FUNCTION__, list->count));
-		len_ret = (__u16) wl_iw_get_scan_prep(list, info, extra, buflen_from_user);
-		kfree(list);
-
-#if defined(WL_IW_USE_ISCAN)
-		p_buf = iscan->list_hdr;
-		
-		while (p_buf != iscan->list_cur) {
-			list_merge = &((wl_iscan_results_t*)p_buf->iscan_buf)->results;
-			WL_TRACE(("%s: Bcast APs list=%d\n", __FUNCTION__, list_merge->count));
-			if (list_merge->count > 0)
-				len_ret += (__u16) wl_iw_get_scan_prep(list_merge, info,
-				    extra+len_ret, buflen_from_user -len_ret);
-			p_buf = p_buf->next;
-		}
-#else
-		list_merge = (wl_scan_results_t *) g_scan;
-		WL_TRACE(("%s: Bcast APs list=%d\n", __FUNCTION__, list_merge->count));
-		if (list_merge->count > 0)
-			len_ret += (__u16) wl_iw_get_scan_prep(list_merge, info, extra+len_ret,
-				buflen_from_user -len_ret);
-#endif 
-	}
-	else {
-		list = (wl_scan_results_t *) g_scan;
-		len_ret = (__u16) wl_iw_get_scan_prep(list, info, extra, buflen_from_user);
-	}
-#endif	
-
-#if defined(WL_IW_USE_ISCAN)
-	
-	g_scan_specified_ssid = 0;
-#endif 
-	
-	if ((len_ret + WE_ADD_EVENT_FIX) < buflen_from_user)
-		len = len_ret;
-
-	dwrq->length = len;
-	dwrq->flags = 0;	
-
-	WL_TRACE(("%s return to WE %d bytes APs=%d\n", __FUNCTION__, dwrq->length, list->count));
-	return 0;
-}
-#endif 
-
-#if defined(WL_IW_USE_ISCAN)
-static int
-wl_iw_iscan_get_scan(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	wl_scan_results_t *list;
-	struct iw_event	iwe;
-	wl_bss_info_t *bi = NULL;
-	int ii, j;
-	int apcnt;
-	char *event = extra, *end = extra + dwrq->length, *value;
-	iscan_info_t *iscan = g_iscan;
-	iscan_buf_t * p_buf;
-	uint32  counter = 0;
-	uint8   channel;
-#if !defined(CSCAN)
-	__u16 merged_len = 0;
-	uint buflen_from_user = dwrq->length;
-#endif
-
-	WL_TRACE(("%s %s buflen_from_user %d:\n", dev->name, __FUNCTION__, dwrq->length));
-
-#if defined(SOFTAP)
-	if (ap_cfg_running) {
-		WL_TRACE(("%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
-		return -EINVAL;
-	}
-#endif
-
-	if (!extra) {
-		WL_TRACE(("%s: INVALID SIOCGIWSCAN GET bad parameter\n", dev->name));
-		return -EINVAL;
-	}
-
-#if defined(CONFIG_FIRST_SCAN)
-	if (g_first_broadcast_scan < BROADCAST_SCAN_FIRST_RESULT_READY) {
-		WL_TRACE(("%s %s: first ISCAN results are NOT ready yet \n",
-		          dev->name, __FUNCTION__));
-		return -EAGAIN;
-	}
-#endif	
-	
-	if ((!iscan) || (iscan->tsk_ctl.thr_pid < 0)) {
-		WL_ERROR(("%ssysioc_pid\n", __FUNCTION__));
-		return EAGAIN;
-	}
-
-	
-
-#if !defined(CSCAN)
-	if (g_ss_cache_ctrl.m_timer_expired) {
-		wl_iw_free_ss_cache();
-		g_ss_cache_ctrl.m_timer_expired ^= 1;
-	}
-	if (g_scan_specified_ssid) {
-		return wl_iw_get_scan(dev, info, dwrq, extra);
-	}
-	else {
-		if (g_ss_cache_ctrl.m_link_down) {
-			
-			wl_iw_delete_bss_from_ss_cache(g_ss_cache_ctrl.m_active_bssid);
-		}
-		if (g_ss_cache_ctrl.m_prev_scan_mode || g_ss_cache_ctrl.m_cons_br_scan_cnt > 4) {
-			g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
-			
-			wl_iw_reset_ss_cache();
-		}
-		g_ss_cache_ctrl.m_prev_scan_mode = g_scan_specified_ssid;
-		g_ss_cache_ctrl.m_cons_br_scan_cnt++;
-	}
-#endif 
-
-	WL_TRACE(("%s: SIOCGIWSCAN GET broadcast results\n", dev->name));
-	apcnt = 0;
-	p_buf = iscan->list_hdr;
-	
-	while (p_buf != iscan->list_cur) {
-		list = &((wl_iscan_results_t*)p_buf->iscan_buf)->results;
-
-		counter += list->count;
-
-		if (list->version != WL_BSS_INFO_VERSION) {
-			WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
-			          __FUNCTION__, list->version));
-			return -EINVAL;
-		}
-
-		bi = NULL;
-		for (ii = 0; ii < list->count && apcnt < IW_MAX_AP; apcnt++, ii++) {
-			bi = (bi ?
-			      (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) :
-			      list->bss_info);
-			ASSERT(((uintptr)bi + dtoh32(bi->length)) <= ((uintptr)list +
-			                                              WLC_IW_ISCAN_MAXLEN));
-
-			
-			if (event + ETHER_ADDR_LEN + bi->SSID_len +
-			    IW_EV_UINT_LEN + IW_EV_FREQ_LEN + IW_EV_QUAL_LEN >= end)
-				return -E2BIG;
-			
-			iwe.cmd = SIOCGIWAP;
-			iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
-			memcpy(iwe.u.ap_addr.sa_data, &bi->BSSID, ETHER_ADDR_LEN);
-			event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_ADDR_LEN);
-
-			
-			iwe.u.data.length = dtoh32(bi->SSID_len);
-			iwe.cmd = SIOCGIWESSID;
-			iwe.u.data.flags = 1;
-			event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, bi->SSID);
-
-			
-			if (dtoh16(bi->capability) & (DOT11_CAP_ESS | DOT11_CAP_IBSS)) {
-				iwe.cmd = SIOCGIWMODE;
-				if (dtoh16(bi->capability) & DOT11_CAP_ESS)
-					iwe.u.mode = IW_MODE_INFRA;
-				else
-					iwe.u.mode = IW_MODE_ADHOC;
-				event = IWE_STREAM_ADD_EVENT(info, event, end,
-				                             &iwe, IW_EV_UINT_LEN);
-			}
-
-			
-			iwe.cmd = SIOCGIWFREQ;
-			channel = (bi->ctl_ch == 0) ? CHSPEC_CHANNEL(bi->chanspec) : bi->ctl_ch;
-			iwe.u.freq.m = wf_channel2mhz(channel,
-			                              channel <= CH_MAX_2G_CHANNEL ?
-			                              WF_CHAN_FACTOR_2_4_G : WF_CHAN_FACTOR_5_G);
-			iwe.u.freq.e = 6;
-			event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_FREQ_LEN);
-
-			
-			iwe.cmd = IWEVQUAL;
-			iwe.u.qual.qual = rssi_to_qual(dtoh16(bi->RSSI));
-			iwe.u.qual.level = 0x100 + dtoh16(bi->RSSI);
-			iwe.u.qual.noise = 0x100 + bi->phy_noise;
-			event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_QUAL_LEN);
-
-			
-			wl_iw_handle_scanresults_ies(&event, end, info, bi);
-
-			
-			iwe.cmd = SIOCGIWENCODE;
-			if (dtoh16(bi->capability) & DOT11_CAP_PRIVACY)
-				iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
-			else
-				iwe.u.data.flags = IW_ENCODE_DISABLED;
-			iwe.u.data.length = 0;
-			event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)event);
-
-			
-			if (bi->rateset.count) {
-				if (event + IW_MAX_BITRATES*IW_EV_PARAM_LEN >= end)
-					return -E2BIG;
-
-				value = event + IW_EV_LCP_LEN;
-				iwe.cmd = SIOCGIWRATE;
-				
-				iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
-				for (j = 0; j < bi->rateset.count && j < IW_MAX_BITRATES; j++) {
-					iwe.u.bitrate.value =
-					        (bi->rateset.rates[j] & 0x7f) * 500000;
-					value = IWE_STREAM_ADD_VALUE(info, event, value, end, &iwe,
-					                             IW_EV_PARAM_LEN);
-				}
-				event = value;
-			}
-		}
-		p_buf = p_buf->next;
-	} 
-
-	dwrq->length = event - extra;
-	dwrq->flags = 0;	
-
-#if !defined(CSCAN)
-	
-	wl_iw_merge_scan_cache(info, event, buflen_from_user - dwrq->length, &merged_len);
-	dwrq->length += merged_len;
-	wl_iw_run_ss_cache_timer(0);
-	wl_iw_run_ss_cache_timer(1);
-#endif 
-	
-#if defined(CONFIG_FIRST_SCAN)
-	g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_CONSUMED;
-#endif 
-
-	WL_TRACE(("%s return to WE %d bytes APs=%d\n", __FUNCTION__, dwrq->length, counter));
-
-	return 0;
-}
-#endif 
-
-#define WL_JOIN_PARAMS_MAX 1600
-#ifdef CONFIG_PRESCANNED
-static int
-check_prescan(wl_join_params_t *join_params, int *join_params_size)
-{
-	int cnt = 0;
-	int indx = 0;
-	wl_iw_ss_cache_t *node = NULL;
-	wl_bss_info_t *bi = NULL;
-	iscan_info_t *iscan = g_iscan;
-	iscan_buf_t * buf;
-	wl_scan_results_t *list;
-	char *destbuf;
-
-	buf = iscan->list_hdr;
-
-	while (buf) {
-		list = &((wl_iscan_results_t*)buf->iscan_buf)->results;
-		bi = NULL;
-		for (indx = 0;  indx < list->count; indx++) {
-			bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length))
-				: list->bss_info;
-			if (!(dtoh16(bi->capability) & DOT11_CAP_ESS))
-				continue;
-			if ((dtoh32(bi->SSID_len) != join_params->ssid.SSID_len) ||
-				memcmp(bi->SSID, join_params->ssid.SSID,
-				join_params->ssid.SSID_len))
-				continue;
-			memcpy(&join_params->params.chanspec_list[cnt],
-				&bi->chanspec, sizeof(chanspec_t));
-			WL_ERROR(("iscan : chanspec :%d, count %d \n", bi->chanspec, cnt));
-			cnt++;
-		}
-		buf = buf->next;
-	}
-
-	if (!cnt) {
-		MUTEX_LOCK_WL_SCAN_SET();
-		node = g_ss_cache_ctrl.m_cache_head;
-		for (; node; ) {
-			if (!memcmp(&node->bss_info->SSID, join_params->ssid.SSID,
-				join_params->ssid.SSID_len)) {
-				memcpy(&join_params->params.chanspec_list[cnt],
-					&node->bss_info->chanspec, sizeof(chanspec_t));
-				WL_ERROR(("cache_scan : chanspec :%d, count %d \n",
-				(int)node->bss_info->chanspec, cnt));
-				cnt++;
-			}
-			node = node->next;
-		}
-		MUTEX_UNLOCK_WL_SCAN_SET();
-	}
-
-	if (!cnt) {
-		return 0;
-	}
-
-	destbuf = (char *)&join_params->params.chanspec_list[cnt];
-	*join_params_size = destbuf - (char*)join_params;
-	join_params->ssid.SSID_len = htod32(g_ssid.SSID_len);
-	memcpy(&(join_params->params.bssid), &ether_bcast, ETHER_ADDR_LEN);
-	join_params->params.chanspec_num = htod32(cnt);
-
-	if ((*join_params_size) > WL_JOIN_PARAMS_MAX) {
-		WL_ERROR(("can't fit bssids for all %d APs found\n", cnt));
-			kfree(join_params);
-		return 0;
-	}
-
-	WL_ERROR(("Passing %d channel/bssid pairs.\n", cnt));
-	return cnt;
-}
-#endif 
-
-static int
-wl_iw_set_essid(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	int error;
-	wl_join_params_t *join_params;
-	int join_params_size;
-
-	WL_TRACE(("%s: SIOCSIWESSID\n", dev->name));
-
-	RETURN_IF_EXTRA_NULL(extra);
-
-#ifdef OEM_CHROMIUMOS
-	if (g_set_essid_before_scan)
-		return -EAGAIN;
-#endif
-	if (!(join_params = kmalloc(WL_JOIN_PARAMS_MAX, GFP_KERNEL))) {
-		WL_ERROR(("allocation failed for join_params size is %d\n", WL_JOIN_PARAMS_MAX));
-		return -ENOMEM;
-	}
-
-	memset(join_params, 0, WL_JOIN_PARAMS_MAX);
-
-	
-	memset(&g_ssid, 0, sizeof(g_ssid));
-
-	if (dwrq->length && extra) {
-#if WIRELESS_EXT > 20
-		g_ssid.SSID_len = MIN(sizeof(g_ssid.SSID), dwrq->length);
-#else
-		g_ssid.SSID_len = MIN(sizeof(g_ssid.SSID), dwrq->length-1);
-#endif
-		memcpy(g_ssid.SSID, extra, g_ssid.SSID_len);
-
-#ifdef CONFIG_PRESCANNED
-		memcpy(join_params->ssid.SSID, g_ssid.SSID, g_ssid.SSID_len);
-		join_params->ssid.SSID_len = g_ssid.SSID_len;
-
-		if (check_prescan(join_params, &join_params_size)) {
-			if ((error = dev_wlc_ioctl(dev, WLC_SET_SSID,
-				join_params, join_params_size))) {
-				WL_ERROR(("Invalid ioctl data=%d\n", error));
-				kfree(join_params);
-				return error;
-			}
-			kfree(join_params);
-			return 0;
-		} else {
-			WL_ERROR(("No matched found\n Trying to join to specific channel\n"));
-		}
-#endif 
-	} else {
-		
-		g_ssid.SSID_len = 0;
-	}
-	g_ssid.SSID_len = htod32(g_ssid.SSID_len);
-
-	
-	memset(join_params, 0, sizeof(join_params));
-	join_params_size = sizeof(join_params->ssid);
-
-	memcpy(join_params->ssid.SSID, g_ssid.SSID, g_ssid.SSID_len);
-	join_params->ssid.SSID_len = htod32(g_ssid.SSID_len);
-	memcpy(&(join_params->params.bssid), &ether_bcast, ETHER_ADDR_LEN);
-
-	
-	
-	wl_iw_ch_to_chanspec(g_wl_iw_params.target_channel, join_params, &join_params_size);
-
-	if ((error = dev_wlc_ioctl(dev, WLC_SET_SSID, join_params, join_params_size))) {
-		WL_ERROR(("Invalid ioctl data=%d\n", error));
-		return error;
-	}
-
-	if (g_ssid.SSID_len) {
-		WL_ERROR(("%s: join SSID=%s ch=%d\n", __FUNCTION__,
-			g_ssid.SSID,  g_wl_iw_params.target_channel));
-	}
-	kfree(join_params);
-	return 0;
-}
-
-static int
-wl_iw_get_essid(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	wlc_ssid_t ssid;
-	int error;
-
-	WL_TRACE(("%s: SIOCGIWESSID\n", dev->name));
-
-	if (!extra)
-		return -EINVAL;
-
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_SSID, &ssid, sizeof(ssid)))) {
-		WL_ERROR(("Error getting the SSID\n"));
-		return error;
-	}
-
-	ssid.SSID_len = dtoh32(ssid.SSID_len);
-
-	
-	memcpy(extra, ssid.SSID, ssid.SSID_len);
-
-	dwrq->length = ssid.SSID_len;
-
-	dwrq->flags = 1; 
-
-	return 0;
-}
-
-static int
-wl_iw_set_nick(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	wl_iw_t *iw = NETDEV_PRIV(dev);
-
-	WL_TRACE(("%s: SIOCSIWNICKN\n", dev->name));
-
-	if (!extra)
-		return -EINVAL;
-
-	
-	if (dwrq->length > sizeof(iw->nickname))
-		return -E2BIG;
-
-	memcpy(iw->nickname, extra, dwrq->length);
-	iw->nickname[dwrq->length - 1] = '\0';
-
-	return 0;
-}
-
-static int
-wl_iw_get_nick(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	wl_iw_t *iw = NETDEV_PRIV(dev);
-
-	WL_TRACE(("%s: SIOCGIWNICKN\n", dev->name));
-
-	if (!extra)
-		return -EINVAL;
-
-	strcpy(extra, iw->nickname);
-	dwrq->length = strlen(extra) + 1;
-
-	return 0;
-}
-
-static int
-wl_iw_set_rate(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	wl_rateset_t rateset;
-	int error, rate, i, error_bg, error_a;
-
-	WL_TRACE(("%s: SIOCSIWRATE\n", dev->name));
-
-	
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_CURR_RATESET, &rateset, sizeof(rateset))))
-		return error;
-
-	rateset.count = dtoh32(rateset.count);
-
-	if (vwrq->value < 0) {
-		
-		rate = rateset.rates[rateset.count - 1] & 0x7f;
-	} else if (vwrq->value < rateset.count) {
-		
-		rate = rateset.rates[vwrq->value] & 0x7f;
-	} else {
-		
-		rate = vwrq->value / 500000;
-	}
-
-	if (vwrq->fixed) {
-		
-		error_bg = dev_wlc_intvar_set(dev, "bg_rate", rate);
-		error_a = dev_wlc_intvar_set(dev, "a_rate", rate);
-
-		if (error_bg && error_a)
-			return (error_bg | error_a);
-	} else {
-		
-		
-		error_bg = dev_wlc_intvar_set(dev, "bg_rate", 0);
-		
-		error_a = dev_wlc_intvar_set(dev, "a_rate", 0);
-
-		if (error_bg && error_a)
-			return (error_bg | error_a);
-
-		
-		for (i = 0; i < rateset.count; i++)
-			if ((rateset.rates[i] & 0x7f) > rate)
-				break;
-		rateset.count = htod32(i);
-
-		
-		if ((error = dev_wlc_ioctl(dev, WLC_SET_RATESET, &rateset, sizeof(rateset))))
-			return error;
-	}
-
-	return 0;
-}
-
-static int
-wl_iw_get_rate(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error, rate;
-
-	WL_TRACE(("%s: SIOCGIWRATE\n", dev->name));
-
-	
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_RATE, &rate, sizeof(rate))))
-		return error;
-	rate = dtoh32(rate);
-	vwrq->value = rate * 500000;
-
-	return 0;
-}
-
-static int
-wl_iw_set_rts(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error, rts;
-
-	WL_TRACE(("%s: SIOCSIWRTS\n", dev->name));
-
-	if (vwrq->disabled)
-		rts = DOT11_DEFAULT_RTS_LEN;
-	else if (vwrq->value < 0 || vwrq->value > DOT11_DEFAULT_RTS_LEN)
-		return -EINVAL;
-	else
-		rts = vwrq->value;
-
-	if ((error = dev_wlc_intvar_set(dev, "rtsthresh", rts)))
-		return error;
-
-	return 0;
-}
-
-static int
-wl_iw_get_rts(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error, rts;
-
-	WL_TRACE(("%s: SIOCGIWRTS\n", dev->name));
-
-	if ((error = dev_wlc_intvar_get(dev, "rtsthresh", &rts)))
-		return error;
-
-	vwrq->value = rts;
-	vwrq->disabled = (rts >= DOT11_DEFAULT_RTS_LEN);
-	vwrq->fixed = 1;
-
-	return 0;
-}
-
-static int
-wl_iw_set_frag(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error, frag;
-
-	WL_TRACE(("%s: SIOCSIWFRAG\n", dev->name));
-
-	if (vwrq->disabled)
-		frag = DOT11_DEFAULT_FRAG_LEN;
-	else if (vwrq->value < 0 || vwrq->value > DOT11_DEFAULT_FRAG_LEN)
-		return -EINVAL;
-	else
-		frag = vwrq->value;
-
-	if ((error = dev_wlc_intvar_set(dev, "fragthresh", frag)))
-		return error;
-
-	return 0;
-}
-
-static int
-wl_iw_get_frag(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error, fragthreshold;
-
-	WL_TRACE(("%s: SIOCGIWFRAG\n", dev->name));
-
-	if ((error = dev_wlc_intvar_get(dev, "fragthresh", &fragthreshold)))
-		return error;
-
-	vwrq->value = fragthreshold;
-	vwrq->disabled = (fragthreshold >= DOT11_DEFAULT_FRAG_LEN);
-	vwrq->fixed = 1;
-
-	return 0;
-}
-
-static int
-wl_iw_set_txpow(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error, disable;
-	uint16 txpwrmw;
-	WL_TRACE(("%s: SIOCSIWTXPOW\n", dev->name));
-
-	
-	disable = vwrq->disabled ? WL_RADIO_SW_DISABLE : 0;
-	disable += WL_RADIO_SW_DISABLE << 16;
-
-	disable = htod32(disable);
-	if ((error = dev_wlc_ioctl(dev, WLC_SET_RADIO, &disable, sizeof(disable))))
-		return error;
-
-	
-	if (disable & WL_RADIO_SW_DISABLE)
-		return 0;
-
-	
-	if (!(vwrq->flags & IW_TXPOW_MWATT))
-		return -EINVAL;
-
-	
-	if (vwrq->value < 0)
-		return 0;
-
-	if (vwrq->value > 0xffff) txpwrmw = 0xffff;
-	else txpwrmw = (uint16)vwrq->value;
-
-
-	error = dev_wlc_intvar_set(dev, "qtxpower", (int)(bcm_mw_to_qdbm(txpwrmw)));
-	return error;
-}
-
-static int
-wl_iw_get_txpow(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error, disable, txpwrdbm;
-	uint8 result;
-
-	WL_TRACE(("%s: SIOCGIWTXPOW\n", dev->name));
-
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_RADIO, &disable, sizeof(disable))) ||
-	    (error = dev_wlc_intvar_get(dev, "qtxpower", &txpwrdbm)))
-		return error;
-
-	disable = dtoh32(disable);
-	result = (uint8)(txpwrdbm & ~WL_TXPWR_OVERRIDE);
-	vwrq->value = (int32)bcm_qdbm_to_mw(result);
-	vwrq->fixed = 0;
-	vwrq->disabled = (disable & (WL_RADIO_SW_DISABLE | WL_RADIO_HW_DISABLE)) ? 1 : 0;
-	vwrq->flags = IW_TXPOW_MWATT;
-
-	return 0;
-}
-
-#if WIRELESS_EXT > 10
-static int
-wl_iw_set_retry(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error, lrl, srl;
-
-	WL_TRACE(("%s: SIOCSIWRETRY\n", dev->name));
-
-	
-	if (vwrq->disabled || (vwrq->flags & IW_RETRY_LIFETIME))
-		return -EINVAL;
-
-	
-	if (vwrq->flags & IW_RETRY_LIMIT) {
-
-		
-#if WIRELESS_EXT > 20
-	if ((vwrq->flags & IW_RETRY_LONG) ||(vwrq->flags & IW_RETRY_MAX) ||
-		!((vwrq->flags & IW_RETRY_SHORT) || (vwrq->flags & IW_RETRY_MIN))) {
-#else
-	if ((vwrq->flags & IW_RETRY_MAX) || !(vwrq->flags & IW_RETRY_MIN)) {
-#endif 
-			lrl = htod32(vwrq->value);
-			if ((error = dev_wlc_ioctl(dev, WLC_SET_LRL, &lrl, sizeof(lrl))))
-				return error;
-		}
-
-		
-#if WIRELESS_EXT > 20
-	if ((vwrq->flags & IW_RETRY_SHORT) ||(vwrq->flags & IW_RETRY_MIN) ||
-		!((vwrq->flags & IW_RETRY_LONG) || (vwrq->flags & IW_RETRY_MAX))) {
-#else
-		if ((vwrq->flags & IW_RETRY_MIN) || !(vwrq->flags & IW_RETRY_MAX)) {
-#endif 
-			srl = htod32(vwrq->value);
-			if ((error = dev_wlc_ioctl(dev, WLC_SET_SRL, &srl, sizeof(srl))))
-				return error;
-		}
-	}
-	return 0;
-}
-
-static int
-wl_iw_get_retry(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error, lrl, srl;
-
-	WL_TRACE(("%s: SIOCGIWRETRY\n", dev->name));
-
-	vwrq->disabled = 0;      
-
-	
-	if ((vwrq->flags & IW_RETRY_TYPE) == IW_RETRY_LIFETIME)
-		return -EINVAL;
-
-	
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_LRL, &lrl, sizeof(lrl))) ||
-	    (error = dev_wlc_ioctl(dev, WLC_GET_SRL, &srl, sizeof(srl))))
-		return error;
-
-	lrl = dtoh32(lrl);
-	srl = dtoh32(srl);
-
-	
-	if (vwrq->flags & IW_RETRY_MAX) {
-		vwrq->flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
-		vwrq->value = lrl;
-	} else {
-		vwrq->flags = IW_RETRY_LIMIT;
-		vwrq->value = srl;
-		if (srl != lrl)
-			vwrq->flags |= IW_RETRY_MIN;
-	}
-
-	return 0;
-}
-#endif 
-
-static int
-wl_iw_set_encode(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	wl_wsec_key_t key;
-	int error, val, wsec;
-
-	WL_TRACE(("%s: SIOCSIWENCODE index %d, len %d, flags %04x (%s%s%s%s%s)\n",
-		dev->name, dwrq->flags & IW_ENCODE_INDEX, dwrq->length, dwrq->flags,
-		dwrq->flags & IW_ENCODE_NOKEY ? "NOKEY" : "",
-		dwrq->flags & IW_ENCODE_DISABLED ? " DISABLED" : "",
-		dwrq->flags & IW_ENCODE_RESTRICTED ? " RESTRICTED" : "",
-		dwrq->flags & IW_ENCODE_OPEN ? " OPEN" : "",
-		dwrq->flags & IW_ENCODE_TEMP ? " TEMP" : ""));
-
-	memset(&key, 0, sizeof(key));
-
-	if ((dwrq->flags & IW_ENCODE_INDEX) == 0) {
-		
-		for (key.index = 0; key.index < DOT11_MAX_DEFAULT_KEYS; key.index++) {
-			val = htod32(key.index);
-			if ((error = dev_wlc_ioctl(dev, WLC_GET_KEY_PRIMARY, &val, sizeof(val))))
-				return error;
-			val = dtoh32(val);
-			if (val)
-				break;
-		}
-		
-		if (key.index == DOT11_MAX_DEFAULT_KEYS)
-			key.index = 0;
-	} else {
-		key.index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
-		if (key.index >= DOT11_MAX_DEFAULT_KEYS)
-			return -EINVAL;
-	}
-
-	
-	if (!extra || !dwrq->length || (dwrq->flags & IW_ENCODE_NOKEY)) {
-		
-		val = htod32(key.index);
-		if ((error = dev_wlc_ioctl(dev, WLC_SET_KEY_PRIMARY, &val, sizeof(val))))
-			return error;
-	} else {
-		key.len = dwrq->length;
-
-		if (dwrq->length > sizeof(key.data))
-			return -EINVAL;
-
-		memcpy(key.data, extra, dwrq->length);
-
-		key.flags = WL_PRIMARY_KEY;
-		switch (key.len) {
-		case WEP1_KEY_SIZE:
-			key.algo = CRYPTO_ALGO_WEP1;
-			break;
-		case WEP128_KEY_SIZE:
-			key.algo = CRYPTO_ALGO_WEP128;
-			break;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 14)
-		case TKIP_KEY_SIZE:
-			key.algo = CRYPTO_ALGO_TKIP;
-			break;
-#endif
-		case AES_KEY_SIZE:
-			key.algo = CRYPTO_ALGO_AES_CCM;
-			break;
-		default:
-			return -EINVAL;
-		}
-
-		
-		swap_key_from_BE(&key);
-		if ((error = dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key))))
-			return error;
-	}
-
-	
-	val = (dwrq->flags & IW_ENCODE_DISABLED) ? 0 : WEP_ENABLED;
-
-	if ((error = dev_wlc_intvar_get(dev, "wsec", &wsec)))
-		return error;
-
-	wsec  &= ~(WEP_ENABLED);
-	wsec |= val;
-
-	if ((error = dev_wlc_intvar_set(dev, "wsec", wsec)))
-		return error;
-
-	
-	val = (dwrq->flags & IW_ENCODE_RESTRICTED) ? 1 : 0;
-	val = htod32(val);
-	if ((error = dev_wlc_ioctl(dev, WLC_SET_AUTH, &val, sizeof(val))))
-		return error;
-
-	return 0;
-}
-
-static int
-wl_iw_get_encode(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	wl_wsec_key_t key;
-	int error, val, wsec, auth;
-
-	WL_TRACE(("%s: SIOCGIWENCODE\n", dev->name));
-
-	
-	bzero(&key, sizeof(wl_wsec_key_t));
-
-	if ((dwrq->flags & IW_ENCODE_INDEX) == 0) {
-		
-		for (key.index = 0; key.index < DOT11_MAX_DEFAULT_KEYS; key.index++) {
-			val = key.index;
-			if ((error = dev_wlc_ioctl(dev, WLC_GET_KEY_PRIMARY, &val, sizeof(val))))
-				return error;
-			val = dtoh32(val);
-			if (val)
-				break;
-		}
-	} else
-		key.index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
-
-	if (key.index >= DOT11_MAX_DEFAULT_KEYS)
-		key.index = 0;
-
-	
-
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_WSEC, &wsec, sizeof(wsec))) ||
-	    (error = dev_wlc_ioctl(dev, WLC_GET_AUTH, &auth, sizeof(auth))))
-		return error;
-
-	swap_key_to_BE(&key);
-
-	wsec = dtoh32(wsec);
-	auth = dtoh32(auth);
-	
-	dwrq->length = MIN(DOT11_MAX_KEY_SIZE, key.len);
-
-	
-	dwrq->flags = key.index + 1;
-	if (!(wsec & (WEP_ENABLED | TKIP_ENABLED | AES_ENABLED))) {
-		
-		dwrq->flags |= IW_ENCODE_DISABLED;
-	}
-	if (auth) {
-		
-		dwrq->flags |= IW_ENCODE_RESTRICTED;
-	}
-
-	
-	if (dwrq->length && extra)
-		memcpy(extra, key.data, dwrq->length);
-
-	return 0;
-}
-
-static int
-wl_iw_set_power(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error, pm;
-
-	WL_TRACE(("%s: SIOCSIWPOWER\n", dev->name));
-
-	pm = vwrq->disabled ? PM_OFF : PM_MAX;
-
-	pm = htod32(pm);
-	if ((error = dev_wlc_ioctl(dev, WLC_SET_PM, &pm, sizeof(pm))))
-		return error;
-
-	return 0;
-}
-
-static int
-wl_iw_get_power(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error, pm;
-
-	WL_TRACE(("%s: SIOCGIWPOWER\n", dev->name));
-
-	if ((error = dev_wlc_ioctl(dev, WLC_GET_PM, &pm, sizeof(pm))))
-		return error;
-
-	pm = dtoh32(pm);
-	vwrq->disabled = pm ? 0 : 1;
-	vwrq->flags = IW_POWER_ALL_R;
-
-	return 0;
-}
-
-#if WIRELESS_EXT > 17
-static int
-wl_iw_set_wpaie(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *iwp,
-	char *extra
-)
-{
-
-	WL_TRACE(("%s: SIOCSIWGENIE\n", dev->name));
-
-	RETURN_IF_EXTRA_NULL(extra);
-
-#ifdef DHD_DEBUG
-	{
-		int i;
-
-		for (i = 0; i < iwp->length; i++)
-			WL_TRACE(("%02X ", extra[i]));
-		WL_TRACE(("\n"));
-	}
-#endif
-
-		dev_wlc_bufvar_set(dev, "wpaie", extra, iwp->length);
-
-	return 0;
-}
-
-static int
-wl_iw_get_wpaie(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *iwp,
-	char *extra
-)
-{
-	WL_TRACE(("%s: SIOCGIWGENIE\n", dev->name));
-	iwp->length = 64;
-	dev_wlc_bufvar_get(dev, "wpaie", extra, iwp->length);
-	return 0;
-}
-
-static int
-wl_iw_set_encodeext(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *extra
-)
-{
-	wl_wsec_key_t key;
-	int error;
-	struct iw_encode_ext *iwe;
-
-	WL_TRACE(("%s: SIOCSIWENCODEEXT\n", dev->name));
-
-	RETURN_IF_EXTRA_NULL(extra);
-
-	memset(&key, 0, sizeof(key));
-	iwe = (struct iw_encode_ext *)extra;
-
-	
-	if (dwrq->flags & IW_ENCODE_DISABLED) {
-
-	}
-
-	
-	key.index = 0;
-	if (dwrq->flags & IW_ENCODE_INDEX)
-		key.index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
-
-	key.len = iwe->key_len;
-
-	
-	if (!ETHER_ISMULTI(iwe->addr.sa_data))
-		bcopy((void *)&iwe->addr.sa_data, (char *)&key.ea, ETHER_ADDR_LEN);
-
-	
-	if (key.len == 0) {
-		if (iwe->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
-			WL_WSEC(("Changing the the primary Key to %d\n", key.index));
-			
-			key.index = htod32(key.index);
-			error = dev_wlc_ioctl(dev, WLC_SET_KEY_PRIMARY,
-				&key.index, sizeof(key.index));
-			if (error)
-				return error;
-		}
-		
-		else {
-			swap_key_from_BE(&key);
-			dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
-		}
-	}
-	else {
-		if (iwe->key_len > sizeof(key.data))
-			return -EINVAL;
-
-		WL_WSEC(("Setting the key index %d\n", key.index));
-		if (iwe->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
-			WL_WSEC(("key is a Primary Key\n"));
-			key.flags = WL_PRIMARY_KEY;
-		}
-
-		bcopy((void *)iwe->key, key.data, iwe->key_len);
-
-		if (iwe->alg == IW_ENCODE_ALG_TKIP) {
-			uint8 keybuf[8];
-			bcopy(&key.data[24], keybuf, sizeof(keybuf));
-			bcopy(&key.data[16], &key.data[24], sizeof(keybuf));
-			bcopy(keybuf, &key.data[16], sizeof(keybuf));
-		}
-
-		
-		if (iwe->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
-			uchar *ivptr;
-			ivptr = (uchar *)iwe->rx_seq;
-			key.rxiv.hi = (ivptr[5] << 24) | (ivptr[4] << 16) |
-				(ivptr[3] << 8) | ivptr[2];
-			key.rxiv.lo = (ivptr[1] << 8) | ivptr[0];
-			key.iv_initialized = TRUE;
-		}
-
-		switch (iwe->alg) {
-			case IW_ENCODE_ALG_NONE:
-				key.algo = CRYPTO_ALGO_OFF;
-				break;
-			case IW_ENCODE_ALG_WEP:
-				if (iwe->key_len == WEP1_KEY_SIZE)
-					key.algo = CRYPTO_ALGO_WEP1;
-				else
-					key.algo = CRYPTO_ALGO_WEP128;
-				break;
-			case IW_ENCODE_ALG_TKIP:
-				key.algo = CRYPTO_ALGO_TKIP;
-				break;
-			case IW_ENCODE_ALG_CCMP:
-				key.algo = CRYPTO_ALGO_AES_CCM;
-				break;
-			default:
-				break;
-		}
-		swap_key_from_BE(&key);
-
-		dhd_wait_pend8021x(dev);
-
-		error = dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
-		if (error)
-			return error;
-	}
-	return 0;
-}
-
-#if WIRELESS_EXT > 17
-struct {
-	pmkid_list_t pmkids;
-	pmkid_t foo[MAXPMKID-1];
-} pmkid_list;
-
-static int
-wl_iw_set_pmksa(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	struct iw_pmksa *iwpmksa;
-	uint i;
-	int ret = 0;
-	char eabuf[ETHER_ADDR_STR_LEN];
-
-	WL_WSEC(("%s: SIOCSIWPMKSA\n", dev->name));
-
-	RETURN_IF_EXTRA_NULL(extra);
-
-	iwpmksa = (struct iw_pmksa *)extra;
-	bzero((char *)eabuf, ETHER_ADDR_STR_LEN);
-
-	if (iwpmksa->cmd == IW_PMKSA_FLUSH) {
-		WL_WSEC(("wl_iw_set_pmksa - IW_PMKSA_FLUSH\n"));
-		bzero((char *)&pmkid_list, sizeof(pmkid_list));
-	}
-
-	else if (iwpmksa->cmd == IW_PMKSA_REMOVE) {
-		{
-			pmkid_list_t pmkid, *pmkidptr;
-			uint j;
-			pmkidptr = &pmkid;
-
-			bcopy(&iwpmksa->bssid.sa_data[0], &pmkidptr->pmkid[0].BSSID,
-				ETHER_ADDR_LEN);
-			bcopy(&iwpmksa->pmkid[0], &pmkidptr->pmkid[0].PMKID, WPA2_PMKID_LEN);
-
-			WL_WSEC(("wl_iw_set_pmksa,IW_PMKSA_REMOVE - PMKID: %s = ",
-				bcm_ether_ntoa(&pmkidptr->pmkid[0].BSSID,
-				eabuf)));
-			for (j = 0; j < WPA2_PMKID_LEN; j++)
-				WL_WSEC(("%02x ", pmkidptr->pmkid[0].PMKID[j]));
-			WL_WSEC(("\n"));
-		}
-
-		for (i = 0; i < pmkid_list.pmkids.npmkid; i++)
-			if (!bcmp(&iwpmksa->bssid.sa_data[0], &pmkid_list.pmkids.pmkid[i].BSSID,
-				ETHER_ADDR_LEN))
-				break;
-
-		if ((pmkid_list.pmkids.npmkid > 0) && (i < pmkid_list.pmkids.npmkid)) {
-			bzero(&pmkid_list.pmkids.pmkid[i], sizeof(pmkid_t));
-			for (; i < (pmkid_list.pmkids.npmkid - 1); i++) {
-				bcopy(&pmkid_list.pmkids.pmkid[i+1].BSSID,
-					&pmkid_list.pmkids.pmkid[i].BSSID,
-					ETHER_ADDR_LEN);
-				bcopy(&pmkid_list.pmkids.pmkid[i+1].PMKID,
-					&pmkid_list.pmkids.pmkid[i].PMKID,
-					WPA2_PMKID_LEN);
-			}
-			pmkid_list.pmkids.npmkid--;
-		}
-		else
-			ret = -EINVAL;
-	}
-
-	else if (iwpmksa->cmd == IW_PMKSA_ADD) {
-		for (i = 0; i < pmkid_list.pmkids.npmkid; i++)
-			if (!bcmp(&iwpmksa->bssid.sa_data[0], &pmkid_list.pmkids.pmkid[i].BSSID,
-				ETHER_ADDR_LEN))
-				break;
-		if (i < MAXPMKID) {
-			bcopy(&iwpmksa->bssid.sa_data[0],
-				&pmkid_list.pmkids.pmkid[i].BSSID,
-				ETHER_ADDR_LEN);
-			bcopy(&iwpmksa->pmkid[0], &pmkid_list.pmkids.pmkid[i].PMKID,
-				WPA2_PMKID_LEN);
-			if (i == pmkid_list.pmkids.npmkid)
-				pmkid_list.pmkids.npmkid++;
-		}
-		else
-			ret = -EINVAL;
-
-		{
-			uint j;
-			uint k;
-			k = pmkid_list.pmkids.npmkid;
-			WL_WSEC(("wl_iw_set_pmksa,IW_PMKSA_ADD - PMKID: %s = ",
-				bcm_ether_ntoa(&pmkid_list.pmkids.pmkid[k].BSSID,
-				eabuf)));
-			for (j = 0; j < WPA2_PMKID_LEN; j++)
-				WL_WSEC(("%02x ", pmkid_list.pmkids.pmkid[k].PMKID[j]));
-			WL_WSEC(("\n"));
-		}
-	}
-	WL_WSEC(("PRINTING pmkid LIST - No of elements %d", pmkid_list.pmkids.npmkid));
-	for (i = 0; i < pmkid_list.pmkids.npmkid; i++) {
-		uint j;
-		WL_WSEC(("\nPMKID[%d]: %s = ", i,
-			bcm_ether_ntoa(&pmkid_list.pmkids.pmkid[i].BSSID,
-			eabuf)));
-		for (j = 0; j < WPA2_PMKID_LEN; j++)
-			WL_WSEC(("%02x ", pmkid_list.pmkids.pmkid[i].PMKID[j]));
-	}
-	WL_WSEC(("\n"));
-
-	if (!ret)
-		ret = dev_wlc_bufvar_set(dev, "pmkid_info", (char *)&pmkid_list,
-			sizeof(pmkid_list));
-	return ret;
-}
-#endif 
-
-static int
-wl_iw_get_encodeext(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	WL_TRACE(("%s: SIOCGIWENCODEEXT\n", dev->name));
-	return 0;
-}
-
-
-static uint32
-wl_iw_create_wpaauth_wsec(struct net_device *dev)
-{
-	wl_iw_t *iw = NETDEV_PRIV(dev);
-	uint32 wsec;
-
-	
-	if (iw->pcipher & (IW_AUTH_CIPHER_WEP40 | IW_AUTH_CIPHER_WEP104))
-		wsec = WEP_ENABLED;
-	else if (iw->pcipher & IW_AUTH_CIPHER_TKIP)
-		wsec = TKIP_ENABLED;
-	else if (iw->pcipher & IW_AUTH_CIPHER_CCMP)
-		wsec = AES_ENABLED;
-	else
-		wsec = 0;
-
-	
-	if (iw->gcipher & (IW_AUTH_CIPHER_WEP40 | IW_AUTH_CIPHER_WEP104))
-		wsec |= WEP_ENABLED;
-	else if (iw->gcipher & IW_AUTH_CIPHER_TKIP)
-		wsec |= TKIP_ENABLED;
-	else if (iw->gcipher & IW_AUTH_CIPHER_CCMP)
-		wsec |= AES_ENABLED;
-
-	
-	if (wsec == 0 && iw->privacy_invoked)
-		wsec = WEP_ENABLED;
-
-	WL_INFORM(("%s: returning wsec of %d\n", __FUNCTION__, wsec));
-
-	return wsec;
-}
-
-static int
-wl_iw_set_wpaauth(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error = 0;
-	int paramid;
-	int paramval;
-	int val = 0;
-	wl_iw_t *iw = NETDEV_PRIV(dev);
-
-	paramid = vwrq->flags & IW_AUTH_INDEX;
-	paramval = vwrq->value;
-
-	WL_TRACE(("%s: SIOCSIWAUTH, %s(%d), paramval = 0x%0x\n",
-		dev->name,
-		paramid == IW_AUTH_WPA_VERSION ? "IW_AUTH_WPA_VERSION" :
-		paramid == IW_AUTH_CIPHER_PAIRWISE ? "IW_AUTH_CIPHER_PAIRWISE" :
-		paramid == IW_AUTH_CIPHER_GROUP ? "IW_AUTH_CIPHER_GROUP" :
-		paramid == IW_AUTH_KEY_MGMT ? "IW_AUTH_KEY_MGMT" :
-		paramid == IW_AUTH_TKIP_COUNTERMEASURES ? "IW_AUTH_TKIP_COUNTERMEASURES" :
-		paramid == IW_AUTH_DROP_UNENCRYPTED ? "IW_AUTH_DROP_UNENCRYPTED" :
-		paramid == IW_AUTH_80211_AUTH_ALG ? "IW_AUTH_80211_AUTH_ALG" :
-		paramid == IW_AUTH_WPA_ENABLED ? "IW_AUTH_WPA_ENABLED" :
-		paramid == IW_AUTH_RX_UNENCRYPTED_EAPOL ? "IW_AUTH_RX_UNENCRYPTED_EAPOL" :
-		paramid == IW_AUTH_ROAMING_CONTROL ? "IW_AUTH_ROAMING_CONTROL" :
-		paramid == IW_AUTH_PRIVACY_INVOKED ? "IW_AUTH_PRIVACY_INVOKED" :
-		"UNKNOWN",
-		paramid, paramval));
-
-#if defined(SOFTAP)
-	if (ap_cfg_running) {
-		WL_TRACE(("%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
-		return 0;
-	}
-#endif
-
-	switch (paramid) {
-	case IW_AUTH_WPA_VERSION:
-		
-		iw->wpaversion = paramval;
-		break;
-
-	case IW_AUTH_CIPHER_PAIRWISE:
-		iw->pcipher = paramval;
-		val = wl_iw_create_wpaauth_wsec(dev);
-		if ((error = dev_wlc_intvar_set(dev, "wsec", val)))
-			return error;
-		break;
-
-	case IW_AUTH_CIPHER_GROUP:
-		iw->gcipher = paramval;
-		val = wl_iw_create_wpaauth_wsec(dev);
-		if ((error = dev_wlc_intvar_set(dev, "wsec", val)))
-			return error;
-		break;
-
-	case IW_AUTH_KEY_MGMT:
-		if (paramval & IW_AUTH_KEY_MGMT_PSK) {
-			if (iw->wpaversion == IW_AUTH_WPA_VERSION_WPA)
-				val = WPA_AUTH_PSK;
-			else if (iw->wpaversion == IW_AUTH_WPA_VERSION_WPA2)
-				val = WPA2_AUTH_PSK;
-			else 
-				val = WPA_AUTH_DISABLED;
-		} else if (paramval & IW_AUTH_KEY_MGMT_802_1X) {
-			if (iw->wpaversion == IW_AUTH_WPA_VERSION_WPA)
-				val = WPA_AUTH_UNSPECIFIED;
-			else if (iw->wpaversion == IW_AUTH_WPA_VERSION_WPA2)
-				val = WPA2_AUTH_UNSPECIFIED;
-			else 
-				val = WPA_AUTH_DISABLED;
-		}
-		else
-			val = WPA_AUTH_DISABLED;
-
-		WL_INFORM(("%s: %d: setting wpa_auth to %d\n", __FUNCTION__, __LINE__, val));
-		if ((error = dev_wlc_intvar_set(dev, "wpa_auth", val)))
-			return error;
-		break;
-
-	case IW_AUTH_TKIP_COUNTERMEASURES:
-		dev_wlc_bufvar_set(dev, "tkip_countermeasures", (char *)&paramval, 1);
-		break;
-
-	case IW_AUTH_80211_AUTH_ALG:
-		
-		WL_INFORM(("Setting the D11auth %d\n", paramval));
-		if (paramval == IW_AUTH_ALG_OPEN_SYSTEM)
-			val = 0;
-		else if (paramval == IW_AUTH_ALG_SHARED_KEY)
-			val = 1;
-		else if (paramval == (IW_AUTH_ALG_OPEN_SYSTEM | IW_AUTH_ALG_SHARED_KEY))
-			val = 2;
-		else
-			error = 1;
-		if (!error && (error = dev_wlc_intvar_set(dev, "auth", val)))
-			return error;
-		break;
-
-	case IW_AUTH_WPA_ENABLED:
-		if (paramval == 0) {
-			iw->privacy_invoked = 0; 
-			iw->pcipher = 0;
-			iw->gcipher = 0;
-			val = wl_iw_create_wpaauth_wsec(dev);
-			if ((error = dev_wlc_intvar_set(dev, "wsec", val)))
-				return error;
-			WL_INFORM(("%s: %d: setting wpa_auth to %d, wsec to %d\n",
-				__FUNCTION__, __LINE__, paramval, val));
-			dev_wlc_intvar_set(dev, "wpa_auth", paramval);
-			return error;
-		}
-
-		
-		break;
-
-	case IW_AUTH_DROP_UNENCRYPTED:
-		if ((error = dev_wlc_intvar_set(dev, "wsec_restrict", paramval)))
-			return error;
-		break;
-
-	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
-		dev_wlc_bufvar_set(dev, "rx_unencrypted_eapol", (char *)&paramval, 1);
-		break;
-
-#if WIRELESS_EXT > 17
-	case IW_AUTH_ROAMING_CONTROL:
-		WL_INFORM(("%s: IW_AUTH_ROAMING_CONTROL\n", __FUNCTION__));
-		
-		break;
-
-	case IW_AUTH_PRIVACY_INVOKED:
-		iw->privacy_invoked = paramval;
-		val = wl_iw_create_wpaauth_wsec(dev);
-		if ((error = dev_wlc_intvar_set(dev, "wsec", val)))
-			return error;
-		break;
-
-#endif 
-	default:
-		break;
-	}
-	return 0;
-}
-#define VAL_PSK(_val) (((_val) & WPA_AUTH_PSK) || ((_val) & WPA2_AUTH_PSK))
-
-static int
-wl_iw_get_wpaauth(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_param *vwrq,
-	char *extra
-)
-{
-	int error;
-	int paramid;
-	int paramval = 0;
-	int val;
-	wl_iw_t *iw = NETDEV_PRIV(dev);
-
-	WL_TRACE(("%s: SIOCGIWAUTH\n", dev->name));
-
-	paramid = vwrq->flags & IW_AUTH_INDEX;
-
-	switch (paramid) {
-	case IW_AUTH_WPA_VERSION:
-		paramval = iw->wpaversion;
-		break;
-
-	case IW_AUTH_CIPHER_PAIRWISE:
-		paramval = iw->pcipher;
-		break;
-
-	case IW_AUTH_CIPHER_GROUP:
-		paramval = iw->gcipher;
-		break;
-
-	case IW_AUTH_KEY_MGMT:
-		
-		if ((error = dev_wlc_intvar_get(dev, "wpa_auth", &val)))
-			return error;
-		if (VAL_PSK(val))
-			paramval = IW_AUTH_KEY_MGMT_PSK;
-		else
-			paramval = IW_AUTH_KEY_MGMT_802_1X;
-
-		break;
-
-	case IW_AUTH_TKIP_COUNTERMEASURES:
-		dev_wlc_bufvar_get(dev, "tkip_countermeasures", (char *)&paramval, 1);
-		break;
-
-	case IW_AUTH_DROP_UNENCRYPTED:
-		dev_wlc_intvar_get(dev, "wsec_restrict", &paramval);
-		break;
-
-	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
-		dev_wlc_bufvar_get(dev, "rx_unencrypted_eapol", (char *)&paramval, 1);
-		break;
-
-	case IW_AUTH_80211_AUTH_ALG:
-		
-		if ((error = dev_wlc_intvar_get(dev, "auth", &val)))
-			return error;
-		if (!val)
-			paramval = IW_AUTH_ALG_OPEN_SYSTEM;
-		else
-			paramval = IW_AUTH_ALG_SHARED_KEY;
-		break;
-	case IW_AUTH_WPA_ENABLED:
-		if ((error = dev_wlc_intvar_get(dev, "wpa_auth", &val)))
-			return error;
-		if (val)
-			paramval = TRUE;
-		else
-			paramval = FALSE;
-		break;
-#if WIRELESS_EXT > 17
-	case IW_AUTH_ROAMING_CONTROL:
-		WL_ERROR(("%s: IW_AUTH_ROAMING_CONTROL\n", __FUNCTION__));
-		
-		break;
-	case IW_AUTH_PRIVACY_INVOKED:
-		paramval = iw->privacy_invoked;
-		break;
-
-#endif 
-	}
-	vwrq->value = paramval;
-	return 0;
-}
-#endif 
-
-
-#ifdef SOFTAP
-
-static int ap_macmode = MACLIST_MODE_DISABLED;
-static struct mflist ap_black_list;
-
-static int
-wl_iw_parse_wep(char *keystr, wl_wsec_key_t *key)
-{
-	char hex[] = "XX";
-	unsigned char *data = key->data;
-
-	switch (strlen(keystr)) {
-	case 5:
-	case 13:
-	case 16:
-		key->len = strlen(keystr);
-		memcpy(data, keystr, key->len + 1);
-		break;
-	case 12:
-	case 28:
-	case 34:
-	case 66:
-		
-		if (!strnicmp(keystr, "0x", 2))
-			keystr += 2;
-		else
-			return -1;
-		
-	case 10:
-	case 26:
-	case 32:
-	case 64:
-		key->len = strlen(keystr) / 2;
-		while (*keystr) {
-			strncpy(hex, keystr, 2);
-			*data++ = (char) bcm_strtoul(hex, NULL, 16);
-			keystr += 2;
-		}
-		break;
-	default:
-		return -1;
-	}
-
-	switch (key->len) {
-	case 5:
-		key->algo = CRYPTO_ALGO_WEP1;
-		break;
-	case 13:
-		key->algo = CRYPTO_ALGO_WEP128;
-		break;
-	case 16:
-		
-		key->algo = CRYPTO_ALGO_AES_CCM;
-		break;
-	case 32:
-		key->algo = CRYPTO_ALGO_TKIP;
-		break;
-	default:
-		return -1;
-	}
-
-	
-	key->flags |= WL_PRIMARY_KEY;
-
-	return 0;
-}
-
-#ifdef EXT_WPA_CRYPTO
-#define SHA1HashSize 20
-extern void pbkdf2_sha1(const char *passphrase, const char *ssid, size_t ssid_len,
-                        int iterations, u8 *buf, size_t buflen);
-
-#else
-
-#define SHA1HashSize 20
-static int
-pbkdf2_sha1(const char *passphrase, const char *ssid, size_t ssid_len,
-            int iterations, u8 *buf, size_t buflen)
-{
-	WL_ERROR(("WARNING: %s is not implemented !!!\n", __FUNCTION__));
-	return -1;
-}
-
-#endif 
-
-
-static int
-dev_iw_write_cfg1_bss_var(struct net_device *dev, int val)
-{
-	struct {
-		int cfg;
-		int val;
-	} bss_setbuf;
-
-	int bss_set_res;
-	char smbuf[WLC_IOCTL_SMLEN];
-	memset(smbuf, 0, sizeof(smbuf));
-
-	bss_setbuf.cfg = 1;
-	bss_setbuf.val = val;
-
-	bss_set_res = dev_iw_iovar_setbuf(dev, "bss",
-		&bss_setbuf, sizeof(bss_setbuf), smbuf, sizeof(smbuf));
-	WL_TRACE(("%s: bss_set_result:%d set with %d\n", __FUNCTION__, bss_set_res, val));
-
-	return bss_set_res;
-}
-
-
-
-#ifndef AP_ONLY
-static int
-wl_bssiovar_mkbuf(
-		const char *iovar,
-		int bssidx,
-		void *param,
-		int paramlen,
-		void *bufptr,
-		int buflen,
-		int *perr)
-{
-	const char *prefix = "bsscfg:";
-	int8* p;
-	uint prefixlen;
-	uint namelen;
-	uint iolen;
-
-	prefixlen = strlen(prefix);	
-	namelen = strlen(iovar) + 1;	
-	iolen = prefixlen + namelen + sizeof(int) + paramlen;
-
-	
-	if (buflen < 0 || iolen > (uint)buflen) {
-		*perr = BCME_BUFTOOSHORT;
-		return 0;
-	}
-
-	p = (int8*)bufptr;
-
-	
-	memcpy(p, prefix, prefixlen);
-	p += prefixlen;
-
-	
-	memcpy(p, iovar, namelen);
-	p += namelen;
-
-	
-	bssidx = htod32(bssidx);
-	memcpy(p, &bssidx, sizeof(int32));
-	p += sizeof(int32);
-
-	
-	if (paramlen)
-		memcpy(p, param, paramlen);
-
-	*perr = 0;
-	return iolen;
-}
-#endif 
-
-
-
-
-#define strtoul(nptr, endptr, base) bcm_strtoul((nptr), (endptr), (base))
-
-
-#if defined(CSCAN)
-
-
-
-static int
-wl_iw_combined_scan_set(struct net_device *dev, wlc_ssid_t* ssids_local, int nssid, int nchan)
-{
-	int params_size = WL_SCAN_PARAMS_FIXED_SIZE + WL_NUMCHANNELS * sizeof(uint16);
-	int err = 0;
-	char *p;
-	int i;
-	iscan_info_t *iscan = g_iscan;
-
-	WL_TRACE(("%s nssid=%d nchan=%d\n", __FUNCTION__, nssid, nchan));
-
-	if ((!dev) && (!g_iscan) && (!iscan->iscan_ex_params_p)) {
-		WL_ERROR(("%s error exit\n", __FUNCTION__));
-		err = -1;
-		goto exit;
-	}
-
-#ifdef PNO_SUPPORT
-	
-	if  (dhd_dev_get_pno_status(dev)) {
-		WL_ERROR(("%s: Scan called when PNO is active\n", __FUNCTION__));
-	}
-#endif 
-
-	params_size += WL_SCAN_PARAMS_SSID_MAX * sizeof(wlc_ssid_t);
-
-	
-	if (nssid > 0) {
-		i = OFFSETOF(wl_scan_params_t, channel_list) + nchan * sizeof(uint16);
-		i = ROUNDUP(i, sizeof(uint32));
-		if (i + nssid * sizeof(wlc_ssid_t) > params_size) {
-			printf("additional ssids exceed params_size\n");
-			err = -1;
-			goto exit;
-		}
-
-		p = ((char*)&iscan->iscan_ex_params_p->params) + i;
-		memcpy(p, ssids_local, nssid * sizeof(wlc_ssid_t));
-		p += nssid * sizeof(wlc_ssid_t);
-	} else {
-		p = (char*)iscan->iscan_ex_params_p->params.channel_list + nchan * sizeof(uint16);
-	}
-
-	
-	iscan->iscan_ex_params_p->params.channel_num =
-	        htod32((nssid << WL_SCAN_PARAMS_NSSID_SHIFT) |
-	               (nchan & WL_SCAN_PARAMS_COUNT_MASK));
-
-	nssid = (uint)
-	        ((iscan->iscan_ex_params_p->params.channel_num >> WL_SCAN_PARAMS_NSSID_SHIFT) &
-	         WL_SCAN_PARAMS_COUNT_MASK);
-
-	
-	params_size = (int) (p - (char*)iscan->iscan_ex_params_p + nssid * sizeof(wlc_ssid_t));
-	iscan->iscan_ex_param_size = params_size;
-
-	iscan->list_cur = iscan->list_hdr;
-	iscan->iscan_state = ISCAN_STATE_SCANING;
-	wl_iw_set_event_mask(dev);
-	mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
-
-	iscan->timer_on = 1;
-
-#ifdef SCAN_DUMP
-	{
-		int i;
-		WL_SCAN(("\n### List of SSIDs to scan ###\n"));
-		for (i = 0; i < nssid; i++) {
-			if (!ssids_local[i].SSID_len)
-				WL_SCAN(("%d: Broadcast scan\n", i));
-			else
-			WL_SCAN(("%d: scan  for  %s size =%d\n", i,
-				ssids_local[i].SSID, ssids_local[i].SSID_len));
-		}
-		WL_SCAN(("### List of channels to scan ###\n"));
-		for (i = 0; i < nchan; i++)
-		{
-			WL_SCAN(("%d ", iscan->iscan_ex_params_p->params.channel_list[i]));
-		}
-		WL_SCAN(("\nnprobes=%d\n", iscan->iscan_ex_params_p->params.nprobes));
-		WL_SCAN(("active_time=%d\n", iscan->iscan_ex_params_p->params.active_time));
-		WL_SCAN(("passive_time=%d\n", iscan->iscan_ex_params_p->params.passive_time));
-		WL_SCAN(("home_time=%d\n", iscan->iscan_ex_params_p->params.home_time));
-		WL_SCAN(("scan_type=%d\n", iscan->iscan_ex_params_p->params.scan_type));
-		WL_SCAN(("\n###################\n"));
-	}
-#endif 
-
-	if (params_size > WLC_IOCTL_MEDLEN) {
-			WL_ERROR(("Set ISCAN for %s due to params_size=%d  \n",
-				__FUNCTION__, params_size));
-			err = -1;
-	}
-
-	if ((err = dev_iw_iovar_setbuf(dev, "iscan", iscan->iscan_ex_params_p,
-	                               iscan->iscan_ex_param_size,
-	                               iscan->ioctlbuf, sizeof(iscan->ioctlbuf)))) {
-		WL_TRACE(("Set ISCAN for %s failed with %d\n", __FUNCTION__, err));
-		err = -1;
-	}
-
-exit:
-	return err;
-}
-
-
-static int
-iwpriv_set_cscan(struct net_device *dev, struct iw_request_info *info,
-                 union iwreq_data *wrqu, char *ext)
-{
-	int res;
-	char  *extra = NULL;
-	iscan_info_t *iscan = g_iscan;
-	wlc_ssid_t ssids_local[WL_SCAN_PARAMS_SSID_MAX];
-	int nssid = 0;
-	int nchan = 0;
-	char *str_ptr;
-
-	WL_TRACE(("%s: info->cmd:%x, info->flags:%x, u.data=0x%p, u.len=%d\n",
-		__FUNCTION__, info->cmd, info->flags,
-		wrqu->data.pointer, wrqu->data.length));
-
-	if (g_onoff == G_WLAN_SET_OFF) {
-		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
-		return -ENODEV;
-	}
-
-	if (wrqu->data.length == 0) {
-		WL_ERROR(("IWPRIV argument len = 0\n"));
-		return -EINVAL;
-	}
-
-	if (!iscan->iscan_ex_params_p) {
-		return -EFAULT;
-	}
-
-	if (!(extra = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
-		return -ENOMEM;
-
-	if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
-		res = -EFAULT;
-		goto exit_proc;
-	}
-
-	extra[wrqu->data.length] = 0;
-	WL_ERROR(("Got str param in iw_point:\n %s\n", extra));
-
-	str_ptr = extra;
-
-	
-	if (strncmp(str_ptr, GET_SSID, strlen(GET_SSID))) {
-		WL_ERROR(("%s Error: extracting SSID='' string\n", __FUNCTION__));
-		res = -EINVAL;
-		goto exit_proc;
-	}
-
-	str_ptr += strlen(GET_SSID);
-	nssid = wl_iw_parse_ssid_list(&str_ptr, ssids_local, nssid,
-	                              WL_SCAN_PARAMS_SSID_MAX);
-	if (nssid == -1) {
-		WL_ERROR(("%s wrong ssid list", __FUNCTION__));
-		res = -EINVAL;
-		goto exit_proc;
-	}
-
-	memset(iscan->iscan_ex_params_p, 0, iscan->iscan_ex_param_size);
-	ASSERT(iscan->iscan_ex_param_size < WLC_IOCTL_MAXLEN);
-
-	
-	wl_iw_iscan_prep(&iscan->iscan_ex_params_p->params, NULL);
-	iscan->iscan_ex_params_p->version = htod32(ISCAN_REQ_VERSION);
-	iscan->iscan_ex_params_p->action = htod16(WL_SCAN_ACTION_START);
-	iscan->iscan_ex_params_p->scan_duration = htod16(0);
-
-	
-	if ((nchan = wl_iw_parse_channel_list(&str_ptr,
-	                                      &iscan->iscan_ex_params_p->params.channel_list[0],
-	                                      WL_NUMCHANNELS)) == -1) {
-		WL_ERROR(("%s missing channel list\n", __FUNCTION__));
-		res = -EINVAL;
-		goto exit_proc;
-	}
-
-	
-	get_parameter_from_string(&str_ptr,
-	                          GET_NPROBE, PTYPE_INTDEC,
-	                          &iscan->iscan_ex_params_p->params.nprobes, 2);
-
-	get_parameter_from_string(&str_ptr, GET_ACTIVE_ASSOC_DWELL, PTYPE_INTDEC,
-	                          &iscan->iscan_ex_params_p->params.active_time, 4);
-
-	get_parameter_from_string(&str_ptr, GET_PASSIVE_ASSOC_DWELL, PTYPE_INTDEC,
-	                          &iscan->iscan_ex_params_p->params.passive_time, 4);
-
-	get_parameter_from_string(&str_ptr, GET_HOME_DWELL, PTYPE_INTDEC,
-	                          &iscan->iscan_ex_params_p->params.home_time, 4);
-
-	get_parameter_from_string(&str_ptr, GET_SCAN_TYPE, PTYPE_INTDEC,
-	                          &iscan->iscan_ex_params_p->params.scan_type, 1);
-
-	
-	res = wl_iw_combined_scan_set(dev, ssids_local, nssid, nchan);
-
-exit_proc:
-	kfree(extra);
-
-	return res;
-}
-
-
-static int
-wl_iw_set_cscan(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *extra
-)
-{
-	int res = -1;
-	iscan_info_t *iscan = g_iscan;
-	wlc_ssid_t ssids_local[WL_SCAN_PARAMS_SSID_MAX];
-	int nssid = 0;
-	int nchan = 0;
-	cscan_tlv_t *cscan_tlv_temp;
-	char type;
-	char *str_ptr;
-	int tlv_size_left;
-#ifdef TLV_DEBUG
-	int i;
-	char tlv_in_example[] = {
-		'C', 'S', 'C', 'A', 'N', ' ',
-		0x53, 0x01, 0x00, 0x00,
-		'S',	  
-		0x00, 
-		'S',    
-		0x04, 
-		'B', 'R', 'C', 'M',
-		'C',
-		0x06, 
-		'P', 
-		0x94,
-		0x11,
-		'T',     
-		0x01  
-	};
-#endif 
-
-	WL_TRACE(("\n### %s: info->cmd:%x, info->flags:%x, u.data=0x%p, u.len=%d\n",
-		__FUNCTION__, info->cmd, info->flags,
-		wrqu->data.pointer, wrqu->data.length));
-
-	net_os_wake_lock(dev);
-
-	if (g_onoff == G_WLAN_SET_OFF) {
-		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
-		return -1;
-	}
-
-	if (wrqu->data.length < (strlen(CSCAN_COMMAND) + sizeof(cscan_tlv_t))) {
-		WL_ERROR(("%s argument=%d  less %d\n", __FUNCTION__,
-		          wrqu->data.length, (int)(strlen(CSCAN_COMMAND) + sizeof(cscan_tlv_t))));
-		return -1;
-	}
-
-#ifdef TLV_DEBUG
-	memcpy(extra, tlv_in_example, sizeof(tlv_in_example));
-	wrqu->data.length = sizeof(tlv_in_example);
-	for (i = 0; i < wrqu->data.length; i++)
-		printf("%02X ", extra[i]);
-	printf("\n");
-#endif 
-
-	str_ptr = extra;
-	str_ptr +=  strlen(CSCAN_COMMAND);
-	tlv_size_left = wrqu->data.length - strlen(CSCAN_COMMAND);
-
-	cscan_tlv_temp = (cscan_tlv_t *)str_ptr;
-	memset(ssids_local, 0, sizeof(ssids_local));
-	
-	if ((cscan_tlv_temp->prefix == CSCAN_TLV_PREFIX) &&
-		(cscan_tlv_temp->version == CSCAN_TLV_VERSION) &&
-		(cscan_tlv_temp->subver == CSCAN_TLV_SUBVERSION))
-	{
-		str_ptr += sizeof(cscan_tlv_t);
-		tlv_size_left  -= sizeof(cscan_tlv_t);
-
-		
-		if ((nssid = wl_iw_parse_ssid_list_tlv(&str_ptr, ssids_local,
-			WL_SCAN_PARAMS_SSID_MAX, &tlv_size_left)) <= 0) {
-			WL_ERROR(("SSID is not presented or corrupted ret=%d\n", nssid));
-			goto exit_proc;
-		}
-		else {
-			
-			memset(iscan->iscan_ex_params_p, 0, iscan->iscan_ex_param_size);
-
-			
-			wl_iw_iscan_prep(&iscan->iscan_ex_params_p->params, NULL);
-			iscan->iscan_ex_params_p->version = htod32(ISCAN_REQ_VERSION);
-			iscan->iscan_ex_params_p->action = htod16(WL_SCAN_ACTION_START);
-			iscan->iscan_ex_params_p->scan_duration = htod16(0);
-
-			
-			while (tlv_size_left > 0)
-			{
-			type = str_ptr[0];
-			switch (type) {
-				case CSCAN_TLV_TYPE_CHANNEL_IE:
-					
-					if ((nchan = wl_iw_parse_channel_list_tlv(&str_ptr,
-					&iscan->iscan_ex_params_p->params.channel_list[0],
-					WL_NUMCHANNELS, &tlv_size_left)) == -1) {
-					WL_ERROR(("%s missing channel list\n",
-						__FUNCTION__));
-						goto exit_proc;
-					}
-				break;
-				case CSCAN_TLV_TYPE_NPROBE_IE:
-					if ((res = wl_iw_parse_data_tlv(&str_ptr,
-						&iscan->iscan_ex_params_p->params.nprobes,
-						sizeof(iscan->iscan_ex_params_p->params.nprobes),
-						type, sizeof(char), &tlv_size_left)) == -1) {
-						WL_ERROR(("%s return %d\n",
-							__FUNCTION__, res));
-							goto exit_proc;
-					}
-				break;
-				case CSCAN_TLV_TYPE_ACTIVE_IE:
-					if ((res = wl_iw_parse_data_tlv(&str_ptr,
-					&iscan->iscan_ex_params_p->params.active_time,
-					sizeof(iscan->iscan_ex_params_p->params.active_time),
-					type, sizeof(short), &tlv_size_left)) == -1) {
-						WL_ERROR(("%s return %d\n",
-						__FUNCTION__, res));
-						goto exit_proc;
-					}
-				break;
-				case CSCAN_TLV_TYPE_PASSIVE_IE:
-					if ((res = wl_iw_parse_data_tlv(&str_ptr,
-					&iscan->iscan_ex_params_p->params.passive_time,
-					sizeof(iscan->iscan_ex_params_p->params.passive_time),
-					type, sizeof(short), &tlv_size_left)) == -1) {
-						WL_ERROR(("%s return %d\n",
-						__FUNCTION__, res));
-						goto exit_proc;
-					}
-				break;
-				case CSCAN_TLV_TYPE_HOME_IE:
-					if ((res = wl_iw_parse_data_tlv(&str_ptr,
-					&iscan->iscan_ex_params_p->params.home_time,
-					sizeof(iscan->iscan_ex_params_p->params.home_time),
-					type, sizeof(short), &tlv_size_left)) == -1) {
-						WL_ERROR(("%s return %d\n",
-						__FUNCTION__, res));
-						goto exit_proc;
-					}
-				break;
-				case CSCAN_TLV_TYPE_STYPE_IE:
-					if ((res = wl_iw_parse_data_tlv(&str_ptr,
-					&iscan->iscan_ex_params_p->params.scan_type,
-					sizeof(iscan->iscan_ex_params_p->params.scan_type),
-					type, sizeof(char), &tlv_size_left)) == -1) {
-					WL_ERROR(("%s return %d\n",
-						__FUNCTION__, res));
-						goto exit_proc;
-					}
-				break;
-
-				default :
-					WL_ERROR(("%s get unkwown type %X\n",
-					__FUNCTION__, type));
-					goto exit_proc;
-				break;
-				}
-			} 
-			}
-		}
-		else {
-			WL_ERROR(("%s get wrong TLV command\n", __FUNCTION__));
-			goto exit_proc;
-		}
-
-#if defined(CONFIG_FIRST_SCAN)
-		if (g_first_broadcast_scan < BROADCAST_SCAN_FIRST_RESULT_CONSUMED) {
-			if (++g_first_counter_scans == MAX_ALLOWED_BLOCK_SCAN_FROM_FIRST_SCAN) {
-				
-				WL_ERROR(("%s Clean up First scan flag which is %d\n",
-					__FUNCTION__, g_first_broadcast_scan));
-				g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_CONSUMED;
-			}
-			else {
-				WL_ERROR(("%s Ignoring CSCAN : First Scan is not done yet %d\n",
-					__FUNCTION__, g_first_counter_scans));
-				return -EBUSY;
-			}
-		}
-#endif 
-
-		
-		res = wl_iw_combined_scan_set(dev, ssids_local, nssid, nchan);
-
-exit_proc:
-	net_os_wake_unlock(dev);
-	return res;
-}
-
-#endif 
-
-
-
-#ifdef SOFTAP
-#ifndef AP_ONLY
-
-
-static int
-thr_wait_for_2nd_eth_dev(void *data)
-{
-	wl_iw_t *iw;
-	int ret = 0;
-	unsigned long flags = 0;
-
-	tsk_ctl_t *tsk_ctl = (tsk_ctl_t *)data;
-	struct net_device *dev = (struct net_device *)tsk_ctl->parent;
-	iw = *(wl_iw_t **)netdev_priv(dev);
-
-	DAEMONIZE("wl0_eth_wthread");
-
-
-	WL_SOFTAP(("\n>%s threda started:, PID:%x\n", __FUNCTION__, current->pid));
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-	if (!iw) {
-		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
-		tsk_ctl->thr_pid = -1;
-		complete(&tsk_ctl->completed);
-		return -1;
-	}
-	DHD_OS_WAKE_LOCK(iw->pub);
-	complete(&tsk_ctl->completed);
-	if (down_timeout(&tsk_ctl->sema, msecs_to_jiffies(1000)) != 0) {
-#else
-	if (down_interruptible(&tsk_ctl->sema) != 0) {
-#endif 
-		WL_ERROR(("\n%s: sap_eth_sema timeout \n", __FUNCTION__));
-		ret = -1;
-		goto fail;
-	}
-
-	SMP_RD_BARRIER_DEPENDS();
-	if (tsk_ctl->terminated) {
-			ret = -1;
-			goto fail;
-	}
-
-	flags = dhd_os_spin_lock(iw->pub);
-	if (!ap_net_dev) {
-		WL_ERROR((" ap_net_dev is null !!!"));
-		ret = -1;
-		dhd_os_spin_unlock(iw->pub, flags);
-		goto fail;
-	}
-
-	WL_SOFTAP(("\n>%s: Thread:'softap ethdev IF:%s is detected!'\n\n",
-		__FUNCTION__, ap_net_dev->name));
-
-	ap_cfg_running = TRUE;
-
-	dhd_os_spin_unlock(iw->pub, flags);
-	bcm_mdelay(500); 
-
-	
-	wl_iw_send_priv_event(priv_dev, "AP_SET_CFG_OK");
-
-fail:
-
-	DHD_OS_WAKE_UNLOCK(iw->pub);
-
-	WL_SOFTAP(("\n>%s, thread completed\n", __FUNCTION__));
-
-	complete_and_exit(&tsk_ctl->completed, 0);
-	return ret;
-}
-#endif 
-#ifndef AP_ONLY
-static int last_auto_channel = 6;
-#endif
-
-static int
-get_softap_auto_channel(struct net_device *dev, struct ap_profile *ap)
-{
-	int chosen = 0;
-	wl_uint32_list_t request;
-	int retry = 0;
-	int updown = 0;
-	int ret = 0;
-	wlc_ssid_t null_ssid;
-	int res = 0;
-#ifndef AP_ONLY
-	int iolen = 0;
-	int mkvar_err = 0;
-	int bsscfg_index = 1;
-	char buf[WLC_IOCTL_SMLEN];
-#endif
-	WL_SOFTAP(("Enter %s\n", __FUNCTION__));
-
-#ifndef AP_ONLY
-	if (ap_cfg_running) {
-		ap->channel = last_auto_channel;
-		return res;
-	}
-#endif
-	memset(&null_ssid, 0, sizeof(wlc_ssid_t));
-	res |= dev_wlc_ioctl(dev, WLC_UP, &updown, sizeof(updown));
-#ifdef AP_ONLY
-	res |= dev_wlc_ioctl(dev, WLC_SET_SSID, &null_ssid, sizeof(null_ssid));
-#else
-	iolen = wl_bssiovar_mkbuf("ssid", bsscfg_index, (char *)(&null_ssid),
-		null_ssid.SSID_len+4, buf, sizeof(buf), &mkvar_err);
-	ASSERT(iolen);
-	res |= dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen);
-
-#endif
-
-	request.count = htod32(0);
-	ret = dev_wlc_ioctl(dev, WLC_START_CHANNEL_SEL, &request, sizeof(request));
-	if (ret < 0) {
-		WL_ERROR(("can't start auto channel scan\n"));
-		goto fail;
-	}
-
-	get_channel_retry:
-		bcm_mdelay(350);
-
-	ret = dev_wlc_ioctl(dev, WLC_GET_CHANNEL_SEL, &chosen, sizeof(chosen));
-		if (ret < 0 || dtoh32(chosen) == 0) {
-			if (retry++ < 15) {
-				goto get_channel_retry;
-			} else {
-				if (ret < 0) {
-					WL_ERROR(("can't get auto channel sel, err = %d, "
-					          "chosen = 0x%04X\n", ret, (uint16)chosen));
-					goto fail;
-				} else {
-					ap->channel = (uint16)last_auto_channel;
-					WL_ERROR(("auto channel sel timed out. we get channel %d\n",
-						ap->channel));
-				}
-			}
-		}
-
-		if (chosen) {
-			ap->channel = (uint16)chosen & 0x00FF;
-			WL_SOFTAP(("%s: Got auto channel = %d, attempt:%d\n",
-				__FUNCTION__, ap->channel, retry));
-		}
-
-		if ((res = dev_wlc_ioctl(dev, WLC_DOWN, &updown, sizeof(updown))) < 0) {
-			WL_ERROR(("%s fail to set up err =%d\n", __FUNCTION__, res));
-			goto fail;
-		}
-
-#ifndef AP_ONLY
-	if (!res || !ret)
-		last_auto_channel = ap->channel;
-#endif
-
-fail :
-	if (ret < 0) {
-		WL_TRACE(("%s: return value %d\n", __FUNCTION__, ret));
-		return ret;
-	}
-	return res;
-} 
-
-
-static int
-set_ap_cfg(struct net_device *dev, struct ap_profile *ap)
-{
-	int updown = 0;
-	int channel = 0;
-
-	wlc_ssid_t ap_ssid;
-	int max_assoc = 8;
-
-	int res = 0;
-	int apsta_var = 0;
-#ifndef AP_ONLY
-	int mpc = 0;
-	int iolen = 0;
-	int mkvar_err = 0;
-	int bsscfg_index = 1;
-	char buf[WLC_IOCTL_SMLEN];
-#endif
-
-	if (!dev) {
-		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
-		return -1;
-	}
-
-	net_os_wake_lock(dev);
-	DHD_OS_MUTEX_LOCK(&wl_softap_lock);
-
-	WL_SOFTAP(("wl_iw: set ap profile:\n"));
-	WL_SOFTAP(("	ssid = '%s'\n", ap->ssid));
-	WL_SOFTAP(("	security = '%s'\n", ap->sec));
-	if (ap->key[0] != '\0')
-		WL_SOFTAP(("	key = '%s'\n", ap->key));
-	WL_SOFTAP(("	channel = %d\n", ap->channel));
-	WL_SOFTAP(("	max scb = %d\n", ap->max_scb));
-
-#ifdef AP_ONLY
-	if (ap_cfg_running) {
-		wl_iw_softap_deassoc_stations(dev, NULL); 
-		ap_cfg_running = FALSE;
-	}
-#endif	
-
-	
-	if (ap_cfg_running == FALSE) {
-
-#ifndef AP_ONLY
-
-		
-		sema_init(&ap_eth_ctl.sema, 0);
-
-		mpc = 0;
-		if ((res = dev_wlc_intvar_set(dev, "mpc", mpc))) {
-			WL_ERROR(("%s fail to set mpc\n", __FUNCTION__));
-			goto fail;
-		}
-#endif
-
-		updown = 0;
-		if ((res = dev_wlc_ioctl(dev, WLC_DOWN, &updown, sizeof(updown)))) {
-			WL_ERROR(("%s fail to set updown\n", __FUNCTION__));
-			goto fail;
-		}
-
-#ifdef AP_ONLY
-		
-		apsta_var = 0;
-		if ((res = dev_wlc_ioctl(dev, WLC_SET_AP, &apsta_var, sizeof(apsta_var)))) {
-			WL_ERROR(("%s fail to set apsta_var 0\n", __FUNCTION__));
-			goto fail;
-		}
-		apsta_var = 1;
-		if ((res = dev_wlc_ioctl(dev, WLC_SET_AP, &apsta_var, sizeof(apsta_var)))) {
-			WL_ERROR(("%s fail to set apsta_var 1\n", __FUNCTION__));
-			goto fail;
-		}
-		res = dev_wlc_ioctl(dev, WLC_GET_AP, &apsta_var, sizeof(apsta_var));
-#else
-		
-		apsta_var = 1;
-		iolen = wl_bssiovar_mkbuf("apsta",
-			bsscfg_index,  &apsta_var, sizeof(apsta_var)+4,
-			buf, sizeof(buf), &mkvar_err);
-		ASSERT(iolen);
-		if ((res = dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen)) < 0) {
-			WL_ERROR(("%s fail to set apsta \n", __FUNCTION__));
-			goto fail;
-		}
-		WL_TRACE(("\n>in %s: apsta set result: %d \n", __FUNCTION__, res));
-
-
-		mpc = 0;
-		if ((res = dev_wlc_intvar_set(dev, "mpc", mpc))) {
-			WL_ERROR(("%s fail to set mpc\n", __FUNCTION__));
-			goto fail;
-		}
-
-
-#endif 
-
-		updown = 1;
-		if ((res = dev_wlc_ioctl(dev, WLC_UP, &updown, sizeof(updown))) < 0) {
-			WL_ERROR(("%s fail to set apsta \n", __FUNCTION__));
-			goto fail;
-		}
-
-	} else {
-		
-		if (!ap_net_dev) {
-			WL_ERROR(("%s: ap_net_dev is null\n", __FUNCTION__));
-			goto fail;
-		}
-
-		res = wl_iw_softap_deassoc_stations(ap_net_dev, NULL);
-
-		
-		if ((res = dev_iw_write_cfg1_bss_var(dev, 0)) < 0) {
-			WL_ERROR(("%s fail to set bss down\n", __FUNCTION__));
-			goto fail;
-		}
-	}
-
-
-	if (strlen(ap->country_code)) {
-		WL_ERROR(("%s: Igonored: Country MUST be specified"
-			"COUNTRY command with \n",	__FUNCTION__));
-	} else {
-		WL_SOFTAP(("%s: Country code is not specified,"
-			" will use Radio's default\n",
-			__FUNCTION__));
-
-	}
-	iolen = wl_bssiovar_mkbuf("closednet",
-		bsscfg_index,  &ap->closednet, sizeof(ap->closednet)+4,
-		buf, sizeof(buf), &mkvar_err);
-	ASSERT(iolen);
-	if ((res = dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen)) < 0) {
-		WL_ERROR(("%s failed to set 'closednet'for apsta \n", __FUNCTION__));
-		goto fail;
-	}
-
-	
-	if ((ap->channel == 0) && (get_softap_auto_channel(dev, ap) < 0)) {
-		ap->channel = 1;
-		WL_ERROR(("%s auto channel failed, use channel=%d\n",
-		          __FUNCTION__, ap->channel));
-	}
-
-	channel = ap->channel;
-	if ((res = dev_wlc_ioctl(dev, WLC_SET_CHANNEL, &channel, sizeof(channel)))) {
-		WL_ERROR(("%s fail to set channel\n", __FUNCTION__));
-	}
-
-
-	if (ap_cfg_running == FALSE) {
-		updown = 0;
-		if ((res = dev_wlc_ioctl(dev, WLC_UP, &updown, sizeof(updown)))) {
-			WL_ERROR(("%s fail to set up\n", __FUNCTION__));
-			goto fail;
-		}
-	}
-
-	max_assoc = ap->max_scb;
-	if ((res = dev_wlc_intvar_set(dev, "maxassoc", max_assoc))) {
-		WL_ERROR(("%s fail to set maxassoc\n", __FUNCTION__));
-		goto fail;
-	}
-
-	ap_ssid.SSID_len = strlen(ap->ssid);
-	strncpy(ap_ssid.SSID, ap->ssid, ap_ssid.SSID_len);
-
-	
-#ifdef AP_ONLY
-	if ((res = wl_iw_set_ap_security(dev, &my_ap)) != 0) {
-		WL_ERROR(("ERROR:%d in:%s, wl_iw_set_ap_security is skipped\n",
-		          res, __FUNCTION__));
-		goto fail;
-	}
-	wl_iw_send_priv_event(dev, "ASCII_CMD=AP_BSS_START");
-	ap_cfg_running = TRUE;
-#else
-
-	iolen = wl_bssiovar_mkbuf("ssid", bsscfg_index, (char *)(&ap_ssid),
-		ap_ssid.SSID_len+4, buf, sizeof(buf), &mkvar_err);
-	ASSERT(iolen);
-	if ((res = dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen)) != 0) {
-		WL_ERROR(("ERROR:%d in:%s, Security & BSS reconfiguration is skipped\n",
-		          res, __FUNCTION__));
-		goto fail;
-	}
-	if (ap_cfg_running == FALSE) {
-		
-		PROC_START(thr_wait_for_2nd_eth_dev, dev, &ap_eth_ctl, 0);
-	} else {
-		ap_eth_ctl.thr_pid = -1;
-		
-		if (ap_net_dev == NULL) {
-			WL_ERROR(("%s ERROR: ap_net_dev is NULL !!!\n", __FUNCTION__));
-			goto fail;
-		}
-
-		WL_ERROR(("%s: %s Configure security & restart AP bss \n",
-		          __FUNCTION__, ap_net_dev->name));
-
-		
-		if ((res = wl_iw_set_ap_security(ap_net_dev, &my_ap)) < 0) {
-			WL_ERROR(("%s fail to set security : %d\n", __FUNCTION__, res));
-			goto fail;
-		}
-
-		
-		if ((res = dev_iw_write_cfg1_bss_var(dev, 1)) < 0) {
-			WL_ERROR(("%s fail to set bss up\n", __FUNCTION__));
-			goto fail;
-		}
-	}
-#endif 
-fail:
-	WL_SOFTAP(("%s exit with %d\n", __FUNCTION__, res));
-
-	DHD_OS_MUTEX_UNLOCK(&wl_softap_lock);
-	net_os_wake_unlock(dev);
-
-	return res;
-}
-#endif 
-
-
-
-static int
-wl_iw_set_ap_security(struct net_device *dev, struct ap_profile *ap)
-{
-	int wsec = 0;
-	int wpa_auth = 0;
-	int res = 0;
-	int i;
-	char *ptr;
-#ifdef AP_ONLY
-	int mpc = 0;
-	wlc_ssid_t ap_ssid;
-#endif
-	wl_wsec_key_t key;
-
-	WL_SOFTAP(("\nsetting SOFTAP security mode:\n"));
-	WL_SOFTAP(("wl_iw: set ap profile:\n"));
-	WL_SOFTAP(("	ssid = '%s'\n", ap->ssid));
-	WL_SOFTAP(("	security = '%s'\n", ap->sec));
-	if (ap->key[0] != '\0')
-		WL_SOFTAP(("	key = '%s'\n", ap->key));
-	WL_SOFTAP(("	channel = %d\n", ap->channel));
-	WL_SOFTAP(("	max scb = %d\n", ap->max_scb));
-
-	if (strnicmp(ap->sec, "open", strlen("open")) == 0) {
-
-	   
-		wsec = 0;
-		res = dev_wlc_intvar_set(dev, "wsec", wsec);
-		wpa_auth = WPA_AUTH_DISABLED;
-		res |= dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
-
-		WL_SOFTAP(("=====================\n"));
-		WL_SOFTAP((" wsec & wpa_auth set 'OPEN', result:&d %d\n", res));
-		WL_SOFTAP(("=====================\n"));
-
-	} else if (strnicmp(ap->sec, "wep", strlen("wep")) == 0) {
-
-	   
-		memset(&key, 0, sizeof(key));
-
-		wsec = WEP_ENABLED;
-		res = dev_wlc_intvar_set(dev, "wsec", wsec);
-
-		key.index = 0;
-		if (wl_iw_parse_wep(ap->key, &key)) {
-			WL_SOFTAP(("wep key parse err!\n"));
-			return -1;
-		}
-
-		key.index = htod32(key.index);
-		key.len = htod32(key.len);
-		key.algo = htod32(key.algo);
-		key.flags = htod32(key.flags);
-
-		res |= dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
-
-		wpa_auth = WPA_AUTH_DISABLED;
-		res |= dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
-
-		WL_SOFTAP(("=====================\n"));
-		WL_SOFTAP((" wsec & auth set 'WEP', result:&d %d\n", res));
-		WL_SOFTAP(("=====================\n"));
-
-	} else if (strnicmp(ap->sec, "wpa2-psk", strlen("wpa2-psk")) == 0) {
-
-	   
-
-		wsec_pmk_t psk;
-		size_t key_len;
-
-		wsec = AES_ENABLED;
-		dev_wlc_intvar_set(dev, "wsec", wsec);
-
-		key_len = strlen(ap->key);
-		if (key_len < WSEC_MIN_PSK_LEN || key_len > WSEC_MAX_PSK_LEN) {
-			WL_SOFTAP(("passphrase must be between %d and %d characters long\n",
-			WSEC_MIN_PSK_LEN, WSEC_MAX_PSK_LEN));
-			return -1;
-		}
-
-		
-		if (key_len < WSEC_MAX_PSK_LEN) {
-			unsigned char output[2*SHA1HashSize];
-			char key_str_buf[WSEC_MAX_PSK_LEN+1];
-
-			
-			memset(output, 0, sizeof(output));
-			pbkdf2_sha1(ap->key, ap->ssid, strlen(ap->ssid), 4096, output, 32);
-			
-			ptr = key_str_buf;
-			for (i = 0; i < (WSEC_MAX_PSK_LEN/8); i++) {
-				
-				sprintf(ptr, "%02x%02x%02x%02x", (uint)output[i*4],
-				        (uint)output[i*4+1], (uint)output[i*4+2],
-				        (uint)output[i*4+3]);
-				ptr += 8;
-			}
-			WL_SOFTAP(("%s: passphase = %s\n", __FUNCTION__, key_str_buf));
-
-			psk.key_len = htod16((ushort)WSEC_MAX_PSK_LEN);
-			memcpy(psk.key, key_str_buf, psk.key_len);
-		} else {
-			psk.key_len = htod16((ushort) key_len);
-			memcpy(psk.key, ap->key, key_len);
-		}
-		psk.flags = htod16(WSEC_PASSPHRASE);
-		dev_wlc_ioctl(dev, WLC_SET_WSEC_PMK, &psk, sizeof(psk));
-
-		wpa_auth = WPA2_AUTH_PSK;
-		dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
-
-	} else if (strnicmp(ap->sec, "wpa-psk", strlen("wpa-psk")) == 0) {
-
-		
-		wsec_pmk_t psk;
-		size_t key_len;
-
-		wsec = TKIP_ENABLED;
-		res = dev_wlc_intvar_set(dev, "wsec", wsec);
-
-		key_len = strlen(ap->key);
-		if (key_len < WSEC_MIN_PSK_LEN || key_len > WSEC_MAX_PSK_LEN) {
-			WL_SOFTAP(("passphrase must be between %d and %d characters long\n",
-			WSEC_MIN_PSK_LEN, WSEC_MAX_PSK_LEN));
-			return -1;
-		}
-
-		
-		if (key_len < WSEC_MAX_PSK_LEN) {
-			unsigned char output[2*SHA1HashSize];
-			char key_str_buf[WSEC_MAX_PSK_LEN+1];
-			bzero(output, 2*SHA1HashSize);
-
-			WL_SOFTAP(("%s: do passhash...\n", __FUNCTION__));
-			
-			pbkdf2_sha1(ap->key, ap->ssid, strlen(ap->ssid), 4096, output, 32);
-			
-			ptr = key_str_buf;
-			for (i = 0; i < (WSEC_MAX_PSK_LEN/8); i++) {
-				WL_SOFTAP(("[%02d]: %08x\n", i, *((unsigned int*)&output[i*4])));
-				
-				sprintf(ptr, "%02x%02x%02x%02x", (uint)output[i*4],
-					(uint)output[i*4+1], (uint)output[i*4+2],
-				        (uint)output[i*4+3]);
-				ptr += 8;
-			}
-			printk("%s: passphase = %s\n", __FUNCTION__, key_str_buf);
-
-			psk.key_len = htod16((ushort)WSEC_MAX_PSK_LEN);
-			memcpy(psk.key, key_str_buf, psk.key_len);
-		} else {
-			psk.key_len = htod16((ushort) key_len);
-			memcpy(psk.key, ap->key, key_len);
-		}
-
-		psk.flags = htod16(WSEC_PASSPHRASE);
-		res |= dev_wlc_ioctl(dev, WLC_SET_WSEC_PMK, &psk, sizeof(psk));
-
-		wpa_auth = WPA_AUTH_PSK;
-		res |= dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
-
-		WL_SOFTAP((" wsec & auth set 'wpa-psk' (TKIP), result:&d %d\n", res));
-	}
-
-#ifdef AP_ONLY
-		ap_ssid.SSID_len = strlen(ap->ssid);
-		strncpy(ap_ssid.SSID, ap->ssid, ap_ssid.SSID_len);
-		res |= dev_wlc_ioctl(dev, WLC_SET_SSID, &ap_ssid, sizeof(ap_ssid));
-		mpc = 0;
-		res |= dev_wlc_intvar_set(dev, "mpc", mpc);
-		if (strnicmp(ap->sec, "wep", strlen("wep")) == 0) {
-			res |= dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
-		}
-#endif
-	return res;
-}
-
-
-
-static int
-get_parameter_from_string(
-			char **str_ptr, const char *token,
-			int param_type, void  *dst, int param_max_len)
-{
-	char int_str[7] = "0";
-	int parm_str_len;
-	char  *param_str_begin;
-	char  *param_str_end;
-	char  *orig_str = *str_ptr;
-
-	if ((*str_ptr) && !strncmp(*str_ptr, token, strlen(token))) {
-
-		strsep(str_ptr, "=,"); 
-		param_str_begin = *str_ptr;
-		strsep(str_ptr, "=,"); 
-
-		if (*str_ptr == NULL) {
-			
-			parm_str_len = strlen(param_str_begin);
-		} else {
-			param_str_end = *str_ptr-1;  
-			parm_str_len = param_str_end - param_str_begin;
-		}
-
-		WL_TRACE((" 'token:%s', len:%d, ", token, parm_str_len));
-
-		if (parm_str_len > param_max_len) {
-			WL_ERROR((" WARNING: extracted param len:%d is > MAX:%d\n",
-				parm_str_len, param_max_len));
-
-			parm_str_len = param_max_len;
-		}
-
-		switch (param_type) {
-
-			case PTYPE_INTDEC: {
-			
-				int *pdst_int = dst;
-				char *eptr;
-
-				if (parm_str_len > sizeof(int_str))
-					 parm_str_len = sizeof(int_str);
-
-				memcpy(int_str, param_str_begin, parm_str_len);
-
-				*pdst_int = simple_strtoul(int_str, &eptr, 10);
-
-				WL_TRACE((" written as integer:%d\n",  *pdst_int));
-			}
-			break;
-			case PTYPE_STR_HEX: {
-				u8 *buf = dst;
-				
-				param_max_len = param_max_len >> 1;  
-				hstr_2_buf(param_str_begin, buf, param_max_len);
-				dhd_print_buf(buf, param_max_len, 0);
-			}
-			break;
-			default:
-				
-				memcpy(dst, param_str_begin, parm_str_len);
-				*((char *)dst + parm_str_len) = 0; 
-				WL_ERROR((" written as a string:%s\n", (char *)dst));
-			break;
-
-		}
-
-		return 0;
-	} else {
-		WL_ERROR(("\n %s: ERROR: can't find token:%s in str:%s \n",
-			__FUNCTION__, token, orig_str));
-
-	 return -1;
-	}
-}
-
-static int wl_iw_softap_deassoc_stations(struct net_device *dev, u8 *mac)
-{
-	int i;
-	int res = 0;
-	char mac_buf[128] = {0};
-	char z_mac[6] = {0, 0, 0, 0, 0, 0};
-	char *sta_mac;
-	struct maclist *assoc_maclist = (struct maclist *) mac_buf;
-	bool deauth_all = FALSE;
-
-	
-	if (mac == NULL) {
-		deauth_all = TRUE;
-		sta_mac = z_mac;  
-	} else {
-		sta_mac = mac;  
-	}
-
-	memset(assoc_maclist, 0, sizeof(mac_buf));
-	assoc_maclist->count = 8; 
-
-	res = dev_wlc_ioctl(dev, WLC_GET_ASSOCLIST, assoc_maclist, 128);
-	if (res != 0) {
-		WL_SOFTAP(("%s: Error:%d Couldn't get ASSOC List\n", __FUNCTION__, res));
-		return res;
-	}
-
-	if (assoc_maclist->count)
-		for (i = 0; i < assoc_maclist->count; i++) {
-		scb_val_t scbval;
-		scbval.val = htod32(1);
-		
-		bcopy(&assoc_maclist->ea[i], &scbval.ea, ETHER_ADDR_LEN);
-
-		if (deauth_all || (memcmp(&scbval.ea, sta_mac, ETHER_ADDR_LEN) == 0))  {
-			
-			WL_SOFTAP(("%s, deauth STA:%d \n", __FUNCTION__, i));
-			res |= dev_wlc_ioctl(dev, WLC_SCB_DEAUTHENTICATE_FOR_REASON,
-				&scbval, sizeof(scb_val_t));
-		}
-	} else WL_SOFTAP(("%s: No Stations \n", __FUNCTION__));
-
-	if (res != 0) {
-		WL_ERROR(("%s: Error:%d\n", __FUNCTION__, res));
-	} else if (assoc_maclist->count) {
-		
-		bcm_mdelay(200);
-	}
-	return res;
-}
-
-
-
-static int
-iwpriv_softap_stop(struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *ext)
-{
-	int res = 0;
-
-	WL_SOFTAP(("got iwpriv AP_BSS_STOP \n"));
-
-	if ((!dev) && (!ap_net_dev)) {
-		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
-		return res;
-	}
-
-	net_os_wake_lock(dev);
-	DHD_OS_MUTEX_LOCK(&wl_softap_lock);
-
-	if  ((ap_cfg_running == TRUE)) {
-#ifdef AP_ONLY
-		 wl_iw_softap_deassoc_stations(dev, NULL);
-#else
-		 wl_iw_softap_deassoc_stations(ap_net_dev, NULL);
-		if ((res = dev_iw_write_cfg1_bss_var(dev, 2)) < 0)
-			WL_ERROR(("%s failed to del BSS err = %d", __FUNCTION__, res));
-#endif
-
-		
-		bcm_mdelay(100);
-
-		wrqu->data.length = 0;
-		ap_cfg_running = FALSE;
-	} else
-		WL_ERROR(("%s: was called when SoftAP is OFF : move on\n", __FUNCTION__));
-
-	WL_SOFTAP(("%s Done with %d\n", __FUNCTION__, res));
-	DHD_OS_MUTEX_UNLOCK(&wl_softap_lock);
-	net_os_wake_unlock(dev);
-
-	return res;
-}
-
-
-
-static int
-iwpriv_fw_reload(struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu,
-	char *ext)
-{
-	int ret = -1;
-	char extra[256];
-	char *fwstr = fw_path ; 
-
-	WL_SOFTAP(("current firmware_path[]=%s\n", fwstr));
-
-	WL_TRACE((">Got FW_RELOAD cmd:"
-	          "info->cmd:%x, info->flags:%x, u.data:%p, u.len:%d, "
-	          "fw_path:%p, len:%d \n",
-	          info->cmd, info->flags,
-	          wrqu->data.pointer, wrqu->data.length, fwstr, strlen(fwstr)));
-
-	if ((wrqu->data.length > 4) && (wrqu->data.length < sizeof(extra))) {
-		char *str_ptr;
-
-		if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
-			ret = -EFAULT;
-			goto exit_proc;
-		}
-
-		
-		extra[wrqu->data.length] = 8;
-		str_ptr = extra;
-
-		if (get_parameter_from_string(&str_ptr,
-		                              "FW_PATH=", PTYPE_STRING, fwstr, 255) != 0) {
-			WL_ERROR(("Error: extracting FW_PATH='' string\n"));
-			goto exit_proc;
-		}
-
-		if  (strstr(fwstr, "apsta") != NULL) {
-			  WL_SOFTAP(("GOT APSTA FIRMWARE\n"));
-			  ap_fw_loaded = TRUE;
-		} else {
-			WL_SOFTAP(("GOT STA FIRMWARE\n"));
-			ap_fw_loaded = FALSE;
-		}
-
-		WL_SOFTAP(("SET firmware_path[]=%s , str_p:%p\n", fwstr, fwstr));
-		ret = 0;
-	} else {
-		WL_ERROR(("Error: ivalid param len:%d\n", wrqu->data.length));
-	}
-
-exit_proc:
-	return ret;
-}
-
-#ifdef SOFTAP
-
-static int
-iwpriv_wpasupp_loop_tst(struct net_device *dev,
-            struct iw_request_info *info,
-            union iwreq_data *wrqu,
-            char *ext)
-{
-	int res = 0;
-	char *params = NULL;
-
-	WL_TRACE((">Got IWPRIV  wp_supp loopback cmd test:"
-	          "info->cmd:%x, info->flags:%x, u.data:%p, u.len:%d\n",
-	          info->cmd, info->flags,
-	          wrqu->data.pointer, wrqu->data.length));
-
-	if (wrqu->data.length != 0) {
-
-		if (!(params = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
-			return -ENOMEM;
-
-
-		if (copy_from_user(params, wrqu->data.pointer, wrqu->data.length)) {
-			kfree(params);
-			return -EFAULT;
-		}
-
-		params[wrqu->data.length] = 0;
-		WL_SOFTAP(("\n>> copied from user:\n %s\n", params));
-	} else {
-		WL_ERROR(("ERROR param length is 0\n"));
-		return -EFAULT;
-	}
-
-	
-	res = wl_iw_send_priv_event(dev, params);
-	kfree(params);
-
-	return res;
-}
-#endif 
-
-
-static int
-iwpriv_en_ap_bss(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	void *wrqu,
-	char *extra)
-{
-	int res = 0;
-
-	if (!dev) {
-		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
-		return -1;
-	}
-
-	net_os_wake_lock(dev);
-	DHD_OS_MUTEX_LOCK(&wl_softap_lock);
-
-	WL_TRACE(("%s: rcvd IWPRIV IOCTL:  for dev:%s\n", __FUNCTION__, dev->name));
-
-	
-#ifndef AP_ONLY
-	if ((res = wl_iw_set_ap_security(dev, &my_ap)) != 0) {
-		WL_ERROR((" %s ERROR setting SOFTAP security in :%d\n", __FUNCTION__, res));
-	}
-	else {
-		
-		if ((res = dev_iw_write_cfg1_bss_var(dev, 1)) < 0)
-			WL_ERROR(("%s fail to set bss up err=%d\n", __FUNCTION__, res));
-		else
-			
-			bcm_mdelay(100);
-	}
-
-#endif 
-	WL_SOFTAP(("%s done with res %d \n", __FUNCTION__, res));
-
-	DHD_OS_MUTEX_UNLOCK(&wl_softap_lock);
-	net_os_wake_unlock(dev);
-
-	return res;
-}
-
-static int
-get_assoc_sta_list(struct net_device *dev, char *buf, int len)
-{
-	
-	WL_TRACE(("%s: dev_wlc_ioctl(dev:%p, cmd:%d, buf:%p, len:%d)\n",
-		__FUNCTION__, dev, WLC_GET_ASSOCLIST, buf, len));
-
-	return dev_wlc_ioctl(dev, WLC_GET_ASSOCLIST, buf, len);
-
-}
-
-
-void check_error(int res, const char *msg, const char *func, int line)
-{
-	if (res != 0)
-		WL_ERROR(("%s, %d function:%s, line:%d\n", msg, res, func, line));
-}
-
-static int
-set_ap_mac_list(struct net_device *dev, void *buf)
-{
-	struct mac_list_set *mac_list_set = (struct mac_list_set *)buf;
-	struct maclist *maclist = (struct maclist *)&mac_list_set->mac_list;
-	int length;
-	int i;
-	int mac_mode = mac_list_set->mode;
-	int ioc_res = 0;
-	ap_macmode = mac_list_set->mode;  
-
-	
-	bzero(&ap_black_list, sizeof(struct mflist));
-
-	if (mac_mode == MACLIST_MODE_DISABLED) {
-		
-		ioc_res = dev_wlc_ioctl(dev, WLC_SET_MACMODE, &mac_mode, sizeof(mac_mode));
-		check_error(ioc_res, "ioctl ERROR:", __FUNCTION__, __LINE__);
-		WL_SOFTAP(("%s: MAC filtering disabled\n", __FUNCTION__));
-	} else {
-		
-		scb_val_t scbval;
-		char mac_buf[256] = {0};
-		struct maclist *assoc_maclist = (struct maclist *) mac_buf;
-
-		
-		bcopy(maclist, &ap_black_list, sizeof(ap_black_list));
-
-		
-		ioc_res = dev_wlc_ioctl(dev, WLC_SET_MACMODE, &mac_mode, sizeof(mac_mode));
-		check_error(ioc_res, "ioctl ERROR:", __FUNCTION__, __LINE__);
-
-		
-		length = sizeof(maclist->count) + maclist->count*ETHER_ADDR_LEN;
-		dev_wlc_ioctl(dev, WLC_SET_MACLIST, maclist, length);
-
-		WL_SOFTAP(("%s: applied MAC List, mode:%d, length %d:\n",
-			__FUNCTION__, mac_mode, length));
-
-		for (i = 0; i < maclist->count; i++)
-			WL_SOFTAP(("mac %d: %02X:%02X:%02X:%02X:%02X:%02X\n",
-				i, maclist->ea[i].octet[0], maclist->ea[i].octet[1],
-				maclist->ea[i].octet[2],
-				maclist->ea[i].octet[3], maclist->ea[i].octet[4],
-				maclist->ea[i].octet[5]));
-
-		
-		assoc_maclist->count = 8;
-		ioc_res = dev_wlc_ioctl(dev, WLC_GET_ASSOCLIST, assoc_maclist, 256);
-		check_error(ioc_res, "ioctl ERROR:", __FUNCTION__, __LINE__);
-		WL_SOFTAP((" Cur assoc clients:%d\n", assoc_maclist->count));
-
-		
-		if (assoc_maclist->count)
-			for (i = 0; i < assoc_maclist->count; i++) {
-				int j;
-				bool assoc_mac_matched = FALSE;
-				
-				WL_SOFTAP(("\n Cheking assoc STA: "));
-				dhd_print_buf(&assoc_maclist->ea[i], 6, 7);
-				WL_SOFTAP(("with the b/w list:"));
-
-				for (j = 0; j < maclist->count; j++)
-					if (!bcmp(&assoc_maclist->ea[i], &maclist->ea[j],
-						ETHER_ADDR_LEN)) {
-						
-						assoc_mac_matched = TRUE;
-						break;
-					}
-
-				
-				if (((mac_mode == MACLIST_MODE_ALLOW) && !assoc_mac_matched) ||
-					((mac_mode == MACLIST_MODE_DENY) && assoc_mac_matched)) {
-
-					WL_SOFTAP(("b-match or w-mismatch,"
-								" do deauth/disassoc \n"));
-							scbval.val = htod32(1);
-							bcopy(&assoc_maclist->ea[i], &scbval.ea,
-							ETHER_ADDR_LEN);
-							ioc_res = dev_wlc_ioctl(dev,
-								WLC_SCB_DEAUTHENTICATE_FOR_REASON,
-								&scbval, sizeof(scb_val_t));
-							check_error(ioc_res,
-								"ioctl ERROR:",
-								__FUNCTION__, __LINE__);
-
-				} else {
-					WL_SOFTAP((" no b/w list hits, let it be\n"));
-				}
-		} else {
-			WL_SOFTAP(("No ASSOC CLIENTS\n"));
-		} 
-
-	} 
-
-	WL_SOFTAP(("%s iocres:%d\n", __FUNCTION__, ioc_res));
-	return ioc_res;
-}
-#endif 
-
-
-
-#ifdef SOFTAP
-#define PARAM_OFFSET PROFILE_OFFSET
-
-static int
-wl_iw_process_private_ascii_cmd(
-			struct net_device *dev,
-			struct iw_request_info *info,
-			union iwreq_data *dwrq,
-			char *cmd_str)
-{
-	int ret = 0;
-	char *sub_cmd = cmd_str + PROFILE_OFFSET + strlen("ASCII_CMD=");
-
-	WL_SOFTAP(("\n %s: ASCII_CMD: offs_0:%s, offset_32:\n'%s'\n",
-		__FUNCTION__, cmd_str, cmd_str + PROFILE_OFFSET));
-
-	if (strnicmp(sub_cmd, "AP_CFG", strlen("AP_CFG")) == 0) {
-
-		WL_SOFTAP((" AP_CFG \n"));
-
-		
-		if (init_ap_profile_from_string(cmd_str+PROFILE_OFFSET, &my_ap) != 0) {
-				WL_ERROR(("ERROR: SoftAP CFG prams !\n"));
-				ret = -1;
-		} else {
-			ret = set_ap_cfg(dev, &my_ap);
-		}
-
-	} else if (strnicmp(sub_cmd, "AP_BSS_START", strlen("AP_BSS_START")) == 0) {
-
-		WL_SOFTAP(("\n SOFTAP - ENABLE BSS \n"));
-
-		
-		WL_SOFTAP(("\n!!! got 'WL_AP_EN_BSS' from WPA supplicant, dev:%s\n", dev->name));
-
-#ifndef AP_ONLY
-		if (ap_net_dev == NULL) {
-				 printf("\n ERROR: SOFTAP net_dev* is NULL !!!\n");
-		} else {
-			  
-			if ((ret = iwpriv_en_ap_bss(ap_net_dev, info, dwrq, cmd_str)) < 0)
-				WL_ERROR(("%s line %d fail to set bss up\n",
-					__FUNCTION__, __LINE__));
-		}
-#else
-		if ((ret = iwpriv_en_ap_bss(dev, info, dwrq, cmd_str)) < 0)
-				WL_ERROR(("%s line %d fail to set bss up\n",
-					__FUNCTION__, __LINE__));
-#endif
-	} else if (strnicmp(sub_cmd, "ASSOC_LST", strlen("ASSOC_LST")) == 0) {
-
-		
-
-	} else if (strnicmp(sub_cmd, "AP_BSS_STOP", strlen("AP_BSS_STOP")) == 0) {
-
-		WL_SOFTAP((" \n temp DOWN SOFTAP\n"));
-#ifndef AP_ONLY
-		if ((ret = dev_iw_write_cfg1_bss_var(dev, 0)) < 0) {
-				WL_ERROR(("%s line %d fail to set bss down\n",
-					__FUNCTION__, __LINE__));
-		}
-#endif
-	}
-
-	return ret;
-
-}
-#endif 
-
-
-static int
-wl_iw_set_priv(
-	struct net_device *dev,
-	struct iw_request_info *info,
-	struct iw_point *dwrq,
-	char *ext
-)
-{
-	int ret = 0;
-	char * extra;
-
-	if (!(extra = kmalloc(dwrq->length, GFP_KERNEL)))
-	    return -ENOMEM;
-
-	if (copy_from_user(extra, dwrq->pointer, dwrq->length)) {
-	    kfree(extra);
-	    return -EFAULT;
-	}
-
-	WL_TRACE(("%s: SIOCSIWPRIV request %s, info->cmd:%x, info->flags:%d\n dwrq->length:%d\n",
-		dev->name, extra, info->cmd, info->flags, dwrq->length));
-
-	
-
-	net_os_wake_lock(dev);
-
-	if (dwrq->length && extra) {
-		if (strnicmp(extra, "START", strlen("START")) == 0) {
-			wl_iw_control_wl_on(dev, info);
-			WL_TRACE(("%s, Received regular START command\n", __FUNCTION__));
-		}
-
-		if (g_onoff == G_WLAN_SET_OFF) {
-			WL_TRACE(("%s, missing START, Fail\n", __FUNCTION__));
-			kfree(extra);
-			net_os_wake_unlock(dev);
-			return -EFAULT;
-		}
-
-		if (strnicmp(extra, "SCAN-ACTIVE", strlen("SCAN-ACTIVE")) == 0) {
-#ifdef ENABLE_ACTIVE_PASSIVE_SCAN_SUPPRESS
-			WL_TRACE(("%s: active scan setting suppressed\n", dev->name));
-#else
-			ret = wl_iw_set_active_scan(dev, info, (union iwreq_data *)dwrq, extra);
-#endif 
-		}
-		else if (strnicmp(extra, "SCAN-PASSIVE", strlen("SCAN-PASSIVE")) == 0)
-#ifdef ENABLE_ACTIVE_PASSIVE_SCAN_SUPPRESS
-			WL_TRACE(("%s: passive scan setting suppressed\n", dev->name));
-#else
-			ret = wl_iw_set_passive_scan(dev, info, (union iwreq_data *)dwrq, extra);
-#endif 
-		else if (strnicmp(extra, "RSSI", strlen("RSSI")) == 0)
-			ret = wl_iw_get_rssi(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, "LINKSPEED", strlen("LINKSPEED")) == 0)
-			ret = wl_iw_get_link_speed(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, "MACADDR", strlen("MACADDR")) == 0)
-			ret = wl_iw_get_macaddr(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, "COUNTRY", strlen("COUNTRY")) == 0)
-			ret = wl_iw_set_country(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, "STOP", strlen("STOP")) == 0)
-			ret = wl_iw_control_wl_off(dev, info);
-		else if (strnicmp(extra, BAND_GET_CMD, strlen(BAND_GET_CMD)) == 0)
-			ret = wl_iw_get_band(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, BAND_SET_CMD, strlen(BAND_SET_CMD)) == 0)
-			ret = wl_iw_set_band(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, DTIM_SKIP_GET_CMD, strlen(DTIM_SKIP_GET_CMD)) == 0)
-			ret = wl_iw_get_dtim_skip(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, DTIM_SKIP_SET_CMD, strlen(DTIM_SKIP_SET_CMD)) == 0)
-			ret = wl_iw_set_dtim_skip(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, SETSUSPEND_CMD, strlen(SETSUSPEND_CMD)) == 0)
-			ret = wl_iw_set_suspend(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, TXPOWER_SET_CMD, strlen(TXPOWER_SET_CMD)) == 0)
-			ret = wl_iw_set_txpower(dev, info, (union iwreq_data *)dwrq, extra);
-#if defined(PNO_SUPPORT)
-		else if (strnicmp(extra, PNOSSIDCLR_SET_CMD, strlen(PNOSSIDCLR_SET_CMD)) == 0)
-			ret = wl_iw_set_pno_reset(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, PNOSETUP_SET_CMD, strlen(PNOSETUP_SET_CMD)) == 0)
-			ret = wl_iw_set_pno_set(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, PNOENABLE_SET_CMD, strlen(PNOENABLE_SET_CMD)) == 0)
-			ret = wl_iw_set_pno_enable(dev, info, (union iwreq_data *)dwrq, extra);
-#endif 
-#if defined(CSCAN)
-	    
-		else if (strnicmp(extra, CSCAN_COMMAND, strlen(CSCAN_COMMAND)) == 0)
-			ret = wl_iw_set_cscan(dev, info, (union iwreq_data *)dwrq, extra);
-#endif 
-		else if (strnicmp(extra, "POWERMODE", strlen("POWERMODE")) == 0)
-			ret = wl_iw_set_power_mode(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, "BTCOEXMODE", strlen("BTCOEXMODE")) == 0)
-			ret = wl_iw_set_btcoex_dhcp(dev, info, (union iwreq_data *)dwrq, extra);
-		else if (strnicmp(extra, "GETPOWER", strlen("GETPOWER")) == 0)
-			ret = wl_iw_get_power_mode(dev, info, (union iwreq_data *)dwrq, extra);
-#ifdef SOFTAP
-		else if (strnicmp(extra, "ASCII_CMD", strlen("ASCII_CMD")) == 0) {
-			wl_iw_process_private_ascii_cmd(dev, info, (union iwreq_data *)dwrq, extra);
-		}
-		else if (strnicmp(extra, "AP_MAC_LIST_SET", strlen("AP_MAC_LIST_SET")) == 0) {
-			WL_SOFTAP(("penguin, set AP_MAC_LIST_SET\n"));
-			set_ap_mac_list(dev, (extra + PROFILE_OFFSET));
-		}
-#endif 
-	    else {
-			WL_ERROR(("Unknown PRIVATE command %s - ignored\n", extra));
-			snprintf(extra, MAX_WX_STRING, "OK");
-			dwrq->length = strlen("OK") + 1;
-		}
-	}
-
-	net_os_wake_unlock(dev);
-
-	if (extra) {
-	    if (copy_to_user(dwrq->pointer, extra, dwrq->length)) {
-			kfree(extra);
-			return -EFAULT;
-	    }
-
-	    kfree(extra);
-	}
-
-	return ret;
-}
-
-static const iw_handler wl_iw_handler[] =
-{
-	(iw_handler) wl_iw_config_commit,	
-	(iw_handler) wl_iw_get_name,		
-	(iw_handler) NULL,			
-	(iw_handler) NULL,			
-	(iw_handler) wl_iw_set_freq,		
-	(iw_handler) wl_iw_get_freq,		
-	(iw_handler) wl_iw_set_mode,		
-	(iw_handler) wl_iw_get_mode,		
-	(iw_handler) NULL,			
-	(iw_handler) NULL,			
-	(iw_handler) NULL,			
-	(iw_handler) wl_iw_get_range,		
-	(iw_handler) wl_iw_set_priv,		
-	(iw_handler) NULL,			
-	(iw_handler) NULL,			
-	(iw_handler) NULL,			
-	(iw_handler) wl_iw_set_spy,		
-	(iw_handler) wl_iw_get_spy,		
-	(iw_handler) NULL,			
-	(iw_handler) NULL,			
-	(iw_handler) wl_iw_set_wap,		
-	(iw_handler) wl_iw_get_wap,		
-#if WIRELESS_EXT > 17
-	(iw_handler) wl_iw_mlme,		
-#else
-	(iw_handler) NULL,			
-#endif
-#if defined(WL_IW_USE_ISCAN)
-	(iw_handler) wl_iw_iscan_get_aplist,	
-#else
-	(iw_handler) wl_iw_get_aplist,		
-#endif 
-#if WIRELESS_EXT > 13
-#if defined(WL_IW_USE_ISCAN)
-	(iw_handler) wl_iw_iscan_set_scan,	
-	(iw_handler) wl_iw_iscan_get_scan,	
-#else
-	(iw_handler) wl_iw_set_scan,		
-	(iw_handler) wl_iw_get_scan,		
-#endif
-#else	
-	(iw_handler) NULL,			
-	(iw_handler) NULL,			
-#endif	
-	(iw_handler) wl_iw_set_essid,		
-	(iw_handler) wl_iw_get_essid,		
-	(iw_handler) wl_iw_set_nick,		
-	(iw_handler) wl_iw_get_nick,		
-	(iw_handler) NULL,			
-	(iw_handler) NULL,			
-	(iw_handler) wl_iw_set_rate,		
-	(iw_handler) wl_iw_get_rate,		
-	(iw_handler) wl_iw_set_rts,		
-	(iw_handler) wl_iw_get_rts,		
-	(iw_handler) wl_iw_set_frag,		
-	(iw_handler) wl_iw_get_frag,		
-	(iw_handler) wl_iw_set_txpow,		
-	(iw_handler) wl_iw_get_txpow,		
-#if WIRELESS_EXT > 10
-	(iw_handler) wl_iw_set_retry,		
-	(iw_handler) wl_iw_get_retry,		
-#endif 
-	(iw_handler) wl_iw_set_encode,		
-	(iw_handler) wl_iw_get_encode,		
-	(iw_handler) wl_iw_set_power,		
-	(iw_handler) wl_iw_get_power,		
-#if WIRELESS_EXT > 17
-	(iw_handler) NULL,			
-	(iw_handler) NULL,			
-	(iw_handler) wl_iw_set_wpaie,		
-	(iw_handler) wl_iw_get_wpaie,		
-	(iw_handler) wl_iw_set_wpaauth,		
-	(iw_handler) wl_iw_get_wpaauth,		
-	(iw_handler) wl_iw_set_encodeext,	
-	(iw_handler) wl_iw_get_encodeext,	
-	(iw_handler) wl_iw_set_pmksa,			
-#endif 
-};
-
-#if WIRELESS_EXT > 12
-static const iw_handler wl_iw_priv_handler[] = {
-	NULL,
-	(iw_handler)wl_iw_set_active_scan,
-	NULL,
-	(iw_handler)wl_iw_get_rssi,
-	NULL,
-	(iw_handler)wl_iw_set_passive_scan,
-	NULL,
-	(iw_handler)wl_iw_get_link_speed,
-	NULL,
-	(iw_handler)wl_iw_get_macaddr,
-	NULL,
-	(iw_handler)wl_iw_control_wl_off,
-	NULL,
-	(iw_handler)wl_iw_control_wl_on,
-#ifdef SOFTAP       
-
-	
-	NULL,
-	(iw_handler)iwpriv_set_ap_config,
-
-	
-	
-	NULL,
-	(iw_handler)iwpriv_get_assoc_list,
-
-	
-	NULL,
-	(iw_handler)iwpriv_set_mac_filters,
-
-	
-	NULL,
-	(iw_handler)iwpriv_en_ap_bss,
-
-	
-	NULL,
-	(iw_handler)iwpriv_wpasupp_loop_tst,
-	
-	NULL,
-	(iw_handler)iwpriv_softap_stop,
-	
-	NULL,
-	(iw_handler)iwpriv_fw_reload,
-	NULL, 
-	(iw_handler)iwpriv_set_ap_sta_disassoc,
-#endif 
-#if defined(CSCAN)
-	
-	NULL,
-	(iw_handler)iwpriv_set_cscan
-#endif 	
-};
-
-static const struct iw_priv_args wl_iw_priv_args[] =
-{
-	{	
-		WL_IW_SET_ACTIVE_SCAN,
-		0,
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
-		"SCAN-ACTIVE"
-	},
-	{
-		WL_IW_GET_RSSI,
-		0,
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
-		"RSSI"
-	},
-	{
-		WL_IW_SET_PASSIVE_SCAN,
-		0,
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
-		"SCAN-PASSIVE"
-	},
-	{
-		WL_IW_GET_LINK_SPEED,
-		0,
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
-		"LINKSPEED"
-	},
-	{
-		WL_IW_GET_CURR_MACADDR,
-		0,
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
-		"Macaddr"
-	},
-	{
-		WL_IW_SET_STOP,
-		0,
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
-		"STOP"
-	},
-	{
-		WL_IW_SET_START,
-		0,
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
-		"START"
-	},
-
-#ifdef SOFTAP
-	
-	
-	{
-		WL_SET_AP_CFG,
-		IW_PRIV_TYPE_CHAR |  256,      
-		0,
-		"AP_SET_CFG"
-	},
-
-	{
-		WL_AP_STA_LIST,
-		0,                     
-		IW_PRIV_TYPE_CHAR | 0, 
-		"AP_GET_STA_LIST"
-	},
-
-	{
-		WL_AP_MAC_FLTR,
-		IW_PRIV_TYPE_CHAR | 256,                      
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,    
-		"AP_SET_MAC_FLTR"
-	},
-
-	{ 
-		WL_AP_BSS_START,
-		0,
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
-		"AP_BSS_START"
-	},
-
-	{
-		AP_LPB_CMD,
-		IW_PRIV_TYPE_CHAR | 256,   
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,    
-		"AP_LPB_CMD"
-	},
-
-	{ 
-		WL_AP_STOP,
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,   
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,   
-		"AP_BSS_STOP"
-	},
-	{ 
-		WL_FW_RELOAD,
-		IW_PRIV_TYPE_CHAR | 256,
-		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,
-		"WL_FW_RELOAD"
-	},
-#endif 
-#if defined(CSCAN)
-	{ 
-		WL_COMBO_SCAN,
-		IW_PRIV_TYPE_CHAR | 1024,  
-		0,
-		"CSCAN"
-	},
-#endif 
-	};
-
-const struct iw_handler_def wl_iw_handler_def =
-{
-	.num_standard = ARRAYSIZE(wl_iw_handler),
-	.standard = (iw_handler *) wl_iw_handler,
-	.num_private = ARRAYSIZE(wl_iw_priv_handler),
-	.num_private_args = ARRAY_SIZE(wl_iw_priv_args),
-	.private = (iw_handler *)wl_iw_priv_handler,
-	.private_args = (void *) wl_iw_priv_args,
-
-#if WIRELESS_EXT >= 19
-	get_wireless_stats: dhd_get_wireless_stats,
-#endif 
-	};
-#endif 
-
-
-
-int
-wl_iw_ioctl(
-	struct net_device *dev,
-	struct ifreq *rq,
-	int cmd
-)
-{
-	struct iwreq *wrq = (struct iwreq *) rq;
-	struct iw_request_info info;
-	iw_handler handler;
-	char *extra = NULL;
-	size_t token_size = 1;
-	int max_tokens = 0, ret = 0;
-
-	net_os_wake_lock(dev);
-
-	WL_TRACE(("\n%s, cmd:%x called via dhd->do_ioctl()entry point\n", __FUNCTION__, cmd));
-	if (cmd < SIOCIWFIRST ||
-		IW_IOCTL_IDX(cmd) >= ARRAYSIZE(wl_iw_handler) ||
-		!(handler = wl_iw_handler[IW_IOCTL_IDX(cmd)])) {
-			WL_ERROR(("%s: error in cmd=%x : not supported\n", __FUNCTION__, cmd));
-			net_os_wake_unlock(dev);
-			return -EOPNOTSUPP;
-	}
-
-	switch (cmd) {
-
-	case SIOCSIWESSID:
-	case SIOCGIWESSID:
-	case SIOCSIWNICKN:
-	case SIOCGIWNICKN:
-		max_tokens = IW_ESSID_MAX_SIZE + 1;
-		break;
-
-	case SIOCSIWENCODE:
-	case SIOCGIWENCODE:
-#if WIRELESS_EXT > 17
-	case SIOCSIWENCODEEXT:
-	case SIOCGIWENCODEEXT:
-#endif
-		max_tokens = wrq->u.data.length;
-		break;
-
-	case SIOCGIWRANGE:
-		
-		max_tokens = sizeof(struct iw_range) + 500;
-		break;
-
-	case SIOCGIWAPLIST:
-		token_size = sizeof(struct sockaddr) + sizeof(struct iw_quality);
-		max_tokens = IW_MAX_AP;
-		break;
-
-#if WIRELESS_EXT > 13
-	case SIOCGIWSCAN:
-#if defined(WL_IW_USE_ISCAN)
-	if (g_iscan)
-		max_tokens = wrq->u.data.length;
-	else
-#endif
-		max_tokens = IW_SCAN_MAX_DATA;
-		break;
-#endif 
-
-	case SIOCSIWSPY:
-		token_size = sizeof(struct sockaddr);
-		max_tokens = IW_MAX_SPY;
-		break;
-
-	case SIOCGIWSPY:
-		token_size = sizeof(struct sockaddr) + sizeof(struct iw_quality);
-		max_tokens = IW_MAX_SPY;
-		break;
-
-#if WIRELESS_EXT > 17
-	case SIOCSIWPMKSA:
-	case SIOCSIWGENIE:
-#endif 
-	case SIOCSIWPRIV:
-		max_tokens = wrq->u.data.length;
-		break;
-	}
-
-	if (max_tokens && wrq->u.data.pointer) {
-		if (wrq->u.data.length > max_tokens) {
-			WL_ERROR(("%s: error in cmd=%x wrq->u.data.length=%d  > max_tokens=%d\n",
-				__FUNCTION__, cmd, wrq->u.data.length, max_tokens));
-			ret = -E2BIG;
-			goto wl_iw_ioctl_done;
-		}
-		if (!(extra = kmalloc(max_tokens * token_size, GFP_KERNEL))) {
-			ret = -ENOMEM;
-			goto wl_iw_ioctl_done;
-		}
-
-		if (copy_from_user(extra, wrq->u.data.pointer, wrq->u.data.length * token_size)) {
-			kfree(extra);
-			ret = -EFAULT;
-			goto wl_iw_ioctl_done;
-		}
-	}
-
-	info.cmd = cmd;
-	info.flags = 0;
-
-	ret = handler(dev, &info, &wrq->u, extra);
-
-	if (extra) {
-		if (copy_to_user(wrq->u.data.pointer, extra, wrq->u.data.length * token_size)) {
-			kfree(extra);
-			ret = -EFAULT;
-			goto wl_iw_ioctl_done;
-		}
-
-		kfree(extra);
-	}
-
-wl_iw_ioctl_done:
-
-	net_os_wake_unlock(dev);
-
-	return ret;
-}
-
-
-static bool
-wl_iw_conn_status_str(uint32 event_type, uint32 status, uint32 reason,
-	char* stringBuf, uint buflen)
-{
-	typedef struct conn_fail_event_map_t {
-		uint32 inEvent;			
-		uint32 inStatus;		
-		uint32 inReason;		
-		const char* outName;	
-		const char* outCause;	
-	} conn_fail_event_map_t;
-
-	
-#define WL_IW_DONT_CARE	9999
-	const conn_fail_event_map_t event_map [] = {
-		
-		
-		{WLC_E_SET_SSID,     WLC_E_STATUS_SUCCESS,   WL_IW_DONT_CARE,
-		"Conn", "Success"},
-		{WLC_E_SET_SSID,     WLC_E_STATUS_NO_NETWORKS, WL_IW_DONT_CARE,
-		"Conn", "NoNetworks"},
-		{WLC_E_SET_SSID,     WLC_E_STATUS_FAIL,      WL_IW_DONT_CARE,
-		"Conn", "ConfigMismatch"},
-		{WLC_E_PRUNE,        WL_IW_DONT_CARE,        WLC_E_PRUNE_ENCR_MISMATCH,
-		"Conn", "EncrypMismatch"},
-		{WLC_E_PRUNE,        WL_IW_DONT_CARE,        WLC_E_RSN_MISMATCH,
-		"Conn", "RsnMismatch"},
-		{WLC_E_AUTH,         WLC_E_STATUS_TIMEOUT,   WL_IW_DONT_CARE,
-		"Conn", "AuthTimeout"},
-		{WLC_E_AUTH,         WLC_E_STATUS_FAIL,      WL_IW_DONT_CARE,
-		"Conn", "AuthFail"},
-		{WLC_E_AUTH,         WLC_E_STATUS_NO_ACK,    WL_IW_DONT_CARE,
-		"Conn", "AuthNoAck"},
-		{WLC_E_REASSOC,      WLC_E_STATUS_FAIL,      WL_IW_DONT_CARE,
-		"Conn", "ReassocFail"},
-		{WLC_E_REASSOC,      WLC_E_STATUS_TIMEOUT,   WL_IW_DONT_CARE,
-		"Conn", "ReassocTimeout"},
-		{WLC_E_REASSOC,      WLC_E_STATUS_ABORT,     WL_IW_DONT_CARE,
-		"Conn", "ReassocAbort"},
-		{WLC_E_PSK_SUP,      WLC_SUP_KEYED,          WL_IW_DONT_CARE,
-		"Sup", "ConnSuccess"},
-		{WLC_E_PSK_SUP,      WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
-		"Sup", "WpaHandshakeFail"},
-		{WLC_E_DEAUTH_IND,   WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
-		"Conn", "Deauth"},
-		{WLC_E_DISASSOC_IND, WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
-		"Conn", "DisassocInd"},
-		{WLC_E_DISASSOC,     WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
-		"Conn", "Disassoc"}
-	};
-
-	const char* name = "";
-	const char* cause = NULL;
-	int i;
-
-	
-	for (i = 0;  i < sizeof(event_map)/sizeof(event_map[0]);  i++) {
-		const conn_fail_event_map_t* row = &event_map[i];
-		if (row->inEvent == event_type &&
-		    (row->inStatus == status || row->inStatus == WL_IW_DONT_CARE) &&
-		    (row->inReason == reason || row->inReason == WL_IW_DONT_CARE)) {
-			name = row->outName;
-			cause = row->outCause;
-			break;
-		}
-	}
-
-	
-	if (cause) {
-		memset(stringBuf, 0, buflen);
-		snprintf(stringBuf, buflen, "%s %s %02d %02d",
-			name, cause, status, reason);
-		WL_INFORM(("Connection status: %s\n", stringBuf));
-		return TRUE;
-	} else {
-		return FALSE;
-	}
-}
-
-#if WIRELESS_EXT > 14
-
-static bool
-wl_iw_check_conn_fail(wl_event_msg_t *e, char* stringBuf, uint buflen)
-{
-	uint32 event = ntoh32(e->event_type);
-	uint32 status =  ntoh32(e->status);
-	uint32 reason =  ntoh32(e->reason);
-
-	if (wl_iw_conn_status_str(event, status, reason, stringBuf, buflen)) {
-		return TRUE;
-	}
-	else
-		return FALSE;
-}
-#endif 
-
-#ifndef IW_CUSTOM_MAX
-#define IW_CUSTOM_MAX 256 
-#endif 
-
-void
-wl_iw_event(struct net_device *dev, wl_event_msg_t *e, void* data)
-{
-#if WIRELESS_EXT > 13
-	union iwreq_data wrqu;
-	char extra[IW_CUSTOM_MAX + 1];
-	int cmd = 0;
-	uint32 event_type = ntoh32(e->event_type);
-	uint16 flags =  ntoh16(e->flags);
-	uint32 datalen = ntoh32(e->datalen);
-	uint32 status =  ntoh32(e->status);
-	uint32 toto;
-	static  uint32 roam_no_success = 0;
-	static bool roam_no_success_send = FALSE;
-	memset(&wrqu, 0, sizeof(wrqu));
-	memset(extra, 0, sizeof(extra));
-
-	if (!dev) {
-		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
-		return;
-	}
-
-	net_os_wake_lock(dev);
-
-	WL_TRACE(("%s: dev=%s event=%d \n", __FUNCTION__, dev->name, event_type));
-
-	
-	switch (event_type) {
-#if defined(SOFTAP)
-	case WLC_E_PRUNE:
-		if (ap_cfg_running) {
-			char *macaddr = (char *)&e->addr;
-			WL_SOFTAP(("PRUNE received, %02X:%02X:%02X:%02X:%02X:%02X!\n",
-				macaddr[0], macaddr[1], macaddr[2], macaddr[3],
-				macaddr[4], macaddr[5]));
-
-			
-			if (ap_macmode)
-			{
-				int i;
-				for (i = 0; i < ap_black_list.count; i++) {
-					if (!bcmp(macaddr, &ap_black_list.ea[i],
-						sizeof(struct ether_addr))) {
-						WL_SOFTAP(("mac in black list, ignore it\n"));
-						break;
-					}
-				}
-
-				if (i == ap_black_list.count) {
-					
-					char mac_buf[32] = {0};
-					sprintf(mac_buf, "STA_BLOCK %02X:%02X:%02X:%02X:%02X:%02X",
-						macaddr[0], macaddr[1], macaddr[2],
-						macaddr[3], macaddr[4], macaddr[5]);
-					wl_iw_send_priv_event(priv_dev, mac_buf);
-				}
-			}
-		}
-		break;
-#endif 
-	case WLC_E_TXFAIL:
-		cmd = IWEVTXDROP;
-		memcpy(wrqu.addr.sa_data, &e->addr, ETHER_ADDR_LEN);
-		wrqu.addr.sa_family = ARPHRD_ETHER;
-		break;
-#if WIRELESS_EXT > 14
-	case WLC_E_JOIN:
-	case WLC_E_ASSOC_IND:
-	case WLC_E_REASSOC_IND:
-#if defined(SOFTAP)
-		WL_SOFTAP(("STA connect received %d\n", event_type));
-		if (ap_cfg_running) {
-			wl_iw_send_priv_event(priv_dev, "STA_JOIN");
-			goto wl_iw_event_end;
-		}
-#endif 
-		memcpy(wrqu.addr.sa_data, &e->addr, ETHER_ADDR_LEN);
-		wrqu.addr.sa_family = ARPHRD_ETHER;
-		cmd = IWEVREGISTERED;
-		break;
-	case WLC_E_ROAM:
-		if (status != WLC_E_STATUS_SUCCESS) {
-			roam_no_success++;
-			if ((roam_no_success == 3) && (roam_no_success_send == FALSE)) {
-				
-				roam_no_success_send = TRUE;
-				bzero(wrqu.addr.sa_data, ETHER_ADDR_LEN);
-				bzero(&extra, ETHER_ADDR_LEN);
-				cmd = SIOCGIWAP;
-				WL_ERROR(("%s  ROAMING did not succeeded , send Link Down\n",
-					__FUNCTION__));
-			} else {
-				WL_TRACE(("##### ROAMING did not succeeded %d\n", roam_no_success));
-				goto wl_iw_event_end;
-			}
-		} else {
-			memcpy(wrqu.addr.sa_data, &e->addr.octet, ETHER_ADDR_LEN);
-			wrqu.addr.sa_family = ARPHRD_ETHER;
-			cmd = SIOCGIWAP;
-		}
-	break;
-	case WLC_E_DEAUTH_IND:
-	case WLC_E_DISASSOC_IND:
-#if defined(SOFTAP)
-		WL_SOFTAP(("STA disconnect received %d\n", event_type));
-		if (ap_cfg_running) {
-			wl_iw_send_priv_event(priv_dev, "STA_LEAVE");
-			goto wl_iw_event_end;
-		}
-#endif 
-		cmd = SIOCGIWAP;
-		bzero(wrqu.addr.sa_data, ETHER_ADDR_LEN);
-		wrqu.addr.sa_family = ARPHRD_ETHER;
-		bzero(&extra, ETHER_ADDR_LEN);
-		break;
-	case WLC_E_LINK:
-	case WLC_E_NDIS_LINK:
-		cmd = SIOCGIWAP;
-		if (!(flags & WLC_EVENT_MSG_LINK)) {
-			
-			
-#ifdef SOFTAP
-#ifdef AP_ONLY
-		if (ap_cfg_running) {
-#else
-		if (ap_cfg_running && !strncmp(dev->name, "wl0.1", 5)) {
-#endif	
-			
-			WL_SOFTAP(("AP DOWN %d\n", event_type));
-			wl_iw_send_priv_event(priv_dev, "AP_DOWN");
-		} else {
-			WL_TRACE(("STA_Link Down\n"));
-			g_ss_cache_ctrl.m_link_down = 1;
-		}
-#else		
-		g_ss_cache_ctrl.m_link_down = 1;
-#endif 
-			WL_TRACE(("Link Down\n"));
-
-			bzero(wrqu.addr.sa_data, ETHER_ADDR_LEN);
-			bzero(&extra, ETHER_ADDR_LEN);
-		}
-		else {
-			
-			memcpy(wrqu.addr.sa_data, &e->addr, ETHER_ADDR_LEN);
-			g_ss_cache_ctrl.m_link_down = 0;
-			
-			memcpy(g_ss_cache_ctrl.m_active_bssid, &e->addr, ETHER_ADDR_LEN);
-#ifdef SOFTAP
-
-#ifdef AP_ONLY
-			if (ap_cfg_running) {
-#else
-			if (ap_cfg_running && !strncmp(dev->name, "wl0.1", 5)) {
-#endif
-			
-				WL_SOFTAP(("AP UP %d\n", event_type));
-				wl_iw_send_priv_event(priv_dev, "AP_UP");
-			} else {
-				WL_TRACE(("STA_LINK_UP\n"));
-				roam_no_success_send = FALSE;
-				roam_no_success = 0;
-			}
-#else
-#endif 
-			WL_TRACE(("Link UP\n"));
-
-		}
-		net_os_wake_lock_timeout_enable(dev);
-		wrqu.addr.sa_family = ARPHRD_ETHER;
-		break;
-	case WLC_E_ACTION_FRAME:
-		cmd = IWEVCUSTOM;
-		if (datalen + 1 <= sizeof(extra)) {
-			wrqu.data.length = datalen + 1;
-			extra[0] = WLC_E_ACTION_FRAME;
-			memcpy(&extra[1], data, datalen);
-			WL_TRACE(("WLC_E_ACTION_FRAME len %d \n", wrqu.data.length));
-		}
-		break;
-
-	case WLC_E_ACTION_FRAME_COMPLETE:
-		cmd = IWEVCUSTOM;
-		memcpy(&toto, data, 4);
-		if (sizeof(status) + 1 <= sizeof(extra)) {
-			wrqu.data.length = sizeof(status) + 1;
-			extra[0] = WLC_E_ACTION_FRAME_COMPLETE;
-			memcpy(&extra[1], &status, sizeof(status));
-			printf("wl_iw_event status %d PacketId %d \n", status, toto);
-			printf("WLC_E_ACTION_FRAME_COMPLETE len %d \n", wrqu.data.length);
-		}
-		break;
-#endif 
-#if WIRELESS_EXT > 17
-	case WLC_E_MIC_ERROR: {
-		struct	iw_michaelmicfailure  *micerrevt = (struct  iw_michaelmicfailure  *)&extra;
-		cmd = IWEVMICHAELMICFAILURE;
-		wrqu.data.length = sizeof(struct iw_michaelmicfailure);
-		if (flags & WLC_EVENT_MSG_GROUP)
-			micerrevt->flags |= IW_MICFAILURE_GROUP;
-		else
-			micerrevt->flags |= IW_MICFAILURE_PAIRWISE;
-		memcpy(micerrevt->src_addr.sa_data, &e->addr, ETHER_ADDR_LEN);
-		micerrevt->src_addr.sa_family = ARPHRD_ETHER;
-
-		break;
-	}
-	case WLC_E_PMKID_CACHE: {
-		if (data)
-		{
-			struct iw_pmkid_cand *iwpmkidcand = (struct iw_pmkid_cand *)&extra;
-			pmkid_cand_list_t *pmkcandlist;
-			pmkid_cand_t	*pmkidcand;
-			int count;
-
-			cmd = IWEVPMKIDCAND;
-			pmkcandlist = data;
-			count = ntoh32_ua((uint8 *)&pmkcandlist->npmkid_cand);
-			ASSERT(count >= 0);
-			wrqu.data.length = sizeof(struct iw_pmkid_cand);
-			pmkidcand = pmkcandlist->pmkid_cand;
-			while (count) {
-				bzero(iwpmkidcand, sizeof(struct iw_pmkid_cand));
-				if (pmkidcand->preauth)
-					iwpmkidcand->flags |= IW_PMKID_CAND_PREAUTH;
-				bcopy(&pmkidcand->BSSID, &iwpmkidcand->bssid.sa_data,
-					ETHER_ADDR_LEN);
-				wireless_send_event(dev, cmd, &wrqu, extra);
-				pmkidcand++;
-				count--;
-			}
-		}
-		goto wl_iw_event_end;
-	}
-#endif 
-
-	case WLC_E_SCAN_COMPLETE:
-#if defined(WL_IW_USE_ISCAN)
-		if ((g_iscan) && (g_iscan->tsk_ctl.thr_pid >= 0) &&
-			(g_iscan->iscan_state != ISCAN_STATE_IDLE))
-		{
-			up(&g_iscan->tsk_ctl.sema);
-		} else {
-			cmd = SIOCGIWSCAN;
-			wrqu.data.length = strlen(extra);
-			WL_TRACE(("Event WLC_E_SCAN_COMPLETE from specific scan %d\n",
-				g_iscan->iscan_state));
-		}
-#else
-		cmd = SIOCGIWSCAN;
-		wrqu.data.length = strlen(extra);
-		WL_TRACE(("Event WLC_E_SCAN_COMPLETE\n"));
-#endif 
-	break;
-
-	
-	case WLC_E_PFN_NET_FOUND:
-	{
-		wl_pfn_net_info_t *netinfo;
-		netinfo = (wl_pfn_net_info_t *)(data + sizeof(wl_pfn_scanresults_t) -
-		            sizeof(wl_pfn_net_info_t));
-		WL_ERROR(("%s Event WLC_E_PFN_NET_FOUND, send %s up : find %s len=%d\n",
-		   __FUNCTION__, PNO_EVENT_UP, netinfo->pfnsubnet.SSID,
-		   netinfo->pfnsubnet.SSID_len));
-		net_os_wake_lock_timeout_enable(dev);
-		cmd = IWEVCUSTOM;
-		memset(&wrqu, 0, sizeof(wrqu));
-		strcpy(extra, PNO_EVENT_UP);
-		wrqu.data.length = strlen(extra);
-	}
-	break;
-
-	default:
-		
-		WL_TRACE(("Unknown Event %d: ignoring\n", event_type));
-		break;
-	}
-		if (cmd) {
-			if (cmd == SIOCGIWSCAN)
-				wireless_send_event(dev, cmd, &wrqu, NULL);
-			else
-				wireless_send_event(dev, cmd, &wrqu, extra);
-		}
-
-#if WIRELESS_EXT > 14
-	
-	memset(extra, 0, sizeof(extra));
-	if (wl_iw_check_conn_fail(e, extra, sizeof(extra))) {
-		cmd = IWEVCUSTOM;
-		wrqu.data.length = strlen(extra);
-		wireless_send_event(dev, cmd, &wrqu, extra);
-	}
-#endif 
-
-	goto wl_iw_event_end;	
-wl_iw_event_end:
-
-	net_os_wake_unlock(dev);
-#endif 
-}
-
-int
-wl_iw_get_wireless_stats(struct net_device *dev, struct iw_statistics *wstats)
-{
-	int res = 0;
-	wl_cnt_t cnt;
-	int phy_noise;
-	int rssi;
-	scb_val_t scb_val;
-
-	phy_noise = 0;
-	if ((res = dev_wlc_ioctl(dev, WLC_GET_PHY_NOISE, &phy_noise, sizeof(phy_noise))))
-		goto done;
-
-	phy_noise = dtoh32(phy_noise);
-	WL_TRACE(("wl_iw_get_wireless_stats phy noise=%d\n", phy_noise));
-
-	bzero(&scb_val, sizeof(scb_val_t));
-	if ((res = dev_wlc_ioctl(dev, WLC_GET_RSSI, &scb_val, sizeof(scb_val_t))))
-		goto done;
-
-	rssi = dtoh32(scb_val.val);
-	WL_TRACE(("wl_iw_get_wireless_stats rssi=%d\n", rssi));
-	if (rssi <= WL_IW_RSSI_NO_SIGNAL)
-		wstats->qual.qual = 0;
-	else if (rssi <= WL_IW_RSSI_VERY_LOW)
-		wstats->qual.qual = 1;
-	else if (rssi <= WL_IW_RSSI_LOW)
-		wstats->qual.qual = 2;
-	else if (rssi <= WL_IW_RSSI_GOOD)
-		wstats->qual.qual = 3;
-	else if (rssi <= WL_IW_RSSI_VERY_GOOD)
-		wstats->qual.qual = 4;
-	else
-		wstats->qual.qual = 5;
-
-	
-	wstats->qual.level = 0x100 + rssi;
-	wstats->qual.noise = 0x100 + phy_noise;
-#if WIRELESS_EXT > 18
-	wstats->qual.updated |= (IW_QUAL_ALL_UPDATED | IW_QUAL_DBM);
-#else
-	wstats->qual.updated |= 7;
-#endif 
-
-#if WIRELESS_EXT > 11
-	WL_TRACE(("wl_iw_get_wireless_stats counters=%d\n", (int)sizeof(wl_cnt_t)));
-
-	memset(&cnt, 0, sizeof(wl_cnt_t));
-	res = dev_wlc_bufvar_get(dev, "counters", (char *)&cnt, sizeof(wl_cnt_t));
-	if (res)
-	{
-		WL_ERROR(("wl_iw_get_wireless_stats counters failed error=%d\n", res));
-		goto done;
-	}
-
-	cnt.version = dtoh16(cnt.version);
-	if (cnt.version != WL_CNT_T_VERSION) {
-		WL_TRACE(("\tIncorrect version of counters struct: expected %d; got %d\n",
-			WL_CNT_T_VERSION, cnt.version));
-		goto done;
-	}
-
-	wstats->discard.nwid = 0;
-	wstats->discard.code = dtoh32(cnt.rxundec);
-	wstats->discard.fragment = dtoh32(cnt.rxfragerr);
-	wstats->discard.retries = dtoh32(cnt.txfail);
-	wstats->discard.misc = dtoh32(cnt.rxrunt) + dtoh32(cnt.rxgiant);
-	wstats->miss.beacon = 0;
-
-	WL_TRACE(("wl_iw_get_wireless_stats counters txframe=%d txbyte=%d\n",
-		dtoh32(cnt.txframe), dtoh32(cnt.txbyte)));
-	WL_TRACE(("wl_iw_get_wireless_stats counters rxfrmtoolong=%d\n", dtoh32(cnt.rxfrmtoolong)));
-	WL_TRACE(("wl_iw_get_wireless_stats counters rxbadplcp=%d\n", dtoh32(cnt.rxbadplcp)));
-	WL_TRACE(("wl_iw_get_wireless_stats counters rxundec=%d\n", dtoh32(cnt.rxundec)));
-	WL_TRACE(("wl_iw_get_wireless_stats counters rxfragerr=%d\n", dtoh32(cnt.rxfragerr)));
-	WL_TRACE(("wl_iw_get_wireless_stats counters txfail=%d\n", dtoh32(cnt.txfail)));
-	WL_TRACE(("wl_iw_get_wireless_stats counters rxrunt=%d\n", dtoh32(cnt.rxrunt)));
-	WL_TRACE(("wl_iw_get_wireless_stats counters rxgiant=%d\n", dtoh32(cnt.rxgiant)));
-
-#endif 
-
-done:
-	return res;
-}
-#if defined(COEX_DHCP)
-static void
-wl_iw_bt_flag_set(
-	struct net_device *dev,
-	bool set)
-{
-#if defined(BT_DHCP_USE_FLAGS)
-	char buf_flag7_dhcp_on[8] = { 7, 00, 00, 00, 0x1, 0x0, 0x00, 0x00 };
-	char buf_flag7_default[8]   = { 7, 00, 00, 00, 0x0, 0x00, 0x00, 0x00};
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-	rtnl_lock();
-#endif
-
-
-#if defined(BT_DHCP_eSCO_FIX)
-	
-	set_btc_esco_params(dev, set);
-#endif
-
-
-#if defined(BT_DHCP_USE_FLAGS)
-	WL_TRACE_COEX(("WI-FI priority boost via bt flags, set:%d\n", set));
-	if (set == TRUE) {
-		
-		dev_wlc_bufvar_set(dev, "btc_flags",
-			(char *)&buf_flag7_dhcp_on[0], sizeof(buf_flag7_dhcp_on));
-	}
-	else  {
-		
-		dev_wlc_bufvar_set(dev, "btc_flags",
-			(char *)&buf_flag7_default[0], sizeof(buf_flag7_default));
-	}
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
-	rtnl_unlock();
-#endif
-}
-
-static void
-wl_iw_bt_timerfunc(ulong data)
-{
-	bt_info_t  *bt_local = (bt_info_t *)data;
-	bt_local->timer_on = 0;
-	WL_TRACE(("%s\n", __FUNCTION__));
-	
-	up(&bt_local->tsk_ctl.sema);
-}
-
-static int
-_bt_dhcp_sysioc_thread(void *data)
-{
-	tsk_ctl_t *tsk_ctl =  (tsk_ctl_t *)data;
-
-	DAEMONIZE("dhcp_sysioc");
-
-	complete(&tsk_ctl->completed);
-
-	while (down_interruptible(&tsk_ctl->sema) == 0) {
-
-		SMP_RD_BARRIER_DEPENDS();
-		if (tsk_ctl->terminated) {
-			break;
-		}
-
-		if (g_bt->timer_on) {
-			g_bt->timer_on = 0;
-			del_timer_sync(&g_bt->timer);
-		}
-
-		switch (g_bt->bt_state) {
-			case BT_DHCP_START:
-				
-				WL_TRACE_COEX(("%s bt_dhcp stm: started \n", __FUNCTION__));
-				g_bt->bt_state = BT_DHCP_OPPORTUNITY_WINDOW;
-				mod_timer(&g_bt->timer,
-				          jiffies + BT_DHCP_OPPORTUNITY_WINDOW_TIME*HZ/1000);
-				g_bt->timer_on = 1;
-				break;
-
-			case BT_DHCP_OPPORTUNITY_WINDOW:
-				if 	(g_bt->dhcp_done) {
-					WL_TRACE_COEX(("%s DHCP Done before T1 expiration\n",
-						__FUNCTION__));
-					goto btc_coex_idle;
-				}
-
-				
-				WL_TRACE_COEX(("%s DHCP T1:%d expired\n",
-					__FUNCTION__, BT_DHCP_OPPORTUNITY_WINDOW_TIME));
-				
-				if (g_bt->dev) wl_iw_bt_flag_set(g_bt->dev, TRUE);
-				g_bt->bt_state = BT_DHCP_FLAG_FORCE_TIMEOUT;
-				mod_timer(&g_bt->timer, jiffies + BT_DHCP_FLAG_FORCE_TIME*HZ/1000);
-				g_bt->timer_on = 1;
-				break;
-
-			case BT_DHCP_FLAG_FORCE_TIMEOUT:
-				if 	(g_bt->dhcp_done) {
-					WL_TRACE_COEX(("%s DHCP Done before T2 expiration\n",
-						__FUNCTION__));
-				} else  {
-					
-					WL_TRACE_COEX(("%s DHCP wait interval T2:%d msec expired\n",
-						__FUNCTION__, BT_DHCP_FLAG_FORCE_TIME));
-				}
-
-				
-				if (g_bt->dev)  wl_iw_bt_flag_set(g_bt->dev, FALSE);
-			btc_coex_idle:
-				g_bt->bt_state = BT_DHCP_IDLE;
-				g_bt->timer_on = 0;
-				break;
-
-			default:
-				WL_ERROR(("%s error g_status=%d !!!\n", __FUNCTION__,
-				          g_bt->bt_state));
-				if (g_bt->dev) wl_iw_bt_flag_set(g_bt->dev, FALSE);
-				g_bt->bt_state = BT_DHCP_IDLE;
-				g_bt->timer_on = 0;
-				break;
-		 }
-
-		net_os_wake_unlock(g_bt->dev);
-	}
-
-	if (g_bt->timer_on) {
-		g_bt->timer_on = 0;
-		del_timer_sync(&g_bt->timer);
-	}
-	complete_and_exit(&tsk_ctl->completed, 0);
-}
-
-static void
-wl_iw_bt_release(void)
-{
-	bt_info_t *bt_local = g_bt;
-
-	if (!bt_local) {
-		return;
-	}
-
-	if (bt_local->tsk_ctl.thr_pid >= 0) {
-		PROC_STOP(&bt_local->tsk_ctl);
-	}
-	kfree(bt_local);
-	g_bt = NULL;
-}
-
-static int
-wl_iw_bt_init(struct net_device *dev)
-{
-	bt_info_t *bt_dhcp = NULL;
-
-	bt_dhcp = kmalloc(sizeof(bt_info_t), GFP_KERNEL);
-	if (!bt_dhcp)
-		return -ENOMEM;
-
-	memset(bt_dhcp, 0, sizeof(bt_info_t));
-
-	g_bt = bt_dhcp;
-	bt_dhcp->dev = dev;
-	bt_dhcp->bt_state = BT_DHCP_IDLE;
-
-	
-	bt_dhcp->timer_ms    = 10;
-	init_timer(&bt_dhcp->timer);
-	bt_dhcp->timer.data = (ulong)bt_dhcp;
-	bt_dhcp->timer.function = wl_iw_bt_timerfunc;
-	bt_dhcp->ts_dhcp_start = 0;
-	bt_dhcp->ts_dhcp_ok = 0;
-
-	PROC_START(_bt_dhcp_sysioc_thread, bt_dhcp, &bt_dhcp->tsk_ctl, 0);
-	if (bt_dhcp->tsk_ctl.thr_pid < 0) {
-		WL_ERROR(("Failed in %s\n", __FUNCTION__));
-		return -ENOMEM;
-	}
-
-	return 0;
-}
-#endif 
-
-int
-wl_iw_attach(struct net_device *dev, void * dhdp)
-{
-#if defined(WL_IW_USE_ISCAN)
-	int params_size = 0;
-#endif 
-	wl_iw_t *iw;
-#if defined(WL_IW_USE_ISCAN)
-	iscan_info_t *iscan = NULL;
-#endif
-
-	DHD_OS_MUTEX_INIT(&wl_cache_lock);
-	DHD_OS_MUTEX_INIT(&wl_softap_lock);
-
-#if defined(WL_IW_USE_ISCAN)
-	if (!dev)
-		return 0;
-
-	
-	memset(&g_wl_iw_params, 0, sizeof(wl_iw_extra_params_t));
-
-	
-#ifdef CSCAN
-	params_size = (WL_SCAN_PARAMS_FIXED_SIZE + OFFSETOF(wl_iscan_params_t, params)) +
-	    (WL_NUMCHANNELS * sizeof(uint16)) + WL_SCAN_PARAMS_SSID_MAX * sizeof(wlc_ssid_t);
-#else
-	params_size = (WL_SCAN_PARAMS_FIXED_SIZE + OFFSETOF(wl_iscan_params_t, params));
-#endif 
-	iscan = kmalloc(sizeof(iscan_info_t), GFP_KERNEL);
-	if (!iscan)
-		return -ENOMEM;
-	memset(iscan, 0, sizeof(iscan_info_t));
-
-	
-	iscan->iscan_ex_params_p = (wl_iscan_params_t*)kmalloc(params_size, GFP_KERNEL);
-	if (!iscan->iscan_ex_params_p) {
-		kfree(iscan);
-		return -ENOMEM;
-	}
-	iscan->iscan_ex_param_size = params_size;
-
-	
-	g_iscan = iscan;
-	iscan->dev = dev;
-	iscan->iscan_state = ISCAN_STATE_IDLE;
-
-#if defined(CONFIG_FIRST_SCAN)
-	g_first_broadcast_scan = BROADCAST_SCAN_FIRST_IDLE;
-	g_first_counter_scans = 0;
-	g_iscan->scan_flag = 0;
-#endif 
-
-	
-	iscan->timer_ms    = 8000;
-	init_timer(&iscan->timer);
-	iscan->timer.data = (ulong)iscan;
-	iscan->timer.function = wl_iw_timerfunc;
-
-	PROC_START(_iscan_sysioc_thread, iscan, &iscan->tsk_ctl, 0);
-	if (iscan->tsk_ctl.thr_pid < 0)
-		return -ENOMEM;
-#endif 
-
-	iw = *(wl_iw_t **)netdev_priv(dev);
-	iw->pub = (dhd_pub_t *)dhdp;
-#ifdef SOFTAP
-	priv_dev = dev;
-#endif 
-	g_scan = NULL;
-
-	
-	g_scan = (void *)kmalloc(G_SCAN_RESULTS, GFP_KERNEL);
-	if (!g_scan)
-		return -ENOMEM;
-
-	memset(g_scan, 0, G_SCAN_RESULTS);
-	g_scan_specified_ssid = 0;
-
-#if !defined(CSCAN)
-	
-	wl_iw_init_ss_cache_ctrl();
-#endif 
-#ifdef COEX_DHCP
-	
-	wl_iw_bt_init(dev);
-#endif 
-
-
-	return 0;
-}
-
-void
-wl_iw_detach(void)
-{
-#if defined(WL_IW_USE_ISCAN)
-	iscan_buf_t  *buf;
-	iscan_info_t *iscan = g_iscan;
-
-	if (!iscan)
-		return;
-	if (iscan->tsk_ctl.thr_pid >= 0) {
-		PROC_STOP(&iscan->tsk_ctl);
-	}
-	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
-	while (iscan->list_hdr) {
-		buf = iscan->list_hdr->next;
-		kfree(iscan->list_hdr);
-		iscan->list_hdr = buf;
-	}
-	kfree(iscan->iscan_ex_params_p);
-	kfree(iscan);
-	g_iscan = NULL;
-	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
-#endif 
-
-	if (g_scan)
-		kfree(g_scan);
-
-	g_scan = NULL;
-#if !defined(CSCAN)
-	wl_iw_release_ss_cache_ctrl();
-#endif 
-#ifdef COEX_DHCP
-	wl_iw_bt_release();
-#endif 
-
-#ifdef SOFTAP
-	if (ap_cfg_running) {
-		WL_TRACE(("\n%s AP is going down\n", __FUNCTION__));
-		
-		wl_iw_send_priv_event(priv_dev, "AP_DOWN");
-	}
-#endif
-
-}
diff --git a/drivers/net/wireless/bcmdhd/wldev_common.c b/drivers/net/wireless/bcmdhd/wldev_common.c
deleted file mode 100644
index b01e4a2b8c9..00000000000
--- a/drivers/net/wireless/bcmdhd/wldev_common.c
+++ /dev/null
@@ -1,333 +0,0 @@
-/*
- * Common function shared by Linux WEXT, cfg80211 and p2p drivers
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- * 
- *         Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- * 
- *      As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module.  An independent module is a module which is not
- * derived from this software.  The special exception does not apply to any
- * modifications of the software.
- * 
- *      Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: wldev_common.c,v 1.1.4.1.2.14 2011-02-09 01:40:07 Exp $
- */
-
-#include <linux/module.h>
-#include <linux/netdevice.h>
-
-#include <wldev_common.h>
-#include <bcmutils.h>
-#include <dhd_dbg.h>
-
-#define htod32(i) i
-#define htod16(i) i
-#define dtoh32(i) i
-#define dtoh16(i) i
-#define htodchanspec(i) i
-#define dtohchanspec(i) i
-
-extern int dhd_ioctl_entry_local(struct net_device *net, wl_ioctl_t *ioc, int cmd);
-
-s32 wldev_ioctl(
-	struct net_device *dev, u32 cmd, void *arg, u32 len, u32 set)
-{
-	s32 ret = 0;
-	struct wl_ioctl ioc;
-
-	memset(&ioc, 0, sizeof(ioc));
-	ioc.cmd = cmd;
-	ioc.buf = arg;
-	ioc.len = len;
-	ioc.set = set;
-
-	ret = dhd_ioctl_entry_local(dev, &ioc, cmd);
-	return ret;
-}
-
-/* Format a iovar buffer, not bsscfg indexed. The bsscfg index will be
- * taken care of in dhd_ioctl_entry. Internal use only, not exposed to
- * wl_iw, wl_cfg80211 and wl_cfgp2p
- */
-static s32 wldev_mkiovar(
-	s8 *iovar_name, s8 *param, s32 paramlen,
-	s8 *iovar_buf, u32 buflen)
-{
-	s32 iolen = 0;
-
-	iolen = bcm_mkiovar(iovar_name, param, paramlen, iovar_buf, buflen);
-	return iolen;
-}
-
-s32 wldev_iovar_getbuf(
-	struct net_device *dev, s8 *iovar_name,
-	void *param, s32 paramlen, void *buf, s32 buflen)
-{
-	s32 ret = 0;
-	s32 iovar_len = 0;
-
-	iovar_len = wldev_mkiovar(iovar_name, param, paramlen, buf, buflen);
-	ret = wldev_ioctl(dev, WLC_GET_VAR, buf, iovar_len, FALSE);
-	return ret;
-}
-
-
-s32 wldev_iovar_setbuf(
-	struct net_device *dev, s8 *iovar_name,
-	void *param, s32 paramlen, void *buf, s32 buflen)
-{
-	s32 ret = 0;
-	s32 iovar_len;
-
-	iovar_len = wldev_mkiovar(iovar_name, param, paramlen, buf, buflen);
-	ret = wldev_ioctl(dev, WLC_SET_VAR, buf, iovar_len, TRUE);
-	return ret;
-}
-
-s32 wldev_iovar_setint(
-	struct net_device *dev, s8 *iovar, s32 val)
-{
-	s8 iovar_buf[WLC_IOCTL_SMLEN];
-
-	val = htod32(val);
-	memset(iovar_buf, 0, sizeof(iovar_buf));
-	return wldev_iovar_setbuf(dev, iovar, &val, sizeof(val), iovar_buf,
-		sizeof(iovar_buf));
-}
-
-
-s32 wldev_iovar_getint(
-	struct net_device *dev, s8 *iovar, s32 *pval)
-{
-	s8 iovar_buf[WLC_IOCTL_SMLEN];
-	s32 err;
-
-	memset(iovar_buf, 0, sizeof(iovar_buf));
-	err = wldev_iovar_getbuf(dev, iovar, pval, sizeof(*pval), iovar_buf,
-		sizeof(iovar_buf));
-	if (err == 0)
-	{
-		memcpy(pval, iovar_buf, sizeof(*pval));
-		*pval = dtoh32(*pval);
-	}
-	return err;
-}
-
-/** Format a bsscfg indexed iovar buffer. The bsscfg index will be
- *  taken care of in dhd_ioctl_entry. Internal use only, not exposed to
- *  wl_iw, wl_cfg80211 and wl_cfgp2p
- */
-s32 wldev_mkiovar_bsscfg(
-	const s8 *iovar_name, s8 *param, s32 paramlen,
-	s8 *iovar_buf, s32 buflen, s32 bssidx)
-{
-	const s8 *prefix = "bsscfg:";
-	s8 *p;
-	u32 prefixlen;
-	u32 namelen;
-	u32 iolen;
-
-	if (bssidx == 0) {
-		return wldev_mkiovar((s8*)iovar_name, (s8 *)param, paramlen,
-			(s8 *) iovar_buf, buflen);
-	}
-
-	prefixlen = (u32) strlen(prefix); /* lengh of bsscfg prefix */
-	namelen = (u32) strlen(iovar_name) + 1; /* lengh of iovar  name + null */
-	iolen = prefixlen + namelen + sizeof(u32) + paramlen;
-
-	if (buflen < 0 || iolen > (u32)buflen)
-	{
-		DHD_ERROR(("%s: buffer is too short\n", __FUNCTION__));
-		return BCME_BUFTOOSHORT;
-	}
-
-	p = (s8 *)iovar_buf;
-
-	/* copy prefix, no null */
-	memcpy(p, prefix, prefixlen);
-	p += prefixlen;
-
-	/* copy iovar name including null */
-	memcpy(p, iovar_name, namelen);
-	p += namelen;
-
-	/* bss config index as first param */
-	bssidx = htod32(bssidx);
-	memcpy(p, &bssidx, sizeof(u32));
-	p += sizeof(u32);
-
-	/* parameter buffer follows */
-	if (paramlen)
-		memcpy(p, param, paramlen);
-
-	return iolen;
-
-}
-
-s32 wldev_iovar_getbuf_bsscfg(
-	struct net_device *dev, s8 *iovar_name,
-	void *param, s32 paramlen, void *buf, s32 buflen, s32 bsscfg_idx)
-{
-	s32 ret = 0;
-	s32 iovar_len = 0;
-
-	iovar_len = wldev_mkiovar_bsscfg(iovar_name, param, paramlen, buf, buflen, bsscfg_idx);
-	ret = wldev_ioctl(dev, WLC_GET_VAR, buf, iovar_len, FALSE);
-	return ret;
-
-}
-
-s32 wldev_iovar_setbuf_bsscfg(
-	struct net_device *dev, s8 *iovar_name,
-	void *param, s32 paramlen, void *buf, s32 buflen, s32 bsscfg_idx)
-{
-	s32 ret = 0;
-	s32 iovar_len;
-
-	iovar_len = wldev_mkiovar_bsscfg(iovar_name, param, paramlen, buf, buflen, bsscfg_idx);
-	ret = wldev_ioctl(dev, WLC_SET_VAR, buf, iovar_len, TRUE);
-	return ret;
-}
-
-s32 wldev_iovar_setint_bsscfg(
-	struct net_device *dev, s8 *iovar, s32 val, s32 bssidx)
-{
-	s8 iovar_buf[WLC_IOCTL_SMLEN];
-
-	val = htod32(val);
-	memset(iovar_buf, 0, sizeof(iovar_buf));
-	return wldev_iovar_setbuf_bsscfg(dev, iovar, &val, sizeof(val), iovar_buf,
-		sizeof(iovar_buf), bssidx);
-}
-
-
-s32 wldev_iovar_getint_bsscfg(
-	struct net_device *dev, s8 *iovar, s32 *pval, s32 bssidx)
-{
-	s8 iovar_buf[WLC_IOCTL_SMLEN];
-	s32 err;
-
-	memset(iovar_buf, 0, sizeof(iovar_buf));
-	err = wldev_iovar_getbuf_bsscfg(dev, iovar, pval, sizeof(*pval), iovar_buf,
-		sizeof(iovar_buf), bssidx);
-	if (err == 0)
-	{
-		memcpy(pval, iovar_buf, sizeof(*pval));
-		*pval = dtoh32(*pval);
-	}
-	return err;
-}
-
-int wldev_get_link_speed(
-	struct net_device *dev, int *plink_speed)
-{
-	int error;
-
-	if (!plink_speed)
-		return -ENOMEM;
-	error = wldev_ioctl(dev, WLC_GET_RATE, plink_speed, sizeof(int), 0);
-	if (unlikely(error))
-		return error;
-
-	/* Convert internal 500Kbps to Kbps */
-	*plink_speed *= 500;
-	return error;
-}
-
-int wldev_get_rssi(
-	struct net_device *dev, int *prssi)
-{
-	scb_val_t scb_val;
-	int error;
-
-	if (!prssi)
-		return -ENOMEM;
-	bzero(&scb_val, sizeof(scb_val_t));
-
-	error = wldev_ioctl(dev, WLC_GET_RSSI, &scb_val, sizeof(scb_val_t), 0);
-	if (unlikely(error))
-		return error;
-
-	*prssi = dtoh32(scb_val.val);
-	return error;
-}
-
-int wldev_get_ssid(
-	struct net_device *dev, wlc_ssid_t *pssid)
-{
-	int error;
-
-	if (!pssid)
-		return -ENOMEM;
-	error = wldev_ioctl(dev, WLC_GET_SSID, pssid, sizeof(wlc_ssid_t), 0);
-	if (unlikely(error))
-		return error;
-	pssid->SSID_len = dtoh32(pssid->SSID_len);
-	return error;
-}
-
-int wldev_get_band(
-	struct net_device *dev, uint *pband)
-{
-	int error;
-
-	error = wldev_ioctl(dev, WLC_GET_BAND, pband, sizeof(uint), 0);
-	return error;
-}
-
-int wldev_set_band(
-	struct net_device *dev, uint band)
-{
-	int error = -1;
-
-	if ((band == WLC_BAND_AUTO) || (band == WLC_BAND_5G) || (band == WLC_BAND_2G)) {
-		error = wldev_ioctl(dev, WLC_SET_BAND, &band, sizeof(band), 1);
-	}
-	return error;
-}
-
-int wldev_set_country(
-	struct net_device *dev, char *country_code)
-{
-	int error = -1;
-	wl_country_t cspec = {{0}, 0, {0}};
-	scb_val_t scbval;
-	char smbuf[WLC_IOCTL_SMLEN];
-
-	if (!country_code)
-		return error;
-
-	bzero(&scbval, sizeof(scb_val_t));
-	error = wldev_ioctl(dev, WLC_DISASSOC, &scbval, sizeof(scb_val_t), 1);
-	if (error < 0) {
-		DHD_ERROR(("%s: set country failed due to Disassoc error\n", __FUNCTION__));
-		return error;
-	}
-	cspec.rev = -1;
-	memcpy(cspec.country_abbrev, country_code, WLC_CNTRY_BUF_SZ);
-	memcpy(cspec.ccode, country_code, WLC_CNTRY_BUF_SZ);
-	get_customized_country_code((char *)&cspec.country_abbrev, &cspec);
-	error = wldev_iovar_setbuf(dev, "country", &cspec, sizeof(cspec),
-					smbuf, sizeof(smbuf));
-	if (error < 0) {
-		DHD_ERROR(("%s: set country for %s as %s rev %d failed\n",
-			__FUNCTION__, country_code, cspec.ccode, cspec.rev));
-		return error;
-	}
-	dhd_bus_country_set(dev, &cspec);
-	DHD_INFO(("%s: set country for %s as %s rev %d\n",
-		__FUNCTION__, country_code, cspec.ccode, cspec.rev));
-	return 0;
-}
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index e57b50b3856..a4fcd78c6d6 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -235,4 +235,29 @@ config CHARGER_GPIO
 	  This driver can be build as a module. If so, the module will be
 	  called gpio-charger.
 
+config AB8500_BM
+	bool "AB8500 Battery Management Driver"
+	depends on AB8500_CORE && AB8500_GPADC && ARCH_U8500
+	help
+	  Say Y to include support for AB8500 battery management.
+
+config AB8500_BATTERY_THERM_ON_BATCTRL
+	bool "Thermistor connected on BATCTRL ADC"
+	depends on AB8500_BM
+	help
+	  Say Y to enable battery temperature measurements using
+	  thermistor connected on BATCTRL ADC.
+
+config AB5500_BM
+	bool "AB5500 Battery Management Driver"
+	depends on AB5500_CORE && AB8500_GPADC && ARCH_U8500
+	help
+	  Say Y to include support for AB5500 battery management.
+
+config AB5500_BATTERY_THERM_ON_BATCTRL
+	bool "Thermistor connected on BATCTRL ADC"
+	depends on AB5500_BM
+	help
+	  Say Y to enable battery temperature measurements using
+	  thermistor connected on BATCTRL ADC.
 endif # POWER_SUPPLY
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index 009a90fa8ac..f9f9fba0c48 100644
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -36,3 +36,5 @@ obj-$(CONFIG_CHARGER_ISP1704)	+= isp1704_charger.o
 obj-$(CONFIG_CHARGER_MAX8903)	+= max8903_charger.o
 obj-$(CONFIG_CHARGER_TWL4030)	+= twl4030_charger.o
 obj-$(CONFIG_CHARGER_GPIO)	+= gpio-charger.o
+obj-$(CONFIG_AB8500_BM)		+= ab8500_charger.o ab8500_btemp.o ab8500_fg.o ab8500_chargalg.o
+obj-$(CONFIG_AB5500_BM)		+= ab5500_charger.o abx500_chargalg.o ab5500_fg.o ab5500_btemp.o
diff --git a/drivers/power/ab5500_btemp.c b/drivers/power/ab5500_btemp.c
new file mode 100644
index 00000000000..04ad6bbb7ac
--- /dev/null
+++ b/drivers/power/ab5500_btemp.c
@@ -0,0 +1,877 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Battery temperature driver for ab5500
+ *
+ * License Terms: GNU General Public License v2
+ * Authors:
+ *	Johan Palsson <johan.palsson@stericsson.com>
+ *	Karl Komierowski <karl.komierowski@stericsson.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/completion.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500-bm.h>
+#include <linux/mfd/abx500/ab5500-gpadc.h>
+
+#define BTEMP_THERMAL_LOW_LIMIT		-10
+#define BTEMP_THERMAL_MED_LIMIT		0
+#define BTEMP_THERMAL_HIGH_LIMIT_62	62
+
+#define BTEMP_BATCTRL_CURR_SRC_7UA	7
+#define BTEMP_BATCTRL_CURR_SRC_15UA	15
+#define BTEMP_BATCTRL_CURR_SRC_20UA	20
+
+#define UART_MODE			0x0F
+#define BAT_CUR_SRC			0x1F
+#define RESIS_ID_MODE			0x03
+#define RESET				0x00
+#define ADOUT_10K_PULL_UP		0x07
+
+#define to_ab5500_btemp_device_info(x) container_of((x), \
+	struct ab5500_btemp, btemp_psy);
+
+/**
+ * struct ab5500_btemp_interrupts - ab5500 interrupts
+ * @name:	name of the interrupt
+ * @isr		function pointer to the isr
+ */
+struct ab5500_btemp_interrupts {
+	char *name;
+	irqreturn_t (*isr)(int irq, void *data);
+};
+
+struct ab5500_btemp_events {
+	bool batt_rem;
+	bool usb_conn;
+};
+
+/**
+ * struct ab5500_btemp - ab5500 BTEMP device information
+ * @dev:		Pointer to the structure device
+ * @chip_id:		Chip-Id of the AB5500
+ * @curr_source:	What current source we use, in uA
+ * @bat_temp:		Battery temperature in degree Celcius
+ * @prev_bat_temp	Last dispatched battery temperature
+ * @node:		struct of type list_head
+ * @parent:		Pointer to the struct ab5500
+ * @gpadc:		Pointer to the struct gpadc
+ * @gpadc-auto:		Pointer to the struct adc_auto_input
+ * @pdata:		Pointer to the ab5500_btemp platform data
+ * @bat:		Pointer to the ab5500_bm platform data
+ * @btemp_psy:		Structure for BTEMP specific battery properties
+ * @events:		Structure for information about events triggered
+ * @btemp_wq:		Work queue for measuring the temperature periodically
+ * @btemp_periodic_work:	Work for measuring the temperature periodically
+ */
+struct ab5500_btemp {
+	struct device *dev;
+	u8 chip_id;
+	int curr_source;
+	int bat_temp;
+	int prev_bat_temp;
+	struct list_head node;
+	struct ab5500 *parent;
+	struct ab5500_gpadc *gpadc;
+	struct adc_auto_input *gpadc_auto;
+	struct abx500_btemp_platform_data *pdata;
+	struct abx500_bm_data *bat;
+	struct power_supply btemp_psy;
+	struct ab5500_btemp_events events;
+	struct workqueue_struct *btemp_wq;
+	struct delayed_work btemp_periodic_work;
+};
+
+/* BTEMP power supply properties */
+static enum power_supply_property ab5500_btemp_props[] = {
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_TEMP,
+};
+
+static LIST_HEAD(ab5500_btemp_list);
+
+struct ab5500_btemp *ab5500_btemp_get(void)
+{
+	struct ab5500_btemp *di;
+	di = list_first_entry(&ab5500_btemp_list, struct ab5500_btemp, node);
+
+	return di;
+}
+
+/**
+ * ab5500_btemp_batctrl_volt_to_res() - convert batctrl voltage to resistance
+ * @di:		pointer to the ab5500_btemp structure
+ * @v_batctrl:	measured batctrl voltage
+ *
+ * This function returns the battery resistance that is
+ * derived from the BATCTRL voltage.
+ * Returns value in Ohms.
+ */
+static int ab5500_btemp_batctrl_volt_to_res(struct ab5500_btemp *di,
+	int v_batctrl)
+{
+	int rbs;
+
+	if (di->bat->adc_therm == ABx500_ADC_THERM_BATCTRL) {
+		/*
+		 * If the battery has internal NTC, we use the current
+		 * source to calculate the resistance, 7uA or 20uA
+		 */
+		rbs = v_batctrl * 1000 / di->curr_source;
+	} else {
+		/*
+		 * BAT_CTRL is internally
+		 * connected to 1.8V through a 10k resistor
+		 */
+		rbs = (10000 * (v_batctrl)) / (1800 - v_batctrl);
+	}
+	return rbs;
+}
+
+/**
+ * ab5500_btemp_read_batctrl_voltage() - measure batctrl voltage
+ * @di:		pointer to the ab5500_btemp structure
+ *
+ * This function returns the voltage on BATCTRL. Returns value in mV.
+ */
+static int ab5500_btemp_read_batctrl_voltage(struct ab5500_btemp *di)
+{
+	int vbtemp;
+	static int prev;
+
+	vbtemp = ab5500_gpadc_convert(di->gpadc, BAT_CTRL);
+	if (vbtemp < 0) {
+		dev_err(di->dev,
+			"%s gpadc conversion failed, using previous value",
+			__func__);
+		return prev;
+	}
+	prev = vbtemp;
+	return vbtemp;
+}
+
+/**
+ * ab5500_btemp_curr_source_enable() - enable/disable batctrl current source
+ * @di:		pointer to the ab5500_btemp structure
+ * @enable:	enable or disable the current source
+ *
+ * Enable or disable the current sources for the BatCtrl AD channel
+ */
+static int ab5500_btemp_curr_source_enable(struct ab5500_btemp *di,
+	bool enable)
+{
+	int ret = 0;
+
+	/* Only do this for batteries with internal NTC */
+	if (di->bat->adc_therm == ABx500_ADC_THERM_BATCTRL && enable) {
+
+		dev_dbg(di->dev, "Set BATCTRL %duA\n", di->curr_source);
+
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_UART,
+			UART_MODE, RESIS_ID_MODE);
+		if (ret) {
+			dev_err(di->dev,
+				"%s failed setting resistance identification mode\n",
+				__func__);
+			return ret;
+		}
+
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_URI,
+			BAT_CUR_SRC, BAT_CTRL_15U_ENA);
+		if (ret) {
+			dev_err(di->dev, "%s failed enabling current source\n",
+				__func__);
+			goto disable_curr_source;
+		}
+	} else if (di->bat->adc_therm == ABx500_ADC_THERM_BATCTRL && !enable) {
+		dev_dbg(di->dev, "Disable BATCTRL curr source\n");
+
+		/* Write 0 to the curr bits */
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_URI,
+			BAT_CUR_SRC, RESET);
+		if (ret) {
+			dev_err(di->dev, "%s failed disabling current source\n",
+				__func__);
+			goto disable_curr_source;
+		}
+
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_UART,
+			UART_MODE, RESET);
+		if (ret) {
+			dev_err(di->dev, "%s failed disabling force comp\n",
+				__func__);
+		}
+	}
+	return ret;
+disable_curr_source:
+	/* Write 0 to the curr bits */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_URI,
+			BAT_CUR_SRC, RESET);
+	if (ret) {
+		dev_err(di->dev, "%s failed disabling current source\n",
+			__func__);
+	}
+	return ret;
+}
+
+/**
+ * ab5500_btemp_get_batctrl_res() - get battery resistance
+ * @di:		pointer to the ab5500_btemp structure
+ *
+ * This function returns the battery pack identification resistance.
+ * Returns value in Ohms.
+ */
+static int ab5500_btemp_get_batctrl_res(struct ab5500_btemp *di)
+{
+	int ret;
+	int batctrl;
+	int res;
+
+	ret = ab5500_btemp_curr_source_enable(di, true);
+	/* TODO: This delay has to be optimised */
+	mdelay(1000);
+	if (ret) {
+		dev_err(di->dev, "%s curr source enable failed\n", __func__);
+		return ret;
+	}
+
+	batctrl = ab5500_btemp_read_batctrl_voltage(di);
+	res = ab5500_btemp_batctrl_volt_to_res(di, batctrl);
+
+	ret = ab5500_btemp_curr_source_enable(di, false);
+	if (ret) {
+		dev_err(di->dev, "%s curr source disable failed\n", __func__);
+		return ret;
+	}
+
+	dev_dbg(di->dev, "%s batctrl: %d res: %d ",
+		__func__, batctrl, res);
+
+	return res;
+}
+
+/**
+ * ab5500_btemp_res_to_temp() - resistance to temperature
+ * @di:		pointer to the ab5500_btemp structure
+ * @tbl:	pointer to the resiatance to temperature table
+ * @tbl_size:	size of the resistance to temperature table
+ * @res:	resistance to calculate the temperature from
+ *
+ * This function returns the battery temperature in degrees Celcius
+ * based on the NTC resistance.
+ */
+static int ab5500_btemp_res_to_temp(struct ab5500_btemp *di,
+	const struct abx500_res_to_temp *tbl, int tbl_size, int res)
+{
+	int i, temp;
+	/*
+	 * Calculate the formula for the straight line
+	 * Simple interpolation if we are within
+	 * the resistance table limits, extrapolate
+	 * if resistance is outside the limits.
+	 */
+	if (res > tbl[0].resist)
+		i = 0;
+	else if (res <= tbl[tbl_size - 1].resist)
+		i = tbl_size - 2;
+	else {
+		i = 0;
+		while (!(res <= tbl[i].resist &&
+			res > tbl[i + 1].resist))
+			i++;
+	}
+
+	temp = tbl[i].temp + ((tbl[i + 1].temp - tbl[i].temp) *
+		(res - tbl[i].resist)) / (tbl[i + 1].resist - tbl[i].resist);
+	return temp;
+}
+
+/**
+ * ab5500_btemp_measure_temp() - measure battery temperature
+ * @di:		pointer to the ab5500_btemp structure
+ *
+ * Returns battery temperature (on success) else the previous temperature
+ */
+static int ab5500_btemp_measure_temp(struct ab5500_btemp *di)
+{
+	int temp, ret;
+	static int prev;
+	int rbat, vntc;
+	int rntc = 0;
+	u8 id;
+
+	id = di->bat->batt_id;
+	if (di->bat->adc_therm == ABx500_ADC_THERM_BATCTRL &&
+			id != BATTERY_UNKNOWN) {
+		rbat = ab5500_btemp_get_batctrl_res(di);
+		if (rbat < 0) {
+			dev_err(di->dev, "%s get batctrl res failed\n",
+				__func__);
+			/*
+			 * Return out-of-range temperature so that
+			 * charging is stopped
+			 */
+			return BTEMP_THERMAL_LOW_LIMIT;
+		}
+
+		temp = ab5500_btemp_res_to_temp(di,
+			di->bat->bat_type[id].r_to_t_tbl,
+			di->bat->bat_type[id].n_temp_tbl_elements, rbat);
+	} else {
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_UART,
+			UART_MODE, ADOUT_10K_PULL_UP);
+		if (ret) {
+			dev_err(di->dev,
+				"failed to enable 10k pull up to Vadout\n");
+		}
+		vntc = ab5500_gpadc_convert(di->gpadc, BTEMP_BALL);
+		if (vntc < 0) {
+			dev_err(di->dev,
+				"%s gpadc conversion failed,"
+				" using previous value\n", __func__);
+			return prev;
+		}
+		/*
+		 * The PCB NTC is sourced from 2.75v via a 10kOhm
+		 * resistor.
+		 */
+		rntc = 10000 * vntc / (27500 - vntc);
+
+		temp = ab5500_btemp_res_to_temp(di,
+			di->bat->bat_type[id].r_to_t_tbl,
+			di->bat->bat_type[id].n_temp_tbl_elements, rntc);
+		prev = temp;
+	}
+	dev_dbg(di->dev, "Battery temperature is %d\n", temp);
+	return temp;
+}
+
+/**
+ * ab5500_btemp_id() - Identify the connected battery
+ * @di:		pointer to the ab5500_btemp structure
+ *
+ * This function will try to identify the battery by reading the ID
+ * resistor. Some brands use a combined ID resistor with a NTC resistor to
+ * both be able to identify and to read the temperature of it.
+ */
+static int ab5500_btemp_id(struct ab5500_btemp *di)
+{
+	int res;
+	u8 i;
+
+	di->curr_source = BTEMP_BATCTRL_CURR_SRC_7UA;
+	di->bat->batt_id = BATTERY_UNKNOWN;
+
+	res =  ab5500_btemp_get_batctrl_res(di);
+	if (res < 0) {
+		dev_err(di->dev, "%s get batctrl res failed\n", __func__);
+		return -ENXIO;
+	}
+
+	/* BATTERY_UNKNOWN is defined on position 0, skip it! */
+	for (i = BATTERY_UNKNOWN + 1; i < di->bat->n_btypes; i++) {
+		if ((res <= di->bat->bat_type[i].resis_high) &&
+			(res >= di->bat->bat_type[i].resis_low)) {
+			dev_dbg(di->dev, "Battery detected on %s"
+				" low %d < res %d < high: %d"
+				" index: %d\n",
+				di->bat->adc_therm == ABx500_ADC_THERM_BATCTRL ?
+				"BATCTRL" : "BATTEMP",
+				di->bat->bat_type[i].resis_low, res,
+				di->bat->bat_type[i].resis_high, i);
+
+			di->bat->batt_id = i;
+			break;
+		}
+	}
+
+	if (di->bat->batt_id == BATTERY_UNKNOWN) {
+		dev_warn(di->dev, "Battery identified as unknown"
+			", resistance %d Ohm\n", res);
+		return -ENXIO;
+	}
+
+	/*
+	 * We only have to change current source if the
+	 * detected type is Type 1, else we use the 7uA source
+	 */
+	if (di->bat->adc_therm == ABx500_ADC_THERM_BATCTRL &&
+			di->bat->batt_id == 1) {
+		dev_dbg(di->dev, "Set BATCTRL current source to 15uA\n");
+		di->curr_source = BTEMP_BATCTRL_CURR_SRC_15UA;
+	}
+
+	return di->bat->batt_id;
+}
+
+/**
+ * ab5500_btemp_periodic_work() - Measuring the temperature periodically
+ * @work:	pointer to the work_struct structure
+ *
+ * Work function for measuring the temperature periodically
+ */
+static void ab5500_btemp_periodic_work(struct work_struct *work)
+{
+	struct ab5500_btemp *di = container_of(work,
+		struct ab5500_btemp, btemp_periodic_work.work);
+
+	di->bat_temp = ab5500_btemp_measure_temp(di);
+
+	if (di->bat_temp != di->prev_bat_temp) {
+		di->prev_bat_temp = di->bat_temp;
+		power_supply_changed(&di->btemp_psy);
+	}
+	di->bat->temp_now = di->bat_temp;
+
+	/* Schedule a new measurement */
+	queue_delayed_work(di->btemp_wq,
+		&di->btemp_periodic_work,
+		round_jiffies(20 * HZ));
+}
+
+/**
+ * ab5500_btemp_batt_removal_handler() - battery removal detected
+ * @irq:       interrupt number
+ * @_di:       void pointer that has to address of ab5500_btemp
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_btemp_batt_removal_handler(int irq, void *_di)
+{
+	struct ab5500_btemp *di = _di;
+	dev_err(di->dev, "Battery removal detected!\n");
+
+	di->events.batt_rem = true;
+	power_supply_changed(&di->btemp_psy);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_btemp_batt_attach_handler() - battery insertion detected
+ * @irq:       interrupt number
+ * @_di:       void pointer that has to address of ab5500_btemp
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_btemp_batt_attach_handler(int irq, void *_di)
+{
+	struct ab5500_btemp *di = _di;
+	dev_err(di->dev, "Battery attached!\n");
+
+	di->events.batt_rem = false;
+	power_supply_changed(&di->btemp_psy);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_btemp_periodic() - Periodic temperature measurements
+ * @di:		pointer to the ab5500_btemp structure
+ * @enable:	enable or disable periodic temperature measurements
+ *
+ * Starts of stops periodic temperature measurements. Periodic measurements
+ * should only be done when a charger is connected.
+ */
+static void ab5500_btemp_periodic(struct ab5500_btemp *di,
+	bool enable)
+{
+	dev_dbg(di->dev, "Enable periodic temperature measurements: %d\n",
+		enable);
+
+	if (enable)
+		queue_delayed_work(di->btemp_wq, &di->btemp_periodic_work, 0);
+	else
+		cancel_delayed_work_sync(&di->btemp_periodic_work);
+}
+
+/**
+ * ab5500_btemp_get_property() - get the btemp properties
+ * @psy:        pointer to the power_supply structure
+ * @psp:        pointer to the power_supply_property structure
+ * @val:        pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the btemp
+ * properties by reading the sysfs files.
+ * online:	presence of the battery
+ * present:	presence of the battery
+ * technology:	battery technology
+ * temp:	battery temperature
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_btemp_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct ab5500_btemp *di;
+
+	di = to_ab5500_btemp_device_info(psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_PRESENT:
+	case POWER_SUPPLY_PROP_ONLINE:
+		if (di->events.batt_rem)
+			val->intval = 0;
+		else
+			val->intval = 1;
+		break;
+	case POWER_SUPPLY_PROP_TECHNOLOGY:
+		val->intval = di->bat->bat_type[di->bat->batt_id].name;
+		break;
+	case POWER_SUPPLY_PROP_TEMP:
+		if (di->bat->batt_id == BATTERY_UNKNOWN)
+		/*
+		 * In case the battery is not identified, its assumed that
+		 * we are using the power supply and since no monitoring is
+		 * done for the same, a nominal temp is hardocded.
+		 */
+			val->intval = 250;
+		else
+			val->intval = di->bat_temp * 10;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ab5500_btemp_get_ext_psy_data(struct device *dev, void *data)
+{
+	struct power_supply *psy;
+	struct power_supply *ext;
+	struct ab5500_btemp *di;
+	union power_supply_propval ret;
+	int i, j;
+	bool psy_found = false;
+
+	psy = (struct power_supply *)data;
+	ext = dev_get_drvdata(dev);
+	di = to_ab5500_btemp_device_info(psy);
+
+	/*
+	 * For all psy where the name of your driver
+	 * appears in any supplied_to
+	 */
+	for (i = 0; i < ext->num_supplicants; i++) {
+		if (!strcmp(ext->supplied_to[i], psy->name))
+			psy_found = true;
+	}
+
+	if (!psy_found)
+		return 0;
+
+	/* Go through all properties for the psy */
+	for (j = 0; j < ext->num_properties; j++) {
+		enum power_supply_property prop;
+		prop = ext->properties[j];
+
+		if (ext->get_property(ext, prop, &ret))
+			continue;
+
+		switch (prop) {
+		case POWER_SUPPLY_PROP_PRESENT:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_USB:
+				/* USB disconnected */
+				if (!ret.intval && di->events.usb_conn) {
+					di->events.usb_conn = false;
+						ab5500_btemp_periodic(di,
+							false);
+				}
+				/* USB connected */
+				else if (ret.intval && !di->events.usb_conn) {
+					di->events.usb_conn = true;
+						ab5500_btemp_periodic(di, true);
+				}
+				break;
+			default:
+				break;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+/**
+ * ab5500_btemp_external_power_changed() - callback for power supply changes
+ * @psy:       pointer to the structure power_supply
+ *
+ * This function is pointing to the function pointer external_power_changed
+ * of the structure power_supply.
+ * This function gets executed when there is a change in the external power
+ * supply to the btemp.
+ */
+static void ab5500_btemp_external_power_changed(struct power_supply *psy)
+{
+	struct ab5500_btemp *di = to_ab5500_btemp_device_info(psy);
+
+	class_for_each_device(power_supply_class, NULL,
+		&di->btemp_psy, ab5500_btemp_get_ext_psy_data);
+}
+
+/* ab5500 btemp driver interrupts and their respective isr */
+static struct ab5500_btemp_interrupts ab5500_btemp_irq[] = {
+	{"BATT_REMOVAL", ab5500_btemp_batt_removal_handler},
+	{"BATT_ATTACH", ab5500_btemp_batt_attach_handler},
+};
+static int ab5500_btemp_bat_temp_trig(int mux)
+{
+	struct ab5500_btemp *di = ab5500_btemp_get();
+
+	if (di->bat_temp < BTEMP_THERMAL_LOW_LIMIT) {
+		dev_err(di->dev,
+			"battery temp less than lower threshold (-10 deg cel)\n");
+		power_supply_changed(&di->btemp_psy);
+	} else if (di->bat_temp > BTEMP_THERMAL_HIGH_LIMIT_62) {
+		dev_err(di->dev, "battery temp greater them max threshold\n");
+		power_supply_changed(&di->btemp_psy);
+	}
+	return 0;;
+}
+
+static int ab5500_btemp_auto_temp(struct ab5500_btemp *di)
+{
+	struct adc_auto_input *auto_ip;
+	int ret = 0;
+
+	auto_ip = kzalloc(sizeof(struct adc_auto_input), GFP_KERNEL);
+	if (!auto_ip) {
+		dev_err(di->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	auto_ip->mux = BTEMP_BALL;
+	auto_ip->freq = MS500;
+	auto_ip->min = BTEMP_THERMAL_LOW_LIMIT;
+	auto_ip->max = BTEMP_THERMAL_HIGH_LIMIT_62;
+	auto_ip->auto_adc_callback = ab5500_btemp_bat_temp_trig;
+	di->gpadc_auto = auto_ip;
+	ret = ab5500_gpadc_convert_auto(di->gpadc, di->gpadc_auto);
+	if (ret)
+		dev_err(di->dev,
+			"failed to set auto trigger for battery temp\n");
+	return ret;
+}
+
+#if defined(CONFIG_PM)
+static int ab5500_btemp_resume(struct platform_device *pdev)
+{
+	struct ab5500_btemp *di = platform_get_drvdata(pdev);
+
+	if (di->events.usb_conn)
+		ab5500_btemp_periodic(di, true);
+
+	return 0;
+}
+
+static int ab5500_btemp_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct ab5500_btemp *di = platform_get_drvdata(pdev);
+
+	if (di->events.usb_conn)
+		ab5500_btemp_periodic(di, false);
+
+	return 0;
+}
+#else
+#define ab5500_btemp_suspend      NULL
+#define ab5500_btemp_resume       NULL
+#endif
+
+static int __devexit ab5500_btemp_remove(struct platform_device *pdev)
+{
+	struct ab5500_btemp *di = platform_get_drvdata(pdev);
+	int i, irq;
+
+	/* Disable interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_btemp_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_btemp_irq[i].name);
+		free_irq(irq, di);
+	}
+
+	/* Delete the work queue */
+	destroy_workqueue(di->btemp_wq);
+
+	flush_scheduled_work();
+	power_supply_unregister(&di->btemp_psy);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di->gpadc_auto);
+	kfree(di);
+
+	return 0;
+}
+
+static int __devinit ab5500_btemp_probe(struct platform_device *pdev)
+{
+	int irq, i, ret = 0;
+	struct abx500_bm_plat_data *plat_data;
+
+	struct ab5500_btemp *di =
+		kzalloc(sizeof(struct ab5500_btemp), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	/* get parent data */
+	di->dev = &pdev->dev;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
+	di->gpadc = ab5500_gpadc_get("ab5500-adc.0");
+
+	plat_data = pdev->dev.platform_data;
+	di->pdata = plat_data->btemp;
+	di->bat = plat_data->battery;
+
+	/* get btemp specific platform data */
+	if (!di->pdata) {
+		dev_err(di->dev, "no btemp platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+
+	/* get battery specific platform data */
+	if (!di->bat) {
+		dev_err(di->dev, "no battery platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+
+	/* BTEMP supply */
+	di->btemp_psy.name = "ab5500_btemp";
+	di->btemp_psy.type = POWER_SUPPLY_TYPE_BATTERY;
+	di->btemp_psy.properties = ab5500_btemp_props;
+	di->btemp_psy.num_properties = ARRAY_SIZE(ab5500_btemp_props);
+	di->btemp_psy.get_property = ab5500_btemp_get_property;
+	di->btemp_psy.supplied_to = di->pdata->supplied_to;
+	di->btemp_psy.num_supplicants = di->pdata->num_supplicants;
+	di->btemp_psy.external_power_changed =
+		ab5500_btemp_external_power_changed;
+
+
+	/* Create a work queue for the btemp */
+	di->btemp_wq =
+		create_singlethread_workqueue("ab5500_btemp_wq");
+	if (di->btemp_wq == NULL) {
+		dev_err(di->dev, "failed to create work queue\n");
+		goto free_device_info;
+	}
+
+	/* Init work for measuring temperature periodically */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->btemp_periodic_work,
+		ab5500_btemp_periodic_work);
+
+	/* Get Chip ID of the ABB ASIC  */
+	ret = abx500_get_chip_id(di->dev);
+	if (ret < 0) {
+		dev_err(di->dev, "failed to get chip ID\n");
+		goto free_btemp_wq;
+	}
+	di->chip_id = ret;
+	dev_dbg(di->dev, "ab5500 CID is: 0x%02x\n",
+		di->chip_id);
+
+	/* Identify the battery */
+	if (ab5500_btemp_id(di) < 0)
+		dev_warn(di->dev, "failed to identify the battery\n");
+
+	/* Measure temperature once initially */
+	di->bat_temp = ab5500_btemp_measure_temp(di);
+	di->bat->temp_now = di->bat_temp;
+
+	/* Register BTEMP power supply class */
+	ret = power_supply_register(di->dev, &di->btemp_psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register BTEMP psy\n");
+		goto free_btemp_wq;
+	}
+
+	/* Register interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_btemp_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_btemp_irq[i].name);
+		ret = request_threaded_irq(irq, NULL, ab5500_btemp_irq[i].isr,
+			IRQF_SHARED | IRQF_NO_SUSPEND,
+			ab5500_btemp_irq[i].name, di);
+
+		if (ret) {
+			dev_err(di->dev, "failed to request %s IRQ %d: %d\n"
+				, ab5500_btemp_irq[i].name, irq, ret);
+			goto free_irq;
+		}
+		dev_dbg(di->dev, "Requested %s IRQ %d: %d\n",
+			ab5500_btemp_irq[i].name, irq, ret);
+	}
+	ret = ab5500_btemp_auto_temp(di);
+	if (ret) {
+		dev_err(di->dev,
+			"failed to register auto trigger for battery temp\n");
+		goto free_irq;
+	}
+
+	platform_set_drvdata(pdev, di);
+
+	dev_info(di->dev, "probe success\n");
+	return ret;
+
+free_irq:
+	power_supply_unregister(&di->btemp_psy);
+
+	/* We also have to free all successfully registered irqs */
+	for (i = i - 1; i >= 0; i--) {
+		irq = platform_get_irq_byname(pdev, ab5500_btemp_irq[i].name);
+		free_irq(irq, di);
+	}
+free_btemp_wq:
+	destroy_workqueue(di->btemp_wq);
+free_device_info:
+	kfree(di);
+
+	return ret;
+}
+
+static struct platform_driver ab5500_btemp_driver = {
+	.probe = ab5500_btemp_probe,
+	.remove = __devexit_p(ab5500_btemp_remove),
+	.suspend = ab5500_btemp_suspend,
+	.resume = ab5500_btemp_resume,
+	.driver = {
+		.name = "ab5500-btemp",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab5500_btemp_init(void)
+{
+	return platform_driver_register(&ab5500_btemp_driver);
+}
+
+static void __exit ab5500_btemp_exit(void)
+{
+	platform_driver_unregister(&ab5500_btemp_driver);
+}
+
+subsys_initcall_sync(ab5500_btemp_init);
+module_exit(ab5500_btemp_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Johan Palsson, Karl Komierowski");
+MODULE_ALIAS("platform:ab5500-btemp");
+MODULE_DESCRIPTION("AB5500 battery temperature driver");
diff --git a/drivers/power/ab5500_charger.c b/drivers/power/ab5500_charger.c
new file mode 100644
index 00000000000..2fd7888ad9d
--- /dev/null
+++ b/drivers/power/ab5500_charger.c
@@ -0,0 +1,1836 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Charger driver for AB5500
+ *
+ * License Terms: GNU General Public License v2
+ * Authors:
+ *	Johan Palsson <johan.palsson@stericsson.com>
+ *	Karl Komierowski <karl.komierowski@stericsson.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/completion.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500-bm.h>
+#include <linux/mfd/abx500/ab5500-gpadc.h>
+#include <linux/mfd/abx500/ux500_chargalg.h>
+
+/* Charger constants */
+#define NO_PW_CONN			0
+#define USB_PW_CONN			2
+
+/* HW failure constants */
+#define VBUS_CH_NOK			0x0A
+#define VBUS_OVV_TH			0x06
+
+/* AB5500 Charger constants */
+#define AB5500_USB_LINK_STATUS		0x78
+#define USB_CHARG_DET_ENA_MASK		0x01
+#define USB_CHARG_DET_ENA		0x01
+#define CHARGER_REV_SUP			0x10
+#define SW_EOC				0x40
+#define USB_CHAR_DET			0x02
+#define USB_CHAR_DET_DONE		0x02
+#define VBUS_RISING			0x20
+#define USB_LINK_UPDATE			0x02
+#define USB_CH_TH_PROT_LOW		0x02
+#define USB_CH_TH_PROT_HIGH		0x01
+#define USB_ID_HOST_DET_ENA_MASK	0x02
+#define USB_ID_HOST_DET_ENA		0x02
+#define USB_ID_DEVICE_DET_ENA_MASK	0x01
+#define USB_ID_DEVICE_DET_ENA		0x01
+#define CHARGER_ISET_IN_1_1A		0x0C
+#define LED_ENABLE			0x01
+#define RESET				0x00
+#define SSW_ENABLE_REBOOT		0x80
+#define SSW_REBOOT_EN			0x40
+#define SSW_CONTROL_AUTOC		0x04
+#define SSW_PSEL_480S			0x00
+
+/* UsbLineStatus register - usb types */
+enum ab5500_charger_link_status {
+	USB_STAT_NOT_CONFIGURED,
+	USB_STAT_STD_HOST_NC,
+	USB_STAT_STD_HOST_C_NS,
+	USB_STAT_STD_HOST_C_S,
+	USB_STAT_HOST_CHG_NM,
+	USB_STAT_HOST_CHG_HS,
+	USB_STAT_HOST_CHG_HS_CHIRP,
+	USB_STAT_DEDICATED_CHG,
+	USB_STAT_ACA_RID_A,
+	USB_STAT_ACA_RID_B,
+	USB_STAT_ACA_RID_C_NM,
+	USB_STAT_ACA_RID_C_HS,
+	USB_STAT_ACA_RID_C_HS_CHIRP,
+	USB_STAT_HM_IDGND,
+	USB_STAT_RESERVED,
+	USB_STAT_NOT_VALID_LINK,
+};
+
+enum ab5500_usb_state {
+	AB5500_BM_USB_STATE_RESET_HS,	/* HighSpeed Reset */
+	AB5500_BM_USB_STATE_RESET_FS,	/* FullSpeed/LowSpeed Reset */
+	AB5500_BM_USB_STATE_CONFIGURED,
+	AB5500_BM_USB_STATE_SUSPEND,
+	AB5500_BM_USB_STATE_RESUME,
+	AB5500_BM_USB_STATE_MAX,
+};
+
+/* VBUS input current limits supported in AB5500 in mA */
+#define USB_CH_IP_CUR_LVL_0P05		50
+#define USB_CH_IP_CUR_LVL_0P09		98
+#define USB_CH_IP_CUR_LVL_0P19		193
+#define USB_CH_IP_CUR_LVL_0P29		290
+#define USB_CH_IP_CUR_LVL_0P38		380
+#define USB_CH_IP_CUR_LVL_0P45		450
+#define USB_CH_IP_CUR_LVL_0P5		500
+#define USB_CH_IP_CUR_LVL_0P6		600
+#define USB_CH_IP_CUR_LVL_0P7		700
+#define USB_CH_IP_CUR_LVL_0P8		800
+#define USB_CH_IP_CUR_LVL_0P9		900
+#define USB_CH_IP_CUR_LVL_1P0		1000
+#define USB_CH_IP_CUR_LVL_1P1		1100
+#define USB_CH_IP_CUR_LVL_1P3		1300
+#define USB_CH_IP_CUR_LVL_1P4		1400
+#define USB_CH_IP_CUR_LVL_1P5		1500
+
+#define to_ab5500_charger_usb_device_info(x) container_of((x), \
+	struct ab5500_charger, usb_chg)
+
+/**
+ * struct ab5500_charger_interrupts - ab5500 interupts
+ * @name:	name of the interrupt
+ * @isr		function pointer to the isr
+ */
+struct ab5500_charger_interrupts {
+	char *name;
+	irqreturn_t (*isr)(int irq, void *data);
+};
+
+struct ab5500_charger_info {
+	int charger_connected;
+	int charger_online;
+	int charger_voltage;
+	int cv_active;
+	bool wd_expired;
+};
+
+struct ab5500_charger_event_flags {
+	bool usb_thermal_prot;
+	bool vbus_ovv;
+	bool usbchargernotok;
+	bool vbus_collapse;
+};
+
+struct ab5500_charger_usb_state {
+	bool usb_changed;
+	int usb_current;
+	enum ab5500_usb_state state;
+	spinlock_t usb_lock;
+};
+
+/**
+ * struct ab5500_charger - ab5500 Charger device information
+ * @dev:		Pointer to the structure device
+ * @chip_id:		Chip-Id of the ab5500
+ * @max_usb_in_curr:	Max USB charger input current
+ * @vbus_detected:	VBUS detected
+ * @vbus_detected_start:
+ *			VBUS detected during startup
+ * @parent:		Pointer to the struct ab5500
+ * @gpadc:		Pointer to the struct gpadc
+ * @pdata:		Pointer to the ab5500_charger platform data
+ * @bat:		Pointer to the ab5500_bm platform data
+ * @flags:		Structure for information about events triggered
+ * @usb_state:		Structure for usb stack information
+ * @usb_chg:		USB charger power supply
+ * @ac:			Structure that holds the AC charger properties
+ * @usb:		Structure that holds the USB charger properties
+ * @charger_wq:		Work queue for the IRQs and checking HW state
+ * @check_hw_failure_work:	Work for checking HW state
+ * @check_usbchgnotok_work:	Work for checking USB charger not ok status
+ * @ac_work:			Work for checking AC charger connection
+ * @detect_usb_type_work:	Work for detecting the USB type connected
+ * @usb_link_status_work:	Work for checking the new USB link status
+ * @usb_state_changed_work:	Work for checking USB state
+ * @check_main_thermal_prot_work:
+ *				Work for checking Main thermal status
+ * @check_usb_thermal_prot_work:
+ *				Work for checking USB thermal status
+ */
+struct ab5500_charger {
+	struct device *dev;
+	u8 chip_id;
+	int max_usb_in_curr;
+	bool vbus_detected;
+	bool vbus_detected_start;
+	struct ab5500 *parent;
+	struct ab5500_gpadc *gpadc;
+	struct abx500_charger_platform_data *pdata;
+	struct abx500_bm_data *bat;
+	struct ab5500_charger_event_flags flags;
+	struct ab5500_charger_usb_state usb_state;
+	struct ux500_charger usb_chg;
+	struct ab5500_charger_info usb;
+	struct workqueue_struct *charger_wq;
+	struct delayed_work check_hw_failure_work;
+	struct delayed_work check_usbchgnotok_work;
+	struct work_struct detect_usb_type_work;
+	struct work_struct usb_link_status_work;
+	struct work_struct usb_state_changed_work;
+	struct work_struct check_usb_thermal_prot_work;
+};
+
+/*
+ * TODO: This variable is static in order to get information
+ * about maximum current and USB state from the USB driver
+ * This should be solved in a better way
+ */
+static struct ab5500_charger *static_di;
+
+/* USB properties */
+static enum power_supply_property ab5500_charger_usb_props[] = {
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_CURRENT_AVG,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+};
+
+/**
+ * ab5500_charger_get_vbus_voltage() - get vbus voltage
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * This function returns the vbus voltage.
+ * Returns vbus voltage (on success)
+ */
+static int ab5500_charger_get_vbus_voltage(struct ab5500_charger *di)
+{
+	int vch;
+
+	/* Only measure voltage if the charger is connected */
+	if (di->usb.charger_connected) {
+		vch = ab5500_gpadc_convert(di->gpadc, VBUS_V);
+		if (vch < 0)
+			dev_err(di->dev, "%s gpadc conv failed\n", __func__);
+	} else {
+		vch = 0;
+	}
+	return vch;
+}
+
+/**
+ * ab5500_charger_get_usb_current() - get usb charger current
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * This function returns the usb charger current.
+ * Returns usb current (on success) and error code on failure
+ */
+static int ab5500_charger_get_usb_current(struct ab5500_charger *di)
+{
+	int ich;
+
+	/* Only measure current if the charger is online */
+	if (di->usb.charger_online) {
+		ich = ab5500_gpadc_convert(di->gpadc, USB_CHARGER_C);
+		if (ich < 0)
+			dev_err(di->dev, "%s gpadc conv failed\n", __func__);
+	} else {
+		ich = 0;
+	}
+	return ich;
+}
+
+/**
+ * ab5500_charger_detect_chargers() - Detect the connected chargers
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * Returns the type of charger connected.
+ * For USB it will not mean we can actually charge from it
+ * but that there is a USB cable connected that we have to
+ * identify. This is used during startup when we don't get
+ * interrupts of the charger detection
+ *
+ * Returns an integer value, that means,
+ * NO_PW_CONN  no power supply is connected
+ * USB_PW_CONN  if the USB power supply is connected
+ */
+static int ab5500_charger_detect_chargers(struct ab5500_charger *di)
+{
+	int result = NO_PW_CONN;
+	int ret;
+	u8 val;
+	/* Check for USB charger */
+	/*
+	 * TODO: Since there are no status register validating by
+	 * reading the IT souce registers
+	 */
+	ret = abx500_get_register_interruptible(di->dev, AB5500_BANK_IT,
+		AB5500_IT_SOURCE8, &val);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+		return ret;
+	}
+
+	if (val & VBUS_RISING)
+		result |= USB_PW_CONN;
+
+	ret = abx500_get_register_interruptible(di->dev, AB5500_BANK_IT,
+		AB5500_IT_SOURCE9, &val);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+		return ret;
+	}
+
+	if (val & USB_CHAR_DET_DONE)
+		result |= USB_PW_CONN;
+
+	return result;
+}
+
+/**
+ * ab5500_charger_max_usb_curr() - get the max curr for the USB type
+ * @di:			pointer to the ab5500_charger structure
+ * @link_status:	the identified USB type
+ *
+ * Get the maximum current that is allowed to be drawn from the host
+ * based on the USB type.
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab5500_charger_max_usb_curr(struct ab5500_charger *di,
+	enum ab5500_charger_link_status link_status)
+{
+	int ret = 0;
+
+	switch (link_status) {
+	case USB_STAT_STD_HOST_NC:
+	case USB_STAT_STD_HOST_C_NS:
+	case USB_STAT_STD_HOST_C_S:
+		dev_dbg(di->dev, "USB Type - Standard host is "
+			"detected through USB driver\n");
+		ret = -1;
+		break;
+	case USB_STAT_HOST_CHG_HS_CHIRP:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
+		break;
+	case USB_STAT_HOST_CHG_HS:
+	case USB_STAT_ACA_RID_C_HS:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P9;
+		break;
+	case USB_STAT_ACA_RID_A:
+		/*
+		 * Dedicated charger level minus maximum current accessory
+		 * can consume (300mA). Closest level is 1100mA
+		 */
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P1;
+		break;
+	case USB_STAT_ACA_RID_B:
+		/*
+		 * Dedicated charger level minus 120mA (20mA for ACA and
+		 * 100mA for potential accessory). Closest level is 1300mA
+		 */
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P3;
+		break;
+	case USB_STAT_DEDICATED_CHG:
+	case USB_STAT_HOST_CHG_NM:
+	case USB_STAT_ACA_RID_C_HS_CHIRP:
+	case USB_STAT_ACA_RID_C_NM:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P5;
+		break;
+	case USB_STAT_RESERVED:
+		/*
+		 * This state is used to indicate that VBUS has dropped below
+		 * the detection level 4 times in a row. This is due to the
+		 * charger output current is set to high making the charger
+		 * voltage collapse. This have to be propagated through to
+		 * chargalg. This is done using the property
+		 * POWER_SUPPLY_PROP_CURRENT_AVG = 1
+		 */
+		di->flags.vbus_collapse = true;
+		dev_dbg(di->dev, "USB Type - USB_STAT_RESERVED "
+			"VBUS has collapsed\n");
+		ret = -1;
+		break;
+	case USB_STAT_HM_IDGND:
+	case USB_STAT_NOT_CONFIGURED:
+	case USB_STAT_NOT_VALID_LINK:
+		dev_err(di->dev, "USB Type - Charging not allowed\n");
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		ret = -ENXIO;
+		break;
+	default:
+		dev_err(di->dev, "USB Type - Unknown\n");
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		ret = -ENXIO;
+		break;
+	};
+
+	dev_dbg(di->dev, "USB Type - 0x%02x MaxCurr: %d",
+		link_status, di->max_usb_in_curr);
+
+	return ret;
+}
+
+/**
+ * ab5500_charger_read_usb_type() - read the type of usb connected
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * Detect the type of the plugged USB
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab5500_charger_read_usb_type(struct ab5500_charger *di)
+{
+	int ret;
+	u8 val;
+
+	ret = abx500_get_register_interruptible(di->dev,
+		AB5500_BANK_IT, AB5500_IT_SOURCE22_REG, &val);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+		return ret;
+	}
+	ret = abx500_get_register_interruptible(di->dev, AB5500_BANK_USB,
+		AB5500_USB_LINE_STATUS, &val);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+		return ret;
+	}
+
+	/* get the USB type */
+	val = (val & AB5500_USB_LINK_STATUS) >> 3;
+	ret = ab5500_charger_max_usb_curr(di,
+		(enum ab5500_charger_link_status) val);
+
+	return ret;
+}
+
+/**
+ * ab5500_charger_detect_usb_type() - get the type of usb connected
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * Detect the type of the plugged USB
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab5500_charger_detect_usb_type(struct ab5500_charger *di)
+{
+	int i, ret;
+	u8 val;
+
+	/*
+	 * On getting the VBUS rising edge detect interrupt there
+	 * is a 250ms delay after which the register UsbLineStatus
+	 * is filled with valid data.
+	 */
+	for (i = 0; i < 10; i++) {
+		msleep(250);
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_IT, AB5500_IT_SOURCE22_REG,
+			&val);
+		if (ret < 0) {
+			dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+			return ret;
+		}
+		if (!(val & USB_LINK_UPDATE))
+			continue;
+
+		ret = abx500_get_register_interruptible(di->dev,
+				AB5500_BANK_USB, AB5500_USB_LINE_STATUS, &val);
+		if (ret < 0) {
+			dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+			return ret;
+		}
+		/*
+		 * Until the IT source register is read the UsbLineStatus
+		 * register is not updated, hence doing the same
+		 * Revisit this:
+		 */
+
+		/* get the USB type */
+		val = (val & AB5500_USB_LINK_STATUS) >> 3;
+		if (val) {
+			dev_dbg(di->dev, "values = %d\n", val);
+			break;
+		}
+	}
+	ret = ab5500_charger_max_usb_curr(di,
+		(enum ab5500_charger_link_status) val);
+
+	return ret;
+}
+
+static int ab5500_charger_voltage_map[] = {
+	3500 ,
+	3525 ,
+	3550 ,
+	3575 ,
+	3600 ,
+	3625 ,
+	3650 ,
+	3675 ,
+	3700 ,
+	3725 ,
+	3750 ,
+	3775 ,
+	3800 ,
+	3825 ,
+	3850 ,
+	3875 ,
+	3900 ,
+	3925 ,
+	3950 ,
+	3975 ,
+	4000 ,
+	4025 ,
+	4050 ,
+	4060 ,
+	4070 ,
+	4080 ,
+	4090 ,
+	4100 ,
+	4110 ,
+	4120 ,
+	4130 ,
+	4140 ,
+	4150 ,
+	4160 ,
+	4170 ,
+	4180 ,
+	4190 ,
+	4200 ,
+	4210 ,
+	4220 ,
+	4230 ,
+	4240 ,
+	4250 ,
+	4260 ,
+	4270 ,
+	4280 ,
+	4290 ,
+	4300 ,
+	4310 ,
+	4320 ,
+	4330 ,
+	4340 ,
+	4350 ,
+	4360 ,
+	4370 ,
+	4380 ,
+	4390 ,
+	4400 ,
+	4410 ,
+	4420 ,
+	4430 ,
+	4440 ,
+	4450 ,
+	4460 ,
+	4470 ,
+	4480 ,
+	4490 ,
+	4500 ,
+	4510 ,
+	4520 ,
+	4530 ,
+	4540 ,
+	4550 ,
+	4560 ,
+	4570 ,
+	4580 ,
+	4590 ,
+	4600 ,
+};
+
+/*
+ * This array maps the raw hex value to charger current used by the ab5500
+ * Values taken from the AB5500 product specification manual
+ */
+static int ab5500_charger_current_map[] = {
+	100 ,
+	200 ,
+	300 ,
+	400 ,
+	500 ,
+	600 ,
+	700 ,
+	800 ,
+	900 ,
+	1000,
+	1100,
+	1200,
+	1300,
+	1400,
+	1500,
+	1500,
+};
+
+static int ab5500_icsr_current_map[] = {
+	50,
+	93,
+	193,
+	290,
+	380,
+	450,
+	500 ,
+	600 ,
+	700 ,
+	800 ,
+	900 ,
+	1000,
+	1100,
+	1300,
+	1400,
+	1500,
+};
+
+static int ab5500_cvrec_voltage_map[] = {
+	3300,
+	3325,
+	3350,
+	3375,
+	3400,
+	3425,
+	3450,
+	3475,
+	3500,
+	3525,
+	3550,
+	3575,
+	3600,
+	3625,
+	3650,
+	3675,
+	3700,
+	3725,
+	3750,
+	3775,
+	3800,
+	3825,
+	3850,
+	3875,
+	3900,
+	3925,
+	4000,
+	4025,
+	4050,
+	4075,
+	4100,
+	4125,
+	4150,
+	4175,
+	4200,
+	4225,
+	4250,
+	4275,
+	4300,
+	4325,
+	4350,
+	4375,
+	4400,
+	4425,
+	4450,
+	4475,
+	4500,
+	4525,
+	4550,
+	4575,
+	4600,
+};
+
+static int ab5500_cvrec_voltage_to_regval(int voltage)
+{
+	int i;
+
+	/* Special case for voltage below 3.3V */
+	if (voltage < ab5500_cvrec_voltage_map[0])
+		return 0;
+
+	for (i = 1; i < ARRAY_SIZE(ab5500_cvrec_voltage_map); i++) {
+		if (voltage < ab5500_cvrec_voltage_map[i])
+			return i - 1;
+	}
+
+	/* If not last element, return error */
+	i = ARRAY_SIZE(ab5500_cvrec_voltage_map) - 1;
+	if (voltage == ab5500_cvrec_voltage_map[i])
+		return i;
+	else
+		return -1;
+}
+
+static int ab5500_voltage_to_regval(int voltage)
+{
+	int i;
+
+	/* Special case for voltage below 3.3V */
+	if (voltage < ab5500_charger_voltage_map[0])
+		return 0;
+
+	for (i = 1; i < ARRAY_SIZE(ab5500_charger_voltage_map); i++) {
+		if (voltage < ab5500_charger_voltage_map[i])
+			return i - 1;
+	}
+
+	/* If not last element, return error */
+	i = ARRAY_SIZE(ab5500_charger_voltage_map) - 1;
+	if (voltage == ab5500_charger_voltage_map[i])
+		return i;
+	else
+		return -1;
+}
+
+static int ab5500_icsr_curr_to_regval(int curr)
+{
+	int i;
+
+	if (curr < ab5500_icsr_current_map[0])
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(ab5500_icsr_current_map); i++) {
+		if (curr < ab5500_icsr_current_map[i])
+			return i - 1;
+	}
+
+	/* If not last element, return error */
+	i = ARRAY_SIZE(ab5500_icsr_current_map) - 1;
+	if (curr == ab5500_icsr_current_map[i])
+		return i;
+	else
+		return -1;
+}
+
+static int ab5500_current_to_regval(int curr)
+{
+	int i;
+
+	if (curr < ab5500_charger_current_map[0])
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(ab5500_charger_current_map); i++) {
+		if (curr < ab5500_charger_current_map[i])
+			return i - 1;
+	}
+
+	/* If not last element, return error */
+	i = ARRAY_SIZE(ab5500_charger_current_map) - 1;
+	if (curr == ab5500_charger_current_map[i])
+		return i;
+	else
+		return -1;
+}
+
+/**
+ * ab5500_charger_get_usb_cur() - get usb current
+ * @di:		pointer to the ab5500_charger structre
+ *
+ * The usb stack provides the maximum current that can be drawn from
+ * the standard usb host. This will be in mA.
+ * This function converts current in mA to a value that can be written
+ * to the register. Returns -1 if charging is not allowed
+ */
+static int ab5500_charger_get_usb_cur(struct ab5500_charger *di)
+{
+	switch (di->usb_state.usb_current) {
+	case 50:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		break;
+	case 100:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P09;
+		break;
+	case 200:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P19;
+		break;
+	case 300:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P29;
+		break;
+	case 400:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P38;
+		break;
+	case 500:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
+		break;
+	default:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		return -1;
+		break;
+	};
+	return 0;
+}
+
+/**
+ * ab5500_charger_set_vbus_in_curr() - set VBUS input current limit
+ * @di:		pointer to the ab5500_charger structure
+ * @ich_in:	charger input current limit
+ *
+ * Sets the current that can be drawn from the USB host
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_charger_set_vbus_in_curr(struct ab5500_charger *di,
+		int ich_in)
+{
+	int ret;
+	int input_curr_index;
+	int min_value;
+
+	/* We should always use to lowest current limit */
+	min_value = min(di->bat->chg_params->usb_curr_max, ich_in);
+
+	input_curr_index = ab5500_icsr_curr_to_regval(min_value);
+	if (input_curr_index < 0) {
+		dev_err(di->dev, "VBUS input current limit too high\n");
+		return -ENXIO;
+	}
+
+	ret = abx500_set_register_interruptible(di->dev, AB5500_BANK_CHG,
+		AB5500_ICSR, input_curr_index);
+	if (ret)
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+
+	return ret;
+}
+
+/**
+ * ab5500_charger_usb_en() - enable usb charging
+ * @di:		pointer to the ab5500_charger structure
+ * @enable:	enable/disable flag
+ * @vset:	charging voltage
+ * @ich_out:	charger output current
+ *
+ * Enable/Disable USB charging and turns on/off the charging led respectively.
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_charger_usb_en(struct ux500_charger *charger,
+	int enable, int vset, int ich_out)
+{
+	int ret;
+	int volt_index;
+	int curr_index;
+
+	struct ab5500_charger *di = to_ab5500_charger_usb_device_info(charger);
+
+	if (enable) {
+		/* Check if USB is connected */
+		if (!di->usb.charger_connected) {
+			dev_err(di->dev, "USB charger not connected\n");
+			return -ENXIO;
+		}
+
+		/* Enable USB charging */
+		dev_dbg(di->dev, "Enable USB: %dmV %dmA\n", vset, ich_out);
+
+		volt_index = ab5500_voltage_to_regval(vset);
+		curr_index = ab5500_current_to_regval(ich_out) ;
+
+		/* ChVoltLevel: max voltage upto which battery can be charged */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_VSRC, (u8) volt_index);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+
+		/* current that can be drawn from the usb */
+		ret = ab5500_charger_set_vbus_in_curr(di, ich_out);
+		if (ret) {
+			dev_err(di->dev, "%s setting icsr failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+
+		/* ChOutputCurentLevel: protected output current */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_OCSRV, (u8) curr_index);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+
+		/*
+		 * Battery voltage when charging should be resumed after
+		 * completion of charging
+		 */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_CVREC,
+				ab5500_cvrec_voltage_to_regval(
+			di->bat->bat_type[di->bat->batt_id].recharge_vol));
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+		/*
+		 * Battery temperature:
+		 * Input to the TBDATA register corresponds to the battery
+		 * temperature(temp being multiples of 2)
+		 * In order to obatain the value to be written to this reg
+		 * divide the temperature obtained from gpadc by 2
+		 */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_TBDATA,
+				di->bat->temp_now / 2);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+
+		/* If success power on charging LED indication */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_LEDT, LED_ENABLE);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+		di->usb.charger_online = 1;
+	} else {
+		/* ChVoltLevel: max voltage upto which battery can be charged */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_VSRC, RESET);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+		/* USBChInputCurr: current that can be drawn from the usb */
+		ret = ab5500_charger_set_vbus_in_curr(di, RESET);
+		if (ret) {
+			dev_err(di->dev, "%s resetting icsr failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+		/* If success power off charging LED indication */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_LEDT, RESET);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+		di->usb.charger_online = 0;
+		di->usb.wd_expired = false;
+		dev_dbg(di->dev, "%s Disabled USB charging\n", __func__);
+	}
+	power_supply_changed(&di->usb_chg.psy);
+
+	return ret;
+}
+
+/**
+ * ab5500_charger_watchdog_kick() - kick charger watchdog
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * Kick charger watchdog
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_charger_watchdog_kick(struct ux500_charger *charger)
+{
+	int ret;
+	struct ab5500_charger *di;
+	int volt_index, curr_index;
+	u8 value = 0;
+
+	/* TODO: update */
+	if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+		di = to_ab5500_charger_usb_device_info(charger);
+	else
+		return -ENXIO;
+
+	ret = abx500_get_register_interruptible(di->dev, AB5500_BANK_STARTUP,
+		AB5500_MCB, &value);
+	if (ret)
+		dev_err(di->dev, "Failed to read!\n");
+
+	value = value | (SSW_ENABLE_REBOOT | SSW_REBOOT_EN |
+			SSW_CONTROL_AUTOC | SSW_PSEL_480S);
+	ret = abx500_set_register_interruptible(di->dev, AB5500_BANK_STARTUP,
+		AB5500_MCB, value);
+	if (ret)
+		dev_err(di->dev, "Failed to kick WD!\n");
+
+	volt_index = ab5500_voltage_to_regval(
+			di->bat->bat_type[di->bat->batt_id].normal_vol_lvl);
+	curr_index = ab5500_current_to_regval(di->max_usb_in_curr);
+
+	/* ChVoltLevel: max voltage upto which battery can be charged */
+	ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_VSRC, (u8) volt_index);
+	if (ret) {
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+		return ret;
+	}
+
+	/* current that can be drawn from the usb */
+	ret = ab5500_charger_set_vbus_in_curr(di, di->max_usb_in_curr);
+	if (ret) {
+		dev_err(di->dev, "%s setting icsr failed %d\n",
+				__func__, __LINE__);
+		return ret;
+	}
+
+	/* ChOutputCurentLevel: protected output current */
+	ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_OCSRV, (u8) curr_index);
+	if (ret) {
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+		return ret;
+	}
+
+	/*
+	 * Battery voltage when charging should be resumed after
+	 * completion of charging
+	 */
+	/* Charger_Vrechar[5:0] = '4.025 V' */
+	ret = abx500_set_register_interruptible(di->dev,
+			AB5500_BANK_CHG, AB5500_CVREC,
+			ab5500_cvrec_voltage_to_regval(
+			di->bat->bat_type[di->bat->batt_id].recharge_vol));
+	if (ret) {
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+		return ret;
+	}
+	/*
+	 * Battery temperature:
+	 * Input to the TBDATA register corresponds to the battery
+	 * temperature(temp being multiples of 2)
+	 * In order to obatain the value to be written to this reg
+	 * divide the temperature obtained from gpadc by 2
+	 */
+	ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_TBDATA,
+				di->bat->temp_now / 2);
+	if (ret) {
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+		return ret;
+	}
+	return ret;
+}
+
+/**
+ * ab5500_charger_update_charger_current() - update charger current
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * Update the charger output current for the specified charger
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_charger_update_charger_current(struct ux500_charger *charger,
+		int ich_out)
+{
+	int ret = 0;
+	int curr_index;
+	struct ab5500_charger *di;
+
+	if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+		di = to_ab5500_charger_usb_device_info(charger);
+	else
+		return -ENXIO;
+
+	curr_index = ab5500_current_to_regval(ich_out);
+	if (curr_index < 0) {
+		dev_err(di->dev,
+			"Charger current too high, "
+			"charging not started\n");
+		return -ENXIO;
+	}
+
+	ret = abx500_set_register_interruptible(di->dev, AB5500_BANK_CHG,
+		AB5500_OCSRV, (u8) curr_index);
+	if (ret) {
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+		return ret;
+	}
+
+	return ret;
+}
+
+/**
+ * ab5500_charger_check_hw_failure_work() - check main charger failure
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the main charger status
+ */
+static void ab5500_charger_check_hw_failure_work(struct work_struct *work)
+{
+	int ret;
+	u8 reg_value;
+
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, check_hw_failure_work.work);
+
+	/* Check if the status bits for HW failure is still active */
+	if (di->flags.vbus_ovv) {
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_USB, AB5500_USB_PHY_STATUS,
+			&reg_value);
+		if (ret < 0) {
+			dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+			return;
+		}
+		if (!(reg_value & VBUS_OVV_TH)) {
+			di->flags.vbus_ovv = false;
+			power_supply_changed(&di->usb_chg.psy);
+		}
+	}
+	/* If we still have a failure, schedule a new check */
+	if (di->flags.vbus_ovv) {
+		queue_delayed_work(di->charger_wq,
+			&di->check_hw_failure_work, round_jiffies(HZ));
+	}
+}
+
+/**
+ * ab5500_charger_detect_usb_type_work() - work to detect USB type
+ * @work:	Pointer to the work_struct structure
+ *
+ * Detect the type of USB plugged
+ */
+void ab5500_charger_detect_usb_type_work(struct work_struct *work)
+{
+	int ret;
+
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, detect_usb_type_work);
+
+	/*
+	 * Since we can't be sure that the events are received
+	 * synchronously, we have the check if is
+	 * connected by reading the status register
+	 */
+	ret = ab5500_charger_detect_chargers(di);
+	if (ret < 0)
+		return;
+
+	if (!(ret & USB_PW_CONN)) {
+		di->vbus_detected = 0;
+		di->usb.charger_connected = 0;
+		power_supply_changed(&di->usb_chg.psy);
+	} else {
+		di->vbus_detected = 1;
+
+		ret = ab5500_charger_detect_usb_type(di);
+		if (!ret) {
+			di->usb.charger_connected = 1;
+			power_supply_changed(&di->usb_chg.psy);
+		}
+
+	}
+}
+
+/**
+ * ab5500_charger_usb_link_status_work() - work to detect USB type
+ * @work:	pointer to the work_struct structure
+ *
+ * Detect the type of USB plugged
+ */
+static void ab5500_charger_usb_link_status_work(struct work_struct *work)
+{
+	int ret;
+
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, usb_link_status_work);
+
+	/*
+	 * Since we can't be sure that the events are received
+	 * synchronously, we have the check if  is
+	 * connected by reading the status register
+	 */
+	ret = ab5500_charger_detect_chargers(di);
+	if (ret < 0)
+		return;
+
+	if (!(ret & USB_PW_CONN)) {
+		di->vbus_detected = 0;
+		di->usb.charger_connected = 0;
+		power_supply_changed(&di->usb_chg.psy);
+	} else {
+		di->vbus_detected = 1;
+		ret = ab5500_charger_read_usb_type(di);
+		if (!ret) {
+			/* Update maximum input current */
+			ret = ab5500_charger_set_vbus_in_curr(di,
+					di->max_usb_in_curr);
+			if (ret)
+				return;
+
+			di->usb.charger_connected = 1;
+			power_supply_changed(&di->usb_chg.psy);
+		} else if (ret == -ENXIO) {
+			/* No valid charger type detected */
+			di->usb.charger_connected = 0;
+			power_supply_changed(&di->usb_chg.psy);
+		}
+	}
+}
+
+static void ab5500_charger_usb_state_changed_work(struct work_struct *work)
+{
+	int ret;
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, usb_state_changed_work);
+
+	if (!di->vbus_detected)
+		return;
+
+	spin_lock(&di->usb_state.usb_lock);
+	di->usb_state.usb_changed = false;
+	spin_unlock(&di->usb_state.usb_lock);
+
+	/*
+	 * wait for some time until you get updates from the usb stack
+	 * and negotiations are completed
+	 */
+	msleep(250);
+
+	if (di->usb_state.usb_changed)
+		return;
+
+	dev_dbg(di->dev, "%s USB state: 0x%02x mA: %d\n",
+		__func__, di->usb_state.state, di->usb_state.usb_current);
+
+	switch (di->usb_state.state) {
+	case AB5500_BM_USB_STATE_RESET_HS:
+	case AB5500_BM_USB_STATE_RESET_FS:
+	case AB5500_BM_USB_STATE_SUSPEND:
+	case AB5500_BM_USB_STATE_MAX:
+		di->usb.charger_connected = 0;
+		power_supply_changed(&di->usb_chg.psy);
+		break;
+
+	case AB5500_BM_USB_STATE_RESUME:
+		/*
+		 * when suspend->resume there should be delay
+		 * of 1sec for enabling charging
+		 */
+		msleep(1000);
+		/* Intentional fall through */
+	case AB5500_BM_USB_STATE_CONFIGURED:
+		/*
+		 * USB is configured, enable charging with the charging
+		 * input current obtained from USB driver
+		 */
+		if (!ab5500_charger_get_usb_cur(di)) {
+			/* Update maximum input current */
+			ret = ab5500_charger_set_vbus_in_curr(di,
+					di->max_usb_in_curr);
+			if (ret)
+				return;
+
+			di->usb.charger_connected = 1;
+			power_supply_changed(&di->usb_chg.psy);
+		}
+		break;
+
+	default:
+		break;
+	};
+}
+
+/**
+ * ab5500_charger_check_usbchargernotok_work() - check USB chg not ok status
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the USB charger Not OK status
+ */
+static void ab5500_charger_check_usbchargernotok_work(struct work_struct *work)
+{
+	int ret;
+	u8 reg_value;
+	bool prev_status;
+
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, check_usbchgnotok_work.work);
+
+	/* Check if the status bit for usbchargernotok is still active */
+	ret = abx500_get_register_interruptible(di->dev,
+		AB5500_BANK_USB, AB5500_CHGFSM_CHARGER_DETECT, &reg_value);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+		return;
+	}
+	prev_status = di->flags.usbchargernotok;
+
+	if (reg_value & VBUS_CH_NOK) {
+		di->flags.usbchargernotok = true;
+		/* Check again in 1sec */
+		queue_delayed_work(di->charger_wq,
+			&di->check_usbchgnotok_work, HZ);
+	} else {
+		di->flags.usbchargernotok = false;
+		di->flags.vbus_collapse = false;
+	}
+
+	if (prev_status != di->flags.usbchargernotok)
+		power_supply_changed(&di->usb_chg.psy);
+}
+
+/**
+ * ab5500_charger_check_usb_thermal_prot_work() - check usb thermal status
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the USB thermal prot status
+ */
+static void ab5500_charger_check_usb_thermal_prot_work(
+	struct work_struct *work)
+{
+	int ret;
+	u8 reg_value;
+
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, check_usb_thermal_prot_work);
+
+	/* Check if the status bit for usb_thermal_prot is still active */
+	/* TODO: Interrupt source reg 15 bit 4 */
+	ret = abx500_get_register_interruptible(di->dev,
+		AB5500_BANK_USB, AB5500_CHGFSM_USB_BTEMP_CURR_LIM, &reg_value);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+		return;
+	}
+	if (reg_value & USB_CH_TH_PROT_LOW || reg_value & USB_CH_TH_PROT_HIGH)
+		di->flags.usb_thermal_prot = true;
+	else
+		di->flags.usb_thermal_prot = false;
+
+	power_supply_changed(&di->usb_chg.psy);
+}
+
+/**
+ * ab5500_charger_vbusdetf_handler() - VBUS falling detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_vbusdetf_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev, "VBUS falling detected\n");
+	queue_work(di->charger_wq, &di->detect_usb_type_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_vbusdetr_handler() - VBUS rising detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_vbusdetr_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	di->vbus_detected = true;
+	dev_dbg(di->dev, "VBUS rising detected\n");
+	queue_work(di->charger_wq, &di->detect_usb_type_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_usblinkstatus_handler() - USB link status has changed
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_usblinkstatus_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev, "USB link status changed\n");
+
+	queue_work(di->charger_wq, &di->usb_link_status_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_usbchthprotr_handler() - Die temp is above usb charger
+ * thermal protection threshold
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_usbchthprotr_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev,
+		"Die temp above USB charger thermal protection threshold\n");
+	queue_work(di->charger_wq, &di->check_usb_thermal_prot_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_usbchargernotokr_handler() - USB charger not ok detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_usbchargernotokr_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev, "Not allowed USB charger detected\n");
+	queue_delayed_work(di->charger_wq, &di->check_usbchgnotok_work, 0);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_chwdexp_handler() - Charger watchdog expired
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_chwdexp_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev, "Charger watchdog expired\n");
+
+	/*
+	 * The charger that was online when the watchdog expired
+	 * needs to be restarted for charging to start again
+	 */
+	if (di->usb.charger_online) {
+		di->usb.wd_expired = true;
+		power_supply_changed(&di->usb_chg.psy);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_vbusovv_handler() - VBUS overvoltage detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_vbusovv_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev, "VBUS overvoltage detected\n");
+	di->flags.vbus_ovv = true;
+	power_supply_changed(&di->usb_chg.psy);
+
+	/* Schedule a new HW failure check */
+	queue_delayed_work(di->charger_wq, &di->check_hw_failure_work, 0);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_usb_get_property() - get the usb properties
+ * @psy:        pointer to the power_supply structure
+ * @psp:        pointer to the power_supply_property structure
+ * @val:        pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the usb
+ * properties by reading the sysfs files.
+ * USB properties are online, present and voltage.
+ * online:     usb charging is in progress or not
+ * present:    presence of the usb
+ * voltage:    vbus voltage
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_charger_usb_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct ab5500_charger *di;
+
+	di = to_ab5500_charger_usb_device_info(psy_to_ux500_charger(psy));
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_HEALTH:
+		if (di->flags.usbchargernotok)
+			val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		else if (di->usb.wd_expired)
+			val->intval = POWER_SUPPLY_HEALTH_DEAD;
+		else if (di->flags.usb_thermal_prot)
+			val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		else if (di->flags.vbus_ovv)
+			val->intval = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+		else
+			val->intval = POWER_SUPPLY_HEALTH_GOOD;
+		break;
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = di->usb.charger_online;
+		break;
+	case POWER_SUPPLY_PROP_PRESENT:
+		val->intval = di->usb.charger_connected;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		di->usb.charger_voltage = ab5500_charger_get_vbus_voltage(di);
+		val->intval = di->usb.charger_voltage * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		val->intval = ab5500_charger_get_usb_current(di) * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_AVG:
+		/*
+		 * This property is used to indicate when VBUS has collapsed
+		 * due to too high output current from the USB charger
+		 */
+		if (di->flags.vbus_collapse)
+			val->intval = 1;
+		else
+			val->intval = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/**
+ * ab5500_charger_hw_registers() - Set up charger related registers
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * Set up charger OVV, watchdog and maximum voltage registers as well as
+ * charging of the backup battery
+ */
+static int ab5500_charger_init_hw_registers(struct ab5500_charger *di)
+{
+	int ret = 0;
+
+	/* Enable ID Host and Device detection */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_USB, AB5500_USB_OTG_CTRL,
+			USB_ID_HOST_DET_ENA_MASK, USB_ID_HOST_DET_ENA);
+	if (ret) {
+		dev_err(di->dev, "failed to enable usb charger detection\n");
+		goto out;
+	}
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_USB, AB5500_USB_OTG_CTRL,
+			USB_ID_DEVICE_DET_ENA_MASK, USB_ID_DEVICE_DET_ENA);
+	if (ret) {
+		dev_err(di->dev, "failed to enable usb charger detection\n");
+		goto out;
+	}
+
+	/* Enable USB Charger Detection */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_USB, AB5500_USB_LINE_CTRL2,
+			USB_CHARG_DET_ENA_MASK, USB_CHARG_DET_ENA);
+	if (ret) {
+		dev_err(di->dev, "failed to enable usb charger detection\n");
+		goto out;
+	}
+
+	/* Over current protection for reverse supply */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_CHG, AB5500_CREVS, CHARGER_REV_SUP,
+			CHARGER_REV_SUP);
+	if (ret) {
+		dev_err(di->dev,
+			"failed to enable over current protection for reverse supply\n");
+		goto out;
+	}
+
+	/* Enable SW EOC at flatcurrent detection */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_CHG, AB5500_CCTRL, SW_EOC, SW_EOC);
+	if (ret) {
+		dev_err(di->dev,
+			"failed to enable end of charge at flatcurrent detection\n");
+		goto out;
+	}
+out:
+	return ret;
+}
+
+/*
+ * ab5500 charger driver interrupts and their respective isr
+ */
+static struct ab5500_charger_interrupts ab5500_charger_irq[] = {
+	{"VBUS_FALLING", ab5500_charger_vbusdetf_handler},
+	{"VBUS_RISING", ab5500_charger_vbusdetr_handler},
+	{"USB_LINK_UPDATE", ab5500_charger_usblinkstatus_handler},
+	{"USB_CH_TH_PROTECTION", ab5500_charger_usbchthprotr_handler},
+	{"USB_CH_NOT_OK", ab5500_charger_usbchargernotokr_handler},
+	{"OVV", ab5500_charger_vbusovv_handler},
+	/* TODO: Interrupt missing, will be available in cut 2 */
+	/*{"CHG_SW_TIMER_OUT", ab5500_charger_chwdexp_handler},*/
+};
+
+void ab5500_charger_usb_state_changed(u8 bm_usb_state, u16 mA)
+{
+	struct ab5500_charger *di = static_di;
+
+	dev_dbg(di->dev, "%s usb_state: 0x%02x mA: %d\n",
+		__func__, bm_usb_state, mA);
+
+	spin_lock(&di->usb_state.usb_lock);
+	di->usb_state.usb_changed = true;
+	spin_unlock(&di->usb_state.usb_lock);
+
+	di->usb_state.state = bm_usb_state;
+	di->usb_state.usb_current = mA;
+
+	queue_work(di->charger_wq, &di->usb_state_changed_work);
+
+	return;
+}
+EXPORT_SYMBOL(ab5500_charger_usb_state_changed);
+
+#if defined(CONFIG_PM)
+static int ab5500_charger_resume(struct platform_device *pdev)
+{
+	struct ab5500_charger *di = platform_get_drvdata(pdev);
+
+	/* If we still have a HW failure, schedule a new check */
+	if (di->flags.usbchargernotok || di->flags.vbus_ovv) {
+		queue_delayed_work(di->charger_wq,
+			&di->check_hw_failure_work, 0);
+	}
+
+	return 0;
+}
+
+static int ab5500_charger_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct ab5500_charger *di = platform_get_drvdata(pdev);
+
+	/* Cancel any pending HW failure check */
+	if (delayed_work_pending(&di->check_hw_failure_work))
+		cancel_delayed_work(&di->check_hw_failure_work);
+
+	return 0;
+}
+#else
+#define ab5500_charger_suspend      NULL
+#define ab5500_charger_resume       NULL
+#endif
+
+static int __devexit ab5500_charger_remove(struct platform_device *pdev)
+{
+	struct ab5500_charger *di = platform_get_drvdata(pdev);
+	int i, irq;
+
+	/* Disable USB charging */
+	ab5500_charger_usb_en(&di->usb_chg, false, 0, 0);
+
+	/* Disable interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_charger_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_charger_irq[i].name);
+		free_irq(irq, di);
+	}
+
+	/* Delete the work queue */
+	destroy_workqueue(di->charger_wq);
+
+	flush_scheduled_work();
+	power_supply_unregister(&di->usb_chg.psy);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+
+	return 0;
+}
+
+static int __devinit ab5500_charger_probe(struct platform_device *pdev)
+{
+	int irq, i, charger_status, ret = 0;
+	struct abx500_bm_plat_data *plat_data;
+
+	struct ab5500_charger *di =
+		kzalloc(sizeof(struct ab5500_charger), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	static_di = di;
+
+	/* get parent data */
+	di->dev = &pdev->dev;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
+	di->gpadc = ab5500_gpadc_get("ab5500-adc.0");
+
+	/* initialize lock */
+	spin_lock_init(&di->usb_state.usb_lock);
+
+	plat_data = pdev->dev.platform_data;
+	di->pdata = plat_data->charger;
+	di->bat = plat_data->battery;
+
+	/* get charger specific platform data */
+	if (!di->pdata) {
+		dev_err(di->dev, "no charger platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+
+	/* get battery specific platform data */
+	if (!di->bat) {
+		dev_err(di->dev, "no battery platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+	/* USB supply */
+	/* power_supply base class */
+	di->usb_chg.psy.name = "ab5500_usb";
+	di->usb_chg.psy.type = POWER_SUPPLY_TYPE_USB;
+	di->usb_chg.psy.properties = ab5500_charger_usb_props;
+	di->usb_chg.psy.num_properties = ARRAY_SIZE(ab5500_charger_usb_props);
+	di->usb_chg.psy.get_property = ab5500_charger_usb_get_property;
+	di->usb_chg.psy.supplied_to = di->pdata->supplied_to;
+	di->usb_chg.psy.num_supplicants = di->pdata->num_supplicants;
+	/* ux500_charger sub-class */
+	di->usb_chg.ops.enable = &ab5500_charger_usb_en;
+	di->usb_chg.ops.kick_wd = &ab5500_charger_watchdog_kick;
+	di->usb_chg.ops.update_curr = &ab5500_charger_update_charger_current;
+	di->usb_chg.max_out_volt = ab5500_charger_voltage_map[
+		ARRAY_SIZE(ab5500_charger_voltage_map) - 1];
+	di->usb_chg.max_out_curr = ab5500_charger_current_map[
+		ARRAY_SIZE(ab5500_charger_current_map) - 1];
+
+
+	/* Create a work queue for the charger */
+	di->charger_wq =
+		create_singlethread_workqueue("ab5500_charger_wq");
+	if (di->charger_wq == NULL) {
+		dev_err(di->dev, "failed to create work queue\n");
+		goto free_device_info;
+	}
+
+	/* Init work for HW failure check */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->check_hw_failure_work,
+		ab5500_charger_check_hw_failure_work);
+	INIT_DELAYED_WORK_DEFERRABLE(&di->check_usbchgnotok_work,
+		ab5500_charger_check_usbchargernotok_work);
+
+	/* Init work for charger detection */
+	INIT_WORK(&di->usb_link_status_work,
+		ab5500_charger_usb_link_status_work);
+	INIT_WORK(&di->detect_usb_type_work,
+		ab5500_charger_detect_usb_type_work);
+
+	INIT_WORK(&di->usb_state_changed_work,
+		ab5500_charger_usb_state_changed_work);
+
+	/* Init work for checking HW status */
+	INIT_WORK(&di->check_usb_thermal_prot_work,
+		ab5500_charger_check_usb_thermal_prot_work);
+
+	/* Get Chip ID of the ABB ASIC  */
+	ret = abx500_get_chip_id(di->dev);
+	if (ret < 0) {
+		dev_err(di->dev, "failed to get chip ID\n");
+		goto free_charger_wq;
+	}
+	di->chip_id = ret;
+	dev_dbg(di->dev, "AB5500 CID is: 0x%02x\n", di->chip_id);
+
+	/* Initialize OVV, and other registers */
+	ret = ab5500_charger_init_hw_registers(di);
+	if (ret) {
+		dev_err(di->dev, "failed to initialize ABB registers\n");
+		goto free_device_info;
+	}
+
+	/* Register USB charger class */
+	ret = power_supply_register(di->dev, &di->usb_chg.psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register USB charger\n");
+		goto free_device_info;
+	}
+
+	/* Identify the connected charger types during startup */
+	charger_status = ab5500_charger_detect_chargers(di);
+	if (charger_status & USB_PW_CONN) {
+		dev_dbg(di->dev, "VBUS Detect during startup\n");
+		di->vbus_detected = true;
+		di->vbus_detected_start = true;
+		queue_work(di->charger_wq,
+			&di->detect_usb_type_work);
+	}
+
+	/* Register interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_charger_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_charger_irq[i].name);
+		ret = request_threaded_irq(irq, NULL, ab5500_charger_irq[i].isr,
+			IRQF_SHARED | IRQF_NO_SUSPEND,
+			ab5500_charger_irq[i].name, di);
+
+		if (ret != 0) {
+			dev_err(di->dev, "failed to request %s IRQ %d: %d\n"
+				, ab5500_charger_irq[i].name, irq, ret);
+			goto free_irq;
+		}
+		dev_dbg(di->dev, "Requested %s IRQ %d: %d\n",
+			ab5500_charger_irq[i].name, irq, ret);
+	}
+
+	platform_set_drvdata(pdev, di);
+
+	dev_info(di->dev, "probe success\n");
+	return ret;
+
+free_irq:
+	power_supply_unregister(&di->usb_chg.psy);
+
+	/* We also have to free all successfully registered irqs */
+	for (i = i - 1; i >= 0; i--) {
+		irq = platform_get_irq_byname(pdev, ab5500_charger_irq[i].name);
+		free_irq(irq, di);
+	}
+free_charger_wq:
+	destroy_workqueue(di->charger_wq);
+free_device_info:
+	kfree(di);
+
+	return ret;
+}
+
+static struct platform_driver ab5500_charger_driver = {
+	.probe = ab5500_charger_probe,
+	.remove = __devexit_p(ab5500_charger_remove),
+	.suspend = ab5500_charger_suspend,
+	.resume = ab5500_charger_resume,
+	.driver = {
+		.name = "ab5500-charger",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab5500_charger_init(void)
+{
+	return platform_driver_register(&ab5500_charger_driver);
+}
+
+static void __exit ab5500_charger_exit(void)
+{
+	platform_driver_unregister(&ab5500_charger_driver);
+}
+
+subsys_initcall_sync(ab5500_charger_init);
+module_exit(ab5500_charger_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Johan Palsson, Karl Komierowski");
+MODULE_ALIAS("platform:ab5500-charger");
+MODULE_DESCRIPTION("AB5500 charger management driver");
diff --git a/drivers/power/ab5500_fg.c b/drivers/power/ab5500_fg.c
new file mode 100644
index 00000000000..919af5faadf
--- /dev/null
+++ b/drivers/power/ab5500_fg.c
@@ -0,0 +1,1829 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2011
+ *
+ * Main and Back-up battery management driver.
+ *
+ * Note: Backup battery management is required in case of Li-Ion battery and not
+ * for capacitive battery. HREF boards have capacitive battery and hence backup
+ * battery management is not used and the supported code is available in this
+ * driver.
+ *
+ * License Terms: GNU General Public License v2
+ * Authors:
+ *	Johan Palsson <johan.palsson@stericsson.com>
+ *	Karl Komierowski <karl.komierowski@stericsson.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include <linux/delay.h>
+#include <linux/mfd/abx500/ab5500-gpadc.h>
+#include <linux/mfd/abx500/ab5500-bm.h>
+#include <linux/mfd/abx500.h>
+
+static LIST_HEAD(ab5500_fg_list);
+
+/* U5500 Constants */
+#define FG_ON_MASK			0x04
+#define FG_ON				0x04
+#define FG_ACC_RESET_ON_READ_MASK	0x08
+#define FG_ACC_RESET_ON_READ		0x08
+#define EN_READOUT_MASK			0x01
+#define EN_READOUT			0x01
+#define RESET				0x00
+#define EOC_52_mA			0x04
+#define MILLI_TO_MICRO			1000
+#define FG_LSB_IN_MA			770
+#define QLSB_NANO_AMP_HOURS_X10		1129
+#define SEC_TO_SAMPLE(S)		(S * 4)
+#define NBR_AVG_SAMPLES			20
+#define LOW_BAT_CHECK_INTERVAL		(2 * HZ)
+
+#define VALID_CAPACITY_SEC		(45 * 60) /* 45 minutes */
+
+#define interpolate(x, x1, y1, x2, y2) \
+	((y1) + ((((y2) - (y1)) * ((x) - (x1))) / ((x2) - (x1))));
+
+#define to_ab5500_fg_device_info(x) container_of((x), \
+	struct ab5500_fg, fg_psy);
+
+/**
+ * struct ab5500_fg_interrupts - ab5500 fg interupts
+ * @name:	name of the interrupt
+ * @isr		function pointer to the isr
+ */
+struct ab5500_fg_interrupts {
+	char *name;
+	irqreturn_t (*isr)(int irq, void *data);
+};
+
+enum ab5500_fg_discharge_state {
+	AB5500_FG_DISCHARGE_INIT,
+	AB5500_FG_DISCHARGE_INITMEASURING,
+	AB5500_FG_DISCHARGE_INIT_RECOVERY,
+	AB5500_FG_DISCHARGE_RECOVERY,
+	AB5500_FG_DISCHARGE_READOUT,
+	AB5500_FG_DISCHARGE_WAKEUP,
+};
+
+static char *discharge_state[] = {
+	"DISCHARGE_INIT",
+	"DISCHARGE_INITMEASURING",
+	"DISCHARGE_INIT_RECOVERY",
+	"DISCHARGE_RECOVERY",
+	"DISCHARGE_READOUT",
+	"DISCHARGE_WAKEUP",
+};
+
+enum ab5500_fg_charge_state {
+	AB5500_FG_CHARGE_INIT,
+	AB5500_FG_CHARGE_READOUT,
+};
+
+static char *charge_state[] = {
+	"CHARGE_INIT",
+	"CHARGE_READOUT",
+};
+
+enum ab5500_fg_calibration_state {
+	AB5500_FG_CALIB_INIT,
+	AB5500_FG_CALIB_WAIT,
+	AB5500_FG_CALIB_END,
+};
+
+struct ab5500_fg_avg_cap {
+	int avg;
+	int samples[NBR_AVG_SAMPLES];
+	__kernel_time_t time_stamps[NBR_AVG_SAMPLES];
+	int pos;
+	int nbr_samples;
+	int sum;
+};
+
+struct ab5500_fg_battery_capacity {
+	int max_mah_design;
+	int max_mah;
+	int mah;
+	int permille;
+	int level;
+	int prev_mah;
+	int prev_percent;
+	int prev_level;
+};
+
+struct ab5500_fg_flags {
+	bool fg_enabled;
+	bool conv_done;
+	bool charging;
+	bool fully_charged;
+	bool low_bat_delay;
+	bool low_bat;
+	bool bat_ovv;
+	bool batt_unknown;
+	bool calibrate;
+};
+
+/**
+ * struct ab5500_fg - ab5500 FG device information
+ * @dev:		Pointer to the structure device
+ * @vbat:		Battery voltage in mV
+ * @vbat_nom:		Nominal battery voltage in mV
+ * @inst_curr:		Instantenous battery current in mA
+ * @avg_curr:		Average battery current in mA
+ * @fg_samples:		Number of samples used in the FG accumulation
+ * @accu_charge:	Accumulated charge from the last conversion
+ * @recovery_cnt:	Counter for recovery mode
+ * @high_curr_cnt:	Counter for high current mode
+ * @init_cnt:		Counter for init mode
+ * @v_to_cap:		capacity based on battery voltage
+ * @recovery_needed:	Indicate if recovery is needed
+ * @high_curr_mode:	Indicate if we're in high current mode
+ * @init_capacity:	Indicate if initial capacity measuring should be done
+ * @calib_state		State during offset calibration
+ * @discharge_state:	Current discharge state
+ * @charge_state:	Current charge state
+ * @flags:		Structure for information about events triggered
+ * @bat_cap:		Structure for battery capacity specific parameters
+ * @avg_cap:		Average capacity filter
+ * @parent:		Pointer to the struct ab5500
+ * @gpadc:		Pointer to the struct gpadc
+ * @gpadc_auto:		Pointer tot he struct adc_auto_input
+ * @pdata:		Pointer to the ab5500_fg platform data
+ * @bat:		Pointer to the ab5500_bm platform data
+ * @fg_psy:		Structure that holds the FG specific battery properties
+ * @fg_wq:		Work queue for running the FG algorithm
+ * @fg_periodic_work:	Work to run the FG algorithm periodically
+ * @fg_low_bat_work:	Work to check low bat condition
+ * @fg_work:		Work to run the FG algorithm instantly
+ * @fg_acc_cur_work:	Work to read the FG accumulator
+ * @cc_lock:		Mutex for locking the CC
+ * @node:		struct of type list_head
+ */
+struct ab5500_fg {
+	struct device *dev;
+	int vbat;
+	int vbat_nom;
+	int inst_curr;
+	int avg_curr;
+	int fg_samples;
+	int accu_charge;
+	int recovery_cnt;
+	int high_curr_cnt;
+	int init_cnt;
+	int v_to_cap;
+	bool recovery_needed;
+	bool high_curr_mode;
+	bool init_capacity;
+	enum ab5500_fg_calibration_state calib_state;
+	enum ab5500_fg_discharge_state discharge_state;
+	enum ab5500_fg_charge_state charge_state;
+	struct ab5500_fg_flags flags;
+	struct ab5500_fg_battery_capacity bat_cap;
+	struct ab5500_fg_avg_cap avg_cap;
+	struct ab5500 *parent;
+	struct ab5500_gpadc *gpadc;
+	struct adc_auto_input *gpadc_auto;
+	struct abx500_fg_platform_data *pdata;
+	struct abx500_bm_data *bat;
+	struct power_supply fg_psy;
+	struct workqueue_struct *fg_wq;
+	struct delayed_work fg_periodic_work;
+	struct delayed_work fg_low_bat_work;
+	struct work_struct fg_work;
+	struct delayed_work fg_acc_cur_work;
+	struct mutex cc_lock;
+	struct list_head node;
+	struct timer_list avg_current_timer;
+};
+
+/* Main battery properties */
+static enum power_supply_property ab5500_fg_props[] = {
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_CURRENT_AVG,
+	POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN,
+	POWER_SUPPLY_PROP_ENERGY_FULL,
+	POWER_SUPPLY_PROP_ENERGY_NOW,
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
+	POWER_SUPPLY_PROP_CHARGE_FULL,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_CAPACITY_LEVEL,
+};
+
+struct ab5500_fg *ab5500_fg_get(void)
+{
+	struct ab5500_fg *di;
+	di = list_first_entry(&ab5500_fg_list, struct ab5500_fg, node);
+
+	return di;
+}
+
+/**
+ * ab5500_fg_is_low_curr() - Low or high current mode
+ * @di:		pointer to the ab5500_fg structure
+ * @curr:	the current to base or our decision on
+ *
+ * Low current mode if the current consumption is below a certain threshold
+ */
+static int ab5500_fg_is_low_curr(struct ab5500_fg *di, int curr)
+{
+	/*
+	 * We want to know if we're in low current mode
+	 */
+	if (curr > -di->bat->fg_params->high_curr_threshold)
+		return true;
+	else
+		return false;
+}
+
+/**
+ * ab5500_fg_add_cap_sample() - Add capacity to average filter
+ * @di:		pointer to the ab5500_fg structure
+ * @sample:	the capacity in mAh to add to the filter
+ *
+ * A capacity is added to the filter and a new mean capacity is calculated and
+ * returned
+ */
+static int ab5500_fg_add_cap_sample(struct ab5500_fg *di, int sample)
+{
+	struct timespec ts;
+	struct ab5500_fg_avg_cap *avg = &di->avg_cap;
+
+	getnstimeofday(&ts);
+
+	do {
+		avg->sum += sample - avg->samples[avg->pos];
+		avg->samples[avg->pos] = sample;
+		avg->time_stamps[avg->pos] = ts.tv_sec;
+		avg->pos++;
+
+		if (avg->pos == NBR_AVG_SAMPLES)
+			avg->pos = 0;
+
+		if (avg->nbr_samples < NBR_AVG_SAMPLES)
+			avg->nbr_samples++;
+
+		/*
+		 * Check the time stamp for each sample. If too old,
+		 * replace with latest sample
+		 */
+	} while (ts.tv_sec - VALID_CAPACITY_SEC > avg->time_stamps[avg->pos]);
+
+	avg->avg = avg->sum / avg->nbr_samples;
+
+	return avg->avg;
+}
+
+/**
+ * ab5500_fg_fill_cap_sample() - Fill average filter
+ * @di:		pointer to the ab5500_fg structure
+ * @sample:	the capacity in mAh to fill the filter with
+ *
+ * The capacity filter is filled with a capacity in mAh
+ */
+static void ab5500_fg_fill_cap_sample(struct ab5500_fg *di, int sample)
+{
+	int i;
+	struct timespec ts;
+	struct ab5500_fg_avg_cap *avg = &di->avg_cap;
+
+	getnstimeofday(&ts);
+
+	for (i = 0; i < NBR_AVG_SAMPLES; i++) {
+		avg->samples[i] = sample;
+		avg->time_stamps[i] = ts.tv_sec;
+	}
+
+	avg->pos = 0;
+	avg->nbr_samples = NBR_AVG_SAMPLES;
+	avg->sum = sample * NBR_AVG_SAMPLES;
+	avg->avg = sample;
+}
+
+/**
+ * ab5500_fg_coulomb_counter() - enable coulomb counter
+ * @di:		pointer to the ab5500_fg structure
+ * @enable:	enable/disable
+ *
+ * Enable/Disable coulomb counter.
+ * On failure returns negative value.
+ */
+static int ab5500_fg_coulomb_counter(struct ab5500_fg *di, bool enable)
+{
+	int ret = 0;
+	mutex_lock(&di->cc_lock);
+	if (enable) {
+		/* Power-up the CC */
+		ret = abx500_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_A,
+			(FG_ON | FG_ACC_RESET_ON_READ));
+		if (ret)
+			goto cc_err;
+
+		di->flags.fg_enabled = true;
+	} else {
+		/* Stop the CC */
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_A,
+			FG_ON_MASK, RESET);
+		if (ret)
+			goto cc_err;
+
+		di->flags.fg_enabled = false;
+
+	}
+	dev_dbg(di->dev, " CC enabled: %d Samples: %d\n",
+		enable, di->fg_samples);
+
+	mutex_unlock(&di->cc_lock);
+
+	return ret;
+cc_err:
+	dev_err(di->dev, "%s Enabling coulomb counter failed\n", __func__);
+	mutex_unlock(&di->cc_lock);
+	return ret;
+}
+
+/**
+ * ab5500_fg_inst_curr() - battery instantaneous current
+ * @di:         pointer to the ab5500_fg structure
+ *
+ * Returns battery instantenous current(on success) else error code
+ */
+static int ab5500_fg_inst_curr(struct ab5500_fg *di)
+{
+	u8 low, high, value;
+	static int val;
+	int ret = 0;
+	bool fg_off = false;
+
+	if (!di->flags.fg_enabled) {
+		fg_off = true;
+		/* Power-up the CC */
+		ab5500_fg_coulomb_counter(di, true);
+		msleep(250);
+	}
+
+	mutex_lock(&di->cc_lock);
+	/*
+	 * Since there is no interrupt for this, just wait for 250ms
+	 * 250ms is one sample conversion time with 32.768 Khz RTC clock
+	 */
+	msleep(250);
+
+	/* Enable read request */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+		AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_B,
+		EN_READOUT_MASK, EN_READOUT);
+	if (ret)
+		goto inst_curr_err;
+
+	/* Read CC Sample conversion value Low and high */
+	ret = abx500_get_register_interruptible(di->dev,
+		AB5500_BANK_FG_BATTCOM_ACC,
+		AB5500_FGDIR_READ0,  &low);
+	if (ret < 0)
+		goto inst_curr_err;
+
+	ret = abx500_get_register_interruptible(di->dev,
+		AB5500_BANK_FG_BATTCOM_ACC,
+		AB5500_FGDIR_READ1,  &high);
+	if (ret < 0)
+		goto inst_curr_err;
+
+	/*
+	 * negative value for Discharging
+	 * convert 2's compliment into decimal
+	 */
+	if (high & 0x10)
+		val = (low | (high << 8) | 0xFFFFE000);
+	else
+		val = (low | (high << 8));
+
+	/*
+	 * Convert to unit value in mA
+	 * R(FGSENSE) = 20 mOhm
+	 * Scaling of LSB: This corresponds fro R(FGSENSE) to a current of
+	 * I = Q/t = 192.7 uC * 4 Hz = 0.77mA
+	 */
+	val = (val * 770) / 1000;
+
+	mutex_unlock(&di->cc_lock);
+
+	if (fg_off) {
+		dev_dbg(di->dev, "%s Disable FG\n", __func__);
+		/* Power-off the CC */
+		ab5500_fg_coulomb_counter(di, false);
+	}
+
+	return val;
+
+inst_curr_err:
+	dev_err(di->dev, "%s Get instanst current failed\n", __func__);
+	mutex_unlock(&di->cc_lock);
+	return ret;
+}
+
+static void ab5500_fg_acc_cur_timer_expired(unsigned long data)
+{
+	struct ab5500_fg *di = (struct ab5500_fg *) data;
+	dev_dbg(di->dev, "Avg current timer expired\n");
+
+	/* Trigger execution of the algorithm instantly */
+	queue_delayed_work(di->fg_wq, &di->fg_acc_cur_work, 0);
+}
+
+/**
+ * ab5500_fg_acc_cur_work() - average battery current
+ * @work:	pointer to the work_struct structure
+ *
+ * Updated the average battery current obtained from the
+ * coulomb counter.
+ */
+static void ab5500_fg_acc_cur_work(struct work_struct *work)
+{
+	int val;
+	int ret;
+	u8 low, med, high, cnt_low, cnt_high;
+
+	struct ab5500_fg *di = container_of(work,
+		struct ab5500_fg, fg_acc_cur_work.work);
+
+	if (!di->flags.fg_enabled) {
+		/* Power-up the CC */
+		ab5500_fg_coulomb_counter(di, true);
+		msleep(250);
+	}
+	mutex_lock(&di->cc_lock);
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_C,
+			EN_READOUT_MASK, EN_READOUT);
+	if (ret < 0)
+		goto exit;
+	/* If charging read charging registers for accumulated values */
+	if (di->flags.charging) {
+		/* Read CC Sample conversion value Low and high */
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_CH0, &low);
+		if (ret < 0)
+			goto exit;
+
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_CH1, &med);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_CH2, &high);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_VAL_COUNT0, &cnt_low);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_VAL_COUNT1, &cnt_high);
+		if (ret < 0)
+			goto exit;
+		queue_delayed_work(di->fg_wq, &di->fg_acc_cur_work,
+				di->bat->interval_charging * HZ);
+	} else { /* discharging */
+		/* Read CC Sample conversion value Low and high */
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_DIS_CH0, &low);
+		if (ret < 0)
+			goto exit;
+
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_DIS_CH1, &med);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_DIS_CH2, &high);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_VAL_COUNT0, &cnt_low);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_VAL_COUNT1, &cnt_high);
+		if (ret < 0)
+			goto exit;
+		queue_delayed_work(di->fg_wq, &di->fg_acc_cur_work,
+				di->bat->interval_not_charging * HZ);
+	}
+	di->fg_samples = (cnt_low | (cnt_high << 8));
+	val = (low | (med << 8) | (high << 16));
+
+	di->accu_charge = (val * QLSB_NANO_AMP_HOURS_X10)/10000;
+	di->avg_curr = (val * FG_LSB_IN_MA) / (di->fg_samples * 1000);
+	di->flags.conv_done = true;
+
+	mutex_unlock(&di->cc_lock);
+
+	queue_work(di->fg_wq, &di->fg_work);
+
+	return;
+exit:
+	dev_err(di->dev,
+		"Failed to read or write gas gauge registers\n");
+	mutex_unlock(&di->cc_lock);
+	queue_work(di->fg_wq, &di->fg_work);
+}
+
+/**
+ * ab5500_fg_bat_voltage() - get battery voltage
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Returns battery voltage(on success) else error code
+ */
+static int ab5500_fg_bat_voltage(struct ab5500_fg *di)
+{
+	int vbat;
+	static int prev;
+
+	vbat = ab5500_gpadc_convert(di->gpadc, MAIN_BAT_V);
+	if (vbat < 0) {
+		dev_err(di->dev,
+			"%s gpadc conversion failed, using previous value\n",
+			__func__);
+		return prev;
+	}
+
+	prev = vbat;
+	return vbat;
+}
+
+/**
+ * ab5500_fg_volt_to_capacity() - Voltage based capacity
+ * @di:		pointer to the ab5500_fg structure
+ * @voltage:	The voltage to convert to a capacity
+ *
+ * Returns battery capacity in per mille based on voltage
+ */
+static int ab5500_fg_volt_to_capacity(struct ab5500_fg *di, int voltage)
+{
+	int i, tbl_size;
+	struct abx500_v_to_cap *tbl;
+	int cap = 0;
+
+	tbl = di->bat->bat_type[di->bat->batt_id].v_to_cap_tbl,
+	tbl_size = di->bat->bat_type[di->bat->batt_id].n_v_cap_tbl_elements;
+
+	for (i = 0; i < tbl_size; ++i) {
+		if (di->vbat < tbl[i].voltage && di->vbat > tbl[i+1].voltage)
+			di->v_to_cap = tbl[i].capacity;
+	}
+
+	for (i = 0; i < tbl_size; ++i) {
+		if (voltage > tbl[i].voltage)
+			break;
+	}
+
+	if ((i > 0) && (i < tbl_size)) {
+		cap = interpolate(voltage,
+			tbl[i].voltage,
+			tbl[i].capacity * 10,
+			tbl[i-1].voltage,
+			tbl[i-1].capacity * 10);
+	} else if (i == 0) {
+		cap = 1000;
+	} else {
+		cap = 0;
+	}
+
+	dev_dbg(di->dev, "%s Vbat: %d, Cap: %d per mille",
+		__func__, voltage, cap);
+
+	return cap;
+}
+
+/**
+ * ab5500_fg_uncomp_volt_to_capacity() - Uncompensated voltage based capacity
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Returns battery capacity based on battery voltage that is not compensated
+ * for the voltage drop due to the load
+ */
+static int ab5500_fg_uncomp_volt_to_capacity(struct ab5500_fg *di)
+{
+	di->vbat = ab5500_fg_bat_voltage(di);
+	return ab5500_fg_volt_to_capacity(di, di->vbat);
+}
+
+/**
+ * ab5500_fg_load_comp_volt_to_capacity() - Load compensated voltage based capacity
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Returns battery capacity based on battery voltage that is load compensated
+ * for the voltage drop
+ */
+static int ab5500_fg_load_comp_volt_to_capacity(struct ab5500_fg *di)
+{
+	int vbat_comp;
+
+	di->inst_curr = ab5500_fg_inst_curr(di);
+	di->vbat = ab5500_fg_bat_voltage(di);
+
+	/* Use Ohms law to get the load compensated voltage */
+	vbat_comp = di->vbat - (di->inst_curr *
+		di->bat->bat_type[di->bat->batt_id].battery_resistance) / 1000;
+
+	dev_dbg(di->dev, "%s Measured Vbat: %dmV,Compensated Vbat %dmV, "
+		"R: %dmOhm, Current: %dmA\n",
+		__func__,
+		di->vbat,
+		vbat_comp,
+		di->bat->bat_type[di->bat->batt_id].battery_resistance,
+		di->inst_curr);
+
+	return ab5500_fg_volt_to_capacity(di, vbat_comp);
+}
+
+/**
+ * ab5500_fg_convert_mah_to_permille() - Capacity in mAh to permille
+ * @di:		pointer to the ab5500_fg structure
+ * @cap_mah:	capacity in mAh
+ *
+ * Converts capacity in mAh to capacity in permille
+ */
+static int ab5500_fg_convert_mah_to_permille(struct ab5500_fg *di, int cap_mah)
+{
+	return (cap_mah * 1000) / di->bat_cap.max_mah_design;
+}
+
+/**
+ * ab5500_fg_convert_permille_to_mah() - Capacity in permille to mAh
+ * @di:		pointer to the ab5500_fg structure
+ * @cap_pm:	capacity in permille
+ *
+ * Converts capacity in permille to capacity in mAh
+ */
+static int ab5500_fg_convert_permille_to_mah(struct ab5500_fg *di, int cap_pm)
+{
+	return cap_pm * di->bat_cap.max_mah_design / 1000;
+}
+
+/**
+ * ab5500_fg_convert_mah_to_uwh() - Capacity in mAh to uWh
+ * @di:		pointer to the ab5500_fg structure
+ * @cap_mah:	capacity in mAh
+ *
+ * Converts capacity in mAh to capacity in uWh
+ */
+static int ab5500_fg_convert_mah_to_uwh(struct ab5500_fg *di, int cap_mah)
+{
+	u64 div_res;
+	u32 div_rem;
+
+	div_res = ((u64) cap_mah) * ((u64) di->vbat_nom);
+	div_rem = do_div(div_res, 1000);
+
+	/* Make sure to round upwards if necessary */
+	if (div_rem >= 1000 / 2)
+		div_res++;
+
+	return (int) div_res;
+}
+
+/**
+ * ab5500_fg_calc_cap_charging() - Calculate remaining capacity while charging
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Return the capacity in mAh based on previous calculated capcity and the FG
+ * accumulator register value. The filter is filled with this capacity
+ */
+static int ab5500_fg_calc_cap_charging(struct ab5500_fg *di)
+{
+	dev_dbg(di->dev, "%s cap_mah %d accu_charge %d\n",
+		__func__,
+		di->bat_cap.mah,
+		di->accu_charge);
+
+	/* Capacity should not be less than 0 */
+	if (di->bat_cap.mah + di->accu_charge > 0)
+		di->bat_cap.mah += di->accu_charge;
+	else
+		di->bat_cap.mah = 0;
+
+	/*
+	 * We force capacity to 100% as long as the algorithm
+	 * reports that it's full.
+	*/
+	if (di->bat_cap.mah >= di->bat_cap.max_mah_design ||
+		di->flags.fully_charged)
+		di->bat_cap.mah = di->bat_cap.max_mah_design;
+
+	ab5500_fg_fill_cap_sample(di, di->bat_cap.mah);
+	di->bat_cap.permille =
+		ab5500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+
+	/* We need to update battery voltage and inst current when charging */
+	di->vbat = ab5500_fg_bat_voltage(di);
+	di->inst_curr = ab5500_fg_inst_curr(di);
+
+	return di->bat_cap.mah;
+}
+
+/**
+ * ab5500_fg_calc_cap_discharge_voltage() - Capacity in discharge with voltage
+ * @di:		pointer to the ab5500_fg structure
+ * @comp:	if voltage should be load compensated before capacity calc
+ *
+ * Return the capacity in mAh based on the battery voltage. The voltage can
+ * either be load compensated or not. This value is added to the filter and a
+ * new mean value is calculated and returned.
+ */
+static int ab5500_fg_calc_cap_discharge_voltage(struct ab5500_fg *di, bool comp)
+{
+	int permille, mah;
+
+	if (comp)
+		permille = ab5500_fg_load_comp_volt_to_capacity(di);
+	else
+		permille = ab5500_fg_uncomp_volt_to_capacity(di);
+
+	mah = ab5500_fg_convert_permille_to_mah(di, permille);
+
+	di->bat_cap.mah = ab5500_fg_add_cap_sample(di, mah);
+	di->bat_cap.permille =
+		ab5500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+
+	return di->bat_cap.mah;
+}
+
+/**
+ * ab5500_fg_calc_cap_discharge_fg() - Capacity in discharge with FG
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Return the capacity in mAh based on previous calculated capcity and the FG
+ * accumulator register value. This value is added to the filter and a
+ * new mean value is calculated and returned.
+ */
+static int ab5500_fg_calc_cap_discharge_fg(struct ab5500_fg *di)
+{
+	int permille_volt, permille;
+
+	dev_dbg(di->dev, "%s cap_mah %d accu_charge %d\n",
+		__func__,
+		di->bat_cap.mah,
+		di->accu_charge);
+
+	/* Capacity should not be less than 0 */
+	if (di->bat_cap.mah + di->accu_charge > 0)
+		di->bat_cap.mah += di->accu_charge;
+	else
+		di->bat_cap.mah = 0;
+
+	if (di->bat_cap.mah >= di->bat_cap.max_mah_design)
+		di->bat_cap.mah = di->bat_cap.max_mah_design;
+
+	/*
+	 * Check against voltage based capacity. It can not be lower
+	 * than what the uncompensated voltage says
+	 */
+	permille = ab5500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+	permille_volt = ab5500_fg_uncomp_volt_to_capacity(di);
+
+	if (permille < permille_volt) {
+		di->bat_cap.permille = permille_volt;
+		di->bat_cap.mah = ab5500_fg_convert_permille_to_mah(di,
+			di->bat_cap.permille);
+
+		dev_dbg(di->dev, "%s voltage based: perm %d perm_volt %d\n",
+			__func__,
+			permille,
+			permille_volt);
+
+		ab5500_fg_fill_cap_sample(di, di->bat_cap.mah);
+	} else {
+		ab5500_fg_fill_cap_sample(di, di->bat_cap.mah);
+		di->bat_cap.permille =
+			ab5500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+	}
+
+	return di->bat_cap.mah;
+}
+
+/**
+ * ab5500_fg_capacity_level() - Get the battery capacity level
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Get the battery capacity level based on the capacity in percent
+ */
+static int ab5500_fg_capacity_level(struct ab5500_fg *di)
+{
+	int ret, percent;
+
+	percent = di->bat_cap.permille / 10;
+
+	if (percent <= di->bat->cap_levels->critical ||
+		di->flags.low_bat)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_CRITICAL;
+	else if (percent <= di->bat->cap_levels->low)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_LOW;
+	else if (percent <= di->bat->cap_levels->normal)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_NORMAL;
+	else if (percent <= di->bat->cap_levels->high)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_HIGH;
+	else
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_FULL;
+
+	return ret;
+}
+
+/**
+ * ab5500_fg_check_capacity_limits() - Check if capacity has changed
+ * @di:		pointer to the ab5500_fg structure
+ * @init:	capacity is allowed to go up in init mode
+ *
+ * Check if capacity or capacity limit has changed and notify the system
+ * about it using the power_supply framework
+ */
+static void ab5500_fg_check_capacity_limits(struct ab5500_fg *di, bool init)
+{
+	bool changed = false;
+
+	di->bat_cap.level = ab5500_fg_capacity_level(di);
+
+	if (di->bat_cap.level != di->bat_cap.prev_level) {
+		/*
+		 * We do not allow reported capacity level to go up
+		 * unless we're charging or if we're in init
+		 */
+		if (!(!di->flags.charging && di->bat_cap.level >
+			di->bat_cap.prev_level) || init) {
+			dev_dbg(di->dev, "level changed from %d to %d\n",
+				di->bat_cap.prev_level,
+				di->bat_cap.level);
+			di->bat_cap.prev_level = di->bat_cap.level;
+			changed = true;
+		} else {
+			dev_dbg(di->dev, "level not allowed to go up "
+				"since no charger is connected: %d to %d\n",
+				di->bat_cap.prev_level,
+				di->bat_cap.level);
+		}
+	}
+
+	/*
+	 * If we have received the LOW_BAT IRQ, set capacity to 0 to initiate
+	 * shutdown
+	 */
+	if (di->flags.low_bat) {
+		dev_dbg(di->dev, "Battery low, set capacity to 0\n");
+		di->bat_cap.prev_percent = 0;
+		di->bat_cap.permille = 0;
+		di->bat_cap.prev_mah = 0;
+		di->bat_cap.mah = 0;
+		changed = true;
+	} else if (di->bat_cap.prev_percent != di->bat_cap.permille / 10) {
+		if (di->bat_cap.permille / 10 == 0) {
+			/*
+			 * We will not report 0% unless we've got
+			 * the LOW_BAT IRQ, no matter what the FG
+			 * algorithm says.
+			 */
+			di->bat_cap.prev_percent = 1;
+			di->bat_cap.permille = 1;
+			di->bat_cap.prev_mah = 1;
+			di->bat_cap.mah = 1;
+
+			changed = true;
+		} else if (!(!di->flags.charging &&
+			(di->bat_cap.permille / 10) >
+			di->bat_cap.prev_percent) || init) {
+			/*
+			 * We do not allow reported capacity to go up
+			 * unless we're charging or if we're in init
+			 */
+			dev_dbg(di->dev,
+				"capacity changed from %d to %d (%d)\n",
+				di->bat_cap.prev_percent,
+				di->bat_cap.permille / 10,
+				di->bat_cap.permille);
+			di->bat_cap.prev_percent = di->bat_cap.permille / 10;
+			di->bat_cap.prev_mah = di->bat_cap.mah;
+
+			changed = true;
+		} else {
+			dev_dbg(di->dev, "capacity not allowed to go up since "
+				"no charger is connected: %d to %d (%d)\n",
+				di->bat_cap.prev_percent,
+				di->bat_cap.permille / 10,
+				di->bat_cap.permille);
+		}
+	}
+
+	if (changed)
+		power_supply_changed(&di->fg_psy);
+
+}
+
+static void ab5500_fg_charge_state_to(struct ab5500_fg *di,
+	enum ab5500_fg_charge_state new_state)
+{
+	dev_dbg(di->dev, "Charge state from %d [%s] to %d [%s]\n",
+		di->charge_state,
+		charge_state[di->charge_state],
+		new_state,
+		charge_state[new_state]);
+
+	di->charge_state = new_state;
+}
+
+static void ab5500_fg_discharge_state_to(struct ab5500_fg *di,
+	enum ab5500_fg_charge_state new_state)
+{
+	dev_dbg(di->dev, "Disharge state from %d [%s] to %d [%s]\n",
+		di->discharge_state,
+		discharge_state[di->discharge_state],
+		new_state,
+		discharge_state[new_state]);
+
+	di->discharge_state = new_state;
+}
+
+/**
+ * ab5500_fg_algorithm_charging() - FG algorithm for when charging
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Battery capacity calculation state machine for when we're charging
+ */
+static void ab5500_fg_algorithm_charging(struct ab5500_fg *di)
+{
+	/*
+	 * If we change to discharge mode
+	 * we should start with recovery
+	 */
+	if (di->discharge_state != AB5500_FG_DISCHARGE_INIT_RECOVERY)
+		ab5500_fg_discharge_state_to(di,
+			AB5500_FG_DISCHARGE_INIT_RECOVERY);
+
+	switch (di->charge_state) {
+	case AB5500_FG_CHARGE_INIT:
+		di->fg_samples = SEC_TO_SAMPLE(
+			di->bat->fg_params->accu_charging);
+
+		ab5500_fg_coulomb_counter(di, true);
+		ab5500_fg_charge_state_to(di, AB5500_FG_CHARGE_READOUT);
+
+		break;
+
+	case AB5500_FG_CHARGE_READOUT:
+		/*
+		 * Read the FG and calculate the new capacity
+		 */
+		mutex_lock(&di->cc_lock);
+		if (!di->flags.conv_done) {
+			/* Wasn't the CC IRQ that got us here */
+			mutex_unlock(&di->cc_lock);
+			dev_dbg(di->dev, "%s CC conv not done\n",
+				__func__);
+
+			break;
+		}
+		di->flags.conv_done = false;
+		mutex_unlock(&di->cc_lock);
+
+		ab5500_fg_calc_cap_charging(di);
+
+		break;
+
+	default:
+		break;
+	}
+
+	/* Check capacity limits */
+	ab5500_fg_check_capacity_limits(di, false);
+}
+
+/**
+ * ab5500_fg_algorithm_discharging() - FG algorithm for when discharging
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Battery capacity calculation state machine for when we're discharging
+ */
+static void ab5500_fg_algorithm_discharging(struct ab5500_fg *di)
+{
+	int sleep_time;
+
+	/* If we change to charge mode we should start with init */
+	if (di->charge_state != AB5500_FG_CHARGE_INIT)
+		ab5500_fg_charge_state_to(di, AB5500_FG_CHARGE_INIT);
+
+	switch (di->discharge_state) {
+	case AB5500_FG_DISCHARGE_INIT:
+		/* We use the FG IRQ to work on */
+		di->init_cnt = 0;
+		di->fg_samples = SEC_TO_SAMPLE(di->bat->fg_params->init_timer);
+		ab5500_fg_coulomb_counter(di, true);
+		ab5500_fg_discharge_state_to(di,
+			AB5500_FG_DISCHARGE_INITMEASURING);
+
+		/* Intentional fallthrough */
+	case AB5500_FG_DISCHARGE_INITMEASURING:
+		/*
+		 * Discard a number of samples during startup.
+		 * After that, use compensated voltage for a few
+		 * samples to get an initial capacity.
+		 * Then go to READOUT
+		 */
+		sleep_time = di->bat->fg_params->init_timer;
+
+		/* Discard the first [x] seconds */
+		if (di->init_cnt >
+			di->bat->fg_params->init_discard_time) {
+
+			ab5500_fg_calc_cap_discharge_voltage(di, true);
+
+			ab5500_fg_check_capacity_limits(di, true);
+		}
+
+		di->init_cnt += sleep_time;
+		if (di->init_cnt >
+			di->bat->fg_params->init_total_time) {
+			di->fg_samples = SEC_TO_SAMPLE(
+				di->bat->fg_params->accu_high_curr);
+
+			ab5500_fg_coulomb_counter(di, true);
+			ab5500_fg_discharge_state_to(di,
+				AB5500_FG_DISCHARGE_READOUT);
+		}
+
+		break;
+
+	case AB5500_FG_DISCHARGE_INIT_RECOVERY:
+		di->recovery_cnt = 0;
+		di->recovery_needed = true;
+		ab5500_fg_discharge_state_to(di,
+			AB5500_FG_DISCHARGE_RECOVERY);
+
+		/* Intentional fallthrough */
+
+	case AB5500_FG_DISCHARGE_RECOVERY:
+		sleep_time = di->bat->fg_params->recovery_sleep_timer;
+
+		/*
+		 * We should check the power consumption
+		 * If low, go to READOUT (after x min) or
+		 * RECOVERY_SLEEP if time left.
+		 * If high, go to READOUT
+		 */
+		di->inst_curr = ab5500_fg_inst_curr(di);
+
+		if (ab5500_fg_is_low_curr(di, di->inst_curr)) {
+			if (di->recovery_cnt >
+				di->bat->fg_params->recovery_total_time) {
+				di->fg_samples = SEC_TO_SAMPLE(
+					di->bat->fg_params->accu_high_curr);
+				ab5500_fg_coulomb_counter(di, true);
+				ab5500_fg_discharge_state_to(di,
+					AB5500_FG_DISCHARGE_READOUT);
+				di->recovery_needed = false;
+			} else {
+				queue_delayed_work(di->fg_wq,
+					&di->fg_periodic_work,
+					sleep_time * HZ);
+			}
+			di->recovery_cnt += sleep_time;
+		} else {
+			di->fg_samples = SEC_TO_SAMPLE(
+				di->bat->fg_params->accu_high_curr);
+			ab5500_fg_coulomb_counter(di, true);
+			ab5500_fg_discharge_state_to(di,
+				AB5500_FG_DISCHARGE_READOUT);
+		}
+
+		break;
+
+	case AB5500_FG_DISCHARGE_READOUT:
+		di->inst_curr = ab5500_fg_inst_curr(di);
+
+		if (ab5500_fg_is_low_curr(di, di->inst_curr)) {
+			/* Detect mode change */
+			if (di->high_curr_mode) {
+				di->high_curr_mode = false;
+				di->high_curr_cnt = 0;
+			}
+
+			if (di->recovery_needed) {
+				ab5500_fg_discharge_state_to(di,
+					AB5500_FG_DISCHARGE_RECOVERY);
+
+				queue_delayed_work(di->fg_wq,
+					&di->fg_periodic_work,
+					0);
+
+				break;
+			}
+
+			ab5500_fg_calc_cap_discharge_voltage(di, true);
+		} else {
+			mutex_lock(&di->cc_lock);
+			if (!di->flags.conv_done) {
+				/* Wasn't the CC IRQ that got us here */
+				mutex_unlock(&di->cc_lock);
+				dev_dbg(di->dev, "%s CC conv not done\n",
+					__func__);
+
+				break;
+			}
+			di->flags.conv_done = false;
+			mutex_unlock(&di->cc_lock);
+
+			/* Detect mode change */
+			if (!di->high_curr_mode) {
+				di->high_curr_mode = true;
+				di->high_curr_cnt = 0;
+			}
+
+			di->high_curr_cnt +=
+				di->bat->fg_params->accu_high_curr;
+			if (di->high_curr_cnt >
+				di->bat->fg_params->high_curr_time)
+				di->recovery_needed = true;
+
+			ab5500_fg_calc_cap_discharge_fg(di);
+		}
+
+		ab5500_fg_check_capacity_limits(di, false);
+
+		break;
+
+	case AB5500_FG_DISCHARGE_WAKEUP:
+		ab5500_fg_coulomb_counter(di, true);
+		di->inst_curr = ab5500_fg_inst_curr(di);
+
+		ab5500_fg_calc_cap_discharge_voltage(di, true);
+
+		di->fg_samples = SEC_TO_SAMPLE(
+			di->bat->fg_params->accu_high_curr);
+		/* Re-program number of samples set above */
+		ab5500_fg_coulomb_counter(di, true);
+		ab5500_fg_discharge_state_to(di, AB5500_FG_DISCHARGE_READOUT);
+
+		ab5500_fg_check_capacity_limits(di, false);
+
+		break;
+
+	default:
+		break;
+	}
+}
+
+/**
+ * ab5500_fg_algorithm_calibrate() - Internal columb counter offset calibration
+ * @di:		pointer to the ab5500_fg structure
+ *
+ */
+static void ab5500_fg_algorithm_calibrate(struct ab5500_fg *di)
+{
+	int ret;
+
+	switch (di->calib_state) {
+	case AB5500_FG_CALIB_INIT:
+		dev_dbg(di->dev, "Calibration ongoing...\n");
+		/* TODO: For Cut 1.1 no calibration */
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_A,
+			FG_ACC_RESET_ON_READ_MASK, FG_ACC_RESET_ON_READ);
+		if (ret)
+			goto err;
+		di->calib_state = AB5500_FG_CALIB_WAIT;
+		break;
+	case AB5500_FG_CALIB_END:
+		di->flags.calibrate = false;
+		dev_dbg(di->dev, "Calibration done...\n");
+		queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+		break;
+	case AB5500_FG_CALIB_WAIT:
+		dev_dbg(di->dev, "Calibration WFI\n");
+	default:
+		break;
+	}
+	return;
+err:
+	/* Something went wrong, don't calibrate then */
+	dev_err(di->dev, "failed to calibrate the CC\n");
+	di->flags.calibrate = false;
+	di->calib_state = AB5500_FG_CALIB_INIT;
+	queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+}
+
+/**
+ * ab5500_fg_algorithm() - Entry point for the FG algorithm
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Entry point for the battery capacity calculation state machine
+ */
+static void ab5500_fg_algorithm(struct ab5500_fg *di)
+{
+	if (di->flags.calibrate)
+		ab5500_fg_algorithm_calibrate(di);
+	else {
+		if (di->flags.charging)
+			ab5500_fg_algorithm_charging(di);
+		else
+			ab5500_fg_algorithm_discharging(di);
+	}
+
+	dev_dbg(di->dev, "[FG_DATA] %d %d %d %d %d %d %d %d %d "
+		"%d %d %d %d %d %d %d\n",
+		di->bat_cap.max_mah_design,
+		di->bat_cap.mah,
+		di->bat_cap.permille,
+		di->bat_cap.level,
+		di->bat_cap.prev_mah,
+		di->bat_cap.prev_percent,
+		di->bat_cap.prev_level,
+		di->vbat,
+		di->inst_curr,
+		di->avg_curr,
+		di->accu_charge,
+		di->flags.charging,
+		di->charge_state,
+		di->discharge_state,
+		di->high_curr_mode,
+		di->recovery_needed);
+}
+
+/**
+ * ab5500_fg_periodic_work() - Run the FG state machine periodically
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for periodic work
+ */
+static void ab5500_fg_periodic_work(struct work_struct *work)
+{
+	struct ab5500_fg *di = container_of(work, struct ab5500_fg,
+		fg_periodic_work.work);
+
+	if (di->init_capacity) {
+		/* A dummy read that will return 0 */
+		di->inst_curr = ab5500_fg_inst_curr(di);
+		/* Get an initial capacity calculation */
+		ab5500_fg_calc_cap_discharge_voltage(di, true);
+		ab5500_fg_check_capacity_limits(di, true);
+		di->init_capacity = false;
+		queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+	} else
+		ab5500_fg_algorithm(di);
+}
+
+/**
+ * ab5500_fg_low_bat_work() - Check LOW_BAT condition
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the LOW_BAT condition
+ */
+static void ab5500_fg_low_bat_work(struct work_struct *work)
+{
+	int vbat;
+
+	struct ab5500_fg *di = container_of(work, struct ab5500_fg,
+		fg_low_bat_work.work);
+
+	vbat = ab5500_fg_bat_voltage(di);
+
+	/* Check if LOW_BAT still fulfilled */
+	if (vbat < di->bat->fg_params->lowbat_threshold) {
+		di->flags.low_bat = true;
+		dev_warn(di->dev, "Battery voltage still LOW\n");
+
+		/*
+		 * We need to re-schedule this check to be able to detect
+		 * if the voltage increases again during charging
+		 */
+		queue_delayed_work(di->fg_wq, &di->fg_low_bat_work,
+			round_jiffies(LOW_BAT_CHECK_INTERVAL));
+	} else {
+		di->flags.low_bat = false;
+		dev_warn(di->dev, "Battery voltage OK again\n");
+	}
+
+	/* This is needed to dispatch LOW_BAT */
+	ab5500_fg_check_capacity_limits(di, false);
+
+	/* Set this flag to check if LOW_BAT IRQ still occurs */
+	di->flags.low_bat_delay = false;
+}
+
+/**
+ * ab5500_fg_instant_work() - Run the FG state machine instantly
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for instant work
+ */
+static void ab5500_fg_instant_work(struct work_struct *work)
+{
+	struct ab5500_fg *di = container_of(work, struct ab5500_fg, fg_work);
+
+	ab5500_fg_algorithm(di);
+}
+
+/**
+ * ab5500_fg_get_property() - get the fg properties
+ * @psy:	pointer to the power_supply structure
+ * @psp:	pointer to the power_supply_property structure
+ * @val:	pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the
+ * fg properties by reading the sysfs files.
+ * voltage_now:		battery voltage
+ * current_now:		battery instant current
+ * current_avg:		battery average current
+ * charge_full_design:	capacity where battery is considered full
+ * charge_now:		battery capacity in nAh
+ * capacity:		capacity in percent
+ * capacity_level:	capacity level
+ *
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab5500_fg_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct ab5500_fg *di;
+	int i, tbl_size;
+	struct abx500_v_to_cap *tbl;
+
+	di = to_ab5500_fg_device_info(psy);
+
+	/*
+	 * If battery is identified as unknown and charging of unknown
+	 * batteries is disabled, we always report 100% capacity and
+	 * capacity level UNKNOWN, since we can't calculate
+	 * remaining capacity
+	 */
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		if (di->flags.bat_ovv)
+			val->intval = 47500000;
+		else
+			val->intval = ab5500_gpadc_convert
+						(di->gpadc, MAIN_BAT_V) * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		di->inst_curr = ab5500_fg_inst_curr(di);
+		val->intval = di->inst_curr * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_AVG:
+		val->intval = di->avg_curr * 1000;
+		break;
+	case POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN:
+		val->intval = ab5500_fg_convert_mah_to_uwh(di,
+				di->bat_cap.max_mah_design);
+		break;
+	case POWER_SUPPLY_PROP_ENERGY_FULL:
+		val->intval = ab5500_fg_convert_mah_to_uwh(di,
+				di->bat_cap.max_mah);
+		break;
+	case POWER_SUPPLY_PROP_ENERGY_NOW:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = ab5500_fg_convert_mah_to_uwh(di,
+					di->bat_cap.max_mah);
+		else
+			val->intval = ab5500_fg_convert_mah_to_uwh(di,
+					di->bat_cap.prev_mah);
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
+		val->intval = di->bat_cap.max_mah_design;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL:
+		val->intval = di->bat_cap.max_mah;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = di->bat_cap.max_mah;
+		else
+			val->intval = di->bat_cap.prev_mah;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = 100;
+		else if (di->bat->bat_type[di->bat->batt_id].v_to_cap_tbl) {
+			tbl = di->bat->bat_type[di->bat->batt_id].v_to_cap_tbl,
+			tbl_size = di->bat->bat_type[
+				di->bat->batt_id].n_v_cap_tbl_elements;
+
+			for (i = 0; i < tbl_size; ++i) {
+				if (di->vbat < tbl[i].voltage &&
+						di->vbat > tbl[i+1].voltage) {
+					di->v_to_cap = tbl[i].capacity;
+					break;
+				}
+			}
+			val->intval = di->v_to_cap;
+		} else
+			val->intval = di->bat_cap.prev_percent;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY_LEVEL:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_UNKNOWN;
+		else
+			val->intval = di->bat_cap.prev_level;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ab5500_fg_get_ext_psy_data(struct device *dev, void *data)
+{
+	struct power_supply *psy;
+	struct power_supply *ext;
+	struct ab5500_fg *di;
+	union power_supply_propval ret;
+	int i, j;
+	bool psy_found = false;
+
+	psy = (struct power_supply *)data;
+	ext = dev_get_drvdata(dev);
+	di = to_ab5500_fg_device_info(psy);
+
+	/*
+	 * For all psy where the name of your driver
+	 * appears in any supplied_to
+	 */
+	for (i = 0; i < ext->num_supplicants; i++) {
+		if (!strcmp(ext->supplied_to[i], psy->name))
+			psy_found = true;
+	}
+
+	if (!psy_found)
+		return 0;
+
+	/* Go through all properties for the psy */
+	for (j = 0; j < ext->num_properties; j++) {
+		enum power_supply_property prop;
+		prop = ext->properties[j];
+
+		if (ext->get_property(ext, prop, &ret))
+			continue;
+
+		switch (prop) {
+		case POWER_SUPPLY_PROP_STATUS:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				switch (ret.intval) {
+				case POWER_SUPPLY_STATUS_UNKNOWN:
+				case POWER_SUPPLY_STATUS_DISCHARGING:
+				case POWER_SUPPLY_STATUS_NOT_CHARGING:
+					if (!di->flags.charging)
+						break;
+					di->flags.charging = false;
+					di->flags.fully_charged = false;
+					queue_work(di->fg_wq, &di->fg_work);
+					break;
+				case POWER_SUPPLY_STATUS_FULL:
+					if (di->flags.fully_charged)
+						break;
+					di->flags.fully_charged = true;
+					/* Save current capacity as maximum */
+					di->bat_cap.max_mah = di->bat_cap.mah;
+					queue_work(di->fg_wq, &di->fg_work);
+					break;
+				case POWER_SUPPLY_STATUS_CHARGING:
+					if (di->flags.charging)
+						break;
+					di->flags.charging = true;
+					di->flags.fully_charged = false;
+					queue_work(di->fg_wq, &di->fg_work);
+					break;
+				};
+			default:
+				break;
+			};
+			break;
+		case POWER_SUPPLY_PROP_TECHNOLOGY:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				if (ret.intval)
+					di->flags.batt_unknown = false;
+				else
+					di->flags.batt_unknown = true;
+				break;
+			default:
+				break;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+static int ab5500_fg_bat_v_trig(int mux)
+{
+	struct ab5500_fg *di = ab5500_fg_get();
+
+	/* check if the battery voltage is below low threshold */
+	if (di->vbat < 2700) {
+		dev_warn(di->dev, "Battery voltage is below LOW threshold\n");
+		di->flags.low_bat_delay = true;
+		/*
+		 * Start a timer to check LOW_BAT again after some time
+		 * This is done to avoid shutdown on single voltage dips
+		 */
+		queue_delayed_work(di->fg_wq, &di->fg_low_bat_work,
+			round_jiffies(LOW_BAT_CHECK_INTERVAL));
+	}
+	/* check if battery votlage is above OVV */
+	else if (di->vbat > 4200) {
+		dev_dbg(di->dev, "Battery OVV\n");
+		di->flags.bat_ovv = true;
+
+		power_supply_changed(&di->fg_psy);
+	} else
+		return -EINVAL;
+
+	return 0;
+}
+
+/**
+ * ab5500_fg_init_hw_registers() - Set up FG related registers
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Set up battery OVV, low battery voltage registers
+ */
+static int ab5500_fg_init_hw_registers(struct ab5500_fg *di)
+{
+	int ret;
+	struct adc_auto_input *auto_ip;
+
+	auto_ip = kzalloc(sizeof(struct adc_auto_input), GFP_KERNEL);
+	if (!auto_ip) {
+		dev_err(di->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	auto_ip->mux = MAIN_BAT_V;
+	auto_ip->freq = MS500;
+	auto_ip->min = 2700;
+	auto_ip->max = 4200;
+	auto_ip->auto_adc_callback = ab5500_fg_bat_v_trig;
+	di->gpadc_auto = auto_ip;
+	ret = ab5500_gpadc_convert_auto(di->gpadc, di->gpadc_auto);
+	if (ret)
+		dev_err(di->dev,
+			"failed to set auto trigger for battery votlage\n");
+	/* set End Of Charge current to 247mA */
+	ret = abx500_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_EOC, EOC_52_mA);
+	return ret;
+}
+
+/**
+ * ab5500_fg_external_power_changed() - callback for power supply changes
+ * @psy:       pointer to the structure power_supply
+ *
+ * This function is the entry point of the pointer external_power_changed
+ * of the structure power_supply.
+ * This function gets executed when there is a change in any external power
+ * supply that this driver needs to be notified of.
+ */
+static void ab5500_fg_external_power_changed(struct power_supply *psy)
+{
+	struct ab5500_fg *di = to_ab5500_fg_device_info(psy);
+
+	class_for_each_device(power_supply_class, NULL,
+		&di->fg_psy, ab5500_fg_get_ext_psy_data);
+}
+
+#if defined(CONFIG_PM)
+static int ab5500_fg_resume(struct platform_device *pdev)
+{
+	struct ab5500_fg *di = platform_get_drvdata(pdev);
+
+	/*
+	 * Change state if we're not charging. If we're charging we will wake
+	 * up on the FG IRQ
+	 */
+	if (!di->flags.charging) {
+		ab5500_fg_discharge_state_to(di, AB5500_FG_DISCHARGE_WAKEUP);
+		queue_work(di->fg_wq, &di->fg_work);
+	}
+
+	return 0;
+}
+
+static int ab5500_fg_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct ab5500_fg *di = platform_get_drvdata(pdev);
+
+	flush_delayed_work(&di->fg_periodic_work);
+
+	/*
+	 * If the FG is enabled we will disable it before going to suspend
+	 * only if we're not charging
+	 */
+	if (di->flags.fg_enabled && !di->flags.charging)
+		ab5500_fg_coulomb_counter(di, false);
+
+	return 0;
+}
+#else
+#define ab5500_fg_suspend      NULL
+#define ab5500_fg_resume       NULL
+#endif
+
+static int __devexit ab5500_fg_remove(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct ab5500_fg *di = platform_get_drvdata(pdev);
+
+	/* Disable coulomb counter */
+	ret = ab5500_fg_coulomb_counter(di, false);
+	if (ret)
+		dev_err(di->dev, "failed to disable coulomb counter\n");
+
+	destroy_workqueue(di->fg_wq);
+
+	flush_scheduled_work();
+	power_supply_unregister(&di->fg_psy);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di->gpadc_auto);
+	kfree(di);
+	return ret;
+}
+
+static int __devinit ab5500_fg_probe(struct platform_device *pdev)
+{
+	struct abx500_bm_plat_data *plat_data;
+	int ret = 0;
+
+	struct ab5500_fg *di =
+		kzalloc(sizeof(struct ab5500_fg), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	mutex_init(&di->cc_lock);
+
+	/* get parent data */
+	di->dev = &pdev->dev;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
+	di->gpadc = ab5500_gpadc_get("ab5500-adc.0");
+
+	plat_data = pdev->dev.platform_data;
+	di->pdata = plat_data->fg;
+	di->bat = plat_data->battery;
+
+	/* get fg specific platform data */
+	if (!di->pdata) {
+		dev_err(di->dev, "no fg platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+
+	/* get battery specific platform data */
+	if (!di->bat) {
+		dev_err(di->dev, "no battery platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+
+	di->fg_psy.name = "ab5500_fg";
+	di->fg_psy.type = POWER_SUPPLY_TYPE_BATTERY;
+	di->fg_psy.properties = ab5500_fg_props;
+	di->fg_psy.num_properties = ARRAY_SIZE(ab5500_fg_props);
+	di->fg_psy.get_property = ab5500_fg_get_property;
+	di->fg_psy.supplied_to = di->pdata->supplied_to;
+	di->fg_psy.num_supplicants = di->pdata->num_supplicants;
+	di->fg_psy.external_power_changed = ab5500_fg_external_power_changed;
+
+	di->bat_cap.max_mah_design = MILLI_TO_MICRO *
+		di->bat->bat_type[di->bat->batt_id].charge_full_design;
+
+	di->bat_cap.max_mah = di->bat_cap.max_mah_design;
+
+	di->vbat_nom = di->bat->bat_type[di->bat->batt_id].nominal_voltage;
+
+	di->init_capacity = true;
+
+	ab5500_fg_charge_state_to(di, AB5500_FG_CHARGE_INIT);
+	ab5500_fg_discharge_state_to(di, AB5500_FG_DISCHARGE_INIT);
+
+	/* Create a work queue for running the FG algorithm */
+	di->fg_wq = create_singlethread_workqueue("ab5500_fg_wq");
+	if (di->fg_wq == NULL) {
+		dev_err(di->dev, "failed to create work queue\n");
+		goto free_device_info;
+	}
+
+	/* Init work for running the fg algorithm instantly */
+	INIT_WORK(&di->fg_work, ab5500_fg_instant_work);
+
+	/* Init work for getting the battery accumulated current */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->fg_acc_cur_work,
+			ab5500_fg_acc_cur_work);
+
+	/* Work delayed Queue to run the state machine */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->fg_periodic_work,
+		ab5500_fg_periodic_work);
+
+	/* Work to check low battery condition */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->fg_low_bat_work,
+		ab5500_fg_low_bat_work);
+
+	list_add_tail(&di->node, &ab5500_fg_list);
+
+	/* Initialize OVV, and other registers */
+	ret = ab5500_fg_init_hw_registers(di);
+	if (ret) {
+		dev_err(di->dev, "failed to initialize registers\n");
+		goto free_fg_wq;
+	}
+
+	/* Consider battery unknown until we're informed otherwise */
+	di->flags.batt_unknown = true;
+
+	/* Register FG power supply class */
+	ret = power_supply_register(di->dev, &di->fg_psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register FG psy\n");
+		goto free_fg_wq;
+	}
+
+	di->fg_samples = SEC_TO_SAMPLE(di->bat->fg_params->init_timer);
+	ab5500_fg_coulomb_counter(di, true);
+
+	/* Initilialize avg current timer */
+	init_timer(&di->avg_current_timer);
+	di->avg_current_timer.function = ab5500_fg_acc_cur_timer_expired;
+	di->avg_current_timer.data = (unsigned long) di;
+	di->avg_current_timer.expires = 60 * HZ;;
+	if (!timer_pending(&di->avg_current_timer))
+		add_timer(&di->avg_current_timer);
+	else
+		mod_timer(&di->avg_current_timer, 60 * HZ);
+
+	platform_set_drvdata(pdev, di);
+
+	/* Calibrate the fg first time */
+	di->flags.calibrate = true;
+	di->calib_state = AB5500_FG_CALIB_INIT;
+	/* Run the FG algorithm */
+	queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+	queue_delayed_work(di->fg_wq, &di->fg_acc_cur_work, 0);
+
+	dev_info(di->dev, "probe success\n");
+	return ret;
+
+free_fg_wq:
+	destroy_workqueue(di->fg_wq);
+free_device_info:
+	kfree(di);
+
+	return ret;
+}
+
+static struct platform_driver ab5500_fg_driver = {
+	.probe = ab5500_fg_probe,
+	.remove = __devexit_p(ab5500_fg_remove),
+	.suspend = ab5500_fg_suspend,
+	.resume = ab5500_fg_resume,
+	.driver = {
+		.name = "ab5500-fg",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab5500_fg_init(void)
+{
+	return platform_driver_register(&ab5500_fg_driver);
+}
+
+static void __exit ab5500_fg_exit(void)
+{
+	platform_driver_unregister(&ab5500_fg_driver);
+}
+
+subsys_initcall_sync(ab5500_fg_init);
+module_exit(ab5500_fg_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Johan Palsson, Karl Komierowski");
+MODULE_ALIAS("platform:ab5500-fg");
+MODULE_DESCRIPTION("AB5500 Fuel Gauge driver");
diff --git a/drivers/power/ab8500_btemp.c b/drivers/power/ab8500_btemp.c
new file mode 100644
index 00000000000..7d204b19742
--- /dev/null
+++ b/drivers/power/ab8500_btemp.c
@@ -0,0 +1,1080 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Battery temperature driver for AB8500
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Johan Palsson <johan.palsson@stericsson.com>
+ * Author: Karl Komierowski <karl.komierowski@stericsson.com>
+ * Author: Arun R Murthy <arun.murthy@stericsson.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/completion.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/ab8500/bm.h>
+#include <linux/mfd/ab8500/gpadc.h>
+
+#define VTVOUT_V			1800
+
+#define BTEMP_THERMAL_LOW_LIMIT		-10
+#define BTEMP_THERMAL_MED_LIMIT		0
+#define BTEMP_THERMAL_HIGH_LIMIT_52	52
+#define BTEMP_THERMAL_HIGH_LIMIT_57	57
+#define BTEMP_THERMAL_HIGH_LIMIT_62	62
+
+#define BTEMP_BATCTRL_CURR_SRC_7UA	7
+#define BTEMP_BATCTRL_CURR_SRC_20UA	20
+
+#define to_ab8500_btemp_device_info(x) container_of((x), \
+	struct ab8500_btemp, btemp_psy);
+
+/**
+ * struct ab8500_btemp_interrupts - ab8500 interrupts
+ * @name:	name of the interrupt
+ * @isr		function pointer to the isr
+ */
+struct ab8500_btemp_interrupts {
+	char *name;
+	irqreturn_t (*isr)(int irq, void *data);
+};
+
+struct ab8500_btemp_events {
+	bool batt_rem;
+	bool btemp_high;
+	bool btemp_medhigh;
+	bool btemp_lowmed;
+	bool btemp_low;
+	bool ac_conn;
+	bool usb_conn;
+};
+
+struct ab8500_btemp_ranges {
+	int btemp_high_limit;
+	int btemp_med_limit;
+	int btemp_low_limit;
+};
+
+/**
+ * struct ab8500_btemp - ab8500 BTEMP device information
+ * @dev:		Pointer to the structure device
+ * @chip_id:		Chip-Id of the AB8500
+ * @curr_source:	What current source we use, in uA
+ * @bat_temp:		Battery temperature in degree Celcius
+ * @prev_bat_temp	Last dispatched battery temperature
+ * @parent:		Pointer to the struct ab8500
+ * @gpadc:		Pointer to the struct gpadc
+ * @pdata:		Pointer to the ab8500_btemp platform data
+ * @bat:		Pointer to the ab8500_bm platform data
+ * @btemp_psy:		Structure for BTEMP specific battery properties
+ * @events:		Structure for information about events triggered
+ * @btemp_ranges:	Battery temperature range structure
+ * @btemp_wq:		Work queue for measuring the temperature periodically
+ * @btemp_periodic_work:	Work for measuring the temperature periodically
+ */
+struct ab8500_btemp {
+	struct device *dev;
+	u8 chip_id;
+	int curr_source;
+	int bat_temp;
+	int prev_bat_temp;
+	struct ab8500 *parent;
+	struct ab8500_gpadc *gpadc;
+	struct ab8500_btemp_platform_data *pdata;
+	struct ab8500_bm_data *bat;
+	struct power_supply btemp_psy;
+	struct ab8500_btemp_events events;
+	struct ab8500_btemp_ranges btemp_ranges;
+	struct workqueue_struct *btemp_wq;
+	struct delayed_work btemp_periodic_work;
+};
+
+/* BTEMP power supply properties */
+static enum power_supply_property ab8500_btemp_props[] = {
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_TEMP,
+};
+
+/**
+ * ab8500_btemp_batctrl_volt_to_res() - convert batctrl voltage to resistance
+ * @di:		pointer to the ab8500_btemp structure
+ * @v_batctrl:	measured batctrl voltage
+ *
+ * This function returns the battery resistance that is
+ * derived from the BATCTRL voltage.
+ * Returns value in Ohms.
+ */
+static int ab8500_btemp_batctrl_volt_to_res(struct ab8500_btemp *di,
+	int v_batctrl)
+{
+	int rbs;
+
+	switch (di->chip_id) {
+	case AB8500_CUT1P0:
+	case AB8500_CUT1P1:
+		/*
+		 * For ABB cut1.0 and 1.1 BAT_CTRL is internally
+		 * connected to 1.8V through a 450k resistor
+		 */
+		rbs = (450000 * (v_batctrl)) / (1800 - v_batctrl);
+		break;
+	default:
+		if (di->bat->adc_therm == ADC_THERM_BATCTRL) {
+			/*
+			 * If the battery has internal NTC, we use the current
+			 * source to calculate the resistance, 7uA or 20uA
+			 */
+			rbs = v_batctrl * 1000 / di->curr_source;
+		} else {
+			/*
+			 * BAT_CTRL is internally
+			 * connected to 1.8V through a 80k resistor
+			 */
+			rbs = (80000 * (v_batctrl)) / (1800 - v_batctrl);
+		}
+		break;
+	}
+
+	return rbs;
+}
+
+/**
+ * ab8500_btemp_read_batctrl_voltage() - measure batctrl voltage
+ * @di:		pointer to the ab8500_btemp structure
+ *
+ * This function returns the voltage on BATCTRL. Returns value in mV.
+ */
+static int ab8500_btemp_read_batctrl_voltage(struct ab8500_btemp *di)
+{
+	int vbtemp;
+	static int prev;
+
+	vbtemp = ab8500_gpadc_convert(di->gpadc, BAT_CTRL);
+	if (vbtemp < 0) {
+		dev_err(di->dev,
+			"%s gpadc conversion failed, using previous value",
+			__func__);
+		return prev;
+	}
+	prev = vbtemp;
+	return vbtemp;
+}
+
+/**
+ * ab8500_btemp_curr_source_enable() - enable/disable batctrl current source
+ * @di:		pointer to the ab8500_btemp structure
+ * @enable:	enable or disable the current source
+ *
+ * Enable or disable the current sources for the BatCtrl AD channel
+ */
+static int ab8500_btemp_curr_source_enable(struct ab8500_btemp *di,
+	bool enable)
+{
+	int curr;
+	int ret = 0;
+
+	/*
+	 * BATCTRL current sources are included on AB8500 cut2.0
+	 * and future versions
+	 */
+	if (di->chip_id == AB8500_CUT1P0 || di->chip_id == AB8500_CUT1P1)
+		return 0;
+
+	/* Only do this for batteries with internal NTC */
+	if (di->bat->adc_therm == ADC_THERM_BATCTRL && enable) {
+		if (di->curr_source == BTEMP_BATCTRL_CURR_SRC_7UA)
+			curr = BAT_CTRL_7U_ENA;
+		else
+			curr = BAT_CTRL_20U_ENA;
+
+		dev_dbg(di->dev, "Set BATCTRL %duA\n", di->curr_source);
+
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
+			FORCE_BAT_CTRL_CMP_HIGH, FORCE_BAT_CTRL_CMP_HIGH);
+		if (ret) {
+			dev_err(di->dev, "%s failed setting cmp_force\n",
+				__func__);
+			return ret;
+		}
+
+		/*
+		 * We have to wait one 32kHz cycle before enabling
+		 * the current source, since ForceBatCtrlCmpHigh needs
+		 * to be written in a separate cycle
+		 */
+		udelay(32);
+
+		ret = abx500_set_register_interruptible(di->dev,
+			AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
+			FORCE_BAT_CTRL_CMP_HIGH | curr);
+		if (ret) {
+			dev_err(di->dev, "%s failed enabling current source\n",
+				__func__);
+			goto disable_curr_source;
+		}
+	} else if (di->bat->adc_therm == ADC_THERM_BATCTRL && !enable) {
+		dev_dbg(di->dev, "Disable BATCTRL curr source\n");
+
+		/* Write 0 to the curr bits */
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
+			BAT_CTRL_7U_ENA | BAT_CTRL_20U_ENA,
+			~(BAT_CTRL_7U_ENA | BAT_CTRL_20U_ENA));
+		if (ret) {
+			dev_err(di->dev, "%s failed disabling current source\n",
+				__func__);
+			goto disable_curr_source;
+		}
+
+		/* Enable Pull-Up and comparator */
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_CHARGER,	AB8500_BAT_CTRL_CURRENT_SOURCE,
+			BAT_CTRL_PULL_UP_ENA | BAT_CTRL_CMP_ENA,
+			BAT_CTRL_PULL_UP_ENA | BAT_CTRL_CMP_ENA);
+		if (ret) {
+			dev_err(di->dev, "%s failed enabling PU and comp\n",
+				__func__);
+			goto enable_pu_comp;
+		}
+
+		/*
+		 * We have to wait one 32kHz cycle before disabling
+		 * ForceBatCtrlCmpHigh since this needs to be written
+		 * in a separate cycle
+		 */
+		udelay(32);
+
+		/* Disable 'force comparator' */
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
+			FORCE_BAT_CTRL_CMP_HIGH, ~FORCE_BAT_CTRL_CMP_HIGH);
+		if (ret) {
+			dev_err(di->dev, "%s failed disabling force comp\n",
+				__func__);
+			goto disable_force_comp;
+		}
+	}
+	return ret;
+
+	/*
+	 * We have to try unsetting FORCE_BAT_CTRL_CMP_HIGH one more time
+	 * if we got an error above
+	 */
+disable_curr_source:
+	/* Write 0 to the curr bits */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
+			BAT_CTRL_7U_ENA | BAT_CTRL_20U_ENA,
+			~(BAT_CTRL_7U_ENA | BAT_CTRL_20U_ENA));
+	if (ret) {
+		dev_err(di->dev, "%s failed disabling current source\n",
+			__func__);
+		return ret;
+	}
+enable_pu_comp:
+	/* Enable Pull-Up and comparator */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+		AB8500_CHARGER,	AB8500_BAT_CTRL_CURRENT_SOURCE,
+		BAT_CTRL_PULL_UP_ENA | BAT_CTRL_CMP_ENA,
+		BAT_CTRL_PULL_UP_ENA | BAT_CTRL_CMP_ENA);
+	if (ret) {
+		dev_err(di->dev, "%s failed enabling PU and comp\n",
+			__func__);
+		return ret;
+	}
+
+disable_force_comp:
+	/*
+	 * We have to wait one 32kHz cycle before disabling
+	 * ForceBatCtrlCmpHigh since this needs to be written
+	 * in a separate cycle
+	 */
+	udelay(32);
+
+	/* Disable 'force comparator' */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+		AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
+		FORCE_BAT_CTRL_CMP_HIGH, ~FORCE_BAT_CTRL_CMP_HIGH);
+	if (ret) {
+		dev_err(di->dev, "%s failed disabling force comp\n",
+			__func__);
+		return ret;
+	}
+
+	return ret;
+}
+
+/**
+ * ab8500_btemp_get_batctrl_res() - get battery resistance
+ * @di:		pointer to the ab8500_btemp structure
+ *
+ * This function returns the battery pack identification resistance.
+ * Returns value in Ohms.
+ */
+static int ab8500_btemp_get_batctrl_res(struct ab8500_btemp *di)
+{
+	int ret;
+	int batctrl;
+	int res;
+
+	/*
+	 * BATCTRL current sources are included on AB8500 cut2.0
+	 * and future versions
+	 */
+	ret = ab8500_btemp_curr_source_enable(di, true);
+	if (ret) {
+		dev_err(di->dev, "%s curr source enabled failed\n", __func__);
+		return ret;
+	}
+
+	batctrl = ab8500_btemp_read_batctrl_voltage(di);
+	res = ab8500_btemp_batctrl_volt_to_res(di, batctrl);
+
+	ret = ab8500_btemp_curr_source_enable(di, false);
+	if (ret) {
+		dev_err(di->dev, "%s curr source disable failed\n", __func__);
+		return ret;
+	}
+
+	dev_dbg(di->dev, "%s batctrl: %d res: %d ",
+		__func__, batctrl, res);
+
+	return res;
+}
+
+/**
+ * ab8500_btemp_res_to_temp() - resistance to temperature
+ * @di:		pointer to the ab8500_btemp structure
+ * @tbl:	pointer to the resiatance to temperature table
+ * @tbl_size:	size of the resistance to temperature table
+ * @res:	resistance to calculate the temperature from
+ *
+ * This function returns the battery temperature in degrees Celcius
+ * based on the NTC resistance.
+ */
+static int ab8500_btemp_res_to_temp(struct ab8500_btemp *di,
+	const struct res_to_temp *tbl, int tbl_size, int res)
+{
+	int i, temp;
+	/*
+	 * Calculate the formula for the straight line
+	 * Simple interpolation if we are within
+	 * the resistance table limits, extrapolate
+	 * if resistance is outside the limits.
+	 */
+	if (res > tbl[0].resist)
+		i = 0;
+	else if (res <= tbl[tbl_size - 1].resist)
+		i = tbl_size - 2;
+	else {
+		i = 0;
+		while (!(res <= tbl[i].resist &&
+			res > tbl[i + 1].resist))
+			i++;
+	}
+
+	temp = tbl[i].temp + ((tbl[i + 1].temp - tbl[i].temp) *
+		(res - tbl[i].resist)) / (tbl[i + 1].resist - tbl[i].resist);
+	return temp;
+}
+
+/**
+ * ab8500_btemp_measure_temp() - measure battery temperature
+ * @di:		pointer to the ab8500_btemp structure
+ *
+ * Returns battery temperature (on success) else the previous temperature
+ */
+static int ab8500_btemp_measure_temp(struct ab8500_btemp *di)
+{
+	int temp;
+	static int prev;
+	int rbat, rntc, vntc;
+	u8 id;
+
+	id = di->bat->batt_id;
+
+	if (di->bat->adc_therm == ADC_THERM_BATCTRL &&
+			id != BATTERY_UNKNOWN) {
+
+		rbat = ab8500_btemp_get_batctrl_res(di);
+		if (rbat < 0) {
+			dev_err(di->dev, "%s get batctrl res failed\n",
+				__func__);
+			/*
+			 * Return out-of-range temperature so that
+			 * charging is stopped
+			 */
+			return BTEMP_THERMAL_LOW_LIMIT;
+		}
+
+		temp = ab8500_btemp_res_to_temp(di,
+			di->bat->bat_type[id].r_to_t_tbl,
+			di->bat->bat_type[id].n_temp_tbl_elements, rbat);
+	} else {
+		vntc = ab8500_gpadc_convert(di->gpadc, BTEMP_BALL);
+		if (vntc < 0) {
+			dev_err(di->dev,
+				"%s gpadc conversion failed,"
+				" using previous value\n", __func__);
+			return prev;
+		}
+		/*
+		 * The PCB NTC is sourced from VTVOUT via a 230kOhm
+		 * resistor.
+		 */
+		rntc = 230000 * vntc / (VTVOUT_V - vntc);
+
+		temp = ab8500_btemp_res_to_temp(di,
+			di->bat->bat_type[id].r_to_t_tbl,
+			di->bat->bat_type[id].n_temp_tbl_elements, rntc);
+		prev = temp;
+	}
+	dev_dbg(di->dev, "Battery temperature is %d\n", temp);
+	return temp;
+}
+
+/**
+ * ab8500_btemp_id() - Identify the connected battery
+ * @di:		pointer to the ab8500_btemp structure
+ *
+ * This function will try to identify the battery by reading the ID
+ * resistor. Some brands use a combined ID resistor with a NTC resistor to
+ * both be able to identify and to read the temperature of it.
+ */
+static int ab8500_btemp_id(struct ab8500_btemp *di)
+{
+	int res;
+	u8 i;
+
+	di->curr_source = BTEMP_BATCTRL_CURR_SRC_7UA;
+	di->bat->batt_id = BATTERY_UNKNOWN;
+
+	res =  ab8500_btemp_get_batctrl_res(di);
+	if (res < 0) {
+		dev_err(di->dev, "%s get batctrl res failed\n", __func__);
+		return -ENXIO;
+	}
+
+	/* BATTERY_UNKNOWN is defined on position 0, skip it! */
+	for (i = BATTERY_UNKNOWN + 1; i < di->bat->n_btypes; i++) {
+		if ((res <= di->bat->bat_type[i].resis_high) &&
+			(res >= di->bat->bat_type[i].resis_low)) {
+			dev_dbg(di->dev, "Battery detected on %s"
+				" low %d < res %d < high: %d"
+				" index: %d\n",
+				di->bat->adc_therm == ADC_THERM_BATCTRL ?
+				"BATCTRL" : "BATTEMP",
+				di->bat->bat_type[i].resis_low, res,
+				di->bat->bat_type[i].resis_high, i);
+
+			di->bat->batt_id = i;
+			break;
+		}
+	}
+
+	if (di->bat->batt_id == BATTERY_UNKNOWN) {
+		dev_warn(di->dev, "Battery identified as unknown"
+			", resistance %d Ohm\n", res);
+		return -ENXIO;
+	}
+
+	/*
+	 * We only have to change current source if the
+	 * detected type is Type 1, else we use the 7uA source
+	 */
+	if (di->bat->adc_therm == ADC_THERM_BATCTRL && di->bat->batt_id == 1) {
+		dev_dbg(di->dev, "Set BATCTRL current source to 20uA\n");
+		di->curr_source = BTEMP_BATCTRL_CURR_SRC_20UA;
+	}
+
+	return di->bat->batt_id;
+}
+
+/**
+ * ab8500_btemp_periodic_work() - Measuring the temperature periodically
+ * @work:	pointer to the work_struct structure
+ *
+ * Work function for measuring the temperature periodically
+ */
+static void ab8500_btemp_periodic_work(struct work_struct *work)
+{
+	struct ab8500_btemp *di = container_of(work,
+		struct ab8500_btemp, btemp_periodic_work.work);
+
+	di->bat_temp = ab8500_btemp_measure_temp(di);
+
+	if (di->bat_temp != di->prev_bat_temp) {
+		di->prev_bat_temp = di->bat_temp;
+		power_supply_changed(&di->btemp_psy);
+	}
+
+	/* Schedule a new measurement */
+	queue_delayed_work(di->btemp_wq,
+		&di->btemp_periodic_work,
+		round_jiffies(20 * HZ));
+}
+
+/**
+ * ab8500_btemp_batctrlindb_handler() - battery removal detected
+ * @irq:       interrupt number
+ * @_di:       void pointer that has to address of ab8500_btemp
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_btemp_batctrlindb_handler(int irq, void *_di)
+{
+	struct ab8500_btemp *di = _di;
+	dev_err(di->dev, "Battery removal detected!\n");
+
+	di->events.batt_rem = true;
+	power_supply_changed(&di->btemp_psy);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_btemp_templow_handler() - battery temp lower than 10 degrees
+ * @irq:       interrupt number
+ * @_di:       void pointer that has to address of ab8500_btemp
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_btemp_templow_handler(int irq, void *_di)
+{
+	struct ab8500_btemp *di = _di;
+	switch (di->chip_id) {
+	case AB8500_CUT1P0:
+	case AB8500_CUT1P1:
+	case AB8500_CUT2P0:
+		dev_dbg(di->dev, "Ignore false btemp low irq"
+			" for ABB cut 1.0, 1.1 and 2.0\n");
+
+		break;
+	default:
+		dev_crit(di->dev, "Battery temperature lower than -10deg c\n");
+
+		di->events.btemp_low = true;
+		di->events.btemp_high = false;
+		di->events.btemp_medhigh = false;
+		di->events.btemp_lowmed = false;
+		power_supply_changed(&di->btemp_psy);
+
+		break;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_btemp_temphigh_handler() - battery temp higher than max temp
+ * @irq:       interrupt number
+ * @_di:       void pointer that has to address of ab8500_btemp
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_btemp_temphigh_handler(int irq, void *_di)
+{
+	struct ab8500_btemp *di = _di;
+
+	dev_crit(di->dev, "Battery temperature is higher than MAX temp\n");
+
+	di->events.btemp_high = true;
+	di->events.btemp_medhigh = false;
+	di->events.btemp_lowmed = false;
+	di->events.btemp_low = false;
+	power_supply_changed(&di->btemp_psy);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_btemp_lowmed_handler() - battery temp between low and medium
+ * @irq:       interrupt number
+ * @_di:       void pointer that has to address of ab8500_btemp
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_btemp_lowmed_handler(int irq, void *_di)
+{
+	struct ab8500_btemp *di = _di;
+
+	dev_dbg(di->dev, "Battery temperature is between low and medium\n");
+
+	di->events.btemp_lowmed = true;
+	di->events.btemp_medhigh = false;
+	di->events.btemp_high = false;
+	di->events.btemp_low = false;
+	power_supply_changed(&di->btemp_psy);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_btemp_medhigh_handler() - battery temp between medium and high
+ * @irq:       interrupt number
+ * @_di:       void pointer that has to address of ab8500_btemp
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_btemp_medhigh_handler(int irq, void *_di)
+{
+	struct ab8500_btemp *di = _di;
+
+	dev_dbg(di->dev, "Battery temperature is between medium and high\n");
+
+	di->events.btemp_medhigh = true;
+	di->events.btemp_lowmed = false;
+	di->events.btemp_high = false;
+	di->events.btemp_low = false;
+	power_supply_changed(&di->btemp_psy);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_btemp_periodic() - Periodic temperature measurements
+ * @di:		pointer to the ab8500_btemp structure
+ * @enable:	enable or disable periodic temperature measurements
+ *
+ * Starts of stops periodic temperature measurements. Periodic measurements
+ * should only be done when a charger is connected.
+ */
+static void ab8500_btemp_periodic(struct ab8500_btemp *di,
+	bool enable)
+{
+	dev_dbg(di->dev, "Enable periodic temperature measurements: %d\n",
+		enable);
+
+	if (enable)
+		queue_delayed_work(di->btemp_wq, &di->btemp_periodic_work, 0);
+	else
+		cancel_delayed_work_sync(&di->btemp_periodic_work);
+}
+
+/**
+ * ab8500_btemp_get_temp() - get battery temperature
+ * @di:		pointer to the ab8500_btemp structure
+ *
+ * Returns battery temperature
+ */
+static int ab8500_btemp_get_temp(struct ab8500_btemp *di)
+{
+	int temp = 0;
+
+	/*
+	 * The BTEMP events are not reliabe on AB8500 cut2.0
+	 * and prior versions
+	 */
+	switch (di->chip_id) {
+	case AB8500_CUT1P0:
+	case AB8500_CUT1P1:
+	case AB8500_CUT2P0:
+		temp = di->bat_temp * 10;
+
+		break;
+	default:
+		if (di->events.btemp_low) {
+			if (temp > di->btemp_ranges.btemp_low_limit)
+				temp = di->btemp_ranges.btemp_low_limit;
+			else
+				temp = di->bat_temp * 10;
+		} else if (di->events.btemp_high) {
+			if (temp < di->btemp_ranges.btemp_high_limit)
+				temp = di->btemp_ranges.btemp_high_limit;
+			else
+				temp = di->bat_temp * 10;
+		} else if (di->events.btemp_lowmed) {
+			if (temp > di->btemp_ranges.btemp_med_limit)
+				temp = di->btemp_ranges.btemp_med_limit;
+			else
+				temp = di->bat_temp * 10;
+		} else if (di->events.btemp_medhigh) {
+			if (temp < di->btemp_ranges.btemp_med_limit)
+				temp = di->btemp_ranges.btemp_med_limit;
+			else
+				temp = di->bat_temp * 10;
+		} else
+			temp = di->bat_temp * 10;
+
+		break;
+	}
+	return temp;
+}
+
+/**
+ * ab8500_btemp_get_property() - get the btemp properties
+ * @psy:        pointer to the power_supply structure
+ * @psp:        pointer to the power_supply_property structure
+ * @val:        pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the btemp
+ * properties by reading the sysfs files.
+ * online:	presence of the battery
+ * present:	presence of the battery
+ * technology:	battery technology
+ * temp:	battery temperature
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_btemp_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct ab8500_btemp *di;
+
+	di = to_ab8500_btemp_device_info(psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_PRESENT:
+	case POWER_SUPPLY_PROP_ONLINE:
+		if (di->events.batt_rem)
+			val->intval = 0;
+		else
+			val->intval = 1;
+		break;
+	case POWER_SUPPLY_PROP_TECHNOLOGY:
+		val->intval = di->bat->bat_type[di->bat->batt_id].name;
+		break;
+	case POWER_SUPPLY_PROP_TEMP:
+		val->intval = ab8500_btemp_get_temp(di);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ab8500_btemp_get_ext_psy_data(struct device *dev, void *data)
+{
+	struct power_supply *psy;
+	struct power_supply *ext;
+	struct ab8500_btemp *di;
+	union power_supply_propval ret;
+	int i, j;
+	bool psy_found = false;
+
+	psy = (struct power_supply *)data;
+	ext = dev_get_drvdata(dev);
+	di = to_ab8500_btemp_device_info(psy);
+
+	/*
+	 * For all psy where the name of your driver
+	 * appears in any supplied_to
+	 */
+	for (i = 0; i < ext->num_supplicants; i++) {
+		if (!strcmp(ext->supplied_to[i], psy->name))
+			psy_found = true;
+	}
+
+	if (!psy_found)
+		return 0;
+
+	/* Go through all properties for the psy */
+	for (j = 0; j < ext->num_properties; j++) {
+		enum power_supply_property prop;
+		prop = ext->properties[j];
+
+		if (ext->get_property(ext, prop, &ret))
+			continue;
+
+		switch (prop) {
+		case POWER_SUPPLY_PROP_PRESENT:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_MAINS:
+				/* AC disconnected */
+				if (!ret.intval && di->events.ac_conn) {
+					di->events.ac_conn = false;
+					if (!di->events.usb_conn)
+						ab8500_btemp_periodic(di,
+							false);
+				}
+				/* AC connected */
+				else if (ret.intval && !di->events.ac_conn) {
+					di->events.ac_conn = true;
+					if (!di->events.usb_conn)
+						ab8500_btemp_periodic(di, true);
+				}
+				break;
+			case POWER_SUPPLY_TYPE_USB:
+				/* USB disconnected */
+				if (!ret.intval && di->events.usb_conn) {
+					di->events.usb_conn = false;
+					if (!di->events.ac_conn)
+						ab8500_btemp_periodic(di,
+							false);
+				}
+				/* USB connected */
+				else if (ret.intval && !di->events.usb_conn) {
+					di->events.usb_conn = true;
+					if (!di->events.ac_conn)
+						ab8500_btemp_periodic(di, true);
+				}
+				break;
+			default:
+				break;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+/**
+ * ab8500_btemp_external_power_changed() - callback for power supply changes
+ * @psy:       pointer to the structure power_supply
+ *
+ * This function is pointing to the function pointer external_power_changed
+ * of the structure power_supply.
+ * This function gets executed when there is a change in the external power
+ * supply to the btemp.
+ */
+static void ab8500_btemp_external_power_changed(struct power_supply *psy)
+{
+	struct ab8500_btemp *di = to_ab8500_btemp_device_info(psy);
+
+	class_for_each_device(power_supply_class, NULL,
+		&di->btemp_psy, ab8500_btemp_get_ext_psy_data);
+}
+
+/* ab8500 btemp driver interrupts and their respective isr */
+static struct ab8500_btemp_interrupts ab8500_btemp_irq[] = {
+	{"BAT_CTRL_INDB", ab8500_btemp_batctrlindb_handler},
+	{"BTEMP_LOW", ab8500_btemp_templow_handler},
+	{"BTEMP_HIGH", ab8500_btemp_temphigh_handler},
+	{"BTEMP_LOW_MEDIUM", ab8500_btemp_lowmed_handler},
+	{"BTEMP_MEDIUM_HIGH", ab8500_btemp_medhigh_handler},
+};
+
+#if defined(CONFIG_PM)
+static int ab8500_btemp_resume(struct platform_device *pdev)
+{
+	struct ab8500_btemp *di = platform_get_drvdata(pdev);
+
+	if (di->events.ac_conn || di->events.usb_conn)
+		ab8500_btemp_periodic(di, true);
+
+	return 0;
+}
+
+static int ab8500_btemp_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct ab8500_btemp *di = platform_get_drvdata(pdev);
+
+	if (di->events.ac_conn || di->events.usb_conn)
+		ab8500_btemp_periodic(di, false);
+
+	return 0;
+}
+#else
+#define ab8500_btemp_suspend      NULL
+#define ab8500_btemp_resume       NULL
+#endif
+
+static int __devexit ab8500_btemp_remove(struct platform_device *pdev)
+{
+	struct ab8500_btemp *di = platform_get_drvdata(pdev);
+	int i, irq;
+
+	/* Disable interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab8500_btemp_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab8500_btemp_irq[i].name);
+		free_irq(irq, di);
+	}
+
+	/* Delete the work queue */
+	destroy_workqueue(di->btemp_wq);
+
+	flush_scheduled_work();
+	power_supply_unregister(&di->btemp_psy);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+
+	return 0;
+}
+
+static int __devinit ab8500_btemp_probe(struct platform_device *pdev)
+{
+	int irq, i, ret = 0;
+	u8 val;
+	struct ab8500_platform_data *plat;
+
+	struct ab8500_btemp *di =
+		kzalloc(sizeof(struct ab8500_btemp), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	/* get parent data */
+	di->dev = &pdev->dev;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
+	di->gpadc = ab8500_gpadc_get();
+
+	plat = dev_get_platdata(di->parent->dev);
+
+	/* get btemp specific platform data */
+	if (!plat->btemp) {
+		dev_err(di->dev, "no btemp platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+	di->pdata = plat->btemp;
+
+	/* get battery specific platform data */
+	if (!plat->battery) {
+		dev_err(di->dev, "no battery platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+	di->bat = plat->battery;
+
+	/* BTEMP supply */
+	di->btemp_psy.name = "ab8500_btemp";
+	di->btemp_psy.type = POWER_SUPPLY_TYPE_BATTERY;
+	di->btemp_psy.properties = ab8500_btemp_props;
+	di->btemp_psy.num_properties = ARRAY_SIZE(ab8500_btemp_props);
+	di->btemp_psy.get_property = ab8500_btemp_get_property;
+	di->btemp_psy.supplied_to = di->pdata->supplied_to;
+	di->btemp_psy.num_supplicants = di->pdata->num_supplicants;
+	di->btemp_psy.external_power_changed =
+		ab8500_btemp_external_power_changed;
+
+
+	/* Create a work queue for the btemp */
+	di->btemp_wq =
+		create_singlethread_workqueue("ab8500_btemp_wq");
+	if (di->btemp_wq == NULL) {
+		dev_err(di->dev, "failed to create work queue\n");
+		goto free_device_info;
+	}
+
+	/* Init work for measuring temperature periodically */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->btemp_periodic_work,
+		ab8500_btemp_periodic_work);
+
+	/* Get Chip ID of the ABB ASIC  */
+	ret = abx500_get_chip_id(di->dev);
+	if (ret < 0) {
+		dev_err(di->dev, "failed to get chip ID\n");
+		goto free_btemp_wq;
+	}
+	di->chip_id = ret;
+	dev_dbg(di->dev, "AB8500 CID is: 0x%02x\n",
+		di->chip_id);
+
+	/* Identify the battery */
+	if (ab8500_btemp_id(di) < 0)
+		dev_warn(di->dev, "failed to identify the battery\n");
+
+	/* Set BTEMP thermal limits. Low and Med are fixed */
+	di->btemp_ranges.btemp_low_limit = BTEMP_THERMAL_LOW_LIMIT;
+	di->btemp_ranges.btemp_med_limit = BTEMP_THERMAL_MED_LIMIT;
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_CHARGER,
+		AB8500_BTEMP_HIGH_TH, &val);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+		goto free_btemp_wq;
+	}
+	switch (val) {
+	case BTEMP_HIGH_TH_57_0:
+	case BTEMP_HIGH_TH_57_1:
+		di->btemp_ranges.btemp_high_limit =
+			BTEMP_THERMAL_HIGH_LIMIT_57;
+		break;
+	case BTEMP_HIGH_TH_52:
+		di->btemp_ranges.btemp_high_limit =
+			BTEMP_THERMAL_HIGH_LIMIT_52;
+		break;
+	case BTEMP_HIGH_TH_62:
+		di->btemp_ranges.btemp_high_limit =
+			BTEMP_THERMAL_HIGH_LIMIT_62;
+		break;
+	}
+
+	/* Measure temperature once initially */
+	di->bat_temp = ab8500_btemp_measure_temp(di);
+
+	/* Register BTEMP power supply class */
+	ret = power_supply_register(di->dev, &di->btemp_psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register BTEMP psy\n");
+		goto free_btemp_wq;
+	}
+
+	/* Register interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab8500_btemp_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab8500_btemp_irq[i].name);
+		ret = request_threaded_irq(irq, NULL, ab8500_btemp_irq[i].isr,
+			IRQF_SHARED | IRQF_NO_SUSPEND,
+			ab8500_btemp_irq[i].name, di);
+
+		if (ret) {
+			dev_err(di->dev, "failed to request %s IRQ %d: %d\n"
+				, ab8500_btemp_irq[i].name, irq, ret);
+			goto free_irq;
+		}
+		dev_dbg(di->dev, "Requested %s IRQ %d: %d\n",
+			ab8500_btemp_irq[i].name, irq, ret);
+	}
+
+	platform_set_drvdata(pdev, di);
+
+	return ret;
+
+free_irq:
+	power_supply_unregister(&di->btemp_psy);
+
+	/* We also have to free all successfully registered irqs */
+	for (i = i - 1; i >= 0; i--) {
+		irq = platform_get_irq_byname(pdev, ab8500_btemp_irq[i].name);
+		free_irq(irq, di);
+	}
+free_btemp_wq:
+	destroy_workqueue(di->btemp_wq);
+free_device_info:
+	kfree(di);
+
+	return ret;
+}
+
+static struct platform_driver ab8500_btemp_driver = {
+	.probe = ab8500_btemp_probe,
+	.remove = __devexit_p(ab8500_btemp_remove),
+	.suspend = ab8500_btemp_suspend,
+	.resume = ab8500_btemp_resume,
+	.driver = {
+		.name = "ab8500-btemp",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab8500_btemp_init(void)
+{
+	return platform_driver_register(&ab8500_btemp_driver);
+}
+
+static void __exit ab8500_btemp_exit(void)
+{
+	platform_driver_unregister(&ab8500_btemp_driver);
+}
+
+subsys_initcall_sync(ab8500_btemp_init);
+module_exit(ab8500_btemp_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Johan Palsson, Karl Komierowski, Arun R Murthy");
+MODULE_ALIAS("platform:ab8500-btemp");
+MODULE_DESCRIPTION("AB8500 battery temperature driver");
diff --git a/drivers/power/ab8500_chargalg.c b/drivers/power/ab8500_chargalg.c
new file mode 100644
index 00000000000..cb56c67475c
--- /dev/null
+++ b/drivers/power/ab8500_chargalg.c
@@ -0,0 +1,1938 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Charging algorithm driver for AB8500
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Johan Palsson <johan.palsson@stericsson.com>
+ * Author: Karl Komierowski <karl.komierowski@stericsson.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/completion.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/ab8500/ux500_chargalg.h>
+#include <linux/mfd/ab8500/bm.h>
+#include <linux/mfd/ab8500/gpadc.h>
+
+/* Watchdog kick interval */
+#define CHG_WD_INTERVAL			(60 * HZ)
+
+/* End-of-charge criteria counter */
+#define EOC_COND_CNT			10
+
+/* Recharge criteria counter */
+#define RCH_COND_CNT			3
+
+#define to_ab8500_chargalg_device_info(x) container_of((x), \
+	struct ab8500_chargalg, chargalg_psy);
+
+enum ab8500_chargers {
+	NO_CHG,
+	AC_CHG,
+	USB_CHG,
+};
+
+struct ab8500_chargalg_charger_info {
+	enum ab8500_chargers conn_chg;
+	enum ab8500_chargers prev_conn_chg;
+	enum ab8500_chargers online_chg;
+	enum ab8500_chargers prev_online_chg;
+	enum ab8500_chargers charger_type;
+	bool usb_chg_ok;
+	bool ac_chg_ok;
+	int usb_volt;
+	int usb_curr;
+	int ac_volt;
+	int ac_curr;
+	int usb_vset;
+	int usb_iset;
+	int ac_vset;
+	int ac_iset;
+};
+
+struct ab8500_chargalg_suspension_status {
+	bool suspended_change;
+	bool ac_suspended;
+	bool usb_suspended;
+};
+
+struct ab8500_chargalg_battery_data {
+	int temp;
+	int volt;
+	int avg_curr;
+	int inst_curr;
+	int percent;
+};
+
+enum ab8500_chargalg_states {
+	STATE_HANDHELD_INIT,
+	STATE_HANDHELD,
+	STATE_CHG_NOT_OK_INIT,
+	STATE_CHG_NOT_OK,
+	STATE_HW_TEMP_PROTECT_INIT,
+	STATE_HW_TEMP_PROTECT,
+	STATE_NORMAL_INIT,
+	STATE_NORMAL,
+	STATE_WAIT_FOR_RECHARGE_INIT,
+	STATE_WAIT_FOR_RECHARGE,
+	STATE_MAINTENANCE_A_INIT,
+	STATE_MAINTENANCE_A,
+	STATE_MAINTENANCE_B_INIT,
+	STATE_MAINTENANCE_B,
+	STATE_TEMP_UNDEROVER_INIT,
+	STATE_TEMP_UNDEROVER,
+	STATE_TEMP_LOWHIGH_INIT,
+	STATE_TEMP_LOWHIGH,
+	STATE_SUSPENDED_INIT,
+	STATE_SUSPENDED,
+	STATE_OVV_PROTECT_INIT,
+	STATE_OVV_PROTECT,
+	STATE_SAFETY_TIMER_EXPIRED_INIT,
+	STATE_SAFETY_TIMER_EXPIRED,
+	STATE_BATT_REMOVED_INIT,
+	STATE_BATT_REMOVED,
+	STATE_WD_EXPIRED_INIT,
+	STATE_WD_EXPIRED,
+};
+
+static const char *states[] = {
+	"HANDHELD_INIT",
+	"HANDHELD",
+	"CHG_NOT_OK_INIT",
+	"CHG_NOT_OK",
+	"HW_TEMP_PROTECT_INIT",
+	"HW_TEMP_PROTECT",
+	"NORMAL_INIT",
+	"NORMAL",
+	"WAIT_FOR_RECHARGE_INIT",
+	"WAIT_FOR_RECHARGE",
+	"MAINTENANCE_A_INIT",
+	"MAINTENANCE_A",
+	"MAINTENANCE_B_INIT",
+	"MAINTENANCE_B",
+	"TEMP_UNDEROVER_INIT",
+	"TEMP_UNDEROVER",
+	"TEMP_LOWHIGH_INIT",
+	"TEMP_LOWHIGH",
+	"SUSPENDED_INIT",
+	"SUSPENDED",
+	"OVV_PROTECT_INIT",
+	"OVV_PROTECT",
+	"SAFETY_TIMER_EXPIRED_INIT",
+	"SAFETY_TIMER_EXPIRED",
+	"BATT_REMOVED_INIT",
+	"BATT_REMOVED",
+	"WD_EXPIRED_INIT",
+	"WD_EXPIRED",
+};
+
+struct ab8500_chargalg_events {
+	bool batt_unknown;
+	bool mainextchnotok;
+	bool batt_ovv;
+	bool batt_rem;
+	bool btemp_underover;
+	bool btemp_lowhigh;
+	bool main_thermal_prot;
+	bool usb_thermal_prot;
+	bool main_ovv;
+	bool vbus_ovv;
+	bool usbchargernotok;
+	bool safety_timer_expired;
+	bool maintenance_timer_expired;
+	bool ac_wd_expired;
+	bool usb_wd_expired;
+	bool ac_cv_active;
+	bool usb_cv_active;
+	bool vbus_collapsed;
+};
+
+/**
+ * struct ab8500_charge_curr_maximization - Charger maximization parameters
+ * @original_iset:	the non optimized/maximised charger current
+ * @current_iset:	the charging current used at this moment
+ * @test_delta_i:	the delta between the current we want to charge and the
+			current that is really going into the battery
+ * @condition_cnt:	number of iterations needed before a new charger current
+			is set
+ * @max_current:	maximum charger current
+ * @wait_cnt:		to avoid too fast current step down in case of charger
+ *			voltage collapse, we insert this delay between step
+ *			down
+ * @level:		tells in how many steps the charging current has been
+			increased
+ */
+struct ab8500_charge_curr_maximization {
+	int original_iset;
+	int current_iset;
+	int test_delta_i;
+	int condition_cnt;
+	int max_current;
+	int wait_cnt;
+	u8 level;
+};
+
+enum maxim_ret {
+	MAXIM_RET_NOACTION,
+	MAXIM_RET_CHANGE,
+	MAXIM_RET_IBAT_TOO_HIGH,
+};
+
+/**
+ * struct ab8500_chargalg - ab8500 Charging algorithm device information
+ * @dev:		pointer to the structure device
+ * @charge_status:	battery operating status
+ * @eoc_cnt:		counter used to determine end-of_charge
+ * @rch_cnt:		counter used to determine start of recharge
+ * @maintenance_chg:	indicate if maintenance charge is active
+ * @t_hyst_norm		temperature hysteresis when the temperature has	been
+ *			over or under normal limits
+ * @t_hyst_lowhigh	temperature hysteresis when the temperature has	been
+ *			over or under the high or low limits
+ * @charge_state:	current state of the charging algorithm
+ * @ccm			charging current maximization parameters
+ * @chg_info:		information about connected charger types
+ * @batt_data:		data of the battery
+ * @susp_status:	current charger suspension status
+ * @parent:		pointer to the struct ab8500
+ * @pdata:		pointer to the ab8500_chargalg platform data
+ * @bat:		pointer to the ab8500_bm platform data
+ * @chargalg_psy:	structure that holds the battery properties exposed by
+ *			the charging algorithm
+ * @events:		structure for information about events triggered
+ * @chargalg_wq:		work queue for running the charging algorithm
+ * @chargalg_periodic_work:	work to run the charging algorithm periodically
+ * @chargalg_wd_work:		work to kick the charger watchdog periodically
+ * @chargalg_work:		work to run the charging algorithm instantly
+ * @safety_timer:		charging safety timer
+ * @maintenance_timer:		maintenance charging timer
+ * @chargalg_kobject:		structure of type kobject
+ */
+struct ab8500_chargalg {
+	struct device *dev;
+	int charge_status;
+	int eoc_cnt;
+	int rch_cnt;
+	bool maintenance_chg;
+	int t_hyst_norm;
+	int t_hyst_lowhigh;
+	enum ab8500_chargalg_states charge_state;
+	struct ab8500_charge_curr_maximization ccm;
+	struct ab8500_chargalg_charger_info chg_info;
+	struct ab8500_chargalg_battery_data batt_data;
+	struct ab8500_chargalg_suspension_status susp_status;
+	struct ab8500 *parent;
+	struct ab8500_chargalg_platform_data *pdata;
+	struct ab8500_bm_data *bat;
+	struct power_supply chargalg_psy;
+	struct ux500_charger *ac_chg;
+	struct ux500_charger *usb_chg;
+	struct ab8500_chargalg_events events;
+	struct workqueue_struct *chargalg_wq;
+	struct delayed_work chargalg_periodic_work;
+	struct delayed_work chargalg_wd_work;
+	struct work_struct chargalg_work;
+	struct timer_list safety_timer;
+	struct timer_list maintenance_timer;
+	struct kobject chargalg_kobject;
+};
+
+/* Main battery properties */
+static enum power_supply_property ab8500_chargalg_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_HEALTH,
+};
+
+/**
+ * ab8500_chargalg_safety_timer_expired() - Expiration of the safety timer
+ * @data:	pointer to the ab8500_chargalg structure
+ *
+ * This function gets called when the safety timer for the charger
+ * expires
+ */
+static void ab8500_chargalg_safety_timer_expired(unsigned long data)
+{
+	struct ab8500_chargalg *di = (struct ab8500_chargalg *) data;
+	dev_err(di->dev, "Safety timer expired\n");
+	di->events.safety_timer_expired = true;
+
+	/* Trigger execution of the algorithm instantly */
+	queue_work(di->chargalg_wq, &di->chargalg_work);
+}
+
+/**
+ * ab8500_chargalg_maintenance_timer_expired() - Expiration of
+ * the maintenance timer
+ * @i:		pointer to the ab8500_chargalg structure
+ *
+ * This function gets called when the maintenence timer
+ * expires
+ */
+static void ab8500_chargalg_maintenance_timer_expired(unsigned long data)
+{
+
+	struct ab8500_chargalg *di = (struct ab8500_chargalg *) data;
+	dev_dbg(di->dev, "Maintenance timer expired\n");
+	di->events.maintenance_timer_expired = true;
+
+	/* Trigger execution of the algorithm instantly */
+	queue_work(di->chargalg_wq, &di->chargalg_work);
+}
+
+/**
+ * ab8500_chargalg_state_to() - Change charge state
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * This function gets called when a charge state change should occur
+ */
+static void ab8500_chargalg_state_to(struct ab8500_chargalg *di,
+	enum ab8500_chargalg_states state)
+{
+	dev_dbg(di->dev,
+		"State changed: %s (From state: [%d] %s =to=> [%d] %s )\n",
+		di->charge_state == state ? "NO" : "YES",
+		di->charge_state,
+		states[di->charge_state],
+		state,
+		states[state]);
+
+	di->charge_state = state;
+}
+
+/**
+ * ab8500_chargalg_check_charger_connection() - Check charger connection change
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * This function will check if there is a change in the charger connection
+ * and change charge state accordingly. AC has precedence over USB.
+ */
+static int ab8500_chargalg_check_charger_connection(struct ab8500_chargalg *di)
+{
+	if (di->chg_info.conn_chg != di->chg_info.prev_conn_chg ||
+		di->susp_status.suspended_change) {
+		/*
+		 * Charger state changed or suspension
+		 * has changed since last update
+		 */
+		if ((di->chg_info.conn_chg & AC_CHG) &&
+			!di->susp_status.ac_suspended) {
+			dev_dbg(di->dev, "Charging source is AC\n");
+			if (di->chg_info.charger_type != AC_CHG) {
+				di->chg_info.charger_type = AC_CHG;
+				ab8500_chargalg_state_to(di, STATE_NORMAL_INIT);
+			}
+		} else if ((di->chg_info.conn_chg & USB_CHG) &&
+			!di->susp_status.usb_suspended) {
+			dev_dbg(di->dev, "Charging source is USB\n");
+			di->chg_info.charger_type = USB_CHG;
+			ab8500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		} else if (di->chg_info.conn_chg &&
+			(di->susp_status.ac_suspended ||
+			di->susp_status.usb_suspended)) {
+			dev_dbg(di->dev, "Charging is suspended\n");
+			di->chg_info.charger_type = NO_CHG;
+			ab8500_chargalg_state_to(di, STATE_SUSPENDED_INIT);
+		} else {
+			dev_dbg(di->dev, "Charging source is OFF\n");
+			di->chg_info.charger_type = NO_CHG;
+			ab8500_chargalg_state_to(di, STATE_HANDHELD_INIT);
+		}
+		di->chg_info.prev_conn_chg = di->chg_info.conn_chg;
+		di->susp_status.suspended_change = false;
+	}
+	return di->chg_info.conn_chg;
+}
+
+/**
+ * ab8500_chargalg_start_safety_timer() - Start charging safety timer
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * The safety timer is used to avoid overcharging of old or bad batteries.
+ * There are different timers for AC and USB
+ */
+static void ab8500_chargalg_start_safety_timer(struct ab8500_chargalg *di)
+{
+	unsigned long timer_expiration = 0;
+
+	switch (di->chg_info.charger_type) {
+	case AC_CHG:
+		timer_expiration =
+		round_jiffies(jiffies +
+			(di->bat->main_safety_tmr_h * 3600 * HZ));
+		break;
+
+	case USB_CHG:
+		timer_expiration =
+		round_jiffies(jiffies +
+			(di->bat->usb_safety_tmr_h * 3600 * HZ));
+		break;
+
+	default:
+		dev_err(di->dev, "Unknown charger to charge from\n");
+		break;
+	}
+
+	di->events.safety_timer_expired = false;
+	di->safety_timer.expires = timer_expiration;
+	if (!timer_pending(&di->safety_timer))
+		add_timer(&di->safety_timer);
+	else
+		mod_timer(&di->safety_timer, timer_expiration);
+}
+
+/**
+ * ab8500_chargalg_stop_safety_timer() - Stop charging safety timer
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * The safety timer is stopped whenever the NORMAL state is exited
+ */
+static void ab8500_chargalg_stop_safety_timer(struct ab8500_chargalg *di)
+{
+	di->events.safety_timer_expired = false;
+	del_timer(&di->safety_timer);
+}
+
+/**
+ * ab8500_chargalg_start_maintenance_timer() - Start charging maintenance timer
+ * @di:		pointer to the ab8500_chargalg structure
+ * @duration:	duration of ther maintenance timer in hours
+ *
+ * The maintenance timer is used to maintain the charge in the battery once
+ * the battery is considered full. These timers are chosen to match the
+ * discharge curve of the battery
+ */
+static void ab8500_chargalg_start_maintenance_timer(struct ab8500_chargalg *di,
+	int duration)
+{
+	unsigned long timer_expiration;
+
+	/* Convert from hours to jiffies */
+	timer_expiration = round_jiffies(jiffies + (duration * 3600 * HZ));
+
+	di->events.maintenance_timer_expired = false;
+	di->maintenance_timer.expires = timer_expiration;
+	if (!timer_pending(&di->maintenance_timer))
+		add_timer(&di->maintenance_timer);
+	else
+		mod_timer(&di->maintenance_timer, timer_expiration);
+}
+
+/**
+ * ab8500_chargalg_stop_maintenance_timer() - Stop maintenance timer
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * The maintenance timer is stopped whenever maintenance ends or when another
+ * state is entered
+ */
+static void ab8500_chargalg_stop_maintenance_timer(struct ab8500_chargalg *di)
+{
+	di->events.maintenance_timer_expired = false;
+	del_timer(&di->maintenance_timer);
+}
+
+/**
+ * ab8500_chargalg_kick_watchdog() - Kick charger watchdog
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * The charger watchdog have to be kicked periodically whenever the charger is
+ * on, else the ABB will reset the system
+ */
+static int ab8500_chargalg_kick_watchdog(struct ab8500_chargalg *di)
+{
+	/* Check if charger exists and kick watchdog if charging */
+	if (di->ac_chg && di->ac_chg->ops.kick_wd &&
+			di->chg_info.online_chg & AC_CHG)
+		return di->ac_chg->ops.kick_wd(di->ac_chg);
+	else if (di->usb_chg && di->usb_chg->ops.kick_wd &&
+			di->chg_info.online_chg & USB_CHG)
+		return di->usb_chg->ops.kick_wd(di->usb_chg);
+
+	return -ENXIO;
+}
+
+/**
+ * ab8500_chargalg_ac_en() - Turn on/off the AC charger
+ * @di:		pointer to the ab8500_chargalg structure
+ * @enable:	charger on/off
+ * @vset:	requested charger output voltage
+ * @iset:	requested charger output current
+ *
+ * The AC charger will be turned on/off with the requested charge voltage and
+ * current
+ */
+static int ab8500_chargalg_ac_en(struct ab8500_chargalg *di, int enable,
+	int vset, int iset)
+{
+	if (!di->ac_chg || !di->ac_chg->ops.enable)
+		return -ENXIO;
+
+	/* Select maximum of what both the charger and the battery supports */
+	if (di->ac_chg->max_out_volt)
+		vset = min(vset, di->ac_chg->max_out_volt);
+	if (di->ac_chg->max_out_curr)
+		iset = min(iset, di->ac_chg->max_out_curr);
+
+	di->chg_info.ac_iset = iset;
+	di->chg_info.ac_vset = vset;
+
+	return di->ac_chg->ops.enable(di->ac_chg, enable, vset, iset);
+}
+
+/**
+ * ab8500_chargalg_usb_en() - Turn on/off the USB charger
+ * @di:		pointer to the ab8500_chargalg structure
+ * @enable:	charger on/off
+ * @vset:	requested charger output voltage
+ * @iset:	requested charger output current
+ *
+ * The USB charger will be turned on/off with the requested charge voltage and
+ * current
+ */
+static int ab8500_chargalg_usb_en(struct ab8500_chargalg *di, int enable,
+	int vset, int iset)
+{
+	if (!di->usb_chg || !di->usb_chg->ops.enable)
+		return -ENXIO;
+
+	/* Select maximum of what both the charger and the battery supports */
+	if (di->usb_chg->max_out_volt)
+		vset = min(vset, di->usb_chg->max_out_volt);
+	if (di->usb_chg->max_out_curr)
+		iset = min(iset, di->usb_chg->max_out_curr);
+
+	di->chg_info.usb_iset = iset;
+	di->chg_info.usb_vset = vset;
+
+	return di->usb_chg->ops.enable(di->usb_chg, enable, vset, iset);
+}
+
+/**
+ * ab8500_chargalg_update_chg_curr() - Update charger current
+ * @di:		pointer to the ab8500_chargalg structure
+ * @iset:	requested charger output current
+ *
+ * The charger output current will be updated for the charger
+ * that is currently in use
+ */
+static int ab8500_chargalg_update_chg_curr(struct ab8500_chargalg *di,
+		int iset)
+{
+	/* Check if charger exists and update current if charging */
+	if (di->ac_chg && di->ac_chg->ops.update_curr &&
+			di->chg_info.charger_type & AC_CHG) {
+		/*
+		 * Select maximum of what both the charger
+		 * and the battery supports
+		 */
+		if (di->ac_chg->max_out_curr)
+			iset = min(iset, di->ac_chg->max_out_curr);
+
+		di->chg_info.ac_iset = iset;
+
+		return di->ac_chg->ops.update_curr(di->ac_chg, iset);
+	} else if (di->usb_chg && di->usb_chg->ops.update_curr &&
+			di->chg_info.charger_type & USB_CHG) {
+		/*
+		 * Select maximum of what both the charger
+		 * and the battery supports
+		 */
+		if (di->usb_chg->max_out_curr)
+			iset = min(iset, di->usb_chg->max_out_curr);
+
+		di->chg_info.usb_iset = iset;
+
+		return di->usb_chg->ops.update_curr(di->usb_chg, iset);
+	}
+
+	return -ENXIO;
+}
+
+/**
+ * ab8500_chargalg_stop_charging() - Stop charging
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * This function is called from any state where charging should be stopped.
+ * All charging is disabled and all status parameters and timers are changed
+ * accordingly
+ */
+static void ab8500_chargalg_stop_charging(struct ab8500_chargalg *di)
+{
+	ab8500_chargalg_ac_en(di, false, 0, 0);
+	ab8500_chargalg_usb_en(di, false, 0, 0);
+	ab8500_chargalg_stop_safety_timer(di);
+	ab8500_chargalg_stop_maintenance_timer(di);
+	di->charge_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+	di->maintenance_chg = false;
+	cancel_delayed_work(&di->chargalg_wd_work);
+	power_supply_changed(&di->chargalg_psy);
+}
+
+/**
+ * ab8500_chargalg_hold_charging() - Pauses charging
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * This function is called in the case where maintenance charging has been
+ * disabled and instead a battery voltage mode is entered to check when the
+ * battery voltage has reached a certain recharge voltage
+ */
+static void ab8500_chargalg_hold_charging(struct ab8500_chargalg *di)
+{
+	ab8500_chargalg_ac_en(di, false, 0, 0);
+	ab8500_chargalg_usb_en(di, false, 0, 0);
+	ab8500_chargalg_stop_safety_timer(di);
+	ab8500_chargalg_stop_maintenance_timer(di);
+	di->charge_status = POWER_SUPPLY_STATUS_CHARGING;
+	di->maintenance_chg = false;
+	cancel_delayed_work(&di->chargalg_wd_work);
+	power_supply_changed(&di->chargalg_psy);
+}
+
+/**
+ * ab8500_chargalg_start_charging() - Start the charger
+ * @di:		pointer to the ab8500_chargalg structure
+ * @vset:	requested charger output voltage
+ * @iset:	requested charger output current
+ *
+ * A charger will be enabled depending on the requested charger type that was
+ * detected previously.
+ */
+static void ab8500_chargalg_start_charging(struct ab8500_chargalg *di,
+	int vset, int iset)
+{
+	switch (di->chg_info.charger_type) {
+	case AC_CHG:
+		dev_dbg(di->dev,
+			"AC parameters: Vset %d, Ich %d\n", vset, iset);
+		ab8500_chargalg_usb_en(di, false, 0, 0);
+		ab8500_chargalg_ac_en(di, true, vset, iset);
+		break;
+
+	case USB_CHG:
+		dev_dbg(di->dev,
+			"USB parameters: Vset %d, Ich %d\n", vset, iset);
+		ab8500_chargalg_ac_en(di, false, 0, 0);
+		ab8500_chargalg_usb_en(di, true, vset, iset);
+		break;
+
+	default:
+		dev_err(di->dev, "Unknown charger to charge from\n");
+		break;
+	}
+}
+
+/**
+ * ab8500_chargalg_check_temp() - Check battery temperature ranges
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * The battery temperature is checked against the predefined limits and the
+ * charge state is changed accordingly
+ */
+static void ab8500_chargalg_check_temp(struct ab8500_chargalg *di)
+{
+	if (di->batt_data.temp > (di->bat->temp_low + di->t_hyst_norm) &&
+		di->batt_data.temp < (di->bat->temp_high - di->t_hyst_norm)) {
+		/* Temp OK! */
+		di->events.btemp_underover = false;
+		di->events.btemp_lowhigh = false;
+		di->t_hyst_norm = 0;
+		di->t_hyst_lowhigh = 0;
+	} else {
+		if (((di->batt_data.temp >= di->bat->temp_high) &&
+			(di->batt_data.temp <
+				(di->bat->temp_over - di->t_hyst_lowhigh))) ||
+			((di->batt_data.temp >
+				(di->bat->temp_under + di->t_hyst_lowhigh)) &&
+			(di->batt_data.temp <= di->bat->temp_low))) {
+			/* TEMP minor!!!!! */
+			di->events.btemp_underover = false;
+			di->events.btemp_lowhigh = true;
+			di->t_hyst_norm = di->bat->temp_hysteresis;
+			di->t_hyst_lowhigh = 0;
+		} else if (di->batt_data.temp <= di->bat->temp_under ||
+			di->batt_data.temp >= di->bat->temp_over) {
+			/* TEMP major!!!!! */
+			di->events.btemp_underover = true;
+			di->events.btemp_lowhigh = false;
+			di->t_hyst_norm = 0;
+			di->t_hyst_lowhigh = di->bat->temp_hysteresis;
+		} else {
+		/* Within hysteresis */
+		dev_dbg(di->dev, "Within hysteresis limit temp: %d "
+				"hyst_lowhigh %d, hyst normal %d\n",
+				di->batt_data.temp, di->t_hyst_lowhigh,
+				di->t_hyst_norm);
+		}
+	}
+}
+
+/**
+ * ab8500_chargalg_check_charger_voltage() - Check charger voltage
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * Charger voltage is checked against maximum limit
+ */
+static void ab8500_chargalg_check_charger_voltage(struct ab8500_chargalg *di)
+{
+	if (di->chg_info.usb_volt > di->bat->chg_params->usb_volt_max)
+		di->chg_info.usb_chg_ok = false;
+	else
+		di->chg_info.usb_chg_ok = true;
+
+	if (di->chg_info.ac_volt > di->bat->chg_params->ac_volt_max)
+		di->chg_info.ac_chg_ok = false;
+	else
+		di->chg_info.ac_chg_ok = true;
+
+}
+
+/**
+ * ab8500_chargalg_end_of_charge() - Check if end-of-charge criteria is fulfilled
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * End-of-charge criteria is fulfilled when the battery voltage is above a
+ * certain limit and the battery current is below a certain limit for a
+ * predefined number of consecutive seconds. If true, the battery is full
+ */
+static void ab8500_chargalg_end_of_charge(struct ab8500_chargalg *di)
+{
+	if (di->charge_status == POWER_SUPPLY_STATUS_CHARGING &&
+		di->charge_state == STATE_NORMAL &&
+		!di->maintenance_chg &&	(di->batt_data.volt >=
+		di->bat->bat_type[di->bat->batt_id].termination_vol ||
+		di->events.usb_cv_active || di->events.ac_cv_active) &&
+		di->batt_data.avg_curr <
+		di->bat->bat_type[di->bat->batt_id].termination_curr &&
+		di->batt_data.avg_curr > 0) {
+		if (++di->eoc_cnt >= EOC_COND_CNT) {
+			di->eoc_cnt = 0;
+			di->charge_status = POWER_SUPPLY_STATUS_FULL;
+			di->maintenance_chg = true;
+			dev_dbg(di->dev, "EOC reached!\n");
+			power_supply_changed(&di->chargalg_psy);
+		} else {
+			dev_dbg(di->dev,
+				" EOC limit reached for the %d"
+				" time, out of %d before EOC\n",
+				di->eoc_cnt,
+				EOC_COND_CNT);
+		}
+	} else {
+		di->eoc_cnt = 0;
+	}
+}
+
+static void init_maxim_chg_curr(struct ab8500_chargalg *di)
+{
+	di->ccm.original_iset =
+		di->bat->bat_type[di->bat->batt_id].normal_cur_lvl;
+	di->ccm.current_iset =
+		di->bat->bat_type[di->bat->batt_id].normal_cur_lvl;
+	di->ccm.test_delta_i = di->bat->maxi->charger_curr_step;
+	di->ccm.max_current = di->bat->maxi->chg_curr;
+	di->ccm.condition_cnt = di->bat->maxi->wait_cycles;
+	di->ccm.level = 0;
+}
+
+/**
+ * ab8500_chargalg_chg_curr_maxim - increases the charger current to
+ *			compensate for the system load
+ * @di		pointer to the ab8500_chargalg structure
+ *
+ * This maximization function is used to raise the charger current to get the
+ * battery current as close to the optimal value as possible. The battery
+ * current during charging is affected by the system load
+ */
+static enum maxim_ret ab8500_chargalg_chg_curr_maxim(struct ab8500_chargalg *di)
+{
+	int delta_i;
+
+	if (!di->bat->maxi->ena_maxi)
+		return MAXIM_RET_NOACTION;
+
+	delta_i = di->ccm.original_iset - di->batt_data.inst_curr;
+
+	if (di->events.vbus_collapsed) {
+		dev_dbg(di->dev, "Charger voltage has collapsed %d\n",
+				di->ccm.wait_cnt);
+		if (di->ccm.wait_cnt == 0) {
+			dev_dbg(di->dev, "lowering current\n");
+			di->ccm.wait_cnt++;
+			di->ccm.condition_cnt = di->bat->maxi->wait_cycles;
+			di->ccm.max_current =
+				di->ccm.current_iset - di->ccm.test_delta_i;
+			di->ccm.current_iset = di->ccm.max_current;
+			di->ccm.level--;
+			return MAXIM_RET_CHANGE;
+		} else {
+			dev_dbg(di->dev, "waiting\n");
+			/* Let's go in here twice before lowering curr again */
+			di->ccm.wait_cnt = (di->ccm.wait_cnt + 1) % 3;
+			return MAXIM_RET_NOACTION;
+		}
+	}
+
+	di->ccm.wait_cnt = 0;
+
+	if ((di->batt_data.inst_curr > di->ccm.original_iset)) {
+		dev_dbg(di->dev, " Maximization Ibat (%dmA) too high"
+			" (limit %dmA) (current iset: %dmA)!\n",
+			di->batt_data.inst_curr, di->ccm.original_iset,
+			di->ccm.current_iset);
+
+		if (di->ccm.current_iset == di->ccm.original_iset)
+			return MAXIM_RET_NOACTION;
+
+		di->ccm.condition_cnt =	di->bat->maxi->wait_cycles;
+		di->ccm.current_iset = di->ccm.original_iset;
+		di->ccm.level = 0;
+
+		return MAXIM_RET_IBAT_TOO_HIGH;
+	}
+
+	if (delta_i > di->ccm.test_delta_i &&
+		(di->ccm.current_iset + di->ccm.test_delta_i) <
+		di->ccm.max_current) {
+		if (di->ccm.condition_cnt-- == 0) {
+			/* Increse the iset with cco.test_delta_i */
+			di->ccm.condition_cnt =	di->bat->maxi->wait_cycles;
+			di->ccm.current_iset += di->ccm.test_delta_i;
+			di->ccm.level++;
+			dev_dbg(di->dev, " Maximization needed, increase"
+				" with %d mA to %dmA (Optimal ibat: %d)"
+				" Level %d\n",
+				di->ccm.test_delta_i,
+				di->ccm.current_iset,
+				di->ccm.original_iset,
+				di->ccm.level);
+			return MAXIM_RET_CHANGE;
+		} else {
+			return MAXIM_RET_NOACTION;
+		}
+	}  else {
+		di->ccm.condition_cnt =	di->bat->maxi->wait_cycles;
+		return MAXIM_RET_NOACTION;
+	}
+}
+
+static void handle_maxim_chg_curr(struct ab8500_chargalg *di)
+{
+	enum maxim_ret ret;
+	int result;
+
+	ret = ab8500_chargalg_chg_curr_maxim(di);
+	switch (ret) {
+	case MAXIM_RET_CHANGE:
+		result = ab8500_chargalg_update_chg_curr(di,
+			di->ccm.current_iset);
+		if (result)
+			dev_err(di->dev, "failed to set chg curr\n");
+		break;
+	case MAXIM_RET_IBAT_TOO_HIGH:
+		result = ab8500_chargalg_update_chg_curr(di,
+			di->bat->bat_type[di->bat->batt_id].normal_cur_lvl);
+		if (result)
+			dev_err(di->dev, "failed to set chg curr\n");
+		break;
+
+	case MAXIM_RET_NOACTION:
+	default:
+		/* Do nothing..*/
+		break;
+	}
+}
+
+static int ab8500_chargalg_get_ext_psy_data(struct device *dev, void *data)
+{
+	struct power_supply *psy;
+	struct power_supply *ext;
+	struct ab8500_chargalg *di;
+	union power_supply_propval ret;
+	int i, j;
+	bool psy_found = false;
+
+	psy = (struct power_supply *)data;
+	ext = dev_get_drvdata(dev);
+	di = to_ab8500_chargalg_device_info(psy);
+
+	/* For all psy where the driver name appears in any supplied_to */
+	for (i = 0; i < ext->num_supplicants; i++) {
+		if (!strcmp(ext->supplied_to[i], psy->name))
+			psy_found = true;
+	}
+
+	if (!psy_found)
+		return 0;
+
+	/* Go through all properties for the psy */
+	for (j = 0; j < ext->num_properties; j++) {
+		enum power_supply_property prop;
+		prop = ext->properties[j];
+
+		/* Initialize chargers if not already done */
+		if (!di->ac_chg &&
+			ext->type == POWER_SUPPLY_TYPE_MAINS)
+			di->ac_chg = psy_to_ux500_charger(ext);
+		else if (!di->usb_chg &&
+			ext->type == POWER_SUPPLY_TYPE_USB)
+			di->usb_chg = psy_to_ux500_charger(ext);
+
+		if (ext->get_property(ext, prop, &ret))
+			continue;
+
+		switch (prop) {
+		case POWER_SUPPLY_PROP_PRESENT:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				/* Battery present */
+				if (ret.intval)
+					di->events.batt_rem = false;
+				/* Battery removed */
+				else
+					di->events.batt_rem = true;
+				break;
+			case POWER_SUPPLY_TYPE_MAINS:
+				/* AC disconnected */
+				if (!ret.intval &&
+					(di->chg_info.conn_chg & AC_CHG)) {
+					di->chg_info.prev_conn_chg =
+						di->chg_info.conn_chg;
+					di->chg_info.conn_chg &= ~AC_CHG;
+				}
+				/* AC connected */
+				else if (ret.intval &&
+					!(di->chg_info.conn_chg & AC_CHG)) {
+					di->chg_info.prev_conn_chg =
+						di->chg_info.conn_chg;
+					di->chg_info.conn_chg |= AC_CHG;
+				}
+				break;
+			case POWER_SUPPLY_TYPE_USB:
+				/* USB disconnected */
+				if (!ret.intval &&
+					(di->chg_info.conn_chg & USB_CHG)) {
+					di->chg_info.prev_conn_chg =
+						di->chg_info.conn_chg;
+					di->chg_info.conn_chg &= ~USB_CHG;
+				}
+				/* USB connected */
+				else if (ret.intval &&
+					!(di->chg_info.conn_chg & USB_CHG)) {
+					di->chg_info.prev_conn_chg =
+						di->chg_info.conn_chg;
+					di->chg_info.conn_chg |= USB_CHG;
+				}
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_ONLINE:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				break;
+			case POWER_SUPPLY_TYPE_MAINS:
+				/* AC offline */
+				if (!ret.intval &&
+					(di->chg_info.online_chg & AC_CHG)) {
+					di->chg_info.prev_online_chg =
+						di->chg_info.online_chg;
+					di->chg_info.online_chg &= ~AC_CHG;
+				}
+				/* AC online */
+				else if (ret.intval &&
+					!(di->chg_info.online_chg & AC_CHG)) {
+					di->chg_info.prev_online_chg =
+						di->chg_info.online_chg;
+					di->chg_info.online_chg |= AC_CHG;
+					queue_delayed_work(di->chargalg_wq,
+						&di->chargalg_wd_work, 0);
+				}
+				break;
+			case POWER_SUPPLY_TYPE_USB:
+				/* USB offline */
+				if (!ret.intval &&
+					(di->chg_info.online_chg & USB_CHG)) {
+					di->chg_info.prev_online_chg =
+						di->chg_info.online_chg;
+					di->chg_info.online_chg &= ~USB_CHG;
+				}
+				/* USB online */
+				else if (ret.intval &&
+					!(di->chg_info.online_chg & USB_CHG)) {
+					di->chg_info.prev_online_chg =
+						di->chg_info.online_chg;
+					di->chg_info.online_chg |= USB_CHG;
+					queue_delayed_work(di->chargalg_wq,
+						&di->chargalg_wd_work, 0);
+				}
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_HEALTH:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				break;
+			case POWER_SUPPLY_TYPE_MAINS:
+				switch (ret.intval) {
+				case POWER_SUPPLY_HEALTH_UNSPEC_FAILURE:
+					di->events.mainextchnotok = true;
+					di->events.main_thermal_prot = false;
+					di->events.main_ovv = false;
+					di->events.ac_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_DEAD:
+					di->events.ac_wd_expired = true;
+					di->events.mainextchnotok = false;
+					di->events.main_ovv = false;
+					di->events.main_thermal_prot = false;
+					break;
+				case POWER_SUPPLY_HEALTH_COLD:
+				case POWER_SUPPLY_HEALTH_OVERHEAT:
+					di->events.main_thermal_prot = true;
+					di->events.mainextchnotok = false;
+					di->events.main_ovv = false;
+					di->events.ac_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_OVERVOLTAGE:
+					di->events.main_ovv = true;
+					di->events.mainextchnotok = false;
+					di->events.main_thermal_prot = false;
+					di->events.ac_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_GOOD:
+					di->events.main_thermal_prot = false;
+					di->events.mainextchnotok = false;
+					di->events.main_ovv = false;
+					di->events.ac_wd_expired = false;
+					break;
+				default:
+					break;
+				}
+				break;
+
+			case POWER_SUPPLY_TYPE_USB:
+				switch (ret.intval) {
+				case POWER_SUPPLY_HEALTH_UNSPEC_FAILURE:
+					di->events.usbchargernotok = true;
+					di->events.usb_thermal_prot = false;
+					di->events.vbus_ovv = false;
+					di->events.usb_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_DEAD:
+					di->events.usb_wd_expired = true;
+					di->events.usbchargernotok = false;
+					di->events.usb_thermal_prot = false;
+					di->events.vbus_ovv = false;
+					break;
+				case POWER_SUPPLY_HEALTH_COLD:
+				case POWER_SUPPLY_HEALTH_OVERHEAT:
+					di->events.usb_thermal_prot = true;
+					di->events.usbchargernotok = false;
+					di->events.vbus_ovv = false;
+					di->events.usb_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_OVERVOLTAGE:
+					di->events.vbus_ovv = true;
+					di->events.usbchargernotok = false;
+					di->events.usb_thermal_prot = false;
+					di->events.usb_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_GOOD:
+					di->events.usbchargernotok = false;
+					di->events.usb_thermal_prot = false;
+					di->events.vbus_ovv = false;
+					di->events.usb_wd_expired = false;
+					break;
+				default:
+					break;
+				}
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				di->batt_data.volt = ret.intval / 1000;
+				break;
+			case POWER_SUPPLY_TYPE_MAINS:
+				di->chg_info.ac_volt = ret.intval / 1000;
+				break;
+			case POWER_SUPPLY_TYPE_USB:
+				di->chg_info.usb_volt = ret.intval / 1000;
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_VOLTAGE_AVG:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_MAINS:
+				/* AVG is used to indicate when we are
+				 * in CV mode */
+				if (ret.intval)
+					di->events.ac_cv_active = true;
+				else
+					di->events.ac_cv_active = false;
+
+				break;
+			case POWER_SUPPLY_TYPE_USB:
+				/* AVG is used to indicate when we are
+				 * in CV mode */
+				if (ret.intval)
+					di->events.usb_cv_active = true;
+				else
+					di->events.usb_cv_active = false;
+
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_TECHNOLOGY:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				if (ret.intval)
+					di->events.batt_unknown = false;
+				else
+					di->events.batt_unknown = true;
+
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_TEMP:
+			di->batt_data.temp = ret.intval / 10;
+			break;
+
+		case POWER_SUPPLY_PROP_CURRENT_NOW:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_MAINS:
+					di->chg_info.ac_curr =
+						ret.intval / 1000;
+					break;
+			case POWER_SUPPLY_TYPE_USB:
+					di->chg_info.usb_curr =
+						ret.intval / 1000;
+				break;
+			case POWER_SUPPLY_TYPE_BATTERY:
+				di->batt_data.inst_curr = ret.intval / 1000;
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_CURRENT_AVG:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				di->batt_data.avg_curr = ret.intval / 1000;
+				break;
+			case POWER_SUPPLY_TYPE_USB:
+				if (ret.intval)
+					di->events.vbus_collapsed = true;
+				else
+					di->events.vbus_collapsed = false;
+				break;
+			default:
+				break;
+			}
+			break;
+		case POWER_SUPPLY_PROP_CAPACITY:
+			di->batt_data.percent = ret.intval;
+			break;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+/**
+ * ab8500_chargalg_external_power_changed() - callback for power supply changes
+ * @psy:       pointer to the structure power_supply
+ *
+ * This function is the entry point of the pointer external_power_changed
+ * of the structure power_supply.
+ * This function gets executed when there is a change in any external power
+ * supply that this driver needs to be notified of.
+ */
+static void ab8500_chargalg_external_power_changed(struct power_supply *psy)
+{
+	struct ab8500_chargalg *di = to_ab8500_chargalg_device_info(psy);
+
+	/*
+	 * Trigger execution of the algorithm instantly and read
+	 * all power_supply properties there instead
+	 */
+	queue_work(di->chargalg_wq, &di->chargalg_work);
+}
+
+/**
+ * ab8500_chargalg_algorithm() - Main function for the algorithm
+ * @di:		pointer to the ab8500_chargalg structure
+ *
+ * This is the main control function for the charging algorithm.
+ * It is called periodically or when something happens that will
+ * trigger a state change
+ */
+static void ab8500_chargalg_algorithm(struct ab8500_chargalg *di)
+{
+	int charger_status;
+
+	/* Collect data from all power_supply class devices */
+	class_for_each_device(power_supply_class, NULL,
+		&di->chargalg_psy, ab8500_chargalg_get_ext_psy_data);
+
+	ab8500_chargalg_end_of_charge(di);
+	ab8500_chargalg_check_temp(di);
+	ab8500_chargalg_check_charger_voltage(di);
+	charger_status = ab8500_chargalg_check_charger_connection(di);
+
+	/*
+	 * First check if we have a charger connected.
+	 * Also we don't allow charging of unknown batteries if configured
+	 * this way
+	 */
+	if (!charger_status ||
+		(di->events.batt_unknown && !di->bat->chg_unknown_bat)) {
+		if (di->charge_state != STATE_HANDHELD) {
+			di->events.safety_timer_expired = false;
+			ab8500_chargalg_state_to(di, STATE_HANDHELD_INIT);
+		}
+	}
+
+	/* If suspended, we should not continue checking the flags */
+	else if (di->charge_state == STATE_SUSPENDED_INIT ||
+		di->charge_state == STATE_SUSPENDED) {
+		/* We don't do anything here, just don,t continue */
+	}
+
+	/* Safety timer expiration */
+	else if (di->events.safety_timer_expired) {
+		if (di->charge_state !=	STATE_SAFETY_TIMER_EXPIRED)
+			ab8500_chargalg_state_to(di,
+				STATE_SAFETY_TIMER_EXPIRED_INIT);
+	}
+	/*
+	 * Check if any interrupts has occured
+	 * that will prevent us from charging
+	 */
+
+	/* Battery removed */
+	else if (di->events.batt_rem) {
+		if (di->charge_state != STATE_BATT_REMOVED)
+			ab8500_chargalg_state_to(di, STATE_BATT_REMOVED_INIT);
+	}
+	/* Main or USB charger not ok. */
+	else if (di->events.mainextchnotok || di->events.usbchargernotok) {
+		/*
+		 * If vbus_collapsed is set, we have to lower the charger
+		 * current, which is done in the normal state below
+		 */
+		if (di->charge_state != STATE_CHG_NOT_OK &&
+				!di->events.vbus_collapsed)
+			ab8500_chargalg_state_to(di, STATE_CHG_NOT_OK_INIT);
+	}
+	/* VBUS, Main or VBAT OVV. */
+	else if (di->events.vbus_ovv ||
+			di->events.main_ovv ||
+			di->events.batt_ovv ||
+			!di->chg_info.usb_chg_ok ||
+			!di->chg_info.ac_chg_ok) {
+		if (di->charge_state != STATE_OVV_PROTECT)
+			ab8500_chargalg_state_to(di, STATE_OVV_PROTECT_INIT);
+	}
+	/* USB Thermal, stop charging */
+	else if (di->events.main_thermal_prot ||
+		di->events.usb_thermal_prot) {
+		if (di->charge_state != STATE_HW_TEMP_PROTECT)
+			ab8500_chargalg_state_to(di,
+				STATE_HW_TEMP_PROTECT_INIT);
+	}
+	/* Battery temp over/under */
+	else if (di->events.btemp_underover) {
+		if (di->charge_state != STATE_TEMP_UNDEROVER)
+			ab8500_chargalg_state_to(di,
+				STATE_TEMP_UNDEROVER_INIT);
+	}
+	/* Watchdog expired */
+	else if (di->events.ac_wd_expired ||
+		di->events.usb_wd_expired) {
+		if (di->charge_state != STATE_WD_EXPIRED)
+			ab8500_chargalg_state_to(di, STATE_WD_EXPIRED_INIT);
+	}
+	/* Battery temp high/low */
+	else if (di->events.btemp_lowhigh) {
+		if (di->charge_state != STATE_TEMP_LOWHIGH)
+			ab8500_chargalg_state_to(di, STATE_TEMP_LOWHIGH_INIT);
+	}
+
+	dev_dbg(di->dev,
+		"[CHARGALG] Vb %d Ib_avg %d Ib_inst %d Tb %d Cap %d Maint %d "
+		"State %s Active_chg %d Chg_status %d AC %d USB %d "
+		"AC_online %d USB_online %d AC_CV %d USB_CV %d AC_I %d "
+		"USB_I %d AC_Vset %d AC_Iset %d USB_Vset %d USB_Iset %d\n",
+		di->batt_data.volt,
+		di->batt_data.avg_curr,
+		di->batt_data.inst_curr,
+		di->batt_data.temp,
+		di->batt_data.percent,
+		di->maintenance_chg,
+		states[di->charge_state],
+		di->chg_info.charger_type,
+		di->charge_status,
+		di->chg_info.conn_chg & AC_CHG,
+		di->chg_info.conn_chg & USB_CHG,
+		di->chg_info.online_chg & AC_CHG,
+		di->chg_info.online_chg & USB_CHG,
+		di->events.ac_cv_active,
+		di->events.usb_cv_active,
+		di->chg_info.ac_curr,
+		di->chg_info.usb_curr,
+		di->chg_info.ac_vset,
+		di->chg_info.ac_iset,
+		di->chg_info.usb_vset,
+		di->chg_info.usb_iset);
+
+	switch (di->charge_state) {
+	case STATE_HANDHELD_INIT:
+		ab8500_chargalg_stop_charging(di);
+		di->charge_status = POWER_SUPPLY_STATUS_DISCHARGING;
+		ab8500_chargalg_state_to(di, STATE_HANDHELD);
+		/* Intentional fallthrough */
+
+	case STATE_HANDHELD:
+		break;
+
+	case STATE_SUSPENDED_INIT:
+		if (di->susp_status.ac_suspended)
+			ab8500_chargalg_ac_en(di, false, 0, 0);
+		if (di->susp_status.usb_suspended)
+			ab8500_chargalg_usb_en(di, false, 0, 0);
+		ab8500_chargalg_stop_safety_timer(di);
+		ab8500_chargalg_stop_maintenance_timer(di);
+		di->charge_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+		di->maintenance_chg = false;
+		ab8500_chargalg_state_to(di, STATE_SUSPENDED);
+		power_supply_changed(&di->chargalg_psy);
+		/* Intentional fallthrough */
+
+	case STATE_SUSPENDED:
+		/* CHARGING is suspended */
+		break;
+
+	case STATE_BATT_REMOVED_INIT:
+		ab8500_chargalg_stop_charging(di);
+		ab8500_chargalg_state_to(di, STATE_BATT_REMOVED);
+		/* Intentional fallthrough */
+
+	case STATE_BATT_REMOVED:
+		if (!di->events.batt_rem)
+			ab8500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_HW_TEMP_PROTECT_INIT:
+		ab8500_chargalg_stop_charging(di);
+		ab8500_chargalg_state_to(di, STATE_HW_TEMP_PROTECT);
+		/* Intentional fallthrough */
+
+	case STATE_HW_TEMP_PROTECT:
+		if (!di->events.main_thermal_prot &&
+				!di->events.usb_thermal_prot)
+			ab8500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_OVV_PROTECT_INIT:
+		ab8500_chargalg_stop_charging(di);
+		ab8500_chargalg_state_to(di, STATE_OVV_PROTECT);
+		/* Intentional fallthrough */
+
+	case STATE_OVV_PROTECT:
+		if (!di->events.vbus_ovv &&
+				!di->events.main_ovv &&
+				!di->events.batt_ovv &&
+				di->chg_info.usb_chg_ok &&
+				di->chg_info.ac_chg_ok)
+			ab8500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_CHG_NOT_OK_INIT:
+		ab8500_chargalg_stop_charging(di);
+		ab8500_chargalg_state_to(di, STATE_CHG_NOT_OK);
+		/* Intentional fallthrough */
+
+	case STATE_CHG_NOT_OK:
+		if (!di->events.mainextchnotok &&
+				!di->events.usbchargernotok)
+			ab8500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_SAFETY_TIMER_EXPIRED_INIT:
+		ab8500_chargalg_stop_charging(di);
+		ab8500_chargalg_state_to(di, STATE_SAFETY_TIMER_EXPIRED);
+		/* Intentional fallthrough */
+
+	case STATE_SAFETY_TIMER_EXPIRED:
+		/* We exit this state when charger is removed */
+		break;
+
+	case STATE_NORMAL_INIT:
+		ab8500_chargalg_start_charging(di,
+			di->bat->bat_type[di->bat->batt_id].normal_vol_lvl,
+			di->bat->bat_type[di->bat->batt_id].normal_cur_lvl);
+		ab8500_chargalg_state_to(di, STATE_NORMAL);
+		ab8500_chargalg_start_safety_timer(di);
+		ab8500_chargalg_stop_maintenance_timer(di);
+		init_maxim_chg_curr(di);
+		di->charge_status = POWER_SUPPLY_STATUS_CHARGING;
+		di->eoc_cnt = 0;
+		di->maintenance_chg = false;
+		power_supply_changed(&di->chargalg_psy);
+
+		break;
+
+	case STATE_NORMAL:
+		handle_maxim_chg_curr(di);
+		if (di->charge_status == POWER_SUPPLY_STATUS_FULL &&
+			di->maintenance_chg) {
+			if (di->bat->no_maintenance)
+				ab8500_chargalg_state_to(di,
+					STATE_WAIT_FOR_RECHARGE_INIT);
+			else
+				ab8500_chargalg_state_to(di,
+					STATE_MAINTENANCE_A_INIT);
+		}
+		break;
+
+	/* This state will be used when the maintenance state is disabled */
+	case STATE_WAIT_FOR_RECHARGE_INIT:
+		ab8500_chargalg_hold_charging(di);
+		ab8500_chargalg_state_to(di, STATE_WAIT_FOR_RECHARGE);
+		di->rch_cnt = RCH_COND_CNT;
+		/* Intentional fallthrough */
+
+	case STATE_WAIT_FOR_RECHARGE:
+		if (di->batt_data.volt <=
+			di->bat->bat_type[di->bat->batt_id].recharge_vol) {
+			if (di->rch_cnt-- == 0)
+				ab8500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		} else
+			di->rch_cnt = RCH_COND_CNT;
+		break;
+
+	case STATE_MAINTENANCE_A_INIT:
+		ab8500_chargalg_stop_safety_timer(di);
+		ab8500_chargalg_start_maintenance_timer(di,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_a_chg_timer_h);
+		ab8500_chargalg_start_charging(di,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_a_vol_lvl,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_a_cur_lvl);
+		ab8500_chargalg_state_to(di, STATE_MAINTENANCE_A);
+		power_supply_changed(&di->chargalg_psy);
+		/* Intentional fallthrough*/
+
+	case STATE_MAINTENANCE_A:
+		if (di->events.maintenance_timer_expired) {
+			ab8500_chargalg_stop_maintenance_timer(di);
+			ab8500_chargalg_state_to(di, STATE_MAINTENANCE_B_INIT);
+		}
+		break;
+
+	case STATE_MAINTENANCE_B_INIT:
+		ab8500_chargalg_start_maintenance_timer(di,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_b_chg_timer_h);
+		ab8500_chargalg_start_charging(di,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_b_vol_lvl,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_b_cur_lvl);
+		ab8500_chargalg_state_to(di, STATE_MAINTENANCE_B);
+		power_supply_changed(&di->chargalg_psy);
+		/* Intentional fallthrough*/
+
+	case STATE_MAINTENANCE_B:
+		if (di->events.maintenance_timer_expired) {
+			ab8500_chargalg_stop_maintenance_timer(di);
+			ab8500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		}
+		break;
+
+	case STATE_TEMP_LOWHIGH_INIT:
+		ab8500_chargalg_start_charging(di,
+			di->bat->bat_type[
+				di->bat->batt_id].low_high_vol_lvl,
+			di->bat->bat_type[
+				di->bat->batt_id].low_high_cur_lvl);
+		ab8500_chargalg_stop_maintenance_timer(di);
+		di->charge_status = POWER_SUPPLY_STATUS_CHARGING;
+		ab8500_chargalg_state_to(di, STATE_TEMP_LOWHIGH);
+		power_supply_changed(&di->chargalg_psy);
+		/* Intentional fallthrough */
+
+	case STATE_TEMP_LOWHIGH:
+		if (!di->events.btemp_lowhigh)
+			ab8500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_WD_EXPIRED_INIT:
+		ab8500_chargalg_stop_charging(di);
+		ab8500_chargalg_state_to(di, STATE_WD_EXPIRED);
+		/* Intentional fallthrough */
+
+	case STATE_WD_EXPIRED:
+		if (!di->events.ac_wd_expired &&
+				!di->events.usb_wd_expired)
+			ab8500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_TEMP_UNDEROVER_INIT:
+		ab8500_chargalg_stop_charging(di);
+		ab8500_chargalg_state_to(di, STATE_TEMP_UNDEROVER);
+		/* Intentional fallthrough */
+
+	case STATE_TEMP_UNDEROVER:
+		if (!di->events.btemp_underover)
+			ab8500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+	}
+
+	/* Start charging directly if the new state is a charge state */
+	if (di->charge_state == STATE_NORMAL_INIT ||
+			di->charge_state == STATE_MAINTENANCE_A_INIT ||
+			di->charge_state == STATE_MAINTENANCE_B_INIT)
+		queue_work(di->chargalg_wq, &di->chargalg_work);
+}
+
+/**
+ * ab8500_chargalg_periodic_work() - Periodic work for the algorithm
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for the charging algorithm
+ */
+static void ab8500_chargalg_periodic_work(struct work_struct *work)
+{
+	struct ab8500_chargalg *di = container_of(work,
+		struct ab8500_chargalg, chargalg_periodic_work.work);
+
+	ab8500_chargalg_algorithm(di);
+
+	/*
+	 * If a charger is connected then the battery has to be monitored
+	 * frequently, else the work can be delayed.
+	 */
+	if (di->chg_info.conn_chg)
+		queue_delayed_work(di->chargalg_wq,
+			&di->chargalg_periodic_work,
+			di->bat->interval_charging * HZ);
+	else
+		queue_delayed_work(di->chargalg_wq,
+			&di->chargalg_periodic_work,
+			di->bat->interval_not_charging * HZ);
+}
+
+/**
+ * ab8500_chargalg_wd_work() - periodic work to kick the charger watchdog
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for kicking the charger watchdog
+ */
+static void ab8500_chargalg_wd_work(struct work_struct *work)
+{
+	int ret;
+	struct ab8500_chargalg *di = container_of(work,
+		struct ab8500_chargalg, chargalg_wd_work.work);
+
+	dev_dbg(di->dev, "ab8500_chargalg_wd_work\n");
+
+	ret = ab8500_chargalg_kick_watchdog(di);
+	if (ret < 0)
+		dev_err(di->dev, "failed to kick watchdog\n");
+
+	queue_delayed_work(di->chargalg_wq,
+		&di->chargalg_wd_work, CHG_WD_INTERVAL);
+}
+
+/**
+ * ab8500_chargalg_work() - Work to run the charging algorithm instantly
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for calling the charging algorithm
+ */
+static void ab8500_chargalg_work(struct work_struct *work)
+{
+	struct ab8500_chargalg *di = container_of(work,
+		struct ab8500_chargalg, chargalg_work);
+
+	ab8500_chargalg_algorithm(di);
+}
+
+/**
+ * ab8500_chargalg_get_property() - get the chargalg properties
+ * @psy:	pointer to the power_supply structure
+ * @psp:	pointer to the power_supply_property structure
+ * @val:	pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the
+ * chargalg properties by reading the sysfs files.
+ * status:     charging/discharging/full/unknown
+ * health:     health of the battery
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab8500_chargalg_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct ab8500_chargalg *di;
+
+	di = to_ab8500_chargalg_device_info(psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		val->intval = di->charge_status;
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		if (di->events.batt_ovv) {
+			val->intval = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+		} else if (di->events.btemp_underover) {
+			if (di->batt_data.temp <= di->bat->temp_under)
+				val->intval = POWER_SUPPLY_HEALTH_COLD;
+			else
+				val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		} else {
+			val->intval = POWER_SUPPLY_HEALTH_GOOD;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* Exposure to the sysfs interface */
+
+/**
+ * ab8500_chargalg_sysfs_charger() - sysfs store operations
+ * @kobj:      pointer to the struct kobject
+ * @attr:      pointer to the struct attribute
+ * @buf:       buffer that holds the parameter passed from userspace
+ * @length:    length of the parameter passed
+ *
+ * Returns length of the buffer(input taken from user space) on success
+ * else error code on failure
+ * The operation to be performed on passing the parameters from the user space.
+ */
+static ssize_t ab8500_chargalg_sysfs_charger(struct kobject *kobj,
+	struct attribute *attr, const char *buf, size_t length)
+{
+	struct ab8500_chargalg *di = container_of(kobj,
+		struct ab8500_chargalg, chargalg_kobject);
+	long int param;
+	int ac_usb;
+	int ret;
+	char entry = *attr->name;
+
+	switch (entry) {
+	case 'c':
+		ret = strict_strtol(buf, 10, &param);
+		if (ret < 0)
+			return ret;
+
+		ac_usb = param;
+		switch (ac_usb) {
+		case 0:
+			/* Disable charging */
+			di->susp_status.ac_suspended = true;
+			di->susp_status.usb_suspended = true;
+			di->susp_status.suspended_change = true;
+			/* Trigger a state change */
+			queue_work(di->chargalg_wq,
+				&di->chargalg_work);
+			break;
+		case 1:
+			/* Enable AC Charging */
+			di->susp_status.ac_suspended = false;
+			di->susp_status.suspended_change = true;
+			/* Trigger a state change */
+			queue_work(di->chargalg_wq,
+				&di->chargalg_work);
+			break;
+		case 2:
+			/* Enable USB charging */
+			di->susp_status.usb_suspended = false;
+			di->susp_status.suspended_change = true;
+			/* Trigger a state change */
+			queue_work(di->chargalg_wq,
+				&di->chargalg_work);
+			break;
+		default:
+			dev_info(di->dev, "Wrong input\n"
+				"Enter 0. Disable AC/USB Charging\n"
+				"1. Enable AC charging\n"
+				"2. Enable USB Charging\n");
+		};
+		break;
+	};
+	return strlen(buf);
+}
+
+static struct attribute ab8500_chargalg_en_charger = \
+{
+	.name = "chargalg",
+	.mode = S_IWUGO,
+};
+
+static struct attribute *ab8500_chargalg_chg[] = {
+	&ab8500_chargalg_en_charger,
+	NULL
+};
+
+const struct sysfs_ops ab8500_chargalg_sysfs_ops = {
+	.store = ab8500_chargalg_sysfs_charger,
+};
+
+static struct kobj_type ab8500_chargalg_ktype = {
+	.sysfs_ops = &ab8500_chargalg_sysfs_ops,
+	.default_attrs = ab8500_chargalg_chg,
+};
+
+/**
+ * ab8500_chargalg_sysfs_exit() - de-init of sysfs entry
+ * @di:                pointer to the struct ab8500_chargalg
+ *
+ * This function removes the entry in sysfs.
+ */
+static void ab8500_chargalg_sysfs_exit(struct ab8500_chargalg *di)
+{
+	kobject_del(&di->chargalg_kobject);
+}
+
+/**
+ * ab8500_chargalg_sysfs_init() - init of sysfs entry
+ * @di:                pointer to the struct ab8500_chargalg
+ *
+ * This function adds an entry in sysfs.
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_chargalg_sysfs_init(struct ab8500_chargalg *di)
+{
+	int ret = 0;
+
+	ret = kobject_init_and_add(&di->chargalg_kobject,
+		&ab8500_chargalg_ktype,
+		NULL, "ab8500_chargalg");
+	if (ret < 0)
+		dev_err(di->dev, "failed to create sysfs entry\n");
+
+	return ret;
+}
+/* Exposure to the sysfs interface <<END>> */
+
+#if defined(CONFIG_PM)
+static int ab8500_chargalg_resume(struct platform_device *pdev)
+{
+	struct ab8500_chargalg *di = platform_get_drvdata(pdev);
+
+	/* Kick charger watchdog if charging (any charger online) */
+	if (di->chg_info.online_chg)
+		queue_delayed_work(di->chargalg_wq, &di->chargalg_wd_work, 0);
+
+	/*
+	 * Run the charging algorithm directly to be sure we don't
+	 * do it too seldom
+	 */
+	queue_delayed_work(di->chargalg_wq, &di->chargalg_periodic_work, 0);
+
+	return 0;
+}
+
+static int ab8500_chargalg_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct ab8500_chargalg *di = platform_get_drvdata(pdev);
+
+	if (di->chg_info.online_chg)
+		cancel_delayed_work_sync(&di->chargalg_wd_work);
+
+	cancel_delayed_work_sync(&di->chargalg_periodic_work);
+
+	return 0;
+}
+#else
+#define ab8500_chargalg_suspend      NULL
+#define ab8500_chargalg_resume       NULL
+#endif
+
+static int __devexit ab8500_chargalg_remove(struct platform_device *pdev)
+{
+	struct ab8500_chargalg *di = platform_get_drvdata(pdev);
+
+	/* sysfs interface to enable/disbale charging from user space */
+	ab8500_chargalg_sysfs_exit(di);
+
+	/* Delete the work queue */
+	destroy_workqueue(di->chargalg_wq);
+
+	flush_scheduled_work();
+	power_supply_unregister(&di->chargalg_psy);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+
+	return 0;
+}
+
+static int __devinit ab8500_chargalg_probe(struct platform_device *pdev)
+{
+	struct ab8500_platform_data *plat;
+	int ret = 0;
+
+	struct ab8500_chargalg *di =
+		kzalloc(sizeof(struct ab8500_chargalg), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	/* get parent data */
+	di->dev = &pdev->dev;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
+
+	plat = dev_get_platdata(di->parent->dev);
+
+	/* get chargalg specific platform data */
+	if (!plat->chargalg) {
+		dev_err(di->dev, "no chargalg platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+	di->pdata = plat->chargalg;
+
+	/* get battery specific platform data */
+	if (!plat->battery) {
+		dev_err(di->dev, "no battery platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+	di->bat = plat->battery;
+
+	/* chargalg supply */
+	di->chargalg_psy.name = "ab8500_chargalg";
+	di->chargalg_psy.type = POWER_SUPPLY_TYPE_BATTERY;
+	di->chargalg_psy.properties = ab8500_chargalg_props;
+	di->chargalg_psy.num_properties = ARRAY_SIZE(ab8500_chargalg_props);
+	di->chargalg_psy.get_property = ab8500_chargalg_get_property;
+	di->chargalg_psy.supplied_to = di->pdata->supplied_to;
+	di->chargalg_psy.num_supplicants = di->pdata->num_supplicants;
+	di->chargalg_psy.external_power_changed =
+		ab8500_chargalg_external_power_changed;
+
+	/* Initilialize safety timer */
+	init_timer(&di->safety_timer);
+	di->safety_timer.function = ab8500_chargalg_safety_timer_expired;
+	di->safety_timer.data = (unsigned long) di;
+
+	/* Initilialize maintenance timer */
+	init_timer(&di->maintenance_timer);
+	di->maintenance_timer.function =
+		ab8500_chargalg_maintenance_timer_expired;
+	di->maintenance_timer.data = (unsigned long) di;
+
+	/* Create a work queue for the chargalg */
+	di->chargalg_wq =
+		create_singlethread_workqueue("ab8500_chargalg_wq");
+	if (di->chargalg_wq == NULL) {
+		dev_err(di->dev, "failed to create work queue\n");
+		goto free_device_info;
+	}
+
+	/* Init work for chargalg */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->chargalg_periodic_work,
+		ab8500_chargalg_periodic_work);
+	INIT_DELAYED_WORK_DEFERRABLE(&di->chargalg_wd_work,
+		ab8500_chargalg_wd_work);
+
+	/* Init work for chargalg */
+	INIT_WORK(&di->chargalg_work, ab8500_chargalg_work);
+
+	/* To detect charger at startup */
+	di->chg_info.prev_conn_chg = -1;
+
+	/* Register chargalg power supply class */
+	ret = power_supply_register(di->dev, &di->chargalg_psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register chargalg psy\n");
+		goto free_chargalg_wq;
+	}
+
+	platform_set_drvdata(pdev, di);
+
+	/* sysfs interface to enable/disable charging from user space */
+	ret = ab8500_chargalg_sysfs_init(di);
+	if (ret) {
+		dev_err(di->dev, "failed to create sysfs entry\n");
+		goto free_psy;
+	}
+
+	/* Run the charging algorithm */
+	queue_delayed_work(di->chargalg_wq, &di->chargalg_periodic_work, 0);
+	return ret;
+
+free_psy:
+	power_supply_unregister(&di->chargalg_psy);
+free_chargalg_wq:
+	destroy_workqueue(di->chargalg_wq);
+free_device_info:
+	kfree(di);
+
+	return ret;
+}
+
+static struct platform_driver ab8500_chargalg_driver = {
+	.probe = ab8500_chargalg_probe,
+	.remove = __devexit_p(ab8500_chargalg_remove),
+	.suspend = ab8500_chargalg_suspend,
+	.resume = ab8500_chargalg_resume,
+	.driver = {
+		.name = "ab8500-chargalg",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab8500_chargalg_init(void)
+{
+	return platform_driver_register(&ab8500_chargalg_driver);
+}
+
+static void __exit ab8500_chargalg_exit(void)
+{
+	platform_driver_unregister(&ab8500_chargalg_driver);
+}
+
+module_init(ab8500_chargalg_init);
+module_exit(ab8500_chargalg_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Johan Palsson, Karl Komierowski");
+MODULE_ALIAS("platform:ab8500-chargalg");
+MODULE_DESCRIPTION("AB8500 battery temperature driver");
diff --git a/drivers/power/ab8500_charger.c b/drivers/power/ab8500_charger.c
new file mode 100644
index 00000000000..e9db45494fa
--- /dev/null
+++ b/drivers/power/ab8500_charger.c
@@ -0,0 +1,2572 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Charger driver for AB8500
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Johan Palsson <johan.palsson@stericsson.com>
+ * Author: Karl Komierowski <karl.komierowski@stericsson.com>
+ * Author: Arun R Murthy <arun.murthy@stericsson.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/completion.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/ab8500/bm.h>
+#include <linux/mfd/ab8500/gpadc.h>
+#include <linux/mfd/ab8500/ux500_chargalg.h>
+#include <linux/usb/otg.h>
+
+/* Charger constants */
+#define NO_PW_CONN			0
+#define AC_PW_CONN			1
+#define USB_PW_CONN			2
+
+#define MAIN_WDOG_ENA			0x01
+#define MAIN_WDOG_KICK			0x02
+#define MAIN_WDOG_DIS			0x00
+#define CHARG_WD_KICK			0x01
+#define MAIN_CH_ENA			0x01
+#define MAIN_CH_NO_OVERSHOOT_ENA_N	0x02
+#define USB_CH_ENA			0x01
+#define USB_CHG_NO_OVERSHOOT_ENA_N	0x02
+#define MAIN_CH_DET			0x01
+#define MAIN_CH_CV_ON			0x04
+#define USB_CH_CV_ON			0x08
+#define VBUS_DET_DBNC100		0x02
+#define VBUS_DET_DBNC1			0x01
+#define OTP_ENABLE_WD			0x01
+
+#define MAIN_CH_INPUT_CURR_SHIFT	4
+#define VBUS_IN_CURR_LIM_SHIFT		4
+
+#define LED_INDICATOR_PWM_ENA		0x01
+#define LED_INDICATOR_PWM_DIS		0x00
+#define LED_IND_CUR_5MA			0x04
+#define LED_INDICATOR_PWM_DUTY_252_256	0xBF
+
+/* HW failure constants */
+#define MAIN_CH_TH_PROT			0x02
+#define VBUS_CH_NOK			0x08
+#define USB_CH_TH_PROT			0x02
+#define VBUS_OVV_TH			0x01
+#define MAIN_CH_NOK			0x01
+#define VBUS_DET			0x80
+
+/* UsbLineStatus register bit masks */
+#define AB8500_USB_LINK_STATUS		0x78
+#define AB8500_STD_HOST_SUSP		0x18
+
+/* Watchdog timeout constant */
+#define WD_TIMER			0x30 /* 4min */
+#define WD_KICK_INTERVAL		(60 * HZ)
+
+/* Lowest charger voltage is 3.39V -> 0x4E */
+#define LOW_VOLT_REG			0x4E
+
+/* UsbLineStatus register - usb types */
+enum ab8500_charger_link_status {
+	USB_STAT_NOT_CONFIGURED,
+	USB_STAT_STD_HOST_NC,
+	USB_STAT_STD_HOST_C_NS,
+	USB_STAT_STD_HOST_C_S,
+	USB_STAT_HOST_CHG_NM,
+	USB_STAT_HOST_CHG_HS,
+	USB_STAT_HOST_CHG_HS_CHIRP,
+	USB_STAT_DEDICATED_CHG,
+	USB_STAT_ACA_RID_A,
+	USB_STAT_ACA_RID_B,
+	USB_STAT_ACA_RID_C_NM,
+	USB_STAT_ACA_RID_C_HS,
+	USB_STAT_ACA_RID_C_HS_CHIRP,
+	USB_STAT_HM_IDGND,
+	USB_STAT_RESERVED,
+	USB_STAT_NOT_VALID_LINK,
+};
+
+enum ab8500_usb_state {
+	AB8500_BM_USB_STATE_RESET_HS,	/* HighSpeed Reset */
+	AB8500_BM_USB_STATE_RESET_FS,	/* FullSpeed/LowSpeed Reset */
+	AB8500_BM_USB_STATE_CONFIGURED,
+	AB8500_BM_USB_STATE_SUSPEND,
+	AB8500_BM_USB_STATE_RESUME,
+	AB8500_BM_USB_STATE_MAX,
+};
+
+/* VBUS input current limits supported in AB8500 in mA */
+#define USB_CH_IP_CUR_LVL_0P05		50
+#define USB_CH_IP_CUR_LVL_0P09		98
+#define USB_CH_IP_CUR_LVL_0P19		193
+#define USB_CH_IP_CUR_LVL_0P29		290
+#define USB_CH_IP_CUR_LVL_0P38		380
+#define USB_CH_IP_CUR_LVL_0P45		450
+#define USB_CH_IP_CUR_LVL_0P5		500
+#define USB_CH_IP_CUR_LVL_0P6		600
+#define USB_CH_IP_CUR_LVL_0P7		700
+#define USB_CH_IP_CUR_LVL_0P8		800
+#define USB_CH_IP_CUR_LVL_0P9		900
+#define USB_CH_IP_CUR_LVL_1P0		1000
+#define USB_CH_IP_CUR_LVL_1P1		1100
+#define USB_CH_IP_CUR_LVL_1P3		1300
+#define USB_CH_IP_CUR_LVL_1P4		1400
+#define USB_CH_IP_CUR_LVL_1P5		1500
+
+#define to_ab8500_charger_usb_device_info(x) container_of((x), \
+	struct ab8500_charger, usb_chg)
+#define to_ab8500_charger_ac_device_info(x) container_of((x), \
+	struct ab8500_charger, ac_chg)
+
+/**
+ * struct ab8500_charger_interrupts - ab8500 interupts
+ * @name:	name of the interrupt
+ * @isr		function pointer to the isr
+ */
+struct ab8500_charger_interrupts {
+	char *name;
+	irqreturn_t (*isr)(int irq, void *data);
+};
+
+struct ab8500_charger_info {
+	int charger_connected;
+	int charger_online;
+	int charger_voltage;
+	int cv_active;
+	bool wd_expired;
+};
+
+struct ab8500_charger_event_flags {
+	bool mainextchnotok;
+	bool main_thermal_prot;
+	bool usb_thermal_prot;
+	bool vbus_ovv;
+	bool usbchargernotok;
+	bool chgwdexp;
+	bool vbus_collapse;
+};
+
+struct ab8500_charger_usb_state {
+	bool usb_changed;
+	int usb_current;
+	enum ab8500_usb_state state;
+	spinlock_t usb_lock;
+};
+
+/**
+ * struct ab8500_charger - ab8500 Charger device information
+ * @dev:		Pointer to the structure device
+ * @chip_id:		Chip-Id of the AB8500
+ * @max_usb_in_curr:	Max USB charger input current
+ * @vbus_detected:	VBUS detected
+ * @vbus_detected_start:
+ *			VBUS detected during startup
+ * @ac_conn:		This will be true when the AC charger has been plugged
+ * @vddadc_en:		Indicate if VDD ADC supply is enabled from this driver
+ * @parent:		Pointer to the struct ab8500
+ * @gpadc:		Pointer to the struct gpadc
+ * @pdata:		Pointer to the ab8500_charger platform data
+ * @bat:		Pointer to the ab8500_bm platform data
+ * @flags:		Structure for information about events triggered
+ * @usb_state:		Structure for usb stack information
+ * @ac_chg:		AC charger power supply
+ * @usb_chg:		USB charger power supply
+ * @ac:			Structure that holds the AC charger properties
+ * @usb:		Structure that holds the USB charger properties
+ * @regu:		Pointer to the struct regulator
+ * @charger_wq:		Work queue for the IRQs and checking HW state
+ * @check_hw_failure_work:	Work for checking HW state
+ * @check_usbchgnotok_work:	Work for checking USB charger not ok status
+ * @kick_wd_work:		Work for kicking the charger watchdog in case
+ *				of ABB rev 1.* due to the watchog logic bug
+ * @ac_work:			Work for checking AC charger connection
+ * @detect_usb_type_work:	Work for detecting the USB type connected
+ * @usb_link_status_work:	Work for checking the new USB link status
+ * @usb_state_changed_work:	Work for checking USB state
+ * @check_main_thermal_prot_work:
+ *				Work for checking Main thermal status
+ * @check_usb_thermal_prot_work:
+ *				Work for checking USB thermal status
+ */
+struct ab8500_charger {
+	struct device *dev;
+	u8 chip_id;
+	int max_usb_in_curr;
+	bool vbus_detected;
+	bool vbus_detected_start;
+	bool ac_conn;
+	bool vddadc_en;
+	struct ab8500 *parent;
+	struct ab8500_gpadc *gpadc;
+	struct ab8500_charger_platform_data *pdata;
+	struct ab8500_bm_data *bat;
+	struct ab8500_charger_event_flags flags;
+	struct ab8500_charger_usb_state usb_state;
+	struct ux500_charger ac_chg;
+	struct ux500_charger usb_chg;
+	struct ab8500_charger_info ac;
+	struct ab8500_charger_info usb;
+	struct regulator *regu;
+	struct workqueue_struct *charger_wq;
+	struct delayed_work check_hw_failure_work;
+	struct delayed_work check_usbchgnotok_work;
+	struct delayed_work kick_wd_work;
+	struct work_struct ac_work;
+	struct work_struct detect_usb_type_work;
+	struct work_struct usb_link_status_work;
+	struct work_struct usb_state_changed_work;
+	struct work_struct check_main_thermal_prot_work;
+	struct work_struct check_usb_thermal_prot_work;
+	struct otg_transceiver *otg;
+	struct notifier_block nb;
+};
+
+/* AC properties */
+static enum power_supply_property ab8500_charger_ac_props[] = {
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_VOLTAGE_AVG,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+};
+
+/* USB properties */
+static enum power_supply_property ab8500_charger_usb_props[] = {
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_CURRENT_AVG,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_VOLTAGE_AVG,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+};
+
+/**
+ * ab8500_charger_get_ac_voltage() - get ac charger voltage
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * Returns ac charger voltage (on success)
+ */
+static int ab8500_charger_get_ac_voltage(struct ab8500_charger *di)
+{
+	int vch;
+
+	/* Only measure voltage if the charger is connected */
+	if (di->ac.charger_connected) {
+		vch = ab8500_gpadc_convert(di->gpadc, MAIN_CHARGER_V);
+		if (vch < 0)
+			dev_err(di->dev, "%s gpadc conv failed,\n", __func__);
+	} else {
+		vch = 0;
+	}
+	return vch;
+}
+
+/**
+ * ab8500_charger_ac_cv() - check if the main charger is in CV mode
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * Returns ac charger CV mode (on success) else error code
+ */
+static int ab8500_charger_ac_cv(struct ab8500_charger *di)
+{
+	u8 val;
+	int ret = 0;
+
+	/* Only check CV mode if the charger is online */
+	if (di->ac.charger_online) {
+		ret = abx500_get_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_CH_STATUS1_REG, &val);
+		if (ret < 0) {
+			dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+			return 0;
+		}
+
+		if (val & MAIN_CH_CV_ON)
+			ret = 1;
+		else
+			ret = 0;
+	}
+
+	return ret;
+}
+
+/**
+ * ab8500_charger_get_vbus_voltage() - get vbus voltage
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * This function returns the vbus voltage.
+ * Returns vbus voltage (on success)
+ */
+static int ab8500_charger_get_vbus_voltage(struct ab8500_charger *di)
+{
+	int vch;
+
+	/* Only measure voltage if the charger is connected */
+	if (di->usb.charger_connected) {
+		vch = ab8500_gpadc_convert(di->gpadc, VBUS_V);
+		if (vch < 0)
+			dev_err(di->dev, "%s gpadc conv failed\n", __func__);
+	} else {
+		vch = 0;
+	}
+	return vch;
+}
+
+/**
+ * ab8500_charger_get_usb_current() - get usb charger current
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * This function returns the usb charger current.
+ * Returns usb current (on success) and error code on failure
+ */
+static int ab8500_charger_get_usb_current(struct ab8500_charger *di)
+{
+	int ich;
+
+	/* Only measure current if the charger is online */
+	if (di->usb.charger_online) {
+		ich = ab8500_gpadc_convert(di->gpadc, USB_CHARGER_C);
+		if (ich < 0)
+			dev_err(di->dev, "%s gpadc conv failed\n", __func__);
+	} else {
+		ich = 0;
+	}
+	return ich;
+}
+
+/**
+ * ab8500_charger_get_ac_current() - get ac charger current
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * This function returns the ac charger current.
+ * Returns ac current (on success) and error code on failure.
+ */
+static int ab8500_charger_get_ac_current(struct ab8500_charger *di)
+{
+	int ich;
+
+	/* Only measure current if the charger is online */
+	if (di->ac.charger_online) {
+		ich = ab8500_gpadc_convert(di->gpadc, MAIN_CHARGER_C);
+		if (ich < 0)
+			dev_err(di->dev, "%s gpadc conv failed\n", __func__);
+	} else {
+		ich = 0;
+	}
+	return ich;
+}
+
+/**
+ * ab8500_charger_usb_cv() - check if the usb charger is in CV mode
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * Returns ac charger CV mode (on success) else error code
+ */
+static int ab8500_charger_usb_cv(struct ab8500_charger *di)
+{
+	int ret;
+	u8 val;
+
+	/* Only check CV mode if the charger is online */
+	if (di->usb.charger_online) {
+		ret = abx500_get_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_CH_USBCH_STAT1_REG, &val);
+		if (ret < 0) {
+			dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+			return 0;
+		}
+
+		if (val & USB_CH_CV_ON)
+			ret = 1;
+		else
+			ret = 0;
+	} else {
+		ret = 0;
+	}
+
+	return ret;
+}
+
+/**
+ * ab8500_charger_detect_chargers() - Detect the connected chargers
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * Returns the type of charger connected.
+ * For USB it will not mean we can actually charge from it
+ * but that there is a USB cable connected that we have to
+ * identify. This is used during startup when we don't get
+ * interrupts of the charger detection
+ *
+ * Returns an integer value, that means,
+ * NO_PW_CONN  no power supply is connected
+ * AC_PW_CONN  if the AC power supply is connected
+ * USB_PW_CONN  if the USB power supply is connected
+ * AC_PW_CONN + USB_PW_CONN if USB and AC power supplies are both connected
+ */
+static int ab8500_charger_detect_chargers(struct ab8500_charger *di)
+{
+	int result = NO_PW_CONN;
+	int ret;
+	u8 val;
+
+	/* Check for AC charger */
+	ret = abx500_get_register_interruptible(di->dev, AB8500_CHARGER,
+		AB8500_CH_STATUS1_REG, &val);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+		goto out;
+	}
+
+	if (val & MAIN_CH_DET)
+		result = AC_PW_CONN;
+
+	/* Check for USB charger */
+	ret = abx500_get_register_interruptible(di->dev, AB8500_CHARGER,
+		AB8500_CH_USBCH_STAT1_REG, &val);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+		goto out;
+	}
+
+	if (val & (VBUS_DET_DBNC100 | VBUS_DET_DBNC1))
+		result |= USB_PW_CONN;
+
+	/*
+	 * Due to a bug in AB8500, BTEMP_HIGH/LOW interrupts
+	 * will be triggered everytime we enable the VDD ADC supply.
+	 * This will turn off charging for a short while.
+	 * It can be avoided by having the supply on when
+	 * there is a charger connected. Normally the VDD ADC supply
+	 * is enabled everytime a GPADC conversion is triggered. We will
+	 * force it to be enabled from this driver to have
+	 * the GPADC module independant of the AB8500 chargers
+	 */
+	if (result == NO_PW_CONN && di->vddadc_en) {
+		regulator_disable(di->regu);
+		di->vddadc_en = false;
+	} else if ((result & AC_PW_CONN || result & USB_PW_CONN) &&
+		!di->vddadc_en) {
+		regulator_enable(di->regu);
+		di->vddadc_en = true;
+	}
+
+	return result;
+
+out:
+	if (di->vddadc_en) {
+		regulator_disable(di->regu);
+		di->vddadc_en = false;
+	}
+	return ret;
+}
+
+/**
+ * ab8500_charger_max_usb_curr() - get the max curr for the USB type
+ * @di:			pointer to the ab8500_charger structure
+ * @link_status:	the identified USB type
+ *
+ * Get the maximum current that is allowed to be drawn from the host
+ * based on the USB type.
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab8500_charger_max_usb_curr(struct ab8500_charger *di,
+	enum ab8500_charger_link_status link_status)
+{
+	int ret = 0;
+
+	switch (link_status) {
+	case USB_STAT_STD_HOST_NC:
+	case USB_STAT_STD_HOST_C_NS:
+	case USB_STAT_STD_HOST_C_S:
+		dev_dbg(di->dev, "USB Type - Standard host is "
+			"detected through USB driver\n");
+		ret = -1;
+		break;
+	case USB_STAT_HOST_CHG_HS_CHIRP:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
+		break;
+	case USB_STAT_HOST_CHG_HS:
+	case USB_STAT_ACA_RID_C_HS:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P9;
+		break;
+	case USB_STAT_ACA_RID_A:
+		/*
+		 * Dedicated charger level minus maximum current accessory
+		 * can consume (300mA). Closest level is 1100mA
+		 */
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P1;
+		break;
+	case USB_STAT_ACA_RID_B:
+		/*
+		 * Dedicated charger level minus 120mA (20mA for ACA and
+		 * 100mA for potential accessory). Closest level is 1300mA
+		 */
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P3;
+		break;
+	case USB_STAT_DEDICATED_CHG:
+	case USB_STAT_HOST_CHG_NM:
+	case USB_STAT_ACA_RID_C_HS_CHIRP:
+	case USB_STAT_ACA_RID_C_NM:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P5;
+		break;
+	case USB_STAT_RESERVED:
+		/*
+		 * This state is used to indicate that VBUS has dropped below
+		 * the detection level 4 times in a row. This is due to the
+		 * charger output current is set to high making the charger
+		 * voltage collapse. This have to be propagated through to
+		 * chargalg. This is done using the property
+		 * POWER_SUPPLY_PROP_CURRENT_AVG = 1
+		 */
+		di->flags.vbus_collapse = true;
+		dev_dbg(di->dev, "USB Type - USB_STAT_RESERVED "
+			"VBUS has collapsed\n");
+		ret = -1;
+		break;
+	case USB_STAT_HM_IDGND:
+	case USB_STAT_NOT_CONFIGURED:
+	case USB_STAT_NOT_VALID_LINK:
+		dev_err(di->dev, "USB Type - Charging not allowed\n");
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		ret = -ENXIO;
+		break;
+	default:
+		dev_err(di->dev, "USB Type - Unknown\n");
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		ret = -ENXIO;
+		break;
+	};
+
+	dev_dbg(di->dev, "USB Type - 0x%02x MaxCurr: %d",
+		link_status, di->max_usb_in_curr);
+
+	return ret;
+}
+
+/**
+ * ab8500_charger_read_usb_type() - read the type of usb connected
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * Detect the type of the plugged USB
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab8500_charger_read_usb_type(struct ab8500_charger *di)
+{
+	int ret;
+	u8 val;
+
+	ret = abx500_get_register_interruptible(di->dev,
+		AB8500_INTERRUPT, AB8500_IT_SOURCE21_REG, &val);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+		return ret;
+	}
+	ret = abx500_get_register_interruptible(di->dev, AB8500_USB,
+		AB8500_USB_LINE_STAT_REG, &val);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+		return ret;
+	}
+
+	/* get the USB type */
+	val = (val & AB8500_USB_LINK_STATUS) >> 3;
+	ret = ab8500_charger_max_usb_curr(di,
+		(enum ab8500_charger_link_status) val);
+
+	return ret;
+}
+
+/**
+ * ab8500_charger_detect_usb_type() - get the type of usb connected
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * Detect the type of the plugged USB
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab8500_charger_detect_usb_type(struct ab8500_charger *di)
+{
+	int i, ret;
+	u8 val;
+
+	/*
+	 * On getting the VBUS rising edge detect interrupt there
+	 * is a 250ms delay after which the register UsbLineStatus
+	 * is filled with valid data.
+	 */
+	for (i = 0; i < 10; i++) {
+		msleep(250);
+		ret = abx500_get_register_interruptible(di->dev,
+			AB8500_INTERRUPT, AB8500_IT_SOURCE21_REG,
+			&val);
+		if (ret < 0) {
+			dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+			return ret;
+		}
+		ret = abx500_get_register_interruptible(di->dev, AB8500_USB,
+			AB8500_USB_LINE_STAT_REG, &val);
+		if (ret < 0) {
+			dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+			return ret;
+		}
+		/*
+		 * Until the IT source register is read the UsbLineStatus
+		 * register is not updated, hence doing the same
+		 * Revisit this:
+		 */
+
+		/* get the USB type */
+		val = (val & AB8500_USB_LINK_STATUS) >> 3;
+		if (val)
+			break;
+	}
+	ret = ab8500_charger_max_usb_curr(di,
+		(enum ab8500_charger_link_status) val);
+
+	return ret;
+}
+
+/*
+ * This array maps the raw hex value to charger voltage used by the AB8500
+ * Values taken from the UM0836
+ */
+static int ab8500_charger_voltage_map[] = {
+	3500 ,
+	3525 ,
+	3550 ,
+	3575 ,
+	3600 ,
+	3625 ,
+	3650 ,
+	3675 ,
+	3700 ,
+	3725 ,
+	3750 ,
+	3775 ,
+	3800 ,
+	3825 ,
+	3850 ,
+	3875 ,
+	3900 ,
+	3925 ,
+	3950 ,
+	3975 ,
+	4000 ,
+	4025 ,
+	4050 ,
+	4060 ,
+	4070 ,
+	4080 ,
+	4090 ,
+	4100 ,
+	4110 ,
+	4120 ,
+	4130 ,
+	4140 ,
+	4150 ,
+	4160 ,
+	4170 ,
+	4180 ,
+	4190 ,
+	4200 ,
+	4210 ,
+	4220 ,
+	4230 ,
+	4240 ,
+	4250 ,
+	4260 ,
+	4270 ,
+	4280 ,
+	4290 ,
+	4300 ,
+	4310 ,
+	4320 ,
+	4330 ,
+	4340 ,
+	4350 ,
+	4360 ,
+	4370 ,
+	4380 ,
+	4390 ,
+	4400 ,
+	4410 ,
+	4420 ,
+	4430 ,
+	4440 ,
+	4450 ,
+	4460 ,
+	4470 ,
+	4480 ,
+	4490 ,
+	4500 ,
+	4510 ,
+	4520 ,
+	4530 ,
+	4540 ,
+	4550 ,
+	4560 ,
+	4570 ,
+	4580 ,
+	4590 ,
+	4600 ,
+};
+
+/*
+ * This array maps the raw hex value to charger current used by the AB8500
+ * Values taken from the UM0836
+ */
+static int ab8500_charger_current_map[] = {
+	100 ,
+	200 ,
+	300 ,
+	400 ,
+	500 ,
+	600 ,
+	700 ,
+	800 ,
+	900 ,
+	1000 ,
+	1100 ,
+	1200 ,
+	1300 ,
+	1400 ,
+	1500 ,
+};
+
+/*
+ * This array maps the raw hex value to VBUS input current used by the AB8500
+ * Values taken from the UM0836
+ */
+static int ab8500_charger_vbus_in_curr_map[] = {
+	USB_CH_IP_CUR_LVL_0P05,
+	USB_CH_IP_CUR_LVL_0P09,
+	USB_CH_IP_CUR_LVL_0P19,
+	USB_CH_IP_CUR_LVL_0P29,
+	USB_CH_IP_CUR_LVL_0P38,
+	USB_CH_IP_CUR_LVL_0P45,
+	USB_CH_IP_CUR_LVL_0P5,
+	USB_CH_IP_CUR_LVL_0P6,
+	USB_CH_IP_CUR_LVL_0P7,
+	USB_CH_IP_CUR_LVL_0P8,
+	USB_CH_IP_CUR_LVL_0P9,
+	USB_CH_IP_CUR_LVL_1P0,
+	USB_CH_IP_CUR_LVL_1P1,
+	USB_CH_IP_CUR_LVL_1P3,
+	USB_CH_IP_CUR_LVL_1P4,
+	USB_CH_IP_CUR_LVL_1P5,
+};
+
+static int ab8500_voltage_to_regval(int voltage)
+{
+	int i;
+
+	/* Special case for voltage below 3.5V */
+	if (voltage < ab8500_charger_voltage_map[0])
+		return LOW_VOLT_REG;
+
+	for (i = 1; i < ARRAY_SIZE(ab8500_charger_voltage_map); i++) {
+		if (voltage < ab8500_charger_voltage_map[i])
+			return i - 1;
+	}
+
+	/* If not last element, return error */
+	i = ARRAY_SIZE(ab8500_charger_voltage_map) - 1;
+	if (voltage == ab8500_charger_voltage_map[i])
+		return i;
+	else
+		return -1;
+}
+
+static int ab8500_current_to_regval(int curr)
+{
+	int i;
+
+	if (curr < ab8500_charger_current_map[0])
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(ab8500_charger_current_map); i++) {
+		if (curr < ab8500_charger_current_map[i])
+			return i - 1;
+	}
+
+	/* If not last element, return error */
+	i = ARRAY_SIZE(ab8500_charger_current_map) - 1;
+	if (curr == ab8500_charger_current_map[i])
+		return i;
+	else
+		return -1;
+}
+
+static int ab8500_vbus_in_curr_to_regval(int curr)
+{
+	int i;
+
+	if (curr < ab8500_charger_vbus_in_curr_map[0])
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(ab8500_charger_vbus_in_curr_map); i++) {
+		if (curr < ab8500_charger_vbus_in_curr_map[i])
+			return i - 1;
+	}
+
+	/* If not last element, return error */
+	i = ARRAY_SIZE(ab8500_charger_vbus_in_curr_map) - 1;
+	if (curr == ab8500_charger_vbus_in_curr_map[i])
+		return i;
+	else
+		return -1;
+}
+
+/**
+ * ab8500_charger_get_usb_cur() - get usb current
+ * @di:		pointer to the ab8500_charger structre
+ *
+ * The usb stack provides the maximum current that can be drawn from
+ * the standard usb host. This will be in mA.
+ * This function converts current in mA to a value that can be written
+ * to the register. Returns -1 if charging is not allowed
+ */
+static int ab8500_charger_get_usb_cur(struct ab8500_charger *di)
+{
+	switch (di->usb_state.usb_current) {
+	case 100:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P09;
+		break;
+	case 200:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P19;
+		break;
+	case 300:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P29;
+		break;
+	case 400:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P38;
+		break;
+	case 500:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
+		break;
+	default:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		return -1;
+		break;
+	};
+	return 0;
+}
+
+/**
+ * ab8500_charger_set_vbus_in_curr() - set VBUS input current limit
+ * @di:		pointer to the ab8500_charger structure
+ * @ich_in:	charger input current limit
+ *
+ * Sets the current that can be drawn from the USB host
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_charger_set_vbus_in_curr(struct ab8500_charger *di,
+		int ich_in)
+{
+	int ret;
+	int input_curr_index;
+	int min_value;
+
+	/* We should always use to lowest current limit */
+	min_value = min(di->bat->chg_params->usb_curr_max, ich_in);
+
+	input_curr_index = ab8500_vbus_in_curr_to_regval(min_value);
+	if (input_curr_index < 0) {
+		dev_err(di->dev, "VBUS input current limit too high\n");
+		return -ENXIO;
+	}
+
+	ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+		AB8500_USBCH_IPT_CRNTLVL_REG,
+		input_curr_index << VBUS_IN_CURR_LIM_SHIFT);
+	if (ret)
+		dev_err(di->dev, "%s write failed\n", __func__);
+
+	return ret;
+}
+
+/**
+ * ab8500_charger_led_en() - turn on/off chargign led
+ * @di:		pointer to the ab8500_charger structure
+ * @on:		flag to turn on/off the chargign led
+ *
+ * Power ON/OFF charging LED indication
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_charger_led_en(struct ab8500_charger *di, int on)
+{
+	int ret;
+
+	if (on) {
+		/* Power ON charging LED indicator, set LED current to 5mA */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_LED_INDICATOR_PWM_CTRL,
+			(LED_IND_CUR_5MA | LED_INDICATOR_PWM_ENA));
+		if (ret) {
+			dev_err(di->dev, "Power ON LED failed\n");
+			return ret;
+		}
+		/* LED indicator PWM duty cycle 252/256 */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_LED_INDICATOR_PWM_DUTY,
+			LED_INDICATOR_PWM_DUTY_252_256);
+		if (ret) {
+			dev_err(di->dev, "Set LED PWM duty cycle failed\n");
+			return ret;
+		}
+	} else {
+		/* Power off charging LED indicator */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_LED_INDICATOR_PWM_CTRL,
+			LED_INDICATOR_PWM_DIS);
+		if (ret) {
+			dev_err(di->dev, "Power-off LED failed\n");
+			return ret;
+		}
+	}
+
+	return ret;
+}
+
+/**
+ * ab8500_charger_ac_en() - enable or disable ac charging
+ * @di:		pointer to the ab8500_charger structure
+ * @enable:	enable/disable flag
+ * @vset:	charging voltage
+ * @iset:	charging current
+ *
+ * Enable/Disable AC/Mains charging and turns on/off the charging led
+ * respectively.
+ **/
+static int ab8500_charger_ac_en(struct ux500_charger *charger,
+	int enable, int vset, int iset)
+{
+	int ret;
+	int volt_index;
+	int curr_index;
+	int input_curr_index;
+	u8 overshoot = 0;
+
+	struct ab8500_charger *di = to_ab8500_charger_ac_device_info(charger);
+
+	if (enable) {
+		/* Check if AC is connected */
+		if (!di->ac.charger_connected) {
+			dev_err(di->dev, "AC charger not connected\n");
+			return -ENXIO;
+		}
+
+		/* Enable AC charging */
+		dev_dbg(di->dev, "Enable AC: %dmV %dmA\n", vset, iset);
+
+		/* Check if the requested voltage or current is valid */
+		volt_index = ab8500_voltage_to_regval(vset);
+		curr_index = ab8500_current_to_regval(iset);
+		input_curr_index = ab8500_current_to_regval(
+			di->bat->chg_params->ac_curr_max);
+		if (volt_index < 0 || curr_index < 0 || input_curr_index < 0) {
+			dev_err(di->dev,
+				"Charger voltage or current too high, "
+				"charging not started\n");
+			return -ENXIO;
+		}
+
+		/* ChVoltLevel: maximum battery charging voltage */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_CH_VOLT_LVL_REG, (u8) volt_index);
+		if (ret) {
+			dev_err(di->dev, "%s write failed\n", __func__);
+			return ret;
+		}
+		/* MainChInputCurr: current that can be drawn from the charger*/
+		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_MCH_IPT_CURLVL_REG,
+			input_curr_index << MAIN_CH_INPUT_CURR_SHIFT);
+		if (ret) {
+			dev_err(di->dev, "%s write failed\n", __func__);
+			return ret;
+		}
+		/* ChOutputCurentLevel: protected output current */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_CH_OPT_CRNTLVL_REG, (u8) curr_index);
+		if (ret) {
+			dev_err(di->dev, "%s write failed\n", __func__);
+			return ret;
+		}
+
+		/* Check if VBAT overshoot control should be enabled */
+		if (!di->bat->enable_overshoot)
+			overshoot = MAIN_CH_NO_OVERSHOOT_ENA_N;
+
+		/* Enable Main Charger */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_MCH_CTRL1, MAIN_CH_ENA | overshoot);
+		if (ret) {
+			dev_err(di->dev, "%s write failed\n", __func__);
+			return ret;
+		}
+
+		/* Power on charging LED indication */
+		ret = ab8500_charger_led_en(di, true);
+		if (ret < 0)
+			dev_err(di->dev, "failed to enable LED\n");
+
+		di->ac.charger_online = 1;
+	} else {
+		/* Disable AC charging */
+
+		switch (di->chip_id) {
+		case AB8500_CUT1P0:
+		case AB8500_CUT1P1:
+			/*
+			 * For ABB revision 1.0 and 1.1 there is a bug in the
+			 * watchdog logic. That means we have to continously
+			 * kick the charger watchdog even when no charger is
+			 * connected. This is only valid once the AC charger
+			 * has been enabled. This is a bug that is not handled
+			 * by the algorithm and the watchdog have to be kicked
+			 * by the charger driver when the AC charger
+			 * is disabled
+			 */
+			if (di->ac_conn) {
+				queue_delayed_work(di->charger_wq,
+					&di->kick_wd_work,
+					round_jiffies(WD_KICK_INTERVAL));
+			}
+
+			/*
+			 * We can't turn off charging completely
+			 * due to a bug in AB8500 cut1.
+			 * If we do, charging will not start again.
+			 * That is why we set the lowest voltage
+			 * and current possible
+			 */
+			ret = abx500_set_register_interruptible(di->dev,
+				AB8500_CHARGER,
+				AB8500_CH_VOLT_LVL_REG, CH_VOL_LVL_3P5);
+			if (ret) {
+				dev_err(di->dev,
+					"%s write failed\n", __func__);
+				return ret;
+			}
+
+			ret = abx500_set_register_interruptible(di->dev,
+				AB8500_CHARGER,
+				AB8500_CH_OPT_CRNTLVL_REG, CH_OP_CUR_LVL_0P1);
+			if (ret) {
+				dev_err(di->dev,
+					"%s write failed\n", __func__);
+				return ret;
+			}
+			break;
+
+		case AB8500_CUT2P0:
+		default:
+			ret = abx500_set_register_interruptible(di->dev,
+				AB8500_CHARGER,
+				AB8500_MCH_CTRL1, 0);
+			if (ret) {
+				dev_err(di->dev,
+					"%s write failed\n", __func__);
+				return ret;
+			}
+			break;
+		}
+
+		ret = ab8500_charger_led_en(di, false);
+		if (ret < 0)
+			dev_err(di->dev, "failed to disable LED\n");
+
+		di->ac.charger_online = 0;
+		di->ac.wd_expired = false;
+		dev_dbg(di->dev, "%s Disabled AC charging\n", __func__);
+	}
+	power_supply_changed(&di->ac_chg.psy);
+
+	return ret;
+}
+
+/**
+ * ab8500_charger_usb_en() - enable usb charging
+ * @di:		pointer to the ab8500_charger structure
+ * @enable:	enable/disable flag
+ * @vset:	charging voltage
+ * @ich_out:	charger output current
+ *
+ * Enable/Disable USB charging and turns on/off the charging led respectively.
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_charger_usb_en(struct ux500_charger *charger,
+	int enable, int vset, int ich_out)
+{
+	int ret;
+	int volt_index;
+	int curr_index;
+	u8 overshoot = 0;
+
+	struct ab8500_charger *di = to_ab8500_charger_usb_device_info(charger);
+
+	if (enable) {
+		/* Check if USB is connected */
+		if (!di->usb.charger_connected) {
+			dev_err(di->dev, "USB charger not connected\n");
+			return -ENXIO;
+		}
+
+		/* Enable USB charging */
+		dev_dbg(di->dev, "Enable USB: %dmV %dmA\n", vset, ich_out);
+
+		/* Check if the requested voltage or current is valid */
+		volt_index = ab8500_voltage_to_regval(vset);
+		curr_index = ab8500_current_to_regval(ich_out);
+		if (volt_index < 0 || curr_index < 0) {
+			dev_err(di->dev,
+				"Charger voltage or current too high, "
+				"charging not started\n");
+			return -ENXIO;
+		}
+
+		/* ChVoltLevel: max voltage upto which battery can be charged */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_CH_VOLT_LVL_REG, (u8) volt_index);
+		if (ret) {
+			dev_err(di->dev, "%s write failed\n", __func__);
+			return ret;
+		}
+		/* USBChInputCurr: current that can be drawn from the usb */
+		ret = ab8500_charger_set_vbus_in_curr(di, di->max_usb_in_curr);
+		if (ret) {
+			dev_err(di->dev, "setting USBChInputCurr failed\n");
+			return ret;
+		}
+		/* ChOutputCurentLevel: protected output current */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_CH_OPT_CRNTLVL_REG, (u8) curr_index);
+		if (ret) {
+			dev_err(di->dev, "%s write failed\n", __func__);
+			return ret;
+		}
+		/* Check if VBAT overshoot control should be enabled */
+		if (!di->bat->enable_overshoot)
+			overshoot = USB_CHG_NO_OVERSHOOT_ENA_N;
+
+		/* Enable USB Charger */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_USBCH_CTRL1_REG, USB_CH_ENA | overshoot);
+		if (ret) {
+			dev_err(di->dev, "%s write failed\n", __func__);
+			return ret;
+		}
+
+		/* If success power on charging LED indication */
+		ret = ab8500_charger_led_en(di, true);
+		if (ret < 0)
+			dev_err(di->dev, "failed to enable LED\n");
+
+		di->usb.charger_online = 1;
+	} else {
+		/* Disable USB charging */
+		ret = abx500_set_register_interruptible(di->dev,
+			AB8500_CHARGER,
+			AB8500_USBCH_CTRL1_REG, 0);
+		if (ret) {
+			dev_err(di->dev,
+				"%s write failed\n", __func__);
+			return ret;
+		}
+
+		ret = ab8500_charger_led_en(di, false);
+		if (ret < 0)
+			dev_err(di->dev, "failed to disable LED\n");
+
+		di->usb.charger_online = 0;
+		di->usb.wd_expired = false;
+		dev_dbg(di->dev, "%s Disabled USB charging\n", __func__);
+	}
+	power_supply_changed(&di->usb_chg.psy);
+
+	return ret;
+}
+
+/**
+ * ab8500_charger_watchdog_kick() - kick charger watchdog
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * Kick charger watchdog
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_charger_watchdog_kick(struct ux500_charger *charger)
+{
+	int ret;
+	struct ab8500_charger *di;
+
+	if (charger->psy.type == POWER_SUPPLY_TYPE_MAINS)
+		di = to_ab8500_charger_ac_device_info(charger);
+	else if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+		di = to_ab8500_charger_usb_device_info(charger);
+	else
+		return -ENXIO;
+
+	ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+		AB8500_CHARG_WD_CTRL, CHARG_WD_KICK);
+	if (ret)
+		dev_err(di->dev, "Failed to kick WD!\n");
+
+	return ret;
+}
+
+/**
+ * ab8500_charger_update_charger_current() - update charger current
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * Update the charger output current for the specified charger
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_charger_update_charger_current(struct ux500_charger *charger,
+		int ich_out)
+{
+	int ret;
+	int curr_index;
+	struct ab8500_charger *di;
+
+	if (charger->psy.type == POWER_SUPPLY_TYPE_MAINS)
+		di = to_ab8500_charger_ac_device_info(charger);
+	else if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+		di = to_ab8500_charger_usb_device_info(charger);
+	else
+		return -ENXIO;
+
+	curr_index = ab8500_current_to_regval(ich_out);
+	if (curr_index < 0) {
+		dev_err(di->dev,
+			"Charger current too high, "
+			"charging not started\n");
+		return -ENXIO;
+	}
+
+	ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+		AB8500_CH_OPT_CRNTLVL_REG, (u8) curr_index);
+	if (ret) {
+		dev_err(di->dev, "%s write failed\n", __func__);
+		return ret;
+	}
+
+	/* Reset the main and usb drop input current measurement counter */
+	ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+				AB8500_CHARGER_CTRL,
+				0x1);
+	if (ret) {
+		dev_err(di->dev, "%s write failed\n", __func__);
+		return ret;
+	}
+
+	return ret;
+}
+
+/**
+ * ab8500_charger_check_hw_failure_work() - check main charger failure
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the main charger status
+ */
+static void ab8500_charger_check_hw_failure_work(struct work_struct *work)
+{
+	int ret;
+	u8 reg_value;
+
+	struct ab8500_charger *di = container_of(work,
+		struct ab8500_charger, check_hw_failure_work.work);
+
+	/* Check if the status bits for HW failure is still active */
+	if (di->flags.mainextchnotok) {
+		ret = abx500_get_register_interruptible(di->dev,
+			AB8500_CHARGER, AB8500_CH_STATUS2_REG, &reg_value);
+		if (ret < 0) {
+			dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+			return;
+		}
+		if (!(reg_value & MAIN_CH_NOK)) {
+			di->flags.mainextchnotok = false;
+			power_supply_changed(&di->ac_chg.psy);
+		}
+	}
+	if (di->flags.vbus_ovv) {
+		ret = abx500_get_register_interruptible(di->dev,
+			AB8500_CHARGER, AB8500_CH_USBCH_STAT2_REG,
+			&reg_value);
+		if (ret < 0) {
+			dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+			return;
+		}
+		if (!(reg_value & VBUS_OVV_TH)) {
+			di->flags.vbus_ovv = false;
+			power_supply_changed(&di->usb_chg.psy);
+		}
+	}
+	/* If we still have a failure, schedule a new check */
+	if (di->flags.mainextchnotok || di->flags.vbus_ovv) {
+		queue_delayed_work(di->charger_wq,
+			&di->check_hw_failure_work, round_jiffies(HZ));
+	}
+}
+
+/**
+ * ab8500_charger_kick_watchdog_work() - kick the watchdog
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for kicking the charger watchdog.
+ *
+ * For ABB revision 1.0 and 1.1 there is a bug in the watchdog
+ * logic. That means we have to continously kick the charger
+ * watchdog even when no charger is connected. This is only
+ * valid once the AC charger has been enabled. This is
+ * a bug that is not handled by the algorithm and the
+ * watchdog have to be kicked by the charger driver
+ * when the AC charger is disabled
+ */
+static void ab8500_charger_kick_watchdog_work(struct work_struct *work)
+{
+	int ret;
+
+	struct ab8500_charger *di = container_of(work,
+		struct ab8500_charger, kick_wd_work.work);
+
+	ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+		AB8500_CHARG_WD_CTRL, CHARG_WD_KICK);
+	if (ret)
+		dev_err(di->dev, "Failed to kick WD!\n");
+
+	/* Schedule a new watchdog kick */
+	queue_delayed_work(di->charger_wq,
+		&di->kick_wd_work, round_jiffies(WD_KICK_INTERVAL));
+}
+
+/**
+ * ab8500_charger_ac_work() - work to get and set main charger status
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the main charger status
+ */
+static void ab8500_charger_ac_work(struct work_struct *work)
+{
+	int ret;
+
+	struct ab8500_charger *di = container_of(work,
+		struct ab8500_charger, ac_work);
+
+	/*
+	 * Since we can't be sure that the events are received
+	 * synchronously, we have the check if the main charger is
+	 * connected by reading the status register
+	 */
+	ret = ab8500_charger_detect_chargers(di);
+	if (ret < 0)
+		return;
+
+	if (ret & AC_PW_CONN) {
+		di->ac.charger_connected = 1;
+		di->ac_conn = true;
+	} else {
+		di->ac.charger_connected = 0;
+	}
+
+	power_supply_changed(&di->ac_chg.psy);
+}
+
+/**
+ * ab8500_charger_detect_usb_type_work() - work to detect USB type
+ * @work:	Pointer to the work_struct structure
+ *
+ * Detect the type of USB plugged
+ */
+void ab8500_charger_detect_usb_type_work(struct work_struct *work)
+{
+	int ret;
+
+	struct ab8500_charger *di = container_of(work,
+		struct ab8500_charger, detect_usb_type_work);
+
+	/*
+	 * Since we can't be sure that the events are received
+	 * synchronously, we have the check if is
+	 * connected by reading the status register
+	 */
+	ret = ab8500_charger_detect_chargers(di);
+	if (ret < 0)
+		return;
+
+	if (!(ret & USB_PW_CONN)) {
+		di->vbus_detected = 0;
+		di->usb.charger_connected = 0;
+		power_supply_changed(&di->usb_chg.psy);
+	} else {
+		di->vbus_detected = 1;
+
+		switch (di->chip_id) {
+		case AB8500_CUT1P0:
+		case AB8500_CUT1P1:
+			ret = ab8500_charger_detect_usb_type(di);
+			if (!ret) {
+				di->usb.charger_connected = 1;
+				power_supply_changed(&di->usb_chg.psy);
+			}
+			break;
+
+		case AB8500_CUT2P0:
+		default:
+			/* For ABB cut2.0 and onwards we have an IRQ,
+			 * USB_LINK_STATUS that will be triggered when the USB
+			 * link status changes. The exception is USB connected
+			 * during startup. Then we don't get a
+			 * USB_LINK_STATUS IRQ
+			 */
+			if (di->vbus_detected_start) {
+				di->vbus_detected_start = false;
+				ret = ab8500_charger_detect_usb_type(di);
+				if (!ret) {
+					di->usb.charger_connected = 1;
+					power_supply_changed(&di->usb_chg.psy);
+				}
+			}
+			break;
+		}
+	}
+}
+
+/**
+ * ab8500_charger_usb_link_status_work() - work to detect USB type
+ * @work:	pointer to the work_struct structure
+ *
+ * Detect the type of USB plugged
+ */
+static void ab8500_charger_usb_link_status_work(struct work_struct *work)
+{
+	int ret;
+
+	struct ab8500_charger *di = container_of(work,
+		struct ab8500_charger, usb_link_status_work);
+
+	/*
+	 * Since we can't be sure that the events are received
+	 * synchronously, we have the check if  is
+	 * connected by reading the status register
+	 */
+	ret = ab8500_charger_detect_chargers(di);
+	if (ret < 0)
+		return;
+
+	if (!(ret & USB_PW_CONN)) {
+		di->vbus_detected = 0;
+		di->usb.charger_connected = 0;
+		power_supply_changed(&di->usb_chg.psy);
+	} else {
+		di->vbus_detected = 1;
+		ret = ab8500_charger_read_usb_type(di);
+		if (!ret) {
+			/* Update maximum input current */
+			ret = ab8500_charger_set_vbus_in_curr(di,
+					di->max_usb_in_curr);
+			if (ret)
+				return;
+
+			di->usb.charger_connected = 1;
+			power_supply_changed(&di->usb_chg.psy);
+		} else if (ret == -ENXIO) {
+			/* No valid charger type detected */
+			di->usb.charger_connected = 0;
+			power_supply_changed(&di->usb_chg.psy);
+		}
+	}
+}
+
+static void ab8500_charger_usb_state_changed_work(struct work_struct *work)
+{
+	int ret;
+	unsigned long flags;
+
+	struct ab8500_charger *di = container_of(work,
+		struct ab8500_charger, usb_state_changed_work);
+
+	if (!di->vbus_detected)
+		return;
+
+	spin_lock_irqsave(&di->usb_state.usb_lock, flags);
+	di->usb_state.usb_changed = false;
+	spin_unlock_irqrestore(&di->usb_state.usb_lock, flags);
+
+	/*
+	 * wait for some time until you get updates from the usb stack
+	 * and negotiations are completed
+	 */
+	msleep(250);
+
+	if (di->usb_state.usb_changed)
+		return;
+
+	dev_dbg(di->dev, "%s USB state: 0x%02x mA: %d\n",
+		__func__, di->usb_state.state, di->usb_state.usb_current);
+
+	switch (di->usb_state.state) {
+	case AB8500_BM_USB_STATE_RESET_HS:
+	case AB8500_BM_USB_STATE_RESET_FS:
+	case AB8500_BM_USB_STATE_SUSPEND:
+	case AB8500_BM_USB_STATE_MAX:
+		di->usb.charger_connected = 0;
+		power_supply_changed(&di->usb_chg.psy);
+		break;
+
+	case AB8500_BM_USB_STATE_RESUME:
+		/*
+		 * when suspend->resume there should be delay
+		 * of 1sec for enabling charging
+		 */
+		msleep(1000);
+		/* Intentional fall through */
+	case AB8500_BM_USB_STATE_CONFIGURED:
+		/*
+		 * USB is configured, enable charging with the charging
+		 * input current obtained from USB driver
+		 */
+		if (!ab8500_charger_get_usb_cur(di)) {
+			/* Update maximum input current */
+			ret = ab8500_charger_set_vbus_in_curr(di,
+					di->max_usb_in_curr);
+			if (ret)
+				return;
+
+			di->usb.charger_connected = 1;
+			power_supply_changed(&di->usb_chg.psy);
+		}
+		break;
+
+	default:
+		break;
+	};
+}
+
+/**
+ * ab8500_charger_check_usbchargernotok_work() - check USB chg not ok status
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the USB charger Not OK status
+ */
+static void ab8500_charger_check_usbchargernotok_work(struct work_struct *work)
+{
+	int ret;
+	u8 reg_value;
+	bool prev_status;
+
+	struct ab8500_charger *di = container_of(work,
+		struct ab8500_charger, check_usbchgnotok_work.work);
+
+	/* Check if the status bit for usbchargernotok is still active */
+	ret = abx500_get_register_interruptible(di->dev,
+		AB8500_CHARGER, AB8500_CH_USBCH_STAT2_REG, &reg_value);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+		return;
+	}
+	prev_status = di->flags.usbchargernotok;
+
+	if (reg_value & VBUS_CH_NOK) {
+		di->flags.usbchargernotok = true;
+		/* Check again in 1sec */
+		queue_delayed_work(di->charger_wq,
+			&di->check_usbchgnotok_work, HZ);
+	} else {
+		di->flags.usbchargernotok = false;
+		di->flags.vbus_collapse = false;
+	}
+
+	if (prev_status != di->flags.usbchargernotok)
+		power_supply_changed(&di->usb_chg.psy);
+}
+
+/**
+ * ab8500_charger_check_main_thermal_prot_work() - check main thermal status
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the Main thermal prot status
+ */
+static void ab8500_charger_check_main_thermal_prot_work(
+	struct work_struct *work)
+{
+	int ret;
+	u8 reg_value;
+
+	struct ab8500_charger *di = container_of(work,
+		struct ab8500_charger, check_main_thermal_prot_work);
+
+	/* Check if the status bit for main_thermal_prot is still active */
+	ret = abx500_get_register_interruptible(di->dev,
+		AB8500_CHARGER, AB8500_CH_STATUS2_REG, &reg_value);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+		return;
+	}
+	if (reg_value & MAIN_CH_TH_PROT)
+		di->flags.main_thermal_prot = true;
+	else
+		di->flags.main_thermal_prot = false;
+
+	power_supply_changed(&di->ac_chg.psy);
+}
+
+/**
+ * ab8500_charger_check_usb_thermal_prot_work() - check usb thermal status
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the USB thermal prot status
+ */
+static void ab8500_charger_check_usb_thermal_prot_work(
+	struct work_struct *work)
+{
+	int ret;
+	u8 reg_value;
+
+	struct ab8500_charger *di = container_of(work,
+		struct ab8500_charger, check_usb_thermal_prot_work);
+
+	/* Check if the status bit for usb_thermal_prot is still active */
+	ret = abx500_get_register_interruptible(di->dev,
+		AB8500_CHARGER, AB8500_CH_USBCH_STAT2_REG, &reg_value);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab8500 read failed\n", __func__);
+		return;
+	}
+	if (reg_value & USB_CH_TH_PROT)
+		di->flags.usb_thermal_prot = true;
+	else
+		di->flags.usb_thermal_prot = false;
+
+	power_supply_changed(&di->usb_chg.psy);
+}
+
+/**
+ * ab8500_charger_mainchunplugdet_handler() - main charger unplugged
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_mainchunplugdet_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev, "Main charger unplugged\n");
+	queue_work(di->charger_wq, &di->ac_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_mainchplugdet_handler() - main charger plugged
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_mainchplugdet_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev, "Main charger plugged\n");
+	queue_work(di->charger_wq, &di->ac_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_mainextchnotok_handler() - main charger not ok
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_mainextchnotok_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev, "Main charger not ok\n");
+	di->flags.mainextchnotok = true;
+	power_supply_changed(&di->ac_chg.psy);
+
+	/* Schedule a new HW failure check */
+	queue_delayed_work(di->charger_wq, &di->check_hw_failure_work, 0);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_mainchthprotr_handler() - Die temp is above main charger
+ * thermal protection threshold
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_mainchthprotr_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev,
+		"Die temp above Main charger thermal protection threshold\n");
+	queue_work(di->charger_wq, &di->check_main_thermal_prot_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_mainchthprotf_handler() - Die temp is below main charger
+ * thermal protection threshold
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_mainchthprotf_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev,
+		"Die temp ok for Main charger thermal protection threshold\n");
+	queue_work(di->charger_wq, &di->check_main_thermal_prot_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_vbusdetf_handler() - VBUS falling detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_vbusdetf_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev, "VBUS falling detected\n");
+	queue_work(di->charger_wq, &di->detect_usb_type_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_vbusdetr_handler() - VBUS rising detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_vbusdetr_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	di->vbus_detected = true;
+	dev_dbg(di->dev, "VBUS rising detected\n");
+	queue_work(di->charger_wq, &di->detect_usb_type_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_usblinkstatus_handler() - USB link status has changed
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_usblinkstatus_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev, "USB link status changed\n");
+
+	queue_work(di->charger_wq, &di->usb_link_status_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_usbchthprotr_handler() - Die temp is above usb charger
+ * thermal protection threshold
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_usbchthprotr_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev,
+		"Die temp above USB charger thermal protection threshold\n");
+	queue_work(di->charger_wq, &di->check_usb_thermal_prot_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_usbchthprotf_handler() - Die temp is below usb charger
+ * thermal protection threshold
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_usbchthprotf_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev,
+		"Die temp ok for USB charger thermal protection threshold\n");
+	queue_work(di->charger_wq, &di->check_usb_thermal_prot_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_usbchargernotokr_handler() - USB charger not ok detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_usbchargernotokr_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev, "Not allowed USB charger detected\n");
+	queue_delayed_work(di->charger_wq, &di->check_usbchgnotok_work, 0);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_chwdexp_handler() - Charger watchdog expired
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_chwdexp_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev, "Charger watchdog expired\n");
+
+	/*
+	 * The charger that was online when the watchdog expired
+	 * needs to be restarted for charging to start again
+	 */
+	if (di->ac.charger_online) {
+		di->ac.wd_expired = true;
+		power_supply_changed(&di->ac_chg.psy);
+	}
+	if (di->usb.charger_online) {
+		di->usb.wd_expired = true;
+		power_supply_changed(&di->usb_chg.psy);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_vbusovv_handler() - VBUS overvoltage detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_charger_vbusovv_handler(int irq, void *_di)
+{
+	struct ab8500_charger *di = _di;
+
+	dev_dbg(di->dev, "VBUS overvoltage detected\n");
+	di->flags.vbus_ovv = true;
+	power_supply_changed(&di->usb_chg.psy);
+
+	/* Schedule a new HW failure check */
+	queue_delayed_work(di->charger_wq, &di->check_hw_failure_work, 0);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_charger_ac_get_property() - get the ac/mains properties
+ * @psy:       pointer to the power_supply structure
+ * @psp:       pointer to the power_supply_property structure
+ * @val:       pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the ac/mains
+ * properties by reading the sysfs files.
+ * AC/Mains properties are online, present and voltage.
+ * online:     ac/mains charging is in progress or not
+ * present:    presence of the ac/mains
+ * voltage:    AC/Mains voltage
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_charger_ac_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct ab8500_charger *di;
+
+	di = to_ab8500_charger_ac_device_info(psy_to_ux500_charger(psy));
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_HEALTH:
+		if (di->flags.mainextchnotok)
+			val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		else if (di->ac.wd_expired || di->usb.wd_expired)
+			val->intval = POWER_SUPPLY_HEALTH_DEAD;
+		else if (di->flags.main_thermal_prot)
+			val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		else
+			val->intval = POWER_SUPPLY_HEALTH_GOOD;
+		break;
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = di->ac.charger_online;
+		break;
+	case POWER_SUPPLY_PROP_PRESENT:
+		val->intval = di->ac.charger_connected;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		di->ac.charger_voltage = ab8500_charger_get_ac_voltage(di);
+		val->intval = di->ac.charger_voltage * 1000;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_AVG:
+		/*
+		 * This property is used to indicate when CV mode is entered
+		 * for the AC charger
+		 */
+		di->ac.cv_active = ab8500_charger_ac_cv(di);
+		val->intval = di->ac.cv_active;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		val->intval = ab8500_charger_get_ac_current(di) * 1000;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/**
+ * ab8500_charger_usb_get_property() - get the usb properties
+ * @psy:        pointer to the power_supply structure
+ * @psp:        pointer to the power_supply_property structure
+ * @val:        pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the usb
+ * properties by reading the sysfs files.
+ * USB properties are online, present and voltage.
+ * online:     usb charging is in progress or not
+ * present:    presence of the usb
+ * voltage:    vbus voltage
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_charger_usb_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct ab8500_charger *di;
+
+	di = to_ab8500_charger_usb_device_info(psy_to_ux500_charger(psy));
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_HEALTH:
+		if (di->flags.usbchargernotok)
+			val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		else if (di->ac.wd_expired || di->usb.wd_expired)
+			val->intval = POWER_SUPPLY_HEALTH_DEAD;
+		else if (di->flags.usb_thermal_prot)
+			val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		else if (di->flags.vbus_ovv)
+			val->intval = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+		else
+			val->intval = POWER_SUPPLY_HEALTH_GOOD;
+		break;
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = di->usb.charger_online;
+		break;
+	case POWER_SUPPLY_PROP_PRESENT:
+		val->intval = di->usb.charger_connected;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		di->usb.charger_voltage = ab8500_charger_get_vbus_voltage(di);
+		val->intval = di->usb.charger_voltage * 1000;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_AVG:
+		/*
+		 * This property is used to indicate when CV mode is entered
+		 * for the USB charger
+		 */
+		di->usb.cv_active = ab8500_charger_usb_cv(di);
+		val->intval = di->usb.cv_active;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		val->intval = ab8500_charger_get_usb_current(di) * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_AVG:
+		/*
+		 * This property is used to indicate when VBUS has collapsed
+		 * due to too high output current from the USB charger
+		 */
+		if (di->flags.vbus_collapse)
+			val->intval = 1;
+		else
+			val->intval = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/**
+ * ab8500_charger_init_hw_registers() - Set up charger related registers
+ * @di:		pointer to the ab8500_charger structure
+ *
+ * Set up charger OVV, watchdog and maximum voltage registers as well as
+ * charging of the backup battery
+ */
+static int ab8500_charger_init_hw_registers(struct ab8500_charger *di)
+{
+	int ret = 0;
+
+	/* Setup maximum charger current and voltage for ABB cut2.0 */
+	switch (di->chip_id) {
+	case AB8500_CUT1P0:
+	case AB8500_CUT1P1:
+		break;
+	case AB8500_CUT2P0:
+	default:
+		ret = abx500_set_register_interruptible(di->dev,
+			AB8500_CHARGER,
+			AB8500_CH_VOLT_LVL_MAX_REG, CH_VOL_LVL_4P6);
+		if (ret) {
+			dev_err(di->dev,
+				"failed to set CH_VOLT_LVL_MAX_REG\n");
+			goto out;
+		}
+
+		ret = abx500_set_register_interruptible(di->dev,
+			AB8500_CHARGER,
+			AB8500_CH_OPT_CRNTLVL_MAX_REG, CH_OP_CUR_LVL_1P6);
+		if (ret) {
+			dev_err(di->dev,
+				"failed to set CH_OPT_CRNTLVL_MAX_REG\n");
+			goto out;
+		}
+
+		break;
+	}
+
+	/* VBUS OVV set to 6.3V and enable automatic current limitiation */
+	ret = abx500_set_register_interruptible(di->dev,
+		AB8500_CHARGER,
+		AB8500_USBCH_CTRL2_REG,
+		VBUS_OVV_SELECT_6P3V | VBUS_AUTO_IN_CURR_LIM_ENA);
+	if (ret) {
+		dev_err(di->dev, "failed to set VBUS OVV\n");
+		goto out;
+	}
+
+	/* Enable main watchdog in OTP */
+	ret = abx500_set_register_interruptible(di->dev,
+		AB8500_OTP_EMUL, AB8500_OTP_CONF_15, OTP_ENABLE_WD);
+	if (ret) {
+		dev_err(di->dev, "failed to enable main WD in OTP\n");
+		goto out;
+	}
+
+	/* Enable main watchdog */
+	ret = abx500_set_register_interruptible(di->dev,
+		AB8500_SYS_CTRL2_BLOCK,
+		AB8500_MAIN_WDOG_CTRL_REG, MAIN_WDOG_ENA);
+	if (ret) {
+		dev_err(di->dev, "faile to enable main watchdog\n");
+		goto out;
+	}
+
+	/*
+	 * Due to internal synchronisation, Enable and Kick watchdog bits
+	 * cannot be enabled in a single write.
+	 * A minimum delay of 2*32 kHz period (62.5µs) must be inserted
+	 * between writing Enable then Kick bits.
+	 */
+	udelay(63);
+
+	/* Kick main watchdog */
+	ret = abx500_set_register_interruptible(di->dev,
+		AB8500_SYS_CTRL2_BLOCK,
+		AB8500_MAIN_WDOG_CTRL_REG,
+		(MAIN_WDOG_ENA | MAIN_WDOG_KICK));
+	if (ret) {
+		dev_err(di->dev, "failed to kick main watchdog\n");
+		goto out;
+	}
+
+	/* Disable main watchdog */
+	ret = abx500_set_register_interruptible(di->dev,
+		AB8500_SYS_CTRL2_BLOCK,
+		AB8500_MAIN_WDOG_CTRL_REG, MAIN_WDOG_DIS);
+	if (ret) {
+		dev_err(di->dev, "failed to disable main watchdog\n");
+		goto out;
+	}
+
+	/* Set watchdog timeout */
+	ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+		AB8500_CH_WD_TIMER_REG, WD_TIMER);
+	if (ret) {
+		dev_err(di->dev, "failed to set charger watchdog timeout\n");
+		goto out;
+	}
+
+	/* Backup battery voltage and current */
+	ret = abx500_set_register_interruptible(di->dev,
+		AB8500_RTC,
+		AB8500_RTC_BACKUP_CHG_REG,
+		di->bat->bkup_bat_v |
+		di->bat->bkup_bat_i);
+	if (ret) {
+		dev_err(di->dev, "failed to setup backup battery charging\n");
+		goto out;
+	}
+
+	/* Enable backup battery charging */
+	abx500_mask_and_set_register_interruptible(di->dev,
+		AB8500_RTC, AB8500_RTC_CTRL_REG,
+		RTC_BUP_CH_ENA, RTC_BUP_CH_ENA);
+	if (ret < 0)
+		dev_err(di->dev, "%s mask and set failed\n", __func__);
+
+out:
+	return ret;
+}
+
+/*
+ * ab8500 charger driver interrupts and their respective isr
+ */
+static struct ab8500_charger_interrupts ab8500_charger_irq[] = {
+	{"MAIN_CH_UNPLUG_DET", ab8500_charger_mainchunplugdet_handler},
+	{"MAIN_CHARGE_PLUG_DET", ab8500_charger_mainchplugdet_handler},
+	{"MAIN_EXT_CH_NOT_OK", ab8500_charger_mainextchnotok_handler},
+	{"MAIN_CH_TH_PROT_R", ab8500_charger_mainchthprotr_handler},
+	{"MAIN_CH_TH_PROT_F", ab8500_charger_mainchthprotf_handler},
+	{"VBUS_DET_F", ab8500_charger_vbusdetf_handler},
+	{"VBUS_DET_R", ab8500_charger_vbusdetr_handler},
+	{"USB_LINK_STATUS", ab8500_charger_usblinkstatus_handler},
+	{"USB_CH_TH_PROT_R", ab8500_charger_usbchthprotr_handler},
+	{"USB_CH_TH_PROT_F", ab8500_charger_usbchthprotf_handler},
+	{"USB_CHARGER_NOT_OKR", ab8500_charger_usbchargernotokr_handler},
+	{"VBUS_OVV", ab8500_charger_vbusovv_handler},
+	{"CH_WD_EXP", ab8500_charger_chwdexp_handler},
+};
+
+static int ab8500_charger_usb_notifier_call(struct notifier_block *nb,
+		unsigned long event, void *power)
+{
+	struct ab8500_charger *di =
+		container_of(nb, struct ab8500_charger, nb);
+	enum ab8500_usb_state bm_usb_state;
+	unsigned mA = *((unsigned *)power);
+
+	/* TODO: State is fabricate  here. See if charger really needs USB
+	 * state or if mA is enough
+	 */
+	if ((di->usb_state.usb_current == 2) && (mA > 2))
+		bm_usb_state = AB8500_BM_USB_STATE_RESUME;
+	else if (mA == 0)
+		bm_usb_state = AB8500_BM_USB_STATE_RESET_HS;
+	else if (mA == 2)
+		bm_usb_state = AB8500_BM_USB_STATE_SUSPEND;
+	else if (mA >= 8) /* 8, 100, 500 */
+		bm_usb_state = AB8500_BM_USB_STATE_CONFIGURED;
+	else /* Should never occur */
+		bm_usb_state = AB8500_BM_USB_STATE_RESET_FS;
+
+	dev_dbg(di->dev, "%s usb_state: 0x%02x mA: %d\n",
+		__func__, bm_usb_state, mA);
+
+	spin_lock(&di->usb_state.usb_lock);
+	di->usb_state.usb_changed = true;
+	spin_unlock(&di->usb_state.usb_lock);
+
+	di->usb_state.state = bm_usb_state;
+	di->usb_state.usb_current = mA;
+
+	queue_work(di->charger_wq, &di->usb_state_changed_work);
+
+	return NOTIFY_OK;
+}
+
+#if defined(CONFIG_PM)
+static int ab8500_charger_resume(struct platform_device *pdev)
+{
+	int ret;
+	struct ab8500_charger *di = platform_get_drvdata(pdev);
+
+	/*
+	 * For ABB revision 1.0 and 1.1 there is a bug in the watchdog
+	 * logic. That means we have to continously kick the charger
+	 * watchdog even when no charger is connected. This is only
+	 * valid once the AC charger has been enabled. This is
+	 * a bug that is not handled by the algorithm and the
+	 * watchdog have to be kicked by the charger driver
+	 * when the AC charger is disabled
+	 */
+	if (di->ac_conn && (di->chip_id == AB8500_CUT1P0 ||
+		di->chip_id == AB8500_CUT1P1)) {
+		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
+			AB8500_CHARG_WD_CTRL, CHARG_WD_KICK);
+		if (ret)
+			dev_err(di->dev, "Failed to kick WD!\n");
+
+		/* If not already pending start a new timer */
+		if (!delayed_work_pending(
+			&di->kick_wd_work)) {
+			queue_delayed_work(di->charger_wq, &di->kick_wd_work,
+				round_jiffies(WD_KICK_INTERVAL));
+		}
+	}
+
+	/* If we still have a HW failure, schedule a new check */
+	if (di->flags.mainextchnotok || di->flags.vbus_ovv) {
+		queue_delayed_work(di->charger_wq,
+			&di->check_hw_failure_work, 0);
+	}
+
+	return 0;
+}
+
+static int ab8500_charger_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct ab8500_charger *di = platform_get_drvdata(pdev);
+
+	/* Cancel any pending HW failure check */
+	if (delayed_work_pending(&di->check_hw_failure_work))
+		cancel_delayed_work(&di->check_hw_failure_work);
+
+	return 0;
+}
+#else
+#define ab8500_charger_suspend      NULL
+#define ab8500_charger_resume       NULL
+#endif
+
+static int __devexit ab8500_charger_remove(struct platform_device *pdev)
+{
+	struct ab8500_charger *di = platform_get_drvdata(pdev);
+	int i, irq, ret;
+
+	/* Disable AC charging */
+	ab8500_charger_ac_en(&di->ac_chg, false, 0, 0);
+
+	/* Disable USB charging */
+	ab8500_charger_usb_en(&di->usb_chg, false, 0, 0);
+
+	/* Disable interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab8500_charger_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab8500_charger_irq[i].name);
+		free_irq(irq, di);
+	}
+
+	/* disable the regulator */
+	regulator_put(di->regu);
+
+	/* Backup battery voltage and current disable */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+		AB8500_RTC, AB8500_RTC_CTRL_REG, RTC_BUP_CH_ENA, 0);
+	if (ret < 0)
+		dev_err(di->dev, "%s mask and set failed\n", __func__);
+
+	otg_unregister_notifier(di->otg, &di->nb);
+	otg_put_transceiver(di->otg);
+
+	/* Delete the work queue */
+	destroy_workqueue(di->charger_wq);
+
+	flush_scheduled_work();
+	power_supply_unregister(&di->usb_chg.psy);
+	power_supply_unregister(&di->ac_chg.psy);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+
+	return 0;
+}
+
+static int __devinit ab8500_charger_probe(struct platform_device *pdev)
+{
+	int irq, i, charger_status, ret = 0;
+	struct ab8500_platform_data *plat;
+
+	struct ab8500_charger *di =
+		kzalloc(sizeof(struct ab8500_charger), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	/* get parent data */
+	di->dev = &pdev->dev;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
+	di->gpadc = ab8500_gpadc_get();
+
+	/* initialize lock */
+	spin_lock_init(&di->usb_state.usb_lock);
+
+	plat = dev_get_platdata(di->parent->dev);
+
+	/* get charger specific platform data */
+	if (!plat->charger) {
+		dev_err(di->dev, "no charger platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+	di->pdata = plat->charger;
+
+	/* get battery specific platform data */
+	if (!plat->battery) {
+		dev_err(di->dev, "no battery platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+	di->bat = plat->battery;
+
+	/* AC supply */
+	/* power_supply base class */
+	di->ac_chg.psy.name = "ab8500_ac";
+	di->ac_chg.psy.type = POWER_SUPPLY_TYPE_MAINS;
+	di->ac_chg.psy.properties = ab8500_charger_ac_props;
+	di->ac_chg.psy.num_properties = ARRAY_SIZE(ab8500_charger_ac_props);
+	di->ac_chg.psy.get_property = ab8500_charger_ac_get_property;
+	di->ac_chg.psy.supplied_to = di->pdata->supplied_to;
+	di->ac_chg.psy.num_supplicants = di->pdata->num_supplicants;
+	/* ux500_charger sub-class */
+	di->ac_chg.ops.enable = &ab8500_charger_ac_en;
+	di->ac_chg.ops.kick_wd = &ab8500_charger_watchdog_kick;
+	di->ac_chg.ops.update_curr = &ab8500_charger_update_charger_current;
+	di->ac_chg.max_out_volt = ab8500_charger_voltage_map[
+		ARRAY_SIZE(ab8500_charger_voltage_map) - 1];
+	di->ac_chg.max_out_curr = ab8500_charger_current_map[
+		ARRAY_SIZE(ab8500_charger_current_map) - 1];
+
+	/* USB supply */
+	/* power_supply base class */
+	di->usb_chg.psy.name = "ab8500_usb";
+	di->usb_chg.psy.type = POWER_SUPPLY_TYPE_USB;
+	di->usb_chg.psy.properties = ab8500_charger_usb_props;
+	di->usb_chg.psy.num_properties = ARRAY_SIZE(ab8500_charger_usb_props);
+	di->usb_chg.psy.get_property = ab8500_charger_usb_get_property;
+	di->usb_chg.psy.supplied_to = di->pdata->supplied_to;
+	di->usb_chg.psy.num_supplicants = di->pdata->num_supplicants;
+	/* ux500_charger sub-class */
+	di->usb_chg.ops.enable = &ab8500_charger_usb_en;
+	di->usb_chg.ops.kick_wd = &ab8500_charger_watchdog_kick;
+	di->usb_chg.ops.update_curr = &ab8500_charger_update_charger_current;
+	di->usb_chg.max_out_volt = ab8500_charger_voltage_map[
+		ARRAY_SIZE(ab8500_charger_voltage_map) - 1];
+	di->usb_chg.max_out_curr = ab8500_charger_current_map[
+		ARRAY_SIZE(ab8500_charger_current_map) - 1];
+
+
+	/* Create a work queue for the charger */
+	di->charger_wq =
+		create_singlethread_workqueue("ab8500_charger_wq");
+	if (di->charger_wq == NULL) {
+		dev_err(di->dev, "failed to create work queue\n");
+		goto free_device_info;
+	}
+
+	/* Init work for HW failure check */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->check_hw_failure_work,
+		ab8500_charger_check_hw_failure_work);
+	INIT_DELAYED_WORK_DEFERRABLE(&di->check_usbchgnotok_work,
+		ab8500_charger_check_usbchargernotok_work);
+
+
+	/*
+	 * For ABB revision 1.0 and 1.1 there is a bug in the watchdog
+	 * logic. That means we have to continously kick the charger
+	 * watchdog even when no charger is connected. This is only
+	 * valid once the AC charger has been enabled. This is
+	 * a bug that is not handled by the algorithm and the
+	 * watchdog have to be kicked by the charger driver
+	 * when the AC charger is disabled
+	 */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->kick_wd_work,
+		ab8500_charger_kick_watchdog_work);
+
+	/* Init work for charger detection */
+	INIT_WORK(&di->usb_link_status_work,
+		ab8500_charger_usb_link_status_work);
+	INIT_WORK(&di->ac_work, ab8500_charger_ac_work);
+	INIT_WORK(&di->detect_usb_type_work,
+		ab8500_charger_detect_usb_type_work);
+
+	INIT_WORK(&di->usb_state_changed_work,
+		ab8500_charger_usb_state_changed_work);
+
+	/* Init work for checking HW status */
+	INIT_WORK(&di->check_main_thermal_prot_work,
+		ab8500_charger_check_main_thermal_prot_work);
+	INIT_WORK(&di->check_usb_thermal_prot_work,
+		ab8500_charger_check_usb_thermal_prot_work);
+
+	/* Get Chip ID of the ABB ASIC  */
+	ret = abx500_get_chip_id(di->dev);
+	if (ret < 0) {
+		dev_err(di->dev, "failed to get chip ID\n");
+		goto free_charger_wq;
+	}
+	di->chip_id = ret;
+	dev_dbg(di->dev, "AB8500 CID is: 0x%02x\n", di->chip_id);
+
+	/*
+	 * VDD ADC supply needs to be enabled from this driver when there
+	 * is a charger connected to avoid erroneous BTEMP_HIGH/LOW
+	 * interrupts during charging
+	 */
+	di->regu = regulator_get(di->dev, "vddadc");
+	if (IS_ERR(di->regu)) {
+		ret = PTR_ERR(di->regu);
+		dev_err(di->dev, "failed to get vddadc regulator\n");
+		goto free_charger_wq;
+	}
+
+
+	/* Initialize OVV, and other registers */
+	ret = ab8500_charger_init_hw_registers(di);
+	if (ret) {
+		dev_err(di->dev, "failed to initialize ABB registers\n");
+		goto free_regulator;
+	}
+
+	/* Register AC charger class */
+	ret = power_supply_register(di->dev, &di->ac_chg.psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register AC charger\n");
+		goto free_regulator;
+	}
+
+	/* Register USB charger class */
+	ret = power_supply_register(di->dev, &di->usb_chg.psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register USB charger\n");
+		goto free_ac;
+	}
+
+	di->otg = otg_get_transceiver();
+	if (!di->otg) {
+		dev_err(di->dev, "failed to get otg transceiver\n");
+		goto free_usb;
+	}
+	di->nb.notifier_call = ab8500_charger_usb_notifier_call;
+	ret = otg_register_notifier(di->otg, &di->nb);
+	if (ret) {
+		dev_err(di->dev, "failed to register otg notifier\n");
+		goto put_otg_transceiver;
+	}
+
+	/* Identify the connected charger types during startup */
+	charger_status = ab8500_charger_detect_chargers(di);
+	if (charger_status & AC_PW_CONN) {
+		di->ac.charger_connected = 1;
+		di->ac_conn = true;
+		power_supply_changed(&di->ac_chg.psy);
+	}
+
+	if (charger_status & USB_PW_CONN) {
+		dev_dbg(di->dev, "VBUS Detect during startup\n");
+		di->vbus_detected = true;
+		di->vbus_detected_start = true;
+		queue_work(di->charger_wq,
+			&di->detect_usb_type_work);
+	}
+
+	/* Register interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab8500_charger_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab8500_charger_irq[i].name);
+		ret = request_threaded_irq(irq, NULL, ab8500_charger_irq[i].isr,
+			IRQF_SHARED | IRQF_NO_SUSPEND,
+			ab8500_charger_irq[i].name, di);
+
+		if (ret != 0) {
+			dev_err(di->dev, "failed to request %s IRQ %d: %d\n"
+				, ab8500_charger_irq[i].name, irq, ret);
+			goto free_irq;
+		}
+		dev_dbg(di->dev, "Requested %s IRQ %d: %d\n",
+			ab8500_charger_irq[i].name, irq, ret);
+	}
+
+	platform_set_drvdata(pdev, di);
+
+	return ret;
+
+free_irq:
+	otg_unregister_notifier(di->otg, &di->nb);
+
+	/* We also have to free all successfully registered irqs */
+	for (i = i - 1; i >= 0; i--) {
+		irq = platform_get_irq_byname(pdev, ab8500_charger_irq[i].name);
+		free_irq(irq, di);
+	}
+put_otg_transceiver:
+	otg_put_transceiver(di->otg);
+free_usb:
+	power_supply_unregister(&di->usb_chg.psy);
+free_ac:
+	power_supply_unregister(&di->ac_chg.psy);
+free_regulator:
+	regulator_put(di->regu);
+free_charger_wq:
+	destroy_workqueue(di->charger_wq);
+free_device_info:
+	kfree(di);
+
+	return ret;
+}
+
+static struct platform_driver ab8500_charger_driver = {
+	.probe = ab8500_charger_probe,
+	.remove = __devexit_p(ab8500_charger_remove),
+	.suspend = ab8500_charger_suspend,
+	.resume = ab8500_charger_resume,
+	.driver = {
+		.name = "ab8500-charger",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab8500_charger_init(void)
+{
+	return platform_driver_register(&ab8500_charger_driver);
+}
+
+static void __exit ab8500_charger_exit(void)
+{
+	platform_driver_unregister(&ab8500_charger_driver);
+}
+
+subsys_initcall_sync(ab8500_charger_init);
+module_exit(ab8500_charger_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Johan Palsson, Karl Komierowski, Arun R Murthy");
+MODULE_ALIAS("platform:ab8500-charger");
+MODULE_DESCRIPTION("AB8500 charger management driver");
diff --git a/drivers/power/ab8500_fg.c b/drivers/power/ab8500_fg.c
new file mode 100644
index 00000000000..b7462d493e0
--- /dev/null
+++ b/drivers/power/ab8500_fg.c
@@ -0,0 +1,2061 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * Main and Back-up battery management driver.
+ *
+ * Note: Backup battery management is required in case of Li-Ion battery and not
+ * for capacitive battery. HREF boards have capacitive battery and hence backup
+ * battery management is not used and the supported code is available in this
+ * driver.
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Johan Palsson <johan.palsson@stericsson.com>
+ * Author: Karl Komierowski <karl.komierowski@stericsson.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/kobject.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+#include <linux/slab.h>
+#include <linux/mfd/ab8500/bm.h>
+#include <linux/delay.h>
+#include <linux/mfd/ab8500/gpadc.h>
+#include <linux/mfd/abx500.h>
+#include <linux/time.h>
+
+#define MILLI_TO_MICRO			1000
+#define FG_LSB_IN_MA			1627
+#define QLSB_NANO_AMP_HOURS_X10		1129
+
+#define SEC_TO_SAMPLE(S)		(S * 4)
+
+#define NBR_AVG_SAMPLES			20
+
+#define LOW_BAT_CHECK_INTERVAL		(2 * HZ)
+
+#define VALID_CAPACITY_SEC		(45 * 60) /* 45 minutes */
+
+#define interpolate(x, x1, y1, x2, y2) \
+	((y1) + ((((y2) - (y1)) * ((x) - (x1))) / ((x2) - (x1))));
+
+#define to_ab8500_fg_device_info(x) container_of((x), \
+	struct ab8500_fg, fg_psy);
+
+/**
+ * struct ab8500_fg_interrupts - ab8500 fg interupts
+ * @name:	name of the interrupt
+ * @isr		function pointer to the isr
+ */
+struct ab8500_fg_interrupts {
+	char *name;
+	irqreturn_t (*isr)(int irq, void *data);
+};
+
+enum ab8500_fg_discharge_state {
+	AB8500_FG_DISCHARGE_INIT,
+	AB8500_FG_DISCHARGE_INITMEASURING,
+	AB8500_FG_DISCHARGE_INIT_RECOVERY,
+	AB8500_FG_DISCHARGE_RECOVERY,
+	AB8500_FG_DISCHARGE_READOUT,
+	AB8500_FG_DISCHARGE_WAKEUP,
+};
+
+static char *discharge_state[] = {
+	"DISCHARGE_INIT",
+	"DISCHARGE_INITMEASURING",
+	"DISCHARGE_INIT_RECOVERY",
+	"DISCHARGE_RECOVERY",
+	"DISCHARGE_READOUT",
+	"DISCHARGE_WAKEUP",
+};
+
+enum ab8500_fg_charge_state {
+	AB8500_FG_CHARGE_INIT,
+	AB8500_FG_CHARGE_READOUT,
+};
+
+static char *charge_state[] = {
+	"CHARGE_INIT",
+	"CHARGE_READOUT",
+};
+
+enum ab8500_fg_calibration_state {
+	AB8500_FG_CALIB_INIT,
+	AB8500_FG_CALIB_WAIT,
+	AB8500_FG_CALIB_END,
+};
+
+struct ab8500_fg_avg_cap {
+	int avg;
+	int samples[NBR_AVG_SAMPLES];
+	__kernel_time_t time_stamps[NBR_AVG_SAMPLES];
+	int pos;
+	int nbr_samples;
+	int sum;
+};
+
+struct ab8500_fg_battery_capacity {
+	int max_mah_design;
+	int max_mah;
+	int mah;
+	int permille;
+	int level;
+	int prev_mah;
+	int prev_percent;
+	int prev_level;
+};
+
+struct ab8500_fg_flags {
+	bool fg_enabled;
+	bool conv_done;
+	bool charging;
+	bool fully_charged;
+	bool low_bat_delay;
+	bool low_bat;
+	bool bat_ovv;
+	bool batt_unknown;
+	bool calibrate;
+};
+
+/**
+ * struct ab8500_fg - ab8500 FG device information
+ * @dev:		Pointer to the structure device
+ * @vbat:		Battery voltage in mV
+ * @vbat_nom:		Nominal battery voltage in mV
+ * @inst_curr:		Instantenous battery current in mA
+ * @avg_curr:		Average battery current in mA
+ * @fg_samples:		Number of samples used in the FG accumulation
+ * @accu_charge:	Accumulated charge from the last conversion
+ * @recovery_cnt:	Counter for recovery mode
+ * @high_curr_cnt:	Counter for high current mode
+ * @init_cnt:		Counter for init mode
+ * @recovery_needed:	Indicate if recovery is needed
+ * @high_curr_mode:	Indicate if we're in high current mode
+ * @init_capacity:	Indicate if initial capacity measuring should be done
+ * @calib_state		State during offset calibration
+ * @discharge_state:	Current discharge state
+ * @charge_state:	Current charge state
+ * @flags:		Structure for information about events triggered
+ * @bat_cap:		Structure for battery capacity specific parameters
+ * @avg_cap:		Average capacity filter
+ * @parent:		Pointer to the struct ab8500
+ * @gpadc:		Pointer to the struct gpadc
+ * @pdata:		Pointer to the ab8500_fg platform data
+ * @bat:		Pointer to the ab8500_bm platform data
+ * @fg_psy:		Structure that holds the FG specific battery properties
+ * @fg_wq:		Work queue for running the FG algorithm
+ * @fg_periodic_work:	Work to run the FG algorithm periodically
+ * @fg_low_bat_work:	Work to check low bat condition
+ * @fg_work:		Work to run the FG algorithm instantly
+ * @fg_acc_cur_work:	Work to read the FG accumulator
+ * @cc_lock:		Mutex for locking the CC
+ * @fg_kobject:		Structure of type kobject
+ */
+struct ab8500_fg {
+	struct device *dev;
+	int vbat;
+	int vbat_nom;
+	int inst_curr;
+	int avg_curr;
+	int fg_samples;
+	int accu_charge;
+	int recovery_cnt;
+	int high_curr_cnt;
+	int init_cnt;
+	bool recovery_needed;
+	bool high_curr_mode;
+	bool init_capacity;
+	enum ab8500_fg_calibration_state calib_state;
+	enum ab8500_fg_discharge_state discharge_state;
+	enum ab8500_fg_charge_state charge_state;
+	struct ab8500_fg_flags flags;
+	struct ab8500_fg_battery_capacity bat_cap;
+	struct ab8500_fg_avg_cap avg_cap;
+	struct ab8500 *parent;
+	struct ab8500_gpadc *gpadc;
+	struct ab8500_fg_platform_data *pdata;
+	struct ab8500_bm_data *bat;
+	struct power_supply fg_psy;
+	struct workqueue_struct *fg_wq;
+	struct delayed_work fg_periodic_work;
+	struct delayed_work fg_low_bat_work;
+	struct work_struct fg_work;
+	struct work_struct fg_acc_cur_work;
+	struct mutex cc_lock;
+	struct kobject fg_kobject;
+};
+
+/* Main battery properties */
+static enum power_supply_property ab8500_fg_props[] = {
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_CURRENT_AVG,
+	POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN,
+	POWER_SUPPLY_PROP_ENERGY_FULL,
+	POWER_SUPPLY_PROP_ENERGY_NOW,
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
+	POWER_SUPPLY_PROP_CHARGE_FULL,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_CAPACITY_LEVEL,
+};
+
+/*
+ * This array maps the raw hex value to lowbat voltage used by the AB8500
+ * Values taken from the UM0836
+ */
+static int ab8500_fg_lowbat_voltage_map[] = {
+	2300 ,
+	2325 ,
+	2350 ,
+	2375 ,
+	2400 ,
+	2425 ,
+	2450 ,
+	2475 ,
+	2500 ,
+	2525 ,
+	2550 ,
+	2575 ,
+	2600 ,
+	2625 ,
+	2650 ,
+	2675 ,
+	2700 ,
+	2725 ,
+	2750 ,
+	2775 ,
+	2800 ,
+	2825 ,
+	2850 ,
+	2875 ,
+	2900 ,
+	2925 ,
+	2950 ,
+	2975 ,
+	3000 ,
+	3025 ,
+	3050 ,
+	3075 ,
+	3100 ,
+	3125 ,
+	3150 ,
+	3175 ,
+	3200 ,
+	3225 ,
+	3250 ,
+	3275 ,
+	3300 ,
+	3325 ,
+	3350 ,
+	3375 ,
+	3400 ,
+	3425 ,
+	3450 ,
+	3475 ,
+	3500 ,
+	3525 ,
+	3550 ,
+	3575 ,
+	3600 ,
+	3625 ,
+	3650 ,
+	3675 ,
+	3700 ,
+	3725 ,
+	3750 ,
+	3775 ,
+	3800 ,
+	3825 ,
+	3850 ,
+	3850 ,
+};
+
+static u8 ab8500_volt_to_regval(int voltage)
+{
+	int i;
+
+	if (voltage < ab8500_fg_lowbat_voltage_map[0])
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(ab8500_fg_lowbat_voltage_map); i++) {
+		if (voltage < ab8500_fg_lowbat_voltage_map[i])
+			return (u8) i - 1;
+	}
+
+	/* If not captured above, return index of last element */
+	return (u8) ARRAY_SIZE(ab8500_fg_lowbat_voltage_map) - 1;
+}
+
+/**
+ * ab8500_fg_is_low_curr() - Low or high current mode
+ * @di:		pointer to the ab8500_fg structure
+ * @curr:	the current to base or our decision on
+ *
+ * Low current mode if the current consumption is below a certain threshold
+ */
+static int ab8500_fg_is_low_curr(struct ab8500_fg *di, int curr)
+{
+	/*
+	 * We want to know if we're in low current mode
+	 */
+	if (curr > -di->bat->fg_params->high_curr_threshold)
+		return true;
+	else
+		return false;
+}
+
+/**
+ * ab8500_fg_add_cap_sample() - Add capacity to average filter
+ * @di:		pointer to the ab8500_fg structure
+ * @sample:	the capacity in mAh to add to the filter
+ *
+ * A capacity is added to the filter and a new mean capacity is calculated and
+ * returned
+ */
+static int ab8500_fg_add_cap_sample(struct ab8500_fg *di, int sample)
+{
+	struct timespec ts;
+	struct ab8500_fg_avg_cap *avg = &di->avg_cap;
+
+	getnstimeofday(&ts);
+
+	do {
+		avg->sum += sample - avg->samples[avg->pos];
+		avg->samples[avg->pos] = sample;
+		avg->time_stamps[avg->pos] = ts.tv_sec;
+		avg->pos++;
+
+		if (avg->pos == NBR_AVG_SAMPLES)
+			avg->pos = 0;
+
+		if (avg->nbr_samples < NBR_AVG_SAMPLES)
+			avg->nbr_samples++;
+
+		/*
+		 * Check the time stamp for each sample. If too old,
+		 * replace with latest sample
+		 */
+	} while (ts.tv_sec - VALID_CAPACITY_SEC > avg->time_stamps[avg->pos]);
+
+	avg->avg = avg->sum / avg->nbr_samples;
+
+	return avg->avg;
+}
+
+/**
+ * ab8500_fg_fill_cap_sample() - Fill average filter
+ * @di:		pointer to the ab8500_fg structure
+ * @sample:	the capacity in mAh to fill the filter with
+ *
+ * The capacity filter is filled with a capacity in mAh
+ */
+static void ab8500_fg_fill_cap_sample(struct ab8500_fg *di, int sample)
+{
+	int i;
+	struct timespec ts;
+	struct ab8500_fg_avg_cap *avg = &di->avg_cap;
+
+	getnstimeofday(&ts);
+
+	for (i = 0; i < NBR_AVG_SAMPLES; i++) {
+		avg->samples[i] = sample;
+		avg->time_stamps[i] = ts.tv_sec;
+	}
+
+	avg->pos = 0;
+	avg->nbr_samples = NBR_AVG_SAMPLES;
+	avg->sum = sample * NBR_AVG_SAMPLES;
+	avg->avg = sample;
+}
+
+/**
+ * ab8500_fg_coulomb_counter() - enable coulomb counter
+ * @di:		pointer to the ab8500_fg structure
+ * @enable:	enable/disable
+ *
+ * Enable/Disable coulomb counter.
+ * On failure returns negative value.
+ */
+static int ab8500_fg_coulomb_counter(struct ab8500_fg *di, bool enable)
+{
+	int ret = 0;
+	mutex_lock(&di->cc_lock);
+	if (enable) {
+		/* To be able to reprogram the number of samples, we have to
+		 * first stop the CC and then enable it again */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+			AB8500_RTC_CC_CONF_REG, 0x00);
+		if (ret)
+			goto cc_err;
+
+		/* Program the samples */
+		ret = abx500_set_register_interruptible(di->dev,
+			AB8500_GAS_GAUGE, AB8500_GASG_CC_NCOV_ACCU,
+			di->fg_samples);
+		if (ret)
+			goto cc_err;
+
+		/* Start the CC */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+			AB8500_RTC_CC_CONF_REG,
+			(CC_DEEP_SLEEP_ENA | CC_PWR_UP_ENA));
+		if (ret)
+			goto cc_err;
+
+		di->flags.fg_enabled = true;
+	} else {
+		/* Clear any pending read requests */
+		ret = abx500_set_register_interruptible(di->dev,
+			AB8500_GAS_GAUGE, AB8500_GASG_CC_CTRL_REG, 0);
+		if (ret)
+			goto cc_err;
+
+		/* Stop the CC */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+			AB8500_RTC_CC_CONF_REG, 0);
+		if (ret)
+			goto cc_err;
+
+		di->flags.fg_enabled = false;
+
+	}
+	dev_dbg(di->dev, " CC enabled: %d Samples: %d\n",
+		enable, di->fg_samples);
+
+	mutex_unlock(&di->cc_lock);
+
+	return ret;
+cc_err:
+	dev_err(di->dev, "%s Enabling coulomb counter failed\n", __func__);
+	mutex_unlock(&di->cc_lock);
+	return ret;
+}
+
+/**
+ * ab8500_fg_inst_curr() - battery instantaneous current
+ * @di:         pointer to the ab8500_fg structure
+ *
+ * Returns battery instantenous current(on success) else error code
+ */
+static int ab8500_fg_inst_curr(struct ab8500_fg *di)
+{
+	u8 low, high, reg_val;
+	static int val;
+	int ret = 0;
+	bool fg_off = false;
+
+	mutex_lock(&di->cc_lock);
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
+		AB8500_RTC_CC_CONF_REG, &reg_val);
+	if (ret < 0)
+		goto inst_curr_err;
+
+	if (!(reg_val & CC_PWR_UP_ENA)) {
+		dev_dbg(di->dev, "%s Enable FG\n", __func__);
+		fg_off = true;
+
+		/* Program the samples */
+		ret = abx500_set_register_interruptible(di->dev,
+			AB8500_GAS_GAUGE, AB8500_GASG_CC_NCOV_ACCU,
+			SEC_TO_SAMPLE(10));
+		if (ret)
+			goto inst_curr_err;
+
+		/* Start the CC */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+			AB8500_RTC_CC_CONF_REG,
+			(CC_DEEP_SLEEP_ENA | CC_PWR_UP_ENA));
+		if (ret)
+			goto inst_curr_err;
+	}
+
+	/* Reset counter and Read request */
+	ret = abx500_set_register_interruptible(di->dev, AB8500_GAS_GAUGE,
+		AB8500_GASG_CC_CTRL_REG, (RESET_ACCU | READ_REQ));
+	if (ret)
+		goto inst_curr_err;
+
+	/*
+	 * Since there is no interrupt for this, just wait for 250ms
+	 * 250ms is one sample conversion time with 32.768 Khz RTC clock
+	 */
+	msleep(250);
+
+	/* Read CC Sample conversion value Low and high */
+	ret = abx500_get_register_interruptible(di->dev, AB8500_GAS_GAUGE,
+		AB8500_GASG_CC_SMPL_CNVL_REG,  &low);
+	if (ret < 0)
+		goto inst_curr_err;
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_GAS_GAUGE,
+		AB8500_GASG_CC_SMPL_CNVH_REG,  &high);
+	if (ret < 0)
+		goto inst_curr_err;
+
+	/*
+	 * negative value for Discharging
+	 * convert 2's compliment into decimal
+	 */
+	if (high & 0x10)
+		val = (low | (high << 8) | 0xFFFFE000);
+	else
+		val = (low | (high << 8));
+
+	/*
+	 * Convert to unit value in mA
+	 * Full scale input voltage is
+	 * 66.660mV => LSB = 66.660mV/(4096*res) = 1.627mA
+	 * resistance is in mOhm
+	 */
+	val = ((val * 66660)  / (4096 * di->bat->fg_res));
+
+	if (fg_off) {
+		dev_dbg(di->dev, "%s Disable FG\n", __func__);
+
+		/* Clear any pending read requests */
+		ret = abx500_set_register_interruptible(di->dev,
+			AB8500_GAS_GAUGE, AB8500_GASG_CC_CTRL_REG, 0);
+		if (ret)
+			goto inst_curr_err;
+
+		/* Stop the CC */
+		ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+			AB8500_RTC_CC_CONF_REG, 0);
+		if (ret)
+			goto inst_curr_err;
+	}
+
+	mutex_unlock(&di->cc_lock);
+
+	return val;
+
+inst_curr_err:
+	dev_err(di->dev, "%s Get instanst current failed\n", __func__);
+	mutex_unlock(&di->cc_lock);
+	return ret;
+}
+
+/**
+ * ab8500_fg_acc_cur_work() - average battery current
+ * @work:	pointer to the work_struct structure
+ *
+ * Updated the average battery current obtained from the
+ * coulomb counter.
+ */
+static void ab8500_fg_acc_cur_work(struct work_struct *work)
+{
+	int val;
+	int ret;
+	u8 low, med, high;
+
+	struct ab8500_fg *di = container_of(work,
+		struct ab8500_fg, fg_acc_cur_work);
+
+	mutex_lock(&di->cc_lock);
+	ret = abx500_set_register_interruptible(di->dev, AB8500_GAS_GAUGE,
+		AB8500_GASG_CC_NCOV_ACCU_CTRL, RD_NCONV_ACCU_REQ);
+	if (ret)
+		goto exit;
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_GAS_GAUGE,
+		AB8500_GASG_CC_NCOV_ACCU_LOW,  &low);
+	if (ret < 0)
+		goto exit;
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_GAS_GAUGE,
+		AB8500_GASG_CC_NCOV_ACCU_MED,  &med);
+	if (ret < 0)
+		goto exit;
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_GAS_GAUGE,
+		AB8500_GASG_CC_NCOV_ACCU_HIGH, &high);
+	if (ret < 0)
+		goto exit;
+
+	/* Check for sign bit in case of negative value, 2's compliment */
+	if (high & 0x10)
+		val = (low | (med << 8) | (high << 16) | 0xFFE00000);
+	else
+		val = (low | (med << 8) | (high << 16));
+
+	di->accu_charge = (val * QLSB_NANO_AMP_HOURS_X10)/10000;
+
+	di->avg_curr = (val * FG_LSB_IN_MA) / (di->fg_samples * 1000);
+	di->flags.conv_done = true;
+
+	mutex_unlock(&di->cc_lock);
+
+	queue_work(di->fg_wq, &di->fg_work);
+
+	return;
+exit:
+	dev_err(di->dev,
+		"Failed to read or write gas gauge registers\n");
+	mutex_unlock(&di->cc_lock);
+	queue_work(di->fg_wq,
+		&di->fg_work);
+}
+
+/**
+ * ab8500_fg_bat_voltage() - get battery voltage
+ * @di:		pointer to the ab8500_fg structure
+ *
+ * Returns battery voltage(on success) else error code
+ */
+static int ab8500_fg_bat_voltage(struct ab8500_fg *di)
+{
+	int vbat;
+	static int prev;
+
+	vbat = ab8500_gpadc_convert(di->gpadc, MAIN_BAT_V);
+	if (vbat < 0) {
+		dev_err(di->dev,
+			"%s gpadc conversion failed, using previous value\n",
+			__func__);
+		return prev;
+	}
+
+	prev = vbat;
+	return vbat;
+}
+
+/**
+ * ab8500_fg_volt_to_capacity() - Voltage based capacity
+ * @di:		pointer to the ab8500_fg structure
+ * @voltage:	The voltage to convert to a capacity
+ *
+ * Returns battery capacity in per mille based on voltage
+ */
+static int ab8500_fg_volt_to_capacity(struct ab8500_fg *di, int voltage)
+{
+	int i, tbl_size;
+	struct v_to_cap *tbl;
+	int cap = 0;
+
+	tbl = di->bat->bat_type[di->bat->batt_id].v_to_cap_tbl,
+	tbl_size = di->bat->bat_type[di->bat->batt_id].n_v_cap_tbl_elements;
+
+	for (i = 0; i < tbl_size; ++i) {
+		if (voltage > tbl[i].voltage)
+			break;
+	}
+
+	if ((i > 0) && (i < tbl_size)) {
+		cap = interpolate(voltage,
+			tbl[i].voltage,
+			tbl[i].capacity * 10,
+			tbl[i-1].voltage,
+			tbl[i-1].capacity * 10);
+	} else if (i == 0) {
+		cap = 1000;
+	} else {
+		cap = 0;
+	}
+
+	dev_dbg(di->dev, "%s Vbat: %d, Cap: %d per mille",
+		__func__, voltage, cap);
+
+	return cap;
+}
+
+/**
+ * ab8500_fg_uncomp_volt_to_capacity() - Uncompensated voltage based capacity
+ * @di:		pointer to the ab8500_fg structure
+ *
+ * Returns battery capacity based on battery voltage that is not compensated
+ * for the voltage drop due to the load
+ */
+static int ab8500_fg_uncomp_volt_to_capacity(struct ab8500_fg *di)
+{
+	di->vbat = ab8500_fg_bat_voltage(di);
+	return ab8500_fg_volt_to_capacity(di, di->vbat);
+}
+
+/**
+ * ab8500_fg_load_comp_volt_to_capacity() - Load compensated voltage based capacity
+ * @di:		pointer to the ab8500_fg structure
+ *
+ * Returns battery capacity based on battery voltage that is load compensated
+ * for the voltage drop
+ */
+static int ab8500_fg_load_comp_volt_to_capacity(struct ab8500_fg *di)
+{
+	int vbat_comp;
+
+	di->inst_curr = ab8500_fg_inst_curr(di);
+	di->vbat = ab8500_fg_bat_voltage(di);
+
+	/* Use Ohms law to get the load compensated voltage */
+	vbat_comp = di->vbat - (di->inst_curr *
+		di->bat->bat_type[di->bat->batt_id].battery_resistance) / 1000;
+
+	dev_dbg(di->dev, "%s Measured Vbat: %dmV,Compensated Vbat %dmV, "
+		"R: %dmOhm, Current: %dmA\n",
+		__func__,
+		di->vbat,
+		vbat_comp,
+		di->bat->bat_type[di->bat->batt_id].battery_resistance,
+		di->inst_curr);
+
+	return ab8500_fg_volt_to_capacity(di, vbat_comp);
+}
+
+/**
+ * ab8500_fg_convert_mah_to_permille() - Capacity in mAh to permille
+ * @di:		pointer to the ab8500_fg structure
+ * @cap_mah:	capacity in mAh
+ *
+ * Converts capacity in mAh to capacity in permille
+ */
+static int ab8500_fg_convert_mah_to_permille(struct ab8500_fg *di, int cap_mah)
+{
+	return (cap_mah * 1000) / di->bat_cap.max_mah_design;
+}
+
+/**
+ * ab8500_fg_convert_permille_to_mah() - Capacity in permille to mAh
+ * @di:		pointer to the ab8500_fg structure
+ * @cap_pm:	capacity in permille
+ *
+ * Converts capacity in permille to capacity in mAh
+ */
+static int ab8500_fg_convert_permille_to_mah(struct ab8500_fg *di, int cap_pm)
+{
+	return cap_pm * di->bat_cap.max_mah_design / 1000;
+}
+
+/**
+ * ab8500_fg_convert_mah_to_uwh() - Capacity in mAh to uWh
+ * @di:		pointer to the ab8500_fg structure
+ * @cap_mah:	capacity in mAh
+ *
+ * Converts capacity in mAh to capacity in uWh
+ */
+static int ab8500_fg_convert_mah_to_uwh(struct ab8500_fg *di, int cap_mah)
+{
+	u64 div_res;
+	u32 div_rem;
+
+	div_res = ((u64) cap_mah) * ((u64) di->vbat_nom);
+	div_rem = do_div(div_res, 1000);
+
+	/* Make sure to round upwards if necessary */
+	if (div_rem >= 1000 / 2)
+		div_res++;
+
+	return (int) div_res;
+}
+
+/**
+ * ab8500_fg_calc_cap_charging() - Calculate remaining capacity while charging
+ * @di:		pointer to the ab8500_fg structure
+ *
+ * Return the capacity in mAh based on previous calculated capcity and the FG
+ * accumulator register value. The filter is filled with this capacity
+ */
+static int ab8500_fg_calc_cap_charging(struct ab8500_fg *di)
+{
+	dev_dbg(di->dev, "%s cap_mah %d accu_charge %d\n",
+		__func__,
+		di->bat_cap.mah,
+		di->accu_charge);
+
+	/* Capacity should not be less than 0 */
+	if (di->bat_cap.mah + di->accu_charge > 0)
+		di->bat_cap.mah += di->accu_charge;
+	else
+		di->bat_cap.mah = 0;
+
+	/*
+	 * We force capacity to 100% as long as the algorithm
+	 * reports that it's full.
+	*/
+	if (di->bat_cap.mah >= di->bat_cap.max_mah_design ||
+		di->flags.fully_charged)
+		di->bat_cap.mah = di->bat_cap.max_mah_design;
+
+	ab8500_fg_fill_cap_sample(di, di->bat_cap.mah);
+	di->bat_cap.permille =
+		ab8500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+
+	/* We need to update battery voltage and inst current when charging */
+	di->vbat = ab8500_fg_bat_voltage(di);
+	di->inst_curr = ab8500_fg_inst_curr(di);
+
+	return di->bat_cap.mah;
+}
+
+/**
+ * ab8500_fg_calc_cap_discharge_voltage() - Capacity in discharge with voltage
+ * @di:		pointer to the ab8500_fg structure
+ * @comp:	if voltage should be load compensated before capacity calc
+ *
+ * Return the capacity in mAh based on the battery voltage. The voltage can
+ * either be load compensated or not. This value is added to the filter and a
+ * new mean value is calculated and returned.
+ */
+static int ab8500_fg_calc_cap_discharge_voltage(struct ab8500_fg *di, bool comp)
+{
+	int permille, mah;
+
+	if (comp)
+		permille = ab8500_fg_load_comp_volt_to_capacity(di);
+	else
+		permille = ab8500_fg_uncomp_volt_to_capacity(di);
+
+	mah = ab8500_fg_convert_permille_to_mah(di, permille);
+
+	di->bat_cap.mah = ab8500_fg_add_cap_sample(di, mah);
+	di->bat_cap.permille =
+		ab8500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+
+	return di->bat_cap.mah;
+}
+
+/**
+ * ab8500_fg_calc_cap_discharge_fg() - Capacity in discharge with FG
+ * @di:		pointer to the ab8500_fg structure
+ *
+ * Return the capacity in mAh based on previous calculated capcity and the FG
+ * accumulator register value. This value is added to the filter and a
+ * new mean value is calculated and returned.
+ */
+static int ab8500_fg_calc_cap_discharge_fg(struct ab8500_fg *di)
+{
+	int permille_volt, permille;
+
+	dev_dbg(di->dev, "%s cap_mah %d accu_charge %d\n",
+		__func__,
+		di->bat_cap.mah,
+		di->accu_charge);
+
+	/* Capacity should not be less than 0 */
+	if (di->bat_cap.mah + di->accu_charge > 0)
+		di->bat_cap.mah += di->accu_charge;
+	else
+		di->bat_cap.mah = 0;
+
+	if (di->bat_cap.mah >= di->bat_cap.max_mah_design)
+		di->bat_cap.mah = di->bat_cap.max_mah_design;
+
+	/*
+	 * Check against voltage based capacity. It can not be lower
+	 * than what the uncompensated voltage says
+	 */
+	permille = ab8500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+	permille_volt = ab8500_fg_uncomp_volt_to_capacity(di);
+
+	if (permille < permille_volt) {
+		di->bat_cap.permille = permille_volt;
+		di->bat_cap.mah = ab8500_fg_convert_permille_to_mah(di,
+			di->bat_cap.permille);
+
+		dev_dbg(di->dev, "%s voltage based: perm %d perm_volt %d\n",
+			__func__,
+			permille,
+			permille_volt);
+
+		ab8500_fg_fill_cap_sample(di, di->bat_cap.mah);
+	} else {
+		ab8500_fg_fill_cap_sample(di, di->bat_cap.mah);
+		di->bat_cap.permille =
+			ab8500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+	}
+
+	return di->bat_cap.mah;
+}
+
+/**
+ * ab8500_fg_capacity_level() - Get the battery capacity level
+ * @di:		pointer to the ab8500_fg structure
+ *
+ * Get the battery capacity level based on the capacity in percent
+ */
+static int ab8500_fg_capacity_level(struct ab8500_fg *di)
+{
+	int ret, percent;
+
+	percent = di->bat_cap.permille / 10;
+
+	if (percent <= di->bat->cap_levels->critical ||
+		di->flags.low_bat)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_CRITICAL;
+	else if (percent <= di->bat->cap_levels->low)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_LOW;
+	else if (percent <= di->bat->cap_levels->normal)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_NORMAL;
+	else if (percent <= di->bat->cap_levels->high)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_HIGH;
+	else
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_FULL;
+
+	return ret;
+}
+
+/**
+ * ab8500_fg_check_capacity_limits() - Check if capacity has changed
+ * @di:		pointer to the ab8500_fg structure
+ * @init:	capacity is allowed to go up in init mode
+ *
+ * Check if capacity or capacity limit has changed and notify the system
+ * about it using the power_supply framework
+ */
+static void ab8500_fg_check_capacity_limits(struct ab8500_fg *di, bool init)
+{
+	bool changed = false;
+
+	di->bat_cap.level = ab8500_fg_capacity_level(di);
+
+	if (di->bat_cap.level != di->bat_cap.prev_level) {
+		/*
+		 * We do not allow reported capacity level to go up
+		 * unless we're charging or if we're in init
+		 */
+		if (!(!di->flags.charging && di->bat_cap.level >
+			di->bat_cap.prev_level) || init) {
+			dev_dbg(di->dev, "level changed from %d to %d\n",
+				di->bat_cap.prev_level,
+				di->bat_cap.level);
+			di->bat_cap.prev_level = di->bat_cap.level;
+			changed = true;
+		} else {
+			dev_dbg(di->dev, "level not allowed to go up "
+				"since no charger is connected: %d to %d\n",
+				di->bat_cap.prev_level,
+				di->bat_cap.level);
+		}
+	}
+
+	/*
+	 * If we have received the LOW_BAT IRQ, set capacity to 0 to initiate
+	 * shutdown
+	 */
+	if (di->flags.low_bat) {
+		dev_dbg(di->dev, "Battery low, set capacity to 0\n");
+		di->bat_cap.prev_percent = 0;
+		di->bat_cap.permille = 0;
+		di->bat_cap.prev_mah = 0;
+		di->bat_cap.mah = 0;
+		changed = true;
+	} else if (di->bat_cap.prev_percent != di->bat_cap.permille / 10) {
+		if (di->bat_cap.permille / 10 == 0) {
+			/*
+			 * We will not report 0% unless we've got
+			 * the LOW_BAT IRQ, no matter what the FG
+			 * algorithm says.
+			 */
+			di->bat_cap.prev_percent = 1;
+			di->bat_cap.permille = 1;
+			di->bat_cap.prev_mah = 1;
+			di->bat_cap.mah = 1;
+
+			changed = true;
+		} else if (!(!di->flags.charging &&
+			(di->bat_cap.permille / 10) >
+			di->bat_cap.prev_percent) || init) {
+			/*
+			 * We do not allow reported capacity to go up
+			 * unless we're charging or if we're in init
+			 */
+			dev_dbg(di->dev,
+				"capacity changed from %d to %d (%d)\n",
+				di->bat_cap.prev_percent,
+				di->bat_cap.permille / 10,
+				di->bat_cap.permille);
+			di->bat_cap.prev_percent = di->bat_cap.permille / 10;
+			di->bat_cap.prev_mah = di->bat_cap.mah;
+
+			changed = true;
+		} else {
+			dev_dbg(di->dev, "capacity not allowed to go up since "
+				"no charger is connected: %d to %d (%d)\n",
+				di->bat_cap.prev_percent,
+				di->bat_cap.permille / 10,
+				di->bat_cap.permille);
+		}
+	}
+
+	if (changed)
+		power_supply_changed(&di->fg_psy);
+
+}
+
+static void ab8500_fg_charge_state_to(struct ab8500_fg *di,
+	enum ab8500_fg_charge_state new_state)
+{
+	dev_dbg(di->dev, "Charge state from %d [%s] to %d [%s]\n",
+		di->charge_state,
+		charge_state[di->charge_state],
+		new_state,
+		charge_state[new_state]);
+
+	di->charge_state = new_state;
+}
+
+static void ab8500_fg_discharge_state_to(struct ab8500_fg *di,
+	enum ab8500_fg_charge_state new_state)
+{
+	dev_dbg(di->dev, "Disharge state from %d [%s] to %d [%s]\n",
+		di->discharge_state,
+		discharge_state[di->discharge_state],
+		new_state,
+		discharge_state[new_state]);
+
+	di->discharge_state = new_state;
+}
+
+/**
+ * ab8500_fg_algorithm_charging() - FG algorithm for when charging
+ * @di:		pointer to the ab8500_fg structure
+ *
+ * Battery capacity calculation state machine for when we're charging
+ */
+static void ab8500_fg_algorithm_charging(struct ab8500_fg *di)
+{
+	/*
+	 * If we change to discharge mode
+	 * we should start with recovery
+	 */
+	if (di->discharge_state != AB8500_FG_DISCHARGE_INIT_RECOVERY)
+		ab8500_fg_discharge_state_to(di,
+			AB8500_FG_DISCHARGE_INIT_RECOVERY);
+
+	switch (di->charge_state) {
+	case AB8500_FG_CHARGE_INIT:
+		di->fg_samples = SEC_TO_SAMPLE(
+			di->bat->fg_params->accu_charging);
+
+		ab8500_fg_coulomb_counter(di, true);
+		ab8500_fg_charge_state_to(di, AB8500_FG_CHARGE_READOUT);
+
+		break;
+
+	case AB8500_FG_CHARGE_READOUT:
+		/*
+		 * Read the FG and calculate the new capacity
+		 */
+		mutex_lock(&di->cc_lock);
+		if (!di->flags.conv_done) {
+			/* Wasn't the CC IRQ that got us here */
+			mutex_unlock(&di->cc_lock);
+			dev_dbg(di->dev, "%s CC conv not done\n",
+				__func__);
+
+			break;
+		}
+		di->flags.conv_done = false;
+		mutex_unlock(&di->cc_lock);
+
+		ab8500_fg_calc_cap_charging(di);
+
+		break;
+
+	default:
+		break;
+	}
+
+	/* Check capacity limits */
+	ab8500_fg_check_capacity_limits(di, false);
+}
+
+/**
+ * ab8500_fg_algorithm_discharging() - FG algorithm for when discharging
+ * @di:		pointer to the ab8500_fg structure
+ *
+ * Battery capacity calculation state machine for when we're discharging
+ */
+static void ab8500_fg_algorithm_discharging(struct ab8500_fg *di)
+{
+	int sleep_time;
+
+	/* If we change to charge mode we should start with init */
+	if (di->charge_state != AB8500_FG_CHARGE_INIT)
+		ab8500_fg_charge_state_to(di, AB8500_FG_CHARGE_INIT);
+
+	switch (di->discharge_state) {
+	case AB8500_FG_DISCHARGE_INIT:
+		/* We use the FG IRQ to work on */
+		di->init_cnt = 0;
+		di->fg_samples = SEC_TO_SAMPLE(di->bat->fg_params->init_timer);
+		ab8500_fg_coulomb_counter(di, true);
+		ab8500_fg_discharge_state_to(di,
+			AB8500_FG_DISCHARGE_INITMEASURING);
+
+		/* Intentional fallthrough */
+	case AB8500_FG_DISCHARGE_INITMEASURING:
+		/*
+		 * Discard a number of samples during startup.
+		 * After that, use compensated voltage for a few
+		 * samples to get an initial capacity.
+		 * Then go to READOUT
+		 */
+		sleep_time = di->bat->fg_params->init_timer;
+
+		/* Discard the first [x] seconds */
+		if (di->init_cnt >
+			di->bat->fg_params->init_discard_time) {
+
+			ab8500_fg_calc_cap_discharge_voltage(di, true);
+
+			ab8500_fg_check_capacity_limits(di, true);
+		}
+
+		di->init_cnt += sleep_time;
+		if (di->init_cnt >
+			di->bat->fg_params->init_total_time) {
+			di->fg_samples = SEC_TO_SAMPLE(
+				di->bat->fg_params->accu_high_curr);
+
+			ab8500_fg_coulomb_counter(di, true);
+			ab8500_fg_discharge_state_to(di,
+				AB8500_FG_DISCHARGE_READOUT);
+		}
+
+		break;
+
+	case AB8500_FG_DISCHARGE_INIT_RECOVERY:
+		di->recovery_cnt = 0;
+		di->recovery_needed = true;
+		ab8500_fg_discharge_state_to(di,
+			AB8500_FG_DISCHARGE_RECOVERY);
+
+		/* Intentional fallthrough */
+
+	case AB8500_FG_DISCHARGE_RECOVERY:
+		sleep_time = di->bat->fg_params->recovery_sleep_timer;
+
+		/*
+		 * We should check the power consumption
+		 * If low, go to READOUT (after x min) or
+		 * RECOVERY_SLEEP if time left.
+		 * If high, go to READOUT
+		 */
+		di->inst_curr = ab8500_fg_inst_curr(di);
+
+		if (ab8500_fg_is_low_curr(di, di->inst_curr)) {
+			if (di->recovery_cnt >
+				di->bat->fg_params->recovery_total_time) {
+				di->fg_samples = SEC_TO_SAMPLE(
+					di->bat->fg_params->accu_high_curr);
+				ab8500_fg_coulomb_counter(di, true);
+				ab8500_fg_discharge_state_to(di,
+					AB8500_FG_DISCHARGE_READOUT);
+				di->recovery_needed = false;
+			} else {
+				queue_delayed_work(di->fg_wq,
+					&di->fg_periodic_work,
+					sleep_time * HZ);
+			}
+			di->recovery_cnt += sleep_time;
+		} else {
+			di->fg_samples = SEC_TO_SAMPLE(
+				di->bat->fg_params->accu_high_curr);
+			ab8500_fg_coulomb_counter(di, true);
+			ab8500_fg_discharge_state_to(di,
+				AB8500_FG_DISCHARGE_READOUT);
+		}
+
+		break;
+
+	case AB8500_FG_DISCHARGE_READOUT:
+		di->inst_curr = ab8500_fg_inst_curr(di);
+
+		if (ab8500_fg_is_low_curr(di, di->inst_curr)) {
+			/* Detect mode change */
+			if (di->high_curr_mode) {
+				di->high_curr_mode = false;
+				di->high_curr_cnt = 0;
+			}
+
+			if (di->recovery_needed) {
+				ab8500_fg_discharge_state_to(di,
+					AB8500_FG_DISCHARGE_RECOVERY);
+
+				queue_delayed_work(di->fg_wq,
+					&di->fg_periodic_work,
+					0);
+
+				break;
+			}
+
+			ab8500_fg_calc_cap_discharge_voltage(di, true);
+		} else {
+			mutex_lock(&di->cc_lock);
+			if (!di->flags.conv_done) {
+				/* Wasn't the CC IRQ that got us here */
+				mutex_unlock(&di->cc_lock);
+				dev_dbg(di->dev, "%s CC conv not done\n",
+					__func__);
+
+				break;
+			}
+			di->flags.conv_done = false;
+			mutex_unlock(&di->cc_lock);
+
+			/* Detect mode change */
+			if (!di->high_curr_mode) {
+				di->high_curr_mode = true;
+				di->high_curr_cnt = 0;
+			}
+
+			di->high_curr_cnt +=
+				di->bat->fg_params->accu_high_curr;
+			if (di->high_curr_cnt >
+				di->bat->fg_params->high_curr_time)
+				di->recovery_needed = true;
+
+			ab8500_fg_calc_cap_discharge_fg(di);
+		}
+
+		ab8500_fg_check_capacity_limits(di, false);
+
+		break;
+
+	case AB8500_FG_DISCHARGE_WAKEUP:
+		ab8500_fg_coulomb_counter(di, true);
+		di->inst_curr = ab8500_fg_inst_curr(di);
+
+		ab8500_fg_calc_cap_discharge_voltage(di, true);
+
+		di->fg_samples = SEC_TO_SAMPLE(
+			di->bat->fg_params->accu_high_curr);
+		/* Re-program number of samples set above */
+		ab8500_fg_coulomb_counter(di, true);
+		ab8500_fg_discharge_state_to(di, AB8500_FG_DISCHARGE_READOUT);
+
+		ab8500_fg_check_capacity_limits(di, false);
+
+		break;
+
+	default:
+		break;
+	}
+}
+
+/**
+ * ab8500_fg_algorithm_calibrate() - Internal columb counter offset calibration
+ * @di:		pointer to the ab8500_fg structure
+ *
+ */
+static void ab8500_fg_algorithm_calibrate(struct ab8500_fg *di)
+{
+	int ret;
+
+	switch (di->calib_state) {
+	case AB8500_FG_CALIB_INIT:
+		dev_dbg(di->dev, "Calibration ongoing...\n");
+
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_GAS_GAUGE, AB8500_GASG_CC_CTRL_REG,
+			CC_INT_CAL_N_AVG_MASK, CC_INT_CAL_SAMPLES_8);
+		if (ret < 0)
+			goto err;
+
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_GAS_GAUGE, AB8500_GASG_CC_CTRL_REG,
+			CC_INTAVGOFFSET_ENA, CC_INTAVGOFFSET_ENA);
+		if (ret < 0)
+			goto err;
+		di->calib_state = AB8500_FG_CALIB_WAIT;
+		break;
+	case AB8500_FG_CALIB_END:
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_GAS_GAUGE, AB8500_GASG_CC_CTRL_REG,
+			CC_MUXOFFSET, CC_MUXOFFSET);
+		if (ret < 0)
+			goto err;
+		di->flags.calibrate = false;
+		dev_dbg(di->dev, "Calibration done...\n");
+		queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+		break;
+	case AB8500_FG_CALIB_WAIT:
+		dev_dbg(di->dev, "Calibration WFI\n");
+	default:
+		break;
+	}
+	return;
+err:
+	/* Something went wrong, don't calibrate then */
+	dev_err(di->dev, "failed to calibrate the CC\n");
+	di->flags.calibrate = false;
+	di->calib_state = AB8500_FG_CALIB_INIT;
+	queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+}
+
+/**
+ * ab8500_fg_algorithm() - Entry point for the FG algorithm
+ * @di:		pointer to the ab8500_fg structure
+ *
+ * Entry point for the battery capacity calculation state machine
+ */
+static void ab8500_fg_algorithm(struct ab8500_fg *di)
+{
+	if (di->flags.calibrate)
+		ab8500_fg_algorithm_calibrate(di);
+	else {
+		if (di->flags.charging)
+			ab8500_fg_algorithm_charging(di);
+		else
+			ab8500_fg_algorithm_discharging(di);
+	}
+
+	dev_dbg(di->dev, "[FG_DATA] %d %d %d %d %d %d %d %d %d "
+		"%d %d %d %d %d %d %d\n",
+		di->bat_cap.max_mah_design,
+		di->bat_cap.mah,
+		di->bat_cap.permille,
+		di->bat_cap.level,
+		di->bat_cap.prev_mah,
+		di->bat_cap.prev_percent,
+		di->bat_cap.prev_level,
+		di->vbat,
+		di->inst_curr,
+		di->avg_curr,
+		di->accu_charge,
+		di->flags.charging,
+		di->charge_state,
+		di->discharge_state,
+		di->high_curr_mode,
+		di->recovery_needed);
+}
+
+/**
+ * ab8500_fg_periodic_work() - Run the FG state machine periodically
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for periodic work
+ */
+static void ab8500_fg_periodic_work(struct work_struct *work)
+{
+	struct ab8500_fg *di = container_of(work, struct ab8500_fg,
+		fg_periodic_work.work);
+
+	if (di->init_capacity) {
+		/* A dummy read that will return 0 */
+		di->inst_curr = ab8500_fg_inst_curr(di);
+		/* Get an initial capacity calculation */
+		ab8500_fg_calc_cap_discharge_voltage(di, true);
+		ab8500_fg_check_capacity_limits(di, true);
+		di->init_capacity = false;
+		queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+	} else
+		ab8500_fg_algorithm(di);
+}
+
+/**
+ * ab8500_fg_low_bat_work() - Check LOW_BAT condition
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the LOW_BAT condition
+ */
+static void ab8500_fg_low_bat_work(struct work_struct *work)
+{
+	int vbat;
+
+	struct ab8500_fg *di = container_of(work, struct ab8500_fg,
+		fg_low_bat_work.work);
+
+	vbat = ab8500_fg_bat_voltage(di);
+
+	/* Check if LOW_BAT still fulfilled */
+	if (vbat < di->bat->fg_params->lowbat_threshold) {
+		di->flags.low_bat = true;
+		dev_warn(di->dev, "Battery voltage still LOW\n");
+
+		/*
+		 * We need to re-schedule this check to be able to detect
+		 * if the voltage increases again during charging
+		 */
+		queue_delayed_work(di->fg_wq, &di->fg_low_bat_work,
+			round_jiffies(LOW_BAT_CHECK_INTERVAL));
+	} else {
+		di->flags.low_bat = false;
+		dev_warn(di->dev, "Battery voltage OK again\n");
+	}
+
+	/* This is needed to dispatch LOW_BAT */
+	ab8500_fg_check_capacity_limits(di, false);
+
+	/* Set this flag to check if LOW_BAT IRQ still occurs */
+	di->flags.low_bat_delay = false;
+}
+
+/**
+ * ab8500_fg_instant_work() - Run the FG state machine instantly
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for instant work
+ */
+static void ab8500_fg_instant_work(struct work_struct *work)
+{
+	struct ab8500_fg *di = container_of(work, struct ab8500_fg, fg_work);
+
+	ab8500_fg_algorithm(di);
+}
+
+/**
+ * ab8500_fg_cc_convend_handler() - isr to get battery avg current.
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_fg structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_fg_cc_int_calib_handler(int irq, void *_di)
+{
+	struct ab8500_fg *di = _di;
+	di->calib_state = AB8500_FG_CALIB_END;
+	queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_fg_cc_convend_handler() - isr to get battery avg current.
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_fg structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_fg_cc_convend_handler(int irq, void *_di)
+{
+	struct ab8500_fg *di = _di;
+
+	queue_work(di->fg_wq, &di->fg_acc_cur_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_fg_batt_ovv_handler() - Battery OVV occured
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_fg structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_fg_batt_ovv_handler(int irq, void *_di)
+{
+	struct ab8500_fg *di = _di;
+
+	dev_dbg(di->dev, "Battery OVV\n");
+	di->flags.bat_ovv = true;
+
+	power_supply_changed(&di->fg_psy);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_fg_lowbatf_handler() - Battery voltage is below LOW threshold
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab8500_fg structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_fg_lowbatf_handler(int irq, void *_di)
+{
+	struct ab8500_fg *di = _di;
+
+	if (!di->flags.low_bat_delay) {
+		dev_warn(di->dev, "Battery voltage is below LOW threshold\n");
+		di->flags.low_bat_delay = true;
+		/*
+		 * Start a timer to check LOW_BAT again after some time
+		 * This is done to avoid shutdown on single voltage dips
+		 */
+		queue_delayed_work(di->fg_wq, &di->fg_low_bat_work,
+			round_jiffies(LOW_BAT_CHECK_INTERVAL));
+	}
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab8500_fg_get_property() - get the fg properties
+ * @psy:	pointer to the power_supply structure
+ * @psp:	pointer to the power_supply_property structure
+ * @val:	pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the
+ * fg properties by reading the sysfs files.
+ * voltage_now:		battery voltage
+ * current_now:		battery instant current
+ * current_avg:		battery average current
+ * charge_full_design:	capacity where battery is considered full
+ * charge_now:		battery capacity in nAh
+ * capacity:		capacity in percent
+ * capacity_level:	capacity level
+ *
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab8500_fg_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	/*
+	 * If battery is identified as unknown and charging of unknown
+	 * batteries is disabled, we always report 100% capacity and
+	 * capacity level UNKNOWN, since we can't calculate
+	 * remaining capacity
+	 */
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		if (di->flags.bat_ovv)
+			val->intval = 47500000;
+		else
+			val->intval = di->vbat * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		val->intval = di->inst_curr * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_AVG:
+		val->intval = di->avg_curr * 1000;
+		break;
+	case POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN:
+		val->intval = ab8500_fg_convert_mah_to_uwh(di,
+				di->bat_cap.max_mah_design);
+		break;
+	case POWER_SUPPLY_PROP_ENERGY_FULL:
+		val->intval = ab8500_fg_convert_mah_to_uwh(di,
+				di->bat_cap.max_mah);
+		break;
+	case POWER_SUPPLY_PROP_ENERGY_NOW:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = ab8500_fg_convert_mah_to_uwh(di,
+					di->bat_cap.max_mah);
+		else
+			val->intval = ab8500_fg_convert_mah_to_uwh(di,
+					di->bat_cap.prev_mah);
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
+		val->intval = di->bat_cap.max_mah_design;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL:
+		val->intval = di->bat_cap.max_mah;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = di->bat_cap.max_mah;
+		else
+			val->intval = di->bat_cap.prev_mah;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = 100;
+		else
+			val->intval = di->bat_cap.prev_percent;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY_LEVEL:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_UNKNOWN;
+		else
+			val->intval = di->bat_cap.prev_level;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ab8500_fg_get_ext_psy_data(struct device *dev, void *data)
+{
+	struct power_supply *psy;
+	struct power_supply *ext;
+	struct ab8500_fg *di;
+	union power_supply_propval ret;
+	int i, j;
+	bool psy_found = false;
+
+	psy = (struct power_supply *)data;
+	ext = dev_get_drvdata(dev);
+	di = to_ab8500_fg_device_info(psy);
+
+	/*
+	 * For all psy where the name of your driver
+	 * appears in any supplied_to
+	 */
+	for (i = 0; i < ext->num_supplicants; i++) {
+		if (!strcmp(ext->supplied_to[i], psy->name))
+			psy_found = true;
+	}
+
+	if (!psy_found)
+		return 0;
+
+	/* Go through all properties for the psy */
+	for (j = 0; j < ext->num_properties; j++) {
+		enum power_supply_property prop;
+		prop = ext->properties[j];
+
+		if (ext->get_property(ext, prop, &ret))
+			continue;
+
+		switch (prop) {
+		case POWER_SUPPLY_PROP_STATUS:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				switch (ret.intval) {
+				case POWER_SUPPLY_STATUS_UNKNOWN:
+				case POWER_SUPPLY_STATUS_DISCHARGING:
+				case POWER_SUPPLY_STATUS_NOT_CHARGING:
+					if (!di->flags.charging)
+						break;
+					di->flags.charging = false;
+					di->flags.fully_charged = false;
+					queue_work(di->fg_wq, &di->fg_work);
+					break;
+				case POWER_SUPPLY_STATUS_FULL:
+					if (di->flags.fully_charged)
+						break;
+					di->flags.fully_charged = true;
+					/* Save current capacity as maximum */
+					di->bat_cap.max_mah = di->bat_cap.mah;
+					sysfs_notify(&di->fg_kobject,
+						NULL, "charge_full");
+					queue_work(di->fg_wq, &di->fg_work);
+					break;
+				case POWER_SUPPLY_STATUS_CHARGING:
+					if (di->flags.charging)
+						break;
+					di->flags.charging = true;
+					di->flags.fully_charged = false;
+					queue_work(di->fg_wq, &di->fg_work);
+					break;
+				};
+			default:
+				break;
+			};
+			break;
+		case POWER_SUPPLY_PROP_TECHNOLOGY:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				if (ret.intval)
+					di->flags.batt_unknown = false;
+				else
+					di->flags.batt_unknown = true;
+				break;
+			default:
+				break;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+/**
+ * ab8500_fg_init_hw_registers() - Set up FG related registers
+ * @di:		pointer to the ab8500_fg structure
+ *
+ * Set up battery OVV, low battery voltage registers
+ */
+static int ab8500_fg_init_hw_registers(struct ab8500_fg *di)
+{
+	int ret;
+
+	/* Set up VBAT OVV register */
+	ret = abx500_set_register_interruptible(di->dev,
+		AB8500_CHARGER,
+		AB8500_BATT_OVV,
+		(BATT_OVV_ENA | BATT_OVV_TH_4P75));
+	if (ret) {
+		dev_err(di->dev, "failed to set BATT_OVV\n");
+		goto out;
+	}
+
+	/* Low Battery Voltage */
+	ret = abx500_set_register_interruptible(di->dev,
+		AB8500_SYS_CTRL2_BLOCK,
+		AB8500_LOW_BAT_REG,
+		ab8500_volt_to_regval(
+			di->bat->fg_params->lowbat_threshold) << 1 |
+		LOW_BAT_ENABLE);
+	if (ret) {
+		dev_err(di->dev, "%s write failed\n", __func__);
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+/**
+ * ab8500_fg_external_power_changed() - callback for power supply changes
+ * @psy:       pointer to the structure power_supply
+ *
+ * This function is the entry point of the pointer external_power_changed
+ * of the structure power_supply.
+ * This function gets executed when there is a change in any external power
+ * supply that this driver needs to be notified of.
+ */
+static void ab8500_fg_external_power_changed(struct power_supply *psy)
+{
+	struct ab8500_fg *di = to_ab8500_fg_device_info(psy);
+
+	class_for_each_device(power_supply_class, NULL,
+		&di->fg_psy, ab8500_fg_get_ext_psy_data);
+}
+
+/* Exposure to the sysfs interface */
+
+struct ab8500_fg_sysfs_entry {
+	struct attribute attr;
+	ssize_t (*show)(struct ab8500_fg *, char *);
+	ssize_t (*store)(struct ab8500_fg *, const char *, size_t);
+};
+
+static ssize_t charge_full_show(struct ab8500_fg *di, char *buf)
+{
+	return sprintf(buf, "%d\n", di->bat_cap.max_mah);
+}
+
+static ssize_t charge_full_store(struct ab8500_fg *di, const char *buf,
+				 size_t count)
+{
+	unsigned long charge_full;
+	ssize_t ret = -EINVAL;
+
+	ret = strict_strtoul(buf, 10, &charge_full);
+
+	dev_dbg(di->dev, "Ret %d charge_full %lu", ret, charge_full);
+
+	if (!ret) {
+		di->bat_cap.max_mah = (int) charge_full;
+		ret = count;
+	}
+
+	return ret;
+}
+static struct ab8500_fg_sysfs_entry charge_full_attr =
+	__ATTR(charge_full, 0644, charge_full_show, charge_full_store);
+
+static ssize_t
+ab8500_fg_show(struct kobject *kobj, struct attribute *attr, char *buf)
+{
+	struct ab8500_fg_sysfs_entry *entry;
+	struct ab8500_fg *di;
+
+	entry = container_of(attr, struct ab8500_fg_sysfs_entry, attr);
+	di = container_of(kobj, struct ab8500_fg, fg_kobject);
+
+	if (!entry->show)
+		return -EIO;
+
+	return entry->show(di, buf);
+}
+
+static ssize_t
+ab8500_fg_store(struct kobject *kobj, struct attribute *attr, const char *buf,
+		size_t count)
+{
+	struct ab8500_fg_sysfs_entry *entry;
+	struct ab8500_fg *di;
+
+	entry = container_of(attr, struct ab8500_fg_sysfs_entry, attr);
+	di = container_of(kobj, struct ab8500_fg, fg_kobject);
+
+	if (!entry->store)
+		return -EIO;
+
+	return entry->store(di, buf, count);
+}
+
+const struct sysfs_ops ab8500_fg_sysfs_ops = {
+	.show = ab8500_fg_show,
+	.store = ab8500_fg_store,
+};
+
+static struct attribute *ab8500_fg_attrs[] = {
+	&charge_full_attr.attr,
+	NULL,
+};
+
+static struct kobj_type ab8500_fg_ktype = {
+	.sysfs_ops = &ab8500_fg_sysfs_ops,
+	.default_attrs = ab8500_fg_attrs,
+};
+
+/**
+ * ab8500_chargalg_sysfs_exit() - de-init of sysfs entry
+ * @di:                pointer to the struct ab8500_chargalg
+ *
+ * This function removes the entry in sysfs.
+ */
+static void ab8500_fg_sysfs_exit(struct ab8500_fg *di)
+{
+	kobject_del(&di->fg_kobject);
+}
+
+/**
+ * ab8500_chargalg_sysfs_init() - init of sysfs entry
+ * @di:                pointer to the struct ab8500_chargalg
+ *
+ * This function adds an entry in sysfs.
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_fg_sysfs_init(struct ab8500_fg *di)
+{
+	int ret = 0;
+
+	ret = kobject_init_and_add(&di->fg_kobject,
+		&ab8500_fg_ktype,
+		NULL, "ab8500_fg");
+	if (ret < 0)
+		dev_err(di->dev, "failed to create sysfs entry\n");
+
+	return ret;
+}
+/* Exposure to the sysfs interface <<END>> */
+
+#if defined(CONFIG_PM)
+static int ab8500_fg_resume(struct platform_device *pdev)
+{
+	struct ab8500_fg *di = platform_get_drvdata(pdev);
+
+	/*
+	 * Change state if we're not charging. If we're charging we will wake
+	 * up on the FG IRQ
+	 */
+	if (!di->flags.charging) {
+		ab8500_fg_discharge_state_to(di, AB8500_FG_DISCHARGE_WAKEUP);
+		queue_work(di->fg_wq, &di->fg_work);
+	}
+
+	return 0;
+}
+
+static int ab8500_fg_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct ab8500_fg *di = platform_get_drvdata(pdev);
+
+	flush_delayed_work(&di->fg_periodic_work);
+
+	/*
+	 * If the FG is enabled we will disable it before going to suspend
+	 * only if we're not charging
+	 */
+	if (di->flags.fg_enabled && !di->flags.charging)
+		ab8500_fg_coulomb_counter(di, false);
+
+	return 0;
+}
+#else
+#define ab8500_fg_suspend      NULL
+#define ab8500_fg_resume       NULL
+#endif
+
+static int __devexit ab8500_fg_remove(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct ab8500_fg *di = platform_get_drvdata(pdev);
+
+	/* Disable coulomb counter */
+	ret = ab8500_fg_coulomb_counter(di, false);
+	if (ret)
+		dev_err(di->dev, "failed to disable coulomb counter\n");
+
+	destroy_workqueue(di->fg_wq);
+	ab8500_fg_sysfs_exit(di);
+
+	flush_scheduled_work();
+	power_supply_unregister(&di->fg_psy);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+	return ret;
+}
+
+/* ab8500 fg driver interrupts and their respective isr */
+static struct ab8500_fg_interrupts ab8500_fg_irq[] = {
+	{"NCONV_ACCU", ab8500_fg_cc_convend_handler},
+	{"BATT_OVV", ab8500_fg_batt_ovv_handler},
+	{"LOW_BAT_F", ab8500_fg_lowbatf_handler},
+	{"CC_INT_CALIB", ab8500_fg_cc_int_calib_handler},
+};
+
+static int __devinit ab8500_fg_probe(struct platform_device *pdev)
+{
+	int i, irq;
+	struct ab8500_platform_data *plat;
+	int ret = 0;
+
+	struct ab8500_fg *di =
+		kzalloc(sizeof(struct ab8500_fg), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	mutex_init(&di->cc_lock);
+
+	/* get parent data */
+	di->dev = &pdev->dev;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
+	di->gpadc = ab8500_gpadc_get();
+
+	plat = dev_get_platdata(di->parent->dev);
+
+	/* get fg specific platform data */
+	if (!plat->fg) {
+		dev_err(di->dev, "no fg platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+	di->pdata = plat->fg;
+
+	/* get battery specific platform data */
+	if (!plat->battery) {
+		dev_err(di->dev, "no battery platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+	di->bat = plat->battery;
+
+	di->fg_psy.name = "ab8500_fg";
+	di->fg_psy.type = POWER_SUPPLY_TYPE_BATTERY;
+	di->fg_psy.properties = ab8500_fg_props;
+	di->fg_psy.num_properties = ARRAY_SIZE(ab8500_fg_props);
+	di->fg_psy.get_property = ab8500_fg_get_property;
+	di->fg_psy.supplied_to = di->pdata->supplied_to;
+	di->fg_psy.num_supplicants = di->pdata->num_supplicants;
+	di->fg_psy.external_power_changed = ab8500_fg_external_power_changed;
+
+	di->bat_cap.max_mah_design = MILLI_TO_MICRO *
+		di->bat->bat_type[di->bat->batt_id].charge_full_design;
+
+	di->bat_cap.max_mah = di->bat_cap.max_mah_design;
+
+	di->vbat_nom = di->bat->bat_type[di->bat->batt_id].nominal_voltage;
+
+	di->init_capacity = true;
+
+	ab8500_fg_charge_state_to(di, AB8500_FG_CHARGE_INIT);
+	ab8500_fg_discharge_state_to(di, AB8500_FG_DISCHARGE_INIT);
+
+	/* Create a work queue for running the FG algorithm */
+	di->fg_wq = create_singlethread_workqueue("ab8500_fg_wq");
+	if (di->fg_wq == NULL) {
+		dev_err(di->dev, "failed to create work queue\n");
+		goto free_device_info;
+	}
+
+	/* Init work for running the fg algorithm instantly */
+	INIT_WORK(&di->fg_work, ab8500_fg_instant_work);
+
+	/* Init work for getting the battery accumulated current */
+	INIT_WORK(&di->fg_acc_cur_work, ab8500_fg_acc_cur_work);
+
+	/* Work delayed Queue to run the state machine */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->fg_periodic_work,
+		ab8500_fg_periodic_work);
+
+	/* Work to check low battery condition */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->fg_low_bat_work,
+		ab8500_fg_low_bat_work);
+
+	/* Initialize OVV, and other registers */
+	ret = ab8500_fg_init_hw_registers(di);
+	if (ret) {
+		dev_err(di->dev, "failed to initialize registers\n");
+		goto free_fg_wq;
+	}
+
+	/* Consider battery unknown until we're informed otherwise */
+	di->flags.batt_unknown = true;
+
+	/* Register FG power supply class */
+	ret = power_supply_register(di->dev, &di->fg_psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register FG psy\n");
+		goto free_fg_wq;
+	}
+
+	di->fg_samples = SEC_TO_SAMPLE(di->bat->fg_params->init_timer);
+	ab8500_fg_coulomb_counter(di, true);
+
+	/* Register interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab8500_fg_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab8500_fg_irq[i].name);
+		ret = request_threaded_irq(irq, NULL, ab8500_fg_irq[i].isr,
+			IRQF_SHARED | IRQF_NO_SUSPEND,
+			ab8500_fg_irq[i].name, di);
+
+		if (ret != 0) {
+			dev_err(di->dev, "failed to request %s IRQ %d: %d\n"
+				, ab8500_fg_irq[i].name, irq, ret);
+			goto free_irq;
+		}
+		dev_dbg(di->dev, "Requested %s IRQ %d: %d\n",
+			ab8500_fg_irq[i].name, irq, ret);
+	}
+
+	platform_set_drvdata(pdev, di);
+
+	ret = ab8500_fg_sysfs_init(di);
+	if (ret) {
+		dev_err(di->dev, "failed to create sysfs entry\n");
+		goto free_irq;
+	}
+
+	/* Calibrate the fg first time */
+	di->flags.calibrate = true;
+	di->calib_state = AB8500_FG_CALIB_INIT;
+	/* Run the FG algorithm */
+	queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+
+	return ret;
+
+free_irq:
+	power_supply_unregister(&di->fg_psy);
+
+	/* We also have to free all successfully registered irqs */
+	for (i = i - 1; i >= 0; i--) {
+		irq = platform_get_irq_byname(pdev, ab8500_fg_irq[i].name);
+		free_irq(irq, di);
+	}
+free_fg_wq:
+	destroy_workqueue(di->fg_wq);
+free_device_info:
+	kfree(di);
+
+	return ret;
+}
+
+static struct platform_driver ab8500_fg_driver = {
+	.probe = ab8500_fg_probe,
+	.remove = __devexit_p(ab8500_fg_remove),
+	.suspend = ab8500_fg_suspend,
+	.resume = ab8500_fg_resume,
+	.driver = {
+		.name = "ab8500-fg",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab8500_fg_init(void)
+{
+	return platform_driver_register(&ab8500_fg_driver);
+}
+
+static void __exit ab8500_fg_exit(void)
+{
+	platform_driver_unregister(&ab8500_fg_driver);
+}
+
+subsys_initcall_sync(ab8500_fg_init);
+module_exit(ab8500_fg_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Johan Palsson, Karl Komierowski");
+MODULE_ALIAS("platform:ab8500-fg");
+MODULE_DESCRIPTION("AB8500 Fuel Gauge driver");
diff --git a/drivers/power/abx500_chargalg.c b/drivers/power/abx500_chargalg.c
new file mode 100644
index 00000000000..bb0fa42b109
--- /dev/null
+++ b/drivers/power/abx500_chargalg.c
@@ -0,0 +1,1920 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Charging algorithm driver for abx500 variants
+ *
+ * License Terms: GNU General Public License v2
+ * Authors:
+ *	Johan Palsson <johan.palsson@stericsson.com>
+ *	Karl Komierowski <karl.komierowski@stericsson.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/completion.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ux500_chargalg.h>
+#include <linux/mfd/abx500/ab5500-bm.h>
+
+/* Watchdog kick interval */
+#define CHG_WD_INTERVAL			(6 * HZ)
+
+/* End-of-charge criteria counter */
+#define EOC_COND_CNT			10
+
+/* Recharge criteria counter */
+#define RCH_COND_CNT			3
+
+#define to_abx500_chargalg_device_info(x) container_of((x), \
+	struct abx500_chargalg, chargalg_psy);
+
+enum abx500_chargers {
+	NO_CHG,
+	AC_CHG,
+	USB_CHG,
+};
+
+struct abx500_chargalg_charger_info {
+	enum abx500_chargers conn_chg;
+	enum abx500_chargers prev_conn_chg;
+	enum abx500_chargers online_chg;
+	enum abx500_chargers prev_online_chg;
+	enum abx500_chargers charger_type;
+	bool usb_chg_ok;
+	bool ac_chg_ok;
+	int usb_volt;
+	int usb_curr;
+	int ac_volt;
+	int ac_curr;
+	int usb_vset;
+	int usb_iset;
+	int ac_vset;
+	int ac_iset;
+};
+
+struct abx500_chargalg_suspension_status {
+	bool suspended_change;
+	bool ac_suspended;
+	bool usb_suspended;
+};
+
+struct abx500_chargalg_battery_data {
+	int temp;
+	int volt;
+	int avg_curr;
+	int inst_curr;
+	int percent;
+};
+
+enum abx500_chargalg_states {
+	STATE_HANDHELD_INIT,
+	STATE_HANDHELD,
+	STATE_CHG_NOT_OK_INIT,
+	STATE_CHG_NOT_OK,
+	STATE_HW_TEMP_PROTECT_INIT,
+	STATE_HW_TEMP_PROTECT,
+	STATE_NORMAL_INIT,
+	STATE_NORMAL,
+	STATE_WAIT_FOR_RECHARGE_INIT,
+	STATE_WAIT_FOR_RECHARGE,
+	STATE_MAINTENANCE_A_INIT,
+	STATE_MAINTENANCE_A,
+	STATE_MAINTENANCE_B_INIT,
+	STATE_MAINTENANCE_B,
+	STATE_TEMP_UNDEROVER_INIT,
+	STATE_TEMP_UNDEROVER,
+	STATE_TEMP_LOWHIGH_INIT,
+	STATE_TEMP_LOWHIGH,
+	STATE_SUSPENDED_INIT,
+	STATE_SUSPENDED,
+	STATE_OVV_PROTECT_INIT,
+	STATE_OVV_PROTECT,
+	STATE_SAFETY_TIMER_EXPIRED_INIT,
+	STATE_SAFETY_TIMER_EXPIRED,
+	STATE_BATT_REMOVED_INIT,
+	STATE_BATT_REMOVED,
+	STATE_WD_EXPIRED_INIT,
+	STATE_WD_EXPIRED,
+};
+
+static const char *states[] = {
+	"HANDHELD_INIT",
+	"HANDHELD",
+	"CHG_NOT_OK_INIT",
+	"CHG_NOT_OK",
+	"HW_TEMP_PROTECT_INIT",
+	"HW_TEMP_PROTECT",
+	"NORMAL_INIT",
+	"NORMAL",
+	"WAIT_FOR_RECHARGE_INIT",
+	"WAIT_FOR_RECHARGE",
+	"MAINTENANCE_A_INIT",
+	"MAINTENANCE_A",
+	"MAINTENANCE_B_INIT",
+	"MAINTENANCE_B",
+	"TEMP_UNDEROVER_INIT",
+	"TEMP_UNDEROVER",
+	"TEMP_LOWHIGH_INIT",
+	"TEMP_LOWHIGH",
+	"SUSPENDED_INIT",
+	"SUSPENDED",
+	"OVV_PROTECT_INIT",
+	"OVV_PROTECT",
+	"SAFETY_TIMER_EXPIRED_INIT",
+	"SAFETY_TIMER_EXPIRED",
+	"BATT_REMOVED_INIT",
+	"BATT_REMOVED",
+	"WD_EXPIRED_INIT",
+	"WD_EXPIRED",
+};
+
+struct abx500_chargalg_events {
+	bool batt_unknown;
+	bool mainextchnotok;
+	bool batt_ovv;
+	bool batt_rem;
+	bool btemp_underover;
+	bool btemp_lowhigh;
+	bool main_thermal_prot;
+	bool usb_thermal_prot;
+	bool main_ovv;
+	bool vbus_ovv;
+	bool usbchargernotok;
+	bool safety_timer_expired;
+	bool maintenance_timer_expired;
+	bool ac_wd_expired;
+	bool usb_wd_expired;
+	bool ac_cv_active;
+	bool usb_cv_active;
+	bool vbus_collapsed;
+};
+
+/**
+ * struct abx500_charge_curr_maximization - Charger maximization parameters
+ * @original_iset:	the non optimized/maximised charger current
+ * @current_iset:	the charging current used at this moment
+ * @test_delta_i:	the delta between the current we want to charge and the
+			current that is really going into the battery
+ * @condition_cnt:	number of iterations needed before a new charger current
+			is set
+ * @max_current:	maximum charger current
+ * @wait_cnt:		to avoid too fast current step down in case of charger
+ *			voltage collapse, we insert this delay between step
+ *			down
+ * @level:		tells in how many steps the charging current has been
+			increased
+ */
+struct abx500_charge_curr_maximization {
+	int original_iset;
+	int current_iset;
+	int test_delta_i;
+	int condition_cnt;
+	int max_current;
+	int wait_cnt;
+	u8 level;
+};
+
+enum maxim_ret {
+	MAXIM_RET_NOACTION,
+	MAXIM_RET_CHANGE,
+	MAXIM_RET_IBAT_TOO_HIGH,
+};
+
+/**
+ * struct abx500_chargalg - abx500 Charging algorithm device information
+ * @dev:		pointer to the structure device
+ * @charge_status:	battery operating status
+ * @eoc_cnt:		counter used to determine end-of_charge
+ * @rch_cnt:		counter used to determine start of recharge
+ * @maintenance_chg:	indicate if maintenance charge is active
+ * @t_hyst_norm		temperature hysteresis when the temperature has been
+ *			over or under normal limits
+ * @t_hyst_lowhigh	temperature hysteresis when the temperature has been
+ *			over or under the high or low limits
+ * @charge_state:	current state of the charging algorithm
+ * @ccm			charging current maximization parameters
+ * @chg_info:		information about connected charger types
+ * @batt_data:		data of the battery
+ * @susp_status:	current charger suspension status
+ * @pdata:		pointer to the abx500_chargalg platform data
+ * @bat:		pointer to the abx500_bm platform data
+ * @chargalg_psy:	structure that holds the battery properties exposed by
+ *			the charging algorithm
+ * @events:		structure for information about events triggered
+ * @chargalg_wq:		work queue for running the charging algorithm
+ * @chargalg_periodic_work:	work to run the charging algorithm periodically
+ * @chargalg_wd_work:		work to kick the charger watchdog periodically
+ * @chargalg_work:		work to run the charging algorithm instantly
+ * @safety_timer:		charging safety timer
+ * @maintenance_timer:		maintenance charging timer
+ * @chargalg_kobject:		structure of type kobject
+ */
+struct abx500_chargalg {
+	struct device *dev;
+	int charge_status;
+	int eoc_cnt;
+	int rch_cnt;
+	bool maintenance_chg;
+	int t_hyst_norm;
+	int t_hyst_lowhigh;
+	enum abx500_chargalg_states charge_state;
+	struct abx500_charge_curr_maximization ccm;
+	struct abx500_chargalg_charger_info chg_info;
+	struct abx500_chargalg_battery_data batt_data;
+	struct abx500_chargalg_suspension_status susp_status;
+	struct abx500_chargalg_platform_data *pdata;
+	struct abx500_bm_data *bat;
+	struct power_supply chargalg_psy;
+	struct ux500_charger *ac_chg;
+	struct ux500_charger *usb_chg;
+	struct abx500_chargalg_events events;
+	struct workqueue_struct *chargalg_wq;
+	struct delayed_work chargalg_periodic_work;
+	struct delayed_work chargalg_wd_work;
+	struct work_struct chargalg_work;
+	struct timer_list safety_timer;
+	struct timer_list maintenance_timer;
+	struct kobject chargalg_kobject;
+};
+
+/* Main battery properties */
+static enum power_supply_property abx500_chargalg_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_HEALTH,
+};
+
+/**
+ * abx500_chargalg_safety_timer_expired() - Expiration of the safety timer
+ * @data:	pointer to the abx500_chargalg structure
+ *
+ * This function gets called when the safety timer for the charger
+ * expires
+ */
+static void abx500_chargalg_safety_timer_expired(unsigned long data)
+{
+	struct abx500_chargalg *di = (struct abx500_chargalg *) data;
+	dev_err(di->dev, "Safety timer expired\n");
+	di->events.safety_timer_expired = true;
+
+	/* Trigger execution of the algorithm instantly */
+	queue_work(di->chargalg_wq, &di->chargalg_work);
+}
+
+/**
+ * abx500_chargalg_maintenance_timer_expired() - Expiration of
+ * the maintenance timer
+ * @i:		pointer to the abx500_chargalg structure
+ *
+ * This function gets called when the maintenence timer
+ * expires
+ */
+static void abx500_chargalg_maintenance_timer_expired(unsigned long data)
+{
+
+	struct abx500_chargalg *di = (struct abx500_chargalg *) data;
+	dev_dbg(di->dev, "Maintenance timer expired\n");
+	di->events.maintenance_timer_expired = true;
+
+	/* Trigger execution of the algorithm instantly */
+	queue_work(di->chargalg_wq, &di->chargalg_work);
+}
+
+/**
+ * abx500_chargalg_state_to() - Change charge state
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * This function gets called when a charge state change should occur
+ */
+static void abx500_chargalg_state_to(struct abx500_chargalg *di,
+	enum abx500_chargalg_states state)
+{
+	dev_dbg(di->dev,
+		"State changed: %s (From state: [%d] %s =to=> [%d] %s )\n",
+		di->charge_state == state ? "NO" : "YES",
+		di->charge_state,
+		states[di->charge_state],
+		state,
+		states[state]);
+
+	di->charge_state = state;
+}
+
+/**
+ * abx500_chargalg_check_charger_connection() - Check charger connection change
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * This function will check if there is a change in the charger connection
+ * and change charge state accordingly. AC has precedence over USB.
+ */
+static int abx500_chargalg_check_charger_connection(struct abx500_chargalg *di)
+{
+	if (di->chg_info.conn_chg != di->chg_info.prev_conn_chg ||
+		di->susp_status.suspended_change) {
+		/*
+		 * Charger state changed or suspension
+		 * has changed since last update
+		 */
+		if ((di->chg_info.conn_chg & AC_CHG) &&
+			!di->susp_status.ac_suspended) {
+			dev_dbg(di->dev, "Charging source is AC\n");
+			if (di->chg_info.charger_type != AC_CHG) {
+				di->chg_info.charger_type = AC_CHG;
+				abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+			}
+		} else if ((di->chg_info.conn_chg & USB_CHG) &&
+			!di->susp_status.usb_suspended) {
+			dev_dbg(di->dev, "Charging source is USB\n");
+			di->chg_info.charger_type = USB_CHG;
+			abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		} else if (di->chg_info.conn_chg &&
+			(di->susp_status.ac_suspended ||
+			di->susp_status.usb_suspended)) {
+			dev_dbg(di->dev, "Charging is suspended\n");
+			di->chg_info.charger_type = NO_CHG;
+			abx500_chargalg_state_to(di, STATE_SUSPENDED_INIT);
+		} else {
+			dev_dbg(di->dev, "Charging source is OFF\n");
+			di->chg_info.charger_type = NO_CHG;
+			abx500_chargalg_state_to(di, STATE_HANDHELD_INIT);
+		}
+		di->chg_info.prev_conn_chg = di->chg_info.conn_chg;
+		di->susp_status.suspended_change = false;
+	}
+	return di->chg_info.conn_chg;
+}
+
+/**
+ * abx500_chargalg_start_safety_timer() - Start charging safety timer
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * The safety timer is used to avoid overcharging of old or bad batteries.
+ * There are different timers for AC and USB
+ */
+static void abx500_chargalg_start_safety_timer(struct abx500_chargalg *di)
+{
+	unsigned long timer_expiration = 0;
+
+	switch (di->chg_info.charger_type) {
+	case AC_CHG:
+		timer_expiration =
+		round_jiffies(jiffies +
+			(di->bat->main_safety_tmr_h * 3600 * HZ));
+		break;
+
+	case USB_CHG:
+		timer_expiration =
+		round_jiffies(jiffies +
+			(di->bat->usb_safety_tmr_h * 3600 * HZ));
+		break;
+
+	default:
+		dev_err(di->dev, "Unknown charger to charge from\n");
+		break;
+	}
+
+	di->events.safety_timer_expired = false;
+	di->safety_timer.expires = timer_expiration;
+	if (!timer_pending(&di->safety_timer))
+		add_timer(&di->safety_timer);
+	else
+		mod_timer(&di->safety_timer, timer_expiration);
+}
+
+/**
+ * abx500_chargalg_stop_safety_timer() - Stop charging safety timer
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * The safety timer is stopped whenever the NORMAL state is exited
+ */
+static void abx500_chargalg_stop_safety_timer(struct abx500_chargalg *di)
+{
+	di->events.safety_timer_expired = false;
+	del_timer(&di->safety_timer);
+}
+
+/**
+ * abx500_chargalg_start_maintenance_timer() - Start charging maintenance timer
+ * @di:		pointer to the abx500_chargalg structure
+ * @duration:	duration of ther maintenance timer in hours
+ *
+ * The maintenance timer is used to maintain the charge in the battery once
+ * the battery is considered full. These timers are chosen to match the
+ * discharge curve of the battery
+ */
+static void abx500_chargalg_start_maintenance_timer(struct abx500_chargalg *di,
+	int duration)
+{
+	unsigned long timer_expiration;
+
+	/* Convert from hours to jiffies */
+	timer_expiration = round_jiffies(jiffies + (duration * 3600 * HZ));
+
+	di->events.maintenance_timer_expired = false;
+	di->maintenance_timer.expires = timer_expiration;
+	if (!timer_pending(&di->maintenance_timer))
+		add_timer(&di->maintenance_timer);
+	else
+		mod_timer(&di->maintenance_timer, timer_expiration);
+}
+
+/**
+ * abx500_chargalg_stop_maintenance_timer() - Stop maintenance timer
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * The maintenance timer is stopped whenever maintenance ends or when another
+ * state is entered
+ */
+static void abx500_chargalg_stop_maintenance_timer(struct abx500_chargalg *di)
+{
+	di->events.maintenance_timer_expired = false;
+	del_timer(&di->maintenance_timer);
+}
+
+/**
+ * abx500_chargalg_kick_watchdog() - Kick charger watchdog
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * The charger watchdog have to be kicked periodically whenever the charger is
+ * on, else the ABB will reset the system
+ */
+static int abx500_chargalg_kick_watchdog(struct abx500_chargalg *di)
+{
+	/* Check if charger exists and kick watchdog if charging */
+	if (di->ac_chg && di->ac_chg->ops.kick_wd &&
+			di->chg_info.online_chg & AC_CHG)
+		return di->ac_chg->ops.kick_wd(di->ac_chg);
+	else if (di->usb_chg && di->usb_chg->ops.kick_wd &&
+			di->chg_info.online_chg & USB_CHG)
+		return di->usb_chg->ops.kick_wd(di->usb_chg);
+
+	return -ENXIO;
+}
+
+/**
+ * abx500_chargalg_ac_en() - Turn on/off the AC charger
+ * @di:		pointer to the abx500_chargalg structure
+ * @enable:	charger on/off
+ * @vset:	requested charger output voltage
+ * @iset:	requested charger output current
+ *
+ * The AC charger will be turned on/off with the requested charge voltage and
+ * current
+ */
+static int abx500_chargalg_ac_en(struct abx500_chargalg *di, int enable,
+	int vset, int iset)
+{
+	if (!di->ac_chg || !di->ac_chg->ops.enable)
+		return -ENXIO;
+
+	/* Select maximum of what both the charger and the battery supports */
+	if (di->ac_chg->max_out_volt)
+		vset = min(vset, di->ac_chg->max_out_volt);
+	if (di->ac_chg->max_out_curr)
+		iset = min(iset, di->ac_chg->max_out_curr);
+
+	di->chg_info.ac_iset = iset;
+	di->chg_info.ac_vset = vset;
+
+	return di->ac_chg->ops.enable(di->ac_chg, enable, vset, iset);
+}
+
+/**
+ * abx500_chargalg_usb_en() - Turn on/off the USB charger
+ * @di:		pointer to the abx500_chargalg structure
+ * @enable:	charger on/off
+ * @vset:	requested charger output voltage
+ * @iset:	requested charger output current
+ *
+ * The USB charger will be turned on/off with the requested charge voltage and
+ * current
+ */
+static int abx500_chargalg_usb_en(struct abx500_chargalg *di, int enable,
+	int vset, int iset)
+{
+	if (!di->usb_chg || !di->usb_chg->ops.enable)
+		return -ENXIO;
+
+	/* Select maximum of what both the charger and the battery supports */
+	if (di->usb_chg->max_out_volt)
+		vset = min(vset, di->usb_chg->max_out_volt);
+	if (di->usb_chg->max_out_curr)
+		iset = min(iset, di->usb_chg->max_out_curr);
+
+	di->chg_info.usb_iset = iset;
+	di->chg_info.usb_vset = vset;
+
+	return di->usb_chg->ops.enable(di->usb_chg, enable, vset, iset);
+}
+
+/**
+ * abx500_chargalg_update_chg_curr() - Update charger current
+ * @di:		pointer to the abx500_chargalg structure
+ * @iset:	requested charger output current
+ *
+ * The charger output current will be updated for the charger
+ * that is currently in use
+ */
+static int abx500_chargalg_update_chg_curr(struct abx500_chargalg *di,
+		int iset)
+{
+	/* Check if charger exists and update current if charging */
+	if (di->ac_chg && di->ac_chg->ops.update_curr &&
+			di->chg_info.charger_type & AC_CHG) {
+		/*
+		 * Select maximum of what both the charger
+		 * and the battery supports
+		 */
+		if (di->ac_chg->max_out_curr)
+			iset = min(iset, di->ac_chg->max_out_curr);
+
+		di->chg_info.ac_iset = iset;
+
+		return di->ac_chg->ops.update_curr(di->ac_chg, iset);
+	} else if (di->usb_chg && di->usb_chg->ops.update_curr &&
+			di->chg_info.charger_type & USB_CHG) {
+		/*
+		 * Select maximum of what both the charger
+		 * and the battery supports
+		 */
+		if (di->usb_chg->max_out_curr)
+			iset = min(iset, di->usb_chg->max_out_curr);
+
+		di->chg_info.usb_iset = iset;
+
+		return di->usb_chg->ops.update_curr(di->usb_chg, iset);
+	}
+
+	return -ENXIO;
+}
+
+/**
+ * abx500_chargalg_stop_charging() - Stop charging
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * This function is called from any state where charging should be stopped.
+ * All charging is disabled and all status parameters and timers are changed
+ * accordingly
+ */
+static void abx500_chargalg_stop_charging(struct abx500_chargalg *di)
+{
+	abx500_chargalg_ac_en(di, false, 0, 0);
+	abx500_chargalg_usb_en(di, false, 0, 0);
+	abx500_chargalg_stop_safety_timer(di);
+	abx500_chargalg_stop_maintenance_timer(di);
+	di->charge_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+	di->maintenance_chg = false;
+	cancel_delayed_work(&di->chargalg_wd_work);
+	power_supply_changed(&di->chargalg_psy);
+}
+
+/**
+ * abx500_chargalg_hold_charging() - Pauses charging
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * This function is called in the case where maintenance charging has been
+ * disabled and instead a battery voltage mode is entered to check when the
+ * battery voltage has reached a certain recharge voltage
+ */
+static void abx500_chargalg_hold_charging(struct abx500_chargalg *di)
+{
+	abx500_chargalg_ac_en(di, false, 0, 0);
+	abx500_chargalg_usb_en(di, false, 0, 0);
+	abx500_chargalg_stop_safety_timer(di);
+	abx500_chargalg_stop_maintenance_timer(di);
+	di->charge_status = POWER_SUPPLY_STATUS_CHARGING;
+	di->maintenance_chg = false;
+	cancel_delayed_work(&di->chargalg_wd_work);
+	power_supply_changed(&di->chargalg_psy);
+}
+
+/**
+ * abx500_chargalg_start_charging() - Start the charger
+ * @di:		pointer to the abx500_chargalg structure
+ * @vset:	requested charger output voltage
+ * @iset:	requested charger output current
+ *
+ * A charger will be enabled depending on the requested charger type that was
+ * detected previously.
+ */
+static void abx500_chargalg_start_charging(struct abx500_chargalg *di,
+	int vset, int iset)
+{
+	switch (di->chg_info.charger_type) {
+	case AC_CHG:
+		dev_dbg(di->dev,
+			"AC parameters: Vset %d, Ich %d\n", vset, iset);
+		abx500_chargalg_usb_en(di, false, 0, 0);
+		abx500_chargalg_ac_en(di, true, vset, iset);
+		break;
+
+	case USB_CHG:
+		dev_dbg(di->dev,
+			"USB parameters: Vset %d, Ich %d\n", vset, iset);
+		abx500_chargalg_ac_en(di, false, 0, 0);
+		abx500_chargalg_usb_en(di, true, vset, iset);
+		break;
+
+	default:
+		dev_err(di->dev, "Unknown charger to charge from\n");
+		break;
+	}
+}
+
+/**
+ * abx500_chargalg_check_temp() - Check battery temperature ranges
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * The battery temperature is checked against the predefined limits and the
+ * charge state is changed accordingly
+ */
+static void abx500_chargalg_check_temp(struct abx500_chargalg *di)
+{
+	if (di->batt_data.temp > (di->bat->temp_low + di->t_hyst_norm) &&
+		di->batt_data.temp < (di->bat->temp_high - di->t_hyst_norm)) {
+		/* Temp OK! */
+		di->events.btemp_underover = false;
+		di->events.btemp_lowhigh = false;
+		di->t_hyst_norm = 0;
+		di->t_hyst_lowhigh = 0;
+	} else {
+		if (((di->batt_data.temp >= di->bat->temp_high) &&
+			(di->batt_data.temp <
+				(di->bat->temp_over - di->t_hyst_lowhigh))) ||
+			((di->batt_data.temp >
+				(di->bat->temp_under + di->t_hyst_lowhigh)) &&
+			(di->batt_data.temp <= di->bat->temp_low))) {
+			/* TEMP minor!!!!! */
+			di->events.btemp_underover = false;
+			di->events.btemp_lowhigh = true;
+			di->t_hyst_norm = di->bat->temp_hysteresis;
+			di->t_hyst_lowhigh = 0;
+		} else if (di->batt_data.temp <= di->bat->temp_under ||
+			di->batt_data.temp >= di->bat->temp_over) {
+			/* TEMP major!!!!! */
+			di->events.btemp_underover = true;
+			di->events.btemp_lowhigh = false;
+			di->t_hyst_norm = 0;
+			di->t_hyst_lowhigh = di->bat->temp_hysteresis;
+		} else {
+		/* Within hysteresis */
+		dev_dbg(di->dev, "Within hysteresis limit temp: %d "
+				"hyst_lowhigh %d, hyst normal %d\n",
+				di->batt_data.temp, di->t_hyst_lowhigh,
+				di->t_hyst_norm);
+		}
+	}
+}
+
+/**
+ * abx500_chargalg_check_charger_voltage() - Check charger voltage
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * Charger voltage is checked against maximum limit
+ */
+static void abx500_chargalg_check_charger_voltage(struct abx500_chargalg *di)
+{
+	if (di->chg_info.usb_volt > di->bat->chg_params->usb_volt_max)
+		di->chg_info.usb_chg_ok = false;
+	else
+		di->chg_info.usb_chg_ok = true;
+
+	if (di->chg_info.ac_volt > di->bat->chg_params->ac_volt_max)
+		di->chg_info.ac_chg_ok = false;
+	else
+		di->chg_info.ac_chg_ok = true;
+
+}
+
+/**
+ * abx500_chargalg_end_of_charge() - Check if end-of-charge criteria is fulfilled
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * End-of-charge criteria is fulfilled when the battery voltage is above a
+ * certain limit and the battery current is below a certain limit for a
+ * predefined number of consecutive seconds. If true, the battery is full
+ */
+static void abx500_chargalg_end_of_charge(struct abx500_chargalg *di)
+{
+	if (di->charge_status == POWER_SUPPLY_STATUS_CHARGING &&
+		di->charge_state == STATE_NORMAL &&
+		!di->maintenance_chg && (di->batt_data.volt >=
+		di->bat->bat_type[di->bat->batt_id].termination_vol ||
+		di->events.usb_cv_active || di->events.ac_cv_active) &&
+		di->batt_data.avg_curr <
+		di->bat->bat_type[di->bat->batt_id].termination_curr &&
+		di->batt_data.avg_curr > 0) {
+		if (++di->eoc_cnt >= EOC_COND_CNT) {
+			di->eoc_cnt = 0;
+			di->charge_status = POWER_SUPPLY_STATUS_FULL;
+			di->maintenance_chg = true;
+			dev_dbg(di->dev, "EOC reached!\n");
+			power_supply_changed(&di->chargalg_psy);
+		} else {
+			dev_dbg(di->dev,
+				" EOC limit reached for the %d"
+				" time, out of %d before EOC\n",
+				di->eoc_cnt,
+				EOC_COND_CNT);
+		}
+	} else {
+		di->eoc_cnt = 0;
+	}
+}
+
+static void init_maxim_chg_curr(struct abx500_chargalg *di)
+{
+	di->ccm.original_iset =
+		di->bat->bat_type[di->bat->batt_id].normal_cur_lvl;
+	di->ccm.current_iset =
+		di->bat->bat_type[di->bat->batt_id].normal_cur_lvl;
+	di->ccm.test_delta_i = di->bat->maxi->charger_curr_step;
+	di->ccm.max_current = di->bat->maxi->chg_curr;
+	di->ccm.condition_cnt = di->bat->maxi->wait_cycles;
+	di->ccm.level = 0;
+}
+
+/**
+ * abx500_chargalg_chg_curr_maxim - increases the charger current to
+ *			compensate for the system load
+ * @di		pointer to the abx500_chargalg structure
+ *
+ * This maximization function is used to raise the charger current to get the
+ * battery current as close to the optimal value as possible. The battery
+ * current during charging is affected by the system load
+ */
+static enum maxim_ret abx500_chargalg_chg_curr_maxim(struct abx500_chargalg *di)
+{
+	int delta_i;
+
+	if (!di->bat->maxi->ena_maxi)
+		return MAXIM_RET_NOACTION;
+
+	delta_i = di->ccm.original_iset - di->batt_data.inst_curr;
+
+	if (di->events.vbus_collapsed) {
+		dev_dbg(di->dev, "Charger voltage has collapsed %d\n",
+				di->ccm.wait_cnt);
+		if (di->ccm.wait_cnt == 0) {
+			dev_dbg(di->dev, "lowering current\n");
+			di->ccm.wait_cnt++;
+			di->ccm.condition_cnt = di->bat->maxi->wait_cycles;
+			di->ccm.max_current =
+				di->ccm.current_iset - di->ccm.test_delta_i;
+			di->ccm.current_iset = di->ccm.max_current;
+			di->ccm.level--;
+			return MAXIM_RET_CHANGE;
+		} else {
+			dev_dbg(di->dev, "waiting\n");
+			/* Let's go in here twice before lowering curr again */
+			di->ccm.wait_cnt = (di->ccm.wait_cnt + 1) % 3;
+			return MAXIM_RET_NOACTION;
+		}
+	}
+
+	di->ccm.wait_cnt = 0;
+
+	if ((di->batt_data.inst_curr > di->ccm.original_iset)) {
+		dev_dbg(di->dev, " Maximization Ibat (%dmA) too high"
+			" (limit %dmA) (current iset: %dmA)!\n",
+			di->batt_data.inst_curr, di->ccm.original_iset,
+			di->ccm.current_iset);
+
+		if (di->ccm.current_iset == di->ccm.original_iset)
+			return MAXIM_RET_NOACTION;
+
+		di->ccm.condition_cnt = di->bat->maxi->wait_cycles;
+		di->ccm.current_iset = di->ccm.original_iset;
+		di->ccm.level = 0;
+
+		return MAXIM_RET_IBAT_TOO_HIGH;
+	}
+
+	if (delta_i > di->ccm.test_delta_i &&
+		(di->ccm.current_iset + di->ccm.test_delta_i) <
+		di->ccm.max_current) {
+		if (di->ccm.condition_cnt-- == 0) {
+			/* Increse the iset with cco.test_delta_i */
+			di->ccm.condition_cnt = di->bat->maxi->wait_cycles;
+			di->ccm.current_iset += di->ccm.test_delta_i;
+			di->ccm.level++;
+			dev_dbg(di->dev, " Maximization needed, increase"
+				" with %d mA to %dmA (Optimal ibat: %d)"
+				" Level %d\n",
+				di->ccm.test_delta_i,
+				di->ccm.current_iset,
+				di->ccm.original_iset,
+				di->ccm.level);
+			return MAXIM_RET_CHANGE;
+		} else {
+			return MAXIM_RET_NOACTION;
+		}
+	}  else {
+		di->ccm.condition_cnt = di->bat->maxi->wait_cycles;
+		return MAXIM_RET_NOACTION;
+	}
+}
+
+static void handle_maxim_chg_curr(struct abx500_chargalg *di)
+{
+	enum maxim_ret ret;
+	int result;
+
+	ret = abx500_chargalg_chg_curr_maxim(di);
+	switch (ret) {
+	case MAXIM_RET_CHANGE:
+		result = abx500_chargalg_update_chg_curr(di,
+			di->ccm.current_iset);
+		if (result)
+			dev_err(di->dev, "failed to set chg curr\n");
+		break;
+	case MAXIM_RET_IBAT_TOO_HIGH:
+		result = abx500_chargalg_update_chg_curr(di,
+			di->bat->bat_type[di->bat->batt_id].normal_cur_lvl);
+		if (result)
+			dev_err(di->dev, "failed to set chg curr\n");
+		break;
+
+	case MAXIM_RET_NOACTION:
+	default:
+		/* Do nothing..*/
+		break;
+	}
+}
+
+static int abx500_chargalg_get_ext_psy_data(struct device *dev, void *data)
+{
+	struct power_supply *psy;
+	struct power_supply *ext;
+	struct abx500_chargalg *di;
+	union power_supply_propval ret;
+	int i, j;
+	bool psy_found = false;
+
+	psy = (struct power_supply *)data;
+	ext = dev_get_drvdata(dev);
+	di = to_abx500_chargalg_device_info(psy);
+	/* For all psy where the driver name appears in any supplied_to */
+	for (i = 0; i < ext->num_supplicants; i++) {
+		if (!strcmp(ext->supplied_to[i], psy->name))
+			psy_found = true;
+	}
+	if (!psy_found)
+		return 0;
+
+	/* Go through all properties for the psy */
+	for (j = 0; j < ext->num_properties; j++) {
+		enum power_supply_property prop;
+		prop = ext->properties[j];
+
+		/* Initialize chargers if not already done */
+		if (!di->ac_chg &&
+			ext->type == POWER_SUPPLY_TYPE_MAINS)
+			di->ac_chg = psy_to_ux500_charger(ext);
+		else if (!di->usb_chg &&
+			ext->type == POWER_SUPPLY_TYPE_USB)
+			di->usb_chg = psy_to_ux500_charger(ext);
+
+		if (ext->get_property(ext, prop, &ret))
+			continue;
+		switch (prop) {
+		case POWER_SUPPLY_PROP_PRESENT:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				/* Battery present */
+				if (ret.intval)
+					di->events.batt_rem = false;
+				/* Battery removed */
+				else
+					di->events.batt_rem = true;
+				break;
+			case POWER_SUPPLY_TYPE_MAINS:
+				/* AC disconnected */
+				if (!ret.intval &&
+					(di->chg_info.conn_chg & AC_CHG)) {
+					di->chg_info.prev_conn_chg =
+						di->chg_info.conn_chg;
+					di->chg_info.conn_chg &= ~AC_CHG;
+				}
+				/* AC connected */
+				else if (ret.intval &&
+					!(di->chg_info.conn_chg & AC_CHG)) {
+					di->chg_info.prev_conn_chg =
+						di->chg_info.conn_chg;
+					di->chg_info.conn_chg |= AC_CHG;
+				}
+				break;
+			case POWER_SUPPLY_TYPE_USB:
+				/* USB disconnected */
+				if (!ret.intval &&
+					(di->chg_info.conn_chg & USB_CHG)) {
+					di->chg_info.prev_conn_chg =
+						di->chg_info.conn_chg;
+					di->chg_info.conn_chg &= ~USB_CHG;
+				}
+				/* USB connected */
+				else if (ret.intval &&
+					!(di->chg_info.conn_chg & USB_CHG)) {
+					di->chg_info.prev_conn_chg =
+						di->chg_info.conn_chg;
+					di->chg_info.conn_chg |= USB_CHG;
+				}
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_ONLINE:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				break;
+			case POWER_SUPPLY_TYPE_MAINS:
+				/* AC offline */
+				if (!ret.intval &&
+					(di->chg_info.online_chg & AC_CHG)) {
+					di->chg_info.prev_online_chg =
+						di->chg_info.online_chg;
+					di->chg_info.online_chg &= ~AC_CHG;
+				}
+				/* AC online */
+				else if (ret.intval &&
+					!(di->chg_info.online_chg & AC_CHG)) {
+					di->chg_info.prev_online_chg =
+						di->chg_info.online_chg;
+					di->chg_info.online_chg |= AC_CHG;
+					queue_delayed_work(di->chargalg_wq,
+						&di->chargalg_wd_work, 0);
+				}
+				break;
+			case POWER_SUPPLY_TYPE_USB:
+				/* USB offline */
+				if (!ret.intval &&
+					(di->chg_info.online_chg & USB_CHG)) {
+					di->chg_info.prev_online_chg =
+						di->chg_info.online_chg;
+					di->chg_info.online_chg &= ~USB_CHG;
+				}
+				/* USB online */
+				else if (ret.intval &&
+					!(di->chg_info.online_chg & USB_CHG)) {
+					di->chg_info.prev_online_chg =
+						di->chg_info.online_chg;
+					di->chg_info.online_chg |= USB_CHG;
+					queue_delayed_work(di->chargalg_wq,
+						&di->chargalg_wd_work, 0);
+				}
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_HEALTH:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				break;
+			case POWER_SUPPLY_TYPE_MAINS:
+				switch (ret.intval) {
+				case POWER_SUPPLY_HEALTH_UNSPEC_FAILURE:
+					di->events.mainextchnotok = true;
+					di->events.main_thermal_prot = false;
+					di->events.main_ovv = false;
+					di->events.ac_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_DEAD:
+					di->events.ac_wd_expired = true;
+					di->events.mainextchnotok = false;
+					di->events.main_ovv = false;
+					di->events.main_thermal_prot = false;
+					break;
+				case POWER_SUPPLY_HEALTH_COLD:
+				case POWER_SUPPLY_HEALTH_OVERHEAT:
+					di->events.main_thermal_prot = true;
+					di->events.mainextchnotok = false;
+					di->events.main_ovv = false;
+					di->events.ac_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_OVERVOLTAGE:
+					di->events.main_ovv = true;
+					di->events.mainextchnotok = false;
+					di->events.main_thermal_prot = false;
+					di->events.ac_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_GOOD:
+					di->events.main_thermal_prot = false;
+					di->events.mainextchnotok = false;
+					di->events.main_ovv = false;
+					di->events.ac_wd_expired = false;
+					break;
+				default:
+					break;
+				}
+				break;
+
+			case POWER_SUPPLY_TYPE_USB:
+				switch (ret.intval) {
+				case POWER_SUPPLY_HEALTH_UNSPEC_FAILURE:
+					di->events.usbchargernotok = true;
+					di->events.usb_thermal_prot = false;
+					di->events.vbus_ovv = false;
+					di->events.usb_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_DEAD:
+					di->events.usb_wd_expired = true;
+					di->events.usbchargernotok = false;
+					di->events.usb_thermal_prot = false;
+					di->events.vbus_ovv = false;
+					break;
+				case POWER_SUPPLY_HEALTH_COLD:
+				case POWER_SUPPLY_HEALTH_OVERHEAT:
+					di->events.usb_thermal_prot = true;
+					di->events.usbchargernotok = false;
+					di->events.vbus_ovv = false;
+					di->events.usb_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_OVERVOLTAGE:
+					di->events.vbus_ovv = true;
+					di->events.usbchargernotok = false;
+					di->events.usb_thermal_prot = false;
+					di->events.usb_wd_expired = false;
+					break;
+				case POWER_SUPPLY_HEALTH_GOOD:
+					di->events.usbchargernotok = false;
+					di->events.usb_thermal_prot = false;
+					di->events.vbus_ovv = false;
+					di->events.usb_wd_expired = false;
+					break;
+				default:
+					break;
+				}
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				di->batt_data.volt = ret.intval / 1000;
+				break;
+			case POWER_SUPPLY_TYPE_MAINS:
+				di->chg_info.ac_volt = ret.intval / 1000;
+				break;
+			case POWER_SUPPLY_TYPE_USB:
+				di->chg_info.usb_volt = ret.intval / 1000;
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_VOLTAGE_AVG:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_MAINS:
+				/* AVG is used to indicate when we are
+				 * in CV mode */
+				if (ret.intval)
+					di->events.ac_cv_active = true;
+				else
+					di->events.ac_cv_active = false;
+
+				break;
+			case POWER_SUPPLY_TYPE_USB:
+				/* AVG is used to indicate when we are
+				 * in CV mode */
+				if (ret.intval)
+					di->events.usb_cv_active = true;
+				else
+					di->events.usb_cv_active = false;
+
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_TECHNOLOGY:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				if (ret.intval)
+					di->events.batt_unknown = false;
+				else
+					di->events.batt_unknown = true;
+
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_TEMP:
+			di->batt_data.temp = ret.intval / 10;
+			break;
+
+		case POWER_SUPPLY_PROP_CURRENT_NOW:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_MAINS:
+					di->chg_info.ac_curr =
+						ret.intval / 1000;
+					break;
+			case POWER_SUPPLY_TYPE_USB:
+					di->chg_info.usb_curr =
+						ret.intval / 1000;
+				break;
+			case POWER_SUPPLY_TYPE_BATTERY:
+				di->batt_data.inst_curr = ret.intval / 1000;
+				break;
+			default:
+				break;
+			}
+			break;
+
+		case POWER_SUPPLY_PROP_CURRENT_AVG:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				di->batt_data.avg_curr = ret.intval / 1000;
+				break;
+			case POWER_SUPPLY_TYPE_USB:
+				if (ret.intval)
+					di->events.vbus_collapsed = true;
+				else
+					di->events.vbus_collapsed = false;
+				break;
+			default:
+				break;
+			}
+			break;
+		case POWER_SUPPLY_PROP_CAPACITY:
+			di->batt_data.percent = ret.intval;
+			break;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+/**
+ * abx500_chargalg_external_power_changed() - callback for power supply changes
+ * @psy:       pointer to the structure power_supply
+ *
+ * This function is the entry point of the pointer external_power_changed
+ * of the structure power_supply.
+ * This function gets executed when there is a change in any external power
+ * supply that this driver needs to be notified of.
+ */
+static void abx500_chargalg_external_power_changed(struct power_supply *psy)
+{
+	struct abx500_chargalg *di = to_abx500_chargalg_device_info(psy);
+
+	/*
+	 * Trigger execution of the algorithm instantly and read
+	 * all power_supply properties there instead
+	 */
+	queue_work(di->chargalg_wq, &di->chargalg_work);
+}
+
+/**
+ * abx500_chargalg_algorithm() - Main function for the algorithm
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * This is the main control function for the charging algorithm.
+ * It is called periodically or when something happens that will
+ * trigger a state change
+ */
+static void abx500_chargalg_algorithm(struct abx500_chargalg *di)
+{
+	int charger_status;
+
+	/* Collect data from all power_supply class devices */
+	class_for_each_device(power_supply_class, NULL,
+		&di->chargalg_psy, abx500_chargalg_get_ext_psy_data);
+
+	abx500_chargalg_end_of_charge(di);
+	abx500_chargalg_check_temp(di);
+	abx500_chargalg_check_charger_voltage(di);
+
+	charger_status = abx500_chargalg_check_charger_connection(di);
+	/*
+	 * First check if we have a charger connected.
+	 * Also we don't allow charging of unknown batteries if configured
+	 * this way
+	 */
+	if (!charger_status ||
+		(di->events.batt_unknown && !di->bat->chg_unknown_bat)) {
+		if (di->charge_state != STATE_HANDHELD) {
+			di->events.safety_timer_expired = false;
+			abx500_chargalg_state_to(di, STATE_HANDHELD_INIT);
+		}
+	}
+
+	/* If suspended, we should not continue checking the flags */
+	else if (di->charge_state == STATE_SUSPENDED_INIT ||
+		di->charge_state == STATE_SUSPENDED) {
+		/* We don't do anything here, just don,t continue */
+	}
+
+	/* Safety timer expiration */
+	else if (di->events.safety_timer_expired) {
+		if (di->charge_state != STATE_SAFETY_TIMER_EXPIRED)
+			abx500_chargalg_state_to(di,
+				STATE_SAFETY_TIMER_EXPIRED_INIT);
+	}
+	/*
+	 * Check if any interrupts has occured
+	 * that will prevent us from charging
+	 */
+
+	/* Battery removed */
+	else if (di->events.batt_rem) {
+		if (di->charge_state != STATE_BATT_REMOVED)
+			abx500_chargalg_state_to(di, STATE_BATT_REMOVED_INIT);
+	}
+	/* Main or USB charger not ok. */
+	else if (di->events.mainextchnotok || di->events.usbchargernotok) {
+		/*
+		 * If vbus_collapsed is set, we have to lower the charger
+		 * current, which is done in the normal state below
+		 */
+		if (di->charge_state != STATE_CHG_NOT_OK &&
+				!di->events.vbus_collapsed)
+			abx500_chargalg_state_to(di, STATE_CHG_NOT_OK_INIT);
+	}
+	/* VBUS, Main or VBAT OVV. */
+	else if (di->events.vbus_ovv ||
+			di->events.main_ovv ||
+			di->events.batt_ovv ||
+			!di->chg_info.usb_chg_ok ||
+			!di->chg_info.ac_chg_ok) {
+		if (di->charge_state != STATE_OVV_PROTECT)
+			abx500_chargalg_state_to(di, STATE_OVV_PROTECT_INIT);
+	}
+	/* USB Thermal, stop charging */
+	else if (di->events.main_thermal_prot ||
+		di->events.usb_thermal_prot) {
+		if (di->charge_state != STATE_HW_TEMP_PROTECT)
+			abx500_chargalg_state_to(di,
+				STATE_HW_TEMP_PROTECT_INIT);
+	}
+	/* Battery temp over/under */
+	else if (di->events.btemp_underover) {
+		if (di->charge_state != STATE_TEMP_UNDEROVER)
+			abx500_chargalg_state_to(di,
+				STATE_TEMP_UNDEROVER_INIT);
+	}
+	/* Watchdog expired */
+	else if (di->events.ac_wd_expired ||
+		di->events.usb_wd_expired) {
+		if (di->charge_state != STATE_WD_EXPIRED)
+			abx500_chargalg_state_to(di, STATE_WD_EXPIRED_INIT);
+	}
+	/* Battery temp high/low */
+	else if (di->events.btemp_lowhigh) {
+		if (di->charge_state != STATE_TEMP_LOWHIGH)
+			abx500_chargalg_state_to(di, STATE_TEMP_LOWHIGH_INIT);
+	}
+
+	dev_dbg(di->dev,
+		"[CHARGALG] Vb %d Ib_avg %d Ib_inst %d Tb %d Cap %d Maint %d "
+		"State %s Active_chg %d Chg_status %d AC %d USB %d "
+		"AC_online %d USB_online %d AC_CV %d USB_CV %d AC_I %d "
+		"USB_I %d AC_Vset %d AC_Iset %d USB_Vset %d USB_Iset %d\n",
+		di->batt_data.volt,
+		di->batt_data.avg_curr,
+		di->batt_data.inst_curr,
+		di->batt_data.temp,
+		di->batt_data.percent,
+		di->maintenance_chg,
+		states[di->charge_state],
+		di->chg_info.charger_type,
+		di->charge_status,
+		di->chg_info.conn_chg & AC_CHG,
+		di->chg_info.conn_chg & USB_CHG,
+		di->chg_info.online_chg & AC_CHG,
+		di->chg_info.online_chg & USB_CHG,
+		di->events.ac_cv_active,
+		di->events.usb_cv_active,
+		di->chg_info.ac_curr,
+		di->chg_info.usb_curr,
+		di->chg_info.ac_vset,
+		di->chg_info.ac_iset,
+		di->chg_info.usb_vset,
+		di->chg_info.usb_iset);
+
+	switch (di->charge_state) {
+	case STATE_HANDHELD_INIT:
+		abx500_chargalg_stop_charging(di);
+		di->charge_status = POWER_SUPPLY_STATUS_DISCHARGING;
+		abx500_chargalg_state_to(di, STATE_HANDHELD);
+		/* Intentional fallthrough */
+
+	case STATE_HANDHELD:
+		break;
+
+	case STATE_SUSPENDED_INIT:
+		if (di->susp_status.ac_suspended)
+			abx500_chargalg_ac_en(di, false, 0, 0);
+		if (di->susp_status.usb_suspended)
+			abx500_chargalg_usb_en(di, false, 0, 0);
+		abx500_chargalg_stop_safety_timer(di);
+		abx500_chargalg_stop_maintenance_timer(di);
+		di->charge_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+		di->maintenance_chg = false;
+		abx500_chargalg_state_to(di, STATE_SUSPENDED);
+		power_supply_changed(&di->chargalg_psy);
+		/* Intentional fallthrough */
+
+	case STATE_SUSPENDED:
+		/* CHARGING is suspended */
+		break;
+
+	case STATE_BATT_REMOVED_INIT:
+		abx500_chargalg_stop_charging(di);
+		abx500_chargalg_state_to(di, STATE_BATT_REMOVED);
+		/* Intentional fallthrough */
+
+	case STATE_BATT_REMOVED:
+		if (!di->events.batt_rem)
+			abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_HW_TEMP_PROTECT_INIT:
+		abx500_chargalg_stop_charging(di);
+		abx500_chargalg_state_to(di, STATE_HW_TEMP_PROTECT);
+		/* Intentional fallthrough */
+
+	case STATE_HW_TEMP_PROTECT:
+		if (!di->events.main_thermal_prot &&
+				!di->events.usb_thermal_prot)
+			abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_OVV_PROTECT_INIT:
+		abx500_chargalg_stop_charging(di);
+		abx500_chargalg_state_to(di, STATE_OVV_PROTECT);
+		/* Intentional fallthrough */
+
+	case STATE_OVV_PROTECT:
+		if (!di->events.vbus_ovv &&
+				!di->events.main_ovv &&
+				!di->events.batt_ovv &&
+				di->chg_info.usb_chg_ok &&
+				di->chg_info.ac_chg_ok)
+			abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_CHG_NOT_OK_INIT:
+		abx500_chargalg_stop_charging(di);
+		abx500_chargalg_state_to(di, STATE_CHG_NOT_OK);
+		/* Intentional fallthrough */
+
+	case STATE_CHG_NOT_OK:
+		if (!di->events.mainextchnotok &&
+				!di->events.usbchargernotok)
+			abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_SAFETY_TIMER_EXPIRED_INIT:
+		abx500_chargalg_stop_charging(di);
+		abx500_chargalg_state_to(di, STATE_SAFETY_TIMER_EXPIRED);
+		/* Intentional fallthrough */
+
+	case STATE_SAFETY_TIMER_EXPIRED:
+		/* We exit this state when charger is removed */
+		break;
+
+	case STATE_NORMAL_INIT:
+		abx500_chargalg_start_charging(di,
+			di->bat->bat_type[di->bat->batt_id].normal_vol_lvl,
+			di->bat->bat_type[di->bat->batt_id].normal_cur_lvl);
+		abx500_chargalg_state_to(di, STATE_NORMAL);
+		abx500_chargalg_start_safety_timer(di);
+		abx500_chargalg_stop_maintenance_timer(di);
+		init_maxim_chg_curr(di);
+		di->charge_status = POWER_SUPPLY_STATUS_CHARGING;
+		di->eoc_cnt = 0;
+		di->maintenance_chg = false;
+		power_supply_changed(&di->chargalg_psy);
+
+		break;
+
+	case STATE_NORMAL:
+		handle_maxim_chg_curr(di);
+		if (di->charge_status == POWER_SUPPLY_STATUS_FULL &&
+			di->maintenance_chg) {
+			if (di->bat->no_maintenance)
+				abx500_chargalg_state_to(di,
+					STATE_WAIT_FOR_RECHARGE_INIT);
+			else
+				abx500_chargalg_state_to(di,
+					STATE_MAINTENANCE_A_INIT);
+		}
+		break;
+
+	/* This state will be used when the maintenance state is disabled */
+	case STATE_WAIT_FOR_RECHARGE_INIT:
+		abx500_chargalg_hold_charging(di);
+		abx500_chargalg_state_to(di, STATE_WAIT_FOR_RECHARGE);
+		di->rch_cnt = RCH_COND_CNT;
+		/* Intentional fallthrough */
+
+	case STATE_WAIT_FOR_RECHARGE:
+		if (di->batt_data.volt <=
+			di->bat->bat_type[di->bat->batt_id].recharge_vol) {
+			if (di->rch_cnt-- == 0)
+				abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		} else
+			di->rch_cnt = RCH_COND_CNT;
+		break;
+
+	case STATE_MAINTENANCE_A_INIT:
+		abx500_chargalg_stop_safety_timer(di);
+		abx500_chargalg_start_maintenance_timer(di,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_a_chg_timer_h);
+		abx500_chargalg_start_charging(di,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_a_vol_lvl,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_a_cur_lvl);
+		abx500_chargalg_state_to(di, STATE_MAINTENANCE_A);
+		power_supply_changed(&di->chargalg_psy);
+		/* Intentional fallthrough*/
+
+	case STATE_MAINTENANCE_A:
+		if (di->events.maintenance_timer_expired) {
+			abx500_chargalg_stop_maintenance_timer(di);
+			abx500_chargalg_state_to(di, STATE_MAINTENANCE_B_INIT);
+		}
+		break;
+
+	case STATE_MAINTENANCE_B_INIT:
+		abx500_chargalg_start_maintenance_timer(di,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_b_chg_timer_h);
+		abx500_chargalg_start_charging(di,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_b_vol_lvl,
+			di->bat->bat_type[
+				di->bat->batt_id].maint_b_cur_lvl);
+		abx500_chargalg_state_to(di, STATE_MAINTENANCE_B);
+		power_supply_changed(&di->chargalg_psy);
+		/* Intentional fallthrough*/
+
+	case STATE_MAINTENANCE_B:
+		if (di->events.maintenance_timer_expired) {
+			abx500_chargalg_stop_maintenance_timer(di);
+			abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		}
+		break;
+
+	case STATE_TEMP_LOWHIGH_INIT:
+		abx500_chargalg_start_charging(di,
+			di->bat->bat_type[
+				di->bat->batt_id].low_high_vol_lvl,
+			di->bat->bat_type[
+				di->bat->batt_id].low_high_cur_lvl);
+		abx500_chargalg_stop_maintenance_timer(di);
+		di->charge_status = POWER_SUPPLY_STATUS_CHARGING;
+		abx500_chargalg_state_to(di, STATE_TEMP_LOWHIGH);
+		power_supply_changed(&di->chargalg_psy);
+		/* Intentional fallthrough */
+
+	case STATE_TEMP_LOWHIGH:
+		if (!di->events.btemp_lowhigh)
+			abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_WD_EXPIRED_INIT:
+		abx500_chargalg_stop_charging(di);
+		abx500_chargalg_state_to(di, STATE_WD_EXPIRED);
+		/* Intentional fallthrough */
+
+	case STATE_WD_EXPIRED:
+		if (!di->events.ac_wd_expired &&
+				!di->events.usb_wd_expired)
+			abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
+	case STATE_TEMP_UNDEROVER_INIT:
+		abx500_chargalg_stop_charging(di);
+		abx500_chargalg_state_to(di, STATE_TEMP_UNDEROVER);
+		/* Intentional fallthrough */
+
+	case STATE_TEMP_UNDEROVER:
+		if (!di->events.btemp_underover)
+			abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+	}
+
+	/* Start charging directly if the new state is a charge state */
+	if (di->charge_state == STATE_NORMAL_INIT ||
+			di->charge_state == STATE_MAINTENANCE_A_INIT ||
+			di->charge_state == STATE_MAINTENANCE_B_INIT)
+		queue_work(di->chargalg_wq, &di->chargalg_work);
+}
+
+/**
+ * abx500_chargalg_periodic_work() - Periodic work for the algorithm
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for the charging algorithm
+ */
+static void abx500_chargalg_periodic_work(struct work_struct *work)
+{
+	struct abx500_chargalg *di = container_of(work,
+		struct abx500_chargalg, chargalg_periodic_work.work);
+
+	abx500_chargalg_algorithm(di);
+
+	/*
+	 * If a charger is connected then the battery has to be monitored
+	 * frequently, else the work can be delayed.
+	 */
+	if (di->chg_info.conn_chg)
+		queue_delayed_work(di->chargalg_wq,
+			&di->chargalg_periodic_work,
+			di->bat->interval_charging * HZ);
+	else
+		queue_delayed_work(di->chargalg_wq,
+			&di->chargalg_periodic_work,
+			di->bat->interval_not_charging * HZ);
+}
+
+/**
+ * abx500_chargalg_wd_work() - periodic work to kick the charger watchdog
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for kicking the charger watchdog
+ */
+static void abx500_chargalg_wd_work(struct work_struct *work)
+{
+	int ret;
+	struct abx500_chargalg *di = container_of(work,
+		struct abx500_chargalg, chargalg_wd_work.work);
+
+	dev_dbg(di->dev, "abx500_chargalg_wd_work\n");
+
+	ret = abx500_chargalg_kick_watchdog(di);
+	if (ret < 0)
+		dev_err(di->dev, "failed to kick watchdog\n");
+
+	queue_delayed_work(di->chargalg_wq,
+		&di->chargalg_wd_work, CHG_WD_INTERVAL);
+}
+
+/**
+ * abx500_chargalg_work() - Work to run the charging algorithm instantly
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for calling the charging algorithm
+ */
+static void abx500_chargalg_work(struct work_struct *work)
+{
+	struct abx500_chargalg *di = container_of(work,
+		struct abx500_chargalg, chargalg_work);
+
+	abx500_chargalg_algorithm(di);
+}
+
+/**
+ * abx500_chargalg_get_property() - get the chargalg properties
+ * @psy:	pointer to the power_supply structure
+ * @psp:	pointer to the power_supply_property structure
+ * @val:	pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the
+ * chargalg properties by reading the sysfs files.
+ * status:     charging/discharging/full/unknown
+ * health:     health of the battery
+ * Returns error code in case of failure else 0 on success
+ */
+static int abx500_chargalg_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct abx500_chargalg *di;
+
+	di = to_abx500_chargalg_device_info(psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		val->intval = di->charge_status;
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		if (di->events.batt_ovv) {
+			val->intval = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+		} else if (di->events.btemp_underover) {
+			if (di->batt_data.temp <= di->bat->temp_under)
+				val->intval = POWER_SUPPLY_HEALTH_COLD;
+			else
+				val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		} else {
+			val->intval = POWER_SUPPLY_HEALTH_GOOD;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* Exposure to the sysfs interface */
+
+/**
+ * abx500_chargalg_sysfs_charger() - sysfs store operations
+ * @kobj:      pointer to the struct kobject
+ * @attr:      pointer to the struct attribute
+ * @buf:       buffer that holds the parameter passed from userspace
+ * @length:    length of the parameter passed
+ *
+ * Returns length of the buffer(input taken from user space) on success
+ * else error code on failure
+ * The operation to be performed on passing the parameters from the user space.
+ */
+static ssize_t abx500_chargalg_sysfs_charger(struct kobject *kobj,
+	struct attribute *attr, const char *buf, size_t length)
+{
+	struct abx500_chargalg *di = container_of(kobj,
+		struct abx500_chargalg, chargalg_kobject);
+	long int param;
+	int ac_usb;
+	int ret;
+	char entry = *attr->name;
+
+	switch (entry) {
+	case 'c':
+		ret = strict_strtol(buf, 10, &param);
+		if (ret < 0)
+			return ret;
+
+		ac_usb = param;
+		switch (ac_usb) {
+		case 0:
+			/* Disable charging */
+			di->susp_status.ac_suspended = true;
+			di->susp_status.usb_suspended = true;
+			di->susp_status.suspended_change = true;
+			/* Trigger a state change */
+			queue_work(di->chargalg_wq,
+				&di->chargalg_work);
+			break;
+		case 1:
+			/* Enable AC Charging */
+			di->susp_status.ac_suspended = false;
+			di->susp_status.suspended_change = true;
+			/* Trigger a state change */
+			queue_work(di->chargalg_wq,
+				&di->chargalg_work);
+			break;
+		case 2:
+			/* Enable USB charging */
+			di->susp_status.usb_suspended = false;
+			di->susp_status.suspended_change = true;
+			/* Trigger a state change */
+			queue_work(di->chargalg_wq,
+				&di->chargalg_work);
+			break;
+		default:
+			dev_info(di->dev, "Wrong input\n"
+				"Enter 0. Disable AC/USB Charging\n"
+				"1. Enable AC charging\n"
+				"2. Enable USB Charging\n");
+		};
+		break;
+	};
+	return strlen(buf);
+}
+
+static struct attribute abx500_chargalg_en_charger = \
+{
+	.name = "chargalg",
+	.mode = S_IWUGO,
+};
+
+static struct attribute *abx500_chargalg_chg[] = {
+	&abx500_chargalg_en_charger,
+	NULL
+};
+
+const struct sysfs_ops abx500_chargalg_sysfs_ops = {
+	.store = abx500_chargalg_sysfs_charger,
+};
+
+static struct kobj_type abx500_chargalg_ktype = {
+	.sysfs_ops = &abx500_chargalg_sysfs_ops,
+	.default_attrs = abx500_chargalg_chg,
+};
+
+/**
+ * abx500_chargalg_sysfs_exit() - de-init of sysfs entry
+ * @di:                pointer to the struct abx500_chargalg
+ *
+ * This function removes the entry in sysfs.
+ */
+static void abx500_chargalg_sysfs_exit(struct abx500_chargalg *di)
+{
+	kobject_del(&di->chargalg_kobject);
+}
+
+/**
+ * abx500_chargalg_sysfs_init() - init of sysfs entry
+ * @di:                pointer to the struct abx500_chargalg
+ *
+ * This function adds an entry in sysfs.
+ * Returns error code in case of failure else 0(on success)
+ */
+static int abx500_chargalg_sysfs_init(struct abx500_chargalg *di)
+{
+	int ret = 0;
+
+	ret = kobject_init_and_add(&di->chargalg_kobject,
+		&abx500_chargalg_ktype,
+		NULL, "abx500_chargalg");
+	if (ret < 0)
+		dev_err(di->dev, "failed to create sysfs entry\n");
+
+	return ret;
+}
+/* Exposure to the sysfs interface <<END>> */
+
+#if defined(CONFIG_PM)
+static int abx500_chargalg_resume(struct platform_device *pdev)
+{
+	struct abx500_chargalg *di = platform_get_drvdata(pdev);
+
+	/* Kick charger watchdog if charging (any charger online) */
+	if (di->chg_info.online_chg)
+		queue_delayed_work(di->chargalg_wq, &di->chargalg_wd_work, 0);
+
+	/*
+	 * Run the charging algorithm directly to be sure we don't
+	 * do it too seldom
+	 */
+	queue_delayed_work(di->chargalg_wq, &di->chargalg_periodic_work, 0);
+
+	return 0;
+}
+
+static int abx500_chargalg_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct abx500_chargalg *di = platform_get_drvdata(pdev);
+
+	if (di->chg_info.online_chg)
+		cancel_delayed_work_sync(&di->chargalg_wd_work);
+
+	cancel_delayed_work_sync(&di->chargalg_periodic_work);
+
+	return 0;
+}
+#else
+#define abx500_chargalg_suspend      NULL
+#define abx500_chargalg_resume       NULL
+#endif
+
+static int __devexit abx500_chargalg_remove(struct platform_device *pdev)
+{
+	struct abx500_chargalg *di = platform_get_drvdata(pdev);
+
+	/* sysfs interface to enable/disbale charging from user space */
+	abx500_chargalg_sysfs_exit(di);
+
+	/* Delete the work queue */
+	destroy_workqueue(di->chargalg_wq);
+
+	flush_scheduled_work();
+	power_supply_unregister(&di->chargalg_psy);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+
+	return 0;
+}
+
+static int __devinit abx500_chargalg_probe(struct platform_device *pdev)
+{
+	struct abx500_bm_plat_data *plat_data;
+	int ret = 0;
+
+	struct abx500_chargalg *di =
+		kzalloc(sizeof(struct abx500_chargalg), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	/* get device struct */
+	di->dev = &pdev->dev;
+
+	plat_data = pdev->dev.platform_data;
+	di->pdata = plat_data->chargalg;
+	di->bat = plat_data->battery;
+
+	/* chargalg supply */
+	di->chargalg_psy.name = "abx500_chargalg";
+	di->chargalg_psy.type = POWER_SUPPLY_TYPE_BATTERY;
+	di->chargalg_psy.properties = abx500_chargalg_props;
+	di->chargalg_psy.num_properties = ARRAY_SIZE(abx500_chargalg_props);
+	di->chargalg_psy.get_property = abx500_chargalg_get_property;
+	di->chargalg_psy.supplied_to = di->pdata->supplied_to;
+	di->chargalg_psy.num_supplicants = di->pdata->num_supplicants;
+	di->chargalg_psy.external_power_changed =
+		abx500_chargalg_external_power_changed;
+
+	/* Initilialize safety timer */
+	init_timer(&di->safety_timer);
+	di->safety_timer.function = abx500_chargalg_safety_timer_expired;
+	di->safety_timer.data = (unsigned long) di;
+
+	/* Initilialize maintenance timer */
+	init_timer(&di->maintenance_timer);
+	di->maintenance_timer.function =
+		abx500_chargalg_maintenance_timer_expired;
+	di->maintenance_timer.data = (unsigned long) di;
+
+	/* Create a work queue for the chargalg */
+	di->chargalg_wq =
+		create_singlethread_workqueue("abx500_chargalg_wq");
+	if (di->chargalg_wq == NULL) {
+		dev_err(di->dev, "failed to create work queue\n");
+		goto free_device_info;
+	}
+
+	/* Init work for chargalg */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->chargalg_periodic_work,
+		abx500_chargalg_periodic_work);
+	INIT_DELAYED_WORK_DEFERRABLE(&di->chargalg_wd_work,
+		abx500_chargalg_wd_work);
+
+	/* Init work for chargalg */
+	INIT_WORK(&di->chargalg_work, abx500_chargalg_work);
+
+	/* To detect charger at startup */
+	di->chg_info.prev_conn_chg = -1;
+
+	/* Register chargalg power supply class */
+	ret = power_supply_register(di->dev, &di->chargalg_psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register chargalg psy\n");
+		goto free_chargalg_wq;
+	}
+
+	platform_set_drvdata(pdev, di);
+
+	/* sysfs interface to enable/disable charging from user space */
+	ret = abx500_chargalg_sysfs_init(di);
+	if (ret) {
+		dev_err(di->dev, "failed to create sysfs entry\n");
+		goto free_psy;
+	}
+
+	/* Run the charging algorithm */
+	queue_delayed_work(di->chargalg_wq, &di->chargalg_periodic_work, 0);
+
+	dev_info(di->dev, "probe success\n");
+	return ret;
+
+free_psy:
+	power_supply_unregister(&di->chargalg_psy);
+free_chargalg_wq:
+	destroy_workqueue(di->chargalg_wq);
+free_device_info:
+	kfree(di);
+
+	return ret;
+}
+
+static struct platform_driver abx500_chargalg_driver = {
+	.probe = abx500_chargalg_probe,
+	.remove = __devexit_p(abx500_chargalg_remove),
+	.suspend = abx500_chargalg_suspend,
+	.resume = abx500_chargalg_resume,
+	.driver = {
+		.name = "abx500-chargalg",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init abx500_chargalg_init(void)
+{
+	return platform_driver_register(&abx500_chargalg_driver);
+}
+
+static void __exit abx500_chargalg_exit(void)
+{
+	platform_driver_unregister(&abx500_chargalg_driver);
+}
+
+module_init(abx500_chargalg_init);
+module_exit(abx500_chargalg_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Johan Palsson, Karl Komierowski");
+MODULE_ALIAS("platform:abx500-chargalg");
+MODULE_DESCRIPTION("abx500 battery charging algorithm");
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index d7ed20f293d..ec1cee88683 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -267,6 +267,13 @@ config REGULATOR_AD5398
 	  This driver supports AD5398 and AD5821 current regulator chips.
 	  If building into module, its name is ad5398.ko.
 
+config REGULATOR_AB5500
+	bool "ST-Ericsson AB5500 Power Regulators"
+	depends on AB5500_CORE
+	help
+	 This driver supports the regulators found on the ST-Ericsson mixed
+	 signal AB5500 PMIC
+
 config REGULATOR_AB8500
 	bool "ST-Ericsson AB8500 Power Regulators"
 	depends on AB8500_CORE
@@ -274,13 +281,33 @@ config REGULATOR_AB8500
 	  This driver supports the regulators found on the ST-Ericsson mixed
 	  signal AB8500 PMIC
 
+config REGULATOR_DBX500_PRCMU
+	bool
+
+config REGULATOR_DB5500_PRCMU
+	bool "ST-Ericsson DB5500 Voltage Domain Regulators"
+	depends on MFD_DB5500_PRCMU
+	select REGULATOR_DBX500_PRCMU
+	help
+	  This driver supports the voltage domain regulators controlled by the
+	  DB5500 PRCMU
+
 config REGULATOR_DB8500_PRCMU
 	bool "ST-Ericsson DB8500 Voltage Domain Regulators"
 	depends on MFD_DB8500_PRCMU
+	select REGULATOR_DBX500_PRCMU
 	help
 	  This driver supports the voltage domain regulators controlled by the
 	  DB8500 PRCMU
 
+config REGULATOR_AB8500_DEBUG
+	bool "AB8500 regulator debug"
+	depends on REGULATOR_AB8500
+	help
+	  Say Y here to add debug functionality for ST-Ericsson
+	  ab8500 regulators. This is a module that exposes a
+	  number of settings and debug output in debugfs.
+
 config REGULATOR_TPS6586X
 	tristate "TI TPS6586X Power regulators"
 	depends on MFD_TPS6586X
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 3932d2ec38f..aff7d7b213e 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -40,8 +40,12 @@ obj-$(CONFIG_REGULATOR_TPS6507X) += tps6507x-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6524X) += tps6524x-regulator.o
 obj-$(CONFIG_REGULATOR_88PM8607) += 88pm8607.o
 obj-$(CONFIG_REGULATOR_ISL6271A) += isl6271a-regulator.o
+obj-$(CONFIG_REGULATOR_AB5500) += ab5500.o
 obj-$(CONFIG_REGULATOR_AB8500)	+= ab8500.o
+obj-$(CONFIG_REGULATOR_DBX500_PRCMU) += dbx500-prcmu.o
+obj-$(CONFIG_REGULATOR_DB5500_PRCMU) += db5500-prcmu.o
 obj-$(CONFIG_REGULATOR_DB8500_PRCMU) += db8500-prcmu.o
 obj-$(CONFIG_REGULATOR_TPS65910) += tps65910-regulator.o
+obj-$(CONFIG_REGULATOR_AB8500_DEBUG) += ab8500-debug.o
 
 ccflags-$(CONFIG_REGULATOR_DEBUG) += -DDEBUG
diff --git a/drivers/regulator/ab5500.c b/drivers/regulator/ab5500.c
new file mode 100644
index 00000000000..fc37c0371e1
--- /dev/null
+++ b/drivers/regulator/ab5500.c
@@ -0,0 +1,579 @@
+/*
+ * Copyright (C) 2011 ST-Ericsson SA
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Based on ab3100.c.
+ *
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson
+ * Author: Rabin Vincent <rabin.vincent@stericsson.com> for ST-Ericsson
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/mfd/abx500.h>
+#include <linux/regulator/ab5500.h>
+
+#define AB5500_LDO_VDIGMIC_ST	0x50
+
+#define AB5500_LDO_S_ST		0x66
+#define AB5500_LDO_S_PWR1	0x67
+#define AB5500_LDO_S_PWR0	0x68
+
+#define AB5500_LDO_D_ST		0x72
+#define AB5500_LDO_D_PWR1	0x73
+#define AB5500_LDO_D_PWR0	0x74
+
+#define AB5500_LDO_G_ST		0x78
+#define AB5500_LDO_G_PWR1	0x79
+#define AB5500_LDO_G_PWR0	0x7a
+
+#define AB5500_LDO_H_ST		0x7b
+#define AB5500_LDO_H_PWR1	0x7c
+#define AB5500_LDO_H_PWR0	0x7d
+
+#define AB5500_LDO_K_ST		0x7e
+#define AB5500_LDO_K_PWR1	0x7f
+#define AB5500_LDO_K_PWR0	0x80
+
+#define AB5500_LDO_L_ST		0x81
+#define AB5500_LDO_L_PWR1	0x82
+#define AB5500_LDO_L_PWR0	0x83
+
+/* In SIM bank */
+#define AB5500_SIM_SUP		0x14
+
+#define AB5500_LDO_MODE_MASK		(0x3 << 4)
+#define AB5500_LDO_MODE_FULLPOWER	(0x3 << 4)
+#define AB5500_LDO_MODE_PWRCTRL		(0x2 << 4)
+#define AB5500_LDO_MODE_LOWPOWER	(0x1 << 4)
+#define AB5500_LDO_MODE_OFF		(0x0 << 4)
+#define AB5500_LDO_VOLT_MASK		0x07
+
+struct ab5500_regulator {
+	struct regulator_desc desc;
+	const int *voltages;
+	int num_holes;
+	bool pwrctrl;
+	bool enabled;
+	int enable_time;
+	u8 bank;
+	u8 reg;
+	u8 mode;
+};
+
+struct ab5500_regulators {
+	struct device *dev;
+	struct ab5500_regulator *regulator[AB5500_NUM_REGULATORS];
+	struct regulator_dev *rdev[AB5500_NUM_REGULATORS];
+};
+
+static int ab5500_regulator_enable_time(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+
+	return r->enable_time; /* microseconds */
+}
+
+static int ab5500_regulator_enable(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	int ret;
+
+	ret = abx500_mask_and_set(ab5500->dev, r->bank, r->reg,
+				  AB5500_LDO_MODE_MASK, r->mode);
+	if (ret < 0)
+		return ret;
+
+	r->enabled = true;
+
+	return 0;
+}
+
+static int ab5500_regulator_disable(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	u8 regval = r->pwrctrl ? AB5500_LDO_MODE_PWRCTRL : AB5500_LDO_MODE_OFF;
+	int ret;
+
+	ret = abx500_mask_and_set(ab5500->dev, r->bank, r->reg,
+				  AB5500_LDO_MODE_MASK, regval);
+	if (ret < 0)
+		return ret;
+
+	r->enabled = false;
+
+	return 0;
+}
+
+static unsigned int ab5500_regulator_get_mode(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+
+	if (r->mode == AB5500_LDO_MODE_LOWPOWER)
+		return REGULATOR_MODE_IDLE;
+
+	return REGULATOR_MODE_NORMAL;
+}
+
+static int ab5500_regulator_set_mode(struct regulator_dev *rdev,
+				     unsigned int mode)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		r->mode = AB5500_LDO_MODE_FULLPOWER;
+		break;
+	case REGULATOR_MODE_IDLE:
+		r->mode = AB5500_LDO_MODE_LOWPOWER;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (r->enabled)
+		return ab5500_regulator_enable(rdev);
+
+	return 0;
+}
+
+static int ab5500_regulator_is_enabled(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	u8 regval;
+	int err;
+
+	err = abx500_get_register_interruptible(ab5500->dev,
+						r->bank, r->reg, &regval);
+	if (err) {
+		dev_err(rdev_get_dev(rdev), "unable to get register 0x%x\n",
+			r->reg);
+		return err;
+	}
+
+	switch (regval & AB5500_LDO_MODE_MASK) {
+	case AB5500_LDO_MODE_PWRCTRL:
+	case AB5500_LDO_MODE_OFF:
+		r->enabled = false;
+		break;
+	default:
+		r->enabled = true;
+		break;
+	}
+
+	return r->enabled;
+}
+
+static int
+ab5500_regulator_list_voltage(struct regulator_dev *rdev, unsigned selector)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	unsigned n_voltages = r->desc.n_voltages;
+	int selindex;
+	int i;
+
+	for (i = 0, selindex = 0; selindex < n_voltages; i++) {
+		int voltage = r->voltages[i];
+
+		if (!voltage)
+			continue;
+
+		if (selindex == selector)
+			return voltage;
+
+		selindex++;
+	}
+
+	return -EINVAL;
+}
+
+static int ab5500_regulator_fixed_get_voltage(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+
+	return r->voltages[0];
+}
+
+static int ab5500_regulator_get_voltage(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	u8 regval;
+	int ret;
+
+	ret = abx500_get_register_interruptible(ab5500->dev,
+						r->bank, r->reg, &regval);
+	if (ret) {
+		dev_warn(rdev_get_dev(rdev),
+			"failed to get regulator value in register "
+			"%02x\n", r->reg);
+		return ret;
+	}
+
+	regval &= AB5500_LDO_VOLT_MASK;
+	if (regval >= r->desc.n_voltages + r->num_holes)
+	       return -EINVAL;
+
+	if (!r->voltages[regval])
+		return -EINVAL;
+
+	return r->voltages[regval];
+}
+
+static int ab5500_get_best_voltage_index(struct ab5500_regulator *r,
+					 int min_uV, int max_uV)
+{
+	unsigned n_voltages = r->desc.n_voltages;
+	int bestmatch = INT_MAX;
+	int bestindex = -EINVAL;
+	int selindex;
+	int i;
+
+	/*
+	 * Locate the minimum voltage fitting the criteria on
+	 * this regulator. The switchable voltages are not
+	 * in strict falling order so we need to check them
+	 * all for the best match.
+	 */
+	for (i = 0, selindex = 0; selindex < n_voltages; i++) {
+		int voltage = r->voltages[i];
+
+		if (!voltage)
+			continue;
+
+		if (voltage <= max_uV &&
+		    voltage >= min_uV &&
+		    voltage < bestmatch) {
+			bestmatch = voltage;
+			bestindex = i;
+		}
+
+		selindex++;
+	}
+
+	return bestindex;
+}
+
+static int ab5500_regulator_set_voltage(struct regulator_dev *rdev,
+					int min_uV, int max_uV,
+					unsigned *selector)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	int bestindex;
+
+	bestindex = ab5500_get_best_voltage_index(r, min_uV, max_uV);
+	if (bestindex < 0) {
+		dev_warn(rdev_get_dev(rdev),
+			 "requested %d<=x<=%d uV, out of range!\n",
+			 min_uV, max_uV);
+		return bestindex;
+	}
+
+	*selector = bestindex;
+
+	return abx500_mask_and_set_register_interruptible(ab5500->dev,
+			r->bank, r->reg, AB5500_LDO_VOLT_MASK, bestindex);
+
+}
+
+static struct regulator_ops ab5500_regulator_variable_ops = {
+	.enable		= ab5500_regulator_enable,
+	.disable	= ab5500_regulator_disable,
+	.is_enabled	= ab5500_regulator_is_enabled,
+	.enable_time	= ab5500_regulator_enable_time,
+	.get_voltage	= ab5500_regulator_get_voltage,
+	.set_voltage	= ab5500_regulator_set_voltage,
+	.list_voltage	= ab5500_regulator_list_voltage,
+	.set_mode	= ab5500_regulator_set_mode,
+	.get_mode	= ab5500_regulator_get_mode,
+};
+
+static struct regulator_ops ab5500_regulator_fixed_ops = {
+	.enable		= ab5500_regulator_enable,
+	.disable	= ab5500_regulator_disable,
+	.is_enabled	= ab5500_regulator_is_enabled,
+	.enable_time	= ab5500_regulator_enable_time,
+	.get_voltage	= ab5500_regulator_fixed_get_voltage,
+	.list_voltage	= ab5500_regulator_list_voltage,
+	.set_mode	= ab5500_regulator_set_mode,
+	.get_mode	= ab5500_regulator_get_mode,
+};
+
+static const int ab5500_ldo_s_voltages[] = {
+	[0x00] = 1200000,
+	[0x01] = 1050000,
+	[0x02] = 1100000,
+	[0x03] = 1500000,
+	[0x04] = 1800000,
+	[0x05] = 2200000,
+	[0x06] = 2500000,
+	[0x07] = 2790000,
+};
+
+static const int ab5500_ldo_d_voltages[] = {
+	[0x00] = 2100000,
+};
+
+static const int ab5500_ldo_lg_voltages[] = {
+	[0x00] = 1200000,
+	[0x01] = 0, /* not used */
+	[0x02] = 1500000,
+	[0x03] = 1800000,
+	[0x04] = 0, /* not used */
+	[0x05] = 2500000,
+	[0x06] = 2730000,
+	[0x07] = 2910000,
+};
+
+static const int ab5500_ldo_kh_voltages[] = {
+	[0x00] = 1200000,
+	[0x01] = 1500000,
+	[0x02] = 1800000,
+	[0x03] = 2100000,
+	[0x04] = 2500000,
+	[0x05] = 2750000,
+	[0x06] = 2790000,
+	[0x07] = 2910000,
+};
+
+static const int ab5500_ldo_vdigmic_voltages[] = {
+	[0x00] = 2100000,
+};
+
+static const int ab5500_ldo_sim_voltages[] = {
+	[0x00] = 1875000,
+	[0x01] = 2800000,
+	[0x02] = 2900000,
+};
+
+static struct ab5500_regulator ab5500_regulators[] = {
+	[AB5500_LDO_S] = {
+		.desc = {
+			.name	= "LDO_S",
+			.id	= AB5500_LDO_S,
+			.ops	= &ab5500_regulator_variable_ops,
+			.type	= REGULATOR_VOLTAGE,
+			.owner	= THIS_MODULE,
+			.n_voltages = ARRAY_SIZE(ab5500_ldo_s_voltages),
+		},
+		.bank		= AB5500_BANK_STARTUP,
+		.reg		= AB5500_LDO_S_ST,
+		.voltages	= ab5500_ldo_s_voltages,
+		.enable_time	= 400,
+		.pwrctrl	= true,
+		.mode		= AB5500_LDO_MODE_FULLPOWER,
+	},
+	[AB5500_LDO_D] = {
+		.desc = {
+			.name	= "LDO_D",
+			.id	= AB5500_LDO_D,
+			.ops	= &ab5500_regulator_fixed_ops,
+			.type	= REGULATOR_VOLTAGE,
+			.owner	= THIS_MODULE,
+			.n_voltages = ARRAY_SIZE(ab5500_ldo_d_voltages),
+		},
+		.bank		= AB5500_BANK_STARTUP,
+		.reg		= AB5500_LDO_D_ST,
+		.voltages	= ab5500_ldo_d_voltages,
+		.enable_time	= 400,
+		.pwrctrl	= true,
+		.mode		= AB5500_LDO_MODE_FULLPOWER,
+	},
+	[AB5500_LDO_L] = {
+		.desc = {
+			.name	= "LDO_L",
+			.id	= AB5500_LDO_L,
+			.ops	= &ab5500_regulator_variable_ops,
+			.type	= REGULATOR_VOLTAGE,
+			.owner	= THIS_MODULE,
+			.n_voltages = ARRAY_SIZE(ab5500_ldo_lg_voltages) - 2,
+		},
+		.bank		= AB5500_BANK_STARTUP,
+		.reg		= AB5500_LDO_L_ST,
+		.voltages	= ab5500_ldo_lg_voltages,
+		.num_holes	= 2, /* 2 register values unused */
+		.enable_time	= 400,
+		.mode		= AB5500_LDO_MODE_FULLPOWER,
+	},
+	[AB5500_LDO_G] = {
+		.desc = {
+			.name	= "LDO_G",
+			.id	= AB5500_LDO_G,
+			.ops	= &ab5500_regulator_variable_ops,
+			.type	= REGULATOR_VOLTAGE,
+			.owner	= THIS_MODULE,
+			.n_voltages = ARRAY_SIZE(ab5500_ldo_lg_voltages) - 2,
+		},
+		.bank		= AB5500_BANK_STARTUP,
+		.reg		= AB5500_LDO_G_ST,
+		.voltages	= ab5500_ldo_lg_voltages,
+		.num_holes	= 2, /* 2 register values unused */
+		.enable_time	= 400,
+		.mode		= AB5500_LDO_MODE_FULLPOWER,
+	},
+	[AB5500_LDO_K] = {
+		.desc = {
+			.name	= "LDO_K",
+			.id	= AB5500_LDO_K,
+			.ops	= &ab5500_regulator_variable_ops,
+			.type	= REGULATOR_VOLTAGE,
+			.owner	= THIS_MODULE,
+			.n_voltages = ARRAY_SIZE(ab5500_ldo_kh_voltages),
+		},
+		.bank		= AB5500_BANK_STARTUP,
+		.reg		= AB5500_LDO_K_ST,
+		.voltages	= ab5500_ldo_kh_voltages,
+		.enable_time	= 400,
+		.mode		= AB5500_LDO_MODE_FULLPOWER,
+	},
+	[AB5500_LDO_H] = {
+		.desc = {
+			.name	= "LDO_H",
+			.id	= AB5500_LDO_H,
+			.ops	= &ab5500_regulator_variable_ops,
+			.type	= REGULATOR_VOLTAGE,
+			.owner	= THIS_MODULE,
+			.n_voltages = ARRAY_SIZE(ab5500_ldo_kh_voltages),
+		},
+		.bank		= AB5500_BANK_STARTUP,
+		.reg		= AB5500_LDO_H_ST,
+		.voltages	= ab5500_ldo_kh_voltages,
+		.enable_time	= 400,
+		.mode		= AB5500_LDO_MODE_FULLPOWER,
+	},
+	[AB5500_LDO_VDIGMIC] = {
+		.desc = {
+			.name	= "LDO_VDIGMIC",
+			.id	= AB5500_LDO_VDIGMIC,
+			.ops	= &ab5500_regulator_fixed_ops,
+			.type	= REGULATOR_VOLTAGE,
+			.owner	= THIS_MODULE,
+			.n_voltages = ARRAY_SIZE(ab5500_ldo_vdigmic_voltages),
+		},
+		.bank		= AB5500_BANK_STARTUP,
+		.reg		= AB5500_LDO_VDIGMIC_ST,
+		.voltages	= ab5500_ldo_vdigmic_voltages,
+		.enable_time	= 450,
+		.mode		= AB5500_LDO_MODE_FULLPOWER,
+	},
+	[AB5500_LDO_SIM] = {
+		.desc = {
+			.name	= "LDO_SIM",
+			.id	= AB5500_LDO_SIM,
+			.ops	= &ab5500_regulator_variable_ops,
+			.type	= REGULATOR_VOLTAGE,
+			.owner	= THIS_MODULE,
+			.n_voltages = ARRAY_SIZE(ab5500_ldo_sim_voltages),
+		},
+		.bank		= AB5500_BANK_SIM_USBSIM,
+		.reg		= AB5500_SIM_SUP,
+		.voltages	= ab5500_ldo_sim_voltages,
+		.enable_time	= 1000,
+		.mode		= AB5500_LDO_MODE_FULLPOWER,
+	},
+};
+
+
+static int __devinit ab5500_regulator_probe(struct platform_device *pdev)
+{
+	struct ab5500_platform_data *ppdata = pdev->dev.parent->platform_data;
+	struct ab5500_regulator_platform_data *pdata = ppdata->regulator;
+	struct ab5500_regulators *ab5500;
+	int err = 0;
+	int i;
+
+	if (!pdata || !pdata->regulator)
+		return -EINVAL;
+
+	ab5500 = kzalloc(sizeof(*ab5500), GFP_KERNEL);
+	if (!ab5500)
+		return -ENOMEM;
+
+	ab5500->dev = &pdev->dev;
+
+	platform_set_drvdata(pdev, ab5500);
+
+	for (i = 0; i < AB5500_NUM_REGULATORS; i++) {
+		struct ab5500_regulator *regulator = &ab5500_regulators[i];
+		struct regulator_dev *rdev;
+
+		ab5500->regulator[i] = regulator;
+
+		rdev = regulator_register(&regulator->desc, &pdev->dev,
+					  &pdata->regulator[i], ab5500);
+		if (IS_ERR(rdev)) {
+			err = PTR_ERR(rdev);
+			dev_err(&pdev->dev, "failed to register regulator %s err %d\n",
+				regulator->desc.name, err);
+			goto err_unregister;
+		}
+
+		ab5500->rdev[i] = rdev;
+	}
+
+	return 0;
+
+err_unregister:
+	/* remove the already registered regulators */
+	while (--i >= 0)
+		regulator_unregister(ab5500->rdev[i]);
+
+	platform_set_drvdata(pdev, NULL);
+	kfree(ab5500);
+
+	return err;
+}
+
+static int __devexit ab5500_regulators_remove(struct platform_device *pdev)
+{
+	struct ab5500_regulators *ab5500 = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < AB5500_NUM_REGULATORS; i++)
+		regulator_unregister(ab5500->rdev[i]);
+
+	platform_set_drvdata(pdev, NULL);
+	kfree(ab5500);
+
+	return 0;
+}
+
+static struct platform_driver ab5500_regulator_driver = {
+	.driver = {
+		.name  = "ab5500-regulator",
+		.owner = THIS_MODULE,
+	},
+	.probe = ab5500_regulator_probe,
+	.remove = __devexit_p(ab5500_regulators_remove),
+};
+
+static __init int ab5500_regulator_init(void)
+{
+	return platform_driver_register(&ab5500_regulator_driver);
+}
+
+static __exit void ab5500_regulator_exit(void)
+{
+	platform_driver_unregister(&ab5500_regulator_driver);
+}
+
+subsys_initcall(ab5500_regulator_init);
+module_exit(ab5500_regulator_exit);
+
+MODULE_DESCRIPTION("AB5500 Regulator Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:ab5500-regulator");
diff --git a/drivers/regulator/ab8500-debug.c b/drivers/regulator/ab8500-debug.c
new file mode 100644
index 00000000000..4aef00be24c
--- /dev/null
+++ b/drivers/regulator/ab8500-debug.c
@@ -0,0 +1,1849 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson.
+ *
+ * License Terms: GNU General Public License v2
+ */
+
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/debugfs.h>
+#include <linux/platform_device.h>
+#include <linux/kobject.h>
+#include <linux/slab.h>
+#include <linux/mfd/abx500.h>
+#include <linux/regulator/ab8500-debug.h>
+#include <linux/io.h>
+#include <mach/db8500-regs.h> /* U8500_BACKUPRAM1_BASE */
+
+/* board profile address - to determine if suspend-force is default */
+#define BOOT_INFO_BACKUPRAM1 (U8500_BACKUPRAM1_BASE + 0xffc)
+#define BOARD_PROFILE_BACKUPRAM1 (0x3)
+
+/* board profile option */
+#define OPTION_BOARD_VERSION_V5X 50
+
+/* for error prints */
+struct device *dev;
+struct platform_device *pdev;
+
+/* setting for suspend force (disabled by default) */
+static bool setting_suspend_force;
+
+/*
+ * regulator states
+ */
+enum ab8500_regulator_state_id {
+	AB8500_REGULATOR_STATE_INIT,
+	AB8500_REGULATOR_STATE_SUSPEND,
+	AB8500_REGULATOR_STATE_SUSPEND_CORE,
+	AB8500_REGULATOR_STATE_RESUME_CORE,
+	AB8500_REGULATOR_STATE_RESUME,
+	AB8500_REGULATOR_STATE_CURRENT,
+	NUM_REGULATOR_STATE
+};
+
+static const char *regulator_state_name[NUM_REGULATOR_STATE] = {
+	[AB8500_REGULATOR_STATE_INIT] = "init",
+	[AB8500_REGULATOR_STATE_SUSPEND] = "suspend",
+	[AB8500_REGULATOR_STATE_SUSPEND_CORE] = "suspend-core",
+	[AB8500_REGULATOR_STATE_RESUME_CORE] = "resume-core",
+	[AB8500_REGULATOR_STATE_RESUME] = "resume",
+	[AB8500_REGULATOR_STATE_CURRENT] = "current",
+};
+
+/*
+ * regulator register definitions
+ */
+enum ab8500_register_id {
+	AB8500_REGU_NOUSE, /* if not defined */
+	AB8500_REGU_REQUEST_CTRL1,
+	AB8500_REGU_REQUEST_CTRL2,
+	AB8500_REGU_REQUEST_CTRL3,
+	AB8500_REGU_REQUEST_CTRL4,
+	AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+	AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+	AB8500_REGU_HW_HP_REQ1_VALID1,
+	AB8500_REGU_HW_HP_REQ1_VALID2,
+	AB8500_REGU_HW_HP_REQ2_VALID1,
+	AB8500_REGU_HW_HP_REQ2_VALID2,
+	AB8500_REGU_SW_HP_REQ_VALID1,
+	AB8500_REGU_SW_HP_REQ_VALID2,
+	AB8500_REGU_SYSCLK_REQ1_VALID,
+	AB8500_REGU_SYSCLK_REQ2_VALID,
+	AB8500_REGU_MISC1,
+	AB8500_REGU_OTG_SUPPLY_CTRL,
+	AB8500_REGU_VUSB_CTRL,
+	AB8500_REGU_VAUDIO_SUPPLY,
+	AB8500_REGU_CTRL1_VAMIC,
+	AB8500_REGU_ARM_REGU1,
+	AB8500_REGU_ARM_REGU2,
+	AB8500_REGU_VAPE_REGU,
+	AB8500_REGU_VSMPS1_REGU,
+	AB8500_REGU_VSMPS2_REGU,
+	AB8500_REGU_VSMPS3_REGU,
+	AB8500_REGU_VPLL_VANA_REGU,
+	AB8500_REGU_VREF_DDR,
+	AB8500_REGU_EXT_SUPPLY_REGU,
+	AB8500_REGU_VAUX12_REGU,
+	AB8500_REGU_VRF1_VAUX3_REGU,
+	AB8500_REGU_VARM_SEL1,
+	AB8500_REGU_VARM_SEL2,
+	AB8500_REGU_VARM_SEL3,
+	AB8500_REGU_VAPE_SEL1,
+	AB8500_REGU_VAPE_SEL2,
+	AB8500_REGU_VAPE_SEL3,
+	AB8500_REGU_VBB_SEL1,
+	AB8500_REGU_VBB_SEL2,
+	AB8500_REGU_VSMPS1_SEL1,
+	AB8500_REGU_VSMPS1_SEL2,
+	AB8500_REGU_VSMPS1_SEL3,
+	AB8500_REGU_VSMPS2_SEL1,
+	AB8500_REGU_VSMPS2_SEL2,
+	AB8500_REGU_VSMPS2_SEL3,
+	AB8500_REGU_VSMPS3_SEL1,
+	AB8500_REGU_VSMPS3_SEL2,
+	AB8500_REGU_VSMPS3_SEL3,
+	AB8500_REGU_VAUX1_SEL,
+	AB8500_REGU_VAUX2_SEL,
+	AB8500_REGU_VRF1_VAUX3_SEL,
+	AB8500_REGU_CTRL_EXT_SUP,
+	AB8500_REGU_VMOD_REGU,
+	AB8500_REGU_VMOD_SEL1,
+	AB8500_REGU_VMOD_SEL2,
+	AB8500_REGU_CTRL_DISCH,
+	AB8500_REGU_CTRL_DISCH2,
+	AB8500_OTHER_SYSCLK_CTRL, /* Other */
+	AB8500_OTHER_VSIM_SYSCLK_CTRL, /* Other */
+	AB8500_OTHER_SYSULPCLK_CTRL1, /* Other */
+	AB8500_OTHER_TVOUT_CTRL, /* Other */
+	NUM_AB8500_REGISTER
+};
+
+struct ab8500_register {
+	const char *name;
+	u8 bank;
+	u8 addr;
+};
+
+static struct ab8500_register
+		ab8500_register[NUM_AB8500_REGISTER] = {
+	[AB8500_REGU_REQUEST_CTRL1] = {
+		.name = "ReguRequestCtrl1",
+		.bank = 0x03,
+		.addr = 0x03,
+	},
+	[AB8500_REGU_REQUEST_CTRL2] = {
+		.name = "ReguRequestCtrl2",
+		.bank = 0x03,
+		.addr = 0x04,
+	},
+	[AB8500_REGU_REQUEST_CTRL3] = {
+		.name = "ReguRequestCtrl3",
+		.bank = 0x03,
+		.addr = 0x05,
+	},
+	[AB8500_REGU_REQUEST_CTRL4] = {
+		.name = "ReguRequestCtrl4",
+		.bank = 0x03,
+		.addr = 0x06,
+	},
+	[AB8500_REGU_SYSCLK_REQ1_HP_VALID1] = {
+		.name = "ReguSysClkReq1HPValid",
+		.bank = 0x03,
+		.addr = 0x07,
+	},
+	[AB8500_REGU_SYSCLK_REQ1_HP_VALID2] = {
+		.name = "ReguSysClkReq1HPValid2",
+		.bank = 0x03,
+		.addr = 0x08,
+	},
+	[AB8500_REGU_HW_HP_REQ1_VALID1] = {
+		.name = "ReguHwHPReq1Valid1",
+		.bank = 0x03,
+		.addr = 0x09,
+	},
+	[AB8500_REGU_HW_HP_REQ1_VALID2] = {
+		.name = "ReguHwHPReq1Valid2",
+		.bank = 0x03,
+		.addr = 0x0a,
+	},
+	[AB8500_REGU_HW_HP_REQ2_VALID1] = {
+		.name = "ReguHwHPReq2Valid1",
+		.bank = 0x03,
+		.addr = 0x0b,
+	},
+	[AB8500_REGU_HW_HP_REQ2_VALID2] = {
+		.name = "ReguHwHPReq2Valid2",
+		.bank = 0x03,
+		.addr = 0x0c,
+	},
+	[AB8500_REGU_SW_HP_REQ_VALID1] = {
+		.name = "ReguSwHPReqValid1",
+		.bank = 0x03,
+		.addr = 0x0d,
+	},
+	[AB8500_REGU_SW_HP_REQ_VALID2] = {
+		.name = "ReguSwHPReqValid2",
+		.bank = 0x03,
+		.addr = 0x0e,
+	},
+	[AB8500_REGU_SYSCLK_REQ1_VALID] = {
+		.name = "ReguSysClkReqValid1",
+		.bank = 0x03,
+		.addr = 0x0f,
+	},
+	[AB8500_REGU_SYSCLK_REQ2_VALID] = {
+		.name = "ReguSysClkReqValid2",
+		.bank = 0x03,
+		.addr = 0x10,
+	},
+	[AB8500_REGU_MISC1] = {
+		.name = "ReguMisc1",
+		.bank = 0x03,
+		.addr = 0x80,
+	},
+	[AB8500_REGU_OTG_SUPPLY_CTRL] = {
+		.name = "OTGSupplyCtrl",
+		.bank = 0x03,
+		.addr = 0x81,
+	},
+	[AB8500_REGU_VUSB_CTRL] = {
+		.name = "VusbCtrl",
+		.bank = 0x03,
+		.addr = 0x82,
+	},
+	[AB8500_REGU_VAUDIO_SUPPLY] = {
+		.name = "VaudioSupply",
+		.bank = 0x03,
+		.addr = 0x83,
+	},
+	[AB8500_REGU_CTRL1_VAMIC] = {
+		.name = "ReguCtrl1VAmic",
+		.bank = 0x03,
+		.addr = 0x84,
+	},
+	[AB8500_REGU_ARM_REGU1] = {
+		.name = "ArmRegu1",
+		.bank = 0x04,
+		.addr = 0x00,
+	},
+	[AB8500_REGU_ARM_REGU2] = {
+		.name = "ArmRegu2",
+		.bank = 0x04,
+		.addr = 0x01,
+	},
+	[AB8500_REGU_VAPE_REGU] = {
+		.name = "VapeRegu",
+		.bank = 0x04,
+		.addr = 0x02,
+	},
+	[AB8500_REGU_VSMPS1_REGU] = {
+		.name = "Vsmps1Regu",
+		.bank = 0x04,
+		.addr = 0x03,
+	},
+	[AB8500_REGU_VSMPS2_REGU] = {
+		.name = "Vsmps2Regu",
+		.bank = 0x04,
+		.addr = 0x04,
+	},
+	[AB8500_REGU_VSMPS3_REGU] = {
+		.name = "Vsmps3Regu",
+		.bank = 0x04,
+		.addr = 0x05,
+	},
+	[AB8500_REGU_VPLL_VANA_REGU] = {
+		.name = "VpllVanaRegu",
+		.bank = 0x04,
+		.addr = 0x06,
+	},
+	[AB8500_REGU_VREF_DDR] = {
+		.name = "VrefDDR",
+		.bank = 0x04,
+		.addr = 0x07,
+	},
+	[AB8500_REGU_EXT_SUPPLY_REGU] = {
+		.name = "ExtSupplyRegu",
+		.bank = 0x04,
+		.addr = 0x08,
+	},
+	[AB8500_REGU_VAUX12_REGU] = {
+		.name = "Vaux12Regu",
+		.bank = 0x04,
+		.addr = 0x09,
+	},
+	[AB8500_REGU_VRF1_VAUX3_REGU] = {
+		.name = "VRF1Vaux3Regu",
+		.bank = 0x04,
+		.addr = 0x0a,
+	},
+	[AB8500_REGU_VARM_SEL1] = {
+		.name = "VarmSel1",
+		.bank = 0x04,
+		.addr = 0x0b,
+	},
+	[AB8500_REGU_VARM_SEL2] = {
+		.name = "VarmSel2",
+		.bank = 0x04,
+		.addr = 0x0c,
+	},
+	[AB8500_REGU_VARM_SEL3] = {
+		.name = "VarmSel3",
+		.bank = 0x04,
+		.addr = 0x0d,
+	},
+	[AB8500_REGU_VAPE_SEL1] = {
+		.name = "VapeSel1",
+		.bank = 0x04,
+		.addr = 0x0e,
+	},
+	[AB8500_REGU_VAPE_SEL2] = {
+		.name = "VapeSel2",
+		.bank = 0x04,
+		.addr = 0x0f,
+	},
+	[AB8500_REGU_VAPE_SEL3] = {
+		.name = "VapeSel3",
+		.bank = 0x04,
+		.addr = 0x10,
+	},
+	[AB8500_REGU_VBB_SEL1] = {
+		.name = "VBBSel1",
+		.bank = 0x04,
+		.addr = 0x11,
+	},
+	[AB8500_REGU_VBB_SEL2] = {
+		.name = "VBBSel2",
+		.bank = 0x04,
+		.addr = 0x12,
+	},
+	[AB8500_REGU_VSMPS1_SEL1] = {
+		.name = "Vsmps1Sel1",
+		.bank = 0x04,
+		.addr = 0x13,
+	},
+	[AB8500_REGU_VSMPS1_SEL2] = {
+		.name = "Vsmps1Sel2",
+		.bank = 0x04,
+		.addr = 0x14,
+	},
+	[AB8500_REGU_VSMPS1_SEL3] = {
+		.name = "Vsmps1Sel3",
+		.bank = 0x04,
+		.addr = 0x15,
+	},
+	[AB8500_REGU_VSMPS2_SEL1] = {
+		.name = "Vsmps2Sel1",
+		.bank = 0x04,
+		.addr = 0x17,
+	},
+	[AB8500_REGU_VSMPS2_SEL2] = {
+		.name = "Vsmps2Sel2",
+		.bank = 0x04,
+		.addr = 0x18,
+	},
+	[AB8500_REGU_VSMPS2_SEL3] = {
+		.name = "Vsmps2Sel3",
+		.bank = 0x04,
+		.addr = 0x19,
+	},
+	[AB8500_REGU_VSMPS3_SEL1] = {
+		.name = "Vsmps3Sel1",
+		.bank = 0x04,
+		.addr = 0x1b,
+	},
+	[AB8500_REGU_VSMPS3_SEL2] = {
+		.name = "Vsmps3Sel2",
+		.bank = 0x04,
+		.addr = 0x1c,
+	},
+	[AB8500_REGU_VSMPS3_SEL3] = {
+		.name = "Vsmps3Sel3",
+		.bank = 0x04,
+		.addr = 0x1d,
+	},
+	[AB8500_REGU_VAUX1_SEL] = {
+		.name = "Vaux1Sel",
+		.bank = 0x04,
+		.addr = 0x1f,
+	},
+	[AB8500_REGU_VAUX2_SEL] = {
+		.name = "Vaux2Sel",
+		.bank = 0x04,
+		.addr = 0x20,
+	},
+	[AB8500_REGU_VRF1_VAUX3_SEL] = {
+		.name = "VRF1Vaux3Sel",
+		.bank = 0x04,
+		.addr = 0x21,
+	},
+	[AB8500_REGU_CTRL_EXT_SUP] = {
+		.name = "ReguCtrlExtSup",
+		.bank = 0x04,
+		.addr = 0x22,
+	},
+	[AB8500_REGU_VMOD_REGU] = {
+		.name = "VmodRegu",
+		.bank = 0x04,
+		.addr = 0x40,
+	},
+	[AB8500_REGU_VMOD_SEL1] = {
+		.name = "VmodSel1",
+		.bank = 0x04,
+		.addr = 0x41,
+	},
+	[AB8500_REGU_VMOD_SEL2] = {
+		.name = "VmodSel2",
+		.bank = 0x04,
+		.addr = 0x42,
+	},
+	[AB8500_REGU_CTRL_DISCH] = {
+		.name = "ReguCtrlDisch",
+		.bank = 0x04,
+		.addr = 0x43,
+	},
+	[AB8500_REGU_CTRL_DISCH2] = {
+		.name = "ReguCtrlDisch2",
+		.bank = 0x04,
+		.addr = 0x44,
+	},
+	/* Outside regulator banks */
+	[AB8500_OTHER_SYSCLK_CTRL] = {
+		.name = "SysClkCtrl",
+		.bank = 0x02,
+		.addr = 0x0c,
+	},
+	[AB8500_OTHER_VSIM_SYSCLK_CTRL] = {
+		.name = "VsimSysClkCtrl",
+		.bank = 0x02,
+		.addr = 0x33,
+	},
+	[AB8500_OTHER_SYSULPCLK_CTRL1] = {
+		.name = "SysUlpClkCtrl1",
+		.bank = 0x02,
+		.addr = 0x0b,
+	},
+	[AB8500_OTHER_TVOUT_CTRL] = {
+		.name = "TVoutCtrl",
+		.bank = 0x06,
+		.addr = 0x80,
+	},
+};
+
+static u8 ab8500_register_state[NUM_REGULATOR_STATE][NUM_AB8500_REGISTER];
+static bool ab8500_register_state_saved[NUM_REGULATOR_STATE];
+static bool ab8500_register_state_save = true;
+
+static int ab8500_regulator_record_state(int state)
+{
+	u8 val;
+	int i;
+	int ret;
+
+	/* check arguments */
+	if ((state > NUM_REGULATOR_STATE) || (state < 0)) {
+		dev_err(dev, "Wrong state specified\n");
+		return -EINVAL;
+	}
+
+	/* record */
+	if (!ab8500_register_state_save)
+		goto exit;
+
+	ab8500_register_state_saved[state] = true;
+
+	for (i = 1; i < NUM_AB8500_REGISTER; i++) {
+		ret = abx500_get_register_interruptible(dev,
+			ab8500_register[i].bank,
+			ab8500_register[i].addr,
+			&val);
+		if (ret < 0) {
+			dev_err(dev, "abx500_get_reg fail %d, %d\n",
+				ret, __LINE__);
+			return -EINVAL;
+		}
+
+		ab8500_register_state[state][i] = val;
+	}
+exit:
+	return 0;
+}
+
+/*
+ * regulator register dump
+ */
+static int ab8500_regulator_dump_print(struct seq_file *s, void *p)
+{
+	struct device *dev = s->private;
+	int state, reg_id, i;
+	int err;
+
+	/* record current state */
+	ab8500_regulator_record_state(AB8500_REGULATOR_STATE_CURRENT);
+
+	/* print dump header */
+	err = seq_printf(s, "ab8500-regulator dump:\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow\n");
+
+	/* print states */
+	for (state = NUM_REGULATOR_STATE - 1; state >= 0; state--) {
+		if (ab8500_register_state_saved[state])
+			err = seq_printf(s, "%16s saved -------",
+				regulator_state_name[state]);
+		else
+			err = seq_printf(s, "%12s not saved -------",
+				regulator_state_name[state]);
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+
+		for (i = 0; i < NUM_REGULATOR_STATE; i++) {
+			if (i < state)
+				err = seq_printf(s, "-----");
+			else if (i == state)
+				err = seq_printf(s, "----+");
+			else
+				err = seq_printf(s, "    |");
+			if (err < 0)
+				dev_err(dev, "seq_printf overflow: %i\n",
+					__LINE__);
+		}
+		err = seq_printf(s, "\n");
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	}
+
+	/* print labels */
+	err = seq_printf(s, "\n                       addr\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+
+	/* dump registers */
+	for (reg_id = 1; reg_id < NUM_AB8500_REGISTER; reg_id++) {
+		err = seq_printf(s, "%22s 0x%02x%02x:",
+			ab8500_register[reg_id].name,
+			ab8500_register[reg_id].bank,
+			ab8500_register[reg_id].addr);
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				reg_id, __LINE__);
+
+		for (state = 0; state < NUM_REGULATOR_STATE; state++) {
+			err = seq_printf(s, " 0x%02x",
+				ab8500_register_state[state][reg_id]);
+			if (err < 0)
+				dev_err(dev, "seq_printf overflow: %i, %i\n",
+					reg_id, __LINE__);
+		}
+
+		err = seq_printf(s, "\n");
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				reg_id, __LINE__);
+	}
+
+	return 0;
+}
+
+static int ab8500_regulator_dump_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_regulator_dump_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_regulator_dump_fops = {
+	.open = ab8500_regulator_dump_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+/*
+ * regulator status print
+ */
+enum ab8500_regulator_id {
+	AB8500_VARM,
+	AB8500_VBBP,
+	AB8500_VBBN,
+	AB8500_VAPE,
+	AB8500_VSMPS1,
+	AB8500_VSMPS2,
+	AB8500_VSMPS3,
+	AB8500_VPLL,
+	AB8500_VREFDDR,
+	AB8500_VMOD,
+	AB8500_VEXTSUPPLY1,
+	AB8500_VEXTSUPPLY2,
+	AB8500_VEXTSUPPLY3,
+	AB8500_VRF1,
+	AB8500_VANA,
+	AB8500_VAUX1,
+	AB8500_VAUX2,
+	AB8500_VAUX3,
+	AB8500_VINTCORE,
+	AB8500_VTVOUT,
+	AB8500_VAUDIO,
+	AB8500_VANAMIC1,
+	AB8500_VANAMIC2,
+	AB8500_VDMIC,
+	AB8500_VUSB,
+	AB8500_VOTG,
+	AB8500_VBUSBIS,
+	AB8500_NUM_REGULATORS,
+};
+
+/*
+ * regulator_voltage
+ */
+struct regulator_volt {
+	u8 value;
+	int volt;
+};
+
+struct regulator_volt_range {
+	struct regulator_volt start;
+	struct regulator_volt step;
+	struct regulator_volt end;
+};
+
+/*
+ * ab8500_regulator
+ * @name
+ * @update_regid
+ * @update_mask
+ * @update_val[4] {off, on, hw, lp}
+ * @hw_mode_regid
+ * @hw_mode_mask
+ * @hw_mode_val[4] {hp/lp, hp/off, hp, hp}
+ * @hw_valid_regid[4] {sysclkreq1, hw1, hw2, sw}
+ * @hw_valid_mask[4] {sysclkreq1, hw1, hw2, sw}
+ * @vsel_sel_regid
+ * @vsel_sel_mask
+ * @vsel_val[333] {sel1, sel2, sel3, sel3}
+ * @vsel_regid
+ * @vsel_mask
+ * @vsel_range
+ * @vsel_range_len
+ */
+struct ab8500_regulator {
+	const char *name;
+	int update_regid;
+	u8 update_mask;
+	u8 update_val[4];
+	int hw_mode_regid;
+	u8 hw_mode_mask;
+	u8 hw_mode_val[4];
+	int hw_valid_regid[4];
+	u8 hw_valid_mask[4];
+	int vsel_sel_regid;
+	u8 vsel_sel_mask;
+	u8 vsel_sel_val[4];
+	int vsel_regid[3];
+	u8 vsel_mask[3];
+	struct regulator_volt_range const *vsel_range[3];
+	int vsel_range_len[3];
+};
+
+static const char *update_val_name[] = {
+	"off",
+	"on ",
+	"hw ",
+	"lp ",
+	" - " /* undefined value */
+};
+
+static const char *hw_mode_val_name[] = {
+	"hp/lp ",
+	"hp/off",
+	"hp    ",
+	"hp    ",
+	"-/-   ", /* undefined value */
+};
+
+/* voltage selection */
+static const struct regulator_volt_range varm_vape_vmod_vsel[] = {
+	{ {0x00,  700000}, {0x01,   12500}, {0x35, 1362500} },
+	{ {0x36, 1362500}, {0x01,       0}, {0x3f, 1362500} },
+};
+
+static const struct regulator_volt_range vbbp_vsel[] = {
+	{ {0x00,       0}, {0x10,  100000}, {0x40,  400000} },
+	{ {0x50,  400000}, {0x10,       0}, {0x70,  400000} },
+	{ {0x80, -400000}, {0x10,       0}, {0xb0, -400000} },
+	{ {0xc0, -400000}, {0x10,  100000}, {0xf0, -100000} },
+};
+
+static const struct regulator_volt_range vbbn_vsel[] = {
+	{ {0x00,       0}, {0x01, -100000}, {0x04, -400000} },
+	{ {0x05, -400000}, {0x01,       0}, {0x07, -400000} },
+	{ {0x08,       0}, {0x01,  100000}, {0x0c,  400000} },
+	{ {0x0d,  400000}, {0x01,       0}, {0x0f,  400000} },
+};
+
+static const struct regulator_volt_range vsmps1_vsel[] = {
+	{ {0x00, 1100000}, {0x01,       0}, {0x1f, 1100000} },
+	{ {0x20, 1100000}, {0x01,   12500}, {0x30, 1300000} },
+	{ {0x31, 1300000}, {0x01,       0}, {0x3f, 1300000} },
+};
+
+static const struct regulator_volt_range vsmps2_vsel[] = {
+	{ {0x00, 1800000}, {0x01,       0}, {0x38, 1800000} },
+	{ {0x39, 1800000}, {0x01,   12500}, {0x7f, 1875000} },
+};
+
+static const struct regulator_volt_range vsmps3_vsel[] = {
+	{ {0x00,  700000}, {0x01,   12500}, {0x35, 1363500} },
+	{ {0x36, 1363500}, {0x01,       0}, {0x7f, 1363500} },
+};
+
+static const struct regulator_volt_range vaux1_vaux2_vsel[] = {
+	{ {0x00, 1100000}, {0x01,  100000}, {0x04, 1500000} },
+	{ {0x05, 1800000}, {0x01,   50000}, {0x07, 1900000} },
+	{ {0x08, 2500000}, {0x01,       0}, {0x08, 2500000} },
+	{ {0x09, 2650000}, {0x01,   50000}, {0x0c, 2800000} },
+	{ {0x0d, 2900000}, {0x01,  100000}, {0x0e, 3000000} },
+	{ {0x0f, 3300000}, {0x01,       0}, {0x0f, 3300000} },
+};
+
+static const struct regulator_volt_range vaux3_vsel[] = {
+	{ {0x00, 1200000}, {0x01,  300000}, {0x03, 2100000} },
+	{ {0x04, 2500000}, {0x01,  250000}, {0x05, 2750000} },
+	{ {0x06, 2790000}, {0x01,       0}, {0x06, 2790000} },
+	{ {0x07, 2910000}, {0x01,       0}, {0x07, 2910000} },
+};
+
+static const struct regulator_volt_range vrf1_vsel[] = {
+	{ {0x00, 1800000}, {0x10,  200000}, {0x10, 2000000} },
+	{ {0x20, 2150000}, {0x10,       0}, {0x20, 2150000} },
+	{ {0x30, 2500000}, {0x10,       0}, {0x30, 2500000} },
+};
+
+static const struct regulator_volt_range vintcore12_vsel[] = {
+	{ {0x00, 1200000}, {0x08,   25000}, {0x30, 1350000} },
+	{ {0x38, 1350000}, {0x01,       0}, {0x38, 1350000} },
+};
+
+/* regulators */
+static struct ab8500_regulator ab8500_regulator[AB8500_NUM_REGULATORS] = {
+	[AB8500_VARM] = {
+		.name              = "Varm",
+		.update_regid      = AB8500_REGU_ARM_REGU1,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL1,
+		.hw_mode_mask      = 0x03,
+		.hw_mode_val       = {0x00, 0x01, 0x02, 0x03},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x02,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x02,
+		.vsel_sel_regid    = AB8500_REGU_ARM_REGU1,
+		.vsel_sel_mask     = 0x0c,
+		.vsel_sel_val      = {0x00, 0x04, 0x08, 0x0c},
+		.vsel_regid[0]     = AB8500_REGU_VARM_SEL1,
+		.vsel_mask[0]      = 0x3f,
+		.vsel_range[0]     = varm_vape_vmod_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(varm_vape_vmod_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VARM_SEL2,
+		.vsel_mask[1]      = 0x3f,
+		.vsel_range[1]     = varm_vape_vmod_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(varm_vape_vmod_vsel),
+		.vsel_regid[2]     = AB8500_REGU_VARM_SEL3,
+		.vsel_mask[2]      = 0x3f,
+		.vsel_range[2]     = varm_vape_vmod_vsel,
+		.vsel_range_len[2] = ARRAY_SIZE(varm_vape_vmod_vsel),
+	},
+	[AB8500_VBBP] = {
+		.name              = "Vbbp",
+		.update_regid      = AB8500_REGU_ARM_REGU2,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x00},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x04,
+		.vsel_sel_regid    = AB8500_REGU_ARM_REGU1,
+		.vsel_sel_mask     = 0x10,
+		.vsel_sel_val      = {0x00, 0x10, 0x00, 0x00},
+		.vsel_regid[0]     = AB8500_REGU_VBB_SEL1,
+		.vsel_mask[0]      = 0xf0,
+		.vsel_range[0]     = vbbp_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vbbp_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VBB_SEL2,
+		.vsel_mask[1]      = 0xf0,
+		.vsel_range[1]     = vbbp_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(vbbp_vsel),
+	},
+	[AB8500_VBBN] = {
+		.name              = "Vbbn",
+		.update_regid      = AB8500_REGU_ARM_REGU2,
+		.update_mask       = 0x0c,
+		.update_val        = {0x00, 0x04, 0x08, 0x00},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x04,
+		.vsel_sel_regid    = AB8500_REGU_ARM_REGU1,
+		.vsel_sel_mask     = 0x20,
+		.vsel_sel_val      = {0x00, 0x20, 0x00, 0x00},
+		.vsel_regid[0]     = AB8500_REGU_VBB_SEL1,
+		.vsel_mask[0]      = 0x0f,
+		.vsel_range[0]     = vbbn_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vbbn_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VBB_SEL2,
+		.vsel_mask[1]      = 0x0f,
+		.vsel_range[1]     = vbbn_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(vbbn_vsel),
+	},
+	[AB8500_VAPE] = {
+		.name              = "Vape",
+		.update_regid      = AB8500_REGU_VAPE_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL1,
+		.hw_mode_mask      = 0x0c,
+		.hw_mode_val       = {0x00, 0x04, 0x08, 0x0c},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x01,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x01,
+		.vsel_sel_regid    = AB8500_REGU_VAPE_REGU,
+		.vsel_sel_mask     = 0x24,
+		.vsel_sel_val      = {0x00, 0x04, 0x20, 0x24},
+		.vsel_regid[0]     = AB8500_REGU_VAPE_SEL1,
+		.vsel_mask[0]      = 0x3f,
+		.vsel_range[0]     = varm_vape_vmod_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(varm_vape_vmod_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VAPE_SEL2,
+		.vsel_mask[1]      = 0x3f,
+		.vsel_range[1]     = varm_vape_vmod_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(varm_vape_vmod_vsel),
+		.vsel_regid[2]     = AB8500_REGU_VAPE_SEL3,
+		.vsel_mask[2]      = 0x3f,
+		.vsel_range[2]     = varm_vape_vmod_vsel,
+		.vsel_range_len[2] = ARRAY_SIZE(varm_vape_vmod_vsel),
+	},
+	[AB8500_VSMPS1] = {
+		.name              = "Vsmps1",
+		.update_regid      = AB8500_REGU_VSMPS1_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL1,
+		.hw_mode_mask      = 0x30,
+		.hw_mode_val       = {0x00, 0x10, 0x20, 0x30},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x01,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x01,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x01,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x04,
+		.vsel_sel_regid    = AB8500_REGU_VSMPS1_REGU,
+		.vsel_sel_mask     = 0x0c,
+		.vsel_sel_val      = {0x00, 0x04, 0x08, 0x0c},
+		.vsel_regid[0]     = AB8500_REGU_VSMPS1_SEL1,
+		.vsel_mask[0]      = 0x3f,
+		.vsel_range[0]     = vsmps1_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vsmps1_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VSMPS1_SEL2,
+		.vsel_mask[1]      = 0x3f,
+		.vsel_range[1]     = vsmps1_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(vsmps1_vsel),
+		.vsel_regid[2]     = AB8500_REGU_VSMPS1_SEL3,
+		.vsel_mask[2]      = 0x3f,
+		.vsel_range[2]     = vsmps1_vsel,
+		.vsel_range_len[2] = ARRAY_SIZE(vsmps1_vsel),
+	},
+	[AB8500_VSMPS2] = {
+		.name              = "Vsmps2",
+		.update_regid      = AB8500_REGU_VSMPS2_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL1,
+		.hw_mode_mask      = 0xc0,
+		.hw_mode_val       = {0x00, 0x40, 0x80, 0xc0},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x02,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x02,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x02,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x08,
+		.vsel_sel_regid    = AB8500_REGU_VSMPS2_REGU,
+		.vsel_sel_mask     = 0x0c,
+		.vsel_sel_val      = {0x00, 0x04, 0x08, 0x0c},
+		.vsel_regid[0]     = AB8500_REGU_VSMPS2_SEL1,
+		.vsel_mask[0]      = 0x3f,
+		.vsel_range[0]     = vsmps2_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vsmps2_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VSMPS2_SEL2,
+		.vsel_mask[1]      = 0x3f,
+		.vsel_range[1]     = vsmps2_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(vsmps2_vsel),
+		.vsel_regid[2]     = AB8500_REGU_VSMPS2_SEL3,
+		.vsel_mask[2]      = 0x3f,
+		.vsel_range[2]     = vsmps2_vsel,
+		.vsel_range_len[2] = ARRAY_SIZE(vsmps2_vsel),
+	},
+	[AB8500_VSMPS3] = {
+		.name              = "Vsmps3",
+		.update_regid      = AB8500_REGU_VSMPS3_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL2,
+		.hw_mode_mask      = 0x03,
+		.hw_mode_val       = {0x00, 0x01, 0x02, 0x03},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x04,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x04,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x04,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x10,
+		.vsel_sel_regid    = AB8500_REGU_VSMPS3_REGU,
+		.vsel_sel_mask     = 0x0c,
+		.vsel_sel_val      = {0x00, 0x04, 0x08, 0x0c},
+		.vsel_regid[0]     = AB8500_REGU_VSMPS3_SEL1,
+		.vsel_mask[0]      = 0x7f,
+		.vsel_range[0]     = vsmps3_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vsmps3_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VSMPS3_SEL2,
+		.vsel_mask[1]      = 0x7f,
+		.vsel_range[1]     = vsmps3_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(vsmps3_vsel),
+		.vsel_regid[2]     = AB8500_REGU_VSMPS3_SEL3,
+		.vsel_mask[2]      = 0x7f,
+		.vsel_range[2]     = vsmps3_vsel,
+		.vsel_range_len[2] = ARRAY_SIZE(vsmps3_vsel),
+	},
+	[AB8500_VPLL] = {
+		.name              = "Vpll",
+		.update_regid      = AB8500_REGU_VPLL_VANA_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL2,
+		.hw_mode_mask      = 0x0c,
+		.hw_mode_val       = {0x00, 0x04, 0x08, 0x0c},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x10,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x10,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x10,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x40,
+	},
+	[AB8500_VREFDDR] = {
+		.name              = "VrefDDR",
+		.update_regid      = AB8500_REGU_VREF_DDR,
+		.update_mask       = 0x01,
+		.update_val        = {0x00, 0x01, 0x00, 0x00},
+	},
+	[AB8500_VMOD] = {
+		.name              = "Vmod",
+		.update_regid      = AB8500_REGU_VMOD_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_VMOD_REGU,
+		.hw_mode_mask      = 0xc0,
+		.hw_mode_val       = {0x00, 0x40, 0x80, 0xc0},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x08,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID2,
+		.hw_valid_mask[1]  = 0x08,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID2,
+		.hw_valid_mask[2]  = 0x08,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x20,
+		.vsel_sel_regid    = AB8500_REGU_VMOD_REGU,
+		.vsel_sel_mask     = 0x04,
+		.vsel_sel_val      = {0x00, 0x04, 0x00, 0x00},
+		.vsel_regid[0]     = AB8500_REGU_VMOD_SEL1,
+		.vsel_mask[0]      = 0x3f,
+		.vsel_range[0]     = varm_vape_vmod_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(varm_vape_vmod_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VMOD_SEL2,
+		.vsel_mask[1]      = 0x3f,
+		.vsel_range[1]     = varm_vape_vmod_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(varm_vape_vmod_vsel),
+	},
+	[AB8500_VEXTSUPPLY1] = {
+		.name              = "Vextsupply1",
+		.update_regid      = AB8500_REGU_EXT_SUPPLY_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL2,
+		.hw_mode_mask      = 0xc0,
+		.hw_mode_val       = {0x00, 0x40, 0x80, 0xc0},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x10,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID2,
+		.hw_valid_mask[1]  = 0x01,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID2,
+		.hw_valid_mask[2]  = 0x01,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x04,
+	},
+	[AB8500_VEXTSUPPLY2] = {
+		.name              = "VextSupply2",
+		.update_regid      = AB8500_REGU_EXT_SUPPLY_REGU,
+		.update_mask       = 0x0c,
+		.update_val        = {0x00, 0x04, 0x08, 0x0c},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL3,
+		.hw_mode_mask      = 0x03,
+		.hw_mode_val       = {0x00, 0x01, 0x02, 0x03},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x20,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID2,
+		.hw_valid_mask[1]  = 0x02,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID2,
+		.hw_valid_mask[2]  = 0x02,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x08,
+	},
+	[AB8500_VEXTSUPPLY3] = {
+		.name              = "VextSupply3",
+		.update_regid      = AB8500_REGU_EXT_SUPPLY_REGU,
+		.update_mask       = 0x30,
+		.update_val        = {0x00, 0x10, 0x20, 0x30},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL3,
+		.hw_mode_mask      = 0x0c,
+		.hw_mode_val       = {0x00, 0x04, 0x08, 0x0c},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x40,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID2,
+		.hw_valid_mask[1]  = 0x04,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID2,
+		.hw_valid_mask[2]  = 0x04,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x10,
+	},
+	[AB8500_VRF1] = {
+		.name              = "Vrf1",
+		.update_regid      = AB8500_REGU_VRF1_VAUX3_REGU,
+		.update_mask       = 0x0c,
+		.update_val        = {0x00, 0x04, 0x08, 0x0c},
+		.vsel_regid[0]     = AB8500_REGU_VRF1_VAUX3_SEL,
+		.vsel_mask[0]      = 0x30,
+		.vsel_range[0]     = vrf1_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vrf1_vsel),
+	},
+	[AB8500_VANA] = {
+		.name              = "Vana",
+		.update_regid      = AB8500_REGU_VPLL_VANA_REGU,
+		.update_mask       = 0x0c,
+		.update_val        = {0x00, 0x04, 0x08, 0x0c},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL2,
+		.hw_mode_mask      = 0x30,
+		.hw_mode_val       = {0x00, 0x10, 0x20, 0x30},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x08,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x08,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x08,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x20,
+	},
+	[AB8500_VAUX1] = {
+		.name              = "Vaux1",
+		.update_regid      = AB8500_REGU_VAUX12_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL3,
+		.hw_mode_mask      = 0x30,
+		.hw_mode_val       = {0x00, 0x10, 0x20, 0x30},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x20,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x20,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x20,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x80,
+		.vsel_regid[0]     = AB8500_REGU_VAUX1_SEL,
+		.vsel_mask[0]      = 0x0f,
+		.vsel_range[0]     = vaux1_vaux2_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vaux1_vaux2_vsel),
+	},
+	[AB8500_VAUX2] = {
+		.name              = "Vaux2",
+		.update_regid      = AB8500_REGU_VAUX12_REGU,
+		.update_mask       = 0x0c,
+		.update_val        = {0x00, 0x04, 0x08, 0x0c},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL3,
+		.hw_mode_mask      = 0xc0,
+		.hw_mode_val       = {0x00, 0x40, 0x80, 0xc0},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x40,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x40,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x40,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x01,
+		.vsel_regid[0]     = AB8500_REGU_VAUX2_SEL,
+		.vsel_mask[0]      = 0x0f,
+		.vsel_range[0]     = vaux1_vaux2_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vaux1_vaux2_vsel),
+	},
+	[AB8500_VAUX3] = {
+		.name              = "Vaux3",
+		.update_regid      = AB8500_REGU_VRF1_VAUX3_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL4,
+		.hw_mode_mask      = 0x03,
+		.hw_mode_val       = {0x00, 0x01, 0x02, 0x03},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x80,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x80,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x80,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x02,
+		.vsel_regid[0]     = AB8500_REGU_VRF1_VAUX3_SEL,
+		.vsel_mask[0]      = 0x07,
+		.vsel_range[0]     = vaux3_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vaux3_vsel),
+	},
+	[AB8500_VINTCORE] = {
+		.name              = "VintCore12",
+		.update_regid      = AB8500_REGU_MISC1,
+		.update_mask       = 0x44,
+		.update_val        = {0x00, 0x04, 0x00, 0x44},
+		.vsel_regid[0]     = AB8500_REGU_MISC1,
+		.vsel_mask[0]      = 0x38,
+		.vsel_range[0]     = vintcore12_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vintcore12_vsel),
+	},
+	[AB8500_VTVOUT] = {
+		.name              = "VTVout",
+		.update_regid      = AB8500_REGU_MISC1,
+		.update_mask       = 0x82,
+		.update_val        = {0x00, 0x02, 0x00, 0x82},
+	},
+	[AB8500_VAUDIO] = {
+		.name              = "Vaudio",
+		.update_regid      = AB8500_REGU_VAUDIO_SUPPLY,
+		.update_mask       = 0x02,
+		.update_val        = {0x00, 0x02, 0x00, 0x00},
+	},
+	[AB8500_VANAMIC1] = {
+		.name              = "Vanamic1",
+		.update_regid      = AB8500_REGU_VAUDIO_SUPPLY,
+		.update_mask       = 0x08,
+		.update_val        = {0x00, 0x08, 0x00, 0x00},
+	},
+	[AB8500_VANAMIC2] = {
+		.name              = "Vanamic2",
+		.update_regid      = AB8500_REGU_VAUDIO_SUPPLY,
+		.update_mask       = 0x10,
+		.update_val        = {0x00, 0x10, 0x00, 0x00},
+	},
+	[AB8500_VDMIC] = {
+		.name              = "Vdmic",
+		.update_regid      = AB8500_REGU_VAUDIO_SUPPLY,
+		.update_mask       = 0x04,
+		.update_val        = {0x00, 0x04, 0x00, 0x00},
+	},
+	[AB8500_VUSB] = {
+		.name              = "Vusb",
+		.update_regid      = AB8500_REGU_VUSB_CTRL,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x00, 0x03},
+	},
+	[AB8500_VOTG] = {
+		.name              = "VOTG",
+		.update_regid      = AB8500_REGU_OTG_SUPPLY_CTRL,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x00, 0x03},
+	},
+	[AB8500_VBUSBIS] = {
+		.name              = "Vbusbis",
+		.update_regid      = AB8500_REGU_OTG_SUPPLY_CTRL,
+		.update_mask       = 0x08,
+		.update_val        = {0x00, 0x08, 0x00, 0x00},
+	},
+};
+
+static int status_state;
+
+static int _get_voltage(struct regulator_volt_range const *volt_range,
+	u8 value, int *volt)
+{
+	u8 start = volt_range->start.value;
+	u8 end = volt_range->end.value;
+	u8 step = volt_range->step.value;
+
+	/* Check if witin range */
+	if (step == 0) {
+		if (value == start) {
+			*volt = volt_range->start.volt;
+			return 1;
+		}
+	} else {
+		if ((start <= value) && (value <= end)) {
+			if ((value - start)%step != 0)
+				return -EINVAL; /* invalid setting */
+			*volt = volt_range->start.volt
+			     + volt_range->step.volt
+			     *((value - start)/step);
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+static int get_voltage(struct regulator_volt_range const *volt_range,
+	int volt_range_len,
+	u8 value)
+{
+	int volt;
+	int i, ret;
+
+	for (i = 0; i < volt_range_len; i++) {
+		ret = _get_voltage(&volt_range[i], value, &volt);
+		if (ret < 0)
+			break; /* invalid setting */
+		if (ret == 1)
+			return volt; /* successful */
+	}
+
+	return -EINVAL;
+}
+
+static int ab8500_regulator_status_print(struct seq_file *s, void *p)
+{
+	struct device *dev = s->private;
+	int id, regid;
+	int i;
+	u8 val;
+	int err;
+
+	/* record current state */
+	ab8500_regulator_record_state(AB8500_REGULATOR_STATE_CURRENT);
+
+	/* check if chosen state is recorded */
+	if (!ab8500_register_state_saved[status_state]) {
+		seq_printf(s, "ab8500-regulator status is not recorded.\n");
+		goto exit;
+	}
+
+	/* print dump header */
+	err = seq_printf(s, "ab8500-regulator status:\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow\n");
+
+	/* print state */
+	err = seq_printf(s, "%12s\n",
+		regulator_state_name[status_state]);
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow\n");
+
+	/* print labels */
+	err = seq_printf(s,
+	      "+-----------+----+--------------+-------------------------+\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	err = seq_printf(s,
+	      "|       name|man |auto          |voltage                  |\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	err = seq_printf(s,
+	      "+-----------+----+--------------+ +-----------------------+\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	err = seq_printf(s,
+	      "|           |mode|mode  |0|1|2|3| |    1  |    2  |    3  |\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	err = seq_printf(s,
+	      "+-----------+----+------+-+-+-+-+-+-------+-------+-------+\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+
+	/* dump registers */
+	for (id = 0; id < AB8500_NUM_REGULATORS; id++) {
+		/* print name */
+		err = seq_printf(s, "|%11s|",
+			ab8500_regulator[id].name);
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				id, __LINE__);
+
+		/* print manual mode */
+		regid = ab8500_regulator[id].update_regid;
+		val = ab8500_register_state[status_state][regid]
+		    & ab8500_regulator[id].update_mask;
+		for (i = 0; i < 4; i++) {
+			if (val == ab8500_regulator[id].update_val[i])
+				break;
+		}
+		err = seq_printf(s, "%4s|",
+			update_val_name[i]);
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				id, __LINE__);
+
+		/* print auto mode */
+		regid = ab8500_regulator[id].hw_mode_regid;
+		if (regid) {
+			val = ab8500_register_state[status_state][regid]
+			    & ab8500_regulator[id].hw_mode_mask;
+			for (i = 0; i < 4; i++) {
+				if (val == ab8500_regulator[id].hw_mode_val[i])
+					break;
+			}
+			err = seq_printf(s, "%6s|",
+				hw_mode_val_name[i]);
+		} else {
+			err = seq_printf(s, "      |");
+		}
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				id, __LINE__);
+
+		/* print valid bits */
+		for (i = 0; i < 4; i++) {
+			regid = ab8500_regulator[id].hw_valid_regid[i];
+			if (regid) {
+				val = ab8500_register_state[status_state][regid]
+				    & ab8500_regulator[id].hw_valid_mask[i];
+				if (val)
+					err = seq_printf(s, "1|");
+				else
+					err = seq_printf(s, "0|");
+			} else {
+				err = seq_printf(s, " |");
+			}
+			if (err < 0)
+				dev_err(dev, "seq_printf overflow: %i, %i\n",
+					regid, __LINE__);
+		}
+
+		/* print voltage selection */
+		regid = ab8500_regulator[id].vsel_sel_regid;
+		if (regid) {
+			val = ab8500_register_state[status_state][regid]
+			    & ab8500_regulator[id].vsel_sel_mask;
+			for (i = 0; i < 3; i++) {
+				if (val == ab8500_regulator[id].vsel_sel_val[i])
+					break;
+			}
+			if (i < 3)
+				seq_printf(s, "%i|", i + 1);
+			else
+				seq_printf(s, "-|");
+		} else {
+			seq_printf(s, " |");
+		}
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				regid, __LINE__);
+
+		for (i = 0; i < 3; i++) {
+			int volt;
+
+			regid = ab8500_regulator[id].vsel_regid[i];
+			if (regid) {
+				val = ab8500_register_state[status_state][regid]
+				    & ab8500_regulator[id].vsel_mask[i];
+				volt = get_voltage(
+					ab8500_regulator[id].vsel_range[i],
+					ab8500_regulator[id].vsel_range_len[i],
+					val);
+				seq_printf(s, "%7i|", volt);
+			} else {
+				seq_printf(s, "       |");
+			}
+			if (err < 0)
+				dev_err(dev, "seq_printf overflow: %i, %i\n",
+					regid, __LINE__);
+		}
+
+		err = seq_printf(s, "\n");
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				regid, __LINE__);
+
+	}
+	err = seq_printf(s,
+	      "+-----------+----+------+-+-+-+-+-+-------+-------+-------+\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	err = seq_printf(s,
+	      "Note! In HW mode, voltage selection is controlled by HW.\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+
+
+exit:
+	return 0;
+}
+
+static int ab8500_regulator_status_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	char buf[32];
+	int buf_size;
+	unsigned long user_val;
+	int err;
+
+	/* copy user data */
+	buf_size = min(count, (sizeof(buf) - 1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	/* convert */
+	err = strict_strtoul(buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+
+	/* set suspend force setting */
+	if (user_val > NUM_REGULATOR_STATE) {
+		dev_err(dev, "debugfs error input > number of states\n");
+		return -EINVAL;
+	}
+
+	status_state = user_val;
+
+	return buf_size;
+}
+
+
+static int ab8500_regulator_status_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_regulator_status_print,
+		inode->i_private);
+}
+
+static const struct file_operations ab8500_regulator_status_fops = {
+	.open = ab8500_regulator_status_open,
+	.write = ab8500_regulator_status_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+#ifdef CONFIG_PM
+
+struct ab8500_force_reg {
+	char *name;
+	u8 bank;
+	u8 addr;
+	u8 mask;
+	u8 val;
+	bool restore;
+	u8 restore_val;
+};
+
+static struct ab8500_force_reg ab8500_force_reg[] = {
+	{
+		/*
+		 * SysClkCtrl
+		 * OTP: 0x00, HSI: 0x06, suspend: 0x00/0x07 (value/mask)
+		 * [  2] USBClkEna = disable SysClk path to USB block
+		 * [  1] TVoutClkEna = disable 27Mhz clock to TVout block
+		 * [  0] TVoutPllEna = disable TVout pll
+		 *                     (generate 27Mhz from SysClk)
+		 */
+		.name = "SysClkCtrl",
+		.bank = 0x02,
+		.addr = 0x0c,
+		.mask = 0x07,
+		.val  = 0x00,
+	},
+	{
+		/*
+		 * ReguSysClkReq1HPValid2
+		 * OTP: 0x03, HSI: 0x40, suspend: 0x60/0x70 (value/mask)
+		 * [  5] VextSupply2SysClkReq1HPValid = Vext2 set by SysClkReq1
+		 */
+		.name = "ReguSysClkReq1HPValid2",
+		.bank = 0x03,
+		.addr = 0x08,
+		.mask = 0x20, /* test and compare with 0x7f */
+		.val  = 0x20,
+	},
+	{
+		/*
+		 * ReguRequestCtrl3
+		 * OTP: 0x00, HSI: 0x00, suspend: 0x05/0x0f (value/mask)
+		 * [1:0] VExtSupply2RequestCtrl[1:0] = VExt2 set in HP/OFF mode
+		 */
+		.name = "ReguRequestCtrl3",
+		.bank = 0x03,
+		.addr = 0x05,
+		.mask = 0x03, /* test and compare with 0xff */
+		.val  = 0x01,
+	},
+	{
+		/*
+		 * VsimSysClkCtrl
+		 * OTP: 0x01, HSI: 0x21, suspend: 0x01/0xff (value/mask)
+		 * [  7] VsimSysClkReq8Valid = no connection
+		 * [  6] VsimSysClkReq7Valid = no connection
+		 * [  5] VsimSysClkReq6Valid = no connection
+		 * [  4] VsimSysClkReq5Valid = no connection
+		 * [  3] VsimSysClkReq4Valid = no connection
+		 * [  2] VsimSysClkReq3Valid = no connection
+		 * [  1] VsimSysClkReq2Valid = no connection
+		 * [  0] VsimSysClkReq1Valid = Vsim set by SysClkReq1
+		 */
+		.name = "VsimSysClkCtrl",
+		.bank = 0x02,
+		.addr = 0x33,
+		.mask = 0xff,
+		.val  = 0x01,
+	},
+	{
+		/*
+		 * SysUlpClkCtrl1
+		 * OTP: 0x00, HSI: 0x00, suspend: 0x00/0x0f (value/mask)
+		 * [  3] 4500SysClkReq = inactive
+		 * [  2] UlpClkReq = inactive
+		 * [1:0] SysUlpClkIntSel[1:0] = no internal clock switching.
+		 *                              Internal clock is SysClk.
+		 */
+		.name = "SysUlpClkCtrl1",
+		.bank = 0x02,
+		.addr = 0x0b,
+		.mask = 0x0f,
+		.val  = 0x00,
+	},
+	{
+		/*
+		 * ExtSupplyRegu (HSI: 0x2a on v2-v40?)
+		 * OTP: 0x15, HSI: 0x28, suspend: 0x28/0x3f (value/mask)
+		 * [5:4] VExtSupply3Regu[1:0] = 10 = Vext3 off
+		 * [3:2] VExtSupply2Regu[1:0] = 10 = Vext2 in HW control
+		 * [1:0] VExtSupply1Regu[1:0] = 00 = Vext1 off
+		 */
+		.name = "ExtSupplyRegu",
+		.bank = 0x04,
+		.addr = 0x08,
+		.mask = 0x3f,
+		.val  = 0x08,
+	},
+	{
+		/*
+		 * TVoutCtrl
+		 * OTP: N/A, HSI: N/A, suspend: 0x00/0x03 (value/mask)
+		 * [  2]   PlugTvOn = plug/unplug detection disabled
+		 * [1:0]   TvoutDacCtrl[1:0] = "0" forced on DAC input (test)
+		 */
+		.name = "TVoutCtrl",
+		.bank = 0x06,
+		.addr = 0x80,
+		.mask = 0x03,
+		.val  = 0x00,
+	},
+};
+
+void ab8500_regulator_debug_force(void)
+{
+	int ret, i;
+
+	/* save state of registers */
+	ret = ab8500_regulator_record_state(AB8500_REGULATOR_STATE_SUSPEND);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Failed to record suspend state.\n");
+
+	/* check if registers should be forced */
+	if (!setting_suspend_force)
+		goto exit;
+
+	/*
+	 * Optimize href v2_v50_pwr board for ApSleep/ApDeepSleep
+	 * power consumption measurements
+	 */
+
+	for (i = 0; i < ARRAY_SIZE(ab8500_force_reg); i++) {
+		dev_vdbg(&pdev->dev, "Save and set %s: "
+			"0x%02x, 0x%02x, 0x%02x, 0x%02x.\n",
+			ab8500_force_reg[i].name,
+			ab8500_force_reg[i].bank,
+			ab8500_force_reg[i].addr,
+			ab8500_force_reg[i].mask,
+			ab8500_force_reg[i].val);
+
+		/* assume that register should be restored */
+		ab8500_force_reg[i].restore = true;
+
+		/* get register value before forcing it */
+		ret = abx500_get_register_interruptible(&pdev->dev,
+			ab8500_force_reg[i].bank,
+			ab8500_force_reg[i].addr,
+			&ab8500_force_reg[i].restore_val);
+		if (ret < 0) {
+			dev_err(dev, "Failed to read %s.\n",
+				ab8500_force_reg[i].name);
+			ab8500_force_reg[i].restore = false;
+			break;
+		}
+
+		/* force register value */
+		ret = abx500_mask_and_set_register_interruptible(&pdev->dev,
+			ab8500_force_reg[i].bank,
+			ab8500_force_reg[i].addr,
+			ab8500_force_reg[i].mask,
+			ab8500_force_reg[i].val);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "Failed to write %s.\n",
+				ab8500_force_reg[i].name);
+			ab8500_force_reg[i].restore = false;
+		}
+	}
+
+exit:
+	/* save state of registers */
+	ret = ab8500_regulator_record_state(
+		AB8500_REGULATOR_STATE_SUSPEND_CORE);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Failed to record suspend state.\n");
+
+	return;
+}
+
+void ab8500_regulator_debug_restore(void)
+{
+	int ret, i;
+
+	/* save state of registers */
+	ret = ab8500_regulator_record_state(AB8500_REGULATOR_STATE_RESUME_CORE);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Failed to record resume state.\n");
+	for (i = ARRAY_SIZE(ab8500_force_reg) - 1; i >= 0; i--) {
+		/* restore register value */
+		if (ab8500_force_reg[i].restore) {
+			ret = abx500_mask_and_set_register_interruptible(
+				 &pdev->dev,
+				 ab8500_force_reg[i].bank,
+				 ab8500_force_reg[i].addr,
+				 ab8500_force_reg[i].mask,
+				 ab8500_force_reg[i].restore_val);
+			if (ret < 0)
+				dev_err(&pdev->dev, "Failed to restore %s.\n",
+					ab8500_force_reg[i].name);
+			dev_vdbg(&pdev->dev, "Restore %s: "
+				"0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
+				ab8500_force_reg[i].name,
+				ab8500_force_reg[i].bank,
+				ab8500_force_reg[i].addr,
+				ab8500_force_reg[i].mask,
+				ab8500_force_reg[i].restore_val);
+		}
+	}
+
+	/* save state of registers */
+	ret = ab8500_regulator_record_state(AB8500_REGULATOR_STATE_RESUME);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Failed to record resume state.\n");
+
+	return;
+}
+
+#endif
+
+static int ab8500_regulator_suspend_force_show(struct seq_file *s, void *p)
+{
+	/* print suspend standby status */
+	if (setting_suspend_force)
+		return seq_printf(s, "suspend force enabled\n");
+	else
+		return seq_printf(s, "no suspend force\n");
+}
+
+static int ab8500_regulator_suspend_force_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	char buf[32];
+	int buf_size;
+	unsigned long user_val;
+	int err;
+
+	/* copy user data */
+	buf_size = min(count, (sizeof(buf) - 1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	/* convert */
+	err = strict_strtoul(buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+
+	/* set suspend force setting */
+	if (user_val > 1) {
+		dev_err(dev, "debugfs error input > 1\n");
+		return -EINVAL;
+	}
+
+	if (user_val)
+		setting_suspend_force = true;
+	else
+		setting_suspend_force = false;
+
+	return buf_size;
+}
+
+static int ab8500_regulator_suspend_force_open(struct inode *inode,
+	struct file *file)
+{
+	return single_open(file, ab8500_regulator_suspend_force_show,
+		inode->i_private);
+}
+
+static const struct file_operations ab8500_regulator_suspend_force_fops = {
+	.open = ab8500_regulator_suspend_force_open,
+	.write = ab8500_regulator_suspend_force_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static struct dentry *ab8500_regulator_dir;
+static struct dentry *ab8500_regulator_dump_file;
+static struct dentry *ab8500_regulator_status_file;
+static struct dentry *ab8500_regulator_suspend_force_file;
+
+static int __devinit ab8500_regulator_debug_probe(struct platform_device *plf)
+{
+	void __iomem *boot_info_backupram;
+	int ret, i;
+
+	/* setup dev pointers */
+	dev = &plf->dev;
+	pdev = plf;
+
+	/* save state of registers */
+	ret = ab8500_regulator_record_state(AB8500_REGULATOR_STATE_INIT);
+	if (ret < 0)
+		dev_err(&plf->dev, "Failed to record init state.\n");
+
+	/* remove force of external regulators on AB8500 3.0 */
+	if (abx500_get_chip_id(&pdev->dev) >= 0x30) {
+		/*
+		 * find ExtSupplyRegu register (bank 0x04, addr 0x08)
+		 * and reset mask and value
+		 */
+		for (i = 0; i < ARRAY_SIZE(ab8500_force_reg); i++) {
+			if (ab8500_force_reg[i].bank == 0x04 &&
+			    ab8500_force_reg[i].addr == 0x08) {
+				ab8500_force_reg[i].mask = 0x00;
+				ab8500_force_reg[i].val = 0x00;
+			}
+		}
+	}
+
+	/* make suspend-force default if board profile is v5x-power */
+	boot_info_backupram = ioremap(BOOT_INFO_BACKUPRAM1, 0x4);
+
+	if (boot_info_backupram) {
+		u8 board_profile;
+		board_profile = readb(
+			boot_info_backupram + BOARD_PROFILE_BACKUPRAM1);
+		dev_dbg(dev, "Board profile is 0x%02x\n", board_profile);
+
+		if (board_profile >= OPTION_BOARD_VERSION_V5X)
+			setting_suspend_force = true;
+
+		iounmap(boot_info_backupram);
+	} else {
+		dev_err(dev, "Failed to read backupram.\n");
+	}
+
+	/* create directory */
+	ab8500_regulator_dir = debugfs_create_dir("ab8500-regulator", NULL);
+	if (!ab8500_regulator_dir)
+		goto exit_no_debugfs;
+
+	/* create "dump" file */
+	ab8500_regulator_dump_file = debugfs_create_file("dump",
+		S_IRUGO, ab8500_regulator_dir, &plf->dev,
+		&ab8500_regulator_dump_fops);
+	if (!ab8500_regulator_dump_file)
+		goto exit_destroy_dir;
+
+	/* create "status" file */
+	ab8500_regulator_status_file = debugfs_create_file("status",
+		S_IRUGO, ab8500_regulator_dir, &plf->dev,
+		&ab8500_regulator_status_fops);
+	if (!ab8500_regulator_status_file)
+		goto exit_destroy_dump_file;
+
+	/*
+	 * create "suspend-force-v5x" file. As indicated by the name, this is
+	 * only applicable for v2_v5x hardware versions.
+	 */
+	ab8500_regulator_suspend_force_file = debugfs_create_file(
+		"suspend-force-v5x",
+		S_IRUGO, ab8500_regulator_dir, &plf->dev,
+		&ab8500_regulator_suspend_force_fops);
+	if (!ab8500_regulator_suspend_force_file)
+		goto exit_destroy_status_file;
+
+	return 0;
+
+exit_destroy_status_file:
+	debugfs_remove(ab8500_regulator_status_file);
+exit_destroy_dump_file:
+	debugfs_remove(ab8500_regulator_dump_file);
+exit_destroy_dir:
+	debugfs_remove(ab8500_regulator_dir);
+exit_no_debugfs:
+	dev_err(&plf->dev, "failed to create debugfs entries.\n");
+	return -ENOMEM;
+}
+
+static int __devexit ab8500_regulator_debug_remove(struct platform_device *plf)
+{
+	debugfs_remove(ab8500_regulator_suspend_force_file);
+	debugfs_remove(ab8500_regulator_status_file);
+	debugfs_remove(ab8500_regulator_dump_file);
+	debugfs_remove(ab8500_regulator_dir);
+
+	return 0;
+}
+
+static struct platform_driver ab8500_regulator_debug_driver = {
+	.driver = {
+		.name = "ab8500-regulator-debug",
+		.owner = THIS_MODULE,
+	},
+	.probe	= ab8500_regulator_debug_probe,
+	.remove	= __devexit_p(ab8500_regulator_debug_remove),
+};
+
+static int __init ab8500_regulator_debug_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&ab8500_regulator_debug_driver);
+	if (ret)
+		pr_err("Failed to register ab8500 regulator: %d\n", ret);
+
+	return ret;
+}
+subsys_initcall(ab8500_regulator_debug_init);
+
+static void __exit ab8500_regulator_debug_exit(void)
+{
+	platform_driver_unregister(&ab8500_regulator_debug_driver);
+}
+module_exit(ab8500_regulator_debug_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Bengt Jonsson <bengt.g.jonsson@stericsson.com");
+MODULE_DESCRIPTION("AB8500 Regulator Debug");
+MODULE_ALIAS("platform:ab8500-regulator-debug");
diff --git a/drivers/regulator/ab8500.c b/drivers/regulator/ab8500.c
index 02f3c2333c8..c96a0910d6e 100644
--- a/drivers/regulator/ab8500.c
+++ b/drivers/regulator/ab8500.c
@@ -20,43 +20,55 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/ab8500.h>
+#include <linux/mfd/ab8500/gpio.h> /* for sysclkreq pins */
+#include <mach/gpio.h> /* for sysclkreq pins */
 
 /**
  * struct ab8500_regulator_info - ab8500 regulator information
  * @dev: device pointer
  * @desc: regulator description
  * @regulator_dev: regulator device
+ * @is_enabled: status of regulator (on/off)
  * @max_uV: maximum voltage (for variable voltage supplies)
  * @min_uV: minimum voltage (for variable voltage supplies)
  * @fixed_uV: typical voltage (for fixed voltage supplies)
+ * @load_lp_uA: maximum load in idle (low power) mode
  * @update_bank: bank to control on/off
  * @update_reg: register to control on/off
- * @update_mask: mask to enable/disable regulator
- * @update_val_enable: bits to enable the regulator in normal (high power) mode
+ * @update_mask: mask to enable/disable and set mode of regulator
+ * @update_val: bits holding the regulator current mode
+ * @update_val_idle: bits to enable the regulator in idle (low power) mode
+ * @update_val_normal: bits to enable the regulator in normal (high power) mode
  * @voltage_bank: bank to control regulator voltage
  * @voltage_reg: register to control regulator voltage
  * @voltage_mask: mask to control regulator voltage
  * @voltages: supported voltage table
  * @voltages_len: number of supported voltages for the regulator
  * @delay: startup/set voltage delay in us
+ * @gpio_pin: ab8500 gpio pin offset number (for sysclkreq regulator only)
  */
 struct ab8500_regulator_info {
 	struct device		*dev;
 	struct regulator_desc	desc;
 	struct regulator_dev	*regulator;
+	bool is_enabled;
 	int max_uV;
 	int min_uV;
 	int fixed_uV;
+	int load_lp_uA;
 	u8 update_bank;
 	u8 update_reg;
 	u8 update_mask;
-	u8 update_val_enable;
+	u8 update_val;
+	u8 update_val_idle;
+	u8 update_val_normal;
 	u8 voltage_bank;
 	u8 voltage_reg;
 	u8 voltage_mask;
 	int const *voltages;
 	int voltages_len;
 	unsigned int delay;
+	unsigned int gpio_pin;
 };
 
 /* voltage tables for the vauxn/vintcore supplies */
@@ -112,15 +124,17 @@ static int ab8500_regulator_enable(struct regulator_dev *rdev)
 
 	ret = abx500_mask_and_set_register_interruptible(info->dev,
 		info->update_bank, info->update_reg,
-		info->update_mask, info->update_val_enable);
+		info->update_mask, info->update_val);
 	if (ret < 0)
 		dev_err(rdev_get_dev(rdev),
 			"couldn't set enable bits for regulator\n");
 
+	info->is_enabled = true;
+
 	dev_vdbg(rdev_get_dev(rdev),
 		"%s-enable (bank, reg, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
 		info->desc.name, info->update_bank, info->update_reg,
-		info->update_mask, info->update_val_enable);
+		info->update_mask, info->update_val);
 
 	return ret;
 }
@@ -142,6 +156,8 @@ static int ab8500_regulator_disable(struct regulator_dev *rdev)
 		dev_err(rdev_get_dev(rdev),
 			"couldn't set disable bits for regulator\n");
 
+	info->is_enabled = false;
+
 	dev_vdbg(rdev_get_dev(rdev),
 		"%s-disable (bank, reg, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
 		info->desc.name, info->update_bank, info->update_reg,
@@ -150,6 +166,88 @@ static int ab8500_regulator_disable(struct regulator_dev *rdev)
 	return ret;
 }
 
+static unsigned int ab8500_regulator_get_optimum_mode(
+		struct regulator_dev *rdev, int input_uV,
+		int output_uV, int load_uA)
+{
+	unsigned int mode;
+
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	if (load_uA <= info->load_lp_uA)
+		mode = REGULATOR_MODE_IDLE;
+	else
+		mode = REGULATOR_MODE_NORMAL;
+
+	return mode;
+}
+
+static int ab8500_regulator_set_mode(struct regulator_dev *rdev,
+				     unsigned int mode)
+{
+	int ret = 0;
+
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		info->update_val = info->update_val_normal;
+		break;
+	case REGULATOR_MODE_IDLE:
+		info->update_val = info->update_val_idle;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (info->is_enabled) {
+		ret = abx500_mask_and_set_register_interruptible(info->dev,
+			info->update_bank, info->update_reg,
+			info->update_mask, info->update_val);
+		if (ret < 0)
+			dev_err(rdev_get_dev(rdev),
+				"couldn't set regulator mode\n");
+
+		dev_vdbg(rdev_get_dev(rdev),
+			"%s-set_mode (bank, reg, mask, value): "
+			"0x%x, 0x%x, 0x%x, 0x%x\n",
+			info->desc.name, info->update_bank, info->update_reg,
+			info->update_mask, info->update_val);
+	}
+
+	return ret;
+}
+
+static unsigned int ab8500_regulator_get_mode(struct regulator_dev *rdev)
+{
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+	int ret;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	if (info->update_val == info->update_val_normal)
+		ret = REGULATOR_MODE_NORMAL;
+	else if (info->update_val == info->update_val_idle)
+		ret = REGULATOR_MODE_IDLE;
+	else
+		ret = -EINVAL;
+
+	return ret;
+}
+
 static int ab8500_regulator_is_enabled(struct regulator_dev *rdev)
 {
 	int ret;
@@ -176,9 +274,11 @@ static int ab8500_regulator_is_enabled(struct regulator_dev *rdev)
 		info->update_mask, regval);
 
 	if (regval & info->update_mask)
-		return true;
+		info->is_enabled = true;
 	else
-		return false;
+		info->is_enabled = false;
+
+	return info->is_enabled;
 }
 
 static int ab8500_list_voltage(struct regulator_dev *rdev, unsigned selector)
@@ -274,8 +374,12 @@ static int ab8500_regulator_set_voltage(struct regulator_dev *rdev,
 
 	*selector = ret;
 
+	if (info->desc.id == AB8500_LDO_INTCORE)
+		regval = ((u8)ret) << 3;
+	else
+		regval = (u8)ret;
+
 	/* set the registers for the request */
-	regval = (u8)ret;
 	ret = abx500_mask_and_set_register_interruptible(info->dev,
 			info->voltage_bank, info->voltage_reg,
 			info->voltage_mask, regval);
@@ -315,15 +419,18 @@ static int ab8500_regulator_set_voltage_time_sel(struct regulator_dev *rdev,
 	return info->delay;
 }
 
-static struct regulator_ops ab8500_regulator_ops = {
-	.enable		= ab8500_regulator_enable,
-	.disable	= ab8500_regulator_disable,
-	.is_enabled	= ab8500_regulator_is_enabled,
-	.get_voltage	= ab8500_regulator_get_voltage,
-	.set_voltage	= ab8500_regulator_set_voltage,
-	.list_voltage	= ab8500_list_voltage,
-	.enable_time	= ab8500_regulator_enable_time,
-	.set_voltage_time_sel = ab8500_regulator_set_voltage_time_sel,
+static struct regulator_ops ab8500_regulator_volt_mode_ops = {
+	.enable			= ab8500_regulator_enable,
+	.disable		= ab8500_regulator_disable,
+	.is_enabled		= ab8500_regulator_is_enabled,
+	.get_optimum_mode	= ab8500_regulator_get_optimum_mode,
+	.set_mode		= ab8500_regulator_set_mode,
+	.get_mode		= ab8500_regulator_get_mode,
+	.get_voltage		= ab8500_regulator_get_voltage,
+	.set_voltage		= ab8500_regulator_set_voltage,
+	.list_voltage		= ab8500_list_voltage,
+	.enable_time		= ab8500_regulator_enable_time,
+	.set_voltage_time_sel	= ab8500_regulator_set_voltage_time_sel,
 };
 
 static int ab8500_fixed_get_voltage(struct regulator_dev *rdev)
@@ -338,20 +445,121 @@ static int ab8500_fixed_get_voltage(struct regulator_dev *rdev)
 	return info->fixed_uV;
 }
 
-static struct regulator_ops ab8500_regulator_fixed_ops = {
-	.enable		= ab8500_regulator_enable,
-	.disable	= ab8500_regulator_disable,
-	.is_enabled	= ab8500_regulator_is_enabled,
+static struct regulator_ops ab8500_regulator_mode_ops = {
+	.enable			= ab8500_regulator_enable,
+	.disable		= ab8500_regulator_disable,
+	.is_enabled		= ab8500_regulator_is_enabled,
+	.get_optimum_mode	= ab8500_regulator_get_optimum_mode,
+	.set_mode		= ab8500_regulator_set_mode,
+	.get_mode		= ab8500_regulator_get_mode,
+	.get_voltage		= ab8500_fixed_get_voltage,
+	.list_voltage		= ab8500_list_voltage,
+	.enable_time		= ab8500_regulator_enable_time,
+	.set_voltage_time_sel	= ab8500_regulator_set_voltage_time_sel,
+};
+
+static struct regulator_ops ab8500_regulator_ops = {
+	.enable			= ab8500_regulator_enable,
+	.disable		= ab8500_regulator_disable,
+	.is_enabled		= ab8500_regulator_is_enabled,
+	.get_voltage		= ab8500_fixed_get_voltage,
+	.list_voltage		= ab8500_list_voltage,
+	.enable_time		= ab8500_regulator_enable_time,
+	.set_voltage_time_sel	= ab8500_regulator_set_voltage_time_sel,
+};
+
+static int ab8500_sysclkreq_enable(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	ret = ab8500_gpio_config_select(info->dev, info->gpio_pin, false);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"couldn't set sysclkreq pin selection\n");
+		return ret;
+	}
+
+	info->is_enabled = true;
+
+	dev_vdbg(rdev_get_dev(rdev),
+		"%s-enable (gpio_pin, gpio_select): %i, false\n",
+		info->desc.name, info->gpio_pin);
+
+	return ret;
+}
+
+static int ab8500_sysclkreq_disable(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	ret = ab8500_gpio_config_select(info->dev, info->gpio_pin, true);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"couldn't set gpio pin selection\n");
+		return ret;
+	}
+
+	info->is_enabled = false;
+
+	dev_vdbg(rdev_get_dev(rdev),
+		"%s-disable (gpio_pin, gpio_select): %i, true\n",
+		info->desc.name, info->gpio_pin);
+
+	return ret;
+}
+
+static int ab8500_sysclkreq_is_enabled(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+	bool gpio_select;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	ret = ab8500_gpio_config_get_select(info->dev, info->gpio_pin,
+		&gpio_select);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"couldn't read gpio pin selection\n");
+		return ret;
+	}
+
+	info->is_enabled = !gpio_select;
+
+	dev_vdbg(rdev_get_dev(rdev),
+		"%s-is_enabled (gpio_pin, is_enabled): %i, %i\n",
+		info->desc.name, info->gpio_pin, !gpio_select);
+
+	return info->is_enabled;
+}
+
+static struct regulator_ops ab8500_sysclkreq_ops = {
+	.enable		= ab8500_sysclkreq_enable,
+	.disable	= ab8500_sysclkreq_disable,
+	.is_enabled	= ab8500_sysclkreq_is_enabled,
 	.get_voltage	= ab8500_fixed_get_voltage,
 	.list_voltage	= ab8500_list_voltage,
-	.enable_time	= ab8500_regulator_enable_time,
-	.set_voltage_time_sel = ab8500_regulator_set_voltage_time_sel,
 };
 
 static struct ab8500_regulator_info
 		ab8500_regulator_info[AB8500_NUM_REGULATORS] = {
 	/*
-	 * Variable Voltage Regulators
+	 * Variable Voltage Regulators and normal/idle modes
 	 *   name, min mV, max mV,
 	 *   update bank, reg, mask, enable val
 	 *   volt bank, reg, mask, table, table length
@@ -359,7 +567,7 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_AUX1] = {
 		.desc = {
 			.name		= "LDO-AUX1",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_AUX1,
 			.owner		= THIS_MODULE,
@@ -367,10 +575,13 @@ static struct ab8500_regulator_info
 		},
 		.min_uV			= 1100000,
 		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x09,
 		.update_mask		= 0x03,
-		.update_val_enable	= 0x01,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x1f,
 		.voltage_mask		= 0x0f,
@@ -380,7 +591,7 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_AUX2] = {
 		.desc = {
 			.name		= "LDO-AUX2",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_AUX2,
 			.owner		= THIS_MODULE,
@@ -388,10 +599,13 @@ static struct ab8500_regulator_info
 		},
 		.min_uV			= 1100000,
 		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x09,
 		.update_mask		= 0x0c,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x20,
 		.voltage_mask		= 0x0f,
@@ -401,7 +615,7 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_AUX3] = {
 		.desc = {
 			.name		= "LDO-AUX3",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_AUX3,
 			.owner		= THIS_MODULE,
@@ -409,10 +623,13 @@ static struct ab8500_regulator_info
 		},
 		.min_uV			= 1100000,
 		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x0a,
 		.update_mask		= 0x03,
-		.update_val_enable	= 0x01,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x21,
 		.voltage_mask		= 0x07,
@@ -422,7 +639,7 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_INTCORE] = {
 		.desc = {
 			.name		= "LDO-INTCORE",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_INTCORE,
 			.owner		= THIS_MODULE,
@@ -430,10 +647,13 @@ static struct ab8500_regulator_info
 		},
 		.min_uV			= 1100000,
 		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x80,
 		.update_mask		= 0x44,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x44,
+		.update_val_idle	= 0x44,
+		.update_val_normal	= 0x04,
 		.voltage_bank		= 0x03,
 		.voltage_reg		= 0x80,
 		.voltage_mask		= 0x38,
@@ -442,14 +662,14 @@ static struct ab8500_regulator_info
 	},
 
 	/*
-	 * Fixed Voltage Regulators
+	 * Fixed Voltage Regulators and normal/idle modes
 	 *   name, fixed mV,
 	 *   update bank, reg, mask, enable val
 	 */
 	[AB8500_LDO_TVOUT] = {
 		.desc = {
 			.name		= "LDO-TVOUT",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_TVOUT,
 			.owner		= THIS_MODULE,
@@ -457,15 +677,18 @@ static struct ab8500_regulator_info
 		},
 		.delay			= 10000,
 		.fixed_uV		= 2000000,
+		.load_lp_uA		= 1000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x80,
 		.update_mask		= 0x82,
-		.update_val_enable	= 0x02,
+		.update_val		= 0x02,
+		.update_val_idle	= 0x82,
+		.update_val_normal	= 0x02,
 	},
 	[AB8500_LDO_USB] = {
 		.desc = {
 			.name           = "LDO-USB",
-			.ops            = &ab8500_regulator_fixed_ops,
+			.ops            = &ab8500_regulator_mode_ops,
 			.type           = REGULATOR_VOLTAGE,
 			.id             = AB8500_LDO_USB,
 			.owner          = THIS_MODULE,
@@ -475,12 +698,12 @@ static struct ab8500_regulator_info
 		.update_bank            = 0x03,
 		.update_reg             = 0x82,
 		.update_mask            = 0x03,
-		.update_val_enable      = 0x01,
+		.update_val		= 0x01,
 	},
 	[AB8500_LDO_AUDIO] = {
 		.desc = {
 			.name		= "LDO-AUDIO",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_AUDIO,
 			.owner		= THIS_MODULE,
@@ -490,12 +713,12 @@ static struct ab8500_regulator_info
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x02,
-		.update_val_enable	= 0x02,
+		.update_val		= 0x02,
 	},
 	[AB8500_LDO_ANAMIC1] = {
 		.desc = {
 			.name		= "LDO-ANAMIC1",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_ANAMIC1,
 			.owner		= THIS_MODULE,
@@ -505,12 +728,12 @@ static struct ab8500_regulator_info
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x08,
-		.update_val_enable	= 0x08,
+		.update_val		= 0x08,
 	},
 	[AB8500_LDO_ANAMIC2] = {
 		.desc = {
 			.name		= "LDO-ANAMIC2",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_ANAMIC2,
 			.owner		= THIS_MODULE,
@@ -520,12 +743,12 @@ static struct ab8500_regulator_info
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x10,
-		.update_val_enable	= 0x10,
+		.update_val		= 0x10,
 	},
 	[AB8500_LDO_DMIC] = {
 		.desc = {
 			.name		= "LDO-DMIC",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_DMIC,
 			.owner		= THIS_MODULE,
@@ -535,25 +758,58 @@ static struct ab8500_regulator_info
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x04,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x04,
 	},
+
+	/*
+	 * Regulators with fixed voltage and normal/idle modes
+	 */
 	[AB8500_LDO_ANA] = {
 		.desc = {
 			.name		= "LDO-ANA",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_ANA,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
 		},
 		.fixed_uV		= 1200000,
+		.load_lp_uA		= 1000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x06,
 		.update_mask		= 0x0c,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
 	},
 
-
+	/*
+	 * SysClkReq regulators
+	 */
+	[AB8500_SYSCLKREQ_2] = {
+		.desc = {
+			.name		= "SYSCLKREQ-2",
+			.ops		= &ab8500_sysclkreq_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_SYSCLKREQ_2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 1, /* bogus value */
+		.gpio_pin		= 0, /* AB8500_PIN_GPIO1 */
+	},
+	[AB8500_SYSCLKREQ_4] = {
+		.desc = {
+			.name		= "SYSCLKREQ-4",
+			.ops		= &ab8500_sysclkreq_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_SYSCLKREQ_4,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 1, /* bogus value */
+		.gpio_pin		= 2, /* AB8500_PIN_GPIO3 */
+	},
 };
 
 struct ab8500_reg_init {
@@ -572,7 +828,7 @@ struct ab8500_reg_init {
 static struct ab8500_reg_init ab8500_reg_init[] = {
 	/*
 	 * 0x30, VanaRequestCtrl
-	 * 0x0C, VpllRequestCtrl
+	 * 0x0c, VpllRequestCtrl
 	 * 0xc0, VextSupply1RequestCtrl
 	 */
 	REG_INIT(AB8500_REGUREQUESTCTRL2,	0x03, 0x04, 0xfc),
@@ -589,12 +845,16 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	 */
 	REG_INIT(AB8500_REGUREQUESTCTRL4,	0x03, 0x06, 0x07),
 	/*
+	 * 0x01, Vsmps1SysClkReq1HPValid
+	 * 0x02, Vsmps2SysClkReq1HPValid
+	 * 0x04, Vsmps3SysClkReq1HPValid
 	 * 0x08, VanaSysClkReq1HPValid
+	 * 0x10, VpllSysClkReq1HPValid
 	 * 0x20, Vaux1SysClkReq1HPValid
 	 * 0x40, Vaux2SysClkReq1HPValid
 	 * 0x80, Vaux3SysClkReq1HPValid
 	 */
-	REG_INIT(AB8500_REGUSYSCLKREQ1HPVALID1,	0x03, 0x07, 0xe8),
+	REG_INIT(AB8500_REGUSYSCLKREQ1HPVALID1,	0x03, 0x07, 0xff),
 	/*
 	 * 0x10, VextSupply1SysClkReq1HPValid
 	 * 0x20, VextSupply2SysClkReq1HPValid
@@ -673,6 +933,40 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	 */
 	REG_INIT(AB8500_REGUCTRL1VAMIC,		0x03, 0x84, 0x03),
 	/*
+	 * 0x0c, VBBNRegu
+	 * 0x03, VBBPRegu
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB8500_ARMREGU2,		0x04, 0x01, 0x0f),
+	/*
+	 * 0x0c, VBBPSel1
+	 * 0x03, VBBNSel1
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB8500_VBBSEL1,		0x04, 0x11, 0x0f),
+	/*
+	 * 0x0c, VBBNSel2
+	 * 0x03, VBBPSel2
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB8500_VBBSEL2,		0x04, 0x12, 0x0f),
+	/*
+	 * 0x03, Vsmps1Regu
+	 * 0x0c, Vsmps1SelCtrl
+	 */
+	REG_INIT(AB8500_VSMPS1REGU,		0x04, 0x03, 0x0f),
+	/*
+	 * 0x03, Vsmps2Regu
+	 * 0x0c, Vsmps2SelCtrl
+	 */
+	REG_INIT(AB8500_VSMPS2REGU,		0x04, 0x04, 0x0f),
+	/*
+	 * 0x03, Vsmps3Regu
+	 * 0x0c, Vsmps3SelCtrl
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB8500_VSMPS3REGU,		0x04, 0x05, 0x0f),
+	/*
 	 * 0x0c, VanaRegu
 	 * 0x03, VpllRegu
 	 */
@@ -696,14 +990,23 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	 */
 	REG_INIT(AB8500_VAUX12REGU,		0x04, 0x09, 0x0f),
 	/*
+	 * 0x0c, Vrf1Regu
 	 * 0x03, Vaux3Regu
 	 */
-	REG_INIT(AB8500_VRF1VAUX3REGU,		0x04, 0x0a, 0x03),
+	REG_INIT(AB8500_VRF1VAUX3REGU,		0x04, 0x0a, 0x0f),
 	/*
 	 * 0x3f, Vsmps1Sel1
 	 */
 	REG_INIT(AB8500_VSMPS1SEL1,		0x04, 0x13, 0x3f),
 	/*
+	 * 0x7f, Vsmps3Sel1
+	 */
+	REG_INIT(AB8500_VSMPS3SEL1,             0x04, 0x1b, 0x7f),
+	/*
+	 * 0x7f, Vsmps3Sel2
+	 */
+	REG_INIT(AB8500_VSMPS3SEL2,             0x04, 0x1c, 0x7f),
+	/*
 	 * 0x0f, Vaux1Sel
 	 */
 	REG_INIT(AB8500_VAUX1SEL,		0x04, 0x1f, 0x0f),
@@ -799,6 +1102,24 @@ static __devinit int ab8500_regulator_probe(struct platform_device *pdev)
 			value);
 	}
 
+	/*
+	 * Remove below when ab8500v2.0 is no longer important.
+	 * Below only affects power consumption and is depending on the
+	 * HREF OTP configuration.
+	 * It changes the default setting for VextSupply3Regu to Low Power.
+	 * Active high or low is depending on OTP which is changed from ab8500v3.0.
+	 */
+	if (abx500_get_chip_id(&pdev->dev) < 0x30) {
+		err = abx500_mask_and_set_register_interruptible(&pdev->dev,
+			AB8500_REGU_CTRL2, 0x08, 0x30, 0x30);
+		if (err < 0) {
+			dev_err(&pdev->dev,
+				"Failed to override 0x%02x, 0x%02x.\n",
+				AB8500_REGU_CTRL2, 0x08);
+			return err;
+		}
+	}
+
 	/* register all regulators */
 	for (i = 0; i < ARRAY_SIZE(ab8500_regulator_info); i++) {
 		struct ab8500_regulator_info *info = NULL;
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index d3e38790906..5cdcb615a68 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -78,6 +78,7 @@ struct regulator {
 	char *supply_name;
 	struct device_attribute dev_attr;
 	struct regulator_dev *rdev;
+	int use;
 };
 
 static int _regulator_is_enabled(struct regulator_dev *rdev);
@@ -87,7 +88,7 @@ static int _regulator_get_voltage(struct regulator_dev *rdev);
 static int _regulator_get_current_limit(struct regulator_dev *rdev);
 static unsigned int _regulator_get_mode(struct regulator_dev *rdev);
 static void _notifier_call_chain(struct regulator_dev *rdev,
-				  unsigned long event, void *data);
+				  unsigned long event, void *data, int lock_sublevel);
 static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 				     int min_uV, int max_uV);
 
@@ -164,11 +165,13 @@ static int regulator_check_consumers(struct regulator_dev *rdev,
 		 */
 		if (!regulator->min_uV && !regulator->max_uV)
 			continue;
-
-		if (*max_uV > regulator->max_uV)
-			*max_uV = regulator->max_uV;
-		if (*min_uV < regulator->min_uV)
-			*min_uV = regulator->min_uV;
+		
+		if (regulator->use) {
+			if (*max_uV > regulator->max_uV)
+				*max_uV = regulator->max_uV;
+			if (*min_uV < regulator->min_uV)
+				*min_uV = regulator->min_uV;
+		}
 	}
 
 	if (*min_uV > *max_uV)
@@ -563,6 +566,32 @@ static ssize_t regulator_suspend_standby_state_show(struct device *dev,
 static DEVICE_ATTR(suspend_standby_state, 0444,
 		regulator_suspend_standby_state_show, NULL);
 
+static ssize_t regulator_use_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct regulator_dev *rdev = dev_get_drvdata(dev);
+	struct regulator *reg;
+	size_t size = 0;
+
+	if (rdev->use_count == 0)
+		return sprintf(buf, "no users\n");
+
+	list_for_each_entry(reg, &rdev->consumer_list, list) {
+		if (!reg->use)
+			continue;
+
+		if (reg->dev != NULL)
+			size += sprintf((buf + size), "%s (%d) ",
+					dev_name(reg->dev), reg->use);
+		else
+			size += sprintf((buf + size), "unknown (%d) ",
+					reg->use);
+	}
+	size += sprintf((buf + size), "\n");
+
+	return size;
+}
+static DEVICE_ATTR(use, 0444, regulator_use_show, NULL);
 
 /*
  * These are the only attributes are present for all regulators.
@@ -1391,6 +1420,9 @@ int regulator_enable(struct regulator *regulator)
 	mutex_lock(&rdev->mutex);
 	ret = _regulator_enable(rdev);
 	mutex_unlock(&rdev->mutex);
+	if (ret == 0)
+		regulator->use++;
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(regulator_enable);
@@ -1424,7 +1456,7 @@ static int _regulator_disable(struct regulator_dev *rdev,
 			trace_regulator_disable_complete(rdev_get_name(rdev));
 
 			_notifier_call_chain(rdev, REGULATOR_EVENT_DISABLE,
-					     NULL);
+					     NULL, 0);
 		}
 
 		/* decrease our supplies ref count and disable if required */
@@ -1474,6 +1506,9 @@ int regulator_disable(struct regulator *regulator)
 		mutex_unlock(&rdev->mutex);
 	}
 
+	if (ret == 0)
+		regulator->use--;
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(regulator_disable);
@@ -1494,7 +1529,7 @@ static int _regulator_force_disable(struct regulator_dev *rdev,
 		}
 		/* notify other consumers that power has been forced off */
 		_notifier_call_chain(rdev, REGULATOR_EVENT_FORCE_DISABLE |
-			REGULATOR_EVENT_DISABLE, NULL);
+			REGULATOR_EVENT_DISABLE, NULL, 0);
 	}
 
 	/* decrease our supplies ref count and disable if required */
@@ -1720,7 +1755,7 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 
 	if (ret == 0)
 		_notifier_call_chain(rdev, REGULATOR_EVENT_VOLTAGE_CHANGE,
-				     NULL);
+				     NULL, 0);
 
 	trace_regulator_set_voltage_complete(rdev_get_name(rdev), selector);
 
@@ -2202,19 +2237,23 @@ EXPORT_SYMBOL_GPL(regulator_unregister_notifier);
 
 /* notify regulator consumers and downstream regulator consumers.
  * Note mutex must be held by caller.
+ * lock_sublevel should always be 0, only used for recursive calls.
  */
 static void _notifier_call_chain(struct regulator_dev *rdev,
-				  unsigned long event, void *data)
+				  unsigned long event, void *data, int lock_sublevel)
 {
 	struct regulator_dev *_rdev;
 
 	/* call rdev chain first */
 	blocking_notifier_call_chain(&rdev->notifier, event, NULL);
 
+	/* increase sublevel before stepping into nested regulators */
+	lock_sublevel++;
+
 	/* now notify regulator we supply */
 	list_for_each_entry(_rdev, &rdev->supply_list, slist) {
-		mutex_lock(&_rdev->mutex);
-		_notifier_call_chain(_rdev, event, data);
+		mutex_lock_nested(&_rdev->mutex, lock_sublevel);
+		_notifier_call_chain(_rdev, event, data, lock_sublevel);
 		mutex_unlock(&_rdev->mutex);
 	}
 }
@@ -2368,7 +2407,7 @@ EXPORT_SYMBOL_GPL(regulator_bulk_free);
 int regulator_notifier_call_chain(struct regulator_dev *rdev,
 				  unsigned long event, void *data)
 {
-	_notifier_call_chain(rdev, event, data);
+	_notifier_call_chain(rdev, event, data, 0);
 	return NOTIFY_DONE;
 
 }
@@ -2408,6 +2447,10 @@ static int add_regulator_attributes(struct regulator_dev *rdev)
 	struct regulator_ops	*ops = rdev->desc->ops;
 	int			status = 0;
 
+	status = device_create_file(dev, &dev_attr_use);
+	if (status < 0)
+		dev_warn(dev, "Create sysfs file \"use\" failed");
+
 	/* some attributes need specific methods to be displayed */
 	if (ops->get_voltage || ops->get_voltage_sel) {
 		status = device_create_file(dev, &dev_attr_microvolts);
diff --git a/drivers/regulator/db5500-prcmu.c b/drivers/regulator/db5500-prcmu.c
new file mode 100644
index 00000000000..d52f459fc3c
--- /dev/null
+++ b/drivers/regulator/db5500-prcmu.c
@@ -0,0 +1,351 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Authors: Sundar Iyer <sundar.iyer@stericsson.com> for ST-Ericsson
+ *          Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson
+ *
+ * Power domain regulators on DB5500
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/db5500-prcmu.h>
+
+#include <mach/prcmu.h>
+
+/**
+ * struct db5500_regulator_info - db5500 regulator information
+ * @dev: device pointer
+ * @desc: regulator description
+ * @rdev: regulator device pointer
+ * @is_enabled: status of the regulator
+ * @epod_id: id for EPOD (power domain)
+ * @is_ramret: RAM retention switch for EPOD (power domain)
+ * @operating_point: operating point (only for vape, to be removed)
+ *
+ */
+struct db5500_regulator_info {
+	struct device *dev;
+	struct regulator_desc desc;
+	struct regulator_dev *rdev;
+	bool is_enabled;
+	u16 epod_id;
+	bool is_ramret;
+	bool exclude_from_power_state;
+	unsigned int operating_point;
+};
+
+static int db5500_regulator_enable(struct regulator_dev *rdev)
+{
+	struct db5500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev), "regulator-%s-enable\n",
+		info->desc.name);
+
+	info->is_enabled = true;
+	if (!info->exclude_from_power_state)
+		power_state_active_enable();
+
+	return 0;
+}
+
+static int db5500_regulator_disable(struct regulator_dev *rdev)
+{
+	struct db5500_regulator_info *info = rdev_get_drvdata(rdev);
+	int ret = 0;
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev), "regulator-%s-disable\n",
+		info->desc.name);
+
+	info->is_enabled = false;
+	if (!info->exclude_from_power_state)
+		ret = power_state_active_disable();
+
+	return ret;
+}
+
+static int db5500_regulator_is_enabled(struct regulator_dev *rdev)
+{
+	struct db5500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev), "regulator-%s-is_enabled (is_enabled):"
+		" %i\n", info->desc.name, info->is_enabled);
+
+	return info->is_enabled;
+}
+
+/* db5500 regulator operations */
+static struct regulator_ops db5500_regulator_ops = {
+	.enable			= db5500_regulator_enable,
+	.disable		= db5500_regulator_disable,
+	.is_enabled		= db5500_regulator_is_enabled,
+};
+
+/*
+ * EPOD control
+ */
+static bool epod_on[NUM_EPOD_ID];
+static bool epod_ramret[NUM_EPOD_ID];
+
+static inline int epod_id_to_index(u16 epod_id)
+{
+	return epod_id - DB5500_EPOD_ID_BASE;
+}
+
+static int enable_epod(u16 epod_id, bool ramret)
+{
+	int idx = epod_id_to_index(epod_id);
+	int ret;
+
+	if (ramret) {
+		if (!epod_on[idx]) {
+			ret = prcmu_set_epod(epod_id, EPOD_STATE_RAMRET);
+			if (ret < 0)
+				return ret;
+		}
+		epod_ramret[idx] = true;
+	} else {
+		ret = prcmu_set_epod(epod_id, EPOD_STATE_ON);
+		if (ret < 0)
+			return ret;
+		epod_on[idx] = true;
+	}
+
+	return 0;
+}
+
+static int disable_epod(u16 epod_id, bool ramret)
+{
+	int idx = epod_id_to_index(epod_id);
+	int ret;
+
+	if (ramret) {
+		if (!epod_on[idx]) {
+			ret = prcmu_set_epod(epod_id, EPOD_STATE_OFF);
+			if (ret < 0)
+				return ret;
+		}
+		epod_ramret[idx] = false;
+	} else {
+		if (epod_ramret[idx]) {
+			ret = prcmu_set_epod(epod_id, EPOD_STATE_RAMRET);
+			if (ret < 0)
+				return ret;
+		} else {
+			ret = prcmu_set_epod(epod_id, EPOD_STATE_OFF);
+			if (ret < 0)
+				return ret;
+		}
+		epod_on[idx] = false;
+	}
+
+	return 0;
+}
+
+/*
+ * Regulator switch
+ */
+static int db5500_regulator_switch_enable(struct regulator_dev *rdev)
+{
+	struct db5500_regulator_info *info = rdev_get_drvdata(rdev);
+	int ret;
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev), "regulator-switch-%s-enable\n",
+		info->desc.name);
+
+	ret = enable_epod(info->epod_id, info->is_ramret);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"regulator-switch-%s-enable: prcmu call failed\n",
+			info->desc.name);
+		goto out;
+	}
+
+	info->is_enabled = true;
+out:
+	return ret;
+}
+
+static int db5500_regulator_switch_disable(struct regulator_dev *rdev)
+{
+	struct db5500_regulator_info *info = rdev_get_drvdata(rdev);
+	int ret;
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev), "regulator-switch-%s-disable\n",
+		info->desc.name);
+
+	ret = disable_epod(info->epod_id, info->is_ramret);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"regulator_switch-%s-disable: prcmu call failed\n",
+			info->desc.name);
+		goto out;
+	}
+
+	info->is_enabled = 0;
+out:
+	return ret;
+}
+
+static int db5500_regulator_switch_is_enabled(struct regulator_dev *rdev)
+{
+	struct db5500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev),
+		"regulator-switch-%s-is_enabled (is_enabled): %i\n",
+		info->desc.name, info->is_enabled);
+
+	return info->is_enabled;
+}
+
+static struct regulator_ops db5500_regulator_switch_ops = {
+	.enable			= db5500_regulator_switch_enable,
+	.disable		= db5500_regulator_switch_disable,
+	.is_enabled		= db5500_regulator_switch_is_enabled,
+};
+
+/*
+ * Regulator information
+ */
+#define DB5500_REGULATOR_SWITCH(_name, reg)                               \
+	[DB5500_REGULATOR_SWITCH_##reg] = {                               \
+		.desc = {                                               \
+			.name   = _name,                                \
+			.id     = DB5500_REGULATOR_SWITCH_##reg,          \
+			.ops    = &db5500_regulator_switch_ops,          \
+			.type   = REGULATOR_VOLTAGE,                    \
+			.owner  = THIS_MODULE,                          \
+		},                                                      \
+		.epod_id = DB5500_EPOD_ID_##reg,                         \
+}
+
+static struct db5500_regulator_info
+		db5500_regulator_info[DB5500_NUM_REGULATORS] = {
+	[DB5500_REGULATOR_VAPE] = {
+		.desc = {
+			.name	= "db5500-vape",
+			.id	= DB5500_REGULATOR_VAPE,
+			.ops	= &db5500_regulator_ops,
+			.type	= REGULATOR_VOLTAGE,
+			.owner	= THIS_MODULE,
+		},
+	},
+	DB5500_REGULATOR_SWITCH("db5500-sga", SGA),
+	DB5500_REGULATOR_SWITCH("db5500-hva", HVA),
+	DB5500_REGULATOR_SWITCH("db5500-sia", SIA),
+	DB5500_REGULATOR_SWITCH("db5500-disp", DISP),
+	DB5500_REGULATOR_SWITCH("db5500-esram12", ESRAM12),
+};
+
+static int __devinit db5500_regulator_probe(struct platform_device *pdev)
+{
+	struct regulator_init_data *db5500_init_data =
+					dev_get_platdata(&pdev->dev);
+	int i, err;
+
+	/* register all regulators */
+	for (i = 0; i < ARRAY_SIZE(db5500_regulator_info); i++) {
+		struct db5500_regulator_info *info;
+		struct regulator_init_data *init_data = &db5500_init_data[i];
+
+		/* assign per-regulator data */
+		info = &db5500_regulator_info[i];
+		info->dev = &pdev->dev;
+
+		/* register with the regulator framework */
+		info->rdev = regulator_register(&info->desc, &pdev->dev,
+				init_data, info);
+		if (IS_ERR(info->rdev)) {
+			err = PTR_ERR(info->rdev);
+			dev_err(&pdev->dev, "failed to register %s: err %i\n",
+				info->desc.name, err);
+
+			/* if failing, unregister all earlier regulators */
+			i--;
+			while (i >= 0) {
+				info = &db5500_regulator_info[i];
+				regulator_unregister(info->rdev);
+				i--;
+			}
+			return err;
+		}
+
+		dev_dbg(rdev_get_dev(info->rdev),
+			"regulator-%s-probed\n", info->desc.name);
+	}
+
+	return 0;
+}
+
+static int __exit db5500_regulator_remove(struct platform_device *pdev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(db5500_regulator_info); i++) {
+		struct db5500_regulator_info *info;
+		info = &db5500_regulator_info[i];
+
+		dev_vdbg(rdev_get_dev(info->rdev),
+			"regulator-%s-remove\n", info->desc.name);
+
+		regulator_unregister(info->rdev);
+	}
+
+	return 0;
+}
+
+static struct platform_driver db5500_regulator_driver = {
+	.driver = {
+		.name = "db5500-prcmu-regulators",
+		.owner = THIS_MODULE,
+	},
+	.probe = db5500_regulator_probe,
+	.remove = __exit_p(db5500_regulator_remove),
+};
+
+static int __init db5500_regulator_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&db5500_regulator_driver);
+	if (ret < 0)
+		return -ENODEV;
+
+	return 0;
+}
+
+static void __exit db5500_regulator_exit(void)
+{
+	platform_driver_unregister(&db5500_regulator_driver);
+}
+
+arch_initcall(db5500_regulator_init);
+module_exit(db5500_regulator_exit);
+
+MODULE_AUTHOR("STMicroelectronics/ST-Ericsson");
+MODULE_DESCRIPTION("DB5500 regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/db8500-prcmu.c b/drivers/regulator/db8500-prcmu.c
index 2bb8f451cc0..99cd20514e2 100644
--- a/drivers/regulator/db8500-prcmu.c
+++ b/drivers/regulator/db8500-prcmu.c
@@ -13,52 +13,11 @@
 #include <linux/err.h>
 #include <linux/spinlock.h>
 #include <linux/platform_device.h>
-#include <linux/mfd/db8500-prcmu.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/db8500-prcmu.h>
 
-/*
- * power state reference count
- */
-static int power_state_active_cnt; /* will initialize to zero */
-static DEFINE_SPINLOCK(power_state_active_lock);
-
-static void power_state_active_enable(void)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&power_state_active_lock, flags);
-	power_state_active_cnt++;
-	spin_unlock_irqrestore(&power_state_active_lock, flags);
-}
-
-static int power_state_active_disable(void)
-{
-	int ret = 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&power_state_active_lock, flags);
-	if (power_state_active_cnt <= 0) {
-		pr_err("power state: unbalanced enable/disable calls\n");
-		ret = -EINVAL;
-		goto out;
-	}
-
-	power_state_active_cnt--;
-out:
-	spin_unlock_irqrestore(&power_state_active_lock, flags);
-	return ret;
-}
-
-/*
- * Exported interface for CPUIdle only. This function is called when interrupts
- * are turned off. Hence, no locking.
- */
-int power_state_active_is_enabled(void)
-{
-	return (power_state_active_cnt > 0);
-}
+#include <mach/prcmu.h>
 
 /**
  * struct db8500_regulator_info - db8500 regulator information
diff --git a/drivers/regulator/dbx500-prcmu.c b/drivers/regulator/dbx500-prcmu.c
new file mode 100644
index 00000000000..1ee6f6a7c4a
--- /dev/null
+++ b/drivers/regulator/dbx500-prcmu.c
@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Authors: Sundar Iyer <sundar.iyer@stericsson.com> for ST-Ericsson
+ *          Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson
+ *
+ * UX500 common part of Power domain regulators (atomic regulators)
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/dbx500-prcmu.h>
+
+/*
+ * power state reference count
+ */
+static int power_state_active_cnt; /* will initialize to zero */
+static DEFINE_SPINLOCK(power_state_active_lock);
+
+void power_state_active_enable(void)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&power_state_active_lock, flags);
+	power_state_active_cnt++;
+	spin_unlock_irqrestore(&power_state_active_lock, flags);
+}
+EXPORT_SYMBOL_GPL(power_state_active_enable);
+
+int power_state_active_disable(void)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&power_state_active_lock, flags);
+	if (power_state_active_cnt <= 0) {
+		pr_err("power state: unbalanced enable/disable calls\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	power_state_active_cnt--;
+out:
+	spin_unlock_irqrestore(&power_state_active_lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(power_state_active_disable);
+
+/*
+ * Exported interface for CPUIdle only. This function is called when interrupts
+ * are turned off. Hence, no locking.
+ */
+int power_state_active_is_enabled(void)
+{
+	return (power_state_active_cnt > 0);
+}
+EXPORT_SYMBOL_GPL(power_state_active_is_enabled);
+
+struct ux500_regulator {
+	char *name;
+	void (*enable)(void);
+	int (*disable)(void);
+};
+
+/*
+ * Don't add any clients to this struct without checking with regulator
+ * responsible!
+ */
+static struct ux500_regulator ux500_atomic_regulators[] = {
+	{
+		.name    = "dma40.0",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "ssp0",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "ssp1",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "spi0",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "spi1",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "spi2",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "spi3",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "cryp1",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "hash1",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+};
+
+struct ux500_regulator *__must_check ux500_regulator_get(struct device *dev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ux500_atomic_regulators); i++) {
+		if (!strcmp(dev_name(dev), ux500_atomic_regulators[i].name))
+			return &ux500_atomic_regulators[i];
+	}
+
+	return  ERR_PTR(-EINVAL);
+}
+EXPORT_SYMBOL_GPL(ux500_regulator_get);
+
+int ux500_regulator_atomic_enable(struct ux500_regulator *regulator)
+{
+	if (regulator) {
+		regulator->enable();
+		return 0;
+	}
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(ux500_regulator_atomic_enable);
+
+int ux500_regulator_atomic_disable(struct ux500_regulator *regulator)
+{
+	if (regulator)
+		return regulator->disable();
+	else
+		return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(ux500_regulator_atomic_disable);
+
+void ux500_regulator_put(struct ux500_regulator *regulator)
+{
+	/* Here for symetric reasons and for possible future use */
+}
+EXPORT_SYMBOL_GPL(ux500_regulator_put);
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 27c37743e2c..227dd734e56 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -715,6 +715,13 @@ config RTC_DRV_PCF50633
 	  If you say yes here you get support for the RTC subsystem of the
 	  NXP PCF50633 used in embedded systems.
 
+config RTC_DRV_AB
+	tristate "ST-Ericsson AB5500 RTC"
+	depends on AB5500_CORE
+	help
+	  Select this to enable the ST-Ericsson AB5500 Mixed Signal IC RTC
+	  support. This chip contains a battery- and capacitor-backed RTC.
+
 config RTC_DRV_AB3100
 	tristate "ST-Ericsson AB3100 RTC"
 	depends on AB3100_CORE
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 7d279581043..4403ee3f75f 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -18,6 +18,7 @@ rtc-core-$(CONFIG_RTC_INTF_SYSFS) += rtc-sysfs.o
 # Keep the list ordered.
 
 obj-$(CONFIG_RTC_DRV_88PM860X)  += rtc-88pm860x.o
+obj-$(CONFIG_RTC_DRV_AB)	+= rtc-ab.o
 obj-$(CONFIG_RTC_DRV_AB3100)	+= rtc-ab3100.o
 obj-$(CONFIG_RTC_DRV_AB8500)	+= rtc-ab8500.o
 obj-$(CONFIG_RTC_DRV_AT32AP700X)+= rtc-at32ap700x.o
diff --git a/drivers/rtc/rtc-ab.c b/drivers/rtc/rtc-ab.c
new file mode 100644
index 00000000000..8e595e05d99
--- /dev/null
+++ b/drivers/rtc/rtc-ab.c
@@ -0,0 +1,482 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ * Author: Rabin Vincent <rabin.vincent@stericsson.com>
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/mfd/abx500.h>
+
+#define AB5500_RTC_CLOCK_RATE	32768
+#define AB5500_RTC		0x00
+#define AB5500_RTC_ALARM	(1 << 1)
+#define AB5500_READREQ		0x01
+#define AB5500_READREQ_REQ	0x01
+#define AB5500_AL0		0x02
+#define AB5500_TI0		0x06
+
+/**
+ * struct ab_rtc - variant specific data
+ * @irqname: optional name for the alarm interrupt resource
+ * @epoch: epoch to adjust year to
+ * @bank: AB bank where this block is present
+ * @rtc: address of the "RTC" (control) register
+ * @rtc_alarmon: mask of the alarm enable bit in the above register
+ * @ti0: address of the TI0 register.  The rest of the TI
+ * registers are assumed to contiguously follow this one.
+ * @nr_ti: number of TI* registers
+ * @al0: address of the AL0 register.  The rest of the
+ * AL registers are assumed to contiguously follow this one.
+ * @nr_al: number of AL* registers
+ * @startup: optional function to initialize the RTC
+ * @alarm_to_regs: function to convert alarm time in seconds
+ * to a list of AL register values
+ * @time_to_regs: function to convert alarm time in seconds
+ * to a list of TI register values
+ * @regs_to_alarm: function to convert a list of AL register
+ * values to the alarm time in seconds
+ * @regs_to_time: function to convert a list of TI register
+ * values to the alarm time in seconds
+ * @request_read: optional function to request a read from the TI* registers
+ * @request_write: optional function to request a write to the TI* registers
+ */
+struct ab_rtc {
+	const char *irqname;
+	unsigned int epoch;
+
+	u8 bank;
+	u8 rtc;
+	u8 rtc_alarmon;
+	u8 ti0;
+	int nr_ti;
+	u8 al0;
+	int nr_al;
+
+	int (*startup)(struct device *dev);
+	void (*alarm_to_regs)(struct device *dev, unsigned long secs, u8 *regs);
+	void (*time_to_regs)(struct device *dev, unsigned long secs, u8 *regs);
+	unsigned long (*regs_to_alarm)(struct device *dev, u8 *regs);
+	unsigned long (*regs_to_time)(struct device *dev, u8 *regs);
+	int (*request_read)(struct device *dev);
+	int (*request_write)(struct device *dev);
+};
+
+static const struct ab_rtc *to_ab_rtc(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	return (struct ab_rtc *)pdev->id_entry->driver_data;
+}
+
+/* Calculate the number of seconds since year, for epoch adjustment */
+static unsigned long ab_rtc_get_elapsed_seconds(unsigned int year)
+{
+	unsigned long secs;
+	struct rtc_time tm = {
+		.tm_year = year - 1900,
+		.tm_mday = 1,
+	};
+
+	rtc_tm_to_time(&tm, &secs);
+
+	return secs;
+}
+
+static int ab5500_rtc_request_read(struct device *dev)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	unsigned long timeout;
+	int err;
+
+	err = abx500_set_register_interruptible(dev, variant->bank,
+						AB5500_READREQ,
+						AB5500_READREQ_REQ);
+	if (err < 0)
+		return err;
+
+	timeout = jiffies + HZ;
+	while (time_before(jiffies, timeout)) {
+		u8 value;
+
+		err = abx500_get_register_interruptible(dev, variant->bank,
+				AB5500_READREQ, &value);
+		if (err < 0)
+			return err;
+
+		if (!(value & AB5500_READREQ_REQ))
+			return 0;
+
+		msleep(1);
+	}
+
+	return -EIO;
+}
+
+static void
+ab5500_rtc_time_to_regs(struct device *dev, unsigned long secs, u8 *regs)
+{
+	unsigned long mins = secs / 60;
+	u64 fat_time;
+
+	secs %= 60;
+
+	fat_time = secs * AB5500_RTC_CLOCK_RATE;
+	fat_time |= (u64)mins << 21;
+
+	regs[0] = (fat_time) & 0xFF;
+	regs[1] = (fat_time >> 8) & 0xFF;
+	regs[2] = (fat_time >> 16) & 0xFF;
+	regs[3] = (fat_time >> 24) & 0xFF;
+	regs[4] = (fat_time >> 32) & 0xFF;
+	regs[5] = (fat_time >> 40) & 0xFF;
+}
+
+static unsigned long
+ab5500_rtc_regs_to_time(struct device *dev, u8 *regs)
+{
+	u64 fat_time = ((u64)regs[5] << 40) | ((u64)regs[4] << 32) |
+		       ((u64)regs[3] << 24) | ((u64)regs[2] << 16) |
+		       ((u64)regs[1] << 8)  | regs[0];
+	unsigned long secs = (fat_time & 0x1fffff) / AB5500_RTC_CLOCK_RATE;
+	unsigned long mins = fat_time >> 21;
+
+	return mins * 60 + secs;
+}
+
+static void
+ab5500_rtc_alarm_to_regs(struct device *dev, unsigned long secs, u8 *regs)
+{
+	unsigned long mins = secs / 60;
+
+#ifdef CONFIG_ANDROID
+	/*
+	 * Needed because Android believes all hw have a wake-up resolution in
+	 * seconds.
+	 */
+	mins++;
+#endif
+
+	regs[0] = mins & 0xFF;
+	regs[1] = (mins >> 8) & 0xFF;
+	regs[2] = (mins >> 16) & 0xFF;
+}
+
+static unsigned long
+ab5500_rtc_regs_to_alarm(struct device *dev, u8 *regs)
+{
+	unsigned long mins = ((unsigned long)regs[2] << 16) |
+			     ((unsigned long)regs[1] << 8) |
+			     regs[0];
+	unsigned long secs = mins * 60;
+
+	return secs;
+}
+
+static const struct ab_rtc ab5500_rtc = {
+	.irqname	= "RTC_Alarm",
+	.bank		= AB5500_BANK_RTC,
+	.rtc		= AB5500_RTC,
+	.rtc_alarmon	= AB5500_RTC_ALARM,
+	.ti0		= AB5500_TI0,
+	.nr_ti		= 6,
+	.al0		= AB5500_AL0,
+	.nr_al		= 3,
+	.epoch		= 2000,
+	.time_to_regs	= ab5500_rtc_time_to_regs,
+	.regs_to_time	= ab5500_rtc_regs_to_time,
+	.alarm_to_regs	= ab5500_rtc_alarm_to_regs,
+	.regs_to_alarm	= ab5500_rtc_regs_to_alarm,
+	.request_read	= ab5500_rtc_request_read,
+};
+
+static int ab_rtc_request_read(struct device *dev)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+
+	if (!variant->request_read)
+		return 0;
+
+	return variant->request_read(dev);
+}
+
+static int ab_rtc_request_write(struct device *dev)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+
+	if (!variant->request_write)
+		return 0;
+
+	return variant->request_write(dev);
+}
+
+static bool ab_rtc_valid_time(struct device *dev, struct rtc_time *time)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+
+	if (!variant->epoch)
+		return true;
+
+	return time->tm_year >= variant->epoch - 1900;
+}
+
+static int
+ab_rtc_tm_to_time(struct device *dev, struct rtc_time *tm, unsigned long *secs)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+
+	rtc_tm_to_time(tm, secs);
+
+	if (variant->epoch)
+		*secs -= ab_rtc_get_elapsed_seconds(variant->epoch);
+
+	return 0;
+}
+
+static int
+ab_rtc_time_to_tm(struct device *dev, unsigned long secs, struct rtc_time *tm)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+
+	if (variant->epoch)
+		secs += ab_rtc_get_elapsed_seconds(variant->epoch);
+
+	rtc_time_to_tm(secs, tm);
+
+	return 0;
+}
+
+static int ab_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	unsigned char buf[variant->nr_ti];
+	unsigned long secs;
+	int err;
+
+	err = ab_rtc_request_read(dev);
+	if (err)
+		return err;
+
+	err = abx500_get_register_page_interruptible(dev, variant->bank,
+						     variant->ti0,
+						     buf, variant->nr_ti);
+	if (err)
+		return err;
+
+	secs = variant->regs_to_time(dev, buf);
+	ab_rtc_time_to_tm(dev, secs, tm);
+
+	return rtc_valid_tm(tm);
+}
+
+static int ab_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	unsigned char buf[variant->nr_ti];
+	unsigned long secs;
+	u8 reg = variant->ti0;
+	int err;
+	int i;
+
+	if (!ab_rtc_valid_time(dev, tm))
+		return -EINVAL;
+
+	ab_rtc_tm_to_time(dev, tm, &secs);
+	variant->time_to_regs(dev, secs, buf);
+
+	for (i = 0; i < variant->nr_ti; i++, reg++) {
+		err = abx500_set_register_interruptible(dev, variant->bank,
+							reg, buf[i]);
+		if (err)
+			return err;
+	}
+
+	return ab_rtc_request_write(dev);
+}
+
+static int ab_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	unsigned long secs;
+	u8 buf[variant->nr_al];
+	u8 rtcval;
+	int err;
+
+	err = abx500_get_register_interruptible(dev, variant->bank,
+						variant->rtc, &rtcval);
+	if (err)
+		return err;
+
+	alarm->enabled = !!(rtcval & variant->rtc_alarmon);
+	alarm->pending = 0;
+
+	err = abx500_get_register_page_interruptible(dev, variant->bank,
+						     variant->al0, buf,
+						     variant->nr_al);
+	if (err)
+		return err;
+
+	secs = variant->regs_to_alarm(dev, buf);
+	ab_rtc_time_to_tm(dev, secs, &alarm->time);
+
+	return rtc_valid_tm(&alarm->time);
+}
+
+static int ab_rtc_alarm_enable(struct device *dev, unsigned int enabled)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	u8 mask = variant->rtc_alarmon;
+	u8 value = enabled ? mask : 0;
+
+	return abx500_mask_and_set_register_interruptible(dev, variant->bank,
+							  variant->rtc, mask,
+							  value);
+}
+
+static int ab_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	unsigned char buf[variant->nr_al];
+	unsigned long secs;
+	u8 reg = variant->al0;
+	int err;
+	int i;
+
+	if (!ab_rtc_valid_time(dev, &alarm->time))
+		return -EINVAL;
+
+	ab_rtc_tm_to_time(dev, &alarm->time, &secs);
+	variant->alarm_to_regs(dev, secs, buf);
+
+	/*
+	 * Disable alarm first.  Otherwise the RTC may not detect an alarm
+	 * reprogrammed for the same time without disabling the alarm in
+	 * between the programmings.
+	 */
+	err = ab_rtc_alarm_enable(dev, false);
+	if (err)
+		return err;
+
+	for (i = 0; i < variant->nr_al; i++, reg++) {
+		err = abx500_set_register_interruptible(dev, variant->bank,
+							reg, buf[i]);
+		if (err)
+			return err;
+	}
+
+	return ab_rtc_alarm_enable(dev, true);
+}
+
+static const struct rtc_class_ops ab_rtc_ops = {
+	.read_time		= ab_rtc_read_time,
+	.set_time		= ab_rtc_set_time,
+	.read_alarm		= ab_rtc_read_alarm,
+	.set_alarm		= ab_rtc_set_alarm,
+	.alarm_irq_enable	= ab_rtc_alarm_enable,
+};
+
+static irqreturn_t ab_rtc_irq(int irq, void *dev_id)
+{
+	unsigned long events = RTC_IRQF | RTC_AF;
+	struct rtc_device *rtc = dev_id;
+
+	rtc_update_irq(rtc, 1, events);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit ab_rtc_probe(struct platform_device *pdev)
+{
+	const struct ab_rtc *variant = to_ab_rtc(&pdev->dev);
+	int err;
+	struct rtc_device *rtc;
+	int irq = -ENXIO;
+
+	if (variant->irqname) {
+		irq = platform_get_irq_byname(pdev, variant->irqname);
+		if (irq < 0)
+			return irq;
+	}
+
+	if (variant->startup) {
+		err = variant->startup(&pdev->dev);
+		if (err)
+			return err;
+	}
+
+	rtc = rtc_device_register("ab8500-rtc", &pdev->dev, &ab_rtc_ops,
+			THIS_MODULE);
+	if (IS_ERR(rtc)) {
+		dev_err(&pdev->dev, "Registration failed\n");
+		err = PTR_ERR(rtc);
+		return err;
+	}
+
+	if (irq >= 0) {
+		err = request_any_context_irq(irq, ab_rtc_irq,
+					      IRQF_NO_SUSPEND,
+					      pdev->id_entry->name,
+					      rtc);
+		if (err < 0) {
+			dev_err(&pdev->dev, "could not get irq: %d\n", err);
+			goto out_unregister;
+		}
+	}
+
+	platform_set_drvdata(pdev, rtc);
+
+	return 0;
+
+out_unregister:
+	rtc_device_unregister(rtc);
+	return err;
+}
+
+static int __devexit ab_rtc_remove(struct platform_device *pdev)
+{
+	const struct ab_rtc *variant = to_ab_rtc(&pdev->dev);
+	struct rtc_device *rtc = platform_get_drvdata(pdev);
+	int irq = platform_get_irq_byname(pdev, variant->irqname);
+
+	if (irq >= 0)
+		free_irq(irq, rtc);
+	rtc_device_unregister(rtc);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_device_id ab_rtc_id_table[] = {
+	{ "ab5500-rtc", (kernel_ulong_t)&ab5500_rtc, },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, ab_rtc_id_table);
+
+static struct platform_driver ab_rtc_driver = {
+	.driver.name	= "ab-rtc",
+	.driver.owner	= THIS_MODULE,
+	.id_table	= ab_rtc_id_table,
+	.probe		= ab_rtc_probe,
+	.remove		= __devexit_p(ab_rtc_remove),
+};
+
+static int __init ab_rtc_init(void)
+{
+	return platform_driver_register(&ab_rtc_driver);
+}
+module_init(ab_rtc_init);
+
+static void __exit ab_rtc_exit(void)
+{
+	platform_driver_unregister(&ab_rtc_driver);
+}
+module_exit(ab_rtc_exit);
+
+MODULE_AUTHOR("Rabin Vincent <rabin.vincent@stericsson.com>");
+MODULE_DESCRIPTION("AB5500 RTC Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/rtc/rtc-ab8500.c b/drivers/rtc/rtc-ab8500.c
index e346705aae9..89e282292d3 100644
--- a/drivers/rtc/rtc-ab8500.c
+++ b/drivers/rtc/rtc-ab8500.c
@@ -241,8 +241,19 @@ static int ab8500_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 	 */
 	secs -= get_elapsed_seconds(AB8500_RTC_EPOCH);
 
+#ifndef CONFIG_ANDROID
+	secs += 30; /* Round to nearest minute */
+#endif
+
 	mins = secs / 60;
 
+#ifdef CONFIG_ANDROID
+	/*
+	 * Needed due to Android believes all hw have a wake-up resolution
+	 * in seconds.
+	 */
+	mins++;
+#endif
 	buf[2] = mins & 0xFF;
 	buf[1] = (mins >> 8) & 0xFF;
 	buf[0] = (mins >> 16) & 0xFF;
@@ -316,8 +327,8 @@ static int __devinit ab8500_rtc_probe(struct platform_device *pdev)
 		return err;
 	}
 
-	err = request_threaded_irq(irq, NULL, rtc_alarm_handler, 0,
-				   "ab8500-rtc", rtc);
+	err = request_threaded_irq(irq, NULL, rtc_alarm_handler,
+		IRQF_NO_SUSPEND, "ab8500-rtc", rtc);
 	if (err < 0) {
 		rtc_device_unregister(rtc);
 		return err;
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index de35c3ad8a6..4edbcff78fb 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -113,6 +113,20 @@ config SPI_BITBANG
 	  need it.  You only need to select this explicitly to support driver
 	  modules that aren't part of this kernel tree.
 
+config STM_MSP_SPI
+	tristate "STM MSP CONTROLLER (SPI master)"
+	default y
+	help
+	  This enables using the STM MSP controller in master
+	  mode.
+
+config SPI_WORKQUEUE
+	bool "SPI_WORKQUEUE"
+	depends on STM_MSP_SPI
+	default n
+	help
+	  This feature allow SPI works to be deferred in MSP driver.
+
 config SPI_BUTTERFLY
 	tristate "Parallel port adapter for AVR Butterfly (DEVELOPMENT)"
 	depends on PARPORT
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index 0f8c69b6b19..d50739d10d5 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -55,6 +55,7 @@ obj-$(CONFIG_SPI_SH_SCI)		+= spi_sh_sci.o
 obj-$(CONFIG_SPI_SH_MSIOF)		+= spi_sh_msiof.o
 obj-$(CONFIG_SPI_STMP3XXX)		+= spi_stmp.o
 obj-$(CONFIG_SPI_NUC900)		+= spi_nuc900.o
+obj-$(CONFIG_STM_MSP_SPI)               += stm_msp.o
 
 # special build for s3c24xx spi driver with fiq support
 spi_s3c24xx_hw-y			:= spi_s3c24xx.o
diff --git a/drivers/spi/amba-pl022.c b/drivers/spi/amba-pl022.c
index d18ce9e946d..0288654571a 100644
--- a/drivers/spi/amba-pl022.c
+++ b/drivers/spi/amba-pl022.c
@@ -42,6 +42,7 @@
 #include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
 #include <linux/scatterlist.h>
+#include <linux/pm_runtime.h>
 
 /*
  * This macro is used to define some register default values.
@@ -391,6 +392,8 @@ struct pl022 {
 	struct sg_table			sgt_tx;
 	char				*dummypage;
 #endif
+	enum ssp_rx_level_trig		rx_lev_trig;
+	enum ssp_tx_level_trig		tx_lev_trig;
 };
 
 /**
@@ -517,6 +520,7 @@ static void giveback(struct pl022 *pl022)
 	clk_disable(pl022->clk);
 	amba_pclk_disable(pl022->adev);
 	amba_vcore_disable(pl022->adev);
+	pm_runtime_put(&pl022->adev->dev);
 }
 
 /**
@@ -924,6 +928,12 @@ static int configure_dma(struct pl022 *pl022)
 	struct dma_async_tx_descriptor *rxdesc;
 	struct dma_async_tx_descriptor *txdesc;
 
+	/* DMA burstsize should be same as the FIFO trigger level */
+	rx_conf.src_maxburst = pl022->rx_lev_trig ? 1 <<
+		(pl022->rx_lev_trig + 1) : pl022->rx_lev_trig;
+	tx_conf.dst_maxburst = pl022->tx_lev_trig ? 1 <<
+		(pl022->tx_lev_trig + 1) : pl022->tx_lev_trig;
+
 	/* Check that the channels are available */
 	if (!rxchan || !txchan)
 		return -ENODEV;
@@ -1498,6 +1508,7 @@ static void pump_messages(struct work_struct *work)
 	 */
 	amba_vcore_enable(pl022->adev);
 	amba_pclk_enable(pl022->adev);
+	pm_runtime_get_sync(&pl022->adev->dev);
 	clk_enable(pl022->clk);
 	restore_state(pl022);
 	flush(pl022);
@@ -1874,6 +1885,9 @@ static int pl022_setup(struct spi_device *spi)
 		goto err_config_params;
 	}
 
+	pl022->rx_lev_trig = chip_info->rx_lev_trig;
+	pl022->tx_lev_trig = chip_info->tx_lev_trig;
+
 	/* Now set controller state based on controller data */
 	chip->xfer_type = chip_info->com_mode;
 	if (!chip_info->cs_control) {
@@ -2095,6 +2109,9 @@ pl022_probe(struct amba_device *adev, const struct amba_id *id)
 	printk(KERN_INFO "pl022: mapped registers from 0x%08x to %p\n",
 	       adev->res.start, pl022->virtbase);
 
+	pm_runtime_enable(dev);
+	pm_runtime_resume(dev);
+
 	pl022->clk = clk_get(&adev->dev, NULL);
 	if (IS_ERR(pl022->clk)) {
 		status = PTR_ERR(pl022->clk);
@@ -2188,6 +2205,7 @@ pl022_remove(struct amba_device *adev)
 	free_irq(adev->irq[0], pl022);
 	clk_disable(pl022->clk);
 	clk_put(pl022->clk);
+	pm_runtime_disable(&adev->dev);
 	iounmap(pl022->virtbase);
 	amba_release_regions(adev);
 	tasklet_disable(&pl022->pump_transfers);
diff --git a/drivers/spi/stm_msp.c b/drivers/spi/stm_msp.c
new file mode 100644
index 00000000000..65dc316b37b
--- /dev/null
+++ b/drivers/spi/stm_msp.c
@@ -0,0 +1,1929 @@
+/*
+ * drivers/spi/stm_msp.c
+ *
+ * Copyright (C) 2010 STMicroelectronics Pvt. Ltd.
+ *
+ * Author: Sachin Verma <sachin.verma@st.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/ioport.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/spi/spi.h>
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/amba/bus.h>
+#include <linux/spi/stm_msp.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/interrupt.h>
+
+/**
+ * MSP Controller Register Offsets
+ */
+#define MSP_DR(r)			(r + 0x000)
+#define MSP_GCR(r)			(r + 0x004)
+#define MSP_TCF(r)			(r + 0x008)
+#define MSP_RCF(r)			(r + 0x00C)
+#define MSP_SRG(r)			(r + 0x010)
+#define MSP_FLR(r)			(r + 0x014)
+#define MSP_DMACR(r)			(r + 0x018)
+#define MSP_IMSC(r)			(r + 0x020)
+#define MSP_RIS(r)			(r + 0x024)
+#define MSP_MIS(r)			(r + 0x028)
+#define MSP_ICR(r)			(r + 0x02C)
+#define MSP_MCR(r)			(r + 0x030)
+#define MSP_RCV(r)			(r + 0x034)
+#define MSP_RCM(r)			(r + 0x038)
+#define MSP_TCE0(r)			(r + 0x040)
+#define MSP_TCE1(r)			(r + 0x044)
+#define MSP_TCE2(r)			(r + 0x048)
+#define MSP_TCE3(r)			(r + 0x04C)
+#define MSP_RCE0(r)			(r + 0x060)
+#define MSP_RCE1(r)			(r + 0x064)
+#define MSP_RCE2(r)			(r + 0x068)
+#define MSP_RCE3(r)			(r + 0x06C)
+#define MSP_PID0(r)			(r + 0xFE0)
+#define MSP_PID1(r)			(r + 0xFE4)
+#define MSP_PID2(r)			(r + 0xFE8)
+#define MSP_PID3(r)			(r + 0xFEC)
+
+/**
+ * MSP Global Configuration Register - MSP_GCR
+ */
+#define MSP_GCR_MASK_RXEN		((u32)(0x1UL << 0))
+#define MSP_GCR_MASK_RFFEN		((u32)(0x1UL << 1))
+#define MSP_GCR_MASK_RFSPOL		((u32)(0x1UL << 2))
+#define MSP_GCR_MASK_DCM		((u32)(0x1UL << 3))
+#define MSP_GCR_MASK_RFSSEL		((u32)(0x1UL << 4))
+#define MSP_GCR_MASK_RCKPOL		((u32)(0x1UL << 5))
+#define MSP_GCR_MASK_RCKSEL		((u32)(0x1UL << 6))
+#define MSP_GCR_MASK_LBM		((u32)(0x1UL << 7))
+#define MSP_GCR_MASK_TXEN		((u32)(0x1UL << 8))
+#define MSP_GCR_MASK_TFFEN		((u32)(0x1UL << 9))
+#define MSP_GCR_MASK_TFSPOL		((u32)(0x1UL << 10))
+#define MSP_GCR_MASK_TFSSEL		((u32)(0x3UL << 11))
+#define MSP_GCR_MASK_TCKPOL		((u32)(0x1UL << 13))
+#define MSP_GCR_MASK_TCKSEL		((u32)(0x1UL << 14))
+#define MSP_GCR_MASK_TXDDL		((u32)(0x1UL << 15))
+#define MSP_GCR_MASK_SGEN		((u32)(0x1UL << 16))
+#define MSP_GCR_MASK_SCKPOL		((u32)(0x1UL << 17))
+#define MSP_GCR_MASK_SCKSEL		((u32)(0x3UL << 18))
+#define MSP_GCR_MASK_FGEN		((u32)(0x1UL << 20))
+#define MSP_GCR_MASK_SPICKM		((u32)(0x3UL << 21))
+#define MSP_GCR_MASK_SPIBME		((u32)(0x1UL << 23))
+
+/**
+ * MSP Transmit Configuration Register - MSP_TCF
+ */
+#define MSP_TCF_MASK_TP1ELEN		((u32)(0x7UL << 0))
+#define MSP_TCF_MASK_TP1FLEN		((u32)(0x7FUL << 3))
+#define MSP_TCF_MASK_TDTYP		((u32)(0x3UL << 10))
+#define MSP_TCF_MASK_TENDN		((u32)(0x1UL << 12))
+#define MSP_TCF_MASK_TDDLY		((u32)(0x3UL << 13))
+#define MSP_TCF_MASK_TFSIG		((u32)(0x1UL << 15))
+#define MSP_TCF_MASK_TP2ELEN		((u32)(0x7UL << 16))
+#define MSP_TCF_MASK_TP2FLEN		((u32)(0x7FUL << 19))
+#define MSP_TCF_MASK_TP2SM		((u32)(0x1UL << 26))
+#define MSP_TCF_MASK_TP2EN		((u32)(0x1UL << 27))
+#define MSP_TCF_MASK_TBSWAP		((u32)(0x3UL << 28))
+
+/**
+ * MSP Receive Configuration Register - MSP_RCF
+ */
+#define MSP_RCF_MASK_RP1ELEN		((u32)(0x7UL << 0))
+#define MSP_RCF_MASK_RP1FLEN		((u32)(0x7FUL << 3))
+#define MSP_RCF_MASK_RDTYP		((u32)(0x3UL << 10))
+#define MSP_RCF_MASK_RENDN		((u32)(0x1UL << 12))
+#define MSP_RCF_MASK_RDDLY		((u32)(0x3UL << 13))
+#define MSP_RCF_MASK_RFSIG		((u32)(0x1UL << 15))
+#define MSP_RCF_MASK_RP2ELEN		((u32)(0x7UL << 16))
+#define MSP_RCF_MASK_RP2FLEN		((u32)(0x7FUL << 19))
+#define MSP_RCF_MASK_RP2SM		((u32)(0x1UL << 26))
+#define MSP_RCF_MASK_RP2EN		((u32)(0x1UL << 27))
+#define MSP_RCF_MASK_RBSWAP		((u32)(0x3UL << 28))
+
+/**
+ * MSP Sample Rate Generator Register - MSP_SRG
+ */
+#define MSP_SRG_MASK_SCKDIV		((u32)(0x3FFUL << 0))
+#define MSP_SRG_MASK_FRWID		((u32)(0x3FUL << 10))
+#define MSP_SRG_MASK_FRPER		((u32)(0x1FFFUL << 16))
+
+/**
+ * MSP Flag Register - MSP_FLR
+ */
+#define MSP_FLR_MASK_RBUSY		((u32)(0x1UL << 0))
+#define MSP_FLR_MASK_RFE		((u32)(0x1UL << 1))
+#define MSP_FLR_MASK_RFU		((u32)(0x1UL << 2))
+#define MSP_FLR_MASK_TBUSY		((u32)(0x1UL << 3))
+#define MSP_FLR_MASK_TFE		((u32)(0x1UL << 4))
+#define MSP_FLR_MASK_TFU		((u32)(0x1UL << 5))
+
+/**
+ * MSP DMA Control Register - MSP_DMACR
+ */
+#define MSP_DMACR_MASK_RDMAE		((u32)(0x1UL << 0))
+#define MSP_DMACR_MASK_TDMAE		((u32)(0x1UL << 1))
+
+/**
+ * MSP Interrupt Mask Set/Clear Register - MSP_IMSC
+ */
+#define MSP_IMSC_MASK_RXIM		((u32)(0x1UL << 0))
+#define MSP_IMSC_MASK_ROEIM		((u32)(0x1UL << 1))
+#define MSP_IMSC_MASK_RSEIM		((u32)(0x1UL << 2))
+#define MSP_IMSC_MASK_RFSIM		((u32)(0x1UL << 3))
+#define MSP_IMSC_MASK_TXIM		((u32)(0x1UL << 4))
+#define MSP_IMSC_MASK_TUEIM		((u32)(0x1UL << 5))
+#define MSP_IMSC_MASK_TSEIM		((u32)(0x1UL << 6))
+#define MSP_IMSC_MASK_TFSIM		((u32)(0x1UL << 7))
+#define MSP_IMSC_MASK_RFOIM		((u32)(0x1UL << 8))
+#define MSP_IMSC_MASK_TFOIM		((u32)(0x1UL << 9))
+
+/**
+ * MSP Raw Interrupt status Register - MSP_RIS
+ */
+#define MSP_RIS_MASK_RXRIS		((u32)(0x1UL << 0))
+#define MSP_RIS_MASK_ROERIS		((u32)(0x1UL << 1))
+#define MSP_RIS_MASK_RSERIS		((u32)(0x1UL << 2))
+#define MSP_RIS_MASK_RFSRIS		((u32)(0x1UL << 3))
+#define MSP_RIS_MASK_TXRIS		((u32)(0x1UL << 4))
+#define MSP_RIS_MASK_TUERIS		((u32)(0x1UL << 5))
+#define MSP_RIS_MASK_TSERIS		((u32)(0x1UL << 6))
+#define MSP_RIS_MASK_TFSRIS		((u32)(0x1UL << 7))
+#define MSP_RIS_MASK_RFORIS		((u32)(0x1UL << 8))
+#define MSP_RIS_MASK_TFORIS		((u32)(0x1UL << 9))
+
+/**
+ * MSP Masked Interrupt status Register - MSP_MIS
+ */
+#define MSP_MIS_MASK_RXMIS		((u32)(0x1UL << 0))
+#define MSP_MIS_MASK_ROEMIS		((u32)(0x1UL << 1))
+#define MSP_MIS_MASK_RSEMIS		((u32)(0x1UL << 2))
+#define MSP_MIS_MASK_RFSMIS		((u32)(0x1UL << 3))
+#define MSP_MIS_MASK_TXMIS		((u32)(0x1UL << 4))
+#define MSP_MIS_MASK_TUEMIS		((u32)(0x1UL << 5))
+#define MSP_MIS_MASK_TSEMIS		((u32)(0x1UL << 6))
+#define MSP_MIS_MASK_TFSMIS		((u32)(0x1UL << 7))
+#define MSP_MIS_MASK_RFOMIS		((u32)(0x1UL << 8))
+#define MSP_MIS_MASK_TFOMIS		((u32)(0x1UL << 9))
+
+/**
+ * MSP Interrupt Clear Register - MSP_ICR
+ */
+#define MSP_ICR_MASK_ROEIC		((u32)(0x1UL << 1))
+#define MSP_ICR_MASK_RSEIC		((u32)(0x1UL << 2))
+#define MSP_ICR_MASK_RFSIC		((u32)(0x1UL << 3))
+#define MSP_ICR_MASK_TUEIC		((u32)(0x1UL << 5))
+#define MSP_ICR_MASK_TSEIC		((u32)(0x1UL << 6))
+#define MSP_ICR_MASK_TFSIC		((u32)(0x1UL << 7))
+
+#define GEN_MASK_BITS(val, mask, sb)	((u32)((((u32)val) << (sb)) & (mask)))
+#define MSP_WBITS(reg, val, mask, sb)	((reg) = (((reg) & ~(mask)) |\
+					(((val) << (sb)) & (mask))))
+#define DEFAULT_MSP_REG_DMACR		0x00000000
+#define DEFAULT_MSP_REG_SRG		0x1FFF0000
+
+#define DEFAULT_MSP_REG_GCR	( \
+	GEN_MASK_BITS(MSP_RECEIVER_DISABLED, MSP_GCR_MASK_RXEN, 0)	|\
+	GEN_MASK_BITS(MSP_RX_FIFO_ENABLED, MSP_GCR_MASK_RFFEN, 1)	|\
+	GEN_MASK_BITS(MSP_LOOPBACK_DISABLED, MSP_GCR_MASK_LBM, 7)	|\
+	GEN_MASK_BITS(MSP_TRANSMITTER_DISABLED, MSP_GCR_MASK_TXEN, 8)	|\
+	GEN_MASK_BITS(MSP_TX_FIFO_ENABLED, MSP_GCR_MASK_TFFEN, 9)	|\
+	GEN_MASK_BITS(MSP_TX_FRAME_SYNC_POL_LOW, MSP_GCR_MASK_TFSPOL, 10)|\
+	GEN_MASK_BITS(MSP_TX_FRAME_SYNC_INT, MSP_GCR_MASK_TFSSEL, 11)	|\
+	GEN_MASK_BITS(MSP_TX_CLOCK_POL_HIGH, MSP_GCR_MASK_TCKPOL, 13)	|\
+	GEN_MASK_BITS(MSP_IS_SPI_MASTER, MSP_GCR_MASK_TCKSEL, 14)	|\
+	GEN_MASK_BITS(MSP_TRANSMIT_DATA_WITHOUT_DELAY, MSP_GCR_MASK_TXDDL, 15)|\
+	GEN_MASK_BITS(MSP_SAMPLE_RATE_GEN_ENABLE, MSP_GCR_MASK_SGEN, 16)|\
+	GEN_MASK_BITS(MSP_CLOCK_INTERNAL, MSP_GCR_MASK_SCKSEL, 18)	|\
+	GEN_MASK_BITS(MSP_FRAME_GEN_ENABLE, MSP_GCR_MASK_FGEN, 20)	|\
+	GEN_MASK_BITS(MSP_SPI_PHASE_ZERO_CYCLE_DELAY, MSP_GCR_MASK_SPICKM, 21)|\
+	GEN_MASK_BITS(SPI_BURST_MODE_DISABLE, MSP_GCR_MASK_SPIBME, 23)\
+	)
+#define DEFAULT_MSP_REG_RCF	( \
+	GEN_MASK_BITS(MSP_DATA_BITS_32, MSP_RCF_MASK_RP1ELEN, 0) | \
+	GEN_MASK_BITS(MSP_IGNORE_RX_FRAME_SYNC_PULSE, MSP_RCF_MASK_RFSIG, 15) |\
+	GEN_MASK_BITS(MSP_RX_1BIT_DATA_DELAY, MSP_RCF_MASK_RDDLY, 13) | \
+	GEN_MASK_BITS(MSP_RX_ENDIANESS_LSB, MSP_RCF_MASK_RENDN, 12)  \
+	)
+
+#define DEFAULT_MSP_REG_TCF	( \
+	GEN_MASK_BITS(MSP_DATA_BITS_32, MSP_TCF_MASK_TP1ELEN, 0) | \
+	GEN_MASK_BITS(MSP_IGNORE_TX_FRAME_SYNC_PULSE, MSP_TCF_MASK_TFSIG, 15) |\
+	GEN_MASK_BITS(MSP_TX_1BIT_DATA_DELAY, MSP_TCF_MASK_TDDLY, 13) | \
+	GEN_MASK_BITS(MSP_TX_ENDIANESS_LSB, MSP_TCF_MASK_TENDN, 12)  \
+	)
+
+/**
+ * MSP Receiver/Transmitter states (enabled or disabled)
+ */
+#define MSP_RECEIVER_DISABLED		0
+#define MSP_RECEIVER_ENABLED		1
+#define MSP_TRANSMITTER_DISABLED	0
+#define MSP_TRANSMITTER_ENABLED		1
+
+/**
+ * MSP Receiver/Transmitter FIFO constants
+ */
+#define MSP_LOOPBACK_DISABLED		0
+#define MSP_LOOPBACK_ENABLED		1
+
+#define MSP_TX_FIFO_DISABLED		0
+#define MSP_TX_FIFO_ENABLED		1
+#define MSP_TX_ENDIANESS_MSB		0
+#define MSP_TX_ENDIANESS_LSB		1
+
+#define MSP_RX_FIFO_DISABLED		0
+#define MSP_RX_FIFO_ENABLED		1
+#define MSP_RX_ENDIANESS_MSB		0
+#define MSP_RX_ENDIANESS_LSB		1
+
+#define MSP_TX_FRAME_SYNC_EXT		0x0
+#define MSP_TX_FRAME_SYNC_INT		0x2
+#define MSP_TX_FRAME_SYNC_INT_CFG	0x3
+
+#define MSP_TX_FRAME_SYNC_POL_HIGH	0
+#define MSP_TX_FRAME_SYNC_POL_LOW	1
+
+#define MSP_HANDLE_RX_FRAME_SYNC_PULSE	0
+#define MSP_IGNORE_RX_FRAME_SYNC_PULSE	1
+
+#define MSP_RX_NO_DATA_DELAY		0x0
+#define MSP_RX_1BIT_DATA_DELAY		0x1
+#define MSP_RX_2BIT_DATA_DELAY		0x2
+#define MSP_RX_3BIT_DATA_DELAY		0x3
+
+#define MSP_HANDLE_TX_FRAME_SYNC_PULSE	0
+#define MSP_IGNORE_TX_FRAME_SYNC_PULSE	1
+
+#define MSP_TX_NO_DATA_DELAY		0x0
+#define MSP_TX_1BIT_DATA_DELAY		0x1
+#define MSP_TX_2BIT_DATA_DELAY		0x2
+#define MSP_TX_3BIT_DATA_DELAY		0x3
+
+#define MSP_TX_CLOCK_POL_LOW		0
+#define MSP_TX_CLOCK_POL_HIGH		1
+
+#define MSP_SPI_PHASE_ZERO_CYCLE_DELAY	0x2
+#define MSP_SPI_PHASE_HALF_CYCLE_DELAY	0x3
+
+#define MSP_IS_SPI_SLAVE		0
+#define MSP_IS_SPI_MASTER		1
+
+#define MSP_FRAME_GEN_DISABLE		0
+#define MSP_FRAME_GEN_ENABLE		1
+
+#define MSP_SAMPLE_RATE_GEN_DISABLE	0
+#define MSP_SAMPLE_RATE_GEN_ENABLE	1
+
+#define SPI_BURST_MODE_DISABLE		0
+#define SPI_BURST_MODE_ENABLE		1
+
+#define MSP_TRANSMIT_DATA_WITHOUT_DELAY	0
+#define MSP_TRANSMIT_DATA_WITH_DELAY	1
+
+#define MSP_CLOCK_INTERNAL		0x0 /* 48 MHz */
+
+/* SRG is derived from MSPSCK pin but is resynchronized on MSPRFS
+ * (Receive Frame Sync signal) */
+#define MSP_CLOCK_EXTERNAL		0x2
+#define MSP_CLOCK_EXTERNAL_RESYNC	0x3
+
+#define DISABLE_ALL_MSP_INTERRUPTS	(0x0)
+#define ENABLE_ALL_MSP_INTERRUPTS	(0x333)
+#define CLEAR_ALL_MSP_INTERRUPTS	(0xEE)
+#define DEFAULT_MSP_CLK			(48000000)
+#define MAX_SCKDIV			(1023)
+
+#define MSP_FIFO_DEPTH			8
+
+/**
+ * Queue State
+ */
+#define QUEUE_RUNNING			(0)
+#define QUEUE_STOPPED			(1)
+
+#define START_STATE			((void *)0)
+#define RUNNING_STATE			((void *)1)
+#define DONE_STATE			((void *)2)
+#define ERROR_STATE			((void *)-1)
+
+/* Default values */
+#define SPI_DEFAULT_MAX_SPEED_HZ	48000
+#define SPI_TRANSFER_TIMEOUT_MS		5000
+
+/* CONTROLLER COMMANDS */
+enum cntlr_commands {
+	DISABLE_CONTROLLER = 0,
+	ENABLE_CONTROLLER ,
+	DISABLE_ALL_INTERRUPT ,
+	ENABLE_ALL_INTERRUPT ,
+	FLUSH_FIFO ,
+	RESTORE_STATE ,
+	LOAD_DEFAULT_CONFIG ,
+	CLEAR_ALL_INTERRUPT,
+};
+
+struct stm_msp {
+	struct amba_device		*adev;
+	struct spi_master		*master;
+	struct stm_msp_controller	*master_info;
+	void __iomem			*regs;
+	struct clk			*clk;
+#ifdef CONFIG_SPI_WORKQUEUE
+	struct workqueue_struct		*workqueue;
+#endif
+	struct work_struct		spi_work;
+	spinlock_t			lock;
+	struct list_head		queue;
+	int				busy;
+	int				run;
+	struct tasklet_struct		pump_transfers;
+	struct timer_list		spi_notify_timer;
+	int				spi_io_error;
+	struct spi_message		*cur_msg;
+	struct spi_transfer		*cur_transfer;
+	struct chip_data		*cur_chip;
+	void				*tx;
+	void				*tx_end;
+	void				*rx;
+	void				*rx_end;
+	void				(*write)(struct stm_msp *stm_msp);
+	void				(*read)(struct stm_msp *stm_msp);
+	void				(*delay)(struct stm_msp *stm_msp);
+};
+
+/**
+ * struct chip_data - To maintain runtime state of SPICntlr for each client chip
+ * @ctr_regs: void pointer which is assigned a struct having regs of the cntlr.
+ * @chip_id: Chip Id assigned to this client to identify it.
+ * @n_bytes: how many bytes(power of 2) reqd for a given data width of client
+ * @write: function to be used to write when doing xfer for this chip
+ * @null_write: function to be used for dummy write for receiving data.
+ * @read: function to be used to read when doing xfer for this chip
+ * @null_read: function to be used to for dummy read while writting data.
+ * @cs_control: chip select callback provided by chip
+ * @xfer_type: polling/interrupt
+ *
+ * Runtime state of the SPI controller, maintained per chip,
+ * This would be set according to the current message that would be served
+ */
+struct chip_data {
+	void *ctr_regs;
+	u32 chip_id;
+	u8 n_bytes;
+	void (*write) (struct stm_msp *stm_msp);
+	void (*null_write) (struct stm_msp *stm_msp);
+	void (*read) (struct stm_msp *stm_msp);
+	void (*null_read) (struct stm_msp *stm_msp);
+	void (*delay) (struct stm_msp *stm_msp);
+	void (*cs_control) (u32 command);
+	int xfer_type;
+};
+
+/**
+ * struct msp_regs - Used to store MSP controller registry values
+ *		used by the driver.
+ * @gcr: global configuration register
+ * @tcf: transmit configuration register
+ * @rcf: receive configuration register
+ * @srg: sample rate generator register
+ * @dmacr: DMA configuration register
+ */
+struct msp_regs {
+	u32 gcr;
+	u32 tcf;
+	u32 rcf;
+	u32 srg;
+	u32 dmacr;
+};
+
+/**
+ * stm_msp_controller_cmd - To execute controller commands for MSP
+ * @stm_msp: SPI driver private data structure
+ * @cmd: Command which is to be executed on the controller
+ */
+static int stm_msp_controller_cmd(struct stm_msp *stm_msp, int cmd)
+{
+	int retval = 0;
+	struct msp_regs *msp_regs = NULL;
+
+	switch (cmd) {
+	case DISABLE_CONTROLLER: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Disabling MSP controller...\n");
+			writel((readl(MSP_GCR(stm_msp->regs)) &
+				(~(MSP_GCR_MASK_TXEN | MSP_GCR_MASK_RXEN))),
+				MSP_GCR(stm_msp->regs));
+			break;
+		}
+	case ENABLE_CONTROLLER: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Enabling MSP controller...\n");
+			writel((readl(MSP_GCR(stm_msp->regs)) |
+				(MSP_GCR_MASK_TXEN | MSP_GCR_MASK_RXEN)),
+				MSP_GCR(stm_msp->regs));
+			break;
+		}
+	case DISABLE_ALL_INTERRUPT: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Disabling all MSP interrupts...\n");
+			writel(DISABLE_ALL_MSP_INTERRUPTS,
+				MSP_IMSC(stm_msp->regs));
+			break;
+		}
+	case ENABLE_ALL_INTERRUPT: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Enabling all MSP interrupts...\n");
+			writel(ENABLE_ALL_MSP_INTERRUPTS,
+				MSP_IMSC(stm_msp->regs));
+			break;
+		}
+	case CLEAR_ALL_INTERRUPT: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Clearing all MSP interrupts...\n");
+			writel(CLEAR_ALL_MSP_INTERRUPTS,
+				MSP_ICR(stm_msp->regs));
+			break;
+		}
+	case FLUSH_FIFO: {
+			unsigned long limit = loops_per_jiffy << 1;
+
+			dev_dbg(&stm_msp->adev->dev, "MSP FIFO flushed\n");
+
+			do {
+				while (!(readl(MSP_FLR(stm_msp->regs)) &
+					MSP_FLR_MASK_RFE)) {
+					readl(MSP_DR(stm_msp->regs));
+				}
+			} while ((readl(MSP_FLR(stm_msp->regs)) &
+				(MSP_FLR_MASK_TBUSY | MSP_FLR_MASK_RBUSY)) &&
+				limit--);
+
+			retval = limit;
+			break;
+		}
+	case RESTORE_STATE: {
+			msp_regs =
+				(struct msp_regs *)stm_msp->cur_chip->ctr_regs;
+
+			dev_dbg(&stm_msp->adev->dev,
+				"Restoring MSP state...\n");
+
+			writel(msp_regs->gcr, MSP_GCR(stm_msp->regs));
+			writel(msp_regs->tcf, MSP_TCF(stm_msp->regs));
+			writel(msp_regs->rcf, MSP_RCF(stm_msp->regs));
+			writel(msp_regs->srg, MSP_SRG(stm_msp->regs));
+			writel(msp_regs->dmacr, MSP_DMACR(stm_msp->regs));
+			writel(DISABLE_ALL_MSP_INTERRUPTS,
+			       MSP_IMSC(stm_msp->regs));
+			writel(CLEAR_ALL_MSP_INTERRUPTS,
+			       MSP_ICR(stm_msp->regs));
+			break;
+		}
+	case LOAD_DEFAULT_CONFIG: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Loading default MSP config...\n");
+
+			writel(DEFAULT_MSP_REG_GCR, MSP_GCR(stm_msp->regs));
+			writel(DEFAULT_MSP_REG_TCF, MSP_TCF(stm_msp->regs));
+			writel(DEFAULT_MSP_REG_RCF, MSP_RCF(stm_msp->regs));
+			writel(DEFAULT_MSP_REG_SRG, MSP_SRG(stm_msp->regs));
+			writel(DEFAULT_MSP_REG_DMACR, MSP_DMACR(stm_msp->regs));
+			writel(DISABLE_ALL_MSP_INTERRUPTS,
+				MSP_IMSC(stm_msp->regs));
+			writel(CLEAR_ALL_MSP_INTERRUPTS,
+			       MSP_ICR(stm_msp->regs));
+			break;
+		}
+	default:
+		dev_dbg(&stm_msp->adev->dev, "Unknown command\n");
+		retval = -1;
+		break;
+	}
+
+	return retval;
+}
+
+/**
+ * giveback - current spi_message is over, schedule next spi_message
+ * @message: current SPI message
+ * @stm_msp: spi driver private data structure
+ *
+ * current spi_message is over, schedule next spi_message and call
+ * callback of this msg.
+ */
+static void giveback(struct spi_message *message, struct stm_msp *stm_msp)
+{
+	struct spi_transfer *last_transfer;
+	unsigned long flags;
+	struct spi_message *msg;
+	void (*curr_cs_control)(u32 command);
+
+	spin_lock_irqsave(&stm_msp->lock, flags);
+	msg = stm_msp->cur_msg;
+
+	curr_cs_control = stm_msp->cur_chip->cs_control;
+
+	stm_msp->cur_msg = NULL;
+	stm_msp->cur_transfer = NULL;
+	stm_msp->cur_chip = NULL;
+#ifdef CONFIG_SPI_WORKQUEUE
+	queue_work(stm_msp->workqueue, &stm_msp->spi_work);
+#else
+	schedule_work(&stm_msp->spi_work);
+#endif
+	spin_unlock_irqrestore(&stm_msp->lock, flags);
+
+	last_transfer = list_entry(msg->transfers.prev,
+			struct spi_transfer, transfer_list);
+
+	if (!last_transfer->cs_change)
+		curr_cs_control(SPI_CHIP_DESELECT);
+
+	msg->state = NULL;
+
+	if (msg->complete)
+		msg->complete(msg->context);
+
+	stm_msp_controller_cmd(stm_msp, DISABLE_CONTROLLER);
+	clk_disable(stm_msp->clk);
+}
+
+/**
+ * spi_notify - Handles Polling hang issue over spi bus.
+ * @data: main driver data
+ * Context: Process.
+ *
+ * This is  used to handle error condition in transfer and receive function used
+ * in polling mode.
+ * Sometimes due to passing wrong protocol desc , polling transfer may hang.
+ * To prevent this, timer is added.
+ *
+ * Returns void.
+ */
+static void spi_notify(unsigned long data)
+{
+	struct stm_msp *stm_msp = (struct stm_msp *)data;
+	stm_msp->spi_io_error = 1;
+
+	dev_err(&stm_msp->adev->dev,
+		"Polling is taking time, maybe device not responding\n");
+
+	del_timer(&stm_msp->spi_notify_timer);
+}
+
+/**
+ * stm_msp_transfer - transfer function registered to SPI master framework
+ * @spi: spi device which is requesting transfer
+ * @msg: spi message which is to handled is queued to driver queue
+ *
+ * This function is registered to the SPI framework for this SPI master
+ * controller. It will queue the spi_message in the queue of driver if
+ * the queue is not stopped and return.
+ */
+static int stm_msp_transfer(struct spi_device *spi, struct spi_message *msg)
+{
+	struct stm_msp *stm_msp = spi_master_get_devdata(spi->master);
+	unsigned long flags;
+
+	spin_lock_irqsave(&stm_msp->lock, flags);
+
+	if (stm_msp->run == QUEUE_STOPPED) {
+		spin_unlock_irqrestore(&stm_msp->lock, flags);
+		return -ESHUTDOWN;
+	}
+	dev_err(&spi->dev, "Regular request (No infinite DMA ongoing)\n");
+
+	msg->actual_length = 0;
+	msg->status = -EINPROGRESS;
+	msg->state = START_STATE;
+
+	list_add_tail(&msg->queue, &stm_msp->queue);
+
+	if ((stm_msp->run == QUEUE_RUNNING) && (!stm_msp->busy))
+#ifdef CONFIG_SPI_WORKQUEUE
+		queue_work(stm_msp->workqueue, &stm_msp->spi_work);
+#else
+		schedule_work(&stm_msp->spi_work);
+#endif
+	spin_unlock_irqrestore(&stm_msp->lock, flags);
+	return 0;
+}
+
+/**
+ * next_transfer - Move to the Next transfer in the current spi message
+ * @stm_msp: spi driver private data structure
+ *
+ * This function moves though the linked list of spi transfers in the
+ * current spi message and returns with the state of current spi
+ * message i.e whether its last transfer is done(DONE_STATE) or
+ * Next transfer is ready(RUNNING_STATE)
+ */
+static void *next_transfer(struct stm_msp *stm_msp)
+{
+	struct spi_message *msg = stm_msp->cur_msg;
+	struct spi_transfer *trans = stm_msp->cur_transfer;
+
+	/* Move to next transfer */
+	if (trans->transfer_list.next != &msg->transfers) {
+		stm_msp->cur_transfer = list_entry(trans->transfer_list.next,
+					struct spi_transfer,
+					transfer_list);
+		return RUNNING_STATE;
+	}
+	return DONE_STATE;
+}
+
+static void do_interrupt_transfer(void *data)
+{
+	struct stm_msp *stm_msp = (struct stm_msp *)data;
+
+	stm_msp->tx = (void *)stm_msp->cur_transfer->tx_buf;
+	stm_msp->tx_end = stm_msp->tx + stm_msp->cur_transfer->len;
+
+	stm_msp->rx = (void *)stm_msp->cur_transfer->rx_buf;
+	stm_msp->rx_end = stm_msp->rx + stm_msp->cur_transfer->len;
+
+	stm_msp->write = stm_msp->tx ?
+		stm_msp->cur_chip->write : stm_msp->cur_chip->null_write;
+	stm_msp->read = stm_msp->rx ?
+		stm_msp->cur_chip->read : stm_msp->cur_chip->null_read;
+
+	stm_msp->cur_chip->cs_control(SPI_CHIP_SELECT);
+
+	stm_msp_controller_cmd(stm_msp, ENABLE_ALL_INTERRUPT);
+	stm_msp_controller_cmd(stm_msp, ENABLE_CONTROLLER);
+}
+
+static void do_polling_transfer(void *data)
+{
+	struct stm_msp *stm_msp = (struct stm_msp *)data;
+	struct spi_message *message = NULL;
+	struct spi_transfer *transfer = NULL;
+	struct spi_transfer *previous = NULL;
+	struct chip_data *chip;
+	unsigned long limit = 0;
+	u32 timer_expire = 0;
+
+	chip = stm_msp->cur_chip;
+	message = stm_msp->cur_msg;
+
+	while (message->state != DONE_STATE) {
+		/* Handle for abort */
+		if (message->state == ERROR_STATE)
+			break;
+
+		transfer = stm_msp->cur_transfer;
+
+		/* Delay if requested at end of transfer */
+		if (message->state == RUNNING_STATE) {
+			previous = list_entry(transfer->transfer_list.prev,
+					struct spi_transfer,
+					transfer_list);
+
+			if (previous->delay_usecs)
+				udelay(previous->delay_usecs);
+
+			if (previous->cs_change)
+				stm_msp->cur_chip->cs_control(SPI_CHIP_SELECT);
+		} else {
+			/* START_STATE */
+			message->state = RUNNING_STATE;
+			stm_msp->cur_chip->cs_control(SPI_CHIP_SELECT);
+		}
+
+		/* Configuration Changing Per Transfer */
+		stm_msp->tx = (void *)transfer->tx_buf;
+		stm_msp->tx_end = stm_msp->tx + stm_msp->cur_transfer->len;
+		stm_msp->rx = (void *)transfer->rx_buf;
+		stm_msp->rx_end = stm_msp->rx + stm_msp->cur_transfer->len;
+
+		stm_msp->write = stm_msp->tx ?
+			stm_msp->cur_chip->write :
+				stm_msp->cur_chip->null_write;
+		stm_msp->read = stm_msp->rx ?
+			stm_msp->cur_chip->read :
+				stm_msp->cur_chip->null_read;
+		stm_msp->delay = stm_msp->cur_chip->delay;
+
+		stm_msp_controller_cmd(stm_msp, FLUSH_FIFO);
+		stm_msp_controller_cmd(stm_msp, ENABLE_CONTROLLER);
+
+		timer_expire = stm_msp->cur_transfer->len / 1024;
+
+		if (!timer_expire)
+			timer_expire = SPI_TRANSFER_TIMEOUT_MS;
+		else
+			timer_expire =
+				(stm_msp->cur_transfer->len / 1024) *
+				SPI_TRANSFER_TIMEOUT_MS;
+
+		stm_msp->spi_notify_timer.expires =
+			jiffies + msecs_to_jiffies(timer_expire);
+
+		add_timer(&stm_msp->spi_notify_timer);
+
+		dev_dbg(&stm_msp->adev->dev, "Polling transfer ongoing...\n");
+
+		while (stm_msp->tx < stm_msp->tx_end) {
+
+			stm_msp_controller_cmd(stm_msp, DISABLE_CONTROLLER);
+			stm_msp->read(stm_msp);
+			stm_msp->write(stm_msp);
+
+			stm_msp_controller_cmd(stm_msp, ENABLE_CONTROLLER);
+
+			if (stm_msp->delay)
+				stm_msp->delay(stm_msp);
+
+			if (stm_msp->spi_io_error == 1)
+				break;
+		}
+
+		del_timer(&stm_msp->spi_notify_timer);
+
+		if (stm_msp->spi_io_error == 1)
+			goto out;
+
+		limit = loops_per_jiffy << 1;
+
+		while ((stm_msp->rx < stm_msp->rx_end) && (limit--))
+			stm_msp->read(stm_msp);
+
+		/* Update total byte transfered */
+		message->actual_length += stm_msp->cur_transfer->len;
+
+		if (stm_msp->cur_transfer->cs_change)
+			stm_msp->cur_chip->cs_control(SPI_CHIP_DESELECT);
+
+		stm_msp_controller_cmd(stm_msp, DISABLE_CONTROLLER);
+
+		/* Move to next transfer */
+		message->state = next_transfer(stm_msp);
+	}
+out:
+	/* Handle end of message */
+	if (message->state == DONE_STATE)
+		message->status = 0;
+	else
+		message->status = -EIO;
+
+	giveback(message, stm_msp);
+
+	stm_msp->spi_io_error = 0; /* Reset state for further transfers */
+
+	return;
+}
+
+/**
+ * pump_messages - Workqueue function which processes spi message queue
+ * @work: pointer to work
+ *
+ * This function checks if there is any spi message in the queue that
+ * needs processing and delegate control to appropriate function
+ * do_polling_transfer()/do_interrupt_transfer()/do_dma_transfer()
+ * based on the kind of the transfer
+ *
+ */
+static void pump_messages(struct work_struct *work)
+{
+	struct stm_msp *stm_msp = container_of(work, struct stm_msp, spi_work);
+	unsigned long flags;
+
+	/* Lock queue and check for queue work */
+	spin_lock_irqsave(&stm_msp->lock, flags);
+
+	if (list_empty(&stm_msp->queue) || stm_msp->run == QUEUE_STOPPED) {
+		dev_dbg(&stm_msp->adev->dev, "work_queue: Queue Empty\n");
+		stm_msp->busy = 0;
+		spin_unlock_irqrestore(&stm_msp->lock, flags);
+		return;
+	}
+	/* Make sure we are not already running a message */
+	if (stm_msp->cur_msg) {
+		spin_unlock_irqrestore(&stm_msp->lock, flags);
+		return;
+	}
+
+	clk_enable(stm_msp->clk);
+
+	/* Extract head of queue */
+	stm_msp->cur_msg = list_entry(stm_msp->queue.next,
+				struct spi_message,
+				queue);
+
+	list_del_init(&stm_msp->cur_msg->queue);
+	stm_msp->busy = 1;
+	spin_unlock_irqrestore(&stm_msp->lock, flags);
+
+	/* Initial message state */
+	stm_msp->cur_msg->state = START_STATE;
+	stm_msp->cur_transfer = list_entry(stm_msp->cur_msg->transfers.next,
+				struct spi_transfer,
+				transfer_list);
+
+	/* Setup the SPI using the per chip configuration */
+	stm_msp->cur_chip = spi_get_ctldata(stm_msp->cur_msg->spi);
+	stm_msp_controller_cmd(stm_msp, RESTORE_STATE);
+	stm_msp_controller_cmd(stm_msp, FLUSH_FIFO);
+
+	if (stm_msp->cur_chip->xfer_type == SPI_POLLING_TRANSFER)
+		do_polling_transfer(stm_msp);
+	else if (stm_msp->cur_chip->xfer_type == SPI_INTERRUPT_TRANSFER)
+		do_interrupt_transfer(stm_msp);
+}
+
+/**
+ * pump_transfers - Tasklet function which schedules next interrupt xfer
+ * @data: spi driver private data structure
+ */
+static void pump_transfers(unsigned long data)
+{
+	struct stm_msp *stm_msp = (struct stm_msp *)data;
+	struct spi_message *message = NULL;
+	struct spi_transfer *transfer = NULL;
+	struct spi_transfer *previous = NULL;
+
+	message = stm_msp->cur_msg;
+
+	/* Handle for abort */
+	if (message->state == ERROR_STATE) {
+		message->status = -EIO;
+		giveback(message, stm_msp);
+		return;
+	}
+
+	/* Handle end of message */
+	if (message->state == DONE_STATE) {
+		message->status = 0;
+		giveback(message, stm_msp);
+		return;
+	}
+	transfer = stm_msp->cur_transfer;
+
+	/* Delay if requested at end of transfer */
+	if (message->state == RUNNING_STATE) {
+		previous = list_entry(transfer->transfer_list.prev,
+					struct spi_transfer, transfer_list);
+		if (previous->delay_usecs)
+			udelay(previous->delay_usecs);
+		if (previous->cs_change)
+			stm_msp->cur_chip->cs_control(SPI_CHIP_SELECT);
+	} else {
+		/* START_STATE */
+		message->state = RUNNING_STATE;
+	}
+	stm_msp->tx = (void *)transfer->tx_buf;
+	stm_msp->tx_end = stm_msp->tx + stm_msp->cur_transfer->len;
+	stm_msp->rx = (void *)transfer->rx_buf;
+	stm_msp->rx_end = stm_msp->rx + stm_msp->cur_transfer->len;
+
+	stm_msp->write = stm_msp->tx ?
+		stm_msp->cur_chip->write : stm_msp->cur_chip->null_write;
+	stm_msp->read = stm_msp->rx ?
+		stm_msp->cur_chip->read : stm_msp->cur_chip->null_read;
+
+	stm_msp_controller_cmd(stm_msp, FLUSH_FIFO);
+	stm_msp_controller_cmd(stm_msp, ENABLE_ALL_INTERRUPT);
+}
+
+static int init_queue(struct stm_msp *stm_msp)
+{
+	INIT_LIST_HEAD(&stm_msp->queue);
+	spin_lock_init(&stm_msp->lock);
+
+	stm_msp->run = QUEUE_STOPPED;
+	stm_msp->busy = 0;
+
+	tasklet_init(&stm_msp->pump_transfers, pump_transfers,
+			(unsigned long)stm_msp);
+	INIT_WORK(&stm_msp->spi_work, pump_messages);
+
+#ifdef CONFIG_SPI_WORKQUEUE
+	stm_msp->workqueue = create_singlethread_workqueue(
+		dev_name(&stm_msp->master->dev));
+
+	if (stm_msp->workqueue == NULL)
+		return -EBUSY;
+#endif /* CONFIG_SPI_WORKQUEUE */
+
+	init_timer(&stm_msp->spi_notify_timer);
+
+	stm_msp->spi_notify_timer.expires = jiffies + msecs_to_jiffies(1000);
+	stm_msp->spi_notify_timer.function = spi_notify;
+	stm_msp->spi_notify_timer.data = (unsigned long)stm_msp;
+
+	return 0;
+}
+
+static int start_queue(struct stm_msp *stm_msp)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&stm_msp->lock, flags);
+
+	if (stm_msp->run == QUEUE_RUNNING || stm_msp->busy) {
+		spin_unlock_irqrestore(&stm_msp->lock, flags);
+		return -EBUSY;
+	}
+
+	stm_msp->run = QUEUE_RUNNING;
+	stm_msp->cur_msg = NULL;
+	stm_msp->cur_transfer = NULL;
+	stm_msp->cur_chip = NULL;
+	spin_unlock_irqrestore(&stm_msp->lock, flags);
+	return 0;
+}
+
+static int stop_queue(struct stm_msp *stm_msp)
+{
+	unsigned long flags;
+	unsigned limit = 500;
+	int status = 0;
+
+	spin_lock_irqsave(&stm_msp->lock, flags);
+
+	/* This is a bit lame, but is optimized for the common execution path.
+	 * A wait_queue on the stm_msp->busy could be used, but then the common
+	 * execution path (pump_messages) would be required to call wake_up or
+	 * friends on every SPI message. Do this instead */
+
+	stm_msp->run = QUEUE_STOPPED;
+
+	while (!list_empty(&stm_msp->queue) && stm_msp->busy && limit--) {
+		spin_unlock_irqrestore(&stm_msp->lock, flags);
+		msleep(10);
+		spin_lock_irqsave(&stm_msp->lock, flags);
+	}
+
+	if (!list_empty(&stm_msp->queue) || stm_msp->busy)
+		status = -EBUSY;
+
+	spin_unlock_irqrestore(&stm_msp->lock, flags);
+
+	return status;
+}
+
+static int destroy_queue(struct stm_msp *stm_msp)
+{
+	int status;
+
+	status = stop_queue(stm_msp);
+
+	if (status != 0)
+		return status;
+#ifdef CONFIG_SPI_WORKQUEUE
+	destroy_workqueue(stm_msp->workqueue);
+#endif
+	del_timer_sync(&stm_msp->spi_notify_timer);
+
+	return 0;
+}
+
+/**
+ * stm_msp_null_writer - To Write Dummy Data in Data register
+ * @stm_msp: spi driver private data structure
+ *
+ * This function is set as a write function for transfer which have
+ * Tx transfer buffer as NULL. It simply writes '0' in the Data
+ * register
+ */
+static void stm_msp_null_writer(struct stm_msp *stm_msp)
+{
+	u32 cur_write = 0;
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_TFU) ||
+			(stm_msp->tx >= stm_msp->tx_end))
+				return;
+
+		writel(0x0, MSP_DR(stm_msp->regs));
+		stm_msp->tx += (stm_msp->cur_chip->n_bytes);
+		cur_write++;
+
+		if (cur_write == 8)
+			return;
+	}
+}
+
+/**
+ * stm_msp_null_reader - To read data from Data register and discard it
+ * @stm_msp: spi driver private data structure
+ *
+ * This function is set as a reader function for transfer which have
+ * Rx Transfer buffer as null. Read Data is rejected
+ */
+static void stm_msp_null_reader(struct stm_msp *stm_msp)
+{
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_RFE) ||
+			(stm_msp->rx >= stm_msp->rx_end))
+			return;
+
+		readl(MSP_DR(stm_msp->regs));
+		stm_msp->rx += (stm_msp->cur_chip->n_bytes);
+	}
+}
+
+/**
+ * stm_msp_u8_writer - Write FIFO data in Data register as a 8 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function writes data in Tx FIFO till it is not full
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary write ptr tx in stm_msp which maintains
+ * current write position in transfer buffer. we do not write data more than
+ * FIFO depth
+ */
+void stm_msp_u8_writer(struct stm_msp *stm_msp)
+{
+	u32 cur_write = 0;
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_TFU) ||
+			(stm_msp->tx >= stm_msp->tx_end))
+			return;
+
+		writel((u32)(*(u8 *)(stm_msp->tx)), MSP_DR(stm_msp->regs));
+		stm_msp->tx += (stm_msp->cur_chip->n_bytes);
+		cur_write++;
+
+		if (cur_write == MSP_FIFO_DEPTH)
+			return;
+	}
+}
+
+/**
+ * stm_msp_u8_reader - Read FIFO data in Data register as a 8 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function reads data in Rx FIFO till it is not empty
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary Read ptr rx in stm_msp which maintains
+ * current read position in transfer buffer
+ */
+void stm_msp_u8_reader(struct stm_msp *stm_msp)
+{
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_RFE) ||
+			(stm_msp->rx >= stm_msp->rx_end))
+			return;
+
+		*(u8 *)(stm_msp->rx) = (u8)readl(MSP_DR(stm_msp->regs));
+		stm_msp->rx += (stm_msp->cur_chip->n_bytes);
+	}
+}
+
+/**
+ * stm_msp_u16_writer - Write FIFO data in Data register as a 16 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function writes data in Tx FIFO till it is not full
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary write ptr tx in stm_msp which maintains
+ * current write position in transfer buffer. we do not write data more than
+ * FIFO depth
+ */
+void stm_msp_u16_writer(struct stm_msp *stm_msp)
+{
+	u32 cur_write = 0;
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_TFU) ||
+			(stm_msp->tx >= stm_msp->tx_end))
+			return;
+
+		writel((u32)(*(u16 *)(stm_msp->tx)), MSP_DR(stm_msp->regs));
+		stm_msp->tx += (stm_msp->cur_chip->n_bytes);
+		cur_write++;
+
+		if (cur_write == MSP_FIFO_DEPTH)
+			return;
+	}
+}
+
+/**
+ * stm_msp_u16_reader - Read FIFO data in Data register as a 16 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function reads data in Rx FIFO till it is not empty
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary Read ptr rx in stm_msp which maintains
+ * current read position in transfer buffer
+ */
+void stm_msp_u16_reader(struct stm_msp *stm_msp)
+{
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_RFE) ||
+			(stm_msp->rx >= stm_msp->rx_end))
+			return;
+
+		*(u16 *)(stm_msp->rx) = (u16)readl(MSP_DR(stm_msp->regs));
+		stm_msp->rx += (stm_msp->cur_chip->n_bytes);
+	}
+}
+
+/**
+ * stm_msp_u32_writer - Write FIFO data in Data register as a 32 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function writes data in Tx FIFO till it is not full
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary write ptr tx in stm_msp which maintains
+ * current write position in transfer buffer. we do not write data more than
+ * FIFO depth
+ */
+void stm_msp_u32_writer(struct stm_msp *stm_msp)
+{
+	u32 cur_write = 0;
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_TFU) ||
+			(stm_msp->tx >= stm_msp->tx_end))
+			return;
+
+		/* Write Data to Data Register */
+		writel(*(u32 *)(stm_msp->tx), MSP_DR(stm_msp->regs));
+		stm_msp->tx += (stm_msp->cur_chip->n_bytes);
+		cur_write++;
+
+		if (cur_write == MSP_FIFO_DEPTH)
+			return;
+	}
+}
+
+/**
+ * stm_msp_u32_reader - Read FIFO data in Data register as a 32 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function reads data in Rx FIFO till it is not empty
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary Read ptr rx in stm_msp which maintains
+ * current read position in transfer buffer
+ */
+void stm_msp_u32_reader(struct stm_msp *stm_msp)
+{
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_RFE) ||
+			(stm_msp->rx >= stm_msp->rx_end))
+			return;
+
+		*(u32 *)(stm_msp->rx) = readl(MSP_DR(stm_msp->regs));
+		stm_msp->rx += (stm_msp->cur_chip->n_bytes);
+	}
+}
+
+/**
+ * stm_msp_interrupt_handler - Interrupt hanlder function
+ */
+static irqreturn_t stm_msp_interrupt_handler(int irq, void *dev_id)
+{
+	struct stm_msp *stm_msp = (struct stm_msp *)dev_id;
+	struct spi_message *msg = stm_msp->cur_msg;
+	u32 irq_status = 0;
+	u32 flag = 0;
+
+	if (!msg) {
+		dev_err(&stm_msp->adev->dev,
+			"Bad message state in interrupt handler");
+		/* Never fail */
+		return IRQ_HANDLED;
+	}
+
+	/* Read the Interrupt Status Register */
+	irq_status = readl(MSP_MIS(stm_msp->regs));
+
+	if (irq_status) {
+		if (irq_status & MSP_MIS_MASK_ROEMIS) {	/* Overrun interrupt */
+			/* Bail out our Data has been corrupted */
+			dev_dbg(&stm_msp->adev->dev,
+				"Received ROR interrupt\n");
+
+			stm_msp_controller_cmd(stm_msp, DISABLE_ALL_INTERRUPT);
+			stm_msp_controller_cmd(stm_msp, CLEAR_ALL_INTERRUPT);
+			stm_msp_controller_cmd(stm_msp, DISABLE_CONTROLLER);
+			msg->state = ERROR_STATE;
+			tasklet_schedule(&stm_msp->pump_transfers);
+			return IRQ_HANDLED;
+		}
+
+		stm_msp->read(stm_msp);
+		stm_msp->write(stm_msp);
+
+		if ((stm_msp->tx == stm_msp->tx_end) && (flag == 0)) {
+			flag = 1;
+			/* Disable Transmit interrupt */
+			writel(readl(MSP_IMSC(stm_msp->regs)) &
+				(~MSP_IMSC_MASK_TXIM) & (~MSP_IMSC_MASK_TFOIM),
+			       (stm_msp->regs + 0x14));
+		}
+
+		/* Clearing any Xmit underrun error. Overrun already handled */
+		stm_msp_controller_cmd(stm_msp, CLEAR_ALL_INTERRUPT);
+
+		if (stm_msp->rx == stm_msp->rx_end) {
+			stm_msp_controller_cmd(stm_msp, DISABLE_ALL_INTERRUPT);
+			stm_msp_controller_cmd(stm_msp, CLEAR_ALL_INTERRUPT);
+
+			dev_dbg(&stm_msp->adev->dev,
+				"Interrupt transfer completed.\n");
+
+			/* Update total bytes transfered */
+			msg->actual_length += stm_msp->cur_transfer->len;
+
+			if (stm_msp->cur_transfer->cs_change)
+				stm_msp->cur_chip->cs_control(
+					SPI_CHIP_DESELECT);
+
+			/* Move to next transfer */
+			msg->state = next_transfer(stm_msp);
+			tasklet_schedule(&stm_msp->pump_transfers);
+			return IRQ_HANDLED;
+		}
+	}
+	return IRQ_HANDLED;
+}
+
+/**
+ * stm_msp_cleanup - cleanup function registered to SPI master framework
+ * @spi: spi device which is requesting cleanup
+ *
+ * This function is registered to the SPI framework for this SPI master
+ * controller. It will free the runtime state of chip.
+ */
+static void stm_msp_cleanup(struct spi_device *spi)
+{
+	struct chip_data *chip = spi_get_ctldata((struct spi_device *)spi);
+	struct stm_msp *stm_msp = spi_master_get_devdata(spi->master);
+	struct spi_master *master;
+	master = stm_msp->master;
+
+	if (chip) {
+		kfree(chip->ctr_regs);
+		kfree(chip);
+		spi_set_ctldata(spi, NULL);
+	}
+}
+
+/**
+ * null_cs_control - Dummy chip select function
+ * @command: select/delect the chip
+ *
+ * If no chip select function is provided by client this is used as dummy
+ * chip select
+ */
+static void null_cs_control(u32 command)
+{
+	/* Nothing to do */
+	(void)command;
+}
+
+static int verify_msp_controller_parameters(struct stm_msp_config_chip
+		*chip_info)
+{
+
+	/* FIXME: check clock params */
+	if ((chip_info->lbm != SPI_LOOPBACK_ENABLED) &&
+		(chip_info->lbm != SPI_LOOPBACK_DISABLED)) {
+		dev_dbg(chip_info->dev,
+			"Loopback Mode is configured incorrectly\n");
+		return -1;
+	}
+	if ((chip_info->hierarchy != SPI_MASTER) &&
+		(chip_info->hierarchy != SPI_SLAVE)) {
+		dev_dbg(chip_info->dev,
+			"hierarchy is configured incorrectly\n");
+		return -1;
+	}
+	if ((chip_info->endian_rx != SPI_FIFO_MSB) &&
+		(chip_info->endian_rx != SPI_FIFO_LSB)) {
+		dev_dbg(chip_info->dev,
+			"Rx FIFO endianess is configured incorrectly\n");
+		return -1;
+	}
+	if ((chip_info->endian_tx != SPI_FIFO_MSB) &&
+		(chip_info->endian_tx != SPI_FIFO_LSB)) {
+		dev_dbg(chip_info->dev,
+			"Tx FIFO endianess is configured incorrectly\n");
+		return -1;
+	}
+	if ((chip_info->data_size < MSP_DATA_BITS_8) ||
+		(chip_info->data_size > MSP_DATA_BITS_32)) {
+		dev_dbg(chip_info->dev,
+			"MSP DATA Size is configured incorrectly\n");
+		return -1;
+	}
+	if ((chip_info->com_mode != SPI_INTERRUPT_TRANSFER) &&
+		(chip_info->com_mode != SPI_POLLING_TRANSFER)) {
+		dev_dbg(chip_info->dev,
+			"Communication mode is configured incorrectly\n");
+		return -1;
+	}
+	if (((chip_info->proto_params).clk_phase !=
+	     SPI_CLK_ZERO_CYCLE_DELAY) &&
+		((chip_info->proto_params).clk_phase !=
+		SPI_CLK_HALF_CYCLE_DELAY)) {
+		dev_dbg(chip_info->dev,
+			"Clock Phase is configured incorrectly\n");
+		return -1;
+	}
+	if (((chip_info->proto_params).clk_pol !=
+	     SPI_CLK_POL_IDLE_LOW) &&
+		((chip_info->proto_params).clk_pol !=
+		SPI_CLK_POL_IDLE_HIGH)) {
+		dev_dbg(chip_info->dev,
+			"Clk Polarity configured incorrectly\n");
+		return -1;
+	}
+	if (chip_info->cs_control == NULL) {
+		dev_dbg(chip_info->dev,
+			"Chip Select Function is NULL for this chip\n");
+		chip_info->cs_control = null_cs_control;
+	}
+	return 0;
+}
+
+static struct stm_msp_config_chip *allocate_default_msp_chip_cfg(
+		struct spi_device *spi)
+{
+	struct stm_msp_config_chip *chip_info;
+
+	chip_info = kzalloc(sizeof(struct stm_msp_config_chip), GFP_KERNEL);
+
+	if (!chip_info) {
+		dev_err(&spi->dev, "setup - cannot allocate controller data");
+		return NULL;
+	}
+	dev_dbg(&spi->dev, "Allocated Memory for controller data\n");
+
+	chip_info->lbm = SPI_LOOPBACK_DISABLED;
+	chip_info->com_mode = SPI_POLLING_TRANSFER;
+	chip_info->hierarchy = SPI_MASTER;
+	chip_info->endian_tx = SPI_FIFO_LSB;
+	chip_info->endian_rx = SPI_FIFO_LSB;
+	chip_info->data_size = MSP_DATA_BITS_32;
+
+	if (spi->max_speed_hz != 0)
+		chip_info->freq = spi->max_speed_hz;
+	else
+		chip_info->freq = SPI_DEFAULT_MAX_SPEED_HZ;
+
+	chip_info->proto_params.clk_phase = SPI_CLK_HALF_CYCLE_DELAY;
+	chip_info->proto_params.clk_pol = SPI_CLK_POL_IDLE_LOW;
+	chip_info->cs_control = null_cs_control;
+
+	return chip_info;
+}
+
+static void stm_msp_delay(struct stm_msp *stm_msp)
+{
+	udelay(15);
+
+	while (readl(MSP_FLR(stm_msp->regs)) &
+		(MSP_FLR_MASK_RBUSY | MSP_FLR_MASK_TBUSY))
+			udelay(1);
+}
+
+/**
+ * stm_msp_setup - setup function registered to SPI master framework
+ * @spi: spi device which is requesting setup
+ *
+ * This function is registered to the SPI framework for this SPI master
+ * controller. If it is the first time when setup is called by this device,
+ * this function will initialize the runtime state for this chip and save
+ * the same in the device structure. Else it will update the runtime info
+ * with the updated chip info.
+ */
+static int stm_msp_setup(struct spi_device *spi)
+{
+	struct stm_msp_config_chip *chip_info;
+	struct chip_data *curr_cfg;
+	struct spi_master *master;
+	int status = 0;
+	u16 sckdiv = 0;
+	s16 bus_num = 0;
+	struct stm_msp *stm_msp = spi_master_get_devdata(spi->master);
+	struct msp_regs *msp_regs;
+	master = stm_msp->master;
+	bus_num = master->bus_num - 1;
+
+	/* Get controller data */
+	chip_info = spi->controller_data;
+	/* Get controller_state */
+	curr_cfg = spi_get_ctldata(spi);
+
+	if (curr_cfg == NULL) {
+		curr_cfg = kzalloc(sizeof(struct chip_data), GFP_KERNEL);
+
+		if (!curr_cfg) {
+			dev_err(&stm_msp->adev->dev,
+				"setup - cannot allocate controller state");
+			return -ENOMEM;
+		}
+
+		curr_cfg->chip_id = spi->chip_select;
+		curr_cfg->ctr_regs = kzalloc(sizeof(struct msp_regs),
+						GFP_KERNEL);
+
+		if (curr_cfg->ctr_regs == NULL) {
+			dev_err(&stm_msp->adev->dev,
+				"setup - cannot allocate mem for regs");
+			goto err_first_setup;
+		}
+
+		dev_err(&stm_msp->adev->dev,
+			"chip Id = %d\n", curr_cfg->chip_id);
+
+		if (chip_info == NULL) {
+			chip_info = allocate_default_msp_chip_cfg(spi);
+
+			if (!chip_info) {
+				dev_err(&stm_msp->adev->dev,
+					"setup - cannot allocate cntlr data");
+				status = -ENOMEM;
+				goto err_first_setup;
+			}
+
+			spi->controller_data = chip_info;
+		}
+	}
+
+	/* Pointer back to the SPI device */
+	chip_info->dev = &spi->dev;
+
+	if (chip_info->freq == 0) {
+		/* Calculate Specific Freq. */
+		if ((MSP_INTERNAL_CLK == chip_info->clk_freq.clk_src) ||
+			(MSP_EXTERNAL_CLK == chip_info->clk_freq.clk_src)) {
+			sckdiv = chip_info->clk_freq.sckdiv;
+		} else {
+			status = -1;
+			dev_err(&stm_msp->adev->dev,
+				"setup - controller clock data is incorrect");
+			goto err_config_params;
+		}
+	} else {
+		/* Calculate Effective Freq. */
+		sckdiv = (DEFAULT_MSP_CLK / (chip_info->freq)) - 1;
+
+		if (sckdiv > MAX_SCKDIV) {
+			dev_dbg(&stm_msp->adev->dev,
+				"SPI: Cannot set frequency less than 48Khz,"
+				"setting lowest(48 Khz)\n");
+			sckdiv = MAX_SCKDIV;
+		}
+	}
+
+	status = verify_msp_controller_parameters(chip_info);
+
+	if (status) {
+		dev_err(&stm_msp->adev->dev,
+			"setup - controller data is incorrect");
+		goto err_config_params;
+	}
+
+	/* Now set controller state based on controller data */
+	curr_cfg->xfer_type = chip_info->com_mode;
+	curr_cfg->cs_control = chip_info->cs_control;
+	curr_cfg->delay = stm_msp_delay;
+
+	curr_cfg->null_write = stm_msp_null_writer;
+	curr_cfg->null_read = stm_msp_null_reader;
+
+	if (chip_info->data_size <= MSP_DATA_BITS_8) {
+		dev_dbg(&stm_msp->adev->dev, "Less than 8 bits per word...\n");
+
+		curr_cfg->n_bytes = 1;
+		curr_cfg->read = stm_msp_u8_reader;
+		curr_cfg->write = stm_msp_u8_writer;
+	} else if (chip_info->data_size <= MSP_DATA_BITS_16) {
+		dev_dbg(&stm_msp->adev->dev, "Less than 16 bits per word...\n");
+
+		curr_cfg->n_bytes = 2;
+		curr_cfg->read = stm_msp_u16_reader;
+		curr_cfg->write = stm_msp_u16_writer;
+	} else {
+		dev_dbg(&stm_msp->adev->dev, "Less than 32 bits per word...\n");
+
+		curr_cfg->n_bytes = 4;
+		curr_cfg->read = stm_msp_u32_reader;
+		curr_cfg->write = stm_msp_u32_writer;
+	}
+
+	/* Now initialize all register settings reqd. for this chip */
+
+	msp_regs = (struct msp_regs *)(curr_cfg->ctr_regs);
+	msp_regs->gcr = 0x0;
+	msp_regs->tcf = 0x0;
+	msp_regs->rcf = 0x0;
+	msp_regs->srg = 0x0;
+	msp_regs->dmacr = 0x0;
+
+	MSP_WBITS(msp_regs->dmacr, 0x0, MSP_DMACR_MASK_RDMAE, 0);
+	MSP_WBITS(msp_regs->dmacr, 0x0, MSP_DMACR_MASK_TDMAE, 1);
+
+	/* GCR Reg Config */
+
+	MSP_WBITS(msp_regs->gcr,
+			MSP_RECEIVER_DISABLED, MSP_GCR_MASK_RXEN, 0);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_RX_FIFO_ENABLED, MSP_GCR_MASK_RFFEN, 1);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_TRANSMITTER_DISABLED, MSP_GCR_MASK_TXEN, 8);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_TX_FIFO_ENABLED, MSP_GCR_MASK_TFFEN, 9);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_TX_FRAME_SYNC_POL_LOW, MSP_GCR_MASK_TFSPOL, 10);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_TX_FRAME_SYNC_INT, MSP_GCR_MASK_TFSSEL, 11);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_TRANSMIT_DATA_WITH_DELAY, MSP_GCR_MASK_TXDDL, 15);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_SAMPLE_RATE_GEN_ENABLE, MSP_GCR_MASK_SGEN, 16);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_CLOCK_INTERNAL, MSP_GCR_MASK_SCKSEL, 18);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_FRAME_GEN_ENABLE, MSP_GCR_MASK_FGEN, 20);
+	MSP_WBITS(msp_regs->gcr,
+			SPI_BURST_MODE_DISABLE, MSP_GCR_MASK_SPIBME, 23);
+
+	if (chip_info->lbm == SPI_LOOPBACK_ENABLED)
+		MSP_WBITS(msp_regs->gcr,
+				MSP_LOOPBACK_ENABLED, MSP_GCR_MASK_LBM, 7);
+	else
+		MSP_WBITS(msp_regs->gcr,
+				MSP_LOOPBACK_DISABLED, MSP_GCR_MASK_LBM, 7);
+
+	if (chip_info->hierarchy == SPI_MASTER)
+		MSP_WBITS(msp_regs->gcr,
+				MSP_IS_SPI_MASTER, MSP_GCR_MASK_TCKSEL, 14);
+	else
+		MSP_WBITS(msp_regs->gcr,
+				MSP_IS_SPI_SLAVE, MSP_GCR_MASK_TCKSEL, 14);
+
+	if (chip_info->proto_params.clk_phase == SPI_CLK_ZERO_CYCLE_DELAY)
+		MSP_WBITS(msp_regs->gcr,
+				MSP_SPI_PHASE_ZERO_CYCLE_DELAY,
+				MSP_GCR_MASK_SPICKM, 21);
+	else
+		MSP_WBITS(msp_regs->gcr,
+				MSP_SPI_PHASE_HALF_CYCLE_DELAY,
+				MSP_GCR_MASK_SPICKM, 21);
+
+	if (chip_info->proto_params.clk_pol == SPI_CLK_POL_IDLE_HIGH)
+		MSP_WBITS(msp_regs->gcr,
+				MSP_TX_CLOCK_POL_HIGH, MSP_GCR_MASK_TCKPOL, 13);
+	else
+		MSP_WBITS(msp_regs->gcr,
+				MSP_TX_CLOCK_POL_LOW, MSP_GCR_MASK_TCKPOL, 13);
+
+	/* RCF Reg Config */
+	MSP_WBITS(msp_regs->rcf,
+			MSP_IGNORE_RX_FRAME_SYNC_PULSE, MSP_RCF_MASK_RFSIG, 15);
+	MSP_WBITS(msp_regs->rcf,
+			MSP_RX_1BIT_DATA_DELAY, MSP_RCF_MASK_RDDLY, 13);
+
+	if (chip_info->endian_rx == SPI_FIFO_LSB)
+		MSP_WBITS(msp_regs->rcf,
+				MSP_RX_ENDIANESS_LSB, MSP_RCF_MASK_RENDN, 12);
+	else
+		MSP_WBITS(msp_regs->rcf,
+				MSP_RX_ENDIANESS_MSB, MSP_RCF_MASK_RENDN, 12);
+
+	MSP_WBITS(msp_regs->rcf, chip_info->data_size, MSP_RCF_MASK_RP1ELEN, 0);
+
+	/* TCF Reg Config */
+
+	MSP_WBITS(msp_regs->tcf,
+			MSP_IGNORE_TX_FRAME_SYNC_PULSE, MSP_TCF_MASK_TFSIG, 15);
+	MSP_WBITS(msp_regs->tcf,
+			MSP_TX_1BIT_DATA_DELAY, MSP_TCF_MASK_TDDLY, 13);
+
+	if (chip_info->endian_rx == SPI_FIFO_LSB)
+		MSP_WBITS(msp_regs->tcf,
+				MSP_TX_ENDIANESS_LSB, MSP_TCF_MASK_TENDN, 12);
+	else
+		MSP_WBITS(msp_regs->tcf,
+				MSP_TX_ENDIANESS_MSB, MSP_TCF_MASK_TENDN, 12);
+	MSP_WBITS(msp_regs->tcf, chip_info->data_size, MSP_TCF_MASK_TP1ELEN, 0);
+
+	/* SRG Reg Config */
+
+	MSP_WBITS(msp_regs->srg, sckdiv, MSP_SRG_MASK_SCKDIV, 0);
+
+	/* Save controller_state */
+	spi_set_ctldata(spi, curr_cfg);
+
+	return status;
+
+err_config_params:
+err_first_setup:
+
+	kfree(curr_cfg);
+	return status;
+}
+
+static int __init stm_msp_probe(struct amba_device *adev, const struct amba_id *id)
+{
+	struct device *dev = &adev->dev;
+	struct stm_msp_controller *platform_info = adev->dev.platform_data;
+	struct spi_master *master;
+	struct stm_msp *stm_msp = NULL;	/* Data for this driver */
+	int irq, status = 0;
+
+	dev_info(dev, "STM MSP driver, device ID: 0x%08x\n", adev->periphid);
+
+	if (platform_info == NULL) {
+		dev_err(dev, "probe - no platform data supplied\n");
+		status = -ENODEV;
+		goto err_no_pdata;
+	}
+
+	/* Allocate master with space for data */
+	master = spi_alloc_master(dev, sizeof(struct stm_msp));
+
+	if (master == NULL) {
+		dev_err(dev, "probe - cannot alloc spi_master\n");
+		status = -ENOMEM;
+		goto err_no_mem;
+	}
+
+	stm_msp = spi_master_get_devdata(master);
+	stm_msp->master = master;
+	stm_msp->master_info = platform_info;
+	stm_msp->adev = adev;
+
+	stm_msp->clk = clk_get(&adev->dev, NULL);
+
+	if (IS_ERR(stm_msp->clk)) {
+		dev_err(dev, "probe - cannot find clock\n");
+		status = PTR_ERR(stm_msp->clk);
+		goto free_master;
+	}
+
+	/* Fetch the Resources, using platform data */
+	status = amba_request_regions(adev, NULL);
+
+	if (status) {
+		status = -ENODEV;
+		goto disable_clk;
+	}
+
+	/* Get Hold of Device Register Area... */
+	stm_msp->regs = ioremap(adev->res.start, resource_size(&adev->res));
+
+	if (stm_msp->regs == NULL) {
+		status = -ENODEV;
+		goto disable_clk;
+	}
+
+	irq = adev->irq[0];
+
+	if (irq <= 0) {
+		status = -ENODEV;
+		goto err_no_iores;
+	}
+
+	stm_msp_controller_cmd(stm_msp, LOAD_DEFAULT_CONFIG);
+
+	/* Required Info for an SPI controller */
+	/* Bus Number Which Assigned to this SPI controller on this board */
+	master->bus_num = (u16) platform_info->id;
+	master->num_chipselect = platform_info->num_chipselect;
+	master->setup = stm_msp_setup;
+	master->cleanup = (void *)stm_msp_cleanup;
+	master->transfer = stm_msp_transfer;
+
+	dev_dbg(dev, "BUSNO: %d\n", master->bus_num);
+
+	/* Initialize and start queue */
+	status = init_queue(stm_msp);
+
+	if (status != 0) {
+		dev_err(dev, "probe - problem initializing queue\n");
+		goto err_init_queue;
+	}
+
+	status = start_queue(stm_msp);
+
+	if (status != 0) {
+		dev_err(dev, "probe - problem starting queue\n");
+		goto err_start_queue;
+	}
+
+	amba_set_drvdata(adev, stm_msp);
+
+	dev_dbg(dev, "probe succeded\n");
+	dev_dbg(dev, "Bus No = %d, IRQ Line = %d, Virtual Addr: %x\n",
+		master->bus_num, irq, (u32)(stm_msp->regs));
+
+	status = request_irq(stm_msp->adev->irq[0],
+			stm_msp_interrupt_handler,
+			0, stm_msp->master_info->device_name,
+			stm_msp);
+
+	if (status < 0) {
+		dev_err(dev, "probe - cannot get IRQ (%d)\n", status);
+		goto err_irq;
+	}
+
+	/* Register with the SPI framework */
+	status = spi_register_master(master);
+
+	if (status != 0) {
+		dev_err(dev, "probe - problem registering spi master\n");
+		goto err_spi_register;
+	}
+
+	return 0;
+
+err_spi_register:
+	free_irq(stm_msp->adev->irq[0], stm_msp);
+err_irq:
+err_init_queue:
+err_start_queue:
+	destroy_queue(stm_msp);
+err_no_iores:
+	iounmap(stm_msp->regs);
+disable_clk:
+	clk_put(stm_msp->clk);
+free_master:
+	spi_master_put(master);
+err_no_mem:
+err_no_pdata:
+	return status;
+}
+
+static int __exit stm_msp_remove(struct amba_device *adev)
+{
+	struct stm_msp *stm_msp = amba_get_drvdata(adev);
+	int status = 0;
+
+	if (!stm_msp)
+		return 0;
+
+	/* Remove the queue */
+	status = destroy_queue(stm_msp);
+
+	if (status != 0) {
+		dev_err(&adev->dev, "queue remove failed (%d)\n", status);
+		return status;
+	}
+
+	stm_msp_controller_cmd(stm_msp, LOAD_DEFAULT_CONFIG);
+
+	/* Release map resources */
+	iounmap(stm_msp->regs);
+	amba_release_regions(adev);
+	tasklet_disable(&stm_msp->pump_transfers);
+	free_irq(stm_msp->adev->irq[0], stm_msp);
+
+	/* Disconnect from the SPI framework */
+	spi_unregister_master(stm_msp->master);
+
+	clk_put(stm_msp->clk);
+
+	/* Prevent double remove */
+	amba_set_drvdata(adev, NULL);
+	dev_dbg(&adev->dev, "remove succeded\n");
+	return status;
+}
+
+#ifdef CONFIG_PM
+
+/**
+ * stm_msp_suspend - MSP suspend function registered with PM framework.
+ * @dev: Reference to amba device structure of the device
+ * @state: power mgmt state.
+ *
+ * This function is invoked when the system is going into sleep, called
+ * by the power management framework of the linux kernel.
+ */
+static int stm_msp_suspend(struct amba_device *adev, pm_message_t state)
+{
+	struct stm_msp *stm_msp = amba_get_drvdata(adev);
+	int status = 0;
+
+	status = stop_queue(stm_msp);
+
+	if (status != 0) {
+		dev_warn(&adev->dev, "suspend cannot stop queue\n");
+		return status;
+	}
+
+	dev_dbg(&adev->dev, "suspended\n");
+	return 0;
+}
+
+/**
+ * stm_msp_resume - MSP Resume function registered with PM framework.
+ * @dev: Reference to amba device structure of the device
+ *
+ * This function is invoked when the system is coming out of sleep, called
+ * by the power management framework of the linux kernel.
+ */
+static int stm_msp_resume(struct amba_device *adev)
+{
+	struct stm_msp *stm_msp = amba_get_drvdata(adev);
+	int status = 0;
+
+	/* Start the queue running */
+	status = start_queue(stm_msp);
+
+	if (status)
+		dev_err(&adev->dev, "problem starting queue (%d)\n", status);
+	else
+		dev_dbg(&adev->dev, "resumed\n");
+
+	return status;
+}
+
+#else
+#define stm_msp_suspend	NULL
+#define stm_msp_resume	NULL
+#endif /* CONFIG_PM */
+
+static struct amba_id stm_msp_ids[] = {
+	{
+		.id = 0x00280021,
+		.mask = 0x00ffffff,
+	},
+	{
+		0,
+		0,
+	},
+};
+
+static struct amba_driver __refdata stm_msp_driver = {
+	.drv = {
+		.name	= "MSP",
+	},
+	.id_table	= stm_msp_ids,
+	.probe		= stm_msp_probe,
+	.remove		= __exit_p(stm_msp_remove),
+	.resume		= stm_msp_resume,
+	.suspend	= stm_msp_suspend,
+};
+
+static int __init stm_msp_init(void)
+{
+	return amba_driver_register(&stm_msp_driver);
+}
+
+static void __exit stm_msp_exit(void)
+{
+	amba_driver_unregister(&stm_msp_driver);
+}
+
+module_init(stm_msp_init);
+module_exit(stm_msp_exit);
+
+MODULE_AUTHOR("Sachin Verma <sachin.verma@st.com>");
+MODULE_DESCRIPTION("STM MSP (SPI protocol) Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 11a4b5b3596..d9ca441d053 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -24,6 +24,16 @@ menuconfig STAGING
 
 if STAGING
 
+config AB5500_SIM
+        bool "ST-Ericsson AB5500 SIM Interface driver"
+        depends on AB5500_CORE
+        help
+          SIM Interface driver provides interface to configure
+          various parameters of AB5550 SIM Level Shifter.Support provided are:
+          Configure Pull up on sim lines
+          Configure Operation Mode
+          Notify Sim Insert/Extract Interrupt
+
 source "drivers/staging/tty/Kconfig"
 
 source "drivers/staging/generic_serial/Kconfig"
@@ -162,4 +172,14 @@ source "drivers/staging/mei/Kconfig"
 
 source "drivers/staging/nvec/Kconfig"
 
+source "drivers/staging/cg2900/Kconfig"
+
+source "drivers/staging/cw1200/Kconfig"
+
+source "drivers/staging/mmio/Kconfig"
+
+source "drivers/staging/nmf-cm/Kconfig"
+
+source "drivers/staging/camera_flash/Kconfig"
+
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index ae62e923e1b..052526edb3b 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -16,8 +16,8 @@ obj-$(CONFIG_USBIP_CORE)	+= usbip/
 obj-$(CONFIG_W35UND)		+= winbond/
 obj-$(CONFIG_PRISM2_USB)	+= wlan-ng/
 obj-$(CONFIG_ECHO)		+= echo/
-obj-$(CONFIG_BRCMSMAC)		+= brcm80211/
 obj-$(CONFIG_BRCMFMAC)		+= brcm80211/
+obj-$(CONFIG_CG2900)		+= cg2900/
 obj-$(CONFIG_COMEDI)		+= comedi/
 obj-$(CONFIG_FB_OLPC_DCON)	+= olpc_dcon/
 obj-$(CONFIG_ASUS_OLED)		+= asus_oled/
@@ -72,3 +72,8 @@ obj-$(CONFIG_TOUCHSCREEN_SYNAPTICS_I2C_RMI4)	+= ste_rmi4/
 obj-$(CONFIG_DRM_PSB)		+= gma500/
 obj-$(CONFIG_INTEL_MEI)		+= mei/
 obj-$(CONFIG_MFD_NVEC)		+= nvec/
+obj-$(CONFIG_CW1200)   		+= cw1200/
+obj-$(CONFIG_U8500_MMIO)	+= mmio/
+obj-$(CONFIG_U8500_FLASH)	+= camera_flash/
+obj-$(CONFIG_U8500_CM)		+= nmf-cm/
+obj-$(CONFIG_AB5500_SIM)	+= ab5500_sim/
diff --git a/drivers/staging/ab5500_sim/Makefile b/drivers/staging/ab5500_sim/Makefile
new file mode 100644
index 00000000000..520717e4dd7
--- /dev/null
+++ b/drivers/staging/ab5500_sim/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_AB5500_SIM)	+= ab5500-sim.o
diff --git a/drivers/staging/ab5500_sim/ab5500-sim.c b/drivers/staging/ab5500_sim/ab5500-sim.c
new file mode 100644
index 00000000000..788a7984a46
--- /dev/null
+++ b/drivers/staging/ab5500_sim/ab5500-sim.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (C) ST Ericsson SA 2010
+ *
+ * Sim Interface driver for AB5500
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Bibek Basu <bibek.basu@stericsson.com>
+ */
+#include <linux/init.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/platform_device.h>
+#include <linux/kobject.h>
+#include <linux/mfd/abx500.h>
+#include <linux/io.h>
+#include <linux/err.h>
+
+#define USIM_SUP2_REG		0x13
+#define USIM_SUP_REG		0x14
+#define USIM_SIMCTRL_REG	0x17
+#define USIM_SIMCTRL2_REG	0x18
+#define USIM_USBUICC_REG	0x19
+#define USIM_USBUICC2_REG	0x20
+#define SIM_DAT_PULLUP_10K	0x0F
+#define SIM_LDO_1_8V		1875000
+#define SIM_LDO_2_8V		2800000
+#define SIM_LDO_2_9V		2900000
+
+enum shift {
+	SHIFT0,
+	SHIFT1,
+	SHIFT2,
+	SHIFT3,
+	SHIFT4,
+	SHIFT5,
+	SHIFT6,
+	SHIFT7,
+};
+
+enum mask {
+	MASK1 = 1,
+	MASK3 = 3,
+	MASK7 = 7,
+};
+
+enum sim_mode {
+	OFF_MODE,
+	LOW_PWR,
+	PWRCTRL,
+	FULL_PWR,
+};
+/**
+ * struct ab5500_sim - ab5500 Sim Interface device information
+ * @dev:		pointer to the structure device
+ * @lock:		mutex lock
+ * @sim_int_status:	Sim presence status
+ * @irq_base:	Base of the two irqs
+ */
+struct ab5500_sim {
+	struct device *dev;
+	struct mutex lock;
+	bool sim_int_status;
+	u8 irq_base;
+};
+
+/* Exposure to the sysfs interface */
+int ab5500_sim_weak_pulldforce(struct device *dev,
+		struct device_attribute *attr,
+		const char *user_buf, size_t count)
+{
+	unsigned long user_val;
+	int err;
+	bool enable;
+
+	err = strict_strtoul(user_buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	enable = user_val ? true : false;
+	err = abx500_mask_and_set(dev, AB5500_BANK_SIM_USBSIM,
+		USIM_USBUICC2_REG, MASK1 << SHIFT5, user_val << SHIFT5);
+	if (err)
+		return -EINVAL;
+	return count;
+}
+
+int ab5500_sim_load_sel(struct device *dev,
+		struct device_attribute *attr,
+		const char *user_buf, size_t count)
+{
+	unsigned long user_val;
+	int err;
+	bool enable;
+
+	err = strict_strtoul(user_buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	enable = user_val ? true : false;
+	err = abx500_mask_and_set(dev, AB5500_BANK_SIM_USBSIM,
+		USIM_USBUICC_REG, MASK1 << SHIFT1, user_val << SHIFT1);
+	if (err)
+		return -EINVAL;
+	return count;
+}
+
+int ab5500_sim_mode_sel(struct device *dev,
+		struct device_attribute *attr,
+		const char *user_buf, size_t count)
+{
+	unsigned long user_val;
+	int err;
+
+	err = strict_strtoul(user_buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	err = abx500_mask_and_set(dev, AB5500_BANK_SIM_USBSIM,
+		USIM_SIMCTRL2_REG, MASK3 << SHIFT4, user_val << SHIFT4);
+	if (err)
+		return -EINVAL;
+	return count;
+}
+
+int ab5500_sim_dat_pullup(struct device *dev,
+		struct device_attribute *attr,
+		const char *user_buf, size_t count)
+{
+	unsigned long user_val;
+	int err;
+
+	err = strict_strtoul(user_buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	err = abx500_mask_and_set(dev, AB5500_BANK_SIM_USBSIM,
+		USIM_SIMCTRL_REG, MASK7, user_val);
+	if (err)
+		return -EINVAL;
+	return count;
+}
+
+int ab5500_sim_enable_pullup(struct device *dev,
+		struct device_attribute *attr,
+		const char *user_buf, size_t count)
+{
+	unsigned long user_val;
+	int err;
+	bool enable;
+
+	err = strict_strtoul(user_buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	enable = user_val ? true : false;
+	err = abx500_mask_and_set(dev, AB5500_BANK_SIM_USBSIM,
+		USIM_SIMCTRL_REG, MASK1 << SHIFT3, enable << SHIFT3);
+	if (err)
+		return -EINVAL;
+	return count;
+}
+
+static ssize_t ab5500_simoff_int(struct device *dev,
+			       struct device_attribute *devattr, char *user_buf)
+{
+	struct ab5500_sim *di = dev_get_drvdata(dev);
+	int len;
+
+	mutex_lock(&di->lock);
+	len = sprintf(user_buf, "%d\n", di->sim_int_status);
+	mutex_unlock(&di->lock);
+	return len;
+}
+
+static DEVICE_ATTR(enable_pullup, S_IWUSR, NULL, ab5500_sim_enable_pullup);
+static DEVICE_ATTR(dat_pullup, S_IWUSR, NULL, ab5500_sim_dat_pullup);
+static DEVICE_ATTR(mode_sel, S_IWUSR, NULL, ab5500_sim_mode_sel);
+static DEVICE_ATTR(load_sel, S_IWUSR, NULL, ab5500_sim_load_sel);
+static DEVICE_ATTR(weak_pulldforce, S_IWUSR, NULL, ab5500_sim_weak_pulldforce);
+static DEVICE_ATTR(simoff_int, S_IRUGO, ab5500_simoff_int, NULL);
+
+static struct attribute *ab5500_sim_attributes[] = {
+	&dev_attr_enable_pullup.attr,
+	&dev_attr_dat_pullup.attr,
+	&dev_attr_mode_sel.attr,
+	&dev_attr_load_sel.attr,
+	&dev_attr_weak_pulldforce.attr,
+	&dev_attr_simoff_int.attr,
+	NULL
+};
+
+static const struct attribute_group ab5500sim_attr_grp = {
+	.attrs = ab5500_sim_attributes,
+};
+
+static irqreturn_t ab5500_sim_irq_handler(int irq, void *irq_data)
+{
+	struct platform_device *pdev = irq_data;
+	struct ab5500_sim *data = platform_get_drvdata(pdev);
+
+	if (irq == data->irq_base)
+		data->sim_int_status = true;
+	else
+		data->sim_int_status = false;
+	sysfs_notify(&pdev->dev.kobj, NULL, "simoff_int");
+
+	return IRQ_HANDLED;
+}
+
+static int __devexit ab5500_sim_remove(struct platform_device *pdev)
+{
+	struct ab5500_sim *di = platform_get_drvdata(pdev);
+	int irq = platform_get_irq_byname(pdev, "SIMOFF");
+
+	if (irq >= 0) {
+		free_irq(irq, di);
+		irq++;
+		free_irq(irq, di);
+	}
+	sysfs_remove_group(&pdev->dev.kobj, &ab5500sim_attr_grp);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+
+	return 0;
+}
+
+static int __devinit ab5500_sim_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	int irq;
+	struct ab5500_sim *di =
+		kzalloc(sizeof(struct ab5500_sim), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	dev_info(&pdev->dev, "ab5500_sim_driver PROBE\n");
+	irq = platform_get_irq_byname(pdev, "SIMOFF");
+	if (irq < 0) {
+		dev_err(&pdev->dev, "Get irq by name failed\n");
+		return irq;
+	}
+	di->irq_base = irq;
+	di->dev = &pdev->dev;
+	mutex_init(&di->lock);
+	platform_set_drvdata(pdev, di);
+	/* sysfs interface to configure sim reg from user space */
+	if (sysfs_create_group(&pdev->dev.kobj, &ab5500sim_attr_grp) < 0) {
+		dev_err(&pdev->dev, " Failed creating sysfs group\n");
+		return -ENOMEM;
+	}
+	ret = request_threaded_irq(irq, NULL, ab5500_sim_irq_handler,
+			IRQF_NO_SUSPEND , "ab5500-sim", pdev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Request threaded irq failed (%d)\n", ret);
+		goto exit;
+	}
+	/* this is the contiguous irq for sim removal,falling edge */
+	irq = irq + 1;
+	ret = request_threaded_irq(irq, NULL, ab5500_sim_irq_handler,
+			IRQF_NO_SUSPEND , "ab5500-sim", pdev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Request threaded irq failed (%d)\n", ret);
+		free_irq(--irq, di);
+		goto exit;
+	}
+	return ret;
+exit:
+	sysfs_remove_group(&pdev->dev.kobj, &ab5500sim_attr_grp);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+	return ret;
+}
+
+static struct platform_driver ab5500_sim_driver = {
+	.probe = ab5500_sim_probe,
+	.remove = __devexit_p(ab5500_sim_remove),
+	.driver = {
+		.name = "ab5500-sim",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab5500_sim_init(void)
+{
+	return platform_driver_register(&ab5500_sim_driver);
+}
+
+static void __exit ab5500_sim_exit(void)
+{
+	platform_driver_unregister(&ab5500_sim_driver);
+}
+
+module_init(ab5500_sim_init);
+module_exit(ab5500_sim_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Bibek Basu");
+MODULE_ALIAS("platform:ab5500-sim");
+MODULE_DESCRIPTION("AB5500 sim interface driver");
diff --git a/drivers/staging/ab5500_sim/sysfs-sim b/drivers/staging/ab5500_sim/sysfs-sim
new file mode 100644
index 00000000000..b809b21e39e
--- /dev/null
+++ b/drivers/staging/ab5500_sim/sysfs-sim
@@ -0,0 +1,83 @@
+What:		/sys/devices/platform/ab5500-core.0/ab5500-sim.4/
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4 directory contains attributes
+		allowing the user space to check and configure ab5500 sim level
+		shifter interface caracteristics for communication to SIM card
+
+What:		/sys/devices/.../enable_pullup
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/enable_pullup attribute allows
+		the user space to configure if internal pull up in SIMIO lines
+		has to be enabled or disabled. For enabling write 1 to the file
+		and 0 for disabling
+
+
+What:		/sys/devices/.../dat_pullup
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/dat_pullup attribute allows
+		the user space to configure the resistance value for internal
+		pull up in SIMIO lines. Following value can be written on the file
+		0	SIM_DAT pull-up disabled
+		1	SIM_DAT pull-up 4kOhm
+		2	SIM_DAT pull-up 5kOhm
+		3	SIM_DAT pull-up 6kOhm
+		4	SIM_DAT pull-up 7kOhm
+		5	SIM_DAT pull-up 8kOhm
+		6	SIM_DAT pull-up 9kOhm
+		7	SIM_DAT pull-up 10kOhm
+
+What:		/sys/devices/.../mode_sel
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/mode_sel attribute allows
+		the user space to configure the mode at which the level shifter
+		will work. Following value can be written on the file
+		0	TG mode and LI mode off
+		1	TG mode on
+		2	LI mode on
+		3	TG mode and LI mode off
+
+What:		/sys/devices/.../load_sel
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/load_sel attribute allows
+		the user space to configure the load on the USBUICC lines.
+		Following value can be written on the file.
+		0	Data line load < 21pF
+		1	Data line load 21-30pF
+
+What:		/sys/devices/.../weak_pulldforce
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/weak_pulldforce attribute allows
+		the user space to configure the weak pull down on the USBUICC lines.
+		Following value can be written on the file.
+		0	USB-UICC data lines weak pull down active
+		1	USB-UICC data lines weak pull down not active
+
+What:		/sys/devices/.../simoff_int
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/simoff_int attribute allows
+		the user space to poll this file and get notified in case a sim
+		hot swap has happened. a zero means sim extracetd and a one means
+		inserted.
+
+
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
index 24719499237..6f54dabfff8 100644
--- a/drivers/staging/android/Kconfig
+++ b/drivers/staging/android/Kconfig
@@ -90,6 +90,14 @@ config ANDROID_LOW_MEMORY_KILLER
 	---help---
 	  Register processes to be killed when memory is low
 
+config ANDROID_STE_TIMED_VIBRA
+	bool "ST-Ericsson Timed Output Vibrator"
+	depends on SND_SOC_AB8500
+	depends on ANDROID_TIMED_OUTPUT
+	default y
+	help
+	  ST-Ericsson's vibrator driver using timed output class device
+
 endif # if ANDROID
 
 endmenu
diff --git a/drivers/staging/android/Makefile b/drivers/staging/android/Makefile
index 8e057e626d1..7aee0cb4e85 100644
--- a/drivers/staging/android/Makefile
+++ b/drivers/staging/android/Makefile
@@ -4,3 +4,4 @@ obj-$(CONFIG_ANDROID_RAM_CONSOLE)	+= ram_console.o
 obj-$(CONFIG_ANDROID_TIMED_OUTPUT)	+= timed_output.o
 obj-$(CONFIG_ANDROID_TIMED_GPIO)	+= timed_gpio.o
 obj-$(CONFIG_ANDROID_LOW_MEMORY_KILLER)	+= lowmemorykiller.o
+obj-$(CONFIG_ANDROID_STE_TIMED_VIBRA)	+= ste_timed_vibra.o
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
index e13b4c48340..b0883fcfb91 100644
--- a/drivers/staging/android/binder.c
+++ b/drivers/staging/android/binder.c
@@ -1610,6 +1610,7 @@ static void binder_transaction(struct binder_proc *proc,
 					proc->pid, thread->pid,
 					fp->binder, node->debug_id,
 					fp->cookie, node->cookie);
+				return_error = BR_FAILED_REPLY;
 				goto err_binder_get_ref_for_node_failed;
 			}
 			ref = binder_get_ref_for_node(target_proc, node);
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
index fa76ce7678a..531bdbeede8 100644
--- a/drivers/staging/android/logger.c
+++ b/drivers/staging/android/logger.c
@@ -555,10 +555,10 @@ static struct logger_log VAR = { \
 	.size = SIZE, \
 };
 
-DEFINE_LOGGER_DEVICE(log_main, LOGGER_LOG_MAIN, 256*1024)
+DEFINE_LOGGER_DEVICE(log_main, LOGGER_LOG_MAIN, 64*1024)
 DEFINE_LOGGER_DEVICE(log_events, LOGGER_LOG_EVENTS, 256*1024)
-DEFINE_LOGGER_DEVICE(log_radio, LOGGER_LOG_RADIO, 256*1024)
-DEFINE_LOGGER_DEVICE(log_system, LOGGER_LOG_SYSTEM, 256*1024)
+DEFINE_LOGGER_DEVICE(log_radio, LOGGER_LOG_RADIO, 64*1024)
+DEFINE_LOGGER_DEVICE(log_system, LOGGER_LOG_SYSTEM, 64*1024)
 
 static struct logger_log *get_log_from_minor(int minor)
 {
diff --git a/drivers/staging/android/ste_timed_vibra.c b/drivers/staging/android/ste_timed_vibra.c
new file mode 100644
index 00000000000..20ff93e4b85
--- /dev/null
+++ b/drivers/staging/android/ste_timed_vibra.c
@@ -0,0 +1,353 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Marcin Mielczarczyk <marcin.mielczarczyk@tieto.com>
+ *	   for ST-Ericsson
+ * License Terms: GNU General Public License v2
+ */
+
+#include <linux/kernel.h>
+#include <linux/hrtimer.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/ste_timed_vibra.h>
+#include <linux/delay.h>
+#include "timed_output.h"
+
+/**
+ * struct vibra_info - Vibrator information structure
+ * @tdev:		Pointer to timed output device structure
+ * @vibra_workqueue:    Pointer to vibrator workqueue structure
+ * @vibra_work:		Vibrator work
+ * @vibra_lock:		Vibrator lock
+ * @vibra_timer:	Vibrator high resolution timer
+ * @vibra_state:	Actual vibrator state
+ * @state_force:	Indicates if oppositive state is requested
+ * @timeout:		Indicates how long time the vibrator will be enabled
+ * @time_passed:	Total time passed in states
+ * @pdata:		Local pointer to platform data with vibrator parameters
+ *
+ * Structure vibra_info holds vibrator information
+ **/
+struct vibra_info {
+	struct timed_output_dev			tdev;
+	struct workqueue_struct			*vibra_workqueue;
+	struct work_struct			vibra_work;
+	spinlock_t				vibra_lock;
+	struct hrtimer				vibra_timer;
+	enum ste_timed_vibra_states		vibra_state;
+	bool					state_force;
+	unsigned int				timeout;
+	unsigned int				time_passed;
+	struct ste_timed_vibra_platform_data	*pdata;
+};
+
+/*
+ * For Linear type vibrators, resonance frequency waveform is
+ * generated using software loop and for same negative and
+ * positive cycle time period need to be calibrated. For 150Hz
+ * frequency nearest delay value is found out to be equal to "3".
+ */
+#define LINEAR_VIBRA_150HZ_DELAY	3
+
+/**
+ * vibra_control_work() - Vibrator work, turns on/off vibrator
+ * @work:	Pointer to work structure
+ *
+ * This function is called from workqueue, turns on/off vibrator
+ **/
+static void vibra_control_work(struct work_struct *work)
+{
+	struct vibra_info *vinfo =
+			container_of(work, struct vibra_info, vibra_work);
+	unsigned val = 0;
+	unsigned char speed_pos = 0, speed_neg = 0;
+	unsigned long flags;
+	unsigned long jiffies_till;
+
+	spin_lock_irqsave(&vinfo->vibra_lock, flags);
+	/* Should be already expired */
+	hrtimer_cancel(&vinfo->vibra_timer);
+
+	switch (vinfo->vibra_state) {
+	case STE_VIBRA_BOOST:
+		/* Turn on both vibrators with boost speed */
+		speed_pos = vinfo->pdata->boost_level;
+		val = vinfo->pdata->boost_time;
+		break;
+	case STE_VIBRA_ON:
+		/* Turn on both vibrators with speed */
+		speed_pos = vinfo->pdata->on_level;
+		val = vinfo->timeout - vinfo->pdata->boost_time;
+		break;
+	case STE_VIBRA_OFF:
+		/* Turn on both vibrators with reversed speed */
+		speed_neg = vinfo->pdata->off_level;
+		val = vinfo->pdata->off_time;
+		break;
+	case STE_VIBRA_IDLE:
+		vinfo->time_passed = 0;
+		break;
+	default:
+		break;
+	}
+	spin_unlock_irqrestore(&vinfo->vibra_lock, flags);
+
+	if (vinfo->pdata->is_linear_vibra) {
+		jiffies_till = jiffies + msecs_to_jiffies(vinfo->timeout);
+		while (time_before(jiffies, jiffies_till)) {
+			vinfo->pdata->timed_vibra_control(
+					speed_pos, speed_neg,
+					speed_pos, speed_neg);
+			mdelay(LINEAR_VIBRA_150HZ_DELAY);
+			vinfo->pdata->timed_vibra_control(
+					speed_neg, speed_pos,
+					speed_neg, speed_pos);
+			mdelay(LINEAR_VIBRA_150HZ_DELAY);
+		}
+
+		/* Disable Vibrator */
+		speed_pos = speed_neg = 0;
+		vinfo->pdata->timed_vibra_control(
+				speed_pos, speed_neg,
+				speed_pos, speed_neg);
+		vinfo->vibra_state = STE_VIBRA_IDLE;
+	} else {
+		/* Send new settings */
+		vinfo->pdata->timed_vibra_control(
+				speed_pos, speed_neg,
+				speed_pos, speed_neg);
+
+		/* Start timer if it's not in IDLE state */
+		if (vinfo->vibra_state != STE_VIBRA_IDLE) {
+			hrtimer_start(&vinfo->vibra_timer,
+				ktime_set(val / 1000, (val % 1000) * 1000000),
+				HRTIMER_MODE_REL);
+		}
+	}
+}
+
+/**
+ * vibra_enable() - Enables vibrator
+ * @tdev:      Pointer to timed output device structure
+ * @timeout:	Time indicating how long vibrator will be enabled
+ *
+ * This function enables vibrator
+ **/
+static void vibra_enable(struct timed_output_dev *tdev, int timeout)
+{
+	struct vibra_info *vinfo = dev_get_drvdata(tdev->dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&vinfo->vibra_lock, flags);
+	switch (vinfo->vibra_state) {
+	case STE_VIBRA_IDLE:
+		if (timeout)
+			vinfo->vibra_state = STE_VIBRA_BOOST;
+		else    /* Already disabled */
+			break;
+
+		vinfo->state_force = false;
+		/* Trim timeout */
+		timeout = timeout < vinfo->pdata->boost_time ?
+				vinfo->pdata->boost_time : timeout;
+		/* Remember timeout value */
+		vinfo->timeout = timeout;
+		queue_work(vinfo->vibra_workqueue, &vinfo->vibra_work);
+		break;
+	case STE_VIBRA_BOOST:
+		/* Force only when user requested OFF while BOOST */
+		if (!timeout)
+			vinfo->state_force = true;
+		break;
+	case STE_VIBRA_ON:
+		/* If user requested OFF */
+		if (!timeout && !vinfo->pdata->is_linear_vibra) {
+			hrtimer_cancel(&vinfo->vibra_timer);
+			vinfo->vibra_state = STE_VIBRA_OFF;
+			queue_work(vinfo->vibra_workqueue, &vinfo->vibra_work);
+		}
+		break;
+	case STE_VIBRA_OFF:
+		/* Force only when user requested ON while OFF */
+		if (timeout)
+			vinfo->state_force = true;
+		break;
+	default:
+		break;
+	}
+	spin_unlock_irqrestore(&vinfo->vibra_lock, flags);
+}
+
+/**
+ * vibra_timer_expired() - Handles vibrator machine state
+ * @hrtimer:      Pointer to high resolution timer structure
+ *
+ * This function handles vibrator machine state
+ *
+ * Returns:
+ *	Returns value which indicates wether hrtimer should be restarted
+ **/
+static enum hrtimer_restart vibra_timer_expired(struct hrtimer *hrtimer)
+{
+	struct vibra_info *vinfo =
+			container_of(hrtimer, struct vibra_info, vibra_timer);
+	unsigned long flags;
+
+	spin_lock_irqsave(&vinfo->vibra_lock, flags);
+	switch (vinfo->vibra_state) {
+	case STE_VIBRA_BOOST:
+		/* If BOOST finished and force, go to OFF */
+		if (vinfo->state_force)
+			vinfo->vibra_state = STE_VIBRA_OFF;
+		else
+			vinfo->vibra_state = STE_VIBRA_ON;
+		vinfo->time_passed = vinfo->pdata->boost_time;
+		break;
+	case STE_VIBRA_ON:
+		vinfo->vibra_state = STE_VIBRA_OFF;
+		vinfo->time_passed = vinfo->timeout;
+		break;
+	case STE_VIBRA_OFF:
+		/* If OFF finished and force, go to ON */
+		if (vinfo->state_force)
+			vinfo->vibra_state = STE_VIBRA_ON;
+		else
+			vinfo->vibra_state = STE_VIBRA_IDLE;
+		vinfo->time_passed += vinfo->pdata->off_time;
+		break;
+	case STE_VIBRA_IDLE:
+		break;
+	default:
+		break;
+	}
+	vinfo->state_force = false;
+	spin_unlock_irqrestore(&vinfo->vibra_lock, flags);
+
+	queue_work(vinfo->vibra_workqueue, &vinfo->vibra_work);
+
+	return HRTIMER_NORESTART;
+}
+
+/**
+ * vibra_get_time() - Returns remaining time to disabling vibration
+ * @tdev:      Pointer to timed output device structure
+ *
+ * This function returns time remaining to disabling vibration
+ *
+ * Returns:
+ *	Returns remaining time to disabling vibration
+ **/
+static int vibra_get_time(struct timed_output_dev *tdev)
+{
+	struct vibra_info *vinfo = dev_get_drvdata(tdev->dev);
+	u32 ms;
+
+	if (hrtimer_active(&vinfo->vibra_timer)) {
+		ktime_t remain = hrtimer_get_remaining(&vinfo->vibra_timer);
+		ms = (u32) ktime_to_ms(remain);
+		return ms + vinfo->time_passed;
+	} else
+		return 0;
+}
+
+static int __devinit ste_timed_vibra_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct vibra_info *vinfo;
+
+	if (!pdev->dev.platform_data) {
+		dev_err(&pdev->dev, "No platform data supplied\n");
+		return -ENODEV;
+	}
+
+	vinfo = kmalloc(sizeof *vinfo, GFP_KERNEL);
+	if (!vinfo) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	vinfo->tdev.name = "vibrator";
+	vinfo->tdev.enable = vibra_enable;
+	vinfo->tdev.get_time = vibra_get_time;
+	vinfo->time_passed = 0;
+	vinfo->vibra_state = STE_VIBRA_IDLE;
+	vinfo->state_force = false;
+	vinfo->pdata = pdev->dev.platform_data;
+
+	if (vinfo->pdata->is_linear_vibra)
+		dev_info(&pdev->dev, "Linear Type Vibrators\n");
+	else
+		dev_info(&pdev->dev, "Rotary Type Vibrators\n");
+
+	ret = timed_output_dev_register(&vinfo->tdev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register timed output device\n");
+		goto exit_free_vinfo;
+	}
+
+
+	vinfo->tdev.dev->parent = pdev->dev.parent;
+	dev_set_drvdata(vinfo->tdev.dev, vinfo);
+
+	/* Create workqueue just for timed output vibrator */
+	vinfo->vibra_workqueue =
+		create_singlethread_workqueue("ste-timed-output-vibra");
+	if (!vinfo->vibra_workqueue) {
+		dev_err(&pdev->dev, "failed to allocate workqueue\n");
+		ret = -ENOMEM;
+		goto exit_timed_output_unregister;
+	}
+
+	INIT_WORK(&vinfo->vibra_work, vibra_control_work);
+	spin_lock_init(&vinfo->vibra_lock);
+	hrtimer_init(&vinfo->vibra_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	vinfo->vibra_timer.function = vibra_timer_expired;
+	platform_set_drvdata(pdev, vinfo);
+	return 0;
+
+exit_timed_output_unregister:
+	timed_output_dev_unregister(&vinfo->tdev);
+exit_free_vinfo:
+	kfree(vinfo);
+	return ret;
+}
+
+static int __devexit ste_timed_vibra_remove(struct platform_device *pdev)
+{
+	struct vibra_info *vinfo = platform_get_drvdata(pdev);
+
+	timed_output_dev_unregister(&vinfo->tdev);
+	destroy_workqueue(vinfo->vibra_workqueue);
+	kfree(vinfo);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver ste_timed_vibra_driver = {
+	.driver = {
+		.name = "ste_timed_output_vibra",
+		.owner = THIS_MODULE,
+	},
+	.probe  = ste_timed_vibra_probe,
+	.remove = __devexit_p(ste_timed_vibra_remove)
+};
+
+static int __init ste_timed_vibra_init(void)
+{
+	return platform_driver_register(&ste_timed_vibra_driver);
+}
+module_init(ste_timed_vibra_init);
+
+static void __exit ste_timed_vibra_exit(void)
+{
+	platform_driver_unregister(&ste_timed_vibra_driver);
+}
+module_exit(ste_timed_vibra_exit);
+
+MODULE_AUTHOR("Marcin Mielczarczyk <marcin.mielczarczyk@tieto.com>");
+MODULE_DESCRIPTION("STE Timed Output Vibrator");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/camera_flash/Kconfig b/drivers/staging/camera_flash/Kconfig
new file mode 100644
index 00000000000..187217d763f
--- /dev/null
+++ b/drivers/staging/camera_flash/Kconfig
@@ -0,0 +1,7 @@
+
+config U8500_FLASH
+	bool "ST-Ericsson Flash (Camera) Driver"
+	depends on ARCH_U8500
+	help
+	  Adds ST-Ericsson Flash (Camera) Driver
+
diff --git a/drivers/staging/camera_flash/Makefile b/drivers/staging/camera_flash/Makefile
new file mode 100644
index 00000000000..bf2f5aa2dd3
--- /dev/null
+++ b/drivers/staging/camera_flash/Makefile
@@ -0,0 +1,5 @@
+export ADP1653_SUPPORT
+EXTRA_CFLAGS += -DADP1653_SUPPORT
+obj-$(CONFIG_U8500_FLASH) := camera_flash.o
+camera_flash-y := flash_common.o
+camera_flash-y += adp1653.o
diff --git a/drivers/staging/camera_flash/adp1653.c b/drivers/staging/camera_flash/adp1653.c
new file mode 100644
index 00000000000..c97014fc5ad
--- /dev/null
+++ b/drivers/staging/camera_flash/adp1653.c
@@ -0,0 +1,595 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * adp1653: Driver Adp1653 HPLED flash driver chip. This driver
+ *          currently support I2C interface, 2bit interface is not supported.
+ * Author: Pankaj Chauhan/pankaj.chauhan@stericsson.com for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <asm/mach-types.h>
+#include "flash_common.h"
+#include "adp1653.h"
+#include "camera_flash.h"
+
+/* This data is platform specific for 8500 href-v1 platform,
+ * Ideally this should be supplied from platform code
+ */
+//#define ADAPTER_I2C2	(2)
+#define STR_GPIO		(6)
+
+static int strobe_gpio = 0; //TODO: set to 0 when it works
+static int adapter_i2c2 = 2;
+static int flash_irq = 7;
+static int enable_gpio = 272;
+static int flash_position = 0;
+module_param(strobe_gpio, int, S_IRUGO);
+MODULE_PARM_DESC(strobe_gpio, "use GPIO 6 to strobe, 0 means that someone else (sensor?) will strobe for us");
+module_param(adapter_i2c2, int, S_IRUGO);
+MODULE_PARM_DESC(adapter_i2c2, "use the given I2C adaptater to communicate with the chip");
+module_param(flash_irq, int, S_IRUGO);
+MODULE_PARM_DESC(flash_irq, "the GPIO number associated to the i2c irq line");
+module_param(enable_gpio, int, S_IRUGO);
+MODULE_PARM_DESC(enable_gpio, "use the given GPIO line to enable the chip");
+module_param(flash_position, int, S_IRUGO);
+MODULE_PARM_DESC(flash_position, "the position of the flash chip (0=PRIMARY, 1=SECONDARY)");
+
+
+int __flash_gpio_to_irq(int gpio)
+{
+
+	return NOMADIK_GPIO_TO_IRQ(gpio);
+}
+
+#define DEBUG_LOG(...) printk(KERN_DEBUG "Adp1653 flash driver: " __VA_ARGS__)
+
+#define ADP1653_SUPPORTED_MODES (FLASH_MODE_VIDEO_LED | FLASH_MODE_STILL_LED | \
+	FLASH_MODE_STILL_LED_EXTERNAL_STROBE |                                 \
+	FLASH_MODE_AF_ASSISTANT | FLASH_MODE_INDICATOR)
+
+#define ADP1653_SELFTEST_SUPPORTED_MODES (FLASH_SELFTEST_CONNECTION | FLASH_SELFTEST_FLASH_WITH_STROBE | \
+	FLASH_SELFTEST_VIDEO_LIGHT | FLASH_SELFTEST_AF_LIGHT | FLASH_SELFTEST_INDICATOR | FLASH_SELFTEST_TORCH_LIGHT)
+
+static int adp1653_trigger_strobe(void *priv_data, int enable);
+
+static int adp1653_get_modes(void *priv_data,unsigned long *modes)
+{
+	int err;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv_data;
+	err = i2c_smbus_read_byte_data(priv_p->i2c_client, FAULT_STATUS_REG);
+	if (err)
+		*modes = 0x0;
+	else
+		*modes = ADP1653_SUPPORTED_MODES;
+	return 0;
+}
+
+static int adp1653_get_mode_details(void *priv_data, unsigned long mode,
+struct flash_mode_details *details_p)
+{
+	int err = 0;
+	memset(details_p,0,sizeof(struct flash_mode_details));
+
+	details_p->led_type = 2;
+
+	/* Still LED settings*/
+	details_p->nbFaultRegisters = 1;
+	if(mode & (FLASH_MODE_STILL_LED | FLASH_MODE_STILL_LED_EXTERNAL_STROBE)){
+		details_p->max_intensity_uAmp = FLASH_MAX_INTENSITY;
+		details_p->min_intensity_uAmp = FLASH_MIN_INTENSITY;
+		details_p->max_strobe_duration_uSecs = FLASH_MAX_STROBE_DURATION;
+		details_p->feature_bitmap = INTENSITY_PROGRAMMABLE | DURATION_PROGRAMMABLE;
+		goto out;
+	}
+	/*Video LED settings*/
+	if(mode & FLASH_MODE_VIDEO_LED){
+		details_p->max_intensity_uAmp = TORCH_MAX_INTENSITY;
+		details_p->min_intensity_uAmp = TORCH_MIN_INTENSITY;
+		details_p->max_strobe_duration_uSecs = 0;
+		details_p->feature_bitmap = INTENSITY_PROGRAMMABLE;
+		goto out;
+	}
+	/*Privacy Indicator settings */
+	if(mode & FLASH_MODE_INDICATOR){
+		details_p->max_intensity_uAmp = ILED_MAX_INTENSITY;
+		details_p->min_intensity_uAmp = ILED_MIN_INTENSITY;
+		details_p->max_strobe_duration_uSecs = 0;
+		details_p->feature_bitmap = INTENSITY_PROGRAMMABLE;
+		goto out;
+	}
+	DEBUG_LOG("Mode %lx, not supported\n",mode);
+	err = EINVAL;
+out:
+	return err;
+}
+
+static int adp1653_enable_flash_mode(void *priv_data,
+				     unsigned long mode, int enable)
+{
+	int err = 0;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv_data;
+
+	if(enable){
+
+		if(!(mode & ADP1653_SUPPORTED_MODES)){
+			DEBUG_LOG("Unsupported mode %lx\n",mode);
+			err = -EINVAL;
+			goto out;
+		}
+		/*Nothing to be done in enabling, just set current mode and return*/
+		/*May be enable disable can be done here but why not enable in
+		*probe and keep it on always
+		*/
+		adp1653_trigger_strobe(priv_p,0);
+		priv_p->curr_mode = mode;
+	}else{
+		adp1653_trigger_strobe(priv_p,0);
+		priv_p->curr_mode =0;
+	}
+out:
+	return err;
+}
+
+static int adp1653_configure_flash_mode(void *priv,unsigned long mode,
+struct flash_mode_params *params_p)
+{
+	int err = 0;
+	unsigned char intensity_code;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv;
+
+	if(!(mode & ADP1653_SUPPORTED_MODES)){
+		DEBUG_LOG("Mode %lx not supported\n",mode);
+		err = -EINVAL;
+		goto out;
+	}
+	switch(mode){
+	case FLASH_MODE_STILL_LED:
+	case FLASH_MODE_STILL_LED_EXTERNAL_STROBE:
+		{
+			FLASH_UAMP_TO_CODE(intensity_code,params_p->intensity_uAmp);
+			if(params_p->duration_uSecs){
+				DURATION_USEC_TO_CODE(priv_p->flash_duration,
+					params_p->duration_uSecs);
+				DEBUG_LOG("Duration %lu, code 0x%x\n",params_p->duration_uSecs,
+					priv_p->flash_duration);
+				priv_p->flash_duration |= TIMER_ENABLE;
+			}else{
+				priv_p->flash_duration = 0;
+			}
+			priv_p->flash_intensity = intensity_code << 3;
+		}
+		break;
+	case FLASH_MODE_VIDEO_LED:
+		{
+			TORCH_UAMP_TO_CODE(intensity_code,params_p->intensity_uAmp);
+			DEBUG_LOG("Torch mode setting intensity 0x%x, current(uA) %lu\n",
+				intensity_code,params_p->intensity_uAmp);
+			priv_p->torch_intensity = intensity_code << 3;
+		}
+		break;
+	case FLASH_MODE_INDICATOR:
+		{
+			ILED_UAMP_TO_CODE(intensity_code,params_p->intensity_uAmp);
+			DEBUG_LOG("ILED setting intensity 0x%x, current(uA) %lu\n",
+				intensity_code,params_p->intensity_uAmp);
+			priv_p->indicator_intensity = intensity_code;
+		}
+		break;
+	default:
+		err = -EINVAL;
+		DEBUG_LOG("Unsupported mode %lx\n",mode);
+		break;
+	}
+
+	if((mode == FLASH_MODE_STILL_LED_EXTERNAL_STROBE) || (mode == FLASH_MODE_STILL_LED))
+	{
+		adp1653_trigger_strobe(priv_p,0);
+		DEBUG_LOG("CONFIG_TIMER_REG : 0x%x\n",priv_p->flash_duration);
+		DEBUG_LOG("OUTPUT_SEL_REG : 0x%x\n",priv_p->flash_intensity);
+
+		/*TimeOut Must be programmed before Intensity*/
+		err = i2c_smbus_write_byte_data(priv_p->i2c_client,CONFIG_TIMER_REG,
+			priv_p->flash_duration);
+		if(err){
+			DEBUG_LOG("I2C: Unsable to write timer config, err %d\n",err);
+			goto out;
+		}
+		err = i2c_smbus_write_byte_data(priv_p->i2c_client,OUTPUT_SEL_REG,
+			priv_p->flash_intensity);
+		if(err){
+			DEBUG_LOG("I2C: Unable to write   OUTPUT_SEL_REG , err %d\n",err);
+			goto out;
+		}
+	}
+out:
+	return err;
+}
+
+static int adp1653_set_intensity(struct adp1653_priv_data *priv_p, uint8_t intensity)
+{
+	return i2c_smbus_write_byte_data(priv_p->i2c_client,OUTPUT_SEL_REG,intensity);
+}
+
+static int adp1653_strobe_still_led(struct adp1653_priv_data *priv_p,int enable)
+{
+	int err=0,gpio_val;
+	uint8_t intensity,duration;
+
+	if(enable){
+		intensity = priv_p->flash_intensity;
+		duration = priv_p->flash_duration;
+		gpio_val = 1;
+	}else{
+		intensity = 0;
+		duration = 0;
+		gpio_val = 0;
+	}
+
+	err = adp1653_set_intensity(priv_p,intensity);
+	if(err){
+		DEBUG_LOG("I2C: Unable to write OUTPUT_SEL_REG reg, err %d\n",err);
+		goto out;
+	}
+
+	/*TimeOut Must be programmed before Intensity*/
+	err = i2c_smbus_write_byte_data(priv_p->i2c_client,CONFIG_TIMER_REG,
+		priv_p->flash_duration);
+	if(err){
+		DEBUG_LOG("I2C: Unsable to write timer config, err %d\n",err);
+		goto out;
+	}
+	err = i2c_smbus_write_byte_data(priv_p->i2c_client,OUTPUT_SEL_REG,intensity);
+	if(err){
+		DEBUG_LOG("I2C: Unable to write OUTPUT_SEL_REG, err %d\n",err);
+		goto out;
+	}
+
+	if (strobe_gpio != 0 )
+	{
+		gpio_set_value(priv_p->strobe_gpio,gpio_val);
+	}
+
+out:
+	return err;
+}
+
+static int adp1653_trigger_strobe(void *priv, int enable)
+{
+	int err = 0;
+	uint8_t intensity;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv;
+
+	switch(priv_p->curr_mode){
+	case FLASH_MODE_STILL_LED:
+	case FLASH_MODE_STILL_LED_EXTERNAL_STROBE:
+		err = adp1653_strobe_still_led(priv_p,enable);
+		break;
+	case FLASH_MODE_VIDEO_LED:
+		{
+			if(enable)
+				intensity = priv_p->torch_intensity;
+			else
+				intensity = 0;
+			err = adp1653_set_intensity(priv_p,intensity);
+		}
+		break;
+	case FLASH_MODE_INDICATOR:
+		{
+			if(enable)
+				intensity = priv_p->indicator_intensity;
+			else
+				intensity =0;
+			err = adp1653_set_intensity(priv_p,intensity);
+		}
+		break;
+	default:
+		DEBUG_LOG("Unsupported mode %lx\n",priv_p->curr_mode);
+		goto out;
+	}
+	if(err){
+		DEBUG_LOG("Unable to enable/disable %d, strobe. Mode %lx, err %d\n",enable,
+			priv_p->curr_mode,err);
+		goto out;
+	}
+	disable_irq(priv_p->i2c_client->irq);
+	if(enable)
+		SET_FLASH_STATUS(priv_p->status,FLASH_STATUS_LIT);
+	else
+		CLR_FLASH_STATUS(priv_p->status,FLASH_STATUS_LIT);
+
+	enable_irq(priv_p->i2c_client->irq);
+
+out:
+	return err;
+}
+#define FLASH_ERR_ALL	(FLASH_ERR_OVER_CHARGE |FLASH_ERR_OVER_HEAT |	\
+	FLASH_ERR_SHORT_CIRCUIT | FLASH_ERR_TIMEOUT |			\
+	FLASH_ERR_OVER_VOLTAGE)
+int  adp1653_get_status(void *priv_data,unsigned long *status)
+{
+	struct adp1653_priv_data *priv_p= (struct adp1653_priv_data *)priv_data;
+	disable_irq(priv_p->i2c_client->irq);
+	if(priv_p->fault){
+		if(priv_p->fault & OVER_VOLTAGE_FAULT)
+			SET_FLASH_ERROR(priv_p->status,FLASH_ERR_OVER_VOLTAGE);
+		if(priv_p->fault & TIMEOUT_FAULT)
+			SET_FLASH_ERROR(priv_p->status,FLASH_ERR_TIMEOUT);
+		if(priv_p->fault & OVER_TEMPERATURE_FAULT)
+			SET_FLASH_ERROR(priv_p->status,FLASH_ERR_OVER_HEAT);
+		if(priv_p->fault & SHORT_CIRCUIT_FAULT){
+			CLR_FLASH_STATUS(priv_p->status,FLASH_STATUS_READY);
+			SET_FLASH_STATUS(priv_p->status,FLASH_STATUS_BROKEN);
+			SET_FLASH_ERROR(priv_p->status,FLASH_ERR_SHORT_CIRCUIT);
+		}
+		priv_p->fault =0;
+	}else{
+		CLR_FLASH_ERROR(priv_p->status,FLASH_ERR_ALL);
+	}
+	enable_irq(priv_p->i2c_client->irq);
+	*status = priv_p->status;
+	return 0;
+}
+
+int adp1653_get_selftest_modes(void *priv_data, unsigned long *modes)
+{
+	int err;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv_data;
+	err = i2c_smbus_read_byte_data(priv_p->i2c_client, FAULT_STATUS_REG);
+	if (err) *modes = 0x0;
+	else     *modes = ADP1653_SELFTEST_SUPPORTED_MODES;
+	return 0;
+}
+
+int adp1653_get_fault_registers(void *priv_data, unsigned long mode, unsigned long *status)
+{
+	int err = 0;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv_data;
+
+	*status = i2c_smbus_read_byte_data(priv_p->i2c_client, FAULT_STATUS_REG);
+
+	/* clear fault register */
+	err = i2c_smbus_write_byte_data(priv_p->i2c_client,OUTPUT_SEL_REG,0);
+	if(0 != err)
+	{
+		DEBUG_LOG("Unable to write OUTPUT_SEL_REG, err %d\n",err);
+	}
+	return err;
+}
+
+struct flash_chip_ops adp1653_ops = {
+	.get_modes = adp1653_get_modes,
+	.get_mode_details = adp1653_get_mode_details,
+	.get_status = adp1653_get_status,
+	.enable_flash_mode = adp1653_enable_flash_mode,
+	.configure_flash_mode = adp1653_configure_flash_mode,
+	.trigger_strobe = adp1653_trigger_strobe,
+	.get_selftest_modes = adp1653_get_selftest_modes,
+	.get_fault_registers = adp1653_get_fault_registers
+};
+
+static irqreturn_t adp1653_irq_hdlr(int irq_no,void *data)
+{
+	int err;
+	struct adp1653_priv_data *priv_p= (struct adp1653_priv_data *)data;
+
+	priv_p->fault = i2c_smbus_read_byte_data(priv_p->i2c_client,
+		FAULT_STATUS_REG);
+	DEBUG_LOG("Got Fault, status 0x%x\n",priv_p->fault);
+	/*Writing 0 to OUTPUT_SEL_REG clears the interrtup
+	*and FAULT_STATUS_REG register
+	*/
+	err = i2c_smbus_write_byte_data(priv_p->i2c_client,OUTPUT_SEL_REG,0);
+	if(err)
+		DEBUG_LOG("Unable to write OUTPUT_SEL_REG to clr intr, err %d\n",err);
+	/*TBD: send even to user process*/
+	return IRQ_HANDLED;
+}
+static int __devinit adp1653_probe(struct i2c_client *client,
+				   const struct i2c_device_id *id)
+{
+	int err=0;
+	struct flash_chip *flash_chip_p=NULL;
+	struct adp1653_priv_data *priv_p=NULL;
+
+	DEBUG_LOG("> adp1653_probe\n");
+
+	priv_p = kzalloc(sizeof(struct adp1653_priv_data),GFP_KERNEL);
+	if(!priv_p){
+		DEBUG_LOG("Kmalloc failed for priv data\n");
+		err = ENOMEM;
+		goto out;
+	}
+	priv_p->i2c_client = client;
+	flash_chip_p = kzalloc(sizeof(struct flash_chip),GFP_KERNEL);
+	if(!flash_chip_p){
+		DEBUG_LOG("Kmalloc failed for flash_chip_p");
+		err = ENOMEM;
+		goto out;
+	}
+	flash_chip_p->priv_data = priv_p;
+	flash_chip_p->ops = &adp1653_ops;
+	SET_FLASHCHIP_TYPE(flash_chip_p,FLASH_TYPE_HPLED);
+	SET_FLASHCHIP_ID(flash_chip_p,ADP1653_ID);
+
+	strncpy(flash_chip_p->name,"Adp1653",FLASH_NAME_SIZE);
+
+	i2c_set_clientdata(client,priv_p);
+	/*Request GPIO and Register IRQ if supported by platform and flash chip*/
+
+	if(machine_is_hrefv60())
+		enable_gpio = 21;
+	else
+		enable_gpio = 272;
+
+	if(enable_gpio){
+		err = gpio_request(enable_gpio,"Camera LED flash Enable");
+		if(err){
+			DEBUG_LOG("Unable to get GPIO %d, for enable\n",enable_gpio);
+			goto out;
+		}
+		priv_p->enable_gpio = enable_gpio;
+
+		err = gpio_direction_output(priv_p->enable_gpio, 1);
+		if(err){
+			DEBUG_LOG("Unable to set GPIO %lu in output mode, err %d\n",priv_p->enable_gpio,err);
+			gpio_free(priv_p->enable_gpio);
+			goto out;
+		}
+		gpio_set_value(priv_p->enable_gpio, 1);
+	}
+
+	if (strobe_gpio != 0)
+	{
+		err = gpio_request(STR_GPIO,"Camera flash strobe\n");
+		if(err){
+			DEBUG_LOG("Unable to request strobe GPIO\n");
+			// somebody else requested this gpio ...
+			//goto out;
+		}
+
+		err = gpio_direction_output(STR_GPIO, 0);
+		if(err){
+			DEBUG_LOG("Unable to set GPIO %d in output mode, err %d\n",STR_GPIO,err);
+			goto out;
+		}
+	}
+	else
+	{
+		err = gpio_request(STR_GPIO,"Camera flash strobe\n");
+		if(err){
+			DEBUG_LOG("Unable to request strobe GPIO\n");
+			// somebody else requested this gpio ...
+			//goto out;
+		}
+
+		err = gpio_direction_input(STR_GPIO);
+		if(err){
+			DEBUG_LOG("Unable to set GPIO %d in input mode, err %d\n",STR_GPIO,err);
+			goto out;
+		}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+		err = nmk_gpio_set_pull(STR_GPIO, NMK_GPIO_PULL_DOWN);
+#endif
+		if(err){
+			DEBUG_LOG("Unable to set pull down on GPIO %d\n",STR_GPIO);
+			goto out;
+		}
+
+	}
+
+	priv_p->strobe_gpio = STR_GPIO;
+
+	if(client->irq){
+		err = request_threaded_irq(client->irq,NULL,adp1653_irq_hdlr,
+			IRQF_ONESHOT|IRQF_TRIGGER_FALLING,
+			"Adp1653 flash",priv_p);
+		if(err){
+			DEBUG_LOG("Unable to register flash IRQ handler, irq %d, err %d\n",
+				client->irq,err);
+			goto out;
+		}
+	}
+	err = register_flash_chip(flash_position,flash_chip_p);
+	if(err){
+		DEBUG_LOG("Failed to register Adp1653 as flash for %s camera\n",
+			(flash_position?"Primary":"Secondary"));
+		goto out;
+	}
+	SET_FLASH_STATUS(priv_p->status,FLASH_STATUS_READY);
+	DEBUG_LOG("< adp1653_probe ok\n");
+	return err;
+out:
+	if(priv_p->irq_no)
+		free_irq(priv_p->irq_no,NULL);
+	if(priv_p->enable_gpio)
+		gpio_free(priv_p->enable_gpio);
+	if(priv_p)
+		kfree(priv_p);
+	if(flash_chip_p)
+		kfree(flash_chip_p);
+	DEBUG_LOG("< adp1653_probe (%d)\n", err);
+	return err;
+}
+
+static int __devexit adp1653_remove(struct i2c_client *client)
+{
+	int err=0;
+	/*Nothing here yet, implement it later.*/
+	return err;
+}
+static const struct i2c_device_id adp1653_id[] = {
+	{ "adp1653", 0},
+	{}
+};
+static struct i2c_driver adp1653_i2c_driver = {
+	.driver = {
+		.name = "adp1653",
+		.owner = THIS_MODULE,
+	},
+	.probe = adp1653_probe,
+	.remove = __devexit_p(adp1653_remove),
+	.id_table = adp1653_id,
+};
+
+int adp1653_init(void){
+	int err=0;
+	struct i2c_adapter *adap_p;
+	struct i2c_board_info info;
+
+	/*Registration of I2C flash device is platform specific code
+	*Ideally it should be done from kernel (arch/arm/mach-XXX).
+	*Do it locally till the time it gets into platform code
+	*OR This portion (registration of device) and flash chip init
+	*Routine can be moved to Flash chip module init.
+	*/
+	DEBUG_LOG("getting I2C adaptor %d\n",adapter_i2c2);
+	adap_p = i2c_get_adapter(adapter_i2c2);
+	if(!adap_p){
+		DEBUG_LOG("Unable to get I2C adaptor\n");
+		goto out;
+	}
+	memset(&info,0,sizeof( struct i2c_board_info));
+
+	info.irq	= __flash_gpio_to_irq(flash_irq);
+
+	strcpy(&info.type[0],"adp1653");
+	DEBUG_LOG("trying to register %s at position %d, GPIO enable %d, GPIO IRQ line %d\n",
+		info.type,
+		flash_position,
+		enable_gpio,
+		flash_irq);
+
+	/* I2C framework expects least significant 7 bits as address, not complete
+	* 8 bits with bit 0 (read/write bit)
+	*/
+	info.addr = 0x60 >> 1;
+
+
+	err = i2c_add_driver(&adp1653_i2c_driver);
+	if(err)
+	{
+		DEBUG_LOG("Failed to register i2c driver\n");
+		goto out;
+	}
+
+
+	DEBUG_LOG("Initialized adp1653\n");
+	if(!i2c_new_device(adap_p,&info)){
+		DEBUG_LOG("Unable to add i2c dev: %s (err=%d)\n",info.type, err);
+		goto out;
+	}
+
+out:
+	return err;
+}
+
+/*
+MODULE_DEPEND
+*/
diff --git a/drivers/staging/camera_flash/adp1653.h b/drivers/staging/camera_flash/adp1653.h
new file mode 100755
index 00000000000..3035ab56d99
--- /dev/null
+++ b/drivers/staging/camera_flash/adp1653.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __ADP1653_H__
+#define __ADP1653_H__
+
+#include <linux/types.h>
+#define ADP1653_ID	(0)	/*chip does not give any id :) so be it zero!*/
+
+#define OUTPUT_SEL_REG		(0x00)
+#define CONFIG_TIMER_REG	(0x01)
+#define SW_STROBE_REG		(0x02)
+#define FAULT_STATUS_REG	(0x03)
+
+/* Fault codes, FALUT_STATUS_REG bits */
+#define OVER_VOLTAGE_FAULT		(0x01)
+#define TIMEOUT_FAULT			(0x02)
+#define OVER_TEMPERATURE_FAULT	(0x04)
+#define SHORT_CIRCUIT_FAULT		(0x08)
+
+/*CONFIG_TIMER_REG bits*/
+#define TIMER_ENABLE	(0x10)
+
+struct adp1653_priv_data{
+	struct i2c_client *i2c_client;
+	unsigned long curr_mode;
+	unsigned long enable_gpio;
+	unsigned long strobe_gpio;
+	unsigned long irq_no;
+	unsigned long status;
+	uint8_t fault;
+	uint8_t flash_intensity;
+	uint8_t flash_duration;
+	uint8_t torch_intensity;
+	uint8_t indicator_intensity;
+};
+
+/*Intensity current limits in Micro Amps*/
+/* over 250mA flash current is reduced */
+/* do not know why, neither really care about */
+//#define FLASH_MAX_INTENSITY	(500000)	/*code - 31*/
+#define FLASH_MAX_INTENSITY	(250000)
+#define FLASH_MIN_INTENSITY	(215000)	/*code - 12*/
+#define TORCH_MAX_INTENSITY	(200000)	/*code - 11*/
+#define TORCH_MIN_INTENSITY	(50000)		/*code - 1*/
+#define ILED_MAX_INTENSITY  (17500)		/*Code - 7*/
+#define ILED_MIN_INTENSITY	(2500)		/*code - 1*/
+
+#define FLASH_MAX_STROBE_DURATION (820000)	 /*820 uSec*/
+
+#define DURATION_USEC_TO_CODE(_code,_duration) do{			\
+	 if(_duration > FLASH_MAX_STROBE_DURATION)				\
+	 	_duration = FLASH_MAX_STROBE_DURATION;				\
+	_code = (FLASH_MAX_STROBE_DURATION - _duration) / 54600;\
+}while(0);
+
+#define HPLED_UAMP_TO_CODE(_current) 	((_current - 35000) / 15000)
+
+#define FLASH_UAMP_TO_CODE(_code,_current){		\
+	if(_current > FLASH_MAX_INTENSITY)			\
+		_current = FLASH_MAX_INTENSITY;			\
+	if(_current < FLASH_MIN_INTENSITY)			\
+		_current = FLASH_MIN_INTENSITY;			\
+	_code = HPLED_UAMP_TO_CODE(_current);		\
+}while(0)
+
+#define TORCH_UAMP_TO_CODE(_code,_current){		\
+	if(_current > TORCH_MAX_INTENSITY)			\
+		_current = TORCH_MAX_INTENSITY;			\
+	if(_current < TORCH_MIN_INTENSITY)			\
+		_current = TORCH_MIN_INTENSITY;			\
+	_code = HPLED_UAMP_TO_CODE(_current);		\
+}while(0)
+
+#define ILED_UAMP_TO_CODE(_code,_current) do {								\
+	 if(_current > ILED_MAX_INTENSITY)										\
+	 	_current = ILED_MAX_INTENSITY;										\
+	_code = _current / ILED_MIN_INTENSITY; /* Min current: 2.5mA/2500uA*/	\
+}while(0)
+
+#endif
diff --git a/drivers/staging/camera_flash/camera_flash.h b/drivers/staging/camera_flash/camera_flash.h
new file mode 100644
index 00000000000..15faf706dc9
--- /dev/null
+++ b/drivers/staging/camera_flash/camera_flash.h
@@ -0,0 +1,74 @@
+#ifndef __CAMERA_FLASH_H__
+#define __CAMERA_FLASH_H__
+
+#define FLASH_NAME_SIZE	(20)
+
+struct flash_mode_details {
+	unsigned long led_type;
+	unsigned long max_intensity_uAmp;
+	unsigned long min_intensity_uAmp;
+	unsigned long max_strobe_duration_uSecs;
+	unsigned long feature_bitmap;
+	unsigned char nbFaultRegisters;
+};
+
+/*feature_bitmap (in struct flash_mode_details) bit values*/
+#define INTENSITY_PROGRAMMABLE (0x01)
+#define DURATION_PROGRAMMABLE  (0x02)
+#define TIMEOUT_PROGRAMMABLE   (0x04)
+
+/*Status word returned by driver has status in lower 16 bits
+ *and Error in higher 16 bits. definition of status and error
+ *bits are there in flash_bitfields.h
+ */
+#define SET_FLASH_STATUS(_bitmap, _status) (_bitmap |= (_status & 0xffff))
+#define CLR_FLASH_STATUS(_bitmap, _status) (_bitmap &= ~(_status & 0xffff))
+#define SET_FLASH_ERROR(_bitmap, _status) (_bitmap |= (_status << 16))
+#define CLR_FLASH_ERROR(_bitmap, _status) (_bitmap &= ~(_status << 16))
+#define GET_FLASH_STATUS(_bitmap) (_bitmap & 0xffff)
+#define GET_FLASH_ERROR(_bitmap) (_bitmap >> 16)
+
+struct flash_mode_params {
+	unsigned long duration_uSecs;
+	unsigned long intensity_uAmp;
+	unsigned long timeout_uSecs;
+};
+
+struct flash_ioctl_args_t {
+	unsigned long flash_mode;
+	unsigned long cam;
+	unsigned long status;
+	union mode_arg{
+		struct flash_mode_details details;
+		struct flash_mode_params  params;
+		unsigned long strobe_enable;
+	} mode_arg;
+};
+
+#define FLASH_MAGIC_NUMBER 0x17
+#define FLASH_GET_MODES			_IOR(FLASH_MAGIC_NUMBER, 1,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_MODE_DETAILS		_IOWR(FLASH_MAGIC_NUMBER, 2,\
+struct flash_ioctl_args_t *)
+#define FLASH_ENABLE_MODE		_IOW(FLASH_MAGIC_NUMBER, 3,\
+struct flash_ioctl_args_t *)
+#define FLASH_DISABLE_MODE		_IOW(FLASH_MAGIC_NUMBER, 4,\
+struct flash_ioctl_args_t *)
+#define FLASH_CONFIGURE_MODE		_IOW(FLASH_MAGIC_NUMBER, 5,\
+struct flash_ioctl_args_t *)
+#define FLASH_TRIGGER_STROBE		_IOW(FLASH_MAGIC_NUMBER, 6,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_STATUS		_IOW(FLASH_MAGIC_NUMBER, 7,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_LIFE_COUNTER		_IOW(FLASH_MAGIC_NUMBER, 8,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_SELF_TEST_MODES	_IOR(FLASH_MAGIC_NUMBER, 9,\
+struct flash_ioctl_args_t *)
+#define FLASH_SELF_TEST			_IOW(FLASH_MAGIC_NUMBER, 10,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_FAULT_REGISTERS	_IOR(FLASH_MAGIC_NUMBER, 11,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_SELF_TEST_RESULT	_IOR(FLASH_MAGIC_NUMBER, 12,\
+struct flash_ioctl_args_t *)
+
+#endif
diff --git a/drivers/staging/camera_flash/camera_flash_bitfields.h b/drivers/staging/camera_flash/camera_flash_bitfields.h
new file mode 100644
index 00000000000..05da9c5ef58
--- /dev/null
+++ b/drivers/staging/camera_flash/camera_flash_bitfields.h
@@ -0,0 +1,83 @@
+/*
+ * 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. 
+ */
+/**
+* \file    camera_flash_bitfields.h
+* \brief   Define some constants for the flash drivers API.
+* \author  ST-Ericsson
+*/
+#ifndef __CAMERA_FLASH_BITFIELDS_H__
+#define __CAMERA_FLASH_BITFIELDS_H__
+
+/* Flash Mode definitions */
+/* All Operating Modes are off (shutdown low power state)*/
+#define FLASH_MODE_NONE				(0x000)
+/* Enables the xenon driver. Strobe is managed by the flash driver itself.
+Charges the xenon. Automatic periodic recharge is abstracted by the driver */
+#define FLASH_MODE_XENON			(0x001)
+/* Enables the xenon driver. Strobe is managed externally to the driver */
+#define FLASH_MODE_XENON_EXTERNAL_STROBE	(0x002)
+/* Enables the video led driver. Strobing is managed by the driver */
+#define FLASH_MODE_VIDEO_LED			(0x004)
+/* Enables the video led driver. Strobing is managed externally to driver */
+#define FLASH_MODE_VIDEO_LED_EXTERNAL_STROBE	(0x008)
+/* Enables the still LED driver. Strobing is managed by the driver itself */
+#define FLASH_MODE_STILL_LED			(0x010)
+/* Enables the still LED driver. Strobe is managed externally to the driver */
+#define FLASH_MODE_STILL_LED_EXTERNAL_STROBE	(0x020)
+/* Enables the AF assistant driver. Strobe is managed by the driver */
+#define FLASH_MODE_AF_ASSISTANT			(0x040)
+/* Enable the driver. Strobe is managed by the driver */
+#define FLASH_MODE_INDICATOR			(0x080)
+/* Enables the still HP LED driver. Strobing is managed by the driver itself */
+#define FLASH_MODE_STILL_HPLED			(0x100)
+/* Enables the still HP LED driver. Strobe is managed externally to the
+driver */
+#define FLASH_MODE_STILL_HPLED_EXTERNAL_STROBE	(0x200)
+
+
+/* The flash is not usable anymore */
+#define FLASH_STATUS_BROKEN     (0x00)
+/* The flash is ready to be fired and unlit */
+#define FLASH_STATUS_READY      (0x01)
+/* The flash is discharged and by construction, charging; usually an
+application shall not try to fire it in that state (although possible
+typically in sport mode flash) */
+#define FLASH_STATUS_NOT_READY  (0x02)
+/* The flash is in shutdown state */
+#define FLASH_STATUS_SHUTDOWN   (0x04)
+/* Intermediate state that may exist where I2C registers can be programmed */
+#define FLASH_STATUS_STANDBY    (0x08)
+/* The flash is already strobing */
+#define FLASH_STATUS_LIT        (0x10)
+
+#define FLASH_SELFTEST_NONE              0x000
+/* tests connections to flash driver ICs */
+#define FLASH_SELFTEST_CONNECTION        0x001
+/* tests capture flash without using strobe signal from camera */
+#define FLASH_SELFTEST_FLASH             0x002
+/* tests capture flash using strobe signal from camera: ONLY this one needs to
+be done in idle state from flash tests cases */
+#define FLASH_SELFTEST_FLASH_WITH_STROBE 0x004
+/* tests video light */
+#define FLASH_SELFTEST_VIDEO_LIGHT       0x008
+/* tests AF assistance light */
+#define FLASH_SELFTEST_AF_LIGHT          0x010
+/* tests capture indicator light */
+#define FLASH_SELFTEST_INDICATOR         0x020
+/* tests flash in torch mode */
+#define FLASH_SELFTEST_TORCH_LIGHT       0x040
+
+/** \brief Flash Error */
+enum TFlashError {
+	FLASH_ERR_NONE		, /* None */
+	FLASH_ERR_OVER_CHARGE	, /* Error happened during the charge */
+	FLASH_ERR_OVER_HEAT	, /* Over temperature */
+	FLASH_ERR_SHORT_CIRCUIT	, /* Short circuit */
+	FLASH_ERR_TIMEOUT	, /* Timeout */
+	FLASH_ERR_OVER_VOLTAGE	/* Over voltage */
+} ;
+
+#endif
diff --git a/drivers/staging/camera_flash/flash_common.c b/drivers/staging/camera_flash/flash_common.c
new file mode 100644
index 00000000000..fc59879a170
--- /dev/null
+++ b/drivers/staging/camera_flash/flash_common.c
@@ -0,0 +1,460 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * camera flash: Flash driver to export camera flash to user space application.
+ *               It supports two flashes, one for primary and one for secondary camera
+ * Author: Pankaj Chauhan/pankaj.chauhan@stericsson.com for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/kthread.h>
+#include <linux/jiffies.h>
+#include <linux/miscdevice.h>
+#include "camera_flash.h"
+#include "flash_common.h"
+
+#define DEBUG_LOG(...) printk(KERN_DEBUG "Camera Flash driver: " __VA_ARGS__)
+
+#define PRIMARY_CAMERA		(0)
+#define SECONDARY_CAMERA	(1)
+static struct miscdevice misc_dev;
+struct flash_chip *flash_chips[2];
+struct fasync_struct * async_queue;
+struct task_struct* ptaskStruct;
+wait_queue_head_t waitQueue;
+int waitCondition = 0;
+struct flash_ioctl_args_t flashArg;
+
+#define COPY_ARG_FROM_USER(_to,_from_usr)	do{												\
+	memset((_to),0,sizeof(struct flash_ioctl_args_t));													\
+	if (copy_from_user((_to), (struct flash_ioctl_args_t*) (_from_usr), sizeof(struct flash_ioctl_args_t))) {	\
+		DEBUG_LOG("Could not copy data from userspace successfully\n");							\
+		break;																					\
+	}																							\
+}while(0)
+
+#define COPY_ARG_TO_USER(_to_usr,_from) do{											\
+	if (copy_to_user((struct flash_ioctl_args_t *)(_to_usr), (_from), sizeof(struct flash_ioctl_args_t))) {	\
+		DEBUG_LOG("Could not copy data from userspace successfully\n");						\
+		break;																				\
+	}																						\
+}while(0)
+
+
+static long flash_ioctl(struct file *file_p, unsigned int cmd, unsigned long arg)
+{
+	int err=0;
+	struct flash_chip *flash_p = NULL;
+	struct flash_chip_ops *ops = NULL;
+	char *my_name=NULL;
+	struct flash_ioctl_args_t flash_arg;
+
+	if (_IOC_TYPE(cmd) != FLASH_MAGIC_NUMBER) {
+                printk(KERN_ALERT "Flash driver: Not an ioctl for this module\n");
+		err =  -EINVAL;
+	}
+
+	COPY_ARG_FROM_USER(&flash_arg,arg);
+
+	if(flash_arg.cam == SECONDARY_CAMERA || flash_arg.cam == PRIMARY_CAMERA)
+		flash_p = flash_chips[flash_arg.cam];
+	else{
+		DEBUG_LOG("unsupported cam %lu\n",flash_arg.cam);
+		err = -ENODEV;
+		goto out;
+	}
+	my_name = flash_arg.cam ?"Secondary":"Primary";
+
+	if (flash_arg.cam == PRIMARY_CAMERA)
+	{
+		ops = flash_p->ops;
+	}
+
+	switch(cmd){
+	case FLASH_GET_MODES:
+	{
+		if (flash_arg.cam == PRIMARY_CAMERA)
+		{
+			err  = ops->get_modes(flash_p->priv_data,&flash_arg.flash_mode);
+			if(!err){
+				DEBUG_LOG("Supported flash modes for %s camera: %lx\n",
+					my_name,flash_arg.flash_mode);
+				COPY_ARG_TO_USER(arg,&flash_arg);
+			}else{
+				DEBUG_LOG("unable to get supported modes for %s camera\n",my_name);
+			}
+		}
+		else
+		{
+			flash_arg.flash_mode = FLASH_MODE_NONE;
+			COPY_ARG_TO_USER(arg,&flash_arg);
+		}
+	}
+		break;
+	case FLASH_GET_MODE_DETAILS:
+	{
+		err = ops->get_mode_details(flash_p->priv_data,flash_arg.flash_mode,
+				&flash_arg.mode_arg.details);
+		if(!err){
+			COPY_ARG_TO_USER(arg,&flash_arg);
+		}else{
+			DEBUG_LOG("Unable to get mode details for %s camera, flash mode %lx\n",
+				my_name,flash_arg.flash_mode);
+		}
+	}
+		break;
+	case FLASH_ENABLE_MODE:
+	case FLASH_DISABLE_MODE:
+	{
+		int enable=0;
+		if(cmd == FLASH_ENABLE_MODE){
+			enable = 1;
+		}
+		err = ops->enable_flash_mode(flash_p->priv_data,flash_arg.flash_mode,enable);
+		if(err){
+			DEBUG_LOG("Unable to %s:  %s camera, flash mode %lx\n",
+				(enable ?"Enable":"Disable"), my_name,flash_arg.flash_mode);
+		}
+	}
+		break;
+	case FLASH_CONFIGURE_MODE:
+		err = ops->configure_flash_mode(flash_p->priv_data,flash_arg.flash_mode,
+				&flash_arg.mode_arg.params);
+		if(err){
+			DEBUG_LOG("Unable to configure  %s camera, flash mode %lx\n",
+				my_name,flash_arg.flash_mode);
+		}
+		break;
+	case FLASH_TRIGGER_STROBE:
+		err = ops->trigger_strobe(flash_p->priv_data,flash_arg.mode_arg.strobe_enable);
+		if(err){
+			DEBUG_LOG("Unable to %s:  %s camera strobe trigger, mode %lx\n",
+				(arg ?"Enable":"Disable"), my_name,flash_arg.flash_mode);
+		}
+		break;
+	case FLASH_GET_STATUS:
+		err = ops->get_status(flash_p->priv_data,&flash_arg.status);
+		if(!err){
+			COPY_ARG_TO_USER(arg,&flash_arg);
+		}else{
+			DEBUG_LOG("Unable to get status details for %s camera, flash mode %lx\n",
+				my_name,flash_arg.flash_mode);
+		}
+		 break;
+	case FLASH_GET_LIFE_COUNTER:
+		 DEBUG_LOG("Not Implemented\n");
+		 break;
+        case FLASH_SELF_TEST:
+		flashArg = flash_arg;
+		if (0 != (flashArg.cam & (FLASH_SELFTEST_FLASH | FLASH_SELFTEST_FLASH_WITH_STROBE)))
+		{
+			err = ENODEV;
+		}
+		else
+		{
+			/* wake up worker thread */
+			waitCondition = 1;
+			wake_up_interruptible(&waitQueue);
+		}
+		break;
+        case FLASH_GET_SELF_TEST_MODES:
+	{
+		if (flash_arg.cam == PRIMARY_CAMERA)
+		{
+			err  = ops->get_selftest_modes(flash_p->priv_data,&flash_arg.flash_mode);
+			if(!err){
+				COPY_ARG_TO_USER(arg,&flash_arg);
+			}else{
+				DEBUG_LOG("unable to get supported modes for %s camera\n",my_name);
+			}
+		}
+		else
+		{
+			flash_arg.flash_mode = FLASH_SELFTEST_NONE;
+			COPY_ARG_TO_USER(arg,&flash_arg);
+		}
+		break;
+	}
+        case FLASH_GET_FAULT_REGISTERS:
+	{
+		err  = ops->get_fault_registers(flash_p->priv_data,flash_arg.flash_mode,&flash_arg.status);
+		if(!err){
+			COPY_ARG_TO_USER(arg,&flash_arg);
+		}else{
+			DEBUG_LOG("unable to get supported modes for %s camera\n",my_name);
+		}
+
+		break;
+	}
+        case FLASH_GET_SELF_TEST_RESULT:
+	{
+		COPY_ARG_TO_USER(arg,&flashArg);
+		DEBUG_LOG("FLASH_GET_SELF_TEST_RESULT arg : 0x%lx\n", flashArg.status);
+		break;
+	}
+	default:
+		DEBUG_LOG("Unknown command %x\n",cmd);
+
+	}
+out:
+	return err;
+}
+
+int worker_thread (void* data)
+{
+	int err = 0;
+	struct flash_chip *flash_p=NULL;
+	struct flash_chip_ops *ops=NULL;
+	struct flash_mode_params params;
+	struct flash_mode_details details;
+
+	while (1)
+	{
+		/* waiting for some job to do */
+		wait_event_interruptible(waitQueue, (waitCondition != 0));
+		waitCondition = 0;
+
+		DEBUG_LOG("worker_thread wakes up\n");
+		/* do we need to stop ? */
+		err = kthread_should_stop();
+		if (0 != err)
+		{
+			DEBUG_LOG("worker_thread stops\n");
+			break;
+		}
+
+		/* do the job */
+		flash_p = flash_chips[flashArg.cam];
+		ops = flash_p->ops;
+
+		/* clear fault registers */
+		err  = ops->get_fault_registers(flash_p->priv_data, FLASH_MODE_INDICATOR, &flashArg.status);
+		if (0 != err)
+		{
+			flashArg.status = flashArg.flash_mode;
+			flashArg.flash_mode = 0;
+		}
+		flashArg.status = 0;
+
+		/* do all selftests */
+		while (flashArg.flash_mode != FLASH_SELFTEST_NONE)
+		{
+			if (0 != (flashArg.flash_mode & FLASH_SELFTEST_CONNECTION))
+			{
+				err = ops->get_mode_details(flash_p->priv_data, FLASH_MODE_INDICATOR, &details);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to get mode FLASH_MODE_INDICATOR details\n");
+					flashArg.status |= FLASH_SELFTEST_CONNECTION;
+				}
+				flashArg.flash_mode &= ~FLASH_SELFTEST_CONNECTION;
+			}
+			else if (0 != (flashArg.flash_mode & (FLASH_SELFTEST_FLASH | FLASH_SELFTEST_FLASH_WITH_STROBE)))
+			{
+				if (0 != (flashArg.flash_mode & FLASH_SELFTEST_FLASH))
+				{
+					flashArg.status |= FLASH_SELFTEST_FLASH;
+					flashArg.flash_mode &= ~FLASH_SELFTEST_FLASH;
+				}
+				else
+				{
+					flashArg.status |= FLASH_SELFTEST_FLASH_WITH_STROBE;
+					flashArg.flash_mode &= ~FLASH_SELFTEST_FLASH_WITH_STROBE;
+				}
+			}
+			/* FLASH_SELFTEST_VIDEO_LIGHT | FLASH_SELFTEST_AF_LIGHT | FLASH_SELFTEST_INDICATOR | FLASH_SELFTEST_TORCH_LIGHT */
+			else
+			{
+				unsigned long currentSelftest = FLASH_SELFTEST_NONE;
+				unsigned long currentFlashMode = FLASH_MODE_NONE;
+
+				if (0 != (flashArg.flash_mode & FLASH_SELFTEST_VIDEO_LIGHT))
+				{
+					currentSelftest = FLASH_SELFTEST_VIDEO_LIGHT;
+					currentFlashMode = FLASH_MODE_VIDEO_LED;
+				}
+				else if (0 != (flashArg.flash_mode & FLASH_SELFTEST_AF_LIGHT))
+				{
+					currentSelftest = FLASH_SELFTEST_AF_LIGHT;
+					currentFlashMode = FLASH_MODE_AF_ASSISTANT;
+				}
+				else if (0 != (flashArg.flash_mode & FLASH_SELFTEST_INDICATOR))
+				{
+					currentSelftest = FLASH_SELFTEST_INDICATOR;
+					currentFlashMode = FLASH_MODE_INDICATOR;
+				}
+				else
+				{
+					currentSelftest = FLASH_SELFTEST_TORCH_LIGHT;
+					currentFlashMode = FLASH_MODE_VIDEO_LED;
+				}
+
+				err = ops->get_mode_details(flash_p->priv_data, currentFlashMode, &details);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to get mode 0x%lx details\n",currentFlashMode);
+					flashArg.status |= currentSelftest;
+					flashArg.flash_mode &= ~currentSelftest;
+					continue;
+				}
+
+				err = ops->enable_flash_mode(flash_p->priv_data, currentFlashMode, 1);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to enable flash mode 0x%lx\n",currentFlashMode);
+					flashArg.status |= currentSelftest;
+					flashArg.flash_mode &= ~currentSelftest;
+					continue;
+				}
+
+				params.duration_uSecs = 0;
+				params.intensity_uAmp = details.max_intensity_uAmp;
+				params.timeout_uSecs = 0;
+				err = ops->configure_flash_mode(flash_p->priv_data, currentFlashMode, &params);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to configure flash mode 0x%lx\n",currentFlashMode);
+					flashArg.status |= currentSelftest;
+					flashArg.flash_mode &= ~currentSelftest;
+					continue;
+				}
+
+				err = ops->trigger_strobe(flash_p->priv_data,1);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to strobe, mode : 0x%lx\n",currentFlashMode);
+					flashArg.status |= currentSelftest;
+					flashArg.flash_mode &= ~currentSelftest;
+					continue;
+				}
+
+				wait_event_timeout(waitQueue, 0, msecs_to_jiffies(1000));
+
+				err = ops->trigger_strobe(flash_p->priv_data,0);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to strobe, mode : 0x%lx\n",currentFlashMode);
+					flashArg.status |= currentSelftest;
+					flashArg.flash_mode &= ~currentSelftest;
+					continue;
+				}
+				flashArg.flash_mode &= ~currentSelftest;
+			}
+		}
+
+		/* job's done ! */
+		flash_async_notify();
+	}
+	return 0;
+}
+
+int flash_open(struct inode *node, struct file *file_p)
+{
+	// init sleep queue
+	init_waitqueue_head(&waitQueue);
+
+	// start worker thread
+	ptaskStruct = kthread_run (&worker_thread, NULL, "flashDriverWorker");
+
+	return 0;
+}
+
+int register_flash_chip(unsigned int cam, struct flash_chip *flash_p)
+{
+	int err =0;
+	DEBUG_LOG("Registering cam %d\n", cam);
+	DEBUG_LOG("flash_p: name=%s\n", flash_p->name);
+	if(cam > 1 || !flash_p){
+		DEBUG_LOG("Registration: something is wrong! cam %d, flash_p %x \n",cam,(int)flash_p);
+		err = EINVAL;
+		goto out;
+	}
+	if(!flash_chips[cam]){
+		flash_chips[cam] = flash_p;
+		DEBUG_LOG("Registered flash: id %lx, %s for camera %d\n",
+			flash_p->id,flash_p->name,cam);
+	}else{
+		DEBUG_LOG("%s flash already registered for camera %d, ignore flash %s\n",
+		flash_chips[cam]->name,cam, flash_p->name);
+	}
+out:
+	return err;
+}
+
+int flash_async_notify ()
+{
+	kill_fasync(&async_queue, SIGIO, POLL_IN);
+	return 0;
+}
+
+static int flash_fasync(int fd, struct file *filp, int mode)
+{
+        DEBUG_LOG("registered async notification on %d fd\n",fd);
+        return fasync_helper(fd, filp, mode, &async_queue);
+}
+
+static int flash_release(struct inode *node, struct file *file_p)
+{
+	int err = 0;
+
+	fasync_helper(-1, file_p, 0, &async_queue);
+
+	// stop worker thread
+	waitCondition = 1;
+	err = kthread_stop(ptaskStruct);
+	return err;
+}
+
+static struct file_operations flash_fops = {
+	owner:THIS_MODULE,
+	unlocked_ioctl:flash_ioctl,
+	open:flash_open,
+	release:flash_release,
+        fasync:flash_fasync,
+};
+
+int  major_device_number;
+
+/*Temporary here (adp_init)*/
+extern int adp1653_init(void);
+static int __init flash_init(void)
+{
+	int err = 0;
+	err = adp1653_init();
+	if(err){
+		DEBUG_LOG("Unable to initialize adp1653, err %d\n",err);
+		goto out;
+	}
+	/* Register misc device */
+    	misc_dev.minor = MISC_DYNAMIC_MINOR;
+    	misc_dev.name = "camera_flash";
+    	misc_dev.fops = &flash_fops;
+    	err = misc_register(&misc_dev);
+	if (err < 0) {
+                printk(KERN_INFO "camera_flash driver misc_register failed (%d)\n", err);
+                return err;
+	} else {
+                major_device_number = err;
+		printk(KERN_INFO "camera_flash driver initialized with minor=%d\n", misc_dev.minor);
+	}
+out:
+	return err;
+}
+
+static void __exit flash_exit(void)
+{
+	misc_deregister(&misc_dev);
+	printk(KERN_INFO"Camera flash driver unregistered\n");
+}
+
+module_init(flash_init);
+module_exit(flash_exit);
+MODULE_LICENSE("GPL");
+EXPORT_SYMBOL(register_flash_chip);
+EXPORT_SYMBOL(flash_async_notify);
diff --git a/drivers/staging/camera_flash/flash_common.h b/drivers/staging/camera_flash/flash_common.h
new file mode 100755
index 00000000000..d1f63631e82
--- /dev/null
+++ b/drivers/staging/camera_flash/flash_common.h
@@ -0,0 +1,57 @@
+#ifndef __FLASH_COMMON_H__
+#define __FLASH_COMMON_H__
+
+#include "camera_flash_bitfields.h"
+#include "camera_flash.h"
+
+struct flash_chip_ops{
+	int (*get_modes)( void *priv_data, unsigned long *modes);
+	int (*get_mode_details)(void *priv_data,unsigned long mode,
+				struct flash_mode_details *details_p);
+	int (*enable_flash_mode) (void *priv_data,unsigned long mode,
+		int enable);
+	int (*configure_flash_mode) (void *priv_data, unsigned long mode,
+				struct flash_mode_params *params_p);
+	int (*trigger_strobe) (void *priv_data, int enable);
+	int (*get_life_counter) (void *priv_data);
+	int (*get_status)	(void *priv_data, unsigned long *status);
+	int (*get_selftest_modes)	(void *priv_data,
+		unsigned long *modes);
+	int (*get_fault_registers)	(void *priv_data, unsigned long mode,
+		unsigned long *status);
+};
+
+#define FLASH_TYPE_XENON	(0x1)
+#define FLASH_TYPE_HPLED	(0x2)
+
+#define SET_FLASHCHIP_TYPE(flash_chip_p,_TYPE) ((flash_chip_p)->id |= _TYPE)
+#define GET_FLASHHIP_TYPE(flash_chip_p) ((flash_chip_p)->id & 0xffff)
+#define GET_FLASHCHIP_ID(flash_chip_p)  ((flash_chip_p)->id >> 16)
+#define SET_FLASHCHIP_ID(flash_chip_p,_ID) ((flash_chip_p)->id |= (_ID << 16))
+
+struct flash_chip {
+	unsigned long id;
+	struct flash_chip_ops *ops;
+	void *priv_data;
+	unsigned char name[FLASH_NAME_SIZE];
+};
+
+/**
+ * struct flash_platform_data:
+ * platform specific data For flash chip driver
+ * @cam : 0 - primary, 1 - secondary
+ * @strobe_gpio: GPIO used as strobe
+ * @enable_gpio: GPIO used for enable/reset input
+ */
+struct flash_platform_data{
+	unsigned long cam;
+	unsigned long strobe_gpio;
+	unsigned long strobe_gpio_alt_func;
+	unsigned long enable_gpio;
+	unsigned long enable_gpio_alt_func;
+};
+
+extern int register_flash_chip(unsigned int cam, struct flash_chip *flash_p);
+extern int flash_async_notify (void );
+
+#endif
diff --git a/drivers/staging/cg2900/Kconfig b/drivers/staging/cg2900/Kconfig
new file mode 100644
index 00000000000..2a0d14c4fbe
--- /dev/null
+++ b/drivers/staging/cg2900/Kconfig
@@ -0,0 +1,72 @@
+#
+# CG2900
+#
+
+config CG2900
+	tristate "Support ST-Ericsson CG2900 main structure"
+	depends on NET && MFD_SUPPORT
+	help
+	  ST-Ericsson CG2900 Connectivity Combo controller main
+	  structure.
+	  Supports multiple functionalities muxed over a Bluetooth HCI H:4
+	  interface.
+	  CG2900 support Bluetooth, FM radio, and GPS.
+
+config CG2900_CHIP
+	tristate "Support CG2900 Connectivity controller"
+	depends on CG2900
+	help
+	  ST-Ericsson CG2900 Connectivity Controller chip handler.
+	  Contains chip handler performing driver initialization
+	  such as patchdownload and also instantiates the supported
+	  MFD devices.
+
+config STLC2690_CHIP
+	tristate "Support STLC2690 Connectivity controller"
+	depends on CG2900
+	help
+	  ST-Ericsson STLC2690 Connectivity Controller chip handler.
+	  Contains chip handler performing driver initialization
+	  such as patchdownload and also instantiates the supported
+	  MFD devices.
+
+config CG2900_UART
+	tristate "Support CG2900 UART transport"
+	depends on CG2900
+	select BT
+	select BT_HCIUART
+	help
+	  UART driver for ST-Ericsson CG2900 Connectivity Controller.
+	  Contains functions for setting baud rate and to transport
+	  data to and from the CG2900 controller over UART.
+	  Also handles low power handling for the CG2900 when using UART as
+	  transport.
+
+config CG2900_AUDIO
+	tristate "Support CG2900 audio interface"
+	depends on CG2900
+	help
+	  ST-Ericsson CG2900 Connectivity audio interface driver.
+	  Gives a module the ability to setup audio paths
+	  within the CG2900 controller.
+	  Supports both a normal function API and using character device
+	  from user space.
+
+config CG2900_TEST
+	tristate "Support CG2900 Test Char Device"
+	depends on CG2900
+	help
+	  ST-Ericsson CG2900 Test Character Device driver.
+	  Creates a character device which can be used by
+	  a test framework in user space to emulate a connected chip.
+	  Note that this is used to test the chip handler driver,
+	  not to test the connected chip.
+
+config BT_CG2900
+	tristate "ST-Ericsson CG2900 Bluetooth driver"
+	depends on CG2900 && BT
+	help
+	  Select if ST-Ericsson CG2900 Connectivity controller shall be used as
+	  Bluetooth controller for BlueZ.
+	  This driver registers to the Bluetooth stack and when opened,
+	  enables the CG2900 controller in a proper way.
diff --git a/drivers/staging/cg2900/Makefile b/drivers/staging/cg2900/Makefile
new file mode 100644
index 00000000000..c6414c21c5d
--- /dev/null
+++ b/drivers/staging/cg2900/Makefile
@@ -0,0 +1,12 @@
+#
+# Makefile for ST-Ericsson CG2900 connectivity combo controller
+#
+
+ccflags-y :=					\
+	-Idrivers/staging/cg2900/include \
+	-Iarch/arm/mach-ux500
+
+obj-$(CONFIG_CG2900)	+= board-mop500-cg2900.o devices-cg2900.o
+
+obj-y				+= mfd/
+obj-y				+= bluetooth/
diff --git a/drivers/staging/cg2900/TODO b/drivers/staging/cg2900/TODO
new file mode 100644
index 00000000000..e122eba6d53
--- /dev/null
+++ b/drivers/staging/cg2900/TODO
@@ -0,0 +1,23 @@
+TODO
+----
+
+ - Decide upon main driver architecture.
+
+ - Decide if the CG2900 driver should be a separate driver as today or if it
+   should be a sub-driver using the TI-ST (Shared Transport) driver that is also
+   written for a combo connectivity controller.
+
+ - Decide if cg2900_uart should register on top of hci_ldisc.c (as now) or if it
+   should instead register on top of hci_h4.c thereby reusing hci_h4
+   implementation.
+
+ - Update the hci_ldisc.c so that it will allow drivers to be registered without
+   registering them directly to the Bluetooth stack. Also extend the hci_ldisc.c
+   with more functions to abstract the tty API in a conformative way (currently
+   sometimes the tty API used, sometimes the hci_ldisc interface).
+
+ - Decide if the CG2900 driver should use imported structs and defines to create
+   Bluetooth packets as today or if the Bluetooth stack in the Kernel should be
+   extended so it is possible to use generic functions to send and receive
+   commands and events both from the Bluetooth stack itself and from external
+   drivers such as the CG2900 driver.
diff --git a/drivers/staging/cg2900/bluetooth/Makefile b/drivers/staging/cg2900/bluetooth/Makefile
new file mode 100644
index 00000000000..936a4a257da
--- /dev/null
+++ b/drivers/staging/cg2900/bluetooth/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for ST-Ericsson CG2900 connectivity combo controller
+#
+
+ccflags-y :=					\
+	-Idrivers/staging/cg2900/include
+
+obj-$(CONFIG_BT_CG2900)		+= btcg2900.o
+obj-$(CONFIG_CG2900_UART)	+= cg2900_uart.o hci_ldisc.o
diff --git a/drivers/staging/cg2900/bluetooth/btcg2900.c b/drivers/staging/cg2900/bluetooth/btcg2900.c
new file mode 100644
index 00000000000..0eb39523c1e
--- /dev/null
+++ b/drivers/staging/cg2900/bluetooth/btcg2900.c
@@ -0,0 +1,1208 @@
+/*
+ * Bluetooth driver for ST-Ericsson CG2900 connectivity controller.
+ *
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com)
+ * Henrik Possung (henrik.possung@stericsson.com)
+ * Josef Kindberg (josef.kindberg@stericsson.com)
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com)
+ * Kjell Andersson (kjell.k.andersson@stericsson.com)
+ *
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <asm/byteorder.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/platform_device.h>
+#include <linux/skbuff.h>
+#include <linux/time.h>
+#include <linux/timer.h>
+#include <linux/types.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/core.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "cg2900.h"
+
+#define BT_HEADER_LENGTH		0x03
+
+#define STLC2690_HCI_REV		0x0600
+#define CG2900_PG1_HCI_REV		0x0101
+#define CG2900_PG2_HCI_REV		0x0200
+#define CG2900_PG1_SPECIAL_HCI_REV	0x0700
+
+#define NAME				"BTCG2900 "
+
+/* Wait for 5 seconds for a response to our requests */
+#define RESP_TIMEOUT			5000
+
+/* Bluetooth error codes */
+#define HCI_ERR_NO_ERROR		0x00
+#define HCI_ERR_CMD_DISALLOWED		0x0C
+
+/**
+ * enum reset_state - RESET-states of the HCI driver.
+ *
+ * @RESET_IDLE:		No reset in progress.
+ * @RESET_ACTIVATED:	Reset in progress.
+ * @RESET_UNREGISTERED:	hdev is unregistered.
+ */
+
+enum reset_state {
+	RESET_IDLE,
+	RESET_ACTIVATED,
+	RESET_UNREGISTERED
+};
+
+/**
+ * enum enable_state - ENABLE-states of the HCI driver.
+ *
+ * @ENABLE_IDLE:			The HCI driver is loaded but not opened.
+ * @ENABLE_WAITING_BT_ENABLED_CC:	The HCI driver is waiting for a command
+ *					complete event from the BT chip as a
+ *					response to a BT Enable (true) command.
+ * @ENABLE_BT_ENABLED:			The BT chip is enabled.
+ * @ENABLE_WAITING_BT_DISABLED_CC:	The HCI driver is waiting for a command
+ *					complete event from the BT chip as a
+ *					response to a BT Enable (false) command.
+ * @ENABLE_BT_DISABLED:			The BT chip is disabled.
+ * @ENABLE_BT_ERROR:			The HCI driver is in a bad state, some
+ *					thing has failed and is not expected to
+ *					work properly.
+ */
+enum enable_state {
+	ENABLE_IDLE,
+	ENABLE_WAITING_BT_ENABLED_CC,
+	ENABLE_BT_ENABLED,
+	ENABLE_WAITING_BT_DISABLED_CC,
+	ENABLE_BT_DISABLED,
+	ENABLE_BT_ERROR
+};
+
+/* Defines which state the driver has when BT is active */
+#define BTCG2900_ACTIVE_STATE		ENABLE_BT_ENABLED
+
+/**
+ * struct btcg2900_info - Specifies HCI driver private data.
+ *
+ * This type specifies CG2900 HCI driver private data.
+ *
+ * @list:		list_head struct.
+ * @parent:		Parent to this BT device. All BT channels will have
+ *			common parent.
+ * @cmd:		Device structure for BT command channel.
+ * @evt:		Device structure for BT event channel.
+ * @acl:		Device structure for BT ACL channel.
+ * @pdev:		Device structure for platform device.
+ * @hdev:		Device structure for HCI device.
+ * @reset_state:	Device enum for HCI driver reset state.
+ * @enable_state:	Device enum for HCI driver BT enable state.
+ */
+struct btcg2900_info {
+	struct list_head	list;
+	struct device		*parent;
+	struct device		*cmd;
+	struct device		*evt;
+	struct device		*acl;
+	struct hci_dev		*hdev;
+	enum reset_state	reset_state;
+	enum enable_state	enable_state;
+};
+
+/**
+ * struct enable_info - Specifies data for sending enable commands.
+ *
+ * @enable:		True if command should enable the functionality.
+ * @name:		Name of the command, only informative.
+ * @get_cmd:		Function for retrieving command.
+ * @success:		State to set upon success.
+ * @awaiting_cc:	State to set while waiting for response.
+ * @failed:		State to set upon failure.
+ */
+struct enable_info {
+	bool	enable;
+	char	*name;
+	struct sk_buff* (*get_cmd)(struct btcg2900_info *info, bool enable);
+	enum enable_state success;
+	enum enable_state awaiting_cc;
+	enum enable_state failed;
+};
+
+/**
+ * struct dev_info - Specifies private data used when receiving callbacks from CG2900 driver.
+ *
+ * @hci_data_type:	Type of data according to BlueZ.
+ */
+struct dev_info {
+	u8	hci_data_type;
+};
+
+/* Defines for vs_bt_enable_cmd */
+#define BT_VS_BT_ENABLE			0xFF10
+#define VS_BT_DISABLE			0x00
+#define VS_BT_ENABLE			0x01
+
+/**
+ * struct vs_bt_enable_cmd - Specifies HCI VS Bluetooth_Enable command.
+ *
+ * @op_code:	HCI command op code.
+ * @len:	Parameter length of command.
+ * @enable:	0 for disable BT, 1 for enable BT.
+ */
+struct vs_bt_enable_cmd {
+	__le16	op_code;
+	u8	len;
+	u8	enable;
+} __packed;
+
+/*
+ * hci_wait_queue - Main Wait Queue in HCI driver.
+ */
+static DECLARE_WAIT_QUEUE_HEAD(hci_wait_queue);
+
+/*
+ * btcg2900_devices - List of active CG2900 BT devices.
+ */
+static LIST_HEAD(btcg2900_devices);
+
+/* Internal function declarations */
+static int register_bluetooth(struct btcg2900_info *info);
+
+/* Internal functions */
+
+/**
+ * get_bt_enable_cmd() - Get HCI BT enable command.
+ * @info:	Device info structure.
+ * @bt_enable:	true if Bluetooth IP shall be enabled, false otherwise.
+ *
+ * Returns:
+ *   NULL if no command shall be sent,
+ *   sk_buffer with command otherwise.
+ */
+static struct sk_buff *get_bt_enable_cmd(struct btcg2900_info *info,
+					 bool bt_enable)
+{
+	struct sk_buff *skb;
+	struct vs_bt_enable_cmd *cmd;
+	struct cg2900_rev_data rev_data;
+	struct cg2900_user_data *pf_data;
+
+	pf_data = dev_get_platdata(info->cmd);
+
+	if (!pf_data->get_local_revision(pf_data, &rev_data)) {
+		BT_ERR(NAME "Couldn't get revision");
+		return NULL;
+	}
+
+	/* If connected chip does not support the command return NULL */
+	if (CG2900_PG1_SPECIAL_HCI_REV != rev_data.revision &&
+	    CG2900_PG1_HCI_REV != rev_data.revision &&
+	    CG2900_PG2_HCI_REV != rev_data.revision)
+		return NULL;
+
+	/* CG2900 used */
+	skb = pf_data->alloc_skb(sizeof(*cmd), GFP_KERNEL);
+	if (!skb) {
+		BT_ERR(NAME "Could not allocate skb");
+		return NULL;
+	}
+
+	cmd = (struct vs_bt_enable_cmd *)skb_put(skb, sizeof(*cmd));
+	cmd->op_code = cpu_to_le16(BT_VS_BT_ENABLE);
+	cmd->len = sizeof(*cmd) - BT_HEADER_LENGTH;
+	if (bt_enable)
+		cmd->enable = VS_BT_ENABLE;
+	else
+		cmd->enable = VS_BT_DISABLE;
+
+	return skb;
+}
+
+/**
+ * close_bt_users() - Close all BT channels.
+ * @info:	HCI driver info structure.
+ */
+static void close_bt_users(struct btcg2900_info *info)
+{
+	struct cg2900_user_data *pf_data;
+
+	pf_data = dev_get_platdata(info->cmd);
+	if (pf_data->opened)
+		pf_data->close(pf_data);
+
+	pf_data = dev_get_platdata(info->acl);
+	if (pf_data->opened)
+		pf_data->close(pf_data);
+
+	pf_data = dev_get_platdata(info->evt);
+	if (pf_data->opened)
+		pf_data->close(pf_data);
+}
+
+/**
+ * handle_bt_enable_comp() - Handle received BtEnable Complete event.
+ * @info:	Info structure.
+ * @skb:	Buffer with data coming from device.
+ *
+ * Returns:
+ *   true if data has been handled internally,
+ *   false otherwise.
+ */
+static bool handle_bt_enable_comp(struct btcg2900_info *info, u8 status)
+{
+	if (info->enable_state != ENABLE_WAITING_BT_ENABLED_CC &&
+	    info->enable_state != ENABLE_WAITING_BT_DISABLED_CC)
+		return false;
+	/*
+	 * This is the command complete event for
+	 * the HCI_Cmd_VS_Bluetooth_Enable.
+	 * Check result and update state.
+	 *
+	 * The BT chip is enabled/disabled. Either it was enabled/
+	 * disabled now (status NO_ERROR) or it was already enabled/
+	 * disabled (assuming status CMD_DISALLOWED is already enabled/
+	 * disabled).
+	 */
+	if (status != HCI_ERR_NO_ERROR && status != HCI_ERR_CMD_DISALLOWED) {
+		BT_ERR(NAME "Could not enable/disable BT core (0x%X)",
+			   status);
+		BT_DBG("New enable_state: ENABLE_BT_ERROR");
+		info->enable_state = ENABLE_BT_ERROR;
+		goto finished;
+	}
+
+	if (info->enable_state == ENABLE_WAITING_BT_ENABLED_CC) {
+		BT_DBG("New enable_state: ENABLE_BT_ENABLED");
+		info->enable_state = ENABLE_BT_ENABLED;
+		BT_INFO("CG2900 BT core is enabled");
+	} else {
+		BT_DBG("New enable_state: ENABLE_BT_DISABLED");
+		info->enable_state = ENABLE_BT_DISABLED;
+		BT_INFO("CG2900 BT core is disabled");
+	}
+
+finished:
+	/* Wake up whoever is waiting for this result. */
+	wake_up_all(&hci_wait_queue);
+	return true;
+}
+
+/**
+ * handle_bt_enable_stat() - Handle received BtEnable Status event.
+ * @info:	Info structure.
+ * @skb:	Buffer with data coming from device.
+ *
+ * Returns:
+ *   true if data has been handled internally,
+ *   false otherwise.
+ */
+static bool handle_bt_enable_stat(struct btcg2900_info *info, u8 status)
+{
+	if (info->enable_state != ENABLE_WAITING_BT_DISABLED_CC &&
+	    info->enable_state != ENABLE_WAITING_BT_ENABLED_CC)
+		return false;
+
+	BT_DBG("HCI Driver received Command Status (BT enable): 0x%X", status);
+	/*
+	 * This is the command status event for the HCI_Cmd_VS_Bluetooth_Enable.
+	 * Just free the packet.
+	 */
+	return true;
+}
+
+/**
+ * handle_rx_evt() - Check if received data is response to internal command.
+ * @info:	Info structure.
+ * @skb:	Buffer with data coming from device.
+ *
+ * Returns:
+ *   true if data has been handled internally,
+ *   false otherwise.
+ */
+static bool handle_rx_evt(struct btcg2900_info *info, struct sk_buff *skb)
+{
+	struct hci_event_hdr *evt = (struct hci_event_hdr *)skb->data;
+	struct hci_ev_cmd_complete *cmd_complete;
+	struct hci_ev_cmd_status *cmd_status;
+	u16 op_code;
+	u8 status;
+	bool pkt_handled = false;
+
+	/* If BT is active no internal packets shall be generated */
+	if (info->enable_state == BTCG2900_ACTIVE_STATE)
+		return false;
+
+	if (evt->evt == HCI_EV_CMD_COMPLETE) {
+		cmd_complete = (struct hci_ev_cmd_complete *)(evt + 1);
+		status = *((u8 *)(cmd_complete + 1));
+		op_code = le16_to_cpu(cmd_complete->opcode);
+
+		if (op_code == BT_VS_BT_ENABLE)
+			pkt_handled = handle_bt_enable_comp(info, status);
+	} else if (evt->evt == HCI_EV_CMD_STATUS) {
+		cmd_status = (struct hci_ev_cmd_status *)(evt + 1);
+		op_code = le16_to_cpu(cmd_status->opcode);
+		status = cmd_status->status;
+
+		if (op_code == BT_VS_BT_ENABLE)
+			pkt_handled = handle_bt_enable_stat(info, status);
+	}
+
+	if (pkt_handled)
+		kfree_skb(skb);
+
+	return pkt_handled;
+}
+
+/**
+ * hci_read_cb() - Callback for handling data received from CG2900 driver.
+ * @dev:	Device receiving data.
+ * @skb:	Buffer with data coming from device.
+ */
+static void hci_read_cb(struct cg2900_user_data *user, struct sk_buff *skb)
+{
+	int err = 0;
+	struct dev_info *dev_info;
+	struct btcg2900_info *info;
+
+	dev_info = cg2900_get_usr(user);
+	info = dev_get_drvdata(user->dev);
+
+	if (user->dev != info->evt || !handle_rx_evt(info, skb)) {
+		bt_cb(skb)->pkt_type = dev_info->hci_data_type;
+		skb->dev = (struct net_device *)info->hdev;
+		/* Update BlueZ stats */
+		info->hdev->stat.byte_rx += skb->len;
+		if (bt_cb(skb)->pkt_type == HCI_ACLDATA_PKT)
+			info->hdev->stat.acl_rx++;
+		else
+			info->hdev->stat.evt_rx++;
+
+		BT_DBG("Data receive %d bytes", skb->len);
+
+		/* Provide BlueZ with received frame*/
+		err = hci_recv_frame(skb);
+		/* If err, skb have been freed in hci_recv_frame() */
+		if (err)
+			BT_ERR(NAME "Failed in supplying packet to Bluetooth"
+			       " stack (%d)", err);
+	}
+}
+
+/**
+ * hci_reset_cb() - Callback for handling reset from CG2900 driver.
+ * @dev:	CPD device resetting.
+ */
+static void hci_reset_cb(struct cg2900_user_data *dev)
+{
+	int err;
+	struct btcg2900_info *info;
+	struct cg2900_user_data *pf_data;
+
+	BT_INFO(NAME "hci_reset_cb");
+
+	info = dev_get_drvdata(dev->dev);
+
+	BT_DBG("New reset_state: RESET_ACTIVATED");
+	info->reset_state = RESET_ACTIVATED;
+
+	/*
+	 * Continue to deregister hdev if all channels has been reset else
+	 * return.
+	 */
+	pf_data = dev_get_platdata(info->acl);
+	if (pf_data->opened)
+		return;
+	pf_data = dev_get_platdata(info->cmd);
+	if (pf_data->opened)
+		return;
+	pf_data = dev_get_platdata(info->evt);
+	if (pf_data->opened)
+		return;
+
+	/*
+	 * Deregister HCI device. Close and Destruct functions should
+	 * in turn be called by BlueZ.
+	 */
+	BT_DBG("Deregister HCI device");
+	err = hci_unregister_dev(info->hdev);
+	if (err)
+		BT_ERR(NAME "Can not deregister HCI device! (%d)", err);
+		/*
+		 * Now we are in trouble. Try to register a new hdev
+		 * anyway even though this will cost some memory.
+		 */
+
+	wait_event_timeout(hci_wait_queue,
+			(RESET_UNREGISTERED == info->reset_state),
+			msecs_to_jiffies(RESP_TIMEOUT));
+	if (RESET_UNREGISTERED != info->reset_state)
+		/*
+		 * Now we are in trouble. Try to register a new hdev
+		 * anyway even though this will cost some memory.
+		 */
+		BT_ERR(NAME "Timeout expired. Could not deregister HCI device");
+
+	/* Init and register hdev */
+	BT_DBG("Register HCI device");
+	err = register_bluetooth(info);
+	if (err)
+		BT_ERR(NAME "HCI Device registration error (%d)", err);
+}
+
+/**
+ * send_enable_cmd() - Send a command with only enable/disable functionality.
+ * @info:	Info structure.
+ * @en_info:	Enable info structure.
+ *
+ * Returns:
+ *   0 if successful,
+ *   -EACCES if correct response to command is not received,
+ *   Error codes from CG2900 write.
+ */
+static int send_enable_cmd(struct btcg2900_info *info,
+			   struct enable_info *en_info)
+{
+	struct sk_buff *enable_cmd;
+	int err;
+	struct cg2900_user_data *pf_data;
+
+	/*
+	 * Call function that returns the chip dependent enable HCI command.
+	 * If NULL is returned, then no bt_enable command should be sent to the
+	 * chip.
+	 */
+	enable_cmd = en_info->get_cmd(info, en_info->enable);
+	if (!enable_cmd) {
+		BT_DBG("%s New enable_state: %d", en_info->name,
+		       en_info->success);
+		info->enable_state = en_info->success;
+		return 0;
+	}
+
+	/* Set the HCI state before sending command to chip. */
+	BT_DBG("%s New enable_state: %d", en_info->name, en_info->awaiting_cc);
+	info->enable_state = en_info->awaiting_cc;
+
+	/* Send command to chip */
+	pf_data = dev_get_platdata(info->cmd);
+	err = pf_data->write(pf_data, enable_cmd);
+	if (err) {
+		BT_ERR("Couldn't send %s command (%d)", en_info->name, err);
+		kfree_skb(enable_cmd);
+		info->enable_state = en_info->failed;
+		return err;
+	}
+
+	/*
+	 * Wait for callback to receive command complete and then wake us up
+	 * again.
+	 */
+	wait_event_timeout(hci_wait_queue,
+				info->enable_state == en_info->success,
+				msecs_to_jiffies(RESP_TIMEOUT));
+	/* Check the current state to see if it worked */
+	if (info->enable_state != en_info->success) {
+		BT_ERR("Could not change %s state (%d)",
+		       en_info->name, info->enable_state);
+		BT_DBG("%s New enable_state: %d", en_info->name,
+		       en_info->failed);
+		info->enable_state = en_info->failed;
+		return -EACCES;
+	}
+
+	return 0;
+}
+
+/**
+ * btcg2900_open() - Open HCI interface.
+ * @hdev:	HCI device being opened.
+ *
+ * BlueZ callback function for opening HCI interface to device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL pointer is supplied.
+ *   -EOPNOTSUPP if supplied packet type is not supported.
+ *   -EBUSY if device is already opened.
+ *   -EACCES if opening of channels failed.
+ */
+static int btcg2900_open(struct hci_dev *hdev)
+{
+	struct btcg2900_info *info;
+	struct cg2900_user_data *pf_data;
+	int err;
+	struct enable_info en_info;
+
+	BT_INFO("Open ST-Ericsson CG2900 driver");
+
+	if (!hdev) {
+		BT_ERR(NAME "NULL supplied for hdev");
+		return -EINVAL;
+	}
+
+	info = (struct btcg2900_info *)hdev->driver_data;
+	if (!info) {
+		BT_ERR(NAME "NULL supplied for driver_data");
+		return -EINVAL;
+	}
+
+	if (test_and_set_bit(HCI_RUNNING, &(hdev->flags))) {
+		BT_ERR(NAME "Device already opened!");
+		return -EBUSY;
+	}
+
+	pf_data = dev_get_platdata(info->acl);
+	err = pf_data->open(pf_data);
+	if (err) {
+		BT_ERR("Couldn't open BT ACL channel (%d)", err);
+		goto handle_error;
+	}
+
+	pf_data = dev_get_platdata(info->cmd);
+	err = pf_data->open(pf_data);
+	if (err) {
+		BT_ERR("Couldn't open BT CMD channel (%d)", err);
+		goto handle_error;
+	}
+
+	pf_data = dev_get_platdata(info->evt);
+	err = pf_data->open(pf_data);
+	if (err) {
+		BT_ERR("Couldn't open BT EVT channel (%d)", err);
+		goto handle_error;
+	}
+
+	if (info->reset_state == RESET_ACTIVATED) {
+		BT_DBG("New reset_state: RESET_IDLE");
+		info->reset_state = RESET_IDLE;
+	}
+
+	/* First enable the BT core */
+	en_info.enable = true;
+	en_info.get_cmd = get_bt_enable_cmd;
+	en_info.name = "VS BT Enable (true)";
+	en_info.success = ENABLE_BT_ENABLED;
+	en_info.awaiting_cc = ENABLE_WAITING_BT_ENABLED_CC;
+	en_info.failed = ENABLE_BT_DISABLED;
+
+	err = send_enable_cmd(info, &en_info);
+	if (err) {
+		BT_ERR("Couldn't enable BT core (%d)", err);
+		goto handle_error;
+	}
+
+	return 0;
+
+handle_error:
+	close_bt_users(info);
+	clear_bit(HCI_RUNNING, &(hdev->flags));
+	return err;
+
+}
+
+/**
+ * btcg2900_close() - Close HCI interface.
+ * @hdev:	HCI device being closed.
+ *
+ * BlueZ callback function for closing HCI interface.
+ * It flushes the interface first.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL pointer is supplied.
+ *   -EOPNOTSUPP if supplied packet type is not supported.
+ *   -EBUSY if device is not opened.
+ */
+static int btcg2900_close(struct hci_dev *hdev)
+{
+	struct btcg2900_info *info = NULL;
+	int err;
+	struct enable_info en_info;
+
+	BT_DBG("btcg2900_close");
+
+	if (!hdev) {
+		BT_ERR(NAME "NULL supplied for hdev");
+		return -EINVAL;
+	}
+
+	info = (struct btcg2900_info *)hdev->driver_data;
+	if (!info) {
+		BT_ERR(NAME "NULL supplied for driver_data");
+		return -EINVAL;
+	}
+
+	if (!test_and_clear_bit(HCI_RUNNING, &(hdev->flags))) {
+		BT_ERR(NAME "Device already closed!");
+		return -EBUSY;
+	}
+
+	/* Do not do this if there is an reset ongoing */
+	if (info->reset_state == RESET_ACTIVATED)
+		goto remove_users;
+
+	/* Now disable the BT core */
+	en_info.enable = false;
+	en_info.get_cmd = get_bt_enable_cmd;
+	en_info.name = "VS BT Enable (false)";
+	en_info.success = ENABLE_BT_DISABLED;
+	en_info.awaiting_cc = ENABLE_WAITING_BT_DISABLED_CC;
+	en_info.failed = ENABLE_BT_ENABLED;
+
+	err = send_enable_cmd(info, &en_info);
+	if (err)
+		BT_ERR("Couldn't disable BT core (%d)", err);
+
+remove_users:
+	/* Finally deregister all users and free allocated data */
+	close_bt_users(info);
+	return 0;
+}
+
+/**
+ * btcg2900_send() - Send packet to device.
+ * @skb:	sk buffer to be sent.
+ *
+ * BlueZ callback function for sending sk buffer.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL pointer is supplied.
+ *   -EOPNOTSUPP if supplied packet type is not supported.
+ *   Error codes from cg2900_write.
+ */
+static int btcg2900_send(struct sk_buff *skb)
+{
+	struct hci_dev *hdev;
+	struct btcg2900_info *info;
+	struct cg2900_user_data *pf_data;
+	int err = 0;
+
+	if (!skb) {
+		BT_ERR(NAME "NULL supplied for skb");
+		return -EINVAL;
+	}
+
+	hdev = (struct hci_dev *)(skb->dev);
+	if (!hdev) {
+		BT_ERR(NAME "NULL supplied for hdev");
+		return -EINVAL;
+	}
+
+	info = (struct btcg2900_info *)hdev->driver_data;
+	if (!info) {
+		BT_ERR(NAME "NULL supplied for info");
+		return -EINVAL;
+	}
+
+	/* Update BlueZ stats */
+	hdev->stat.byte_tx += skb->len;
+
+	BT_DBG("Data transmit %d bytes", skb->len);
+
+	switch (bt_cb(skb)->pkt_type) {
+	case HCI_COMMAND_PKT:
+		BT_DBG("Sending HCI_COMMAND_PKT");
+		pf_data = dev_get_platdata(info->cmd);
+		err = pf_data->write(pf_data, skb);
+		hdev->stat.cmd_tx++;
+		break;
+	case HCI_ACLDATA_PKT:
+		BT_DBG("Sending HCI_ACLDATA_PKT");
+		pf_data = dev_get_platdata(info->acl);
+		err = pf_data->write(pf_data, skb);
+		hdev->stat.acl_tx++;
+		break;
+	default:
+		BT_ERR(NAME "Trying to transmit unsupported packet type"
+		       " (0x%.2X)", bt_cb(skb)->pkt_type);
+		err = -EOPNOTSUPP;
+		break;
+	};
+
+	return err;
+}
+
+/**
+ * btcg2900_destruct() - Destruct HCI interface.
+ * @hdev:	HCI device being destructed.
+ */
+static void btcg2900_destruct(struct hci_dev *hdev)
+{
+	struct btcg2900_info *info;
+
+	BT_DBG("btcg2900_destruct");
+
+	info = hdev->driver_data;
+	if (!info) {
+		BT_ERR(NAME "NULL supplied for info");
+		return;
+	}
+
+	/*
+	 * When destruct is called it means that the Bluetooth stack is done
+	 * with the HCI device and we can now free it.
+	 * Normally we do this only when removing the whole module through
+	 * btcg2900_remove(), but when being reset we free the device here and
+	 * we then set the reset state so that the reset handler can allocate a
+	 * new HCI device and then register it to the Bluetooth stack.
+	 */
+	if (info->reset_state == RESET_ACTIVATED) {
+		if (info->hdev) {
+			hci_free_dev(info->hdev);
+			info->hdev = NULL;
+		}
+		BT_DBG("New reset_state: RESET_UNREGISTERED");
+		info->reset_state = RESET_UNREGISTERED;
+		wake_up_all(&hci_wait_queue);
+	}
+}
+
+/**
+ * get_info() - Return info structure for this device.
+ * @dev:	Current device.
+ *
+ * Returns:
+ *   Pointer to info struct if there is no error.
+ *   ERR_PTR(-ENOMEM) if allocation fails.
+ */
+static struct btcg2900_info *get_info(struct device *dev)
+{
+	struct list_head *cursor;
+	struct btcg2900_info *tmp;
+	struct btcg2900_info *info = NULL;
+
+	/* Find the info structure */
+	list_for_each(cursor, &btcg2900_devices) {
+		tmp = list_entry(cursor, struct btcg2900_info, list);
+		if (tmp->parent == dev->parent) {
+			info = tmp;
+			break;
+		}
+	}
+
+	if (info)
+		return info;
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		BT_ERR("Could not allocate info struct");
+		return ERR_PTR(-ENOMEM);
+	}
+	info->parent = dev->parent;
+	list_add_tail(&info->list, &btcg2900_devices);
+	BT_DBG("CG2900 device added");
+	return info;
+}
+
+/**
+ * device_removed() - Remove device from list if there are no channels left.
+ * @info:	BTCG2900 info structure.
+ */
+static void device_removed(struct btcg2900_info *info)
+{
+	struct list_head *cursor;
+	struct btcg2900_info *tmp;
+
+	if (info->acl || info->cmd || info->evt)
+		/* There are still devices active */
+		return;
+
+	/* Find the info structure and delete it */
+	list_for_each(cursor, &btcg2900_devices) {
+		tmp = list_entry(cursor, struct btcg2900_info, list);
+		if (tmp == info) {
+			list_del(cursor);
+			break;
+		}
+	}
+	kfree(info);
+	BT_DBG("CG2900 device removed");
+}
+
+/**
+ * register_bluetooth() - Initialize module.
+ *
+ * Alloc, init, and register HCI device to BlueZ.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation fails.
+ *   Error codes from hci_register_dev.
+ */
+static int register_bluetooth(struct btcg2900_info *info)
+{
+	int err;
+	struct cg2900_user_data *pf_data;
+
+	/* Check if all channels have been probed */
+	if (!info->acl || !info->cmd || !info->evt)
+		return 0;
+
+	pf_data = dev_get_platdata(info->cmd);
+
+	info->hdev = hci_alloc_dev();
+	if (!info->hdev) {
+		BT_ERR("Could not allocate mem for CG2900 BT driver");
+		return -ENOMEM;
+	}
+
+	SET_HCIDEV_DEV(info->hdev, info->parent);
+	info->hdev->bus = pf_data->channel_data.bt_bus;
+	info->hdev->driver_data = info;
+	info->hdev->owner = THIS_MODULE;
+	info->hdev->open = btcg2900_open;
+	info->hdev->close = btcg2900_close;
+	info->hdev->send = btcg2900_send;
+	info->hdev->destruct = btcg2900_destruct;
+
+	err = hci_register_dev(info->hdev);
+	if (err) {
+		BT_ERR("Can not register BTCG2900 HCI device (%d)", err);
+		hci_free_dev(info->hdev);
+		info->hdev = NULL;
+	}
+
+	BT_INFO("CG2900 registered");
+
+	BT_DBG("New enable_state: ENABLE_IDLE");
+	info->enable_state = ENABLE_IDLE;
+	BT_DBG("New reset_state: RESET_IDLE");
+	info->reset_state = RESET_IDLE;
+
+	return err;
+}
+
+/**
+ * probe_common() - Initialize channel and register to BT stack.
+ * @dev:		Current device.
+ * @info:		BTCG2900 info structure.
+ * @hci_data_type:	Data type of this channel, e.g. ACL.
+ *
+ * Allocate and initialize private data. Register to Bluetooth stack.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation fails.
+ *   Error codes from register_bluetooth.
+ */
+static int probe_common(struct platform_device *pdev,
+			struct btcg2900_info *info,
+			u8 hci_data_type)
+{
+	int err;
+	struct cg2900_user_data *pf_data;
+	struct dev_info *dev_info;
+	struct device *dev = &pdev->dev;
+
+	dev_info = kzalloc(sizeof(*dev_info), GFP_KERNEL);
+	if (!dev_info) {
+		BT_ERR("Could not allocate dev_info");
+		return -ENOMEM;
+	}
+
+	dev_set_drvdata(dev, info);
+
+	pf_data = mfd_get_cell(pdev)->platform_data;
+	pf_data->dev = dev;
+	pf_data->read_cb = hci_read_cb;
+	pf_data->reset_cb = hci_reset_cb;
+
+	/* Set platform data */
+	dev->platform_data = pf_data;
+
+	/* Init and register hdev */
+	err = register_bluetooth(info);
+	if (err) {
+		BT_ERR("HCI Device registration error (%d)", err);
+		kfree(dev_info);
+		return err;
+	}
+	dev_info->hci_data_type = hci_data_type;
+	cg2900_set_usr(pf_data, dev_info);
+
+	return 0;
+}
+
+/**
+ * btcg2900_cmd_probe() - Initialize command channel.
+ * @pdev:	Platform device.
+ *
+ * Allocate and initialize private data.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from get_info and probe_common.
+ */
+static int __devinit btcg2900_cmd_probe(struct platform_device *pdev)
+{
+	int err;
+	struct btcg2900_info *info;
+
+	BT_DBG("Starting CG2900 Command channel");
+
+	info = get_info(&pdev->dev);
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	info->cmd = &pdev->dev;
+
+	err = probe_common(pdev, info, HCI_COMMAND_PKT);
+	if (err) {
+		BT_ERR("Failed to initialize channel");
+		info->cmd = NULL;
+		device_removed(info);
+		return err;
+	}
+
+	return 0;
+}
+
+/**
+ * btcg2900_acl_probe() - Initialize command channel.
+ * @pdev:	Platform device.
+ *
+ * Allocate and initialize private data.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from get_info and probe_common.
+ */
+static int __devinit btcg2900_acl_probe(struct platform_device *pdev)
+{
+	int err;
+	struct btcg2900_info *info;
+
+	BT_DBG("Starting CG2900 ACL channel");
+
+	info = get_info(&pdev->dev);
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	info->acl = &pdev->dev;
+
+	err = probe_common(pdev, info, HCI_ACLDATA_PKT);
+	if (err) {
+		BT_ERR("Failed to initialize channel");
+		info->acl = NULL;
+		device_removed(info);
+		return err;
+	}
+
+	return 0;
+}
+
+/**
+ * btcg2900_evt_probe() - Initialize event channel.
+ * @pdev:	Platform device.
+ *
+ * Allocate and initialize private data.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from get_info and probe_common.
+ */
+static int __devinit btcg2900_evt_probe(struct platform_device *pdev)
+{
+	int err;
+	struct btcg2900_info *info;
+
+	BT_DBG("Starting CG2900 Event channel");
+
+	info = get_info(&pdev->dev);
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	info->evt = &pdev->dev;
+
+	err = probe_common(pdev, info, HCI_EVENT_PKT);
+	if (err) {
+		BT_ERR("Failed to initialize channel");
+		info->evt = NULL;
+		device_removed(info);
+		return err;
+	}
+
+	return 0;
+}
+
+/**
+ * remove_common() - Remove channel.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from hci_unregister_dev.
+ */
+static int remove_common(struct platform_device *pdev,
+			 struct btcg2900_info *info)
+{
+	int err = 0;
+	struct cg2900_user_data *pf_data;
+	struct dev_info *dev_info;
+
+	pf_data = mfd_get_cell(pdev)->platform_data;
+	dev_info = cg2900_get_usr(pf_data);
+
+	kfree(dev_info);
+	cg2900_set_usr(pf_data, NULL);
+
+	if (!info->hdev)
+		goto finished;
+
+	BT_INFO("Unregistering CG2900");
+	err = hci_unregister_dev(info->hdev);
+	if (err)
+		BT_ERR("Can not unregister HCI device (%d)", err);
+	hci_free_dev(info->hdev);
+	info->hdev = NULL;
+
+finished:
+	device_removed(info);
+	return err;
+}
+
+/**
+ * btcg2900_cmd_remove() - Remove command channel.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from remove_common.
+ */
+static int __devexit btcg2900_cmd_remove(struct platform_device *pdev)
+{
+	struct btcg2900_info *info;
+
+	BT_DBG("Removing CG2900 Command channel");
+
+	info = dev_get_drvdata(&pdev->dev);
+	info->cmd = NULL;
+	return remove_common(pdev, info);
+}
+
+/**
+ * btcg2900_acl_remove() - Remove ACL channel.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from remove_common.
+ */
+static int __devexit btcg2900_acl_remove(struct platform_device *pdev)
+{
+	struct btcg2900_info *info;
+
+	BT_DBG("Removing CG2900 ACL channel");
+
+	info = dev_get_drvdata(&pdev->dev);
+	info->acl = NULL;
+	return remove_common(pdev, info);
+}
+
+/**
+ * btcg2900_evt_remove() - Remove event channel.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from remove_common.
+ */
+static int __devexit btcg2900_evt_remove(struct platform_device *pdev)
+{
+	struct btcg2900_info *info;
+
+	BT_DBG("Removing CG2900 Event channel");
+
+	info = dev_get_drvdata(&pdev->dev);
+	info->evt = NULL;
+	return remove_common(pdev, info);
+}
+
+static struct platform_driver btcg2900_cmd_driver = {
+	.driver = {
+		.name	= "cg2900-btcmd",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= btcg2900_cmd_probe,
+	.remove	= __devexit_p(btcg2900_cmd_remove),
+};
+
+static struct platform_driver btcg2900_acl_driver = {
+	.driver = {
+		.name	= "cg2900-btacl",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= btcg2900_acl_probe,
+	.remove	= __devexit_p(btcg2900_acl_remove),
+};
+
+static struct platform_driver btcg2900_evt_driver = {
+	.driver = {
+		.name	= "cg2900-btevt",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= btcg2900_evt_probe,
+	.remove	= __devexit_p(btcg2900_evt_remove),
+};
+
+/**
+ * btcg2900_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init btcg2900_init(void)
+{
+	int err;
+
+	BT_DBG("btcg2900_init");
+
+	err = platform_driver_register(&btcg2900_cmd_driver);
+	if (err) {
+		BT_ERR("Failed to register cmd (%d)", err);
+		return err;
+	}
+	err = platform_driver_register(&btcg2900_acl_driver);
+	if (err) {
+		BT_ERR("Failed to register acl (%d)", err);
+		return err;
+	}
+	err = platform_driver_register(&btcg2900_evt_driver);
+	if (err) {
+		BT_ERR("Failed to register evt (%d)", err);
+		return err;
+	}
+	return err;
+}
+
+/**
+ * btcg2900_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit btcg2900_exit(void)
+{
+	BT_DBG("btcg2900_exit");
+	platform_driver_unregister(&btcg2900_cmd_driver);
+	platform_driver_unregister(&btcg2900_acl_driver);
+	platform_driver_unregister(&btcg2900_evt_driver);
+}
+
+module_init(btcg2900_init);
+module_exit(btcg2900_exit);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_AUTHOR("Henrik Possung ST-Ericsson");
+MODULE_AUTHOR("Josef Kindberg ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux Bluetooth HCI H:4 Driver for ST-Ericsson controller");
diff --git a/drivers/staging/cg2900/bluetooth/cg2900_uart.c b/drivers/staging/cg2900/bluetooth/cg2900_uart.c
new file mode 100644
index 00000000000..c0c8d8e30bd
--- /dev/null
+++ b/drivers/staging/cg2900/bluetooth/cg2900_uart.c
@@ -0,0 +1,2104 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * Lukasz Rymanowski (lukasz.rymanowski@tieto.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth UART Driver for ST-Ericsson CG2900 connectivity controller.
+ */
+#define NAME			"cg2900_uart"
+#define pr_fmt(fmt)		NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <linux/atomic.h>
+#include <linux/device.h>
+#include <linux/gpio.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/timer.h>
+#include <linux/regulator/consumer.h>
+#include <linux/tty.h>
+#include <linux/tty_ldisc.h>
+#include <linux/types.h>
+#include <linux/wakelock.h>
+#include <linux/workqueue.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+
+#include "cg2900.h"
+
+#include "hci_uart.h"
+
+#define MAIN_DEV		(uart_info->dev)
+
+/* Workqueues' names */
+#define UART_WQ_NAME		"cg2900_uart_wq"
+#define UART_NAME		"cg2900_uart"
+
+/*
+ * A BT command complete event without any parameters is the defined size plus
+ * 1 byte extra for the status field which is always present in a
+ * command complete event.
+ */
+#define HCI_BT_CMD_COMPLETE_LEN	(sizeof(struct hci_ev_cmd_complete) + 1)
+
+/* Timers used in milliseconds */
+#define UART_TX_TIMEOUT		100
+#define UART_RX_TIMEOUT		20
+#define UART_RESP_TIMEOUT	1000
+#define UART_RESUME_TIMEOUT	20
+
+/* Number of bytes to reserve at start of sk_buffer when receiving packet */
+#define RX_SKB_RESERVE		8
+/* Max size of received packet (not including reserved bytes) */
+#define RX_SKB_MAX_SIZE		1024
+
+/* Size of the header in the different packets */
+#define HCI_BT_EVT_HDR_SIZE	2
+#define HCI_BT_ACL_HDR_SIZE	4
+#define HCI_FM_RADIO_HDR_SIZE	1
+#define HCI_GNSS_HDR_SIZE	3
+
+/* Position of length field in the different packets */
+#define HCI_EVT_LEN_POS		2
+#define HCI_ACL_LEN_POS		3
+#define FM_RADIO_LEN_POS	1
+#define GNSS_LEN_POS		2
+
+/* Baud rate defines */
+#define ZERO_BAUD_RATE		0
+#define DEFAULT_BAUD_RATE	115200
+#define HIGH_BAUD_RATE		3000000
+
+#define BT_SIZE_OF_HDR				(sizeof(__le16) + sizeof(__u8))
+#define BT_PARAM_LEN(__pkt_len)			(__pkt_len - BT_SIZE_OF_HDR)
+
+/* Standardized Bluetooth H:4 channels */
+#define HCI_BT_CMD_H4_CHANNEL			0x01
+#define HCI_BT_ACL_H4_CHANNEL			0x02
+#define HCI_BT_SCO_H4_CHANNEL			0x03
+#define HCI_BT_EVT_H4_CHANNEL			0x04
+
+#define BT_BDADDR_SIZE				6
+
+/* Reserve 1 byte for the HCI H:4 header */
+#define HCI_H4_SIZE				1
+#define CG2900_SKB_RESERVE			HCI_H4_SIZE
+
+/* Default H4 channels which may change depending on connected controller */
+#define HCI_FM_RADIO_H4_CHANNEL			0x08
+#define HCI_GNSS_H4_CHANNEL			0x09
+
+/* Bluetooth error codes */
+#define HCI_BT_ERROR_NO_ERROR			0x00
+
+/* Bytes in the command Hci_Cmd_ST_Set_Uart_Baud_Rate */
+#define CG2900_BAUD_RATE_57600				0x03
+#define CG2900_BAUD_RATE_115200				0x02
+#define CG2900_BAUD_RATE_230400				0x01
+#define CG2900_BAUD_RATE_460800				0x00
+#define CG2900_BAUD_RATE_921600				0x20
+#define CG2900_BAUD_RATE_2000000			0x25
+#define CG2900_BAUD_RATE_3000000			0x27
+#define CG2900_BAUD_RATE_4000000			0x2B
+
+/* GNSS */
+struct gnss_hci_hdr {
+	__u8	op_code;
+	__le16	plen;
+} __packed;
+
+/* FM legacy command packet */
+struct fm_leg_cmd {
+	__u8	length;
+	__u8	opcode;
+	__u8	read_write;
+	__u8	fm_function;
+	union { /* Payload varies with function */
+		__le16	irqmask;
+		struct fm_leg_fm_cmd {
+			__le16	head;
+			__le16	data[];
+		} fm_cmd;
+	};
+} __packed;
+
+/* FM legacy command complete packet */
+struct fm_leg_cmd_cmpl {
+	__u8	param_length;
+	__u8	status;
+	__u8	opcode;
+	__u8	read_write;
+	__u8	cmd_status;
+	__u8	fm_function;
+	__le16	response_head;
+	__le16	data[];
+} __packed;
+
+/* FM legacy interrupt packet, PG2 style */
+struct fm_leg_irq_v2 {
+	__u8	param_length;
+	__u8	status;
+	__u8	opcode;
+	__u8	event_type;
+	__u8	event_id;
+	__le16	irq;
+} __packed;
+
+/* FM legacy interrupt packet, PG1 style */
+struct fm_leg_irq_v1 {
+	__u8	param_length;
+	__u8	opcode;
+	__u8	event_id;
+	__le16	irq;
+} __packed;
+
+union fm_leg_evt_or_irq {
+	__u8			param_length;
+	struct fm_leg_cmd_cmpl	evt;
+	struct fm_leg_irq_v2	irq_v2;
+	struct fm_leg_irq_v1	irq_v1;
+} __packed;
+
+/* BT VS SetBaudRate command */
+#define CG2900_BT_OP_VS_SET_BAUD_RATE			0xFC09
+struct bt_vs_set_baud_rate_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	baud_rate;
+} __packed;
+
+/**
+ * enum uart_rx_state - UART RX-state for UART.
+ * @W4_PACKET_TYPE:	Waiting for packet type.
+ * @W4_EVENT_HDR:	Waiting for BT event header.
+ * @W4_ACL_HDR:		Waiting for BT ACL header.
+ * @W4_FM_RADIO_HDR:	Waiting for FM header.
+ * @W4_GNSS_HDR:	Waiting for GNSS header.
+ * @W4_DATA:		Waiting for data in rest of the packet (after header).
+ */
+enum uart_rx_state {
+	W4_PACKET_TYPE,
+	W4_EVENT_HDR,
+	W4_ACL_HDR,
+	W4_FM_RADIO_HDR,
+	W4_GNSS_HDR,
+	W4_DATA
+};
+
+/**
+  * enum sleep_state - Sleep-state for UART.
+  * @CHIP_AWAKE:  Chip is awake.
+  * @CHIP_FALLING_ASLEEP:  Chip is falling asleep.
+  * @CHIP_ASLEEP: Chip is asleep.
+  * @CHIP_SUSPENDED: Chip in suspend state.
+  * @CHIP_RESUMING: Chip is going back from suspend state.
+  * @CHIP_POWERED_DOWN: Chip is off.
+  */
+enum sleep_state {
+	CHIP_AWAKE,
+	CHIP_FALLING_ASLEEP,
+	CHIP_ASLEEP,
+	CHIP_SUSPENDED,
+	CHIP_RESUMING,
+	CHIP_POWERED_DOWN
+};
+
+/**
+ * enum baud_rate_change_state - Baud rate-state for UART.
+ * @BAUD_IDLE:		No baud rate change is ongoing.
+ * @BAUD_SENDING_RESET:	HCI reset has been sent. Waiting for command complete
+ *			event.
+ * @BAUD_START:		Set baud rate cmd scheduled for sending.
+ * @BAUD_SENDING:	Set baud rate cmd sending in progress.
+ * @BAUD_WAITING:	Set baud rate cmd sent, waiting for command complete
+ *			event.
+ * @BAUD_SUCCESS:	Baud rate change has succeeded.
+ * @BAUD_FAIL:		Baud rate change has failed.
+ */
+enum baud_rate_change_state {
+	BAUD_IDLE,
+	BAUD_SENDING_RESET,
+	BAUD_START,
+	BAUD_SENDING,
+	BAUD_WAITING,
+	BAUD_SUCCESS,
+	BAUD_FAIL
+};
+
+/**
+ * struct uart_work_struct - Work structure for UART module.
+ * @work:	Work structure.
+ * @data:	Pointer to private data.
+ *
+ * This structure is used to pack work for work queue.
+ */
+struct uart_work_struct {
+	struct work_struct	work;
+	void			*data;
+};
+
+/**
+ * struct uart_delayed_work_struct - Work structure for UART module.
+ * @delayed_work:	Work structure.
+ * @data:	Pointer to private data.
+ *
+ * This structure is used to pack work for work queue.
+ */
+struct uart_delayed_work_struct {
+	struct delayed_work	work;
+	void			*data;
+};
+
+/**
+ * struct uart_info - Main UART info structure.
+ * @rx_state:		Current RX state.
+ * @rx_count:		Number of bytes left to receive.
+ * @rx_skb:		SK_buffer to store the received data into.
+ * @tx_queue:		TX queue for sending data to chip.
+ * @hu:			Hci uart structure.
+ * @wq:			UART work queue.
+ * @baud_rate_state:	UART baud rate change state.
+ * @baud_rate:		Current baud rate setting.
+ * @sleep_state:	UART sleep state.
+ * @sleep_work:	Delayed sleep work struct.
+ * @wakeup_work:	Wake-up work struct.
+ * @restart_sleep_work:	Reschedule sleep_work and wake-up work struct.
+ * @sleep_state_lock:	Used to protect chip state.
+ * @sleep_allowed:	Indicates if tty has functions needed for sleep mode.
+ * @tx_in_progress:	Indicates data sending in progress.
+ * @rx_in_progress:	Indicates data receiving in progress.
+ * @transmission_lock:	Spin_lock to protect tx/rx_in_progress.
+ * @regulator:		Regulator.
+ * @regulator_enabled:	True if regulator is enabled.
+ * @dev:		Pointer to CG2900 uart device.
+ * @chip_dev:		Chip device for current UART transport.
+ * @cts_irq:		CTS interrupt for this UART.
+ * @cts_gpio:		CTS GPIO for this UART.
+ * @wake_lock:		Wake lock for keeping user space awake (for Android).
+ * @suspend_blocked:	True if suspend operation is blocked in the framework.
+ */
+struct uart_info {
+	enum uart_rx_state		rx_state;
+	unsigned long			rx_count;
+	struct sk_buff			*rx_skb;
+	struct sk_buff_head		tx_queue;
+
+	struct hci_uart			*hu;
+
+	struct workqueue_struct		*wq;
+	enum baud_rate_change_state	baud_rate_state;
+	int				baud_rate;
+	enum sleep_state		sleep_state;
+	struct uart_delayed_work_struct	sleep_work;
+	struct uart_work_struct		wakeup_work;
+	struct uart_work_struct		restart_sleep_work;
+	struct mutex			sleep_state_lock;
+	bool				sleep_allowed;
+	bool				tx_in_progress;
+	bool				rx_in_progress;
+	spinlock_t			transmission_lock;
+	struct	regulator		*regulator;
+	bool				regulator_enabled;
+	struct device			*dev;
+	struct cg2900_chip_dev		chip_dev;
+	int				cts_irq;
+	int				cts_gpio;
+	struct wake_lock		wake_lock;
+	bool				suspend_blocked;
+};
+
+/* Module parameters */
+static int uart_default_baud = DEFAULT_BAUD_RATE;
+static int uart_high_baud = HIGH_BAUD_RATE;
+static int uart_debug;
+
+static DECLARE_WAIT_QUEUE_HEAD(uart_wait_queue);
+
+static void wake_up_chip(struct uart_info *uart_info);
+
+/**
+ * is_chip_flow_off() - Check if chip has set flow off.
+ * @tty:	Pointer to tty.
+ *
+ * Returns:
+ *   true - chip flows off.
+ *   false - chip flows on.
+ */
+static bool is_chip_flow_off(struct uart_info *uart_info)
+{
+	int lines;
+
+	lines = hci_uart_tiocmget(uart_info->hu);
+
+	if (lines & TIOCM_CTS)
+		return false;
+	else
+		return true;
+}
+
+/**
+ * create_work_item() - Create work item and add it to the work queue.
+ * @uart_info:	Main Uart structure.
+ * @work_func:	Work function.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EBUSY if not possible to queue work.
+ *   -ENOMEM if allocation fails.
+ */
+static int create_work_item(struct uart_info *uart_info,
+					work_func_t work_func)
+{
+	struct uart_work_struct *new_work;
+	int res;
+
+	new_work = kmalloc(sizeof(*new_work), GFP_ATOMIC);
+	if (!new_work) {
+		dev_err(MAIN_DEV,
+			"Failed to alloc memory for uart_work_struct\n");
+		return -ENOMEM;
+	}
+
+	new_work->data = uart_info;
+	INIT_WORK(&new_work->work, work_func);
+
+	res = queue_work(uart_info->wq, &new_work->work);
+	if (!res) {
+		dev_err(MAIN_DEV,
+			"Failed to queue work_struct because it's already "
+			"in the queue\n");
+		kfree(new_work);
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+/**
+ * handle_cts_irq() - Called to handle CTS interrupt in work context.
+ * @work:	work which needs to be done.
+ *
+ * The handle_cts_irq() function is a work handler called if interrupt on CTS
+ * occurred. It wakes up the transport.
+ */
+static void handle_cts_irq(struct work_struct *work)
+{
+	struct uart_work_struct *current_work =
+		container_of(work, struct uart_work_struct, work);
+	struct uart_info *uart_info = (struct uart_info *)current_work->data;
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+	/* Mark that there is an ongoing transfer. */
+	uart_info->rx_in_progress = true;
+	spin_unlock_bh(&(uart_info->transmission_lock));
+
+	/* Cancel pending sleep work if there is any. */
+	cancel_delayed_work_sync(&uart_info->sleep_work.work);
+
+	mutex_lock(&(uart_info->sleep_state_lock));
+
+	if (uart_info->sleep_state == CHIP_SUSPENDED) {
+		dev_dbg(MAIN_DEV, "New sleep_state: CHIP_RESUMING\n");
+		uart_info->sleep_state = CHIP_RESUMING;
+		mutex_unlock(&(uart_info->sleep_state_lock));
+	} else {
+		mutex_unlock(&(uart_info->sleep_state_lock));
+		wake_up_chip(uart_info);
+	}
+
+	kfree(current_work);
+}
+
+/**
+ * cts_interrupt() - Called to handle CTS interrupt.
+ * @irq:	Interrupt that occurred.
+ * @dev_id:	Device ID where interrupt occurred.
+ *
+ * The cts_interrupt() function is called if interrupt on CTS occurred.
+ * It disables the interrupt and starts a new work thread to handle
+ * the interrupt.
+ */
+static irqreturn_t cts_interrupt(int irq, void *dev_id)
+{
+	struct uart_info *uart_info = dev_get_drvdata(dev_id);
+#ifdef CONFIG_PM
+	disable_irq_wake(irq);
+#endif
+	disable_irq_nosync(irq);
+	if (!uart_info->suspend_blocked) {
+		wake_lock(&uart_info->wake_lock);
+		uart_info->suspend_blocked = true;
+	}
+
+	/* Create work and leave IRQ context. */
+	(void)create_work_item(uart_info, handle_cts_irq);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * set_cts_irq() - Enable interrupt on CTS.
+ * @uart_info: Main Uart structure.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from request_irq and disable_uart.
+ */
+static int set_cts_irq(struct uart_info *uart_info)
+{
+	int err;
+	int cts_val = 0;
+
+	/* Set IRQ on CTS. */
+	err = request_irq(uart_info->cts_irq,
+			  cts_interrupt,
+			  IRQF_TRIGGER_FALLING,
+			  UART_NAME,
+			  uart_info->dev);
+	if (err) {
+		dev_err(MAIN_DEV, "Could not request CTS IRQ (%d)\n", err);
+		return err;
+	}
+
+	/*
+	 * It may happen that there was already an interrupt on CTS just before
+	 * the enable_irq() call above. If the CTS line is low now it means that
+	 * it's happened, so disable the CTS interrupt and return -ECANCELED.
+	 */
+	cts_val = gpio_get_value(uart_info->cts_gpio);
+	if (!cts_val) {
+		dev_dbg(MAIN_DEV, "Missed interrupt, going back to "
+			"awake state\n");
+		free_irq(uart_info->cts_irq, uart_info->dev);
+		return -ECANCELED;
+	}
+
+#ifdef CONFIG_PM
+	enable_irq_wake(uart_info->cts_irq);
+#endif
+	return 0;
+}
+
+/**
+ * disable_uart_pins() - Disable the UART pins.
+ * @uart_info: Main Uart structure.
+ */
+static void disable_uart_pins(struct uart_info *uart_info)
+{
+	struct cg2900_platform_data *pf_data;
+
+	pf_data = dev_get_platdata(uart_info->dev);
+
+	if (pf_data->uart.disable_uart) {
+		int err = pf_data->uart.disable_uart(&uart_info->chip_dev);
+		if (err)
+			dev_err(MAIN_DEV,
+				"Unable to disable UART Hardware (%d)\n", err);
+	}
+}
+
+/**
+ * enable_uart_pins() - Enable the UART pins.
+ * @uart_info: Main Uart structure.
+ */
+static void enable_uart_pins(struct uart_info *uart_info)
+{
+	struct cg2900_platform_data *pf_data;
+
+	pf_data = dev_get_platdata(uart_info->dev);
+
+	if (pf_data->uart.enable_uart) {
+		int err = pf_data->uart.enable_uart(&uart_info->chip_dev);
+		if (err)
+			dev_err(MAIN_DEV,
+				"Unable to enable UART Hardware (%d)\n", err);
+	}
+}
+
+/**
+ * unset_cts_irq() - Disable interrupt on CTS.
+ * @uart_info: Main Uart structure.
+ */
+static void unset_cts_irq(struct uart_info *uart_info)
+{
+	/* Free CTS interrupt */
+	free_irq(uart_info->cts_irq, uart_info->dev);
+}
+
+/**
+ * get_sleep_timeout() - Get sleep timeout.
+ * @uart_info: Main Uart structure.
+ *
+ * Check all conditions for sleep and return sleep timeout.
+ * Return:
+ *	0: sleep not allowed.
+ *	other: Timeout value in ms.
+ */
+static unsigned long get_sleep_timeout(struct uart_info *uart_info)
+{
+	unsigned long timeout_jiffies = cg2900_get_sleep_timeout();
+
+	if (timeout_jiffies &&
+		uart_info->hu &&
+		uart_info->hu->fd &&
+		uart_info->sleep_allowed)
+		return timeout_jiffies;
+
+	return 0;
+}
+
+/**
+ * work_wake_up_chip() - Called to wake up of the transport in work context.
+ * @work:	work which needs to be done.
+ */
+static void work_wake_up_chip(struct work_struct *work)
+{
+	struct uart_work_struct *current_work =
+		container_of(work, struct uart_work_struct, work);
+	struct uart_info *uart_info = (struct uart_info *)current_work->data;
+
+	wake_up_chip(uart_info);
+}
+
+/**
+ * wake_up_chip() - Wakes up the chip and transport.
+ * @work:	pointer to a work struct if the function was called that way.
+ *
+ * Depending on the current sleep state it may wake up the transport.
+ */
+static void wake_up_chip(struct uart_info *uart_info)
+{
+	unsigned long timeout_jiffies = get_sleep_timeout(uart_info);
+
+	/* Resuming state is special. Need to get back chip to awake state. */
+	if (!timeout_jiffies && uart_info->sleep_state != CHIP_RESUMING)
+		return;
+
+	mutex_lock(&(uart_info->sleep_state_lock));
+
+	/*
+	 * If chip is powered down we cannot wake it up here. It has to be woken
+	 * up through a call to uart_set_chip_power()
+	 */
+	if (CHIP_POWERED_DOWN == uart_info->sleep_state)
+		goto finished;
+
+	if (!uart_info->suspend_blocked) {
+		wake_lock(&uart_info->wake_lock);
+		uart_info->suspend_blocked = true;
+	}
+
+	/*
+	 * This function indicates data is transmitted.
+	 * Therefore see to that the chip is awake.
+	 */
+	if (CHIP_AWAKE == uart_info->sleep_state)
+		goto finished;
+
+	if (CHIP_ASLEEP == uart_info->sleep_state ||
+		CHIP_RESUMING == uart_info->sleep_state) {
+		/* Wait before disabling IRQ */
+		schedule_timeout_killable(
+				msecs_to_jiffies(UART_RESUME_TIMEOUT));
+
+		/* Disable IRQ only when it was enabled. */
+		unset_cts_irq(uart_info);
+		(void)hci_uart_set_baudrate(uart_info->hu,
+							uart_info->baud_rate);
+
+		enable_uart_pins(uart_info);
+
+		/*
+		 * Wait before flowing on. Otherwise UART might not be ready in
+		 * time
+		 */
+		schedule_timeout_killable(
+				msecs_to_jiffies(UART_RESUME_TIMEOUT));
+
+		/* Set FLOW on. */
+		hci_uart_flow_ctrl(uart_info->hu, FLOW_ON);
+	}
+
+	/* Unset BREAK. */
+	dev_dbg(MAIN_DEV, "wake_up_chip: Clear break\n");
+	hci_uart_set_break(uart_info->hu, BREAK_OFF);
+
+	dev_dbg(MAIN_DEV, "New sleep_state: CHIP_AWAKE\n");
+	uart_info->sleep_state = CHIP_AWAKE;
+
+finished:
+	mutex_unlock(&(uart_info->sleep_state_lock));
+}
+
+/**
+ * set_chip_sleep_mode() - Put the chip and transport to sleep mode.
+ * @work:	pointer to work_struct.
+ *
+ * The set_chip_sleep_mode() function is called if there are no ongoing data
+ * transmissions. It tries to put the chip in sleep mode.
+ *
+ */
+static void set_chip_sleep_mode(struct work_struct *work)
+{
+	int err = 0;
+	struct delayed_work *delayed_work =
+			container_of(work, struct delayed_work, work);
+	struct uart_delayed_work_struct *current_work =	container_of(
+			delayed_work, struct uart_delayed_work_struct, work);
+	struct uart_info *uart_info = (struct uart_info *)current_work->data;
+	unsigned long timeout_jiffies = get_sleep_timeout(uart_info);
+	int chars_in_buffer;
+
+	if (!timeout_jiffies)
+		return;
+
+	if (uart_info->tx_in_progress || uart_info->rx_in_progress) {
+		dev_dbg(MAIN_DEV, "Not going to sleep, TX/RX in progress\n");
+		return;
+	}
+
+	mutex_lock(&(uart_info->sleep_state_lock));
+
+	switch (uart_info->sleep_state) {
+	case CHIP_FALLING_ASLEEP:
+		if (!is_chip_flow_off(uart_info)) {
+			dev_dbg(MAIN_DEV, "Chip flow is on, it's not ready to"
+				"sleep yet\n");
+			goto schedule_sleep_work;
+		}
+
+		/* Flow OFF. */
+		hci_uart_flow_ctrl(uart_info->hu, FLOW_OFF);
+
+		disable_uart_pins(uart_info);
+
+		/*
+		 * Set baud zero.
+		 * This cause shut off UART clock as well.
+		 */
+		(void)hci_uart_set_baudrate(uart_info->hu,
+							ZERO_BAUD_RATE);
+		err = set_cts_irq(uart_info);
+		if (err < 0) {
+			enable_uart_pins(uart_info);
+			(void)hci_uart_set_baudrate(uart_info->hu,
+							uart_info->baud_rate);
+			hci_uart_flow_ctrl(uart_info->hu, FLOW_ON);
+			hci_uart_set_break(uart_info->hu, BREAK_OFF);
+
+			dev_dbg(MAIN_DEV, "New sleep_state: CHIP_AWAKE\n");
+			uart_info->sleep_state = CHIP_AWAKE;
+
+			if (err == -ECANCELED)
+				goto finished;
+			else {
+				dev_err(MAIN_DEV, "Can not set interrupt on "
+						"CTS, err:%d\n", err);
+				goto error;
+			}
+		}
+
+		dev_dbg(MAIN_DEV, "New sleep_state: CHIP_ASLEEP\n");
+		uart_info->sleep_state = CHIP_ASLEEP;
+		if (uart_info->suspend_blocked) {
+			wake_unlock(&uart_info->wake_lock);
+			uart_info->suspend_blocked = false;
+		}
+		break;
+	case CHIP_AWAKE:
+		chars_in_buffer = hci_uart_chars_in_buffer(uart_info->hu);
+		if (chars_in_buffer) {
+			dev_dbg(MAIN_DEV, "sleep_timer_expired: "
+					"tx not finished, stay awake and "
+					"restart the sleep timer\n");
+			goto schedule_sleep_work;
+		}
+
+		dev_dbg(MAIN_DEV, "sleep_timer_expired: Set break\n");
+		hci_uart_set_break(uart_info->hu, BREAK_ON);
+
+		dev_dbg(MAIN_DEV, "New sleep_state: CHIP_FALLING_ASLEEP\n");
+		uart_info->sleep_state = CHIP_FALLING_ASLEEP;
+		goto schedule_sleep_work;
+
+	case CHIP_POWERED_DOWN:
+	case CHIP_SUSPENDED:
+	case CHIP_ASLEEP: /* Fallthrough. */
+	default:
+		dev_dbg(MAIN_DEV,
+			"Chip sleeps, is suspended or powered down\n");
+		break;
+	}
+
+	mutex_unlock(&(uart_info->sleep_state_lock));
+
+	return;
+
+finished:
+	mutex_unlock(&(uart_info->sleep_state_lock));
+	return;
+schedule_sleep_work:
+	mutex_unlock(&(uart_info->sleep_state_lock));
+	if (timeout_jiffies)
+		queue_delayed_work(uart_info->wq, &uart_info->sleep_work.work,
+				timeout_jiffies);
+	return;
+error:
+	/* Disable sleep mode.*/
+	dev_err(MAIN_DEV, "Disable sleep mode\n");
+	uart_info->sleep_allowed = false;
+	mutex_unlock(&(uart_info->sleep_state_lock));
+}
+
+#ifdef CONFIG_PM
+/**
+ * cg2900_uart_suspend() - Called by Linux PM to put the device in a low power mode.
+ * @pdev:	Pointer to platform device.
+ * @state:	New state.
+ *
+ * In UART case, CG2900 driver does nothing on suspend.
+ *
+ * Returns:
+ *   0 - Success.
+ */
+static int cg2900_uart_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int err = 0;
+	struct uart_info *uart_info = dev_get_drvdata(&pdev->dev);
+
+	mutex_lock(&(uart_info->sleep_state_lock));
+
+	if (uart_info->sleep_state == CHIP_POWERED_DOWN)
+		goto finished;
+
+	if (uart_info->sleep_state != CHIP_ASLEEP) {
+		err = -EBUSY;
+		goto finished;
+	}
+
+	dev_dbg(MAIN_DEV, "New sleep_state: CHIP_SUSPENDED\n");
+	uart_info->sleep_state = CHIP_SUSPENDED;
+
+finished:
+	mutex_unlock(&(uart_info->sleep_state_lock));
+	return err;
+}
+
+/**
+ * cg2900_uart_resume() - Called to bring a device back from a low power state.
+ * @pdev:	Pointer to platform device.
+ *
+ * In UART case, CG2900 driver does nothing on resume.
+ *
+ * Returns:
+ *   0 - Success.
+ */
+static int cg2900_uart_resume(struct platform_device *pdev)
+{
+	struct uart_info *uart_info = dev_get_drvdata(&pdev->dev);
+
+	mutex_lock(&(uart_info->sleep_state_lock));
+
+	if (uart_info->sleep_state == CHIP_RESUMING)
+		/* System resume because of trafic on UART. Lets wakeup.*/
+		(void)queue_work(uart_info->wq, &uart_info->wakeup_work.work);
+	else if (uart_info->sleep_state != CHIP_POWERED_DOWN) {
+		/* No need to wakeup chip. Go back to Asleep state.*/
+		dev_dbg(MAIN_DEV, "New sleep_state: CHIP_ASLEEP\n");
+		uart_info->sleep_state = CHIP_ASLEEP;
+	}
+
+	mutex_unlock(&(uart_info->sleep_state_lock));
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+/**
+ * cg2900_enable_regulator() - Enable regulator.
+ * @uart_info: Main Uart structure.
+ *
+ * Returns:
+ *   0 - Success.
+ *   Error from regulator_get, regulator_enable.
+ */
+static int cg2900_enable_regulator(struct uart_info *uart_info)
+{
+#ifdef CONFIG_REGULATOR
+	int err;
+
+	/* Get and enable regulator. */
+	uart_info->regulator = regulator_get(uart_info->dev, "gbf_1v8");
+	if (IS_ERR(uart_info->regulator)) {
+		dev_err(MAIN_DEV, "Not able to find regulator\n");
+		err = PTR_ERR(uart_info->regulator);
+	} else {
+		err = regulator_enable(uart_info->regulator);
+		if (err)
+			dev_err(MAIN_DEV, "Not able to enable regulator\n");
+		else
+			uart_info->regulator_enabled = true;
+	}
+	return err;
+#else
+	return 0;
+#endif
+}
+
+/**
+ * cg2900_disable_regulator() - Disable regulator.
+ * @uart_info: Main Uart structure.
+ *
+ */
+static void cg2900_disable_regulator(struct uart_info *uart_info)
+{
+#ifdef CONFIG_REGULATOR
+	/* Disable and put regulator. */
+	if (uart_info->regulator && uart_info->regulator_enabled) {
+		regulator_disable(uart_info->regulator);
+		uart_info->regulator_enabled = false;
+	}
+	regulator_put(uart_info->regulator);
+	uart_info->regulator = NULL;
+#endif
+}
+
+/**
+ * is_set_baud_rate_cmd() - Checks if data contains set baud rate hci cmd.
+ * @data:	Pointer to data array to check.
+ *
+ * Returns:
+ *   true - if cmd found;
+ *   false - otherwise.
+ */
+static bool is_set_baud_rate_cmd(const char *data)
+{
+	struct hci_command_hdr *cmd;
+
+	if (data[0] != HCI_BT_CMD_H4_CHANNEL)
+		return false;
+
+	cmd = (struct hci_command_hdr *)&data[1];
+	if (le16_to_cpu(cmd->opcode) == CG2900_BT_OP_VS_SET_BAUD_RATE &&
+	    cmd->plen == BT_PARAM_LEN(sizeof(struct bt_vs_set_baud_rate_cmd)))
+		return true;
+
+	return false;
+}
+
+/**
+ * is_bt_cmd_complete_no_param() - Checks if data contains command complete event for a certain command.
+ * @skb:	sk_buffer containing the data including H:4 header.
+ * @opcode:	Command op code.
+ * @status:	Command status.
+ *
+ * Returns:
+ *   true - If this is the command complete we were looking for;
+ *   false - otherwise.
+ */
+static bool is_bt_cmd_complete_no_param(struct sk_buff *skb, u16 opcode,
+					u8 *status)
+{
+	struct hci_event_hdr *event;
+	struct hci_ev_cmd_complete *complete;
+	u8 *data = &(skb->data[0]);
+
+	if (HCI_BT_EVT_H4_CHANNEL != *data)
+		return false;
+
+	data += HCI_H4_SIZE;
+	event = (struct hci_event_hdr *)data;
+	if (HCI_EV_CMD_COMPLETE != event->evt ||
+	    HCI_BT_CMD_COMPLETE_LEN != event->plen)
+		return false;
+
+	data += sizeof(*event);
+	complete = (struct hci_ev_cmd_complete *)data;
+	if (opcode != le16_to_cpu(complete->opcode))
+		return false;
+
+	if (status) {
+		/*
+		 * All command complete have the status field at first byte of
+		 * packet data.
+		 */
+		data += sizeof(*complete);
+		*status = *data;
+	}
+	return true;
+}
+
+/**
+ * alloc_rx_skb() - Alloc an sk_buff structure for receiving data from controller.
+ * @size:	Size in number of octets.
+ * @priority:	Allocation priority, e.g. GFP_KERNEL.
+ *
+ * Returns:
+ *   Pointer to sk_buff structure.
+ */
+static struct sk_buff *alloc_rx_skb(unsigned int size, gfp_t priority)
+{
+	struct sk_buff *skb;
+
+	/* Allocate the SKB and reserve space for the header */
+	skb = alloc_skb(size + RX_SKB_RESERVE, priority);
+	if (skb)
+		skb_reserve(skb, RX_SKB_RESERVE);
+
+	return skb;
+}
+
+/**
+ * finish_setting_baud_rate() - Handles sending the ste baud rate hci cmd.
+ * @hu:	Pointer to associated Hci uart structure.
+ *
+ * finish_setting_baud_rate() makes sure that the set baud rate cmd has
+ * been really sent out on the wire and then switches the tty driver to new
+ * baud rate.
+ */
+static void finish_setting_baud_rate(struct hci_uart *hu)
+{
+	struct uart_info *uart_info =
+			(struct uart_info *)dev_get_drvdata(hu->proto->dev);
+	/*
+	 * Give the tty driver time to send data and proceed. If it hasn't
+	 * been sent we can't do much about it anyway.
+	 */
+	schedule_timeout_killable(msecs_to_jiffies(UART_TX_TIMEOUT));
+
+	/*
+	 * Now set the termios struct to the new baudrate. Start by storing
+	 * the old termios.
+	 */
+	if (hci_uart_set_baudrate(hu, uart_info->baud_rate) < 0) {
+		/* Something went wrong.*/
+		dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_IDLE\n");
+		uart_info->baud_rate_state = BAUD_IDLE;
+	} else {
+		dev_dbg(MAIN_DEV, "Setting termios to new baud rate\n");
+		dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_WAITING\n");
+		uart_info->baud_rate_state = BAUD_WAITING;
+	}
+
+	hci_uart_flow_ctrl(hu, FLOW_ON);
+}
+
+/**
+ * alloc_set_baud_rate_cmd() - Allocates new sk_buff and fills in the change baud rate hci cmd.
+ * @uart_info:	Main Uart structure.
+ * @baud:	(in/out) Requested new baud rate. Updated to default baud rate
+ *		upon invalid value.
+ *
+ * Returns:
+ *   Pointer to allocated sk_buff if successful;
+ *   NULL otherwise.
+ */
+static struct sk_buff *alloc_set_baud_rate_cmd(struct uart_info *uart_info,
+							int *baud)
+{
+	struct sk_buff *skb;
+	u8 *h4;
+	struct bt_vs_set_baud_rate_cmd *cmd;
+
+	skb = alloc_skb(sizeof(*cmd) + CG2900_SKB_RESERVE, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(MAIN_DEV,
+			"alloc_set_baud_rate_cmd: Failed to alloc skb\n");
+		return NULL;
+	}
+	skb_reserve(skb, CG2900_SKB_RESERVE);
+
+	cmd = (struct bt_vs_set_baud_rate_cmd *)skb_put(skb, sizeof(cmd));
+
+	/* Create the Hci_Cmd_ST_Set_Uart_Baud_Rate packet */
+	cmd->opcode = cpu_to_le16(CG2900_BT_OP_VS_SET_BAUD_RATE);
+	cmd->plen = BT_PARAM_LEN(sizeof(cmd));
+
+	switch (*baud) {
+	case 57600:
+		cmd->baud_rate = CG2900_BAUD_RATE_57600;
+		break;
+	case 115200:
+		cmd->baud_rate = CG2900_BAUD_RATE_115200;
+		break;
+	case 230400:
+		cmd->baud_rate = CG2900_BAUD_RATE_230400;
+		break;
+	case 460800:
+		cmd->baud_rate = CG2900_BAUD_RATE_460800;
+		break;
+	case 921600:
+		cmd->baud_rate = CG2900_BAUD_RATE_921600;
+		break;
+	case 2000000:
+		cmd->baud_rate = CG2900_BAUD_RATE_2000000;
+		break;
+	case 3000000:
+		cmd->baud_rate = CG2900_BAUD_RATE_3000000;
+		break;
+	case 4000000:
+		cmd->baud_rate = CG2900_BAUD_RATE_4000000;
+		break;
+	default:
+		dev_err(MAIN_DEV,
+			"Invalid speed requested (%d), using 115200 bps "
+			"instead\n", *baud);
+		cmd->baud_rate = CG2900_BAUD_RATE_115200;
+		*baud = 115200;
+		break;
+	};
+
+	h4 = skb_push(skb, HCI_H4_SIZE);
+	*h4 = HCI_BT_CMD_H4_CHANNEL;
+
+	return skb;
+}
+
+/**
+ * work_do_transmit() - Transmit data packet to connectivity controller over UART.
+ * @work:	Pointer to work info structure. Contains uart_info structure
+ *		pointer.
+ */
+static void work_do_transmit(struct work_struct *work)
+{
+	struct uart_work_struct *current_work =
+			container_of(work, struct uart_work_struct, work);
+	struct uart_info *uart_info = (struct uart_info *)current_work->data;
+
+	kfree(current_work);
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+	/* Mark that there is an ongoing transfer. */
+	uart_info->tx_in_progress = true;
+	spin_unlock_bh(&(uart_info->transmission_lock));
+
+	/* Cancel pending sleep work if there is any. */
+	cancel_delayed_work_sync(&uart_info->sleep_work.work);
+
+	/* Wake up the chip and transport. */
+	wake_up_chip(uart_info);
+
+	(void)hci_uart_tx_wakeup(uart_info->hu);
+}
+
+/**
+ * work_hw_deregistered() - Handle HW deregistered.
+ * @work: Reference to work data.
+ */
+static void work_hw_deregistered(struct work_struct *work)
+{
+	struct uart_work_struct *current_work;
+	struct uart_info *uart_info;
+	int err;
+	current_work = container_of(work, struct uart_work_struct, work);
+	uart_info = (struct uart_info *)current_work->data;
+
+	err = cg2900_deregister_trans_driver(&uart_info->chip_dev);
+	if (err)
+		dev_err(MAIN_DEV, "Could not deregister UART from Core (%d)\n",
+			err);
+
+	kfree(current_work);
+}
+
+/**
+ * set_baud_rate() - Sets new baud rate for the UART.
+ * @hu:		Pointer to hci_uart structure.
+ * @baud:	New baud rate.
+ *
+ * This function first sends the HCI command
+ * Hci_Cmd_ST_Set_Uart_Baud_Rate. It then changes the baud rate in HW, and
+ * finally it waits for the Command Complete event for the
+ * Hci_Cmd_ST_Set_Uart_Baud_Rate command.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EALREADY if baud rate change is already in progress.
+ *   -EFAULT if one or more of the UART related structs is not allocated.
+ *   -ENOMEM if skb allocation has failed.
+ *   -EPERM if setting the new baud rate has failed.
+ *   Errors from create_work_item.
+ */
+static int set_baud_rate(struct hci_uart *hu, int baud)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	int old_baud_rate;
+	struct uart_info *uart_info =
+			(struct uart_info *)dev_get_drvdata(hu->proto->dev);
+
+	dev_dbg(MAIN_DEV, "set_baud_rate (%d baud)\n", baud);
+
+	if (uart_info->baud_rate_state != BAUD_IDLE) {
+		dev_err(MAIN_DEV,
+			"Trying to set new baud rate before old setting "
+			   "is finished\n");
+		return -EALREADY;
+	}
+
+	/*
+	 * Wait some time to be sure that any RX process has finished (which
+	 * flows on RTS in the end) before flowing off the RTS.
+	 */
+	schedule_timeout_killable(msecs_to_jiffies(UART_RX_TIMEOUT));
+	hci_uart_flow_ctrl(uart_info->hu, FLOW_OFF);
+
+	/*
+	 * Store old baud rate so that we can restore it if something goes
+	 * wrong.
+	 */
+	old_baud_rate = uart_info->baud_rate;
+
+	skb = alloc_set_baud_rate_cmd(uart_info, &baud);
+	if (!skb) {
+		dev_err(MAIN_DEV, "alloc_set_baud_rate_cmd failed\n");
+		return -ENOMEM;
+	}
+
+	dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_START\n");
+	uart_info->baud_rate_state = BAUD_START;
+	uart_info->baud_rate = baud;
+
+	/* Queue the sk_buffer... */
+	skb_queue_tail(&uart_info->tx_queue, skb);
+
+	/* ... and call the common UART TX function */
+	err = create_work_item(uart_info, work_do_transmit);
+	if (err) {
+		dev_err(MAIN_DEV,
+			"Failed to send change baud rate cmd, freeing skb\n");
+		skb = skb_dequeue_tail(&uart_info->tx_queue);
+		dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_IDLE\n");
+		uart_info->baud_rate_state = BAUD_IDLE;
+		uart_info->baud_rate = old_baud_rate;
+		kfree_skb(skb);
+		return err;
+	}
+
+	dev_dbg(MAIN_DEV, "Set baud rate cmd scheduled for sending\n");
+
+	/*
+	 * Now wait for the command complete.
+	 * It will come at the new baudrate.
+	 */
+	wait_event_timeout(uart_wait_queue,
+				((BAUD_SUCCESS == uart_info->baud_rate_state) ||
+				 (BAUD_FAIL    == uart_info->baud_rate_state)),
+				 msecs_to_jiffies(UART_RESP_TIMEOUT));
+	if (BAUD_SUCCESS == uart_info->baud_rate_state)
+		dev_info(MAIN_DEV, "Baud rate changed to %d baud\n", baud);
+	else {
+		dev_err(MAIN_DEV, "Failed to set new baud rate (%d)\n",
+			   uart_info->baud_rate_state);
+		err = -EPERM;
+	}
+
+	/* Finally flush the TTY so we are sure that is no bad data there */
+	hci_uart_flush_buffer(hu);
+	dev_dbg(MAIN_DEV, "Flushing TTY after baud rate change\n");
+	/* Finished. Set state to IDLE */
+	dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_IDLE\n");
+	uart_info->baud_rate_state = BAUD_IDLE;
+
+	return err;
+}
+
+/**
+ * uart_write() - Transmit data to CG2900 over UART.
+ * @dev:	Transport device information.
+ * @skb:	SK buffer to transmit.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Errors from create_work_item.
+ */
+static int uart_write(struct cg2900_chip_dev *dev, struct sk_buff *skb)
+{
+	int err;
+	struct uart_info *uart_info = dev_get_drvdata(dev->dev);
+
+	if (uart_debug)
+		dev_dbg(MAIN_DEV, "uart_write: data len = %d\n", skb->len);
+
+	/* Queue the sk_buffer... */
+	skb_queue_tail(&uart_info->tx_queue, skb);
+
+	/* ...and start TX operation */
+
+	err = create_work_item(uart_info, work_do_transmit);
+	if (err)
+		dev_err(MAIN_DEV,
+			"Failed to create work item (%d) uart_tty_wakeup\n",
+			err);
+
+	return err;
+}
+
+/**
+ * uart_open() - Open the CG2900 UART for data transfers.
+ * @dev:	Transport device information.
+ *
+ * Returns:
+ *   0 if there is no error,
+ *   -EACCES if write to transport failed,
+ *   -EIO if chip did not answer to commands.
+ *   Errors from set_baud_rate.
+ */
+static int uart_open(struct cg2900_chip_dev *dev)
+{
+	u8 *h4;
+	struct sk_buff *skb;
+	struct hci_command_hdr *cmd;
+	struct uart_info *uart_info = dev_get_drvdata(dev->dev);
+
+	/*
+	 * Chip has just been started up. It has a system to autodetect
+	 * exact baud rate and transport to use. There are only a few commands
+	 * it will recognize and HCI Reset is one of them.
+	 * We therefore start with sending that before actually changing
+	 * baud rate.
+	 *
+	 * Create the Hci_Reset packet
+	 */
+
+	skb = alloc_skb(sizeof(*cmd) + HCI_H4_SIZE, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(MAIN_DEV, "Couldn't allocate sk_buff with length %d\n",
+			     sizeof(*cmd));
+		return -EACCES;
+	}
+	skb_reserve(skb, HCI_H4_SIZE);
+	cmd = (struct hci_command_hdr *)skb_put(skb, sizeof(*cmd));
+	cmd->opcode = cpu_to_le16(HCI_OP_RESET);
+	cmd->plen = 0; /* No parameters for HCI reset */
+
+	h4 = skb_push(skb, HCI_H4_SIZE);
+	*h4 = HCI_BT_CMD_H4_CHANNEL;
+
+	dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_SENDING_RESET\n");
+	uart_info->baud_rate_state = BAUD_SENDING_RESET;
+	dev_dbg(MAIN_DEV, "Sending HCI reset before baud rate change\n");
+
+
+	/* Queue the sk_buffer... */
+	skb_queue_tail(&uart_info->tx_queue, skb);
+
+	(void)hci_uart_tx_wakeup(uart_info->hu);
+
+	/*
+	 * Wait for command complete. If error, exit without changing
+	 * baud rate.
+	 */
+	wait_event_timeout(uart_wait_queue,
+					BAUD_IDLE == uart_info->baud_rate_state,
+					msecs_to_jiffies(UART_RESP_TIMEOUT));
+	if (BAUD_IDLE != uart_info->baud_rate_state) {
+		dev_err(MAIN_DEV, "Failed to send HCI Reset\n");
+		dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_IDLE\n");
+		uart_info->baud_rate_state = BAUD_IDLE;
+		return -EIO;
+	}
+
+	/* Just return if there will be no change of baud rate */
+	if (uart_default_baud != uart_high_baud)
+		return set_baud_rate(uart_info->hu, uart_high_baud);
+	else
+		return 0;
+}
+
+/**
+ * uart_set_chip_power() - Enable or disable the CG2900.
+ * @chip_on:	true if chip shall be enabled, false otherwise.
+ */
+static void uart_set_chip_power(struct cg2900_chip_dev *dev, bool chip_on)
+{
+	int uart_baudrate = uart_default_baud;
+	struct cg2900_platform_data *pf_data;
+	struct uart_info *uart_info;
+
+	pf_data = dev_get_platdata(dev->dev);
+	uart_info = dev_get_drvdata(dev->dev);
+
+	dev_info(MAIN_DEV, "Set chip power: %s\n",
+		    (chip_on ? "ENABLE" : "DISABLE"));
+
+	/* Cancel any ongoing works.*/
+	cancel_work_sync(&uart_info->wakeup_work.work);
+	cancel_delayed_work_sync(&uart_info->sleep_work.work);
+
+	mutex_lock(&uart_info->sleep_state_lock);
+
+	if (!uart_info->hu) {
+		dev_err(MAIN_DEV, "Hci uart struct is not allocated\n");
+		goto unlock;
+	}
+
+	if (chip_on) {
+		if (!uart_info->suspend_blocked) {
+			wake_lock(&uart_info->wake_lock);
+			uart_info->suspend_blocked = true;
+		}
+		if (uart_info->sleep_state != CHIP_POWERED_DOWN) {
+			dev_err(MAIN_DEV, "Chip is already powered up (%d)\n",
+				uart_info->sleep_state);
+			goto unlock;
+		}
+
+		if (cg2900_enable_regulator(uart_info))
+			goto unlock;
+
+		if (pf_data->enable_chip) {
+			pf_data->enable_chip(dev);
+			dev_dbg(MAIN_DEV, "New sleep_state: CHIP_AWAKE\n");
+			uart_info->sleep_state = CHIP_AWAKE;
+		}
+
+		(void)hci_uart_set_baudrate(uart_info->hu, uart_baudrate);
+
+		hci_uart_flow_ctrl(uart_info->hu, FLOW_ON);
+		hci_uart_set_break(uart_info->hu, BREAK_OFF);
+	} else {
+		/* Turn off the chip.*/
+		switch (uart_info->sleep_state) {
+		case CHIP_AWAKE:
+			break;
+		case CHIP_FALLING_ASLEEP:
+			hci_uart_set_break(uart_info->hu, BREAK_OFF);
+			break;
+		case CHIP_SUSPENDED:
+		case CHIP_ASLEEP:
+			unset_cts_irq(uart_info);
+			enable_uart_pins(uart_info);
+			break;
+		default:
+			break;
+		}
+
+		if (uart_info->suspend_blocked) {
+			wake_unlock(&uart_info->wake_lock);
+			uart_info->suspend_blocked = false;
+		}
+
+		if (pf_data->disable_chip) {
+			pf_data->disable_chip(dev);
+			dev_dbg(MAIN_DEV,
+				"New sleep_state: CHIP_POWERED_DOWN\n");
+			uart_info->sleep_state = CHIP_POWERED_DOWN;
+		}
+		cg2900_disable_regulator(uart_info);
+		/*
+		 * Setting baud rate to 0 will tell UART driver to shut off its
+		 * clocks.
+		 */
+		(void)hci_uart_set_baudrate(uart_info->hu, ZERO_BAUD_RATE);
+	}
+
+unlock:
+	mutex_unlock(&(uart_info->sleep_state_lock));
+}
+
+/**
+ * uart_chip_startup_finished() - CG2900 startup finished.
+ * @dev:	Transport device information.
+ */
+static void uart_chip_startup_finished(struct cg2900_chip_dev *dev)
+{
+	struct uart_info *uart_info = dev_get_drvdata(dev->dev);
+	unsigned long timeout_jiffies = get_sleep_timeout(uart_info);
+
+	/* Schedule work to put the chip and transport to sleep. */
+	if (timeout_jiffies)
+		queue_delayed_work(uart_info->wq, &uart_info->sleep_work.work,
+				timeout_jiffies);
+}
+/**
+ * uart_close() - Close the CG2900 UART for data transfers.
+ * @dev:	Transport device information.
+ *
+ * Returns:
+ *   0 if there is no error.
+ */
+static int uart_close(struct cg2900_chip_dev *dev)
+{
+	/* The chip is already shut down. Power off the chip. */
+	uart_set_chip_power(dev, false);
+	return 0;
+}
+
+/**
+ * send_skb_to_core() - Sends packet received from UART to CG2900 Core.
+ * @skb:	Received data packet.
+ *
+ * This function checks if UART is waiting for Command complete event,
+ * see set_baud_rate.
+ * If it is waiting it checks if it is the expected packet and the status.
+ * If not is passes the packet to CG2900 Core.
+ */
+static void send_skb_to_core(struct uart_info *uart_info, struct sk_buff *skb)
+{
+	u8 status;
+
+	if (!skb) {
+		dev_err(MAIN_DEV, "send_skb_to_core: Received NULL as skb\n");
+		return;
+	}
+
+	if (BAUD_WAITING == uart_info->baud_rate_state) {
+		/*
+		 * Should only really be one packet received now:
+		 * the CmdComplete for the SetBaudrate command
+		 * Let's see if this is the packet we are waiting for.
+		 */
+		if (!is_bt_cmd_complete_no_param(skb,
+				CG2900_BT_OP_VS_SET_BAUD_RATE, &status)) {
+			/*
+			 * Received other event. Should not really happen,
+			 * but pass the data to CG2900 Core anyway.
+			 */
+			dev_dbg(MAIN_DEV, "Sending packet to CG2900 Core while "
+				"waiting for BaudRate CmdComplete\n");
+			uart_info->chip_dev.c_cb.data_from_chip
+				(&uart_info->chip_dev, skb);
+			return;
+		}
+
+		/*
+		 * We have received complete event for our baud rate
+		 * change command
+		 */
+		if (HCI_BT_ERROR_NO_ERROR == status) {
+			dev_dbg(MAIN_DEV, "Received baud rate change complete "
+				"event OK\n");
+			dev_dbg(MAIN_DEV,
+				"New baud_rate_state: BAUD_SUCCESS\n");
+			uart_info->baud_rate_state = BAUD_SUCCESS;
+		} else {
+			dev_err(MAIN_DEV,
+				"Received baud rate change complete event "
+				"with status 0x%X\n", status);
+			dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_FAIL\n");
+			uart_info->baud_rate_state = BAUD_FAIL;
+		}
+		wake_up_all(&uart_wait_queue);
+		kfree_skb(skb);
+	} else if (BAUD_SENDING_RESET == uart_info->baud_rate_state) {
+		/*
+		 * Should only really be one packet received now:
+		 * the CmdComplete for the Reset command
+		 * Let's see if this is the packet we are waiting for.
+		 */
+		if (!is_bt_cmd_complete_no_param(skb, HCI_OP_RESET, &status)) {
+			/*
+			 * Received other event. Should not really happen,
+			 * but pass the data to CG2900 Core anyway.
+			 */
+			dev_dbg(MAIN_DEV, "Sending packet to CG2900 Core while "
+				"waiting for Reset CmdComplete\n");
+			uart_info->chip_dev.c_cb.data_from_chip
+					(&uart_info->chip_dev, skb);
+			return;
+		}
+
+		/*
+		 * We have received complete event for our baud rate
+		 * change command
+		 */
+		if (HCI_BT_ERROR_NO_ERROR == status) {
+			dev_dbg(MAIN_DEV,
+				"Received HCI reset complete event OK\n");
+			/*
+			 * Go back to BAUD_IDLE since this was not really
+			 * baud rate change but just a preparation of the chip
+			 * to be ready to receive commands.
+			 */
+			dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_IDLE\n");
+			uart_info->baud_rate_state = BAUD_IDLE;
+		} else {
+			dev_err(MAIN_DEV,
+				"Received HCI reset complete event with "
+				"status 0x%X", status);
+			dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_FAIL\n");
+			uart_info->baud_rate_state = BAUD_FAIL;
+		}
+		wake_up_all(&uart_wait_queue);
+		kfree_skb(skb);
+	} else {
+		/* Just pass data to CG2900 Core */
+		uart_info->chip_dev.c_cb.data_from_chip
+						(&uart_info->chip_dev, skb);
+	}
+}
+
+/**
+ * check_data_len() - Check number of bytes to receive.
+ * @len:	Number of bytes left to receive.
+ */
+static void check_data_len(struct uart_info *uart_info, int len)
+{
+	/* First get number of bytes left in the sk_buffer */
+	register int room = skb_tailroom(uart_info->rx_skb);
+
+	if (!len) {
+		/* No data left to receive. Transmit to CG2900 Core */
+		send_skb_to_core(uart_info, uart_info->rx_skb);
+	} else if (len > room) {
+		dev_err(MAIN_DEV, "Data length is too large (%d > %d)\n",
+			len, room);
+		kfree_skb(uart_info->rx_skb);
+	} else {
+		/*
+		 * "Normal" case. Switch to data receiving state and store
+		 * data length.
+		 */
+		uart_info->rx_state = W4_DATA;
+		uart_info->rx_count = len;
+		return;
+	}
+
+	uart_info->rx_state = W4_PACKET_TYPE;
+	uart_info->rx_skb   = NULL;
+	uart_info->rx_count = 0;
+}
+
+/**
+ * work_restart_sleep() - Cancel pending sleep_work, wake-up driver and
+ * schedule new sleep_work in a work context.
+ * @work:	work which needs to be done.
+ */
+static void work_restart_sleep(struct work_struct *work)
+{
+	struct uart_work_struct *current_work =
+		container_of(work, struct uart_work_struct, work);
+	struct uart_info *uart_info = (struct uart_info *)current_work->data;
+	unsigned long timeout_jiffies = get_sleep_timeout(uart_info);
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+	uart_info->rx_in_progress = false;
+	spin_unlock_bh(&(uart_info->transmission_lock));
+
+	/* Cancel pending sleep work if there is any. */
+	cancel_delayed_work_sync(&uart_info->sleep_work.work);
+
+	wake_up_chip(uart_info);
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+	/*
+	 * If there are no ongoing transfers schedule the sleep work.
+	 */
+	if (!(uart_info->tx_in_progress) && timeout_jiffies)
+		queue_delayed_work(uart_info->wq,
+				&uart_info->sleep_work.work,
+				timeout_jiffies);
+	spin_unlock_bh(&(uart_info->transmission_lock));
+}
+
+/**
+ * cg2900_hu_receive() - Handles received UART data.
+ * @data:	Data received
+ * @count:	Number of bytes received
+ *
+ * The cg2900_hu_receive() function handles received UART data and puts it
+ * together to one complete packet.
+ *
+ * Returns:
+ *   Number of bytes not handled, i.e. 0 = no error.
+ */
+static int cg2900_hu_receive(struct hci_uart *hu,
+					void *data, int count)
+{
+	const u8 *r_ptr;
+	u8 *w_ptr;
+	int len;
+	struct hci_event_hdr	*evt;
+	struct hci_acl_hdr	*acl;
+	union fm_leg_evt_or_irq	*fm;
+	struct gnss_hci_hdr	*gnss;
+	struct uart_info *uart_info = dev_get_drvdata(hu->proto->dev);
+	u8 *tmp;
+
+	r_ptr = (const u8 *)data;
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+	/* Mark that there is an ongoing transfer. */
+	uart_info->rx_in_progress = true;
+	spin_unlock_bh(&(uart_info->transmission_lock));
+
+	/* Cancel pending sleep work if there is any. */
+	cancel_delayed_work(&uart_info->sleep_work.work);
+
+	if (uart_debug)
+		print_hex_dump_bytes(NAME " RX:\t", DUMP_PREFIX_NONE,
+				     data, count);
+
+	/* Continue while there is data left to handle */
+	while (count) {
+		/*
+		 * If we have already received a packet we know how many bytes
+		 * there are left.
+		 */
+		if (!uart_info->rx_count)
+			goto check_h4_header;
+
+		/* First copy received data into the skb_rx */
+		len = min_t(unsigned int, uart_info->rx_count, count);
+		memcpy(skb_put(uart_info->rx_skb, len), r_ptr, len);
+		/* Update counters from the length and step the data pointer */
+		uart_info->rx_count -= len;
+		count -= len;
+		r_ptr += len;
+
+		if (uart_info->rx_count)
+			/*
+			 * More data to receive to current packet. Break and
+			 * wait for next data on the UART.
+			 */
+			break;
+
+		/* Handle the different states */
+		tmp = uart_info->rx_skb->data + CG2900_SKB_RESERVE;
+		switch (uart_info->rx_state) {
+		case W4_DATA:
+			/*
+			 * Whole data packet has been received.
+			 * Transmit it to CG2900 Core.
+			 */
+			send_skb_to_core(uart_info, uart_info->rx_skb);
+
+			uart_info->rx_state = W4_PACKET_TYPE;
+			uart_info->rx_skb = NULL;
+			continue;
+
+		case W4_EVENT_HDR:
+			evt = (struct hci_event_hdr *)tmp;
+			check_data_len(uart_info, evt->plen);
+			/* Header read. Continue with next bytes */
+			continue;
+
+		case W4_ACL_HDR:
+			acl = (struct hci_acl_hdr *)tmp;
+			check_data_len(uart_info, le16_to_cpu(acl->dlen));
+			/* Header read. Continue with next bytes */
+			continue;
+
+		case W4_FM_RADIO_HDR:
+			fm = (union fm_leg_evt_or_irq *)tmp;
+			check_data_len(uart_info, fm->param_length);
+			/* Header read. Continue with next bytes */
+			continue;
+
+		case W4_GNSS_HDR:
+			gnss = (struct gnss_hci_hdr *)tmp;
+			check_data_len(uart_info, le16_to_cpu(gnss->plen));
+			/* Header read. Continue with next bytes */
+			continue;
+
+		default:
+			dev_err(MAIN_DEV,
+				"Bad state indicating memory overwrite "
+				"(0x%X)\n", (u8)(uart_info->rx_state));
+			break;
+		}
+
+check_h4_header:
+		/* Check which H:4 packet this is and update RX states */
+		if (*r_ptr == HCI_BT_EVT_H4_CHANNEL) {
+			uart_info->rx_state = W4_EVENT_HDR;
+			uart_info->rx_count = HCI_BT_EVT_HDR_SIZE;
+		} else if (*r_ptr == HCI_BT_ACL_H4_CHANNEL) {
+			uart_info->rx_state = W4_ACL_HDR;
+			uart_info->rx_count = HCI_BT_ACL_HDR_SIZE;
+		} else if (*r_ptr == HCI_FM_RADIO_H4_CHANNEL) {
+			uart_info->rx_state = W4_FM_RADIO_HDR;
+			uart_info->rx_count = HCI_FM_RADIO_HDR_SIZE;
+		} else if (*r_ptr == HCI_GNSS_H4_CHANNEL) {
+			uart_info->rx_state = W4_GNSS_HDR;
+			uart_info->rx_count = HCI_GNSS_HDR_SIZE;
+		} else {
+			dev_err(MAIN_DEV, "Unknown HCI packet type 0x%X\n",
+				(u8)*r_ptr);
+			r_ptr++;
+			count--;
+			continue;
+		}
+
+		/*
+		 * Allocate packet. We do not yet know the size and therefore
+		 * allocate max size.
+		 */
+		uart_info->rx_skb = alloc_rx_skb(RX_SKB_MAX_SIZE, GFP_ATOMIC);
+		if (!uart_info->rx_skb) {
+			dev_err(MAIN_DEV,
+				"Can't allocate memory for new packet\n");
+			uart_info->rx_state = W4_PACKET_TYPE;
+			uart_info->rx_count = 0;
+
+			spin_lock_bh(&(uart_info->transmission_lock));
+			uart_info->rx_in_progress = false;
+			spin_unlock_bh(&(uart_info->transmission_lock));
+			return 0;
+		}
+
+		/* Write the H:4 header first in the sk_buffer */
+		w_ptr = skb_put(uart_info->rx_skb, 1);
+		*w_ptr = *r_ptr;
+
+		/* First byte (H4 header) read. Goto next byte */
+		r_ptr++;
+		count--;
+	}
+
+	(void)queue_work(uart_info->wq, &uart_info->restart_sleep_work.work);
+
+	return count;
+}
+
+/**
+ * cg2900_hu_open() - Called when UART line discipline changed to N_HCI.
+ * @hu:	Pointer to associated Hci uart structure.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Errors from cg2900_register_trans_driver.
+ */
+static int cg2900_hu_open(struct hci_uart *hu)
+{
+	int err;
+	struct uart_info *uart_info = dev_get_drvdata(hu->proto->dev);
+
+	if (!uart_info)
+		return -EACCES;
+
+	dev_info(MAIN_DEV, "UART opened\n");
+
+	skb_queue_head_init(&uart_info->tx_queue);
+
+	uart_info->hu = hu;
+
+	/* Tell CG2900 Core that UART is connected */
+	err = cg2900_register_trans_driver(&uart_info->chip_dev);
+	if (err)
+		dev_err(MAIN_DEV, "Could not register transport driver (%d)\n",
+			err);
+
+	if (hu->tty->ops->tiocmget && hu->tty->ops->break_ctl)
+		uart_info->sleep_allowed = true;
+	else {
+		dev_err(MAIN_DEV, "Sleep mode not available\n");
+		uart_info->sleep_allowed = false;
+	}
+
+	return err;
+
+}
+
+/**
+ * cg2900_hu_close() - Close UART tty.
+ * @hu:	Pointer to associated hci_uart structure.
+ *
+ * The uart_tty_close() function is called when the line discipline is changed
+ * to something else, the TTY is closed, or the TTY detects a hangup.
+ */
+static int cg2900_hu_close(struct hci_uart *hu)
+{
+	int err;
+	struct uart_info *uart_info = dev_get_drvdata(hu->proto->dev);
+
+
+	BUG_ON(!uart_info);
+	BUG_ON(!uart_info->wq);
+
+	/* Purge any stored sk_buffers */
+	skb_queue_purge(&uart_info->tx_queue);
+	if (uart_info->rx_skb) {
+		kfree_skb(uart_info->rx_skb);
+		uart_info->rx_skb = NULL;
+	}
+
+	dev_info(MAIN_DEV, "UART closed\n");
+	err = create_work_item(uart_info, work_hw_deregistered);
+	if (err)
+		dev_err(MAIN_DEV, "Failed to create work item (%d) "
+			"work_hw_deregistered\n", err);
+
+	uart_info->hu = NULL;
+
+	return 0;
+}
+
+/**
+ * cg2900_hu_dequeue() - Get new skbuff.
+ * @hu: Pointer to associated hci_uart structure.
+ *
+ * The uart_tty_close() function is called when the line discipline is changed
+ * to something else, the TTY is closed, or the TTY detects a hangup.
+ */
+static struct sk_buff *cg2900_hu_dequeue(struct hci_uart *hu)
+{
+	struct sk_buff *skb;
+	struct uart_info *uart_info = dev_get_drvdata(hu->proto->dev);
+	unsigned long timeout_jiffies = get_sleep_timeout(uart_info);
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+
+	skb = skb_dequeue(&uart_info->tx_queue);
+
+	if (!skb)
+		uart_info->tx_in_progress = false;
+
+	/*
+	 * If there are no ongoing transfers schedule the sleep work.
+	 */
+	if (!(uart_info->rx_in_progress) && timeout_jiffies && !skb)
+		queue_delayed_work(uart_info->wq,
+				&uart_info->sleep_work.work,
+				timeout_jiffies);
+
+	spin_unlock_bh(&(uart_info->transmission_lock));
+
+	if (BAUD_SENDING == uart_info->baud_rate_state && !skb)
+		finish_setting_baud_rate(hu);
+	/*
+	 * If it's set baud rate cmd set correct baud state and after
+	 * sending is finished inform the tty driver about the new
+	 * baud rate.
+	 */
+	if ((BAUD_START == uart_info->baud_rate_state) &&
+		skb && (is_set_baud_rate_cmd(skb->data))) {
+		dev_dbg(MAIN_DEV, "UART set baud rate cmd found\n");
+		uart_info->baud_rate_state = BAUD_SENDING;
+	}
+
+	if (uart_debug && skb)
+		print_hex_dump_bytes(NAME " TX:\t", DUMP_PREFIX_NONE,
+				     skb->data, skb->len);
+
+	return skb;
+}
+
+/**
+ * cg2900_hu_flush() - Flush buffers.
+ * @hu: Pointer to associated hci_uart structure.
+ *
+ */
+static int cg2900_hu_flush(struct hci_uart *hu)
+{
+	struct uart_info *uart_info = dev_get_drvdata(hu->proto->dev);
+
+	dev_dbg(MAIN_DEV, "ui %p", uart_info);
+	skb_queue_purge(&uart_info->tx_queue);
+	return 0;
+}
+
+/**
+ * cg2900_uart_probe() - Initialize CG2900 UART resources.
+ * @pdev:	Platform device.
+ *
+ * This function initializes the module and registers to the UART framework.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM for failed alloc or structure creation.
+ *   -ECHILD for failed work queue creation.
+ *   Error codes generated by tty_register_ldisc.
+ */
+static int __devinit cg2900_uart_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	struct uart_info *uart_info;
+	struct hci_uart_proto *p;
+	struct resource *resource;
+
+	pr_debug("cg2900_uart_probe");
+
+	uart_info = kzalloc(sizeof(*uart_info), GFP_KERNEL);
+	if (!uart_info) {
+		pr_err("Couldn't allocate uart_info");
+		return -ENOMEM;
+	}
+
+	uart_info->sleep_state = CHIP_POWERED_DOWN;
+	mutex_init(&(uart_info->sleep_state_lock));
+
+	spin_lock_init(&(uart_info->transmission_lock));
+
+	uart_info->chip_dev.t_cb.open = uart_open;
+	uart_info->chip_dev.t_cb.close = uart_close;
+	uart_info->chip_dev.t_cb.write = uart_write;
+	uart_info->chip_dev.t_cb.set_chip_power = uart_set_chip_power;
+	uart_info->chip_dev.t_cb.chip_startup_finished =
+			uart_chip_startup_finished;
+	uart_info->chip_dev.pdev = pdev;
+	uart_info->chip_dev.dev = &pdev->dev;
+	uart_info->chip_dev.t_data = uart_info;
+	wake_lock_init(&uart_info->wake_lock, WAKE_LOCK_SUSPEND, NAME);
+	uart_info->suspend_blocked = false;
+
+	resource = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
+						"cts_irq");
+	if (!resource) {
+		dev_err(&pdev->dev, "CTS IRQ does not exist\n");
+		err = -EINVAL;
+		goto error_handling_free;
+	}
+	uart_info->cts_irq = resource->start;
+
+	resource = platform_get_resource_byname(pdev, IORESOURCE_IO,
+						"cts_gpio");
+	if (!resource) {
+		dev_err(&pdev->dev, "CTS GPIO does not exist\n");
+		err = -EINVAL;
+		goto error_handling_free;
+	}
+	uart_info->cts_gpio = resource->start;
+
+	/* Init UART TX work queue */
+	uart_info->wq = create_singlethread_workqueue(UART_WQ_NAME);
+	if (!uart_info->wq) {
+		dev_err(MAIN_DEV, "Could not create workqueue\n");
+		err = -ECHILD; /* No child processes */
+		goto error_handling_free;
+	}
+
+	/* Initialize sleep work data */
+	uart_info->sleep_work.data = uart_info;
+	INIT_DELAYED_WORK(&uart_info->sleep_work.work, set_chip_sleep_mode);
+
+	/* Initialize wake-up work data */
+	uart_info->wakeup_work.data = uart_info;
+	INIT_WORK(&uart_info->wakeup_work.work, work_wake_up_chip);
+
+	/* Initialize after_receive work data */
+	uart_info->restart_sleep_work.data = uart_info;
+	INIT_WORK(&uart_info->restart_sleep_work.work, work_restart_sleep);
+
+	uart_info->dev = &pdev->dev;
+
+	p = kzalloc(sizeof(*p), GFP_KERNEL);
+	if (!p) {
+		dev_err(MAIN_DEV, "cg2900_uart_probe: Could not allocate p\n");
+		goto error_handling_wq;
+	}
+
+	p->dev		= uart_info->dev;
+	p->id		= HCI_UART_STE;
+	p->open		= &cg2900_hu_open;
+	p->close	= &cg2900_hu_close;
+	p->recv		= &cg2900_hu_receive;
+	p->dequeue	= &cg2900_hu_dequeue;
+	p->flush	= &cg2900_hu_flush;
+
+	dev_set_drvdata(uart_info->dev, (void *)uart_info);
+
+	err = hci_uart_register_proto(p);
+	if (err) {
+		dev_err(MAIN_DEV, "cg2900_uart_probe: Can not register "
+			"protocol\n");
+		kfree(p);
+		goto error_handling_wq;
+	}
+
+	goto finished;
+
+error_handling_wq:
+	destroy_workqueue(uart_info->wq);
+error_handling_free:
+	kfree(uart_info);
+	uart_info = NULL;
+finished:
+	return err;
+}
+
+/**
+ * cg2900_uart_remove() - Release CG2900 UART resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   Error codes generated by tty_unregister_ldisc.
+ */
+static int __devexit cg2900_uart_remove(struct platform_device *pdev)
+{
+	struct uart_info *uart_info = dev_get_drvdata(&pdev->dev);
+
+	pr_debug("cg2900_uart_remove");
+
+	if (!uart_info)
+		return -ECHILD;
+
+	if (uart_info->hu)
+		hci_uart_unregister_proto(uart_info->hu->proto);
+
+	wake_lock_destroy(&uart_info->wake_lock);
+	destroy_workqueue(uart_info->wq);
+
+	dev_info(MAIN_DEV, "CG2900 UART removed\n");
+	kfree(uart_info);
+	uart_info = NULL;
+	return 0;
+}
+
+static struct platform_driver cg2900_uart_driver = {
+	.driver = {
+		.name	= "cg2900-uart",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_uart_probe,
+	.remove	= __devexit_p(cg2900_uart_remove),
+#ifdef CONFIG_PM
+	.suspend = cg2900_uart_suspend,
+	.resume = cg2900_uart_resume
+#endif
+};
+
+
+/**
+ * cg2900_uart_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_uart_init(void)
+{
+	pr_debug("cg2900_uart_init");
+	return platform_driver_register(&cg2900_uart_driver);
+}
+
+/**
+ * cg2900_uart_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_uart_exit(void)
+{
+	pr_debug("cg2900_uart_exit");
+	platform_driver_unregister(&cg2900_uart_driver);
+}
+
+module_init(cg2900_uart_init);
+module_exit(cg2900_uart_exit);
+
+module_param(uart_default_baud, int, S_IRUGO);
+MODULE_PARM_DESC(uart_default_baud,
+		 "Default UART baud rate, e.g. 115200. If not set 115200 will "
+		 "be used.");
+
+module_param(uart_high_baud, int, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(uart_high_baud,
+		 "High speed UART baud rate, e.g. 4000000.  If not set 3000000 "
+		 "will be used.");
+
+module_param(uart_debug, int, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(uart_debug, "Enable/Disable debug. 0 means Debug disabled.");
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("ST-Ericsson CG2900 UART Driver");
diff --git a/drivers/staging/cg2900/bluetooth/hci_ldisc.c b/drivers/staging/cg2900/bluetooth/hci_ldisc.c
new file mode 100644
index 00000000000..bb480317f6d
--- /dev/null
+++ b/drivers/staging/cg2900/bluetooth/hci_ldisc.c
@@ -0,0 +1,657 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * This file is a staging solution and shall be integrated into
+ * /drivers/bluetooth/hci_ldisc.c.
+ *
+ * Original hci_ldisc.c file:
+ *  Copyright (C) 2000-2001  Qualcomm Incorporated
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2004-2005  Marcel Holtmann <marcel@holtmann.org>
+ */
+
+#include <linux/module.h>
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/fcntl.h>
+#include <linux/interrupt.h>
+#include <linux/ptrace.h>
+#include <linux/poll.h>
+
+#include <linux/slab.h>
+#include <linux/tty.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/signal.h>
+#include <linux/ioctl.h>
+#include <linux/skbuff.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "hci_uart.h"
+
+#define VERSION "2.3"
+
+#define TTY_BREAK_ON		(-1)
+#define TTY_BREAK_OFF		(0)
+
+static int reset;
+
+static struct hci_uart_proto *hup[HCI_UART_MAX_PROTO];
+
+int cg2900_hci_uart_register_proto(struct hci_uart_proto *p)
+{
+	if (p->id >= HCI_UART_MAX_PROTO)
+		return -EINVAL;
+
+	if (hup[p->id])
+		return -EEXIST;
+
+	hup[p->id] = p;
+
+	return 0;
+}
+
+int cg2900_hci_uart_unregister_proto(struct hci_uart_proto *p)
+{
+	if (p->id >= HCI_UART_MAX_PROTO)
+		return -EINVAL;
+
+	if (!hup[p->id])
+		return -EINVAL;
+
+	hup[p->id] = NULL;
+
+	return 0;
+}
+
+static struct hci_uart_proto *hci_uart_get_proto(unsigned int id)
+{
+	if (id >= HCI_UART_MAX_PROTO)
+		return NULL;
+
+	return hup[id];
+}
+
+static inline void hci_uart_tx_complete(struct hci_uart *hu, int pkt_type)
+{
+	struct hci_dev *hdev = hu->hdev;
+
+	if (!hdev)
+		return;
+
+	/* Update HCI stat counters */
+	switch (pkt_type) {
+	case HCI_COMMAND_PKT:
+		hdev->stat.cmd_tx++;
+		break;
+
+	case HCI_ACLDATA_PKT:
+		hdev->stat.acl_tx++;
+		break;
+
+	case HCI_SCODATA_PKT:
+		hdev->stat.sco_tx++;
+		break;
+	}
+}
+
+static inline struct sk_buff *hci_uart_dequeue(struct hci_uart *hu)
+{
+	struct sk_buff *skb = hu->tx_skb;
+
+	if (!skb)
+		skb = hu->proto->dequeue(hu);
+	else
+		hu->tx_skb = NULL;
+
+	return skb;
+}
+
+int cg2900_hci_uart_tx_wakeup(struct hci_uart *hu)
+{
+	struct tty_struct *tty = hu->tty;
+	struct hci_dev *hdev = hu->hdev;
+	struct sk_buff *skb;
+
+	if (test_and_set_bit(HCI_UART_SENDING, &hu->tx_state)) {
+		set_bit(HCI_UART_TX_WAKEUP, &hu->tx_state);
+		return 0;
+	}
+
+	BT_DBG("");
+
+restart:
+	clear_bit(HCI_UART_TX_WAKEUP, &hu->tx_state);
+
+	while ((skb = hci_uart_dequeue(hu))) {
+		int len;
+
+		set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
+		len = tty->ops->write(tty, skb->data, skb->len);
+		if (hdev)
+			hdev->stat.byte_tx += len;
+
+		skb_pull(skb, len);
+		if (skb->len) {
+			hu->tx_skb = skb;
+			break;
+		}
+
+		hci_uart_tx_complete(hu, bt_cb(skb)->pkt_type);
+		kfree_skb(skb);
+	}
+
+	if (test_bit(HCI_UART_TX_WAKEUP, &hu->tx_state))
+		goto restart;
+
+	clear_bit(HCI_UART_SENDING, &hu->tx_state);
+	return 0;
+}
+
+int cg2900_hci_uart_set_break(struct hci_uart *hu, bool break_on)
+{
+	struct tty_struct *tty = hu->tty;
+	int state = TTY_BREAK_OFF;
+
+	if (break_on)
+		state = TTY_BREAK_ON;
+
+	if (tty->ops->break_ctl)
+		return tty->ops->break_ctl(tty, state);
+	else
+		return -EOPNOTSUPP;
+}
+
+void cg2900_hci_uart_flow_ctrl(struct hci_uart *hu, bool flow_on)
+{
+	if (flow_on)
+		tty_unthrottle(hu->tty);
+	else
+		tty_throttle(hu->tty);
+}
+
+int cg2900_hci_uart_set_baudrate(struct hci_uart *hu, int baud)
+{
+	struct ktermios old_termios;
+	struct tty_struct *tty = hu->tty;
+
+	if (!tty->ops->set_termios)
+		return -EOPNOTSUPP;
+
+	mutex_lock(&(tty->termios_mutex));
+	/* Start by storing the old termios. */
+	memcpy(&old_termios, tty->termios, sizeof(old_termios));
+
+	tty_encode_baud_rate(tty, baud, baud);
+
+	/* Finally inform the driver */
+	tty->ops->set_termios(tty, &old_termios);
+
+	mutex_unlock(&(tty->termios_mutex));
+
+	return 0;
+}
+
+int cg2900_hci_uart_tiocmget(struct hci_uart *hu)
+{
+	struct tty_struct *tty = hu->tty;
+
+	if (!tty->ops->tiocmget ||  !hu->fd)
+		return -EOPNOTSUPP;
+
+	return tty->ops->tiocmget(tty);
+}
+
+void cg2900_hci_uart_flush_buffer(struct hci_uart *hu)
+{
+	tty_driver_flush_buffer(hu->tty);
+}
+
+int cg2900_hci_uart_chars_in_buffer(struct hci_uart *hu)
+{
+	return tty_chars_in_buffer(hu->tty);
+}
+
+/* ------- Interface to HCI layer ------ */
+/* Initialize device */
+static int hci_uart_open(struct hci_dev *hdev)
+{
+	BT_DBG("%s %p", hdev->name, hdev);
+
+	/* Nothing to do for UART driver */
+
+	set_bit(HCI_RUNNING, &hdev->flags);
+
+	return 0;
+}
+
+/* Reset device */
+static int hci_uart_flush(struct hci_dev *hdev)
+{
+	struct hci_uart *hu  = (struct hci_uart *) hdev->driver_data;
+	struct tty_struct *tty = hu->tty;
+
+	BT_DBG("hdev %p tty %p", hdev, tty);
+
+	if (hu->tx_skb) {
+		kfree_skb(hu->tx_skb); hu->tx_skb = NULL;
+	}
+
+	/* Flush any pending characters in the driver and discipline. */
+	tty_ldisc_flush(tty);
+	tty_driver_flush_buffer(tty);
+
+	if (test_bit(HCI_UART_PROTO_SET, &hu->flags))
+		hu->proto->flush(hu);
+
+	return 0;
+}
+
+/* Close device */
+static int hci_uart_close(struct hci_dev *hdev)
+{
+	BT_DBG("hdev %p", hdev);
+
+	if (!test_and_clear_bit(HCI_RUNNING, &hdev->flags))
+		return 0;
+
+	hci_uart_flush(hdev);
+	hdev->flush = NULL;
+	return 0;
+}
+
+/* Send frames from HCI layer */
+static int hci_uart_send_frame(struct sk_buff *skb)
+{
+	struct hci_dev* hdev = (struct hci_dev *) skb->dev;
+	struct hci_uart *hu;
+
+	if (!hdev) {
+		BT_ERR("Frame for unknown device (hdev=NULL)");
+		return -ENODEV;
+	}
+
+	if (!test_bit(HCI_RUNNING, &hdev->flags))
+		return -EBUSY;
+
+	hu = (struct hci_uart *) hdev->driver_data;
+
+	BT_DBG("%s: type %d len %d", hdev->name, bt_cb(skb)->pkt_type,
+	       skb->len);
+
+	hu->proto->enqueue(hu, skb);
+
+	hci_uart_tx_wakeup(hu);
+
+	return 0;
+}
+
+static void hci_uart_destruct(struct hci_dev *hdev)
+{
+	if (!hdev)
+		return;
+
+	BT_DBG("%s", hdev->name);
+	kfree(hdev->driver_data);
+}
+
+/* ------ LDISC part ------ */
+/* hci_uart_tty_open
+ *
+ *     Called when line discipline changed to HCI_UART.
+ *
+ * Arguments:
+ *     tty    pointer to tty info structure
+ * Return Value:
+ *     0 if success, otherwise error code
+ */
+static int hci_uart_tty_open(struct tty_struct *tty)
+{
+	struct hci_uart *hu = (void *) tty->disc_data;
+
+	BT_DBG("tty %p", tty);
+
+	/* FIXME: This btw is bogus, nothing requires the old ldisc to clear
+	   the pointer */
+	if (hu)
+		return -EEXIST;
+
+	/* Error if the tty has no write op instead of leaving an exploitable
+	   hole */
+	if (tty->ops->write == NULL)
+		return -EOPNOTSUPP;
+
+	hu = kzalloc(sizeof(struct hci_uart), GFP_KERNEL);
+	if (!hu) {
+		BT_ERR("Can't allocate control structure");
+		return -ENFILE;
+	}
+
+	tty->disc_data = hu;
+	hu->tty = tty;
+	tty->receive_room = 65536;
+
+	spin_lock_init(&hu->rx_lock);
+
+	/* Flush any pending characters in the driver and line discipline. */
+
+	/* FIXME: why is this needed. Note don't use ldisc_ref here as the
+	   open path is before the ldisc is referencable */
+
+	if (tty->ldisc->ops->flush_buffer)
+		tty->ldisc->ops->flush_buffer(tty);
+	tty_driver_flush_buffer(tty);
+
+	return 0;
+}
+
+/* hci_uart_tty_close()
+ *
+ *    Called when the line discipline is changed to something
+ *    else, the tty is closed, or the tty detects a hangup.
+ */
+static void hci_uart_tty_close(struct tty_struct *tty)
+{
+	struct hci_uart *hu = (void *)tty->disc_data;
+
+	BT_DBG("tty %p", tty);
+
+	/* Detach from the tty */
+	tty->disc_data = NULL;
+
+	if (hu) {
+		struct hci_dev *hdev = hu->hdev;
+
+		if (hdev)
+			hci_uart_close(hdev);
+
+		if (test_and_clear_bit(HCI_UART_PROTO_SET, &hu->flags)) {
+			hu->proto->close(hu);
+			if (hdev) {
+				hci_unregister_dev(hdev);
+				hci_free_dev(hdev);
+			}
+		}
+	}
+}
+
+/* hci_uart_tty_wakeup()
+ *
+ *    Callback for transmit wakeup. Called when low level
+ *    device driver can accept more send data.
+ *
+ * Arguments:        tty    pointer to associated tty instance data
+ * Return Value:    None
+ */
+static void hci_uart_tty_wakeup(struct tty_struct *tty)
+{
+	struct hci_uart *hu = (void *)tty->disc_data;
+
+	BT_DBG("");
+
+	if (!hu)
+		return;
+
+	clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
+
+	if (tty != hu->tty)
+		return;
+
+	if (test_bit(HCI_UART_PROTO_SET, &hu->flags))
+		hci_uart_tx_wakeup(hu);
+}
+
+/* hci_uart_tty_receive()
+ *
+ *     Called by tty low level driver when receive data is
+ *     available.
+ *
+ * Arguments:  tty          pointer to tty isntance data
+ *             data         pointer to received data
+ *             flags        pointer to flags for data
+ *             count        count of received data in bytes
+ *
+ * Return Value:    None
+ */
+static void hci_uart_tty_receive(struct tty_struct *tty, const u8 *data,
+				 char *flags, int count)
+{
+	struct hci_uart *hu = (void *)tty->disc_data;
+
+	if (!hu || tty != hu->tty)
+		return;
+
+	if (!test_bit(HCI_UART_PROTO_SET, &hu->flags))
+		return;
+
+	spin_lock(&hu->rx_lock);
+	hu->proto->recv(hu, (void *) data, count);
+	if (hu->hdev)
+		hu->hdev->stat.byte_rx += count;
+	spin_unlock(&hu->rx_lock);
+
+	tty_unthrottle(tty);
+}
+
+static int hci_uart_register_dev(struct hci_uart *hu)
+{
+	struct hci_dev *hdev;
+
+	BT_DBG("");
+
+	/* Initialize and register HCI device */
+	hdev = hci_alloc_dev();
+	if (!hdev) {
+		BT_ERR("Can't allocate HCI device");
+		return -ENOMEM;
+	}
+
+	hu->hdev = hdev;
+
+	hdev->bus = HCI_UART;
+	hdev->driver_data = hu;
+
+	hdev->open  = hci_uart_open;
+	hdev->close = hci_uart_close;
+	hdev->flush = hci_uart_flush;
+	hdev->send  = hci_uart_send_frame;
+	hdev->destruct = hci_uart_destruct;
+
+	hdev->owner = THIS_MODULE;
+
+	if (!reset)
+		set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+
+	if (test_bit(HCI_UART_RAW_DEVICE, &hu->hdev_flags))
+		set_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks);
+
+	if (hci_register_dev(hdev) < 0) {
+		BT_ERR("Can't register HCI device");
+		hci_free_dev(hdev);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int hci_uart_set_proto(struct hci_uart *hu, int id)
+{
+	struct hci_uart_proto *p;
+	int err;
+
+	p = hci_uart_get_proto(id);
+	if (!p)
+		return -EPROTONOSUPPORT;
+
+	hu->proto = p;
+
+	err = p->open(hu);
+	if (err)
+		return err;
+
+	/*
+	 * Protocol might register hdev by itself.
+	 * In that case, there is no need to register it here.
+	 */
+	if (!hu->proto->register_hci_dev)
+		return 0;
+
+	err = hci_uart_register_dev(hu);
+	if (err) {
+		p->close(hu);
+		return err;
+	}
+
+	return 0;
+}
+
+/* hci_uart_tty_ioctl()
+ *
+ *    Process IOCTL system call for the tty device.
+ *
+ * Arguments:
+ *
+ *    tty        pointer to tty instance data
+ *    file       pointer to open file object for device
+ *    cmd        IOCTL command code
+ *    arg        argument for IOCTL call (cmd dependent)
+ *
+ * Return Value:    Command dependent
+ */
+static int hci_uart_tty_ioctl(struct tty_struct *tty, struct file * file,
+					unsigned int cmd, unsigned long arg)
+{
+	struct hci_uart *hu = (void *)tty->disc_data;
+	int err = 0;
+
+	BT_DBG("");
+
+	/* Verify the status of the device */
+	if (!hu)
+		return -EBADF;
+
+	switch (cmd) {
+	case HCIUARTSETPROTO:
+		if (!test_and_set_bit(HCI_UART_PROTO_SET, &hu->flags)) {
+			err = hci_uart_set_proto(hu, arg);
+			if (err) {
+				clear_bit(HCI_UART_PROTO_SET, &hu->flags);
+				return err;
+			}
+			/* Keep file descriptor.*/
+			hu->fd = file;
+		} else
+			return -EBUSY;
+		break;
+
+	case HCIUARTGETPROTO:
+		if (test_bit(HCI_UART_PROTO_SET, &hu->flags))
+			return hu->proto->id;
+		return -EUNATCH;
+
+	case HCIUARTGETDEVICE:
+		if (test_bit(HCI_UART_PROTO_SET, &hu->flags)) {
+			if (hu->hdev)
+				return hu->hdev->id;
+			else
+				return -ENOMSG;
+		}
+		return -EUNATCH;
+
+	case HCIUARTSETFLAGS:
+		if (test_bit(HCI_UART_PROTO_SET, &hu->flags))
+			return -EBUSY;
+		hu->hdev_flags = arg;
+		break;
+
+	case HCIUARTGETFLAGS:
+		return hu->hdev_flags;
+
+	default:
+		err = n_tty_ioctl_helper(tty, file, cmd, arg);
+		break;
+	};
+
+	return err;
+}
+
+/*
+ * We don't provide read/write/poll interface for user space.
+ */
+static ssize_t hci_uart_tty_read(struct tty_struct *tty, struct file *file,
+					unsigned char __user *buf, size_t nr)
+{
+	return 0;
+}
+
+static ssize_t hci_uart_tty_write(struct tty_struct *tty, struct file *file,
+					const unsigned char *data, size_t count)
+{
+	return 0;
+}
+
+static unsigned int hci_uart_tty_poll(struct tty_struct *tty,
+					struct file *filp, poll_table *wait)
+{
+	return 0;
+}
+
+static int __init cg2900_hci_uart_init(void)
+{
+	static struct tty_ldisc_ops hci_uart_ldisc;
+	int err;
+
+	BT_INFO("HCI UART driver ver %s", VERSION);
+
+	/* Register the tty discipline */
+
+	memset(&hci_uart_ldisc, 0, sizeof(hci_uart_ldisc));
+	hci_uart_ldisc.magic		= TTY_LDISC_MAGIC;
+	hci_uart_ldisc.name		= "n_cg2900_hci";
+	hci_uart_ldisc.open		= hci_uart_tty_open;
+	hci_uart_ldisc.close		= hci_uart_tty_close;
+	hci_uart_ldisc.read		= hci_uart_tty_read;
+	hci_uart_ldisc.write		= hci_uart_tty_write;
+	hci_uart_ldisc.ioctl		= hci_uart_tty_ioctl;
+	hci_uart_ldisc.poll		= hci_uart_tty_poll;
+	hci_uart_ldisc.receive_buf	= hci_uart_tty_receive;
+	hci_uart_ldisc.write_wakeup	= hci_uart_tty_wakeup;
+	hci_uart_ldisc.owner		= THIS_MODULE;
+
+	err = tty_register_ldisc(N_CG2900_HCI, &hci_uart_ldisc);
+	if (err) {
+		BT_ERR("HCI line discipline registration failed. (%d)", err);
+		return err;
+	}
+
+	return 0;
+}
+
+static void __exit cg2900_hci_uart_exit(void)
+{
+	int err;
+
+	/* Release tty registration of line discipline */
+	err = tty_unregister_ldisc(N_CG2900_HCI);
+	if (err)
+		BT_ERR("Can't unregister HCI line discipline (%d)", err);
+}
+
+module_init(cg2900_hci_uart_init);
+module_exit(cg2900_hci_uart_exit);
+
+module_param(reset, bool, 0644);
+MODULE_PARM_DESC(reset, "Send HCI reset command on initialization");
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl <par-gunnar.p.hjalmdahl@stericsson.com>");
+MODULE_DESCRIPTION("CG2900 Staging Bluetooth HCI UART driver ver " VERSION);
+MODULE_VERSION(VERSION);
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_LDISC(N_CG2900_HCI);
diff --git a/drivers/staging/cg2900/bluetooth/hci_uart.h b/drivers/staging/cg2900/bluetooth/hci_uart.h
new file mode 100644
index 00000000000..23a69519ccd
--- /dev/null
+++ b/drivers/staging/cg2900/bluetooth/hci_uart.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * This file is a staging solution and shall be integrated into
+ * /drivers/bluetooth/hci_uart.h.
+ *
+ * Original hci_uart.h file:
+ *  Copyright (C) 2000-2001  Qualcomm Incorporated
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2004-2005  Marcel Holtmann <marcel@holtmann.org>
+ */
+
+/*
+ * Staging CG2900 Bluetooth HCI UART. Will be replaced by normal N_HCI when
+ * moved to normal driver folder.
+ */
+#ifndef N_CG2900_HCI
+#define N_CG2900_HCI		23
+#endif /* N_CG2900_HCI */
+
+/* Ioctls */
+#define HCIUARTSETPROTO		_IOW('U', 200, int)
+#define HCIUARTGETPROTO		_IOR('U', 201, int)
+#define HCIUARTGETDEVICE	_IOR('U', 202, int)
+#define HCIUARTSETFLAGS		_IOW('U', 203, int)
+#define HCIUARTGETFLAGS		_IOR('U', 204, int)
+
+/* UART protocols */
+#define HCI_UART_MAX_PROTO	7
+
+#define HCI_UART_H4	0
+#define HCI_UART_BCSP	1
+#define HCI_UART_3WIRE	2
+#define HCI_UART_H4DS	3
+#define HCI_UART_LL	4
+#define HCI_UART_ATH3K	5
+#define HCI_UART_STE	6
+
+#define HCI_UART_RAW_DEVICE	0
+
+/* UART break and flow control parameters */
+#define BREAK_ON		true
+#define BREAK_OFF		false
+#define FLOW_ON			true
+#define FLOW_OFF		false
+
+struct hci_uart;
+
+struct hci_uart_proto {
+	unsigned int id;
+	int (*open)(struct hci_uart *hu);
+	int (*close)(struct hci_uart *hu);
+	int (*flush)(struct hci_uart *hu);
+	int (*recv)(struct hci_uart *hu, void *data, int len);
+	int (*enqueue)(struct hci_uart *hu, struct sk_buff *skb);
+	struct sk_buff *(*dequeue)(struct hci_uart *hu);
+	bool register_hci_dev;
+	struct device *dev;
+};
+
+struct hci_uart {
+	struct tty_struct	*tty;
+	struct hci_dev		*hdev;
+	unsigned long		flags;
+	unsigned long		hdev_flags;
+
+	struct hci_uart_proto	*proto;
+	void			*priv;
+
+	struct sk_buff		*tx_skb;
+	unsigned long		tx_state;
+	spinlock_t		rx_lock;
+
+	struct file		*fd;
+};
+
+/* HCI_UART proto flag bits */
+#define HCI_UART_PROTO_SET	0
+
+/* TX states  */
+#define HCI_UART_SENDING	1
+#define HCI_UART_TX_WAKEUP	2
+
+int cg2900_hci_uart_register_proto(struct hci_uart_proto *p);
+int cg2900_hci_uart_unregister_proto(struct hci_uart_proto *p);
+int cg2900_hci_uart_tx_wakeup(struct hci_uart *hu);
+int cg2900_hci_uart_set_baudrate(struct hci_uart *hu, int baud);
+int cg2900_hci_uart_set_break(struct hci_uart *hu, bool break_on);
+int cg2900_hci_uart_tiocmget(struct hci_uart *hu);
+void cg2900_hci_uart_flush_buffer(struct hci_uart *hu);
+void cg2900_hci_uart_flow_ctrl(struct hci_uart *hu, bool flow_on);
+int cg2900_hci_uart_chars_in_buffer(struct hci_uart *hu);
+
+#define hci_uart_register_proto cg2900_hci_uart_register_proto
+#define hci_uart_unregister_proto cg2900_hci_uart_unregister_proto
+#define hci_uart_tx_wakeup cg2900_hci_uart_tx_wakeup
+#define hci_uart_set_baudrate cg2900_hci_uart_set_baudrate
+#define hci_uart_set_break cg2900_hci_uart_set_break
+#define hci_uart_tiocmget cg2900_hci_uart_tiocmget
+#define hci_uart_flush_buffer cg2900_hci_uart_flush_buffer
+#define hci_uart_flow_ctrl cg2900_hci_uart_flow_ctrl
+#define hci_uart_chars_in_buffer cg2900_hci_uart_chars_in_buffer
diff --git a/drivers/staging/cg2900/board-mop500-cg2900.c b/drivers/staging/cg2900/board-mop500-cg2900.c
new file mode 100644
index 00000000000..cfef96f6472
--- /dev/null
+++ b/drivers/staging/cg2900/board-mop500-cg2900.c
@@ -0,0 +1,207 @@
+/*
+ * Copyright (C) 2008-2011 ST-Ericsson
+ *
+ * Author: Par-Gunnar Hjalmdahl <par-gunnar.p.hjalmdahl@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <asm/mach-types.h>
+
+#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+
+#include <plat/pincfg.h>
+
+#include "cg2900.h"
+#include "devices-cg2900.h"
+#include "pins-db8500.h"
+
+
+#define CG2900_BT_ENABLE_GPIO		170
+#define CG2900_GBF_ENA_RESET_GPIO	171
+#define CG2900_BT_CTS_GPIO		0
+
+enum cg2900_gpio_pull_sleep cg2900_sleep_gpio[21] = {
+	CG2900_NO_PULL,		/* GPIO 0:  PTA_CONFX */
+	CG2900_PULL_DN,		/* GPIO 1:  PTA_STATUS */
+	CG2900_NO_PULL,		/* GPIO 2:  UART_CTSN */
+	CG2900_PULL_UP,		/* GPIO 3:  UART_RTSN */
+	CG2900_PULL_UP,		/* GPIO 4:  UART_TXD */
+	CG2900_NO_PULL,		/* GPIO 5:  UART_RXD */
+	CG2900_PULL_DN,		/* GPIO 6:  IOM_DOUT */
+	CG2900_NO_PULL,		/* GPIO 7:  IOM_FSC */
+	CG2900_NO_PULL,		/* GPIO 8:  IOM_CLK */
+	CG2900_NO_PULL,		/* GPIO 9:  IOM_DIN */
+	CG2900_PULL_DN,		/* GPIO 10: PWR_REQ */
+	CG2900_PULL_DN,		/* GPIO 11: HOST_WAKEUP */
+	CG2900_PULL_DN,		/* GPIO 12: IIS_DOUT */
+	CG2900_NO_PULL,		/* GPIO 13: IIS_WS */
+	CG2900_NO_PULL,		/* GPIO 14: IIS_CLK */
+	CG2900_NO_PULL,		/* GPIO 15: IIS_DIN */
+	CG2900_PULL_DN,		/* GPIO 16: PTA_FREQ */
+	CG2900_PULL_DN,		/* GPIO 17: PTA_RF_ACTIVE */
+	CG2900_NO_PULL,		/* GPIO 18: NotConnected (J6428) */
+	CG2900_NO_PULL,		/* GPIO 19: EXT_DUTY_CYCLE */
+	CG2900_NO_PULL,		/* GPIO 20: EXT_FRM_SYNCH */
+};
+
+static struct platform_device ux500_cg2900_device = {
+	.name = "cg2900",
+};
+
+static struct platform_device ux500_cg2900_chip_device = {
+	.name = "cg2900-chip",
+	.dev = {
+		.parent = &ux500_cg2900_device.dev,
+	},
+};
+
+static struct platform_device ux500_stlc2690_chip_device = {
+	.name = "stlc2690-chip",
+	.dev = {
+		.parent = &ux500_cg2900_device.dev,
+	},
+};
+
+static struct cg2900_platform_data cg2900_test_platform_data = {
+	.bus = HCI_VIRTUAL,
+	.gpio_sleep = cg2900_sleep_gpio,
+};
+
+static struct platform_device ux500_cg2900_test_device = {
+	.name = "cg2900-test",
+	.dev = {
+		.parent = &ux500_cg2900_device.dev,
+		.platform_data = &cg2900_test_platform_data,
+	},
+};
+
+static struct resource cg2900_uart_resources[] = {
+	{
+		.start = CG2900_GBF_ENA_RESET_GPIO,
+		.end = CG2900_GBF_ENA_RESET_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "gbf_ena_reset",
+	},
+	{
+		.start = CG2900_BT_ENABLE_GPIO,
+		.end = CG2900_BT_ENABLE_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "bt_enable",
+	},
+	{
+		.start = CG2900_BT_CTS_GPIO,
+		.end = CG2900_BT_CTS_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "cts_gpio",
+	},
+	{
+		.start = NOMADIK_GPIO_TO_IRQ(CG2900_BT_CTS_GPIO),
+		.end = NOMADIK_GPIO_TO_IRQ(CG2900_BT_CTS_GPIO),
+		.flags = IORESOURCE_IRQ,
+		.name = "cts_irq",
+	},
+};
+
+static pin_cfg_t cg2900_uart_enabled[] = {
+	GPIO0_U0_CTSn   | PIN_INPUT_PULLUP,
+	GPIO1_U0_RTSn   | PIN_OUTPUT_HIGH,
+	GPIO2_U0_RXD    | PIN_INPUT_PULLUP,
+	GPIO3_U0_TXD    | PIN_OUTPUT_HIGH
+};
+
+static pin_cfg_t cg2900_uart_disabled[] = {
+	GPIO0_GPIO   | PIN_INPUT_PULLUP,	/* CTS pull up. */
+	GPIO1_GPIO   | PIN_OUTPUT_HIGH,		/* RTS high-flow off. */
+	GPIO2_GPIO   | PIN_INPUT_PULLUP,	/* RX pull down. */
+	GPIO3_GPIO   | PIN_OUTPUT_LOW		/* TX low - break on. */
+};
+
+static struct cg2900_platform_data cg2900_uart_platform_data = {
+	.bus = HCI_UART,
+	.gpio_sleep = cg2900_sleep_gpio,
+	.uart = {
+		.n_uart_gpios = 4,
+		.uart_enabled = cg2900_uart_enabled,
+		.uart_disabled = cg2900_uart_disabled,
+	},
+	.regulator_id =	"vdd",
+};
+
+static struct platform_device ux500_cg2900_uart_device = {
+	.name = "cg2900-uart",
+	.dev = {
+		.platform_data = &cg2900_uart_platform_data,
+		.parent = &ux500_cg2900_device.dev,
+	},
+	.num_resources = ARRAY_SIZE(cg2900_uart_resources),
+	.resource = cg2900_uart_resources,
+};
+
+static bool mach_supported(void)
+{
+	if (machine_is_u8500() ||
+	    machine_is_u5500() ||
+	    machine_is_hrefv60() ||
+	    machine_is_nomadik() ||
+	    machine_is_snowball())
+		return true;
+
+	return false;
+}
+
+static int __init board_cg2900_init(void)
+{
+	int err;
+
+	if (!mach_supported())
+		return 0;
+
+	dcg2900_init_platdata(&cg2900_test_platform_data);
+	dcg2900_init_platdata(&cg2900_uart_platform_data);
+
+	err = platform_device_register(&ux500_cg2900_device);
+	if (err)
+		return err;
+	err = platform_device_register(&ux500_cg2900_uart_device);
+	if (err)
+		return err;
+	err = platform_device_register(&ux500_cg2900_test_device);
+	if (err)
+		return err;
+	err = platform_device_register(&ux500_cg2900_chip_device);
+	if (err)
+		return err;
+	err = platform_device_register(&ux500_stlc2690_chip_device);
+	if (err)
+		return err;
+
+	dev_info(&ux500_cg2900_device.dev, "CG2900 initialized\n");
+	return 0;
+}
+
+static void __exit board_cg2900_exit(void)
+{
+	if (!mach_supported())
+		return;
+
+	platform_device_unregister(&ux500_stlc2690_chip_device);
+	platform_device_unregister(&ux500_cg2900_chip_device);
+	platform_device_unregister(&ux500_cg2900_test_device);
+	platform_device_unregister(&ux500_cg2900_uart_device);
+	platform_device_unregister(&ux500_cg2900_device);
+
+	dev_info(&ux500_cg2900_device.dev, "CG2900 removed\n");
+}
+
+module_init(board_cg2900_init);
+module_exit(board_cg2900_exit);
diff --git a/drivers/staging/cg2900/devices-cg2900.c b/drivers/staging/cg2900/devices-cg2900.c
new file mode 100644
index 00000000000..3c9f8e3e16f
--- /dev/null
+++ b/drivers/staging/cg2900/devices-cg2900.c
@@ -0,0 +1,374 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Board specific device support for the Linux Bluetooth HCI H:4 Driver
+ * for ST-Ericsson connectivity controller.
+ */
+#define NAME			"devices-cg2900"
+#define pr_fmt(fmt)		NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <asm-generic/errno-base.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/ioport.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/spinlock.h>
+#include <linux/string.h>
+#include <linux/types.h>
+#include <plat/pincfg.h>
+#include <asm/mach-types.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/regulator/consumer.h>
+
+#include "cg2900.h"
+#include "devices-cg2900.h"
+
+#define BT_VS_POWER_SWITCH_OFF		0xFD40
+
+#define H4_HEADER_LENGTH		0x01
+#define BT_HEADER_LENGTH		0x03
+
+#define STLC2690_HCI_REV		0x0600
+#define CG2900_PG1_HCI_REV		0x0101
+#define CG2900_PG2_HCI_REV		0x0200
+#define CG2900_PG1_SPECIAL_HCI_REV	0x0700
+
+#define CHIP_INITIAL_LOW_TIMEOUT	20 /* ms */
+
+struct vs_power_sw_off_cmd {
+	__le16	op_code;
+	u8	len;
+	u8	gpio_0_7_pull_up;
+	u8	gpio_8_15_pull_up;
+	u8	gpio_16_20_pull_up;
+	u8	gpio_0_7_pull_down;
+	u8	gpio_8_15_pull_down;
+	u8	gpio_16_20_pull_down;
+} __packed;
+
+struct dcg2900_info {
+	int	gbf_gpio;
+	int	bt_gpio;
+	bool	sleep_gpio_set;
+	u8	gpio_0_7_pull_up;
+	u8	gpio_8_15_pull_up;
+	u8	gpio_16_20_pull_up;
+	u8	gpio_0_7_pull_down;
+	u8	gpio_8_15_pull_down;
+	u8	gpio_16_20_pull_down;
+	spinlock_t	pdb_toggle_lock;
+	struct regulator	*regulator_wlan;
+};
+
+static void dcg2900_enable_chip(struct cg2900_chip_dev *dev)
+{
+	struct dcg2900_info *info = dev->b_data;
+
+	if (info->gbf_gpio == -1)
+		return;
+
+	/*
+	 * Due to a bug in CG2900 we cannot just set GPIO high to enable
+	 * the chip. We must wait more than 20 msecs before enbling the
+	 * chip.
+	 * - Set PDB to low.
+	 * - Wait for 20 msecs
+	 * - Set PDB to high.
+	 */
+	gpio_set_value(info->gbf_gpio, 0);
+	schedule_timeout_uninterruptible(msecs_to_jiffies(
+				CHIP_INITIAL_LOW_TIMEOUT));
+	gpio_set_value(info->gbf_gpio, 1);
+}
+
+static void dcg2900_disable_chip(struct cg2900_chip_dev *dev)
+{
+	struct dcg2900_info *info = dev->b_data;
+
+	if (info->gbf_gpio != -1)
+		gpio_set_value(info->gbf_gpio, 0);
+}
+
+static struct sk_buff *dcg2900_get_power_switch_off_cmd
+				(struct cg2900_chip_dev *dev, u16 *op_code)
+{
+	struct sk_buff *skb;
+	struct vs_power_sw_off_cmd *cmd;
+	struct dcg2900_info *info;
+	int i;
+
+	/* If connected chip does not support the command return NULL */
+	if (CG2900_PG1_SPECIAL_HCI_REV != dev->chip.hci_revision &&
+	    CG2900_PG1_HCI_REV != dev->chip.hci_revision &&
+	    CG2900_PG2_HCI_REV != dev->chip.hci_revision)
+		return NULL;
+
+	dev_dbg(dev->dev, "Generating PowerSwitchOff command\n");
+
+	info = dev->b_data;
+
+	skb = alloc_skb(sizeof(*cmd) + H4_HEADER_LENGTH, GFP_KERNEL);
+	if (!skb) {
+		dev_err(dev->dev, "Could not allocate skb\n");
+		return NULL;
+	}
+
+	skb_reserve(skb, H4_HEADER_LENGTH);
+	cmd = (struct vs_power_sw_off_cmd *)skb_put(skb, sizeof(*cmd));
+	cmd->op_code = cpu_to_le16(BT_VS_POWER_SWITCH_OFF);
+	cmd->len = sizeof(*cmd) - BT_HEADER_LENGTH;
+	/*
+	 * Enter system specific GPIO settings here:
+	 * Section data[3-5] is GPIO pull-up selection
+	 * Section data[6-8] is GPIO pull-down selection
+	 * Each section is a bitfield where
+	 * - byte 0 bit 0 is GPIO 0
+	 * - byte 0 bit 1 is GPIO 1
+	 * - up to
+	 * - byte 2 bit 4 which is GPIO 20
+	 * where each bit means:
+	 * - 0: No pull-up / no pull-down
+	 * - 1: Pull-up / pull-down
+	 * All GPIOs are set as input.
+	 */
+	if (!info->sleep_gpio_set) {
+		struct cg2900_platform_data *pf_data;
+
+		pf_data = dev_get_platdata(dev->dev);
+		for (i = 0; i < 8; i++) {
+			if (pf_data->gpio_sleep[i] == CG2900_PULL_UP)
+				info->gpio_0_7_pull_up |= (1 << i);
+			else if (pf_data->gpio_sleep[i] == CG2900_PULL_DN)
+				info->gpio_0_7_pull_down |= (1 << i);
+		}
+		for (i = 8; i < 16; i++) {
+			if (pf_data->gpio_sleep[i] == CG2900_PULL_UP)
+				info->gpio_8_15_pull_up |= (1 << (i - 8));
+			else if (pf_data->gpio_sleep[i] == CG2900_PULL_DN)
+				info->gpio_8_15_pull_down |= (1 << (i - 8));
+		}
+		for (i = 16; i < 21; i++) {
+			if (pf_data->gpio_sleep[i] == CG2900_PULL_UP)
+				info->gpio_16_20_pull_up |= (1 << (i - 16));
+			else if (pf_data->gpio_sleep[i] == CG2900_PULL_DN)
+				info->gpio_16_20_pull_down |= (1 << (i - 16));
+		}
+		info->sleep_gpio_set = true;
+	}
+	cmd->gpio_0_7_pull_up = info->gpio_0_7_pull_up;
+	cmd->gpio_8_15_pull_up = info->gpio_8_15_pull_up;
+	cmd->gpio_16_20_pull_up = info->gpio_16_20_pull_up;
+	cmd->gpio_0_7_pull_down = info->gpio_0_7_pull_down;
+	cmd->gpio_8_15_pull_down = info->gpio_8_15_pull_down;
+	cmd->gpio_16_20_pull_down = info->gpio_16_20_pull_down;
+
+
+	if (op_code)
+		*op_code = BT_VS_POWER_SWITCH_OFF;
+
+	return skb;
+}
+
+static int dcg2900_init(struct cg2900_chip_dev *dev)
+{
+	int err = 0;
+	struct dcg2900_info *info;
+	struct resource *resource;
+	const char *gbf_name;
+	const char *bt_name = NULL;
+	struct cg2900_platform_data *pdata = dev_get_platdata(dev->dev);
+
+	/* First retrieve and save the resources */
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(dev->dev, "Could not allocate dcg2900_info\n");
+		return -ENOMEM;
+	}
+
+	spin_lock_init(&info->pdb_toggle_lock);
+
+	if (!dev->pdev->num_resources) {
+		dev_dbg(dev->dev, "No resources available\n");
+		info->gbf_gpio = -1;
+		info->bt_gpio = -1;
+		goto finished;
+	}
+
+	resource = platform_get_resource_byname(dev->pdev, IORESOURCE_IO,
+						"gbf_ena_reset");
+	if (!resource) {
+		dev_err(dev->dev, "GBF GPIO does not exist\n");
+		err = -EINVAL;
+		goto err_handling;
+	}
+	info->gbf_gpio = resource->start;
+	gbf_name = resource->name;
+
+	resource = platform_get_resource_byname(dev->pdev, IORESOURCE_IO,
+						"bt_enable");
+	/* BT Enable GPIO may not exist */
+	if (resource) {
+		info->bt_gpio = resource->start;
+		bt_name = resource->name;
+	}
+
+	/* Now setup the GPIOs */
+	err = gpio_request(info->gbf_gpio, gbf_name);
+	if (err < 0) {
+		dev_err(dev->dev, "gpio_request failed with err: %d\n", err);
+		goto err_handling;
+	}
+
+	err = gpio_direction_output(info->gbf_gpio, 0);
+	if (err < 0) {
+		dev_err(dev->dev, "gpio_direction_output failed with err: %d\n",
+			err);
+		goto err_handling_free_gpio_gbf;
+	}
+
+	if (!bt_name) {
+		info->bt_gpio = -1;
+		goto finished;
+	}
+
+	err = gpio_request(info->bt_gpio, bt_name);
+	if (err < 0) {
+		dev_err(dev->dev, "gpio_request failed with err: %d\n", err);
+		goto err_handling_free_gpio_gbf;
+	}
+
+	err = gpio_direction_output(info->bt_gpio, 1);
+	if (err < 0) {
+		dev_err(dev->dev, "gpio_direction_output failed with err: %d\n",
+			err);
+		goto err_handling_free_gpio_bt;
+	}
+
+	/*
+	 * Enable the power on snowball
+	 */
+	if (machine_is_snowball()) {
+		/* Take the regulator */
+		if (pdata->regulator_id) {
+			info->regulator_wlan = regulator_get(dev->dev,
+					pdata->regulator_id);
+			if (IS_ERR(info->regulator_wlan)) {
+				err = PTR_ERR(info->regulator_wlan);
+				dev_warn(dev->dev,
+					"%s: Failed to get regulator '%s'\n",
+					__func__, pdata->regulator_id);
+				info->regulator_wlan = NULL;
+				goto err_handling_free_gpio_bt;
+			}
+			/* Enable it also */
+			err = regulator_enable(info->regulator_wlan);
+			if (err < 0) {
+				dev_warn(dev->dev, "%s: regulator_enable failed\n",
+						__func__);
+				goto err_handling_put_reg;
+			}
+		} else {
+			dev_warn(dev->dev, "%s: no regulator defined for snowball.\n",
+					__func__);
+		}
+	}
+
+finished:
+	dev->b_data = info;
+	return 0;
+err_handling_put_reg:
+	regulator_put(info->regulator_wlan);
+err_handling_free_gpio_bt:
+	gpio_free(info->bt_gpio);
+err_handling_free_gpio_gbf:
+	gpio_free(info->gbf_gpio);
+err_handling:
+	kfree(info);
+	return err;
+}
+
+static void dcg2900_exit(struct cg2900_chip_dev *dev)
+{
+	struct dcg2900_info *info = dev->b_data;
+
+	if (machine_is_snowball()) {
+		/* Turn off power if we have any */
+		if (info->regulator_wlan) {
+			regulator_disable(info->regulator_wlan);
+			regulator_put(info->regulator_wlan);
+		}
+	}
+
+	dcg2900_disable_chip(dev);
+	if (info->bt_gpio != -1)
+		gpio_free(info->bt_gpio);
+	if (info->gbf_gpio != -1)
+		gpio_free(info->gbf_gpio);
+	kfree(info);
+	dev->b_data = NULL;
+}
+
+static int dcg2900_disable_uart(struct cg2900_chip_dev *dev)
+{
+	int err;
+	struct cg2900_platform_data *pdata = dev_get_platdata(dev->dev);
+
+	/*
+	 * Without this delay we get interrupt on CTS immediately
+	 * due to some turbulences on this line.
+	 */
+	mdelay(4);
+
+	/* Disable UART functions. */
+	err = nmk_config_pins(pdata->uart.uart_disabled,
+			      pdata->uart.n_uart_gpios);
+	if (err)
+		goto error;
+
+	return 0;
+
+error:
+	(void)nmk_config_pins(pdata->uart.uart_enabled,
+			      pdata->uart.n_uart_gpios);
+	dev_err(dev->dev, "Cannot set interrupt (%d)\n", err);
+	return err;
+}
+
+static int dcg2900_enable_uart(struct cg2900_chip_dev *dev)
+{
+	int err;
+	struct cg2900_platform_data *pdata = dev_get_platdata(dev->dev);
+
+	/* Restore UART settings. */
+	err = nmk_config_pins(pdata->uart.uart_enabled,
+			      pdata->uart.n_uart_gpios);
+	if (err)
+		dev_err(dev->dev, "Unable to enable UART (%d)\n", err);
+
+	return err;
+}
+
+void dcg2900_init_platdata(struct cg2900_platform_data *data)
+{
+	data->init = dcg2900_init;
+	data->exit = dcg2900_exit;
+	data->enable_chip = dcg2900_enable_chip;
+	data->disable_chip = dcg2900_disable_chip;
+	data->get_power_switch_off_cmd = dcg2900_get_power_switch_off_cmd;
+
+	data->uart.enable_uart = dcg2900_enable_uart;
+	data->uart.disable_uart = dcg2900_disable_uart;
+}
diff --git a/drivers/staging/cg2900/devices-cg2900.h b/drivers/staging/cg2900/devices-cg2900.h
new file mode 100644
index 00000000000..e365a4fae1e
--- /dev/null
+++ b/drivers/staging/cg2900/devices-cg2900.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Par-Gunnar Hjalmdahl <par-gunnar.p.hjalmdahl@stericsson.com>
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __DEVICES_CG2900_H
+#define __DEVICES_CG2900_H
+
+#include "cg2900.h"
+
+/**
+ * enum cg2900_gpio_pull_sleep - GPIO pull setting in sleep.
+ * @CG2900_NO_PULL:	Normal input in sleep (no pull up or down).
+ * @CG2900_PULL_UP:	Pull up in sleep.
+ * @CG2900_PULL_DN:	Pull down in sleep.
+ */
+enum cg2900_gpio_pull_sleep {
+	CG2900_NO_PULL,
+	CG2900_PULL_UP,
+	CG2900_PULL_DN
+};
+
+/**
+ * dcg2900_init_platdata() - Initializes platform data with callback functions.
+ * @data:	Platform data.
+ */
+extern void dcg2900_init_platdata(struct cg2900_platform_data *data);
+
+#endif /* __DEVICES_CG2900_H */
diff --git a/drivers/staging/cg2900/include/cg2900.h b/drivers/staging/cg2900/include/cg2900.h
new file mode 100644
index 00000000000..bd165c0048b
--- /dev/null
+++ b/drivers/staging/cg2900/include/cg2900.h
@@ -0,0 +1,280 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 connectivity
+ * controller.
+ */
+
+#ifndef _CG2900_H_
+#define _CG2900_H_
+
+#include <linux/types.h>
+
+/* Perform reset. No parameters used */
+#define CG2900_CHAR_DEV_IOCTL_RESET		_IOW('U', 210, int)
+/* Check for reset */
+#define CG2900_CHAR_DEV_IOCTL_CHECK4RESET	_IOR('U', 212, int)
+/* Retrieve revision info */
+#define CG2900_CHAR_DEV_IOCTL_GET_REVISION	_IOR('U', 213, \
+						     struct cg2900_rev_data)
+
+#define CG2900_CHAR_DEV_IOCTL_EVENT_IDLE	0
+#define CG2900_CHAR_DEV_IOCTL_EVENT_RESET	1
+
+/**
+ * struct cg2900_rev_data - Contains revision data for the local controller.
+ * @revision:		Revision of the controller, e.g. to indicate that it is
+ *			a CG2900 controller.
+ * @sub_version:	Subversion of the controller, e.g. to indicate a certain
+ *			tape-out of the controller.
+ *
+ * The values to match retrieved values to each controller may be retrieved from
+ * the manufacturer.
+ */
+struct cg2900_rev_data {
+	int revision;
+	int sub_version;
+};
+
+#ifdef __KERNEL__
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/skbuff.h>
+
+/* Temporary solution while in staging directory */
+#include "cg2900_hci.h"
+
+/**
+ * struct cg2900_chip_rev_info - Chip info structure.
+ * @manufacturer:	Chip manufacturer.
+ * @hci_version:	Bluetooth version supported over HCI.
+ * @hci_revision:	Chip revision, i.e. which chip is this.
+ * @lmp_pal_version:	Bluetooth version supported over air.
+ * @hci_sub_version:	Chip sub-version, i.e. which tape-out is this.
+ *
+ * Note that these values match the Bluetooth Assigned Numbers,
+ * see http://www.bluetooth.org/
+ */
+struct cg2900_chip_rev_info {
+	u16	manufacturer;
+	u8	hci_version;
+	u16	hci_revision;
+	u8	lmp_pal_version;
+	u16	hci_sub_version;
+};
+
+struct cg2900_chip_dev;
+
+/**
+ * struct cg2900_id_callbacks - Chip handler identification callbacks.
+ * @check_chip_support:	Called when chip is connected. If chip is supported by
+ *			driver, return true and fill in @callbacks in @dev.
+ *
+ * Note that the callback may be NULL. It must always be NULL checked before
+ * calling.
+ */
+struct cg2900_id_callbacks {
+	bool (*check_chip_support)(struct cg2900_chip_dev *dev);
+};
+
+/**
+ * struct cg2900_chip_callbacks - Callback functions registered by chip handler.
+ * @data_from_chip:	Called when data shall be transmitted to user.
+ * @chip_removed:	Called when chip is removed.
+ *
+ * Note that some callbacks may be NULL. They must always be NULL checked before
+ * calling.
+ */
+struct cg2900_chip_callbacks {
+	void (*data_from_chip)(struct cg2900_chip_dev *dev,
+			       struct sk_buff *skb);
+	void (*chip_removed)(struct cg2900_chip_dev *dev);
+};
+
+/**
+ * struct cg2900_trans_callbacks - Callback functions registered by transport.
+ * @open:		CG2900 Core needs a transport.
+ * @close:		CG2900 Core does not need a transport.
+ * @write:		CG2900 Core transmits to the chip.
+ * @set_chip_power:	CG2900 Core enables or disables the chip.
+ * @chip_startup_finished:	CG2900 Chip startup finished notification.
+ *
+ * Note that some callbacks may be NULL. They must always be NULL checked before
+ * calling.
+ */
+struct cg2900_trans_callbacks {
+	int (*open)(struct cg2900_chip_dev *dev);
+	int (*close)(struct cg2900_chip_dev *dev);
+	int (*write)(struct cg2900_chip_dev *dev, struct sk_buff *skb);
+	void (*set_chip_power)(struct cg2900_chip_dev *dev, bool chip_on);
+	void (*chip_startup_finished)(struct cg2900_chip_dev *dev);
+};
+
+/**
+ * struct cg2900_chip_dev - Chip handler info structure.
+ * @dev:	Device associated with this chip.
+ * @pdev:	Platform device associated with this chip.
+ * @chip:	Chip info such as manufacturer.
+ * @c_cb:	Callback structure for the chip handler.
+ * @t_cb:	Callback structure for the transport.
+ * @c_data:	Arbitrary data set by chip handler.
+ * @t_data:	Arbitrary data set by transport.
+ * @b_data:	Arbitrary data set by board handler.
+ * @prv_data:	Arbitrary data set by CG2900 Core.
+ */
+struct cg2900_chip_dev {
+	struct device			*dev;
+	struct platform_device		*pdev;
+	struct cg2900_chip_rev_info	chip;
+	struct cg2900_chip_callbacks	c_cb;
+	struct cg2900_trans_callbacks	t_cb;
+	void				*c_data;
+	void				*t_data;
+	void				*b_data;
+	void				*prv_data;
+};
+
+/**
+ * struct cg2900_platform_data - Contains platform data for CG2900.
+ * @init:		Callback called upon system start.
+ * @exit:		Callback called upon system shutdown.
+ * @enable_chip:	Callback called for enabling CG2900 chip.
+ * @disable_chip:	Callback called for disabling CG2900 chip.
+ * @get_power_switch_off_cmd:	Callback called to retrieve
+ *				HCI VS_Power_Switch_Off command (command
+ *				HCI requires platform specific GPIO data).
+ * @regulator_id:	Id of the regulator that powers on the chip
+ * @bus:		Transport used, see @include/net/bluetooth/hci.h.
+ * @gpio_sleep:		Array of GPIO sleep settings.
+ * @enable_uart:	Callback called when switching from UART GPIO to
+ *			UART HW.
+ * @disable_uart:	Callback called when switching from UART HW to
+ *			UART GPIO.
+ * @n_uart_gpios:	Number of UART GPIOs.
+ * @uart_enabled:	Array of size @n_uart_gpios with GPIO setting for
+ *			enabling UART HW (switching from GPIO mode).
+ * @uart_disabled:	Array of size @n_uart_gpios with GPIO setting for
+ *			disabling UART HW (switching to GPIO mode).
+ * @uart:		Platform data structure for UART transport.
+ *
+ * Any callback may be NULL if not needed.
+ */
+struct cg2900_platform_data {
+	int (*init)(struct cg2900_chip_dev *dev);
+	void (*exit)(struct cg2900_chip_dev *dev);
+	void (*enable_chip)(struct cg2900_chip_dev *dev);
+	void (*disable_chip)(struct cg2900_chip_dev *dev);
+	struct sk_buff* (*get_power_switch_off_cmd)(struct cg2900_chip_dev *dev,
+						    u16 *op_code);
+
+	char *regulator_id;
+	__u8 bus;
+	enum cg2900_gpio_pull_sleep *gpio_sleep;
+
+	struct {
+		int (*enable_uart)(struct cg2900_chip_dev *dev);
+		int (*disable_uart)(struct cg2900_chip_dev *dev);
+		int n_uart_gpios;
+		unsigned long *uart_enabled;
+		unsigned long *uart_disabled;
+	} uart;
+};
+
+/**
+ * struct cg2900_user_data - Contains platform data for CG2900 user.
+ * @dev:		Current device. Set by CG2900 user upon probe.
+ * @opened:		True if channel is opened.
+ * @user_data:		Data set and used by CG2900 user.
+ * @private_data:	Data set and used by CG2900 driver.
+ * @h4_channel:		H4 channel. Set by CG2900 driver.
+ * @is_audio:		True if this channel is an audio channel. Set by CG2900
+ *			driver.
+ * @chip_independent:	True if this channel does not require chip to be
+ *			powered. Set by CG2900 driver.
+ * @bt_bus:		Transport used, see @include/net/bluetooth/hci.h.
+ * @char_dev_name:	Name to be used for character device.
+ * @channel_data:	Input data specific to current device.
+ * @open:		Open device channel. Set by CG2900 driver.
+ * @close:		Close device channel. Set by CG2900 driver.
+ * @reset:		Reset connectivity controller. Set by CG2900 driver.
+ * @alloc_skb:		Alloc sk_buffer. Set by CG2900 driver.
+ * @write:		Write to device channel. Set by CG2900 driver.
+ * @get_local_revision:	Get revision data of conncected chip. Set by CG2900
+ *			driver.
+ * @read_cb:		Callback function called when data is received on the
+ *			device channel. Set by CG2900 user. Mandatory.
+ * @reset_cb:		Callback function called when the connectivity
+ *			controller has been reset. Set by CG2900 user.
+ *
+ * Any callback may be NULL if not needed.
+ */
+struct cg2900_user_data {
+	struct device *dev;
+	bool opened;
+
+	void *user_data;
+	void *private_data;
+
+	int	h4_channel;
+	bool	is_audio;
+	bool	chip_independent;
+
+	union {
+		__u8 bt_bus;
+		char *char_dev_name;
+	} channel_data;
+
+	int (*open)(struct cg2900_user_data *user_data);
+	void (*close)(struct cg2900_user_data *user_data);
+	int (*reset)(struct cg2900_user_data *user_data);
+	struct sk_buff * (*alloc_skb)(unsigned int size, gfp_t priority);
+	int (*write)(struct cg2900_user_data *user_data, struct sk_buff *skb);
+	bool (*get_local_revision)(struct cg2900_user_data *user_data,
+				   struct cg2900_rev_data *rev_data);
+
+	void (*read_cb)(struct cg2900_user_data *user_data,
+			struct sk_buff *skb);
+	void (*reset_cb)(struct cg2900_user_data *user_data);
+};
+
+static inline void *cg2900_get_usr(struct cg2900_user_data *dev)
+{
+	if (dev)
+		return dev->user_data;
+	return NULL;
+}
+
+static inline void cg2900_set_usr(struct cg2900_user_data *dev, void *data)
+{
+	if (dev)
+		dev->user_data = data;
+}
+
+static inline void *cg2900_get_prv(struct cg2900_user_data *dev)
+{
+	if (dev)
+		return dev->private_data;
+	return NULL;
+}
+
+static inline void cg2900_set_prv(struct cg2900_user_data *dev, void *data)
+{
+	if (dev)
+		dev->private_data = data;
+}
+
+extern int cg2900_register_chip_driver(struct cg2900_id_callbacks *cb);
+extern void cg2900_deregister_chip_driver(struct cg2900_id_callbacks *cb);
+extern int cg2900_register_trans_driver(struct cg2900_chip_dev *dev);
+extern int cg2900_deregister_trans_driver(struct cg2900_chip_dev *dev);
+extern unsigned long cg2900_get_sleep_timeout(void);
+
+#endif /* __KERNEL__ */
+#endif /* _CG2900_H_ */
diff --git a/drivers/staging/cg2900/include/cg2900_audio.h b/drivers/staging/cg2900/include/cg2900_audio.h
new file mode 100644
index 00000000000..ff0f053fa53
--- /dev/null
+++ b/drivers/staging/cg2900/include/cg2900_audio.h
@@ -0,0 +1,473 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth Audio Driver for ST-Ericsson controller.
+ */
+
+#ifndef _CG2900_AUDIO_H_
+#define _CG2900_AUDIO_H_
+
+#include <linux/types.h>
+
+/*
+ * Digital Audio Interface configuration types
+ */
+
+/** CG2900_A2DP_MAX_AVDTP_HDR_LEN - Max length of a AVDTP header.
+ * Max length of a AVDTP header for an A2DP packet.
+ */
+#define CG2900_A2DP_MAX_AVDTP_HDR_LEN		25
+
+/*
+ * Op codes used when writing commands to the audio interface from user space
+ * using the char device.
+ */
+#define CG2900_OPCODE_SET_DAI_CONF		0x01
+#define CG2900_OPCODE_GET_DAI_CONF		0x02
+#define CG2900_OPCODE_CONFIGURE_ENDPOINT	0x03
+#define CG2900_OPCODE_START_STREAM		0x04
+#define CG2900_OPCODE_STOP_STREAM		0x05
+
+/**
+ * enum cg2900_dai_dir - Contains the DAI port directions alternatives.
+ * @DAI_DIR_B_RX_A_TX: Port B as Rx and port A as Tx.
+ * @DAI_DIR_B_TX_A_RX: Port B as Tx and port A as Rx.
+ */
+enum cg2900_dai_dir {
+	DAI_DIR_B_RX_A_TX = 0x00,
+	DAI_DIR_B_TX_A_RX = 0x01
+};
+
+/**
+ * enum cg2900_dai_mode - DAI mode alternatives.
+ * @DAI_MODE_SLAVE: Slave.
+ * @DAI_MODE_MASTER: Master.
+ */
+enum cg2900_dai_mode {
+	DAI_MODE_SLAVE = 0x00,
+	DAI_MODE_MASTER = 0x01
+};
+
+/**
+ * enum cg2900_dai_stream_ratio - Voice stream ratio alternatives.
+ * @STREAM_RATIO_FM16_VOICE16:	FM 16kHz, Voice 16kHz.
+ * @STREAM_RATIO_FM16_VOICE8:	FM 16kHz, Voice 8kHz.
+ * @STREAM_RATIO_FM48_VOICE16:	FM 48kHz, Voice 16Khz.
+ * @STREAM_RATIO_FM48_VOICE8:	FM 48kHz, Voice 8kHz.
+ *
+ * Contains the alternatives for the voice stream ratio between the Audio stream
+ * sample rate and the Voice stream sample rate.
+ */
+enum cg2900_dai_stream_ratio {
+	STREAM_RATIO_FM16_VOICE16 = 0x01,
+	STREAM_RATIO_FM16_VOICE8 = 0x02,
+	STREAM_RATIO_FM48_VOICE16 = 0x03,
+	STREAM_RATIO_FM48_VOICE8 = 0x06
+};
+
+/**
+ * enum cg2900_dai_fs_duration - Frame sync duration alternatives.
+ * @SYNC_DURATION_8: 8 frames sync duration.
+ * @SYNC_DURATION_16: 16 frames sync duration.
+ * @SYNC_DURATION_24: 24 frames sync duration.
+ * @SYNC_DURATION_32: 32 frames sync duration.
+ * @SYNC_DURATION_48: 48 frames sync duration.
+ * @SYNC_DURATION_50: 50 frames sync duration.
+ * @SYNC_DURATION_64: 64 frames sync duration.
+ * @SYNC_DURATION_75: 75 frames sync duration.
+ * @SYNC_DURATION_96: 96 frames sync duration.
+ * @SYNC_DURATION_125: 125 frames sync duration.
+ * @SYNC_DURATION_128: 128 frames sync duration.
+ * @SYNC_DURATION_150: 150 frames sync duration.
+ * @SYNC_DURATION_192: 192 frames sync duration.
+ * @SYNC_DURATION_250: 250 frames sync duration.
+ * @SYNC_DURATION_256: 256 frames sync duration.
+ * @SYNC_DURATION_300: 300 frames sync duration.
+ * @SYNC_DURATION_384: 384 frames sync duration.
+ * @SYNC_DURATION_500: 500 frames sync duration.
+ * @SYNC_DURATION_512: 512 frames sync duration.
+ * @SYNC_DURATION_600: 600 frames sync duration.
+ * @SYNC_DURATION_768: 768 frames sync duration.
+ *
+ * This parameter sets the PCM frame sync duration. It is calculated as the
+ * ratio between the bit clock and the frame rate. For example, if the bit
+ * clock is 512 kHz and the stream sample rate is 8 kHz, the PCM frame sync
+ * duration is 512 / 8 = 64.
+ */
+enum cg2900_dai_fs_duration {
+	SYNC_DURATION_8   = 0,
+	SYNC_DURATION_16  = 1,
+	SYNC_DURATION_24  = 2,
+	SYNC_DURATION_32  = 3,
+	SYNC_DURATION_48  = 4,
+	SYNC_DURATION_50  = 5,
+	SYNC_DURATION_64  = 6,
+	SYNC_DURATION_75  = 7,
+	SYNC_DURATION_96  = 8,
+	SYNC_DURATION_125 = 9,
+	SYNC_DURATION_128 = 10,
+	SYNC_DURATION_150 = 11,
+	SYNC_DURATION_192 = 12,
+	SYNC_DURATION_250 = 13,
+	SYNC_DURATION_256 = 14,
+	SYNC_DURATION_300 = 15,
+	SYNC_DURATION_384 = 16,
+	SYNC_DURATION_500 = 17,
+	SYNC_DURATION_512 = 18,
+	SYNC_DURATION_600 = 19,
+	SYNC_DURATION_768 = 20
+};
+
+/**
+ * enum cg2900_dai_bit_clk - Bit Clock alternatives.
+ * @BIT_CLK_128:	128 Kbits clock.
+ * @BIT_CLK_256:	256 Kbits clock.
+ * @BIT_CLK_512:	512 Kbits clock.
+ * @BIT_CLK_768:	768 Kbits clock.
+ * @BIT_CLK_1024:	1024 Kbits clock.
+ * @BIT_CLK_1411_76:	1411.76 Kbits clock.
+ * @BIT_CLK_1536:	1536 Kbits clock.
+ * @BIT_CLK_2000:	2000 Kbits clock.
+ * @BIT_CLK_2048:	2048 Kbits clock.
+ * @BIT_CLK_2400:	2400 Kbits clock.
+ * @BIT_CLK_2823_52:	2823.52 Kbits clock.
+ * @BIT_CLK_3072:	3072 Kbits clock.
+ *
+ *  This parameter sets the bit clock speed. This is the clocking of the actual
+ *  data. A usual parameter for eSCO voice is 512 kHz.
+ */
+enum cg2900_dai_bit_clk {
+	BIT_CLK_128 = 0x00,
+	BIT_CLK_256 = 0x01,
+	BIT_CLK_512 = 0x02,
+	BIT_CLK_768 = 0x03,
+	BIT_CLK_1024 = 0x04,
+	BIT_CLK_1411_76 = 0x05,
+	BIT_CLK_1536 = 0x06,
+	BIT_CLK_2000 = 0x07,
+	BIT_CLK_2048 = 0x08,
+	BIT_CLK_2400 = 0x09,
+	BIT_CLK_2823_52 = 0x0A,
+	BIT_CLK_3072 = 0x0B
+};
+
+/**
+ * enum cg2900_dai_sample_rate - Sample rates alternatives.
+ * @SAMPLE_RATE_8:	8 kHz sample rate.
+ * @SAMPLE_RATE_16:	16 kHz sample rate.
+ * @SAMPLE_RATE_44_1:	44.1 kHz sample rate.
+ * @SAMPLE_RATE_48:	48 kHz sample rate.
+ */
+enum cg2900_dai_sample_rate {
+	SAMPLE_RATE_8    = 0,
+	SAMPLE_RATE_16   = 1,
+	SAMPLE_RATE_44_1 = 2,
+	SAMPLE_RATE_48   = 3
+};
+
+/**
+ * enum cg2900_dai_port_protocol - Port protocol alternatives.
+ * @PORT_PROTOCOL_PCM: Protocol PCM.
+ * @PORT_PROTOCOL_I2S: Protocol I2S.
+ */
+enum cg2900_dai_port_protocol {
+	PORT_PROTOCOL_PCM = 0x00,
+	PORT_PROTOCOL_I2S = 0x01
+};
+
+/**
+ * enum cg2900_dai_channel_sel - The channel selection alternatives.
+ * @CHANNEL_SELECTION_RIGHT: Right channel used.
+ * @CHANNEL_SELECTION_LEFT: Left channel used.
+ * @CHANNEL_SELECTION_BOTH: Both channels used.
+ */
+enum cg2900_dai_channel_sel {
+	CHANNEL_SELECTION_RIGHT = 0x00,
+	CHANNEL_SELECTION_LEFT = 0x01,
+	CHANNEL_SELECTION_BOTH = 0x02
+};
+
+/**
+ * struct cg2900_dai_conf_i2s_pcm - Port configuration structure.
+ * @mode:		Operational mode of the port configured.
+ * @i2s_channel_sel:	I2S channels used. Only valid if used in I2S mode.
+ * @slot_0_used:	True if SCO slot 0 is used.
+ * @slot_1_used:	True if SCO slot 1 is used.
+ * @slot_2_used:	True if SCO slot 2 is used.
+ * @slot_3_used:	True if SCO slot 3 is used.
+ * @slot_0_dir:		Direction of slot 0.
+ * @slot_1_dir:		Direction of slot 1.
+ * @slot_2_dir:		Direction of slot 2.
+ * @slot_3_dir:		Direction of slot 3.
+ * @slot_0_start:	Slot 0 start (relative to the PCM frame sync).
+ * @slot_1_start:	Slot 1 start (relative to the PCM frame sync)
+ * @slot_2_start:	Slot 2 start (relative to the PCM frame sync)
+ * @slot_3_start:	Slot 3 start (relative to the PCM frame sync)
+ * @ratio:		Voice stream ratio between the Audio stream sample rate
+ *			and the Voice stream sample rate.
+ * @protocol:		Protocol used on port.
+ * @duration:		Frame sync duration.
+ * @clk:		Bit clock.
+ * @sample_rate:	Sample rate.
+ */
+struct cg2900_dai_conf_i2s_pcm {
+	enum cg2900_dai_mode mode;
+	enum cg2900_dai_channel_sel i2s_channel_sel;
+	bool slot_0_used;
+	bool slot_1_used;
+	bool slot_2_used;
+	bool slot_3_used;
+	enum cg2900_dai_dir slot_0_dir;
+	enum cg2900_dai_dir slot_1_dir;
+	enum cg2900_dai_dir slot_2_dir;
+	enum cg2900_dai_dir slot_3_dir;
+	__u8 slot_0_start;
+	__u8 slot_1_start;
+	__u8 slot_2_start;
+	__u8 slot_3_start;
+	enum cg2900_dai_stream_ratio ratio;
+	enum cg2900_dai_port_protocol protocol;
+	enum cg2900_dai_fs_duration duration;
+	enum cg2900_dai_bit_clk clk;
+	enum cg2900_dai_sample_rate sample_rate;
+};
+
+/**
+ * enum cg2900_dai_half_period - Half period duration alternatives.
+ * @HALF_PER_DUR_8:	8 Bits.
+ * @HALF_PER_DUR_16:	16 Bits.
+ * @HALF_PER_DUR_24:	24 Bits.
+ * @HALF_PER_DUR_25:	25 Bits.
+ * @HALF_PER_DUR_32:	32 Bits.
+ * @HALF_PER_DUR_48:	48 Bits.
+ * @HALF_PER_DUR_64:	64 Bits.
+ * @HALF_PER_DUR_75:	75 Bits.
+ * @HALF_PER_DUR_96:	96 Bits.
+ * @HALF_PER_DUR_128:	128 Bits.
+ * @HALF_PER_DUR_150:	150 Bits.
+ * @HALF_PER_DUR_192:	192 Bits.
+ *
+ * This parameter sets the number of bits contained in each I2S half period,
+ * i.e. each channel slot. A usual value is 16 bits.
+ */
+enum cg2900_dai_half_period {
+	HALF_PER_DUR_8 = 0x00,
+	HALF_PER_DUR_16 = 0x01,
+	HALF_PER_DUR_24 = 0x02,
+	HALF_PER_DUR_25 = 0x03,
+	HALF_PER_DUR_32 = 0x04,
+	HALF_PER_DUR_48 = 0x05,
+	HALF_PER_DUR_64 = 0x06,
+	HALF_PER_DUR_75 = 0x07,
+	HALF_PER_DUR_96 = 0x08,
+	HALF_PER_DUR_128 = 0x09,
+	HALF_PER_DUR_150 = 0x0A,
+	HALF_PER_DUR_192 = 0x0B
+};
+
+/**
+ * enum cg2900_dai_word_width - Word width alternatives.
+ * @WORD_WIDTH_16: 16 bits words.
+ * @WORD_WIDTH_32: 32 bits words.
+ */
+enum cg2900_dai_word_width {
+	WORD_WIDTH_16 = 0x00,
+	WORD_WIDTH_32 = 0x01
+};
+
+/**
+ * struct cg2900_dai_conf_i2s - Port configuration struct for I2S.
+ * @mode:		Operational mode of the port.
+ * @half_period:	Half period duration.
+ * @channel_sel:	Channel selection.
+ * @sample_rate:	Sample rate.
+ * @word_width:		Word width.
+ */
+struct cg2900_dai_conf_i2s {
+	enum cg2900_dai_mode			mode;
+	enum cg2900_dai_half_period		half_period;
+	enum cg2900_dai_channel_sel		channel_sel;
+	enum cg2900_dai_sample_rate		sample_rate;
+	enum cg2900_dai_word_width		word_width;
+};
+
+/**
+ * union cg2900_dai_port_conf - DAI port configuration union.
+ * @i2s: The configuration struct for a port supporting only I2S.
+ * @i2s_pcm: The configuration struct for a port supporting both PCM and I2S.
+ */
+union cg2900_dai_port_conf {
+	struct cg2900_dai_conf_i2s i2s;
+	struct cg2900_dai_conf_i2s_pcm i2s_pcm;
+};
+
+/**
+ * enum cg2900_dai_ext_port_id - DAI external port id alternatives.
+ * @PORT_0_I2S: Port id is 0 and it supports only I2S.
+ * @PORT_1_I2S_PCM: Port id is 1 and it supports both I2S and PCM.
+ */
+enum cg2900_dai_ext_port_id {
+	PORT_0_I2S,
+	PORT_1_I2S_PCM
+};
+
+/**
+ * enum cg2900_audio_endpoint_id - Audio endpoint id alternatives.
+ * @ENDPOINT_PORT_0_I2S:	Internal audio endpoint of the external I2S
+ *				interface.
+ * @ENDPOINT_PORT_1_I2S_PCM:	Internal audio endpoint of the external I2S/PCM
+ *				interface.
+ * @ENDPOINT_SLIMBUS_VOICE:	Internal audio endpoint of the external Slimbus
+ *				voice interface. (Currently not supported)
+ * @ENDPOINT_SLIMBUS_AUDIO:	Internal audio endpoint of the external Slimbus
+ *				audio interface. (Currently not supported)
+ * @ENDPOINT_BT_SCO_INOUT:	Bluetooth SCO bidirectional.
+ * @ENDPOINT_BT_A2DP_SRC:	Bluetooth A2DP source.
+ * @ENDPOINT_BT_A2DP_SNK:	Bluetooth A2DP sink.
+ * @ENDPOINT_FM_RX:		FM receive.
+ * @ENDPOINT_FM_TX:		FM transmit.
+ * @ENDPOINT_ANALOG_OUT:	Analog out.
+ * @ENDPOINT_DSP_AUDIO_IN:	DSP audio in.
+ * @ENDPOINT_DSP_AUDIO_OUT:	DSP audio out.
+ * @ENDPOINT_DSP_VOICE_IN:	DSP voice in.
+ * @ENDPOINT_DSP_VOICE_OUT:	DSP voice out.
+ * @ENDPOINT_DSP_TONE_IN:	DSP tone in.
+ * @ENDPOINT_BURST_BUFFER_IN:	Burst buffer in.
+ * @ENDPOINT_BURST_BUFFER_OUT:	Burst buffer out.
+ * @ENDPOINT_MUSIC_DECODER:	Music decoder.
+ * @ENDPOINT_HCI_AUDIO_IN:	HCI audio in.
+ */
+enum cg2900_audio_endpoint_id {
+	ENDPOINT_PORT_0_I2S,
+	ENDPOINT_PORT_1_I2S_PCM,
+	ENDPOINT_SLIMBUS_VOICE,
+	ENDPOINT_SLIMBUS_AUDIO,
+	ENDPOINT_BT_SCO_INOUT,
+	ENDPOINT_BT_A2DP_SRC,
+	ENDPOINT_BT_A2DP_SNK,
+	ENDPOINT_FM_RX,
+	ENDPOINT_FM_TX,
+	ENDPOINT_ANALOG_OUT,
+	ENDPOINT_DSP_AUDIO_IN,
+	ENDPOINT_DSP_AUDIO_OUT,
+	ENDPOINT_DSP_VOICE_IN,
+	ENDPOINT_DSP_VOICE_OUT,
+	ENDPOINT_DSP_TONE_IN,
+	ENDPOINT_BURST_BUFFER_IN,
+	ENDPOINT_BURST_BUFFER_OUT,
+	ENDPOINT_MUSIC_DECODER,
+	ENDPOINT_HCI_AUDIO_IN
+};
+
+/**
+ * struct cg2900_dai_config - Configuration struct for Digital Audio Interface.
+ * @port: The port id to configure. Acts as a discriminator for @conf parameter
+ *	  which is a union.
+ * @conf: The configuration union that contains the parameters for the port.
+ */
+struct cg2900_dai_config {
+	enum cg2900_dai_ext_port_id	port;
+	union cg2900_dai_port_conf	conf;
+};
+
+/*
+ * Endpoint configuration types
+ */
+
+/**
+ * enum cg2900_endpoint_sample_rate - Audio endpoint configuration sample rate alternatives.
+ *
+ * This enum defines the same values as @cg2900_dai_sample_rate, but
+ * is kept to preserve the API.
+ *
+ * @ENDPOINT_SAMPLE_RATE_8_KHZ: 8 kHz sample rate.
+ * @ENDPOINT_SAMPLE_RATE_16_KHZ: 16 kHz sample rate.
+ * @ENDPOINT_SAMPLE_RATE_44_1_KHZ: 44.1 kHz sample rate.
+ * @ENDPOINT_SAMPLE_RATE_48_KHZ: 48 kHz sample rate.
+ */
+enum cg2900_endpoint_sample_rate {
+	ENDPOINT_SAMPLE_RATE_8_KHZ	= SAMPLE_RATE_8,
+	ENDPOINT_SAMPLE_RATE_16_KHZ	= SAMPLE_RATE_16,
+	ENDPOINT_SAMPLE_RATE_44_1_KHZ	= SAMPLE_RATE_44_1,
+	ENDPOINT_SAMPLE_RATE_48_KHZ	= SAMPLE_RATE_48
+};
+
+
+/**
+ * struct cg2900_endpoint_config_a2dp_src - A2DP source audio endpoint configurations.
+ * @sample_rate: Sample rate.
+ * @channel_count: Number of channels.
+ */
+struct cg2900_endpoint_config_a2dp_src {
+	enum cg2900_endpoint_sample_rate	sample_rate;
+	unsigned int				channel_count;
+};
+
+/**
+ * struct cg2900_endpoint_config_fm - Configuration parameters for an FM endpoint.
+ * @sample_rate: The sample rate alternatives for the FM audio endpoints.
+ */
+struct cg2900_endpoint_config_fm {
+	enum cg2900_endpoint_sample_rate	sample_rate;
+};
+
+
+/**
+ * struct cg2900_endpoint_config_sco_in_out - SCO audio endpoint configuration structure.
+ * @sample_rate: Sample rate, valid values are
+ *		 * ENDPOINT_SAMPLE_RATE_8_KHZ
+ *		 * ENDPOINT_SAMPLE_RATE_16_KHZ.
+ */
+struct cg2900_endpoint_config_sco_in_out {
+	enum cg2900_endpoint_sample_rate	sample_rate;
+};
+
+/**
+ * union cg2900_endpoint_config - Different audio endpoint configurations.
+ * @sco:	SCO audio endpoint configuration structure.
+ * @a2dp_src:	A2DP source audio endpoint configuration structure.
+ * @fm:		FM audio endpoint configuration structure.
+ */
+union cg2900_endpoint_config_union {
+	struct cg2900_endpoint_config_sco_in_out	sco;
+	struct cg2900_endpoint_config_a2dp_src		a2dp_src;
+	struct cg2900_endpoint_config_fm		fm;
+};
+
+/**
+ * struct cg2900_endpoint_config - Audio endpoint configuration.
+ * @endpoint_id:	Identifies the audio endpoint. Works as a discriminator
+ *			for the config union.
+ * @config:		Union holding the configuration parameters for
+ *			the endpoint.
+ */
+struct cg2900_endpoint_config {
+	enum cg2900_audio_endpoint_id		endpoint_id;
+	union cg2900_endpoint_config_union	config;
+};
+
+#ifdef __KERNEL__
+#include <linux/device.h>
+
+int cg2900_audio_get_devices(struct device *devices[], __u8 size);
+int cg2900_audio_open(unsigned int *session, struct device *parent);
+int cg2900_audio_close(unsigned int *session);
+int cg2900_audio_set_dai_config(unsigned int session,
+				struct cg2900_dai_config *config);
+int cg2900_audio_get_dai_config(unsigned int session,
+				struct cg2900_dai_config *config);
+int cg2900_audio_config_endpoint(unsigned int session,
+				 struct cg2900_endpoint_config *config);
+int cg2900_audio_start_stream(unsigned int session,
+			      enum cg2900_audio_endpoint_id ep_1,
+			      enum cg2900_audio_endpoint_id ep_2,
+			      unsigned int *stream_handle);
+int cg2900_audio_stop_stream(unsigned int session,
+			     unsigned int stream_handle);
+
+#endif /* __KERNEL__ */
+#endif /* _CG2900_AUDIO_H_ */
diff --git a/drivers/staging/cg2900/include/cg2900_hci.h b/drivers/staging/cg2900/include/cg2900_hci.h
new file mode 100644
index 00000000000..e094a9dddbd
--- /dev/null
+++ b/drivers/staging/cg2900/include/cg2900_hci.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * This file is a staging solution and shall be integrated into
+ * /include/net/bluetooth/hci.h.
+ */
+
+#ifndef __CG2900_HCI_H
+#define __CG2900_HCI_H
+
+#define HCI_EV_HW_ERROR			0x10
+struct hci_ev_hw_error {
+	__u8	hw_code;
+} __packed;
+
+#endif /* __CG2900_HCI_H */
diff --git a/drivers/staging/cg2900/mfd/Makefile b/drivers/staging/cg2900/mfd/Makefile
new file mode 100644
index 00000000000..bdbd8de90ee
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/Makefile
@@ -0,0 +1,18 @@
+#
+# Makefile for ST-Ericsson CG2900 connectivity combo controller
+#
+
+ccflags-y :=					\
+	-Idrivers/staging/cg2900/include
+
+obj-$(CONFIG_CG2900)	+= cg2900_core.o cg2900_lib.o
+export-objs			:= cg2900_core.o cg2900_lib.o
+
+obj-$(CONFIG_CG2900)	+= cg2900_char_devices.o
+
+obj-$(CONFIG_CG2900_TEST)	+= cg2900_test.o
+
+obj-$(CONFIG_CG2900_CHIP)	+= cg2900_chip.o
+obj-$(CONFIG_STLC2690_CHIP)	+= stlc2690_chip.o
+
+obj-$(CONFIG_CG2900_AUDIO)	+= cg2900_audio.o
diff --git a/drivers/staging/cg2900/mfd/cg2900_audio.c b/drivers/staging/cg2900/mfd/cg2900_audio.c
new file mode 100644
index 00000000000..6eadd96b2de
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_audio.c
@@ -0,0 +1,3462 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth Audio Driver for ST-Ericsson CG2900 controller.
+ */
+#define NAME					"cg2900_audio"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <linux/cdev.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/types.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+
+#include "cg2900.h"
+#include "cg2900_audio.h"
+#include "cg2900_chip.h"
+
+#define MAX_NBR_OF_USERS			10
+#define FIRST_USER				1
+
+#define DEFAULT_SCO_HANDLE			0x0008
+
+/* Use a timeout of 5 seconds when waiting for a command response */
+#define RESP_TIMEOUT				5000
+
+#define BT_DEV					(info->dev_bt)
+#define FM_DEV					(info->dev_fm)
+
+/* Bluetooth error codes */
+#define HCI_BT_ERROR_NO_ERROR			0x00
+
+/* Used to select proper API, ignoring subrevisions etc */
+enum chip_revision {
+	CHIP_REV_PG1,
+	CHIP_REV_PG2
+};
+
+/**
+ * enum chip_resp_state - State when communicating with the CG2900 controller.
+ * @IDLE:		No outstanding packets to the controller.
+ * @WAITING:		Packet has been sent to the controller. Waiting for
+ *			response.
+ * @RESP_RECEIVED:	Response from controller has been received but not yet
+ *			handled.
+ */
+enum chip_resp_state {
+	IDLE,
+	WAITING,
+	RESP_RECEIVED
+};
+
+/**
+ * enum main_state - Main state for the CG2900 Audio driver.
+ * @OPENED:	Audio driver has registered to CG2900 Core.
+ * @CLOSED:	Audio driver is not registered to CG2900 Core.
+ * @RESET:	A reset of CG2900 Core has occurred and no user has re-opened
+ *		the audio driver.
+ */
+enum main_state {
+	OPENED,
+	CLOSED,
+	RESET
+};
+
+/**
+ * struct endpoint_list - List for storing endpoint configuration nodes.
+ * @ep_list:		Pointer to first node in list.
+ * @management_mutex:	Mutex for handling access to list.
+ */
+struct endpoint_list {
+	struct list_head	ep_list;
+	struct mutex		management_mutex;
+};
+
+/**
+ * struct endpoint_config_node - Node for storing endpoint configuration.
+ * @list:		list_head struct.
+ * @endpoint_id:	Endpoint ID.
+ * @config:		Stored configuration for this endpoint.
+ */
+struct endpoint_config_node {
+	struct list_head			list;
+	enum cg2900_audio_endpoint_id		endpoint_id;
+	union cg2900_endpoint_config_union	config;
+};
+
+/**
+ * struct audio_info - Main CG2900 Audio driver info structure.
+ * @list:			list_head struct.
+ * @state:			Current state of the CG2900 Audio driver.
+ * @revision:			Chip revision, used to select API.
+ * @misc_dev:			The misc device created by this driver.
+ * @misc_registered:		True if misc device is registered.
+ * @parent:			Parent device.
+ * @dev_bt:			Device registered by this driver for the BT
+ *				audio channel.
+ * @dev_fm:			Device registered by this driver for the FM
+ *				audio channel.
+ * @management_mutex:		Mutex for handling access to CG2900 Audio driver
+ *				management.
+ * @bt_mutex:			Mutex for handling access to BT audio channel.
+ * @fm_mutex:			Mutex for handling access to FM audio channel.
+ * @nbr_of_users_active:	Number of sessions open in the CG2900 Audio
+ *				driver.
+ * @i2s_config:			DAI I2S configuration.
+ * @i2s_pcm_config:		DAI PCM_I2S configuration.
+ * @i2s_config_known:		@true if @i2s_config has been set,
+ *				@false otherwise.
+ * @i2s_pcm_config_known:	@true if @i2s_pcm_config has been set,
+ *				@false otherwise.
+ * @endpoints:			List containing the endpoint configurations.
+ * @stream_ids:			Bitmask for in-use stream ids (only used with
+ *				PG2 chip API).
+ */
+struct audio_info {
+	struct list_head		list;
+	enum main_state			state;
+	enum chip_revision		revision;
+	struct miscdevice		misc_dev;
+	bool				misc_registered;
+	struct device			*parent;
+	struct device			*dev_bt;
+	struct device			*dev_fm;
+	struct mutex			management_mutex;
+	struct mutex			bt_mutex;
+	struct mutex			fm_mutex;
+	int				nbr_of_users_active;
+	struct cg2900_dai_conf_i2s	i2s_config;
+	struct cg2900_dai_conf_i2s_pcm	i2s_pcm_config;
+	bool				i2s_config_known;
+	bool				i2s_pcm_config_known;
+	struct endpoint_list		endpoints;
+	u8				stream_ids[16];
+};
+
+/**
+ * struct audio_user - CG2900 audio user info structure.
+ * @session:	Stored session for the char device.
+ * @resp_state:	State for controller communications.
+ * @info:	CG2900 audio info structure.
+ */
+struct audio_user {
+	int			session;
+	enum chip_resp_state	resp_state;
+	struct audio_info	*info;
+};
+
+/**
+ * struct audio_cb_info - Callback info structure registered in @user_data.
+ * @user:	Audio user currently awaiting data on the channel.
+ * @wq:		Wait queue for this channel.
+ * @skb_queue:	Sk buffer queue.
+ */
+struct audio_cb_info {
+	struct audio_user	*user;
+	wait_queue_head_t	wq;
+	struct sk_buff_head	skb_queue;
+};
+
+/**
+ * struct char_dev_info - CG2900 character device info structure.
+ * @session:		Stored session for the char device.
+ * @stored_data:	Data returned when executing last command, if any.
+ * @stored_data_len:	Length of @stored_data in bytes.
+ * @management_mutex:	Mutex for handling access to char dev management.
+ * @rw_mutex:		Mutex for handling access to char dev writes and reads.
+ * @info:		CG2900 audio info struct.
+ * @rx_queue:		Data queue.
+ */
+struct char_dev_info {
+	int			session;
+	u8			*stored_data;
+	int			stored_data_len;
+	struct mutex		management_mutex;
+	struct mutex		rw_mutex;
+	struct audio_info	*info;
+	struct sk_buff_head	rx_queue;
+};
+
+/*
+ * cg2900_audio_devices - List of active CG2900 audio devices.
+ */
+LIST_HEAD(cg2900_audio_devices);
+
+/*
+ * cg2900_audio_sessions - Pointers to currently opened sessions (maps
+ *			   session ID to user info).
+ */
+static struct audio_user *cg2900_audio_sessions[MAX_NBR_OF_USERS];
+
+/*
+ *	Internal conversion functions
+ *
+ *	Since the CG2900 APIs uses several different ways to encode the
+ *	same parameter in different cases, we have to use translator
+ *	functions.
+ */
+
+/**
+ * session_config_sample_rate() - Convert sample rate to format used in VS_Set_SessionConfiguration.
+ * @rate: Sample rate in API encoding.
+ */
+static u8 session_config_sample_rate(enum cg2900_endpoint_sample_rate rate)
+{
+	static const u8 codes[] = {
+		[ENDPOINT_SAMPLE_RATE_8_KHZ]    = CG2900_BT_SESSION_RATE_8K,
+		[ENDPOINT_SAMPLE_RATE_16_KHZ]   = CG2900_BT_SESSION_RATE_16K,
+		[ENDPOINT_SAMPLE_RATE_44_1_KHZ] = CG2900_BT_SESSION_RATE_44_1K,
+		[ENDPOINT_SAMPLE_RATE_48_KHZ]   = CG2900_BT_SESSION_RATE_48K
+	};
+
+	return codes[rate];
+}
+
+/**
+ * mc_i2s_sample_rate() - Convert sample rate to format used in VS_Port_Config for I2S.
+ * @rate: Sample rate in API encoding.
+ */
+static u8 mc_i2s_sample_rate(enum cg2900_dai_sample_rate rate)
+{
+	static const u8 codes[] = {
+		[SAMPLE_RATE_8]    = CG2900_MC_I2S_SAMPLE_RATE_8,
+		[SAMPLE_RATE_16]   = CG2900_MC_I2S_SAMPLE_RATE_16,
+		[SAMPLE_RATE_44_1] = CG2900_MC_I2S_SAMPLE_RATE_44_1,
+		[SAMPLE_RATE_48]   = CG2900_MC_I2S_SAMPLE_RATE_48
+	};
+
+	return codes[rate];
+}
+
+/**
+ * mc_pcm_sample_rate() - Convert sample rate to format used in VS_Port_Config for PCM/I2S.
+ * @rate: Sample rate in API encoding.
+ */
+static u8 mc_pcm_sample_rate(enum cg2900_dai_sample_rate rate)
+{
+	static const u8 codes[] = {
+		[SAMPLE_RATE_8]    = CG2900_MC_PCM_SAMPLE_RATE_8,
+		[SAMPLE_RATE_16]   = CG2900_MC_PCM_SAMPLE_RATE_16,
+		[SAMPLE_RATE_44_1] = CG2900_MC_PCM_SAMPLE_RATE_44_1,
+		[SAMPLE_RATE_48]   = CG2900_MC_PCM_SAMPLE_RATE_48
+	};
+
+	return codes[rate];
+}
+
+/**
+ * mc_i2s_channel_select() - Convert channel selection to format used in VS_Port_Config.
+ * @sel: Channel selection in API encoding.
+ */
+static u8 mc_i2s_channel_select(enum cg2900_dai_channel_sel sel)
+{
+	static const u8 codes[] = {
+		[CHANNEL_SELECTION_RIGHT] = CG2900_MC_I2S_RIGHT_CHANNEL,
+		[CHANNEL_SELECTION_LEFT]  = CG2900_MC_I2S_LEFT_CHANNEL,
+		[CHANNEL_SELECTION_BOTH]  = CG2900_MC_I2S_BOTH_CHANNELS
+	};
+	return codes[sel];
+}
+
+/**
+ * get_fs_duration() - Convert framesync-enumeration to real value.
+ * @duration: Framsync duration (API encoding).
+ *
+ * Returns:
+ * Duration in bits.
+ */
+static u16 get_fs_duration(enum cg2900_dai_fs_duration duration)
+{
+	static const u16 values[] = {
+		[SYNC_DURATION_8] = 8,
+		[SYNC_DURATION_16] = 16,
+		[SYNC_DURATION_24] = 24,
+		[SYNC_DURATION_32] = 32,
+		[SYNC_DURATION_48] = 48,
+		[SYNC_DURATION_50] = 50,
+		[SYNC_DURATION_64] = 64,
+		[SYNC_DURATION_75] = 75,
+		[SYNC_DURATION_96] = 96,
+		[SYNC_DURATION_125] = 125,
+		[SYNC_DURATION_128] = 128,
+		[SYNC_DURATION_150] = 150,
+		[SYNC_DURATION_192] = 192,
+		[SYNC_DURATION_250] = 250,
+		[SYNC_DURATION_256] = 256,
+		[SYNC_DURATION_300] = 300,
+		[SYNC_DURATION_384] = 384,
+		[SYNC_DURATION_500] = 500,
+		[SYNC_DURATION_512] = 512,
+		[SYNC_DURATION_600] = 600,
+		[SYNC_DURATION_768] = 768
+	};
+	return values[duration];
+}
+
+/**
+ * mc_i2s_role() - Convert master/slave encoding to format for I2S-ports.
+ * @mode: Master/slave in API encoding.
+ */
+static u8 mc_i2s_role(enum cg2900_dai_mode mode)
+{
+	if (mode == DAI_MODE_SLAVE)
+		return CG2900_I2S_MODE_SLAVE;
+	else
+		return CG2900_I2S_MODE_MASTER;
+}
+
+/**
+ * mc_pcm_role() - Convert master/slave encoding to format for PCM/I2S-port.
+ * @mode: Master/slave in API encoding.
+ */
+static u8 mc_pcm_role(enum cg2900_dai_mode mode)
+{
+	if (mode == DAI_MODE_SLAVE)
+		return CG2900_PCM_MODE_SLAVE;
+	else
+		return CG2900_PCM_MODE_MASTER;
+}
+
+/**
+ * fm_get_conversion() - Convert sample rate to convert up/down used in X_Set_Control FM commands.
+ * @srate: Sample rate.
+ */
+static u16 fm_get_conversion(enum cg2900_endpoint_sample_rate srate)
+{
+	if (srate >= ENDPOINT_SAMPLE_RATE_44_1_KHZ)
+		return CG2900_FM_CMD_SET_CTRL_CONV_UP;
+	else
+		return CG2900_FM_CMD_SET_CTRL_CONV_DOWN;
+}
+
+/**
+ * get_info() - Return info structure for this device.
+ * @dev:	Current device.
+ *
+ * This function returns the info structure on the following basis:
+ *	* If dev is NULL return first info struct found. If none is found return
+ *	  NULL.
+ *	* If dev is valid we will return corresponding info struct if dev is the
+ *	  parent of the info struct or if dev's parent is the parent of the info
+ *	  struct.
+ *	* If dev is valid and no info structure is found, a new info struct is
+ *	  allocated, initialized, and returned.
+ *
+ * Returns:
+ *   Pointer to info struct if there is no error.
+ *   NULL if NULL was supplied and no info structure exist.
+ *   ERR_PTR(-ENOMEM) if allocation fails.
+ */
+static struct audio_info *get_info(struct device *dev)
+{
+	struct list_head *cursor;
+	struct audio_info *tmp;
+	struct audio_info *info = NULL;
+
+	/*
+	 * Find the info structure for dev. If NULL is supplied for dev
+	 * just return first device found.
+	 */
+	list_for_each(cursor, &cg2900_audio_devices) {
+		tmp = list_entry(cursor, struct audio_info, list);
+		if (!dev || tmp->parent == dev->parent || tmp->parent == dev) {
+			info = tmp;
+			break;
+		}
+	}
+
+	if (!dev || info)
+		return info;
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(dev, "Could not allocate info struct\n");
+		return ERR_PTR(-ENOMEM);
+	}
+	info->parent = dev->parent;
+
+	/* Initiate the mutexes */
+	mutex_init(&(info->management_mutex));
+	mutex_init(&(info->bt_mutex));
+	mutex_init(&(info->fm_mutex));
+	mutex_init(&(info->endpoints.management_mutex));
+
+	/* Initiate the endpoint list */
+	INIT_LIST_HEAD(&info->endpoints.ep_list);
+
+	list_add_tail(&info->list, &cg2900_audio_devices);
+
+	dev_info(dev, "CG2900 device added\n");
+	return info;
+}
+
+/**
+ * flush_endpoint_list() - Deletes all stored endpoints in @list.
+ * @list:	List of endpoints.
+ */
+static void flush_endpoint_list(struct endpoint_list *list)
+{
+	struct list_head *cursor, *next;
+	struct endpoint_config_node *tmp;
+
+	mutex_lock(&list->management_mutex);
+	list_for_each_safe(cursor, next, &(list->ep_list)) {
+		tmp = list_entry(cursor, struct endpoint_config_node, list);
+		list_del(cursor);
+		kfree(tmp);
+	}
+	mutex_unlock(&list->management_mutex);
+}
+
+/**
+ * device_removed() - Remove device from list if there are no channels left.
+ * @info:	CG2900 audio info structure.
+ */
+static void device_removed(struct audio_info *info)
+{
+	struct list_head *cursor;
+	struct audio_info *tmp;
+
+	if (info->dev_bt || info->dev_fm)
+		/* There are still devices active */
+		return;
+
+	/* Find the stored info structure */
+	list_for_each(cursor, &cg2900_audio_devices) {
+		tmp = list_entry(cursor, struct audio_info, list);
+		if (tmp == info) {
+			list_del(cursor);
+			break;
+		}
+	}
+
+	flush_endpoint_list(&info->endpoints);
+
+	mutex_destroy(&info->management_mutex);
+	mutex_destroy(&info->bt_mutex);
+	mutex_destroy(&info->fm_mutex);
+	mutex_destroy(&info->endpoints.management_mutex);
+
+	kfree(info);
+	pr_info("CG2900 Audio device removed");
+}
+
+/**
+ * read_cb() - Handle data received from STE connectivity driver.
+ * @dev:	Device receiving data.
+ * @skb:	Buffer with data coming form device.
+ */
+static void read_cb(struct cg2900_user_data *dev, struct sk_buff *skb)
+{
+	struct audio_cb_info *cb_info;
+
+	cb_info = cg2900_get_usr(dev);
+
+	if (!(cb_info->user)) {
+		dev_err(dev->dev, "NULL supplied as cb_info->user\n");
+		return;
+	}
+
+	/* Mark that packet has been received */
+	dev_dbg(dev->dev, "New resp_state: RESP_RECEIVED");
+	cb_info->user->resp_state = RESP_RECEIVED;
+	skb_queue_tail(&cb_info->skb_queue, skb);
+	wake_up_all(&cb_info->wq);
+}
+
+/**
+ * reset_cb() - Reset callback function.
+ * @dev:	CG2900_Core device resetting.
+ */
+static void reset_cb(struct cg2900_user_data *dev)
+{
+	struct audio_info *info;
+
+	dev_dbg(dev->dev, "reset_cb\n");
+
+	info = dev_get_drvdata(dev->dev);
+	mutex_lock(&info->management_mutex);
+	info->nbr_of_users_active = 0;
+	info->state = RESET;
+	mutex_unlock(&info->management_mutex);
+}
+
+/**
+ * get_session_user() - Check that supplied session is within valid range.
+ * @session:	Session ID.
+ *
+ * Returns:
+ *   Audio_user if there is no error.
+ *   NULL for bad session ID.
+ */
+static struct audio_user *get_session_user(int session)
+{
+	struct audio_user *audio_user;
+
+	if (session < FIRST_USER || session >= MAX_NBR_OF_USERS) {
+		pr_err("Calling with invalid session %d", session);
+		return NULL;
+	}
+
+	audio_user = cg2900_audio_sessions[session];
+	if (!audio_user)
+		pr_err("Calling with non-opened session %d", session);
+	return audio_user;
+}
+
+/**
+ * del_endpoint_private() - Deletes an endpoint from @list.
+ * @endpoint_id:	Endpoint ID.
+ * @list:		List of endpoints.
+ *
+ * Deletes an endpoint from the supplied endpoint list.
+ * This function is not protected by any semaphore.
+ */
+static void del_endpoint_private(enum cg2900_audio_endpoint_id endpoint_id,
+				 struct endpoint_list *list)
+{
+	struct list_head *cursor, *next;
+	struct endpoint_config_node *tmp;
+
+	list_for_each_safe(cursor, next, &(list->ep_list)) {
+		tmp = list_entry(cursor, struct endpoint_config_node, list);
+		if (tmp->endpoint_id == endpoint_id) {
+			list_del(cursor);
+			kfree(tmp);
+		}
+	}
+}
+
+/**
+ * add_endpoint() - Add endpoint node to @list.
+ * @ep_config:	Endpoint configuration.
+ * @list:	List of endpoints.
+ *
+ * Add endpoint node to the supplied list and copies supplied config to node.
+ * If a node already exists for the supplied endpoint, the old node is removed
+ * and replaced by the new node.
+ */
+static void add_endpoint(struct cg2900_endpoint_config *ep_config,
+			 struct endpoint_list *list)
+{
+	struct endpoint_config_node *item;
+
+	item = kzalloc(sizeof(*item), GFP_KERNEL);
+	if (!item) {
+		pr_err("add_endpoint: Failed to alloc memory");
+		return;
+	}
+
+	/* Store values */
+	item->endpoint_id = ep_config->endpoint_id;
+	memcpy(&(item->config), &(ep_config->config), sizeof(item->config));
+
+	mutex_lock(&(list->management_mutex));
+
+	/*
+	 * Check if endpoint ID already exist in list.
+	 * If that is the case, remove it.
+	 */
+	if (!list_empty(&(list->ep_list)))
+		del_endpoint_private(ep_config->endpoint_id, list);
+
+	list_add_tail(&(item->list), &(list->ep_list));
+
+	mutex_unlock(&(list->management_mutex));
+}
+
+/**
+ * find_endpoint() - Finds endpoint identified by @endpoint_id in @list.
+ * @endpoint_id:	Endpoint ID.
+ * @list:		List of endpoints.
+ *
+ * Returns:
+ *   Endpoint configuration if there is no error.
+ *   NULL if no configuration can be found for @endpoint_id.
+ */
+static union cg2900_endpoint_config_union *
+find_endpoint(enum cg2900_audio_endpoint_id endpoint_id,
+	      struct endpoint_list *list)
+{
+	struct list_head *cursor, *next;
+	struct endpoint_config_node *tmp;
+	struct endpoint_config_node *ret_ep = NULL;
+
+	mutex_lock(&list->management_mutex);
+	list_for_each_safe(cursor, next, &(list->ep_list)) {
+		tmp = list_entry(cursor, struct endpoint_config_node, list);
+		if (tmp->endpoint_id == endpoint_id) {
+			ret_ep = tmp;
+			break;
+		}
+	}
+	mutex_unlock(&list->management_mutex);
+
+	if (ret_ep)
+		return &(ret_ep->config);
+	else
+		return NULL;
+}
+
+/**
+ * new_stream_id() - Allocate a new stream id.
+ * @info:	Current audio info struct.
+ *
+ * Returns:
+ *  0-127 new valid id.
+ *  -ENOMEM if no id is available.
+ */
+static s8 new_stream_id(struct audio_info *info)
+{
+	int r;
+
+	mutex_lock(&info->management_mutex);
+
+	r = find_first_zero_bit(info->stream_ids,
+				8 * sizeof(info->stream_ids));
+
+	if (r >= 8 * sizeof(info->stream_ids)) {
+		r = -ENOMEM;
+		goto out;
+	}
+
+	set_bit(r, (unsigned long int *)info->stream_ids);
+
+out:
+	mutex_unlock(&info->management_mutex);
+	return r;
+}
+
+/**
+ * release_stream_id() - Release a stream id.
+ * @info:	Current audio info struct.
+ * @id:		Stream to release.
+ */
+static void release_stream_id(struct audio_info *info, u8 id)
+{
+	if (id >= 8 * sizeof(info->stream_ids))
+		return;
+
+	mutex_lock(&info->management_mutex);
+	clear_bit(id, (unsigned long int *)info->stream_ids);
+	mutex_unlock(&info->management_mutex);
+}
+
+/**
+ * receive_fm_write_response() - Wait for and handle the response to an FM Legacy WriteCommand request.
+ * @audio_user:	Audio user to check for.
+ * @command:	FM command to wait for.
+ *
+ * This function first waits (up to 5 seconds) for a response to an FM
+ * write command and when one arrives, it checks that it is the one we
+ * are waiting for and also that no error has occurred.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -EIO for other errors.
+ */
+static int receive_fm_write_response(struct audio_user *audio_user,
+				     u16 command)
+{
+	int err = 0;
+	int res;
+	struct sk_buff *skb;
+	struct fm_leg_cmd_cmpl *pkt;
+	u16 rsp_cmd;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = audio_user->info;
+	pf_data = dev_get_platdata(info->dev_fm);
+	cb_info = cg2900_get_usr(pf_data);
+
+	/*
+	 * Wait for callback to receive command complete and then wake us up
+	 * again.
+	 */
+	res = wait_event_timeout(cb_info->wq,
+				 audio_user->resp_state == RESP_RECEIVED,
+				 msecs_to_jiffies(RESP_TIMEOUT));
+	if (!res) {
+		dev_err(FM_DEV, "Timeout while waiting for return packet\n");
+		return -ECOMM;
+	} else if (res < 0) {
+		dev_err(FM_DEV,
+			"Error %d occurred while waiting for return packet\n",
+			res);
+		return -ECOMM;
+	}
+
+	/* OK, now we should have received answer. Let's check it. */
+	skb = skb_dequeue_tail(&cb_info->skb_queue);
+	if (!skb) {
+		dev_err(FM_DEV, "No skb in queue when it should be there\n");
+		return -EIO;
+	}
+
+	pkt = (struct fm_leg_cmd_cmpl *)skb->data;
+
+	/* Check if we received the correct event */
+	if (pkt->opcode != CG2900_FM_GEN_ID_LEGACY) {
+		dev_err(FM_DEV,
+			"Received unknown FM packet. 0x%X %X %X %X %X\n",
+			skb->data[0], skb->data[1], skb->data[2],
+			skb->data[3], skb->data[4]);
+		err = -EIO;
+		goto error_handling_free_skb;
+	}
+
+	/* FM Legacy Command complete event */
+	rsp_cmd = cg2900_get_fm_cmd_id(le16_to_cpu(pkt->response_head));
+
+	if (pkt->fm_function != CG2900_FM_CMD_PARAM_WRITECOMMAND ||
+	    rsp_cmd != command) {
+		dev_err(FM_DEV,
+			"Received unexpected packet func 0x%X cmd 0x%04X\n",
+			pkt->fm_function, rsp_cmd);
+		err = -EIO;
+		goto error_handling_free_skb;
+	}
+
+	if (pkt->cmd_status != CG2900_FM_CMD_STATUS_COMMAND_SUCCEEDED) {
+		dev_err(FM_DEV, "FM Command failed (%d)\n", pkt->cmd_status);
+		err = -EIO;
+		goto error_handling_free_skb;
+	}
+	/* Operation succeeded. We are now done */
+
+error_handling_free_skb:
+	kfree_skb(skb);
+	return err;
+}
+
+/**
+ * receive_bt_cmd_complete() - Wait for and handle an BT Command Complete event.
+ * @audio_user:	Audio user to check for.
+ * @rsp:	Opcode of BT command to wait for.
+ * @data:	Pointer to buffer if any received data should be stored (except
+ *		status).
+ * @data_len:	Length of @data in bytes.
+ *
+ * This function first waits for BT Command Complete event (up to 5 seconds)
+ * and when one arrives, it checks that it is the one we are waiting for and
+ * also that no error has occurred.
+ * If @data is supplied it also copies received data into @data.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -EIO for other errors.
+ */
+static int receive_bt_cmd_complete(struct audio_user *audio_user, u16 rsp,
+				   void *data, int data_len)
+{
+	int err = 0;
+	int res;
+	struct sk_buff *skb;
+	struct bt_cmd_cmpl_event *evt;
+	u16 opcode;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = audio_user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	/*
+	 * Wait for callback to receive command complete and then wake us up
+	 * again.
+	 */
+	res = wait_event_timeout(cb_info->wq,
+				 audio_user->resp_state == RESP_RECEIVED,
+				 msecs_to_jiffies(RESP_TIMEOUT));
+	if (!res) {
+		dev_err(BT_DEV, "Timeout while waiting for return packet\n");
+		return -ECOMM;
+	} else if (res < 0) {
+		/* We timed out or an error occurred */
+		dev_err(BT_DEV,
+			"Error %d occurred while waiting for return packet\n",
+			res);
+		return -ECOMM;
+	}
+
+	/* OK, now we should have received answer. Let's check it. */
+	skb = skb_dequeue_tail(&cb_info->skb_queue);
+	if (!skb) {
+		dev_err(BT_DEV, "No skb in queue when it should be there\n");
+		return -EIO;
+	}
+
+	evt = (struct bt_cmd_cmpl_event *)skb->data;
+	if (evt->eventcode != HCI_EV_CMD_COMPLETE) {
+		dev_err(BT_DEV,
+			"We did not receive the event we expected (0x%X)\n",
+			evt->eventcode);
+		err = -EIO;
+		goto error_handling_free_skb;
+	}
+
+	opcode = le16_to_cpu(evt->opcode);
+	if (opcode != rsp) {
+		dev_err(BT_DEV,
+			"Received cmd complete for unexpected command: "
+			"0x%04X\n", opcode);
+		err = -EIO;
+		goto error_handling_free_skb;
+	}
+
+	if (evt->status != HCI_BT_ERROR_NO_ERROR) {
+		dev_err(BT_DEV, "Received command complete with err %d\n",
+			evt->status);
+		err = -EIO;
+		/*
+		* In data there might be more detailed error code.
+		* Let's copy it.
+		*/
+	}
+
+	/*
+	 * Copy the rest of the parameters if a buffer has been supplied.
+	 * The caller must have set the length correctly.
+	 */
+	if (data)
+		memcpy(data, evt->data, data_len);
+
+	/* Operation succeeded. We are now done */
+
+error_handling_free_skb:
+	kfree_skb(skb);
+	return err;
+}
+
+/**
+ * send_vs_delete_stream() - Delete an audio stream defined by @stream_handle.
+ * @audio_user:		Audio user to check for.
+ * @stream_handle:	Handle of the audio stream.
+ *
+ * This function is used to delete an audio stream defined by a stream
+ * handle.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -ENOMEM upon allocation errors.
+ *   Errors from @cg2900_write.
+ *   -EIO for other errors.
+ */
+static int send_vs_delete_stream(struct audio_user *audio_user,
+			    unsigned int stream_handle)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	u16 opcode;
+	struct audio_info *info = audio_user->info;
+	struct cg2900_user_data *pf_data = dev_get_platdata(info->dev_bt);
+	struct audio_cb_info *cb_info = cg2900_get_usr(pf_data);
+
+	/* Now delete the stream - format command... */
+	if (info->revision == CHIP_REV_PG1) {
+		struct bt_vs_reset_session_cfg_cmd *cmd;
+
+		dev_dbg(BT_DEV, "BT: HCI_VS_Reset_Session_Configuration\n");
+
+		skb = pf_data->alloc_skb(sizeof(*cmd), GFP_KERNEL);
+		if (!skb) {
+			dev_err(BT_DEV, "Could not allocate skb\n");
+			err = -ENOMEM;
+			return err;
+		}
+
+		cmd = (struct bt_vs_reset_session_cfg_cmd *)
+			skb_put(skb, sizeof(*cmd));
+
+		opcode = CG2900_BT_VS_RESET_SESSION_CONFIG;
+		cmd->opcode = cpu_to_le16(opcode);
+		cmd->plen   = BT_PARAM_LEN(sizeof(*cmd));
+		cmd->id     = (u8)stream_handle;
+	} else {
+		struct mc_vs_delete_stream_cmd *cmd;
+
+		dev_dbg(BT_DEV, "BT: HCI_VS_Delete_Stream\n");
+
+		skb = pf_data->alloc_skb(sizeof(*cmd), GFP_KERNEL);
+		if (!skb) {
+			dev_err(BT_DEV, "Could not allocate skb\n");
+			err = -ENOMEM;
+			return err;
+		}
+
+		cmd = (struct mc_vs_delete_stream_cmd *)
+			skb_put(skb, sizeof(*cmd));
+
+		opcode = CG2900_MC_VS_DELETE_STREAM;
+		cmd->opcode = cpu_to_le16(opcode);
+		cmd->plen   = BT_PARAM_LEN(sizeof(*cmd));
+		cmd->stream = (u8)stream_handle;
+	}
+
+	/* ...and send it */
+	cb_info->user = audio_user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	audio_user->resp_state = WAITING;
+
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		goto error_handling_free_skb;
+	}
+
+	/* wait for response */
+	if (info->revision == CHIP_REV_PG1) {
+		err = receive_bt_cmd_complete(audio_user, opcode, NULL, 0);
+	} else {
+		u8 vs_err;
+
+		/* All commands in PG2 API returns one byte extra status */
+		err = receive_bt_cmd_complete(audio_user, opcode,
+					      &vs_err, sizeof(vs_err));
+
+	if (err)
+		dev_err(BT_DEV,
+			"VS_DELETE_STREAM - failed with error 0x%02X\n",
+			vs_err);
+		else
+			release_stream_id(info, stream_handle);
+	}
+
+	return err;
+
+error_handling_free_skb:
+	kfree_skb(skb);
+	return err;
+}
+
+/**
+ * send_vs_session_ctrl() - Formats an sends a CG2900_BT_VS_SESSION_CTRL command.
+ * @user:		Audio user this command belongs to.
+ * @stream_handle:	Handle to stream.
+ * @command:		Command to execute on stream, should be one of
+ *			CG2900_BT_SESSION_START, CG2900_BT_SESSION_STOP,
+ *			CG2900_BT_SESSION_PAUSE, CG2900_BT_SESSION_RESUME.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the bt_mutex held.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_vs_session_ctrl(struct audio_user *user,
+				u8 stream_handle, u8 command)
+{
+	int err = 0;
+	struct bt_vs_session_ctrl_cmd *pkt;
+	struct sk_buff *skb;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV, "BT: HCI_VS_Session_Control handle: %d cmd: %d\n",
+		stream_handle, command);
+
+	skb = pf_data->alloc_skb(sizeof(*pkt), GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV,
+			"send_vs_session_ctrl: Could not allocate skb\n");
+		return -ENOMEM;
+	}
+
+	/* Enter data into the skb */
+	pkt = (struct bt_vs_session_ctrl_cmd *) skb_put(skb, sizeof(*pkt));
+
+	pkt->opcode  = cpu_to_le16(CG2900_BT_VS_SESSION_CTRL);
+	pkt->plen    = BT_PARAM_LEN(sizeof(*pkt));
+	pkt->id      = stream_handle;
+	pkt->control = command; /* Start/stop etc */
+
+	cb_info->user = user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished;
+	}
+
+	err = receive_bt_cmd_complete(user, CG2900_BT_VS_SESSION_CTRL,
+				      NULL, 0);
+finished:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * send_vs_session_config() - Formats an sends a CG2900_BT_VS_SESSION_CONFIG command.
+ * @user:		Audio user this command belongs to.
+ * @config_stream:	Custom function for configuring the stream.
+ * @priv_data:		Private data passed to @config_stream untouched.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the bt_mutex held.
+ *
+ * Space is allocated for one stream and a custom function is used to
+ * fill in the stream configuration.
+ *
+ * Returns:
+ *  0-255 stream handle if no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_vs_session_config(struct audio_user *user,
+	void(*config_stream)(struct audio_info *, void *,
+			     struct session_config_stream *),
+	void *priv_data)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	struct bt_vs_session_config_cmd *pkt;
+	u8 session_id;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV, "BT: HCI_VS_Set_Session_Configuration\n");
+
+	skb = pf_data->alloc_skb(sizeof(*pkt), GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV,
+			"send_vs_session_config: Could not allocate skb\n");
+		return -ENOMEM;
+	}
+
+	pkt = (struct bt_vs_session_config_cmd *)skb_put(skb, sizeof(*pkt));
+	/* zero the packet so we don't have to set all reserved fields */
+	memset(pkt, 0, sizeof(*pkt));
+
+	/* Common parameters */
+	pkt->opcode    = cpu_to_le16(CG2900_BT_VS_SET_SESSION_CONFIG);
+	pkt->plen      = BT_PARAM_LEN(sizeof(*pkt));
+	pkt->n_streams = 1; /* 1 stream configuration supplied */
+
+	/* Let the custom-function fill in the rest */
+	config_stream(info, priv_data, &pkt->stream);
+
+	cb_info->user = user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished;
+	}
+
+	err = receive_bt_cmd_complete(user,
+				      CG2900_BT_VS_SET_SESSION_CONFIG,
+				      &session_id, sizeof(session_id));
+	/* Return session id/stream handle if success */
+	if (!err)
+		err = session_id;
+
+finished:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * send_fm_write_1_param() - Formats and sends an FM legacy write command with one parameter.
+ * @user:	Audio user this command belongs to.
+ * @command:	Command.
+ * @param:	Parameter for command.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the fm_mutex held.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_fm_write_1_param(struct audio_user *user,
+				 u16 command, u16 param)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	struct fm_leg_cmd *cmd;
+	size_t len;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_fm);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(FM_DEV, "send_fm_write_1_param cmd 0x%X param 0x%X\n",
+		command, param);
+
+	/* base package + one parameter */
+	len = sizeof(*cmd) + sizeof(cmd->fm_cmd.data[0]);
+
+	skb = pf_data->alloc_skb(len, GFP_KERNEL);
+	if (!skb) {
+		dev_err(FM_DEV,
+			"send_fm_write_1_param: Could not allocate skb\n");
+		return -ENOMEM;
+	}
+
+	cmd = (struct fm_leg_cmd *)skb_put(skb, len);
+
+	cmd->length      = CG2900_FM_CMD_PARAM_LEN(len);
+	cmd->opcode      = CG2900_FM_GEN_ID_LEGACY;
+	cmd->read_write  = CG2900_FM_CMD_LEG_PARAM_WRITE;
+	cmd->fm_function = CG2900_FM_CMD_PARAM_WRITECOMMAND;
+	/* one parameter - builtin assumption for this function */
+	cmd->fm_cmd.head    = cpu_to_le16(cg2900_make_fm_cmd_id(command, 1));
+	cmd->fm_cmd.data[0] = cpu_to_le16(param);
+
+	cb_info->user = user;
+	dev_dbg(FM_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(FM_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished;
+	}
+
+	err = receive_fm_write_response(user, command);
+finished:
+	dev_dbg(FM_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * send_vs_stream_ctrl() - Formats an sends a CG2900_MC_VS_STREAM_CONTROL command.
+ * @user:	Audio user this command belongs to.
+ * @stream:	Stream id.
+ * @command:	Start/stop etc.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the bt_mutex held.
+ *
+ * While the HCI command allows for multiple streams in one command,
+ * this function only handles one.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_vs_stream_ctrl(struct audio_user *user, u8 stream, u8 command)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	struct mc_vs_stream_ctrl_cmd *cmd;
+	size_t len;
+	u8 vs_err;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV, "send_vs_stream_ctrl stream %d command %d\n", stream,
+		command);
+
+	/* basic length + one stream */
+	len = sizeof(*cmd) + sizeof(cmd->stream[0]);
+
+	skb = pf_data->alloc_skb(len, GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV, "send_vs_stream_ctrl:Could not allocate skb\n");
+		return -ENOMEM;
+	}
+
+	cmd = (struct mc_vs_stream_ctrl_cmd *)skb_put(skb, len);
+
+	cmd->opcode  = cpu_to_le16(CG2900_MC_VS_STREAM_CONTROL);
+	cmd->plen    = BT_PARAM_LEN(len);
+	cmd->command = command;
+
+	/* one stream */
+	cmd->n_streams  = 1;
+	cmd->stream[0] = stream;
+
+	cb_info->user = user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished;
+	}
+
+	/* All commands in PG2 API returns one byte with extra status */
+	err = receive_bt_cmd_complete(user,
+				      CG2900_MC_VS_STREAM_CONTROL,
+				      &vs_err, sizeof(vs_err));
+	if (err)
+		dev_err(BT_DEV,
+			"VS_STREAM_CONTROL - failed with error 0x%02x\n",
+			vs_err);
+
+finished:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * send_vs_create_stream() - Formats an sends a CG2900_MC_VS_CREATE_STREAM command.
+ * @user:	Audio user this command belongs to.
+ * @inport:	Stream id.
+ * @outport:	Start/stop etc.
+ * @order:	Activation order.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the bt_mutex held.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_vs_create_stream(struct audio_user *user, u8 inport,
+				 u8 outport, u8 order)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	struct mc_vs_create_stream_cmd *cmd;
+	s8 id;
+	u8 vs_err;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV,
+		"send_vs_create_stream inport %d outport %d order %d\n",
+		inport, outport, order);
+
+	id = new_stream_id(info);
+	if (id < 0) {
+		dev_err(BT_DEV, "No free stream id\n");
+		err = -EIO;
+		goto finished;
+	}
+
+	skb = pf_data->alloc_skb(sizeof(*cmd), GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV,
+			"send_vs_create_stream: Could not allocate skb\n");
+		err = -ENOMEM;
+		goto finished_release_id;
+	}
+
+	cmd = (struct mc_vs_create_stream_cmd *)skb_put(skb, sizeof(*cmd));
+
+	cmd->opcode  = cpu_to_le16(CG2900_MC_VS_CREATE_STREAM);
+	cmd->plen    = BT_PARAM_LEN(sizeof(*cmd));
+	cmd->id      = (u8)id;
+	cmd->inport  = inport;
+	cmd->outport = outport;
+	cmd->order   = order;
+
+	cb_info->user = user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished_release_id;
+	}
+
+	/* All commands in PG2 API returns one byte with extra status */
+	err = receive_bt_cmd_complete(user,
+				      CG2900_MC_VS_CREATE_STREAM,
+				      &vs_err, sizeof(vs_err));
+	if (err) {
+		dev_err(BT_DEV,
+			"VS_CREATE_STREAM - failed with error 0x%02x\n",
+			vs_err);
+		goto finished_release_id;
+	}
+
+	err = id;
+	goto finished;
+
+finished_release_id:
+	release_stream_id(info, id);
+finished:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * send_vs_port_cfg() - Formats an sends a CG2900_MC_VS_PORT_CONFIG command.
+ * @user:	Audio user this command belongs to.
+ * @port:	Port id to configure.
+ * @cfg:	Pointer to specific configuration.
+ * @cfglen:	Length of configuration.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the bt_mutex held.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_vs_port_cfg(struct audio_user *user, u8 port,
+			    const void *cfg, size_t cfglen)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	struct mc_vs_port_cfg_cmd *cmd;
+	void *ptr;
+	u8 vs_err;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV, "send_vs_port_cfg len %d\n", cfglen);
+
+	skb = pf_data->alloc_skb(sizeof(*cmd) + cfglen, GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV, "send_vs_port_cfg: Could not allocate skb\n");
+		return -ENOMEM;
+	}
+
+	/* Fill in common part */
+	cmd = (struct mc_vs_port_cfg_cmd *) skb_put(skb, sizeof(*cmd));
+	cmd->opcode = cpu_to_le16(CG2900_MC_VS_PORT_CONFIG);
+	cmd->plen = BT_PARAM_LEN(sizeof(*cmd) + cfglen);
+	cmd->type = port;
+
+	/* Copy specific configuration */
+	ptr = skb_put(skb, cfglen);
+	memcpy(ptr, cfg, cfglen);
+
+	/* Send */
+	cb_info->user = user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished;
+	}
+
+	/* All commands in PG2 API returns one byte with extra status */
+	err = receive_bt_cmd_complete(user, CG2900_MC_VS_PORT_CONFIG,
+				      &vs_err, sizeof(vs_err));
+	if (err)
+		dev_err(BT_DEV, "VS_PORT_CONFIG - failed with error 0x%02x\n",
+			vs_err);
+
+finished:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * set_dai_config_pg1() - Internal implementation of @cg2900_audio_set_dai_config for PG1 hardware.
+ * @audio_user:	Pointer to audio user struct.
+ * @config:	Pointer to the configuration to set.
+ *
+ * Sets the Digital Audio Interface (DAI) configuration for PG1
+ * hardware. This is and internal function and basic
+ * argument-verification should have been done by the caller.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -EACCESS if port is not supported.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int set_dai_config_pg1(struct audio_user *audio_user,
+			      struct cg2900_dai_config *config)
+{
+	int err = 0;
+	struct cg2900_dai_conf_i2s_pcm *i2s_pcm;
+	struct sk_buff *skb = NULL;
+	struct bt_vs_set_hw_cfg_cmd_i2s *i2s_cmd;
+	struct bt_vs_set_hw_cfg_cmd_pcm *pcm_cmd;
+	struct audio_info *info = audio_user->info;
+	struct cg2900_user_data *pf_data = dev_get_platdata(info->dev_bt);
+	struct audio_cb_info *cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV, "set_dai_config_pg1 port %d\n", config->port);
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any time on
+	 * each channel.
+	 */
+	mutex_lock(&info->bt_mutex);
+
+	/* Allocate the sk_buffer. The length is actually a max length since
+	 * length varies depending on logical transport.
+	 */
+	skb = pf_data->alloc_skb(CG2900_BT_LEN_VS_SET_HARDWARE_CONFIG,
+				 GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV, "set_dai_config_pg1: Could not allocate skb\n");
+		err = -ENOMEM;
+		goto finished_unlock_mutex;
+	}
+
+	/* Fill in hci-command according to received configuration */
+	switch (config->port) {
+	case PORT_0_I2S:
+		i2s_cmd = (struct bt_vs_set_hw_cfg_cmd_i2s *)
+			skb_put(skb, sizeof(*i2s_cmd));
+
+		i2s_cmd->opcode = cpu_to_le16(CG2900_BT_VS_SET_HARDWARE_CONFIG);
+		i2s_cmd->plen   = BT_PARAM_LEN(sizeof(*i2s_cmd));
+
+		i2s_cmd->vp_type = PORT_PROTOCOL_I2S;
+		i2s_cmd->port_id = 0x00; /* First/only I2S port */
+		i2s_cmd->half_period = config->conf.i2s.half_period;
+
+		i2s_cmd->master_slave = mc_i2s_role(config->conf.i2s.mode);
+
+		/* Store the new configuration */
+		mutex_lock(&info->management_mutex);
+		memcpy(&info->i2s_config, &config->conf.i2s,
+		       sizeof(config->conf.i2s));
+		info->i2s_config_known = true;
+		mutex_unlock(&info->management_mutex);
+		break;
+
+	case PORT_1_I2S_PCM:
+		pcm_cmd = (struct bt_vs_set_hw_cfg_cmd_pcm *)
+			skb_put(skb, sizeof(*pcm_cmd));
+
+		pcm_cmd->opcode = cpu_to_le16(CG2900_BT_VS_SET_HARDWARE_CONFIG);
+		pcm_cmd->plen   = BT_PARAM_LEN(sizeof(*pcm_cmd));
+
+		i2s_pcm = &config->conf.i2s_pcm;
+
+		/*
+		 * PG1 chips don't support I2S over the PCM/I2S bus,
+		 * and PG2 chips don't use this command
+		 */
+		if (i2s_pcm->protocol != PORT_PROTOCOL_PCM) {
+			dev_err(BT_DEV,
+				"I2S not supported over the PCM/I2S bus\n");
+			err = -EACCES;
+			goto error_handling_free_skb;
+		}
+
+		pcm_cmd->vp_type = PORT_PROTOCOL_PCM;
+		pcm_cmd->port_id = 0x00; /* First/only PCM port */
+
+		HWCONFIG_PCM_SET_MODE(pcm_cmd, mc_pcm_role(i2s_pcm->mode));
+
+		HWCONFIG_PCM_SET_DIR(pcm_cmd, 0, i2s_pcm->slot_0_dir);
+		HWCONFIG_PCM_SET_DIR(pcm_cmd, 1, i2s_pcm->slot_1_dir);
+		HWCONFIG_PCM_SET_DIR(pcm_cmd, 2, i2s_pcm->slot_2_dir);
+		HWCONFIG_PCM_SET_DIR(pcm_cmd, 3, i2s_pcm->slot_3_dir);
+
+		pcm_cmd->bit_clock = i2s_pcm->clk;
+		pcm_cmd->frame_len =
+			cpu_to_le16(get_fs_duration(i2s_pcm->duration));
+
+		/* Store the new configuration */
+		mutex_lock(&info->management_mutex);
+		memcpy(&info->i2s_pcm_config, &config->conf.i2s_pcm,
+		       sizeof(config->conf.i2s_pcm));
+		info->i2s_pcm_config_known = true;
+		mutex_unlock(&info->management_mutex);
+		break;
+
+	default:
+		dev_err(BT_DEV, "Unknown port configuration %d\n",
+			config->port);
+		err = -EACCES;
+		goto error_handling_free_skb;
+	};
+
+	cb_info->user = audio_user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	audio_user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		goto error_handling_free_skb;
+	}
+
+	err = receive_bt_cmd_complete(audio_user,
+				      CG2900_BT_VS_SET_HARDWARE_CONFIG,
+				      NULL, 0);
+
+	goto finished_unlock_mutex;
+
+error_handling_free_skb:
+	kfree_skb(skb);
+finished_unlock_mutex:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	audio_user->resp_state = IDLE;
+	mutex_unlock(&info->bt_mutex);
+	return err;
+}
+
+/**
+ * set_dai_config_pg2() - Internal implementation of @cg2900_audio_set_dai_config for PG2 hardware.
+ * @audio_user:	Pointer to audio user struct.
+ * @config:	Pointer to the configuration to set.
+ *
+ * Sets the Digital Audio Interface (DAI) configuration for PG2
+ * hardware. This is an internal function and basic
+ * argument-verification should have been done by the caller.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -EACCESS if port is not supported.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int set_dai_config_pg2(struct audio_user *audio_user,
+			      struct cg2900_dai_config *config)
+{
+	int err = 0;
+	struct cg2900_dai_conf_i2s *i2s;
+	struct cg2900_dai_conf_i2s_pcm *i2s_pcm;
+
+	struct mc_vs_port_cfg_i2s i2s_cfg;
+	struct mc_vs_port_cfg_pcm_i2s pcm_cfg;
+	struct audio_info *info = audio_user->info;
+
+	dev_dbg(BT_DEV, "set_dai_config_pg2 port %d\n", config->port);
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any time on
+	 * each channel.
+	 */
+	mutex_lock(&info->bt_mutex);
+
+	switch (config->port) {
+	case PORT_0_I2S:
+		i2s = &config->conf.i2s;
+
+		memset(&i2s_cfg, 0, sizeof(i2s_cfg)); /* just to be safe  */
+
+		/* master/slave */
+		PORTCFG_I2S_SET_ROLE(i2s_cfg, mc_i2s_role(i2s->mode));
+
+		PORTCFG_I2S_SET_HALFPERIOD(i2s_cfg, i2s->half_period);
+		PORTCFG_I2S_SET_CHANNELS(i2s_cfg,
+			mc_i2s_channel_select(i2s->channel_sel));
+		PORTCFG_I2S_SET_SRATE(i2s_cfg,
+			mc_i2s_sample_rate(i2s->sample_rate));
+		switch (i2s->word_width) {
+		case WORD_WIDTH_16:
+			PORTCFG_I2S_SET_WORDLEN(i2s_cfg, CG2900_MC_I2S_WORD_16);
+			break;
+		case WORD_WIDTH_32:
+			PORTCFG_I2S_SET_WORDLEN(i2s_cfg, CG2900_MC_I2S_WORD_32);
+			break;
+		}
+
+		/* Store the new configuration */
+		mutex_lock(&info->management_mutex);
+		memcpy(&(info->i2s_config), &(config->conf.i2s),
+		       sizeof(config->conf.i2s));
+		info->i2s_config_known = true;
+		mutex_unlock(&info->management_mutex);
+
+		/* Send */
+		err = send_vs_port_cfg(audio_user, CG2900_MC_PORT_I2S,
+				       &i2s_cfg, sizeof(i2s_cfg));
+		break;
+
+	case PORT_1_I2S_PCM:
+		i2s_pcm = &config->conf.i2s_pcm;
+
+		memset(&pcm_cfg, 0, sizeof(pcm_cfg)); /* just to be safe  */
+
+		/* master/slave */
+		PORTCFG_PCM_SET_ROLE(pcm_cfg, mc_pcm_role(i2s_pcm->mode));
+
+		/* set direction for all 4 slots */
+		PORTCFG_PCM_SET_DIR(pcm_cfg, 0, i2s_pcm->slot_0_dir);
+		PORTCFG_PCM_SET_DIR(pcm_cfg, 1, i2s_pcm->slot_1_dir);
+		PORTCFG_PCM_SET_DIR(pcm_cfg, 2, i2s_pcm->slot_2_dir);
+		PORTCFG_PCM_SET_DIR(pcm_cfg, 3, i2s_pcm->slot_3_dir);
+
+		/* set used SCO slots, other use cases not supported atm */
+		PORTCFG_PCM_SET_SCO_USED(pcm_cfg, 0, i2s_pcm->slot_0_used);
+		PORTCFG_PCM_SET_SCO_USED(pcm_cfg, 1, i2s_pcm->slot_1_used);
+		PORTCFG_PCM_SET_SCO_USED(pcm_cfg, 2, i2s_pcm->slot_2_used);
+		PORTCFG_PCM_SET_SCO_USED(pcm_cfg, 3, i2s_pcm->slot_3_used);
+
+		/* slot starts */
+		pcm_cfg.slot_start[0] = i2s_pcm->slot_0_start;
+		pcm_cfg.slot_start[1] = i2s_pcm->slot_1_start;
+		pcm_cfg.slot_start[2] = i2s_pcm->slot_2_start;
+		pcm_cfg.slot_start[3] = i2s_pcm->slot_3_start;
+
+		/* audio/voice sample-rate ratio */
+		PORTCFG_PCM_SET_RATIO(pcm_cfg, i2s_pcm->ratio);
+
+		/* PCM or I2S mode */
+		PORTCFG_PCM_SET_MODE(pcm_cfg, i2s_pcm->protocol);
+
+		pcm_cfg.frame_len = i2s_pcm->duration;
+
+		PORTCFG_PCM_SET_BITCLK(pcm_cfg, i2s_pcm->clk);
+		PORTCFG_PCM_SET_SRATE(pcm_cfg,
+			mc_pcm_sample_rate(i2s_pcm->sample_rate));
+
+		/* Store the new configuration */
+		mutex_lock(&info->management_mutex);
+		memcpy(&(info->i2s_pcm_config), &(config->conf.i2s_pcm),
+		       sizeof(config->conf.i2s_pcm));
+		info->i2s_pcm_config_known = true;
+		mutex_unlock(&info->management_mutex);
+
+		/* Send */
+		err = send_vs_port_cfg(audio_user, CG2900_MC_PORT_PCM_I2S,
+				       &pcm_cfg, sizeof(pcm_cfg));
+		break;
+
+	default:
+		dev_err(BT_DEV, "Unknown port configuration %d\n",
+			config->port);
+		err = -EACCES;
+	};
+
+	mutex_unlock(&info->bt_mutex);
+	return err;
+}
+
+/**
+ * struct i2s_fm_stream_config_priv - Helper struct for stream i2s-fm streams.
+ * @fm_config:	FM endpoint configuration.
+ * @rx:		true for FM-RX, false for FM-TX.
+ */
+struct i2s_fm_stream_config_priv {
+	struct cg2900_endpoint_config_fm	*fm_config;
+	bool					rx;
+
+};
+
+/**
+ * config_i2s_fm_stream() - Callback for @send_vs_session_config.
+ * @info:	Audio info structure.
+ * @_priv:	Pointer to a @i2s_fm_stream_config_priv struct.
+ * @cfg:	Pointer to stream config block in command packet.
+ *
+ * Fills in stream configuration for I2S-FM RX/TX.
+ */
+
+static void config_i2s_fm_stream(struct audio_info *info, void *_priv,
+				 struct session_config_stream *cfg)
+{
+	struct i2s_fm_stream_config_priv *priv = _priv;
+	struct session_config_vport *fm;
+	struct session_config_vport *i2s;
+
+	cfg->media_type = CG2900_BT_SESSION_MEDIA_TYPE_AUDIO;
+
+	if (info->i2s_config.channel_sel == CHANNEL_SELECTION_BOTH)
+		SESSIONCFG_SET_CHANNELS(cfg, CG2900_BT_MEDIA_CONFIG_STEREO);
+	else
+		SESSIONCFG_SET_CHANNELS(cfg, CG2900_BT_MEDIA_CONFIG_MONO);
+
+	SESSIONCFG_I2S_SET_SRATE(cfg,
+		session_config_sample_rate(priv->fm_config->sample_rate));
+
+	cfg->codec_type = CG2900_CODEC_TYPE_NONE;
+	/* codec mode and parameters not used  */
+
+	if (priv->rx) {
+		fm  = &cfg->inport;  /* FM is input */
+		i2s = &cfg->outport; /* I2S is output */
+	} else {
+		i2s = &cfg->inport;  /* I2S is input */
+		fm  = &cfg->outport; /* FM is output */
+	}
+
+	fm->type = CG2900_BT_VP_TYPE_FM;
+
+	i2s->type = CG2900_BT_VP_TYPE_I2S;
+	i2s->i2s.index   = CG2900_BT_SESSION_I2S_INDEX_I2S;
+	i2s->i2s.channel = info->i2s_config.channel_sel;
+}
+
+/**
+ * conn_start_i2s_to_fm_rx() - Start an audio stream connecting FM RX to I2S.
+ * @audio_user:		Audio user to check for.
+ * @stream_handle:	[out] Pointer where to store the stream handle.
+ *
+ * This function sets up an FM RX to I2S stream.
+ * It does this by first setting the output mode and then the configuration of
+ * the External Sample Rate Converter.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -ENOMEM upon allocation errors.
+ *   -EIO for other errors.
+ */
+static int conn_start_i2s_to_fm_rx(struct audio_user *audio_user,
+				   unsigned int *stream_handle)
+{
+	int err = 0;
+	union cg2900_endpoint_config_union *fm_config;
+	struct audio_info *info = audio_user->info;
+
+	dev_dbg(FM_DEV, "conn_start_i2s_to_fm_rx\n");
+
+	fm_config = find_endpoint(ENDPOINT_FM_RX, &info->endpoints);
+	if (!fm_config) {
+		dev_err(FM_DEV, "FM RX not configured before stream start\n");
+		return -EIO;
+	}
+
+	if (!(info->i2s_config_known)) {
+		dev_err(FM_DEV,
+			"I2S DAI not configured before stream start\n");
+		return -EIO;
+	}
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any
+	 * time on each channel.
+	 */
+	mutex_lock(&info->fm_mutex);
+	mutex_lock(&info->bt_mutex);
+
+	/*
+	 * Now set the output mode of the External Sample Rate Converter by
+	 * sending HCI_Write command with AUP_EXT_SetMode.
+	 */
+	err = send_fm_write_1_param(audio_user,
+				    CG2900_FM_CMD_ID_AUP_EXT_SET_MODE,
+				    CG2900_FM_CMD_AUP_EXT_SET_MODE_PARALLEL);
+	if (err)
+		goto finished_unlock_mutex;
+
+	/*
+	 * Now configure the External Sample Rate Converter by sending
+	 * HCI_Write command with AUP_EXT_SetControl.
+	 */
+	err = send_fm_write_1_param(
+		audio_user, CG2900_FM_CMD_ID_AUP_EXT_SET_CTRL,
+		fm_get_conversion(fm_config->fm.sample_rate));
+	if (err)
+		goto finished_unlock_mutex;
+
+	/* Set up the stream */
+	if (info->revision == CHIP_REV_PG1) {
+		struct i2s_fm_stream_config_priv stream_priv;
+
+		/* Now send HCI_VS_Set_Session_Configuration command */
+		stream_priv.fm_config = &fm_config->fm;
+		stream_priv.rx = true;
+		err = send_vs_session_config(audio_user, config_i2s_fm_stream,
+					     &stream_priv);
+	} else {
+		struct mc_vs_port_cfg_fm fm_cfg;
+
+		memset(&fm_cfg, 0, sizeof(fm_cfg));
+
+		/* Configure port FM RX */
+		/* Expects 0-3 - same as user API - so no conversion needed */
+		PORTCFG_FM_SET_SRATE(fm_cfg, (u8)fm_config->fm.sample_rate);
+
+		err = send_vs_port_cfg(audio_user, CG2900_MC_PORT_FM_RX_1,
+				       &fm_cfg, sizeof(fm_cfg));
+		if (err)
+			goto finished_unlock_mutex;
+
+		/* CreateStream */
+		err = send_vs_create_stream(audio_user,
+					    CG2900_MC_PORT_FM_RX_1,
+					    CG2900_MC_PORT_I2S,
+					    0); /* chip doesn't care */
+	}
+
+	if (err < 0)
+		goto finished_unlock_mutex;
+
+	/* Store the stream handle (used for start and stop stream) */
+	*stream_handle = (u8)err;
+	dev_dbg(FM_DEV, "stream_handle set to %d\n", *stream_handle);
+
+	/* Now start the stream */
+	if (info->revision == CHIP_REV_PG1)
+		err = send_vs_session_ctrl(audio_user, *stream_handle,
+					   CG2900_BT_SESSION_START);
+	else
+		err = send_vs_stream_ctrl(audio_user, *stream_handle,
+					  CG2900_MC_STREAM_START);
+	/*Let's delete a stream.*/
+	if (err < 0) {
+		dev_dbg(BT_DEV, "Could not start a stream.");
+		(void)send_vs_delete_stream(audio_user, *stream_handle);
+	}
+
+finished_unlock_mutex:
+	dev_dbg(FM_DEV, "New resp_state: IDLE\n");
+	audio_user->resp_state = IDLE;
+	mutex_unlock(&info->bt_mutex);
+	mutex_unlock(&info->fm_mutex);
+	return err;
+}
+
+/**
+ * conn_start_i2s_to_fm_tx() - Start an audio stream connecting FM TX to I2S.
+ * @audio_user:		Audio user to check for.
+ * @stream_handle:	[out] Pointer where to store the stream handle.
+ *
+ * This function sets up an I2S to FM TX stream.
+ * It does this by first setting the Audio Input source and then setting the
+ * configuration and input source of BT sample rate converter.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -ENOMEM upon allocation errors.
+ *   -EIO for other errors.
+ */
+static int conn_start_i2s_to_fm_tx(struct audio_user *audio_user,
+				   unsigned int *stream_handle)
+{
+	int err = 0;
+	union cg2900_endpoint_config_union *fm_config;
+	struct audio_info *info = audio_user->info;
+
+	dev_dbg(FM_DEV, "conn_start_i2s_to_fm_tx\n");
+
+	fm_config = find_endpoint(ENDPOINT_FM_TX, &info->endpoints);
+	if (!fm_config) {
+		dev_err(FM_DEV, "FM TX not configured before stream start\n");
+		return -EIO;
+	}
+
+	if (!(info->i2s_config_known)) {
+		dev_err(FM_DEV,
+			"I2S DAI not configured before stream start\n");
+		return -EIO;
+	}
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any time
+	 * on each channel.
+	 */
+	mutex_lock(&info->fm_mutex);
+	mutex_lock(&info->bt_mutex);
+
+	/*
+	 * Select Audio Input Source by sending HCI_Write command with
+	 * AIP_SetMode.
+	 */
+	dev_dbg(FM_DEV, "FM: AIP_SetMode\n");
+	err = send_fm_write_1_param(audio_user, CG2900_FM_CMD_ID_AIP_SET_MODE,
+				    CG2900_FM_CMD_AIP_SET_MODE_INPUT_DIG);
+	if (err)
+		goto finished_unlock_mutex;
+
+	/*
+	 * Now configure the BT sample rate converter by sending HCI_Write
+	 * command with AIP_BT_SetControl.
+	 */
+	dev_dbg(FM_DEV, "FM: AIP_BT_SetControl\n");
+	err = send_fm_write_1_param(
+		audio_user, CG2900_FM_CMD_ID_AIP_BT_SET_CTRL,
+		fm_get_conversion(fm_config->fm.sample_rate));
+	if (err)
+		goto finished_unlock_mutex;
+
+	/*
+	 * Now set input of the BT sample rate converter by sending HCI_Write
+	 * command with AIP_BT_SetMode.
+	 */
+	dev_dbg(FM_DEV, "FM: AIP_BT_SetMode\n");
+	err = send_fm_write_1_param(audio_user,
+				    CG2900_FM_CMD_ID_AIP_BT_SET_MODE,
+				    CG2900_FM_CMD_AIP_BT_SET_MODE_INPUT_PAR);
+	if (err)
+		goto finished_unlock_mutex;
+
+	/* Set up the stream */
+	if (info->revision == CHIP_REV_PG1) {
+		struct i2s_fm_stream_config_priv stream_priv;
+
+		/* Now send HCI_VS_Set_Session_Configuration command */
+		stream_priv.fm_config = &fm_config->fm;
+		stream_priv.rx = false;
+		err = send_vs_session_config(audio_user, config_i2s_fm_stream,
+					     &stream_priv);
+	} else {
+		struct mc_vs_port_cfg_fm fm_cfg;
+
+		memset(&fm_cfg, 0, sizeof(fm_cfg));
+
+		/* Configure port FM TX */
+		/* Expects 0-3 - same as user API - so no conversion needed */
+		PORTCFG_FM_SET_SRATE(fm_cfg, (u8)fm_config->fm.sample_rate);
+
+		err = send_vs_port_cfg(audio_user, CG2900_MC_PORT_FM_TX,
+				       &fm_cfg, sizeof(fm_cfg));
+		if (err)
+			goto finished_unlock_mutex;
+
+		/* CreateStream */
+		err = send_vs_create_stream(audio_user,
+					    CG2900_MC_PORT_I2S,
+					    CG2900_MC_PORT_FM_TX,
+					    0); /* chip doesn't care */
+	}
+
+	if (err < 0)
+		goto finished_unlock_mutex;
+
+	/* Store the stream handle (used for start and stop stream) */
+	*stream_handle = (u8)err;
+	dev_dbg(FM_DEV, "stream_handle set to %d\n", *stream_handle);
+
+	/* Now start the stream */
+	if (info->revision == CHIP_REV_PG1)
+		err = send_vs_session_ctrl(audio_user, *stream_handle,
+					   CG2900_BT_SESSION_START);
+	else
+		err = send_vs_stream_ctrl(audio_user, *stream_handle,
+					  CG2900_MC_STREAM_START);
+	/* Let's delete and release stream.*/
+	if (err < 0) {
+		dev_dbg(BT_DEV, "Could not start a stream.");
+		(void)send_vs_delete_stream(audio_user, *stream_handle);
+	}
+
+finished_unlock_mutex:
+	dev_dbg(FM_DEV, "New resp_state: IDLE\n");
+	audio_user->resp_state = IDLE;
+	mutex_unlock(&info->bt_mutex);
+	mutex_unlock(&info->fm_mutex);
+	return err;
+}
+
+/**
+ * config_pcm_sco_stream() - Callback for @send_vs_session_config.
+ * @info:	Audio info structure.
+ * @_priv:	Pointer to a @cg2900_endpoint_config_sco_in_out struct.
+ * @cfg:	Pointer to stream config block in command packet.
+ *
+ * Fills in stream configuration for PCM-SCO.
+ */
+static void config_pcm_sco_stream(struct audio_info *info, void *_priv,
+				  struct session_config_stream *cfg)
+{
+	struct cg2900_endpoint_config_sco_in_out *sco_ep = _priv;
+
+	cfg->media_type = CG2900_BT_SESSION_MEDIA_TYPE_AUDIO;
+
+	SESSIONCFG_SET_CHANNELS(cfg, CG2900_BT_MEDIA_CONFIG_MONO);
+	SESSIONCFG_I2S_SET_SRATE(cfg,
+		session_config_sample_rate(sco_ep->sample_rate));
+
+	cfg->codec_type = CG2900_CODEC_TYPE_NONE;
+	/* codec mode and parameters not used  */
+
+	cfg->inport.type = CG2900_BT_VP_TYPE_BT_SCO;
+	cfg->inport.sco.acl_handle = cpu_to_le16(DEFAULT_SCO_HANDLE);
+
+	cfg->outport.type = CG2900_BT_VP_TYPE_PCM;
+	cfg->outport.pcm.index = CG2900_BT_SESSION_PCM_INDEX_PCM_I2S;
+
+	SESSIONCFG_PCM_SET_USED(cfg->outport, 0,
+				info->i2s_pcm_config.slot_0_used);
+	SESSIONCFG_PCM_SET_USED(cfg->outport, 1,
+				info->i2s_pcm_config.slot_1_used);
+	SESSIONCFG_PCM_SET_USED(cfg->outport, 2,
+				info->i2s_pcm_config.slot_2_used);
+	SESSIONCFG_PCM_SET_USED(cfg->outport, 3,
+				info->i2s_pcm_config.slot_3_used);
+
+	cfg->outport.pcm.slot_start[0] =
+		info->i2s_pcm_config.slot_0_start;
+	cfg->outport.pcm.slot_start[1] =
+		info->i2s_pcm_config.slot_1_start;
+	cfg->outport.pcm.slot_start[2] =
+		info->i2s_pcm_config.slot_2_start;
+	cfg->outport.pcm.slot_start[3] =
+		info->i2s_pcm_config.slot_3_start;
+}
+
+/**
+ * conn_start_pcm_to_sco() - Start an audio stream connecting Bluetooth (e)SCO to PCM_I2S.
+ * @audio_user:		Audio user to check for.
+ * @stream_handle:	[out] Pointer where to store the stream handle.
+ *
+ * This function sets up a BT to_from PCM_I2S stream. It does this by
+ * first setting the Session configuration and then starting the Audio
+ * Stream.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -ENOMEM upon allocation errors.
+ *   Errors from @cg2900_write
+ *   -EIO for other errors.
+ */
+static int conn_start_pcm_to_sco(struct audio_user *audio_user,
+				 unsigned int *stream_handle)
+{
+	int err = 0;
+	union cg2900_endpoint_config_union *bt_config;
+	struct audio_info *info = audio_user->info;
+
+	dev_dbg(BT_DEV, "conn_start_pcm_to_sco\n");
+
+	bt_config = find_endpoint(ENDPOINT_BT_SCO_INOUT, &info->endpoints);
+	if (!bt_config) {
+		dev_err(BT_DEV, "BT not configured before stream start\n");
+		return -EIO;
+	}
+
+	if (!(info->i2s_pcm_config_known)) {
+		dev_err(BT_DEV,
+			"I2S_PCM DAI not configured before stream start\n");
+		return -EIO;
+	}
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any time on each
+	 * channel.
+	 */
+	mutex_lock(&info->bt_mutex);
+
+	/* Set up the stream */
+	if (info->revision == CHIP_REV_PG1) {
+		err = send_vs_session_config(audio_user, config_pcm_sco_stream,
+					     &bt_config->sco);
+	} else {
+		struct mc_vs_port_cfg_sco sco_cfg;
+
+		/* zero codec params etc */
+		memset(&sco_cfg, 0, sizeof(sco_cfg));
+		sco_cfg.acl_id = DEFAULT_SCO_HANDLE;
+		PORTCFG_SCO_SET_WBS(sco_cfg, 0); /* No WBS yet */
+		PORTCFG_SCO_SET_CODEC(sco_cfg, CG2900_CODEC_TYPE_NONE);
+
+		err = send_vs_port_cfg(audio_user, CG2900_MC_PORT_BT_SCO,
+				       &sco_cfg, sizeof(sco_cfg));
+		if (err)
+			goto finished_unlock_mutex;
+
+		/* CreateStream */
+		err = send_vs_create_stream(audio_user,
+					    CG2900_MC_PORT_PCM_I2S,
+					    CG2900_MC_PORT_BT_SCO,
+					    0); /* chip doesn't care */
+	}
+
+	if (err < 0)
+		goto finished_unlock_mutex;
+
+	/* Store the stream handle (used for start and stop stream) */
+	*stream_handle = (u8)err;
+	dev_dbg(BT_DEV, "stream_handle set to %d\n", *stream_handle);
+
+	/* Now start the stream */
+	if (info->revision == CHIP_REV_PG1)
+		err = send_vs_session_ctrl(audio_user, *stream_handle,
+					   CG2900_BT_SESSION_START);
+	else
+		err = send_vs_stream_ctrl(audio_user, *stream_handle,
+					  CG2900_MC_STREAM_START);
+	/* Let's delete and release stream.*/
+	if (err < 0) {
+		dev_dbg(BT_DEV, "Could not start a stream.");
+		(void)send_vs_delete_stream(audio_user, *stream_handle);
+	}
+
+finished_unlock_mutex:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	audio_user->resp_state = IDLE;
+	mutex_unlock(&info->bt_mutex);
+	return err;
+}
+
+/**
+ * conn_stop_stream() - Stops an audio stream defined by @stream_handle.
+ * @audio_user:		Audio user to check for.
+ * @stream_handle:	Handle of the audio stream.
+ *
+ * This function is used to stop an audio stream defined by a stream
+ * handle. It does this by first stopping the stream and then
+ * resetting the session/stream.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -ENOMEM upon allocation errors.
+ *   Errors from @cg2900_write.
+ *   -EIO for other errors.
+ */
+static int conn_stop_stream(struct audio_user *audio_user,
+			    unsigned int stream_handle)
+{
+	int err = 0;
+	struct audio_info *info = audio_user->info;
+
+	dev_dbg(BT_DEV, "conn_stop_stream handle %d\n", stream_handle);
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any
+	 * time on each channel.
+	 */
+	mutex_lock(&info->bt_mutex);
+
+	/* Now stop the stream */
+	if (info->revision == CHIP_REV_PG1)
+		err = send_vs_session_ctrl(audio_user, stream_handle,
+					   CG2900_BT_SESSION_STOP);
+	else
+		err = send_vs_stream_ctrl(audio_user, stream_handle,
+					  CG2900_MC_STREAM_STOP);
+	if (err)
+		goto finished_unlock_mutex;
+
+	err = send_vs_delete_stream(audio_user, stream_handle);
+
+finished_unlock_mutex:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	audio_user->resp_state = IDLE;
+	mutex_unlock(&info->bt_mutex);
+	return err;
+}
+
+/**
+ * cg2900_audio_get_devices() - Returns connected CG2900 Audio devices.
+ * @devices:	Array of CG2900 Audio devices.
+ * @size:	Max number of devices in array.
+ *
+ * Returns:
+ *   0 if no devices exist.
+ *   > 0 is the number of devices inserted in the list.
+ *   -EINVAL upon bad input parameter.
+ */
+int cg2900_audio_get_devices(struct device *devices[], __u8 size)
+{
+	struct list_head *cursor;
+	struct audio_info *tmp;
+	int i = 0;
+
+	if (!size) {
+		pr_err("No space to insert devices into list\n");
+		return 0;
+	}
+
+	if (!devices) {
+		pr_err("NULL submitted as devices array\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Go through and store the devices. If NULL is supplied for dev
+	 * just return first device found.
+	 */
+	list_for_each(cursor, &cg2900_audio_devices) {
+		tmp = list_entry(cursor, struct audio_info, list);
+		devices[i] = tmp->parent;
+		i++;
+		if (i == size)
+			break;
+	}
+	return i;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_get_devices);
+
+/**
+ * cg2900_audio_open() - Opens a session to the ST-Ericsson CG2900 Audio control interface.
+ * @session:	[out] Address where to store the session identifier.
+ *		Allocated by caller, must not be NULL.
+ * @parent:	Parent device representing the CG2900 controller connected.
+ *		If NULL is supplied the first available device is used.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EACCES if no info structure can be found.
+ *   -EINVAL upon bad input parameter.
+ *   -ENOMEM upon allocation failure.
+ *   -EMFILE if no more user session could be opened.
+ *   -EIO upon failure to register to CG2900.
+ *   Error codes from get_info.
+ */
+int cg2900_audio_open(unsigned int *session, struct device *parent)
+{
+	int err = 0;
+	int i;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data_bt;
+	struct cg2900_user_data *pf_data_fm;
+
+	pr_debug("cg2900_audio_open");
+
+	info = get_info(parent);
+	if (!info) {
+		pr_err("No audio info exist");
+		return -EACCES;
+	} else if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	if (!session) {
+		pr_err("NULL supplied as session");
+		return -EINVAL;
+	}
+
+	mutex_lock(&info->management_mutex);
+
+	*session = 0;
+
+	/*
+	 * First find a free session to use and allocate the session structure.
+	 */
+	for (i = FIRST_USER;
+	     i < MAX_NBR_OF_USERS && cg2900_audio_sessions[i];
+	     i++)
+		; /* Just loop until found or end reached */
+
+	if (i >= MAX_NBR_OF_USERS) {
+		pr_err("Couldn't find free user");
+		err = -EMFILE;
+		goto finished;
+	}
+
+	cg2900_audio_sessions[i] =
+		kzalloc(sizeof(*(cg2900_audio_sessions[0])), GFP_KERNEL);
+	if (!cg2900_audio_sessions[i]) {
+		pr_err("Could not allocate user");
+		err = -ENOMEM;
+		goto finished;
+	}
+	pr_debug("Found free session %d", i);
+	*session = i;
+	info->nbr_of_users_active++;
+
+	cg2900_audio_sessions[*session]->resp_state = IDLE;
+	cg2900_audio_sessions[*session]->session = *session;
+	cg2900_audio_sessions[*session]->info = info;
+
+	pf_data_bt = dev_get_platdata(info->dev_bt);
+	pf_data_fm = dev_get_platdata(info->dev_fm);
+
+	if (info->nbr_of_users_active == 1) {
+		struct cg2900_rev_data rev_data;
+
+		/*
+		 * First user so register to CG2900 Core.
+		 * First the BT audio device.
+		 */
+		err = pf_data_bt->open(pf_data_bt);
+		if (err) {
+			dev_err(BT_DEV, "Failed to open BT audio channel\n");
+			goto error_handling;
+		}
+
+		/* Then the FM audio device */
+		err = pf_data_fm->open(pf_data_fm);
+		if (err) {
+			dev_err(FM_DEV, "Failed to open FM audio channel\n");
+			goto error_handling;
+		}
+
+		/* Read chip revision data */
+		if (!pf_data_bt->get_local_revision(pf_data_bt, &rev_data)) {
+			pr_err("Couldn't retrieve revision data");
+			err = -EIO;
+			goto error_handling;
+		}
+
+		/* Decode revision data */
+		switch (rev_data.revision) {
+		case CG2900_PG1_REV:
+		case CG2900_PG1_SPECIAL_REV:
+			info->revision = CHIP_REV_PG1;
+			break;
+
+		case CG2900_PG2_REV:
+			info->revision = CHIP_REV_PG2;
+			break;
+
+		default:
+			pr_err("Chip rev 0x%04X sub 0x%04X not supported",
+			       rev_data.revision, rev_data.sub_version);
+			err = -EIO;
+			goto error_handling;
+		}
+
+		info->state = OPENED;
+	}
+
+	pr_info("Session %d opened", *session);
+
+	goto finished;
+
+error_handling:
+	if (pf_data_fm->opened)
+		pf_data_fm->close(pf_data_fm);
+	if (pf_data_bt->opened)
+		pf_data_bt->close(pf_data_bt);
+	info->nbr_of_users_active--;
+	kfree(cg2900_audio_sessions[*session]);
+	cg2900_audio_sessions[*session] = NULL;
+finished:
+	mutex_unlock(&info->management_mutex);
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_open);
+
+/**
+ * cg2900_audio_close() - Closes an opened session to the ST-Ericsson CG2900 audio control interface.
+ * @session:	[in_out] Pointer to session identifier to close.
+ *		Will be 0 after this call.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter.
+ *   -EIO if driver has not been opened.
+ *   -EACCES if session has not opened.
+ */
+int cg2900_audio_close(unsigned int *session)
+{
+	int err = 0;
+	struct audio_user *audio_user;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data_bt;
+	struct cg2900_user_data *pf_data_fm;
+
+	pr_debug("cg2900_audio_close");
+
+	if (!session) {
+		pr_err("NULL pointer supplied");
+		return -EINVAL;
+	}
+
+	audio_user = get_session_user(*session);
+	if (!audio_user) {
+		pr_err("Invalid session ID");
+		return -EINVAL;
+	}
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	mutex_lock(&info->management_mutex);
+
+	pf_data_bt = dev_get_platdata(info->dev_bt);
+	pf_data_fm = dev_get_platdata(info->dev_fm);
+
+	if (!cg2900_audio_sessions[*session]) {
+		dev_err(BT_DEV, "Session %d not opened\n", *session);
+		err = -EACCES;
+		goto err_unlock_mutex;
+	}
+
+	kfree(cg2900_audio_sessions[*session]);
+	cg2900_audio_sessions[*session] = NULL;
+
+	info->nbr_of_users_active--;
+	if (info->nbr_of_users_active == 0) {
+		/* No more sessions open. Close channels */
+		pf_data_fm->close(pf_data_fm);
+		pf_data_bt->close(pf_data_bt);
+		info->state = CLOSED;
+	}
+
+	dev_info(BT_DEV, "Session %d closed\n", *session);
+
+	*session = 0;
+
+err_unlock_mutex:
+	mutex_unlock(&info->management_mutex);
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_close);
+
+/**
+ * cg2900_audio_set_dai_config() -  Sets the Digital Audio Interface configuration.
+ * @session:	Session identifier this call is related to.
+ * @config:	Pointer to the configuration to set.
+ *		Allocated by caller, must not be NULL.
+ *
+ * Sets the Digital Audio Interface (DAI) configuration. The DAI is the external
+ * interface between the combo chip and the platform.
+ * For example the PCM or I2S interface.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter.
+ *   -EIO if driver has not been opened.
+ *   -ENOMEM upon allocation failure.
+ *   -EACCES if trying to set unsupported configuration.
+ *   Errors from @receive_bt_cmd_complete.
+ */
+int cg2900_audio_set_dai_config(unsigned int session,
+				struct cg2900_dai_config *config)
+{
+	int err = 0;
+	struct audio_user *audio_user;
+	struct audio_info *info;
+
+	pr_debug("cg2900_audio_set_dai_config session %d", session);
+
+	audio_user = get_session_user(session);
+	if (!audio_user)
+		return -EINVAL;
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	/* Different commands are used for PG1 and PG2 */
+	if (info->revision == CHIP_REV_PG1)
+		err = set_dai_config_pg1(audio_user, config);
+	else if (info->revision == CHIP_REV_PG2)
+		err = set_dai_config_pg2(audio_user, config);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_set_dai_config);
+
+/**
+ * cg2900_audio_get_dai_config() - Gets the current Digital Audio Interface configuration.
+ * @session:	Session identifier this call is related to.
+ * @config:	[out] Pointer to the configuration to get.
+ *		Allocated by caller, must not be NULL.
+ *
+ * Gets the current Digital Audio Interface configuration. Currently this method
+ * can only be called after some one has called
+ * cg2900_audio_set_dai_config(), there is today no way of getting
+ * the static settings file parameters from this method.
+ * Note that the @port parameter within @config must be set when calling this
+ * function so that the ST-Ericsson CG2900 Audio driver will know which
+ * configuration to return.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter.
+ *   -EIO if driver has not been opened or configuration has not been set.
+ */
+int cg2900_audio_get_dai_config(unsigned int session,
+				struct cg2900_dai_config *config)
+{
+	int err = 0;
+	struct audio_user *audio_user;
+	struct audio_info *info;
+
+	pr_debug("cg2900_audio_get_dai_config session %d", session);
+
+	if (!config) {
+		pr_err("NULL supplied as config structure");
+		return -EINVAL;
+	}
+
+	audio_user = get_session_user(session);
+	if (!audio_user)
+		return -EINVAL;
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	/*
+	 * Return DAI configuration based on the received port.
+	 * If port has not been configured return error.
+	 */
+	switch (config->port) {
+	case PORT_0_I2S:
+		mutex_lock(&info->management_mutex);
+		if (info->i2s_config_known)
+			memcpy(&config->conf.i2s,
+			       &info->i2s_config,
+			       sizeof(config->conf.i2s));
+		else
+			err = -EIO;
+		mutex_unlock(&info->management_mutex);
+		break;
+
+	case PORT_1_I2S_PCM:
+		mutex_lock(&info->management_mutex);
+		if (info->i2s_pcm_config_known)
+			memcpy(&config->conf.i2s_pcm,
+			       &info->i2s_pcm_config,
+			       sizeof(config->conf.i2s_pcm));
+		else
+			err = -EIO;
+		mutex_unlock(&info->management_mutex);
+		break;
+
+	default:
+		dev_err(BT_DEV, "Unknown port configuration %d\n",
+			config->port);
+		err = -EIO;
+		break;
+	};
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_get_dai_config);
+
+/**
+ * cg2900_audio_config_endpoint() - Configures one endpoint in the combo chip's audio system.
+ * @session:	Session identifier this call is related to.
+ * @config:	Pointer to the endpoint's configuration structure.
+ *
+ * Configures one endpoint in the combo chip's audio system.
+ * Supported @endpoint_id values are:
+ *  * ENDPOINT_BT_SCO_INOUT
+ *  * ENDPOINT_BT_A2DP_SRC
+ *  * ENDPOINT_FM_RX
+ *  * ENDPOINT_FM_TX
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter.
+ *   -EIO if driver has not been opened.
+ *   -EACCES if supplied cg2900_dai_config struct contains not supported
+ *   endpoint_id.
+ */
+int cg2900_audio_config_endpoint(unsigned int session,
+				 struct cg2900_endpoint_config *config)
+{
+	struct audio_user *audio_user;
+	struct audio_info *info;
+
+	pr_debug("cg2900_audio_config_endpoint\n");
+
+	if (!config) {
+		pr_err("NULL supplied as configuration structure");
+		return -EINVAL;
+	}
+
+	audio_user = get_session_user(session);
+	if (!audio_user)
+		return -EINVAL;
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	switch (config->endpoint_id) {
+	case ENDPOINT_BT_SCO_INOUT:
+	case ENDPOINT_BT_A2DP_SRC:
+	case ENDPOINT_FM_RX:
+	case ENDPOINT_FM_TX:
+		add_endpoint(config, &info->endpoints);
+		break;
+
+	case ENDPOINT_PORT_0_I2S:
+	case ENDPOINT_PORT_1_I2S_PCM:
+	case ENDPOINT_SLIMBUS_VOICE:
+	case ENDPOINT_SLIMBUS_AUDIO:
+	case ENDPOINT_BT_A2DP_SNK:
+	case ENDPOINT_ANALOG_OUT:
+	case ENDPOINT_DSP_AUDIO_IN:
+	case ENDPOINT_DSP_AUDIO_OUT:
+	case ENDPOINT_DSP_VOICE_IN:
+	case ENDPOINT_DSP_VOICE_OUT:
+	case ENDPOINT_DSP_TONE_IN:
+	case ENDPOINT_BURST_BUFFER_IN:
+	case ENDPOINT_BURST_BUFFER_OUT:
+	case ENDPOINT_MUSIC_DECODER:
+	case ENDPOINT_HCI_AUDIO_IN:
+	default:
+		dev_err(BT_DEV, "Unsupported endpoint_id %d\n",
+			config->endpoint_id);
+		return -EACCES;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_config_endpoint);
+
+static bool is_dai_port(enum cg2900_audio_endpoint_id ep)
+{
+	/* These are the only supported ones */
+	return (ep == ENDPOINT_PORT_0_I2S) || (ep == ENDPOINT_PORT_1_I2S_PCM);
+}
+
+/**
+ * cg2900_audio_start_stream() - Connects two endpoints and starts the audio stream.
+ * @session:		Session identifier this call is related to.
+ * @ep_1:		One of the endpoints, no relation to direction or role.
+ * @ep_2:		The other endpoint, no relation to direction or role.
+ * @stream_handle:	Pointer where to store the stream handle.
+ *			Allocated by caller, must not be NULL.
+ *
+ * Connects two endpoints and starts the audio stream.
+ * Note that the endpoints need to be configured before the stream is started;
+ * DAI endpoints, such as ENDPOINT_PORT_0_I2S, are
+ * configured through @cg2900_audio_set_dai_config() while other
+ * endpoints are configured through @cg2900_audio_config_endpoint().
+ *
+ * Supported @endpoint_id values are:
+ *  * ENDPOINT_PORT_0_I2S
+ *  * ENDPOINT_PORT_1_I2S_PCM
+ *  * ENDPOINT_BT_SCO_INOUT
+ *  * ENDPOINT_FM_RX
+ *  * ENDPOINT_FM_TX
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter or unsupported configuration.
+ *   -EIO if driver has not been opened.
+ *   Errors from @conn_start_i2s_to_fm_rx, @conn_start_i2s_to_fm_tx, and
+ *   @conn_start_pcm_to_sco.
+ */
+int cg2900_audio_start_stream(unsigned int session,
+			      enum cg2900_audio_endpoint_id ep_1,
+			      enum cg2900_audio_endpoint_id ep_2,
+			      unsigned int *stream_handle)
+{
+	int err;
+	struct audio_user *audio_user;
+	struct audio_info *info;
+
+	pr_debug("cg2900_audio_start_stream session %d ep_1 %d ep_2 %d",
+		 session, ep_1, ep_2);
+
+	audio_user = get_session_user(session);
+	if (!audio_user)
+		return -EINVAL;
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	/* Put digital interface in ep_1 to simplify comparison below */
+	if (!is_dai_port(ep_1)) {
+		/* Swap endpoints */
+		enum cg2900_audio_endpoint_id t = ep_1;
+		ep_1 = ep_2;
+		ep_2 = t;
+	}
+
+	if (ep_1 == ENDPOINT_PORT_1_I2S_PCM && ep_2 == ENDPOINT_BT_SCO_INOUT) {
+		err = conn_start_pcm_to_sco(audio_user, stream_handle);
+	} else if (ep_1 == ENDPOINT_PORT_0_I2S && ep_2 == ENDPOINT_FM_RX) {
+		err = conn_start_i2s_to_fm_rx(audio_user, stream_handle);
+	} else if (ep_1 == ENDPOINT_PORT_0_I2S && ep_2 == ENDPOINT_FM_TX) {
+		err = conn_start_i2s_to_fm_tx(audio_user, stream_handle);
+	} else {
+		dev_err(BT_DEV, "Endpoint config not handled: ep1: %d, "
+			"ep2: %d\n", ep_1, ep_2);
+		err = -EINVAL;
+	}
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_start_stream);
+
+/**
+ * cg2900_audio_stop_stream() - Stops a stream and disconnects the endpoints.
+ * @session:		Session identifier this call is related to.
+ * @stream_handle:	Handle to the stream to stop.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter.
+ *   -EIO if driver has not been opened.
+ */
+int cg2900_audio_stop_stream(unsigned int session, unsigned int stream_handle)
+{
+	struct audio_user *audio_user;
+	struct audio_info *info;
+
+	pr_debug("cg2900_audio_stop_stream handle %d", stream_handle);
+
+	audio_user = get_session_user(session);
+	if (!audio_user)
+		return -EINVAL;
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	return conn_stop_stream(audio_user, stream_handle);
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_stop_stream);
+
+/**
+ * audio_dev_open() - Open char device.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation failed.
+ *   Errors from @cg2900_audio_open.
+ */
+static int audio_dev_open(struct inode *inode, struct file *filp)
+{
+	int err;
+	struct char_dev_info *char_dev_info;
+	int minor;
+	struct audio_info *info = NULL;
+	struct audio_info *tmp;
+	struct list_head *cursor;
+
+	pr_debug("audio_dev_open");
+
+	minor = iminor(inode);
+
+	/* Find the info struct for this file */
+	list_for_each(cursor, &cg2900_audio_devices) {
+		tmp = list_entry(cursor, struct audio_info, list);
+		if (tmp->misc_dev.minor == minor) {
+			info = tmp;
+			break;
+		}
+	}
+	if (!info) {
+		pr_err("Could not identify device in inode");
+		return -EINVAL;
+	}
+
+	/*
+	 * Allocate the char dev info structure. It will be stored inside
+	 * the file pointer and supplied when file_ops are called.
+	 * It's free'd in audio_dev_release.
+	 */
+	char_dev_info = kzalloc(sizeof(*char_dev_info), GFP_KERNEL);
+	if (!char_dev_info) {
+		dev_err(BT_DEV, "Couldn't allocate char_dev_info\n");
+		return -ENOMEM;
+	}
+	filp->private_data = char_dev_info;
+	char_dev_info->info = info;
+
+	mutex_init(&char_dev_info->management_mutex);
+	mutex_init(&char_dev_info->rw_mutex);
+	skb_queue_head_init(&char_dev_info->rx_queue);
+
+	mutex_lock(&char_dev_info->management_mutex);
+	err = cg2900_audio_open(&char_dev_info->session, info->dev_bt->parent);
+	mutex_unlock(&char_dev_info->management_mutex);
+	if (err) {
+		dev_err(BT_DEV, "Failed to open CG2900 Audio driver (%d)\n",
+			err);
+		goto error_handling_free_mem;
+	}
+
+	return 0;
+
+error_handling_free_mem:
+	kfree(char_dev_info);
+	filp->private_data = NULL;
+	return err;
+}
+
+/**
+ * audio_dev_release() - Release char device.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EBADF if NULL pointer was supplied in private data.
+ *   Errors from @cg2900_audio_close.
+ */
+static int audio_dev_release(struct inode *inode, struct file *filp)
+{
+	int err = 0;
+	struct char_dev_info *dev = filp->private_data;
+	struct audio_info *info = dev->info;
+
+	dev_dbg(BT_DEV, "audio_dev_release\n");
+
+	mutex_lock(&dev->management_mutex);
+	err = cg2900_audio_close(&dev->session);
+	if (err)
+		/*
+		 * Just print the error. Still free the char_dev_info since we
+		 * don't know the filp structure is valid after this call
+		 */
+		dev_err(BT_DEV, "Error %d when closing CG2900 audio driver\n",
+			err);
+
+	mutex_unlock(&dev->management_mutex);
+
+	kfree(dev);
+	filp->private_data = NULL;
+
+	return err;
+}
+
+/**
+ * audio_dev_read() - Return information to the user from last @write call.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Received buffer.
+ * @count:	Size of buffer.
+ * @f_pos:	Position in buffer.
+ *
+ * The audio_dev_read() function returns information from
+ * the last @write call to same char device.
+ * The data is in the following format:
+ *   * OpCode of command for this data
+ *   * Data content (Length of data is determined by the command OpCode, i.e.
+ *     fixed for each command)
+ *
+ * Returns:
+ *   Bytes successfully read (could be 0).
+ *   -EBADF if NULL pointer was supplied in private data.
+ *   -EFAULT if copy_to_user fails.
+ *   -ENOMEM upon allocation failure.
+ */
+static ssize_t audio_dev_read(struct file *filp, char __user *buf, size_t count,
+			      loff_t *f_pos)
+{
+	struct char_dev_info *dev = filp->private_data;
+	struct audio_info *info = dev->info;
+	unsigned int bytes_to_copy;
+	int err = 0;
+	struct sk_buff *skb;
+
+	dev_dbg(BT_DEV, "audio_dev_read count %d\n", count);
+
+	mutex_lock(&dev->rw_mutex);
+
+	skb = skb_dequeue(&dev->rx_queue);
+	if (!skb) {
+		/* No data to read */
+		bytes_to_copy = 0;
+		goto finished;
+	}
+
+	bytes_to_copy = min(count, (unsigned int)(skb->len));
+
+	err = copy_to_user(buf, skb->data, bytes_to_copy);
+	if (err) {
+		dev_err(BT_DEV, "copy_to_user error %d\n", err);
+		skb_queue_head(&dev->rx_queue, skb);
+		err = -EFAULT;
+		goto error_handling;
+	}
+
+	skb_pull(skb, bytes_to_copy);
+
+	if (skb->len > 0)
+		skb_queue_head(&dev->rx_queue, skb);
+	else
+		kfree_skb(skb);
+
+	goto finished;
+
+error_handling:
+	mutex_unlock(&dev->rw_mutex);
+	return (ssize_t)err;
+finished:
+	mutex_unlock(&dev->rw_mutex);
+	return bytes_to_copy;
+}
+
+/**
+ * audio_dev_write() - Call CG2900 Audio API function.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Write buffer.
+ * @count:	Size of the buffer write.
+ * @f_pos:	Position of buffer.
+ *
+ * audio_dev_write() function executes supplied data and
+ * interprets it as if it was a function call to the CG2900 Audio API.
+ * The data is according to:
+ *   * OpCode (4 bytes, see API).
+ *   * Data according to OpCode (see API). No padding between parameters.
+ *
+ * Returns:
+ *   Bytes successfully written (could be 0). Equals input @count if successful.
+ *   -EBADF if NULL pointer was supplied in private data.
+ *   -EFAULT if copy_from_user fails.
+ *   Error codes from all CG2900 Audio API functions.
+ */
+static ssize_t audio_dev_write(struct file *filp, const char __user *buf,
+			       size_t count, loff_t *f_pos)
+{
+	u8 *rec_data;
+	struct char_dev_info *dev = filp->private_data;
+	struct audio_info *info;
+	int err = 0;
+	int op_code = 0;
+	u8 *curr_data;
+	unsigned int stream_handle;
+	struct cg2900_dai_config dai_config;
+	struct cg2900_endpoint_config ep_config;
+	enum cg2900_audio_endpoint_id ep_1;
+	enum cg2900_audio_endpoint_id ep_2;
+	int bytes_left = count;
+
+	pr_debug("audio_dev_write count %d", count);
+
+	if (!dev) {
+		pr_err("No dev supplied in private data");
+		return -EBADF;
+	}
+	info = dev->info;
+
+	rec_data = kmalloc(count, GFP_KERNEL);
+	if (!rec_data) {
+		dev_err(BT_DEV, "kmalloc failed (%d bytes)\n", count);
+		return -ENOMEM;
+	}
+
+	mutex_lock(&dev->rw_mutex);
+
+	err = copy_from_user(rec_data, buf, count);
+	if (err) {
+		dev_err(BT_DEV, "copy_from_user failed (%d)\n", err);
+		err = -EFAULT;
+		goto finished_mutex_unlock;
+	}
+
+	/* Initialize temporary data pointer used to traverse the packet */
+	curr_data = rec_data;
+
+	op_code = curr_data[0];
+	/* OpCode is int size to keep data int aligned */
+	curr_data += sizeof(unsigned int);
+	bytes_left -= sizeof(unsigned int);
+
+	switch (op_code) {
+	case CG2900_OPCODE_SET_DAI_CONF:
+		if (bytes_left < sizeof(dai_config)) {
+			dev_err(BT_DEV, "Not enough data supplied for "
+				"CG2900_OPCODE_SET_DAI_CONF\n");
+			err = -EINVAL;
+			goto finished_mutex_unlock;
+		}
+		memcpy(&dai_config, curr_data, sizeof(dai_config));
+		dev_dbg(BT_DEV, "CG2900_OPCODE_SET_DAI_CONF port %d\n",
+			dai_config.port);
+		err = cg2900_audio_set_dai_config(dev->session, &dai_config);
+		break;
+
+	case CG2900_OPCODE_GET_DAI_CONF:
+		if (bytes_left < sizeof(dai_config)) {
+			dev_err(BT_DEV, "Not enough data supplied for "
+				"CG2900_OPCODE_GET_DAI_CONF\n");
+			err = -EINVAL;
+			goto finished_mutex_unlock;
+		}
+		/*
+		 * Only need to copy the port really, but let's copy
+		 * like this for simplicity. It's only test functionality
+		 * after all.
+		 */
+		memcpy(&dai_config, curr_data, sizeof(dai_config));
+		dev_dbg(BT_DEV, "CG2900_OPCODE_GET_DAI_CONF port %d\n",
+			dai_config.port);
+		err = cg2900_audio_get_dai_config(dev->session, &dai_config);
+		if (!err) {
+			int len;
+			struct sk_buff *skb;
+
+			/*
+			 * Command succeeded. Store data so it can be returned
+			 * when calling read.
+			 */
+			len = sizeof(op_code) + sizeof(dai_config);
+			skb = alloc_skb(len, GFP_KERNEL);
+			if (!skb) {
+				dev_err(BT_DEV, "CG2900_OPCODE_GET_DAI_CONF: "
+						"Could not allocate skb\n");
+				err = -ENOMEM;
+				goto finished_mutex_unlock;
+			}
+			memcpy(skb_put(skb, sizeof(op_code)), &op_code,
+			       sizeof(op_code));
+			memcpy(skb_put(skb, sizeof(dai_config)),
+			       &dai_config, sizeof(dai_config));
+			skb_queue_tail(&dev->rx_queue, skb);
+		}
+		break;
+
+	case CG2900_OPCODE_CONFIGURE_ENDPOINT:
+		if (bytes_left < sizeof(ep_config)) {
+			dev_err(BT_DEV, "Not enough data supplied for "
+				"CG2900_OPCODE_CONFIGURE_ENDPOINT\n");
+			err = -EINVAL;
+			goto finished_mutex_unlock;
+		}
+		memcpy(&ep_config, curr_data, sizeof(ep_config));
+		dev_dbg(BT_DEV, "CG2900_OPCODE_CONFIGURE_ENDPOINT ep_id %d\n",
+			ep_config.endpoint_id);
+		err = cg2900_audio_config_endpoint(dev->session, &ep_config);
+		break;
+
+	case CG2900_OPCODE_START_STREAM:
+		if (bytes_left < (sizeof(ep_1) + sizeof(ep_2))) {
+			dev_err(BT_DEV, "Not enough data supplied for "
+				"CG2900_OPCODE_START_STREAM\n");
+			err = -EINVAL;
+			goto finished_mutex_unlock;
+		}
+		memcpy(&ep_1, curr_data, sizeof(ep_1));
+		curr_data += sizeof(ep_1);
+		memcpy(&ep_2, curr_data, sizeof(ep_2));
+		dev_dbg(BT_DEV, "CG2900_OPCODE_START_STREAM ep_1 %d ep_2 %d\n",
+			ep_1, ep_2);
+
+		err = cg2900_audio_start_stream(dev->session,
+			ep_1, ep_2, &stream_handle);
+		if (!err) {
+			int len;
+			struct sk_buff *skb;
+
+			/*
+			 * Command succeeded. Store data so it can be returned
+			 * when calling read.
+			 */
+			len = sizeof(op_code) + sizeof(stream_handle);
+			skb = alloc_skb(len, GFP_KERNEL);
+			if (!skb) {
+				dev_err(BT_DEV, "CG2900_OPCODE_START_STREAM: "
+						"Could not allocate skb\n");
+				err = -ENOMEM;
+				goto finished_mutex_unlock;
+			}
+			memcpy(skb_put(skb, sizeof(op_code)), &op_code,
+			       sizeof(op_code));
+			memcpy(skb_put(skb, sizeof(stream_handle)),
+			       &stream_handle, sizeof(stream_handle));
+			skb_queue_tail(&dev->rx_queue, skb);
+
+			dev_dbg(BT_DEV, "stream_handle %d\n", stream_handle);
+		}
+		break;
+
+	case CG2900_OPCODE_STOP_STREAM:
+		if (bytes_left < sizeof(stream_handle)) {
+			dev_err(BT_DEV, "Not enough data supplied for "
+				"CG2900_OPCODE_STOP_STREAM\n");
+			err = -EINVAL;
+			goto finished_mutex_unlock;
+		}
+		memcpy(&stream_handle, curr_data, sizeof(stream_handle));
+		dev_dbg(BT_DEV, "CG2900_OPCODE_STOP_STREAM stream_handle %d\n",
+			stream_handle);
+		err = cg2900_audio_stop_stream(dev->session, stream_handle);
+		break;
+
+	default:
+		dev_err(BT_DEV, "Received bad op_code %d\n", op_code);
+		break;
+	};
+
+finished_mutex_unlock:
+	kfree(rec_data);
+	mutex_unlock(&dev->rw_mutex);
+
+	if (err)
+		return err;
+	else
+		return count;
+}
+
+/**
+ * audio_dev_poll() - Handle POLL call to the interface.
+ * @filp:	Pointer to the file struct.
+ * @wait:	Poll table supplied to caller.
+ *
+ * This function is used by the User Space application to see if the device is
+ * still open and if there is any data available for reading.
+ *
+ * Returns:
+ *   Mask of current set POLL values.
+ */
+static unsigned int audio_dev_poll(struct file *filp, poll_table *wait)
+{
+	struct char_dev_info *dev = filp->private_data;
+	struct audio_info *info;
+	unsigned int mask = 0;
+
+	if (!dev) {
+		pr_err("No dev supplied in private data");
+		return POLLERR | POLLRDHUP;
+	}
+	info = dev->info;
+
+	if (RESET == info->state)
+		mask |= POLLERR | POLLRDHUP | POLLPRI;
+	else
+		/* Unless RESET we can transmit */
+		mask |= POLLOUT;
+
+	if (!skb_queue_empty(&dev->rx_queue))
+		mask |= POLLIN | POLLRDNORM;
+
+	return mask;
+}
+
+static const struct file_operations char_dev_fops = {
+	.open = audio_dev_open,
+	.release = audio_dev_release,
+	.read = audio_dev_read,
+	.write = audio_dev_write,
+	.poll = audio_dev_poll
+};
+
+/**
+ * probe_common() - Register misc device.
+ * @info:	Audio info structure.
+ * @dev:	Current device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation fails.
+ *   Error codes from misc_register.
+ */
+static int probe_common(struct audio_info *info, struct device *dev)
+{
+	struct audio_cb_info *cb_info;
+	struct cg2900_user_data *pf_data;
+	int err;
+
+	cb_info = kzalloc(sizeof(*cb_info), GFP_KERNEL);
+	if (!cb_info) {
+		dev_err(dev, "Failed to allocate cb_info\n");
+		return -ENOMEM;
+	}
+	init_waitqueue_head(&cb_info->wq);
+	skb_queue_head_init(&cb_info->skb_queue);
+
+	pf_data = dev_get_platdata(dev);
+	cg2900_set_usr(pf_data, cb_info);
+	pf_data->dev = dev;
+	pf_data->read_cb = read_cb;
+	pf_data->reset_cb = reset_cb;
+
+	/* Only register misc device when both devices (BT and FM) are probed */
+	if (!info->dev_bt || !info->dev_fm)
+		return 0;
+
+	/* Prepare and register MISC device */
+	info->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	info->misc_dev.name = NAME;
+	info->misc_dev.fops = &char_dev_fops;
+	info->misc_dev.parent = dev;
+	info->misc_dev.mode = S_IRUGO | S_IWUGO;
+
+	err = misc_register(&info->misc_dev);
+	if (err) {
+		dev_err(dev, "Error %d registering misc dev\n", err);
+		return err;
+	}
+	info->misc_registered = true;
+
+	dev_info(dev, "CG2900 Audio driver started\n");
+	return 0;
+}
+
+/**
+ * cg2900_audio_bt_probe() - Initialize CG2900 BT audio resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation fails.
+ *   -EEXIST if device has already been started.
+ *   Error codes from probe_common.
+ */
+static int __devinit cg2900_audio_bt_probe(struct platform_device *pdev)
+{
+	int err;
+	struct audio_info *info;
+
+	dev_dbg(&pdev->dev, "cg2900_audio_bt_probe\n");
+
+	info = get_info(&pdev->dev);
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	info->dev_bt = &pdev->dev;
+	dev_set_drvdata(&pdev->dev, info);
+
+	err = probe_common(info, &pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "Could not probe audio BT (%d)\n", err);
+		dev_set_drvdata(&pdev->dev, NULL);
+		device_removed(info);
+	}
+
+	return err;
+}
+
+/**
+ * cg2900_audio_bt_probe() - Initialize CG2900 FM audio resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation fails.
+ *   -EEXIST if device has already been started.
+ *   Error codes from probe_common.
+ */
+static int __devinit cg2900_audio_fm_probe(struct platform_device *pdev)
+{
+	int err;
+	struct audio_info *info;
+
+	dev_dbg(&pdev->dev, "cg2900_audio_fm_probe\n");
+
+	info = get_info(&pdev->dev);
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	info->dev_fm = &pdev->dev;
+	dev_set_drvdata(&pdev->dev, info);
+
+	err = probe_common(info, &pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "Could not probe audio FM (%d)\n", err);
+		dev_set_drvdata(&pdev->dev, NULL);
+		device_removed(info);
+	}
+
+	return err;
+}
+
+/**
+ * common_remove() - Dergister misc device.
+ * @info:	Audio info structure.
+ * @dev:	Current device.
+ *
+ * Returns:
+ *   0 if success.
+ *   Error codes from misc_deregister.
+ */
+static int common_remove(struct audio_info *info, struct device *dev)
+{
+	int err;
+	struct audio_cb_info *cb_info;
+	struct cg2900_user_data *pf_data;
+
+	pf_data = dev_get_platdata(dev);
+	cb_info = cg2900_get_usr(pf_data);
+	skb_queue_purge(&cb_info->skb_queue);
+	wake_up_all(&cb_info->wq);
+	kfree(cb_info);
+
+	if (!info->misc_registered)
+		return 0;
+
+	err = misc_deregister(&info->misc_dev);
+	if (err)
+		dev_err(dev, "Error %d deregistering misc dev\n", err);
+	info->misc_registered = false;
+
+	dev_info(dev, "CG2900 Audio driver removed\n");
+	return err;
+}
+
+/**
+ * cg2900_audio_bt_remove() - Release CG2900 audio resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   Error codes from common_remove.
+ */
+static int __devexit cg2900_audio_bt_remove(struct platform_device *pdev)
+{
+	int err;
+	struct audio_info *info;
+
+	dev_dbg(&pdev->dev, "cg2900_audio_bt_remove\n");
+
+	info = dev_get_drvdata(&pdev->dev);
+
+	info->dev_bt = NULL;
+
+	err = common_remove(info, &pdev->dev);
+	if (err)
+		dev_err(&pdev->dev,
+			"cg2900_audio_bt_remove:common_remove failed\n");
+
+	device_removed(info);
+
+	return 0;
+}
+
+/**
+ * cg2900_audio_fm_remove() - Release CG2900 audio resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   Error codes from common_remove.
+ */
+static int __devexit cg2900_audio_fm_remove(struct platform_device *pdev)
+{
+	int err;
+	struct audio_info *info;
+
+	dev_dbg(&pdev->dev, "cg2900_audio_fm_remove\n");
+
+	info = dev_get_drvdata(&pdev->dev);
+
+	info->dev_fm = NULL;
+
+	err = common_remove(info, &pdev->dev);
+	if (err)
+		dev_err(&pdev->dev,
+			"cg2900_audio_fm_remove:common_remove failed\n");
+
+	device_removed(info);
+
+	return 0;
+}
+
+static struct platform_driver cg2900_audio_bt_driver = {
+	.driver = {
+		.name	= "cg2900-audiobt",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_audio_bt_probe,
+	.remove	= __devexit_p(cg2900_audio_bt_remove),
+};
+
+static struct platform_driver cg2900_audio_fm_driver = {
+	.driver = {
+		.name	= "cg2900-audiofm",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_audio_fm_probe,
+	.remove	= __devexit_p(cg2900_audio_fm_remove),
+};
+
+/**
+ * cg2900_audio_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_audio_init(void)
+{
+	int err;
+
+	pr_debug("cg2900_audio_init");
+
+	err = platform_driver_register(&cg2900_audio_bt_driver);
+	if (err)
+		return err;
+	return platform_driver_register(&cg2900_audio_fm_driver);
+}
+
+/**
+ * cg2900_audio_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_audio_exit(void)
+{
+	pr_debug("cg2900_audio_exit");
+	platform_driver_unregister(&cg2900_audio_fm_driver);
+	platform_driver_unregister(&cg2900_audio_bt_driver);
+}
+
+module_init(cg2900_audio_init);
+module_exit(cg2900_audio_exit);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_AUTHOR("Kjell Andersson ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux Bluetooth Audio ST-Ericsson controller");
diff --git a/drivers/staging/cg2900/mfd/cg2900_char_devices.c b/drivers/staging/cg2900/mfd/cg2900_char_devices.c
new file mode 100644
index 00000000000..0f1627e82ac
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_char_devices.c
@@ -0,0 +1,706 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson connectivity controller.
+ */
+#define NAME					"cg2900_char_dev"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <linux/compiler.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/mfd/core.h>
+
+#include "cg2900.h"
+#include "cg2900_core.h"
+
+#define MAIN_DEV				(dev->dev)
+
+/**
+ * struct char_dev_user - Stores device information.
+ * @dev:		Current device.
+ * @miscdev:		Registered device struct.
+ * @name:		Name of device.
+ * @rx_queue:		Data queue.
+ * @rx_wait_queue:	Wait queue.
+ * @reset_wait_queue:	Reset Wait queue.
+ * @read_mutex:		Read mutex.
+ * @write_mutex:	Write mutex.
+ * @list:		List header for inserting into device list.
+ */
+struct char_dev_user {
+	struct device		*dev;
+	struct miscdevice	miscdev;
+	char			*name;
+	struct sk_buff_head	rx_queue;
+	wait_queue_head_t	rx_wait_queue;
+	wait_queue_head_t	reset_wait_queue;
+	struct mutex		read_mutex;
+	struct mutex		write_mutex;
+	struct list_head	list;
+};
+
+/**
+ * struct char_info - Stores all current users.
+ * @open_mutex:	Open mutex (used for both open and release).
+ * @man_mutex:	Management mutex.
+ * @dev_users:	List of char dev users.
+ */
+struct char_info {
+	struct mutex		open_mutex;
+	struct mutex		man_mutex;
+	struct list_head	dev_users;
+};
+
+static struct char_info *char_info;
+
+/**
+ * char_dev_read_cb() - Handle data received from controller.
+ * @dev:	Device receiving data.
+ * @skb:	Buffer with data coming from controller.
+ *
+ * The char_dev_read_cb() function handles data received from the CG2900 driver.
+ */
+static void char_dev_read_cb(struct cg2900_user_data *dev, struct sk_buff *skb)
+{
+	struct char_dev_user *char_dev = dev_get_drvdata(dev->dev);
+
+	dev_dbg(dev->dev, "char_dev_read_cb len %d\n", skb->len);
+
+	skb_queue_tail(&char_dev->rx_queue, skb);
+
+	wake_up_interruptible(&char_dev->rx_wait_queue);
+}
+
+/**
+ * char_dev_reset_cb() - Handle reset from controller.
+ * @dev:	Device resetting.
+ *
+ * The char_dev_reset_cb() function handles reset from the CG2900 driver.
+ */
+static void char_dev_reset_cb(struct cg2900_user_data *dev)
+{
+	struct char_dev_user *char_dev = dev_get_drvdata(dev->dev);
+
+	dev_dbg(dev->dev, "char_dev_reset_cb\n");
+
+	wake_up_interruptible(&char_dev->rx_wait_queue);
+	wake_up_interruptible(&char_dev->reset_wait_queue);
+}
+
+/**
+ * char_dev_open() - Open char device.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * The char_dev_open() function opens the char device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if device cannot be found in device list.
+ *   Error codes from cg2900->open.
+ */
+static int char_dev_open(struct inode *inode, struct file *filp)
+{
+	int err;
+	int minor;
+	struct char_dev_user *dev = NULL;
+	struct char_dev_user *tmp;
+	struct list_head *cursor;
+	struct cg2900_user_data *user;
+
+	mutex_lock(&char_info->open_mutex);
+
+	minor = iminor(inode);
+
+	/* Find the device for this file */
+	mutex_lock(&char_info->man_mutex);
+	list_for_each(cursor, &char_info->dev_users) {
+		tmp = list_entry(cursor, struct char_dev_user, list);
+		if (tmp->miscdev.minor == minor) {
+			dev = tmp;
+			break;
+		}
+	}
+	mutex_unlock(&char_info->man_mutex);
+	if (!dev) {
+		pr_err("Could not identify device in inode");
+		err = -EINVAL;
+		goto error_handling;
+	}
+
+	filp->private_data = dev;
+	user = dev_get_platdata(dev->dev);
+
+	/* First initiate wait queues for this device. */
+	init_waitqueue_head(&dev->rx_wait_queue);
+	init_waitqueue_head(&dev->reset_wait_queue);
+
+	/* Register to CG2900 Driver */
+	err = user->open(user);
+	if (err) {
+		dev_err(MAIN_DEV,
+			"Couldn't register to CG2900 for H:4 channel %s\n",
+			dev->name);
+		goto error_handling;
+	}
+	dev_info(MAIN_DEV, "char_dev %s opened\n", dev->name);
+
+error_handling:
+	mutex_unlock(&char_info->open_mutex);
+	return err;
+}
+
+/**
+ * char_dev_release() - Release char device.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * The char_dev_release() function release the char device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EBADF if NULL pointer was supplied in private data.
+ */
+static int char_dev_release(struct inode *inode, struct file *filp)
+{
+	int err = 0;
+	struct char_dev_user *dev = filp->private_data;
+	struct cg2900_user_data *user;
+
+	pr_debug("char_dev_release");
+
+	if (!dev) {
+		pr_err("Calling with NULL pointer");
+		return -EBADF;
+	}
+
+	mutex_lock(&char_info->open_mutex);
+	mutex_lock(&dev->read_mutex);
+	mutex_lock(&dev->write_mutex);
+
+	user = dev_get_platdata(dev->dev);
+	if (user->opened)
+		user->close(user);
+
+	dev_info(MAIN_DEV, "char_dev %s closed\n", dev->name);
+
+	filp->private_data = NULL;
+	wake_up_interruptible(&dev->rx_wait_queue);
+	wake_up_interruptible(&dev->reset_wait_queue);
+
+	mutex_unlock(&dev->write_mutex);
+	mutex_unlock(&dev->read_mutex);
+	mutex_unlock(&char_info->open_mutex);
+
+	return err;
+}
+
+/**
+ * char_dev_read() - Queue and copy buffer to user.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Received buffer.
+ * @count:	Size of buffer.
+ * @f_pos:	Position in buffer.
+ *
+ * The char_dev_read() function queues and copy the received buffer to
+ * the user space char device. If no data is available this function will block.
+ *
+ * Returns:
+ *   Bytes successfully read (could be 0).
+ *   -EBADF if NULL pointer was supplied in private data.
+ *   -EFAULT if copy_to_user fails.
+ *   Error codes from wait_event_interruptible.
+ */
+static ssize_t char_dev_read(struct file *filp, char __user *buf, size_t count,
+			     loff_t *f_pos)
+{
+	struct char_dev_user *dev = filp->private_data;
+	struct cg2900_user_data *user;
+	struct sk_buff *skb;
+	int bytes_to_copy;
+	int err = 0;
+
+	pr_debug("char_dev_read");
+
+	if (!dev) {
+		pr_err("Calling with NULL pointer");
+		return -EBADF;
+	}
+	mutex_lock(&dev->read_mutex);
+
+	user = dev_get_platdata(dev->dev);
+
+	if (user->opened && skb_queue_empty(&dev->rx_queue)) {
+		err = wait_event_interruptible(dev->rx_wait_queue,
+				(!(skb_queue_empty(&dev->rx_queue))) ||
+				!user->opened);
+		if (err) {
+			dev_err(MAIN_DEV, "Failed to wait for event\n");
+			goto error_handling;
+		}
+	}
+
+	if (!user->opened) {
+		dev_err(MAIN_DEV, "Channel has been closed\n");
+		err = -EBADF;
+		goto error_handling;
+	}
+
+	skb = skb_dequeue(&dev->rx_queue);
+	if (!skb) {
+		dev_dbg(MAIN_DEV,
+			"skb queue is empty - return with zero bytes\n");
+		bytes_to_copy = 0;
+		goto finished;
+	}
+
+	bytes_to_copy = min(count, skb->len);
+
+	err = copy_to_user(buf, skb->data, bytes_to_copy);
+	if (err) {
+		dev_err(MAIN_DEV, "Error %d from copy_to_user\n", err);
+		skb_queue_head(&dev->rx_queue, skb);
+		err = -EFAULT;
+		goto error_handling;
+	}
+
+	skb_pull(skb, bytes_to_copy);
+
+	if (skb->len > 0)
+		skb_queue_head(&dev->rx_queue, skb);
+	else
+		kfree_skb(skb);
+
+	goto finished;
+
+error_handling:
+	mutex_unlock(&dev->read_mutex);
+	return (ssize_t)err;
+finished:
+	mutex_unlock(&dev->read_mutex);
+	return bytes_to_copy;
+}
+
+/**
+ * char_dev_write() - Copy buffer from user and write to CG2900 driver.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Write buffer.
+ * @count:	Size of the buffer write.
+ * @f_pos:	Position of buffer.
+ *
+ * Returns:
+ *   Bytes successfully written (could be 0).
+ *   -EBADF if NULL pointer was supplied in private data.
+ *   -EFAULT if copy_from_user fails.
+ */
+static ssize_t char_dev_write(struct file *filp, const char __user *buf,
+			      size_t count, loff_t *f_pos)
+{
+	struct sk_buff *skb;
+	struct char_dev_user *dev = filp->private_data;
+	struct cg2900_user_data *user;
+	int err = 0;
+
+	pr_debug("char_dev_write");
+
+	if (!dev) {
+		pr_err("Calling with NULL pointer");
+		return -EBADF;
+	}
+
+	user = dev_get_platdata(dev->dev);
+	if (!user->opened) {
+		dev_err(MAIN_DEV, "char_dev_write: Channel not opened\n");
+		return -EACCES;
+	}
+
+	mutex_lock(&dev->write_mutex);
+
+	skb = user->alloc_skb(count, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(MAIN_DEV, "Couldn't allocate sk_buff with length %d\n",
+			count);
+		goto error_handling;
+	}
+
+	err = copy_from_user(skb_put(skb, count), buf, count);
+	if (err) {
+		dev_err(MAIN_DEV, "Error %d from copy_from_user\n", err);
+		kfree_skb(skb);
+		err = -EFAULT;
+		goto error_handling;
+	}
+
+	err = user->write(user, skb);
+	if (err) {
+		dev_err(MAIN_DEV, "cg2900_write failed (%d)\n", err);
+		kfree_skb(skb);
+		goto error_handling;
+	}
+
+	mutex_unlock(&dev->write_mutex);
+	return count;
+
+error_handling:
+	mutex_unlock(&dev->write_mutex);
+	return err;
+}
+
+/**
+ * char_dev_unlocked_ioctl() - Handle IOCTL call to the interface.
+ * @filp:	Pointer to the file struct.
+ * @cmd:	IOCTL command.
+ * @arg:	IOCTL argument.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if supplied cmd is not supported.
+ *   For cmd CG2900_CHAR_DEV_IOCTL_CHECK4RESET 0x01 is returned if device is
+ *   reset and 0x02 is returned if device is closed.
+ */
+static long char_dev_unlocked_ioctl(struct file *filp, unsigned int cmd,
+				    unsigned long arg)
+{
+	struct char_dev_user *dev = filp->private_data;
+	struct cg2900_user_data *user;
+	struct cg2900_rev_data rev_data;
+	int err = 0;
+	int ret_val;
+	void __user *user_arg = (void __user *)arg;
+
+	dev_dbg(dev->dev, "char_dev_unlocked_ioctl for %s\n"
+		"\tDIR: %d\n"
+		"\tTYPE: %d\n"
+		"\tNR: %d\n"
+		"\tSIZE: %d",
+		dev->name, _IOC_DIR(cmd), _IOC_TYPE(cmd), _IOC_NR(cmd),
+		_IOC_SIZE(cmd));
+
+	user = dev_get_platdata(dev->dev);
+
+	switch (cmd) {
+	case CG2900_CHAR_DEV_IOCTL_RESET:
+		if (!user->opened)
+			return -EACCES;
+		dev_dbg(MAIN_DEV, "ioctl reset command for device %s\n",
+			dev->name);
+		err = user->reset(user);
+		break;
+
+	case CG2900_CHAR_DEV_IOCTL_CHECK4RESET:
+		if (user->opened)
+			ret_val = CG2900_CHAR_DEV_IOCTL_EVENT_IDLE;
+		else
+			ret_val = CG2900_CHAR_DEV_IOCTL_EVENT_RESET;
+
+		dev_dbg(MAIN_DEV, "ioctl check for reset command for device %s",
+			dev->name);
+
+		err = copy_to_user(user_arg, &ret_val, sizeof(ret_val));
+		if (err) {
+			dev_err(MAIN_DEV,
+				"Error %d from copy_to_user for reset\n", err);
+			return -EFAULT;
+		}
+		break;
+
+	case CG2900_CHAR_DEV_IOCTL_GET_REVISION:
+		if (!user->get_local_revision(user, &rev_data)) {
+			dev_err(MAIN_DEV, "No revision data available\n");
+			return -EIO;
+		}
+		dev_dbg(MAIN_DEV, "ioctl check for local revision info\n"
+			"\trevision 0x%04X\n"
+			"\tsub_version 0x%04X\n",
+			rev_data.revision, rev_data.sub_version);
+		err = copy_to_user(user_arg, &rev_data, sizeof(rev_data));
+		if (err) {
+			dev_err(MAIN_DEV,
+				"Error %d from copy_to_user for "
+				"revision\n", err);
+			return -EFAULT;
+		}
+		break;
+
+	default:
+		dev_err(MAIN_DEV, "Unknown ioctl command %08X\n", cmd);
+		err = -EINVAL;
+		break;
+	};
+
+	return err;
+}
+
+/**
+ * char_dev_poll() - Handle POLL call to the interface.
+ * @filp:	Pointer to the file struct.
+ * @wait:	Poll table supplied to caller.
+ *
+ * Returns:
+ *   Mask of current set POLL values
+ */
+static unsigned int char_dev_poll(struct file *filp, poll_table *wait)
+{
+	struct char_dev_user *dev = filp->private_data;
+	struct cg2900_user_data *user;
+	unsigned int mask = 0;
+
+	if (!dev) {
+		pr_debug("Device not open");
+		return POLLERR | POLLRDHUP;
+	}
+
+	user = dev_get_platdata(dev->dev);
+
+	poll_wait(filp, &dev->reset_wait_queue, wait);
+	poll_wait(filp, &dev->rx_wait_queue, wait);
+
+	if (!user->opened)
+		mask |= POLLERR | POLLRDHUP | POLLPRI;
+	else
+		mask |= POLLOUT; /* We can TX unless there is an error */
+
+	if (!(skb_queue_empty(&dev->rx_queue)))
+		mask |= POLLIN | POLLRDNORM;
+
+	return mask;
+}
+
+/*
+ * struct char_dev_fops - Char devices file operations.
+ * @read:		Function that reads from the char device.
+ * @write:		Function that writes to the char device.
+ * @unlocked_ioctl:	Function that performs IO operations with
+ *			the char device.
+ * @poll:		Function that checks if there are possible operations
+ *			with the char device.
+ * @open:		Function that opens the char device.
+ * @release:		Function that release the char device.
+ */
+static const struct file_operations char_dev_fops = {
+	.read		= char_dev_read,
+	.write		= char_dev_write,
+	.unlocked_ioctl	= char_dev_unlocked_ioctl,
+	.poll		= char_dev_poll,
+	.open		= char_dev_open,
+	.release	= char_dev_release
+};
+
+/**
+ * remove_dev() - Remove char device structure for device.
+ * @dev_usr:	Char device user.
+ *
+ * The remove_dev() function releases the char_dev structure for this device.
+ */
+static void remove_dev(struct char_dev_user *dev_usr)
+{
+	if (!dev_usr)
+		return;
+
+	dev_dbg(dev_usr->dev,
+		"Removing char device %s with major %d and minor %d\n",
+		dev_usr->name,
+		MAJOR(dev_usr->miscdev.this_device->devt),
+		MINOR(dev_usr->miscdev.this_device->devt));
+
+	skb_queue_purge(&dev_usr->rx_queue);
+
+	mutex_destroy(&dev_usr->read_mutex);
+	mutex_destroy(&dev_usr->write_mutex);
+
+	/* Remove device node in file system. */
+	misc_deregister(&dev_usr->miscdev);
+	kfree(dev_usr);
+}
+
+/**
+ * cg2900_char_probe() - Initialize char device module.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM if allocation fails.
+ *   -EACCES if device already have been initiated.
+ */
+static int __devinit cg2900_char_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	struct char_dev_user *dev_usr;
+	struct cg2900_user_data *user;
+	struct device *dev = &pdev->dev;
+
+	dev_dbg(&pdev->dev, "cg2900_char_probe\n");
+
+	user = mfd_get_cell(pdev)->platform_data;
+	user->dev = dev;
+	user->read_cb = char_dev_read_cb;
+	user->reset_cb = char_dev_reset_cb;
+
+	/* Set platform data */
+	dev->platform_data = user;
+
+	dev_usr = kzalloc(sizeof(*dev_usr), GFP_KERNEL);
+	if (!dev_usr) {
+		dev_err(&pdev->dev, "Couldn't allocate dev_usr\n");
+		return -ENOMEM;
+	}
+
+	dev_set_drvdata(&pdev->dev, dev_usr);
+	dev_usr->dev = &pdev->dev;
+
+	/* Store device name */
+	dev_usr->name = user->channel_data.char_dev_name;
+
+	/* Prepare miscdevice struct before registering the device */
+	dev_usr->miscdev.minor = MISC_DYNAMIC_MINOR;
+	dev_usr->miscdev.name = dev_usr->name;
+	dev_usr->miscdev.nodename = dev_usr->name;
+	dev_usr->miscdev.fops = &char_dev_fops;
+	dev_usr->miscdev.parent = &pdev->dev;
+	dev_usr->miscdev.mode = S_IRUGO | S_IWUGO;
+
+	err = misc_register(&dev_usr->miscdev);
+	if (err) {
+		dev_err(&pdev->dev, "Error %d registering misc dev\n", err);
+		goto err_free_usr;
+	}
+
+	dev_dbg(&pdev->dev, "Added char device %s with major %d and minor %d\n",
+		dev_usr->name, MAJOR(dev_usr->miscdev.this_device->devt),
+		MINOR(dev_usr->miscdev.this_device->devt));
+
+	mutex_init(&dev_usr->read_mutex);
+	mutex_init(&dev_usr->write_mutex);
+
+	skb_queue_head_init(&dev_usr->rx_queue);
+
+	mutex_lock(&char_info->man_mutex);
+	list_add_tail(&dev_usr->list, &char_info->dev_users);
+	mutex_unlock(&char_info->man_mutex);
+
+	return 0;
+
+err_free_usr:
+	kfree(dev_usr);
+	dev_set_drvdata(&pdev->dev, NULL);
+	return err;
+}
+
+/**
+ * cg2900_char_remove() - Release the char device module.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success (always success).
+ */
+static int __devexit cg2900_char_remove(struct platform_device *pdev)
+{
+	struct list_head *cursor, *next;
+	struct char_dev_user *tmp;
+	struct char_dev_user *user;
+
+	dev_dbg(&pdev->dev, "cg2900_char_remove\n");
+
+	user = dev_get_drvdata(&pdev->dev);
+
+	mutex_lock(&char_info->man_mutex);
+	list_for_each_safe(cursor, next, &char_info->dev_users) {
+		tmp = list_entry(cursor, struct char_dev_user, list);
+		if (tmp == user) {
+			list_del(cursor);
+			remove_dev(tmp);
+			dev_set_drvdata(&pdev->dev, NULL);
+			break;
+		}
+	}
+	mutex_unlock(&char_info->man_mutex);
+	return 0;
+}
+
+static struct platform_driver cg2900_char_driver = {
+	.driver = {
+		.name	= "cg2900-chardev",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_char_probe,
+	.remove	= __devexit_p(cg2900_char_remove),
+};
+
+/**
+ * cg2900_char_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_char_init(void)
+{
+	pr_debug("cg2900_char_init");
+
+	/* Initialize private data. */
+	char_info = kzalloc(sizeof(*char_info), GFP_ATOMIC);
+	if (!char_info) {
+		pr_err("Could not alloc char_info struct");
+		return -ENOMEM;
+	}
+
+	mutex_init(&char_info->open_mutex);
+	mutex_init(&char_info->man_mutex);
+	INIT_LIST_HEAD(&char_info->dev_users);
+
+	return platform_driver_register(&cg2900_char_driver);
+}
+
+/**
+ * cg2900_char_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_char_exit(void)
+{
+	struct list_head *cursor, *next;
+	struct char_dev_user *tmp;
+
+	pr_debug("cg2900_char_exit");
+
+	platform_driver_unregister(&cg2900_char_driver);
+
+	if (!char_info)
+		return;
+
+	list_for_each_safe(cursor, next, &char_info->dev_users) {
+		tmp = list_entry(cursor, struct char_dev_user, list);
+		list_del(cursor);
+		remove_dev(tmp);
+	}
+
+	mutex_destroy(&char_info->open_mutex);
+	mutex_destroy(&char_info->man_mutex);
+
+	kfree(char_info);
+	char_info = NULL;
+}
+
+module_init(cg2900_char_init);
+module_exit(cg2900_char_exit);
+
+MODULE_AUTHOR("Henrik Possung ST-Ericsson");
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("ST-Ericsson CG2900 Char Devices Driver");
diff --git a/drivers/staging/cg2900/mfd/cg2900_chip.c b/drivers/staging/cg2900/mfd/cg2900_chip.c
new file mode 100644
index 00000000000..fa243dcab15
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_chip.c
@@ -0,0 +1,3393 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+#define NAME					"cg2900_chip"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/gfp.h>
+#include <linux/init.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/limits.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/stat.h>
+#include <linux/string.h>
+#include <linux/time.h>
+#include <linux/timer.h>
+#include <linux/types.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/core.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+
+#include "cg2900.h"
+#include "cg2900_chip.h"
+#include "cg2900_core.h"
+#include "cg2900_lib.h"
+
+#define MAIN_DEV				(main_info->dev)
+#define BOOT_DEV				(info->user_in_charge->dev)
+
+#define WQ_NAME					"cg2900_chip_wq"
+
+/*
+ * After waiting the first 500 ms we should just try to get the selftest results
+ * for another number of poll attempts
+ */
+#define MAX_NBR_OF_POLLS			50
+
+#define LINE_TOGGLE_DETECT_TIMEOUT		50	/* ms */
+#define CHIP_READY_TIMEOUT			100	/* ms */
+#define CHIP_STARTUP_TIMEOUT			15000	/* ms */
+#define CHIP_SHUTDOWN_TIMEOUT			15000	/* ms */
+#define POWER_SW_OFF_WAIT			500	/* ms */
+#define SELFTEST_INITIAL			500	/* ms */
+#define SELFTEST_POLLING			20	/* ms */
+
+/** CHANNEL_BT_CMD - Bluetooth HCI H:4 channel
+ * for Bluetooth commands in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_CMD				0x01
+
+/** CHANNEL_BT_ACL - Bluetooth HCI H:4 channel
+ * for Bluetooth ACL data in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_ACL				0x02
+
+/** CHANNEL_BT_EVT - Bluetooth HCI H:4 channel
+ * for Bluetooth events in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_EVT				0x04
+
+/** CHANNEL_FM_RADIO - Bluetooth HCI H:4 channel
+ * for FM radio in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_FM_RADIO			0x08
+
+/** CHANNEL_GNSS - Bluetooth HCI H:4 channel
+ * for GNSS in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_GNSS				0x09
+
+/** CHANNEL_DEBUG - Bluetooth HCI H:4 channel
+ * for internal debug data in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_DEBUG				0x0B
+
+/** CHANNEL_STE_TOOLS - Bluetooth HCI H:4 channel
+ * for development tools data in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_STE_TOOLS			0x0D
+
+/** CHANNEL_HCI_LOGGER - Bluetooth HCI H:4 channel
+ * for logging all transmitted H4 packets (on all channels).
+ */
+#define CHANNEL_HCI_LOGGER			0xFA
+
+/** CHANNEL_CORE - Bluetooth HCI H:4 channel
+ * for user space control of the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_CORE				0xFD
+
+/** CG2900_BT_CMD - Bluetooth HCI H4 channel for Bluetooth commands.
+ */
+#define CG2900_BT_CMD				"cg2900_bt_cmd"
+
+/** CG2900_BT_ACL - Bluetooth HCI H4 channel for Bluetooth ACL data.
+ */
+#define CG2900_BT_ACL				"cg2900_bt_acl"
+
+/** CG2900_BT_EVT - Bluetooth HCI H4 channel for Bluetooth events.
+ */
+#define CG2900_BT_EVT				"cg2900_bt_evt"
+
+/** CG2900_FM_RADIO - Bluetooth HCI H4 channel for FM radio.
+ */
+#define CG2900_FM_RADIO				"cg2900_fm_radio"
+
+/** CG2900_GNSS - Bluetooth HCI H4 channel for GNSS.
+ */
+#define CG2900_GNSS				"cg2900_gnss"
+
+/** CG2900_DEBUG - Bluetooth HCI H4 channel for internal debug data.
+ */
+#define CG2900_DEBUG				"cg2900_debug"
+
+/** CG2900_STE_TOOLS - Bluetooth HCI H4 channel for development tools data.
+ */
+#define CG2900_STE_TOOLS			"cg2900_ste_tools"
+
+/** CG2900_HCI_LOGGER - BT channel for logging all transmitted H4 packets.
+ * Data read is copy of all data transferred on the other channels.
+ * Only write allowed is configuration of the HCI Logger.
+ */
+#define CG2900_HCI_LOGGER			"cg2900_hci_logger"
+
+/** CG2900_BT_AUDIO - HCI Channel for BT audio configuration commands.
+ * Maps to Bluetooth command and event channels.
+ */
+#define CG2900_BT_AUDIO				"cg2900_bt_audio"
+
+/** CG2900_FM_AUDIO - HCI channel for FM audio configuration commands.
+ * Maps to FM Radio channel.
+ */
+#define CG2900_FM_AUDIO				"cg2900_fm_audio"
+
+/** CG2900_CORE- Channel for keeping ST-Ericsson CG2900 enabled.
+ * Opening this channel forces the chip to stay powered.
+ * No data can be written to or read from this channel.
+ */
+#define CG2900_CORE				"cg2900_core"
+
+/**
+ * enum main_state - Main-state for CG2900 driver.
+ * @CG2900_INIT:	CG2900 initializing.
+ * @CG2900_IDLE:	No user registered to CG2900 driver.
+ * @CG2900_BOOTING:	CG2900 booting after first user is registered.
+ * @CG2900_CLOSING:	CG2900 closing after last user has deregistered.
+ * @CG2900_RESETING:	CG2900 reset requested.
+ * @CG2900_ACTIVE:	CG2900 up and running with at least one user.
+ */
+enum main_state {
+	CG2900_INIT,
+	CG2900_IDLE,
+	CG2900_BOOTING,
+	CG2900_CLOSING,
+	CG2900_RESETING,
+	CG2900_ACTIVE
+};
+
+/**
+ * enum boot_state - BOOT-state for CG2900 chip driver.
+ * @BOOT_NOT_STARTED:			Boot has not yet started.
+ * @BOOT_SEND_BD_ADDRESS:		VS Store In FS command with BD address
+ *					has been sent.
+ * @BOOT_GET_FILES_TO_LOAD:		CG2900 chip driver is retrieving file to
+ *					load.
+ * @BOOT_DOWNLOAD_PATCH:		CG2900 chip driver is downloading
+ *					patches.
+ * @BOOT_ACTIVATE_PATCHES_AND_SETTINGS:	CG2900 chip driver is activating patches
+ *					and settings.
+ * @BOOT_READ_SELFTEST_RESULT:		CG2900 is performing selftests that
+ *					shall be read out.
+ * @BOOT_DISABLE_BT:			Disable BT Core.
+ * @BOOT_READY:				CG2900 chip driver boot is ready.
+ * @BOOT_FAILED:			CG2900 chip driver boot failed.
+ */
+enum boot_state {
+	BOOT_NOT_STARTED,
+	BOOT_SEND_BD_ADDRESS,
+	BOOT_GET_FILES_TO_LOAD,
+	BOOT_DOWNLOAD_PATCH,
+	BOOT_ACTIVATE_PATCHES_AND_SETTINGS,
+	BOOT_READ_SELFTEST_RESULT,
+	BOOT_DISABLE_BT,
+	BOOT_READY,
+	BOOT_FAILED
+};
+
+/**
+ * enum closing_state - CLOSING-state for CG2900 chip driver.
+ * @CLOSING_RESET:		HCI RESET_CMD has been sent.
+ * @CLOSING_POWER_SWITCH_OFF:	HCI VS_POWER_SWITCH_OFF command has been sent.
+ * @CLOSING_SHUT_DOWN:		We have now shut down the chip.
+ */
+enum closing_state {
+	CLOSING_RESET,
+	CLOSING_POWER_SWITCH_OFF,
+	CLOSING_SHUT_DOWN
+};
+
+/**
+ * enum file_load_state - BOOT_FILE_LOAD-state for CG2900 chip driver.
+ * @FILE_LOAD_GET_PATCH:		Loading patches.
+ * @FILE_LOAD_GET_STATIC_SETTINGS:	Loading static settings.
+ * @FILE_LOAD_NO_MORE_FILES:		No more files to load.
+ * @FILE_LOAD_FAILED:			File loading failed.
+ */
+enum file_load_state {
+	FILE_LOAD_GET_PATCH,
+	FILE_LOAD_GET_STATIC_SETTINGS,
+	FILE_LOAD_NO_MORE_FILES,
+	FILE_LOAD_FAILED
+};
+
+/**
+ * enum download_state - BOOT_DOWNLOAD state.
+ * @DOWNLOAD_PENDING:	Download in progress.
+ * @DOWNLOAD_SUCCESS:	Download successfully finished.
+ * @DOWNLOAD_FAILED:	Downloading failed.
+ */
+enum download_state {
+	DOWNLOAD_PENDING,
+	DOWNLOAD_SUCCESS,
+	DOWNLOAD_FAILED
+};
+
+/**
+ * enum fm_radio_mode - FM Radio mode.
+ * It's needed because some FM do-commands generate interrupts only when
+ * the FM driver is in specific mode and we need to know if we should expect
+ * the interrupt.
+ * @FM_RADIO_MODE_IDLE:	Radio mode is Idle (default).
+ * @FM_RADIO_MODE_FMT:	Radio mode is set to FMT (transmitter).
+ * @FM_RADIO_MODE_FMR:	Radio mode is set to FMR (receiver).
+ */
+enum fm_radio_mode {
+	FM_RADIO_MODE_IDLE = 0,
+	FM_RADIO_MODE_FMT = 1,
+	FM_RADIO_MODE_FMR = 2
+};
+
+
+/**
+ * struct cg2900_channel_item - List object for channel.
+ * @list:	list_head struct.
+ * @user:	User for this channel.
+ */
+struct cg2900_channel_item {
+	struct list_head	list;
+	struct cg2900_user_data	*user;
+};
+
+/**
+ * struct cg2900_delayed_work_struct - Work structure for CG2900 chip.
+ * @delayed_work:	Work structure.
+ * @data:		Pointer to private data.
+ */
+struct cg2900_delayed_work_struct {
+	struct delayed_work	work;
+	void			*data;
+};
+
+/**
+ * struct cg2900_skb_data - Structure for storing private data in an sk_buffer.
+ * @dev:	CG2900 device for this sk_buffer.
+ */
+struct cg2900_skb_data {
+	struct cg2900_user_data *user;
+};
+#define cg2900_skb_data(__skb) ((struct cg2900_skb_data *)((__skb)->cb))
+
+/**
+ * struct cg2900_chip_info - Main info structure for CG2900 chip driver.
+ * @dev:			Current device. Same as @chip_dev->dev.
+ * @patch_file_name:		Stores patch file name.
+ * @settings_file_name:		Stores settings file name.
+ * @file_info:			Firmware file info (patch or settings).
+ * @boot_state:			Current BOOT-state of CG2900 chip driver.
+ * @closing_state:		Current CLOSING-state of CG2900 chip driver.
+ * @file_load_state:		Current BOOT_FILE_LOAD-state of CG2900 chip
+ *				driver.
+ * @download_state:		Current BOOT_DOWNLOAD-state of CG2900 chip
+ *				driver.
+ * @wq:				CG2900 chip driver workqueue.
+ * @chip_dev:			Chip handler info.
+ * @tx_bt_lock:			Spinlock used to protect some global structures
+ *				related to internal BT command flow control.
+ * @tx_fm_lock:			Spinlock used to protect some global structures
+ *				related to internal FM command flow control.
+ * @tx_fm_audio_awaiting_irpt:	Indicates if an FM interrupt event related to
+ *				audio driver command is expected.
+ * @fm_radio_mode:		Current FM radio mode.
+ * @tx_nr_pkts_allowed_bt:	Number of packets allowed to send on BT HCI CMD
+ *				H4 channel.
+ * @audio_bt_cmd_op:		Stores the OpCode of the last sent audio driver
+ *				HCI BT CMD.
+ * @audio_fm_cmd_id:		Stores the command id of the last sent
+ *				HCI FM RADIO command by the fm audio user.
+ * @hci_fm_cmd_func:		Stores the command function of the last sent
+ *				HCI FM RADIO command by the fm radio user.
+ * @tx_queue_bt:		TX queue for HCI BT commands when nr of commands
+ *				allowed is 0 (CG2900 internal flow control).
+ * @tx_queue_fm:		TX queue for HCI FM commands when nr of commands
+ *				allowed is 0 (CG2900 internal flow control).
+ * @user_in_charge:		User currently operating. Normally used at
+ *				channel open and close.
+ * @last_user:			Last user of this chip. To avoid complications
+ *				this will never be set for bt_audio and
+ *				fm_audio.
+ * @logger:			Logger user of this chip.
+ * @selftest_work:		Delayed work for reading selftest results.
+ * @nbr_of_polls:		Number of times we should poll for selftest
+ *				results.
+ */
+struct cg2900_chip_info {
+	struct device			*dev;
+	char				*patch_file_name;
+	char				*settings_file_name;
+	struct cg2900_file_info		file_info;
+	enum main_state			main_state;
+	enum boot_state			boot_state;
+	enum closing_state		closing_state;
+	enum file_load_state		file_load_state;
+	enum download_state		download_state;
+	struct workqueue_struct		*wq;
+	struct cg2900_chip_dev		*chip_dev;
+	spinlock_t			tx_bt_lock;
+	spinlock_t			tx_fm_lock;
+	spinlock_t			rw_lock;
+	bool				tx_fm_audio_awaiting_irpt;
+	enum fm_radio_mode		fm_radio_mode;
+	int				tx_nr_pkts_allowed_bt;
+	u16				audio_bt_cmd_op;
+	u16				audio_fm_cmd_id;
+	u16				hci_fm_cmd_func;
+	struct sk_buff_head		tx_queue_bt;
+	struct sk_buff_head		tx_queue_fm;
+	struct list_head		open_channels;
+	struct cg2900_user_data		*user_in_charge;
+	struct cg2900_user_data		*last_user;
+	struct cg2900_user_data		*logger;
+	struct cg2900_user_data		*bt_audio;
+	struct cg2900_user_data		*fm_audio;
+	struct cg2900_delayed_work_struct	selftest_work;
+	int				nbr_of_polls;
+};
+
+/**
+ * struct main_info - Main info structure for CG2900 chip driver.
+ * @dev:			Device structure.
+ * @cell_base_id:		Base ID for MFD cells.
+ * @man_mutex:			Management mutex.
+ */
+struct main_info {
+	struct device			*dev;
+	int				cell_base_id;
+	struct mutex			man_mutex;
+};
+
+static struct main_info *main_info;
+
+/*
+ * main_wait_queue - Main Wait Queue in CG2900 driver.
+ */
+static DECLARE_WAIT_QUEUE_HEAD(main_wait_queue);
+
+static void chip_startup_finished(struct cg2900_chip_info *info, int err);
+static void chip_shutdown(struct cg2900_user_data *user);
+
+/**
+ * bt_is_open() - Checks if any BT user is in open state.
+ * @info:	CG2900 info.
+ *
+ * Returns:
+ *   true if a BT channel is open.
+ *   false if no BT channel is open.
+ */
+static bool bt_is_open(struct cg2900_chip_info *info)
+{
+	struct list_head *cursor;
+	struct cg2900_channel_item *tmp;
+
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		if (tmp->user->h4_channel == CHANNEL_BT_CMD)
+			return true;
+	}
+	return false;
+}
+
+/**
+ * fm_is_open() - Checks if any FM user is in open state.
+ * @info:	CG2900 info.
+ *
+ * Returns:
+ *   true if a FM channel is open.
+ *   false if no FM channel is open.
+ */
+static bool fm_is_open(struct cg2900_chip_info *info)
+{
+	struct list_head *cursor;
+	struct cg2900_channel_item *tmp;
+
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		if (tmp->user->h4_channel == CHANNEL_FM_RADIO)
+			return true;
+	}
+	return false;
+}
+
+/**
+ * fm_irpt_expected() - check if this FM command will generate an interrupt.
+ * @cmd_id:	command identifier.
+ *
+ * Returns:
+ *   true if the command will generate an interrupt.
+ *   false if it won't.
+ */
+static bool fm_irpt_expected(struct cg2900_chip_info *info, u16 cmd_id)
+{
+	bool retval = false;
+
+	switch (cmd_id) {
+	case CG2900_FM_DO_AIP_FADE_START:
+		if (info->fm_radio_mode == FM_RADIO_MODE_FMT)
+			retval = true;
+		break;
+
+	case CG2900_FM_DO_AUP_BT_FADE_START:
+	case CG2900_FM_DO_AUP_EXT_FADE_START:
+	case CG2900_FM_DO_AUP_FADE_START:
+		if (info->fm_radio_mode == FM_RADIO_MODE_FMR)
+			retval = true;
+		break;
+
+	case CG2900_FM_DO_FMR_SETANTENNA:
+	case CG2900_FM_DO_FMR_SP_AFSWITCH_START:
+	case CG2900_FM_DO_FMR_SP_AFUPDATE_START:
+	case CG2900_FM_DO_FMR_SP_BLOCKSCAN_START:
+	case CG2900_FM_DO_FMR_SP_PRESETPI_START:
+	case CG2900_FM_DO_FMR_SP_SCAN_START:
+	case CG2900_FM_DO_FMR_SP_SEARCH_START:
+	case CG2900_FM_DO_FMR_SP_SEARCHPI_START:
+	case CG2900_FM_DO_FMR_SP_TUNE_SETCHANNEL:
+	case CG2900_FM_DO_FMR_SP_TUNE_STEPCHANNEL:
+	case CG2900_FM_DO_FMT_PA_SETCTRL:
+	case CG2900_FM_DO_FMT_PA_SETMODE:
+	case CG2900_FM_DO_FMT_SP_TUNE_SETCHANNEL:
+	case CG2900_FM_DO_GEN_ANTENNACHECK_START:
+	case CG2900_FM_DO_GEN_GOTOMODE:
+	case CG2900_FM_DO_GEN_POWERSUPPLY_SETMODE:
+	case CG2900_FM_DO_GEN_SELECTREFERENCECLOCK:
+	case CG2900_FM_DO_GEN_SETPROCESSINGCLOCK:
+	case CG2900_FM_DO_GEN_SETREFERENCECLOCKPLL:
+	case CG2900_FM_DO_TST_TX_RAMP_START:
+		retval = true;
+		break;
+
+	default:
+		break;
+	}
+
+	if (retval)
+		dev_dbg(info->dev, "Following interrupt event expected for this"
+			" Cmd complete evt: cmd_id = 0x%X\n",
+			cmd_id);
+
+	return retval;
+}
+
+/**
+ * fm_is_do_cmd_irpt() - Check if irpt_val is one of the FM DO command related interrupts.
+ * @irpt_val:	interrupt value.
+ *
+ * Returns:
+ *   true if it's do-command related interrupt value.
+ *   false if it's not.
+ */
+static bool fm_is_do_cmd_irpt(u16 irpt_val)
+{
+	if ((irpt_val & CG2900_FM_IRPT_OPERATION_SUCCEEDED) ||
+	    (irpt_val & CG2900_FM_IRPT_OPERATION_FAILED)) {
+		dev_dbg(MAIN_DEV, "Irpt evt for FM do-command found, "
+			"irpt_val = 0x%X\n", irpt_val);
+		return true;
+	}
+
+	return false;
+}
+
+/**
+ * fm_reset_flow_ctrl - Clears up internal FM flow control.
+ *
+ * Resets outstanding commands and clear FM TX list and set CG2900 FM mode to
+ * idle.
+ */
+static void fm_reset_flow_ctrl(struct cg2900_chip_info *info)
+{
+	dev_dbg(info->dev, "fm_reset_flow_ctrl\n");
+
+	skb_queue_purge(&info->tx_queue_fm);
+
+	/* Reset the fm_cmd_id. */
+	info->audio_fm_cmd_id = CG2900_FM_CMD_NONE;
+	info->hci_fm_cmd_func = CG2900_FM_CMD_PARAM_NONE;
+
+	info->fm_radio_mode = FM_RADIO_MODE_IDLE;
+}
+
+
+/**
+ * fm_parse_cmd - Parses a FM command packet.
+ * @data:	FM command packet.
+ * @cmd_func:	Out: FM legacy command function.
+ * @cmd_id:	Out: FM legacy command ID.
+ */
+static void fm_parse_cmd(u8 *data, u8 *cmd_func, u16 *cmd_id)
+{
+	/* Move past H4-header to start of actual package */
+	struct fm_leg_cmd *pkt = (struct fm_leg_cmd *)(data + HCI_H4_SIZE);
+
+	*cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	*cmd_id   = CG2900_FM_CMD_NONE;
+
+	if (pkt->opcode != CG2900_FM_GEN_ID_LEGACY) {
+		dev_err(MAIN_DEV, "fm_parse_cmd: Not an FM legacy command "
+			"0x%02X\n", pkt->opcode);
+		return;
+	}
+
+	*cmd_func = pkt->fm_function;
+	if (*cmd_func == CG2900_FM_CMD_PARAM_WRITECOMMAND)
+		*cmd_id = cg2900_get_fm_cmd_id(le16_to_cpu(pkt->fm_cmd.head));
+}
+
+
+/**
+ * fm_parse_event - Parses a FM event packet
+ * @data:	FM event packet.
+ * @event:	Out: FM event.
+ * @cmd_func:	Out: FM legacy command function.
+ * @cmd_id:	Out: FM legacy command ID.
+ * @intr_val:	Out: FM interrupt value.
+ */
+static void fm_parse_event(u8 *data, u8 *event, u8 *cmd_func, u16 *cmd_id,
+			   u16 *intr_val)
+{
+	/* Move past H4-header to start of actual package */
+	union fm_leg_evt_or_irq *pkt = (union fm_leg_evt_or_irq *)data;
+
+	*cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	*cmd_id = CG2900_FM_CMD_NONE;
+	*intr_val = 0;
+	*event = CG2900_FM_EVENT_UNKNOWN;
+
+	if (pkt->evt.opcode == CG2900_FM_GEN_ID_LEGACY &&
+	    pkt->evt.read_write == CG2900_FM_CMD_LEG_PARAM_WRITE) {
+		/* Command complete */
+		*event = CG2900_FM_EVENT_CMD_COMPLETE;
+		*cmd_func = pkt->evt.fm_function;
+		if (*cmd_func == CG2900_FM_CMD_PARAM_WRITECOMMAND)
+			*cmd_id = cg2900_get_fm_cmd_id(
+				le16_to_cpu(pkt->evt.response_head));
+	} else if (pkt->irq_v2.opcode == CG2900_FM_GEN_ID_LEGACY &&
+		   pkt->irq_v2.event_type == CG2900_FM_CMD_LEG_PARAM_IRQ) {
+		/* Interrupt, PG2 style */
+		*event = CG2900_FM_EVENT_INTERRUPT;
+		*intr_val = le16_to_cpu(pkt->irq_v2.irq);
+	} else if (pkt->irq_v1.opcode == CG2900_FM_GEN_ID_LEGACY) {
+		/* Interrupt, PG1 style */
+		*event = CG2900_FM_EVENT_INTERRUPT;
+		*intr_val = le16_to_cpu(pkt->irq_v1.irq);
+	} else
+		dev_err(MAIN_DEV, "fm_parse_event: Not an FM legacy command "
+			"0x%X %X %X %X\n", data[0], data[1], data[2], data[3]);
+}
+
+/**
+ * fm_update_mode - Updates the FM mode state machine.
+ * @data:	FM command packet.
+ *
+ * Parses a FM command packet and updates the FM mode state machine.
+ */
+static void fm_update_mode(struct cg2900_chip_info *info, u8 *data)
+{
+	u8 cmd_func;
+	u16 cmd_id;
+
+	fm_parse_cmd(data, &cmd_func, &cmd_id);
+
+	if (cmd_func == CG2900_FM_CMD_PARAM_WRITECOMMAND &&
+	    cmd_id == CG2900_FM_DO_GEN_GOTOMODE) {
+		/* Move past H4-header to start of actual package */
+		struct fm_leg_cmd *pkt =
+			(struct fm_leg_cmd *)(data + HCI_H4_SIZE);
+
+		info->fm_radio_mode = le16_to_cpu(pkt->fm_cmd.data[0]);
+		dev_dbg(info->dev, "FM Radio mode changed to %d\n",
+			info->fm_radio_mode);
+	}
+}
+
+
+/**
+ * transmit_skb_from_tx_queue_bt() - Check flow control info and transmit skb.
+ *
+ * The transmit_skb_from_tx_queue_bt() function checks if there are tickets
+ * available and commands waiting in the TX queue and if so transmits them
+ * to the controller.
+ * It shall always be called within spinlock_bh.
+ */
+static void transmit_skb_from_tx_queue_bt(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_user_data *user;
+	struct cg2900_chip_info *info = dev->c_data;
+	struct sk_buff *skb;
+
+	dev_dbg(dev->dev, "transmit_skb_from_tx_queue_bt\n");
+
+	/* Dequeue an skb from the head of the list */
+	skb = skb_dequeue(&info->tx_queue_bt);
+	while (skb) {
+		if (info->tx_nr_pkts_allowed_bt <= 0) {
+			/*
+			 * If no more packets allowed just return, we'll get
+			 * back here after next Command Complete/Status event.
+			 * Put skb back at head of queue.
+			 */
+			skb_queue_head(&info->tx_queue_bt, skb);
+			return;
+		}
+
+		(info->tx_nr_pkts_allowed_bt)--;
+		dev_dbg(dev->dev, "tx_nr_pkts_allowed_bt = %d\n",
+			info->tx_nr_pkts_allowed_bt);
+
+		user = cg2900_skb_data(skb)->user; /* user is never NULL */
+
+		/*
+		 * If it's a command from audio application, store the OpCode,
+		 * it'll be used later to decide where to dispatch
+		 * the Command Complete event.
+		 */
+		if (info->bt_audio == user) {
+			struct hci_command_hdr *hdr = (struct hci_command_hdr *)
+				(skb->data + HCI_H4_SIZE);
+
+			info->audio_bt_cmd_op = le16_to_cpu(hdr->opcode);
+			dev_dbg(user->dev,
+				"Sending cmd from audio driver, saving "
+				"OpCode = 0x%04X\n", info->audio_bt_cmd_op);
+		}
+
+		cg2900_tx_to_chip(user, info->logger, skb);
+
+		/* Dequeue an skb from the head of the list */
+		skb = skb_dequeue(&info->tx_queue_bt);
+	}
+}
+
+/**
+ * transmit_skb_from_tx_queue_fm() - Check flow control info and transmit skb.
+ *
+ * The transmit_skb_from_tx_queue_fm() function checks if it possible to
+ * transmit and commands waiting in the TX queue and if so transmits them
+ * to the controller.
+ * It shall always be called within spinlock_bh.
+ */
+static void transmit_skb_from_tx_queue_fm(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_user_data *user;
+	struct cg2900_chip_info *info = dev->c_data;
+	struct sk_buff *skb;
+
+	dev_dbg(dev->dev, "transmit_skb_from_tx_queue_fm\n");
+
+	/* Dequeue an skb from the head of the list */
+	skb = skb_dequeue(&info->tx_queue_fm);
+	while (skb) {
+		u16 cmd_id;
+		u8 cmd_func;
+
+		if (info->audio_fm_cmd_id != CG2900_FM_CMD_NONE ||
+		    info->hci_fm_cmd_func != CG2900_FM_CMD_PARAM_NONE) {
+			/*
+			 * There are currently outstanding FM commands.
+			 * Wait for them to finish. We will get back here later.
+			 * Queue back the skb at head of list.
+			 */
+			skb_queue_head(&info->tx_queue_bt, skb);
+			return;
+		}
+
+		user = cg2900_skb_data(skb)->user; /* user is never NULL */
+
+		if (!user->opened) {
+			/*
+			 * Channel is not open. That means that the user that
+			 * originally sent it has deregistered.
+			 * Just throw it away and check the next skb in the
+			 * queue.
+			 */
+			kfree_skb(skb);
+			/* Dequeue an skb from the head of the list */
+			skb = skb_dequeue(&info->tx_queue_fm);
+			continue;
+		}
+
+		fm_parse_cmd(&(skb->data[0]), &cmd_func, &cmd_id);
+
+		/*
+		 * Store the FM command function , it'll be used later to decide
+		 * where to dispatch the Command Complete event.
+		 */
+		if (info->fm_audio == user) {
+			info->audio_fm_cmd_id = cmd_id;
+			dev_dbg(user->dev, "Sending FM audio cmd 0x%04X\n",
+				info->audio_fm_cmd_id);
+		} else {
+			/* FM radio command */
+			info->hci_fm_cmd_func = cmd_func;
+			fm_update_mode(info, &skb->data[0]);
+			dev_dbg(user->dev, "Sending FM radio cmd 0x%04X\n",
+				info->hci_fm_cmd_func);
+		}
+
+		/*
+		 * We have only one ticket on FM. Just return after
+		 * sending the skb.
+		 */
+		cg2900_tx_to_chip(user, info->logger, skb);
+		return;
+	}
+}
+
+/**
+ * update_flow_ctrl_bt() - Update number of outstanding commands for BT CMD.
+ * @dev:	Current chip device.
+ * @skb:	skb with received packet.
+ *
+ * The update_flow_ctrl_bt() checks if incoming data packet is
+ * BT Command Complete/Command Status Event and if so updates number of tickets
+ * and number of outstanding commands. It also calls function to send queued
+ * commands (if the list of queued commands is not empty).
+ */
+static void update_flow_ctrl_bt(struct cg2900_chip_dev *dev,
+				const struct sk_buff * const skb)
+{
+	u8 *data = skb->data;
+	struct hci_event_hdr *event;
+	struct cg2900_chip_info *info = dev->c_data;
+
+	event = (struct hci_event_hdr *)data;
+	data += sizeof(*event);
+
+	if (HCI_EV_CMD_COMPLETE == event->evt) {
+		struct hci_ev_cmd_complete *complete;
+		complete = (struct hci_ev_cmd_complete *)data;
+
+		/*
+		 * If it's HCI Command Complete Event then we might get some
+		 * HCI tickets back. Also we can decrease the number outstanding
+		 * HCI commands (if it's not NOP command or one of the commands
+		 * that generate both Command Status Event and Command Complete
+		 * Event).
+		 * Check if we have any HCI commands waiting in the TX list and
+		 * send them if there are tickets available.
+		 */
+		spin_lock_bh(&info->tx_bt_lock);
+		info->tx_nr_pkts_allowed_bt = complete->ncmd;
+		dev_dbg(dev->dev, "New tx_nr_pkts_allowed_bt = %d\n",
+			info->tx_nr_pkts_allowed_bt);
+
+		if (!skb_queue_empty(&info->tx_queue_bt))
+			transmit_skb_from_tx_queue_bt(dev);
+		spin_unlock_bh(&info->tx_bt_lock);
+	} else if (HCI_EV_CMD_STATUS == event->evt) {
+		struct hci_ev_cmd_status *status;
+		status = (struct hci_ev_cmd_status *)data;
+
+		/*
+		 * If it's HCI Command Status Event then we might get some
+		 * HCI tickets back. Also we can decrease the number outstanding
+		 * HCI commands (if it's not NOP command).
+		 * Check if we have any HCI commands waiting in the TX queue and
+		 * send them if there are tickets available.
+		 */
+		spin_lock_bh(&info->tx_bt_lock);
+		info->tx_nr_pkts_allowed_bt = status->ncmd;
+		dev_dbg(dev->dev, "New tx_nr_pkts_allowed_bt = %d\n",
+			info->tx_nr_pkts_allowed_bt);
+
+		if (!skb_queue_empty(&info->tx_queue_bt))
+			transmit_skb_from_tx_queue_bt(dev);
+		spin_unlock_bh(&info->tx_bt_lock);
+	}
+}
+
+/**
+ * update_flow_ctrl_fm() - Update packets allowed for FM channel.
+ * @dev:	Current chip device.
+ * @skb:	skb with received packet.
+ *
+ * The update_flow_ctrl_fm() checks if incoming data packet is FM packet
+ * indicating that the previous command has been handled and if so update
+ * packets. It also calls function to send queued commands (if the list of
+ * queued commands is not empty).
+ */
+static void update_flow_ctrl_fm(struct cg2900_chip_dev *dev,
+				const struct sk_buff * const skb)
+{
+	u8 cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	u16 cmd_id = CG2900_FM_CMD_NONE;
+	u16 irpt_val = 0;
+	u8 event = CG2900_FM_EVENT_UNKNOWN;
+	struct cg2900_chip_info *info = dev->c_data;
+
+	fm_parse_event(&(skb->data[0]), &event, &cmd_func, &cmd_id, &irpt_val);
+
+	if (event == CG2900_FM_EVENT_CMD_COMPLETE) {
+		/* FM legacy command complete event */
+		spin_lock_bh(&info->tx_fm_lock);
+		/*
+		 * Check if it's not an write command complete event, because
+		 * then it cannot be a DO command.
+		 * If it's a write command complete event check that is not a
+		 * DO command complete event before setting the outstanding
+		 * FM packets to none.
+		 */
+		if (cmd_func != CG2900_FM_CMD_PARAM_WRITECOMMAND ||
+		    !fm_irpt_expected(info, cmd_id)) {
+			info->hci_fm_cmd_func = CG2900_FM_CMD_PARAM_NONE;
+			info->audio_fm_cmd_id = CG2900_FM_CMD_NONE;
+			dev_dbg(dev->dev,
+				"FM_Write: Outstanding FM commands:\n"
+				"\tRadio: 0x%04X\n"
+				"\tAudio: 0x%04X\n",
+				info->hci_fm_cmd_func,
+				info->audio_fm_cmd_id);
+			transmit_skb_from_tx_queue_fm(dev);
+
+		/*
+		 * If there was a write do command complete event check if it is
+		 * DO command previously sent by the FM audio user. If that's
+		 * the case we need remember that in order to be able to
+		 * dispatch the interrupt to the correct user.
+		 */
+		} else if (cmd_id == info->audio_fm_cmd_id) {
+			info->tx_fm_audio_awaiting_irpt = true;
+			dev_dbg(dev->dev,
+				"FM Audio waiting for interrupt = true\n");
+		}
+		spin_unlock_bh(&info->tx_fm_lock);
+	} else if (event == CG2900_FM_EVENT_INTERRUPT) {
+		/* FM legacy interrupt */
+		if (fm_is_do_cmd_irpt(irpt_val)) {
+			/*
+			 * If it is an interrupt related to a DO command update
+			 * the outstanding flow control and transmit blocked
+			 * FM commands.
+			 */
+			spin_lock_bh(&info->tx_fm_lock);
+			info->hci_fm_cmd_func = CG2900_FM_CMD_PARAM_NONE;
+			info->audio_fm_cmd_id = CG2900_FM_CMD_NONE;
+			dev_dbg(dev->dev,
+				"FM_INT: Outstanding FM commands:\n"
+				"\tRadio: 0x%04X\n"
+				"\tAudio: 0x%04X\n",
+				info->hci_fm_cmd_func,
+				info->audio_fm_cmd_id);
+			info->tx_fm_audio_awaiting_irpt = false;
+			dev_dbg(dev->dev,
+				"FM Audio waiting for interrupt = false\n");
+			transmit_skb_from_tx_queue_fm(dev);
+			spin_unlock_bh(&info->tx_fm_lock);
+		}
+	}
+}
+
+/**
+ * send_bt_enable() - Send HCI VS BT Enable command to the chip.
+ * @info:	Chip info structure.
+ * @bt_enable:	Value for BT Enable parameter (e.g. CG2900_BT_DISABLE).
+ */
+static void send_bt_enable(struct cg2900_chip_info *info, u8 bt_enable)
+{
+	struct bt_vs_bt_enable_cmd cmd;
+
+	cmd.op_code = cpu_to_le16(CG2900_BT_OP_VS_BT_ENABLE);
+	cmd.plen = BT_PARAM_LEN(sizeof(cmd));
+	cmd.enable = bt_enable;
+	cg2900_send_bt_cmd(info->user_in_charge, info->logger,
+			   &cmd, sizeof(cmd));
+}
+
+/**
+ * send_bd_address() - Send HCI VS command with BD address to the chip.
+ */
+static void send_bd_address(struct cg2900_chip_info *info)
+{
+	struct bt_vs_store_in_fs_cmd *cmd;
+	u8 plen = sizeof(*cmd) + BT_BDADDR_SIZE;
+
+	cmd = kmalloc(plen, GFP_KERNEL);
+	if (!cmd) {
+		dev_err(info->dev, "send_bd_address could not allocate cmd\n");
+		return;
+	}
+
+	cmd->opcode = cpu_to_le16(CG2900_BT_OP_VS_STORE_IN_FS);
+	cmd->plen = BT_PARAM_LEN(plen);
+	cmd->user_id = CG2900_VS_STORE_IN_FS_USR_ID_BD_ADDR;
+	cmd->len = BT_BDADDR_SIZE;
+	/* Now copy the BD address received from user space control app. */
+	memcpy(cmd->data, bd_address, BT_BDADDR_SIZE);
+
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_SEND_BD_ADDRESS\n");
+	info->boot_state = BOOT_SEND_BD_ADDRESS;
+
+	cg2900_send_bt_cmd(info->user_in_charge, info->logger, cmd, plen);
+
+	kfree(cmd);
+}
+
+/**
+ * send_settings_file() - Transmit settings file.
+ *
+ * The send_settings_file() function transmit settings file.
+ * The file is read in parts to fit in HCI packets. When finished,
+ * close the settings file and send HCI reset to activate settings and patches.
+ */
+static void send_settings_file(struct cg2900_chip_info *info)
+{
+	int bytes_sent;
+
+	bytes_sent = cg2900_read_and_send_file_part(info->user_in_charge,
+						    info->logger,
+						    &info->file_info);
+	if (bytes_sent > 0) {
+		/* Data sent. Wait for CmdComplete */
+		return;
+	} else if (bytes_sent < 0) {
+		dev_err(BOOT_DEV, "send_settings_file: Error %d occurred\n",
+			bytes_sent);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		chip_startup_finished(info, bytes_sent);
+		return;
+	}
+
+	/* No data was sent. This file is finished */
+	info->download_state = DOWNLOAD_SUCCESS;
+
+	/* Settings file finished. Release used resources */
+	dev_dbg(BOOT_DEV, "Settings file finished, release used resources\n");
+	release_firmware(info->file_info.fw_file);
+	info->file_info.fw_file = NULL;
+
+	dev_dbg(BOOT_DEV, "New file_load_state: FILE_LOAD_NO_MORE_FILES\n");
+	info->file_load_state = FILE_LOAD_NO_MORE_FILES;
+
+	/* Create and send HCI VS Store In FS command with bd address. */
+	send_bd_address(info);
+}
+
+/**
+ * send_patch_file - Transmit patch file.
+ *
+ * The send_patch_file() function transmit patch file.
+ * The file is read in parts to fit in HCI packets. When the complete file is
+ * transmitted, the file is closed.
+ * When finished, continue with settings file.
+ */
+static void send_patch_file(struct cg2900_chip_dev *dev)
+{
+	int err;
+	int bytes_sent;
+	struct cg2900_chip_info *info = dev->c_data;
+	int file_name_size = strlen("CG2900_XXXX_XXXX_settings.fw");
+
+	bytes_sent = cg2900_read_and_send_file_part(info->user_in_charge,
+						    info->logger,
+						    &info->file_info);
+	if (bytes_sent > 0) {
+		/* Data sent. Wait for CmdComplete */
+		return;
+	} else if (bytes_sent < 0) {
+		dev_err(BOOT_DEV, "send_patch_file: Error %d occurred\n",
+			bytes_sent);
+		err = bytes_sent;
+		goto error_handling;
+	}
+
+	/* No data was sent. This file is finished */
+	info->download_state = DOWNLOAD_SUCCESS;
+
+	dev_dbg(BOOT_DEV, "Patch file finished, release used resources\n");
+	release_firmware(info->file_info.fw_file);
+	info->file_info.fw_file = NULL;
+
+	/*
+	 * Create the settings file name from HCI revision and sub_version.
+	 * file_name_size does not include terminating NULL character
+	 * so add 1.
+	 */
+	err = snprintf(info->settings_file_name, file_name_size + 1,
+			"CG2900_%04X_%04X_settings.fw", dev->chip.hci_revision,
+			dev->chip.hci_sub_version);
+	if (err == file_name_size) {
+		dev_dbg(BOOT_DEV, "Downloading settings file %s\n",
+				info->settings_file_name);
+	} else {
+		dev_err(BOOT_DEV, "Settings file name failed! err=%d\n", err);
+		goto error_handling;
+	}
+
+	/* Retrieve the settings file */
+	err = request_firmware(&info->file_info.fw_file,
+			       info->settings_file_name,
+			       info->dev);
+	if (err) {
+		dev_err(BOOT_DEV, "Couldn't get settings file (%d)\n", err);
+		goto error_handling;
+	}
+	/* Now send the settings file */
+	dev_dbg(BOOT_DEV,
+		"New file_load_state: FILE_LOAD_GET_STATIC_SETTINGS\n");
+	info->file_load_state = FILE_LOAD_GET_STATIC_SETTINGS;
+	dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_PENDING\n");
+	info->download_state = DOWNLOAD_PENDING;
+	send_settings_file(info);
+	return;
+
+error_handling:
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+	info->boot_state = BOOT_FAILED;
+	chip_startup_finished(info, err);
+}
+
+/**
+ * work_power_off_chip() - Work item to power off the chip.
+ * @work:	Reference to work data.
+ *
+ * The work_power_off_chip() function handles transmission of the HCI command
+ * vs_power_switch_off and then informs the CG2900 Core that this chip driver is
+ * finished and the Core driver can now shut off the chip.
+ */
+static void work_power_off_chip(struct work_struct *work)
+{
+	struct sk_buff *skb = NULL;
+	u8 *h4_header;
+	struct cg2900_platform_data *pf_data;
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_power_off_chip: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	/*
+	 * Get the VS Power Switch Off command to use based on connected
+	 * connectivity controller
+	 */
+	pf_data = dev_get_platdata(dev->dev);
+	if (pf_data->get_power_switch_off_cmd)
+		skb = pf_data->get_power_switch_off_cmd(dev, NULL);
+
+	/*
+	 * Transmit the received command.
+	 * If no command found for the device, just continue
+	 */
+	if (!skb) {
+		dev_err(dev->dev,
+			"Could not retrieve PowerSwitchOff command\n");
+		goto shut_down_chip;
+	}
+
+	dev_dbg(dev->dev,
+		"Got power_switch_off command. Add H4 header and transmit\n");
+
+	/*
+	 * Move the data pointer to the H:4 header position and store
+	 * the H4 header
+	 */
+	h4_header = skb_push(skb, CG2900_SKB_RESERVE);
+	*h4_header = CHANNEL_BT_CMD;
+
+	dev_dbg(dev->dev, "New closing_state: CLOSING_POWER_SWITCH_OFF\n");
+	info->closing_state = CLOSING_POWER_SWITCH_OFF;
+
+	if (info->user_in_charge)
+		cg2900_tx_to_chip(info->user_in_charge, info->logger, skb);
+	else
+		cg2900_tx_no_user(dev, skb);
+
+	/*
+	 * Mandatory to wait 500ms after the power_switch_off command has been
+	 * transmitted, in order to make sure that the controller is ready.
+	 */
+	schedule_timeout_killable(msecs_to_jiffies(POWER_SW_OFF_WAIT));
+
+shut_down_chip:
+	dev_dbg(dev->dev, "New closing_state: CLOSING_SHUT_DOWN\n");
+	info->closing_state = CLOSING_SHUT_DOWN;
+
+	/* Close the transport, which will power off the chip */
+	if (dev->t_cb.close)
+		dev->t_cb.close(dev);
+
+	/* Chip shut-down finished, set correct state and wake up the chip. */
+	dev_dbg(dev->dev, "New main_state: CG2900_IDLE\n");
+	info->main_state = CG2900_IDLE;
+	wake_up_all(&main_wait_queue);
+
+	kfree(my_work);
+}
+
+/**
+ * work_chip_shutdown() - Shut down the chip.
+ * @work:	Reference to work data.
+ */
+static void work_chip_shutdown(struct work_struct *work)
+{
+	struct cg2900_work *my_work;
+	struct cg2900_user_data *user;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_chip_shutdown: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	user = my_work->user_data;
+
+	chip_shutdown(user);
+
+	kfree(my_work);
+}
+
+/**
+ * work_reset_after_error() - Handle reset.
+ * @work:	Reference to work data.
+ *
+ * Handle a reset after received Command Complete event.
+ */
+static void work_reset_after_error(struct work_struct *work)
+{
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_reset_after_error: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	chip_startup_finished(info, -EIO);
+
+	kfree(my_work);
+}
+
+/**
+ * work_load_patch_and_settings() - Start loading patches and settings.
+ * @work:	Reference to work data.
+ */
+static void work_load_patch_and_settings(struct work_struct *work)
+{
+	int err = 0;
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+	int file_name_size = strlen("CG2900_XXXX_XXXX_patch.fw");
+
+	if (!work) {
+		dev_err(MAIN_DEV,
+			"work_load_patch_and_settings: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	/* Check that we are in the right state */
+	if (info->boot_state != BOOT_GET_FILES_TO_LOAD)
+		goto finished;
+
+	/*
+	 * Create the patch file name from HCI revision and sub_version.
+	 * file_name_size does not include terminating NULL character
+	 * so add 1.
+	 */
+	err = snprintf(info->patch_file_name, file_name_size + 1,
+			"CG2900_%04X_%04X_patch.fw", dev->chip.hci_revision,
+			dev->chip.hci_sub_version);
+	if (err == file_name_size) {
+		dev_dbg(BOOT_DEV, "Downloading patch file %s\n",
+				info->patch_file_name);
+	} else {
+		dev_err(BOOT_DEV, "Patch file name failed! err=%d\n", err);
+		goto error_handling;
+	}
+
+	/* We now all info needed */
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_DOWNLOAD_PATCH\n");
+	info->boot_state = BOOT_DOWNLOAD_PATCH;
+	dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_PENDING\n");
+	info->download_state = DOWNLOAD_PENDING;
+	dev_dbg(BOOT_DEV, "New file_load_state: FILE_LOAD_GET_PATCH\n");
+	info->file_load_state = FILE_LOAD_GET_PATCH;
+	info->file_info.chunk_id = 0;
+	info->file_info.file_offset = 0;
+	info->file_info.fw_file = NULL;
+
+	/* OK. Now it is time to download the patches */
+	err = request_firmware(&(info->file_info.fw_file),
+			       info->patch_file_name,
+			       dev->dev);
+	if (err < 0) {
+		dev_err(BOOT_DEV, "Couldn't get patch file (%d)\n", err);
+		goto error_handling;
+	}
+	send_patch_file(dev);
+
+	goto finished;
+
+error_handling:
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+	info->boot_state = BOOT_FAILED;
+	chip_startup_finished(info, -EIO);
+finished:
+	kfree(my_work);
+}
+
+/**
+ * work_cont_file_download() - A file block has been written.
+ * @work:	Reference to work data.
+ *
+ * Handle a received HCI VS Write File Block Complete event.
+ * Normally this means continue to send files to the controller.
+ */
+static void work_cont_file_download(struct work_struct *work)
+{
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_cont_file_download: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	/* Continue to send patches or settings to the controller */
+	if (info->file_load_state == FILE_LOAD_GET_PATCH)
+		send_patch_file(dev);
+	else if (info->file_load_state == FILE_LOAD_GET_STATIC_SETTINGS)
+		send_settings_file(info);
+	else
+		dev_dbg(BOOT_DEV, "No more files to load\n");
+
+	kfree(my_work);
+}
+
+/**
+ * work_send_read_selftest_cmd() - HCI VS Read_SelfTests_Result command shall be sent.
+ * @work:	Reference to work data.
+ */
+static void work_send_read_selftest_cmd(struct work_struct *work)
+{
+	struct delayed_work *del_work;
+	struct cg2900_delayed_work_struct *current_work;
+	struct cg2900_chip_info *info;
+	struct hci_command_hdr cmd;
+
+	if (!work) {
+		dev_err(MAIN_DEV,
+			"work_send_read_selftest_cmd: work == NULL\n");
+		return;
+	}
+
+	del_work = to_delayed_work(work);
+	current_work = container_of(del_work,
+				    struct cg2900_delayed_work_struct, work);
+	info = current_work->data;
+
+	if (info->boot_state != BOOT_READ_SELFTEST_RESULT)
+		return;
+
+	cmd.opcode = cpu_to_le16(CG2900_BT_OP_VS_READ_SELTESTS_RESULT);
+	cmd.plen = 0; /* No parameters for Read Selftests Result */
+	cg2900_send_bt_cmd(info->user_in_charge, info->logger, &cmd,
+			   sizeof(cmd));
+}
+
+/**
+ * handle_reset_cmd_complete() - Handles HCI Reset Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_reset_cmd_complete(struct cg2900_chip_dev *dev, u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	dev_dbg(BOOT_DEV, "Received Reset complete event with status 0x%X\n",
+		status);
+
+	if (CG2900_CLOSING != info->main_state &&
+	    CLOSING_RESET != info->closing_state)
+		return false;
+
+	if (HCI_BT_ERROR_NO_ERROR != status) {
+		/*
+		 * Continue in case of error, the chip is going to be shut down
+		 * anyway.
+		 */
+		dev_err(BOOT_DEV, "Command complete for HciReset received with "
+			"error 0x%X\n", status);
+	}
+
+	cg2900_create_work_item(info->wq, work_power_off_chip, dev);
+
+	return true;
+}
+
+/**
+ * handle_vs_store_in_fs_cmd_complete() - Handles HCI VS StoreInFS Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_store_in_fs_cmd_complete(struct cg2900_chip_dev *dev,
+					       u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	dev_dbg(BOOT_DEV,
+		"Received Store_in_FS complete event with status 0x%X\n",
+		status);
+
+	if (info->boot_state != BOOT_SEND_BD_ADDRESS)
+		return false;
+
+	if (HCI_BT_ERROR_NO_ERROR == status) {
+		struct hci_command_hdr cmd;
+
+		/* Send HCI SystemReset command to activate patches */
+		dev_dbg(BOOT_DEV,
+			"New boot_state: BOOT_ACTIVATE_PATCHES_AND_SETTINGS\n");
+		info->boot_state = BOOT_ACTIVATE_PATCHES_AND_SETTINGS;
+
+		cmd.opcode = cpu_to_le16(CG2900_BT_OP_VS_SYSTEM_RESET);
+		cmd.plen = 0; /* No parameters for System Reset */
+		cg2900_send_bt_cmd(info->user_in_charge, info->logger, &cmd,
+				   sizeof(cmd));
+	} else {
+		dev_err(BOOT_DEV,
+			"Command complete for StoreInFS received with error "
+			"0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_write_file_block_cmd_complete() - Handles HCI VS WriteFileBlock Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_write_file_block_cmd_complete(struct cg2900_chip_dev *dev,
+						    u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DOWNLOAD_PATCH ||
+	    info->download_state != DOWNLOAD_PENDING)
+		return false;
+
+	if (HCI_BT_ERROR_NO_ERROR == status)
+		cg2900_create_work_item(info->wq, work_cont_file_download, dev);
+	else {
+		dev_err(BOOT_DEV,
+			"Command complete for WriteFileBlock received with"
+			" error 0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_FAILED\n");
+		info->download_state = DOWNLOAD_FAILED;
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		if (info->file_info.fw_file) {
+			release_firmware(info->file_info.fw_file);
+			info->file_info.fw_file = NULL;
+		}
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_write_file_block_cmd_status() - Handles HCI VS WriteFileBlock Command Status event.
+ * @status:	Returned status of WriteFileBlock command.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_write_file_block_cmd_status(struct cg2900_chip_dev *dev,
+						  u8 status)
+{
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DOWNLOAD_PATCH ||
+	    info->download_state != DOWNLOAD_PENDING)
+		return false;
+
+	/*
+	 * Only do something if there is an error. Otherwise we will wait for
+	 * CmdComplete.
+	 */
+	if (HCI_BT_ERROR_NO_ERROR != status) {
+		dev_err(BOOT_DEV,
+			"Command status for WriteFileBlock received with"
+			" error 0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_FAILED\n");
+		info->download_state = DOWNLOAD_FAILED;
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		if (info->file_info.fw_file) {
+			release_firmware(info->file_info.fw_file);
+			info->file_info.fw_file = NULL;
+		}
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_power_switch_off_cmd_complete() - Handles HCI VS PowerSwitchOff Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_power_switch_off_cmd_complete(struct cg2900_chip_dev *dev,
+						    u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (CLOSING_POWER_SWITCH_OFF != info->closing_state)
+		return false;
+
+	dev_dbg(BOOT_DEV,
+		"handle_vs_power_switch_off_cmd_complete status %d\n", status);
+
+	/*
+	 * We were waiting for this but we don't need to do anything upon
+	 * reception except warn for error status
+	 */
+	if (HCI_BT_ERROR_NO_ERROR != status)
+		dev_err(BOOT_DEV,
+			"Command Complete for PowerSwitchOff received with "
+			"error 0x%X", status);
+
+	return true;
+}
+
+/**
+ * handle_vs_system_reset_cmd_complete() - Handle HCI VS SystemReset Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_system_reset_cmd_complete(struct cg2900_chip_dev *dev,
+						u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_ACTIVATE_PATCHES_AND_SETTINGS)
+		return false;
+
+	dev_dbg(BOOT_DEV, "handle_vs_system_reset_cmd_complete status %d\n",
+		status);
+
+	if (HCI_BT_ERROR_NO_ERROR == status) {
+		if (dev->chip.hci_revision == CG2900_PG2_REV) {
+			/*
+			 * We must now wait for the selftest results. They will
+			 * take a certain amount of time to finish so start a
+			 * delayed work that will then send the command.
+			 */
+			dev_dbg(BOOT_DEV,
+				"New boot_state: BOOT_READ_SELFTEST_RESULT\n");
+			info->boot_state = BOOT_READ_SELFTEST_RESULT;
+			queue_delayed_work(info->wq, &info->selftest_work.work,
+					   msecs_to_jiffies(SELFTEST_INITIAL));
+			info->nbr_of_polls = 0;
+		} else {
+			/*
+			 * We are now almost finished. Shut off BT Core. It will
+			 * be re-enabled by the Bluetooth driver when needed.
+			 */
+			dev_dbg(BOOT_DEV, "New boot_state: BOOT_DISABLE_BT\n");
+			info->boot_state = BOOT_DISABLE_BT;
+			send_bt_enable(info, CG2900_BT_DISABLE);
+		}
+	} else {
+		dev_err(BOOT_DEV,
+			"Received Reset complete event with status 0x%X\n",
+			status);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		chip_startup_finished(info, -EIO);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_read_selftests_cmd_complete() - Handle HCI VS ReadSelfTestsResult Command Complete event.
+ * @dev:	Current chip.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_read_selftests_cmd_complete(struct cg2900_chip_dev *dev,
+						  u8 *data)
+{
+	struct bt_vs_read_selftests_result_evt *evt =
+			(struct bt_vs_read_selftests_result_evt *)data;
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_READ_SELFTEST_RESULT)
+		return false;
+
+	dev_dbg(BOOT_DEV,
+		"handle_vs_read_selftests_cmd_complete status %d result %d\n",
+		evt->status, evt->result);
+
+	if (HCI_BT_ERROR_NO_ERROR != evt->status)
+		goto err_handling;
+
+	if (CG2900_BT_SELFTEST_SUCCESSFUL == evt->result ||
+	    CG2900_BT_SELFTEST_FAILED == evt->result) {
+		if (CG2900_BT_SELFTEST_FAILED == evt->result)
+			dev_err(BOOT_DEV, "CG2900 self test failed\n");
+
+		/*
+		 * We are now almost finished. Shut off BT Core. It will
+		 * be re-enabled by the Bluetooth driver when needed.
+		 */
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_DISABLE_BT\n");
+		info->boot_state = BOOT_DISABLE_BT;
+		send_bt_enable(info, CG2900_BT_DISABLE);
+		return true;
+	} else if (CG2900_BT_SELFTEST_NOT_COMPLETED == evt->result) {
+		/*
+		 * Self tests are not yet finished. Wait some more time
+		 * before resending the command
+		 */
+		if (info->nbr_of_polls > MAX_NBR_OF_POLLS) {
+			dev_err(BOOT_DEV, "Selftest results reached max"
+				" number of polls\n");
+			goto err_handling;
+		}
+		queue_delayed_work(info->wq, &info->selftest_work.work,
+				   msecs_to_jiffies(SELFTEST_POLLING));
+		info->nbr_of_polls++;
+		return true;
+	}
+
+err_handling:
+	dev_err(BOOT_DEV,
+		"Received Read SelfTests Result complete event with "
+		"status 0x%X and result 0x%X\n",
+		evt->status, evt->result);
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+	info->boot_state = BOOT_FAILED;
+	chip_startup_finished(info, -EIO);
+	return true;
+}
+
+/**
+ * handle_vs_bt_enable_cmd_status() - Handles HCI VS BtEnable Command Status event.
+ * @status:	Returned status of BtEnable command.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_bt_enable_cmd_status(struct cg2900_chip_dev *dev,
+					   u8 status)
+{
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DISABLE_BT)
+		return false;
+
+	dev_dbg(BOOT_DEV, "handle_vs_bt_enable_cmd_status status %d\n", status);
+
+	/*
+	 * Only do something if there is an error. Otherwise we will wait for
+	 * CmdComplete.
+	 */
+	if (HCI_BT_ERROR_NO_ERROR != status) {
+		dev_err(BOOT_DEV,
+			"Received BtEnable status event with status 0x%X\n",
+			status);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		chip_startup_finished(info, -EIO);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_bt_enable_cmd_complete() - Handle HCI VS BtEnable Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_bt_enable_cmd_complete(struct cg2900_chip_dev *dev,
+					     u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DISABLE_BT)
+		return false;
+
+	dev_dbg(BOOT_DEV, "handle_vs_bt_enable_cmd_complete status %d\n",
+		status);
+
+	if (HCI_BT_ERROR_NO_ERROR == status) {
+		/*
+		 * The boot sequence is now finished successfully.
+		 * Set states and signal to waiting thread.
+		 */
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_READY\n");
+		info->boot_state = BOOT_READY;
+		chip_startup_finished(info, 0);
+	} else {
+		dev_err(BOOT_DEV,
+			"Received BtEnable complete event with status 0x%X\n",
+			status);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		chip_startup_finished(info, -EIO);
+	}
+
+	return true;
+}
+
+/**
+ * handle_rx_data_bt_evt() - Check if received data should be handled in CG2900 chip driver.
+ * @skb:	Data packet
+ *
+ * The handle_rx_data_bt_evt() function checks if received data should be
+ * handled in CG2900 chip driver. If so handle it correctly.
+ * Received data is always HCI BT Event.
+ *
+ * Returns:
+ *   True,  if packet was handled internally,
+ *   False, otherwise.
+ */
+static bool handle_rx_data_bt_evt(struct cg2900_chip_dev *dev,
+				  struct sk_buff *skb)
+{
+	bool pkt_handled = false;
+	/* skb cannot be NULL here so it is safe to de-reference */
+	u8 *data = skb->data;
+	struct hci_event_hdr *evt;
+	u16 op_code;
+
+	evt = (struct hci_event_hdr *)data;
+	data += sizeof(*evt);
+
+	/* First check the event code. */
+	if (HCI_EV_CMD_COMPLETE == evt->evt) {
+		struct hci_ev_cmd_complete *cmd_complete;
+
+		cmd_complete = (struct hci_ev_cmd_complete *)data;
+		op_code = le16_to_cpu(cmd_complete->opcode);
+		dev_dbg(dev->dev,
+			"Received Command Complete: op_code = 0x%04X\n",
+			op_code);
+		/* Move to first byte after OCF */
+		data += sizeof(*cmd_complete);
+
+		if (op_code == HCI_OP_RESET)
+			pkt_handled = handle_reset_cmd_complete(dev, data);
+		else if (op_code == CG2900_BT_OP_VS_STORE_IN_FS)
+			pkt_handled = handle_vs_store_in_fs_cmd_complete(dev,
+									 data);
+		else if (op_code == CG2900_BT_OP_VS_WRITE_FILE_BLOCK)
+			pkt_handled =
+				handle_vs_write_file_block_cmd_complete(dev,
+									data);
+		else if (op_code == CG2900_BT_OP_VS_POWER_SWITCH_OFF)
+			pkt_handled =
+				handle_vs_power_switch_off_cmd_complete(dev,
+									data);
+		else if (op_code == CG2900_BT_OP_VS_SYSTEM_RESET)
+			pkt_handled = handle_vs_system_reset_cmd_complete(dev,
+									  data);
+		else if (op_code == CG2900_BT_OP_VS_BT_ENABLE)
+			pkt_handled = handle_vs_bt_enable_cmd_complete(dev,
+								       data);
+		else if (op_code == CG2900_BT_OP_VS_READ_SELTESTS_RESULT)
+			pkt_handled = handle_vs_read_selftests_cmd_complete(dev,
+									data);
+	} else if (HCI_EV_CMD_STATUS == evt->evt) {
+		struct hci_ev_cmd_status *cmd_status;
+
+		cmd_status = (struct hci_ev_cmd_status *)data;
+
+		op_code = le16_to_cpu(cmd_status->opcode);
+
+		dev_dbg(dev->dev, "Received Command Status: op_code = 0x%04X\n",
+			op_code);
+
+		if (op_code == CG2900_BT_OP_VS_WRITE_FILE_BLOCK)
+			pkt_handled = handle_vs_write_file_block_cmd_status
+				(dev, cmd_status->status);
+		else if (op_code == CG2900_BT_OP_VS_BT_ENABLE)
+			pkt_handled = handle_vs_bt_enable_cmd_status
+				(dev, cmd_status->status);
+	} else if (HCI_EV_HW_ERROR == evt->evt) {
+		struct hci_ev_hw_error *hw_error;
+
+		hw_error = (struct hci_ev_hw_error *)data;
+		/*
+		 * Only do a printout. There might be a receiving stack that can
+		 * handle this event
+		 */
+		dev_err(dev->dev, "HW Error event received with error 0x%02X\n",
+			hw_error->hw_code);
+		return false;
+	} else
+		return false;
+
+	if (pkt_handled)
+		kfree_skb(skb);
+
+	return pkt_handled;
+}
+
+/**
+ * transmit_skb_with_flow_ctrl_bt() - Send the BT skb to the controller if it is allowed or queue it.
+ * @user:	Current user.
+ * @skb:	Data packet.
+ *
+ * The transmit_skb_with_flow_ctrl_bt() function checks if there are
+ * tickets available and if so transmits buffer to controller. Otherwise the skb
+ * and user name is stored in a list for later sending.
+ * If enabled, copy the transmitted data to the HCI logger as well.
+ */
+static void transmit_skb_with_flow_ctrl_bt(struct cg2900_user_data *user,
+					   struct sk_buff *skb)
+{
+	struct cg2900_chip_dev *dev = cg2900_get_prv(user);
+	struct cg2900_chip_info *info = dev->c_data;
+
+	/*
+	 * Because there are more users of some H4 channels (currently audio
+	 * application for BT command and FM channel) we need to have an
+	 * internal HCI command flow control in CG2900 driver.
+	 * So check here how many tickets we have and store skb in a queue if
+	 * there are no tickets left. The skb will be sent later when we get
+	 * more ticket(s).
+	 */
+	spin_lock_bh(&info->tx_bt_lock);
+
+	if (info->tx_nr_pkts_allowed_bt > 0) {
+		info->tx_nr_pkts_allowed_bt--;
+		dev_dbg(user->dev, "New tx_nr_pkts_allowed_bt = %d\n",
+			info->tx_nr_pkts_allowed_bt);
+
+		/*
+		 * If it's command from audio app store the OpCode,
+		 * it'll be used later to decide where to dispatch Command
+		 * Complete event.
+		 */
+		if (info->bt_audio == user) {
+			struct hci_command_hdr *hdr = (struct hci_command_hdr *)
+				(skb->data + HCI_H4_SIZE);
+
+			info->audio_bt_cmd_op = le16_to_cpu(hdr->opcode);
+			dev_dbg(user->dev,
+				"Sending cmd from audio driver, saving "
+				"OpCode = 0x%X\n",
+				info->audio_bt_cmd_op);
+		}
+
+		cg2900_tx_to_chip(user, info->logger, skb);
+	} else {
+		dev_dbg(user->dev, "Not allowed to send cmd to controller, "
+			"storing in TX queue\n");
+
+		cg2900_skb_data(skb)->user = user;
+		skb_queue_tail(&info->tx_queue_bt, skb);
+	}
+	spin_unlock_bh(&info->tx_bt_lock);
+}
+
+/**
+ * transmit_skb_with_flow_ctrl_fm() - Send the FM skb to the controller if it is allowed or queue it.
+ * @user:	Current user.
+ * @skb:	Data packet.
+ *
+ * The transmit_skb_with_flow_ctrl_fm() function checks if chip is available and
+ * if so transmits buffer to controller. Otherwise the skb and user name is
+ * stored in a list for later sending.
+ * Also it updates the FM radio mode if it's FM GOTOMODE command, this is needed
+ * to know how to handle some FM DO commands complete events.
+ * If enabled, copy the transmitted data to the HCI logger as well.
+ */
+static void transmit_skb_with_flow_ctrl_fm(struct cg2900_user_data *user,
+					   struct sk_buff *skb)
+{
+	u8 cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	u16 cmd_id = CG2900_FM_CMD_NONE;
+	struct cg2900_chip_dev *dev = cg2900_get_prv(user);
+	struct cg2900_chip_info *info = dev->c_data;
+
+	fm_parse_cmd(&(skb->data[0]), &cmd_func, &cmd_id);
+
+	/*
+	 * If this is an FM IP disable or reset send command and also reset
+	 * the flow control and audio user.
+	 */
+	if (cmd_func == CG2900_FM_CMD_PARAM_DISABLE ||
+	    cmd_func == CG2900_FM_CMD_PARAM_RESET) {
+		spin_lock_bh(&info->tx_fm_lock);
+		fm_reset_flow_ctrl(info);
+		spin_unlock_bh(&info->tx_fm_lock);
+		cg2900_tx_to_chip(user, info->logger, skb);
+		return;
+	}
+
+	/*
+	 * If this is a FM user and no FM audio user command pending just send
+	 * FM command. It is up to the user of the FM channel to handle its own
+	 * flow control.
+	 */
+	spin_lock_bh(&info->tx_fm_lock);
+	if (info->fm_audio != user &&
+	    info->audio_fm_cmd_id == CG2900_FM_CMD_NONE) {
+		info->hci_fm_cmd_func = cmd_func;
+		dev_dbg(user->dev, "Sending FM radio command 0x%04X\n",
+			info->hci_fm_cmd_func);
+		/* If a GotoMode command update FM mode */
+		fm_update_mode(info, &skb->data[0]);
+		cg2900_tx_to_chip(user, info->logger, skb);
+	} else if (info->fm_audio == user &&
+		   info->hci_fm_cmd_func == CG2900_FM_CMD_PARAM_NONE &&
+		   info->audio_fm_cmd_id == CG2900_FM_CMD_NONE) {
+		/*
+		 * If it's command from fm audio user store the command id.
+		 * It'll be used later to decide where to dispatch
+		 * command complete event.
+		 */
+		info->audio_fm_cmd_id = cmd_id;
+		dev_dbg(user->dev, "Sending FM audio command 0x%04X\n",
+			info->audio_fm_cmd_id);
+		cg2900_tx_to_chip(user, info->logger, skb);
+	} else {
+		dev_dbg(user->dev,
+			"Not allowed to send FM cmd to controller, storing in "
+			"TX queue\n");
+
+		cg2900_skb_data(skb)->user = user;
+		skb_queue_tail(&info->tx_queue_fm, skb);
+	}
+	spin_unlock_bh(&info->tx_fm_lock);
+}
+
+/**
+ * is_bt_audio_user() - Checks if this packet is for the BT audio user.
+ * @info:	CG2900 info.
+ * @h4_channel:	H:4 channel for this packet.
+ * @skb:	Packet to check.
+ *
+ * Returns:
+ *   true if packet is for BT audio user.
+ *   false otherwise.
+ */
+static bool is_bt_audio_user(struct cg2900_chip_info *info, int h4_channel,
+			     const struct sk_buff * const skb)
+{
+	struct hci_event_hdr *hdr;
+	u8 *payload;
+	u16 opcode;
+
+	if (h4_channel != CHANNEL_BT_EVT)
+		return false;
+
+	hdr = (struct hci_event_hdr *)skb->data;
+	payload = (u8 *)(hdr + 1); /* follows header */
+
+	if (HCI_EV_CMD_COMPLETE == hdr->evt)
+		opcode = le16_to_cpu(
+			((struct hci_ev_cmd_complete *)payload)->opcode);
+	else if (HCI_EV_CMD_STATUS == hdr->evt)
+		opcode = le16_to_cpu(
+			((struct hci_ev_cmd_status *)payload)->opcode);
+	else
+		return false;
+
+	if (opcode != info->audio_bt_cmd_op)
+		return false;
+
+	dev_dbg(info->bt_audio->dev, "Audio BT OpCode match = 0x%04X\n",
+		opcode);
+	info->audio_bt_cmd_op = CG2900_BT_OPCODE_NONE;
+	return true;
+}
+
+/**
+ * is_fm_audio_user() - Checks if this packet is for the FM audio user.
+ * @info:	CG2900 info.
+ * @h4_channel:	H:4 channel for this packet.
+ * @skb:	Packet to check.
+ *
+ * Returns:
+ *   true if packet is for BT audio user.
+ *   false otherwise.
+ */
+static bool is_fm_audio_user(struct cg2900_chip_info *info, int h4_channel,
+			     const struct sk_buff * const skb)
+{
+	u8 cmd_func;
+	u16 cmd_id;
+	u16 irpt_val;
+	u8 event;
+
+	if (h4_channel != CHANNEL_FM_RADIO)
+		return false;
+
+	cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	cmd_id = CG2900_FM_CMD_NONE;
+	irpt_val = 0;
+	event = CG2900_FM_EVENT_UNKNOWN;
+
+	fm_parse_event(&skb->data[0], &event, &cmd_func, &cmd_id,
+		       &irpt_val);
+	/* Check if command complete event FM legacy interface. */
+	if ((event == CG2900_FM_EVENT_CMD_COMPLETE) &&
+	    (cmd_func == CG2900_FM_CMD_PARAM_WRITECOMMAND) &&
+	    (cmd_id == info->audio_fm_cmd_id)) {
+		dev_dbg(info->fm_audio->dev,
+			"FM Audio Function Code match = 0x%04X\n",
+			cmd_id);
+		return true;
+	}
+
+	/* Check if Interrupt legacy interface. */
+	if ((event == CG2900_FM_EVENT_INTERRUPT) &&
+	    (fm_is_do_cmd_irpt(irpt_val)) &&
+	    (info->tx_fm_audio_awaiting_irpt))
+		return true;
+
+	return false;
+}
+
+/**
+ * data_from_chip() - Called when data is received from the chip.
+ * @dev:	Chip info.
+ * @cg2900_dev:	CG2900 user for this packet.
+ * @skb:	Packet received.
+ *
+ * The data_from_chip() function updates flow control and checks
+ * if packet is a response for a packet it itself has transmitted. If not it
+ * finds the correct user and sends the packet* to the user.
+ */
+static void data_from_chip(struct cg2900_chip_dev *dev,
+			   struct sk_buff *skb)
+{
+	int h4_channel;
+	struct list_head *cursor;
+	struct cg2900_channel_item *tmp;
+	struct cg2900_chip_info *info = dev->c_data;
+	struct cg2900_user_data *user = NULL;
+
+	spin_lock_bh(&info->rw_lock);
+	/* Copy RX Data into logger.*/
+	if (info->logger)
+		cg2900_send_to_hci_logger(info->logger, skb,
+						LOGGER_DIRECTION_RX);
+
+	h4_channel = skb->data[0];
+	skb_pull(skb, HCI_H4_SIZE);
+
+	/* First check if it is a BT or FM audio event */
+	if (is_bt_audio_user(info, h4_channel, skb))
+		user = info->bt_audio;
+	else if (is_fm_audio_user(info, h4_channel, skb))
+		user = info->fm_audio;
+	spin_unlock_bh(&info->rw_lock);
+
+	/* Now check if we should update flow control */
+	if (h4_channel == CHANNEL_BT_EVT)
+		update_flow_ctrl_bt(dev, skb);
+	else if (h4_channel == CHANNEL_FM_RADIO)
+		update_flow_ctrl_fm(dev, skb);
+
+	/* Then check if this is a response to data we have sent */
+	if (h4_channel == CHANNEL_BT_EVT && handle_rx_data_bt_evt(dev, skb))
+		return;
+
+	spin_lock_bh(&info->rw_lock);
+
+	if (user)
+		goto user_found;
+
+	/* Let's see if it is the last user */
+	if (info->last_user && info->last_user->h4_channel == h4_channel) {
+		user = info->last_user;
+		goto user_found;
+	}
+
+	/* Search through the list of all open channels to find the user */
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		if (tmp->user->h4_channel == h4_channel) {
+			user = tmp->user;
+			goto user_found;
+		}
+	}
+
+user_found:
+	if (user != info->bt_audio && user != info->fm_audio)
+		info->last_user = user;
+
+	spin_unlock_bh(&info->rw_lock);
+
+	if (user)
+		user->read_cb(user, skb);
+	else {
+		dev_err(dev->dev,
+			"Could not find corresponding user to h4_channel %d\n",
+			h4_channel);
+		kfree_skb(skb);
+	}
+}
+
+/**
+ * chip_removed() - Called when transport has been removed.
+ * @dev:	Chip device.
+ *
+ * Removes registered MFD devices and frees internal resources.
+ */
+static void chip_removed(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_chip_info *info = dev->c_data;
+
+	cancel_delayed_work(&info->selftest_work.work);
+	mfd_remove_devices(dev->dev);
+	kfree(info->settings_file_name);
+	kfree(info->patch_file_name);
+	destroy_workqueue(info->wq);
+	kfree(info);
+	dev->c_data = NULL;
+	dev->c_cb.chip_removed = NULL;
+	dev->c_cb.data_from_chip = NULL;
+}
+
+/**
+ * last_bt_user_removed() - Called when last BT user is removed.
+ * @info:	Chip handler info.
+ *
+ * Clears out TX queue for BT.
+ */
+static void last_bt_user_removed(struct cg2900_chip_info *info)
+{
+	spin_lock_bh(&info->tx_bt_lock);
+	skb_queue_purge(&info->tx_queue_bt);
+
+	/*
+	 * Reset number of packets allowed and number of outstanding
+	 * BT commands.
+	 */
+	info->tx_nr_pkts_allowed_bt = 1;
+	/* Reset the audio_bt_cmd_op. */
+	info->audio_bt_cmd_op = CG2900_BT_OPCODE_NONE;
+	spin_unlock_bh(&info->tx_bt_lock);
+}
+
+/**
+ * last_fm_user_removed() - Called when last FM user is removed.
+ * @info:	Chip handler info.
+ *
+ * Clears out TX queue for BT.
+ */
+static void last_fm_user_removed(struct cg2900_chip_info *info)
+{
+	spin_lock_bh(&info->tx_fm_lock);
+	fm_reset_flow_ctrl(info);
+	spin_unlock_bh(&info->tx_fm_lock);
+}
+
+/**
+ * chip_shutdown() - Reset and power the chip off.
+ * @user:	MFD device.
+ */
+static void chip_shutdown(struct cg2900_user_data *user)
+{
+	struct hci_command_hdr cmd;
+	struct cg2900_chip_dev *dev = cg2900_get_prv(user);
+	struct cg2900_chip_info *info = dev->c_data;
+
+	dev_dbg(user->dev, "chip_shutdown\n");
+
+	/* First do a quick power switch of the chip to assure a good state */
+	if (dev->t_cb.set_chip_power)
+		dev->t_cb.set_chip_power(dev, false);
+
+	/*
+	 * Wait 50ms before continuing to be sure that the chip detects
+	 * chip power off.
+	 */
+	schedule_timeout_killable(
+			msecs_to_jiffies(LINE_TOGGLE_DETECT_TIMEOUT));
+
+	if (dev->t_cb.set_chip_power)
+		dev->t_cb.set_chip_power(dev, true);
+
+	/* Wait 100ms before continuing to be sure that the chip is ready */
+	schedule_timeout_killable(msecs_to_jiffies(CHIP_READY_TIMEOUT));
+
+	if (user != info->bt_audio && user != info->fm_audio)
+		info->last_user = user;
+	info->user_in_charge = user;
+
+	/*
+	 * Transmit HCI reset command to ensure the chip is using
+	 * the correct transport and to put BT part in reset.
+	 */
+	dev_dbg(user->dev, "New closing_state: CLOSING_RESET\n");
+	info->closing_state = CLOSING_RESET;
+	cmd.opcode = cpu_to_le16(HCI_OP_RESET);
+	cmd.plen = 0; /* No parameters for HCI reset */
+	cg2900_send_bt_cmd(info->user_in_charge, info->logger, &cmd,
+			   sizeof(cmd));
+}
+
+/**
+ * chip_startup_finished() - Called when chip startup has finished.
+ * @info:	Chip handler info.
+ * @err:	Result of chip startup, 0 for no error.
+ *
+ * Shuts down the chip upon error, sets state to active, wakes waiting threads,
+ * and informs transport that startup has finished.
+ */
+static void chip_startup_finished(struct cg2900_chip_info *info, int err)
+{
+	dev_dbg(BOOT_DEV, "chip_startup_finished (%d)\n", err);
+
+	if (err)
+		/* Shutdown the chip */
+		cg2900_create_work_item(info->wq, work_chip_shutdown,
+					info->user_in_charge);
+	else {
+		dev_dbg(BOOT_DEV, "New main_state: CG2900_ACTIVE\n");
+		info->main_state = CG2900_ACTIVE;
+	}
+
+	wake_up_all(&main_wait_queue);
+
+	if (err)
+		return;
+
+	if (!info->chip_dev->t_cb.chip_startup_finished)
+		dev_dbg(BOOT_DEV, "chip_startup_finished callback not found\n");
+	else
+		info->chip_dev->t_cb.chip_startup_finished(info->chip_dev);
+}
+
+/**
+ * cg2900_open() - Called when user wants to open an H4 channel.
+ * @user:	MFD device to open.
+ *
+ * Checks that H4 channel is not already opened. If chip is not started, starts
+ * up the chip. Sets channel as opened and adds user to active users.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user is NULL or read_cb is NULL.
+ *   -EBUSY if chip is in transit state (being started or shutdown).
+ *   -EACCES if H4 channel is already opened.
+ *   -ENOMEM if allocation fails.
+ *   -EIO if chip startup fails.
+ *   Error codes generated by t_cb.open.
+ */
+static int cg2900_open(struct cg2900_user_data *user)
+{
+	int err;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+	struct list_head *cursor;
+	struct cg2900_channel_item *tmp;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "cg2900_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	if (!user->read_cb) {
+		dev_err(user->dev, "cg2900_open: read_cb missing\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "cg2900_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	mutex_lock(&main_info->man_mutex);
+
+	/*
+	 * Add a minor wait in order to avoid CPU blocking, looping openings.
+	 * Note there will of course be no wait if we are already in the right
+	 * state.
+	 */
+	err = wait_event_timeout(main_wait_queue,
+			(CG2900_IDLE == info->main_state ||
+			 CG2900_ACTIVE == info->main_state),
+			msecs_to_jiffies(LINE_TOGGLE_DETECT_TIMEOUT));
+	if (err <= 0) {
+		if (CG2900_INIT == info->main_state)
+			dev_err(user->dev, "Transport not opened\n");
+		else
+			dev_err(user->dev, "cg2900_open currently busy (0x%X). "
+				"Try again\n", info->main_state);
+		err = -EBUSY;
+		goto err_free_mutex;
+	}
+
+	err = 0;
+
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		if (tmp->user->h4_channel == user->h4_channel &&
+		    tmp->user->is_audio == user->is_audio) {
+			dev_err(user->dev, "Channel %d is already opened\n",
+				user->h4_channel);
+			err = -EACCES;
+			goto err_free_mutex;
+		}
+	}
+
+	tmp = kzalloc(sizeof(*tmp), GFP_KERNEL);
+	if (!tmp) {
+		dev_err(user->dev, "Could not allocate tmp\n");
+		err = -ENOMEM;
+		goto err_free_mutex;
+	}
+	tmp->user = user;
+
+	if (CG2900_ACTIVE != info->main_state &&
+	    !user->chip_independent) {
+		/* Open transport and start-up the chip */
+		if (dev->t_cb.set_chip_power)
+			dev->t_cb.set_chip_power(dev, true);
+
+		/* Wait to be sure that the chip is ready */
+		schedule_timeout_killable(
+				msecs_to_jiffies(CHIP_READY_TIMEOUT));
+
+		if (dev->t_cb.open) {
+			err = dev->t_cb.open(dev);
+			if (err) {
+				if (dev->t_cb.set_chip_power)
+					dev->t_cb.set_chip_power(dev, false);
+				goto err_free_list_item;
+			}
+		}
+
+		/* Start the boot sequence */
+		info->user_in_charge = user;
+		if (user != info->bt_audio && user != info->fm_audio)
+			info->last_user = user;
+		dev_dbg(user->dev, "New boot_state: BOOT_GET_FILES_TO_LOAD\n");
+		info->boot_state = BOOT_GET_FILES_TO_LOAD;
+		dev_dbg(user->dev, "New main_state: CG2900_BOOTING\n");
+		info->main_state = CG2900_BOOTING;
+		cg2900_create_work_item(info->wq, work_load_patch_and_settings,
+					dev);
+
+		dev_dbg(user->dev, "Wait up to 15 seconds for chip to start\n");
+		wait_event_timeout(main_wait_queue,
+			(CG2900_ACTIVE == info->main_state ||
+			 CG2900_IDLE   == info->main_state),
+			msecs_to_jiffies(CHIP_STARTUP_TIMEOUT));
+		if (CG2900_ACTIVE != info->main_state) {
+			dev_err(user->dev, "CG2900 driver failed to start\n");
+
+			if (dev->t_cb.close)
+				dev->t_cb.close(dev);
+
+			dev_dbg(user->dev, "New main_state: CG2900_IDLE\n");
+			info->main_state = CG2900_IDLE;
+			err = -EIO;
+			goto err_free_list_item;
+		}
+	}
+
+	list_add_tail(&tmp->list, &info->open_channels);
+
+	user->opened = true;
+
+	dev_dbg(user->dev, "H:4 channel opened\n");
+
+	mutex_unlock(&main_info->man_mutex);
+	return 0;
+err_free_list_item:
+	kfree(tmp);
+err_free_mutex:
+	mutex_unlock(&main_info->man_mutex);
+	return err;
+}
+
+/**
+ * cg2900_hci_log_open() - Called when user wants to open HCI logger channel.
+ * @user:	MFD device to open.
+ *
+ * Registers user as hci_logger and calls @cg2900_open to open the channel.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user is NULL.
+ *   -EACCES if H4 channel is already opened.
+ *   Error codes generated by cg2900_open.
+ */
+static int cg2900_hci_log_open(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+	int err;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_hci_log_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "cg2900_hci_log_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (info->logger) {
+		dev_err(user->dev, "HCI Logger already stored\n");
+		return -EACCES;
+	}
+
+	info->logger = user;
+	err = cg2900_open(user);
+	if (err)
+		info->logger = NULL;
+	return err;
+}
+
+/**
+ * cg2900_bt_audio_open() - Called when user wants to open BT audio channel.
+ * @user:	MFD device to open.
+ *
+ * Registers user as bt_audio and calls @cg2900_open to open the channel.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user is NULL.
+ *   -EACCES if H4 channel is already opened.
+ *   Error codes generated by cg2900_open.
+ */
+static int cg2900_bt_audio_open(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+	int err;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_bt_audio_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "cg2900_bt_audio_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (info->bt_audio) {
+		dev_err(user->dev, "BT Audio already stored\n");
+		return -EACCES;
+	}
+
+	info->bt_audio = user;
+	err = cg2900_open(user);
+	if (err)
+		info->bt_audio = NULL;
+	return err;
+}
+
+/**
+ * cg2900_fm_audio_open() - Called when user wants to open FM audio channel.
+ * @user:	MFD device to open.
+ *
+ * Registers user as fm_audio and calls @cg2900_open to open the channel.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user is NULL.
+ *   -EACCES if H4 channel is already opened.
+ *   Error codes generated by cg2900_open.
+ */
+static int cg2900_fm_audio_open(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+	int err;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_fm_audio_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "cg2900_fm_audio_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (info->fm_audio) {
+		dev_err(user->dev, "FM Audio already stored\n");
+		return -EACCES;
+	}
+
+	info->fm_audio = user;
+	err = cg2900_open(user);
+	if (err)
+		info->fm_audio = NULL;
+	return err;
+}
+
+/**
+ * cg2900_close() - Called when user wants to close an H4 channel.
+ * @user:	MFD device to close.
+ *
+ * Clears up internal resources, sets channel as closed, and shuts down chip if
+ * this was the last user.
+ */
+static void cg2900_close(struct cg2900_user_data *user)
+{
+	bool keep_powered = false;
+	struct list_head *cursor, *next;
+	struct cg2900_channel_item *tmp;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "cg2900_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "cg2900_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	mutex_lock(&main_info->man_mutex);
+
+	/*
+	 * Go through each open channel. Remove our channel and check if there
+	 * is any other channel that want to keep the chip running
+	 */
+	list_for_each_safe(cursor, next, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		if (tmp->user == user) {
+			list_del(cursor);
+			kfree(tmp);
+		} else if (!tmp->user->chip_independent)
+			keep_powered = true;
+	}
+
+	if (user->h4_channel == CHANNEL_BT_CMD && !bt_is_open(info))
+		last_bt_user_removed(info);
+	else if (user->h4_channel == CHANNEL_FM_RADIO && !fm_is_open(info))
+		last_fm_user_removed(info);
+
+	if (keep_powered)
+		/* This was not the last user, we're done. */
+		goto finished;
+
+	if (CG2900_IDLE == info->main_state)
+		/* Chip has already been shut down. */
+		goto finished;
+
+	dev_dbg(user->dev, "New main_state: CG2900_CLOSING\n");
+	info->main_state = CG2900_CLOSING;
+	chip_shutdown(user);
+
+	dev_dbg(user->dev, "Wait up to 15 seconds for chip to shut-down\n");
+	wait_event_timeout(main_wait_queue,
+				(CG2900_IDLE == info->main_state),
+				msecs_to_jiffies(CHIP_SHUTDOWN_TIMEOUT));
+
+	/* Force shutdown if we timed out */
+	if (CG2900_IDLE != info->main_state) {
+		dev_err(user->dev,
+			"ST-Ericsson CG2900 Core Driver was shut-down with "
+			"problems\n");
+
+		if (dev->t_cb.close)
+			dev->t_cb.close(dev);
+
+		dev_dbg(user->dev, "New main_state: CG2900_IDLE\n");
+		info->main_state = CG2900_IDLE;
+	}
+
+finished:
+	mutex_unlock(&main_info->man_mutex);
+	user->opened = false;
+	dev_dbg(user->dev, "H:4 channel closed\n");
+}
+
+/**
+ * cg2900_hci_log_close() - Called when user wants to close HCI logger channel.
+ * @user:	MFD device to close.
+ *
+ * Clears hci_logger user and calls @cg2900_close to close the channel.
+ */
+static void cg2900_hci_log_close(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_hci_log_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "cg2900_hci_log_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (user != info->logger) {
+		dev_err(user->dev, "cg2900_hci_log_close: Trying to remove "
+			"another user\n");
+		return;
+	}
+
+	info->logger = NULL;
+	cg2900_close(user);
+}
+
+/**
+ * cg2900_bt_audio_close() - Called when user wants to close BT audio channel.
+ * @user:	MFD device to close.
+ *
+ * Clears bt_audio user and calls @cg2900_close to close the channel.
+ */
+static void cg2900_bt_audio_close(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_bt_audio_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "cg2900_bt_audio_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (user != info->bt_audio) {
+		dev_err(user->dev, "cg2900_bt_audio_close: Trying to remove "
+			"another user\n");
+		return;
+	}
+
+	info->bt_audio = NULL;
+	cg2900_close(user);
+}
+
+/**
+ * cg2900_fm_audio_close() - Called when user wants to close FM audio channel.
+ * @user:	MFD device to close.
+ *
+ * Clears fm_audio user and calls @cg2900_close to close the channel.
+ */
+static void cg2900_fm_audio_close(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_fm_audio_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "cg2900_fm_audio_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (user != info->fm_audio) {
+		dev_err(user->dev, "cg2900_fm_audio_close: Trying to remove "
+			"another user\n");
+		return;
+	}
+
+	info->fm_audio = NULL;
+	cg2900_close(user);
+}
+
+/**
+ * cg2900_reset() - Called when user wants to reset the chip.
+ * @user:	MFD device to reset.
+ *
+ * Closes down the chip and calls reset_cb for all open users.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user is NULL.
+ */
+static int cg2900_reset(struct cg2900_user_data *user)
+{
+	struct list_head *cursor, *next;
+	struct cg2900_channel_item *tmp;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	if (!user) {
+		dev_err(MAIN_DEV, "cg2900_reset: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	dev_info(user->dev, "cg2900_reset\n");
+
+	BUG_ON(!main_info);
+
+	mutex_lock(&main_info->man_mutex);
+
+	dev_dbg(user->dev, "New main_state: CG2900_RESETING\n");
+	info->main_state = CG2900_RESETING;
+
+	chip_shutdown(user);
+
+	/*
+	 * Inform all opened channels about the reset and free the user devices
+	 */
+	list_for_each_safe(cursor, next, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		list_del(cursor);
+		tmp->user->opened  = false;
+		tmp->user->reset_cb(tmp->user);
+		kfree(tmp);
+	}
+
+	/* Reset finished. We are now idle until first channel is opened */
+	dev_dbg(user->dev, "New main_state: CG2900_IDLE\n");
+	info->main_state = CG2900_IDLE;
+
+	mutex_unlock(&main_info->man_mutex);
+
+	/*
+	 * Send wake-up since this might have been called from a failed boot.
+	 * No harm done if it is a CG2900 chip user who called.
+	 */
+	wake_up_all(&main_wait_queue);
+
+	return 0;
+}
+
+/**
+ * cg2900_alloc_skb() - Allocates socket buffer.
+ * @size:	Sk_buffer size in bytes.
+ * @priority:	GFP priorit for allocation.
+ *
+ * Allocates a sk_buffer and reserves space for H4 header.
+ *
+ * Returns:
+ *   sk_buffer if success.
+ *   NULL if allocation fails.
+ */
+static struct sk_buff *cg2900_alloc_skb(unsigned int size, gfp_t priority)
+{
+	struct sk_buff *skb;
+
+	dev_dbg(MAIN_DEV, "cg2900_alloc_skb size %d bytes\n", size);
+
+	/* Allocate the SKB and reserve space for the header */
+	skb = alloc_skb(size + CG2900_SKB_RESERVE, priority);
+	if (skb)
+		skb_reserve(skb, CG2900_SKB_RESERVE);
+
+	return skb;
+}
+
+/**
+ * cg2900_write() - Called when user wants to write to the chip.
+ * @user:	MFD device representing H4 channel to write to.
+ * @skb:	Sk_buffer to transmit.
+ *
+ * Transmits the sk_buffer to the chip. If it is a BT cmd or FM audio packet it
+ * is checked that it is allowed to transmit the chip.
+ * Note that if error is returned it is up to the user to free the skb.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user or skb is NULL.
+ *   -EACCES if channel is closed.
+ */
+static int cg2900_write(struct cg2900_user_data *user, struct sk_buff *skb)
+{
+	u8 *h4_header;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "cg2900_write: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	if (!skb) {
+		dev_err(user->dev, "cg2900_write with no sk_buffer\n");
+		return -EINVAL;
+	}
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	dev_dbg(user->dev, "cg2900_write length %d bytes\n", skb->len);
+
+	if (!user->opened) {
+		dev_err(user->dev,
+			"Trying to transmit data on a closed channel\n");
+		return -EACCES;
+	}
+
+	/*
+	 * Move the data pointer to the H:4 header position and
+	 * store the H4 header.
+	 */
+	h4_header = skb_push(skb, CG2900_SKB_RESERVE);
+	*h4_header = (u8)user->h4_channel;
+
+	if (user->h4_channel == CHANNEL_BT_CMD)
+		transmit_skb_with_flow_ctrl_bt(user, skb);
+	else if (user->h4_channel == CHANNEL_FM_RADIO)
+		transmit_skb_with_flow_ctrl_fm(user, skb);
+	else
+		cg2900_tx_to_chip(user, info->logger, skb);
+
+	return 0;
+}
+
+/**
+ * cg2900_no_write() - Used for channels where it is not allowed to write.
+ * @user:	MFD device representing H4 channel to write to.
+ * @skb:	Sk_buffer to transmit.
+ *
+ * Returns:
+ *   -EPERM.
+ */
+static int cg2900_no_write(struct cg2900_user_data *user,
+			   __attribute__((unused)) struct sk_buff *skb)
+{
+	dev_err(user->dev, "Not allowed to send on this channel\n");
+	return -EPERM;
+}
+
+/**
+ * cg2900_get_local_revision() - Called to retrieve revision data for the chip.
+ * @user:	MFD device to check.
+ * @rev_data:	Revision data to fill in.
+ *
+ * Returns:
+ *   true if success.
+ *   false upon failure.
+ */
+static bool cg2900_get_local_revision(struct cg2900_user_data *user,
+				      struct cg2900_rev_data *rev_data)
+{
+	struct cg2900_chip_dev *dev;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "cg2900_get_local_revision: Calling with "
+			"NULL pointer\n");
+		return false;
+	}
+
+	if (!rev_data) {
+		dev_err(user->dev, "Calling with rev_data NULL\n");
+		return false;
+	}
+
+	dev = cg2900_get_prv(user);
+
+	rev_data->revision = dev->chip.hci_revision;
+	rev_data->sub_version = dev->chip.hci_sub_version;
+
+	return true;
+}
+
+static struct cg2900_user_data btcmd_data = {
+	.h4_channel = CHANNEL_BT_CMD,
+};
+static struct cg2900_user_data btacl_data = {
+	.h4_channel = CHANNEL_BT_ACL,
+};
+static struct cg2900_user_data btevt_data = {
+	.h4_channel = CHANNEL_BT_EVT,
+};
+static struct cg2900_user_data fm_data = {
+	.h4_channel = CHANNEL_FM_RADIO,
+};
+static struct cg2900_user_data gnss_data = {
+	.h4_channel = CHANNEL_GNSS,
+};
+static struct cg2900_user_data debug_data = {
+	.h4_channel = CHANNEL_DEBUG,
+};
+static struct cg2900_user_data ste_tools_data = {
+	.h4_channel = CHANNEL_STE_TOOLS,
+};
+static struct cg2900_user_data hci_logger_data = {
+	.h4_channel = CHANNEL_HCI_LOGGER,
+	.chip_independent = true,
+	.write = cg2900_no_write,
+	.open = cg2900_hci_log_open,
+	.close = cg2900_hci_log_close,
+};
+static struct cg2900_user_data core_data = {
+	.h4_channel = CHANNEL_CORE,
+	.write = cg2900_no_write,
+};
+static struct cg2900_user_data audio_bt_data = {
+	.h4_channel = CHANNEL_BT_CMD,
+	.is_audio = true,
+	.open = cg2900_bt_audio_open,
+	.close = cg2900_bt_audio_close,
+};
+static struct cg2900_user_data audio_fm_data = {
+	.h4_channel = CHANNEL_FM_RADIO,
+	.is_audio = true,
+	.open = cg2900_fm_audio_open,
+	.close = cg2900_fm_audio_close,
+};
+
+static struct mfd_cell cg2900_devs[] = {
+	{
+		.name = "cg2900-btcmd",
+		.platform_data = &btcmd_data,
+	},
+	{
+		.name = "cg2900-btacl",
+		.platform_data = &btacl_data,
+	},
+	{
+		.name = "cg2900-btevt",
+		.platform_data = &btevt_data,
+	},
+	{
+		.name = "cg2900-fm",
+		.platform_data = &fm_data,
+	},
+	{
+		.name = "cg2900-gnss",
+		.platform_data = &gnss_data,
+	},
+	{
+		.name = "cg2900-debug",
+		.platform_data = &debug_data,
+	},
+	{
+		.name = "cg2900-stetools",
+		.platform_data = &ste_tools_data,
+	},
+	{
+		.name = "cg2900-hcilogger",
+		.platform_data = &hci_logger_data,
+	},
+	{
+		.name = "cg2900-core",
+		.platform_data = &core_data,
+	},
+	{
+		.name = "cg2900-audiobt",
+		.platform_data = &audio_bt_data,
+	},
+	{
+		.name = "cg2900-audiofm",
+		.platform_data = &audio_fm_data,
+	},
+};
+
+static struct cg2900_user_data char_btcmd_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_BT_CMD,
+	},
+	.h4_channel = CHANNEL_BT_CMD,
+};
+static struct cg2900_user_data char_btacl_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_BT_ACL,
+	},
+	.h4_channel = CHANNEL_BT_ACL,
+};
+static struct cg2900_user_data char_btevt_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_BT_EVT,
+	},
+	.h4_channel = CHANNEL_BT_EVT,
+};
+static struct cg2900_user_data char_fm_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_FM_RADIO,
+	},
+	.h4_channel = CHANNEL_FM_RADIO,
+};
+static struct cg2900_user_data char_gnss_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_GNSS,
+	},
+	.h4_channel = CHANNEL_GNSS,
+};
+static struct cg2900_user_data char_debug_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_DEBUG,
+	},
+	.h4_channel = CHANNEL_DEBUG,
+};
+static struct cg2900_user_data char_ste_tools_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_STE_TOOLS,
+	},
+	.h4_channel = CHANNEL_STE_TOOLS,
+};
+static struct cg2900_user_data char_hci_logger_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_HCI_LOGGER,
+	},
+	.h4_channel = CHANNEL_HCI_LOGGER,
+	.chip_independent = true,
+	.write = cg2900_no_write,
+	.open = cg2900_hci_log_open,
+	.close = cg2900_hci_log_close,
+};
+static struct cg2900_user_data char_core_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_CORE,
+	},
+	.h4_channel = CHANNEL_CORE,
+	.write = cg2900_no_write,
+};
+static struct cg2900_user_data char_audio_bt_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_BT_AUDIO,
+	},
+	.h4_channel = CHANNEL_BT_CMD,
+	.is_audio = true,
+};
+static struct cg2900_user_data char_audio_fm_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_FM_AUDIO,
+	},
+	.h4_channel = CHANNEL_FM_RADIO,
+	.is_audio = true,
+};
+
+static struct mfd_cell cg2900_char_devs[] = {
+	{
+		.name = "cg2900-chardev",
+		.id = 0,
+		.platform_data = &char_btcmd_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 1,
+		.platform_data = &char_btacl_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 2,
+		.platform_data = &char_btevt_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 3,
+		.platform_data = &char_fm_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 4,
+		.platform_data = &char_gnss_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 5,
+		.platform_data = &char_debug_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 6,
+		.platform_data = &char_ste_tools_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 7,
+		.platform_data = &char_hci_logger_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 8,
+		.platform_data = &char_core_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 9,
+		.platform_data = &char_audio_bt_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 10,
+		.platform_data = &char_audio_fm_data,
+	},
+};
+
+/**
+ * set_plat_data() - Initializes data for an MFD cell.
+ * @cell:	MFD cell.
+ * @dev:	Current chip.
+ *
+ * Sets each callback to default function unless already set.
+ */
+static void set_plat_data(struct mfd_cell *cell, struct cg2900_chip_dev *dev)
+{
+	struct cg2900_user_data *pf_data = cell->platform_data;
+
+	if (!pf_data->open)
+		pf_data->open = cg2900_open;
+	if (!pf_data->close)
+		pf_data->close = cg2900_close;
+	if (!pf_data->reset)
+		pf_data->reset = cg2900_reset;
+	if (!pf_data->alloc_skb)
+		pf_data->alloc_skb = cg2900_alloc_skb;
+	if (!pf_data->write)
+		pf_data->write = cg2900_write;
+	if (!pf_data->get_local_revision)
+		pf_data->get_local_revision = cg2900_get_local_revision;
+
+	cg2900_set_prv(pf_data, dev);
+}
+
+/**
+ * check_chip_support() - Checks if connected chip is handled by this driver.
+ * @dev:	Chip info structure.
+ *
+ * First check if chip is supported by this driver. If that is the case fill in
+ * the callbacks in @dev and initiate internal variables. Finally create MFD
+ * devices for all supported H4 channels. When finished power off the chip.
+ *
+ * Returns:
+ *   true if chip is handled by this driver.
+ *   false otherwise.
+ */
+static bool check_chip_support(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_platform_data *pf_data;
+	struct cg2900_chip_info *info;
+	int i;
+	int err;
+
+	dev_dbg(dev->dev, "check_chip_support\n");
+
+	/*
+	 * Check if this is a CG2900 revision.
+	 * We do not care about the sub-version at the moment. Change this if
+	 * necessary.
+	 */
+	if ((dev->chip.manufacturer != CG2900_SUPP_MANUFACTURER) ||
+	    (dev->chip.hci_revision != CG2900_PG1_SPECIAL_REV &&
+	     (dev->chip.hci_revision < CG2900_SUPP_REVISION_MIN ||
+	      dev->chip.hci_revision > CG2900_SUPP_REVISION_MAX))) {
+		dev_dbg(dev->dev, "Chip not supported by CG2900 driver\n"
+			"\tMan: 0x%02X\n"
+			"\tRev: 0x%04X\n"
+			"\tSub: 0x%04X\n",
+			dev->chip.manufacturer, dev->chip.hci_revision,
+			dev->chip.hci_sub_version);
+		return false;
+	}
+
+	/* Store needed data */
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(dev->dev, "Couldn't allocate info struct\n");
+		return false;
+	}
+
+	/* Initialize all variables */
+	skb_queue_head_init(&info->tx_queue_bt);
+	skb_queue_head_init(&info->tx_queue_fm);
+
+	INIT_LIST_HEAD(&info->open_channels);
+
+	spin_lock_init(&info->tx_bt_lock);
+	spin_lock_init(&info->tx_fm_lock);
+	spin_lock_init(&info->rw_lock);
+
+	info->tx_nr_pkts_allowed_bt = 1;
+	info->audio_bt_cmd_op = CG2900_BT_OPCODE_NONE;
+	info->audio_fm_cmd_id = CG2900_FM_CMD_NONE;
+	info->hci_fm_cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	info->fm_radio_mode = FM_RADIO_MODE_IDLE;
+	info->chip_dev = dev;
+	info->dev = dev->dev;
+
+	info->wq = create_singlethread_workqueue(WQ_NAME);
+	if (!info->wq) {
+		dev_err(dev->dev, "Could not create workqueue\n");
+		goto err_handling_free_info;
+	}
+
+	info->patch_file_name = kzalloc(NAME_MAX + 1, GFP_ATOMIC);
+	if (!info->patch_file_name) {
+		dev_err(dev->dev,
+			"Couldn't allocate name buffer for patch file\n");
+		goto err_handling_destroy_wq;
+	}
+
+	info->settings_file_name = kzalloc(NAME_MAX + 1, GFP_ATOMIC);
+	if (!info->settings_file_name) {
+		dev_err(dev->dev,
+			"Couldn't allocate name buffers settings file\n");
+		goto err_handling_free_patch_name;
+	}
+
+	info->selftest_work.data = info;
+	INIT_DELAYED_WORK(&info->selftest_work.work,
+			  work_send_read_selftest_cmd);
+
+	dev->c_data = info;
+	/* Set the callbacks */
+	dev->c_cb.data_from_chip = data_from_chip;
+	dev->c_cb.chip_removed = chip_removed;
+
+	mutex_lock(&main_info->man_mutex);
+
+	pf_data = dev_get_platdata(dev->dev);
+	btcmd_data.channel_data.bt_bus = pf_data->bus;
+	btacl_data.channel_data.bt_bus = pf_data->bus;
+	btevt_data.channel_data.bt_bus = pf_data->bus;
+
+	for (i = 0; i < ARRAY_SIZE(cg2900_devs); i++)
+		set_plat_data(&cg2900_devs[i], dev);
+	for (i = 0; i < ARRAY_SIZE(cg2900_char_devs); i++)
+		set_plat_data(&cg2900_char_devs[i], dev);
+
+	err = mfd_add_devices(dev->dev, main_info->cell_base_id, cg2900_devs,
+			      ARRAY_SIZE(cg2900_devs), NULL, 0);
+	if (err) {
+		dev_err(dev->dev, "Failed to add cg2900_devs (%d)\n", err);
+		goto err_handling_free_settings_name;
+	}
+
+	err = mfd_add_devices(dev->dev, main_info->cell_base_id,
+			      cg2900_char_devs, ARRAY_SIZE(cg2900_char_devs),
+			      NULL, 0);
+	if (err) {
+		dev_err(dev->dev, "Failed to add cg2900_char_devs (%d)\n", err);
+		goto err_handling_remove_devs;
+	}
+
+	main_info->cell_base_id += 30;
+	mutex_unlock(&main_info->man_mutex);
+
+	dev_info(dev->dev, "Chip supported by the CG2900 chip driver\n");
+
+	/* Finish by turning off the chip */
+	cg2900_create_work_item(info->wq, work_power_off_chip, dev);
+
+	return true;
+
+err_handling_remove_devs:
+	mfd_remove_devices(dev->dev);
+err_handling_free_settings_name:
+	kfree(info->settings_file_name);
+	mutex_unlock(&main_info->man_mutex);
+err_handling_free_patch_name:
+	kfree(info->patch_file_name);
+err_handling_destroy_wq:
+	destroy_workqueue(info->wq);
+err_handling_free_info:
+	kfree(info);
+	return false;
+}
+
+static struct cg2900_id_callbacks chip_support_callbacks = {
+	.check_chip_support = check_chip_support,
+};
+
+/**
+ * cg2900_chip_probe() - Initialize CG2900 chip handler resources.
+ * @pdev:	Platform device.
+ *
+ * This function initializes the CG2900 driver, then registers to
+ * the CG2900 Core.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM for failed alloc or structure creation.
+ *   Error codes generated by cg2900_register_chip_driver.
+ */
+static int __devinit cg2900_chip_probe(struct platform_device *pdev)
+{
+	int err;
+
+	dev_dbg(&pdev->dev, "cg2900_chip_probe\n");
+
+	main_info = kzalloc(sizeof(*main_info), GFP_ATOMIC);
+	if (!main_info) {
+		dev_err(&pdev->dev, "Couldn't allocate main_info\n");
+		return -ENOMEM;
+	}
+
+	main_info->dev = &pdev->dev;
+	mutex_init(&main_info->man_mutex);
+
+	err = cg2900_register_chip_driver(&chip_support_callbacks);
+	if (err) {
+		dev_err(&pdev->dev,
+			"Couldn't register chip driver (%d)\n", err);
+		goto error_handling;
+	}
+
+	dev_info(&pdev->dev, "CG2900 chip driver started\n");
+
+	return 0;
+
+error_handling:
+	mutex_destroy(&main_info->man_mutex);
+	kfree(main_info);
+	main_info = NULL;
+	return err;
+}
+
+/**
+ * cg2900_chip_remove() - Release CG2900 chip handler resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success (always success).
+ */
+static int __devexit cg2900_chip_remove(struct platform_device *pdev)
+{
+	dev_info(&pdev->dev, "CG2900 chip driver removed\n");
+
+	cg2900_deregister_chip_driver(&chip_support_callbacks);
+
+	if (!main_info)
+		return 0;
+	mutex_destroy(&main_info->man_mutex);
+	kfree(main_info);
+	main_info = NULL;
+	return 0;
+}
+
+static struct platform_driver cg2900_chip_driver = {
+	.driver = {
+		.name	= "cg2900-chip",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_chip_probe,
+	.remove	= __devexit_p(cg2900_chip_remove),
+};
+
+/**
+ * cg2900_chip_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_chip_init(void)
+{
+	pr_debug("cg2900_chip_init");
+	return platform_driver_register(&cg2900_chip_driver);
+}
+
+/**
+ * cg2900_chip_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_chip_exit(void)
+{
+	pr_debug("cg2900_chip_exit");
+	platform_driver_unregister(&cg2900_chip_driver);
+}
+
+module_init(cg2900_chip_init);
+module_exit(cg2900_chip_exit);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux CG2900 Connectivity Device Driver");
diff --git a/drivers/staging/cg2900/mfd/cg2900_chip.h b/drivers/staging/cg2900/mfd/cg2900_chip.h
new file mode 100644
index 00000000000..886015976d3
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_chip.h
@@ -0,0 +1,613 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+
+#ifndef _CG2900_CHIP_H_
+#define _CG2900_CHIP_H_
+
+/*
+ *	Utility
+ */
+
+static inline void set_low_nibble(__u8 *var, __u8 value)
+{
+	*var = (*var & 0xf0) | (value & 0x0f);
+}
+
+static inline void set_high_nibble(__u8 *var, __u8 value)
+{
+	*var = (*var & 0x0f) | (value << 4);
+}
+
+static inline void store_bit(__u8 *var, size_t bit, __u8 value)
+{
+	*var = (*var & ~(1u << bit)) | (value << bit);
+}
+
+/*
+ *	General chip defines
+ */
+
+/* Supported chips */
+#define CG2900_SUPP_MANUFACTURER			0x30
+#define CG2900_SUPP_REVISION_MIN			0x0100
+#define CG2900_SUPP_REVISION_MAX			0x0200
+
+/* Specific chip version data */
+#define CG2900_PG1_REV					0x0101
+#define CG2900_PG2_REV					0x0200
+#define CG2900_PG1_SPECIAL_REV				0x0700
+
+/*
+ *	Bluetooth
+ */
+
+#define BT_SIZE_OF_HDR				(sizeof(__le16) + sizeof(__u8))
+#define BT_PARAM_LEN(__pkt_len)			(__pkt_len - BT_SIZE_OF_HDR)
+
+struct bt_cmd_cmpl_event {
+	__u8	eventcode;
+	__u8	plen;
+	__u8	n_commands;
+	__le16	opcode;
+	/*
+	 * According to BT-specification what follows is "parameters"
+	 * and unique to every command, but all commands start the
+	 * parameters with the status field so include it here for
+	 * convenience
+	 */
+	__u8	status;
+	__u8	data[];
+} __packed;
+
+/* BT VS Store In FS command */
+#define CG2900_BT_OP_VS_STORE_IN_FS			0xFC22
+struct bt_vs_store_in_fs_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	user_id;
+	__u8	len;
+	__u8	data[];
+} __packed;
+
+#define CG2900_VS_STORE_IN_FS_USR_ID_BD_ADDR		0xFE
+
+/* BT VS Write File Block command */
+#define CG2900_BT_OP_VS_WRITE_FILE_BLOCK		0xFC2E
+struct bt_vs_write_file_block_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	id;
+	__u8	data[];
+} __packed;
+
+#define CG2900_BT_DISABLE				0x00
+#define CG2900_BT_ENABLE				0x01
+
+/* BT VS BT Enable command */
+#define CG2900_BT_OP_VS_BT_ENABLE			0xFF10
+struct bt_vs_bt_enable_cmd {
+	__le16	op_code;
+	u8	plen;
+	u8	enable;
+} __packed;
+
+/* Bytes in the command Hci_Cmd_ST_Set_Uart_Baud_Rate */
+#define CG2900_BAUD_RATE_57600				0x03
+#define CG2900_BAUD_RATE_115200				0x02
+#define CG2900_BAUD_RATE_230400				0x01
+#define CG2900_BAUD_RATE_460800				0x00
+#define CG2900_BAUD_RATE_921600				0x20
+#define CG2900_BAUD_RATE_2000000			0x25
+#define CG2900_BAUD_RATE_3000000			0x27
+#define CG2900_BAUD_RATE_4000000			0x2B
+
+/* BT VS SetBaudRate command */
+#define CG2900_BT_OP_VS_SET_BAUD_RATE			0xFC09
+struct bt_vs_set_baud_rate_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	baud_rate;
+} __packed;
+
+#define CG2900_BT_SELFTEST_SUCCESSFUL			0x00
+#define CG2900_BT_SELFTEST_FAILED			0x01
+#define CG2900_BT_SELFTEST_NOT_COMPLETED		0x02
+
+/* BT VS ReadSelfTestsResult command & event */
+#define CG2900_BT_OP_VS_READ_SELTESTS_RESULT		0xFC10
+struct bt_vs_read_selftests_result_evt {
+	__u8	status;
+	__u8	result;
+} __packed;
+
+/* Bluetooth Vendor Specific Opcodes */
+#define CG2900_BT_OP_VS_POWER_SWITCH_OFF		0xFD40
+#define CG2900_BT_OP_VS_SYSTEM_RESET			0xFF12
+
+#define CG2900_BT_OPCODE_NONE				0xFFFF
+
+/*
+ *	Common multimedia
+ */
+
+#define CG2900_CODEC_TYPE_NONE				0x00
+#define CG2900_CODEC_TYPE_SBC				0x01
+
+#define CG2900_PCM_MODE_SLAVE				0x00
+#define CG2900_PCM_MODE_MASTER				0x01
+
+#define CG2900_I2S_MODE_MASTER				0x00
+#define CG2900_I2S_MODE_SLAVE				0x01
+
+/*
+ *	CG2900 PG1 multimedia API
+ */
+
+#define CG2900_BT_VP_TYPE_PCM				0x00
+#define CG2900_BT_VP_TYPE_I2S				0x01
+#define CG2900_BT_VP_TYPE_SLIMBUS			0x02
+#define CG2900_BT_VP_TYPE_FM				0x03
+#define CG2900_BT_VP_TYPE_BT_SCO			0x04
+#define CG2900_BT_VP_TYPE_BT_A2DP			0x05
+#define CG2900_BT_VP_TYPE_ANALOG			0x07
+
+#define CG2900_BT_VS_SET_HARDWARE_CONFIG		0xFD54
+/* These don't have the same length, so a union won't work */
+struct bt_vs_set_hw_cfg_cmd_pcm {
+	__le16	opcode;
+	__u8	plen;
+	__u8	vp_type;
+	__u8	port_id;
+	__u8	mode_dir; /* NB: mode is in bit 1 (not 0) */
+	__u8	bit_clock;
+	__le16	frame_len;
+} __packed;
+#define HWCONFIG_PCM_SET_MODE(pcfg, mode)		\
+	set_low_nibble(&(pcfg)->mode_dir, (mode) << 1)
+#define HWCONFIG_PCM_SET_DIR(pcfg, idx, dir)		\
+	store_bit(&(pcfg)->mode_dir, (idx) + 4, (dir))
+
+struct bt_vs_set_hw_cfg_cmd_i2s {
+	__le16	opcode;
+	__u8	plen;
+	__u8	vp_type;
+	__u8	port_id;
+	__u8	half_period;
+	__u8	master_slave;
+} __packed;
+
+/* Max length for allocating */
+#define CG2900_BT_LEN_VS_SET_HARDWARE_CONFIG	\
+	(sizeof(struct bt_vs_set_hw_cfg_cmd_pcm))
+
+#define CG2900_BT_VS_SET_SESSION_CONFIG			0xFD55
+struct session_config_vport {
+	__u8	type;
+	union {
+		struct {
+			__le16	acl_handle;
+			__u8	reserved[10];
+		} sco;
+		struct {
+			__u8	reserved[12];
+		} fm;
+		struct {
+			__u8	index;
+			__u8	slots_used;
+			__u8	slot_start[4];
+			__u8	reserved[6];
+		} pcm;
+		struct {
+			__u8	index;
+			__u8	channel;
+			__u8	reserved[10];
+		} i2s;
+	};
+} __packed;
+#define SESSIONCFG_PCM_SET_USED(port, idx, use)		\
+	store_bit(&(port).pcm.slots_used, (idx), (use))
+
+struct session_config_stream {
+	__u8	media_type;
+	__u8	csel_srate;
+	__u8	codec_type;
+	__u8	codec_mode;
+	__u8	codec_params[3];
+	struct session_config_vport inport;
+	struct session_config_vport outport;
+} __packed;
+#define SESSIONCFG_SET_CHANNELS(pcfg, chnl)		\
+	set_low_nibble(&(pcfg)->csel_srate, (chnl))
+#define SESSIONCFG_I2S_SET_SRATE(pcfg, rate)		\
+	set_high_nibble(&(pcfg)->csel_srate, (rate))
+
+struct bt_vs_session_config_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	n_streams; /* we only support one here */
+	struct session_config_stream stream;
+} __packed;
+
+#define CG2900_BT_SESSION_MEDIA_TYPE_AUDIO		0x00
+
+#define CG2900_BT_SESSION_RATE_8K			0x01
+#define CG2900_BT_SESSION_RATE_16K			0x02
+#define CG2900_BT_SESSION_RATE_44_1K			0x04
+#define CG2900_BT_SESSION_RATE_48K			0x05
+
+#define CG2900_BT_MEDIA_CONFIG_MONO			0x00
+#define CG2900_BT_MEDIA_CONFIG_STEREO			0x01
+#define CG2900_BT_MEDIA_CONFIG_JOINT_STEREO		0x02
+#define CG2900_BT_MEDIA_CONFIG_DUAL_CHANNEL		0x03
+
+#define CG2900_BT_SESSION_I2S_INDEX_I2S			0x00
+#define CG2900_BT_SESSION_PCM_INDEX_PCM_I2S		0x00
+
+
+#define CG2900_BT_VS_SESSION_CTRL			0xFD57
+struct bt_vs_session_ctrl_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	id;
+	__u8	control;
+} __packed;
+
+#define CG2900_BT_SESSION_START				0x00
+#define CG2900_BT_SESSION_STOP				0x01
+#define CG2900_BT_SESSION_PAUSE				0x02
+#define CG2900_BT_SESSION_RESUME			0x03
+
+#define CG2900_BT_VS_RESET_SESSION_CONFIG		0xFD56
+struct bt_vs_reset_session_cfg_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	id;
+} __packed;
+
+/*
+ *	CG2900 PG2 multimedia API
+ */
+
+#define CG2900_MC_PORT_PCM_I2S				0x00
+#define CG2900_MC_PORT_I2S				0x01
+#define CG2900_MC_PORT_BT_SCO				0x04
+#define CG2900_MC_PORT_FM_RX_0				0x07
+#define CG2900_MC_PORT_FM_RX_1				0x08
+#define CG2900_MC_PORT_FM_TX				0x09
+
+#define CG2900_MC_VS_PORT_CONFIG			0xFD64
+struct mc_vs_port_cfg_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	type;
+	/*
+	 * one of the following configuration structs should follow, but they
+	 * have different lengths so a union will not work
+	 */
+} __packed;
+
+struct mc_vs_port_cfg_pcm_i2s {
+	__u8 role_dir;
+	__u8 sco_a2dp_slots_used;
+	__u8 fm_slots_used;
+	__u8 ring_slots_used;
+	__u8 slot_start[4];
+	__u8 ratio_mode;
+	__u8 frame_len;
+	__u8 bitclk_srate;
+} __packed;
+#define PORTCFG_PCM_SET_ROLE(cfg, role)			\
+	set_low_nibble(&(cfg).role_dir, (role))
+#define PORTCFG_PCM_SET_DIR(cfg, idx, dir)		\
+	store_bit(&(cfg).role_dir, (idx) + 4, (dir))
+static inline void portcfg_pcm_set_sco_used(struct mc_vs_port_cfg_pcm_i2s *cfg,
+					    size_t index, __u8 use)
+{
+	if (use) {
+		/* clear corresponding slot in all cases */
+		cfg->sco_a2dp_slots_used &= ~(0x11 << index);
+		cfg->fm_slots_used &= ~(0x11 << index);
+		cfg->ring_slots_used &= ~(0x11 << index);
+		/* set for sco */
+		cfg->sco_a2dp_slots_used |= (1u << index);
+	} else {
+		/* only clear for sco */
+		cfg->sco_a2dp_slots_used &= ~(1u << index);
+	}
+}
+#define PORTCFG_PCM_SET_SCO_USED(cfg, idx, use)		\
+	portcfg_pcm_set_sco_used(&cfg, idx, use)
+#define PORTCFG_PCM_SET_RATIO(cfg, r)			\
+	set_low_nibble(&(cfg).ratio_mode, (r))
+#define PORTCFG_PCM_SET_MODE(cfg, mode)			\
+	set_high_nibble(&(cfg).ratio_mode, (mode))
+#define PORTCFG_PCM_SET_BITCLK(cfg, clk)		\
+	set_low_nibble(&(cfg).bitclk_srate, (clk))
+#define PORTCFG_PCM_SET_SRATE(cfg, rate)		\
+	set_high_nibble(&(cfg).bitclk_srate, (rate))
+
+#define CG2900_MC_PCM_SAMPLE_RATE_8			1
+#define CG2900_MC_PCM_SAMPLE_RATE_16			2
+#define CG2900_MC_PCM_SAMPLE_RATE_44_1			4
+#define CG2900_MC_PCM_SAMPLE_RATE_48			6
+
+struct mc_vs_port_cfg_i2s {
+	__u8 role_hper;
+	__u8 csel_srate;
+	__u8 wordlen;
+};
+#define PORTCFG_I2S_SET_ROLE(cfg, role)			\
+	set_low_nibble(&(cfg).role_hper, (role))
+#define PORTCFG_I2S_SET_HALFPERIOD(cfg, hper)		\
+	set_high_nibble(&(cfg).role_hper, (hper))
+#define PORTCFG_I2S_SET_CHANNELS(cfg, chnl)		\
+	set_low_nibble(&(cfg).csel_srate, (chnl))
+#define PORTCFG_I2S_SET_SRATE(cfg, rate)		\
+	set_high_nibble(&(cfg).csel_srate, (rate))
+#define PORTCFG_I2S_SET_WORDLEN(cfg, len)		\
+	set_low_nibble(&(cfg).wordlen, len)
+
+#define CG2900_MC_I2S_RIGHT_CHANNEL			1
+#define CG2900_MC_I2S_LEFT_CHANNEL			2
+#define CG2900_MC_I2S_BOTH_CHANNELS			3
+
+#define CG2900_MC_I2S_SAMPLE_RATE_8			0
+#define CG2900_MC_I2S_SAMPLE_RATE_16			1
+#define CG2900_MC_I2S_SAMPLE_RATE_44_1			2
+#define CG2900_MC_I2S_SAMPLE_RATE_48			4
+
+#define CG2900_MC_I2S_WORD_16				1
+#define CG2900_MC_I2S_WORD_32				3
+
+struct mc_vs_port_cfg_fm {
+	__u8 srate; /* NB: value goes in _upper_ nibble! */
+};
+#define PORTCFG_FM_SET_SRATE(cfg, rate)		\
+	set_high_nibble(&(cfg).srate, (rate))
+
+struct mc_vs_port_cfg_sco {
+	__le16	acl_id;
+	__u8	wbs_codec;
+	__u8	sbc_params[3]; /* replace when we actually enable WBS... */
+} __packed;
+#define PORTCFG_SCO_SET_WBS(cfg, wbs)		\
+	set_low_nibble(&(cfg).wbs_codec, (wbs))
+#define PORTCFG_SCO_SET_CODEC(cfg, codec)	\
+	set_high_nibble(&(cfg).wbs_codec, (codec))
+
+#define CG2900_MC_VS_CREATE_STREAM			0xFD66
+struct mc_vs_create_stream_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	id;
+	__u8	inport;
+	__u8	outport;
+	__u8	order; /* NB: not used by chip */
+} __packed;
+
+#define CG2900_MC_VS_DELETE_STREAM			0xFD67
+struct mc_vs_delete_stream_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	stream;
+} __packed;
+
+#define CG2900_MC_VS_STREAM_CONTROL			0xFD68
+struct mc_vs_stream_ctrl_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	command;
+	__u8	n_streams;
+	__u8	stream[];
+} __packed;
+
+#define CG2900_MC_STREAM_START				0x00
+#define CG2900_MC_STREAM_STOP				0x01
+#define CG2900_MC_STREAM_STOP_FLUSH			0x02
+
+#define CG2900_MC_VS_SET_FM_START_MODE			0xFD69
+
+/*
+ *	FM
+ */
+
+/* FM legacy command packet */
+struct fm_leg_cmd {
+	__u8	length;
+	__u8	opcode;
+	__u8	read_write;
+	__u8	fm_function;
+	union { /* Payload varies with function */
+		__le16	irqmask;
+		struct fm_leg_fm_cmd {
+			__le16	head;
+			__le16	data[];
+		} fm_cmd;
+	};
+} __packed;
+
+/* FM legacy command complete packet */
+struct fm_leg_cmd_cmpl {
+	__u8	param_length;
+	__u8	status;
+	__u8	opcode;
+	__u8	read_write;
+	__u8	cmd_status;
+	__u8	fm_function;
+	__le16	response_head;
+	__le16	data[];
+} __packed;
+
+/* FM legacy interrupt packet, PG2 style */
+struct fm_leg_irq_v2 {
+	__u8	param_length;
+	__u8	status;
+	__u8	opcode;
+	__u8	event_type;
+	__u8	event_id;
+	__le16	irq;
+} __packed;
+
+/* FM legacy interrupt packet, PG1 style */
+struct fm_leg_irq_v1 {
+	__u8	param_length;
+	__u8	opcode;
+	__u8	event_id;
+	__le16	irq;
+} __packed;
+
+union fm_leg_evt_or_irq {
+	__u8			param_length;
+	struct fm_leg_cmd_cmpl	evt;
+	struct fm_leg_irq_v2	irq_v2;
+	struct fm_leg_irq_v1	irq_v1;
+} __packed;
+
+/* FM Opcode generic*/
+#define CG2900_FM_GEN_ID_LEGACY				0xFE
+
+/* FM event*/
+#define CG2900_FM_EVENT_UNKNOWN				0
+#define CG2900_FM_EVENT_CMD_COMPLETE			1
+#define CG2900_FM_EVENT_INTERRUPT			2
+
+/* FM do-command identifiers. */
+#define CG2900_FM_DO_AIP_FADE_START			0x0046
+#define CG2900_FM_DO_AUP_BT_FADE_START			0x01C2
+#define CG2900_FM_DO_AUP_EXT_FADE_START			0x0102
+#define CG2900_FM_DO_AUP_FADE_START			0x00A2
+#define CG2900_FM_DO_FMR_SETANTENNA			0x0663
+#define CG2900_FM_DO_FMR_SP_AFSWITCH_START		0x04A3
+#define CG2900_FM_DO_FMR_SP_AFUPDATE_START		0x0463
+#define CG2900_FM_DO_FMR_SP_BLOCKSCAN_START		0x0683
+#define CG2900_FM_DO_FMR_SP_PRESETPI_START		0x0443
+#define CG2900_FM_DO_FMR_SP_SCAN_START			0x0403
+#define CG2900_FM_DO_FMR_SP_SEARCH_START		0x03E3
+#define CG2900_FM_DO_FMR_SP_SEARCHPI_START		0x0703
+#define CG2900_FM_DO_FMR_SP_TUNE_SETCHANNEL		0x03C3
+#define CG2900_FM_DO_FMR_SP_TUNE_STEPCHANNEL		0x04C3
+#define CG2900_FM_DO_FMT_PA_SETCTRL			0x01A4
+#define CG2900_FM_DO_FMT_PA_SETMODE			0x01E4
+#define CG2900_FM_DO_FMT_SP_TUNE_SETCHANNEL		0x0064
+#define CG2900_FM_DO_GEN_ANTENNACHECK_START		0x02A1
+#define CG2900_FM_DO_GEN_GOTOMODE			0x0041
+#define CG2900_FM_DO_GEN_POWERSUPPLY_SETMODE		0x0221
+#define CG2900_FM_DO_GEN_SELECTREFERENCECLOCK		0x0201
+#define CG2900_FM_DO_GEN_SETPROCESSINGCLOCK		0x0241
+#define CG2900_FM_DO_GEN_SETREFERENCECLOCKPLL		0x01A1
+#define CG2900_FM_DO_TST_TX_RAMP_START			0x0147
+#define CG2900_FM_CMD_NONE				0xFFFF
+#define CG2900_FM_CMD_ID_GEN_GOTO_POWER_DOWN		0x0081
+#define CG2900_FM_CMD_ID_GEN_GOTO_STANDBY		0x0061
+
+/* FM Command IDs */
+#define CG2900_FM_CMD_ID_AUP_EXT_SET_MODE		0x0162
+#define CG2900_FM_CMD_ID_AUP_EXT_SET_CTRL		0x0182
+#define CG2900_FM_CMD_ID_AIP_SET_MODE			0x01C6
+#define CG2900_FM_CMD_ID_AIP_BT_SET_CTRL		0x01A6
+#define CG2900_FM_CMD_ID_AIP_BT_SET_MODE		0x01E6
+
+/* FM Command Parameters. */
+#define CG2900_FM_CMD_PARAM_ENABLE			0x00
+#define CG2900_FM_CMD_PARAM_DISABLE			0x01
+#define CG2900_FM_CMD_PARAM_RESET			0x02
+#define CG2900_FM_CMD_PARAM_WRITECOMMAND		0x10
+#define CG2900_FM_CMD_PARAM_SET_INT_MASK_ALL		0x20
+#define CG2900_FM_CMD_PARAM_GET_INT_MASK_ALL		0x21
+#define CG2900_FM_CMD_PARAM_SET_INT_MASK		0x22
+#define CG2900_FM_CMD_PARAM_GET_INT_MASK		0x23
+#define CG2900_FM_CMD_PARAM_FM_FW_DOWNLOAD		0x30
+#define CG2900_FM_CMD_PARAM_NONE			0xFF
+
+/* FM Legacy Command Parameters */
+#define CG2900_FM_CMD_LEG_PARAM_WRITE			0x00
+#define CG2900_FM_CMD_LEG_PARAM_IRQ			0x01
+
+/* FM Command Status. */
+#define CG2900_FM_CMD_STATUS_COMMAND_SUCCEEDED		0x00
+#define CG2900_FM_CMD_STATUS_HW_FAILURE			0x03
+#define CG2900_FM_CMD_STATUS_INVALID_PARAMS		0x12
+#define CG2900_FM_CMD_STATUS_UNINITILIZED		0x15
+#define CG2900_FM_CMD_STATUS_UNSPECIFIED_ERROR		0x1F
+#define CG2900_FM_CMD_STATUS_COMMAND_DISALLOWED		0x0C
+#define CG2900_FM_CMD_STATUS_FW_WRONG_SEQUENCE_NR	0xF1
+#define CG2900_FM_CMD_STATUS_FW_UNKNOWN_FILE		0xF2
+#define CG2900_FM_CMD_STATUS_FW_FILE_VER_MISMATCH	0xF3
+
+/* FM Interrupts. */
+#define CG2900_FM_IRPT_FIQ				0x0000
+#define CG2900_FM_IRPT_OPERATION_SUCCEEDED		0x0001
+#define CG2900_FM_IRPT_OPERATION_FAILED			0x0002
+#define CG2900_FM_IRPT_BUFFER_FULL			0x0008
+#define CG2900_FM_IRPT_BUFFER_EMPTY			0x0008
+#define CG2900_FM_IRPT_SIGNAL_QUALITY_LOW		0x0010
+#define CG2900_FM_IRPT_MUTE_STATUS_CHANGED		0x0010
+#define CG2900_FM_IRPT_MONO_STEREO_TRANSITION		0x0020
+#define CG2900_FM_IRPT_OVER_MODULATION			0x0020
+#define CG2900_FM_IRPT_RDS_SYNC_FOUND			0x0040
+#define CG2900_FM_IRPT_INPUT_OVERDRIVE			0x0040
+#define CG2900_FM_IRPT_RDS_SYNC_LOST			0x0080
+#define CG2900_FM_IRPT_PI_CODE_CHANGED			0x0100
+#define CG2900_FM_IRPT_REQUEST_BLOCK_AVALIBLE		0x0200
+#define CG2900_FM_IRPT_BUFFER_CLEARED			0x2000
+#define CG2900_FM_IRPT_WARM_BOOT_READY			0x4000
+#define CG2900_FM_IRPT_COLD_BOOT_READY			0x8000
+
+/* FM Legacy Function Command Parameters */
+
+/* AUP_EXT_SetMode Output enum */
+#define CG2900_FM_CMD_AUP_EXT_SET_MODE_DISABLED		0x0000
+#define CG2900_FM_CMD_AUP_EXT_SET_MODE_I2S		0x0001
+#define CG2900_FM_CMD_AUP_EXT_SET_MODE_PARALLEL		0x0002
+
+/* SetControl Conversion enum */
+#define CG2900_FM_CMD_SET_CTRL_CONV_UP			0x0000
+#define CG2900_FM_CMD_SET_CTRL_CONV_DOWN		0x0001
+
+/* AIP_SetMode Input enum */
+#define CG2900_FM_CMD_AIP_SET_MODE_INPUT_ANA		0x0000
+#define CG2900_FM_CMD_AIP_SET_MODE_INPUT_DIG		0x0001
+
+/* AIP_BT_SetMode Input enum */
+#define CG2900_FM_CMD_AIP_BT_SET_MODE_INPUT_RESERVED	0x0000
+#define CG2900_FM_CMD_AIP_BT_SET_MODE_INPUT_I2S		0x0001
+#define CG2900_FM_CMD_AIP_BT_SET_MODE_INPUT_PAR		0x0002
+#define CG2900_FM_CMD_AIP_BT_SET_MODE_INPUT_FIFO	0x0003
+
+/* FM Parameter Lengths = FM command length - length field (1 byte) */
+#define CG2900_FM_CMD_PARAM_LEN(len) (len - 1)
+
+/*
+ * FM Command ID mapped per byte and shifted 3 bits left
+ * Also adds number of parameters at first 3 bits of LSB.
+ */
+static inline __u16 cg2900_get_fm_cmd_id(__u16 opcode)
+{
+	return opcode >> 3;
+}
+
+static inline __u16 cg2900_make_fm_cmd_id(__u16 id, __u8 num_params)
+{
+	return (id << 3) | num_params;
+}
+
+/*
+ *	GNSS
+ */
+
+struct gnss_hci_hdr {
+	__u8	op_code;
+	__le16	plen;
+} __packed;
+
+#endif /* _CG2900_CHIP_H_ */
diff --git a/drivers/staging/cg2900/mfd/cg2900_core.c b/drivers/staging/cg2900/mfd/cg2900_core.c
new file mode 100644
index 00000000000..66a452f817a
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_core.c
@@ -0,0 +1,713 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+#define NAME					"cg2900_core"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <linux/firmware.h>
+#include <linux/fs.h>
+#include <linux/gfp.h>
+#include <linux/init.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/stat.h>
+#include <linux/time.h>
+#include <linux/timer.h>
+#include <linux/types.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/core.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+
+#include "cg2900.h"
+#include "cg2900_core.h"
+
+/* Device names */
+#define CG2900_CDEV_NAME		"cg2900_core_test"
+#define CG2900_CLASS_NAME		"cg2900_class"
+#define CG2900_DEVICE_NAME		"cg2900_driver"
+#define CORE_WQ_NAME			"cg2900_core_wq"
+
+#define LOGGER_DIRECTION_TX		0
+#define LOGGER_DIRECTION_RX		1
+
+/*
+ * Timeout values
+ */
+#define CHIP_READY_TIMEOUT		(100)	/* ms */
+#define REVISION_READOUT_TIMEOUT	(500)	/* ms */
+#define SLEEP_TIMEOUT_MS		(10000)	/* ms */
+
+/**
+ * enum boot_state - BOOT-state for CG2900 Core.
+ * @BOOT_RESET:					HCI Reset has been sent.
+ * @BOOT_READ_LOCAL_VERSION_INFORMATION:	ReadLocalVersionInformation
+ *						command has been sent.
+ * @BOOT_READY:					CG2900 Core boot is ready.
+ * @BOOT_FAILED:				CG2900 Core boot failed.
+ */
+enum boot_state {
+	BOOT_RESET,
+	BOOT_READ_LOCAL_VERSION_INFORMATION,
+	BOOT_READY,
+	BOOT_FAILED
+};
+
+/**
+ * struct chip_handler_item - Structure to store chip handler cb.
+ * @list:	list_head struct.
+ * @cb:		Chip handler callback struct.
+ */
+struct chip_handler_item {
+	struct list_head		list;
+	struct cg2900_id_callbacks	cb;
+};
+
+/**
+ * struct core_info - Main info structure for CG2900 Core.
+ * @boot_state:		Current BOOT-state of CG2900 Core.
+ * @wq:			CG2900 Core workqueue.
+ * @chip_dev:		Device structure for chip driver.
+ * @work:		Work structure.
+ */
+struct core_info {
+	enum boot_state			boot_state;
+	struct workqueue_struct		*wq;
+	struct cg2900_chip_dev		*chip_dev;
+	struct work_struct		work;
+};
+
+/**
+ * struct main_info - Main info structure for CG2900 Core.
+ * @dev:		Device structure for STE Connectivity driver.
+ * @man_mutex:		Management mutex.
+ * @chip_handlers:	List of the register handlers for different chips.
+ * @wq:			Wait queue.
+ */
+struct main_info {
+	struct device		*dev;
+	struct mutex		man_mutex;
+	struct list_head	chip_handlers;
+	wait_queue_head_t	wq;
+};
+
+/* core_info - Main information object for CG2900 Core. */
+static struct main_info *main_info;
+
+/* Module parameters */
+u8 bd_address[] = {0x00, 0xBE, 0xAD, 0xDE, 0x80, 0x00};
+EXPORT_SYMBOL_GPL(bd_address);
+int bd_addr_count = BT_BDADDR_SIZE;
+
+static int sleep_timeout_ms = SLEEP_TIMEOUT_MS;
+
+/**
+ * send_bt_cmd() - Copy and send sk_buffer with no assigned user.
+ * @dev:	Current chip to transmit to.
+ * @data:	Data to send.
+ * @length:	Length in bytes of data.
+ *
+ * The send_bt_cmd() function allocate sk_buffer, copy supplied
+ * data to it, and send the sk_buffer to controller.
+ */
+void send_bt_cmd(struct cg2900_chip_dev *dev, void *data, int length)
+{
+	struct sk_buff *skb;
+	int err;
+
+	skb = alloc_skb(length + HCI_H4_SIZE, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(dev->dev, "send_bt_cmd: Couldn't alloc sk_buff with "
+			"length %d\n", length);
+		return;
+	}
+
+	skb_reserve(skb, HCI_H4_SIZE);
+	memcpy(skb_put(skb, length), data, length);
+	skb_push(skb, HCI_H4_SIZE);
+	skb->data[0] = HCI_BT_CMD_H4_CHANNEL;
+
+	err = dev->t_cb.write(dev, skb);
+	if (err) {
+		dev_err(dev->dev, "send_bt_cmd: Transport write failed (%d)\n",
+			err);
+		kfree_skb(skb);
+	}
+}
+
+/**
+ * handle_reset_cmd_complete_evt() - Handle a received HCI Command Complete event for a Reset command.
+ * @dev:	Current device.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   True,  if packet was handled internally,
+ *   False, otherwise.
+ */
+static bool handle_reset_cmd_complete_evt(struct cg2900_chip_dev *dev, u8 *data)
+{
+	bool pkt_handled = false;
+	u8 status = data[0];
+	struct hci_command_hdr cmd;
+	struct core_info *info = dev->prv_data;
+
+	dev_dbg(dev->dev, "Received Reset complete event with status 0x%X\n",
+		status);
+
+	if (info->boot_state == BOOT_RESET) {
+		/* Transmit HCI Read Local Version Information command */
+		dev_dbg(dev->dev, "New boot_state: "
+			"BOOT_READ_LOCAL_VERSION_INFORMATION\n");
+		info->boot_state = BOOT_READ_LOCAL_VERSION_INFORMATION;
+		cmd.opcode = cpu_to_le16(HCI_OP_READ_LOCAL_VERSION);
+		cmd.plen = 0; /* No parameters for HCI reset */
+		send_bt_cmd(dev, &cmd, sizeof(cmd));
+
+		pkt_handled = true;
+	}
+
+	return pkt_handled;
+}
+
+/**
+ * handle_read_local_version_info_cmd_complete_evt() - Handle a received HCI Command Complete event for a ReadLocalVersionInformation command.
+ * @dev:	Current device.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   True,  if packet was handled internally,
+ *   False, otherwise.
+ */
+static bool
+handle_read_local_version_info_cmd_complete_evt(struct cg2900_chip_dev *dev,
+						u8 *data)
+{
+	struct hci_rp_read_local_version *evt;
+	struct core_info *info = dev->prv_data;
+
+	/* Check we're in the right state */
+	if (info->boot_state != BOOT_READ_LOCAL_VERSION_INFORMATION)
+		return false;
+
+	/* We got an answer for our HCI command. Extract data */
+	evt = (struct hci_rp_read_local_version *)data;
+
+	/* We will handle the packet */
+	if (HCI_BT_ERROR_NO_ERROR != evt->status) {
+		dev_err(dev->dev, "Received Read Local Version Information "
+			"with status 0x%X\n", evt->status);
+		dev_dbg(dev->dev, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		wake_up_all(&main_info->wq);
+		return true;
+	}
+
+	/* The command worked. Store the data */
+	dev->chip.hci_version = evt->hci_ver;
+	dev->chip.hci_revision = le16_to_cpu(evt->hci_rev);
+	dev->chip.lmp_pal_version = evt->lmp_ver;
+	dev->chip.manufacturer = le16_to_cpu(evt->manufacturer);
+	dev->chip.hci_sub_version = le16_to_cpu(evt->lmp_subver);
+	dev_info(dev->dev, "Received Read Local Version Information with:\n"
+		 "\thci_version:  0x%02X\n"
+		 "\thci_revision: 0x%04X\n"
+		 "\tlmp_pal_version: 0x%02X\n"
+		 "\tmanufacturer: 0x%04X\n"
+		 "\thci_sub_version: 0x%04X\n",
+		 dev->chip.hci_version, dev->chip.hci_revision,
+		 dev->chip.lmp_pal_version, dev->chip.manufacturer,
+		 dev->chip.hci_sub_version);
+
+	dev_dbg(dev->dev, "New boot_state: BOOT_READY\n");
+	info->boot_state = BOOT_READY;
+	wake_up_all(&main_info->wq);
+
+	return true;
+}
+
+/**
+ * handle_rx_data_bt_evt() - Check if data should be handled in CG2900 Core.
+ * @dev:	Current chip
+ * @skb:	Data packet
+ *
+ * The handle_rx_data_bt_evt() function checks if received data should be
+ * handled in CG2900 Core. If so handle it correctly.
+ * Received data is always HCI BT Event.
+ *
+ * Returns:
+ *   True,  if packet was handled internally,
+ *   False, otherwise.
+ */
+static bool handle_rx_data_bt_evt(struct cg2900_chip_dev *dev,
+				  struct sk_buff *skb)
+{
+	bool pkt_handled = false;
+	u8 *data = &skb->data[CG2900_SKB_RESERVE];
+	struct hci_event_hdr *evt;
+	struct hci_ev_cmd_complete *cmd_complete;
+	u16 op_code;
+
+	evt = (struct hci_event_hdr *)data;
+
+	/* First check the event code */
+	if (HCI_EV_CMD_COMPLETE != evt->evt)
+		return false;
+
+	data += sizeof(*evt);
+	cmd_complete = (struct hci_ev_cmd_complete *)data;
+
+	op_code = le16_to_cpu(cmd_complete->opcode);
+
+	dev_dbg(dev->dev, "Received Command Complete: op_code = 0x%04X\n",
+		op_code);
+	data += sizeof(*cmd_complete); /* Move to first byte after OCF */
+
+	if (op_code == HCI_OP_RESET)
+		pkt_handled = handle_reset_cmd_complete_evt(dev, data);
+	else if (op_code == HCI_OP_READ_LOCAL_VERSION)
+		pkt_handled = handle_read_local_version_info_cmd_complete_evt
+					(dev, data);
+
+	if (pkt_handled)
+		kfree_skb(skb);
+
+	return pkt_handled;
+}
+
+static void cg2900_data_from_chip(struct cg2900_chip_dev *dev,
+				  struct sk_buff *skb)
+{
+	u8 h4_channel;
+
+	dev_dbg(dev->dev, "cg2900_data_from_chip\n");
+
+	if (!skb) {
+		dev_err(dev->dev, "No data supplied\n");
+		return;
+	}
+
+	h4_channel = skb->data[0];
+
+	/*
+	 * First check if this is the response for something
+	 * we have sent internally.
+	 */
+	if (HCI_BT_EVT_H4_CHANNEL == h4_channel &&
+	    handle_rx_data_bt_evt(dev, skb)) {
+		dev_dbg(dev->dev, "Received packet handled internally\n");
+	} else {
+		dev_err(dev->dev,
+			"cg2900_data_from_chip: Received unexpected packet\n");
+		kfree_skb(skb);
+	}
+}
+
+/**
+ * work_hw_registered() - Called when the interface to HW has been established.
+ * @work:	Reference to work data.
+ *
+ * Since there now is a transport identify the connected chip and decide which
+ * chip handler to use.
+ */
+static void work_hw_registered(struct work_struct *work)
+{
+	struct hci_command_hdr cmd;
+	struct cg2900_chip_dev *dev;
+	struct core_info *info;
+	bool chip_handled = false;
+	struct list_head *cursor;
+	struct chip_handler_item *tmp;
+
+	dev_dbg(main_info->dev, "work_hw_registered\n");
+
+	if (!work) {
+		dev_err(main_info->dev, "work_hw_registered: work == NULL\n");
+		return;
+	}
+
+	info = container_of(work, struct core_info, work);
+	dev = info->chip_dev;
+
+	/*
+	 * This might look strange, but we need to read out
+	 * the revision info in order to be able to shutdown the chip properly.
+	 */
+	if (dev->t_cb.set_chip_power)
+		dev->t_cb.set_chip_power(dev, true);
+
+	/* Wait 100ms before continuing to be sure that the chip is ready */
+	schedule_timeout_killable(msecs_to_jiffies(CHIP_READY_TIMEOUT));
+
+	/* Set our function to receive data from chip */
+	dev->c_cb.data_from_chip = cg2900_data_from_chip;
+
+	/*
+	 * Transmit HCI reset command to ensure the chip is using
+	 * the correct transport
+	 */
+	dev_dbg(dev->dev, "New boot_state: BOOT_RESET\n");
+	info->boot_state = BOOT_RESET;
+	cmd.opcode = cpu_to_le16(HCI_OP_RESET);
+	cmd.plen = 0; /* No parameters for HCI reset */
+	send_bt_cmd(dev, &cmd, sizeof(cmd));
+
+	dev_dbg(dev->dev,
+		"Wait up to 500 milliseconds for revision to be read\n");
+	wait_event_timeout(main_info->wq,
+			   (BOOT_READY == info->boot_state ||
+			    BOOT_FAILED == info->boot_state),
+			    msecs_to_jiffies(REVISION_READOUT_TIMEOUT));
+
+	if (BOOT_READY != info->boot_state) {
+		dev_err(dev->dev,
+			"Could not read out revision from the chip\n");
+		return;
+	}
+
+	dev->c_cb.data_from_chip = NULL;
+
+	mutex_lock(&main_info->man_mutex);
+	list_for_each(cursor, &main_info->chip_handlers) {
+		tmp = list_entry(cursor, struct chip_handler_item, list);
+		chip_handled = tmp->cb.check_chip_support(dev);
+		if (chip_handled) {
+			dev_info(dev->dev, "Chip handler found\n");
+			break;
+		}
+	}
+	mutex_unlock(&main_info->man_mutex);
+
+	if (!chip_handled)
+		dev_info(dev->dev, "No chip handler found\n");
+}
+
+/**
+ * cg2900_register_chip_driver() - Register a chip handler.
+ * @cb:	Callbacks to call when chip is connected.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL is supplied as @cb.
+ *   -ENOMEM if allocation fails or work queue can't be created.
+ */
+int cg2900_register_chip_driver(struct cg2900_id_callbacks *cb)
+{
+	struct chip_handler_item *item;
+
+	dev_dbg(main_info->dev, "cg2900_register_chip_driver\n");
+
+	if (!cb) {
+		dev_err(main_info->dev, "NULL supplied as cb\n");
+		return -EINVAL;
+	}
+
+	item = kzalloc(sizeof(*item), GFP_KERNEL);
+	if (!item) {
+		dev_err(main_info->dev,
+			"cg2900_register_chip_driver: "
+			"Failed to alloc memory\n");
+		return -ENOMEM;
+	}
+
+	memcpy(&item->cb, cb, sizeof(cb));
+	mutex_lock(&main_info->man_mutex);
+	list_add_tail(&item->list, &main_info->chip_handlers);
+	mutex_unlock(&main_info->man_mutex);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(cg2900_register_chip_driver);
+
+/**
+ * cg2900_deregister_chip_driver() - Deregister a chip handler.
+ * @cb:	Callbacks to call when chip is connected.
+ */
+void cg2900_deregister_chip_driver(struct cg2900_id_callbacks *cb)
+{
+	struct chip_handler_item *tmp;
+	struct list_head *cursor, *next;
+
+	dev_dbg(main_info->dev, "cg2900_deregister_chip_driver\n");
+
+	if (!cb) {
+		dev_err(main_info->dev, "NULL supplied as cb\n");
+		return;
+	}
+	mutex_lock(&main_info->man_mutex);
+	list_for_each_safe(cursor, next, &main_info->chip_handlers) {
+		tmp = list_entry(cursor, struct chip_handler_item, list);
+		if (tmp->cb.check_chip_support == cb->check_chip_support) {
+			list_del(cursor);
+			kfree(tmp);
+			break;
+		}
+	}
+	mutex_unlock(&main_info->man_mutex);
+}
+EXPORT_SYMBOL_GPL(cg2900_deregister_chip_driver);
+
+/**
+ * cg2900_register_trans_driver() - Register a transport driver.
+ * @dev:	Transport device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL is supplied as @cb.
+ *   -ENOMEM if allocation fails or work queue can't be created.
+ *   -EACCES if work can't be queued.
+ */
+int cg2900_register_trans_driver(struct cg2900_chip_dev *dev)
+{
+	int err;
+	struct cg2900_platform_data *pf_data;
+	struct core_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!dev || !dev->dev) {
+		dev_err(main_info->dev, "cg2900_register_trans_driver: "
+			"Received NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(dev->dev, "cg2900_register_trans_driver\n");
+
+	if (!dev->t_cb.write) {
+		dev_err(dev->dev, "cg2900_register_trans_driver: Write function"
+			" missing\n");
+		return -EINVAL;
+	}
+
+	pf_data = dev_get_platdata(dev->dev);
+	if (!pf_data) {
+		dev_err(dev->dev, "cg2900_register_trans_driver: Missing "
+			"platform data\n");
+		return -EINVAL;
+	}
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(dev->dev, "Couldn't allocate info\n");
+		return -ENOMEM;
+	}
+
+	if (pf_data->init) {
+		err = pf_data->init(dev);
+		if (err) {
+			dev_err(dev->dev, "Platform init failed (%d)\n", err);
+			goto error_handling;
+		}
+	}
+
+	info->chip_dev = dev;
+	dev->prv_data = info;
+
+	info->wq = create_singlethread_workqueue(CORE_WQ_NAME);
+	if (!info->wq) {
+		dev_err(dev->dev, "Could not create workqueue\n");
+		err = -ENOMEM;
+		goto error_handling_exit;
+	}
+
+	dev_info(dev->dev, "Transport connected\n");
+
+	INIT_WORK(&info->work, work_hw_registered);
+	if (!queue_work(info->wq, &info->work)) {
+		dev_err(dev->dev, "Failed to queue work_hw_registered because "
+			"it's already in the queue\n");
+		err = -EACCES;
+		goto error_handling_wq;
+	}
+
+	return 0;
+
+error_handling_wq:
+	destroy_workqueue(info->wq);
+error_handling_exit:
+	if (pf_data->exit)
+		pf_data->exit(dev);
+error_handling:
+	kfree(info);
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_register_trans_driver);
+
+/**
+ * cg2900_deregister_trans_driver() - Deregister a transport driver.
+ * @dev:	Transport device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL is supplied as @cb.
+ *   -ENOMEM if allocation fails or work queue can't be created.
+ */
+int cg2900_deregister_trans_driver(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_platform_data *pf_data;
+	struct core_info *info = dev->prv_data;
+
+	BUG_ON(!main_info);
+
+	dev_dbg(dev->dev, "cg2900_deregister_trans_driver\n");
+
+	if (dev->c_cb.chip_removed)
+		dev->c_cb.chip_removed(dev);
+
+	destroy_workqueue(info->wq);
+
+	dev->prv_data = NULL;
+	kfree(info);
+
+	dev_info(dev->dev, "Transport disconnected\n");
+
+	pf_data = dev_get_platdata(dev->dev);
+	if (!pf_data) {
+		dev_err(dev->dev, "Missing platform data\n");
+		return -EINVAL;
+	}
+
+	if (pf_data->exit)
+		pf_data->exit(dev);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(cg2900_deregister_trans_driver);
+
+/**
+ * cg2900_get_sleep_timeout() - Return sleep timeout in jiffies.
+ *
+ * Returns:
+ *   Sleep timeout in jiffies. 0 means that sleep timeout shall not be used.
+ */
+unsigned long cg2900_get_sleep_timeout(void)
+{
+	if (!sleep_timeout_ms)
+		return 0;
+
+	return msecs_to_jiffies(sleep_timeout_ms);
+}
+EXPORT_SYMBOL_GPL(cg2900_get_sleep_timeout);
+
+/**
+ * cg2900_probe() - Initialize module.
+ *
+ * @pdev:	Platform device.
+ *
+ * This function initialize the transport and CG2900 Core, then
+ * register to the transport framework.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM for failed alloc or structure creation.
+ */
+static int __devinit cg2900_probe(struct platform_device *pdev)
+{
+	dev_dbg(&pdev->dev, "cg2900_probe\n");
+
+	main_info = kzalloc(sizeof(*main_info), GFP_KERNEL);
+	if (!main_info) {
+		dev_err(&pdev->dev, "Couldn't allocate main_info\n");
+		return -ENOMEM;
+	}
+
+	main_info->dev = &pdev->dev;
+	mutex_init(&main_info->man_mutex);
+	INIT_LIST_HEAD(&main_info->chip_handlers);
+	init_waitqueue_head(&main_info->wq);
+
+	dev_info(&pdev->dev, "CG2900 Core driver started\n");
+
+	return 0;
+}
+
+/**
+ * cg2900_remove() - Remove module.
+ *
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM if core_info does not exist.
+ *   -EINVAL if platform data does not exist in the device.
+ */
+static int __devexit cg2900_remove(struct platform_device *pdev)
+{
+	dev_dbg(&pdev->dev, "cg2900_remove\n");
+
+	kfree(main_info);
+	main_info = NULL;
+
+	dev_info(&pdev->dev, "CG2900 Core driver removed\n");
+
+	return 0;
+}
+
+static struct platform_driver cg2900_driver = {
+	.driver = {
+		.name	= "cg2900",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_probe,
+	.remove	= __devexit_p(cg2900_remove),
+};
+
+/**
+ * cg2900_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_init(void)
+{
+	pr_debug("cg2900_init");
+	return platform_driver_register(&cg2900_driver);
+}
+
+/**
+ * cg2900_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_exit(void)
+{
+	pr_debug("cg2900_exit");
+	platform_driver_unregister(&cg2900_driver);
+}
+
+module_init(cg2900_init);
+module_exit(cg2900_exit);
+
+module_param(sleep_timeout_ms, int, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(sleep_timeout_ms,
+		 "Sleep timeout for data transmissions:\n"
+		 "\tDefault 10000 ms\n"
+		 "\t0 = disable\n"
+		 "\t>0 = sleep timeout in milliseconds");
+
+module_param_array(bd_address, byte, &bd_addr_count,
+		   S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(bd_address,
+		 "Bluetooth Device address. "
+		 "Default 0x00 0x80 0xDE 0xAD 0xBE 0xEF. "
+		 "Enter as comma separated value.");
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux Bluetooth HCI H:4 CG2900 Connectivity Device Driver");
diff --git a/drivers/staging/cg2900/mfd/cg2900_core.h b/drivers/staging/cg2900/mfd/cg2900_core.h
new file mode 100644
index 00000000000..bdd951a501d
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_core.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+
+#ifndef _CG2900_CORE_H_
+#define _CG2900_CORE_H_
+
+#include <linux/device.h>
+#include <linux/skbuff.h>
+
+/* Reserve 1 byte for the HCI H:4 header */
+#define HCI_H4_SIZE				1
+#define CG2900_SKB_RESERVE			HCI_H4_SIZE
+
+/* Number of bytes to reserve at start of sk_buffer when receiving packet */
+#define RX_SKB_RESERVE				8
+
+#define BT_BDADDR_SIZE				6
+
+/* Standardized Bluetooth H:4 channels */
+#define HCI_BT_CMD_H4_CHANNEL			0x01
+#define HCI_BT_ACL_H4_CHANNEL			0x02
+#define HCI_BT_SCO_H4_CHANNEL			0x03
+#define HCI_BT_EVT_H4_CHANNEL			0x04
+
+/* Default H4 channels which may change depending on connected controller */
+#define HCI_FM_RADIO_H4_CHANNEL			0x08
+#define HCI_GNSS_H4_CHANNEL			0x09
+
+/* Bluetooth error codes */
+#define HCI_BT_ERROR_NO_ERROR			0x00
+
+/* Bluetooth lengths */
+#define HCI_BT_SEND_FILE_MAX_CHUNK_SIZE		254
+
+#define LOGGER_DIRECTION_TX			0
+#define LOGGER_DIRECTION_RX			1
+
+/* module_param declared in cg2900_core.c */
+extern u8 bd_address[BT_BDADDR_SIZE];
+
+#endif /* _CG2900_CORE_H_ */
diff --git a/drivers/staging/cg2900/mfd/cg2900_lib.c b/drivers/staging/cg2900/mfd/cg2900_lib.c
new file mode 100644
index 00000000000..cb8ad46a486
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_lib.c
@@ -0,0 +1,281 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+#define NAME					"cg2900_lib"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <linux/kernel.h>
+#include <linux/limits.h>
+#include <linux/skbuff.h>
+#include <linux/types.h>
+
+#include "cg2900.h"
+#include "cg2900_chip.h"
+#include "cg2900_core.h"
+#include "cg2900_lib.h"
+
+/*
+ * Max length in bytes for line buffer used to parse settings and patch file.
+ * Must be max length of name plus characters used to define chip version.
+ */
+#define LINE_BUFFER_LENGTH			(NAME_MAX + 30)
+#define LOGGER_HEADER_SIZE			1
+/**
+ * cg2900_tx_to_chip() - Transmit buffer to the transport.
+ * @user:	User data for BT command channel.
+ * @logger:	User data for logger channel.
+ * @skb:	Data packet.
+ *
+ * The transmit_skb_to_chip() function transmit buffer to the transport.
+ * If enabled, copy the transmitted data to the HCI logger as well.
+ */
+void cg2900_tx_to_chip(struct cg2900_user_data *user,
+		       struct cg2900_user_data *logger, struct sk_buff *skb)
+{
+	int err;
+	struct cg2900_chip_dev *chip_dev;
+
+	dev_dbg(user->dev, "cg2900_tx_to_chip %d bytes.\n", skb->len);
+
+	if (logger)
+		cg2900_send_to_hci_logger(logger, skb, LOGGER_DIRECTION_TX);
+
+	chip_dev = cg2900_get_prv(user);
+	err = chip_dev->t_cb.write(chip_dev, skb);
+	if (err) {
+		dev_err(user->dev, "cg2900_tx_to_chip: Transport write failed "
+			"(%d)\n", err);
+		kfree_skb(skb);
+	}
+}
+EXPORT_SYMBOL_GPL(cg2900_tx_to_chip);
+
+/**
+ * cg2900_tx_no_user() - Transmit buffer to the transport.
+ * @dev:	Current chip to transmit to.
+ * @skb:	Data packet.
+ *
+ * This function transmits buffer to the transport when no user exist (system
+ * startup for example).
+ */
+void cg2900_tx_no_user(struct cg2900_chip_dev *dev, struct sk_buff *skb)
+{
+	int err;
+
+	dev_dbg(dev->dev, "cg2900_tx_no_user %d bytes.\n", skb->len);
+
+	err = dev->t_cb.write(dev, skb);
+	if (err) {
+		dev_err(dev->dev, "cg2900_tx_no_user: Transport write failed "
+			"(%d)\n", err);
+		kfree_skb(skb);
+	}
+}
+EXPORT_SYMBOL_GPL(cg2900_tx_no_user);
+
+/**
+ * create_and_send_bt_cmd() - Copy and send sk_buffer.
+ * @user:	User data for current channel.
+ * @logger:	User data for logger channel.
+ * @data:	Data to send.
+ * @length:	Length in bytes of data.
+ *
+ * The create_and_send_bt_cmd() function allocate sk_buffer, copy supplied data
+ * to it, and send the sk_buffer to controller.
+ */
+void cg2900_send_bt_cmd(struct cg2900_user_data *user,
+			struct cg2900_user_data *logger,
+			void *data, int length)
+{
+	struct sk_buff *skb;
+
+	skb = user->alloc_skb(length, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(user->dev, "cg2900_send_bt_cmd: Couldn't alloc "
+			"sk_buff with length %d\n", length);
+		return;
+	}
+
+	memcpy(skb_put(skb, length), data, length);
+	skb_push(skb, HCI_H4_SIZE);
+	skb->data[0] = HCI_BT_CMD_H4_CHANNEL;
+
+	cg2900_tx_to_chip(user, logger, skb);
+}
+EXPORT_SYMBOL_GPL(cg2900_send_bt_cmd);
+
+/**
+ * cg2900_send_bt_cmd_no_user() - Copy and send sk_buffer with no assigned user.
+ * @dev:	Current chip to transmit to.
+ * @data:	Data to send.
+ * @length:	Length in bytes of data.
+ *
+ * The cg2900_send_bt_cmd_no_user() function allocate sk_buffer, copy supplied
+ * data to it, and send the sk_buffer to controller.
+ */
+void cg2900_send_bt_cmd_no_user(struct cg2900_chip_dev *dev, void *data,
+				int length)
+{
+	struct sk_buff *skb;
+
+	skb = alloc_skb(length + HCI_H4_SIZE, GFP_KERNEL);
+	if (!skb) {
+		dev_err(dev->dev, "cg2900_send_bt_cmd_no_user: Couldn't alloc "
+			"sk_buff with length %d\n", length);
+		return;
+	}
+
+	skb_reserve(skb, HCI_H4_SIZE);
+	memcpy(skb_put(skb, length), data, length);
+	skb_push(skb, HCI_H4_SIZE);
+	skb->data[0] = HCI_BT_CMD_H4_CHANNEL;
+
+	cg2900_tx_no_user(dev, skb);
+}
+EXPORT_SYMBOL_GPL(cg2900_send_bt_cmd_no_user);
+
+/**
+ * create_work_item() - Create work item and add it to the work queue.
+ * @wq:		Work queue.
+ * @work_func:	Work function.
+ * @user_data:	Arbitrary data set by user.
+ *
+ * The create_work_item() function creates work item and add it to
+ * the work queue.
+ * Note that work is allocated by kmalloc and work must be freed when work
+ * function is started.
+ */
+void cg2900_create_work_item(struct workqueue_struct *wq, work_func_t work_func,
+			     void *user_data)
+{
+	struct cg2900_work *new_work;
+	int err;
+
+	new_work = kmalloc(sizeof(*new_work), GFP_ATOMIC);
+	if (!new_work) {
+		pr_err("Failed to alloc memory for new_work");
+		return;
+	}
+
+	INIT_WORK(&new_work->work, work_func);
+	new_work->user_data = user_data;
+
+	err = queue_work(wq, &new_work->work);
+	if (!err) {
+		pr_err("Failed to queue work_struct because it's already "
+		       "in the queue");
+		kfree(new_work);
+	}
+}
+EXPORT_SYMBOL_GPL(cg2900_create_work_item);
+
+/**
+ * read_and_send_file_part() - Transmit a part of the supplied file.
+ * @user:	User data for current channel.
+ * @logger:	User data for logger channel.
+ * @info:	File information.
+ *
+ * The cg2900_read_and_send_file_part() function transmit a part of the supplied
+ * file to the controller.
+ *
+ * Returns:
+ *   0 if there is no more data in the file.
+ *   >0 for number of bytes sent.
+ *   -ENOMEM if skb allocation failed.
+ */
+int cg2900_read_and_send_file_part(struct cg2900_user_data *user,
+				   struct cg2900_user_data *logger,
+				   struct cg2900_file_info *info)
+{
+	int bytes_to_copy;
+	struct sk_buff *skb;
+	struct bt_vs_write_file_block_cmd *cmd;
+	int plen;
+
+	/*
+	 * Calculate number of bytes to copy;
+	 * either max bytes for HCI packet or number of bytes left in file
+	 */
+	bytes_to_copy = min((int)HCI_BT_SEND_FILE_MAX_CHUNK_SIZE,
+			    (int)(info->fw_file->size - info->file_offset));
+
+	if (bytes_to_copy <= 0) {
+		/* Nothing more to read in file. */
+		dev_dbg(user->dev, "File download finished\n");
+		info->chunk_id = 0;
+		info->file_offset = 0;
+		return 0;
+	}
+
+	/* There is more data to send */
+	plen = sizeof(*cmd) + bytes_to_copy;
+	skb = user->alloc_skb(plen, GFP_KERNEL);
+	if (!skb) {
+		dev_err(user->dev, "Couldn't allocate sk_buffer\n");
+		return -ENOMEM;
+	}
+
+	skb_put(skb, plen);
+
+	cmd = (struct bt_vs_write_file_block_cmd *)skb->data;
+	cmd->opcode = cpu_to_le16(CG2900_BT_OP_VS_WRITE_FILE_BLOCK);
+	cmd->plen = BT_PARAM_LEN(plen);
+	cmd->id = info->chunk_id;
+	info->chunk_id++;
+
+	/* Copy the data from offset position */
+	memcpy(cmd->data,
+	       &(info->fw_file->data[info->file_offset]),
+	       bytes_to_copy);
+
+	/* Increase offset with number of bytes copied */
+	info->file_offset += bytes_to_copy;
+
+	skb_push(skb, CG2900_SKB_RESERVE);
+	skb->data[0] = HCI_BT_CMD_H4_CHANNEL;
+
+	cg2900_tx_to_chip(user, logger, skb);
+
+	return bytes_to_copy;
+}
+EXPORT_SYMBOL_GPL(cg2900_read_and_send_file_part);
+
+void cg2900_send_to_hci_logger(struct cg2900_user_data *logger,
+							struct sk_buff *skb,
+							u8 direction)
+{
+	struct sk_buff *skb_log;
+	u8 *p;
+
+	/*
+	 * Alloc a new sk_buff and copy the data into it. Then send it to
+	 * the HCI logger.
+	 */
+	skb_log = alloc_skb(skb->len + LOGGER_HEADER_SIZE, GFP_NOWAIT);
+	if (!skb_log) {
+		pr_err("cg2900_send_to_hci_logger:\
+				 Couldn't allocate skb_log\n");
+		return;
+	}
+	/* Reserve 1 byte for direction.*/
+	skb_reserve(skb_log, LOGGER_HEADER_SIZE);
+
+	memcpy(skb_put(skb_log, skb->len), skb->data, skb->len);
+	p = skb_push(skb_log, LOGGER_HEADER_SIZE);
+	*p = (u8) direction;
+
+	if (logger->read_cb)
+		logger->read_cb(logger, skb_log);
+
+	return;
+}
+EXPORT_SYMBOL_GPL(cg2900_send_to_hci_logger);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux CG2900 Library functions");
diff --git a/drivers/staging/cg2900/mfd/cg2900_lib.h b/drivers/staging/cg2900/mfd/cg2900_lib.h
new file mode 100644
index 00000000000..99d5ce6cfdb
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_lib.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+
+#ifndef _CG2900_LIB_H_
+#define _CG2900_LIB_H_
+
+#include <linux/firmware.h>
+#include <linux/skbuff.h>
+#include <linux/workqueue.h>
+
+#include "cg2900.h"
+
+/**
+ * struct cg2900_work - Generic work structure.
+ * @work:	Work structure.
+ * @user_data:	Arbitrary data set by user.
+ */
+struct cg2900_work {
+	struct work_struct work;
+	void *user_data;
+};
+
+/**
+ * struct cg2900_file_info - Info structure for file to download.
+ * @fw_file:		Stores firmware file.
+ * @file_offset:	Current read offset in firmware file.
+ * @chunk_id:		Stores current chunk ID of write file
+ *			operations.
+ */
+struct cg2900_file_info {
+	const struct firmware	*fw_file;
+	int			file_offset;
+	u8			chunk_id;
+};
+
+extern void cg2900_tx_to_chip(struct cg2900_user_data *user,
+			      struct cg2900_user_data *logger,
+			      struct sk_buff *skb);
+extern void cg2900_tx_no_user(struct cg2900_chip_dev *dev, struct sk_buff *skb);
+extern void cg2900_send_bt_cmd(struct cg2900_user_data *user,
+			       struct cg2900_user_data *logger,
+			       void *data, int length);
+extern void cg2900_send_bt_cmd_no_user(struct cg2900_chip_dev *dev, void *data,
+				       int length);
+extern void cg2900_create_work_item(struct workqueue_struct *wq,
+				    work_func_t work_func,
+				    void *user_data);
+extern int cg2900_read_and_send_file_part(struct cg2900_user_data *user,
+					  struct cg2900_user_data *logger,
+					  struct cg2900_file_info *info);
+extern void cg2900_send_to_hci_logger(struct cg2900_user_data *logger,
+							struct sk_buff *skb,
+							u8 direction);
+
+#endif /* _CG2900_LIB_H_ */
diff --git a/drivers/staging/cg2900/mfd/cg2900_test.c b/drivers/staging/cg2900/mfd/cg2900_test.c
new file mode 100644
index 00000000000..58ac6166af6
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_test.c
@@ -0,0 +1,402 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Driver for ST-Ericsson CG2900 test character device.
+ */
+#define NAME					"cg2900_test"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <linux/firmware.h>
+#include <linux/fs.h>
+#include <linux/gfp.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/wait.h>
+
+#include "cg2900.h"
+#include "cg2900_core.h"
+
+#define MISC_DEV			(info->misc_dev.this_device)
+
+/* Device names */
+#define CG2900_CDEV_NAME		"cg2900_core_test"
+
+/**
+ * struct test_info - Main info structure for CG2900 test char device.
+ * @misc_dev:	Registered Misc Device.
+ * @rx_queue:	RX data queue.
+ * @dev:	Device structure for STE Connectivity driver.
+ * @pdev:	Platform device structure for STE Connectivity driver.
+ */
+struct test_info {
+	struct miscdevice	misc_dev;
+	struct sk_buff_head	rx_queue;
+	struct device		*dev;
+	struct platform_device	*pdev;
+};
+
+static struct test_info *test_info;
+
+/*
+ * main_wait_queue - Char device Wait Queue in CG2900 Core.
+ */
+static DECLARE_WAIT_QUEUE_HEAD(char_wait_queue);
+
+/**
+ * tx_to_char_dev() - Handle data received from CG2900 Core.
+ * @dev:	Current chip device information.
+ * @skb:	Buffer with data coming form device.
+ */
+static int tx_to_char_dev(struct cg2900_chip_dev *dev, struct sk_buff *skb)
+{
+	struct test_info *info = dev->t_data;
+	skb_queue_tail(&info->rx_queue, skb);
+	wake_up_interruptible_all(&char_wait_queue);
+	return 0;
+}
+
+/**
+ * cg2900_test_open() - User space char device has been opened.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EACCES if transport already exists.
+ *   -ENOMEM if allocation fails.
+ *   Errors from create_work_item.
+ */
+static int cg2900_test_open(struct inode *inode, struct file *filp)
+{
+	struct test_info *info = test_info;
+	struct cg2900_chip_dev *dev;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(MISC_DEV, "Cannot allocate test_dev\n");
+		return -ENOMEM;
+	}
+	dev->dev = info->dev;
+	dev->pdev = info->pdev;
+	dev->t_data = info;
+	dev->t_cb.write = tx_to_char_dev;
+	filp->private_data = dev;
+
+	dev_info(MISC_DEV, "CG2900 test char dev opened\n");
+	return cg2900_register_trans_driver(dev);
+}
+
+/**
+ * cg2900_test_release() - User space char device has been closed.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * Returns:
+ *   0 if there is no error.
+ */
+static int cg2900_test_release(struct inode *inode, struct file *filp)
+{
+	struct cg2900_chip_dev *dev = filp->private_data;
+	struct test_info *info = dev->t_data;
+
+	dev_info(MISC_DEV, "CG2900 test char dev closed\n");
+	skb_queue_purge(&info->rx_queue);
+	cg2900_deregister_trans_driver(dev);
+	kfree(dev);
+
+	return 0;
+}
+
+/**
+ * cg2900_test_read() - Queue and copy buffer to user space char device.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Received buffer.
+ * @count:	Count of received data in bytes.
+ * @f_pos:	Position in buffer.
+ *
+ * Returns:
+ *   >= 0 is number of bytes read.
+ *   -EFAULT if copy_to_user fails.
+ */
+static ssize_t cg2900_test_read(struct file *filp, char __user *buf,
+				size_t count, loff_t *f_pos)
+{
+	struct sk_buff *skb;
+	int bytes_to_copy;
+	int err;
+	struct cg2900_chip_dev *dev = filp->private_data;
+	struct test_info *info = dev->t_data;
+	struct sk_buff_head *rx_queue = &info->rx_queue;
+
+	dev_dbg(MISC_DEV, "cg2900_test_read count %d\n", count);
+
+	if (skb_queue_empty(rx_queue))
+		wait_event_interruptible(char_wait_queue,
+					 !(skb_queue_empty(rx_queue)));
+
+	skb = skb_dequeue(rx_queue);
+	if (!skb) {
+		dev_dbg(MISC_DEV,
+			"skb queue is empty - return with zero bytes\n");
+		bytes_to_copy = 0;
+		goto finished;
+	}
+
+	bytes_to_copy = min(count, skb->len);
+	err = copy_to_user(buf, skb->data, bytes_to_copy);
+	if (err) {
+		skb_queue_head(rx_queue, skb);
+		return -EFAULT;
+	}
+
+	skb_pull(skb, bytes_to_copy);
+
+	if (skb->len > 0)
+		skb_queue_head(rx_queue, skb);
+	else
+		kfree_skb(skb);
+
+finished:
+	return bytes_to_copy;
+}
+
+/**
+ * cg2900_test_write() - Copy buffer from user and write to CG2900 Core.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Read buffer.
+ * @count:	Size of the buffer write.
+ * @f_pos:	Position in buffer.
+ *
+ * Returns:
+ *   >= 0 is number of bytes written.
+ *   -EFAULT if copy_from_user fails.
+ */
+static ssize_t cg2900_test_write(struct file *filp, const char __user *buf,
+				 size_t count, loff_t *f_pos)
+{
+	struct sk_buff *skb;
+	struct cg2900_chip_dev *dev = filp->private_data;
+	struct test_info *info = dev->t_data;
+
+	dev_dbg(MISC_DEV, "cg2900_test_write count %d\n", count);
+
+	/* Allocate the SKB and reserve space for the header */
+	skb = alloc_skb(count + RX_SKB_RESERVE, GFP_KERNEL);
+	if (!skb) {
+		dev_err(MISC_DEV, "cg2900_test_write: Failed to alloc skb\n");
+		return -ENOMEM;
+	}
+	skb_reserve(skb, RX_SKB_RESERVE);
+
+	if (copy_from_user(skb_put(skb, count), buf, count)) {
+		kfree_skb(skb);
+		return -EFAULT;
+	}
+
+	dev->c_cb.data_from_chip(dev, skb);
+
+	return count;
+}
+
+/**
+ * cg2900_test_poll() - Handle POLL call to the interface.
+ * @filp:	Pointer to the file struct.
+ * @wait:	Poll table supplied to caller.
+ *
+ * Returns:
+ *   Mask of current set POLL values (0 or (POLLIN | POLLRDNORM))
+ */
+static unsigned int cg2900_test_poll(struct file *filp, poll_table *wait)
+{
+	struct cg2900_chip_dev *dev = filp->private_data;
+	struct test_info *info = dev->t_data;
+	unsigned int mask = 0;
+
+	poll_wait(filp, &char_wait_queue, wait);
+
+	if (!(skb_queue_empty(&info->rx_queue)))
+		mask |= POLLIN | POLLRDNORM;
+
+	return mask;
+}
+
+static const struct file_operations test_char_dev_fops = {
+	.open = cg2900_test_open,
+	.release = cg2900_test_release,
+	.read = cg2900_test_read,
+	.write = cg2900_test_write,
+	.poll = cg2900_test_poll
+};
+
+/**
+ * test_char_dev_create() - Create a char device for testing.
+ * @info:	Test device info.
+ *
+ * Creates a separate char device that will interact directly with userspace
+ * test application.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from misc_register.
+ */
+static int test_char_dev_create(struct test_info *info)
+{
+	int err;
+
+	/* Initialize the RX queue */
+	skb_queue_head_init(&info->rx_queue);
+
+	/* Prepare miscdevice struct before registering the device */
+	info->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	info->misc_dev.name = CG2900_CDEV_NAME;
+	info->misc_dev.fops = &test_char_dev_fops;
+	info->misc_dev.parent = info->dev;
+	info->misc_dev.mode = S_IRUGO | S_IWUGO;
+
+	err = misc_register(&info->misc_dev);
+	if (err) {
+		dev_err(info->dev, "Error %d registering misc dev", err);
+		return err;
+	}
+
+	return 0;
+}
+
+/**
+ * test_char_dev_destroy() - Clean up after test_char_dev_create().
+ * @info:	Test device info.
+ */
+static void test_char_dev_destroy(struct test_info *info)
+{
+	int err;
+
+	err = misc_deregister(&info->misc_dev);
+	if (err)
+		dev_err(info->dev, "Error %d deregistering misc dev\n", err);
+
+	/* Clean the message queue */
+	skb_queue_purge(&info->rx_queue);
+}
+
+/**
+ * cg2900_test_probe() - Initialize module.
+ *
+ * @pdev:	Platform device.
+ *
+ * This function initializes and registers the test misc char device.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM for failed alloc or structure creation.
+ *   -EEXIST if device already exists.
+ *   Error codes generated by test_char_dev_create.
+ */
+static int __devinit cg2900_test_probe(struct platform_device *pdev)
+{
+	int err;
+
+	dev_dbg(&pdev->dev, "cg2900_test_probe\n");
+
+	if (test_info) {
+		dev_err(&pdev->dev, "test_info exists\n");
+		return -EEXIST;
+	}
+
+	test_info = kzalloc(sizeof(*test_info), GFP_KERNEL);
+	if (!test_info) {
+		dev_err(&pdev->dev, "Couldn't allocate test_info\n");
+		return -ENOMEM;
+	}
+
+	test_info->dev = &pdev->dev;
+	test_info->pdev = pdev;
+
+	/* Create and add test char device. */
+	err = test_char_dev_create(test_info);
+	if (err) {
+		kfree(test_info);
+		test_info = NULL;
+		return err;
+	}
+
+	dev_set_drvdata(&pdev->dev, test_info);
+
+	dev_info(&pdev->dev, "CG2900 test char device driver started\n");
+
+	return 0;
+}
+
+/**
+ * cg2900_test_remove() - Remove module.
+ *
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM if core_info does not exist.
+ *   -EINVAL if platform data does not exist in the device.
+ */
+static int __devexit cg2900_test_remove(struct platform_device *pdev)
+{
+	struct test_info *test_info;
+
+	dev_dbg(&pdev->dev, "cg2900_test_remove\n");
+	test_info = dev_get_drvdata(&pdev->dev);
+	test_char_dev_destroy(test_info);
+	dev_set_drvdata(&pdev->dev, NULL);
+	kfree(test_info);
+	test_info = NULL;
+	dev_info(&pdev->dev, "CG2900 Test char device driver removed\n");
+	return 0;
+}
+
+static struct platform_driver cg2900_test_driver = {
+	.driver = {
+		.name	= "cg2900-test",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_test_probe,
+	.remove	= __devexit_p(cg2900_test_remove),
+};
+
+/**
+ * cg2900_test_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_test_init(void)
+{
+	pr_debug("cg2900_test_init");
+	return platform_driver_register(&cg2900_test_driver);
+}
+
+/**
+ * cg2900_test_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_test_exit(void)
+{
+	pr_debug("cg2900_test_exit");
+	platform_driver_unregister(&cg2900_test_driver);
+}
+
+module_init(cg2900_test_init);
+module_exit(cg2900_test_exit);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux CG2900 Test Char Device Driver");
diff --git a/drivers/staging/cg2900/mfd/stlc2690_chip.c b/drivers/staging/cg2900/mfd/stlc2690_chip.c
new file mode 100644
index 00000000000..3e394670f1f
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/stlc2690_chip.c
@@ -0,0 +1,1643 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson STLC2690 BT/FM controller.
+ */
+#define NAME					"stlc2690_chip"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/gfp.h>
+#include <linux/init.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/limits.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/stat.h>
+#include <linux/time.h>
+#include <linux/timer.h>
+#include <linux/types.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/core.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+
+#include "cg2900.h"
+#include "cg2900_core.h"
+#include "cg2900_lib.h"
+#include "stlc2690_chip.h"
+
+#define MAIN_DEV				(main_info->dev)
+#define BOOT_DEV				(info->user_in_charge->dev)
+
+#define WQ_NAME					"stlc2690_chip_wq"
+
+#define LINE_TOGGLE_DETECT_TIMEOUT		50	/* ms */
+#define CHIP_READY_TIMEOUT			100	/* ms */
+#define CHIP_STARTUP_TIMEOUT			15000	/* ms */
+#define CHIP_SHUTDOWN_TIMEOUT			15000	/* ms */
+
+/** CHANNEL_BT_CMD - Bluetooth HCI H:4 channel
+ * for Bluetooth commands in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_CMD				0x01
+
+/** CHANNEL_BT_ACL - Bluetooth HCI H:4 channel
+ * for Bluetooth ACL data in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_ACL				0x02
+
+/** CHANNEL_BT_EVT - Bluetooth HCI H:4 channel
+ * for Bluetooth events in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_EVT				0x04
+
+/** CHANNEL_HCI_LOGGER - Bluetooth HCI H:4 channel
+ * for logging all transmitted H4 packets (on all channels).
+ */
+#define CHANNEL_HCI_LOGGER			0xFA
+
+/** CHANNEL_CORE - Bluetooth HCI H:4 channel
+ * for user space control of the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_CORE				0xFD
+
+/*
+ * For the char dev names we keep the same names in order to be able to reuse
+ * the users and to keep a consistent interface.
+ */
+
+/** STLC2690_BT_CMD - Bluetooth HCI H4 channel for Bluetooth commands.
+ */
+#define STLC2690_BT_CMD				"cg2900_bt_cmd"
+
+/** STLC2690_BT_ACL - Bluetooth HCI H4 channel for Bluetooth ACL data.
+ */
+#define STLC2690_BT_ACL				"cg2900_bt_acl"
+
+/** STLC2690_BT_EVT - Bluetooth HCI H4 channel for Bluetooth events.
+ */
+#define STLC2690_BT_EVT				"cg2900_bt_evt"
+
+/** STLC2690_HCI_LOGGER - BT channel for logging all transmitted H4 packets.
+ * Data read is copy of all data transferred on the other channels.
+ * Only write allowed is configuration of the HCI Logger.
+ */
+#define STLC2690_HCI_LOGGER			"cg2900_hci_logger"
+
+/** STLC2690_CORE- Channel for keeping ST-Ericsson STLC2690 enabled.
+ * Opening this channel forces the chip to stay powered.
+ * No data can be written to or read from this channel.
+ */
+#define STLC2690_CORE				"cg2900_core"
+
+/**
+ * enum main_state - Main-state for STLC2690 driver.
+ * @STLC2690_INIT:	STLC2690 initializing.
+ * @STLC2690_IDLE:	No user registered to STLC2690 driver.
+ * @STLC2690_BOOTING:	STLC2690 booting after first user is registered.
+ * @STLC2690_CLOSING:	STLC2690 closing after last user has deregistered.
+ * @STLC2690_RESETING:	STLC2690 reset requested.
+ * @STLC2690_ACTIVE:	STLC2690 up and running with at least one user.
+ */
+enum main_state {
+	STLC2690_INIT,
+	STLC2690_IDLE,
+	STLC2690_BOOTING,
+	STLC2690_CLOSING,
+	STLC2690_RESETING,
+	STLC2690_ACTIVE
+};
+
+/**
+ * enum boot_state - BOOT-state for STLC2690 chip driver.
+ * @BOOT_RESET:				HCI Reset has been sent.
+ * @BOOT_SEND_BD_ADDRESS:		VS Store In FS command with BD address
+ *					has been sent.
+ * @BOOT_GET_FILES_TO_LOAD:		STLC2690 chip driver is retrieving file
+ *					to load.
+ * @BOOT_DOWNLOAD_PATCH:		STLC2690 chip driver is downloading
+ *					patches.
+ * @BOOT_ACTIVATE_PATCHES_AND_SETTINGS:	STLC2690 chip driver is activating
+ *					patches and settings.
+ * @BOOT_READY:				STLC2690 chip driver boot is ready.
+ * @BOOT_FAILED:			STLC2690 chip driver boot failed.
+ */
+enum boot_state {
+	BOOT_RESET,
+	BOOT_SEND_BD_ADDRESS,
+	BOOT_GET_FILES_TO_LOAD,
+	BOOT_DOWNLOAD_PATCH,
+	BOOT_ACTIVATE_PATCHES_AND_SETTINGS,
+	BOOT_READY,
+	BOOT_FAILED
+};
+
+/**
+ * enum file_load_state - BOOT_FILE_LOAD-state for STLC2690 chip driver.
+ * @FILE_LOAD_GET_PATCH:		Loading patches.
+ * @FILE_LOAD_GET_STATIC_SETTINGS:	Loading static settings.
+ * @FILE_LOAD_NO_MORE_FILES:		No more files to load.
+ * @FILE_LOAD_FAILED:			File loading failed.
+ */
+enum file_load_state {
+	FILE_LOAD_GET_PATCH,
+	FILE_LOAD_GET_STATIC_SETTINGS,
+	FILE_LOAD_NO_MORE_FILES,
+	FILE_LOAD_FAILED
+};
+
+/**
+ * enum download_state - BOOT_DOWNLOAD state.
+ * @DOWNLOAD_PENDING:	Download in progress.
+ * @DOWNLOAD_SUCCESS:	Download successfully finished.
+ * @DOWNLOAD_FAILED:	Downloading failed.
+ */
+enum download_state {
+	DOWNLOAD_PENDING,
+	DOWNLOAD_SUCCESS,
+	DOWNLOAD_FAILED
+};
+
+
+/**
+ * struct stlc2690_channel_item - List object for channel.
+ * @list:	list_head struct.
+ * @user:	User for this channel.
+ */
+struct stlc2690_channel_item {
+	struct list_head	list;
+	struct cg2900_user_data	*user;
+};
+
+/**
+ * struct stlc2690_skb_data - Structure for storing private data in an sk_buffer.
+ * @dev:	STLC2690 device for this sk_buffer.
+ */
+struct stlc2690_skb_data {
+	struct cg2900_user_data *user;
+};
+#define stlc2690_skb_data(__skb) ((struct stlc2690_skb_data *)((__skb)->cb))
+
+/**
+ * struct stlc2690_chip_info - Main info structure for STLC2690 chip driver.
+ * @patch_file_name:		Stores patch file name.
+ * @settings_file_name:		Stores settings file name.
+ * @file_info:			Firmware file info (patch or settings).
+ * @main_state:			Current MAIN-state of STLC2690 chip driver.
+ * @boot_state:			Current BOOT-state of STLC2690 chip driver.
+ * @file_load_state:		Current BOOT_FILE_LOAD-state of STLC2690 chip
+ *				driver.
+ * @download_state:		Current BOOT_DOWNLOAD-state of STLC2690 chip
+ *				driver.
+ * @wq:				STLC2690 chip driver workqueue.
+ * @chip_dev:			Chip handler info.
+ * @user_in_charge:		User currently operating. Normally used at
+ *				channel open and close.
+ * @last_user:			Last user of this chip.
+ * @logger:			Logger user of this chip.
+ */
+struct stlc2690_chip_info {
+	char				*patch_file_name;
+	char				*settings_file_name;
+	struct cg2900_file_info		file_info;
+	enum main_state			main_state;
+	enum boot_state			boot_state;
+	enum file_load_state		file_load_state;
+	enum download_state		download_state;
+	struct workqueue_struct		*wq;
+	struct cg2900_chip_dev		*chip_dev;
+	spinlock_t			rw_lock;
+	struct list_head		open_channels;
+	struct cg2900_user_data		*user_in_charge;
+	struct cg2900_user_data		*last_user;
+	struct cg2900_user_data		*logger;
+};
+
+/**
+ * struct main_info - Main info structure for STLC2690 chip driver.
+ * @dev:			Device structure.
+ * @cell_base_id:		Base ID for MFD cells.
+ * @man_mutex:			Management mutex.
+ */
+struct main_info {
+	struct device			*dev;
+	int				cell_base_id;
+	struct mutex			man_mutex;
+};
+
+static struct main_info *main_info;
+
+/*
+ * main_wait_queue - Main Wait Queue in STLC2690 driver.
+ */
+static DECLARE_WAIT_QUEUE_HEAD(main_wait_queue);
+
+static void chip_startup_finished(struct stlc2690_chip_info *info, int err);
+
+/**
+ * send_bd_address() - Send HCI VS command with BD address to the chip.
+ */
+static void send_bd_address(struct stlc2690_chip_info *info)
+{
+	struct bt_vs_store_in_fs_cmd *cmd;
+	u8 plen = sizeof(*cmd) + BT_BDADDR_SIZE;
+
+	cmd = kmalloc(plen, GFP_KERNEL);
+	if (!cmd)
+		return;
+
+	cmd->opcode = cpu_to_le16(STLC2690_BT_OP_VS_STORE_IN_FS);
+	cmd->plen = BT_PARAM_LEN(plen);
+	cmd->user_id = STLC2690_VS_STORE_IN_FS_USR_ID_BD_ADDR;
+	cmd->len = BT_BDADDR_SIZE;
+	/* Now copy the BD address received from user space control app. */
+	memcpy(cmd->data, bd_address, BT_BDADDR_SIZE);
+
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_SEND_BD_ADDRESS\n");
+	info->boot_state = BOOT_SEND_BD_ADDRESS;
+
+	cg2900_send_bt_cmd(info->user_in_charge, info->logger, cmd, plen);
+
+	kfree(cmd);
+}
+
+/**
+ * send_settings_file() - Transmit settings file.
+ *
+ * The send_settings_file() function transmit settings file.
+ * The file is read in parts to fit in HCI packets. When finished,
+ * close the settings file and send HCI reset to activate settings and patches.
+ */
+static void send_settings_file(struct stlc2690_chip_info *info)
+{
+	int bytes_sent;
+
+	bytes_sent = cg2900_read_and_send_file_part(info->user_in_charge,
+						    info->logger,
+						    &info->file_info);
+	if (bytes_sent > 0) {
+		/* Data sent. Wait for CmdComplete */
+		return;
+	} else if (bytes_sent < 0) {
+		dev_err(BOOT_DEV, "send_settings_file: Error %d occurred\n",
+			bytes_sent);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		chip_startup_finished(info, bytes_sent);
+		return;
+	}
+
+	/* No data was sent. This file is finished */
+	info->download_state = DOWNLOAD_SUCCESS;
+
+	/* Settings file finished. Release used resources */
+	dev_dbg(BOOT_DEV, "Settings file finished, release used resources\n");
+	release_firmware(info->file_info.fw_file);
+	info->file_info.fw_file = NULL;
+
+	dev_dbg(BOOT_DEV, "New file_load_state: FILE_LOAD_NO_MORE_FILES\n");
+	info->file_load_state = FILE_LOAD_NO_MORE_FILES;
+
+	/* Create and send HCI VS Store In FS command with bd address. */
+	send_bd_address(info);
+}
+
+/**
+ * send_patch_file - Transmit patch file.
+ *
+ * The send_patch_file() function transmit patch file.
+ * The file is read in parts to fit in HCI packets. When the complete file is
+ * transmitted, the file is closed.
+ * When finished, continue with settings file.
+ */
+static void send_patch_file(struct cg2900_chip_dev *dev)
+{
+	int err;
+	int bytes_sent;
+	struct stlc2690_chip_info *info = dev->c_data;
+	int file_name_size = strlen("STLC2690_XXXX_XXXX_settings.fw");
+
+	bytes_sent = cg2900_read_and_send_file_part(info->user_in_charge,
+						    info->logger,
+						    &info->file_info);
+	if (bytes_sent > 0) {
+		/* Data sent. Wait for CmdComplete */
+		return;
+	} else if (bytes_sent < 0) {
+		dev_err(BOOT_DEV, "send_patch_file: Error %d occurred\n",
+			bytes_sent);
+		err = bytes_sent;
+		goto error_handling;
+	}
+
+	/* No data was sent. This file is finished */
+	info->download_state = DOWNLOAD_SUCCESS;
+
+	dev_dbg(BOOT_DEV, "Patch file finished, release used resources\n");
+	release_firmware(info->file_info.fw_file);
+	info->file_info.fw_file = NULL;
+
+	/*
+	 * Create the settings file name from HCI revision and sub_version.
+	 * file_name_size does not include terminating NULL character
+	 * so add 1.
+	 */
+	err = snprintf(info->settings_file_name, file_name_size + 1,
+			"STLC2690_%04X_%04X_settings.fw",
+			dev->chip.hci_revision, dev->chip.hci_sub_version);
+	if (err == file_name_size) {
+		dev_dbg(BOOT_DEV, "Downloading settings file %s\n",
+				info->settings_file_name);
+	} else {
+		dev_err(BOOT_DEV, "Settings file name failed! err=%d\n", err);
+		goto error_handling;
+	}
+
+	/* Retrieve the settings file */
+	err = request_firmware(&info->file_info.fw_file,
+			       info->settings_file_name,
+			       info->chip_dev->dev);
+	if (err) {
+		dev_err(BOOT_DEV, "Couldn't get settings file (%d)\n", err);
+		goto error_handling;
+	}
+	/* Now send the settings file */
+	dev_dbg(BOOT_DEV,
+		"New file_load_state: FILE_LOAD_GET_STATIC_SETTINGS\n");
+	info->file_load_state = FILE_LOAD_GET_STATIC_SETTINGS;
+	dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_PENDING\n");
+	info->download_state = DOWNLOAD_PENDING;
+	send_settings_file(info);
+	return;
+
+error_handling:
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+	info->boot_state = BOOT_FAILED;
+	chip_startup_finished(info, err);
+}
+
+/**
+ * work_reset_after_error() - Handle reset.
+ * @work:	Reference to work data.
+ *
+ * Handle a reset after received Command Complete event.
+ */
+static void work_reset_after_error(struct work_struct *work)
+{
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_reset_after_error: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	chip_startup_finished(info, -EIO);
+
+	kfree(my_work);
+}
+
+/**
+ * work_load_patch_and_settings() - Start loading patches and settings.
+ * @work:	Reference to work data.
+ */
+static void work_load_patch_and_settings(struct work_struct *work)
+{
+	int err = 0;
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+	int file_name_size = strlen("STLC2690_XXXX_XXXX_patch.fw");
+
+	if (!work) {
+		dev_err(MAIN_DEV,
+			"work_load_patch_and_settings: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	/* Check that we are in the right state */
+	if (info->boot_state != BOOT_GET_FILES_TO_LOAD)
+		goto finished;
+
+	/*
+	 * Create the patch file name from HCI revision and sub_version.
+	 * file_name_size does not include terminating NULL character
+	 * so add 1.
+	 */
+	err = snprintf(info->patch_file_name, file_name_size + 1,
+			"STLC2690_%04X_%04X_patch.fw", dev->chip.hci_revision,
+			dev->chip.hci_sub_version);
+	if (err == file_name_size) {
+		dev_dbg(BOOT_DEV, "Downloading patch file %s\n",
+				info->patch_file_name);
+	} else {
+		dev_err(BOOT_DEV, "Patch file name failed! err=%d\n", err);
+		goto error_handling;
+	}
+
+	/* We now all info needed */
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_DOWNLOAD_PATCH\n");
+	info->boot_state = BOOT_DOWNLOAD_PATCH;
+	dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_PENDING\n");
+	info->download_state = DOWNLOAD_PENDING;
+	dev_dbg(BOOT_DEV, "New file_load_state: FILE_LOAD_GET_PATCH\n");
+	info->file_load_state = FILE_LOAD_GET_PATCH;
+	info->file_info.chunk_id = 0;
+	info->file_info.file_offset = 0;
+	info->file_info.fw_file = NULL;
+
+	/* OK. Now it is time to download the patches */
+	err = request_firmware(&(info->file_info.fw_file),
+			       info->patch_file_name,
+			       dev->dev);
+	if (err < 0) {
+		dev_err(BOOT_DEV, "Couldn't get patch file (%d)\n", err);
+		goto error_handling;
+	}
+	send_patch_file(dev);
+
+	goto finished;
+
+error_handling:
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+	info->boot_state = BOOT_FAILED;
+	chip_startup_finished(info, -EIO);
+finished:
+	kfree(my_work);
+}
+
+/**
+ * work_cont_file_download() - A file block has been written.
+ * @work:	Reference to work data.
+ *
+ * Handle a received HCI VS Write File Block Complete event.
+ * Normally this means continue to send files to the controller.
+ */
+static void work_cont_file_download(struct work_struct *work)
+{
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_cont_file_download: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	/* Continue to send patches or settings to the controller */
+	if (info->file_load_state == FILE_LOAD_GET_PATCH)
+		send_patch_file(dev);
+	else if (info->file_load_state == FILE_LOAD_GET_STATIC_SETTINGS)
+		send_settings_file(info);
+	else
+		dev_dbg(BOOT_DEV, "No more files to load\n");
+
+	kfree(my_work);
+}
+
+/**
+ * handle_reset_cmd_complete() - Handles HCI Reset Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_reset_cmd_complete(struct cg2900_chip_dev *dev, u8 *data)
+{
+	u8 status = data[0];
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	dev_dbg(BOOT_DEV, "Received Reset complete event with status 0x%X\n",
+		status);
+
+	if (BOOT_RESET != info->boot_state &&
+	    BOOT_ACTIVATE_PATCHES_AND_SETTINGS != info->boot_state)
+		return false;
+
+	if (HCI_BT_ERROR_NO_ERROR != status) {
+		dev_err(BOOT_DEV, "Command complete for HciReset received with "
+			"error 0x%X\n", status);
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+		return true;
+	}
+
+	if (BOOT_RESET == info->boot_state) {
+		info->boot_state = BOOT_GET_FILES_TO_LOAD;
+		cg2900_create_work_item(info->wq, work_load_patch_and_settings,
+					dev);
+	} else {
+		/*
+		 * The boot sequence is now finished successfully.
+		 * Set states and signal to waiting thread.
+		 */
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_READY\n");
+		info->boot_state = BOOT_READY;
+		chip_startup_finished(info, 0);
+	}
+
+	return true;
+}
+
+
+/**
+ * handle_vs_store_in_fs_cmd_complete() - Handles HCI VS StoreInFS Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_store_in_fs_cmd_complete(struct cg2900_chip_dev *dev,
+					       u8 *data)
+{
+	u8 status = data[0];
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	dev_dbg(BOOT_DEV,
+		"Received Store_in_FS complete event with status 0x%X\n",
+		status);
+
+	if (info->boot_state != BOOT_SEND_BD_ADDRESS)
+		return false;
+
+	if (HCI_BT_ERROR_NO_ERROR == status) {
+		struct hci_command_hdr cmd;
+
+		/* Send HCI Reset command to activate patches */
+		dev_dbg(BOOT_DEV,
+			"New boot_state: BOOT_ACTIVATE_PATCHES_AND_SETTINGS\n");
+		info->boot_state = BOOT_ACTIVATE_PATCHES_AND_SETTINGS;
+
+		cmd.opcode = cpu_to_le16(HCI_OP_RESET);
+		cmd.plen = 0; /* No parameters for Reset */
+		cg2900_send_bt_cmd(info->user_in_charge, info->logger, &cmd,
+				   sizeof(cmd));
+	} else {
+		dev_err(BOOT_DEV,
+			"Command complete for StoreInFS received with error "
+			"0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_write_file_block_cmd_complete() - Handles HCI VS WriteFileBlock Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_write_file_block_cmd_complete(struct cg2900_chip_dev *dev,
+						    u8 *data)
+{
+	u8 status = data[0];
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DOWNLOAD_PATCH ||
+	    info->download_state != DOWNLOAD_PENDING)
+		return false;
+
+	if (HCI_BT_ERROR_NO_ERROR == status)
+		cg2900_create_work_item(info->wq, work_cont_file_download, dev);
+	else {
+		dev_err(BOOT_DEV,
+			"Command complete for WriteFileBlock received with"
+			" error 0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_FAILED\n");
+		info->download_state = DOWNLOAD_FAILED;
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		if (info->file_info.fw_file) {
+			release_firmware(info->file_info.fw_file);
+			info->file_info.fw_file = NULL;
+		}
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_write_file_block_cmd_status() - Handles HCI VS WriteFileBlock Command Status event.
+ * @status:	Returned status of WriteFileBlock command.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_write_file_block_cmd_status(struct cg2900_chip_dev *dev,
+						  u8 status)
+{
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DOWNLOAD_PATCH ||
+	    info->download_state != DOWNLOAD_PENDING)
+		return false;
+
+	/*
+	 * Only do something if there is an error. Otherwise we will wait for
+	 * CmdComplete.
+	 */
+	if (HCI_BT_ERROR_NO_ERROR != status) {
+		dev_err(BOOT_DEV,
+			"Command status for WriteFileBlock received with"
+			" error 0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_FAILED\n");
+		info->download_state = DOWNLOAD_FAILED;
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		if (info->file_info.fw_file) {
+			release_firmware(info->file_info.fw_file);
+			info->file_info.fw_file = NULL;
+		}
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_rx_data_bt_evt() - Check if received data should be handled in STLC2690 chip driver.
+ * @skb:	Data packet
+ *
+ * The handle_rx_data_bt_evt() function checks if received data should be
+ * handled in STLC2690 chip driver. If so handle it correctly.
+ * Received data is always HCI BT Event.
+ *
+ * Returns:
+ *   True,  if packet was handled internally,
+ *   False, otherwise.
+ */
+static bool handle_rx_data_bt_evt(struct cg2900_chip_dev *dev,
+				  struct sk_buff *skb)
+{
+	bool pkt_handled = false;
+	/* skb cannot be NULL here so it is safe to de-reference */
+	u8 *data = skb->data;
+	struct hci_event_hdr *evt;
+	u16 op_code;
+
+	evt = (struct hci_event_hdr *)data;
+	data += sizeof(*evt);
+
+	/* First check the event code. */
+	if (HCI_EV_CMD_COMPLETE == evt->evt) {
+		struct hci_ev_cmd_complete *cmd_complete;
+
+		cmd_complete = (struct hci_ev_cmd_complete *)data;
+		op_code = le16_to_cpu(cmd_complete->opcode);
+		dev_dbg(dev->dev,
+			"Received Command Complete: op_code = 0x%04X\n",
+			op_code);
+		/* Move to first byte after OCF */
+		data += sizeof(*cmd_complete);
+
+		if (op_code == HCI_OP_RESET)
+			pkt_handled = handle_reset_cmd_complete(dev, data);
+		else if (op_code == STLC2690_BT_OP_VS_STORE_IN_FS)
+			pkt_handled = handle_vs_store_in_fs_cmd_complete(dev,
+									 data);
+		else if (op_code == STLC2690_BT_OP_VS_WRITE_FILE_BLOCK)
+			pkt_handled =
+				handle_vs_write_file_block_cmd_complete(dev,
+									data);
+	} else if (HCI_EV_CMD_STATUS == evt->evt) {
+		struct hci_ev_cmd_status *cmd_status;
+
+		cmd_status = (struct hci_ev_cmd_status *)data;
+
+		op_code = le16_to_cpu(cmd_status->opcode);
+
+		dev_dbg(dev->dev, "Received Command Status: op_code = 0x%04X\n",
+			op_code);
+
+		if (op_code == STLC2690_BT_OP_VS_WRITE_FILE_BLOCK)
+			pkt_handled = handle_vs_write_file_block_cmd_status
+				(dev, cmd_status->status);
+	} else if (HCI_EV_HW_ERROR == evt->evt) {
+		struct hci_ev_hw_error *hw_error;
+
+		hw_error = (struct hci_ev_hw_error *)data;
+		/*
+		 * Only do a printout. There might be a receiving stack that can
+		 * handle this event
+		 */
+		dev_err(dev->dev, "HW Error event received with error 0x%02X\n",
+			hw_error->hw_code);
+		return false;
+	} else
+		return false;
+
+	if (pkt_handled)
+		kfree_skb(skb);
+
+	return pkt_handled;
+}
+
+/**
+ * data_from_chip() - Called when data is received from the chip.
+ * @dev:	Chip info.
+ * @skb:	Packet received.
+ *
+ * The data_from_chip() function checks if packet is a response for a packet it
+ * itself has transmitted. If not it finds the correct user and sends the packet
+ * to the user.
+ */
+static void data_from_chip(struct cg2900_chip_dev *dev,
+			   struct sk_buff *skb)
+{
+	int h4_channel;
+	struct list_head *cursor;
+	struct stlc2690_channel_item *tmp;
+	struct stlc2690_chip_info *info = dev->c_data;
+	struct cg2900_user_data *user = NULL;
+
+	h4_channel = skb->data[0];
+	skb_pull(skb, HCI_H4_SIZE);
+
+	/* Then check if this is a response to data we have sent */
+	if (h4_channel == CHANNEL_BT_EVT && handle_rx_data_bt_evt(dev, skb))
+		return;
+
+	spin_lock_bh(&info->rw_lock);
+
+	/* Let's see if this packet has the same user as the last one */
+	if (info->last_user && info->last_user->h4_channel == h4_channel) {
+		user = info->last_user;
+		goto user_found;
+	}
+
+	/* Search through the list of all open channels to find the user */
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct stlc2690_channel_item, list);
+		if (tmp->user->h4_channel == h4_channel) {
+			user = tmp->user;
+			goto user_found;
+		}
+	}
+
+user_found:
+	info->last_user = user;
+	spin_unlock_bh(&info->rw_lock);
+
+	if (user)
+		user->read_cb(user, skb);
+	else {
+		dev_err(dev->dev,
+			"Could not find corresponding user to h4_channel %d\n",
+			h4_channel);
+		kfree_skb(skb);
+	}
+}
+
+static void chip_removed(struct cg2900_chip_dev *dev)
+{
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	mfd_remove_devices(dev->dev);
+	kfree(info->settings_file_name);
+	kfree(info->patch_file_name);
+	destroy_workqueue(info->wq);
+	kfree(info);
+	dev->c_data = NULL;
+	dev->c_cb.chip_removed = NULL;
+	dev->c_cb.data_from_chip = NULL;
+}
+
+/**
+ * chip_shutdown() - Reset and power the chip off.
+ */
+static void chip_shutdown(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev = cg2900_get_prv(user);
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	dev_dbg(user->dev, "chip_shutdown\n");
+
+	/* Close the transport, which will power off the chip */
+	if (dev->t_cb.close)
+		dev->t_cb.close(dev);
+
+	/* Chip shut-down finished, set correct state and wake up the chip. */
+	dev_dbg(dev->dev, "New main_state: STLC2690_IDLE\n");
+	info->main_state = STLC2690_IDLE;
+	wake_up_all(&main_wait_queue);
+}
+
+static void chip_startup_finished(struct stlc2690_chip_info *info, int err)
+{
+	dev_dbg(BOOT_DEV, "chip_startup_finished (%d)\n", err);
+
+	if (err)
+		/* Shutdown the chip */
+		chip_shutdown(info->user_in_charge);
+	else {
+		dev_dbg(BOOT_DEV, "New main_state: CORE_ACTIVE\n");
+		info->main_state = STLC2690_ACTIVE;
+	}
+
+	wake_up_all(&main_wait_queue);
+
+	if (err)
+		return;
+
+	if (!info->chip_dev->t_cb.chip_startup_finished)
+		dev_err(BOOT_DEV, "chip_startup_finished callback not found\n");
+	else
+		info->chip_dev->t_cb.chip_startup_finished(info->chip_dev);
+}
+
+static int stlc2690_open(struct cg2900_user_data *user)
+{
+	int err;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+	struct list_head *cursor;
+	struct stlc2690_channel_item *tmp;
+	struct hci_command_hdr cmd;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "stlc2690_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "stlc2690_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	mutex_lock(&main_info->man_mutex);
+
+	/* Add a minor wait in order to avoid CPU blocking, looping openings */
+	err = wait_event_timeout(main_wait_queue,
+				 (STLC2690_IDLE == info->main_state ||
+				  STLC2690_ACTIVE == info->main_state),
+				 msecs_to_jiffies(LINE_TOGGLE_DETECT_TIMEOUT));
+	if (err <= 0) {
+		if (STLC2690_INIT == info->main_state)
+			dev_err(user->dev, "Transport not opened\n");
+		else
+			dev_err(user->dev, "stlc2690_open currently busy "
+				"(0x%X). Try again\n", info->main_state);
+		err = -EBUSY;
+		goto err_free_mutex;
+	}
+
+	err = 0;
+
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct stlc2690_channel_item, list);
+		if (tmp->user->h4_channel == user->h4_channel) {
+			dev_err(user->dev, "Channel %d is already opened\n",
+				user->h4_channel);
+			err = -EACCES;
+			goto err_free_mutex;
+		}
+	}
+
+	tmp = kzalloc(sizeof(*tmp), GFP_KERNEL);
+	if (!tmp) {
+		dev_err(user->dev, "Could not allocate tmp\n");
+		err = -ENOMEM;
+		goto err_free_mutex;
+	}
+	tmp->user = user;
+
+	if (STLC2690_ACTIVE != info->main_state &&
+	    !user->chip_independent) {
+		/* Open transport and start-up the chip */
+		if (dev->t_cb.set_chip_power)
+			dev->t_cb.set_chip_power(dev, true);
+
+		/* Wait to be sure that the chip is ready */
+		schedule_timeout_killable(
+				msecs_to_jiffies(CHIP_READY_TIMEOUT));
+
+		if (dev->t_cb.open)
+			err = dev->t_cb.open(dev);
+		if (err) {
+			if (dev->t_cb.set_chip_power)
+				dev->t_cb.set_chip_power(dev, false);
+			goto err_free_list_item;
+		}
+
+		/* Start the boot sequence */
+		info->user_in_charge = user;
+		info->last_user = user;
+		dev_dbg(user->dev, "New boot_state: BOOT_RESET\n");
+		info->boot_state = BOOT_RESET;
+		dev_dbg(user->dev, "New main_state: STLC2690_BOOTING\n");
+		info->main_state = STLC2690_BOOTING;
+		cmd.opcode = cpu_to_le16(HCI_OP_RESET);
+		cmd.plen = 0; /* No parameters for HCI reset */
+		cg2900_send_bt_cmd(user, info->logger, &cmd, sizeof(cmd));
+
+		dev_dbg(user->dev, "Wait up to 15 seconds for chip to start\n");
+		wait_event_timeout(main_wait_queue,
+				   (STLC2690_ACTIVE == info->main_state ||
+				    STLC2690_IDLE   == info->main_state),
+				   msecs_to_jiffies(CHIP_STARTUP_TIMEOUT));
+		if (STLC2690_ACTIVE != info->main_state) {
+			dev_err(user->dev, "STLC2690 driver failed to start\n");
+
+			if (dev->t_cb.close)
+				dev->t_cb.close(dev);
+
+			dev_dbg(user->dev, "New main_state: CORE_IDLE\n");
+			info->main_state = STLC2690_IDLE;
+			err = -EIO;
+			goto err_free_list_item;
+		}
+	}
+
+	list_add_tail(&tmp->list, &info->open_channels);
+
+	user->opened = true;
+
+	dev_dbg(user->dev, "H:4 channel opened\n");
+
+	mutex_unlock(&main_info->man_mutex);
+	return 0;
+err_free_list_item:
+	kfree(tmp);
+err_free_mutex:
+	mutex_unlock(&main_info->man_mutex);
+	return err;
+}
+
+static int stlc2690_hci_log_open(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+	int err;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"stlc2690_hci_log_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "stlc2690_hci_log_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (info->logger) {
+		dev_err(user->dev, "HCI Logger already stored\n");
+		return -EACCES;
+	}
+
+	info->logger = user;
+	err = stlc2690_open(user);
+	if (err)
+		info->logger = NULL;
+	return err;
+}
+
+static void stlc2690_close(struct cg2900_user_data *user)
+{
+	bool keep_powered = false;
+	struct list_head *cursor, *next;
+	struct stlc2690_channel_item *tmp;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"stlc2690_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "stlc2690_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	mutex_lock(&main_info->man_mutex);
+
+	/*
+	 * Go through each open channel. Remove our channel and check if there
+	 * is any other channel that want to keep the chip running
+	 */
+	list_for_each_safe(cursor, next, &info->open_channels) {
+		tmp = list_entry(cursor, struct stlc2690_channel_item, list);
+		if (tmp->user == user) {
+			list_del(cursor);
+			kfree(tmp);
+		} else if (!tmp->user->chip_independent)
+			keep_powered = true;
+	}
+
+	if (keep_powered)
+		/* This was not the last user, we're done. */
+		goto finished;
+
+	if (STLC2690_IDLE == info->main_state)
+		/* Chip has already been shut down. */
+		goto finished;
+
+	dev_dbg(user->dev, "New main_state: CORE_CLOSING\n");
+	info->main_state = STLC2690_CLOSING;
+	chip_shutdown(user);
+
+	dev_dbg(user->dev, "Wait up to 15 seconds for chip to shut-down\n");
+	wait_event_timeout(main_wait_queue,
+			   STLC2690_IDLE == info->main_state,
+			   msecs_to_jiffies(CHIP_SHUTDOWN_TIMEOUT));
+
+	/* Force shutdown if we timed out */
+	if (STLC2690_IDLE != info->main_state) {
+		dev_err(user->dev,
+			"ST-Ericsson STLC2690 Core Driver was shut-down with "
+			"problems\n");
+
+		if (dev->t_cb.close)
+			dev->t_cb.close(dev);
+
+		dev_dbg(user->dev, "New main_state: CORE_IDLE\n");
+		info->main_state = STLC2690_IDLE;
+	}
+
+finished:
+	mutex_unlock(&main_info->man_mutex);
+	user->opened = false;
+	dev_dbg(user->dev, "H:4 channel closed\n");
+}
+
+static void stlc2690_hci_log_close(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"stlc2690_hci_log_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "stlc2690_hci_log_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	info->logger = NULL;
+	stlc2690_close(user);
+}
+
+static int stlc2690_reset(struct cg2900_user_data *user)
+{
+	struct list_head *cursor, *next;
+	struct stlc2690_channel_item *tmp;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"stlc2690_reset: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	dev_info(user->dev, "stlc2690_reset\n");
+
+	BUG_ON(!main_info);
+
+	mutex_lock(&main_info->man_mutex);
+
+	dev_dbg(user->dev, "New main_state: CORE_RESETING\n");
+	info->main_state = STLC2690_RESETING;
+
+	chip_shutdown(user);
+
+	/*
+	 * Inform all opened channels about the reset and free the user devices
+	 */
+	list_for_each_safe(cursor, next, &info->open_channels) {
+		tmp = list_entry(cursor, struct stlc2690_channel_item, list);
+		list_del(cursor);
+		tmp->user->opened = false;
+		tmp->user->reset_cb(tmp->user);
+		kfree(tmp);
+	}
+
+	/* Reset finished. We are now idle until first channel is opened */
+	dev_dbg(user->dev, "New main_state: STLC2690_IDLE\n");
+	info->main_state = STLC2690_IDLE;
+
+	mutex_unlock(&main_info->man_mutex);
+
+	/*
+	 * Send wake-up since this might have been called from a failed boot.
+	 * No harm done if it is a STLC2690 chip user who called.
+	 */
+	wake_up_all(&main_wait_queue);
+
+	return 0;
+}
+
+static struct sk_buff *stlc2690_alloc_skb(unsigned int size, gfp_t priority)
+{
+	struct sk_buff *skb;
+
+	dev_dbg(MAIN_DEV, "stlc2690_alloc_skb size %d bytes\n", size);
+
+	/* Allocate the SKB and reserve space for the header */
+	skb = alloc_skb(size + CG2900_SKB_RESERVE, priority);
+	if (skb)
+		skb_reserve(skb, CG2900_SKB_RESERVE);
+
+	return skb;
+}
+
+static int stlc2690_write(struct cg2900_user_data *user, struct sk_buff *skb)
+{
+	int err = 0;
+	u8 *h4_header;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"stlc2690_write: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	if (!skb) {
+		dev_err(user->dev, "stlc2690_write with no sk_buffer\n");
+		return -EINVAL;
+	}
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	dev_dbg(user->dev, "stlc2690_write length %d bytes\n", skb->len);
+
+	if (!user->opened) {
+		dev_err(user->dev,
+			"Trying to transmit data on a closed channel\n");
+		return -EACCES;
+	}
+
+	/*
+	 * Move the data pointer to the H:4 header position and
+	 * store the H4 header.
+	 */
+	h4_header = skb_push(skb, CG2900_SKB_RESERVE);
+	*h4_header = (u8)user->h4_channel;
+	cg2900_tx_to_chip(user, info->logger, skb);
+
+	return err;
+}
+
+static int stlc2690_no_write(struct cg2900_user_data *user,
+			     struct sk_buff *skb)
+{
+	dev_err(user->dev, "Not allowed to send on this channel\n");
+	return -EPERM;
+}
+
+static bool stlc2690_get_local_revision(struct cg2900_user_data *user,
+					struct cg2900_rev_data *rev_data)
+{
+	struct cg2900_chip_dev *dev;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "stlc2690_get_local_revision: Calling with "
+			"NULL pointer\n");
+		return false;
+	}
+
+	if (!rev_data) {
+		dev_err(user->dev, "Calling with rev_data NULL\n");
+		return false;
+	}
+
+	dev = cg2900_get_prv(user);
+
+	rev_data->revision = dev->chip.hci_revision;
+	rev_data->sub_version = dev->chip.hci_sub_version;
+
+	return true;
+}
+
+static struct cg2900_user_data btcmd_data = {
+	.h4_channel = CHANNEL_BT_CMD,
+};
+static struct cg2900_user_data btacl_data = {
+	.h4_channel = CHANNEL_BT_ACL,
+};
+static struct cg2900_user_data btevt_data = {
+	.h4_channel = CHANNEL_BT_EVT,
+};
+static struct cg2900_user_data hci_logger_data = {
+	.h4_channel = CHANNEL_HCI_LOGGER,
+	.chip_independent = true,
+	.write = stlc2690_no_write,
+	.open = stlc2690_hci_log_open,
+	.close = stlc2690_hci_log_close,
+};
+static struct cg2900_user_data core_data = {
+	.h4_channel = CHANNEL_CORE,
+	.write = stlc2690_no_write,
+};
+
+static struct mfd_cell stlc2690_devs[] = {
+	{
+		.name = "cg2900-btcmd",
+		.platform_data = &btcmd_data,
+	},
+	{
+		.name = "cg2900-btacl",
+		.platform_data = &btacl_data,
+	},
+	{
+		.name = "cg2900-btevt",
+		.platform_data = &btevt_data,
+	},
+	{
+		.name = "cg2900-hcilogger",
+		.platform_data = &hci_logger_data,
+	},
+	{
+		.name = "cg2900-core",
+		.platform_data = &core_data,
+	},
+};
+
+static struct cg2900_user_data char_btcmd_data = {
+	.channel_data = {
+		.char_dev_name = STLC2690_BT_CMD,
+	},
+	.h4_channel = CHANNEL_BT_CMD,
+};
+static struct cg2900_user_data char_btacl_data = {
+	.channel_data = {
+		.char_dev_name = STLC2690_BT_ACL,
+	},
+	.h4_channel = CHANNEL_BT_ACL,
+};
+static struct cg2900_user_data char_btevt_data = {
+	.channel_data = {
+		.char_dev_name = STLC2690_BT_EVT,
+	},
+	.h4_channel = CHANNEL_BT_EVT,
+};
+static struct cg2900_user_data char_hci_logger_data = {
+	.channel_data = {
+		.char_dev_name = STLC2690_HCI_LOGGER,
+	},
+	.h4_channel = CHANNEL_HCI_LOGGER,
+	.chip_independent = true,
+	.write = stlc2690_no_write,
+	.open = stlc2690_hci_log_open,
+	.close = stlc2690_hci_log_close,
+};
+static struct cg2900_user_data char_core_data = {
+	.channel_data = {
+		.char_dev_name = STLC2690_CORE,
+	},
+	.h4_channel = CHANNEL_CORE,
+	.write = stlc2690_no_write,
+};
+
+static struct mfd_cell stlc2690_char_devs[] = {
+	{
+		.name = "cg2900-chardev",
+		.id = 0,
+		.platform_data = &char_btcmd_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 1,
+		.platform_data = &char_btacl_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 2,
+		.platform_data = &char_btevt_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 7,
+		.platform_data = &char_hci_logger_data,
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 8,
+		.platform_data = &char_core_data,
+	},
+};
+
+/**
+ * set_plat_data() - Initializes data for an MFD cell.
+ * @cell:	MFD cell.
+ * @dev:	Current chip.
+ *
+ * Sets each callback to default function unless already set.
+ */
+static void set_plat_data(struct mfd_cell *cell, struct cg2900_chip_dev *dev)
+{
+	struct cg2900_user_data *user = cell->platform_data;
+
+	if (!user->open)
+		user->open = stlc2690_open;
+	if (!user->close)
+		user->close = stlc2690_close;
+	if (!user->reset)
+		user->reset = stlc2690_reset;
+	if (!user->alloc_skb)
+		user->alloc_skb = stlc2690_alloc_skb;
+	if (!user->write)
+		user->write = stlc2690_write;
+	if (!user->get_local_revision)
+		user->get_local_revision = stlc2690_get_local_revision;
+
+	cg2900_set_prv(user, dev);
+}
+
+/**
+ * check_chip_support() - Checks if connected chip is handled by this driver.
+ * @dev:	Chip info structure.
+ *
+ * If supported return true and fill in @callbacks.
+ *
+ * Returns:
+ *   true if chip is handled by this driver.
+ *   false otherwise.
+ */
+static bool check_chip_support(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_platform_data *pf_data;
+	struct stlc2690_chip_info *info;
+	int i;
+	int err;
+
+	dev_dbg(dev->dev, "check_chip_support\n");
+
+	/*
+	 * Check if this is a STLC2690 revision.
+	 * We do not care about the sub-version at the moment. Change this if
+	 * necessary.
+	 */
+	if (dev->chip.manufacturer != STLC2690_SUPP_MANUFACTURER ||
+	    dev->chip.hci_revision < STLC2690_SUPP_REVISION_MIN ||
+	    dev->chip.hci_revision > STLC2690_SUPP_REVISION_MAX) {
+		dev_dbg(dev->dev, "Chip not supported by STLC2690 driver\n"
+			"\tMan: 0x%02X\n"
+			"\tRev: 0x%04X\n"
+			"\tSub: 0x%04X\n",
+			dev->chip.manufacturer, dev->chip.hci_revision,
+			dev->chip.hci_sub_version);
+		return false;
+	}
+
+	/* Store needed data */
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(dev->dev, "Couldn't allocate info struct\n");
+		return false;
+	}
+
+	/* Initialize all variables */
+	INIT_LIST_HEAD(&info->open_channels);
+	spin_lock_init(&info->rw_lock);
+	info->chip_dev = dev;
+
+	info->wq = create_singlethread_workqueue(WQ_NAME);
+	if (!info->wq) {
+		dev_err(dev->dev, "Could not create workqueue\n");
+		goto err_handling_free_info;
+	}
+
+	info->patch_file_name = kzalloc(NAME_MAX + 1, GFP_ATOMIC);
+	if (!info->patch_file_name) {
+		dev_err(dev->dev,
+			"Couldn't allocate name buffer for patch file\n");
+		goto err_handling_destroy_wq;
+	}
+
+	info->settings_file_name = kzalloc(NAME_MAX + 1, GFP_ATOMIC);
+	if (!info->settings_file_name) {
+		dev_err(dev->dev,
+			"Couldn't allocate name buffers settings file\n");
+		goto err_handling_free_patch_name;
+	}
+
+	dev->c_data = info;
+	/* Set the callbacks */
+	dev->c_cb.data_from_chip = data_from_chip;
+	dev->c_cb.chip_removed = chip_removed,
+	info->chip_dev = dev;
+
+	mutex_lock(&main_info->man_mutex);
+
+	pf_data = dev_get_platdata(dev->dev);
+	btcmd_data.channel_data.bt_bus = pf_data->bus;
+	btacl_data.channel_data.bt_bus = pf_data->bus;
+	btevt_data.channel_data.bt_bus = pf_data->bus;
+
+	for (i = 0; i < ARRAY_SIZE(stlc2690_devs); i++)
+		set_plat_data(&stlc2690_devs[i], dev);
+	for (i = 0; i < ARRAY_SIZE(stlc2690_char_devs); i++)
+		set_plat_data(&stlc2690_char_devs[i], dev);
+
+	err = mfd_add_devices(dev->dev, main_info->cell_base_id, stlc2690_devs,
+			      ARRAY_SIZE(stlc2690_devs), NULL, 0);
+	if (err) {
+		dev_err(dev->dev, "Failed to add stlc2690_devs (%d)\n", err);
+		goto err_handling_free_settings_name;
+	}
+
+	err = mfd_add_devices(dev->dev, main_info->cell_base_id,
+			      stlc2690_char_devs,
+			      ARRAY_SIZE(stlc2690_char_devs), NULL, 0);
+	if (err) {
+		dev_err(dev->dev, "Failed to add stlc2690_char_devs (%d)\n",
+			err);
+		goto err_handling_remove_devs;
+	}
+
+	main_info->cell_base_id += 30;
+	mutex_unlock(&main_info->man_mutex);
+
+	dev_info(dev->dev, "Chip supported by the STLC2690 chip driver\n");
+
+	/* Close the transport, which will power off the chip */
+	if (dev->t_cb.close)
+		dev->t_cb.close(dev);
+
+	dev_dbg(dev->dev, "New main_state: STLC2690_IDLE\n");
+	info->main_state = STLC2690_IDLE;
+
+	return true;
+
+err_handling_remove_devs:
+	mfd_remove_devices(dev->dev);
+err_handling_free_settings_name:
+	kfree(info->settings_file_name);
+err_handling_free_patch_name:
+	kfree(info->patch_file_name);
+err_handling_destroy_wq:
+	destroy_workqueue(info->wq);
+err_handling_free_info:
+	kfree(info);
+	return false;
+}
+
+static struct cg2900_id_callbacks chip_support_callbacks = {
+	.check_chip_support = check_chip_support,
+};
+
+/**
+ * stlc2690_chip_probe() - Initialize STLC2690 chip handler resources.
+ * @pdev:	Platform device.
+ *
+ * This function initializes the STLC2690 driver, then registers to
+ * the CG2900 Core.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM for failed alloc or structure creation.
+ *   Error codes generated by cg2900_register_chip_driver.
+ */
+static int __devinit stlc2690_chip_probe(struct platform_device *pdev)
+{
+	int err;
+
+	dev_dbg(&pdev->dev, "stlc2690_chip_probe\n");
+
+	main_info = kzalloc(sizeof(*main_info), GFP_ATOMIC);
+	if (!main_info) {
+		dev_err(&pdev->dev, "Couldn't allocate main_info\n");
+		return -ENOMEM;
+	}
+
+	main_info->dev = &pdev->dev;
+	mutex_init(&main_info->man_mutex);
+
+	err = cg2900_register_chip_driver(&chip_support_callbacks);
+	if (err) {
+		dev_err(&pdev->dev,
+			"Couldn't register chip driver (%d)\n", err);
+		goto error_handling;
+	}
+
+	dev_info(&pdev->dev, "STLC2690 chip driver started\n");
+
+	return 0;
+
+error_handling:
+	mutex_destroy(&main_info->man_mutex);
+	kfree(main_info);
+	main_info = NULL;
+	return err;
+}
+
+/**
+ * stlc2690_chip_remove() - Release STLC2690 chip handler resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success (always success).
+ */
+static int __devexit stlc2690_chip_remove(struct platform_device *pdev)
+{
+	dev_info(&pdev->dev, "STLC2690 chip driver removed\n");
+
+	cg2900_deregister_chip_driver(&chip_support_callbacks);
+
+	if (!main_info)
+		return 0;
+
+	mutex_destroy(&main_info->man_mutex);
+	kfree(main_info);
+	main_info = NULL;
+	return 0;
+}
+
+static struct platform_driver stlc2690_chip_driver = {
+	.driver = {
+		.name	= "stlc2690-chip",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= stlc2690_chip_probe,
+	.remove	= __devexit_p(stlc2690_chip_remove),
+};
+
+/**
+ * stlc2690_chip_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init stlc2690_chip_init(void)
+{
+	pr_debug("stlc2690_chip_init");
+	return platform_driver_register(&stlc2690_chip_driver);
+}
+
+/**
+ * stlc2690_chip_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit stlc2690_chip_exit(void)
+{
+	pr_debug("stlc2690_chip_exit");
+	platform_driver_unregister(&stlc2690_chip_driver);
+}
+
+module_init(stlc2690_chip_init);
+module_exit(stlc2690_chip_exit);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux STLC2690 Connectivity Device Driver");
diff --git a/drivers/staging/cg2900/mfd/stlc2690_chip.h b/drivers/staging/cg2900/mfd/stlc2690_chip.h
new file mode 100644
index 00000000000..d14e7737636
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/stlc2690_chip.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson STLC2690 BT/FM controller.
+ */
+
+#ifndef _STLC2690_CHIP_H_
+#define _STLC2690_CHIP_H_
+
+/* Supported chips */
+#define STLC2690_SUPP_MANUFACTURER		0x30
+#define STLC2690_SUPP_REVISION_MIN		0x0500
+#define STLC2690_SUPP_REVISION_MAX		0x06FF
+
+#define BT_SIZE_OF_HDR				(sizeof(__le16) + sizeof(__u8))
+#define BT_PARAM_LEN(__pkt_len)			(__pkt_len - BT_SIZE_OF_HDR)
+
+/* BT VS Store In FS command */
+#define STLC2690_BT_OP_VS_STORE_IN_FS		0xFC22
+struct bt_vs_store_in_fs_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	user_id;
+	__u8	len;
+	__u8	data[];
+} __packed;
+
+/* BT VS Write File Block command */
+#define STLC2690_BT_OP_VS_WRITE_FILE_BLOCK	0xFC2E
+struct bt_vs_write_file_block_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	id;
+	__u8	data[];
+} __packed;
+
+/* User ID for storing BD address in chip using Store_In_FS command */
+#define STLC2690_VS_STORE_IN_FS_USR_ID_BD_ADDR	0xFE
+
+#endif /* _STLC2690_CHIP_H_ */
diff --git a/drivers/staging/cw1200/.gitignore b/drivers/staging/cw1200/.gitignore
new file mode 100644
index 00000000000..6ad0d1ec58e
--- /dev/null
+++ b/drivers/staging/cw1200/.gitignore
@@ -0,0 +1,10 @@
+*.o
+*.ko
+*.ko.cmd
+.tmp_versions
+modules.order
+Module.symvers
+Module.markers
+*.o.cmd
+*.mod.c
+*.swp
diff --git a/drivers/staging/cw1200/Kconfig b/drivers/staging/cw1200/Kconfig
new file mode 100644
index 00000000000..5c6f18267e1
--- /dev/null
+++ b/drivers/staging/cw1200/Kconfig
@@ -0,0 +1,74 @@
+config CW1200
+       tristate "CW1200 WLAN support"
+       select MAC80211
+       select CFG80211
+       help
+
+         This is an experimental driver for the cw1200 chip-set.
+         Enabling this option enables the generic driver without
+         any platform support.
+
+         Please select the appropriate platform below.
+
+if CW1200
+
+config CW1200_NON_POWER_OF_TWO_BLOCKSIZES
+      bool "Platform supports non-power-of-two SDIO transfer"
+      depends on CW1200
+      help
+        Say N here only if you are running the driver on a platform
+        which does not have support for non-power-of-two SDIO transfer.
+        If unsure, say Y.
+
+config CW1200_USE_GPIO_IRQ
+      bool "Use GPIO interrupt"
+      depends on CW1200
+      help
+        Say Y here if you want to include GPIO IRQ support instead of SDIO IRQ.
+        If unsure, say N.
+
+config CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE
+      bool "Software keep-alive (DEVELOPMENT)"
+      depends on CW1200
+      help
+        Say Y if your firmware does not support keep-alive functionality
+        or you suspect problems with the implementation.
+        Please finalize software the software keep-alive functionality in
+        that case.
+        If unsure, say N.
+
+config CW1200_5GHZ_SUPPORT
+      bool "5GHz band support"
+      depends on CW1200
+      help
+        Say Y if your device supports 5GHz band. Should be disabled for
+        CW1100 silicon.
+        If unsure, say N.
+
+menu "Driver debug features"
+      depends on CW1200
+
+config CW1200_DEBUGFS
+      bool "Expose driver internals to DebugFS (DEVELOPMENT)"
+
+config CW1200_BH_DEBUG
+      bool "Enable low-level device communication logs (DEVELOPMENT)"
+
+config CW1200_WSM_DEBUG
+      bool "Enable WSM API debug messages (DEVELOPMENT)"
+
+config CW1200_WSM_DUMPS
+      bool "Verbose WSM API logging (DEVELOPMENT)"
+
+config CW1200_TXRX_DEBUG
+      bool "Enable TX/RX debug messages (DEVELOPMENT)"
+
+config CW1200_TX_POLICY_DEBUG
+      bool "Enable TX policy debug (DEVELOPMENT)"
+
+config CW1200_STA_DEBUG
+      bool "Enable STA/AP debug (DEVELOPMENT)"
+
+endmenu
+
+endif
diff --git a/drivers/staging/cw1200/Makefile b/drivers/staging/cw1200/Makefile
new file mode 100644
index 00000000000..fae984d10ce
--- /dev/null
+++ b/drivers/staging/cw1200/Makefile
@@ -0,0 +1,18 @@
+cw1200_core-y := \
+		fwio.o \
+		txrx.o \
+		main.o \
+		queue.o \
+		hwio.o \
+		bh.o \
+		wsm.o \
+		sta.o \
+		ap.o \
+		scan.o
+cw1200_core-$(CONFIG_CW1200_DEBUGFS) += debug.o
+
+cw1200_wlan-y := cw1200_sdio.o
+
+obj-$(CONFIG_CW1200) += cw1200_core.o
+obj-$(CONFIG_CW1200) += cw1200_wlan.o
+
diff --git a/drivers/staging/cw1200/TODO b/drivers/staging/cw1200/TODO
new file mode 100644
index 00000000000..0d2be40e1f4
--- /dev/null
+++ b/drivers/staging/cw1200/TODO
@@ -0,0 +1,10 @@
+TODO:
+        - IBSS: Not implemented (3-10 m*d).
+        - 11n: Almost done. WSM API upgrade is required fo finish implementation. (2-3 m*d).
+        - 11n: verification (??? m*d Resources? WLAN RF lab? 11n sniffers
+          availability? Bring up of the test equipment?).
+        - memory leakage verification and proper cleanup: not done (1-3 m*d).
+        - AP (hot-spot) mode: Implemented, some problems with WEP104/WPA/WPA2 security.
+          FW bug? To be investigated.
+        - U-APSD configuration (0.5-1 m*d).
+        - Cleanup of debug printouts (1 m*d).
diff --git a/drivers/staging/cw1200/ap.c b/drivers/staging/cw1200/ap.c
new file mode 100644
index 00000000000..561dab3f94f
--- /dev/null
+++ b/drivers/staging/cw1200/ap.c
@@ -0,0 +1,612 @@
+/*
+ * mac80211 STA and AP API for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include "cw1200.h"
+#include "sta.h"
+#include "ap.h"
+#include "bh.h"
+
+#if defined(CONFIG_CW1200_STA_DEBUG)
+#define ap_printk(...) printk(__VA_ARGS__)
+#else
+#define ap_printk(...)
+#endif
+
+static int cw1200_upload_beacon(struct cw1200_common *priv);
+static int cw1200_start_ap(struct cw1200_common *priv);
+static int cw1200_update_beaconing(struct cw1200_common *priv);
+
+
+/* ******************************************************************** */
+/* AP API								*/
+
+int cw1200_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+                   struct ieee80211_sta *sta)
+{
+	struct cw1200_common *priv = hw->priv;
+	struct cw1200_sta_priv *sta_priv =
+			(struct cw1200_sta_priv *)&sta->drv_priv;
+	struct wsm_map_link map_link = {
+		.link_id = 0,
+	};
+
+
+	/* Link ID mapping works fine in STA mode as well.
+	 * It's better to keep same handling for both STA ans AP modes */
+#if 0
+	if (priv->mode != NL80211_IFTYPE_AP)
+		return 0;
+#endif
+
+	map_link.link_id = ffs(~(priv->link_id_map | 1)) - 1;
+	if (map_link.link_id > CW1200_MAX_STA_IN_AP_MODE) {
+		sta_priv->link_id = 0;
+		printk(KERN_INFO "[AP] No more link ID available.\n");
+		return -ENOENT;
+	}
+
+	memcpy(map_link.mac_addr, sta->addr, ETH_ALEN);
+	if (!WARN_ON(wsm_map_link(priv, &map_link))) {
+		sta_priv->link_id = map_link.link_id;
+		priv->link_id_map |= 1 << map_link.link_id;
+		ap_printk(KERN_DEBUG "[AP] STA added, link_id: %d\n",
+			map_link.link_id);
+	}
+	return 0;
+}
+
+int cw1200_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+                      struct ieee80211_sta *sta)
+{
+	struct cw1200_common *priv = hw->priv;
+	struct cw1200_sta_priv *sta_priv =
+			(struct cw1200_sta_priv *)&sta->drv_priv;
+	struct wsm_reset reset = {
+		.link_id = 0,
+		.reset_statistics = false,
+	};
+
+	if (sta_priv->link_id) {
+		ap_printk(KERN_DEBUG "[AP] STA removed, link_id: %d\n",
+			sta_priv->link_id);
+		reset.link_id = sta_priv->link_id;
+		priv->link_id_map &= ~(1 << sta_priv->link_id);
+		sta_priv->link_id = 0;
+		WARN_ON(wsm_reset(priv, &reset));
+	}
+	return 0;
+}
+
+void cw1200_sta_notify(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
+		       enum sta_notify_cmd notify_cmd,
+		       struct ieee80211_sta *sta)
+{
+	struct cw1200_common *priv = dev->priv;
+	struct cw1200_sta_priv *sta_priv =
+		(struct cw1200_sta_priv *)&sta->drv_priv;
+	u32 bit = 1 << sta_priv->link_id;
+
+	switch (notify_cmd) {
+		case STA_NOTIFY_SLEEP:
+			priv->sta_asleep_mask |= bit;
+			break;
+		case STA_NOTIFY_AWAKE:
+			priv->sta_asleep_mask &= ~bit;
+			cw1200_bh_wakeup(priv);
+			break;
+	}
+}
+
+static int cw1200_set_tim_impl(struct cw1200_common *priv, bool multicast)
+{
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_BEACON,
+	};
+	u16 tim_offset, tim_length;
+
+	ap_printk(KERN_DEBUG "[AP] %s.\n", __func__);
+
+	frame.skb = ieee80211_beacon_get_tim(priv->hw, priv->vif,
+			&tim_offset, &tim_length);
+	if (WARN_ON(!frame.skb))
+		return -ENOMEM;
+
+	if (tim_offset && tim_length >= 6) {
+		/* Ignore DTIM count from mac80211:
+		 * firmware handles DTIM internally. */
+		frame.skb->data[tim_offset + 2] = 0;
+
+		/* Set/reset aid0 bit */
+		if (multicast)
+			frame.skb->data[tim_offset + 4] |= 1;
+		else
+			frame.skb->data[tim_offset + 4] &= ~1;
+	}
+
+	WARN_ON(wsm_set_template_frame(priv, &frame));
+
+	dev_kfree_skb(frame.skb);
+
+	return 0;
+}
+
+void cw1200_set_tim_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, set_tim_work);
+	(void)cw1200_set_tim_impl(priv, !priv->suspend_multicast);
+}
+
+int cw1200_set_tim(struct ieee80211_hw *dev, struct ieee80211_sta *sta,
+		   bool set)
+{
+	struct cw1200_common *priv = dev->priv;
+	queue_work(priv->workqueue, &priv->set_tim_work);
+	return 0;
+}
+
+void cw1200_bss_info_changed(struct ieee80211_hw *dev,
+			     struct ieee80211_vif *vif,
+			     struct ieee80211_bss_conf *info,
+			     u32 changed)
+{
+	struct cw1200_common *priv = dev->priv;
+	struct ieee80211_conf *conf = &dev->conf;
+
+	mutex_lock(&priv->conf_mutex);
+	if (changed & BSS_CHANGED_BSSID) {
+		memcpy(priv->bssid, info->bssid, ETH_ALEN);
+		cw1200_setup_mac(priv);
+	}
+
+	/* TODO: BSS_CHANGED_IBSS */
+	/* TODO: BSS_CHANGED_ARP_FILTER */
+
+	if (changed & BSS_CHANGED_BEACON_ENABLED)
+		priv->enable_beacon = info->enable_beacon;
+
+	if (changed & BSS_CHANGED_BEACON)
+		WARN_ON(cw1200_upload_beacon(priv));
+
+	if (changed & (BSS_CHANGED_BEACON_ENABLED | BSS_CHANGED_BEACON |
+			BSS_CHANGED_BEACON_INT))
+		WARN_ON(cw1200_update_beaconing(priv));
+
+	if (changed & BSS_CHANGED_ASSOC) {
+		wsm_lock_tx(priv);
+		priv->wep_default_key_id = -1;
+		wsm_unlock_tx(priv);
+
+		if (!info->assoc /* && !info->ibss_joined */) {
+			priv->cqm_link_loss_count = 60;
+			priv->cqm_beacon_loss_count = 20;
+			priv->cqm_tx_failure_thold = 0;
+		}
+		priv->cqm_tx_failure_count = 0;
+	}
+
+	if (changed &
+	    (BSS_CHANGED_ASSOC |
+	     BSS_CHANGED_BASIC_RATES |
+	     BSS_CHANGED_ERP_PREAMBLE |
+	     BSS_CHANGED_HT |
+	     BSS_CHANGED_ERP_SLOT)) {
+		ap_printk(KERN_DEBUG "BSS_CHANGED_ASSOC.\n");
+		if (info->assoc) { /* TODO: ibss_joined */
+			int dtim_interval = conf->ps_dtim_period;
+			int listen_interval = conf->listen_interval;
+			struct ieee80211_sta *sta = NULL;
+
+			/* Associated: kill join timeout */
+			cancel_delayed_work_sync(&priv->join_timeout);
+
+			/* TODO: This code is not verified {{{ */
+			rcu_read_lock();
+			if (info->bssid)
+				sta = ieee80211_find_sta(vif, info->bssid);
+			if (sta) {
+				BUG_ON(!priv->channel);
+				priv->ht_info.ht_cap = sta->ht_cap;
+				priv->bss_params.operationalRateSet =
+					__cpu_to_le32(
+					cw1200_rate_mask_to_wsm(priv,
+					sta->supp_rates[priv->channel->band]));
+				priv->ht_info.channel_type =
+						info->channel_type;
+				priv->ht_info.operation_mode =
+						info->ht_operation_mode;
+			} else {
+				memset(&priv->ht_info, 0,
+						sizeof(priv->ht_info));
+				priv->bss_params.operationalRateSet = -1;
+			}
+			rcu_read_unlock();
+			/* }}} */
+
+			priv->association_mode.greenfieldMode =
+				cw1200_ht_greenfield(&priv->ht_info);
+			priv->association_mode.flags =
+				WSM_ASSOCIATION_MODE_SNOOP_ASSOC_FRAMES |
+				WSM_ASSOCIATION_MODE_USE_PREAMBLE_TYPE |
+				WSM_ASSOCIATION_MODE_USE_HT_MODE |
+				WSM_ASSOCIATION_MODE_USE_BASIC_RATE_SET |
+				WSM_ASSOCIATION_MODE_USE_MPDU_START_SPACING;
+			priv->association_mode.preambleType =
+				info->use_short_preamble ?
+				WSM_JOIN_PREAMBLE_SHORT :
+				WSM_JOIN_PREAMBLE_LONG;
+			priv->association_mode.basicRateSet = __cpu_to_le32(
+				cw1200_rate_mask_to_wsm(priv,
+				info->basic_rates));
+			priv->association_mode.mpduStartSpacing =
+				cw1200_ht_ampdu_density(&priv->ht_info);
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+			priv->cqm_beacon_loss_count =
+					info->cqm_beacon_miss_thold;
+			priv->cqm_tx_failure_thold =
+					info->cqm_tx_fail_thold;
+			priv->cqm_tx_failure_count = 0;
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+
+			priv->bss_params.beaconLostCount =
+					priv->cqm_beacon_loss_count ?
+					priv->cqm_beacon_loss_count :
+					priv->cqm_link_loss_count;
+
+			priv->bss_params.aid = info->aid;
+
+			if (dtim_interval < 1)
+				dtim_interval = 1;
+			if (dtim_interval < priv->join_dtim_period)
+				dtim_interval = priv->join_dtim_period;
+			if (listen_interval < dtim_interval)
+				listen_interval = 0;
+
+			ap_printk(KERN_DEBUG "[STA] DTIM %d, listen %d\n",
+				dtim_interval, listen_interval);
+			ap_printk(KERN_DEBUG "[STA] Preamble: %d, " \
+				"Greenfield: %d, Aid: %d, " \
+				"Rates: 0x%.8X, Basic: 0x%.8X\n",
+				priv->association_mode.preambleType,
+				priv->association_mode.greenfieldMode,
+				priv->bss_params.aid,
+				priv->bss_params.operationalRateSet,
+				priv->association_mode.basicRateSet);
+			WARN_ON(wsm_set_association_mode(priv,
+				&priv->association_mode));
+			WARN_ON(wsm_set_bss_params(priv, &priv->bss_params));
+			WARN_ON(wsm_set_beacon_wakeup_period(priv,
+				dtim_interval, listen_interval));
+#if 0
+			/* It's better to override internal TX rete; otherwise
+			 * device sends RTS at too high rate. However device
+			 * can't receive CTS at 1 and 2 Mbps. Well, 5.5 is a
+			 * good choice for RTS/CTS, but that means PS poll
+			 * will be sent at the same rate - impact on link
+			 * budget. Not sure what is better.. */
+
+			/* Update: internal rate selection algorythm is not
+			 * bad: if device is not receiving CTS at high rate,
+			 * it drops RTS rate.
+			 * So, conclusion: if-0 the code. Keep code just for
+			 * information:
+			 * Do not touch WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE! */
+
+			/* ~3 is a bug in device: RTS/CTS is not working at
+			 * low rates */
+
+			__le32 internal_tx_rate = __cpu_to_le32(__ffs(
+				priv->association_mode.basicRateSet & ~3));
+			WARN_ON(wsm_write_mib(priv,
+				WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE,
+				&internal_tx_rate,
+				sizeof(internal_tx_rate)));
+#endif
+		} else {
+			memset(&priv->association_mode, 0,
+				sizeof(priv->association_mode));
+			memset(&priv->bss_params, 0, sizeof(priv->bss_params));
+		}
+	}
+	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_ERP_CTS_PROT)) {
+		__le32 use_cts_prot = info->use_cts_prot ?
+			__cpu_to_le32(1) : 0;
+
+		ap_printk(KERN_DEBUG "[STA] CTS protection %d\n",
+			info->use_cts_prot);
+		WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_NON_ERP_PROTECTION,
+			&use_cts_prot, sizeof(use_cts_prot)));
+	}
+	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_ERP_SLOT)) {
+		__le32 slot_time = info->use_short_slot ?
+			__cpu_to_le32(9) : __cpu_to_le32(20);
+		ap_printk(KERN_DEBUG "[STA] Slot time :%d us.\n",
+			__le32_to_cpu(slot_time));
+		WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_DOT11_SLOT_TIME,
+			&slot_time, sizeof(slot_time)));
+	}
+	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_CQM)) {
+		struct wsm_rcpi_rssi_threshold threshold = {
+			.rssiRcpiMode = WSM_RCPI_RSSI_USE_RSSI,
+			.rollingAverageCount = 1,
+		};
+
+#if 0
+		/* For verification purposes */
+		info->cqm_rssi_thold = -50;
+		info->cqm_rssi_hyst = 4;
+#endif /* 0 */
+
+		ap_printk(KERN_DEBUG "[CQM] RSSI threshold subscribe: %d +- %d\n",
+			info->cqm_rssi_thold, info->cqm_rssi_hyst);
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+		ap_printk(KERN_DEBUG "[CQM] Beacon loss subscribe: %d\n",
+			info->cqm_beacon_miss_thold);
+		ap_printk(KERN_DEBUG "[CQM] TX failure subscribe: %d\n",
+			info->cqm_tx_fail_thold);
+		priv->cqm_rssi_thold = info->cqm_rssi_thold;
+		priv->cqm_rssi_hyst = info->cqm_rssi_hyst;
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+		if (info->cqm_rssi_thold || info->cqm_rssi_hyst) {
+			/* RSSI subscription enabled */
+			/* TODO: It's not a correct way of setting threshold.
+			 * Upper and lower must be set equal here and adjusted
+			 * in callback. However current implementation is much
+			 * more relaible and stable. */
+			threshold.upperThreshold =
+				info->cqm_rssi_thold + info->cqm_rssi_hyst;
+			threshold.lowerThreshold =
+				info->cqm_rssi_thold;
+			threshold.rssiRcpiMode |=
+				WSM_RCPI_RSSI_THRESHOLD_ENABLE;
+		} else {
+			/* There is a bug in FW, see sta.c. We have to enable
+			 * dummy subscription to get correct RSSI values. */
+			threshold.rssiRcpiMode |=
+				WSM_RCPI_RSSI_THRESHOLD_ENABLE |
+				WSM_RCPI_RSSI_DONT_USE_UPPER |
+				WSM_RCPI_RSSI_DONT_USE_LOWER;
+		}
+		WARN_ON(wsm_set_rcpi_rssi_threshold(priv, &threshold));
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+		priv->cqm_tx_failure_thold = info->cqm_tx_fail_thold;
+		priv->cqm_tx_failure_count = 0;
+
+		if (priv->cqm_beacon_loss_count !=
+				info->cqm_beacon_miss_thold) {
+			priv->cqm_beacon_loss_count =
+				info->cqm_beacon_miss_thold;
+			priv->bss_params.beaconLostCount =
+				priv->cqm_beacon_loss_count ?
+				priv->cqm_beacon_loss_count :
+				priv->cqm_link_loss_count;
+			WARN_ON(wsm_set_bss_params(priv, &priv->bss_params));
+		}
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+	}
+	mutex_unlock(&priv->conf_mutex);
+}
+
+void cw1200_multicast_start_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, multicast_start_work);
+
+        (void)cw1200_set_tim_impl(priv, true);
+	wsm_lock_tx(priv);
+	priv->suspend_multicast = false;
+	wsm_unlock_tx(priv);
+	cw1200_bh_wakeup(priv);
+}
+
+void cw1200_multicast_stop_work(struct work_struct *work)
+{
+        struct cw1200_common *priv =
+                container_of(work, struct cw1200_common, multicast_stop_work);
+
+	/* Lock flushes send queue in device. Just to make sure DTIM beacom
+	 * and frames are sent. */
+	wsm_lock_tx(priv);
+	priv->suspend_multicast = true;
+        (void)cw1200_set_tim_impl(priv, false);
+	wsm_unlock_tx(priv);
+}
+
+int cw1200_ampdu_action(struct ieee80211_hw *hw,
+			struct ieee80211_vif *vif,
+			enum ieee80211_ampdu_mlme_action action,
+			struct ieee80211_sta *sta, u16 tid, u16 *ssn)
+{
+	/* Aggregation is implemented fully in firmware,
+	 * including block ack negotiation. Do not allow
+	 * mac80211 stack to do anything: it interferes with
+	 * the firmware. */
+	return -ENOTSUPP;
+}
+
+/* ******************************************************************** */
+/* WSM callback								*/
+void cw1200_suspend_resume(struct cw1200_common *priv,
+			  struct wsm_suspend_resume *arg)
+{
+	int queue = 1 << wsm_queue_id_to_linux(arg->queue);
+	u32 unicast = 1 << arg->link_id;
+	u32 after_dtim = 1 << CW1200_LINK_ID_AFTER_DTIM;
+	u32 wakeup_required = 0;
+	u32 set = 0;
+	u32 clear;
+	u32 tx_suspend_mask;
+	int i;
+
+	if (!arg->link_id) /* For all links */
+		unicast = (1 << (CW1200_MAX_STA_IN_AP_MODE + 1)) - 2;
+
+	ap_printk(KERN_DEBUG "[AP] %s: %s\n",
+		arg->stop ? "stop" : "start",
+		arg->multicast ? "broadcast" : "unicast");
+
+	if (arg->multicast) {
+		if (arg->stop)
+			queue_work(priv->workqueue,
+				&priv->multicast_stop_work);
+		else {
+			/* Handle only if there is data to be sent */
+			for (i = 0; i < 4; ++i) {
+				if (cw1200_queue_get_num_queued(
+						&priv->tx_queue[i],
+						after_dtim)) {
+					queue_work(priv->workqueue,
+						&priv->multicast_start_work);
+					break;
+				}
+			}
+		}
+	} else {
+		if (arg->stop)
+			set = unicast;
+		else
+			set = 0;
+
+		clear = set ^ unicast;
+
+		/* TODO: if (!priv->uapsd) */
+		queue = 0x0F;
+
+		for (i = 0; i < 4; ++i) {
+			if (!(queue & (1 << i)))
+				continue;
+
+			tx_suspend_mask = priv->tx_suspend_mask[i];
+			priv->tx_suspend_mask[i] =
+				(tx_suspend_mask & ~clear) | set;
+
+			wakeup_required = wakeup_required ||
+				cw1200_queue_get_num_queued(
+					&priv->tx_queue[i],
+					tx_suspend_mask & clear);
+		}
+	}
+	if (wakeup_required)
+		cw1200_bh_wakeup(priv);
+	return;
+}
+
+
+/* ******************************************************************** */
+/* AP privates								*/
+
+static int cw1200_upload_beacon(struct cw1200_common *priv)
+{
+	int ret = 0;
+	const u8 *ssidie;
+	const struct ieee80211_mgmt *mgmt;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_BEACON,
+	};
+
+	ap_printk(KERN_DEBUG "[AP] %s.\n", __func__);
+
+	frame.skb = ieee80211_beacon_get(priv->hw, priv->vif);
+	if (WARN_ON(!frame.skb))
+		return -ENOMEM;
+
+	mgmt = (struct ieee80211_mgmt *)frame.skb->data;
+	ssidie = cfg80211_find_ie(WLAN_EID_SSID,
+		mgmt->u.beacon.variable,
+		frame.skb->len - (mgmt->u.beacon.variable - frame.skb->data));
+	memset(priv->ssid, 0, sizeof(priv->ssid));
+	if (ssidie) {
+		priv->ssid_length = ssidie[1];
+		if (WARN_ON(priv->ssid_length > sizeof(priv->ssid)))
+			priv->ssid_length = sizeof(priv->ssid);
+		memcpy(priv->ssid, &ssidie[2], priv->ssid_length);
+	} else {
+		priv->ssid_length = 0;
+	}
+
+	ret = wsm_set_template_frame(priv, &frame);
+	if (!ret) {
+		/* TODO: Distille probe resp; remove TIM
+		 * and other beacon-specific IEs */
+		*(__le16 *)frame.skb->data =
+			__cpu_to_le16(IEEE80211_FTYPE_MGMT |
+				      IEEE80211_STYPE_PROBE_RESP);
+		frame.frame_type = WSM_FRAME_TYPE_PROBE_RESPONSE;
+		ret = wsm_set_template_frame(priv, &frame);
+	}
+	dev_kfree_skb(frame.skb);
+
+	return ret;
+}
+
+static int cw1200_start_ap(struct cw1200_common *priv)
+{
+	int ret;
+	struct ieee80211_bss_conf *conf = &priv->vif->bss_conf;
+	struct wsm_start start = {
+		.mode = WSM_START_MODE_AP,
+		.band = (priv->channel->band == IEEE80211_BAND_5GHZ) ?
+				WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G,
+		.channelNumber = priv->channel->hw_value,
+		.beaconInterval = conf->beacon_int,
+		.DTIMPeriod = conf->dtim_period,
+		.preambleType = conf->use_short_preamble ?
+				WSM_JOIN_PREAMBLE_SHORT :
+				WSM_JOIN_PREAMBLE_LONG,
+		.probeDelay = 100,
+		.basicRateSet = cw1200_rate_mask_to_wsm(priv,
+				conf->basic_rates),
+		.ssidLength = priv->ssid_length,
+	};
+	struct wsm_beacon_transmit transmit = {
+		.enableBeaconing = priv->enable_beacon,
+	};
+
+	memcpy(&start.ssid[0], priv->ssid, start.ssidLength);
+
+	ap_printk(KERN_DEBUG "[AP] ch: %d(%d), bcn: %d(%d), brt: 0x%.8X, ssid: %.*s %s.\n",
+		start.channelNumber, start.band,
+		start.beaconInterval, start.DTIMPeriod,
+		start.basicRateSet,
+		start.ssidLength, start.ssid,
+		transmit.enableBeaconing ? "ena" : "dis");
+	ret = WARN_ON(wsm_start(priv, &start));
+	if (!ret)
+		ret = WARN_ON(cw1200_upload_keys(priv));
+	if (!ret)
+		ret = WARN_ON(wsm_beacon_transmit(priv, &transmit));
+	if (!ret) {
+		WARN_ON(wsm_set_block_ack_policy(priv,
+			priv->ba_tid_mask, priv->ba_tid_mask));
+		priv->join_status = CW1200_JOIN_STATUS_AP;
+		cw1200_update_filtering(priv);
+	}
+	return ret;
+}
+
+static int cw1200_update_beaconing(struct cw1200_common *priv)
+{
+	struct wsm_reset reset = {
+		.link_id = 0,
+		.reset_statistics = true,
+	};
+
+	if (priv->mode == NL80211_IFTYPE_AP) {
+		ap_printk(KERN_DEBUG "[AP] %s.\n", __func__);
+		WARN_ON(wsm_reset(priv, &reset));
+		priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+		WARN_ON(cw1200_start_ap(priv));
+	}
+	return 0;
+}
+
diff --git a/drivers/staging/cw1200/ap.h b/drivers/staging/cw1200/ap.h
new file mode 100644
index 00000000000..63e6165ed69
--- /dev/null
+++ b/drivers/staging/cw1200/ap.h
@@ -0,0 +1,40 @@
+/*
+ * mac80211 STA and AP API for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef AP_H_INCLUDED
+#define AP_H_INCLUDED
+
+int cw1200_set_tim(struct ieee80211_hw *dev, struct ieee80211_sta *sta,
+		   bool set);
+int cw1200_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		   struct ieee80211_sta *sta);
+int cw1200_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		      struct ieee80211_sta *sta);
+void cw1200_sta_notify(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
+		       enum sta_notify_cmd notify_cmd,
+		       struct ieee80211_sta *sta);
+void cw1200_bss_info_changed(struct ieee80211_hw *dev,
+			     struct ieee80211_vif *vif,
+			     struct ieee80211_bss_conf *info,
+			     u32 changed);
+int cw1200_ampdu_action(struct ieee80211_hw *hw,
+			struct ieee80211_vif *vif,
+			enum ieee80211_ampdu_mlme_action action,
+			struct ieee80211_sta *sta, u16 tid, u16 *ssn);
+
+void cw1200_suspend_resume(struct cw1200_common *priv,
+			  struct wsm_suspend_resume *arg);
+void cw1200_set_tim_work(struct work_struct *work);
+void cw1200_multicast_start_work(struct work_struct *work);
+void cw1200_multicast_stop_work(struct work_struct *work);
+
+
+#endif
diff --git a/drivers/staging/cw1200/bh.c b/drivers/staging/cw1200/bh.c
new file mode 100644
index 00000000000..d49f9fdf732
--- /dev/null
+++ b/drivers/staging/cw1200/bh.c
@@ -0,0 +1,475 @@
+/*
+ * Device handling thread implementation for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * ST-Ericsson UMAC CW1200 driver, which is
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Ajitpal Singh <ajitpal.singh@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <net/mac80211.h>
+#include <linux/kthread.h>
+
+#include "cw1200.h"
+#include "bh.h"
+#include "hwio.h"
+#include "wsm.h"
+#include "sbus.h"
+
+#if defined(CONFIG_CW1200_BH_DEBUG)
+#define bh_printk(...) printk(__VA_ARGS__)
+#else
+#define bh_printk(...)
+#endif
+
+static int cw1200_bh(void *arg);
+
+/* TODO: Verify these numbers with WSM specification. */
+#define DOWNLOAD_BLOCK_SIZE_WR	(0x1000 - 4)
+/* an SPI message cannot be bigger than (2"12-1)*2 bytes
+ * "*2" to cvt to bytes */
+#define MAX_SZ_RD_WR_BUFFERS	(DOWNLOAD_BLOCK_SIZE_WR*2)
+#define PIGGYBACK_CTRL_REG	(2)
+#define EFFECTIVE_BUF_SIZE	(MAX_SZ_RD_WR_BUFFERS - PIGGYBACK_CTRL_REG)
+
+typedef int (*cw1200_wsm_handler)(struct cw1200_common *priv,
+	u8 *data, size_t size);
+
+
+int cw1200_register_bh(struct cw1200_common *priv)
+{
+	int err = 0;
+	struct sched_param param = { .sched_priority = 1 };
+	bh_printk(KERN_DEBUG "[BH] register.\n");
+	BUG_ON(priv->bh_thread);
+	atomic_set(&priv->bh_rx, 0);
+	atomic_set(&priv->bh_tx, 0);
+	atomic_set(&priv->bh_term, 0);
+	priv->buf_id_tx = 0;
+	priv->buf_id_rx = 0;
+	init_waitqueue_head(&priv->bh_wq);
+	init_waitqueue_head(&priv->hw_bufs_used_wq);
+	priv->bh_thread = kthread_create(&cw1200_bh, priv, "cw1200_bh");
+	if (IS_ERR(priv->bh_thread)) {
+		err = PTR_ERR(priv->bh_thread);
+		priv->bh_thread = NULL;
+	} else {
+		WARN_ON(sched_setscheduler(priv->bh_thread,
+			SCHED_FIFO, &param));
+#ifdef HAS_PUT_TASK_STRUCT
+		get_task_struct(priv->bh_thread);
+#endif
+		wake_up_process(priv->bh_thread);
+	}
+	return err;
+}
+
+void cw1200_unregister_bh(struct cw1200_common *priv)
+{
+	struct task_struct *thread = priv->bh_thread;
+	if (WARN_ON(!thread))
+		return;
+
+	priv->bh_thread = NULL;
+	bh_printk(KERN_DEBUG "[BH] unregister.\n");
+	atomic_add(1, &priv->bh_term);
+	wake_up_interruptible(&priv->bh_wq);
+	kthread_stop(thread);
+#ifdef HAS_PUT_TASK_STRUCT
+	put_task_struct(thread);
+#endif
+}
+
+void cw1200_irq_handler(struct cw1200_common *priv)
+{
+	bh_printk(KERN_DEBUG "[BH] irq.\n");
+	if (/* WARN_ON */(priv->bh_error))
+		return;
+
+	if (atomic_add_return(1, &priv->bh_rx) == 1)
+		wake_up_interruptible(&priv->bh_wq);
+}
+
+void cw1200_bh_wakeup(struct cw1200_common *priv)
+{
+	bh_printk(KERN_DEBUG "[BH] wakeup.\n");
+	if (WARN_ON(priv->bh_error))
+		return;
+
+	if (atomic_add_return(1, &priv->bh_tx) == 1)
+		wake_up_interruptible(&priv->bh_wq);
+}
+
+static inline void wsm_alloc_tx_buffer(struct cw1200_common *priv)
+{
+	++priv->hw_bufs_used;
+}
+
+int wsm_release_tx_buffer(struct cw1200_common *priv, int count)
+{
+	int ret = 0;
+	int hw_bufs_used = priv->hw_bufs_used;
+
+	priv->hw_bufs_used -= count;
+	if (WARN_ON(priv->hw_bufs_used < 0))
+		ret = -1;
+	else if (hw_bufs_used >= priv->wsm_caps.numInpChBufs)
+		ret = 1;
+	if (!priv->hw_bufs_used)
+		wake_up_interruptible(&priv->hw_bufs_used_wq);
+	return ret;
+}
+
+static struct sk_buff *cw1200_get_skb(struct cw1200_common *priv, size_t len)
+{
+	struct sk_buff *skb;
+	size_t alloc_len = (len > SDIO_BLOCK_SIZE) ? len : SDIO_BLOCK_SIZE;
+
+	if (len > SDIO_BLOCK_SIZE || !priv->skb_cache) {
+		skb = dev_alloc_skb(alloc_len
+				+ WSM_TX_EXTRA_HEADROOM
+				+ 8  /* TKIP IV */
+				+ 12 /* TKIP ICV + MIC */
+				- 2  /* Piggyback */);
+		/* In AP mode RXed SKB can be looped back as a broadcast.
+		 * Here we reserve enough space for headers. */
+		skb_reserve(skb, WSM_TX_EXTRA_HEADROOM
+				+ 8 /* TKIP IV */
+				- WSM_RX_EXTRA_HEADROOM);
+	} else {
+		skb = priv->skb_cache;
+		priv->skb_cache = NULL;
+	}
+	return skb;
+}
+
+static void cw1200_put_skb(struct cw1200_common *priv, struct sk_buff *skb)
+{
+	if (priv->skb_cache)
+		dev_kfree_skb(skb);
+	else
+		priv->skb_cache = skb;
+}
+
+static int cw1200_bh_read_ctrl_reg(struct cw1200_common *priv,
+					  u16 *ctrl_reg)
+{
+	int ret;
+
+	ret = cw1200_reg_read_16(priv,
+			ST90TDS_CONTROL_REG_ID, ctrl_reg);
+	if (ret) {
+		ret = cw1200_reg_read_16(priv,
+				ST90TDS_CONTROL_REG_ID, ctrl_reg);
+		if (ret)
+			printk(KERN_ERR
+				"[BH] Failed to read control register.\n");
+		else
+			printk(KERN_WARNING
+				"[BH] Second attempt to read control "
+				"register passed. This is a firmware bug.\n");
+	}
+
+	return ret;
+}
+
+static int cw1200_device_wakeup(struct cw1200_common *priv)
+{
+	u16 ctrl_reg;
+	int ret;
+
+	bh_printk(KERN_DEBUG "[BH] Device wakeup.\n");
+
+	/* To force the device to be always-on, the host sets WLAN_UP to 1 */
+	ret = cw1200_reg_write_16(priv, ST90TDS_CONTROL_REG_ID,
+			ST90TDS_CONT_WUP_BIT);
+	if (WARN_ON(ret))
+		return ret;
+
+	ret = cw1200_bh_read_ctrl_reg(priv, &ctrl_reg);
+	if (WARN_ON(ret))
+		return ret;
+
+	/* If the device returns WLAN_RDY as 1, the device is active and will
+	 * remain active. */
+	if (ctrl_reg & ST90TDS_CONT_RDY_BIT) {
+		bh_printk(KERN_DEBUG "[BH] Device awake.\n");
+		return 1;
+	}
+
+	return 0;
+}
+
+/* Must be called from BH thraed. */
+void cw1200_enable_powersave(struct cw1200_common *priv,
+			     bool enable)
+{
+	bh_printk(KERN_DEBUG "[BH] Powerave is %s.\n",
+			enable ? "enabled" : "disabled");
+	priv->powersave_enabled = enable;
+}
+
+static int cw1200_bh(void *arg)
+{
+	struct cw1200_common *priv = arg;
+	struct sk_buff *skb_rx = NULL;
+	size_t read_len = 0;
+	int rx, tx, term;
+	struct wsm_hdr *wsm;
+	size_t wsm_len;
+	int wsm_id;
+	u8 wsm_seq;
+	int rx_resync = 1;
+	u16 ctrl_reg = 0;
+	int tx_allowed;
+	int pending_tx = 0;
+	long status;
+
+	for (;;) {
+		if (!priv->hw_bufs_used
+				&& priv->powersave_enabled
+				&& !priv->device_can_sleep)
+			status = 1 * HZ;
+		else
+			status = MAX_SCHEDULE_TIMEOUT;
+
+		status = wait_event_interruptible_timeout(priv->bh_wq, ({
+				rx = atomic_xchg(&priv->bh_rx, 0);
+				tx = atomic_xchg(&priv->bh_tx, 0);
+				term = atomic_xchg(&priv->bh_term, 0);
+				(rx || tx || term);
+			}), status);
+
+		if (status < 0 || term)
+			break;
+
+		if (!status) {
+			bh_printk(KERN_DEBUG "[BH] Device wakedown.\n");
+			WARN_ON(cw1200_reg_write_16(priv, ST90TDS_CONTROL_REG_ID, 0));
+			priv->device_can_sleep = true;
+			continue;
+		}
+
+		tx += pending_tx;
+		pending_tx = 0;
+
+		if (rx) {
+			size_t alloc_len;
+			u8 *data;
+
+			if (WARN_ON(cw1200_bh_read_ctrl_reg(
+					priv, &ctrl_reg)))
+				break;
+rx:
+			read_len = (ctrl_reg & ST90TDS_CONT_NEXT_LEN_MASK) * 2;
+			if (!read_len)
+				goto tx;
+
+			if (WARN_ON((read_len < sizeof(struct wsm_hdr)) ||
+					(read_len > EFFECTIVE_BUF_SIZE))) {
+				printk(KERN_DEBUG "Invalid read len: %d",
+					read_len);
+				break;
+			}
+
+			/* Add SIZE of PIGGYBACK reg (CONTROL Reg)
+			 * to the NEXT Message length + 2 Bytes for SKB */
+			read_len = read_len + 2;
+
+			BUG_ON(SDIO_BLOCK_SIZE & (SDIO_BLOCK_SIZE - 1));
+
+#if defined(CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES)
+			alloc_len = priv->sbus_ops->align_size(
+					priv->sbus_priv, read_len);
+#else /* CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES */
+			/* Platform's SDIO workaround */
+			alloc_len = read_len & ~(SDIO_BLOCK_SIZE - 1);
+			if (read_len & (SDIO_BLOCK_SIZE - 1))
+				alloc_len += SDIO_BLOCK_SIZE;
+#endif /* CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES */
+
+			skb_rx = cw1200_get_skb(priv, alloc_len);
+			if (WARN_ON(!skb_rx))
+				break;
+
+			skb_trim(skb_rx, 0);
+			skb_put(skb_rx, read_len);
+			data = skb_rx->data;
+			if (WARN_ON(!data))
+				break;
+
+			if (WARN_ON(cw1200_data_read(priv, data, alloc_len)))
+				break;
+
+			/* Piggyback */
+			ctrl_reg = __le16_to_cpu(
+				((__le16 *)data)[alloc_len / 2 - 1]);
+
+			wsm = (struct wsm_hdr *)data;
+			wsm_len = __le32_to_cpu(wsm->len);
+			if (WARN_ON(wsm_len > read_len))
+				break;
+
+#if defined(CONFIG_CW1200_WSM_DUMPS)
+			print_hex_dump_bytes("<-- ", DUMP_PREFIX_NONE,
+				data, wsm_len);
+#endif /* CONFIG_CW1200_WSM_DUMPS */
+
+			wsm_id  = __le32_to_cpu(wsm->id) & 0xFFF;
+			wsm_seq = (__le32_to_cpu(wsm->id) >> 13) & 7;
+
+			skb_trim(skb_rx, wsm_len);
+
+			if (unlikely(wsm_id == 0x0800)) {
+				wsm_handle_exception(priv,
+					 &data[sizeof(*wsm)],
+					wsm_len - sizeof(*wsm));
+				break;
+			} else if (unlikely(!rx_resync)) {
+				if (WARN_ON(wsm_seq != priv->wsm_rx_seq))
+					break;
+			}
+			priv->wsm_rx_seq = (wsm_seq + 1) & 7;
+			rx_resync = 0;
+
+			if (wsm_id & 0x0400) {
+				int rc = wsm_release_tx_buffer(priv, 1);
+				if (WARN_ON(rc < 0))
+					break;
+				else if (rc > 0)
+					tx = 1;
+			}
+
+			/* cw1200_wsm_rx takes care on SKB livetime */
+			if (WARN_ON(wsm_handle_rx(priv, wsm_id, wsm, &skb_rx)))
+				break;
+
+			if (skb_rx) {
+				cw1200_put_skb(priv, skb_rx);
+				skb_rx = NULL;
+			}
+
+			read_len = 0;
+		}
+
+tx:
+		/* HACK! One buffer is reserved for control path */
+		BUG_ON(priv->hw_bufs_used > priv->wsm_caps.numInpChBufs);
+		tx_allowed =
+			priv->hw_bufs_used < priv->wsm_caps.numInpChBufs;
+
+		if (tx && tx_allowed) {
+			size_t tx_len;
+			u8 *data;
+			int ret;
+
+	                if (priv->device_can_sleep) {
+				ret = cw1200_device_wakeup(priv);
+				if (WARN_ON(ret < 0))
+					break;
+				else if (ret)
+					priv->device_can_sleep = false;
+				else {
+					/* Wait for "awake" interrupt */
+					pending_tx = tx;
+					continue;
+				}
+			}
+
+			wsm_alloc_tx_buffer(priv);
+			ret = wsm_get_tx(priv, &data, &tx_len);
+			if (ret <= 0) {
+				wsm_release_tx_buffer(priv, 1);
+				if (WARN_ON(ret < 0))
+					break;
+			} else {
+				wsm = (struct wsm_hdr *)data;
+				BUG_ON(tx_len < sizeof(*wsm));
+				BUG_ON(__le32_to_cpu(wsm->len) != tx_len);
+
+#if 0 /* count is not implemented */
+				if (ret > 1)
+					atomic_add(1, &priv->bh_tx);
+#else
+				atomic_add(1, &priv->bh_tx);
+#endif
+
+
+#if defined(CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES)
+				tx_len = priv->sbus_ops->align_size(
+						priv->sbus_priv, tx_len);
+#else /* CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES */
+				/* HACK!!! Platform limitation.
+				* It is also supported by upper layer:
+				* there is always enough space at the
+				* end of the buffer. */
+				if (tx_len & (SDIO_BLOCK_SIZE - 1)) {
+					tx_len &= ~(SDIO_BLOCK_SIZE - 1);
+					tx_len += SDIO_BLOCK_SIZE;
+				}
+#endif /* CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES */
+
+				wsm->id |= __cpu_to_le32(
+					priv->wsm_tx_seq << 13);
+
+				if (WARN_ON(cw1200_data_write(priv,
+				    data, tx_len))) {
+					wsm_release_tx_buffer(priv, 1);
+					break;
+				}
+
+#if defined(CONFIG_CW1200_WSM_DUMPS)
+				print_hex_dump_bytes("--> ", DUMP_PREFIX_NONE,
+					data, __le32_to_cpu(wsm->len));
+#endif /* CONFIG_CW1200_WSM_DUMPS */
+
+				wsm_txed(priv, data);
+				priv->wsm_tx_seq = (priv->wsm_tx_seq + 1) & 7;
+			}
+		}
+
+		/* HACK!!! Device tends not to send interrupt
+		 * if this extra check is missing */
+		if (!(ctrl_reg & ST90TDS_CONT_NEXT_LEN_MASK)) {
+			if (WARN_ON(cw1200_bh_read_ctrl_reg(
+					priv, &ctrl_reg)))
+				break;
+		}
+
+		if (ctrl_reg & ST90TDS_CONT_NEXT_LEN_MASK)
+			goto rx;
+	}
+
+	if (skb_rx) {
+		cw1200_put_skb(priv, skb_rx);
+		skb_rx = NULL;
+	}
+
+
+	if (!term) {
+		cw1200_dbg(CW1200_DBG_ERROR, "[BH] Fatal error, exitting.\n");
+		priv->bh_error = 1;
+		/* TODO: schedule_work(recovery) */
+#ifndef HAS_PUT_TASK_STRUCT
+		/* The only reason of having this stupid code here is
+		 * that __put_task_struct is not exported by kernel. */
+		for (;;) {
+			int status = wait_event_interruptible(priv->bh_wq, ({
+				term = atomic_xchg(&priv->bh_term, 0);
+				(term);
+				}));
+
+			if (status || term)
+				break;
+		}
+#endif
+	}
+	return 0;
+}
diff --git a/drivers/staging/cw1200/bh.h b/drivers/staging/cw1200/bh.h
new file mode 100644
index 00000000000..14b641fbe1d
--- /dev/null
+++ b/drivers/staging/cw1200/bh.h
@@ -0,0 +1,30 @@
+/*
+ * Device handling thread interface for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_BH_H
+#define CW1200_BH_H
+
+/* extern */ struct cw1200_common;
+
+/* TODO: 512, actually. Was increased to 1024
+ * for compatibility with particular FW. */
+#define SDIO_BLOCK_SIZE (1024)
+
+int cw1200_register_bh(struct cw1200_common *priv);
+void cw1200_unregister_bh(struct cw1200_common *priv);
+void cw1200_irq_handler(struct cw1200_common *priv);
+void cw1200_bh_wakeup(struct cw1200_common *priv);
+/* Must be called from BH thread. */
+void cw1200_enable_powersave(struct cw1200_common *priv,
+			     bool enable);
+int wsm_release_tx_buffer(struct cw1200_common *priv, int count);
+
+#endif /* CW1200_BH_H */
diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
new file mode 100644
index 00000000000..cd5061d1097
--- /dev/null
+++ b/drivers/staging/cw1200/cw1200.h
@@ -0,0 +1,225 @@
+/*
+ * Common private data for ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on the mac80211 Prism54 code, which is
+ * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ *
+ * Based on the islsm (softmac prism54) driver, which is:
+ * Copyright 2004-2006 Jean-Baptiste Note <jbnote@gmail.com>, et al.
+ *
+ * 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 CW1200_H
+#define CW1200_H
+
+#include <linux/wait.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/atomic.h>
+#include <net/mac80211.h>
+
+#include "queue.h"
+#include "wsm.h"
+#include "scan.h"
+#include "txrx.h"
+#include "ht.h"
+
+/* extern */ struct sbus_ops;
+/* extern */ struct task_struct;
+/* extern */ struct cw1200_debug_priv;
+
+#if defined(CONFIG_CW1200_TXRX_DEBUG)
+#define txrx_printk(...) printk(__VA_ARGS__)
+#else
+#define txrx_printk(...)
+#endif
+
+#define CW1200_MAX_CTRL_FRAME_LEN	(0x1000)
+
+#define CW1200_MAX_STA_IN_AP_MODE	(5)
+#define CW1200_LINK_ID_AFTER_DTIM	(CW1200_MAX_STA_IN_AP_MODE + 1)
+
+enum cw1200_join_status {
+	CW1200_JOIN_STATUS_PASSIVE = 0,
+	CW1200_JOIN_STATUS_MONITOR,
+	CW1200_JOIN_STATUS_STA,
+	CW1200_JOIN_STATUS_AP,
+};
+
+struct cw1200_common {
+	struct cw1200_queue		tx_queue[4];
+	struct cw1200_queue_stats	tx_queue_stats;
+	struct cw1200_debug_priv	*debug;
+
+	struct ieee80211_hw		*hw;
+	struct ieee80211_vif		*vif;
+	struct device			*pdev;
+	struct workqueue_struct		*workqueue;
+
+	struct mutex			conf_mutex;
+
+	const struct sbus_ops		*sbus_ops;
+	struct sbus_priv		*sbus_priv;
+
+	/* HW type (HIF_...) */
+	int				hw_type;
+	int				hw_revision;
+
+	/* firmware/hardware info */
+	unsigned int tx_hdr_len;
+
+	/* Radio data */
+	int output_power;
+	int noise;
+
+	/* calibration, output power limit and rssi<->dBm conversation data */
+
+	/* BBP/MAC state */
+	struct ieee80211_rate		*rates;
+	struct ieee80211_rate		*mcs_rates;
+	u8 mac_addr[ETH_ALEN];
+	struct ieee80211_channel	*channel;
+	u8 bssid[ETH_ALEN];
+	struct wsm_edca_params		edca;
+	struct wsm_association_mode	association_mode;
+	struct wsm_set_bss_params	bss_params;
+	struct cw1200_ht_info		ht_info;
+	struct wsm_set_pm		powersave_mode;
+	int				cqm_rssi_thold;
+	unsigned			cqm_rssi_hyst;
+	unsigned			cqm_tx_failure_thold;
+	unsigned			cqm_tx_failure_count;
+	int				cqm_link_loss_count;
+	int				cqm_beacon_loss_count;
+	int				channel_switch_in_progress;
+	wait_queue_head_t		channel_switch_done;
+	u8				long_frame_max_tx_count;
+	u8				short_frame_max_tx_count;
+	int				mode;
+	bool				enable_beacon;
+	size_t				ssid_length;
+	u8				ssid[IEEE80211_MAX_SSID_LEN];
+	bool				listening;
+	struct wsm_rx_filter		rx_filter;
+	struct wsm_beacon_filter_control bf_control;
+	u8				ba_tid_mask;
+
+	/* BH */
+	atomic_t			bh_rx;
+	atomic_t			bh_tx;
+	atomic_t			bh_term;
+	struct task_struct		*bh_thread;
+	int				bh_error;
+	wait_queue_head_t		bh_wq;
+	int				buf_id_tx;	/* byte */
+	int				buf_id_rx;	/* byte */
+	int				wsm_rx_seq;	/* byte */
+	int				wsm_tx_seq;	/* byte */
+	int				hw_bufs_used;
+	wait_queue_head_t		hw_bufs_used_wq;
+	struct sk_buff			*skb_cache;
+	bool				powersave_enabled;
+	bool				device_can_sleep;
+
+	/* WSM */
+	struct wsm_caps			wsm_caps;
+	struct mutex			wsm_cmd_mux;
+	struct wsm_buf			wsm_cmd_buf;
+	struct wsm_cmd			wsm_cmd;
+	wait_queue_head_t		wsm_cmd_wq;
+	wait_queue_head_t		wsm_startup_done;
+	struct wsm_cbc			wsm_cbc;
+	atomic_t			tx_lock;
+
+	/* Scan status */
+	struct cw1200_scan scan;
+
+	/* WSM Join */
+	enum cw1200_join_status	join_status;
+	u8			join_bssid[ETH_ALEN];
+	const struct wsm_tx	*join_pending_frame;
+	struct work_struct	join_work;
+	struct delayed_work	join_timeout;
+	struct work_struct	unjoin_work;
+	int			join_dtim_period;
+
+	/* TX/RX and security */
+	s8			wep_default_key_id;
+	struct work_struct	wep_key_work;
+	u32			key_map;
+	struct wsm_add_key	keys[WSM_KEY_MAX_INDEX + 1];
+	unsigned long		rx_timestamp;
+
+	/* AP powersave */
+	u32			link_id_map;
+	u32			tx_suspend_mask[4];
+	u32			sta_asleep_mask;
+	bool			suspend_multicast;
+	struct work_struct	set_tim_work;
+	struct work_struct	multicast_start_work;
+	struct work_struct	multicast_stop_work;
+
+
+	/* WSM events and CQM implementation */
+	spinlock_t		event_queue_lock;
+	struct list_head	event_queue;
+	struct work_struct	event_handler;
+	struct delayed_work	bss_loss_work;
+	struct delayed_work	connection_loss_work;
+#if defined(CONFIG_CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE)
+	struct delayed_work	keep_alive_work;
+	unsigned long		last_activity_time;
+#endif /* CONFIG_CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE */
+	struct work_struct	tx_failure_work;
+	int			delayed_link_loss;
+
+	/* TX rate policy cache */
+	struct tx_policy_cache tx_policy_cache;
+	struct work_struct tx_policy_upload_work;
+
+	/* cryptographic engine information */
+
+	/* bit field of glowing LEDs */
+	u16 softled_state;
+
+	/* statistics */
+	struct ieee80211_low_level_stats stats;
+};
+
+struct cw1200_sta_priv {
+        int link_id;
+};
+
+/* interfaces for the drivers */
+int cw1200_probe(const struct sbus_ops *sbus_ops,
+		 struct sbus_priv *sbus,
+		 struct device *pdev,
+		 struct cw1200_common **pself);
+void cw1200_release(struct cw1200_common *self);
+
+#define CW1200_DBG_MSG		0x00000001
+#define CW1200_DBG_NIY		0x00000002
+#define CW1200_DBG_SBUS		0x00000004
+#define CW1200_DBG_INIT		0x00000008
+#define CW1200_DBG_ERROR	0x00000010
+#define CW1200_DBG_LEVEL	0xFFFFFFFF
+
+#define cw1200_dbg(level, ...)				\
+	do {						\
+		if ((level) & CW1200_DBG_LEVEL)		\
+			printk(KERN_DEBUG __VA_ARGS__);	\
+	} while (0)
+
+#define STUB()						\
+	do {						\
+		cw1200_dbg(CW1200_DBG_NIY, "%s: STUB at line %d.\n", \
+		__func__, __LINE__);			\
+	} while (0)
+
+#endif /* CW1200_H */
diff --git a/drivers/staging/cw1200/cw1200_plat.h b/drivers/staging/cw1200/cw1200_plat.h
new file mode 100644
index 00000000000..ed354de0edf
--- /dev/null
+++ b/drivers/staging/cw1200/cw1200_plat.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/ioport.h>
+
+struct cw1200_platform_data {
+	const char *mmc_id;
+	const struct resource *irq;
+	const struct resource *reset;
+	int (*power_ctrl)(const struct cw1200_platform_data *pdata,
+			  bool enable);
+};
+
+/* Declaration only. Should be implemented in arch/xxx/mach-yyy */
+const struct cw1200_platform_data *cw1200_get_platform_data(void);
diff --git a/drivers/staging/cw1200/cw1200_sdio.c b/drivers/staging/cw1200/cw1200_sdio.c
new file mode 100644
index 00000000000..e106129f4a3
--- /dev/null
+++ b/drivers/staging/cw1200/cw1200_sdio.c
@@ -0,0 +1,416 @@
+/*
+ * Mac80211 SDIO driver for ST-Ericsson CW1200 device
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/sdio.h>
+#include <linux/spinlock.h>
+#include <asm/mach-types.h>
+#include <net/mac80211.h>
+
+#include "cw1200.h"
+#include "sbus.h"
+#include "cw1200_plat.h"
+
+MODULE_AUTHOR("Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>");
+MODULE_DESCRIPTION("mac80211 ST-Ericsson CW1200 SDIO driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("cw1200_wlan");
+
+struct sbus_priv {
+	struct sdio_func	*func;
+	struct cw1200_common	*core;
+	const struct cw1200_platform_data *pdata;
+	spinlock_t		lock;
+	sbus_irq_handler	irq_handler;
+	void			*irq_priv;
+};
+
+static const struct sdio_device_id if_sdio_ids[] = {
+	{ SDIO_DEVICE(SDIO_ANY_ID, SDIO_ANY_ID) },
+	{ /* end: all zeroes */			},
+};
+
+/* sbus_ops implemetation */
+
+static int cw1200_sdio_memcpy_fromio(struct sbus_priv *self,
+				     unsigned int addr,
+				     void *dst, int count)
+{
+	int ret = sdio_memcpy_fromio(self->func, dst, addr, count);
+	if (ret) {
+		printk(KERN_ERR "!!! Can't read %d bytes from 0x%.8X. Err %d.\n",
+				count, addr, ret);
+	}
+	return ret;
+}
+
+static int cw1200_sdio_memcpy_toio(struct sbus_priv *self,
+				   unsigned int addr,
+				   const void *src, int count)
+{
+	return sdio_memcpy_toio(self->func, addr, (void *)src, count);
+}
+
+static void cw1200_sdio_lock(struct sbus_priv *self)
+{
+	sdio_claim_host(self->func);
+}
+
+static void cw1200_sdio_unlock(struct sbus_priv *self)
+{
+	sdio_release_host(self->func);
+}
+
+#ifndef CONFIG_CW1200_USE_GPIO_IRQ
+static void cw1200_sdio_irq_handler(struct sdio_func *func)
+{
+	struct sbus_priv *self = sdio_get_drvdata(func);
+	unsigned long flags;
+
+	BUG_ON(!self);
+	spin_lock_irqsave(&self->lock, flags);
+	if (self->irq_handler)
+		self->irq_handler(self->irq_priv);
+	spin_unlock_irqrestore(&self->lock, flags);
+}
+#else /* CONFIG_CW1200_USE_GPIO_IRQ */
+static irqreturn_t cw1200_gpio_irq_handler(int irq, void *dev_id)
+{
+	struct sbus_priv *self = dev_id;
+
+	BUG_ON(!self);
+	if (self->irq_handler)
+		self->irq_handler(self->irq_priv);
+	return IRQ_HANDLED;
+}
+
+static int cw1200_request_irq(struct sbus_priv *self,
+			      irq_handler_t handler)
+{
+	int ret;
+	int func_num;
+	const struct resource *irq = self->pdata->irq;
+	u8 cccr;
+
+	ret = request_any_context_irq(irq->start, handler,
+			IRQF_TRIGGER_RISING, irq->name, self);
+	if (WARN_ON(ret < 0))
+		goto exit;
+
+	ret = enable_irq_wake(irq->start);
+	if (WARN_ON(ret))
+		goto free_irq;
+
+	/* Hack to access Fuction-0 */
+	func_num = self->func->num;
+	self->func->num = 0;
+
+	cccr = sdio_readb(self->func, SDIO_CCCR_IENx, &ret);
+	if (WARN_ON(ret))
+		goto set_func;
+
+	/* Master interrupt enable ... */
+	cccr |= 1;
+
+	/* ... for our function */
+	cccr |= 1 << func_num;
+
+	sdio_writeb(self->func, cccr, SDIO_CCCR_IENx, &ret);
+	if (WARN_ON(ret))
+		goto set_func;
+
+	/* Restore the WLAN function number */
+	self->func->num = func_num;
+	return 0;
+
+set_func:
+	self->func->num = func_num;
+	disable_irq_wake(irq->start);
+free_irq:
+	free_irq(irq->start, self);
+exit:
+	return ret;
+}
+#endif /* CONFIG_CW1200_USE_GPIO_IRQ */
+
+static int cw1200_sdio_irq_subscribe(struct sbus_priv *self,
+				     sbus_irq_handler handler,
+				     void *priv)
+{
+	int ret;
+	unsigned long flags;
+
+	if (!handler)
+		return -EINVAL;
+
+	spin_lock_irqsave(&self->lock, flags);
+	self->irq_priv = priv;
+	self->irq_handler = handler;
+	spin_unlock_irqrestore(&self->lock, flags);
+
+	printk(KERN_DEBUG "SW IRQ subscribe\n");
+	sdio_claim_host(self->func);
+#ifndef CONFIG_CW1200_USE_GPIO_IRQ
+	ret = sdio_claim_irq(self->func, cw1200_sdio_irq_handler);
+#else
+	ret = cw1200_request_irq(self, cw1200_gpio_irq_handler);
+#endif
+	sdio_release_host(self->func);
+	return ret;
+}
+
+static int cw1200_sdio_irq_unsubscribe(struct sbus_priv *self)
+{
+	int ret = 0;
+	unsigned long flags;
+#ifdef CONFIG_CW1200_USE_GPIO_IRQ
+	const struct resource *irq = self->pdata->irq;
+#endif
+
+	WARN_ON(!self->irq_handler);
+	if (!self->irq_handler)
+		return 0;
+
+	printk(KERN_DEBUG "SW IRQ unsubscribe\n");
+#ifndef CONFIG_CW1200_USE_GPIO_IRQ
+	sdio_claim_host(self->func);
+	ret = sdio_release_irq(self->func);
+	sdio_release_host(self->func);
+#else
+	disable_irq_wake(irq->start);
+	free_irq(irq->start, self);
+#endif
+
+	spin_lock_irqsave(&self->lock, flags);
+	self->irq_priv = NULL;
+	self->irq_handler = NULL;
+	spin_unlock_irqrestore(&self->lock, flags);
+
+	return ret;
+}
+
+static int cw1200_detect_card(const struct cw1200_platform_data *pdata)
+{
+	/* HACK!!!
+	 * Rely on mmc->class_dev.class set in mmc_alloc_host
+	 * Tricky part: a new mmc hook is being (temporary) created
+	 * to discover mmc_host class.
+	 * Do you know more elegant way how to enumerate mmc_hosts?
+	 */
+
+	struct mmc_host *mmc = NULL;
+	struct class_dev_iter iter;
+	struct device *dev;
+
+	mmc = mmc_alloc_host(0, NULL);
+	if (!mmc)
+		return -ENOMEM;
+
+	BUG_ON(!mmc->class_dev.class);
+	class_dev_iter_init(&iter, mmc->class_dev.class, NULL, NULL);
+	for (;;) {
+		dev = class_dev_iter_next(&iter);
+		if (!dev) {
+			printk(KERN_ERR "cw1200: %s is not found.\n",
+				pdata->mmc_id);
+			break;
+		} else {
+			struct mmc_host *host = container_of(dev,
+				struct mmc_host, class_dev);
+
+			if (dev_name(&host->class_dev) &&
+				strcmp(dev_name(&host->class_dev),
+					pdata->mmc_id))
+				continue;
+
+			mmc_detect_change(host, 10);
+			break;
+		}
+	}
+	mmc_free_host(mmc);
+	return 0;
+}
+
+static int cw1200_sdio_off(const struct cw1200_platform_data *pdata)
+{
+	const struct resource *reset = pdata->reset;
+	gpio_set_value(reset->start, 0);
+	cw1200_detect_card(pdata);
+	gpio_free(reset->start);
+	return 0;
+}
+
+static int cw1200_sdio_on(const struct cw1200_platform_data *pdata)
+{
+	const struct resource *reset = pdata->reset;
+	gpio_request(reset->start, reset->name);
+	gpio_direction_output(reset->start, 1);
+	/* It is not stated in the datasheet, but at least some of devices
+	 * have problems with reset if this stage is omited. */
+	msleep(50);
+	gpio_set_value(reset->start, 0);
+	/* A valid reset shall be obtained by maintaining WRESETN
+	 * active (low) for at least two cycles of LP_CLK after VDDIO
+	 * is stable within it operating range. */
+	msleep(1);
+	gpio_set_value(reset->start, 1);
+	/* The host should wait 30 ms after the WRESETN release
+	 * for the on-chip LDO to stabilize */
+	msleep(30);
+	cw1200_detect_card(pdata);
+	return 0;
+}
+
+static int cw1200_sdio_reset(struct sbus_priv *self)
+{
+	cw1200_sdio_off(self->pdata);
+	msleep(1000);
+	cw1200_sdio_on(self->pdata);
+	return 0;
+}
+
+static size_t cw1200_align_size(struct sbus_priv *self, size_t size)
+{
+	size_t aligned = sdio_align_size(self->func, size);
+	/* HACK!!! Problems with DMA size on u8500 platform  */
+	if ((aligned & 0x1F) && (aligned & ~0x1F)) {
+		aligned &= ~0x1F;
+		aligned += 0x20;
+	}
+
+	return aligned;
+}
+
+static struct sbus_ops cw1200_sdio_sbus_ops = {
+	.sbus_memcpy_fromio	= cw1200_sdio_memcpy_fromio,
+	.sbus_memcpy_toio	= cw1200_sdio_memcpy_toio,
+	.lock			= cw1200_sdio_lock,
+	.unlock			= cw1200_sdio_unlock,
+	.irq_subscribe		= cw1200_sdio_irq_subscribe,
+	.irq_unsubscribe	= cw1200_sdio_irq_unsubscribe,
+	.reset			= cw1200_sdio_reset,
+	.align_size		= cw1200_align_size,
+};
+
+/* Probe Function to be called by SDIO stack when device is discovered */
+static int cw1200_sdio_probe(struct sdio_func *func,
+			      const struct sdio_device_id *id)
+{
+	struct sbus_priv *self;
+	int status;
+
+	cw1200_dbg(CW1200_DBG_INIT, "Probe called\n");
+
+	self = kzalloc(sizeof(*self), GFP_KERNEL);
+	if (!self) {
+		cw1200_dbg(CW1200_DBG_ERROR, "Can't allocate SDIO sbus_priv.");
+		return -ENOMEM;
+	}
+
+	spin_lock_init(&self->lock);
+	self->pdata = cw1200_get_platform_data();
+	self->func = func;
+	sdio_set_drvdata(func, self);
+	sdio_claim_host(func);
+	sdio_enable_func(func);
+	sdio_release_host(func);
+
+	status = cw1200_probe(&cw1200_sdio_sbus_ops,
+			      self, &func->dev, &self->core);
+	if (status) {
+		sdio_claim_host(func);
+		sdio_disable_func(func);
+		sdio_release_host(func);
+		sdio_set_drvdata(func, NULL);
+		kfree(self);
+	}
+
+	return status;
+}
+
+/* Disconnect Function to be called by SDIO stack when device is disconnected */
+static void cw1200_sdio_disconnect(struct sdio_func *func)
+{
+	struct sbus_priv *self = sdio_get_drvdata(func);
+
+	if (self) {
+		if (self->core) {
+			cw1200_release(self->core);
+			self->core = NULL;
+		}
+		sdio_claim_host(func);
+		sdio_disable_func(func);
+		sdio_release_host(func);
+		sdio_set_drvdata(func, NULL);
+		kfree(self);
+	}
+}
+
+static struct sdio_driver sdio_driver = {
+	.name		= "cw1200_wlan",
+	.id_table	= if_sdio_ids,
+	.probe		= cw1200_sdio_probe,
+	.remove		= cw1200_sdio_disconnect,
+};
+
+/* Init Module function -> Called by insmod */
+static int __init cw1200_sdio_init(void)
+{
+	const struct cw1200_platform_data *pdata;
+	int ret;
+
+	pdata = cw1200_get_platform_data();
+
+	ret = sdio_register_driver(&sdio_driver);
+	if (ret)
+		goto err_reg;
+
+	if (pdata->power_ctrl) {
+		ret = pdata->power_ctrl(pdata, true);
+		if (ret)
+			goto err_power;
+	}
+
+	ret = cw1200_sdio_on(pdata);
+	if (ret)
+		goto err_on;
+
+	return 0;
+
+err_on:
+	if (pdata->power_ctrl)
+		pdata->power_ctrl(pdata, false);
+err_power:
+	sdio_unregister_driver(&sdio_driver);
+err_reg:
+	return ret;
+}
+
+/* Called at Driver Unloading */
+static void __exit cw1200_sdio_exit(void)
+{
+	const struct cw1200_platform_data *pdata;
+	pdata = cw1200_get_platform_data();
+	sdio_unregister_driver(&sdio_driver);
+	cw1200_sdio_off(pdata);
+	if (pdata->power_ctrl)
+		pdata->power_ctrl(pdata, false);
+}
+
+
+module_init(cw1200_sdio_init);
+module_exit(cw1200_sdio_exit);
diff --git a/drivers/staging/cw1200/debug.c b/drivers/staging/cw1200/debug.c
new file mode 100644
index 00000000000..13c4570372f
--- /dev/null
+++ b/drivers/staging/cw1200/debug.c
@@ -0,0 +1,317 @@
+/*
+ * mac80211 glue code for mac80211 ST-Ericsson CW1200 drivers
+ * DebugFS code
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include "cw1200.h"
+#include "debug.h"
+
+/* join_status */
+static const char * const cw1200_debug_join_status[] = {
+	"passive",
+	"monitor",
+	"station",
+	"access point",
+};
+
+/* WSM_JOIN_PREAMBLE_... */
+static const char * const cw1200_debug_preamble[] = {
+	"long",
+	"short",
+	"long on 1 and 2 Mbps",
+};
+
+static const char * const cw1200_debug_fw_types[] = {
+	"ETF",
+	"WFM",
+	"WSM",
+	"HI test",
+	"Platform test",
+};
+
+
+static const char *cw1200_debug_mode(int mode)
+{
+	switch (mode) {
+	case NL80211_IFTYPE_UNSPECIFIED:
+		return "unspecified";
+	case NL80211_IFTYPE_MONITOR:
+		return "monitor";
+	case NL80211_IFTYPE_STATION:
+		return "station";
+	case NL80211_IFTYPE_ADHOC:
+		return "ad-hok";
+	case NL80211_IFTYPE_MESH_POINT:
+		return "mesh point";
+	case NL80211_IFTYPE_AP:
+		return "access point";
+	case NL80211_IFTYPE_P2P_CLIENT:
+		return "p2p client";
+	case NL80211_IFTYPE_P2P_GO:
+		return "p2p go";
+	default:
+		return "unsupported";
+	}
+}
+
+static void cw1200_queue_status_show(struct seq_file *seq,
+				     struct cw1200_queue *q)
+{
+	int i;
+	seq_printf(seq, "Queue       %d:\n", q->queue_id);
+	seq_printf(seq, "  capacity: %d\n", q->capacity);
+	seq_printf(seq, "  queued:   %d\n", q->num_queued);
+	seq_printf(seq, "  pending:  %d\n", q->num_pending);
+	seq_printf(seq, "  sent:     %d\n", q->num_sent);
+	seq_printf(seq, "  locked:   %s\n", q->tx_locked_cnt ? "yes" : "no");
+	seq_printf(seq, "  overfull: %s\n", q->overfull ? "yes" : "no");
+	seq_puts(seq,   "  link map: 0-> ");
+	for (i = 0; i < q->stats->map_capacity; ++i)
+		seq_printf(seq, "%.2d ", q->link_map_cache[i]);
+	seq_printf(seq, "<-%d\n", q->stats->map_capacity);
+}
+
+static void cw1200_debug_print_map(struct seq_file *seq,
+				   struct cw1200_common *priv,
+				   const char *label,
+				   u32 map)
+{
+	int i;
+	seq_printf(seq, "%s0-> ", label);
+	for (i = 0; i < priv->tx_queue_stats.map_capacity; ++i)
+		seq_printf(seq, "%s ", (map & BIT(i)) ? "**" : "..");
+	seq_printf(seq, "<-%d\n", priv->tx_queue_stats.map_capacity);
+}
+
+static int cw1200_status_show(struct seq_file *seq, void *v)
+{
+	int i;
+	struct cw1200_common *priv = seq->private;
+	struct cw1200_debug_priv *d = priv->debug;
+	seq_puts(seq,   "CW1200 Wireless LAN driver status\n");
+	seq_printf(seq, "Hardware:   %d.%d\n",
+		priv->wsm_caps.hardwareId,
+		priv->wsm_caps.hardwareSubId);
+	seq_printf(seq, "Firmware:   %s %d.%d\n",
+		cw1200_debug_fw_types[priv->wsm_caps.firmwareType],
+		priv->wsm_caps.firmwareVersion,
+		priv->wsm_caps.firmwareBuildNumber);
+	seq_printf(seq, "FW API:     %d\n",
+		priv->wsm_caps.firmwareApiVer);
+	seq_printf(seq, "FW caps:    0x%.4X\n",
+		priv->wsm_caps.firmwareCap);
+	seq_printf(seq, "Mode:       %s%s\n",
+		cw1200_debug_mode(priv->mode),
+		priv->listening ? " (listening)" : "");
+	seq_printf(seq, "Assoc:      %s\n",
+		cw1200_debug_join_status[priv->join_status]);
+	if (priv->channel)
+		seq_printf(seq, "Channel:    %d%s\n",
+			priv->channel->hw_value,
+			priv->channel_switch_in_progress ?
+			" (switching)" : "");
+	if (priv->rx_filter.promiscuous)
+		seq_puts(seq,   "Filter:     promisc\n");
+	else if (priv->rx_filter.fcs)
+		seq_puts(seq,   "Filter:     fcs\n");
+	if (priv->rx_filter.bssid)
+		seq_puts(seq,   "Filter:     bssid\n");
+	if (priv->bf_control.bcn_count)
+		seq_puts(seq,   "Filter:     beacons\n");
+
+	if (priv->enable_beacon ||
+			priv->mode == NL80211_IFTYPE_AP ||
+			priv->mode == NL80211_IFTYPE_ADHOC ||
+			priv->mode == NL80211_IFTYPE_MESH_POINT ||
+			priv->mode == NL80211_IFTYPE_P2P_GO)
+		seq_printf(seq, "Beaconing:  %s\n",
+			priv->enable_beacon ?
+			"enabled" : "disabled");
+	if (priv->ssid_length ||
+			priv->mode == NL80211_IFTYPE_AP ||
+			priv->mode == NL80211_IFTYPE_ADHOC ||
+			priv->mode == NL80211_IFTYPE_MESH_POINT ||
+			priv->mode == NL80211_IFTYPE_P2P_GO)
+		seq_printf(seq, "SSID:       %.*s\n",
+			priv->ssid_length, priv->ssid);
+
+	for (i = 0; i < 4; ++i) {
+		seq_printf(seq, "EDCA(%d):    %d, %d, %d, %d, %d\n", i,
+			priv->edca.params[i].cwMin,
+			priv->edca.params[i].cwMax,
+			priv->edca.params[i].aifns,
+			priv->edca.params[i].txOpLimit,
+			priv->edca.params[i].maxReceiveLifetime);
+	}
+	if (priv->join_status == CW1200_JOIN_STATUS_STA) {
+		seq_printf(seq, "Preamble:   %s\n",
+			cw1200_debug_preamble[
+			priv->association_mode.preambleType]);
+		seq_printf(seq, "AMPDU spcn: %d\n",
+			priv->association_mode.mpduStartSpacing);
+		seq_printf(seq, "Basic rate: 0x%.8X\n",
+			le32_to_cpu(priv->association_mode.basicRateSet));
+		seq_printf(seq, "Bss lost:   %d beacons\n",
+			priv->bss_params.beaconLostCount);
+		seq_printf(seq, "AID:        %d\n",
+			priv->bss_params.aid);
+		seq_printf(seq, "Rates:      0x%.8X\n",
+			priv->bss_params.operationalRateSet);
+		seq_printf(seq, "Powersave:  %s\n",
+			priv->powersave_mode.pmMode ? "off" : "on");
+	}
+	seq_printf(seq, "HT:         %s\n",
+		cw1200_is_ht(&priv->ht_info) ? "on" : "off");
+	if (cw1200_is_ht(&priv->ht_info)) {
+		seq_printf(seq, "Greenfield: %s\n",
+			cw1200_ht_greenfield(&priv->ht_info) ? "yes" : "no");
+		seq_printf(seq, "AMPDU dens: %d\n",
+			cw1200_ht_ampdu_density(&priv->ht_info));
+	}
+	seq_printf(seq, "RSSI thold: %d\n",
+		priv->cqm_rssi_thold);
+	seq_printf(seq, "RSSI hyst:  %d\n",
+		priv->cqm_rssi_hyst);
+	seq_printf(seq, "TXFL thold: %d\n",
+		priv->cqm_tx_failure_thold);
+	seq_printf(seq, "Linkloss:   %d\n",
+		priv->cqm_link_loss_count);
+	seq_printf(seq, "Bcnloss:    %d\n",
+		priv->cqm_beacon_loss_count);
+	seq_printf(seq, "Long retr:  %d\n",
+		priv->long_frame_max_tx_count);
+	seq_printf(seq, "Short retr: %d\n",
+		priv->short_frame_max_tx_count);
+
+	seq_puts(seq, "\n");
+	for (i = 0; i < 4; ++i) {
+		char buf[32];
+		snprintf(buf, sizeof(buf), "TX lock(%d): ", i);
+		cw1200_queue_status_show(seq, &priv->tx_queue[i]);
+		cw1200_debug_print_map(seq, priv, buf,
+			priv->tx_suspend_mask[i]);
+		seq_puts(seq, "\n");
+	}
+
+	cw1200_debug_print_map(seq, priv, "Link map:   ",
+		priv->link_id_map);
+	cw1200_debug_print_map(seq, priv, "Asleep map: ",
+		priv->sta_asleep_mask);
+
+	seq_puts(seq, "\n");
+
+	seq_printf(seq, "BH status:  %s\n",
+		atomic_read(&priv->bh_term) ? "terminated" : "alive");
+	seq_printf(seq, "Pending RX: %d\n",
+		atomic_read(&priv->bh_rx));
+	seq_printf(seq, "Pending TX: %d\n",
+		atomic_read(&priv->bh_tx));
+	if (priv->bh_error)
+		seq_printf(seq, "BH errcode: %d\n",
+			priv->bh_error);
+	seq_printf(seq, "TX bufs:    %d x %d bytes\n",
+		priv->wsm_caps.numInpChBufs,
+		priv->wsm_caps.sizeInpChBuf);
+	seq_printf(seq, "Used bufs:  %d\n",
+		priv->hw_bufs_used);
+	seq_printf(seq, "Powermgmt:  %s\n",
+		priv->powersave_enabled ? "on" : "off");
+	seq_printf(seq, "Device:     %s\n",
+		priv->device_can_sleep ? "alseep" : "awake");
+
+	spin_lock(&priv->wsm_cmd.lock);
+	seq_printf(seq, "WSM status: %s\n",
+		priv->wsm_cmd.done ? "idle" : "active");
+	seq_printf(seq, "WSM cmd:    0x%.4X (%d bytes)\n",
+		priv->wsm_cmd.cmd, priv->wsm_cmd.len);
+	seq_printf(seq, "WSM retval: %d\n",
+		priv->wsm_cmd.ret);
+	spin_unlock(&priv->wsm_cmd.lock);
+
+	seq_printf(seq, "Datapath:   %s\n",
+		atomic_read(&priv->tx_lock) ? "locked" : "unlocked");
+	if (atomic_read(&priv->tx_lock))
+		seq_printf(seq, "TXlock cnt: %d\n",
+			atomic_read(&priv->tx_lock));
+
+	seq_printf(seq, "TXed:       %d\n",
+		d->tx);
+	seq_printf(seq, "AGG TXed:   %d\n",
+		d->tx_agg);
+	seq_printf(seq, "MULTI TXed: %d (%d)\n",
+		d->tx_multi, d->tx_multi_frames);
+	seq_printf(seq, "RXed:       %d\n",
+		d->rx);
+	seq_printf(seq, "AGG RXed:   %d\n",
+		d->rx_agg);
+	seq_printf(seq, "TX miss:    %d\n",
+		d->tx_cache_miss);
+	seq_printf(seq, "TX copy:    %d\n",
+		d->tx_copy);
+	seq_printf(seq, "Scan:       %s\n",
+		atomic_read(&priv->scan.in_progress) ? "active" : "idle");
+	seq_printf(seq, "Led state:  0x%.2X\n",
+		priv->softled_state);
+
+	return 0;
+}
+
+static int cw1200_status_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, &cw1200_status_show,
+		inode->i_private);
+}
+
+static const struct file_operations fops_status = {
+	.open = cw1200_status_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+int cw1200_debug_init(struct cw1200_common *priv)
+{
+	struct cw1200_debug_priv *d = kzalloc(sizeof(struct cw1200_debug_priv),
+			GFP_KERNEL);
+	priv->debug = d;
+	if (!d)
+		return -ENOMEM;
+
+	d->debugfs_phy = debugfs_create_dir("cw1200",
+			priv->hw->wiphy->debugfsdir);
+	if (!d->debugfs_phy)
+		goto err;
+
+	if (!debugfs_create_file("status", S_IRUSR, d->debugfs_phy,
+			priv, &fops_status))
+		goto err;
+
+	return 0;
+
+err:
+	priv->debug = NULL;
+	debugfs_remove_recursive(d->debugfs_phy);
+	kfree(d);
+	return -ENOMEM;
+}
+
+void cw1200_debug_release(struct cw1200_common *priv)
+{
+	struct cw1200_debug_priv *d = priv->debug;
+	priv->debug = NULL;
+
+	if (d) {
+		debugfs_remove_recursive(d->debugfs_phy);
+		kfree(d);
+	}
+}
diff --git a/drivers/staging/cw1200/debug.h b/drivers/staging/cw1200/debug.h
new file mode 100644
index 00000000000..e7fc4d2daef
--- /dev/null
+++ b/drivers/staging/cw1200/debug.h
@@ -0,0 +1,102 @@
+#ifndef CW1200_DEBUG_H_INCLUDED
+#define CW1200_DEBUG_H_INCLUDED
+
+struct cw200_common;
+
+#ifdef CONFIG_CW1200_DEBUGFS
+
+struct cw1200_debug_priv {
+	struct dentry *debugfs_phy;
+	int tx;
+	int tx_agg;
+	int rx;
+	int rx_agg;
+	int tx_multi;
+	int tx_multi_frames;
+	int tx_cache_miss;
+	int tx_copy;
+};
+
+int cw1200_debug_init(struct cw1200_common *priv);
+void cw1200_debug_release(struct cw1200_common *priv);
+
+static inline void cw1200_debug_txed(struct cw1200_common *priv)
+{
+	++priv->debug->tx;
+}
+
+static inline void cw1200_debug_txed_agg(struct cw1200_common *priv)
+{
+	++priv->debug->tx_agg;
+}
+
+static inline void cw1200_debug_txed_multi(struct cw1200_common *priv,
+					   int count)
+{
+	++priv->debug->tx_multi;
+	priv->debug->tx_multi_frames += count;
+}
+
+static inline void cw1200_debug_rxed(struct cw1200_common *priv)
+{
+	++priv->debug->rx;
+}
+
+static inline void cw1200_debug_rxed_agg(struct cw1200_common *priv)
+{
+	++priv->debug->rx_agg;
+}
+
+static inline void cw1200_debug_tx_cache_miss(struct cw1200_common *priv)
+{
+	++priv->debug->tx_cache_miss;
+}
+
+static inline void cw1200_debug_tx_copy(struct cw1200_common *priv)
+{
+	++priv->debug->tx_copy;
+}
+
+#else /* CONFIG_CW1200_DEBUGFS */
+
+static inline int cw1200_debug_init(struct cw1200_common *priv)
+{
+	return 0;
+}
+
+static inline void cw1200_debug_release(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_txed(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_txed_agg(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_txed_multi(struct cw1200_common *priv,
+					   int count)
+{
+}
+
+static inline void cw1200_debug_rxed(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_rxed_agg(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_tx_cache_miss(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_tx_copy(struct cw1200_common *priv)
+{
+}
+
+#endif /* CONFIG_CW1200_DEBUGFS */
+
+#endif /* CW1200_DEBUG_H_INCLUDED */
diff --git a/drivers/staging/cw1200/fwio.c b/drivers/staging/cw1200/fwio.c
new file mode 100644
index 00000000000..da6d3705e6c
--- /dev/null
+++ b/drivers/staging/cw1200/fwio.c
@@ -0,0 +1,594 @@
+/*
+ * Firmware I/O code for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * ST-Ericsson UMAC CW1200 driver which is
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Ajitpal Singh <ajitpal.singh@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/vmalloc.h>
+#include <linux/sched.h>
+#include <linux/firmware.h>
+
+#include "cw1200.h"
+#include "fwio.h"
+#include "hwio.h"
+#include "sbus.h"
+#include "bh.h"
+
+static int cw1200_get_hw_type(u32 config_reg_val, int *major_revision)
+{
+	int hw_type = -1;
+	u32 silicon_type = (config_reg_val >> 24) & 0x3;
+	u32 silicon_vers = (config_reg_val >> 31) & 0x1;
+
+	/* Check if we have CW1200 or STLC9000 */
+	if ((silicon_type == 0x1) || (silicon_type == 0x2)) {
+		*major_revision = silicon_type;
+		if (silicon_vers)
+			hw_type = HIF_8601_VERSATILE;
+		else
+			hw_type = HIF_8601_SILICON;
+	} else {
+		*major_revision = 1;
+		hw_type = HIF_9000_SILICON_VERSTAILE;
+	}
+
+	return hw_type;
+}
+
+static int config_reg_read_stlc9000(struct cw1200_common *priv,
+				    u16 reg, u32 *val)
+{
+	u16 val16;
+	int ret = cw1200_reg_read_16(priv, reg, &val16);
+	if (ret < 0)
+		return ret;
+	*val = val16;
+	return 0;
+}
+
+static int config_reg_write_stlc9000(struct cw1200_common *priv,
+				     u16 reg, u32 val)
+{
+	return cw1200_reg_write_16(priv, reg, (u16)val);
+}
+
+static int cw1200_load_firmware_cw1200(struct cw1200_common *priv)
+{
+	int ret, block, num_blocks;
+	unsigned i;
+	u32 val32;
+	u32 put = 0, get = 0;
+	u8 *buf = NULL;
+	const char *fw_path;
+	const struct firmware *firmware = NULL;
+
+	/* Macroses are local. */
+#define APB_WRITE(reg, val) \
+	do { \
+		ret = cw1200_apb_write_32(priv, CW12000_APB(reg), (val)); \
+		if (ret < 0) { \
+			cw1200_dbg(CW1200_DBG_ERROR, \
+				"%s: can't write %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+#define APB_READ(reg, val) \
+	do { \
+		ret = cw1200_apb_read_32(priv, CW12000_APB(reg), &(val)); \
+		if (ret < 0) { \
+			cw1200_dbg(CW1200_DBG_ERROR, \
+				"%s: can't read %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+#define REG_WRITE(reg, val) \
+	do { \
+		ret = cw1200_reg_write_32(priv, (reg), (val)); \
+		if (ret < 0) { \
+			cw1200_dbg(CW1200_DBG_ERROR, \
+				"%s: can't write %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+#define REG_READ(reg, val) \
+	do { \
+		ret = cw1200_reg_read_32(priv, (reg), &(val)); \
+		if (ret < 0) { \
+			cw1200_dbg(CW1200_DBG_ERROR, \
+				"%s: can't read %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+
+	switch (priv->hw_revision) {
+	case CW1200_HW_REV_CUT10:
+		fw_path = FIRMWARE_CUT10;
+		break;
+	case CW1200_HW_REV_CUT11:
+		fw_path = FIRMWARE_CUT11;
+		break;
+	case CW1200_HW_REV_CUT20:
+		fw_path = FIRMWARE_CUT20;
+		break;
+	case CW1200_HW_REV_CUT22:
+		fw_path = FIRMWARE_CUT22;
+		break;
+	default:
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: invalid silicon revision %d.\n",
+			__func__, priv->hw_revision);
+		return -EINVAL;
+	}
+
+	/* Initialize common registers */
+	APB_WRITE(DOWNLOAD_IMAGE_SIZE_REG, DOWNLOAD_ARE_YOU_HERE);
+	APB_WRITE(DOWNLOAD_PUT_REG, 0);
+	APB_WRITE(DOWNLOAD_GET_REG, 0);
+	APB_WRITE(DOWNLOAD_STATUS_REG, DOWNLOAD_PENDING);
+	APB_WRITE(DOWNLOAD_FLAGS_REG, 0);
+
+	/* Write the NOP Instruction */
+	REG_WRITE(ST90TDS_SRAM_BASE_ADDR_REG_ID, 0xFFF20000);
+	REG_WRITE(ST90TDS_AHB_DPORT_REG_ID, 0xEAFFFFFE);
+
+	/* Release CPU from RESET */
+	REG_READ(ST90TDS_CONFIG_REG_ID, val32);
+	val32 &= ~ST90TDS_CONFIG_CPU_RESET_BIT;
+	REG_WRITE(ST90TDS_CONFIG_REG_ID, val32);
+
+	/* Enable Clock */
+	val32 &= ~ST90TDS_CONFIG_CPU_CLK_DIS_BIT;
+	REG_WRITE(ST90TDS_CONFIG_REG_ID, val32);
+
+	/* Load a firmware file */
+	ret = request_firmware(&firmware, fw_path, priv->pdev);
+	if (ret) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't load firmware file %s.\n",
+			__func__, fw_path);
+		goto error;
+	}
+	BUG_ON(!firmware->data);
+
+	buf = kmalloc(DOWNLOAD_BLOCK_SIZE, GFP_KERNEL | GFP_DMA);
+	if (!buf) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't allocate firmware buffer.\n", __func__);
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	/* Check if the bootloader is ready */
+	for (i = 0; i < 100; i += 1 + i / 2) {
+		APB_READ(DOWNLOAD_IMAGE_SIZE_REG, val32);
+		if (val32 == DOWNLOAD_I_AM_HERE)
+			break;
+		mdelay(i);
+	} /* End of for loop */
+
+	if (val32 != DOWNLOAD_I_AM_HERE) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: bootloader is not ready.\n", __func__);
+		ret = -ETIMEDOUT;
+		goto error;
+	}
+
+	/* Calculcate number of download blocks */
+	num_blocks = (firmware->size - 1) / DOWNLOAD_BLOCK_SIZE + 1;
+
+	/* Updating the length in Download Ctrl Area */
+	val32 = firmware->size; /* Explicit cast from size_t to u32 */
+	APB_WRITE(DOWNLOAD_IMAGE_SIZE_REG, val32);
+
+	/* Firmware downloading loop */
+	for (block = 0; block < num_blocks ; block++) {
+		size_t tx_size;
+		size_t block_size;
+
+		/* check the download status */
+		APB_READ(DOWNLOAD_STATUS_REG, val32);
+		if (val32 != DOWNLOAD_PENDING) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: bootloader reported error %d.\n",
+				__func__, val32);
+			ret = -EIO;
+			goto error;
+		}
+
+		/* loop until put - get <= 24K */
+		for (i = 0; i < 100; i++) {
+			APB_READ(DOWNLOAD_GET_REG, get);
+			if ((put - get) <=
+			    (DOWNLOAD_FIFO_SIZE - DOWNLOAD_BLOCK_SIZE))
+				break;
+			mdelay(i);
+		}
+
+		if ((put - get) > (DOWNLOAD_FIFO_SIZE - DOWNLOAD_BLOCK_SIZE)) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Timeout waiting for FIFO.\n",
+				__func__);
+			return -ETIMEDOUT;
+		}
+
+		/* calculate the block size */
+		tx_size = block_size = min((size_t)(firmware->size - put),
+			(size_t)DOWNLOAD_BLOCK_SIZE);
+
+		memcpy(buf, &firmware->data[put], block_size);
+		if (block_size < DOWNLOAD_BLOCK_SIZE) {
+			memset(&buf[block_size],
+				0, DOWNLOAD_BLOCK_SIZE - block_size);
+			tx_size = DOWNLOAD_BLOCK_SIZE;
+		}
+
+		/* send the block to sram */
+		ret = cw1200_apb_write(priv,
+			CW12000_APB(DOWNLOAD_FIFO_OFFSET +
+				(put & (DOWNLOAD_FIFO_SIZE - 1))),
+			buf, tx_size);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: can't write block at line %d.\n",
+				__func__, __LINE__);
+			goto error;
+		}
+
+		/* update the put register */
+		put += block_size;
+		APB_WRITE(DOWNLOAD_PUT_REG, put);
+	} /* End of firmware download loop */
+
+	/* Wait for the download completion */
+	for (i = 0; i < 300; i += 1 + i / 2) {
+		APB_READ(DOWNLOAD_STATUS_REG, val32);
+		if (val32 != DOWNLOAD_PENDING)
+			break;
+		mdelay(i);
+	}
+	if (val32 != DOWNLOAD_SUCCESS) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: wait for download completion failed. " \
+			"Read: 0x%.8X\n", __func__, val32);
+		ret = -ETIMEDOUT;
+		goto error;
+	} else {
+		cw1200_dbg(CW1200_DBG_MSG,
+			"Firmware download completed.\n");
+		ret = 0;
+	}
+
+error:
+	kfree(buf);
+	if (firmware)
+		release_firmware(firmware);
+	return ret;
+
+#undef APB_WRITE
+#undef APB_READ
+#undef REG_WRITE
+#undef REG_READ
+}
+
+int cw1200_load_firmware(struct cw1200_common *priv)
+{
+	int ret;
+	int i;
+	u32 val32;
+	u16 val16;
+	u32 dpll = 0;
+	int major_revision;
+	int (*config_reg_read)(struct cw1200_common *priv, u16 reg, u32 *val);
+	int (*config_reg_write)(struct cw1200_common *priv, u16 reg, u32 val);
+
+	BUG_ON(!priv);
+
+	/* Read CONFIG Register Value - We will read 32 bits */
+	ret = cw1200_reg_read_32(priv, ST90TDS_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't read config register.\n", __func__);
+		goto out;
+	}
+
+	priv->hw_type = cw1200_get_hw_type(val32, &major_revision);
+	if (priv->hw_type < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't deduct hardware type.\n", __func__);
+		ret = -ENOTSUPP;
+		goto out;
+	}
+
+	switch (priv->hw_type) {
+	case HIF_8601_VERSATILE:
+	case HIF_8601_SILICON:
+		dpll = DPLL_INIT_VAL_CW1200;
+		config_reg_read = cw1200_reg_read_32;
+		config_reg_write = cw1200_reg_write_32;
+		break;
+	case HIF_9000_SILICON_VERSTAILE:
+		dpll = DPLL_INIT_VAL_9000;
+		config_reg_read = config_reg_read_stlc9000;
+		config_reg_write = config_reg_write_stlc9000;
+		break;
+	default:
+		BUG_ON(1);
+	}
+
+	ret = cw1200_reg_write_32(priv, ST90TDS_TSET_GEN_R_W_REG_ID, dpll);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't write DPLL register.\n", __func__);
+		goto out;
+	}
+
+	msleep(20);
+
+	/* Read DPLL Reg value and compare with value written */
+	ret = cw1200_reg_read_32(priv,
+		ST90TDS_TSET_GEN_R_W_REG_ID, &val32);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't read DPLL register.\n", __func__);
+		goto out;
+	}
+
+	if (val32 != dpll) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: unable to initialise " \
+			"DPLL register. Wrote 0x%.8X, read 0x%.8X.\n",
+			__func__, dpll, val32);
+		ret = -EIO;
+		goto out;
+	}
+
+	/* Set wakeup bit in device */
+	ret = cw1200_reg_read_16(priv, ST90TDS_CONTROL_REG_ID, &val16);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: set_wakeup: can't read " \
+			"control register.\n", __func__);
+		goto out;
+	}
+
+	ret = cw1200_reg_write_16(priv, ST90TDS_CONTROL_REG_ID,
+		val16 | ST90TDS_CONT_WUP_BIT);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: set_wakeup: can't write " \
+			"control register.\n", __func__);
+		goto out;
+	}
+
+	/* Wait for wakeup */
+	for (i = 0 ; i < 300 ; i += 1 + i / 2) {
+		ret = cw1200_reg_read_16(priv,
+			ST90TDS_CONTROL_REG_ID, &val16);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: wait_for_wakeup: can't read " \
+				"control register.\n", __func__);
+			goto out;
+		}
+
+		if (val16 & ST90TDS_CONT_RDY_BIT) {
+			cw1200_dbg(CW1200_DBG_MSG,
+				"WLAN device is ready.\n");
+			break;
+		}
+		msleep(i);
+	}
+
+	if ((val16 & ST90TDS_CONT_RDY_BIT) == 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: wait_for_wakeup: device is not responding.\n",
+			__func__);
+		ret = -ETIMEDOUT;
+		goto out;
+	}
+
+	if (major_revision == 1) {
+		/* CW1200 Hardware detection logic : Check for CUT1.1 */
+		ret = cw1200_ahb_read_32(priv, CW1200_CUT_ID_ADDR, &val32);
+		if (ret) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: HW detection: can't read CUT ID.\n",
+				__func__);
+			goto out;
+		}
+
+		switch (val32) {
+		case CW1200_CUT_11_ID_STR:
+			cw1200_dbg(CW1200_DBG_MSG,
+				   "Cut 1.1 silicon is detected.\n");
+			priv->hw_revision = CW1200_HW_REV_CUT11;
+			break;
+		default:
+			cw1200_dbg(CW1200_DBG_MSG,
+				   "Cut 1.0 silicon is detected.\n");
+			priv->hw_revision = CW1200_HW_REV_CUT10;
+			break;
+		}
+	} else if (major_revision == 2) {
+		u32 ar1, ar2, ar3;
+		cw1200_dbg(CW1200_DBG_MSG, "Cut 2.x silicon is detected.\n");
+
+		ret = cw1200_ahb_read_32(priv, CW1200_CUT2_ID_ADDR, &ar1);
+		if (ret) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: (1) HW detection: can't read CUT ID.\n",
+				__func__);
+			goto out;
+		}
+		ret = cw1200_ahb_read_32(priv, CW1200_CUT2_ID_ADDR + 4, &ar2);
+		if (ret) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: (2) HW detection: can't read CUT ID.\n",
+				__func__);
+			goto out;
+		}
+
+		ret = cw1200_ahb_read_32(priv, CW1200_CUT2_ID_ADDR + 8, &ar3);
+		if (ret) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: (3) HW detection: can't read CUT ID.\n",
+				__func__);
+			goto out;
+		}
+
+		if (ar1 == CW1200_CUT_22_ID_STR1 &&
+		    ar2 == CW1200_CUT_22_ID_STR2 &&
+		    ar3 == CW1200_CUT_22_ID_STR3) {
+			cw1200_dbg(CW1200_DBG_MSG, "Cut 2.2 detected.\n");
+			priv->hw_revision = CW1200_HW_REV_CUT22;
+		} else {
+			cw1200_dbg(CW1200_DBG_MSG, "Cut 2.0 detected.\n");
+			priv->hw_revision = CW1200_HW_REV_CUT20;
+		}
+	} else {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: unsupported silicon major revision %d.\n",
+			__func__, major_revision);
+		ret = -ENOTSUPP;
+		goto out;
+	}
+
+	/* Checking for access mode */
+	ret = config_reg_read(priv, ST90TDS_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: check_access_mode: can't read " \
+			"config register.\n", __func__);
+		goto out;
+	}
+
+	if (val32 & ST90TDS_CONFIG_ACCESS_MODE_BIT) {
+		switch (priv->hw_type)  {
+		case HIF_8601_SILICON:
+			cw1200_dbg(CW1200_DBG_MSG,
+				"%s: CW1200 detected.\n", __func__);
+			ret = cw1200_load_firmware_cw1200(priv);
+			break;
+		case HIF_8601_VERSATILE:
+			/* TODO: Not implemented yet!
+			ret = cw1200_load_firmware_cw1100(priv);
+			*/
+			ret = -ENOTSUPP;
+			goto out;
+		case HIF_9000_SILICON_VERSTAILE:
+			/* TODO: Not implemented yet!
+			ret = cw1200_load_firmware_stlc9000(priv);
+			*/
+			ret = -ENOTSUPP;
+			goto out;
+		default:
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Unknown hardware: %d.\n",
+				__func__, priv->hw_type);
+		}
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: can't download firmware.\n", __func__);
+			goto out;
+		}
+	} else {
+		cw1200_dbg(CW1200_DBG_MSG,
+			"%s: check_access_mode: device is already " \
+			"in QUEUE mode.\n", __func__);
+		/* TODO: verify this branch. Do we need something to do? */
+	}
+
+	/* Register Interrupt Handler */
+	ret = priv->sbus_ops->irq_subscribe(priv->sbus_priv,
+		(sbus_irq_handler)cw1200_irq_handler, priv);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't register IRQ handler.\n", __func__);
+		goto out;
+	}
+
+	if (HIF_8601_SILICON  == priv->hw_type) {
+		/* If device is CW1200 the IRQ enable/disable bits
+		 * are in CONFIG register */
+		ret = config_reg_read(priv, ST90TDS_CONFIG_REG_ID, &val32);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: enable_irq: can't read " \
+				"config register.\n", __func__);
+			goto unsubscribe;
+		}
+		ret = config_reg_write(priv, ST90TDS_CONFIG_REG_ID,
+			val32 | ST90TDS_CONF_IRQ_RDY_ENABLE);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: enable_irq: can't write " \
+				"config register.\n", __func__);
+			goto unsubscribe;
+		}
+	} else {
+		/* If device is STLC9000 the IRQ enable/disable bits
+		 * are in CONTROL register */
+		/* Enable device interrupts - Both DATA_RDY and WLAN_RDY */
+		ret = cw1200_reg_read_16(priv, ST90TDS_CONFIG_REG_ID, &val16);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: enable_irq: can't read " \
+				"control register.\n", __func__);
+			goto unsubscribe;
+		}
+		ret = cw1200_reg_write_16(priv, ST90TDS_CONFIG_REG_ID,
+			val16 | ST90TDS_CONT_IRQ_RDY_ENABLE);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: enable_irq: can't write " \
+				"control register.\n", __func__);
+			goto unsubscribe;
+		}
+
+	}
+
+	/* Configure device for MESSSAGE MODE */
+	ret = config_reg_read(priv, ST90TDS_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: set_mode: can't read config register.\n",
+			__func__);
+		goto unsubscribe;
+	}
+	ret = config_reg_write(priv, ST90TDS_CONFIG_REG_ID,
+		val32 & ~ST90TDS_CONFIG_ACCESS_MODE_BIT);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: set_mode: can't write config register.\n",
+			__func__);
+		goto unsubscribe;
+	}
+
+	/* Unless we read the CONFIG Register we are
+	 * not able to get an interrupt */
+	mdelay(10);
+	config_reg_read(priv, ST90TDS_CONFIG_REG_ID, &val32);
+
+out:
+	return ret;
+
+unsubscribe:
+	priv->sbus_ops->irq_unsubscribe(priv->sbus_priv);
+	return ret;
+}
+
diff --git a/drivers/staging/cw1200/fwio.h b/drivers/staging/cw1200/fwio.h
new file mode 100644
index 00000000000..cb91b8dc481
--- /dev/null
+++ b/drivers/staging/cw1200/fwio.h
@@ -0,0 +1,36 @@
+/*
+ * Firmware API for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * ST-Ericsson UMAC CW1200 driver which is
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Ajitpal Singh <ajitpal.singh@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FWIO_H_INCLUDED
+#define FWIO_H_INCLUDED
+
+#define FIRMWARE_CUT22		("cw1200/wsm_22.bin")
+#define FIRMWARE_CUT20		("cw1200/wsm_20.bin")
+#define FIRMWARE_CUT11		("cw1200/wsm_11.bin")
+#define FIRMWARE_CUT10		("cw1200/wsm_10.bin")
+#define SDD_FILE_22		("cw1200/sdd_22.bin")
+#define SDD_FILE_20		("cw1200/sdd_20.bin")
+#define SDD_FILE_11		("cw1200/sdd_11.bin")
+#define SDD_FILE_10		("cw1200/sdd_10.bin")
+
+#define CW1200_HW_REV_CUT10	(10)
+#define CW1200_HW_REV_CUT11	(11)
+#define CW1200_HW_REV_CUT20	(20)
+#define CW1200_HW_REV_CUT22	(22)
+
+int cw1200_load_firmware(struct cw1200_common *priv);
+
+#endif
diff --git a/drivers/staging/cw1200/ht.h b/drivers/staging/cw1200/ht.h
new file mode 100644
index 00000000000..5c486a634c7
--- /dev/null
+++ b/drivers/staging/cw1200/ht.h
@@ -0,0 +1,43 @@
+/*
+ *  HT-related code for ST-Ericsson CW1200 driver
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_HT_H_INCLUDED
+#define CW1200_HT_H_INCLUDED
+
+#include <net/mac80211.h>
+
+struct cw1200_ht_info {
+	struct ieee80211_sta_ht_cap	ht_cap;
+	enum nl80211_channel_type	channel_type;
+	u16				operation_mode;
+};
+
+static inline int cw1200_is_ht(const struct cw1200_ht_info *ht_info)
+{
+	return ht_info->channel_type != NL80211_CHAN_NO_HT;
+}
+
+static inline int cw1200_ht_greenfield(const struct cw1200_ht_info *ht_info)
+{
+	return cw1200_is_ht(ht_info) &&
+		(ht_info->ht_cap.cap & IEEE80211_HT_CAP_GRN_FLD) &&
+		!(ht_info->operation_mode &
+			IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT);
+}
+
+static inline int cw1200_ht_ampdu_density(const struct cw1200_ht_info *ht_info)
+{
+	if (!cw1200_is_ht(ht_info))
+		return 0;
+	return ht_info->ht_cap.ampdu_density;
+}
+
+#endif /* CW1200_HT_H_INCLUDED */
diff --git a/drivers/staging/cw1200/hwio.c b/drivers/staging/cw1200/hwio.c
new file mode 100644
index 00000000000..094ce8234f0
--- /dev/null
+++ b/drivers/staging/cw1200/hwio.c
@@ -0,0 +1,268 @@
+/*
+ * Low-level device IO routines for ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * ST-Ericsson UMAC CW1200 driver, which is
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Ajitpal Singh <ajitpal.singh@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/types.h>
+
+#include "cw1200.h"
+#include "hwio.h"
+#include "sbus.h"
+
+ /* Sdio addr is 4*spi_addr */
+#define SPI_REG_ADDR_TO_SDIO(spi_reg_addr) ((spi_reg_addr) << 2)
+#define SDIO_ADDR17BIT(buf_id, mpf, rfu, reg_id_ofs) \
+				((((buf_id)    & 0x1F) << 7) \
+				| (((mpf)        & 1) << 6) \
+				| (((rfu)        & 1) << 5) \
+				| (((reg_id_ofs) & 0x1F) << 0))
+
+
+static int __cw1200_reg_read(struct cw1200_common *priv, u16 addr,
+				void *buf, size_t buf_len, int buf_id)
+{
+	u16 addr_sdio;
+	u32 sdio_reg_addr_17bit ;
+
+	/* Check if buffer is aligned to 4 byte boundary */
+	if (WARN_ON(((unsigned long)buf & 3) && (buf_len > 4))) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			   "%s: buffer is not aligned.\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Convert to SDIO Register Address */
+	addr_sdio = SPI_REG_ADDR_TO_SDIO(addr);
+	sdio_reg_addr_17bit = SDIO_ADDR17BIT(buf_id, 0, 0, addr_sdio);
+
+	BUG_ON(!priv->sbus_ops);
+	return priv->sbus_ops->sbus_memcpy_fromio(priv->sbus_priv,
+						  sdio_reg_addr_17bit,
+						  buf, buf_len);
+}
+
+static int __cw1200_reg_write(struct cw1200_common *priv, u16 addr,
+				const void *buf, size_t buf_len, int buf_id)
+{
+	u16 addr_sdio;
+	u32 sdio_reg_addr_17bit ;
+
+#if 0
+	/* Check if buffer is aligned to 4 byte boundary */
+	if (WARN_ON(((unsigned long)buf & 3) && (buf_len > 4))) {
+		cw1200_dbg(CW1200_DBG_ERROR, "%s: buffer is not aligned.\n",
+				__func__);
+		return -EINVAL;
+	}
+#endif
+
+	/* Convert to SDIO Register Address */
+	addr_sdio = SPI_REG_ADDR_TO_SDIO(addr);
+	sdio_reg_addr_17bit = SDIO_ADDR17BIT(buf_id, 0, 0, addr_sdio);
+
+	BUG_ON(!priv->sbus_ops);
+	return priv->sbus_ops->sbus_memcpy_toio(priv->sbus_priv,
+						sdio_reg_addr_17bit,
+						buf, buf_len);
+}
+
+static inline int __cw1200_reg_read_32(struct cw1200_common *priv,
+					u16 addr, u32 *val)
+{
+	return __cw1200_reg_read(priv, addr, val, sizeof(val), 0);
+}
+
+static inline int __cw1200_reg_write_32(struct cw1200_common *priv,
+					u16 addr, u32 val)
+{
+	return __cw1200_reg_write(priv, addr, &val, sizeof(val), 0);
+}
+
+int cw1200_reg_read(struct cw1200_common *priv, u16 addr, void *buf,
+			size_t buf_len)
+{
+	int ret;
+	BUG_ON(!priv->sbus_ops);
+	priv->sbus_ops->lock(priv->sbus_priv);
+	ret = __cw1200_reg_read(priv, addr, buf, buf_len, 0);
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
+int cw1200_reg_write(struct cw1200_common *priv, u16 addr, const void *buf,
+			size_t buf_len)
+{
+	int ret;
+	BUG_ON(!priv->sbus_ops);
+	priv->sbus_ops->lock(priv->sbus_priv);
+	ret = __cw1200_reg_write(priv, addr, buf, buf_len, 0);
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
+int cw1200_data_read(struct cw1200_common *priv, void *buf, size_t buf_len)
+{
+	int ret;
+	BUG_ON(!priv->sbus_ops);
+	priv->sbus_ops->lock(priv->sbus_priv);
+	{
+		int buf_id_rx = priv->buf_id_rx;
+		ret = __cw1200_reg_read(priv, ST90TDS_IN_OUT_QUEUE_REG_ID, buf,
+						buf_len, buf_id_rx + 1);
+		if (!ret) {
+			buf_id_rx = (buf_id_rx + 1) & 3;
+			priv->buf_id_rx = buf_id_rx;
+		}
+	}
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
+int cw1200_data_write(struct cw1200_common *priv, const void *buf,
+			size_t buf_len)
+{
+	int ret;
+	BUG_ON(!priv->sbus_ops);
+	priv->sbus_ops->lock(priv->sbus_priv);
+	{
+		int buf_id_tx = priv->buf_id_tx;
+		ret = __cw1200_reg_write(priv, ST90TDS_IN_OUT_QUEUE_REG_ID, buf,
+						buf_len, buf_id_tx);
+		if (!ret) {
+			buf_id_tx = (buf_id_tx + 1) & 31;
+			priv->buf_id_tx = buf_id_tx;
+		}
+	}
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
+int cw1200_indirect_read(struct cw1200_common *priv, u32 addr, void *buf,
+			 size_t buf_len, u32 prefetch, u16 port_addr)
+{
+	u32 val32 = 0;
+	int i, ret;
+
+	if ((buf_len / 2) >= 0x1000) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't read more than 0xfff words.\n",
+				__func__);
+		WARN_ON(1);
+		return -EINVAL;
+		goto out;
+	}
+
+	priv->sbus_ops->lock(priv->sbus_priv);
+	/* Write address */
+	ret = __cw1200_reg_write_32(priv, ST90TDS_SRAM_BASE_ADDR_REG_ID, addr);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't write address register.\n",
+				__func__);
+		goto out;
+	}
+
+	/* Read CONFIG Register Value - We will read 32 bits */
+	ret = __cw1200_reg_read_32(priv, ST90TDS_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't read config register.\n",
+				__func__);
+		goto out;
+	}
+
+	/* Set PREFETCH bit */
+	ret = __cw1200_reg_write_32(priv, ST90TDS_CONFIG_REG_ID,
+					val32 | prefetch);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't write prefetch bit.\n",
+				__func__);
+		goto out;
+	}
+
+	/* Check for PRE-FETCH bit to be cleared */
+	for (i = 0; i < 20; i++) {
+		ret = __cw1200_reg_read_32(priv, ST90TDS_CONFIG_REG_ID, &val32);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+					"%s: Can't check prefetch bit.\n",
+					__func__);
+			goto out;
+		}
+		if (!(val32 & prefetch))
+			break;
+
+		mdelay(i);
+	}
+
+	if (val32 & prefetch) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Prefetch bit is not cleared.\n",
+				__func__);
+		goto out;
+	}
+
+	/* Read data port */
+	ret = __cw1200_reg_read(priv, port_addr, buf, buf_len, 0);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't read data port.\n",
+				__func__);
+		goto out;
+	}
+
+out:
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
+int cw1200_apb_write(struct cw1200_common *priv, u32 addr, const void *buf,
+			size_t buf_len)
+{
+	int ret;
+
+	if ((buf_len / 2) >= 0x1000) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't wrire more than 0xfff words.\n",
+				__func__);
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	priv->sbus_ops->lock(priv->sbus_priv);
+
+	/* Write address */
+	ret = __cw1200_reg_write_32(priv, ST90TDS_SRAM_BASE_ADDR_REG_ID, addr);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't write address register.\n",
+				__func__);
+		goto out;
+	}
+
+	/* Write data port */
+	ret = __cw1200_reg_write(priv, ST90TDS_SRAM_DPORT_REG_ID,
+					buf, buf_len, 0);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR, "%s: Can't write data port.\n",
+				__func__);
+		goto out;
+	}
+
+out:
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
diff --git a/drivers/staging/cw1200/hwio.h b/drivers/staging/cw1200/hwio.h
new file mode 100644
index 00000000000..427b75e0523
--- /dev/null
+++ b/drivers/staging/cw1200/hwio.h
@@ -0,0 +1,238 @@
+/*
+ * Low-level API for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * ST-Ericsson UMAC CW1200 driver which is
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Ajitpal Singh <ajitpal.singh@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_HWIO_H_INCLUDED
+#define CW1200_HWIO_H_INCLUDED
+
+/* extern */ struct cw1200_common;
+
+/* DPLL initial values */
+#define DPLL_INIT_VAL_9000		(0x00000191)
+#define DPLL_INIT_VAL_CW1200		(0x0EC4F121)
+
+/* Hardware Type Definitions */
+#define HIF_8601_VERSATILE		(0)
+#define HIF_8601_SILICON		(1)
+#define HIF_9000_SILICON_VERSTAILE	(2)
+
+#define CW1200_CUT_11_ID_STR		(0x302E3830)
+#define CW1200_CUT_22_ID_STR1		(0x302e3132)
+#define CW1200_CUT_22_ID_STR2		(0x32302e30)
+#define CW1200_CUT_22_ID_STR3		(0x3335)
+#define CW1200_CUT_ID_ADDR		(0xFFF17F90)
+#define CW1200_CUT2_ID_ADDR		(0xFFF1FF90)
+
+/* Download control area */
+/* boot loader start address in SRAM */
+#define DOWNLOAD_BOOT_LOADER_OFFSET	(0x00000000)
+/* 32K, 0x4000 to 0xDFFF */
+#define DOWNLOAD_FIFO_OFFSET		(0x00004000)
+/* 32K */
+#define DOWNLOAD_FIFO_SIZE		(0x00008000)
+/* 128 bytes, 0xFF80 to 0xFFFF */
+#define DOWNLOAD_CTRL_OFFSET		(0x0000FF80)
+#define DOWNLOAD_CTRL_DATA_DWORDS	(32-6)
+
+struct download_cntl_t {
+	/* size of whole firmware file (including Cheksum), host init */
+	u32 ImageSize;
+	/* downloading flags */
+	u32 Flags;
+	/* No. of bytes put into the download, init & updated by host */
+	u32 Put;
+	/* last traced program counter, last ARM reg_pc */
+	u32 TracePc;
+	/* No. of bytes read from the download, host init, device updates */
+	u32 Get;
+	/* r0, boot losader status, host init to pending, device updates */
+	u32 Status;
+	/* Extra debug info, r1 to r14 if status=r0=DOWNLOAD_EXCEPTION */
+	u32 DebugData[DOWNLOAD_CTRL_DATA_DWORDS];
+};
+
+#define	DOWNLOAD_IMAGE_SIZE_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, ImageSize))
+#define	DOWNLOAD_FLAGS_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Flags))
+#define DOWNLOAD_PUT_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Put))
+#define DOWNLOAD_TRACE_PC_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, TracePc))
+#define	DOWNLOAD_GET_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Get))
+#define	DOWNLOAD_STATUS_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Status))
+#define DOWNLOAD_DEBUG_DATA_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, DebugData))
+#define DOWNLOAD_DEBUG_DATA_LEN		(108)
+
+#define DOWNLOAD_BLOCK_SIZE		(1024)
+
+/* For boot loader detection */
+#define DOWNLOAD_ARE_YOU_HERE		(0x87654321)
+#define DOWNLOAD_I_AM_HERE		(0x12345678)
+
+/* Download error code */
+#define DOWNLOAD_PENDING		(0xFFFFFFFF)
+#define DOWNLOAD_SUCCESS		(0)
+#define DOWNLOAD_EXCEPTION		(1)
+#define DOWNLOAD_ERR_MEM_1		(2)
+#define DOWNLOAD_ERR_MEM_2		(3)
+#define DOWNLOAD_ERR_SOFTWARE		(4)
+#define DOWNLOAD_ERR_FILE_SIZE		(5)
+#define DOWNLOAD_ERR_CHECKSUM		(6)
+#define DOWNLOAD_ERR_OVERFLOW		(7)
+#define DOWNLOAD_ERR_IMAGE		(8)
+#define DOWNLOAD_ERR_HOST		(9)
+#define DOWNLOAD_ERR_ABORT		(10)
+
+
+#define SYS_BASE_ADDR_SILICON		(0)
+#define PAC_BASE_ADDRESS_SILICON	(SYS_BASE_ADDR_SILICON + 0x09000000)
+#define PAC_SHARED_MEMORY_SILICON	(PAC_BASE_ADDRESS_SILICON)
+
+#define CW12000_APB(addr)		(PAC_SHARED_MEMORY_SILICON + (addr))
+
+/* ***************************************************************
+*Device register definitions
+*************************************************************** */
+/* WBF - SPI Register Addresses */
+#define ST90TDS_ADDR_ID_BASE		(0x0000)
+/* 16/32 bits */
+#define ST90TDS_CONFIG_REG_ID		(0x0000)
+/* 16/32 bits */
+#define ST90TDS_CONTROL_REG_ID		(0x0001)
+/* 16 bits, Q mode W/R */
+#define ST90TDS_IN_OUT_QUEUE_REG_ID	(0x0002)
+/* 32 bits, AHB bus R/W */
+#define ST90TDS_AHB_DPORT_REG_ID	(0x0003)
+/* 16/32 bits */
+#define ST90TDS_SRAM_BASE_ADDR_REG_ID   (0x0004)
+/* 32 bits, APB bus R/W */
+#define ST90TDS_SRAM_DPORT_REG_ID	(0x0005)
+/* 32 bits, t_settle/general */
+#define ST90TDS_TSET_GEN_R_W_REG_ID	(0x0006)
+/* 16 bits, Q mode read, no length */
+#define ST90TDS_FRAME_OUT_REG_ID	(0x0007)
+#define ST90TDS_ADDR_ID_MAX		(ST90TDS_FRAME_OUT_REG_ID)
+
+/* WBF - Control register bit set */
+/* next o/p length, bit 11 to 0 */
+#define ST90TDS_CONT_NEXT_LEN_MASK	(0x0FFF)
+#define ST90TDS_CONT_WUP_BIT		(BIT(12))
+#define ST90TDS_CONT_RDY_BIT		(BIT(13))
+#define ST90TDS_CONT_IRQ_ENABLE		(BIT(14))
+#define ST90TDS_CONT_RDY_ENABLE		(BIT(15))
+#define ST90TDS_CONT_IRQ_RDY_ENABLE	(BIT(14)|BIT(15))
+
+/* SPI Config register bit set */
+#define ST90TDS_CONFIG_FRAME_BIT	(BIT(2))
+#define ST90TDS_CONFIG_WORD_MODE_BITS	(BIT(3)|BIT(4))
+#define ST90TDS_CONFIG_WORD_MODE_1	(BIT(3))
+#define ST90TDS_CONFIG_WORD_MODE_2	(BIT(4))
+#define ST90TDS_CONFIG_ERROR_0_BIT	(BIT(5))
+#define ST90TDS_CONFIG_ERROR_1_BIT	(BIT(6))
+#define ST90TDS_CONFIG_ERROR_2_BIT	(BIT(7))
+/* TBD: Sure??? */
+#define ST90TDS_CONFIG_CSN_FRAME_BIT	(BIT(7))
+#define ST90TDS_CONFIG_ERROR_3_BIT	(BIT(8))
+#define ST90TDS_CONFIG_ERROR_4_BIT	(BIT(9))
+/* QueueM */
+#define ST90TDS_CONFIG_ACCESS_MODE_BIT	(BIT(10))
+/* AHB bus */
+#define ST90TDS_CONFIG_AHB_PFETCH_BIT	(BIT(11))
+#define ST90TDS_CONFIG_CPU_CLK_DIS_BIT	(BIT(12))
+/* APB bus */
+#define ST90TDS_CONFIG_PFETCH_BIT	(BIT(13))
+/* cpu reset */
+#define ST90TDS_CONFIG_CPU_RESET_BIT	(BIT(14))
+#define ST90TDS_CONFIG_CLEAR_INT_BIT	(BIT(15))
+
+/* For CW1200 the IRQ Enable and Ready Bits are in CONFIG register */
+#define ST90TDS_CONF_IRQ_RDY_ENABLE	(BIT(16)|BIT(17))
+
+int cw1200_data_read(struct cw1200_common *priv,
+		     void *buf, size_t buf_len);
+int cw1200_data_write(struct cw1200_common *priv,
+		      const void *buf, size_t buf_len);
+
+int cw1200_reg_read(struct cw1200_common *priv, u16 addr,
+		    void *buf, size_t buf_len);
+int cw1200_reg_write(struct cw1200_common *priv, u16 addr,
+		     const void *buf, size_t buf_len);
+
+static inline int cw1200_reg_read_16(struct cw1200_common *priv,
+				     u16 addr, u16 *val)
+{
+	return cw1200_reg_read(priv, addr, val, sizeof(val));
+}
+
+static inline int cw1200_reg_write_16(struct cw1200_common *priv,
+				      u16 addr, u16 val)
+{
+	return cw1200_reg_write(priv, addr, &val, sizeof(val));
+}
+
+static inline int cw1200_reg_read_32(struct cw1200_common *priv,
+				     u16 addr, u32 *val)
+{
+	return cw1200_reg_read(priv, addr, val, sizeof(val));
+}
+
+static inline int cw1200_reg_write_32(struct cw1200_common *priv,
+				      u16 addr, u32 val)
+{
+	return cw1200_reg_write(priv, addr, &val, sizeof(val));
+}
+
+int cw1200_indirect_read(struct cw1200_common *priv, u32 addr, void *buf,
+			 size_t buf_len, u32 prefetch, u16 port_addr);
+int cw1200_apb_write(struct cw1200_common *priv, u32 addr, const void *buf,
+		     size_t buf_len);
+
+static inline int cw1200_apb_read(struct cw1200_common *priv, u32 addr,
+				  void *buf, size_t buf_len)
+{
+	return cw1200_indirect_read(priv, addr, buf, buf_len,
+		ST90TDS_CONFIG_PFETCH_BIT, ST90TDS_SRAM_DPORT_REG_ID);
+}
+
+static inline int cw1200_ahb_read(struct cw1200_common *priv, u32 addr,
+				  void *buf, size_t buf_len)
+{
+	return cw1200_indirect_read(priv, addr, buf, buf_len,
+		ST90TDS_CONFIG_AHB_PFETCH_BIT, ST90TDS_AHB_DPORT_REG_ID);
+}
+
+static inline int cw1200_apb_read_32(struct cw1200_common *priv,
+				     u32 addr, u32 *val)
+{
+	return cw1200_apb_read(priv, addr, val, sizeof(val));
+}
+
+static inline int cw1200_apb_write_32(struct cw1200_common *priv,
+				      u32 addr, u32 val)
+{
+	return cw1200_apb_write(priv, addr, &val, sizeof(val));
+}
+
+static inline int cw1200_ahb_read_32(struct cw1200_common *priv,
+				     u32 addr, u32 *val)
+{
+	return cw1200_ahb_read(priv, addr, val, sizeof(val));
+}
+
+#endif /* CW1200_HWIO_H_INCLUDED */
diff --git a/drivers/staging/cw1200/main.c b/drivers/staging/cw1200/main.c
new file mode 100644
index 00000000000..7aef0ea1c60
--- /dev/null
+++ b/drivers/staging/cw1200/main.c
@@ -0,0 +1,498 @@
+/*
+ * mac80211 glue code for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ * Copyright (c) 2007-2009, Christian Lamparter <chunkeey@web.de>
+ * Copyright 2008, Johannes Berg <johannes@sipsolutions.net>
+ *
+ * Based on:
+ * - the islsm (softmac prism54) driver, which is:
+ *   Copyright 2004-2006 Jean-Baptiste Note <jbnote@gmail.com>, et al.
+ * - stlc45xx driver
+ *   Copyright (C) 2008 Nokia Corporation and/or its subsidiary(-ies).
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/firmware.h>
+#include <linux/etherdevice.h>
+#include <linux/vmalloc.h>
+#include <linux/random.h>
+#include <linux/sched.h>
+
+#include <net/mac80211.h>
+
+#include "cw1200.h"
+#include "txrx.h"
+#include "sbus.h"
+#include "fwio.h"
+#include "hwio.h"
+#include "bh.h"
+#include "sta.h"
+#include "ap.h"
+#include "scan.h"
+#include "debug.h"
+
+MODULE_AUTHOR("Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>");
+MODULE_DESCRIPTION("Softmac ST-Ericsson CW1200 common code");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("cw1200_core");
+
+/* Accept MAC address of the form macaddr=0x00,0x80,0xE1,0x30,0x40,0x50 */
+static u8 cw1200_mac_template[ETH_ALEN] = {0x00, 0x80, 0xe1, 0x00, 0x00, 0x00};
+module_param_array_named(macaddr, cw1200_mac_template, byte, NULL, S_IRUGO);
+MODULE_PARM_DESC(macaddr, "MAC address");
+
+/* TODO: use rates and channels from the device */
+#define RATETAB_ENT(_rate, _rateid, _flags)		\
+	{						\
+		.bitrate	= (_rate),		\
+		.hw_value	= (_rateid),		\
+		.flags		= (_flags),		\
+	}
+
+static struct ieee80211_rate cw1200_rates[] = {
+	RATETAB_ENT(10,  0,   0),
+	RATETAB_ENT(20,  1,   0),
+	RATETAB_ENT(55,  2,   0),
+	RATETAB_ENT(110, 3,   0),
+	RATETAB_ENT(60,  6,  0),
+	RATETAB_ENT(90,  7,  0),
+	RATETAB_ENT(120, 8,  0),
+	RATETAB_ENT(180, 9,  0),
+	RATETAB_ENT(240, 10, 0),
+	RATETAB_ENT(360, 11, 0),
+	RATETAB_ENT(480, 12, 0),
+	RATETAB_ENT(540, 13, 0),
+	RATETAB_ENT(65,  14, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(130, 15, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(195, 16, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(260, 17, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(390, 18, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(520, 19, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(585, 20, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(650, 21, IEEE80211_TX_RC_MCS),
+};
+
+#define cw1200_a_rates		(cw1200_rates + 4)
+#define cw1200_a_rates_size	(ARRAY_SIZE(cw1200_rates) - 4)
+#define cw1200_g_rates		(cw1200_rates + 0)
+#define cw1200_g_rates_size	(ARRAY_SIZE(cw1200_rates))
+#define cw1200_n_rates		(cw1200_rates + 12)
+#define cw1200_n_rates_size	(ARRAY_SIZE(cw1200_rates) - 12)
+
+
+#define CHAN2G(_channel, _freq, _flags) {			\
+	.band			= IEEE80211_BAND_2GHZ,		\
+	.center_freq		= (_freq),			\
+	.hw_value		= (_channel),			\
+	.flags			= (_flags),			\
+	.max_antenna_gain	= 0,				\
+	.max_power		= 30,				\
+}
+
+#define CHAN5G(_channel, _flags) {				\
+	.band			= IEEE80211_BAND_5GHZ,		\
+	.center_freq	= 5000 + (5 * (_channel)),		\
+	.hw_value		= (_channel),			\
+	.flags			= (_flags),			\
+	.max_antenna_gain	= 0,				\
+	.max_power		= 30,				\
+}
+
+static struct ieee80211_channel cw1200_2ghz_chantable[] = {
+	CHAN2G(1, 2412, 0),
+	CHAN2G(2, 2417, 0),
+	CHAN2G(3, 2422, 0),
+	CHAN2G(4, 2427, 0),
+	CHAN2G(5, 2432, 0),
+	CHAN2G(6, 2437, 0),
+	CHAN2G(7, 2442, 0),
+	CHAN2G(8, 2447, 0),
+	CHAN2G(9, 2452, 0),
+	CHAN2G(10, 2457, 0),
+	CHAN2G(11, 2462, 0),
+	CHAN2G(12, 2467, 0),
+	CHAN2G(13, 2472, 0),
+	CHAN2G(14, 2484, 0),
+};
+
+#ifdef CONFIG_CW1200_5GHZ_SUPPORT
+static struct ieee80211_channel cw1200_5ghz_chantable[] = {
+	CHAN5G(34, 0),		CHAN5G(36, 0),
+	CHAN5G(38, 0),		CHAN5G(40, 0),
+	CHAN5G(42, 0),		CHAN5G(44, 0),
+	CHAN5G(46, 0),		CHAN5G(48, 0),
+	CHAN5G(52, 0),		CHAN5G(56, 0),
+	CHAN5G(60, 0),		CHAN5G(64, 0),
+	CHAN5G(100, 0),		CHAN5G(104, 0),
+	CHAN5G(108, 0),		CHAN5G(112, 0),
+	CHAN5G(116, 0),		CHAN5G(120, 0),
+	CHAN5G(124, 0),		CHAN5G(128, 0),
+	CHAN5G(132, 0),		CHAN5G(136, 0),
+	CHAN5G(140, 0),		CHAN5G(149, 0),
+	CHAN5G(153, 0),		CHAN5G(157, 0),
+	CHAN5G(161, 0),		CHAN5G(165, 0),
+	CHAN5G(184, 0),		CHAN5G(188, 0),
+	CHAN5G(192, 0),		CHAN5G(196, 0),
+	CHAN5G(200, 0),		CHAN5G(204, 0),
+	CHAN5G(208, 0),		CHAN5G(212, 0),
+	CHAN5G(216, 0),
+};
+#endif /* CONFIG_CW1200_5GHZ_SUPPORT */
+
+static struct ieee80211_supported_band cw1200_band_2ghz = {
+	.channels = cw1200_2ghz_chantable,
+	.n_channels = ARRAY_SIZE(cw1200_2ghz_chantable),
+	.bitrates = cw1200_g_rates,
+	.n_bitrates = cw1200_g_rates_size,
+	.ht_cap = {
+		.cap = IEEE80211_HT_CAP_SM_PS |
+			IEEE80211_HT_CAP_GRN_FLD |
+			/* HT Rx STBC: Rx support of one spatial stream */
+			0x0100 |
+			IEEE80211_HT_CAP_DELAY_BA |
+			IEEE80211_HT_CAP_MAX_AMSDU,
+		.ht_supported = 1,
+		.ampdu_factor = IEEE80211_HT_MAX_AMPDU_8K,
+		.ampdu_density = IEEE80211_HT_MPDU_DENSITY_NONE,
+		.mcs = {
+			.rx_mask[0] = 0xFF,
+			.rx_highest = __cpu_to_le16(0x41),
+			.tx_params = IEEE80211_HT_MCS_TX_DEFINED,
+		},
+	},
+};
+
+#ifdef CONFIG_CW1200_5GHZ_SUPPORT
+static struct ieee80211_supported_band cw1200_band_5ghz = {
+	.channels = cw1200_5ghz_chantable,
+	.n_channels = ARRAY_SIZE(cw1200_5ghz_chantable),
+	.bitrates = cw1200_a_rates,
+	.n_bitrates = cw1200_a_rates_size,
+	.ht_cap = {
+		.cap = IEEE80211_HT_CAP_SM_PS |
+			IEEE80211_HT_CAP_GRN_FLD |
+			/* HT Rx STBC: Rx support of one spatial stream */
+			0x0100 |
+			IEEE80211_HT_CAP_DELAY_BA |
+			IEEE80211_HT_CAP_MAX_AMSDU,
+		.ht_supported = 1,
+		.ampdu_factor = IEEE80211_HT_MAX_AMPDU_8K,
+		.ampdu_density = IEEE80211_HT_MPDU_DENSITY_NONE,
+		.mcs = {
+			.rx_mask[0] = 0xFF,
+			.rx_highest = __cpu_to_le16(0x41),
+			.tx_params = IEEE80211_HT_MCS_TX_DEFINED,
+		},
+	},
+};
+#endif /* CONFIG_CW1200_5GHZ_SUPPORT */
+
+static const struct ieee80211_ops cw1200_ops = {
+	.start			= cw1200_start,
+	.stop			= cw1200_stop,
+	.add_interface		= cw1200_add_interface,
+	.remove_interface	= cw1200_remove_interface,
+	.tx			= cw1200_tx,
+	.hw_scan		= cw1200_hw_scan,
+	.set_tim		= cw1200_set_tim,
+	.sta_notify		= cw1200_sta_notify,
+	.sta_add		= cw1200_sta_add,
+	.sta_remove		= cw1200_sta_remove,
+	.set_key		= cw1200_set_key,
+	.set_rts_threshold	= cw1200_set_rts_threshold,
+	.config			= cw1200_config,
+	.bss_info_changed	= cw1200_bss_info_changed,
+	.configure_filter	= cw1200_configure_filter,
+	.conf_tx		= cw1200_conf_tx,
+	.get_stats		= cw1200_get_stats,
+	.ampdu_action		= cw1200_ampdu_action,
+	.flush			= cw1200_flush,
+	/* Intentionally not offloaded:					*/
+	/*.channel_switch	= cw1200_channel_switch,		*/
+	/*.remain_on_channel	= cw1200_remain_on_channel,		*/
+	/*.cancel_remain_on_channel = cw1200_cancel_remain_on_channel,	*/
+};
+
+struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
+{
+	int i;
+	struct ieee80211_hw *hw;
+	struct cw1200_common *priv;
+
+	hw = ieee80211_alloc_hw(priv_data_len, &cw1200_ops);
+	if (!hw)
+		return NULL;
+
+	priv = hw->priv;
+	priv->hw = hw;
+	priv->mode = NL80211_IFTYPE_UNSPECIFIED;
+	priv->rates = cw1200_rates; /* TODO: fetch from FW */
+	priv->mcs_rates = cw1200_n_rates;
+	/* Enable block ACK for every TID but voice. */
+	priv->ba_tid_mask = 0x3F;
+
+	hw->flags = IEEE80211_HW_SIGNAL_DBM |
+		    IEEE80211_HW_SUPPORTS_PS |
+		    /* IEEE80211_HW_SUPPORTS_UAPSD | */
+		    IEEE80211_HW_CONNECTION_MONITOR |
+		    IEEE80211_HW_SUPPORTS_CQM_RSSI |
+		    /* Aggregation is fully controlled by firmware.
+		     * Do not need any support from the mac80211 stack */
+		    /* IEEE80211_HW_AMPDU_AGGREGATION | */
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+		    IEEE80211_HW_SUPPORTS_CQM_BEACON_MISS |
+		    IEEE80211_HW_SUPPORTS_CQM_TX_FAIL |
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+		    IEEE80211_HW_BEACON_FILTER;
+
+	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+					  BIT(NL80211_IFTYPE_ADHOC) |
+					  BIT(NL80211_IFTYPE_AP) |
+					  BIT(NL80211_IFTYPE_MESH_POINT);
+
+	hw->channel_change_time = 1000;	/* TODO: find actual value */
+	/* priv->beacon_req_id = cpu_to_le32(0); */
+	hw->queues = 4;
+	priv->noise = -94;
+
+	hw->max_rates = 8;
+	hw->max_rate_tries = 15;
+	hw->extra_tx_headroom = WSM_TX_EXTRA_HEADROOM;
+
+	hw->sta_data_size = sizeof(struct cw1200_sta_priv);
+
+	/*
+	 * For now, disable PS by default because it affects
+	 * link stability significantly.
+	 */
+	hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
+
+	hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &cw1200_band_2ghz;
+#ifdef CONFIG_CW1200_5GHZ_SUPPORT
+	hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &cw1200_band_5ghz;
+#endif /* CONFIG_CW1200_5GHZ_SUPPORT */
+
+	hw->wiphy->max_scan_ssids = 2;
+	hw->wiphy->max_scan_ie_len = IEEE80211_MAX_DATA_LEN;
+
+	SET_IEEE80211_PERM_ADDR(hw, cw1200_mac_template);
+
+	if (hw->wiphy->perm_addr[3] == 0 &&
+	    hw->wiphy->perm_addr[4] == 0 &&
+	    hw->wiphy->perm_addr[5] == 0) {
+		get_random_bytes(&hw->wiphy->perm_addr[3], 3);
+	}
+
+	mutex_init(&priv->wsm_cmd_mux);
+	mutex_init(&priv->conf_mutex);
+	priv->workqueue = create_singlethread_workqueue("cw1200_wq");
+	sema_init(&priv->scan.lock, 1);
+	INIT_WORK(&priv->scan.work, cw1200_scan_work);
+	INIT_DELAYED_WORK(&priv->scan.probe_work, cw1200_probe_work);
+	INIT_DELAYED_WORK(&priv->scan.timeout, cw1200_scan_timeout);
+	INIT_WORK(&priv->join_work, cw1200_join_work);
+	INIT_DELAYED_WORK(&priv->join_timeout, cw1200_join_timeout);
+	INIT_WORK(&priv->unjoin_work, cw1200_unjoin_work);
+	INIT_WORK(&priv->wep_key_work, cw1200_wep_key_work);
+	INIT_WORK(&priv->tx_policy_upload_work, tx_policy_upload_work);
+	INIT_LIST_HEAD(&priv->event_queue);
+	INIT_WORK(&priv->event_handler, cw1200_event_handler);
+	INIT_DELAYED_WORK(&priv->bss_loss_work, cw1200_bss_loss_work);
+	INIT_DELAYED_WORK(&priv->connection_loss_work,
+			  cw1200_connection_loss_work);
+#if defined(CONFIG_CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE)
+	INIT_DELAYED_WORK(&priv->keep_alive_work, cw1200_keep_alive_work);
+#endif /* CONFIG_CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE */
+	INIT_WORK(&priv->tx_failure_work, cw1200_tx_failure_work);
+	INIT_WORK(&priv->set_tim_work, cw1200_set_tim_work);
+	INIT_WORK(&priv->multicast_start_work, cw1200_multicast_start_work);
+	INIT_WORK(&priv->multicast_stop_work, cw1200_multicast_stop_work);
+
+	if (unlikely(cw1200_queue_stats_init(&priv->tx_queue_stats,
+			CW1200_LINK_ID_AFTER_DTIM + 1))) {
+		ieee80211_free_hw(hw);
+		return NULL;
+	}
+
+	for (i = 0; i < 4; ++i) {
+		if (unlikely(cw1200_queue_init(&priv->tx_queue[i],
+				&priv->tx_queue_stats, i, 16))) {
+			for (; i > 0; i--)
+				cw1200_queue_deinit(&priv->tx_queue[i - 1]);
+			cw1200_queue_stats_deinit(&priv->tx_queue_stats);
+			ieee80211_free_hw(hw);
+			return NULL;
+		}
+	}
+
+	init_waitqueue_head(&priv->channel_switch_done);
+	init_waitqueue_head(&priv->wsm_cmd_wq);
+	init_waitqueue_head(&priv->wsm_startup_done);
+	wsm_buf_init(&priv->wsm_cmd_buf);
+	tx_policy_init(priv);
+
+	return hw;
+}
+EXPORT_SYMBOL_GPL(cw1200_init_common);
+
+int cw1200_register_common(struct ieee80211_hw *dev)
+{
+	struct cw1200_common *priv = dev->priv;
+	int err;
+
+	err = ieee80211_register_hw(dev);
+	if (err) {
+		cw1200_dbg(CW1200_DBG_ERROR, "Cannot register device (%d).\n",
+				err);
+		return err;
+	}
+
+#ifdef CONFIG_CW1200_LEDS
+	err = cw1200_init_leds(priv);
+	if (err)
+		return err;
+#endif /* CONFIG_CW1200_LEDS */
+
+	cw1200_debug_init(priv);
+
+	cw1200_dbg(CW1200_DBG_MSG, "is registered as '%s'\n",
+			wiphy_name(dev->wiphy));
+	return 0;
+}
+EXPORT_SYMBOL_GPL(cw1200_register_common);
+
+void cw1200_free_common(struct ieee80211_hw *dev)
+{
+	/* struct cw1200_common *priv = dev->priv; */
+	/* unsigned int i; */
+
+	ieee80211_free_hw(dev);
+}
+EXPORT_SYMBOL_GPL(cw1200_free_common);
+
+void cw1200_unregister_common(struct ieee80211_hw *dev)
+{
+	struct cw1200_common *priv = dev->priv;
+	int i;
+
+	cw1200_debug_release(priv);
+
+	priv->sbus_ops->irq_unsubscribe(priv->sbus_priv);
+	cw1200_unregister_bh(priv);
+
+#ifdef CONFIG_CW1200_LEDS
+	cw1200_unregister_leds(priv);
+#endif /* CONFIG_CW1200_LEDS */
+
+	ieee80211_unregister_hw(dev);
+	mutex_destroy(&priv->conf_mutex);
+	mutex_destroy(&priv->eeprom_mutex);
+
+	wsm_buf_deinit(&priv->wsm_cmd_buf);
+
+	kfree(priv->scan.ie);
+	priv->scan.ie = NULL;
+	priv->scan.ie_len = 0;
+	destroy_workqueue(priv->workqueue);
+	priv->workqueue = NULL;
+
+	if (priv->skb_cache) {
+		dev_kfree_skb(priv->skb_cache);
+		priv->skb_cache = NULL;
+	}
+
+	for (i = 0; i < 4; ++i)
+		cw1200_queue_deinit(&priv->tx_queue[i]);
+	cw1200_queue_stats_deinit(&priv->tx_queue_stats);
+}
+EXPORT_SYMBOL_GPL(cw1200_unregister_common);
+
+int cw1200_probe(const struct sbus_ops *sbus_ops,
+		 struct sbus_priv *sbus,
+		 struct device *pdev,
+		 struct cw1200_common **pself)
+{
+	int err = -ENOMEM;
+	struct ieee80211_hw *dev;
+	struct cw1200_common *priv;
+	struct wsm_operational_mode mode = {
+		.power_mode = wsm_power_mode_quiescent,
+		.disableMoreFlagUsage = true,
+	};
+
+	dev = cw1200_init_common(sizeof(struct cw1200_common));
+	if (!dev)
+		goto err;
+
+	priv = dev->priv;
+
+	priv->sbus_ops = sbus_ops;
+	priv->sbus_priv = sbus;
+	priv->pdev = pdev;
+	SET_IEEE80211_DEV(priv->hw, pdev);
+
+	/* WSM callbacks. */
+	priv->wsm_cbc.scan_complete = cw1200_scan_complete_cb;
+	priv->wsm_cbc.tx_confirm = cw1200_tx_confirm_cb;
+	priv->wsm_cbc.rx = cw1200_rx_cb;
+	priv->wsm_cbc.suspend_resume = cw1200_suspend_resume;
+	/* priv->wsm_cbc.set_pm_complete = cw1200_set_pm_complete_cb; */
+	priv->wsm_cbc.channel_switch = cw1200_channel_switch_cb;
+
+	err = cw1200_register_bh(priv);
+	if (err)
+		goto err1;
+
+	err = cw1200_load_firmware(priv);
+	if (err)
+		goto err2;
+
+	if (wait_event_interruptible_timeout(priv->wsm_startup_done,
+				priv->wsm_caps.firmwareReady, 3*HZ) <= 0) {
+		/* TODO: Needs to find how to reset device */
+		/*       in QUEUE mode properly.           */
+		goto err3;
+	}
+
+	/* Set low-power mode. */
+	WARN_ON(wsm_set_operational_mode(priv, &mode));
+
+	/* Enable multi-TX confirmation */
+	WARN_ON(wsm_use_multi_tx_conf(priv, true));
+
+	err = cw1200_register_common(dev);
+	if (err) {
+		priv->sbus_ops->irq_unsubscribe(priv->sbus_priv);
+		goto err3;
+	}
+
+	*pself = dev->priv;
+	return err;
+
+err3:
+	sbus_ops->reset(sbus);
+err2:
+	cw1200_unregister_bh(priv);
+err1:
+	cw1200_free_common(dev);
+err:
+	return err;
+}
+EXPORT_SYMBOL_GPL(cw1200_probe);
+
+void cw1200_release(struct cw1200_common *self)
+{
+	cw1200_unregister_common(self->hw);
+	cw1200_free_common(self->hw);
+	return;
+}
+EXPORT_SYMBOL_GPL(cw1200_release);
diff --git a/drivers/staging/cw1200/queue.c b/drivers/staging/cw1200/queue.c
new file mode 100644
index 00000000000..f81e46cd4f4
--- /dev/null
+++ b/drivers/staging/cw1200/queue.c
@@ -0,0 +1,485 @@
+/*
+ * O(1) TX queue with built-in allocator for ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include "net/mac80211.h"
+#include "queue.h"
+#include "wsm.h"
+#include "txrx.h"
+#include "cw1200.h"
+
+/* private */ struct cw1200_queue_item
+{
+	struct list_head	head;
+	struct sk_buff		*skb;
+	u32			packetID;
+	/* For safety purposes only. I do not trust device too much.
+	 * It was observed (last time quite long time ago) that it
+	 * indicates TX for a packet several times, so it was not enough
+	 * to use offset or address as an uniquie ID in a
+	 * queue.
+	 */
+	u8			generation;
+	u8			link_id;
+	u8			reserved[2];
+};
+
+static inline void __cw1200_queue_lock(struct cw1200_queue *queue,
+						struct cw1200_common *cw1200)
+{
+	if (queue->tx_locked_cnt++ == 0) {
+		txrx_printk(KERN_DEBUG "[TX] Queue %d is locked.\n",
+				queue->queue_id);
+		ieee80211_stop_queue(cw1200->hw, queue->queue_id);
+	}
+}
+
+static inline void __cw1200_queue_unlock(struct cw1200_queue *queue,
+						struct cw1200_common *cw1200)
+{
+	BUG_ON(!queue->tx_locked_cnt);
+	if (--queue->tx_locked_cnt == 0) {
+		txrx_printk(KERN_DEBUG "[TX] Queue %d is unlocked.\n",
+				queue->queue_id);
+		ieee80211_wake_queue(cw1200->hw, queue->queue_id);
+	}
+}
+
+static inline void cw1200_queue_parse_id(u32 packetID, u8 *queue_generation,
+						u8 *queue_id,
+						u8 *item_generation,
+						u8 *item_id)
+{
+	*item_id		= (packetID >>  0) & 0xFF;
+	*item_generation	= (packetID >>  8) & 0xFF;
+	*queue_id		= (packetID >> 16) & 0xFF;
+	*queue_generation	= (packetID >> 24) & 0xFF;
+}
+
+static inline u32 cw1200_queue_make_packet_id(u8 queue_generation, u8 queue_id,
+						u8 item_generation, u8 item_id)
+{
+	return ((u32)item_id << 0) |
+		((u32)item_generation << 8) |
+		((u32)queue_id << 16) |
+		((u32)queue_generation << 24);
+}
+
+int cw1200_queue_stats_init(struct cw1200_queue_stats *stats,
+			    size_t map_capacity)
+{
+	memset(stats, 0, sizeof(*stats));
+	stats->map_capacity = map_capacity;
+	spin_lock_init(&stats->lock);
+	init_waitqueue_head(&stats->wait_link_id_empty);
+
+	stats->link_map_cache = kzalloc(sizeof(int[map_capacity]),
+			GFP_KERNEL);
+	if (!stats->link_map_cache)
+		return -ENOMEM;
+
+	return 0;
+}
+
+int cw1200_queue_init(struct cw1200_queue *queue,
+		      struct cw1200_queue_stats *stats,
+		      u8 queue_id,
+		      size_t capacity)
+{
+	size_t i;
+
+	memset(queue, 0, sizeof(*queue));
+	queue->stats = stats;
+	queue->capacity = capacity;
+	queue->queue_id = queue_id;
+	INIT_LIST_HEAD(&queue->queue);
+	INIT_LIST_HEAD(&queue->pending);
+	INIT_LIST_HEAD(&queue->free_pool);
+	spin_lock_init(&queue->lock);
+
+	queue->pool = kzalloc(sizeof(struct cw1200_queue_item) * capacity,
+			GFP_KERNEL);
+	if (!queue->pool)
+		return -ENOMEM;
+
+	queue->link_map_cache = kzalloc(sizeof(int[stats->map_capacity]),
+			GFP_KERNEL);
+	if (!queue->link_map_cache) {
+		kfree(queue->pool);
+		queue->pool = NULL;
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < capacity; ++i)
+		list_add_tail(&queue->pool[i].head, &queue->free_pool);
+
+	return 0;
+}
+
+int cw1200_queue_clear(struct cw1200_queue *queue)
+{
+	int i;
+	struct cw1200_queue_stats *stats = queue->stats;
+
+	spin_lock_bh(&queue->lock);
+	queue->generation++;
+	list_splice_tail_init(&queue->queue, &queue->pending);
+	while (!list_empty(&queue->pending)) {
+		struct cw1200_queue_item *item = list_first_entry(
+			&queue->pending, struct cw1200_queue_item, head);
+		WARN_ON(!item->skb);
+		if (likely(item->skb)) {
+			dev_kfree_skb_any(item->skb);
+			item->skb = NULL;
+		}
+		list_move_tail(&item->head, &queue->free_pool);
+	}
+	queue->num_queued = 0;
+	queue->num_pending = 0;
+	queue->num_sent = 0;
+
+	spin_lock_bh(&stats->lock);
+	for (i = 0; i < stats->map_capacity; ++i) {
+		stats->num_queued -= queue->link_map_cache[i];
+		stats->link_map_cache[i] -= queue->link_map_cache[i];
+		queue->link_map_cache[i] = 0;
+	}
+	spin_unlock_bh(&stats->lock);
+	spin_unlock_bh(&queue->lock);
+	wake_up_interruptible(&stats->wait_link_id_empty);
+	return 0;
+}
+
+void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats)
+{
+	kfree(stats->link_map_cache);
+	stats->link_map_cache = NULL;
+}
+
+void cw1200_queue_deinit(struct cw1200_queue *queue)
+{
+	cw1200_queue_clear(queue);
+	INIT_LIST_HEAD(&queue->free_pool);
+	kfree(queue->pool);
+	kfree(queue->link_map_cache);
+	queue->pool = NULL;
+	queue->link_map_cache = NULL;
+	queue->capacity = 0;
+}
+
+size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
+				   u32 link_id_map)
+{
+	size_t ret;
+	int i, bit;
+	size_t map_capacity = queue->stats->map_capacity;
+
+	if (!link_id_map)
+		return 0;
+
+	spin_lock_bh(&queue->lock);
+	if (likely(link_id_map == (u32) -1))
+		ret = queue->num_queued - queue->num_pending;
+	else {
+		ret = 0;
+		for (i = 0, bit = 1; i < map_capacity; ++i, bit <<= 1) {
+			if (link_id_map & bit)
+				ret += queue->link_map_cache[i];
+		}
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *priv,
+			struct sk_buff *skb, u8 link_id)
+{
+	int ret;
+	struct wsm_tx *wsm;
+	struct cw1200_queue_stats *stats = queue->stats;
+
+	wsm = (struct wsm_tx *)skb_push(skb, sizeof(struct wsm_tx));
+	ret = cw1200_skb_to_wsm(priv, skb, wsm);
+	if (ret)
+		return ret;
+
+	if (link_id >= queue->stats->map_capacity)
+		return -EINVAL;
+
+	spin_lock_bh(&queue->lock);
+	if (!WARN_ON(list_empty(&queue->free_pool))) {
+		struct cw1200_queue_item *item = list_first_entry(
+			&queue->free_pool, struct cw1200_queue_item, head);
+		BUG_ON(item->skb);
+
+		list_move_tail(&item->head, &queue->queue);
+		item->skb = skb;
+		item->packetID = cw1200_queue_make_packet_id(
+			queue->generation, queue->queue_id,
+			item->generation, item - queue->pool);
+		wsm->packetID = __cpu_to_le32(item->packetID);
+		item->link_id = link_id;
+
+		++queue->num_queued;
+		++queue->link_map_cache[link_id];
+
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued;
+		++stats->link_map_cache[link_id];
+		spin_unlock_bh(&stats->lock);
+
+		if (queue->num_queued >= queue->capacity) {
+			queue->overfull = true;
+			__cw1200_queue_lock(queue, priv);
+		}
+	} else {
+		ret = -ENOENT;
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+int cw1200_queue_get(struct cw1200_queue *queue,
+		     u32 link_id_map,
+		     struct wsm_tx **tx,
+		     struct ieee80211_tx_info **tx_info)
+{
+	int ret = -ENOENT;
+	struct cw1200_queue_item *item;
+	struct cw1200_queue_stats *stats = queue->stats;
+	bool wakeup_stats = false;
+
+	spin_lock_bh(&queue->lock);
+	list_for_each_entry(item, &queue->queue, head) {
+		if (link_id_map & BIT(item->link_id)) {
+			ret = 0;
+			break;
+		}
+	}
+
+	if (!WARN_ON(ret)) {
+		*tx = (struct wsm_tx *)item->skb->data;
+		*tx_info = IEEE80211_SKB_CB(item->skb);
+		list_move_tail(&item->head, &queue->pending);
+		++queue->num_pending;
+		--queue->link_map_cache[item->link_id];
+
+		spin_lock_bh(&stats->lock);
+		--stats->num_queued;
+		if (!--stats->link_map_cache[item->link_id])
+			wakeup_stats = true;
+		spin_unlock_bh(&stats->lock);
+	}
+	spin_unlock_bh(&queue->lock);
+	if (wakeup_stats)
+		wake_up_interruptible(&stats->wait_link_id_empty);
+	return ret;
+}
+
+int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packetID)
+{
+	int ret = 0;
+	u8 queue_generation, queue_id, item_generation, item_id;
+	struct cw1200_queue_item *item;
+	struct cw1200_queue_stats *stats = queue->stats;
+
+	cw1200_queue_parse_id(packetID, &queue_generation, &queue_id,
+				&item_generation, &item_id);
+
+	item = &queue->pool[item_id];
+
+	spin_lock_bh(&queue->lock);
+	BUG_ON(queue_id != queue->queue_id);
+	if (unlikely(queue_generation != queue->generation)) {
+		ret = -ENOENT;
+	} else if (unlikely(item_id >= (unsigned) queue->capacity)) {
+		WARN_ON(1);
+		ret = -EINVAL;
+	} else if (unlikely(item->generation != item_generation)) {
+		WARN_ON(1);
+		ret = -ENOENT;
+	} else {
+		struct wsm_tx *wsm = (struct wsm_tx *)item->skb->data;
+		--queue->num_pending;
+		++queue->link_map_cache[item->link_id];
+
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued;
+		++stats->link_map_cache[item->link_id];
+		spin_unlock_bh(&stats->lock);
+
+		item->generation = ++item_generation;
+		item->packetID = cw1200_queue_make_packet_id(
+			queue_generation, queue_id, item_generation, item_id);
+		wsm->packetID = __cpu_to_le32(item->packetID);
+		list_move_tail(&item->head, &queue->queue);
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+int cw1200_queue_requeue_all(struct cw1200_queue *queue)
+{
+	struct cw1200_queue_stats *stats = queue->stats;
+	spin_lock_bh(&queue->lock);
+	while (!list_empty(&queue->pending)) {
+		struct cw1200_queue_item *item = list_first_entry(
+			&queue->pending, struct cw1200_queue_item, head);
+		struct wsm_tx *wsm = (struct wsm_tx *)item->skb->data;
+
+		--queue->num_pending;
+		++queue->link_map_cache[item->link_id];
+
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued;
+		++stats->link_map_cache[item->link_id];
+		spin_unlock_bh(&stats->lock);
+
+		++item->generation;
+		item->packetID = cw1200_queue_make_packet_id(
+			queue->generation, queue->queue_id,
+			item->generation, item - queue->pool);
+		wsm->packetID = __cpu_to_le32(item->packetID);
+		list_move_tail(&item->head, &queue->queue);
+	}
+	spin_unlock_bh(&queue->lock);
+
+	return 0;
+}
+
+int cw1200_queue_remove(struct cw1200_queue *queue, struct cw1200_common *priv,
+				u32 packetID)
+{
+	int ret = 0;
+	u8 queue_generation, queue_id, item_generation, item_id;
+	struct cw1200_queue_item *item;
+	struct sk_buff *skb_to_free = NULL;
+	cw1200_queue_parse_id(packetID, &queue_generation, &queue_id,
+				&item_generation, &item_id);
+
+	item = &queue->pool[item_id];
+
+	spin_lock_bh(&queue->lock);
+	BUG_ON(queue_id != queue->queue_id);
+	if (unlikely(queue_generation != queue->generation)) {
+		ret = -ENOENT;
+	} else if (unlikely(item_id >= (unsigned) queue->capacity)) {
+		WARN_ON(1);
+		ret = -EINVAL;
+	} else if (unlikely(item->generation != item_generation)) {
+		WARN_ON(1);
+		ret = -ENOENT;
+	} else {
+		--queue->num_pending;
+		--queue->num_queued;
+		++queue->num_sent;
+		++item->generation;
+		skb_to_free = item->skb;
+		item->skb = NULL;
+		/* Do not use list_move_tail here, but list_move:
+		 * try to utilize cache row.
+		 */
+		list_move(&item->head, &queue->free_pool);
+
+		if (unlikely(queue->overfull) &&
+		    (queue->num_queued <= (queue->capacity >> 1))) {
+			queue->overfull = false;
+			__cw1200_queue_unlock(queue, priv);
+		}
+	}
+	spin_unlock_bh(&queue->lock);
+
+	if (skb_to_free)
+		dev_kfree_skb_any(item->skb);
+
+	return ret;
+}
+
+int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packetID,
+				struct sk_buff **skb)
+{
+	int ret = 0;
+	u8 queue_generation, queue_id, item_generation, item_id;
+	struct cw1200_queue_item *item;
+	cw1200_queue_parse_id(packetID, &queue_generation, &queue_id,
+				&item_generation, &item_id);
+
+	item = &queue->pool[item_id];
+
+	spin_lock_bh(&queue->lock);
+	BUG_ON(queue_id != queue->queue_id);
+	if (unlikely(queue_generation != queue->generation)) {
+		ret = -ENOENT;
+	} else if (unlikely(item_id >= (unsigned) queue->capacity)) {
+		WARN_ON(1);
+		ret = -EINVAL;
+	} else if (unlikely(item->generation != item_generation)) {
+		WARN_ON(1);
+		ret = -ENOENT;
+	} else {
+		*skb = item->skb;
+		item->skb = NULL;
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+void cw1200_queue_lock(struct cw1200_queue *queue, struct cw1200_common *cw1200)
+{
+	spin_lock_bh(&queue->lock);
+	__cw1200_queue_lock(queue, cw1200);
+	spin_unlock_bh(&queue->lock);
+}
+
+void cw1200_queue_unlock(struct cw1200_queue *queue,
+				struct cw1200_common *cw1200)
+{
+	spin_lock_bh(&queue->lock);
+	__cw1200_queue_unlock(queue, cw1200);
+	spin_unlock_bh(&queue->lock);
+}
+
+/*
+int cw1200_queue_get_stats(struct cw1200_queue *queue,
+				struct ieee80211_tx_queue_stats *stats)
+{
+	spin_lock_bh(&queue->lock);
+	stats->len = queue->num_queued;
+	stats->limit = queue->capacity;
+	stats->count = queue->num_sent;
+	spin_unlock_bh(&queue->lock);
+
+	return 0;
+}
+*/
+
+bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats,
+				 u32 link_id_map)
+{
+	bool empty = true;
+
+	spin_lock_bh(&stats->lock);
+	if (link_id_map == (u32)-1)
+		empty = stats->num_queued == 0;
+	else {
+		int i;
+		for (i = 0; i < stats->map_capacity; ++i) {
+			if (link_id_map & BIT(i)) {
+				if (stats->link_map_cache[i]) {
+					empty = false;
+					break;
+				}
+			}
+		}
+	}
+	spin_unlock_bh(&stats->lock);
+
+	return empty;
+}
diff --git a/drivers/staging/cw1200/queue.h b/drivers/staging/cw1200/queue.h
new file mode 100644
index 00000000000..fc5d4613efb
--- /dev/null
+++ b/drivers/staging/cw1200/queue.h
@@ -0,0 +1,90 @@
+/*
+ * O(1) TX queue with built-in allocator for ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_QUEUE_H_INCLUDED
+#define CW1200_QUEUE_H_INCLUDED
+
+/* private */ struct cw1200_queue_item;
+
+/* extern */ struct sk_buff;
+/* extern */ struct wsm_tx;
+/* extern */ struct cw1200_common;
+/* extern */ struct ieee80211_tx_queue_stats;
+
+/* forward */ struct cw1200_queue_stats;
+
+struct cw1200_queue {
+	struct cw1200_queue_stats *stats;
+	size_t			capacity;
+	size_t			num_queued;
+	size_t			num_pending;
+	size_t			num_sent;
+	struct cw1200_queue_item *pool;
+	struct list_head	queue;
+	struct list_head	free_pool;
+	struct list_head	pending;
+	int			tx_locked_cnt;
+	int			*link_map_cache;
+	bool			overfull;
+	spinlock_t		lock;
+	u8			queue_id;
+	u8			generation;
+};
+
+struct cw1200_queue_stats {
+	spinlock_t		lock;
+	int			*link_map_cache;
+	int			num_queued;
+	size_t			map_capacity;
+	wait_queue_head_t	wait_link_id_empty;
+};
+
+int cw1200_queue_stats_init(struct cw1200_queue_stats *stats,
+			    size_t map_capacity);
+int cw1200_queue_init(struct cw1200_queue *queue,
+		      struct cw1200_queue_stats *stats,
+		      u8 queue_id,
+		      size_t capacity);
+int cw1200_queue_clear(struct cw1200_queue *queue);
+void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats);
+void cw1200_queue_deinit(struct cw1200_queue *queue);
+
+size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
+				   u32 link_id_map);
+int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *cw1200,
+			struct sk_buff *skb, u8 link_id);
+int cw1200_queue_get(struct cw1200_queue *queue,
+		     u32 link_id_map,
+		     struct wsm_tx **tx,
+		     struct ieee80211_tx_info **tx_info);
+int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packetID);
+int cw1200_queue_requeue_all(struct cw1200_queue *queue);
+int cw1200_queue_remove(struct cw1200_queue *queue, struct cw1200_common *priv,
+				u32 packetID);
+int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packetID,
+				struct sk_buff **skb);
+void cw1200_queue_lock(struct cw1200_queue *queue,
+			struct cw1200_common *cw1200);
+void cw1200_queue_unlock(struct cw1200_queue *queue,
+				struct cw1200_common *cw1200);
+
+bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats,
+				 u32 link_id_map);
+
+/* int cw1200_queue_get_stats(struct cw1200_queue *queue,
+struct ieee80211_tx_queue_stats *stats); */
+
+static inline u8 cw1200_queue_get_queue_id(u32 packetID)
+{
+	return (packetID >> 16) & 0xFF;
+}
+
+#endif /* CW1200_QUEUE_H_INCLUDED */
diff --git a/drivers/staging/cw1200/sbus.h b/drivers/staging/cw1200/sbus.h
new file mode 100644
index 00000000000..c31a2f30a22
--- /dev/null
+++ b/drivers/staging/cw1200/sbus.h
@@ -0,0 +1,37 @@
+/*
+ * Common sbus abstraction layer interface for cw1200 wireless driver
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_SBUS_H
+#define CW1200_SBUS_H
+
+/*
+ * sbus priv forward definition.
+ * Implemented and instantiated in particular modules.
+ */
+struct sbus_priv;
+
+typedef void (*sbus_irq_handler)(void *priv);
+
+struct sbus_ops {
+	int (*sbus_memcpy_fromio)(struct sbus_priv *self, unsigned int addr,
+					void *dst, int count);
+	int (*sbus_memcpy_toio)(struct sbus_priv *self, unsigned int addr,
+					const void *src, int count);
+	void (*lock)(struct sbus_priv *self);
+	void (*unlock)(struct sbus_priv *self);
+	int (*irq_subscribe)(struct sbus_priv *self, sbus_irq_handler handler,
+				void *priv);
+	int (*irq_unsubscribe)(struct sbus_priv *self);
+	int (*reset)(struct sbus_priv *self);
+	size_t (*align_size)(struct sbus_priv *self, size_t size);
+};
+
+#endif /* CW1200_SBUS_H */
diff --git a/drivers/staging/cw1200/scan.c b/drivers/staging/cw1200/scan.c
new file mode 100644
index 00000000000..71268291181
--- /dev/null
+++ b/drivers/staging/cw1200/scan.c
@@ -0,0 +1,374 @@
+/*
+ * Scan implementation for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/sched.h>
+#include "cw1200.h"
+#include "scan.h"
+
+static int cw1200_scan_start(struct cw1200_common *priv, struct wsm_scan *scan)
+{
+	int ret, i;
+	int tmo = 1000;
+
+	for (i = 0; i < scan->numOfChannels; ++i)
+		tmo += scan->ch[i].maxChannelTime + 10;
+
+	atomic_set(&priv->scan.in_progress, 1);
+	queue_delayed_work(priv->workqueue, &priv->scan.timeout,
+		tmo * HZ / 1000);
+	ret = wsm_scan(priv, scan);
+	if (unlikely(ret)) {
+		atomic_set(&priv->scan.in_progress, 0);
+		cancel_delayed_work_sync(&priv->scan.timeout);
+	}
+	return ret;
+}
+
+int cw1200_hw_scan(struct ieee80211_hw *hw,
+		   struct ieee80211_vif *vif,
+		   struct cfg80211_scan_request *req)
+{
+	struct cw1200_common *priv = hw->priv;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_PROBE_REQUEST,
+	};
+	int i;
+
+	if (!priv->vif)
+		return -EINVAL;
+
+	if (req->n_ssids == 1 && !req->ssids[0].ssid_len)
+		req->n_ssids = 0;
+
+	printk(KERN_DEBUG "[SCAN] Scan request for %d SSIDs.\n",
+		req->n_ssids);
+
+	if (req->n_ssids > WSM_SCAN_MAX_NUM_OF_SSIDS)
+		return -EINVAL;
+
+	if (req->ie_len != priv->scan.ie_len ||
+	    memcmp(req->ie, priv->scan.ie, req->ie_len)) {
+		frame.skb = ieee80211_probereq_get(hw, priv->vif, NULL, 0,
+			req->ie, req->ie_len);
+		if (!frame.skb)
+			return -ENOMEM;
+		kfree(priv->scan.ie);
+		priv->scan.ie = NULL;
+		priv->scan.ie_len = 0;
+		if (req->ie_len) {
+			priv->scan.ie = kmalloc(req->ie_len, GFP_KERNEL);
+			if (priv->scan.ie) {
+				memcpy(priv->scan.ie, req->ie, req->ie_len);
+				priv->scan.ie_len = req->ie_len;
+			}
+		}
+	}
+
+	/* will be unlocked in cw1200_scan_work() */
+	down(&priv->scan.lock);
+	mutex_lock(&priv->conf_mutex);
+	if (frame.skb) {
+		int ret = wsm_set_template_frame(priv, &frame);
+		if (ret) {
+			mutex_unlock(&priv->conf_mutex);
+			up(&priv->scan.lock);
+			dev_kfree_skb(frame.skb);
+			return ret;
+		}
+	}
+
+	wsm_lock_tx(priv);
+
+	if (priv->join_status == CW1200_JOIN_STATUS_STA &&
+			priv->powersave_mode.pmMode != WSM_PSM_PS) {
+		struct wsm_set_pm pm = priv->powersave_mode;
+		pm.pmMode = WSM_PSM_PS;
+		WARN_ON(wsm_set_pm(priv, &pm));
+	}
+
+	BUG_ON(priv->scan.req);
+	priv->scan.req = req;
+	priv->scan.n_ssids = 0;
+	priv->scan.status = 0;
+	priv->scan.begin = &req->channels[0];
+	priv->scan.end = &req->channels[req->n_channels];
+	priv->scan.output_power = priv->output_power;
+
+	for (i = 0; i < req->n_ssids; ++i) {
+		if (req->ssids[i].ssid_len) {
+			struct wsm_ssid *dst =
+				&priv->scan.ssids[priv->scan.n_ssids];
+			BUG_ON(req->ssids[i].ssid_len > sizeof(dst->ssid));
+			memcpy(&dst->ssid[0], req->ssids[i].ssid,
+				sizeof(dst->ssid));
+			dst->length = req->ssids[i].ssid_len;
+			++priv->scan.n_ssids;
+		}
+	}
+
+	mutex_unlock(&priv->conf_mutex);
+	if (frame.skb)
+		dev_kfree_skb(frame.skb);
+	queue_work(priv->workqueue, &priv->scan.work);
+	return 0;
+}
+
+void cw1200_scan_work(struct work_struct *work)
+{
+	struct cw1200_common *priv = container_of(work, struct cw1200_common,
+							scan.work);
+	struct ieee80211_channel **it;
+	struct wsm_scan scan = {
+		.scanType = WSM_SCAN_TYPE_FOREGROUND,
+		.scanFlags = WSM_SCAN_FLAG_SPLIT_METHOD,
+	};
+	int i;
+
+	mutex_lock(&priv->conf_mutex);
+
+	if (!priv->scan.req || (priv->scan.begin == priv->scan.end)) {
+		if (priv->scan.output_power != priv->output_power)
+			WARN_ON(wsm_set_output_power(priv,
+						priv->output_power * 10));
+		if (priv->join_status == CW1200_JOIN_STATUS_STA &&
+				priv->powersave_mode.pmMode != WSM_PSM_PS)
+			WARN_ON(wsm_set_pm(priv, &priv->powersave_mode));
+
+		if (priv->scan.req)
+			printk(KERN_DEBUG "[SCAN] Scan completed.\n");
+		else
+			printk(KERN_DEBUG "[SCAN] Scan canceled.\n");
+
+		priv->scan.req = NULL;
+
+		if (priv->delayed_link_loss) {
+			priv->delayed_link_loss = 0;
+			/* Restart beacon loss timer and requeue
+			   BSS loss work. */
+			printk(KERN_DEBUG "[CQM] Requeue BSS loss in %d " \
+					"beacons.\n",
+				priv->cqm_beacon_loss_count);
+			cancel_delayed_work_sync(&priv->bss_loss_work);
+			queue_delayed_work(priv->workqueue,
+					&priv->bss_loss_work,
+					priv->cqm_beacon_loss_count * HZ / 10);
+		}
+
+		wsm_unlock_tx(priv);
+		mutex_unlock(&priv->conf_mutex);
+		ieee80211_scan_completed(priv->hw, priv->scan.status ? 1 : 0);
+		up(&priv->scan.lock);
+		return;
+	} else {
+		struct ieee80211_channel *first = *priv->scan.begin;
+		for (it = priv->scan.begin + 1, i = 1;
+		     it != priv->scan.end && i < WSM_SCAN_MAX_NUM_OF_CHANNELS;
+		     ++it, ++i) {
+			if ((*it)->band != first->band)
+				break;
+			if (((*it)->flags ^ first->flags) &
+					IEEE80211_CHAN_PASSIVE_SCAN)
+				break;
+			if (!(first->flags & IEEE80211_CHAN_PASSIVE_SCAN) &&
+			    (*it)->max_power != first->max_power)
+				break;
+		}
+		scan.band = first->band;
+		/* TODO: Is it optimal? */
+		scan.maxTransmitRate = WSM_TRANSMIT_RATE_1;
+		/* TODO: Is it optimal? */
+		scan.numOfProbeRequests =
+			(first->flags & IEEE80211_CHAN_PASSIVE_SCAN) ? 0 : 2;
+		scan.numOfSSIDs = priv->scan.n_ssids;
+		scan.ssids = &priv->scan.ssids[0];
+		scan.numOfChannels = it - priv->scan.begin;
+		/* TODO: Is it optimal? */
+		scan.probeDelay = 100;
+		scan.ch = kzalloc(
+			sizeof(struct wsm_scan_ch[it - priv->scan.begin]),
+			GFP_KERNEL);
+		if (!scan.ch) {
+			priv->scan.status = -ENOMEM;
+			goto fail;
+		}
+		for (i = 0; i < scan.numOfChannels; ++i) {
+			scan.ch[i].number = priv->scan.begin[i]->hw_value;
+			scan.ch[i].minChannelTime = 50;
+			scan.ch[i].maxChannelTime = 110;
+		}
+		if (!(first->flags & IEEE80211_CHAN_PASSIVE_SCAN) &&
+		    priv->scan.output_power != first->max_power) {
+			priv->scan.output_power = first->max_power;
+			WARN_ON(wsm_set_output_power(priv,
+						priv->scan.output_power * 10));
+		}
+		priv->scan.status = cw1200_scan_start(priv, &scan);
+		kfree(scan.ch);
+		if (WARN_ON(priv->scan.status))
+			goto fail;
+		priv->scan.begin = it;
+	}
+	mutex_unlock(&priv->conf_mutex);
+	return;
+
+fail:
+	priv->scan.begin = priv->scan.end;
+	mutex_unlock(&priv->conf_mutex);
+	queue_work(priv->workqueue, &priv->scan.work);
+	return;
+}
+
+static void cw1200_scan_complete(struct cw1200_common *priv)
+{
+	if (priv->scan.direct_probe) {
+		printk(KERN_DEBUG "[SCAN] Direct probe complete.\n");
+		priv->scan.direct_probe = 0;
+
+		if (priv->delayed_link_loss) {
+			priv->delayed_link_loss = 0;
+			/* Requeue BSS loss work now. Direct probe does not
+			 * affect BSS loss subscription. */
+			printk(KERN_DEBUG "[CQM] Requeue BSS loss now.\n");
+			cancel_delayed_work_sync(&priv->bss_loss_work);
+			queue_delayed_work(priv->workqueue,
+						&priv->bss_loss_work, 0);
+		}
+
+		up(&priv->scan.lock);
+		wsm_unlock_tx(priv);
+	} else {
+		queue_work(priv->workqueue, &priv->scan.work);
+	}
+}
+
+void cw1200_scan_complete_cb(struct cw1200_common *priv,
+				struct wsm_scan_complete *arg)
+{
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+		/* STA is stopped. */
+		return;
+	}
+
+	if (likely(atomic_xchg(&priv->scan.in_progress, 0))) {
+		cancel_delayed_work_sync(&priv->scan.timeout);
+		cw1200_scan_complete(priv);
+	}
+}
+
+void cw1200_scan_timeout(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, scan.timeout.work);
+	if (likely(atomic_xchg(&priv->scan.in_progress, 0))) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"CW1200 FW: Timeout waiting for scan " \
+			"complete notification.\n");
+		cw1200_scan_complete(priv);
+	}
+}
+
+void cw1200_probe_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, scan.probe_work.work);
+	struct wsm_tx *wsm = (struct wsm_tx *)
+		priv->scan.probe_skb->data;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_PROBE_REQUEST,
+		.skb = priv->scan.probe_skb,
+	};
+	struct wsm_ssid ssids[1] = {{
+		.length = 0,
+	} };
+	struct wsm_scan_ch ch[1] = {{
+		.minChannelTime = 0,
+		.maxChannelTime = 10,
+	} };
+	struct wsm_scan scan = {
+		.scanType = WSM_SCAN_TYPE_FOREGROUND,
+		.maxTransmitRate = wsm->maxTxRate,
+		.numOfProbeRequests = 1,
+		.probeDelay = 0,
+		.numOfChannels = 1,
+		.ssids = ssids,
+		.ch = ch,
+	};
+	u8 *ies;
+	size_t ies_len;
+	int ret;
+
+	printk(KERN_DEBUG "[SCAN] Direct probe work.\n");
+
+	if (!priv->channel) {
+		dev_kfree_skb(priv->scan.probe_skb);
+		priv->scan.probe_skb = NULL;
+		wsm_unlock_tx(priv);
+		return;
+	}
+
+	if (unlikely(down_trylock(&priv->scan.lock))) {
+		/* Scan is already in progress. Requeue self. */
+		schedule();
+		queue_delayed_work(priv->workqueue,
+					&priv->scan.probe_work, HZ / 10);
+		return;
+	}
+
+	scan.band = (priv->channel->band == IEEE80211_BAND_5GHZ) ?
+		WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G;
+	ch[0].number = priv->channel->hw_value;
+
+	skb_pull(frame.skb, sizeof(struct wsm_tx));
+
+	ies = &frame.skb->data[sizeof(struct ieee80211_hdr_3addr)];
+	ies_len = frame.skb->len - sizeof(struct ieee80211_hdr_3addr);
+
+	if (ies_len) {
+		u8 *ssidie =
+			(u8 *)cfg80211_find_ie(WLAN_EID_SSID, ies, ies_len);
+		if (ssidie && ssidie[1] && ssidie[1] <= sizeof(ssids[0].ssid)) {
+			u8 *nextie = &ssidie[2 + ssidie[1]];
+			/* Remove SSID from the IE list. It has to be provided
+			 * as a separate argument in cw1200_scan_start call */
+
+			/* Store SSID localy */
+			ssids[0].length = ssidie[1];
+			memcpy(ssids[0].ssid, &ssidie[2], ssids[0].length);
+			scan.numOfSSIDs = 1;
+
+			/* Remove SSID from IE list */
+			ssidie[1] = 0;
+			memmove(&ssidie[2], nextie, &ies[ies_len] - nextie);
+			skb_trim(frame.skb, frame.skb->len - ssids[0].length);
+		}
+	}
+
+	mutex_lock(&priv->conf_mutex);
+	ret = WARN_ON(wsm_set_template_frame(priv, &frame));
+	priv->scan.direct_probe = 1;
+	if (!ret) {
+		wsm_flush_tx(priv);
+		ret = WARN_ON(cw1200_scan_start(priv, &scan));
+	}
+	mutex_unlock(&priv->conf_mutex);
+
+	/* TODO: Report TX status to ieee80211 layer */
+	dev_kfree_skb(priv->scan.probe_skb);
+	priv->scan.probe_skb = NULL;
+
+	if (ret) {
+		priv->scan.direct_probe = 0;
+		up(&priv->scan.lock);
+		wsm_unlock_tx(priv);
+	}
+
+	return;
+}
diff --git a/drivers/staging/cw1200/scan.h b/drivers/staging/cw1200/scan.h
new file mode 100644
index 00000000000..3e7f6a42d6c
--- /dev/null
+++ b/drivers/staging/cw1200/scan.h
@@ -0,0 +1,56 @@
+/*
+ * Scan interface for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef SCAN_H_INCLUDED
+#define SCAN_H_INCLUDED
+
+#include <linux/semaphore.h>
+#include "wsm.h"
+
+/* external */ struct sk_buff;
+/* external */ struct cfg80211_scan_request;
+/* external */ struct ieee80211_channel;
+/* external */ struct ieee80211_hw;
+/* external */ struct work_struct;
+
+struct cw1200_scan {
+	struct semaphore lock;
+	struct work_struct work;
+	struct delayed_work timeout;
+	struct cfg80211_scan_request *req;
+	struct ieee80211_channel **begin;
+	struct ieee80211_channel **end;
+	struct wsm_ssid ssids[WSM_SCAN_MAX_NUM_OF_SSIDS];
+	u8 *ie;
+	size_t ie_len;
+	int output_power;
+	int n_ssids;
+	int status;
+	atomic_t in_progress;
+	/* Direct probe requests workaround */
+	struct delayed_work probe_work;
+	struct sk_buff *probe_skb;
+	int direct_probe;
+};
+
+int cw1200_hw_scan(struct ieee80211_hw *hw,
+			struct ieee80211_vif *vif,
+			struct cfg80211_scan_request *req);
+void cw1200_scan_work(struct work_struct *work);
+void cw1200_scan_timeout(struct work_struct *work);
+void cw1200_scan_complete_cb(struct cw1200_common *priv,
+				struct wsm_scan_complete *arg);
+
+/* ******************************************************************** */
+/* Raw probe requests TX workaround					*/
+void cw1200_probe_work(struct work_struct *work);
+
+#endif
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
new file mode 100644
index 00000000000..65df33ac61f
--- /dev/null
+++ b/drivers/staging/cw1200/sta.c
@@ -0,0 +1,1112 @@
+/*
+ * Mac80211 STA API for ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/vmalloc.h>
+#include <linux/sched.h>
+#include <linux/firmware.h>
+
+#include "cw1200.h"
+#include "sta.h"
+#include "fwio.h"
+#include "bh.h"
+#include "debug.h"
+
+#if defined(CONFIG_CW1200_STA_DEBUG)
+#define sta_printk(...) printk(__VA_ARGS__)
+#else
+#define sta_printk(...)
+#endif
+
+
+static int cw1200_cancel_scan(struct cw1200_common *priv);
+static int __cw1200_flush(struct cw1200_common *priv, bool drop);
+
+static inline void __cw1200_free_event_queue(struct list_head *list)
+{
+	while (!list_empty(list)) {
+		struct cw1200_wsm_event *event =
+			list_first_entry(list, struct cw1200_wsm_event,
+			link);
+		list_del(&event->link);
+		kfree(event);
+	}
+}
+
+/* ******************************************************************** */
+/* STA API								*/
+
+int cw1200_start(struct ieee80211_hw *dev)
+{
+	struct cw1200_common *priv = dev->priv;
+	int ret = 0;
+
+	mutex_lock(&priv->conf_mutex);
+
+	/* default ECDA */
+	WSM_EDCA_SET(&priv->edca, 0, 0x0002, 0x0003, 0x0007, 47);
+	WSM_EDCA_SET(&priv->edca, 1, 0x0002, 0x0007, 0x000f, 94);
+	WSM_EDCA_SET(&priv->edca, 2, 0x0003, 0x000f, 0x03ff, 0);
+	WSM_EDCA_SET(&priv->edca, 3, 0x0007, 0x000f, 0x03ff, 0);
+	ret = wsm_set_edca_params(priv, &priv->edca);
+	if (WARN_ON(ret))
+		goto out;
+
+	memset(priv->bssid, ~0, ETH_ALEN);
+	memcpy(priv->mac_addr, dev->wiphy->perm_addr, ETH_ALEN);
+	priv->mode = NL80211_IFTYPE_MONITOR;
+	priv->softled_state = 0;
+	priv->wep_default_key_id = -1;
+
+	priv->cqm_link_loss_count = 60;
+	priv->cqm_beacon_loss_count = 20;
+
+	ret = cw1200_setup_mac(priv);
+	if (WARN_ON(ret))
+		goto out;
+
+	/* err = cw1200_set_leds(priv); */
+
+out:
+	mutex_unlock(&priv->conf_mutex);
+	return ret;
+}
+
+void cw1200_stop(struct ieee80211_hw *dev)
+{
+	struct cw1200_common *priv = dev->priv;
+	LIST_HEAD(list);
+	int i;
+
+	struct wsm_reset reset = {
+		.reset_statistics = true,
+	};
+
+	wsm_lock_tx(priv);
+
+	while (down_trylock(&priv->scan.lock)) {
+		/* Scan is in progress. Force it to stop. */
+		priv->scan.req = NULL;
+		schedule();
+	}
+	up(&priv->scan.lock);
+
+	mutex_lock(&priv->conf_mutex);
+	cw1200_free_keys(priv);
+	priv->mode = NL80211_IFTYPE_UNSPECIFIED;
+	priv->listening = false;
+	mutex_unlock(&priv->conf_mutex);
+
+	cancel_delayed_work_sync(&priv->scan.probe_work);
+	cancel_delayed_work_sync(&priv->scan.timeout);
+	cancel_delayed_work_sync(&priv->join_timeout);
+	cancel_delayed_work_sync(&priv->bss_loss_work);
+	cancel_delayed_work_sync(&priv->connection_loss_work);
+#if defined(CONFIG_CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE)
+	cancel_delayed_work_sync(&priv->keep_alive_work);
+#endif /* CONFIG_CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE */
+
+	mutex_lock(&priv->conf_mutex);
+	switch (priv->join_status) {
+	case CW1200_JOIN_STATUS_STA:
+		wsm_lock_tx(priv);
+		if (queue_work(priv->workqueue, &priv->unjoin_work) <= 0)
+			wsm_unlock_tx(priv);
+		break;
+	case CW1200_JOIN_STATUS_AP:
+		/* If you see this warning please change the code to iterate
+		 * through the map and reset each link separately. */
+		WARN_ON(priv->link_id_map);
+		priv->sta_asleep_mask = 0;
+		priv->suspend_multicast = false;
+		wsm_reset(priv, &reset);
+		break;
+	case CW1200_JOIN_STATUS_MONITOR:
+		cw1200_update_listening(priv, false);
+		break;
+	default:
+		break;
+	}
+	mutex_unlock(&priv->conf_mutex);
+
+	flush_workqueue(priv->workqueue);
+	mutex_lock(&priv->conf_mutex);
+
+	priv->softled_state = 0;
+	/* cw1200_set_leds(priv); */
+
+	spin_lock(&priv->event_queue_lock);
+	list_splice_init(&priv->event_queue, &list);
+	spin_unlock(&priv->event_queue_lock);
+	__cw1200_free_event_queue(&list);
+
+	priv->delayed_link_loss = 0;
+
+	priv->link_id_map = 0;
+	priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+
+	for (i = 0; i < 4; i++)
+		cw1200_queue_clear(&priv->tx_queue[i]);
+
+	/* HACK! */
+	if (atomic_xchg(&priv->tx_lock, 1) != 1)
+		sta_printk(KERN_DEBUG "[STA] TX is force-unlocked due to stop " \
+			"request.\n");
+
+	wsm_unlock_tx(priv);
+
+	mutex_unlock(&priv->conf_mutex);
+}
+
+int cw1200_add_interface(struct ieee80211_hw *dev,
+			 struct ieee80211_vif *vif)
+{
+	int ret;
+	struct cw1200_common *priv = dev->priv;
+	/* __le32 auto_calibration_mode = __cpu_to_le32(1); */
+
+	mutex_lock(&priv->conf_mutex);
+
+	if (priv->mode != NL80211_IFTYPE_MONITOR) {
+		mutex_unlock(&priv->conf_mutex);
+		return -EOPNOTSUPP;
+	}
+
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_MESH_POINT:
+	case NL80211_IFTYPE_AP:
+		priv->mode = vif->type;
+		break;
+	default:
+		mutex_unlock(&priv->conf_mutex);
+		return -EOPNOTSUPP;
+	}
+
+	priv->vif = vif;
+	memcpy(priv->mac_addr, vif->addr, ETH_ALEN);
+
+	ret = WARN_ON(cw1200_setup_mac(priv));
+	/* Enable auto-calibration */
+	/* Exception in subsequent channel switch; disabled.
+	WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_SET_AUTO_CALIBRATION_MODE,
+		&auto_calibration_mode, sizeof(auto_calibration_mode)));
+	*/
+
+	mutex_unlock(&priv->conf_mutex);
+	return ret;
+}
+
+void cw1200_remove_interface(struct ieee80211_hw *dev,
+			     struct ieee80211_vif *vif)
+{
+	struct cw1200_common *priv = dev->priv;
+
+	struct wsm_reset reset = {
+		.reset_statistics = true,
+	};
+
+	mutex_lock(&priv->conf_mutex);
+
+	priv->vif = NULL;
+	priv->mode = NL80211_IFTYPE_MONITOR;
+	memset(priv->mac_addr, 0, ETH_ALEN);
+	memset(priv->bssid, 0, ETH_ALEN);
+	WARN_ON(wsm_reset(priv, &reset));
+	cw1200_free_keys(priv);
+	cw1200_setup_mac(priv);
+	priv->listening = false;
+	priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+
+	mutex_unlock(&priv->conf_mutex);
+}
+
+int cw1200_config(struct ieee80211_hw *dev, u32 changed)
+{
+	int ret = 0;
+	struct cw1200_common *priv = dev->priv;
+	struct ieee80211_conf *conf = &dev->conf;
+
+	mutex_lock(&priv->conf_mutex);
+	/* TODO: IEEE80211_CONF_CHANGE_QOS */
+	if (changed & IEEE80211_CONF_CHANGE_POWER) {
+		priv->output_power = conf->power_level;
+		sta_printk(KERN_DEBUG "[STA] TX power: %d\n", priv->output_power);
+		WARN_ON(wsm_set_output_power(priv, priv->output_power * 10));
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_LISTEN_INTERVAL) {
+		/* TODO: Not sure. Needs to be verified. */
+		/* TODO: DTIM skipping */
+		int dtim_interval = conf->ps_dtim_period;
+		int listen_interval = conf->listen_interval;
+		if (dtim_interval < 1)
+			dtim_interval = 1;
+		if (listen_interval < dtim_interval)
+			listen_interval = 0;
+		/* TODO: max_sleep_period is not supported
+		 * and silently skipped. */
+		sta_printk(KERN_DEBUG "[STA] DTIM %d, listen %d\n",
+			dtim_interval, listen_interval);
+		WARN_ON(wsm_set_beacon_wakeup_period(priv,
+			dtim_interval, listen_interval));
+	}
+
+
+	if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) &&
+			(priv->channel != conf->channel)) {
+		struct ieee80211_channel *ch = conf->channel;
+		struct wsm_switch_channel channel = {
+			.newChannelNumber = ch->hw_value,
+		};
+		cw1200_cancel_scan(priv);
+		sta_printk(KERN_DEBUG "[STA] Freq %d (wsm ch: %d).\n",
+			ch->center_freq, ch->hw_value);
+		WARN_ON(wait_event_interruptible_timeout(
+			priv->channel_switch_done,
+			!priv->channel_switch_in_progress, 3 * HZ) <= 0);
+
+		ret = WARN_ON(__cw1200_flush(priv, false));
+		if (!ret) {
+			ret = WARN_ON(wsm_switch_channel(priv, &channel));
+			if (!ret)
+				priv->channel = ch;
+			else
+				wsm_unlock_tx(priv);
+		}
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_PS) {
+		priv->powersave_mode.pmMode =
+				(conf->flags & IEEE80211_CONF_PS) ?
+				WSM_PSM_PS : WSM_PSM_ACTIVE;
+		if (priv->join_status == CW1200_JOIN_STATUS_STA)
+			WARN_ON(wsm_set_pm(priv, &priv->powersave_mode));
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_MONITOR) {
+		/* TBD: It looks like it's transparent
+		 * there's a monitor interface present -- use this
+		 * to determine for example whether to calculate
+		 * timestamps for packets or not, do not use instead
+		 * of filter flags! */
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
+		struct wsm_operational_mode mode = {
+			.power_mode = (conf->flags & IEEE80211_CONF_IDLE) ?
+				wsm_power_mode_quiescent :
+				wsm_power_mode_doze,
+			.disableMoreFlagUsage = true,
+		};
+		wsm_lock_tx(priv);
+		WARN_ON(wsm_set_operational_mode(priv, &mode));
+		wsm_unlock_tx(priv);
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS) {
+		sta_printk(KERN_DEBUG "[STA] Retry limits: %d (long), " \
+			"%d (short).\n",
+			conf->long_frame_max_tx_count,
+			conf->short_frame_max_tx_count);
+		spin_lock_bh(&priv->tx_policy_cache.lock);
+		priv->long_frame_max_tx_count = conf->long_frame_max_tx_count;
+		priv->short_frame_max_tx_count =
+			(conf->short_frame_max_tx_count < 0x0F) ?
+			conf->short_frame_max_tx_count : 0x0F;
+		priv->hw->max_rate_tries = priv->short_frame_max_tx_count;
+		spin_unlock_bh(&priv->tx_policy_cache.lock);
+		/* TBD: I think we don't need tx_policy_force_upload().
+		 * Outdated policies will leave cache in a normal way. */
+		/* WARN_ON(tx_policy_force_upload(priv)); */
+	}
+	mutex_unlock(&priv->conf_mutex);
+	return ret;
+}
+
+void cw1200_update_filtering(struct cw1200_common *priv)
+{
+	int ret;
+
+	if (priv->join_status == CW1200_JOIN_STATUS_PASSIVE)
+		return;
+
+	ret = wsm_set_rx_filter(priv, &priv->rx_filter);
+	if (!ret)
+		ret = wsm_beacon_filter_control(priv, &priv->bf_control);
+	if (!ret)
+		ret = wsm_set_bssid_filtering(priv, !priv->rx_filter.bssid);
+	if (ret)
+		wiphy_err(priv->hw->wiphy,
+				"%s: Update filtering failed: %d.\n",
+				__func__, ret);
+	return;
+}
+
+void cw1200_configure_filter(struct ieee80211_hw *dev,
+			     unsigned int changed_flags,
+			     unsigned int *total_flags,
+			     u64 multicast)
+{
+	struct cw1200_common *priv = dev->priv;
+	bool listening = !!(*total_flags &
+			(FIF_PROMISC_IN_BSS |
+			 FIF_OTHER_BSS |
+			 FIF_BCN_PRBRESP_PROMISC |
+			 FIF_PROBE_REQ));
+
+	*total_flags &= FIF_PROMISC_IN_BSS |
+			FIF_OTHER_BSS |
+			FIF_FCSFAIL |
+			FIF_BCN_PRBRESP_PROMISC |
+			FIF_PROBE_REQ;
+
+	mutex_lock(&priv->conf_mutex);
+
+	priv->rx_filter.promiscuous = (*total_flags & FIF_PROMISC_IN_BSS)
+			? 1 : 0;
+	priv->rx_filter.bssid = (*total_flags & (FIF_OTHER_BSS |
+			FIF_PROBE_REQ)) ? 1 : 0;
+	priv->rx_filter.fcs = (*total_flags & FIF_FCSFAIL) ? 1 : 0;
+	priv->bf_control.bcn_count = (*total_flags &
+			(FIF_BCN_PRBRESP_PROMISC |
+			 FIF_PROMISC_IN_BSS |
+			 FIF_PROBE_REQ)) ? 1 : 0;
+	if (priv->listening ^ listening) {
+		priv->listening = listening;
+		cw1200_update_listening(priv, listening);
+	}
+	cw1200_update_filtering(priv);
+	mutex_unlock(&priv->conf_mutex);
+}
+
+int cw1200_conf_tx(struct ieee80211_hw *dev, u16 queue,
+		   const struct ieee80211_tx_queue_params *params)
+{
+	struct cw1200_common *priv = dev->priv;
+	int ret = 0;
+
+	mutex_lock(&priv->conf_mutex);
+
+	if (queue < dev->queues) {
+		WSM_EDCA_SET(&priv->edca, queue, params->aifs,
+			params->cw_min, params->cw_max, params->txop);
+		ret = wsm_set_edca_params(priv, &priv->edca);
+	} else
+		ret = -EINVAL;
+
+	mutex_unlock(&priv->conf_mutex);
+	return ret;
+}
+
+int cw1200_get_stats(struct ieee80211_hw *dev,
+		     struct ieee80211_low_level_stats *stats)
+{
+	struct cw1200_common *priv = dev->priv;
+
+	memcpy(stats, &priv->stats, sizeof(*stats));
+	return 0;
+}
+
+/*
+int cw1200_get_tx_stats(struct ieee80211_hw *dev,
+			struct ieee80211_tx_queue_stats *stats)
+{
+	int i;
+	struct cw1200_common *priv = dev->priv;
+
+	for (i = 0; i < dev->queues; ++i)
+		cw1200_queue_get_stats(&priv->tx_queue[i], &stats[i]);
+
+	return 0;
+}
+*/
+
+int cw1200_set_key(struct ieee80211_hw *dev, enum set_key_cmd cmd,
+		   struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+		   struct ieee80211_key_conf *key)
+{
+	int ret = -EOPNOTSUPP;
+	struct cw1200_common *priv = dev->priv;
+
+	mutex_lock(&priv->conf_mutex);
+
+	if (cmd == SET_KEY) {
+		u8 *peer_addr = NULL;
+		int pairwise = (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) ?
+			1 : 0;
+		int idx = cw1200_alloc_key(priv);
+		struct wsm_add_key *wsm_key = &priv->keys[idx];
+
+		if (idx < 0) {
+			ret = -EINVAL;
+			goto finally;
+		}
+
+		BUG_ON(pairwise && !sta);
+		if (sta)
+			peer_addr = sta->addr;
+
+		switch (key->cipher) {
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			if (key->keylen > 16) {
+				cw1200_free_key(priv, idx);
+				ret = -EINVAL;
+				goto finally;
+			}
+
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_WEP_PAIRWISE;
+				memcpy(wsm_key->wepPairwiseKey.peerAddress,
+					 peer_addr, ETH_ALEN);
+				memcpy(wsm_key->wepPairwiseKey.keyData,
+					&key->key[0], key->keylen);
+				wsm_key->wepPairwiseKey.keyLength = key->keylen;
+			} else {
+				wsm_key->type = WSM_KEY_TYPE_WEP_DEFAULT;
+				memcpy(wsm_key->wepGroupKey.keyData,
+					&key->key[0], key->keylen);
+				wsm_key->wepGroupKey.keyLength = key->keylen;
+				wsm_key->wepGroupKey.keyId = key->keyidx;
+			}
+			break;
+		case WLAN_CIPHER_SUITE_TKIP:
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_TKIP_PAIRWISE;
+				memcpy(wsm_key->tkipPairwiseKey.peerAddress,
+					peer_addr, ETH_ALEN);
+				memcpy(wsm_key->tkipPairwiseKey.tkipKeyData,
+					&key->key[0],  16);
+				memcpy(wsm_key->tkipPairwiseKey.txMicKey,
+					&key->key[16],  8);
+				memcpy(wsm_key->tkipPairwiseKey.rxMicKey,
+					&key->key[24],  8);
+			} else {
+				size_t mic_offset =
+					(priv->mode == NL80211_IFTYPE_AP) ?
+					16 : 24;
+				wsm_key->type = WSM_KEY_TYPE_TKIP_GROUP;
+				memcpy(wsm_key->tkipGroupKey.tkipKeyData,
+					&key->key[0],  16);
+				memcpy(wsm_key->tkipGroupKey.rxMicKey,
+					&key->key[mic_offset],  8);
+
+				/* TODO: Where can I find TKIP SEQ? */
+				memset(wsm_key->tkipGroupKey.rxSeqCounter,
+					0,		8);
+				wsm_key->tkipGroupKey.keyId = key->keyidx;
+
+				print_hex_dump_bytes("TKIP: ", DUMP_PREFIX_NONE,
+					key->key, key->keylen);
+			}
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_AES_PAIRWISE;
+				memcpy(wsm_key->aesPairwiseKey.peerAddress,
+					peer_addr, ETH_ALEN);
+				memcpy(wsm_key->aesPairwiseKey.aesKeyData,
+					&key->key[0],  16);
+			} else {
+				wsm_key->type = WSM_KEY_TYPE_AES_GROUP;
+				memcpy(wsm_key->aesGroupKey.aesKeyData,
+					&key->key[0],  16);
+				/* TODO: Where can I find AES SEQ? */
+				memset(wsm_key->aesGroupKey.rxSeqCounter,
+					0,              8);
+				wsm_key->aesGroupKey.keyId = key->keyidx;
+			}
+			break;
+#if 0
+		case WLAN_CIPHER_SUITE_WAPI:
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_WAPI_PAIRWISE;
+				memcpy(wsm_key->wapiPairwiseKey.peerAddress,
+					peer_addr, ETH_ALEN);
+				memcpy(wsm_key->wapiPairwiseKey.wapiKeyData,
+					&key->key[0],  16);
+				memcpy(wsm_key->wapiPairwiseKey.micKeyData,
+					&key->key[16], 16);
+				wsm_key->wapiPairwiseKey.keyId = key->keyidx;
+			} else {
+				wsm_key->type = WSM_KEY_TYPE_WAPI_GROUP;
+				memcpy(wsm_key->wapiGroupKey.wapiKeyData,
+					&key->key[0],  16);
+				memcpy(wsm_key->wapiGroupKey.micKeyData,
+					&key->key[16], 16);
+				wsm_key->wapiGroupKey.keyId = key->keyidx;
+			}
+			break;
+#endif
+		default:
+			WARN_ON(1);
+			cw1200_free_key(priv, idx);
+			ret = -EOPNOTSUPP;
+			goto finally;
+		}
+		ret = WARN_ON(wsm_add_key(priv, wsm_key));
+		if (!ret)
+			key->hw_key_idx = idx;
+		else
+			cw1200_free_key(priv, idx);
+	} else if (cmd == DISABLE_KEY) {
+		struct wsm_remove_key wsm_key = {
+			.entryIndex = key->hw_key_idx,
+		};
+
+		if (wsm_key.entryIndex > WSM_KEY_MAX_INDEX) {
+			ret = -EINVAL;
+			goto finally;
+		}
+
+		cw1200_free_key(priv, wsm_key.entryIndex);
+		ret = wsm_remove_key(priv, &wsm_key);
+	} else {
+		BUG_ON("Unsupported command");
+	}
+
+finally:
+	mutex_unlock(&priv->conf_mutex);
+	return ret;
+}
+
+void cw1200_wep_key_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, wep_key_work);
+	__le32 wep_default_key_id = __cpu_to_le32(
+		priv->wep_default_key_id);
+
+	sta_printk(KERN_DEBUG "[STA] Setting default WEP key: %d\n",
+		priv->wep_default_key_id);
+	wsm_flush_tx(priv);
+	WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_DOT11_WEP_DEFAULT_KEY_ID,
+		&wep_default_key_id, sizeof(wep_default_key_id)));
+	wsm_unlock_tx(priv);
+}
+
+int cw1200_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
+{
+	int ret;
+	__le32 val32;
+
+	if (value != (u32) -1)
+		val32 = __cpu_to_le32(value);
+	else
+		val32 = 0; /* disabled */
+
+	/* mutex_lock(&priv->conf_mutex); */
+	ret = WARN_ON(wsm_write_mib(hw->priv, WSM_MIB_ID_DOT11_RTS_THRESHOLD,
+		&val32, sizeof(val32)));
+	/* mutex_unlock(&priv->conf_mutex); */
+	return ret;
+}
+
+static int __cw1200_flush(struct cw1200_common *priv, bool drop)
+{
+	int i, ret;
+
+	if (drop) {
+		for (i = 0; i < 4; ++i)
+			cw1200_queue_clear(&priv->tx_queue[i]);
+	}
+
+	for (;;) {
+		ret = wait_event_interruptible_timeout(
+				priv->tx_queue_stats.wait_link_id_empty,
+				cw1200_queue_stats_is_empty(
+					&priv->tx_queue_stats, -1),
+				10 * HZ);
+
+		if (unlikely(ret <= 0)) {
+			if (!ret)
+				ret = -ETIMEDOUT;
+			break;
+		} else {
+			ret = 0;
+		}
+		ret = 0;
+
+		wsm_lock_tx(priv);
+		if (unlikely(!cw1200_queue_stats_is_empty(
+				&priv->tx_queue_stats, -1))) {
+			/* Highly unlekely: WSM requeued frames. */
+			wsm_unlock_tx(priv);
+			continue;
+		}
+		break;
+	}
+	return ret;
+}
+
+void cw1200_flush(struct ieee80211_hw *hw, bool drop)
+{
+	struct cw1200_common *priv = hw->priv;
+
+	if (!WARN_ON(__cw1200_flush(priv, drop)))
+		wsm_unlock_tx(priv);
+
+	return;
+}
+
+/* ******************************************************************** */
+/* WSM callbacks							*/
+
+void cw1200_channel_switch_cb(struct cw1200_common *priv)
+{
+	wsm_unlock_tx(priv);
+}
+
+void cw1200_free_event_queue(struct cw1200_common *priv)
+{
+	LIST_HEAD(list);
+
+	spin_lock(&priv->event_queue_lock);
+	list_splice_init(&priv->event_queue, &list);
+	spin_unlock(&priv->event_queue_lock);
+
+	__cw1200_free_event_queue(&list);
+}
+
+void cw1200_event_handler(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, event_handler);
+	struct cw1200_wsm_event *event;
+	LIST_HEAD(list);
+
+	spin_lock(&priv->event_queue_lock);
+	list_splice_init(&priv->event_queue, &list);
+	spin_unlock(&priv->event_queue_lock);
+
+	list_for_each_entry(event, &list, link) {
+		switch (event->evt.eventId) {
+		case WSM_EVENT_ERROR:
+			/* I even don't know what is it about.. */
+			STUB();
+			break;
+		case WSM_EVENT_BSS_LOST:
+		{
+			sta_printk(KERN_DEBUG "[CQM] BSS lost.\n");
+			cancel_delayed_work_sync(&priv->bss_loss_work);
+			cancel_delayed_work_sync(&priv->connection_loss_work);
+			if (!down_trylock(&priv->scan.lock)) {
+				up(&priv->scan.lock);
+				priv->delayed_link_loss = 0;
+				queue_delayed_work(priv->workqueue,
+						&priv->bss_loss_work, 0);
+			} else {
+				/* Scan is in progress. Delay reporting. */
+				/* Scan complete will trigger bss_loss_work */
+				priv->delayed_link_loss = 1;
+				/* Also we're starting watchdog. */
+				queue_delayed_work(priv->workqueue,
+						&priv->bss_loss_work, 10 * HZ);
+			}
+			break;
+		}
+		case WSM_EVENT_BSS_REGAINED:
+		{
+			sta_printk(KERN_DEBUG "[CQM] BSS regained.\n");
+			priv->delayed_link_loss = 0;
+			cancel_delayed_work_sync(&priv->bss_loss_work);
+			cancel_delayed_work_sync(&priv->connection_loss_work);
+			break;
+		}
+		case WSM_EVENT_RADAR_DETECTED:
+			STUB();
+			break;
+		case WSM_EVENT_RCPI_RSSI:
+		{
+			int rssi = (int)(s8)(event->evt.eventData & 0xFF);
+			int cqm_evt = (rssi <= priv->cqm_rssi_thold) ?
+				NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW :
+				NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH;
+			sta_printk(KERN_DEBUG "[CQM] RSSI event: %d", rssi);
+			ieee80211_cqm_rssi_notify(priv->vif, cqm_evt,
+								GFP_KERNEL);
+			break;
+		}
+		case WSM_EVENT_BT_INACTIVE:
+			STUB();
+			break;
+		case WSM_EVENT_BT_ACTIVE:
+			STUB();
+			break;
+		}
+	}
+	__cw1200_free_event_queue(&list);
+}
+
+void cw1200_bss_loss_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, bss_loss_work.work);
+	int timeout; /* in beacons */
+
+	timeout = priv->cqm_link_loss_count -
+		priv->cqm_beacon_loss_count;
+
+	if (priv->cqm_beacon_loss_count) {
+		sta_printk(KERN_DEBUG "[CQM] Beacon loss.\n");
+		if (timeout <= 0)
+			timeout = 0;
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+		ieee80211_cqm_beacon_miss_notify(priv->vif, GFP_KERNEL);
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+	} else {
+		timeout = 0;
+	}
+
+	cancel_delayed_work_sync(&priv->connection_loss_work);
+	queue_delayed_work(priv->workqueue,
+		&priv->connection_loss_work,
+		timeout * HZ / 10);
+}
+
+void cw1200_connection_loss_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common,
+				connection_loss_work.work);
+	sta_printk(KERN_DEBUG "[CQM] Reporting connection loss.\n");
+	ieee80211_connection_loss(priv->vif);
+}
+
+#if defined(CONFIG_CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE)
+void cw1200_keep_alive_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, keep_alive_work.work);
+	unsigned long now = jiffies;
+	unsigned long delta = now - priv->last_activity_time;
+	unsigned long tmo = 30 * HZ;
+
+	if (delta >= tmo) {
+		sta_printk(KERN_DEBUG "[CQM] Keep-alive ping.\n");
+		STUB();
+		/* TODO: Do a keep-alive ping :) */
+		priv->last_activity_time = now;
+	} else {
+		tmo -= delta;
+	}
+	queue_delayed_work(priv->workqueue,
+		&priv->keep_alive_work, tmo);
+}
+#endif /* CONFIG_CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE */
+
+void cw1200_tx_failure_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, tx_failure_work);
+	sta_printk(KERN_DEBUG "[CQM] Reporting TX failure.\n");
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	ieee80211_cqm_tx_fail_notify(priv->vif, GFP_KERNEL);
+#else /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+	(void)priv;
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+}
+
+/* ******************************************************************** */
+/* Internal API								*/
+
+int cw1200_setup_mac(struct cw1200_common *priv)
+{
+	/* TBD: Do you know how to assing MAC address without
+	 * annoying uploading RX data? */
+	u8 prev_mac[ETH_ALEN];
+
+	/* NOTE: There is a bug in FW: it reports signal
+	* as RSSI if RSSI subscription is enabled.
+	* It's not enough to set WSM_RCPI_RSSI_USE_RSSI. */
+	struct wsm_rcpi_rssi_threshold threshold = {
+		.rssiRcpiMode = WSM_RCPI_RSSI_USE_RSSI |
+		WSM_RCPI_RSSI_THRESHOLD_ENABLE |
+		WSM_RCPI_RSSI_DONT_USE_UPPER |
+		WSM_RCPI_RSSI_DONT_USE_LOWER,
+		.rollingAverageCount = 16,
+	};
+	int ret = 0;
+
+	if (wsm_get_station_id(priv, &prev_mac[0])
+	    || memcmp(prev_mac, priv->mac_addr, ETH_ALEN)) {
+		const char *sdd_path = NULL;
+		const struct firmware *firmware = NULL;
+		struct wsm_configuration cfg = {
+			.dot11StationId = &priv->mac_addr[0],
+		};
+
+		switch (priv->hw_revision) {
+		case CW1200_HW_REV_CUT10:
+			sdd_path = SDD_FILE_10;
+			break;
+		case CW1200_HW_REV_CUT11:
+			sdd_path = SDD_FILE_11;
+			break;
+		case CW1200_HW_REV_CUT20:
+			sdd_path = SDD_FILE_20;
+			break;
+		case CW1200_HW_REV_CUT22:
+			sdd_path = SDD_FILE_22;
+			break;
+		default:
+			BUG_ON(1);
+		}
+
+		ret = request_firmware(&firmware,
+			sdd_path, priv->pdev);
+
+		if (unlikely(ret)) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: can't load sdd file %s.\n",
+				__func__, sdd_path);
+			return ret;
+		}
+
+		cfg.dpdData = firmware->data;
+		cfg.dpdData_size = firmware->size;
+		ret = WARN_ON(wsm_configuration(priv, &cfg));
+
+		release_firmware(firmware);
+	}
+	if (ret)
+		return ret;
+
+	/* Configure RSSI/SCPI reporting as RSSI. */
+	WARN_ON(wsm_set_rcpi_rssi_threshold(priv, &threshold));
+
+	/* TODO: */
+	switch (priv->mode) {
+	case NL80211_IFTYPE_STATION:
+	case NL80211_IFTYPE_MONITOR:
+	case NL80211_IFTYPE_AP:
+		break;
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_MESH_POINT:
+		/* TODO: Not verified yet. */
+		STUB();
+		break;
+	}
+
+	return 0;
+}
+
+void cw1200_join_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, join_work);
+	const struct wsm_tx *wsm = priv->join_pending_frame;
+	const u8 *frame = (u8 *)&wsm[1];
+	const u8 *bssid = &frame[4]; /* AP SSID in a 802.11 frame */
+	struct cfg80211_bss *bss;
+	const u8 *ssidie;
+	const u8 *dtimie;
+	const struct ieee80211_tim_ie *tim = NULL;
+	u8 queueId = wsm_queue_id_to_linux(wsm->queueId);
+
+	cancel_delayed_work_sync(&priv->join_timeout);
+
+	bss = cfg80211_get_bss(priv->hw->wiphy, NULL, bssid, NULL, 0, 0, 0);
+	if (!bss) {
+		priv->join_pending_frame = NULL;
+		cw1200_queue_remove(&priv->tx_queue[queueId],
+			priv, __le32_to_cpu(wsm->packetID));
+		return;
+	}
+	ssidie = cfg80211_find_ie(WLAN_EID_SSID,
+		bss->information_elements,
+		bss->len_information_elements);
+	dtimie = cfg80211_find_ie(WLAN_EID_TIM,
+		bss->information_elements,
+		bss->len_information_elements);
+	if (dtimie)
+		tim = (struct ieee80211_tim_ie *)&dtimie[2];
+
+	mutex_lock(&priv->conf_mutex);
+	{
+		struct wsm_join join = {
+			.mode = (bss->capability & WLAN_CAPABILITY_IBSS) ?
+				WSM_JOIN_MODE_IBSS : WSM_JOIN_MODE_BSS,
+			.preambleType = WSM_JOIN_PREAMBLE_SHORT,
+			.probeForJoin = 1,
+			/* dtimPeriod will be updated after association */
+			.dtimPeriod = 1,
+			.beaconInterval = bss->beacon_interval,
+			/* basicRateSet will be updated after association */
+			.basicRateSet = 7,
+		};
+
+		if (tim && tim->dtim_period > 1) {
+			join.dtimPeriod = tim->dtim_period;
+			priv->join_dtim_period = tim->dtim_period;
+			sta_printk(KERN_DEBUG "[STA] Join DTIM: %d\n",
+				join.dtimPeriod);
+		}
+
+		priv->join_pending_frame = NULL;
+		BUG_ON(!wsm);
+		BUG_ON(!priv->channel);
+
+		join.channelNumber = priv->channel->hw_value;
+		join.band = (priv->channel->band == IEEE80211_BAND_5GHZ) ?
+			WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G;
+
+		memcpy(&join.bssid[0], bssid, sizeof(join.bssid));
+		memcpy(&priv->join_bssid[0], bssid, sizeof(priv->join_bssid));
+
+		if (ssidie) {
+			join.ssidLength = ssidie[1];
+			if (WARN_ON(join.ssidLength > sizeof(join.ssid)))
+				join.ssidLength = sizeof(join.ssid);
+			memcpy(&join.ssid[0], &ssidie[2], join.ssidLength);
+		}
+
+		wsm_flush_tx(priv);
+
+		WARN_ON(wsm_set_block_ack_policy(priv,
+				priv->ba_tid_mask, priv->ba_tid_mask));
+
+#if defined(CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE)
+		priv->last_activity_time = jiffies;
+		/* Queue keep-alive ping avery 30 sec. */
+		queue_delayed_work(priv->workqueue,
+			&priv->keep_alive_work, 30 * HZ);
+#endif /* CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE */
+		/* Queue unjoin if not associated in 3 sec. */
+		queue_delayed_work(priv->workqueue,
+			&priv->join_timeout, 3 * HZ);
+
+		cw1200_update_listening(priv, false);
+		if (wsm_join(priv, &join)) {
+			memset(&priv->join_bssid[0],
+				0, sizeof(priv->join_bssid));
+			cw1200_queue_remove(&priv->tx_queue[queueId],
+				priv, __le32_to_cpu(wsm->packetID));
+			cancel_delayed_work_sync(&priv->join_timeout);
+#if defined(CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE)
+			cancel_delayed_work_sync(&priv->keep_alive_work);
+#endif /* CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE */
+			cw1200_update_listening(priv, priv->listening);
+			WARN_ON(wsm_set_pm(priv, &priv->powersave_mode));
+		} else {
+			/* Upload keys */
+			WARN_ON(cw1200_upload_keys(priv));
+#if !defined(CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE)
+			WARN_ON(wsm_keep_alive_period(priv, 30 /* sec */));
+#endif /* CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE */
+			cw1200_queue_requeue(&priv->tx_queue[queueId],
+				__le32_to_cpu(wsm->packetID));
+		}
+		cw1200_update_filtering(priv);
+	}
+	mutex_unlock(&priv->conf_mutex);
+	cfg80211_put_bss(bss);
+}
+
+void cw1200_join_timeout(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, join_timeout.work);
+	sta_printk(KERN_DEBUG "[WSM] Issue unjoin command (TMO).\n");
+	wsm_lock_tx(priv);
+	cw1200_unjoin_work(&priv->unjoin_work);
+}
+
+void cw1200_unjoin_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, unjoin_work);
+
+	struct wsm_reset reset = {
+		.reset_statistics = true,
+	};
+
+	mutex_lock(&priv->conf_mutex);
+	BUG_ON(priv->join_status &&
+			priv->join_status != CW1200_JOIN_STATUS_STA);
+	if (priv->join_status == CW1200_JOIN_STATUS_STA) {
+		memset(&priv->join_bssid[0], 0, sizeof(priv->join_bssid));
+		priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+
+		/* Unjoin is a reset. */
+		wsm_flush_tx(priv);
+		WARN_ON(wsm_reset(priv, &reset));
+		priv->join_dtim_period = 0;
+		WARN_ON(cw1200_setup_mac(priv));
+		cw1200_free_event_queue(priv);
+		cancel_work_sync(&priv->event_handler);
+		cancel_delayed_work_sync(&priv->connection_loss_work);
+#if defined(CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE)
+		cancel_delayed_work_sync(&priv->keep_alive_work);
+#endif /* CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE */
+		cw1200_update_listening(priv, priv->listening);
+		cw1200_update_filtering(priv);
+		sta_printk(KERN_DEBUG "[STA] Unjoin.\n");
+	}
+	mutex_unlock(&priv->conf_mutex);
+	wsm_unlock_tx(priv);
+}
+
+static inline int cw1200_enable_listening(struct cw1200_common *priv)
+{
+	struct wsm_start start = {
+		.mode = WSM_START_MODE_P2P_DEV,
+		.band = (priv->channel->band == IEEE80211_BAND_5GHZ) ?
+				WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G,
+		.channelNumber = priv->channel->hw_value,
+		.beaconInterval = 100,
+		.DTIMPeriod = 1,
+		.probeDelay = 0,
+		.basicRateSet = 0x0F,
+	};
+	return wsm_start(priv, &start);
+}
+
+static inline int cw1200_disable_listening(struct cw1200_common *priv)
+{
+	struct wsm_reset reset = {
+		.reset_statistics = true,
+	};
+	return wsm_reset(priv, &reset);
+}
+
+void cw1200_update_listening(struct cw1200_common *priv, bool enabled)
+{
+	if (enabled) {
+		switch (priv->join_status) {
+		case CW1200_JOIN_STATUS_PASSIVE:
+			if (!WARN_ON(cw1200_enable_listening(priv)))
+				priv->join_status = CW1200_JOIN_STATUS_MONITOR;
+			break;
+		default:
+			break;
+		}
+	} else {
+		switch (priv->join_status) {
+		case CW1200_JOIN_STATUS_MONITOR:
+			if (!WARN_ON(cw1200_disable_listening(priv)))
+				priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+		default:
+			break;
+		}
+	}
+}
+
+
+/* ******************************************************************** */
+/* STA privates								*/
+
+static int cw1200_cancel_scan(struct cw1200_common *priv)
+{
+	/* STUB(); */
+	return 0;
+}
diff --git a/drivers/staging/cw1200/sta.h b/drivers/staging/cw1200/sta.h
new file mode 100644
index 00000000000..2a3c8836c0b
--- /dev/null
+++ b/drivers/staging/cw1200/sta.h
@@ -0,0 +1,73 @@
+/*
+ * Mac80211 STA interface for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef STA_H_INCLUDED
+#define STA_H_INCLUDED
+
+/* ******************************************************************** */
+/* mac80211 API								*/
+
+int cw1200_start(struct ieee80211_hw *dev);
+void cw1200_stop(struct ieee80211_hw *dev);
+int cw1200_add_interface(struct ieee80211_hw *dev,
+			 struct ieee80211_vif *vif);
+void cw1200_remove_interface(struct ieee80211_hw *dev,
+			     struct ieee80211_vif *vif);
+int cw1200_config(struct ieee80211_hw *dev, u32 changed);
+void cw1200_configure_filter(struct ieee80211_hw *dev,
+			     unsigned int changed_flags,
+			     unsigned int *total_flags,
+			     u64 multicast);
+int cw1200_conf_tx(struct ieee80211_hw *dev, u16 queue,
+		   const struct ieee80211_tx_queue_params *params);
+int cw1200_get_stats(struct ieee80211_hw *dev,
+		     struct ieee80211_low_level_stats *stats);
+/* Not more a part of interface?
+int cw1200_get_tx_stats(struct ieee80211_hw *dev,
+			struct ieee80211_tx_queue_stats *stats);
+*/
+int cw1200_set_key(struct ieee80211_hw *dev, enum set_key_cmd cmd,
+		   struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+		   struct ieee80211_key_conf *key);
+
+int cw1200_set_rts_threshold(struct ieee80211_hw *hw, u32 value);
+
+void cw1200_flush(struct ieee80211_hw *hw, bool drop);
+
+/* ******************************************************************** */
+/* WSM callbacks							*/
+
+/* void cw1200_set_pm_complete_cb(struct cw1200_common *priv,
+	struct wsm_set_pm_complete *arg); */
+void cw1200_channel_switch_cb(struct cw1200_common *priv);
+
+/* ******************************************************************** */
+/* WSM events								*/
+
+void cw1200_free_event_queue(struct cw1200_common *priv);
+void cw1200_event_handler(struct work_struct *work);
+void cw1200_bss_loss_work(struct work_struct *work);
+void cw1200_connection_loss_work(struct work_struct *work);
+void cw1200_keep_alive_work(struct work_struct *work);
+void cw1200_tx_failure_work(struct work_struct *work);
+
+/* ******************************************************************** */
+/* Internal API								*/
+
+int cw1200_setup_mac(struct cw1200_common *priv);
+void cw1200_join_work(struct work_struct *work);
+void cw1200_join_timeout(struct work_struct *work);
+void cw1200_unjoin_work(struct work_struct *work);
+void cw1200_wep_key_work(struct work_struct *work);
+void cw1200_update_listening(struct cw1200_common *priv, bool enabled);
+void cw1200_update_filtering(struct cw1200_common *priv);
+
+#endif
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
new file mode 100644
index 00000000000..8616d1d2ffe
--- /dev/null
+++ b/drivers/staging/cw1200/txrx.c
@@ -0,0 +1,800 @@
+/*
+ * Datapath implementation for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <net/mac80211.h>
+
+#include "cw1200.h"
+#include "wsm.h"
+#include "bh.h"
+#include "debug.h"
+
+#if defined(CONFIG_CW1200_TX_POLICY_DEBUG)
+#define tx_policy_printk(...) printk(__VA_ARGS__)
+#else
+#define tx_policy_printk(...)
+#endif
+
+static int cw1200_handle_action_rx(struct cw1200_common *priv,
+				   struct sk_buff *skb);
+static int cw1200_handle_action_tx(struct cw1200_common *priv,
+				   struct sk_buff *skb);
+static const struct ieee80211_rate *
+cw1200_get_tx_rate(const struct cw1200_common *priv,
+		   const struct ieee80211_tx_rate *rate);
+
+/* ******************************************************************** */
+/* TX queue lock / unlock						*/
+
+static inline void cw1200_tx_queues_lock(struct cw1200_common *priv)
+{
+	int i;
+	for (i = 0; i < 4; ++i)
+		cw1200_queue_lock(&priv->tx_queue[i], priv);
+}
+
+static inline void cw1200_tx_queues_unlock(struct cw1200_common *priv)
+{
+	int i;
+	for (i = 0; i < 4; ++i)
+		cw1200_queue_unlock(&priv->tx_queue[i], priv);
+}
+
+/* ******************************************************************** */
+/* TX policy cache implementation					*/
+
+static void tx_policy_dump(struct tx_policy *policy)
+{
+	tx_policy_printk(KERN_DEBUG "[TX policy] "
+		"%.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X"
+		"%.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X"
+		"%.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X: %d\n",
+		policy->raw[0] & 0x0F,  policy->raw[0] >> 4,
+		policy->raw[1] & 0x0F,  policy->raw[1] >> 4,
+		policy->raw[2] & 0x0F,  policy->raw[2] >> 4,
+		policy->raw[3] & 0x0F,  policy->raw[3] >> 4,
+		policy->raw[4] & 0x0F,  policy->raw[4] >> 4,
+		policy->raw[5] & 0x0F,  policy->raw[5] >> 4,
+		policy->raw[6] & 0x0F,  policy->raw[6] >> 4,
+		policy->raw[7] & 0x0F,  policy->raw[7] >> 4,
+		policy->raw[8] & 0x0F,  policy->raw[8] >> 4,
+		policy->raw[9] & 0x0F,  policy->raw[9] >> 4,
+		policy->raw[10] & 0x0F,  policy->raw[10] >> 4,
+		policy->raw[11] & 0x0F,  policy->raw[11] >> 4,
+		policy->defined);
+}
+
+static void tx_policy_build(const struct cw1200_common *priv,
+	/* [out] */ struct tx_policy *policy,
+	struct ieee80211_tx_rate *rates, size_t count)
+{
+	int i, j;
+	unsigned limit = priv->short_frame_max_tx_count;
+	unsigned total = 0;
+	BUG_ON(rates[0].idx < 0);
+	memset(policy, 0, sizeof(*policy));
+
+	/* minstrel is buggy a little bit, so distille
+	 * incoming rates first. */
+
+	/* Sort rates in descending order. */
+	for (i = 1; i < count; ++i) {
+		if (rates[i].idx < 0) {
+			count = i;
+			break;
+		}
+		if (rates[i].idx > rates[i - 1].idx) {
+			struct ieee80211_tx_rate tmp = rates[i - 1];
+			rates[i - 1] = rates[i];
+			rates[i] = tmp;
+		}
+	}
+
+	/* Eliminate duplicates. */
+	total = rates[0].count;
+	for (i = 0, j = 1; j < count; ++j) {
+		if (rates[j].idx == rates[i].idx) {
+			rates[i].count += rates[j].count;
+		} else if (rates[j].idx > rates[i].idx) {
+			break;
+		} else {
+			++i;
+			if (i != j)
+				rates[i] = rates[j];
+		}
+		total += rates[j].count;
+	}
+	count = i + 1;
+
+	/* Re-fill policy trying to keep every requested rate and with
+	 * respect to the global max tx retransmission count. */
+	if (limit < count)
+		limit = count;
+	if (total > limit) {
+		for (i = 0; i < count; ++i) {
+			int left = count - i - 1;
+			if (rates[i].count > limit - left)
+				rates[i].count = limit - left;
+			limit -= rates[i].count;
+		}
+	}
+	policy->defined = cw1200_get_tx_rate(priv, &rates[0])->hw_value + 1;
+
+	for (i = 0; i < count; ++i) {
+		register unsigned rateid, off, shift, retries;
+
+		rateid = cw1200_get_tx_rate(priv, &rates[i])->hw_value;
+		off = rateid >> 3;		/* eq. rateid / 8 */
+		shift = (rateid & 0x07) << 2;	/* eq. (rateid % 8) * 4 */
+
+		retries = rates[i].count;
+		if (unlikely(retries > 0x0F))
+			rates[i].count = retries = 0x0F;
+		policy->tbl[off] |= __cpu_to_le32(retries << shift);
+		policy->retry_count += retries;
+	}
+
+	tx_policy_printk(KERN_DEBUG "[TX policy] Policy (%d): " \
+		"%d:%d, %d:%d, %d:%d, %d:%d, %d:%d\n",
+		count,
+		rates[0].idx, rates[0].count,
+		rates[1].idx, rates[1].count,
+		rates[2].idx, rates[2].count,
+		rates[3].idx, rates[3].count,
+		rates[4].idx, rates[4].count);
+}
+
+static inline bool tx_policy_is_equal(const struct tx_policy *wanted,
+					const struct tx_policy *cached)
+{
+	size_t count = wanted->defined >> 1;
+	if (wanted->defined > cached->defined)
+		return false;
+	if (count) {
+		if (memcmp(wanted->raw, cached->raw, count))
+			return false;
+	}
+	if (wanted->defined & 1) {
+		if ((wanted->raw[count] & 0x0F) != (cached->raw[count] & 0x0F))
+			return false;
+	}
+	return true;
+}
+
+static int tx_policy_find(struct tx_policy_cache *cache,
+				const struct tx_policy *wanted)
+{
+	/* O(n) complexity. Not so good, but there's only 8 entries in
+	 * the cache.
+	 * Also lru helps to reduce search time. */
+	struct tx_policy_cache_entry *it;
+	/* First search for policy in "used" list */
+	list_for_each_entry(it, &cache->used, link) {
+		if (tx_policy_is_equal(wanted, &it->policy))
+			return it - cache->cache;
+	}
+	/* Then - in "free list" */
+	list_for_each_entry(it, &cache->free, link) {
+		if (tx_policy_is_equal(wanted, &it->policy))
+			return it - cache->cache;
+	}
+	return -1;
+}
+
+static inline void tx_policy_use(struct tx_policy_cache *cache,
+				 struct tx_policy_cache_entry *entry)
+{
+	++entry->policy.usage_count;
+	list_move(&entry->link, &cache->used);
+}
+
+static inline int tx_policy_release(struct tx_policy_cache *cache,
+				    struct tx_policy_cache_entry *entry)
+{
+	int ret = --entry->policy.usage_count;
+	if (!ret)
+		list_move(&entry->link, &cache->free);
+	return ret;
+}
+
+/* ******************************************************************** */
+/* External TX policy cache API						*/
+
+void tx_policy_init(struct cw1200_common *priv)
+{
+	struct tx_policy_cache *cache = &priv->tx_policy_cache;
+	int i;
+
+	memset(cache, 0, sizeof(*cache));
+
+	spin_lock_init(&cache->lock);
+	INIT_LIST_HEAD(&cache->used);
+	INIT_LIST_HEAD(&cache->free);
+
+	for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i)
+		list_add(&cache->cache[i].link, &cache->free);
+}
+
+static int tx_policy_get(struct cw1200_common *priv,
+		  struct ieee80211_tx_rate *rates,
+		  size_t count, bool *renew)
+{
+	int idx;
+	struct tx_policy_cache *cache = &priv->tx_policy_cache;
+	struct tx_policy wanted;
+
+	tx_policy_build(priv, &wanted, rates, count);
+
+	spin_lock_bh(&cache->lock);
+	BUG_ON(list_empty(&cache->free));
+	idx = tx_policy_find(cache, &wanted);
+	if (idx >= 0) {
+		tx_policy_printk(KERN_DEBUG "[TX policy] Used TX policy: %d\n",
+					idx);
+		*renew = false;
+	} else {
+		struct tx_policy_cache_entry *entry;
+		*renew = true;
+		/* If policy is not found create a new one
+		 * using the oldest entry in "free" list */
+		entry = list_entry(cache->free.prev,
+			struct tx_policy_cache_entry, link);
+		entry->policy = wanted;
+		idx = entry - cache->cache;
+		tx_policy_printk(KERN_DEBUG "[TX policy] New TX policy: %d\n",
+					idx);
+		tx_policy_dump(&entry->policy);
+	}
+	tx_policy_use(cache, &cache->cache[idx]);
+	if (unlikely(list_empty(&cache->free))) {
+		/* Lock TX queues. */
+		cw1200_tx_queues_lock(priv);
+	}
+	spin_unlock_bh(&cache->lock);
+	return idx;
+}
+
+void tx_policy_put(struct cw1200_common *priv, int idx)
+{
+	int usage, locked;
+	struct tx_policy_cache *cache = &priv->tx_policy_cache;
+
+	spin_lock_bh(&cache->lock);
+	locked = list_empty(&cache->free);
+	usage = tx_policy_release(cache, &cache->cache[idx]);
+	if (unlikely(locked) && !usage) {
+		/* Unlock TX queues. */
+		cw1200_tx_queues_unlock(priv);
+	}
+	spin_unlock_bh(&cache->lock);
+}
+
+/*
+bool tx_policy_cache_full(struct cw1200_common *priv)
+{
+	bool ret;
+	struct tx_policy_cache *cache = &priv->tx_policy_cache;
+	spin_lock_bh(&cache->lock);
+	ret = list_empty(&cache->free);
+	spin_unlock_bh(&cache->lock);
+	return ret;
+}
+*/
+
+static int tx_policy_upload(struct cw1200_common *priv)
+{
+	struct tx_policy_cache *cache = &priv->tx_policy_cache;
+	int i;
+	struct wsm_set_tx_rate_retry_policy arg = {
+		.hdr = {
+			.numTxRatePolicies = 0,
+		}
+	};
+	spin_lock_bh(&cache->lock);
+
+	/* Upload only modified entries. */
+	for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i) {
+		struct tx_policy *src = &cache->cache[i].policy;
+		if (src->retry_count && !src->uploaded) {
+			struct wsm_set_tx_rate_retry_policy_policy *dst =
+				&arg.tbl[arg.hdr.numTxRatePolicies];
+			dst->policyIndex = i;
+			dst->shortRetryCount = priv->short_frame_max_tx_count;
+			dst->longRetryCount = priv->long_frame_max_tx_count;
+
+			/* BIT(2) - Terminate retries when Tx rate retry policy
+			 *          finishes.
+			 * BIT(3) - Count initial frame transmission as part of
+			 *          rate retry counting but not as a retry
+			 *          attempt */
+			dst->policyFlags = BIT(2) | BIT(3);
+
+			memcpy(dst->rateCountIndices, src->tbl,
+					sizeof(dst->rateCountIndices));
+			src->uploaded = 1;
+			++arg.hdr.numTxRatePolicies;
+		}
+	}
+	spin_unlock_bh(&cache->lock);
+	cw1200_debug_tx_cache_miss(priv);
+	tx_policy_printk(KERN_DEBUG "[TX policy] Upload %d policies\n",
+				arg.hdr.numTxRatePolicies);
+	return wsm_set_tx_rate_retry_policy(priv, &arg);
+}
+
+void tx_policy_upload_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, tx_policy_upload_work);
+
+	tx_policy_printk(KERN_DEBUG "[TX] TX policy upload.\n");
+	WARN_ON(tx_policy_upload(priv));
+
+	wsm_unlock_tx(priv);
+	cw1200_tx_queues_unlock(priv);
+}
+
+/* ******************************************************************** */
+/* cw1200 TX implementation						*/
+
+u32 cw1200_rate_mask_to_wsm(struct cw1200_common *priv, u32 rates)
+{
+	u32 ret = 0;
+	int i;
+	for (i = 0; i < 32; ++i) {
+		if (rates & (1 << i))
+			ret |= 1 << priv->rates[i].hw_value;
+	}
+	return ret;
+}
+
+static const struct ieee80211_rate *
+cw1200_get_tx_rate(const struct cw1200_common *priv,
+		   const struct ieee80211_tx_rate *rate)
+{
+	if (rate->idx < 0)
+		return NULL;
+	if (rate->flags & IEEE80211_TX_RC_MCS)
+		return &priv->mcs_rates[rate->idx];
+	return &priv->hw->wiphy->bands[priv->channel->band]->
+		bitrates[rate->idx];
+}
+
+/* NOTE: cw1200_skb_to_wsm executes in atomic context. */
+int cw1200_skb_to_wsm(struct cw1200_common *priv, struct sk_buff *skb,
+			struct wsm_tx *wsm)
+{
+	bool tx_policy_renew = false;
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
+	const struct ieee80211_rate *rate = cw1200_get_tx_rate(priv,
+		&tx_info->control.rates[0]);
+
+	memset(wsm, 0, sizeof(*wsm));
+	wsm->hdr.len = __cpu_to_le16(skb->len);
+	wsm->hdr.id = __cpu_to_le16(0x0004);
+	if (rate) {
+		wsm->maxTxRate = rate->hw_value;
+		if (rate->flags & IEEE80211_TX_RC_MCS) {
+			if (cw1200_ht_greenfield(&priv->ht_info))
+				wsm->htTxParameters |=
+					__cpu_to_le32(WSM_HT_TX_GREENFIELD);
+			else
+				wsm->htTxParameters |=
+					__cpu_to_le32(WSM_HT_TX_MIXED);
+		}
+	}
+	wsm->flags = tx_policy_get(priv,
+		tx_info->control.rates, IEEE80211_TX_MAX_RATES,
+		&tx_policy_renew) << 4;
+
+	if (tx_policy_renew) {
+		tx_policy_printk(KERN_DEBUG "[TX] TX policy renew.\n");
+		/* It's not so optimal to stop TX queues every now and then.
+		 * Maybe it's better to reimplement task scheduling with
+		 * a counter. */
+		/* cw1200_tx_queues_lock(priv); */
+		/* Definetly better. TODO. */
+		wsm_lock_tx_async(priv);
+		cw1200_tx_queues_lock(priv);
+		queue_work(priv->workqueue, &priv->tx_policy_upload_work);
+	}
+
+	wsm->queueId = wsm_queue_id_to_wsm(skb_get_queue_mapping(skb));
+	return 0;
+}
+
+/* ******************************************************************** */
+
+int cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
+{
+	struct cw1200_common *priv = dev->priv;
+	unsigned queue = skb_get_queue_mapping(skb);
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_hdr *hdr =
+		(struct ieee80211_hdr *)skb->data;
+	struct cw1200_sta_priv *sta_priv =
+		(struct cw1200_sta_priv *)&tx_info->control.sta->drv_priv;
+	int link_id = 0;
+	int ret;
+
+	if (tx_info->flags | IEEE80211_TX_CTL_SEND_AFTER_DTIM)
+		link_id = CW1200_LINK_ID_AFTER_DTIM;
+	else if (tx_info->control.sta)
+		link_id = sta_priv->link_id;
+
+	txrx_printk(KERN_DEBUG "[TX] TX %d bytes (queue: %d, link_id: %d).\n",
+			skb->len, queue, link_id);
+
+	if (WARN_ON(queue >= 4))
+		goto err;
+
+#if 0
+	{
+		/* HACK!!!
+		* Workarounnd against a bug in WSM_A21.05.0288 firmware.
+		* In AP mode FW calculates FCS incorrectly when DA
+		* is FF:FF:FF:FF:FF:FF. Just for verification,
+		* do not enable this code in the real live. */
+		static const u8 mac_ff[] =
+			{0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+		static const u8 mac_mc[] =
+			{0x01, 0x00, 0x5e, 0x00, 0x00, 0x16};
+		if (!memcmp(&skb->data[4], mac_ff, sizeof(mac_ff)))
+			memcpy(&skb->data[4], mac_mc, sizeof(mac_mc));
+	}
+#endif
+
+
+	/* IV/ICV injection. */
+	/* TODO: Quite unoptimal. It's better co modify mac80211
+	 * to reserve space for IV */
+	if (tx_info->control.hw_key &&
+	    (hdr->frame_control &
+	     __cpu_to_le32(IEEE80211_FCTL_PROTECTED))) {
+		size_t hdrlen = ieee80211_hdrlen(hdr->frame_control);
+		size_t iv_len = tx_info->control.hw_key->iv_len;
+		size_t icv_len = tx_info->control.hw_key->icv_len;
+		u8 *icv;
+		u8 *newhdr;
+
+		if (tx_info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP) {
+			icv_len += 8; /* MIC */
+		}
+
+		if (skb_headroom(skb) < iv_len + WSM_TX_EXTRA_HEADROOM
+				|| skb_tailroom(skb) < icv_len) {
+			wiphy_err(priv->hw->wiphy,
+				"Bug: no space allocated "
+				"for crypto headers.\n"
+				"headroom: %d, tailroom: %d, "
+				"req_headroom: %d, req_tailroom: %d\n"
+				"Please fix it in cw1200_get_skb().\n",
+				skb_headroom(skb), skb_tailroom(skb),
+				iv_len + WSM_TX_EXTRA_HEADROOM, icv_len);
+			goto err;
+		}
+
+		newhdr = skb_push(skb, iv_len);
+		memmove(newhdr, newhdr + iv_len, hdrlen);
+		memset(&newhdr[hdrlen], 0, iv_len);
+		icv = skb_put(skb, icv_len);
+		memset(icv, 0, icv_len);
+	}
+
+	if ((size_t)skb->data & 3) {
+		size_t offset = (size_t)skb->data & 3;
+		u8 *p;
+		if (skb_headroom(skb) < 4) {
+			wiphy_err(priv->hw->wiphy,
+					"Bug: no space allocated "
+					"for DMA alignment.\n"
+					"headroom: %d\n",
+					skb_headroom(skb));
+			goto err;
+		}
+		p = skb_push(skb, offset);
+		memmove(p, &p[offset], skb->len - offset);
+		skb_trim(skb, skb->len - offset);
+		cw1200_debug_tx_copy(priv);
+	}
+
+	if (ieee80211_is_action(hdr->frame_control))
+		if (cw1200_handle_action_tx(priv, skb))
+			goto drop;
+
+	ret = cw1200_queue_put(&priv->tx_queue[queue], priv, skb,
+			link_id);
+	if (!WARN_ON(ret))
+		cw1200_bh_wakeup(priv);
+	else
+		goto err;
+
+	return NETDEV_TX_OK;
+
+err:
+	/* TODO: Update TX failure counters */
+	dev_kfree_skb_any(skb);
+	return NETDEV_TX_OK;
+
+drop:
+	dev_kfree_skb_any(skb);
+	return NETDEV_TX_OK;
+}
+
+/* ******************************************************************** */
+
+static int cw1200_handle_action_rx(struct cw1200_common *priv,
+				   struct sk_buff *skb)
+{
+	struct ieee80211_mgmt *mgmt = (void *)skb->data;
+
+	/* Filter block ACK negotiation: fully controlled by firmware */
+	if (mgmt->u.action.category == WLAN_CATEGORY_BACK)
+		return 1;
+
+	return 0;
+}
+
+static int cw1200_handle_action_tx(struct cw1200_common *priv,
+				   struct sk_buff *skb)
+{
+	struct ieee80211_mgmt *mgmt = (void *)skb->data;
+
+	/* Filter block ACK negotiation: fully controlled by firmware */
+	if (mgmt->u.action.category == WLAN_CATEGORY_BACK)
+		return 1;
+
+	return 0;
+}
+
+/* ******************************************************************** */
+
+void cw1200_tx_confirm_cb(struct cw1200_common *priv,
+			  struct wsm_tx_confirm *arg)
+{
+	u8 queue_id = cw1200_queue_get_queue_id(arg->packetID);
+	struct cw1200_queue *queue = &priv->tx_queue[queue_id];
+	struct sk_buff *skb;
+
+	txrx_printk(KERN_DEBUG "[TX] TX confirm.\n");
+
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+		/* STA is stopped. */
+		return;
+	}
+
+	if (WARN_ON(queue_id >= 4))
+		return;
+
+	if ((arg->status == WSM_REQUEUE) &&
+	    (arg->flags & WSM_TX_STATUS_REQUEUE)) {
+		WARN_ON(cw1200_queue_requeue(queue, arg->packetID));
+	} else if (!WARN_ON(cw1200_queue_get_skb(queue, arg->packetID, &skb))) {
+		struct ieee80211_tx_info *tx = IEEE80211_SKB_CB(skb);
+		struct wsm_tx *wsm_tx = (struct wsm_tx *)skb->data;
+		int rate_id = (wsm_tx->flags >> 4) & 0x07;
+		int tx_count = arg->ackFailures;
+		u8 ht_flags = 0;
+		int i;
+
+		if (cw1200_ht_greenfield(&priv->ht_info))
+			ht_flags |= IEEE80211_TX_RC_GREEN_FIELD;
+
+		/* Release used TX rate policy */
+		tx_policy_put(priv, rate_id);
+
+		if (likely(!arg->status)) {
+			tx->flags |= IEEE80211_TX_STAT_ACK;
+#if defined(CONFIG_CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE)
+			priv->last_activity_time = jiffies;
+#endif /* CONFIG_CW1200_FIRMWARE_DOES_NOT_SUPPORT_KEEPALIVE */
+			priv->cqm_tx_failure_count = 0;
+			++tx_count;
+			cw1200_debug_txed(priv);
+			if (arg->flags & WSM_TX_STATUS_AGGREGATION) {
+				/* Do not report aggregation to mac80211:
+				 * it confuses minstrel a lot. */
+				/* tx->flags |= IEEE80211_TX_STAT_AMPDU; */
+				cw1200_debug_txed_agg(priv);
+			}
+		} else {
+			/* TODO: Update TX failure counters */
+			if (unlikely(priv->cqm_tx_failure_thold &&
+			     (++priv->cqm_tx_failure_count >
+			      priv->cqm_tx_failure_thold))) {
+				priv->cqm_tx_failure_thold = 0;
+				queue_work(priv->workqueue,
+						&priv->tx_failure_work);
+			}
+		}
+
+		for (i = 0; i < IEEE80211_TX_MAX_RATES; ++i) {
+			if (tx->status.rates[i].count >= tx_count) {
+				tx->status.rates[i].count = tx_count;
+				break;
+			}
+			tx_count -= tx->status.rates[i].count;
+			if (tx->status.rates[i].flags & IEEE80211_TX_RC_MCS)
+				tx->status.rates[i].flags |= ht_flags;
+		}
+
+		for (++i; i < IEEE80211_TX_MAX_RATES; ++i) {
+			tx->status.rates[i].count = 0;
+			tx->status.rates[i].idx = -1;
+		}
+
+		skb_pull(skb, sizeof(struct wsm_tx));
+		ieee80211_tx_status(priv->hw, skb);
+
+		WARN_ON(cw1200_queue_remove(queue, priv, arg->packetID));
+	}
+}
+
+void cw1200_rx_cb(struct cw1200_common *priv,
+		  struct wsm_rx *arg,
+		  struct sk_buff **skb_p)
+{
+	struct sk_buff *skb = *skb_p;
+	struct ieee80211_rx_status *hdr = IEEE80211_SKB_RXCB(skb);
+	const struct ieee80211_rate *rate;
+	__le16 frame_control;
+	hdr->flag = 0;
+
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+		/* STA is stopped. */
+		goto drop;
+	}
+
+	if (unlikely(arg->status)) {
+		if (arg->status == WSM_STATUS_MICFAILURE) {
+			txrx_printk(KERN_DEBUG "[RX] MIC failure.\n");
+			hdr->flag |= RX_FLAG_MMIC_ERROR;
+		} else if (arg->status == WSM_STATUS_NO_KEY_FOUND) {
+			txrx_printk(KERN_DEBUG "[RX] No key found.\n");
+			goto drop;
+		} else {
+			txrx_printk(KERN_DEBUG "[RX] Receive failure: %d.\n",
+				arg->status);
+			goto drop;
+		}
+	}
+
+	if (skb->len < sizeof(struct ieee80211_hdr_3addr)) {
+		wiphy_warn(priv->hw->wiphy, "Mailformed SDU rx'ed. "
+				"Size is lesser than IEEE header.\n");
+		goto drop;
+	}
+
+	frame_control = *(__le16*)skb->data;
+	hdr->mactime = 0; /* Not supported by WSM */
+	hdr->freq = ieee80211_channel_to_frequency(arg->channelNumber);
+	hdr->band = (hdr->freq >= 5000) ?
+		IEEE80211_BAND_5GHZ : IEEE80211_BAND_2GHZ;
+
+	if (arg->rxedRate >= 4)
+		rate = &priv->rates[arg->rxedRate - 2];
+	else
+		rate = &priv->rates[arg->rxedRate];
+
+	if (rate >= priv->mcs_rates) {
+		hdr->rate_idx = rate - priv->mcs_rates;
+		hdr->flag |= RX_FLAG_HT;
+	} else {
+		hdr->rate_idx = rate - priv->rates;
+	}
+
+	hdr->signal = (s8)arg->rcpiRssi;
+	hdr->antenna = 0;
+
+	if (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
+		size_t iv_len = 0, icv_len = 0;
+		size_t hdrlen = ieee80211_hdrlen(frame_control);
+
+		hdr->flag |= RX_FLAG_DECRYPTED | RX_FLAG_IV_STRIPPED;
+
+		/* Oops... There is no fast way to ask mac80211 about
+		 * IV/ICV lengths. Even defineas are not exposed.*/
+		switch (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
+		case WSM_RX_STATUS_WEP:
+			iv_len = 4 /* WEP_IV_LEN */;
+			icv_len = 4 /* WEP_ICV_LEN */;
+			break;
+		case WSM_RX_STATUS_TKIP:
+			iv_len = 8 /* TKIP_IV_LEN */;
+			icv_len = 4 /* TKIP_ICV_LEN */
+				+ 8 /*MICHAEL_MIC_LEN*/;
+			hdr->flag |= RX_FLAG_MMIC_STRIPPED;
+			break;
+		case WSM_RX_STATUS_AES:
+			iv_len = 8 /* CCMP_HDR_LEN */;
+			icv_len = 8 /* CCMP_MIC_LEN */;
+			break;
+		case WSM_RX_STATUS_WAPI:
+			iv_len = 18 /* WAPI_HDR_LEN */;
+			icv_len = 16 /* WAPI_MIC_LEN */;
+			break;
+		default:
+			WARN_ON("Unknown encryption type");
+			goto drop;
+		}
+
+		if (skb->len < hdrlen + iv_len + icv_len) {
+			wiphy_warn(priv->hw->wiphy, "Mailformed SDU rx'ed. "
+				"Size is lesser than crypto headers.\n");
+			goto drop;
+		}
+
+		/* Remove IV, ICV and MIC */
+		skb_trim(skb, skb->len - icv_len);
+		memmove(skb->data + iv_len, skb->data, hdrlen);
+		skb_pull(skb, iv_len);
+	}
+
+	cw1200_debug_rxed(priv);
+	if (arg->flags & WSM_RX_STATUS_AGGREGATE)
+		cw1200_debug_rxed_agg(priv);
+
+	if (ieee80211_is_action(frame_control) &&
+			(arg->flags & WSM_RX_STATUS_ADDRESS1))
+		if (cw1200_handle_action_rx(priv, skb))
+			return;
+
+	/* Not that we really need _irqsafe variant here,
+	 * but it offloads realtime bh thread and improve
+	 * system performance. */
+	ieee80211_rx_irqsafe(priv->hw, skb);
+	*skb_p = NULL;
+	return;
+
+drop:
+	/* TODO: update failure counters */
+	return;
+}
+
+/* ******************************************************************** */
+/* Security								*/
+
+int cw1200_alloc_key(struct cw1200_common *priv)
+{
+	int idx;
+
+	idx = ffs(~priv->key_map) - 1;
+	if (idx < 0 || idx > WSM_KEY_MAX_INDEX)
+		return -1;
+
+	priv->key_map |= 1 << idx;
+	priv->keys[idx].entryIndex = idx;
+	return idx;
+}
+
+void cw1200_free_key(struct cw1200_common *priv, int idx)
+{
+	BUG_ON(!(priv->key_map & (1 << idx)));
+	memset(&priv->keys[idx], 0, sizeof(priv->keys[idx]));
+	priv->key_map &= ~(1 << idx);
+}
+
+void cw1200_free_keys(struct cw1200_common *priv)
+{
+	memset(&priv->keys, 0, sizeof(priv->keys));
+	priv->key_map = 0;
+}
+
+int cw1200_upload_keys(struct cw1200_common *priv)
+{
+	int idx, ret = 0;
+	for (idx = 0; idx <= WSM_KEY_MAX_INDEX; ++idx)
+		if (priv->key_map & (1 << idx)) {
+			ret = wsm_add_key(priv, &priv->keys[idx]);
+			if (ret < 0)
+				break;
+		}
+	return ret;
+}
diff --git a/drivers/staging/cw1200/txrx.h b/drivers/staging/cw1200/txrx.h
new file mode 100644
index 00000000000..12a9b49028b
--- /dev/null
+++ b/drivers/staging/cw1200/txrx.h
@@ -0,0 +1,87 @@
+/*
+ * Datapath interface for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_TXRX_H
+#define CW1200_TXRX_H
+
+#include <linux/list.h>
+
+/* extern */ struct ieee80211_hw;
+/* extern */ struct sk_buff;
+/* extern */ struct wsm_tx;
+/* extern */ struct wsm_tx_confirm;
+
+struct tx_policy {
+	union {
+		__le32 tbl[3];
+		u8 raw[12];
+	};
+	u8  defined;		/* TODO: u32 or u8, profile and select best */
+	u8  usage_count;	/* --// -- */
+	u8  retry_count;	/* --// -- */
+	u8  uploaded;
+};
+
+struct tx_policy_cache_entry {
+	struct tx_policy policy;
+	struct list_head link;
+};
+
+#define TX_POLICY_CACHE_SIZE	(8)
+struct tx_policy_cache {
+	struct tx_policy_cache_entry cache[TX_POLICY_CACHE_SIZE];
+	struct list_head used;
+	struct list_head free;
+	spinlock_t lock;
+};
+
+/* ******************************************************************** */
+/* TX policy cache							*/
+/* Intention of TX policy cache is an overcomplicated WSM API.
+ * Device does not accept per-PDU tx retry sequence.
+ * It uses "tx retry policy id" instead, so driver code has to sync
+ * linux tx retry sequences with a retry policy table in the device.
+ */
+void tx_policy_init(struct cw1200_common *priv);
+void tx_policy_upload_work(struct work_struct *work);
+void tx_policy_put(struct cw1200_common *priv, int idx);
+
+/* ******************************************************************** */
+/* TX implementation							*/
+
+u32 cw1200_rate_mask_to_wsm(struct cw1200_common *priv,
+			       u32 rates);
+int cw1200_skb_to_wsm(struct cw1200_common *priv,
+		      struct sk_buff *skb, struct wsm_tx *wsm);
+int cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb);
+
+/* ******************************************************************** */
+/* WSM callbacks							*/
+
+void cw1200_tx_confirm_cb(struct cw1200_common *priv,
+			  struct wsm_tx_confirm *arg);
+void cw1200_rx_cb(struct cw1200_common *priv,
+		  struct wsm_rx *arg,
+		  struct sk_buff **skb_p);
+
+/* ******************************************************************** */
+/* Timeout								*/
+
+void cw1200_tx_timeout(struct work_struct *work);
+
+/* ******************************************************************** */
+/* Security								*/
+int cw1200_alloc_key(struct cw1200_common *priv);
+void cw1200_free_key(struct cw1200_common *priv, int idx);
+void cw1200_free_keys(struct cw1200_common *priv);
+int cw1200_upload_keys(struct cw1200_common *priv);
+
+#endif /* CW1200_TXRX_H */
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
new file mode 100644
index 00000000000..c562f982477
--- /dev/null
+++ b/drivers/staging/cw1200/wsm.c
@@ -0,0 +1,1591 @@
+/*
+ * WSM host interface (HI) implementation for
+ * ST-Ericsson CW1200 mac80211 drivers.
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/skbuff.h>
+#include <linux/wait.h>
+#include <linux/skbuff.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+
+#include "cw1200.h"
+#include "wsm.h"
+#include "bh.h"
+#include "debug.h"
+
+#if defined(CONFIG_CW1200_WSM_DEBUG)
+#define wsm_printk(...) printk(__VA_ARGS__)
+#else
+#define wsm_printk(...)
+#endif
+
+#define WSM_CMD_TIMEOUT		(1 * HZ)
+#define WSM_CMD_JOIN_TIMEOUT	(7 * HZ) /* Join timeout is 5 sec. in FW */
+#define WSM_CMD_START_TIMEOUT	(7 * HZ)
+#define WSM_TX_TIMEOUT		(1 * HZ)
+#define WSM_CMD_LAST_CHANCE_TIMEOUT (10 * HZ)
+
+#define WSM_SKIP(buf, size)						\
+	do {								\
+		if (unlikely(buf->data + size > buf->end))		\
+			goto underflow;					\
+		buf->data += size;					\
+	} while (0)
+
+#define WSM_GET(buf, ptr, size)						\
+	do {								\
+		if (unlikely(buf->data + size > buf->end))		\
+			goto underflow;					\
+		memcpy(ptr, buf->data, size);				\
+		buf->data += size;					\
+	} while (0)
+
+#define __WSM_GET(buf, type, cvt)					\
+	({								\
+		type val;						\
+		if (unlikely(buf->data + sizeof(type) > buf->end))	\
+			goto underflow;					\
+		val = cvt(*(type *)buf->data);				\
+		buf->data += sizeof(type);				\
+		val;							\
+	})
+
+#define WSM_GET8(buf)  __WSM_GET(buf, u8, (u8))
+#define WSM_GET16(buf) __WSM_GET(buf, u16, __le16_to_cpu)
+#define WSM_GET32(buf) __WSM_GET(buf, u32, __le32_to_cpu)
+
+#define WSM_PUT(buf, ptr, size)						\
+	do {								\
+		if (unlikely(buf->data + size > buf->end))		\
+			if (unlikely(wsm_buf_reserve(buf, size)))	\
+				goto nomem;				\
+		memcpy(buf->data, ptr, size);				\
+		buf->data += size;					\
+	} while (0)
+
+#define __WSM_PUT(buf, val, type, cvt)					  \
+	do {								  \
+		if (unlikely(buf->data + sizeof(type) > buf->end))	  \
+			if (unlikely(wsm_buf_reserve(buf, sizeof(type)))) \
+				goto nomem;				  \
+		*(type *)buf->data = cvt(val);				  \
+		buf->data += sizeof(type);				  \
+	} while (0)
+
+#define WSM_PUT8(buf, val)  __WSM_PUT(buf, val, u8, (u8))
+#define WSM_PUT16(buf, val) __WSM_PUT(buf, val, u16, __cpu_to_le16)
+#define WSM_PUT32(buf, val) __WSM_PUT(buf, val, u32, __cpu_to_le32)
+
+static void wsm_buf_reset(struct wsm_buf *buf);
+static int wsm_buf_reserve(struct wsm_buf *buf, size_t extra_size);
+
+static int wsm_cmd_send(struct cw1200_common *priv,
+			struct wsm_buf *buf,
+			void *arg, u16 cmd, long tmo);
+
+static inline void wsm_cmd_lock(struct cw1200_common *priv)
+{
+	mutex_lock(&priv->wsm_cmd_mux);
+}
+
+static inline void wsm_cmd_unlock(struct cw1200_common *priv)
+{
+	mutex_unlock(&priv->wsm_cmd_mux);
+}
+
+/* ******************************************************************** */
+/* WSM API implementation						*/
+
+static int wsm_generic_confirm(struct cw1200_common *priv,
+			     void *arg,
+			     struct wsm_buf *buf)
+{
+	u32 status = WSM_GET32(buf);
+	if (status != WSM_STATUS_SUCCESS)
+		return -EINVAL;
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+int wsm_configuration(struct cw1200_common *priv, struct wsm_configuration *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT32(buf, arg->dot11MaxTransmitMsduLifeTime);
+	WSM_PUT32(buf, arg->dot11MaxReceiveLifeTime);
+	WSM_PUT32(buf, arg->dot11RtsThreshold);
+
+	/* DPD block. */
+	WSM_PUT16(buf, arg->dpdData_size + 12);
+	WSM_PUT16(buf, 1); /* DPD version */
+	WSM_PUT(buf, arg->dot11StationId, ETH_ALEN);
+	WSM_PUT16(buf, 5); /* DPD flags */
+	WSM_PUT(buf, arg->dpdData, arg->dpdData_size);
+
+	ret = wsm_cmd_send(priv, buf, arg, 0x0009, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+static int wsm_configuration_confirm(struct cw1200_common *priv,
+				     struct wsm_configuration *arg,
+				     struct wsm_buf *buf)
+{
+	int i;
+	int status;
+
+	status = WSM_GET32(buf);
+	if (WARN_ON(status != WSM_STATUS_SUCCESS))
+		return -EINVAL;
+
+	WSM_GET(buf, arg->dot11StationId, ETH_ALEN);
+	arg->dot11FrequencyBandsSupported = WSM_GET8(buf);
+	WSM_SKIP(buf, 1);
+	arg->supportedRateMask = WSM_GET32(buf);
+	for (i = 0; i < 2; ++i) {
+		arg->txPowerRange[i].min_power_level = WSM_GET32(buf);
+		arg->txPowerRange[i].max_power_level = WSM_GET32(buf);
+		arg->txPowerRange[i].stepping = WSM_GET32(buf);
+	}
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+/* ******************************************************************** */
+
+int wsm_reset(struct cw1200_common *priv, const struct wsm_reset *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	u16 cmd = 0x000A | ((arg->link_id & 0x0F) << 6);
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT32(buf, arg->reset_statistics ? 0 : 1);
+	ret = wsm_cmd_send(priv, buf, NULL, cmd, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+struct wsm_mib {
+	u16 mibId;
+	void *buf;
+	size_t buf_size;
+};
+
+int wsm_read_mib(struct cw1200_common *priv, u16 mibId, void *_buf,
+			size_t buf_size)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	struct wsm_mib mib_buf = {
+		.mibId = mibId,
+		.buf = _buf,
+		.buf_size = buf_size,
+	};
+	wsm_cmd_lock(priv);
+
+	WSM_PUT16(buf, mibId);
+	WSM_PUT16(buf, 0);
+
+	ret = wsm_cmd_send(priv, buf, &mib_buf, 0x0005, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+static int wsm_read_mib_confirm(struct cw1200_common *priv,
+				struct wsm_mib *arg,
+				struct wsm_buf *buf)
+{
+	u16 size;
+	if (WARN_ON(WSM_GET32(buf) != WSM_STATUS_SUCCESS))
+		return -EINVAL;
+
+	if (WARN_ON(WSM_GET16(buf) != arg->mibId))
+		return -EINVAL;
+
+	size = WSM_GET16(buf);
+	if (size > arg->buf_size)
+		size = arg->buf_size;
+
+	WSM_GET(buf, arg->buf, size);
+	arg->buf_size = size;
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+/* ******************************************************************** */
+
+int wsm_write_mib(struct cw1200_common *priv, u16 mibId, void *_buf,
+			size_t buf_size)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	struct wsm_mib mib_buf = {
+		.mibId = mibId,
+		.buf = _buf,
+		.buf_size = buf_size,
+	};
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT16(buf, mibId);
+	WSM_PUT16(buf, buf_size);
+	WSM_PUT(buf, _buf, buf_size);
+
+	ret = wsm_cmd_send(priv, buf, &mib_buf, 0x0006, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+static int wsm_write_mib_confirm(struct cw1200_common *priv,
+				struct wsm_mib *arg,
+				struct wsm_buf *buf)
+{
+	int ret;
+
+	ret = wsm_generic_confirm(priv, arg, buf);
+	if (ret)
+		return ret;
+
+	if (arg->mibId == 0x1006) {
+		/* OperationalMode: update PM status. */
+		const char *p = arg->buf;
+		cw1200_enable_powersave(priv,
+				(p[0] & 0x0F) ? true : false);
+	}
+	return 0;
+}
+
+/* ******************************************************************** */
+
+int wsm_scan(struct cw1200_common *priv, const struct wsm_scan *arg)
+{
+	int i;
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	if (unlikely(arg->numOfChannels > 48))
+		return -EINVAL;
+
+	if (unlikely(arg->numOfSSIDs > 2))
+		return -EINVAL;
+
+	if (unlikely(arg->band > 1))
+		return -EINVAL;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, arg->band);
+	WSM_PUT8(buf, arg->scanType);
+	WSM_PUT8(buf, arg->scanFlags);
+	WSM_PUT8(buf, arg->maxTransmitRate);
+	WSM_PUT32(buf, arg->autoScanInterval);
+	WSM_PUT8(buf, arg->numOfProbeRequests);
+	WSM_PUT8(buf, arg->numOfChannels);
+	WSM_PUT8(buf, arg->numOfSSIDs);
+	WSM_PUT8(buf, arg->probeDelay);
+
+	for (i = 0; i < arg->numOfChannels; ++i) {
+		WSM_PUT16(buf, arg->ch[i].number);
+		WSM_PUT16(buf, 0);
+		WSM_PUT32(buf, arg->ch[i].minChannelTime);
+		WSM_PUT32(buf, arg->ch[i].maxChannelTime);
+		WSM_PUT32(buf, 0);
+	}
+
+	for (i = 0; i < arg->numOfSSIDs; ++i) {
+		WSM_PUT32(buf, arg->ssids[i].length);
+		WSM_PUT(buf, &arg->ssids[i].ssid[0],
+				sizeof(arg->ssids[i].ssid));
+	}
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0007, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_stop_scan(struct cw1200_common *priv)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	wsm_cmd_lock(priv);
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0008, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+}
+
+
+static int wsm_tx_confirm(struct cw1200_common *priv, struct wsm_buf *buf)
+{
+	struct wsm_tx_confirm tx_confirm;
+
+	tx_confirm.packetID = WSM_GET32(buf);
+	tx_confirm.status = WSM_GET32(buf);
+	tx_confirm.txedRate = WSM_GET8(buf);
+	tx_confirm.ackFailures = WSM_GET8(buf);
+	tx_confirm.flags = WSM_GET16(buf);
+	tx_confirm.mediaDelay = WSM_GET32(buf);
+	tx_confirm.txQueueDelay = WSM_GET32(buf);
+
+	if (priv->wsm_cbc.tx_confirm)
+		priv->wsm_cbc.tx_confirm(priv, &tx_confirm);
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+static int wsm_multi_tx_confirm(struct cw1200_common *priv,
+				struct wsm_buf *buf)
+{
+	int ret;
+	int count;
+	int i;
+
+	count = WSM_GET32(buf);
+	if (WARN_ON(count <= 0))
+		return -EINVAL;
+	else if (count > 1) {
+		ret = wsm_release_tx_buffer(priv, count - 1);
+		if (ret < 0)
+			return ret;
+		else if (ret > 0)
+			cw1200_bh_wakeup(priv);
+	}
+
+	cw1200_debug_txed_multi(priv, count);
+	for (i = 0; i < count; ++i) {
+		ret = wsm_tx_confirm(priv, buf);
+		if (ret)
+			return ret;
+	}
+	return ret;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+/* ******************************************************************** */
+
+static int wsm_join_confirm(struct cw1200_common *priv,
+			    struct wsm_join *arg,
+			    struct wsm_buf *buf)
+{
+	if (WARN_ON(WSM_GET32(buf) != WSM_STATUS_SUCCESS)) {
+		priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+		wsm_unlock_tx(priv);
+		return -EINVAL;
+	}
+
+	arg->minPowerLevel = WSM_GET32(buf);
+	arg->maxPowerLevel = WSM_GET32(buf);
+
+	priv->join_status = CW1200_JOIN_STATUS_STA;
+	wsm_unlock_tx(priv);
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+	wsm_unlock_tx(priv);
+	return -EINVAL;
+}
+
+int wsm_join(struct cw1200_common *priv, struct wsm_join *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, arg->mode);
+	WSM_PUT8(buf, arg->band);
+	WSM_PUT16(buf, arg->channelNumber);
+	WSM_PUT(buf, &arg->bssid[0], sizeof(arg->bssid));
+	WSM_PUT16(buf, arg->atimWindow);
+	WSM_PUT8(buf, arg->preambleType);
+	WSM_PUT8(buf, arg->probeForJoin);
+	WSM_PUT8(buf, arg->dtimPeriod);
+	WSM_PUT8(buf, arg->flags);
+	WSM_PUT32(buf, arg->ssidLength);
+	WSM_PUT(buf, &arg->ssid[0], sizeof(arg->ssid));
+	WSM_PUT32(buf, arg->beaconInterval);
+	WSM_PUT32(buf, arg->basicRateSet);
+
+	ret = wsm_cmd_send(priv, buf, arg, 0x000B, WSM_CMD_JOIN_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_bss_params(struct cw1200_common *priv,
+			const struct wsm_set_bss_params *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, 0);
+	WSM_PUT8(buf, arg->beaconLostCount);
+	WSM_PUT16(buf, arg->aid);
+	WSM_PUT32(buf, arg->operationalRateSet);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0011, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_add_key(struct cw1200_common *priv, const struct wsm_add_key *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT(buf, arg, sizeof(*arg));
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x000C, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_remove_key(struct cw1200_common *priv, const struct wsm_remove_key *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, arg->entryIndex);
+	WSM_PUT8(buf, 0);
+	WSM_PUT16(buf, 0);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x000D, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_tx_queue_params(struct cw1200_common *priv,
+				const struct wsm_set_tx_queue_params *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	/* TODO: verify me. */
+	WSM_PUT8(buf, arg->queueId);
+	WSM_PUT8(buf, 0);
+	WSM_PUT8(buf, arg->ackPolicy);
+	WSM_PUT8(buf, 0);
+	WSM_PUT32(buf, arg->maxTransmitLifetime);
+	WSM_PUT16(buf, arg->allowedMediumTime);
+	WSM_PUT16(buf, 0);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0012, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_edca_params(struct cw1200_common *priv,
+				const struct wsm_edca_params *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	/* TODO: verify me. */
+	/* Implemented according to specification. Note that there is a
+	 * mismatch in BK and BE mapping. */
+
+	WSM_PUT16(buf, arg->params[1].cwMin);
+	WSM_PUT16(buf, arg->params[0].cwMin);
+	WSM_PUT16(buf, arg->params[2].cwMin);
+	WSM_PUT16(buf, arg->params[3].cwMin);
+
+	WSM_PUT16(buf, arg->params[1].cwMax);
+	WSM_PUT16(buf, arg->params[0].cwMax);
+	WSM_PUT16(buf, arg->params[2].cwMax);
+	WSM_PUT16(buf, arg->params[3].cwMax);
+
+	WSM_PUT8(buf, arg->params[1].aifns);
+	WSM_PUT8(buf, arg->params[0].aifns);
+	WSM_PUT8(buf, arg->params[2].aifns);
+	WSM_PUT8(buf, arg->params[3].aifns);
+
+	WSM_PUT16(buf, arg->params[1].txOpLimit);
+	WSM_PUT16(buf, arg->params[0].txOpLimit);
+	WSM_PUT16(buf, arg->params[2].txOpLimit);
+	WSM_PUT16(buf, arg->params[3].txOpLimit);
+
+	WSM_PUT32(buf, arg->params[1].maxReceiveLifetime);
+	WSM_PUT32(buf, arg->params[0].maxReceiveLifetime);
+	WSM_PUT32(buf, arg->params[2].maxReceiveLifetime);
+	WSM_PUT32(buf, arg->params[3].maxReceiveLifetime);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0013, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_switch_channel(struct cw1200_common *priv,
+			const struct wsm_switch_channel *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_lock_tx(priv);
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, arg->channelMode);
+	WSM_PUT8(buf, arg->channelSwitchCount);
+	WSM_PUT16(buf, arg->newChannelNumber);
+
+	priv->channel_switch_in_progress = 1;
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0016, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	if (ret) {
+		wsm_unlock_tx(priv);
+		priv->channel_switch_in_progress = 0;
+	}
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	wsm_unlock_tx(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_pm(struct cw1200_common *priv, const struct wsm_set_pm *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT32(buf, arg->pmMode);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0010, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_start(struct cw1200_common *priv, const struct wsm_start *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, arg->mode);
+	WSM_PUT8(buf, arg->band);
+	WSM_PUT16(buf, arg->channelNumber);
+	WSM_PUT32(buf, arg->CTWindow);
+	WSM_PUT32(buf, arg->beaconInterval);
+	WSM_PUT8(buf, arg->DTIMPeriod);
+	WSM_PUT8(buf, arg->preambleType);
+	WSM_PUT8(buf, arg->probeDelay);
+	WSM_PUT8(buf, arg->ssidLength);
+	WSM_PUT(buf, arg->ssid, sizeof(arg->ssid));
+	WSM_PUT32(buf, arg->basicRateSet);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0017, WSM_CMD_START_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_beacon_transmit(struct cw1200_common *priv,
+			const struct wsm_beacon_transmit *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT32(buf, arg->enableBeaconing ? 1 : 0);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0018, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_start_find(struct cw1200_common *priv)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0019, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+}
+
+/* ******************************************************************** */
+
+int wsm_stop_find(struct cw1200_common *priv)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+	ret = wsm_cmd_send(priv, buf, NULL, 0x001A, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+}
+
+/* ******************************************************************** */
+
+int wsm_map_link(struct cw1200_common *priv, const struct wsm_map_link *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	u16 cmd = 0x001C | ((arg->link_id & 0x0F) << 6);
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT(buf, &arg->mac_addr[0], sizeof(arg->mac_addr));
+	WSM_PUT16(buf, 0);
+
+	ret = wsm_cmd_send(priv, buf, NULL, cmd, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+
+/* ******************************************************************** */
+/* WSM indication events implementation					*/
+
+static int wsm_startup_indication(struct cw1200_common *priv,
+					struct wsm_buf *buf)
+{
+	u16 status;
+	char fw_label[129];
+	static const char * const fw_types[] = {
+		"ETF",
+		"WFM",
+		"WSM",
+		"HI test",
+		"Platform test"
+	};
+
+	priv->wsm_caps.numInpChBufs	= WSM_GET16(buf);
+	priv->wsm_caps.sizeInpChBuf	= WSM_GET16(buf);
+	priv->wsm_caps.hardwareId	= WSM_GET16(buf);
+	priv->wsm_caps.hardwareSubId	= WSM_GET16(buf);
+	status				= WSM_GET16(buf);
+	priv->wsm_caps.firmwareCap	= WSM_GET16(buf);
+	priv->wsm_caps.firmwareType	= WSM_GET16(buf);
+	priv->wsm_caps.firmwareApiVer	= WSM_GET16(buf);
+	priv->wsm_caps.firmwareBuildNumber = WSM_GET16(buf);
+	priv->wsm_caps.firmwareVersion	= WSM_GET16(buf);
+	WSM_GET(buf, &fw_label[0], sizeof(fw_label) - 1);
+	fw_label[sizeof(fw_label) - 1] = 0; /* Do not trust FW too much. */
+
+	if (WARN_ON(status))
+		return -EINVAL;
+
+	if (WARN_ON(priv->wsm_caps.firmwareType > 4))
+		return -EINVAL;
+
+	printk(KERN_INFO "CW1200 WSM init done.\n"
+		"   Input buffers: %d x %d bytes\n"
+		"   Hardware: %d.%d\n"
+		"   %s firmware [%s], ver: %d, build: %d,"
+		    " api: %d, cap: 0x%.4X\n",
+		priv->wsm_caps.numInpChBufs, priv->wsm_caps.sizeInpChBuf,
+		priv->wsm_caps.hardwareId, priv->wsm_caps.hardwareSubId,
+		fw_types[priv->wsm_caps.firmwareType],
+		&fw_label[0], priv->wsm_caps.firmwareVersion,
+		priv->wsm_caps.firmwareBuildNumber,
+		priv->wsm_caps.firmwareApiVer, priv->wsm_caps.firmwareCap);
+
+	priv->wsm_caps.firmwareReady = 1;
+
+	wake_up_interruptible(&priv->wsm_startup_done);
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+static int wsm_receive_indication(struct cw1200_common *priv,
+					struct wsm_buf *buf,
+					struct sk_buff **skb_p)
+{
+	priv->rx_timestamp = jiffies;
+	if (priv->wsm_cbc.rx) {
+		struct wsm_rx rx;
+		size_t hdr_len;
+		__le16 fctl;
+
+		rx.status = WSM_GET32(buf);
+		rx.channelNumber = WSM_GET16(buf);
+		rx.rxedRate = WSM_GET8(buf);
+		rx.rcpiRssi = WSM_GET8(buf);
+		rx.flags = WSM_GET32(buf);
+		fctl = *(__le16 *)buf->data;
+		hdr_len = buf->data - buf->begin;
+		skb_pull(*skb_p, hdr_len);
+		if (!rx.status && unlikely(ieee80211_is_deauth(fctl))) {
+			if (priv->join_status == CW1200_JOIN_STATUS_STA) {
+				/* Shedule unjoin work */
+				wsm_printk(KERN_DEBUG \
+					"[WSM] Issue unjoin command (RX).\n");
+				wsm_lock_tx_async(priv);
+				if (queue_work(priv->workqueue,
+						&priv->unjoin_work) <= 0)
+					wsm_unlock_tx(priv);
+			}
+		}
+		priv->wsm_cbc.rx(priv, &rx, skb_p);
+		if (*skb_p)
+			skb_push(*skb_p, hdr_len);
+	}
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+static int wsm_event_indication(struct cw1200_common *priv, struct wsm_buf *buf)
+{
+	int first;
+	struct cw1200_wsm_event *event;
+
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+		/* STA is stopped. */
+		return 0;
+	}
+
+	event = kzalloc(sizeof(struct cw1200_wsm_event), GFP_KERNEL);
+
+	event->evt.eventId = __le32_to_cpu(WSM_GET32(buf));
+	event->evt.eventData = __le32_to_cpu(WSM_GET32(buf));
+
+	wsm_printk(KERN_DEBUG "[WSM] Event: %d(%d)\n",
+		event->evt.eventId, event->evt.eventData);
+
+	spin_lock(&priv->event_queue_lock);
+	first = list_empty(&priv->event_queue);
+	list_add_tail(&event->link, &priv->event_queue);
+	spin_unlock(&priv->event_queue_lock);
+
+	if (first)
+		queue_work(priv->workqueue, &priv->event_handler);
+
+	return 0;
+
+underflow:
+	kfree(event);
+	return -EINVAL;
+}
+
+static int wsm_channel_switch_indication(struct cw1200_common *priv,
+						struct wsm_buf *buf)
+{
+	wsm_unlock_tx(priv); /* Re-enable datapath */
+	WARN_ON(WSM_GET32(buf));
+
+	priv->channel_switch_in_progress = 0;
+	wake_up_interruptible(&priv->channel_switch_done);
+
+	if (priv->wsm_cbc.channel_switch)
+		priv->wsm_cbc.channel_switch(priv);
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+static int wsm_set_pm_indication(struct cw1200_common *priv,
+					struct wsm_buf *buf)
+{
+	return 0;
+}
+
+static int wsm_scan_complete_indication(struct cw1200_common *priv,
+					struct wsm_buf *buf)
+{
+	if (priv->wsm_cbc.scan_complete) {
+		struct wsm_scan_complete arg;
+		arg.status = WSM_GET32(buf);
+		arg.psm = WSM_GET8(buf);
+		arg.numChannels = WSM_GET8(buf);
+		priv->wsm_cbc.scan_complete(priv, &arg);
+	}
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+static int wsm_find_complete_indication(struct cw1200_common *priv,
+					struct wsm_buf *buf)
+{
+	/* TODO: Implement me. */
+	STUB();
+	return 0;
+}
+
+static int wsm_suspend_resume_indication(struct cw1200_common *priv,
+					 int link_id, struct wsm_buf *buf)
+{
+	if (priv->wsm_cbc.suspend_resume) {
+		u32 flags;
+		struct wsm_suspend_resume arg;
+
+		flags = WSM_GET32(buf);
+		arg.link_id = link_id;
+		arg.stop = !(flags & 1);
+		arg.multicast = !!(flags & 8);
+		arg.queue = (flags >> 1) & 3;
+
+		priv->wsm_cbc.suspend_resume(priv, &arg);
+	}
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+
+/* ******************************************************************** */
+/* WSM TX								*/
+
+int wsm_cmd_send(struct cw1200_common *priv,
+		 struct wsm_buf *buf,
+		 void *arg, u16 cmd, long tmo)
+{
+	size_t buf_len = buf->data - buf->begin;
+	int ret;
+
+	if (cmd == 0x0006) /* Write MIB */
+		wsm_printk(KERN_DEBUG "[WSM] >>> 0x%.4X [MIB: 0x%.4X] (%d)\n",
+			cmd, __le16_to_cpu(((__le16 *)buf->begin)[2]),
+			buf_len);
+	else
+		wsm_printk(KERN_DEBUG "[WSM] >>> 0x%.4X (%d)\n", cmd, buf_len);
+
+	/* Fill HI message header */
+	/* BH will add sequence number */
+	((__le16 *)buf->begin)[0] = __cpu_to_le16(buf_len);
+	((__le16 *)buf->begin)[1] = __cpu_to_le16(cmd);
+
+	spin_lock(&priv->wsm_cmd.lock);
+	BUG_ON(priv->wsm_cmd.ptr);
+	priv->wsm_cmd.done = 0;
+	priv->wsm_cmd.ptr = buf->begin;
+	priv->wsm_cmd.len = buf_len;
+	priv->wsm_cmd.arg = arg;
+	priv->wsm_cmd.cmd = cmd;
+	spin_unlock(&priv->wsm_cmd.lock);
+
+	cw1200_bh_wakeup(priv);
+
+	if (unlikely(priv->bh_error)) {
+		/* Do not wait for timeout if BH is dead. Exit immediately. */
+		ret = 0;
+	} else {
+		long rx_timestamp;
+		/* Firmware prioritizes data traffic over control confirm.
+		 * Loop below checks if data was RXed and increases timeout
+		 * accordingly. */
+		do {
+			/* It's safe to use unprotected access to
+			 * wsm_cmd.done here */
+			ret = wait_event_interruptible_timeout(
+					priv->wsm_cmd_wq,
+					priv->wsm_cmd.done, tmo);
+			rx_timestamp = jiffies - priv->rx_timestamp;
+			if (unlikely(rx_timestamp < 0))
+				rx_timestamp = tmo + 1;
+		} while (!ret && rx_timestamp <= tmo);
+	}
+
+	if (unlikely(ret == 0)) {
+		u16 raceCheck;
+
+		spin_lock(&priv->wsm_cmd.lock);
+		raceCheck = priv->wsm_cmd.cmd;
+		priv->wsm_cmd.arg = NULL;
+		priv->wsm_cmd.ptr = NULL;
+		spin_unlock(&priv->wsm_cmd.lock);
+
+		/* Race condition check to make sure _confirm is not called
+		 * after exit of _send */
+		if (raceCheck == 0xFFFF) {
+			/* If wsm_handle_rx got stuck in _confirm we will hang
+			 * system there. It's better than silently currupt
+			 * stack or heap, isn't it? */
+			BUG_ON(wait_event_interruptible_timeout(
+					priv->wsm_cmd_wq, priv->wsm_cmd.done,
+					WSM_CMD_LAST_CHANCE_TIMEOUT) <= 0);
+		}
+		ret = -ETIMEDOUT;
+	} else {
+		spin_lock(&priv->wsm_cmd.lock);
+		BUG_ON(!priv->wsm_cmd.done);
+		ret = priv->wsm_cmd.ret;
+		spin_unlock(&priv->wsm_cmd.lock);
+	}
+	wsm_buf_reset(buf);
+	return ret;
+}
+
+/* ******************************************************************** */
+/* WSM TX port control							*/
+
+void wsm_lock_tx(struct cw1200_common *priv)
+{
+	wsm_cmd_lock(priv);
+	if (atomic_add_return(1, &priv->tx_lock) == 1) {
+		WARN_ON(wait_event_interruptible_timeout(priv->hw_bufs_used_wq,
+			!priv->hw_bufs_used, WSM_CMD_LAST_CHANCE_TIMEOUT) <= 0);
+		wsm_printk(KERN_DEBUG "[WSM] TX is locked.\n");
+	}
+	wsm_cmd_unlock(priv);
+}
+
+void wsm_lock_tx_async(struct cw1200_common *priv)
+{
+	if (atomic_add_return(1, &priv->tx_lock) == 1)
+		wsm_printk(KERN_DEBUG "[WSM] TX is locked.\n");
+}
+
+void wsm_flush_tx(struct cw1200_common *priv)
+{
+	BUG_ON(!atomic_read(&priv->tx_lock));
+	WARN_ON(wait_event_interruptible_timeout(priv->hw_bufs_used_wq,
+		!priv->hw_bufs_used, WSM_CMD_LAST_CHANCE_TIMEOUT) <= 0);
+}
+
+void wsm_unlock_tx(struct cw1200_common *priv)
+{
+	int tx_lock;
+	tx_lock = atomic_sub_return(1, &priv->tx_lock);
+	if (tx_lock < 0) {
+		BUG_ON(1);
+	} else if (tx_lock == 0) {
+		cw1200_bh_wakeup(priv);
+		wsm_printk(KERN_DEBUG "[WSM] TX is unlocked.\n");
+	}
+}
+
+/* ******************************************************************** */
+/* WSM RX								*/
+
+int wsm_handle_exception(struct cw1200_common *priv, u8 *data, size_t len)
+{
+	STUB();
+	return 0;
+}
+
+int wsm_handle_rx(struct cw1200_common *priv, int id,
+		  struct wsm_hdr *wsm, struct sk_buff **skb_p)
+{
+	int ret = 0;
+	struct wsm_buf wsm_buf;
+	int link_id = (id >> 6) & 0x0F;
+
+	/* Strip link id. */
+	id &= ~(0x0F << 6);
+
+	wsm_buf.begin = (u8 *)&wsm[0];
+	wsm_buf.data = (u8 *)&wsm[1];
+	wsm_buf.end = &wsm_buf.begin[__le32_to_cpu(wsm->len)];
+
+	wsm_printk(KERN_DEBUG "[WSM] <<< 0x%.4X (%d)\n", id,
+			wsm_buf.end - wsm_buf.begin);
+
+	if (id == 0x404) {
+		ret = wsm_tx_confirm(priv, &wsm_buf);
+	} else if (id == 0x41E) {
+		ret = wsm_multi_tx_confirm(priv, &wsm_buf);
+	} else if (id & 0x0400) {
+		void *wsm_arg;
+		u16 wsm_cmd;
+
+		/* Do not trust FW too much. Protection against repeated
+		 * response and race condition removal (see above). */
+		spin_lock(&priv->wsm_cmd.lock);
+		wsm_arg = priv->wsm_cmd.arg;
+		wsm_cmd = priv->wsm_cmd.cmd & ~(0x0F << 6);
+		priv->wsm_cmd.cmd = 0xFFFF;
+		spin_unlock(&priv->wsm_cmd.lock);
+
+		if (WARN_ON((id & ~0x0400) != wsm_cmd)) {
+			/* Note that any non-zero is a fatal retcode. */
+			ret = -EINVAL;
+			goto out;
+		}
+
+		switch (id) {
+		case 0x0409:
+			/* Note that wsm_arg can be NULL in case of timeout in
+			 * wsm_cmd_send(). */
+			if (likely(wsm_arg))
+				ret = wsm_configuration_confirm(priv, wsm_arg,
+								&wsm_buf);
+			break;
+		case 0x0405:
+			if (likely(wsm_arg))
+				ret = wsm_read_mib_confirm(priv, wsm_arg,
+								&wsm_buf);
+			break;
+		case 0x0406:
+			if (likely(wsm_arg))
+				ret = wsm_write_mib_confirm(priv, wsm_arg,
+								&wsm_buf);
+			break;
+		case 0x040B:
+			if (likely(wsm_arg))
+				ret = wsm_join_confirm(priv, wsm_arg, &wsm_buf);
+			break;
+		case 0x0407: /* start-scan */
+		case 0x0408: /* stop-scan */
+		case 0x040A: /* wsm_reset */
+		case 0x040C: /* add_key */
+		case 0x040D: /* remove_key */
+		case 0x0410: /* wsm_set_pm */
+		case 0x0411: /* set_bss_params */
+		case 0x0412: /* set_tx_queue_params */
+		case 0x0413: /* set_edca_params */
+		case 0x0416: /* switch_channel */
+		case 0x0417: /* start */
+		case 0x0418: /* beacon_transmit */
+		case 0x0419: /* start_find */
+		case 0x041A: /* stop_find */
+		case 0x041C: /* map_link */
+			WARN_ON(wsm_arg != NULL);
+			ret = wsm_generic_confirm(priv, wsm_arg, &wsm_buf);
+			if (ret)
+				printk(KERN_ERR
+					"[WSM] wsm_generic_confirm "
+					"failed for request 0x%.4X.\n",
+					id);
+			break;
+		default:
+			BUG_ON(1);
+		}
+
+		spin_lock(&priv->wsm_cmd.lock);
+		priv->wsm_cmd.ret = ret;
+		priv->wsm_cmd.done = 1;
+		spin_unlock(&priv->wsm_cmd.lock);
+		ret = 0; /* Error response from device should ne stop BH. */
+
+		wake_up_interruptible(&priv->wsm_cmd_wq);
+	} else if (id & 0x0800) {
+		switch (id) {
+		case 0x0801:
+			ret = wsm_startup_indication(priv, &wsm_buf);
+			break;
+		case 0x0804:
+			ret = wsm_receive_indication(priv, &wsm_buf, skb_p);
+			break;
+		case 0x0805:
+			ret = wsm_event_indication(priv, &wsm_buf);
+			break;
+		case 0x080A:
+			ret = wsm_channel_switch_indication(priv, &wsm_buf);
+			break;
+		case 0x0809:
+			ret = wsm_set_pm_indication(priv, &wsm_buf);
+			break;
+		case 0x0806:
+			ret = wsm_scan_complete_indication(priv, &wsm_buf);
+			break;
+		case 0x080B:
+			ret = wsm_find_complete_indication(priv, &wsm_buf);
+			break;
+		case 0x080C:
+			ret = wsm_suspend_resume_indication(priv, link_id, &wsm_buf);
+			break;
+		default:
+			STUB();
+		}
+	} else {
+		WARN_ON(1);
+		ret = -EINVAL;
+	}
+out:
+	return ret;
+}
+
+static bool wsm_handle_tx_data(struct cw1200_common *priv,
+			       const struct wsm_tx *wsm,
+			       const struct ieee80211_tx_info *tx_info)
+{
+	bool handled = false;
+	const struct ieee80211_hdr *frame =
+		(struct ieee80211_hdr *)&wsm[1];
+	__le16 fctl = frame->frame_control;
+	enum {
+		doProbe,
+		doDrop,
+		doJoin,
+		doWep,
+		doTx,
+	} action = doTx;
+
+	switch (priv->mode) {
+	case NL80211_IFTYPE_STATION:
+		if (unlikely(!priv->join_status ||
+			memcmp(frame->addr1, priv->join_bssid,
+				sizeof(priv->join_bssid)))) {
+			if (ieee80211_is_auth(fctl))
+				action = doJoin;
+			else
+				action = doDrop;
+		}
+		break;
+	case NL80211_IFTYPE_AP:
+		if (unlikely(!priv->join_status))
+			action = doDrop;
+		break;
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_MESH_POINT:
+		STUB();
+	case NL80211_IFTYPE_MONITOR:
+	default:
+		action = doDrop;
+		break;
+	}
+
+	if (action == doTx) {
+		if (unlikely(ieee80211_is_probe_req(fctl)))
+			action = doProbe;
+		else if ((fctl & __cpu_to_le32(IEEE80211_FCTL_PROTECTED)) &&
+			tx_info->control.hw_key &&
+			unlikely(tx_info->control.hw_key->keyidx !=
+					priv->wep_default_key_id) &&
+			(tx_info->control.hw_key->cipher ==
+					WLAN_CIPHER_SUITE_WEP40 ||
+			 tx_info->control.hw_key->cipher ==
+					WLAN_CIPHER_SUITE_WEP104))
+			action = doWep;
+	}
+
+	switch (action) {
+	case doProbe:
+	{
+		/* An interesting FW "feature". Device filters
+		 * probe responses.
+		 * The easiest way to get it back is to convert
+		 * probe request into WSM start_scan command. */
+		int rate_id = (wsm->flags >> 4) & 0x07;
+		struct cw1200_queue *queue =
+			&priv->tx_queue[cw1200_queue_get_queue_id(
+				wsm->packetID)];
+		wsm_printk(KERN_DEBUG \
+			"[WSM] Convert probe request to scan.\n");
+		wsm_lock_tx_async(priv);
+		BUG_ON(priv->scan.probe_skb);
+		BUG_ON(cw1200_queue_get_skb(queue,
+					wsm->packetID,
+					&priv->scan.probe_skb));
+		BUG_ON(cw1200_queue_remove(queue, priv,
+						wsm->packetID));
+		/* Release used TX rate policy */
+		tx_policy_put(priv, rate_id);
+		queue_delayed_work(priv->workqueue,
+				&priv->scan.probe_work, 0);
+		handled = true;
+	}
+	break;
+	case doDrop:
+	{
+		/* See detailed description of "join" below.
+		 * We are dropping everything except AUTH in non-joined mode. */
+		struct sk_buff *skb;
+		int rate_id = (wsm->flags >> 4) & 0x07;
+		struct cw1200_queue *queue =
+			&priv->tx_queue[cw1200_queue_get_queue_id(
+				wsm->packetID)];
+		wsm_printk(KERN_DEBUG "[WSM] Drop frame (0x%.4X):"
+			" not joined.\n", fctl);
+		BUG_ON(cw1200_queue_get_skb(queue, wsm->packetID, &skb));
+		BUG_ON(cw1200_queue_remove(queue, priv, wsm->packetID));
+		/* Release used TX rate policy */
+		tx_policy_put(priv, rate_id);
+		/* Release SKB. TODO: report TX failure. */
+		dev_kfree_skb(skb);
+		handled = true;
+	}
+	break;
+	case doJoin:
+	{
+		/* There is one more interesting "feature"
+		 * in FW: it can't do RX/TX before "join".
+		 * "Join" here is not an association,
+		 * but just a syncronization between AP and STA.
+		 * BTW that means device can't receive frames
+		 * in monitor mode.
+		 * priv->join_status is used only in bh thread and does
+		 * not require protection */
+		wsm_printk(KERN_DEBUG "[WSM] Issue join command.\n");
+		wsm_lock_tx_async(priv);
+		BUG_ON(priv->join_pending_frame);
+		priv->join_pending_frame = wsm;
+		if (queue_work(priv->workqueue, &priv->join_work) <= 0)
+			wsm_unlock_tx(priv);
+		handled = true;
+	}
+	break;
+	case doWep:
+	{
+		wsm_printk(KERN_DEBUG "[WSM] Issue set_default_wep_key.\n");
+		wsm_lock_tx_async(priv);
+		priv->wep_default_key_id = tx_info->control.hw_key->keyidx;
+		if (queue_work(priv->workqueue, &priv->wep_key_work) <= 0)
+			wsm_unlock_tx(priv);
+		handled = true;
+	}
+	break;
+	case doTx:
+	{
+#if 0
+		/* Kept for history. If you want to implement wsm->more,
+		 * make sure you are able to send a frame after that. */
+		wsm->more = (count > 1) ? 1 : 0;
+		if (wsm->more) {
+			/* HACK!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+			 * It's undocumented in WSM spec, but CW1200 hangs
+			 * if 'more' is set and no TX is performed due to TX
+			 * buffers limitation. */
+			if (priv->hw_bufs_used + 1 ==
+					priv->wsm_caps.numInpChBufs)
+				wsm->more = 0;
+		}
+
+		/* BUG!!! FIXME: we can't use 'more' at all: we don't know
+		 * future. It could be a request from upper layer with TX lock
+		 * requirements (scan, for example). If "more" is set device
+		 * will not send data and wsm_tx_lock() will fail...
+		 * It's not obvious how to fix this deadlock. Any ideas?
+		 * As a workaround more is set to 0. */
+		wsm->more = 0;
+#endif /* 0 */
+
+		if (ieee80211_is_deauth(fctl) &&
+				priv->mode != NL80211_IFTYPE_AP) {
+			/* Shedule unjoin work */
+			wsm_printk(KERN_DEBUG "[WSM] Issue unjoin command"
+				" (TX).\n");
+#if 0
+			wsm->more = 0;
+#endif /* 0 */
+			wsm_lock_tx_async(priv);
+			if (queue_work(priv->workqueue,
+					&priv->unjoin_work) <= 0)
+				wsm_unlock_tx(priv);
+		}
+	}
+	break;
+	}
+	return handled;
+}
+
+static int wsm_get_tx_queue_and_mask(struct cw1200_common *priv,
+				     struct cw1200_queue **queue_p,
+				     u32* tx_allowed_mask_p,
+				     bool *more)
+{
+	int i;
+	struct cw1200_queue *queue = NULL;
+	u32 tx_allowed_mask;
+	int mcasts = 0;
+
+	/* Search for a queue with multicast frames buffered */
+	if (priv->sta_asleep_mask && !priv->suspend_multicast) {
+		tx_allowed_mask = 1 << CW1200_LINK_ID_AFTER_DTIM;
+		for (i = 0; i < 4; ++i) {
+			mcasts += cw1200_queue_get_num_queued(
+					&priv->tx_queue[i], tx_allowed_mask);
+			if (!queue && mcasts)
+				queue = &priv->tx_queue[i];
+			if (mcasts > 1)
+				break;
+		}
+		if (mcasts)
+			goto found;
+	}
+
+	/* Search for unicast traffic */
+	for (i = 0; i < 4; ++i) {
+		queue = &priv->tx_queue[i];
+		tx_allowed_mask = ~priv->sta_asleep_mask;
+		if (priv->sta_asleep_mask) {
+			tx_allowed_mask |= ~priv->tx_suspend_mask[i];
+		} else {
+			tx_allowed_mask |= 1 << CW1200_LINK_ID_AFTER_DTIM;
+		}
+		if (cw1200_queue_get_num_queued(
+				queue, tx_allowed_mask))
+			goto found;
+	}
+	return -ENOENT;
+
+found:
+	*queue_p = queue;
+	*tx_allowed_mask_p = tx_allowed_mask;
+	*more = mcasts > 1;
+	return 0;
+}
+
+int wsm_get_tx(struct cw1200_common *priv, u8 **data,
+	       size_t *tx_len)
+{
+	struct wsm_tx *wsm = NULL;
+	struct ieee80211_tx_info *tx_info;
+	struct cw1200_queue *queue;
+	struct cw1200_sta_priv *sta_priv;
+	u32 tx_allowed_mask = 0;
+	/*
+	 * Count was intended as an input for wsm->more flag.
+	 * During implementation it was found that wsm->more
+	 * is not usable, see details above. It is kept just
+	 * in case you would like to try to implement it again.
+	 */
+	int count = 0;
+
+	/* More is used only for broadcasts. */
+	bool more;
+
+	if (priv->wsm_cmd.ptr) {
+		++count;
+		spin_lock(&priv->wsm_cmd.lock);
+		BUG_ON(!priv->wsm_cmd.ptr);
+		*data = priv->wsm_cmd.ptr;
+		*tx_len = priv->wsm_cmd.len;
+		spin_unlock(&priv->wsm_cmd.lock);
+	} else {
+		for (;;) {
+			if (atomic_add_return(0, &priv->tx_lock))
+				break;
+
+			if (wsm_get_tx_queue_and_mask(priv, &queue,
+					&tx_allowed_mask, &more))
+				break;
+
+			if (cw1200_queue_get(queue,
+					tx_allowed_mask,
+					&wsm, &tx_info))
+				continue;
+
+			if (wsm_handle_tx_data(priv, wsm, tx_info))
+				continue;  /* Handled by WSM */
+
+			if (tx_info->control.sta) {
+				/* Update link id */
+				sta_priv = (struct cw1200_sta_priv *)
+					&tx_info->control.sta->drv_priv;
+				wsm->hdr.id &= __cpu_to_le16(~(0x0F << 6));
+				wsm->hdr.id |=
+					cpu_to_le16(sta_priv->link_id << 6);
+			}
+
+			*data = (u8 *)wsm;
+			*tx_len = __le16_to_cpu(wsm->hdr.len);
+			if (more) {
+				struct ieee80211_hdr *hdr =
+					(struct ieee80211_hdr *) &wsm[1];
+				/* more buffered multicast/broadcast frames
+				 *  ==> set MoreData flag in IEEE 802.11 header
+				 *  to inform PS STAs */
+				hdr->frame_control |=
+					cpu_to_le16(IEEE80211_FCTL_MOREDATA);
+			} else if (priv->mode == NL80211_IFTYPE_AP &&
+					!priv->suspend_multicast) {
+				priv->suspend_multicast = true;
+				queue_work(priv->workqueue,
+					&priv->multicast_stop_work);
+			}
+
+			wsm_printk(KERN_DEBUG "[WSM] >>> 0x%.4X (%d) %p %c\n",
+				0x0004, *tx_len, *data,
+				wsm->more ? 'M' : ' ');
+			++count;
+			break;
+		}
+	}
+
+	return count;
+}
+
+void wsm_txed(struct cw1200_common *priv, u8 *data)
+{
+	if (data == priv->wsm_cmd.ptr) {
+		spin_lock(&priv->wsm_cmd.lock);
+		priv->wsm_cmd.ptr = NULL;
+		spin_unlock(&priv->wsm_cmd.lock);
+	}
+}
+
+/* ******************************************************************** */
+/* WSM buffer								*/
+
+void wsm_buf_init(struct wsm_buf *buf)
+{
+	BUG_ON(buf->begin);
+	buf->begin = kmalloc(SDIO_BLOCK_SIZE, GFP_KERNEL | GFP_DMA);
+	buf->end = buf->begin ? &buf->begin[SDIO_BLOCK_SIZE] : buf->begin;
+	wsm_buf_reset(buf);
+}
+
+void wsm_buf_deinit(struct wsm_buf *buf)
+{
+	kfree(buf->begin);
+	buf->begin = buf->data = buf->end = NULL;
+}
+
+static void wsm_buf_reset(struct wsm_buf *buf)
+{
+	if (buf->begin) {
+		buf->data = &buf->begin[4];
+		*(u32 *)buf->begin = 0;
+	} else
+		buf->data = buf->begin;
+}
+
+static int wsm_buf_reserve(struct wsm_buf *buf, size_t extra_size)
+{
+	size_t pos = buf->data - buf->begin;
+	size_t size = pos + extra_size;
+
+
+	if (size & (SDIO_BLOCK_SIZE - 1)) {
+		size &= SDIO_BLOCK_SIZE;
+		size += SDIO_BLOCK_SIZE;
+	}
+
+	buf->begin = krealloc(buf->begin, size, GFP_KERNEL | GFP_DMA);
+	if (buf->begin) {
+		buf->data = &buf->begin[pos];
+		buf->end = &buf->begin[size];
+		return 0;
+	} else {
+		buf->end = buf->data = buf->begin;
+		return -ENOMEM;
+	}
+}
+
+
diff --git a/drivers/staging/cw1200/wsm.h b/drivers/staging/cw1200/wsm.h
new file mode 100644
index 00000000000..4d446e9b924
--- /dev/null
+++ b/drivers/staging/cw1200/wsm.h
@@ -0,0 +1,1527 @@
+/*
+ * WSM host interface (HI) interface for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on CW1200 UMAC WSM API, which is
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Stewart Mathers <stewart.mathers@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_WSM_H_INCLUDED
+#define CW1200_WSM_H_INCLUDED
+
+#include <linux/spinlock.h>
+
+struct cw1200_common;
+
+/* Bands */
+/* Radio band 2.412 -2.484 GHz. */
+#define WSM_PHY_BAND_2_4G		(0)
+
+/* Radio band 4.9375-5.8250 GHz. */
+#define WSM_PHY_BAND_5G			(1)
+
+/* Transmit rates */
+/* 1   Mbps            ERP-DSSS */
+#define WSM_TRANSMIT_RATE_1		(0)
+
+/* 2   Mbps            ERP-DSSS */
+#define WSM_TRANSMIT_RATE_2		(1)
+
+/* 5.5 Mbps            ERP-CCK, ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_5		(2) */
+
+/* 11  Mbps            ERP-CCK, ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_11		(3) */
+
+/* 22  Mbps            ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_22		(4) */
+
+/* 33  Mbps            ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_33		(5) */
+
+/* 6   Mbps   (3 Mbps) ERP-OFDM, BPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_6		(6)
+
+/* 9   Mbps (4.5 Mbps) ERP-OFDM, BPSK coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_9		(7)
+
+/* 12  Mbps  (6 Mbps)  ERP-OFDM, QPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_12		(8)
+
+/* 18  Mbps  (9 Mbps)  ERP-OFDM, QPSK coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_18		(9)
+
+/* 24  Mbps (12 Mbps)  ERP-OFDM, 16QAM coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_24		(10)
+
+/* 36  Mbps (18 Mbps)  ERP-OFDM, 16QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_36		(11)
+
+/* 48  Mbps (24 Mbps)  ERP-OFDM, 64QAM coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_48		(12)
+
+/* 54  Mbps (27 Mbps)  ERP-OFDM, 64QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_54		(13)
+
+/* 6.5 Mbps            HT-OFDM, BPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_HT_6		(14)
+
+/* 13  Mbps            HT-OFDM, QPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_HT_13		(15)
+
+/* 19.5 Mbps           HT-OFDM, QPSK coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_HT_19		(16)
+
+/* 26  Mbps            HT-OFDM, 16QAM coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_HT_26		(17)
+
+/* 39  Mbps            HT-OFDM, 16QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_HT_39		(18)
+
+/* 52  Mbps            HT-OFDM, 64QAM coding rate 2/3 */
+#define WSM_TRANSMIT_RATE_HT_52		(19)
+
+/* 58.5 Mbps           HT-OFDM, 64QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_HT_58		(20)
+
+/* 65  Mbps            HT-OFDM, 64QAM coding rate 5/6 */
+#define WSM_TRANSMIT_RATE_HT_65		(21)
+
+/* Scan types */
+/* Foreground scan */
+#define WSM_SCAN_TYPE_FOREGROUND	(0)
+
+/* Background scan */
+#define WSM_SCAN_TYPE_BACKGROUND	(1)
+
+/* Auto scan */
+#define WSM_SCAN_TYPE_AUTO		(2)
+
+/* Scan flags */
+/* Forced background scan means if the station cannot */
+/* enter the power-save mode, it shall force to perform a */
+/* background scan. Only valid when ScanType is */
+/* background scan. */
+#define WSM_SCAN_FLAG_FORCE_BACKGROUND	(BIT(0))
+
+/* The WLAN device scans one channel at a time so */
+/* that disturbance to the data traffic is minimized. */
+#define WSM_SCAN_FLAG_SPLIT_METHOD	(BIT(1))
+
+/* Preamble Type. Long if not set. */
+#define WSM_SCAN_FLAG_SHORT_PREAMBLE	(BIT(2))
+
+/* 11n Tx Mode. Mixed if not set. */
+#define WSM_SCAN_FLAG_11N_GREENFIELD	(BIT(3))
+
+/* Scan constraints */
+/* Maximum number of channels to be scanned. */
+#define WSM_SCAN_MAX_NUM_OF_CHANNELS	(48)
+
+/* The maximum number of SSIDs that the device can scan for. */
+#define WSM_SCAN_MAX_NUM_OF_SSIDS	(2)
+
+/* Power management modes */
+/* 802.11 Active mode */
+#define WSM_PSM_ACTIVE			(0)
+
+/* 802.11 PS mode */
+#define WSM_PSM_PS			(1)
+
+/* Undetermined */
+/* Note : Undetermined status is reported when the */
+/* NULL data frame used to advertise the PM mode to */
+/* the AP at Pre or Post Background Scan is not Acknowledged */
+#define WSM_PSM_UNKNOWN			(2)
+
+/* Use this flag to enable the fast power-saving mode */
+#define WSM_PM_F_FAST_PSM_ENABLE	(0x80)
+
+/* Queue IDs */
+/* best effort/legacy */
+#define WSM_QUEUE_BEST_EFFORT		(0)
+
+/* background */
+#define WSM_QUEUE_BACKGROUND		(1)
+
+/* video */
+#define WSM_QUEUE_VIDEO			(2)
+
+/* voice */
+#define WSM_QUEUE_VOICE			(3)
+
+/* HT TX parameters */
+/* Non-HT */
+#define WSM_HT_TX_NON_HT		(0)
+
+/* Mixed format */
+#define WSM_HT_TX_MIXED			(1)
+
+/* Greenfield format */
+#define WSM_HT_TX_GREENFIELD		(2)
+
+/* STBC allowed */
+#define WSM_HT_TX_STBC			(BIT(7))
+
+/* TX status */
+/* Frame was sent aggregated */
+/* Only valid for WSM_SUCCESS status. */
+#define WSM_TX_STATUS_AGGREGATION	(BIT(0))
+
+/* Host should requeue this frame later. */
+/* Valid only when status is WSM_REQUEUE. */
+#define WSM_TX_STATUS_REQUEUE		(BIT(1))
+
+/* Normal Ack */
+#define WSM_TX_STATUS_NORMAL_ACK	(0<<2)
+
+/* No Ack */
+#define WSM_TX_STATUS_NO_ACK		(1<<2)
+
+/* No explicit acknowledgement */
+#define WSM_TX_STATUS_NO_EXPLICIT_ACK	(2<<2)
+
+/* Block Ack */
+/* Only valid for WSM_SUCCESS status. */
+#define WSM_TX_STATUS_BLOCK_ACK		(3<<2)
+
+/* RX status */
+/* Unencrypted */
+#define WSM_RX_STATUS_UNENCRYPTED	(0<<0)
+
+/* WEP */
+#define WSM_RX_STATUS_WEP		(1<<0)
+
+/* TKIP */
+#define WSM_RX_STATUS_TKIP		(2<<0)
+
+/* AES */
+#define WSM_RX_STATUS_AES		(3<<0)
+
+/* WAPI */
+#define WSM_RX_STATUS_WAPI		(4<<0)
+
+/* Macro to fetch encryption subfield. */
+#define WSM_RX_STATUS_ENCRYPTION(status) ((status) & 0x07)
+
+/* Frame was part of an aggregation */
+#define WSM_RX_STATUS_AGGREGATE		(BIT(3))
+
+/* Frame was first in the aggregation */
+#define WSM_RX_STATUS_AGGREGATE_FIRST	(BIT(4))
+
+/* Frame was last in the aggregation */
+#define WSM_RX_STATUS_AGGREGATE_LAST	(BIT(5))
+
+/* Indicates a defragmented frame */
+#define WSM_RX_STATUS_DEFRAGMENTED	(BIT(6))
+
+/* Indicates a Beacon frame */
+#define WSM_RX_STATUS_BEACON		(BIT(7))
+
+/* Indicates STA bit beacon TIM field */
+#define WSM_RX_STATUS_TIM		(BIT(8))
+
+/* Indicates Beacon frame's virtual bitmap contains multicast bit */
+#define WSM_RX_STATUS_MULTICAST		(BIT(9))
+
+/* Indicates frame contains a matching SSID */
+#define WSM_RX_STATUS_MATCHING_SSID	(BIT(10))
+
+/* Indicates frame contains a matching BSSI */
+#define WSM_RX_STATUS_MATCHING_BSSI	(BIT(11))
+
+/* Indicates More bit set in Framectl field */
+#define WSM_RX_STATUS_MORE_DATA		(BIT(12))
+
+/* Indicates frame received during a measurement process */
+#define WSM_RX_STATUS_MEASUREMENT	(BIT(13))
+
+/* Indicates frame received as an HT packet */
+#define WSM_RX_STATUS_HT		(BIT(14))
+
+/* Indicates frame received with STBC */
+#define WSM_RX_STATUS_STBC		(BIT(15))
+
+/* Indicates Address 1 field matches dot11StationId */
+#define WSM_RX_STATUS_ADDRESS1		(BIT(16))
+
+/* Indicates Group address present in the Address 1 field */
+#define WSM_RX_STATUS_GROUP		(BIT(17))
+
+/* Indicates Broadcast address present in the Address 1 field */
+#define WSM_RX_STATUS_BROADCAST		(BIT(18))
+
+/* Indicates group key used with encrypted frames */
+#define WSM_RX_STATUS_GROUP_KEY		(BIT(19))
+
+/* Macro to fetch encryption key index. */
+#define WSM_RX_STATUS_KEY_IDX(status)	(((status >> 20)) & 0x0F)
+
+/* Join mode */
+/* IBSS */
+#define WSM_JOIN_MODE_IBSS		(0)
+
+/* BSS */
+#define WSM_JOIN_MODE_BSS		(1)
+
+/* PLCP preamble type */
+/* For long preamble */
+#define WSM_JOIN_PREAMBLE_LONG		(0)
+
+/* For short preamble (Long for 1Mbps) */
+#define WSM_JOIN_PREAMBLE_SHORT		(1)
+
+/* For short preamble (Long for 1 and 2Mbps) */
+#define WSM_JOIN_PREAMBLE_SHORT_2	(2)
+
+/* Join flags */
+/* Unsynchronized */
+#define WSM_JOIN_FLAGS_UNSYNCRONIZED	(1)
+
+/* Key types */
+#define WSM_KEY_TYPE_WEP_DEFAULT	(0)
+#define WSM_KEY_TYPE_WEP_PAIRWISE	(1)
+#define WSM_KEY_TYPE_TKIP_GROUP		(2)
+#define WSM_KEY_TYPE_TKIP_PAIRWISE	(3)
+#define WSM_KEY_TYPE_AES_GROUP		(4)
+#define WSM_KEY_TYPE_AES_PAIRWISE	(5)
+#define WSM_KEY_TYPE_WAPI_GROUP		(6)
+#define WSM_KEY_TYPE_WAPI_PAIRWISE	(7)
+
+/* Key indexes */
+#define WSM_KEY_MAX_INDEX		(10)
+
+/* ACK policy */
+#define WSM_ACK_POLICY_NORMAL		(0)
+#define WSM_ACK_POLICY_NO_ACK		(1)
+
+/* Start modes */
+#define WSM_START_MODE_AP		(0)	/* Mini AP */
+#define WSM_START_MODE_P2P_GO		(1)	/* P2P GO */
+#define WSM_START_MODE_P2P_DEV		(2)	/* P2P device */
+
+/* SetAssociationMode MIB flags */
+#define WSM_ASSOCIATION_MODE_USE_PREAMBLE_TYPE		(BIT(0))
+#define WSM_ASSOCIATION_MODE_USE_HT_MODE		(BIT(1))
+#define WSM_ASSOCIATION_MODE_USE_BASIC_RATE_SET		(BIT(2))
+#define WSM_ASSOCIATION_MODE_USE_MPDU_START_SPACING	(BIT(3))
+#define WSM_ASSOCIATION_MODE_SNOOP_ASSOC_FRAMES		(BIT(4))
+
+/* RcpiRssiThreshold MIB flags */
+#define WSM_RCPI_RSSI_THRESHOLD_ENABLE	(BIT(0))
+#define WSM_RCPI_RSSI_USE_RSSI		(BIT(1))
+#define WSM_RCPI_RSSI_DONT_USE_UPPER	(BIT(2))
+#define WSM_RCPI_RSSI_DONT_USE_LOWER	(BIT(3))
+
+/* WSM events */
+/* Error */
+#define WSM_EVENT_ERROR			(0)
+
+/* BSS lost */
+#define WSM_EVENT_BSS_LOST		(1)
+
+/* BSS regained */
+#define WSM_EVENT_BSS_REGAINED		(2)
+
+/* Radar detected */
+#define WSM_EVENT_RADAR_DETECTED	(3)
+
+/* RCPI or RSSI threshold triggered */
+#define WSM_EVENT_RCPI_RSSI		(4)
+
+/* BT inactive */
+#define WSM_EVENT_BT_INACTIVE		(5)
+
+/* BT active */
+#define WSM_EVENT_BT_ACTIVE		(6)
+
+/* MIB IDs */
+/* 4.1  dot11StationId */
+#define WSM_MIB_ID_DOT11_STATION_ID		0x0000
+
+/* 4.2  dot11MaxtransmitMsduLifeTime */
+#define WSM_MIB_ID_DOT11_MAX_TRANSMIT_LIFTIME	0x0001
+
+/* 4.3  dot11MaxReceiveLifeTime */
+#define WSM_MIB_ID_DOT11_MAX_RECEIVE_LIFETIME	0x0002
+
+/* 4.4  dot11SlotTime */
+#define WSM_MIB_ID_DOT11_SLOT_TIME		0x0003
+
+/* 4.5  dot11GroupAddressesTable */
+#define WSM_MIB_ID_DOT11_GROUP_ADDRESSES_TABLE	0x0004
+
+/* 4.6  dot11WepDefaultKeyId */
+#define WSM_MIB_ID_DOT11_WEP_DEFAULT_KEY_ID	0x0005
+
+/* 4.7  dot11CurrentTxPowerLevel */
+#define WSM_MIB_ID_DOT11_CURRENT_TX_POWER_LEVEL	0x0006
+
+/* 4.8  dot11RTSThreshold */
+#define WSM_MIB_ID_DOT11_RTS_THRESHOLD		0x0007
+
+/* 4.9  NonErpProtection */
+#define WSM_MIB_ID_NON_ERP_PROTECTION		0x1000
+
+/* 4.10 ArpIpAddressesTable */
+#define WSM_MIB_ID_ARP_IP_ADDRESSES_TABLE	0x1001
+
+/* 4.11 TemplateFrame */
+#define WSM_MIB_ID_TEMPLATE_FRAME		0x1002
+
+/* 4.12 RxFilter */
+#define WSM_MIB_ID_RX_FILTER			0x1003
+
+/* 4.13 BeaconFilterTable */
+#define WSM_MIB_ID_BEACON_FILTER_TABLE		0x1004
+
+/* 4.14 BeaconFilterEnable */
+#define WSM_MIB_ID_BEACON_FILTER_ENABLE		0x1005
+
+/* 4.15 OperationalPowerMode */
+#define WSM_MIB_ID_OPERATIONAL_POWER_MODE	0x1006
+
+/* 4.16 BeaconWakeUpPeriod */
+#define WSM_MIB_ID_BEACON_WAKEUP_PERIOD		0x1007
+
+/* 4.17 RcpiRssiThreshold */
+#define WSM_MIB_ID_RCPI_RSSI_THRESHOLD		0x1009
+
+/* 4.18 StatisticsTable */
+#define WSM_MIB_ID_STATISTICS_TABLE		0x100A
+
+/* 4.19 IbssPsConfig */
+#define WSM_MIB_ID_IBSS_PS_CONFIG		0x100B
+
+/* 4.20 CountersTable */
+#define WSM_MIB_ID_COUNTERS_TABLE		0x100C
+
+/* 4.21 BlockAckPolicy */
+#define WSM_MIB_ID_BLOCK_ACK_POLICY		0x100E
+
+/* 4.22 OverrideInternalTxRate */
+#define WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE	0x100F
+
+/* 4.23 SetAssociationMode */
+#define WSM_MIB_ID_SET_ASSOCIATION_MODE		0x1010
+
+/* 4.24 UpdateEptaConfigData */
+#define WSM_MIB_ID_UPDATE_EPTA_CONFIG_DATA	0x1011
+
+/* 4.25 SelectCcaMethod */
+#define WSM_MIB_ID_SELECT_CCA_METHOD		0x1012
+
+/* 4.26 SetUpasdInformation */
+#define WSM_MIB_ID_SET_UAPSD_INFORMATION	0x1013
+
+/* 4.27 SetAutoCalibrationMode  WBF00004073 */
+#define WSM_MIB_ID_SET_AUTO_CALIBRATION_MODE	0x1015
+
+/* 4.28 SetTxRateRetryPolicy */
+#define WSM_MIB_ID_SET_TX_RATE_RETRY_POLICY	0x1016
+
+/* 4.29 SetHostMessageTypeFilter */
+#define WSM_MIB_ID_SET_HOST_MSG_TYPE_FILTER	0x1017
+
+/* 4.30 P2PFindInfo */
+#define WSM_MIB_ID_P2P_FIND_INFO		0x1018
+
+/* 4.31 P2PPsModeInfo */
+#define WSM_MIB_ID_P2P_PS_MODE_INFO		0x1019
+
+/* 4.32 SetEtherTypeDataFrameFilter */
+#define WSM_MIB_ID_SET_ETHERTYPE_DATAFRAME_FILTER 0x101A
+
+/* 4.33 SetUDPPortDataFrameFilter */
+#define WSM_MIB_ID_SET_UDPPORT_DATAFRAME_FILTER	0x101B
+
+/* 4.34 SetMagicDataFrameFilter */
+#define WSM_MIB_ID_SET_MAGIC_DATAFRAME_FILTER	0x101C
+
+/* This is the end of specification. */
+
+/* 4.35 P2PDeviceInfo */
+#define WSM_MIB_ID_P2P_DEVICE_INFO		0x101D
+
+/* 4.36 SetWCDMABand */
+#define WSM_MIB_ID_SET_WCDMA_BAND		0x101E
+
+/* 4.37 GroupTxSequenceCounter */
+#define WSM_MIB_ID_GRP_SEQ_COUNTER		0x101F
+
+/* 1020 4.38 ProtectedMgmtPolicy */
+
+/* 4.39 SetHtProtection */
+#define WSM_MID_ID_SET_HT_PROTECTION		0x1021
+
+/* 4.40 GPIO Command */
+#define WSM_MIB_ID_GPIO_COMMAND			0x1022
+
+/* 4.41 TSF Counter Value */
+#define WSM_MIB_ID_TSF_COUNTER			0x1023
+
+/* Test Purposes Only */
+#define WSM_MIB_ID_BLOCK_ACK_INFO		0x100D
+
+/* 4.42 UseMultiTxConfMessage */
+#define WSM_MIB_USE_MULTI_TX_CONF		0x1024
+
+/* 4.43 Keep-alive period */
+#define WSM_MIB_ID_KEEP_ALIVE_PERIOD		0x1025
+
+/* 4.44 Disable BSSID filter */
+#define WSM_MIB_ID_DISABLE_BSSID_FILTER		0x1026
+
+/* Frame template types */
+#define WSM_FRAME_TYPE_PROBE_REQUEST	(0)
+#define WSM_FRAME_TYPE_BEACON		(1)
+#define WSM_FRAME_TYPE_NULL		(2)
+#define WSM_FRAME_TYPE_QOS_NULL		(3)
+#define WSM_FRAME_TYPE_PS_POLL		(4)
+#define WSM_FRAME_TYPE_PROBE_RESPONSE	(5)
+
+#define WSM_FRAME_GREENFIELD		(0x80)	/* See 4.11 */
+
+/* Status */
+/* The WSM firmware has completed a request */
+/* successfully. */
+#define WSM_STATUS_SUCCESS              (0)
+
+/* This is a generic failure code if other error codes do */
+/* not apply. */
+#define WSM_STATUS_FAILURE              (1)
+
+/* A request contains one or more invalid parameters. */
+#define WSM_INVALID_PARAMETER           (2)
+
+/* The request cannot perform because the device is in */
+/* an inappropriate mode. */
+#define WSM_ACCESS_DENIED               (3)
+
+/* The frame received includes a decryption error. */
+#define WSM_STATUS_DECRYPTFAILURE       (4)
+
+/* A MIC failure is detected in the received packets. */
+#define WSM_STATUS_MICFAILURE           (5)
+
+/* The transmit request failed due to retry limit being */
+/* exceeded. */
+#define WSM_STATUS_RETRY_EXCEEDED       (6)
+
+/* The transmit request failed due to MSDU life time */
+/* being exceeded. */
+#define WSM_STATUS_TX_LIFETIME_EXCEEDED (7)
+
+/* The link to the AP is lost. */
+#define WSM_STATUS_LINK_LOST            (8)
+
+/* No key was found for the encrypted frame */
+#define WSM_STATUS_NO_KEY_FOUND         (9)
+
+/* Jammer was detected when transmitting this frame */
+#define WSM_STATUS_JAMMER_DETECTED      (10)
+
+/* The message should be requeued later. */
+/* This is applicable only to Transmit */
+#define WSM_REQUEUE                     (11)
+
+
+struct wsm_hdr {
+	__le16 len;
+	__le16 id;
+};
+
+/* ******************************************************************** */
+/* WSM capcbility							*/
+
+struct wsm_caps {
+	u16 numInpChBufs;
+	u16 sizeInpChBuf;
+	u16 hardwareId;
+	u16 hardwareSubId;
+	u16 firmwareCap;
+	u16 firmwareType;
+	u16 firmwareApiVer;
+	u16 firmwareBuildNumber;
+	u16 firmwareVersion;
+	int firmwareReady;
+};
+
+/* ******************************************************************** */
+/* WSM commands								*/
+
+struct wsm_tx_power_range {
+	int min_power_level;
+	int max_power_level;
+	u32 stepping;
+};
+
+/* 3.1 */
+struct wsm_configuration {
+	/* [in] */ u32 dot11MaxTransmitMsduLifeTime;
+	/* [in] */ u32 dot11MaxReceiveLifeTime;
+	/* [in] */ u32 dot11RtsThreshold;
+	/* [in, out] */ u8 *dot11StationId;
+	/* [in] */ const void *dpdData;
+	/* [in] */ size_t dpdData_size;
+	/* [out] */ u8 dot11FrequencyBandsSupported;
+	/* [out] */ u32 supportedRateMask;
+	/* [out] */ struct wsm_tx_power_range txPowerRange[2];
+};
+
+int wsm_configuration(struct cw1200_common *priv,
+		      struct wsm_configuration *arg);
+
+/* 3.3 */
+struct wsm_reset {
+	/* [in] */ int link_id;
+	/* [in] */ bool reset_statistics;
+};
+
+int wsm_reset(struct cw1200_common *priv, const struct wsm_reset *arg);
+
+/* 3.5 */
+int wsm_read_mib(struct cw1200_common *priv, u16 mibId, void *buf,
+		 size_t buf_size);
+
+/* 3.7 */
+int wsm_write_mib(struct cw1200_common *priv, u16 mibId, void *buf,
+		  size_t buf_size);
+
+/* 3.9 */
+struct wsm_ssid {
+	u8 ssid[32];
+	u32 length;
+};
+
+struct wsm_scan_ch {
+	u16 number;
+	u32 minChannelTime;
+	u32 maxChannelTime;
+	u32 txPowerLevel;
+};
+
+/* 3.13 */
+struct wsm_scan_complete {
+	/* WSM_STATUS_... */
+	u32 status;
+
+	/* WSM_PSM_... */
+	u8 psm;
+
+	/* Number of channels that the scan operation completed. */
+	u8 numChannels;
+};
+
+typedef void (*wsm_scan_complete_cb) (struct cw1200_common *priv,
+				      struct wsm_scan_complete *arg);
+
+/* 3.9 */
+struct wsm_scan {
+	/* WSM_PHY_BAND_... */
+	/* [in] */ u8 band;
+
+	/* WSM_SCAN_TYPE_... */
+	/* [in] */ u8 scanType;
+
+	/* WSM_SCAN_FLAG_... */
+	/* [in] */ u8 scanFlags;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [in] */ u8 maxTransmitRate;
+
+	/* Interval period in TUs that the device shall the re- */
+	/* execute the requested scan. Max value supported by the device */
+	/* is 256s. */
+	/* [in] */ u32 autoScanInterval;
+
+	/* Number of probe requests (per SSID) sent to one (1) */
+	/* channel. Zero (0) means that none is send, which */
+	/* means that a passive scan is to be done. Value */
+	/* greater than zero (0) means that an active scan is to */
+	/* be done. */
+	/* [in] */ u32 numOfProbeRequests;
+
+	/* Number of channels to be scanned. */
+	/* Maximum value is WSM_SCAN_MAX_NUM_OF_CHANNELS. */
+	/* [in] */ u8 numOfChannels;
+
+	/* Number of SSID provided in the scan command (this */
+	/* is zero (0) in broadcast scan) */
+	/* The maximum number of SSIDs is WSM_SCAN_MAX_NUM_OF_SSIDS. */
+	/* [in] */ u8 numOfSSIDs;
+
+	/* The delay time (in microseconds) period */
+	/* before sending a probe-request. */
+	/* [in] */ u8 probeDelay;
+
+	/* SSIDs to be scanned [numOfSSIDs]; */
+	/* [in] */ struct wsm_ssid *ssids;
+
+	/* Channels to be scanned [numOfChannels]; */
+	/* [in] */ struct wsm_scan_ch *ch;
+};
+
+int wsm_scan(struct cw1200_common *priv, const struct wsm_scan *arg);
+
+/* 3.11 */
+int wsm_stop_scan(struct cw1200_common *priv);
+
+/* 3.14 */
+struct wsm_tx_confirm {
+	/* Packet identifier used in wsm_tx. */
+	/* [out] */ u32 packetID;
+
+	/* WSM_STATUS_... */
+	/* [out] */ u32 status;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [out] */ u8 txedRate;
+
+	/* The number of times the frame was transmitted */
+	/* without receiving an acknowledgement. */
+	/* [out] */ u8 ackFailures;
+
+	/* WSM_TX_STATUS_... */
+	/* [out] */ u16 flags;
+
+	/* The total time in microseconds that the frame spent in */
+	/* the WLAN device before transmission as completed. */
+	/* [out] */ u32 mediaDelay;
+
+	/* The total time in microseconds that the frame spent in */
+	/* the WLAN device before transmission was started. */
+	/* [out] */ u32 txQueueDelay;
+};
+
+/* 3.15 */
+typedef void (*wsm_tx_confirm_cb) (struct cw1200_common *priv,
+				   struct wsm_tx_confirm *arg);
+
+/* Note that ideology of wsm_tx struct is different against the rest of
+ * WSM API. wsm_hdr is /not/ a caller-adapted struct to be used as an input
+ * argument for WSM call, but a prepared bytestream to be sent to firmware.
+ * It is filled partly in cw1200_skb_to_wsm, partly in low-level WSM code.
+ * Please pay attention once again: ideology is different.
+ *
+ * Legend:
+ * - [in]: cw1200_skb_to_wsm must fill this field.
+ * - [wsm]: the field is filled by low-level WSM.
+ */
+struct wsm_tx {
+	/* common WSM header */
+	/* [in/wsm] */ struct wsm_hdr hdr;
+
+	/* Packet identifier that meant to be used in completion. */
+	/* [in] */ __le32 packetID;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [in] */ u8 maxTxRate;
+
+	/* WSM_QUEUE_... */
+	/* [in] */ u8 queueId;
+
+	/* True: another packet is pending on the host for transmission. */
+	/* [wsm] */ u8 more;
+
+	/* Bit 0 = 0 - Start expiry time from first Tx attempt (default) */
+	/* Bit 0 = 1 - Start expiry time from receipt of Tx Request */
+	/* Bits 3:1  - PTA Priority */
+	/* Bits 6:4  - Tx Rate Retry Policy */
+	/* Bit 7 - Reserved */
+	/* [in] */ u8 flags;
+
+	/* Should be 0. */
+	/* [in] */ __le32 reserved;
+
+	/* The elapsed time in TUs, after the initial transmission */
+	/* of an MSDU, after which further attempts to transmit */
+	/* the MSDU shall be terminated. Overrides the global */
+	/* dot11MaxTransmitMsduLifeTime setting [optional] */
+	/* Device will set the default value if this is 0. */
+	/* [wsm] */ __le32 expireTime;
+
+	/* WSM_HT_TX_... */
+	/* [in] */ __le32 htTxParameters;
+};
+
+/* = sizeof(generic hi hdr) + sizeof(wsm hdr) + sizeof(alignment) */
+#define WSM_TX_EXTRA_HEADROOM (28)
+
+/* 3.16 */
+struct wsm_rx {
+	/* WSM_STATUS_... */
+	/* [out] */ u32 status;
+
+	/* Specifies the channel of the received packet. */
+	/* [out] */ u16 channelNumber;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [out] */ u8 rxedRate;
+
+	/* This value is expressed in signed Q8.0 format for */
+	/* RSSI and unsigned Q7.1 format for RCPI. */
+	/* [out] */ u8 rcpiRssi;
+
+	/* WSM_RX_STATUS_... */
+	/* [out] */ u32 flags;
+
+	/* An 802.11 frame. */
+	/* [out] */ void *frame;
+
+	/* Size of the frame */
+	/* [out] */ size_t frame_size;
+};
+
+/* = sizeof(generic hi hdr) + sizeof(wsm hdr) */
+#define WSM_RX_EXTRA_HEADROOM (16)
+
+typedef void (*wsm_rx_cb) (struct cw1200_common *priv, struct wsm_rx *arg,
+			   struct sk_buff **skb_p);
+
+/* 3.17 */
+struct wsm_event {
+	/* WSM_STATUS_... */
+	/* [out] */ u32 eventId;
+
+	/* Indication parameters. */
+	/* For error indication, this shall be a 32-bit WSM status. */
+	/* For RCPI or RSSI indication, this should be an 8-bit */
+	/* RCPI or RSSI value. */
+	/* [out] */ u32 eventData;
+};
+
+struct cw1200_wsm_event {
+	struct list_head link;
+	struct wsm_event evt;
+};
+
+/* 3.18 - 3.22 */
+/* Measurement. Skipped for now. Irrelevent. */
+
+typedef void (*wsm_event_cb) (struct cw1200_common *priv,
+			      struct wsm_event *arg);
+
+/* 3.23 */
+struct wsm_join {
+	/* WSM_JOIN_MODE_... */
+	/* [in] */ u8 mode;
+
+	/* WSM_PHY_BAND_... */
+	/* [in] */ u8 band;
+
+	/* Specifies the channel number to join. The channel */
+	/* number will be mapped to an actual frequency */
+	/* according to the band */
+	/* [in] */ u16 channelNumber;
+
+	/* Specifies the BSSID of the BSS or IBSS to be joined */
+	/* or the IBSS to be started. */
+	/* [in] */ u8 bssid[6];
+
+	/* ATIM window of IBSS */
+	/* When ATIM window is zero the initiated IBSS does */
+	/* not support power saving. */
+	/* [in] */ u16 atimWindow;
+
+	/* WSM_JOIN_PREAMBLE_... */
+	/* [in] */ u8 preambleType;
+
+	/* Specifies if a probe request should be send with the */
+	/* specified SSID when joining to the network. */
+	/* [in] */ u8 probeForJoin;
+
+	/* DTIM Period (In multiples of beacon interval) */
+	/* [in] */ u8 dtimPeriod;
+
+	/* WSM_JOIN_FLAGS_... */
+	/* [in] */ u8 flags;
+
+	/* Length of the SSID */
+	/* [in] */ u32 ssidLength;
+
+	/* Specifies the SSID of the IBSS to join or start */
+	/* [in] */ u8 ssid[32];
+
+	/* Specifies the time between TBTTs in TUs */
+	/* [in] */ u32 beaconInterval;
+
+	/* A bit mask that defines the BSS basic rate set. */
+	/* [in] */ u32 basicRateSet;
+
+	/* Minimum transmission power level in units of 0.1dBm */
+	/* [out] */ int minPowerLevel;
+
+	/* Maximum transmission power level in units of 0.1dBm */
+	/* [out] */ int maxPowerLevel;
+};
+
+int wsm_join(struct cw1200_common *priv, struct wsm_join *arg);
+
+/* 3.25 */
+struct wsm_set_pm {
+	/* WSM_PSM_... */
+	/* [in] */ u8 pmMode;
+
+	/* in unit of 500us; 0 to use default */
+	/* [in] */ u8 fastPsmIdlePeriod;
+
+	/* in unit of 500us; 0 to use default */
+	/* [in] */ u8 apPsmChangePeriod;
+
+	/* in unit of 500us; 0 to disable auto-pspoll */
+	/* [in] */ u8 minAutoPsPollPeriod;
+};
+
+int wsm_set_pm(struct cw1200_common *priv, const struct wsm_set_pm *arg);
+
+/* 3.27 */
+struct wsm_set_pm_complete {
+	u8 psm;			/* WSM_PSM_... */
+};
+
+typedef void (*wsm_set_pm_complete_cb) (struct cw1200_common *priv,
+					struct wsm_set_pm_complete *arg);
+
+/* 3.28 */
+struct wsm_set_bss_params {
+	/* The number of lost consecutive beacons after which */
+	/* the WLAN device should indicate the BSS-Lost event */
+	/* to the WLAN host driver. */
+	u8 beaconLostCount;
+
+	/* The AID received during the association process. */
+	u16 aid;
+
+	/* The operational rate set mask */
+	u32 operationalRateSet;
+};
+
+int wsm_set_bss_params(struct cw1200_common *priv,
+		       const struct wsm_set_bss_params *arg);
+
+/* 3.30 */
+struct wsm_add_key {
+	u8 type;		/* WSM_KEY_TYPE_... */
+	u8 entryIndex;		/* Key entry index: 0 -- WSM_KEY_MAX_INDEX */
+	u16 reserved;
+	union {
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u8 reserved;
+			u8 keyLength;		/* Key length in bytes */
+			u8 keyData[16];		/* Key data */
+		} __packed wepPairwiseKey;
+		struct {
+			u8 keyId;		/* Unique per key identifier
+						 * (0..3) */
+			u8 keyLength;		/* Key length in bytes */
+			u16 reserved;
+			u8 keyData[16];		/* Key data */
+		} __packed wepGroupKey;
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u8 reserved[2];
+			u8 tkipKeyData[16];	/* TKIP key data */
+			u8 rxMicKey[8];		/* Rx MIC key */
+			u8 txMicKey[8];		/* Tx MIC key */
+		} __packed tkipPairwiseKey;
+		struct {
+			u8 tkipKeyData[16];	/* TKIP key data */
+			u8 rxMicKey[8];		/* Rx MIC key */
+			u8 keyId;		/* Key ID */
+			u8 reserved[3];
+			u8 rxSeqCounter[8];	/* Receive Sequence Counter */
+		} __packed tkipGroupKey;
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u16 reserved;
+			u8 aesKeyData[16];	/* AES key data */
+		} __packed aesPairwiseKey;
+		struct {
+			u8 aesKeyData[16];	/* AES key data */
+			u8 keyId;		/* Key ID */
+			u8 reserved[3];
+			u8 rxSeqCounter[8];	/* Receive Sequence Counter */
+		} __packed aesGroupKey;
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u8 keyId;		/* Key ID */
+			u8 reserved;
+			u8 wapiKeyData[16];	/* WAPI key data */
+			u8 micKeyData[16];	/* MIC key data */
+		} __packed wapiPairwiseKey;
+		struct {
+			u8 wapiKeyData[16];	/* WAPI key data */
+			u8 micKeyData[16];	/* MIC key data */
+			u8 keyId;		/* Key ID */
+			u8 reserved[3];
+		} __packed wapiGroupKey;
+	} __packed;
+} __packed;
+
+int wsm_add_key(struct cw1200_common *priv, const struct wsm_add_key *arg);
+
+/* 3.32 */
+struct wsm_remove_key {
+	/* Key entry index : 0-10 */
+	u8 entryIndex;
+};
+
+int wsm_remove_key(struct cw1200_common *priv,
+		   const struct wsm_remove_key *arg);
+
+/* 3.34 */
+struct wsm_set_tx_queue_params {
+	/* 0 best effort/legacy */
+	/* 1 background */
+	/* 2 video */
+	/* 3 voice */
+	u8 queueId;
+
+	/* WSM_ACK_POLICY_... */
+	u8 ackPolicy;
+
+	/* Medium Time of TSPEC (in 32us units) allowed per */
+	/* One Second Averaging Period for this queue. */
+	u16 allowedMediumTime;
+
+	/* dot11MaxTransmitMsduLifetime to be used for the */
+	/* specified queue. */
+	u32 maxTransmitLifetime;
+};
+
+int wsm_set_tx_queue_params(struct cw1200_common *priv,
+			    const struct wsm_set_tx_queue_params *arg);
+
+/* 3.36 */
+struct wsm_edca_queue_params {
+	/* CWmin (in slots) for the access class. */
+	/* [in] */ u16 cwMin;
+
+	/* CWmax (in slots) for the access class. */
+	/* [in] */ u16 cwMax;
+
+	/* AIFS (in slots) for the access class. */
+	/* [in] */ u8 aifns;
+
+	/* TX OP Limit (in microseconds) for the access class. */
+	/* [in] */ u16 txOpLimit;
+
+	/* dot11MaxReceiveLifetime to be used for the specified */
+	/* the access class. Overrides the global */
+	/* dot11MaxReceiveLifetime value */
+	/* [in] */ u32 maxReceiveLifetime;
+};
+
+struct wsm_edca_params {
+	/* NOTE: index is a linux queue id. */
+	struct wsm_edca_queue_params params[4];
+};
+
+#define WSM_EDCA_SET(edca, queue, aifs, cw_min, cw_max, txop)	\
+	do {							\
+		struct wsm_edca_queue_params *p = &(edca)->params[queue]; \
+		p->cwMin = (cw_min);				\
+		p->cwMax = (cw_max);				\
+		p->aifns = (aifs);				\
+		p->txOpLimit = (txop);				\
+	} while (0)
+
+int wsm_set_edca_params(struct cw1200_common *priv,
+			const struct wsm_edca_params *arg);
+
+/* 3.38 */
+/* Set-System info. Skipped for now. Irrelevent. */
+
+/* 3.40 */
+struct wsm_switch_channel {
+	/* 1 - means the STA shall not transmit any further */
+	/* frames until the channel switch has completed */
+	/* [in] */ u8 channelMode;
+
+	/* Number of TBTTs until channel switch occurs. */
+	/* 0 - indicates switch shall occur at any time */
+	/* 1 - occurs immediately before the next TBTT */
+	/* [in] */ u8 channelSwitchCount;
+
+	/* The new channel number to switch to. */
+	/* Note this is defined as per section 2.7. */
+	/* [in] */ u16 newChannelNumber;
+};
+
+int wsm_switch_channel(struct cw1200_common *priv,
+		       const struct wsm_switch_channel *arg);
+
+typedef void (*wsm_channel_switch_cb) (struct cw1200_common *priv);
+
+struct wsm_start {
+	/* WSM_START_MODE_... */
+	/* [in] */ u8 mode;
+
+	/* WSM_PHY_BAND_... */
+	/* [in] */ u8 band;
+
+	/* Channel number */
+	/* [in] */ u16 channelNumber;
+
+	/* Client Traffic window in units of TU */
+	/* Valid only when mode == ..._P2P */
+	/* [in] */ u32 CTWindow;
+
+	/* Interval between two consecutive */
+	/* beacon transmissions in TU. */
+	/* [in] */ u32 beaconInterval;
+
+	/* DTIM period in terms of beacon intervals */
+	/* [in] */ u8 DTIMPeriod;
+
+	/* WSM_JOIN_PREAMBLE_... */
+	/* [in] */ u8 preambleType;
+
+	/* The delay time (in microseconds) period */
+	/* before sending a probe-request. */
+	/* [in] */ u8 probeDelay;
+
+	/* Length of the SSID */
+	/* [in] */ u8 ssidLength;
+
+	/* SSID of the BSS or P2P_GO to be started now. */
+	/* [in] */ u8 ssid[32];
+
+	/* The basic supported rates for the MiniAP. */
+	/* [in] */ u32 basicRateSet;
+};
+
+int wsm_start(struct cw1200_common *priv, const struct wsm_start *arg);
+
+struct wsm_beacon_transmit {
+	/* 1: enable; 0: disable */
+	/* [in] */ u8 enableBeaconing;
+};
+
+int wsm_beacon_transmit(struct cw1200_common *priv,
+			const struct wsm_beacon_transmit *arg);
+
+int wsm_start_find(struct cw1200_common *priv);
+
+int wsm_stop_find(struct cw1200_common *priv);
+
+typedef void (*wsm_find_complete_cb) (struct cw1200_common *priv, u32 status);
+
+struct wsm_suspend_resume {
+	/* See 3.52 */
+	/* Link ID */
+	/* [out] */ int link_id;
+	/* Stop sending further Tx requests down to device for this link */
+	/* [out] */ bool stop;
+	/* Transmit multicast Frames */
+	/* [out] */ bool multicast;
+	/* The AC on which Tx to be suspended /resumed. */
+	/* This is applicable only for U-APSD */
+	/* WSM_QUEUE_... */
+	/* [out] */ int queue;
+};
+
+typedef void (*wsm_suspend_resume_cb) (struct cw1200_common *priv,
+				       struct wsm_suspend_resume *arg);
+
+/* 3.53 Update-IE request: Not implemented: not relevant. */
+
+/* 3.56 */
+struct wsm_map_link {
+	/* MAC address of the remote device */
+	/* [in] */ u8 mac_addr[6];
+	/* [in] */ u8 link_id;
+};
+
+int wsm_map_link(struct cw1200_common *priv, const struct wsm_map_link *arg);
+
+struct wsm_cbc {
+	wsm_scan_complete_cb scan_complete;
+	wsm_tx_confirm_cb tx_confirm;
+	wsm_rx_cb rx;
+	wsm_event_cb event;
+	wsm_set_pm_complete_cb set_pm_complete;
+	wsm_channel_switch_cb channel_switch;
+	wsm_find_complete_cb find_complete;
+	wsm_suspend_resume_cb suspend_resume;
+};
+
+/* ******************************************************************** */
+/* MIB shortcats							*/
+
+static inline int wsm_set_output_power(struct cw1200_common *priv,
+				       int power_level)
+{
+	__le32 val = __cpu_to_le32(power_level);
+	return wsm_write_mib(priv, WSM_MIB_ID_DOT11_CURRENT_TX_POWER_LEVEL,
+			     &val, sizeof(val));
+}
+
+static inline int wsm_set_beacon_wakeup_period(struct cw1200_common *priv,
+					       unsigned dtim_interval,
+					       unsigned listen_interval)
+{
+	struct {
+		u8 numBeaconPeriods;
+		u8 reserved;
+		__le16 listenInterval;
+	} val = {
+	dtim_interval, 0, __cpu_to_le16(listen_interval)};
+	if (dtim_interval > 0xFF || listen_interval > 0xFFFF)
+		return -EINVAL;
+	else
+		return wsm_write_mib(priv, WSM_MIB_ID_BEACON_WAKEUP_PERIOD,
+				     &val, sizeof(val));
+}
+
+struct wsm_rcpi_rssi_threshold {
+	u8 rssiRcpiMode;	/* WSM_RCPI_RSSI_... */
+	u8 lowerThreshold;
+	u8 upperThreshold;
+	u8 rollingAverageCount;
+};
+
+static inline int wsm_set_rcpi_rssi_threshold(struct cw1200_common *priv,
+					struct wsm_rcpi_rssi_threshold *arg)
+{
+	return wsm_write_mib(priv, WSM_MIB_ID_RCPI_RSSI_THRESHOLD, arg,
+			     sizeof(*arg));
+}
+
+static inline int wsm_get_station_id(struct cw1200_common *priv, u8 * mac)
+{
+	return wsm_read_mib(priv, WSM_MIB_ID_DOT11_STATION_ID, mac, ETH_ALEN);
+}
+
+struct wsm_rx_filter {
+	bool promiscuous;
+	bool bssid;
+	bool fcs;
+};
+
+static inline int wsm_set_rx_filter(struct cw1200_common *priv,
+				    const struct wsm_rx_filter *arg)
+{
+	__le32 val = 0;
+	if (arg->promiscuous)
+		val |= __cpu_to_le32(BIT(0));
+	if (arg->bssid)
+		val |= __cpu_to_le32(BIT(1));
+	if (arg->fcs)
+		val |= __cpu_to_le32(BIT(2));
+	return wsm_write_mib(priv, WSM_MIB_ID_RX_FILTER, &val, sizeof(val));
+}
+
+struct wsm_beacon_filter_control {
+	int enabled;
+	int bcn_count;
+};
+
+static inline int wsm_beacon_filter_control(struct cw1200_common *priv,
+					struct wsm_beacon_filter_control *arg)
+{
+	struct {
+		__le32 enabled;
+		__le32 bcn_count;
+	} val;
+	val.enabled = __cpu_to_le32(arg->enabled);
+	val.bcn_count = __cpu_to_le32(arg->bcn_count);
+	return wsm_write_mib(priv, WSM_MIB_ID_BEACON_FILTER_ENABLE, &val,
+			     sizeof(val));
+}
+
+enum wsm_power_mode {
+	wsm_power_mode_active = 0,
+	wsm_power_mode_doze = 1,
+	wsm_power_mode_quiescent = 2,
+};
+
+struct wsm_operational_mode {
+	enum wsm_power_mode power_mode;
+	int disableMoreFlagUsage;
+	int performAntDiversity;
+};
+
+static inline int wsm_set_operational_mode(struct cw1200_common *priv,
+					const struct wsm_operational_mode *arg)
+{
+	u8 val = arg->power_mode;
+	if (arg->disableMoreFlagUsage)
+		val |= BIT(4);
+	if (arg->performAntDiversity)
+		val |= BIT(5);
+	return wsm_write_mib(priv, WSM_MIB_ID_OPERATIONAL_POWER_MODE, &val,
+			     sizeof(val));
+}
+
+struct wsm_template_frame {
+	u8 frame_type;
+	u8 rate;
+	struct sk_buff *skb;
+};
+
+static inline int wsm_set_template_frame(struct cw1200_common *priv,
+					 struct wsm_template_frame *arg)
+{
+	int ret;
+	u8 *p = skb_push(arg->skb, 4);
+	p[0] = arg->frame_type;
+	p[1] = arg->rate;
+	((u16 *) p)[1] = __cpu_to_le32(arg->skb->len - 4);
+	ret = wsm_write_mib(priv, WSM_MIB_ID_TEMPLATE_FRAME, p, arg->skb->len);
+	skb_pull(arg->skb, 4);
+	return ret;
+}
+
+static inline int wsm_set_block_ack_policy(struct cw1200_common *priv,
+					   u8 blockAckTxTidPolicy,
+					   u8 blockAckRxTidPolicy)
+{
+	struct {
+		u8 blockAckTxTidPolicy;
+		u8 reserved1;
+		u8 blockAckRxTidPolicy;
+		u8 reserved2;
+	} val = {
+		.blockAckTxTidPolicy = blockAckTxTidPolicy,
+		.blockAckRxTidPolicy = blockAckRxTidPolicy,
+	};
+	return wsm_write_mib(priv, WSM_MIB_ID_BLOCK_ACK_POLICY, &val,
+			     sizeof(val));
+}
+
+struct wsm_association_mode {
+	u8 flags;		/* WSM_ASSOCIATION_MODE_... */
+	u8 preambleType;	/* WSM_JOIN_PREAMBLE_... */
+	u8 greenfieldMode;	/* 1 for greenfield */
+	u8 mpduStartSpacing;
+	__le32 basicRateSet;
+};
+
+static inline int wsm_set_association_mode(struct cw1200_common *priv,
+					   struct wsm_association_mode *arg)
+{
+	return wsm_write_mib(priv, WSM_MIB_ID_SET_ASSOCIATION_MODE, arg,
+			     sizeof(*arg));
+}
+
+struct wsm_set_tx_rate_retry_policy_header {
+	u8 numTxRatePolicies;
+	u8 reserved[3];
+} __packed;
+
+struct wsm_set_tx_rate_retry_policy_policy {
+	u8 policyIndex;
+	u8 shortRetryCount;
+	u8 longRetryCount;
+	u8 policyFlags;
+	u8 rateRecoveryCount;
+	u8 reserved[3];
+	__le32 rateCountIndices[3];
+} __packed;
+
+struct wsm_set_tx_rate_retry_policy {
+	struct wsm_set_tx_rate_retry_policy_header hdr;
+	struct wsm_set_tx_rate_retry_policy_policy tbl[8];
+} __packed;
+
+static inline int wsm_set_tx_rate_retry_policy(struct cw1200_common *priv,
+				struct wsm_set_tx_rate_retry_policy *arg)
+{
+	size_t size = sizeof(struct wsm_set_tx_rate_retry_policy_header) +
+	    arg->hdr.numTxRatePolicies *
+	    sizeof(struct wsm_set_tx_rate_retry_policy_policy);
+	return wsm_write_mib(priv, WSM_MIB_ID_SET_TX_RATE_RETRY_POLICY, arg,
+			     size);
+}
+
+/* Undocumented MIBs: */
+/* 4.35 P2PDeviceInfo */
+#define D11_MAX_SSID_LEN		(32)
+
+struct wsm_p2p_device_type {
+	__le16 categoryId;
+	u8 oui[4];
+	__le16 subCategoryId;
+} __packed;
+
+struct wsm_p2p_device_info {
+	struct wsm_p2p_device_type primaryDevice;
+	u8 reserved1[3];
+	u8 devNameSize;
+	u8 localDevName[D11_MAX_SSID_LEN];
+	u8 reserved2[3];
+	u8 numSecDevSupported;
+	struct wsm_p2p_device_type secondaryDevices[0];
+} __packed;
+
+/* 4.36 SetWCDMABand - WO */
+struct wsm_cdma_band {
+	u8 WCDMA_Band;
+	u8 reserved[3];
+} __packed;
+
+/* 4.37 GroupTxSequenceCounter - RO */
+struct wsm_group_tx_seq {
+	__le32 bits_47_16;
+	__le16 bits_15_00;
+	__le16 reserved;
+} __packed;
+
+/* 4.39 SetHtProtection - WO */
+#define WSM_DUAL_CTS_PROT_ENB		(1 << 0)
+#define WSM_NON_GREENFIELD_STA		PRESENT(1 << 1)
+#define WSM_HT_PROT_MODE__NO_PROT	(0 << 2)
+#define WSM_HT_PROT_MODE__NON_MEMBER	(1 << 2)
+#define WSM_HT_PROT_MODE__20_MHZ	(2 << 2)
+#define WSM_HT_PROT_MODE__NON_HT_MIXED	(3 << 2)
+#define WSM_LSIG_TXOP_PROT_FULL		(1 << 4)
+#define WSM_LARGE_L_LENGTH_PROT		(1 << 5)
+
+struct wsm_ht_protection {
+	__le32 flags;
+} __packed;
+
+/* 4.40 GPIO Command - R/W */
+#define WSM_GPIO_COMMAND_SETUP	0
+#define WSM_GPIO_COMMAND_READ	1
+#define WSM_GPIO_COMMAND_WRITE	2
+#define WSM_GPIO_COMMAND_RESET	3
+#define WSM_GPIO_ALL_PINS	0xFF
+
+struct wsm_gpio_command {
+	u8 GPIO_Command;
+	u8 pin;
+	__le16 config;
+} __packed;
+
+/* 4.41 TSFCounter - RO */
+struct wsm_tsf_counter {
+	__le64 TSF_Counter;
+} __packed;
+
+/* 4.43 Keep alive period */
+struct wsm_keep_alive_period {
+	__le16 keepAlivePeriod;
+	u8 reserved[2];
+} __packed;
+
+static inline int wsm_keep_alive_period(struct cw1200_common *priv,
+					int period)
+{
+	struct wsm_keep_alive_period arg = {
+		.keepAlivePeriod = period,
+	};
+	return wsm_write_mib(priv, WSM_MIB_ID_KEEP_ALIVE_PERIOD,
+			&arg, sizeof(arg));
+};
+
+/* BSSID filtering */
+struct wsm_set_bssid_filtering {
+	u8 filter;
+	u8 reserved[3];
+} __packed;
+
+static inline int wsm_set_bssid_filtering(struct cw1200_common *priv,
+					  bool enabled)
+{
+	struct wsm_set_bssid_filtering arg = {
+		.filter = !enabled,
+	};
+	return wsm_write_mib(priv, WSM_MIB_ID_DISABLE_BSSID_FILTER,
+			&arg, sizeof(arg));
+}
+
+/* UseMultiTxConfMessage */
+
+static inline int wsm_use_multi_tx_conf(struct cw1200_common *priv,
+					bool enabled)
+{
+	__le32 arg = enabled ? __cpu_to_le32(1) : 0;
+
+	return wsm_write_mib(priv, WSM_MIB_USE_MULTI_TX_CONF,
+			&arg, sizeof(arg));
+}
+
+/* ******************************************************************** */
+/* WSM TX port control							*/
+
+void wsm_lock_tx(struct cw1200_common *priv);
+void wsm_lock_tx_async(struct cw1200_common *priv);
+void wsm_flush_tx(struct cw1200_common *priv);
+void wsm_unlock_tx(struct cw1200_common *priv);
+
+/* ******************************************************************** */
+/* WSM / BH API								*/
+
+int wsm_handle_exception(struct cw1200_common *priv, u8 * data, size_t len);
+int wsm_handle_rx(struct cw1200_common *priv, int id, struct wsm_hdr *wsm,
+		  struct sk_buff **skb_p);
+
+/* ******************************************************************** */
+/* wsm_buf API								*/
+
+struct wsm_buf {
+	u8 *begin;
+	u8 *data;
+	u8 *end;
+};
+
+void wsm_buf_init(struct wsm_buf *buf);
+void wsm_buf_deinit(struct wsm_buf *buf);
+
+/* ******************************************************************** */
+/* wsm_cmd API								*/
+
+struct wsm_cmd {
+	spinlock_t lock;
+	int done;
+	u8 *ptr;
+	size_t len;
+	void *arg;
+	int ret;
+	u16 cmd;
+};
+
+/* ******************************************************************** */
+/* WSM TX buffer access							*/
+
+int wsm_get_tx(struct cw1200_common *priv, u8 ** data, size_t * tx_len);
+void wsm_txed(struct cw1200_common *priv, u8 * data);
+
+/* ******************************************************************** */
+/* Queue mapping: WSM <---> linux					*/
+/* Linux: VO VI BE BK							*/
+/* WSM:   BE BK VI VO							*/
+
+static inline u8 wsm_queue_id_to_linux(u8 queueId)
+{
+	static const u8 queue_mapping[] = {
+		2, 3, 1, 0
+	};
+	return queue_mapping[queueId];
+}
+
+static inline u8 wsm_queue_id_to_wsm(u8 queueId)
+{
+	static const u8 queue_mapping[] = {
+		3, 2, 0, 1
+	};
+	return queue_mapping[queueId];
+}
+
+#endif /* CW1200_HWIO_H_INCLUDED */
diff --git a/drivers/staging/mmio/Kconfig b/drivers/staging/mmio/Kconfig
new file mode 100644
index 00000000000..d6a5a9ad918
--- /dev/null
+++ b/drivers/staging/mmio/Kconfig
@@ -0,0 +1,11 @@
+
+config U8500_MMIO
+	bool "ST-Ericsson MMIO (Camera) Driver"
+	depends on ARCH_U8500
+	help
+	  Enables the ST-Ericsson MMIO (Camera) Driver
+
+config U5500_MMIO
+	bool "ST-Ericsson U5500 MMIO (Camera) Driver"
+	depends on UX500_SOC_DB5500
+
diff --git a/drivers/staging/mmio/Makefile b/drivers/staging/mmio/Makefile
new file mode 100644
index 00000000000..bec2a6efe63
--- /dev/null
+++ b/drivers/staging/mmio/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_U8500_MMIO) := st_mmio.o
diff --git a/drivers/staging/mmio/mmio.h b/drivers/staging/mmio/mmio.h
new file mode 100644
index 00000000000..1c6f68e3556
--- /dev/null
+++ b/drivers/staging/mmio/mmio.h
@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Joakim Axelsson <joakim.axelsson@stericsson.com> for ST-Ericsson
+ * Author: Rajat Verma <rajat.verma@stericsson.com> for ST-Ericsson
+ * License Terms: GNU General Public License v2
+ */
+
+#ifndef MMIO_H
+#define MMIO_H
+
+#include <linux/ioctl.h>
+
+#define MMIO_NAME "mmio_camera"
+#define SRA_SUPPORT 1
+
+#ifdef SRA_SUPPORT
+#define SREG_16_BIT (0x1)
+#define SREG_32_BIT (0x2)
+#endif
+/* Kernel side interface for MMIO */
+/* Which camera is currently active */
+enum camera_slot_t {
+	PRIMARY_CAMERA = 0,
+	SECONDARY_CAMERA,
+	CAMERA_SLOT_END
+};
+struct mmio_gpio {
+	int gpio;	/* Set to zero if not in use */
+	int active_high;/* Set if pin is active high */
+	int udelay;	/* Time to wait when activating the pin, in usec */
+};
+enum mmio_select_i2c_t {
+	MMIO_ACTIVATE_IPI2C2 = 0,
+	MMIO_ACTIVATE_I2C_HOST,
+	MMIO_DEACTIVATE_I2C
+};
+
+enum mmio_select_xshutdown_t {
+	MMIO_ENABLE_XSHUTDOWN_FW = 0,
+	MMIO_ENABLE_XSHUTDOWN_HOST,
+	MMIO_DISABLE_XSHUTDOWN
+};
+struct mmio_platform_data {
+	struct device *dev;
+	enum camera_slot_t camera_slot; /* Which camera is currently used,
+					 * Primary/Secondary */
+	void *extra;			/* Board's private data structure
+					 * placeholder */
+	int reset_ipgpio[CAMERA_SLOT_END]; /* Contains logical IP GPIO for
+					    * reset pin */
+	int sia_base;
+	int cr_base;
+	int (*platform_init)(struct mmio_platform_data *pdata);
+	void (*platform_exit)(struct mmio_platform_data *pdata);
+	int (*power_enable)(struct mmio_platform_data *pdata);
+	void (*power_disable)(struct mmio_platform_data *pdata);
+	int (*config_xshutdown_pins)(struct mmio_platform_data *pdata,
+		enum mmio_select_xshutdown_t select, int is_active_high);
+	int (*config_i2c_pins)(struct mmio_platform_data *pdata,
+		enum mmio_select_i2c_t select);
+	int (*clock_enable)(struct mmio_platform_data *pdata);
+	void (*clock_disable)(struct mmio_platform_data *pdata);
+	void (*set_xshutdown)(struct mmio_platform_data *pdata);
+};
+
+#define USER_SIDE_INTERFACE 1
+/* User side is only allowed to access code in USER_SIDE_INTERFACE block */
+#ifdef USER_SIDE_INTERFACE
+enum mmio_bool_t {
+	MMIO_FALSE = 0,
+	MMIO_TRUE = !MMIO_FALSE,
+	MMIO_BOOL_MAX = 0x7FFFFFFF
+};
+
+struct xshutdown_info_t {
+	int ip_gpio;
+	int camera_function;
+};
+
+struct xp70_fw_t {
+	void __iomem *addr_sdram_ext;
+	void __iomem *addr_esram_ext;
+	void __iomem *addr_split;
+	void __iomem *addr_data;
+	unsigned int size_sdram_ext;
+	unsigned int size_esram_ext;
+	unsigned int size_split;
+	unsigned int size_data;
+};
+
+struct isp_write_t {
+	unsigned long t1_dest;
+	unsigned long *data;
+	unsigned long count;
+};
+
+struct trace_buf_t {
+	void *address;
+	unsigned int size;
+};
+
+#ifdef SRA_SUPPORT
+struct s_reg {
+	unsigned int addr;
+	unsigned int value;
+	unsigned int mask;
+};
+
+struct s_reg_list {
+	unsigned int access_mode;
+	unsigned int entries;
+	struct s_reg  *s_regs_p;
+};
+#endif
+struct mmio_input_output_t {
+	union {
+		enum mmio_bool_t	power_on;
+		struct xp70_fw_t	xp70_fw;
+		struct isp_write_t	isp_write;
+		unsigned int		addr_to_map;
+		struct xshutdown_info_t	xshutdown_info;
+		enum camera_slot_t	camera_slot;
+		struct trace_buf_t	trace_buf;
+#ifdef SRA_SUPPORT
+		struct s_reg_list s_reg_list;
+#endif
+	} mmio_arg;
+};
+
+#define MMIO_TRUE	(1)
+#define MMIO_FALSE	(0)
+#define MMIO_INVALID	(~0)
+
+/*Xshutdown from host takes two arguments*/
+#define MMIO_XSHUTDOWN_ENABLE			(0x1)
+#define MMIO_XSHUTDOWN_ACTIVE_HIGH		(0x2)
+
+#define MMIO_MAGIC_NUMBER 0x15
+
+#define MMIO_CAM_INITBOARD	_IOW(MMIO_MAGIC_NUMBER, 1,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_PWR_SENSOR	_IOW(MMIO_MAGIC_NUMBER, 2,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_SET_EXT_CLK	_IOW(MMIO_MAGIC_NUMBER, 3,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_SET_PRI_HWIF	_IO(MMIO_MAGIC_NUMBER, 4)
+#define MMIO_CAM_SET_SEC_HWIF	_IO(MMIO_MAGIC_NUMBER, 5)
+#define MMIO_CAM_INITMMDSPTIMER	_IO(MMIO_MAGIC_NUMBER, 6)
+#define MMIO_CAM_LOAD_XP70_FW	_IOW(MMIO_MAGIC_NUMBER, 7,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_MAP_STATS_AREA	_IOWR(MMIO_MAGIC_NUMBER, 8,\
+struct mmio_input_output_t*)
+#define MMIO_ACTIVATE_I2C2	_IOW(MMIO_MAGIC_NUMBER, 9, int*)
+#define MMIO_ENABLE_XSHUTDOWN_FROM_HOST _IOW(MMIO_MAGIC_NUMBER, 10, int*)
+#define MMIO_CAM_ISP_WRITE	_IOW(MMIO_MAGIC_NUMBER, 11,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_GET_IP_GPIO	_IOWR(MMIO_MAGIC_NUMBER, 12,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_DESINITBOARD	_IO(MMIO_MAGIC_NUMBER, 13)
+#define MMIO_CAM_SET_TRACE_BUFFER _IOW(MMIO_MAGIC_NUMBER, 14,\
+struct mmio_input_output_t*)
+
+#ifdef SRA_SUPPORT
+#define MMIO_CAM_READ_REGS	_IOWR(MMIO_MAGIC_NUMBER, 15,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_MODIFY_REGS	_IOWR(MMIO_MAGIC_NUMBER, 16,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_WRITE_REGS	_IOWR(MMIO_MAGIC_NUMBER, 17,\
+struct mmio_input_output_t*)
+#endif
+
+#endif /* USER_SIDE_INTERFACE */
+
+#endif
+/* MMIO_H */
diff --git a/drivers/staging/mmio/st_mmio.c b/drivers/staging/mmio/st_mmio.c
new file mode 100644
index 00000000000..c79c2768d19
--- /dev/null
+++ b/drivers/staging/mmio/st_mmio.c
@@ -0,0 +1,1163 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pankaj Chauhan <pankaj.chauhan@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#include <linux/delay.h>
+#include <linux/init.h>		/* Initiliasation support */
+#include <linux/module.h>	/* Module support */
+#include <linux/kernel.h>	/* Kernel support */
+#include <linux/version.h>	/* Kernel version */
+#include <linux/fs.h>		/* File operations (fops) defines */
+#include <linux/errno.h>	/* Defines standard err codes */
+#include <linux/io.h>
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+#include <linux/workqueue.h>
+#include <mach/prcmu.h>
+#include "mmio.h"
+
+#define ISP_REGION_IO				(0xE0000000)
+#define SIA_ISP_REG_ADDR			(0x521E4)
+#define SIA_BASE_ADDR				(0x54000)
+#define SIA_ISP_MEM				(0x56000)
+#define SIA_TIMER_ITC				(0x5BC00)
+#define SIA_ISP_MCU_SYS_SIZE			(0x100000)
+#define SIA_ISP_MEM_PAGE_REG			(0x54070)
+#define SIA_ISP_MCU_SYS_ADDR0_OFFSET	(SIA_BASE_ADDR + 0x40)
+#define SIA_ISP_MCU_SYS_SIZE0_OFFSET	(SIA_BASE_ADDR + 0x42)
+#define SIA_ISP_MCU_SYS_ADDR1_OFFSET	(SIA_ISP_MCU_SYS_ADDR0_OFFSET + 0x04)
+#define SIA_ISP_MCU_SYS_SIZE1_OFFSET	(SIA_ISP_MCU_SYS_SIZE0_OFFSET + 0x04)
+#define SIA_ISP_MCU_IO_ADDR0_HI		(SIA_BASE_ADDR + 0x60)
+
+/* HTimer enable in CR register */
+#define CR_REG0_HTIMEN				(1 << 26)
+#define PICTOR_IN_XP70_L2_MEM_BASE_ADDR		(0x40000)
+#define PICTOR_IN_XP70_TCDM_MEM_BASE_ADDR	(0x60000)
+#define L2_PSRAM_MEM_SIZE			(0x10000)
+
+#define FW_TO_HOST_ADDR_MASK		(0x00001FFF)
+#define FW_TO_HOST_ADDR_SHIFT		(0xD)
+#define FW_TO_HOST_CLR_MASK		(0x3F)
+#define PHY_TO_ISP_MCU_IO_ADDR0_HI(x)	(((x) >> 24) << 8)
+#define XP70_ADDR_MASK			(0x00FFFFFF)
+
+#define CLOCK_ENABLE_DELAY		(0x2)
+
+#define MAX_PRCMU_QOS_APP		(0x64)
+
+#define ISP_WRITE_DATA_SIZE		(0x4)
+
+#define clrbits32(_addr, _clear) \
+	writel(readl(_addr) & ~(u32)(_clear), _addr)
+#define setbits32(_addr, _set) \
+	writel(readl(_addr) | (u32)(_set), _addr)
+
+#define XP70_BLOCK_SIZE		124
+#define XP70_NB_BLOCK		50
+/*
+ * For 30 fps video, there is 33 msec delay between every two frames
+ * MMIO driver reads traces from trace buffer every XP70_TIMEOUT_MSEC.
+ * If traces are not read in time from trace buffer, camera firmware
+ * will start overwiting the traces as size of trace buffer is limited.
+ */
+#define XP70_TIMEOUT_MSEC	30
+#define XP70_DEFAULT_MSG_ID	(0xCDCDCDCD)
+#define XP70_MAX_BLOCK_ID	(0xFFFFFFFF)
+
+#define upper_16_bits(n) ((u16)((u32)(n) >> 16))
+
+struct trace_block {
+	u32 msg_id;
+	char data[XP70_BLOCK_SIZE];
+};
+
+struct mmio_trace {
+	u32 nb_block;
+	u32 block_size;
+	u32 block_id;
+	u32 overwrite_count;
+	struct trace_block block[XP70_NB_BLOCK];
+};
+
+struct trace_buffer_status {
+	u32 prev_overwrite_count;
+	u32 prev_block_id;
+};
+
+struct mmio_info {
+	struct mmio_platform_data *pdata; /* Config from board */
+	struct device *dev; /* My device */
+	/* Runtime variables */
+	struct miscdevice misc_dev;
+	void __iomem *siabase;
+	void __iomem *crbase;
+	/* States */
+	int xshutdown_enabled;
+	int xshutdown_is_active_high;
+	/* tracing */
+	struct trace_buffer_status trace_status;
+	struct mmio_trace *trace_buffer;
+	struct delayed_work trace_work;
+	int trace_allowed;
+};
+
+/*
+ * The one and only private data holder. Default inited to NULL.
+ * Declare it here so no code above can use it directly.
+ */
+static struct mmio_info *info;
+
+/*
+ * This function converts a given logical memory region size
+ * to appropriate ISP_MCU_SYS_SIZEx register value.
+ */
+static int get_mcu_sys_size(u32 size, u32 *val)
+{
+	int ret = 0;
+
+	if (size > 0 && size <= SZ_4K)
+		*val = 4;
+	else if (size > SZ_4K && size <= SZ_8K)
+		*val = 5;
+	else if (size > SZ_8K && size <= SZ_16K)
+		*val = 6;
+	else if (size > SZ_16K && size <= SZ_32K)
+		*val = 7;
+	else if (size > SZ_32K && size <= SZ_64K)
+		*val = 0;
+	else if (size > SZ_64K && size <= SZ_1M)
+		*val = 1;
+	else if (size > SZ_1M  && size <= SZ_16M)
+		*val = 2;
+	else if (size > SZ_16M && size <= SZ_256M)
+		*val = 3;
+	else
+		ret = -EINVAL;
+
+	return ret;
+}
+
+static int mmio_cam_pwr_sensor(struct mmio_info *info, int on)
+{
+	int err = 0;
+
+	if (on) {
+		err = info->pdata->power_enable(info->pdata);
+
+		if (err)
+			dev_err(info->dev,
+				"power_enable failed. err = %d\n", err);
+
+		/*
+		 * When switching from secondary YUV camera
+		 * to primary Raw Bayer Camera, a hang is observed without the
+		 * below delay. I2C access failure are observed while
+		 * communicating with primary camera sensor indicating camera
+		 * sensor was not powered up correctly.
+		 */
+		mdelay(CLOCK_ENABLE_DELAY);
+	} else {
+		info->pdata->power_disable(info->pdata);
+	}
+
+	return err;
+}
+
+static int mmio_cam_control_clocks(struct mmio_info *info,
+				   enum mmio_bool_t power_on)
+{
+	int err = 0;
+
+	if (power_on) {
+		err = info->pdata->clock_enable(info->pdata);
+
+		if (err)
+			dev_err(info->dev,
+				"clock_enable failed, err = %d\n",
+				err);
+	} else {
+		info->pdata->clock_disable(info->pdata);
+	}
+
+	return err;
+}
+
+static int mmio_cam_set_pri_hwif(struct mmio_info *info)
+{
+	if (info->xshutdown_enabled)
+		info->pdata->set_xshutdown(info->pdata);
+
+	return 0;
+}
+
+static int mmio_cam_set_sec_hwif(struct mmio_info *info)
+{
+	if (info->xshutdown_enabled)
+		info->pdata->set_xshutdown(info->pdata);
+
+	return 0;
+}
+
+static int mmio_cam_init_mmdsp_timer(struct mmio_info *info)
+{
+	/* Disabling Accelerators timers */
+	clrbits32(info->crbase, CR_REG0_HTIMEN);
+	/* Write MMDSPTimer */
+	writel(0, info->siabase + SIA_TIMER_ITC);
+	/* Enabling Accelerators timers */
+	setbits32(info->crbase, CR_REG0_HTIMEN);
+	return 0;
+}
+
+static u32 t1_to_arm(u32 t1_addr, void __iomem *smia_base_address,
+		     u16 *p_mem_page)
+{
+	u16 mem_page_update = 0;
+	mem_page_update = (t1_addr >> FW_TO_HOST_ADDR_SHIFT) &
+			  FW_TO_HOST_CLR_MASK;
+
+	if (mem_page_update != *p_mem_page) {
+		/* Update sia_mem_page register */
+		dev_dbg(info->dev, "mem_page_update=0x%x, mem_page=0x%x\n",
+			mem_page_update, *p_mem_page);
+		writew(mem_page_update, smia_base_address +
+		       SIA_ISP_MEM_PAGE_REG);
+		*p_mem_page = mem_page_update;
+	}
+
+	return SIA_ISP_MEM + (t1_addr & FW_TO_HOST_ADDR_MASK);
+}
+
+static int copy_user_buffer(void __iomem **dest_buf,
+			    void __iomem *src_buf, u32 size)
+{
+	int err = 0;
+
+	if (!src_buf)
+		return -EINVAL;
+
+	*dest_buf = kmalloc(size, GFP_KERNEL);
+
+	if (!dest_buf) {
+		err = -ENOMEM;
+		goto nomem;
+	}
+
+	if (copy_from_user(*dest_buf, src_buf, size)) {
+		err = -EFAULT;
+		goto cp_failed;
+	}
+
+	return err;
+cp_failed:
+	kfree(*dest_buf);
+nomem:
+	return err;
+}
+static int mmio_load_xp70_fw(struct mmio_info *info,
+			     struct xp70_fw_t *xp70_fw)
+{
+	u32 i = 0;
+	u32 offset = 0;
+	u32 itval = 0;
+	u16 mem_page = 0;
+	void __iomem *addr_split = NULL;
+	void __iomem *addr_data = NULL;
+	int err = 0;
+
+	if (xp70_fw->size_split != 0) {
+		err = copy_user_buffer(&addr_split, xp70_fw->addr_split,
+				       xp70_fw->size_split);
+
+		if (err)
+			goto err_exit;
+
+		writel(0x0, info->siabase + SIA_ISP_REG_ADDR);
+
+		/* Put the low 64k IRP firmware in ISP MCU L2 PSRAM */
+		for (i = PICTOR_IN_XP70_L2_MEM_BASE_ADDR;
+				i < (PICTOR_IN_XP70_L2_MEM_BASE_ADDR +
+				     L2_PSRAM_MEM_SIZE); i = i + 2) {
+			itval = t1_to_arm(i, info->siabase, &mem_page);
+			itval = ((u32) info->siabase) + itval;
+			/* Copy fw in L2 */
+			writew((*((u16 *) addr_split + offset++)), itval);
+		}
+
+		kfree(addr_split);
+	}
+
+	if (xp70_fw->size_data != 0) {
+		mem_page = 0;
+		offset = 0;
+		err = copy_user_buffer(&addr_data, xp70_fw->addr_data,
+				       xp70_fw->size_data);
+
+		if (err)
+			goto err_exit;
+
+		writel(0x0, info->siabase + SIA_ISP_REG_ADDR);
+
+		for (i = PICTOR_IN_XP70_TCDM_MEM_BASE_ADDR;
+				i < (PICTOR_IN_XP70_TCDM_MEM_BASE_ADDR +
+				     (xp70_fw->size_data)); i = i + 2) {
+			itval = t1_to_arm(i, info->siabase, &mem_page);
+			itval = ((u32) info->siabase) + itval;
+			/* Copy fw data in TCDM */
+			writew((*((u16 *) addr_data + offset++)), itval);
+		}
+
+		kfree(addr_data);
+	}
+
+	if (xp70_fw->size_esram_ext != 0) {
+		/*
+		 * ISP_MCU_SYS_ADDRx XP70 register (@ of ESRAM where the
+		 * external code has been loaded
+		 */
+		writew(upper_16_bits(xp70_fw->addr_esram_ext),
+		       info->siabase + SIA_ISP_MCU_SYS_ADDR0_OFFSET);
+		/* ISP_MCU_SYS_SIZEx XP70 register (size of the code =64KB) */
+		writew(0x0, info->siabase + SIA_ISP_MCU_SYS_SIZE0_OFFSET);
+	}
+
+	if (xp70_fw->size_sdram_ext != 0) {
+		/*
+		 * ISP_MCU_SYS_ADDRx XP70 register (@ of SDRAM where the
+		 * external code has been loaded
+		 */
+		writew(upper_16_bits(xp70_fw->addr_sdram_ext),
+		       info->siabase + SIA_ISP_MCU_SYS_ADDR1_OFFSET);
+		/* ISP_MCU_SYS_SIZEx XP70 register */
+		err = get_mcu_sys_size(xp70_fw->size_sdram_ext, &itval);
+
+		if (err)
+			goto err_exit;
+
+		writew(itval, info->siabase + SIA_ISP_MCU_SYS_SIZE1_OFFSET);
+	}
+
+	return 0;
+err_exit:
+	dev_err(info->dev, "Loading XP70 fw failed\n");
+	return -EFAULT;
+}
+
+static int mmio_map_statistics_mem_area(struct mmio_info *info,
+					void __iomem *addr_to_map)
+{
+	u16 value;
+	BUG_ON(addr_to_map == NULL);
+	/* 16 Mbyte aligned page */
+	value = PHY_TO_ISP_MCU_IO_ADDR0_HI(*((u32 *)addr_to_map));
+	writew(value, info->siabase + SIA_ISP_MCU_IO_ADDR0_HI);
+	/* Return the address in the XP70 address space */
+	*((u32 *)addr_to_map) = (*((u32 *)addr_to_map) & XP70_ADDR_MASK) |
+				ISP_REGION_IO;
+	return 0;
+}
+
+static int mmio_activate_i2c2(struct mmio_info *info, unsigned long enable)
+{
+	int err = 0;
+
+	switch (enable) {
+	case MMIO_ACTIVATE_I2C_HOST:
+		/* Select I2C-2 */
+		err = info->pdata->config_i2c_pins(info->pdata,
+						   MMIO_ACTIVATE_I2C_HOST);
+
+		if (err) {
+			dev_err(info->dev, "Failed to Enable I2C-2, err %d\n",
+				err);
+			goto out;
+		}
+
+		break;
+	case MMIO_ACTIVATE_IPI2C2:
+		/* Select IPI2C */
+		err = info->pdata->config_i2c_pins(info->pdata,
+						   MMIO_ACTIVATE_IPI2C2);
+
+		if (err) {
+			dev_err(info->dev, "Failed to Enable IPI2C, err %d\n",
+				err);
+			goto out;
+		}
+
+		break;
+	case MMIO_DEACTIVATE_I2C: {
+		info->pdata->config_i2c_pins(info->pdata, MMIO_DEACTIVATE_I2C);
+	}
+	break;
+	default:
+		dev_warn(info->dev, "Invalid I2C2 config\n");
+		err = -EINVAL;
+		break;
+	}
+
+out:
+	return err;
+}
+
+static int mmio_enable_xshutdown_from_host(struct mmio_info *info,
+		unsigned long enable)
+{
+	int err = 0;
+	info->xshutdown_is_active_high = enable & MMIO_XSHUTDOWN_ACTIVE_HIGH;
+
+	if (enable & MMIO_XSHUTDOWN_ENABLE) {
+		err = info->pdata->config_xshutdown_pins(info->pdata,
+				MMIO_ENABLE_XSHUTDOWN_HOST, enable &
+				MMIO_XSHUTDOWN_ACTIVE_HIGH);
+	} else {
+		info->pdata->config_xshutdown_pins(info->pdata,
+						MMIO_ENABLE_XSHUTDOWN_FW, -1);
+		/*
+		 * XShutdown is controlled by firmware, initial output value is
+		 * provided by firmware
+		 */
+	}
+
+	info->xshutdown_enabled = enable & MMIO_XSHUTDOWN_ENABLE;
+	return 0;
+}
+
+static int mmio_cam_initboard(struct mmio_info *info)
+{
+	int err = 0;
+	err = prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP, MMIO_NAME,
+					MAX_PRCMU_QOS_APP);
+
+	if (err) {
+		dev_err(info->dev, "Error adding PRCMU QoS requirement %d\n",
+			err);
+		goto out;
+	}
+
+	/* Configure xshutdown to be disabled by default */
+	err = mmio_enable_xshutdown_from_host(info, 0);
+
+	if (err)
+		goto out;
+
+	/* Enable IPI2C */
+	err = mmio_activate_i2c2(info, MMIO_ACTIVATE_IPI2C2);
+out:
+	return err;
+}
+
+static int mmio_cam_desinitboard(struct mmio_info *info)
+{
+	prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP, MMIO_NAME);
+	return 0;
+}
+
+static int mmio_isp_write(struct mmio_info *info,
+			  struct isp_write_t *isp_write_p)
+{
+	int err = 0, i;
+	void __iomem *data = NULL;
+	void __iomem *addr = NULL;
+	u16 mem_page = 0;
+
+	if (!isp_write_p->count) {
+		dev_warn(info->dev, "no data to write to isp\n");
+		return -EINVAL;
+	}
+
+	err = copy_user_buffer(&data, isp_write_p->data,
+			       isp_write_p->count * ISP_WRITE_DATA_SIZE);
+
+	if (err)
+		goto out;
+
+	for (i = 0; i < isp_write_p->count; i++) {
+		addr = (void *)(info->siabase + t1_to_arm(isp_write_p->t1_dest
+				+ ISP_WRITE_DATA_SIZE * i,
+				info->siabase, &mem_page));
+		*((u32 *)addr) = *((u32 *)data + i);
+	}
+
+	kfree(data);
+out:
+	return err;
+}
+
+static int mmio_set_trace_buffer(struct mmio_info *info,
+				 struct trace_buf_t *buf)
+{
+	u32 i;
+	int ret = 0;
+
+	if (info->trace_allowed != 1) {
+		dev_warn(info->dev, "trace disabled in kernel\n");
+		ret = -EPERM;
+		goto out;
+	}
+
+	if (!buf->size || !buf->address
+			|| buf->size < sizeof(struct mmio_trace)) {
+		dev_err(info->dev, "invalid xp70 trace buffer\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (info->trace_buffer) {
+		dev_info(info->dev, "unmap old buffer");
+		iounmap(info->trace_buffer);
+		info->trace_buffer = NULL;
+	}
+
+	info->trace_buffer = ioremap((u32)buf->address, buf->size);
+
+	if (!info->trace_buffer) {
+		dev_err(info->dev, "failed to map trace buffer\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	dev_info(info->dev, "xp70 overwrite_cnt=%d (0x%x) blk_id=%d (0x%x)",
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->block_id, info->trace_buffer->block_id);
+#ifndef CAM_SHARED_MEM_DEBUG
+
+	/* Reset the allocated buffer contents */
+	for (i = 0; i < XP70_NB_BLOCK; i++)
+		info->trace_buffer->block[i].msg_id = XP70_DEFAULT_MSG_ID;
+
+#endif /* CAM_SHARED_MEMORY_DEBUG */
+	dev_info(info->dev, "xp70 overwrite_cnt=%d (0x%x) blk_id=%d (0x%x)\n",
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->block_id, info->trace_buffer->block_id);
+	info->trace_status.prev_overwrite_count = 0;
+	info->trace_status.prev_block_id = 0;
+
+	/* schedule work */
+	if (!schedule_delayed_work(&info->trace_work,
+				   msecs_to_jiffies(XP70_TIMEOUT_MSEC)))
+		dev_err(info->dev, "failed to schedule work\n");
+
+out:
+	return ret;
+}
+
+static long mmio_ioctl(struct file *filp, u32 cmd,
+		      unsigned long arg)
+{
+	struct mmio_input_output_t data;
+	int no_of_bytes;
+	int enable;
+	int ret = 0;
+	struct mmio_info *info = (struct mmio_info *)filp->private_data;
+	BUG_ON(info == NULL);
+
+	switch (cmd) {
+	case MMIO_CAM_INITBOARD:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user(&data, (struct mmio_input_output_t *)arg,
+				   no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		info->pdata->camera_slot = data.mmio_arg.camera_slot;
+		ret = mmio_cam_initboard(info);
+		break;
+	case MMIO_CAM_DESINITBOARD:
+		ret = mmio_cam_desinitboard(info);
+		break;
+	case MMIO_CAM_PWR_SENSOR:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_cam_pwr_sensor(info, data.mmio_arg.power_on);
+		break;
+	case MMIO_CAM_SET_EXT_CLK:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_cam_control_clocks(info, data.mmio_arg.power_on);
+		break;
+	case MMIO_CAM_LOAD_XP70_FW:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_load_xp70_fw(info, &data.mmio_arg.xp70_fw);
+		break;
+	case MMIO_CAM_MAP_STATS_AREA:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_map_statistics_mem_area(info,
+						   &data.mmio_arg.addr_to_map);
+
+		if (0 != ret) {
+			dev_err(info->dev,
+				"Unable to map Statistics Mem area\n");
+			break;
+		}
+
+		if (copy_to_user((struct mmio_input_output_t *)arg,
+				 &data, sizeof(no_of_bytes))) {
+			dev_err(info->dev,
+				"Copy to userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		break;
+	case MMIO_CAM_SET_PRI_HWIF:
+		ret = mmio_cam_set_pri_hwif(info);
+		break;
+	case MMIO_CAM_SET_SEC_HWIF:
+		ret = mmio_cam_set_sec_hwif(info);
+		break;
+	case MMIO_CAM_INITMMDSPTIMER:
+		ret = mmio_cam_init_mmdsp_timer(info);
+		break;
+	case MMIO_CAM_ISP_WRITE:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_isp_write(info, &data.mmio_arg.isp_write);
+		break;
+	case MMIO_ACTIVATE_I2C2:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&enable, (int *)arg, sizeof(enable))) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_activate_i2c2(info, enable);
+		break;
+	case MMIO_ENABLE_XSHUTDOWN_FROM_HOST:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&enable, (int *)arg, sizeof(enable))) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_enable_xshutdown_from_host(info, enable);
+		break;
+	case MMIO_CAM_GET_IP_GPIO:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		data.mmio_arg.xshutdown_info.ip_gpio =
+			info->pdata->reset_ipgpio
+			[data.mmio_arg.xshutdown_info.camera_function];
+
+		if (copy_to_user((struct mmio_input_output_t *)arg,
+				 &data, sizeof(no_of_bytes))) {
+			dev_err(info->dev,
+				"Copy to userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		break;
+	case MMIO_CAM_SET_TRACE_BUFFER:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *) arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_set_trace_buffer(info, &data.mmio_arg.trace_buf);
+		break;
+	default:
+		dev_err(info->dev, "Not an ioctl for this module\n");
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int mmio_release(struct inode *node, struct file *filp)
+{
+	struct mmio_info *info = filp->private_data;
+	BUG_ON(info == NULL);
+	mmio_activate_i2c2(info, MMIO_DEACTIVATE_I2C);
+	info->pdata->config_xshutdown_pins(info->pdata, MMIO_DISABLE_XSHUTDOWN,
+					   -1);
+
+	if (info->trace_buffer) {
+		iounmap(info->trace_buffer);
+		info->trace_buffer = NULL;
+	}
+
+	flush_delayed_work(&info->trace_work);
+	return 0;
+}
+
+static int mmio_open(struct inode *node, struct file *filp)
+{
+	filp->private_data = info;	/* Hook our mmio info */
+	return 0;
+}
+
+static const struct file_operations mmio_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = mmio_ioctl,
+	.open = mmio_open,
+	.release = mmio_release,
+};
+
+
+static ssize_t xp70_data_show(struct device *device,
+			      struct device_attribute *attr, char *buf)
+{
+	int i;
+	int len;
+	int size = 0;
+	int count = 0;
+	int first_index;
+	first_index = info->trace_status.prev_block_id + 1;
+
+	if (!info->trace_buffer || info->trace_buffer->block_id ==
+			XP70_MAX_BLOCK_ID)
+		goto out_unlock;
+
+	if (info->trace_allowed != 1) {
+		dev_warn(info->dev, "xp70 trace disabled in kernel\n");
+		size = sprintf(buf, "xp70 trace disabled in kernel, "
+			       "use sysfs to enable\n");
+		goto out_unlock;
+	}
+
+	count = info->trace_buffer->block_id - info->trace_status.prev_block_id;
+
+	if ((info->trace_buffer->overwrite_count -
+			info->trace_status.prev_overwrite_count) * XP70_NB_BLOCK
+			+ (info->trace_buffer->block_id -
+			   info->trace_status.prev_block_id)
+			>= XP70_NB_BLOCK) {
+		/* overflow case */
+		info->trace_status.prev_block_id =
+			info->trace_buffer->block_id - XP70_NB_BLOCK;
+		first_index = info->trace_buffer->block_id + 1;
+		count = XP70_NB_BLOCK;
+		len = sprintf(buf, "XP70 trace overflow\n");
+		size += len;
+		buf += len;
+	}
+
+	for (i = first_index; count; count--) {
+		int msg_len;
+
+		if (i < 0 || i >= XP70_NB_BLOCK || count > XP70_NB_BLOCK) {
+			dev_err(info->dev, "trace index out-of-bounds\n");
+			goto out_unlock;
+		}
+
+		msg_len = strnlen(info->trace_buffer->block[i].data,
+				  XP70_BLOCK_SIZE);
+
+		if (msg_len > 0) {
+			/* zero terminate full length message */
+			if (msg_len == XP70_BLOCK_SIZE)
+				info->trace_buffer->block[i].data[
+					XP70_BLOCK_SIZE - 1] = '\0';
+
+			len = snprintf(buf, PAGE_SIZE - size, "%d %s\n",
+				       info->trace_buffer->block[i].msg_id,
+				       info->trace_buffer->block[i].data);
+
+			if (len > PAGE_SIZE - size) {
+				dev_err(info->dev, "sysfs buffer overflow\n");
+				size = PAGE_SIZE;
+				goto out_unlock;
+			}
+
+			size += len;
+			buf += len;
+		}
+
+		i = (i + 1) % XP70_NB_BLOCK;
+	}
+
+out_unlock:
+	return size;
+}
+
+static ssize_t xp70_trace_allowed_show(struct device *device,
+				       struct device_attribute *attr,
+				       char *buf)
+{
+	int len;
+	len = sprintf(buf, "%d\n", info->trace_allowed);
+	return len;
+}
+
+static ssize_t xp70_trace_allowed_store(struct device *device,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	if (count <= 0) {
+		dev_err(info->dev, "empty buffer to store\n");
+		return 0;
+	}
+
+	if (buf[0] == '1')
+		info->trace_allowed = 1;
+	else if (buf[0] == '0')
+		info->trace_allowed = 0;
+	else
+		dev_err(info->dev, "illegal trace_allowed val %c\n",
+			buf[0]);
+
+	return count;
+}
+
+static struct device_attribute xp70_device_attrs[] = {
+	__ATTR_RO(xp70_data),
+	__ATTR(trace_allowed, S_IRUGO | S_IWUSR, xp70_trace_allowed_show,
+	xp70_trace_allowed_store),
+	__ATTR_NULL
+};
+
+static void xp70_buffer_wqtask(struct work_struct *data)
+{
+	int i;
+	int first_index = info->trace_status.prev_block_id + 1;
+	int count;
+
+	if (!info->trace_buffer)
+		goto out_err;
+
+	dev_dbg(info->dev, "xp70 overwrite_cnt=%d (0x%x) blk_id=%d (0x%x)",
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->block_id, info->trace_buffer->block_id);
+
+	/* check if trace already started */
+	if (info->trace_buffer->block_id == XP70_MAX_BLOCK_ID ||
+		info->trace_buffer->block_id == XP70_DEFAULT_MSG_ID ||
+		info->trace_buffer->overwrite_count == XP70_DEFAULT_MSG_ID)
+		goto out;
+
+	if ((info->trace_buffer->overwrite_count -
+			info->trace_status.prev_overwrite_count) * XP70_NB_BLOCK
+			+ (info->trace_buffer->block_id -
+			   info->trace_status.prev_block_id)
+			>= XP70_NB_BLOCK) {
+		/* overflow case */
+		info->trace_status.prev_block_id =
+			info->trace_buffer->block_id - XP70_NB_BLOCK;
+		first_index = info->trace_buffer->block_id + 1;
+		count = XP70_NB_BLOCK;
+
+		if (printk_ratelimit())
+			dev_info(info->dev, "XP70 trace overflow\n");
+	} else if (info->trace_buffer->block_id
+			>= info->trace_status.prev_block_id) {
+		count = info->trace_buffer->block_id -
+			info->trace_status.prev_block_id;
+	} else {
+		u32 block_id, prev_block_id, diff;
+		block_id = (u32)(info->trace_buffer->block_id);
+		prev_block_id = (u32)(info->trace_status.prev_block_id);
+		diff = (block_id + XP70_NB_BLOCK) - prev_block_id;
+		count = (u32)diff;
+	}
+
+	for (i = first_index; count; count--) {
+		if (i < 0 || i >= XP70_NB_BLOCK || count > XP70_NB_BLOCK) {
+			if (printk_ratelimit())
+				dev_info(info->dev, "trace index out-of-bounds"
+					 "i=%d count=%d XP70_NB_BLOCK=%d\n",
+					 i, count, XP70_NB_BLOCK);
+
+			break;
+		}
+
+		if (info->trace_buffer->block[i].msg_id !=
+				XP70_DEFAULT_MSG_ID) {
+			int msg_len = strnlen(
+					      info->trace_buffer->block[i].data,
+					      XP70_BLOCK_SIZE);
+
+			/* zero terminate full length message */
+			if (msg_len > 0) {
+				if (msg_len == XP70_BLOCK_SIZE)
+					info->trace_buffer->block[i].data[
+						XP70_BLOCK_SIZE - 1] = '\0';
+
+				dev_info(info->dev, "%d %s\n",
+					 info->trace_buffer->block[i].msg_id,
+					 info->trace_buffer->block[i].data);
+			}
+		}
+
+		i = (i + 1) % XP70_NB_BLOCK;
+	}
+
+	info->trace_status.prev_overwrite_count =
+		info->trace_buffer->overwrite_count;
+	info->trace_status.prev_block_id = info->trace_buffer->block_id;
+out:
+
+	/* Schedule work */
+	if (!schedule_delayed_work(&info->trace_work,
+				   msecs_to_jiffies(XP70_TIMEOUT_MSEC)))
+		dev_info(info->dev, "failed to schedule work\n");
+
+out_err:
+	return;
+}
+
+/**
+* mmio_probe() - Initialize MMIO Camera resources.
+* @pdev: Platform device.
+*
+* Initialize the module and register misc device.
+*
+* Returns:
+*	0 if there is no err.
+*	-ENOMEM if allocation fails.
+*	-EEXIST if device has already been started.
+*	Error codes from misc_register.
+*/
+static int __devinit mmio_probe(struct platform_device *pdev)
+{
+	int err;
+	int i;
+	int ret;
+	printk(KERN_INFO "%s\n", __func__);
+	/* Initialize private data. */
+	info = kzalloc(sizeof(struct mmio_info), GFP_KERNEL);
+
+	if (!info) {
+		dev_err(&pdev->dev, "Could not alloc info struct\n");
+		err = -ENOMEM;
+		goto err_alloc;
+	}
+
+	/* Fill in private data */
+	info->pdata = pdev->dev.platform_data;
+	info->dev = &pdev->dev;
+	info->pdata->dev = &pdev->dev;
+	info->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	info->misc_dev.name = MMIO_NAME;
+	info->misc_dev.fops = &mmio_fops;
+	info->misc_dev.parent = pdev->dev.parent;
+	info->xshutdown_enabled = 0;
+	info->xshutdown_is_active_high = 0;
+	info->trace_allowed = 0;
+	/* Register Misc character device */
+	err = misc_register(&(info->misc_dev));
+
+	if (err) {
+		dev_err(&pdev->dev, "Error %d registering misc dev!", err);
+		goto err_miscreg;
+	}
+
+	/* Memory mapping */
+	info->siabase = ioremap(info->pdata->sia_base, SIA_ISP_MCU_SYS_SIZE);
+
+	if (!info->siabase) {
+		dev_err(info->dev, "Could not ioremap SIA_BASE\n");
+		err = -ENOMEM;
+		goto err_ioremap_sia_base;
+	}
+
+	info->crbase = ioremap(info->pdata->cr_base, PAGE_SIZE);
+
+	if (!info->crbase) {
+		dev_err(info->dev, "Could not ioremap CR_BASE\n");
+		err = -ENOMEM;
+		goto err_ioremap_cr_base;
+	}
+
+	/* Initialize platform specific data */
+	err = info->pdata->platform_init(info->pdata);
+
+	if (err)
+		goto err_platform_init;
+
+	/* create sysfs entries */
+	for (i = 0; attr_name(xp70_device_attrs[i]); i++) {
+		ret = device_create_file(info->misc_dev.this_device,
+					 &xp70_device_attrs[i]);
+
+		if (ret) {
+			dev_err(info->dev, "Error creating SYSFS entry"
+				" %s (%d)\n", xp70_device_attrs[i].attr.name,
+				ret);
+		}
+	}
+
+	INIT_DELAYED_WORK(&info->trace_work, xp70_buffer_wqtask);
+	dev_info(&pdev->dev, "MMIO driver initialized with minor=%d\n",
+		 info->misc_dev.minor);
+	return 0;
+err_platform_init:
+	iounmap(info->crbase);
+err_ioremap_cr_base:
+	iounmap(info->siabase);
+err_ioremap_sia_base:
+	misc_deregister(&info->misc_dev);
+err_miscreg:
+	kfree(info);
+	info = NULL;
+err_alloc:
+	return err;
+}
+
+/**
+* mmio_remove() - Release MMIO Camera resources.
+* @pdev:	Platform device.
+*
+* Remove misc device and free resources.
+*
+* Returns:
+*	0 if success.
+*	Error codes from misc_deregister.
+*/
+static int __devexit mmio_remove(struct platform_device *pdev)
+{
+	int err;
+	int i;
+
+	if (!info)
+		return 0;
+
+	flush_scheduled_work();
+
+	/* sysfs parameters */
+	for (i = 0; attr_name(xp70_device_attrs[i]); i++)
+		device_remove_file(info->misc_dev.this_device,
+				   &xp70_device_attrs[i]);
+
+	err = misc_deregister(&info->misc_dev);
+
+	if (err)
+		dev_err(&pdev->dev, "Error %d deregistering misc dev", err);
+
+	info->pdata->platform_exit(info->pdata);
+	iounmap(info->siabase);
+	iounmap(info->crbase);
+	kfree(info);
+	info = NULL;
+	return 0;
+}
+static struct platform_driver mmio_driver = {
+	.driver = {
+		.name = MMIO_NAME,
+		.owner = THIS_MODULE,
+	},
+	.probe = mmio_probe,
+	.remove = __devexit_p(mmio_remove)
+};
+
+/**
+* mmio_init() - Initialize module.
+*
+* Registers platform driver.
+*/
+static int __init mmio_init(void)
+{
+	printk(KERN_INFO "%s\n", __func__);
+	return platform_driver_register(&mmio_driver);
+}
+
+/**
+* mmio_exit() - Remove module.
+*
+* Unregisters platform driver.
+*/
+static void __exit mmio_exit(void)
+{
+	printk(KERN_INFO "%s\n", __func__);
+	platform_driver_unregister(&mmio_driver);
+}
+
+module_init(mmio_init);
+module_exit(mmio_exit);
+
+MODULE_AUTHOR("Joakim Axelsson ST-Ericsson");
+MODULE_AUTHOR("Pankaj Chauhan ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("MMIO Camera driver");
diff --git a/drivers/staging/nmf-cm/Kconfig b/drivers/staging/nmf-cm/Kconfig
new file mode 100644
index 00000000000..9545fd5acd1
--- /dev/null
+++ b/drivers/staging/nmf-cm/Kconfig
@@ -0,0 +1,12 @@
+
+config U8500_CM
+	tristate "U8500 Component Manager driver"
+	depends on UX500_SOC_DB8500
+	help
+	  This is the Component Manager driver. It is part of the
+	  Nomadik Multiprocessing Framework.
+
+	  Note: This option allows the kernel developers to build
+	  the driver in kernel to ease there life. By default, this driver
+	  must be built outside this kernel source tree.
+
diff --git a/drivers/staging/nmf-cm/Make.config b/drivers/staging/nmf-cm/Make.config
new file mode 100644
index 00000000000..ccbc150158b
--- /dev/null
+++ b/drivers/staging/nmf-cm/Make.config
@@ -0,0 +1,8 @@
+# Copyright (C) ST-Ericsson SA 2011. All rights reserved.
+# This code is ST-Ericsson proprietary and confidential.
+# Any use of the code for whatever purpose is subject to
+# specific written permission of ST-Ericsson SA.
+
+#CM driver file to copy but not to compile
+CMENGINESRC_COPY_NO_BUILD = cm/engine/elf/src/elfxx.c
+
diff --git a/drivers/staging/nmf-cm/Makefile b/drivers/staging/nmf-cm/Makefile
new file mode 100644
index 00000000000..82fa97f81d1
--- /dev/null
+++ b/drivers/staging/nmf-cm/Makefile
@@ -0,0 +1,99 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+# License terms: GNU General Public License (GPL), version 2.
+#
+
+#
+# Rules to build kernel modules
+#
+ifndef MM_MAKEFILES_DIR
+
+  # $(src): current relative dir; $(kbuild-dir): cur absolute dir
+  ifdef kbuild-dir
+    SRCDIR = $(realpath $(kbuild-dir))
+  else
+    SRCDIR = $(realpath $(src))
+  endif
+  include $(SRCDIR)/Make.config
+  ifndef FIXED_CPPFLAGS
+    # In Android env, we can not depend on files that are out of kernel tree.
+    # and thus we can't include $(SRCDIR)/../../../../mmenv/SharedARMFlags.mk
+    # where FIXED_CPPFLAGS is defined.
+    # So, define FIXED_CPPFLAGS here
+    FIXED_CPPFLAGS=-D__STN_8500=30 -DLINUX -D__ARM_LINUX
+  endif
+  EXTRA_CFLAGS := -I$(SRCDIR) $(FIXED_CPPFLAGS)
+  EXTRA_CFLAGS += -Wall -Werror
+  #EXTRA_CFLAGS += -DCM_DEBUG_ALLOC
+
+  #
+  # CM object files to compile with
+  #
+  GENERIC_CM_FILES:=$(shell cd $(SRCDIR); find cm -name "*.c")
+  GENERIC_CM_FILES := $(filter-out $(CMENGINESRC_COPY_NO_BUILD), $(GENERIC_CM_FILES))
+
+  CM_OBJS := $(GENERIC_CM_FILES:.c=.o)
+  CM_OBJS += cmld.o cm_syscall.o osal-kernel.o cm_service.o configuration.o
+  CM_OBJS += cm_dma.o
+
+  obj-$(CONFIG_U8500_CM) := cm.o
+
+  #Note: build system prepends the $(PWD) directory to these objects paths
+  cm-objs := $(CM_OBJS)
+
+else
+
+  # CM module is built in kernel in android env
+  # or as module otherwise (OSI env, ...)
+  export CONFIG_U8500_CM ?= m
+
+  ifeq ($(findstring install,$(MAKECMDGOALS)),)
+    # If not only performing install then include needed files for build
+    include $(MM_MAKEFILES_DIR)/SharedARMFlags.mk
+    export FIXED_CPPFLAGS
+    -include $(MM_MAKEFILES_DIR)/KernelConfig.mk
+
+    ifeq ($(findstring clean,$(MAKECMDGOALS)),)
+      ifndef KERNEL_BUILD_DIR
+        $(error KERNEL_BUILD_DIR not defined)
+      endif
+    endif
+  endif
+
+  include $(MM_MAKEFILES_DIR)/SharedConfig.mk
+
+  module:
+	$(MAKE) ARCH=arm CROSS_COMPILE=$(CROSS_COMPILE) -C $(KERNEL_BUILD_DIR) \
+          M=$(PWD) INSTALL_HEADER_DIR=$(INSTALL_HEADER_DIR) \
+          modules
+
+  all: module
+	$(MAKE) ARCH=arm CROSS_COMPILE=$(CROSS_COMPILE) -C $(KERNEL_BUILD_DIR) \
+          M=$(PWD) INSTALL_HEADER_DIR=$(INSTALL_HEADER_DIR) \
+          INSTALL_MOD_PATH=$(PWD)/lib/$(PLATFORM) \
+          modules_install
+	rm -f $(PWD)/lib/$(PLATFORM)/lib/modules/*/modules.*
+
+  #
+  # Rules to clean and install
+  #
+  clean:
+	@rm -rf $(PLATFORM) $(CM_OBJS) .built-in.o.cmd .cm*o.cmd Module.symvers \
+                .tmp_versions modules.order cm.ko cm.o cm.mod.* lib \
+		$(foreach f,$(CM_OBJS), $(dir $f).$(notdir $f).cmd)
+
+  realclean: clean
+	$(foreach platform, \
+          $(shell grep property ../../component/component.xml | cut -d\" -f 4), \
+          rm -rf $(platform);)
+	@rm -rf *~
+
+  install:
+	$(GEN_LN) -d lib/$(PLATFORM)/lib $(INSTALL_LIB_DIR)/lib
+
+  uninstall:
+	$(GEN_LN) -r -d lib/$(PLATFORM)/lib $(INSTALL_LIB_DIR)/lib
+
+endif #ifdef KERNELRELEASE
+
diff --git a/drivers/staging/nmf-cm/cm/engine/api/channel_engine.h b/drivers/staging/nmf-cm/cm/engine/api/channel_engine.h
new file mode 100644
index 00000000000..19353ee7328
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/channel_engine.h
@@ -0,0 +1,101 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Communication Component Manager internal API type.
+ */
+
+#ifndef CHANNEL_ENGINE_H
+#define CHANNEL_ENGINE_H
+
+#include <nmf/inc/channel_type.h>
+#include <nmf/inc/service_type.h>
+#include <cm/engine/communication/inc/communication_type.h>
+
+/*!
+ * \brief Internal channel identification.
+ *
+ * Same as t_nmf_channel meaning but this the channel used internaly by
+ * OS Integration part
+ *
+ * \ingroup CM_OS_API
+ */
+typedef t_uint32 t_os_channel;
+
+/*!
+ * \brief Invalid value for os_channel
+ *
+ * Invalid value for os channel.
+ *
+ * \ingroup CM_OS_API
+ */
+#define NMF_OS_CHANNEL_INVALID_HANDLE       0xffffffff
+
+/*!
+ * \brief Structure used for storing required parameters for Interface Callback
+ * messages.
+ *
+ * This struture is used internally by CM_GetMessage() and CM_ExecuteMessage() as
+ * the message content in the given buffer.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef struct {
+    t_nmf_mpc2host_handle       THIS;               //!< Context of interface implementation
+    t_uint32                    methodIndex;        //!< Method index in interface
+    char                        params[1];          //!< Is of variable length concretely
+} t_interface_data;
+
+/*!
+ * \brief Structure used for storing required parameters for Service Callback
+ * messages.
+ *
+ * This struture is used internally by CM_GetMessage() and CM_ExecuteMessage() as
+ * the message content in the given buffer.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef struct {
+    t_nmf_service_type type;        //!< Type of the service message
+    t_nmf_service_data data;
+} t_service_data;
+
+typedef enum {
+	MSG_INTERFACE,
+	MSG_SERVICE
+} t_message_type;
+
+/*!
+ * \brief Structure used for storing required parameters for the internal NMF
+ * messages.
+ *
+ * This struture is used internally by CM_GetMessage() and CM_ExecuteMessage() as
+ * the message content in the given buffer.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef struct {
+    t_message_type type;           //!< Type of the nmf message
+    union {
+	t_interface_data    itf;
+	t_service_data      srv;
+    } data;
+} t_os_message;
+
+/*!
+ * \brief Structure used for storing required parameters for the internal NMF
+ * messages.
+ *
+ * This struture is used internally by CM_GetMessage() and CM_ExecuteMessage() as
+ * the message content in the given buffer.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef struct {
+    t_nmf_channel               channel;        //!< Channel (required to handle service message)
+    t_os_message                osMsg;
+} t_nmf_message;
+
+#endif /* CHANNEL_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/cm_engine.h b/drivers/staging/nmf-cm/cm/engine/api/cm_engine.h
new file mode 100644
index 00000000000..0f4c1e4219e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/cm_engine.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief CM Engine API.
+ *
+ * This file contains the Component Manager Engine API.
+ */
+
+/*!
+ * \defgroup CM_ENGINE_MODULE CM Engine
+ */
+/*!
+ * \defgroup CM_ENGINE_API CM Engine API
+ *
+ * \note This API is not for user developers, this API is only an internal API.
+ *
+ * \warning All parameters in out from this API means that the parameter is a reference to a data that is complete by the call.
+ *
+ * This API is provided by CM Engine and shall be required by driver kernel part.
+ * \ingroup CM_ENGINE_MODULE
+ */
+
+#ifndef CM_ENGINE_H_
+#define CM_ENGINE_H_
+
+#include <cm/engine/api/configuration_engine.h>
+
+#include <cm/engine/api/component_engine.h>
+
+#include <cm/engine/api/memory_engine.h>
+
+#include <cm/engine/api/communication_engine.h>
+
+#include <cm/engine/api/perfmeter_engine.h>
+
+#include <cm/engine/api/executive_engine_mgt_engine.h>
+
+#include <cm/engine/api/repository_mgt_engine.h>
+
+#include <cm/engine/api/domain_engine.h>
+
+#include <cm/engine/api/migration_engine.h>
+
+#endif /*CM_ENGINE_H_*/
+
diff --git a/drivers/staging/nmf-cm/cm/engine/api/communication_engine.h b/drivers/staging/nmf-cm/cm/engine/api/communication_engine.h
new file mode 100644
index 00000000000..477a66a4002
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/communication_engine.h
@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Communication User Engine API.
+ *
+ * This file contains the Communication Engine API for manipulating components.
+ *
+ */
+#ifndef COMMUNICATION_ENGINE_H_
+#define COMMUNICATION_ENGINE_H_
+
+#include <cm/engine/communication/inc/communication_type.h>
+
+/*!
+ * \brief Allocate Event buffer where parameters will be marshalled.
+ *
+ * In order to optimize call, this method don't need to be exported to user space,
+ * but must be used by CM driver.
+ *
+ * See \ref HOST2MPC "Host->MPC binding" for seeing an integration example.
+ *
+ * \note This method is not called from user space!!!
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_event_params_handle CM_ENGINE_AllocEvent(t_cm_bf_host2mpc_handle host2mpcId);
+
+/*!
+ * \brief Push a event in Fifo.
+ *
+ * In order to optimize call, this method don't need to be exported to user space,
+ * but must be used by CM driver.
+ *
+ * See \ref HOST2MPC "Host->MPC binding" for seeing an integration example.
+ *
+ * \note This method is not called from user space!!!
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_PushEvent(t_cm_bf_host2mpc_handle host2mpcId, t_event_params_handle h, t_uint32 methodIndex);
+
+/*!
+ * \brief Push a event in Fifo.
+ *
+ * In order to optimize call, this method need to be exported to user space
+ * and must be implemented by CM driver.
+ *
+ * See \ref HOST2MPC "Host->MPC binding" for seeing an integration example.
+ *
+ * \note No implementation of this method is provided in kernel CM engine!!!
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC t_cm_error CM_ENGINE_PushEventWithSize(t_cm_bf_host2mpc_handle host2mpcId, t_event_params_handle h, t_uint32 size, t_uint32 methodIndex);
+
+/*!
+ * \brief Aknowledge a Fifo that the received event has been demarshalled.
+ *
+ * In order to optimize call, this method don't need to be exported to user space,
+ * but must be used by CM driver.
+ *
+ * See \ref MPC2HOST "MPC->Host binding" for seeing an integration example.
+ *
+ * \note This method is not called from user space!!!
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED void CM_ENGINE_AcknowledgeEvent(t_cm_bf_mpc2host_handle mpc2hostId);
+
+#endif /*COMMUNICATION_ENGINE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/api/component_engine.h b/drivers/staging/nmf-cm/cm/engine/api/component_engine.h
new file mode 100644
index 00000000000..cbd61769597
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/component_engine.h
@@ -0,0 +1,403 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Components Component Manager User Engine API.
+ *
+ * This file contains the Component Manager Engine API for manipulating components.
+ *
+ */
+
+#ifndef COMPONENT_ENGINE_H_
+#define COMPONENT_ENGINE_H_
+
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/component/inc/component_type.h>
+#include <cm/engine/communication/inc/communication_type.h>
+#include <inc/nmf-limits.h>
+
+/*!
+ * \brief Instantiate a new component.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_InstantiateComponent(
+    const char templateName[MAX_TEMPLATE_NAME_LENGTH],          //!< [in] Null terminated string (Max size=\ref MAX_TEMPLATE_NAME_LENGTH)
+    t_cm_domain_id domainId,                                    //!< [in] Domain
+    t_nmf_client_id clientId,                                   //!< [in] Client ID (aka PID)
+    t_nmf_ee_priority priority,                                 //!< [in] Component priority
+    const char localName[MAX_COMPONENT_NAME_LENGTH],            //!< [in] Null terminated string (Max size=\ref MAX_COMPONENT_NAME_LENGTH)
+    const char *dataFile,                                       //!< [in] Optional reference on file where component is stored
+    t_cm_instance_handle *component                             //!< [out] component
+    );
+
+/*!
+ * \brief Start a component.
+ *
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_StartComponent(
+    t_cm_instance_handle component,
+    t_nmf_client_id clientId);
+
+/*!
+ * \brief Stop a component.
+ *
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_StopComponent(
+    t_cm_instance_handle component,
+    t_nmf_client_id clientId);
+
+/*!
+ * \brief Destroy a component.
+ *
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_DestroyComponent(
+    t_cm_instance_handle component,
+    t_nmf_client_id clientId);
+
+/*!
+ * \brief Stop and destroy all components belonging to the given client.
+ *
+ * \param[in] client
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_FlushComponents(
+        t_nmf_client_id client);
+
+/*!
+ * \brief Bind two components together.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_BindComponent(
+    const t_cm_instance_handle      client,                                             //!<
+    const char                      requiredItfClientName[MAX_INTERFACE_NAME_LENGTH],   //!< Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+    const t_cm_instance_handle      server,                                             //!<
+    const char                      providedItfServerName[MAX_INTERFACE_NAME_LENGTH],   //!< Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+    t_bool                          traced,                                             //!< FALSE for synchronous binding, TRUE for traced one
+    t_nmf_client_id                 clientId,                                           //!< Client ID
+    const char                      *dataFileTrace                                      //!< Component file data in case on traced (Note: could be null if file already in cache)
+    );
+
+/*!
+ * \brief Unbind a component.
+ *
+ * \param[in] client
+ * \param[in] requiredItfClientName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_UnbindComponent(
+    const t_cm_instance_handle      client,
+    const char                      * requiredItfClientName,
+    t_nmf_client_id                 clientId);
+
+/*!
+ * \brief Bind a component to void (silently ignore a call).
+ *
+ * \param[in] client
+ * \param[in] requiredItfClientName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_BindComponentToVoid(
+    const t_cm_instance_handle      client,
+    const char                      requiredItfClientName[MAX_INTERFACE_NAME_LENGTH],
+    t_nmf_client_id                 clientId);
+
+/*!
+ * \brief Bind two components together in an asynchronous way
+ * (the components can be on the same MPC or on two different MPC)
+ *
+ * \param[in] client
+ * \param[in] requiredItfClientName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[in] server
+ * \param[in] providedItfServerName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[in] fifosize
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_BindComponentAsynchronous(
+    const t_cm_instance_handle      client,
+    const char                      * requiredItfClientName,
+    const t_cm_instance_handle      server,
+    const char                      * providedItfServerName,
+    t_uint32                        fifosize,
+    t_cm_mpc_memory_type            eventMemType,
+    t_nmf_client_id                 clientId,
+    const char                      *dataFileSkeletonOrEvent,
+    const char                      *dataFileStub);
+
+/*!
+ * \brief Unbind a component previously binded asynchronously
+ *
+ * \param[in] client
+ * \param[in] requiredItfClientName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentAsynchronous(
+    const t_cm_instance_handle      client,
+    const char                      * requiredItfClientName,
+    t_nmf_client_id                 clientId);
+
+/*!
+ * \brief Bind the Host to a component.
+ *
+ * \param[in] server
+ * \param[in] providedItfServerName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[in] fifosize
+ * \param[out] host2mpcId
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_BindComponentFromCMCore(
+    const t_cm_instance_handle      server,
+    const char                      * providedItfServerName,
+    t_uint32                        fifosize,
+    t_cm_mpc_memory_type            eventMemType,
+    t_cm_bf_host2mpc_handle         *host2mpcId,
+    t_nmf_client_id                 clientId,
+    const char                      *dataFileSkeleton);
+
+/*!
+ * \brief Unbind a component from the Host.
+ *
+ * \param[in] host2mpcId
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentFromCMCore(
+        t_cm_bf_host2mpc_handle         host2mpcId);
+
+/*!
+ * \brief Bind a component to the Host, see \ref CM_ENGINE_BindComponentToCMCore.
+ *
+ * See \ref MPC2HOST "MPC->Host binding" for seeing an integration example.
+ *
+ * \note This method is not called from CM Proxy, its only there for wrapping purpose!!!
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_BindComponentToCMCore(
+        const t_cm_instance_handle  client,
+        const char                  *requiredItfClientName,
+        t_uint32                    fifosize,
+        t_nmf_mpc2host_handle       upLayerThis,
+        const char                  *dataFileStub,
+        t_cm_bf_mpc2host_handle     *mpc2hostId,
+        t_nmf_client_id             clientId);
+
+/*!
+ * \brief Unbind a component to the Host, see \ref CM_ENGINE_UnbindComponentToCMCore.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentToCMCore(
+        const t_cm_instance_handle      client,
+        const char                      *requiredItfClientName,
+        t_nmf_mpc2host_handle           *upLayerThis,
+        t_nmf_client_id                 clientId);
+
+/*!
+ * \brief Read a value on an attribute exported by a component instance.
+ *
+ * \param[in] component
+ * \param[in] attrName  Null terminated string (Max size=\ref MAX_ATTRIBUTE_NAME_LENGTH).
+ * \param[out] value
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_ReadComponentAttribute(
+    const t_cm_instance_handle component,
+    const char* attrName,
+    t_uint24 *value);
+
+/*!
+ * \brief Get the older component.
+ *
+ * \param[in] client
+ * \param[out] headerComponent
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentListHeader(
+        const t_nmf_client_id       client,
+	t_cm_instance_handle        *headerComponent);
+
+/*!
+ * \brief Get the next component.
+ *
+ * \param[in] client
+ * \param[in] prevComponent
+ * \param[out] nextComponent
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentListNext(
+        const t_nmf_client_id       client,
+	const t_cm_instance_handle  prevComponent,
+	t_cm_instance_handle        *nextComponent);
+
+/*!
+ * \brief Get a component description
+ *
+ * \param[in] component
+ * \param[in] templateNameLength
+ * \param[in] localNameLength
+ * \param[out] templateName         Null terminated string (Size=templateNameLength, Max size=\ref MAX_TEMPLATE_NAME_LENGTH).
+ * \param[out] coreId
+ * \param[out] localName            Null terminated string (Size=localNameLength, Max size=\ref MAX_COMPONENT_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentDescription(
+        const t_cm_instance_handle  component,
+        char                        *templateName,
+        t_uint32                    templateNameLength,
+        t_nmf_core_id               *coreId,
+        char                        *localName,
+        t_uint32                    localNameLength,
+	t_nmf_ee_priority           *priority);
+
+/*!
+ * \brief Get number of interface required by a component.
+ *
+ * \param[in] component
+ * \param[out] numberRequiredInterfaces
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterfaceNumber(
+    const t_cm_instance_handle  component,
+    t_uint8                     *numberRequiredInterfaces);
+
+/*!
+ * \brief Return information about required interface.
+ *
+ * \param[in] component
+ * \param[in] index
+ * \param[in] itfNameLength
+ * \param[in] itfTypeLength
+ * \param[out] itfName          Null terminated string (Size=itfNameLength, Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[out] itfType          Null terminated string (Size=itfTypeLength, Max size=\ref MAX_INTERFACE_TYPE_NAME_LENGTH).
+ * \param[out] collectionSize
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterface(
+        const t_cm_instance_handle  component,
+        const t_uint8               index,
+        char                        *itfName,
+        t_uint32                    itfNameLength,
+        char                        *itfType,
+        t_uint32                    itfTypeLength,
+        t_cm_require_state          *requireState,
+        t_sint16                    *collectionSize);
+
+/*!
+ * \brief Get the component binded to a required interface.
+ *
+ * \param[in] component
+ * \param[in] itfName               Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[in] serverItfNameLength
+ * \param[out] server
+ * \param[out] serverItfName        Null terminated string (Size=serverItfNameLength, Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterfaceBinding(
+        const t_cm_instance_handle  component,
+        const char                  *itfName,
+        t_cm_instance_handle        *server,
+        char                        *serverItfName,
+        t_uint32                    serverItfNameLength);
+
+/*!
+ * \brief Get number of interface provided by a component.
+ *
+ * \param[in] component
+ * \param[out] numberProvidedInterfaces
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentProvidedInterfaceNumber(
+    const t_cm_instance_handle  component,
+    t_uint8                     *numberProvidedInterfaces);
+
+/*!
+ * \brief Return information about provided interface.
+ *
+ * \param[in] component
+ * \param[in] index
+ * \param[in] itfNameLength
+ * \param[in] itfTypeLength
+ * \param[out] itfName          Null terminated string (Size=itfNameLength, Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[out] itfType          Null terminated string (Size=itfTypeLength, Max size=\ref MAX_INTERFACE_TYPE_NAME_LENGTH).
+ * \param[out] collectionSize
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentProvidedInterface(
+        const t_cm_instance_handle  component,
+        const t_uint8               index,
+        char                        *itfName,
+        t_uint32                    itfNameLength,
+        char                        *itfType,
+        t_uint32                    itfTypeLength,
+        t_sint16                    *collectionSize);
+
+/*!
+ * \brief Get number of properties of a component.
+ *
+ * \param[in] component
+ * \param[out] numberProperties
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyNumber(
+    const t_cm_instance_handle  component,
+    t_uint8                     *numberProperties);
+
+/*!
+ * \brief Return the name of a property.
+ *
+ * \param[in] component
+ * \param[in] index
+ * \param[in] propertyNameLength
+ * \param[out] propertyName         Null terminated string (Size=propertyNameLength, Max size=\ref MAX_PROPERTY_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyName(
+        const t_cm_instance_handle  component,
+        const t_uint8               index,
+        char                        *propertyName,
+        t_uint32                    propertyNameLength);
+
+/*!
+ * \brief Get property value of a component.
+ *
+ * \param[in] component
+ * \param[in] propertyName
+ * \param[in] propertyValueLength
+ * \param[out] propertyValue         Null terminated string (Size=propertyValueLength, Max size=\ref MAX_PROPERTY_VALUE_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyValue(
+        const t_cm_instance_handle  component,
+        const char                  *propertyName,
+        char                        *propertyValue,
+        t_uint32                    propertyValueLength);
+
+#endif /*COMPONENT_ENGINE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/api/configuration_engine.h b/drivers/staging/nmf-cm/cm/engine/api/configuration_engine.h
new file mode 100644
index 00000000000..0336f62265e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/configuration_engine.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Configuration Component Manager User Engine API.
+ *
+ * This file contains the Configuration CM Engine API for manipulating CM.
+ *
+ */
+
+#ifndef CONFIGURATION_ENGINE_H
+#define CONFIGURATION_ENGINE_H
+
+#include <cm/engine/configuration/inc/configuration_type.h>
+
+/*!
+ * \brief Dynamically set some debug parameters of the CM
+ *
+ * \param[in] aCmdID The command for the parameter to update
+ * \param[in] aParam The actual value to set for the given command
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_SetMode(t_cm_cmd_id aCmdID, t_sint32 aParam);
+
+#endif /* CONFIGURATION_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/control/configuration_engine.h b/drivers/staging/nmf-cm/cm/engine/api/control/configuration_engine.h
new file mode 100644
index 00000000000..a9543a2af39
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/control/configuration_engine.h
@@ -0,0 +1,193 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Configuration Component Manager User Engine API.
+ *
+ * This file contains the Configuration CM Engine API for manipulating CM.
+ */
+
+#ifndef CONTROL_CONFIGURATION_ENGINE_H
+#define CONTROL_CONFIGURATION_ENGINE_H
+
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/memory/inc/memory_type.h>
+#include <cm/engine/communication/inc/communication_type.h>
+
+/*****************************************************************************************/
+/*  Component Manager dedicated (for Configuration purpose) structured types definition  */
+/*****************************************************************************************/
+
+/*!
+ * \brief Description of the Nomadik HW mapping configuration
+ *
+ * Describe the Nomadik mapping that is to say:
+ *   - the ESRAM memory managed by the CM (The ESRAM address space SHALL BE declared as non cacheable, non bufferable inside host MMU table)
+ *   - the mapping of the System HW Semaphore IP
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef struct {
+    t_nmf_memory_segment  esramDesc;                   //!< Description of the ESRAM memory mapping into Nomadik SOC
+    t_cm_system_address   hwSemaphoresMappingBaseAddr; //!< Description of the System HW Semaphores IP mapping into Nomadik SOC
+} t_nmf_hw_mapping_desc;
+
+/*!
+ * @defgroup t_nmf_nomadik_version t_nmf_nomadik_version
+ * \brief Description of the various supported Nomadik SOC version
+ * @{
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef t_uint8 t_nmf_nomadik_version;                              //!< Fake enumeration type
+#define NOMADIK_8810                ((t_nmf_nomadik_version)0)      //!< STn8810 chip (any cut)
+#define NOMADIK_8815A0              ((t_nmf_nomadik_version)1)      //!< STn8815 chip (cut A0)
+#define NOMADIK_8815                ((t_nmf_nomadik_version)2)      //!< STn8815 chip (other cuts)
+#define NOMADIK_8820                ((t_nmf_nomadik_version)3)      //!< STn8820 chip
+#define NOMADIK_8500                ((t_nmf_nomadik_version)4)      //!< STn8500 chip
+/* @} */
+
+/*!
+ * \brief Description of the configuration parameters of the Component Manager
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef struct {
+    t_nmf_coms_location     comsLocation;      //!< Configure where CM Communications objects are put (see \ref t_nmf_coms_location)
+} t_nmf_config_desc;
+
+/*!
+ * @defgroup t_nmf_power_ctx t_nmf_power_ctx
+ * \brief Definition of the CM-engine context
+ *
+ * OS integrator uses this value to known the context where the associated OSAL routine is called
+ *
+ * @{
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+
+typedef t_uint32 t_nmf_power_ctx;                            //!< Fake enumeration type
+#define PWR_FLUSH_REQ_INTERRUPT_CTX ((t_nmf_power_ctx)0x00)  //!< Interrupt context - called by \ref CM_ProcessMpcEvent
+#define PWR_FLUSH_REQ_NORMAL_CTX    ((t_nmf_power_ctx)0x01)  //!< Normal context (CM user call)
+
+/* @} */
+
+
+/****************************************************************************************************************/
+/*  Component Manager dedicated (for Media Processors Cores Configuration purpose) structured types definition  */
+/****************************************************************************************************************/
+/*!
+ * @defgroup t_nmf_executive_engine_id t_nmf_executive_engine_id
+ * \brief Identification of the Media Processor Executive Engine to deploy
+ * @{
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef t_uint8 t_nmf_executive_engine_id;                                  //!< Fake enumeration type
+#define SYNCHRONOUS_EXECUTIVE_ENGINE        ((t_nmf_executive_engine_id)0)  //!< MPC Synchronous executive engine
+#define HYBRID_EXECUTIVE_ENGINE             ((t_nmf_executive_engine_id)1)  //!< MPC Hybrid synchronous executive engine
+/* @} */
+
+/*!
+ * @defgroup t_nmf_semaphore_type_id t_nmf_semaphore_type_id
+ * \brief Definition of which type semaphore shall be used for the given Media Processor communication mechanism
+ * @{
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef t_uint8 t_nmf_semaphore_type_id;                                    //!< Fake enumeration type
+#define LOCAL_SEMAPHORES                    ((t_nmf_semaphore_type_id)0)    //!< Embedded MMDSP macrocell semaphore, so CM_ProcessMpcEvent(<coreId>) shall be called under ISR connected to local MMDSP IRQ0
+#define SYSTEM_SEMAPHORES                   ((t_nmf_semaphore_type_id)1)    //!< Shared system HW Semaphores, so CM_ProcessMpcEvent(ARM_CORE_ID) shall be called under ISR connected to shared HW Sem Host IRQ
+/* @} */
+
+
+/*!
+ * \brief Opaque type for allocator, returned at CM configuration.
+ */
+typedef t_uint32 t_cfg_allocator_id;
+
+/********************************************************************************/
+/*       Configuration Component Manager API prototypes                         */
+/********************************************************************************/
+
+/*!
+ * \brief Initialisation part
+ *
+ * This routine initialize and configure the Component Manager.
+ *
+ * \param[in] pNmfHwMappingDesc hardware mapping description
+ * \param[in] pNmfConfigDesc NMF (mainly CM) Configuration description
+ *
+ * \exception TBD
+ * \return exception number.
+ *
+ * \warning The ESRAM address space SHALL BE declared as non cacheable, non bufferable inside host MMU table
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC t_cm_error CM_ENGINE_Init(
+        const t_nmf_hw_mapping_desc *pNmfHwMappingDesc,
+        const t_nmf_config_desc *pNmfConfigDesc
+        );
+
+
+/*!
+ * \brief Media Processor core initialisation part
+ *
+ * This routine configures a given Media Processor core
+ *
+ * \param[in] coreId Media Processor identifier
+ * \param[in] executiveEngineId Media Processor Executive Engine identifier
+ * \param[in] semaphoreTypeId Media Processor semaphores (to be used by communication mechanism) identifier
+ * \param[in] nbYramBanks is the number of tcm ram banks to reserved for y memory
+ * \param[in] mediaProcessorMappingBaseAddr Media Processor mapping into host CPU addressable space
+ * \param[in] commDomain Domain for allocating communication FIFOs
+ * \param[in] eeDomain Domain for EE instantiation
+ * \param[in] sdramCodeAllocId Allocator Id for the SDRAM Code segment
+ * \param[in] sdramDataAllocId Allocator Id for the SDRAM Data segment
+ *
+ * \exception TBD
+ * \return exception number.
+ *
+ * \warning The Media Processor mapping address space SHALL BE declared as non cacheable, non bufferable inside host MMU table
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC t_cm_error CM_ENGINE_ConfigureMediaProcessorCore(
+        t_nmf_core_id coreId,
+        t_nmf_executive_engine_id executiveEngineId,
+        t_nmf_semaphore_type_id semaphoreTypeId,
+        t_uint8 nbYramBanks,
+        const t_cm_system_address *mediaProcessorMappingBaseAddr,
+        const t_cm_domain_id eeDomain,
+        const t_cfg_allocator_id sdramCodeAllocId,
+        const t_cfg_allocator_id sdramDataAllocId
+        );
+
+/*!
+ * \brief Configure a memory segment for later
+ *
+ * \exception TBD
+ * \return TBD
+ *
+ * \warning
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC t_cm_error CM_ENGINE_AddMpcSdramSegment(
+        const t_nmf_memory_segment          *pDesc,                     //!< [in]  Memory segment description.
+        t_cfg_allocator_id                  *allocId,                   //!< [out] Identifier of the created allocator.
+        const char                          *memoryname                 //!< [in]  Memory purpose name
+        );
+
+/********************************************************************************/
+/*       Destruction Component Manager API prototypes                         */
+/********************************************************************************/
+/*!
+ * \brief Destruction part
+ *
+ * This routine destroyes and releases all resources used by the Component Manager.
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC void CM_ENGINE_Destroy(void);
+
+
+#endif /* CONTROL_CONFIGURATION_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/control/control_engine.h b/drivers/staging/nmf-cm/cm/engine/api/control/control_engine.h
new file mode 100644
index 00000000000..1d823b27fc1
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/control/control_engine.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief CM Engine API.
+ *
+ * This file contains the Component Manager Engine API.
+ */
+/*!
+ * \defgroup CM_ENGINE_CONTROL_API CM Engine Control API
+ * \note This API is not for OS integrator, it's only for low level system integration.
+ * \ingroup CM_ENGINE_MODULE
+ */
+
+#ifndef CM_CONTROL_H_
+#define CM_CONTROL_H_
+
+#include <cm/engine/api/control/configuration_engine.h>
+
+#include <cm/engine/api/control/irq_engine.h>
+
+#include <cm/engine/api/control/power_engine.h>
+
+#endif /*CM_CONTROL_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/api/control/irq_engine.h b/drivers/staging/nmf-cm/cm/engine/api/control/irq_engine.h
new file mode 100644
index 00000000000..20313f13256
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/control/irq_engine.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief NMF API for interrupt handler.
+ *
+ * This file contains the Component Manager API for interrupt handler.
+ */
+#ifndef CONTROL_IRQ_ENGINE_H
+#define CONTROL_IRQ_ENGINE_H
+
+#include <share/inc/nmf.h>
+#include <cm/inc/cm_type.h>
+#include <nmf/inc/service_type.h>
+
+/*!
+ * \brief MPCs -> HOST communication handler
+ *
+ * This routine shall be integrated as interrupt handler into the OS
+ *
+ * If the given Media Processor Core has been configured (through CM_ConfigureMediaProcessorCore()) as using \ref LOCAL_SEMAPHORES, then
+ * the NMF communication mechanism will use the embedded MMDSP macrocell semaphore,
+ * so CM_ProcessMpcEvent(<\e coreId>) shall be called under ISR connected to local MMDSP IRQ0, with the related \e coreId as parameter.
+ *
+ * If the given Media Processor Core has been configured (through CM_ConfigureMediaProcessorCore()) as using \ref SYSTEM_SEMAPHORES, then
+ * the NMF communication mechanism will use the shared system HW Semaphores,
+ * so CM_ProcessMpcEvent(\ref ARM_CORE_ID) shall be called under ISR connected to shared HW Sem Host IRQ, with \ref ARM_CORE_ID as parameter.
+ *
+ * NB: A Media Processor Core belonging to the distribution pool shall be configured with \ref SYSTEM_SEMAPHORES
+ *
+ * \see t_nmf_semaphore_type_id description
+ *
+ * \param[in] coreId identification of the source of the interrupt
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC IMPORT_SHARED void CM_ProcessMpcEvent(t_nmf_core_id coreId);
+
+/*!
+ * \brief Service type
+ *
+ * \note We used an enumeration in structure since this description remain inside the kernel
+ *  and we assume that everything in the kernel is compile with same compiler and option.
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef enum { // Allowed since i
+        CM_MPC_SERVICE_NONE = 0,                                //!< No service found
+        CM_MPC_SERVICE_PANIC = 1,                               //!< Panic service found
+        CM_MPC_SERVICE_PRINT = 2                                //!< Print service found
+} t_cm_service_type;
+                                          //!< Service description type
+/*!
+ * \brief Service description data
+ *
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef struct {
+    union {
+       t_nmf_panic_data panic;                                                 //!< Panic description
+       struct {
+           t_uint32                     dspAddress;
+           t_uint32                     value1;
+           t_uint32                     value2;
+       } print;                                                 //!< Printf like description
+    } u;                                                    //!< Union of service description
+} t_cm_service_description;
+
+/*!
+ * \brief MPC Panic handler
+ *
+ * This routine shall be called as interrupt handler into the OS.
+ *
+ * So CM_getPanicDescription shall be called under ISR connected to local MMDSP IRQ1, with the related \e coreId as parameter.
+ *
+ * \param[in] coreId                identification of the source of the interrupt
+ * \param[out] srcType              Pointer on service type
+ * \param[out] srcDescr             Pointer on service description
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_getServiceDescription(
+        t_nmf_core_id               coreId,
+        t_cm_service_type           *srcType,
+	t_cm_service_description    *srcDescr);
+
+/*!
+ * \brief Read a null terminated string inside an MPC
+ *
+ * This routine could be used to read the MPC string give as parameter during an CM_NMF_SERVICE_PRINT
+ *
+ * \param[in] coreId                Identification of the code where read string
+ * \param[in] dspAddress            Address of the string in the MPC
+ * \param[out] buffer             Buffer pointer where returning null terminated string
+ * \param[in] bufferSize            Buffer size
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ReadMPCString(
+        t_nmf_core_id               coreId,
+        t_uint32                    dspAddress,
+        char *                      buffer,
+        t_uint32                    bufferSize);
+
+#endif /* CONTROL_IRQ_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/domain_engine.h b/drivers/staging/nmf-cm/cm/engine/api/domain_engine.h
new file mode 100644
index 00000000000..7cc6f33ed90
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/domain_engine.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Public Component Manager Memory User SYSCALL API.
+ *
+ * This file contains the Component Manager SYSCALL API for manipulating domains.
+ *
+ */
+
+#ifndef __INC_DOMAIN_ENGINE_H
+#define __INC_DOMAIN_ENGINE_H
+
+#include <cm/engine/memory/inc/domain_type.h>
+
+/*!
+ * \brief Create a domain.
+ *
+ * Create a memory domain for use in the CM for component instantiation and memory allocation.
+ *
+ * \param[in]  client Id of the client.
+ * \param[in]  domain Description of domain memories.
+ * \param[out] handle Idetifier of the created domain
+ *
+ * \exception CM_INVALID_DOMAIN_DEFINITION
+ * \exception CM_INTERNAL_DOMAIN_OVERFLOW
+ * \exception CM_OK
+ *
+ * \return Error code.
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_CreateMemoryDomain(
+        const t_nmf_client_id     client,
+        const t_cm_domain_memory  *domain,
+        t_cm_domain_id            *handle
+        );
+
+/*!
+ * \brief Create a scratch domain.
+ *
+ * Create a scratch memory domain. Scratch domains
+ * are used to perform overlapping allocations.
+ *
+ * \param[in]  client Id of the client.
+ * \param[in]  parentId Identifier of the parent domain.
+ * \param[in]  domain Description of domain memories.
+ * \param[out] handle Idetifier of the created domain
+ *
+ * \exception CM_INVALID_DOMAIN_DEFINITION
+ * \exception CM_INTERNAL_DOMAIN_OVERFLOW
+ * \exception CM_NO_MORE_MEMORY
+ * \exception CM_OK
+ *
+ * \return Error code.
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_CreateMemoryDomainScratch(
+        const t_nmf_client_id      client,
+        const t_cm_domain_id       parentId,
+        const t_cm_domain_memory  *domain,
+        t_cm_domain_id            *handle
+        );
+
+/*!
+ * \brief Destroy a memory domain.
+
+ * \param[in] handle Domain identifier to destroy.
+ *
+ * \exception CM_INVALID_DOMAIN_HANDLE
+ * \exception CM_OK
+ *
+ * \return Error code.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_DestroyMemoryDomain(
+        t_cm_domain_id handle);
+
+/*!
+ * \brief Destroy all domains belonging to a given client.
+ *
+ * \param[in] client
+ *
+ * \return Error code.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_FlushMemoryDomains(
+        t_nmf_client_id client);
+
+/*!
+ * \brief Retrieve the coreId for a given domain. Utility.
+
+ * \param[in]  domainId   Domain identifier.
+ * \param[out] coreId     Core identifier.
+ *
+ * \exception CM_INVALID_DOMAIN_HANDLE  Invalid domain handle
+ * \exception CM_OK
+ *
+ * \return Error code.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetDomainCoreId(const t_cm_domain_id domainId, t_nmf_core_id *coreId);
+
+#endif /* __INC_DOMAIN_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/executive_engine_mgt_engine.h b/drivers/staging/nmf-cm/cm/engine/api/executive_engine_mgt_engine.h
new file mode 100644
index 00000000000..9cb8bc1481b
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/executive_engine_mgt_engine.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief CM executive engine management Engine API.
+ *
+ * This file contains the Component Manager executive engine management Engine API.
+ */
+#ifndef CM_EXECUTIVE_ENGINE_MANAGEMENT_ENGINE_H_
+#define CM_EXECUTIVE_ENGINE_MANAGEMENT_ENGINE_H_
+
+#include <cm/inc/cm_type.h>
+
+/*!
+ * \brief Return executive engine handle for given core
+ *
+ * \param[in] coreId The core for which we want executive engine handle.
+ * \param[out] executiveEngineHandle executive engine instance (null if the executive engine is not loaded)
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetExecutiveEngineHandle(
+        t_cm_domain_id domainId,
+        t_cm_instance_handle *executiveEngineHandle);
+
+#endif /*CM_EXECUTIVE_ENGINE_MANAGEMENT_ENGINE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/api/memory_engine.h b/drivers/staging/nmf-cm/cm/engine/api/memory_engine.h
new file mode 100644
index 00000000000..49c0c815669
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/memory_engine.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Public Component Manager Memory User Engine API.
+ *
+ * This file contains the Component Manager Engine API for manipulating memory.
+ *
+ */
+
+#ifndef CM_MEMORY_ENGINE_H_
+#define CM_MEMORY_ENGINE_H_
+
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/memory/inc/memory_type.h>
+
+/*!
+ * \brief Allocate memory in a Media Processor Core memory
+ *
+ * \param[in] domainId
+ * \param[in] memType
+ * \param[in] size
+ * \param[in] memAlignment
+ * \param[out] pHandle
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_AllocMpcMemory(
+        t_cm_domain_id domainId,
+        t_nmf_client_id clientId,                                   //!< [in] Client ID (aka PID)
+        t_cm_mpc_memory_type memType,
+        t_cm_size size,
+        t_cm_mpc_memory_alignment memAlignment,
+        t_cm_memory_handle *pHandle
+        );
+
+
+/*!
+ * \brief Free a MPC memory block.
+ *
+ * \param[in] handle
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_FreeMpcMemory(t_cm_memory_handle handle);
+
+/*!
+ * \brief Get the start address of the MPC memory block seen by the host CPU (physical and logical)
+ *
+ * The logical system address returned by this method is valid only in kernel space and the physical
+ * address is accessible only from kernel space too.
+ *
+ * \see OSMem "OS Memory management" for seeing an integration example.
+ *
+ * \param[in] handle
+ * \param[out] pSystemAddress
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemorySystemAddress(
+        t_cm_memory_handle handle,
+        t_cm_system_address *pSystemAddress);
+
+/*!
+ * \brief Get the start address of the memory block seen by the Media Processor Core
+ *
+ * \param[in] handle
+ * \param[out] pMpcAddress
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemoryMpcAddress(
+        t_cm_memory_handle handle,
+        t_uint32 *pMpcAddress);
+
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemorySize(
+	t_cm_memory_handle handle,
+	t_uint32 *pSize);
+
+/*!
+ * \brief Get the memory status for given memory type of a given Media Processor Core
+ *
+ * \param[in] domainId
+ * \param[in] memType
+ * \param[out] pStatus
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemoryStatus(
+        t_cm_domain_id domainId,
+        t_cm_mpc_memory_type memType,
+        t_cm_allocator_status *pStatus);
+
+
+#endif /* CM_MEMORY_ENGINE_H_ */
+
diff --git a/drivers/staging/nmf-cm/cm/engine/api/migration_engine.h b/drivers/staging/nmf-cm/cm/engine/api/migration_engine.h
new file mode 100644
index 00000000000..77a266d4459
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/migration_engine.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#ifndef CM_MIGRATION_ENGINE_H
+#define CM_MIGRATION_ENGINE_H
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/memory/inc/domain_type.h>
+
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_Migrate(const t_cm_domain_id srcShared, const t_cm_domain_id src, const t_cm_domain_id dst);
+
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_Unmigrate(void);
+
+#endif /* CM_MIGRATION_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/perfmeter_engine.h b/drivers/staging/nmf-cm/cm/engine/api/perfmeter_engine.h
new file mode 100644
index 00000000000..bead49dc81e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/perfmeter_engine.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief CM Performance Meter Engine API.
+ *
+ * This file contains the Component Manager Performance Meter Engine API.
+ */
+#ifndef CM_ENGINE_PERFMETER_ENGINE_H_
+#define CM_ENGINE_PERFMETER_ENGINE_H_
+
+#include <cm/engine/perfmeter/inc/perfmeter_type.h>
+
+/*!
+ * \brief MPC cpu load
+ *
+ * \param[in] coreId identification of mpc from which we want cpu load
+ * \param[out] mpcLoadCounter will contain mpc cpu load counters value if success
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_getMpcLoadCounter(
+        t_nmf_core_id coreId,
+        t_cm_mpc_load_counter *mpcLoadCounter);
+
+/*!
+ * \brief MPC cpu load
+ * Same as \ref CM_ENGINE_getMpcLoadCounter() without lock
+ *
+ * \param[in] coreId identification of mpc from which we want cpu load
+ * \param[out] mpcLoadCounter will contain mpc cpu load counters value if success
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_GetMpcLoadCounter(
+        t_nmf_core_id coreId,
+        t_cm_mpc_load_counter *mpcLoadCounter);
+
+#endif /*CM_ENGINE_PERFMETER_ENGINE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/api/repository_mgt_engine.h b/drivers/staging/nmf-cm/cm/engine/api/repository_mgt_engine.h
new file mode 100644
index 00000000000..3520f974be6
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/repository_mgt_engine.h
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Repository Component Manager User Engine API.
+ *
+ * This file contains the Component Manager Engine API for manipulating the components files.
+ */
+
+#ifndef REPOSITORY_MGT_ENGINE_H_
+#define REPOSITORY_MGT_ENGINE_H_
+
+#include <inc/nmf-limits.h>
+#include <cm/engine/repository_mgt/inc/repository_type.h>
+
+/*!
+ * \brief Get the name(s) of the component(s) to load.
+ *
+ * \param[in] client                Handle of the client component (optional)
+ * \param[in] requiredItfClientName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH)  (optional).
+ * \param[in] server                Handle of the server component  (optional)
+ * \param[in] providedItfServerName Null terminated string (Max size==\ref MAX_INTERFACE_NAME_LENGTH) (optional).
+ * \param[out] fileList             List of required component(s).
+ * \param[in,out] listSize          Initial size of the list as input. Updated with the number of entries really used.
+ * \param[out] type                 Interface type of the client required or server provided interface. Null terminated string (Max size=\ref MAX_INTERFACE_TYPE_NAME_LENGTH) (optional) .
+ * \param[out] methodNumber         Number of method in the interface type of the client required interface. (only used when called from CM_BindComponentToUser) (optional)
+ *
+ * \note It returns the component(s) name(s) to load, depending on the first four parameters.
+ *
+ *  - If all 4 are NULL, it returns the name of the Executive Engine components to load
+ *  - If 'client' is NULL, it returns the name of the required components for a Bind From CMCore.
+ *  - If 'server' is NULL, it returns the name of the required components for a Bind To CMCore.
+ *  - If none is NULL, it returns the name of the required components for an asynchronous binding
+ *
+ * The names are returned in fileList, whose initial size is specified in listSize.
+ * (sizeList must be the number of provided entries of \ref MAX_INTERFACE_TYPE_NAME_LENGTH length
+ * If not enough space is provided, CM_NO_MORE_MEMORY is returned
+ *
+ * sizeList is updated with the number entries really filled.
+ *
+ * This method is also used to retrieve the interface type when called from CM_BindComponentToUser and CM_BindComponentFromUser
+ * and the number of methods when called from CM_BindComponentToUser.
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetRequiredComponentFiles(
+        // IN
+        t_action_to_do action,
+        const t_cm_instance_handle client,
+        const char *requiredItfClientName,
+        const t_cm_instance_handle server,
+        const char *providedItfServerName,
+        // OUT component to be pushed
+        char fileList[][MAX_INTERFACE_TYPE_NAME_LENGTH],
+        // IN max component allowed to be pushed
+        t_uint32 listSize,
+        // OUT interface information
+        char type[MAX_INTERFACE_TYPE_NAME_LENGTH],
+        t_uint32 *methodNumber);
+
+/*!
+ * \brief Push a component into the CM Component Cache.
+ *
+ * \param[in] name Component name, null terminated string (Max size=\ref MAX_INTERFACE_TYPE_NAME_LENGTH)
+ * \param[in] data Pointer to _user_ data of the component.
+ * \param[in] size Size of the data.
+ *
+ * \note Push a component in the Component Cache
+ *       The 'data' must be provided such a way that they can be freed by a call to OSAL_Free()
+ *       The caller doesn't need and must NOT free the data, even in case of failure.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_PushComponent(const char *name, const void *data, t_cm_size size);
+
+/*!
+ * \brief Remove a component from the CM Component Cache.
+ *
+ * \param[in] name Component name, null terminated string (Max size=\ref MAX_INTERFACE_TYPE_NAME_LENGTH)
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_ReleaseComponent (const char *name);
+
+#endif /*REPOSITORY_MGT_ENGINE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/communication/fifo/inc/nmf_fifo_arm.h b/drivers/staging/nmf-cm/cm/engine/communication/fifo/inc/nmf_fifo_arm.h
new file mode 100644
index 00000000000..41c4f7a7ad6
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/communication/fifo/inc/nmf_fifo_arm.h
@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#ifndef __INC_NMF_FIFO_ARM
+#define __INC_NMF_FIFO_ARM
+
+#include <cm/inc/cm_type.h>
+#include <share/communication/inc/nmf_fifo_desc.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/component/inc/instance.h>
+#include <share/semaphores/inc/semaphores.h>
+#include <cm/engine/memory/inc/domain.h>
+
+/*
+ * ARM Fifo descriptor (encapsulate the share one)
+ */
+typedef struct
+{
+    t_uint32 magic;
+    t_memory_handle chunkHandle;
+    t_nmf_core_id pusherCoreId;
+    t_nmf_core_id poperCoreId;
+    t_shared_addr dspAdress;
+    t_dsp_address_info dspAddressInfo;
+    t_nmf_fifo_desc *fifoDesc;       //used for all fifo operations and systematically updated by the migrated offset (see cm_AllocEvent)
+    t_nmf_fifo_desc *fifoDescShadow; //shadow desc, is used to restore state after migration and perform the update of the real desc
+
+    // ExtendedField
+    t_memory_handle extendedFieldHandle;
+    t_shared_field  *extendedField;
+} t_nmf_fifo_arm_desc;
+
+PUBLIC t_uint32 fifo_isFifoIdValid(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_nmf_fifo_arm_desc* fifo_alloc(
+        t_nmf_core_id pusherCoreId, t_nmf_core_id poperCoreId,
+        t_uint16 size_in_16bit, t_uint16 nbElem, t_uint16 nbExtendedSharedFields,
+        t_dsp_memory_type_id memType, t_dsp_memory_type_id memExtendedFieldType, t_cm_domain_id domainId);
+PUBLIC void fifo_free(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_uint16 fifo_normalizeDepth(t_uint16 requestedDepth);
+
+PUBLIC t_shared_addr fifo_getAndAckNextElemToWritePointer(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_shared_addr fifo_getAndAckNextElemToReadPointer(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_shared_addr fifo_getNextElemToWritePointer(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_shared_addr fifo_getNextElemToReadPointer(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC void fifo_acknowledgeRead(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC void fifo_acknowledgeWrite(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC void fifo_coms_acknowledgeWriteAndInterruptGeneration(t_nmf_fifo_arm_desc *pArmFifo);
+
+PUBLIC t_cm_error fifo_params_setSharedField(t_nmf_fifo_arm_desc *pArmFifo, t_uint32 sharedFieldIndex, t_shared_field value);
+
+#endif /* __INC_NMF_FIFO_ARM */
diff --git a/drivers/staging/nmf-cm/cm/engine/communication/fifo/src/nmf_fifo_arm.c b/drivers/staging/nmf-cm/cm/engine/communication/fifo/src/nmf_fifo_arm.c
new file mode 100644
index 00000000000..48d7f9e9f03
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/communication/fifo/src/nmf_fifo_arm.c
@@ -0,0 +1,241 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <share/communication/inc/nmf_fifo_desc.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include "../inc/nmf_fifo_arm.h"
+
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/trace/inc/trace.h>
+
+/* define value of fifo magic number */
+#define NMF_FIFO_MAGIC_NB                   0xF1F0BEEF
+
+PRIVATE t_uint16 fifo_getCount(
+    t_uint16 writeIndex,
+    t_uint16 readIndex,
+    t_uint16 fifoSize
+)
+{
+    if (writeIndex >= readIndex) {return writeIndex - readIndex;}
+    else {return fifoSize - readIndex + writeIndex;}
+}
+
+PRIVATE t_uint16 fifo_incrementIndex(
+    t_uint16 index,
+    t_uint16 wrappingValue
+)
+{
+    if (++index == wrappingValue) {index = 0;}
+
+    return index;
+}
+
+PUBLIC t_uint16 fifo_normalizeDepth(t_uint16 requestedDepth)
+{
+    /* with new implementation we don't align on power of two */
+    return requestedDepth;
+}
+
+PUBLIC t_nmf_fifo_arm_desc* fifo_alloc(
+        t_nmf_core_id pusherCoreId, t_nmf_core_id poperCoreId,
+        t_uint16 size_in_16bit, t_uint16 nbElem, t_uint16 nbExtendedSharedFields,
+        t_dsp_memory_type_id memType, t_dsp_memory_type_id memExtendedFieldType, t_cm_domain_id domainId)
+{
+    t_uint16 realNbElem = nbElem + 1;/* we need one more elem in new implementation */
+    t_uint16 sizeToAlloc = sizeof(t_nmf_fifo_desc) + ((size_in_16bit<<1)*realNbElem);
+    t_nmf_fifo_arm_desc *pArmFifoDesc;
+
+    pArmFifoDesc = (t_nmf_fifo_arm_desc*)OSAL_Alloc(sizeof (t_nmf_fifo_arm_desc));
+    if (pArmFifoDesc == NULL)
+        goto errorde;
+
+    pArmFifoDesc->chunkHandle = cm_DM_Alloc(domainId, memType,
+            (sizeToAlloc/2), CM_MM_ALIGN_2WORDS, TRUE); /* size in 16-bit since we use EXT16 memory */
+    if (pArmFifoDesc->chunkHandle == INVALID_MEMORY_HANDLE)
+        goto errorsh;
+
+    pArmFifoDesc->magic = NMF_FIFO_MAGIC_NB;
+    pArmFifoDesc->pusherCoreId = pusherCoreId;
+    pArmFifoDesc->poperCoreId = poperCoreId;
+
+    pArmFifoDesc->fifoDesc = (t_nmf_fifo_desc *)cm_DSP_GetHostLogicalAddress(pArmFifoDesc->chunkHandle);
+    cm_DSP_GetDspAddress(pArmFifoDesc->chunkHandle, &pArmFifoDesc->dspAdress);
+
+    pArmFifoDesc->fifoDescShadow = pArmFifoDesc->fifoDesc;
+    cm_DSP_GetDspDataAddressInfo(cm_DM_GetDomainCoreId(domainId), pArmFifoDesc->dspAdress, &pArmFifoDesc->dspAddressInfo);
+
+    pArmFifoDesc->extendedFieldHandle = INVALID_MEMORY_HANDLE;
+    pArmFifoDesc->extendedField = NULL;
+
+    pArmFifoDesc->fifoDesc->elemSize = size_in_16bit;
+    pArmFifoDesc->fifoDesc->fifoFullValue = nbElem;
+    pArmFifoDesc->fifoDesc->wrappingValue = realNbElem;
+
+    pArmFifoDesc->fifoDesc->semId = cm_SEM_Alloc(pusherCoreId, poperCoreId);
+    pArmFifoDesc->fifoDesc->readIndex = 0;
+    pArmFifoDesc->fifoDesc->writeIndex = 0;
+
+    LOG_INTERNAL(2, "\n##### Fifo alloc 0x%x (0x%x)\n\n", pArmFifoDesc, pArmFifoDesc->fifoDesc, 0, 0, 0, 0);
+
+    if (nbExtendedSharedFields >= 1)
+    {
+        if(poperCoreId == ARM_CORE_ID)
+        {
+            /* Optimization: Don't put extended Field in DSP memory since use only by ARM if popper */
+            pArmFifoDesc->extendedField = (t_shared_field*)OSAL_Alloc(nbExtendedSharedFields * sizeof(t_shared_field));
+            if (pArmFifoDesc->extendedField == NULL)
+                goto errorex;
+
+            pArmFifoDesc->fifoDesc->extendedField = (t_uint32)pArmFifoDesc->extendedField;
+        }
+        else
+        {
+            pArmFifoDesc->extendedFieldHandle = cm_DM_Alloc(domainId, memExtendedFieldType,
+                    nbExtendedSharedFields * sizeof(t_shared_field) / 4, CM_MM_ALIGN_WORD, TRUE);
+            if (pArmFifoDesc->extendedFieldHandle == INVALID_MEMORY_HANDLE)
+                goto errorex;
+
+            pArmFifoDesc->extendedField = (t_shared_field*)cm_DSP_GetHostLogicalAddress(pArmFifoDesc->extendedFieldHandle);
+            cm_DSP_GetDspAddress(pArmFifoDesc->extendedFieldHandle, (t_uint32*)&pArmFifoDesc->fifoDesc->extendedField);
+        }
+
+        pArmFifoDesc->extendedField[EXTENDED_FIELD_BCTHIS_OR_TOP] = (t_shared_field)0;
+    }
+
+    return pArmFifoDesc;
+
+errorex:
+    (void)cm_DM_Free(pArmFifoDesc->chunkHandle, TRUE);
+errorsh:
+    OSAL_Free(pArmFifoDesc);
+errorde:
+    return NULL;
+}
+
+PUBLIC t_uint32 fifo_isFifoIdValid(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    if (((t_uint32)pArmFifo & CM_MM_ALIGN_WORD) != 0) {return FALSE;}
+    if (pArmFifo->magic == NMF_FIFO_MAGIC_NB)  {return TRUE;}
+    else {return FALSE;}
+}
+
+PUBLIC void fifo_free(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    CM_ASSERT(pArmFifo->pusherCoreId != ARM_CORE_ID || pArmFifo->poperCoreId != ARM_CORE_ID);
+
+    pArmFifo->magic = ~NMF_FIFO_MAGIC_NB;
+
+    if(pArmFifo->extendedFieldHandle != INVALID_MEMORY_HANDLE)
+        (void)cm_DM_Free(pArmFifo->extendedFieldHandle, TRUE);
+    else if(pArmFifo->extendedField != NULL)
+        OSAL_Free(pArmFifo->extendedField);
+
+    (void)cm_DM_Free(pArmFifo->chunkHandle, TRUE);
+    OSAL_Free(pArmFifo);
+}
+
+PUBLIC t_shared_addr fifo_getAndAckNextElemToWritePointer(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_shared_addr retValue;
+
+    retValue = fifo_getNextElemToWritePointer(pArmFifo);
+    if (retValue != 0)
+    {
+        fifo_acknowledgeWrite(pArmFifo);
+    }
+
+    return retValue;
+}
+
+PUBLIC t_shared_addr fifo_getAndAckNextElemToReadPointer(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_shared_addr retValue;
+
+    retValue = fifo_getNextElemToReadPointer(pArmFifo);
+    if (retValue != 0)
+    {
+        fifo_acknowledgeRead(pArmFifo);
+    }
+
+    return retValue;
+}
+
+PUBLIC t_shared_addr fifo_getNextElemToWritePointer(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_shared_addr retValue = 0;
+    t_nmf_fifo_desc *pDesc;
+    t_uint16 count;
+
+    if ((NULL == pArmFifo) || (NULL == (pDesc = pArmFifo->fifoDesc)))
+	    return 0;
+
+    count = fifo_getCount(pDesc->writeIndex, pDesc->readIndex,pDesc->wrappingValue);
+    if (count < pDesc->fifoFullValue)
+    {
+        retValue = ((t_shared_addr)pDesc + sizeof(t_nmf_fifo_desc) + (pDesc->writeIndex*(pDesc->elemSize<<1)));
+    }
+
+    return retValue;
+}
+
+PUBLIC t_shared_addr fifo_getNextElemToReadPointer(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_shared_addr retValue = 0;
+    t_nmf_fifo_desc *pDesc;
+    t_uint16 count;
+
+    if ((NULL == pArmFifo) || (NULL == (pDesc = pArmFifo->fifoDesc)))
+	    return 0;
+
+    count = fifo_getCount(pDesc->writeIndex, pDesc->readIndex,pDesc->wrappingValue);
+    if (count != 0)
+    {
+        retValue = ((t_shared_addr)pDesc+ sizeof(t_nmf_fifo_desc) + (pDesc->readIndex*(pDesc->elemSize<<1)));
+    }
+
+    return retValue;
+}
+
+PUBLIC void fifo_acknowledgeRead(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_nmf_fifo_desc *pDesc = pArmFifo->fifoDesc;
+
+    pDesc->readIndex = fifo_incrementIndex(pDesc->readIndex, pDesc->wrappingValue);
+}
+
+PUBLIC void fifo_acknowledgeWrite(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_nmf_fifo_desc *pDesc = pArmFifo->fifoDesc;
+
+    pDesc->writeIndex = fifo_incrementIndex(pDesc->writeIndex, pDesc->wrappingValue);
+}
+
+PUBLIC void fifo_coms_acknowledgeWriteAndInterruptGeneration(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_nmf_fifo_desc *pDesc = pArmFifo->fifoDesc;
+
+    fifo_acknowledgeWrite(pArmFifo);
+    //Be sure before generate irq that fifo has been updated
+    OSAL_mb();
+    cm_SEM_GenerateIrq[pArmFifo->poperCoreId](pArmFifo->poperCoreId, pDesc->semId);
+    //cm_SEM_Take[pArmFifo->poperCoreId](pArmFifo->poperCoreId, pDesc->semId);
+    //cm_SEM_GiveWithInterruptGeneration[pArmFifo->poperCoreId](pArmFifo->poperCoreId, pDesc->semId);
+}
+
+PUBLIC t_cm_error fifo_params_setSharedField(t_nmf_fifo_arm_desc *pArmFifo, t_uint32 sharedFieldIndex, t_shared_field value)
+{
+    pArmFifo->extendedField[sharedFieldIndex] = value;
+
+    return CM_OK;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/communication/inc/communication.h b/drivers/staging/nmf-cm/cm/engine/communication/inc/communication.h
new file mode 100644
index 00000000000..53ab87b7096
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/communication/inc/communication.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Components Management internal methods - Communication part.
+ *
+ */
+#ifndef __INC_NMF_COM
+#define __INC_NMF_COM
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/communication/fifo/inc/nmf_fifo_arm.h>
+#include <cm/engine/memory/inc/memory.h>
+
+#include <cm/engine/communication/inc/communication_type.h>
+
+extern t_dsp_memory_type_id comsLocation;
+extern t_dsp_memory_type_id paramsLocation;
+extern t_dsp_memory_type_id extendedFieldLocation;
+
+PUBLIC t_cm_error cm_COM_Init(t_nmf_coms_location comsLocation);
+PUBLIC t_cm_error cm_COM_AllocateMpc(t_nmf_core_id coreId);
+PUBLIC void cm_COM_InitMpc(t_nmf_core_id coreId);
+PUBLIC void cm_COM_FreeMpc(t_nmf_core_id coreId);
+
+PUBLIC t_cm_error cm_PushEventTrace(t_nmf_fifo_arm_desc*, t_event_params_handle h, t_uint32 methodIndex, t_uint32 isTrace);
+PUBLIC t_cm_error cm_PushEvent(t_nmf_fifo_arm_desc *pArmFifo, t_event_params_handle h, t_uint32 methodIndex);
+PUBLIC void cm_AcknowledgeEvent(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_event_params_handle cm_AllocEvent(t_nmf_fifo_arm_desc *pArmFifo);
+
+/*!
+ * \internal
+ * \brief Definition of custom value for userTHIS parameter of PostDfc OSAL call
+ *
+ * This value is used as 1st parameter of a pPostDfc call to indicate that a given interrupt is linked to an internal Component Manager event
+ */
+#define NMF_INTERNAL_USERTHIS ((void*)MASK_ALL32)
+
+typedef void (*t_callback_method)(t_nmf_core_id coreId, t_event_params_handle pParam);
+
+#endif /* __INC_NMF_COM */
diff --git a/drivers/staging/nmf-cm/cm/engine/communication/inc/communication_type.h b/drivers/staging/nmf-cm/cm/engine/communication/inc/communication_type.h
new file mode 100644
index 00000000000..db426845a82
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/communication/inc/communication_type.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Communication Component Manager API type.
+ */
+#ifndef COMMUNICATION_TYPE_H_
+#define COMMUNICATION_TYPE_H_
+
+#include <cm/inc/cm_type.h>
+
+
+/*!
+ * \brief Buffer type used for (un)marshalling parameters.
+ *
+ * This buffer type is used for (un)marshalling paramaters. It can either be a
+ * shared memory buffer (ESRAM or SDRAM) or a pure host software memory (stack).
+
+ * \ingroup CM_ENGINE_API
+ */
+typedef t_uint16 *t_event_params_handle;
+
+/*!
+ * \brief Component manager handle to Host -> MPC communication.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef t_uint32 t_cm_bf_host2mpc_handle;
+
+/*!
+ * \brief Component manager handle to MPC -> Host communication.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef t_uint32 t_cm_bf_mpc2host_handle;
+
+/*!
+ * \brief Component manager proxy handle to MPC -> Host skeleton context.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef t_uint32 t_nmf_mpc2host_handle;
+
+/*!
+ * @defgroup t_nmf_coms_location t_nmf_coms_location
+ * \brief Definition of the location of the internal CM communication objects
+ *
+ * @{
+ * \ingroup CM_ENGINE_API
+ */
+typedef t_uint8 t_nmf_coms_location;                                //!< Fake enumeration type
+#define COMS_IN_ESRAM               ((t_nmf_coms_location)0)        //!< All coms objects (coms and params fifos) will be in embedded RAM
+#define COMS_IN_SDRAM               ((t_nmf_coms_location)1)        //!< All coms objects (coms and params fifos) will be in external RAM
+/* @} */
+
+#endif /*COMMUNICATION_TYPE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/communication/src/communication.c b/drivers/staging/nmf-cm/cm/engine/communication/src/communication.c
new file mode 100644
index 00000000000..8f48f02afc2
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/communication/src/communication.c
@@ -0,0 +1,326 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#include <cm/inc/cm_type.h>
+#include "../inc/communication.h"
+#include <share/communication/inc/communication_fifo.h>
+#include <cm/engine/api/control/irq_engine.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/communication/fifo/inc/nmf_fifo_arm.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/memory/inc/migration.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/trace/inc/xtitrace.h>
+
+#include <cm/engine/component/inc/initializer.h>
+
+#define ARM_DSP_EVENT_FIFO_SIZE 16
+
+t_dsp_memory_type_id comsLocation;
+t_dsp_memory_type_id paramsLocation;
+t_dsp_memory_type_id extendedFieldLocation;
+
+#define __DEBUG
+
+#ifdef __DEBUG
+PRIVATE volatile t_uint32 armdspCounter = 0;
+PRIVATE volatile t_uint32 armdspIrqCounter = 0;
+PRIVATE volatile t_uint32 dsparmCounter = 0;
+PRIVATE volatile t_uint32 dsparmIrqCounter = 0;
+#endif /* __DEBUG */
+
+t_nmf_fifo_arm_desc* mpc2mpcComsFifoId[NB_CORE_IDS][NB_CORE_IDS];
+
+PRIVATE const t_callback_method internalHostJumptable[] = {
+    processAsyncAcknowledge,
+    processAsyncAcknowledge,
+    processAsyncAcknowledge,
+    processSyncAcknowledge,
+    processAsyncAcknowledge,
+    processAsyncAcknowledge,
+    processAsyncAcknowledge,
+    processSyncAcknowledge,
+    processAsyncAcknowledge,
+    processSyncAcknowledge,
+    processSyncAcknowledge
+};
+
+PUBLIC t_cm_error cm_COM_Init(t_nmf_coms_location _comsLocation)
+{
+    t_nmf_core_id coreId, localCoreId;
+
+    /*
+     * Configure the default location of coms and params fifo (configuration by user) */
+    switch(_comsLocation)
+    {
+        case COMS_IN_SDRAM:
+            comsLocation = SDRAM_EXT16;
+            paramsLocation = SDRAM_EXT16;
+            extendedFieldLocation = SDRAM_EXT24;
+            break;
+        case COMS_IN_ESRAM:
+            comsLocation = ESRAM_EXT16;
+            paramsLocation = ESRAM_EXT16;
+            extendedFieldLocation = ESRAM_EXT24;
+            break;
+        default: CM_ASSERT(0);
+    }
+
+    for (coreId =  ARM_CORE_ID; coreId < NB_CORE_IDS; coreId++)
+    {
+        for (localCoreId = ARM_CORE_ID; localCoreId < NB_CORE_IDS; localCoreId++)
+        {
+            mpc2mpcComsFifoId[coreId][localCoreId] = NULL;
+        }
+    }
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_COM_AllocateMpc(t_nmf_core_id coreId)
+{
+    t_nmf_core_id localCoreId;
+
+    /*
+     * Allocation of the coms fifo with neighbor MPCs
+     * if they are already initialized (known through initializedCoresMask)
+     */
+    for (localCoreId = ARM_CORE_ID; localCoreId < NB_CORE_IDS; localCoreId++)
+    {
+        if (localCoreId == coreId) continue; /* no coms fifo with itself ;) */
+        if(cm_DSP_GetState(localCoreId)->state != MPC_STATE_BOOTED) continue;
+
+        /*
+         * coms fifo from other initialized MPCs to the given one
+         */
+        if (mpc2mpcComsFifoId[coreId][localCoreId] != NULL) continue; /* coms fifo already allocated */
+
+        mpc2mpcComsFifoId[coreId][localCoreId] = fifo_alloc(
+                coreId, localCoreId,
+                EVENT_ELEM_SIZE_IN_BYTE/2, ARM_DSP_EVENT_FIFO_SIZE,
+                0, comsLocation, extendedFieldLocation, cm_DSP_GetState(coreId)->domainEE
+                );
+        if (mpc2mpcComsFifoId[coreId][localCoreId] == NULL)
+            goto oom;
+
+        /*
+         * coms fifo from the given MPC to the other initialized ones
+         */
+        if (mpc2mpcComsFifoId[localCoreId][coreId] != NULL) continue; /* coms fifo already allocated */
+
+        mpc2mpcComsFifoId[localCoreId][coreId] = fifo_alloc(
+                localCoreId, coreId,
+                EVENT_ELEM_SIZE_IN_BYTE/2, ARM_DSP_EVENT_FIFO_SIZE,
+                0, comsLocation, extendedFieldLocation, cm_DSP_GetState(coreId)->domainEE
+                );
+        if (mpc2mpcComsFifoId[localCoreId][coreId] == NULL)
+            goto oom;
+    }
+
+    return CM_OK;
+oom:
+    cm_COM_FreeMpc(coreId);
+    return CM_NO_MORE_MEMORY;
+}
+
+PUBLIC void cm_COM_InitMpc(t_nmf_core_id coreId)
+{
+    // Here we assume that attribute are in XRAM, thus we don't need memory type
+    t_uint32* toNeighborsComsFifoIdSharedVar[NB_CORE_IDS];
+    t_uint32* fromNeighborsComsFifoIdSharedVar[NB_CORE_IDS];
+
+    t_nmf_core_id localCoreId;
+
+    /*
+     * Initialization of the core identifier of a given Executive Engine
+     * Used into communication scheme so the init is done here, will be moved MAY BE into EE loading module!!!
+     */
+    cm_writeAttribute(cm_EEM_getExecutiveEngine(coreId)->instance, "semaphores/myCoreId", coreId);
+
+    /*
+     * Initialization of the coms fifo with the Host for the given coreId
+     */
+    for (localCoreId = FIRST_MPC_ID/* NOT ARM*/; localCoreId <= LAST_CORE_ID; localCoreId++)
+    {
+        // Note: This loop will also include coreId in order to fill
+        if(cm_DSP_GetState(localCoreId)->state != MPC_STATE_BOOTED) continue;/* no coms fifo initialisation with not booted MPC */
+
+        toNeighborsComsFifoIdSharedVar[localCoreId] = (t_uint32*)cm_getAttributeHostAddr(cm_EEM_getExecutiveEngine(localCoreId)->instance, "comms/toNeighborsComsFifoId");
+
+        fromNeighborsComsFifoIdSharedVar[localCoreId] = (t_uint32*)cm_getAttributeHostAddr(cm_EEM_getExecutiveEngine(localCoreId)->instance, "comms/fromNeighborsComsFifoId");
+    }
+
+    toNeighborsComsFifoIdSharedVar[coreId][ARM_CORE_ID] = mpc2mpcComsFifoId[coreId][ARM_CORE_ID]->dspAdress;
+    fromNeighborsComsFifoIdSharedVar[coreId][ARM_CORE_ID] = mpc2mpcComsFifoId[ARM_CORE_ID][coreId]->dspAdress;
+
+    for (localCoreId = FIRST_MPC_ID/* NOT ARM*/; localCoreId <= LAST_CORE_ID; localCoreId++)
+    {
+        if (localCoreId == coreId) continue; /* no coms fifo with itself ;) */
+        if(cm_DSP_GetState(localCoreId)->state != MPC_STATE_BOOTED) continue;/* no coms fifo initialisation with not booted MPC */
+
+        toNeighborsComsFifoIdSharedVar[coreId][localCoreId] = mpc2mpcComsFifoId[coreId][localCoreId]->dspAdress;
+        fromNeighborsComsFifoIdSharedVar[localCoreId][coreId] = mpc2mpcComsFifoId[coreId][localCoreId]->dspAdress;
+
+        fromNeighborsComsFifoIdSharedVar[coreId][localCoreId] = mpc2mpcComsFifoId[localCoreId][coreId]->dspAdress;
+        toNeighborsComsFifoIdSharedVar[localCoreId][coreId] = mpc2mpcComsFifoId[localCoreId][coreId]->dspAdress;
+    }
+}
+
+PUBLIC void cm_COM_FreeMpc(t_nmf_core_id coreId)
+{
+    t_nmf_core_id localCoreId;
+
+    for (localCoreId = ARM_CORE_ID; localCoreId < NB_CORE_IDS; localCoreId++)
+    {
+        /*
+         * Free coms fifo from other initialized MPCs to the given one
+         */
+        if ( mpc2mpcComsFifoId[coreId][localCoreId] != NULL)
+        {
+            fifo_free(mpc2mpcComsFifoId[coreId][localCoreId]);
+            mpc2mpcComsFifoId[coreId][localCoreId] = NULL;
+        }
+
+        /*
+         * Free coms fifo from the given MPC to the other initialized ones
+         */
+        if ( mpc2mpcComsFifoId[localCoreId][coreId] != NULL)
+        {
+            fifo_free(mpc2mpcComsFifoId[localCoreId][coreId]);
+            mpc2mpcComsFifoId[localCoreId][coreId] = NULL;
+        }
+    }
+}
+
+PUBLIC t_event_params_handle cm_AllocEvent(t_nmf_fifo_arm_desc *pArmFifo)
+
+{
+    t_uint32 retValue;
+
+    //migration impacts the ARM-side address of the fifoDesc,
+    //thus translate the fifo desc adress systematically.
+    pArmFifo->fifoDesc = (t_nmf_fifo_desc*)cm_migration_translate(pArmFifo->dspAddressInfo.segmentType, (t_shared_addr)pArmFifo->fifoDescShadow);
+
+    retValue = fifo_getAndAckNextElemToWritePointer(pArmFifo);
+
+    return (t_event_params_handle)retValue;
+}
+
+PUBLIC void cm_AcknowledgeEvent(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    fifo_acknowledgeRead(pArmFifo);
+}
+
+PUBLIC t_cm_error cm_PushEventTrace(t_nmf_fifo_arm_desc *pArmFifo, t_event_params_handle h, t_uint32 methodIndex, t_uint32 isTrace)
+{
+    t_uint32 retValue;
+
+    retValue = fifo_getNextElemToWritePointer(mpc2mpcComsFifoId[ARM_CORE_ID][pArmFifo->poperCoreId]);
+
+    if(retValue != 0x0) {
+        t_shared_field *pEvent = (t_shared_field *)retValue;
+
+#ifdef __DEBUG
+        armdspCounter++;
+#endif /* __DEBUG */
+
+        pEvent[EVENT_ELEM_METHOD_IDX]   = (t_shared_addr)methodIndex;
+        pEvent[EVENT_ELEM_PARAM_IDX]    = pArmFifo->dspAdress + (((t_cm_logical_address)h - (t_cm_logical_address)pArmFifo->fifoDesc) >> 1); //note byte to half-word conversion
+        pEvent[EVENT_ELEM_EXTFIELD_IDX] = pArmFifo->fifoDesc->extendedField;
+
+        if (isTrace)
+        {
+            cm_TRC_traceCommunication(
+                    TRACE_COMMUNICATION_COMMAND_SEND,
+                    ARM_CORE_ID,
+                    pArmFifo->poperCoreId);
+        }
+        fifo_coms_acknowledgeWriteAndInterruptGeneration(mpc2mpcComsFifoId[ARM_CORE_ID][pArmFifo->poperCoreId]);
+
+        return CM_OK;
+    }
+
+    ERROR("CM_MPC_NOT_RESPONDING: FIFO COM full '%s'\n", 0, 0, 0, 0, 0, 0);
+    return CM_MPC_NOT_RESPONDING;
+}
+
+PUBLIC t_cm_error cm_PushEvent(t_nmf_fifo_arm_desc *pArmFifo, t_event_params_handle h, t_uint32 methodIndex)
+{
+    return cm_PushEventTrace(pArmFifo,h,methodIndex,1);
+}
+
+static void cmProcessMPCFifo(t_nmf_core_id coreId)
+{
+    t_shared_field *pEvent;
+
+    while((pEvent = (t_shared_field *)fifo_getNextElemToReadPointer(mpc2mpcComsFifoId[coreId][ARM_CORE_ID])) != NULL)
+    {
+        t_event_params_handle pParamsAddr;
+        t_shared_field *pParamsFifoESFDesc;
+
+        pParamsAddr = (t_event_params_handle)cm_DSP_ConvertDspAddressToHostLogicalAddress(
+                coreId,
+                pEvent[EVENT_ELEM_PARAM_IDX]);
+        pParamsFifoESFDesc = (t_shared_field *)pEvent[EVENT_ELEM_EXTFIELD_IDX];
+#ifdef __DEBUG
+        dsparmCounter++;
+#endif /* __DEBUG */
+
+        if(pParamsFifoESFDesc[EXTENDED_FIELD_BCTHIS_OR_TOP] == (t_shared_field)NMF_INTERNAL_USERTHIS)
+        {
+            internalHostJumptable[pEvent[EVENT_ELEM_METHOD_IDX]](coreId, pParamsAddr);
+        }
+        else
+        {
+            cm_TRC_traceCommunication(
+                    TRACE_COMMUNICATION_COMMAND_RECEIVE,
+                    ARM_CORE_ID,
+                    coreId);
+
+            OSAL_PostDfc(
+                    pParamsFifoESFDesc[EXTENDED_FIELD_BCTHIS_OR_TOP],
+                    pEvent[EVENT_ELEM_METHOD_IDX],
+                    pParamsAddr,
+                    pParamsFifoESFDesc[EXTENDED_FIELD_BCDESC]);
+        }
+
+        // [Pwr] mpc2hostComsFifoId value is checked to support the case where
+        //       CM_PostCleanUpAndFlush method is called under interrupt context
+        //       -> mpc2hostComsFifoId can be released.
+        if (mpc2mpcComsFifoId[coreId][ARM_CORE_ID] != NULL)
+            fifo_acknowledgeRead(mpc2mpcComsFifoId[coreId][ARM_CORE_ID]);
+        else
+            break;
+    }
+}
+
+PUBLIC EXPORT_SHARED void CM_ProcessMpcEvent(t_nmf_core_id coreId)
+{
+#ifdef __DEBUG
+    dsparmIrqCounter++;
+#endif /* __DEBUG */
+
+    if (coreId != ARM_CORE_ID)
+    {
+        /* Acknowledge DSP communication interrupt */
+        cm_DSP_AcknowledgeDspIrq(coreId, DSP2ARM_IRQ_0);
+
+        cmProcessMPCFifo(coreId);
+    }
+    else
+    {
+        while((coreId = cm_HSEM_GetCoreIdFromIrqSrc()) <= LAST_MPC_ID)
+            cmProcessMPCFifo(coreId);
+    }
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/bind.h b/drivers/staging/nmf-cm/cm/engine/component/inc/bind.h
new file mode 100644
index 00000000000..325703e3367
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/bind.h
@@ -0,0 +1,443 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ * \brief Binding Factories internal API.
+ *
+ * \defgroup BF_COMMON Binding factories: Common API
+ * \defgroup BF_PRIMITIVE Binding Factories: Primitive API
+ * \defgroup BF_TRACE Binding Factories: Trace API
+ * \defgroup BF_ASYNCHRONOUS Binding Factories: Asynchronous API
+ * \defgroup BF_DISTRIBUTED Binding Factories: Distributed API
+ */
+#ifndef __INC_CM_BIND_H
+#define __INC_CM_BIND_H
+
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/communication/inc/communication.h>
+#include <cm/engine/utils/inc/table.h>
+
+/**
+ * \internal
+ * \ingroup BF_COMMON
+ *
+ * \brief Identification number of prefedined Binding Factories
+ */
+typedef enum {
+    BF_SYNCHRONOUS,     //!< Intra-DSP Synchronous Binding Factory Identifier
+    BF_TRACE,           //!< Intra-DSP trace synchronous Binding Factory Identifier
+    BF_ASYNCHRONOUS, 	//!< Intra-DSP Asynchronous Binding Factory Identifier
+    BF_DSP2HOST,        //!< DSP to Host Binding Factory Identifier
+    BF_HOST2DSP,        //!< Host to DSP Binding Factory Identifier
+    BF_DSP2DSP,         //!< DSP to DSP Binding Factory Identifier
+} t_bf_info_ID;
+
+/*!
+ * \internal
+ * \brief Description of a provided interface
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct _t_interface_reference {
+	const t_component_instance          *instance;      //!< Component instance that provide this interface
+    t_uint8                             provideIndex;   //!< Index of the interface in the provide array
+    t_uint8                             collectionIndex;//!< Index in the collection if provided interface is a collection
+    t_bf_info_ID                        bfInfoID;       //!< Identification of BF used for creating binding
+    void*                               bfInfo;         //!< Storage of the binding factory info
+} t_interface_reference;
+
+/**
+ * \internal
+ * \ingroup BF_COMMON
+ *
+ * Make some basic sanity check for a client:
+ *  - component stopped
+ *  - Interface really required
+ *
+ * \param[in] client The client component instance handle.
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[out] requiredItf return the required interface (avoid user searching)
+ */
+t_cm_error cm_checkValidClient(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        t_interface_require_description *itfRequire,
+        t_bool *bindable);
+/**
+ * \internal
+ * \ingroup BF_COMMON
+ *
+ * Make some basic sanity check for a server:
+ *  - Interface really provided
+ *
+ * \param[in] server The server component instance handle.
+ * \param[in] providedItfServerName The server provided interface name
+ * \param[out] itf return the provided interface (avoid user searching)
+ */
+t_cm_error cm_checkValidServer(
+        const t_component_instance* server,
+        const char* providedItfServerName,
+        t_interface_provide_description *itfProvide);
+
+/**
+ * \internal
+ * \ingroup BF_COMMON
+ *
+ * Make some basic sanity check for a binding:
+ *  - Sanity check for a server
+ *  - Sanity check for a client (and potentially wait initialisation)
+ *  - Provided and required interface matches
+ *
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[in] server The server component instance handle
+ * \param[in] providedItfServerName The server provided interface name
+ * \param[out] requiredItf return the required interface (avoid user searching)
+ * \param[out] itf return the provided interface (avoid user searching)
+ */
+t_cm_error cm_checkValidBinding(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        const t_component_instance* server,
+        const char* requiredItfServerName,
+        t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide,
+        t_bool *bindable);
+
+/**
+ * \internal
+ * \ingroup BF_COMMON
+ *
+ * Make some basic sanity check for each unbinding:
+ *  - Interface really required
+ *  - Component stopped
+ *
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[out] itfRequire return the previously binded required interface (avoid user searching)
+ * \param[out] itfProvide return the previously binded provided interface (avoid user searching)
+ * \param[out] bfInfoID return the binding factory identifiant which done the previously bind
+ * \param[out] bfInfo return the binding factory information which done the previously bind
+ */
+t_cm_error cm_checkValidUnbinding(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide);
+
+/**
+ * \internal
+ * \ingroup BF_PRIMITIVE
+ *
+ * Create a primitive binding between a client to a server interface.
+ *
+ * \param[in] itfRequire The client required interface description
+ * \param[in] itfProvide The server provided interface description
+ */
+t_cm_error cm_bindInterface(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide);
+
+/**
+ * \internal
+ * \ingroup BF_PRIMITIVE
+ *
+ * Unbind a previously binded client.
+ *
+ * \param[in] itfRequire The client required interafce description
+ */
+void cm_unbindInterface(
+        const t_interface_require_description *itfRequire);
+
+/**
+ * \internal
+ * \ingroup BF_PRIMITIVE
+ *
+ * Get a server interface previouly binded to a client
+ *
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[out] itf The server interface
+ */
+t_cm_error cm_lookupInterface(
+        const t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide);
+
+/**
+ * \internal
+ * \ingroup BF_PRIMITIVE
+ *
+ * Create a void binding.
+ *
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ */
+t_cm_error cm_bindInterfaceToVoid(
+        const t_interface_require_description *itfRequire);
+
+/**
+ * \internal
+ * \ingroup BF_TRACE
+ *
+ * Trace synchronous binding factory Information
+ */
+typedef struct {
+    t_component_instance    *traceInstance;   //!< Trace binding component instance
+} t_trace_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_TRACE
+ *
+ * Create a traced binding between a client to a server interface.
+ *
+ * \param[in] itfRequire The client required interface description
+ * \param[in] itfProvide The server provided interface description
+ */
+t_cm_error cm_bindInterfaceTrace(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_elfdescription *elfhandleTrace);
+
+/**
+ * \internal
+ * \ingroup BF_TRACE
+ *
+ * Unbind a previously binded client.
+ *
+ * \param[in] itfRequire The client required interafce description
+ */
+void cm_unbindInterfaceTrace(
+        const t_interface_require_description *itfRequire,
+        t_trace_bf_info                       *bfInfo);
+
+
+/**
+ * \internal
+ * \ingroup BF_ASYNCHRONOUS
+ *
+ * Asynchronous binding factory Information
+ */
+typedef struct {
+    t_component_instance 	*eventInstance;   //!< Event binding component instance
+    t_memory_handle 		dspfifoHandle;    //!< Memory handle of allocated event fifo (pass to the event binding component)
+} t_async_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_ASYNCHRONOUS
+ *
+ * Create a asynchronous binding between a client to a server interface.
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[in] itf The server interface
+ * \param[in] fifosize Number of waited event in the fifo
+ */
+t_cm_error cm_bindInterfaceAsynchronous(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription *elfhandleEvent);
+/**
+ * \internal
+ * \ingroup BF_ASYNCHRONOUS
+ *
+ * Destroy a asynchronous binding between a client to a server interface.
+ * \param[in] itfRequire the required interface
+ */
+void cm_unbindInterfaceAsynchronous(
+        const t_interface_require_description   *itfRequire,
+        t_async_bf_info                         *bfInfo);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Stub information in distributed binding factory (client side)
+ */
+typedef struct {
+    t_component_instance    *stubInstance;      //!< Stub
+} t_dspstub_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Skeleton information in distributed binding factory (server side)
+ */
+typedef struct {
+    t_component_instance    *skelInstance;      //!< Skeleton binding component instance
+    t_memory_handle         dspfifoHandle;      //!< Memory handle of allocated event fifo (pass to the event binding component)
+} t_dspskel_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Host to DSP distributed binding factory Information
+ */
+typedef struct {
+    t_dspskel_bf_info       dspskeleton;            //!< Information about the DSP skeleton (server side)
+    t_nmf_fifo_arm_desc*    fifo;                   //!< Handle of the fifo params
+    t_nmf_client_id         clientId;               //!< Client ID of the host client
+} t_host2mpc_bf_info;
+
+/*
+ * Table of instantiated of host2mpc bindings
+ */
+extern t_nmf_table Host2MpcBindingTable; /**< list (table) of host2mpc bindings */
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Create a Host to DSP distributed binding between a host client interface to a server interface.
+ * (Not manage in the same way as distributed binding since the Host programming model is not component aware).
+ * \param[in] itfServer The server interface
+ * \param[in] fifosize Number of waited event in the fifo
+ * \param[in] dspEventMemType The type of memory to use
+ * \param[in] bfInfo info structure
+ */
+t_cm_error cm_bindComponentFromCMCore(
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription *elfhandleSkeleton,
+        t_host2mpc_bf_info **bfInfo);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Destroy a Host to DSP distributed binding between a host client interface to a server interface.
+ * \param[in] bfInfo The Host to DSP distributed binding factory information
+ */
+void cm_unbindComponentFromCMCore(
+        t_host2mpc_bf_info *bfInfo);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * DSP to Host distributed binding factory Information
+ */
+typedef struct {
+    t_dspstub_bf_info           dspstub;            //!< Information about the DSP stub (client side)
+    t_nmf_fifo_arm_desc*        fifo;               //!< Handle of the fifo params
+    t_uint32                    context;
+} t_mpc2host_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Create a DSP to Host distributed binding between a client interface to a host server interface.
+ * (Not manage in the same way as distributed binding since the Host programming model is not component aware).
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[in] itfref The host server interface to be called
+ * \param[in] fifosize Number of waited event in the fifo
+ */
+t_cm_error cm_bindComponentToCMCore(
+        const t_interface_require_description   *itfRequire,
+        t_uint32                                fifosize,
+        t_uint32                                context,
+        t_elfdescription                        *elfhandleStub,
+        t_mpc2host_bf_info                      ** bfInfo);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Destroy a DSP to Host distributed binding between a client interface to a server interface.
+ * \param[in] itfRequire The required interface
+ * \param[out] upLayerThis The 'THIS' context of upper layer
+ */
+void cm_unbindComponentToCMCore(
+        const t_interface_require_description   *itfRequire,
+        t_mpc2host_bf_info                      *bfInfo);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Asynchronous distributed binding factory Information
+ */
+typedef struct {
+    t_nmf_fifo_arm_desc*    fifo;                   //!< Handle of the fifo params
+    t_dspstub_bf_info       dspstub;                //!< Information about the DSP stub (client side)
+    t_dspskel_bf_info       dspskeleton;            //!< Information about the DSP skeleton (server side)
+} t_mpc2mpc_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Create a asynchronous distributed binding between a client interface to a server interface.
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[in] itf The server interface
+ * \param[in] fifosize Number of waited event in the fifo
+ */
+t_cm_error cm_bindInterfaceDistributed(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription                        *elfhandleSkeleton,
+        t_elfdescription                        *elfhandleStub);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Destroy a asynchronous distributed binding between a client interface to a server interface.
+ * \param[in] itfRequire The required interface
+ */
+void cm_unbindInterfaceDistributed(
+        const t_interface_require_description   *itfRequire,
+        t_mpc2mpc_bf_info                       *bfInfo);
+
+/**
+ * \internal
+ *
+ * Bind a static interrupt to server provide interface name.
+ * \param[in] coreId The core to which component is loaded
+ * \param[in] interruptLine Interrupt line number to use
+ * \param[in] server Server instance that provide interrupt service
+ * \param[in] providedItfServerName Interface name hat provide interrupt service
+ */
+t_cm_error cm_bindInterfaceStaticInterrupt(
+        const t_nmf_core_id coreId,
+        const int interruptLine,
+        const t_component_instance *server,
+        const char* providedItfServerName);
+
+/**
+ * \internal
+ *
+ * Unbind a static interrupt.
+ * \param[in] coreId The core to which component is loaded
+ * \param[in] interruptLine Interrupt line number to use
+ */
+t_cm_error cm_unbindInterfaceStaticInterrupt(
+        const t_nmf_core_id coreId,
+        const int interruptLine);
+
+void cm_destroyRequireInterface(t_component_instance* component, t_nmf_client_id clientId);
+void cm_registerSingletonBinding(
+        t_component_instance*                   component,
+        t_interface_require_description*        itfRequire,
+        t_interface_provide_description*        itfProvide,
+        t_nmf_client_id                         clientId);
+t_bool cm_unregisterSingletonBinding(
+        t_component_instance*                   component,
+        t_interface_require_description*        itfRequire,
+        t_interface_provide_description*        itfProvide,
+        t_nmf_client_id                         clientId);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/component_type.h b/drivers/staging/nmf-cm/cm/engine/component/inc/component_type.h
new file mode 100644
index 00000000000..35571dde06d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/component_type.h
@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Components Component Manager API type.
+ *
+ * \defgroup COMPONENT CM Components API
+ * \ingroup CM_USER_API
+ */
+
+#ifndef COMPONENT_TYPE_H_
+#define COMPONENT_TYPE_H_
+
+#include <cm/inc/cm_type.h>
+#include <nmf/inc/component_type.h>
+
+/*!
+ * @defgroup t_nmf_ee_priority t_nmf_ee_priority
+ * \brief Identification of the execution engine priority and sub priority.
+ * @{
+ * \ingroup COMPONENT
+ */
+typedef t_uint32 t_nmf_ee_priority;                                      //!< Fake enumeration type
+
+#define NMF_SCHED_BACKGROUND                    ((t_nmf_ee_priority)0)  //!< Background priority
+#define NMF_SCHED_NORMAL                        ((t_nmf_ee_priority)1)  //!< Normal priority
+#define NMF_SCHED_URGENT                        ((t_nmf_ee_priority)2)  //!< Urgent priority
+/* @} */
+
+
+/*!
+ * \brief Identification of host component returned during introspection
+ *
+ * \ingroup COMPONENT_INTROSPECTION
+ */
+#define NMF_HOST_COMPONENT		((t_cm_instance_handle)0xFFFFFFFF)
+
+/*!
+ * \brief Identification of void component returned during introspection
+ *
+ * \ingroup COMPONENT_INTROSPECTION
+ */
+#define NMF_VOID_COMPONENT      ((t_cm_instance_handle)0xFFFFFFFE)
+
+
+/*!
+ * @defgroup t_nmf_ee_priority t_nmf_ee_priority
+ * \brief Identification of the execution engine priority and sub priority.
+ * @{
+ * \ingroup COMPONENT
+ */
+typedef t_uint8 t_cm_require_state;                                     //!< Fake enumeration type
+
+#define CM_REQUIRE_STATIC                    ((t_cm_require_state)0)  //!< Required interface is static
+#define CM_REQUIRE_OPTIONAL                  ((t_cm_require_state)1)  //!< Required interface is optional
+#define CM_REQUIRE_COLLECTION                ((t_cm_require_state)2)  //!< Required interface is a collection
+
+/* @} */
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/description.h b/drivers/staging/nmf-cm/cm/engine/component/inc/description.h
new file mode 100644
index 00000000000..b7d3c34654d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/description.h
@@ -0,0 +1,109 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#ifndef __INC_CM_COMPONENT_DESCRIPTION_H
+#define __INC_CM_COMPONENT_DESCRIPTION_H
+
+#include <cm/engine/elf/inc/memory.h>
+#include <cm/engine/utils/inc/string.h>
+
+#include <inc/nmf-limits.h>
+
+/*!
+ * \internal
+ * \brief Description of an interface
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct _t_interface_description {
+    t_dup_char              type;                  //!< Type of the interface
+    t_uint16                referenceCounter;       //!< Number of template referencing the interface
+    t_uint8                 methodNumber;           //!< Number of method in the interfaces
+    struct _t_interface_description* next;
+    t_dup_char              methodNames[];        //!< Array of method names
+} t_interface_description;
+
+/*!
+ * \internal
+ * \brief Description of a variable memory on a collection index
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_uint32            offset;            //!< Offset in the memory
+    const t_elfmemory   *memory;          //!< Memory
+} t_memory_reference;
+
+/*!
+ * \internal
+ * \brief Description of a required interface on a collection index
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_uint32                numberOfClient;                //!< Number of interface descriptor really connected to this interface
+    t_memory_reference      *memories;                   /*!< Memory where each interface reference descriptor resides
+                                                                                \note memories[numberOfClient] */
+} t_interface_require_index;
+
+/*!
+ * \internal
+ * \brief Description of a required interface
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_dup_char                      name;                                //!< Name of the interface
+    t_interface_description         *interface;                           //!< Description of the interface
+    t_uint8                         requireTypes;                         //!< Mask of t_elf_interface_require_type
+    t_uint8                         collectionSize;                       //!< Size of the collection (1 if not a collection)
+    t_interface_require_index       *indexes;         /*!< Require information for each collection index
+                                                                \note indexes[collectionSize] */
+} t_interface_require;
+
+/*!
+ * \internal
+ * \brief Description of a provided interface method on a collection index
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_uint32               methodAddresses;             //!< Address of each method ; Available only when template loaded
+    t_memory_reference     memory;                      //!< Memory of the method
+} t_interface_provide_index;
+
+/*!
+ * \internal
+ * \brief Description of a provided interface
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_dup_char                     name;                                 //!< Name of the interface
+    t_interface_description        *interface;                            //!< Description of the interface
+    t_uint8                         provideTypes;                         //!< Mask of t_elf_interface_provide_type
+    t_uint8                        interruptLine;                         //!< Interrupt line if interrupt (0 if not)
+    t_uint8                        collectionSize;                        //!< Size of the collection (1 if not a collection)
+    t_interface_provide_index      **indexes;                         //!< Provide information for each collection index
+} t_interface_provide;
+
+/*!
+ * \internal
+ * \brief Description of a attribute
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_dup_char              name;                             //!< Name of the attribute
+    t_memory_reference      memory;                            //!< Memory where the attribute reside
+} t_attribute;
+
+/*!
+ * \internal
+ * \brief Description of a property
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_dup_char          name;           //!< Name of this attribute
+    t_dup_char          value;          //!< String of the value
+} t_property;
+
+
+
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/dspevent.h b/drivers/staging/nmf-cm/cm/engine/component/inc/dspevent.h
new file mode 100644
index 00000000000..bb47363c0ae
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/dspevent.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_DSP_EVENT
+#define __INC_DSP_EVENT
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/memory/inc/memory.h>
+
+/* value should be size of t_remote_event in mmdsp word */
+#define DSP_REMOTE_EVENT_SIZE_IN_DSPWORD 5
+
+t_cm_error dspevent_createDspEventFifo(
+    const t_component_instance *pComp,
+    const char* nameOfTOP,
+    t_uint32 fifoNbElem,
+    t_uint32 fifoElemSizeInWord,
+    t_dsp_memory_type_id dspEventMemType,
+    t_memory_handle *pHandle);
+void dspevent_destroyDspEventFifo(t_memory_handle handle);
+
+#endif /* __INC_DSP_EVENT */
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/initializer.h b/drivers/staging/nmf-cm/cm/engine/component/inc/initializer.h
new file mode 100644
index 00000000000..b4416f3ce20
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/initializer.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_NMF_INITIALIZER
+#define __INC_NMF_INITIALIZER
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/component/inc/instance.h>
+#include <share/communication/inc/initializer.h>
+
+PUBLIC t_cm_error cm_COMP_INIT_Init(t_nmf_core_id coreId);
+PUBLIC t_cm_error cm_COMP_CallService(int serviceIndex, t_component_instance *pComp, t_uint32 methodAddress);
+PUBLIC void cm_COMP_INIT_Close(t_nmf_core_id coreId);
+PUBLIC t_cm_error cm_COMP_UpdateStack(t_nmf_core_id coreId, t_uint32 stackSize);
+PUBLIC t_cm_error cm_COMP_ULPForceWakeup(t_nmf_core_id coreId);
+PUBLIC t_cm_error cm_COMP_ULPAllowSleep(t_nmf_core_id coreId);
+PUBLIC t_cm_error cm_COMP_InstructionCacheLock(t_nmf_core_id coreId, t_uint32 mmdspAddr, t_uint32 mmdspSize);
+PUBLIC t_cm_error cm_COMP_InstructionCacheUnlock(t_nmf_core_id coreId, t_uint32 mmdspAddr, t_uint32 mmdspSize);
+
+
+PUBLIC void processAsyncAcknowledge(t_nmf_core_id coreId, t_event_params_handle pParam);
+PUBLIC void processSyncAcknowledge(t_nmf_core_id coreId, t_event_params_handle pParam);
+
+#endif /* __INC_NMF_INITIALIZER */
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/instance.h b/drivers/staging/nmf-cm/cm/engine/component/inc/instance.h
new file mode 100644
index 00000000000..1bef76501de
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/instance.h
@@ -0,0 +1,220 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Components Management internal methods - Instance API.
+ *
+ */
+#ifndef __INC_CM_INSTANCE_H
+#define __INC_CM_INSTANCE_H
+
+#include <cm/engine/component/inc/template.h>
+#include <cm/engine/repository_mgt/inc/repository_mgt.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/utils/inc/table.h>
+#include <cm/engine/utils/inc/string.h>
+
+/*----------------------------------------------------------------------------
+ * Component Instance API.
+ *----------------------------------------------------------------------------*/
+struct _t_interface_reference;
+
+/*!
+ * \internal
+ * \brief Component life cycle state
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef enum {
+    STATE_NONE,
+    STATE_STOPPED,
+    STATE_RUNNABLE,
+    // STATE_DESTROYED identified when component remove from component list
+} t_component_state;
+
+struct t_client_of_singleton
+{
+    struct t_client_of_singleton    *next;
+    t_nmf_client_id                 clientId;
+    t_uint16                        numberOfInstance;
+    t_uint16                        numberOfStart;
+    t_uint16                        numberOfBind;
+};
+
+/*!
+ * \internal
+ * \brief Description of a component instance
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct t_component_instance {
+    t_dup_char              pathname;                         //!< Path Name of this component in the components architecture
+
+    t_component_state       state;                            //!< Component state
+    t_nmf_ee_priority       priority;                         //!< Executive engine component priority
+    t_component_template   *template;                         //!< Component template
+
+    t_uint32                thisAddress;                      //!< Cached value of cm_DSP_GetDspAddress(component->memories[data], &thisAddress);
+
+    t_memory_handle         memories[NUMBER_OF_MMDSP_MEMORY];  //!<Reference in different memory where datas are (YES, we fix implementation to MMDSP)
+
+    struct _t_interface_reference **interfaceReferences;    /*!< Interface references
+                                      (Share same index as template->u.p.requires)
+                                      type == targets[interface_index][collection_index] */
+
+    t_uint16                providedItfUsedCount;             //!< Use count to reference the number of components binded to this once, ie count the number of provided interfaces in use
+    t_cm_instance_handle    instance;                         //!< index of this component within the ComponentTable
+    t_cm_domain_id          domainId;                         //!< Domain where the component has been installed
+
+    struct t_client_of_singleton    *clientOfSingleton;       //!< Client of singleton list
+} t_component_instance;
+
+t_component_template* cm_lookupTemplate(t_nmf_core_id dspId, t_dup_char str);
+
+/*!
+ * \internal
+ * \brief Load a component template.
+ *
+ * ...
+ *
+ * \param[in] templateName name of the template to load
+ * \param[in] coreId DSP where template must be loaded
+ * \praem[in] pRepComponent Pointer to the component entry stored in the Component Cache Repository
+ * \param[in, out] template reference to put the loaded template (null if first instance)
+ *
+ * \exception CM_COMPONENT_NOT_FOUND
+ * \exception CM_NO_MORE_MEMORY
+ *
+ * \return exception number.
+ *
+ * \warning For Component manager use only.
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_loadComponent(
+        t_dup_char templateName,
+        t_cm_domain_id domainId,
+        t_elfdescription* elfhandle,
+        t_component_template **reftemplate);
+
+/*!
+ * \internal
+ * \brief Unload a component template.
+ *
+ * ...
+ *
+ * \param[in] template template to be unloaded
+ * \praem[in] Private memories that has been created from component binary file
+ *
+ * \return exception number.
+ *
+ * \warning For Component manager use only.
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_unloadComponent(
+        t_component_template *template);
+
+/*!
+ * \internal
+ * \brief Instantiate a component.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_instantiateComponent(const char* templateName,
+        t_cm_domain_id domainId,
+        t_nmf_ee_priority priority,
+        const char* pathName,
+        t_elfdescription *elfhandle,
+        t_component_instance** refcomponent);
+
+struct t_client_of_singleton* cm_getClientOfSingleton(t_component_instance* component, t_bool createdIfNotExist, t_nmf_client_id clientId);
+
+/*!
+ * \internal
+ * \brief Start a component.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_startComponent(t_component_instance* component, t_nmf_client_id clientId);
+
+/*!
+ * \internal
+ * \brief Stop a component.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_stopComponent(t_component_instance* component, t_nmf_client_id clientId);
+
+/*!
+ * \internal
+ */
+typedef enum {
+    DESTROY_NORMAL,
+    DESTROY_WITHOUT_CHECK,
+    DESTROY_WITHOUT_CHECK_CALL
+} t_destroy_state;
+
+/*!
+ * \internal
+ * \brief Destroy a component instance.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_destroyInstance(t_component_instance* component, t_destroy_state forceDestroy);
+
+/*!
+ * \internal
+ * \brief Destroy a component instance.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_destroyInstanceForClient(t_component_instance* component, t_destroy_state forceDestroy, t_nmf_client_id clientId);
+
+/*!
+ * \internal
+ * \brief
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+void cm_delayedDestroyComponent(t_component_instance *component);
+
+/*!
+ * \internal
+ * \brief
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_component_instance *cm_lookupComponent(const t_cm_instance_handle hdl);
+
+/*!
+ * \internal
+ * \brief
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_bool cm_isComponentOnCoreId(t_nmf_core_id coreId);
+
+/*!
+ * \internal
+ * \brief
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_COMP_Init(void);
+
+/*!
+ * \internal
+ * \brief
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+void cm_COMP_Destroy(void);
+
+/*
+ * Table of instantiated components.
+ */
+extern t_nmf_table ComponentTable; /**< list (table) of components */
+#define componentEntry(i) ((t_component_instance *)ComponentTable.entries[i])
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/introspection.h b/drivers/staging/nmf-cm/cm/engine/component/inc/introspection.h
new file mode 100644
index 00000000000..cfb55c91779
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/introspection.h
@@ -0,0 +1,109 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Components Management internal methods - Introspection.
+ *
+ */
+#ifndef __INC_CM_INTROSPECTION_H
+#define __INC_CM_INTROSPECTION_H
+
+#include <cm/engine/component/inc/instance.h>
+
+/*!
+ * \internal
+ * \brief Description of a required interface reference
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    const t_component_instance          *client;            //!< Component that provide this interface
+    t_uint8                             requireIndex;       //!< Index of the interface in the require array
+    t_uint8                             collectionIndex;    //!< Index in the collection if required interface is a collection
+    const char*                         origName;           //!< Name of the component interface
+} t_interface_require_description;
+
+/*!
+ * \internal
+ * \brief Description of a provided interface
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    const t_component_instance          *server;            //!< Component that provide this interface
+    t_uint8                             provideIndex;       //!< Index of the interface in the provide array
+    t_uint8                             collectionIndex;    //!< Index in the collection if provided interface is a collection
+    const char*                         origName;           //!< Name of the component interface
+} t_interface_provide_description;
+
+
+/*!
+ * \internal
+ * \brief Get property of a component.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_getComponentProperty(
+	const t_component_instance *component,
+	const char                 *propName,
+	char                       value[MAX_PROPERTY_VALUE_LENGTH],
+    t_uint32                   valueLength);
+
+
+t_dsp_address cm_getAttributeMpcAddress(
+        const t_component_instance  *component,
+        const char                  *attrName);
+
+t_cm_logical_address cm_getAttributeHostAddr(
+        const t_component_instance  *component,
+        const char                  *attrName);
+
+t_uint32 cm_readAttributeNoError(
+        const t_component_instance  *component,
+                const char                  *attrName);
+
+t_cm_error cm_readAttribute(
+        const t_component_instance  *component,
+        const char                  *attrName,
+        t_uint32                    *value);
+
+t_cm_error cm_writeAttribute(
+        const t_component_instance  *component,
+        const char                  *attrName,
+        t_uint32                    value);
+
+/*!
+ * \internal
+ * \brief Get internal component symbol
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_dsp_address cm_getFunction(
+        const t_component_instance* component,
+        const char* interfaceName,
+        const char* methodName);
+
+/*!
+ * \internal
+ * \brief Get interface provided by a component instance.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_getProvidedInterface(const t_component_instance* server,
+    const char* itfName,
+    t_interface_provide_description *itfProvide);
+
+/*!
+ * \internal
+ * \brief Get interface required by a component instance.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_getRequiredInterface(const t_component_instance* server,
+    const char* itfName,
+    t_interface_require_description *itfRequire);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/nmfheaderabi.h b/drivers/staging/nmf-cm/cm/engine/component/inc/nmfheaderabi.h
new file mode 100644
index 00000000000..59d2186f157
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/nmfheaderabi.h
@@ -0,0 +1,154 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief NMF component description ABI
+ *
+ * \defgroup NMF_HEADER NMF Component Description ABI
+ * The NMF component description ABI is stored in the nmf_segment in the ELF component file.
+ * The NMF component description section start by the t_elf_component_header structure.
+ *
+ * \warning <B>The format of this section is not fixed and is able to be changed without concerting.</B>
+ * \note You can use the nmfHeaderVersion to check if the format has changed.
+ * \note Each pointers in this section is relative to the beginning of the section and must be relocated before used.
+ * \ingroup NMF_ABI
+ */
+#ifndef __INC_CM_NMF_HEADERABI_H
+#define __INC_CM_NMF_HEADERABI_H
+
+#include <cm/inc/cm_type.h>
+
+/*!
+ * \brief Description of a interface
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+    char                    *type;                  //!< Type of this Interface
+    t_uint8                 methodNumber;           //!< Number of method in the interfaces
+    t_uint8                 reserved1, reserved2, reserved3;
+    char                    *methodNames[];        //!< Array of method names
+} t_elf_interface_description;
+
+/*!
+ * \brief Description of required interface type (value could be combinated)
+ * \ingroup NMF_HEADER
+ */
+typedef enum {
+    COLLECTION_REQUIRE = 1,                     //!< Required interface is a collection
+    OPTIONAL_REQUIRE = 2,                       //!< Required interface if optional
+    STATIC_REQUIRE = 4,                         //!< Required interface is static
+    VIRTUAL_REQUIRE = 8,                        //!< Required interface is virtual (only for introspection purpose)
+    INTRINSEC_REQUIRE = 16                      //!< Required interface is intrinsec (bind automatically done by runtime)
+} t_elf_interface_require_type;
+
+/*!
+ * \brief Description of a required interface on a collection index
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+    t_uint32     numberOfClient;                //!< Number of interface descriptor really connected to this interface
+    t_uint32     symbols[1];          /*!< Symbol of the real name of the attribute
+                                        \note Real type symbols[numberOfClient]
+                                        \note Use relocation in order to get symbol information */
+} t_elf_interface_require_index;
+
+/*!
+ * \brief Description of an interface required
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+	char        *name;		                            //!< name of the interface: offset in string segment
+    t_uint8     requireTypes;                           //!< Mask of t_elf_interface_require_type
+    t_uint8     collectionSize;                         //!< Size of the collection (1 if not a collection)
+    t_uint8     reserved1, reserved2;
+    t_elf_interface_description     *interface;         //!< Interface description
+    t_elf_interface_require_index   indexes[];           /*!< Require information for each collection index
+                                                                        \note Real type: indexes[collectionSize],
+                                                                        available only if not static interface */
+}  t_elf_required_interface;
+
+/*!
+ * \brief Description of provided interface type (value could be combinated)
+ * \ingroup NMF_HEADER
+ */
+typedef enum {
+    COLLECTION_PROVIDE = 1,                               //!< Provided interface is a collection
+    VIRTUAL_PROVIDE = 2                         //!< Provided interface is virtual (only for introspection purpose)
+} t_elf_interface_provide_type;
+
+/*!
+ * \brief Description of an interface provided
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+	char* 			                name;		            //!< name of the interface: offset in string segment
+    t_uint8                         provideTypes;           //!< Mask of t_elf_interface_provide_type
+    t_uint8                         interruptLine;          //!< Interrupt line if interrupt (0 if not)
+    t_uint8                         collectionSize;         //!< Size of the collection (1 if not a collection)
+    t_uint8                         reserved1;
+	t_elf_interface_description     *interface;             //!< Interface description
+    t_uint32                        methodSymbols[];        /*!< Symbol of the real name of methods of the interface for each collection index
+                                                                    \note Real type: methodSymbols[collectionSize][methodNumber]
+                                                                    \note Use relocation in order to get symbol information*/
+} t_elf_provided_interface;
+
+/*!
+ * \brief Description of an attribute
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+	char* 	    name;			//!< Name of this attribute
+    t_uint32    symbols;          /*!< Symbol of the real name of the attribute
+                                        \note Use relocation in order to get symbol information */
+} t_elf_attribute;
+
+/*!
+ * \brief Description of an property
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+	char* 	name;			//!< Name of this attribute
+	char*	value;			//!< String of the value
+} t_elf_property;
+
+#define MAGIC_COMPONENT 0x123    //!< Magic Number for a component \ingroup NMF_HEADER
+#define MAGIC_SINGLETON 0x321    //!< Magic Number for a singleton component \ingroup NMF_HEADER
+#define MAGIC_FIRMWARE 0x456      //!< Magic Number for Execution Engine Component \ingroup NMF_HEADER
+
+/*!
+ * \brief Description of a ELF component header
+ *
+ * The NMF component description section start by this structure.
+ *
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+	t_uint32                    magic;		    //!< Magic Number
+    t_uint32                    nmfVersion;     //!< Version of the NMF Header
+
+    char*                       templateName;   //!< Name of the component template
+
+    t_uint32                    LCCConstruct;   //!< Life cycle Constructor offset
+    t_uint32                    LCCStart;       //!< Life cycle Starter offset
+    t_uint32                    LCCStop;        //!< Life cycle Stopper offset
+    t_uint32                    LCCDestroy;     //!< Life cycle Destructer offset
+
+    t_uint32                    minStackSize;   //!< Minimum stack size
+
+    t_uint32                    attributeNumber;//!< Number of attributes
+    t_elf_attribute             *attributes;    //!< Array of attributes (be careful, this reference must be relocated before use)
+
+    t_uint32                    propertyNumber; //!< Number of properties
+    t_elf_property              *properties;    //!< Array of properties (be careful, this reference must be relocated before use)
+
+    t_uint32                    provideNumber;  //!< Number of interfaces provided
+    t_elf_provided_interface    *provides;      //!< Array of interfaces provided (be careful, this reference must be relocated before use)
+
+    t_uint32                    requireNumber;     //!< Array of interfaces required
+    t_elf_required_interface    *requires;      //!< Array of interfaces required (be careful, this reference must be relocated before use)
+
+} t_elf_component_header;
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/template.h b/drivers/staging/nmf-cm/cm/engine/component/inc/template.h
new file mode 100644
index 00000000000..3e1a2c001e4
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/template.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Components Management internal methods - Template API.
+ *
+ * \defgroup COMPONENT_INTERNAL Private component instances API
+ */
+#ifndef __INC_CM_TEMPLATE_H
+#define __INC_CM_TEMPLATE_H
+
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/component/inc/description.h>
+#include <cm/engine/elf/inc/elfapi.h>
+#include <cm/engine/utils/inc/string.h>
+
+
+/*!
+ * \internal
+ * \brief Class of a component
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef enum {
+	COMPONENT, 			//!< Primitive component
+	SINGLETON,          //!< Singleton component
+	FIRMWARE,			//!< Firmware composite component
+} t_component_classe;
+
+/*!
+ * \internal
+ * \brief Description of delayed relocation
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct _t_function_relocation {
+    t_dup_char                      symbol_name;
+    t_uint32                        type;
+    char                            *reloc_addr;
+    struct _t_function_relocation   *next;
+} t_function_relocation;
+
+struct t_component_instance;
+
+/*!
+ * \internal
+ * \brief Description of a component template
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct _t_component_template {
+    t_dup_char                  name;                   //!< Template name (a.k.a component type)
+
+    t_component_classe          classe;                 //!< Class of the component
+    //TODO, juraj, remove dspId
+    t_nmf_core_id               dspId;                  //!< Reference on DSP where template is loaded
+
+    t_uint8                     numberOfInstance;       //!< Number of same instance (or singleton copy) create from this template
+
+    t_uint8                     propertyNumber;         //!< Number of properties in this template
+    t_uint8                     attributeNumber;        //!< Number of attributes in this template
+    t_uint8                     provideNumber;          //!< Number of interface provided by this template
+    t_uint8                     requireNumber;          //!< Number of interface required by this template
+
+    t_uint32                    LCCConstructAddress;    //!< Life cycle Constructor address
+    t_uint32                    LCCStartAddress;        //!< Life cycle Starter address
+    t_uint32                    LCCStopAddress;         //!< Life cycle Stopper address
+    t_uint32                    LCCDestroyAddress;      //!< Life cycle Destructer address
+
+    t_uint32                    minStackSize;           //!< Minimum stack size
+
+    t_memory_handle             memories[NUMBER_OF_MMDSP_MEMORY];   //!< Reference in different memory where datas are (YES, we fix implementation to MMDSP)
+    const t_elfmemory           *thisMemory;            //!< Memory used to determine this
+    const t_elfmemory           *codeMemory;            //!< Memory used to determine code
+
+    t_function_relocation       *delayedRelocation;     //!< List of reference that can't been relocatable while appropritae binding done.
+
+    t_property	        *properties;            //!< Array of properties in this template
+    t_attribute         *attributes;            //!< Array of attributes in this template
+    t_interface_provide *provides;              //!< Array of interface provided by this template
+    t_interface_require *requires;              //!< Array of interface required by this template
+
+    t_bool                      descriptionAssociatedWithTemplate;
+
+    struct _t_component_template *prev, *next;
+    struct t_component_instance  *singletonIfAvaliable;
+} t_component_template;
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/binder.c b/drivers/staging/nmf-cm/cm/engine/component/src/binder.c
new file mode 100644
index 00000000000..8586fdfcbc8
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/binder.c
@@ -0,0 +1,1310 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include "../inc/bind.h"
+#include "../inc/dspevent.h"
+#include <cm/engine/communication/fifo/inc/nmf_fifo_arm.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/component/inc/introspection.h>
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/trace/inc/xtitrace.h>
+
+#include <cm/engine/utils/inc/string.h>
+
+#define CM_IT_NAME_MAX_LENGTH               8
+
+t_nmf_table Host2MpcBindingTable; /**< list (table) of host2mpc bindings */
+
+static void cm_fillItName(int interruptLine, char *itName);
+static t_uint16 getNumberOfBind(t_component_instance* component);
+
+/*
+ * Bind virtual interface, here we assume that:
+ * - client component require this interface as last one and without collection,
+ * - server component provide only this interface and without collection.
+ * Fixed in loader.c.
+ */
+static void cm_bindVirtualInterface(
+        t_component_instance* client,
+        const t_component_instance* server) {
+    t_interface_require_description itfRequire;
+
+    if(cm_getRequiredInterface(client, "coms", &itfRequire) == CM_OK)
+    {
+        t_interface_reference* itfRef = client->interfaceReferences[itfRequire.requireIndex];
+
+        /*
+         * Memorise this reference
+         */
+        itfRef->provideIndex = 0;
+        itfRef->collectionIndex = 0;
+        itfRef->instance = server;
+        itfRef->bfInfoID = (t_bf_info_ID)0;
+        itfRef->bfInfo = (void*)-1; // TODO
+    }
+    else
+    {
+        ERROR("Internal Error in cm_bindVirtualInterface\n", 0, 0, 0, 0, 0, 0);
+    }
+}
+
+static void cm_unbindVirtualInterface(
+        t_component_instance* client) {
+    t_interface_require_description itfRequire;
+
+    if(cm_getRequiredInterface(client, "coms", &itfRequire) == CM_OK)
+    {
+        t_interface_reference* itfRef = client->interfaceReferences[itfRequire.requireIndex];
+        itfRef->instance = NULL;
+    }
+    else
+    {
+        ERROR("Internal Error in cm_unbindVirtualInterface\n", 0, 0, 0, 0, 0, 0);
+    }
+}
+
+/*
+ * Bind component
+ */
+static void cm_bindLowLevelInterface(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfLocalBC,	         /* On the same DSP */
+        t_bf_info_ID bfInfoID, void* bfInfo)
+{
+    const t_component_instance* client = itfRequire->client;
+    t_component_instance* server = (t_component_instance*)itfLocalBC->server;
+    t_interface_require *require = &client->template->requires[itfRequire->requireIndex];
+    t_interface_provide* provide = &server->template->provides[itfLocalBC->provideIndex];
+    int k, j;
+
+    if(require->indexes != NULL)
+    {
+        t_interface_require_index *requireindex = &require->indexes[itfRequire->collectionIndex];
+
+        for(k = 0; k < requireindex->numberOfClient; k++) {
+            t_uint32 *hostAddr;
+
+            hostAddr = (t_uint32*)(
+                    cm_DSP_GetHostLogicalAddress(client->memories[requireindex->memories[k].memory->id]) +
+                    requireindex->memories[k].offset * requireindex->memories[k].memory->memEntSize);
+
+            LOG_INTERNAL(2, "Fill ItfRef %s.%s mem=%s Off=%x @=%x\n",
+                    client->pathname, require->name,
+                    requireindex->memories[k].memory->memoryName,
+                    requireindex->memories[k].offset,
+                    hostAddr, 0);
+
+            /*
+             * Fill the interface references. We start by This then methods in order to keep
+             * Unbinded panic as long as possible and not used method with wrong This. This is
+             * relevent only for optional since we must go in stop state before rebinding other
+             * required interface.
+             *
+             * Direct write to DSP memory without go through DSP abstraction since we know we are in 24bits
+             */
+            // Write THIS reference into the Data field of the interface reference
+            // Write the interface methods reference
+
+            if(((t_uint32)hostAddr & 0x7) == 0 && require->interface->methodNumber > 0)
+            {
+                // We are 64word byte aligned, combine this write with first method
+                *(volatile t_uint64*)hostAddr =
+                        ((t_uint64)server->thisAddress << 0) |
+                        ((t_uint64)provide->indexes[itfLocalBC->collectionIndex][0].methodAddresses << 32);
+                hostAddr += 2;
+                j = 1;
+            }
+            else
+            {
+                // We are not, write this which will align us
+                *hostAddr++ = (t_uint32)server->thisAddress;
+                j = 0;
+            }
+
+            // Word align copy
+            for(; j < require->interface->methodNumber - 1; j+=2) {
+                *(volatile t_uint64*)hostAddr =
+                        ((t_uint64)provide->indexes[itfLocalBC->collectionIndex][j].methodAddresses << 0) |
+                        ((t_uint64)provide->indexes[itfLocalBC->collectionIndex][j+1].methodAddresses << 32);
+                hostAddr += 2;
+            }
+
+            // Last word align if required
+            if(j < require->interface->methodNumber)
+                *hostAddr = provide->indexes[itfLocalBC->collectionIndex][j].methodAddresses;
+        }
+    }
+    else
+    {
+        t_function_relocation *reloc = client->template->delayedRelocation;
+        while(reloc != NULL) {
+            for(j = 0; j < provide->interface->methodNumber; j++)
+            {
+                if(provide->interface->methodNames[j] == reloc->symbol_name) {
+                    cm_ELF_performRelocation(
+                                        reloc->type,
+                                        reloc->symbol_name,
+                                        provide->indexes[itfLocalBC->collectionIndex][j].methodAddresses,
+                                        reloc->reloc_addr);
+                    break;
+                }
+            }
+
+            reloc = reloc -> next;
+        }
+    }
+
+    /*
+     * Memorise this reference
+     */
+    {
+        t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+
+        itfRef->provideIndex = itfLocalBC->provideIndex;
+        itfRef->collectionIndex = itfLocalBC->collectionIndex;
+        itfRef->instance = itfLocalBC->server;
+        itfRef->bfInfoID = bfInfoID;
+        itfRef->bfInfo = bfInfo;
+
+        /*
+         * Do not count binding from EE (ie interrupt line), as this will prevent
+         * cm_destroyInstance() of server to succeed (interrupt line bindings are
+         * destroyed after the check in cm_destroyInstance()
+         */
+        if (client->template->classe != FIRMWARE)
+            server->providedItfUsedCount++;
+    }
+}
+
+static void cm_registerLowLevelInterfaceToConst(
+    const t_interface_require_description *itfRequire,
+    const t_component_instance* targetInstance)
+{
+    const t_component_instance* client = itfRequire->client;
+
+    /*
+     * Memorise this no reference
+     */
+    {
+        t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+
+        // This is an unbind from a true component (not to void)
+        // Do not count bindings from EE (ie interrupt line)
+        if ((targetInstance == NULL)
+                && (client->template->classe != FIRMWARE)
+                && (itfRef->instance != (t_component_instance *)NMF_VOID_COMPONENT)
+                && (itfRef->instance != NULL))
+        {
+            ((t_component_instance*)itfRef->instance)->providedItfUsedCount--;
+        }
+
+        itfRef->instance = targetInstance;
+        itfRef->bfInfoID = BF_SYNCHRONOUS; // Just to memorize no Binding component used and unbind ToVoid happy ;-).
+    }
+}
+
+static void cm_bindLowLevelInterfaceToConst(
+        const t_interface_require_description *itfRequire,
+        const t_dsp_address functionAddress,
+        const t_component_instance* targetInstance) {
+    const t_component_instance* client = itfRequire->client;
+    t_interface_require *require = &client->template->requires[itfRequire->requireIndex];
+    int j, k;
+
+
+    // If DSP is off/panic/... -> write nothing
+    if(
+            require->indexes != NULL
+            && cm_DSP_GetState(client->template->dspId)->state == MPC_STATE_BOOTED)
+    {
+        t_interface_require_index *requireindex = &require->indexes[itfRequire->collectionIndex];
+
+        for(k = 0; k < requireindex->numberOfClient; k++) {
+            t_uint32 *hostAddr;
+
+            hostAddr = (t_uint32*)(
+                    cm_DSP_GetHostLogicalAddress(client->memories[requireindex->memories[k].memory->id]) +
+                    requireindex->memories[k].offset * requireindex->memories[k].memory->memEntSize);
+
+            /*
+             * Fill the interface references. We start by Methods then This in order to swith to
+             * Unbinded panic as fast as possible and not used method with wrong This. This is
+             * relevent only for optional since we must go in stop state before rebinding other
+             * required interface.
+             *
+             * Direct write to DSP memory without go through DSP abstraction since we know we are in 24bits
+             */
+            /*
+             * Write THIS reference into the Data field of the interface reference
+             * Hack for simplifying debug just to keep THIS reference with caller one
+             * (could be removed if __return_address MMDSP intrinsec provided by compiler).
+             */
+            // Write the interface methods reference
+
+            if(((t_uint32)hostAddr & 0x7) == 0 && require->interface->methodNumber > 0)
+            {
+                // We are 64word byte aligned, combine this write with first method
+                *(volatile t_uint64*)hostAddr =
+                        ((t_uint64)client->thisAddress << 0) |
+                        ((t_uint64)functionAddress << 32);
+                hostAddr += 2;
+                j = 1;
+            }
+            else
+            {
+                // We are not, write this which will align us
+                *hostAddr++ = (t_uint32)client->thisAddress;
+                j = 0;
+            }
+
+            // Word align copy
+            for(; j < require->interface->methodNumber - 1; j+=2) {
+                *(volatile t_uint64*)hostAddr =
+                        ((t_uint64)functionAddress << 0) |
+                        ((t_uint64)functionAddress << 32);
+                hostAddr += 2;
+            }
+
+            // Last word align if required
+            if(j < require->interface->methodNumber)
+                *hostAddr = functionAddress;
+        }
+    }
+
+    cm_registerLowLevelInterfaceToConst(itfRequire, targetInstance);
+}
+
+/*
+ * Bind User component though primitive binding factory
+ */
+t_cm_error cm_bindInterface(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide) {
+
+    LOG_INTERNAL(1, "\n##### Bind Synchronous %s/%x.%s -> %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+		 itfProvide->server->pathname, itfProvide->server, itfProvide->origName);
+
+    cm_bindLowLevelInterface(
+            itfRequire,
+            itfProvide,
+            BF_SYNCHRONOUS, NULL);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_SYNCHRONOUS,
+            itfRequire->client, itfProvide->server,
+            itfRequire->client->template->requires[itfRequire->requireIndex].name,
+            itfProvide->server->template->provides[itfProvide->provideIndex].name);
+
+  	return CM_OK;
+}
+
+/*
+ *
+ */
+void cm_unbindInterface(
+        const t_interface_require_description *itfRequire) {
+
+    LOG_INTERNAL(1, "\n##### UnBind synchronous %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_SYNCHRONOUS,
+            itfRequire->client, NULL,
+            itfRequire->client->template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    cm_bindLowLevelInterfaceToConst(itfRequire,
+            0x0,
+            NULL);
+}
+
+/*
+ *
+ */
+t_cm_error cm_bindInterfaceToVoid(
+        const t_interface_require_description *itfRequire) {
+    LOG_INTERNAL(1, "\n##### Bind %s/%x.%s -> Void #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_bindLowLevelInterfaceToConst(itfRequire,
+            cm_EEM_getExecutiveEngine(itfRequire->client->template->dspId)->voidAddr,
+            (t_component_instance*)NMF_VOID_COMPONENT);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_SYNCHRONOUS,
+            itfRequire->client, NULL,
+            itfRequire->client->template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+	return CM_OK;
+}
+/*
+ * Find the server and its interface inded to a given required interface for a given component
+ */
+t_cm_error cm_lookupInterface(
+        const t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide) {
+    const t_component_instance* client = itfRequire->client;
+    t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+
+	if(itfRef->instance != NULL)
+	{
+	    itfProvide->server = itfRef->instance;
+    	itfProvide->provideIndex = itfRef->provideIndex;
+    	itfProvide->collectionIndex = itfRef->collectionIndex;
+
+	    return CM_OK;
+    } else {
+        itfProvide->server = NULL;
+        return CM_INTERFACE_NOT_BINDED;
+    }
+}
+
+/*
+ *
+ */
+t_cm_error cm_bindInterfaceTrace(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_elfdescription *elfhandleTrace)
+{
+    t_interface_require *require = &itfRequire->client->template->requires[itfRequire->requireIndex];
+    t_interface_require_description bcitfRequire;
+    t_interface_provide_description bcitfProvide;
+    t_trace_bf_info *bfInfo;
+    t_cm_error error;
+
+    LOG_INTERNAL(1, "\n##### Bind Synchronous Trace %s/%x.%s -> %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+		 itfProvide->server->pathname, itfProvide->server, itfProvide->origName);
+
+    /* Allocate aynchronous binding factory information */
+    bfInfo = (t_trace_bf_info*)OSAL_Alloc(sizeof(t_trace_bf_info));
+    if(bfInfo == 0)
+        return CM_NO_MORE_MEMORY;
+
+    /*
+     * Instantiate related trace on dsp
+     */
+    {
+        char traceTemplateName[4 + MAX_INTERFACE_TYPE_NAME_LENGTH + 1];
+
+        cm_StringCopy(traceTemplateName,"_tr.", sizeof(traceTemplateName));
+        cm_StringConcatenate(traceTemplateName, require->interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+        if ((error = cm_instantiateComponent(
+                traceTemplateName,
+                itfRequire->client->domainId,
+                itfProvide->server->priority,
+                traceDup,
+                elfhandleTrace,
+                &bfInfo->traceInstance)) != CM_OK) {
+            OSAL_Free(bfInfo);
+            return (error == CM_COMPONENT_NOT_FOUND)?CM_BINDING_COMPONENT_NOT_FOUND : error;
+        }
+    }
+
+    /* Bind event to server interface (Error must not occure) */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->traceInstance, "target", &bcitfRequire) == CM_OK);
+
+    cm_bindLowLevelInterface(&bcitfRequire, itfProvide, BF_SYNCHRONOUS, NULL);
+
+    /* Get the event interface (Error must not occure) */
+    CM_ASSERT(cm_getProvidedInterface(bfInfo->traceInstance, "target", &bcitfProvide) == CM_OK);
+
+    /* Bind client to event (Error must not occure) */
+    cm_bindLowLevelInterface(itfRequire, &bcitfProvide, BF_TRACE, bfInfo);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_SYNCHRONOUS,
+            itfRequire->client, itfProvide->server,
+            itfRequire->client->template->requires[itfRequire->requireIndex].name,
+            itfProvide->server->template->provides[itfProvide->provideIndex].name);
+
+    return CM_OK;
+}
+
+void cm_unbindInterfaceTrace(
+        const t_interface_require_description *itfRequire,
+        t_trace_bf_info                       *bfInfo)
+{
+    t_interface_require_description traceitfRequire;
+
+    LOG_INTERNAL(1, "\n##### UnBind trace synchronous %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_SYNCHRONOUS,
+            itfRequire->client, NULL,
+            itfRequire->client->template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    /* Unbind Client from Event Binding Component */
+    cm_bindLowLevelInterfaceToConst(itfRequire, 0x0, NULL);
+
+    /* Unbind explicitly Event from Server Binding Component */
+    /* This is mandatory to fix the providedItfUsedCount of the server */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->traceInstance, "target", &traceitfRequire) == CM_OK);
+
+    cm_registerLowLevelInterfaceToConst(&traceitfRequire, NULL);
+
+    /* Destroy Event Binding Component */
+    cm_destroyInstance(bfInfo->traceInstance, DESTROY_WITHOUT_CHECK);
+
+    /* Free BF info */
+    OSAL_Free(bfInfo);
+}
+
+
+/*
+ *
+ */
+t_cm_error cm_bindInterfaceAsynchronous(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription *elfhandleEvent) {
+    t_interface_require *require = &itfRequire->client->template->requires[itfRequire->requireIndex];
+    t_interface_require_description eventitfRequire;
+    t_interface_provide_description eventitfProvide;
+    t_async_bf_info *bfInfo;
+    t_cm_error error;
+
+    LOG_INTERNAL(1, "\n##### Bind Asynchronous %s/%x.%s -> %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+		 itfProvide->server->pathname, itfProvide->server, itfProvide->origName);
+
+    /* Allocate aynchronous binding factory information */
+    bfInfo = (t_async_bf_info*)OSAL_Alloc(sizeof(t_async_bf_info));
+    if(bfInfo == 0)
+        return CM_NO_MORE_MEMORY;
+
+    /*
+     * Instantiate related event on dsp
+     */
+    {
+        char eventTemplateName[4 + MAX_INTERFACE_TYPE_NAME_LENGTH + 1];
+
+        cm_StringCopy(eventTemplateName,"_ev.", sizeof(eventTemplateName));
+        cm_StringConcatenate(eventTemplateName, require->interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+        if ((error = cm_instantiateComponent(
+                        eventTemplateName,
+                        itfRequire->client->domainId,
+                        itfProvide->server->priority,
+                        eventDup,
+                        elfhandleEvent,
+                        &bfInfo->eventInstance)) != CM_OK) {
+            OSAL_Free(bfInfo);
+            return (error == CM_COMPONENT_NOT_FOUND)?CM_BINDING_COMPONENT_NOT_FOUND : error;
+        }
+    }
+
+    /*
+     * Initialize the event component
+     */
+    {
+        unsigned int size;
+
+        // Get fifo elem size (which was store in TOP by convention)
+        size = cm_readAttributeNoError(bfInfo->eventInstance, "TOP");
+        LOG_INTERNAL(3, "DspEvent Fifo element size = %d\n", size, 0, 0, 0, 0, 0);
+
+        // Allocate fifo
+        if ((error = dspevent_createDspEventFifo(bfInfo->eventInstance,
+                        "TOP",
+                        fifosize, size,
+                        dspEventMemType,
+                        &bfInfo->dspfifoHandle)) != CM_OK)
+        {
+            cm_destroyInstance(bfInfo->eventInstance, DESTROY_WITHOUT_CHECK);
+            OSAL_Free(bfInfo);
+            return error;
+        }
+    }
+
+    /* Bind event to server interface (Error must not occure) */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->eventInstance, "target", &eventitfRequire) == CM_OK);
+
+    cm_bindLowLevelInterface(&eventitfRequire, itfProvide, BF_SYNCHRONOUS, NULL);
+
+    /* Get the event interface (Error must not occure) */
+    CM_ASSERT(cm_getProvidedInterface(bfInfo->eventInstance, "target", &eventitfProvide) == CM_OK);
+
+    /* Bind client to event (Error must not occure) */
+    cm_bindLowLevelInterface(itfRequire, &eventitfProvide, BF_ASYNCHRONOUS, bfInfo);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_ASYNCHRONOUS,
+            itfRequire->client, itfProvide->server,
+            itfRequire->client->template->requires[itfRequire->requireIndex].name,
+            itfProvide->server->template->provides[itfProvide->provideIndex].name);
+
+    return CM_OK;
+}
+
+void cm_unbindInterfaceAsynchronous(
+        const t_interface_require_description   *itfRequire,
+        t_async_bf_info                         *bfInfo)
+{
+    t_interface_require_description eventitfRequire;
+
+    LOG_INTERNAL(1, "\n##### UnBind asynchronous %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_ASYNCHRONOUS,
+            itfRequire->client, NULL,
+            itfRequire->client->template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    /* Unbind Client from Event Binding Component */
+    cm_bindLowLevelInterfaceToConst(itfRequire, 0x0, NULL);
+
+    /* Unbind explicitly Event from Server Binding Component */
+    /* This is mandatory to fix the providedItfUsedCount of the server */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->eventInstance, "target", &eventitfRequire) == CM_OK);
+
+    cm_registerLowLevelInterfaceToConst(&eventitfRequire, NULL);
+
+    /* Destroy Event fifo */
+    dspevent_destroyDspEventFifo(bfInfo->dspfifoHandle);
+
+    /* Destroy Event Binding Component */
+    cm_destroyInstance(bfInfo->eventInstance, DESTROY_WITHOUT_CHECK);
+
+    /* Free BF info */
+    OSAL_Free(bfInfo);
+}
+
+/*!
+ * Create Shared FIFO and set stub and skeleton to it
+ */
+PRIVATE t_cm_error cm_createParamsFifo(t_component_instance *stub,
+        t_component_instance *skeleton,
+        t_cm_domain_id domainId,
+        t_uint32 fifosize,
+        t_nmf_fifo_arm_desc **fifo,
+        t_uint32 *fifoElemSize,
+        t_uint32 bcDescSize)
+{
+    t_nmf_core_id stubcore = (stub != NULL) ?(stub->template->dspId): ARM_CORE_ID;
+    t_nmf_core_id skelcore = (skeleton != NULL) ?(skeleton->template->dspId) : ARM_CORE_ID;
+    t_component_instance *bcnotnull = (stub != NULL) ? stub : skeleton;
+    int _fifoelemsize;
+
+    CM_ASSERT(bcnotnull != NULL);
+
+    /* Get fifo param elem size (which was store in FIFO by convention) */
+    _fifoelemsize = cm_readAttributeNoError(bcnotnull, "FIFO");
+    LOG_INTERNAL(3, "Fifo Params element size = %d\n", _fifoelemsize, 0, 0, 0, 0, 0);
+    if(fifoElemSize != NULL)
+        *fifoElemSize = _fifoelemsize;
+
+    /* Allocation of the fifo params */
+    *fifo = fifo_alloc(stubcore, skelcore, _fifoelemsize, fifosize, 1+bcDescSize, paramsLocation, extendedFieldLocation, domainId); /* 1+nbMethods fro hostBCThis_or_TOP space */
+    if(*fifo == NULL)
+        return CM_NO_MORE_MEMORY;
+
+    if(stub != NULL)
+    {
+        /* Set stub FIFO attribute (Error mut not occure) */
+        cm_writeAttribute(stub, "FIFO", (*fifo)->dspAdress);
+
+        LOG_INTERNAL(2, "  FIFO param %x:%x\n", *fifo, (*fifo)->dspAdress, 0, 0, 0, 0);
+    }
+
+    if(skeleton != NULL)
+    {
+        /* Set Skeleton FIFO attribute (Error mut not occure) */
+        cm_writeAttribute(skeleton, "FIFO", (*fifo)->dspAdress);
+
+        LOG_INTERNAL(2, "  FIFO param %x:%x\n", *fifo, (*fifo)->dspAdress, 0, 0, 0, 0);
+    }
+
+    return CM_OK;
+}
+/**
+ *
+ */
+static void cm_destroyParamsFifo(t_nmf_fifo_arm_desc *fifo) {
+    fifo_free(fifo);
+}
+
+/*!
+ * Create DSP skeleton
+ */
+PRIVATE t_cm_error cm_createDSPSkeleton(
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType, //INTERNAL_XRAM24
+        t_elfdescription *elfhandleSkeleton,
+        t_dspskel_bf_info *bfInfo)
+{
+    t_interface_provide *provide = &itfProvide->server->template->provides[itfProvide->provideIndex];
+    t_interface_require_description skelitfRequire;
+    t_cm_error error;
+    unsigned int fifoeventsize = 0;
+
+    /* Instantiate related stub on dsp */
+    {
+        char stubTemplateName[4 + MAX_INTERFACE_TYPE_NAME_LENGTH + 1];
+
+        cm_StringCopy(stubTemplateName,"_sk.", sizeof(stubTemplateName));
+        cm_StringConcatenate(stubTemplateName, provide->interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+        if ((error = cm_instantiateComponent(
+                        stubTemplateName,
+                        itfProvide->server->domainId,
+                        itfProvide->server->priority,
+                        skeletonDup,
+                        elfhandleSkeleton,
+                        &bfInfo->skelInstance)) != CM_OK) {
+            return ((error == CM_COMPONENT_NOT_FOUND)?CM_BINDING_COMPONENT_NOT_FOUND:error);
+        }
+    }
+
+    /* Get fifo elem size (which was store in TOP by convention) */
+    fifoeventsize = cm_readAttributeNoError(bfInfo->skelInstance, "TOP");
+    LOG_INTERNAL(3, "DspEvent Fifo element size = %d\n", fifoeventsize, 0, 0, 0, 0, 0);
+
+    /* Allocation of the itf event dsp fifo */
+    if ((error = dspevent_createDspEventFifo(
+                    bfInfo->skelInstance,
+                    "TOP",
+                    fifosize,
+                    fifoeventsize,
+                    dspEventMemType,
+                    &bfInfo->dspfifoHandle)) != CM_OK)
+    {
+	    cm_destroyInstance(bfInfo->skelInstance, DESTROY_WITHOUT_CHECK);
+        return error;
+    }
+
+    /* Bind stub to server component (Error must not occure) */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->skelInstance, "target", &skelitfRequire) == CM_OK);
+
+    cm_bindLowLevelInterface(&skelitfRequire, itfProvide, BF_SYNCHRONOUS, NULL);
+
+    return CM_OK;
+}
+
+/**
+ * Destroy DSP Skeleton
+ */
+PRIVATE t_cm_error cm_destroyDSPSkeleton(t_dspskel_bf_info *bfInfo) {
+    t_interface_require_description skelitfRequire;
+
+    /* Unbind explicitly stub from server component (Error must not occure) */
+    /* This is mandatory to fix the providedItfUsedCount of the server */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->skelInstance, "target", &skelitfRequire) == CM_OK);
+
+    cm_registerLowLevelInterfaceToConst(&skelitfRequire, NULL);
+
+    /* Destroy Event fifo */
+    dspevent_destroyDspEventFifo(bfInfo->dspfifoHandle);
+
+    /* Destroy Event Binding Component */
+    return cm_destroyInstance(bfInfo->skelInstance, DESTROY_WITHOUT_CHECK);
+}
+
+/*
+ *
+ */
+t_cm_error cm_bindComponentFromCMCore(
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription *elfhandleSkeleton,
+        t_host2mpc_bf_info **bfInfo) {
+    t_interface_provide *provide = &itfProvide->server->template->provides[itfProvide->provideIndex];
+    t_dsp_offset shareVarOffset;
+    t_cm_error error;
+
+    LOG_INTERNAL(1, "\n##### Bind HOST -> %s/%x.%s #####\n",
+		 itfProvide->server->pathname, itfProvide->server, itfProvide->origName, 0, 0, 0);
+
+    /* Allocate host2dsp binding factory information */
+    *bfInfo = (t_host2mpc_bf_info*)OSAL_Alloc(sizeof(t_host2mpc_bf_info));
+    if((*bfInfo) == 0)
+        return CM_NO_MORE_MEMORY;
+
+    /* Create the Skeleton */
+    if ((error = cm_createDSPSkeleton(itfProvide,
+                    fifo_normalizeDepth(fifosize), /* We SHALL create DSP Skeleton before creating the Params Fifo, but we need in advance the real depth of this fifo */
+                    dspEventMemType,
+                    elfhandleSkeleton,
+                    &(*bfInfo)->dspskeleton)) != CM_OK)
+    {
+        OSAL_Free((*bfInfo));
+        return error;
+    }
+
+    /* Create the FIFO Params */
+    if ((error = cm_createParamsFifo(NULL,
+                    (*bfInfo)->dspskeleton.skelInstance,
+                    itfProvide->server->domainId,
+                    fifosize,
+                    &(*bfInfo)->fifo,
+                    NULL,
+                    provide->interface->methodNumber)) != CM_OK)
+    {
+	cm_destroyDSPSkeleton(&(*bfInfo)->dspskeleton);
+        OSAL_Free((*bfInfo));
+        return error;
+    }
+
+    /* Set Target info in FIFO param to TOP */
+    shareVarOffset = cm_getAttributeMpcAddress((*bfInfo)->dspskeleton.skelInstance, "TOP");
+
+    /*
+     * Set Target info in FIFO param to armThis
+     * Should not return any error
+     */
+    fifo_params_setSharedField((*bfInfo)->fifo, 0, (t_shared_field)shareVarOffset /* ArmBCThis_or_TOP */);
+
+    /* Initialise FIFO Param bcDesc with Skeleton methods */
+    {
+        int i;
+        t_component_instance *skel = (*bfInfo)->dspskeleton.skelInstance;
+        for (i=0; i < provide->interface->methodNumber; i++)
+        {
+            /* should not return error */
+            fifo_params_setSharedField(
+                    (*bfInfo)->fifo,
+                    1+i,
+                    skel->template->provides[0].indexes[0][i].methodAddresses
+                    );
+        }
+    }
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_ASYNCHRONOUS,
+            ARM_TRACE_COMPONENT, itfProvide->server,
+            NULL,
+            itfProvide->server->template->provides[itfProvide->provideIndex].name);
+
+    return CM_OK;
+}
+
+void cm_unbindComponentFromCMCore(
+        t_host2mpc_bf_info* bfInfo) {
+    t_component_instance *skel = bfInfo->dspskeleton.skelInstance;
+    t_interface_reference* itfProvide = &skel->interfaceReferences[0][0];
+    t_interface_provide *provide = &itfProvide->instance->template->provides[itfProvide->provideIndex];
+
+    LOG_INTERNAL(1, "\n##### UnBind HOST -> %s/%x.%s #####\n",
+		 itfProvide->instance->pathname, itfProvide->instance, provide->name, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_ASYNCHRONOUS,
+            ARM_TRACE_COMPONENT, itfProvide->instance,
+            NULL,
+            itfProvide->instance->template->provides[itfProvide->provideIndex].name);
+
+    // Destroy FIFO params
+    cm_destroyParamsFifo(bfInfo->fifo);
+
+    // Destory Skeleton
+    cm_destroyDSPSkeleton(&bfInfo->dspskeleton);
+
+    // Free BF info (which contains bcDecr(==dspfct) and arm This)
+    OSAL_Free(bfInfo);
+}
+
+/**
+ * Create DSP Stub
+ */
+PRIVATE t_cm_error cm_createDSPStub(
+        const t_interface_require_description *itfRequire,
+        const char* itfType,
+        t_dspstub_bf_info* bfInfo,
+        t_elfdescription *elfhandleStub,
+        t_interface_provide_description *itfstubProvide) {
+    t_cm_error error;
+
+    /*
+     * Instantiate related skel on dsp
+     */
+    {
+        char skelTemplateName[4 + MAX_INTERFACE_TYPE_NAME_LENGTH + 1];
+
+        cm_StringCopy(skelTemplateName, "_st.", sizeof(skelTemplateName));
+        cm_StringConcatenate(skelTemplateName, itfType, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+        if ((error = cm_instantiateComponent(
+                        skelTemplateName,
+                        itfRequire->client->domainId,
+                        itfRequire->client->priority,
+                        stubDup,
+                        elfhandleStub,
+                        &bfInfo->stubInstance)) != CM_OK) {
+            return (error == CM_COMPONENT_NOT_FOUND)?CM_BINDING_COMPONENT_NOT_FOUND : error;
+        }
+    }
+
+    /* Get the internal component that serve this interface (Error must not occure) */
+    (void)cm_getProvidedInterface(bfInfo->stubInstance, "source", itfstubProvide);
+
+    return CM_OK;
+}
+
+PRIVATE t_cm_error cm_destroyDSPStub(
+        const t_interface_require_description *itfRequire,
+        t_dspstub_bf_info* bfInfo) {
+
+    /* Unbind Client from Event Binding Component */
+    cm_bindLowLevelInterfaceToConst(itfRequire,
+            0x0,
+            NULL);
+
+    /* Destroy Event Binding Component */
+    return cm_destroyInstance(bfInfo->stubInstance, DESTROY_WITHOUT_CHECK);
+}
+/*
+ *
+ */
+t_cm_error cm_bindComponentToCMCore(
+        const t_interface_require_description   *itfRequire,
+        t_uint32                                fifosize,
+        t_uint32                                context,
+        t_elfdescription                        *elfhandleStub,
+        t_mpc2host_bf_info                      ** bfInfo) {
+    t_interface_require *require = &itfRequire->client->template->requires[itfRequire->requireIndex];
+    t_interface_provide_description itfstubProvide;
+    t_cm_error error;
+    t_uint32 fifoelemsize;
+
+    LOG_INTERNAL(1, "\n##### Bind %s/%x.%s -> HOST #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    /* Allocate dsp2host binding factory information */
+    *bfInfo = (t_mpc2host_bf_info*)OSAL_Alloc(sizeof(t_mpc2host_bf_info));
+    if(*bfInfo == 0)
+        return CM_NO_MORE_MEMORY;
+    (*bfInfo)->context = context;
+
+    if ((error = cm_createDSPStub(itfRequire,
+                    require->interface->type,
+                    &(*bfInfo)->dspstub,
+                    elfhandleStub,
+                    &itfstubProvide)) != CM_OK)
+    {
+        OSAL_Free(*bfInfo);
+        return error;
+    }
+
+    /* Create the FIFO Params */
+    if ((error = cm_createParamsFifo(
+                    (*bfInfo)->dspstub.stubInstance,
+                    NULL,
+                    itfRequire->client->domainId,
+                    fifosize,
+                    &(*bfInfo)->fifo,
+                    &fifoelemsize,
+                    1)) != CM_OK) /* 1 => we used first field as max params size */
+    {
+	    cm_destroyDSPStub(itfRequire, &(*bfInfo)->dspstub);
+        OSAL_Free(*bfInfo);
+        return error;
+    }
+
+    /* Bind client to stub component (Error must not occure) */
+    cm_bindLowLevelInterface(itfRequire, &itfstubProvide, BF_DSP2HOST, *bfInfo);
+
+    /* Bind stub component to host (virtual bind) */
+    cm_bindVirtualInterface((*bfInfo)->dspstub.stubInstance, (t_component_instance*)NMF_HOST_COMPONENT);
+
+    /*
+     * Set Target info in FIFO param to armThis
+     * Initialise FIFO Param bcDesc with  Jumptable
+     * Should not return any error
+     */
+    fifo_params_setSharedField((*bfInfo)->fifo, 0, (t_shared_field)context /* ArmBCThis_or_TOP */);
+    fifo_params_setSharedField((*bfInfo)->fifo, 1, (t_shared_field)fifoelemsize * 2/* bcDescRef */);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_ASYNCHRONOUS,
+            itfRequire->client, ARM_TRACE_COMPONENT,
+            itfRequire->client->template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    return error;
+}
+
+void cm_unbindComponentToCMCore(
+        const t_interface_require_description   *itfRequire,
+        t_mpc2host_bf_info                      *bfInfo)
+{
+    LOG_INTERNAL(1, "\n##### UnBind %s/%x.%s -> HOST #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_ASYNCHRONOUS,
+            itfRequire->client, ARM_TRACE_COMPONENT,
+            itfRequire->client->template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    /* Unbind virtual interface coms */
+    cm_unbindVirtualInterface(bfInfo->dspstub.stubInstance);
+
+    // Destroy FIFO params
+    cm_destroyParamsFifo(bfInfo->fifo);
+
+    // Destroy DSP Stub
+    cm_destroyDSPStub(itfRequire, &bfInfo->dspstub);
+
+    /* Free BF info */
+    OSAL_Free(bfInfo);
+}
+
+/*!
+ *
+ */
+t_cm_error cm_bindInterfaceDistributed(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription                        *elfhandleSkeleton,
+        t_elfdescription                        *elfhandleStub) {
+    t_interface_require *require = &itfRequire->client->template->requires[itfRequire->requireIndex];
+    t_interface_provide_description itfstubProvide;
+    t_cm_error error;
+    t_mpc2mpc_bf_info *bfInfo;
+    t_dsp_offset shareVarOffset;
+
+    LOG_INTERNAL(1, "\n##### Bind Distributed %s/%x.%s -> %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+		 itfProvide->server->pathname, itfProvide->server, itfProvide->origName);
+
+    /* Allocate aynchronous binding factory information */
+    bfInfo = (t_mpc2mpc_bf_info*)OSAL_Alloc(sizeof(t_mpc2mpc_bf_info));
+    if(bfInfo == 0)
+        return CM_NO_MORE_MEMORY;
+
+    /* Create the Skeleton */
+    if ((error = cm_createDSPSkeleton(itfProvide,
+                    fifo_normalizeDepth(fifosize), /* We SHALL create DSP Skeleton before creating the Params Fifo, but we need in advance the real depth of this fifo */
+                    dspEventMemType,
+                    elfhandleSkeleton,
+                    &bfInfo->dspskeleton)) != CM_OK)
+    {
+        OSAL_Free(bfInfo);
+        return error;
+    }
+
+    // Create DSP Stub
+    if ((error = cm_createDSPStub(itfRequire,
+                    require->interface->type,
+                    &bfInfo->dspstub,
+                    elfhandleStub,
+                    &itfstubProvide)) != CM_OK)
+    {
+	cm_destroyDSPSkeleton(&bfInfo->dspskeleton);
+        OSAL_Free(bfInfo);
+        return error;
+    }
+
+    /* Bind client to stub component (Error must not occure) */
+    cm_bindLowLevelInterface(itfRequire, &itfstubProvide, BF_DSP2DSP, bfInfo);
+
+    /* Create the FIFO Params */
+    if ((error = cm_createParamsFifo(
+                    bfInfo->dspstub.stubInstance,
+                    bfInfo->dspskeleton.skelInstance,
+                    itfProvide->server->domainId,
+                    fifosize,
+                    &bfInfo->fifo,
+                    NULL,
+                    require->interface->methodNumber)) != CM_OK)
+    {
+	cm_destroyDSPStub(itfRequire, &bfInfo->dspstub);
+	cm_destroyDSPSkeleton(&bfInfo->dspskeleton);
+        OSAL_Free(bfInfo);
+        return error;
+    }
+
+    /* Bind stub component to host (virtual bind) */
+    cm_bindVirtualInterface(bfInfo->dspstub.stubInstance, bfInfo->dspskeleton.skelInstance);
+
+    /* Set Target info in FIFO param to TOP */
+    shareVarOffset = cm_getAttributeMpcAddress(bfInfo->dspskeleton.skelInstance, "TOP");
+
+    /*
+     * Set Target info in FIFO param to armThis
+     * Should not return any error
+     */
+    fifo_params_setSharedField(bfInfo->fifo, 0, (t_shared_field)shareVarOffset /* ArmBCThis_or_TOP */);
+
+    /* Initialise FIFO Param bcDesc with Skeleton methods */
+    {
+        int i;
+        t_component_instance *skel = bfInfo->dspskeleton.skelInstance;
+        for (i=0; i < require->interface->methodNumber; i++)
+        {
+            /* should not return error */
+            fifo_params_setSharedField(
+                    bfInfo->fifo,
+                    1+i,
+                    skel->template->provides[0].indexes[0][i].methodAddresses
+                    );
+        }
+    }
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_ASYNCHRONOUS,
+            itfRequire->client, itfProvide->server,
+            itfRequire->client->template->requires[itfRequire->requireIndex].name,
+            itfProvide->server->template->provides[itfProvide->provideIndex].name);
+
+    return CM_OK;
+}
+
+/*!
+ *
+ */
+void cm_unbindInterfaceDistributed(
+        const t_interface_require_description   *itfRequire,
+        t_mpc2mpc_bf_info                       *bfInfo)
+{
+    LOG_INTERNAL(1, "\n##### UnBind distributed %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_ASYNCHRONOUS,
+            itfRequire->client, NULL,
+            itfRequire->client->template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    /* Unbind virtual interface */
+    cm_unbindVirtualInterface(bfInfo->dspstub.stubInstance);
+
+    // Destroy FIFO params
+    cm_destroyParamsFifo(bfInfo->fifo);
+
+    // Destroy DSP Stub
+    cm_destroyDSPStub(itfRequire, &bfInfo->dspstub);
+
+    // Destory DSP Skeleton
+    cm_destroyDSPSkeleton(&bfInfo->dspskeleton);
+
+    // Destroy BF Info
+    OSAL_Free(bfInfo);
+}
+
+t_cm_error cm_bindInterfaceStaticInterrupt(
+        const t_nmf_core_id coreId,
+        const int interruptLine,
+        const t_component_instance *server,
+        const char* providedItfServerName
+)
+{
+    char requiredItfClientName[CM_IT_NAME_MAX_LENGTH];
+    t_component_instance *client = cm_EEM_getExecutiveEngine(coreId)->instance;
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_cm_error error;
+
+    //build it[%d] name
+    if (interruptLine < 0 || interruptLine > 255) {return CM_OUT_OF_LIMITS;}
+    cm_fillItName(interruptLine, requiredItfClientName);
+
+    //do binding
+    if ((error = cm_getRequiredInterface(client,requiredItfClientName,&itfRequire)) !=  CM_OK) {return error;}
+    if ((error = cm_getProvidedInterface(server,providedItfServerName,&itfProvide)) !=  CM_OK) {return error;}
+    if((error = cm_bindInterface(&itfRequire, &itfProvide)) != CM_OK) {return error;}
+
+    return CM_OK;
+}
+
+t_cm_error cm_unbindInterfaceStaticInterrupt(
+        const t_nmf_core_id coreId,
+        const int interruptLine
+)
+{
+    char requiredItfClientName[CM_IT_NAME_MAX_LENGTH];
+    t_component_instance *client = cm_EEM_getExecutiveEngine(coreId)->instance;
+    t_interface_require_description itfRequire;
+    t_cm_error error;
+
+    //build it[%d] name
+    if (interruptLine < 0 || interruptLine > 255) {return CM_OUT_OF_LIMITS;}
+    cm_fillItName(interruptLine, requiredItfClientName);
+
+    //do unbinding
+    if ((error = cm_getRequiredInterface(client,requiredItfClientName,&itfRequire)) !=  CM_OK) {return error;}
+    cm_unbindInterface(&itfRequire);
+
+    return CM_OK;
+}
+
+void cm_destroyRequireInterface(t_component_instance* component, t_nmf_client_id clientId)
+{
+    int i, j;
+
+    /*
+     * Special code for SINGLETON handling
+     */
+    if(component->template->classe == SINGLETON)
+    {
+        if(getNumberOfBind(component) > 0)
+            return;
+    }
+
+    for(i = 0; i < component->template->requireNumber; i++)
+    {
+        int nb = component->template->requires[i].collectionSize;
+        for(j = 0; j < nb; j++)
+        {
+            if(component->interfaceReferences[i][j].instance != NULL)
+            {
+                t_interface_reference* itfRef = &component->interfaceReferences[i][j];
+                t_interface_require_description itfRequire;
+
+                itfRequire.client = component;
+                itfRequire.requireIndex = i;
+                itfRequire.collectionIndex = j;
+                itfRequire.origName = component->template->requires[i].name;
+
+                switch (itfRef->bfInfoID) {
+                case BF_SYNCHRONOUS:
+                    /* Error ignored as it is always OK */
+                    cm_unbindInterface(&itfRequire);
+                    break;
+                case BF_TRACE:
+                    cm_unbindInterfaceTrace(&itfRequire,
+                            (t_trace_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+                    break;
+                case BF_ASYNCHRONOUS:
+                    cm_unbindInterfaceAsynchronous(&itfRequire,
+                            (t_async_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+                    break;
+                case BF_DSP2HOST: 
+                    /* This 'mpc2host handle' is provided by the host at OS Integration level.
+                           It must then be handled and released in OS specific part.
+                     */
+                    cm_unbindComponentToCMCore(&itfRequire,
+                            (t_mpc2host_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+                    break;
+                case BF_HOST2DSP:
+                    /* These bindings are from CM Core to DSP, they are not listed
+                           here and must be handles/freed by host at OS Integration level
+                     */
+                    break;
+                case BF_DSP2DSP:
+                    cm_unbindInterfaceDistributed(&itfRequire,
+                            (t_mpc2mpc_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+                    break;
+                default:
+                    break;
+                }
+            }
+        }
+    }
+}
+
+void cm_registerSingletonBinding(
+        t_component_instance*                   component,
+        t_interface_require_description*        itfRequire,
+        t_interface_provide_description*        itfProvide,
+        t_nmf_client_id                         clientId)
+{
+    if(component->template->classe == SINGLETON)
+    {
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, FALSE, clientId);
+        if(cl != NULL)
+            cl->numberOfBind++;
+
+        if(itfProvide != NULL)
+            LOG_INTERNAL(1, "  -> Singleton[%d] : Register binding %s/%x.%s -> %s/%x\n",
+                    clientId,
+                    itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+                    itfProvide->server->pathname, itfProvide->server);
+        else
+            LOG_INTERNAL(1, "  -> Singleton[%d] : Register binding %s/%x.%s -> ARM/VOID\n",
+                    clientId,
+                    itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0);
+    }
+}
+
+t_bool cm_unregisterSingletonBinding(
+        t_component_instance*                   component,
+        t_interface_require_description*        itfRequire,
+        t_interface_provide_description*        itfProvide,
+        t_nmf_client_id                         clientId)
+{
+    if(component->template->classe == SINGLETON)
+    {
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, FALSE, clientId);
+        if(cl != NULL)
+            cl->numberOfBind--;
+
+        if(itfProvide->server == (t_component_instance *)NMF_VOID_COMPONENT)
+            LOG_INTERNAL(1, "  -> Singleton[%d] : Unregister binding %s/%x.%s -> ARM/VOID\n",
+                    clientId,
+                    itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0);
+        else if(itfProvide->server == NULL)
+            LOG_INTERNAL(1, "  -> Singleton[%d] : Unregister binding %s/%x.%s -> ?? <already unbound>\n",
+                    clientId,
+                    itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0);
+        else
+            LOG_INTERNAL(1, "  -> Singleton[%d] : Unregister binding %s/%x.%s -> %s/%x\n",
+                    clientId,
+                    itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+                    itfProvide->server->pathname, itfProvide->server);
+
+        if(getNumberOfBind(component) == 0)
+        {
+            LOG_INTERNAL(1, "  -> Singleton[%d] : All required of %s/%x logically unbound, perform physical unbind\n",
+                    clientId, itfRequire->client->pathname, itfRequire->client, 0, 0, 0);
+
+            (void)cm_EEM_ForceWakeup(component->template->dspId);
+
+            // This is the last binding unbind all !!!
+            cm_destroyRequireInterface(component, clientId);
+
+            cm_EEM_AllowSleep(component->template->dspId);
+        }
+        else if(itfProvide->server != NULL)
+        {
+            t_interface_require* itfReq;
+            itfReq = &itfRequire->client->template->requires[itfRequire->requireIndex];
+            if((itfReq->requireTypes & OPTIONAL_REQUIRE) != 0x0)
+                return TRUE;
+        }
+
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+static t_uint16 getNumberOfBind(t_component_instance* component)
+{
+    t_uint16 bindNumber = 0;
+    struct t_client_of_singleton* cur = component->clientOfSingleton;
+
+    for( ; cur != NULL ; cur = cur->next)
+    {
+        bindNumber += cur->numberOfBind;
+    }
+
+    return bindNumber;
+}
+
+static void cm_fillItName(int interruptLine, char *itName)
+{
+    int divider = 10000;
+
+    *itName++ = 'i';
+    *itName++ = 't';
+    *itName++ = '[';
+
+    // Find first significant divider
+    while(divider > interruptLine)
+        divider /= 10;
+
+    // Compute number
+    do
+    {
+        *itName++ = "0123456789"[interruptLine / divider];
+        interruptLine %= divider;
+        divider /= 10;
+    } while(divider != 0);
+
+    *itName++ = ']';
+    *itName++ = '\0';
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/binder_check.c b/drivers/staging/nmf-cm/cm/engine/component/src/binder_check.c
new file mode 100644
index 00000000000..b141064c1c1
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/binder_check.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include "../inc/bind.h"
+#include <cm/engine/trace/inc/trace.h>
+
+#include <cm/engine/utils/inc/string.h>
+
+t_cm_error cm_checkValidClient(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        t_interface_require_description *itfRequire,
+        t_bool *bindable) {
+    t_cm_error error;
+
+    // Component LC state check
+    if (NULL == client)
+        return  CM_INVALID_COMPONENT_HANDLE;
+
+    // Check if the requiredItfClientName is required by client component
+    if ((error = cm_getRequiredInterface(client, requiredItfClientName, itfRequire)) != CM_OK)
+        return error;
+
+    // Check required interface not already binded
+    {
+        t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+
+        if(itfRef->instance != (t_component_instance*)NULL)
+        {
+            if(client->template->classe == SINGLETON)
+            {
+                // Singleton is immutable thus we can't rebind it, nevertheless it's not an issue
+                *bindable = FALSE;
+                return CM_OK;
+            }
+            else
+            {
+                t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+
+                if(itfRef->instance == (const t_component_instance*)NMF_VOID_COMPONENT)
+                    ERROR("CM_INTERFACE_ALREADY_BINDED(): Component (%s<%s>.s) already bound to VOID\n",
+                            client->pathname, client->template->name, requiredItfClientName, 0, 0, 0);
+                else
+                    ERROR("CM_INTERFACE_ALREADY_BINDED(): Component (%s<%s>.s) already bound to another server (%s<%s>.%s)\n",
+                            client->pathname, client->template->name, requiredItfClientName,
+                            itfRef->instance->pathname, itfRef->instance->template->name, itfRef->instance->template->provides[itfRef->provideIndex].name);
+                return CM_INTERFACE_ALREADY_BINDED;
+            }
+        }
+    }
+
+    // Delayed Component LC state check done only if not optional required interface or intrinsic one that has been solved by loader
+    {
+        t_interface_require* itfReq = &client->template->requires[itfRequire->requireIndex];
+
+        if((itfReq->requireTypes & (OPTIONAL_REQUIRE | INTRINSEC_REQUIRE)) == 0) {
+            if(client->state == STATE_RUNNABLE)
+                return CM_COMPONENT_NOT_STOPPED;
+        }
+    }
+
+    *bindable = TRUE;
+
+    return CM_OK;
+}
+
+t_cm_error cm_checkValidServer(
+        const t_component_instance* server,
+        const char* providedItfServerName,
+        t_interface_provide_description *itfProvide) {
+    t_cm_error error;
+
+    // Check if the components are initialized
+    //if (server->state == STATE_INSTANCIATED)
+    //    return CM_COMPONENT_NOT_INITIALIZED;
+    if(NULL == server)
+        return  CM_INVALID_COMPONENT_HANDLE;
+
+    // Check if the providedItfServerName is provided by server component
+    if((error = cm_getProvidedInterface(server, providedItfServerName, itfProvide)) != CM_OK)
+        return error;
+
+    return CM_OK;
+}
+
+t_cm_error cm_checkValidBinding(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        const t_component_instance* server,
+        const char* providedItfServerName,
+        t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide,
+        t_bool *bindable) {
+    t_interface_require *require;
+    t_interface_provide *provide;
+    t_cm_error error;
+
+    // Check Server
+    if((error = cm_checkValidServer(server, providedItfServerName, itfProvide)) != CM_OK)
+        return error;
+
+    // Check Client
+    if((error = cm_checkValidClient(client, requiredItfClientName, itfRequire, bindable)) != CM_OK)
+        return error;
+
+    // If this is a singleton which has been already bound check that next binding is at the same server
+    if(*bindable == FALSE
+            && client->template->classe == SINGLETON)
+    {
+        t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+        while( itfRef->instance != server
+                || itfRef->provideIndex != itfProvide->provideIndex
+                || itfRef->collectionIndex != itfProvide->collectionIndex )
+        {
+            if(itfRef->instance == (const t_component_instance*)NMF_VOID_COMPONENT)
+            {
+                ERROR("CM_INTERFACE_ALREADY_BINDED(): Singleton (%s<%s>.s) already bound to VOID\n",
+                        client->pathname, client->template->name, requiredItfClientName, 0, 0, 0);
+                return CM_INTERFACE_ALREADY_BINDED;
+            }
+            else if(itfRef->bfInfoID == BF_ASYNCHRONOUS || itfRef->bfInfoID == BF_TRACE)
+            {
+                t_interface_require_description eventitfRequire;
+                CM_ASSERT(cm_getRequiredInterface(itfRef->instance, "target", &eventitfRequire) == CM_OK);
+                itfRef = &itfRef->instance->interfaceReferences[eventitfRequire.requireIndex][eventitfRequire.collectionIndex];
+
+                // Go to see client of event if the same
+            }
+            else
+            {
+                ERROR("CM_INTERFACE_ALREADY_BINDED(): Singleton (%s<%s>.s) already bound to different server (%s<%s>.%s)\n",
+                        client->pathname, client->template->name, requiredItfClientName,
+                        itfRef->instance->pathname, itfRef->instance->template->name, itfRef->instance->template->provides[itfRef->provideIndex].name);
+                return CM_INTERFACE_ALREADY_BINDED;
+            }
+        }
+    }
+
+    // Check if provided and required type matches
+    require = &client->template->requires[itfRequire->requireIndex];
+    provide = &server->template->provides[itfProvide->provideIndex];
+    if(require->interface != provide->interface)
+    {
+        ERROR("CM_ILLEGAL_BINDING(%s, %s)\n", require->interface->type, provide->interface->type, 0, 0, 0, 0);
+        return CM_ILLEGAL_BINDING;
+    }
+
+    // Check if static required interface binded to singleton component
+    if((require->requireTypes & STATIC_REQUIRE) &&
+            (server->template->classe != SINGLETON))
+    {
+        ERROR("CM_ILLEGAL_BINDING(): Can't bind static required interface to not singleton component\n",
+                0, 0, 0, 0, 0, 0);
+        return CM_ILLEGAL_BINDING;
+    }
+
+    return CM_OK;
+}
+
+t_cm_error cm_checkValidUnbinding(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide) {
+    t_cm_error error;
+    t_interface_require* itfReq;
+
+    // Component LC state check
+    if (NULL == client)
+        return CM_INVALID_COMPONENT_HANDLE;
+
+    // Check if the requiredItfClientName is required by client component
+    if ((error = cm_getRequiredInterface(client, requiredItfClientName, itfRequire)) != CM_OK)
+        return error;
+
+    itfReq = &client->template->requires[itfRequire->requireIndex];
+
+    // Check if the requiredItfClientName is required by client component
+    if ((error = cm_lookupInterface(itfRequire, itfProvide)) != CM_OK)
+    {
+        // We allow to unbind optional required of singleton even if not binded, since it could have been unbound previously but we don't
+        // want to break bind singleton reference counter
+        if((client->template->classe == SINGLETON) &&
+                (itfReq->requireTypes & OPTIONAL_REQUIRE) != 0x0)
+            return CM_OK;
+
+        return error;
+    }
+
+    // Singleton is immutable, don't unbind it
+    if(client->template->classe == SINGLETON)
+        return CM_OK;
+
+    /* if interface is optionnal then allow unbinding even if not stop */
+    if((itfReq->requireTypes & OPTIONAL_REQUIRE) == 0x0)
+    {
+        if(client->state == STATE_RUNNABLE)
+            return CM_COMPONENT_NOT_STOPPED;
+    }
+
+    return CM_OK;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/component_wrapper.c b/drivers/staging/nmf-cm/cm/engine/component/src/component_wrapper.c
new file mode 100644
index 00000000000..07d20d0266e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/component_wrapper.c
@@ -0,0 +1,1297 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/api/component_engine.h>
+#include <cm/engine/api/communication_engine.h>
+
+#include <cm/engine/component/inc/bind.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/memory/inc/domain.h>
+
+#include <cm/engine/configuration/inc/configuration.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+
+/*
+ * Component mangement wrapping.
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_InstantiateComponent(
+        const char* templateName,
+        t_cm_domain_id domainId,
+        t_nmf_client_id clientId,
+        t_nmf_ee_priority priority,
+        const char localName[MAX_COMPONENT_NAME_LENGTH],
+        const char *dataFile,
+        t_cm_instance_handle *instance) {
+    t_cm_error error;
+    t_nmf_core_id coreId;
+    t_component_instance *comp;
+    t_elfdescription *elfhandle = NULL;
+
+    OSAL_LOCK_API();
+
+    /*
+     * Load Elf File
+     */
+    if(dataFile != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFile,
+                    TRUE,
+                    &elfhandle)) != CM_OK)
+        goto out;
+
+    //only allow instantiation in non-scratch domains (ie. DOMAIN_NORMAL)!
+    if ((error = cm_DM_CheckDomainWithClient(domainId, DOMAIN_NORMAL, clientId)) != CM_OK)
+        goto out;
+
+    coreId = cm_DM_GetDomainCoreId(domainId);
+
+    if(coreId < FIRST_MPC_ID || coreId > LAST_CORE_ID)
+    {
+        error = CM_INVALID_PARAMETER;
+        goto out;
+    }
+
+    if ((error = cm_CFG_CheckMpcStatus(coreId)) != CM_OK)
+        goto out;
+
+    if ((error = cm_EEM_ForceWakeup(coreId)) != CM_OK)
+        goto out;
+
+    error = cm_instantiateComponent(
+            templateName,
+            domainId,
+            priority,
+            localName,
+            elfhandle,
+            &comp);
+    if(error == CM_OK)
+        *instance = comp->instance;
+
+    cm_EEM_AllowSleep(coreId);
+
+out:
+    cm_ELF_CloseFile(TRUE, elfhandle);
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_StartComponent(
+        t_cm_instance_handle instance,
+        t_nmf_client_id clientId) {
+    t_cm_error error;
+    t_component_instance *component;
+
+    OSAL_LOCK_API();
+
+    component = cm_lookupComponent(instance);
+    if (NULL == component)
+        error = CM_INVALID_COMPONENT_HANDLE;
+    else
+    {
+        error = cm_startComponent(component, clientId);
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_StopComponent(
+        t_cm_instance_handle instance,
+        t_nmf_client_id clientId) {
+    t_cm_error error;
+    t_component_instance *component;
+
+    OSAL_LOCK_API();
+
+    component = cm_lookupComponent(instance);
+    if (NULL == component)
+        error = CM_INVALID_COMPONENT_HANDLE;
+    else
+    {
+        error = cm_stopComponent(component, clientId);
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_DestroyComponent(
+        t_cm_instance_handle instance,
+        t_nmf_client_id clientId)
+{
+    t_cm_error error;
+    t_component_instance *component;
+
+    OSAL_LOCK_API();
+
+    component = cm_lookupComponent(instance);
+    if (NULL == component)
+    {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    }
+    else
+    {
+        t_nmf_core_id coreId = component->template->dspId;
+
+        (void)cm_EEM_ForceWakeup(coreId);
+
+        error = cm_destroyInstanceForClient(component, DESTROY_NORMAL, clientId);
+
+        cm_CFG_ReleaseMpc(coreId);
+
+        cm_EEM_AllowSleep(coreId);
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_FlushComponents(t_nmf_client_id clientId)
+{
+    t_cm_error error = CM_OK;
+    t_component_instance *instance;
+    t_uint32 i;
+
+    if (clientId == 0)
+    	return CM_INVALID_PARAMETER;
+
+    OSAL_LOCK_API();
+
+    // We don't know exactly where components will be, wake up everybody !!
+    (void)cm_EEM_ForceWakeup(SVA_CORE_ID);
+    (void)cm_EEM_ForceWakeup(SIA_CORE_ID);
+
+    /* Destroy all host2mpc bindings */
+    for (i=0; i<Host2MpcBindingTable.idxMax; i++)
+    {
+	    t_host2mpc_bf_info* bfInfo;
+	    OSAL_LOCK_COM();
+	    bfInfo = Host2MpcBindingTable.entries[i];
+	    if ((bfInfo != NULL) && (bfInfo->clientId == clientId)) {
+		    cm_delEntry(&Host2MpcBindingTable, i);
+		    OSAL_UNLOCK_COM();
+		    cm_unbindComponentFromCMCore(bfInfo);
+	    } else
+		    OSAL_UNLOCK_COM();
+    }
+
+    /* First, stop all remaining components for this client */
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	if ((instance = componentEntry(i)) == NULL)
+		continue;
+        if (/* skip EE */
+                (instance->template->classe == FIRMWARE) ||
+                /* Skip all binding components */
+                (cm_StringCompare(instance->template->name, "_ev.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_st.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_sk.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_tr.", 4) == 0))
+            continue;
+
+        /*
+         * Special code for SINGLETON handling
+         */
+        if(instance->template->classe == SINGLETON)
+        {
+            struct t_client_of_singleton* cl = cm_getClientOfSingleton(instance, FALSE, clientId);
+            if(cl == NULL)
+                continue;
+
+            cl->numberOfStart = 1; // == 1 since it will go to 0 in cm_stopComponent
+            cl->numberOfInstance = 1; // == 1 since it will go to 0 in cm_destroyInstanceForClient
+            cl->numberOfBind = 0; // == 0 since we don't want anymore binding for this component
+        }
+        else if(domainDesc[instance->domainId].client != clientId)
+            /* Skip all components not belonging to our client */
+            continue;
+
+        // Stop the component
+        error = cm_stopComponent(instance, clientId);
+        if (error != CM_OK && error != CM_COMPONENT_NOT_STARTED)
+            LOG_INTERNAL(0, "Error stopping component %s/%x (%s, error=%d, client=%u)\n", instance->pathname, instance, instance->template->name, error, clientId, 0);
+
+        // Destroy dependencies
+        cm_destroyRequireInterface(instance, clientId);
+    }
+
+    /* Destroy all remaining components for this client */
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	if ((instance = componentEntry(i)) == NULL)
+		continue;
+        if (/* skip EE */
+                (instance->template->classe == FIRMWARE) ||
+                /* Skip all binding components */
+                (cm_StringCompare(instance->template->name, "_ev.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_st.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_sk.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_tr.", 4) == 0)) {
+		continue;
+        }
+
+
+       /*
+        * Special code for SINGLETON handling
+        */
+       if(instance->template->classe == SINGLETON)
+       {
+           struct t_client_of_singleton* cl = cm_getClientOfSingleton(instance, FALSE, clientId);
+           if(cl == NULL)
+               continue;
+       }
+       else if(domainDesc[instance->domainId].client != clientId)
+           /* Skip all components not belonging to our client */
+           continue;
+
+
+        // Destroy the component
+        error = cm_destroyInstanceForClient(instance, DESTROY_WITHOUT_CHECK, clientId);
+
+        if (error != CM_OK)
+        {
+            /* FIXME : add component name instance in log message but need to make a copy before cm_flushComponent()
+             *         because it's no more available after.
+             */
+            LOG_INTERNAL(0, "Error flushing component (error=%d, client=%u)\n", error, clientId, 0, 0, 0, 0);
+        }
+    }
+
+    cm_CFG_ReleaseMpc(SVA_CORE_ID);
+    cm_CFG_ReleaseMpc(SIA_CORE_ID);
+
+    cm_EEM_AllowSleep(SVA_CORE_ID);
+    cm_EEM_AllowSleep(SIA_CORE_ID);
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+/*
+ * Component binding wrapping.
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_BindComponent(
+        const t_cm_instance_handle clientInstance,
+        const char* requiredItfClientName,
+        const t_cm_instance_handle serverInstance,
+        const char* providedItfServerName,
+        t_bool traced,
+        t_nmf_client_id clientId,
+        const char *dataFileTrace) {
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_bool bindable;
+    t_cm_error error;
+    t_component_instance *client, *server;
+    t_elfdescription *elfhandleTrace = NULL;
+
+    OSAL_LOCK_API();
+
+    /*
+     * Load Elf File
+     */
+    if(dataFileTrace != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFileTrace,
+                    TRUE,
+                    &elfhandleTrace)) != CM_OK)
+        goto out;
+
+    client = cm_lookupComponent(clientInstance);
+    server = cm_lookupComponent(serverInstance);
+    // Sanity check
+    if((error = cm_checkValidBinding(client, requiredItfClientName,
+				     server, providedItfServerName,
+				     &itfRequire, &itfProvide, &bindable)) != CM_OK)
+        goto out;
+
+    // Check that client and server component run on same DSP
+    if (itfRequire.client->template->dspId != itfProvide.server->template->dspId)
+    {
+        error = CM_ILLEGAL_BINDING;
+        goto out;
+    }
+
+    // Check if we really need to bind
+    if(bindable)
+    {
+        if ((error = cm_EEM_ForceWakeup(itfRequire.client->template->dspId)) != CM_OK)
+            goto out;
+
+        /*
+         * Synchronous binding, so no binding component
+         */
+        if(traced)
+            error = cm_bindInterfaceTrace(&itfRequire, &itfProvide, elfhandleTrace);
+        else
+            error = cm_bindInterface(&itfRequire, &itfProvide);
+
+        cm_EEM_AllowSleep(itfRequire.client->template->dspId);
+    }
+
+    cm_registerSingletonBinding(client, &itfRequire, &itfProvide, clientId);
+
+out:
+    cm_ELF_CloseFile(TRUE, elfhandleTrace);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_UnbindComponent(
+        const t_cm_instance_handle clientInstance,
+        const char* requiredItfClientName,
+        t_nmf_client_id clientId) {
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_bf_info_ID bfInfoID;
+    t_cm_error error;
+    t_component_instance *client;
+
+    OSAL_LOCK_API();
+
+    client = cm_lookupComponent(clientInstance);
+    // Sanity check
+    if((error = cm_checkValidUnbinding(client, requiredItfClientName,
+				       &itfRequire, &itfProvide)) != CM_OK)
+        goto out;
+
+    // Check if this is a Primitive binding
+    bfInfoID = itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfoID;
+    if(bfInfoID != BF_SYNCHRONOUS && bfInfoID != BF_TRACE)
+    {
+        error = CM_ILLEGAL_UNBINDING;
+        goto out;
+    }
+
+    // Check if we really need to unbind
+    if(cm_unregisterSingletonBinding(client, &itfRequire, &itfProvide, clientId))
+    {
+        (void)cm_EEM_ForceWakeup(itfRequire.client->template->dspId);
+
+        if(bfInfoID == BF_SYNCHRONOUS)
+            cm_unbindInterface(&itfRequire);
+        else
+            cm_unbindInterfaceTrace(
+                    &itfRequire,
+                    (t_trace_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+
+        cm_EEM_AllowSleep(itfRequire.client->template->dspId);
+
+        error = CM_OK;
+    }
+
+out:
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_BindComponentToVoid(
+    const t_cm_instance_handle clientInstance,
+    const char                 requiredItfClientName[MAX_INTERFACE_NAME_LENGTH],
+    t_nmf_client_id clientId)
+{
+    t_interface_require_description itfRequire;
+    t_bool bindable;
+    t_cm_error error;
+    t_component_instance *client;
+
+    OSAL_LOCK_API();
+
+    client = cm_lookupComponent(clientInstance);
+    // Check invalid binding
+    if((error = cm_checkValidClient(client, requiredItfClientName,
+                    &itfRequire, &bindable)) != CM_OK)
+        goto out;
+
+   // Check if we really need to bind
+    if(bindable)
+    {
+        if ((error = cm_EEM_ForceWakeup(itfRequire.client->template->dspId)) != CM_OK)
+            goto out;
+
+        error = cm_bindInterfaceToVoid(&itfRequire);
+
+        cm_EEM_AllowSleep(itfRequire.client->template->dspId);
+    }
+
+    cm_registerSingletonBinding(client, &itfRequire, NULL, clientId);
+
+out:
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_BindComponentAsynchronous(
+        const t_cm_instance_handle clientInstance,
+        const char* requiredItfClientName,
+        const t_cm_instance_handle serverInstance,
+        const char* providedItfServerName,
+        t_uint32 fifosize,
+        t_cm_mpc_memory_type eventMemType,
+        t_nmf_client_id clientId,
+        const char *dataFileSkeletonOrEvent,
+        const char *dataFileStub) {
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_dsp_memory_type_id dspEventMemType;
+    t_bool bindable;
+    t_cm_error error;
+    t_component_instance *client, *server;
+    t_elfdescription *elfhandleSkeletonOrEvent = NULL;
+    t_elfdescription *elfhandleStub = NULL;
+
+    OSAL_LOCK_API();
+
+    /*
+     * Load Elf File
+     */
+    if(dataFileSkeletonOrEvent != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFileSkeletonOrEvent,
+                    TRUE,
+                    &elfhandleSkeletonOrEvent)) != CM_OK)
+        goto out;
+    if(dataFileStub != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFileStub,
+                    TRUE,
+                    &elfhandleStub)) != CM_OK)
+        goto out;
+
+    client = cm_lookupComponent(clientInstance);
+    server = cm_lookupComponent(serverInstance);
+    // Check invalid binding
+    if((error = cm_checkValidBinding(client, requiredItfClientName,
+				     server, providedItfServerName,
+				     &itfRequire, &itfProvide, &bindable)) != CM_OK)
+        goto out;
+
+    switch(eventMemType)
+    {
+    case CM_MM_MPC_TCM24_X:
+        dspEventMemType = INTERNAL_XRAM24;
+        break;
+    case CM_MM_MPC_ESRAM24:
+        dspEventMemType = ESRAM_EXT24;
+        break;
+    case CM_MM_MPC_SDRAM24:
+        dspEventMemType = SDRAM_EXT24;
+        break;
+    default:
+        error = CM_INVALID_PARAMETER;
+        goto out;
+    }
+
+    // Check if we really need to bind
+    if(bindable)
+    {
+        // Create the binding and bind it to the client (or all sub-components clients ....)
+        if (itfRequire.client->template->dspId != itfProvide.server->template->dspId)
+        {
+            if ((error = cm_EEM_ForceWakeup(itfRequire.client->template->dspId)) != CM_OK)
+                goto out;
+            if ((error = cm_EEM_ForceWakeup(itfProvide.server->template->dspId)) != CM_OK)
+            {
+                cm_EEM_AllowSleep(itfRequire.client->template->dspId);
+                goto out;
+            }
+
+            // This is a distribute communication
+            error = cm_bindInterfaceDistributed(
+                    &itfRequire,
+                    &itfProvide,
+                    fifosize,
+                    dspEventMemType,
+                    elfhandleSkeletonOrEvent,
+                    elfhandleStub);
+
+            cm_EEM_AllowSleep(itfRequire.client->template->dspId);
+            cm_EEM_AllowSleep(itfProvide.server->template->dspId);
+        }
+        else
+        {
+            if ((error = cm_EEM_ForceWakeup(itfRequire.client->template->dspId)) != CM_OK)
+                goto out;
+
+            // This is a acynchronous communication
+            error = cm_bindInterfaceAsynchronous(
+                    &itfRequire,
+                    &itfProvide,
+                    fifosize,
+                    dspEventMemType,
+                    elfhandleSkeletonOrEvent);
+
+            cm_EEM_AllowSleep(itfRequire.client->template->dspId);
+        }
+    }
+
+    cm_registerSingletonBinding(client, &itfRequire, &itfProvide, clientId);
+
+out:
+    cm_ELF_CloseFile(TRUE, elfhandleSkeletonOrEvent);
+    cm_ELF_CloseFile(TRUE, elfhandleStub);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentAsynchronous(
+        const t_cm_instance_handle instance,
+        const char* requiredItfClientName,
+        t_nmf_client_id clientId) {
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_bf_info_ID bfInfoID;
+    t_cm_error error;
+    t_component_instance *client;
+
+    OSAL_LOCK_API();
+
+    client = cm_lookupComponent(instance);
+    // Sanity check
+    if((error = cm_checkValidUnbinding(client, requiredItfClientName,
+				       &itfRequire, &itfProvide)) != CM_OK)
+        goto out;
+
+    bfInfoID = itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfoID;
+
+    // Check if we really need to unbind
+    if(cm_unregisterSingletonBinding(client, &itfRequire, &itfProvide, clientId))
+    {
+        // Check if this is a Asynchronous binding
+        if(bfInfoID == BF_DSP2DSP)
+        {
+            t_nmf_core_id clientDsp = itfRequire.client->template->dspId;
+            t_nmf_core_id serverDsp = itfProvide.server->template->dspId;
+
+            (void)cm_EEM_ForceWakeup(clientDsp);
+            (void)cm_EEM_ForceWakeup(serverDsp);
+
+            cm_unbindInterfaceDistributed(
+                    &itfRequire,
+                    (t_mpc2mpc_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+
+            cm_EEM_AllowSleep(clientDsp);
+            cm_EEM_AllowSleep(serverDsp);
+
+            error = CM_OK;
+        }
+        else if(bfInfoID == BF_ASYNCHRONOUS)
+        {
+            t_nmf_core_id clientDsp = itfRequire.client->template->dspId;
+
+            (void)cm_EEM_ForceWakeup(clientDsp);
+
+            cm_unbindInterfaceAsynchronous(
+                    &itfRequire,
+                    (t_async_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+
+            cm_EEM_AllowSleep(clientDsp);
+
+            error = CM_OK;
+        }
+        else
+            error = CM_ILLEGAL_UNBINDING;
+    }
+
+ out:
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_BindComponentFromCMCore(
+        const t_cm_instance_handle server,
+        const char* providedItfServerName,
+        t_uint32 fifosize,
+        t_cm_mpc_memory_type eventMemType,
+        t_cm_bf_host2mpc_handle *bfHost2mpcHdl,
+        t_nmf_client_id clientId,
+        const char *dataFileSkeleton) {
+    t_interface_provide_description itfProvide;
+    t_dsp_memory_type_id dspEventMemType;
+    t_cm_error error;
+    t_component_instance* component;
+    t_host2mpc_bf_info *bfInfo;
+    t_elfdescription *elfhandleSkeleton = NULL;
+
+    OSAL_LOCK_API();
+
+    /*
+     * Load Elf File
+     */
+    if(dataFileSkeleton != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFileSkeleton,
+                    TRUE,
+                    &elfhandleSkeleton)) != CM_OK)
+        goto out;
+
+    component = cm_lookupComponent(server);
+    // Check server validity
+    if((error = cm_checkValidServer(component, providedItfServerName,
+                    &itfProvide)) != CM_OK)
+        goto out;
+
+    if ((error = cm_EEM_ForceWakeup(itfProvide.server->template->dspId)) != CM_OK)
+        goto out;
+
+    switch(eventMemType)
+    {
+    case CM_MM_MPC_TCM24_X:
+        dspEventMemType = INTERNAL_XRAM24;
+        break;
+    case CM_MM_MPC_ESRAM24:
+        dspEventMemType = ESRAM_EXT24;
+        break;
+    case CM_MM_MPC_SDRAM24:
+        dspEventMemType = SDRAM_EXT24;
+        break;
+    default:
+        goto out;
+    }
+
+    error = cm_bindComponentFromCMCore(&itfProvide,
+				       fifosize,
+				       dspEventMemType,
+				       elfhandleSkeleton,
+				       &bfInfo);
+
+    cm_EEM_AllowSleep(itfProvide.server->template->dspId);
+
+out:
+    cm_ELF_CloseFile(TRUE, elfhandleSkeleton);
+    OSAL_UNLOCK_API();
+
+    if (error == CM_OK) {
+	    bfInfo->clientId = clientId;
+	    OSAL_LOCK_COM();
+	    *bfHost2mpcHdl = cm_addEntry(&Host2MpcBindingTable, bfInfo);
+	    if (*bfHost2mpcHdl == 0)
+		    error = CM_NO_MORE_MEMORY;
+	    OSAL_UNLOCK_COM();
+
+	    if (error != CM_OK) {
+		    OSAL_LOCK_API();
+		    (void)cm_EEM_ForceWakeup(itfProvide.server->template->dspId);
+		    cm_unbindComponentFromCMCore(bfInfo);
+		    cm_EEM_AllowSleep(itfProvide.server->template->dspId);
+		    OSAL_UNLOCK_API();
+	    }
+    }
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentFromCMCore(
+        t_cm_bf_host2mpc_handle bfHost2mpcId) {
+	t_host2mpc_bf_info* bfInfo;
+	t_nmf_core_id coreId;
+
+    OSAL_LOCK_COM();
+    bfInfo = cm_lookupEntry(&Host2MpcBindingTable, bfHost2mpcId);
+    if (bfInfo)
+	    cm_delEntry(&Host2MpcBindingTable, bfHost2mpcId & INDEX_MASK);
+    OSAL_UNLOCK_COM();
+    if (NULL == bfInfo)
+	    return  CM_INVALID_PARAMETER;
+
+    OSAL_LOCK_API();
+
+    // Check if this is a DSP to Host binding
+    //if(bfInfo->id != BF_HOST2DSP)
+    //    return CM_ILLEGAL_UNBINDING;
+    coreId = bfInfo->dspskeleton.skelInstance->template->dspId;
+
+    (void)cm_EEM_ForceWakeup(coreId);
+
+    cm_unbindComponentFromCMCore(bfInfo);
+
+    cm_EEM_AllowSleep(coreId);
+
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_BindComponentToCMCore(
+        const t_cm_instance_handle  instance,
+        const char                  *requiredItfClientName,
+        t_uint32                    fifosize,
+        t_nmf_mpc2host_handle       upLayerThis,
+        const char                  *dataFileStub,
+        t_cm_bf_mpc2host_handle     *mpc2hostId,
+        t_nmf_client_id             clientId) {
+    t_interface_require_description itfRequire;
+    t_bool bindable;
+    t_cm_error error;
+    t_component_instance* client;
+    t_elfdescription *elfhandleStub = NULL;
+
+    OSAL_LOCK_API();
+
+    /*
+     * Load Elf File
+     */
+    if(dataFileStub != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFileStub,
+                    TRUE,
+                    &elfhandleStub)) != CM_OK)
+        goto out;
+
+    client = cm_lookupComponent(instance);
+    // Check invalid binding
+    if((error = cm_checkValidClient(client, requiredItfClientName,
+                    &itfRequire, &bindable)) != CM_OK)
+        goto out;
+
+    // Check if we really need to bind
+    if(bindable)
+    {
+        if ((error = cm_EEM_ForceWakeup(itfRequire.client->template->dspId)) != CM_OK)
+            goto out;
+
+        error = cm_bindComponentToCMCore(
+                &itfRequire,
+                fifosize,
+                upLayerThis,
+                elfhandleStub,
+                (t_mpc2host_bf_info**)mpc2hostId);
+
+        cm_EEM_AllowSleep(itfRequire.client->template->dspId);
+    }
+
+    cm_registerSingletonBinding(client, &itfRequire, NULL, clientId);
+
+out:
+    cm_ELF_CloseFile(TRUE, elfhandleStub);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentToCMCore(
+        const t_cm_instance_handle      instance,
+        const char                      *requiredItfClientName,
+        t_nmf_mpc2host_handle           *upLayerThis,
+        t_nmf_client_id clientId) {
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_cm_error error;
+    t_mpc2host_bf_info *bfInfo;
+    t_component_instance* client;
+
+    OSAL_LOCK_API();
+
+    client = cm_lookupComponent(instance);
+    // Sanity check
+    if((error = cm_checkValidUnbinding(client, requiredItfClientName,
+				       &itfRequire, &itfProvide)) != CM_OK)
+        goto out;
+
+    // Check if this is a DSP to Host binding
+    if(itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfoID != BF_DSP2HOST)
+    {
+        error = CM_ILLEGAL_UNBINDING;
+        goto out;
+    }
+
+    bfInfo = (t_mpc2host_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo;
+
+    // Get client information
+    *upLayerThis = bfInfo->context;
+
+    // Check if we really need to unbind
+    if(cm_unregisterSingletonBinding(client, &itfRequire, &itfProvide, clientId))
+    {
+        (void)cm_EEM_ForceWakeup(itfRequire.client->template->dspId);
+
+        cm_unbindComponentToCMCore(&itfRequire, bfInfo);
+
+        cm_EEM_AllowSleep(itfRequire.client->template->dspId);
+
+        error = CM_OK;
+    }
+out:
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_event_params_handle CM_ENGINE_AllocEvent(t_cm_bf_host2mpc_handle host2mpcId) {
+    t_host2mpc_bf_info* bfInfo;
+    t_event_params_handle eventHandle;
+
+    OSAL_LOCK_COM();
+    bfInfo = cm_lookupEntry(&Host2MpcBindingTable, host2mpcId);
+    if (NULL == bfInfo) {
+	    OSAL_UNLOCK_COM();
+	    return NULL;
+    }
+
+    if(bfInfo->dspskeleton.skelInstance->interfaceReferences[0][0].instance->state != STATE_RUNNABLE) {
+        ERROR("CM_COMPONENT_NOT_STARTED: Call interface before start component %s<%s>\n",
+                bfInfo->dspskeleton.skelInstance->pathname,
+                bfInfo->dspskeleton.skelInstance->template->name, 0, 0, 0, 0);
+    }
+
+    eventHandle = cm_AllocEvent(bfInfo->fifo);
+
+    OSAL_UNLOCK_COM();
+
+    return eventHandle;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_PushEvent(t_cm_bf_host2mpc_handle host2mpcId, t_event_params_handle h, t_uint32 methodIndex) {
+	t_host2mpc_bf_info* bfInfo;
+    t_cm_error error;
+
+    OSAL_LOCK_COM();
+    bfInfo = cm_lookupEntry(&Host2MpcBindingTable, host2mpcId);
+    if (NULL == bfInfo) {
+	    OSAL_UNLOCK_COM();
+	    return CM_INVALID_PARAMETER;
+    }
+    error = cm_PushEvent(bfInfo->fifo, h, methodIndex);
+    OSAL_UNLOCK_COM();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED void CM_ENGINE_AcknowledgeEvent(t_cm_bf_mpc2host_handle mpc2hostId) {
+    t_mpc2host_bf_info* bfInfo = (t_mpc2host_bf_info*)mpc2hostId;
+
+    //t_dsp2host_bf_info* bfInfo = (t_host2mpc_bf_info*)mpc2hostId;
+    OSAL_LOCK_COM();
+    cm_AcknowledgeEvent(bfInfo->fifo);
+    OSAL_UNLOCK_COM();
+}
+
+/*
+ * Get a reference on a given attribute of a given component
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_ReadComponentAttribute(
+        const t_cm_instance_handle instance,
+        const char* attrName,
+        t_uint24 *attrValue)
+{
+    t_cm_error error;
+    t_component_instance* component;
+
+    OSAL_LOCK_API();
+
+    component = cm_lookupComponent(instance);
+    if (NULL == component)
+        error = CM_INVALID_COMPONENT_HANDLE;
+    else
+    {
+        if ((error = cm_EEM_ForceWakeup(component->template->dspId)) != CM_OK)
+            goto out;
+
+        // t_uint24 -> t_uint32 possible since we know it same size
+        error = cm_readAttribute(component, attrName, (t_uint32*)attrValue);
+
+        cm_EEM_AllowSleep(component->template->dspId);
+    }
+
+out:
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+/*===============================================================================
+ * Introspection API
+ *===============================================================================*/
+/*
+ * Component
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentListHeader(
+    const t_nmf_client_id       client,
+    t_cm_instance_handle        *headerComponent) {
+    t_uint32 i;
+
+    OSAL_LOCK_API();
+
+    *headerComponent = 0;
+    for (i=0; i < ComponentTable.idxMax; i++) {
+	    if ((componentEntry(i) != NULL) &&
+		(componentEntry(i)->template->classe != FIRMWARE) &&
+		(domainDesc[componentEntry(i)->domainId].client == client)) {
+		    *headerComponent = ENTRY2HANDLE(componentEntry(i), i);;
+		    break;  
+	    }
+    }
+
+    OSAL_UNLOCK_API();
+
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentListNext(
+    const t_nmf_client_id       client,
+    const t_cm_instance_handle  prevComponent,
+    t_cm_instance_handle        *nextComponent){
+    t_cm_error error;
+    t_uint32 i = prevComponent & INDEX_MASK;
+
+    OSAL_LOCK_API();
+
+    // Sanity check
+    if ((i >= ComponentTable.idxMax)
+	|| (((unsigned int)componentEntry(i) << INDEX_SHIFT) != (prevComponent & ~INDEX_MASK)))
+        error =  CM_INVALID_COMPONENT_HANDLE;
+    else {
+	*nextComponent = 0;
+	for (i++; i < ComponentTable.idxMax; i++) {
+	    if ((componentEntry(i) != NULL) &&
+		(componentEntry(i)->template->classe != FIRMWARE) &&
+		(domainDesc[componentEntry(i)->domainId].client == client)) {
+		    *nextComponent = ENTRY2HANDLE(componentEntry(i), i);;
+		    break;  
+	    }
+	}
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentDescription(
+        const t_cm_instance_handle  instance,
+        char                        *templateName,
+        t_uint32                    templateNameLength,
+        t_nmf_core_id               *coreId,
+        char                        *localName,
+        t_uint32                    localNameLength,
+	t_nmf_ee_priority           *priority) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else {
+        cm_StringCopy(
+                templateName,
+                comp->template->name,
+                templateNameLength);
+        *coreId = comp->template->dspId;
+        cm_StringCopy(
+                localName,
+                comp->pathname,
+                localNameLength);
+	if (priority)
+		*priority = comp->priority;
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+/*
+ * Require interface
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterfaceNumber(
+    const t_cm_instance_handle  instance,
+    t_uint8                     *numberRequiredInterfaces) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else {
+        *numberRequiredInterfaces = comp->template->requireNumber;
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterface(
+        const t_cm_instance_handle  instance,
+        const t_uint8               index,
+        char                        *itfName,
+        t_uint32                    itfNameLength,
+        char                        *itfType,
+        t_uint32                    itfTypeLength,
+        t_cm_require_state          *requireState,
+        t_sint16                    *collectionSize) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else if(index >= comp->template->requireNumber) {
+        error = CM_NO_SUCH_REQUIRED_INTERFACE;
+    } else {
+        cm_StringCopy(
+                itfName,
+                comp->template->requires[index].name,
+                itfNameLength);
+        cm_StringCopy(
+                itfType,
+                comp->template->requires[index].interface->type,
+                itfTypeLength);
+        if(comp->template->requires[index].requireTypes & COLLECTION_REQUIRE)
+            *collectionSize = comp->template->requires[index].collectionSize;
+        else
+            *collectionSize = -1;
+
+	if(requireState != NULL) {
+		*requireState = 0;
+		if(comp->template->requires[index].requireTypes & COLLECTION_REQUIRE)
+			*requireState |= CM_REQUIRE_COLLECTION;
+        	if(comp->template->requires[index].requireTypes & OPTIONAL_REQUIRE)
+                	*requireState |= CM_REQUIRE_OPTIONAL;
+        	if(comp->template->requires[index].requireTypes & STATIC_REQUIRE)
+                	*requireState |= CM_REQUIRE_STATIC;
+	}
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterfaceBinding(
+        const t_cm_instance_handle  instance,
+        const char                  *itfName,
+        t_cm_instance_handle        *server,
+        char                        *serverItfName,
+        t_uint32                    serverItfNameLength) {
+    t_component_instance *comp;
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if(NULL == comp) {
+         error = CM_INVALID_COMPONENT_HANDLE;
+    } else if ((error = cm_getRequiredInterface(comp, itfName, &itfRequire)) != CM_OK) {
+        // Check if the requiredItfClientName is required by client component
+    } else if ((error = cm_lookupInterface(&itfRequire, &itfProvide)) != CM_OK) {
+        // Check if the requiredItfClientName is required by client component
+    } else {
+        if ((t_cm_instance_handle)itfProvide.server == NMF_HOST_COMPONENT
+               || (t_cm_instance_handle)itfProvide.server == NMF_VOID_COMPONENT)
+            *server = (t_cm_instance_handle)itfProvide.server;
+        else
+            *server = itfProvide.server->instance;
+        if(*server == NMF_HOST_COMPONENT) {
+            cm_StringCopy(
+                    serverItfName,
+                    "unknown",
+                    serverItfNameLength);
+        } else if(*server == NMF_VOID_COMPONENT) {
+            cm_StringCopy(
+                    serverItfName,
+                    "void",
+                    serverItfNameLength);
+        } else if(*server != 0) {
+            cm_StringCopy(
+                    serverItfName,
+                    itfProvide.server->template->provides[itfProvide.provideIndex].name,
+                    serverItfNameLength);
+            if(itfProvide.server->template->provides[itfProvide.provideIndex].provideTypes & COLLECTION_PROVIDE) {
+                int len = cm_StringLength(serverItfName, serverItfNameLength);
+                serverItfName[len++] = '[';
+                if(itfProvide.collectionIndex >= 100)
+                    serverItfName[len++] = '0' + (itfProvide.collectionIndex / 100);
+                if(itfProvide.collectionIndex >= 10)
+                    serverItfName[len++] = '0' + ((itfProvide.collectionIndex % 100) / 10);
+                serverItfName[len++] = '0' + (itfProvide.collectionIndex % 10);
+                serverItfName[len++] = ']';
+                serverItfName[len] = 0;
+            }
+        }
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+/*
+ * Provide interface
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentProvidedInterfaceNumber(
+    const t_cm_instance_handle  instance,
+    t_uint8                     *numberProvidedInterfaces) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else {
+        *numberProvidedInterfaces = comp->template->provideNumber;
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentProvidedInterface(
+        const t_cm_instance_handle  instance,
+        const t_uint8               index,
+        char                        *itfName,
+        t_uint32                    itfNameLength,
+        char                        *itfType,
+        t_uint32                    itfTypeLength,
+        t_sint16                    *collectionSize) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else if(index >= comp->template->provideNumber) {
+        error = CM_NO_SUCH_PROVIDED_INTERFACE;
+    } else {
+        cm_StringCopy(
+                itfName,
+                comp->template->provides[index].name,
+                itfNameLength);
+        cm_StringCopy(
+                itfType,
+                comp->template->provides[index].interface->type,
+                itfTypeLength);
+        if(comp->template->provides[index].provideTypes & COLLECTION_PROVIDE)
+            *collectionSize = comp->template->provides[index].collectionSize;
+        else
+            *collectionSize = -1;
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+/*
+ * Component Property
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyNumber(
+    const t_cm_instance_handle  instance,
+    t_uint8                     *numberProperties) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else {
+        *numberProperties = comp->template->propertyNumber;
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyName(
+        const t_cm_instance_handle  instance,
+        const t_uint8               index,
+        char                        *propertyName,
+        t_uint32                    propertyNameLength) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else if(index >= comp->template->propertyNumber) {
+        error = CM_NO_SUCH_PROPERTY;
+    } else {
+        cm_StringCopy(
+                propertyName,
+                comp->template->properties[index].name,
+                propertyNameLength);
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyValue(
+        const t_cm_instance_handle  instance,
+        const char                  *propertyName,
+        char                        *propertyValue,
+        t_uint32                    propertyValueLength)
+{
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    if (NULL == comp)
+        error = CM_INVALID_COMPONENT_HANDLE;
+    else
+    {
+        error = cm_getComponentProperty(
+            comp,
+            propertyName,
+            propertyValue,
+            propertyValueLength);
+
+        if(error == CM_NO_SUCH_PROPERTY)
+            ERROR("CM_NO_SUCH_PROPERTY(%s, %s)\n", comp->pathname, propertyName, 0, 0, 0, 0);
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/dspevent.c b/drivers/staging/nmf-cm/cm/engine/component/src/dspevent.c
new file mode 100644
index 00000000000..0c055c7f6f9
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/dspevent.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/communication/inc/communication.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h>
+#include <cm/engine/trace/inc/trace.h>
+#include "../inc/dspevent.h"
+
+
+#define DSP_REMOTE_EVENT_SIZE_IN_BYTE (4*DSP_REMOTE_EVENT_SIZE_IN_DSPWORD)
+#define DSP_REMOTE_EVENT_NEXT_FIELD_OFFSET      0
+#define DSP_REMOTE_EVENT_REACTION_FIELD_OFFSET  1
+#define DSP_REMOTE_EVENT_THIS_FIELD_OFFSET      2
+#define DSP_REMOTE_EVENT_PRIORITY_FIELD_OFFSET  3
+#define DSP_REMOTE_EVENT_DATA_FIELD_OFFSET      4
+
+t_cm_error dspevent_createDspEventFifo(
+    const t_component_instance *pComp,
+    const char* nameOfTOP,
+    t_uint32 fifoNbElem,
+    t_uint32 fifoElemSizeInWord,
+    t_dsp_memory_type_id dspEventMemType,
+    t_memory_handle *pHandle)
+{
+    t_uint32 dspElementAddr;
+    t_uint32 *elemAddr32;
+    int i;
+
+    // Allocate fifo
+    *pHandle = cm_DM_Alloc(pComp->domainId, dspEventMemType, fifoNbElem*fifoElemSizeInWord, CM_MM_ALIGN_2WORDS, TRUE);
+    if(*pHandle == INVALID_MEMORY_HANDLE)
+		return 	CM_NO_MORE_MEMORY;
+
+    cm_DSP_GetDspAddress(*pHandle, &dspElementAddr);
+
+    elemAddr32 = (t_uint32*)cm_DSP_GetHostLogicalAddress(*pHandle);
+
+    LOG_INTERNAL(2, "\n##### FIFO (dsp event): ARM=0x%x DSP=0x%x\n", elemAddr32, dspElementAddr, 0, 0, 0, 0);
+
+    // Read attribute addr (we assume that variable in XRAM)
+    cm_writeAttribute(pComp, nameOfTOP, dspElementAddr);
+
+    // Initialise the linked list (next...)
+    for (i = 0; i < fifoNbElem - 1; i++)
+    {
+        dspElementAddr += fifoElemSizeInWord;
+
+        /* Write next field  */
+        *elemAddr32 = dspElementAddr;
+        /* Write THIS field & priority field */
+        *(volatile t_uint64*)&elemAddr32[DSP_REMOTE_EVENT_THIS_FIELD_OFFSET] =
+                ((t_uint64)pComp->thisAddress | (((t_uint64)pComp->priority) << 32));
+
+        elemAddr32 += fifoElemSizeInWord;
+    }
+
+    /* Last element: Write next field */
+    *elemAddr32 = 0x0 /* NULL */;
+    /* Last element: Write THIS field & priority field */
+    *(volatile t_uint64*)&elemAddr32[DSP_REMOTE_EVENT_THIS_FIELD_OFFSET] =
+            ((t_uint64)pComp->thisAddress | (((t_uint64)pComp->priority) << 32));
+
+    return CM_OK;
+}
+
+
+
+void dspevent_destroyDspEventFifo(t_memory_handle handle)
+{
+    (void)cm_DM_Free(handle, TRUE);
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/initializer.c b/drivers/staging/nmf-cm/cm/engine/component/src/initializer.c
new file mode 100644
index 00000000000..7391669419e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/initializer.c
@@ -0,0 +1,329 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/communication/inc/communication.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h>
+
+#include <cm/engine/power_mgt/inc/power.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+#include <cm/engine/trace/inc/trace.h>
+
+#include "../inc/dspevent.h"
+#include "../inc/initializer.h"
+
+// Since now due to semaphore use call is synchrone so we only need a fifo size of three
+// (due to updateStack + (InstructionCacheLock or InstructionCacheUnlock))
+#define DEFAULT_INITIALIZER_FIFO_SIZE 3
+
+/* private prototype */
+PRIVATE t_cm_error cm_COMP_generic(t_nmf_core_id coreId, t_event_params_handle paramArray, t_uint32 paramNumber, t_uint32 serviceIndex);
+
+/*
+ * This module is tightly coupled with cm_DSP_components one (communication/initializer)
+ */
+static struct {
+    t_nmf_fifo_arm_desc* downlinkFifo;
+    t_nmf_fifo_arm_desc* uplinkFifo;
+    t_memory_handle dspfifoHandle;
+    t_nmf_osal_sem_handle fifoSemHandle;
+} initializerDesc[NB_CORE_IDS];
+
+PUBLIC t_cm_error cm_COMP_INIT_Init(t_nmf_core_id coreId)
+{
+    t_uint32 i;
+    t_cm_error error;
+    t_component_instance *ee;
+    t_dsp_offset sharedVarOffset;
+    t_interface_provide_description itfProvide;
+    t_interface_provide* provide;
+
+    ee = cm_EEM_getExecutiveEngine(coreId)->instance;
+
+    // Get interface description
+    if((error = cm_getProvidedInterface(ee, "service", &itfProvide)) != CM_OK)
+        return error;
+    provide = &ee->template->provides[itfProvide.provideIndex];
+
+
+    if ((error = dspevent_createDspEventFifo(
+                    ee, "comms/TOP",
+                    DEFAULT_INITIALIZER_FIFO_SIZE,
+                    DSP_REMOTE_EVENT_SIZE_IN_DSPWORD,
+                    INTERNAL_XRAM24,
+                    &initializerDesc[coreId].dspfifoHandle)) != CM_OK)
+        return error;
+
+    /* create fifo semaphore */
+    initializerDesc[coreId].fifoSemHandle = OSAL_CreateSemaphore(DEFAULT_INITIALIZER_FIFO_SIZE);
+    if (initializerDesc[coreId].fifoSemHandle == 0) {
+        dspevent_destroyDspEventFifo(initializerDesc[coreId].dspfifoHandle);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    /* static armTHis initialisation */
+    /*
+     * In the two next fifo_alloc call (1+n) means that we want to manage the hostThis_or_TOP and one method for each params fifos */
+    initializerDesc[coreId].downlinkFifo =
+        fifo_alloc(ARM_CORE_ID, coreId,
+                INIT_COMPONENT_CMD_SIZE, DEFAULT_INITIALIZER_FIFO_SIZE,
+                (1+provide->interface->methodNumber), paramsLocation,  extendedFieldLocation, cm_DSP_GetState(coreId)->domainEE);
+    if (initializerDesc[coreId].downlinkFifo == NULL)
+    {
+        OSAL_DestroySemaphore(initializerDesc[coreId].fifoSemHandle);
+        dspevent_destroyDspEventFifo(initializerDesc[coreId].dspfifoHandle);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    initializerDesc[coreId].uplinkFifo =
+        fifo_alloc(coreId, ARM_CORE_ID,
+                INIT_COMPONENT_ACK_SIZE, DEFAULT_INITIALIZER_FIFO_SIZE,
+                (1), paramsLocation, extendedFieldLocation, cm_DSP_GetState(coreId)->domainEE);              /* 1 is mandatory to compute internally the indexMask */
+                                                                                              /* this statement is acceptable only written by skilled man ;) */
+                                                                                              /* We don't used bcDescRef, since we assume that we don't need params size */
+    if (initializerDesc[coreId].uplinkFifo == NULL)
+    {
+        OSAL_DestroySemaphore(initializerDesc[coreId].fifoSemHandle);
+        fifo_free(initializerDesc[coreId].downlinkFifo);
+        dspevent_destroyDspEventFifo(initializerDesc[coreId].dspfifoHandle);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    cm_writeAttribute(ee, "comms/FIFOcmd", initializerDesc[coreId].downlinkFifo->dspAdress);
+
+    cm_writeAttribute(ee, "comms/FIFOack", initializerDesc[coreId].uplinkFifo->dspAdress);
+
+    sharedVarOffset = cm_getAttributeMpcAddress(ee, "comms/TOP");
+
+    /* HOST->DSP ParamsFifo extended fields initialisation */
+    fifo_params_setSharedField(
+            initializerDesc[coreId].downlinkFifo,
+            0,
+            (t_shared_field)sharedVarOffset /* TOP DSP Address */
+    );
+    for(i=0; i<provide->interface->methodNumber; i++)
+    {
+        fifo_params_setSharedField(
+                initializerDesc[coreId].downlinkFifo,
+                i + 1,
+                provide->indexes[itfProvide.collectionIndex][i].methodAddresses);
+    }
+
+    /* DSP->HOST ParamsFifo extended fields initialisation */
+    fifo_params_setSharedField(
+            initializerDesc[coreId].uplinkFifo,
+            0,
+            (t_shared_field)NMF_INTERNAL_USERTHIS
+            );
+
+    return CM_OK;
+}
+
+
+PUBLIC t_cm_error cm_COMP_CallService(
+        int serviceIndex,
+        t_component_instance *pComp,
+        t_uint32 methodAddress) {
+    t_cm_error error;
+    t_uint16 params[INIT_COMPONENT_CMD_SIZE];
+    t_bool isSynchronous = (serviceIndex == NMF_CONSTRUCT_SYNC_INDEX ||
+                            serviceIndex == NMF_START_SYNC_INDEX ||
+                            serviceIndex == NMF_DESTROY_INDEX)?TRUE:FALSE;
+
+    params[INIT_COMPONENT_CMD_HANDLE_INDEX] = (t_uint16)((unsigned int)pComp & 0xFFFF);
+    params[INIT_COMPONENT_CMD_HANDLE_INDEX+1] =  (t_uint16)((unsigned int)pComp >> 16);
+    params[INIT_COMPONENT_CMD_THIS_INDEX] =  (t_uint16)(pComp->thisAddress & 0xFFFF);
+    params[INIT_COMPONENT_CMD_THIS_INDEX+1] =  (t_uint16)(pComp->thisAddress >> 16);
+    params[INIT_COMPONENT_CMD_METHOD_INDEX] =  (t_uint16)(methodAddress & 0xFFFF);
+    params[INIT_COMPONENT_CMD_METHOD_INDEX+1] =  (t_uint16)(methodAddress >> 16);
+
+    error = cm_COMP_generic(pComp->template->dspId, params, sizeof(params) / sizeof(t_uint16), serviceIndex);
+
+    if (isSynchronous == TRUE && error == CM_OK) {
+        if (OSAL_SEMAPHORE_WAIT_TIMEOUT(semHandle) != SYNC_OK)
+            error = CM_MPC_NOT_RESPONDING;
+    }
+
+    return error;
+}
+
+PUBLIC void cm_COMP_INIT_Close(t_nmf_core_id coreId)
+{
+    unsigned int i;
+
+    /* wait for semaphore to be sure it would not be touch later on */
+    /* in case of timeout we break and try to clean everythink */
+    for(i = 0; i < DEFAULT_INITIALIZER_FIFO_SIZE; i++) {
+        if (OSAL_SEMAPHORE_WAIT_TIMEOUT(initializerDesc[coreId].fifoSemHandle) != SYNC_OK)
+            break;
+    }
+
+    /* destroy semaphore */
+    OSAL_DestroySemaphore(initializerDesc[coreId].fifoSemHandle);
+
+    /* Unallocate initializerDesc[index].uplinkFifo */
+    /* (who is used in this particular case to store dummy (with no data space (only descriptor)) DSP->HOST params fifo */
+    fifo_free(initializerDesc[coreId].uplinkFifo);
+
+    /* Unallocate initializerDesc[index].downlinkFifo */
+    fifo_free(initializerDesc[coreId].downlinkFifo);
+
+    /* Unallocate initializerDesc[index].dspfifoHandle */
+    dspevent_destroyDspEventFifo(initializerDesc[coreId].dspfifoHandle);
+}
+
+PUBLIC void processAsyncAcknowledge(t_nmf_core_id coreId, t_event_params_handle pParam)
+{
+    cm_AcknowledgeEvent(initializerDesc[coreId].uplinkFifo);
+
+    OSAL_SemaphorePost(initializerDesc[coreId].fifoSemHandle,1);
+}
+
+PUBLIC void processSyncAcknowledge(t_nmf_core_id coreId, t_event_params_handle pParam)
+{
+    cm_AcknowledgeEvent(initializerDesc[coreId].uplinkFifo);
+
+    OSAL_SemaphorePost(initializerDesc[coreId].fifoSemHandle,1);
+    OSAL_SemaphorePost(semHandle,1);
+}
+
+PUBLIC t_cm_error cm_COMP_UpdateStack(
+    t_nmf_core_id coreId,
+    t_uint32 stackSize
+)
+{
+    t_uint16 params[2];
+
+    // Marshall parameter
+    params[0] =  (t_uint16)((unsigned int)stackSize & 0xFFFF);
+    params[1] =  (t_uint16)((unsigned int)stackSize >> 16);
+
+    return cm_COMP_generic(coreId, params, sizeof(params) / sizeof(t_uint16), NMF_UPDATE_STACK);
+}
+
+PUBLIC t_cm_error cm_COMP_ULPForceWakeup(
+    t_nmf_core_id coreId
+)
+{
+    t_cm_error error;
+
+    error = cm_COMP_generic(coreId, NULL, 0, NMF_ULP_FORCEWAKEUP);
+
+    if (error == CM_OK) {
+        if (OSAL_SEMAPHORE_WAIT_TIMEOUT(semHandle) != SYNC_OK)
+            error = CM_MPC_NOT_RESPONDING;
+    }
+
+    return error;
+}
+
+PUBLIC t_cm_error cm_COMP_ULPAllowSleep(
+    t_nmf_core_id coreId
+)
+{
+    return cm_COMP_generic(coreId, NULL, 0, NMF_ULP_ALLOWSLEEP);
+}
+
+PUBLIC t_cm_error cm_COMP_InstructionCacheLock(
+    t_nmf_core_id coreId,
+    t_uint32 mmdspAddr,
+    t_uint32 mmdspSize
+)
+{
+    t_uint16 params[4];
+    t_uint32 startAddr = cm_DSP_GetState(coreId)->locked_offset;
+    int way;
+
+    for(way = 1; startAddr < mmdspAddr + mmdspSize; startAddr += MMDSP_CODE_CACHE_WAY_SIZE, way++)
+    {
+        if(mmdspAddr < startAddr + MMDSP_CODE_CACHE_WAY_SIZE)
+        {
+            t_cm_error error;
+
+            // Marshall parameter
+            params[0] =  (t_uint16)((unsigned int)startAddr & 0xFFFF);
+            params[1] =  (t_uint16)((unsigned int)startAddr >> 16);
+            params[2] =  (t_uint16)((unsigned int)way & 0xFFFF);
+            params[3] =  (t_uint16)((unsigned int)way >> 16);
+
+             if((error = cm_COMP_generic(coreId, params, sizeof(params) / sizeof(t_uint16), NMF_LOCK_CACHE)) != CM_OK)
+                 return error;
+        }
+    }
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_COMP_InstructionCacheUnlock(
+    t_nmf_core_id coreId,
+    t_uint32 mmdspAddr,
+    t_uint32 mmdspSize
+)
+{
+    t_uint16 params[2];
+    t_uint32 startAddr = cm_DSP_GetState(coreId)->locked_offset;
+    int way;
+
+    for(way = 1; startAddr < mmdspAddr + mmdspSize; startAddr += MMDSP_CODE_CACHE_WAY_SIZE, way++)
+    {
+        if(mmdspAddr < startAddr + MMDSP_CODE_CACHE_WAY_SIZE)
+        {
+            t_cm_error error;
+
+            // Marshall parameter
+            params[0] =  (t_uint16)((unsigned int)way & 0xFFFF);
+            params[1] =  (t_uint16)((unsigned int)way >> 16);
+
+             if((error = cm_COMP_generic(coreId, params, sizeof(params) / sizeof(t_uint16), NMF_UNLOCK_CACHE)) != CM_OK)
+                 return error;
+        }
+    }
+
+    return CM_OK;
+}
+
+/* private method */
+PRIVATE t_cm_error cm_COMP_generic(
+        t_nmf_core_id coreId,
+    t_event_params_handle paramArray,
+    t_uint32 paramNumber,
+    t_uint32 serviceIndex
+)
+{
+    t_event_params_handle _xyuv_data;
+    t_cm_error error;
+    t_uint32 i;
+
+    // wait for an event in fifo
+    if (OSAL_SEMAPHORE_WAIT_TIMEOUT(initializerDesc[coreId].fifoSemHandle) != SYNC_OK)
+        return CM_MPC_NOT_RESPONDING;
+
+    // AllocEvent
+    if((_xyuv_data = cm_AllocEvent(initializerDesc[coreId].downlinkFifo)) == NULL)
+    {
+        ERROR("CM_INTERNAL_FIFO_OVERFLOW: service FIFO full\n", 0, 0, 0, 0, 0, 0);
+        error = CM_INTERNAL_FIFO_OVERFLOW;
+        goto unlock;
+    }
+
+    // Copy param
+    for(i=0;i<paramNumber;i++)
+        _xyuv_data[i] = paramArray[i];
+
+    OSAL_LOCK_COM();
+
+    // Send Command
+    error = cm_PushEventTrace(initializerDesc[coreId].downlinkFifo, _xyuv_data, serviceIndex,0);
+
+unlock:
+    OSAL_UNLOCK_COM();
+
+    return error;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/instantiater.c b/drivers/staging/nmf-cm/cm/engine/component/src/instantiater.c
new file mode 100644
index 00000000000..2f517a98179
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/instantiater.c
@@ -0,0 +1,819 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/component/inc/bind.h>
+#include <cm/engine/component/inc/initializer.h>
+
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/configuration/inc/configuration_status.h>
+
+#include <cm/engine/dsp/inc/dsp.h>
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/trace/inc/xtitrace.h>
+
+#include <cm/engine/memory/inc/domain.h>
+
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/utils/inc/mem.h>
+#include <cm/engine/utils/inc/convert.h>
+
+#include <cm/engine/power_mgt/inc/power.h>
+
+
+t_nmf_table ComponentTable; /**< list (table) of components */
+
+static t_uint32 cm_getMaxStackValue(t_component_instance *pComponent);
+static t_uint16 getNumberOfInstance(t_component_instance* component);
+static t_uint16 getNumberOfStart(t_component_instance* component);
+
+
+t_cm_error cm_COMP_Init(void) {
+	t_cm_error error;
+	error = cm_initTable(&ComponentTable);
+	if (error == CM_OK)
+		error = cm_initTable(&Host2MpcBindingTable);
+	return error;
+}
+
+void cm_COMP_Destroy(void) {
+	cm_destroyTable(&ComponentTable);
+	cm_destroyTable(&Host2MpcBindingTable);
+}
+
+/** cm_addComponent - Add an internal handler to the list
+ *
+ *  1. Increase the size of the list if it's full
+ *  2. Search an empty entry
+ *  3. Add the element to the list
+ *  4. Compute and return the "user handle" (= t_cm_instance_handle)
+ */
+static t_cm_instance_handle cm_addComponent(t_component_instance *comp)
+{
+	OSAL_DisableServiceMessages();
+	comp->instance = cm_addEntry(&ComponentTable, comp);
+	OSAL_EnableServiceMessages();
+
+	return comp->instance;
+}
+
+/** cm_delComponent - remove the given component from the list
+ *
+ *  1. Check if the handle is valid
+ *  2. Search the entry and free it
+ */
+static void cm_delComponent(t_component_instance *comp)
+{
+	if (comp == NULL)
+		return;
+
+	OSAL_DisableServiceMessages();
+	cm_delEntry(&ComponentTable, comp->instance & INDEX_MASK);
+	OSAL_EnableServiceMessages();
+}
+
+/** cm_lookupComponent - search the component corresponding to
+ *                       the component instance.
+ *
+ * 1. Check if the instance is valid
+ * 2. Return a pointer to the component
+ */
+t_component_instance *cm_lookupComponent(const t_cm_instance_handle hdl)
+{
+	return cm_lookupEntry(&ComponentTable, hdl);
+}
+
+static void cm_DestroyComponentMemory(t_component_instance *component)
+{
+    int i;
+
+    /*
+     * Remove instance from list
+     */
+    cm_delComponent(component);
+
+    /*
+     * Destroy instance
+     */
+    {
+        struct t_client_of_singleton* cur = component->clientOfSingleton;
+
+        for( ; cur != NULL ; )
+        {
+            struct t_client_of_singleton* tmp = cur;
+            cur = cur->next;
+
+            OSAL_Free(tmp);
+        }
+    }
+
+    for(i = 0; i < component->template->requireNumber; i++)
+    {
+        OSAL_Free(component->interfaceReferences[i]);
+    }
+
+    cm_StringRelease(component->pathname);
+
+    cm_ELF_FreeInstance(component->template->dspId, component->template->memories, component->memories);
+
+    cm_unloadComponent(component->template);
+    OSAL_Free(component);
+}
+
+/**
+ * Non-Require:
+ *    - MMDSP could be sleep (Since we access it only through HSEM)
+ */
+void cm_delayedDestroyComponent(t_component_instance *component) {
+    int i;
+
+    /*
+     * Remove component from load map here
+     */
+    cm_DSPABI_RemoveLoadMap(
+            component->domainId,
+            component->template->name,
+            component->memories,
+            component->pathname,
+            component);
+
+    // Generate XTI/STM trace
+    cm_TRC_traceLoadMap(TRACE_COMPONENT_COMMAND_REMOVE, component);
+
+    /*
+     * disconnect interrupt handler if needed
+     */
+    for(i = 0; i < component->template->provideNumber; i++)
+    {
+        if(component->template->provides[i].interruptLine)
+        {
+            cm_unbindInterfaceStaticInterrupt(component->template->dspId, component->template->provides[i].interruptLine);
+        }
+    }
+
+    /*
+     * Update dsp stack size if needed
+     */
+    if (component->template->minStackSize > MIN_STACK_SIZE)
+    {
+        if (cm_EEM_isStackUpdateNeed(component->template->dspId, component->priority, FALSE, component->template->minStackSize))
+        {
+            t_uint32 newStackValue;
+            t_uint32 maxComponentStackSize;
+
+            maxComponentStackSize = cm_getMaxStackValue(component);
+            cm_EEM_UpdateStack(component->template->dspId, component->priority, maxComponentStackSize, &newStackValue);
+	    if (cm_DSP_GetState(component->template->dspId)->state == MPC_STATE_BOOTED)
+		    cm_COMP_UpdateStack(component->template->dspId, newStackValue);
+        }
+    }
+
+    cm_DestroyComponentMemory(component);
+}
+
+/**
+ * Pre-Require:
+ *    - MMDSP wakeup (when loading in TCM)
+ */
+t_cm_error cm_instantiateComponent(const char* templateName,
+                                   t_cm_domain_id domainId,
+                                   t_nmf_ee_priority priority,
+                                   const char* pathName,
+                                   t_elfdescription *elfhandle,
+                                   t_component_instance** refcomponent)
+{
+    t_nmf_core_id coreId = cm_DM_GetDomainCoreId(domainId);
+    t_dup_char templateNameDup;
+    t_component_template* template;
+    t_component_instance *component;
+    /* coverity[var_decl] */
+    t_cm_error error;
+    int i, j, k;
+
+    *refcomponent = NULL;
+
+    templateNameDup = cm_StringDuplicate(templateName);
+    if(templateNameDup == NULL)
+        return CM_NO_MORE_MEMORY;
+
+    /*
+     * Lookup in template list
+     */
+    template = cm_lookupTemplate(coreId, templateNameDup);
+    if(template != NULL)
+    {
+        if(template->classe == SINGLETON)
+        {
+            // Return same handle for singleton component
+            struct t_client_of_singleton* cl;
+
+            cm_StringRelease(templateNameDup);
+
+            cl = cm_getClientOfSingleton(template->singletonIfAvaliable, TRUE, domainDesc[domainId].client);
+            if(cl == NULL)
+                return CM_NO_MORE_MEMORY;
+            cl->numberOfInstance++;
+
+            *refcomponent = template->singletonIfAvaliable;
+            LOG_INTERNAL(1, "##### Singleton : New handle of %s/%x component on %s provItf=%d#####\n",
+                    template->singletonIfAvaliable->pathname, template->singletonIfAvaliable, cm_getDspName(coreId),
+                    template->singletonIfAvaliable->providedItfUsedCount, 0, 0);
+            return CM_OK;
+        }
+    }
+
+    // Get the dataFile (identity if already pass as parameter)
+    if((elfhandle = cm_REP_getComponentFile(templateNameDup, elfhandle)) == NULL)
+    {
+        cm_StringRelease(templateNameDup);
+        return CM_COMPONENT_NOT_FOUND;
+    }
+
+    // Load template
+    if((error = cm_loadComponent(templateNameDup, domainId, elfhandle, &template)) != CM_OK)
+    {
+        cm_StringRelease(templateNameDup);
+        return error;
+    }
+
+    // templateNameDup no more used, release it
+    cm_StringRelease(templateNameDup);
+
+    // Allocated component
+    component = (t_component_instance*)OSAL_Alloc_Zero(
+            sizeof(t_component_instance) +
+            sizeof(t_interface_reference*) * template->requireNumber);
+    if(component == NULL)
+    {
+        cm_unloadComponent(template);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    component->interfaceReferences = (t_interface_reference**)((char*)component + sizeof(t_component_instance));
+    component->template = template;
+
+    /*
+     * Update linked list
+     */
+    if (cm_addComponent(component) == 0) {
+        cm_unloadComponent(template);
+        OSAL_Free(component);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    // NOTE: From here use cm_DestroyComponentMemory
+
+    component->pathname = pathName ? cm_StringDuplicate(pathName) : cm_StringReference(anonymousDup);
+    if(component->pathname == NULL)
+    {
+        cm_DestroyComponentMemory(component);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    LOG_INTERNAL(1, "\n##### Instantiate %s/%x (%s) component on %s at priority %d #####\n", component->pathname, component, template->name, cm_getDspName(coreId), priority, 0);
+
+    if((error = cm_ELF_LoadInstance(domainId, elfhandle, template->memories, component->memories)) != CM_OK)
+    {
+        cm_DestroyComponentMemory(component);
+        return error;
+    }
+
+    if((error = cm_ELF_relocatePrivateSegments(
+            component->memories,
+            elfhandle,
+            template)) != CM_OK)
+    {
+        cm_DestroyComponentMemory(component);
+        return error;
+    }
+
+    cm_ELF_FlushInstance(coreId, template->memories, component->memories);
+
+    /*
+     * Create a new component instance
+     */
+    component->domainId = domainId;
+    component->priority = priority;
+    component->thisAddress = 0xFFFFFFFF;
+    component->state = STATE_NONE;
+
+    if(component->template->classe == SINGLETON)
+    {   // Return same handle for singleton component
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, TRUE, domainDesc[domainId].client);
+        if(cl == NULL)
+        {
+            cm_DestroyComponentMemory(component);
+            return CM_NO_MORE_MEMORY;
+        }
+
+        cl->numberOfInstance = 1;
+        template->singletonIfAvaliable = component;
+    }
+
+    if(component->memories[template->thisMemory->id] != INVALID_MEMORY_HANDLE)
+        cm_DSP_GetDspAddress(component->memories[template->thisMemory->id], &component->thisAddress);
+    else {
+        // In case of singleton or component without data
+        component->thisAddress = 0;
+    }
+
+    /*
+     * Create empty required interfaces array and set method interface to Panic
+     */
+    for(i = 0; i < template->requireNumber; i++) // For all required interface
+    {
+        component->interfaceReferences[i] =
+                (t_interface_reference*)OSAL_Alloc_Zero(sizeof(t_interface_reference) * template->requires[i].collectionSize);
+        if(component->interfaceReferences[i] == NULL)
+        {
+            cm_DestroyComponentMemory(component);
+            return CM_NO_MORE_MEMORY;
+        }
+
+        for(j = 0; j < template->requires[i].collectionSize; j++) // ... and for each index in collection (set THIS&method for each client)
+        {
+            component->interfaceReferences[i][j].instance = NULL;
+            component->interfaceReferences[i][j].bfInfoID = BF_SYNCHRONOUS; // Just to memorize no Binding component used and unbind ToVoid happy ;-).
+
+            if(template->classe == COMPONENT && template->requires[i].indexes != NULL)
+            {
+                // If component, fill THIS to itself to detect UNBINDED panic with rigth DSP
+                t_interface_require_index *requireindex = &template->requires[i].indexes[j];
+                for(k = 0; k < requireindex->numberOfClient; k++)
+                {
+                    t_uint32 *hostAddr;
+
+                    hostAddr = (t_uint32*)(
+                            cm_DSP_GetHostLogicalAddress(
+                                    component->memories[requireindex->memories[k].memory->id]) +
+                                    requireindex->memories[k].offset * requireindex->memories[k].memory->memEntSize);
+                    *hostAddr++ = (t_uint32)component->thisAddress;
+                }
+            }
+        }
+    }
+
+    /*
+     * Inform debugger about new component
+     */
+    if ((error = cm_DSPABI_AddLoadMap(
+            domainId,
+            template->name,
+            component->pathname,
+            component->memories,
+            component)) != CM_OK)
+    {
+        cm_DestroyComponentMemory(component);
+        return error;
+    }
+
+    // Generate XTI/STM trace
+    cm_TRC_traceLoadMap(TRACE_COMPONENT_COMMAND_ADD, component);
+
+    // NOTE: From here use cm_delayedDestroyComponent
+
+    /*
+     * Relocate interrupt if this is an interrupt
+     */
+    for(i = 0; i < template->provideNumber; i++)
+    {
+        if(template->provides[i].interruptLine)
+        {
+            if ((error = cm_bindInterfaceStaticInterrupt(coreId,
+                    template->provides[i].interruptLine,
+                    component,
+                    template->provides[i].name)) != CM_OK)
+            {
+                cm_delayedDestroyComponent(component);
+                return error;
+            }
+        }
+    }
+
+    /*
+     * For first instance of a component; Update ee stack size if needed
+     */
+    if(template->classe != FIRMWARE && template->numberOfInstance == 1 && template->minStackSize > MIN_STACK_SIZE)
+    {
+        t_uint32 newStackValue;
+
+        if (cm_EEM_isStackUpdateNeed(template->dspId, priority, TRUE, template->minStackSize))
+        {
+            error = cm_EEM_UpdateStack(template->dspId, priority, template->minStackSize, &newStackValue);
+            if (error != CM_OK)
+            {
+                cm_delayedDestroyComponent(component);
+                return error;
+            }
+            cm_COMP_UpdateStack(template->dspId, newStackValue);
+        }
+    }
+
+
+    /*
+     * For component or first instance
+     */
+    if(template->classe == SINGLETON || template->classe == COMPONENT)
+    {
+        /*
+         * Call init function generated by the compiler (one per .elf)
+         */
+        LOG_INTERNAL(2, "constructor call(s) <%s>\n", template->name, 0, 0, 0, 0, 0);
+        if (cm_DSP_GetState(template->dspId)->state != MPC_STATE_BOOTED)
+        {
+            cm_delayedDestroyComponent(component);
+            return CM_MPC_NOT_RESPONDING;
+        }
+        else if ((error = cm_COMP_CallService(
+                (priority > cm_EEM_getExecutiveEngine(coreId)->instance->priority)?NMF_CONSTRUCT_SYNC_INDEX:NMF_CONSTRUCT_INDEX,
+                component,
+                template->LCCConstructAddress)) != CM_OK)
+        {
+            if (error == CM_MPC_NOT_RESPONDING)
+                ERROR("CM_MPC_NOT_RESPONDING: can't call constructor '%s'\n", component->pathname, 0, 0, 0, 0, 0);
+            cm_delayedDestroyComponent(component);
+            return error;
+        }
+    }
+    else
+    {
+        /* be sure everything is write into memory, not required elsewhere since will be done by cm_COMP_CallService */
+        OSAL_mb();
+    }
+
+    // For firmware; Directly switch to STARTED state, don't need to start it
+    if (template->classe == FIRMWARE)
+        component->state = STATE_RUNNABLE;
+    else
+        component->state = STATE_STOPPED;
+
+    *refcomponent = component;
+    return CM_OK;
+}
+
+struct t_client_of_singleton* cm_getClientOfSingleton(t_component_instance* component, t_bool createdIfNotExist, t_nmf_client_id clientId)
+{
+    struct t_client_of_singleton* cur = component->clientOfSingleton;
+
+    for( ; cur != NULL ; cur = cur->next)
+    {
+        if(cur->clientId == clientId)
+        {
+            return cur;
+        }
+    }
+
+    //if(createdIfNotExist)
+    {
+        cur = OSAL_Alloc(sizeof(struct t_client_of_singleton));
+        if(cur != NULL)
+        {
+            cur->clientId = clientId;
+            cur->next = component->clientOfSingleton;
+            cur->numberOfBind = 0;
+            cur->numberOfInstance= 0;
+            cur->numberOfStart = 0;
+            component->clientOfSingleton = cur;
+        }
+    }
+    return cur;
+}
+
+/**
+ * Non-Require:
+ *    - MMDSP could be sleep (Since we access it only through HSEM)
+ */
+t_cm_error cm_startComponent(t_component_instance* component, t_nmf_client_id clientId)
+{
+    t_cm_error error;
+    char  value[MAX_PROPERTY_VALUE_LENGTH];
+    int i;
+
+    /*
+     * Special code for SINGLETON handling
+     */
+    if(component->template->classe == SINGLETON)
+    {
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, FALSE, clientId);
+        if(cl != NULL)
+            cl->numberOfStart++;
+        // A singleton could be started twice, thus start it only if first client starter
+        if(getNumberOfStart(component) > 1)
+            return CM_OK;
+
+        // Fall through and start really the singleton.
+    }
+
+    if(component->state == STATE_RUNNABLE)
+        return CM_COMPONENT_NOT_STOPPED;
+
+    // CM_ASSERT component->state == STATE_STOPPED
+
+    /*
+     * Check that all required binding have been binded!
+     */
+    for(i = 0; i < component->template->requireNumber; i++)
+    {
+        int nb = component->template->requires[i].collectionSize, j;
+        for(j = 0; j < nb; j++)
+        {
+            if(component->interfaceReferences[i][j].instance == NULL  &&
+                    (component->template->requires[i].requireTypes & (OPTIONAL_REQUIRE | INTRINSEC_REQUIRE)) == 0)
+            {
+                ERROR("CM_REQUIRE_INTERFACE_UNBINDED: Required interface '%s'.'%s' binded\n", component->pathname, component->template->requires[i].name, 0, 0, 0, 0);
+                return CM_REQUIRE_INTERFACE_UNBINDED;
+            }
+        }
+    }
+
+    component->state = STATE_RUNNABLE;
+
+    /*
+     * Power on, HW resources if required
+     */
+    if(cm_getComponentProperty(
+            component,
+            "hardware",
+            value,
+            sizeof(value)) == CM_OK)
+    {
+        // The PRCMU seem not supporting the transition of asking HW IP on while DSP in retention
+        // -> Thus force wake up of the MMDSP before asking the transition
+        if ((error = cm_EEM_ForceWakeup(component->template->dspId)) != CM_OK)
+            return error;
+
+        error = cm_PWR_EnableMPC(MPC_PWR_HWIP, component->template->dspId);
+
+        cm_EEM_AllowSleep(component->template->dspId);
+
+        if(error != CM_OK)
+            return error;
+    }
+
+    /*
+     * Call starter if available
+     */
+    if(component->template->LCCStartAddress != 0)
+    {
+        if (cm_DSP_GetState(component->template->dspId)->state != MPC_STATE_BOOTED)
+        {
+            return CM_MPC_NOT_RESPONDING;
+        }
+        else if ((error = cm_COMP_CallService(
+                (component->priority > cm_EEM_getExecutiveEngine(component->template->dspId)->instance->priority)?NMF_START_SYNC_INDEX:NMF_START_INDEX,
+                component,
+                component->template->LCCStartAddress)) != CM_OK)
+        {
+            if (error == CM_MPC_NOT_RESPONDING)
+                ERROR("CM_MPC_NOT_RESPONDING: can't call starter '%s'\n", component->pathname, 0, 0, 0, 0, 0);
+            return error;
+        }
+    }
+
+    return CM_OK;
+}
+
+/**
+ * Non-Require:
+ *    - MMDSP could be sleep (Since we access it only through HSEM)
+ */
+t_cm_error cm_stopComponent(t_component_instance* component, t_nmf_client_id clientId)
+{
+    char  value[MAX_PROPERTY_VALUE_LENGTH];
+    t_cm_error error = CM_OK;
+
+    /*
+     * Special code for SINGLETON handling
+     */
+    if(component->template->classe == SINGLETON)
+    {
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, FALSE, clientId);
+        if(cl != NULL)
+            cl->numberOfStart--;
+        // A singleton could be started twice, thus stop it only if no more client starter
+        if(getNumberOfStart(component) > 0)
+            return CM_OK;
+
+        // Fall through and stop really the singleton.
+    }
+
+    /*
+     * Component life cycle sanity check
+     */
+    if(component->state == STATE_STOPPED)
+        return CM_COMPONENT_NOT_STARTED;
+
+    // CM_ASSERT component->state == STATE_RUNNABLE
+    component->state = STATE_STOPPED;
+
+    if (cm_DSP_GetState(component->template->dspId)->state != MPC_STATE_BOOTED)
+    {
+        error = CM_MPC_NOT_RESPONDING;
+    }
+    else
+    {
+        /*
+         * Call stopper if available
+         */
+        if(component->template->LCCStopAddress != 0)
+        {
+            if ((error = cm_COMP_CallService(
+                    NMF_STOP_INDEX,
+                    component,
+                    component->template->LCCStopAddress)) != CM_OK)
+            {
+                if (error == CM_MPC_NOT_RESPONDING)
+                    ERROR("CM_MPC_NOT_RESPONDING: can't call stopper '%s'\n", component->pathname, 0, 0, 0, 0, 0);
+            }
+        }
+    }
+
+    /*
+     * Power on, HW resources if required
+     */
+    if(cm_getComponentProperty(
+            component,
+            "hardware",
+            value,
+            sizeof(value)) == CM_OK)
+    {
+        cm_PWR_DisableMPC(MPC_PWR_HWIP, component->template->dspId);
+    }
+
+    return error;
+}
+
+t_cm_error cm_destroyInstance(t_component_instance* component, t_destroy_state forceDestroy)
+{
+    int i, j;
+
+    LOG_INTERNAL(1, "\n##### Destroy %s/%x (%s) component on %s #####\n",
+                 component->pathname, component, component->template->name, cm_getDspName(component->template->dspId), 0, 0);
+
+    /*
+     * Component life cycle sanity check; do it only when destroying last reference.
+     */
+    if(forceDestroy == DESTROY_NORMAL)
+    {
+        if (component->state == STATE_RUNNABLE)
+            return CM_COMPONENT_NOT_STOPPED;
+
+        // CM_ASSERT component->state == STATE_STOPPED
+
+        // Check that all required binding have been unbound!
+        for(i = 0; i < component->template->requireNumber; i++)
+        {
+            int nb = component->template->requires[i].collectionSize;
+            for(j = 0; j < nb; j++)
+            {
+                if(component->interfaceReferences[i][j].instance != NULL)
+                {
+                    ERROR("CM_COMPONENT_NOT_UNBINDED: Required interface %s/%x.%s still binded\n",
+                            component->pathname, component, component->template->requires[i].name, 0, 0, 0);
+                    return CM_COMPONENT_NOT_UNBINDED;
+                }
+            }
+        }
+
+        // Check that all provided bindings have been unbound!
+        if (component->providedItfUsedCount != 0)
+        {
+            unsigned idx;
+
+            ERROR("CM_COMPONENT_NOT_UNBINDED: Still %d binding to %s/%x provided interface\n",
+                    component->providedItfUsedCount, component->pathname, component, 0, 0, 0);
+
+            /* Find which interface is still bound to gracefully print an error message */
+            for (idx=0; idx<ComponentTable.idxMax; idx++)
+            {
+                if ((componentEntry(idx) == NULL) || (componentEntry(idx) == component))
+                    continue;
+                for (i = 0; i < componentEntry(idx)->template->requireNumber; i++)
+                {
+                    for (j = 0; j < componentEntry(idx)->template->requires[i].collectionSize; j++)
+                    {
+                        if(componentEntry(idx)->interfaceReferences[i][j].instance == component
+                                && component->template->provides[componentEntry(idx)->interfaceReferences[i][j].provideIndex].interruptLine == 0)
+                        {
+                            ERROR("  -> %s/%x.%s still used by %s/%x.%s\n",
+                                    component->pathname, component,
+                                    component->template->provides[componentEntry(idx)->interfaceReferences[i][j].provideIndex].name,
+                                    componentEntry(idx)->pathname,
+                                    componentEntry(idx),
+                                    componentEntry(idx)->template->requires[i].name);
+                        }
+                    }
+                }
+            }
+
+            return CM_COMPONENT_NOT_UNBINDED;
+        }
+    }
+
+    // Sanity check finished, here, we will do the JOB whatever error
+
+    if (cm_DSP_GetState(component->template->dspId)->state == MPC_STATE_BOOTED)
+    {
+        /*
+         * Call destroy if available
+         */
+        /* Call the destructor only if we don't want to force the destruction */
+        if(forceDestroy != DESTROY_WITHOUT_CHECK_CALL && component->template->LCCDestroyAddress != 0)
+        {
+            if (cm_COMP_CallService(
+                    NMF_DESTROY_INDEX,
+                    component,
+                    component->template->LCCDestroyAddress) != CM_OK)
+            {
+                ERROR("CM_MPC_NOT_RESPONDING: can't call destroy '%s'\n", component->pathname, 0, 0, 0, 0, 0);
+            }
+        }
+    }
+
+    cm_delayedDestroyComponent(component);
+
+    return CM_OK;
+}
+
+/**
+ * Pre-Require:
+ *    - MMDSP wakeup (when accessing loadmap)
+ */
+t_cm_error cm_destroyInstanceForClient(t_component_instance* component, t_destroy_state forceDestroy, t_nmf_client_id clientId)
+{
+    /*
+     * Special code for SINGLETON handling
+     */
+    if(component->template->classe == SINGLETON)
+    {
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, FALSE, clientId);
+        int nbinstance;
+        if(cl != NULL)
+            cl->numberOfInstance--;
+
+        // A singleton could be instantiate twice, thus destroy it only if no more client constructor
+        nbinstance = getNumberOfInstance(component);
+        if(nbinstance > 0)
+        {
+            LOG_INTERNAL(1, "##### Singleton : Delete handle of %s/%x (%s) component on %s [%d] provItf=%d #####\n",
+                 component->pathname, component, component->template->name, cm_getDspName(component->template->dspId),
+                 nbinstance, component->providedItfUsedCount);
+            return CM_OK;
+        }
+
+        // Fall through
+    }
+
+    return cm_destroyInstance(component, forceDestroy);
+}
+
+
+static t_uint32 cm_getMaxStackValue(t_component_instance *pComponent)
+{
+    t_nmf_executive_engine_id executiveEngineId = cm_EEM_getExecutiveEngine(pComponent->template->dspId)->executiveEngineId;
+    t_uint32 res = MIN_STACK_SIZE;
+    unsigned int i;
+
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	if ((componentEntry(i) != NULL) &&
+	    (componentEntry(i) != pComponent) &&
+            (pComponent->template->dspId == componentEntry(i)->template->dspId) &&
+            (executiveEngineId == SYNCHRONOUS_EXECUTIVE_ENGINE || componentEntry(i)->priority == pComponent->priority))
+        {
+		if (componentEntry(i)->template->minStackSize > res)
+                res = componentEntry(i)->template->minStackSize;
+        }
+    }
+
+    return res;
+}
+
+static t_uint16 getNumberOfInstance(t_component_instance* component)
+{
+    t_uint16 instanceNumber = 0;
+    struct t_client_of_singleton* cur = component->clientOfSingleton;
+
+    for( ; cur != NULL ; cur = cur->next)
+    {
+        instanceNumber += cur->numberOfInstance;
+    }
+
+    return instanceNumber;
+}
+
+static t_uint16 getNumberOfStart(t_component_instance* component)
+{
+    t_uint16 startNumber = 0;
+    struct t_client_of_singleton* cur = component->clientOfSingleton;
+
+    for( ; cur != NULL ; cur = cur->next)
+    {
+        startNumber += cur->numberOfStart;
+    }
+
+    return startNumber;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/introspection.c b/drivers/staging/nmf-cm/cm/engine/component/src/introspection.c
new file mode 100644
index 00000000000..a4e0a01a242
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/introspection.c
@@ -0,0 +1,325 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/string.h>
+
+/*
+ *
+ */
+t_cm_error cm_getComponentProperty(
+	const t_component_instance *component,
+	const char                 *propName,
+    char                       value[MAX_PROPERTY_VALUE_LENGTH],
+    t_uint32                   valueLength){
+	t_component_template* template = component->template;
+    int i;
+
+    for(i = 0; i < template->propertyNumber; i++) {
+        if(cm_StringCompare(template->properties[i].name, propName, MAX_PROPERTY_NAME_LENGTH) == 0) {
+        	cm_StringCopy(
+                value,
+                template->properties[i].value,
+                valueLength);
+            return CM_OK;
+        }
+    }
+
+    return CM_NO_SUCH_PROPERTY;
+}
+
+/**
+ *
+ */
+static t_attribute* cm_getAttributeDescriptor(
+        const t_component_instance  *component,
+        const char                  *attrName)
+{
+    int i;
+
+    for(i = 0; i < component->template->attributeNumber; i++)
+    {
+        if(cm_StringCompare(component->template->attributes[i].name, attrName, MAX_ATTRIBUTE_NAME_LENGTH) == 0)
+        {
+            return &component->template->attributes[i];
+        }
+    }
+
+    return NULL;
+}
+
+t_dsp_address cm_getAttributeMpcAddress(
+        const t_component_instance  *component,
+        const char                  *attrName)
+{
+    t_attribute* attribute;
+    t_uint32 dspAddress;
+
+    if((attribute = cm_getAttributeDescriptor(component, attrName)) == NULL)
+        return 0x0;
+
+    cm_DSP_GetDspAddress(component->memories[attribute->memory.memory->id], &dspAddress);
+
+    return (dspAddress +
+            attribute->memory.offset);
+}
+
+t_cm_logical_address cm_getAttributeHostAddr(
+        const t_component_instance  *component,
+        const char                  *attrName)
+{
+    t_attribute* attribute;
+
+    if((attribute = cm_getAttributeDescriptor(component, attrName)) == NULL)
+        return 0x0;
+
+    // TODO JPF: component->template->attributes[i].memory.offset could be converted in byte during load
+    return cm_DSP_GetHostLogicalAddress(component->memories[attribute->memory.memory->id]) +
+            attribute->memory.offset * attribute->memory.memory->memEntSize;
+}
+
+
+t_cm_error cm_readAttribute(
+        const t_component_instance* component,
+        const char* attrName,
+        t_uint32* value)
+{
+    t_attribute* attribute;
+    t_cm_logical_address hostAddr;
+
+    if((attribute = cm_getAttributeDescriptor(component, attrName)) == NULL)
+    {
+        ERROR("CM_NO_SUCH_ATTRIBUTE(%s, %s)\n", component->pathname, attrName, 0, 0, 0, 0);
+        return CM_NO_SUCH_ATTRIBUTE;
+    }
+
+    // TODO JPF: component->template->attributes[i].memory.offset could be converted in byte during load
+    hostAddr = cm_DSP_GetHostLogicalAddress(component->memories[attribute->memory.memory->id]) +
+            attribute->memory.offset * attribute->memory.memory->memEntSize;
+
+    if(attribute->memory.memory->memEntSize != 2)
+        *value = *((t_uint32 *)hostAddr) & ~MASK_BYTE3;
+    else
+        *value = *((t_uint16 *)hostAddr);
+
+    LOG_INTERNAL(3, "cm_readAttribute: [%s:%s, %x]=%x\n",
+            component->pathname, attrName, hostAddr, *value, 0, 0);
+
+    return CM_OK;
+}
+
+t_uint32 cm_readAttributeNoError(
+        const t_component_instance* component,
+        const char* attrName)
+{
+    t_uint32 value;
+
+    if(cm_readAttribute(component, attrName, &value) != CM_OK)
+        value = 0;
+
+    return value;
+}
+
+t_cm_error cm_writeAttribute(
+        const t_component_instance* component,
+        const char* attrName,
+        t_uint32 value)
+{
+    t_attribute* attribute;
+    t_cm_logical_address hostAddr;
+
+    if((attribute = cm_getAttributeDescriptor(component, attrName)) == NULL)
+    {
+        ERROR("CM_NO_SUCH_ATTRIBUTE(%s, %s)\n", component->pathname, attrName, 0, 0, 0, 0);
+        return CM_NO_SUCH_ATTRIBUTE;
+    }
+
+    // TODO JPF: component->template->attributes[i].memory.offset could be converted in byte during load
+    hostAddr = cm_DSP_GetHostLogicalAddress(component->memories[attribute->memory.memory->id]) +
+            attribute->memory.offset * attribute->memory.memory->memEntSize;
+
+    if(attribute->memory.memory->memEntSize != 2)
+        *((t_uint32 *)hostAddr)  = value & ~MASK_BYTE3;
+    else
+        *((t_uint16 *)hostAddr) = value;
+
+    /* be sure attribute is write into memory */
+    OSAL_mb();
+
+    LOG_INTERNAL(3, "cm_writeAttribute: [%s:%s, %x]=%x\n",
+            component->pathname, attrName, hostAddr, value, 0, 0);
+
+    return CM_OK;
+}
+
+
+/**
+ *
+ */
+t_dsp_address cm_getFunction(
+        const t_component_instance* component,
+        const char* interfaceName,
+        const char* methodName)
+{
+    t_interface_provide_description itfProvide;
+    t_interface_provide* provide;
+    t_cm_error error;
+    int i;
+
+    // Get interface description
+    if((error = cm_getProvidedInterface(component, interfaceName, &itfProvide)) != CM_OK)
+        return error;
+
+    provide = &component->template->provides[itfProvide.provideIndex];
+
+    for(i = 0; i < provide->interface->methodNumber; i++)
+    {
+        if(cm_StringCompare(provide->interface->methodNames[i], methodName, MAX_INTERFACE_METHOD_NAME_LENGTH) == 0)
+        {
+            return provide->indexes[itfProvide.collectionIndex][i].methodAddresses;
+        }
+    }
+
+    return 0x0;
+}
+
+/**
+ *
+ */
+PRIVATE t_uint8 compareItfName(const char* simplename, const char* complexname, int *collectionIndex) {
+    int i;
+
+    // Search if simplename is a prefix of complexname ??
+    for(i = 0; simplename[i] != 0; i++) {
+        if(simplename[i] != complexname[i])
+            return 1;                       // NO
+    }
+
+    // YES
+    if(complexname[i] == '[') {
+        // This is a collection
+        int value = 0;
+        i++;
+        if(complexname[i] < '0' || complexname[i] > '9') {
+            return 1;
+        }
+        for(; complexname[i] >= '0' && complexname[i] <= '9'; i++) {
+            value = value * 10 + (complexname[i] - '0');
+        }
+        if(complexname[i++] != ']')
+            return 1;
+        *collectionIndex = value;
+    } else
+        *collectionIndex = -1;
+
+    if(complexname[i] != 0) {
+        // Complexe name has not been fully parsed -> different name
+        return 1;
+    }
+
+    return 0;
+}
+
+
+/**
+ *
+ */
+PUBLIC t_cm_error cm_getProvidedInterface(const t_component_instance* server,
+        const char* itfName,
+        t_interface_provide_description *itfProvide){
+    int i;
+
+    for(i = 0; i < server->template->provideNumber; i++)
+    {
+        int collectionIndex;
+        if(compareItfName(server->template->provides[i].name, itfName, &collectionIndex) == 0)
+        {
+            t_interface_provide *provide = &server->template->provides[i];
+            if(collectionIndex >= 0)
+            {
+                if(! (provide->provideTypes & COLLECTION_PROVIDE)) {
+                    ERROR("CM_NO_SUCH_PROVIDED_INTERFACE(%s, %s)\n",
+                            server->pathname, itfName, 0, 0, 0, 0);
+                    goto out;
+                }
+                if(collectionIndex >= provide->collectionSize) {
+                    ERROR("CM_NO_SUCH_PROVIDED_INTERFACE(%s, %s): out of range [0..%d[\n",
+                            server->pathname, itfName, provide->collectionSize,
+                            0, 0, 0);
+                    goto out;
+                }
+            }
+            else
+            {
+                if(provide->provideTypes & COLLECTION_PROVIDE) {
+                    ERROR("CM_NO_SUCH_PROVIDED_INTERFACE(%s, %s): interface is a collection [0..%d[\n",
+                            server->pathname, itfName, provide->collectionSize,
+                            0, 0, 0);
+                    goto out;
+                }
+                collectionIndex = 0;
+            }
+            itfProvide->provideIndex = i;
+            itfProvide->server = server;
+            itfProvide->collectionIndex = collectionIndex;
+            itfProvide->origName = itfName;
+            return CM_OK;
+        }
+    }
+
+    ERROR("CM_NO_SUCH_PROVIDED_INTERFACE(%s, %s)\n", server->pathname, itfName, 0, 0, 0, 0);
+out:
+    itfProvide->provideIndex = 0;
+    itfProvide->server = NULL;
+    itfProvide->collectionIndex = 0;
+    itfProvide->origName = NULL;
+    return CM_NO_SUCH_PROVIDED_INTERFACE;
+}
+
+/**
+ *
+ */
+t_cm_error cm_getRequiredInterface(const t_component_instance* client,
+        const char* itfName,
+        t_interface_require_description *itfRequire){
+    int i;
+
+    for(i = 0; i < client->template->requireNumber; i++) {
+        int collectionIndex;
+        if(compareItfName(client->template->requires[i].name, itfName, &collectionIndex) == 0) {
+            t_interface_require *require = &client->template->requires[i];
+             if(collectionIndex >= 0) {
+                if(! (require->requireTypes & COLLECTION_REQUIRE)) {
+                    ERROR("CM_NO_SUCH_REQUIRED_INTERFACE(%s, %s)\n",
+                        client->pathname, itfName, 0, 0, 0, 0);
+                    return CM_NO_SUCH_REQUIRED_INTERFACE;
+                }
+                if(collectionIndex >= require->collectionSize) {
+                    ERROR("CM_NO_SUCH_REQUIRED_INTERFACE(%s, %s): out of range [0..%d[\n",
+                        client->pathname, itfName, require->collectionSize,
+                        0, 0, 0);
+                    return CM_NO_SUCH_REQUIRED_INTERFACE;
+                }
+            } else {
+                if(require->requireTypes & COLLECTION_REQUIRE) {
+                    ERROR("CM_NO_SUCH_REQUIRED_INTERFACE(%s, %s): interface is a collection [0..%d[\n",
+                        client->pathname, itfName, require->collectionSize,
+                        0, 0, 0);
+                    return CM_NO_SUCH_REQUIRED_INTERFACE;
+                }
+                collectionIndex = 0;
+            }
+            itfRequire->client = client;
+            itfRequire->requireIndex = i;
+            itfRequire->collectionIndex = collectionIndex;
+            itfRequire->origName = itfName;
+            return CM_OK;
+        }
+    }
+
+    ERROR("CM_NO_SUCH_REQUIRED_INTERFACE(%s, %s)\n", client->pathname, itfName, 0, 0, 0, 0);
+    return CM_NO_SUCH_REQUIRED_INTERFACE;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/loader.c b/drivers/staging/nmf-cm/cm/engine/component/src/loader.c
new file mode 100644
index 00000000000..762f74c4479
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/loader.c
@@ -0,0 +1,338 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/component/inc/bind.h>
+
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/convert.h>
+
+void START(void);
+void END(char*);
+
+#define NHASH 79       //Use a prime number!
+#define MULT 17
+
+static t_component_template *templates[NB_CORE_IDS][NHASH];
+
+static unsigned int templateHash(const char *str)
+{
+    unsigned int h = 0;
+    for(; *str; str++)
+        h = MULT * h + *str;
+    return h % NHASH;
+}
+
+static void templateAdd(t_component_template *template)
+{
+    unsigned int h = templateHash(template->name);
+
+    if(templates[template->dspId][h] != NULL)
+        templates[template->dspId][h]->prev = template;
+    template->next = templates[template->dspId][h];
+    template->prev = NULL;
+    templates[template->dspId][h] = template;
+}
+
+static void templateRemove(t_component_template *template)
+{
+    unsigned int h = templateHash(template->name);
+
+    if(template->prev != NULL)
+        template->prev->next = template->next;
+    if(template->next != NULL)
+        template->next->prev = template->prev;
+    if(template == templates[template->dspId][h])
+        templates[template->dspId][h] = template->next;
+}
+
+
+t_component_template* cm_lookupTemplate(t_nmf_core_id dspId, t_dup_char str)
+{
+    t_component_template *template;
+
+    for(template = templates[dspId][templateHash(str)]; template != NULL; template = template->next)
+    {
+        if(str == template->name)
+            return template;
+    }
+
+    return NULL;
+}
+
+t_bool cm_isComponentOnCoreId(t_nmf_core_id coreId) {
+    t_uint32 i;
+
+    for(i = 0; i < NHASH; i++)
+    {
+	if ((templates[coreId][i] != NULL)
+	    && (templates[coreId][i]->classe != FIRMWARE)) // Skip firmware
+            return TRUE;
+    }
+
+    return FALSE;
+}
+
+
+static t_dsp_address MemoryToDspAdress(t_component_template *template, t_memory_reference *memory)
+{
+    if(memory->memory == NULL)
+        return (t_dsp_address)memory->offset;
+    else
+    {
+        t_dsp_address address;
+
+        cm_DSP_GetDspAddress(template->memories[memory->memory->id], &address);
+
+        return (t_dsp_address)(address + memory->offset);
+    }
+}
+
+/*
+ * Method callback
+ */
+t_uint32 cm_resolvSymbol(
+        void* context,
+        t_uint32 type,
+        t_dup_char symbolName,
+        char* reloc_addr)
+{
+    t_component_template *template = (t_component_template*)context;
+    t_component_instance* ee = cm_EEM_getExecutiveEngine(template->dspId)->instance;
+    int i, j;
+
+    // Search if this method is provided by EE and resolve it directly
+    for(i = 0; i < ee->template->provideNumber; i++)
+    {
+        t_interface_provide* provide = &ee->template->provides[i];
+
+        for(j = 0; j < provide->interface->methodNumber; j++)
+        {
+            if(provide->interface->methodNames[j] == symbolName)
+            {
+                return provide->indexes[0][j].methodAddresses; // Here we assume no collection provided !!
+            }
+        }
+    }
+
+    // Lookup if the method is statically required, ands delay relocation when bind occur
+    for(i = 0; i < template->requireNumber; i++)
+    {
+        if((template->requires[i].requireTypes & STATIC_REQUIRE) == 0)
+            continue;
+
+        for(j = 0; j < template->requires[i].interface->methodNumber; j++)
+        {
+            if(template->requires[i].interface->methodNames[j] == symbolName)
+            {
+                t_function_relocation* delayedRelocation = (t_function_relocation*)OSAL_Alloc(sizeof(t_function_relocation));
+                if(delayedRelocation == NULL)
+                    return 0xFFFFFFFE;
+
+                delayedRelocation->type = type;
+                delayedRelocation->symbol_name = cm_StringReference(symbolName);
+                delayedRelocation->reloc_addr = reloc_addr;
+                delayedRelocation->next = template->delayedRelocation;
+                template->delayedRelocation = delayedRelocation;
+
+                return 0xFFFFFFFF;
+            }
+        }
+    }
+
+    //Symbol not found
+    return 0x0;
+}
+
+/*
+ * Template Management
+ */
+t_cm_error cm_loadComponent(
+        t_dup_char templateName,
+        t_cm_domain_id domainId,
+        t_elfdescription* elfhandle,
+        t_component_template **reftemplate)
+{
+    t_nmf_core_id coreId = cm_DM_GetDomainCoreId(domainId);
+    t_cm_error error;
+    int i, j, k;
+
+    /*
+     * Allocate new component template if first instance
+     */
+    if(*reftemplate == NULL)
+    {
+        t_component_template *template;
+
+        LOG_INTERNAL(1, "\n##### Load template %s on %s #####\n", templateName, cm_getDspName(coreId), 0, 0, 0, 0);
+
+        /*
+         * Sanity check
+         */
+        if(elfhandle->foundedTemplateName != templateName)
+        {
+            ERROR("CM_INVALID_ELF_FILE: template name %s != %s\n", templateName, elfhandle->foundedTemplateName, 0, 0, 0, 0);
+            return CM_INVALID_ELF_FILE;
+        }
+
+        // Alloc & Reset variable in order to use unloadComponent either with partial constructed template
+        *reftemplate = template = (t_component_template*)OSAL_Alloc_Zero(sizeof(t_component_template));
+        if(template == NULL)
+            return CM_NO_MORE_MEMORY;
+        template->name = cm_StringReference(elfhandle->foundedTemplateName);
+
+        // Get information from elfhandle
+        template->descriptionAssociatedWithTemplate = elfhandle->temporaryDescription;
+        template->requireNumber = elfhandle->requireNumber;
+        template->requires = elfhandle->requires;
+        template->attributeNumber = elfhandle->attributeNumber;
+        template->attributes = elfhandle->attributes;
+        template->propertyNumber = elfhandle->propertyNumber;
+        template->properties = elfhandle->properties;
+        template->provideNumber = elfhandle->provideNumber;
+        template->provides = elfhandle->provides;
+        if(template->descriptionAssociatedWithTemplate)
+        {
+            elfhandle->requires = NULL;
+            elfhandle->attributes = NULL;
+            elfhandle->properties = NULL;
+            elfhandle->provides = NULL;
+        }
+
+        // Compute simple information
+        template->numberOfInstance = 1;
+        template->dspId = coreId;
+        LOG_INTERNAL(3, "load<%x> = %s\n", (int)template, template->name, 0, 0, 0, 0);
+        switch(elfhandle->magicNumber) {
+        case MAGIC_COMPONENT:
+            template->classe = COMPONENT;
+            break;
+        case MAGIC_SINGLETON:
+            template->classe = SINGLETON;
+            break;
+        case MAGIC_FIRMWARE:
+            template->classe = FIRMWARE;
+            break;
+        }
+        template->minStackSize = elfhandle->minStackSize;
+
+        /*
+         * Load shared memory from file
+         */
+        // START();
+        if((error = cm_ELF_LoadTemplate(domainId, elfhandle, template->memories)) != CM_OK)
+            goto out;
+        MMDSP_serializeMemories(elfhandle->instanceProperty, &template->codeMemory, &template->thisMemory);
+        // END("cm_ELF_LoadTemplate");
+
+        /*
+         * Copy LCC functions information
+         * Since MMDSP require Constructor & Destructor (for cache flush and debug purpose) to be called
+         * either if not provided by user for allowing defered breakpoint, we use Void method if not provided.
+         */
+        template->LCCConstructAddress = MemoryToDspAdress(template, &elfhandle->memoryForConstruct);
+        template->LCCStartAddress = MemoryToDspAdress(template, &elfhandle->memoryForStart);
+        template->LCCStopAddress = MemoryToDspAdress(template, &elfhandle->memoryForStop);
+        template->LCCDestroyAddress = MemoryToDspAdress(template, &elfhandle->memoryForDestroy);
+        if(template->LCCConstructAddress == 0 && template->classe != FIRMWARE)
+            template->LCCConstructAddress = cm_EEM_getExecutiveEngine(coreId)->voidAddr;
+
+        // Compute provide methodIndex
+        for(i = 0; i < template->provideNumber; i++)
+        {
+            for(j = 0; j < template->provides[i].collectionSize; j++)
+            {
+                for(k = 0; k < template->provides[i].interface->methodNumber; k++)
+                {
+                    template->provides[i].indexes[j][k].methodAddresses =
+                            MemoryToDspAdress(template, &template->provides[i].indexes[j][k].memory);
+
+                    LOG_INTERNAL(2, "    [%d, %d] method '%s' @ %x\n",
+                            j, k, template->provides[i].interface->methodNames[k], template->provides[i].indexes[j][k].methodAddresses, 0, 0);
+                }
+            }
+        }
+
+        /*
+         * TODO
+
+        if((error = elfhandle->errorOccured) != CM_OK)
+            goto out;
+            */
+
+      //  START();
+        if(template->classe != FIRMWARE)
+        {
+            if((error = cm_ELF_relocateSharedSegments(
+                    template->memories,
+                    elfhandle,
+                    template)) != CM_OK)
+                goto out;
+        }
+       // END("cm_ELF_relocateSharedSegments");
+
+        cm_ELF_FlushTemplate(coreId, template->memories);
+
+        templateAdd(template);
+
+        return CM_OK;
+    out:
+        cm_unloadComponent(template);
+        return error;
+    }
+    else
+    {
+        (*reftemplate)->numberOfInstance++;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_unloadComponent(
+        t_component_template *template)
+{
+    /*
+     * Destroy template if last instance
+     */
+    if(--template->numberOfInstance == 0) {
+        t_function_relocation* reloc;
+
+        LOG_INTERNAL(3, "unload<%s>\n", template->name, 0, 0, 0, 0, 0);
+
+        templateRemove(template);
+
+        // Free delayedRelocation
+        reloc = template->delayedRelocation;
+        while(reloc != NULL)
+        {
+            t_function_relocation *tofree = reloc;
+            reloc = reloc->next;
+            cm_StringRelease(tofree->symbol_name);
+            OSAL_Free(tofree);
+        }
+
+        if(template->descriptionAssociatedWithTemplate)
+        {
+            cm_ELF_ReleaseDescription(
+                    template->requireNumber, template->requires,
+                    template->attributeNumber, template->attributes,
+                    template->propertyNumber, template->properties,
+                    template->provideNumber, template->provides);
+        }
+
+        // Free shared memories
+        cm_ELF_FreeTemplate(template->dspId, template->memories);
+
+        cm_StringRelease(template->name);
+
+        OSAL_Free(template);
+    }
+
+    return CM_OK;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration.h b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration.h
new file mode 100644
index 00000000000..7156769bb2d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_CONFIGURATION_H_
+#define __INC_CONFIGURATION_H_
+
+#include <cm/engine/api/control/configuration_engine.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <inc/nmf-limits.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+/******************************************************************************/
+/************************ FUNCTIONS PROTOTYPES ********************************/
+/******************************************************************************/
+
+PUBLIC t_cm_error cm_CFG_ConfigureMediaProcessorCore(t_nmf_core_id coreId,
+        t_nmf_executive_engine_id executiveEngineId,
+        t_nmf_semaphore_type_id semaphoreTypeId, t_uint8 nbYramBanks,
+        const t_cm_system_address *mediaProcessorMappingBaseAddr,
+        const t_cm_domain_id eeDomain,
+        t_dsp_allocator_desc* sdramCodeAllocId,
+        t_dsp_allocator_desc* sdramDataAllocId
+        );
+
+PUBLIC t_cm_error cm_CFG_AddMpcSdramSegment(const t_nmf_memory_segment *pDesc,
+        const char *memoryname, t_dsp_allocator_desc **allocDesc);
+
+PUBLIC t_cm_error cm_CFG_CheckMpcStatus(t_nmf_core_id coreId);
+
+void cm_CFG_ReleaseMpc(t_nmf_core_id coreId);
+
+PUBLIC t_cm_error cm_CFG_GetRequiredExecutiveEngineComponentNames(
+        char fileList[][MAX_INTERFACE_TYPE_NAME_LENGTH], t_uint32 listSize);
+
+#endif /* __INC_CONFIGURATION_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_status.h b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_status.h
new file mode 100644
index 00000000000..0c75b9c49b0
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_status.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_CONFIGSTATUS_H_
+#define __INC_CONFIGSTATUS_H
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/utils/inc/string.h>
+
+/*
+ * Variable to active intensive check
+ *
+ * \ingroup CM_CONFIGURATION_API
+ */
+extern t_sint32 cmIntensiveCheckState;
+
+/*
+ * Variable to active trace level
+ *
+ * \ingroup CM_CONFIGURATION_API
+ */
+extern t_sint32 cm_debug_level;
+
+/*
+ * Variable to active error break
+ *
+ * \ingroup CM_CONFIGURATION_API
+ */
+extern t_sint32 cm_error_break;
+
+/*
+ * Variable to activate Ulp
+ *
+ * \ingroup CM_CONFIGURATION_API
+ */
+extern t_bool cmUlpEnable;
+
+extern t_dup_char anonymousDup, eventDup, skeletonDup, stubDup, traceDup;
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_type.h b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_type.h
new file mode 100644
index 00000000000..abfe452cf78
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_type.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Configuration Component Manager API type.
+ */
+#ifndef CONFIGURATION_TYPE_H
+#define CONFIGURATION_TYPE_H
+
+#include <cm/inc/cm_type.h>
+
+/*!
+ * @defgroup t_cm_cmd_id t_cm_cmd_id
+ * \brief Definition of the command ID
+ * \ingroup CM_CONFIGURATION_API
+ *
+ * CM_CMD_XXX designates the command ID used by the \ref CM_SetMode routine.
+ *
+ * \remarks Other command IDs are not yet implemented.
+ */
+
+typedef t_uint32 t_cm_cmd_id;                                     //!< Fake enumeration type \ingroup t_cm_cmd_id
+#define CM_CMD_SYNC            ((t_cm_cmd_id)0x01)  //!< Synchronize on-going operations (no parameter) \ingroup t_cm_cmd_id
+
+#define CM_CMD_WARM_RESET      ((t_cm_cmd_id)0x02)  //!< Reset a part of the CM-engine (parameter indicates the part which must be reseted) \ingroup t_cm_cmd_id
+
+#define CM_CMD_PWR_MGR         ((t_cm_cmd_id)0x10)  //!< Enable/Disable the internal power management module (0=Disable, 1=Enable) \ingroup t_cm_cmd_id
+
+#define CM_CMD_DBG_MODE        ((t_cm_cmd_id)0x40)  //!< Enable/Disable DEBUG mode, Pwr Mgr is also disabled (0=Disable, 1=Enable) \ingroup t_cm_cmd_id
+
+#define CM_CMD_TRACE_ON             ((t_cm_cmd_id)0x41)     //!< Enable STM/XTI tracing and force network resetting and dumping \note Since MPC trace will be usable, you can enable them if not \ingroup t_cm_cmd_id
+#define CM_CMD_TRACE_OFF            ((t_cm_cmd_id)0x42)     //!< Disable STM/XTI tracing \note Since MPC trace will not be usable, you can also disable them \ingroup t_cm_cmd_id
+
+#define CM_CMD_MPC_TRACE_ON         ((t_cm_cmd_id)0x50)     //!< Enable MPC STM/XTI tracing (param == coreId). \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+#define CM_CMD_MPC_TRACE_OFF        ((t_cm_cmd_id)0x51)     //!< Disable MPC STM/XTI tracing (param == coreId) This is the default configuration. \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+
+#define CM_CMD_MPC_PRINT_OFF        ((t_cm_cmd_id)0x52)     //!< Set to OFF the level of MPC traces (param == coreId) \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+#define CM_CMD_MPC_PRINT_ERROR      ((t_cm_cmd_id)0x53)     //!< Set to ERROR the level of MPC traces param == coreId) \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+#define CM_CMD_MPC_PRINT_WARNING    ((t_cm_cmd_id)0x54)     //!< Set to WARNING the level of MPC traces param == coreId) \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+#define CM_CMD_MPC_PRINT_INFO       ((t_cm_cmd_id)0x55)     //!< Set to INFO the level of MPC traces (param == coreId) \note This command is not execute if execution engine not started on the coreId This is the default configuration. \ingroup t_cm_cmd_id
+#define CM_CMD_MPC_PRINT_VERBOSE    ((t_cm_cmd_id)0x56)     //!< Set to VERBOSE the level of MPC traces param == coreId) \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+
+/*!
+ * \brief  Define the level of internal CM log traces
+ *
+ * Define the level of internal CM log traces (-1 to 3)
+ *    -# <b>-1 </b> all internal LOG/ERROR traces are disabled
+ *    -# <b> 0 </b> all internal LOG traces are disabled (<b>default/reset value</b>)
+ *    -# <b> 1, 2, 3 </b> Most and most
+ *
+ * \ingroup t_cm_cmd_id
+ */
+#define CM_CMD_TRACE_LEVEL     ((t_cm_cmd_id)0x80)
+
+/*!
+ * \brief  Enable/Disable intensive internal check
+ *
+ * Enable/Disable intensive internal check (0=Disable, 1=Enable):
+ *    - Component handle checking
+ *
+ * Must be used during the integration phase (additional process is time consuming).
+ *
+ * \ingroup t_cm_cmd_id
+ */
+#define CM_CMD_INTENSIVE_CHECK ((t_cm_cmd_id)0x100)
+
+/*!
+ * \brief  Enable/Disable ulp mode
+ *
+ * Enable/Disable Ultra Low Power mode.
+ *
+ * \ingroup t_cm_cmd_id
+ */
+#define CM_CMD_ULP_MODE_ON      ((t_cm_cmd_id)0x111)    //!< Enable ULP mode \ingroup t_cm_cmd_id
+#define CM_CMD_ULP_MODE_OFF     ((t_cm_cmd_id)0x110)    //!< Deprecated (must be removed in 2.10) !!!
+
+#endif /* CONFIGURATION_TYPE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration.c b/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration.c
new file mode 100644
index 00000000000..f3242af5da8
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration.c
@@ -0,0 +1,213 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/configuration/inc/configuration.h>
+#include <cm/engine/component/inc/initializer.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/communication/inc/communication.h>
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/repository_mgt/inc/repository_mgt.h>
+#include <inc/nmf-limits.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/memory/inc/domain.h>
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/convert.h>
+
+#include <cm/engine/power_mgt/inc/power.h>
+
+t_sint32 cmIntensiveCheckState = 0;
+t_sint32 cm_debug_level = 1;
+t_sint32 cm_error_break = 0;
+t_bool cmUlpEnable = FALSE;
+
+
+#define MAX_EE_NAME_LENGTH      32
+typedef struct {
+    char eeName[MAX_EE_NAME_LENGTH];
+    t_nmf_executive_engine_id executiveEngineId;
+    t_uint32 EEmemoryCount;
+} t_cfg_mpc_desc;
+
+static t_cfg_mpc_desc        cfgMpcDescArray[NB_CORE_IDS];
+
+PUBLIC t_cm_error cm_CFG_ConfigureMediaProcessorCore(
+        t_nmf_core_id coreId,
+        t_nmf_executive_engine_id executiveEngineId,
+        t_nmf_semaphore_type_id semaphoreTypeId,
+        t_uint8 nbYramBanks,
+        const t_cm_system_address *mediaProcessorMappingBaseAddr,
+        const t_cm_domain_id eeDomain,
+        t_dsp_allocator_desc *sdramCodeAllocDesc,
+        t_dsp_allocator_desc *sdramDataAllocDesc)
+{
+    /* Process requested configuration (save it) */
+    cfgMpcDescArray[coreId].EEmemoryCount = 0;
+    cfgMpcDescArray[coreId].executiveEngineId = executiveEngineId;
+    /* Build Executive Engine Name */
+    switch(executiveEngineId)
+    {
+        case SYNCHRONOUS_EXECUTIVE_ENGINE:
+            cm_StringCopy(cfgMpcDescArray[coreId].eeName, "synchronous_", MAX_EE_NAME_LENGTH);
+            break;
+        case HYBRID_EXECUTIVE_ENGINE:
+            cm_StringCopy(cfgMpcDescArray[coreId].eeName, "hybrid_", MAX_EE_NAME_LENGTH);
+            break;
+    }
+
+    switch(semaphoreTypeId)
+    {
+        case LOCAL_SEMAPHORES:
+            cm_StringConcatenate(cfgMpcDescArray[coreId].eeName, "lsem", MAX_EE_NAME_LENGTH);
+            break;
+        case SYSTEM_SEMAPHORES:
+            cm_StringConcatenate(cfgMpcDescArray[coreId].eeName, "hsem", MAX_EE_NAME_LENGTH);
+            break;
+    }
+
+    cm_SEM_InitMpc(coreId, semaphoreTypeId);
+
+    return cm_DSP_Add(coreId, nbYramBanks, mediaProcessorMappingBaseAddr, eeDomain, sdramCodeAllocDesc, sdramDataAllocDesc);
+}
+
+// TODO JPF: Move in dsp.c
+PUBLIC t_cm_error cm_CFG_AddMpcSdramSegment(const t_nmf_memory_segment *pDesc, const char* memoryname, t_dsp_allocator_desc **allocDesc)
+{
+    t_dsp_allocator_desc *desc;
+    if ( (pDesc == NULL) ||
+            ((pDesc->systemAddr.logical & CM_MM_ALIGN_64BYTES) != 0) )
+        return CM_INVALID_PARAMETER;
+
+    //TODO, juraj, the right place and way to do this?
+    desc = (t_dsp_allocator_desc*)OSAL_Alloc(sizeof (t_dsp_allocator_desc));
+    if (desc == 0)
+        return CM_NO_MORE_MEMORY;
+
+    desc->allocDesc = cm_MM_CreateAllocator(pDesc->size, 0, memoryname);
+    if (desc->allocDesc == 0) {
+    	OSAL_Free(desc);
+    	return CM_NO_MORE_MEMORY;
+    }
+    desc->baseAddress = pDesc->systemAddr;
+    desc->referenceCounter = 0;
+
+    *allocDesc = desc;
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_CFG_CheckMpcStatus(t_nmf_core_id coreId)
+{
+    t_cm_error error;
+
+    if (cm_DSP_GetState(coreId)->state == MPC_STATE_BOOTABLE)
+    {
+        /* Allocate coms fifo for a given MPC */
+        if ((error = cm_COM_AllocateMpc(coreId)) != CM_OK)
+            return error;
+
+        /* Launch EE */
+        if ((error = cm_EEM_Init(coreId,
+                                 cfgMpcDescArray[coreId].eeName,
+                                 cfgMpcDescArray[coreId].executiveEngineId)) != CM_OK)
+        {
+            cm_COM_FreeMpc(coreId);
+            return error;
+        }
+
+        /* Initialize coms fifo for a given MPC */
+        cm_COM_InitMpc(coreId);
+
+        /* Initialisation of the dedicated communication channel for component initialization */
+        if((error = cm_COMP_INIT_Init(coreId)) != CM_OK)
+        {
+            cm_EEM_Close(coreId);
+            cm_COM_FreeMpc(coreId);
+            return error;
+        }
+
+        cfgMpcDescArray[coreId].EEmemoryCount = cm_PWR_GetMPCMemoryCount(coreId);
+
+        if(cmUlpEnable)
+        {
+            // We have finish boot, allow MMDSP to go in auto idle
+            cm_EEM_AllowSleep(coreId);
+        }
+    }
+
+    if (cm_DSP_GetState(coreId)->state != MPC_STATE_BOOTED)
+        return CM_MPC_NOT_INITIALIZED;
+
+    return CM_OK;
+}
+
+void cm_CFG_ReleaseMpc(t_nmf_core_id coreId)
+{
+    t_uint32 memoryCount = cm_PWR_GetMPCMemoryCount(coreId);
+
+    // If No more memory and no more component (to avoid switch off in case of component using no memory)
+    if(
+            cm_PWR_GetMode() == NORMAL_PWR_MODE &&
+            memoryCount != 0 /* Just to see if there is something */ &&
+            memoryCount == cfgMpcDescArray[coreId].EEmemoryCount &&
+            cm_isComponentOnCoreId(coreId) == FALSE)
+    {
+        LOG_INTERNAL(1, "\n##### Shutdown %s #####\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+
+        (void)cm_EEM_ForceWakeup(coreId);
+
+        /* remove ee from load map here */
+        cm_COMP_INIT_Close(coreId);
+        cm_EEM_Close(coreId);
+        cm_COM_FreeMpc(coreId);
+
+        cfgMpcDescArray[coreId].EEmemoryCount = 0; // For debug purpose
+    }
+}
+
+/****************************************************************************/
+/* NAME: cm_CFG_GetRequiredExecutiveEngineComponentNames(                   */
+/*                          char *fileList[],                               */
+/*                          t_uint32 *listSize                              */
+/*                         )                                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: retrieves the names of the required Executive Engine        */
+/*              component(s) to load.                                       */
+/****************************************************************************/
+PUBLIC t_cm_error cm_CFG_GetRequiredExecutiveEngineComponentNames(
+	char fileList[][MAX_INTERFACE_TYPE_NAME_LENGTH],
+	t_uint32 listSize)
+{
+    t_nmf_core_id coreId;
+    t_uint32 nb=0;
+
+    for (coreId = ARM_CORE_ID; coreId <= LAST_CORE_ID; coreId++)
+    {
+        if ((cm_DSP_GetState(coreId)->state == MPC_STATE_BOOTABLE)
+                && (cm_REP_lookupComponent(cfgMpcDescArray[coreId].eeName, NULL) != CM_OK))
+        {
+            /* Check if we didn't already register this name in the list */
+            int j, present=0;
+            for (j=0; j<nb; j++) {
+                if (cm_StringCompare(cfgMpcDescArray[coreId].eeName, fileList[j], MAX_EE_NAME_LENGTH) == 0) {
+                    present = 1;
+                    break;
+                }
+            }
+            if (present)
+                continue;
+            if (nb >= listSize)
+                return CM_NO_MORE_MEMORY;
+            cm_StringCopy(fileList[nb], cfgMpcDescArray[coreId].eeName, MAX_EE_NAME_LENGTH);
+            nb++;
+        }
+    }
+    return CM_OK;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration_wrapper.c b/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration_wrapper.c
new file mode 100644
index 00000000000..34789752b7d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration_wrapper.c
@@ -0,0 +1,301 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/api/configuration_engine.h>
+#include <cm/engine/communication/inc/communication.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/memory/inc/chunk_mgr.h>
+#include <cm/engine/repository_mgt/inc/repository_mgt.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/configuration/inc/configuration.h>
+#include <cm/engine/power_mgt/inc/power.h>
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/component/inc/bind.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/api/executive_engine_mgt_engine.h>
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/trace/inc/xtitrace.h>
+
+t_dup_char anonymousDup, eventDup, skeletonDup, stubDup, traceDup;
+
+PUBLIC t_cm_error CM_ENGINE_Init(
+        const t_nmf_hw_mapping_desc *pNmfHwMappingDesc,
+        const t_nmf_config_desc *pNmfConfigDesc
+        )
+{
+    t_cm_error error;
+
+    // The purpose of that is just to not free/unfree some String frequently used
+    anonymousDup = cm_StringDuplicate("anonymous");
+    eventDup = cm_StringDuplicate("event");
+    skeletonDup = cm_StringDuplicate("skeleton");
+    stubDup = cm_StringDuplicate("stub");
+    traceDup = cm_StringDuplicate("trace");
+
+    if ((
+                error = cm_OSAL_Init()
+        ) != CM_OK) { return error; }
+
+    if ((
+                error = cm_COMP_Init()
+        ) != CM_OK) { return error; }
+
+    if ((
+    		    error = cm_PWR_Init()
+         ) != CM_OK) { return error; }
+
+    cm_TRC_traceReset();
+
+    if ((
+                error = cm_DM_Init()
+         ) != CM_OK) {return error; }
+
+    if ((
+                error = cm_SEM_Init(&pNmfHwMappingDesc->hwSemaphoresMappingBaseAddr)
+        ) != CM_OK) { return error; }
+
+    if ((error = cm_COM_Init(pNmfConfigDesc->comsLocation)) != CM_OK)
+        return error;
+
+    cm_DSP_Init(&pNmfHwMappingDesc->esramDesc);
+
+    return CM_OK;
+}
+
+PUBLIC void CM_ENGINE_Destroy(void)
+{
+    t_component_instance *instance;
+    t_cm_error error;
+    t_uint32 i;
+
+    /* PP: Well, on Linux (and probably on Symbian too), this is called when driver is removed
+     * => the module (driver) can't be removed if there are some pending clients
+     * => all remaining components should have been destroyed in CM_ENGINE_FlushClient()
+     * => So, if we found some components here, we are in BIG trouble ...
+     */
+    /* First, stop all remaining components  */
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	t_nmf_client_id clientId;
+
+	if ((instance = componentEntry(i)) == NULL)
+		continue;
+	clientId = domainDesc[instance->domainId].client;
+        LOG_INTERNAL(0, "Found a remaining component %s (%s) when destroying the CM !!!\n", instance->pathname, instance->template->name, 0, 0, 0, 0);
+        if (/* skip EE */
+                (instance->template->classe == FIRMWARE) ||
+                /* Skip all binding components */
+                (cm_StringCompare(instance->template->name, "_ev.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_st.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_sk.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_tr.", 4) == 0))
+            continue;
+
+        /*
+         * Special code for SINGLETON handling
+         */
+        if(instance->template->classe == SINGLETON)
+        {
+            struct t_client_of_singleton* cl = instance->clientOfSingleton;
+
+            clientId = instance->clientOfSingleton->clientId;
+            for( ; cl != NULL ; cl = cl->next)
+            {
+                if(cl == instance->clientOfSingleton)
+                {
+                    cl->numberOfStart = 1; // == 1 since it will go to 0 in cm_stopComponent
+                    cl->numberOfInstance = 1; // == 1 since it will go to 0 in cm_destroyInstanceForClient
+                }
+                else
+                {
+                    cl->numberOfStart = 0;
+                    cl->numberOfInstance = 0;
+                }
+                cl->numberOfBind = 0;
+            }
+        }
+
+        // Stop the component
+        error = cm_stopComponent(instance, clientId);
+        if (error != CM_OK && error != CM_COMPONENT_NOT_STARTED)
+            LOG_INTERNAL(0, "Error stopping component %s/%x (%s, error=%d, client=%u)\n", instance->pathname, instance, instance->template->name, error, clientId, 0);
+
+        // Destroy dependencies
+        cm_destroyRequireInterface(instance, clientId);
+    }
+
+    /* Destroy all remaining components */
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	t_nmf_client_id clientId;
+
+	if ((instance = componentEntry(i)) == NULL)
+		continue;
+        clientId = domainDesc[instance->domainId].client;
+
+        if (/* skip EE */
+                (instance->template->classe == FIRMWARE) ||
+                /* Skip all binding components */
+                (cm_StringCompare(instance->template->name, "_ev.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_st.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_sk.", 4) == 0) ||
+                (cm_StringCompare(instance->template->name, "_tr.", 4) == 0)) {
+            continue;
+        }
+
+        if(instance->template->classe == SINGLETON)
+        {
+            clientId = instance->clientOfSingleton->clientId;
+        }
+
+        // Destroy the component
+        error = cm_destroyInstanceForClient(instance, DESTROY_WITHOUT_CHECK, clientId);
+
+        if (error != CM_OK)
+        {
+            /* FIXME : add component name instance in log message but need to make a copy before cm_flushComponent()
+             *         because it's no more available after.
+             */
+            LOG_INTERNAL(0, "Error flushing component (error=%d, client=%u)\n", error, clientId, 0, 0, 0, 0);
+        }
+    }
+
+    /* This will power off all ressources and destroy EE */
+    cm_PWR_SetMode(NORMAL_PWR_MODE);
+    cm_DSP_Destroy();
+    cm_DM_Destroy();
+    /* Nothing to do about SEM */
+    //cm_MM_Destroy();
+    cm_REP_Destroy();
+    cm_COMP_Destroy();
+    cm_OSAL_Destroy();
+
+    cm_StringRelease(traceDup);
+    cm_StringRelease(stubDup);
+    cm_StringRelease(skeletonDup);
+    cm_StringRelease(eventDup);
+    cm_StringRelease(anonymousDup);
+}
+
+PUBLIC t_cm_error CM_ENGINE_ConfigureMediaProcessorCore(
+        t_nmf_core_id coreId,
+        t_nmf_executive_engine_id executiveEngineId,
+        t_nmf_semaphore_type_id semaphoreTypeId,
+        t_uint8 nbYramBanks,
+        const t_cm_system_address *mediaProcessorMappingBaseAddr,
+        const t_cm_domain_id eeDomain,
+        const t_cfg_allocator_id sdramCodeAllocId,
+        const t_cfg_allocator_id sdramDataAllocId
+        )
+{
+    return cm_CFG_ConfigureMediaProcessorCore(
+            coreId,
+            executiveEngineId,
+            semaphoreTypeId,
+            nbYramBanks,
+            mediaProcessorMappingBaseAddr,
+            eeDomain,
+            (t_dsp_allocator_desc*)sdramCodeAllocId,
+            (t_dsp_allocator_desc*)sdramDataAllocId
+            );
+}
+
+PUBLIC t_cm_error CM_ENGINE_AddMpcSdramSegment(
+        const t_nmf_memory_segment *pDesc,
+        t_cfg_allocator_id         *id,
+        const char                 *memoryname
+        )
+{
+    return cm_CFG_AddMpcSdramSegment(pDesc, memoryname == NULL ? "" : memoryname, (t_dsp_allocator_desc**)id);
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_SetMode(t_cm_cmd_id aCmdID, t_sint32 aParam)
+{
+	t_cm_error error = CM_OK;
+	int i;
+
+    OSAL_LOCK_API();
+
+    switch(aCmdID) {
+    case CM_CMD_DBG_MODE:
+        cm_PWR_SetMode(( aParam==1 ) ? DISABLE_PWR_MODE : NORMAL_PWR_MODE);
+        switch(cm_PWR_GetMode())
+        {
+        case NORMAL_PWR_MODE:
+            // Release the MPC (which will switch it off if no more used)
+            for (i=FIRST_MPC_ID; i<NB_CORE_IDS; i++)
+            {
+                cm_CFG_ReleaseMpc(i);
+            }
+            break;
+        case DISABLE_PWR_MODE:
+            // Force the load of the EE if not already done.
+            for (i=FIRST_MPC_ID; i<NB_CORE_IDS;i++)
+            {
+                if((error = cm_CFG_CheckMpcStatus(i)) != CM_OK)
+                    break;
+            }
+            break;
+        }
+        break;
+	case CM_CMD_TRACE_LEVEL:
+	    if (aParam<-1) cm_debug_level = -1;
+	    else cm_debug_level = aParam;
+	    break;
+	case CM_CMD_INTENSIVE_CHECK:
+	    cmIntensiveCheckState = aParam;
+	    break;
+
+	case CM_CMD_TRACE_ON:
+	    cm_trace_enabled = TRUE;
+        cm_TRC_Dump();
+	    break;
+	case CM_CMD_TRACE_OFF:
+	    cm_trace_enabled = FALSE;
+	    break;
+
+	case CM_CMD_MPC_TRACE_ON:
+	    cm_EEM_setTraceMode((t_nmf_core_id)aParam, 1);
+	    break;
+	case CM_CMD_MPC_TRACE_OFF:
+	    cm_EEM_setTraceMode((t_nmf_core_id)aParam, 0);
+	    break;
+
+	case CM_CMD_MPC_PRINT_OFF:
+	    cm_EEM_setPrintLevel((t_nmf_core_id)aParam, 0);
+	    break;
+	case CM_CMD_MPC_PRINT_ERROR:
+	    cm_EEM_setPrintLevel((t_nmf_core_id)aParam, 1);
+	    break;
+	case CM_CMD_MPC_PRINT_WARNING:
+	    cm_EEM_setPrintLevel((t_nmf_core_id)aParam, 2);
+	    break;
+	case CM_CMD_MPC_PRINT_INFO:
+	    cm_EEM_setPrintLevel((t_nmf_core_id)aParam, 3);
+	    break;
+	case CM_CMD_MPC_PRINT_VERBOSE:
+	    cm_EEM_setPrintLevel((t_nmf_core_id)aParam, 4);
+	    break;
+
+	case CM_CMD_ULP_MODE_ON:
+	    cmUlpEnable = TRUE;
+	    break;
+
+	default:
+	    error = CM_INVALID_PARAMETER;
+	    break;
+	}
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/dsp/inc/dsp.h b/drivers/staging/nmf-cm/cm/engine/dsp/inc/dsp.h
new file mode 100644
index 00000000000..5cd65289ff8
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/dsp/inc/dsp.h
@@ -0,0 +1,454 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief DSP abstraction layer
+ *
+ * \defgroup DSP_INTERNAL Private DSP Abstraction Layer API.
+ *
+ */
+#ifndef __INC_CM_DSP_H
+#define __INC_CM_DSP_H
+
+#include <cm/inc/cm_type.h>
+#include <share/inc/nmf.h>
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <share/semaphores/inc/semaphores.h>
+#include <cm/engine/memory/inc/remote_allocator.h>
+
+
+#define SxA_NB_BLOCK_RAM        8 /*32kworks (24-bit) */
+
+#define SxA_LOCKED_WAY          1
+
+/*
+ * Type defintion to handle dsp offset in word
+ */
+typedef t_uint32 t_dsp_offset;
+
+typedef t_uint32 t_dsp_address;
+
+typedef enum {
+    DSP2ARM_IRQ_0,
+    DSP2ARM_IRQ_1
+} t_mpc2host_irq_num;
+
+typedef enum {
+    ARM2DSP_IRQ_0,
+    ARM2DSP_IRQ_1,
+    ARM2DSP_IRQ_2,
+    ARM2DSP_IRQ_3
+} t_host2mpc_irq_num;
+
+typedef enum {
+    INTERNAL_XRAM24 = 0,  /* 24-bit XRAM */
+    INTERNAL_XRAM16 = 1,  /* 16-bit XRAM */
+    INTERNAL_YRAM24 = 2,  /* 24-bit YRAM */
+    INTERNAL_YRAM16 = 3,  /* 16-bit YRAM */
+    SDRAM_EXT24     = 4,  /* 24-bit external "X" memory */
+    SDRAM_EXT16     = 5,  /* 16-bit external "X" memory */
+    ESRAM_EXT24     = 6,  /* ESRAM24 */
+    ESRAM_EXT16     = 7,  /* ESRAM16 */
+    SDRAM_CODE      = 8,  /* Program memory */
+    ESRAM_CODE      = 9,  /* ESRAM code */
+    LOCKED_CODE     = 10, /* For way locking */
+    NB_DSP_MEMORY_TYPE,
+    DEFAULT_DSP_MEM_TYPE = MASK_ALL16
+} t_dsp_memory_type_id;
+
+typedef struct {
+    t_cm_allocator_desc  *allocDesc;
+    t_cm_system_address   baseAddress;
+    t_uint32              referenceCounter;
+} t_dsp_allocator_desc;
+
+typedef struct {
+    t_cm_system_address  base;
+    t_uint32             size;
+} t_dsp_segment;
+
+typedef enum {
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    SDRAM_CODE_EE,
+    SDRAM_CODE_USER,
+    SDRAM_DATA_EE,
+    SDRAM_DATA_USER,
+    NB_MIGRATION_SEGMENT,
+    ESRAM_CODE_EE = NB_MIGRATION_SEGMENT,
+    ESRAM_CODE_USER,
+    ESRAM_DATA_EE,
+    ESRAM_DATA_USER,
+#else
+    SDRAM_CODE_EE,
+    SDRAM_DATA_EE,
+    ESRAM_CODE_EE,
+    ESRAM_DATA_EE,
+#endif
+    NB_DSP_SEGMENT_TYPE
+} t_dsp_segment_type;
+
+typedef struct {
+    t_dsp_segment_type  segmentType;
+    t_uint32            baseOffset;
+} t_dsp_address_info;
+
+typedef enum  {
+    MPC_STATE_UNCONFIGURED,
+    MPC_STATE_BOOTABLE,
+    MPC_STATE_BOOTED,
+    MPC_STATE_PANIC,
+} t_dsp_state;
+
+typedef struct {
+    t_dsp_state             state;
+    t_uint8                 nbYramBank;
+    t_cm_domain_id          domainEE;
+    t_dsp_allocator_desc   *allocator[NB_DSP_MEMORY_TYPE];
+    t_dsp_segment           segments[NB_DSP_SEGMENT_TYPE];
+    t_uint32                yram_offset;
+    t_uint32                yram_size;
+    t_uint32                locked_offset;
+    t_uint32                locked_size;
+} t_dsp_desc;
+
+typedef struct {
+    t_nmf_core_id coreId;
+    t_dsp_memory_type_id memType; // Index in MPC desc allocator
+    t_cm_allocator_desc *alloc;
+} t_dsp_chunk_info;
+
+PUBLIC const t_dsp_desc* cm_DSP_GetState(t_nmf_core_id coreId);
+PUBLIC void cm_DSP_SetStatePanic(t_nmf_core_id coreId);
+
+PUBLIC void cm_DSP_Init(const t_nmf_memory_segment *pEsramDesc);
+PUBLIC void cm_DSP_Destroy(void);
+
+/*!
+ * \brief Initialize the memory segments management of a given MPC
+ *
+ * \param[in] coreId Identifier of the DSP to initialize
+ * \param[in] pDspMapDesc DSP mapping into host space
+ * \param[in] memConf configuration of the DSP memories (standalone or shared)
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_Add(t_nmf_core_id coreId,
+        t_uint8 nbYramBanks,
+        const t_cm_system_address *pDspMapDesc,
+        const t_cm_domain_id eeDomain,
+        t_dsp_allocator_desc *sdramCodeAllocDesc,
+        t_dsp_allocator_desc *sdramDataAllocDesc);
+
+
+
+/*!
+ * \brief Configure a given Media Processor Core
+ *
+ * This routine programs the configuration (caches, ahb wrapper, ...) registers of a given MPC.
+ *
+ * \param[in] coreId Identifier of the DSP to initialize
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_Boot(t_nmf_core_id coreId);
+
+/*!
+ * \brief Boot a given DSP
+ *
+ * This routine allows after having initialized and loaded the EE into a given DSP to start it (boot it)
+ *
+ * \param[in] coreId identifier of the DSP to boot
+ * \param[in] panicReasonOffset offset of panic reason which will pass to NONE_PANIC when DSP booted.
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_ConfigureAfterBoot(t_nmf_core_id coreId);
+
+PUBLIC void cm_DSP_Start(t_nmf_core_id coreId);
+
+PUBLIC void cm_DSP_Stop(t_nmf_core_id coreId);
+
+/*!
+ * \brief Shutdown a given DSP
+ *
+ * This routine allows to stop and shutdown a given DSP
+ *
+ * \param[in] coreId identifier of the DSP to shutdown
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_Shutdown(t_nmf_core_id coreId);
+
+PUBLIC t_uint32 cm_DSP_ReadXRamWord(t_nmf_core_id coreId, t_uint32 dspOffset);
+PUBLIC void cm_DSP_WriteXRamWord(t_nmf_core_id coreId, t_uint32 dspOffset, t_uint32 value);
+
+/*!
+ * \brief Convert a Dsp address (offset inside a given DSP memory segment) into the  host address (logical)
+ *
+ * \param[in] coreId identifier of the given DSP
+ * \param[in] dspAddress dsp address to be converted
+ * \param[in] memType memory type identifier
+ *
+ * \retval t_cm_logical_address
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_logical_address cm_DSP_ConvertDspAddressToHostLogicalAddress(t_nmf_core_id coreId, t_shared_addr dspAddress);
+
+/*!
+ * \brief Acknowledge the local interrupt of a given DSP (when not using HW semaphore mechanisms)
+ *
+ * \param[in] coreId identifier of the given DSP
+ * \param[in] irqNum irq identifier
+ *
+ * \retval void
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_AcknowledgeDspIrq(t_nmf_core_id coreId, t_mpc2host_irq_num irqNum);
+
+
+/*
+ * Memory Management API routines
+ */
+
+/*!
+ * \brief Retrieve DSP information for a memory chunk.
+ *
+ * This function retrieves information stored in user-data of the allocated chunk.
+ * See also \ref{t_dsp_chunk_info}.
+ *
+ * \param[in]  memHandle Handle to the allocated chunk.
+ * \param[out] info Dsp information structure.
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_GetDspChunkInfo(t_memory_handle memHandle, t_dsp_chunk_info *info);
+
+/*!
+ * \brief Get memory allocator for a given memory type on a DSP.
+ *
+ * \param[in]  coreId Dsp identifier.
+ * \param[in]  memType Memory type identifier.
+ *
+ * \retval reference to the allocator descriptor (or null)
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_allocator_desc* cm_DSP_GetAllocator(t_nmf_core_id coreId, t_dsp_memory_type_id memType);
+
+/*!
+ * \brief Get DSP internal memory (TCM) information for allocation.
+ *
+ * For DSP-internal memories (TCMX, Y 16/24), return the offset and size of the allocation zone (for domain
+ * mechanism) and the allocation memory type.
+ *
+ * \param[in]  coreId Dsp identifier.
+ * \param[in]  memType Memory type identifier.
+ * \param[out] mem_info Memory information structure.
+ *
+ * \retval CM_OK
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_GetInternalMemoriesInfo(t_cm_domain_id domainId, t_dsp_memory_type_id memType,
+        t_uint32 *offset, t_uint32 *size);
+
+
+/*!
+ * \brief Convert word size to byte size.
+ *
+ * \param[in] memType Memory type identifier.
+ * \param[in] wordSize Word size to be converted.
+ *
+ * \retval Byte size.
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_uint32 cm_DSP_ConvertSize(t_dsp_memory_type_id memType, t_uint32 wordSize);
+
+/*!
+ * \brief Provide the Memory status of a given memory type for a given DSP
+ *
+ * \param[in] coreId dsp identifier.
+ * \param[in] memType Type of memory.
+ * \param[out] pStatus requested memory status
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_GetAllocatorStatus(t_nmf_core_id coreId, t_dsp_memory_type_id memType, t_uint32 offset, t_uint32 size, t_cm_allocator_status *pStatus);
+
+/*!
+ * \brief Provide DSP memory host shared address
+ *
+ * \param[in] memHandle Allocated block handle
+ * \param[out] pAddr Returned system address.
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_GetHostSystemAddress( t_memory_handle memHandle, t_cm_system_address *pAddr);
+
+/*!
+ * \brief Get physical address of a memory chunk.
+ *
+ * \param[in] memHandle Memory handle.
+ *
+ * \retval Physical address.
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_physical_address cm_DSP_GetPhysicalAdress(t_memory_handle memHandle);
+
+/*!
+ * \brief Return Logical Address of an allocated memory chunk.
+ *
+ * \param[in] memHandle Allocated chunk handle
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_logical_address cm_DSP_GetHostLogicalAddress(t_memory_handle memHandle);
+
+/*!
+ * \brief Provide DSP memory DSP address (offset inside a given DSP memory segment)
+ *
+ * \param[in] memHandle Allocated block handle
+ * \param[out] dspAddress allocated block address seen by the given DSP
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_GetDspAddress(t_memory_handle handle, t_uint32 *pDspAddress);
+
+/*!
+ * \brief Return the adress of the DSP base associated to the memory type.
+ * Caution, this information is valid only in normal state (not when migrated).
+ *
+ * \param[in]  coreId DSP Identifier.
+ * \param[in]  memType Type of memory.
+ * \param[out] pAddr Base address.
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_GetDspBaseAddress(t_nmf_core_id coreId, t_dsp_memory_type_id memType, t_cm_system_address *pAddr);
+
+/*!
+ * \brief Return DSP memory handle offset (offset inside a given DSP memory)
+ *
+ * \param[in] coreId dsp identifier.
+ * \param[in] memType Type of memory.
+ * \param[in] memHandle Allocated block handle
+ *
+ * \retval t_uint32: Offset of memory handle inside memory
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_uint32 cm_DSP_GetDspMemoryHandleOffset(
+        t_nmf_core_id coreId,
+        t_dsp_memory_type_id dspMemType,
+        t_memory_handle memHandle);
+
+/*!
+ * \brief Provide DSP memory handle size
+ *
+ * \param[in] memHandle Allocated block handle
+ * \param[out] pDspSize Size of the given memory handle
+
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_GetDspMemoryHandleSize(t_memory_handle memHandle, t_uint32 *pDspSize);
+
+/*!
+ * \brief Resize xram allocator to reserve spave for stack.
+ *
+ * \param[in] coreId dsp identifier.
+ * \param[in] newStackSize New required stack size.
+
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_setStackSize(t_nmf_core_id coreId, t_uint32 newStackSize);
+
+/*!
+ * \brief Allow to know if nbYramBanks parameter is valid for coreId. This api is need since use of nbYramBanks
+ *        is deferred.
+ *
+ * \param[in] coreId dsp identifier.
+ * \param[in] nbYramBanks number of yramBanks to use.
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_IsNbYramBanksValid(t_nmf_core_id coreId, t_uint8 nbYramBanks);
+
+/*!
+ * \brief Allow to know stack base address according to coreId and nbYramBanks use.
+ *
+ * \param[in] coreId dsp identifier.
+ * \param[in] nbYramBanks number of yramBanks to use.
+ *
+ * \retval t_uint32 return stack address
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_uint32 cm_DSP_getStackAddr(t_nmf_core_id coreId);
+
+/*!
+ * \brief For a give dsp adress return the offset from the hardware base that the adress is relative to.
+ *
+ * \param[in]  coreId DSP identifier.
+ * \param[in]  adr    DSP address.
+ * \param[out] info   Info structure containing (hw base id, offset)
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_GetDspDataAddressInfo(t_nmf_core_id coreId, t_uint32 adr, t_dsp_address_info *info);
+
+/*!
+ * \brief Modify the mapping of a code hardware base. Used for memory migration.
+ *
+ * The function calculates the new hardware base so that in the DSP address-space,
+ * the source address will be mapped to the destination address.
+ *
+ * \param[in] coreId DSP Identifier.
+ * \param[in] hwSegment Identifier of the hardware segment (thus hardware base).
+ * \param[in] src Source address
+ * \param[in] dst Destination address
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_updateCodeBase(t_nmf_core_id coreId, t_dsp_segment_type hwSegment, t_cm_system_address src, t_cm_system_address dst);
+
+/*!
+ * \brief Modify the mapping of a data hardware base. Used for memory migration.
+ *
+ * The function calculates the new hardware base so that in the DSP address-space,
+ * the source address will be mapped to the destination address.
+ *
+ * \param[in] coreId DSP Identifier.
+ * \param[in] hwSegment Identifier of the hardware segment (thus hardware base).
+ * \param[in] src Source address
+ * \param[in] dst Destination address
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_updateDataBase(t_nmf_core_id coreId, t_dsp_segment_type hwSegment, t_cm_system_address src, t_cm_system_address dst);
+
+#endif /* __INC_CM_DSP_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/dsp/inc/semaphores_dsp.h b/drivers/staging/nmf-cm/cm/engine/dsp/inc/semaphores_dsp.h
new file mode 100644
index 00000000000..1bb1c34cced
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/dsp/inc/semaphores_dsp.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_CM_SEMAPHORES_DSP_H
+#define __INC_CM_SEMAPHORES_DSP_H
+
+#include <share/semaphores/inc/semaphores.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+PUBLIC void cm_DSP_SEM_Take(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_DSP_SEM_Give(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_DSP_SEM_GenerateIrq(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_DSP_AssertDspIrq(t_nmf_core_id coreId, t_host2mpc_irq_num irqNum);
+
+PUBLIC void cm_DSP_AcknowledgeDspIrq(t_nmf_core_id coreId, t_mpc2host_irq_num irqNum);
+
+#endif /* __INC_CM_SEMAPHORES_DSP_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h b/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h
new file mode 100644
index 00000000000..0ddc71d2c4f
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h
@@ -0,0 +1,959 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_MMDSP_HWP_H
+#define __INC_MMDSP_HWP_H
+
+#include <cm/inc/cm_type.h>
+
+#define MMDSP_NB_BLOCK_RAM      8
+#define MMDSP_RAM_BLOCK_SIZE    4096    /* 0x1000 */
+#define MMDSP_NB_TIMER          3
+#define MMDSP_NB_BIT_SEM        8
+#define MMDSP_NB_DMA_IF         8
+#define MMDSP_NB_DMA_CTRL       4
+#define MMDSP_NB_ITREMAP_REG    32
+
+#define MMDSP_INSTRUCTION_WORD_SIZE     (sizeof(t_uint64))
+#define MMDSP_ICACHE_LINE_SIZE_IN_INST  (4)
+#define MMDSP_ICACHE_LINE_SIZE  (MMDSP_ICACHE_LINE_SIZE_IN_INST * MMDSP_INSTRUCTION_WORD_SIZE)
+
+#define MMDSP_DATA_WORD_SIZE     (3)
+#define MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE (sizeof(t_uint32))
+#define MMDSP_DATA_WORD_SIZE_IN_EXT24 (sizeof(t_uint32))
+#define MMDSP_DATA_WORD_SIZE_IN_EXT16 (sizeof(t_uint16))
+#define MMDSP_DCACHE_LINE_SIZE_IN_WORDS  (8)
+#define MMDSP_DCACHE_LINE_SIZE  (MMDSP_DCACHE_LINE_SIZE_IN_WORDS * sizeof(t_uint32))
+
+#define MMDSP_NB_IO             16
+
+#define MMDSP_CODE_CACHE_WAY_SIZE   256
+
+//#define MMDSP_ESRAM_DSP_BASE_ADDR         0xE0000 /* 64-bit words */
+//#define MMDSP_DATA24_DSP_BASE_ADDR        0x10000
+//#define MMDSP_DATA16_DSP_BASE_ADDR        0x800000
+//#define MMDSP_MMIO_DSP_BASE_ADDR          0xF80000
+
+/* Specified according MMDSP & ELF convention */
+/* Note: Here we assume that ESRAM is less than 2MB */
+#define SDRAMTEXT_BASE_ADDR             0x00000000
+#define ESRAMTEXT_BASE_ADDR             0x000E0000
+
+#define SDRAMMEM24_BASE_ADDR            0x00010000
+#define ESRAMMEM24_BASE_ADDR            0x00600000 /* ELF == 0x00400000 TODO: Update it in MMDSP ELF compiler */
+#define SDRAMMEM16_BASE_ADDR            0x00800000
+#define ESRAMMEM16_BASE_ADDR            0x00D80000 /* ELF == 0x00BC0000 TODO: Update it in MMDSP ELF compiler */
+
+#define MMIO_BASE_ADDR                  0x00F80000
+
+/*
+ * Definition of indirect host registers
+ */
+#define IHOST_ICACHE_FLUSH_REG                            0x0
+#define IHOST_ICACHE_FLUSH_CMD_ENABLE                     (t_uint64)MASK_BIT0
+#define IHOST_ICACHE_FLUSH_ALL_ENTRIES_CMD                (t_uint64)0x0
+#if 0
+#define IHOST_ICACHE_INVALID_ALL_UNLOCKED_L2_LINES_CMD    (t_uint64)0x8
+#define IHOST_ICACHE_INVALID_ALL_LOCKED_L2_LINES_CMD      (t_uint64)0xA
+#define IHOST_ICACHE_UNLOCK_ALL_LOCKED_L2_LINES_CMD       (t_uint64)0xC
+#define IHOST_ICACHE_LOCK_ALL_WAYS_LESSER_THAN_LOCK_V_CMD (t_uint64)0xE
+#else
+#define IHOST_ICACHE_INVALID_ALL_UNLOCKED_L2_LINES_CMD    (t_uint64)0x10
+#define IHOST_ICACHE_INVALID_ALL_LOCKED_L2_LINES_CMD      (t_uint64)0x12
+#define IHOST_ICACHE_UNLOCK_ALL_LOCKED_L2_LINES_CMD       (t_uint64)0x14
+#define IHOST_ICACHE_LOCK_ALL_WAYS_LESSER_THAN_LOCK_V_CMD (t_uint64)0x16
+#define IHOST_ICACHE_FLUSH_BY_SERVICE                     (t_uint64)0x18
+#define IHOST_ICACHE_FLUSH_OUTSIDE_RANGE                  (t_uint64)0x1A
+#endif
+
+#define IHOST_ICACHE_LOCK_V_REG                           0x1
+
+#define IHOST_ICACHE_MODE_REG                             0x2
+#define IHOST_ICACHE_MODE_PERFMETER_ON                    (t_uint64)MASK_BIT0
+#define IHOST_ICACHE_MODE_PERFMETER_OFF                   (t_uint64)0x0
+#define IHOST_ICACHE_MODE_L2_CACHE_ON                     (t_uint64)MASK_BIT1
+#define IHOST_ICACHE_MODE_L2_CACHE_OFF                    (t_uint64)0x0
+#define IHOST_ICACHE_MODE_L1_CACHE_ON                     (t_uint64)MASK_BIT2
+#define IHOST_ICACHE_MODE_L1_CACHE_OFF                    (t_uint64)0x0
+#define IHOST_ICACHE_MODE_FILL_MODE_ON                    (t_uint64)MASK_BIT3
+#define IHOST_ICACHE_MODE_FILL_MODE_OFF                   (t_uint64)0x0
+
+#define IHOST_CLEAR_PERFMETER_REG                         0x3
+#define IHOST_CLEAR_PERFMETER_ON                          (t_uint64)0x1
+#define IHOST_CLEAR_PERFMETER_OFF                         (t_uint64)0x0
+
+#define IHOST_PERF_HIT_STATUS_REG                         0x4
+
+#define IHOST_PERF_MISS_STATUS_REG                        0x5
+
+#define IHOST_FILL_START_WAY_REG                          0x6
+#define IHOST_FILL_START_ADDR_VALUE_SHIFT                 0U
+#define IHOST_FILL_WAY_NUMBER_SHIFT                       20U
+
+#define IHOST_PRG_BASE_ADDR_REG                           0x7
+#define IHOST_PRG_BASE1_ADDR_SHIFT                        0
+#define IHOST_PRG_BASE2_ADDR_SHIFT                        32
+
+#if defined(__STN_8500) && (__STN_8500>10)
+#define IHOST_PRG_BASE_34_ADDR_REG                        0x1A
+#define IHOST_PRG_BASE3_ADDR_SHIFT                        0
+#define IHOST_PRG_BASE4_ADDR_SHIFT                        32
+#endif
+
+#if defined(__STN_8815) /* __STN_8815 */
+#define IHOST_PRG_AHB_CONF_REG                            0x8
+#define IHOST_PRG_AHB_LOCKED_SHIFT                        0U
+#define IHOST_PRG_AHB_PROT_SHIFT                          1U
+
+#define AHB_LOCKED_ON                                   (t_uint64)1
+#define AHB_LOCKED_OFF                                  (t_uint64)0
+
+#define AHB_PROT_USER                                   (t_uint64)0
+#define AHB_PROT_PRIVILEGED                             (t_uint64)MASK_BIT0
+#define AHB_PROT_NONBUFFERABLE                          (t_uint64)0
+#define AHB_PROT_BUFFERABLE                             (t_uint64)MASK_BIT1
+#define AHB_PROT_NONCACHEABLE                           (t_uint64)0
+#define AHB_PROT_CACHEABLE                              (t_uint64)MASK_BIT2
+
+
+#define IHOST_DATA_AHB_CONF_REG                           0x9
+#define IHOST_DATA_AHB_LOCKED_SHIFT                       0U
+#define IHOST_DATA_AHB_PROT_SHIFT                         1U
+#else /* def __STN_8820 or __STN_8500 */
+#define IHOST_STBUS_ID_CONF_REG                           0x8
+#define SAA_STBUS_ID                                      176 /* = 0xB0 */
+#define SVA_STBUS_ID                                      4   /* = 0x4  */
+#define SIA_STBUS_ID                                      180 /* = 0xB4 */
+
+#define IHOST_STBUF_CONF_REG                              0x9  /* RESERVED */
+#endif /* __STN_8820 or __STN_8500 */
+
+#define IHOST_DATA_EXT_BUS_BASE_REG                       0xA
+#define IHOST_DATA_EXT_BUS_BASE_16_SHIFT                  32ULL
+#define IHOST_DATA_EXT_BUS_BASE_24_SHIFT                  0ULL
+
+#define IHOST_EXT_MMIO_BASE_DATA_EXT_BUS_TOP_REG          0xB
+#define IHOST_EXT_MMIO_DATA_EXT_BUS_TOP_SHIFT             0ULL
+#define IHOST_EXT_MMIO_BASE_ADDR_SHIFT                    32ULL
+
+#define IHOST_DATA_EXT_BUS_BASE2_REG                      0xC
+#define IHOST_DATA_EXT_BUS_BASE2_16_SHIFT                 32ULL
+#define IHOST_DATA_EXT_BUS_BASE2_24_SHIFT                 0ULL
+
+#if defined(__STN_8500) && (__STN_8500>10)
+
+#define IHOST_DATA_EXT_BUS_BASE3_REG                      0x1B
+#define IHOST_DATA_EXT_BUS_BASE3_16_SHIFT                 32ULL
+#define IHOST_DATA_EXT_BUS_BASE3_24_SHIFT                 0ULL
+
+#define IHOST_DATA_EXT_BUS_BASE4_REG                      0x1C
+#define IHOST_DATA_EXT_BUS_BASE4_16_SHIFT                 32ULL
+#define IHOST_DATA_EXT_BUS_BASE4_24_SHIFT                 0ULL
+
+#endif
+
+#define IHOST_ICACHE_STATE_REG                            0xD
+#define IHOST_ICACHE_STATE_RESET                          0x0
+#define IHOST_ICACHE_STATE_INITAGL2                       0x1
+#define IHOST_ICACHE_STATE_READY_TO_START                 0x2
+#define IHOST_ICACHE_STATE_WAIT_FOR_MISS                  0x3
+#define IHOST_ICACHE_STATE_FILLDATARAM0                   0x4
+#define IHOST_ICACHE_STATE_FILLDATARAM1                   0x5
+#define IHOST_ICACHE_STATE_FILLDATARAM2                   0x6
+#define IHOST_ICACHE_STATE_FILLDATARAM3                   0x7
+#define IHOST_ICACHE_STATE_FLUSH                          0x8
+#define IHOST_ICACHE_STATE_FILL_INIT                      0x9
+#define IHOST_ICACHE_STATE_FILL_LOOP                      0xA
+#define IHOST_ICACHE_STATE_FILL_LOOP0                     0xB
+#define IHOST_ICACHE_STATE_FILL_LOOP1                     0xC
+#define IHOST_ICACHE_STATE_FILL_LOOP2                     0xD
+#define IHOST_ICACHE_STATE_FILL_LOOP3                     0xE
+#define IHOST_ICACHE_STATE_FILL_END                       0xF
+#define IHOST_ICACHE_STATE_SPECIFIC_FLUSH_R               0x10
+#define IHOST_ICACHE_STATE_SPECIFIC_FLUSH_W               0x11
+#define IHOST_ICACHE_STATE_SPECIFIC_FLUSH_END             0x12
+#define IHOST_ICACHE_STATE_OTHERS                         0x1F
+
+#define IHOST_EN_EXT_BUS_TIMEOUT_REG                      0xE
+#define IHOST_TIMEOUT_ENABLE                              1ULL
+#define IHOST_TIMEOUT_DISABLE                             0ULL
+
+#define IHOST_DATA2_1624_XA_BASE_REG                      0xF
+#define IHOST_DATA2_24_XA_BASE_SHIFT                      0ULL
+#define IHOST_DATA2_16_XA_BASE_SHIFT                      32ULL
+#if defined(__STN_8500) && (__STN_8500>10)
+#define IHOST_DATA3_24_XA_BASE_SHIFT                      8ULL
+#define IHOST_DATA3_16_XA_BASE_SHIFT                      40ULL
+#define IHOST_DATA4_24_XA_BASE_SHIFT                      16ULL
+#define IHOST_DATA4_16_XA_BASE_SHIFT                      48ULL
+#endif
+
+#define IHOST_PERFMETERS_MODE_REG                         0x10
+
+#if defined(__STN_8815) /* __STN_8815 */
+#define IHOST_EXT_MMIO_AHB_CONF_REG                       0x11
+#define IHOST_EXT_MMIO_AHB_LOCKED_SHIFT                   0U
+#define IHOST_EXT_MMIO_AHB_PROT_SHIFT                     1U
+#else /* def __STN_8820 or __STN_8500 */
+#define IHOST_EXT_MMIO_STBS_CONF_REG                      0x11 /* RESERVED */
+#endif /* __STN_8820 or __STN_8500 */
+
+#define IHOST_PRG_BASE_SEL_REG                            0x12
+#define IHOST_PRG_BASE_SEL_OFF                            (t_uint64)0
+#define IHOST_PRG_BASE_SEL_ON                             (t_uint64)1
+
+#define IHOST_PRG_BASE2_ACTIV_REG                         0x13
+#define IHOST_PRG_BASE2_ACTIV_OFF                         (t_uint64)0
+#if defined(__STN_8500) && (__STN_8500>10)
+/* TODO : for the moment just divide mmdsp in fix 4 spaces */
+    #define IHOST_PRG_BASE2_ACTIV_ON                      (t_uint64)((((t_uint64)0xf0000>>10)<<48) | (((t_uint64)0xe0000>>10)<<32) | (((t_uint64)0x70000>>10)<<16) | 1)
+#else
+    #define IHOST_PRG_BASE2_ACTIV_ON                      (t_uint64)1
+#endif
+
+#define IHOST_DATA_EXT_BUS_TOP_16_24_REG                      0x14
+#define IHOST_DATA_EXT_BUS_TOP_24_SHIFT                       0ULL
+#define IHOST_DATA_EXT_BUS_TOP_16_SHIFT                       32ULL
+
+#define IHOST_DATA_TOP_16_24_CHK_REG                      0x16
+#define IHOST_DATA_TOP_16_24_CHK_OFF                      (t_uint64)0
+#define IHOST_DATA_TOP_16_24_CHK_ON                       (t_uint64)1
+
+#define IHOST_EXT_BUS_TOP2_16_24_REG                      0x15
+#define IHOST_DATA_EXT_BUS_TOP2_24_SHIFT                  0ULL
+#define IHOST_DATA_EXT_BUS_TOP2_16_SHIFT                  32ULL
+
+#if defined(__STN_8500) && (__STN_8500>10)
+
+#define IHOST_EXT_BUS_TOP3_16_24_REG                      0x1D
+#define IHOST_DATA_EXT_BUS_TOP3_24_SHIFT                  0ULL
+#define IHOST_DATA_EXT_BUS_TOP3_16_SHIFT                  32ULL
+
+#define IHOST_EXT_BUS_TOP4_16_24_REG                      0x1E
+#define IHOST_DATA_EXT_BUS_TOP4_24_SHIFT                  0ULL
+#define IHOST_DATA_EXT_BUS_TOP4_16_SHIFT                  32ULL
+
+#endif
+
+#define IHOST_DATA_BASE2_ACTIV_REG                        0x17
+#define IHOST_DATA_BASE2_ACTIV_OFF                        (t_uint64)0
+#define IHOST_DATA_BASE2_ACTIV_ON                         (t_uint64)1
+
+#define IHOST_INST_BURST_SZ_REG                           0x18
+#define IHOST_INST_BURST_SZ_ALWAYS_1_LINE                 (t_uint64)0x0
+#define IHOST_INST_BURST_SZ_ALWAYS_2_LINES                (t_uint64)0x1
+#define IHOST_INST_BURST_SZ_AUTO                          (t_uint64)0x2  /* 2 lines for SDRAM [0, 0xE0000[, 1 line for ESRAM [0xE0000, 0xFFFFF] */
+
+#define IHOST_ICACHE_END_CLEAR_REG                        0x19
+#define IHOST_ICACHE_START_CLEAR_REG                      IHOST_FILL_START_WAY_REG
+
+/*
+ * Definition of value of the ucmd register
+ */
+#define MMDSP_UCMD_WRITE                  0
+#define MMDSP_UCMD_READ                   4
+#define MMDSP_UCMD_CTRL_STATUS_ACCESS     0x10 // (MASK_BIT4 | !MASK_BIT3 | !MASK_BIT0)
+#define MMDSP_UCMD_DECREMENT_ADDR         MASK_BIT5
+#define MMDSP_UCMD_INCREMENT_ADDR         MASK_BIT1
+
+/*
+ * Definition of value of the ubkcmd register
+ */
+#define MMDSP_UBKCMD_EXT_CODE_MEM_ACCESS_ENABLE    MASK_BIT3
+#define MMDSP_UBKCMD_EXT_CODE_MEM_ACCESS_DISABLE   0
+
+/*
+ * Definition of value of the clockcmd register
+ */
+#define MMDSP_CLOCKCMD_STOP_CLOCK         MASK_BIT0
+#define MMDSP_CLOCKCMD_START_CLOCK        0
+
+/*
+ * Definition of macros used to access indirect addressed host register
+ */
+#define WRITE_INDIRECT_HOST_REG(pRegs, addr, value64) \
+{ \
+    (pRegs)->host_reg.emul_uaddrl = addr; \
+    (pRegs)->host_reg.emul_uaddrm = 0;    \
+    (pRegs)->host_reg.emul_uaddrh = 0;    \
+    (pRegs)->host_reg.emul_udata[0] = ((value64 >> 0ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[1] = ((value64 >> 8ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[2] = ((value64 >> 16ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[3] = ((value64 >> 24ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[4] = ((value64 >> 32ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[5] = ((value64 >> 40ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[6] = ((value64 >> 48ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[7] = ((value64 >> 56ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_ucmd = (MMDSP_UCMD_CTRL_STATUS_ACCESS | MMDSP_UCMD_WRITE); \
+}
+
+#define READ_INDIRECT_HOST_REG(pRegs, addr, value64) \
+{ \
+    (pRegs)->host_reg.emul_udata[0] = 0;  \
+    (pRegs)->host_reg.emul_udata[1] = 0;  \
+    (pRegs)->host_reg.emul_udata[2] = 0;  \
+    (pRegs)->host_reg.emul_udata[3] = 0;  \
+    (pRegs)->host_reg.emul_udata[4] = 0;  \
+    (pRegs)->host_reg.emul_udata[5] = 0;  \
+    (pRegs)->host_reg.emul_udata[6] = 0;  \
+    (pRegs)->host_reg.emul_udata[7] = 0;  \
+    (pRegs)->host_reg.emul_uaddrl = addr; \
+    (pRegs)->host_reg.emul_uaddrm = 0;    \
+    (pRegs)->host_reg.emul_uaddrh = 0;    \
+    (pRegs)->host_reg.emul_ucmd = (MMDSP_UCMD_CTRL_STATUS_ACCESS | MMDSP_UCMD_READ); \
+    value64 = (((t_uint64)((pRegs)->host_reg.emul_udata[0])) << 0ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[1])) << 8ULL)  |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[2])) << 16ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[3])) << 24ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[4])) << 32ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[5])) << 40ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[6])) << 48ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[7])) << 56ULL); \
+}
+
+/* Common type to handle 64-bit modulo field in 32-bit mode */
+typedef struct {
+    t_uint32 value;
+    t_uint32 dummy;
+} t_mmdsp_field_32;
+
+typedef struct {
+    t_uint16 value;
+    t_uint16 dummy;
+} t_mmdsp_field_16;
+
+/* DCache registers */
+#define DCACHE_MODE_ENABLE              MASK_BIT0
+#define DCACHE_MODE_DISABLE             0
+#define DCACHE_MODE_DIVIDE_PER_2        MASK_BIT1
+#define DCACHE_MODE_DIVIDE_PER_4        MASK_BIT2
+#define DCACHE_MODE_CHECK_TAG_ENABLE    MASK_BIT3
+#define DCACHE_MODE_CHECK_TAG_DISABLE   0
+#define DCACHE_MODE_FORCE_LOCK_MODE     MASK_BIT4
+#define DCACHE_MODE_LOCK_BIT            MASK_BIT5
+
+#define DCACHE_CONTROL_PREFETCH_LINE            MASK_BIT0
+#define DCACHE_CONTROL_NON_BLOCKING_REFILL      0
+#define DCACHE_CONTROL_FAST_READ_DISABLE        MASK_BIT1
+#define DCACHE_CONTROL_FAST_READ_ENABLE         0
+#define DCACHE_CONTROL_ON_FLY_FILL_ACCESS_OFF   MASK_BIT2
+#define DCACHE_CONTROL_ON_FLY_FILL_ACCESS_ON    0
+#define DCACHE_CONTROL_BURST_1_WRAP8            MASK_BIT3
+#define DCACHE_CONTROL_BURST_2_WRAP4            0
+#define DCACHE_CONTROL_NOT_USE_DATA_BUFFER      MASK_BIT4
+#define DCACHE_CONTROL_USE_DATA_BUFFER          0
+#define DCACHE_CONTROL_WRITE_POSTING_ENABLE     MASK_BIT5
+#define DCACHE_CONTROL_WRITE_POSTING_DISABLE    0
+
+#define DCACHE_CMD_NOP                          0
+#define DCACHE_CMD_DISCARD_WAY                  2 //see Dcache_way reg
+#define DCACHE_CMD_DISCARD_LINE                 3 //see Dcache_line reg
+#define DCACHE_CMD_FREE_WAY                     4 //see Dcache_way reg
+#define DCACHE_CMD_FREE_LINE                    5 //see Dchache_line reg
+#define DCACHE_CMD_FLUSH                        7
+
+#define DCACHE_STATUS_CURRENT_WAY_MASK          (MASK_BIT2 | MASK_BIT1 | MASK_BIT0)
+#define DCACHE_STATUS_TAG_HIT_MASK              MASK_BIT3
+#define DCACHE_STATUS_TAG_LOCKED_MASK           MASK_BIT4
+#define DCACHE_STATUS_PROTECTION_ERROR_MASK     MASK_BIT5
+
+#define DCACHE_CPTRSEL_COUNTER_1_MASK           (MASK_BIT3 | MASK_BIT2 | MASK_BIT1 | MASK_BIT0)
+#define DCACHE_CPTRSEL_COUNTER_1_SHIFT          0
+#define DCACHE_CPTRSEL_COUNTER_2_MASK           (MASK_BIT7 | MASK_BIT6 | MASK_BIT5 | MASK_BIT4)
+#define DCACHE_CPTRSEL_COUNTER_2_SHIFT          4
+#define DCACHE_CPTRSEL_COUNTER_3_MASK           (MASK_BIT11 | MASK_BIT10 | MASK_BIT9 | MASK_BIT8)
+#define DCACHE_CPTRSEL_COUNTER_3_SHIFT          8
+#define DCACHE_CPTRSEL_XBUS_ACCESS_TO_CACHE_RAM 1
+#define DCACHE_CPTRSEL_CACHE_HIT                2
+#define DCACHE_CPTRSEL_LINE_MATCH               3
+#define DCACHE_CPTRSEL_XBUS_WS                  4
+#define DCACHE_CPTRSEL_EXTMEM_WS                5
+#define DCACHE_CPTRSEL_CACHE_READ               6
+#define DCACHE_CPTRSEL_CACHE_WRITE              7
+#define DCACHE_CPTRSEL_TAG_HIT_READ             8
+#define DCACHE_CPTRSEL_TAG_LOCKED_ACCESS        9
+#define DCACHE_CPTRSEL_TAG_MEM_READ_CYCLE       10
+#define DCACHE_CPTRSEL_TAG_MEM_WRITE_CYCLE      11
+
+
+typedef volatile struct {
+    t_uint16 padding_1[5];
+    t_uint16 mode;
+    t_uint16 control;
+    t_uint16 way;
+    t_uint16 line;
+    t_uint16 command;
+    t_uint16 status;
+    t_uint16 cptr1l;
+    t_uint16 cptr1h;
+    t_uint16 cptr2l;
+    t_uint16 cptr2h;
+    t_uint16 cptr3l;
+    t_uint16 cptr3h;
+    t_uint16 cptrsel;
+    t_uint16 flush_base_lsb; /* only on STn8820 and STn8500 */
+    t_uint16 flush_base_msb; /* only on STn8820 and STn8500 */
+    t_uint16 flush_top_lsb; /* only on STn8820 and STn8500 */
+    t_uint16 flush_top_msb; /* only on STn8820 and STn8500 */
+    t_uint16 padding_2[10];
+} t_mmdsp_dcache_regs_16;
+
+typedef volatile struct {
+    t_uint32 padding_1[5];
+    t_uint32 mode;
+    t_uint32 control;
+    t_uint32 way;
+    t_uint32 line;
+    t_uint32 command;
+    t_uint32 status;
+    t_uint32 cptr1l;
+    t_uint32 cptr1h;
+    t_uint32 cptr2l;
+    t_uint32 cptr2h;
+    t_uint32 cptr3l;
+    t_uint32 cptr3h;
+    t_uint32 cptrsel;
+    t_uint32 flush_base_lsb; /* only on STn8820 and STn8500 */
+    t_uint32 flush_base_msb; /* only on STn8820 and STn8500 */
+    t_uint32 flush_top_lsb; /* only on STn8820 and STn8500 */
+    t_uint32 flush_top_msb; /* only on STn8820 and STn8500 */
+    t_uint32 padding_2[10];
+} t_mmdsp_dcache_regs_32;
+
+/* TIMER Registers */
+typedef volatile struct {
+    t_mmdsp_field_16    timer_msb;
+    t_mmdsp_field_16    timer_lsb;
+} t_mmdsp_timer_regs_16;
+
+typedef volatile struct {
+    t_mmdsp_field_32    timer_msb;
+    t_mmdsp_field_32    timer_lsb;
+} t_mmdsp_timer_regs_32;
+
+
+/* DMA interface Registers */
+typedef volatile struct {
+    t_uint16    arm_dma_sreq;        /* dma0: 5e800, dma1: +0x20 ...*/
+    t_uint16    arm_dma_breq;        /* ... 5e802 */
+    t_uint16    arm_dma_lsreq;       /* ... 5e804 */
+    t_uint16    arm_dma_lbreq;
+    t_uint16    arm_dma_maskit;
+    t_uint16    arm_dma_it;
+    t_uint16    arm_dma_auto;
+    t_uint16    arm_dma_lauto;
+    t_uint16    dma_reserved[8];
+} t_mmdsp_dma_if_regs_16;
+
+typedef volatile struct {
+    t_uint32    arm_dma_sreq;        /* dma0: 3a800, dma1: +0x40 ...*/
+    t_uint32    arm_dma_breq;        /* ... 3a804 */
+    t_uint32    arm_dma_lsreq;       /* ... 3a808 */
+    t_uint32    arm_dma_lbreq;
+    t_uint32    arm_dma_maskit;
+    t_uint32    arm_dma_it;
+    t_uint32    arm_dma_auto;
+    t_uint32    arm_dma_lauto;
+    t_uint32    dma_reserved[8];
+} t_mmdsp_dma_if_regs_32;
+
+/* MMDSP DMA controller Registers */
+typedef volatile struct {
+    t_uint16    dma_ctrl;            /* dma0: 0x5d400, dma1: +0x10 ... */
+    t_uint16    dma_int_base;        /* ... 0x5d402 */
+    t_uint16    dma_int_length;      /* ... 0x5d404 */
+    t_uint16    dma_ext_baseh;
+    t_uint16    dma_ext_basel;
+    t_uint16    dma_count;
+    t_uint16    dma_ext_length;
+    t_uint16    dma_it_status;
+} t_mmdsp_dma_ctrl_regs_16;
+
+typedef volatile struct {
+    t_uint32    dma_ctrl;            /* dma0: 0x3a800, dma1: +0x20 ... */
+    t_uint32    dma_int_base;        /* ... 0x3a804 */
+    t_uint32    dma_int_length;      /* ... 0x3a808 */
+    t_uint32    dma_ext_baseh;
+    t_uint32    dma_ext_basel;
+    t_uint32    dma_count;
+    t_uint32    dma_ext_length;
+    t_uint32    dma_it_status;
+} t_mmdsp_dma_ctrl_regs_32;
+
+/* IO registers */
+typedef volatile struct {
+    t_mmdsp_field_16 io_bit[MMDSP_NB_IO];
+    t_mmdsp_field_16 io_lsb;
+    t_mmdsp_field_16 io_msb;
+    t_mmdsp_field_16 io_all;
+    t_mmdsp_field_16 io_en;
+} t_mmdsp_io_regs_16;
+
+typedef volatile struct {
+    t_mmdsp_field_32 io_bit[MMDSP_NB_IO];
+    t_mmdsp_field_32 io_lsb;
+    t_mmdsp_field_32 io_msb;
+    t_mmdsp_field_32 io_all;
+    t_mmdsp_field_32 io_en;
+} t_mmdsp_io_regs_32;
+
+/* HOST Registers bit mapping */
+#define HOST_GATEDCLK_ITREMAP   MASK_BIT0
+#define HOST_GATEDCLK_SYSDMA     MASK_BIT1
+#define HOST_GATEDCLK_INTEG_REGS MASK_BIT2
+#define HOST_GATEDCLK_TIMER_GPIO MASK_BIT3
+#define HOST_GATEDCLK_XBUSDMA   MASK_BIT4
+#define HOST_GATEDCLK_STACKCTRL MASK_BIT5
+#define HOST_GATEDCLK_ITC       MASK_BIT6
+
+/* Only for STn8820 and STn8500 */
+#define HOST_PWR_DBG_MODE       MASK_BIT0
+#define HOST_PWR_DC_STATUS      (MASK_BIT1 | MASK_BIT2 | MASK_BIT3 | MASK_BIT4 | MASK_BIT5)
+#define HOST_PWR_DE_STATUS      MASK_BIT6
+#define HOST_PWR_STOV_STATUS    MASK_BIT7
+
+/* HOST Registers */
+typedef volatile struct {
+    t_uint16    ident;               /*0x...60000*/
+    t_uint16    identx[4];           /*0x...60002..8*/
+    t_uint16    r5;                  /*0x...6000a*/
+    t_uint16    r6;                  /*0x...6000c*/
+    t_uint16    inte[2];             /*0x...6000e..10*/
+    t_uint16    intx[2];             /*0x...60012..14*/
+    t_uint16    int_ris[2];          /*0x...60016..18*/
+    t_uint16    intpol;              /*0x...6001a*/
+    t_uint16    pwr;                 /*0x...6001c*/ /* only on STn8820 and STn8500 */
+    t_uint16    gatedclk;            /*0x...6001e*/
+    t_uint16    softreset;           /*0x...60020*/
+    t_uint16    int_icr[2];          /*0x...60022..24*/
+    t_uint16    cmd[4];              /*0x...60026..2c*/
+    t_uint16    RESERVED4;
+    t_uint16    int_mis0;            /*0x...60030*/
+    t_uint16    RESERVED5;
+    t_uint16    RESERVED6;
+    t_uint16    RESERVED7;
+    t_uint16    i2cdiv;              /*0x...60038*/
+    t_uint16    int_mis1;            /*0x...6003a*/
+    t_uint16    RESERVED8;
+    t_uint16    RESERVED9;
+    t_uint16    emul_udata[8];      /*0x...60040..4e*/
+    t_uint16    emul_uaddrl;        /*0x...60050*/
+    t_uint16    emul_uaddrm;        /*0x...60052*/
+    t_uint16    emul_ucmd;          /*0x...60054*/
+    t_uint16    emul_ubkcmd;        /*0x...60056*/
+    t_uint16    emul_bk2addl;       /*0x...60058*/
+    t_uint16    emul_bk2addm;       /*0x...6005a*/
+    t_uint16    emul_bk2addh;       /*0x...6005c*/
+    t_uint16    emul_mdata[3];      /*0x...6005e..62*/
+    t_uint16    emul_maddl;         /*0x...60064*/
+    t_uint16    emul_maddm;         /*0x...60066*/
+    t_uint16    emul_mcmd;          /*0x...60068*/
+    t_uint16    emul_maddh;         /*0x...6006a*/
+    t_uint16    emul_uaddrh;        /*0x...6006c*/
+    t_uint16    emul_bk_eql;        /*0x...6006e*/
+    t_uint16    emul_bk_eqh;        /*0x...60070*/
+    t_uint16    emul_bk_combi;      /*0x...60072*/
+    t_uint16    emul_clockcmd;      /*0x...60074*/
+    t_uint16    emul_stepcmd;       /*0x...60076*/
+    t_uint16    emul_scanreg;       /*0x...60078*/
+    t_uint16    emul_breakcountl;   /*0x...6007a*/
+    t_uint16    emul_breakcounth;   /*0x...6007c*/
+    t_uint16    emul_forcescan;     /*0x...6007e*/
+    t_uint16    user_area[(0x200 - 0x80)>>1];
+} t_mmdsp_host_regs_16;
+
+typedef volatile struct {
+    t_uint32    ident;               /*0x...60000*/
+    t_uint32    identx[4];           /*0x...60004..10*/
+    t_uint32    r5;                  /*0x...60014*/
+    t_uint32    r6;                  /*0x...60018*/
+    t_uint32    inte[2];             /*0x...6001c..20*/
+    t_uint32    intx[2];             /*0x...60024..28*/
+    t_uint32    int_ris[2];          /*0x...6002c..30*/
+    t_uint32    intpol;              /*0x...60034*/
+    t_uint32    pwr;                 /*0x...60038*/ /* only on STn8820 and STn8500 */
+    t_uint32    gatedclk;            /*0x...6003c*/
+    t_uint32    softreset;           /*0x...60040*/
+    t_uint32    int_icr[2];          /*0x...60044..48*/
+    t_uint32    cmd[4];              /*0x...6004c..58*/
+    t_uint32    RESERVED4;
+    t_uint32    int_mis0;            /*0x...60060*/
+    t_uint32    RESERVED5;
+    t_uint32    RESERVED6;
+    t_uint32    RESERVED7;
+    t_uint32    i2cdiv;              /*0x...60070*/
+    t_uint32    int_mis1;            /*0x...60074*/
+    t_uint32    RESERVED8;
+    t_uint32    RESERVED9;
+    t_uint32    emul_udata[8];      /*0x...60080..9c*/
+    t_uint32    emul_uaddrl;        /*0x...600a0*/
+    t_uint32    emul_uaddrm;        /*0x...600a4*/
+    t_uint32    emul_ucmd;          /*0x...600a8*/
+    t_uint32    emul_ubkcmd;        /*0x...600ac*/
+    t_uint32    emul_bk2addl;       /*0x...600b0*/
+    t_uint32    emul_bk2addm;       /*0x...600b4*/
+    t_uint32    emul_bk2addh;       /*0x...600b8*/
+    t_uint32    emul_mdata[3];      /*0x...600bc..c4*/
+    t_uint32    emul_maddl;         /*0x...600c8*/
+    t_uint32    emul_maddm;         /*0x...600cc*/
+    t_uint32    emul_mcmd;          /*0x...600d0*/
+    t_uint32    emul_maddh;         /*0x...600d4*/
+    t_uint32    emul_uaddrh;        /*0x...600d8*/
+    t_uint32    emul_bk_eql;        /*0x...600dc*/
+    t_uint32    emul_bk_eqh;        /*0x...600e0*/
+    t_uint32    emul_bk_combi;      /*0x...600e4*/
+    t_uint32    emul_clockcmd;      /*0x...600e8*/
+    t_uint32    emul_stepcmd;       /*0x...600ec*/
+    t_uint32    emul_scanreg;       /*0x...600f0*/
+    t_uint32    emul_breakcountl;   /*0x...600f4*/
+    t_uint32    emul_breakcounth;   /*0x...600f8*/
+    t_uint32    emul_forcescan;     /*0x...600fc*/
+    t_uint32    user_area[(0x400 - 0x100)>>2];
+} t_mmdsp_host_regs_32;
+
+/* MMIO blocks */
+#if defined(__STN_8820) || defined(__STN_8500)
+typedef volatile struct {
+    t_uint16 RESERVED1[(0xD400-0x8000)>>1];
+
+    t_mmdsp_dma_ctrl_regs_16 dma_ctrl[MMDSP_NB_DMA_CTRL];
+
+    t_uint16 RESERVED2[(0xD800-0xD440)>>1];
+
+    t_mmdsp_dcache_regs_16 dcache;
+
+    t_uint16 RESERVED3[(0xE000-0xD840)>>1];
+
+    t_mmdsp_io_regs_16 io;
+
+    t_uint16 RESERVED4[(0x60-0x50)>>1];
+
+    t_mmdsp_timer_regs_16 timer[MMDSP_NB_TIMER];
+
+    t_uint16 RESERVED5[(0x410-0x78)>>1];
+
+    t_mmdsp_field_16 sem[MMDSP_NB_BIT_SEM];
+
+    t_uint16 RESERVED6[(0x450-0x430)>>1];
+
+    t_mmdsp_field_16 ipen;
+    t_uint16 itip_0;
+    t_uint16 itip_1;
+    t_uint16 itip_2;
+    t_uint16 itip_3;
+    t_uint16 itop_0;
+    t_uint16 itop_1;
+    t_uint16 itop_2;
+    t_uint16 itop_3;
+    t_uint16 RESERVED7[(0x8a-0x64)>>1];
+    t_uint16 itip_4;
+    t_uint16 itop_4;
+
+    t_uint16 RESERVED8[(0x7e0-0x48e)>>1];
+
+    t_mmdsp_field_16 id[4];
+    t_mmdsp_field_16 idp[4];
+
+    t_mmdsp_dma_if_regs_16 dma_if[MMDSP_NB_DMA_IF];
+
+    t_uint16 RESERVED9[(0xC00-0x900)>>1];
+
+    t_mmdsp_field_16 emu_unit_maskit;
+    t_mmdsp_field_16 RESERVED[3];
+    t_mmdsp_field_16 config_data_mem;
+    t_mmdsp_field_16 compatibility;
+
+    t_uint16 RESERVED10[(0xF000-0xEC18)>>1];
+
+    t_uint16 stbus_if_config;
+    t_uint16 stbus_if_mode;
+    t_uint16 stbus_if_status;
+    t_uint16 stbus_if_security;
+    t_uint16 stbus_if_flush;
+    t_uint16 stbus_reserved;
+    t_uint16 stbus_if_priority;
+    t_uint16 stbus_msb_attribut;
+
+    t_uint16 RESERVED11[(0xFC00-0xF010)>>1];
+
+    t_mmdsp_field_16 itremap_reg[MMDSP_NB_ITREMAP_REG];
+    t_mmdsp_field_16 itmsk_l_reg;
+    t_mmdsp_field_16 itmsk_h_reg;
+
+    t_uint16 RESERVED12[(0xfc9c - 0xfc88)>>1];
+
+    t_mmdsp_field_16 itmemo_l_reg;
+    t_mmdsp_field_16 itmeme_h_reg;
+
+    t_uint16 RESERVED13[(0xfd00 - 0xfca4)>>1];
+
+    t_mmdsp_field_16 itremap1_reg[MMDSP_NB_ITREMAP_REG];
+
+    t_uint16 RESERVED14[(0x60000 - 0x5fd80)>>1];
+} t_mmdsp_mmio_regs_16;
+
+
+typedef volatile struct {
+    t_uint32 RESERVED1[(0xa800)>>2];
+
+    t_mmdsp_dma_ctrl_regs_32 dma_ctrl[MMDSP_NB_DMA_CTRL];
+
+    t_uint32 RESERVED2[(0xb000-0xa880)>>2];
+
+    t_mmdsp_dcache_regs_32 dcache;
+
+    t_uint32 RESERVED3[(0xc000-0xb080)>>2];
+
+    t_mmdsp_io_regs_32 io;
+
+    t_uint32 RESERVED4[(0xc0-0xa0)>>2];
+
+    t_mmdsp_timer_regs_32 timer[MMDSP_NB_TIMER];
+
+    t_uint32 RESERVED5[(0x820-0x0f0)>>2];
+
+    t_mmdsp_field_32 sem[MMDSP_NB_BIT_SEM];
+
+    t_uint32 RESERVED6[(0x8a0-0x860)>>2];
+
+    t_mmdsp_field_32 ipen;
+    t_uint32 itip_0;
+    t_uint32 itip_1;
+    t_uint32 itip_2;
+    t_uint32 itip_3;
+    t_uint32 itop_0;
+    t_uint32 itop_1;
+    t_uint32 itop_2;
+    t_uint32 itop_3;
+    t_uint32 RESERVED7[(0x914-0x8c8)>>2];
+    t_uint32 itip_4;
+    t_uint32 itop_4;
+
+    t_uint32 RESERVED8[(0xcfc0-0xc91c)>>2];
+
+    t_mmdsp_field_32 id[4];
+    t_mmdsp_field_32 idp[4];
+
+    t_mmdsp_dma_if_regs_32 dma_if[MMDSP_NB_DMA_IF];
+
+    t_uint32 RESERVED9[(0x800-0x200)>>2];
+
+    t_mmdsp_field_32 emu_unit_maskit;
+    t_mmdsp_field_32 RESERVED[3];
+    t_mmdsp_field_32 config_data_mem;
+    t_mmdsp_field_32 compatibility;
+
+    t_uint32 RESERVED10[(0xE000-0xD830)>>2];
+
+    t_uint32 stbus_if_config;
+    t_uint32 stbus_if_mode;
+    t_uint32 stbus_if_status;
+    t_uint32 stbus_if_security;
+    t_uint32 stbus_if_flush;
+    t_uint32 stbus_reserved;
+    t_uint32 stbus_if_priority;
+    t_uint32 stbus_msb_attribut;
+
+    t_uint32 RESERVED11[(0xF800-0xE020)>>2];
+
+    t_mmdsp_field_32 itremap_reg[MMDSP_NB_ITREMAP_REG];
+    t_mmdsp_field_32 itmsk_l_reg;
+    t_mmdsp_field_32 itmsk_h_reg;
+
+    t_uint32 RESERVED12[(0xf938 - 0xf910)>>2];
+
+    t_mmdsp_field_32 itmemo_l_reg;
+    t_mmdsp_field_32 itmeme_h_reg;
+
+    t_uint32 RESERVED13[(0xfa00 - 0xf948)>>2];
+
+    t_mmdsp_field_32 itremap1_reg[MMDSP_NB_ITREMAP_REG];
+
+    t_uint32 RESERVED14[(0x40000 - 0x3fb00)>>2];
+} t_mmdsp_mmio_regs_32;
+#endif /* __STN_8820 or __STN_8500 */
+
+#ifdef __STN_8815
+typedef volatile struct {
+    t_uint16 RESERVED1[(0xD400-0x8000)>>1];
+
+    t_mmdsp_dma_ctrl_regs_16 dma_ctrl[MMDSP_NB_DMA_CTRL];
+
+    t_uint16 RESERVED2[(0xD800-0xD440)>>1];
+
+    t_mmdsp_dcache_regs_16 dcache;
+
+    t_uint16 RESERVED3[(0xE000-0xD840)>>1];
+
+    t_mmdsp_io_regs_16 io;
+
+    t_uint16 RESERVED4[(0x60-0x50)>>1];
+
+    t_mmdsp_timer_regs_16 timer[MMDSP_NB_TIMER];
+
+    t_uint16 RESERVED5[(0x410-0x78)>>1];
+
+    t_mmdsp_field_16 sem[MMDSP_NB_BIT_SEM];
+
+    t_uint16 RESERVED6[(0x450-0x430)>>1];
+
+    t_mmdsp_field_16 ipen;
+    t_uint16 itip_0;
+    t_uint16 itip_1;
+    t_uint16 itip_2;
+    t_uint16 itip_3;
+    t_uint16 itop_0;
+    t_uint16 itop_1;
+    t_uint16 itop_2;
+    t_uint16 itop_3;
+    t_uint16 RESERVED7[(0x8a-0x64)>>1];
+    t_uint16 itip_4;
+    t_uint16 itop_4;
+
+    t_uint16 RESERVED8[(0x7e0-0x48e)>>1];
+
+    t_mmdsp_field_16 id[4];
+    t_mmdsp_field_16 idp[4];
+
+    t_mmdsp_dma_if_regs_16 dma_if[MMDSP_NB_DMA_IF];
+
+    t_uint16 RESERVED9[(0xC00-0x900)>>1];
+
+    t_mmdsp_field_16 emu_unit_maskit;
+    t_mmdsp_field_16 RESERVED[3];
+    t_mmdsp_field_16 config_data_mem;
+    t_mmdsp_field_16 compatibility;
+
+    t_uint16 RESERVED10[(0xF000-0xEC18)>>1];
+
+    t_uint16 ahb_if_config;
+    t_uint16 ahb_if_mode;
+    t_uint16 ahb_if_status;
+    t_uint16 ahb_if_security;
+    t_uint16 ahb_if_flush;
+
+    t_uint16 RESERVED11[(0xFC00-0xF00A)>>1];
+
+    t_mmdsp_field_16 itremap_reg[MMDSP_NB_ITREMAP_REG];
+    t_mmdsp_field_16 itmsk_l_reg;
+    t_mmdsp_field_16 itmsk_h_reg;
+
+    t_uint16 RESERVED12[(0xfc9c - 0xfc88)>>1];
+
+    t_mmdsp_field_16 itmemo_l_reg;
+    t_mmdsp_field_16 itmeme_h_reg;
+
+    t_uint16 RESERVED13[(0xfd00 - 0xfca4)>>1];
+
+    t_mmdsp_field_16 itremap1_reg[MMDSP_NB_ITREMAP_REG];
+
+    t_uint16 RESERVED14[(0x60000 - 0x5fd80)>>1];
+} t_mmdsp_mmio_regs_16;
+
+
+typedef volatile struct {
+    t_uint32 RESERVED1[(0xa800)>>2];
+
+    t_mmdsp_dma_ctrl_regs_32 dma_ctrl[MMDSP_NB_DMA_CTRL];
+
+    t_uint32 RESERVED2[(0xb000-0xa880)>>2];
+
+    t_mmdsp_dcache_regs_32 dcache;
+
+    t_uint32 RESERVED3[(0xc000-0xb080)>>2];
+
+    t_mmdsp_io_regs_32 io;
+
+    t_uint32 RESERVED4[(0xc0-0xa0)>>2];
+
+    t_mmdsp_timer_regs_32 timer[MMDSP_NB_TIMER];
+
+    t_uint32 RESERVED5[(0x820-0x0f0)>>2];
+
+    t_mmdsp_field_32 sem[MMDSP_NB_BIT_SEM];
+
+    t_uint32 RESERVED6[(0x8a0-0x860)>>2];
+
+    t_mmdsp_field_32 ipen;
+    t_uint32 itip_0;
+    t_uint32 itip_1;
+    t_uint32 itip_2;
+    t_uint32 itip_3;
+    t_uint32 itop_0;
+    t_uint32 itop_1;
+    t_uint32 itop_2;
+    t_uint32 itop_3;
+    t_uint32 RESERVED7[(0x914-0x8c8)>>2];
+    t_uint32 itip_4;
+    t_uint32 itop_4;
+
+    t_uint32 RESERVED8[(0xcfc0-0xc91c)>>2];
+
+    t_mmdsp_field_32 id[4];
+    t_mmdsp_field_32 idp[4];
+
+    t_mmdsp_dma_if_regs_32 dma_if[MMDSP_NB_DMA_IF];
+
+    t_uint32 RESERVED9[(0x800-0x200)>>2];
+
+    t_mmdsp_field_32 emu_unit_maskit;
+    t_mmdsp_field_32 RESERVED[3];
+    t_mmdsp_field_32 config_data_mem;
+    t_mmdsp_field_32 compatibility;
+
+    t_uint32 RESERVED10[(0xE000-0xD830)>>2];
+
+    t_uint32 ahb_if_config;
+    t_uint32 ahb_if_mode;
+    t_uint32 ahb_if_status;
+    t_uint32 ahb_if_security;
+    t_uint32 ahb_if_flush;
+
+    t_uint32 RESERVED11[(0xF800-0xE014)>>2];
+
+    t_mmdsp_field_32 itremap_reg[MMDSP_NB_ITREMAP_REG];
+    t_mmdsp_field_32 itmsk_l_reg;
+    t_mmdsp_field_32 itmsk_h_reg;
+
+    t_uint32 RESERVED12[(0xf938 - 0xf910)>>2];
+
+    t_mmdsp_field_32 itmemo_l_reg;
+    t_mmdsp_field_32 itmeme_h_reg;
+
+    t_uint32 RESERVED13[(0xfa00 - 0xf948)>>2];
+
+    t_mmdsp_field_32 itremap1_reg[MMDSP_NB_ITREMAP_REG];
+
+    t_uint32 RESERVED14[(0x40000 - 0x3fb00)>>2];
+} t_mmdsp_mmio_regs_32;
+#endif /* __STN_8815 */
+
+/* Smart xx Accelerator memory map */
+typedef volatile struct {
+    t_uint32    mem24[MMDSP_NB_BLOCK_RAM*MMDSP_RAM_BLOCK_SIZE];  /* 0x0000 -> 0x20000 */
+
+    t_uint32    RESERVED1[(0x30000 - 0x20000)>>2];
+
+    t_mmdsp_mmio_regs_32 mmio_32;
+
+    t_uint16    mem16[MMDSP_NB_BLOCK_RAM*MMDSP_RAM_BLOCK_SIZE];  /* 0x40000 -> 0x50000 */
+
+    t_uint32    RESERVED2[(0x58000 - 0x50000)>>2];
+
+    t_mmdsp_mmio_regs_16 mmio_16;
+
+    t_mmdsp_host_regs_16 host_reg;
+    /*
+    union host_reg {
+        t_mmdsp_host_regs_16 reg16;
+        t_mmdsp_host_regs_32 reg32;
+    };
+    */
+} t_mmdsp_hw_regs;
+
+#endif // __INC_MMDSP_HWP_H
diff --git a/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_macros.h b/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_macros.h
new file mode 100644
index 00000000000..b8911d27609
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_macros.h
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_MMDSP_DSP_MACROS
+#define __INC_MMDSP_DSP_MACROS
+
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h>
+
+#define MMDSP_ENABLE_WRITE_POSTING(pRegs) \
+{                                                       \
+   (pRegs)->mmio_16.dcache.control |= DCACHE_CONTROL_WRITE_POSTING_ENABLE;  \
+}
+
+#define MMDSP_FLUSH_DCACHE(pRegs) \
+{ /* Today, only full cache flush (clear all the ways) */ \
+   (pRegs)->mmio_16.dcache.command = DCACHE_CMD_FLUSH; \
+}
+
+#define MMDSP_FLUSH_DCACHE_BY_SERVICE(pRegs, startAddr, endAddr)
+
+#define MMDSP_FLUSH_ICACHE(pRegs) \
+{ /* Flush the Instruction cache */ \
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_ICACHE_FLUSH_REG, (IHOST_ICACHE_FLUSH_ALL_ENTRIES_CMD | IHOST_ICACHE_FLUSH_CMD_ENABLE)); \
+}
+
+#ifndef __STN_8810
+#define MMDSP_FLUSH_ICACHE_BY_SERVICE(pRegs, startAddr, endAddr) \
+{ /* Flush the Instruction cache by service */ \
+    /*t_uint64 start_clear_addr = startAddr & ~(MMDSP_ICACHE_LINE_SIZE_IN_INST - 1);*/ \
+    t_uint64 start_clear_addr = (startAddr)>>2; \
+    t_uint64 end_clear_addr = ((endAddr) + MMDSP_ICACHE_LINE_SIZE_IN_INST) & ~(MMDSP_ICACHE_LINE_SIZE_IN_INST - 1); \
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_ICACHE_START_CLEAR_REG, start_clear_addr); \
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_ICACHE_END_CLEAR_REG, end_clear_addr); \
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_ICACHE_FLUSH_REG, (IHOST_ICACHE_FLUSH_BY_SERVICE | IHOST_ICACHE_FLUSH_CMD_ENABLE)); \
+}
+#else
+#define MMDSP_FLUSH_ICACHE_BY_SERVICE(pRegs, startAddr, endAddr) {(void)pRegs; (void)startAddr; (void)endAddr; }
+#endif
+
+#define MMDSP_RESET_CORE(pRegs) \
+{  /* Assert DSP core soft reset */ \
+   (pRegs)->host_reg.softreset = 1; \
+}
+
+#define MMDSP_START_CORE(pRegs) \
+{ \
+   /* Enable external memory access (set bit 3 of ubkcmd) */ \
+   (pRegs)->host_reg.emul_ubkcmd |= MMDSP_UBKCMD_EXT_CODE_MEM_ACCESS_ENABLE; \
+  \
+   /* Start core clock */ \
+   (pRegs)->host_reg.emul_clockcmd = MMDSP_CLOCKCMD_START_CLOCK; \
+}
+
+#define MMDSP_STOP_CORE(pRegs) \
+{ \
+   /* Disable external memory access (reset bit 3 of ubkcmd) */ \
+   (pRegs)->host_reg.emul_ubkcmd = MMDSP_UBKCMD_EXT_CODE_MEM_ACCESS_DISABLE; \
+  \
+   /* Stop core clock */ \
+   (pRegs)->host_reg.emul_clockcmd = MMDSP_CLOCKCMD_STOP_CLOCK; \
+}
+
+#define MMDSP_ASSERT_IRQ(pRegs, irqNum) \
+{ \
+   (pRegs)->host_reg.cmd[irqNum] = 1; \
+}
+
+#define MMDSP_ACKNOWLEDGE_IRQ(pRegs, irqNum) \
+{ \
+    volatile t_uint16 dummy; \
+    dummy =(pRegs)->host_reg.intx[irqNum]; \
+}
+
+#define MMDSP_WRITE_XWORD(pRegs, offset, value) \
+{ \
+    (pRegs)->mem24[offset] = value; \
+}
+
+#define MMDSP_READ_XWORD(pRegs, offset) (pRegs)->mem24[offset]
+
+#endif /* __INC_MMDSP_DSP_MACROS */
diff --git a/drivers/staging/nmf-cm/cm/engine/dsp/src/dsp.c b/drivers/staging/nmf-cm/cm/engine/dsp/src/dsp.c
new file mode 100644
index 00000000000..de4ea3b7ba5
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/dsp/src/dsp.c
@@ -0,0 +1,1074 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_macros.h>
+
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/power_mgt/inc/power.h>
+#include <cm/engine/memory/inc/migration.h>
+#include <cm/engine/trace/inc/trace.h>
+
+#include <share/inc/nomadik_mapping.h>
+
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/component/inc/component_type.h>
+
+static t_dsp_allocator_desc     esramDesc;
+static t_dsp_desc               mpcDesc[NB_CORE_IDS];
+static t_mmdsp_hw_regs          *pMmdspRegs[NB_CORE_IDS];
+
+struct s_base_descr
+{
+    t_uint32            startAddress[2 /* DSP16 = 0, DSP24 = 1*/];
+    t_dsp_segment_type  segmentType;
+};
+
+#if defined(__STN_8500) && (__STN_8500 > 10)
+
+#define DATA_BASE_NUMBER    4
+
+// In bytes
+#define SDRAM_CODE_SPACE_SPLIT 0x8000
+#define ESRAM_CODE_SPACE_SPLIT 0x4000
+#define SDRAM_DATA_SPACE_SPLIT 0x40000 // This is the modulo constraint of mmdsp
+#define ESRAM_DATA_SPACE_SPLIT 0x40000
+
+// In MMDSP word
+static const struct s_base_descr DATA_ADDRESS_BASE[DATA_BASE_NUMBER + 1 /* For guard */] = {
+        {{SDRAMMEM16_BASE_ADDR,                                      SDRAMMEM24_BASE_ADDR},                                 SDRAM_DATA_EE},
+        {{SDRAMMEM16_BASE_ADDR + (SDRAM_DATA_SPACE_SPLIT / 2),       SDRAMMEM24_BASE_ADDR + (SDRAM_DATA_SPACE_SPLIT / 4)},  SDRAM_DATA_USER},
+        {{ESRAMMEM16_BASE_ADDR,                                      ESRAMMEM24_BASE_ADDR},                                 ESRAM_DATA_EE},
+        {{ESRAMMEM16_BASE_ADDR + (ESRAM_DATA_SPACE_SPLIT / 2),       ESRAMMEM24_BASE_ADDR + (ESRAM_DATA_SPACE_SPLIT / 4)},  ESRAM_DATA_USER},
+        {{MMIO_BASE_ADDR,                                            SDRAMMEM16_BASE_ADDR},                                 NB_DSP_SEGMENT_TYPE /* Not used*/}
+};
+
+#else
+
+#define DATA_BASE_NUMBER    2
+
+// In MMDSP word
+static const struct s_base_descr DATA_ADDRESS_BASE[DATA_BASE_NUMBER + 1 /* For guard */] = {
+        {{SDRAMMEM16_BASE_ADDR,                                      SDRAMMEM24_BASE_ADDR},                                 SDRAM_DATA_EE},
+        {{ESRAMMEM16_BASE_ADDR,                                      ESRAMMEM24_BASE_ADDR},                                 ESRAM_DATA_EE},
+        {{MMIO_BASE_ADDR,                                            SDRAMMEM16_BASE_ADDR},                                 NB_DSP_SEGMENT_TYPE /* Not used*/}
+};
+
+#endif
+
+#if defined(__STN_8500) && (__STN_8500 > 10)
+// In word
+static const t_uint32 CODE_ADDRESS_BASE[4] = {
+        SDRAMTEXT_BASE_ADDR,
+        SDRAMTEXT_BASE_ADDR + (SDRAM_CODE_SPACE_SPLIT / 8),
+        ESRAMTEXT_BASE_ADDR,
+        ESRAMTEXT_BASE_ADDR + (ESRAM_CODE_SPACE_SPLIT / 8)
+};
+#endif
+
+static void arm_Init(void);
+static t_cm_error mmdsp_Init(const t_cm_system_address *dspSystemAddr,
+        t_uint8 nbXramBlocks, t_uint8 nbYramBlocks,
+        t_dsp_allocator_desc *sdramCodeDesc,
+        t_dsp_allocator_desc *sdramDataDesc,
+        t_cm_domain_id eeDomain,
+        t_dsp_desc *pDspDesc,
+        t_mmdsp_hw_regs **pRegs);
+static t_cm_error mmdsp_Configure(t_nmf_core_id coreId, t_mmdsp_hw_regs *pRegs, const t_dsp_desc *pDspDesc);
+static t_cm_error mmdsp_ConfigureAfterBoot(t_nmf_core_id coreId, t_uint8 nbXramBlocks, t_uint8 nbYramBlocks);
+static void cm_DSP_SEM_Init(t_nmf_core_id coreId);
+
+PUBLIC const t_dsp_desc* cm_DSP_GetState(t_nmf_core_id coreId)
+{
+    return &mpcDesc[coreId];
+}
+PUBLIC void cm_DSP_SetStatePanic(t_nmf_core_id coreId)
+{
+    mpcDesc[coreId].state = MPC_STATE_PANIC;
+}
+
+PUBLIC void cm_DSP_Init(const t_nmf_memory_segment *pEsramDesc)
+{
+    t_nmf_core_id coreId;
+    int i;
+
+    /* Create esram desc */
+    esramDesc.allocDesc = cm_MM_CreateAllocator(pEsramDesc->size, 0, "esram");
+    esramDesc.baseAddress = pEsramDesc->systemAddr;
+    esramDesc.referenceCounter = 1; // Don't free it with destroy mechanism
+
+    /* Create ARM */
+    arm_Init();
+
+    mpcDesc[ARM_CORE_ID].state = MPC_STATE_BOOTED;
+
+    /* Reset MPC configuration */
+    for (coreId = FIRST_MPC_ID; coreId <= LAST_CORE_ID; coreId++)
+    {
+        mpcDesc[coreId].state = MPC_STATE_UNCONFIGURED;
+
+        for(i = 0; i < NB_DSP_MEMORY_TYPE; i++)
+            mpcDesc[coreId].allocator[i] = NULL;
+    }
+
+}
+
+PUBLIC void cm_DSP_Destroy(void)
+{
+    t_nmf_core_id coreId;
+    int i;
+
+    for (coreId = ARM_CORE_ID; coreId <= LAST_CORE_ID; coreId++)
+    {
+        for(i = 0; i < NB_DSP_MEMORY_TYPE; i++)
+        {
+            if (mpcDesc[coreId].allocator[i] != NULL)
+            {
+                if(--mpcDesc[coreId].allocator[i]->referenceCounter == 0)
+                {
+                    cm_MM_DeleteAllocator(mpcDesc[coreId].allocator[i]->allocDesc);
+
+                    OSAL_Free(mpcDesc[coreId].allocator[i]);
+                }
+            }
+        }
+    }
+
+    cm_MM_DeleteAllocator(esramDesc.allocDesc);
+}
+
+
+PUBLIC t_cm_error cm_DSP_Add(t_nmf_core_id coreId,
+        t_uint8 nbYramBanks,
+        const t_cm_system_address *pDspMapDesc,
+        const t_cm_domain_id eeDomain,
+        t_dsp_allocator_desc *sdramCodeAllocDesc,
+        t_dsp_allocator_desc *sdramDataAllocDesc)
+{
+    t_cm_error error;
+
+    /* checking nbYramBanks is valid */
+    if (nbYramBanks >= SxA_NB_BLOCK_RAM)
+        return CM_MPC_INVALID_CONFIGURATION;
+
+    if((error = cm_DM_CheckDomain(eeDomain, DOMAIN_NORMAL)) != CM_OK)
+        return error;
+
+    mpcDesc[coreId].domainEE = eeDomain;
+    mpcDesc[coreId].nbYramBank = nbYramBanks;
+    mpcDesc[coreId].state = MPC_STATE_BOOTABLE;
+
+    return mmdsp_Init(
+            pDspMapDesc,
+            SxA_NB_BLOCK_RAM, /* nb of data tcm bank minus one (reserved for cache) */
+            nbYramBanks,
+            sdramCodeAllocDesc,
+            sdramDataAllocDesc,
+            eeDomain,
+            &mpcDesc[coreId],
+            &pMmdspRegs[coreId]
+    );
+}
+
+PUBLIC t_cm_error cm_DSP_Boot(t_nmf_core_id coreId)
+{
+    t_cm_error error;
+
+    // Enable the associated power domain
+    if((error = cm_PWR_EnableMPC(MPC_PWR_CLOCK, coreId)) != CM_OK)
+      return error;
+
+    cm_SEM_PowerOn[coreId](coreId);
+
+    if((error = mmdsp_Configure(
+            coreId,
+            pMmdspRegs[coreId],
+            &mpcDesc[coreId])) != CM_OK)
+    {
+        cm_PWR_DisableMPC(MPC_PWR_CLOCK, coreId);
+    }
+
+    // Put it in auto idle mode ; it's the default in Step 2 of power implementation
+    if((error = cm_PWR_EnableMPC(MPC_PWR_AUTOIDLE, coreId)) != CM_OK)
+      return error;
+
+    return error;
+}
+
+/*
+ * This method is required since MMDSP C bootstrap set some value that must be set differently !!!
+ */
+PUBLIC void cm_DSP_ConfigureAfterBoot(t_nmf_core_id coreId)
+{
+    mpcDesc[coreId].state = MPC_STATE_BOOTED;
+
+    mmdsp_ConfigureAfterBoot(coreId, SxA_NB_BLOCK_RAM, mpcDesc[coreId].nbYramBank);
+
+    cm_DSP_SEM_Init(coreId);
+}
+
+PUBLIC void cm_DSP_Stop(t_nmf_core_id coreId)
+{
+    MMDSP_STOP_CORE(pMmdspRegs[coreId]);
+
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+}
+
+PUBLIC void cm_DSP_Start(t_nmf_core_id coreId)
+{
+    MMDSP_START_CORE(pMmdspRegs[coreId]);
+
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+}
+
+PUBLIC void cm_DSP_Shutdown(t_nmf_core_id coreId)
+{
+    MMDSP_FLUSH_DCACHE(pMmdspRegs[coreId]);
+    MMDSP_FLUSH_ICACHE(pMmdspRegs[coreId]);
+
+    // Due to a hardware bug that breaks MTU when DSP are powered off, don't do that
+    // on mop500_ed for now
+#if !defined(__STN_8500) ||  (__STN_8500 > 10)
+    MMDSP_RESET_CORE(pMmdspRegs[coreId]);
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+    MMDSP_STOP_CORE(pMmdspRegs[coreId]);
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+#endif
+
+    mpcDesc[coreId].state = MPC_STATE_BOOTABLE;
+
+    cm_SEM_PowerOff[coreId](coreId);
+
+    cm_PWR_DisableMPC(MPC_PWR_AUTOIDLE, coreId);
+    cm_PWR_DisableMPC(MPC_PWR_CLOCK, coreId);
+}
+
+PUBLIC t_uint32 cm_DSP_ReadXRamWord(t_nmf_core_id coreId, t_uint32 dspOffset)
+{
+    t_uint32 value;
+
+    value = pMmdspRegs[coreId]->mem24[dspOffset];
+
+    LOG_INTERNAL(3, "cm_DSP_ReadXRamWord: [%x]=%x\n",
+            dspOffset, value,
+            0, 0, 0, 0);
+
+    return value;
+}
+
+
+PUBLIC void cm_DSP_WriteXRamWord(t_nmf_core_id coreId, t_uint32 dspOffset, t_uint32 value)
+{
+    LOG_INTERNAL(3, "cm_DSP_WriteXRamWord: [%x]<-%x\n",
+            dspOffset, value,
+            0, 0, 0, 0);
+
+    pMmdspRegs[coreId]->mem24[dspOffset] = value;
+}
+
+static void cm_DSP_SEM_Init(t_nmf_core_id coreId)
+{
+    pMmdspRegs[coreId]->mmio_16.sem[1].value = 1;
+}
+
+PUBLIC void cm_DSP_SEM_Take(t_nmf_core_id coreId, t_semaphore_id semId)
+{
+    /* take semaphore */
+    while(pMmdspRegs[coreId]->mmio_16.sem[1].value) ;
+}
+
+PUBLIC void cm_DSP_SEM_Give(t_nmf_core_id coreId, t_semaphore_id semId)
+{
+    /* release semaphore */
+    pMmdspRegs[coreId]->mmio_16.sem[1].value = 1;
+}
+
+PUBLIC void cm_DSP_SEM_GenerateIrq(t_nmf_core_id coreId, t_semaphore_id semId)
+{
+    MMDSP_ASSERT_IRQ(pMmdspRegs[coreId], ARM2DSP_IRQ_0);
+}
+
+
+PUBLIC void cm_DSP_AssertDspIrq(t_nmf_core_id coreId, t_host2mpc_irq_num irqNum)
+{
+    MMDSP_ASSERT_IRQ(pMmdspRegs[coreId], irqNum);
+    return;
+}
+
+PUBLIC void cm_DSP_AcknowledgeDspIrq(t_nmf_core_id coreId, t_mpc2host_irq_num irqNum)
+{
+    MMDSP_ACKNOWLEDGE_IRQ(pMmdspRegs[coreId], irqNum);
+    return;
+}
+
+//TODO, juraj, cleanup INTERNAL_XRAM vs INTERNAL_XRAM16/24
+static const t_uint32 dspMemoryTypeId2OffsetShifter[NB_DSP_MEMORY_TYPE] =
+{
+    2, /* INTERNAL_XRAM24:   Internal X memory but seen by host as 32-bit memory */
+    2, /* INTERNAL_XRAM16:   Internal X memory but seen by host as 16-bit memory */
+    2, /* INTERNAL_YRAM24:   Internal Y memory but seen by host as 32-bit memory */
+    2, /* INTERNAL_YRAM16:   Internal Y memory but seen by host as 16-bit memory */
+    2, /* SDRAM_EXT24:       24-bit external "X" memory */
+    1, /* SDRAM_EXT16:       16-bit external "X" memory */
+    2, /* ESRAM_EXT24:       ESRAM24 */
+    1, /* ESRAM_EXT16:       ESRAM16 */
+    3, /* SDRAM_CODE:        Program memory */
+    3, /* ESRAM_CODE:        ESRAM code */
+    3, /* LOCKED_CODE:        ESRAM code */
+};
+
+//TODO, juraj, use these values in mmdsp_Configure
+static const t_uint32 dspMemoryTypeId2DspAddressOffset[NB_DSP_MEMORY_TYPE] =
+{
+    0, /* INTERNAL_XRAM24 */
+    0, /* INTERNAL_XRAM16 */
+    0, /* INTERNAL_YRAM24 */
+    0, /* INTERNAL_YRAM16 */
+    SDRAMMEM24_BASE_ADDR,           /* SDRAM_EXT24:       24-bit external "X" memory */
+    SDRAMMEM16_BASE_ADDR,           /* SDRAM_EXT16:       16-bit external "X" memory */
+    ESRAMMEM24_BASE_ADDR,           /* ESRAM_EXT24:       ESRAM24 */
+    ESRAMMEM16_BASE_ADDR,           /* ESRAM_EXT16:       ESRAM16 */
+    SDRAMTEXT_BASE_ADDR,            /* SDRAM_CODE:        Program memory */
+    ESRAMTEXT_BASE_ADDR,            /* ESRAM_CODE:        ESRAM code */
+    SDRAMTEXT_BASE_ADDR,            /* ESRAM_CODE:        ESRAM code */
+};
+
+PUBLIC t_cm_allocator_desc* cm_DSP_GetAllocator(t_nmf_core_id coreId, t_dsp_memory_type_id memType)
+{
+    return mpcDesc[coreId].allocator[memType] ? mpcDesc[coreId].allocator[memType]->allocDesc : NULL;
+}
+
+PUBLIC void cm_DSP_GetDspChunkInfo(t_memory_handle memHandle, t_dsp_chunk_info *info)
+{
+    t_uint16 userData;
+
+    cm_MM_GetMemoryHandleUserData(memHandle, &userData, &info->alloc);
+
+    info->coreId            = (t_nmf_core_id)       ((userData >> SHIFT_BYTE1) & MASK_BYTE0);
+    info->memType           = (t_dsp_memory_type_id)((userData >> SHIFT_BYTE0) & MASK_BYTE0);
+}
+
+PUBLIC t_cm_error cm_DSP_GetInternalMemoriesInfo(t_cm_domain_id domainId, t_dsp_memory_type_id memType,
+        t_uint32 *offset, t_uint32 *size)
+{
+    t_nmf_core_id coreId = domainDesc[domainId].domain.coreId;
+
+    switch(memType)
+    {
+    case INTERNAL_XRAM24:
+    case INTERNAL_XRAM16:
+        *offset = 0;
+        *size = mpcDesc[coreId].yram_offset;
+        break;
+    case INTERNAL_YRAM24:
+    case INTERNAL_YRAM16:
+        *offset = mpcDesc[coreId].yram_offset;
+        *size = mpcDesc[coreId].yram_size;
+        break;
+    case LOCKED_CODE:
+        *offset = mpcDesc[coreId].locked_offset;
+        *size = mpcDesc[coreId].locked_size;
+        break;
+    case SDRAM_EXT24:
+    case SDRAM_EXT16:
+        *offset = domainDesc[domainId].domain.sdramData.offset;
+        *size   = domainDesc[domainId].domain.sdramData.size;
+        break;
+    case ESRAM_EXT24:
+    case ESRAM_EXT16:
+        *offset = domainDesc[domainId].domain.esramData.offset;
+        *size   = domainDesc[domainId].domain.esramData.size;
+        break;
+    case SDRAM_CODE:
+        *offset = domainDesc[domainId].domain.sdramCode.offset;
+        *size   = domainDesc[domainId].domain.sdramCode.size;
+
+        // update domain size to take into account .locked section
+        if(*offset + *size > mpcDesc[coreId].locked_offset)
+            *size = mpcDesc[coreId].locked_offset - *offset;
+        break;
+    case ESRAM_CODE:
+        *offset = domainDesc[domainId].domain.esramCode.offset;
+        *size   = domainDesc[domainId].domain.esramCode.size;
+        break;
+    default:
+        //return CM_INVALID_PARAMETER;
+        //params are checked at the level above, so this should never occur
+        ERROR("Invalid memType\n",0,0,0,0,0,0);
+        *offset = 0;
+        *size   = 0;
+        CM_ASSERT(0);
+    }
+
+    return CM_OK;
+}
+
+
+PUBLIC t_uint32 cm_DSP_ConvertSize(t_dsp_memory_type_id memType, t_uint32 wordSize)
+{
+    return wordSize << dspMemoryTypeId2OffsetShifter[memType];
+}
+
+PUBLIC t_cm_logical_address cm_DSP_ConvertDspAddressToHostLogicalAddress(t_nmf_core_id coreId, t_shared_addr dspAddress)
+{
+    t_dsp_address_info info;
+    cm_DSP_GetDspDataAddressInfo(coreId, dspAddress, &info);
+    return mpcDesc[coreId].segments[info.segmentType].base.logical + info.baseOffset;
+}
+
+PUBLIC t_cm_error cm_DSP_GetAllocatorStatus(t_nmf_core_id coreId, t_dsp_memory_type_id dspMemType, t_uint32 offset, t_uint32 size, t_cm_allocator_status *pStatus)
+{
+    t_cm_error error;
+
+    if(mpcDesc[coreId].allocator[dspMemType] == NULL)
+        return CM_UNKNOWN_MEMORY_HANDLE;
+
+    error = cm_MM_GetAllocatorStatus(cm_DSP_GetAllocator(coreId, dspMemType), offset, size, pStatus);
+    if (error != CM_OK)
+        return error;
+
+    // complete status with stack sizes, for all dsps
+    //NOTE, well, surely this isn't very clean, as dsp and memory allocator are different things ..
+    {
+        t_uint8 i;
+        for (i = 0; i < NB_CORE_IDS; i++) {
+            //*(pStatus->stack[i].sizes) = *(eeState[i].currentStackSize);
+            pStatus->stack[i].sizes[0] = eeState[i].currentStackSize[0];
+            pStatus->stack[i].sizes[1] = eeState[i].currentStackSize[1];
+            pStatus->stack[i].sizes[2] = eeState[i].currentStackSize[2];
+        }
+    }
+
+    // Change bytes to words
+    pStatus->global.accumulate_free_memory  = pStatus->global.accumulate_free_memory >> dspMemoryTypeId2OffsetShifter[dspMemType];
+    pStatus->global.accumulate_used_memory  = pStatus->global.accumulate_used_memory >> dspMemoryTypeId2OffsetShifter[dspMemType];
+    pStatus->global.maximum_free_size  = pStatus->global.maximum_free_size >> dspMemoryTypeId2OffsetShifter[dspMemType];
+    pStatus->global.minimum_free_size  = pStatus->global.minimum_free_size >> dspMemoryTypeId2OffsetShifter[dspMemType];
+
+    return error;
+}
+
+PUBLIC void cm_DSP_GetHostSystemAddress(t_memory_handle memHandle, t_cm_system_address *pAddr)
+{
+    t_dsp_chunk_info chunk_info;
+    t_uint32 offset; //in bytes
+
+    cm_DSP_GetDspChunkInfo(memHandle, &chunk_info);
+
+    offset = cm_MM_GetOffset(memHandle);
+
+    /* MMDSP mem16 array is very specific to host access, so .... */
+    /* We compute by hand the Host System address to take into account the specifities of the mmdsp mem16 array */
+    /* 1 dsp word = 2 host bytes AND mem16 array is "exported" by MMDSP External Bus wrapper at the 0x40000 offet */
+    if (chunk_info.memType == INTERNAL_XRAM16 || chunk_info.memType == INTERNAL_YRAM16) {
+        offset = (offset >> 1) + FIELD_OFFSET(t_mmdsp_hw_regs, mem16);
+    }
+
+    //TODO, juraj, calculate correct value here - based on segments desc etc..
+    pAddr->logical  = mpcDesc[chunk_info.coreId].allocator[chunk_info.memType]->baseAddress.logical  + offset;
+    pAddr->physical = mpcDesc[chunk_info.coreId].allocator[chunk_info.memType]->baseAddress.physical + offset;
+}
+
+
+PUBLIC t_physical_address cm_DSP_GetPhysicalAdress(t_memory_handle memHandle)
+{
+    t_cm_system_address addr;
+    cm_DSP_GetHostSystemAddress(memHandle, &addr);
+    return addr.physical;
+}
+
+PUBLIC t_cm_logical_address cm_DSP_GetHostLogicalAddress(t_memory_handle memHandle)
+{
+    t_cm_system_address addr;
+    cm_DSP_GetHostSystemAddress(memHandle, &addr);
+    return addr.logical;
+}
+
+PUBLIC void cm_DSP_GetDspAddress(t_memory_handle memHandle, t_uint32 *pDspAddress)
+{
+    t_dsp_chunk_info chunk_info;
+
+    cm_DSP_GetDspChunkInfo(memHandle, &chunk_info);
+
+    *pDspAddress =
+            (cm_MM_GetOffset(memHandle) >> dspMemoryTypeId2OffsetShifter[chunk_info.memType]) +
+            dspMemoryTypeId2DspAddressOffset[chunk_info.memType];
+}
+
+PUBLIC t_cm_error cm_DSP_GetDspBaseAddress(t_nmf_core_id coreId, t_dsp_memory_type_id memType, t_cm_system_address *pAddr)
+{
+    cm_migration_check_state(coreId, STATE_NORMAL);
+    *pAddr = mpcDesc[coreId].allocator[memType]->baseAddress;
+    return CM_OK;
+}
+
+PUBLIC void cm_DSP_GetDspMemoryHandleSize(t_memory_handle memHandle, t_uint32 *pDspSize)
+{
+    t_dsp_chunk_info chunk_info;
+    cm_DSP_GetDspChunkInfo(memHandle, &chunk_info);
+    *pDspSize = cm_MM_GetSize(memHandle) >> dspMemoryTypeId2OffsetShifter[chunk_info.memType];
+}
+
+PUBLIC t_cm_error cm_DSP_setStackSize(t_nmf_core_id coreId, t_uint32 newStackSize)
+{
+    t_uint8 nbXramBanks;
+    t_uint32 xramSize;
+
+    /* compute size of xram allocator */
+    nbXramBanks = SxA_NB_BLOCK_RAM - mpcDesc[coreId].nbYramBank;
+
+    /* check first that required stack size is less then xram memory ....*/
+    if (newStackSize >= nbXramBanks * 4 * ONE_KB)
+        return CM_NO_MORE_MEMORY;
+
+    /* compute new xram allocator size */
+    xramSize = nbXramBanks * 4 * ONE_KB - newStackSize;
+
+    /* try to resize it */
+    return cm_MM_ResizeAllocator(cm_DSP_GetAllocator(coreId, INTERNAL_XRAM24),
+                                 xramSize << dspMemoryTypeId2OffsetShifter[INTERNAL_XRAM24]);
+}
+
+PUBLIC t_cm_error cm_DSP_IsNbYramBanksValid(t_nmf_core_id coreId, t_uint8 nbYramBanks)
+{
+    /* we use one bank for cache */
+    t_uint8 nbOfRamBanksWithCacheReserved = SxA_NB_BLOCK_RAM;
+
+    /* we want to keep at least one bank of xram */
+    if (nbYramBanks < nbOfRamBanksWithCacheReserved) {return CM_OK;}
+    else {return CM_MPC_INVALID_CONFIGURATION;}
+}
+
+PUBLIC t_uint32 cm_DSP_getStackAddr(t_nmf_core_id coreId)
+{
+    /* we use one bank for cache */
+    //t_uint8 nbOfRamBanksWithCacheReserved = SxA_NB_BLOCK_RAM;
+    /* */
+    //return ((nbOfRamBanksWithCacheReserved * MMDSP_RAM_BLOCK_SIZE * MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE) - mpcDesc[coreId].yram_offset);
+    return mpcDesc[coreId].yram_offset / MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE;
+}
+
+static void arm_Init(void)
+{
+    mpcDesc[ARM_CORE_ID].allocator[INTERNAL_XRAM24] = 0;
+    mpcDesc[ARM_CORE_ID].allocator[INTERNAL_XRAM16] = 0;
+
+    mpcDesc[ARM_CORE_ID].allocator[INTERNAL_YRAM24] = 0;
+    mpcDesc[ARM_CORE_ID].allocator[INTERNAL_YRAM16] = 0;
+
+    mpcDesc[ARM_CORE_ID].allocator[SDRAM_CODE] = 0;
+    mpcDesc[ARM_CORE_ID].allocator[ESRAM_CODE] = 0;
+
+    mpcDesc[ARM_CORE_ID].allocator[SDRAM_EXT16] = 0;
+    mpcDesc[ARM_CORE_ID].allocator[SDRAM_EXT24] = 0;
+
+    mpcDesc[ARM_CORE_ID].allocator[ESRAM_EXT16] = &esramDesc;
+    mpcDesc[ARM_CORE_ID].allocator[ESRAM_EXT16]->referenceCounter++;
+    mpcDesc[ARM_CORE_ID].allocator[ESRAM_EXT24] = &esramDesc;
+    mpcDesc[ARM_CORE_ID].allocator[ESRAM_EXT24]->referenceCounter++;
+}
+
+static void _init_Segment(
+        t_dsp_segment *seg,
+        const t_cm_system_address base, const t_uint32 arm_offset,
+        const t_uint32 size)
+{
+    seg->base.logical = base.logical + arm_offset;
+    seg->base.physical = base.physical + arm_offset;
+    seg->size = size;
+}
+
+static t_cm_error mmdsp_Init(
+        const t_cm_system_address *dspSystemAddr,
+        t_uint8 nbXramBlocks, t_uint8 nbYramBlocks,
+        t_dsp_allocator_desc *sdramCodeDesc,
+        t_dsp_allocator_desc *sdramDataDesc,
+        t_cm_domain_id eeDomain,
+        t_dsp_desc *pDspDesc,
+        t_mmdsp_hw_regs **pRegs)
+{
+    t_cm_system_address xramSysAddr;
+    t_uint32 sizeInBytes;
+
+    /* Initialize reference on hw ressources */
+    *pRegs = (t_mmdsp_hw_regs *) dspSystemAddr->logical;
+
+    /* Initialize memory segments management */
+    xramSysAddr.logical = (t_cm_logical_address)(((t_mmdsp_hw_regs *)dspSystemAddr->logical)->mem24);
+    xramSysAddr.physical = (t_cm_physical_address)(((t_mmdsp_hw_regs *)dspSystemAddr->physical)->mem24);
+
+    /* The last (x)ram block will be used by cache, so ... */
+    /* And the NB_YRAM_BLOCKS last available block(s) will be used as YRAM */
+
+    /* XRAM*/
+    pDspDesc->allocator[INTERNAL_XRAM16] = pDspDesc->allocator[INTERNAL_XRAM24] = (t_dsp_allocator_desc*)OSAL_Alloc(sizeof (t_dsp_allocator_desc));
+    if (pDspDesc->allocator[INTERNAL_XRAM24] == NULL)
+        return CM_NO_MORE_MEMORY;
+
+    pDspDesc->allocator[INTERNAL_XRAM24]->allocDesc = cm_MM_CreateAllocator(
+            ((nbXramBlocks-nbYramBlocks)*MMDSP_RAM_BLOCK_SIZE)*MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE,
+            0,
+            "XRAM");
+    pDspDesc->allocator[INTERNAL_XRAM24]->baseAddress = xramSysAddr;
+    pDspDesc->allocator[INTERNAL_XRAM24]->referenceCounter = 2;
+
+    /* YRAM */
+    pDspDesc->allocator[INTERNAL_YRAM16] = pDspDesc->allocator[INTERNAL_YRAM24] = (t_dsp_allocator_desc*)OSAL_Alloc(sizeof (t_dsp_allocator_desc));
+    if (pDspDesc->allocator[INTERNAL_YRAM24] == 0) {
+    	OSAL_Free(pDspDesc->allocator[INTERNAL_XRAM24]);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    pDspDesc->allocator[INTERNAL_YRAM24]->allocDesc = cm_MM_CreateAllocator(
+            (nbYramBlocks*MMDSP_RAM_BLOCK_SIZE)*MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE,
+            ((nbXramBlocks-nbYramBlocks)*MMDSP_RAM_BLOCK_SIZE)*MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE,
+            "YRAM");
+    pDspDesc->allocator[INTERNAL_YRAM24]->baseAddress = xramSysAddr; /* use xram base address but offset is not null */
+    pDspDesc->allocator[INTERNAL_YRAM24]->referenceCounter = 2;
+
+    pDspDesc->yram_offset = ((nbXramBlocks-nbYramBlocks)*MMDSP_RAM_BLOCK_SIZE)*MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE;
+    pDspDesc->yram_size = (nbYramBlocks*MMDSP_RAM_BLOCK_SIZE)*MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE;
+
+    /* SDRAM & ESRAM */
+    pDspDesc->allocator[SDRAM_CODE] = sdramCodeDesc;
+    pDspDesc->allocator[SDRAM_CODE]->referenceCounter++;
+    pDspDesc->allocator[ESRAM_CODE] = &esramDesc;
+    pDspDesc->allocator[ESRAM_CODE]->referenceCounter++;
+
+    /* LOCKED CODE at end of SDRAM code*/
+    pDspDesc->allocator[LOCKED_CODE] = sdramCodeDesc;
+    pDspDesc->allocator[LOCKED_CODE]->referenceCounter++;
+
+    pDspDesc->locked_offset = cm_MM_GetAllocatorSize(pDspDesc->allocator[SDRAM_CODE]->allocDesc) - MMDSP_CODE_CACHE_WAY_SIZE * 8 * SxA_LOCKED_WAY;
+    pDspDesc->locked_size = MMDSP_CODE_CACHE_WAY_SIZE * 8 * SxA_LOCKED_WAY;
+
+    /* Data_16/24 memory management */
+    pDspDesc->allocator[SDRAM_EXT16] = sdramDataDesc;
+    pDspDesc->allocator[SDRAM_EXT16]->referenceCounter++;
+    pDspDesc->allocator[SDRAM_EXT24] = sdramDataDesc;
+    pDspDesc->allocator[SDRAM_EXT24]->referenceCounter++;
+
+    pDspDesc->allocator[ESRAM_EXT16] = &esramDesc;
+    pDspDesc->allocator[ESRAM_EXT16]->referenceCounter++;
+    pDspDesc->allocator[ESRAM_EXT24] = &esramDesc;
+    pDspDesc->allocator[ESRAM_EXT24]->referenceCounter++;
+
+    sizeInBytes = cm_MM_GetAllocatorSize(pDspDesc->allocator[SDRAM_CODE]->allocDesc);
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    _init_Segment(&pDspDesc->segments[SDRAM_CODE_EE],
+            pDspDesc->allocator[SDRAM_CODE]->baseAddress,
+            domainDesc[eeDomain].domain.sdramCode.offset,
+            domainDesc[eeDomain].domain.sdramCode.size);
+    _init_Segment(&pDspDesc->segments[SDRAM_CODE_USER],
+            pDspDesc->allocator[SDRAM_CODE]->baseAddress,
+            domainDesc[eeDomain].domain.sdramCode.offset + domainDesc[eeDomain].domain.sdramCode.size,
+            sizeInBytes - domainDesc[eeDomain].domain.sdramCode.size);
+#else
+    _init_Segment(&pDspDesc->segments[SDRAM_CODE_EE],
+            pDspDesc->allocator[SDRAM_CODE]->baseAddress,
+            0x0,
+            sizeInBytes);
+#endif
+
+    sizeInBytes = cm_MM_GetAllocatorSize(pDspDesc->allocator[ESRAM_CODE]->allocDesc);
+#if defined(__STN_8500) && (__STN_8500 > 10)
+   _init_Segment(&pDspDesc->segments[ESRAM_CODE_EE],
+            pDspDesc->allocator[ESRAM_CODE]->baseAddress,
+            domainDesc[eeDomain].domain.esramCode.offset,
+            domainDesc[eeDomain].domain.esramCode.size);
+    _init_Segment(&pDspDesc->segments[ESRAM_CODE_USER],
+            pDspDesc->allocator[ESRAM_CODE]->baseAddress,
+            domainDesc[eeDomain].domain.esramCode.offset + domainDesc[eeDomain].domain.esramCode.size,
+            sizeInBytes - domainDesc[eeDomain].domain.esramCode.size);
+#else
+    _init_Segment(&pDspDesc->segments[ESRAM_CODE_EE],
+            pDspDesc->allocator[ESRAM_CODE]->baseAddress,
+            0x0,
+            sizeInBytes);
+#endif
+
+    //the difference in the following code is the segment size used to calculate the top!!
+    sizeInBytes = cm_MM_GetAllocatorSize(pDspDesc->allocator[SDRAM_EXT16]->allocDesc);
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    _init_Segment(&pDspDesc->segments[SDRAM_DATA_EE],
+            pDspDesc->allocator[SDRAM_EXT16]->baseAddress,
+            domainDesc[eeDomain].domain.sdramData.offset,
+            domainDesc[eeDomain].domain.sdramData.size);
+    _init_Segment(&pDspDesc->segments[SDRAM_DATA_USER],
+            pDspDesc->allocator[SDRAM_EXT16]->baseAddress,
+            domainDesc[eeDomain].domain.sdramData.offset + domainDesc[eeDomain].domain.sdramData.size,
+            sizeInBytes - domainDesc[eeDomain].domain.sdramData.size);
+#else
+    _init_Segment(&pDspDesc->segments[SDRAM_DATA_EE],
+            pDspDesc->allocator[SDRAM_EXT16]->baseAddress,
+            0x0,
+            sizeInBytes);
+#endif
+
+    sizeInBytes = cm_MM_GetAllocatorSize(pDspDesc->allocator[ESRAM_EXT16]->allocDesc);
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    _init_Segment(&pDspDesc->segments[ESRAM_DATA_EE],
+            pDspDesc->allocator[ESRAM_EXT16]->baseAddress,
+            domainDesc[eeDomain].domain.esramData.offset,
+            domainDesc[eeDomain].domain.esramData.size);
+    _init_Segment(&pDspDesc->segments[ESRAM_DATA_USER],
+            pDspDesc->allocator[ESRAM_EXT16]->baseAddress,
+            domainDesc[eeDomain].domain.esramData.offset + domainDesc[eeDomain].domain.esramData.size,
+            sizeInBytes - domainDesc[eeDomain].domain.esramData.size);
+#else
+    _init_Segment(&pDspDesc->segments[ESRAM_DATA_EE],
+            pDspDesc->allocator[ESRAM_EXT16]->baseAddress,
+            0x0,
+            sizeInBytes);
+#endif
+
+    return CM_OK;
+}
+
+//TODO, juraj, reuse cm_DSP_UpdateBase functions
+static t_cm_error mmdsp_Configure(t_nmf_core_id coreId, t_mmdsp_hw_regs *pRegs, const t_dsp_desc *pDspDesc)
+{
+    t_uint64 regValue;
+    static const t_uint64 coreId2stbusId[NB_CORE_IDS] =
+    {
+        0, /* ARM_CORE_ID no meaning */
+        SVA_STBUS_ID, /* SVA_CORE_ID */
+        SIA_STBUS_ID  /* SIA_CORE_ID */
+    };
+
+    //t_cm_system_address sysAddr;
+    //t_cm_size sizeInBytes;
+
+    /* Stop core (stop clock) */
+    MMDSP_RESET_CORE(pRegs);
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+    MMDSP_STOP_CORE(pRegs);
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+
+#if 0
+    /* Reset DSP internal memory (xram) */
+    {
+        t_uint32 *pSrc = (t_uint32 *)(pRegs->mem24);
+        t_uint32 tcmSize;
+        int i;
+        cm_MM_GetAllocatorSize(pDspDesc->allocator[INTERNAL_XRAM], &sizeInBytes);
+        tcmSize = sizeInBytes;
+        cm_MM_GetAllocatorSize(pDspDesc->allocator[INTERNAL_YRAM], &sizeInBytes);
+        tcmSize += sizeInBytes;
+        for (i = 0; i < (tcmSize/sizeof(t_uint32)); i++)
+            *(pSrc++) = 0;
+    }
+#endif
+
+    /* Configure all blocks as X only, except the Y ones (MOVED TO mmdsp_InitAfterBoot()) */
+
+    /* __STN_8815 --> __STN_8820 or __STN_8500 */
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_STBUS_ID_CONF_REG, coreId2stbusId[coreId]);
+
+    /* Configure External Bus timeout reg */
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EN_EXT_BUS_TIMEOUT_REG, IHOST_TIMEOUT_ENABLE);
+
+    /* Program memory management */
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    {
+        const t_uint32 r0 = CODE_ADDRESS_BASE[1] >> 10;
+        const t_uint32 r1 = CODE_ADDRESS_BASE[2] >> 10;
+        const t_uint32 r2 = CODE_ADDRESS_BASE[3] >> 10;
+        const t_uint32 sdram0 = pDspDesc->segments[SDRAM_CODE_EE].base.physical;
+        const t_uint32 sdram1 = pDspDesc->segments[SDRAM_CODE_USER].base.physical;
+        const t_uint32 esram0 = pDspDesc->segments[ESRAM_CODE_EE].base.physical;
+        const t_uint32 esram1 = pDspDesc->segments[ESRAM_CODE_USER].base.physical;
+
+        /* Bases for first two segments, going to sdram */
+        regValue = ((t_uint64)(sdram1) << IHOST_PRG_BASE2_ADDR_SHIFT) + (t_uint64)sdram0;
+        WRITE_INDIRECT_HOST_REG(pRegs, IHOST_PRG_BASE_ADDR_REG, regValue);
+
+        /* Bases for second two segments, going to esram */
+        regValue = ((t_uint64)(esram1) << IHOST_PRG_BASE4_ADDR_SHIFT) + (t_uint64)esram0;
+        WRITE_INDIRECT_HOST_REG(pRegs, IHOST_PRG_BASE_34_ADDR_REG, regValue);
+
+        /* Split mmdsp program adress-space and activate the mechanism */
+        regValue =  (t_uint64)((t_uint64)(r2) << 48 | (t_uint64)(r1) <<32 | (t_uint64)(r0) << 16 | 1);
+        WRITE_INDIRECT_HOST_REG(pRegs, IHOST_PRG_BASE2_ACTIV_REG, regValue);
+    }
+#else
+    {
+        const t_uint32 sdram0 = pDspDesc->segments[SDRAM_CODE_EE].base.physical;
+        const t_uint32 esram0 = pDspDesc->segments[ESRAM_CODE_EE].base.physical;
+
+        regValue = (t_uint64)sdram0 | ( ((t_uint64)esram0) << IHOST_PRG_BASE2_ADDR_SHIFT );
+        WRITE_INDIRECT_HOST_REG(pRegs, IHOST_PRG_BASE_ADDR_REG, regValue);
+
+        WRITE_INDIRECT_HOST_REG(pRegs, IHOST_PRG_BASE2_ACTIV_REG, IHOST_PRG_BASE2_ACTIV_ON);
+    }
+#endif
+
+    /* Data_16/24 memory management */
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    /* Segments 1 and 2 for 16/24 map to sdram continuously */
+    /* Base 1 */
+    regValue = (((t_uint64)pDspDesc->segments[SDRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE_24_SHIFT) |
+               (((t_uint64)pDspDesc->segments[SDRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE_REG, regValue);
+    /* Top 1 */
+    regValue = (((t_uint64)(pDspDesc->segments[SDRAM_DATA_EE].base.physical + pDspDesc->segments[SDRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[SDRAM_DATA_EE].base.physical + pDspDesc->segments[SDRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_TOP_16_24_REG, regValue);
+
+    /* Base 2 */
+    regValue = (((t_uint64)pDspDesc->segments[SDRAM_DATA_USER].base.physical) << IHOST_DATA_EXT_BUS_BASE2_24_SHIFT) |
+               (((t_uint64)pDspDesc->segments[SDRAM_DATA_USER].base.physical) << IHOST_DATA_EXT_BUS_BASE2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE2_REG, regValue);
+    /* Top 2 */
+    regValue = (((t_uint64)(pDspDesc->segments[SDRAM_DATA_USER].base.physical + pDspDesc->segments[SDRAM_DATA_USER].size - 1)) << IHOST_DATA_EXT_BUS_TOP2_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[SDRAM_DATA_USER].base.physical + pDspDesc->segments[SDRAM_DATA_USER].size - 1)) << IHOST_DATA_EXT_BUS_TOP2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EXT_BUS_TOP2_16_24_REG, regValue);
+
+    /* Segments 3 and 4 for 16/24 map to esram continuously */
+    /* Base 3 */
+    regValue = (((t_uint64)pDspDesc->segments[ESRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE3_24_SHIFT) |
+               (((t_uint64)pDspDesc->segments[ESRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE3_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE3_REG, regValue);
+    /* Top 3 */
+    regValue = (((t_uint64)(pDspDesc->segments[ESRAM_DATA_EE].base.physical + pDspDesc->segments[ESRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP3_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[ESRAM_DATA_EE].base.physical + pDspDesc->segments[ESRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP3_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EXT_BUS_TOP3_16_24_REG, regValue);
+
+    /* Base 4 */
+    regValue = (((t_uint64)pDspDesc->segments[ESRAM_DATA_USER].base.physical) << IHOST_DATA_EXT_BUS_BASE4_24_SHIFT) |
+               (((t_uint64)pDspDesc->segments[ESRAM_DATA_USER].base.physical) << IHOST_DATA_EXT_BUS_BASE4_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE4_REG, regValue);
+    /* Top 4 */
+    regValue = (((t_uint64)(pDspDesc->segments[ESRAM_DATA_USER].base.physical + pDspDesc->segments[ESRAM_DATA_USER].size - 1)) << IHOST_DATA_EXT_BUS_TOP4_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[ESRAM_DATA_USER].base.physical + pDspDesc->segments[ESRAM_DATA_USER].size - 1)) << IHOST_DATA_EXT_BUS_TOP4_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EXT_BUS_TOP4_16_24_REG, regValue);
+
+    /* Define base 2 thresholds/offset (1MB for each up segment) */
+    regValue =  ((t_uint64)DATA_ADDRESS_BASE[1].startAddress[1]>>SHIFT_HALFWORD1)<< IHOST_DATA2_24_XA_BASE_SHIFT;
+    regValue |= ((t_uint64)DATA_ADDRESS_BASE[1].startAddress[0]>>SHIFT_HALFWORD1)<< IHOST_DATA2_16_XA_BASE_SHIFT;
+
+    /* Define base 3 thresholds/offset (1MB for each up segment) */
+    regValue |= ((t_uint64)DATA_ADDRESS_BASE[2].startAddress[1]>>SHIFT_HALFWORD1)<< IHOST_DATA3_24_XA_BASE_SHIFT;
+    regValue |= ((t_uint64)DATA_ADDRESS_BASE[2].startAddress[0]>>SHIFT_HALFWORD1)<< IHOST_DATA3_16_XA_BASE_SHIFT;
+
+    /* Define base 4 thresholds/offset (1MB for each up segment) */
+    regValue |= ((t_uint64)DATA_ADDRESS_BASE[3].startAddress[1]>>SHIFT_HALFWORD1)<< IHOST_DATA4_24_XA_BASE_SHIFT;
+    regValue |= ((t_uint64)DATA_ADDRESS_BASE[3].startAddress[0]>>SHIFT_HALFWORD1)<< IHOST_DATA4_16_XA_BASE_SHIFT;
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA2_1624_XA_BASE_REG, regValue);
+
+#else
+    /* Program data24/16 base 1 */
+    regValue = (((t_uint64)pDspDesc->segments[SDRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE_24_SHIFT) |
+               (((t_uint64)pDspDesc->segments[SDRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE_REG, regValue);
+
+    /* Program data24/16 top 1 */
+    regValue = (((t_uint64)(pDspDesc->segments[SDRAM_DATA_EE].base.physical + pDspDesc->segments[SDRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[SDRAM_DATA_EE].base.physical + pDspDesc->segments[SDRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_TOP_16_24_REG, regValue);
+
+    /* Program data24/16 base 2 */
+    regValue = (((t_uint64)pDspDesc->segments[ESRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE2_24_SHIFT) |
+                (((t_uint64)pDspDesc->segments[ESRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE2_REG, regValue);
+
+    /* Program data24/16 top 2 */
+    regValue = (((t_uint64)(pDspDesc->segments[ESRAM_DATA_EE].base.physical + pDspDesc->segments[ESRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP2_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[ESRAM_DATA_EE].base.physical + pDspDesc->segments[ESRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EXT_BUS_TOP2_16_24_REG, regValue);
+
+    /* Define base 2 thresholds/offset (1MB for each up segment) */
+    regValue = ((t_uint64)(DATA_ADDRESS_BASE[1].startAddress[1]>>SHIFT_HALFWORD1))<< IHOST_DATA2_24_XA_BASE_SHIFT; // Top address minus ONE_MB => 256KW (24/32-bit)
+    regValue |= ((t_uint64)(DATA_ADDRESS_BASE[1].startAddress[0]>>SHIFT_HALFWORD1))<< IHOST_DATA2_16_XA_BASE_SHIFT; // Top address minus ONE_MB => 512KW (16-bit)
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA2_1624_XA_BASE_REG, regValue);
+#endif
+
+    /* Enable top check */
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_TOP_16_24_CHK_REG, IHOST_DATA_TOP_16_24_CHK_ON);
+
+    /* Enable both bases */
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_BASE2_ACTIV_REG, IHOST_DATA_BASE2_ACTIV_ON);
+
+    /* MMIO management */
+    regValue = (((t_uint64)STM_BASE_ADDR) << IHOST_EXT_MMIO_BASE_ADDR_SHIFT) |
+            (((t_uint64)DMA_CTRL_END_ADDR) << IHOST_EXT_MMIO_DATA_EXT_BUS_TOP_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EXT_MMIO_BASE_DATA_EXT_BUS_TOP_REG, regValue);
+
+    /* Configure Icache */
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_INST_BURST_SZ_REG, IHOST_INST_BURST_SZ_AUTO);
+
+    regValue = (t_uint64)(IHOST_ICACHE_MODE_PERFMETER_OFF | IHOST_ICACHE_MODE_L2_CACHE_ON |
+            IHOST_ICACHE_MODE_L1_CACHE_ON | IHOST_ICACHE_MODE_FILL_MODE_OFF);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_ICACHE_MODE_REG, regValue);
+    
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_DSP_updateCodeBase(
+        t_nmf_core_id coreId,
+        t_dsp_segment_type hwSegment,
+        t_cm_system_address src,
+        t_cm_system_address dst
+        )
+{
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    t_mmdsp_hw_regs *pRegs = pMmdspRegs[coreId];
+    t_uint32 offset = src.physical - mpcDesc[coreId].segments[hwSegment].base.physical;
+    t_cm_system_address base;
+    t_uint32 altBase = 0;
+    t_uint64 regValue = 0;
+    t_uint8  reg = 0;
+
+    base.physical = dst.physical - offset;
+    base.logical  = dst.logical  - offset;
+
+    switch(hwSegment) {
+    case SDRAM_CODE_EE:
+        altBase  = mpcDesc[coreId].segments[SDRAM_CODE_USER].base.physical;
+        regValue = ((t_uint64)(altBase) << IHOST_PRG_BASE2_ADDR_SHIFT) + (t_uint64)base.physical;
+        reg = IHOST_PRG_BASE_ADDR_REG;
+        break;
+    case SDRAM_CODE_USER:
+        altBase  = mpcDesc[coreId].segments[SDRAM_CODE_EE].base.physical;
+        regValue = ((t_uint64)(base.physical) << IHOST_PRG_BASE2_ADDR_SHIFT) + (t_uint64)altBase;
+        reg = IHOST_PRG_BASE_ADDR_REG;
+        break;
+    case ESRAM_CODE_EE:
+        altBase = mpcDesc[coreId].segments[ESRAM_CODE_USER].base.physical;
+        regValue = ((t_uint64)(altBase) << IHOST_PRG_BASE4_ADDR_SHIFT) + (t_uint64)base.physical;
+        reg = IHOST_PRG_BASE_34_ADDR_REG;
+        break;
+    case ESRAM_CODE_USER:
+        altBase = mpcDesc[coreId].segments[ESRAM_CODE_EE].base.physical;
+        regValue = ((t_uint64)(base.physical) << IHOST_PRG_BASE4_ADDR_SHIFT) + (t_uint64)altBase;
+        reg = IHOST_PRG_BASE_34_ADDR_REG;
+        break;
+    default:
+        CM_ASSERT(0);
+    }
+
+    LOG_INTERNAL(1, "##### DSP Code Base Update [%d]: 0x%x -> 0x%x (0x%x)\n",
+            hwSegment, mpcDesc[coreId].segments[hwSegment].base.physical, base.physical, base.logical, 0, 0);
+
+    WRITE_INDIRECT_HOST_REG(pRegs, reg, regValue);
+
+    mpcDesc[coreId].segments[hwSegment].base = base;
+#endif
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_DSP_updateDataBase(
+        t_nmf_core_id coreId,
+        t_dsp_segment_type hwSegment,
+        t_cm_system_address src,
+        t_cm_system_address dst
+        )
+{
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    t_mmdsp_hw_regs *pRegs = pMmdspRegs[coreId];
+    t_uint32 offset = src.physical - mpcDesc[coreId].segments[hwSegment].base.physical;
+    t_cm_system_address base;
+    t_uint32 size = mpcDesc[coreId].segments[hwSegment].size; //in bytes
+    t_uint64 regValue;
+    t_uint8  reg = 0;
+    t_uint8  top = 0;
+
+    base.physical = dst.physical - offset;
+    base.logical  = dst.logical  - offset;
+
+    switch(hwSegment) {
+    case SDRAM_DATA_EE:
+        reg = IHOST_DATA_EXT_BUS_BASE_REG;
+        top = IHOST_DATA_EXT_BUS_TOP_16_24_REG;
+        break;
+    case SDRAM_DATA_USER:
+        reg = IHOST_DATA_EXT_BUS_BASE2_REG;
+        top = IHOST_EXT_BUS_TOP2_16_24_REG;
+        break;
+    case ESRAM_DATA_EE:
+        reg = IHOST_DATA_EXT_BUS_BASE3_REG;
+        top = IHOST_EXT_BUS_TOP3_16_24_REG;
+        break;
+    case ESRAM_DATA_USER:
+        reg = IHOST_DATA_EXT_BUS_BASE4_REG;
+        top = IHOST_EXT_BUS_TOP4_16_24_REG;
+        break;
+    default:
+        CM_ASSERT(0);
+    }
+
+    LOG_INTERNAL(1, "##### DSP Data Base Update [%d]: 0x%x -> 0x%x (0x%x)\n",
+            hwSegment, mpcDesc[coreId].segments[hwSegment].base.physical, base.physical, base.logical, 0, 0);
+
+    /* Program data24/16 base */
+    regValue = (((t_uint64)(base.physical)) << IHOST_DATA_EXT_BUS_BASE2_24_SHIFT) |
+               (((t_uint64)(base.physical)) << IHOST_DATA_EXT_BUS_BASE2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, reg, regValue);
+
+    /* Program data24/16 top */
+    regValue = (((t_uint64)(base.physical + size - 1)) << IHOST_DATA_EXT_BUS_TOP2_24_SHIFT) |
+               (((t_uint64)(base.physical + size - 1)) << IHOST_DATA_EXT_BUS_TOP2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, top, regValue);
+
+    mpcDesc[coreId].segments[hwSegment].base = base;
+#endif
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_DSP_GetDspDataAddressInfo(t_nmf_core_id coreId, t_uint32 addr, t_dsp_address_info *info)
+{
+    t_uint32 i, j;
+
+    for(j = 0; j < 2; j++)
+    {
+        for(i = 0; i < DATA_BASE_NUMBER; i++)
+        {
+            if(DATA_ADDRESS_BASE[i].startAddress[j] <= addr && addr < DATA_ADDRESS_BASE[i + 1].startAddress[j])
+            {
+                info->segmentType = DATA_ADDRESS_BASE[i].segmentType;
+                info->baseOffset = (addr - DATA_ADDRESS_BASE[i].startAddress[j]) * (2 + j * 2);
+
+                return CM_OK;
+            }
+        }
+    }
+
+    CM_ASSERT(0);
+    //return CM_INVALID_PARAMETER;
+}
+
+static t_cm_error mmdsp_ConfigureAfterBoot(t_nmf_core_id coreId, t_uint8 nbXramBlocks, t_uint8 nbYramBlocks)
+{
+    /* Configure all blocks as X only, except the Y ones */
+    pMmdspRegs[coreId]->mmio_16.config_data_mem.value = (t_uint16)(~(((1U << nbYramBlocks) - 1) << (nbXramBlocks-nbYramBlocks)));
+    
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    /* enable write posting */
+    MMDSP_ENABLE_WRITE_POSTING(pMmdspRegs[coreId]);
+#endif
+    
+    return CM_OK;
+}
+
+
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/bfd.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/bfd.h
new file mode 100644
index 00000000000..2bccf9c073b
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/bfd.h
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf bfd relocation.
+ *
+ * \defgroup ELFLOADER MMDSP ELF loader.
+ */
+#ifndef __INC_CM_ELF_BFD_H
+#define __INC_CM_ELF_BFD_H
+
+#include <cm/inc/cm_type.h>
+
+/*
+ * Relocation spcification
+ */
+enum complain_overflow
+{
+  /* Do not complain on overflow.  */
+  complain_overflow_dont,
+
+  /* Complain if the bitfield overflows, whether it is considered
+     as signed or unsigned.  */
+  complain_overflow_bitfield,
+
+  /* Complain if the value overflows when considered as signed
+     number.  */
+  complain_overflow_signed,
+
+  /* Complain if the value overflows when considered as an
+     unsigned number.  */
+  complain_overflow_unsigned
+};
+
+struct reloc_howto_struct
+{
+  /*  The type field has mainly a documentary use - the back end can
+      do what it wants with it, though normally the back end's
+      external idea of what a reloc number is stored
+      in this field.  For example, a PC relative word relocation
+      in a coff environment has the type 023 - because that's
+      what the outside world calls a R_PCRWORD reloc.  */
+  unsigned int type;
+
+  /*  The value the final relocation is shifted right by.  This drops
+      unwanted data from the relocation.  */
+  unsigned int rightshift;
+
+  /*  The size of the item to be relocated.  This is *not* a
+      power-of-two measure.  To get the number of bytes operated
+      on by a type of relocation, use bfd_get_reloc_size.  */
+  int size;
+
+  /*  The number of bits in the item to be relocated.  This is used
+      when doing overflow checking.  */
+  unsigned int bitsize;
+
+  /*  Notes that the relocation is relative to the location in the
+      data section of the addend.  The relocation function will
+      subtract from the relocation value the address of the location
+      being relocated.  */
+  t_uint64 pc_relative;
+
+  /*  The bit position of the reloc value in the destination.
+      The relocated value is left shifted by this amount.  */
+  unsigned int bitpos;
+
+  /* What type of overflow error should be checked for when
+     relocating.  */
+  enum complain_overflow complain_on_overflow;
+
+  void (*special_function)(void);
+
+  /* The textual name of the relocation type.  */
+  char *name;
+
+  /* Some formats record a relocation addend in the section contents
+     rather than with the relocation.  For ELF formats this is the
+     distinction between USE_REL and USE_RELA (though the code checks
+     for USE_REL == 1/0).  The value of this field is TRUE if the
+     addend is recorded with the section contents; when performing a
+     partial link (ld -r) the section contents (the data) will be
+     modified.  The value of this field is FALSE if addends are
+     recorded with the relocation (in arelent.addend); when performing
+     a partial link the relocation will be modified.
+     All relocations for all ELF USE_RELA targets should set this field
+     to FALSE (values of TRUE should be looked on with suspicion).
+     However, the converse is not true: not all relocations of all ELF
+     USE_REL targets set this field to TRUE.  Why this is so is peculiar
+     to each particular target.  For relocs that aren't used in partial
+     links (e.g. GOT stuff) it doesn't matter what this is set to.  */
+  char partial_inplace;
+
+  /* src_mask selects the part of the instruction (or data) to be used
+     in the relocation sum.  If the target relocations don't have an
+     addend in the reloc, eg. ELF USE_REL, src_mask will normally equal
+     dst_mask to extract the addend from the section contents.  If
+     relocations do have an addend in the reloc, eg. ELF USE_RELA, this
+     field should be zero.  Non-zero values for ELF USE_RELA targets are
+     bogus as in those cases the value in the dst_mask part of the
+     section contents should be treated as garbage.  */
+  t_uint64 src_mask;
+
+  /* dst_mask selects which parts of the instruction (or data) are
+     replaced with a relocated value.  */
+  t_uint64 dst_mask;
+
+  /* When some formats create PC relative instructions, they leave
+     the value of the pc of the place being relocated in the offset
+     slot of the instruction, so that a PC relative relocation can
+     be made just by adding in an ordinary offset (e.g., sun3 a.out).
+     Some formats leave the displacement part of an instruction
+     empty (e.g., m88k bcs); this flag signals the fact.  */
+  char pcrel_offset;
+};
+
+#define HOWTO(C, R, S, B, P, BI, O, SF, NAME, INPLACE, MASKSRC, MASKDST, PC) \
+  { (unsigned) C, R, S, B, P, BI, O, SF, NAME, INPLACE, MASKSRC, MASKDST, PC }
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/common.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/common.h
new file mode 100644
index 00000000000..c51845d5f96
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/common.h
@@ -0,0 +1,125 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf common definition.
+ */
+#ifndef __INC_CM_ELF_COMMON_H
+#define __INC_CM_ELF_COMMON_H
+
+#include <cm/engine/component/inc/nmfheaderabi.h>
+#include <cm/engine/elf/inc/elfabi.h>
+#include <cm/engine/elf/inc/reloc.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/component/inc/description.h>
+#include <cm/engine/utils/inc/string.h>
+
+
+#define MAX_SEGMENT  20 // Just in order to not allocate them dynamically
+
+struct XXElf;
+
+/**
+ * \brief Structure used as database of pushed component.
+ */
+typedef struct {
+    t_instance_property instanceProperty;
+    t_uint32            magicNumber;          //!< Magic Number
+    t_dup_char          foundedTemplateName;
+    t_uint32            minStackSize;           //!< Minimum stack size
+
+    struct XXElf        *ELF;
+
+    t_elfSegment segments[NUMBER_OF_MMDSP_MEMORY];
+
+    t_bool              temporaryDescription;
+
+    t_memory_reference          memoryForConstruct;
+    t_memory_reference          memoryForStart;
+    t_memory_reference          memoryForStop;
+    t_memory_reference          memoryForDestroy;
+
+    t_uint8                     requireNumber;          //!< Number of interface required by this template
+    t_uint8                     attributeNumber;        //!< Number of attributes in this template
+    t_uint8                     propertyNumber;         //!< Number of properties in this template
+    t_uint8                     provideNumber;          //!< Number of interface provided by this template
+
+    t_interface_require         *requires;              //!< Array of interface required by this template
+    t_attribute                 *attributes;            //!< Array of attributes in this template
+    t_property                  *properties;            //!< Array of properties in this template
+    t_interface_provide         *provides;              //!< Array of interface provided by this template
+
+} t_elfdescription;
+
+/**
+ * \brief Temporary structure used as database when pushing component.
+ */
+typedef struct
+{
+    const char              *elfdata;
+    const char              *sectionData[50]; // YES it must be dynamic, but i'm tired.
+
+    t_bool                  isExecutable;
+
+    t_sint32                nmfSectionIndex;
+    const void              *relaNmfSegment, *relaNmfSegmentEnd;
+    const void              *relaNmfSegmentSymbols;
+    const char              *relaNmfSegmentStrings;
+
+    const t_elf_component_header*elfheader;
+
+
+} t_tmp_elfdescription;
+
+
+t_cm_error ELF64_LoadComponent(
+        t_uint16                    e_machine,
+        const char                  *elfdata,
+        t_elfdescription            **elfhandlePtr,
+        t_tmp_elfdescription        *elftmp);
+t_cm_error ELF64_ComputeSegment(
+        t_elfdescription            *elfhandle,
+        t_tmp_elfdescription        *elftmp);
+
+void ELF64_UnloadComponent(
+        t_elfdescription            *elfhandle);
+
+t_cm_error ELF64_loadSegment(
+        t_elfdescription            *elfhandle,
+        t_memory_handle             *memory,
+        t_memory_property           property);
+t_cm_error ELF64_relocateSegments(
+        t_memory_handle             *memories,
+        t_elfdescription            *elf,
+        t_memory_property           property,
+        void                        *cbContext);
+t_cm_error ELF64_getRelocationMemory(
+        t_elfdescription            *elfhandle,
+        t_tmp_elfdescription        *elftmp,
+        t_uint32                    offsetInNmf,
+        t_memory_reference          *memory);
+
+const t_elfmemory* MMDSP_getMappingById(t_memory_id memId);
+const t_elfmemory* MMDSP_getMappingByName(const char* sectionName, t_instance_property property);
+void MMDSP_serializeMemories(t_instance_property property,
+        const t_elfmemory** codeMemory, const t_elfmemory** thisMemory);
+void MMDSP_copySection(t_uint32 origAddr, t_uint32 remoteAddr, t_uint32 sizeInByte);
+void MMDSP_bzeroSection(t_uint32 remoteAddr, t_uint32 sizeInByte);
+void MMDSP_loadedSection(t_nmf_core_id coreId, t_memory_id memId, t_memory_handle handle);
+void MMDSP_unloadedSection(t_nmf_core_id coreId, t_memory_id memId, t_memory_handle handle);
+
+void MMDSP_copyCode(t_uint64 * remoteAddr64, const char* origAddr, int nb);
+void MMDSP_copyData24(t_uint32 * remoteAddr32, const char* origAddr, int nb);
+void MMDSP_copyData16(t_uint16 * remoteAddr16, const char* origAddr, int nb);
+
+t_uint32 cm_resolvSymbol(
+        void* context,
+        t_uint32 type,
+        t_dup_char symbolName,
+        char* reloc_addr);
+
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/elfabi.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/elfabi.h
new file mode 100644
index 00000000000..cbcc6db3b9b
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/elfabi.h
@@ -0,0 +1,539 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#ifndef _CM_ELF_H
+#define _CM_ELF_H 1
+
+typedef t_uint16 Elf32_Half;
+typedef t_uint16 Elf64_Half;
+
+typedef t_uint32 Elf32_Word;
+typedef t_sint32 Elf32_Sword;
+typedef t_uint32 Elf64_Word;
+typedef t_sint32 Elf64_Sword;
+
+typedef t_uint64 Elf32_Xword;
+typedef t_sint64 Elf32_Sxword;
+typedef t_uint64 Elf64_Xword;
+typedef t_sint64 Elf64_Sxword;
+
+typedef t_uint32 Elf32_Addr;
+typedef t_uint64 Elf64_Addr;
+
+typedef t_uint32 Elf32_Off;
+typedef t_uint64 Elf64_Off;
+
+typedef t_uint16 Elf32_Section;
+typedef t_uint16 Elf64_Section;
+
+typedef Elf32_Half Elf32_Versym;
+typedef Elf64_Half Elf64_Versym;
+
+
+/*********************************************
+ * Header
+ *********************************************/
+#define EI_NIDENT (16)                              //!< Size of e_ident[]
+
+#define EI_MAG0         0               //!< File identification
+#define ELFMAG0             0x7f
+
+#define EI_MAG1         1               //!< File identification
+#define ELFMAG1             'E'
+
+#define EI_MAG2         2               //!< File identification
+#define ELFMAG2             'L'
+
+#define EI_MAG3         3               //!< File identification
+#define ELFMAG3             'F'
+
+#define EI_CLASS        4               //!< File class
+#define ELFCLASSNONE        0               //!< Invalid class
+#define ELFCLASS32          1               //!< 32-bit objects
+#define ELFCLASS64          2               //!< 64-bit objects
+
+#define EI_DATA         5               //!< Data encoding
+#define ELFDATANONE         0               //!< Invalid data encoding
+#define ELFDATA2LSB         1               //!< 2's complement, little endian
+#define ELFDATA2MSB         2               //!< 2's complement, big endian
+
+#define EI_VERSION      6               //!< File version
+
+#define EI_OSABI        7               //!< OS ABI identification
+#define ELFOSABI_NONE       0               //!<  No extension
+#define ELFOSABI_HPUX       1               //!<  HP-UX
+#define ELFOSABI_NETBSD     2               //!<  NetBSD
+#define ELFOSABI_LINUX      3               //!<  Linux
+#define ELFOSABI_SOLARIS    6               //!<  Sun Solaris
+#define ELFOSABI_AIX        7               //!<  AIX
+#define ELFOSABI_IRIX       8               //!<  IRIX
+#define ELFOSABI_FREEBSD    9               //!<  FreeBSD
+#define ELFOSABI_TRU64      10              //!<  Compaq TRU64 UNIX
+#define ELFOSABI_MODESTO    11              //!<  Novell Modesto
+#define ELFOSABI_OPENBSD    12              //!<  Open BSD
+#define ELFOSABI_OPENVMS    13              //!<  Open VMS
+#define ELFOSABI_NSK        14              //!<  HP Non-Stop-Kernel
+
+#define EI_ABIVERSION   8               //!< ABI version
+
+#define EI_PAD          9               //!< Start of padding byte
+
+
+typedef struct
+{
+    unsigned char       e_ident[EI_NIDENT];             //!< The initial bytes mark the file as an object file and provide machine-independent data with which to decode and interpret the file's contents
+    Elf32_Half          e_type;                         //!< This member identifies the object file type
+    Elf32_Half          e_machine;                      //!< This member's value specifies the required architecture for an individual file
+    Elf32_Word          e_version;                      //!< This member identifies the object file version
+    Elf32_Addr          e_entry;                        //!< This member gives the virtual address to which the system first transfers control, thus starting the process
+    Elf32_Off           e_phoff;                        //!< This member holds the program header table's file offset in bytes
+    Elf32_Off           e_shoff;                        //!< This member holds the section header table's file offset in bytes
+    Elf32_Word          e_flags;                        //!< This member holds processor-specific flags associated with the file
+    Elf32_Half          e_ehsize;                       //!< This member holds the ELF header's size in bytes
+    Elf32_Half          e_phentsize;                    //!< This member holds the size in bytes of one entry in the file's program header table; all entries are the same size
+    Elf32_Half          e_phnum;                        //!< This member holds the number of entries in the program header table
+    Elf32_Half          e_shentsize;                    //!< This member holds a section header's size in bytes
+    Elf32_Half          e_shnum;                        //!< This member holds the number of entries in the section header table
+    Elf32_Half          e_shstrndx;                     //!< This member holds the section header table index of the entry associated with the section name string table
+} Elf32_Ehdr;       //!< 32bit Entry Header
+
+typedef struct
+{
+    unsigned char       e_ident[EI_NIDENT];             //!< The initial bytes mark the file as an object file and provide machine-independent data with which to decode and interpret the file's contents
+    Elf64_Half          e_type;                         //!< This member identifies the object file type
+    Elf64_Half          e_machine;                      //!< This member's value specifies the required architecture for an individual file
+    Elf64_Word          e_version;                      //!< This member identifies the object file version
+    Elf64_Addr          e_entry;                        //!< This member gives the virtual address to which the system first transfers control, thus starting the process
+    Elf64_Off           e_phoff;                        //!< This member holds the program header table's file offset in bytes
+    Elf64_Off           e_shoff;                        //!< This member holds the section header table's file offset in bytes
+    Elf64_Word          e_flags;                        //!< This member holds processor-specific flags associated with the file
+    Elf64_Half          e_ehsize;                       //!< This member holds the ELF header's size in bytes
+    Elf64_Half          e_phentsize;                    //!< This member holds the size in bytes of one entry in the file's program header table; all entries are the same size
+    Elf64_Half          e_phnum;                        //!< This member holds the number of entries in the program header table
+    Elf64_Half          e_shentsize;                    //!< This member holds a section header's size in bytes
+    Elf64_Half          e_shnum;                        //!< This member holds the number of entries in the section header table
+    Elf64_Half          e_shstrndx;                     //!< This member holds the section header table index of the entry associated with the section name string table
+} Elf64_Ehdr;       //!< 64bit Entry Header
+
+/*
+ * e_type
+ */
+#define ET_NONE         0                   //!< No file type
+#define ET_REL          1                   //!< Relocatable file
+#define ET_EXEC         2                   //!< Executable file
+#define ET_DYN          3                   //!< Shared object file
+#define ET_CORE         4                   //!< Core file
+#define ET_LOOS         0xfe00              //!< Operating system-specific
+#define ET_HIOS         0xfeff              //!< Operating system-specific
+#define ET_LOPROC       0xff00              //!< Processor-specific
+#define ET_HIPROC       0xffff              //!< Processor-specific
+
+/*
+ * e_machine
+ */
+#define EM_NONE             0               //!< No machine
+#define EM_M32              1               //!< AT&T WE 32100
+#define EM_SPARC            2               //!< SUN SPARC
+#define EM_386              3               //!< Intel 80386
+#define EM_68K              4               //!< Motorola 68000
+#define EM_88K              5               //!< Motorola 88000
+#define EM_860              7               //!< Intel 80860
+#define EM_MIPS             8               //!< MIPS I architecture
+#define EM_S370             9               //!< IBM System/370
+#define EM_MIPS_RS3_LE      10              //!< MIPS R3000 little-endian
+#define EM_PARISC           15              //!< HPPA
+#define EM_VPP500           17              //!< Fujitsu VPP500
+#define EM_SPARC32PLUS      18              //!< Enhanced instruction set SPARC
+#define EM_960              19              //!< Intel 80960
+#define EM_PPC              20              //!< PowerPC
+#define EM_PPC64            21              //!< 64-bit PowerPC
+#define EM_S390             22              //!< IBM System/390 Processor
+#define EM_V800             36              //!< NEC V800
+#define EM_FR20             37              //!< Fujitsu FR20
+#define EM_RH32             38              //!< TRW RH-32
+#define EM_RCE              39              //!< Motorola RCE
+#define EM_ARM              40              //!< Advanced RISC Machines ARM
+#define EM_FAKE_ALPHA       41              //!< Digital Alpha
+#define EM_SH               42              //!< Hitachi SH
+#define EM_SPARCV9          43              //!< SPARC Version 9
+#define EM_TRICORE          44              //!< Siemens TriCore embedded processor
+#define EM_ARC              45              //!< Argonaut RISC Core, Argonaut Technologies Inc
+#define EM_H8_300           46              //!< Hitachi H8/300
+#define EM_H8_300H          47              //!< Hitachi H8/300H
+#define EM_H8S              48              //!< Hitachi H8S
+#define EM_H8_500           49              //!< Hitachi H8/500
+#define EM_IA_64            50              //!< Intel IA-64 processor architecture
+#define EM_MIPS_X           51              //!< Stanford MIPS-X
+#define EM_COLDFIRE         52              //!< Motorola ColdFire
+#define EM_68HC12           53              //!< Motorola M68HC12
+#define EM_MMA              54              //!< Fujitsu MMA Multimedia Accelerator
+#define EM_PCP              55              //!< Siemens PCP
+#define EM_NCPU             56              //!< Sony nCPU embedded RISC processor
+#define EM_NDR1             57              //!< Denso NDR1 microprocessor
+#define EM_STARCORE         58              //!< Motorola Start*Core processor
+#define EM_ME16             59              //!< Toyota ME16 processor
+#define EM_ST100            60              //!< STMicroelectronics ST100 processor
+#define EM_TINYJ            61              //!< Advanced Logic Corp. TinyJ embedded processor family
+#define EM_X86_64           62              //!< AMD x86-64 architecture
+#define EM_PDSP             63              //!< Sony DSP Processor
+#define EM_PDP10            64              //!< Digital Equipment Corp. PDP-10
+#define EM_PDP11            65              //!< Digital Equipment Corp. PDP-11
+#define EM_FX66             66              //!< Siemens FX66 microcontroller
+#define EM_ST9PLUS          67              //!< STMicroelectronics ST9+ 8/16 bit microcontroller
+#define EM_ST7              68              //!< STMicroelectronics ST7 8-bit microcontroller
+#define EM_68HC16           69              //!< Motorola MC68HC16 Microcontroller
+#define EM_68HC11           70              //!< Motorola MC68HC11 Microcontroller
+#define EM_68HC08           71              //!< Motorola MC68HC08 Microcontroller
+#define EM_68HC05           72              //!< Motorola MC68HC05 Microcontroller
+#define EM_SVX              73              //!< Silicon Graphics SVx
+#define EM_ST19             74              //!< STMicroelectronics ST19 8-bit microcontroller
+#define EM_VAX              75              //!< Digital VAX
+#define EM_CRIS             76              //!< Axis Communications 32-bit embedded processor
+#define EM_JAVELIN          77              //!< Infifineon Technologies 32-bit embedded processor
+#define EM_FIREPATH         78              //!< Element 14 64-bit DSP Processor
+#define EM_ZSP              79              //!< LSI Logic 16-bit DSP Processor
+#define EM_MMIX             80              //!< Donald Knuth's educational 64-bit processor
+#define EM_HUANY            81              //!< Harvard University machine-independent object files
+#define EM_PRISM            82              //!< SiTera Prism
+#define EM_AVR              83              //!< Atmel AVR 8-bit microcontroller
+#define EM_FR30             84              //!< Fujitsu FR30
+#define EM_D10V             85              //!< Mitsubishi D10V
+#define EM_D30V             86              //!< Mitsubishi D30V
+#define EM_V850             87              //!< NEC v850
+#define EM_M32R             88              //!< Mitsubishi M32R
+#define EM_MN10300          89              //!< Matsushita MN10300
+#define EM_MN10200          90              //!< Matsushita MN10200
+#define EM_PJ               91              //!< picoJava
+#define EM_OPENRISC         92              //!< OpenRISC 32-bit embedded processor
+#define EM_ARC_A5           93              //!< ARC International ARCompact processor (old spelling/synonym: EM_ARC_A5)
+#define EM_XTENSA           94              //!< Tensilica Xtensa Architecture
+#define EM_VIDEOCORE        95              //!< Alphamosaic VideoCore processor
+#define EM_TMM_GPP          96              //!< Thompson Multimedia General Purpose Processor
+#define EM_NS32K            97              //!< National Semiconductor 32000 series
+#define EM_TPC              98              //!< Tenor Network TPC processor
+#define EM_SNP1K            99              //!< Trebia SNP 1000 processor
+#define EM_ST200            100             //!< STMicroelectronics (www.st.com) ST200 microcontroller
+#define EM_IP2K             101             //!< Ubicom IP2xxx microcontroller family
+#define EM_MAX              102             //!< MAX Processor
+#define EM_CR               103             //!< National Semiconductor CompactRISC microprocessor
+#define EM_F2MC16           104             //!< Fujitsu F2MC16
+#define EM_MSP430           105             //!< Texaxas Instruments embedded microcontroller msp430
+#define EM_BLACKFIN         106             //!< Analog Devices Blackfin (DSP) processor
+#define EM_SE_C33           107             //!< S1C33 Family of Seiko Epspson processors
+#define EM_SEP              108             //!< Sharp embedded microprocessor
+#define EM_ARCA             109             //!< Arca RISC Microprocessor
+#define EM_UNICORE          110             //!< Microprocessor series from PKU-Unity Ltd. and MPRC of Peking University
+#define EM_EXCESS           111             //!< eXcess: 16/32/64-bit configurable embedded CPU
+#define EM_DXP              112             //!< Icera Semiconductor Inc. Deep Execution Processor
+#define EM_ALTERA_NIOS2     113             //!< Altera Nios II soft-core processor
+#define EM_CRX              114             //!< National Semiconductor CompactRISC CRX microprocessor
+#define EM_XGATE            115             //!< Motorola XGATE embedded processor
+#define EM_C166             116             //!< Infifineon C16x/XC16x processor
+#define EM_M16C             117             //!< Renesas M16C series microprocessors
+#define EM_DSPIC30F         118             //!< Microchip Technology dsPIC30F Digital Signal Controller
+#define EM_CE               119             //!< Freescale Communication Engine RISC core
+#define EM_M32C             120             //!< Renesas M32C series microprocessors
+#define EM_TSK3000          131             //!< Altium TSK3000 core
+#define EM_RS08             132             //!< Freescale RS08 embedded processor
+#define EM_ECOG2            134             //!< Cyan Technology eCOG2 microprocessor
+#define EM_SCORE7           135             //!< Sunplus S+core7 RISC processor
+#define EM_DSP24            136             //!< New Japan Radio (NJR) 24-bit DSP Processor
+#define EM_VIDEOCORE3       137             //!< Broadcom VideoCore III processor
+#define EM_LATTICEMICO32    138             //!< RISC processor for Lattice FPGA architecture
+#define EM_SE_C17           139             //!< Seiko Epspson C17 family
+#define EM_TI_C6000         140             //!< The Texaxas Instruments TMS320C6000 DSP family
+#define EM_TI_C2000         141             //!< The Texaxas Instruments TMS320C2000 DSP family
+#define EM_TI_C5500         142             //!< The Texaxas Instruments TMS320C55x DSP family
+#define EM_MMDSP_PLUS       160             //!< STMicroelectronics 64bit VLIW Data Signal Processor
+#define EM_CYPRESS_M8C      161             //!< Cypress M8C microprocessor
+#define EM_R32C             162             //!< Renesas R32C series microprocessors
+#define EM_TRIMEDIA         163             //!< NXP Semiconductors TriMedia architecture family
+#define EM_QDSP6            164             //!< QUALCOMM DSP6 Processor
+#define EM_8051             165             //!< Intel 8051 and variants
+#define EM_STXP7X           166             //!< STMicroelectronics STxP7x family of configurable and extensible RISC processors
+#define EM_NDS32            167             //!< Andes Technology compact code size embedded RISC processor family
+#define EM_ECOG1            168             //!< Cyan Technology eCOG1X family
+#define EM_ECOG1X           168             //!< Cyan Technology eCOG1X family
+#define EM_MAXQ30           169             //!< Dallas Semiconductor MAXQ30 Core Micro-controllers
+#define EM_XIMO16           170             //!< New Japan Radio (NJR) 16-bit DSP Processor
+#define EM_MANIK            171             //!< M2000 Reconfigurable RISC Microprocessor
+#define EM_CRAYNV2          172             //!< Cray Inc. NV2 vector architecture
+#define EM_RX               173             //!< Renesas RX family
+#define EM_METAG            174             //!< Imagination Technologies META processor architecture
+#define EM_MCST_ELBRUS      175             //!< MCST Elbrus general purpose hardware architecture
+#define EM_ECOG16           176             //!< Cyan Technology eCOG16 family
+#define EM_CR16             177             //!< National Semiconductor CompactRISC CR16 16-bit microprocessor
+#define EM_ETPU             178             //!< Freescale Extended Time Processing Unit
+#define EM_SLE9X            179             //!< Infifineon Technologies SLE9X core
+#define EM_AVR32            185             //!< Atmel Corporation 32-bit microprocessor family
+#define EM_STM8             186             //!< STMicroeletronics STM8 8-bit microcontroller
+#define EM_TILE64           187             //!< Tilera TILE64 multicore architecture family
+#define EM_TILEPRO          188             //!< Tilera TILEPro multicore architecture family
+#define EM_MICROBLAZE       189             //!< Xilinx MicroBlaze 32-bit RISC soft processor core
+#define EM_CUDA             190             //!< NVIDIA CUDA architecture
+#define EM_TILEGX           191             //!< Tilera TILE-Gx multicore architecture family
+
+/*
+ * e_version (version)
+ */
+#define EV_NONE             0               //!< Invalid version
+#define EV_CURRENT          1               //!< Current version
+
+
+/*********************************************
+ * Section
+ *********************************************/
+typedef struct
+{
+  Elf32_Word            sh_name;                    //!< This member specifies the name of the section
+  Elf32_Word            sh_type;                    //!< This member categorizes the section's contents and semantics
+  Elf32_Word            sh_flags;                   //!< Sections support 1-bit flags that describe miscellaneous attributes
+  Elf32_Addr            sh_addr;                    //!< If the section will appear in the memory image of a process, this member gives the address at which the section's first byte should reside
+  Elf32_Off             sh_offset;                  //!< This member's value gives the byte offset from the beginning of the file to the first byte in the section
+  Elf32_Word            sh_size;                    //!< This member gives the section's size in bytes
+  Elf32_Word            sh_link;                    //!< This member holds a section header table index link, whose interpretation depends on the section type
+  Elf32_Word            sh_info;                    //!< This member holds extra information, whose interpretation depends on the section type
+  Elf32_Word            sh_addralign;               //!< Some sections have address alignment constraints
+  Elf32_Word            sh_entsize;                 //!< Some sections hold a table of fixed-size entries, such as a symbol table
+} Elf32_Shdr;       //!< 32bit Section header
+
+typedef struct
+{
+  Elf64_Word            sh_name;                    //!< This member specifies the name of the section
+  Elf64_Word            sh_type;                    //!< This member categorizes the section's contents and semantics
+  Elf64_Xword           sh_flags;                   //!< Sections support 1-bit flags that describe miscellaneous attributes
+  Elf64_Addr            sh_addr;                    //!< If the section will appear in the memory image of a process, this member gives the address at which the section's first byte should reside
+  Elf64_Off             sh_offset;                  //!< This member's value gives the byte offset from the beginning of the file to the first byte in the section
+  Elf64_Xword           sh_size;                    //!< This member gives the section's size in bytes
+  Elf64_Word            sh_link;                    //!< This member holds a section header table index link, whose interpretation depends on the section type
+  Elf64_Word            sh_info;                    //!< This member holds extra information, whose interpretation depends on the section type
+  Elf64_Xword           sh_addralign;               //!< Some sections have address alignment constraints
+  Elf64_Xword           sh_entsize;                 //!< Some sections hold a table of fixed-size entries, such as a symbol table
+} Elf64_Shdr;       //!< 64bit Section header
+
+/*
+ * Special Section Indexes
+ */
+#define SHN_UNDEF           0                   //!< This value marks an undefined, missing, irrelevant, or otherwise meaningless section reference
+#define SHN_LORESERVE       0xff00              //!< This value specifies the lower bound of the range of reserved indexes
+#define SHN_LOPROC          0xff00              //!< Values in this inclusive range are reserved for processor-specific semantics
+#define SHN_HIPROC          0xff1f              //!< Values in this inclusive range are reserved for processor-specific semantics
+#define SHN_LOOS            0xff20              //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define SHN_HIOS            0xff3f              //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define SHN_ABS             0xfff1              //!< This value specifies absolute values for the corresponding reference
+#define SHN_COMMON          0xfff2              //!< Symbols defined relative to this section are common symbols
+#define SHN_XINDEX          0xffff              //!< This value is an escape value
+#define SHN_HIRESERVE       0xffff              //!< This value specifies the upper bound of the range of reserved indexes
+
+/*
+ * sh_type
+ */
+#define SHT_NULL            0                   //!< This value marks the section header as inactive
+#define SHT_PROGBITS        1                   //!< The section holds information defined by the program
+#define SHT_SYMTAB          2                   //!< These sections hold a symbol table
+#define SHT_STRTAB          3                   //!< The section holds a string table
+#define SHT_RELA            4                   //!< The section holds relocation entries with explicit addends, such as type Elf32_Rela for the 32-bit class of object files or type Elf64_Rela for the 64-bit class of object files
+#define SHT_HASH            5                   //!< The section holds a symbol hash table
+#define SHT_DYNAMIC         6                   //!< The section holds information for dynamic linking
+#define SHT_NOTE            7                   //!< The section holds information that marks the file in some way
+#define SHT_NOBITS          8                   //!< A section of this type occupies no space in the file but otherwise resembles SHT_PROGBITS
+#define SHT_REL             9                   //!< The section holds relocation entries without explicit addends, such as type Elf32_Rel for the 32-bit class of object files or type Elf64_Rel for the 64-bit class of object files
+#define SHT_SHLIB           10                  //!< This section type is reserved but has unspecified semantics
+#define SHT_DYNSYM          11                  //!<
+#define SHT_INIT_ARRAY      14                  //!< This section contains an array of pointers to initialization functions
+#define SHT_FINI_ARRAY      15                  //!< This section contains an array of pointers to termination functions
+#define SHT_PREINIT_ARRAY   16                  //!< This section contains an array of pointers to functions that are invoked before all other initialization functions
+#define SHT_GROUP           17                  //!< This section defines a section group
+#define SHT_SYMTAB_SHNDX    18                  //!< This section is associated with a section of type SHT_SYMTAB and is required if any of the section header indexes referenced by that symbol table contain the escape value SHN_XINDEX
+#define SHT_LOOS            0x60000000          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define SHT_HIOS            0x6fffffff          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define SHT_LOPROC          0x70000000          //!< Values in this inclusive range are reserved for processor-specific semantics
+#define SHT_HIPROC          0x7fffffff          //!< Values in this inclusive range are reserved for processor-specific semantics
+#define SHT_LOUSER          0x80000000          //!< This value specifies the upper bound of the range of indexes reserved for application programs
+#define SHT_HIUSER          0x8fffffff          //!< This value specifies the upper bound of the range of indexes reserved for application programs
+
+/*
+ * sh_flags
+ */
+#define SHF_WRITE               0x1             //!< The section contains data that should be writable during process execution
+#define SHF_ALLOC               0x2             //!< The section occupies memory during process execution
+#define SHF_EXECINSTR           0x4             //!< The section contains executable machine instructions
+#define SHF_MERGE               0x10            //!< The data in the section may be merged to eliminate duplication
+#define SHF_STRINGS             0x20            //!< The data elements in the section consist of null-terminated character strings
+#define SHF_INFO_LINK           0x40            //!< The sh_info field of this section header holds a section header table index
+#define SHF_LINK_ORDER          0x80            //!< This flag adds special ordering requirements for link editors
+#define SHF_OS_NONCONFORMING    0x100           //!< This section requires special OS-specific processing (beyond the standard linking rules) to avoid incorrect behavior
+#define SHF_GROUP               0x200           //!< This section is a member (perhaps the only one) of a section group
+#define SHF_TLS                 0x400           //!< This section holds Thread-Local Storage, meaning that each separate execution flow has its own distinct instance of this data
+#define SHF_MASKOS              0x0ff00000      //!< All bits included in this mask are reserved for operating system-specific semantics
+#define SHF_MASKPROC            0xf0000000      //!< All bits included in this mask are reserved for processor-specific semantics
+
+
+/*********************************************
+ * Symbol
+ *********************************************/
+typedef struct
+{
+    Elf32_Word          st_name;                //!< This member holds an index into the object file's symbol string table, which holds the character representations of the symbol names
+    Elf32_Addr          st_value;               //!< This member gives the value of the associated symbol
+    Elf32_Word          st_size;                //!< Many symbols have associated sizes
+    unsigned char       st_info;                //!< This member specifies the symbol's type and binding attributes
+    unsigned char       st_other;               //!< This member currently specifies a symbol's visibility
+    Elf32_Section       st_shndx;               //!< Every symbol table entry is defined in relation to some section
+} Elf32_Sym;
+
+typedef struct
+{
+    Elf64_Word          st_name;                //!< This member holds an index into the object file's symbol string table, which holds the character representations of the symbol names
+    unsigned char       st_info;                //!< This member specifies the symbol's type and binding attributes
+    unsigned char       st_other;               //!< This member currently specifies a symbol's visibility
+    Elf64_Section       st_shndx;               //!< Every symbol table entry is defined in relation to some section
+    Elf64_Addr          st_value;               //!< This member gives the value of the associated symbol
+    Elf64_Xword         st_size;                //!< Many symbols have associated sizes
+} Elf64_Sym;
+
+/*
+ * st_info
+ */
+#define ELF32_ST_BIND(i)            ((i)>>4)
+#define ELF32_ST_TYPE(i)            ((i)&0xf)
+#define ELF32_ST_INFO(b,t)          (((b)<<4)+((t)&0xf))
+
+#define ELF64_ST_BIND(i)            ((i)>>4)
+#define ELF64_ST_TYPE(i)            ((i)&0xf)
+#define ELF64_ST_INFO(b,t)          (((b)<<4)+((t)&0xf))
+
+
+/* st_info (symbol binding) */
+#define STB_LOCAL               0           //!< Local symbols are not visible outside the object file containing their definition
+#define STB_GLOBAL              1           //!< Global symbols are visible to all object files being combined
+#define STB_WEAK                2           //!< Weak symbols resemble global symbols, but their definitions have lower precedence
+#define STB_LOOS                10          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define STB_HIOS                12          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define STB_LOPROC              13          //!< Values in this inclusive range are reserved for processor-specific semantics
+#define STB_HIPROC              15          //!< Values in this inclusive range are reserved for processor-specific semantics
+
+/* st_info (symbol type)  */
+#define STT_NOTYPE              0           //!< The symbol's type is not specified
+#define STT_OBJECT              1           //!< The symbol is associated with a data object, such as a variable, an array, and so on
+#define STT_FUNC                2           //!< The symbol is associated with a function or other executable code
+#define STT_SECTION             3           //!< The symbol is associated with a section
+#define STT_FILE                4           //!< Conventionally, the symbol's name gives the name of the source file associated with the object file
+#define STT_COMMON              5           //!< The symbol labels an uninitialized common block
+#define STT_TLS                 6           //!<  The symbol specifies a Thread-Local Storage entity
+#define STT_LOOS                10          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define STT_HIOS                12          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define STT_LOPROC              13          //!< Values in this inclusive range are reserved for processor-specific semantics
+#define STT_HIPROC              15          //!< Values in this inclusive range are reserved for processor-specific semantics
+
+/*
+ * st_other
+ */
+#define ELF32_ST_VISIBILITY(o)          ((o)&0x3)
+#define ELF64_ST_VISIBILITY(o)          ((o)&0x3)
+
+
+#define STV_DEFAULT             0         //!< The visibility of symbols with the STV_DEFAULT attribute is as specified by the symbol's binding type
+#define STV_INTERNAL            1         //!< A symbol defined in the current component is protected if it is visible in other components but not preemptable, meaning that any reference to such a symbol from within the defining component must be resolved to the definition in that component, even if there is a definition in another component that would preempt by the default rules
+#define STV_HIDDEN              2         //!< A symbol defined in the current component is hidden if its name is not visible to other components
+#define STV_PROTECTED           3         //!< The meaning of this visibility attribute may be defined by processor supplements to further constrain hidden symbols
+
+
+/*********************************************
+ * Relocation
+ *********************************************/
+typedef struct
+{
+    Elf32_Addr          r_offset;           //!< This member gives the location at which to apply the relocation action
+    Elf32_Word          r_info;             //!< This member gives both the symbol table index with respect to which the relocation must be made, and the type of relocation to apply
+} Elf32_Rel;        //!< 32bits Relocation Entries
+
+typedef struct
+{
+    Elf64_Addr          r_offset;           //!< This member gives the location at which to apply the relocation action
+    Elf64_Xword         r_info;             //!< This member gives both the symbol table index with respect to which the relocation must be made, and the type of relocation to apply
+} Elf64_Rel;        //!< 32bits Relocation Entries
+
+typedef struct
+{
+    Elf32_Addr          r_offset;           //!< This member gives the location at which to apply the relocation action
+    Elf32_Word          r_info;             //!< This member gives both the symbol table index with respect to which the relocation must be made, and the type of relocation to apply
+    Elf32_Sword         r_addend;           //!< This member specifies a constant addend used to compute the value to be stored into the relocatable field
+} Elf32_Rela;        //!< 32bits Relocation Addend Entries
+
+typedef struct
+{
+    Elf64_Addr          r_offset;           //!< This member gives the location at which to apply the relocation action
+    Elf64_Xword         r_info;             //!< This member gives both the symbol table index with respect to which the relocation must be made, and the type of relocation to apply
+    Elf64_Sxword        r_addend;           //!< This member specifies a constant addend used to compute the value to be stored into the relocatable field
+} Elf64_Rela;        //!< 32bits Relocation Addend Entries
+
+
+/*
+ * r_info
+ */
+#define ELF32_R_SYM(i)              ((i)>>8)
+#define ELF32_R_TYPE(i)             ((unsigned char)(i))
+#define ELF32_R_INFO(s,t)           (((s)<<8)+(unsigned char)(t))
+
+#define ELF64_R_SYM(i)              ((i)>>32)
+#define ELF64_R_TYPE(i)             ((i)&0xffffffffL)
+#define ELF64_R_INFO(s,t)           (((s)<<32)+((t)&0xffffffffL))
+
+
+
+/*********************************************
+ * Program
+ *********************************************/
+typedef struct
+{
+    Elf32_Word          p_type;             //!< This member tells what kind of segment this array element describes or how to interpret the array element's information
+    Elf32_Off           p_offset;           //!< This member gives the offset from the beginning of the file at which the first byte of the segment resides
+    Elf32_Addr          p_vaddr;            //!< This member gives the virtual address at which the first byte of the segment resides in memory
+    Elf32_Addr          p_paddr;            //!< On systems for which physical addressing is relevant, this member is reserved for the segment's physical address
+    Elf32_Word          p_filesz;           //!< This member gives the number of bytes in the file image of the segment; it may be zero
+    Elf32_Word          p_memsz;            //!< This member gives the number of bytes in the memory image of the segment; it may be zero
+    Elf32_Word          p_flags;            //!< This member gives flags relevant to the segment
+    Elf32_Word          p_align;            //!< As ``Program Loading'' describes in this chapter of the processor supplement, loadable process segments must have congruent values for p_vaddr and p_offset, modulo the page size
+} Elf32_Phdr;       //!< 32bits Program header
+
+typedef struct
+{
+    Elf64_Word          p_type;             //!< This member tells what kind of segment this array element describes or how to interpret the array element's information
+    Elf64_Word          p_flags;            //!< This member gives flags relevant to the segment
+    Elf64_Off           p_offset;           //!< This member gives the offset from the beginning of the file at which the first byte of the segment resides
+    Elf64_Addr          p_vaddr;            //!< This member gives the virtual address at which the first byte of the segment resides in memory
+    Elf64_Addr          p_paddr;            //!< On systems for which physical addressing is relevant, this member is reserved for the segment's physical address
+    Elf64_Xword         p_filesz;           //!< This member gives the number of bytes in the file image of the segment; it may be zero
+    Elf64_Xword         p_memsz;            //!< This member gives the number of bytes in the memory image of the segment; it may be zero
+    Elf64_Xword         p_align;            //!< As ``Program Loading'' describes in this chapter of the processor supplement, loadable process segments must have congruent values for p_vaddr and p_offset, modulo the page size
+} Elf64_Phdr;       //!< 64bits Program header
+
+/*
+ * p_type
+ */
+#define PT_NULL                 0                   //!< The array element is unused; other members' values are undefined
+#define PT_LOAD                 1                   //!< The array element specifies a loadable segment, described by p_filesz and p_memsz
+#define PT_DYNAMIC              2                   //!< The array element specifies dynamic linking information
+#define PT_INTERP               3                   //!< The array element specifies the location and size of a null-terminated path name to invoke as an interpreter
+#define PT_NOTE                 4                   //!< The array element specifies the location and size of auxiliary information
+#define PT_SHLIB                5                   //!< This segment type is reserved but has unspecified semantics
+#define PT_PHDR                 6                   //!< The array element, if present, specifies the location and size of the program header table itself, both in the file and in the memory image of the program
+#define PT_TLS                  7                   //!< The array element specifies the Thread-Local Storage template
+#define PT_LOOS                 0x60000000          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define PT_HIOS                 0x6fffffff          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define PT_LOPROC               0x70000000          //!< Values in this inclusive range are reserved for processor-specific semantics
+#define PT_HIPROC               0x7fffffff          //!< Values in this inclusive range are reserved for processor-specific semantics
+
+/*
+ * p_flags
+ */
+#define PF_X                    (1 << 0)            //!< Execute
+#define PF_W                    (1 << 1)            //!< Write
+#define PF_R                    (1 << 2)            //!< Read
+#define PF_MASKOS               0x0ff00000          //!< Unspecified
+#define PF_MASKPROC             0xf0000000          //!< Unspecified
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/elfapi.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/elfapi.h
new file mode 100644
index 00000000000..564086db26c
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/elfapi.h
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf loder internal methods.
+ *
+ * \defgroup ELFLOADER MMDSP ELF loader.
+ */
+#ifndef __INC_CM_ELFLOADER_H
+#define __INC_CM_ELFLOADER_H
+
+#include <cm/engine/elf/inc/common.h>
+
+/*!
+ * \internal
+ * \brief ELF Parsing & checking
+ * \ingroup ELFLOADER
+ */
+t_cm_error cm_ELF_CheckFile(
+        const char              *elfdata,
+        t_bool                  temporaryDescription,
+        t_elfdescription        **elfhandlePtr);
+
+void cm_ELF_ReleaseDescription(
+        t_uint32 requireNumber, t_interface_require *requires,
+        t_uint32 attributeNumber, t_attribute *attributes,
+        t_uint32 propertyNumber, t_property *properties,
+        t_uint32 provideNumber, t_interface_provide *provides);
+
+/*!
+ * \internal
+ * \brief ELF closing
+ * \ingroup ELFLOADER
+ */
+void cm_ELF_CloseFile(
+        t_bool                  temporaryDescription,
+        t_elfdescription        *elfhandle);
+
+/*!
+ * \internal
+ * \brief Load a component template shared memories.
+ *
+ * \note In case of error, part of memory could have been allocated and must be free by calling cm_DSPABI_FreeTemplate.
+ */
+t_cm_error cm_ELF_LoadTemplate(
+        t_cm_domain_id          domainId,
+        t_elfdescription        *elfhandle,
+        t_memory_handle         sharedMemories[NUMBER_OF_MMDSP_MEMORY]);
+
+/*!
+ * \internal
+ * \brief Clean cache memory of a component template shared code.
+ */
+void cm_ELF_FlushTemplate(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY]);
+
+void cm_ELF_FlushInstance(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle privateMemories[NUMBER_OF_MMDSP_MEMORY]);
+
+/*!
+ * \internal
+ * \brief Load a component instance private memories.
+ *
+ * \note In case of error, part of memory could have been allocated and must be free by calling cm_DSPABI_FreeInstance.
+ */
+t_cm_error cm_ELF_LoadInstance(
+        t_cm_domain_id          domainId,
+        t_elfdescription        *elfhandle,
+        t_memory_handle         sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle         privateMemories[NUMBER_OF_MMDSP_MEMORY]);
+
+void cm_ELF_FreeInstance(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle privateMemories[NUMBER_OF_MMDSP_MEMORY]);
+void cm_ELF_FreeTemplate(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY]);
+
+
+t_cm_error cm_ELF_relocateSharedSegments(
+        t_memory_handle *memories,
+        t_elfdescription *elfhandle,
+        void                        *cbContext);
+t_cm_error cm_ELF_relocatePrivateSegments(
+        t_memory_handle *memories,
+        t_elfdescription *elfhandle,
+        void                        *cbContext);
+void cm_ELF_performRelocation(
+        t_uint32                    type,
+        const char                  *symbol_name,
+        t_uint32                    symbol_addr,
+        char                        *reloc_addr);
+t_cm_error cm_ELF_GetMemory(
+        t_elfdescription            *elf,
+        t_tmp_elfdescription        *elftmp,
+        t_uint32                    address,
+        t_memory_purpose            purpose,
+        t_memory_reference          *memory);
+
+
+#include <cm/engine/component/inc/component_type.h>
+
+t_cm_error cm_DSPABI_AddLoadMap(
+        t_cm_domain_id domainId,
+        const char* templateName,
+        const char* localname,
+        t_memory_handle *memories,
+        void *componentHandle);
+t_cm_error cm_DSPABI_RemoveLoadMap(
+        t_cm_domain_id domainId,
+        const char* templateName,
+        t_memory_handle *memories,
+        const char* localname,
+        void *componentHandle);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/memory.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/memory.h
new file mode 100644
index 00000000000..9eab94f173c
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/memory.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf memory.
+ */
+#ifndef __INC_CM_ELF_MEMORY_H
+#define __INC_CM_ELF_MEMORY_H
+
+#include <cm/engine/dsp/inc/dsp.h>
+
+/**
+ * \brief Memory identifier
+ */
+typedef t_uint8 t_memory_id;
+
+/**
+ * \brief Memory property
+ */
+typedef enum {
+    MEM_FOR_MULTIINSTANCE,
+    MEM_FOR_SINGLETON,
+    MEM_FOR_LAST
+} t_instance_property;
+
+/**
+ * \brief Memory prupose (for processor with different address space for code and data/
+ */
+typedef enum {
+    MEM_CODE,
+    MEM_DATA
+} t_memory_purpose;
+
+/**
+ * \brief Memory property
+ */
+typedef enum {
+    MEM_PRIVATE,
+    MEM_SHARABLE,
+} t_memory_property;
+
+/**
+ * \brief Elf memory mapping description
+ */
+typedef struct
+{
+    t_memory_id             id;
+    t_dsp_memory_type_id    dspMemType;
+    t_uint32                startAddr;
+    t_cm_memory_alignment   memAlignement;
+    t_memory_property       property;
+    t_memory_purpose        purpose;
+    t_uint8                 fileEntSize;
+    t_uint8                 memEntSize;
+    char*                   memoryName;
+} t_elfmemory;
+
+#define NUMBER_OF_MMDSP_MEMORY  15
+
+/*
+ * \brief Elf segment description
+ */
+typedef struct {
+    // Data in Bytes
+    t_uint32                sumSize;
+    t_bool                  sumSizeSetted;
+    t_cm_logical_address    hostAddr;   // Valid only if section Load in memory
+    t_uint32                maxAlign;
+    // Data in word
+    t_uint32                mpcAddr;    // Valid only if section Load in memory
+} t_elfSegment;
+
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp-loadmap.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp-loadmap.h
new file mode 100644
index 00000000000..78b00388802
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp-loadmap.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf writer internal methods.
+ *
+ * \defgroup LOADMAP MMDSP ELF writer (a linker in fact).
+ */
+#ifndef __INC_CM_LOADMAP_H
+#define __INC_CM_LOADMAP_H
+
+#include <cm/inc/cm_type.h>
+
+/*
+ * Align with loadmap :
+ * https://codex.cro.st.com/wiki/index.php?pagename=Specification%2FLoadmap%2Fv1.2&group_id=310
+ */
+#define LOADMAP_MAGIC_NUMBER 0xFBBF
+
+#define LOADMAP_VERSION_MSB 1
+#define LOADMAP_VERSION_LSB 2
+
+struct LoadMapItem
+{
+  const char*  pSolibFilename;  // Filename of shared library object
+  void*        pAddrProg;       // Load address of program section
+  void*        pAddrEmbProg;    // Load address of embedded program section
+  void*        pThis;           // Data base address of component instance
+  void*        pARMThis;        // ARM component debug ID
+  const char*  pComponentName;  // Pretty name of the component instance, NULL if none.
+  struct LoadMapItem* pNextItem;// Pointer on the next list item, NULL if last one.
+  void*        pXROM;           // Start address of XROM
+  void*        pYROM;           // Start address of YROM
+
+  void*        memHandle;       // handle of allocated memory for this structure and name, local field not use by mmdsp
+};
+
+struct LoadMapHdr
+{
+  t_uint16      nMagicNumber;    // Equal to 0xFBBF.
+  t_uint16      nVersion;        // The version of the load map format.
+  t_uint32      nRevision;       // A counter incremented at each load map list modification.
+  struct LoadMapItem* pFirstItem;// Pointer on the first item, NULL if no shared library loaded.
+};
+
+#endif /* __INC_CM_LOADMAP_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp.h
new file mode 100644
index 00000000000..1662def6c1a
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief MMDSP elf.
+ */
+#ifndef __INC_CM_ELF_MMDSP_H
+#define __INC_CM_ELF_MMDSP_H
+
+#include <cm/engine/elf/inc/common.h>
+
+#define CODE_MEMORY_INDEX           0
+#define ECODE_MEMORY_INDEX          7
+
+#define XROM_MEMORY_INDEX           1
+#define YROM_MEMORY_INDEX           2
+#define PRIVATE_DATA_MEMORY_INDEX   8
+#define SHARE_DATA_MEMORY_INDEX     1
+
+/*
+ * Relocation
+ */
+#define R_MMDSP_IMM16       5
+#define R_MMDSP_IMM20_16    6
+#define R_MMDSP_IMM20_4     7
+#define R_MMDSP_24          13
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/mpcal.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/mpcal.h
new file mode 100644
index 00000000000..718b7f61ceb
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/mpcal.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief MPC Abraction Layer.
+ *
+ * \defgroup MPCAL MPC Abraction Layer.
+ */
+#ifndef __INC_CM_DSP_MPCAL_H
+#define __INC_CM_DSP_MPCAL_H
+
+#include <cm/inc/cm_type.h>
+#include <share/inc/nmf.h>
+
+#include <cm/engine/elf/inc/common.h>
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/reloc.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/reloc.h
new file mode 100644
index 00000000000..b38be48d689
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/reloc.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf relocation.
+ */
+#ifndef __INC_CM_ELF_RELOC_H
+#define __INC_CM_ELF_RELOC_H
+
+
+void MMDSP_performRelocation(
+        t_uint32 type,
+        const char* symbol_name,
+        t_uint32 symbol_addr,
+        char* reloc_addr,
+        const char* inPlaceAddr,
+        t_uint32 reloc_offset);
+
+/*
+ *
+ * Return:
+ * 0x0 returned if symbol not found
+ * 0xFFFFFFFE returned if out of memory
+ * 0xFFFFFFFF returned if symbol found in static required binding
+ */
+typedef t_uint32 (*CBresolvSymbol)(
+        void* context,
+        t_uint32 type,
+        const char* symbolName,
+        char* reloc_addr);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/elf64.c b/drivers/staging/nmf-cm/cm/engine/elf/src/elf64.c
new file mode 100644
index 00000000000..2e0f5928ffd
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/elf64.c
@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/elf/inc/common.h>
+#include <cm/engine/elf/inc/elfabi.h>
+
+#include <cm/engine/utils/inc/swap.h>
+#include <cm/engine/trace/inc/trace.h>
+
+typedef Elf64_Half ElfXX_Half;
+typedef Elf64_Word ElfXX_Word;
+typedef Elf64_Addr ElfXX_Addr;
+typedef Elf64_Off ElfXX_Off;
+
+typedef Elf64_Xword ElfXX_Xword;
+
+typedef Elf64_Ehdr ElfXX_Ehdr;
+typedef Elf64_Shdr ElfXX_Shdr;
+typedef Elf64_Sym ElfXX_Sym;
+typedef Elf64_Rela ElfXX_Rela;
+
+#undef ELFXX_R_SYM
+#define ELFXX_R_SYM ELF64_R_SYM
+#undef ELFXX_R_TYPE
+#define ELFXX_R_TYPE ELF64_R_TYPE
+#undef ELFXX_R_INFO
+#define ELFXX_R_INFO ELF64_R_INFO
+
+// TODO Here we assume big endian (MMDSP !)
+static Elf64_Half swapHalf(Elf64_Half half)
+{
+    return (Elf64_Half)swap16(half);
+}
+
+static Elf64_Word swapWord(Elf64_Word word)
+{
+    return (Elf64_Word)swap32(word);
+}
+
+static Elf64_Xword swapXword(Elf64_Xword xword)
+{
+    return (Elf64_Xword)swap64(xword);
+}
+
+#include "elfxx.c"
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/elfload.c b/drivers/staging/nmf-cm/cm/engine/elf/src/elfload.c
new file mode 100644
index 00000000000..2efe6edbf69
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/elfload.c
@@ -0,0 +1,749 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/elf/inc/elfapi.h>
+#include <cm/engine/elf/inc/mpcal.h>
+#include <cm/inc/cm_def.h>
+
+//#include <cm/engine/component/inc/introspection.h>
+
+#include <cm/engine/utils/inc/mem.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/swap.h>
+#include <cm/engine/utils/inc/string.h>
+
+static void* getElfHeaderReference(t_tmp_elfdescription *elftmp, void* hdrref)
+{
+    if(hdrref != NULL)
+        return (void*)((int)swap32((t_uint32)hdrref) + (int)elftmp->elfheader);
+    else
+        return NULL;
+}
+
+static t_dup_char copyElfString(t_tmp_elfdescription *elftmp, char* idx)
+{
+    return cm_StringDuplicate((char*)getElfHeaderReference(elftmp, (void*)idx));
+}
+
+static t_cm_error getMemoryOffset(
+        t_elfdescription            *elfhandle,
+        t_tmp_elfdescription        *elftmp,
+        t_memory_purpose            purpose,
+        const t_uint32              *addressInNmf,
+        t_memory_reference          *memory) {
+
+    if(elftmp->isExecutable) {
+        return cm_ELF_GetMemory(elfhandle, elftmp,
+                swap32(*addressInNmf),
+                purpose,
+                memory);
+    } else {
+        return ELF64_getRelocationMemory(elfhandle, elftmp,
+                (t_uint32)addressInNmf - (t_uint32)elftmp->elfheader,
+                memory);
+    }
+}
+
+static t_cm_error getAdressForExecutableOffsetElsewhere(
+        t_elfdescription                *elfhandle,
+        t_tmp_elfdescription            *elftmp,
+        const t_uint32                  *addressInNmf,
+        t_memory_reference              *memory) {
+    t_uint32 address;
+
+    address = swap32(*addressInNmf);
+    if(address == 0xFFFFFFFF)
+    {
+        memory->offset = 0x0;
+        memory->memory = NULL;
+        return CM_OK;
+    }
+
+    if(elftmp->isExecutable)
+    {
+        memory->offset = address;
+        memory->memory = NULL;
+        return CM_OK;
+    }
+
+    // Error log in elfhandle by previous call will be check in loadTemplate
+    return ELF64_getRelocationMemory(elfhandle, elftmp,
+            (t_uint32)addressInNmf - (t_uint32)elftmp->elfheader,
+            memory);
+}
+
+/*
+ * Interface Management
+ */
+static t_interface_description* interfaceList = NULL;
+
+static t_interface_description* getInterfaceDescription(t_tmp_elfdescription *elftmp, t_elf_interface_description* elfitf) {
+    t_dup_char itfType;
+    t_interface_description* itf;
+    int i;
+
+    itfType = copyElfString(elftmp, elfitf->type);
+    if(itfType == NULL)
+        return NULL;
+
+    // Search if interfane already loaded
+    for(itf = interfaceList; itf != NULL; itf = itf->next) {
+        if(itf->type == itfType) {
+            // TODO Sanity check
+
+            itf->referenceCounter++;
+            cm_StringRelease(itfType);
+            return itf;
+        }
+    }
+
+    // Create a new interface if not exists
+    itf = (t_interface_description*)OSAL_Alloc_Zero(sizeof(t_interface_description) + sizeof(char*) * (elfitf->methodNumber /*- 1*/));
+    if(itf == NULL)
+        goto out_itf_type;
+    itf->referenceCounter = 1;
+    itf->type = itfType;
+    itf->methodNumber = elfitf->methodNumber;
+    for(i = 0; i < itf->methodNumber; i++) {
+        itf->methodNames[i] = copyElfString(elftmp, elfitf->methodNames[i]);
+        if(itf->methodNames[i] == NULL)
+            goto out_method;
+    }
+
+    // Put it in Top
+    itf->next = interfaceList;
+    interfaceList = itf;
+
+    return itf;
+
+out_method:
+    for(i = 0; i < itf->methodNumber; i++)
+        cm_StringRelease(itf->methodNames[i]);
+    OSAL_Free(itf);
+out_itf_type:
+    cm_StringRelease(itfType);
+    return NULL;
+}
+
+static void releaseInterfaceDescription(t_interface_description* itf) {
+    if(itf == NULL)
+        return;
+
+    if(--itf->referenceCounter == 0) {
+        int i;
+
+        // Remove it from list
+        if(interfaceList == itf) {
+            interfaceList = interfaceList->next;
+        } else {
+            t_interface_description* prev = interfaceList;
+            while(prev->next != itf)
+                prev = prev->next;
+            prev->next = itf->next;
+        }
+
+        // Destroy interface description
+        for(i = 0; i < itf->methodNumber; i++) {
+            cm_StringRelease(itf->methodNames[i]);
+        }
+        cm_StringRelease(itf->type);
+        OSAL_Free(itf);
+    }
+}
+
+
+t_cm_error cm_ELF_CheckFile(
+        const char              *elfdata,
+        t_bool                  temporaryDescription,
+        t_elfdescription        **elfhandlePtr)
+{
+    t_elfdescription        *elfhandle;
+    t_tmp_elfdescription    elftmp;
+    t_cm_error error;
+    t_uint32 version;
+    t_uint32 compatibleVersion;
+    int i, j, k;
+
+    /*
+     * Sanity check
+     */
+    if (elfdata[EI_MAG0] != ELFMAG0 ||
+            elfdata[EI_MAG1] != ELFMAG1 ||
+            elfdata[EI_MAG2] != ELFMAG2 ||
+            elfdata[EI_MAG3] != ELFMAG3 ||
+            elfdata[EI_CLASS] != ELFCLASS64)
+    {
+        ERROR("CM_INVALID_ELF_FILE: component file is not a MMDSP ELF file\n", 0, 0, 0, 0, 0, 0);
+        return CM_INVALID_ELF_FILE;
+    }
+
+    /*
+     * Create elf data
+     */
+    if((error = ELF64_LoadComponent(EM_MMDSP_PLUS, elfdata, elfhandlePtr, &elftmp)) != CM_OK)
+        return error;
+
+    elfhandle = *elfhandlePtr;
+
+    elfhandle->temporaryDescription = temporaryDescription;
+
+    version = swap32(elftmp.elfheader->nmfVersion);
+
+    compatibleVersion = (VERSION_MAJOR(version) == VERSION_MAJOR(NMF_VERSION));
+    if(compatibleVersion)
+    {
+        switch(VERSION_MINOR(NMF_VERSION))
+        {
+        case 10: // Compatible with 2.9, 2.10
+            compatibleVersion =
+                    (VERSION_MINOR(version) == 9) ||
+                    (VERSION_MINOR(version) == 10);
+            break;
+        default: // Strict compatibility 2.x == 2.x
+            compatibleVersion = (VERSION_MINOR(version) == VERSION_MINOR(NMF_VERSION));
+        }
+    }
+
+    if(! compatibleVersion)
+    {
+        ERROR("CM_INVALID_ELF_FILE: incompatible version for Component %d.%d.x != CM:%d.%d.x\n",
+                VERSION_MAJOR(version), VERSION_MINOR(version),
+                VERSION_MAJOR(NMF_VERSION), VERSION_MINOR(NMF_VERSION), 0, 0);
+        error = CM_INVALID_ELF_FILE;
+        goto onerror;
+    }
+
+
+    /*
+     * Commented since to many noise !!!!
+    if(VERSION_PATCH(version) != VERSION_PATCH(NMF_VERSION))
+    {
+        WARNING("CM_INVALID_ELF_FILE: incompatible version, Component:%d.%d.%d != CM:%d.%d.%d\n",
+                VERSION_MAJOR(version), VERSION_MINOR(version), VERSION_PATCH(version),
+                VERSION_MAJOR(NMF_VERSION), VERSION_MINOR(NMF_VERSION), VERSION_PATCH(NMF_VERSION));
+    }
+     */
+
+    if((error = ELF64_ComputeSegment(elfhandle, &elftmp)) != CM_OK)
+        goto onerror;
+
+    //
+    elfhandle->foundedTemplateName =  copyElfString(&elftmp, elftmp.elfheader->templateName);
+    if(elfhandle->foundedTemplateName == NULL)
+        goto oom;
+    elfhandle->minStackSize = swap32(elftmp.elfheader->minStackSize);
+
+    // Get Life-cycle memory
+    if((error = getAdressForExecutableOffsetElsewhere(elfhandle, &elftmp, &elftmp.elfheader->LCCConstruct, &elfhandle->memoryForConstruct)) != CM_OK)
+        goto onerror;
+    if((error = getAdressForExecutableOffsetElsewhere(elfhandle, &elftmp, &elftmp.elfheader->LCCStart, &elfhandle->memoryForStart)) != CM_OK)
+        goto onerror;
+    if((error = getAdressForExecutableOffsetElsewhere(elfhandle, &elftmp, &elftmp.elfheader->LCCStop, &elfhandle->memoryForStop)) != CM_OK)
+        goto onerror;
+    if((error = getAdressForExecutableOffsetElsewhere(elfhandle, &elftmp, &elftmp.elfheader->LCCDestroy, &elfhandle->memoryForDestroy)) != CM_OK)
+        goto onerror;
+
+    // Copy attributes information
+    elfhandle->attributeNumber = swap32(elftmp.elfheader->attributeNumber);
+    if(elfhandle->attributeNumber > 0)
+    {
+        elfhandle->attributes =
+                (t_attribute*)OSAL_Alloc_Zero(sizeof(t_attribute) * elfhandle->attributeNumber);
+        if(elfhandle->attributes == NULL)
+            goto oom;
+
+        if(elfhandle->attributeNumber >  0)
+        {
+            t_elf_attribute *attributes = (t_elf_attribute*)getElfHeaderReference(&elftmp, (void*)elftmp.elfheader->attributes);
+
+            for(i = 0; i < elfhandle->attributeNumber; i++)
+            {
+                elfhandle->attributes[i].name = copyElfString(&elftmp, attributes[i].name);
+                if(elfhandle->attributes[i].name == NULL)
+                    goto oom;
+
+                if((error = getMemoryOffset(elfhandle, &elftmp,
+                        MEM_DATA,
+                        &attributes[i].symbols,
+                        &elfhandle->attributes[i].memory)) != CM_OK)
+                        goto onerror;
+                LOG_INTERNAL(2, "  attribute %s mem=%s offset=%x\n",
+                        elfhandle->attributes[i].name,
+                        elfhandle->attributes[i].memory.memory->memoryName,
+                        elfhandle->attributes[i].memory.offset,
+                        0, 0, 0);
+            }
+        }
+    }
+
+    // Copy properties information
+    elfhandle->propertyNumber = swap32(elftmp.elfheader->propertyNumber);
+    if(elfhandle->propertyNumber > 0)
+    {
+        elfhandle->properties =
+                (t_property*)OSAL_Alloc_Zero(sizeof(t_property) * elfhandle->propertyNumber);
+        if(elfhandle->properties == NULL)
+            goto oom;
+
+        if(elfhandle->propertyNumber > 0)
+        {
+            t_elf_property *properties = (t_elf_property*)getElfHeaderReference(&elftmp, (void*)elftmp.elfheader->properties);
+
+            for(i = 0; i < elfhandle->propertyNumber; i++)
+            {
+                elfhandle->properties[i].name = copyElfString(&elftmp, properties[i].name);
+                if(elfhandle->properties[i].name == NULL)
+                    goto oom;
+
+                elfhandle->properties[i].value = copyElfString(&elftmp, properties[i].value);
+                if(elfhandle->properties[i].value == NULL)
+                    goto oom;
+
+                LOG_INTERNAL(3, "  property %s = %s\n",
+                        elfhandle->properties[i].name,
+                        elfhandle->properties[i].value,
+                        0, 0, 0, 0);
+            }
+        }
+    }
+
+    // Copy requires information
+    elfhandle->requireNumber = swap32(elftmp.elfheader->requireNumber);
+    if(elfhandle->requireNumber > 0)
+    {
+        char *ref = getElfHeaderReference(&elftmp, (void*)elftmp.elfheader->requires);
+
+        elfhandle->requires = (t_interface_require*)OSAL_Alloc_Zero(sizeof(t_interface_require) * elfhandle->requireNumber);
+        if(elfhandle->requires == NULL)
+            goto oom;
+
+        for(i = 0; i < elfhandle->requireNumber; i++)
+        {
+            t_elf_required_interface *require = (t_elf_required_interface*)ref;
+            t_elf_interface_description *interface = (t_elf_interface_description*)getElfHeaderReference(&elftmp, (void*)require->interface);
+
+            elfhandle->requires[i].name = copyElfString(&elftmp, require->name);
+            if(elfhandle->requires[i].name == NULL)
+                goto oom;
+
+            elfhandle->requires[i].requireTypes = require->requireTypes;
+            elfhandle->requires[i].collectionSize = require->collectionSize;
+            elfhandle->requires[i].interface = getInterfaceDescription(&elftmp, interface);
+            if(elfhandle->requires[i].interface == NULL)
+                goto oom;
+
+            LOG_INTERNAL(2, "  require %s <%s> %x\n",
+                    elfhandle->requires[i].name,
+                    elfhandle->requires[i].interface->type,
+                    elfhandle->requires[i].requireTypes, 0, 0, 0);
+            CM_ASSERT(elfhandle->requires[i].collectionSize != 0);
+
+            ref += sizeof(t_elf_required_interface);
+
+            if((elfhandle->requires[i].requireTypes & VIRTUAL_REQUIRE) == 0 &&
+                    (elfhandle->requires[i].requireTypes & STATIC_REQUIRE) == 0)
+            {
+                elfhandle->requires[i].indexes =
+                        (t_interface_require_index*)OSAL_Alloc_Zero(sizeof(t_interface_require_index) * elfhandle->requires[i].collectionSize);
+                if(elfhandle->requires[i].indexes == NULL)
+                    goto oom;
+
+                for(j = 0; j < elfhandle->requires[i].collectionSize; j++)
+                {
+                    t_elf_interface_require_index* index = (t_elf_interface_require_index*)ref;
+
+                    elfhandle->requires[i].indexes[j].numberOfClient = swap32(index->numberOfClient);
+                    if(elfhandle->requires[i].indexes[j].numberOfClient != 0)
+                    {
+                        elfhandle->requires[i].indexes[j].memories =
+                                (t_memory_reference*)OSAL_Alloc(sizeof(t_memory_reference) *  elfhandle->requires[i].indexes[j].numberOfClient);
+                        if(elfhandle->requires[i].indexes[j].memories == NULL)
+                            goto oom;
+
+                        for(k = 0; k < elfhandle->requires[i].indexes[j].numberOfClient; k++) {
+                            if((error = getMemoryOffset(elfhandle,&elftmp,
+                                    MEM_DATA,
+                                    &index->symbols[k],
+                                    &elfhandle->requires[i].indexes[j].memories[k])) != CM_OK)
+                                goto onerror;
+                            LOG_INTERNAL(2, "    [%d, %d] mem=%s offset=%x\n",
+                                    j, k,
+                                    elfhandle->requires[i].indexes[j].memories[k].memory->memoryName,
+                                    elfhandle->requires[i].indexes[j].memories[k].offset,
+                                    0, 0);
+                        }
+                    }
+
+                    ref += sizeof(index->numberOfClient) + elfhandle->requires[i].indexes[j].numberOfClient * sizeof(index->symbols[0]);
+                }
+            }
+        }
+    }
+
+    // Copy provides informations
+    elfhandle->provideNumber = swap32(elftmp.elfheader->provideNumber);
+    if(elfhandle->provideNumber != 0)
+    {
+        elfhandle->provides =
+                (t_interface_provide*)OSAL_Alloc_Zero(sizeof(t_interface_provide) * elfhandle->provideNumber);
+        if(elfhandle->provides == NULL)
+            goto oom;
+
+        if(elfhandle->provideNumber > 0)
+        {
+            char *ref = getElfHeaderReference(&elftmp, (void*)elftmp.elfheader->provides);
+
+            for(i = 0; i < elfhandle->provideNumber; i++)
+            {
+                t_elf_provided_interface *provide = (t_elf_provided_interface*)ref;
+                t_elf_interface_description *interface = (t_elf_interface_description*)getElfHeaderReference(&elftmp, (void*)provide->interface);
+
+                elfhandle->provides[i].name = copyElfString(&elftmp, provide->name);
+                if(elfhandle->provides[i].name == NULL)
+                    goto oom;
+
+                elfhandle->provides[i].provideTypes = provide->provideTypes;
+                elfhandle->provides[i].interruptLine = provide->interruptLine;
+                elfhandle->provides[i].collectionSize = provide->collectionSize;
+                elfhandle->provides[i].interface = getInterfaceDescription(&elftmp, interface);
+                if(elfhandle->provides[i].interface == NULL)
+                    goto oom;
+
+                LOG_INTERNAL(2, "  provide %s <%s>\n",
+                        elfhandle->provides[i].name,
+                        elfhandle->provides[i].interface->type,
+                        0,0, 0, 0);
+                CM_ASSERT(elfhandle->provides[i].collectionSize != 0);
+
+                ref += sizeof(t_elf_required_interface);
+
+                {
+                    t_uint32 *methodSymbols = (t_uint32*)ref;
+
+                    elfhandle->provides[i].indexes = (t_interface_provide_index**)OSAL_Alloc_Zero(
+                            sizeof(t_interface_provide_index*) * elfhandle->provides[i].collectionSize);
+                    if(elfhandle->provides[i].indexes == NULL)
+                        goto oom;
+
+                    if(elfhandle->provides[i].interface->methodNumber != 0)
+                    {
+                        for(j = 0; j < elfhandle->provides[i].collectionSize; j++)
+                        {
+                            elfhandle->provides[i].indexes[j] = (t_interface_provide_index*)OSAL_Alloc(
+                                    sizeof(t_interface_provide_index) * elfhandle->provides[i].interface->methodNumber);
+                            if(elfhandle->provides[i].indexes[j] == NULL)
+                                goto oom;
+
+                            for(k = 0; k < elfhandle->provides[i].interface->methodNumber; k++)
+                            {
+                                if((error = getAdressForExecutableOffsetElsewhere(elfhandle, &elftmp,
+                                        methodSymbols++,
+                                        &elfhandle->provides[i].indexes[j][k].memory)) != CM_OK)
+                                        goto onerror;
+
+                                if(elfhandle->provides[i].indexes[j][k].memory.memory != NULL)
+                                    LOG_INTERNAL(2, "    [%d, %d] method '%s' mem=%s offset=%x\n",
+                                        j, k,
+                                        elfhandle->provides[i].interface->methodNames[k],
+                                        elfhandle->provides[i].indexes[j][k].memory.memory->memoryName,
+                                        elfhandle->provides[i].indexes[j][k].memory.offset,
+                                        0);
+                                else
+                                    LOG_INTERNAL(2, "    [%d, %d] method '%s' address=%x\n",
+                                        j, k,
+                                        elfhandle->provides[i].interface->methodNames[k],
+                                        elfhandle->provides[i].indexes[j][k].memory.offset,
+                                        0, 0);
+                            }
+                        }
+                    }
+
+                    ref += elfhandle->provides[i].collectionSize * elfhandle->provides[i].interface->methodNumber * sizeof(methodSymbols[0]);
+                }
+            }
+        }
+    }
+
+    return CM_OK;
+
+oom:
+    error = CM_NO_MORE_MEMORY;
+onerror:
+    cm_ELF_CloseFile(temporaryDescription, elfhandle);
+    *elfhandlePtr = NULL;
+    return error;
+}
+
+void cm_ELF_ReleaseDescription(
+        t_uint32 requireNumber, t_interface_require *requires,
+        t_uint32 attributeNumber, t_attribute *attributes,
+        t_uint32 propertyNumber, t_property *properties,
+        t_uint32 provideNumber, t_interface_provide *provides)
+{
+    int i, j;
+
+    // Free provides (Number set when array allocated)
+    if(provides != NULL)
+    {
+        for(i = 0; i < provideNumber; i++)
+        {
+            if(provides[i].indexes != NULL)
+            {
+                for(j = 0; j < provides[i].collectionSize; j++)
+                {
+                    OSAL_Free(provides[i].indexes[j]);
+                }
+                OSAL_Free(provides[i].indexes);
+            }
+            releaseInterfaceDescription(provides[i].interface);
+            cm_StringRelease(provides[i].name);
+        }
+        OSAL_Free(provides);
+    }
+
+    // Free requires (Number set when array allocated)
+    if(requires != NULL)
+    {
+        for(i = 0; i < requireNumber; i++)
+        {
+            if(requires[i].indexes != 0)
+            {
+                for(j = 0; j < requires[i].collectionSize; j++)
+                {
+                    OSAL_Free(requires[i].indexes[j].memories);
+                }
+                OSAL_Free(requires[i].indexes);
+            }
+            releaseInterfaceDescription(requires[i].interface);
+            cm_StringRelease(requires[i].name);
+        }
+        OSAL_Free(requires);
+    }
+
+    // Free properties (Number set when array allocated)
+    if(properties != NULL)
+    {
+        for(i = 0; i < propertyNumber; i++)
+        {
+            cm_StringRelease(properties[i].value);
+            cm_StringRelease(properties[i].name);
+        }
+        OSAL_Free(properties);
+    }
+
+    // Free Attributes (Number set when array allocated)
+    if(attributes != NULL)
+    {
+        for(i = 0; i < attributeNumber; i++)
+        {
+            cm_StringRelease(attributes[i].name);
+        }
+        OSAL_Free(attributes);
+    }
+}
+
+void cm_ELF_CloseFile(
+        t_bool                  temporaryDescription,
+        t_elfdescription        *elfhandle)
+{
+    if(elfhandle == NULL)
+        return;
+
+    if(temporaryDescription && ! elfhandle->temporaryDescription)
+        return;
+
+    // Release description if not moved to template
+    cm_ELF_ReleaseDescription(
+            elfhandle->requireNumber, elfhandle->requires,
+            elfhandle->attributeNumber, elfhandle->attributes,
+            elfhandle->propertyNumber, elfhandle->properties,
+            elfhandle->provideNumber, elfhandle->provides);
+
+    cm_StringRelease(elfhandle->foundedTemplateName);
+
+    ELF64_UnloadComponent(elfhandle);
+}
+
+
+static t_cm_error allocSegment(
+        t_cm_domain_id          domainId,
+        t_elfdescription        *elfhandle,
+        t_memory_handle         memories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_property       property) {
+    t_memory_id memId;
+    const t_elfmemory           *thisMemory;            //!< Memory used to determine this
+    const t_elfmemory           *codeMemory;            //!< Memory used to determine code
+
+    MMDSP_serializeMemories(elfhandle->instanceProperty, &codeMemory, &thisMemory);
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        const t_elfmemory* mapping;
+
+        if(elfhandle->segments[memId].sumSize == 0x0)
+            continue;
+
+        mapping = MMDSP_getMappingById(memId);
+
+        if(
+                (mapping->property == property && elfhandle->instanceProperty != MEM_FOR_SINGLETON) ||
+                (property == MEM_SHARABLE && elfhandle->instanceProperty == MEM_FOR_SINGLETON) )
+        {
+            // Allocate segment
+            memories[memId] = cm_DM_Alloc(domainId, mapping->dspMemType,
+                    elfhandle->segments[memId].sumSize / mapping->fileEntSize,
+                    mapping->memAlignement, TRUE);
+
+            if(memories[memId] == INVALID_MEMORY_HANDLE)
+            {
+                ERROR("CM_NO_MORE_MEMORY(%s): %x too big\n", mapping->memoryName, elfhandle->segments[memId].sumSize / mapping->fileEntSize, 0, 0, 0, 0);
+                return CM_NO_MORE_MEMORY;
+            }
+
+            // Get reference in memory
+            elfhandle->segments[memId].hostAddr = cm_DSP_GetHostLogicalAddress(memories[memId]);
+
+            cm_DSP_GetDspAddress(memories[memId], &elfhandle->segments[memId].mpcAddr);
+
+            // Log it
+            LOG_INTERNAL(1, "\t%s%s: 0x%x..+0x%x (0x%x)\n",
+                    mapping->memoryName,
+                    (thisMemory == mapping) ? "(THIS)" : "",
+                    elfhandle->segments[memId].mpcAddr,
+                    elfhandle->segments[memId].sumSize / mapping->fileEntSize,
+                    elfhandle->segments[memId].hostAddr, 0);
+        }
+        else if(property == MEM_PRIVATE) // Since we allocate private segment, if not allocate, it's a share one
+        {
+            // In order to allow further relocation based on cached address like mpcAddr & hostAddr,
+            // initialize them also !
+
+            // Get reference in memory
+            elfhandle->segments[memId].hostAddr = cm_DSP_GetHostLogicalAddress(memories[memId]);
+
+            cm_DSP_GetDspAddress(memories[memId], &elfhandle->segments[memId].mpcAddr);
+        }
+    }
+
+    return CM_OK;
+}
+
+/*
+ * Note: in case of error, part of memory could have been allocated and must be free by calling cm_DSPABI_FreeTemplate
+ */
+t_cm_error cm_ELF_LoadTemplate(
+        t_cm_domain_id          domainId,
+        t_elfdescription        *elfhandle,
+        t_memory_handle         sharedMemories[NUMBER_OF_MMDSP_MEMORY])
+{
+    t_cm_error error;
+
+    if((error = allocSegment(domainId, elfhandle, sharedMemories, MEM_SHARABLE)) != CM_OK)
+        return error;
+
+    // Load each readonly segment
+    if((error = ELF64_loadSegment(elfhandle, sharedMemories, MEM_SHARABLE)) != CM_OK)
+        return error;
+
+    return CM_OK;
+}
+
+t_cm_error cm_ELF_LoadInstance(
+        t_cm_domain_id          domainId,
+        t_elfdescription        *elfhandle,
+        t_memory_handle         sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle         privateMemories[NUMBER_OF_MMDSP_MEMORY])
+{
+    t_memory_id memId;
+    t_cm_error error;
+
+    // Erase whole memories to make free in case of error
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        privateMemories[memId] = sharedMemories[memId];
+    }
+
+    if((error = allocSegment(domainId, elfhandle, privateMemories, MEM_PRIVATE)) != CM_OK)
+        return error;
+
+    // Load each writable memory
+    if((error = ELF64_loadSegment(elfhandle, privateMemories, MEM_PRIVATE)) != CM_OK)
+        return error;
+
+    return CM_OK;
+}
+
+void cm_ELF_FlushTemplate(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY])
+{
+    t_memory_id memId;
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        if(sharedMemories[memId] != INVALID_MEMORY_HANDLE)
+            MMDSP_loadedSection(
+                    coreId, memId,
+                    sharedMemories[memId]);
+    }
+}
+
+void cm_ELF_FlushInstance(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle privateMemories[NUMBER_OF_MMDSP_MEMORY])
+{
+    t_memory_id memId;
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        if(privateMemories[memId] != INVALID_MEMORY_HANDLE && privateMemories[memId] != sharedMemories[memId])
+            MMDSP_loadedSection(
+                    coreId, memId,
+                    privateMemories[memId]);
+    }
+}
+
+void cm_ELF_FreeInstance(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle privateMemories[NUMBER_OF_MMDSP_MEMORY])
+{
+    t_memory_id memId;
+
+    if(privateMemories == NULL)
+        return;
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        if(privateMemories[memId] != INVALID_MEMORY_HANDLE && privateMemories[memId] != sharedMemories[memId])
+        {
+            MMDSP_unloadedSection(coreId, memId, privateMemories[memId]);
+            cm_DM_Free(privateMemories[memId], TRUE);
+        }
+    }
+}
+
+void cm_ELF_FreeTemplate(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY])
+{
+    t_memory_id memId;
+
+    if(sharedMemories == NULL)
+        return;
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        if(sharedMemories[memId] != INVALID_MEMORY_HANDLE)
+        {
+            MMDSP_unloadedSection(coreId, memId, sharedMemories[memId]);
+            cm_DM_Free(sharedMemories[memId], TRUE);
+        }
+    }
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/elfmmdsp.c b/drivers/staging/nmf-cm/cm/engine/elf/src/elfmmdsp.c
new file mode 100644
index 00000000000..452d0f1b175
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/elfmmdsp.c
@@ -0,0 +1,575 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/elf/inc/mmdsp.h>
+#include <cm/engine/elf/inc/bfd.h>
+#include <cm/engine/elf/inc/mpcal.h>
+
+#include <cm/engine/component/inc/initializer.h>
+
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/utils/inc/swap.h>
+#include <cm/engine/trace/inc/trace.h>
+
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h>
+
+static const t_elfmemory mmdspMemories[NUMBER_OF_MMDSP_MEMORY] = {
+    {0,  SDRAM_CODE,      SDRAMTEXT_BASE_ADDR,        CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_CODE, 8, 8, "SDRAM_CODE"},  /* 0: Program memory */
+    {1,  INTERNAL_XRAM24, 0,                          CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 4, "XROM"},        /* 1: Internal X memory */
+    {2,  INTERNAL_YRAM24, 0,                          CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 4, "YROM"},        /* 2: Y memory */
+    {3,  SDRAM_EXT24,     SDRAMMEM24_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 4, "SDR0M24"},     /* 5: SDRAM24 */
+    {4,  SDRAM_EXT16,     SDRAMMEM16_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 2, "SDROM16"},     /* 6: SDRAM16 */
+    {5,  ESRAM_EXT24,     ESRAMMEM24_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 4, "ESROM24"},     /* 8: ESRAM24 */
+    {6,  ESRAM_EXT16,     ESRAMMEM16_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 2, "ESROM16"},     /* 9: ESRAM16 */
+    {7,  ESRAM_CODE,      ESRAMTEXT_BASE_ADDR,        CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_CODE, 8, 8, "ESRAM_CODE"},  /*10: ESRAM code */
+    {8,  INTERNAL_XRAM24, 0,                          CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 4, "XRAM"},        /* 1: Internal X memory */
+    {9,  INTERNAL_YRAM24, 0,                          CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 4, "YRAM"},        /* 2: Y memory */
+    {10, SDRAM_EXT24,     SDRAMMEM24_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 4, "SDRAM24"},     /* 5: SDRAM24 */
+    {11, SDRAM_EXT16,     SDRAMMEM16_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 2, "SDRAM16"},     /* 6: SDRAM16 */
+    {12, ESRAM_EXT24,     ESRAMMEM16_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 4, "ESRAM24"},     /* 8: ESRAM24 */
+    {13, ESRAM_EXT16,     ESRAMMEM16_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 2, "ESRAM16"},     /* 9: ESRAM16 */
+    {14, LOCKED_CODE,     SDRAMTEXT_BASE_ADDR,        CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_CODE, 8, 8, "LOCKED_CODE"},  /*  : .locked */
+};
+
+#define MAX_ELFSECTIONNAME  10
+struct memoryMapping {
+    char        *elfSectionName;
+    t_uint32    memoryIndex[MEM_FOR_LAST]; // memoryIndex[t_instance_property]
+};
+
+static const struct memoryMapping mappingmem0[] = {
+        {"mem0.0",  {0,  0}},
+        {"mem0.1",  {0,  0}},
+        {"mem0.2",  {0,  0}}
+};
+static const struct memoryMapping mappingmem10 =
+        {"mem10",   {7,  7}};
+static const struct memoryMapping mappinglocked =
+        {".locked", {14,  14}};
+static const struct memoryMapping mappingmem1[] = {
+        {"",  {0xff,  0xff}},
+        {"mem1.1",  {1,  1}},
+        {"mem1.2",  {8,  1}},
+        {"mem1.3",  {1,  1}},
+        {"mem1.4",  {8,  1}},
+        {"mem1.stack", {8, 1}}
+};
+static const struct memoryMapping mappingmem2[] = {
+        {"",  {0xff,  0xff}},
+        {"mem2.1",  {2,  2}},
+        {"mem2.2",  {9,  2}},
+        {"mem2.3",  {2,  2}},
+        {"mem2.4",  {9,  2}}
+};
+static const struct memoryMapping mappingmem5[] = {
+        {"",  {0xff,  0xff}},
+        {"mem5.1",  {3,  3}},
+        {"mem5.2",  {10, 3}},
+        {"mem5.3",  {3,  3}},
+        {"mem5.4",  {10, 3}}
+};
+static const struct memoryMapping mappingmem6[] = {
+        {"",  {0xff,  0xff}},
+        {"mem6.1",  {4,  4}},
+        {"mem6.2",  {11, 4}},
+        {"mem6.3",  {4,  4}},
+        {"mem6.4",  {11, 4}}
+};
+static const struct memoryMapping mappingmem8[] = {
+        {"",  {0xff,  0xff}},
+        {"mem8.1",  {5,  5}},
+        {"mem8.2",  {12, 5}},
+        {"mem8.3",  {5,  5}},
+        {"mem8.4",  {12, 5}}
+};
+static const struct memoryMapping mappingmem9[] = {
+        {"",  {0xff,  0xff}},
+        {"mem9.1",  {6,  6}},
+        {"mem9.2",  {13, 6}},
+        {"mem9.3",  {6,  6}},
+        {"mem9.4",  {13, 6}}
+};
+
+static const struct {
+    const struct memoryMapping* mapping;
+    unsigned int        number;
+} hashMappings[10] = {
+        {mappingmem0, sizeof(mappingmem0) / sizeof(mappingmem0[0])},
+        {mappingmem1, sizeof(mappingmem1) / sizeof(mappingmem1[0])},
+        {mappingmem2, sizeof(mappingmem2) / sizeof(mappingmem2[0])},
+        {0x0, 0},
+        {0x0, 0},
+        {mappingmem5, sizeof(mappingmem5) / sizeof(mappingmem5[0])},
+        {mappingmem6, sizeof(mappingmem6) / sizeof(mappingmem6[0])},
+        {0x0, 0},
+        {mappingmem8, sizeof(mappingmem8) / sizeof(mappingmem8[0])},
+        {mappingmem9, sizeof(mappingmem9) / sizeof(mappingmem9[0])},
+};
+
+const t_elfmemory* MMDSP_getMappingById(t_memory_id memId)
+{
+    return &mmdspMemories[memId];
+}
+
+const t_elfmemory* MMDSP_getMappingByName(const char* sectionName, t_instance_property property)
+{
+    if(sectionName[0] == 'm' && sectionName[1] == 'e' && sectionName[2] == 'm')
+    {
+        if(sectionName[4] == '.')
+        {
+            if(sectionName[5] >= '0' && sectionName[5] <= '9')
+            {
+                if(sectionName[3] >= '0' && sectionName[3] <= '9')
+                {
+                    unsigned int m, sm;
+
+                    m = sectionName[3] - '0';
+                    sm = sectionName[5] - '0';
+                    if(sm < hashMappings[m].number)
+                        return &mmdspMemories[hashMappings[m].mapping[sm].memoryIndex[property]];
+                }
+            } else if(sectionName[3] == '1' && sectionName[5] == 's')
+                return &mmdspMemories[mappingmem1[5].memoryIndex[property]];
+        }
+        else if(sectionName[3] == '1' && sectionName[4] == '0')
+            return &mmdspMemories[mappingmem10.memoryIndex[property]];
+    }
+    else if(sectionName[0] == '.' && sectionName[1] == 'l' && sectionName[2] == 'o' && sectionName[3] == 'c' &&
+            sectionName[4] == 'k' && sectionName[5] == 'e' && sectionName[6] == 'd')
+    {
+        return &mmdspMemories[mappinglocked.memoryIndex[property]];
+    }
+
+    return NULL;
+}
+
+void MMDSP_serializeMemories(t_instance_property property,
+        const t_elfmemory** codeMemory, const t_elfmemory** thisMemory) {
+    // Return meory reference
+    *codeMemory = &mmdspMemories[0];
+    if(property == MEM_FOR_SINGLETON)
+    {
+        *thisMemory = &mmdspMemories[1];
+    }
+    else
+    {
+        *thisMemory = &mmdspMemories[8];
+    }
+}
+
+void MMDSP_copyCode(t_uint64 * remoteAddr64, const char* origAddr, int nb)
+{
+    int m;
+
+    // Linux allow unaligned access
+#ifdef LINUX
+    t_uint64  *origAddr64 = (t_uint64*)origAddr;
+#else
+    __packed t_uint64  *origAddr64 = (__packed t_uint64*)origAddr;
+#endif
+
+    for (m = 0; m < nb; m += 8)
+    {
+        *remoteAddr64++ = swap64(*origAddr64++);
+    }
+}
+
+void MMDSP_copyData24(t_uint32 * remoteAddr32, const char* origAddr, int nb)
+{
+    int m;
+
+    for (m = 0; m < nb; m+=4)
+    {
+        t_uint32 value1;
+
+        value1  = (*origAddr++ << 16);
+        value1 |= (*origAddr++ << 8);
+        value1 |= (*origAddr++ << 0);
+        *remoteAddr32++ = value1;
+    }
+}
+
+void MMDSP_copyData16(t_uint16 * remoteAddr16, const char* origAddr, int nb)
+{
+    int m;
+
+    for (m = 0; m < nb; m+=2)
+    {
+        t_uint16 value1;
+
+        origAddr++; // Skip this byte (which is put in elf file for historical reason)
+        value1 = (*origAddr++ << 8);
+        value1 |= (*origAddr++ << 0);
+        *remoteAddr16++ = value1;
+    }
+}
+
+#if 0
+__asm void MMDSP_copyCode(void* dst, const void* src, int nb)
+{
+    PUSH     {r4-r8, lr}
+    SUBS     r2,r2,#0x20
+    BCC      l4
+
+l5
+    SETEND   BE
+    LDR      r4, [r1], #0x4
+    LDR      r3, [r1], #0x4
+    LDR      r6, [r1], #0x4
+    LDR      r5, [r1], #0x4
+    LDR      r8, [r1], #0x4
+    LDR      r7, [r1], #0x4
+    LDR      lr, [r1], #0x4
+    LDR      r12, [r1], #0x4
+
+    SETEND   LE
+    STM      r0!,{r3-r8,r12, lr}
+    SUBS     r2,r2,#0x20
+    BCS      l5
+
+l4
+    LSLS     r12,r2,#28
+
+    SETEND   BE
+    LDRCS    r4, [r1], #0x4
+    LDRCS    r3, [r1], #0x4
+    LDRCS    r6, [r1], #0x4
+    LDRCS    r5, [r1], #0x4
+    SETEND   LE
+    STMCS    r0!,{r3-r6}
+
+    SETEND   BE
+    LDRMI    r4, [r1], #0x4
+    LDRMI    r3, [r1], #0x4
+    SETEND   LE
+    STMMI    r0!,{r3-r4}
+
+    POP      {r4-r8, pc}
+}
+#endif
+
+#ifdef LINUX
+static void PLD5(int r)
+{
+    asm volatile (
+	    "PLD      [r0, #0x20]     \n\t"
+	    "PLD      [r0, #0x40]     \n\t"
+	    "PLD      [r0, #0x60]     \n\t"
+	    "PLD      [r0, #0x80]     \n\t"
+	    "PLD      [r0, #0xA0]" );
+}
+
+static void PLD1(int r)
+{
+    asm volatile (
+	    "PLD      [r0, #0xC0]" );
+}
+#else /* Symbian, Think -> We assume ARMCC */
+static __asm void PLD5(int r)
+{
+    PLD      [r0, #0x20]
+    PLD      [r0, #0x40]
+    PLD      [r0, #0x60]
+    PLD      [r0, #0x80]
+    PLD      [r0, #0xA0]
+
+              bx lr
+}
+
+static __asm void PLD1(int r)
+{
+    PLD      [r0, #0xC0]
+
+              bx lr
+}
+#endif
+
+#if 0
+__asm void COPY(void* dst, const void* src, int nb)
+{
+    PUSH     {r4-r8, lr}
+    SUBS     r2,r2,#0x20
+    BCC      l4a
+    PLD      [r1, #0x20]
+    PLD      [r1, #0x40]
+    PLD      [r1, #0x60]
+    PLD      [r1, #0x80]
+    PLD      [r1, #0xA0]
+
+l5a
+    PLD      [r1, #0xC0]
+    LDM      r1!,{r3-r8,r12,lr}
+    STM      r0!,{r3-r8,r12,lr}
+    SUBS     r2,r2,#0x20
+    BCS      l5a
+
+l4a
+    LSLS     r12,r2,#28
+    LDMCS    r1!,{r3,r4,r12,lr}
+    STMCS    r0!,{r3,r4,r12,lr}
+    LDMMI    r1!,{r3,r4}
+    STMMI    r0!,{r3,r4}
+    POP      {r4-r8,lr}
+    LSLS     r12,r2,#30
+    LDRCS    r3,[r1],#4
+    STRCS    r3,[r0],#4
+    BXEQ     lr
+l6b
+    LSLS     r2,r2,#31
+    LDRHCS   r3,[r1],#2
+    LDRBMI   r2,[r1],#1
+    STRHCS   r3,[r0],#2
+    STRBMI   r2,[r0],#1
+    BX       lr
+}
+#endif
+
+
+void MMDSP_copySection(t_uint32 origAddr, t_uint32 remoteAddr, t_uint32 sizeInByte) {
+    t_uint32 endAddr = remoteAddr + sizeInByte;
+
+    PLD5(origAddr);
+
+    // Align on 32bits
+    if((remoteAddr & 0x3) != 0)
+    {
+        *(t_uint16*)remoteAddr = *(t_uint16*)origAddr;
+        remoteAddr += sizeof(t_uint16);
+        origAddr += sizeof(t_uint16);
+    }
+
+    // Align on 64bits
+    if((remoteAddr & 0x7) != 0 && (remoteAddr <= endAddr - sizeof(t_uint32)))
+    {
+        *(t_uint32*)remoteAddr = *(t_uint32*)origAddr;
+        remoteAddr += sizeof(t_uint32);
+        origAddr += sizeof(t_uint32);
+    }
+
+    // 64bits burst access
+    for(; remoteAddr <= endAddr - sizeof(t_uint64); remoteAddr += sizeof(t_uint64), origAddr += sizeof(t_uint64))
+    {
+        PLD1(origAddr);
+        *(volatile t_uint64*)remoteAddr = *(t_uint64*)origAddr;
+    }
+
+    // Remain 32bits access
+    if(remoteAddr <= endAddr - sizeof(t_uint32))
+    {
+        *(t_uint32*)remoteAddr = *(t_uint32*)origAddr;
+        remoteAddr += sizeof(t_uint32);
+        origAddr += sizeof(t_uint32);
+    }
+
+    // Remain 16bits access
+    if(remoteAddr <= endAddr - sizeof(t_uint16))
+        *(t_uint16*)remoteAddr = *(t_uint16*)origAddr;
+}
+
+
+void MMDSP_bzeroSection(t_uint32 remoteAddr, t_uint32 sizeInByte) {
+    t_uint32 endAddr = remoteAddr + sizeInByte;
+
+    // Align on 32bits
+    if((remoteAddr & 0x3) != 0)
+    {
+        *(t_uint16*)remoteAddr = 0;
+        remoteAddr += sizeof(t_uint16);
+    }
+
+    // Align on 64bits
+    if((remoteAddr & 0x7) != 0 && (remoteAddr <= endAddr - sizeof(t_uint32)))
+    {
+        *(t_uint32*)remoteAddr = 0;
+        remoteAddr += sizeof(t_uint32);
+    }
+
+    // 64bits burst access
+    for(; remoteAddr <= endAddr - sizeof(t_uint64); remoteAddr += sizeof(t_uint64))
+        *(volatile t_uint64*)remoteAddr = 0ULL;
+
+    // Remain 32bits access
+    if(remoteAddr <= endAddr - sizeof(t_uint32))
+    {
+        *(t_uint32*)remoteAddr = 0;
+        remoteAddr += sizeof(t_uint32);
+    }
+
+    // Remain 16bits access
+    if(remoteAddr <= endAddr - sizeof(t_uint16))
+        *(t_uint16*)remoteAddr = 0;
+}
+
+void MMDSP_loadedSection(t_nmf_core_id coreId, t_memory_id memId, t_memory_handle handle)
+{
+    if(mmdspMemories[memId].purpose == MEM_CODE)
+    {
+        OSAL_CleanDCache(cm_DSP_GetHostLogicalAddress(handle), cm_MM_GetSize(handle));
+    }
+
+    if(memId == LOCKED_CODE)
+    {
+        t_uint32 DspAddress, DspSize;
+
+        cm_DSP_GetDspMemoryHandleSize(handle, &DspSize);
+        cm_DSP_GetDspAddress(handle, &DspAddress);
+
+        cm_COMP_InstructionCacheLock(coreId, DspAddress,  DspSize);
+    }
+}
+
+void MMDSP_unloadedSection(t_nmf_core_id coreId, t_memory_id memId, t_memory_handle handle)
+{
+    if(memId == LOCKED_CODE)
+    {
+        t_uint32 DspAddress, DspSize;
+
+        cm_DSP_GetDspMemoryHandleSize(handle, &DspSize);
+        cm_DSP_GetDspAddress(handle, &DspAddress);
+
+        cm_COMP_InstructionCacheUnlock(coreId, DspAddress, DspSize);
+    }
+
+}
+
+static struct reloc_howto_struct elf64_mmdsp_howto_table[] =
+{
+  HOWTO (R_MMDSP_IMM20_16,  /* type */
+     0,         /* rightshift */
+     4,         /* size (0 = byte, 1 = short, 2 = long) */
+     16,            /* bitsize */
+     FALSE,         /* pc_relative */
+     8,         /* bitpos */
+     complain_overflow_dont, /* complain_on_overflow */
+     0x0, /* special_function */
+     "R_MMDSP_IMM20_16",    /* name */
+     FALSE,         /* partial_inplace */
+     0x0,           /* src_mask */
+     0x0000000000ffff00,    /* dst_mask */
+     FALSE),        /* pcrel_offset */
+
+    /* A 4-bit absolute relocation for splitted 20 bits immediate, shifted by 56 */
+
+  HOWTO (R_MMDSP_IMM20_4,   /* type */
+     16,            /* rightshift */
+     4,         /* size (0 = byte, 1 = short, 2 = long) */
+     4,         /* bitsize */
+     FALSE,         /* pc_relative */
+     56,            /* bitpos */
+     complain_overflow_dont, /* complain_on_overflow */
+     0x0, /* special_function */
+     "R_MMDSP_IMM20_4", /* name */
+     FALSE,         /* partial_inplace */
+     0x0,           /* src_mask */
+     0x0f00000000000000LL,  /* dst_mask */
+     FALSE),        /* pcrel_offset */
+
+  HOWTO (R_MMDSP_24,           /* type */
+        0,             /* rightshift */
+        2,             /* size (0 = byte, 1 = short, 2 = long) */
+        24,                /* bitsize */
+        FALSE,             /* pc_relative */
+        0,             /* bitpos */
+        complain_overflow_bitfield,    /* complain_on_overflow */
+        0x0,     /* special_function */
+        "R_MMDSP_24",          /* name */
+        FALSE,             /* partial_inplace */
+        0x0,            /* src_mask */
+        0xffffffff,            /* dst_mask */
+        FALSE),            /* pcrel_offset */
+
+  HOWTO (R_MMDSP_IMM16,       /* type */
+             0,         /* rightshift */
+             4,         /* size (0 = byte, 1 = short, 2 = long) */
+             16,            /* bitsize */
+             FALSE,         /* pc_relative */
+             8,         /* bitpos */
+             complain_overflow_bitfield, /* complain_on_overflow */
+             0x0, /* special_function */
+             "R_MMDSP_IMM16",   /* name */
+             FALSE,         /* partial_inplace */
+             0x0,               /* src_mask */
+             0x0000000000ffff00,    /* dst_mask */
+             FALSE),        /* pcrel_offset */
+};
+
+static const char* lastInPlaceAddr = 0;
+static long long lastInPlaceValue;
+
+void MMDSP_performRelocation(
+        t_uint32 type,
+        const char* symbol_name,
+        t_uint32 symbol_addr,
+        char* reloc_addr,
+        const char* inPlaceAddr,
+        t_uint32 reloc_offset) {
+    int i;
+
+    for(i = 0; i < sizeof(elf64_mmdsp_howto_table) / sizeof(elf64_mmdsp_howto_table[0]); i++)
+    {
+        struct reloc_howto_struct* howto = &elf64_mmdsp_howto_table[i];
+        if(howto->type == type)
+        {
+            t_uint64 relocation;
+
+            LOG_INTERNAL(2, "reloc '%s:0x%x' type %s at 0x%x (0x%x)\n",
+                    symbol_name ? symbol_name : "??", symbol_addr,
+                    howto->name,
+                    reloc_offset, reloc_addr, 0);
+
+            relocation = symbol_addr;
+
+            if (howto->pc_relative) {
+                // Not handle yet
+            }
+
+            if (howto->complain_on_overflow != complain_overflow_dont) {
+                // Not handle yet
+            }
+
+            relocation >>= howto->rightshift;
+
+            relocation <<= howto->bitpos;
+
+#define DOIT(x) \
+    x = ( (x & ~howto->dst_mask) | (((x & howto->src_mask) +  relocation) & howto->dst_mask))
+
+            switch (howto->size) {
+            case 2: {
+                long x = *(long*)inPlaceAddr;
+
+               // CM_ASSERT(*(long*)inPlaceAddr == *(long*)reloc_addr);
+
+                DOIT (x);
+                *(long*)reloc_addr = x;
+            }
+            break;
+            case 4: {
+                long long x;
+                if(lastInPlaceAddr == inPlaceAddr)
+                {
+                    x = lastInPlaceValue;
+                }
+                else
+                {
+                   //  CM_ASSERT(*(__packed long long*)inPlaceAddr == *(long long*)reloc_addr);
+                    x = *(long long*)inPlaceAddr;
+                    lastInPlaceAddr = inPlaceAddr;
+                }
+
+                DOIT (x);
+                *(long long*)reloc_addr = lastInPlaceValue = x;
+            }
+            break;
+            default:
+                CM_ASSERT(0);
+            }
+
+            return;
+        }
+    }
+
+    ERROR("Relocation type %d not supported for '%s'\n", type, symbol_name, 0, 0, 0, 0);
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/elfrelocate.c b/drivers/staging/nmf-cm/cm/engine/elf/src/elfrelocate.c
new file mode 100644
index 00000000000..b08ac6a361e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/elfrelocate.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/elf/inc/bfd.h>
+#include <cm/engine/elf/inc/mpcal.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/string.h>
+
+t_cm_error cm_ELF_relocateSharedSegments(
+        t_memory_handle *memories,
+        t_elfdescription *elfhandle,
+        void                        *cbContext)
+{
+    return ELF64_relocateSegments(
+        memories,
+        elfhandle,
+        MEM_SHARABLE,
+        cbContext);
+}
+
+t_cm_error cm_ELF_relocatePrivateSegments(
+        t_memory_handle *memories,
+        t_elfdescription *elfhandle,
+        void                        *cbContext)
+{
+    return ELF64_relocateSegments(
+        memories,
+        elfhandle,
+        MEM_PRIVATE,
+        cbContext);
+}
+
+void cm_ELF_performRelocation(
+        t_uint32 type,
+        const char* symbol_name,
+        t_uint32 symbol_addr,
+        char* reloc_addr)
+{
+    MMDSP_performRelocation(
+                        type,
+                        symbol_name,
+                        symbol_addr,
+                        reloc_addr,
+                        reloc_addr,
+                        0xBEEF);
+
+    OSAL_CleanDCache((t_uint32)reloc_addr, 8);
+}
+
+t_cm_error cm_ELF_GetMemory(
+        t_elfdescription            *elf,
+        t_tmp_elfdescription        *elftmp,
+        t_uint32                    address,
+        t_memory_purpose            purpose,
+        t_memory_reference          *memory) {
+    t_memory_id memId;
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        const t_elfmemory* mem = MMDSP_getMappingById(memId);
+
+        if(mem->purpose == purpose &&    // Memory correspond
+                elf->segments[mem->id].sumSize != 0 && // Segment allocated
+                (elf->segments[mem->id].mpcAddr <= address) &&
+                (address < elf->segments[mem->id].mpcAddr + elf->segments[mem->id].sumSize / mem->fileEntSize)) {
+            memory->memory = mem;
+            memory->offset = address - elf->segments[mem->id].mpcAddr;
+            return CM_OK;
+        }
+    }
+
+    ERROR("Memory %x,%d not found\n", address, purpose, 0, 0, 0, 0);
+    return CM_INVALID_ELF_FILE;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/elfxx.c b/drivers/staging/nmf-cm/cm/engine/elf/src/elfxx.c
new file mode 100644
index 00000000000..4a2976a6bc1
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/elfxx.c
@@ -0,0 +1,591 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/elf/inc/mpcal.h>
+
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/utils/inc/mem.h>
+
+
+static t_uint32 max(t_uint32 a, t_uint32 b)
+{
+    return (a >= b) ? a : b;
+}
+/*
+static t_uint32 min(t_uint32 a, t_uint32 b)
+{
+    return (a <= b) ? a : b;
+}
+*/
+
+struct XXrelocation
+{
+    t_uint32            st_value;
+    ElfXX_Half          st_shndx;
+    Elf64_Sxword        r_addend;
+    t_uint32            OffsetInElf;
+    t_uint32            type;
+
+    t_dup_char          symbol_name;    // Valid only if st_shndx == SHN_UNDEF
+};
+
+struct XXSection {
+    ElfXX_Word      sh_type;        /* Section type */
+    t_uint32        sh_size;        /* Section size in bytes */
+    ElfXX_Word      sh_info;        /* Additional section information */
+    ElfXX_Word      sh_link;        /* Link to another section */
+    t_uint32        sh_addralign;   /* Some sections have address alignment constraints */
+    t_uint32        sh_addr;        /* Section addr */
+    ElfXX_Xword     sh_flags;       /* Section flags */
+
+    const char      *data;
+    t_uint32        trueDataSize;   /* Valid if different from sh_size */
+    const char      *sectionName;
+
+    t_uint32            offsetInSegment;
+    const t_elfmemory   *meminfo;
+
+    t_uint32                relocationNumber;
+    struct XXrelocation     *relocations;
+};
+
+struct XXElf {
+    t_uint32            e_shnum;
+    struct XXSection                sectionss[1];
+};
+
+t_cm_error ELF64_LoadComponent(
+        t_uint16                    e_machine,
+        const char                  *elfdata,
+        t_elfdescription            **elfhandlePtr,
+        t_tmp_elfdescription        *elftmp)
+{
+    t_elfdescription *elfhandle;
+    const ElfXX_Ehdr *header = (ElfXX_Ehdr*)elfdata;
+    const ElfXX_Shdr *sections;
+    const char *strings;
+    struct XXElf* ELF;
+    int i, nb;
+
+    elftmp->elfdata = elfdata;
+
+    /* Sanity check */
+    if (swapHalf(header->e_machine) != e_machine)
+    {
+        ERROR("This is not a executable for such MPC\n", 0, 0, 0, 0, 0, 0);
+        return CM_INVALID_ELF_FILE;
+    }
+
+    // Cache elf file informations
+    nb = swapHalf(header->e_shnum);
+    elftmp->isExecutable = (swapHalf(header->e_type) == ET_EXEC);
+
+    elfhandle = (t_elfdescription*)OSAL_Alloc_Zero(
+            sizeof(t_elfdescription) + sizeof(struct XXElf) + sizeof(struct XXSection) * (nb - 1));
+    if(elfhandle == NULL)
+        return CM_NO_MORE_MEMORY;
+
+    ELF = elfhandle->ELF = (struct XXElf*)(elfhandle + 1);
+
+    ELF->e_shnum = nb;
+
+    sections = (ElfXX_Shdr*)&elfdata[swapXword(header->e_shoff)];
+    // Compute and swap section infromation
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        ELF->sectionss[i].sh_type = swapWord(sections[i].sh_type);
+        ELF->sectionss[i].sh_info = swapWord(sections[i].sh_info);
+        ELF->sectionss[i].sh_link = swapWord(sections[i].sh_link);
+        ELF->sectionss[i].sh_size = (t_uint32)swapXword(sections[i].sh_size);
+        ELF->sectionss[i].sh_addralign = (t_uint32)swapXword(sections[i].sh_addralign);
+        ELF->sectionss[i].sh_addr = (t_uint32)swapXword(sections[i].sh_addr);
+        ELF->sectionss[i].sh_flags = swapXword(sections[i].sh_flags);
+
+        elftmp->sectionData[i] = &elfdata[(t_uint32)swapXword(sections[i].sh_offset)];
+    }
+
+    /*
+     * search nmf_segment
+     */
+    strings = elftmp->sectionData[swapHalf(header->e_shstrndx)];
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        ELF->sectionss[i].sectionName = &strings[swapWord(sections[i].sh_name)];
+
+        // Found nmf_segment to see if it's
+        if(cm_StringCompare("nmf_segment", ELF->sectionss[i].sectionName, 11) == 0) {
+            elftmp->nmfSectionIndex = i;
+            elftmp->elfheader = (const t_elf_component_header*)elftmp->sectionData[i];
+        }
+    }
+
+    if(elftmp->nmfSectionIndex == 0)
+    {
+        ERROR("This is not a NMF component\n", 0, 0, 0, 0, 0, 0);
+        goto invalid;
+    }
+
+    /*
+     * Determine component type
+     */
+    elfhandle->magicNumber = swap32(elftmp->elfheader->magic);
+    switch(elfhandle->magicNumber) {
+    case MAGIC_COMPONENT:
+        elfhandle->instanceProperty = MEM_FOR_MULTIINSTANCE;
+        break;
+    case MAGIC_SINGLETON:
+    case MAGIC_FIRMWARE:
+        elfhandle->instanceProperty = MEM_FOR_SINGLETON;
+        break;
+    }
+
+    // Copy content
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        ELF->sectionss[i].meminfo = MMDSP_getMappingByName(
+                ELF->sectionss[i].sectionName,
+                elfhandle->instanceProperty);
+
+        if(ELF->sectionss[i].meminfo != NULL)
+            ELF->sectionss[i].trueDataSize = (ELF->sectionss[i].sh_size / ELF->sectionss[i].meminfo->fileEntSize) * ELF->sectionss[i].meminfo->memEntSize;
+
+        if(ELF->sectionss[i].sh_size != 0 &&
+                ELF->sectionss[i].sh_type == SHT_PROGBITS &&
+                (ELF->sectionss[i].sh_flags & SHF_ALLOC) != 0)
+        {
+            const char* elfAddr = elftmp->sectionData[i];
+
+            ELF->sectionss[i].data = OSAL_Alloc(ELF->sectionss[i].trueDataSize);
+            if(ELF->sectionss[i].data == NULL)
+                goto oom;
+
+            if(ELF->sectionss[i].meminfo->purpose == MEM_CODE)
+            {
+                MMDSP_copyCode(
+                        (t_uint64*)ELF->sectionss[i].data,
+                        elfAddr,
+                        ELF->sectionss[i].trueDataSize);
+            }
+            else if(ELF->sectionss[i].meminfo->purpose == MEM_DATA &&
+                    // Always 3 for data ELF->sectionss[i].meminfo->fileEntSize == 3 &&
+                    ELF->sectionss[i].meminfo->memEntSize == 4)
+            {
+                MMDSP_copyData24(
+                        (t_uint32*)ELF->sectionss[i].data,
+                        elfAddr,
+                        ELF->sectionss[i].trueDataSize);
+            }
+            else if(ELF->sectionss[i].meminfo->purpose == MEM_DATA &&
+                    // Always 3 for data ELF->sectionss[i].meminfo->fileEntSize == 3 &&
+                    ELF->sectionss[i].meminfo->memEntSize == 2)
+            {
+                MMDSP_copyData16(
+                        (t_uint16*)ELF->sectionss[i].data,
+                        elfAddr,
+                        ELF->sectionss[i].trueDataSize);
+            }
+            else
+                CM_ASSERT(0);
+        }
+    }
+
+    // Copy relocation
+    // Loop on all relocation section
+    for(i=0; i < ELF->e_shnum; i++)
+    {
+        int sh_info;
+
+        // Does this section is a relocation table (only RELA supported)
+        if((ELF->sectionss[i].sh_type != SHT_RELA) ||
+                ELF->sectionss[i].sh_size == 0) continue;
+
+        // Copy only relocation for loaded section
+        sh_info = ELF->sectionss[i].sh_info;
+        if(ELF->sectionss[sh_info].meminfo != NULL)
+        {
+            const ElfXX_Sym* symtab;
+            const char* strtab;
+            ElfXX_Rela* rel_start;
+            int n;
+
+            ELF->sectionss[sh_info].relocationNumber = ELF->sectionss[i].sh_size / sizeof(ElfXX_Rela);
+            ELF->sectionss[sh_info].relocations = (struct XXrelocation*)OSAL_Alloc_Zero(sizeof(struct XXrelocation) * ELF->sectionss[sh_info].relocationNumber);
+            if(ELF->sectionss[sh_info].relocations == NULL)
+                goto oom;
+
+            symtab = (ElfXX_Sym *)elftmp->sectionData[ELF->sectionss[i].sh_link];
+            strtab = elftmp->sectionData[ELF->sectionss[ELF->sectionss[i].sh_link].sh_link];
+            rel_start = (ElfXX_Rela*)elftmp->sectionData[i];
+            for(n = 0; n < ELF->sectionss[sh_info].relocationNumber; n++, rel_start++)
+            {
+                struct XXrelocation* relocation = &ELF->sectionss[sh_info].relocations[n];
+                ElfXX_Xword r_info = swapXword(rel_start->r_info);
+                int strtab_index = ELFXX_R_SYM(r_info);
+                const char* symbol_name = &strtab[swapWord(symtab[strtab_index].st_name)];
+
+                relocation->st_shndx = swapHalf(symtab[strtab_index].st_shndx);
+                relocation->st_value = (t_uint32)swapXword(symtab[strtab_index].st_value);
+                relocation->r_addend = swapXword(rel_start->r_addend);
+                relocation->OffsetInElf = (t_uint32)swapXword(rel_start->r_offset) / ELF->sectionss[sh_info].meminfo->fileEntSize;
+                relocation->type = ELFXX_R_TYPE(r_info);
+
+                switch(relocation->st_shndx) {
+                case SHN_UNDEF:
+                    relocation->symbol_name = cm_StringDuplicate(symbol_name + 1); /* Remove '_' prefix */
+                    if(relocation->symbol_name == NULL)
+                        goto oom;
+                    break;
+                case SHN_COMMON:
+                    ERROR("SHN_COMMON not handle for %s\n", symbol_name, 0, 0, 0, 0, 0);
+                    goto invalid;
+                }
+            }
+        }
+    }
+
+    *elfhandlePtr = elfhandle;
+    return CM_OK;
+invalid:
+    ELF64_UnloadComponent(elfhandle);
+    return CM_INVALID_ELF_FILE;
+oom:
+    ELF64_UnloadComponent(elfhandle);
+    return CM_NO_MORE_MEMORY;
+}
+
+t_cm_error ELF64_ComputeSegment(
+        t_elfdescription            *elfhandle,
+        t_tmp_elfdescription        *elftmp)
+{
+    struct XXElf* ELF = elfhandle->ELF;
+    int i;
+
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        ELF->sectionss[i].offsetInSegment =  0xFFFFFFFF;
+
+        if(ELF->sectionss[i].sh_type == SHT_PROGBITS || ELF->sectionss[i].sh_type == SHT_NOBITS) {
+            // This is a loadable memory (memory size could be zero since we can have symbol on it)...
+            const t_elfmemory* meminfo = ELF->sectionss[i].meminfo;
+
+            if(meminfo != NULL) {
+                // Which correspond to MPC memory
+
+                if(elftmp->isExecutable)
+                {
+                    if(! elfhandle->segments[meminfo->id].sumSizeSetted)
+                    {
+                        CM_ASSERT(ELF->sectionss[i].sh_addr >= meminfo->startAddr * meminfo->fileEntSize);
+
+                        elfhandle->segments[meminfo->id].sumSizeSetted = TRUE;
+                        elfhandle->segments[meminfo->id].sumSize = ELF->sectionss[i].sh_addr - meminfo->startAddr * meminfo->fileEntSize;
+                    }
+                    else
+                        CM_ASSERT(elfhandle->segments[meminfo->id].sumSize == ELF->sectionss[i].sh_addr - meminfo->startAddr * meminfo->fileEntSize);
+                }
+                else
+                {
+                    while(elfhandle->segments[meminfo->id].sumSize % ELF->sectionss[i].sh_addralign != 0)
+                        elfhandle->segments[meminfo->id].sumSize++;
+                }
+
+                elfhandle->segments[meminfo->id].maxAlign = max(elfhandle->segments[meminfo->id].maxAlign, ELF->sectionss[i].sh_addralign);
+                ELF->sectionss[i].offsetInSegment = elfhandle->segments[meminfo->id].sumSize / meminfo->fileEntSize;
+                elfhandle->segments[meminfo->id].sumSize += ELF->sectionss[i].sh_size;
+            }
+         } else if(ELF->sectionss[i].sh_type == SHT_RELA && ELF->sectionss[i].sh_info == elftmp->nmfSectionIndex) {
+             int secsym = ELF->sectionss[i].sh_link;
+             elftmp->relaNmfSegment = (ElfXX_Rela*)elftmp->sectionData[i];
+             elftmp->relaNmfSegmentEnd = (ElfXX_Rela*)((t_uint32)elftmp->relaNmfSegment + ELF->sectionss[i].sh_size);
+             elftmp->relaNmfSegmentSymbols = (ElfXX_Sym*)elftmp->sectionData[secsym];
+             elftmp->relaNmfSegmentStrings = elftmp->sectionData[ELF->sectionss[secsym].sh_link];
+         }
+    }
+
+    return CM_OK;
+}
+
+void ELF64_UnloadComponent(
+        t_elfdescription            *elfhandle)
+{
+    struct XXElf* ELF = elfhandle->ELF;
+    int i, n;
+
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        if(ELF->sectionss[i].relocations != NULL)
+        {
+            for(n = 0; n < ELF->sectionss[i].relocationNumber; n++)
+                cm_StringRelease(ELF->sectionss[i].relocations[n].symbol_name);
+            OSAL_Free(ELF->sectionss[i].relocations);
+        }
+
+        OSAL_Free((void*)ELF->sectionss[i].data);
+    }
+    OSAL_Free(elfhandle);
+}
+
+t_cm_error ELF64_loadSegment(
+        t_elfdescription            *elfhandle,
+        t_memory_handle             *memory,
+        t_memory_property           property)
+{
+    struct XXElf* ELF = elfhandle->ELF;
+    int i;
+
+    /*
+     * Copy ELF data in this segment
+     */
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        const t_elfmemory* mapping = ELF->sectionss[i].meminfo;
+
+        if(mapping == NULL)
+            continue;
+        if((! (ELF->sectionss[i].sh_flags & SHF_ALLOC)) || (ELF->sectionss[i].sh_size == 0))
+            continue;
+
+        // This is a loadable memory ...
+        if(
+                (mapping->property == property && elfhandle->instanceProperty != MEM_FOR_SINGLETON) ||
+                (property == MEM_SHARABLE && elfhandle->instanceProperty == MEM_FOR_SINGLETON) )
+        {
+            // Where memory exist and waited share/private correspond
+            t_uint32 remoteData = elfhandle->segments[mapping->id].hostAddr +
+                    ELF->sectionss[i].offsetInSegment * mapping->memEntSize;
+
+            if(ELF->sectionss[i].sh_type != SHT_NOBITS)
+            {
+                LOG_INTERNAL(2, "loadSection(%s, 0x%x, 0x%x, 0x%08x)\n",
+                        ELF->sectionss[i].sectionName, remoteData, ELF->sectionss[i].trueDataSize,
+                        (t_uint32)ELF->sectionss[i].data, 0, 0);
+
+                MMDSP_copySection((t_uint32)ELF->sectionss[i].data, remoteData, ELF->sectionss[i].trueDataSize);
+            }
+            else
+            {
+                LOG_INTERNAL(2, "bzeroSection(%s, 0x%x, 0x%x)\n",
+                        ELF->sectionss[i].sectionName, remoteData, ELF->sectionss[i].trueDataSize, 0, 0, 0);
+
+                MMDSP_bzeroSection(remoteData, ELF->sectionss[i].trueDataSize);
+            }
+        }
+    }
+
+    return CM_OK;
+}
+
+
+
+static const t_elfmemory* getSectionAddress(
+        t_memory_handle *memories,
+        t_elfdescription *elfhandle,
+        t_uint32 sectionIdx,
+        t_uint32 *sectionOffset,
+        t_cm_logical_address *sectionAddr) {
+    struct XXElf* ELF = elfhandle->ELF;
+    const t_elfmemory* mapping = ELF->sectionss[sectionIdx].meminfo;
+
+    if(mapping != NULL) {
+        *sectionOffset = (elfhandle->segments[mapping->id].mpcAddr +
+                ELF->sectionss[sectionIdx].offsetInSegment);
+
+        *sectionAddr = (t_cm_logical_address)(elfhandle->segments[mapping->id].hostAddr +
+                ELF->sectionss[sectionIdx].offsetInSegment * mapping->memEntSize);
+    }
+
+    return mapping;
+}
+
+static t_uint32 getSymbolAddress(
+        t_memory_handle             *memories,
+        t_elfdescription            *elfhandle,
+        t_uint32                    symbolSectionIdx,
+        t_uint32                    symbolOffet) {
+    struct XXElf* ELF = elfhandle->ELF;
+    const t_elfmemory* mapping = ELF->sectionss[symbolSectionIdx].meminfo;
+
+    if(mapping == NULL)
+        return 0xFFFFFFFF;
+    // CM_ASSERT(elfhandle->segments[mapping->id].sumSize != 0);
+    // CM_ASSERT(elfhandle->sections[symbolSectionIdx].offsetInSegment != 0xFFFFFFFF);
+
+    return elfhandle->segments[mapping->id].mpcAddr +
+        ELF->sectionss[symbolSectionIdx].offsetInSegment +
+        symbolOffet;
+}
+
+#if 0
+t_bool ELFXX_getSymbolLocation(
+        const t_mpcal_memory    *mpcalmemory,
+        t_elfdescription        *elf,
+        char                    *symbolName,
+        const t_elfmemory       **memory,
+        t_uint32                *offset) {
+    const ElfXX_Ehdr *header = (ElfXX_Ehdr*)elf->elfdata;
+    const ElfXX_Shdr *sections = (ElfXX_Shdr*)&elf->elfdata[swapXword(header->e_shoff)];
+    const char *strings = &elf->elfdata[swapXword(sections[swapHalf(header->e_shstrndx)].sh_offset)];
+    int len = cm_StringLength(symbolName, 256); // TO BE FIXED
+    int i;
+
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        ElfXX_Sym* symtab;
+        const char* strtab;
+        unsigned int size, j;
+
+        if(ELF->sectionss[i].sh_type != SHT_SYMTAB && ELF->sectionss[i].sh_type != SHT_DYNSYM) continue;
+
+        // Section is a symbol table
+        symtab = (ElfXX_Sym*)&elf->elfdata[swapXword(sections[i].sh_offset)];
+        strtab = &elf->elfdata[swapXword(sections[swapWord(sections[i].sh_link)].sh_offset)];
+        size = ELF->sectionss[i].sh_size / (unsigned int)swapXword(sections[i].sh_entsize);
+
+        for(j = 0; j < size; j++) {
+            const char* foundName = &strtab[swapWord(symtab[j].st_name)];
+
+            if(cm_StringCompare(symbolName, foundName, len) == 0) {
+                if(swapHalf(symtab[j].st_shndx) != SHN_UNDEF) {
+                    int sectionIdx = (int)swapHalf(symtab[j].st_shndx);
+                    ElfXX_Xword sh_flags = swapXword(sections[sectionIdx].sh_flags);
+
+                    *memory = mpcalmemory->getMappingByName(&strings[swapWord(sections[sectionIdx].sh_name)],
+                            sh_flags & SHF_WRITE ? MEM_RW : (sh_flags & SHF_EXECINSTR ? MEM_X : MEM_RO));
+                    *offset = (t_uint32)swapXword(symtab[j].st_value);
+
+                    return 1;
+                }
+            }
+        }
+    }
+    return 0;
+}
+#endif
+
+t_cm_error ELF64_relocateSegments(
+        t_memory_handle             *memories,
+        t_elfdescription            *elfhandle,
+        t_memory_property           property,
+        void                        *cbContext) {
+    struct XXElf* ELF = elfhandle->ELF;
+    int sec, n;
+
+    // Loop on all relocation section
+    for(sec=0; sec < ELF->e_shnum; sec++)
+    {
+        t_cm_logical_address sectionAddr = 0;
+        t_uint32 sectionOffset = 0;
+        const t_elfmemory* mapping;
+
+        if(ELF->sectionss[sec].relocations == NULL)
+            continue;
+
+        // Relocate only section in memory
+        mapping = getSectionAddress(memories,
+                elfhandle,
+                sec,
+                &sectionOffset,
+                &sectionAddr);
+        if(mapping == NULL)
+            continue;
+
+        if(
+                (mapping->property == property && elfhandle->instanceProperty != MEM_FOR_SINGLETON) ||
+                (property == MEM_SHARABLE && elfhandle->instanceProperty == MEM_FOR_SINGLETON) )
+        {
+            LOG_INTERNAL(2, "relocSection(%s)\n", ELF->sectionss[sec].sectionName, 0, 0, 0, 0, 0);
+
+            for(n = 0; n < ELF->sectionss[sec].relocationNumber; n++)
+            {
+                struct XXrelocation* relocation = &ELF->sectionss[sec].relocations[n];
+                t_uint32 symbol_addr;
+                char* relocAddr = (char*)(sectionAddr + relocation->OffsetInElf * mapping->memEntSize);
+
+                switch(relocation->st_shndx) {
+                case SHN_ABS:              // Absolute external reference
+                    symbol_addr = relocation->st_value;
+                    break;
+                case SHN_UNDEF:            // External reference
+                     // LOG_INTERNAL(0, "cm_resolvSymbol(%d, %s)\n", relocation->type, relocation->symbol_name, 0,0, 0, 0);
+                    symbol_addr = cm_resolvSymbol(cbContext,
+                            relocation->type,
+                            relocation->symbol_name,
+                            relocAddr);
+                    if(symbol_addr == 0x0) { // Not defined symbol
+                        ERROR("Symbol %s not found\n", relocation->symbol_name, 0, 0, 0, 0, 0);
+                        return CM_INVALID_ELF_FILE;
+                    } else if(symbol_addr == 0xFFFFFFFE) { // OOM
+                        return CM_NO_MORE_MEMORY;
+                    } else if(symbol_addr == 0xFFFFFFFF) { // Defined inside static binding
+                        continue;
+                    }
+                    break;
+                default:                   // Internal reference in loaded section
+                    symbol_addr = getSymbolAddress(
+                            memories,
+                            elfhandle,
+                            (t_uint32)relocation->st_shndx,
+                            relocation->st_value);
+                    if(symbol_addr == 0xFFFFFFFF) {
+                        ERROR("Symbol in section %s+%d not loaded\n",
+                                ELF->sectionss[relocation->st_shndx].sectionName,
+                                relocation->st_value, 0, 0, 0, 0);
+                        return CM_INVALID_ELF_FILE;
+                    }
+                    break;
+                }
+
+                symbol_addr += relocation->r_addend;
+
+                MMDSP_performRelocation(
+                        relocation->type,
+                        relocation->symbol_name,
+                        symbol_addr,
+                        relocAddr,
+                        ELF->sectionss[sec].data + relocation->OffsetInElf * mapping->memEntSize,
+                        sectionOffset + relocation->OffsetInElf);
+            }
+        }
+    }
+
+    return CM_OK;
+}
+
+t_cm_error ELF64_getRelocationMemory(
+        t_elfdescription            *elfhandle,
+        t_tmp_elfdescription        *elftmp,
+        t_uint32                    offsetInNmf,
+        t_memory_reference          *memory) {
+    struct XXElf* ELF = elfhandle->ELF;
+    const ElfXX_Rela* rel_start;
+    const ElfXX_Sym* relaNmfSegmentSymbols = (ElfXX_Sym*)elftmp->relaNmfSegmentSymbols;
+
+    for(rel_start = (ElfXX_Rela*)elftmp->relaNmfSegment; rel_start < (ElfXX_Rela*)elftmp->relaNmfSegmentEnd; rel_start++)
+    {
+        if((t_uint32)swapXword(rel_start->r_offset) == offsetInNmf)
+        {
+            int strtab_index = ELFXX_R_SYM(swapXword(rel_start->r_info));
+            int sectionIdx = (int)swapHalf(relaNmfSegmentSymbols[strtab_index].st_shndx);
+
+            memory->memory = ELF->sectionss[sectionIdx].meminfo;
+
+            if(memory->memory != NULL) {
+                memory->offset = (
+                        ELF->sectionss[sectionIdx].offsetInSegment +                           // Offset in Segment
+                        (t_uint32)swapXword(relaNmfSegmentSymbols[strtab_index].st_value) +    // Offset in Elf Section
+                        (t_uint32)swapXword(rel_start->r_addend));                                  // Addend
+
+                return CM_OK;
+            } else {
+                const char* symbol_name = &elftmp->relaNmfSegmentStrings[swapWord(relaNmfSegmentSymbols[strtab_index].st_name)];
+                ERROR("Symbol %s not found\n", symbol_name, 0, 0, 0, 0, 0);
+                return CM_INVALID_ELF_FILE;
+            }
+        }
+    }
+
+    ERROR("Unknown relocation error\n", 0, 0, 0, 0, 0, 0);
+    return CM_INVALID_ELF_FILE;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/mmdsp-debug.c b/drivers/staging/nmf-cm/cm/engine/elf/src/mmdsp-debug.c
new file mode 100644
index 00000000000..0853e2d25b5
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/mmdsp-debug.c
@@ -0,0 +1,287 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/elf/inc/mmdsp-loadmap.h>
+#include <cm/engine/elf/inc/mmdsp.h>
+#include <cm/engine/dsp/inc/semaphores_dsp.h>
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h>
+
+#include <cm/engine/power_mgt/inc/power.h>
+
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/component/inc/component_type.h>
+#include <inc/nmf-limits.h>
+
+#define LOADMAP_SEMAPHORE_USE_NB                        7
+
+static t_memory_handle headerHandle[NB_CORE_IDS] = {INVALID_MEMORY_HANDLE, };
+static struct LoadMapHdr *headerAddresses[NB_CORE_IDS] = {0, };
+static t_uint32 headerOffsets[NB_CORE_IDS] = {0, };
+
+t_cm_error cm_DSPABI_AddLoadMap(
+        t_cm_domain_id domainId,
+        const char* templateName,
+        const char* localname,
+        t_memory_handle *memories,
+        void *componentHandle)
+{
+    t_nmf_core_id coreId = cm_DM_GetDomainCoreId(domainId);
+
+    if (headerHandle[coreId] == 0) /* Create loadmap header */
+    {
+        headerHandle[coreId] = cm_DM_Alloc(domainId, SDRAM_EXT16,
+                                           sizeof(struct LoadMapHdr)/2, CM_MM_ALIGN_2WORDS, TRUE);
+        if (headerHandle[coreId] == INVALID_MEMORY_HANDLE)
+            return CM_NO_MORE_MEMORY;
+
+        headerAddresses[coreId] = (struct LoadMapHdr*)cm_DSP_GetHostLogicalAddress(headerHandle[coreId]);
+
+        headerAddresses[coreId]->nMagicNumber = LOADMAP_MAGIC_NUMBER;
+        headerAddresses[coreId]->nVersion = (LOADMAP_VERSION_MSB<<8)|(LOADMAP_VERSION_LSB);
+        headerAddresses[coreId]->nRevision = 0;
+        headerAddresses[coreId]->pFirstItem = 0;
+
+        //Register Header into XRAM:2
+        cm_DSP_GetDspAddress(headerHandle[coreId], &headerOffsets[coreId]);
+        cm_DSP_WriteXRamWord(coreId, 2, headerOffsets[coreId]);
+    }
+
+    // update Header nRevision field
+    headerAddresses[coreId]->nRevision++;
+
+    /*
+     * Build loadmap entry
+     */
+    {
+        t_memory_handle handle;
+        struct LoadMapItem* pItem;
+        t_uint32 dspentry;
+        unsigned char* pos;
+        t_uint32 fnlen, lnlen;
+        t_uint32 fnlenaligned, lnlenaligned;
+        t_uint32 address;
+        t_uint32 postStringLength;
+        int i;
+
+        postStringLength = cm_StringLength(".elf", 16);
+        fnlenaligned = fnlen = cm_StringLength(templateName, MAX_COMPONENT_FILE_PATH_LENGTH) + postStringLength + 2;
+        if((fnlenaligned % 2) != 0) fnlenaligned++;
+        lnlenaligned = lnlen = cm_StringLength(localname, MAX_TEMPLATE_NAME_LENGTH);
+        if((lnlenaligned % 2) != 0) lnlenaligned++;
+
+        // Allocate new loap map
+        handle = cm_DM_Alloc(domainId, SDRAM_EXT16,
+                             sizeof(struct LoadMapItem)/2 + (1 + fnlenaligned/2) + (1 + lnlenaligned/2),
+                             CM_MM_ALIGN_2WORDS, TRUE);
+        if (handle == INVALID_MEMORY_HANDLE)
+            return CM_NO_MORE_MEMORY;
+
+        pItem = (struct LoadMapItem*)cm_DSP_GetHostLogicalAddress(handle);
+        cm_DSP_GetDspAddress(handle, &dspentry);
+
+        // Link this new loadmap with the previous one
+        if(headerAddresses[coreId]->pFirstItem == NULL)
+            headerAddresses[coreId]->pFirstItem = (struct LoadMapItem *)dspentry;
+        else
+        {
+            struct LoadMapItem* curItem;
+
+            curItem = headerAddresses[coreId]->pFirstItem;
+            curItem = (struct LoadMapItem*)(((t_uint32)curItem - headerOffsets[coreId]) * 2 + (t_uint32)headerAddresses[coreId]); // To ARM address
+            while(curItem->pNextItem != NULL)
+            {
+                curItem = curItem->pNextItem;
+                curItem = (struct LoadMapItem*)(((t_uint32)curItem - headerOffsets[coreId]) * 2 + (t_uint32)headerAddresses[coreId]); // To ARM address
+            }
+            curItem->pNextItem = (struct LoadMapItem *)dspentry;
+        }
+
+        // DSP Address of the string at the end of the load map
+        pos = (unsigned char*)pItem + sizeof(struct LoadMapItem);
+
+        /*
+         * Set SolibFilename address information
+         *   ->  string = "./origfilename"
+         */
+        pItem->pSolibFilename = (char*)(dspentry + sizeof(struct LoadMapItem) / 2);
+        *(t_uint16*)pos = fnlen;
+        pos += 2;
+        *pos++ = '.';
+        *pos++ = '\\';
+        for(i = 0; i < fnlen - 2 - postStringLength; i++)
+        {
+            *pos++ = (templateName[i] == '.') ? '\\' : templateName[i];
+        }
+        *pos++ = '.';
+        *pos++ = 'e';
+        *pos++ = 'l';
+        *pos++ = 'f';
+        // add padding if needed
+        if ((t_uint32)pos & 1)
+            *pos++ = '\0';
+
+        /*
+         * Set Component Name  address information
+         */
+        if (lnlen != 0)
+        {
+            pItem->pComponentName = (char*)(dspentry + sizeof(struct LoadMapItem) / 2 + 1 + fnlenaligned / 2);
+
+            *(t_uint16*)pos = lnlen;
+            pos += 2;
+            for(i = 0; i < lnlenaligned; i++)
+            {
+                // If not aligned null ending copied
+                *pos++ = localname[i];
+            }
+        }
+        else
+        {
+            pItem->pComponentName = 0;
+        }
+
+        /*
+         * Set PROG information
+         */
+        if(memories[CODE_MEMORY_INDEX] == INVALID_MEMORY_HANDLE)
+            address = 0;
+        else
+            cm_DSP_GetDspAddress(memories[CODE_MEMORY_INDEX], &address);
+        pItem->pAddrProg = (void*)address;
+
+        /*
+         * Set ERAMCODE information
+         */
+        if(memories[ECODE_MEMORY_INDEX] == INVALID_MEMORY_HANDLE)
+            address = 0;
+        else
+            cm_DSP_GetDspAddress(memories[ECODE_MEMORY_INDEX], &address);
+        pItem->pAddrEmbProg = (void*)address;
+
+        /*
+         * Set THIS information
+         */
+        if(memories[PRIVATE_DATA_MEMORY_INDEX] != INVALID_MEMORY_HANDLE) {
+            // Standard component
+            cm_DSP_GetDspAddress(memories[PRIVATE_DATA_MEMORY_INDEX], &address);
+        } else if(memories[SHARE_DATA_MEMORY_INDEX] != INVALID_MEMORY_HANDLE) {
+            // Singleton component where data are shared (simulate THIS with shared memory)
+            cm_DSP_GetDspAddress(memories[SHARE_DATA_MEMORY_INDEX], &address);
+        } else {
+            // Component without data (take unique identifier -> arbitrary take host component handle)
+		address = (t_uint32)componentHandle;
+        }
+        pItem->pThis = (void*)address;
+
+        /*
+         * Set ARM THIS information
+         */
+        pItem->pARMThis = componentHandle;
+
+        /*
+         * Set Link to null (end of list)
+         */
+        pItem->pNextItem = 0;
+
+        /*
+         * Set XROM information
+         */
+        if(memories[XROM_MEMORY_INDEX] == INVALID_MEMORY_HANDLE)
+            address = 0;
+        else
+            cm_DSP_GetDspAddress(memories[XROM_MEMORY_INDEX], &address);
+        pItem->pXROM = (void*)address;
+
+        /*
+         * Set YROM information
+         */
+        if(memories[YROM_MEMORY_INDEX] == INVALID_MEMORY_HANDLE)
+            address = 0;
+        else
+            cm_DSP_GetDspAddress(memories[YROM_MEMORY_INDEX], &address);
+        pItem->pYROM = (void*)address;
+
+        /*
+         * Set memory handle (not used externally)
+         */
+        pItem->memHandle = (void*)handle;
+    }
+
+    OSAL_mb();
+
+    return CM_OK;
+}
+
+t_cm_error cm_DSPABI_RemoveLoadMap(
+        t_cm_domain_id domainId,
+        const char* templateName,
+        t_memory_handle *memories,
+        const char* localname,
+        void *componentHandle)
+{
+    struct LoadMapItem* curItem, **prevItemReference;
+    t_nmf_core_id coreId = cm_DM_GetDomainCoreId(domainId);
+
+    CM_ASSERT (headerHandle[coreId] != INVALID_MEMORY_HANDLE);
+
+    /* parse list until we find this */
+    prevItemReference = &headerAddresses[coreId]->pFirstItem;
+    curItem = *prevItemReference;
+    while(curItem != NULL)
+    {
+        curItem = (struct LoadMapItem*)(((t_uint32)curItem - headerOffsets[coreId]) * 2 + (t_uint32)headerAddresses[coreId]); // To ARM address
+
+        if(curItem->pARMThis == componentHandle)
+            break;
+
+        prevItemReference = &curItem->pNextItem;
+        curItem = *prevItemReference;
+    }
+
+
+    // Remove component from loadmap if founded
+    if(curItem != NULL)
+    {
+        /* take local semaphore */
+        cm_DSP_SEM_Take(coreId,LOADMAP_SEMAPHORE_USE_NB);
+
+        /* remove element from list */
+        *prevItemReference = curItem->pNextItem;
+
+        /* update nRevision field in header */
+        headerAddresses[coreId]->nRevision++;
+
+        /* If this is the last item, deallocate !!! */
+        if(headerAddresses[coreId]->pFirstItem == NULL)
+        {
+            // Deallocate memory
+            cm_DM_Free(headerHandle[coreId], TRUE);
+            headerHandle[coreId] = INVALID_MEMORY_HANDLE;
+
+            //Register Header into XRAM:2
+            cm_DSP_WriteXRamWord(coreId, 2, 0);
+        }
+
+        /* deallocate memory */
+        cm_DM_Free((t_memory_handle)curItem->memHandle, TRUE);
+
+        /* be sure memory is updated before releasing local semaphore */
+        OSAL_mb();
+
+        /* release local semaphore */
+        cm_DSP_SEM_Give(coreId,LOADMAP_SEMAPHORE_USE_NB);
+    }
+    else
+    {
+        ERROR("Component not in LoadMap %s, memory corruption????\n", localname, 0, 0, 0, 0, 0);
+    }
+
+    return CM_OK;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/mpcal.c b/drivers/staging/nmf-cm/cm/engine/elf/src/mpcal.c
new file mode 100644
index 00000000000..93d910a5ed6
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/mpcal.c
@@ -0,0 +1,6 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/elf/inc/mpcal.h>
diff --git a/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h b/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h
new file mode 100644
index 00000000000..3f995982088
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_EE_MGT_H
+#define __INC_EE_MGT_H
+
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+typedef struct {
+    t_component_instance        *instance;
+    t_nmf_executive_engine_id   executiveEngineId;
+    t_uint32                    currentStackSize[NMF_SCHED_URGENT + 1];
+    t_uint32                    voidAddr;
+    t_uint32                    traceState;
+    t_uint32                    printLevel;
+    t_uint32                    nbOfForceWakeup;
+} t_ee_state;
+
+//TODO, juraj, this should be done more properly, like accessor method, instead making this global variable..
+extern t_ee_state eeState[NB_CORE_IDS];
+
+/******************************************************************************/
+/************************ FUNCTIONS PROTOTYPES ********************************/
+/******************************************************************************/
+
+PUBLIC t_cm_error cm_EEM_Init(t_nmf_core_id coreId, const char *eeName, t_nmf_executive_engine_id executiveEngineId);
+PUBLIC void cm_EEM_Close(t_nmf_core_id coreId);
+PUBLIC t_uint32 cm_EEM_isStackUpdateNeed(t_nmf_core_id coreId, t_nmf_ee_priority priority, t_uint32 isInstantiate, t_uint32 needMinStackSize);
+PUBLIC t_cm_error cm_EEM_UpdateStack(t_nmf_core_id coreId, t_nmf_ee_priority priority, t_uint32 needMinStackSize, t_uint32 *pNewStackValue);
+PUBLIC t_ee_state* cm_EEM_getExecutiveEngine(t_nmf_core_id coreId);
+PUBLIC void cm_EEM_setTraceMode(t_nmf_core_id coreId, t_uint32 state);
+PUBLIC void cm_EEM_setPrintLevel(t_nmf_core_id coreId, t_uint32 level);
+t_cm_error cm_EEM_ForceWakeup(t_nmf_core_id coreId);
+void cm_EEM_AllowSleep(t_nmf_core_id coreId);
+
+#endif /* __INC_EE_MGT_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/src/executive_engine_mgt.c b/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/src/executive_engine_mgt.c
new file mode 100644
index 00000000000..14889ec6e4d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/src/executive_engine_mgt.c
@@ -0,0 +1,349 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*----------------------------------------------------------------------------*
+ * This module provides functions that allow to manage DSPs' Firmwares.       *
+ ******************************************************************************/
+
+
+/******************************************************************* Includes
+ ****************************************************************************/
+
+#include "../inc/executive_engine_mgt.h"
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/utils/inc/convert.h>
+#include <cm/engine/component/inc/initializer.h>
+#include <cm/engine/power_mgt/inc/power.h>
+#include <cm/engine/perfmeter/inc/mpcload.h>
+
+#include <share/communication/inc/nmf_service.h>
+
+t_ee_state eeState[NB_CORE_IDS];
+
+/****************************************************************** Functions
+ ****************************************************************************/
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetExecutiveEngineHandle(
+        t_cm_domain_id domainId,
+        t_cm_instance_handle *executiveEngineHandle)
+{
+    t_nmf_core_id coreId;
+
+    if (cm_DM_CheckDomain(domainId, DOMAIN_NORMAL) != CM_OK) {
+        return CM_INVALID_DOMAIN_HANDLE;
+    }
+
+    coreId = cm_DM_GetDomainCoreId(domainId);
+    //in case someone ask for ee on component manager !!!!
+    if (coreId == ARM_CORE_ID) {*executiveEngineHandle = 0;}
+    else {*executiveEngineHandle = eeState[coreId].instance->instance;}
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_EEM_Init(
+    t_nmf_core_id coreId,
+    const char *eeName,
+    t_nmf_executive_engine_id executiveEngineId)
+{
+    t_rep_component *pRepComponent;
+    t_cm_error error;
+    t_uint32 i;
+
+    eeState[coreId].instance = (t_component_instance *)0;
+    eeState[coreId].executiveEngineId = executiveEngineId;
+    for(i = NMF_SCHED_BACKGROUND; i < NMF_SCHED_URGENT + 1;i++)
+    {
+        eeState[coreId].currentStackSize[i] = MIN_STACK_SIZE;
+    }
+
+    // Try to load component file
+    if((error = cm_REP_lookupComponent(eeName, &pRepComponent)) != CM_OK)
+    {
+        if (error == CM_COMPONENT_NOT_FOUND)
+            ERROR("CM_COMPONENT_NOT_FOUND: Execution Engine %s\n", eeName, 0, 0, 0, 0, 0);
+        return error;
+    }
+
+    // Set to 1 during bootstrap since MMDSP forceWakeup is to one also in order to not go in idle state
+    // while configuration not finish !!!
+    eeState[coreId].nbOfForceWakeup = 1;
+
+    if ((error = cm_DSP_Boot(coreId)) != CM_OK)
+        return error;
+
+    if((error = cm_instantiateComponent(
+            eeName,
+            cm_DSP_GetState(coreId)->domainEE,
+            NMF_SCHED_URGENT,
+            eeName,
+            pRepComponent->elfhandle,
+            &eeState[coreId].instance)) != CM_OK)
+    {
+        cm_DSP_Shutdown(coreId);
+        return error;
+    }
+
+    /* Get Void Function */
+    eeState[coreId].voidAddr = cm_getFunction(eeState[coreId].instance, "helper", "Void");
+
+    /* allocate xram space for stack */
+    if (executiveEngineId == SYNCHRONOUS_EXECUTIVE_ENGINE)
+    {
+        error = cm_DSP_setStackSize(coreId, MIN_STACK_SIZE);
+    }
+    else
+    {
+        error = cm_DSP_setStackSize(coreId, (NMF_SCHED_URGENT + 1) * MIN_STACK_SIZE);
+    }
+    if (error != CM_OK)
+    {
+        cm_delayedDestroyComponent(eeState[coreId].instance);
+        eeState[coreId].instance = (t_component_instance *)0;
+        cm_DSP_Shutdown(coreId);
+        return error;
+    }
+
+    /* allocate sdram memory to share perfmeters data */
+    error = cm_PFM_allocatePerfmeterDataMemory(coreId, cm_DSP_GetState(coreId)->domainEE);
+    if (error != CM_OK) {
+        cm_delayedDestroyComponent(eeState[coreId].instance);
+        eeState[coreId].instance = (t_component_instance *)0;
+        cm_DSP_Shutdown(coreId);
+        return error;
+    }
+
+    /* set initial stack value */
+    cm_writeAttribute(eeState[coreId].instance, "rtos/scheduler/topOfStack", cm_DSP_getStackAddr(coreId));
+
+    /* set myCoreId for trace */
+    cm_writeAttribute(eeState[coreId].instance, "xti/myCoreId", coreId - 1);
+    
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    /* set myCoreId for prcmu if exist */
+    cm_writeAttribute(eeState[coreId].instance, "sleep/prcmu/myCoreId", coreId + 1);
+#endif
+
+    /* go go go ... */
+    cm_DSP_Start(coreId);
+    
+    /* Waiting for End Of Boot */
+    //TODO : remove infinite while loop
+    //TODO : to be paranoiac, add a read to serviceReasonOffset before starting core and check value is MPC_SERVICE_BOOT as it should be
+    {
+        while(cm_readAttributeNoError(eeState[coreId].instance, "rtos/commonpart/serviceReason") == MPC_SERVICE_BOOT)
+        {
+            volatile t_uint32 i;
+            for (i=0; i < 1000; i++);
+        }
+    }
+
+    /* set some attributes after boot to avoid being erase by mmdsp boot */
+    cm_writeAttribute(eeState[coreId].instance, "xti/traceActive", eeState[coreId].traceState);
+    cm_writeAttribute(eeState[coreId].instance, "rtos/commonpart/printLevel", eeState[coreId].printLevel);
+
+    cm_DSP_ConfigureAfterBoot(coreId);
+
+    return CM_OK;
+}
+
+/****************************************************************************/
+/* NAME:        cm_EEM_Close                                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Inform us that ee for coreId has been destroyed             */
+/*                                                                          */
+/* PARAMETERS:  id: dsp identifier                                          */
+/*                                                                          */
+/* RETURN:      none                                                        */
+/*                                                                          */
+/****************************************************************************/
+PUBLIC void cm_EEM_Close(t_nmf_core_id coreId)
+{
+    cm_DSP_setStackSize(coreId, 0);
+    cm_delayedDestroyComponent(eeState[coreId].instance);
+    eeState[coreId].instance = (t_component_instance *)0;
+    cm_PFM_deallocatePerfmeterDataMemory(coreId);
+
+    cm_DSP_Shutdown(coreId);
+}
+
+/****************************************************************************/
+/* NAME: cm_EEM_isStackUpdateNeed(                                          */
+/*                                t_nmf_core_id id,                         */
+/*                                t_nmf_ee_priority priority,               */
+/*                                t_uint32 isInstantiate,                   */
+/*                                t_uint32 needMinStackSize                 */
+/*                               )                                          */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Return a boolean to inform if a ee stack size update is need*/
+/*              when instantiate or destroying a component                  */
+/****************************************************************************/
+PUBLIC t_uint32 cm_EEM_isStackUpdateNeed(
+    t_nmf_core_id coreId,
+    t_nmf_ee_priority priority,
+    t_uint32 isInstantiate,
+    t_uint32 needMinStackSize)
+{
+    /* in case of SYNCHRONOUS_EXECUTIVE_ENGINE we only use currentStackSize[NMF_SCHED_BACKGROUND] */
+    if (eeState[coreId].executiveEngineId == SYNCHRONOUS_EXECUTIVE_ENGINE) {priority = NMF_SCHED_BACKGROUND;}
+    if (isInstantiate)
+    {
+        if (needMinStackSize > eeState[coreId].currentStackSize[priority]) {return TRUE;}
+    }
+    else
+    {
+        if (needMinStackSize == eeState[coreId].currentStackSize[priority]) {return TRUE;}
+    }
+
+    return FALSE;
+}
+
+/****************************************************************************/
+/* NAME: cm_EEM_UpdateStack(                                                */
+/*                          t_nmf_core_id id,                               */
+/*                          t_nmf_ee_priority priority,                     */
+/*                          t_uint32 needMinStackSize,                      */
+/*                          t_uint32 *pNewStackValue                        */
+/*                         )                                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: If cm_EEM_isStackUpdateNeed() has return true then caller   */
+/*              must inform EEM about new stack value for priority.         */
+/*              cm_EEM_UpdateStack() will return new global stack size to   */
+/*              provide to ee.                                              */
+/****************************************************************************/
+PUBLIC t_cm_error cm_EEM_UpdateStack(
+    t_nmf_core_id coreId,
+    t_nmf_ee_priority priority,
+    t_uint32 needMinStackSize,
+    t_uint32 *pNewStackValue)
+{
+    t_cm_error error;
+    t_uint32 recoveryStackSize = eeState[coreId].currentStackSize[priority];
+    t_uint32 i;
+
+    /* in case of SYNCHRONOUS_EXECUTIVE_ENGINE we only use currentStackSize[NMF_SCHED_BACKGROUND] */
+    if (eeState[coreId].executiveEngineId == SYNCHRONOUS_EXECUTIVE_ENGINE) {priority = NMF_SCHED_BACKGROUND;}
+    eeState[coreId].currentStackSize[priority] = needMinStackSize;
+    if (eeState[coreId].executiveEngineId == SYNCHRONOUS_EXECUTIVE_ENGINE) {*pNewStackValue = needMinStackSize;}
+    else
+    {
+        *pNewStackValue = 0;
+        for(i = NMF_SCHED_BACKGROUND; i < NMF_SCHED_URGENT + 1;i++)
+        {
+            *pNewStackValue += eeState[coreId].currentStackSize[i];
+        }
+    }
+
+    /* try to increase size of stack by modifying xram allocator size */
+    error = cm_DSP_setStackSize(coreId, *pNewStackValue);
+    if (error != CM_OK) {
+        eeState[coreId].currentStackSize[priority] = recoveryStackSize;
+    } else {
+        LOG_INTERNAL(1, "\n##### Stack update: size=%d, prio=%d on %s #####\n", *pNewStackValue, priority, cm_getDspName(coreId), 0, 0, 0);
+    }
+
+    return error;
+}
+
+/****************************************************************************/
+/* NAME: t_nmf_executive_engine_id(                                         */
+/*                          t_nmf_core_id id                                */
+/*                         )                                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: return executive engine load on id core.                    */
+/****************************************************************************/
+PUBLIC t_ee_state * cm_EEM_getExecutiveEngine(t_nmf_core_id coreId)
+{
+    return &eeState[coreId];
+}
+
+/****************************************************************************/
+/* NAME: cm_EEM_setTraceMode(                                               */
+/*                          t_nmf_core_id id,                               */
+/*                          t_uint32 state                                  */
+/*                         )                                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: activate/deactivate trace for ee running on id. In case ee  */
+/*          is not yet load then information is store.                      */
+/****************************************************************************/
+PUBLIC void cm_EEM_setTraceMode(t_nmf_core_id coreId, t_uint32 state)
+{
+    eeState[coreId].traceState = state;
+    if (eeState[coreId].instance)
+    {
+        if(cm_EEM_ForceWakeup(coreId) == CM_OK)
+        {
+            cm_writeAttribute(eeState[coreId].instance, "xti/traceActive", eeState[coreId].traceState);
+
+            cm_EEM_AllowSleep(coreId);
+        }
+    }
+}
+
+/****************************************************************************/
+/* NAME: cm_EEM_setPrintLevel(                                              */
+/*                          t_nmf_core_id id,                               */
+/*                          t_uint32 level                                  */
+/*                         )                                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: set print level for ee running on id. In case ee            */
+/*          is not yet load then information is store.                      */
+/****************************************************************************/
+PUBLIC void cm_EEM_setPrintLevel(t_nmf_core_id coreId, t_uint32 level)
+{
+    eeState[coreId].printLevel = level;
+    if (eeState[coreId].instance)
+    {
+        if(cm_EEM_ForceWakeup(coreId) == CM_OK)
+        {
+            cm_writeAttribute(eeState[coreId].instance, "rtos/commonpart/printLevel", eeState[coreId].printLevel);
+
+            cm_EEM_AllowSleep(coreId);
+        }
+    }
+}
+
+t_cm_error cm_EEM_ForceWakeup(t_nmf_core_id coreId)
+{
+    if(eeState[coreId].nbOfForceWakeup++ == 0)
+    {
+        t_cm_error error;
+
+        LOG_INTERNAL(2, "ARM: Try to wake up\n", 0, 0, 0, 0, 0, 0);
+
+        if (cm_DSP_GetState(coreId)->state != MPC_STATE_BOOTED)
+        {
+            return CM_MPC_NOT_RESPONDING;
+        }
+        else if ((error = cm_COMP_ULPForceWakeup(coreId)) != CM_OK)
+        {
+            if (error == CM_MPC_NOT_RESPONDING) {
+                ERROR("CM_MPC_NOT_RESPONDING: DSP %s can't be wakeup'ed\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+                cm_DSP_SetStatePanic(coreId);
+            }
+            return error;
+        }
+    }
+    return CM_OK;
+}
+
+void cm_EEM_AllowSleep(t_nmf_core_id coreId)
+{
+    if(--eeState[coreId].nbOfForceWakeup == 0)
+    {
+        LOG_INTERNAL(2, "ARM: Allow sleep\n", 0, 0, 0, 0, 0, 0);
+
+        if (cm_DSP_GetState(coreId)->state != MPC_STATE_BOOTED)
+        {
+        }
+        else if (cm_COMP_ULPAllowSleep(coreId) != CM_OK)
+        {
+            ERROR("CM_MPC_NOT_RESPONDING: DSP %s can't be allow sleep'ed\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+        }
+    }
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/chunk_mgr.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/chunk_mgr.h
new file mode 100644
index 00000000000..340301a9259
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/chunk_mgr.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef CHUNK_MGR_H_
+#define CHUNK_MGR_H_
+
+#include <cm/engine/memory/inc/remote_allocator.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+t_cm_error allocChunkPool(void);
+t_cm_error fillChunkPool(void);
+void freeChunkPool(void);
+
+/***************************************************************************/
+/*
+ * allocChunk
+ * param current  : Pointer on chunck to free
+ *
+ * Add a chunk in the chunck list
+ *
+ */
+/***************************************************************************/
+t_cm_chunk* allocChunk(void);
+
+/***************************************************************************/
+/*
+ * freeChunk
+ * param current  : Pointer on chunck to free
+ *
+ * Remove a chunk in the chunck list
+ *
+ */
+/***************************************************************************/
+void freeChunk(t_cm_chunk *chunk);
+
+#endif /*CHUNK_MGR_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/domain.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/domain.h
new file mode 100644
index 00000000000..bf75915c259
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/domain.h
@@ -0,0 +1,154 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/***************************************************************************/
+/* file    : domain.h
+ * author  : NMF team
+ * version : 1.0
+ *
+ * brief : NMF domain definitions
+ */
+/***************************************************************************/
+
+#ifndef DOMAIN_H_
+#define DOMAIN_H_
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+/*!
+ * \brief Domain type.
+ * \internal
+ * \ingroup CM_DOMAIN_API
+ */
+typedef enum {
+    DOMAIN_ANY = 0,
+    DOMAIN_NORMAL,
+    DOMAIN_SCRATCH_PARENT,
+    DOMAIN_SCRATCH_CHILD
+} t_cm_domain_type;
+
+/*!
+ * \brief Domain descriptor. Holds offsets for all memory types present in the system.
+ * \internal
+ * \ingroup CM_DOMAIN_API
+ */
+typedef struct {
+    t_cm_domain_memory            domain;      // the actual memory ranges
+    t_cm_domain_type              type;        // domain type
+    t_uint32                      refcount;    // reference counter for scratch domain dependencies
+    t_nmf_client_id               client;      // client id for cleaning
+
+    union {
+        struct {
+            t_memory_handle       handle;      // memory handle of the allocated chunk the covers the esram-data scratch region
+        } parent;
+        struct {
+            t_cm_allocator_desc  *alloc;       //allocator descriptor for the scratch domain
+            t_cm_domain_id        parent_ref;  //parent domain reference
+        } child;
+    } scratch;
+} t_cm_domain_desc;
+
+#ifdef DEBUG
+#define DOMAIN_DEBUG(handle) \
+    handle = handle & ~0xc0;
+#else
+#define DOMAIN_DEBUG(handle)
+#endif
+
+/*!
+ * \brief Domain descriptor array.
+ */
+extern t_cm_domain_desc domainDesc[];
+
+typedef struct {
+    t_cm_domain_id parentId;
+    t_cm_domain_id domainId;
+    t_cm_allocator_desc *allocDesc;
+} t_cm_domain_scratch_desc;
+
+extern t_cm_domain_scratch_desc domainScratchDesc[];
+
+typedef struct {
+    t_cm_system_address sdramCode;
+    t_cm_system_address sdramData;
+    t_cm_system_address esramCode;
+    t_cm_system_address esramData;
+} t_cm_domain_info;
+
+/*!
+ * \brief Init of the domain subsystem.
+ */
+PUBLIC t_cm_error cm_DM_Init(void);
+
+/*!
+ * \brief Clean-up of the domain subsystem.
+ */
+PUBLIC void cm_DM_Destroy(void);
+
+/*!
+ * \brief Domain creation.
+ *
+ * Allocates in slot in the domain descriptors array and copies segment infos from the domain
+ * parameter to the descriptor. The resulting handle is returned via @param handle.
+ *
+ * Returns: CM_DOMAIN_INVALID in case of error, otherwise CM_OK.
+ */
+PUBLIC t_cm_error cm_DM_CreateDomain(const t_nmf_client_id client, const t_cm_domain_memory *domain, t_cm_domain_id *handle);
+
+/*!
+ * \brief Scratch (or overlap) domain creation.
+ *
+ * Create a scratch domain, ie domain where allocation may overlap.
+ */
+PUBLIC t_cm_error cm_DM_CreateDomainScratch(const t_nmf_client_id client, const t_cm_domain_id parentId, const t_cm_domain_memory *domain, t_cm_domain_id *handle);
+
+/* !
+ * \brief Retrieve the coreId from a given domain. Utility.
+ */
+PUBLIC t_nmf_core_id cm_DM_GetDomainCoreId(const t_cm_domain_id domainId);
+
+/*!
+ * \brief Destroy all domains belonging to a given client.
+ */
+PUBLIC t_cm_error cm_DM_DestroyDomains(const t_nmf_client_id client);
+
+/*!
+ * \brief Destroy a given domain.
+ */
+PUBLIC t_cm_error cm_DM_DestroyDomain(t_cm_domain_id handle);
+
+/*!
+ * \brief Check if the handle is valid.
+ */
+PUBLIC t_cm_error cm_DM_CheckDomain(t_cm_domain_id handle, t_cm_domain_type type);
+PUBLIC t_cm_error cm_DM_CheckDomainWithClient(t_cm_domain_id handle, t_cm_domain_type type, t_nmf_client_id client);
+
+/*!
+ * \brief Memory allocation in a given domain, for a given memory type (see CM_AllocMpcMemory).
+ */
+PUBLIC t_memory_handle cm_DM_Alloc(t_cm_domain_id domainId, t_dsp_memory_type_id memType, t_uint32 size, t_cm_mpc_memory_alignment memAlignment, t_bool powerOn);
+
+/*!
+ * \brief Memory free using a given domain handle
+ */
+PUBLIC void cm_DM_FreeWithInfo(t_memory_handle memHandle, t_nmf_core_id *coreId, t_dsp_memory_type_id *memType, t_bool powerOff);
+
+/*!
+ * \brief Memory free using a given domain handle
+ */
+PUBLIC void cm_DM_Free(t_memory_handle memHandle, t_bool powerOff);
+
+/*!
+ * \brief Wrapper function for CM_GetMpcMemoryStatus.
+ */
+PUBLIC t_cm_error cm_DM_GetAllocatorStatus(t_cm_domain_id domainId, t_dsp_memory_type_id memType, t_cm_allocator_status *pStatus);
+
+PUBLIC t_cm_error cm_DM_GetDomainAbsAdresses(t_cm_domain_id domainId, t_cm_domain_info *info);
+
+#endif /* DOMAIN_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/domain_type.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/domain_type.h
new file mode 100644
index 00000000000..712d077d894
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/domain_type.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/***************************************************************************/
+/* file    : domain.h
+ * author  : NMF team
+ * version : 1.0
+ *
+ * brief : NMF domain definitions
+ */
+/***************************************************************************/
+
+#ifndef DOMAIN_TYPE_H_
+#define DOMAIN_TYPE_H_
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/memory/inc/memory_type.h>
+
+/*!
+ * \brief Domain identifier
+ * \ingroup CM_DOMAIN_API
+ */
+typedef t_uint8 t_cm_domain_id;
+
+/*!
+ * \brief Client identifier
+ *        0 (zero) is considered as an invalid or 'NO' client identifier
+ * \ingroup CM_DOMAIN_API
+ */
+typedef t_uint32 t_nmf_client_id;
+// TO BE REMOVED LATER, I guess, when default domains will be removed (PP)
+#define NMF_CORE_CLIENT (t_nmf_client_id)-1
+#define NMF_CURRENT_CLIENT (t_nmf_client_id)0
+
+typedef struct {
+    t_uint32 offset;       //!< offset relativ to segment start in memory (in bytes)
+    t_uint32 size;         //!< size in bytes of the domain segment
+} t_cm_domain_segment;
+
+/*!
+ * \brief Domain memory description structure
+ * \ingroup CM_DOMAIN_API
+ */
+typedef struct {
+    t_nmf_core_id         coreId;     //!< MMDSP Core Id for this domain (used for TCM-X and TCM-Y at instantiate)
+    t_cm_domain_segment   esramCode;  //!< ESRAM code segment
+    t_cm_domain_segment   esramData;  //!< ESRAM data segment
+    t_cm_domain_segment   sdramCode;  //!< SDRAM code segment
+    t_cm_domain_segment   sdramData;  //!< SDRAM data segment
+} t_cm_domain_memory;
+
+#define INIT_DOMAIN_SEGMENT {0, 0}
+#define INIT_DOMAIN {MASK_ALL8, INIT_DOMAIN_SEGMENT, INIT_DOMAIN_SEGMENT, INIT_DOMAIN_SEGMENT, INIT_DOMAIN_SEGMENT}
+
+
+#endif /* DOMAIN_TYPE_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/memory.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/memory.h
new file mode 100644
index 00000000000..c6d1fb2dfff
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/memory.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Internal Memory Management API.
+ *
+ * \defgroup MEMORY_INTERNAL Private Memory API.
+ *
+ */
+#ifndef __INC_MEMORY_H
+#define __INC_MEMORY_H
+
+#include <cm/engine/api/control/configuration_engine.h>
+#include <cm/engine/memory/inc/remote_allocator.h>
+
+#endif /* __INC_MEMORY_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/memory_type.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/memory_type.h
new file mode 100644
index 00000000000..91246f1ec0e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/memory_type.h
@@ -0,0 +1,152 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Public Component Manager Memory API type.
+ *
+ * This file contains the Component Manager API type for manipulating memory.
+ */
+#ifndef __INC_MEMORY_TYPE_H
+#define __INC_MEMORY_TYPE_H
+
+#include <cm/inc/cm_type.h>
+
+/*!
+ * @defgroup t_cm_mpc_memory_type t_cm_mpc_memory_type
+ * \brief Definition of symbols used to reference the various type of Media Processor Core adressable memory
+ * @{
+ * \ingroup MEMORY
+ */
+typedef t_uint8 t_cm_mpc_memory_type;                            //!< Fake enumeration type
+#define CM_MM_MPC_TCM16_X           ((t_cm_mpc_memory_type)0)
+#define CM_MM_MPC_TCM24_X           ((t_cm_mpc_memory_type)1)
+#define CM_MM_MPC_ESRAM16           ((t_cm_mpc_memory_type)2)
+#define CM_MM_MPC_ESRAM24           ((t_cm_mpc_memory_type)3)
+#define CM_MM_MPC_SDRAM16           ((t_cm_mpc_memory_type)4)
+#define CM_MM_MPC_SDRAM24           ((t_cm_mpc_memory_type)5)
+#define CM_MM_MPC_TCM16_Y           ((t_cm_mpc_memory_type)6)
+#define CM_MM_MPC_TCM24_Y           ((t_cm_mpc_memory_type)7)
+#define CM_MM_MPC_TCM16             CM_MM_MPC_TCM16_X
+#define CM_MM_MPC_TCM24             CM_MM_MPC_TCM24_X
+
+/* @} */
+
+/*!
+ * @defgroup t_cm_memory_alignment t_cm_memory_alignment
+ * \brief Definition of symbols used to constraint the alignment of the allocated memory
+ * @{
+ * \ingroup MEMORY
+ */
+typedef t_uint16 t_cm_memory_alignment;                            //!< Fake enumeration type
+#define CM_MM_ALIGN_NONE            ((t_cm_memory_alignment)0x00000000)
+#define CM_MM_ALIGN_BYTE            ((t_cm_memory_alignment)CM_MM_ALIGN_NONE)
+#define CM_MM_ALIGN_HALFWORD        ((t_cm_memory_alignment)0x00000001)
+#define CM_MM_ALIGN_WORD            ((t_cm_memory_alignment)0x00000003)
+#define CM_MM_ALIGN_2WORDS          ((t_cm_memory_alignment)0x00000007)
+#define CM_MM_ALIGN_16BYTES         ((t_cm_memory_alignment)0x0000000F)
+#define CM_MM_ALIGN_4WORDS          ((t_cm_memory_alignment)0x0000000F)
+#define CM_MM_ALIGN_AHB_BURST       ((t_cm_memory_alignment)0x0000000F)
+#define CM_MM_ALIGN_32BYTES         ((t_cm_memory_alignment)0x0000001F)
+#define CM_MM_ALIGN_8WORDS          ((t_cm_memory_alignment)0x0000001F)
+#define CM_MM_ALIGN_64BYTES         ((t_cm_memory_alignment)0x0000003F)
+#define CM_MM_ALIGN_16WORDS         ((t_cm_memory_alignment)0x0000003F)
+#define CM_MM_ALIGN_128BYTES        ((t_cm_memory_alignment)0x0000007F)
+#define CM_MM_ALIGN_32WORDS         ((t_cm_memory_alignment)0x0000007F)
+#define CM_MM_ALIGN_256BYTES        ((t_cm_memory_alignment)0x000000FF)
+#define CM_MM_ALIGN_64WORDS         ((t_cm_memory_alignment)0x000000FF)
+#define CM_MM_ALIGN_512BYTES        ((t_cm_memory_alignment)0x000001FF)
+#define CM_MM_ALIGN_128WORDS        ((t_cm_memory_alignment)0x000001FF)
+#define CM_MM_ALIGN_1024BYTES       ((t_cm_memory_alignment)0x000003FF)
+#define CM_MM_ALIGN_256WORDS        ((t_cm_memory_alignment)0x000003FF)
+#define CM_MM_ALIGN_2048BYTES       ((t_cm_memory_alignment)0x000007FF)
+#define CM_MM_ALIGN_512WORDS        ((t_cm_memory_alignment)0x000007FF)
+#define CM_MM_ALIGN_4096BYTES       ((t_cm_memory_alignment)0x00000FFF)
+#define CM_MM_ALIGN_1024WORDS       ((t_cm_memory_alignment)0x00000FFF)
+#define CM_MM_ALIGN_65536BYTES      ((t_cm_memory_alignment)0x0000FFFF)
+#define CM_MM_ALIGN_16384WORDS      ((t_cm_memory_alignment)0x0000FFFF)
+/* @} */
+
+/*!
+ * @defgroup t_cm_mpc_memory_alignment t_cm_mpc_memory_alignment
+ * \brief Definition of symbols used to constraint the alignment of the allocated mpc memory
+ * @{
+ * \ingroup MEMORY
+ */
+typedef t_uint16 t_cm_mpc_memory_alignment;                            //!< Fake enumeration type
+#define CM_MM_MPC_ALIGN_NONE        ((t_cm_mpc_memory_alignment)0x00000000)
+#define CM_MM_MPC_ALIGN_HALFWORD    ((t_cm_mpc_memory_alignment)0x00000001)
+#define CM_MM_MPC_ALIGN_WORD        ((t_cm_mpc_memory_alignment)0x00000003)
+#define CM_MM_MPC_ALIGN_2WORDS      ((t_cm_mpc_memory_alignment)0x00000007)
+#define CM_MM_MPC_ALIGN_4WORDS      ((t_cm_mpc_memory_alignment)0x0000000F)
+#define CM_MM_MPC_ALIGN_8WORDS      ((t_cm_mpc_memory_alignment)0x0000001F)
+#define CM_MM_MPC_ALIGN_16WORDS     ((t_cm_mpc_memory_alignment)0x0000003F)
+#define CM_MM_MPC_ALIGN_32WORDS     ((t_cm_mpc_memory_alignment)0x0000007F)
+#define CM_MM_MPC_ALIGN_64WORDS     ((t_cm_mpc_memory_alignment)0x000000FF)
+#define CM_MM_MPC_ALIGN_128WORDS    ((t_cm_mpc_memory_alignment)0x000001FF)
+#define CM_MM_MPC_ALIGN_256WORDS    ((t_cm_mpc_memory_alignment)0x000003FF)
+#define CM_MM_MPC_ALIGN_512WORDS    ((t_cm_mpc_memory_alignment)0x000007FF)
+#define CM_MM_MPC_ALIGN_1024WORDS   ((t_cm_mpc_memory_alignment)0x00000FFF)
+#define CM_MM_MPC_ALIGN_65536BYTES  ((t_cm_mpc_memory_alignment)0x0000FFFF)
+#define CM_MM_MPC_ALIGN_16384WORDS  ((t_cm_mpc_memory_alignment)0x0000FFFF)
+/* @} */
+
+/*!
+ * \brief Identifier of a memory handle
+ * \ingroup MEMORY
+ */
+typedef t_uint32 t_cm_memory_handle;
+
+/*!
+ * \brief Description of a memory segment
+ *
+ * <=> allocable addressable space
+ * \ingroup MEMORY
+ */
+typedef struct {
+    t_cm_system_address systemAddr; //!< Logical AND physical segment start address
+    t_uint32 size;                //!< segment size (in bytes)
+} t_nmf_memory_segment;
+#define INIT_MEMORY_SEGMENT {{0, 0}, 0}
+
+/*!
+ * \brief Definition of structure used for an allocator status
+ * \ingroup MEMORY
+ */
+typedef struct
+{
+    struct {
+        t_uint32 size;              //!< size of the allocator
+        /* Block counters */
+        t_uint16 used_block_number; //!< used block number
+        t_uint16 free_block_number; //!< free block number
+
+        /* Free memory min/max */
+        t_uint32 maximum_free_size; //!< maximum free size
+        t_uint32 minimum_free_size; //!< minimum free size
+
+        /* Accumulation of free and used memory */
+        t_uint32 accumulate_free_memory; //!< accumulate free memory
+        t_uint32 accumulate_used_memory; //!< accumulate used memory
+    } global;
+
+    struct {
+        t_uint32 size;       //!< size of the domain
+        t_uint32 maximum_free_size; //!< maximum free size in the given domain
+        t_uint32 minimum_free_size; //!< minimum free size in the given domain
+        t_uint32 accumulate_free_memory; //all free memory of the given domain
+        t_uint32 accumulate_used_memory; //all used memory of the given domain
+    } domain;
+
+    struct {
+        t_uint32 sizes[3];
+    } stack[NB_CORE_IDS];
+
+} t_cm_allocator_status;
+
+#endif /* __INC_MEMORY_TYPE_H */
+
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/migration.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/migration.h
new file mode 100644
index 00000000000..824d25374b3
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/migration.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Migration API.
+ *
+ * \defgroup
+ *
+ */
+#ifndef __INC_MIGRATION_H
+#define __INC_MIGRATION_H
+
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+typedef enum {
+    STATE_MIGRATED = 1,
+    STATE_NORMAL   = 0,
+} t_cm_migration_state;
+
+PUBLIC t_cm_error cm_migrate(const t_cm_domain_id srcShared, const t_cm_domain_id src, const t_cm_domain_id dst);
+
+PUBLIC t_cm_error cm_unmigrate(void);
+
+PUBLIC t_uint32 cm_migration_translate(t_dsp_segment_type segmentType, t_uint32 addr);
+
+PUBLIC void cm_migration_check_state(t_nmf_core_id coreId, t_cm_migration_state expected);
+
+#endif /* __INC_MIGRATION_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator.h
new file mode 100644
index 00000000000..e639c94cb46
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator.h
@@ -0,0 +1,262 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ *
+ * \note: In this module, we assume that parameters were checked !!
+ */
+#ifndef __REMOTE_ALLOCATOR_H_
+#define __REMOTE_ALLOCATOR_H_
+
+/*
+ * Include
+ */
+#include <cm/inc/cm_type.h>
+#include <cm/engine/memory/inc/memory_type.h>
+
+
+/*
+ * Description of the memory block status
+ */
+typedef enum {
+    MEM_USED = 0,     /* Memory block is used */
+    MEM_FREE = 1     /* Memory block is free */
+} t_mem_status;
+
+/*
+ * Chunk structure.
+ */
+struct cm_allocator_desc;
+typedef struct chunk_struct
+{
+    /* Double linked list of chunks */
+    struct chunk_struct *prev;
+    struct chunk_struct *next;
+
+    /* Double linked list of free memory */
+    struct chunk_struct *prev_free_mem;
+    struct chunk_struct *next_free_mem;
+
+    /* Offset of the block memory */
+    t_uint32 offset;
+
+    /* Size of the block memory */
+    t_cm_size size;
+
+    /* Status of the block memory */
+    t_mem_status status;
+
+    /* User data */
+    t_uint16 userData;
+
+    /* Alloc debug info*/
+    t_uint32 domainId;
+
+    /* Alloc desc backlink */
+    struct cm_allocator_desc *alloc;
+} t_cm_chunk;
+
+/*!
+ * \brief Identifier of an internal memory handle
+ * \ingroup MEMORY_INTERNAL
+ */
+typedef t_cm_chunk* t_memory_handle;
+
+#define INVALID_MEMORY_HANDLE ((t_cm_chunk*)NULL)
+
+
+/*
+ * Context structure
+ */
+#define BINS                    63
+
+//TODO, juraj, add memType to alloc struct ?
+typedef struct cm_allocator_desc {
+    const char     *pAllocName;             /* Name of the allocator */
+    t_uint32        size;                   /* Size of the allocator */
+    t_cm_chunk     *chunks;                 /* Array of chunk */
+    t_cm_chunk     *free_mem_chunks[BINS];  /* List of free memory */
+    struct cm_allocator_desc* next;         /* List of allocator */
+} t_cm_allocator_desc;
+
+int bin_index(unsigned int sz);
+
+/*
+ * Functions
+ */
+/*!
+ * \brief Create a new allocator for a piece of memory (hw mapped (xram, yram))
+ * Any further allocation into this piece of memory will return an offset inside it.
+ * (a constant offset value can be added to this offset)
+ *
+ * \retval t_cm_allocator_desc*  new memory allocator identifier
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_cm_allocator_desc* cm_MM_CreateAllocator(
+        t_cm_size size,                     //!< [in] Size of the addressable space in bytes
+        t_uint32 offset,                    //!< [in] Constant offset to add to each allocated block base address
+        const char* name                    //!< [in] Name of the allocator
+        );
+
+/*!
+ * \brief Free a memory allocator descriptor
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_cm_error cm_MM_DeleteAllocator(
+        t_cm_allocator_desc* alloc          //!< [in] Identifier of the memory allocator to be freed
+        );
+
+
+/*!
+ * \brief Resize an allocator to the size value.
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_cm_error cm_MM_ResizeAllocator(
+        t_cm_allocator_desc* alloc,         //!< [in] Identifier of the memory allocator used to allocate the piece of memory
+        t_cm_size size                      //!< [in] Size of the addressable space in allocDesc granularity
+        );
+
+/*!
+ * \brief Check validity of a user handle
+ */
+t_cm_error cm_MM_getValidMemoryHandle(t_cm_memory_handle handle, t_memory_handle* validHandle);
+
+/*!
+ * \brief Wrapper routine to allocate some memory into a given allocator
+ *
+ * \retval t_memory_handle handle on the new allocated piece of memory
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_memory_handle cm_MM_Alloc(
+        t_cm_allocator_desc* alloc,             //!< [in] Identifier of the memory allocator
+        t_cm_size size,                         //!< [in] Size of the addressable space
+        t_cm_memory_alignment memAlignment,     //!< [in] Alignment constraint
+        t_uint32 seg_offset,                    //!< [in] Offset of range where allocating
+        t_uint32 seg_size,                      //!< [in] Size of range where allocating
+        t_uint32 domainId
+        );
+
+
+/*!
+ * \brief Routine to reallocate memory for a given handle
+ *
+ * Routine to reallocate memory for a given handle. The chunk can be extended or shrinked in both
+ * directions - top and bottom, depending on the offset and size arguments.
+ *
+ * \retval t_memory_handle handle on the reallocated piece of memory
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_memory_handle cm_MM_Realloc(
+                t_cm_allocator_desc* alloc,
+                const t_cm_size size,
+                const t_uint32 offset,
+                const t_cm_memory_alignment memAlignment,
+                const t_memory_handle handle);
+/*!
+ * \brief Frees the allocated chunk
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC void cm_MM_Free(
+        t_cm_allocator_desc* alloc,             //!< [in] Identifier of the memory allocator
+        t_memory_handle memHandle               //!< [in] Memory handle to free
+        );
+
+
+/*!
+ * \brief Get the allocator status
+ *
+ * \param[in] alloc Identifier of the memory allocator
+ * \param[out] pStatus Status of the allocator
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_cm_error cm_MM_GetAllocatorStatus(t_cm_allocator_desc* alloc, t_uint32 offset, t_uint32 size, t_cm_allocator_status *pStatus);
+
+/*!
+ * \brief Returns the offset into a given memory allocator of an allocated piece of memory
+ *
+ * \param[in] memHandle handle on the given memory
+ *
+ * \retval t_uint32  offset into the given memory allocator
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_uint32 cm_MM_GetOffset(t_memory_handle memHandle);
+
+
+/*!
+ * \brief Returns the size in word size for a given memory allocator of an allocated piece of memory
+ *
+ * \param[in] memHandle handle on the given memory
+ *
+ * \retval t_uint32  size in wordsize for the given memory allocator
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_uint32 cm_MM_GetSize(t_memory_handle memHandle);
+
+/*!
+ * \brief Returns the size in bytes for a given memory allocator
+ *
+ * \param[in] allocDesc Identifier of the memory allocator
+ * \retval size
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_uint32 cm_MM_GetAllocatorSize(t_cm_allocator_desc* allocDesc);
+
+
+/*!
+ * \brief Set the user data of an allocated piece of memory
+ *
+ * \param[in] memHandle handle on the given memory
+ * \param[in] userData UsedData of the given memory piece
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC void cm_MM_SetMemoryHandleUserData (t_memory_handle memHandle, t_uint16 userData);
+
+
+/*!
+ * \brief Return the user data of an allocated piece of memory
+ *
+ * \param[in] memHandle handle on the given memory
+ * \param[out] pUserData returned UsedData of the given memory piece
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC void cm_MM_GetMemoryHandleUserData(t_memory_handle memHandle, t_uint16 *pUserData, t_cm_allocator_desc **alloc);
+
+/*!
+ * \brief Dump chunkd in the range of [start:end]
+ *
+ * \param[in] alloc Allocator descriptor
+ * \param[in] start Range start
+ * \param[in] end Range end
+ *
+ * \retval void
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC void cm_MM_DumpMemory(t_cm_allocator_desc* alloc, t_uint32 start, t_uint32 end);
+
+#endif /* _REMOTE_ALLOCATOR_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator_utils.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator_utils.h
new file mode 100644
index 00000000000..ce99e4d7a94
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator_utils.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef REMOTE_ALLOCATOR_UTILS_H_
+#define REMOTE_ALLOCATOR_UTILS_H_
+
+#include <cm/engine/memory/inc/remote_allocator.h>
+#include <cm/engine/memory/inc/chunk_mgr.h>
+
+typedef enum {
+    FREE_CHUNK_BEFORE,
+    FREE_CHUNK_AFTER,
+} t_mem_split_position;
+
+
+PUBLIC void updateFreeList(t_cm_allocator_desc* alloc, t_cm_chunk* chunk);
+
+PUBLIC void linkChunk(t_cm_chunk* prev,t_cm_chunk* add);
+PUBLIC void unlinkChunk(t_cm_allocator_desc* alloc,t_cm_chunk* current);
+
+PUBLIC void unlinkFreeMem(t_cm_allocator_desc* alloc,t_cm_chunk* current);
+PUBLIC void linkFreeMemBefore(t_cm_chunk* add, t_cm_chunk* next);
+PUBLIC void linkFreeMemAfter(t_cm_chunk* prev,t_cm_chunk* add);
+
+PUBLIC void mergeChunk(t_cm_allocator_desc* alloc,t_cm_chunk *c,t_cm_chunk *destroy);
+PUBLIC t_cm_chunk* splitChunk(t_cm_allocator_desc* alloc, t_cm_chunk *chunk, t_uint32 offset, t_mem_split_position position);
+
+#endif /*REMOTE_ALLOCATOR_UTILS_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/chunk_mgr.c b/drivers/staging/nmf-cm/cm/engine/memory/src/chunk_mgr.c
new file mode 100644
index 00000000000..26c00cdeb77
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/chunk_mgr.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ * Include
+ */
+#include <cm/inc/cm_type.h>
+#include "../inc/chunk_mgr.h"
+#include <cm/engine/trace/inc/trace.h>
+
+#define CHUNKS_PER_PAGE 500
+#define CHUNK_THRESOLD 5
+
+struct t_page_chuncks {
+    struct t_page_chuncks   *nextPage;
+  //  unsigned int            freeChunkInPage;
+    t_cm_chunk              chunks[CHUNKS_PER_PAGE];
+};
+
+static struct t_page_chuncks   *firstPage;
+
+static unsigned int            freeChunks;
+static t_cm_chunk              *firstFreeChunk;
+
+t_cm_chunk* allocChunk()
+{
+    t_cm_chunk* chunk = firstFreeChunk;
+
+    firstFreeChunk = chunk->next;
+
+    chunk->next_free_mem    = 0;
+    chunk->prev_free_mem    = 0;
+    chunk->prev             = 0;
+    chunk->next             = 0;
+    chunk->status           = MEM_FREE;
+    // chunk->offset = 0;
+    // chunk->size   = 0;
+    // chunk->alloc  = 0;
+    // chunk->userData = 0;
+
+    freeChunks--;
+
+    return chunk;
+}
+
+void freeChunk(t_cm_chunk* chunk)
+{
+    // Link chunk in free list
+    chunk->next = firstFreeChunk;
+    firstFreeChunk = chunk;
+
+    // Increase counter
+    freeChunks++;
+}
+
+t_cm_error allocChunkPool(void)
+{
+    struct t_page_chuncks* newPage;
+    int i;
+
+    newPage = (struct t_page_chuncks*)OSAL_Alloc(sizeof(struct t_page_chuncks));
+    if(newPage == NULL)
+        return CM_NO_MORE_MEMORY;
+
+    // Link page
+    newPage->nextPage = firstPage;
+    firstPage = newPage;
+
+    // Put chunk in free list
+    for(i = 0; i < CHUNKS_PER_PAGE; i++)
+        freeChunk(&newPage->chunks[i]);
+
+    return CM_OK;
+}
+
+t_cm_error fillChunkPool(void)
+{
+    if(freeChunks < CHUNK_THRESOLD)
+        return allocChunkPool();
+
+    return CM_OK;
+}
+
+void freeChunkPool(void)
+{
+    while(firstPage != NULL)
+    {
+        struct t_page_chuncks* tofree = firstPage;
+        firstPage = firstPage->nextPage;
+        OSAL_Free(tofree);
+    }
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/domain.c b/drivers/staging/nmf-cm/cm/engine/memory/src/domain.c
new file mode 100644
index 00000000000..61cb25b39ec
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/domain.c
@@ -0,0 +1,590 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <inc/nmf-limits.h>
+
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/memory/inc/migration.h>
+#include <cm/engine/memory/inc/chunk_mgr.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/power_mgt/inc/power.h>
+
+/*
+ * domain_memory structure is all we need
+ */
+#define MAX_USER_DOMAIN_NB 64
+#define MAX_SCRATCH_DOMAIN_NB 16
+
+t_cm_domain_desc domainDesc[MAX_USER_DOMAIN_NB];
+t_cm_domain_scratch_desc domainScratchDesc[MAX_SCRATCH_DOMAIN_NB];
+
+static t_cm_allocator_desc *cm_DM_getAllocator(t_cm_domain_id domainId, t_dsp_memory_type_id memType);
+static void cm_DM_DomainError(const t_cm_domain_id parentId, const t_nmf_client_id client);
+
+#define INIT_DOMAIN_STRUCT(domainDesc)        do {       \
+    domainDesc.client                   = 0;             \
+    domainDesc.type                     = DOMAIN_NORMAL; \
+    domainDesc.refcount                 = 0;             \
+    domainDesc.domain.coreId            = MASK_ALL8;     \
+    domainDesc.domain.esramCode.offset  = 0;             \
+    domainDesc.domain.esramCode.size    = 0;             \
+    domainDesc.domain.esramData.offset  = 0;             \
+    domainDesc.domain.esramData.size    = 0;             \
+    domainDesc.domain.sdramCode.offset  = 0;             \
+    domainDesc.domain.sdramCode.size    = 0;             \
+    domainDesc.domain.sdramData.offset  = 0;             \
+    domainDesc.domain.sdramData.size    = 0;             \
+    domainDesc.scratch.parent.handle    = 0;             \
+    domainDesc.scratch.child.alloc      = 0;             \
+    domainDesc.scratch.child.parent_ref = 0;             \
+    } while (0)
+
+#define FIND_DOMAIN_ID(domainId)                                                           \
+    {                                                                                      \
+        domainId = 0;                                                                      \
+        while (domainDesc[domainId].client != 0 && domainId < MAX_USER_DOMAIN_NB) {     \
+            domainId++;                                                                    \
+        }                                                                                  \
+        if (domainId >= MAX_USER_DOMAIN_NB) {                                              \
+            return CM_INTERNAL_DOMAIN_OVERFLOW;                                            \
+        }                                                                                  \
+    }
+
+#define FIND_SCRATCH_DOMAIN_ID(domainId)                                                   \
+    {                                                                                      \
+        domainId = 0;                                                                      \
+        while (domainScratchDesc[domainId].allocDesc != 0 && domainId < MAX_SCRATCH_DOMAIN_NB) {     \
+            domainId++;                                                                    \
+        }                                                                                  \
+        if (domainId >= MAX_SCRATCH_DOMAIN_NB) {                                           \
+            return CM_INTERNAL_DOMAIN_OVERFLOW;                                            \
+        }                                                                                  \
+    }
+
+PUBLIC t_cm_error cm_DM_CheckDomain(t_cm_domain_id handle, t_cm_domain_type type)
+{
+    if ((handle <= 3)
+            || (handle >= MAX_USER_DOMAIN_NB)) { //remember, domain[0-3] are reserved
+        return CM_INVALID_DOMAIN_HANDLE;
+    }
+
+    if (domainDesc[handle].client == 0) {
+        return CM_INVALID_DOMAIN_HANDLE;
+    }
+
+    if (type != DOMAIN_ANY) {
+        if (domainDesc[handle].type != type) {
+            return CM_INVALID_DOMAIN_HANDLE;
+        }
+    }
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_DM_CheckDomainWithClient(t_cm_domain_id handle, t_cm_domain_type type, t_nmf_client_id client)
+{
+    t_cm_error error;
+
+    if((error = cm_DM_CheckDomain(handle, type)) != CM_OK)
+        return error;
+
+#ifdef CHECK_TO_BE_REACTIVATED_IN_2_11
+    if(domainDesc[handle].client != client)
+    {
+        ERROR("CM_DOMAIN_VIOLATION: domain %d created by client %d not usable by client %d.", handle, domainDesc[handle].client, client, 0, 0, 0);
+        return CM_DOMAIN_VIOLATION;
+    }
+#endif
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_DM_Init(void)
+{
+    t_cm_error error;
+
+    int i = 0;
+    for(i = 0; i < MAX_USER_DOMAIN_NB; i++) {
+        INIT_DOMAIN_STRUCT(domainDesc[i]);
+    }
+
+    //domains[0-3] are reserved - allows to catch some cases of incorrect usage,
+    //especially when user uses coreId instead of domainId, ie id = 1, 2, 3
+    domainDesc[0].client = NMF_CORE_CLIENT;
+    domainDesc[1].client = NMF_CORE_CLIENT;
+    domainDesc[2].client = NMF_CORE_CLIENT;
+    domainDesc[3].client = NMF_CORE_CLIENT;
+
+    for(i = 0; i < MAX_SCRATCH_DOMAIN_NB; i++) {
+        domainScratchDesc[i].domainId  = 0;
+        domainScratchDesc[i].parentId  = 0;
+        domainScratchDesc[i].allocDesc = 0;
+    }
+
+    // Alloc twice for having comfortable chunk
+    if((error = allocChunkPool()) != CM_OK)
+        return error;
+    if((error = allocChunkPool()) != CM_OK)
+    {
+        freeChunkPool();
+        return error;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC void cm_DM_Destroy(void)
+{
+    //cm_DM_Init();
+    freeChunkPool();
+}
+
+PUBLIC t_nmf_core_id cm_DM_GetDomainCoreId(const t_cm_domain_id domainId)
+{
+
+    return domainDesc[domainId].domain.coreId;
+}
+
+#if 0
+static t_uint32 cm_DM_isSegmentOverlaping(const t_cm_domain_segment *d0, const t_cm_domain_segment *d1)
+{
+    t_uint32 min0 = d0->offset;
+    t_uint32 max0 = d0->offset + d0->size;
+    t_uint32 min1 = d1->offset;
+    t_uint32 max1 = d1->offset + d1->size;
+
+    if ( (min0 < min1) && (min1 < max0) ){    /* min0 < min1 < max0  OR  min1 in [min0:max0] */
+        return 1;
+    }
+    if ( (min1 < min0) && (min0 <= max1) ){   /* min1 < min0 < max0  OR  min0 in [min1:max1] */
+        return 1;
+    }
+
+    return 0;
+}
+{
+    ...
+
+    t_uint32 i;
+    //check non-overlapp with other domains
+    for (i = 0; i < MAX_USER_DOMAIN_NB; i++) {
+        if (domainDesc[i].client != 0) {
+            if (cm_DM_isSegmentOverlaping(&domainDesc[i].domain.esramData, &domain->esramData)) {
+                return CM_DOMAIN_OVERLAP;
+            }
+            /*
+            if (cm_DM_isSegmentOverlaping(&domainDesc[i].domain.esramData, &domain->esramData)) {
+                return CM_DOMAIN_OVERLAP;
+            }
+            */
+        }
+    }
+
+    ...
+}
+#endif
+
+PUBLIC t_cm_error cm_DM_CreateDomain(const t_nmf_client_id client, const t_cm_domain_memory *domain, t_cm_domain_id *handle)
+{
+    t_cm_domain_id domainId;
+    FIND_DOMAIN_ID(domainId);
+
+    if (client == 0)
+        return CM_INVALID_PARAMETER;
+
+    if (domain->coreId > LAST_CORE_ID)
+        return CM_INVALID_DOMAIN_DEFINITION;
+
+    //FIXME, juraj, check invalid domain definition
+    domainDesc[domainId].client = client;
+    domainDesc[domainId].domain = *domain;
+
+    *handle = domainId;
+
+    return CM_OK;
+}
+
+//TODO, juraj, add assert to cm_MM_GetOffset(), if domain is scratch parent
+PUBLIC t_cm_error cm_DM_CreateDomainScratch(const t_nmf_client_id client, const t_cm_domain_id parentId, const t_cm_domain_memory *domain, t_cm_domain_id *handle)
+{
+    t_cm_error error;
+    t_memory_handle memhandle;
+    t_cm_allocator_desc *alloc;
+    t_uint32 parentMin, parentMax;
+    t_uint32 scratchMin, scratchMax;
+
+    /* check if the parent domain exists */
+    /* parent could be DOMAIN_NORMAL (1st call) or DOMAIN_SCRATCH_PARENT (other calls) */
+    if ((error = cm_DM_CheckDomain(parentId, DOMAIN_ANY)) != CM_OK) {
+        return error;
+    }
+
+    parentMin = domainDesc[parentId].domain.esramData.offset;
+    parentMax = domainDesc[parentId].domain.esramData.offset + domainDesc[parentId].domain.esramData.size;
+    scratchMin = domain->esramData.offset;
+    scratchMax = domain->esramData.offset + domain->esramData.size;
+    /* check if the scratch domain respects the parent domain (esram data only )*/
+    if ( (parentMin > scratchMin) || (parentMax < scratchMax) ) {
+        return CM_INVALID_DOMAIN_DEFINITION;
+    }
+
+    /* create the scratch domain */
+    if ((error = cm_DM_CreateDomain(client, domain, handle)) != CM_OK) {
+        return error;
+    }
+
+    /* check if this is the first scratch domain */
+    if (domainDesc[parentId].scratch.parent.handle == 0) {
+        /* 1st scratch domain */
+        t_cm_domain_segment tmp;
+
+        /* reserve the zone for the scratch domain */
+        tmp = domainDesc[parentId].domain.esramData;
+        domainDesc[parentId].domain.esramData = domain->esramData;
+        memhandle = cm_DM_Alloc(parentId, ESRAM_EXT16, domain->esramData.size / 2, CM_MM_ALIGN_NONE, FALSE); //note byte to 16bit-word conversion
+        domainDesc[parentId].domain.esramData = tmp;
+        if (memhandle == 0) {
+            cm_DM_DestroyDomain(*handle);
+            cm_DM_DomainError(parentId, client);
+            return CM_NO_MORE_MEMORY;
+        }
+
+        domainDesc[parentId].type = DOMAIN_SCRATCH_PARENT;
+        domainDesc[parentId].refcount = 0; //reinit the refcount
+        domainDesc[parentId].scratch.parent.handle = memhandle;
+
+    } else {
+        /* nth scratch domain */
+        t_uint32 i;
+        t_uint32 oldMin = domainDesc[parentId].domain.esramData.offset + domainDesc[parentId].domain.esramData.offset;
+        t_uint32 oldMax = 0;
+
+        /* compute the new scratch zone size */
+        for(i = 0; i < MAX_USER_DOMAIN_NB; i++) {
+            if ((domainDesc[i].type == DOMAIN_SCRATCH_CHILD) && (domainDesc[i].scratch.child.parent_ref == parentId)) {
+                /* ok, here we have a scratch domain created from the same child domain */
+                t_uint32 min = domainDesc[i].domain.esramData.offset;
+                t_uint32 max = domainDesc[i].domain.esramData.offset + domainDesc[i].domain.esramData.size;
+
+                oldMin = (min < oldMin)?min:oldMin;
+                oldMax = (max > oldMax)?max:oldMax;
+            }
+        }
+
+        /* resize the scratch zone */
+        if ((oldMin > scratchMin) || (oldMax < scratchMax)) {
+            t_uint32 newMin = (oldMin > scratchMin)?scratchMin:oldMin;
+            t_uint32 newMax = (oldMax < scratchMax)?scratchMax:oldMax;
+            t_uint16 userData;
+
+            /* save user data lost during realloc, user data do not change */
+            cm_MM_GetMemoryHandleUserData(domainDesc[parentId].scratch.parent.handle, &userData, 0);
+            memhandle = cm_MM_Realloc(cm_DM_getAllocator(parentId, ESRAM_EXT16), newMax - newMin, newMin,
+                                      CM_MM_MPC_ALIGN_NONE, domainDesc[parentId].scratch.parent.handle);
+            if (memhandle == 0) {
+                /* failed to extend the zone */
+                cm_DM_DestroyDomain(*handle);
+                cm_DM_DomainError(parentId, client);
+                return CM_NO_MORE_MEMORY;
+            }
+
+            cm_MM_SetMemoryHandleUserData(memhandle, userData);
+            domainDesc[parentId].scratch.parent.handle = memhandle;
+        }
+    }
+
+    /* create esram-data allocator in the scratch domain */
+    alloc = cm_MM_CreateAllocator(domainDesc[*handle].domain.esramData.size,
+                                  domainDesc[*handle].domain.esramData.offset,
+                                  "scratch");
+
+    domainDesc[*handle].type = DOMAIN_SCRATCH_CHILD;
+    domainDesc[*handle].scratch.child.parent_ref = parentId;
+    domainDesc[*handle].scratch.child.alloc = alloc;
+    domainDesc[parentId].refcount++;
+
+    return error;
+}
+
+PUBLIC t_cm_error cm_DM_DestroyDomains(const t_nmf_client_id client)
+{
+    t_cm_domain_id handle;
+    t_cm_error error, status=CM_OK;
+
+    for (handle=0; handle<MAX_USER_DOMAIN_NB; handle++) {
+        if ((domainDesc[handle].client == client)
+            && ((error=cm_DM_DestroyDomain(handle)) != CM_OK)) {
+            LOG_INTERNAL(0, "Error (%d) destroying remaining domainId %d for client %u\n", error, handle, client, 0, 0, 0);
+            status = error;
+        }
+    }
+    return status;
+}
+
+PUBLIC t_cm_error cm_DM_DestroyDomain(t_cm_domain_id handle)
+{
+    t_cm_error error = CM_OK;
+    t_uint32 i;
+
+    if ((error = cm_DM_CheckDomain(handle, DOMAIN_ANY)) != CM_OK) {
+        return error;
+    }
+
+    //forbid destruction of cm domains
+    //if (handle == cm_DSP_GetState(domainDesc[handle].domain.coreId)->domainEE)
+    //    return CM_INVALID_DOMAIN_HANDLE;
+
+    /* loop all components and check if there are still components instantiated with this handle */
+    //actually this check is redundant with the usage counters as component instantiations allocate memory
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	    if (NULL != componentEntry(i) && componentEntry(i)->domainId == handle) {
+            return CM_ILLEGAL_DOMAIN_OPERATION;
+        }
+    }
+
+    //perform check based on usage counters
+    if (domainDesc[handle].refcount != 0) {
+        return CM_ILLEGAL_DOMAIN_OPERATION;
+    }
+
+    if (domainDesc[handle].type == DOMAIN_SCRATCH_PARENT) {
+        return CM_ILLEGAL_DOMAIN_OPERATION; //parent destroyed implicitly with the last scratch
+    } else if (domainDesc[handle].type == DOMAIN_SCRATCH_CHILD) {
+        t_cm_allocator_status status;
+        t_cm_domain_id parentId = domainDesc[handle].scratch.child.parent_ref;
+
+        cm_MM_GetAllocatorStatus(domainDesc[handle].scratch.child.alloc, 0, 0xffff, &status);
+        if (status.global.accumulate_used_memory != 0) {
+            //something is still allocated
+            return CM_ILLEGAL_DOMAIN_OPERATION;
+        }
+
+        domainDesc[parentId].refcount--;
+        cm_MM_DeleteAllocator(domainDesc[handle].scratch.child.alloc); //returns no error
+
+        if (domainDesc[parentId].refcount == 0) {
+            /* last scratch domain */
+            cm_DM_Free(domainDesc[parentId].scratch.parent.handle, FALSE);
+            domainDesc[parentId].scratch.parent.handle = 0;
+            domainDesc[parentId].type = DOMAIN_NORMAL;
+        } else {
+            /* other child scratch domains exist, check if the reserved zone needs resize, ie reduce */
+
+            t_uint32 i;
+            /* init oldMin and oldMax to values we are sure will get overwritten below */
+            t_uint32 oldMin = 0xffffffff;
+            t_uint32 oldMax = 0x0;
+            t_uint32 scratchMin = domainDesc[handle].domain.esramData.offset;
+            t_uint32 scratchMax = domainDesc[handle].domain.esramData.offset + domainDesc[handle].domain.esramData.size;
+            t_memory_handle memhandle;
+
+            /* compute the remaining reserved zone size */
+            for(i = 0; i < MAX_USER_DOMAIN_NB; i++) {
+                if (i == handle)
+                    continue; //do not consider the current domain to be destroyed later in this function
+                if ((domainDesc[i].type == DOMAIN_SCRATCH_CHILD) && (domainDesc[i].scratch.child.parent_ref == parentId)) {
+                    /* ok, here we have a scratch domain created from the same child domain */
+                    t_uint32 min = domainDesc[i].domain.esramData.offset;
+                    t_uint32 max = domainDesc[i].domain.esramData.offset + domainDesc[i].domain.esramData.size;
+
+                    oldMin = (min < oldMin)?min:oldMin;
+                    oldMax = (max > oldMax)?max:oldMax;
+                }
+            }
+
+            /* resize the scratch zone */
+            if ((oldMin > scratchMin) || (oldMax < scratchMax)) {
+                t_uint16 userData;
+
+                /* save user data lost during realloc, user data do not change */
+                cm_MM_GetMemoryHandleUserData(domainDesc[parentId].scratch.parent.handle, &userData, 0);
+                memhandle = cm_MM_Realloc(cm_DM_getAllocator(parentId, ESRAM_EXT16), oldMax - oldMin, oldMin,
+                                          CM_MM_MPC_ALIGN_NONE, domainDesc[parentId].scratch.parent.handle);
+                CM_ASSERT(memhandle); //the realloc shouldn't fail..
+
+                cm_MM_SetMemoryHandleUserData(memhandle, userData);
+                domainDesc[parentId].scratch.parent.handle = memhandle;
+            }
+        }
+    }
+
+    //reset the domain desc
+    INIT_DOMAIN_STRUCT(domainDesc[handle]);
+
+    return CM_OK;
+}
+
+/*
+ *   - if the domainId is scratch parent, all allocations are done as in normal domains
+ *   - if the domainId is scratch child
+ *          if allocation type is esram, retrieve the allocator from the domainDesc
+ *          else allocation is done as for normal domain
+ *   - if the domainId is normal, allocator is retrieved from mpcDesc via cm_DSP_GetAllocator()
+ */
+static t_cm_allocator_desc *cm_DM_getAllocator(t_cm_domain_id domainId, t_dsp_memory_type_id memType)
+{
+    t_cm_allocator_desc *alloc = 0;
+
+    if ((domainDesc[domainId].type == DOMAIN_SCRATCH_CHILD)
+        && ((memType == ESRAM_EXT16) || (memType == ESRAM_EXT24))) {
+        alloc = domainDesc[domainId].scratch.child.alloc;
+    } else {
+        alloc = cm_DSP_GetAllocator(domainDesc[domainId].domain.coreId, memType);
+    }
+
+    return alloc;
+}
+
+void START(void);
+void END(const char*);
+
+//TODO, juraj, alloc would need to return finer errors then 0
+PUBLIC t_memory_handle cm_DM_Alloc(t_cm_domain_id domainId, t_dsp_memory_type_id memType, t_uint32 wordSize, t_cm_mpc_memory_alignment memAlignment,  t_bool powerOn)
+{
+    t_nmf_core_id coreId = domainDesc[domainId].domain.coreId;
+    t_memory_handle handle;
+    t_cm_allocator_desc *alloc;
+    t_uint32 offset;
+    t_uint32 size;
+
+    cm_DSP_GetInternalMemoriesInfo(domainId, memType, &offset, &size);
+
+    if ((alloc = cm_DM_getAllocator(domainId, memType)) == 0) {
+        return 0;
+    }
+
+    handle = cm_MM_Alloc(alloc,
+            cm_DSP_ConvertSize(memType, wordSize),
+            (t_cm_memory_alignment) memAlignment,
+            offset, size, domainId);
+
+    if(handle != INVALID_MEMORY_HANDLE)
+    {
+        cm_MM_SetMemoryHandleUserData(handle, (coreId << SHIFT_BYTE1) | (memType << SHIFT_BYTE0));
+
+        if (powerOn) {
+            // [Pwr] The associated power domain can be enabled only after the Alloc request.
+            //       Associated MPC memory chunk is not accessed (Remote allocator feature)
+            cm_PWR_EnableMemory(
+                    coreId,
+                    memType,
+                    /*
+                     * Compute physical address based on cm_DSP_GetHostSystemAddress but in optimized way
+                     * -> See it for information
+                     * -> Note TCM memory is not correctly compute, but it's not used
+                     */
+                    cm_DSP_GetState(coreId)->allocator[memType]->baseAddress.physical + cm_MM_GetOffset(handle),
+                    cm_MM_GetSize(handle));
+        }
+    }
+
+    return handle;
+}
+
+PUBLIC void cm_DM_FreeWithInfo(t_memory_handle memHandle, t_nmf_core_id *coreId, t_dsp_memory_type_id *memType, t_bool powerOff)
+{
+    t_dsp_chunk_info chunk_info;
+
+    cm_DSP_GetDspChunkInfo(memHandle, &chunk_info);
+
+    if (powerOff) {
+        cm_PWR_DisableMemory(
+                chunk_info.coreId,
+                chunk_info.memType,
+                cm_DSP_GetPhysicalAdress(memHandle),
+                cm_MM_GetSize(memHandle));
+    }
+
+    cm_MM_Free(chunk_info.alloc, memHandle);
+
+    *coreId = chunk_info.coreId;
+    *memType = chunk_info.memType;
+}
+
+PUBLIC void cm_DM_Free(t_memory_handle memHandle, t_bool powerOff)
+{
+    t_nmf_core_id coreId;
+    t_dsp_memory_type_id memType;
+
+    cm_DM_FreeWithInfo(memHandle, &coreId, &memType, powerOff);
+}
+
+PUBLIC t_cm_error cm_DM_GetAllocatorStatus(t_cm_domain_id domainId, t_dsp_memory_type_id memType, t_cm_allocator_status *pStatus)
+{
+    t_cm_error error;
+    t_uint32 dOffset;
+    t_uint32 dSize;
+
+    //TODO, scratch
+    error = cm_DM_CheckDomain(domainId, DOMAIN_ANY);
+    if (error != CM_OK) {
+        return error;
+    }
+
+    cm_DSP_GetInternalMemoriesInfo(domainId, memType, &dOffset, &dSize);
+
+    return cm_DSP_GetAllocatorStatus(domainDesc[domainId].domain.coreId, memType,
+            dOffset, dSize, pStatus);
+}
+
+//WARNING: this function is only correct *before* migration! because
+//the computation of absolute adresses of a domain is based on the allocator for the given
+//segment (this is hidden in cm_DSP_GetDspBaseAddress and this info is not valid
+//after migration (non-contiguous address-space from the ARM-side)
+PUBLIC t_cm_error cm_DM_GetDomainAbsAdresses(t_cm_domain_id domainId, t_cm_domain_info *info)
+{
+    t_cm_error error;
+    t_nmf_core_id coreId = domainDesc[domainId].domain.coreId;
+
+    cm_migration_check_state(coreId, STATE_NORMAL);
+
+    error = cm_DM_CheckDomain(domainId, DOMAIN_NORMAL);
+    if (error != CM_OK) {
+        return error;
+    }
+
+    cm_DSP_GetDspBaseAddress(coreId, SDRAM_CODE,  &info->sdramCode);
+    cm_DSP_GetDspBaseAddress(coreId, ESRAM_CODE,  &info->esramCode);
+    cm_DSP_GetDspBaseAddress(coreId, SDRAM_EXT24, &info->sdramData);
+    cm_DSP_GetDspBaseAddress(coreId, ESRAM_EXT24, &info->esramData);
+
+    info->sdramCode.physical += domainDesc[domainId].domain.sdramCode.offset;
+    info->sdramCode.logical  += domainDesc[domainId].domain.sdramCode.offset;
+    info->esramCode.physical += domainDesc[domainId].domain.esramCode.offset;
+    info->esramCode.logical  += domainDesc[domainId].domain.esramCode.offset;
+    info->sdramData.physical += domainDesc[domainId].domain.sdramData.offset;
+    info->sdramData.logical  += domainDesc[domainId].domain.sdramData.offset;
+    info->esramData.physical += domainDesc[domainId].domain.esramData.offset;
+    info->esramData.logical  += domainDesc[domainId].domain.esramData.offset;
+
+    return CM_OK;
+}
+
+static void cm_DM_DomainError(const t_cm_domain_id parentId, const t_nmf_client_id client)
+{
+    int i;
+    LOG_INTERNAL(0, "NMF_DEBUG_SCRATCH failed to allocate domain (client %u): 0x%08x -> 0x%08x\n",
+            client,
+            domainDesc[parentId].domain.esramData.offset,
+            domainDesc[parentId].domain.esramData.offset + domainDesc[parentId].domain.esramData.size,
+            0, 0, 0);
+    for(i = 0; i < MAX_USER_DOMAIN_NB; i++) {
+        if (domainDesc[i].type == DOMAIN_SCRATCH_CHILD) {
+            LOG_INTERNAL(0, "NMF_DEBUG_SCRATCH scratch domain %d allocated (client %u): 0x%08x -> 0x%08x\n",
+                    i, domainDesc[i].client,
+                    domainDesc[i].domain.esramData.offset,
+                    domainDesc[i].domain.esramData.offset + domainDesc[i].domain.esramData.size,
+                    0, 0);
+        }
+    }
+    cm_MM_DumpMemory(cm_DM_getAllocator(parentId, ESRAM_EXT16),
+            domainDesc[parentId].domain.esramData.offset,
+            domainDesc[parentId].domain.esramData.offset + domainDesc[parentId].domain.esramData.size);
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/domain_wrapper.c b/drivers/staging/nmf-cm/cm/engine/memory/src/domain_wrapper.c
new file mode 100644
index 00000000000..ec305812f15
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/domain_wrapper.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/api/domain_engine.h>
+#include <cm/engine/api/migration_engine.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/memory/inc/migration.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_CreateMemoryDomain(
+		const t_nmf_client_id     client,
+        const t_cm_domain_memory  *domain,
+        t_cm_domain_id            *handle
+        )
+{
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+    error = cm_DM_CreateDomain(client, domain, handle);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_CreateMemoryDomainScratch(
+        const t_nmf_client_id     client,
+        const t_cm_domain_id parentId,
+        const t_cm_domain_memory  *domain,
+        t_cm_domain_id            *handle
+        )
+{
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+    error = cm_DM_CreateDomainScratch(client, parentId, domain, handle);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_DestroyMemoryDomain(
+        t_cm_domain_id handle)
+{
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+    error = cm_DM_DestroyDomain(handle);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_FlushMemoryDomains(
+        t_nmf_client_id client)
+{
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+    error = cm_DM_DestroyDomains(client);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetDomainCoreId(const t_cm_domain_id domainId, t_nmf_core_id *coreId)
+{
+    t_cm_error error;
+    OSAL_LOCK_API();
+    //TODO, scratch
+    error = cm_DM_CheckDomain(domainId, DOMAIN_NORMAL);
+    if (error != CM_OK) {
+        OSAL_UNLOCK_API();
+        return error;
+    }
+
+    *coreId = cm_DM_GetDomainCoreId(domainId);
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_Migrate(const t_cm_domain_id srcShared, const t_cm_domain_id src, const t_cm_domain_id dst)
+{
+    t_cm_error error;
+    OSAL_LOCK_API();
+    error = cm_migrate(srcShared, src, dst);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_Unmigrate(void)
+{
+    t_cm_error error;
+    OSAL_LOCK_API();
+    error = cm_unmigrate();
+    OSAL_UNLOCK_API();
+    return error;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/memory_wrapper.c b/drivers/staging/nmf-cm/cm/engine/memory/src/memory_wrapper.c
new file mode 100644
index 00000000000..15961aea516
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/memory_wrapper.c
@@ -0,0 +1,243 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/configuration/inc/configuration.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/trace/inc/trace.h>
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_AllocMpcMemory(
+        t_cm_domain_id domainId,
+        t_nmf_client_id clientId,
+        t_cm_mpc_memory_type memType,
+        t_cm_size size,
+        t_cm_mpc_memory_alignment memAlignment,
+        t_cm_memory_handle *pHandle
+        )
+{
+    t_dsp_memory_type_id dspMemType;
+
+    switch(memType)
+    {
+        case CM_MM_MPC_TCM16_X:
+            dspMemType = INTERNAL_XRAM16;
+            break;
+        case CM_MM_MPC_TCM24_X:
+            dspMemType = INTERNAL_XRAM24;
+            break;
+        case CM_MM_MPC_TCM16_Y:
+            dspMemType = INTERNAL_YRAM16;
+            break;
+        case CM_MM_MPC_TCM24_Y:
+            dspMemType = INTERNAL_YRAM24;
+            break;
+#ifndef __STN_8810
+        case CM_MM_MPC_ESRAM16:
+            dspMemType = ESRAM_EXT16;
+            break;
+        case CM_MM_MPC_ESRAM24:
+            dspMemType = ESRAM_EXT24;
+            break;
+#endif /* ndef __STN_8810 */
+        case CM_MM_MPC_SDRAM16:
+            dspMemType = SDRAM_EXT16;
+            break;
+        case CM_MM_MPC_SDRAM24:
+            dspMemType = SDRAM_EXT24;
+            break;
+        default:
+            return CM_INVALID_PARAMETER;
+    }
+
+    OSAL_LOCK_API();
+    {
+        t_cm_error error;
+        error = cm_DM_CheckDomainWithClient(domainId, DOMAIN_ANY, clientId);
+        if (error != CM_OK) {
+            OSAL_UNLOCK_API();
+            return error;
+        }
+    }
+
+    switch(memAlignment) {
+    case CM_MM_MPC_ALIGN_NONE        :
+    case CM_MM_MPC_ALIGN_HALFWORD    :
+    case CM_MM_MPC_ALIGN_WORD        :
+    case CM_MM_MPC_ALIGN_2WORDS      :
+    case CM_MM_MPC_ALIGN_4WORDS      :
+    case CM_MM_MPC_ALIGN_8WORDS      :
+    case CM_MM_MPC_ALIGN_16WORDS     :
+    case CM_MM_MPC_ALIGN_32WORDS     :
+    case CM_MM_MPC_ALIGN_64WORDS     :
+    case CM_MM_MPC_ALIGN_128WORDS    :
+    case CM_MM_MPC_ALIGN_256WORDS    :
+    case CM_MM_MPC_ALIGN_512WORDS    :
+    case CM_MM_MPC_ALIGN_1024WORDS   :
+    case CM_MM_MPC_ALIGN_65536BYTES  :
+    //case CM_MM_MPC_ALIGN_16384WORDS  : maps to the same value as above
+        break;
+    default:
+        OSAL_UNLOCK_API();
+        return CM_INVALID_PARAMETER;
+    }
+
+    /* in case we allocate in tcm x be sure ee is load before */
+    if ( memType == CM_MM_MPC_TCM16_X || memType == CM_MM_MPC_TCM24_X ||
+         memType == CM_MM_MPC_TCM16_Y || memType == CM_MM_MPC_TCM24_Y )
+    {
+        t_cm_error error;
+        if ((error = cm_CFG_CheckMpcStatus(cm_DM_GetDomainCoreId(domainId))) != CM_OK)
+        {
+            OSAL_UNLOCK_API();
+            return error;
+        }
+    }
+
+    /* alloc memory */
+    *pHandle = (t_cm_memory_handle)cm_DM_Alloc(domainId, dspMemType, size, memAlignment, TRUE);
+    if(*pHandle == (t_cm_memory_handle)INVALID_MEMORY_HANDLE)
+    {
+        OSAL_UNLOCK_API();
+        return CM_NO_MORE_MEMORY;
+    }
+
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_FreeMpcMemory(t_cm_memory_handle handle)
+{
+    t_cm_error error;
+    t_memory_handle validHandle;
+    t_nmf_core_id coreId;
+    t_dsp_memory_type_id memType;
+
+    OSAL_LOCK_API();
+
+    if((error = cm_MM_getValidMemoryHandle(handle, &validHandle)) != CM_OK)
+    {
+        OSAL_UNLOCK_API();
+        return error;
+    }
+
+    cm_DM_FreeWithInfo(validHandle, &coreId, &memType, TRUE);
+
+    /* in case we allocate in tcm x be sure ee is load before */
+    if ( memType == INTERNAL_XRAM16 || memType == INTERNAL_XRAM24 ||
+            memType == INTERNAL_YRAM16 || memType == INTERNAL_YRAM24 )
+    {
+        cm_CFG_ReleaseMpc(coreId);
+    }
+
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemorySystemAddress(t_cm_memory_handle handle, t_cm_system_address *pSystemAddress)
+{
+    t_cm_error error;
+    t_memory_handle validHandle;
+
+    OSAL_LOCK_API();
+
+    if((error = cm_MM_getValidMemoryHandle(handle, &validHandle)) != CM_OK)
+    {
+        OSAL_UNLOCK_API();
+        return error;
+    }
+
+    cm_DSP_GetHostSystemAddress(validHandle, pSystemAddress);
+
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemoryMpcAddress(t_cm_memory_handle handle, t_uint32 *pDspAddress)
+{
+    t_cm_error error;
+    t_memory_handle validHandle;
+
+    OSAL_LOCK_API();
+
+    if((error = cm_MM_getValidMemoryHandle(handle, &validHandle)) != CM_OK)
+    {
+        OSAL_UNLOCK_API();
+        return error;
+    }
+
+    cm_DSP_GetDspAddress(validHandle, pDspAddress);
+
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemorySize(
+	t_cm_memory_handle handle,
+	t_uint32 *pSize)
+{
+    t_cm_error error;
+    t_memory_handle validHandle;
+
+    OSAL_LOCK_API();
+
+    if((error = cm_MM_getValidMemoryHandle(handle, &validHandle)) != CM_OK)
+    {
+        OSAL_UNLOCK_API();
+        return error;
+    }
+
+    *pSize = validHandle->size;
+
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemoryStatus(t_nmf_core_id coreId, t_cm_mpc_memory_type memType, t_cm_allocator_status *pStatus)
+{
+    t_dsp_memory_type_id dspMemType;
+    t_cm_error error;
+
+    switch(memType)
+    {
+        case CM_MM_MPC_TCM16_X:
+            dspMemType = INTERNAL_XRAM16;
+            break;
+        case CM_MM_MPC_TCM24_X:
+            dspMemType = INTERNAL_XRAM24;
+            break;
+        case CM_MM_MPC_TCM16_Y:
+            dspMemType = INTERNAL_YRAM16;
+            break;
+        case CM_MM_MPC_TCM24_Y:
+            dspMemType = INTERNAL_YRAM24;
+            break;
+#ifndef __STN_8810
+        case CM_MM_MPC_ESRAM16:
+            dspMemType = ESRAM_EXT16;
+            break;
+        case CM_MM_MPC_ESRAM24:
+            dspMemType = ESRAM_EXT24;
+            break;
+#endif /* ndef __STN_8810 */
+        case CM_MM_MPC_SDRAM16:
+            dspMemType = SDRAM_EXT16;
+            break;
+        case CM_MM_MPC_SDRAM24:
+            dspMemType = SDRAM_EXT24;
+            break;
+        default:
+            return CM_INVALID_PARAMETER;
+    }
+
+    OSAL_LOCK_API();
+    error = cm_DSP_GetAllocatorStatus(coreId, dspMemType, 0, 0, pStatus);
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/migration.c b/drivers/staging/nmf-cm/cm/engine/memory/src/migration.c
new file mode 100644
index 00000000000..35813a56bf2
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/migration.c
@@ -0,0 +1,391 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <inc/type.h>
+#include <inc/nmf-limits.h>
+
+#include <cm/engine/communication/fifo/inc/nmf_fifo_arm.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/memory/inc/migration.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/mem.h>
+
+#if defined(__STN_8500) && (__STN_8500 > 10)
+
+typedef enum {
+    CM_MIGRATION_OK = 0,
+    CM_MIGRATION_INVALID_ARGUMENT = 1,
+    CM_MIGRATION_ERROR = 2,
+} t_cm_migration_error;
+
+extern t_nmf_fifo_arm_desc* mpc2mpcComsFifoId[NB_CORE_IDS][NB_CORE_IDS];
+
+/*!
+ * \brief Data structure representing a segment to migrate
+ *
+ * segment:
+ *      - used to determine which mmdsp-hw base is to be updated, index in mpcDesc->segments[] structure
+ *      * this is hard-coded in cm_migrate(), could be computed (would be nice) LIMITATION
+ * srcAdr.physical:
+ *      - new base setting
+ *      * computed from the src domain in cm_DM_GetAbsAdresses() which uses the start of the allocator for the memory
+ *        this is a LIMITATION, as this information is valid only before migration
+ * srcAdr.logical:
+ *      - cm_MemCopy()
+ *      * computed as srcAdr.logical
+ * dstAdr.physical: see srcAdr.physical
+ * dstAdr.logical:  see srcAdr.logical
+ * size:
+ *      - cm_MemCopy()
+ *      - setting the top when new base is set
+ */
+typedef struct {
+    t_dsp_segment_type       segment; //!< the link to the segment type
+    t_cm_system_address      srcAdr;  //!< source address
+    t_cm_system_address      dstAdr;  //!< destination address
+    t_uint32                 size;    //!< size of the segment
+} t_cm_migration_segment;
+
+/*!
+ * \brief Internal data structure 1/ during migration, and 2/ between migration and unmigration calls
+ *
+ * all needed information are computed before calling _cm_migration_move()
+ */
+typedef struct {
+    t_cm_migration_state    state;                            //!< migration state
+    t_nmf_core_id           coreId;                           //!< migration only on one mpc
+    t_cm_migration_segment  segments[NB_MIGRATION_SEGMENT];   //!< segments to migrate (selected on migration_move)
+    t_memory_handle         handles[NB_MIGRATION_SEGMENT];    //!< memory handles for destination chunks allocated prior migration
+} t_cm_migration_internal_state;
+
+static t_cm_migration_internal_state migrationState = {STATE_NORMAL, };
+
+static t_cm_error _cm_migration_initSegment(
+        t_dsp_segment_type      dspSegment,
+        t_cm_system_address    *srcAdr,
+        t_uint32                size,
+        t_cm_domain_id          dst,
+        t_cm_migration_internal_state *info
+        )
+{
+    t_cm_system_address      dstAdr;
+    t_cm_migration_segment  *segment = &info->segments[dspSegment];
+    t_memory_handle          handle;
+
+    handle = cm_DM_Alloc(dst, ESRAM_EXT16, size >> 1, CM_MM_ALIGN_AHB_BURST, TRUE); //note: byte to half-word conversion
+    if (handle == 0) {
+        return CM_NO_MORE_MEMORY;
+    }
+
+    info->handles[dspSegment] = handle;
+
+    cm_DSP_GetHostSystemAddress(handle, &dstAdr);
+
+    segment->segment   = dspSegment; //this is redundant and could be avoided by recoding move(), but nice to have for debug
+    segment->size      = size;
+    segment->srcAdr    = *srcAdr;
+    segment->dstAdr    = dstAdr;
+
+    return CM_OK;
+}
+
+static void _cm_migration_releaseSegment(t_cm_migration_internal_state *info, t_dsp_segment_type segId)
+{
+    cm_DM_Free(info->handles[segId], TRUE);
+}
+
+static t_cm_migration_error _cm_migration_release(t_cm_migration_internal_state *info)
+{
+    t_uint32 i = 0;
+    for (i = 0; i < NB_MIGRATION_SEGMENT; i++) {
+        cm_DM_Free(info->handles[i], TRUE);
+    }
+
+    return CM_MIGRATION_OK;
+}
+
+#define SEGMENT_START(seg) \
+    seg.offset
+
+#define SEGMENT_END(seg) \
+    seg.offset + seg.size
+
+static t_cm_error _cm_migration_check(
+        const t_cm_domain_id srcShared,
+        const t_cm_domain_id src,
+        const t_cm_domain_id dst,
+        t_cm_migration_internal_state *info
+        )
+{
+    t_cm_error error = CM_OK;
+    t_cm_domain_info domainInfoSrc;
+    t_cm_domain_info domainInfoShared;
+    t_cm_domain_desc *domainEE;
+    t_cm_domain_desc *domainShared;
+    t_nmf_core_id coreId = cm_DM_GetDomainCoreId(src);
+
+    //coreIds in src, srcShared and dst match
+    if (!((domainDesc[src].domain.coreId == domainDesc[srcShared].domain.coreId)
+        && (domainDesc[src].domain.coreId == domainDesc[dst].domain.coreId))) {
+        return CM_INVALID_PARAMETER;
+    }
+
+    //check srcShared starts at 0
+    //FIXME, juraj, today EE code is in SDRAM, but this is flexible, so must find out where EE is instantiated
+    if (domainDesc[srcShared].domain.sdramCode.offset != 0x0) {
+        return CM_INVALID_PARAMETER;
+    }
+
+    //check srcShared contains EE domain
+    domainEE = &domainDesc[cm_DSP_GetState(coreId)->domainEE];
+    domainShared = &domainDesc[srcShared];
+    if ((SEGMENT_START(domainEE->domain.esramCode) < SEGMENT_START(domainShared->domain.esramCode))
+      ||(SEGMENT_END(domainEE->domain.esramCode) > SEGMENT_END(domainShared->domain.esramCode))
+      ||(SEGMENT_START(domainEE->domain.esramData) < SEGMENT_START(domainShared->domain.esramData))
+      ||(SEGMENT_END(domainEE->domain.esramData) > SEGMENT_END(domainShared->domain.esramData))
+      ||(SEGMENT_START(domainEE->domain.sdramCode) < SEGMENT_START(domainShared->domain.sdramCode))
+      ||(SEGMENT_END(domainEE->domain.sdramCode) > SEGMENT_END(domainShared->domain.sdramCode))
+      ||(SEGMENT_START(domainEE->domain.sdramData) < SEGMENT_START(domainShared->domain.sdramData))
+      ||(SEGMENT_END(domainEE->domain.sdramData) > SEGMENT_END(domainShared->domain.sdramData))
+        ) {
+        return CM_INVALID_PARAMETER;
+    }
+
+    info->coreId = coreId;
+    cm_DM_GetDomainAbsAdresses(srcShared, &domainInfoShared);
+    cm_DM_GetDomainAbsAdresses(src, &domainInfoSrc);
+
+    if ((error = _cm_migration_initSegment(SDRAM_CODE_EE, &domainInfoShared.sdramCode,
+                                           domainDesc[srcShared].domain.sdramCode.size, dst, info)) != CM_OK)
+            goto _migration_error1;
+    if ((error = _cm_migration_initSegment(SDRAM_CODE_USER, &domainInfoSrc.sdramCode,
+                                           domainDesc[src].domain.sdramCode.size, dst, info)) != CM_OK)
+            goto _migration_error2;
+    if ((error = _cm_migration_initSegment(SDRAM_DATA_EE, &domainInfoShared.sdramData,
+                                           domainDesc[srcShared].domain.sdramData.size, dst, info)) != CM_OK)
+            goto _migration_error3;
+    if ((error = _cm_migration_initSegment(SDRAM_DATA_USER, &domainInfoSrc.sdramData,
+                                           domainDesc[src].domain.sdramData.size, dst, info)) != CM_OK)
+            goto _migration_error4;
+    return error;
+
+_migration_error4: _cm_migration_releaseSegment(info, SDRAM_DATA_EE);
+_migration_error3: _cm_migration_releaseSegment(info, SDRAM_CODE_USER);
+_migration_error2: _cm_migration_releaseSegment(info, SDRAM_CODE_EE);
+_migration_error1:
+    OSAL_Log("Couldn't allocate memory for migration\n", 0, 0, 0, 0, 0, 0);
+    return CM_NO_MORE_MEMORY;
+}
+
+typedef t_cm_error (*updateBase_t)(t_nmf_core_id, t_dsp_segment_type, t_cm_system_address, t_cm_system_address);
+
+static t_cm_migration_error _cm_migration_move(
+        t_nmf_core_id           coreId,
+        t_cm_migration_segment *seg,
+        updateBase_t            updateBase,
+        char*                   name
+        )
+{
+    LOG_INTERNAL(1, "##### Migration %s: 0x%x -> 0x%x\n", name, seg->srcAdr.logical, seg->dstAdr.logical, 0, 0, 0);
+    cm_MemCopy((void*)seg->dstAdr.logical, (void*)seg->srcAdr.logical, seg->size);
+    updateBase(coreId, seg->segment, seg->srcAdr, seg->dstAdr);
+    cm_MemSet((void*)seg->srcAdr.logical, 0xdead, seg->size); //for debug, to be sure that we have actually moved the code and bases
+
+    return CM_MIGRATION_OK;
+}
+
+static t_cm_migration_error _cm_migration_update_internal(
+        t_cm_migration_internal_state *info,
+        t_cm_migration_state state
+        )
+{
+    t_nmf_fifo_arm_desc *pArmFifo;
+
+    migrationState.state = state;
+
+    switch(state) {
+    case STATE_MIGRATED:
+        //move fifos
+        pArmFifo = mpc2mpcComsFifoId[ARM_CORE_ID][info->coreId];
+        pArmFifo->fifoDesc = (t_nmf_fifo_desc*)cm_migration_translate(pArmFifo->dspAddressInfo.segmentType, (t_shared_addr)pArmFifo->fifoDescShadow);
+        pArmFifo = mpc2mpcComsFifoId[info->coreId][ARM_CORE_ID];
+        pArmFifo->fifoDesc = (t_nmf_fifo_desc*)cm_migration_translate(pArmFifo->dspAddressInfo.segmentType, (t_shared_addr)pArmFifo->fifoDescShadow);
+        break;
+
+    case STATE_NORMAL:
+        //move fifos
+        pArmFifo = mpc2mpcComsFifoId[ARM_CORE_ID][info->coreId];
+        pArmFifo->fifoDesc = pArmFifo->fifoDescShadow;
+        pArmFifo = mpc2mpcComsFifoId[info->coreId][ARM_CORE_ID];
+        pArmFifo->fifoDesc = pArmFifo->fifoDescShadow;
+        break;
+
+    default:
+        OSAL_Log("unknown state", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+
+    return CM_MIGRATION_OK;
+}
+
+PUBLIC t_cm_error cm_migrate(const t_cm_domain_id srcShared, const t_cm_domain_id src, const t_cm_domain_id dst)
+{
+    t_cm_migration_error mError;
+    t_cm_error error;
+
+    if ((error = _cm_migration_check(srcShared, src, dst, &migrationState)) != CM_OK) {
+        return error;
+    }
+
+    /* stop DSP execution */
+    cm_DSP_Stop(migrationState.coreId);
+
+    /* migrate EE and FX */
+    mError = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_CODE_EE], cm_DSP_updateCodeBase, "code");
+    if (mError) {
+        OSAL_Log("EE code migration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    mError = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_DATA_EE], cm_DSP_updateDataBase, "data");
+    if (mError) {
+        OSAL_Log("EE data migration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    /* migrate user domain */
+    mError = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_CODE_USER], cm_DSP_updateCodeBase, "code");
+    if (mError) {
+        OSAL_Log("User code migration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    mError = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_DATA_USER], cm_DSP_updateDataBase, "data");
+    if (mError) {
+        OSAL_Log("User data migration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+   }
+    /* update CM internal structures */
+    mError = _cm_migration_update_internal(&migrationState, STATE_MIGRATED);
+    if (mError) {
+        OSAL_Log("Update internal data failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+
+    /* Be sure everything has been write before restarting mmdsp */
+    OSAL_mb();
+
+    /* resume DSP execution */
+    cm_DSP_Start(migrationState.coreId);
+
+    return CM_OK;
+}
+
+static void _cm_migration_swapSegments(
+        t_cm_migration_segment *segment
+        )
+{
+    t_cm_system_address tmp;
+    tmp = segment->dstAdr;
+    segment->dstAdr = segment->srcAdr;
+    segment->srcAdr = tmp;
+}
+
+PUBLIC t_cm_error cm_unmigrate(void)
+{
+    t_cm_migration_error merror;
+
+    if (migrationState.state != STATE_MIGRATED)
+        return CM_INVALID_PARAMETER; //TODO, juraj, define a proper error for this migration case
+
+    cm_DSP_Stop(migrationState.coreId);
+
+    _cm_migration_swapSegments(&migrationState.segments[SDRAM_CODE_EE]);
+    _cm_migration_swapSegments(&migrationState.segments[SDRAM_DATA_EE]);
+    _cm_migration_swapSegments(&migrationState.segments[SDRAM_CODE_USER]);
+    _cm_migration_swapSegments(&migrationState.segments[SDRAM_DATA_USER]);
+
+    merror = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_CODE_EE], cm_DSP_updateCodeBase, "code");
+    if (merror) {
+        OSAL_Log("EE code unmigration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    merror = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_DATA_EE], cm_DSP_updateDataBase, "data");
+    if (merror) {
+        OSAL_Log("EE data unmigration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    merror = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_CODE_USER], cm_DSP_updateCodeBase, "code");
+    if (merror) {
+        OSAL_Log("User code unmigration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    merror = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_DATA_USER], cm_DSP_updateDataBase, "data");
+    if (merror) {
+        OSAL_Log("User data unmigration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+
+    /* update CM internal structures */
+    merror = _cm_migration_update_internal(&migrationState, STATE_NORMAL);
+    if (merror) {
+        OSAL_Log("Update internal data failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+
+    /* Be sure everything has been write before restarting mmdsp */
+    OSAL_mb();
+
+    cm_DSP_Start(migrationState.coreId);
+
+    /* update CM internal structures */
+    merror = _cm_migration_release(&migrationState);
+    if (merror) {
+        OSAL_Log("Update internal data failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+
+    return CM_OK;
+}
+
+// here we make the assumption that the offset doesn't depend from the dsp!!
+PUBLIC t_uint32 cm_migration_translate(t_dsp_segment_type segmentType, t_uint32 addr)
+{
+    //TODO, juraj, save delta instead of recalculating it
+    t_sint32 offset;
+    if (migrationState.state == STATE_MIGRATED) {
+        offset = migrationState.segments[segmentType].dstAdr.logical - migrationState.segments[segmentType].srcAdr.logical;
+    } else {
+        offset = 0;
+    }
+    return addr + offset;
+}
+
+PUBLIC void cm_migration_check_state(t_nmf_core_id coreId, t_cm_migration_state expected)
+{
+    CM_ASSERT(migrationState.state == expected);
+}
+
+#else
+PUBLIC t_cm_error cm_migrate(const t_cm_domain_id srcShared, const t_cm_domain_id src, const t_cm_domain_id dst)
+{
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_unmigrate(void)
+{
+    return CM_OK;
+}
+
+PUBLIC t_uint32 cm_migration_translate(t_dsp_segment_type segmentType, t_uint32 addr)
+{
+    return addr;
+}
+
+PUBLIC void cm_migration_check_state(t_nmf_core_id coreId, t_cm_migration_state expected)
+{
+    return;
+}
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator.c b/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator.c
new file mode 100644
index 00000000000..3df552aca4d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator.c
@@ -0,0 +1,680 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ * Include
+ */
+#include "../inc/remote_allocator.h"
+#include "../inc/remote_allocator_utils.h"
+#include "../inc/chunk_mgr.h"
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/trace/inc/xtitrace.h>
+
+static t_cm_chunk* cm_MM_RA_getLastChunk(t_cm_allocator_desc* alloc);
+static void cm_MM_RA_checkAllocator(t_cm_allocator_desc* alloc);
+//static void cm_MM_RA_checkAlloc(t_cm_allocator_desc* alloc, t_uint32 size, t_uint32 align, t_uint32 min, t_uint32 max);
+
+int bin_index(unsigned int sz) {
+    /*
+     * 32 bins of size       2
+     * 16 bins of size      16
+     *  8 bins of size     128
+     *  4 bins of size    1024
+     *  2 bins of size    8192
+     *  1 bin  of size what's left
+     *
+     */
+    return (((sz >> 6) ==    0) ?       (sz >>  1): // 0        -> 0 .. 31
+            ((sz >> 6) <=    4) ?  28 + (sz >>  4): // 64       -> 32 .. 47
+            ((sz >> 6) <=   20) ?  46 + (sz >>  7): // 320      -> 48 .. 55
+            ((sz >> 6) <=   84) ?  55 + (sz >> 10): // 1344     -> 56 .. 59
+            ((sz >> 6) <=  340) ?  59 + (sz >> 13): // 5440     -> 60 .. 61
+            62);                                    // 21824..
+}
+
+static t_cm_allocator_desc* ListOfAllocators = NULL;
+
+PUBLIC t_cm_allocator_desc* cm_MM_CreateAllocator(t_cm_size size, t_uint32 offset, const char* name)
+{
+    t_cm_allocator_desc *alloc;
+
+    CM_ASSERT(fillChunkPool() == CM_OK);
+
+    /* Alloc structure */
+    alloc = (t_cm_allocator_desc*)OSAL_Alloc_Zero(sizeof(t_cm_allocator_desc));
+    CM_ASSERT(alloc != NULL);
+
+    // Add allocator in list
+    alloc->next = ListOfAllocators;
+    ListOfAllocators = alloc;
+
+    /* Create first chunk */
+    alloc->chunks = allocChunk();
+
+    /* assign name */
+    alloc->pAllocName = name;
+
+    alloc->chunks->size    = size;
+    alloc->chunks->offset  = offset;
+    alloc->chunks->alloc   = alloc;
+    alloc->free_mem_chunks[bin_index(alloc->chunks->size)] = alloc->chunks;
+
+    alloc->size = size;
+
+    //TODO, juraj, alloc impacts trace format
+    cm_TRC_traceMemAlloc(TRACE_ALLOCATOR_COMMAND_CREATE, 0, size, name);
+
+    return alloc;
+}
+
+PUBLIC t_cm_error cm_MM_DeleteAllocator(t_cm_allocator_desc *alloc)
+{
+    t_cm_chunk *chunk, *next_cm_chunk;
+
+    cm_TRC_traceMemAlloc(TRACE_ALLOCATOR_COMMAND_DESTROY, 0, 0, alloc->pAllocName);
+
+    /* Parse all chunks and free them */
+    chunk = alloc->chunks;
+    while(chunk != 0)
+    {
+        next_cm_chunk = chunk->next;
+        unlinkChunk(alloc, chunk);
+        freeChunk(chunk);
+
+        chunk = next_cm_chunk;
+    }
+
+    // Remove allocator from the list
+    if(ListOfAllocators == alloc)
+        ListOfAllocators = alloc->next;
+    else {
+        t_cm_allocator_desc *prev = ListOfAllocators;
+        while(prev->next != alloc)
+            prev = prev->next;
+        prev->next = alloc->next;
+    }
+
+
+    /* Free allocator descriptor */
+    OSAL_Free(alloc);
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_MM_ResizeAllocator(t_cm_allocator_desc *alloc, t_cm_size size)
+{
+    t_cm_error error;
+
+    /* sanity check */
+    if (size == 0)
+        return CM_INVALID_PARAMETER;
+
+    if((error = fillChunkPool()) != CM_OK)
+        return error;
+
+    if (size > alloc->size) {
+        /* ok, increase allocator */
+        t_uint32 deltaSize = size - alloc->size;
+        t_cm_chunk *last = cm_MM_RA_getLastChunk(alloc);
+
+        if (last->status == MEM_FREE) {
+            /* last chunk is a free one, just increase size */
+            unlinkFreeMem(alloc, last);
+            last->size += deltaSize;
+            alloc->size += deltaSize;
+            /* now list of free chunk is potentially no more ordered */
+            updateFreeList(alloc, last);
+        } else {
+            /* last chunk is a used one, add new free chunk */
+            last->size += deltaSize;
+            splitChunk(alloc, last, last->offset + deltaSize, FREE_CHUNK_AFTER);
+            alloc->size += deltaSize;
+        }
+    } else {
+        /* reduce allocator */
+        t_uint32 deltaSize = alloc->size - size;
+        t_cm_chunk *last = cm_MM_RA_getLastChunk(alloc);
+
+        /* check if resize is possible */
+        if (last->status == MEM_USED)
+            return CM_NO_MORE_MEMORY;
+        if (last->size < deltaSize)
+            return CM_NO_MORE_MEMORY;
+
+        /* ok, rezise can be performed */
+        if (last->size == deltaSize)        {
+            t_cm_chunk *prev = last->prev;
+
+            /* remove last free chunk */
+            mergeChunk(alloc, prev, last);
+            prev->size -= deltaSize;
+        } else {
+            unlinkFreeMem(alloc, last);
+            /* reduce size of last free chunk */
+            last->size -= deltaSize;
+            /* now list of free chunk is potentially no more ordered */
+            updateFreeList(alloc, last);
+        }
+    }
+
+    alloc->size = size;
+
+    if (cmIntensiveCheckState) {
+        cm_MM_RA_checkAllocator(alloc);
+    }
+
+    return CM_OK;
+}
+
+t_cm_error cm_MM_getValidMemoryHandle(t_cm_memory_handle handle, t_memory_handle* validHandle)
+{
+#ifdef LINUX
+    /* On linux, there is already a check within the linux part
+     * => we don't need to check twice */
+    *validHandle = (t_memory_handle)handle;
+    return CM_OK;
+#else
+    t_cm_allocator_desc *alloc = ListOfAllocators;
+
+    for(; alloc != NULL; alloc = alloc->next)
+    {
+        t_cm_chunk* chunk = alloc->chunks;
+
+        /* Parse all chunks */
+        for(; chunk != NULL; chunk = chunk->next)
+        {
+            if(chunk == (t_memory_handle)handle)
+            {
+                if(chunk->status == MEM_FREE)
+                    return CM_MEMORY_HANDLE_FREED;
+
+                *validHandle = (t_memory_handle)handle;
+
+                return CM_OK;
+            }
+        }
+    }
+
+    return CM_UNKNOWN_MEMORY_HANDLE;
+#endif
+}
+
+//TODO, juraj, add appartenance to allocHandle (of chunk) and degage setUserData
+PUBLIC t_memory_handle cm_MM_Alloc(
+        t_cm_allocator_desc* alloc,
+        t_cm_size size,
+        t_cm_memory_alignment memAlignment,
+        t_uint32 seg_offset,
+        t_uint32 seg_size,
+        t_uint32 domainId)
+{
+    t_cm_chunk* chunk;
+    t_uint32 aligned_offset;
+    t_uint32 aligned_end;
+    t_uint32 seg_end = seg_offset + seg_size;
+    int i;
+
+    /* Sanity check */
+    if ( (size == 0) || (size > seg_size) )
+        return INVALID_MEMORY_HANDLE;
+
+    if(fillChunkPool() != CM_OK)
+        return INVALID_MEMORY_HANDLE;
+
+    /* Get first chunk available for the specific size */
+    // Search a list with a free chunk
+    for(i = bin_index(size); i < BINS; i++)
+    {
+        chunk = alloc->free_mem_chunks[i];
+        while (chunk != 0)
+        {
+            /* Alignment of the lower boundary */
+            aligned_offset = ALIGN_VALUE(MAX(chunk->offset, seg_offset), (memAlignment + 1));
+
+            aligned_end = aligned_offset + size;
+
+            if ((aligned_end <= seg_end)
+                    && aligned_end <= (chunk->offset + chunk->size)
+                    && aligned_offset >= seg_offset
+                    && aligned_offset >= chunk->offset)
+                goto found;
+
+            chunk = chunk->next_free_mem;
+        }
+    }
+
+    return INVALID_MEMORY_HANDLE;
+
+found:
+
+    /* Remove chunk from free list */
+    unlinkFreeMem(alloc, chunk);
+
+    //create an empty chunk before the allocated one
+    if (chunk->offset < aligned_offset) {
+        chunk = splitChunk(alloc, chunk, aligned_offset, FREE_CHUNK_BEFORE);
+    }
+    //create an empty chunk after the allocated one
+    if (chunk->offset + chunk->size > aligned_end) {
+        splitChunk(alloc, chunk, aligned_end, FREE_CHUNK_AFTER);
+    }
+
+    chunk->status = MEM_USED;
+    chunk->prev_free_mem = 0;
+    chunk->next_free_mem = 0;
+    chunk->domainId = domainId;
+
+    //TODO, juraj, alloc impacts trace format
+    cm_TRC_traceMem(TRACE_ALLOC_COMMAND_ALLOC, 0, chunk->offset, chunk->size);
+
+    if (cmIntensiveCheckState) {
+        cm_MM_RA_checkAllocator(alloc);
+    }
+
+    chunk->alloc = alloc;
+    return (t_memory_handle) chunk;
+}
+
+//caution - if successfull, the chunk offset will be aligned with seg_offset
+//caution++ the offset of the allocated chunk changes implicitly
+PUBLIC t_memory_handle cm_MM_Realloc(
+                t_cm_allocator_desc* alloc,
+                const t_cm_size size,
+                const t_uint32 offset,
+                const t_cm_memory_alignment memAlignment,
+                const t_memory_handle handle)
+{
+    t_cm_chunk *chunk = (t_cm_chunk*)handle;
+
+    /* check reallocation is related to this chunk! */
+    CM_ASSERT(chunk->offset <= (offset + size));
+    CM_ASSERT(offset <= (chunk->offset + chunk->size));
+    CM_ASSERT(size);
+
+    /* check if extend low */
+    if (offset < chunk->offset) {
+        /* note: it is enough to check only the previous chunk,
+         *      because adjacent chunks of same status are merged
+         */
+        if ((chunk->prev == 0)
+           ||(chunk->prev->status != MEM_FREE)
+           ||(chunk->prev->offset > offset)) {
+            return INVALID_MEMORY_HANDLE;
+        }
+    }
+
+    /* check if extend high, note as above */
+    if ( (offset + size) > (chunk->offset + chunk->size)) {
+        if ((chunk->next == 0)
+           ||(chunk->next->status != MEM_FREE)
+           ||( (chunk->next->offset + chunk->next->size) < (offset + size))) {
+            return INVALID_MEMORY_HANDLE;
+        }
+    }
+
+    if(fillChunkPool() != CM_OK)
+        return INVALID_MEMORY_HANDLE;
+
+
+#if 0
+    /* extend low
+     *      all conditions should have been checked
+     *      this must not fail
+     */
+    if (offset < chunk->offset) {
+        t_cm_chunk *tmp = splitChunk(alloc, chunk->prev, offset, FREE_CHUNK_BEFORE); //tmp = chunk->prev
+        CM_ASSERT(tmp);
+        tmp->status = MEM_USED;
+        tmp->prev->status = MEM_FREE;
+        mergeChunk(alloc, tmp, chunk);
+        if ((tmp->prev->prev != 0)
+           && (tmp->prev->prev->status == MEM_FREE)) {
+            mergeChunk(alloc, tmp->prev->prev, tmp->prev);
+        }
+        chunk = tmp;
+    }
+
+    /* extend high */
+    if ( (offset + size) > (chunk->offset + chunk->size)) {
+        t_cm_chunk *tmp = splitChunk(alloc, chunk->next, offset + size, FREE_CHUNK_AFTER); //tmp = chunk->next->next
+        CM_ASSERT(tmp);
+        tmp->status = MEM_USED;
+        mergeChunk(alloc, chunk, tmp);
+        if ((tmp->next->next != 0)
+           && (tmp->next->next->status == MEM_FREE)) {
+            mergeChunk(alloc, tmp->next, tmp->next->next);
+        }
+    }
+
+    /* reduce top */
+    if ((offset + size) < (chunk->offset + chunk->size)) {
+        t_cm_chunk *tmp = splitChunk(alloc, chunk, offset + size, FREE_CHUNK_AFTER); //tmp = chunk, chunk = result
+        CM_ASSERT(tmp);
+        tmp->status = MEM_USED;
+        tmp->next->status = MEM_FREE;
+        if ((tmp->next->next != 0)
+           && (tmp->next->next->status == MEM_FREE)) {
+            mergeChunk(alloc, tmp->next, tmp->next->next);
+        }
+    }
+
+    /* reduce bottom */
+    if (offset > chunk->offset) {
+        t_cm_chunk *tmp = splitChunk(alloc, chunk, offset, FREE_CHUNK_BEFORE); //tmp->next = chunk, tmp = result
+        CM_ASSERT(tmp);
+        tmp->status = MEM_USED;
+        tmp->prev->status = MEM_FREE;
+        if ((tmp->prev->prev != 0)
+           &&(tmp->prev->prev->status == MEM_FREE)) {
+            mergeChunk(alloc, tmp->prev->prev, tmp->prev);
+        }
+        chunk = tmp;
+    }
+#else
+    /* extend low
+     *      all conditions should have been checked
+     *      this must not fail
+     */
+    if (offset < chunk->offset) {
+        t_uint32 delta = chunk->prev->offset + chunk->prev->size - offset;
+        CM_ASSERT(chunk->prev->status == MEM_FREE); //TODO, juraj, already checked
+        unlinkFreeMem(alloc, chunk->prev);
+        chunk->prev->size -= delta;
+        chunk->offset -= delta;
+        chunk->size += delta;
+        updateFreeList(alloc, chunk->prev);
+    }
+
+    /* extend high */
+    if ( (offset + size) > (chunk->offset + chunk->size)) {
+        t_uint32 delta = size - chunk->size;
+        CM_ASSERT(chunk->next->status == MEM_FREE); //TODO, juraj, already checked
+        unlinkFreeMem(alloc, chunk->next);
+        chunk->size += delta;
+        chunk->next->offset += delta;
+        chunk->next->size -= delta;
+        updateFreeList(alloc, chunk->next);
+    }
+
+    /* reduce top */
+    if ((offset + size) < (chunk->offset + chunk->size)) {
+        if (chunk->next->status == MEM_FREE) {
+            t_uint32 delta = chunk->size - size;
+            unlinkFreeMem(alloc, chunk->next);
+            chunk->size -= delta;
+            chunk->next->offset -= delta;
+            chunk->next->size += delta;
+            updateFreeList(alloc, chunk->next);
+        } else {
+            t_cm_chunk *tmp = splitChunk(alloc, chunk, offset + size, FREE_CHUNK_AFTER); //tmp = chunk, chunk = result
+            tmp->status = MEM_USED;
+            tmp->next->status = MEM_FREE;
+        }
+    }
+
+    /* reduce bottom */
+    if (offset > chunk->offset) {
+        if (chunk->prev->status == MEM_FREE) {
+            t_uint32 delta = offset - chunk->offset;
+            unlinkFreeMem(alloc, chunk->prev);
+            chunk->prev->size += delta;
+            chunk->offset = offset;
+            chunk->size -= delta;
+            updateFreeList(alloc, chunk->prev);
+        } else {
+            t_cm_chunk *tmp = splitChunk(alloc, chunk, offset, FREE_CHUNK_BEFORE); //tmp->next = chunk, tmp = result
+            tmp->status = MEM_USED;
+            tmp->prev->status = MEM_FREE;
+        }
+    }
+#endif
+    cm_MM_RA_checkAllocator(alloc);
+
+    return (t_memory_handle)chunk;
+}
+
+PUBLIC void cm_MM_Free(t_cm_allocator_desc* alloc, t_memory_handle memHandle)
+{
+    t_cm_chunk* chunk = (t_cm_chunk*)memHandle;
+
+    //TODO, juraj, alloc impacts trace format
+    cm_TRC_traceMem(TRACE_ALLOC_COMMAND_FREE, 0,
+            chunk->offset, chunk->size);
+
+    /* Update chunk status */
+    chunk->status = MEM_FREE;
+    chunk->domainId = 0x0;
+
+    /* Check if the previous chunk is free */
+    if((chunk->prev != 0) && (chunk->prev->status == MEM_FREE)) {
+        chunk = chunk->prev; //chunk, ie. chunk->next, will be freed
+        mergeChunk(alloc, chunk, chunk->next);
+    }
+
+    /* Check if the next chunk is free */
+    if((chunk->next != 0) && (chunk->next->status == MEM_FREE)) {
+        mergeChunk(alloc, chunk, chunk->next);
+    }
+
+    unlinkFreeMem(alloc, chunk);
+    updateFreeList(alloc, chunk);
+
+    if (cmIntensiveCheckState) {
+        cm_MM_RA_checkAllocator(alloc);
+    }
+}
+
+PUBLIC t_cm_error cm_MM_GetAllocatorStatus(t_cm_allocator_desc* alloc, t_uint32 offset, t_uint32 size, t_cm_allocator_status *pStatus)
+{
+    t_cm_chunk* chunk = alloc->chunks;
+    t_uint8 min_free_size_updated = FALSE;
+
+    /* Init status */
+    pStatus->global.used_block_number = 0;
+    pStatus->global.free_block_number = 0;
+    pStatus->global.maximum_free_size = 0;
+    pStatus->global.minimum_free_size = 0xFFFFFFFF;
+    pStatus->global.accumulate_free_memory = 0;
+    pStatus->global.accumulate_used_memory = 0;
+    pStatus->global.size = alloc->size;
+    pStatus->domain.maximum_free_size = 0;
+    pStatus->domain.minimum_free_size = 0xFFFFFFFF;
+    pStatus->domain.accumulate_free_memory = 0;
+    pStatus->domain.accumulate_used_memory = 0;
+    pStatus->domain.size= size;
+
+    //TODO, juraj, get allocator status for a domain
+    /* Parse all chunks */
+    while(chunk != 0)
+    {
+
+        /* Chunk is free */
+        if (chunk->status == MEM_FREE) {
+            pStatus->global.free_block_number++;
+            pStatus->global.accumulate_free_memory += chunk->size;
+
+            /* Check max size */
+            if (chunk->size > pStatus->global.maximum_free_size)
+            {
+                pStatus->global.maximum_free_size = chunk->size;
+            }
+
+            /* Check min size */
+            if (chunk->size < pStatus->global.minimum_free_size)
+            {
+                pStatus->global.minimum_free_size = chunk->size;
+                min_free_size_updated = TRUE;
+            }
+        } else {/* Chunk used */
+            pStatus->global.used_block_number++;
+            pStatus->global.accumulate_used_memory += chunk->size;
+        }
+
+        chunk = chunk->next;
+    }
+
+    /* Put max free size to min free size */
+    if (min_free_size_updated == FALSE) {
+        pStatus->global.minimum_free_size = pStatus->global.maximum_free_size;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC t_uint32 cm_MM_GetOffset(t_memory_handle memHandle)
+{
+    /* Provide offset */
+    return ((t_cm_chunk*)memHandle)->offset;
+}
+
+PUBLIC t_uint32 cm_MM_GetSize(t_memory_handle memHandle)
+{
+    return ((t_cm_chunk*)memHandle)->size;
+}
+
+PUBLIC t_uint32 cm_MM_GetAllocatorSize(t_cm_allocator_desc* alloc)
+{
+    return alloc->size;
+}
+
+PUBLIC void cm_MM_SetMemoryHandleUserData(t_memory_handle memHandle, t_uint16 userData)
+{
+    ((t_cm_chunk*)memHandle)->userData = userData;
+}
+
+PUBLIC void cm_MM_GetMemoryHandleUserData(t_memory_handle memHandle, t_uint16 *pUserData, t_cm_allocator_desc **alloc)
+{
+    *pUserData = ((t_cm_chunk*)memHandle)->userData;
+    if (alloc)
+        *alloc = ((t_cm_chunk*)memHandle)->alloc;
+}
+
+/*
+ * check free list is ordered
+ * check all chunks are correctly linked
+ * check adjacent chunks are not FREE
+ */
+static void cm_MM_RA_checkAllocator(t_cm_allocator_desc* alloc)
+{
+    t_cm_chunk *chunk = alloc->chunks;
+    t_cm_chunk *first = chunk;
+    t_cm_chunk *last = chunk;
+    t_uint32 size = 0;
+    int i;
+
+    while(chunk != 0) {
+        CM_ASSERT(chunk->alloc == alloc);
+
+        if (chunk->next != 0) {
+            CM_ASSERT(!((chunk->status == MEM_FREE) && (chunk->next->status == MEM_FREE))); //two free adjacent blocks
+            CM_ASSERT(chunk->offset < chunk->next->offset); //offsets reverted
+            last = chunk->next;
+        }
+        size += chunk->size;
+        chunk = chunk->next;
+    }
+
+    CM_ASSERT(size == alloc->size);
+
+    for(i = 0; i < BINS; i++)
+    {
+        chunk = alloc->free_mem_chunks[i];
+        while(chunk != 0) {
+            if (chunk->next_free_mem != 0) {
+                CM_ASSERT(chunk->size <= chunk->next_free_mem->size); //free list not ordered
+            }
+            CM_ASSERT(!(chunk->prev == 0 && (chunk != first))); //chunk not linked properly
+            CM_ASSERT(!(chunk->next == 0 && (chunk != last))); //chunk not linked property
+            chunk = chunk->next_free_mem;
+        }
+    }
+}
+
+#if 0
+static void cm_MM_RA_checkAlloc(t_cm_allocator_desc* alloc, t_uint32 size, t_uint32 align, t_uint32 min, t_uint32 max)
+{
+    t_cm_chunk *chunk = alloc->chunks;
+
+    while(chunk != 0) {
+        if (chunk->status == MEM_USED) {
+            chunk = chunk->next;
+            continue;
+        }
+        if (chunk->size < size) {
+            chunk = chunk->next;
+            continue;
+        }
+
+        if (min < chunk->offset) {
+            t_uint32 aligned_offset = ALIGN_VALUE(chunk->offset, align + 1);
+            t_uint32 aligned_end = aligned_offset + size;
+            if ((aligned_offset + size <= chunk->offset + chunk->size)
+                    && (chunk->offset + chunk->size <= aligned_end)){
+                break;
+            }
+        }
+
+        if (min >= chunk->offset) {
+            t_uint32 aligned_offset = ALIGN_VALUE(min, align + 1);
+            t_uint32 aligned_end = aligned_offset + size;
+            if ((aligned_offset + size <= chunk->offset + chunk->size)
+                    && (chunk->offset + chunk->size <= aligned_end)) {
+                break;
+            }
+        }
+
+        chunk = chunk->next;
+    }
+
+    CM_ASSERT(chunk == 0);
+}
+#endif
+
+/***************************************************************************/
+/*
+ * cm_mm_ra_getLastChunk
+ * param handle     : Handle of the allocator
+ * return : last
+ *
+ * Free all chunk in the allocator
+ * Free allocator descriptor
+ *
+ */
+/***************************************************************************/
+static t_cm_chunk* cm_MM_RA_getLastChunk(t_cm_allocator_desc* alloc)
+{
+    t_cm_chunk* pChunk = alloc->chunks;
+
+    while(pChunk->next != 0) {pChunk = pChunk->next;}
+
+    return pChunk;
+}
+
+PUBLIC void cm_MM_DumpMemory(t_cm_allocator_desc* alloc, t_uint32 start, t_uint32 end)
+{
+    t_cm_chunk *chunk = alloc->chunks;
+
+    LOG_INTERNAL(0, "ALLOCATOR Dumping allocator \"%s\" [0x%08x:0x%08x]\n", alloc->pAllocName, start, end, 0, 0, 0);
+    while(chunk != 0) {
+        if (((chunk->offset < start) && (chunk->offset + chunk->size > start))
+          || ((chunk->offset < end) && (chunk->offset + chunk->size > end))
+          || ((chunk->offset > start) && (chunk->offset + chunk->size < end))
+          || ((chunk->offset < start) && (chunk->offset + chunk->size > end)))
+        {
+            LOG_INTERNAL(0, "ALLOCATOR chunk 0x%08x -> 0x%08x: status:%s, domainId: 0x%x\n",
+                    chunk->offset,
+                    chunk->offset + chunk->size,
+                    chunk->status?"FREE":"USED",
+                    chunk->domainId, 0, 0);
+        }
+        chunk = chunk->next;
+    }
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator_utils.c b/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator_utils.c
new file mode 100644
index 00000000000..5c19d3e9b5e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator_utils.c
@@ -0,0 +1,270 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/memory/inc/remote_allocator_utils.h>
+
+/***************************************************************************/
+/*
+ * linkChunk
+ * param prev     : Pointer on previous chunk where the chunk will be added
+ * param add      : Pointer on chunk to add
+ *
+ * Add a chunk in the memory list
+ *
+ */
+/***************************************************************************/
+PUBLIC void linkChunk(t_cm_chunk* prev, t_cm_chunk* add)
+{
+    /* Link previous */
+    add->prev = prev;
+    add->next = prev->next;
+
+    /* Link next */
+    if (prev->next != 0)
+    {
+        prev->next->prev = add;
+    }
+    prev->next = add;
+}
+
+/***************************************************************************/
+/*
+ * unlinkChunk
+ * param allocHandle : Allocator handle
+ * param current     : Pointer on chunk to remove
+ *
+ * Remove a chunk in the memory list and update first pointer
+ *
+ */
+/***************************************************************************/
+PUBLIC void unlinkChunk(t_cm_allocator_desc* alloc ,t_cm_chunk* current)
+{
+    /* Unlink previous */
+    if (current->prev !=0)
+    {
+        current->prev->next = current->next;
+    }
+
+    /* Unlink next */
+    if (current->next !=0)
+    {
+        current->next->prev= current->prev;
+    }
+
+    /* Update first pointer */
+    if (alloc ->chunks == current)
+    {
+    alloc ->chunks = current->next;
+    }
+}
+
+
+/***************************************************************************/
+/*
+ * unlinkFreeMem() unlinks chunk from free memory double-linked list
+ *   makes the previous and next chunk in the list point to each other..
+ * param allocHandle : Allocator handle
+ * param current     : Pointer on chunk to remove
+ *
+ * Remove a chunk in the free memory list and update pointer
+ *
+ */
+/***************************************************************************/
+PUBLIC void unlinkFreeMem(t_cm_allocator_desc* alloc ,t_cm_chunk* current)
+{
+    int bin = bin_index(current->size);
+
+    /* unlink previous */
+    if (current->prev_free_mem != 0)
+    {
+        current->prev_free_mem->next_free_mem = current->next_free_mem;
+    }
+
+    /* Unlink next */
+    if (current->next_free_mem !=0 )
+    {
+        current->next_free_mem->prev_free_mem = current->prev_free_mem;
+    }
+
+    /* update first free pointer */
+    if (alloc->free_mem_chunks[bin] == current)
+    {
+        alloc->free_mem_chunks[bin] = current->next_free_mem;
+    }
+
+    current->prev_free_mem = 0;
+    current->next_free_mem = 0;
+}
+
+/***************************************************************************/
+/*
+ * linkFreeMemBefore
+ * param add      : Pointer on chunk to add
+ * param next     : Pointer on next chunk where the chunk will be added before
+ *
+ * Add a chunk in the free memory list
+ *
+ */
+/***************************************************************************/
+PUBLIC void linkFreeMemBefore(t_cm_chunk* add, t_cm_chunk* next)
+{
+    /* Link next */
+    add->prev_free_mem = next->prev_free_mem;
+    add->next_free_mem = next;
+
+    /* Link previous */
+    if (next->prev_free_mem != 0)
+    {
+        next->prev_free_mem->next_free_mem = add;
+    }
+    next->prev_free_mem = add;
+}
+
+/***************************************************************************/
+/*
+ * linkFreeMemAfter
+ * param add      : Pointer on chunk to add
+ * param prev     : Pointer on previous chunk where the chunk will be added after
+ *
+ * Add a chunk in the free memory list
+ *
+ */
+/***************************************************************************/
+PUBLIC void linkFreeMemAfter(t_cm_chunk* prev,t_cm_chunk* add)
+{
+    /* Link previous */
+    add->prev_free_mem = prev;
+    add->next_free_mem = prev->next_free_mem;
+
+    /* Link next */
+    if (prev->next_free_mem != 0)
+    {
+        prev->next_free_mem->prev_free_mem = add;
+    }
+    prev->next_free_mem = add;
+}
+
+
+/***************************************************************************/
+/*
+ * updateFreeList
+ * param allocHandle : Allocator handle
+ * param offset      : Pointer on chunk
+ *
+ * Update free memory list, ordered by size
+ *
+ */
+/***************************************************************************/
+PUBLIC void updateFreeList(t_cm_allocator_desc* alloc , t_cm_chunk* chunk)
+{
+    t_cm_chunk* free_chunk;
+    int bin = bin_index(chunk->size);
+
+    /* check case with no more free block */
+    if (alloc->free_mem_chunks[bin] == 0)
+    {
+        alloc->free_mem_chunks[bin] = chunk;
+        return ;
+    }
+
+    /* order list */
+    free_chunk = alloc->free_mem_chunks[bin];
+    while ((free_chunk->next_free_mem != 0) && (chunk->size > free_chunk->size))
+    {
+        free_chunk = free_chunk->next_free_mem;
+    }
+
+    /* Add after free chunk if smaller -> we are the last */
+    if(free_chunk->size <= chunk->size)
+    {
+        linkFreeMemAfter(free_chunk,chunk);
+    }
+    else // This mean that we are smaller
+    {
+        linkFreeMemBefore(chunk,free_chunk);
+
+        /* Update first free chunk */
+        if (alloc->free_mem_chunks[bin] == free_chunk)
+        {
+            alloc->free_mem_chunks[bin] = chunk;
+        }
+    }
+}
+
+
+/***************************************************************************/
+/*
+ * mergeChunk
+ * param allocHandle  : Allocator handle
+ * param merged_chunk : Pointer on merged chunk
+ * param destroy      : Pointer on chunk to destroy
+ *
+ * Link and destroy merged chunks
+ *
+ */
+/***************************************************************************/
+PUBLIC void mergeChunk(t_cm_allocator_desc* alloc,t_cm_chunk *merged_chunk, t_cm_chunk *destroy)
+{
+    /* Assign offset to the merged */
+    /* assume chunks ordered!
+    if (merged_chunk->offset > destroy->offset) {
+        merged_chunk->offset = destroy->offset;
+    }
+    */
+
+    /* Remove chunk */
+    unlinkChunk(alloc, destroy);
+    unlinkFreeMem(alloc, destroy);
+
+    if (merged_chunk->status == MEM_FREE)
+        unlinkFreeMem(alloc, merged_chunk);
+
+    /* Update size */
+    merged_chunk->size += destroy->size;
+
+    if (merged_chunk->status == MEM_FREE)
+        updateFreeList(alloc, merged_chunk);
+
+    freeChunk(destroy);
+}
+
+/***************************************************************************/
+/*
+ * splitChunk
+ * param allocHandle : Allocator handle
+ * param chunk       : Current chunk (modified in place)
+ * param offset      : Offset address of the start memory
+ * return            : New chunk handle or 0 if an error occurs
+ *
+ * Create new chunk before/after the current chunk with the size
+ */
+/***************************************************************************/
+PUBLIC t_cm_chunk* splitChunk(t_cm_allocator_desc* alloc ,t_cm_chunk *chunk,
+        t_uint32 offset, t_mem_split_position position)
+{
+    t_cm_chunk *free;
+    t_cm_chunk *returned;
+
+    t_cm_chunk* new_chunk = allocChunk();
+
+    if (position == FREE_CHUNK_AFTER) {
+        returned = chunk;
+        free = new_chunk;
+    } else { //FREE_CHUNK_BEFORE
+        returned = new_chunk;
+        free = chunk;
+    }
+
+    new_chunk->offset = offset;
+    new_chunk->size   = chunk->offset + chunk->size - offset;
+    new_chunk->alloc  = alloc;
+    chunk->size = offset - chunk->offset;
+
+    linkChunk(chunk, new_chunk);
+    unlinkFreeMem(alloc, free);
+    updateFreeList(alloc, free);
+
+    return returned;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h b/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h
new file mode 100644
index 00000000000..8d95436e796
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h
@@ -0,0 +1,471 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief OS Adaptation Layer API
+ *
+ * \defgroup CM_ENGINE_OSAL_API CM Engine OSAL (Operating System Abstraction Layer) API
+ * \ingroup CM_ENGINE_MODULE
+ */
+#ifndef __INC_CM_OSAL_H
+#define __INC_CM_OSAL_H
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/communication/inc/communication_type.h>
+
+/*!
+ * \brief Identifier of a trace channel (id in [0..255])
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+typedef t_uint8 t_nmf_trace_channel;
+
+/*!
+ * \brief Identifier of lock create by OSAL
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+typedef t_uint32 t_nmf_osal_sync_handle;
+
+/*!
+ * \brief Identifier of semaphore create by OSAL
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+typedef t_uint32 t_nmf_osal_sem_handle;
+
+/*!
+ * \brief Identifier of semaphore wait error return by semaphore OSAL API
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+typedef t_uint8 t_nmf_osal_sync_error;
+#define SYNC_ERROR_TIMEOUT      ((t_nmf_osal_sync_error)-1)
+#define SYNC_OK                 ((t_nmf_osal_sync_error)0)
+#define SEM_TIMEOUT_NORMAL      3000
+#define SEM_TIMEOUT_DEBUG       300000
+
+/*!
+ * \brief Description of the Scheduling part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Support of uplink communication path (from Media Processors to Host (ARM))
+ *
+ * Post a function call outside of Host CPU Interrupt mode in order to minimize ISR execution time
+ * \param[in] upLayerTHIS : this one provided by user when calling CM_ENGINE_BindComponentToCMCore() (first field of the interface context) \n
+ * \param[in] methodIndex : index method to be called  \n
+ * \param[in] anyPtr : internal NMF marshaled parameters block (to be passed as second parameter when calling the previous pSkeleton method) \n
+ * \param[in] ptrSize : size of anyPtr in bytes \n
+ *
+ * Called by:
+ *  - CM_ProcessMpcEvent() call (shall be bound by OS integrator to HSEM IRQ)
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+
+PUBLIC void OSAL_PostDfc(
+        t_nmf_mpc2host_handle   upLayerTHIS,
+        t_uint32                    methodIndex,
+        t_event_params_handle       anyPtr,
+        t_uint32                    ptrSize);
+
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to protect global variable against multiple call. Interrupt and scheduler function are use when
+ *       we take hardware/local semaphore. Scheduler lock functions can have empty implementation but this may
+ *       impact performance (dsp waiting semaphore because host thread was preempted whereas it has already take semaphore
+ *       but not yet release it).
+ *
+ * \return handle of the Mutex created
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_nmf_osal_sync_handle OSAL_CreateLock(void);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to protect global variable against multiple call. Interrupt and scheduler function are use when
+ *       we take hardware/local semaphore. Scheduler lock functions can have empty implementation but this may
+ *       impact performance (dsp waiting semaphore because host thread was preempted whereas it has already take semaphore
+ *       but not yet release it).
+ *
+ * \param[in] handle handle of the Mutex to be locked
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_Lock(
+        t_nmf_osal_sync_handle handle);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to protect global variable against multiple call. Interrupt and scheduler function are use when
+ *       we take hardware/local semaphore. Scheduler lock functions can have empty implementation but this may
+ *       impact performance (dsp waiting semaphore because host thread was preempted whereas it has already take semaphore
+ *       but not yet release it).
+ *
+ * \param[in] handle handle of the Mutex to be unlocked
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_Unlock(
+        t_nmf_osal_sync_handle handle);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to protect global variable against multiple call. Interrupt and scheduler function are use when
+ *       we take hardware/local semaphore. Scheduler lock functions can have empty implementation but this may
+ *       impact performance (dsp waiting semaphore because host thread was preempted whereas it has already take semaphore
+ *       but not yet release it).
+ *
+ * \param[in] handle handle of the Mutex to be destroyed
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_DestroyLock(
+        t_nmf_osal_sync_handle handle);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * \param[in] value : Initial value of semaphore.
+ *
+ * \return handle of the Semaphore created
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_nmf_osal_sem_handle OSAL_CreateSemaphore(
+        t_uint32 value);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to allow to synchronize with code running on mpc side. This function can be call under
+ *       Irq context by CM.
+ *
+ * \param[in] handle handle of the Semaphore for which we increase value and so potentially wake up thread.
+ *
+ * \param[in] aCtx is a hint to indicate to os that we are in a none normal context (e.g under interruption).
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_SemaphorePost(
+        t_nmf_osal_sem_handle handle,
+        t_uint8 aCtx);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * \param[in] handle of the Semaphore for which we decrease value and so potentially block current thread.
+ *
+ * \param[in] timeOutInMs maximun time in ms after which the block thread is wake up. In this case function return SYNC_ERROR_TIMEOUT value.
+ *
+ * \return error number: SYNC_ERROR_TIMEOUT in case semaphore is not release withing timeOutInMs.
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_nmf_osal_sync_error OSAL_SemaphoreWaitTimed(
+        t_nmf_osal_sem_handle handle,
+        t_uint32 timeOutInMs);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * \param[in] handle handle of the Semaphore to be destroyed
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_DestroySemaphore(
+        t_nmf_osal_sem_handle handle);
+
+/*!
+ * \brief Description of the System Memory Allocator part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Allocate CM some cacheable and bufferable memory (SDRAM) for internal usage \n
+ * This memory will be accessed only by Host CPU (ARM)
+ *
+ * This function provide a simple, general-purpose  memory  allocation. The
+ * OSAL_Alloc macro returns a pointer to a block of at least size bytes
+ * suitably aligned for any use. If there  is  no  available  memory, this
+ * function returns a null pointer.
+ *
+ * \param[in] size   size in bytes, of memory to be allocated
+ * \return pointer on the beginning of the allocated memory
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void* OSAL_Alloc(
+        t_cm_size size);
+
+/*!
+ * \brief Description of the System Memory Allocator part of the OS Adaptation Layer with memory set to zero
+ *
+ * Compare to \see OSAL_Alloc, same allocation is done but memory is set with zero before returning.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void* OSAL_Alloc_Zero(
+        t_cm_size size);
+
+/*!
+ * \brief Description of the System Memory Allocator part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Free CM some cacheable and bufferable memory (SDRAM) for internal usage \n
+ * This memory will be accessed only by Host CPU (ARM)
+ *
+ * \param[in] pHandle  pointer on the begining of the memory previously allocated
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void  OSAL_Free(
+        void *pHandle);
+
+/*!
+ * \brief Clean data cache in DDR in order to be accessible from peripheral.
+ *
+ * This method must be synchronized with MMDSP Code cache attribute.
+ *   Strongly Ordered -> nothing
+ *   Shared device -> dsb + L2 Write buffer drain
+ *   Non cacheable, Bufferable -> dsb + L2 Write buffer drain
+ *   WT or WB -> Flush cache range + dsb + L2 Write buffer drain
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_CleanDCache(
+        t_uint32                startAddr,          //!< [in] Start data address of range to clean
+        t_uint32                Size                //!< [in] Size of range to clean
+        );
+
+/*!
+ * \brief Flush write buffer.
+ *
+ * This method must be synchronized with MMDSP Data cache attribute.
+ *   Strongly Ordered -> nothing
+ *   Shared device -> dsb + L2 Write buffer drain
+ *   Non cacheable, Bufferable -> dsb + L2 Write buffer drain
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_mb(void);
+
+/*!
+ * \brief Description of the System Memory part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Copy some cacheable and bufferable memory (SDRAM) provided by a client to\n
+ * internal memory.
+ *
+ * \param[in] dst  : pointer on the begining of the internal memory previously allocated
+ * \param[in] src  : pointer on the begining of the client's memory
+ * \param[in] size : The size of the data to copy
+ *
+ * Called by:
+ *  - CM_ENGINE_PushComponent()
+ *
+ * \note This API is mainly provided for the OS were the client application does execute in the same
+ *       address space as the CM.
+ *       For example in Linux or Symbian, the client's address space is userland but the CM execute in
+ *       kernel space. Thus, 'dst' is supposed to be a kernel address but src is supposed to be a user
+ *       space address
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_cm_error OSAL_Copy(
+        void *dst,
+	const void *src,
+	t_cm_size size);
+
+/*!
+ * \brief Description of the internal log traces configuration of the Component Manager
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_Log(
+        const char *format,
+        int param1,
+        int param2,
+        int param3,
+        int param4,
+        int param5,
+        int param6);
+
+/*!
+ * \brief Generate an OS-Panic. Called in from CM_ASSERT().
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_Panic(void);
+
+/*!
+ * \brief Description of the configuration of the trace features
+ *
+ * (trace output itself is provided by user through his custom implementation of the generic APIs)
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_Write64(
+        t_nmf_trace_channel channel,
+        t_uint8 isTimestamped,
+        t_uint64 value);
+
+/*!
+ * \brief Power enabling/disabling commands description.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+typedef enum
+{
+    CM_OSAL_POWER_SxA_CLOCK,     //!< SxA Power & Clock, firstParam contains Core ID
+    CM_OSAL_POWER_SxA_AUTOIDLE,  //!< SxA AutoIdle, firstParam contains Core ID
+    CM_OSAL_POWER_SxA_HARDWARE,  //!< SxA Hardware Power, firstParam contains Core ID
+    CM_OSAL_POWER_HSEM,          //!< HSEM Power
+    CM_OSAL_POWER_SDRAM,         //!< SDRAM memory, firstParam contains physical resource address, secondParam contains size
+    CM_OSAL_POWER_ESRAM          //!< ESRAM memory, firstParam contains physical resource address, secondParam contains size
+} t_nmf_power_resource;
+
+/*!
+ * \brief Description of the Power Management part of the OS Adaptation Layer
+ *
+ * Use by CM engine to disable a logical power domain  (see \ref t_nmf_power_resource)
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_DisablePwrRessource(
+        t_nmf_power_resource    resource,           //!< [in] Describe the domain which must be disabled
+        t_uint32                firstParam,         //!< [in] Eventual first parameter to power to disable
+        t_uint32                secondParam         //!< [in] Eventual second parameter to power to disable
+        );
+
+/*!
+ * \brief Description of the Power Management part of the OS Adaptation Layer
+ *
+ * Use by CM engine to enable a logical power domain  (see \ref t_nmf_power_resource)
+ *
+ * \return
+ *   - \ref CM_OK
+ *   - \ref CM_PWR_NOT_AVAILABLE A specified power domain is not managed (see returned value in aPowerMask)
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_cm_error OSAL_EnablePwrRessource(
+        t_nmf_power_resource    resource,           //!< [in] Describing the domains which must be enabled
+        t_uint32                firstParam,         //!< [in] Eventual first parameter to power to disable
+        t_uint32                secondParam         //!< [in] Eventual second parameter to power to disable
+        );
+
+
+/*!
+ * \brief return prcmu timer value.
+ *
+ * This is need for perfmeter api  (see \ref t_nmf_power_resource)
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_uint64 OSAL_GetPrcmuTimer(void);
+
+/*!
+ * \brief Disable the service message handling (panic, etc)
+ *
+ * It must disable the handling of all service messages
+ * If a service message is currently handled, it must wait till the end
+ * of its managment before returning.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_DisableServiceMessages(void);
+
+/*!
+ * \brief Enable the service message handling (panic, etc)
+ *
+ * It enables the handling of all service messages
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_EnableServiceMessages(void);
+
+extern /*const*/ t_nmf_osal_sync_handle lockHandleApi;
+extern /*const*/ t_nmf_osal_sync_handle lockHandleCom;
+extern /*const*/ t_nmf_osal_sem_handle semHandle;
+
+/*!
+ * \brief Take a lock before entering critical section. Can suspend current thread if lock already taken. \n
+ *        Use this macro in api function. For com function use OSAL_LOCK_COM.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+#define OSAL_LOCK_API() OSAL_Lock(lockHandleApi)
+
+/*!
+ * \brief Release lock before leaving critical section.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+#define OSAL_UNLOCK_API() OSAL_Unlock((lockHandleApi))
+
+/*!
+ * \brief Take a lock before entering critical section. Can suspend current thread if lock already taken. \n
+ *        Use this macro in com function. For com function use OSAL_LOCK_API.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+#define OSAL_LOCK_COM() OSAL_Lock(lockHandleCom)
+
+/*!
+ * \brief Release lock before leaving critical section.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+#define OSAL_UNLOCK_COM() OSAL_Unlock((lockHandleCom))
+
+/*!
+ * \brief Go to sleep untill post done on semaphore or timeout expire. In that case SYNC_ERROR_TIMEOUT is return.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+#define OSAL_SEMAPHORE_WAIT_TIMEOUT(semHandle) OSAL_SemaphoreWaitTimed(semHandle, (cm_PWR_GetMode() == NORMAL_PWR_MODE)?SEM_TIMEOUT_NORMAL:SEM_TIMEOUT_DEBUG)
+
+
+/****************/
+/* Generic part */
+/****************/
+t_cm_error cm_OSAL_Init(void);
+void cm_OSAL_Destroy(void);
+
+#endif /* __INC_CM_OSAL_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/src/os_adaptation_layer.c b/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/src/os_adaptation_layer.c
new file mode 100644
index 00000000000..9beb60dea18
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/src/os_adaptation_layer.c
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+t_nmf_osal_sync_handle lockHandleApi;
+t_nmf_osal_sync_handle lockHandleCom;
+t_nmf_osal_sem_handle semHandle;
+
+/****************/
+/* Generic part */
+/****************/
+PUBLIC t_cm_error cm_OSAL_Init(void)
+{
+
+    /* create locks */
+    lockHandleApi = OSAL_CreateLock();
+    if (lockHandleApi == 0) {return CM_INVALID_PARAMETER;}
+    lockHandleCom = OSAL_CreateLock();
+	if (lockHandleCom == 0) {return CM_INVALID_PARAMETER;}
+
+    /* create semaphore */
+	semHandle = OSAL_CreateSemaphore(0);
+	if (semHandle == 0) {return CM_INVALID_PARAMETER;}
+
+    return CM_OK;
+}
+
+PUBLIC void cm_OSAL_Destroy(void)
+{
+	/* destroy locks */
+	OSAL_DestroyLock(lockHandleApi);
+	OSAL_DestroyLock(lockHandleCom);
+
+	/* destroy semaphore */
+	OSAL_DestroySemaphore(semHandle);
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/mpcload.h b/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/mpcload.h
new file mode 100644
index 00000000000..0831f1940ca
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/mpcload.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ */
+#ifndef MPCLOAD_H_
+#define MPCLOAD_H_
+
+#include <cm/engine/component/inc/instance.h>
+
+/******************************************************************************/
+/************************ FUNCTIONS PROTOTYPES ********************************/
+/******************************************************************************/
+
+PUBLIC t_cm_error cm_PFM_allocatePerfmeterDataMemory(t_nmf_core_id coreId, t_cm_domain_id domainId);
+PUBLIC void cm_PFM_deallocatePerfmeterDataMemory(t_nmf_core_id coreId);
+
+#endif /* MPCLOAD_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/perfmeter_type.h b/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/perfmeter_type.h
new file mode 100644
index 00000000000..78de395acc2
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/perfmeter_type.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Public Component Manager Performance Meter API type.
+ *
+ * This file contains the Component Manager API type for performance meter.
+ *
+ * \defgroup PERFMETER CM Monitoring API
+ * \ingroup CM_USER_API
+ */
+#ifndef CM_COMMON_PERFMETER_TYPE_H_
+#define CM_COMMON_PERFMETER_TYPE_H_
+
+#include <cm/inc/cm_type.h>
+/*!
+ * \brief Description of mpc load structure.
+ *
+ * This contain mpc load value.
+ *
+ * \ingroup PERFMETER
+ */
+typedef struct {
+    t_uint64 totalCounter;
+    t_uint64 loadCounter;
+} t_cm_mpc_load_counter;
+
+
+#endif /* CM_COMMON_PERFMETER_TYPE_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/perfmeter/src/mpcload.c b/drivers/staging/nmf-cm/cm/engine/perfmeter/src/mpcload.c
new file mode 100644
index 00000000000..b157103934d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/perfmeter/src/mpcload.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/perfmeter/inc/mpcload.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+
+#include <cm/engine/api/perfmeter_engine.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+#define PERFMETER_MAX_RETRIES                           32
+#define PERFMETER_DATA_WORD_NB                          7
+
+/* private type */
+typedef struct {
+    t_memory_handle perfmeterDataHandle;
+    t_cm_logical_address perfmeterDataAddr;
+} t_mpcLoad;
+
+/* private globals */
+t_mpcLoad mpcLoad_i[NB_CORE_IDS];
+
+/* engine api */
+PUBLIC EXPORT_SHARED t_cm_error CM_GetMpcLoadCounter(
+    t_nmf_core_id coreId,
+    t_cm_mpc_load_counter *pMpcLoadCounter
+)
+{
+    t_uint24 data[PERFMETER_DATA_WORD_NB];
+    t_uint32 i;
+    t_uint64 prcmuBeforeAttributes;
+    t_uint32 retryCounter = 0;
+    volatile t_uint32 *pData;
+
+    pMpcLoadCounter->totalCounter = 0;
+    pMpcLoadCounter->loadCounter = 0;
+    /* check core id is an mpc */
+    if (coreId < FIRST_MPC_ID || coreId > LAST_CORE_ID) {return CM_INVALID_PARAMETER;}
+
+    /* check core has been booted */
+    pData = (t_uint32 *) mpcLoad_i[coreId].perfmeterDataAddr;
+    if (pData == NULL) {return CM_OK;}
+
+    do {
+        prcmuBeforeAttributes = OSAL_GetPrcmuTimer();
+        /* get attributes */
+        do
+        {
+            for(i = 0;i < PERFMETER_DATA_WORD_NB;i++)
+                data[i] = pData[i];
+        }
+        while(((data[0] & 0xff0000) != (data[1] & 0xff0000) || (data[0] & 0xff0000) != (data[2] & 0xff0000) ||
+               (data[0] & 0xff0000) != (data[3] & 0xff0000) || (data[0] & 0xff0000) != (data[4] & 0xff0000) ||
+               (data[0] & 0xff0000) != (data[5] & 0xff0000) || (data[0] & 0xff0000) != (data[6] & 0xff0000) ||
+               (data[0] & 0xff0000) != (data[6] & 0xff0000))
+               && retryCounter-- < PERFMETER_MAX_RETRIES); // check data coherence
+        if (retryCounter >= PERFMETER_MAX_RETRIES)
+            return CM_MPC_NOT_RESPONDING;
+
+        /* read forever counter for totalCounter */
+        pMpcLoadCounter->totalCounter = OSAL_GetPrcmuTimer();
+    } while(pMpcLoadCounter->totalCounter - prcmuBeforeAttributes >= 32); //we loop until it seems we have not be preempt too long (< 1ms)
+
+    /* we got coherent data, use them */
+    pMpcLoadCounter->loadCounter = ((data[0] & (t_uint64)0xffff) << 32) + ((data[1] & (t_uint64)0xffff) << 16) + ((data[2] & (t_uint64)0xffff) << 0);
+    //fix load counter if needed
+    if ((data[6] & 0xffff) == 1) {
+        t_uint64 lastEvent;
+
+        lastEvent = ((data[3] & (t_uint64)0xffff) << 32) + ((data[4] & (t_uint64)0xffff) << 16) + ((data[5] & (t_uint64)0xffff) << 0);
+        pMpcLoadCounter->loadCounter += pMpcLoadCounter->totalCounter - lastEvent;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_getMpcLoadCounter(
+    t_nmf_core_id coreId,
+    t_cm_mpc_load_counter *pMpcLoadCounter
+)
+{
+	t_cm_error error;
+
+    OSAL_LOCK_API();
+    error = CM_GetMpcLoadCounter(coreId, pMpcLoadCounter);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+/* internal api */
+PUBLIC t_cm_error cm_PFM_allocatePerfmeterDataMemory(t_nmf_core_id coreId, t_cm_domain_id domainId)
+{
+    t_cm_error error = CM_OK;
+    t_mpcLoad *pMpcLoad = (t_mpcLoad *) &mpcLoad_i[coreId];
+
+    pMpcLoad->perfmeterDataHandle = cm_DM_Alloc(domainId, SDRAM_EXT24, PERFMETER_DATA_WORD_NB, CM_MM_ALIGN_WORD, TRUE);
+    if (pMpcLoad->perfmeterDataHandle == INVALID_MEMORY_HANDLE)
+        error = CM_NO_MORE_MEMORY;
+    else {
+        t_uint32 mmdspAddr;
+
+        pMpcLoad->perfmeterDataAddr = cm_DSP_GetHostLogicalAddress(pMpcLoad->perfmeterDataHandle);
+        cm_DSP_GetDspAddress(pMpcLoad->perfmeterDataHandle, &mmdspAddr);
+        cm_writeAttribute(cm_EEM_getExecutiveEngine(coreId)->instance, "rtos/perfmeter/perfmeterDataAddr", mmdspAddr);
+    }
+
+    return error;
+}
+
+PUBLIC void cm_PFM_deallocatePerfmeterDataMemory(t_nmf_core_id coreId)
+{
+    mpcLoad_i[coreId].perfmeterDataAddr = 0;
+    cm_DM_Free(mpcLoad_i[coreId].perfmeterDataHandle, TRUE);
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/power_mgt/inc/power.h b/drivers/staging/nmf-cm/cm/engine/power_mgt/inc/power.h
new file mode 100644
index 00000000000..1ca1b314833
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/power_mgt/inc/power.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Enable a CM power domain by CoreID.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+#ifndef __INC_NMF_POWER
+#define __INC_NMF_POWER
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+typedef enum
+{
+    NORMAL_PWR_MODE = 0x1,        //!< Normal mode
+    DISABLE_PWR_MODE = 0x2        //!< Disable mode - CM Power management is disabled. CM Power domain are always enabled and the EEs are loaded by default
+} t_nmf_power_mode;
+
+PUBLIC t_cm_error cm_PWR_Init(void);
+void cm_PWR_SetMode(t_nmf_power_mode aMode);
+t_nmf_power_mode cm_PWR_GetMode(void);
+t_uint32 cm_PWR_GetMPCMemoryCount(t_nmf_core_id coreId);
+
+typedef enum
+{
+    MPC_PWR_CLOCK,
+    MPC_PWR_AUTOIDLE,
+    MPC_PWR_HWIP
+} t_mpc_power_request;
+
+PUBLIC t_cm_error cm_PWR_EnableMPC(
+        t_mpc_power_request             request,
+        t_nmf_core_id                   coreId);
+PUBLIC void cm_PWR_DisableMPC(
+        t_mpc_power_request             request,
+        t_nmf_core_id                   coreId);
+
+PUBLIC t_cm_error cm_PWR_EnableHSEM(void);
+PUBLIC void cm_PWR_DisableHSEM(void);
+
+PUBLIC t_cm_error cm_PWR_EnableMemory(
+        t_nmf_core_id                   coreId,
+        t_dsp_memory_type_id            dspMemType,
+        t_cm_physical_address           address,
+        t_cm_size                       size);
+PUBLIC void cm_PWR_DisableMemory(
+        t_nmf_core_id                   coreId,
+        t_dsp_memory_type_id            dspMemType,
+        t_cm_physical_address           address,
+        t_cm_size                       size);
+
+
+#endif /* __INC_NMF_POWER */
diff --git a/drivers/staging/nmf-cm/cm/engine/power_mgt/src/cmpower.c b/drivers/staging/nmf-cm/cm/engine/power_mgt/src/cmpower.c
new file mode 100644
index 00000000000..5b206bfb82d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/power_mgt/src/cmpower.c
@@ -0,0 +1,226 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include "../inc/power.h"
+
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/convert.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+// -------------------------------------------------------------------------------
+// Compilation flags
+// -------------------------------------------------------------------------------
+#define __PWR_DEBUG_TRACE_LEVEL 2  					// Debug trave level for CM power module
+
+// -------------------------------------------------------------------------------
+// Internal counter to store the TCM allocated chunk (by MPC)
+// -------------------------------------------------------------------------------
+static t_uint32 _pwrMPCHWIPCountT[NB_CORE_IDS];
+
+// -------------------------------------------------------------------------------
+// Internal counter to store the TCM allocated chunk (by MPC)
+// -------------------------------------------------------------------------------
+static t_uint32 _pwrMPCMemoryCountT[NB_CORE_IDS];
+
+
+// -------------------------------------------------------------------------------
+// Internal data to store the global Power Manager mode (see cm_PWR_Init fct)
+// -------------------------------------------------------------------------------
+static t_nmf_power_mode _pwrMode = NORMAL_PWR_MODE;
+
+// -------------------------------------------------------------------------------
+// cm_PWR_Init
+// -------------------------------------------------------------------------------
+PUBLIC t_cm_error cm_PWR_Init(void)
+{
+	int i;
+
+	for (i=0; i<NB_CORE_IDS;i++)
+	{
+        _pwrMPCHWIPCountT[i] = 0;
+        _pwrMPCMemoryCountT[i] = 0;
+	}
+
+	return CM_OK;
+}
+
+// -------------------------------------------------------------------------------
+// cm_PWR_SetMode
+// -------------------------------------------------------------------------------
+void cm_PWR_SetMode(t_nmf_power_mode aMode)
+{
+	_pwrMode = aMode;
+}
+
+t_nmf_power_mode cm_PWR_GetMode()
+{
+    return _pwrMode;
+}
+
+t_uint32 cm_PWR_GetMPCMemoryCount(t_nmf_core_id coreId)
+{
+    return _pwrMPCMemoryCountT[coreId];
+}
+
+
+PUBLIC t_cm_error cm_PWR_EnableMPC(
+        t_mpc_power_request             request,
+        t_nmf_core_id                   coreId)
+{
+    t_cm_error error;
+
+    switch(request)
+    {
+    case MPC_PWR_CLOCK:
+        LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] MPC %s enable clock\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+        if((error = OSAL_EnablePwrRessource(CM_OSAL_POWER_SxA_CLOCK, coreId, 0)) != CM_OK)
+        {
+            ERROR("[Pwr] MPC %s clock can't be enabled\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+            return error;
+        }
+        break;
+    case MPC_PWR_AUTOIDLE:
+        if((error = OSAL_EnablePwrRessource(CM_OSAL_POWER_SxA_AUTOIDLE, coreId, 0)) != CM_OK)
+        {
+            ERROR("[Pwr] MPC %s clock can't be auto-idle\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+            return error;
+        }
+        break;
+    case MPC_PWR_HWIP:
+        if(_pwrMPCHWIPCountT[coreId]++ == 0)
+        {
+            LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] MPC %s HW IP enable clock\n",cm_getDspName(coreId), 0, 0, 0, 0, 0);
+            if((error = OSAL_EnablePwrRessource(CM_OSAL_POWER_SxA_HARDWARE, coreId, 0)) != CM_OK)
+            {
+                ERROR("[Pwr] MPC %s HW IP clock can't be enabled\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+                return error;
+            }
+        }
+        break;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC void cm_PWR_DisableMPC(
+        t_mpc_power_request             request,
+        t_nmf_core_id                   coreId)
+{
+    switch(request)
+    {
+    case MPC_PWR_CLOCK:
+        LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] MPC %s disable clock\n",cm_getDspName(coreId), 0, 0, 0, 0, 0);
+        OSAL_DisablePwrRessource(CM_OSAL_POWER_SxA_CLOCK, coreId, 0);
+        break;
+    case MPC_PWR_AUTOIDLE:
+        OSAL_DisablePwrRessource(CM_OSAL_POWER_SxA_AUTOIDLE, coreId, 0);
+        break;
+    case MPC_PWR_HWIP:
+        if(--_pwrMPCHWIPCountT[coreId] == 0)
+        {
+            LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] MPC %s HW IP disable clock\n",cm_getDspName(coreId), 0, 0, 0, 0, 0);
+            OSAL_DisablePwrRessource(CM_OSAL_POWER_SxA_HARDWARE, coreId, 0);
+        }
+        break;
+    }
+}
+
+PUBLIC t_cm_error cm_PWR_EnableHSEM(void)
+{
+    t_cm_error error;
+
+    LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] HSEM enable clock\n",0 , 0, 0, 0, 0, 0);
+    if((error = OSAL_EnablePwrRessource(CM_OSAL_POWER_HSEM, 0, 0)) != CM_OK)
+    {
+        ERROR("[Pwr] HSEM clock can't be enabled\n", 0, 0, 0, 0, 0, 0);
+        return error;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC void cm_PWR_DisableHSEM(void)
+{
+    LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] HSEM disable clock\n",0 , 0, 0, 0, 0, 0);
+    OSAL_DisablePwrRessource(CM_OSAL_POWER_HSEM, 0, 0);
+}
+
+PUBLIC t_cm_error cm_PWR_EnableMemory(
+        t_nmf_core_id                   coreId,
+        t_dsp_memory_type_id            dspMemType,
+        t_cm_physical_address           address,
+        t_cm_size                       size)
+{
+    switch(dspMemType)
+    {
+    case INTERNAL_XRAM24:
+    case INTERNAL_XRAM16:
+    case INTERNAL_YRAM24:
+    case INTERNAL_YRAM16:
+        _pwrMPCMemoryCountT[coreId]++;
+        break;
+    case SDRAM_EXT24:
+    case SDRAM_EXT16:
+    case SDRAM_CODE:
+    case LOCKED_CODE:
+        return OSAL_EnablePwrRessource(
+                CM_OSAL_POWER_SDRAM,
+                address,
+                size);
+    case ESRAM_EXT24:
+    case ESRAM_EXT16:
+    case ESRAM_CODE:
+        return OSAL_EnablePwrRessource(
+                CM_OSAL_POWER_ESRAM,
+                address,
+                size);
+    default:
+	    CM_ASSERT(0);
+    }
+
+    return CM_OK;
+}
+
+PUBLIC void cm_PWR_DisableMemory(
+        t_nmf_core_id                   coreId,
+        t_dsp_memory_type_id            dspMemType,
+        t_cm_physical_address           address,
+        t_cm_size                       size)
+{
+    switch(dspMemType)
+    {
+    case INTERNAL_XRAM24:
+    case INTERNAL_XRAM16:
+    case INTERNAL_YRAM24:
+    case INTERNAL_YRAM16:
+        _pwrMPCMemoryCountT[coreId]--;
+        break;
+    case SDRAM_EXT24:
+    case SDRAM_EXT16:
+    case SDRAM_CODE:
+    case LOCKED_CODE:
+        OSAL_DisablePwrRessource(
+                CM_OSAL_POWER_SDRAM,
+                address,
+                size);
+        break;
+    case ESRAM_EXT24:
+    case ESRAM_EXT16:
+    case ESRAM_CODE:
+        OSAL_DisablePwrRessource(
+                CM_OSAL_POWER_ESRAM,
+                address,
+                size);
+        break;
+    default:
+	    CM_ASSERT(0);
+    }
+}
+
+
+
+
+
diff --git a/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_mgt.h b/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_mgt.h
new file mode 100644
index 00000000000..d2c7185b24f
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_mgt.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Component repository internal methods.
+ *
+ * \defgroup REPOSITORY_INTERNAL Component repository.
+ */
+#ifndef __INC_CM_REP_MGT_H
+#define __INC_CM_REP_MGT_H
+
+#include <cm/inc/cm_type.h>
+#include <inc/nmf-limits.h>
+
+/*!
+ * \brief Identification of a component entry.
+ * \ingroup REPOSITORY_INTERNAL
+ */
+typedef struct t_rep_component {
+    t_dup_char             name;
+	struct t_rep_component *prev;
+	struct t_rep_component *next;
+	t_elfdescription *elfhandle;                      //!< Must be last as data will be stored here
+} t_rep_component;
+
+/*!
+ * \brief Search a component entry by name.
+ *
+ * \param[in]  name The name of the component to look for.
+ * \param[out] component The corresponding component entry in the repository
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup REPOSITORY_INTERNAL
+ */
+PUBLIC t_cm_error cm_REP_lookupComponent(const char *name, t_rep_component **component);
+
+/*!
+ * \brief Helper method that return the dataFile found in parameter or in the cache
+ */
+t_elfdescription* cm_REP_getComponentFile(t_dup_char templateName, t_elfdescription* elfhandle);
+
+/*!
+ * \brief Destroy the full repository (remove and free all components)
+ *
+ * \retval none
+ *
+ * \ingroup REPOSITORY_INTERNAL
+ */
+PUBLIC void cm_REP_Destroy(void);
+
+#endif /* __INC_CM_REP_MGT_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_type.h b/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_type.h
new file mode 100644
index 00000000000..7c41fe1bedb
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_type.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Components Component Manager API type.
+ *
+ * \defgroup COMPONENT CM Components API
+ * \ingroup CM_USER_API
+ */
+
+#ifndef REPOSITORY_TYPE_H_
+#define REPOSITORY_TYPE_H_
+
+typedef enum
+{
+    GET_EE_NAME,
+    BIND_ASYNC,
+    BIND_TRACE,
+    BIND_FROMUSER,
+    BIND_TOUSER
+} t_action_to_do;
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/repository_mgt/src/repository_mgt.c b/drivers/staging/nmf-cm/cm/engine/repository_mgt/src/repository_mgt.c
new file mode 100644
index 00000000000..c972783db4b
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/repository_mgt/src/repository_mgt.c
@@ -0,0 +1,311 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/utils/inc/string.h>
+
+#include <cm/engine/component/inc/component_type.h>
+#include <cm/engine/component/inc/bind.h>
+#include <cm/engine/configuration/inc/configuration.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/repository_mgt/inc/repository_mgt.h>
+#include <cm/engine/api/repository_mgt_engine.h>
+#include <cm/engine/trace/inc/trace.h>
+
+
+#define NHASH 157       //Use a prime number!
+#define MULT 17
+
+static t_rep_component *componentCaches[NHASH];
+
+static unsigned int repcomponentHash(const char *str)
+{
+    unsigned int h = 0;
+    for(; *str; str++)
+        h = MULT * h + *str;
+    return h % NHASH;
+}
+
+static void repcomponentAdd(t_rep_component *component)
+{
+    unsigned int h = repcomponentHash(component->name);
+
+    if(componentCaches[h] != NULL)
+        componentCaches[h]->prev = component;
+    component->next = componentCaches[h];
+    component->prev = NULL;
+    componentCaches[h] = component;
+}
+
+static void repcomponentRemove(t_rep_component *component)
+{
+    unsigned int h = repcomponentHash(component->name);
+
+    if(component->prev != NULL)
+        component->prev->next = component->next;
+    if(component->next != NULL)
+        component->next->prev = component->prev;
+    if(component == componentCaches[h])
+        componentCaches[h] = component->next;
+}
+
+
+PUBLIC t_cm_error cm_REP_lookupComponent(const char *name, t_rep_component **component)
+{
+    t_rep_component *tmp;
+
+    for(tmp = componentCaches[repcomponentHash(name)]; tmp != NULL; tmp = tmp->next)
+    {
+        if(cm_StringCompare(name, tmp->name, MAX_TEMPLATE_NAME_LENGTH) == 0)
+        {
+            if(component != NULL)
+                *component = tmp;
+            return CM_OK;
+        }
+    }
+
+    return CM_COMPONENT_NOT_FOUND;
+}
+
+t_elfdescription* cm_REP_getComponentFile(t_dup_char templateName, t_elfdescription* elfhandle)
+{
+    if(elfhandle == NULL)
+    {
+        t_rep_component *pRepComponent;
+
+        for(pRepComponent = componentCaches[repcomponentHash(templateName)]; pRepComponent != NULL; pRepComponent = pRepComponent->next)
+        {
+            if(pRepComponent->name == templateName)
+                return pRepComponent->elfhandle;
+        }
+
+        return NULL;
+    }
+
+    return elfhandle;
+}
+
+
+PUBLIC void cm_REP_Destroy(void)
+{
+	t_rep_component *component, *next;
+	int i;
+
+	for(i = 0; i < NHASH; i++)
+	{
+	    for (component = componentCaches[i]; component != NULL; component = next)
+	    {
+	        next = component->next;
+	        cm_ELF_CloseFile(FALSE, component->elfhandle);
+	        cm_StringRelease(component->name);
+	        OSAL_Free(component);
+	    }
+	    componentCaches[i] = NULL;
+	}
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetRequiredComponentFiles(
+        // IN
+        t_action_to_do action,
+        const t_cm_instance_handle client,
+        const char *requiredItfClientName,
+        const t_cm_instance_handle server,
+        const char *providedItfServerName,
+        // OUT component to be pushed
+        char fileList[][MAX_INTERFACE_TYPE_NAME_LENGTH],
+        t_uint32 listSize,
+        // OUT interface information
+        char type[MAX_INTERFACE_TYPE_NAME_LENGTH],
+        t_uint32 *methodNumber)
+{
+    t_cm_error error;
+    t_component_instance* compClient, *compServer;
+    int n;
+
+    OSAL_LOCK_API();
+
+    // No component required
+    for(n = 0; n < listSize; n++)
+        fileList[n][0] = 0;
+
+    compClient = cm_lookupComponent(client);
+    compServer = cm_lookupComponent(server);
+    switch(action)
+    {
+    case GET_EE_NAME:
+        /* Get Executive Engine names */
+        error = cm_CFG_GetRequiredExecutiveEngineComponentNames(fileList, listSize);
+        break;
+
+    case BIND_FROMUSER:{
+        t_interface_provide_description itfProvide;
+
+        // Check server validity
+        if((error = cm_checkValidServer(compServer, providedItfServerName,
+                &itfProvide)) == CM_OK)
+        {
+            cm_StringCopy(type, itfProvide.server->template->provides[itfProvide.provideIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+            cm_StringCopy(fileList[0], "_sk.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+            cm_StringConcatenate(fileList[0], itfProvide.server->template->provides[itfProvide.provideIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+        }
+    } break;
+
+    case BIND_TOUSER: {
+        /* Get Components names for a BindComponentToCMCore */
+        t_interface_require_description itfRequire;
+        t_bool bindable;
+
+        // Check client validity
+        if((error = cm_checkValidClient(compClient, requiredItfClientName,
+                &itfRequire, &bindable)) == CM_OK)
+        {
+            cm_StringCopy(type, itfRequire.client->template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+            *methodNumber = itfRequire.client->template->requires[itfRequire.requireIndex].interface->methodNumber;
+
+            cm_StringCopy(fileList[0], "_st.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+            cm_StringConcatenate(fileList[0], itfRequire.client->template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+        }
+    }; break;
+
+    case BIND_ASYNC: {
+        /* Get Components names for an asynchronous binding */
+        t_interface_require_description itfRequire;
+        t_interface_provide_description itfProvide;
+        t_bool bindable;
+
+        // Check invalid binding
+        if((error = cm_checkValidBinding(compClient, requiredItfClientName,
+                compServer, providedItfServerName,
+                &itfRequire, &itfProvide, &bindable)) == CM_OK)
+        {
+                if(compClient->template->dspId != compServer->template->dspId)
+                {
+                    cm_StringCopy(fileList[0], "_sk.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+                    cm_StringConcatenate(fileList[0], itfRequire.client->template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+                    cm_StringCopy(fileList[1], "_st.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+                    cm_StringConcatenate(fileList[1], itfRequire.client->template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+                }
+                else
+                {
+                    cm_StringCopy(fileList[0], "_ev.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+                    cm_StringConcatenate(fileList[0], itfRequire.client->template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+                }
+        }
+    }; break;
+
+    case BIND_TRACE: {
+        /* Get Components names for an asynchronous binding */
+        t_interface_require_description itfRequire;
+        t_interface_provide_description itfProvide;
+        t_bool bindable;
+
+        // Check invalid binding
+        if((error = cm_checkValidBinding(compClient, requiredItfClientName,
+                compServer, providedItfServerName,
+                &itfRequire, &itfProvide, &bindable)) == CM_OK)
+        {
+            cm_StringCopy(fileList[0], "_tr.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+            cm_StringConcatenate(fileList[0], itfRequire.client->template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+        }
+    }; break;
+
+    default:
+        error = CM_OK;
+        break;
+    }
+
+    if(error == CM_OK)
+    {
+        for(n = 0; n < listSize; n++)
+        {
+            t_rep_component *comp;
+
+            // If already loaded, don't ask to load it and put the name to NULL
+            if (fileList[n][0] != 0 &&
+                    cm_REP_lookupComponent(fileList[n], &comp) == CM_OK)
+                fileList[n][0] = 0;
+        }
+    }
+
+
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_PushComponent(const char *name, const void *data, t_cm_size size)
+{
+	t_rep_component *comp;
+	t_cm_error error;
+
+	OSAL_LOCK_API();
+
+	if (cm_REP_lookupComponent(name, &comp) == CM_OK) {
+		/* Component is already there: silently ignore it */
+		OSAL_UNLOCK_API();
+		return CM_OK;
+	}
+
+	comp = OSAL_Alloc(sizeof(*comp));
+	if (comp == NULL) {
+		OSAL_UNLOCK_API();
+		return CM_NO_MORE_MEMORY;
+	}
+
+	comp->name = cm_StringDuplicate(name);
+	if(comp->name == NULL)
+	{
+        OSAL_Free(comp);
+        OSAL_UNLOCK_API();
+        return CM_NO_MORE_MEMORY;
+	}
+
+    if((error = cm_ELF_CheckFile(
+            data,
+            FALSE,
+            &comp->elfhandle)) != CM_OK) {
+        cm_StringRelease(comp->name);
+        OSAL_Free(comp);
+        OSAL_UNLOCK_API();
+        return error;
+    }
+/*
+	if (OSAL_Copy(comp->data, data, size)) {
+		OSAL_Free(comp);
+		OSAL_UNLOCK_API();
+		return CM_UNKNOWN_MEMORY_HANDLE;
+	}*/
+
+    repcomponentAdd(comp);
+
+	OSAL_UNLOCK_API();
+	return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_ReleaseComponent (const char *name)
+{
+	t_rep_component *component;
+	t_cm_error err;
+
+	OSAL_LOCK_API();
+	err = cm_REP_lookupComponent(name , &component);
+
+	if (CM_OK == err)
+	{
+	    repcomponentRemove(component);
+
+	    cm_ELF_CloseFile(FALSE, component->elfhandle);
+	    cm_StringRelease(component->name);
+	    OSAL_Free(component);
+	}
+
+	OSAL_UNLOCK_API();
+
+	return err;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h b/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h
new file mode 100644
index 00000000000..bd914195b6d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_HW_SEMA_H_
+#define __INC_HW_SEMA_H_
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <share/semaphores/inc/hwsem_hwp.h>
+
+
+/******************************************************************************/
+/************************ FUNCTIONS PROTOTYPES ********************************/
+/******************************************************************************/
+
+PUBLIC t_cm_error cm_HSEM_Init(const t_cm_system_address *pSystemAddr);
+PUBLIC t_cm_error cm_HSEM_EnableSemIrq(t_semaphore_id semId, t_nmf_core_id toCoreId);
+PUBLIC void cm_HSEM_Take(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_HSEM_Give(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_HSEM_GiveWithInterruptGeneration(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_HSEM_GenerateIrq(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC t_nmf_core_id cm_HSEM_GetCoreIdFromIrqSrc(void);
+
+PUBLIC t_cm_error cm_HSEM_PowerOn(t_nmf_core_id coreId);
+PUBLIC void cm_HSEM_PowerOff(t_nmf_core_id coreId);
+
+#endif /* __INC_HW_SEMA_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/src/hw_semaphores.c b/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/src/hw_semaphores.c
new file mode 100644
index 00000000000..932058cd4f2
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/src/hw_semaphores.c
@@ -0,0 +1,171 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/******************************************************************* Includes
+ ****************************************************************************/
+
+#include "../inc/hw_semaphores.h"
+#include <share/semaphores/inc/hwsem_hwp.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/power_mgt/inc/power.h>
+static t_hw_semaphore_regs *pHwSemRegs = (t_hw_semaphore_regs *)0;
+
+static t_uint32 semaphoreUseCounter = 0;
+static t_uint32 imsc[HSEM_MAX_INTR];
+PRIVATE void restoreMask(void);
+
+/****************************************************************************/
+/* NAME:   t_cm_error cm_HSEM_Init(const t_cm_system_address *pSystemAddr)     */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Initialize the HW Semaphores module                         */
+/*                                                                          */
+/* PARAMETERS:                                                              */
+/*       (in)   pSystemAddr: system base address of the HW semaphores IP    */
+/*                                                                          */
+/* RETURN:      CM_OK always                                                */
+/*                                                                          */
+/****************************************************************************/
+PUBLIC t_cm_error cm_HSEM_Init(const t_cm_system_address *pSystemAddr)
+{
+    t_uint8 i;
+
+    pHwSemRegs = (t_hw_semaphore_regs *)pSystemAddr->logical;
+
+    for (i=HSEM_FIRST_INTR; i < HSEM_MAX_INTR; i++)
+    {
+        imsc[i] = 0; // Mask all interrupt
+    }
+
+    return CM_OK;
+}
+
+static void cm_HSEM_ReInit(void)
+{
+    t_uint8 i;
+
+    pHwSemRegs->icrall = MASK_ALL16;
+
+    for (i=HSEM_FIRST_INTR; i < HSEM_MAX_INTR; i++)
+    {
+        pHwSemRegs->it[i].imsc = imsc[i];
+        pHwSemRegs->it[i].icr = MASK_ALL16;
+    }
+
+    for (i=0; i < NUM_HW_SEMAPHORES; i++)
+    {
+        pHwSemRegs->sem[i] = 0;
+    }
+}
+
+/****************************************************************************/
+/* NAME:   t_cm_error cm_HSEM_EnableSemIrq(                                */
+/*                                          t_semaphore_id semId,           */
+/*                                          t_nmf_core_id toCoreId           */
+/*                                         )                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Enable Irq for a given coreId (communication receiver)      */
+/*                                                                          */
+/* PARAMETERS:                                                              */
+/*       (in)   semId: identifier of the semaphore                          */
+/*       (in)   toCoreId: identifier of coreId destination of the coms      */
+/*                                                                          */
+/* RETURN:      CM_OK always                                                */
+/*                                                                          */
+/****************************************************************************/
+PUBLIC t_cm_error cm_HSEM_EnableSemIrq(t_semaphore_id semId, t_nmf_core_id toCoreId)
+{
+    static t_uint32 CoreIdToIntr[NB_CORE_IDS] = {0, 2, 3};
+    int i = CoreIdToIntr[toCoreId];
+
+    imsc[i] |= (1UL << semId);
+
+    // Allow cm_HSEM_EnableSemIrq to be called before real start in order to save power
+    if(semaphoreUseCounter > 0)
+    {
+        pHwSemRegs->it[i].imsc = imsc[i];
+    }
+
+    return CM_OK;
+}
+
+/****************************************************************************/
+/* NAME:  void cm_HSEM_GenerateIrq(t_semaphore_id semId)                    */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Generate an irq toward correct core according to semId      */
+/*                                                                          */
+/* PARAMETERS:                                                              */
+/*       (in)   semId: identifier of the semaphore to handle                */
+/*                                                                          */
+/* RETURN:      none                                                        */
+/*                                                                          */
+/****************************************************************************/
+PUBLIC void cm_HSEM_GenerateIrq(t_nmf_core_id coreId, t_semaphore_id semId)
+{
+   // TODO Move restore in OS BSP or in PRCMU in order to to it only when wake-up, for now do it always !!!!!!!!!!!!
+            restoreMask();
+
+    pHwSemRegs->sem[semId] = CORE_ID_2_HW_CORE_ID(ARM_CORE_ID);
+    pHwSemRegs->sem[semId] = (HSEM_INTRA_MASK|HSEM_INTRB_MASK|HSEM_INTRC_MASK|HSEM_INTRD_MASK);
+}
+
+/****************************************************************************/
+/* NAME:  t_nmf_core_id cm_HSEM_GetCoreIdFromIrqSrc(void)                   */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Check Masked Interrupt Status to know which semaphore(s)    */
+/*        have pending interrupt and return the identifier of the given dsp */
+/*                                                                          */
+/* PARAMETERS:  none                                                        */
+/*                                                                          */
+/* RETURN:      none                                                        */
+/*                                                                          */
+/****************************************************************************/
+PUBLIC t_nmf_core_id cm_HSEM_GetCoreIdFromIrqSrc(void)
+{
+    t_uword misValue = pHwSemRegs->it[ARM_CORE_ID].mis;
+    t_uint32 mask = 1 << FIRST_NEIGHBOR_SEMID(ARM_CORE_ID) /* == 0 here */;
+    t_nmf_core_id coreId = FIRST_MPC_ID;
+
+    while ((misValue & mask) == 0)
+    {
+        mask <<= 1;
+
+        coreId++;
+        if(coreId > LAST_MPC_ID)
+            return coreId;
+    }
+
+    /* Acknowledge Hsem interrupt */
+    pHwSemRegs->it[ARM_CORE_ID].icr = mask;
+
+    return coreId;
+}
+
+PUBLIC t_cm_error cm_HSEM_PowerOn(t_nmf_core_id coreId)
+{
+    if(semaphoreUseCounter++ == 0)
+    {
+        cm_PWR_EnableHSEM();
+
+        cm_HSEM_ReInit();  // HSEM is called one time only when the HSEM is switched ON
+    }
+
+    return CM_OK;
+}
+
+PUBLIC void cm_HSEM_PowerOff(t_nmf_core_id coreId)
+{
+    if(--semaphoreUseCounter == 0)
+    {
+        cm_PWR_DisableHSEM();
+    }
+}
+
+PRIVATE void restoreMask()
+{
+    t_uint8 i;
+    
+    for (i=HSEM_FIRST_INTR; i < HSEM_MAX_INTR; i++)
+        pHwSemRegs->it[i].imsc = imsc[i];
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/semaphores/inc/semaphores.h b/drivers/staging/nmf-cm/cm/engine/semaphores/inc/semaphores.h
new file mode 100644
index 00000000000..7636d8e7c9d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/semaphores/inc/semaphores.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_NMF_SEMAPHORE_H
+#define __INC_NMF_SEMAPHORE_H
+
+#include <cm/engine/api/control/configuration_engine.h>
+#include <share/semaphores/inc/semaphores.h>
+#include <cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h>
+
+PUBLIC t_cm_error cm_SEM_Init(const t_cm_system_address *pSystemAddr);
+PUBLIC t_cm_error cm_SEM_InitMpc(t_nmf_core_id coreId, t_nmf_semaphore_type_id semTypeId);
+PUBLIC t_semaphore_id cm_SEM_Alloc(t_nmf_core_id fromCoreId, t_nmf_core_id toCoreId);
+
+/* Semaphores management virtualized functions */
+extern void (*cm_SEM_GenerateIrq[NB_CORE_IDS])(t_nmf_core_id coreId, t_semaphore_id semId);
+extern t_cm_error (*cm_SEM_PowerOn[NB_CORE_IDS])(t_nmf_core_id coreId);
+extern void (*cm_SEM_PowerOff[NB_CORE_IDS])(t_nmf_core_id coreId);
+
+#endif /* __INC_NMF_SEMAPHORE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/semaphores/src/semaphores.c b/drivers/staging/nmf-cm/cm/engine/semaphores/src/semaphores.c
new file mode 100644
index 00000000000..daf95355a56
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/semaphores/src/semaphores.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h>
+#include <cm/engine/dsp/inc/semaphores_dsp.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <share/inc/nmf.h>
+
+void (*cm_SEM_GenerateIrq[NB_CORE_IDS])(t_nmf_core_id coreId, t_semaphore_id semId);
+t_cm_error (*cm_SEM_PowerOn[NB_CORE_IDS])(t_nmf_core_id coreId);
+void (*cm_SEM_PowerOff[NB_CORE_IDS])(t_nmf_core_id coreId);
+
+#define SEM_TYPE_ID_DEFAULT_VALUE ((t_nmf_semaphore_type_id)MASK_ALL32)
+static t_nmf_semaphore_type_id semaphoreTypePerCoreId[NB_CORE_IDS];
+
+static t_cm_error cm_LSEM_PowerOn(t_nmf_core_id coreId)
+{
+    return CM_OK;
+}
+
+static void cm_LSEM_PowerOff(t_nmf_core_id coreId)
+{
+}
+
+PUBLIC t_cm_error cm_SEM_Init(const t_cm_system_address *pSystemAddr)
+{
+    t_nmf_core_id coreId;
+
+    for (coreId = ARM_CORE_ID; coreId < NB_CORE_IDS; coreId++)
+    {
+        semaphoreTypePerCoreId[coreId] = SEM_TYPE_ID_DEFAULT_VALUE;
+
+        /* By default, we suppose that we use a full feature NMF ;) */
+        cm_SEM_GenerateIrq[coreId] = NULL;
+        cm_SEM_PowerOn[coreId] = NULL;
+        cm_SEM_PowerOff[coreId] = NULL;
+   }
+
+    cm_HSEM_Init(pSystemAddr);
+    /* if needed local semaphore init will be done coreId per coreId */
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_SEM_InitMpc(t_nmf_core_id coreId, t_nmf_semaphore_type_id semTypeId)
+{
+    if (semaphoreTypePerCoreId[coreId] != SEM_TYPE_ID_DEFAULT_VALUE)
+        return CM_MPC_ALREADY_INITIALIZED;
+
+    if(semTypeId == SYSTEM_SEMAPHORES)
+    {
+        cm_SEM_GenerateIrq[coreId] = cm_HSEM_GenerateIrq;
+        cm_SEM_PowerOn[coreId] = cm_HSEM_PowerOn;
+        cm_SEM_PowerOff[coreId] = cm_HSEM_PowerOff;
+    }
+    else if (semTypeId == LOCAL_SEMAPHORES)
+    {
+        cm_SEM_GenerateIrq[coreId] = cm_DSP_SEM_GenerateIrq;
+        cm_SEM_PowerOn[coreId] = cm_LSEM_PowerOn;
+        cm_SEM_PowerOff[coreId] = cm_LSEM_PowerOff;
+    }
+
+    semaphoreTypePerCoreId[coreId] =  semTypeId;
+
+    return CM_OK;
+}
+
+PUBLIC t_semaphore_id cm_SEM_Alloc(t_nmf_core_id fromCoreId, t_nmf_core_id toCoreId)
+{
+    t_semaphore_id semId;
+    t_nmf_core_id corex;
+
+    semId = FIRST_NEIGHBOR_SEMID(toCoreId);
+    for (corex = FIRST_CORE_ID; corex < fromCoreId; corex++)
+    {
+        if (corex == toCoreId)
+            continue;
+        semId++;
+    }
+
+    if (
+            (toCoreId == ARM_CORE_ID && semaphoreTypePerCoreId[fromCoreId] == SYSTEM_SEMAPHORES) ||
+            (semaphoreTypePerCoreId[toCoreId] == SYSTEM_SEMAPHORES)
+       )
+    {
+        cm_HSEM_EnableSemIrq(semId, toCoreId);
+    }
+
+    return semId;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/trace/inc/trace.h b/drivers/staging/nmf-cm/cm/engine/trace/inc/trace.h
new file mode 100644
index 00000000000..111eaf1324e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/trace/inc/trace.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Trace facilities management API
+ *
+ * \defgroup Trace Facilities
+ */
+#ifndef __INC_CM_TRACE_H
+#define __INC_CM_TRACE_H
+
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/configuration/inc/configuration_status.h>
+
+/*********************/
+/* Log related stuff */
+/*********************/
+#define ERROR(format, param1, param2, param3, param4, param5, param6) \
+do { \
+    if (cm_debug_level != -1)    \
+    OSAL_Log("Error: " format, (int)(param1), (int)(param2), (int)(param3), (int)(param4), (int)(param5), (int)(param6)); \
+    while(cm_error_break);\
+} while(0)
+
+#define WARNING(format, param1, param2, param3, param4, param5, param6) \
+do { \
+    if (cm_debug_level != -1)    \
+    OSAL_Log("Warning: " format, (int)(param1), (int)(param2), (int)(param3), (int)(param4), (int)(param5), (int)(param6)); \
+} while(0)
+
+#define LOG_INTERNAL(level, format, param1, param2, param3, param4, param5, param6) \
+do { \
+    if (level <= cm_debug_level)    \
+        OSAL_Log((const char *)format, (int)(param1), (int)(param2), (int)(param3), (int)(param4), (int)(param5), (int)(param6)); \
+} while(0)
+
+/*************************/
+/* Panic related stuff   */
+/*************************/
+#define CM_ASSERT(cond) \
+do { \
+	if(!(cond)) { OSAL_Log("CM_ASSERT at %s:%d\n", (int)__FILE__, (int)__LINE__, 0, 0, 0, 0); OSAL_Panic(); while(1); } \
+} while (0)
+
+#endif /* __INC_CM_TRACE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/trace/inc/xtitrace.h b/drivers/staging/nmf-cm/cm/engine/trace/inc/xtitrace.h
new file mode 100644
index 00000000000..6ab960b69b2
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/trace/inc/xtitrace.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#ifndef __INC_CM_XTITRACE_H
+#define __INC_CM_XTITRACE_H
+
+#include <cm/engine/component/inc/instance.h>
+
+#include <inc/nmf-tracedescription.h>
+
+extern t_bool cm_trace_enabled;
+
+/*************************/
+/* Trace   related stuff */
+/*************************/
+void cm_TRC_Dump(void);
+
+void cm_TRC_traceReset(void);
+
+void cm_TRC_traceLoadMap(
+        t_nmfTraceComponentCommandDescription cmd,
+        const t_component_instance* component);
+
+#define ARM_TRACE_COMPONENT ((const t_component_instance*)0xFFFFFFFF)
+
+void cm_TRC_traceBinding(
+        t_nmfTraceBindCommandDescription command,
+        const t_component_instance* clientComponent, const t_component_instance* serverComponent,
+        const char *requiredItfName, const char *providedItfName);
+
+void cm_TRC_traceCommunication(
+        t_nmfTraceCommunicationCommandDescription command,
+        t_nmf_core_id coreId,
+        t_nmf_core_id remoteCoreId);
+
+void cm_TRC_traceMemAlloc(t_nmfTraceAllocatorCommandDescription command, t_uint8 allocId, t_uint32 memorySize, const char *allocname);
+
+void cm_TRC_traceMem(t_nmfTraceAllocCommandDescription command, t_uint8 allocId, t_uint32 startAddress, t_uint32 memorySize);
+
+#endif /* __INC_CM_TRACE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/trace/src/panic.c b/drivers/staging/nmf-cm/cm/engine/trace/src/panic.c
new file mode 100644
index 00000000000..832b9f28756
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/trace/src/panic.c
@@ -0,0 +1,222 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/component/inc/bind.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/api/control/irq_engine.h>
+
+#include <cm/engine/utils/inc/convert.h>
+#include <share/communication/inc/nmf_service.h>
+
+
+/*
+ * Panic
+ */
+const struct {
+	char* name;
+    unsigned int info1:1;
+    unsigned int PC:1;
+	unsigned int SP:1;
+	unsigned int interface:1;
+} reason_descrs[] = {
+		{"NONE_PANIC",            0,  0,  0,  0},
+		{"INTERNAL_PANIC",        1,  0,  0,  0},
+		{"Reserved Panic",        0,  0,  0,  0},
+		{"USER_STACK_OVERFLOW",   0,  1,  1,  0},
+		{"SYSTEM_STACK_OVERFLOW", 0,  1,  1,  0},
+		{"UNALIGNED_LONG_ACCESS", 0,  1,  0,  0},
+		{"EVENT_FIFO_OVERFLOW",   0,  0,  0,  1},
+		{"PARAM_FIFO_OVERFLOW",   0,  0,  0,  1},
+		{"INTERFACE_NOT_BINDED",  0,  0,  0,  0},
+		{"USER_PANIC",            1,  0,  0,  0}
+};
+
+static t_component_instance* getCorrespondingInstance(
+        t_panic_reason panicReason,
+        t_uint32 panicThis,
+        t_dup_char *itfName,
+	t_cm_instance_handle *instHandle) {
+    t_component_instance *instance;
+    t_uint32 k;
+
+    for (k=0; k<ComponentTable.idxMax; k++) {
+        if ((instance = componentEntry(k)) == NULL)
+            continue;
+        if(panicReason == PARAM_FIFO_OVERFLOW ||
+                panicReason == EVENT_FIFO_OVERFLOW) {
+            // Panic has been generated by binding component, search the client who has call it
+            // and return the client handle (not the BC one).
+            int i;
+
+            if(instance->thisAddress == panicThis && panicThis == 0) {
+                *itfName = "Internal NMF service";
+                *instHandle = ENTRY2HANDLE(instance, k);
+                return instance;
+            }
+
+            for(i = 0; i < instance->template->requireNumber; i++) {
+                int nb = instance->template->requires[i].collectionSize, j;
+                for(j = 0; j < nb; j++) {
+                    if(instance->interfaceReferences[i][j].instance != NULL &&
+                            instance->interfaceReferences[i][j].instance != (t_component_instance *)NMF_HOST_COMPONENT &&
+                            instance->interfaceReferences[i][j].instance != (t_component_instance *)NMF_VOID_COMPONENT &&
+                            instance->interfaceReferences[i][j].instance->thisAddress == panicThis)
+                    {
+                        *itfName = instance->template->requires[i].name;
+                        *instHandle = ENTRY2HANDLE(instance, k);
+                        return instance;
+                    }
+                }
+            }
+        } else {
+            // The component which has generated the panic is the good one.
+
+            if(instance->thisAddress == panicThis) {
+		*itfName = "?";
+                *instHandle = ENTRY2HANDLE(instance, k);
+                return instance;
+            }
+        }
+    }
+
+    *itfName = "?";
+    *instHandle = 0;
+    return 0;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ReadMPCString(
+        t_nmf_core_id               coreId,
+        t_uint32                    dspAddress,
+        char *                      buffer,
+        t_uint32                    bufferSize) {
+
+    while(--bufferSize > 0)
+    {
+        char ch = cm_DSP_ReadXRamWord(coreId, dspAddress++);
+        if(ch == 0)
+            break;
+
+        *buffer++ = ch;
+    };
+
+    *buffer = 0;
+
+    // Reset panicReason
+    cm_writeAttribute(cm_EEM_getExecutiveEngine(coreId)->instance,
+            "rtos/commonpart/serviceReason", MPC_SERVICE_NONE);
+
+    return CM_OK;
+}
+
+/****************/
+/* Generic part */
+/****************/
+PUBLIC EXPORT_SHARED t_cm_error CM_getServiceDescription(
+        t_nmf_core_id               coreId,
+        t_cm_service_type           *srcType,
+        t_cm_service_description    *srcDescr)
+{
+    t_uint32 serviceReason;
+    t_component_instance *ee;
+
+    // Acknowledge interrupt (do it before resetting panicReason)
+    cm_DSP_AcknowledgeDspIrq(coreId, DSP2ARM_IRQ_1);
+
+    ee = cm_EEM_getExecutiveEngine(coreId)->instance;
+
+    // Read panicReason
+    serviceReason = cm_readAttributeNoError(ee, "rtos/commonpart/serviceReason");
+    if(serviceReason == MPC_SERVICE_PRINT)
+    {
+        *srcType = CM_MPC_SERVICE_PRINT;
+
+        srcDescr->u.print.dspAddress = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo0");
+        srcDescr->u.print.value1 = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo1");
+        srcDescr->u.print.value2 = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo2");
+    }
+    else if(serviceReason != MPC_SERVICE_NONE)
+    {
+        t_uint32 panicThis;
+        t_dup_char itfName;
+        t_component_instance *instance;
+
+        *srcType = CM_MPC_SERVICE_PANIC;
+        srcDescr->u.panic.panicReason = (t_panic_reason)serviceReason;
+        srcDescr->u.panic.panicSource = MPC_EE;
+        srcDescr->u.panic.info.mpc.coreid = coreId;
+
+        // Read panicThis
+        panicThis = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo0");
+
+        instance = getCorrespondingInstance(srcDescr->u.panic.panicReason, panicThis, &itfName, &srcDescr->u.panic.info.mpc.faultingComponent);
+
+        LOG_INTERNAL(0, "Error: Panic(%s, %s), This=%x", cm_getDspName(coreId),
+                reason_descrs[srcDescr->u.panic.panicReason].name, (void*)panicThis, 0, 0, 0);
+
+        if(reason_descrs[srcDescr->u.panic.panicReason].interface != 0)
+        {
+            LOG_INTERNAL(0, ", interface=%s", itfName, 0, 0, 0, 0, 0);
+        }
+
+        if(reason_descrs[srcDescr->u.panic.panicReason].info1 != 0)
+        {
+            // Info 1
+            srcDescr->u.panic.info.mpc.panicInfo1 = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo1");
+
+            LOG_INTERNAL(0, ", Info=%x", srcDescr->u.panic.info.mpc.panicInfo1, 0, 0, 0, 0, 0);
+        }
+
+        if(reason_descrs[srcDescr->u.panic.panicReason].PC != 0)
+        {
+            t_uint32 DspAddress = 0xFFFFFFFF;
+            t_uint32 DspSize = 0x0;
+
+            // PC need to be read in rtos/commonpart/serviceInfo1
+            srcDescr->u.panic.info.mpc.panicInfo1 = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo1");
+
+            if(instance != 0)
+            {
+                cm_DSP_GetDspAddress(instance->memories[instance->template->codeMemory->id], &DspAddress);
+                cm_DSP_GetDspMemoryHandleSize(instance->memories[instance->template->codeMemory->id], &DspSize);
+            }
+
+            if(DspAddress <= srcDescr->u.panic.info.mpc.panicInfo1 &&
+                    srcDescr->u.panic.info.mpc.panicInfo1 < (DspAddress + DspSize))
+                LOG_INTERNAL(0, ", PC:off=%x <abs=%x>",
+                        srcDescr->u.panic.info.mpc.panicInfo1 - DspAddress,
+                        srcDescr->u.panic.info.mpc.panicInfo1, 0, 0, 0, 0);
+            else
+                LOG_INTERNAL(0, ", PC:<abs=%x>", srcDescr->u.panic.info.mpc.panicInfo1, 0, 0, 0, 0, 0);
+        }
+
+        if(reason_descrs[srcDescr->u.panic.panicReason].SP != 0)
+        {
+            srcDescr->u.panic.info.mpc.panicInfo2 = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo2");
+
+            LOG_INTERNAL(0, ", SP=%x", srcDescr->u.panic.info.mpc.panicInfo2, 0, 0, 0, 0, 0);
+        }
+
+        LOG_INTERNAL(0, "\n", 0, 0, 0, 0, 0, 0);
+
+        if(instance != 0)
+        {
+            LOG_INTERNAL(0, "Error:  Component=%s<%s>\n",
+                    instance->pathname, instance->template->name, 0, 0, 0, 0);
+        }
+
+        // We don't set rtos/commonpart/serviceReason = MPC_SERVICE_NONE, since we don't want the
+        // MMDSP to continue execution, and we put in in Panic state
+        cm_DSP_SetStatePanic(coreId);
+    }
+    else
+    {
+        *srcType = CM_MPC_SERVICE_NONE;
+    }
+
+    return CM_OK;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/trace/src/trace.c b/drivers/staging/nmf-cm/cm/engine/trace/src/trace.c
new file mode 100644
index 00000000000..ac948c767b4
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/trace/src/trace.c
@@ -0,0 +1,220 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include "../inc/trace.h"
+#include "../inc/xtitrace.h"
+#include <inc/nmf-tracedescription.h>
+#include <inc/nmf-limits.h>
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+t_bool cm_trace_enabled = FALSE;
+
+/*
+ * STM message dump
+ */
+#define HEADER(t, s) ((t) | (s << 16))
+
+static void writeN(struct t_nmfTraceChannelHeader* header)
+{
+    t_uint64* data = (t_uint64*)header;
+    t_uint64 *end = (t_uint64*)(((unsigned int)data) + header->traceSize - sizeof(t_uint64));
+
+    while(data < end)
+    {
+        OSAL_Write64(CM_CHANNEL, 0,  *data++);
+    }
+
+    OSAL_Write64(CM_CHANNEL, 1, *data);
+}
+
+void cm_TRC_Dump(void)
+{
+    t_uint32 i;
+
+    cm_TRC_traceReset();
+
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	if (componentEntry(i) != NULL)
+	    cm_TRC_traceLoadMap(TRACE_COMPONENT_COMMAND_ADD, componentEntry(i));
+    }
+}
+
+void cm_TRC_traceReset(void)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceReset   trace;
+
+        trace.header.v = HEADER(TRACE_TYPE_RESET, sizeof(trace));
+
+        trace.minorVersion = TRACE_MINOR_VERSION;
+        trace.majorVersion = TRACE_MAJOR_VERSION;
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+    }
+}
+
+void cm_TRC_traceLoadMap(
+        t_nmfTraceComponentCommandDescription command,
+        const t_component_instance* component)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceComponent   trace;
+
+        /*
+         * Generate instantiate trace
+         */
+        trace.header.v = HEADER(TRACE_TYPE_COMPONENT, sizeof(trace));
+
+        trace.command = (t_uint16)command;
+        trace.domainId = (t_uint16)component->template->dspId + 1;
+        trace.componentContext = (t_uint32)component->thisAddress;
+        trace.componentUserContext = (t_uint32)component;
+        cm_StringCopy((char*)trace.componentLocalName, component->pathname, MAX_COMPONENT_NAME_LENGTH);
+        cm_StringCopy((char*)trace.componentTemplateName, component->template->name, MAX_TEMPLATE_NAME_LENGTH);
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+
+        if(command == TRACE_COMPONENT_COMMAND_ADD)
+        {
+            struct t_nmfTraceMethod tracemethod;
+            int i, j, k;
+
+            /*
+             * Generate method trace
+             */
+            tracemethod.header.v = HEADER(TRACE_TYPE_METHOD, sizeof(tracemethod));
+
+            tracemethod.domainId = (t_uint16)component->template->dspId + 1;
+            tracemethod.componentContext = (t_uint32)component->thisAddress;
+
+            for(i = 0; i < component->template->provideNumber; i++)
+            {
+                t_interface_provide* provide = &component->template->provides[i];
+
+                for(j = 0; j < provide->collectionSize; j++)
+                {
+                    for(k = 0; k < provide->interface->methodNumber; k++)
+                    {
+                        tracemethod.methodId = provide->indexes[j][k].methodAddresses;
+
+                        cm_StringCopy((char*)tracemethod.methodName, provide->interface->methodNames[k], MAX_INTERFACE_METHOD_NAME_LENGTH);
+
+                        writeN((struct t_nmfTraceChannelHeader*)&tracemethod);
+                    }
+                }
+            }
+        }
+    }
+}
+
+void cm_TRC_traceBinding(
+        t_nmfTraceBindCommandDescription command,
+        const t_component_instance* clientComponent, const t_component_instance* serverComponent,
+        const char *requiredItfName, const char *providedItfName)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceBind trace;
+
+        trace.header.v = HEADER(TRACE_TYPE_BIND, sizeof(trace));
+
+        trace.command = (t_uint16)command;
+
+        if(clientComponent == ARM_TRACE_COMPONENT) // ARM
+        {
+            trace.clientDomainId = 0x1;
+            trace.clientComponentContext = 0x0;
+        }
+        else
+        {
+            trace.clientDomainId = (t_uint16)clientComponent->template->dspId + 1;
+            trace.clientComponentContext = (t_uint32)clientComponent->thisAddress;
+        }
+        if(requiredItfName != NULL)
+            cm_StringCopy((char*)trace.requiredItfName, requiredItfName, MAX_INTERFACE_NAME_LENGTH);
+        else
+            trace.requiredItfName[0] = 0;
+
+        if(serverComponent == NULL)
+        { // Unbind or VOID
+            trace.serverDomainId = 0;
+            trace.serverComponentContext = 0x0;
+        }
+        else if(serverComponent == ARM_TRACE_COMPONENT)
+        { // ARM
+            trace.serverDomainId = 0x1;
+            trace.serverComponentContext = 0x0;
+        }
+        else
+        {
+            trace.serverDomainId = (t_uint16)serverComponent->template->dspId + 1;
+            trace.serverComponentContext = (t_uint32)serverComponent->thisAddress;
+        }
+        if(providedItfName != NULL)
+            cm_StringCopy((char*)trace.providedItfName, providedItfName, MAX_INTERFACE_NAME_LENGTH);
+        else
+            trace.providedItfName[0] = 0;
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+    }
+}
+
+void cm_TRC_traceCommunication(
+        t_nmfTraceCommunicationCommandDescription command,
+        t_nmf_core_id coreId,
+        t_nmf_core_id remoteCoreId)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceCommunication trace;
+
+        trace.header.v = HEADER(TRACE_TYPE_COMMUNICATION, sizeof(trace));
+
+        trace.command = (t_uint16)command;
+        trace.domainId = (t_uint16)coreId + 1;
+        trace.remoteDomainId = (t_uint16)remoteCoreId + 1;
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+    }
+}
+
+void cm_TRC_traceMemAlloc(t_nmfTraceAllocatorCommandDescription command, t_uint8 allocId, t_uint32 memorySize, const char *allocname)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceAllocator trace;
+
+        trace.header.v = HEADER(TRACE_TYPE_ALLOCATOR, sizeof(trace));
+
+        trace.command = (t_uint16)command;
+        trace.allocId = (t_uint16)allocId;
+        trace.size = memorySize;
+        cm_StringCopy((char*)trace.name, allocname, sizeof(trace.name));
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+    }
+}
+
+void cm_TRC_traceMem(t_nmfTraceAllocCommandDescription command, t_uint8 allocId, t_uint32 startAddress, t_uint32 memorySize)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceAlloc trace;
+
+        trace.header.v = HEADER(TRACE_TYPE_ALLOC, sizeof(trace));
+
+        trace.command = (t_uint16)command;
+        trace.allocId = (t_uint16)allocId;
+        trace.offset = startAddress;
+        trace.size = memorySize;
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+    }
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/inc/convert.h b/drivers/staging/nmf-cm/cm/engine/utils/inc/convert.h
new file mode 100644
index 00000000000..d6912e58687
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/inc/convert.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Conversion utility methods.
+ */
+#ifndef H_CM_CONVERTS_MEM
+#define H_CM_CONVERTS_MEM
+
+#include <share/inc/nmf.h>
+
+/*
+ * Utils convert methods
+ */
+const char* cm_getDspName(t_nmf_core_id dsp);
+
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/inc/mem.h b/drivers/staging/nmf-cm/cm/engine/utils/inc/mem.h
new file mode 100644
index 00000000000..c950a94023d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/inc/mem.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Memory manipulation.
+ */
+#ifndef H_CM_UTILS_MEM
+#define H_CM_UTILS_MEM
+
+/*
+ * Utils libc methods
+ */
+void cm_MemCopy(void* dest, const void *src, int count);
+void cm_MemSet(void *str, int c, int count);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/inc/string.h b/drivers/staging/nmf-cm/cm/engine/utils/inc/string.h
new file mode 100644
index 00000000000..d2b7c0b0823
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/inc/string.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief String manipulation.
+ */
+#ifndef H_CM_UTILS_STRING
+#define H_CM_UTILS_STRING
+
+#include <cm/engine/memory/inc/memory.h>
+
+#define MAX_INTERNAL_STRING_LENGTH 2048
+
+typedef const char *t_dup_char;
+
+t_dup_char cm_StringGet(const char* str);
+t_dup_char cm_StringReference(t_dup_char str);
+t_dup_char cm_StringDuplicate(const char* orig);
+void cm_StringRelease(t_dup_char orig);
+
+/*
+ * Utils libc methods
+ */
+void        cm_StringCopy(char* dest, const char* src, int count);
+int         cm_StringCompare(const char* str1, const char* str2, int count);
+int         cm_StringLength(const char * str, int count);
+void        cm_StringConcatenate(char* dest, const char* src, int count);
+char*       cm_StringSearch(const char* str, int c);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/inc/swap.h b/drivers/staging/nmf-cm/cm/engine/utils/inc/swap.h
new file mode 100644
index 00000000000..e4f5acb3010
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/inc/swap.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Swap integer manipulation.
+ */
+#ifndef H_CM_UTILS_SWAP
+#define H_CM_UTILS_SWAP
+
+#include <cm/inc/cm_type.h>
+
+/*
+ * Swap methods
+ */
+t_uint16 swap16(t_uint16 x);
+t_uint32 swap32(t_uint32 x);
+t_uint64 swap64(t_uint64 x);
+t_uint32 noswap32(t_uint32 x);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/inc/table.h b/drivers/staging/nmf-cm/cm/engine/utils/inc/table.h
new file mode 100644
index 00000000000..9d9828a81f6
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/inc/table.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Dynamic table manipulation.
+ */
+#ifndef H_CM_UTILS_TABLE
+#define H_CM_UTILS_TABLE
+
+#include <cm/inc/cm_type.h>
+
+/*
+  This implement a (generic) dynamic table (the size is dynamic)
+  to register some pointers of a given kind of elements
+  
+  It also allows to compute/convert each kernel pointer registered in the
+  table to a user handler, that can be checked.
+
+  The "user" handler is composed by the index in this table
+  (the low INDEX_SHIFT bits) and the low bits of the "local" pointer
+  shifted by INDEX_SHIFT are stored in the high bits:
+
+  handle bits: 31 ................................ 12 11 ...... 0
+              | lower bits of of the local pointer   |   index   |
+
+  This allows a straight translation from a user handle to a local pointer
+  + a strong check to validate the value of a user handle.
+  The reverse translation from pointer to a user handle is
+  slower as it requires an explicit search in the list.
+ */
+
+
+/* INDEX_SHIFT determines the index size and thus the max index */
+#define INDEX_SHIFT    12
+#define INDEX_MAX      (1UL << INDEX_SHIFT)
+#define INDEX_MASK     (INDEX_MAX-1)
+#define ENTRY2HANDLE(pointer, index) (((unsigned int)pointer << INDEX_SHIFT) | index)
+#define TABLE_DEF_SIZE 0x1000
+
+typedef struct {
+	t_uint32 idxNb;  /**< number of entries used */
+	t_uint32 idxCur; /**< current index: point to next supposed
+			    free entry: used to look for the next
+			    free entry */
+	t_uint32 idxMax; /**< index max currently allowed */
+	void **entries;   /**< table itself */
+} t_nmf_table;
+
+t_cm_error cm_initTable(t_nmf_table* table);
+void cm_destroyTable(t_nmf_table* table);
+t_uint32 cm_addEntry(t_nmf_table *table, void *entry);
+void cm_delEntry(t_nmf_table *table, t_uint32 idx);
+void *cm_lookupEntry(const t_nmf_table *table, const t_uint32 hdl);
+t_uint32 cm_lookupHandle(const t_nmf_table *table, const void *entry);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/src/convert.c b/drivers/staging/nmf-cm/cm/engine/utils/src/convert.c
new file mode 100644
index 00000000000..ad6e097bfe6
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/src/convert.c
@@ -0,0 +1,20 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/utils/inc/convert.h>
+
+const char* dspNames[NB_CORE_IDS] = {
+    "ARM",
+    "SVA",
+    "SIA"
+};
+
+
+const char* cm_getDspName(t_nmf_core_id dsp) {
+	return dspNames[dsp];
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/src/mem.c b/drivers/staging/nmf-cm/cm/engine/utils/src/mem.c
new file mode 100644
index 00000000000..130a044bbf8
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/src/mem.c
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/utils/inc/mem.h>
+
+
+/*
+ * Methods
+ */
+void cm_MemCopy(void* dest, const void *src, int count) {
+    char *tmp = (char *) dest, *s = (char *) src;
+
+    while (count--)
+        *tmp++ = *s++;
+}
+
+void cm_MemSet(void *str, int c, int count) {
+  char *tmp = (char *)str;
+
+  while (count--)
+    *tmp++ = c;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/src/string.c b/drivers/staging/nmf-cm/cm/engine/utils/src/string.c
new file mode 100644
index 00000000000..0ed6fb3a214
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/src/string.c
@@ -0,0 +1,229 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ * Shared string manipulation.
+ * TODO This is a list today, must be a hash later !!!!!
+ */
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/trace/inc/trace.h>
+
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+#define NHASH 257       //Use a prime number!
+#define MULT 17
+
+/*
+ * Data
+ */
+struct t_linkedstring
+{
+    struct t_linkedstring	*next;
+    int						referencer;
+    char					string[1];
+};
+
+static struct t_linkedstring *list[NHASH];
+
+#define myoffsetof(st, m) \
+     ((int) ( (char *)&((st *)(0))->m - (char *)0 ))
+
+unsigned int hash(const char *str)
+{
+    unsigned int h = 0;
+    for(; *str; str++)
+        h = MULT * h + *str;
+    return h % NHASH;
+}
+/*
+ * Methods
+ */
+PRIVATE struct t_linkedstring *lookupString(
+        const char* str,
+        struct t_linkedstring   *first)
+{
+    while(first != 0)
+    {
+        if(cm_StringCompare(str, first->string, MAX_INTERNAL_STRING_LENGTH) == 0)
+            break;
+        first = first->next;
+    }
+
+    return first;
+}
+
+t_dup_char cm_StringGet(const char* str)
+{
+    struct t_linkedstring *entry;
+
+    entry = lookupString(str, list[hash(str)]);
+    CM_ASSERT(entry != 0);
+
+    return (t_dup_char)entry->string;
+}
+
+t_dup_char cm_StringReference(t_dup_char str)
+{
+    struct t_linkedstring* entry = (struct t_linkedstring*)((t_uint32)str - myoffsetof(struct t_linkedstring, string));
+
+    // One more referencer
+    entry->referencer++;
+
+    return (t_dup_char)entry->string;
+}
+
+t_dup_char cm_StringDuplicate(const char* str)
+{
+    struct t_linkedstring *entry;
+    unsigned int h;
+
+    h = hash(str);
+    entry = lookupString(str, list[h]);
+    if(entry != 0)
+    {
+        // One more referencer
+        entry->referencer++;
+    }
+    else
+    {
+        // Allocate new entry
+        entry = (struct t_linkedstring *)OSAL_Alloc(sizeof(struct t_linkedstring)-1 + cm_StringLength(str, MAX_INTERNAL_STRING_LENGTH)+1);
+        if(entry == NULL)
+            return NULL;
+
+        entry->referencer = 1;
+        cm_StringCopy(entry->string, str, MAX_INTERNAL_STRING_LENGTH);
+
+        // Link it in list
+        entry->next = list[h];
+        list[h] = entry;
+    }
+
+    return (t_dup_char)entry->string;
+}
+
+void cm_StringRelease(t_dup_char str)
+{
+    if(str != NULL)
+    {
+        struct t_linkedstring* entry = (struct t_linkedstring*)((t_uint32)str - myoffsetof(struct t_linkedstring, string));
+
+        // One less referencer
+        entry->referencer--;
+
+        if(entry->referencer == 0)
+        {
+            int h = hash(entry->string);
+
+            if(list[h] == entry) // This first first one
+            {
+                list[h] = entry->next;
+            }
+            else
+            {
+                struct t_linkedstring *tmp = list[h];
+
+                // Here we assume that entry is in the list
+                while(/*tmp != NULL && */tmp->next != entry)
+                    tmp = tmp->next;
+
+                tmp->next = entry->next;
+            }
+            OSAL_Free(entry);
+        }
+    }
+}
+
+#if 0
+void checkString()
+{
+    struct t_linkedstring *tmp = list;
+
+    while(tmp != 0)
+    {
+        printf("  stay %s %d\n", tmp->string, tmp->referencer);
+        tmp = tmp->next;
+    }
+}
+#endif
+
+/*
+ * LibC method
+ */
+void cm_StringCopy(char* dest, const char *src, int count)
+{
+    while (count-- && (*dest++ = *src++) != '\0')
+        /* nothing */
+        ;
+}
+#define DETECTNULL(X) (((X) - 0x01010101) & ~(X) & 0x80808080)
+
+int cm_StringCompare(const char* str1, const char* str2, int count)
+{
+    /* If s1 and s2 are word-aligned, compare them a word at a time. */
+    if ((((int)str1 & 3) | ((int)str2 & 3)) == 0)
+    {
+        unsigned int *a1 = (unsigned int*)str1;
+        unsigned int *a2 = (unsigned int*)str2;
+
+        while (count >= sizeof (unsigned int) && *a1 == *a2)
+        {
+            count -= sizeof (unsigned int);
+
+            /* If we've run out of bytes or hit a null, return zero since we already know *a1 == *a2.  */
+            if (count == 0 || DETECTNULL (*a1))
+                return 0;
+
+            a1++;
+            a2++;
+        }
+
+        /* A difference was detected in last few bytes of s1, so search bytewise */
+        str1 = (char*)a1;
+        str2 = (char*)a2;
+    }
+
+    while (count-- > 0 && *str1 == *str2)
+    {
+        /* If we've run out of bytes or hit a null, return zero
+       since we already know *s1 == *s2.  */
+        if (count == 0 || *str1 == '\0')
+            return 0;
+        str1++;
+        str2++;
+    }
+
+    return (*(unsigned char *) str1) - (*(unsigned char *) str2);
+}
+
+int cm_StringLength(const char * str, int count)
+{
+    const char *sc;
+
+    for (sc = str; count-- && *sc != '\0'; ++sc)
+        /* nothing */
+        ;
+    return sc - str;
+}
+
+void cm_StringConcatenate(char* dest, const char* src, int count)
+{
+    while ((*dest) != '\0')
+    {
+        dest++;
+        count--;
+    }
+    cm_StringCopy(dest, src, count);
+}
+
+char* cm_StringSearch(const char* str, int c)
+{
+    for(; *str != (char) c; ++str)
+        if (*str == '\0')
+            return 0;
+    return (char *) str;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/src/swap.c b/drivers/staging/nmf-cm/cm/engine/utils/src/swap.c
new file mode 100644
index 00000000000..e3e2d536144
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/src/swap.c
@@ -0,0 +1,156 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/utils/inc/swap.h>
+
+
+/*
+ * Methods
+ */
+t_uint16 swap16(t_uint16 x)
+{
+    return ((x >> 8) |
+            ((x << 8) & 0xff00U));
+}
+
+#ifdef LINUX
+
+#if defined(__STN_8815) /* __STN_8815 -> ARMv5*/
+t_uint32 swap32(t_uint32 x)
+{
+    asm volatile (
+            "EOR      r1, r0, r0, ROR #16           \n\t"
+            "BIC      r1, r1, #0xFF0000             \n\t"
+            "MOV      r0, r0, ROR #8                \n\t"
+            "EOR      r0, r0, r1, LSR #8"
+            :  :  : "r3" );
+
+    return x;
+}
+
+t_uint64 swap64(t_uint64 x)
+{
+    asm volatile (
+            "MOV      r2, r1                        \n\t"
+            "                                       \n\t"
+            "EOR      r3, r0, r0, ROR #16           \n\t"
+            "BIC      r3, r3, #0xFF0000             \n\t"
+            "MOV      r0, r0, ROR #8                \n\t"
+            "EOR      r1, r0, r3, LSR #8            \n\t"
+            "                                       \n\t"
+            "EOR      r3, r2, r2, ROR #16           \n\t"
+            "BIC      r3, r3, #0xFF0000             \n\t"
+            "MOV      r2, r2, ROR #8                \n\t"
+            "EOR      r0, r2, r3, LSR #8"
+            :  :  : "r3", "r2" );
+
+    return x;
+}
+#else /* -> ARMv6 or later */
+
+t_uint32 swap32(t_uint32 x)
+{
+    asm volatile (
+            "REV      %0, %0"
+            :  "+r"(x) : );
+
+    return x;
+}
+
+t_uint64 swap64(t_uint64 x)
+{
+    asm volatile (
+            "REV      r2, %Q0                \n\t"
+            "REV      %Q0, %R0                \n\t"
+            "MOV      %R0, r2"
+            :  "+&r" (x) : : "r2" );
+
+    return x;
+}
+
+#endif
+
+#else /* Symbian, Think -> We assume ARMCC */
+
+#if defined(__thumb__)
+
+t_uint32 swap32(t_uint32 x)
+{
+    return ((x >> 24) |
+            ((x >> 8) & 0xff00U) |
+            ((x << 8) & 0xff0000U) |
+            ((x << 24) & 0xff000000U));
+}
+
+t_uint64 swap64(t_uint64 x)
+{
+    return ((x >> 56) |
+            ((x >> 40) & 0xff00UL) |
+            ((x >> 24) & 0xff0000UL) |
+            ((x >> 8) & 0xff000000UL) |
+            ((x << 8) & 0xff00000000ULL) |
+            ((x << 24) & 0xff0000000000ULL) |
+            ((x << 40) & 0xff000000000000ULL) |
+            ((x << 56)));
+}
+
+#elif (__TARGET_ARCH_ARM < 6)
+
+__asm t_uint32 swap32(t_uint32 x)
+{
+    EOR      r1, r0, r0, ROR #16
+    BIC      r1, r1, #0xFF0000
+    MOV      r0, r0, ROR #8
+    EOR      r0, r0, r1, LSR #8
+
+    BX       lr
+}
+
+__asm t_uint64 swap64(t_uint64 x)
+{
+    MOV      r2, r1
+
+    EOR      r3, r0, r0, ROR #16    // Swap low (r0) and store it in high (r1)
+    BIC      r3, r3, #0xFF0000
+    MOV      r0, r0, ROR #8
+    EOR      r1, r0, r3, LSR #8
+
+    EOR      r3, r2, r2, ROR #16    // Swap high (r2 = ex r1) and store it in low (r0)
+    BIC      r3, r3, #0xFF0000
+    MOV      r2, r2, ROR #8
+    EOR      r0, r2, r3, LSR #8
+
+    BX       lr
+}
+
+#else /* -> ARMv6 or later */
+
+__asm t_uint32 swap32(t_uint32 x)
+{
+    REV      r0, r0
+
+    BX       lr
+}
+
+__asm t_uint64 swap64(t_uint64 x)
+{
+    REV      r2, r0
+    REV      r0, r1
+    MOV      r1, r2
+
+    BX       lr
+}
+
+#endif
+
+#endif
+
+t_uint32 noswap32(t_uint32 x) {
+    return x;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/src/table.c b/drivers/staging/nmf-cm/cm/engine/utils/src/table.c
new file mode 100644
index 00000000000..708396a01b2
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/src/table.c
@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/mem.h>
+#include <cm/engine/utils/inc/table.h>
+
+/*
+ * Methods
+ */
+t_cm_error cm_initTable(t_nmf_table* table)
+{
+	table->idxMax = TABLE_DEF_SIZE / sizeof(table->entries);
+
+	table->entries = OSAL_Alloc_Zero(table->idxMax*sizeof(table->entries));
+
+	if (table->entries == NULL) {
+		table->idxMax = 0;
+		return CM_NO_MORE_MEMORY;
+	}
+
+	return CM_OK;
+}
+
+void cm_destroyTable(t_nmf_table* table)
+{
+	if (table->idxNb) {
+		ERROR("Attempt to free non-empty table !!!\n", 0, 0, 0, 0, 0, 0);
+		return;
+	}
+	OSAL_Free(table->entries);
+	table->idxMax = 0;
+}
+
+static t_cm_error cm_increaseTable(t_nmf_table* table)
+{
+	t_uint32 new_max;
+	void *mem;
+
+	if (table->idxMax == INDEX_MASK) {
+		ERROR("CM_NO_MORE_MEMORY: Maximum table entries reached\n", 0, 0, 0, 0, 0, 0);
+		return CM_NO_MORE_MEMORY;
+	}
+
+	new_max = table->idxMax
+		+ TABLE_DEF_SIZE / sizeof(table->entries);
+
+	if (new_max > INDEX_MAX)
+		new_max = INDEX_MAX;
+
+	mem = OSAL_Alloc(new_max * sizeof(table->entries));
+
+	if (mem == NULL) {
+		ERROR("CM_NO_MORE_MEMORY: Unable to allocate memory for a table\n", 0, 0, 0, 0, 0, 0);
+		return CM_NO_MORE_MEMORY;
+	}
+
+	cm_MemCopy(mem, table->entries,
+		   table->idxMax*sizeof(table->entries));
+	cm_MemSet((void *)((t_uint32) mem + table->idxMax*sizeof(*table->entries)), 0,
+		  (new_max-table->idxMax) * sizeof(*table->entries));
+
+	OSAL_Free(table->entries);
+	table->entries = mem;
+	table->idxMax = new_max;
+
+	return CM_OK;
+}
+
+/** cm_addEntry - Add an local pointer to an element to the list
+ *
+ *  1. Increase the size of the list if it's full
+ *  2. Search an empty entry
+ *  3. Add the element to the list
+ *  4. Compute and return the "user handle"
+ */
+t_uint32 cm_addEntry(t_nmf_table *table, void *entry)
+{
+	unsigned int i;
+	t_uint32 hdl = 0;
+
+	if (table->idxNb == table->idxMax)
+		cm_increaseTable(table);
+
+	for (i = table->idxCur;
+	     table->entries[i] != 0 && i != (table->idxCur-1);
+	     i = (i+1)%table->idxMax);
+
+	if (table->entries[i] == 0) {
+		table->entries[i] = entry;
+		table->idxCur = (i+1) % table->idxMax;
+		table->idxNb++;
+		hdl = ENTRY2HANDLE(entry, i);
+	} else
+		ERROR("No free entry found in table\n", 0, 0, 0, 0, 0, 0);
+
+	return hdl;
+}
+
+/** cm_delEntry - remove the given element from the list
+ *
+ *  1. Check if the handle is valid
+ *  2. Search the entry and free it
+ */
+void cm_delEntry(t_nmf_table *table, t_uint32 idx)
+{
+	table->entries[idx] = NULL;
+	table->idxNb--;
+}
+
+/** cm_lookupEntry - search the entry corresponding to
+ *                   the user handle.
+ *
+ * 1. Check if the handle is valid
+ * 2. Return a pointer to the element
+ */
+void *cm_lookupEntry(const t_nmf_table *table, const t_uint32 hdl)
+{
+	unsigned int idx = hdl & INDEX_MASK;
+
+	if ((idx >= table->idxMax)
+	    || (((unsigned int)table->entries[idx] << INDEX_SHIFT) != (hdl & ~INDEX_MASK)))
+		return NULL;
+	else
+		return table->entries[idx];
+}
+
+/** cm_lookupHandle - search the handle corresponding
+ *                    to the given element
+ *
+ * 1. Check if the handler is valid or is a special handler
+ * 2. Loop in the table to retrieve the entry matching and return its value
+ */
+t_uint32 cm_lookupHandle(const t_nmf_table *table, const void *entry)
+{
+	t_uint32 i;
+
+	/* NULL is an invalid value that must be handle separatly
+	   as it'll match all used/free entries value */
+	if (entry == NULL)
+		return 0;
+
+	for (i=0; i < table->idxMax; i++) {
+		if (table->entries[i] == entry)
+			return ENTRY2HANDLE(table->entries[i], i);
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/nmf-cm/cm/inc/cm.h b/drivers/staging/nmf-cm/cm/inc/cm.h
new file mode 100644
index 00000000000..37ccb36a5ee
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/inc/cm.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_CM_H
+#define __INC_CM_H
+
+#include <cm/inc/cm_def.h>
+
+/********************************************************************************/
+/*       Component Manager API prototypes                                       */
+/********************************************************************************/
+
+/*
+ * User level wrapper
+ */
+#include <cm/proxy/api/cm_proxy.h>
+
+#endif /* __INC_CM_H */
diff --git a/drivers/staging/nmf-cm/cm/inc/cm_def.h b/drivers/staging/nmf-cm/cm/inc/cm_def.h
new file mode 100644
index 00000000000..dc7a1fdad66
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/inc/cm_def.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+/*!
+ * \brief Component Manager API.
+ *
+ * This file contains the Component Manager API for manipulating components.
+ *
+ */
+
+#ifndef __INC_CM_DEF_H
+#define __INC_CM_DEF_H
+
+#include <cm/inc/cm_type.h>
+#include <inc/nmf-def.h>
+
+/*!
+ * \brief Get the version of the NMF CM engine at runtime
+ *
+ * This method should be used to query the version number  of  the
+ * NMF Component Manager engine at runtime. This is useful when using
+ * to check if version of the engine linked with application correspond
+ * to engine used for development. 
+ *
+ * Such code can be used to check compatibility: \code
+    t_uint32 nmfversion;
+
+    // Print NMF version
+    CM_GetVersion(&nmfversion);
+    LOG("NMF Version %d-%d-%d\n",
+            VERSION_MAJOR(nmfversion),
+            VERSION_MINOR(nmfversion),
+            VERSION_PATCH(nmfversion));
+    if(NMF_VERSION != nmfversion) {
+        LOG("Error: Incompatible API version %d != %d\n", NMF_VERSION, nmfversion);
+        EXIT();
+    }
+ * \endcode
+ *
+ * \param[out] version Internal hardcoded version (use \ref VERSION_MAJOR, \ref  VERSION_MINOR, \ref VERSION_PATCH  macros to decode it).
+ *
+ * \ingroup CM
+ */
+PUBLIC IMPORT_SHARED void CM_GetVersion(t_uint32 *version);
+
+#endif /* __INC_CM_H */
diff --git a/drivers/staging/nmf-cm/cm/inc/cm_macros.h b/drivers/staging/nmf-cm/cm/inc/cm_macros.h
new file mode 100644
index 00000000000..2279c204a20
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/inc/cm_macros.h
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+/*!
+ * \brief Component Manager Macros.
+ *
+ * \defgroup CM_MACROS NMF Macros (ANSI C99)
+ * The Component Manager Macros are provided to ease FromHost interface call and ToHost callback definition.
+ * \attention <b>These macros are only ANSI C99 compliant</b> (ARM RVCT 2.x/3.x, GNU gcc 4.x, ...)
+ * \ingroup CM_USER_API
+ */
+
+#ifndef __INC_CM_MACROS_H
+#define __INC_CM_MACROS_H
+
+/*
+ * The next macros are supported only with C Ansi 99, so....
+ */
+
+/*
+ * The Symbian environment dependency, computation which uses an old gnu cpp,
+ * does not accept "..." parameters.
+ * However the actual compiler (armcc) does.
+ * So remove the macro definitions when computing dependencies.
+ */
+#if ( defined(__CC_ARM) && !defined(__STRICT_ANSI__) ) || !defined(__SYMBIAN32__)
+
+/*
+ * Only for skilled eyes ;)
+ * The following macros are used to implement NMFCALL[VOID] and NMFMETH[VOID] macros in an elegant way
+ */
+#define WITH_PARAM(...)  __VA_ARGS__)
+#define WITH_NOPARAM(...) )
+
+/*!
+ * \brief Macro to ease Host to Dsp interface calling
+ *
+ * \attention <b>This macro is only ANSI C99 compliant</b>
+ *
+ * The <i>NMFCALL</i> macro can be used to call one method of any previously FromHost bounded interface.\n
+ * From Host side, today, we have no way to mask the multi-instance handling, so
+ * this macro is provided to ease FromHost interface calling and to avoid any mistake into the THIS parameter passing.
+ *
+ * So, any fromHost interface method call like: \code
+ * itf.method(itf.THIS, param1, param2, ...);
+ * \endcode
+ * can be replaced by: \code
+ * NMFCALL(itf, method)(param1, param2, ...);
+ * \endcode
+ *
+ * \warning Don't forget to use NMFCALLVOID macro when declaring a FromHost interface method having none application parameter,
+ * else it will lead to erroneous C code expansion
+ * \see NMFCALLVOID
+ * \hideinitializer
+ * \ingroup CM_MACROS
+ */
+#define NMFCALL(itfHandle, itfMethodName)  \
+    (itfHandle).itfMethodName((itfHandle).THIS, WITH_PARAM
+
+/*!
+ * \brief Macro to ease Host to Dsp interface calling (method without any user parameter)
+ *
+ * \attention <b>This macro is only ANSI C99 compliant</b>
+ *
+ * The <i>NMFCALLVOID</i> macro can be used to call one method (those without any user parameter) of any previously FromHost bounded interface.\n
+ * From Host side, today, we have no way to mask the multi-instance handling, so
+ * this macro is provided to ease FromHost interface calling and to avoid any mistake into the THIS parameter passing.
+ *
+ * So, any FromHost interface method call without any application parameter like:\code
+ * itf.method(itf.THIS);
+ * \endcode
+ * can be replaced by: \code
+ * NMFCALLVOID(itf, method)();
+ * \endcode
+ * \see NMFCALL
+ * \hideinitializer
+ * \ingroup CM_MACROS
+ */
+#define NMFCALLVOID(itfHandle, itfMethodName)  \
+    (itfHandle).itfMethodName((itfHandle).THIS WITH_NOPARAM
+
+/*!
+ * \brief Macro to ease Dsp to Host interface method declaration
+ *
+ * \attention <b>This macro definition is only ANSI C99 compliant</b>
+ *
+ * The <i>NMFMETH</i> macro can be used to ease the ToHost interface method declaration.\n
+ * From Host side, today, we have no way to mask the multi-intance handling, so the user shall handle it by hand
+ * by passing the "component" context as first parameter of each ToHost interface method through the void *THIS parameter.
+ * This macro could avoid any mistake into the THIS parameter declaration when never used by the user code.
+ *
+ * So, any ToHost interface method declaration like:\code
+ * void mynotify(void *THIS, mytype1 myparam1, mytype2 myparam2, ...) {
+ * <body of the interface routine>
+ * }
+ * \endcode
+ * can be replaced by: \code
+ * void NMFMETH(mynotify)(mytype1 myparam1, mytype2 myparam2, ...) {
+ * <body of the interface routine>
+ * }
+ * \endcode
+ *
+ * \warning Don't forget to use NMFMETHVOID macro when declaring a ToHost interface method having none application parameter,
+ * else it will lead to erroneous C code expansion
+ *
+ * \see NMFMETHVOID
+ * \hideinitializer
+ * \ingroup CM_MACROS
+ */
+#define NMFMETH(itfMethodName) \
+    itfMethodName(void *THIS, WITH_PARAM
+
+/*!
+ * \brief Macro to ease Dsp to Host interface method declaration (method without any user parameter)
+ *
+ * \attention <b>This macro is only ANSI C99 compliant</b>
+ *
+ * The <i>NMFMETHVOID</i> macro can be used to ease the ToHost interface method (those without any user parameter) declaration.\n
+ * From Host side, today, we have no way to mask the multi-intance handling, so the user shall handle it by hand
+ * by passing the "component" context as first parameter of each ToHost interface method through the void *THIS parameter.
+ * This macro could avoid any mistake into the THIS parameter declaration when never used by the user code.
+ *
+ * So, any ToHost interface method declaration having none application parameter like:\code
+ * void mynotify(void *THIS) {
+ * <body of the interface routine>
+ * }
+ * \endcode
+ * can be replaced by: \code
+ * void NMFMETHVOID(mynotify)(void) {
+ * <body of the interface routine>
+ * }
+ * \endcode
+ *
+ * \see NMFMETH
+ * \hideinitializer
+ * \ingroup CM_MACROS
+ */
+#define NMFMETHVOID(itfMethodName) \
+    itfMethodName(void *THIS WITH_NOPARAM
+
+#endif /* not Symbian environment or compiling with ARMCC and not in strict ANSI */
+
+#endif /* __INC_CM_MACROS_H */
+
diff --git a/drivers/staging/nmf-cm/cm/inc/cm_type.h b/drivers/staging/nmf-cm/cm/inc/cm_type.h
new file mode 100644
index 00000000000..b2f97090a3a
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/inc/cm_type.h
@@ -0,0 +1,147 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Component Manager types.
+ *
+ * This file contains the Component Manager  types.
+ *
+ * \defgroup CM CM Type Definitions
+ * \ingroup CM_USER_API
+ */
+#ifndef _CM_TYPE_H_
+#define _CM_TYPE_H_
+
+#include <share/inc/nmf.h>
+#include <share/inc/macros.h>
+
+#include <nmf/inc/channel_type.h>
+
+/*!
+ * @defgroup t_cm_error t_cm_error
+ * \brief Description of the various errors returned by CM API routines
+ * @{
+ * \ingroup CM
+ */
+typedef t_nmf_error t_cm_error;                                             //!< Error type returned by CM API routines
+
+/*********************************************************************************/
+/* WARNING: UPDATE CM_StringError() func each time an error is added/removed !!! */
+/* CM_StringError() is defined twice in:                                         */
+/*   nmf_core/host/cm/proxy/common/wrapper/src/wrapper.c                         */
+/*   tests/src/common/nte/src/nte.c                                              */
+/*********************************************************************************/
+#define CM_LAST_ERROR_ID                    ((t_cm_error)-128)
+#define CM_INTEGRATION_ERROR               NMF_INTEGRATION_ERROR0       //!< \ref NMF_INTEGRATION_ERROR0
+
+    /* Communication */
+#define CM_FLUSH_MESSAGE                    NMF_FLUSH_MESSAGE           //!< Message send after call to CM_FlushChannel()
+#define CM_BUFFER_OVERFLOW                  ((t_cm_error)-105)          //!< Buffer overflow (interface binding message bigger than buffer)
+#define CM_USER_NOT_REGISTERED              ((t_cm_error)-104)          //!< User not registered
+#define CM_NO_MESSAGE                       NMF_NO_MESSAGE              //!< \ref NMF_NO_MESSAGE
+#define CM_PARAM_FIFO_OVERFLOW              ((t_cm_error)-102)          //!< Param fifo overflow
+#define CM_INTERNAL_FIFO_OVERFLOW           ((t_cm_error)-101)          //!< Internal services fifo overflow (not returned to user)
+#define CM_MPC_NOT_RESPONDING               ((t_cm_error)-100)          //!< MPC not responding (either crash, interrupt handler too long, internal NMF fifo coms overflow, ...).
+
+    /* ELF & File system */
+#define CM_FS_ERROR                         ((t_cm_error)-96)           //!< FileSystem error
+#define CM_NO_SUCH_FILE                     ((t_cm_error)-95)           //!< No such file or directory
+#define CM_INVALID_ELF_FILE                 ((t_cm_error)-94)           //!< File isn't a valid MMDSP ELF file
+#define CM_NO_SUCH_BASE                     ((t_cm_error)-93)           //!< The memory base doesn't exist
+
+    /* Introspection */
+#define CM_NO_SUCH_ATTRIBUTE                NMF_NO_SUCH_ATTRIBUTE       //!< \ref NMF_NO_SUCH_ATTRIBUTE
+#define CM_NO_SUCH_PROPERTY                 NMF_NO_SUCH_PROPERTY        //!< \ref NMF_NO_SUCH_PROPERTY
+
+    /* Component Life Cycle */
+#define CM_COMPONENT_NOT_STOPPED            NMF_COMPONENT_NOT_STOPPED   //!< \ref NMF_COMPONENT_NOT_STOPPED
+#define CM_COMPONENT_NOT_UNBINDED           ((t_cm_error)-79)           //!< Component must be fully unbinded before perform operation
+#define CM_COMPONENT_NOT_STARTED            ((t_cm_error)-78)           //!< Component must be started to perform operation
+#define CM_COMPONENT_WAIT_RUNNABLE          ((t_cm_error)-76)           //!< Component need acknowlegdment of life cycle start function before perform operation
+#define CM_REQUIRE_INTERFACE_UNBINDED       ((t_cm_error)-75)           //!< Required component interfaces must be binded before perform operation
+#define CM_INVALID_COMPONENT_HANDLE         ((t_cm_error)-74)           //!< Try to access a component already destroyed
+
+    /* Binder */
+#define CM_NO_SUCH_PROVIDED_INTERFACE       NMF_NO_SUCH_PROVIDED_INTERFACE   //!< \ref NMF_NO_SUCH_PROVIDED_INTERFACE
+#define CM_NO_SUCH_REQUIRED_INTERFACE       NMF_NO_SUCH_REQUIRED_INTERFACE   //!< \ref NMF_NO_SUCH_REQUIRED_INTERFACE
+#define CM_ILLEGAL_BINDING                  ((t_cm_error)-62)           //!< Client and server interface type mismatch
+#define CM_ILLEGAL_UNBINDING                ((t_cm_error)-61)           //!< Try to unbind component with bad binding Factories
+#define CM_INTERFACE_ALREADY_BINDED         NMF_INTERFACE_ALREADY_BINDED//!< \ref NMF_INTERFACE_ALREADY_BINDED
+#define CM_INTERFACE_NOT_BINDED             NMF_INTERFACE_NOT_BINDED    //!< \ref NMF_INTERFACE_NOT_BINDED
+
+    /* Loader */
+#define CM_BINDING_COMPONENT_NOT_FOUND      ((t_cm_error)-48)           //!< Binding Component template name don't exist on components repository (should be generated thanks nkitf tool)
+#define CM_COMPONENT_NOT_FOUND              ((t_cm_error)-47)           //!< Component template name doesn't exist on components repository
+#define CM_NO_SUCH_SYMBOL                   ((t_cm_error)-46)           //!< Symbol name doesn't exported by the underlying component
+#define CM_COMPONENT_EXIST                  ((t_cm_error)-45)           //!< Component name already exists in the component cache
+
+    /* Fifo management related ones */
+#define CM_FIFO_FULL                        ((t_cm_error)-40)           //!< Fifo is full
+#define CM_FIFO_EMPTY                       ((t_cm_error)-39)           //!< Fifo is empty
+#define CM_UNKNOWN_FIFO_ID                  ((t_cm_error)-38)           //!< Fifo handle doesn't exist
+
+    /* Memory management related ones */
+#define CM_DOMAIN_VIOLATION                 ((t_cm_error)-33)           //!< Domain violation
+#define CM_CREATE_ALLOC_ERROR               ((t_cm_error)-32)           //!< Error during allocator creation
+#define CM_UNKNOWN_MEMORY_HANDLE            ((t_cm_error)-31)           //!< Handle doesn't exists
+#define CM_NO_MORE_MEMORY                   NMF_NO_MORE_MEMORY          //!< \ref NMF_NO_MORE_MEMORY
+#define CM_BAD_MEMORY_ALIGNMENT             ((t_cm_error)-29)           //!< Memory alignment wanted is not correct
+#define CM_MEMORY_HANDLE_FREED              ((t_cm_error)-28)           //!< Handle was alread freed
+#define CM_INVALID_DOMAIN_DEFINITION        ((t_cm_error)-27)           //!< Domain to be created is not correctly defined
+#define CM_INTERNAL_DOMAIN_OVERFLOW         ((t_cm_error)-26)           //!< Internal domain descriptor overflow (too many domains) //TODO, juraj, remove this error
+#define CM_INVALID_DOMAIN_HANDLE            ((t_cm_error)-25)           //!< Invalid domain handle
+#define CM_ILLEGAL_DOMAIN_OPERATION         ((t_cm_error)-21)           //!< Operation on a domain is illegal (like destroy of a domain with referenced components)
+
+    /* Media Processor related ones */
+#define CM_MPC_INVALID_CONFIGURATION        ((t_cm_error)-24)           //!< Media Processor Core invalid configuration
+#define CM_MPC_NOT_INITIALIZED              ((t_cm_error)-23)           //!< Media Processor Core not yet initialized
+#define CM_MPC_ALREADY_INITIALIZED          ((t_cm_error)-22)           //!< Media Processor Core already initialized
+//ERROR 21 is defined above, with the domains
+
+    /* Power Mgt related ones */
+#define CM_PWR_NOT_AVAILABLE                ((t_cm_error)-16)           //!< No modification of the state of the power input
+
+    /* Common errors */
+#define CM_OUT_OF_LIMITS                    ((t_cm_error)-3)            //!< User reach an internal nmf limits of limits.h file
+#define CM_INVALID_PARAMETER                NMF_INVALID_PARAMETER       //!< \ref NMF_INVALID_PARAMETER
+#define CM_NOT_YET_IMPLEMENTED              ((t_cm_error)-1)            //!< CM API not yet implemented
+#define CM_OK                               NMF_OK                      //!< \ref NMF_OK
+
+/** @} */
+
+/*!
+ * \brief Definition of a physical memory address
+ * \ingroup MEMORY
+ */
+typedef t_uint32 t_cm_physical_address;
+
+/*!
+ * \brief Definition of a logical memory address
+ * \ingroup MEMORY
+ */
+typedef t_uint32 t_cm_logical_address;
+
+/*!
+ * \brief Definition of a system address into a system with MMU
+ * \ingroup MEMORY
+ */
+typedef struct {
+    t_cm_physical_address physical; //!< Physical memory address
+    t_cm_logical_address logical;       //!< Logical memory address
+} t_cm_system_address;
+#define INVALID_SYSTEM_ADDRESS {(t_cm_physical_address)MASK_ALL32, (t_cm_logical_address)MASK_ALL32}
+
+
+/*!
+ * \brief Define a type used to manipulate size of various buffers
+ * \ingroup MEMORY
+ */
+typedef t_uint32 t_cm_size;
+
+#endif /* _CM_TYPE_H_ */
+
diff --git a/drivers/staging/nmf-cm/cm_dma.c b/drivers/staging/nmf-cm/cm_dma.c
new file mode 100644
index 00000000000..1db5a3cf951
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_dma.c
@@ -0,0 +1,178 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <asm/io.h>
+
+ #include "cm_dma.h"
+
+ #define CMDMA_LIDX (2)
+ #define CMDMA_BASE (0x801C0000)
+ #define CMDMA_REG_LCLA (0x024)
+
+void __iomem *virtbase = NULL;
+
+static int cmdma_write_cyclic_list_mem2per(
+    unsigned int from_addr,
+    unsigned int to_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS);
+
+static int cmdma_write_cyclic_list_per2mem(
+    unsigned int from_addr,
+    unsigned int to_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS);
+
+int cmdma_setup_relink_area( unsigned int mem_addr,
+    unsigned int per_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS,
+    enum cmdma_type type)
+{
+    switch (type) {
+
+    case CMDMA_MEM_2_PER:
+        return cmdma_write_cyclic_list_mem2per(
+            mem_addr,
+            per_addr,
+            segments,
+            segmentsize,
+            LOS);
+
+    case CMDMA_PER_2_MEM:
+        return cmdma_write_cyclic_list_per2mem(
+            per_addr,
+            mem_addr,
+            segments,
+            segmentsize,
+            LOS);
+
+    default :
+        return -EINVAL;
+    }
+ }
+
+ static unsigned int cmdma_getlcla( void) {
+
+    if(!virtbase)
+        virtbase = ioremap(CMDMA_BASE, CMDMA_REG_LCLA + sizeof(int) );
+
+    return readl(virtbase + CMDMA_REG_LCLA);
+ }
+
+ static void cmdma_write_relink_params_mem2per (
+    int * relink,
+    unsigned int LOS,
+    unsigned int nb_element,
+    unsigned int src_addr,
+    unsigned int dst_addr,
+    unsigned int burst_size) {
+
+    relink[0] =  (((long)(nb_element & 0xFFFF)) << 16) |
+        (src_addr & 0xFFFF);
+
+    relink[1] = (((src_addr >> 16)  & 0xFFFFUL) << 16) |
+        (0x1200UL | (LOS << 1) | (burst_size<<10));
+
+    relink[2] = ((nb_element & 0xFFFF) << 16)  |
+        (dst_addr & 0xFFFF);
+
+    relink[3] = (((dst_addr >> 16)  & 0xFFFFUL) << 16 ) |
+        0x8201UL | ((LOS+1) << 1) | (burst_size<<10);
+}
+
+static void cmdma_write_relink_params_per2mem (
+    int * relink,
+    unsigned int LOS,
+    unsigned int nb_element,
+    unsigned int src_addr,
+    unsigned int dst_addr,
+    unsigned int burst_size) {
+
+    relink[0] =  (((long)(nb_element & 0xFFFF)) << 16) |
+        (src_addr & 0xFFFF);
+
+    relink[1] = (((src_addr >> 16)  & 0xFFFFUL) << 16) |
+        (0x8201UL | (LOS << 1) | (burst_size<<10));
+
+    relink[2] = ((nb_element & 0xFFFF) << 16)  |
+        (dst_addr & 0xFFFF);
+
+    relink[3] = (((dst_addr >> 16)  & 0xFFFFUL) << 16 ) |
+        0x1200UL | ((LOS+1) << 1) | (burst_size<<10);
+}
+
+static int cmdma_write_cyclic_list_mem2per(
+    unsigned int from_addr,
+    unsigned int to_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS) {
+
+    unsigned int i,j;
+    int *relink;
+
+    j = LOS;
+
+    for ( i = 0; i < segments; i++) {
+        relink = phys_to_virt (cmdma_getlcla() + 1024 * CMDMA_LIDX + 8 * j);
+
+        if (i == (segments-1))
+                j = LOS;
+            else
+                j += 2;
+
+        cmdma_write_relink_params_mem2per (
+            relink,
+            j,
+            segmentsize / 4,
+            from_addr,
+            to_addr,
+            0x2);
+
+        from_addr += segmentsize;
+	}
+
+    return 0;
+}
+
+static int cmdma_write_cyclic_list_per2mem(
+    unsigned int from_addr,
+    unsigned int to_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS) {
+
+    unsigned int i,j;
+    int *relink;
+    j = LOS;
+
+    for ( i = 0; i < segments; i++) {
+        relink = phys_to_virt (cmdma_getlcla() + 1024 * CMDMA_LIDX + 8 * j);
+
+        if (i == (segments-1))
+            j = LOS;
+        else
+            j += 2;
+
+        cmdma_write_relink_params_per2mem (
+            relink,
+            j,
+            segmentsize / 4,
+            from_addr,
+            to_addr,
+            0x2);
+
+        to_addr += segmentsize;
+    }
+
+    return 0;
+}
diff --git a/drivers/staging/nmf-cm/cm_dma.h b/drivers/staging/nmf-cm/cm_dma.h
new file mode 100644
index 00000000000..6ee4caafeae
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_dma.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+  * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+
+#ifndef __CMDMA_H
+#define __CMDMA_H
+
+enum cmdma_type {
+    CMDMA_MEM_2_PER,
+    CMDMA_PER_2_MEM
+};
+
+int cmdma_setup_relink_area(
+    unsigned int mem_addr,
+    unsigned int per_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS,
+    enum cmdma_type type);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm_service.c b/drivers/staging/nmf-cm/cm_service.c
new file mode 100644
index 00000000000..cd7602a6cec
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_service.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/** \file cm_service.c
+ *
+ * Nomadik Multiprocessing Framework Linux Driver
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/plist.h>
+#include <linux/slab.h>
+#include <linux/spinlock_types.h>
+#include <linux/sched.h>
+
+#include <cm/engine/api/control/irq_engine.h>
+
+#include "osal-kernel.h"
+#include "cmld.h"
+#include "cm_service.h"
+
+/* Panic managment */
+static void service_tasklet_func(unsigned long);
+unsigned long service_tasklet_data = 0;
+DECLARE_TASKLET(cmld_service_tasklet, service_tasklet_func, 0);
+
+void dispatch_service_msg(struct osal_msg *msg)
+{
+	struct list_head *head, *next;
+
+	/* Note: no lock needed to protect the channel_list to protect against list
+	   changes, as the current tasklet is disabled each time we modify the list */
+	list_for_each_safe(head, next, &channel_list) {
+		struct cm_channel_priv *channelPriv = list_entry(head, struct cm_channel_priv, entry);
+		struct osal_msg *new_msg;
+		size_t msg_size;
+
+		if (channelPriv->state == CHANNEL_CLOSED)
+			continue;
+		msg_size = sizeof(new_msg->hdr) + sizeof(new_msg->d.srv);
+		new_msg = kmalloc(msg_size, GFP_ATOMIC);
+		if (new_msg == NULL) {
+			pr_err("[CM] %s: can't allocate memory, service"
+			       " message not dispatched !!\n", __func__);
+			continue;
+		}
+		memcpy(new_msg, msg, msg_size);
+		plist_node_init(&new_msg->msg_entry, 0);
+		spin_lock_bh(&channelPriv->bh_lock);
+		plist_add(&new_msg->msg_entry, &channelPriv->messageQueue);
+		spin_unlock_bh(&channelPriv->bh_lock);
+		wake_up_interruptible(&channelPriv->waitq);
+	}
+}
+
+static void service_tasklet_func(unsigned long unused)
+{
+	t_cm_service_type type;
+	t_cm_service_description desc;
+	int i=0;
+
+	do {
+		if (test_and_clear_bit(i, &service_tasklet_data)) { 
+			CM_getServiceDescription(osalEnv.mpc[i].coreId, &type, &desc);
+
+			switch (type) {
+			case CM_MPC_SERVICE_PANIC: {
+				struct osal_msg msg;
+
+				msg.msg_type = MSG_SERVICE;
+				msg.d.srv.srvType = NMF_SERVICE_PANIC;
+				msg.d.srv.srvData.panic = desc.u.panic;
+
+				dispatch_service_msg(&msg);
+				break;
+			}
+			case CM_MPC_SERVICE_PRINT: {
+				char msg[256];
+				if (CM_ReadMPCString(osalEnv.mpc[i].coreId,
+						     desc.u.print.dspAddress, msg,
+						     sizeof(msg)) == CM_OK)
+					printk(msg, desc.u.print.value1, desc.u.print.value2);
+				break;
+			}
+			default:
+				pr_err("[CM] %s: MPC Service Type %d not supported\n", __func__, type);
+			}
+			enable_irq(osalEnv.mpc[i].interrupt1);
+		}
+		i = (i+1) % NB_MPC;
+	} while (service_tasklet_data != 0);
+}
diff --git a/drivers/staging/nmf-cm/cm_service.h b/drivers/staging/nmf-cm/cm_service.h
new file mode 100644
index 00000000000..39582eae573
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_service.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/** \file cm_service.c
+ *
+ * Nomadik Multiprocessing Framework Linux Driver
+ *
+ */
+
+#ifndef CM_SERVICE_H
+#define CM_SERVICE_H
+
+#include <linux/interrupt.h>
+
+extern unsigned long service_tasklet_data;
+extern struct tasklet_struct cmld_service_tasklet;
+void dispatch_service_msg(struct osal_msg *msg);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm_syscall.c b/drivers/staging/nmf-cm/cm_syscall.c
new file mode 100644
index 00000000000..8a48ac10c60
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_syscall.c
@@ -0,0 +1,1391 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <cm/engine/api/cm_engine.h>
+#include "cmioctl.h"
+#include "osal-kernel.h"
+#include "cmld.h"
+#include <cm/engine/memory/inc/remote_allocator.h>
+#include <linux/kernel.h>
+
+/** Dequeue and free per-process messages for specific binding
+ *
+ * \note
+ * This is only safe if the per process mutex is held when called.
+ */
+static inline void freeMessages(struct cm_channel_priv* cPriv, t_skelwrapper* binding)
+{
+	struct osal_msg *this, *next;
+
+	spin_lock_bh(&cPriv->bh_lock);
+
+	/* free any pending messages */
+         plist_for_each_entry_safe(this, next, &cPriv->messageQueue, msg_entry) {
+		if (this->msg_type == MSG_INTERFACE
+		    && this->d.itf.skelwrap == binding) {
+			plist_del(&this->msg_entry, &cPriv->messageQueue);
+			kfree(this);
+		}
+	}
+	spin_unlock_bh(&cPriv->bh_lock);
+}
+
+static t_cm_error copy_string_from_user(char *dst, const char __user *src, int len)
+{
+	int ret;
+
+	ret = strncpy_from_user(dst, src, len);
+	if (ret < 0) /* -EFAULT */
+		return CM_INVALID_PARAMETER;
+
+	if (ret >= len)
+		return CM_OUT_OF_LIMITS;
+
+	return 0;
+}
+
+inline int cmld_InstantiateComponent(struct cm_process_priv* procPriv,
+				     CM_InstantiateComponent_t __user *param)
+{
+	CM_InstantiateComponent_t data;
+	char templateName[MAX_TEMPLATE_NAME_LENGTH];
+	char localName[MAX_COMPONENT_NAME_LENGTH];
+	char *dataFile = NULL;
+
+	/* Copy all user data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if (data.in.dataFile != NULL) {
+		dataFile = OSAL_Alloc(data.in.dataFileSize);
+		if (dataFile == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFile, data.in.dataFile, data.in.dataFileSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	if ((data.out.error = copy_string_from_user(templateName,
+						    data.in.templateName,
+						    sizeof(templateName))))
+		goto out;
+
+	if ((data.in.localName != NULL) &&
+	    (data.out.error = copy_string_from_user(localName,
+						    data.in.localName,
+						    sizeof(localName))))
+		goto out;
+
+	/* Do appropriate CM Engine call */
+	data.out.error = CM_ENGINE_InstantiateComponent(templateName,
+							data.in.domainId,
+							procPriv->pid,
+							data.in.priority,
+							data.in.localName ? localName : NULL,
+							dataFile,
+							&data.out.component);
+
+out:
+	if (dataFile)
+		OSAL_Free(dataFile);
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_BindComponentFromCMCore(struct cm_process_priv* procPriv,
+					CM_BindComponentFromCMCore_t __user *param)
+{
+	CM_BindComponentFromCMCore_t data;
+	char providedItfServerName[MAX_INTERFACE_NAME_LENGTH];
+	char *dataFileSkeleton = NULL;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(providedItfServerName,
+						    data.in.providedItfServerName,
+						    sizeof(providedItfServerName))))
+		goto out;
+
+	if (data.in.dataFileSkeleton != NULL) {
+		dataFileSkeleton = OSAL_Alloc(data.in.dataFileSkeletonSize);
+		if (dataFileSkeleton == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFileSkeleton, data.in.dataFileSkeleton,
+				   data.in.dataFileSkeletonSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	data.out.error = CM_ENGINE_BindComponentFromCMCore(data.in.server,
+							   providedItfServerName,
+							   data.in.fifosize,
+							   data.in.eventMemType,
+							   &data.out.host2mpcId,
+							   procPriv->pid,
+							   dataFileSkeleton);
+out:
+	if (dataFileSkeleton)
+		OSAL_Free(dataFileSkeleton);
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+
+	return 0;
+}
+
+inline int cmld_UnbindComponentFromCMCore(CM_UnbindComponentFromCMCore_t __user *param)
+{
+	CM_UnbindComponentFromCMCore_t data;
+
+	/* Copy all user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_UnbindComponentFromCMCore(data.in.host2mpcId);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_BindComponentToCMCore(struct cm_channel_priv* channelPriv,
+				      CM_BindComponentToCMCore_t __user *param)
+{
+	CM_BindComponentToCMCore_t data;
+	t_skelwrapper *skelwrapper;
+	struct cm_process_priv *procPriv = channelPriv->proc;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+	char *dataFileStub = NULL;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	/* Do appropriate CM Engine call */
+	skelwrapper = (t_skelwrapper *)OSAL_Alloc(sizeof(*skelwrapper));
+	if (skelwrapper == NULL) {
+		data.out.error = CM_NO_MORE_MEMORY;
+		goto out;
+	}
+
+	if (data.in.dataFileStub != NULL) {
+		dataFileStub = OSAL_Alloc(data.in.dataFileStubSize);
+		if (dataFileStub == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFileStub, data.in.dataFileStub, data.in.dataFileStubSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	if ((data.out.error = CM_ENGINE_BindComponentToCMCore(
+		     data.in.client,
+		     requiredItfClientName,
+		     data.in.fifosize,
+		     (t_nmf_mpc2host_handle)skelwrapper,
+		     dataFileStub,
+		     &data.out.mpc2hostId,
+		     procPriv->pid)) != CM_OK) {
+		OSAL_Free(skelwrapper);
+		goto out;
+	}
+
+	skelwrapper->upperLayerThis = data.in.upLayerThis;
+	skelwrapper->mpc2hostId = data.out.mpc2hostId;
+	skelwrapper->channelPriv = channelPriv;
+	mutex_lock(&channelPriv->skelListLock);
+	list_add(&skelwrapper->entry, &channelPriv->skelList);
+	mutex_unlock(&channelPriv->skelListLock);
+out:
+	if (dataFileStub != NULL)
+		OSAL_Free(dataFileStub);
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_UnbindComponentToCMCore(struct cm_process_priv* procPriv,
+					CM_UnbindComponentToCMCore_t __user *param)
+{
+	CM_UnbindComponentToCMCore_t data;
+	t_skelwrapper *skelwrapper;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_UnbindComponentToCMCore(
+			data.in.client, requiredItfClientName,
+			(t_nmf_mpc2host_handle*)&skelwrapper,
+								procPriv->pid);
+
+	if (data.out.error != CM_OK && data.out.error != CM_MPC_NOT_RESPONDING)
+		goto out;
+
+	data.out.upLayerThis = skelwrapper->upperLayerThis;
+
+	mutex_lock(&skelwrapper->channelPriv->msgQueueLock);
+	freeMessages(skelwrapper->channelPriv, skelwrapper);
+	mutex_lock(&skelwrapper->channelPriv->skelListLock);
+	list_del(&skelwrapper->entry);
+	mutex_unlock(&skelwrapper->channelPriv->skelListLock);
+	mutex_unlock(&skelwrapper->channelPriv->msgQueueLock);
+	OSAL_Free(skelwrapper);
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_BindComponentAsynchronous(struct cm_process_priv* procPriv,
+					  CM_BindComponentAsynchronous_t __user *param)
+{
+	CM_BindComponentAsynchronous_t data;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+	char providedItfServerName[MAX_INTERFACE_NAME_LENGTH];
+	char *dataFileSkeletonOrEvent = NULL;
+	char *dataFileStub = NULL;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	if ((data.out.error = copy_string_from_user(providedItfServerName,
+						    data.in.providedItfServerName,
+						    sizeof(providedItfServerName))))
+		goto out;
+
+	if (data.in.dataFileSkeletonOrEvent != NULL) {
+		dataFileSkeletonOrEvent =
+			OSAL_Alloc(data.in.dataFileSkeletonOrEventSize);
+		if (dataFileSkeletonOrEvent == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFileSkeletonOrEvent, data.in.dataFileSkeletonOrEvent, data.in.dataFileSkeletonOrEventSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	if (data.in.dataFileStub != NULL) {
+		dataFileStub = OSAL_Alloc(data.in.dataFileStubSize);
+		if (dataFileStub == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFileStub, data.in.dataFileStub, data.in.dataFileStubSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	/* Do appropriate CM Engine call */
+	data.out.error = CM_ENGINE_BindComponentAsynchronous(data.in.client,
+							     requiredItfClientName,
+							     data.in.server,
+							     providedItfServerName,
+							     data.in.fifosize,
+							     data.in.eventMemType,
+							     procPriv->pid,
+							     dataFileSkeletonOrEvent,
+							     dataFileStub);
+
+out:
+	if (dataFileSkeletonOrEvent != NULL)
+		OSAL_Free(dataFileSkeletonOrEvent);
+	if (dataFileStub != NULL)
+		OSAL_Free(dataFileStub);
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_UnbindComponentAsynchronous(struct cm_process_priv* procPriv,
+					    CM_UnbindComponentAsynchronous_t __user *param)
+{
+	CM_UnbindComponentAsynchronous_t data;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+
+	/* Copy all user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	/* Do appropriate CM Engine call */
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_UnbindComponentAsynchronous(data.in.client,
+								requiredItfClientName,
+								procPriv->pid);
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_BindComponent(struct cm_process_priv* procPriv,
+			      CM_BindComponent_t __user *param)
+{
+	CM_BindComponent_t data;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+	char providedItfServerName[MAX_INTERFACE_NAME_LENGTH];
+	char *dataFileTrace = NULL;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	if ((data.out.error = copy_string_from_user(providedItfServerName,
+						    data.in.providedItfServerName,
+						    sizeof(providedItfServerName))))
+		goto out;
+
+	if (data.in.dataFileTrace != NULL) {
+		dataFileTrace = OSAL_Alloc(data.in.dataFileTraceSize);
+		if (dataFileTrace == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFileTrace, data.in.dataFileTrace,
+				   data.in.dataFileTraceSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	/* Do appropriate CM Engine call */
+	data.out.error = CM_ENGINE_BindComponent(data.in.client,
+						 requiredItfClientName,
+						 data.in.server,
+						 providedItfServerName,
+						 data.in.traced,
+						 procPriv->pid,
+						 dataFileTrace);
+out:
+	if (dataFileTrace != NULL)
+		OSAL_Free(dataFileTrace);
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_UnbindComponent(struct cm_process_priv* procPriv,
+				CM_UnbindComponent_t __user *param)
+{
+	CM_UnbindComponent_t data;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	/* Do appropriate CM Engine call */
+	data.out.error = CM_ENGINE_UnbindComponent(data.in.client,
+						   requiredItfClientName,
+						   procPriv->pid);
+
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_BindComponentToVoid(struct cm_process_priv* procPriv,
+				    CM_BindComponentToVoid_t __user *param)
+{
+	CM_BindComponentToVoid_t data;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_BindComponentToVoid(data.in.client,
+							requiredItfClientName,
+								procPriv->pid);
+
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_DestroyComponent(struct cm_process_priv* procPriv,
+				 CM_DestroyComponent_t __user *param)
+{
+	CM_DestroyComponent_t data;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_DestroyComponent(data.in.component,
+						    procPriv->pid);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_CreateMemoryDomain(struct cm_process_priv *procPriv,
+				   CM_CreateMemoryDomain_t __user *param)
+{
+	CM_CreateMemoryDomain_t data;
+	t_cm_domain_memory domain;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if (copy_from_user(&domain, data.in.domain, sizeof(domain)))
+		return -EFAULT;
+
+	if (data.in.client == NMF_CURRENT_CLIENT)
+		data.out.error = CM_ENGINE_CreateMemoryDomain(procPriv->pid,
+							      &domain,
+							      &data.out.handle);
+	else
+		data.out.error = CM_ENGINE_CreateMemoryDomain(data.in.client,
+							      &domain,
+							      &data.out.handle);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_CreateMemoryDomainScratch(struct cm_process_priv *procPriv,
+					  CM_CreateMemoryDomainScratch_t __user *param)
+{
+	CM_CreateMemoryDomainScratch_t data;
+	t_cm_domain_memory domain;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if (copy_from_user(&domain, data.in.domain, sizeof(domain)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_CreateMemoryDomainScratch(procPriv->pid,
+							     data.in.parentId,
+							     &domain,
+							     &data.out.handle);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_DestroyMemoryDomain(CM_DestroyMemoryDomain_t __user *param)
+{
+	CM_DestroyMemoryDomain_t data;
+
+	/* Copy all user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_DestroyMemoryDomain(data.in.domainId);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetDomainCoreId(CM_GetDomainCoreId_t __user *param)
+{
+	CM_GetDomainCoreId_t data;
+
+	/* Copy all user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_GetDomainCoreId(data.in.domainId,
+						   &data.out.coreId);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_AllocMpcMemory(struct cm_process_priv *procPriv,
+			       CM_AllocMpcMemory_t __user *param)
+{
+	t_cm_error err;
+	CM_AllocMpcMemory_t data;
+	t_cm_memory_handle handle = 0;
+	struct memAreaDesc_t* memAreaDesc;
+	t_cm_system_address systemAddress;
+	t_uint32 mpcAddress;
+
+	/* Copy all user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* Disregard alignment information and force 4kB memory alignment,
+	   in any case (see devnotes.txt) */
+	/* PP: Disable this 'force' for now, because of the low amount of
+	   available MPC Memory */
+	//data.in.memAlignment = CM_MM_MPC_ALIGN_1024WORDS;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_AllocMpcMemory(data.in.domainId,
+						  procPriv->pid,
+						  data.in.memType,
+						  data.in.size,
+						  data.in.memAlignment,
+						  &handle);
+
+	data.out.pHandle = handle;
+
+	if (data.out.error != CM_OK)
+		goto out;
+
+	/* Get memory area decriptors in advance
+	   so to fill in list elements right now */
+	err = CM_ENGINE_GetMpcMemorySystemAddress(handle, &systemAddress);
+	if (err != CM_OK) {
+		pr_err("%s: failed CM_ENGINE_GetMpcMemorySystemAddress (%i)\n", __func__, err);
+		/* If we can't manage internally this allocated memory latter, it's
+		   better to report the error now.
+		   Free the handle to not let the driver in an inconsistent state */
+		CM_ENGINE_FreeMpcMemory(handle);
+		return -EFAULT;
+	}
+
+	/* Get MPC address in advance so to fill in list elements right now */
+	err = CM_ENGINE_GetMpcMemoryMpcAddress(handle, &mpcAddress);
+	if (err != CM_OK) {
+		pr_err("%s: failed CM_ENGINE_GetMpcMemoryMpcAddress (%i)\n", __func__, err);
+		/* see comments above */
+		CM_ENGINE_FreeMpcMemory(handle);
+		return -EFAULT;
+	}
+
+	/* Allocate and fill a new memory area descriptor. Add it to the list */
+	memAreaDesc = OSAL_Alloc(sizeof(struct memAreaDesc_t));
+	if (memAreaDesc == NULL) {
+		pr_err("%s: failed allocating memAreaDesc\n", __func__);
+		/* see comments above */
+		CM_ENGINE_FreeMpcMemory(handle);
+		return -ENOMEM;
+	}
+
+	memAreaDesc->procPriv = procPriv;
+	memAreaDesc->handle = handle;
+	memAreaDesc->tid = 0;
+	memAreaDesc->physAddr = systemAddress.physical;
+	memAreaDesc->kernelLogicalAddr = systemAddress.logical;
+	memAreaDesc->userLogicalAddr = 0;
+	memAreaDesc->mpcPhysAddr = mpcAddress;
+	memAreaDesc->size = data.in.size * ((data.in.memType % 2) ? 4 : 2); // betzw: set size in bytes for host (ugly version)
+	atomic_set(&memAreaDesc->count, 0);
+
+	if (lock_process(procPriv)) {
+		/* may be rather call lock_process_uninterruptible() */
+		CM_ENGINE_FreeMpcMemory(handle);
+		OSAL_Free(memAreaDesc);
+		return -ERESTARTSYS;
+	}
+	list_add(&memAreaDesc->list, &procPriv->memAreaDescList);
+	unlock_process(procPriv);
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_FreeMpcMemory(struct cm_process_priv *procPriv,
+			      CM_FreeMpcMemory_t __user *param)
+{
+	CM_FreeMpcMemory_t data;
+	struct list_head *cursor, *next;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* check that it is actually owned by the process */
+	data.out.error = CM_UNKNOWN_MEMORY_HANDLE;
+
+	if (lock_process(procPriv))
+		return -ERESTARTSYS;
+	list_for_each_safe(cursor, next, &procPriv->memAreaDescList){
+		struct memAreaDesc_t* curr;
+		curr = list_entry(cursor, struct memAreaDesc_t, list);
+		if (curr->handle == data.in.handle){
+			if (atomic_read(&curr->count) != 0) {
+				pr_err("%s: Memory area (phyAddr: %x, size: %d) "
+				       "still in use (count=%d)!\n", __func__,
+				       curr->physAddr, curr->size,
+				       atomic_read(&curr->count));
+				data.out.error = CM_INVALID_PARAMETER;
+			} else {
+				data.out.error =
+					CM_ENGINE_FreeMpcMemory(data.in.handle);
+				if (data.out.error == CM_OK) {
+					list_del(cursor);
+					OSAL_Free(curr);
+				}
+			}
+			break;
+		}
+	}
+	unlock_process(procPriv);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetMpcMemoryStatus(CM_GetMpcMemoryStatus_t __user *param)
+{
+	CM_GetMpcMemoryStatus_t data;
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_GetMpcMemoryStatus(data.in.coreId,
+						      data.in.memType,
+						      &data.out.pStatus);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_StartComponent(struct cm_process_priv *procPriv,
+			       CM_StartComponent_t __user *param)
+{
+	CM_StartComponent_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_StartComponent(data.in.client,
+						  procPriv->pid);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_StopComponent(struct cm_process_priv *procPriv,
+			      CM_StopComponent_t __user *param)
+{
+	CM_StopComponent_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_StopComponent(data.in.client,
+						 procPriv->pid);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetMpcLoadCounter(CM_GetMpcLoadCounter_t __user *param)
+{
+	CM_GetMpcLoadCounter_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_getMpcLoadCounter(data.in.coreId,
+						      &data.out.pMpcLoadCounter);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentDescription(struct cm_process_priv *procPriv,
+					CM_GetComponentDescription_t __user *param)
+{
+	CM_GetComponentDescription_t data;
+	char templateName[MAX_TEMPLATE_NAME_LENGTH];
+	char localName[MAX_COMPONENT_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentDescription(data.in.component,
+							   templateName,
+							   data.in.templateNameLength,
+							   &data.out.coreId,
+							   localName,
+							   data.in.localNameLength,
+							   &data.out.priority);
+
+	/* Copy results back to userspace */
+	if (data.out.error == CM_OK) {
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.templateName, templateName, data.in.templateNameLength))
+			return -EFAULT;
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.localName, localName, data.in.localNameLength))
+			return -EFAULT;
+	}
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentListHeader(struct cm_process_priv *procPriv,
+				       CM_GetComponentListHeader_t __user *param)
+{
+	CM_GetComponentListHeader_t data;
+
+	data.out.error = CM_ENGINE_GetComponentListHeader(procPriv->pid,
+							  &data.out.headerComponent);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentListNext(struct cm_process_priv *procPriv,
+				     CM_GetComponentListNext_t __user *param)
+{
+	CM_GetComponentListNext_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentListNext(procPriv->pid,
+							data.in.prevComponent,
+							&data.out.nextComponent);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentRequiredInterfaceNumber(struct cm_process_priv *procPriv,
+						    CM_GetComponentRequiredInterfaceNumber_t __user *param)
+{
+	CM_GetComponentRequiredInterfaceNumber_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentRequiredInterfaceNumber(data.in.component,
+								       &data.out.numberRequiredInterfaces);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentRequiredInterface(struct cm_process_priv *procPriv,
+					      CM_GetComponentRequiredInterface_t __user *param)
+{
+	CM_GetComponentRequiredInterface_t data;
+	char itfName[MAX_INTERFACE_NAME_LENGTH];
+	char itfType[MAX_INTERFACE_TYPE_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentRequiredInterface(data.in.component,
+								 data.in.index,
+								 itfName,
+								 data.in.itfNameLength,
+								 itfType,
+								 data.in.itfTypeLength,
+								 &data.out.requireState,
+								 &data.out.collectionSize);
+
+	/* Copy results back to userspace */
+	if (data.out.error == CM_OK) {
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.itfName, itfName, data.in.itfNameLength))
+			return -EFAULT;
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.itfType, itfType, data.in.itfTypeLength))
+			return -EFAULT;
+	}
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentRequiredInterfaceBinding(struct cm_process_priv *procPriv,
+						     CM_GetComponentRequiredInterfaceBinding_t __user *param)
+{
+	CM_GetComponentRequiredInterfaceBinding_t data;
+	char itfName[MAX_INTERFACE_NAME_LENGTH];
+	char serverItfName[MAX_INTERFACE_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+	if ((data.out.error = copy_string_from_user(itfName,
+						    data.in.itfName,
+						    sizeof(itfName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_GetComponentRequiredInterfaceBinding(data.in.component,
+									itfName,
+									&data.out.server,
+									serverItfName,
+									data.in.serverItfNameLength);
+
+	/* Copy results back to userspace */
+	if (data.out.error != CM_OK)
+		goto out;
+
+	/* coverity[tainted_data : FALSE] */
+	if (copy_to_user(data.in.serverItfName, serverItfName, data.in.serverItfNameLength))
+		return -EFAULT;
+out:
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentProvidedInterfaceNumber(struct cm_process_priv *procPriv,
+						    CM_GetComponentProvidedInterfaceNumber_t __user *param)
+{
+	CM_GetComponentProvidedInterfaceNumber_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentProvidedInterfaceNumber(data.in.component,
+								       &data.out.numberProvidedInterfaces);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentProvidedInterface(struct cm_process_priv *procPriv,
+					      CM_GetComponentProvidedInterface_t __user *param)
+{
+	CM_GetComponentProvidedInterface_t data;
+	char itfName[MAX_INTERFACE_NAME_LENGTH];
+	char itfType[MAX_INTERFACE_TYPE_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentProvidedInterface(data.in.component,
+								 data.in.index,
+								 itfName,
+								 data.in.itfNameLength,
+								 itfType,
+								 data.in.itfTypeLength,
+								 &data.out.collectionSize);
+
+	/* Copy results back to userspace */
+	if (data.out.error == CM_OK) {
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.itfName, itfName, data.in.itfNameLength))
+			return -EFAULT;
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.itfType, itfType, data.in.itfTypeLength))
+			return -EFAULT;
+	}
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentPropertyNumber(struct cm_process_priv *procPriv,
+					   CM_GetComponentPropertyNumber_t __user *param)
+{
+	CM_GetComponentPropertyNumber_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentPropertyNumber(data.in.component,
+							      &data.out.numberProperties);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentPropertyName(struct cm_process_priv *procPriv,
+					 CM_GetComponentPropertyName_t __user *param)
+{
+	CM_GetComponentPropertyName_t data;
+	char propertyName[MAX_PROPERTY_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentPropertyName(data.in.component,
+							    data.in.index,
+							    propertyName,
+							    data.in.propertyNameLength);
+
+	/* Copy results back to userspace */
+	/* coverity[tainted_data : FALSE] */
+	if ((data.out.error == CM_OK) &&
+	    copy_to_user(data.in.propertyName, propertyName, data.in.propertyNameLength))
+		return -EFAULT;
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentPropertyValue(struct cm_process_priv *procPriv,
+					  CM_GetComponentPropertyValue_t __user *param)
+{
+	CM_GetComponentPropertyValue_t data;
+	char propertyName[MAX_PROPERTY_NAME_LENGTH];
+	char propertyValue[MAX_PROPERTY_VALUE_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(propertyName,
+						    data.in.propertyName,
+						    sizeof(propertyName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_GetComponentPropertyValue(data.in.component,
+							     propertyName,
+							     propertyValue,
+							     data.in.propertyValueLength);
+	/* Copy results back to userspace */
+	/* coverity[tainted_data : FALSE] */
+	if ((data.out.error == CM_OK) &&
+	    copy_to_user(data.in.propertyValue, propertyValue, data.in.propertyValueLength))
+		return -EFAULT;
+out:
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_ReadComponentAttribute(struct cm_process_priv *procPriv,
+				       CM_ReadComponentAttribute_t __user *param)
+{
+	CM_ReadComponentAttribute_t data;
+	char attrName[MAX_ATTRIBUTE_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(attrName,
+						    data.in.attrName,
+						    sizeof(attrName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_ReadComponentAttribute(data.in.component,
+							  attrName,
+							  &data.out.value);
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetExecutiveEngineHandle(struct cm_process_priv *procPriv,
+					 CM_GetExecutiveEngineHandle_t __user *param)
+{
+	CM_GetExecutiveEngineHandle_t data;
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_GetExecutiveEngineHandle(data.in.domainId,
+							     &data.out.executiveEngineHandle);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_SetMode(CM_SetMode_t __user *param)
+{
+	CM_SetMode_t data;
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_SetMode(data.in.aCmdID, data.in.aParam);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetRequiredComponentFiles(struct cm_process_priv *procPriv,
+					  CM_GetRequiredComponentFiles_t __user *param)
+{
+	CM_GetRequiredComponentFiles_t data;
+	char components[4][MAX_INTERFACE_TYPE_NAME_LENGTH];
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+	char providedItfServerName[MAX_INTERFACE_NAME_LENGTH];
+	char type[MAX_INTERFACE_TYPE_NAME_LENGTH];
+	unsigned int i;
+	int err;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if (data.in.requiredItfClientName &&
+	    (data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	if (data.in.providedItfServerName &&
+	    (data.out.error = copy_string_from_user(providedItfServerName,
+						    data.in.providedItfServerName,
+						    sizeof(providedItfServerName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_GetRequiredComponentFiles(data.in.action,
+							     data.in.client,
+							     requiredItfClientName,
+							     data.in.server,
+							     providedItfServerName,
+							     components,
+							     data.in.listSize,
+							     data.in.type ? type : NULL,
+							     &data.out.methodNumber);
+
+	if (data.out.error)
+		goto out;
+
+	if (data.in.fileList) {
+		/* Copy results back to userspace */
+		for (i=0; i<data.in.listSize; i++) {
+			err = copy_to_user(&((char*)data.in.fileList)[i*MAX_INTERFACE_TYPE_NAME_LENGTH], components[i], MAX_INTERFACE_TYPE_NAME_LENGTH);
+			if (err)
+				return -EFAULT;
+		}
+	}
+	if (data.in.type
+	    && copy_to_user(data.in.type, type, MAX_INTERFACE_TYPE_NAME_LENGTH))
+			return -EFAULT;
+out:
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_Migrate(CM_Migrate_t __user *param)
+{
+	CM_Migrate_t data;
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_Migrate(data.in.srcShared, data.in.src, data.in.dst);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_Unmigrate(CM_Unmigrate_t __user *param)
+{
+	CM_Unmigrate_t data;
+
+	data.out.error = CM_ENGINE_Unmigrate();
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+int cmld_SetupRelinkArea(CM_SetupRelinkArea_t __user *param)
+{
+	CM_SetupRelinkArea_t data;
+	t_cm_system_address systemAddress;
+	t_uint32 size;
+
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+
+	data.out.error = CM_ENGINE_GetMpcMemorySystemAddress(
+			data.in.mem_handle, &systemAddress);
+
+	if (data.out.error != CM_OK)
+		return 0;
+
+	data.out.error = CM_ENGINE_GetMpcMemorySize(data.in.mem_handle, &size);
+	if (data.out.error != CM_OK)
+		return 0;
+
+	if (size < data.in.segments * data.in.segmentsize)
+	{
+		return -EINVAL;
+	}
+
+	data.out.error = cmdma_setup_relink_area(
+				systemAddress.physical,
+				data.in.peripheral_addr,
+				data.in.segments,
+				data.in.segmentsize,
+				data.in.LOS,
+				data.in.type);
+
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+
+	return 0;
+}
+
+
+inline int cmld_PushComponent(CM_PushComponent_t __user *param)
+{
+	CM_PushComponent_t data;
+	char name[MAX_INTERFACE_TYPE_NAME_LENGTH];
+	void *dataFile = NULL;
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(name,
+						    data.in.name,
+						    sizeof(name))))
+		goto out;
+
+	if (data.in.data != NULL) {
+		dataFile = OSAL_Alloc(data.in.size);
+		if (dataFile == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFile, data.in.data, data.in.size))
+			data.out.error = CM_INVALID_PARAMETER;
+		else
+			data.out.error = CM_ENGINE_PushComponent(name, dataFile,
+								 data.in.size);
+		OSAL_Free(dataFile);
+	}
+
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_ReleaseComponent(CM_ReleaseComponent_t __user *param)
+{
+	CM_ReleaseComponent_t data;
+	char name[MAX_INTERFACE_TYPE_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(name,
+						    data.in.name,
+						    sizeof(name))))
+		goto out;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_ReleaseComponent(name);
+
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_PrivGetMPCMemoryDesc(struct cm_process_priv *procPriv, CM_PrivGetMPCMemoryDesc_t __user *param)
+{
+	CM_PrivGetMPCMemoryDesc_t data;
+	struct list_head* cursor;
+
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if (lock_process(procPriv))
+		return -ERESTARTSYS;
+	/* Scan the memory descriptors list looking for the requested handle */
+	data.out.error = CM_UNKNOWN_MEMORY_HANDLE;
+	list_for_each(cursor, &procPriv->memAreaDescList) {
+		struct memAreaDesc_t* curr;
+		curr = list_entry(cursor, struct memAreaDesc_t, list);
+		if (curr->handle == data.in.handle) {
+			data.out.size = curr->size;
+			data.out.physAddr = curr->physAddr;
+			data.out.kernelLogicalAddr = curr->kernelLogicalAddr;
+			data.out.userLogicalAddr = curr->userLogicalAddr;
+			data.out.mpcPhysAddr = curr->mpcPhysAddr;
+			data.out.error = CM_OK;
+			break;
+		}
+	}
+	unlock_process(procPriv);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+int cmld_PrivReserveMemory(struct cm_process_priv *procPriv, unsigned int physAddr)
+{
+	struct list_head* cursor;
+	struct memAreaDesc_t* curr;
+	int err = -ENXIO;
+
+	if (lock_process(procPriv))
+		return -ERESTARTSYS;
+	list_for_each(cursor, &procPriv->memAreaDescList) {
+		curr = list_entry(cursor, struct memAreaDesc_t, list);
+		if (curr->physAddr == physAddr) {
+			/* Mark this memory area reserved for a mapping for this thread ID */
+			/* It must not be already reserved but this should not happen */
+			if (curr->tid) {
+				/*pr_err("%s: thread %d can't reseveved memory %x already "
+				       "reserved for %d\n",
+				       __func__, current->pid, physAddr, (int)curr->tid);*/
+				err = -EBUSY;
+			} else {
+				curr->tid = 0;
+				//current->pid;
+				err = 0;
+			}
+			break;
+		}
+	}
+	unlock_process(procPriv);
+	return err;
+}
diff --git a/drivers/staging/nmf-cm/cmioctl.h b/drivers/staging/nmf-cm/cmioctl.h
new file mode 100644
index 00000000000..7ca46fc25e8
--- /dev/null
+++ b/drivers/staging/nmf-cm/cmioctl.h
@@ -0,0 +1,588 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+
+#ifndef __CMIOCTL_H
+#define __CMIOCTL_H
+
+#include <cm/engine/component/inc/component_type.h>
+#include <cm/engine/communication/inc/communication_type.h>
+#include <cm/engine/configuration/inc/configuration_type.h>
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/memory/inc/memory_type.h>
+#include <cm/engine/perfmeter/inc/perfmeter_type.h>
+#include <cm/engine/repository_mgt/inc/repository_type.h>
+#include "cm_dma.h"
+
+#define CMLD_DEV_NAME \
+	{ "cm_control", \
+	  "cm_channel"	\
+	}
+
+/*
+ * The following structures are used to exchange CM_SYSCALL parameters with
+ * the driver. There is one structure per ioctl command, ie per CM_SYSCAL.
+ * Each of them contains:
+ *  - One set of fields placed in a struture 'in' which are all input
+ *    parameters of the syscall (parameters that kernel side must retrieve
+ *    from user space)
+ *  - One set of fields placed in a struture 'out' which contains all output
+ *    parameters of the syscall plus the error code.
+ *
+ * NOTE: all pointers to (user) buffer are always placed in struct 'in', including
+ * buffers used as output parameters; because the pointer itself is considered as
+ * an input parameter, as it is directly accessed from kernel space.
+ */
+typedef struct{
+	struct {
+		const char * templateName;
+		t_cm_domain_id domainId;
+		t_nmf_ee_priority priority;
+		const char * localName;
+		const char *dataFile;
+		t_uint32 dataFileSize;
+	} in;
+	struct {
+		t_cm_instance_handle component; /** < Output parameter */
+		t_cm_error error;
+	} out;
+} CM_InstantiateComponent_t;
+
+typedef struct {
+	struct {
+		t_cm_bf_host2mpc_handle host2mpcId;
+		t_event_params_handle h;
+		t_uint32 size;
+		t_uint32 methodIndex;
+	} in;
+	struct {
+		t_cm_error error;
+	} out;
+} CM_PushEventWithSize_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle server;
+		const char * providedItfServerName;
+		t_uint32 fifosize;
+		t_cm_mpc_memory_type eventMemType;
+		const char *dataFileSkeleton;
+		t_uint32 dataFileSkeletonSize;
+	} in;
+	struct {
+		t_cm_bf_host2mpc_handle host2mpcId;  /** < Output parameter */
+		t_cm_error error;
+	} out;
+} CM_BindComponentFromCMCore_t;
+
+typedef struct {
+	struct {
+		t_cm_bf_host2mpc_handle host2mpcId;
+	} in;
+	struct {
+		t_cm_error error;
+	} out;
+} CM_UnbindComponentFromCMCore_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char *requiredItfClientName;
+		t_uint32 fifosize;
+		t_nmf_mpc2host_handle upLayerThis;
+		const char *dataFileStub;
+		t_uint32 dataFileStubSize;
+	} in;
+	struct {
+		t_cm_bf_mpc2host_handle mpc2hostId; /** < Output parameter */
+		t_cm_error error;
+	} out;
+} CM_BindComponentToCMCore_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char *requiredItfClientName;
+	} in;
+	struct {
+		t_nmf_mpc2host_handle upLayerThis;  /** < Output parameter */
+		t_cm_error error;
+	} out;
+} CM_UnbindComponentToCMCore_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+	} in;
+	struct {
+		t_cm_error error;  /** < Output parameter */
+	} out;  /** < Output parameter */
+} CM_DestroyComponent_t;
+
+typedef struct {
+	struct {
+		const t_cm_domain_memory  *domain;
+		t_nmf_client_id            client;
+	} in;
+	struct {
+		t_cm_domain_id handle; /** < Out parameter */
+		t_cm_error error;  /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_CreateMemoryDomain_t;
+
+typedef struct {
+	struct {
+		t_cm_domain_id parentId;
+		const t_cm_domain_memory *domain;
+	} in;
+	struct {
+		t_cm_domain_id handle; /** < Out parameter */
+		t_cm_error error;  /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_CreateMemoryDomainScratch_t;
+
+typedef struct {
+	struct {
+		t_cm_domain_id domainId;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_DestroyMemoryDomain_t;
+
+typedef struct {
+	struct {
+		t_cm_domain_id domainId; /** < In parameter */
+	} in;
+	struct {
+		t_nmf_core_id coreId; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_GetDomainCoreId_t;
+
+typedef struct {
+	struct {
+		t_cm_domain_id domainId;
+		t_cm_mpc_memory_type memType;
+		t_cm_size size;
+		t_cm_memory_alignment memAlignment;
+	} in;
+	struct {
+		t_cm_memory_handle pHandle; /** < Output parameter */
+		t_cm_error error;
+	} out;  /** < Output parameter */
+} CM_AllocMpcMemory_t;
+
+typedef struct{
+	struct {
+		t_cm_memory_handle handle;
+	} in;
+	struct {
+		t_cm_error error;
+	} out;  /** < Output parameter */
+} CM_FreeMpcMemory_t;
+
+typedef struct {
+	struct {
+		t_cm_memory_handle handle;
+	} in;
+	struct {
+		t_uint32 size; /** < Out parameter */
+		t_uint32 physAddr; /** < Out parameter */
+		t_uint32 kernelLogicalAddr; /** < Out parameter */
+		t_uint32 userLogicalAddr; /** < Out parameter */
+		t_uint32 mpcPhysAddr; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;  /** < Output parameter */
+} CM_PrivGetMPCMemoryDesc_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char *requiredItfClientName;
+		t_cm_instance_handle server;
+		const char *providedItfServerName;
+		t_uint32 fifosize;
+		t_cm_mpc_memory_type eventMemType;
+		const char *dataFileSkeletonOrEvent;
+		t_uint32 dataFileSkeletonOrEventSize;
+		const char *dataFileStub;
+		t_uint32 dataFileStubSize;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_BindComponentAsynchronous_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char* requiredItfClientName;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_UnbindComponentAsynchronous_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char *requiredItfClientName;
+		t_cm_instance_handle server;
+		const char *providedItfServerName;
+		t_bool traced;
+		const char *dataFileTrace;
+		t_uint32 dataFileTraceSize;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_BindComponent_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char* requiredItfClientName;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_UnbindComponent_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char* requiredItfClientName;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_BindComponentToVoid_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_StartComponent_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_StopComponent_t;
+
+typedef struct {
+	struct {
+		t_nmf_core_id coreId;
+	} in;
+	struct {
+		t_cm_mpc_load_counter pMpcLoadCounter; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetMpcLoadCounter_t;
+
+typedef struct {
+	struct {
+		t_nmf_core_id coreId;
+		t_cm_mpc_memory_type memType;
+	} in;
+	struct {
+		t_cm_allocator_status pStatus; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_GetMpcMemoryStatus_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		t_uint32 templateNameLength;
+		t_uint32 localNameLength;
+		char *templateName; /** < Out parameter */
+		char *localName; /** < Out parameter */
+	} in;
+	struct {
+		t_nmf_core_id coreId; /** < Out parameter */
+		t_nmf_ee_priority priority; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_GetComponentDescription_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle headerComponent; /** < Output parameter */
+		t_cm_error error; /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_GetComponentListHeader_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle prevComponent;
+	} in;
+	struct {
+		t_cm_instance_handle nextComponent; /** < Output parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentListNext_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+	} in;
+	struct {
+		t_uint8 numberRequiredInterfaces; /** < Output parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentRequiredInterfaceNumber_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		t_uint8 index;
+		t_uint32 itfNameLength;
+		t_uint32 itfTypeLength;
+		char *itfName; /** < Out parameter */
+		char *itfType; /** < Out parameter */
+	} in;
+	struct {
+		t_cm_require_state requireState; /** < Out parameter */
+		t_sint16 collectionSize; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentRequiredInterface_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		const char *itfName;
+		t_uint32 serverItfNameLength;
+		char *serverItfName; /** < Out parameter */
+	} in;
+	struct {
+		t_cm_instance_handle server; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentRequiredInterfaceBinding_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+	} in;
+	struct {
+		t_uint8 numberProvidedInterfaces; /** < Output parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentProvidedInterfaceNumber_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		t_uint8 index;
+		t_uint32 itfNameLength;
+		t_uint32 itfTypeLength;
+		char *itfName; /** < Out parameter */
+		char *itfType; /** < Out parameter */
+	} in;
+	struct {
+		t_sint16 collectionSize; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentProvidedInterface_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+	} in;
+	struct {
+		t_uint8 numberProperties; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentPropertyNumber_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		const char *attrName;
+		t_uint8 index;
+		t_uint32 propertyNameLength;
+		char *propertyName; /** < Out parameter */
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentPropertyName_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		const char *propertyName;
+		t_uint32 propertyValueLength;
+		char *propertyValue; /** < Out parameter */
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentPropertyValue_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		const char *attrName;
+	} in;
+	struct {
+		t_uint32 value; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_ReadComponentAttribute_t;
+
+typedef struct {
+	struct {
+		t_cm_domain_id domainId;
+	} in;
+	struct {
+		t_cm_instance_handle executiveEngineHandle;
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetExecutiveEngineHandle_t;
+
+typedef struct {
+	struct {
+		t_cm_cmd_id aCmdID;
+		t_sint32 aParam;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_SetMode_t;
+
+typedef struct {
+	struct {
+		t_action_to_do action;
+		t_cm_instance_handle client;
+		const char *requiredItfClientName;
+		t_cm_instance_handle server;
+		const char *providedItfServerName;
+		char **fileList;
+		unsigned int listSize;
+		char *type;
+	} in;
+	struct {
+		t_uint32 methodNumber; /** < Output parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetRequiredComponentFiles_t;
+
+typedef struct {
+	struct {
+		const char *name;
+		const void *data;
+		t_cm_size size;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_PushComponent_t;
+
+typedef struct {
+	struct {
+		const char *name;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_ReleaseComponent_t;
+
+typedef struct {
+	struct {
+		t_cm_domain_id srcShared;
+		t_cm_domain_id src;
+		t_cm_domain_id dst;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_Migrate_t;
+
+typedef struct {
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_Unmigrate_t;
+
+typedef struct{
+    struct {
+        t_cm_memory_handle mem_handle;
+        unsigned int peripheral_addr;
+        unsigned int segments;
+        unsigned int segmentsize;
+        unsigned int LOS;
+        enum cmdma_type type;
+    } in;
+    struct {
+        t_cm_error error;
+    } out;
+} CM_SetupRelinkArea_t;
+
+#define CM_PUSHEVENTWITHSIZE 			_IOWR('c', 0, CM_PushEventWithSize_t)
+#define CM_GETVERSION 				_IOR('c', 1, t_uint32)
+#define CM_INSTANTIATECOMPONENT 		_IOWR('c', 2, CM_InstantiateComponent_t)
+#define CM_BINDCOMPONENTFROMCMCORE 		_IOWR('c', 3, CM_BindComponentFromCMCore_t)
+#define CM_UNBINDCOMPONENTFROMCMCORE 		_IOWR('c', 4, CM_UnbindComponentFromCMCore_t)
+#define CM_BINDCOMPONENTTOCMCORE 		_IOWR('c', 5, CM_BindComponentToCMCore_t)
+#define CM_UNBINDCOMPONENTTOCMCORE 		_IOWR('c', 6, CM_UnbindComponentToCMCore_t)
+#define CM_DESTROYCOMPONENT 			_IOWR('c', 7, CM_DestroyComponent_t)
+#define CM_CREATEMEMORYDOMAIN 			_IOWR('c', 8, CM_CreateMemoryDomain_t)
+#define CM_CREATEMEMORYDOMAINSCRATCH 		_IOWR('c', 9, CM_CreateMemoryDomainScratch_t)
+#define CM_DESTROYMEMORYDOMAIN 			_IOWR('c', 10, CM_DestroyMemoryDomain_t)
+#define CM_GETDOMAINCOREID 			_IOWR('c', 11, CM_GetDomainCoreId_t)
+#define CM_ALLOCMPCMEMORY 			_IOWR('c', 12, CM_AllocMpcMemory_t)
+#define CM_FREEMPCMEMORY 			_IOWR('c', 13, CM_FreeMpcMemory_t)
+#define CM_BINDCOMPONENTASYNCHRONOUS 		_IOWR('c', 14, CM_BindComponentAsynchronous_t)
+#define CM_UNBINDCOMPONENTASYNCHRONOUS 		_IOWR('c', 15, CM_UnbindComponentAsynchronous_t)
+#define CM_BINDCOMPONENT 			_IOWR('c', 16, CM_BindComponent_t)
+#define CM_UNBINDCOMPONENT 			_IOWR('c', 17, CM_UnbindComponent_t)
+#define CM_BINDCOMPONENTTOVOID 			_IOWR('c', 18, CM_BindComponentToVoid_t)
+#define CM_STARTCOMPONENT 			_IOWR('c', 19, CM_StartComponent_t)
+#define CM_STOPCOMPONENT 			_IOWR('c', 20, CM_StopComponent_t)
+#define CM_GETMPCLOADCOUNTER 			_IOWR('c', 21, CM_GetMpcLoadCounter_t)
+#define CM_GETMPCMEMORYSTATUS 			_IOWR('c', 22, CM_GetMpcMemoryStatus_t)
+#define CM_GETCOMPONENTDESCRIPTION 		_IOWR('c', 23, CM_GetComponentDescription_t)
+#define CM_GETCOMPONENTLISTHEADER 		_IOWR('c', 24, CM_GetComponentListHeader_t)
+#define CM_GETCOMPONENTLISTNEXT 		_IOWR('c', 25, CM_GetComponentListNext_t)
+#define CM_GETCOMPONENTREQUIREDINTERFACENUMBER  _IOWR('c', 26, CM_GetComponentRequiredInterfaceNumber_t)
+#define CM_GETCOMPONENTREQUIREDINTERFACE        _IOWR('c', 27, CM_GetComponentRequiredInterface_t)
+#define CM_GETCOMPONENTREQUIREDINTERFACEBINDING _IOWR('c', 28, CM_GetComponentRequiredInterfaceBinding_t)
+#define CM_GETCOMPONENTPROVIDEDINTERFACENUMBER 	_IOWR('c', 29, CM_GetComponentProvidedInterfaceNumber_t)
+#define CM_GETCOMPONENTPROVIDEDINTERFACE 	_IOWR('c', 30, CM_GetComponentProvidedInterface_t)
+#define CM_GETCOMPONENTPROPERTYNUMBER 		_IOWR('c', 31, CM_GetComponentPropertyNumber_t)
+#define CM_GETCOMPONENTPROPERTYNAME 		_IOWR('c', 32, CM_GetComponentPropertyName_t)
+#define CM_GETCOMPONENTPROPERTYVALUE 		_IOWR('c', 33, CM_GetComponentPropertyValue_t)
+#define CM_READCOMPONENTATTRIBUTE 		_IOWR('c', 34, CM_ReadComponentAttribute_t)
+#define CM_GETEXECUTIVEENGINEHANDLE 		_IOWR('c', 35, CM_GetExecutiveEngineHandle_t)
+#define CM_SETMODE 				_IOWR('c', 36, CM_SetMode_t)
+#define CM_GETREQUIREDCOMPONENTFILES 		_IOWR('c', 37, CM_GetRequiredComponentFiles_t)
+#define CM_PUSHCOMPONENT 			_IOWR('c', 38, CM_PushComponent_t)
+#define CM_FLUSHCHANNEL 			_IO('c', 39)
+#define CM_MIGRATE 				_IOWR('c', 40, CM_Migrate_t)
+#define CM_UNMIGRATE 				_IOR('c', 41, CM_Unmigrate_t)
+#define CM_RELEASECOMPONENT 			_IOWR('c', 42, CM_ReleaseComponent_t)
+#define CM_SETUPRELINKAREA             		_IOWR('c', 43, CM_SetupRelinkArea_t)
+
+#define CM_PRIVGETMPCMEMORYDESC			_IOWR('c', 100, CM_PrivGetMPCMemoryDesc_t)
+#define CM_PRIVRESERVEMEMORY 			_IOW('c', 101, unsigned int)
+#define CM_PRIV_GETBOARDVERSION 		_IOR('c', 102, unsigned int)
+
+enum board_version {
+	U8500_V1,
+	U8500_V2
+};
+#endif
diff --git a/drivers/staging/nmf-cm/cmld.c b/drivers/staging/nmf-cm/cmld.c
new file mode 100644
index 00000000000..a89fe1ef70f
--- /dev/null
+++ b/drivers/staging/nmf-cm/cmld.c
@@ -0,0 +1,1318 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/** \file cmld.c
+ *
+ * Nomadik Multiprocessing Framework Linux Driver
+ *
+ */
+
+#include <linux/cdev.h>
+#include <linux/fs.h>
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/sched.h>
+
+#include <cm/inc/cm_def.h>
+#include <cm/engine/api/cm_engine.h>
+#include <cm/engine/api/control/irq_engine.h>
+
+#include "cmioctl.h"
+#include "osal-kernel.h"
+#include "cmld.h"
+#include "cm_service.h"
+
+#define CMDRIVER_PATCH_VERSION 106
+#define O_FLUSH 0x1000000
+
+static int cmld_major;
+static struct cdev cmld_cdev;
+static struct class cmld_class = {
+	.name = "cm",
+	.owner = THIS_MODULE,
+};
+const char *cmld_devname[] = CMLD_DEV_NAME;
+static struct device *cmld_dev[ARRAY_SIZE(cmld_devname)];
+
+/* List of per process structure (struct cm_process_priv list) */
+LIST_HEAD(process_list);
+static DEFINE_MUTEX(process_lock); /* lock used to protect previous list */
+/* List of per channel structure (struct cm_channel_priv list).
+   A channel == One file descriptor */
+LIST_HEAD(channel_list);
+static DEFINE_MUTEX(channel_lock); /* lock used to protect previous list */
+
+/* Variables used to manage temporary allocation of ESRAM
+   reserved for DMA and B2R2 + MCDE */
+/* As of now, the reservation is done in RME */
+static int cfgESRAM_ReserveMCDE = 0;
+static int cfgESRAM_ReserveDMA  = 0;
+static int cfgESRAMDmaSize      = 4;   /* in Kb */
+static int cfgESRAMMcdeSize     = 128; /* in Kb */
+module_param(cfgESRAM_ReserveMCDE, bool, S_IRUGO);
+module_param(cfgESRAM_ReserveDMA, bool, S_IRUGO);
+module_param(cfgESRAMDmaSize, uint, S_IRUGO);
+module_param(cfgESRAMMcdeSize, uint, S_IRUGO);
+static t_cm_domain_id dmaDomainId, mcdeDomainId;
+static t_cm_memory_handle dmaMemoryHdl, mcdeMemoryHdl;
+
+static inline struct cm_process_priv *getProcessPriv(void)
+{
+	struct list_head* head;
+	struct cm_process_priv *entry;
+
+	mutex_lock(&process_lock);
+
+	/* Look for an entry for the calling process */
+	list_for_each(head, &process_list) {
+		entry = list_entry(head, struct cm_process_priv, entry);
+		if (entry->pid == current->tgid) {
+			kref_get(&entry->ref);
+			goto out;
+		}
+	}
+	mutex_unlock(&process_lock);
+
+	/* Allocate, init and register a new one otherwise */
+	entry = OSAL_Alloc(sizeof(*entry));
+	if (entry == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	/* init host2mpcLock */
+	mutex_init(&entry->host2mpcLock);
+
+	INIT_LIST_HEAD(&entry->memAreaDescList);
+	kref_init(&entry->ref);
+	mutex_init(&entry->mutex);
+
+	entry->pid = current->tgid;
+	mutex_lock(&process_lock);
+	list_add(&entry->entry, &process_list);
+out:
+	mutex_unlock(&process_lock);
+	return entry;
+}
+
+/* Free all messages */
+static inline void freeMessages(struct cm_channel_priv* channelPriv)
+{
+	struct osal_msg *this, *next;
+	int warn = 0;
+
+	spin_lock_bh(&channelPriv->bh_lock);
+	plist_for_each_entry_safe(this, next, &channelPriv->messageQueue, msg_entry) {
+		plist_del(&this->msg_entry, &channelPriv->messageQueue);
+		kfree(this);
+		warn = 1;
+	}
+	spin_unlock_bh(&channelPriv->bh_lock);
+	if (warn) {
+		pr_err("[CM - PID=%d]: Some remaining"
+		       " message(s) freed\n", current->tgid);
+		warn = 0;
+	}
+}
+
+/* Free all pending memory areas and relative descriptors  */
+static inline void freeMemHandles(struct cm_process_priv* processPriv)
+{
+	struct list_head* head, *next;
+	int warn = 0;
+			
+	list_for_each_safe(head, next, &processPriv->memAreaDescList) {
+		struct memAreaDesc_t* curr;
+		int err;
+		curr = list_entry(head, struct memAreaDesc_t, list);
+		err=CM_ENGINE_FreeMpcMemory(curr->handle);
+		if (err)
+			pr_err("[CM - PID=%d]: Error (%d) freeing remaining memory area "
+			       "handle\n", current->tgid, err);
+		list_del(head);
+		OSAL_Free(curr);
+		warn = 1;
+	}
+	if (warn) {
+		pr_err("[CM - PID=%d]: Some remaining memory area "
+		       "handle(s) freed\n", current->tgid);
+		warn = 0;
+	}
+}
+
+/* Free any skeleton, called when freeing the process entry */
+static inline void freeSkelList(struct list_head* skelList)
+{
+	struct list_head* head, *next;
+	int warn = 0;
+
+	/* No lock held, we know that we are the only and the last user
+	   of the list */
+	list_for_each_safe(head, next, skelList) {
+		t_skelwrapper* curr;
+		curr = list_entry(head, t_skelwrapper, entry);
+		list_del(head);
+		OSAL_Free(curr);
+		warn = 1;
+	}
+	if (warn)
+		pr_err("[CM - PID=%d]: Some remaining skeleton "
+		       "wrapper(s) freed\n", current->tgid);
+}
+
+/* Free any remaining channels belonging to this process */
+/* Called _only_ when freeing the process entry, once the network constructed by
+   this process has been destroyed.
+   See cmld_release() to see why there can be some remaining non-freed channels */
+static inline void freeChannels(struct cm_process_priv* processPriv)
+{
+	struct list_head* head, *next;
+	int warn = 0;
+			
+	list_for_each_safe(head, next, &channel_list) {
+		struct cm_channel_priv *channelPriv;
+		channelPriv = list_entry(head, struct cm_channel_priv, entry);
+		/* Only channels belonging to this process are concerned */
+		if (channelPriv->proc == processPriv) {
+			tasklet_disable(&cmld_service_tasklet);
+			mutex_lock(&channel_lock);
+			list_del(&channelPriv->entry);
+			mutex_unlock(&channel_lock);
+			tasklet_enable(&cmld_service_tasklet);
+
+			/* Free all remaining messages if any
+			   (normally none, but double check) */
+			freeMessages(channelPriv);
+
+			/* Free any pending skeleton wrapper */
+			/* Here it's safe, we know that all bindings have been undone */
+			freeSkelList(&channelPriv->skelList);
+
+			/* Free the per-channel descriptor */
+			OSAL_Free(channelPriv);
+		}
+		warn = 1;
+	}
+	if (warn)
+		pr_err("[CM - PID=%d]: Some remaining channel entries "
+		       "freed\n", current->tgid);
+}
+
+/* Free the process priv structure and all related stuff */
+/* Called only when the last ref to this structure is released */
+static void freeProcessPriv(struct kref *ref)
+{
+	struct cm_process_priv *entry = container_of(ref, struct cm_process_priv, ref);
+	t_nmf_error err;
+
+	mutex_lock(&process_lock);
+	list_del(&entry->entry);
+	mutex_unlock(&process_lock);
+
+	/* Destroy all remaining components */
+	err=CM_ENGINE_FlushComponents(entry->pid);
+	if (err != NMF_OK)
+		pr_err("[CM - PID=%d]: Error while flushing some remaining"
+		       " components: error=%d\n", current->tgid, err);
+
+	freeChannels(entry);
+
+	/* Free any pending memory areas and relative descriptors */
+	freeMemHandles(entry);
+
+	/* Destroy all remaining domains */
+	err=CM_ENGINE_FlushMemoryDomains(entry->pid);
+	if (err != NMF_OK)
+		pr_err("[CM - PID=%d]: Error while flushing some remaining"
+		       " domains: error=%d\n", current->tgid, err);
+
+	/* Free the per-process descriptor */
+	OSAL_Free(entry);
+}
+
+/** Driver's open method
+ * Allocates per-process resources: private data, wait queue,
+ * memory area descriptors linked list, message queue.
+ *
+ * \return POSIX error code
+ */
+static int cmld_open(struct inode *inode, struct file *file)
+{
+	struct cm_process_priv *procPriv = getProcessPriv();
+
+	if (IS_ERR(procPriv))
+		return PTR_ERR(procPriv);		
+
+	if (iminor(inode) == 0)
+		file->private_data = procPriv;
+	else {
+		struct cm_channel_priv *channelPriv = (struct cm_channel_priv*)OSAL_Alloc(sizeof(*channelPriv));
+		if (channelPriv == NULL) {
+			kref_put(&procPriv->ref, freeProcessPriv);
+			return -ENOMEM;
+		}
+
+		channelPriv->proc = procPriv;
+		channelPriv->state = CHANNEL_OPEN;
+
+		/* Initialize wait_queue, lists and mutexes */
+		init_waitqueue_head(&channelPriv->waitq);
+		plist_head_init(&channelPriv->messageQueue);
+		INIT_LIST_HEAD(&channelPriv->skelList);
+		spin_lock_init(&channelPriv->bh_lock);
+		mutex_init(&channelPriv->msgQueueLock);
+		mutex_init(&channelPriv->skelListLock);
+
+		tasklet_disable(&cmld_service_tasklet);
+		mutex_lock(&channel_lock);
+		list_add(&channelPriv->entry, &channel_list);
+		mutex_unlock(&channel_lock);
+		tasklet_enable(&cmld_service_tasklet);
+
+		file->private_data = channelPriv; // store channel private struct in file descriptor
+	}
+	return 0;
+}
+
+/** Driver's release method.
+ * Frees any per-process pending resource: components, bindings, memory areas.
+ *
+ * \return POSIX error code
+ */
+static int cmld_release(struct inode *inode, struct file *file)
+{
+	struct cm_process_priv* procPriv;
+
+	BUG_ON(file->private_data == NULL);
+	
+	/* The driver must guarantee that all related resources are released.
+	   Thus all these checks below are necessary to release all remaining
+	   resources still linked to this 'client', in case of abnormal process
+	   exit.
+	   => These are error cases !
+	   In the usual case, nothing should be done except the free of
+	   the cmPriv itself
+	*/
+
+	if (iminor(inode) != 0) {
+		struct cm_channel_priv* channelPriv;
+		channelPriv = file->private_data;
+		procPriv = channelPriv->proc;
+
+		/* We don't need to synchronize here by using the skelListLock:
+		   the list is only accessed during ioctl() and we can't be here
+		   if an ioctl() is on-going */		   
+		if (list_empty(&channelPriv->skelList)) {
+			/* There is no pending MPC->HOST binding
+			   => we can quietly delete the channel */
+			tasklet_disable(&cmld_service_tasklet);
+			mutex_lock(&channel_lock);
+			list_del(&channelPriv->entry);
+			mutex_unlock(&channel_lock);
+			tasklet_enable(&cmld_service_tasklet);
+
+			/* Free all remaining messages if any */
+			freeMessages(channelPriv);
+
+			/* Free the per-channel descriptor */
+			OSAL_Free(channelPriv);
+		} else {
+			/* Uh: there are still some MPC->HOST binding but we don't have the
+			   required info to unbind them.
+			   => we must keep all skel structures because possibly used in OSAL_PostDfc
+			   (incoming callback msg) */
+			/* We flag the channel as closed to discard any new msg that will never be read anyway */
+			channelPriv->state = CHANNEL_CLOSED;
+
+			/* Already Free all remaining messages if any,
+			   they will never be read anyway */
+			freeMessages(channelPriv);
+		}
+	} else
+		procPriv = file->private_data;
+
+	kref_put(&procPriv->ref, freeProcessPriv);
+	file->private_data = NULL;				
+
+	return 0;
+}
+
+/** Reads Component Manager messages destinated to this process.
+ * The message is composed by three fields:
+ * 1) mpc2host handle (distinguishes interfaces)
+ * 2) methodIndex (distinguishes interface's methods)
+ * 3) Variable length parameters (method's parameters values)
+ *
+ * \note cfr GetEvent()
+ * \return POSIX error code
+ */
+static ssize_t cmld_read(struct file *file, char *buf, size_t count, loff_t *ppos)
+{
+	int err = 0;
+	struct cm_channel_priv* channelPriv = (struct cm_channel_priv*)(file->private_data);
+	int msgSize = 0;
+	struct plist_head* messageQueue;
+	struct osal_msg* msg;
+	t_os_message *os_msg = (t_os_message *)buf;
+	int block = !(file->f_flags & O_NONBLOCK);
+
+	if (iminor(file->f_dentry->d_inode) == 0)
+		return -ENOSYS;
+	
+	BUG_ON(channelPriv == NULL);
+	messageQueue = &channelPriv->messageQueue;
+ 
+	if (mutex_lock_killable(&channelPriv->msgQueueLock))
+		return -ERESTARTSYS;
+wait:
+	while (plist_head_empty(messageQueue)) {
+		mutex_unlock(&channelPriv->msgQueueLock);
+		if (block == 0)
+			return -EAGAIN;
+		/* Wait until there is a message to ferry up */
+		if (wait_event_interruptible(channelPriv->waitq, ((!plist_head_empty(messageQueue)) || (file->f_flags & O_FLUSH))))
+			return -ERESTARTSYS;
+		if (file->f_flags & O_FLUSH) {
+			file->f_flags &= ~O_FLUSH;
+			return 0;
+		}
+		if (mutex_lock_killable(&channelPriv->msgQueueLock))
+			return -ERESTARTSYS;
+	}
+
+	/* Pick up the first message from the queue, making sure that the
+	 * hwsem tasklet does not wreak havoc the queue in the meantime
+	 */
+	spin_lock_bh(&channelPriv->bh_lock);
+	msg = plist_first_entry(messageQueue, struct osal_msg, msg_entry);
+	plist_del(&msg->msg_entry, messageQueue);
+	spin_unlock_bh(&channelPriv->bh_lock);
+
+	switch (msg->msg_type) {
+	case MSG_INTERFACE: {
+
+		/* Check if enough space is available */
+		msgSize = sizeof(msg->msg_type) + msg->d.itf.ptrSize + sizeof(os_msg->data.itf) - sizeof(os_msg->data.itf.params) ;
+		if (msgSize > count) {
+			mutex_unlock(&channelPriv->msgQueueLock);
+			pr_err("CM: message size bigger than buffer size silently ignored!\n");
+			err = -EMSGSIZE;
+			goto out;
+		}
+
+		/* Copy to user message type */
+		err = put_user(msg->msg_type, &os_msg->type);
+		if (err) goto ack_evt;
+	
+		/* Copy to user the t_nmf_mpc2host_handle */
+		err = put_user(msg->d.itf.skelwrap->upperLayerThis, &os_msg->data.itf.THIS);
+		if (err) goto ack_evt;
+
+		/* The methodIndex  */
+		err = put_user(msg->d.itf.methodIdx, &os_msg->data.itf.methodIndex);
+		if (err) goto ack_evt;
+
+		/* And the parameters */
+		err = copy_to_user(os_msg->data.itf.params, msg->d.itf.anyPtr, msg->d.itf.ptrSize);
+	
+		ack_evt:
+		/* This call is void */
+		/* Note: that we cannot release the lock before having called this function
+		   as acknowledgements MUST be executed in the same order as their
+		   respective messages have arrived! */
+		CM_ENGINE_AcknowledgeEvent(msg->d.itf.skelwrap->mpc2hostId);
+
+		mutex_unlock(&channelPriv->msgQueueLock);
+		break;
+	}
+	case MSG_SERVICE: {
+		mutex_unlock(&channelPriv->msgQueueLock);
+		msgSize = sizeof(msg->msg_type) + sizeof(msg->d.srv.srvType)
+			+ sizeof(msg->d.srv.srvData);
+		if (count < msgSize) {
+			pr_err("CM: service message size bigger than buffer size - silently ignored!\n");
+			err = -EMSGSIZE;
+		}
+
+		/* Copy to user message type */
+		err = put_user(msg->msg_type, &os_msg->type);
+		if (err) goto out;
+		err = copy_to_user(&os_msg->data.srv, &msg->d.srv,
+				   sizeof(msg->d.srv.srvType) + sizeof(msg->d.srv.srvData));
+		break;
+	}
+	default:
+		mutex_unlock(&channelPriv->msgQueueLock);
+		pr_err("CM: invalid message type %d discarded\n", msg->msg_type);
+		goto wait;
+	}
+out:
+	/* Destroy the message */
+	kfree(msg);
+
+	return err ? err : msgSize;
+}
+
+/** Part of driver's release method. (ie userspace close())
+ * It wakes up all waiter.
+ *
+ * \return POSIX error code
+ */
+static int cmld_flush(struct file *file, fl_owner_t id)
+{
+	if (iminor(file->f_dentry->d_inode) != 0) {
+		struct cm_channel_priv* channelPriv = (struct cm_channel_priv*)(file->private_data);
+		//channelPriv->closed = CHANNEL_CLOSED;
+		file->f_flags |= O_FLUSH;
+		wake_up_all(&channelPriv->waitq);
+	}
+	return 0;
+}
+
+static long cmld_channel_ctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct cm_channel_priv *channelPriv = file->private_data;
+
+	switch(cmd) {
+		/*
+		 * All channel CM SYSCALL
+		 */
+	case CM_BINDCOMPONENTTOCMCORE:
+		return cmld_BindComponentToCMCore(channelPriv, (CM_BindComponentToCMCore_t *)arg);
+	case CM_FLUSHCHANNEL:
+		return cmld_flush(file, 0);
+	default:
+		pr_err("CM(%s): unsupported command %i\n", __func__, cmd);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static long cmld_control_ctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct cm_process_priv* procPriv = file->private_data;
+	switch(cmd) {
+		/*
+		 * All wrapped CM SYSCALL
+		 */
+	case CM_INSTANTIATECOMPONENT:
+		return cmld_InstantiateComponent(procPriv,
+						 (CM_InstantiateComponent_t *)arg);
+
+	case CM_BINDCOMPONENTFROMCMCORE:
+		return cmld_BindComponentFromCMCore(procPriv,
+						    (CM_BindComponentFromCMCore_t *)arg);
+
+	case CM_UNBINDCOMPONENTFROMCMCORE:
+		return cmld_UnbindComponentFromCMCore((CM_UnbindComponentFromCMCore_t *)arg);
+
+	case CM_UNBINDCOMPONENTTOCMCORE:
+		return cmld_UnbindComponentToCMCore(procPriv, (CM_UnbindComponentToCMCore_t *)arg);
+
+	case CM_BINDCOMPONENTASYNCHRONOUS:
+		return cmld_BindComponentAsynchronous(procPriv, (CM_BindComponentAsynchronous_t *)arg);
+
+	case CM_UNBINDCOMPONENTASYNCHRONOUS:
+		return cmld_UnbindComponentAsynchronous(procPriv, (CM_UnbindComponentAsynchronous_t *)arg);
+
+	case CM_BINDCOMPONENT:
+		return cmld_BindComponent(procPriv, (CM_BindComponent_t *)arg);
+
+	case CM_UNBINDCOMPONENT:
+		return cmld_UnbindComponent(procPriv, (CM_UnbindComponent_t *)arg);
+
+	case CM_BINDCOMPONENTTOVOID:
+		return cmld_BindComponentToVoid(procPriv, (CM_BindComponentToVoid_t *)arg);
+
+	case CM_DESTROYCOMPONENT:
+		return cmld_DestroyComponent(procPriv, (CM_DestroyComponent_t *)arg);
+
+	case CM_CREATEMEMORYDOMAIN:
+		return cmld_CreateMemoryDomain(procPriv, (CM_CreateMemoryDomain_t *)arg);
+
+	case CM_CREATEMEMORYDOMAINSCRATCH:
+		return cmld_CreateMemoryDomainScratch(procPriv, (CM_CreateMemoryDomainScratch_t *)arg);
+
+	case CM_DESTROYMEMORYDOMAIN:
+		return cmld_DestroyMemoryDomain((CM_DestroyMemoryDomain_t *)arg);
+
+	case CM_GETDOMAINCOREID:
+		return cmld_GetDomainCoreId((CM_GetDomainCoreId_t *)arg);
+
+	case CM_ALLOCMPCMEMORY:
+		return cmld_AllocMpcMemory(procPriv, (CM_AllocMpcMemory_t *)arg);
+
+	case CM_FREEMPCMEMORY:
+		return cmld_FreeMpcMemory(procPriv, (CM_FreeMpcMemory_t *)arg);
+
+	case CM_GETMPCMEMORYSTATUS:
+		return cmld_GetMpcMemoryStatus((CM_GetMpcMemoryStatus_t *)arg);
+
+	case CM_STARTCOMPONENT:
+		return cmld_StartComponent(procPriv, (CM_StartComponent_t *)arg);
+
+	case CM_STOPCOMPONENT:
+		return cmld_StopComponent(procPriv, (CM_StopComponent_t *)arg);
+
+	case CM_GETMPCLOADCOUNTER:
+		return cmld_GetMpcLoadCounter((CM_GetMpcLoadCounter_t *)arg);
+
+	case CM_GETCOMPONENTDESCRIPTION:
+		return cmld_GetComponentDescription(procPriv, (CM_GetComponentDescription_t *)arg);
+
+	case CM_GETCOMPONENTLISTHEADER:
+		return cmld_GetComponentListHeader(procPriv, (CM_GetComponentListHeader_t *)arg);
+
+	case CM_GETCOMPONENTLISTNEXT:
+		return cmld_GetComponentListNext(procPriv, (CM_GetComponentListNext_t *)arg);
+
+	case CM_GETCOMPONENTREQUIREDINTERFACENUMBER:
+		return cmld_GetComponentRequiredInterfaceNumber(procPriv,
+								(CM_GetComponentRequiredInterfaceNumber_t *)arg);
+
+	case CM_GETCOMPONENTREQUIREDINTERFACE:
+		return cmld_GetComponentRequiredInterface(procPriv,
+							  (CM_GetComponentRequiredInterface_t *)arg);
+
+	case CM_GETCOMPONENTREQUIREDINTERFACEBINDING:
+		return cmld_GetComponentRequiredInterfaceBinding(procPriv,
+								 (CM_GetComponentRequiredInterfaceBinding_t *)arg);
+
+	case CM_GETCOMPONENTPROVIDEDINTERFACENUMBER:
+		return cmld_GetComponentProvidedInterfaceNumber(procPriv,
+								(CM_GetComponentProvidedInterfaceNumber_t *)arg);
+
+	case CM_GETCOMPONENTPROVIDEDINTERFACE:
+		return cmld_GetComponentProvidedInterface(procPriv,
+							  (CM_GetComponentProvidedInterface_t *)arg);
+
+	case CM_GETCOMPONENTPROPERTYNUMBER:
+		return cmld_GetComponentPropertyNumber(procPriv,
+						       (CM_GetComponentPropertyNumber_t *)arg);
+
+	case CM_GETCOMPONENTPROPERTYNAME:
+		return cmld_GetComponentPropertyName(procPriv,
+						     (CM_GetComponentPropertyName_t *)arg);
+
+	case CM_GETCOMPONENTPROPERTYVALUE:
+		return cmld_GetComponentPropertyValue(procPriv,
+						      (CM_GetComponentPropertyValue_t *)arg);
+
+	case CM_READCOMPONENTATTRIBUTE:
+		return cmld_ReadComponentAttribute(procPriv,
+						   (CM_ReadComponentAttribute_t *)arg);
+
+	case CM_GETEXECUTIVEENGINEHANDLE:
+		return cmld_GetExecutiveEngineHandle(procPriv,
+						     (CM_GetExecutiveEngineHandle_t *)arg);
+
+	case CM_SETMODE:
+		return cmld_SetMode((CM_SetMode_t *)arg);
+
+	case CM_GETREQUIREDCOMPONENTFILES:
+		return cmld_GetRequiredComponentFiles(procPriv,
+						      (CM_GetRequiredComponentFiles_t *)arg);
+
+	case CM_MIGRATE:
+		return cmld_Migrate((CM_Migrate_t *)arg);
+
+	case CM_UNMIGRATE:
+		return cmld_Unmigrate((CM_Unmigrate_t *)arg);
+
+	case CM_SETUPRELINKAREA:
+		return cmld_SetupRelinkArea((CM_SetupRelinkArea_t *)arg);
+
+	case CM_PUSHCOMPONENT:
+		return cmld_PushComponent((CM_PushComponent_t *)arg);
+
+	case CM_RELEASECOMPONENT:
+		return cmld_ReleaseComponent((CM_ReleaseComponent_t *)arg);
+
+		/*
+		 * NMF CALLS (Host->MPC bindings)
+		 */
+	case CM_PUSHEVENTWITHSIZE: {
+		CM_PushEventWithSize_t data;
+		t_event_params_handle event;
+
+		/* coverity[tainted_data_argument : FALSE] */
+		if (copy_from_user(&data.in, (CM_PushEventWithSize_t*)arg, sizeof(data.in)))
+			return -EFAULT;
+
+		/* Take the lock to synchronize CM_ENGINE_AllocEvent()
+		 * and CM_ENGINE_PushEvent()
+		 */
+		if (mutex_lock_killable(&procPriv->host2mpcLock))
+			return -ERESTARTSYS;
+
+		event = CM_ENGINE_AllocEvent(data.in.host2mpcId);
+		if (event == NULL) {
+			mutex_unlock(&procPriv->host2mpcLock);
+			return put_user(CM_PARAM_FIFO_OVERFLOW,
+					&((CM_PushEventWithSize_t*)arg)->out.error);
+		}
+		if (data.in.size != 0)
+			/* coverity[tainted_data : FALSE] */
+			if (copy_from_user(event, data.in.h, data.in.size)) {
+				mutex_unlock(&procPriv->host2mpcLock);
+				return -EFAULT; // TODO: what about the already allocated and acknowledged event!?!
+			}
+
+		data.out.error = CM_ENGINE_PushEvent(data.in.host2mpcId, event, data.in.methodIndex);
+		mutex_unlock(&procPriv->host2mpcLock);
+ 
+		/* copy error value back */
+		return put_user(data.out.error, &((CM_PushEventWithSize_t*)arg)->out.error);
+	}
+	
+		/*
+		 * All private (internal) commands
+		 */
+	case CM_PRIVGETMPCMEMORYDESC:
+		return cmld_PrivGetMPCMemoryDesc(procPriv, (CM_PrivGetMPCMemoryDesc_t *)arg);
+
+	case CM_PRIVRESERVEMEMORY:
+		return cmld_PrivReserveMemory(procPriv, arg);
+
+	case CM_GETVERSION: {
+		t_uint32 nmfversion = NMF_VERSION;
+		return copy_to_user((void*)arg, &nmfversion, sizeof(nmfversion));
+	}
+	case CM_PRIV_GETBOARDVERSION:
+		if (cpu_is_u8500v20_or_later()) {
+			enum board_version v = U8500_V2;
+			return copy_to_user((void*)arg, &v, sizeof(v));
+		} else
+			return -EINVAL;
+
+	default: {
+		pr_err("CM(%s): unsupported command %i\n", __func__, cmd);
+		return -EINVAL;
+	}
+	}
+
+	return 0;
+}
+
+/** Driver's ioctl method
+ * Implements user/kernel crossing for SYSCALL API.
+ *
+ * \return POSIX error code
+ */
+static long cmld_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	BUG_ON(filp->private_data == NULL);
+
+	if (iminor(filp->f_dentry->d_inode) == 0) {
+		return cmld_control_ctl(filp, cmd, arg);
+	} else {
+		return cmld_channel_ctl(filp, cmd, arg);
+	}
+}
+
+/** VMA open callback function
+ */
+static void cmld_vma_open(struct vm_area_struct* vma) {
+	struct memAreaDesc_t* curr = (struct memAreaDesc_t*)vma->vm_private_data;
+
+	atomic_inc(&curr->count);
+}
+
+/** VMA close callback function
+ */
+static void cmld_vma_close(struct vm_area_struct* vma) {
+	struct memAreaDesc_t* curr = (struct memAreaDesc_t*)vma->vm_private_data;
+	
+	atomic_dec(&curr->count);
+}
+
+static struct vm_operations_struct cmld_remap_vm_ops = {
+	.open = cmld_vma_open,
+	.close = cmld_vma_close,
+};
+
+/** mmap implementation.
+ * Remaps just once.
+ * 
+ * \return POSIX error code
+ */
+static int cmld_mmap(struct file* file, struct vm_area_struct* vma)
+{
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	struct list_head* listHead;
+	struct list_head* cursor;
+	struct cm_process_priv* procPriv = file->private_data;
+	struct memAreaDesc_t* curr = NULL;
+	unsigned int vma_size = vma->vm_end-vma->vm_start;
+
+	BUG_ON(procPriv == NULL);
+
+	listHead = &procPriv->memAreaDescList;
+
+	if (lock_process(procPriv)) return -ERESTARTSYS;
+	/* Make sure the memory area has not already been remapped */
+	list_for_each(cursor, listHead) {
+		curr = list_entry(cursor, struct memAreaDesc_t, list);
+		/* For now, the user space aligns any requested physaddr to a page-size limit
+		   This is not safe and must be fixed. But this is the only way to
+		   minimize the allocated TCM memory, needed because of low amount of
+		   TCM memory 
+		   Another way is to add some more check before doing this mmap()
+		   to allow this mmap, for example.
+		   NOTE: this memory must be first reserved via the CM_PRIVRESERVEMEMORY ioctl()
+		*/
+		if ((curr->physAddr&PAGE_MASK) == offset &&
+		    curr->tid == current->pid) {
+			if (curr->userLogicalAddr) {
+				unlock_process(procPriv);
+				return -EINVAL; // already mapped!
+			}
+			/* reset the thread id value, to not confuse any further mmap() */
+			curr->tid = 0;
+			break;
+		}
+	}
+
+	if (cursor == listHead) {
+		unlock_process(procPriv);
+		return -EINVAL; // no matching memory area descriptor found!
+	}
+	
+	/* Very, very important to have consistent buffer transition */
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	vma->vm_flags |= VM_RESERVED | VM_IO | VM_DONTEXPAND | VM_DONTCOPY;
+	
+	if (remap_pfn_range(vma, vma->vm_start, offset>>PAGE_SHIFT,
+			    vma_size, vma->vm_page_prot)) {
+		unlock_process(procPriv);
+		return -EAGAIN;
+	}
+ 
+	/* Offset represents the physical address.
+	 * Update the list entry filling in the logical address assigned to the user
+	 */
+	/*
+	 * NOTE: here the useLogicalAddr is page-aligned, but not necessaly the
+	 *       phycical address. We mmap() more than originaly requested by the
+	 *       user, see in CM User Proxy (file cmsyscallwrapper.c)
+	 */
+	curr->userLogicalAddr = vma->vm_start;
+
+	/* increment reference counter */
+	atomic_inc(&curr->count);
+	
+	unlock_process(procPriv);
+
+	/* set private data structure and callbacks */
+	vma->vm_private_data = (void *)curr;
+	vma->vm_ops = &cmld_remap_vm_ops;
+
+	return 0;
+}
+
+/** MPC Events tasklet
+ *  The parameter is used to know from which interrupts we're comming
+ *  and which core to pass to CM_ProcessMpcEvent():
+ *  0 means HSEM => ARM_CORE_ID
+ *  otherwise, it gives the index+1 of MPC within osalEnv.mpc table
+ */
+static void mpc_events_tasklet_handler(unsigned long core)
+{
+	/* This serves internal events directly. No propagation to user space.
+	 * Calls OSAL_PostDfc implementation for user interface events */
+	if (core == 0) {
+		CM_ProcessMpcEvent(ARM_CORE_ID);
+		enable_irq(IRQ_DB8500_HSEM);
+	} else {
+		--core;
+		CM_ProcessMpcEvent(osalEnv.mpc[core].coreId);
+		enable_irq(osalEnv.mpc[core].interrupt0);
+	}
+}
+
+/** Hardware semaphore and MPC interrupt handler
+ * 'data' param is the one given when registering the IRQ hanlder,
+ * contains the source core (ARM or MPC), and follows the same logic
+ * as for mpc_events_tasklet_handler()
+ * This handler is used for all IRQ handling some com (ie HSEM or
+ * all MPC IRQ line0)
+ */
+static irqreturn_t mpc_events_irq_handler(int irq, void *data)
+{
+	unsigned core = (unsigned)data;
+
+	if (core != 0)
+		--core;
+	disable_irq_nosync(irq);
+	tasklet_schedule(&osalEnv.mpc[core].tasklet);
+
+	return IRQ_HANDLED;
+}
+
+/** MPC panic handler
+ * 'idx' contains the index of the core within the osalEnv.mpc table.
+ * This handler is used for all MPC IRQ line1
+ */
+static irqreturn_t panic_handler(int irq, void *idx)
+{
+	set_bit((int)idx, &service_tasklet_data);
+	disable_irq_nosync(irq);
+	tasklet_schedule(&cmld_service_tasklet);
+	return IRQ_HANDLED;
+}
+
+/** Driver's operations
+ */
+static struct file_operations cmld_fops = {
+	.owner		=	THIS_MODULE,
+	.read		=	cmld_read,
+	.unlocked_ioctl	=	cmld_ioctl,
+	.mmap		=	cmld_mmap,
+	.open		=	cmld_open,
+	.flush		=	cmld_flush,
+	.release	=	cmld_release,
+};
+
+/**
+ * Configure a MPC, called for each MPC to configure
+ *
+ * \param i            index of the MPC to configure (refer to the index
+ *                     of the MPC within the osalEnvironment.mpc table)
+ * \param dataAllocId  allocId of the data segment, passed through each call of
+ *                     this function, and initialized at the first call in case
+ *                     shared data segment
+ */
+static int configureMpc(unsigned i, t_cfg_allocator_id *dataAllocId)
+{
+	int err;
+	t_cm_system_address mpcSystemAddress;
+	t_nmf_memory_segment codeSegment, dataSegment;
+	t_cfg_allocator_id codeAllocId;
+	t_cm_domain_id eeDomainId;
+	t_cm_domain_memory eeDomain = INIT_DOMAIN;
+	char regulator_name[14];
+
+	getMpcSystemAddress(i, &mpcSystemAddress);
+	getMpcSdramSegments(i, &codeSegment, &dataSegment);
+
+	/* Create code segment */
+	err = CM_ENGINE_AddMpcSdramSegment(&codeSegment, &codeAllocId, "Code");
+	if (err != CM_OK) {
+		pr_err("CM_ENGINE_AddMpcSdramSegment() error code: %d\n", err);
+		return -EAGAIN;
+	}
+
+	/* Create data segment
+	 * NOTE: in case of shared data segment, all MPC point to the same data segment
+	 * (see in remapRegions()) and we need to create the segment only at first call.
+	 * => we reuse the same allocId for the following MPCs
+	 */
+	if ((osalEnv.mpc[i].sdramDataL != osalEnv.mpc[0].sdramDataL)
+	    || *dataAllocId == -1) {
+		err = CM_ENGINE_AddMpcSdramSegment(&dataSegment, dataAllocId, "Data");
+		if (err != CM_OK) {
+			pr_err("CM_ENGINE_AddMpcSdramSegment() error code: %d\n", err);
+			return -EAGAIN;
+		}
+	}
+
+	/* create default domain for the given coreId
+	 * this serves for instanciating EE and the LoadMap, only sdram segment is present
+	 * this domain will probably overlap with other user domains
+	 */
+	eeDomain.coreId            = osalEnv.mpc[i].coreId;
+	eeDomain.sdramCode.offset  = 0x0;
+	eeDomain.sdramData.offset  = 0x0;
+	eeDomain.sdramCode.size    = 0x8000;
+	eeDomain.sdramData.size    = 0x40000;
+	eeDomain.esramCode.size    = 0x4000;
+	eeDomain.esramData.size    = 0x40000;
+	err = CM_ENGINE_CreateMemoryDomain(NMF_CORE_CLIENT, &eeDomain, &eeDomainId);
+	if (err != CM_OK) {
+		pr_err("Create EE domain on %s failed with error code: %d\n", osalEnv.mpc[i].name, err);
+		return -EAGAIN;
+	}
+
+	err = CM_ENGINE_ConfigureMediaProcessorCore(
+		osalEnv.mpc[i].coreId,
+		osalEnv.mpc[i].eeId,
+		(cfgSemaphoreTypeHSEM ? SYSTEM_SEMAPHORES : LOCAL_SEMAPHORES),
+		osalEnv.mpc[i].nbYramBanks,
+		&mpcSystemAddress,
+		eeDomainId,
+		codeAllocId,
+		*dataAllocId);
+
+	if (err != CM_OK) {
+		pr_err("CM_ConfigureMediaProcessorCore failed with error code: %d\n", err);	
+		return -EAGAIN;
+	}
+
+        // Communication channel
+        if (! cfgSemaphoreTypeHSEM) {
+		tasklet_init(&osalEnv.mpc[i].tasklet, mpc_events_tasklet_handler, i+1);
+		err = request_irq(osalEnv.mpc[i].interrupt0, mpc_events_irq_handler, IRQF_DISABLED, osalEnv.mpc[i].name, (void*)(i+1));
+		if (err != 0) {
+			pr_err("CM: request_irq failed to register irq0 %i for %s (%i)\n", osalEnv.mpc[i].interrupt0, osalEnv.mpc[i].name, err);
+			return err;
+		}
+        }
+
+        // Panic channel
+	err = request_irq(osalEnv.mpc[i].interrupt1, panic_handler, IRQF_DISABLED, osalEnv.mpc[i].name, (void*)i);
+	if (err != 0) {
+		pr_err("CM: request_irq failed to register irq1 %i for %s (%i)\n", osalEnv.mpc[i].interrupt1, osalEnv.mpc[i].name, err);
+		free_irq(osalEnv.mpc[i].interrupt0, (void*)(i+1));
+		return err;
+	}
+
+	// Retrieve the regulators used for this MPCs
+	sprintf(regulator_name, "%s-mmdsp", osalEnv.mpc[i].name);
+	osalEnv.mpc[i].mmdsp_regulator = regulator_get(cmld_dev[0], regulator_name);
+	if (IS_ERR(osalEnv.mpc[i].mmdsp_regulator)) {
+		long err = PTR_ERR(osalEnv.mpc[i].mmdsp_regulator);
+		pr_err("CM: Error while retrieving the regulator %s: %ld\n", regulator_name, err);
+		osalEnv.mpc[i].mmdsp_regulator = NULL;
+		return err;
+	}
+	sprintf(regulator_name, "%s-pipe", osalEnv.mpc[i].name);
+	osalEnv.mpc[i].pipe_regulator = regulator_get(cmld_dev[0], regulator_name);
+	if (IS_ERR(osalEnv.mpc[i].pipe_regulator)) {
+		long err = PTR_ERR(osalEnv.mpc[i].pipe_regulator);
+		pr_err("CM: Error while retrieving the regulator %s: %ld\n", regulator_name, err);
+		osalEnv.mpc[i].pipe_regulator = NULL;
+		return err;
+	}
+#ifdef CONFIG_HAS_WAKELOCK
+	wake_lock_init(&osalEnv.mpc[i].wakelock, WAKE_LOCK_SUSPEND, osalEnv.mpc[i].name);
+#endif
+	return 0;
+}
+
+/* Free all used MPC irqs and clocks.
+ * max_mpc allows it to be called from init_module and free
+ * only the already configured irqs.
+ */
+static void free_mpc_irqs(int max_mpc)
+{
+	int i;
+	for (i=0; i<max_mpc; i++) {
+		if (! cfgSemaphoreTypeHSEM)
+			free_irq(osalEnv.mpc[i].interrupt0, (void*)(i+1));
+		free_irq(osalEnv.mpc[i].interrupt1, (void*)i);
+		if (osalEnv.mpc[i].mmdsp_regulator)
+			regulator_put(osalEnv.mpc[i].mmdsp_regulator);
+		if (osalEnv.mpc[i].pipe_regulator)
+			regulator_put(osalEnv.mpc[i].pipe_regulator);
+#ifdef CONFIG_HAS_WAKELOCK
+		wake_lock_destroy(&osalEnv.mpc[i].wakelock);
+#endif
+	}
+}
+
+/** Module entry point
+ * Allocate memory chunks. Register hardware semaphore, SIA and SVA interrupts.
+ * Initialize Component Manager. Register hotplug for components download.
+ *
+ * \return POSIX error code
+ */
+static int __init cmld_init_module(void)
+{
+	int err;
+	unsigned i=0;
+	dev_t dev;
+	t_cfg_allocator_id dataAllocId = -1;
+	void *htim_base=NULL;
+
+	/* Component manager initialization descriptors */
+	t_nmf_hw_mapping_desc nmfHwMappingDesc;
+	t_nmf_config_desc nmfConfigDesc = { cfgCommunicationLocationInSDRAM ? COMS_IN_SDRAM : COMS_IN_ESRAM };
+
+	/* OSAL_*Resources() assumes the following, so check that it is correct */
+	if (SVA != COREIDX((int)SVA_CORE_ID)) {
+		pr_err("SVA and (SVA_CORE_ID-1) differs : code must be fixed !\n");
+		return -EIO;
+	}
+	if (SIA != COREIDX((int)SIA_CORE_ID)) {
+		pr_err("SIA and (SIA_CORE_ID-1) differs : code must be fixed !\n");
+		return -EIO;
+	}
+
+#ifdef CM_DEBUG_ALLOC
+	init_debug_alloc();
+#endif
+
+	err = -EIO;
+	prcmu_base = __io_address(U8500_PRCMU_BASE);
+	if (cpu_is_u8500v20_or_later()) {
+		/* power on a clock/timer 90KHz used on SVA */
+		htim_base = ioremap_nocache(U8500_CR_BASE /*0xA03C8000*/, SZ_4K);
+                prcmu_tcdm_base = __io_address(U8500_PRCMU_TCDM_BASE);
+        } else {
+                pr_err("CM: Unsupported chip version\n");
+		goto out;
+        }
+
+	/* Activate SVA 90 KHz timer */
+	if (htim_base == NULL)
+		goto out;
+	iowrite32((1<<26) | ioread32(htim_base), htim_base);
+	iounmap(htim_base);
+
+	/*i = ioread32(PRCM_SVAMMDSPCLK_MGT) & 0xFF;
+	if (i != 0x22)
+		pr_alert("CM: Looks like SVA is not clocked at 200MHz (PRCM_SVAMMDSPCLK_MGT=%x)\n", i);
+	i = ioread32(PRCM_SIAMMDSPCLK_MGT) & 0xFF;
+	if (i != 0x22)
+		pr_alert("CM: Looks like SIA is not clocked at 200MHz (PRCM_SIAMMDSPCLK_MGT=%x)\n", i);
+
+	i = 0;*/
+	err = init_config();
+	if (err)
+		goto out;
+
+	/* Remap all needed regions and store in osalEnv base addresses */
+	err = remapRegions();
+	if (err != 0)
+		goto out;
+	
+	/* Initialize linux devices */
+	err = class_register(&cmld_class);
+	if (err) {
+		pr_err("CM: class_register failed (%d)\n", err);
+                goto out;
+	}
+
+	/* Register char device */
+	err = alloc_chrdev_region(&dev, 0, ARRAY_SIZE(cmld_devname), "cm");
+	if (err) {
+		pr_err("CM: alloc_chrdev_region failed (%d)\n", err);
+		goto out_destroy_class;
+	}
+	cmld_major =  MAJOR(dev);
+
+	cdev_init(&cmld_cdev, &cmld_fops);
+	cmld_cdev.owner = THIS_MODULE;
+	err = cdev_add (&cmld_cdev, dev, ARRAY_SIZE(cmld_devname));
+	if (err) {
+		pr_err("CM: cdev_add failed (%d)\n", err);
+		goto out_destroy_chrdev;
+	}
+
+	for (i=0; i<ARRAY_SIZE(cmld_devname); i++) {
+		cmld_dev[i] = device_create(&cmld_class, NULL, MKDEV(cmld_major, i), NULL,
+					    "%s", cmld_devname[i]);
+		if (IS_ERR(cmld_dev[i])) {
+			err = PTR_ERR(cmld_dev[i]);
+			pr_err("CM: device_create failed (%d)\n", err);
+			goto out_destroy_device;
+		}
+	}
+
+	osalEnv.esram_regulator[ESRAM_12] = regulator_get(cmld_dev[0], "esram12");
+	if (IS_ERR(osalEnv.esram_regulator[ESRAM_12])) {
+		err = PTR_ERR(osalEnv.esram_regulator[ESRAM_12]);
+		pr_err("CM: Error while retrieving the regulator for esram12: %d\n", err);
+		osalEnv.esram_regulator[ESRAM_12] = NULL;
+		goto out_destroy_device;
+	}
+	osalEnv.esram_regulator[ESRAM_34] = regulator_get(cmld_dev[0], "esram34");
+	if (IS_ERR(osalEnv.esram_regulator[ESRAM_34])) {
+		err = PTR_ERR(osalEnv.esram_regulator[ESRAM_34]);
+		pr_err("CM: Error while retrieving the regulator for esram34: %d\n", err);
+		osalEnv.esram_regulator[ESRAM_34] = NULL;
+		goto out_destroy_device;
+	}
+
+	/* Fill in the descriptors needed by CM_ENGINE_Init() */
+	getNmfHwMappingDesc(&nmfHwMappingDesc);
+
+	/* Initialize Component Manager */
+	err = CM_ENGINE_Init(&nmfHwMappingDesc, &nmfConfigDesc);
+	if (err != CM_OK) {
+		pr_err("CM: CM_Init failed with error code: %d\n", err);
+		err = -EAGAIN;
+		goto out_destroy_device;
+	} else {
+		pr_info("Initialize NMF %d.%d.%d Component Manager......\n",
+			VERSION_MAJOR(NMF_VERSION),
+			VERSION_MINOR(NMF_VERSION),
+			VERSION_PATCH(NMF_VERSION));
+		pr_info("[ CM Linux Driver %d.%d.%d ]\n",
+			VERSION_MAJOR(NMF_VERSION),
+			VERSION_MINOR(NMF_VERSION),
+			CMDRIVER_PATCH_VERSION);
+	}
+
+	/* Reserve DMA and MCDE ESRAM if needed */
+	if (cfgESRAM_ReserveDMA) {
+		t_cm_domain_memory  domain = INIT_DOMAIN;
+
+		// Reserve memory used by DMA
+		domain.coreId = ARM_CORE_ID;
+		domain.esramData.offset = 0x0;
+		domain.esramData.size   = cfgESRAMDmaSize * ONE_KB;
+		err = CM_ENGINE_CreateMemoryDomain(NMF_CORE_CLIENT,
+						   &domain, &dmaDomainId);
+		if (err != CM_OK) {
+			pr_err("CM: Create DMA/ESRAM domain failed with error code: %d\n", err);
+			err = -EAGAIN;
+			goto out_all;
+		}
+
+		err = CM_ENGINE_AllocMpcMemory(dmaDomainId,
+					       NMF_CORE_CLIENT,
+					       CM_MM_MPC_ESRAM16,
+					       cfgESRAMDmaSize * ONE_KB / 2,
+					       CM_MM_MPC_ALIGN_NONE,
+					       &dmaMemoryHdl);
+		if (err != CM_OK) {
+			pr_err("CM: Alloc DMA in ESRAM domain failed with error code: %d\n", err);
+			err = -EAGAIN;
+			goto out_all;
+		}
+	}
+
+	if (cfgESRAM_ReserveMCDE) {
+		t_cm_domain_memory  domain = INIT_DOMAIN;
+
+		// Reserve memory used by MCDE
+		domain.coreId = ARM_CORE_ID;
+		domain.esramData.offset = (cfgESRAMSize - cfgESRAMMcdeSize) * ONE_KB;
+		domain.esramData.size   = cfgESRAMMcdeSize * ONE_KB;
+		err = CM_ENGINE_CreateMemoryDomain(NMF_CORE_CLIENT,
+						   &domain, &mcdeDomainId);
+		if (err != CM_OK) {
+			pr_err("CM: Create MCDE/ESRAM domain failed with error code: %d\n", err);
+			err = -EAGAIN;
+			goto out_all;
+		}
+
+		err = CM_ENGINE_AllocMpcMemory(mcdeDomainId,
+					       NMF_CORE_CLIENT,
+					       CM_MM_MPC_ESRAM16,
+					       cfgESRAMMcdeSize * ONE_KB / 2,
+					       CM_MM_MPC_ALIGN_NONE,
+					       &mcdeMemoryHdl);
+		if (err != CM_OK) {
+			pr_err("CM: Alloc MCDE in ESRAM domain failed with error code: %d\n", err);
+			err = -EAGAIN;
+			goto out_all;
+		}
+	}
+
+	/* Configure MPC Cores */
+	for (i=0; i<NB_MPC; i++) {
+		err = configureMpc(i, &dataAllocId);
+		if (err)
+			goto out_all;
+	}
+	/* End of Component Manager initialization phase */
+
+
+        if (cfgSemaphoreTypeHSEM) {
+		/* We use tasklet of mpc[0]. See comments above osalEnvironnent struct */
+		tasklet_init(&osalEnv.mpc[0].tasklet, mpc_events_tasklet_handler, 0);
+		err = request_irq(IRQ_DB8500_HSEM, mpc_events_irq_handler, IRQF_DISABLED,
+				  "hwsem", 0);
+		if (err) {
+			pr_err("CM: request_irq failed to register hwsem irq %i (%i)\n", 
+			       IRQ_DB8500_HSEM, err);
+			goto out_all;
+		}
+	}
+
+	return 0;
+
+out_all:
+	if (err) {
+		if (cfgESRAM_ReserveMCDE) {
+			if (mcdeMemoryHdl)
+				CM_ENGINE_FreeMpcMemory(mcdeMemoryHdl);
+			CM_ENGINE_DestroyMemoryDomain(mcdeDomainId);
+		}
+		if (cfgESRAM_ReserveDMA) {
+			if (dmaMemoryHdl)
+				CM_ENGINE_FreeMpcMemory(dmaMemoryHdl);
+			CM_ENGINE_DestroyMemoryDomain(dmaDomainId);
+		}
+	}
+	free_mpc_irqs(i);
+	CM_ENGINE_Destroy();
+	i=ARRAY_SIZE(cmld_devname);
+out_destroy_device:
+	if (osalEnv.esram_regulator[ESRAM_12])
+		regulator_put(osalEnv.esram_regulator[ESRAM_12]);
+	if (osalEnv.esram_regulator[ESRAM_34])
+		regulator_put(osalEnv.esram_regulator[ESRAM_34]);
+	while (i--)
+		device_destroy(&cmld_class, MKDEV(cmld_major, i));
+	cdev_del(&cmld_cdev);
+out_destroy_chrdev:
+	unregister_chrdev_region(dev, ARRAY_SIZE(cmld_devname));
+out_destroy_class:
+	class_unregister(&cmld_class);
+out:
+	unmapRegions();
+#ifdef 	CM_DEBUG_ALLOC
+	cleanup_debug_alloc();
+#endif
+	return err;
+}
+
+/** Module exit point
+ * Unregister the driver. This will lead to a 'remove' call.
+ */
+static void __exit cmld_cleanup_module(void)
+{
+	unsigned i;
+
+	if (!list_empty(&channel_list))
+		pr_err("CM Driver ending with non empty channel list\n");
+	if (!list_empty(&process_list))
+		pr_err("CM Driver ending with non empty process list\n");
+	
+	if (cfgESRAM_ReserveMCDE) {
+		if (mcdeMemoryHdl)
+			CM_ENGINE_FreeMpcMemory(mcdeMemoryHdl);
+		CM_ENGINE_DestroyMemoryDomain(mcdeDomainId);
+	}
+	if (cfgESRAM_ReserveDMA) {
+		if (dmaMemoryHdl)
+			CM_ENGINE_FreeMpcMemory(dmaMemoryHdl);
+		CM_ENGINE_DestroyMemoryDomain(dmaDomainId);
+	}
+        if (cfgSemaphoreTypeHSEM)
+		free_irq(IRQ_DB8500_HSEM, NULL);
+	free_mpc_irqs(NB_MPC);
+	tasklet_kill(&cmld_service_tasklet);
+
+	if (osalEnv.esram_regulator[ESRAM_12])
+		regulator_put(osalEnv.esram_regulator[ESRAM_12]);
+	if (osalEnv.esram_regulator[ESRAM_34])
+		regulator_put(osalEnv.esram_regulator[ESRAM_34]);
+	for (i=0; i<ARRAY_SIZE(cmld_devname); i++)
+		device_destroy(&cmld_class, MKDEV(cmld_major, i));
+	cdev_del(&cmld_cdev);
+	unregister_chrdev_region(MKDEV(cmld_major, 0), ARRAY_SIZE(cmld_devname));
+	class_unregister(&cmld_class);
+
+	CM_ENGINE_Destroy();
+
+	unmapRegions();
+#ifdef 	CM_DEBUG_ALLOC
+	cleanup_debug_alloc();
+#endif
+}
+module_init(cmld_init_module);
+module_exit(cmld_cleanup_module);
+
+MODULE_AUTHOR("David Siorpaes");
+MODULE_AUTHOR("Wolfgang Betz");
+MODULE_AUTHOR("Pierre Peiffer");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Nomadik Multiprocessing Framework Component Manager Linux driver");
diff --git a/drivers/staging/nmf-cm/cmld.h b/drivers/staging/nmf-cm/cmld.h
new file mode 100644
index 00000000000..234e30db503
--- /dev/null
+++ b/drivers/staging/nmf-cm/cmld.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef CMLD_H
+#define CMLD_H
+
+#include <linux/kref.h>
+#include <linux/mutex.h>
+#include <linux/version.h>
+#include <linux/wait.h>
+#include <inc/nmf-limits.h>
+#include "cmioctl.h"
+
+/** Channel state used within the per-channel private structure 'cm_channel_priv'
+ */
+enum channel_state {
+	CHANNEL_CLOSED = 0,	/**< Channel already closed */
+	CHANNEL_OPEN,		/**< Channel still open */
+};
+
+/** Component Manager per-process private structure
+ *  It is created the first time a process opens /dev/cm0 or /dev/cm1
+ */
+struct cm_process_priv
+{
+	struct kref ref;                  /**< ref count */
+	struct list_head entry;           /**< This entry */
+	pid_t pid;                        /**< pid of process owner */
+	struct mutex mutex;               /**< per process mutex: protect memAreaDescList */
+	struct list_head memAreaDescList; /**< memAreaDesc_t list */
+	struct mutex host2mpcLock;        /**< used to synchronize each AllocEvent + PushEvent */
+};
+
+/** Component Manager per-channel private structure
+ * It is created when a user opens /dev/cm1
+ */
+struct cm_channel_priv
+{
+	enum channel_state state;         /**< Channel state */
+	struct list_head entry;           /**< This entry */
+	struct cm_process_priv *proc;     /**< Pointer to the owner process structure */
+	struct list_head skelList;        /**< t_skelwrapper list */
+	struct mutex skelListLock;        /**< skelList mutex */
+	struct plist_head messageQueue;   /**< queueelem_t list */
+	struct mutex msgQueueLock;        /**< lock used to synchronize MPC to HOST bindings
+					      in case of multiple read (see cmld_read comments) */
+	spinlock_t bh_lock;               /**< lock used to synchronize add/removal of element in/from
+					      the message queue in both user context and tasklet */
+	wait_queue_head_t waitq;          /**< wait queue used to block read() call */
+};
+
+/** Memory area descriptor.
+ */
+struct memAreaDesc_t {
+	struct list_head list;            /**< Doubly linked list descriptor */
+	atomic_t count;                   /**< Reference counter */
+	pid_t tid;                        /**< tid of the process this area is reserved for */
+	t_cm_memory_handle handle;        /**< Component Manager handle */
+	unsigned int size;                /**< Size */
+	unsigned int physAddr;            /**< Physical address */
+	unsigned int kernelLogicalAddr;   /**< Logical address as seen by kernel */
+	unsigned int userLogicalAddr;     /**< Logical address as seen by user */
+	unsigned int mpcPhysAddr;         /**< Physicaladdress as seen by MPC */
+	struct cm_process_priv* procPriv; /**< link to per process private structure */
+};
+
+extern struct list_head channel_list; /**< List of all allocated channel structures */
+extern struct list_head process_list; /**< List of all allocated process private structure */
+
+/** Lock/unlock per process mutex
+ *
+ * \note Must be taken before tasklet_disable (if necessary)!
+ */
+#define lock_process_uninterruptible(proc) (mutex_lock(&proc->mutex))
+#define lock_process(proc) (mutex_lock_killable(&proc->mutex))
+#define unlock_process(proc) (mutex_unlock(&proc->mutex))
+
+
+
+int cmld_InstantiateComponent(struct cm_process_priv *, CM_InstantiateComponent_t __user *);
+int cmld_BindComponentFromCMCore(struct cm_process_priv *,
+				 CM_BindComponentFromCMCore_t __user *);
+int cmld_UnbindComponentFromCMCore(CM_UnbindComponentFromCMCore_t __user *);
+int cmld_BindComponentToCMCore(struct cm_channel_priv *, CM_BindComponentToCMCore_t __user *);
+int cmld_UnbindComponentToCMCore(struct cm_process_priv*, CM_UnbindComponentToCMCore_t __user *);
+int cmld_BindComponentAsynchronous(struct cm_process_priv*, CM_BindComponentAsynchronous_t __user *);
+int cmld_UnbindComponentAsynchronous(struct cm_process_priv*, CM_UnbindComponentAsynchronous_t __user *);
+int cmld_BindComponent(struct cm_process_priv*, CM_BindComponent_t __user *);
+int cmld_UnbindComponent(struct cm_process_priv*, CM_UnbindComponent_t __user *);
+int cmld_BindComponentToVoid(struct cm_process_priv*, CM_BindComponentToVoid_t  __user *);
+int cmld_DestroyComponent(struct cm_process_priv*, CM_DestroyComponent_t __user *);
+int cmld_CreateMemoryDomain(struct cm_process_priv*, CM_CreateMemoryDomain_t __user *);
+int cmld_CreateMemoryDomainScratch(struct cm_process_priv*, CM_CreateMemoryDomainScratch_t __user *);
+int cmld_DestroyMemoryDomain(CM_DestroyMemoryDomain_t __user *);
+int cmld_GetDomainCoreId(CM_GetDomainCoreId_t __user *);
+int cmld_AllocMpcMemory(struct cm_process_priv *, CM_AllocMpcMemory_t __user *);
+int cmld_FreeMpcMemory(struct cm_process_priv *, CM_FreeMpcMemory_t __user *);
+int cmld_GetMpcMemoryStatus(CM_GetMpcMemoryStatus_t __user *);
+int cmld_StartComponent(struct cm_process_priv *, CM_StartComponent_t __user *);
+int cmld_StopComponent(struct cm_process_priv *, CM_StopComponent_t __user *);
+int cmld_GetMpcLoadCounter(CM_GetMpcLoadCounter_t __user *);
+int cmld_GetComponentDescription(struct cm_process_priv *, CM_GetComponentDescription_t __user *);
+int cmld_GetComponentListHeader(struct cm_process_priv *, CM_GetComponentListHeader_t __user *);
+int cmld_GetComponentListNext(struct cm_process_priv *, CM_GetComponentListNext_t __user *);
+int cmld_GetComponentRequiredInterfaceNumber(struct cm_process_priv *,
+					     CM_GetComponentRequiredInterfaceNumber_t __user *);
+int cmld_GetComponentRequiredInterface(struct cm_process_priv *,
+				       CM_GetComponentRequiredInterface_t __user *);
+int cmld_GetComponentRequiredInterfaceBinding(struct cm_process_priv *,
+					      CM_GetComponentRequiredInterfaceBinding_t __user *);
+int cmld_GetComponentProvidedInterfaceNumber(struct cm_process_priv *,
+					     CM_GetComponentProvidedInterfaceNumber_t __user *);
+int cmld_GetComponentProvidedInterface(struct cm_process_priv *,
+				       CM_GetComponentProvidedInterface_t __user *);
+int cmld_GetComponentPropertyNumber(struct cm_process_priv *,
+				    CM_GetComponentPropertyNumber_t __user *);
+int cmld_GetComponentPropertyName(struct cm_process_priv *, CM_GetComponentPropertyName_t __user *);
+int cmld_GetComponentPropertyValue(struct cm_process_priv *, CM_GetComponentPropertyValue_t __user *);
+int cmld_ReadComponentAttribute(struct cm_process_priv *, CM_ReadComponentAttribute_t __user *);
+int cmld_GetExecutiveEngineHandle(struct cm_process_priv *, CM_GetExecutiveEngineHandle_t __user *);
+int cmld_SetMode(CM_SetMode_t __user *);
+int cmld_GetRequiredComponentFiles(struct cm_process_priv *cmPriv,
+				   CM_GetRequiredComponentFiles_t __user *);
+int cmld_Migrate(CM_Migrate_t __user *);
+int cmld_Unmigrate(CM_Unmigrate_t __user *);
+int cmld_SetupRelinkArea(CM_SetupRelinkArea_t __user *);
+int cmld_PushComponent(CM_PushComponent_t __user *);
+int cmld_ReleaseComponent(CM_ReleaseComponent_t __user *);
+int cmld_PrivGetMPCMemoryDesc(struct cm_process_priv *, CM_PrivGetMPCMemoryDesc_t __user *);
+int cmld_PrivReserveMemory(struct cm_process_priv *, unsigned int);
+#endif
diff --git a/drivers/staging/nmf-cm/configuration.c b/drivers/staging/nmf-cm/configuration.c
new file mode 100644
index 00000000000..175b6152b40
--- /dev/null
+++ b/drivers/staging/nmf-cm/configuration.c
@@ -0,0 +1,115 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/** \file configuration.c
+ *
+ * Nomadik Multiprocessing Framework Linux Driver
+ *
+ */
+
+#include <cm/engine/api/configuration_engine.h>
+#include <cm/engine/configuration/inc/configuration_status.h>
+
+#include "osal-kernel.h"
+
+/* Per-driver environment */
+struct OsalEnvironment osalEnv =
+{
+	.mpc = {
+		{
+			.coreId        = SVA_CORE_ID,
+			.name          = "sva",
+			.baseP         = (void*)SVA_BASE_ADDR,
+			.interrupt0     = IRQ_DB8500_SVA,
+			.interrupt1     = IRQ_DB8500_SVA2,
+			.mmdsp_regulator = NULL,
+			.pipe_regulator  = NULL,
+			.monitor_tsk     = NULL,
+			.hwmemCode       = NULL,
+			.hwmemData       = NULL,
+		},
+		{
+			.coreId        = SIA_CORE_ID,
+			.name          = "sia",
+			.baseP         = (void*)SIA_BASE_ADDR,
+			.interrupt0     = IRQ_DB8500_SIA,
+			.interrupt1     = IRQ_DB8500_SIA2,
+			.mmdsp_regulator = NULL,
+			.pipe_regulator  = NULL,
+			.monitor_tsk     = NULL,
+			.hwmemCode       = NULL,
+			.hwmemData       = NULL,
+		}
+	},
+	.esram_regulator = { NULL, NULL},
+	.dsp_sleep = {
+		.sia_auto_pm_enable = PRCMU_AUTO_PM_OFF,
+		.sia_power_on       = 0,
+		.sia_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF,
+		.sva_auto_pm_enable = PRCMU_AUTO_PM_OFF,
+		.sva_power_on       = 0,
+		.sva_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF,		
+	},
+	.dsp_idle = {
+		.sia_auto_pm_enable = PRCMU_AUTO_PM_OFF,
+		.sia_power_on       = 0,
+		.sia_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF,
+		.sva_auto_pm_enable = PRCMU_AUTO_PM_OFF,
+		.sva_power_on       = 0,
+		.sva_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF,		
+	},
+};
+
+module_param_call(cm_debug_level, param_set_int, param_get_int,
+                  &cm_debug_level, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(cm_debug_level, "Debug level of NMF Core");
+
+module_param_call(cm_error_break, param_set_bool, param_get_bool,
+                  &cm_error_break, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(cm_error_break, "Stop on error (in an infinite loop, for debugging purpose)");
+
+module_param_call(cmIntensiveCheckState, param_set_bool, param_get_bool,
+                  &cmIntensiveCheckState, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(cmIntensiveCheckState, "Add additional intensive checks");
+
+DECLARE_MPC_PARAM(SVA, SDRAM_DATA_SIZE, "", 1);
+
+DECLARE_MPC_PARAM(SIA, 0, "\n\t\t\t(0 means shared with SVA)", 2);
+
+int cfgCommunicationLocationInSDRAM = 1;
+module_param(cfgCommunicationLocationInSDRAM, bool, S_IRUGO);
+MODULE_PARM_DESC(cfgCommunicationLocationInSDRAM, "Location of communications (SDRAM or ESRAM)");
+
+int cfgSemaphoreTypeHSEM = 1;
+module_param(cfgSemaphoreTypeHSEM, bool, S_IRUGO);
+MODULE_PARM_DESC(cfgSemaphoreTypeHSEM, "Semaphore used (HSEM or LSEM)");
+
+int cfgESRAMSize = ESRAM_SIZE;
+module_param(cfgESRAMSize, uint, S_IRUGO);
+MODULE_PARM_DESC(cfgESRAMSize, "Size of ESRAM used in the CM (in Kb)");
+
+int init_config(void)
+{
+	if (cfgMpcSDRAMCodeSize_SVA == 0 || cfgMpcSDRAMCodeSize_SIA == 0) {
+		pr_err("SDRAM code size must be greater than 0\n");
+		return -EINVAL;
+	}
+
+	if (cfgMpcSDRAMDataSize_SVA == 0) {
+		pr_err("SDRAM data size for SVA must be greater than 0\n");
+		return -EINVAL;
+	}
+	osalEnv.mpc[SVA].nbYramBanks   = cfgMpcYBanks_SVA;
+	osalEnv.mpc[SVA].eeId          = cfgSchedulerTypeHybrid_SVA ? HYBRID_EXECUTIVE_ENGINE : SYNCHRONOUS_EXECUTIVE_ENGINE;
+	osalEnv.mpc[SVA].sdramCodeSize = cfgMpcSDRAMCodeSize_SVA * ONE_KB;
+	osalEnv.mpc[SVA].sdramDataSize = cfgMpcSDRAMDataSize_SVA * ONE_KB;
+	osalEnv.mpc[SIA].nbYramBanks   = cfgMpcYBanks_SIA;
+	osalEnv.mpc[SIA].eeId          = cfgSchedulerTypeHybrid_SIA ? HYBRID_EXECUTIVE_ENGINE : SYNCHRONOUS_EXECUTIVE_ENGINE;
+	osalEnv.mpc[SIA].sdramCodeSize = cfgMpcSDRAMCodeSize_SIA * ONE_KB;
+	osalEnv.mpc[SIA].sdramDataSize = cfgMpcSDRAMDataSize_SIA * ONE_KB;
+
+	return 0;
+}
diff --git a/drivers/staging/nmf-cm/configuration.h b/drivers/staging/nmf-cm/configuration.h
new file mode 100644
index 00000000000..a3bc1ca9063
--- /dev/null
+++ b/drivers/staging/nmf-cm/configuration.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef CONFIGURATION_H
+#define CONFIGURATION_H
+
+/** Peripherals description.
+ * Some of these values are taken from kernel header description (which should be the
+ * right place of these definition); the missing ones are defined here.
+ */
+
+#include <linux/version.h>
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+#include <generated/autoconf.h>
+#else
+#include <linux/autoconf.h>
+#endif
+
+/** Nomadik embedded Static RAM base address*/
+#define ESRAM_BASE (U8500_ESRAM_BASE + 0x10000) // V1/V2 config: 0-64k: secure;
+#define HWSEM_BASE U8500_HSEM_BASE
+
+/** SxA base address */
+#define SVA_BASE_ADDR U8500_SVA_BASE
+#define SIA_BASE_ADDR U8500_SIA_BASE
+
+/** Nomadik embedded ram size for CM (in Kb) */
+#define ESRAM_SIZE 576
+enum {
+	ESRAM_12,
+	ESRAM_34,
+	NB_ESRAM,
+};
+
+/** MPCs */
+enum {
+	SVA,
+	SIA,
+	NB_MPC,
+};
+#define COREIDX(id) (id-1)
+
+/** Base address of shared SDRAM: use upper SDRAM. We reserve a rather */
+#define SDRAM_CODE_SIZE_SVA (2*ONE_KB)
+#define SDRAM_CODE_SIZE_SIA (2*ONE_KB)
+#define SDRAM_DATA_SIZE     (8*ONE_KB)
+
+extern int cfgCommunicationLocationInSDRAM;
+extern int cfgSemaphoreTypeHSEM;
+extern int cfgESRAMSize;
+
+int init_config(void);
+
+#define DECLARE_MPC_PARAM(mpc, sdramDataSize, extension, ybank)		\
+	static unsigned int cfgMpcYBanks_##mpc = ybank;			\
+	module_param(cfgMpcYBanks_##mpc, uint, S_IRUGO);		\
+	MODULE_PARM_DESC(cfgMpcYBanks_##mpc, "Nb of Y-Ram banks used on " #mpc); \
+									\
+	static int          cfgSchedulerTypeHybrid_##mpc = 1;		\
+	module_param(cfgSchedulerTypeHybrid_##mpc, bool, S_IRUGO);	\
+	MODULE_PARM_DESC(cfgSchedulerTypeHybrid_##mpc, "Scheduler used on " #mpc " (Hybrid or Synchronous)"); \
+									\
+	static unsigned int cfgMpcSDRAMCodeSize_##mpc = SDRAM_CODE_SIZE_##mpc; \
+	module_param(cfgMpcSDRAMCodeSize_##mpc, uint, S_IRUGO);		\
+	MODULE_PARM_DESC(cfgMpcSDRAMCodeSize_##mpc, "Size of code segment on " #mpc " (in Kb)"); \
+									\
+	static unsigned int cfgMpcSDRAMDataSize_##mpc = sdramDataSize;	\
+	module_param(cfgMpcSDRAMDataSize_##mpc, uint, S_IRUGO);		\
+	MODULE_PARM_DESC(cfgMpcSDRAMDataSize_##mpc, "Size of data segment on " #mpc " (in Kb)" extension)
+
+#endif
diff --git a/drivers/staging/nmf-cm/ee/api/ee.h b/drivers/staging/nmf-cm/ee/api/ee.h
new file mode 100644
index 00000000000..bde3cee9425
--- /dev/null
+++ b/drivers/staging/nmf-cm/ee/api/ee.h
@@ -0,0 +1,197 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+/*!
+ * \defgroup HOSTEE_MODULE Host Execution Engine
+ */
+
+/*!
+ * \defgroup HOSTEE Host Execution Engine API
+ * \ingroup HOSTEE_MODULE
+ */
+
+#ifndef HOST_EE_H
+#define HOST_EE_H
+
+
+#include <inc/typedef.h>
+
+#include <nmf/inc/channel_type.h>
+#include <nmf/inc/service_type.h>
+
+#include <ee/api/ee_type.h>
+
+/*!
+ * \brief Get the version of the NMF Host EE at runtime
+ *
+ * This method should be used to query the version number  of  the
+ * NMF Component Manager engine at runtime. This is useful when using
+ * to check if version of the engine linked with application correspond
+ * to engine used for development.
+ *
+ * Such code can be used to check compatibility: \code
+    t_uint32 nmfversion;
+
+    // Print NMF version
+    EE_GetVersion(&nmfversion);
+    LOG("NMF Version %d-%d-%d\n",
+            VERSION_MAJOR(nmfversion),
+            VERSION_MINOR(nmfversion),
+            VERSION_PATCH(nmfversion));
+    if(NMF_VERSION != nmfversion) {
+        LOG("Error: Incompatible API version %d != %d\n", NMF_VERSION, nmfversion);
+        EXIT();
+    }
+ * \endcode
+ *
+ * \param[out] version Internal hardcoded version (use \ref VERSION_MAJOR, \ref  VERSION_MINOR, \ref VERSION_PATCH  macros to decode it).
+ *
+ * \ingroup HOSTEE
+ */
+PUBLIC IMPORT_SHARED void EE_GetVersion(t_uint32 *version);
+
+/*!
+ * \brief Set the mode of the Host EE.
+ *
+ * According the (\ref t_ee_cmd_id) value, this routine allows to modify dynamically the behavior of the Host EE.
+ *
+ * \param[in] aCmdID Command ID.
+ * \param[in] aParam Parameter of command ID if required.
+ *
+ * \ingroup HOSTEE
+ */
+PUBLIC IMPORT_SHARED t_nmf_error EE_SetMode(t_ee_cmd_id aCmdID, t_sint32 aParam);
+
+/*!
+ * \brief Create a channel for communication between host ee and user.
+ *
+ * The purpose of the function is to:
+ * - create a channel or get a channel, regarding the flag parameter
+ *
+ * \param[in]  flags   Whether the caller want to create a new channel (CHANNEL_PRIVATE)
+ *                     or use the shared one (CHANNEL_PRIVATE) (it will be created
+ *                     if it does not yet exist)
+ * \param[out] channel Channel number.
+ *
+ * \exception NMF_NO_MORE_MEMORY Not enough memory to create the callback Channel.
+ * \exception NMF_INVALID_PARAMETER The specified flags is invalid.
+ *
+ * \ingroup HOSTEE
+ */
+PUBLIC IMPORT_SHARED t_nmf_error EE_CreateChannel(t_nmf_channel_flag flags, t_nmf_channel *channel);
+
+/*!
+ * \brief Flush a channel to allow user to safely close it.
+ *
+ * The purpose of the function is to allow safe call of EE_CloseChannel() later on. Calling
+ * EE_FlushChannel() will allow a blocking call to EE_GetMessage() to exit with an error
+ * NMF_FLUSH_MESSAGE. After EE_GetMessage() has exit with such a value user must no more
+ * call EE_GetMessage() and can safely call EE_CloseChannel() that will destroy channel.
+ * In case of the share channel EE_FlushChannel() will return false for isFlushMessageGenerated if
+ * it's internal reference counter is not zero, in that case no NMF_FLUSH_MESSAGE error is return 
+ * by EE_GetMessage() and user can immediatly call EE_CloseChannel().
+ * In case user know that no usage of channel is done when he want to destroy channel, call to this api
+ * is optionnal and user can safely call EE_CloseChannel().
+ *
+ * \param[in]  channel                  Channel number
+ * \param[out] isFlushMessageGenerated  Allow user to know if it must wait for NMF_FLUSH_MESSAGE return
+ *                                      of EE_GetMessage() before calling EE_CloseChannel()
+ *
+ * \exception NMF_INVALID_PARAMETER The specified flags is invalid.
+ *
+ * \ingroup HOSTEE
+ */
+PUBLIC IMPORT_SHARED t_nmf_error EE_FlushChannel(t_nmf_channel channel, t_bool *isFlushMessageGenerated);
+
+/*!
+ * \brief Unregister channel
+ *
+ * The purpose of the function is to:
+ * - destroy a channel from the user to the Host ee.
+ *
+ * The user must call EE_UserDone() as many time as EE_UserInit().
+ * At the last EE_UserDone() call, the channel is closed and definitely destroyed
+ * All service callback must be unregistered first.
+ *
+ * \param[in] channel    Channel number:
+ *
+ * \ingroup HOSTEE
+ */
+PUBLIC IMPORT_SHARED t_nmf_error EE_CloseChannel(t_nmf_channel channel);
+
+/*!
+ * \brief Register a service callback to this channel.
+ *
+ * \param[in] channel           The channel on which the callback must be registered.
+ * \param[in] handler           The given callback.
+ * \param[in] contextHandler    The context associated with this callback.
+ *
+ * \exception NMF_NO_MORE_MEMORY Not enough memory to associate service with the Channel.
+ *
+ * \ingroup HOSTEE
+ */
+PUBLIC IMPORT_SHARED t_nmf_error EE_RegisterService(t_nmf_channel channel, t_nmf_serviceCallback handler, void *contextHandler);
+
+/*!
+ * \brief Unregister a service callback from this channel.
+ *
+ * \param[in] channel           The channel on which the callback must be registered.
+ * \param[in] handler           The given callback.
+ * \param[in] contextHandler    The context associated with this callback.
+ *
+ * \exception NMF_INVALID_PARAMETER The channel or the callback doesn't exist.
+ *
+ * \ingroup HOSTEE
+ */
+PUBLIC IMPORT_SHARED t_nmf_error EE_UnregisterService(t_nmf_channel channel, t_nmf_serviceCallback handler, void *contextHandler);
+
+/*!
+ * \brief Unregister a notify callback for this channel.
+ *
+ * This method will register a callback that will be call each time a message has
+ * been push in queue of the channel.
+ * To unregister your callback just register a null notify.
+ *
+ * \param[in] channel           The channel on which the callback must be registered.
+ * \param[in] notify            The given callback.
+ * \param[in] contextHandler    The context associated with this callback.
+ *
+ * \ingroup HOSTEE
+ */
+PUBLIC IMPORT_SHARED t_nmf_error EE_RegisterNotify(t_nmf_channel channel, t_nmf_notify notify, void *contextHandler);
+
+/*!
+ * \brief Get received message from specified callback channel.
+ *
+ * This method can be used to retrieve callback message from Host ee. Returned message could then
+ * be dispatch through EE_ExecuteMessage.
+ *
+ * \param[in]   channel         The channel from which the message must be retrieved
+ * \param[out]  clientContext   client context.
+ * \param[out]  message         Reference on buffer to be unmarshalled. The buffer is allocated internally.
+ * \param[in]   blockable       Indicate if the call could blocked or not.
+ *
+ * \exception NMF_NO_MESSAGE            No waited message.
+ * \exception NMF_INVALID_PARAMETER     At least one input parameters is invalid
+ *
+ * \ingroup HOSTEE
+ */
+PUBLIC IMPORT_SHARED t_nmf_error EE_GetMessage(t_nmf_channel channel, void **clientContext, char **message, t_bool blockable);
+
+/*!
+ * \brief Execute a message. User callback will be execute.
+ *
+ * This method allow the message retrieved through EE_GetMessage to the right user callback.
+ *
+ * \param[in]   itfref          Interface reference.
+ * \param[in]   message         Reference on buffer to be unmarshalled.
+ *
+ * \ingroup HOSTEE
+ */
+PUBLIC IMPORT_SHARED void EE_ExecuteMessage(void *itfref, char *message);
+
+#endif
diff --git a/drivers/staging/nmf-cm/ee/api/ee_type.h b/drivers/staging/nmf-cm/ee/api/ee_type.h
new file mode 100644
index 00000000000..368cd6d29ba
--- /dev/null
+++ b/drivers/staging/nmf-cm/ee/api/ee_type.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef HOST_EE_TYPE_H
+#define HOST_EE_TYPE_H
+
+/*!
+ * \brief Notify callback method type.
+ *
+ * \ingroup HOSTEE
+ */
+typedef void (*t_nmf_notify)(void *contextHandler);
+
+/*!
+ * \brief Definition of the command ID type
+ */
+typedef t_uint32 t_ee_cmd_id;
+
+/*!
+ * \brief Definition of the command ID
+ */
+typedef enum {
+    EE_CMD_TRACE_ON,                //!< Enable tracing and force network resetting and dumping
+    EE_CMD_TRACE_OFF                //!< Disable tracing
+} t_ee_cmd_idDescription;
+
+
+#endif
diff --git a/drivers/staging/nmf-cm/ee/api/list.idt b/drivers/staging/nmf-cm/ee/api/list.idt
new file mode 100644
index 00000000000..a53a39734d8
--- /dev/null
+++ b/drivers/staging/nmf-cm/ee/api/list.idt
@@ -0,0 +1,18 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef LIST_IDT
+#define LIST_IDT
+
+/* linking elem */
+struct _t_list_link {
+    struct _t_list_link *pNext;
+};
+typedef struct _t_list_link t_list_link;
+
+#endif
+
diff --git a/drivers/staging/nmf-cm/ee/api/outnotify.h b/drivers/staging/nmf-cm/ee/api/outnotify.h
new file mode 100644
index 00000000000..c88185fac16
--- /dev/null
+++ b/drivers/staging/nmf-cm/ee/api/outnotify.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef OUT_IDT
+#define OUT_IDT
+
+typedef void (*t_nmf_notify)(void *contextHandler);
+
+#endif
+
diff --git a/drivers/staging/nmf-cm/ee/api/panic.idt b/drivers/staging/nmf-cm/ee/api/panic.idt
new file mode 100644
index 00000000000..612dfcc8b4b
--- /dev/null
+++ b/drivers/staging/nmf-cm/ee/api/panic.idt
@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \defgroup NMF_EE_TYPE Execution Engine Common Type Definitions
+ * \ingroup COMMON
+ */
+
+#ifndef __INC_PANIC_IDT
+#define __INC_PANIC_IDT
+
+/*!
+ * \brief Panic reason type
+ *
+ * For values, see \ref t_panic_reasonDescription.
+ *
+ * \ingroup NMF_EE_TYPE
+ */
+typedef t_uint8 t_panic_reason;
+
+/*!
+ * \brief The different panic reasons
+ *
+ * \verbatim
+ *  Reason                   | Information                       | Behavior
+ *  -------------------------------------------------------------------
+ *  INTERNAL_PANIC           | Not interpreted                   | Fatal panic, stop MPC
+ *  USER_STACK_OVERFLOW      | Faulting address & SPu            | Fatal panic, stop MPC
+ *  SYSTEM_STACK_OVERFLOW    | Faulting address &  SPu           | Fatal panic, stop MPC
+ *  UNALIGNED_LONG_ACCESS    | Indicative Faulting address & SPu | Fatal panic, stop MPC
+ *  EVENT_FIFO_OVERFLOW      | 0                                 | Abort current task, stop MPC
+ *  PARAM_FIFO_OVERFLOW      | 0                                 | idem
+ *  INTERFACE_NOT_BINDED     | 0                                 | idem
+ *  USER_PANIC               | Not interpreted                   | idem
+ *  UNBIND_INTERRUPT         | Interrupt number                  | Do nothing, just return from interrupt.
+ *  EVENT_FIFO_IN_USE        | Destroy event Fifo while event already schedule (only for HostEE)
+ * \endverbatim
+ *
+ * \ingroup NMF_EE_TYPE
+ */
+typedef enum {
+    INTERNAL_PANIC =                 1,
+    RESERVED_PANIC =                 2,
+    USER_STACK_OVERFLOW =            3,
+    SYSTEM_STACK_OVERFLOW =          4,
+    UNALIGNED_LONG_ACCESS =          5,
+    EVENT_FIFO_OVERFLOW =            6,
+    PARAM_FIFO_OVERFLOW =            7,
+    INTERFACE_NOT_BINDED =           8,
+    USER_PANIC =                     9,
+    UNBIND_INTERRUPT =               10,
+    EVENT_FIFO_IN_USE =              11
+} t_panic_reasonDescription;
+
+/*!
+ * \brief Define the source of the panic
+ *
+ * It indicates the source core of the panic message.\n
+ * It gives the member to use within \ref t_nmf_panic_data (which is a member of the t_nmf_service_data service data structure).
+
+ * \ingroup NMF_EE_TYPE
+ */
+typedef enum {
+	HOST_EE, //!< If the source is the Executive Engine running on the ARM Core
+	MPC_EE   //!< If the source is the Executive Engine running on one of the MPC Core
+} t_panic_source;
+
+#endif
diff --git a/drivers/staging/nmf-cm/ee/api/priority.idt b/drivers/staging/nmf-cm/ee/api/priority.idt
new file mode 100644
index 00000000000..a276ba57727
--- /dev/null
+++ b/drivers/staging/nmf-cm/ee/api/priority.idt
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef PRIORITY_IDT
+#define PRIORITY_IDT
+
+/**
+ * 0 : background
+ * 1 : normal
+ * 2 : urgent
+ * 3 :
+ * 4 : CScall
+ * 5 : CScall
+ * 6 : IO (not linked with priority ordering)
+ */
+#define MAX_SCHEDULER_PRIORITY_NUMBER                       7
+#define MAX_SCHEDULER_SUBPRIORITY_NUMBER					4
+
+#endif
+
diff --git a/drivers/staging/nmf-cm/inc/nmf-def.h b/drivers/staging/nmf-cm/inc/nmf-def.h
new file mode 100644
index 00000000000..345c8256212
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/nmf-def.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ /*!
+ * \brief NMF Version.
+ *
+ * This file contains the NMF Version.
+ *
+ * \defgroup NMF_VERSION NMF Version
+ * \ingroup COMMON
+ */
+
+#ifndef __INC_NMF_DEF_H
+#define __INC_NMF_DEF_H
+
+/*!
+ * \brief Current NMF version number
+ *
+ * \ingroup NMF_VERSION
+ */
+#define NMF_VERSION ((2 << 16) | (10 << 8) | (106))
+
+/*!
+ * \brief Get NMF major version corresponding to NMF version number
+ * \ingroup NMF_VERSION
+ */
+#define VERSION_MAJOR(version)  (((version) >> 16) & 0xFF)
+/*!
+ * \brief Get NMF minor version corresponding to NMF version number
+ * \ingroup NMF_VERSION
+ */
+#define VERSION_MINOR(version)  (((version) >> 8) & 0xFF)
+/*!
+ * \brief Get NMF patch version corresponding to NMF version number
+ * \ingroup NMF_VERSION
+ */
+#define VERSION_PATCH(version)  (((version) >> 0) & 0xFF)
+
+#endif /* __INC_NMF_DEF_H */
diff --git a/drivers/staging/nmf-cm/inc/nmf-limits.h b/drivers/staging/nmf-cm/inc/nmf-limits.h
new file mode 100644
index 00000000000..a942e542233
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/nmf-limits.h
@@ -0,0 +1,107 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Common Nomadik Multiprocessing Framework limits definition
+ *
+ * This file contains the limit definitions used into NMF.
+ *
+ * \warning Don't modify it since it is also hardcoded in tools
+ *
+ * \defgroup NMF_LIMITS NMF limits definition
+ * \ingroup COMMON
+ */
+#ifndef __INC_NMF_LIMITS_H
+#define __INC_NMF_LIMITS_H
+
+/*!
+ * \brief Maximum interface name length
+ *
+ * Define the maximum interface name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_INTERFACE_NAME_LENGTH               32
+
+/*!
+ * \brief Maximum interface method name length
+ *
+ * Define the maximum interface method name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_INTERFACE_METHOD_NAME_LENGTH        64
+
+/*!
+ * \brief Maximum interface type name length
+ *
+ * Define the maximum interface type name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_INTERFACE_TYPE_NAME_LENGTH          128
+
+
+/*!
+ * \brief Maximum template name length
+ *
+ * Define the maximum template name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_TEMPLATE_NAME_LENGTH                128
+
+/*!
+ * \brief Maximum component local name length
+ *
+ * Define the maximum component local name length inside a composite component allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_COMPONENT_NAME_LENGTH               32
+
+/*!
+ * \brief Maximum property name length
+ *
+ * Define the maximum property name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_PROPERTY_NAME_LENGTH                32
+
+/*!
+ * \brief Maximum property value length
+ *
+ * Define the maximum property value length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_PROPERTY_VALUE_LENGTH               128
+
+/*!
+ * \brief Maximum attribute name length
+ *
+ * Define the maximum attribute name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_ATTRIBUTE_NAME_LENGTH               32
+
+/*!
+ * \brief Maximum fifo size allowed for binding component
+ *
+ * Define the maximum fifo size allowed for binding component allowed by NMF when calling
+ * CM_BindComponentFromHost and CM_BindComponentAsynchronous.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_COMMUNICATION_FIFO_SIZE             256
+
+#define MAX_COMPONENT_FILE_PATH_LENGTH          1024
+
+#endif /* __INC_NMF_LIMITS_H */
diff --git a/drivers/staging/nmf-cm/inc/nmf-tracedescription.h b/drivers/staging/nmf-cm/inc/nmf-tracedescription.h
new file mode 100644
index 00000000000..9611803b978
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/nmf-tracedescription.h
@@ -0,0 +1,324 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief NMF xti/stm trace format description
+ *
+ * \defgroup NMF_TRACE_FORMAT NMF xti/stm trace format description
+ *
+ * The NMF trace is output by either xti ip on 8815 or stm ip on 8820 and 8500.
+ * Each type of trace is output on a dedicated channel. Following is a description
+ * of each of this traces.
+ *
+ * Traces have generally a timestamp added by hardware but is not described here.
+ * \ingroup NMF_ABI
+ */
+#ifndef TRACE_FORMAT_H_
+#define TRACE_FORMAT_H_
+
+#include <inc/nmf-limits.h>
+
+/*!
+ * \brief XTI/STM Channel where trace are dumped
+ *
+ * \note This type is only for defining constants, please not reference it.
+ *
+ * \note Ever if this format is able to be generated on same channel, Host EE & CM channel are separated
+ * in order to avoir concurrency and access STM IP without require mutual exclusion.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    MPC_EE_CHANNEL      = 100,                  //!< MPC EE channel (MPC activity) in 32bits bundle
+    CM_CHANNEL          = 101,                  //!< CM channel (MPC deployment) in 64bits bundle
+    HOST_EE_CHANNEL     = 151                   //!< Host EE channel (deployment & activity) in 64bits bundle
+} t_nmfTraceChannelDescription;
+
+/*!
+ * \brief Message trace type
+ *
+ * \note This type is only for defining constants, please not reference it.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_TYPE_RESET            = 1,                //!< Reset trace type
+    TRACE_TYPE_COMPONENT        = 2,                //!< Component instantiate trace type
+    TRACE_TYPE_BIND             = 3,                //!< Component bind trace type
+    TRACE_TYPE_METHOD           = 4,                //!< Component method trace type
+    TRACE_TYPE_ACTIVITY         = 5,                //!< Activity trace type
+    TRACE_TYPE_PANIC            = 6,                //!< Panic trace type
+    TRACE_TYPE_COMMUNICATION    = 7,                //!< Communication trace type
+    TRACE_TYPE_ALLOCATOR        = 8,                //!< Allocator trace type
+    TRACE_TYPE_ALLOC            = 9,                //!< Alloc trace type
+    TRACE_TYPE_USER             = 10                //!< User trace type
+} t_nmfTraceTypeDescription;
+
+#define TRACE_MAJOR_VERSION         1       //!< Current major trace version number \ingroup NMF_TRACE_FORMAT
+#define TRACE_MINOR_VERSION         2       //!< Current minor trace version number \ingroup NMF_TRACE_FORMAT
+
+/*!
+ * \brief Trace header description.
+ *
+ * \note XTI will add 64bits time-stamp in first field of this structure, but not generated by us !
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceChannelHeader {
+    // t_uint64           timeStamp;
+    t_uint8             traceType;          //!< Trace type
+    t_uint8             reserved;
+    t_uint16            traceSize;          //!< Trace size (depending on trace type description)
+};
+
+/*!
+ * \brief Trace header union
+ *
+ * The purpose of this is to optimize header setting in one instruction.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef union {
+    struct t_nmfTraceChannelHeader   s;
+    t_uint32                         v;
+} t_nmfTraceChannelHeaderUnion;
+
+
+/*!
+ * \brief Trace reset description
+ *
+ * Inform tools to reset their internal state because network will be dumped new time.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceReset {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16                       minorVersion;                        //!< NMF trace format minor version
+    t_uint16                       majorVersion;                        //!< NMF trace format major version
+};
+
+/**
+ * \brief Component instantiation trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_COMPONENT_COMMAND_ADD         = 0x1,
+    TRACE_COMPONENT_COMMAND_REMOVE      = 0x2
+} t_nmfTraceComponentCommandDescription;
+
+
+/*!
+ * \brief Component instantiation trace description
+ *
+ * Component instantiation trace is generated each time an instance of a component is added or removed.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceComponent {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceComponentCommandDescription
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA, in SMPEE: 0x1
+    t_uint32            componentContext;                               //!< Component context belonging domain (DSP this or ARM class this)
+    t_uint32            componentUserContext;                           //!< User friendly component Id belonging the channel (CM handle or ARM class this)
+    t_uint8             componentLocalName[MAX_COMPONENT_NAME_LENGTH];  //!< local name of component as given by user (null terminated)
+    t_uint8             componentTemplateName[MAX_TEMPLATE_NAME_LENGTH];//!< template name of component  (null terminated)
+};
+
+/**
+ * \brief Component binding trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_BIND_COMMAND_BIND_SYNCHRONOUS         = 0x1,
+    TRACE_BIND_COMMAND_UNBIND_SYNCHRONOUS       = 0x2,
+    TRACE_BIND_COMMAND_BIND_ASYNCHRONOUS        = 0x3,
+    TRACE_BIND_COMMAND_UNBIND_ASYNCHRONOUS      = 0x4
+} t_nmfTraceBindCommandDescription;
+
+/**
+ * \brief Component binding trace description
+ *
+ * \note clientComponentContext & serverComponentContext take value 0xffffffff when client or server are Component Manager.
+ * \note serverComponentContext take value 0x00000000 when binding to void.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceBind {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceBindCommandDescription
+    t_uint16            reserved;
+    t_uint16            clientDomainId;                                 //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA, in SMPEE: 0x1
+    t_uint16            serverDomainId;                                 //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA, in SMPEE: 0x1
+    t_uint32            clientComponentContext;                         //!< Component context belonging domain (DSP this or ARM class this)
+    t_uint32            serverComponentContext;                         //!< Component context belonging domain (DSP this or ARM class this)
+    t_uint8             requiredItfName[MAX_INTERFACE_NAME_LENGTH];     //!< Required interface name
+    t_uint8             providedItfName[MAX_INTERFACE_NAME_LENGTH];     //!< Provided interface name
+};
+
+/*!
+ * \brief Component interface method name trace description
+ *
+ * For each methods of each interfaces provided by a component, one such trace is dumped.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceMethod {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA, in SMPEE: 0x1
+    t_uint16            reserved;
+    t_uint32            methodId;                                       //!< Unique Method Id belonging the component
+    t_uint32            componentContext;                               //!< Component context belonging domain (DSP this or ARM class this)
+    t_uint8             methodName[MAX_INTERFACE_METHOD_NAME_LENGTH];   //!< Symbolic method name
+};
+
+/**
+ * \brief Activity trace trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_ACTIVITY_START                    = 0x1,                  //!< Start method
+    TRACE_ACTIVITY_END                      = 0x2,                  //!< End method
+    TRACE_ACTIVITY_POST                     = 0x3,                  //!< Post method
+    TRACE_ACTIVITY_CALL                     = 0x4,                  //!< Synchronous call method
+    TRACE_ACTIVITY_RETURN                   = 0x5                   //!< Synchronous return method
+} t_nmfTraceActivityCommandDescription;
+
+/*!
+ * \brief Execution Engine scheduling activity trace description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceActivity {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceActivityCommandDescription
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA
+    t_uint32            componentContext;                               //!< Unique component Id (Component Handle for CM, Component this for EE)
+    t_uint32            methodId;                                       //!< Unique Method Id belonging the component
+};
+
+/**
+ * \brief Component instantiation trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_COMMUNICATION_COMMAND_SEND         = 0x1,
+    TRACE_COMMUNICATION_COMMAND_RECEIVE      = 0x2
+} t_nmfTraceCommunicationCommandDescription;
+
+/**
+ * \brief Inter-processor communication signaling trace description
+ *
+ * Use when trigging interrupt through core.
+ *
+ * \note Not used on SMP EE
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceCommunication {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceCommunicationCommandDescription
+    t_uint16            reserved_0;
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA
+    t_uint16            remoteDomainId;                                 //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA
+};
+
+/**
+ * \brief Component instantiation trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_ALLOCATOR_COMMAND_CREATE      = 0x1,
+    TRACE_ALLOCATOR_COMMAND_DESTROY     = 0x2
+} t_nmfTraceAllocatorCommandDescription;
+
+/*!
+ * \brief Panic trace description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceAllocator {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceAllocatorCommandDescription
+    t_uint16            allocId;                                        //!< Memory allocator ID
+    t_uint32            size;                                           //!< Memory allocator size
+    t_uint8             name[32];                                       //!< Memory allocator name
+};
+
+/**
+ * \brief Component instantiation trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_ALLOC_COMMAND_ALLOC           = 0x1,
+    TRACE_ALLOC_COMMAND_FREE            = 0x2
+} t_nmfTraceAllocCommandDescription;
+
+/*!
+ * \brief Panic trace description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceAlloc {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceAllocatorCommandDescription
+    t_uint16            allocId;                                        //!< Memory allocator ID
+    t_uint32            offset;                                         //!< Memory chunk offet
+    t_uint32            size;                                           //!< Memory chunk size
+};
+
+/*!
+ * \brief Panic trace description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTracePanic {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            reason;                                         //!< See \ref t_panic_reason for description
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA
+    t_uint32            componentContext;                               //!< Unique component Id (Component Handle for CM, Component this for EE)
+    t_uint32            information1;                                   //!< Reason dependent information 1st
+    t_uint32            information2;                                   //!< Reason dependent information 2nd
+};
+
+/*!
+ * \brief User trace description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceUser {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint32            key;                                            //!< User key
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA
+    t_uint16            reserved;
+    t_uint32            componentContext;                               //!< Unique component Id (Component Handle for CM, Component this for EE)
+    t_uint32            callerAddress;                                  //!< Unique code address belonging the component
+};
+
+/*
+struct t_nmfTracePower{
+    struct t_nmfTraceChannelHeader   header;
+};
+*/
+
+#endif /* TRACE_FORMAT_H_ */
diff --git a/drivers/staging/nmf-cm/inc/nmf_type.idt b/drivers/staging/nmf-cm/inc/nmf_type.idt
new file mode 100644
index 00000000000..e8cc4e09946
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/nmf_type.idt
@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+#ifndef NMF_TYPE_H_
+#define NMF_TYPE_H_
+
+/*!
+ * \defgroup NMF_COMMON_TYPE NMF Common Type
+ * \ingroup COMMON
+ */
+
+/*!
+ * \brief Error type returned by NMF  API routines
+ *
+ * Possible value describe by \ref t_nmf_errorDescription
+ *
+ * \ingroup NMF_COMMON_TYPE
+ */
+typedef t_sint8 t_nmf_error;
+
+/*!
+ * \brief Error type values
+ *
+ * \ingroup NMF_COMMON_TYPE
+ */
+typedef enum {
+    NMF_OK =                                 0,        //!< No error
+    NMF_INVALID_PARAMETER =                  -2,       //!< Invalid parameter
+    NMF_NO_MORE_MEMORY =                     -30,      //!< Out of memory
+    NMF_INTERFACE_NOT_BINDED =               -59,      //!< Try to unbind not binded interface
+    NMF_INTERFACE_ALREADY_BINDED =           -60,      //!< Try to bind already binded interface
+    NMF_NO_SUCH_REQUIRED_INTERFACE =         -63,      //!< Interface name not required by a component
+    NMF_NO_SUCH_PROVIDED_INTERFACE =         -64,      //!< Interface name not provided by a component
+    NMF_COMPONENT_NOT_STOPPED =              -80,      //!< Component must be stopped to perform operation
+    NMF_INVALID_COMPONENT_STATE_TRANSITION = -81,      //!< Invalid component state transition caused by user action
+    NMF_NO_SUCH_PROPERTY =                   -87,      //!< Property name doesn't exported by the underlying component
+    NMF_NO_SUCH_ATTRIBUTE =                  -88,      //!< Attribute name not shared (exported) by a component
+    NMF_NO_MESSAGE =                         -103,     //!< No message available
+    NMF_FLUSH_MESSAGE =                      -106,     //!< Message send after call to EE_FlushChannel()
+    NMF_INTEGRATION_ERROR0 =                 -112,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR1 =                 -113,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR2 =                 -114,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR3 =                 -115,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR4 =                 -116,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR5 =                 -117,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR6 =                 -118,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR7 =                 -119,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR8 =                 -120,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR9 =                 -121      //!< OS dependent integration Error [-112 -> -121]
+} t_nmf_errorDescription;
+
+/*!
+ * \brief Define t_nmf_channel type that identify a communication channel between nmf and user.
+ *
+ * \ingroup NMF_COMMON_TYPE
+ */
+typedef t_uint32 t_nmf_channel;
+
+#endif /* NMF_TYPE_H_ */
diff --git a/drivers/staging/nmf-cm/inc/type.h b/drivers/staging/nmf-cm/inc/type.h
new file mode 100644
index 00000000000..d6eafe9aea5
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/type.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/* inc/type.h - Programming Model.
+ *
+ * Copyright (c) 2006, 2007, 2008 STMicroelectronics.
+ *
+ * Reproduction and Communication of this document is strictly prohibited
+ * unless specifically authorized in writing by STMicroelectronics.
+ *
+ * Written by NMF team.
+ */
+#ifndef _NMF_TYPE_H_
+#define _NMF_TYPE_H_
+
+#include <inc/typedef.h>
+
+PUBLIC IMPORT_SHARED void NMF_LOG(const char* fmt, ...);
+PUBLIC IMPORT_SHARED void NMF_PANIC(const char* fmt, ...);
+
+#define NMF_ASSERT(cond)  do { if(!(cond)) NMF_PANIC("NMF_ASSERT at %s:%d\n", (int)__FILE__, (int)__LINE__); } while(0)
+
+#ifndef EXPORT_NMF_COMPONENT
+    #define EXPORT_NMF_COMPONENT    EXPORT_SHARED
+#endif
+
+#ifndef IMPORT_NMF_COMPONENT
+    #define IMPORT_NMF_COMPONENT    IMPORT_SHARED
+#endif
+
+#endif /* _NMF_TYPE_H_ */
diff --git a/drivers/staging/nmf-cm/inc/typedef.h b/drivers/staging/nmf-cm/inc/typedef.h
new file mode 100644
index 00000000000..97af9dec2c2
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/typedef.h
@@ -0,0 +1,193 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \defgroup COMMON Common types and definitions
+ *
+ * \defgroup NMF_COMMON NMF common definition
+ * \ingroup COMMON
+ *
+ * \defgroup NMF_ABI NMF ABI specification
+ * \warning This page is not for multimedia developers !
+ */
+/*!
+ * \brief Primitive Type Definition
+ *
+ * \defgroup NMF_PRIMITIVE_TYPE Primitive type definition
+ * \ingroup COMMON
+ */
+
+#ifndef NMF_TYPEDEF_H_
+#define NMF_TYPEDEF_H_
+
+#undef PRIVATE
+#define PRIVATE  static                 //!< Private macro declaration \ingroup NMF_PRIMITIVE_TYPE
+
+#undef PUBLIC
+#ifdef __cplusplus
+#define PUBLIC extern "C"               //!< Public macro declaration \ingroup NMF_PRIMITIVE_TYPE
+#else
+#define PUBLIC extern                   //!< Public macro declaration \ingroup NMF_PRIMITIVE_TYPE
+#endif
+
+#if defined(__SYMBIAN32__)
+/*!
+ * \brief Declared IMPORT_SHARED to allow dll/shared library creation
+ *
+ * \note Value depend on OS.
+ *
+ * \ingroup NMF_PRIMITIVE_TYPE
+ */
+    #ifndef IMPORT_SHARED
+        #define IMPORT_SHARED   IMPORT_C
+    #endif
+/*!
+ * \brief Declared EXPORT_SHARED to allow dll/shared library creation
+ *
+ * \note Value depend on OS.
+ *
+ * \ingroup NMF_PRIMITIVE_TYPE
+ */
+    #ifndef EXPORT_SHARED
+        #define EXPORT_SHARED   EXPORT_C
+    #endif
+#elif defined(LINUX)
+    #ifndef IMPORT_SHARED
+        #define IMPORT_SHARED
+    #endif
+    #ifndef EXPORT_SHARED
+        #define EXPORT_SHARED   __attribute__ ((visibility ("default")))
+    #endif
+#else
+    #ifndef IMPORT_SHARED
+        #define IMPORT_SHARED
+    #endif
+
+    #ifndef EXPORT_SHARED
+        #define EXPORT_SHARED
+    #endif
+#endif
+
+/*
+ * Definition of type that are used by interface.
+ */
+
+typedef unsigned int t_uword;
+typedef signed int t_sword;
+
+#ifdef __flexcc2__
+
+typedef unsigned char t_bool;
+
+#ifdef __mode16__
+
+typedef signed char t_sint8;
+typedef signed int t_sint16;
+typedef signed long t_sint24;
+typedef signed long t_sint32;
+typedef signed long long t_sint40;
+// bigger type are not handle on this mode
+
+typedef unsigned char t_uint8;
+typedef unsigned int t_uint16;
+typedef unsigned long t_uint24;
+typedef unsigned long t_uint32;
+typedef unsigned long long t_uint40;
+// bigger type are not handle on this mode
+
+// shared addr type definition
+//typedef __SHARED16 t_uint16 * t_shared_addr;
+typedef void * t_shared_field;
+
+#else /* __mode16__ -> __mode24__ */
+
+typedef signed char t_sint8;
+typedef signed short t_sint16;
+typedef signed int t_sint24;
+typedef signed long t_sint32;
+typedef signed long t_sint40;
+typedef signed long t_sint48;
+typedef signed long long t_sint56;
+
+typedef unsigned char t_uint8;
+typedef unsigned short t_uint16;
+typedef unsigned int t_uint24;
+typedef unsigned long t_uint32;
+typedef unsigned long t_uint40;
+typedef unsigned long t_uint48;
+typedef unsigned long long t_uint56;
+
+// shared addr type definition
+//typedef __SHARED16 t_uint16 * t_shared_addr;
+typedef t_uint24 t_shared_field;
+
+#endif /* MMDSP mode24 */
+
+// shared register (ARM world) type definition
+#if 0
+typedef struct {
+    t_uint16 lsb;
+    t_uint16 msb;
+} t_shared_reg;
+#endif
+typedef t_uint32 t_shared_reg;
+
+typedef t_uint32 t_physical_address;
+
+#include <stwdsp.h>
+
+#else /* __flexcc2__ -> RISC 32 Bits */
+
+#ifndef _HCL_DEFS_H
+typedef unsigned char t_bool;                   //!< Boolean primitive type \ingroup NMF_PRIMITIVE_TYPE
+
+typedef unsigned char t_uint8;                  //!< Unsigned 8 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef signed char t_sint8;                    //!< Signed 8 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef unsigned short t_uint16;                //!< Unsigned 16 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef signed short t_sint16;                  //!< Signed 16 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef unsigned long t_uint32;                 //!< Unsigned 32 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef signed long t_sint32;                   //!< Signed 32 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef unsigned long long t_uint64;            //!< Unsigned 64 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef signed long long t_sint64;              //!< Signed 64 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+
+typedef t_uint32 t_physical_address;
+#endif /* _HCL_DEFS_H */
+
+typedef unsigned long t_uint24;
+typedef signed long t_sint24;
+typedef unsigned long long t_uint48;
+typedef signed long long t_sint48;
+
+// shared addr type definition
+typedef t_uint32 t_shared_addr;
+
+// shared register (ARM world) type definition
+typedef t_uint32 t_shared_reg;
+typedef t_uint32 t_shared_field;
+
+#endif /* RISC 32 Bits */
+
+/*
+ * Define boolean type
+ */
+#undef FALSE
+#define FALSE  0                                            //!< Boolean FALSE value
+#undef TRUE
+#define TRUE  1                                             //!< Boolean TRUE value
+
+#ifndef NULL
+    #if defined __flexcc2__ || defined __SYMBIAN32__
+        #define NULL (0x0)                                  //!< Null type \ingroup NMF_PRIMITIVE_TYPE
+    #else
+        #define NULL ((void*)0x0)                           //!< Null type \ingroup NMF_PRIMITIVE_TYPE
+    #endif
+#endif
+
+typedef t_uint32 t_nmf_component_handle;
+
+#endif /* NMF_TYPEDEF_H_ */
diff --git a/drivers/staging/nmf-cm/nmf/inc/channel_type.h b/drivers/staging/nmf-cm/nmf/inc/channel_type.h
new file mode 100644
index 00000000000..7d439ebf0cf
--- /dev/null
+++ b/drivers/staging/nmf-cm/nmf/inc/channel_type.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Common Nomadik Multiprocessing Framework type definition
+ *
+ * This file contains the shared between cm and ee type definitions used into NMF for callback.
+ */
+/*!
+ * \defgroup _t_nmf_channel_flag t_nmf_channel_flag
+ * \ingroup NMF_COMMON
+ */
+
+#ifndef __INC_CHANNEL_TYPE_H
+#define __INC_CHANNEL_TYPE_H
+
+#include <inc/typedef.h>
+#include <inc/nmf_type.idt>
+
+/*!
+ * \brief Define t_nmf_channel_flag type that allow to control if/how a new communication channel is created.
+ * \ingroup _t_nmf_channel_flag
+ */
+typedef t_uint32 t_nmf_channel_flag;
+
+#define NMF_CHANNEL_SHARED                      ((t_nmf_channel_flag)0) //!< \ingroup _t_nmf_channel_flag
+#define NMF_CHANNEL_PRIVATE                     ((t_nmf_channel_flag)1) //!< \ingroup _t_nmf_channel_flag
+
+/*!
+ * \brief Define t_nmf_virtualInterruptHandler function type to allow to dispatch virtual interrupt
+ * \ingroup VIRTUAL_INTERRUPT
+ */
+typedef void (*t_nmf_virtualInterruptHandler)(void *interruptContext); 
+
+#endif
+
diff --git a/drivers/staging/nmf-cm/nmf/inc/component_type.h b/drivers/staging/nmf-cm/nmf/inc/component_type.h
new file mode 100644
index 00000000000..08b63b21225
--- /dev/null
+++ b/drivers/staging/nmf-cm/nmf/inc/component_type.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Common Nomadik Multiprocessing Framework type definition
+ *
+ * This file contains the shared between cm and ee type definitions used into NMF for callback.
+ */
+#ifndef __INC_COMPONENT_TYPE_H
+#define __INC_COMPONENT_TYPE_H
+
+#include <inc/typedef.h>
+
+/*!
+ * \brief Identifier of a component instance handle
+ *
+ * \ingroup NMF_COMMON
+ */
+typedef t_nmf_component_handle t_cm_instance_handle;
+
+#endif
+
diff --git a/drivers/staging/nmf-cm/nmf/inc/service_type.h b/drivers/staging/nmf-cm/nmf/inc/service_type.h
new file mode 100644
index 00000000000..3e5473338ee
--- /dev/null
+++ b/drivers/staging/nmf-cm/nmf/inc/service_type.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Service type and data used through service callback.
+ * \defgroup NMF_SERVICE NMF Service Callback types and data definition
+ * \ingroup NMF_COMMON
+ */
+#ifndef SERVICE_TYPE_H
+#define SERVICE_TYPE_H
+
+#include <ee/api/panic.idt>
+#include <nmf/inc/component_type.h>
+#include <share/inc/nmf.h>
+
+/*!
+ * \brief Define t_nmf_service_type type
+ *
+ * It gives the type of service message passed to service callback.
+ * \ingroup NMF_SERVICE
+ */
+typedef t_uint32 t_nmf_service_type;
+#define NMF_SERVICE_PANIC                          ((t_nmf_service_type)0) //!< \ingroup NMF_SERVICE
+#define NMF_SERVICE_SHUTDOWN                       ((t_nmf_service_type)1) //!< \ingroup NMF_SERVICE
+
+/*
+ * The following structured define each data structure used for each service type
+ * and given to each serviceCallback
+ */
+
+/*!
+ * \brief Define t_nmf_panic_data type
+ *
+ * This is the data structure passed to the service callback (inside \ref t_nmf_service_data)
+ * when t_nmf_service_type == NMF_SERVICE_PANIC
+ * \ingroup NMF_SERVICE
+ */
+typedef struct {
+	t_panic_reason       panicReason; //!< The reason of the panic
+	t_panic_source       panicSource; //!< THe source of the panic (One of the MPC or the ARM-EE)
+	/*!
+	 * union of structures containing specific info, depending on the panicSource
+	 */
+	union {
+		struct {
+			t_nmf_core_id        coreid; //!< The coreId of the MPC on which the panic occured
+			t_cm_instance_handle faultingComponent; //!< The faulting component handle
+			t_uint32             panicInfo1; //!< First info (depend on \ref panicReason)
+			t_uint32             panicInfo2; //!< Second info (depend on \ref panicReason)
+		} mpc; //!< member to use if panicSource == MPC_EE
+		struct {
+			void *               faultingComponent; //!< The faulting component handle
+			t_uint32             panicInfo1; //!< First info (depend on \ref panicReason)
+			t_uint32             panicInfo2; //!< Second info (depend on \ref panicReason)
+		} host; //!< member to use if panicSource == HOST_EE
+	} info; //!< union of structures containing specific info, depending on the panicSource
+} t_nmf_panic_data;
+
+/*!
+ * \brief Define t_nmf_shutdown_data type
+ *
+ * This is the data structure passed to the service callback (inside \ref t_nmf_service_data)
+ * when t_nmf_service_type == NMF_SERVICE_SHUTDOWN
+ * \ingroup NMF_SERVICE
+ */
+typedef struct {
+    t_nmf_core_id        coreid; //!< The coreId of the MPC on which has been shutdown
+} t_nmf_shutdown_data;
+
+/*!
+ * \brief Define t_nmf_service_data type
+ *
+ * It gives the data passed to the service callbacks for each service type
+ * This is an union whose member to use is defined by the given \ref t_nmf_service_type
+ *
+ * \ingroup NMF_SERVICE
+ */
+typedef union {
+	t_nmf_panic_data panic; //!< if service_type == NMF_SERVICE_PANIC
+    t_nmf_shutdown_data shutdown; //!< if service_type == NMF_SERVICE_SHUTDOWN
+} t_nmf_service_data;
+
+/*!
+ * \brief Define t_nmf_serviceCallback function type to allow to dispatch service message to user.
+ * \ingroup NMF_SERVICE
+ */
+typedef void (*t_nmf_serviceCallback)(void *contextHandler, t_nmf_service_type serviceType, t_nmf_service_data *serviceData);
+
+#endif //SERVICE_TYPE_H
diff --git a/drivers/staging/nmf-cm/osal-kernel.c b/drivers/staging/nmf-cm/osal-kernel.c
new file mode 100644
index 00000000000..8f547ba0ad9
--- /dev/null
+++ b/drivers/staging/nmf-cm/osal-kernel.c
@@ -0,0 +1,1162 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/** \file osal-kernel.c
+ * 
+ * Implements NMF OSAL for Linux kernel-space environment
+ */
+
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kthread.h>
+#include <linux/mm.h>
+#include <linux/semaphore.h>
+#include <linux/slab.h>
+#include <linux/timer.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+#include <mach/prcmu.h>
+
+#include <trace/stm.h>
+
+#include <cm/engine/configuration/inc/configuration_status.h>
+
+#include "cmioctl.h"
+#include "osal-kernel.h"
+#include "cm_service.h"
+#include "cmld.h"
+
+__iomem void *prcmu_base = NULL;
+__iomem void *prcmu_tcdm_base = NULL;
+
+/* DSP Load Monitoring */
+#define FULL_OPP 100
+#define HALF_OPP 50
+static unsigned long running_dsp = 0;
+static unsigned int dspLoadMonitorPeriod = 1000;
+module_param(dspLoadMonitorPeriod, uint, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(dspLoadMonitorPeriod, "Period of the DSP-Load monitoring in ms");
+static unsigned int dspLoadHighThreshold = 85;
+module_param(dspLoadHighThreshold, uint, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(dspLoadHighThreshold, "Threshold above which 100 APE OPP is requested");
+static unsigned int dspLoadLowThreshold = 35;
+module_param(dspLoadLowThreshold, uint, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(dspLoadLowThreshold, "Threshold below which 100 APE OPP request is removed");
+
+/** \defgroup ENVIRONMENT_INITIALIZATION Environment initialization
+ * Includes functions that initialize the Linux OSAL itself plus functions that
+ * are responsible to factor configuration objects needed to initialize Component Manager library
+ */
+
+/** \defgroup OSAL_IMPLEMENTATION OSAL implementation
+ *	 Linux-specific implementation of the Component Manager OSAL interface.
+ */
+
+
+/** \ingroup ENVIRONMENT_INITIALIZATION
+ * Remaps IO, SDRAM and ESRAM regions
+ *
+ * \osalEnvironment NMF-Osal descriptor
+ * \return POSIX error code 
+ */
+int remapRegions(void)
+{
+	unsigned i;
+
+	/* Remap DSP base areas */
+	for (i=0; i<NB_MPC; i++) {
+		osalEnv.mpc[i].baseL = ioremap_nocache((int)osalEnv.mpc[i].baseP, (1*ONE_MB));
+		if(osalEnv.mpc[i].baseL == NULL){
+			pr_err("%s: could not remap base address for %s\n", __func__, osalEnv.mpc[i].name);
+			return -ENOMEM;
+		}
+	}
+
+	/* Remap hardware semaphores */
+	osalEnv.hwsem_base = ioremap_nocache(HWSEM_BASE, (4*ONE_KB));
+	if(osalEnv.hwsem_base == NULL){
+		pr_err("%s: could not remap HWSEM Base\n", __func__);
+		return -ENOMEM;
+	}
+
+	/* Remap _all_ ESRAM banks */
+	osalEnv.esram_base = ioremap_nocache(ESRAM_BASE, cfgESRAMSize*ONE_KB);
+	if(osalEnv.esram_base == NULL){
+		pr_err("%s: could not remap ESRAM Base\n", __func__);
+		return -ENOMEM;
+	}
+	memset(osalEnv.esram_base, 0x0, cfgESRAMSize*ONE_KB);
+
+
+	/* Allocate code and data sections for MPC (SVA, SIA) */
+        for (i=0; i<NB_MPC; i++) {
+		/* Allocate MPC SDRAM code area */
+		struct hwmem_mem_chunk mem_chunk;
+		size_t mem_chunk_length;
+		osalEnv.mpc[i].hwmemCode = hwmem_alloc(osalEnv.mpc[i].sdramCodeSize,
+						       //HWMEM_ALLOC_HINT_CACHE_WB,
+						       HWMEM_ALLOC_HINT_WRITE_COMBINE | HWMEM_ALLOC_HINT_UNCACHED,
+						       HWMEM_ACCESS_READ | HWMEM_ACCESS_WRITE,
+						       HWMEM_MEM_CONTIGUOUS_SYS);
+		if (IS_ERR(osalEnv.mpc[i].hwmemCode)) {
+			int err = PTR_ERR(osalEnv.mpc[i].hwmemCode);
+			osalEnv.mpc[i].hwmemCode = NULL;
+			pr_err("%s: could not allocate SDRAM Code for %s\n",
+			       __func__, osalEnv.mpc[i].name);
+			return err;
+		}
+		osalEnv.mpc[i].sdramCodeL = hwmem_kmap(osalEnv.mpc[i].hwmemCode);
+		if (IS_ERR(osalEnv.mpc[i].sdramCodeL)) {
+			int err = PTR_ERR(osalEnv.mpc[i].sdramCodeL);
+			osalEnv.mpc[i].sdramCodeL = NULL;
+			pr_err("%s: could not map SDRAM Code for %s\n", __func__, osalEnv.mpc[i].name);
+			return err;
+		}
+		mem_chunk_length = 1;
+		(void)hwmem_pin(osalEnv.mpc[i].hwmemCode, &mem_chunk, &mem_chunk_length);
+		osalEnv.mpc[i].sdramCodeP = mem_chunk.paddr;
+		/* Allocate MPC SDRAM data area by taking care wether the data are shared or not */
+		if (osalEnv.mpc[i].sdramDataSize == 0) {
+			/* size of 0 means shared data segment, reuse the same param as for first MPC */
+			osalEnv.mpc[i].sdramDataP    = osalEnv.mpc[0].sdramDataP;
+			osalEnv.mpc[i].sdramDataL    = osalEnv.mpc[0].sdramDataL;
+			osalEnv.mpc[i].sdramDataSize = osalEnv.mpc[0].sdramDataSize;
+		} else {
+			/* If we do not share the data segment or if this is the first MPC */
+			osalEnv.mpc[i].hwmemData = hwmem_alloc(osalEnv.mpc[i].sdramDataSize,
+							       HWMEM_ALLOC_HINT_WRITE_COMBINE | HWMEM_ALLOC_HINT_UNCACHED,
+							       HWMEM_ACCESS_READ | HWMEM_ACCESS_WRITE,
+							       HWMEM_MEM_CONTIGUOUS_SYS);
+			if (IS_ERR(osalEnv.mpc[i].hwmemData)) {
+				int err = PTR_ERR(osalEnv.mpc[i].hwmemData);
+				osalEnv.mpc[i].hwmemData = NULL;
+				pr_err("%s: could not allocate SDRAM Data for %s\n",
+				       __func__, osalEnv.mpc[i].name);
+				return err;
+			}
+			mem_chunk_length = 1;
+			(void)hwmem_pin(osalEnv.mpc[i].hwmemData,
+					&mem_chunk, &mem_chunk_length);
+			osalEnv.mpc[i].sdramDataP = mem_chunk.paddr;
+			osalEnv.mpc[i].sdramDataL = hwmem_kmap(osalEnv.mpc[i].hwmemData);
+			if (IS_ERR(osalEnv.mpc[i].sdramDataL)) {
+				int err = PTR_ERR(osalEnv.mpc[i].sdramDataL);
+				osalEnv.mpc[i].sdramDataL = NULL;
+				pr_err("%s: could not map SDRAM Data for %s\n",
+				       __func__, osalEnv.mpc[i].name);
+				return err;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/** \ingroup ENVIRONMENT_INITIALIZATION
+ * Unmaps IO, SDRAM and ESRAM regions
+ *
+ * \return POSIX error code 
+ */
+void unmapRegions(void)
+{
+	unsigned i;
+
+	/* Release SVA, SIA, Hardware sempahores and embedded SRAM mappings */
+	for (i=0; i<NB_MPC; i++) {
+		if(osalEnv.mpc[i].baseL != NULL)
+			iounmap(osalEnv.mpc[i].baseL);
+	}
+
+	if(osalEnv.hwsem_base != NULL)
+		iounmap(osalEnv.hwsem_base);
+	
+	if(osalEnv.esram_base != NULL)
+		iounmap(osalEnv.esram_base);
+
+	/*
+	 * Free SVA and SIA code and data sections or release their mappings
+	 * according on how memory allocations has been achieved
+	 */
+	for (i=0; i<NB_MPC; i++) {
+		if (osalEnv.mpc[i].sdramCodeL != NULL) {
+			hwmem_unpin(osalEnv.mpc[i].hwmemCode);
+			hwmem_kunmap(osalEnv.mpc[i].hwmemCode);
+			if (osalEnv.mpc[i].hwmemCode != NULL)
+				hwmem_release(osalEnv.mpc[i].hwmemCode);
+		}
+
+		/* If data segment is shared, we must free only the first data segment */
+		if (((i == 0) || (osalEnv.mpc[i].sdramDataL != osalEnv.mpc[0].sdramDataL))
+		    && (osalEnv.mpc[i].sdramDataL != NULL)) {
+			hwmem_unpin(osalEnv.mpc[i].hwmemData);
+			hwmem_kunmap(osalEnv.mpc[i].hwmemData);
+			if (osalEnv.mpc[i].hwmemData != NULL)
+				hwmem_release(osalEnv.mpc[i].hwmemData);
+		}
+	}
+}
+
+
+/** \ingroup ENVIRONMENT_INITIALIZATION
+ *	Fills a t_nmf_hw_mapping_desc object
+ *
+ * \param nmfHwMappingDesc Pointer to a t_nmf_hw_mapping_desc object
+ * \return POSIX error code
+ */
+int getNmfHwMappingDesc(t_nmf_hw_mapping_desc* nmfHwMappingDesc)
+{
+
+	if (nmfHwMappingDesc == NULL)
+		return -ENXIO;
+
+	nmfHwMappingDesc->esramDesc.systemAddr.physical = ESRAM_BASE;
+	nmfHwMappingDesc->esramDesc.systemAddr.logical = (t_cm_logical_address)osalEnv.esram_base;
+	nmfHwMappingDesc->esramDesc.size = cfgESRAMSize*ONE_KB;
+
+	nmfHwMappingDesc->hwSemaphoresMappingBaseAddr.physical = HWSEM_BASE;
+	nmfHwMappingDesc->hwSemaphoresMappingBaseAddr.logical = (t_cm_logical_address)osalEnv.hwsem_base;
+
+	return 0;
+}
+
+/**  \ingroup ENVIRONMENT_INITIALIZATION
+ * Fills a t_cm_system_address object
+ *
+ * \param mpcSystemAddress Pointer to a t_cm_system_address object
+ * \return POSIX error code
+ */
+void getMpcSystemAddress(unsigned i, t_cm_system_address* mpcSystemAddress)
+{
+	mpcSystemAddress->physical = (t_cm_physical_address)osalEnv.mpc[i].baseP;
+	mpcSystemAddress->logical  = (t_cm_logical_address)osalEnv.mpc[i].baseL;
+}
+
+
+/** \ingroup ENVIRONMENT_INITIALIZATION
+ *	Fills t_nmf_memory_segment objects for MPC code and data segments
+ *
+ * \param i           Index of the MPC to initialize
+ * \param codeSegment Pointer to a t_nmf_memory_segment (code segment)
+ * \param dataSegment Pointer to a t_nmf_memory_segment (data segment)
+ * \return Always 0
+ */
+void getMpcSdramSegments(unsigned i, t_nmf_memory_segment* codeSegment, t_nmf_memory_segment* dataSegment)
+{
+	codeSegment->systemAddr.logical = (t_cm_logical_address)osalEnv.mpc[i].sdramCodeL;
+	codeSegment->systemAddr.physical = osalEnv.mpc[i].sdramCodeP;
+	codeSegment->size = osalEnv.mpc[i].sdramCodeSize;
+
+	dataSegment->systemAddr.logical = (t_cm_logical_address)osalEnv.mpc[i].sdramDataL;
+	dataSegment->systemAddr.physical = osalEnv.mpc[i].sdramDataP;
+	dataSegment->size = osalEnv.mpc[i].sdramDataSize;
+}
+
+#ifdef CM_DEBUG_ALLOC
+#include <linux/kallsyms.h>
+struct cm_alloc cm_alloc;
+
+/**
+ * These routines initializes the structures used to trace all alloc/free.
+ * These are used in debug mode to track all memory leak about the allocations
+ * done through the OSAL.
+ */
+void init_debug_alloc(void)
+{
+	INIT_LIST_HEAD(&cm_alloc.chain);
+	spin_lock_init(&cm_alloc.lock);
+}
+
+void cleanup_debug_alloc(void)
+{
+	struct cm_alloc_elem *entry, *next;
+	char buffer[128];
+
+	list_for_each_entry_safe(entry, next, &cm_alloc.chain, elem) {
+		sprint_symbol(buffer, (int)entry->caller);
+		pr_err("/!\\ ALLOC(size=%d) not freed from: 0x%p (%s)\n",
+		       entry->size, entry->caller, buffer);
+		list_del(&entry->elem);
+		if ((void*)entry >= (void*)VMALLOC_START
+		    && (void*)entry < (void*)VMALLOC_END)
+			vfree(entry);
+		else
+			kfree(entry);
+	}
+}
+
+void dump_debug_alloc(void)
+{
+	struct cm_alloc_elem *entry, *next;
+	char buffer[128];
+
+	pr_err("Current allocated memory:\n");
+	list_for_each_entry_safe(entry, next, &cm_alloc.chain, elem) {
+		sprint_symbol(buffer, (int)entry->caller);
+		pr_err("=> Alloc of size=%d from: 0x%p (%s)\n",
+		       entry->size, entry->caller, buffer);
+	}
+}
+#endif
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ * Called by CM_ProcessMpcEvent in interrupt/tasklet context. Schedules the DFC. 
+ * Enqueues the new event in the process' message queue.
+ *
+ * \note This is _not_ called in response to internal events such as in
+ * response to a CM_InstantiateComponent. It is called when user-defined
+ * functions need to be called in skeletons. This behavior is different
+ * from 0.8.1 version.
+ */
+void OSAL_PostDfc(t_nmf_mpc2host_handle upLayerTHIS, t_uint32 methodIndex, t_event_params_handle ptr, t_uint32 size)
+{
+	/* skelwrapper has been created in CM_SYSCALL_BindComponentToCMCore and conveys per-process private data */
+	t_skelwrapper* skelwrapper = (t_skelwrapper*)upLayerTHIS;
+	struct osal_msg* message;
+
+	/* If the clannel has been closed, no more reader exists
+	   => discard the message */
+	if (skelwrapper->channelPriv->state == CHANNEL_CLOSED) {
+		pr_warning("%s: message discarded (channel closed)\n",
+			   __func__ );
+		return;
+	}
+
+	/* Create a new message */
+	message = kmalloc(sizeof(*message), GFP_ATOMIC);
+	if (!message) {
+		pr_err("%s: message discarded (alloc failed)\n", __func__ );
+		return;
+	}
+
+	/* Stuff it */
+	plist_node_init(&message->msg_entry, 0);
+	message->msg_type = MSG_INTERFACE;
+	message->d.itf.skelwrap = skelwrapper;
+	message->d.itf.methodIdx = methodIndex;
+	message->d.itf.anyPtr = ptr;
+	message->d.itf.ptrSize = size;
+
+	/* Enqueue it */
+	/* Should be protected with the cmPriv->msgQueueLock held
+	   But we know by design that we are safe here. (Alone here in
+	   tasklet (soft-interrupt) context.
+	   When accessed in process context, soft-irq are disable)
+	*/
+	spin_lock_bh(&skelwrapper->channelPriv->bh_lock);
+	plist_add(&message->msg_entry, &skelwrapper->channelPriv->messageQueue);
+	spin_unlock_bh(&skelwrapper->channelPriv->bh_lock);
+	
+	/* Wake up process' wait queue */
+	wake_up_interruptible(&skelwrapper->channelPriv->waitq);
+}
+
+
+#define MAX_LOCKS 8 // max number of locks/semaphores creatable
+static unsigned long semused = 0; // bit field for used semaphores
+static unsigned long lockused = 0; // bit field for used mutexes
+static struct mutex cmld_locks[MAX_LOCKS];
+
+/** \ingroup OSAL_IMPLEMENTATION
+ */
+t_nmf_osal_sync_handle OSAL_CreateLock(void)
+{
+	int i;
+
+	for (i=0; i<MAX_LOCKS; i++)
+		if (!test_and_set_bit(i, &lockused)) {
+			struct mutex* mutex = &cmld_locks[i];		
+			mutex_init(mutex);
+			return (t_nmf_osal_sync_handle)mutex;
+		}
+
+	return (t_nmf_osal_sync_handle)NULL;
+}
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ */
+void OSAL_Lock(t_nmf_osal_sync_handle handle)
+{
+		// unfortunately there is no return value to this function
+		// so we cannot use 'down_interruptible()'
+		mutex_lock((struct mutex*)handle);
+}
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ */
+void OSAL_Unlock(t_nmf_osal_sync_handle handle)
+{
+		mutex_unlock((struct mutex*)handle);
+}
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ */
+void OSAL_DestroyLock(t_nmf_osal_sync_handle handle)
+{
+	int i;
+
+	// clear the bit in the bits field about used locks
+	i = ((struct mutex*)handle - cmld_locks);
+
+	clear_bit(i, &lockused);
+}
+
+static struct semaphore cmld_semaphores[MAX_LOCKS];
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * Goal: Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * \param[in] value : Initial value of semaphore.
+ * 
+ * \return handle of the Semaphore created 
+ * 
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup OSAL
+ */ 
+t_nmf_osal_sem_handle OSAL_CreateSemaphore(t_uint32 value)
+{
+	int i;
+
+	for (i=0; i<MAX_LOCKS; i++)
+		if (!test_and_set_bit(i, &semused)) {
+			struct semaphore* sem = &cmld_semaphores[i];		
+			sema_init(sem, value);
+			return (t_nmf_osal_sem_handle)sem;
+		}
+
+	return (t_nmf_osal_sem_handle)NULL;
+}
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * Goal: Use by CM to allow to synchronize with code running on mpc side. This function can be called under
+ *       Irq context by CM.
+ *
+ * param[in] : handle of the Semaphore for which we increase value and so potentially wake up thread.
+ * 
+ * param[in] : aCtx is a hint to indicate to os that we are in a none normal context (e.g under interruption).
+ *  
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup OSAL
+ */ 
+void OSAL_SemaphorePost(t_nmf_osal_sem_handle handle, t_uint8 aCtx)
+{
+		up((struct semaphore*)handle);
+}
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * Goal: Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * param[in] : handle of the Semaphore for which we decrease value and so potentially block current thread.
+ *  
+ * param[in] : maximun time in ms after which the block thread is wake up. In this case function return SYNC_ERROR_TIMEOUT value.
+ * 
+ * \return error number: SYNC_ERROR_TIMEOUT in case semaphore is not release withing timeOutInMs.
+ * 
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup OSAL
+ */ 
+t_nmf_osal_sync_error OSAL_SemaphoreWaitTimed(t_nmf_osal_sem_handle handle,
+					      t_uint32 timeOutInMs)
+{
+	if (down_timeout((struct semaphore*)handle, msecs_to_jiffies(timeOutInMs)))
+		return SYNC_ERROR_TIMEOUT;
+	else
+		return SYNC_OK;
+}
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * Goal: Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * param[in] : handle of the Semaphore to be destroyed
+ * 
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup OSAL
+ */
+void OSAL_DestroySemaphore(t_nmf_osal_sem_handle handle)
+{
+	int i;
+
+	// clear the bit in the bits field about used locks
+	i = ((struct semaphore*)handle - cmld_semaphores);
+
+	clear_bit(i, &semused);
+}
+
+/** \ingroup OSAL_IMPLEMENTATION
+ * OSAL alloc implementation
+ *
+ * In both OSAL_Alloc() and OSAL_Alloc_Zero() function, the strategy is to use
+ * kmalloc() as it is the more efficient and most common way to allocate memory.
+ * For big allocation, kmalloc may fail because memory is very fragmented
+ * (kmalloc() allocates contiguous memory). In that case, we fall to vmalloc()
+ * instead.
+ * In OSAL_Free(), we rely on the virtual address to know which of kfree() or
+ * vfree() to use (vmalloc() use its own range of virtual addresses)
+ */
+void* OSAL_Alloc(t_cm_size size)
+{
+#ifdef CM_DEBUG_ALLOC
+	struct cm_alloc_elem *entry;
+
+	if (size == 0)
+		return NULL;
+
+	entry = kmalloc(size + sizeof(*entry), GFP_KERNEL);
+
+	if (entry == NULL) {
+		entry = vmalloc(size + sizeof(*entry));
+
+		if (entry == NULL) {
+			pr_alert("%s: kmalloc(%d) and vmalloc(%d) failed\n",
+				 __func__, (int)size, (int)size);
+			dump_debug_alloc();
+			return NULL;
+		}
+	}
+	/* return address of the caller */
+	entry->caller = __builtin_return_address(0);
+	entry->size = size;
+
+	spin_lock(&cm_alloc.lock);
+	list_add_tail(&entry->elem, &cm_alloc.chain);
+	spin_unlock(&cm_alloc.lock);
+
+	return entry->addr;
+#else
+	void* mem;
+
+	if (size == 0)
+		return NULL;
+	mem = kmalloc(size, GFP_KERNEL);
+	if (mem == NULL) {
+		mem = vmalloc(size);
+		if (mem == NULL)
+			pr_alert("CM (%s): No more memory (requested "
+				 "size=%d) !!!\n", __func__, (int)size);
+	}
+	return mem;
+#endif
+}
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ * OSAL alloc implementation
+ */ 
+void* OSAL_Alloc_Zero(t_cm_size size)
+{
+#ifdef CM_DEBUG_ALLOC
+	struct cm_alloc_elem *entry;
+
+	if (size == 0)
+		return NULL;
+
+	entry = kzalloc(size + sizeof(*entry), GFP_KERNEL);
+	if (entry == NULL) {
+		entry = vmalloc(size + sizeof(*entry));
+		if (entry == NULL) {
+			pr_alert("%s: kmalloc(%d) and vmalloc(%d) failed\n",
+				 __func__, (int)size, (int)size);
+			dump_debug_alloc();
+			return NULL;
+		} else {
+			memset(entry, 0, size + sizeof(*entry));
+		}
+	}
+
+	/* return address of the caller */
+	entry->caller = __builtin_return_address(0);
+	entry->size = size;
+
+	spin_lock(&cm_alloc.lock);
+	list_add_tail(&entry->elem, &cm_alloc.chain);
+	spin_unlock(&cm_alloc.lock);
+
+	return entry->addr;
+#else
+	void* mem;
+
+	if (size == 0)
+		return NULL;
+	mem = kzalloc(size, GFP_KERNEL);
+	if (mem == NULL) {
+		mem = vmalloc(size);
+		if (mem == NULL)
+			pr_alert("CM (%s): No more memory (requested "
+				 "size=%d) !!!\n", __func__, (int)size);
+		else
+			memset(mem, 0, size);
+	}
+
+	return mem;
+#endif
+}
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ *	OSAL free implementation
+ */
+void OSAL_Free(void* mem)
+{
+#ifdef CM_DEBUG_ALLOC
+	struct cm_alloc_elem *entry = container_of(mem, struct cm_alloc_elem, addr);
+	unsigned int i;
+	char pattern[4] = { 0xEF, 0xBE, 0xAD, 0xDE };
+
+	if (mem == NULL)
+		return;
+
+	/* fill with a pattern to detect bad re-use of this area */
+	for (i=0; i<entry->size; i++)
+		entry->addr[i] = pattern[i%4];
+
+	spin_lock(&cm_alloc.lock);
+	list_del(&entry->elem);
+	spin_unlock(&cm_alloc.lock);
+
+	if ((void*)entry >= (void*)VMALLOC_START
+	    && (void*)entry < (void*)VMALLOC_END)
+		vfree(entry);
+	else
+		kfree(entry);
+#else
+	if (mem >= (void*)VMALLOC_START && mem < (void*)VMALLOC_END)
+		vfree(mem);
+	else
+		kfree(mem);
+#endif
+}
+
+/** \ingroup OSAL_IMPLEMENTATION
+ *	OSAL Copy implementation
+ * This copy some data from userspace (address to kernel space.
+ * This implementation differs on Symbian.
+ */
+t_cm_error OSAL_Copy(void *dst, const void *src, t_cm_size size)
+{
+	if (copy_from_user(dst, src, size))
+		return CM_UNKNOWN_MEMORY_HANDLE;
+	return CM_OK;
+}
+
+/**  \ingroup OSAL_IMPLEMENTATION
+ *	OSAL write64 function implementation
+ */
+void OSAL_Write64(t_nmf_trace_channel channel, t_uint8 isTimestamped, t_uint64 value)
+{
+#ifdef CONFIG_STM_TRACE
+	if (isTimestamped)
+		stm_tracet_64(channel, value);
+	else
+		stm_trace_64(channel, value);
+#endif
+}
+
+
+/**  \ingroup OSAL_IMPLEMENTATION
+ *	OSAL log function implementation
+ */
+void OSAL_Log(const char *format, int param1, int param2, int param3, int param4, int param5, int param6)
+{
+	printk(format, param1, param2, param3, param4, param5, param6);
+}
+
+/**
+ * compute the dsp load
+ * 
+ * return -1 if in case of failure, a value between 0 and 100 otherwise
+ */
+static s8 computeDspLoad(t_cm_mpc_load_counter *oldCounter, t_cm_mpc_load_counter *counter)
+{
+	u32 t, l;
+
+	if ((oldCounter->totalCounter == 0) && (oldCounter->loadCounter == 0))
+		return -1; // Failure or not started ?
+	if ((counter->totalCounter == 0) && (counter->loadCounter == 0))
+		return -1; // Failure or already stopped ?
+
+	if (counter->totalCounter < oldCounter->totalCounter)
+		t = (u32)((((u64)-1) - oldCounter->totalCounter)
+			  + counter->totalCounter + 1);
+	else
+		t = (u32)(counter->totalCounter - oldCounter->totalCounter);
+
+	if (counter->loadCounter < oldCounter->loadCounter)
+		l = (u32)((((u64)-1) - oldCounter->loadCounter)
+			  + counter->loadCounter  + 1);
+	else
+		l = (u32)(counter->loadCounter - oldCounter->loadCounter);
+
+	if (t == 0) // not significant
+		return -1;
+
+	if (l > t) // not significant
+		return -1;
+
+	return (l*100) / t;
+}
+
+static void wakeup_process(unsigned long data)
+{
+	wake_up_process((struct task_struct *)data);
+}
+
+/**
+ * Thread function entry for monitorin the CPU load
+ */
+static int dspload_monitor(void *idx)
+{
+	int i = (int)idx;
+	unsigned char current_opp_request = HALF_OPP;
+	struct mpcConfig *mpc = &osalEnv.mpc[i];
+	struct timer_list timer;
+
+	timer.function = wakeup_process;
+	timer.data = (unsigned long)current;
+	init_timer_deferrable(&timer);
+
+	if (prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP,
+				      (char*)mpc->name,
+				      current_opp_request))
+		pr_err("CM Driver: Add QoS failed\n");
+
+	/* init counter */
+	if (CM_GetMpcLoadCounter(mpc->coreId,
+				 &mpc->oldLoadCounter) != CM_OK)
+		pr_err("CM Driver: Failed to init load counter for %s\n",
+		       mpc->name);
+
+	while (!kthread_should_stop()) {
+		t_cm_mpc_load_counter loadCounter;
+		s8 load = -1;
+		unsigned long expire;
+
+		__set_current_state(TASK_UNINTERRUPTIBLE);
+
+		expire = msecs_to_jiffies(dspLoadMonitorPeriod) + jiffies;
+
+		mod_timer(&timer, expire);
+		schedule();
+		/* We can be woken up before the expiration of the timer
+		   but we don't need to handle that case as the
+		   computation of the DSP load takes that into account */
+
+		if (!test_bit(i, &running_dsp))
+			continue;
+
+		if (CM_GetMpcLoadCounter(mpc->coreId,
+					 &loadCounter) != CM_OK)
+			loadCounter = mpc->oldLoadCounter;
+
+		load = computeDspLoad(&mpc->oldLoadCounter, &loadCounter);
+		mpc->oldLoadCounter = loadCounter;
+
+		if (load == -1)
+			continue;
+		/* check if we must request more opp */
+		if ((current_opp_request == HALF_OPP)
+		    && (load > dspLoadHighThreshold)) {
+			current_opp_request = FULL_OPP;
+			if (cm_debug_level)
+				pr_info("CM Driver: Request QoS OPP %d for %s\n",
+					current_opp_request, mpc->name);
+			prcmu_qos_update_requirement(PRCMU_QOS_APE_OPP,
+						     (char*)mpc->name,
+						     current_opp_request);
+		}
+		/* check if we can request less opp */
+		else if ((current_opp_request == FULL_OPP)
+			 && (load < dspLoadLowThreshold)) {
+			current_opp_request = HALF_OPP;
+			if (cm_debug_level)
+				pr_info("CM Driver: Request QoS OPP %d for %s\n",
+					current_opp_request, mpc->name);
+			prcmu_qos_update_requirement(PRCMU_QOS_APE_OPP,
+						     (char*)mpc->name,
+						     current_opp_request);
+		}
+	}
+
+	del_singleshot_timer_sync(&timer);
+	if (cm_debug_level)
+		pr_info("CM Driver: Remove QoS OPP for %s\n", mpc->name);
+	prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP,
+				     (char*)mpc->name);
+ 	return 0;
+}
+
+static int enable_auto_pm = 0;
+module_param(enable_auto_pm, bool, S_IWUSR|S_IRUGO);
+
+/** \ingroup OSAL_IMPLEMENTATION
+ *     Used by CM to disable a power resource
+ */
+void OSAL_DisablePwrRessource(t_nmf_power_resource resource, t_uint32 firstParam, t_uint32 secondParam)
+{
+	switch (resource) {
+	case CM_OSAL_POWER_SxA_CLOCK: {
+		unsigned idx = COREIDX(firstParam);
+		struct osal_msg msg;
+
+		if (idx >= NB_MPC) {
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n",
+			       __func__, (int)resource, (unsigned)firstParam);
+			return;
+		}
+
+		/* Stop the DSP load monitoring */
+		clear_bit(idx, &running_dsp);
+		if (osalEnv.mpc[idx].monitor_tsk) {
+			kthread_stop(osalEnv.mpc[idx].monitor_tsk);
+			osalEnv.mpc[idx].monitor_tsk = NULL;
+		}
+
+		/* Stop the DSP */
+		if (regulator_disable(osalEnv.mpc[idx].mmdsp_regulator) < 0)
+			pr_err("CM Driver(%s): can't disable regulator %s-mmsdp\n",
+			       __func__, osalEnv.mpc[idx].name);
+#ifdef CONFIG_HAS_WAKELOCK
+		wake_unlock(&osalEnv.mpc[idx].wakelock);
+#endif
+
+		/* Create and dispatch a shutdown service message */
+		msg.msg_type = MSG_SERVICE;
+		msg.d.srv.srvType = NMF_SERVICE_SHUTDOWN;
+		msg.d.srv.srvData.shutdown.coreid = firstParam;
+		dispatch_service_msg(&msg);
+		break;
+	}
+	case CM_OSAL_POWER_SxA_AUTOIDLE:
+		switch (firstParam) {
+		case SVA_CORE_ID:
+			osalEnv.dsp_sleep.sva_auto_pm_enable = PRCMU_AUTO_PM_OFF;
+			osalEnv.dsp_sleep.sva_power_on       = 0;
+			osalEnv.dsp_sleep.sva_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF;
+			break;
+		case SIA_CORE_ID:
+			osalEnv.dsp_sleep.sia_auto_pm_enable = PRCMU_AUTO_PM_OFF;
+			osalEnv.dsp_sleep.sia_power_on       = 0;
+			osalEnv.dsp_sleep.sia_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF;
+			break;
+		default:
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n", __func__, (int)resource, (unsigned)firstParam);
+			return;
+		}
+		if (enable_auto_pm)
+			prcmu_configure_auto_pm(&osalEnv.dsp_sleep, &osalEnv.dsp_idle);
+		break;
+	case CM_OSAL_POWER_SxA_HARDWARE: {
+		unsigned idx = COREIDX(firstParam);
+		if (idx >= NB_MPC) {
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n",
+			       __func__, (int)resource, (unsigned)firstParam);
+			return;
+		}
+		if (regulator_disable(osalEnv.mpc[idx].pipe_regulator) < 0)
+			pr_err("CM Driver(%s): can't disable regulator %s-pipe\n",
+			       __func__, osalEnv.mpc[idx].name);
+		break;
+	}
+	case CM_OSAL_POWER_HSEM:
+		break;
+	case CM_OSAL_POWER_SDRAM:
+		break;
+	case CM_OSAL_POWER_ESRAM: {
+		int i;
+		/* firstParam: base address; secondParam: size
+		   U8500_ESRAM_BASE is the start address of BANK 0,
+		   BANK size=0x20000 */
+
+		/* Compute the relative end address of the range,
+		   relative to base address of BANK1 */
+		secondParam = (firstParam+secondParam-(U8500_ESRAM_BASE+0x20000)-1);
+
+		/* if end is below base address of BANK1, it means that full
+		   range of addresses is on Bank0 */
+		if (((int)secondParam) < 0)
+			break;
+		/* Compute the index of the last bank accessed among
+		   esram 1+2 and esram 3+4 banks */
+		secondParam /= 0x40000;
+		WARN_ON(secondParam > 1);
+
+		/* Compute the index of the first bank accessed among esram 1+2
+		   and esram 3+4 banks
+		   Do not manage Bank 0 (secured, must be always ON) */
+		if (firstParam < (U8500_ESRAM_BASE+0x20000))
+			firstParam  = 0;
+		else
+			firstParam  = (firstParam-(U8500_ESRAM_BASE+0x20000))/0x40000;
+
+		/* power off the banks 1+2 and 3+4 if accessed. */
+		for (i=firstParam; i<=secondParam; i++) {
+			if (regulator_disable(osalEnv.esram_regulator[i]) < 0)
+				pr_err("CM Driver(%s): can't disable regulator"
+				       "for esram bank %s\n", __func__,
+				       i ? "34" : "12");
+		}
+		break;
+	}
+	default:
+		pr_err("CM Driver(%s): resource %d unknown/not supported\n",
+		       __func__, (int)resource);
+	}
+}
+
+/** \ingroup OSAL_IMPLEMENTATION
+ *     Used by CM to enable a power resource 
+ */
+t_cm_error OSAL_EnablePwrRessource(t_nmf_power_resource resource, t_uint32 firstParam, t_uint32 secondParam)
+{
+	switch (resource) {
+	case CM_OSAL_POWER_SxA_CLOCK: {
+		unsigned idx = COREIDX(firstParam);
+
+		if (idx > NB_MPC) {
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n", __func__, (int)resource, (unsigned)firstParam);
+			return CM_INVALID_PARAMETER;
+		}
+
+		/* Start the DSP */
+#ifdef CONFIG_HAS_WAKELOCK
+		wake_lock(&osalEnv.mpc[idx].wakelock);
+#endif
+		if (regulator_enable(osalEnv.mpc[idx].mmdsp_regulator) < 0)
+			pr_err("CM Driver(%s): can't enable regulator %s-mmsdp\n", __func__, osalEnv.mpc[idx].name);
+
+		/* Start the DSP load monitoring for this dsp */
+		set_bit(idx, &running_dsp);
+		osalEnv.mpc[idx].monitor_tsk = kthread_run(&dspload_monitor,
+							   (void*)idx,
+							   "%s-loadd",
+							   osalEnv.mpc[idx].name);
+		if (IS_ERR(osalEnv.mpc[idx].monitor_tsk)) {
+			pr_err("CM Driver: failed to start dspmonitord "
+			       "thread: %ld\n", PTR_ERR(osalEnv.mpc[idx].monitor_tsk));
+			osalEnv.mpc[idx].monitor_tsk = NULL;
+		}
+		break;
+	}
+	case CM_OSAL_POWER_SxA_AUTOIDLE:
+		switch (firstParam) {
+		case SVA_CORE_ID:
+			osalEnv.dsp_sleep.sva_auto_pm_enable = PRCMU_AUTO_PM_ON;
+			osalEnv.dsp_sleep.sva_power_on       = PRCMU_AUTO_PM_POWER_ON_HSEM | PRCMU_AUTO_PM_POWER_ON_ABB_FIFO_IT;
+			osalEnv.dsp_sleep.sva_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_RAMRET_HWP_OFF;
+			break;
+		case SIA_CORE_ID:
+			osalEnv.dsp_sleep.sia_auto_pm_enable = PRCMU_AUTO_PM_ON;
+			osalEnv.dsp_sleep.sia_power_on       = PRCMU_AUTO_PM_POWER_ON_HSEM | PRCMU_AUTO_PM_POWER_ON_ABB_FIFO_IT;
+			osalEnv.dsp_sleep.sia_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_RAMRET_HWP_OFF;
+			break;
+		default:
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n", __func__, (int)resource, (unsigned)firstParam);
+			return CM_INVALID_PARAMETER;
+		}
+		if (enable_auto_pm)
+			prcmu_configure_auto_pm(&osalEnv.dsp_sleep, &osalEnv.dsp_idle);
+		break;
+	case CM_OSAL_POWER_SxA_HARDWARE: {
+		unsigned idx = COREIDX(firstParam);
+
+		if (idx > NB_MPC) {
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n", __func__, (int)resource, (unsigned)firstParam);
+			return CM_INVALID_PARAMETER;
+		}
+		if (regulator_enable(osalEnv.mpc[idx].pipe_regulator) < 0)
+			pr_err("CM Driver(%s): can't enable regulator %s-pipe\n", __func__, osalEnv.mpc[idx].name);
+		break;
+	}
+	case CM_OSAL_POWER_HSEM:
+		return CM_OK;
+	case CM_OSAL_POWER_SDRAM:
+		break;
+	case CM_OSAL_POWER_ESRAM:
+	{
+		int i;
+		/* firstParam: base address; secondParam: size
+		   U8500_ESRAM_BASE is the start address of BANK 0,
+		   BANK size=0x20000 */
+
+		/* Compute the relative end address of the range, relative
+		   to base address of BANK1 */
+		secondParam = (firstParam+secondParam-(U8500_ESRAM_BASE+0x20000)-1);
+
+		/* if end is below base address of BANK1, it means that full
+		   range of addresses is on Bank0 */
+		if (((int)secondParam) < 0)
+			break;
+		/* Compute the index of the last bank accessed among esram 1+2
+		   and esram 3+4 banks */
+		secondParam /= 0x40000;
+		WARN_ON(secondParam > 1);
+
+		/* Compute the index of the first bank accessed among esram 1+2
+		   and esram 3+4 banks
+		   Do not manage Bank 0 (secured, must be always ON) */
+		if (firstParam < (U8500_ESRAM_BASE+0x20000))
+			firstParam  = 0;
+		else
+			firstParam  = (firstParam-(U8500_ESRAM_BASE+0x20000))/0x40000;
+
+		/* power on the banks 1+2 and 3+4 if accessed. */
+		for (i=firstParam; i<=secondParam; i++) {
+			if (regulator_enable(osalEnv.esram_regulator[i]) < 0)
+				pr_err("CM Driver(%s): can't enable regulator "
+				       "for esram bank %s\n", __func__,
+				       i ? "34" : "12");
+		}
+		break;
+	}
+	default:
+		pr_err("CM Driver(%s): resource %x unknown/not supported\n",
+		       __func__, (int)resource);
+		return CM_INVALID_PARAMETER;
+	}
+
+	return CM_OK;
+}
+
+/*!
+ * \brief Generate an OS-Panic. Called in from CM_ASSERT().
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+void OSAL_Panic(void)
+{
+	panic("FATAL ISSUE IN THE CM DRIVER !!");
+}
+#include <mach/dcache.h>
+/*!
+ * \brief Clean data cache in DDR in order to be accessible from peripheral.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+void OSAL_CleanDCache(t_uint32 startAddr, t_uint32 size)
+{
+#if 0
+	/*
+	 * Currently, the code sections are non-cached/buffered,
+	 * which normally doesn't required the maintenance done below.
+	 * As the cost is low (doesn't do much thing), I keep it in case
+	 * of the memory settings are changed later.
+	 */
+
+	struct hwmem_region region;
+	struct mpcConfig *mpc;
+	t_uint32 endAddr = startAddr + size;
+
+	if (startAddr >= (u32)osalEnv.mpc[0].sdramCodeL
+	    && endAddr <= (u32)(osalEnv.mpc[0].sdramCodeL
+				+ osalEnv.mpc[0].sdramCodeSize)) {
+		mpc = &osalEnv.mpc[0];
+	} else if (startAddr >= (u32)osalEnv.mpc[1].sdramCodeL
+		   && endAddr <= (u32)(osalEnv.mpc[1].sdramCodeL
+				       + osalEnv.mpc[1].sdramCodeSize)) {
+		mpc = &osalEnv.mpc[1];
+	} else {
+		/* The code may be in esram, in that case, nothing to do */
+		return;
+	}
+
+	region.offset = startAddr - (u32)mpc->sdramCodeL;
+	region.count  = 1;
+	region.start  = 0;
+	region.end    = size;
+	region.size   = size;
+	hwmem_set_domain(mpc->hwmemCode, HWMEM_ACCESS_READ,
+			 HWMEM_DOMAIN_SYNC, &region);
+	/*
+	 * The hwmem keep track of region being sync or not.
+	 * Mark the region as being write-accessed here right now
+	 * to let following clean being done as expected. Today,
+	 * there is no other place to do that in CM Core right now
+	 */
+	hwmem_set_domain(mpc->hwmemCode, HWMEM_ACCESS_WRITE,
+			 HWMEM_DOMAIN_CPU, &region);
+#else
+	dsb();
+	outer_cache.sync();
+#endif
+}
+
+/*!
+ * \brief Flush write-buffer of L2 cache
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+void OSAL_mb(void)
+{
+	mb();
+}
+
+/*!
+ * \brief return prcmu timer value.
+ *
+ * This is need for perfmeter api  (see \ref t_nmf_power_resource)
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+t_uint64 OSAL_GetPrcmuTimer()
+{
+	t_uint64 msbBefore;
+	t_uint32 lsb;
+	t_uint64 msbAfter;
+
+	/* read prcmu timers */
+	msbBefore = ~ioread32(prcmu_tcdm_base+0xDE4);
+	lsb = ~ioread32(prcmu_base+0x454);
+	msbAfter = ~ioread32(prcmu_tcdm_base+0xDE4);
+
+	/* handle rollover test case */
+	// NOTE : there is still a window in prcmu side between counter rollover
+	// and prcmu interrupt handling
+	// to update msb register => this can lead to erroneous value return here
+	if (msbBefore == msbAfter || lsb >= 0x80000000UL)
+		return (((msbBefore & 0xffffffUL) << 32) + lsb);
+	else
+		return (((msbAfter & 0xffffffUL) << 32) + lsb);
+}
+
+/*!
+ * \brief Disable the service message handling (panic, etc)
+ *
+ * It must disable the handling of all service messages
+ * If a service message is currently handled, it must wait till the end
+ * of its managment before returning.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_DisableServiceMessages(void) {
+	tasklet_disable(&cmld_service_tasklet);
+}
+
+/*!
+ * \brief Enable the service message handling (panic, etc)
+ *
+ * It enables the handling of all service messages
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_EnableServiceMessages(void) {
+	tasklet_enable(&cmld_service_tasklet);
+}
diff --git a/drivers/staging/nmf-cm/osal-kernel.h b/drivers/staging/nmf-cm/osal-kernel.h
new file mode 100644
index 00000000000..36baf893fcc
--- /dev/null
+++ b/drivers/staging/nmf-cm/osal-kernel.h
@@ -0,0 +1,145 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef OSAL_KERNEL_H
+#define OSAL_KERNEL_H
+
+#include <linux/interrupt.h>
+#include <linux/hwmem.h>
+#include <linux/regulator/consumer.h>
+#ifdef CONFIG_HAS_WAKELOCK
+#include <linux/wakelock.h>
+#endif
+#include <mach/prcmu.h>
+#include <cm/engine/api/channel_engine.h>
+#include <cm/engine/api/control/configuration_engine.h>
+#include <cm/engine/api/perfmeter_engine.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <linux/plist.h>
+
+#include "configuration.h"
+
+/** Per-MPC configuration structure */
+struct mpcConfig {
+	const t_nmf_core_id coreId;     /**< MPC coreId */
+	const char *name;               /**< MPC name */
+        t_uint8    nbYramBanks;         /**< number of TCM ram banks to reserve for y memory */
+	t_nmf_executive_engine_id eeId; /**< Type of Executive Engine */
+	const void *baseP;              /**< Physical base address of the MPC */
+	void       *baseL;              /**< Remapped base address of the MPC */
+	struct hwmem_alloc *hwmemCode;  /**< hwmem code segment */
+	u32        sdramCodeP;          /**< Physical base address for MPC SDRAM Code region */
+	void       *sdramCodeL;         /**< Remapped base address for MPC SDRAM Code region */
+	size_t     sdramCodeSize;       /**< Size of MPC SDRAM Code region */
+	struct hwmem_alloc *hwmemData;  /**< hwmem data segment */
+	u32        sdramDataP;          /**< Physical base address for MPC SDRAM Data region */
+	void       *sdramDataL;         /**< Remapped base address for MPC SDRAM Data region */
+	size_t     sdramDataSize;       /**< Size of MPC SDRAM Data region */
+	const unsigned int interrupt0;  /**< interrupt line triggered by the MPC, for MPC events (if HSEM not used) */
+	const unsigned int interrupt1;  /**< interrupt line triggered by the MPC, for PANIC events */
+	struct tasklet_struct tasklet;  /**< taskket used to process MPC events */
+	struct regulator *mmdsp_regulator; /**< mmdsp regulator linked to this MPC */
+	struct regulator *pipe_regulator;  /**< hardware pipe linked to this MPC */
+#ifdef CONFIG_HAS_WAKELOCK
+	struct wake_lock wakelock;      /**< wakelock for this MPC to prevent ARM to go in APSLEEP state */
+#endif
+	struct task_struct *monitor_tsk;/**< task to monitor the dsp load; */
+	t_cm_mpc_load_counter oldLoadCounter; /**< previous load counter of the DSP */
+};
+
+/** Describes current Kernel OSAL environment
+ *
+ * Note about mpc.tasklet : we declare one tasklet per MPC but their usage depends
+ * on cfgSemaphoreTypeHSEM.
+ *
+ * This tasklet is scheduled by the interrupt handler to process MPC Events.
+ * - If we use Hardware Semaphore, there is only one interrupt handler used
+ * and thus only one tasklet, tasklet of MPC 0 (ie osalEnv.mpc[0].tasklet)
+ * - If we use local semaphore, there is one interrupt handler and tasklet per mpc
+ */
+struct OsalEnvironment
+{
+	struct mpcConfig mpc[NB_MPC];
+	void* hwsem_base;       /** < Remapped base address of the hardware semaphores */
+	void* esram_base;       /** < Remapped base address for embedded RAM used within the CM */
+	struct regulator *esram_regulator[NB_ESRAM]; /**< regulator for ESRAM bank 1+2 and 3+4 */
+	struct prcmu_auto_pm_config dsp_sleep;
+	struct prcmu_auto_pm_config dsp_idle;
+};
+
+
+/** Structure used to store the skeleton related data.
+ *  It is used for communicattion from a MPC to a user process (=host)
+ */
+typedef struct {
+	struct list_head entry; /**< Doubly linked list descriptor */
+	t_cm_bf_mpc2host_handle mpc2hostId;  /**< mpc2host ID */
+	t_nmf_mpc2host_handle upperLayerThis;/**< upper-layer handle */
+	struct cm_channel_priv* channelPriv; /**< Per-channel private data. The actual message queue is hold here */
+} t_skelwrapper;
+
+/** Message description for MPC to HOST communication
+ */
+struct osal_msg {
+	struct {
+		struct plist_node entry; /**< Doubly linked list descriptor */
+		t_message_type type;     /**< Type of message (callback, service or interrupt for now) */
+	} hdr; /**< Header of the message */
+#define msg_entry hdr.entry
+#define msg_type hdr.type
+	union {
+		struct {
+			t_skelwrapper *skelwrap; /**< Link to the skelwrapper, to retrieve the channel on which this message has to be forwarded */
+			t_uint32 methodIdx; /**< callback data: method index*/
+			t_event_params_handle anyPtr;  /**< callback data: method parameters */
+			t_uint32 ptrSize;  /**< size of the parameters */
+		} itf; /**< structure holding callback data */
+		struct {
+			t_nmf_service_type srvType; /**< Type of the service */
+			t_nmf_service_data srvData; /**< Data of the service */
+		} srv; /**< structure holding service data */
+	} d; /**< data */
+};
+
+extern struct OsalEnvironment osalEnv;
+
+/** Environment initialization/deinitialization */
+int remapRegions(void);
+void unmapRegions(void);
+
+/** Component manager configuration getters for CM_ENGINE_Init() */
+int getNmfHwMappingDesc(t_nmf_hw_mapping_desc* nmfHwMappingDesc);
+
+/** Component manager configuration getters for CM_ConfigureMediaProcessorCore (SVA and SIA) */
+void getMpcSystemAddress(unsigned i, t_cm_system_address* mpcSystemAddress);
+void getMpcSdramSegments(unsigned i, t_nmf_memory_segment* codeSegment, t_nmf_memory_segment* dataSegment);
+
+#ifdef CM_DEBUG_ALLOC
+struct cm_alloc {
+	spinlock_t        lock;
+	struct list_head  chain;
+};
+
+struct cm_alloc_elem {
+	struct list_head elem;
+	void *caller;
+	size_t size;
+	char addr[0];
+};
+
+void init_debug_alloc(void);
+void cleanup_debug_alloc(void);
+#endif /* CM_DEBUG_ALLOC */
+
+/* TODO: To remove later */
+extern __iomem void *prcmu_base;
+extern __iomem void *prcmu_tcdm_base;
+extern const char *cmld_devname[];
+
+#define PRCM_SVAMMDSPCLK_MGT                (prcmu_base + 0x008)
+#define PRCM_SIAMMDSPCLK_MGT                (prcmu_base + 0x00c)
+
+#endif /* OSAL_KERNEL_H */
diff --git a/drivers/staging/nmf-cm/share/communication/inc/communication_fifo.h b/drivers/staging/nmf-cm/share/communication/inc/communication_fifo.h
new file mode 100644
index 00000000000..ea24e82ceae
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/communication/inc/communication_fifo.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_NMF_COM_FIFO
+#define __INC_NMF_COM_FIFO
+
+#include <inc/typedef.h>
+
+#define EVENT_ELEM_METHOD_IDX       0
+#define EVENT_ELEM_PARAM_IDX        1
+#define EVENT_ELEM_EXTFIELD_IDX     2
+
+#define EVENT_ELEM_SIZE_IN_BYTE     (3 * sizeof(t_shared_field))
+
+#endif /* __INC_NMF_COM_FIFO */
diff --git a/drivers/staging/nmf-cm/share/communication/inc/initializer.h b/drivers/staging/nmf-cm/share/communication/inc/initializer.h
new file mode 100644
index 00000000000..ea5aa68f2dc
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/communication/inc/initializer.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_SHARE_INITIALIZER
+#define __INC_SHARE_INITIALIZER
+
+#define NMF_CONSTRUCT_INDEX         0
+#define NMF_START_INDEX             1
+#define NMF_STOP_INDEX              2
+#define NMF_DESTROY_INDEX           3
+#define NMF_UPDATE_STACK            4
+#define NMF_LOCK_CACHE              5
+#define NMF_UNLOCK_CACHE            6
+#define NMF_ULP_FORCEWAKEUP         7
+#define NMF_ULP_ALLOWSLEEP          8
+#define NMF_CONSTRUCT_SYNC_INDEX    9
+#define NMF_START_SYNC_INDEX        10
+
+/*
+ * Index of datas in command parameter format
+ */
+#define INIT_COMPONENT_CMD_HANDLE_INDEX         0
+#define INIT_COMPONENT_CMD_THIS_INDEX           2
+#define INIT_COMPONENT_CMD_METHOD_INDEX         4
+#define INIT_COMPONENT_CMD_SIZE                 6
+
+/*
+ * Index of datas in acknowledge parameter format
+ */
+#define INIT_COMPONENT_ACK_HANDLE_INDEX         0
+#define INIT_COMPONENT_ACK_SIZE                 2
+
+#endif /* __INC_SHARE_INITIALIZER */
diff --git a/drivers/staging/nmf-cm/share/communication/inc/nmf_fifo_desc.h b/drivers/staging/nmf-cm/share/communication/inc/nmf_fifo_desc.h
new file mode 100644
index 00000000000..99caa48b05c
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/communication/inc/nmf_fifo_desc.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_NMF_FIFO_DESC
+#define __INC_NMF_FIFO_DESC
+
+#include <inc/typedef.h>
+#include <share/semaphores/inc/semaphores.h>
+
+/*
+ * SHOULD be mapped onto a AHB burst (16 bytes=8x16-bit)
+ */
+typedef struct {
+    t_semaphore_id semId;
+
+    t_uint16 elemSize;
+    t_uint16 fifoFullValue;
+    t_uint16 readIndex;
+    t_uint16 writeIndex;
+    t_uint16 wrappingValue;
+
+    t_uint32 extendedField; /* in DSP 24 memory when to MPC in Logical Host when to ARM */
+} t_nmf_fifo_desc;
+
+#define EXTENDED_FIELD_BCTHIS_OR_TOP        0       //<! This field will be used:
+                                                    //<! - as hostBCThis for DSP->HOST binding
+                                                    //<! - as TOP else
+#define EXTENDED_FIELD_BCDESC               1       //<! This field will be used for:
+                                                    //<! - interface method address for ->MPC binding
+                                                    //<! - for params size for ->Host binding (today only [0] is used as max size)
+
+#endif /* __INC_NMF_FIFO */
diff --git a/drivers/staging/nmf-cm/share/communication/inc/nmf_service.h b/drivers/staging/nmf-cm/share/communication/inc/nmf_service.h
new file mode 100644
index 00000000000..5ec08434492
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/communication/inc/nmf_service.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_NMF_SERVICE_H
+#define __INC_NMF_SERVICE_H
+
+/* 1 - 0xff Reserved for Panic Reason */
+#define MPC_SERVICE_NONE                    0
+#define MPC_SERVICE_BOOT                    0xB001
+#define MPC_SERVICE_PRINT                   0x1234
+
+#endif
diff --git a/drivers/staging/nmf-cm/share/inc/macros.h b/drivers/staging/nmf-cm/share/inc/macros.h
new file mode 100644
index 00000000000..c96fe031c25
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/inc/macros.h
@@ -0,0 +1,214 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief NMF Macro API.
+ */
+
+#ifndef _COMMON_MACROS_H_
+#define _COMMON_MACROS_H_
+
+#undef ALIGN_VALUE
+#define ALIGN_VALUE(value, alignment) (((value) + (alignment - 1)) & ~(alignment - 1))
+
+#undef MIN
+#define MIN(a,b) (((a)>(b))?(b):(a))
+
+#undef MAX
+#define MAX(a,b) (((a)<(b))?(b):(a))
+
+/*-----------------------------------------------------------------------------
+ * endianess switch macros (32 bits and 16 bits)
+ *---------------------------------------------------------------------------*/
+#define ENDIANESS_32_SWITCH(value) ( \
+        (((value) & MASK_BYTE3) >> SHIFT_BYTE3) | \
+        (((value) & MASK_BYTE2) >> SHIFT_BYTE1) | \
+        (((value) & MASK_BYTE1) << SHIFT_BYTE1) | \
+        (((value) & MASK_BYTE0) << SHIFT_BYTE3)   \
+        )
+
+#define ENDIANESS_16_SWITCH(value) ( \
+        (((value) & MASK_BYTE0) << SHIFT_BYTE1) | \
+        (((value) & MASK_BYTE1) >> SHIFT_BYTE1)   \
+        )
+
+/*-----------------------------------------------------------------------------
+ * field offset extraction from a structure
+ *---------------------------------------------------------------------------*/
+#undef FIELD_OFFSET
+#define FIELD_OFFSET(typeName, fieldName) ((t_uint32)(&(((typeName *)0)->fieldName)))
+
+#undef MASK_BIT
+#define MASK_BIT(n)  				(1UL << ((n) - 1))
+
+/*-----------------------------------------------------------------------------
+ * Misc definition
+ *---------------------------------------------------------------------------*/
+
+#undef ONE_KB
+#define ONE_KB (1024)
+#undef ONE_MB
+#define ONE_MB (ONE_KB * ONE_KB)
+
+/*-----------------------------------------------------------------------------
+ * Bit mask definition
+ *---------------------------------------------------------------------------*/
+#undef MASK_NULL8
+#define MASK_NULL8     0x00U
+#undef MASK_NULL16
+#define MASK_NULL16    0x0000U
+#undef MASK_NULL32
+#define MASK_NULL32    0x00000000UL
+#undef MASK_ALL8
+#define MASK_ALL8      0xFFU
+#undef MASK_ALL16
+#define MASK_ALL16     0xFFFFU
+#undef MASK_ALL32
+#define MASK_ALL32     0xFFFFFFFFUL
+
+#undef MASK_BIT0
+#define MASK_BIT0      (1UL<<0)
+#undef MASK_BIT1
+#define MASK_BIT1      (1UL<<1)
+#undef MASK_BIT2
+#define MASK_BIT2      (1UL<<2)
+#undef MASK_BIT3
+#define MASK_BIT3      (1UL<<3)
+#undef MASK_BIT4
+#define MASK_BIT4      (1UL<<4)
+#undef MASK_BIT5
+#define MASK_BIT5      (1UL<<5)
+#undef MASK_BIT6
+#define MASK_BIT6      (1UL<<6)
+#undef MASK_BIT7
+#define MASK_BIT7      (1UL<<7)
+#undef MASK_BIT8
+#define MASK_BIT8      (1UL<<8)
+#undef MASK_BIT9
+#define MASK_BIT9      (1UL<<9)
+#undef MASK_BIT10
+#define MASK_BIT10     (1UL<<10)
+#undef MASK_BIT11
+#define MASK_BIT11     (1UL<<11)
+#undef MASK_BIT12
+#define MASK_BIT12     (1UL<<12)
+#undef MASK_BIT13
+#define MASK_BIT13     (1UL<<13)
+#undef MASK_BIT14
+#define MASK_BIT14     (1UL<<14)
+#undef MASK_BIT15
+#define MASK_BIT15     (1UL<<15)
+#undef MASK_BIT16
+#define MASK_BIT16     (1UL<<16)
+#undef MASK_BIT17
+#define MASK_BIT17     (1UL<<17)
+#undef MASK_BIT18
+#define MASK_BIT18     (1UL<<18)
+#undef MASK_BIT19
+#define MASK_BIT19     (1UL<<19)
+#undef MASK_BIT20
+#define MASK_BIT20     (1UL<<20)
+#undef MASK_BIT21
+#define MASK_BIT21     (1UL<<21)
+#undef MASK_BIT22
+#define MASK_BIT22     (1UL<<22)
+#undef MASK_BIT23
+#define MASK_BIT23     (1UL<<23)
+#undef MASK_BIT24
+#define MASK_BIT24     (1UL<<24)
+#undef MASK_BIT25
+#define MASK_BIT25     (1UL<<25)
+#undef MASK_BIT26
+#define MASK_BIT26     (1UL<<26)
+#undef MASK_BIT27
+#define MASK_BIT27     (1UL<<27)
+#undef MASK_BIT28
+#define MASK_BIT28     (1UL<<28)
+#undef MASK_BIT29
+#define MASK_BIT29     (1UL<<29)
+#undef MASK_BIT30
+#define MASK_BIT30     (1UL<<30)
+#undef MASK_BIT31
+#define MASK_BIT31     (1UL<<31)
+
+/*-----------------------------------------------------------------------------
+ * quartet shift definition
+ *---------------------------------------------------------------------------*/
+#undef MASK_QUARTET
+#define MASK_QUARTET     (0xFUL)
+#undef SHIFT_QUARTET0
+#define SHIFT_QUARTET0   0
+#undef SHIFT_QUARTET1
+#define SHIFT_QUARTET1   4
+#undef SHIFT_QUARTET2
+#define SHIFT_QUARTET2   8
+#undef SHIFT_QUARTET3
+#define SHIFT_QUARTET3   12
+#undef SHIFT_QUARTET4
+#define SHIFT_QUARTET4   16
+#undef SHIFT_QUARTET5
+#define SHIFT_QUARTET5   20
+#undef SHIFT_QUARTET6
+#define SHIFT_QUARTET6   24
+#undef SHIFT_QUARTET7
+#define SHIFT_QUARTET7   28
+#undef MASK_QUARTET0
+#define MASK_QUARTET0    (MASK_QUARTET << SHIFT_QUARTET0)
+#undef MASK_QUARTET1
+#define MASK_QUARTET1    (MASK_QUARTET << SHIFT_QUARTET1)
+#undef MASK_QUARTET2
+#define MASK_QUARTET2    (MASK_QUARTET << SHIFT_QUARTET2)
+#undef MASK_QUARTET3
+#define MASK_QUARTET3    (MASK_QUARTET << SHIFT_QUARTET3)
+#undef MASK_QUARTET4
+#define MASK_QUARTET4    (MASK_QUARTET << SHIFT_QUARTET4)
+#undef MASK_QUARTET5
+#define MASK_QUARTET5    (MASK_QUARTET << SHIFT_QUARTET5)
+#undef MASK_QUARTET6
+#define MASK_QUARTET6    (MASK_QUARTET << SHIFT_QUARTET6)
+#undef MASK_QUARTET7
+#define MASK_QUARTET7    (MASK_QUARTET << SHIFT_QUARTET7)
+
+/*-----------------------------------------------------------------------------
+ * Byte shift definition
+ *---------------------------------------------------------------------------*/
+#undef MASK_BYTE
+#define MASK_BYTE       (0xFFUL)
+#undef SHIFT_BYTE0
+#define SHIFT_BYTE0 	0U
+#undef SHIFT_BYTE1
+#define SHIFT_BYTE1 	8U
+#undef SHIFT_BYTE2
+#define SHIFT_BYTE2 	16U
+#undef SHIFT_BYTE3
+#define SHIFT_BYTE3 	24U
+#undef MASK_BYTE0
+#define MASK_BYTE0       (MASK_BYTE << SHIFT_BYTE0)
+#undef MASK_BYTE1
+#define MASK_BYTE1       (MASK_BYTE << SHIFT_BYTE1)
+#undef MASK_BYTE2
+#define MASK_BYTE2       (MASK_BYTE << SHIFT_BYTE2)
+#undef MASK_BYTE3
+#define MASK_BYTE3       (MASK_BYTE << SHIFT_BYTE3)
+
+/*-----------------------------------------------------------------------------
+ * Halfword shift definition
+ *---------------------------------------------------------------------------*/
+#undef MASK_HALFWORD
+#define MASK_HALFWORD        (0xFFFFUL)
+#undef SHIFT_HALFWORD0
+#define SHIFT_HALFWORD0 	    0U
+#undef SHIFT_HALFWORD1
+#define SHIFT_HALFWORD1 	    16U
+#undef MASK_HALFWORD0
+#define MASK_HALFWORD0       (MASK_HALFWORD << SHIFT_HALFWORD0)
+#undef MASK_HALFWORD1
+#define MASK_HALFWORD1       (MASK_HALFWORD << SHIFT_HALFWORD1)
+
+#endif /* _COMMON_MACROS_H_ */
+
diff --git a/drivers/staging/nmf-cm/share/inc/nmf.h b/drivers/staging/nmf-cm/share/inc/nmf.h
new file mode 100644
index 00000000000..8be8b41e5e3
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/inc/nmf.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+ 
+/*!
+ * \brief Common Nomadik Multiprocessing Framework type definition
+ *
+ * This file contains the shared type definitions used into NMF.
+ */
+
+#ifndef __INC_NMF_H
+#define __INC_NMF_H
+
+#include <inc/typedef.h>
+
+/*!
+ * \brief Identification of the various cores (host cpu and Media Processors) into Nomadik Platform
+ * In order to improve performance, these ids are those used to interconnect HW Semaphores IP with Cores (Interrupt lines)
+ * \ingroup NMF_COMMON
+ */
+#if defined(__STN_8500)
+    //#warning "TODO : mapping below is not correct, need to think how to change it"
+#endif
+typedef t_uint8 t_nmf_core_id;
+#define ARM_CORE_ID                 ((t_nmf_core_id)0)                  //!< HOST CPU Id
+#define SVA_CORE_ID                 ((t_nmf_core_id)1)                  //!< Smart Video Accelerator Media Processor Code Id
+#define SIA_CORE_ID                 ((t_nmf_core_id)2)                  //!< Smart Imaging Accelerator Media Processor Code Id
+#define NB_CORE_IDS                 ((t_nmf_core_id)3)
+
+#define FIRST_CORE_ID               ((t_nmf_core_id)ARM_CORE_ID)
+#define FIRST_MPC_ID                ((t_nmf_core_id)SVA_CORE_ID)
+#define LAST_CORE_ID                ((t_nmf_core_id)SIA_CORE_ID)
+#define LAST_MPC_ID                 ((t_nmf_core_id)SIA_CORE_ID)
+
+
+/*!
+ * \brief Define minimal stack size use by execution engine
+ */
+#define MIN_STACK_SIZE              128
+
+
+
+#endif /* __INC_NMF_H */
diff --git a/drivers/staging/nmf-cm/share/inc/nomadik_mapping.h b/drivers/staging/nmf-cm/share/inc/nomadik_mapping.h
new file mode 100644
index 00000000000..bec221aa111
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/inc/nomadik_mapping.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_NOMADIK_MAPPING_H
+#define __INC_NOMADIK_MAPPING_H
+
+/*--------------------------------------------------------------------------*/
+#if defined(__STN_8810)
+
+/* XTI (CPU OSMO/OSMOT address space) */
+#define XTI_CPU_BASE_ADDR	0x10000000
+#define XTI_CPU_END_ADDR	0x100FFFFF
+
+/* XTI configuration registers */
+#define XTI_CFG_REG_BASE_ADDR	0x101A0000
+#define XTI_CFG_REG_END_ADDR	0x101AFFFF
+
+/* Core APB Peripherals */
+#define CORE_APB_BASE_ADDR	0x101E0000
+#define CORE_APB_END_ADDR	0x101EFFFF
+
+/* DMA APB Peripherals */
+#define DMA_APB_BASE_ADDR	0x101F0000
+#define DMA_APB_END_ADDR	0x101FFFFF
+
+/* XTI (DSP OSMO/OSMOT address space) */
+#define XTI_DSP_BASE_ADDR	0x10200000
+#define XTI_DSP_END_ADDR	0x1020FFFF
+
+#endif  /*  defined(__STN_8810)   */
+
+/*--------------------------------------------------------------------------*/
+#if defined(__STN_8815)
+
+/* XTI (CPU OSMO/OSMOT address space) */
+#define XTI_CPU_BASE_ADDR	0x10000000
+#define XTI_CPU_END_ADDR	0x100FFFFF
+
+/* XTI configuration registers */
+#define XTI_CFG_REG_BASE_ADDR	0x101A0000
+#define XTI_CFG_REG_END_ADDR	0x101AFFFF
+
+/* Core APB Peripherals */
+#define CORE_APB_BASE_ADDR	0x101E0000
+#define CORE_APB_END_ADDR	0x101EFFFF
+
+/* DMA APB Peripherals */
+#define DMA_APB_BASE_ADDR	0x101F0000
+#define DMA_APB_END_ADDR	0x101FFFFF
+
+/* XTI (DSP OSMO/OSMOT address space) */
+#define XTI_DSP_BASE_ADDR	0x10220000
+#define XTI_DSP_END_ADDR	0x1022FFFF
+
+#endif  /*  defined(__STN_8815)   */
+
+
+/*--------------------------------------------------------------------------*/
+#if defined(__STN_8820)
+
+/* STM (System Trace Module address space) */
+#define STM_BASE_ADDR	0x700F0000
+#define STM_END_ADDR	0x700FFFFF
+
+/* AHB2 Peripherals */
+#define AHB2_PERIPH_BASE_ADDR	0x70100000
+#define AHB2_PERIPH_END_ADDR	0x7010FFFF
+
+/* APB2 Peripherals */
+#define APB2_PERIPH_BASE_ADDR	0x70110000
+#define APB2_PERIPH_END_ADDR	0x7011FFFF
+
+/* APB1 Peripherals */
+#define APB1_PERIPH_BASE_ADDR	0x70120000
+#define APB1_PERIPH_END_ADDR	0x7012FFFF
+
+#endif  /*  defined(__STN_8820)   */
+
+/*--------------------------------------------------------------------------*/
+#if defined(__STN_8500)
+/* STM (System Trace Module address space) */
+#define STM_BASE_ADDR       0x80100000
+#define STM_END_ADDR        0x8010FFFF
+
+#define HSEM_BASE_ADDR      0x80140000
+#define HSEM_END_ADDR       0x8014FFFF
+
+#define DMA_CTRL_BASE_ADDR  0x801C0000
+#define DMA_CTRL_END_ADDR   0x801C0FFF
+
+
+#endif  /*  defined(__STN_8500)   */
+
+#endif /*__INC_NOMADIK_MAPPING_H */
diff --git a/drivers/staging/nmf-cm/share/semaphores/inc/hwsem_hwp.h b/drivers/staging/nmf-cm/share/semaphores/inc/hwsem_hwp.h
new file mode 100644
index 00000000000..b573627beae
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/semaphores/inc/hwsem_hwp.h
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_HWSEM_HWP_H
+#define __INC_HWSEM_HWP_H
+
+#include <share/semaphores/inc/semaphores.h>
+
+#define CORE_ID_2_HW_CORE_ID(coreId) (1U << (coreId))
+
+/*
+ * Definition of the number of hw semaphores into the Nomadik IP
+ */
+#define NUM_HW_SEMAPHORES       32
+
+
+/*
+ * Definition of how HSEM IP interrupts are interconnected with cores
+ */
+typedef enum {
+    HSEM_FIRST_INTR = 0,
+    HSEM_INTRA = HSEM_FIRST_INTR,
+    HSEM_INTRB = 1,
+    HSEM_INTRC = 2,
+    HSEM_INTRD = 3,
+    HSEM_INTRE = 4,
+    HSEM_MAX_INTR
+} t_hw_semaphore_irq_id;
+
+/*
+ * Description of the registers of the HW Sem IP
+ */
+#define HSEM_INTRA_MASK  (1<<(4+HSEM_INTRA))
+#define HSEM_INTRB_MASK  (1<<(4+HSEM_INTRB))
+#define HSEM_INTRC_MASK  (1<<(4+HSEM_INTRC))
+#define HSEM_INTRD_MASK  (1<<(4+HSEM_INTRD))
+#define HSEM_INTRE_MASK  (1<<(4+HSEM_INTRE))
+
+typedef struct {
+    t_shared_reg imsc;
+    t_shared_reg ris;
+    t_shared_reg mis;
+    t_shared_reg icr;
+} t_hsem_it_regs;
+
+typedef volatile struct {
+#if defined(__STN_8500)
+    t_shared_reg cr;
+    t_shared_reg dummy;
+#endif
+    t_shared_reg sem[NUM_HW_SEMAPHORES];
+#if defined(__STN_8820)
+    t_shared_reg RESERVED1[(0x90 - 0x80)>>2];
+#elif defined(__STN_8500)
+    t_shared_reg RESERVED1[(0x90 - 0x88)>>2];
+#else /* __STN_8820 or __STN_8500 -> _STN_8815 */
+    t_shared_reg RESERVED1[(0x90 - 0x40)>>2];
+#endif /* __STN_8820 or __STN_8500 -> _STN_8815 */
+    t_shared_reg icrall;
+    t_shared_reg RESERVED2[(0xa0 - 0x94)>>2];
+    t_hsem_it_regs it[HSEM_MAX_INTR];
+#if defined(__STN_8820) || defined(__STN_8500)
+    t_shared_reg RESERVED3[(0x100 - 0xf0)>>2];
+#else /* __STN_8820 or __STN_8500 -> _STN_8815 */
+    t_shared_reg RESERVED3[(0x100 - 0xe0)>>2];
+#endif /* __STN_8820 or __STN_8500 -> _STN_8815 */
+    t_shared_reg itcr;
+    t_shared_reg RESERVED4;
+    t_shared_reg itop;
+    t_shared_reg RESERVED5[(0xfe0 - 0x10c)>>2];
+    t_shared_reg pid0;
+    t_shared_reg pid1;
+    t_shared_reg pid2;
+    t_shared_reg pid3;
+    t_shared_reg pcid0;
+    t_shared_reg pcid1;
+    t_shared_reg pcid2;
+    t_shared_reg pcid3;
+} t_hw_semaphore_regs, *tp_hw_semaphore_regs;
+
+#endif /* __INC_HWSEM_HWP_H */
diff --git a/drivers/staging/nmf-cm/share/semaphores/inc/semaphores.h b/drivers/staging/nmf-cm/share/semaphores/inc/semaphores.h
new file mode 100644
index 00000000000..c72b64cd709
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/semaphores/inc/semaphores.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_SHARED_SEMAPHORE_H
+#define __INC_SHARED_SEMAPHORE_H
+
+#include <share/inc/nmf.h>
+
+typedef t_uint16 t_semaphore_id;
+
+/*
+ * HW semaphore allocation
+ * -----------------------
+ *  We want to optimize interrupt demultiplexing at dsp interrupt handler level
+ *  so a good solution would be to have sequentially the semaphores for each neighbors
+ *
+ * STn8500 :
+ * ---------
+ * ARM <- SVA  COMS => 0
+ * ARM <- SIA  COMS => 1
+ * SVA <- ARM  COMS => 2
+ * SVA <- SIA  COMS => 3
+ * SIA <- ARM  COMS => 4
+ * SIA <- SVA  COMS => 5
+
+ * The first neighbor is always the ARM, then the other ones (SVA,SIA)
+ */
+
+/*
+ * Local semaphore allocation
+ * -----------------------
+ * 0 : ARM <- DSP
+ * 1 : DSP <- ARM
+ */
+
+#define NB_USED_HSEM_PER_CORE (NB_CORE_IDS - 1)
+#define FIRST_NEIGHBOR_SEMID(coreId) ((coreId)*NB_USED_HSEM_PER_CORE)
+
+#endif /* __INC_SHARED_SEMAPHORE_H */
diff --git a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
index 3e68d58fdff..db470c84793 100644
--- a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
+++ b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
@@ -5,7 +5,7 @@
  *
  * Author: Js HA <js.ha@stericsson.com> for ST-Ericsson
  * Author: Naveen Kumar G <naveen.gaddipati@stericsson.com> for ST-Ericsson
- * Copyright 2010 (c) ST-Ericsson AB
+ * Copyright 2010 (c) ST-Ericsson SA
  */
 /*
  * This file is licensed under the GPL2 license.
@@ -27,16 +27,20 @@
 
 #include <linux/input.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/i2c.h>
 #include <linux/interrupt.h>
 #include <linux/regulator/consumer.h>
+#include <linux/earlysuspend.h>
 #include "synaptics_i2c_rmi4.h"
 
 /* TODO: for multiple device support will need a per-device mutex */
 #define DRIVER_NAME "synaptics_rmi4_i2c"
 
+#define DELTA			8
 #define MAX_ERROR_REPORT	6
-#define MAX_TOUCH_MAJOR		15
+#define	TIMEOUT_PERIOD		1
+#define MAX_WIDTH_MAJOR		255
 #define MAX_RETRY_COUNT		5
 #define STD_QUERY_LEN		21
 #define PAGE_LEN		2
@@ -44,6 +48,7 @@
 #define BUF_LEN			37
 #define QUERY_LEN		9
 #define DATA_LEN		12
+#define RESUME_DELAY		100 /* msecs */
 #define HAS_TAP			0x01
 #define HAS_PALMDETECT		0x01
 #define HAS_ROTATE		0x02
@@ -163,6 +168,9 @@ struct synaptics_rmi4_device_info {
  * @regulator: pointer to the regulator structure
  * @wait: wait queue structure variable
  * @touch_stopped: flag to stop the thread function
+ * @enable: flag to enable/disable the driver event.
+ * @resume_wq_handler: work queue for resume the device
+ * @early_suspend: early suspend structure variable
  *
  * This structure gives the device data information.
  */
@@ -183,8 +191,16 @@ struct synaptics_rmi4_data {
 	struct regulator	*regulator;
 	wait_queue_head_t	wait;
 	bool			touch_stopped;
+	bool			enable;
+	struct work_struct	resume_wq_handler;
+	struct early_suspend	early_suspend;
 };
 
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void synaptics_rmi4_early_suspend(struct early_suspend *data);
+static void synaptics_rmi4_late_resume(struct early_suspend *data);
+#endif
+
 /**
  * synaptics_rmi4_set_page() - sets the page
  * @pdata: pointer to synaptics_rmi4_data structure
@@ -290,6 +306,133 @@ exit:
 }
 
 /**
+ * synaptics_rmi4_enable() - enable the touchpad driver event
+ * @pdata: pointer to synaptics_rmi4_data structure
+ *
+ * This function is to enable the touchpad driver event and returns integer.
+ */
+static int synaptics_rmi4_enable(struct synaptics_rmi4_data *pdata)
+{
+	int retval;
+	unsigned char intr_status;
+
+	if (pdata->board->regulator_en)
+		regulator_enable(pdata->regulator);
+	enable_irq(pdata->board->irq_number);
+	pdata->touch_stopped = false;
+
+	msleep(RESUME_DELAY);
+	retval = synaptics_rmi4_i2c_block_read(pdata,
+					pdata->fn01_data_base_addr + 1,
+					&intr_status,
+					pdata->number_of_interrupt_register);
+	if (retval < 0)
+		return retval;
+
+	retval = synaptics_rmi4_i2c_byte_write(pdata,
+					pdata->fn01_ctrl_base_addr + 1,
+					(intr_status | TOUCHPAD_CTRL_INTR));
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+/**
+ * synaptics_rmi4_disable() - disable the touchpad driver event
+ * @pdata: pointer to synaptics_rmi4_data structure
+ *
+ * This function is to disable the driver event and returns integer.
+ */
+
+static int synaptics_rmi4_disable(struct synaptics_rmi4_data *pdata)
+{
+	int retval;
+	unsigned char intr_status;
+
+	pdata->touch_stopped = true;
+	disable_irq(pdata->board->irq_number);
+
+	retval = synaptics_rmi4_i2c_block_read(pdata,
+					pdata->fn01_data_base_addr + 1,
+					&intr_status,
+					pdata->number_of_interrupt_register);
+	if (retval < 0)
+		return retval;
+
+	retval = synaptics_rmi4_i2c_byte_write(pdata,
+					pdata->fn01_ctrl_base_addr + 1,
+					(intr_status & ~TOUCHPAD_CTRL_INTR));
+	if (retval < 0)
+		return retval;
+	if (pdata->board->regulator_en)
+		regulator_disable(pdata->regulator);
+
+	return 0;
+}
+
+/**
+ * synaptics_rmi4_show_attr_enable() - show the touchpad enable value
+ * @dev: pointer to device data structure
+ * @attr: pointer to attribute structure
+ * @buf: pointer to character buffer
+ *
+ * This function is to show the touchpad enable value and returns ssize_t.
+ */
+static ssize_t synaptics_rmi4_show_attr_enable(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct synaptics_rmi4_data *pdata = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", pdata->enable);
+}
+
+/**
+ * synaptics_rmi4_store_attr_enable() - store the touchpad enable value
+ * @dev: pointer to device data structure
+ * @attr: pointer to attribute structure
+ * @buf: pointer to character buffer
+ * @count: number fo arguments
+ *
+ * This function is to store the touchpad enable value and returns ssize_t.
+ */
+static ssize_t synaptics_rmi4_store_attr_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct synaptics_rmi4_data *pdata = dev_get_drvdata(dev);
+	unsigned long val;
+	int retval = 0;
+
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if ((val != 0) && (val != 1))
+		return -EINVAL;
+
+	if (pdata->enable != val) {
+		pdata->enable = val ? true : false;
+		if (pdata->enable)
+			retval = synaptics_rmi4_enable(pdata);
+		else
+			retval = synaptics_rmi4_disable(pdata);
+
+	}
+	return ((retval < 0) ? retval : count);
+}
+
+static DEVICE_ATTR(enable, S_IWUSR | S_IRUGO,
+	synaptics_rmi4_show_attr_enable, synaptics_rmi4_store_attr_enable);
+
+static struct attribute *synaptics_rmi4_attrs[] = {
+	&dev_attr_enable.attr,
+	NULL,
+};
+
+static struct attribute_group synaptics_rmi4_attr_group = {
+	.attrs = synaptics_rmi4_attrs,
+};
+
+/**
  * synpatics_rmi4_touchpad_report() - reports for the rmi4 touchpad device
  * @pdata: pointer to synaptics_rmi4_data structure
  * @rfi: pointer to synaptics_rmi4_fn structure
@@ -315,8 +458,9 @@ static int synpatics_rmi4_touchpad_report(struct synaptics_rmi4_data *pdata,
 	unsigned char	data[DATA_LEN];
 	int	x[RMI4_NUMBER_OF_MAX_FINGERS];
 	int	y[RMI4_NUMBER_OF_MAX_FINGERS];
-	int	wx[RMI4_NUMBER_OF_MAX_FINGERS];
-	int	wy[RMI4_NUMBER_OF_MAX_FINGERS];
+	int	w[RMI4_NUMBER_OF_MAX_FINGERS];
+	static int	prv_x[RMI4_NUMBER_OF_MAX_FINGERS];
+	static int	prv_y[RMI4_NUMBER_OF_MAX_FINGERS];
 	struct	i2c_client *client = pdata->i2c_client;
 
 	/* get 2D sensor finger data */
@@ -375,11 +519,7 @@ static int synpatics_rmi4_touchpad_report(struct synaptics_rmi4_data *pdata,
 				y[touch_count]	=
 					(data[1] << 4) |
 					((data[2] >> 4) & MASK_4BIT);
-				wy[touch_count]	=
-						(data[3] >> 4) & MASK_4BIT;
-				wx[touch_count]	=
-						(data[3] & MASK_4BIT);
-
+				w[touch_count]	= data[3];
 				if (pdata->board->x_flip)
 					x[touch_count] =
 						pdata->sensor_max_x -
@@ -388,6 +528,25 @@ static int synpatics_rmi4_touchpad_report(struct synaptics_rmi4_data *pdata,
 					y[touch_count] =
 						pdata->sensor_max_y -
 								y[touch_count];
+				if (x[touch_count] < 0)
+					x[touch_count]	=	0;
+				else if (x[touch_count] >= pdata->sensor_max_x)
+					x[touch_count]	=
+							pdata->sensor_max_x - 1;
+
+				if (y[touch_count] < 0)
+					y[touch_count]	=	0;
+				else if (y[touch_count] >= pdata->sensor_max_y)
+					y[touch_count]	=
+							pdata->sensor_max_y - 1;
+			}
+			if ((abs(x[finger] - prv_x[finger]) < DELTA) &&
+				(abs(y[finger] - prv_y[finger]) < DELTA)) {
+				x[finger] = prv_x[finger];
+				y[finger] = prv_y[finger];
+			} else {
+				prv_x[finger] = x[finger];
+				prv_y[finger] = y[finger];
 			}
 			/* number of active touch points */
 			touch_count++;
@@ -398,7 +557,9 @@ static int synpatics_rmi4_touchpad_report(struct synaptics_rmi4_data *pdata,
 	if (touch_count) {
 		for (finger = 0; finger < touch_count; finger++) {
 			input_report_abs(pdata->input_dev, ABS_MT_TOUCH_MAJOR,
-						max(wx[finger] , wy[finger]));
+						max(x[finger] , y[finger]));
+			input_report_abs(pdata->input_dev, ABS_MT_WIDTH_MAJOR,
+								w[finger]);
 			input_report_abs(pdata->input_dev, ABS_MT_POSITION_X,
 								x[finger]);
 			input_report_abs(pdata->input_dev, ABS_MT_POSITION_Y,
@@ -501,7 +662,7 @@ static irqreturn_t synaptics_rmi4_irq(int irq, void *data)
 		touch_count = synaptics_rmi4_sensor_report(pdata);
 		if (touch_count)
 			wait_event_timeout(pdata->wait, pdata->touch_stopped,
-							msecs_to_jiffies(1));
+					msecs_to_jiffies(TIMEOUT_PERIOD));
 		else
 			break;
 	} while (!pdata->touch_stopped);
@@ -880,9 +1041,27 @@ static int synaptics_rmi4_i2c_query_device(struct synaptics_rmi4_data *pdata)
 }
 
 /**
+ * synaptics_rmi4_resume_handler() - work queue for resume handler
+ * @work:work_struct structure pointer
+ *
+ * This work queue handler used to resume the device and returns none
+ */
+static void synaptics_rmi4_resume_handler(struct work_struct *work)
+{
+	struct synaptics_rmi4_data *prmi4_data = container_of(work,
+				struct synaptics_rmi4_data, resume_wq_handler);
+	struct i2c_client *client = prmi4_data->i2c_client;
+	int retval;
+
+	retval = synaptics_rmi4_enable(prmi4_data);
+	if (retval < 0)
+		dev_err(&client->dev, "%s: resume failed\n", __func__);
+}
+
+/**
  * synaptics_rmi4_probe() - Initialze the i2c-client touchscreen driver
- * @i2c: i2c client structure pointer
- * @id:i2c device id pointer
+ * @client: i2c client structure pointer
+ * @dev_id:i2c device id pointer
  *
  * This function will allocate and initialize the instance
  * data and request the irq and set the instance data as the clients
@@ -984,7 +1163,16 @@ static int __devinit synaptics_rmi4_probe
 	input_set_abs_params(rmi4_data->input_dev, ABS_MT_POSITION_Y, 0,
 					rmi4_data->sensor_max_y, 0, 0);
 	input_set_abs_params(rmi4_data->input_dev, ABS_MT_TOUCH_MAJOR, 0,
-						MAX_TOUCH_MAJOR, 0, 0);
+			max(rmi4_data->sensor_max_y, rmi4_data->sensor_max_y),
+							0, 0);
+	input_set_abs_params(rmi4_data->input_dev, ABS_MT_WIDTH_MAJOR, 0,
+						MAX_WIDTH_MAJOR, 0, 0);
+
+	retval = input_register_device(rmi4_data->input_dev);
+	if (retval) {
+		dev_err(&client->dev, "%s:input register failed\n", __func__);
+		goto err_input_register;
+	}
 
 	/* Clear interrupts */
 	synaptics_rmi4_i2c_block_read(rmi4_data,
@@ -997,19 +1185,36 @@ static int __devinit synaptics_rmi4_probe
 	if (retval) {
 		dev_err(&client->dev, "%s:Unable to get attn irq %d\n",
 				__func__, platformdata->irq_number);
-		goto err_query_dev;
+		goto err_request_irq;
 	}
 
-	retval = input_register_device(rmi4_data->input_dev);
+	INIT_WORK(&rmi4_data->resume_wq_handler, synaptics_rmi4_resume_handler);
+
+	/* sysfs implementation for dynamic enable/disable the input event */
+	retval = sysfs_create_group(&client->dev.kobj,
+					&synaptics_rmi4_attr_group);
 	if (retval) {
-		dev_err(&client->dev, "%s:input register failed\n", __func__);
-		goto err_free_irq;
+		dev_err(&client->dev, "failed to create sysfs entries\n");
+		goto err_sysfs;
 	}
-
+	rmi4_data->enable		= true;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	rmi4_data->early_suspend.level =
+				EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	rmi4_data->early_suspend.suspend = synaptics_rmi4_early_suspend;
+	rmi4_data->early_suspend.resume = synaptics_rmi4_late_resume;
+	register_early_suspend(&rmi4_data->early_suspend);
+#endif
 	return retval;
 
-err_free_irq:
+err_sysfs:
+	cancel_work_sync(&rmi4_data->resume_wq_handler);
+err_request_irq:
 	free_irq(platformdata->irq_number, rmi4_data);
+	input_unregister_device(rmi4_data->input_dev);
+err_input_register:
+	i2c_set_clientdata(client, NULL);
 err_query_dev:
 	if (platformdata->regulator_en) {
 		regulator_disable(rmi4_data->regulator);
@@ -1035,8 +1240,10 @@ static int __devexit synaptics_rmi4_remove(struct i2c_client *client)
 	struct synaptics_rmi4_data *rmi4_data = i2c_get_clientdata(client);
 	const struct synaptics_rmi4_platform_data *pdata = rmi4_data->board;
 
+	sysfs_remove_group(&client->dev.kobj, &synaptics_rmi4_attr_group);
 	rmi4_data->touch_stopped = true;
 	wake_up(&rmi4_data->wait);
+	cancel_work_sync(&rmi4_data->resume_wq_handler);
 	free_irq(pdata->irq_number, rmi4_data);
 	input_unregister_device(rmi4_data->input_dev);
 	if (pdata->regulator_en) {
@@ -1048,6 +1255,7 @@ static int __devexit synaptics_rmi4_remove(struct i2c_client *client)
 	return 0;
 }
 
+#ifndef CONFIG_HAS_EARLYSUSPEND
 #ifdef CONFIG_PM
 /**
  * synaptics_rmi4_suspend() - suspend the touch screen controller
@@ -1059,32 +1267,11 @@ static int __devexit synaptics_rmi4_remove(struct i2c_client *client)
 static int synaptics_rmi4_suspend(struct device *dev)
 {
 	/* Touch sleep mode */
-	int retval;
-	unsigned char intr_status;
 	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
-	const struct synaptics_rmi4_platform_data *pdata = rmi4_data->board;
-
-	rmi4_data->touch_stopped = true;
-	disable_irq(pdata->irq_number);
-
-	retval = synaptics_rmi4_i2c_block_read(rmi4_data,
-				rmi4_data->fn01_data_base_addr + 1,
-				&intr_status,
-				rmi4_data->number_of_interrupt_register);
-	if (retval < 0)
-		return retval;
 
-	retval = synaptics_rmi4_i2c_byte_write(rmi4_data,
-					rmi4_data->fn01_ctrl_base_addr + 1,
-					(intr_status & ~TOUCHPAD_CTRL_INTR));
-	if (retval < 0)
-		return retval;
-
-	if (pdata->regulator_en)
-		regulator_disable(rmi4_data->regulator);
-
-	return 0;
+	return synaptics_rmi4_disable(rmi4_data);
 }
+
 /**
  * synaptics_rmi4_resume() - resume the touch screen controller
  * @dev: pointer to device structure
@@ -1094,29 +1281,9 @@ static int synaptics_rmi4_suspend(struct device *dev)
  */
 static int synaptics_rmi4_resume(struct device *dev)
 {
-	int retval;
-	unsigned char intr_status;
 	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
-	const struct synaptics_rmi4_platform_data *pdata = rmi4_data->board;
 
-	if (pdata->regulator_en)
-		regulator_enable(rmi4_data->regulator);
-
-	enable_irq(pdata->irq_number);
-	rmi4_data->touch_stopped = false;
-
-	retval = synaptics_rmi4_i2c_block_read(rmi4_data,
-				rmi4_data->fn01_data_base_addr + 1,
-				&intr_status,
-				rmi4_data->number_of_interrupt_register);
-	if (retval < 0)
-		return retval;
-
-	retval = synaptics_rmi4_i2c_byte_write(rmi4_data,
-					rmi4_data->fn01_ctrl_base_addr + 1,
-					(intr_status | TOUCHPAD_CTRL_INTR));
-	if (retval < 0)
-		return retval;
+	schedule_work(&rmi4_data->resume_wq_handler);
 
 	return 0;
 }
@@ -1127,6 +1294,32 @@ static const struct dev_pm_ops synaptics_rmi4_dev_pm_ops = {
 };
 #endif
 
+#else
+static void synaptics_rmi4_early_suspend(struct early_suspend *data)
+{
+	struct synaptics_rmi4_data *rmi4_data =
+		container_of(data, struct synaptics_rmi4_data, early_suspend);
+	struct i2c_client *client = rmi4_data->i2c_client;
+	int retval;
+
+	retval = synaptics_rmi4_disable(rmi4_data);
+	if (retval < 0)
+		dev_err(&client->dev, "rmi4 disable failed\n");
+}
+
+static void synaptics_rmi4_late_resume(struct early_suspend *data)
+{
+	struct synaptics_rmi4_data *rmi4_data =
+		container_of(data, struct synaptics_rmi4_data, early_suspend);
+	struct i2c_client *client = rmi4_data->i2c_client;
+	int retval;
+
+	retval = synaptics_rmi4_enable(rmi4_data);
+	if (retval < 0)
+		dev_err(&client->dev, "rmi4 enable failed\n");
+}
+#endif
+
 static const struct i2c_device_id synaptics_rmi4_id_table[] = {
 	{ DRIVER_NAME, 0 },
 	{ },
@@ -1137,7 +1330,7 @@ static struct i2c_driver synaptics_rmi4_driver = {
 	.driver = {
 		.name	=	DRIVER_NAME,
 		.owner	=	THIS_MODULE,
-#ifdef CONFIG_PM
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
 		.pm	=	&synaptics_rmi4_dev_pm_ops,
 #endif
 	},
diff --git a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h
index 3686a2ff596..973abc97374 100644
--- a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h
+++ b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h
@@ -34,7 +34,6 @@
  * @irq_type: irq type
  * @x flip: x flip flag
  * @y flip: y flip flag
- * @regulator_en: regulator enable flag
  *
  * This structure gives platform data for rmi4.
  */
diff --git a/drivers/tee/Kconfig b/drivers/tee/Kconfig
new file mode 100644
index 00000000000..a452e888d77
--- /dev/null
+++ b/drivers/tee/Kconfig
@@ -0,0 +1,13 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Martin Hovang (martin.xm.hovang@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+
+# Trursted Execution Environment Configuration
+config TEE_SUPPORT
+	bool "Trusted Execution Environment Support"
+	default y
+	---help---
+	  This implements the Trusted Execution Environment (TEE) Client
+	  API Specification from GlobalPlatform Device Technology.
diff --git a/drivers/tee/Makefile b/drivers/tee/Makefile
new file mode 100644
index 00000000000..b937eb19d72
--- /dev/null
+++ b/drivers/tee/Makefile
@@ -0,0 +1,8 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Martin Hovang (martin.xm.hovang@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+
+obj-$(CONFIG_TEE_SUPPORT) += tee_service.o
+obj-$(CONFIG_TEE_SUPPORT) += tee_driver.o
diff --git a/drivers/tee/tee_driver.c b/drivers/tee/tee_driver.c
new file mode 100644
index 00000000000..73c62871ddc
--- /dev/null
+++ b/drivers/tee/tee_driver.c
@@ -0,0 +1,643 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Martin Hovang <martin.xm.hovang@stericsson.com>
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/mutex.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/tee.h>
+#include <linux/slab.h>
+
+#define TEED_NAME "tee"
+
+#define TEED_STATE_OPEN_DEV 0
+#define TEED_STATE_OPEN_SESSION 1
+
+static struct mutex sync;
+
+static int tee_open(struct inode *inode, struct file *file);
+static int tee_release(struct inode *inode, struct file *file);
+static int tee_read(struct file *filp, char __user *buffer,
+		    size_t length, loff_t *offset);
+static int tee_write(struct file *filp, const char __user *buffer,
+		     size_t length, loff_t *offset);
+
+static inline void set_emsg(struct tee_session *ts, u32 msg)
+{
+	ts->err = msg;
+	ts->origin = TEED_ORIGIN_DRIVER;
+}
+
+static void reset_session(struct tee_session *ts)
+{
+	ts->state = TEED_STATE_OPEN_DEV;
+	ts->err = TEED_SUCCESS;
+	ts->origin = TEED_ORIGIN_DRIVER;
+	ts->id = 0;
+	ts->ta = NULL;
+	ts->uuid = NULL;
+	ts->cmd = 0;
+	ts->driver_cmd = TEED_OPEN_SESSION;
+	ts->ta_size = 0;
+	ts->op = NULL;
+}
+
+static int copy_ta(struct tee_session *ts,
+		   struct tee_session *ku_buffer)
+{
+	ts->ta = kmalloc(ku_buffer->ta_size, GFP_KERNEL);
+	if (ts->ta == NULL) {
+		pr_err("[%s] error, out of memory (ta)\n",
+		       __func__);
+		set_emsg(ts, TEED_ERROR_OUT_OF_MEMORY);
+		return -ENOMEM;
+	}
+
+	ts->ta_size = ku_buffer->ta_size;
+
+	memcpy(ts->ta, ku_buffer->ta, ku_buffer->ta_size);
+	return 0;
+}
+
+static int copy_uuid(struct tee_session *ts,
+		     struct tee_session *ku_buffer)
+{
+	ts->uuid = kmalloc(sizeof(struct tee_uuid), GFP_KERNEL);
+
+	if (ts->uuid == NULL) {
+		pr_err("[%s] error, out of memory (uuid)\n",
+		       __func__);
+		set_emsg(ts, TEED_ERROR_OUT_OF_MEMORY);
+		return -ENOMEM;
+	}
+
+	memcpy(ts->uuid, ku_buffer->uuid, sizeof(struct tee_uuid));
+
+	return 0;
+}
+
+static inline void free_operation(struct tee_session *ts)
+{
+	int i;
+
+	for (i = 0; i < 4; ++i) {
+		kfree(ts->op->shm[i].buffer);
+		ts->op->shm[i].buffer = NULL;
+	}
+
+	kfree(ts->op);
+	ts->op = NULL;
+}
+
+static inline void memrefs_phys_to_virt(struct tee_session *ts)
+{
+	int i;
+
+	for (i = 0; i < 4; ++i) {
+		if (ts->op->flags & (1 << i)) {
+			ts->op->shm[i].buffer =
+				phys_to_virt((unsigned long)
+					     ts->op->shm[i].buffer);
+		}
+	}
+}
+
+static int copy_memref_to_user(struct tee_operation *op,
+			       struct tee_operation *ubuf_op,
+			       int memref)
+{
+	unsigned long bytes_left;
+
+	bytes_left = copy_to_user(ubuf_op->shm[memref].buffer,
+				  op->shm[memref].buffer,
+				  op->shm[memref].size);
+
+	if (bytes_left != 0) {
+		pr_err("[%s] Failed to copy result to user space (%lu "
+		       "bytes left of buffer).\n", __func__, bytes_left);
+		return bytes_left;
+	}
+
+	bytes_left = put_user(op->shm[memref].size, &ubuf_op->shm[memref].size);
+
+	if (bytes_left != 0) {
+		pr_err("[%s] Failed to copy result to user space (%lu "
+		       "bytes left of size).\n", __func__, bytes_left);
+		return -EINVAL;
+	}
+
+	bytes_left = put_user(op->shm[memref].flags,
+			      &ubuf_op->shm[memref].flags);
+	if (bytes_left != 0) {
+		pr_err("[%s] Failed to copy result to user space (%lu "
+		       "bytes left of flags).\n", __func__, bytes_left);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int copy_memref_to_kernel(struct tee_operation *op,
+				 struct tee_operation *kbuf_op,
+				 int memref)
+{
+	/* Buffer freed in invoke_command if this function fails */
+	op->shm[memref].buffer = kmalloc(kbuf_op->shm[memref].size, GFP_KERNEL);
+
+	if (!op->shm[memref].buffer) {
+		pr_err("[%s] out of memory\n", __func__);
+		return -ENOMEM;
+	}
+
+	/*
+	 * Copy shared memory operations to a local kernel
+	 * buffer if they are of type input.
+	 */
+	if (kbuf_op->shm[memref].flags & TEEC_MEM_INPUT) {
+		memcpy(op->shm[memref].buffer,
+		       kbuf_op->shm[memref].buffer,
+		       kbuf_op->shm[memref].size);
+	}
+
+	op->shm[memref].size = kbuf_op->shm[memref].size;
+	op->shm[memref].flags = kbuf_op->shm[memref].flags;
+
+	/* Secure world expects physical addresses. */
+	op->shm[memref].buffer = (void *)virt_to_phys(op->shm[memref].buffer);
+
+	return 0;
+}
+
+static int open_tee_device(struct tee_session *ts,
+			   struct tee_session *ku_buffer)
+{
+	int ret;
+
+	if (ku_buffer->driver_cmd != TEED_OPEN_SESSION) {
+		set_emsg(ts, TEED_ERROR_BAD_STATE);
+		return -EINVAL;
+	}
+
+	if (ku_buffer->ta) {
+		ret = copy_ta(ts, ku_buffer);
+	} else if (ku_buffer->uuid) {
+		ret = copy_uuid(ts, ku_buffer);
+	} else {
+		set_emsg(ts, TEED_ERROR_COMMUNICATION);
+		return -EINVAL;
+	}
+
+	ts->id = 0;
+	ts->state = TEED_STATE_OPEN_SESSION;
+	return ret;
+}
+
+static int invoke_command(struct tee_session *ts,
+			  struct tee_session *ku_buffer,
+			  struct tee_session __user *u_buffer)
+{
+	int i;
+	int ret = 0;
+	struct tee_operation *kbuf_op =
+		(struct tee_operation *)ku_buffer->op;
+
+	ts->op = kmalloc(sizeof(struct tee_operation), GFP_KERNEL);
+
+	if (!ts->op) {
+		if (ts->op == NULL) {
+			pr_err("[%s] error, out of memory "
+			       "(op)\n", __func__);
+			set_emsg(ts, TEED_ERROR_OUT_OF_MEMORY);
+			ret = -ENOMEM;
+			goto err;
+		}
+	}
+
+	/* Copy memrefs to kernel space. */
+	ts->op->flags = kbuf_op->flags;
+	ts->cmd = ku_buffer->cmd;
+
+	for (i = 0; i < 4; ++i) {
+		/* We only want to copy memrefs in use. */
+		if (kbuf_op->flags & (1 << i)) {
+			ret = copy_memref_to_kernel(ts->op, kbuf_op, i);
+
+			if (ret)
+				goto err;
+		} else {
+			ts->op->shm[i].buffer = NULL;
+			ts->op->shm[i].size = 0;
+			ts->op->shm[i].flags = 0;
+		}
+	}
+
+	/* To call secure world */
+	if (call_sec_world(ts, TEED_INVOKE)) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/*
+	 * Convert physical addresses back to virtual address so the
+	 * kernel can free the buffers when closing the session.
+	 */
+	memrefs_phys_to_virt(ts);
+
+	for (i = 0; i < 4; ++i) {
+		if ((kbuf_op->flags & (1 << i)) &&
+		    (kbuf_op->shm[i].flags & TEEC_MEM_OUTPUT)) {
+			struct tee_operation *ubuf_op =
+				(struct tee_operation *)u_buffer->op;
+
+			ret = copy_memref_to_user(ts->op, ubuf_op, i);
+		}
+	}
+err:
+	free_operation(ts);
+
+	return ret;
+}
+
+static int tee_open(struct inode *inode, struct file *filp)
+{
+	struct tee_session *ts;
+
+	filp->private_data = kmalloc(sizeof(struct tee_session),
+				     GFP_KERNEL);
+
+	if (filp->private_data == NULL)
+		return -ENOMEM;
+
+	ts = (struct tee_session *) (filp->private_data);
+
+	reset_session(ts);
+
+	return 0;
+}
+
+static int tee_release(struct inode *inode, struct file *filp)
+{
+	struct tee_session *ts;
+	int i;
+
+	ts = (struct tee_session *) (filp->private_data);
+
+	if (ts == NULL)
+			goto no_ts;
+
+	if (ts->op) {
+		for (i = 0; i < 4; ++i) {
+			kfree(ts->op->shm[i].buffer);
+			ts->op->shm[i].buffer = NULL;
+		}
+	}
+
+	kfree(ts->op);
+	ts->op = NULL;
+
+	kfree(ts->ta);
+	ts->ta = NULL;
+
+no_ts:
+	kfree(filp->private_data);
+	filp->private_data = NULL;
+
+	return 0;
+}
+
+/*
+ * Called when a process, which already opened the dev file, attempts
+ * to read from it. This function gets the current status of the session.
+ */
+static int tee_read(struct file *filp, char __user *buffer,
+		    size_t length, loff_t *offset)
+{
+	struct tee_read buf;
+	struct tee_session *ts;
+
+	if (length != sizeof(struct tee_read)) {
+		pr_err("[%s] error, incorrect input length\n",
+		       __func__);
+		return -EINVAL;
+	}
+
+	ts = (struct tee_session *) (filp->private_data);
+
+	if (ts == NULL) {
+		pr_err("[%s] error, private_data not "
+		       "initialized\n", __func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&sync);
+
+	buf.err = ts->err;
+	buf.origin = ts->origin;
+
+	mutex_unlock(&sync);
+
+	if (copy_to_user(buffer, &buf, length)) {
+		pr_err("[%s] error, copy_to_user failed!\n",
+		       __func__);
+		return -EINVAL;
+	}
+
+	return length;
+}
+
+/*
+ * Called when a process writes to a dev file
+ */
+static int tee_write(struct file *filp, const char __user *buffer,
+		     size_t length, loff_t *offset)
+{
+	struct tee_session ku_buffer;
+	struct tee_session *ts;
+	int ret = length;
+
+	if (length != sizeof(struct tee_session)) {
+		pr_err("[%s] error, incorrect input length\n",
+		       __func__);
+		return -EINVAL;
+	}
+
+	if (copy_from_user(&ku_buffer, buffer, length)) {
+		pr_err("[%s] error, tee_session "
+		       "copy_from_user failed\n", __func__);
+		return -EINVAL;
+	}
+
+	ts = (struct tee_session *) (filp->private_data);
+
+	if (ts == NULL) {
+		pr_err("[%s] error, private_data not "
+		       "initialized\n", __func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&sync);
+
+	switch (ts->state) {
+	case TEED_STATE_OPEN_DEV:
+		ret = open_tee_device(ts, &ku_buffer);
+		break;
+
+	case TEED_STATE_OPEN_SESSION:
+		switch (ku_buffer.driver_cmd) {
+		case TEED_INVOKE:
+			ret = invoke_command(ts, &ku_buffer,
+					     (struct tee_session *)buffer);
+			break;
+
+		case TEED_CLOSE_SESSION:
+			/* no caching implemented yet... */
+			if (call_sec_world(ts, TEED_CLOSE_SESSION))
+				ret = -EINVAL;
+
+			kfree(ts->ta);
+			ts->ta = NULL;
+
+			reset_session(ts);
+			break;
+
+		default:
+			set_emsg(ts, TEED_ERROR_BAD_PARAMETERS);
+			ret = -EINVAL;
+		}
+		break;
+	default:
+		pr_err("[%s] unknown state\n", __func__);
+		set_emsg(ts, TEED_ERROR_BAD_STATE);
+		ret = -EINVAL;
+	}
+
+	/*
+	 * We expect that ret has value zero when reaching the end here.
+	 * If it has any other value some error must have occured.
+	 */
+	if (!ret)
+		ret = length;
+	else
+		ret = -EINVAL;
+
+	mutex_unlock(&sync);
+
+	return ret;
+}
+
+int teec_initialize_context(const char *name, struct tee_context *context)
+{
+	return TEED_SUCCESS;
+}
+EXPORT_SYMBOL(teec_initialize_context);
+
+int teec_finalize_context(struct tee_context *context)
+{
+	return TEED_SUCCESS;
+}
+EXPORT_SYMBOL(teec_finalize_context);
+
+int teec_open_session(struct tee_context *context,
+		      struct tee_session *session,
+		      const struct tee_uuid *destination,
+		      unsigned int connection_method,
+		      void *connection_data, struct tee_operation *operation,
+		      unsigned int *error_origin)
+{
+	int res = TEED_SUCCESS;
+
+	if (session == NULL || destination == NULL) {
+		pr_err("[%s] session or destination == NULL\n", __func__);
+		if (error_origin != NULL)
+			*error_origin = TEED_ORIGIN_DRIVER;
+		res = TEED_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	reset_session(session);
+
+	/*
+	 * Open a session towards an application already loaded inside
+	 * the TEE
+	 */
+	session->uuid = kmalloc(sizeof(struct tee_uuid), GFP_KERNEL);
+
+	if (session->uuid == NULL) {
+		pr_err("[%s] error, out of memory (uuid)\n",
+		       __func__);
+		if (error_origin != NULL)
+			*error_origin = TEED_ORIGIN_DRIVER;
+		res = TEED_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	memcpy(session->uuid, destination, sizeof(struct tee_uuid));
+
+	session->ta = NULL;
+	session->id = 0;
+
+exit:
+	return res;
+}
+EXPORT_SYMBOL(teec_open_session);
+
+int teec_close_session(struct tee_session *session)
+{
+	int res = TEED_SUCCESS;
+
+	mutex_lock(&sync);
+
+	if (session == NULL) {
+		pr_err("[%s] error, session == NULL\n", __func__);
+		res = TEED_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	if (call_sec_world(session, TEED_CLOSE_SESSION)) {
+		pr_err("[%s] error, call_sec_world failed\n", __func__);
+		res = TEED_ERROR_GENERIC;
+		goto exit;
+	}
+
+exit:
+	if (session != NULL) {
+		kfree(session->uuid);
+		session->uuid = NULL;
+	}
+
+	mutex_unlock(&sync);
+	return res;
+}
+EXPORT_SYMBOL(teec_close_session);
+
+int teec_invoke_command(
+	struct tee_session *session, unsigned int command_id,
+	struct tee_operation *operation,
+	unsigned int *error_origin)
+{
+	int res = TEED_SUCCESS;
+	int i;
+
+	mutex_lock(&sync);
+
+	if (session == NULL || operation == NULL || error_origin == NULL) {
+		pr_err("[%s] error, input parameters == NULL\n", __func__);
+		if (error_origin != NULL)
+			*error_origin = TEED_ORIGIN_DRIVER;
+		res = TEED_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	for (i = 0; i < 4; ++i) {
+		/* We only want to translate memrefs in use. */
+		if (operation->flags & (1 << i)) {
+			operation->shm[i].buffer =
+				(void *)virt_to_phys(
+					operation->shm[i].buffer);
+		}
+	}
+	session->op = operation;
+	session->cmd = command_id;
+
+	/*
+	 * Call secure world
+	 */
+	if (call_sec_world(session, TEED_INVOKE)) {
+		pr_err("[%s] error, call_sec_world failed\n", __func__);
+		if (error_origin != NULL)
+			*error_origin = TEED_ORIGIN_DRIVER;
+		res = TEED_ERROR_GENERIC;
+	}
+	if (session->err != TEED_SUCCESS) {
+		pr_err("[%s] error, call_sec_world failed\n", __func__);
+		if (error_origin != NULL)
+			*error_origin = session->origin;
+		res = session->err;
+	}
+
+	memrefs_phys_to_virt(session);
+	session->op = NULL;
+
+exit:
+	mutex_unlock(&sync);
+	return res;
+}
+EXPORT_SYMBOL(teec_invoke_command);
+
+int teec_allocate_shared_memory(struct tee_context *context,
+				struct tee_sharedmemory *shared_memory)
+{
+	int res = TEED_SUCCESS;
+
+	if (shared_memory == NULL) {
+		res = TEED_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	shared_memory->buffer = kmalloc(shared_memory->size,
+					GFP_KERNEL);
+
+	if (shared_memory->buffer == NULL) {
+		res = TEED_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+exit:
+	return res;
+}
+EXPORT_SYMBOL(teec_allocate_shared_memory);
+
+void teec_release_shared_memory(struct tee_sharedmemory *shared_memory)
+{
+	kfree(shared_memory->buffer);
+}
+EXPORT_SYMBOL(teec_release_shared_memory);
+
+static const struct file_operations tee_fops = {
+	.owner = THIS_MODULE,
+	.read = tee_read,
+	.write = tee_write,
+	.open = tee_open,
+	.release = tee_release,
+};
+
+static struct miscdevice tee_dev = {
+	MISC_DYNAMIC_MINOR,
+	TEED_NAME,
+	&tee_fops
+};
+
+static int __init tee_init(void)
+{
+	int err = 0;
+
+	err = misc_register(&tee_dev);
+
+	if (err) {
+		pr_err("[%s] error %d adding character device "
+		       "TEE\n", __func__, err);
+	}
+
+	mutex_init(&sync);
+
+	return err;
+}
+
+static void __exit tee_exit(void)
+{
+	misc_deregister(&tee_dev);
+}
+
+module_init(tee_init);
+module_exit(tee_exit);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION("Trusted Execution Enviroment driver");
diff --git a/drivers/tee/tee_service.c b/drivers/tee/tee_service.c
new file mode 100644
index 00000000000..b01e9d0ac39
--- /dev/null
+++ b/drivers/tee/tee_service.c
@@ -0,0 +1,17 @@
+/*
+ * TEE service to handle the calls to trusted applications.
+ *
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#include <linux/kernel.h>
+#include <linux/tee.h>
+#include <linux/device.h>
+
+int __weak call_sec_world(struct tee_session *ts, int sec_cmd)
+{
+	pr_info("[%s] Generic call_sec_world called!\n", __func__);
+
+	return 0;
+}
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index f5f6831b0a6..ff4761e482c 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -1493,6 +1493,14 @@ static void pl011_shutdown(struct uart_port *port)
 	if (uap->lcrh_rx != uap->lcrh_tx)
 		pl011_shutdown_channel(uap, uap->lcrh_tx);
 
+	if (uap->port.dev->platform_data) {
+		struct amba_pl011_data *plat;
+
+		plat = uap->port.dev->platform_data;
+		if (plat->exit)
+			plat->exit();
+	}
+
 	/*
 	 * Shut down the clock producer
 	 */
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index a428aa080a3..72150c7ea3a 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -1579,12 +1579,14 @@ static void choose_devnum(struct usb_device *udev)
 		 * bus->devnum_next. */
 		devnum = find_next_zero_bit(bus->devmap.devicemap, 128,
 					    bus->devnum_next);
-		if (devnum >= 128)
+		/* Due to Hardware bugs we need to reserve a device address
+		 * for flushing of endpoints. */
+		if (devnum >= 127)
 			devnum = find_next_zero_bit(bus->devmap.devicemap,
 						    128, 1);
-		bus->devnum_next = ( devnum >= 127 ? 1 : devnum + 1);
+		bus->devnum_next = devnum >= 126 ? 1 : devnum + 1;
 	}
-	if (devnum < 128) {
+	if (devnum < 127) {
 		set_bit(devnum, bus->devmap.devicemap);
 		udev->devnum = devnum;
 	}
diff --git a/drivers/usb/gadget/android.c b/drivers/usb/gadget/android.c
index b13633bb684..7cf30330068 100644
--- a/drivers/usb/gadget/android.c
+++ b/drivers/usb/gadget/android.c
@@ -154,7 +154,7 @@ static struct usb_configuration android_config_driver = {
 	.unbind		= android_unbind_config,
 	.bConfigurationValue = 1,
 	.bmAttributes	= USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
-	.bMaxPower	= 0xFA, /* 500ma */
+	.bMaxPower	= CONFIG_USB_GADGET_VBUS_DRAW / 2,
 };
 
 static void android_work(struct work_struct *data)
@@ -373,7 +373,7 @@ static int rndis_function_bind_config(struct android_usb_function *f,
 		rndis->ethaddr[0], rndis->ethaddr[1], rndis->ethaddr[2],
 		rndis->ethaddr[3], rndis->ethaddr[4], rndis->ethaddr[5]);
 
-	ret = gether_setup_name(c->cdev->gadget, rndis->ethaddr, "rndis");
+	ret = gether_setup(c->cdev->gadget, rndis->ethaddr);
 	if (ret) {
 		pr_err("%s: gether_setup failed\n", __func__);
 		return ret;
@@ -996,6 +996,19 @@ static int android_bind(struct usb_composite_dev *cdev)
 	strings_dev[STRING_SERIAL_IDX].id = id;
 	device_desc.iSerialNumber = id;
 
+	/*
+	 * As per USB compliance update, a device that is actively drawing
+	 * more than 100mA from USB must report itself as bus-powered in
+	 * the GetStatus(DEVICE) call.
+	 */
+	if (android_config_driver.bMaxPower <=
+			(USB_SELF_POWER_VBUS_MAX_DRAW / 2)) {
+		android_config_driver.bmAttributes =
+			USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER;
+		usb_gadget_set_selfpowered(gadget);
+	} else
+		android_config_driver.bmAttributes = USB_CONFIG_ATT_ONE;
+
 	gcnum = usb_gadget_controller_number(gadget);
 	if (gcnum >= 0)
 		device_desc.bcdDevice = cpu_to_le16(0x0200 + gcnum);
@@ -1040,6 +1053,7 @@ android_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *c)
 	struct android_dev		*dev = _android_dev;
 	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
 	struct usb_request		*req = cdev->req;
+	struct android_usb_function	**functions = dev->functions;
 	struct android_usb_function	*f;
 	int value = -EOPNOTSUPP;
 	unsigned long flags;
@@ -1049,7 +1063,7 @@ android_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *c)
 	req->length = 0;
 	gadget->ep0->driver_data = cdev;
 
-	list_for_each_entry(f, &dev->enabled_functions, enabled_list) {
+	while ((f = *functions++)) {
 		if (f->ctrlrequest) {
 			value = f->ctrlrequest(f, cdev, c);
 			if (value >= 0)
diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index 5440c6d8fc3..218302561a6 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -475,20 +475,6 @@ static int exception_in_progress(struct fsg_common *common)
 	return common->state > FSG_STATE_IDLE;
 }
 
-/* Make bulk-out requests be divisible by the maxpacket size */
-static void set_bulk_out_req_length(struct fsg_common *common,
-				    struct fsg_buffhd *bh, unsigned int length)
-{
-	unsigned int	rem;
-
-	bh->bulk_out_intended_length = length;
-	rem = length % common->bulk_out_maxpacket;
-	if (rem > 0)
-		length += common->bulk_out_maxpacket - rem;
-	bh->outreq->length = length;
-}
-
-
 /*-------------------------------------------------------------------------*/
 
 static int fsg_set_halt(struct fsg_dev *fsg, struct usb_ep *ep)
@@ -587,9 +573,9 @@ static void bulk_out_complete(struct usb_ep *ep, struct usb_request *req)
 	struct fsg_buffhd	*bh = req->context;
 
 	dump_msg(common, "bulk-out", req->buf, req->actual);
-	if (req->status || req->actual != bh->bulk_out_intended_length)
+	if (req->status || req->actual != req->length)
 		DBG(common, "%s --> %d, %u/%u\n", __func__,
-		    req->status, req->actual, bh->bulk_out_intended_length);
+		    req->status, req->actual, req->length);
 	if (req->status == -ECONNRESET)		/* Request was cancelled */
 		usb_ep_fifo_flush(ep);
 
@@ -981,7 +967,6 @@ static int do_write(struct fsg_common *common)
 			 * the bulk-out maxpacket size
 			 */
 			bh->outreq->length = amount;
-			bh->bulk_out_intended_length = amount;
 			bh->outreq->short_not_ok = 1;
 			if (!start_out_transfer(common, bh))
 				/* Dunno what to do if common->fsg is NULL */
@@ -1627,7 +1612,6 @@ static int throw_away_data(struct fsg_common *common)
 			 * the bulk-out maxpacket size.
 			 */
 			bh->outreq->length = amount;
-			bh->bulk_out_intended_length = amount;
 			bh->outreq->short_not_ok = 1;
 			if (!start_out_transfer(common, bh))
 				/* Dunno what to do if common->fsg is NULL */
@@ -2296,8 +2280,8 @@ static int get_next_command(struct fsg_common *common)
 	}
 
 	/* Queue a request to read a Bulk-only CBW */
-	set_bulk_out_req_length(common, bh, USB_BULK_CB_WRAP_LEN);
-	bh->outreq->short_not_ok = 1;
+	bh->outreq->length = USB_BULK_CB_WRAP_LEN;
+	bh->outreq->short_not_ok = 0;
 	if (!start_out_transfer(common, bh))
 		/* Don't know what to do if common->fsg is NULL */
 		return -EIO;
diff --git a/drivers/usb/gadget/file_storage.c b/drivers/usb/gadget/file_storage.c
index 0360f56221e..9d0e49c9631 100644
--- a/drivers/usb/gadget/file_storage.c
+++ b/drivers/usb/gadget/file_storage.c
@@ -497,19 +497,6 @@ static int exception_in_progress(struct fsg_dev *fsg)
 	return (fsg->state > FSG_STATE_IDLE);
 }
 
-/* Make bulk-out requests be divisible by the maxpacket size */
-static void set_bulk_out_req_length(struct fsg_dev *fsg,
-		struct fsg_buffhd *bh, unsigned int length)
-{
-	unsigned int	rem;
-
-	bh->bulk_out_intended_length = length;
-	rem = length % fsg->bulk_out_maxpacket;
-	if (rem > 0)
-		length += fsg->bulk_out_maxpacket - rem;
-	bh->outreq->length = length;
-}
-
 static struct fsg_dev			*the_fsg;
 static struct usb_gadget_driver		fsg_driver;
 
@@ -730,10 +717,9 @@ static void bulk_out_complete(struct usb_ep *ep, struct usb_request *req)
 	struct fsg_buffhd	*bh = req->context;
 
 	dump_msg(fsg, "bulk-out", req->buf, req->actual);
-	if (req->status || req->actual != bh->bulk_out_intended_length)
+	if (req->status || req->actual != req->length)
 		DBG(fsg, "%s --> %d, %u/%u\n", __func__,
-				req->status, req->actual,
-				bh->bulk_out_intended_length);
+				req->status, req->actual, req->length);
 	if (req->status == -ECONNRESET)		// Request was cancelled
 		usb_ep_fifo_flush(ep);
 
@@ -1349,8 +1335,7 @@ static int do_write(struct fsg_dev *fsg)
 
 			/* amount is always divisible by 512, hence by
 			 * the bulk-out maxpacket size */
-			bh->outreq->length = bh->bulk_out_intended_length =
-					amount;
+			bh->outreq->length = amount;
 			bh->outreq->short_not_ok = 1;
 			start_transfer(fsg, fsg->bulk_out, bh->outreq,
 					&bh->outreq_busy, &bh->state);
@@ -1979,8 +1964,7 @@ static int throw_away_data(struct fsg_dev *fsg)
 
 			/* amount is always divisible by 512, hence by
 			 * the bulk-out maxpacket size */
-			bh->outreq->length = bh->bulk_out_intended_length =
-					amount;
+			bh->outreq->length = amount;
 			bh->outreq->short_not_ok = 1;
 			start_transfer(fsg, fsg->bulk_out, bh->outreq,
 					&bh->outreq_busy, &bh->state);
@@ -2659,8 +2643,8 @@ static int get_next_command(struct fsg_dev *fsg)
 		}
 
 		/* Queue a request to read a Bulk-only CBW */
-		set_bulk_out_req_length(fsg, bh, USB_BULK_CB_WRAP_LEN);
-		bh->outreq->short_not_ok = 1;
+		bh->outreq->length = USB_BULK_CB_WRAP_LEN;
+		bh->outreq->short_not_ok = 0;
 		start_transfer(fsg, fsg->bulk_out, bh->outreq,
 				&bh->outreq_busy, &bh->state);
 
diff --git a/drivers/usb/gadget/storage_common.c b/drivers/usb/gadget/storage_common.c
index a872248f37d..da4db6d336b 100644
--- a/drivers/usb/gadget/storage_common.c
+++ b/drivers/usb/gadget/storage_common.c
@@ -286,13 +286,6 @@ struct fsg_buffhd {
 	enum fsg_buffer_state		state;
 	struct fsg_buffhd		*next;
 
-	/*
-	 * The NetChip 2280 is faster, and handles some protocol faults
-	 * better, if we don't submit any short bulk-out read requests.
-	 * So we will record the intended request length here.
-	 */
-	unsigned int			bulk_out_intended_length;
-
 	struct usb_request		*inreq;
 	int				inreq_busy;
 	struct usb_request		*outreq;
diff --git a/drivers/usb/musb/Kconfig b/drivers/usb/musb/Kconfig
index 13093481f91..f59efe2b3eb 100644
--- a/drivers/usb/musb/Kconfig
+++ b/drivers/usb/musb/Kconfig
@@ -183,3 +183,18 @@ config USB_TUSB_OMAP_DMA
 	help
 	  Enable DMA transfers on TUSB 6010 when OMAP DMA is available.
 
+config USB_UX500_DMA
+	bool
+	depends on USB_MUSB_HDRC && !MUSB_PIO_ONLY
+	default USB_MUSB_UX500
+	help
+	  Enable DMA transfers on UX500 platforms.
+
+config	USB_MUSB_DEBUG
+	depends on USB_MUSB_HDRC
+	bool "Enable debugging messages"
+	default n
+	help
+	  This enables musb debugging. To set the logging level use the debug
+	  module parameter. Starting at level 3, per-transfer (urb, usb_request,
+	  packet, or dma transfer) tracing may kick in.
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index dce7182e1df..1483f81551b 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -1127,8 +1127,8 @@ static struct musb_fifo_cfg __initdata mode_4_cfg[] = {
 
 /* mode 5 - fits in 8KB */
 static struct musb_fifo_cfg __initdata mode_5_cfg[] = {
-{ .hw_ep_num =  1, .style = FIFO_TX,   .maxpacket = 512, },
-{ .hw_ep_num =  1, .style = FIFO_RX,   .maxpacket = 512, },
+{ .hw_ep_num =  1, .style = FIFO_TX,   .maxpacket = 512, .mode = BUF_DOUBLE, },
+{ .hw_ep_num =  1, .style = FIFO_RX,   .maxpacket = 512, .mode = BUF_DOUBLE, },
 { .hw_ep_num =  2, .style = FIFO_TX,   .maxpacket = 512, },
 { .hw_ep_num =  2, .style = FIFO_RX,   .maxpacket = 512, },
 { .hw_ep_num =  3, .style = FIFO_TX,   .maxpacket = 512, },
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index 0e053b58796..78713c8259c 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -324,6 +324,7 @@ struct musb_hw_ep {
 
 	u8			rx_reinit;
 	u8			tx_reinit;
+	bool			do_tx_pio;
 #endif
 
 #ifdef CONFIG_USB_GADGET_MUSB_HDRC
diff --git a/drivers/usb/musb/musb_host.c b/drivers/usb/musb/musb_host.c
index 8b2473fa0f4..73ebcca75b0 100644
--- a/drivers/usb/musb/musb_host.c
+++ b/drivers/usb/musb/musb_host.c
@@ -108,24 +108,41 @@ static void musb_h_tx_flush_fifo(struct musb_hw_ep *ep)
 {
 	struct musb	*musb = ep->musb;
 	void __iomem	*epio = ep->regs;
+	void __iomem	*regs = ep->musb->mregs;
 	u16		csr;
-	u16		lastcsr = 0;
-	int		retries = 1000;
+	u8		addr;
+	int		retries = 3000; /* 3ms */
 
+	/*
+	 * NOTE: We are using a hack here because the FIFO-FLUSH
+	 * bit is broken in hardware! The hack consists of changing
+	 * the TXFUNCADDR to an unused device address and waiting
+	 * for any pending USB packets to hit the 3-strikes and your
+	 * gone rule.
+	 */
+	addr = musb_readb(regs, MUSB_BUSCTL_OFFSET(ep->epnum, MUSB_TXFUNCADDR));
 	csr = musb_readw(epio, MUSB_TXCSR);
 	while (csr & MUSB_TXCSR_FIFONOTEMPTY) {
-		if (csr != lastcsr)
-			dev_dbg(musb->controller, "Host TX FIFONOTEMPTY csr: %02x\n", csr);
-		lastcsr = csr;
-		csr |= MUSB_TXCSR_FLUSHFIFO;
-		musb_writew(epio, MUSB_TXCSR, csr);
+		musb_writeb(regs, MUSB_BUSCTL_OFFSET(ep->epnum,
+			MUSB_TXFUNCADDR), 127);
 		csr = musb_readw(epio, MUSB_TXCSR);
-		if (WARN(retries-- < 1,
-				"Could not flush host TX%d fifo: csr: %04x\n",
-				ep->epnum, csr))
-			return;
-		mdelay(1);
+		retries--;
+		if (retries == 0) {
+			/* can happen if the USB clocks are OFF */
+			dev_dbg(musb->controller, "Could not flush host TX%d "
+				"fifo: csr=0x%04x\n", ep->epnum, csr);
+			break;
+		}
+		udelay(1);
 	}
+	/* clear any errors */
+	csr &= ~(MUSB_TXCSR_H_ERROR
+		| MUSB_TXCSR_H_RXSTALL
+		| MUSB_TXCSR_H_NAKTIMEOUT);
+	musb_writew(epio, MUSB_TXCSR, csr);
+
+	/* restore endpoint address */
+	musb_writeb(regs, MUSB_BUSCTL_OFFSET(ep->epnum, MUSB_TXFUNCADDR), addr);
 }
 
 static void musb_h_ep0_flush_fifo(struct musb_hw_ep *ep)
@@ -290,7 +307,7 @@ start:
 		dev_dbg(musb->controller, "Start TX%d %s\n", epnum,
 			hw_ep->tx_channel ? "dma" : "pio");
 
-		if (!hw_ep->tx_channel)
+		if (!hw_ep->tx_channel || hw_ep->do_tx_pio)
 			musb_h_tx_start(hw_ep);
 		else if (is_cppi_enabled() || tusb_dma_omap())
 			musb_h_tx_dma_start(hw_ep);
@@ -615,7 +632,7 @@ static bool musb_tx_dma_program(struct dma_controller *dma,
 	u16			csr;
 	u8			mode;
 
-#ifdef	CONFIG_USB_INVENTRA_DMA
+#if defined(CONFIG_USB_INVENTRA_DMA) || defined(CONFIG_USB_UX500_DMA)
 	if (length > channel->max_len)
 		length = channel->max_len;
 
@@ -656,8 +673,6 @@ static bool musb_tx_dma_program(struct dma_controller *dma,
 
 	if (!dma->channel_program(channel, pkt_size, mode,
 			urb->transfer_dma + offset, length)) {
-		dma->channel_release(channel);
-		hw_ep->tx_channel = NULL;
 
 		csr = musb_readw(epio, MUSB_TXCSR);
 		csr &= ~(MUSB_TXCSR_AUTOSET | MUSB_TXCSR_DMAENAB);
@@ -792,11 +807,11 @@ static void musb_ep_program(struct musb *musb, u8 epnum,
 		else
 			load_count = min((u32) packet_sz, len);
 
-		if (dma_channel && musb_tx_dma_program(dma_controller,
+		if (!dma_channel || !musb_tx_dma_program(dma_controller,
 					hw_ep, qh, urb, offset, len))
-			load_count = 0;
+			hw_ep->do_tx_pio = true;
 
-		if (load_count) {
+		if (hw_ep->do_tx_pio) {
 			/* PIO to load FIFO */
 			qh->segsize = load_count;
 			musb_write_fifo(hw_ep, load_count, buf);
@@ -1100,7 +1115,7 @@ void musb_host_tx(struct musb *musb, u8 epnum)
 	struct urb		*urb = next_urb(qh);
 	u32			status = 0;
 	void __iomem		*mbase = musb->mregs;
-	struct dma_channel	*dma;
+	struct dma_channel	*dma = NULL;
 	bool			transfer_pending = false;
 
 	musb_ep_select(mbase, epnum);
@@ -1130,7 +1145,9 @@ void musb_host_tx(struct musb *musb, u8 epnum)
 		dev_dbg(musb->controller, "TX 3strikes on ep=%d\n", epnum);
 
 		status = -ETIMEDOUT;
-
+	} else if (tx_csr & MUSB_TXCSR_TXPKTRDY) {
+		/* BUSY - can happen during USB transfer cancel */
+		return;
 	} else if (tx_csr & MUSB_TXCSR_H_NAKTIMEOUT) {
 		dev_dbg(musb->controller, "TX end=%d device not responding\n", epnum);
 
@@ -1533,7 +1550,7 @@ void musb_host_rx(struct musb *musb, u8 epnum)
 
 	/* FIXME this is _way_ too much in-line logic for Mentor DMA */
 
-#ifndef CONFIG_USB_INVENTRA_DMA
+#if !defined(CONFIG_USB_INVENTRA_DMA) && !defined(CONFIG_USB_UX500_DMA)
 	if (rx_csr & MUSB_RXCSR_H_REQPKT)  {
 		/* REVISIT this happened for a while on some short reads...
 		 * the cleanup still needs investigation... looks bad...
@@ -1565,7 +1582,7 @@ void musb_host_rx(struct musb *musb, u8 epnum)
 			| MUSB_RXCSR_RXPKTRDY);
 		musb_writew(hw_ep->regs, MUSB_RXCSR, val);
 
-#ifdef CONFIG_USB_INVENTRA_DMA
+#if defined(CONFIG_USB_INVENTRA_DMA) || defined(CONFIG_USB_UX500_DMA)
 		if (usb_pipeisoc(pipe)) {
 			struct usb_iso_packet_descriptor *d;
 
@@ -1621,7 +1638,7 @@ void musb_host_rx(struct musb *musb, u8 epnum)
 		}
 
 		/* we are expecting IN packets */
-#ifdef CONFIG_USB_INVENTRA_DMA
+#if defined(CONFIG_USB_INVENTRA_DMA) || defined(CONFIG_USB_UX500_DMA)
 		if (dma) {
 			struct dma_controller	*c;
 			u16			rx_count;
@@ -1729,10 +1746,9 @@ void musb_host_rx(struct musb *musb, u8 epnum)
 				dma->desired_mode, buf, length);
 
 			if (!ret) {
-				c->channel_release(dma);
-				hw_ep->rx_channel = NULL;
 				dma = NULL;
-				/* REVISIT reset CSR */
+				/* Restore CSR */
+				musb_writew(epio, MUSB_RXCSR, rx_csr);
 			}
 		}
 #endif	/* Mentor DMA */
diff --git a/drivers/usb/musb/ux500_dma.c b/drivers/usb/musb/ux500_dma.c
index cecace41183..75ed4ff3b7d 100644
--- a/drivers/usb/musb/ux500_dma.c
+++ b/drivers/usb/musb/ux500_dma.c
@@ -65,7 +65,7 @@ static void ux500_tx_work(struct work_struct *data)
 	struct musb *musb = hw_ep->musb;
 	unsigned long flags;
 
-	DBG(4, "DMA tx transfer done on hw_ep=%d\n", hw_ep->epnum);
+	pr_warning("DMA tx transfer done on hw_ep=%d\n", hw_ep->epnum);
 
 	spin_lock_irqsave(&musb->lock, flags);
 	ux500_channel->channel.actual_len = ux500_channel->cur_len;
@@ -84,7 +84,7 @@ static void ux500_rx_work(struct work_struct *data)
 	struct musb *musb = hw_ep->musb;
 	unsigned long flags;
 
-	DBG(4, "DMA rx transfer done on hw_ep=%d\n", hw_ep->epnum);
+	pr_warning("DMA rx transfer done on hw_ep=%d\n", hw_ep->epnum);
 
 	spin_lock_irqsave(&musb->lock, flags);
 	ux500_channel->channel.actual_len = ux500_channel->cur_len;
@@ -117,7 +117,7 @@ static bool ux500_configure_channel(struct dma_channel *channel,
 	dma_addr_t usb_fifo_addr = (MUSB_FIFO_OFFSET(hw_ep->epnum) +
 					ux500_channel->controller->phy_base);
 
-	DBG(4, "packet_sz=%d, mode=%d, dma_addr=0x%x, len=%d is_tx=%d\n",
+	pr_warning("packet_sz=%d, mode=%d, dma_addr=0x%x, len=%d is_tx=%d\n",
 			packet_sz, mode, dma_addr, len, ux500_channel->is_tx);
 
 	ux500_channel->cur_len = len;
@@ -192,7 +192,7 @@ static struct dma_channel *ux500_dma_channel_allocate(struct dma_controller *c,
 	ux500_channel->hw_ep = hw_ep;
 	ux500_channel->is_allocated = 1;
 
-	DBG(7, "hw_ep=%d, is_tx=0x%x, channel=%d\n",
+	pr_devel("hw_ep=%d, is_tx=0x%x, channel=%d\n",
 		hw_ep->epnum, is_tx, ch_num);
 
 	return &(ux500_channel->channel);
@@ -202,7 +202,7 @@ static void ux500_dma_channel_release(struct dma_channel *channel)
 {
 	struct ux500_dma_channel *ux500_channel = channel->private_data;
 
-	DBG(7, "channel=%d\n", ux500_channel->ch_num);
+	pr_devel("channel=%d\n", ux500_channel->ch_num);
 
 	if (ux500_channel->is_allocated) {
 		ux500_channel->is_allocated = 0;
@@ -252,7 +252,7 @@ static int ux500_dma_channel_abort(struct dma_channel *channel)
 	void __iomem *epio = musb->endpoints[ux500_channel->hw_ep->epnum].regs;
 	u16 csr;
 
-	DBG(4, "channel=%d, is_tx=%d\n", ux500_channel->ch_num,
+	pr_warning("channel=%d, is_tx=%d\n", ux500_channel->ch_num,
 						ux500_channel->is_tx);
 
 	if (channel->status == MUSB_DMA_STATUS_BUSY) {
diff --git a/drivers/usb/otg/ab8500-usb.c b/drivers/usb/otg/ab8500-usb.c
index 07ccea9ada4..85daffaa2a5 100644
--- a/drivers/usb/otg/ab8500-usb.c
+++ b/drivers/usb/otg/ab8500-usb.c
@@ -29,22 +29,33 @@
 #include <linux/notifier.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/mfd/abx500.h>
 #include <linux/mfd/ab8500.h>
+#include <linux/regulator/consumer.h>
+#include <mach/prcmu.h>
+#include <mach/usb.h>
+
+#include <linux/wakelock.h>
+static struct wake_lock ab8500_musb_wakelock;
 
 #define AB8500_MAIN_WD_CTRL_REG 0x01
 #define AB8500_USB_LINE_STAT_REG 0x80
 #define AB8500_USB_PHY_CTRL_REG 0x8A
+#define AB8500_VBUS_CTRL_REG 0x82
 
 #define AB8500_BIT_OTG_STAT_ID (1 << 0)
 #define AB8500_BIT_PHY_CTRL_HOST_EN (1 << 0)
 #define AB8500_BIT_PHY_CTRL_DEVICE_EN (1 << 1)
 #define AB8500_BIT_WD_CTRL_ENABLE (1 << 0)
 #define AB8500_BIT_WD_CTRL_KICK (1 << 1)
+#define AB8500_BIT_VBUS_ENABLE (1 << 0)
 
 #define AB8500_V1x_LINK_STAT_WAIT (HZ/10)
 #define AB8500_WD_KICK_DELAY_US 100 /* usec */
 #define AB8500_WD_V11_DISABLE_DELAY_US 100 /* usec */
+#define AB8500_V20_31952_DISABLE_DELAY_US 100 /* usec */
 #define AB8500_WD_V10_DISABLE_DELAY_MS 100 /* ms */
 
 /* Usb line status register */
@@ -67,6 +78,13 @@ enum ab8500_usb_link_status {
 	USB_LINK_NOT_VALID_LINK
 };
 
+enum ab8500_usb_mode {
+	USB_IDLE = 0,
+	USB_PERIPHERAL,
+	USB_HOST,
+	USB_DEDICATED_CHG
+};
+
 struct ab8500_usb {
 	struct otg_transceiver otg;
 	struct device *dev;
@@ -80,6 +98,12 @@ struct ab8500_usb {
 	struct work_struct phy_dis_work;
 	unsigned long link_status_wait;
 	int rev;
+	enum ab8500_usb_mode mode;
+	struct clk *sysclk;
+	struct regulator *v_ape;
+	struct regulator *v_musb;
+	struct regulator *v_ulpi;
+	struct ab8500_usbgpio_platform_data *usb_gpio;
 };
 
 static inline struct ab8500_usb *xceiv_to_ab(struct otg_transceiver *x)
@@ -113,46 +137,107 @@ static void ab8500_usb_wd_workaround(struct ab8500_usb *ab)
 		0);
 }
 
-static void ab8500_usb_phy_ctrl(struct ab8500_usb *ab, bool sel_host,
+static void ab8500_usb_regulator_ctrl(struct ab8500_usb *ab, bool sel_host,
 					bool enable)
 {
-	u8 ctrl_reg;
-	abx500_get_register_interruptible(ab->dev,
-				AB8500_USB,
-				AB8500_USB_PHY_CTRL_REG,
-				&ctrl_reg);
-	if (sel_host) {
-		if (enable)
-			ctrl_reg |= AB8500_BIT_PHY_CTRL_HOST_EN;
-		else
-			ctrl_reg &= ~AB8500_BIT_PHY_CTRL_HOST_EN;
+	if (enable) {
+		regulator_enable(ab->v_ape);
+		regulator_enable(ab->v_ulpi);
+		regulator_enable(ab->v_musb);
+
+		if (sel_host && (ab->rev < 0x20))
+			/* Enable v-usb */
+			abx500_mask_and_set_register_interruptible(ab->dev,
+					AB8500_REGU_CTRL1,
+					AB8500_VBUS_CTRL_REG,
+					AB8500_BIT_VBUS_ENABLE,
+					AB8500_BIT_VBUS_ENABLE);
 	} else {
-		if (enable)
-			ctrl_reg |= AB8500_BIT_PHY_CTRL_DEVICE_EN;
-		else
-			ctrl_reg &= ~AB8500_BIT_PHY_CTRL_DEVICE_EN;
+		regulator_disable(ab->v_musb);
+		regulator_disable(ab->v_ulpi);
+		regulator_disable(ab->v_ape);
+
+		if (sel_host && (ab->rev < 0x20))
+			/* Disable v-usb */
+			abx500_mask_and_set_register_interruptible(ab->dev,
+					AB8500_REGU_CTRL1,
+					AB8500_VBUS_CTRL_REG,
+					AB8500_BIT_VBUS_ENABLE,
+					0);
 	}
+}
 
-	abx500_set_register_interruptible(ab->dev,
+static void ab8500_usb_phy_enable(struct ab8500_usb *ab, bool sel_host)
+{
+	u8 bit;
+	bit = sel_host ? AB8500_BIT_PHY_CTRL_HOST_EN :
+			AB8500_BIT_PHY_CTRL_DEVICE_EN;
+
+	wake_lock(&ab8500_musb_wakelock);
+	ab->usb_gpio->enable();
+	clk_enable(ab->sysclk);
+
+	ab8500_usb_regulator_ctrl(ab, sel_host, true);
+	prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP,
+				(char *)dev_name(ab->dev), 100);
+
+	abx500_mask_and_set_register_interruptible(ab->dev,
 				AB8500_USB,
 				AB8500_USB_PHY_CTRL_REG,
-				ctrl_reg);
+				bit,
+				bit);
 
 	/* Needed to enable the phy.*/
-	if (enable)
-		ab8500_usb_wd_workaround(ab);
+	ab8500_usb_wd_workaround(ab);
+}
+
+static void ab8500_usb_wd_linkstatus(struct ab8500_usb *ab,u8 bit)
+{
+	/* Wrokaround for v2.0 bug # 31952 */
+	if (ab->rev == 0x20) {
+		abx500_mask_and_set_register_interruptible(ab->dev,
+					AB8500_USB,
+					AB8500_USB_PHY_CTRL_REG,
+					bit,
+					bit);
+		udelay(AB8500_V20_31952_DISABLE_DELAY_US);
+	}
 }
 
-#define ab8500_usb_host_phy_en(ab)	ab8500_usb_phy_ctrl(ab, true, true)
-#define ab8500_usb_host_phy_dis(ab)	ab8500_usb_phy_ctrl(ab, true, false)
-#define ab8500_usb_peri_phy_en(ab)	ab8500_usb_phy_ctrl(ab, false, true)
-#define ab8500_usb_peri_phy_dis(ab)	ab8500_usb_phy_ctrl(ab, false, false)
+static void ab8500_usb_phy_disable(struct ab8500_usb *ab, bool sel_host)
+{
+	u8 bit;
+	bit = sel_host ? AB8500_BIT_PHY_CTRL_HOST_EN :
+			AB8500_BIT_PHY_CTRL_DEVICE_EN;
+
+	ab8500_usb_wd_linkstatus(ab,bit);
+
+	abx500_mask_and_set_register_interruptible(ab->dev,
+				AB8500_USB,
+				AB8500_USB_PHY_CTRL_REG,
+				bit,
+				0);
+
+	clk_disable(ab->sysclk);
+
+	ab8500_usb_regulator_ctrl(ab, sel_host, false);
+
+	ab->usb_gpio->disable();
+
+	prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP,
+				(char *)dev_name(ab->dev), 50);
+	wake_unlock(&ab8500_musb_wakelock);
+}
+
+#define ab8500_usb_host_phy_en(ab)	ab8500_usb_phy_enable(ab, true)
+#define ab8500_usb_host_phy_dis(ab)	ab8500_usb_phy_disable(ab, true)
+#define ab8500_usb_peri_phy_en(ab)	ab8500_usb_phy_enable(ab, false)
+#define ab8500_usb_peri_phy_dis(ab)	ab8500_usb_phy_disable(ab, false)
 
 static int ab8500_usb_link_status_update(struct ab8500_usb *ab)
 {
 	u8 reg;
 	enum ab8500_usb_link_status lsts;
-	void *v = NULL;
 	enum usb_xceiv_events event;
 
 	abx500_get_register_interruptible(ab->dev,
@@ -166,10 +251,11 @@ static int ab8500_usb_link_status_update(struct ab8500_usb *ab)
 	case USB_LINK_NOT_CONFIGURED:
 	case USB_LINK_RESERVED:
 	case USB_LINK_NOT_VALID_LINK:
-		/* TODO: Disable regulators. */
-		ab8500_usb_host_phy_dis(ab);
-		ab8500_usb_peri_phy_dis(ab);
-		ab->otg.state = OTG_STATE_B_IDLE;
+		if (ab->mode == USB_HOST)
+			ab8500_usb_host_phy_dis(ab);
+		else if (ab->mode == USB_PERIPHERAL)
+			ab8500_usb_peri_phy_dis(ab);
+		ab->mode = USB_IDLE;
 		ab->otg.default_a = false;
 		ab->vbus_draw = 0;
 		event = USB_EVENT_NONE;
@@ -182,20 +268,17 @@ static int ab8500_usb_link_status_update(struct ab8500_usb *ab)
 	case USB_LINK_HOST_CHG_HS:
 	case USB_LINK_HOST_CHG_HS_CHIRP:
 		if (ab->otg.gadget) {
-			/* TODO: Enable regulators. */
 			ab8500_usb_peri_phy_en(ab);
-			v = ab->otg.gadget;
+			ab->mode = USB_PERIPHERAL;
 		}
 		event = USB_EVENT_VBUS;
 		break;
 
 	case USB_LINK_HM_IDGND:
 		if (ab->otg.host) {
-			/* TODO: Enable regulators. */
 			ab8500_usb_host_phy_en(ab);
-			v = ab->otg.host;
+			ab->mode = USB_HOST;
 		}
-		ab->otg.state = OTG_STATE_A_IDLE;
 		ab->otg.default_a = true;
 		event = USB_EVENT_ID;
 		break;
@@ -208,11 +291,12 @@ static int ab8500_usb_link_status_update(struct ab8500_usb *ab)
 	case USB_LINK_ACA_RID_C_HS_CHIRP:
 	case USB_LINK_DEDICATED_CHG:
 		/* TODO: vbus_draw */
+		ab->mode = USB_DEDICATED_CHG;
 		event = USB_EVENT_CHARGER;
 		break;
 	}
 
-	atomic_notifier_call_chain(&ab->otg.notifier, event, v);
+	atomic_notifier_call_chain(&ab->otg.notifier, event, &ab->vbus_draw);
 
 	return 0;
 }
@@ -225,7 +309,7 @@ static void ab8500_usb_delayed_work(struct work_struct *work)
 	ab8500_usb_link_status_update(ab);
 }
 
-static irqreturn_t ab8500_usb_v1x_common_irq(int irq, void *data)
+static irqreturn_t ab8500_usb_v1x_connect_irq(int irq, void *data)
 {
 	struct ab8500_usb *ab = (struct ab8500_usb *) data;
 
@@ -235,20 +319,33 @@ static irqreturn_t ab8500_usb_v1x_common_irq(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t ab8500_usb_v1x_vbus_fall_irq(int irq, void *data)
+static irqreturn_t ab8500_usb_disconnect_irq(int irq, void *data)
 {
 	struct ab8500_usb *ab = (struct ab8500_usb *) data;
 
 	/* Link status will not be updated till phy is disabled. */
-	ab8500_usb_peri_phy_dis(ab);
+	if (ab->mode == USB_HOST)
+		ab8500_usb_host_phy_dis(ab);
+	else if (ab->mode == USB_PERIPHERAL)
+		ab8500_usb_peri_phy_dis(ab);
+	else if (ab->mode == USB_DEDICATED_CHG && ab->rev == 0x20) {
+		ab8500_usb_wd_linkstatus(ab,AB8500_BIT_PHY_CTRL_DEVICE_EN);
+		abx500_mask_and_set_register_interruptible(ab->dev,
+				AB8500_USB,
+				AB8500_USB_PHY_CTRL_REG,
+				AB8500_BIT_PHY_CTRL_DEVICE_EN,
+				0);
+	}
+	ab->mode = USB_IDLE;
 
-	/* Wait for link status to become stable. */
-	schedule_delayed_work(&ab->dwork, ab->link_status_wait);
+	if (ab->rev < 0x20)
+		/* Wait for link status to become stable. */
+		schedule_delayed_work(&ab->dwork, ab->link_status_wait);
 
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t ab8500_usb_v20_irq(int irq, void *data)
+static irqreturn_t ab8500_usb_v20_link_status_irq(int irq, void *data)
 {
 	struct ab8500_usb *ab = (struct ab8500_usb *) data;
 
@@ -267,6 +364,22 @@ static void ab8500_usb_phy_disable_work(struct work_struct *work)
 
 	if (!ab->otg.gadget)
 		ab8500_usb_peri_phy_dis(ab);
+
+}
+
+static unsigned ab8500_eyediagram_workaroud(struct ab8500_usb *ab, unsigned mA)
+{
+	if (mA > 100) {
+		/* AB V2 has eye diagram issues when drawing more
+		 * than 100mA from VBUS.So setting charging current
+		 * to 100mA in case of standard host
+		 */
+		if (ab->rev < 0x30)
+			mA = 100;
+		else
+			mA = 300;
+	}
+	return mA;
 }
 
 static int ab8500_usb_set_power(struct otg_transceiver *otg, unsigned mA)
@@ -278,18 +391,15 @@ static int ab8500_usb_set_power(struct otg_transceiver *otg, unsigned mA)
 
 	ab = xceiv_to_ab(otg);
 
+	mA = ab8500_eyediagram_workaroud(ab, mA);
+
 	ab->vbus_draw = mA;
 
-	if (mA)
-		atomic_notifier_call_chain(&ab->otg.notifier,
-				USB_EVENT_ENUMERATED, ab->otg.gadget);
+	atomic_notifier_call_chain(&ab->otg.notifier,
+				USB_EVENT_VBUS, &ab->vbus_draw);
 	return 0;
 }
 
-/* TODO: Implement some way for charging or other drivers to read
- * ab->vbus_draw.
- */
-
 static int ab8500_usb_set_suspend(struct otg_transceiver *x, int suspend)
 {
 	/* TODO */
@@ -312,12 +422,10 @@ static int ab8500_usb_set_peripheral(struct otg_transceiver *otg,
 	 */
 
 	if (!gadget) {
-		/* TODO: Disable regulators. */
 		ab->otg.gadget = NULL;
 		schedule_work(&ab->phy_dis_work);
 	} else {
 		ab->otg.gadget = gadget;
-		ab->otg.state = OTG_STATE_B_IDLE;
 
 		/* Phy will not be enabled if cable is already
 		 * plugged-in. Schedule to enable phy.
@@ -345,7 +453,6 @@ static int ab8500_usb_set_host(struct otg_transceiver *otg,
 	 */
 
 	if (!host) {
-		/* TODO: Disable regulators. */
 		ab->otg.host = NULL;
 		schedule_work(&ab->phy_dis_work);
 	} else {
@@ -360,118 +467,173 @@ static int ab8500_usb_set_host(struct otg_transceiver *otg,
 	return 0;
 }
 
+static void ab8500_usb_regulator_put(struct ab8500_usb *ab)
+{
+
+	if (ab->v_ape)
+		regulator_put(ab->v_ape);
+
+	if (ab->v_ulpi)
+		regulator_put(ab->v_ulpi);
+
+	if (ab->v_musb)
+		regulator_put(ab->v_musb);
+}
+
+static int ab8500_usb_regulator_get(struct ab8500_usb *ab)
+{
+	int err;
+
+	ab->v_ape = regulator_get(ab->dev, "v-ape");
+	if (IS_ERR(ab->v_ape)) {
+		dev_err(ab->dev, "Could not get v-ape supply\n");
+		err = PTR_ERR(ab->v_ape);
+		goto reg_error;
+	}
+
+	ab->v_ulpi = regulator_get(ab->dev, "vddulpivio18");
+	if (IS_ERR(ab->v_ulpi)) {
+		dev_err(ab->dev, "Could not get vddulpivio18 supply\n");
+		err = PTR_ERR(ab->v_ulpi);
+		goto reg_error;
+	}
+
+	ab->v_musb = regulator_get(ab->dev, "musb_1v8");
+	if (IS_ERR(ab->v_musb)) {
+		dev_err(ab->dev, "Could not get musb_1v8 supply\n");
+		err = PTR_ERR(ab->v_musb);
+		goto reg_error;
+	}
+
+	return 0;
+
+reg_error:
+	ab8500_usb_regulator_put(ab);
+	return err;
+}
+
 static void ab8500_usb_irq_free(struct ab8500_usb *ab)
 {
-	if (ab->rev < 0x20) {
+	if (ab->irq_num_id_rise)
 		free_irq(ab->irq_num_id_rise, ab);
+
+	if (ab->irq_num_id_fall)
 		free_irq(ab->irq_num_id_fall, ab);
+
+	if (ab->irq_num_vbus_rise)
 		free_irq(ab->irq_num_vbus_rise, ab);
+
+	if (ab->irq_num_vbus_fall)
 		free_irq(ab->irq_num_vbus_fall, ab);
-	} else {
+
+	if (ab->irq_num_link_status)
 		free_irq(ab->irq_num_link_status, ab);
-	}
 }
 
-static int ab8500_usb_v1x_res_setup(struct platform_device *pdev,
+static int ab8500_usb_irq_setup(struct platform_device *pdev,
 				struct ab8500_usb *ab)
 {
 	int err;
+	int irq;
 
-	ab->irq_num_id_rise = platform_get_irq_byname(pdev, "ID_WAKEUP_R");
-	if (ab->irq_num_id_rise < 0) {
-		dev_err(&pdev->dev, "ID rise irq not found\n");
-		return ab->irq_num_id_rise;
-	}
-	err = request_threaded_irq(ab->irq_num_id_rise, NULL,
-		ab8500_usb_v1x_common_irq,
-		IRQF_NO_SUSPEND | IRQF_SHARED,
-		"usb-id-rise", ab);
-	if (err < 0) {
-		dev_err(ab->dev, "request_irq failed for ID rise irq\n");
-		goto fail0;
+	if (ab->rev < 0x20) {
+		irq = platform_get_irq_byname(pdev, "ID_WAKEUP_R");
+		if (irq < 0) {
+			err = irq;
+			dev_err(&pdev->dev, "ID rise irq not found\n");
+			goto irq_fail;
+		}
+		err = request_threaded_irq(irq, NULL,
+			ab8500_usb_v1x_connect_irq,
+			IRQF_NO_SUSPEND | IRQF_SHARED,
+			"usb-id-rise", ab);
+		if (err < 0) {
+			dev_err(ab->dev,
+				"request_irq failed for ID rise irq\n");
+			goto irq_fail;
+		}
+		ab->irq_num_id_rise = irq;
+
+		irq = platform_get_irq_byname(pdev, "VBUS_DET_R");
+		if (irq < 0) {
+			err = irq;
+			dev_err(&pdev->dev, "VBUS rise irq not found\n");
+			goto irq_fail;
+		}
+		err = request_threaded_irq(irq, NULL,
+			ab8500_usb_v1x_connect_irq,
+			IRQF_NO_SUSPEND | IRQF_SHARED,
+			"usb-vbus-rise", ab);
+		if (err < 0) {
+			dev_err(ab->dev,
+				"request_irq failed for Vbus rise irq\n");
+			goto irq_fail;
+		}
+		ab->irq_num_vbus_rise = irq;
+	}else { /* 0x20 */
+		irq = platform_get_irq_byname(pdev, "USB_LINK_STATUS");
+		if (irq < 0) {
+			err = irq;
+			dev_err(&pdev->dev, "Link status irq not found\n");
+			goto irq_fail;
+		}
+
+		err = request_threaded_irq(irq, NULL,
+			ab8500_usb_v20_link_status_irq,
+			IRQF_NO_SUSPEND | IRQF_SHARED,
+			"usb-link-status", ab);
+		if (err < 0) {
+			dev_err(ab->dev,
+				"request_irq failed for link status irq\n");
+			return err;
+		}
+		ab->irq_num_link_status = irq;
 	}
 
-	ab->irq_num_id_fall = platform_get_irq_byname(pdev, "ID_WAKEUP_F");
-	if (ab->irq_num_id_fall < 0) {
+	irq = platform_get_irq_byname(pdev, "ID_WAKEUP_F");
+	if (irq < 0) {
+		err = irq;
 		dev_err(&pdev->dev, "ID fall irq not found\n");
 		return ab->irq_num_id_fall;
 	}
-	err = request_threaded_irq(ab->irq_num_id_fall, NULL,
-		ab8500_usb_v1x_common_irq,
+	err = request_threaded_irq(irq, NULL,
+		ab8500_usb_disconnect_irq,
 		IRQF_NO_SUSPEND | IRQF_SHARED,
 		"usb-id-fall", ab);
 	if (err < 0) {
 		dev_err(ab->dev, "request_irq failed for ID fall irq\n");
-		goto fail1;
+		goto irq_fail;
 	}
+	ab->irq_num_id_fall = irq;
 
-	ab->irq_num_vbus_rise = platform_get_irq_byname(pdev, "VBUS_DET_R");
-	if (ab->irq_num_vbus_rise < 0) {
-		dev_err(&pdev->dev, "VBUS rise irq not found\n");
-		return ab->irq_num_vbus_rise;
-	}
-	err = request_threaded_irq(ab->irq_num_vbus_rise, NULL,
-		ab8500_usb_v1x_common_irq,
-		IRQF_NO_SUSPEND | IRQF_SHARED,
-		"usb-vbus-rise", ab);
-	if (err < 0) {
-		dev_err(ab->dev, "request_irq failed for Vbus rise irq\n");
-		goto fail2;
-	}
-
-	ab->irq_num_vbus_fall = platform_get_irq_byname(pdev, "VBUS_DET_F");
-	if (ab->irq_num_vbus_fall < 0) {
+	irq = platform_get_irq_byname(pdev, "VBUS_DET_F");
+	if (irq < 0) {
+		err = irq;
 		dev_err(&pdev->dev, "VBUS fall irq not found\n");
-		return ab->irq_num_vbus_fall;
+		goto irq_fail;
 	}
-	err = request_threaded_irq(ab->irq_num_vbus_fall, NULL,
-		ab8500_usb_v1x_vbus_fall_irq,
+	err = request_threaded_irq(irq, NULL,
+		ab8500_usb_disconnect_irq,
 		IRQF_NO_SUSPEND | IRQF_SHARED,
 		"usb-vbus-fall", ab);
 	if (err < 0) {
 		dev_err(ab->dev, "request_irq failed for Vbus fall irq\n");
-		goto fail3;
+		goto irq_fail;
 	}
+	ab->irq_num_vbus_fall = irq;
 
 	return 0;
-fail3:
-	free_irq(ab->irq_num_vbus_rise, ab);
-fail2:
-	free_irq(ab->irq_num_id_fall, ab);
-fail1:
-	free_irq(ab->irq_num_id_rise, ab);
-fail0:
-	return err;
-}
-
-static int ab8500_usb_v2_res_setup(struct platform_device *pdev,
-				struct ab8500_usb *ab)
-{
-	int err;
 
-	ab->irq_num_link_status = platform_get_irq_byname(pdev,
-						"USB_LINK_STATUS");
-	if (ab->irq_num_link_status < 0) {
-		dev_err(&pdev->dev, "Link status irq not found\n");
-		return ab->irq_num_link_status;
-	}
-
-	err = request_threaded_irq(ab->irq_num_link_status, NULL,
-		ab8500_usb_v20_irq,
-		IRQF_NO_SUSPEND | IRQF_SHARED,
-		"usb-link-status", ab);
-	if (err < 0) {
-		dev_err(ab->dev,
-			"request_irq failed for link status irq\n");
-		return err;
-	}
-
-	return 0;
+irq_fail:
+	ab8500_usb_irq_free(ab);
+	return err;
 }
 
 static int __devinit ab8500_usb_probe(struct platform_device *pdev)
 {
 	struct ab8500_usb	*ab;
+	struct ab8500_platform_data *ab8500_pdata =
+				dev_get_platdata(pdev->dev.parent);
 	int err;
 	int rev;
 
@@ -492,11 +654,12 @@ static int __devinit ab8500_usb_probe(struct platform_device *pdev)
 	ab->rev			= rev;
 	ab->otg.dev		= ab->dev;
 	ab->otg.label		= "ab8500";
-	ab->otg.state		= OTG_STATE_UNDEFINED;
+	ab->otg.state		= OTG_STATE_B_IDLE;
 	ab->otg.set_host	= ab8500_usb_set_host;
 	ab->otg.set_peripheral	= ab8500_usb_set_peripheral;
 	ab->otg.set_suspend	= ab8500_usb_set_suspend;
 	ab->otg.set_power	= ab8500_usb_set_power;
+	ab->usb_gpio		=	ab8500_pdata->usb;
 
 	platform_set_drvdata(pdev, ab);
 
@@ -510,27 +673,49 @@ static int __devinit ab8500_usb_probe(struct platform_device *pdev)
 	/* all: Disable phy when called from set_host and set_peripheral */
 	INIT_WORK(&ab->phy_dis_work, ab8500_usb_phy_disable_work);
 
-	if (ab->rev < 0x20) {
-		err = ab8500_usb_v1x_res_setup(pdev, ab);
-		ab->link_status_wait = AB8500_V1x_LINK_STAT_WAIT;
-	} else {
-		err = ab8500_usb_v2_res_setup(pdev, ab);
+	err = ab8500_usb_regulator_get(ab);
+	if (err)
+		goto fail0;
+
+	ab->sysclk = clk_get(ab->dev, "sysclk");
+	if (IS_ERR(ab->sysclk)) {
+		err = PTR_ERR(ab->sysclk);
+		goto fail1;
 	}
 
+	if (ab->rev < 0x20)
+		ab->link_status_wait = AB8500_V1x_LINK_STAT_WAIT;
+
+	err = ab8500_usb_irq_setup(pdev, ab);
 	if (err < 0)
-		goto fail0;
+		goto fail2;
 
 	err = otg_set_transceiver(&ab->otg);
 	if (err) {
 		dev_err(&pdev->dev, "Can't register transceiver\n");
-		goto fail1;
+		goto fail3;
 	}
 
-	dev_info(&pdev->dev, "AB8500 usb driver initialized\n");
+	/* Needed to enable ID detection. */
+	ab8500_usb_wd_workaround(ab);
 
+	err = ab->usb_gpio->get(ab->dev);
+	if (err < 0)
+		goto fail3;
+
+	prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP,
+			(char *)dev_name(ab->dev), 50);
+
+	dev_info(&pdev->dev, "revision 0x%2x driver initialized\n", ab->rev);
+
+	wake_lock_init(&ab8500_musb_wakelock, WAKE_LOCK_SUSPEND, "ab8500-usb");
 	return 0;
-fail1:
+fail3:
 	ab8500_usb_irq_free(ab);
+fail2:
+	clk_put(ab->sysclk);
+fail1:
+	ab8500_usb_regulator_put(ab);
 fail0:
 	kfree(ab);
 	return err;
@@ -548,8 +733,16 @@ static int __devexit ab8500_usb_remove(struct platform_device *pdev)
 
 	otg_set_transceiver(NULL);
 
-	ab8500_usb_host_phy_dis(ab);
-	ab8500_usb_peri_phy_dis(ab);
+	if (ab->mode == USB_HOST)
+		ab8500_usb_host_phy_dis(ab);
+	else if (ab->mode == USB_PERIPHERAL)
+		ab8500_usb_peri_phy_dis(ab);
+
+	clk_put(ab->sysclk);
+
+	ab8500_usb_regulator_put(ab);
+
+	ab->usb_gpio->put();
 
 	platform_set_drvdata(pdev, NULL);
 
diff --git a/drivers/usb/otg/otg_id.c b/drivers/usb/otg/otg_id.c
index ce22b462130..2398b1a951d 100644
--- a/drivers/usb/otg/otg_id.c
+++ b/drivers/usb/otg/otg_id.c
@@ -44,24 +44,16 @@ static void __otg_id_notify(void)
 {
 	int ret;
 	struct otg_id_notifier_block *otg_id_nb;
-	bool proxy_wait = false;
+
 	if (plist_head_empty(&otg_id_plist))
 		return;
 
 	plist_for_each_entry(otg_id_nb, &otg_id_plist, p) {
-		if (proxy_wait) {
-			if (otg_id_nb->proxy_wait)
-				ret = otg_id_nb->proxy_wait(otg_id_nb);
-		} else {
-			ret = otg_id_nb->detect(otg_id_nb);
-		}
+		ret = otg_id_nb->detect(otg_id_nb);
 		if (ret == OTG_ID_HANDLED) {
 			otg_id_active = otg_id_nb;
 			return;
 		}
-		if (ret == OTG_ID_PROXY_WAIT)
-			proxy_wait = true;
-
 	}
 
 	WARN(1, "otg id event not handled");
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 4c85a4b15f9..8f043091f42 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -279,6 +279,8 @@ config FB_CIRRUS
 	  Say N unless you have such a graphics board or plan to get one
 	  before you next recompile the kernel.
 
+source "drivers/video/mcde/Kconfig"
+
 config FB_PM2
 	tristate "Permedia2 support"
 	depends on FB && ((AMIGA && BROKEN) || PCI)
@@ -2390,6 +2392,8 @@ source "drivers/video/omap2/Kconfig"
 
 source "drivers/video/backlight/Kconfig"
 source "drivers/video/display/Kconfig"
+source "drivers/video/av8100/Kconfig"
+source "drivers/video/b2r2/Kconfig"
 
 if VT
 	source "drivers/video/console/Kconfig"
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 8b83129e209..06a239f5ef7 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -134,6 +134,9 @@ obj-$(CONFIG_FB_SH_MOBILE_MERAM)  += sh_mobile_meram.o
 obj-$(CONFIG_FB_SH_MOBILE_LCDC)	  += sh_mobile_lcdcfb.o
 obj-$(CONFIG_FB_OMAP)             += omap/
 obj-y                             += omap2/
+obj-$(CONFIG_FB_MCDE)		  += mcde/
+obj-$(CONFIG_AV8100)		  += av8100/
+obj-y				  += b2r2/
 obj-$(CONFIG_XEN_FBDEV_FRONTEND)  += xen-fbfront.o
 obj-$(CONFIG_FB_CARMINE)          += carminefb.o
 obj-$(CONFIG_FB_MB862XX)	  += mb862xx/
diff --git a/drivers/video/av8100/Kconfig b/drivers/video/av8100/Kconfig
new file mode 100644
index 00000000000..40b9943aaa9
--- /dev/null
+++ b/drivers/video/av8100/Kconfig
@@ -0,0 +1,48 @@
+config AV8100
+       tristate "AV8100 driver support(HDMI/CVBS)"
+       default n
+       help
+           Please enable this feature if hdmi/tvout driver support is required.
+
+config HDMI_AV8100_DEBUG
+	bool "HDMI and AV8100 debug messages"
+	default n
+	depends on AV8100
+	---help---
+	  Say Y here if you want the HDMI and AV8100 driver to
+	  output debug messages.
+
+choice
+	prompt "AV8100 HW trig method"
+	default AV8100_HWTRIG_DSI_TE
+
+config AV8100_HWTRIG_INT
+	bool "AV8100 HW trig on INT"
+	depends on AV8100
+	---help---
+	  If you say Y here AV8100 will use HW triggering
+	  from AV8100 INT to MCDE sync0.
+
+config AV8100_HWTRIG_I2SDAT3
+	bool "AV8100 HW trig on I2SDAT3"
+	depends on AV8100
+	---help---
+	  If you say Y here AV8100 will use HW triggering
+	  from AV8100 I2SDAT3 to MCDE sync1.
+
+config AV8100_HWTRIG_DSI_TE
+	bool "AV8100 HW trig on DSI"
+	depends on AV8100
+	---help---
+	  If you say Y here AV8100 will use HW triggering
+	  using DSI TE polling between AV8100 and MCDE.
+
+config AV8100_HWTRIG_NONE
+	bool "AV8100 SW trig"
+	depends on AV8100
+	---help---
+	  If you say Y here AV8100 will use SW triggering
+	  between AV8100 and MCDE.
+
+endchoice
+
diff --git a/drivers/video/av8100/Makefile b/drivers/video/av8100/Makefile
new file mode 100644
index 00000000000..2d3028b18ca
--- /dev/null
+++ b/drivers/video/av8100/Makefile
@@ -0,0 +1,10 @@
+# Make file for compiling and loadable module HDMI
+
+obj-$(CONFIG_AV8100)		+= av8100.o hdmi.o
+
+ifdef CONFIG_HDMI_AV8100_DEBUG
+EXTRA_CFLAGS += -DDEBUG
+endif
+
+clean-files := av8100.o hdmi.o built-in.o modules.order
+
diff --git a/drivers/video/av8100/av8100.c b/drivers/video/av8100/av8100.c
new file mode 100644
index 00000000000..a6160e088eb
--- /dev/null
+++ b/drivers/video/av8100/av8100.c
@@ -0,0 +1,3822 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * AV8100 driver
+ *
+ * Author: Per Persson <per.xb.persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/i2c.h>
+#include <linux/fs.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/kthread.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/timer.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+#include <mach/prcmu.h>
+
+#include "av8100_regs.h"
+#include <video/av8100.h>
+#include <video/hdmi.h>
+#include "av8100_fw.h"
+
+/* Interrupts */
+#define AV8100_INT_EVENT 0x1
+#define AV8100_PLUGSTARTUP_EVENT 0x4
+
+#define AV8100_PLUGSTARTUP_TIME 100
+
+/* Standby search time */
+#define AV8100_ON_TIME 1	/* 9 ms step */
+#define AV8100_DENC_OFF_TIME 3	/* 275 ms step if > V1. Not used if V1 */
+#define AV8100_HDMI_OFF_TIME 2	/* 140 ms step if V2. 80 ms step if V1 */
+
+/* Command offsets */
+#define AV8100_COMMAND_OFFSET		0x10
+#define AV8100_COMMAND_MAX_LENGTH	0x81
+#define AV8100_CMD_BUF_OFFSET		(AV8100_COMMAND_OFFSET + 1)
+#define AV8100_2ND_RET_BYTE_OFFSET	(AV8100_COMMAND_OFFSET + 1)
+#define AV8100_CEC_RET_BUF_OFFSET	(AV8100_COMMAND_OFFSET + 4)
+#define AV8100_HDCP_RET_BUF_OFFSET	(AV8100_COMMAND_OFFSET + 2)
+#define AV8100_EDID_RET_BUF_OFFSET	(AV8100_COMMAND_OFFSET + 1)
+#define AV8100_FUSE_CRC_OFFSET		(AV8100_COMMAND_OFFSET + 2)
+#define AV8100_FUSE_PRGD_OFFSET		(AV8100_COMMAND_OFFSET + 3)
+#define AV8100_CRC32_OFFSET		(AV8100_COMMAND_OFFSET + 2)
+#define AV8100_CEC_ADDR_OFFSET		(AV8100_COMMAND_OFFSET + 3)
+
+/* Tearing effect line numbers */
+#define AV8100_TE_LINE_NB_14	14
+#define AV8100_TE_LINE_NB_17	17
+#define AV8100_TE_LINE_NB_18	18
+#define AV8100_TE_LINE_NB_21	21
+#define AV8100_TE_LINE_NB_22	22
+#define AV8100_TE_LINE_NB_24	24
+#define AV8100_TE_LINE_NB_25	25
+#define AV8100_TE_LINE_NB_26	26
+#define AV8100_TE_LINE_NB_29	29
+#define AV8100_TE_LINE_NB_30	30
+#define AV8100_TE_LINE_NB_32	32
+#define AV8100_TE_LINE_NB_38	38
+#define AV8100_TE_LINE_NB_40	40
+#define AV8100_UI_X4_DEFAULT	6
+
+#define HDMI_REQUEST_FOR_REVOCATION_LIST_INPUT 2
+#define HDMI_CEC_MESSAGE_WRITE_BUFFER_SIZE 16
+#define HDMI_HDCP_SEND_KEY_SIZE 7
+#define HDMI_INFOFRAME_DATA_SIZE 28
+#define HDMI_FUSE_AES_KEY_SIZE 16
+#define HDMI_FUSE_AES_KEY_RET_SIZE 2
+#define HDMI_LOADAES_END_BLK_NR 145
+#define HDMI_CRC32_SIZE 4
+#define HDMI_REVOC_LIST_SIZE 30
+#define HDMI_EDIDREAD_SIZE 0x7F
+
+#define HPDS_INVALID 0xF
+#define CPDS_INVALID 0xF
+#define CECRX_INVALID 0xF
+
+#define REG_16_8_LSB(p)		((u8)(p & 0xFF))
+#define REG_16_8_MSB(p)		((u8)((p & 0xFF00)>>8))
+#define REG_32_8_MSB(p)		((u8)((p & 0xFF000000)>>24))
+#define REG_32_8_MMSB(p)	((u8)((p & 0x00FF0000)>>16))
+#define REG_32_8_MLSB(p)	((u8)((p & 0x0000FF00)>>8))
+#define REG_32_8_LSB(p)		((u8)(p & 0x000000FF))
+#define REG_10_8_MSB(p)		((u8)((p & 0x300)>>8))
+#define REG_12_8_MSB(p)		((u8)((p & 0xf00)>>8))
+
+#define AV8100_WAITTIME_1MS 1
+#define AV8100_WAITTIME_5MS 5
+#define AV8100_WAITTIME_10MS 10
+#define AV8100_WAITTIME_50MS 50
+#define AV8100_WATTIME_100US 100
+
+/* Master clock timing */
+#define MCLK_RNG_17_22	0	/* Use with 19.2 MHZ clock */
+#define MCLK_RNG_22_27	1
+#define MCLK_RNG_27_33	2
+#define MCLK_RNG_31_38	3	/* Use with 38.4 MHZ clock */
+#define MCLK_RNG_36_44	4
+#define MCLK_RNG_40_49	5
+#define MCLK_RNG_45_55	6
+#define MCLK_RNG_49_60	7
+#define MCLK_RNG_54_66	8
+#define MCLK_RNG_58_71	9
+#define MCLK_RNG_63_77	10
+#define MCLK_RNG_67_82	11
+#define MCLK_RNG_72_88	12
+#define MCLK_RNG_76_93	13
+#define MCLK_RNG_85_104	14
+#define MCLK_RNG_94_115	15
+
+DEFINE_MUTEX(av8100_hw_mutex);
+#define LOCK_AV8100_HW mutex_lock(&av8100_hw_mutex)
+#define UNLOCK_AV8100_HW mutex_unlock(&av8100_hw_mutex)
+
+struct color_conversion_cmd {
+	unsigned short	c0;
+	unsigned short	c1;
+	unsigned short	c2;
+	unsigned short	c3;
+	unsigned short	c4;
+	unsigned short	c5;
+	unsigned short	c6;
+	unsigned short	c7;
+	unsigned short	c8;
+	unsigned short	aoffset;
+	unsigned short	boffset;
+	unsigned short	coffset;
+	unsigned char	lmax;
+	unsigned char	lmin;
+	unsigned char	cmax;
+	unsigned char	cmin;
+};
+
+struct av8100_config_t {
+	struct i2c_client *client;
+	struct i2c_device_id *id;
+	struct av8100_video_input_format_cmd hdmi_video_input_cmd;
+	struct av8100_audio_input_format_cmd hdmi_audio_input_cmd;
+	struct av8100_video_output_format_cmd hdmi_video_output_cmd;
+	struct av8100_video_scaling_format_cmd hdmi_video_scaling_cmd;
+	enum av8100_color_transform color_transform;
+	struct av8100_cec_message_write_format_cmd
+		hdmi_cec_message_write_cmd;
+	struct av8100_cec_message_read_back_format_cmd
+		hdmi_cec_message_read_back_cmd;
+	struct av8100_denc_format_cmd hdmi_denc_cmd;
+	struct av8100_hdmi_cmd hdmi_cmd;
+	struct av8100_hdcp_send_key_format_cmd hdmi_hdcp_send_key_cmd;
+	struct av8100_hdcp_management_format_cmd
+		hdmi_hdcp_management_format_cmd;
+	struct av8100_infoframes_format_cmd	hdmi_infoframes_cmd;
+	struct av8100_edid_section_readback_format_cmd
+		hdmi_edid_section_readback_cmd;
+	struct av8100_pattern_generator_format_cmd hdmi_pattern_generator_cmd;
+	struct av8100_fuse_aes_key_format_cmd hdmi_fuse_aes_key_cmd;
+};
+
+enum av8100_plug_state {
+	AV8100_UNPLUGGED,
+	AV8100_PLUGGED_STARTUP,
+	AV8100_PLUGGED
+};
+
+struct av8100_globals_t {
+	int denc_off_time;/* 5 volt time */
+	int hdmi_off_time;/* 5 volt time */
+	int on_time;/* 5 volt time */
+	u8 hpdm;/*stby_int_mask*/
+	u8 cpdm;/*stby_int_mask*/
+	u8 cecm;/*gen_int_mask*/
+	u8 hdcpm;/*gen_int_mask*/
+	void (*hdmi_ev_cb)(enum av8100_hdmi_event);
+	enum av8100_plug_state plug_state;
+	struct clk *inputclk;
+	bool inputclk_requested;
+	bool opp_requested;
+	struct regulator *regulator_pwr;
+	bool regulator_requested;
+};
+
+/**
+ * struct av8100_cea - CEA(consumer electronic access) standard structure
+ * @cea_id:
+ * @cea_nb:
+ * @vtotale:
+ **/
+
+struct av8100_cea {
+	char cea_id[40];
+	int cea_nb;
+	int vtotale;
+	int vactive;
+	int vsbp;
+	int vslen;
+	int vsfp;
+	char vpol[5];
+	int htotale;
+	int hactive;
+	int hbp;
+	int hslen;
+	int hfp;
+	int frequence;
+	char hpol[5];
+	int reg_line_duration;
+	int blkoel_duration;
+	int uix4;
+	int pll_mult;
+	int pll_div;
+};
+
+enum av8100_command_size {
+	AV8100_COMMAND_VIDEO_INPUT_FORMAT_SIZE  = 0x17,
+	AV8100_COMMAND_AUDIO_INPUT_FORMAT_SIZE  = 0x8,
+	AV8100_COMMAND_VIDEO_OUTPUT_FORMAT_SIZE = 0x1E,
+	AV8100_COMMAND_VIDEO_SCALING_FORMAT_SIZE = 0x11,
+	AV8100_COMMAND_COLORSPACECONVERSION_SIZE = 0x1D,
+	AV8100_COMMAND_CEC_MESSAGE_WRITE_SIZE = 0x12,
+	AV8100_COMMAND_CEC_MESSAGE_READ_BACK_SIZE = 0x1,
+	AV8100_COMMAND_DENC_SIZE = 0x6,
+	AV8100_COMMAND_HDMI_SIZE = 0x4,
+	AV8100_COMMAND_HDCP_SENDKEY_SIZE = 0xA,
+	AV8100_COMMAND_HDCP_MANAGEMENT_SIZE = 0x3,
+	AV8100_COMMAND_INFOFRAMES_SIZE = 0x21,
+	AV8100_COMMAND_EDID_SECTION_READBACK_SIZE  = 0x3,
+	AV8100_COMMAND_PATTERNGENERATOR_SIZE  = 0x4,
+	AV8100_COMMAND_FUSE_AES_KEY_SIZE = 0x12,
+	AV8100_COMMAND_FUSE_AES_CHK_SIZE = 0x2,
+};
+
+static unsigned int waittime_retry[10] = {1, 2, 4, 6, 8, 10, 10, 10, 10, 10};
+
+static int av8100_5V_w(u8 denc_off, u8 hdmi_off, u8 on);
+static void clr_plug_status(enum av8100_plugin_status status);
+static void set_plug_status(enum av8100_plugin_status status);
+static void cec_rx(void);
+static void cec_txerr(void);
+static void hdcp_changed(void);
+static const struct color_conversion_cmd *get_color_transform_cmd(
+				enum av8100_color_transform transform);
+static int av8100_open(struct inode *inode, struct file *filp);
+static int av8100_release(struct inode *inode, struct file *filp);
+static long av8100_ioctl(struct file *file,
+				unsigned int cmd, unsigned long arg);
+static int __devinit av8100_probe(struct i2c_client *i2cClient,
+	const struct i2c_device_id *id);
+static int __devexit av8100_remove(struct i2c_client *i2cClient);
+
+static struct av8100_config_t *av8100_config;
+static struct av8100_status	g_av8100_status = {0};
+static struct timer_list av8100_plugtimer;
+static wait_queue_head_t av8100_event;
+static int av8100_flag = 0x0;
+static struct av8100_globals_t *av8100_globals;
+static u8 chip_version;
+static char av8100_receivetab[AV8100_FW_SIZE];
+struct device *av8100dev;
+
+static const struct file_operations av8100_fops = {
+	.owner =    THIS_MODULE,
+	.open =     av8100_open,
+	.release =  av8100_release,
+	.unlocked_ioctl = av8100_ioctl
+};
+
+static struct miscdevice av8100_miscdev = {
+	MISC_DYNAMIC_MINOR,
+	"av8100",
+	&av8100_fops
+};
+
+struct av8100_cea av8100_all_cea[29] = {
+/* cea id
+ *	cea_nr	vtot	vact	vsbpp	vslen
+ *	vsfp	vpol	htot	hact	hbp	hslen	hfp	freq
+ *	hpol	rld	bd	uix4	pm	pd */
+{ "0  CUSTOM                            ",
+	0,	0,	0,	0,	0,
+	0,	"-",	800,	640,	16,	96,	10,	25200000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be defined*/
+{ "1  CEA 1 VESA 4 640x480p @ 60 Hz     ",
+	1,	525,	480,	33,	2,
+	10,	"-",	800,	640,	49,	290,	146,	25200000,
+	"-",	2438,	1270,	6,	32,	1},/*RGB888*/
+{ "2  CEA 2 - 3    720x480p @ 60 Hz 4:3 ",
+	2,	525,	480,	30,	6,
+	9,	"-",	858,	720,	34,	130,	128,	27027000,
+	"-",	1828,	0x3C0,	8,	24,	1},/*RGB565*/
+{ "3  CEA 4        1280x720p @ 60 Hz    ",
+	4,	750,	720,	20,	5,
+	5,	"+",	1650,	1280,	114,	39,	228,	74250000,
+	"+",	1706,	164,	6,	32,	1},/*RGB565*/
+{ "4  CEA 5        1920x1080i @ 60 Hz   ",
+	5,	1125,	540,	20,	5,
+	0,	"+",	2200,	1920,	88,	44,	10,	74250000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "5  CEA 6-7      480i (NTSC)          ",
+	6,	525,	240,	44,	5,
+	0,	"-",	858,	720,	12,	64,	10,	13513513,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "6  CEA 14-15    480p @ 60 Hz         ",
+	14,	525,	480,	44,	5,
+	0,	"-",	858,	720,	12,	64,	10,	27027000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "7  CEA 16       1920x1080p @ 60 Hz   ",
+	16,	1125,	1080,	36,	5,
+	0,	"+",	1980,	1280,	440,	40,	10,	133650000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "8  CEA 17-18    720x576p @ 50 Hz     ",
+	17,	625,	576,	44,	5,
+	0,	"-",	864,	720,	12,	64,	10,	27000000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "9  CEA 19       1280x720p @ 50 Hz    ",
+	19,	750,	720,	25,	5,
+	0,	"+",	1980,	1280,	440,	40,	10,	74250000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "10 CEA 20       1920 x 1080i @ 50 Hz ",
+	20,	1125,	540,	20,	5,
+	0,	"+",	2640,	1920,	528,	44,	10,	74250000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "11 CEA 21-22    576i (PAL)           ",
+	21,	625,	288,	44,	5,
+	0,	"-",	1728,	1440,	12,	64,	10,	27000000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "12 CEA 29/30    576p                 ",
+	29,	625,	576,	44,	5,
+	0,	"-",	864,	720,	12,	64,	10,	27000000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "13 CEA 31       1080p 50Hz           ",
+	31,	1125,	1080,	44,	5,
+	0,	"-",	2640,	1920,	12,	64,	10,	148500000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "14 CEA 32       1920x1080p @ 24 Hz   ",
+	32,	1125,	1080,	36,	5,
+	4,	"+",	2750,	1920,	660,	44,	153,	74250000,
+	"+",	2844,	0x530,	6,	32,	1},/*RGB565*/
+{ "15 CEA 33       1920x1080p @ 25 Hz   ",
+	33,	1125,	1080,	36,	5,
+	4,	"+",	2640,	1920,	528,	44,	10,	74250000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "16 CEA 34       1920x1080p @ 30Hz    ",
+	34,	1125,	1080,	36,	5,
+	4,	"+",	2200,	1920,	91,	44,	153,	74250000,
+	"+",	2275,	0xAB,	6,	32,	1},/*RGB565*/
+{ "17 CEA 60       1280x720p @ 24 Hz    ",
+	60,	750,	720,	20,	5,
+	5,	"+",	3300,	1280,	284,	50,	2276,	59400000,
+	"+",	4266,	0xAD0,	5,	32,	1},/*RGB565*/
+{ "18 CEA 61       1280x720p @ 25 Hz    ",
+	61,	750,	720,	20,	5,
+	5,	"+",	3960,	1280,	228,	39,	2503,	74250000,
+	"+",	4096,	0x500,	5,	32,	1},/*RGB565*/
+{ "19 CEA 62       1280x720p @ 30 Hz    ",
+	62,	750,	720,	20,	5,
+	5,	"+",	3300,	1280,	228,	39,	1820,	74250000,
+	"+",	3413,	0x770,	5,	32,	1},/*RGB565*/
+{ "20 VESA 9       800x600 @ 60 Hz      ",
+	109,	628,	600,	28,	4,
+	0,	"+",	1056,	800,	40,	128,	10,	40000000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "21 VESA 14      848x480  @ 60 Hz     ",
+	114,	517,	480,	20,	5,
+	0,	"+",	1088,	848,	24,	80,	10,	33750000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "22 VESA 16      1024x768 @ 60 Hz     ",
+	116,	806,	768,	38,	6,
+	0,	"-",	1344,	1024,	24,	135,	10,	65000000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "23 VESA 22      1280x768 @ 60 Hz     ",
+	122,	790,	768,	34,	4,
+	0,	"+",	1440,	1280,	48,	160,	10,	68250000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "24 VESA 23      1280x768 @ 60 Hz     ",
+	123,	798,	768,	30,	7,
+	0,	"+",	1664,	1280,	64,	128,	10,	79500000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "25 VESA 27      1280x800 @ 60 Hz     ",
+	127,	823,	800,	23,	6,
+	0,	"+",	1440,	1280,	48,	32,	10,	71000000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "26 VESA 28      1280x800 @ 60 Hz     ",
+	128,	831,	800,	31,	6,
+	0,	"+",	1680,	1280,	72,	128,	10,	83500000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "27 VESA 39      1360x768 @ 60 Hz     ",
+	139,	795,	768,	22,	5,
+	0,	"-",	1792,	1360,	48,	32,	10,	85500000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "28 VESA 81      1366x768 @ 60 Hz     ",
+	181,	798,	768,	30,	5,
+	0,	"+",	1792,	1366,	72,	136,	10,	85750000,
+	"-",	0,	0,	0,	0,	0} /*Settings to be define*/
+};
+
+const struct color_conversion_cmd col_trans_identity = {
+	.c0      = 0x0100, .c1      = 0x0000, .c2      = 0x0000,
+	.c3      = 0x0000, .c4      = 0x0100, .c5      = 0x0000,
+	.c6      = 0x0000, .c7      = 0x0000, .c8      = 0x0100,
+	.aoffset = 0x0000, .boffset = 0x0000, .coffset = 0x0000,
+	.lmax    = 0xff,
+	.lmin    = 0x00,
+	.cmax    = 0xff,
+	.cmin    = 0x00,
+};
+
+const struct color_conversion_cmd col_trans_identity_clamp_yuv = {
+	.c0      = 0x0100, .c1      = 0x0000, .c2      = 0x0000,
+	.c3      = 0x0000, .c4      = 0x0100, .c5      = 0x0000,
+	.c6      = 0x0000, .c7      = 0x0000, .c8      = 0x0100,
+	.aoffset = 0x0000, .boffset = 0x0000, .coffset = 0x0000,
+	.lmax    = 0xeb,
+	.lmin    = 0x10,
+	.cmax    = 0xf0,
+	.cmin    = 0x10,
+};
+
+const struct color_conversion_cmd col_trans_yuv_to_rgb_v1 = {
+	.c0      = 0x0087, .c1      = 0x0000, .c2      = 0x00ba,
+	.c3      = 0x0087, .c4      = 0xffd3, .c5      = 0xffa1,
+	.c6      = 0x0087, .c7      = 0x00eb, .c8      = 0x0000,
+	.aoffset = 0xffab, .boffset = 0x004e, .coffset = 0xff92,
+	.lmax    = 0xff,
+	.lmin    = 0x00,
+	.cmax    = 0xff,
+	.cmin    = 0x00,
+};
+
+const struct color_conversion_cmd col_trans_yuv_to_rgb_v2 = {
+	.c0      = 0x0198, .c1      = 0x012a, .c2      = 0x0000,
+	.c3      = 0xff30, .c4      = 0x012a, .c5      = 0xff9c,
+	.c6      = 0x0000, .c7      = 0x012a, .c8      = 0x0204,
+	.aoffset = 0xff21, .boffset = 0x0088, .coffset = 0xfeeb,
+	.lmax    = 0xff,
+	.lmin    = 0x00,
+	.cmax    = 0xff,
+	.cmin    = 0x00,
+};
+
+const struct color_conversion_cmd col_trans_yuv_to_denc = {
+	.c0      = 0x0100, .c1      = 0x0000, .c2      = 0x0000,
+	.c3      = 0x0000, .c4      = 0x0100, .c5      = 0x0000,
+	.c6      = 0x0000, .c7      = 0x0000, .c8      = 0x0100,
+	.aoffset = 0x0000, .boffset = 0x0000, .coffset = 0x0000,
+	.lmax    = 0xeb,
+	.lmin    = 0x10,
+	.cmax    = 0xf0,
+	.cmin    = 0x10,
+};
+
+const struct color_conversion_cmd col_trans_rgb_to_denc = {
+	.c0      = 0x0070, .c1      = 0xffb6, .c2      = 0xffda,
+	.c3      = 0x0019, .c4      = 0x0081, .c5      = 0x0042,
+	.c6      = 0xffee, .c7      = 0xffa2, .c8      = 0x0070,
+	.aoffset = 0x007f, .boffset = 0x0010, .coffset = 0x007f,
+	.lmax    = 0xff,
+	.lmin    = 0x00,
+	.cmax    = 0xff,
+	.cmin    = 0x00,
+};
+
+static const struct i2c_device_id av8100_id[] = {
+	{ "av8100", 0 },
+	{ }
+};
+
+#ifdef CONFIG_PM
+static int av8100_suspend(struct i2c_client *i2c_client, pm_message_t state)
+{
+	int ret;
+
+	dev_dbg(av8100dev, "%s\n", __func__);
+
+	ret = av8100_powerdown();
+	if (ret)
+		dev_err(av8100dev, "av8100_powerdown failed\n");
+
+	return ret;
+}
+
+static int av8100_resume(struct i2c_client *i2c_client)
+{
+	int ret;
+	u8 hpds = 0;
+
+	dev_dbg(av8100dev, "%s\n", __func__);
+
+	ret = av8100_powerup();
+	if (ret) {
+		dev_err(av8100dev, "av8100_powerup failed\n");
+		return ret;
+	}
+
+	/* Check HDMI plug status */
+	if (av8100_reg_stby_r(NULL, NULL, &hpds, NULL, NULL)) {
+		dev_warn(av8100dev, "av8100_reg_stby_r failed\n");
+		goto av8100_resume_end;
+	}
+
+	if (hpds)
+		set_plug_status(AV8100_HDMI_PLUGIN); /* Plugged*/
+	else
+		clr_plug_status(AV8100_HDMI_PLUGIN); /* Unplugged*/
+
+	av8100_globals->hpdm = AV8100_STANDBY_INTERRUPT_MASK_HPDM_HIGH;
+	av8100_enable_interrupt();
+
+av8100_resume_end:
+	return 0;
+}
+#else
+#define av8100_suspend NULL
+#define av8100_resume NULL
+#endif
+
+static struct i2c_driver av8100_driver = {
+	.probe	= av8100_probe,
+	.remove = av8100_remove,
+	.suspend = av8100_suspend,
+	.resume = av8100_resume,
+	.driver = {
+		.name	= "av8100",
+	},
+	.id_table	= av8100_id,
+};
+
+static void av8100_plugtimer_int(unsigned long value)
+{
+	av8100_flag |= AV8100_PLUGSTARTUP_EVENT;
+	wake_up_interruptible(&av8100_event);
+	del_timer(&av8100_plugtimer);
+}
+
+static int av8100_int_event_handle(void)
+{
+	u8 hpdi = 0;
+	u8 cpdi = 0;
+	u8 hdcpi = 0;
+	u8 ceci = 0;
+	u8 hpds = 0;
+	u8 cpds = 0;
+	u8 hdcps = 0;
+	u8 cectxerr = 0;
+	u8 cecrx = 0;
+	u8 cectx = 0;
+
+	if (!av8100_globals)
+		return -EINVAL;
+
+	/* STANDBY_PENDING_INTERRUPT reg */
+	if (av8100_reg_stby_pend_int_r(&hpdi, &cpdi, NULL, NULL)) {
+		dev_dbg(av8100dev, "av8100_reg_stby_pend_int_r failed\n");
+		goto av8100_int_event_handle_1;
+	}
+
+	/* Plug event */
+	if (hpdi | cpdi) {
+		/* Clear pending interrupts */
+		(void)av8100_reg_stby_pend_int_w(1, 1, 1, 0);
+
+		/* STANDBY reg */
+		if (av8100_reg_stby_r(NULL, NULL, &hpds, &cpds, NULL)) {
+			dev_dbg(av8100dev, "av8100_reg_stby_r failed\n");
+			goto av8100_int_event_handle_1;
+		}
+	}
+
+	if (cpdi & av8100_globals->cpdm) {
+		/* TVout plugin change */
+		if (cpds) {
+			dev_dbg(av8100dev, "cpds 1\n");
+			set_plug_status(AV8100_CVBS_PLUGIN);
+		} else {
+			dev_dbg(av8100dev, "cpds 0\n");
+			clr_plug_status(AV8100_CVBS_PLUGIN);
+		}
+	}
+
+	if (hpdi & av8100_globals->hpdm) {
+		/* HDMI plugin change */
+		if (hpds) {
+			/* Plugged */
+			/* Set 5V always on */
+			av8100_5V_w(av8100_globals->denc_off_time,
+					0,
+					av8100_globals->on_time);
+			dev_dbg(av8100dev, "hpds 1\n");
+			set_plug_status(AV8100_HDMI_PLUGIN);
+		} else {
+			/* Unplugged */
+			av8100_5V_w(av8100_globals->denc_off_time,
+					av8100_globals->hdmi_off_time,
+					av8100_globals->on_time);
+			dev_dbg(av8100dev, "hpds 0\n");
+			clr_plug_status(AV8100_HDMI_PLUGIN);
+		}
+	}
+
+av8100_int_event_handle_1:
+	/* GENERAL_INTERRUPT reg */
+	if (av8100_reg_gen_int_r(NULL, NULL, NULL, &ceci,
+			&hdcpi, NULL, NULL)) {
+		dev_dbg(av8100dev, "av8100_reg_stby_pend_int_r failed\n");
+		return -EINVAL;
+	}
+
+	/* CEC or HDCP event */
+	if (ceci | hdcpi) {
+		/* Clear pending interrupts */
+		av8100_reg_gen_int_w(1, 1, 1, 1, 1, 1);
+
+		/* GENERAL_STATUS reg */
+		if (av8100_reg_gen_status_r(&cectxerr, &cecrx, &cectx, NULL,
+				NULL, &hdcps) != 0) {
+			dev_dbg(av8100dev, "av8100_reg_gen_status_r fail\n");
+			return -EINVAL;
+		}
+	}
+
+	/* CEC received */
+	if (ceci && cecrx) {
+		dev_dbg(av8100dev, "cecrx\n");
+		/* Report CEC event */
+		cec_rx();
+	}
+
+	/* CEC tx error */
+	if (ceci && cectx && cectxerr) {
+		dev_dbg(av8100dev, "cectxerr\n");
+		/* Report CEC tx error event */
+		cec_txerr();
+	}
+
+	/* HDCP event */
+	if (hdcpi) {
+		dev_dbg(av8100dev, "hdcpch:%0x\n", hdcps);
+		/* Report HDCP status change event */
+		hdcp_changed();
+	}
+
+	return 0;
+}
+
+static int av8100_plugstartup_event_handle(void)
+{
+	u8 hpds = 0;
+	u8 cpds = 0;
+
+	if (!av8100_globals)
+		return -EINVAL;
+
+	switch (av8100_globals->plug_state) {
+	case AV8100_UNPLUGGED:
+	case AV8100_PLUGGED:
+	default:
+		break;
+
+	case AV8100_PLUGGED_STARTUP:
+		/* Unmask interrupt */
+		av8100_globals->hpdm = AV8100_STANDBY_INTERRUPT_MASK_HPDM_HIGH;
+		if (av8100_reg_stby_int_mask_w(av8100_globals->hpdm,
+			av8100_globals->cpdm,
+			AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_INPUT,
+			AV8100_STANDBY_INTERRUPT_MASK_IPOL_LOW)) {
+			dev_dbg(av8100dev,
+				"av8100_reg_stby_int_mask_w fail\n");
+		}
+
+		msleep(1);
+
+		/* Get actual plug status */
+		if (av8100_reg_stby_r(NULL, NULL, &hpds, &cpds,	NULL))
+			dev_dbg(av8100dev, "av8100_reg_stby_r fail\n");
+
+		/* Set plugstate */
+		if (hpds) {
+			av8100_globals->plug_state = AV8100_PLUGGED;
+			dev_dbg(av8100dev, "plug_state:2\n");
+		} else {
+			av8100_globals->plug_state = AV8100_UNPLUGGED;
+			dev_dbg(av8100dev, "plug_state:0\n");
+
+			if (av8100_globals->hdmi_ev_cb)
+				av8100_globals->hdmi_ev_cb(
+					AV8100_HDMI_EVENT_HDMI_PLUGOUT);
+		}
+		break;
+	}
+
+	return 0;
+}
+
+static int av8100_thread(void *p)
+{
+	u8 flags;
+
+	while (1) {
+		wait_event_interruptible(av8100_event, (av8100_flag != 0));
+		flags = av8100_flag;
+		av8100_flag = 0;
+
+		if (!av8100_globals ||
+			g_av8100_status.av8100_state < AV8100_OPMODE_STANDBY)
+			continue;
+
+		if (flags & AV8100_INT_EVENT)
+			(void)av8100_int_event_handle();
+
+		if (flags & AV8100_PLUGSTARTUP_EVENT)
+			(void)av8100_plugstartup_event_handle();
+	}
+
+	return 0;
+}
+
+static irqreturn_t av8100_intr_handler(int irq, void *p)
+{
+	av8100_flag |= AV8100_INT_EVENT;
+	wake_up_interruptible(&av8100_event);
+	return IRQ_HANDLED;
+}
+
+static u16 av8100_get_te_line_nb(
+	enum av8100_output_CEA_VESA output_video_format)
+{
+	u16 retval;
+
+	switch (output_video_format) {
+	case AV8100_CEA1_640X480P_59_94HZ:
+	case AV8100_CEA2_3_720X480P_59_94HZ:
+	case AV8100_VESA16_1024X768P_60HZ:
+		retval = AV8100_TE_LINE_NB_30;
+		break;
+
+	case AV8100_CEA4_1280X720P_60HZ:
+	case AV8100_CEA60_1280X720P_24HZ:
+	case AV8100_CEA61_1280X720P_25HZ:
+	case AV8100_CEA62_1280X720P_30HZ:
+		retval = AV8100_TE_LINE_NB_21;
+		break;
+
+	case AV8100_CEA5_1920X1080I_60HZ:
+	case AV8100_CEA6_7_NTSC_60HZ:
+	case AV8100_CEA20_1920X1080I_50HZ:
+	case AV8100_CEA21_22_576I_PAL_50HZ:
+	case AV8100_VESA27_1280X800P_59_91HZ:
+		retval = AV8100_TE_LINE_NB_18;
+		break;
+
+	case AV8100_CEA14_15_480p_60HZ:
+		retval = AV8100_TE_LINE_NB_32;
+		break;
+
+	case AV8100_CEA17_18_720X576P_50HZ:
+	case AV8100_CEA29_30_576P_50HZ:
+		retval = AV8100_TE_LINE_NB_40;
+		break;
+
+	case AV8100_CEA19_1280X720P_50HZ:
+	case AV8100_VESA39_1360X768P_60_02HZ:
+		retval = AV8100_TE_LINE_NB_22;
+		break;
+
+	case AV8100_CEA32_1920X1080P_24HZ:
+	case AV8100_CEA33_1920X1080P_25HZ:
+	case AV8100_CEA34_1920X1080P_30HZ:
+		retval = AV8100_TE_LINE_NB_38;
+		break;
+
+	case AV8100_VESA9_800X600P_60_32HZ:
+		retval = AV8100_TE_LINE_NB_24;
+		break;
+
+	case AV8100_VESA14_848X480P_60HZ:
+		retval = AV8100_TE_LINE_NB_29;
+		break;
+
+	case AV8100_VESA22_1280X768P_59_99HZ:
+		retval = AV8100_TE_LINE_NB_17;
+		break;
+
+	case AV8100_VESA23_1280X768P_59_87HZ:
+	case AV8100_VESA81_1366X768P_59_79HZ:
+		retval = AV8100_TE_LINE_NB_25;
+		break;
+
+	case AV8100_VESA28_1280X800P_59_81HZ:
+		retval = AV8100_TE_LINE_NB_26;
+		break;
+
+	case AV8100_CEA16_1920X1080P_60HZ:
+	case AV8100_CEA31_1920x1080P_50Hz:
+	default:
+		/* TODO */
+		retval = AV8100_TE_LINE_NB_38;
+		break;
+	}
+
+	return retval;
+}
+
+static u16 av8100_get_ui_x4(
+	enum av8100_output_CEA_VESA output_video_format)
+{
+	return AV8100_UI_X4_DEFAULT;
+}
+
+static int av8100_config_video_output_dep(enum av8100_output_CEA_VESA
+	output_format)
+{
+	int retval;
+	union av8100_configuration config;
+
+	/* video input */
+	config.video_input_format.dsi_input_mode =
+		AV8100_HDMI_DSI_COMMAND_MODE;
+	config.video_input_format.input_pixel_format = AV8100_INPUT_PIX_RGB565;
+	config.video_input_format.total_horizontal_pixel =
+		av8100_all_cea[output_format].htotale;
+	config.video_input_format.total_horizontal_active_pixel =
+		av8100_all_cea[output_format].hactive;
+	config.video_input_format.total_vertical_lines =
+		av8100_all_cea[output_format].vtotale;
+	config.video_input_format.total_vertical_active_lines =
+		av8100_all_cea[output_format].vactive;
+
+	switch (output_format) {
+	case AV8100_CEA5_1920X1080I_60HZ:
+	case AV8100_CEA20_1920X1080I_50HZ:
+	case AV8100_CEA21_22_576I_PAL_50HZ:
+	case AV8100_CEA6_7_NTSC_60HZ:
+		config.video_input_format.video_mode =
+			AV8100_VIDEO_INTERLACE;
+		break;
+
+	default:
+		config.video_input_format.video_mode =
+			AV8100_VIDEO_PROGRESSIVE;
+		break;
+	}
+
+	config.video_input_format.nb_data_lane =
+		AV8100_DATA_LANES_USED_2;
+	config.video_input_format.nb_virtual_ch_command_mode = 0;
+	config.video_input_format.nb_virtual_ch_video_mode = 0;
+	config.video_input_format.ui_x4 = av8100_get_ui_x4(output_format);
+	config.video_input_format.TE_line_nb = av8100_get_te_line_nb(
+		output_format);
+	config.video_input_format.TE_config = AV8100_TE_OFF;
+	config.video_input_format.master_clock_freq = 0;
+
+	retval = av8100_conf_prep(
+		AV8100_COMMAND_VIDEO_INPUT_FORMAT, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* DENC */
+	switch (output_format) {
+	case AV8100_CEA21_22_576I_PAL_50HZ:
+		config.denc_format.cvbs_video_format = AV8100_CVBS_625;
+		config.denc_format.standard_selection = AV8100_PAL_BDGHI;
+		break;
+
+	case AV8100_CEA6_7_NTSC_60HZ:
+		config.denc_format.cvbs_video_format = AV8100_CVBS_525;
+		config.denc_format.standard_selection = AV8100_NTSC_M;
+		break;
+
+	default:
+		/* Not supported */
+		break;
+	}
+
+	return 0;
+}
+
+static int av8100_config_init(void)
+{
+	int retval;
+	union av8100_configuration config;
+
+	dev_dbg(av8100dev, "%s\n", __func__);
+
+	av8100_config = kzalloc(sizeof(struct av8100_config_t), GFP_KERNEL);
+	if (!av8100_config) {
+		dev_err(av8100dev, "%s: Failed to allocate config\n", __func__);
+		return AV8100_FAIL;
+	}
+
+	memset(&config, 0, sizeof(union av8100_configuration));
+	memset(av8100_config, 0, sizeof(union av8100_configuration));
+
+	/* Color conversion */
+	config.color_transform = AV8100_COLOR_TRANSFORM_INDENTITY;
+	retval = av8100_conf_prep(
+		AV8100_COMMAND_COLORSPACECONVERSION, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* DENC */
+	config.denc_format.cvbs_video_format = AV8100_CVBS_625;
+	config.denc_format.standard_selection = AV8100_PAL_BDGHI;
+	config.denc_format.enable = 0;
+	config.denc_format.macrovision_enable = 0;
+	config.denc_format.internal_generator = 0;
+	retval = av8100_conf_prep(AV8100_COMMAND_DENC, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* Video output */
+	config.video_output_format.video_output_cea_vesa =
+		AV8100_CEA4_1280X720P_60HZ;
+
+	retval = av8100_conf_prep(
+		AV8100_COMMAND_VIDEO_OUTPUT_FORMAT, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* Video input */
+	av8100_config_video_output_dep(
+		config.video_output_format.video_output_cea_vesa);
+
+	/* Pattern generator */
+	config.pattern_generator_format.pattern_audio_mode =
+		AV8100_PATTERN_AUDIO_OFF;
+	config.pattern_generator_format.pattern_type =
+		AV8100_PATTERN_GENERATOR;
+	config.pattern_generator_format.pattern_video_format =
+		AV8100_PATTERN_720P;
+	retval = av8100_conf_prep(AV8100_COMMAND_PATTERNGENERATOR,
+		&config);
+	if (retval)
+		return -EFAULT;
+
+	/* Audio input */
+	config.audio_input_format.audio_input_if_format	=
+		AV8100_AUDIO_I2SDELAYED_MODE;
+	config.audio_input_format.i2s_input_nb = 1;
+	config.audio_input_format.sample_audio_freq = AV8100_AUDIO_FREQ_48KHZ;
+	config.audio_input_format.audio_word_lg = AV8100_AUDIO_16BITS;
+	config.audio_input_format.audio_format = AV8100_AUDIO_LPCM_MODE;
+	config.audio_input_format.audio_if_mode = AV8100_AUDIO_MASTER;
+	config.audio_input_format.audio_mute = AV8100_AUDIO_MUTE_DISABLE;
+	retval = av8100_conf_prep(
+		AV8100_COMMAND_AUDIO_INPUT_FORMAT, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* HDMI mode */
+	config.hdmi_format.hdmi_mode	= AV8100_HDMI_ON;
+	config.hdmi_format.hdmi_format	= AV8100_HDMI;
+	config.hdmi_format.dvi_format	= AV8100_DVI_CTRL_CTL0;
+	retval = av8100_conf_prep(AV8100_COMMAND_HDMI, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* EDID section readback */
+	config.edid_section_readback_format.address = 0xA0;
+	config.edid_section_readback_format.block_number = 0;
+	retval = av8100_conf_prep(
+		AV8100_COMMAND_EDID_SECTION_READBACK, &config);
+	if (retval)
+		return -EFAULT;
+
+	return 0;
+}
+
+static void av8100_config_exit(void)
+{
+	dev_dbg(av8100dev, "%s\n", __func__);
+
+	kfree(av8100_config);
+	av8100_config = NULL;
+}
+
+static int av8100_globals_init(void)
+{
+	dev_dbg(av8100dev, "%s\n", __func__);
+
+	av8100_globals = kzalloc(sizeof(struct av8100_globals_t), GFP_KERNEL);
+	if (!av8100_globals) {
+		dev_err(av8100dev, "%s: Alloc failure\n", __func__);
+		return AV8100_FAIL;
+	}
+
+	av8100_globals->denc_off_time = AV8100_DENC_OFF_TIME;
+	av8100_globals->hdmi_off_time = AV8100_HDMI_OFF_TIME;
+	av8100_globals->on_time = AV8100_ON_TIME;
+
+	av8100_globals->hpdm = AV8100_STANDBY_INTERRUPT_MASK_HPDM_HIGH;
+	av8100_globals->cpdm = AV8100_STANDBY_INTERRUPT_MASK_CPDM_HIGH;
+	av8100_globals->hdcpm = AV8100_GENERAL_INTERRUPT_MASK_HDCPM_HIGH;
+	av8100_globals->cecm = AV8100_GENERAL_INTERRUPT_MASK_CECM_HIGH;
+
+	av8100_globals->plug_state = AV8100_UNPLUGGED;
+	av8100_globals->inputclk = NULL;
+	av8100_globals->inputclk_requested = false;
+	av8100_globals->opp_requested = false;
+	av8100_globals->regulator_requested = false;
+
+	return 0;
+}
+
+static void av8100_globals_exit(void)
+{
+	dev_dbg(av8100dev, "%s\n", __func__);
+
+	kfree(av8100_globals);
+	av8100_globals = NULL;
+}
+
+static void clr_plug_status(enum av8100_plugin_status status)
+{
+	g_av8100_status.av8100_plugin_status &= ~status;
+
+	switch (status) {
+	case AV8100_HDMI_PLUGIN:
+		if (!av8100_globals)
+			return;
+
+		switch (av8100_globals->plug_state) {
+		case AV8100_UNPLUGGED:
+		case AV8100_PLUGGED_STARTUP:
+		default:
+			break;
+
+		case AV8100_PLUGGED:
+			av8100_globals->plug_state =
+				AV8100_UNPLUGGED;
+			dev_dbg(av8100dev, "plug_state:0\n");
+
+			if (av8100_globals->hdmi_ev_cb)
+				av8100_globals->hdmi_ev_cb(
+					AV8100_HDMI_EVENT_HDMI_PLUGOUT);
+			break;
+		}
+		break;
+
+	case AV8100_CVBS_PLUGIN:
+		/* TODO */
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void set_plug_status(enum av8100_plugin_status status)
+{
+	g_av8100_status.av8100_plugin_status |= status;
+
+	switch (status) {
+	case AV8100_HDMI_PLUGIN:
+		if (!av8100_globals)
+			return;
+
+		switch (av8100_globals->plug_state) {
+		case AV8100_UNPLUGGED:
+			av8100_globals->plug_state =
+				AV8100_PLUGGED_STARTUP;
+
+			dev_dbg(av8100dev, "plug_state:1\n");
+
+			/*
+			 * Mask interrupts to avoid plug detect during
+			 * startup
+			 *  */
+			av8100_globals->hpdm =
+				AV8100_STANDBY_INTERRUPT_MASK_HPDM_LOW;
+			if (av8100_reg_stby_int_mask_w(
+				av8100_globals->hpdm,
+				av8100_globals->cpdm,
+				AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_INPUT,
+				AV8100_STANDBY_INTERRUPT_MASK_IPOL_LOW)) {
+				dev_dbg(av8100dev,
+					"av8100_reg_stby_int_mask_w"
+					"fail\n");
+			}
+
+			/* Set plug startup timer */
+			init_timer(&av8100_plugtimer);
+			av8100_plugtimer.expires = jiffies +
+					AV8100_PLUGSTARTUP_TIME;
+			av8100_plugtimer.function =
+					av8100_plugtimer_int;
+			av8100_plugtimer.data = 0;
+			add_timer(&av8100_plugtimer);
+
+			if (av8100_globals->hdmi_ev_cb)
+				av8100_globals->hdmi_ev_cb(
+					AV8100_HDMI_EVENT_HDMI_PLUGIN);
+			break;
+
+		case AV8100_PLUGGED_STARTUP:
+		case AV8100_PLUGGED:
+		default:
+			break;
+		}
+		break;
+
+	case AV8100_CVBS_PLUGIN:
+		/* TODO */
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void cec_rx(void)
+{
+	if (av8100_globals && av8100_globals->hdmi_ev_cb)
+		av8100_globals->hdmi_ev_cb(AV8100_HDMI_EVENT_CEC);
+}
+
+static void cec_txerr(void)
+{
+	if (av8100_globals && av8100_globals->hdmi_ev_cb)
+		av8100_globals->hdmi_ev_cb(AV8100_HDMI_EVENT_CECTXERR);
+}
+
+static void hdcp_changed(void)
+{
+	if (av8100_globals && av8100_globals->hdmi_ev_cb)
+		av8100_globals->hdmi_ev_cb(AV8100_HDMI_EVENT_HDCP);
+}
+
+static void av8100_set_state(enum av8100_operating_mode state)
+{
+	g_av8100_status.av8100_state = state;
+
+	if (state <= AV8100_OPMODE_STANDBY) {
+		clr_plug_status(AV8100_HDMI_PLUGIN);
+		clr_plug_status(AV8100_CVBS_PLUGIN);
+		g_av8100_status.hdmi_on = false;
+	}
+}
+
+/**
+ * write_single_byte() - Write a single byte to av8100
+ * through i2c interface.
+ * @client:	i2c client structure
+ * @reg:	register offset
+ * @data:	data byte to be written
+ *
+ * This funtion uses smbus byte write API to write a single byte to av8100
+ **/
+static int write_single_byte(struct i2c_client *client, u8 reg,
+	u8 data)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, reg, data);
+	if (ret < 0)
+		dev_dbg(av8100dev, "i2c smbus write byte failed\n");
+
+	return ret;
+}
+
+/**
+ * read_single_byte() - read single byte from av8100
+ * through i2c interface
+ * @client:     i2c client structure
+ * @reg:        register offset
+ * @val:	register value
+ *
+ * This funtion uses smbus read block API to read single byte from the reg
+ * offset.
+ **/
+static int read_single_byte(struct i2c_client *client, u8 reg, u8 *val)
+{
+	int value;
+
+	value = i2c_smbus_read_byte_data(client, reg);
+	if (value < 0) {
+		dev_dbg(av8100dev, "i2c smbus read byte failed,read data = %x "
+			"from offset:%x\n" , value, reg);
+		return AV8100_FAIL;
+	}
+
+	*val = (u8) value;
+	return 0;
+}
+
+/**
+ * write_multi_byte() - Write a multiple bytes to av8100 through
+ * i2c interface.
+ * @client:	i2c client structure
+ * @buf:	buffer to be written
+ * @nbytes:	nunmber of bytes to be written
+ *
+ * This funtion uses smbus block write API's to write n number of bytes to the
+ * av8100
+ **/
+static int write_multi_byte(struct i2c_client *client, u8 reg,
+		u8 *buf, u8 nbytes)
+{
+	int ret;
+
+	ret = i2c_smbus_write_i2c_block_data(client, reg, nbytes, buf);
+	if (ret < 0)
+		dev_dbg(av8100dev, "i2c smbus write multi byte error\n");
+
+	return ret;
+}
+
+static int configuration_video_input_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = av8100_config->hdmi_video_input_cmd.dsi_input_mode;
+	buffer[1] = av8100_config->hdmi_video_input_cmd.input_pixel_format;
+	buffer[2] = REG_16_8_MSB(av8100_config->hdmi_video_input_cmd.
+		total_horizontal_pixel);
+	buffer[3] = REG_16_8_LSB(av8100_config->hdmi_video_input_cmd.
+		total_horizontal_pixel);
+	buffer[4] = REG_16_8_MSB(av8100_config->hdmi_video_input_cmd.
+		total_horizontal_active_pixel);
+	buffer[5] = REG_16_8_LSB(av8100_config->hdmi_video_input_cmd.
+		total_horizontal_active_pixel);
+	buffer[6] = REG_16_8_MSB(av8100_config->hdmi_video_input_cmd.
+		total_vertical_lines);
+	buffer[7] = REG_16_8_LSB(av8100_config->hdmi_video_input_cmd.
+		total_vertical_lines);
+	buffer[8] = REG_16_8_MSB(av8100_config->hdmi_video_input_cmd.
+		total_vertical_active_lines);
+	buffer[9] = REG_16_8_LSB(av8100_config->hdmi_video_input_cmd.
+		total_vertical_active_lines);
+	buffer[10] = av8100_config->hdmi_video_input_cmd.video_mode;
+	buffer[11] = av8100_config->hdmi_video_input_cmd.nb_data_lane;
+	buffer[12] = av8100_config->hdmi_video_input_cmd.
+		nb_virtual_ch_command_mode;
+	buffer[13] = av8100_config->hdmi_video_input_cmd.
+		nb_virtual_ch_video_mode;
+	buffer[14] = REG_16_8_MSB(av8100_config->hdmi_video_input_cmd.
+		TE_line_nb);
+	buffer[15] = REG_16_8_LSB(av8100_config->hdmi_video_input_cmd.
+		TE_line_nb);
+	buffer[16] = av8100_config->hdmi_video_input_cmd.TE_config;
+	buffer[17] = REG_32_8_MSB(av8100_config->hdmi_video_input_cmd.
+		master_clock_freq);
+	buffer[18] = REG_32_8_MMSB(av8100_config->hdmi_video_input_cmd.
+		master_clock_freq);
+	buffer[19] = REG_32_8_MLSB(av8100_config->hdmi_video_input_cmd.
+		master_clock_freq);
+	buffer[20] = REG_32_8_LSB(av8100_config->hdmi_video_input_cmd.
+		master_clock_freq);
+	buffer[21] = av8100_config->hdmi_video_input_cmd.ui_x4;
+
+	*length = AV8100_COMMAND_VIDEO_INPUT_FORMAT_SIZE - 1;
+	return 0;
+
+}
+
+static int configuration_audio_input_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = av8100_config->hdmi_audio_input_cmd.
+		audio_input_if_format;
+	buffer[1] = av8100_config->hdmi_audio_input_cmd.i2s_input_nb;
+	buffer[2] = av8100_config->hdmi_audio_input_cmd.sample_audio_freq;
+	buffer[3] = av8100_config->hdmi_audio_input_cmd.audio_word_lg;
+	buffer[4] = av8100_config->hdmi_audio_input_cmd.audio_format;
+	buffer[5] = av8100_config->hdmi_audio_input_cmd.audio_if_mode;
+	buffer[6] = av8100_config->hdmi_audio_input_cmd.audio_mute;
+
+	*length = AV8100_COMMAND_AUDIO_INPUT_FORMAT_SIZE - 1;
+	return 0;
+}
+
+static int configuration_video_output_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = av8100_config->hdmi_video_output_cmd.
+		video_output_cea_vesa;
+
+	if (buffer[0] == AV8100_CUSTOM) {
+		buffer[1] = av8100_config->hdmi_video_output_cmd.
+			vsync_polarity;
+		buffer[2] = av8100_config->hdmi_video_output_cmd.
+			hsync_polarity;
+		buffer[3] = REG_16_8_MSB(av8100_config->
+			hdmi_video_output_cmd.total_horizontal_pixel);
+		buffer[4] = REG_16_8_LSB(av8100_config->
+			hdmi_video_output_cmd.total_horizontal_pixel);
+		buffer[5] = REG_16_8_MSB(av8100_config->
+			hdmi_video_output_cmd.total_horizontal_active_pixel);
+		buffer[6] = REG_16_8_LSB(av8100_config->
+			hdmi_video_output_cmd.total_horizontal_active_pixel);
+		buffer[7] = REG_16_8_MSB(av8100_config->
+			hdmi_video_output_cmd.total_vertical_in_half_lines);
+		buffer[8] = REG_16_8_LSB(av8100_config->
+			hdmi_video_output_cmd.total_vertical_in_half_lines);
+		buffer[9] = REG_16_8_MSB(av8100_config->
+			hdmi_video_output_cmd.
+				total_vertical_active_in_half_lines);
+		buffer[10] = REG_16_8_LSB(av8100_config->
+			hdmi_video_output_cmd.
+				total_vertical_active_in_half_lines);
+		buffer[11] = REG_16_8_MSB(av8100_config->
+			hdmi_video_output_cmd.hsync_start_in_pixel);
+		buffer[12] = REG_16_8_LSB(av8100_config->
+			hdmi_video_output_cmd.hsync_start_in_pixel);
+		buffer[13] = REG_16_8_MSB(av8100_config->
+			hdmi_video_output_cmd.hsync_length_in_pixel);
+		buffer[14] = REG_16_8_LSB(av8100_config->
+			hdmi_video_output_cmd.hsync_length_in_pixel);
+		buffer[15] = REG_16_8_MSB(av8100_config->
+			hdmi_video_output_cmd.vsync_start_in_half_line);
+		buffer[16] = REG_16_8_LSB(av8100_config->
+			hdmi_video_output_cmd.vsync_start_in_half_line);
+		buffer[17] = REG_16_8_MSB(av8100_config->
+			hdmi_video_output_cmd.vsync_length_in_half_line);
+		buffer[18] = REG_16_8_LSB(av8100_config->
+			hdmi_video_output_cmd.vsync_length_in_half_line);
+		buffer[19] = REG_16_8_MSB(av8100_config->
+			hdmi_video_output_cmd.hor_video_start_pixel);
+		buffer[20] = REG_16_8_LSB(av8100_config->
+			hdmi_video_output_cmd.hor_video_start_pixel);
+		buffer[21] = REG_16_8_MSB(av8100_config->
+			hdmi_video_output_cmd.vert_video_start_pixel);
+		buffer[22] = REG_16_8_LSB(av8100_config->
+			hdmi_video_output_cmd.vert_video_start_pixel);
+		buffer[23] = av8100_config->
+			hdmi_video_output_cmd.video_type;
+		buffer[24] = av8100_config->
+			hdmi_video_output_cmd.pixel_repeat;
+		buffer[25] = REG_32_8_MSB(av8100_config->
+			hdmi_video_output_cmd.pixel_clock_freq_Hz);
+		buffer[26] = REG_32_8_MMSB(av8100_config->
+			hdmi_video_output_cmd.pixel_clock_freq_Hz);
+		buffer[27] = REG_32_8_MLSB(av8100_config->
+			hdmi_video_output_cmd.pixel_clock_freq_Hz);
+		buffer[28] = REG_32_8_LSB(av8100_config->
+			hdmi_video_output_cmd.pixel_clock_freq_Hz);
+
+		*length = AV8100_COMMAND_VIDEO_OUTPUT_FORMAT_SIZE - 1;
+	} else {
+		*length = 1;
+	}
+
+	return 0;
+}
+
+static int configuration_video_scaling_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = REG_16_8_MSB(av8100_config->hdmi_video_scaling_cmd.
+		h_start_in_pixel);
+	buffer[1] = REG_16_8_LSB(av8100_config->hdmi_video_scaling_cmd.
+		h_start_in_pixel);
+	buffer[2] = REG_16_8_MSB(av8100_config->hdmi_video_scaling_cmd.
+		h_stop_in_pixel);
+	buffer[3] = REG_16_8_LSB(av8100_config->hdmi_video_scaling_cmd.
+		h_stop_in_pixel);
+	buffer[4] = REG_16_8_MSB(av8100_config->hdmi_video_scaling_cmd.
+		v_start_in_line);
+	buffer[5] = REG_16_8_LSB(av8100_config->hdmi_video_scaling_cmd.
+		v_start_in_line);
+	buffer[6] = REG_16_8_MSB(av8100_config->hdmi_video_scaling_cmd.
+		v_stop_in_line);
+	buffer[7] = REG_16_8_LSB(av8100_config->hdmi_video_scaling_cmd.
+		v_stop_in_line);
+	buffer[8] = REG_16_8_MSB(av8100_config->hdmi_video_scaling_cmd.
+		h_start_out_pixel);
+	buffer[9] = REG_16_8_LSB(av8100_config->hdmi_video_scaling_cmd
+		.h_start_out_pixel);
+	buffer[10] = REG_16_8_MSB(av8100_config->hdmi_video_scaling_cmd.
+		h_stop_out_pixel);
+	buffer[11] = REG_16_8_LSB(av8100_config->hdmi_video_scaling_cmd.
+		h_stop_out_pixel);
+	buffer[12] = REG_16_8_MSB(av8100_config->hdmi_video_scaling_cmd.
+		v_start_out_line);
+	buffer[13] = REG_16_8_LSB(av8100_config->hdmi_video_scaling_cmd.
+		v_start_out_line);
+	buffer[14] = REG_16_8_MSB(av8100_config->hdmi_video_scaling_cmd.
+		v_stop_out_line);
+	buffer[15] = REG_16_8_LSB(av8100_config->hdmi_video_scaling_cmd.
+		v_stop_out_line);
+
+	*length = AV8100_COMMAND_VIDEO_SCALING_FORMAT_SIZE - 1;
+	return 0;
+}
+
+static int configuration_colorspace_conversion_get(char *buffer,
+	unsigned int *length)
+{
+	const struct color_conversion_cmd *hdmi_color_space_conversion_cmd;
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	hdmi_color_space_conversion_cmd =
+		get_color_transform_cmd(av8100_config->color_transform);
+
+	buffer[0] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c0);
+	buffer[1] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c0);
+	buffer[2] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c1);
+	buffer[3] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c1);
+	buffer[4] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c2);
+	buffer[5] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c2);
+	buffer[6] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c3);
+	buffer[7] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c3);
+	buffer[8] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c4);
+	buffer[9] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c4);
+	buffer[10] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c5);
+	buffer[11] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c5);
+	buffer[12] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c6);
+	buffer[13] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c6);
+	buffer[14] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c7);
+	buffer[15] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c7);
+	buffer[16] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c8);
+	buffer[17] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c8);
+	buffer[18] = REG_10_8_MSB(hdmi_color_space_conversion_cmd->aoffset);
+	buffer[19] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->aoffset);
+	buffer[20] = REG_10_8_MSB(hdmi_color_space_conversion_cmd->boffset);
+	buffer[21] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->boffset);
+	buffer[22] = REG_10_8_MSB(hdmi_color_space_conversion_cmd->coffset);
+	buffer[23] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->coffset);
+	buffer[24] = hdmi_color_space_conversion_cmd->lmax;
+	buffer[25] = hdmi_color_space_conversion_cmd->lmin;
+	buffer[26] = hdmi_color_space_conversion_cmd->cmax;
+	buffer[27] = hdmi_color_space_conversion_cmd->cmin;
+
+	*length = AV8100_COMMAND_COLORSPACECONVERSION_SIZE - 1;
+	return 0;
+}
+
+static int configuration_cec_message_write_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = av8100_config->hdmi_cec_message_write_cmd.buffer_length;
+	memcpy(&buffer[1], av8100_config->hdmi_cec_message_write_cmd.buffer,
+		HDMI_CEC_MESSAGE_WRITE_BUFFER_SIZE);
+
+	*length = AV8100_COMMAND_CEC_MESSAGE_WRITE_SIZE - 1;
+	return 0;
+}
+
+static int configuration_cec_message_read_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	/* No buffer data */
+	*length = AV8100_COMMAND_CEC_MESSAGE_READ_BACK_SIZE - 1;
+	return 0;
+}
+
+static int configuration_denc_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = av8100_config->hdmi_denc_cmd.cvbs_video_format;
+	buffer[1] = av8100_config->hdmi_denc_cmd.standard_selection;
+	buffer[2] = av8100_config->hdmi_denc_cmd.enable;
+	buffer[3] = av8100_config->hdmi_denc_cmd.macrovision_enable;
+	buffer[4] = av8100_config->hdmi_denc_cmd.internal_generator;
+
+	*length = AV8100_COMMAND_DENC_SIZE - 1;
+	return 0;
+}
+
+static int configuration_hdmi_get(char *buffer, unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = av8100_config->hdmi_cmd.hdmi_mode;
+	buffer[1] = av8100_config->hdmi_cmd.hdmi_format;
+	buffer[2] = av8100_config->hdmi_cmd.dvi_format;
+
+	*length = AV8100_COMMAND_HDMI_SIZE - 1;
+	return 0;
+}
+
+static int configuration_hdcp_sendkey_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = av8100_config->hdmi_hdcp_send_key_cmd.key_number;
+	memcpy(&buffer[1], av8100_config->hdmi_hdcp_send_key_cmd.data,
+		av8100_config->hdmi_hdcp_send_key_cmd.data_len);
+
+	*length = av8100_config->hdmi_hdcp_send_key_cmd.data_len + 1;
+	return 0;
+}
+
+static int configuration_hdcp_management_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = av8100_config->hdmi_hdcp_management_format_cmd.req_type;
+	buffer[1] = av8100_config->hdmi_hdcp_management_format_cmd.encr_use;
+
+	*length = AV8100_COMMAND_HDCP_MANAGEMENT_SIZE - 1;
+	return 0;
+}
+
+static int configuration_infoframe_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = av8100_config->hdmi_infoframes_cmd.type;
+	buffer[1] = av8100_config->hdmi_infoframes_cmd.version;
+	buffer[2] = av8100_config->hdmi_infoframes_cmd.length;
+	buffer[3] = av8100_config->hdmi_infoframes_cmd.crc;
+	memcpy(&buffer[4], av8100_config->hdmi_infoframes_cmd.data,
+	HDMI_INFOFRAME_DATA_SIZE);
+
+	*length = AV8100_COMMAND_INFOFRAMES_SIZE - 1;
+	return 0;
+}
+
+static int av8100_edid_section_readback_get(char *buffer, unsigned int *length)
+{
+	buffer[0] = av8100_config->hdmi_edid_section_readback_cmd.address;
+	buffer[1] = av8100_config->hdmi_edid_section_readback_cmd.
+		block_number;
+
+	*length = AV8100_COMMAND_EDID_SECTION_READBACK_SIZE - 1;
+	return 0;
+}
+
+static int configuration_pattern_generator_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = av8100_config->hdmi_pattern_generator_cmd.pattern_type;
+	buffer[1] = av8100_config->hdmi_pattern_generator_cmd.
+		pattern_video_format;
+	buffer[2] = av8100_config->hdmi_pattern_generator_cmd.
+		pattern_audio_mode;
+
+	*length = AV8100_COMMAND_PATTERNGENERATOR_SIZE - 1;
+	return 0;
+}
+
+static int configuration_fuse_aes_key_get(char *buffer,
+	unsigned int *length)
+{
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	buffer[0] = av8100_config->hdmi_fuse_aes_key_cmd.fuse_operation;
+	if (av8100_config->hdmi_fuse_aes_key_cmd.fuse_operation) {
+		/* Write key command */
+		memcpy(&buffer[1], av8100_config->hdmi_fuse_aes_key_cmd.key,
+			HDMI_FUSE_AES_KEY_SIZE);
+
+		*length = AV8100_COMMAND_FUSE_AES_KEY_SIZE - 1;
+	} else {
+		/* Check key command */
+		*length = AV8100_COMMAND_FUSE_AES_CHK_SIZE - 1;
+	}
+	return 0;
+}
+
+static int get_command_return_first(struct i2c_client *i2c,
+		enum av8100_command_type command_type) {
+	int retval = 0;
+	char val;
+
+	retval = read_single_byte(i2c, AV8100_COMMAND_OFFSET, &val);
+	if (retval) {
+		dev_dbg(av8100dev, "%s 1st ret failed\n", __func__);
+		return retval;
+	}
+
+	if (val != (0x80 | command_type)) {
+		dev_dbg(av8100dev, "%s 1st ret wrong:%x\n", __func__, val);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int get_command_return_data(struct i2c_client *i2c,
+	enum av8100_command_type command_type,
+	u8 *command_buffer,
+	u8 *buffer_length,
+	u8 *buffer)
+{
+	int retval = 0;
+	char val;
+	int index = 0;
+
+	if (buffer_length)
+		*buffer_length = 0;
+
+	switch (command_type) {
+	case AV8100_COMMAND_VIDEO_INPUT_FORMAT:
+	case AV8100_COMMAND_AUDIO_INPUT_FORMAT:
+	case AV8100_COMMAND_VIDEO_OUTPUT_FORMAT:
+	case AV8100_COMMAND_VIDEO_SCALING_FORMAT:
+	case AV8100_COMMAND_COLORSPACECONVERSION:
+	case AV8100_COMMAND_CEC_MESSAGE_WRITE:
+	case AV8100_COMMAND_DENC:
+	case AV8100_COMMAND_HDMI:
+	case AV8100_COMMAND_INFOFRAMES:
+	case AV8100_COMMAND_PATTERNGENERATOR:
+		/* Get the second return byte */
+		retval = read_single_byte(i2c,
+			AV8100_2ND_RET_BYTE_OFFSET, &val);
+		if (retval)
+			goto get_command_return_data_fail2r;
+
+		if (val) {
+			retval = AV8100_FAIL;
+			goto get_command_return_data_fail2v;
+		}
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_READ_BACK:
+		if ((buffer == NULL) ||	(buffer_length == NULL)) {
+			retval = AV8100_FAIL;
+			goto get_command_return_data_fail;
+		}
+
+		/* Get the return buffer length */
+		retval = read_single_byte(i2c, AV8100_CEC_ADDR_OFFSET, &val);
+		if (retval)
+			goto get_command_return_data_fail;
+
+		/* TODO: buffer_length is always zero */
+		/* *buffer_length = val;*/
+		dev_dbg(av8100dev, "cec buflen:%d\n", val);
+		*buffer_length = val;
+
+		if (*buffer_length >
+			HDMI_CEC_READ_MAXSIZE) {
+			dev_dbg(av8100dev, "CEC size too large %d\n",
+				*buffer_length);
+			*buffer_length = HDMI_CEC_READ_MAXSIZE;
+		}
+
+		dev_dbg(av8100dev, "return data: ");
+
+		/* Get the return buffer */
+		for (index = 0; index < *buffer_length; ++index) {
+			retval = read_single_byte(i2c,
+				AV8100_CEC_RET_BUF_OFFSET + index, &val);
+			if (retval) {
+				*buffer_length = 0;
+				goto get_command_return_data_fail;
+			} else {
+				*(buffer + index) = val;
+				dev_dbg(av8100dev, "%02x ", *(buffer + index));
+			}
+		}
+
+		dev_dbg(av8100dev, "\n");
+		break;
+
+	case AV8100_COMMAND_HDCP_MANAGEMENT:
+		/* Get the second return byte */
+		retval = read_single_byte(i2c,
+			AV8100_2ND_RET_BYTE_OFFSET, &val);
+		if (retval) {
+			goto get_command_return_data_fail2r;
+		} else {
+			/* Check the second return byte */
+			if (val)
+				goto get_command_return_data_fail2v;
+		}
+
+		if ((buffer == NULL) || (buffer_length == NULL))
+			/* Ignore return data */
+			break;
+
+		/* Get the return buffer length */
+		if (command_buffer[0] ==
+			HDMI_REQUEST_FOR_REVOCATION_LIST_INPUT) {
+			*buffer_length = HDMI_REVOC_LIST_SIZE;
+		} else {
+			*buffer_length = 0x0;
+		}
+
+		dev_dbg(av8100dev, "return data: ");
+
+		/* Get the return buffer */
+		for (index = 0; index < *buffer_length; ++index) {
+			retval = read_single_byte(i2c,
+				AV8100_HDCP_RET_BUF_OFFSET + index, &val);
+			if (retval) {
+				*buffer_length = 0;
+				goto get_command_return_data_fail;
+			} else {
+				*(buffer + index) = val;
+				dev_dbg(av8100dev, "%02x ", *(buffer + index));
+			}
+		}
+
+		dev_dbg(av8100dev, "\n");
+		break;
+
+	case AV8100_COMMAND_EDID_SECTION_READBACK:
+		if ((buffer == NULL) ||	(buffer_length == NULL)) {
+			retval = AV8100_FAIL;
+			goto get_command_return_data_fail;
+		}
+
+		/* Return buffer length is fixed */
+		*buffer_length = HDMI_EDIDREAD_SIZE;
+
+		dev_dbg(av8100dev, "return data: ");
+
+		/* Get the return buffer */
+		for (index = 0; index < *buffer_length; ++index) {
+			retval = read_single_byte(i2c,
+				AV8100_EDID_RET_BUF_OFFSET + index, &val);
+			if (retval) {
+				*buffer_length = 0;
+				goto get_command_return_data_fail;
+			} else {
+				*(buffer + index) = val;
+				dev_dbg(av8100dev, "%02x ", *(buffer + index));
+			}
+		}
+
+		dev_dbg(av8100dev, "\n");
+		break;
+
+	case AV8100_COMMAND_FUSE_AES_KEY:
+		if ((buffer == NULL) ||	(buffer_length == NULL)) {
+			retval = AV8100_FAIL;
+			goto get_command_return_data_fail;
+		}
+
+		/* Get the second return byte */
+		retval = read_single_byte(i2c,
+			AV8100_2ND_RET_BYTE_OFFSET, &val);
+
+		if (retval)
+			goto get_command_return_data_fail2r;
+
+		/* Check the second return byte */
+		if (val) {
+			retval = AV8100_FAIL;
+			goto get_command_return_data_fail2v;
+		}
+
+		/* Return buffer length is fixed */
+		*buffer_length = HDMI_FUSE_AES_KEY_RET_SIZE;
+
+		/* Get CRC */
+		retval = read_single_byte(i2c,
+			AV8100_FUSE_CRC_OFFSET, &val);
+		if (retval)
+			goto get_command_return_data_fail;
+
+		*buffer = val;
+		dev_dbg(av8100dev, "CRC:%02x ", val);
+
+		/* Get programmed status */
+		retval = read_single_byte(i2c,
+			AV8100_FUSE_PRGD_OFFSET, &val);
+		if (retval)
+			goto get_command_return_data_fail;
+
+		*(buffer + 1) = val;
+
+		dev_dbg(av8100dev, "programmed:%02x ", val);
+		break;
+
+	case AV8100_COMMAND_HDCP_SENDKEY:
+		if ((command_buffer[0] == HDMI_LOADAES_END_BLK_NR) &&
+			((buffer == NULL) || (buffer_length == NULL))) {
+			retval = AV8100_FAIL;
+			goto get_command_return_data_fail;
+		}
+
+		/* Get the second return byte */
+		retval = read_single_byte(i2c,
+			AV8100_2ND_RET_BYTE_OFFSET, &val);
+		if (retval)
+			goto get_command_return_data_fail2r;
+
+		if (val) {
+			retval = AV8100_FAIL;
+			goto get_command_return_data_fail2v;
+		}
+
+		if (command_buffer[0] == HDMI_LOADAES_END_BLK_NR) {
+			/* Return CRC32 if last AES block */
+			int cnt;
+
+			dev_dbg(av8100dev, "CRC32:");
+			for (cnt = 0; cnt < HDMI_CRC32_SIZE; cnt++) {
+				if (read_single_byte(i2c,
+					AV8100_CRC32_OFFSET + cnt, &val))
+					goto get_command_return_data_fail;
+				*(buffer + cnt) = val;
+				dev_dbg(av8100dev, "%02x", val);
+			}
+
+			*buffer_length = HDMI_CRC32_SIZE;
+		}
+		break;
+
+	default:
+		retval = AV8100_INVALID_COMMAND;
+		break;
+	}
+
+	return retval;
+get_command_return_data_fail2r:
+	dev_dbg(av8100dev, "%s Reading 2nd return byte failed\n", __func__);
+	return retval;
+get_command_return_data_fail2v:
+	dev_dbg(av8100dev, "%s 2nd return byte is wrong:%x\n", __func__, val);
+	return retval;
+get_command_return_data_fail:
+	dev_dbg(av8100dev, "%s FAIL\n", __func__);
+	return retval;
+}
+
+static int av8100_powerup1(void)
+{
+	int retval;
+	struct av8100_platform_data *pdata = av8100dev->platform_data;
+
+	/* Regulator enable */
+	if ((av8100_globals->regulator_pwr) &&
+			(av8100_globals->regulator_requested == false)) {
+		retval = regulator_enable(av8100_globals->regulator_pwr);
+		if (retval < 0) {
+			dev_warn(av8100dev, "%s: regulator_enable failed\n",
+			__func__);
+			return retval;
+		}
+		dev_dbg(av8100dev, "regulator_enable ok\n");
+		av8100_globals->regulator_requested = true;
+	}
+
+	/* Reset av8100 */
+	gpio_set_value(pdata->reset, 1);
+
+	/* Need to wait before proceeding */
+	msleep(AV8100_WAITTIME_1MS);
+
+	if (pdata->alt_powerupseq) {
+		dev_dbg(av8100dev, "powerup seq alt\n");
+		retval = av8100_5V_w(0, 0, AV8100_ON_TIME);
+		if (retval) {
+			dev_err(av8100dev, "%s reg_wr err 1\n", __func__);
+			goto av8100_powerup1_err;
+		}
+
+		udelay(AV8100_WATTIME_100US);
+
+		retval = av8100_reg_stby_pend_int_w(
+				AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_HIGH);
+		if (retval) {
+			dev_err(av8100dev, "%s reg_wr err 2\n", __func__);
+			goto av8100_powerup1_err;
+		}
+
+		udelay(AV8100_WATTIME_100US);
+
+		retval = av8100_reg_stby_w(AV8100_STANDBY_CPD_LOW,
+				AV8100_STANDBY_STBY_HIGH, pdata->mclk_freq);
+		if (retval) {
+			dev_err(av8100dev, "%s reg_wr err 3\n", __func__);
+			goto av8100_powerup1_err;
+		}
+
+		msleep(AV8100_WAITTIME_1MS);
+
+		retval = av8100_reg_stby_w(AV8100_STANDBY_CPD_LOW,
+				AV8100_STANDBY_STBY_LOW, pdata->mclk_freq);
+		if (retval) {
+			dev_err(av8100dev, "%s reg_wr err 4\n", __func__);
+			goto av8100_powerup1_err;
+		}
+
+		msleep(AV8100_WAITTIME_1MS);
+
+		retval = av8100_reg_stby_pend_int_w(
+				AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_LOW);
+		if (retval) {
+			dev_err(av8100dev, "%s reg_wr err 5\n", __func__);
+			goto av8100_powerup1_err;
+		}
+
+		msleep(AV8100_WAITTIME_1MS);
+	}
+
+	av8100_set_state(AV8100_OPMODE_STANDBY);
+
+	/* Get chip version */
+	retval = av8100_reg_stby_pend_int_r(NULL, NULL, NULL, &chip_version);
+	if (retval) {
+		dev_err(av8100dev, "Failed to read chip version\n");
+		goto av8100_powerup1_err;
+	}
+
+	dev_info(av8100dev, "chip version:%d\n", chip_version);
+
+	switch (chip_version) {
+	case AV8100_CHIPVER_1:
+	case AV8100_CHIPVER_2:
+		break;
+
+	default:
+		dev_err(av8100dev, "Unsupported chip version:%d\n",
+			chip_version);
+		goto av8100_powerup1_err;
+		break;
+	}
+
+	return 0;
+
+av8100_powerup1_err:
+	av8100_powerdown();
+	return -EFAULT;
+}
+
+static int av8100_powerup2(void)
+{
+	int retval;
+	struct av8100_platform_data *pdata = av8100dev->platform_data;
+
+	/* Master clock timing, running, search for plug */
+	retval = av8100_reg_stby_w(AV8100_STANDBY_CPD_HIGH,
+		AV8100_STANDBY_STBY_HIGH, pdata->mclk_freq);
+	if (retval) {
+		dev_err(av8100dev,
+			"Failed to write the value to av8100 register\n");
+		return retval;
+	}
+
+	msleep(AV8100_WAITTIME_1MS);
+
+	/* ON time & OFF time on 5v HDMI plug detect */
+	retval = av8100_5V_w(av8100_globals->denc_off_time,
+				av8100_globals->hdmi_off_time,
+				av8100_globals->on_time);
+	if (retval) {
+		dev_err(av8100dev,
+			"Failed to write the value to av8100 register\n");
+		return retval;
+	}
+
+	msleep(AV8100_WAITTIME_1MS);
+
+	av8100_set_state(AV8100_OPMODE_SCAN);
+
+	return 0;
+}
+
+int av8100_powerup(void)
+{
+	int ret = 0;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state < AV8100_OPMODE_STANDBY) {
+		ret = av8100_powerup1();
+		if (ret) {
+			dev_err(av8100dev, "av8100_powerup1 fail\n");
+			return -EFAULT;
+		}
+	}
+
+	if (av8100_status_get().av8100_state < AV8100_OPMODE_SCAN)
+		ret = av8100_powerup2();
+
+	av8100_enable_interrupt();
+
+	return ret;
+}
+EXPORT_SYMBOL(av8100_powerup);
+
+int av8100_powerdown(void)
+{
+	int retval = 0;
+
+	struct av8100_platform_data *pdata = av8100dev->platform_data;
+
+	av8100_disable_interrupt();
+
+	if (pdata->alt_powerupseq) {
+		retval = av8100_reg_stby_pend_int_w(
+				AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_HIGH);
+
+		if (retval)
+			dev_err(av8100dev, "%s reg_wr err\n", __func__);
+		msleep(AV8100_WAITTIME_50MS);
+	}
+
+	if (gpio_get_value(pdata->reset) == 0)
+		return 0;
+
+	/* Remove APE OPP requirement */
+	if (av8100_globals->opp_requested) {
+		prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP,
+				(char *)av8100_miscdev.name);
+		prcmu_qos_remove_requirement(PRCMU_QOS_DDR_OPP,
+				(char *)av8100_miscdev.name);
+		av8100_globals->opp_requested = false;
+	}
+
+	/* Clock disable */
+	if (av8100_globals->inputclk && av8100_globals->inputclk_requested) {
+		clk_disable(av8100_globals->inputclk);
+		av8100_globals->inputclk_requested = false;
+	}
+
+	/* Regulator disable */
+	if ((av8100_globals->regulator_pwr) &&
+			(av8100_globals->regulator_requested)) {
+		dev_dbg(av8100dev, "regulator_disable\n");
+		regulator_disable(av8100_globals->regulator_pwr);
+		av8100_globals->regulator_requested = false;
+	}
+
+	gpio_set_value(pdata->reset, 0);
+
+	if (pdata->alt_powerupseq)
+		msleep(AV8100_WAITTIME_5MS);
+
+	av8100_set_state(AV8100_OPMODE_SHUTDOWN);
+
+	return retval;
+}
+EXPORT_SYMBOL(av8100_powerdown);
+
+int av8100_download_firmware(char *fw_buff, int nbytes,
+	enum interface_type if_type)
+{
+	int retval;
+	int temp = 0x0;
+	int increment = 15;
+	int index = 0;
+	int size = 0x0;
+	int tempnext = 0x0;
+	char val = 0x0;
+	char CheckSum = 0;
+	int cnt;
+	int cnt_max;
+	struct i2c_client *i2c;
+	u8 uc;
+	u8 fdl;
+	u8 hld;
+	u8 wa;
+	u8 ra;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	if (!av8100_config)
+		return -EINVAL;
+
+	/* Clock enable */
+	if (av8100_globals->inputclk &&
+			av8100_globals->inputclk_requested == false) {
+		if (clk_enable(av8100_globals->inputclk)) {
+			dev_err(av8100dev, "inputclk en failed\n");
+			return -EFAULT;
+		}
+
+		av8100_globals->inputclk_requested = true;
+	}
+
+	/* Request 100% APE OPP */
+	if (av8100_globals->opp_requested == false) {
+		if (prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP,
+				(char *)av8100_miscdev.name, 100)) {
+			dev_err(av8100dev, "APE OPP 100 failed\n");
+			return -EFAULT;
+		}
+		if (prcmu_qos_add_requirement(PRCMU_QOS_DDR_OPP,
+				(char *)av8100_miscdev.name, 100)) {
+			dev_err(av8100dev, "DDR OPP 100 failed\n");
+			return -EFAULT;
+		}
+
+		av8100_globals->opp_requested = true;
+	}
+
+	msleep(AV8100_WAITTIME_10MS);
+
+	fw_buff = av8100_fw_buff;
+	nbytes = AV8100_FW_SIZE;
+
+	i2c = av8100_config->client;
+
+	/* Enable firmware download */
+	retval = av8100_reg_gen_ctrl_w(
+		AV8100_GENERAL_CONTROL_FDL_HIGH,
+		AV8100_GENERAL_CONTROL_HLD_HIGH,
+		AV8100_GENERAL_CONTROL_WA_LOW,
+		AV8100_GENERAL_CONTROL_RA_LOW);
+	if (retval) {
+		dev_err(av8100dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	retval = av8100_reg_gen_ctrl_r(&fdl, &hld, &wa, &ra);
+	if (retval) {
+		dev_err(av8100dev,
+			"Failed to read the value from av8100 register\n");
+		return -EFAULT;
+	} else {
+		dev_dbg(av8100dev, "GENERAL_CONTROL_REG register fdl:%d "
+			"hld:%d wa:%d ra:%d\n", fdl, hld, wa, ra);
+	}
+
+	LOCK_AV8100_HW;
+
+	temp = nbytes % increment;
+	for (size = 0; size < (nbytes-temp); size = size + increment,
+		index += increment) {
+		if (if_type == I2C_INTERFACE) {
+			retval = write_multi_byte(i2c,
+				AV8100_FIRMWARE_DOWNLOAD_ENTRY, fw_buff + size,
+				increment);
+			if (retval) {
+				dev_dbg(av8100dev, "Failed to download the "
+					"av8100 firmware\n");
+				UNLOCK_AV8100_HW;
+				return -EFAULT;
+			}
+		} else if (if_type == DSI_INTERFACE) {
+			dev_dbg(av8100dev,
+				"DSI_INTERFACE is currently not supported\n");
+			UNLOCK_AV8100_HW;
+			return -EINVAL;
+		} else {
+			UNLOCK_AV8100_HW;
+			return -EINVAL;
+		}
+
+		for (tempnext = size; tempnext < (increment+size); tempnext++)
+			av8100_receivetab[tempnext] = fw_buff[tempnext];
+	}
+
+	/* Transfer last firmware bytes */
+	if (if_type == I2C_INTERFACE) {
+		retval = write_multi_byte(i2c,
+			AV8100_FIRMWARE_DOWNLOAD_ENTRY, fw_buff + size, temp);
+		if (retval) {
+			dev_dbg(av8100dev,
+				"Failed to download the av8100 firmware\n");
+			UNLOCK_AV8100_HW;
+			return -EFAULT;
+		}
+	} else if (if_type == DSI_INTERFACE) {
+		/* TODO: Add support for DSI firmware download */
+		UNLOCK_AV8100_HW;
+		return -EINVAL;
+	} else {
+		UNLOCK_AV8100_HW;
+		return -EINVAL;
+	}
+
+	for (tempnext = size; tempnext < (size+temp); tempnext++)
+		av8100_receivetab[tempnext] = fw_buff[tempnext];
+
+	/* check transfer*/
+	for (size = 0; size < nbytes; size++) {
+		CheckSum = CheckSum ^ fw_buff[size];
+		if (av8100_receivetab[size] != fw_buff[size]) {
+			dev_dbg(av8100dev, ">Fw download fail....i=%d\n", size);
+			dev_dbg(av8100dev, "Transm = %x, Receiv = %x\n",
+				fw_buff[size], av8100_receivetab[size]);
+		}
+	}
+
+	UNLOCK_AV8100_HW;
+
+	retval = av8100_reg_fw_dl_entry_r(&val);
+	if (retval) {
+		dev_dbg(av8100dev,
+			"Failed to read the value from the av8100 register\n");
+		return -EFAULT;
+	}
+
+	dev_dbg(av8100dev, "CheckSum:%x,val:%x\n", CheckSum, val);
+
+	if (CheckSum != val) {
+		dev_dbg(av8100dev,
+			">Fw downloading.... FAIL CheckSum issue\n");
+		dev_dbg(av8100dev, "Checksum = %d\n", CheckSum);
+		dev_dbg(av8100dev, "Checksum read: %d\n", val);
+		return -EFAULT;
+	} else {
+		dev_dbg(av8100dev, ">Fw downloading.... success\n");
+	}
+
+	/* Set to idle mode */
+	av8100_reg_gen_ctrl_w(AV8100_GENERAL_CONTROL_FDL_LOW,
+		AV8100_GENERAL_CONTROL_HLD_LOW,	AV8100_GENERAL_CONTROL_WA_LOW,
+		AV8100_GENERAL_CONTROL_RA_LOW);
+	if (retval) {
+		dev_dbg(av8100dev,
+			"Failed to write the value to the av8100 register\n");
+		return -EFAULT;
+	}
+
+	/* Wait Internal Micro controler ready */
+	cnt = 0;
+	cnt_max = ARRAY_SIZE(waittime_retry);
+	retval = av8100_reg_gen_status_r(NULL, NULL, NULL, &uc,
+		NULL, NULL);
+	while ((retval == 0) && (uc != 0x1) && (cnt++ < cnt_max)) {
+		msleep(waittime_retry[cnt]);
+		retval = av8100_reg_gen_status_r(NULL, NULL, NULL,
+			&uc, NULL, NULL);
+	}
+	dev_dbg(av8100dev, "av8100 fwdl cnt:%d\n", cnt);
+
+	if (retval)	{
+		dev_dbg(av8100dev,
+			"Failed to read the value from the av8100 register\n");
+		return -EFAULT;
+	}
+
+	if (uc != 0x1)
+		dev_dbg(av8100dev, "UC is not ready\n");
+
+	av8100_set_state(AV8100_OPMODE_IDLE);
+
+	return 0;
+}
+EXPORT_SYMBOL(av8100_download_firmware);
+
+int av8100_disable_interrupt(void)
+{
+	int retval;
+	struct i2c_client *i2c;
+	u8 hpdm = 0;
+	u8 cpdm = 0;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	if (!av8100_config)
+		return -EINVAL;
+
+	i2c = av8100_config->client;
+
+	retval = av8100_reg_stby_pend_int_w(
+			AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_LOW);
+	if (retval) {
+		dev_dbg(av8100dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	retval = av8100_reg_gen_int_mask_w(
+			AV8100_GENERAL_INTERRUPT_MASK_EOCM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_VSIM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_VSOM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_CECM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_HDCPM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_UOVBM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_TEM_LOW);
+	if (retval) {
+		dev_dbg(av8100dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	if (av8100_globals) {
+		hpdm = av8100_globals->hpdm;
+		cpdm = av8100_globals->cpdm;
+	}
+
+	retval = av8100_reg_stby_int_mask_w(
+			AV8100_STANDBY_INTERRUPT_MASK_HPDM_LOW,
+			AV8100_STANDBY_INTERRUPT_MASK_CPDM_LOW,
+			AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_INPUT,
+			AV8100_STANDBY_INTERRUPT_MASK_IPOL_LOW);
+	if (retval) {
+		dev_dbg(av8100dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	if (av8100_globals) {
+		av8100_globals->hpdm = hpdm;
+		av8100_globals->cpdm = cpdm;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(av8100_disable_interrupt);
+
+int av8100_enable_interrupt(void)
+{
+	int retval;
+	struct i2c_client *i2c;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	if (!av8100_globals || !av8100_config)
+		return -EINVAL;
+
+	i2c = av8100_config->client;
+
+	retval = av8100_reg_stby_pend_int_w(
+			AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_LOW);
+	if (retval) {
+		dev_dbg(av8100dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	retval = av8100_reg_gen_int_mask_w(
+			AV8100_GENERAL_INTERRUPT_MASK_EOCM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_VSIM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_VSOM_LOW,
+			av8100_globals->cecm,
+			av8100_globals->hdcpm,
+			AV8100_GENERAL_INTERRUPT_MASK_UOVBM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_TEM_LOW);
+	if (retval) {
+		dev_dbg(av8100dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	retval = av8100_reg_stby_int_mask_w(
+			av8100_globals->hpdm,
+			av8100_globals->cpdm,
+			AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_INPUT,
+			AV8100_STANDBY_INTERRUPT_MASK_IPOL_LOW);
+	if (retval) {
+		dev_dbg(av8100dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(av8100_enable_interrupt);
+
+static int register_write_internal(u8 offset, u8 value)
+{
+	int retval;
+	struct i2c_client *i2c;
+
+	if (!av8100_config)
+		return -EINVAL;
+
+	i2c = av8100_config->client;
+
+	/* Write to register */
+	retval = write_single_byte(i2c, offset, value);
+	if (retval) {
+		dev_dbg(av8100dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(register_write_internal);
+
+int av8100_reg_stby_w(
+		u8 cpd, u8 stby, u8 mclkrng)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_STANDBY_CPD(cpd) | AV8100_STANDBY_STBY(stby) |
+		AV8100_STANDBY_MCLKRNG(mclkrng);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_STANDBY, val);
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_w);
+
+static int av8100_5V_w(u8 denc_off, u8 hdmi_off, u8 on)
+{
+	u8 val;
+	int retval;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value.
+	 * chip_version == 1 have one common off time
+	 * chip_version > 1 support different off time for hdmi and tvout. */
+	if (chip_version == 1)
+		val = AV8100_HDMI_5_VOLT_TIME_OFF_TIME(hdmi_off) |
+			AV8100_HDMI_5_VOLT_TIME_ON_TIME(on);
+	else
+		val = AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME(denc_off) |
+			AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME(hdmi_off) |
+			AV8100_HDMI_5_VOLT_TIME_ON_TIME(on);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_HDMI_5_VOLT_TIME, val);
+
+	UNLOCK_AV8100_HW;
+
+	return retval;
+}
+
+int av8100_reg_hdmi_5_volt_time_w(u8 denc_off, u8 hdmi_off, u8 on)
+{
+	int retval;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	retval = av8100_5V_w(denc_off, hdmi_off, on);
+
+	/* Set vars */
+	if (chip_version > 1)
+		av8100_globals->denc_off_time = denc_off;
+
+	av8100_globals->hdmi_off_time = hdmi_off;
+	if (on)
+		av8100_globals->on_time = on;
+
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_hdmi_5_volt_time_w);
+
+int av8100_reg_stby_int_mask_w(
+		u8 hpdm, u8 cpdm, u8 stbygpiocfg, u8 ipol)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_STANDBY_INTERRUPT_MASK_HPDM(hpdm) |
+		AV8100_STANDBY_INTERRUPT_MASK_CPDM(cpdm) |
+		AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG(stbygpiocfg) |
+		AV8100_STANDBY_INTERRUPT_MASK_IPOL(ipol);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_STANDBY_INTERRUPT_MASK, val);
+
+	av8100_globals->hpdm = hpdm;
+	av8100_globals->cpdm = cpdm;
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_int_mask_w);
+
+int av8100_reg_stby_pend_int_w(
+		u8 hpdi, u8 cpdi, u8 oni, u8 bpdig)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_STANDBY_PENDING_INTERRUPT_HPDI(hpdi) |
+		AV8100_STANDBY_PENDING_INTERRUPT_CPDI(cpdi) |
+		AV8100_STANDBY_PENDING_INTERRUPT_ONI(oni) |
+		AV8100_STANDBY_PENDING_INTERRUPT_BPDIG(bpdig);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_STANDBY_PENDING_INTERRUPT, val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_pend_int_w);
+
+int av8100_reg_gen_int_mask_w(
+		u8 eocm, u8 vsim, u8 vsom, u8 cecm, u8 hdcpm, u8 uovbm, u8 tem)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_GENERAL_INTERRUPT_MASK_EOCM(eocm) |
+		AV8100_GENERAL_INTERRUPT_MASK_VSIM(vsim) |
+		AV8100_GENERAL_INTERRUPT_MASK_VSOM(vsom) |
+		AV8100_GENERAL_INTERRUPT_MASK_CECM(cecm) |
+		AV8100_GENERAL_INTERRUPT_MASK_HDCPM(hdcpm) |
+		AV8100_GENERAL_INTERRUPT_MASK_UOVBM(uovbm) |
+		AV8100_GENERAL_INTERRUPT_MASK_TEM(tem);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_GENERAL_INTERRUPT_MASK,	val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_int_mask_w);
+
+int av8100_reg_gen_int_w(
+		u8 eoci, u8 vsii, u8 vsoi, u8 ceci, u8 hdcpi, u8 uovbi)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_GENERAL_INTERRUPT_EOCI(eoci) |
+		AV8100_GENERAL_INTERRUPT_VSII(vsii) |
+		AV8100_GENERAL_INTERRUPT_VSOI(vsoi) |
+		AV8100_GENERAL_INTERRUPT_CECI(ceci) |
+		AV8100_GENERAL_INTERRUPT_HDCPI(hdcpi) |
+		AV8100_GENERAL_INTERRUPT_UOVBI(uovbi);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_GENERAL_INTERRUPT, val);
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_int_w);
+
+int av8100_reg_gpio_conf_w(
+		u8 dat3dir, u8 dat3val, u8 dat2dir, u8 dat2val,	u8 dat1dir,
+		u8 dat1val, u8 ucdbg)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_GPIO_CONFIGURATION_DAT3DIR(dat3dir) |
+		AV8100_GPIO_CONFIGURATION_DAT3VAL(dat3val) |
+		AV8100_GPIO_CONFIGURATION_DAT2DIR(dat2dir) |
+		AV8100_GPIO_CONFIGURATION_DAT2VAL(dat2val) |
+		AV8100_GPIO_CONFIGURATION_DAT1DIR(dat1dir) |
+		AV8100_GPIO_CONFIGURATION_DAT1VAL(dat1val) |
+		AV8100_GPIO_CONFIGURATION_UCDBG(ucdbg);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_GPIO_CONFIGURATION, val);
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gpio_conf_w);
+
+int av8100_reg_gen_ctrl_w(
+		u8 fdl, u8 hld, u8 wa, u8 ra)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_GENERAL_CONTROL_FDL(fdl) |
+		AV8100_GENERAL_CONTROL_HLD(hld) |
+		AV8100_GENERAL_CONTROL_WA(wa) |
+		AV8100_GENERAL_CONTROL_RA(ra);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_GENERAL_CONTROL, val);
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_ctrl_w);
+
+int av8100_reg_fw_dl_entry_w(
+	u8 mbyte_code_entry)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY(
+		mbyte_code_entry);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_FIRMWARE_DOWNLOAD_ENTRY, val);
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_fw_dl_entry_w);
+
+int av8100_reg_w(
+		u8 offset, u8 value)
+{
+	int retval = 0;
+	struct i2c_client *i2c;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	if (!av8100_config) {
+		UNLOCK_AV8100_HW;
+		return -EINVAL;
+	}
+
+	i2c = av8100_config->client;
+
+	/* Write to register */
+	retval = write_single_byte(i2c, offset, value);
+	if (retval) {
+		dev_dbg(av8100dev,
+			"Failed to write the value to av8100 register\n");
+		UNLOCK_AV8100_HW;
+		return -EFAULT;
+	}
+
+	UNLOCK_AV8100_HW;
+	return 0;
+}
+EXPORT_SYMBOL(av8100_reg_w);
+
+int register_read_internal(u8 offset, u8 *value)
+{
+	int retval = 0;
+	struct i2c_client *i2c;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	if (!av8100_config)
+		return -EINVAL;
+
+	i2c = av8100_config->client;
+
+	/* Read from register */
+	retval = read_single_byte(i2c, offset, value);
+	if (retval)	{
+		dev_dbg(av8100dev,
+			"Failed to read the value from av8100 register\n");
+		return -EFAULT;
+	}
+
+	return retval;
+}
+EXPORT_SYMBOL(register_read_internal);
+
+int av8100_reg_stby_r(
+		u8 *cpd, u8 *stby, u8 *hpds, u8 *cpds, u8 *mclkrng)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_STANDBY, &val);
+
+	/* Set return params */
+	if (cpd)
+		*cpd = AV8100_STANDBY_CPD_GET(val);
+	if (stby)
+		*stby = AV8100_STANDBY_STBY_GET(val);
+	if (hpds)
+		*hpds = AV8100_STANDBY_HPDS_GET(val);
+	if (cpds)
+		*cpds = AV8100_STANDBY_CPDS_GET(val);
+	if (mclkrng)
+		*mclkrng = AV8100_STANDBY_MCLKRNG_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_r);
+
+int av8100_reg_hdmi_5_volt_time_r(
+		u8 *denc_off_time, u8 *hdmi_off_time, u8 *on_time)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_HDMI_5_VOLT_TIME, &val);
+
+	/* Set return params */
+	if (chip_version == 1) {
+		if (denc_off_time)
+			*denc_off_time = 0;
+		if (hdmi_off_time)
+			*hdmi_off_time =
+				AV8100_HDMI_5_VOLT_TIME_OFF_TIME_GET(val);
+	} else {
+		if (denc_off_time)
+			*denc_off_time =
+				AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME_GET(val);
+		if (hdmi_off_time)
+			*hdmi_off_time =
+				AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME_GET(val);
+	}
+
+	if (on_time)
+		*on_time = AV8100_HDMI_5_VOLT_TIME_ON_TIME_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_hdmi_5_volt_time_r);
+
+int av8100_reg_stby_int_mask_r(
+		u8 *hpdm, u8 *cpdm, u8 *stbygpiocfg, u8 *ipol)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_STANDBY_INTERRUPT_MASK, &val);
+
+	/* Set return params */
+	if (hpdm)
+		*hpdm = AV8100_STANDBY_INTERRUPT_MASK_HPDM_GET(val);
+	if (cpdm)
+		*cpdm = AV8100_STANDBY_INTERRUPT_MASK_CPDM_GET(val);
+	if (stbygpiocfg)
+		*stbygpiocfg =
+			AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_GET(val);
+	if (ipol)
+		*ipol = AV8100_STANDBY_INTERRUPT_MASK_IPOL_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_int_mask_r);
+
+int av8100_reg_stby_pend_int_r(
+		u8 *hpdi, u8 *cpdi, u8 *oni, u8 *sid)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_STANDBY_PENDING_INTERRUPT,
+			&val);
+
+	/* Set return params */
+	if (hpdi)
+		*hpdi = AV8100_STANDBY_PENDING_INTERRUPT_HPDI_GET(val);
+	if (cpdi)
+		*cpdi = AV8100_STANDBY_PENDING_INTERRUPT_CPDI_GET(val);
+	if (oni)
+		*oni = AV8100_STANDBY_PENDING_INTERRUPT_ONI_GET(val);
+	if (sid)
+		*sid = AV8100_STANDBY_PENDING_INTERRUPT_SID_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_pend_int_r);
+
+int av8100_reg_gen_int_mask_r(
+		u8 *eocm,
+		u8 *vsim,
+		u8 *vsom,
+		u8 *cecm,
+		u8 *hdcpm,
+		u8 *uovbm,
+		u8 *tem)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_GENERAL_INTERRUPT_MASK, &val);
+
+	/* Set return params */
+	if (eocm)
+		*eocm = AV8100_GENERAL_INTERRUPT_MASK_EOCM_GET(val);
+	if (vsim)
+		*vsim = AV8100_GENERAL_INTERRUPT_MASK_VSIM_GET(val);
+	if (vsom)
+		*vsom = AV8100_GENERAL_INTERRUPT_MASK_VSOM_GET(val);
+	if (cecm)
+		*cecm = AV8100_GENERAL_INTERRUPT_MASK_CECM_GET(val);
+	if (hdcpm)
+		*hdcpm = AV8100_GENERAL_INTERRUPT_MASK_HDCPM_GET(val);
+	if (uovbm)
+		*uovbm = AV8100_GENERAL_INTERRUPT_MASK_UOVBM_GET(val);
+	if (tem)
+		*tem = AV8100_GENERAL_INTERRUPT_MASK_TEM_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_int_mask_r);
+
+int av8100_reg_gen_int_r(
+		u8 *eoci,
+		u8 *vsii,
+		u8 *vsoi,
+		u8 *ceci,
+		u8 *hdcpi,
+		u8 *uovbi,
+		u8 *tei)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_GENERAL_INTERRUPT, &val);
+
+	/* Set return params */
+	if (eoci)
+		*eoci = AV8100_GENERAL_INTERRUPT_EOCI_GET(val);
+	if (vsii)
+		*vsii = AV8100_GENERAL_INTERRUPT_VSII_GET(val);
+	if (vsoi)
+		*vsoi = AV8100_GENERAL_INTERRUPT_VSOI_GET(val);
+	if (ceci)
+		*ceci = AV8100_GENERAL_INTERRUPT_CECI_GET(val);
+	if (hdcpi)
+		*hdcpi = AV8100_GENERAL_INTERRUPT_HDCPI_GET(val);
+	if (uovbi)
+		*uovbi = AV8100_GENERAL_INTERRUPT_UOVBI_GET(val);
+	if (tei)
+		*tei = AV8100_GENERAL_INTERRUPT_TEI_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_int_r);
+
+int av8100_reg_gen_status_r(
+		u8 *cectxerr,
+		u8 *cecrec,
+		u8 *cectrx,
+		u8 *uc,
+		u8 *onuvb,
+		u8 *hdcps)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_GENERAL_STATUS, &val);
+
+	/* Set return params */
+	if (cectxerr)
+		*cectxerr = AV8100_GENERAL_STATUS_CECTXERR_GET(val);
+	if (cecrec)
+		*cecrec	= AV8100_GENERAL_STATUS_CECREC_GET(val);
+	if (cectrx)
+		*cectrx	= AV8100_GENERAL_STATUS_CECTRX_GET(val);
+	if (uc)
+		*uc = AV8100_GENERAL_STATUS_UC_GET(val);
+	if (onuvb)
+		*onuvb = AV8100_GENERAL_STATUS_ONUVB_GET(val);
+	if (hdcps)
+		*hdcps = AV8100_GENERAL_STATUS_HDCPS_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_status_r);
+
+int av8100_reg_gpio_conf_r(
+		u8 *dat3dir,
+		u8 *dat3val,
+		u8 *dat2dir,
+		u8 *dat2val,
+		u8 *dat1dir,
+		u8 *dat1val,
+		u8 *ucdbg)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_GPIO_CONFIGURATION, &val);
+
+	/* Set return params */
+	if (dat3dir)
+		*dat3dir = AV8100_GPIO_CONFIGURATION_DAT3DIR_GET(val);
+	if (dat3val)
+		*dat3val = AV8100_GPIO_CONFIGURATION_DAT3VAL_GET(val);
+	if (dat2dir)
+		*dat2dir = AV8100_GPIO_CONFIGURATION_DAT2DIR_GET(val);
+	if (dat2val)
+		*dat2val = AV8100_GPIO_CONFIGURATION_DAT2VAL_GET(val);
+	if (dat1dir)
+		*dat1dir = AV8100_GPIO_CONFIGURATION_DAT1DIR_GET(val);
+	if (dat1val)
+		*dat1val = AV8100_GPIO_CONFIGURATION_DAT1VAL_GET(val);
+	if (ucdbg)
+		*ucdbg = AV8100_GPIO_CONFIGURATION_UCDBG_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gpio_conf_r);
+
+int av8100_reg_gen_ctrl_r(
+		u8 *fdl,
+		u8 *hld,
+		u8 *wa,
+		u8 *ra)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_GENERAL_CONTROL, &val);
+	/* Set return params */
+	if (fdl)
+		*fdl = AV8100_GENERAL_CONTROL_FDL_GET(val);
+	if (hld)
+		*hld = AV8100_GENERAL_CONTROL_HLD_GET(val);
+	if (wa)
+		*wa = AV8100_GENERAL_CONTROL_WA_GET(val);
+	if (ra)
+		*ra = AV8100_GENERAL_CONTROL_RA_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_ctrl_r);
+
+int av8100_reg_fw_dl_entry_r(
+	u8 *mbyte_code_entry)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_FIRMWARE_DOWNLOAD_ENTRY,	&val);
+
+	/* Set return params */
+	if (mbyte_code_entry)
+		*mbyte_code_entry =
+		AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_fw_dl_entry_r);
+
+int av8100_reg_r(
+		u8 offset,
+		u8 *value)
+{
+	int retval = 0;
+	struct i2c_client *i2c;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	if (!av8100_config) {
+		retval = AV8100_FAIL;
+		goto av8100_register_read_out;
+	}
+
+	i2c = av8100_config->client;
+
+	/* Read from register */
+	retval = read_single_byte(i2c, offset, value);
+	if (retval)	{
+		dev_dbg(av8100dev,
+			"Failed to read the value from av8100 register\n");
+		retval = -EFAULT;
+		goto av8100_register_read_out;
+	}
+
+av8100_register_read_out:
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_r);
+
+int av8100_conf_get(enum av8100_command_type command_type,
+	union av8100_configuration *config)
+{
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	if (!av8100_config || !config)
+		return AV8100_FAIL;
+
+	/* Put configuration data to the corresponding data struct depending
+	 * on command type */
+	switch (command_type) {
+	case AV8100_COMMAND_VIDEO_INPUT_FORMAT:
+		memcpy(&config->video_input_format,
+			&av8100_config->hdmi_video_input_cmd,
+			sizeof(struct av8100_video_input_format_cmd));
+		break;
+
+	case AV8100_COMMAND_AUDIO_INPUT_FORMAT:
+		memcpy(&config->audio_input_format,
+			&av8100_config->hdmi_audio_input_cmd,
+			sizeof(struct av8100_audio_input_format_cmd));
+		break;
+
+	case AV8100_COMMAND_VIDEO_OUTPUT_FORMAT:
+		memcpy(&config->video_output_format,
+			&av8100_config->hdmi_video_output_cmd,
+			sizeof(struct av8100_video_output_format_cmd));
+		break;
+
+	case AV8100_COMMAND_VIDEO_SCALING_FORMAT:
+		memcpy(&config->video_scaling_format,
+			&av8100_config->hdmi_video_scaling_cmd,
+			sizeof(struct av8100_video_scaling_format_cmd));
+		break;
+
+	case AV8100_COMMAND_COLORSPACECONVERSION:
+		config->color_transform = av8100_config->color_transform;
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_WRITE:
+		memcpy(&config->cec_message_write_format,
+			&av8100_config->hdmi_cec_message_write_cmd,
+			sizeof(struct av8100_cec_message_write_format_cmd));
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_READ_BACK:
+		memcpy(&config->cec_message_read_back_format,
+			&av8100_config->hdmi_cec_message_read_back_cmd,
+			sizeof(struct av8100_cec_message_read_back_format_cmd));
+		break;
+
+	case AV8100_COMMAND_DENC:
+		memcpy(&config->denc_format, &av8100_config->hdmi_denc_cmd,
+				sizeof(struct av8100_denc_format_cmd));
+		break;
+
+	case AV8100_COMMAND_HDMI:
+		memcpy(&config->hdmi_format, &av8100_config->hdmi_cmd,
+				sizeof(struct av8100_hdmi_cmd));
+		break;
+
+	case AV8100_COMMAND_HDCP_SENDKEY:
+		memcpy(&config->hdcp_send_key_format,
+			&av8100_config->hdmi_hdcp_send_key_cmd,
+			sizeof(struct av8100_hdcp_send_key_format_cmd));
+		break;
+
+	case AV8100_COMMAND_HDCP_MANAGEMENT:
+		memcpy(&config->hdcp_management_format,
+			&av8100_config->hdmi_hdcp_management_format_cmd,
+			sizeof(struct av8100_hdcp_management_format_cmd));
+		break;
+
+	case AV8100_COMMAND_INFOFRAMES:
+		memcpy(&config->infoframes_format,
+			&av8100_config->hdmi_infoframes_cmd,
+			sizeof(struct av8100_infoframes_format_cmd));
+		break;
+
+	case AV8100_COMMAND_EDID_SECTION_READBACK:
+		memcpy(&config->edid_section_readback_format,
+			&av8100_config->hdmi_edid_section_readback_cmd,
+			sizeof(struct
+				av8100_edid_section_readback_format_cmd));
+		break;
+
+	case AV8100_COMMAND_PATTERNGENERATOR:
+		memcpy(&config->pattern_generator_format,
+			&av8100_config->hdmi_pattern_generator_cmd,
+			sizeof(struct av8100_pattern_generator_format_cmd));
+		break;
+
+	case AV8100_COMMAND_FUSE_AES_KEY:
+		memcpy(&config->fuse_aes_key_format,
+			&av8100_config->hdmi_fuse_aes_key_cmd,
+			sizeof(struct av8100_fuse_aes_key_format_cmd));
+		break;
+
+	default:
+		return AV8100_FAIL;
+		break;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(av8100_conf_get);
+
+int av8100_conf_prep(enum av8100_command_type command_type,
+	union av8100_configuration *config)
+{
+	if (!av8100_config || !config)
+		return AV8100_FAIL;
+
+	/* Put configuration data to the corresponding data struct depending
+	 * on command type */
+	switch (command_type) {
+	case AV8100_COMMAND_VIDEO_INPUT_FORMAT:
+		memcpy(&av8100_config->hdmi_video_input_cmd,
+			&config->video_input_format,
+			sizeof(struct av8100_video_input_format_cmd));
+		break;
+
+	case AV8100_COMMAND_AUDIO_INPUT_FORMAT:
+		memcpy(&av8100_config->hdmi_audio_input_cmd,
+			&config->audio_input_format,
+			sizeof(struct av8100_audio_input_format_cmd));
+		break;
+
+	case AV8100_COMMAND_VIDEO_OUTPUT_FORMAT:
+		memcpy(&av8100_config->hdmi_video_output_cmd,
+			&config->video_output_format,
+			sizeof(struct av8100_video_output_format_cmd));
+
+		/* Set params that depend on video output */
+		av8100_config_video_output_dep(av8100_config->
+			hdmi_video_output_cmd.video_output_cea_vesa);
+		break;
+
+	case AV8100_COMMAND_VIDEO_SCALING_FORMAT:
+		memcpy(&av8100_config->hdmi_video_scaling_cmd,
+			&config->video_scaling_format,
+			sizeof(struct av8100_video_scaling_format_cmd));
+		break;
+
+	case AV8100_COMMAND_COLORSPACECONVERSION:
+		av8100_config->color_transform = config->color_transform;
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_WRITE:
+		memcpy(&av8100_config->hdmi_cec_message_write_cmd,
+			&config->cec_message_write_format,
+			sizeof(struct av8100_cec_message_write_format_cmd));
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_READ_BACK:
+		memcpy(&av8100_config->hdmi_cec_message_read_back_cmd,
+			&config->cec_message_read_back_format,
+			sizeof(struct av8100_cec_message_read_back_format_cmd));
+		break;
+
+	case AV8100_COMMAND_DENC:
+		memcpy(&av8100_config->hdmi_denc_cmd, &config->denc_format,
+				sizeof(struct av8100_denc_format_cmd));
+		break;
+
+	case AV8100_COMMAND_HDMI:
+		memcpy(&av8100_config->hdmi_cmd, &config->hdmi_format,
+				sizeof(struct av8100_hdmi_cmd));
+		break;
+
+	case AV8100_COMMAND_HDCP_SENDKEY:
+		memcpy(&av8100_config->hdmi_hdcp_send_key_cmd,
+			&config->hdcp_send_key_format,
+			sizeof(struct av8100_hdcp_send_key_format_cmd));
+		break;
+
+	case AV8100_COMMAND_HDCP_MANAGEMENT:
+		memcpy(&av8100_config->hdmi_hdcp_management_format_cmd,
+			&config->hdcp_management_format,
+			sizeof(struct av8100_hdcp_management_format_cmd));
+		break;
+
+	case AV8100_COMMAND_INFOFRAMES:
+		memcpy(&av8100_config->hdmi_infoframes_cmd,
+			&config->infoframes_format,
+			sizeof(struct av8100_infoframes_format_cmd));
+		break;
+
+	case AV8100_COMMAND_EDID_SECTION_READBACK:
+		memcpy(&av8100_config->hdmi_edid_section_readback_cmd,
+			&config->edid_section_readback_format,
+			sizeof(struct
+				av8100_edid_section_readback_format_cmd));
+		break;
+
+	case AV8100_COMMAND_PATTERNGENERATOR:
+		memcpy(&av8100_config->hdmi_pattern_generator_cmd,
+			&config->pattern_generator_format,
+			sizeof(struct av8100_pattern_generator_format_cmd));
+		break;
+
+	case AV8100_COMMAND_FUSE_AES_KEY:
+		memcpy(&av8100_config->hdmi_fuse_aes_key_cmd,
+			&config->fuse_aes_key_format,
+			sizeof(struct av8100_fuse_aes_key_format_cmd));
+		break;
+
+	default:
+		return AV8100_FAIL;
+		break;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(av8100_conf_prep);
+
+int av8100_conf_w(enum av8100_command_type command_type,
+	u8 *return_buffer_length,
+	u8 *return_buffer, enum interface_type if_type)
+{
+	int retval = 0;
+	u8 cmd_buffer[AV8100_COMMAND_MAX_LENGTH];
+	u32 cmd_length = 0;
+	struct i2c_client *i2c;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	if (return_buffer_length)
+		*return_buffer_length = 0;
+
+	if (!av8100_config)
+		return AV8100_FAIL;
+
+	i2c = av8100_config->client;
+
+	memset(&cmd_buffer, 0x00, AV8100_COMMAND_MAX_LENGTH);
+
+#define PRNK_MODE(_m) dev_dbg(av8100dev, "cmd: " #_m "\n");
+
+	/* Fill the command buffer with configuration data */
+	switch (command_type) {
+	case AV8100_COMMAND_VIDEO_INPUT_FORMAT:
+		PRNK_MODE(AV8100_COMMAND_VIDEO_INPUT_FORMAT);
+		configuration_video_input_get(cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_AUDIO_INPUT_FORMAT:
+		PRNK_MODE(AV8100_COMMAND_AUDIO_INPUT_FORMAT);
+		configuration_audio_input_get(cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_VIDEO_OUTPUT_FORMAT:
+		PRNK_MODE(AV8100_COMMAND_VIDEO_OUTPUT_FORMAT);
+		configuration_video_output_get(cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_VIDEO_SCALING_FORMAT:
+		PRNK_MODE(AV8100_COMMAND_VIDEO_SCALING_FORMAT);
+		configuration_video_scaling_get(cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_COLORSPACECONVERSION:
+		PRNK_MODE(AV8100_COMMAND_COLORSPACECONVERSION);
+		configuration_colorspace_conversion_get(cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_WRITE:
+		PRNK_MODE(AV8100_COMMAND_CEC_MESSAGE_WRITE);
+		configuration_cec_message_write_get(cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_READ_BACK:
+		PRNK_MODE(AV8100_COMMAND_CEC_MESSAGE_READ_BACK);
+		configuration_cec_message_read_get(cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_DENC:
+		PRNK_MODE(AV8100_COMMAND_DENC);
+		configuration_denc_get(cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_HDMI:
+		PRNK_MODE(AV8100_COMMAND_HDMI);
+		configuration_hdmi_get(cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_HDCP_SENDKEY:
+		PRNK_MODE(AV8100_COMMAND_HDCP_SENDKEY);
+		configuration_hdcp_sendkey_get(cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_HDCP_MANAGEMENT:
+		PRNK_MODE(AV8100_COMMAND_HDCP_MANAGEMENT);
+		configuration_hdcp_management_get(cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_INFOFRAMES:
+		PRNK_MODE(AV8100_COMMAND_INFOFRAMES);
+		configuration_infoframe_get(cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_EDID_SECTION_READBACK:
+		PRNK_MODE(AV8100_COMMAND_EDID_SECTION_READBACK);
+		av8100_edid_section_readback_get(cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_PATTERNGENERATOR:
+		PRNK_MODE(AV8100_COMMAND_PATTERNGENERATOR);
+		configuration_pattern_generator_get(cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_FUSE_AES_KEY:
+		PRNK_MODE(AV8100_COMMAND_FUSE_AES_KEY);
+		configuration_fuse_aes_key_get(cmd_buffer, &cmd_length);
+		break;
+
+	default:
+		dev_dbg(av8100dev, "Invalid command type\n");
+		retval = AV8100_INVALID_COMMAND;
+		break;
+	}
+
+	LOCK_AV8100_HW;
+
+	if (if_type == I2C_INTERFACE) {
+		int cnt = 0;
+		int cnt_max;
+
+		dev_dbg(av8100dev, "av8100_conf_w cmd_type:%02x length:%02x ",
+			command_type, cmd_length);
+		dev_dbg(av8100dev, "buffer: ");
+		while (cnt < cmd_length) {
+			dev_dbg(av8100dev, "%02x ", cmd_buffer[cnt]);
+			cnt++;
+		}
+
+		/* Write the command buffer */
+		retval = write_multi_byte(i2c,
+			AV8100_CMD_BUF_OFFSET, cmd_buffer, cmd_length);
+		if (retval) {
+			UNLOCK_AV8100_HW;
+			return retval;
+		}
+
+		/* Write the command */
+		retval = write_single_byte(i2c, AV8100_COMMAND_OFFSET,
+			command_type);
+		if (retval) {
+			UNLOCK_AV8100_HW;
+			return retval;
+		}
+
+
+		/* Get the first return byte */
+		msleep(AV8100_WAITTIME_1MS);
+		cnt = 0;
+		cnt_max = ARRAY_SIZE(waittime_retry);
+		retval = get_command_return_first(i2c, command_type);
+		while (retval && (cnt++ < cnt_max)) {
+			msleep(waittime_retry[cnt]);
+			retval = get_command_return_first(i2c, command_type);
+		}
+		dev_dbg(av8100dev, "first return cnt:%d\n", cnt);
+
+		if (retval) {
+			UNLOCK_AV8100_HW;
+			return retval;
+		}
+
+		retval = get_command_return_data(i2c, command_type, cmd_buffer,
+			return_buffer_length, return_buffer);
+	} else if (if_type == DSI_INTERFACE) {
+		/* TODO */
+	} else {
+		retval = AV8100_INVALID_INTERFACE;
+		dev_dbg(av8100dev, "Invalid command type\n");
+	}
+
+	if (command_type == AV8100_COMMAND_HDMI) {
+		g_av8100_status.hdmi_on = ((av8100_config->hdmi_cmd.
+			hdmi_mode == AV8100_HDMI_ON) &&
+			(av8100_config->hdmi_cmd.hdmi_format == AV8100_HDMI));
+	}
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_conf_w);
+
+int av8100_conf_w_raw(enum av8100_command_type command_type,
+	u8 buffer_length,
+	u8 *buffer,
+	u8 *return_buffer_length,
+	u8 *return_buffer)
+{
+	int retval = 0;
+	struct i2c_client *i2c;
+	int cnt;
+	int cnt_max;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	if (return_buffer_length)
+		*return_buffer_length = 0;
+
+	if (!av8100_config) {
+		retval = AV8100_FAIL;
+		goto av8100_conf_w_raw_out;
+	}
+
+	i2c = av8100_config->client;
+
+	/* Write the command buffer */
+	retval = write_multi_byte(i2c,
+		AV8100_CMD_BUF_OFFSET, buffer, buffer_length);
+	if (retval)
+		goto av8100_conf_w_raw_out;
+
+	/* Write the command */
+	retval = write_single_byte(i2c, AV8100_COMMAND_OFFSET,
+		command_type);
+	if (retval)
+		goto av8100_conf_w_raw_out;
+
+
+	/* Get the first return byte */
+	msleep(AV8100_WAITTIME_1MS);
+	cnt = 0;
+	cnt_max = ARRAY_SIZE(waittime_retry);
+	retval = get_command_return_first(i2c, command_type);
+	while (retval && (cnt++ < cnt_max)) {
+		msleep(waittime_retry[cnt]);
+		retval = get_command_return_first(i2c, command_type);
+	}
+	dev_dbg(av8100dev, "first return cnt:%d\n", cnt);
+	if (retval)
+		goto av8100_conf_w_raw_out;
+
+	retval = get_command_return_data(i2c, command_type, buffer,
+		return_buffer_length, return_buffer);
+
+av8100_conf_w_raw_out:
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_conf_w_raw);
+
+struct av8100_status av8100_status_get(void)
+{
+	return g_av8100_status;
+}
+EXPORT_SYMBOL(av8100_status_get);
+
+enum av8100_output_CEA_VESA av8100_video_output_format_get(int xres,
+	int yres,
+	int htot,
+	int vtot,
+	int pixelclk,
+	bool interlaced)
+{
+	enum av8100_output_CEA_VESA index = 1;
+	int yres_div = !interlaced ? 1 : 2;
+	int hres_div = 1;
+	long freq1;
+	long freq2;
+
+	/*
+	* 720_576_I need a divider for hact and htot since
+	* these params need to be twice as large as expected in av8100_all_cea,
+	* which is used as input parameter to video input config.
+	*/
+	if ((xres == 720) && (yres == 576) && (interlaced == true))
+		hres_div = 2;
+
+	freq1 = 1000000 / htot * 1000000 / vtot / pixelclk + 1;
+	while (index < sizeof(av8100_all_cea)/sizeof(struct av8100_cea)) {
+		freq2 = av8100_all_cea[index].frequence /
+			av8100_all_cea[index].htotale /
+			av8100_all_cea[index].vtotale;
+
+		dev_dbg(av8100dev, "freq1:%ld freq2:%ld\n", freq1, freq2);
+		if ((xres == av8100_all_cea[index].hactive / hres_div) &&
+			(yres == av8100_all_cea[index].vactive * yres_div) &&
+			(htot == av8100_all_cea[index].htotale / hres_div) &&
+			(vtot == av8100_all_cea[index].vtotale) &&
+			(abs(freq1 - freq2) < 2)) {
+			goto av8100_video_output_format_get_out;
+		}
+		index++;
+	}
+
+av8100_video_output_format_get_out:
+	dev_dbg(av8100dev, "av8100_video_output_format_get %d %d %d %d %d\n",
+		xres, yres, htot, vtot, index);
+	return index;
+}
+EXPORT_SYMBOL(av8100_video_output_format_get);
+
+void av8100_hdmi_event_cb_set(void (*hdmi_ev_cb)(enum av8100_hdmi_event))
+{
+	if (av8100_globals)
+		av8100_globals->hdmi_ev_cb = hdmi_ev_cb;
+}
+EXPORT_SYMBOL(av8100_hdmi_event_cb_set);
+
+u8 av8100_ver_get(void)
+{
+	u8 ret;
+
+	LOCK_AV8100_HW;
+	ret = chip_version;
+	UNLOCK_AV8100_HW;
+
+	return ret;
+}
+EXPORT_SYMBOL(av8100_ver_get);
+
+static const struct color_conversion_cmd *get_color_transform_cmd(
+				enum av8100_color_transform transform)
+{
+	const struct color_conversion_cmd *result;
+
+	switch (transform) {
+	case AV8100_COLOR_TRANSFORM_INDENTITY:
+		result = &col_trans_identity;
+		break;
+	case AV8100_COLOR_TRANSFORM_INDENTITY_CLAMP_YUV:
+		result = &col_trans_identity_clamp_yuv;
+		break;
+	case AV8100_COLOR_TRANSFORM_YUV_TO_RGB:
+		if (chip_version == AV8100_CHIPVER_1)
+			result = &col_trans_yuv_to_rgb_v1;
+		else
+			result = &col_trans_yuv_to_rgb_v2;
+		break;
+	case AV8100_COLOR_TRANSFORM_YUV_TO_DENC:
+		result = &col_trans_yuv_to_denc;
+		break;
+	case AV8100_COLOR_TRANSFORM_RGB_TO_DENC:
+		result = &col_trans_rgb_to_denc;
+		break;
+	default:
+		dev_warn(av8100dev, "Unknown color space transform\n");
+		result = &col_trans_identity;
+		break;
+	}
+	return result;
+}
+
+static int av8100_open(struct inode *inode, struct file *filp)
+{
+	dev_dbg(av8100dev, "av8100_open is called\n");
+	return 0;
+}
+
+static int av8100_release(struct inode *inode, struct file *filp)
+{
+	dev_dbg(av8100dev, "av8100_release is called\n");
+	return 0;
+}
+
+static long av8100_ioctl(struct file *file,
+				unsigned int cmd, unsigned long arg)
+{
+	return 0;
+}
+
+static int __devinit av8100_probe(struct i2c_client *i2cClient,
+	const struct i2c_device_id *id)
+{
+	int ret = 0;
+	struct av8100_platform_data *pdata = i2cClient->dev.platform_data;
+
+	av8100dev = &i2cClient->dev;
+
+	dev_dbg(av8100dev, "%s\n", __func__);
+
+	av8100_set_state(AV8100_OPMODE_UNDEFINED);
+
+	ret = av8100_config_init();
+	if (ret) {
+		dev_info(av8100dev, "av8100_config_init failed\n");
+		goto err;
+	}
+
+	ret = av8100_globals_init();
+	if (ret) {
+		dev_info(av8100dev, "av8100_globals_init failed\n");
+		goto err;
+	}
+
+	if (!i2c_check_functionality(i2cClient->adapter,
+		I2C_FUNC_SMBUS_BYTE_DATA |
+		I2C_FUNC_SMBUS_READ_WORD_DATA)) {
+		ret = -ENODEV;
+		dev_info(av8100dev, "av8100 i2c_check_functionality failed\n");
+		goto err;
+	}
+
+	init_waitqueue_head(&av8100_event);
+
+	av8100_config->client = i2cClient;
+	av8100_config->id = (struct i2c_device_id *) id;
+	i2c_set_clientdata(i2cClient, av8100_config);
+
+	kthread_run(av8100_thread, NULL, "av8100_thread");
+
+	ret = request_irq(pdata->irq, av8100_intr_handler,
+			IRQF_TRIGGER_RISING, "av8100", av8100_config);
+	if (ret) {
+		dev_err(av8100dev, "av8100_hw request_irq %d failed %d\n",
+			pdata->irq, ret);
+		gpio_free(pdata->irq);
+		goto err;
+	}
+
+	/* Get regulator resource */
+	if (pdata->regulator_pwr_id) {
+		av8100_globals->regulator_pwr = regulator_get(av8100dev,
+				pdata->regulator_pwr_id);
+		if (IS_ERR(av8100_globals->regulator_pwr)) {
+			ret = PTR_ERR(av8100_globals->regulator_pwr);
+			dev_warn(av8100dev,
+				"%s: Failed to get regulator '%s'\n",
+				__func__, pdata->regulator_pwr_id);
+			av8100_globals->regulator_pwr = NULL;
+			return ret;
+		}
+	}
+
+	/* Get clock resource */
+	if (pdata->inputclk_id) {
+		av8100_globals->inputclk = clk_get(NULL, pdata->inputclk_id);
+		if (IS_ERR(av8100_globals->inputclk)) {
+			av8100_globals->inputclk = NULL;
+			dev_warn(av8100dev, "%s: Failed to get clock '%s'\n",
+					__func__, pdata->inputclk_id);
+		}
+	}
+
+	av8100_set_state(AV8100_OPMODE_SHUTDOWN);
+
+	/* Obtain the chip version */
+	if (av8100_powerup1()) {
+		dev_err(av8100dev, "av8100_powerup1 fail\n");
+		return -EFAULT;
+	}
+
+	ret = av8100_powerdown();
+
+err:
+	return ret;
+}
+
+static int __devexit av8100_remove(struct i2c_client *i2cClient)
+{
+	dev_dbg(av8100dev, "%s\n", __func__);
+
+	av8100_config_exit();
+	av8100_globals_exit();
+	if (av8100_globals->inputclk)
+		clk_put(av8100_globals->inputclk);
+
+	/* Release regulator resource */
+	if (av8100_globals->regulator_pwr)
+		regulator_put(av8100_globals->regulator_pwr);
+
+	return 0;
+}
+
+int av8100_init(void)
+{
+	int ret;
+
+	pr_debug("%s\n", __func__);
+
+	ret = i2c_add_driver(&av8100_driver);
+	if (ret) {
+		pr_err("av8100 i2c_add_driver failed\n");
+		goto av8100_init_err;
+	}
+
+	ret = misc_register(&av8100_miscdev);
+	if (ret) {
+		pr_err("av8100 misc_register failed\n");
+		goto av8100_init_err;
+	}
+
+	return ret;
+
+av8100_init_err:
+	return ret;
+}
+module_init(av8100_init);
+
+void av8100_exit(void)
+{
+	pr_debug("%s\n", __func__);
+
+	misc_deregister(&av8100_miscdev);
+	i2c_del_driver(&av8100_driver);
+}
+module_exit(av8100_exit);
+
+MODULE_AUTHOR("Per Persson <per.xb.persson@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson hdmi display driver");
diff --git a/drivers/video/av8100/av8100_fw.h b/drivers/video/av8100/av8100_fw.h
new file mode 100644
index 00000000000..2beb0736922
--- /dev/null
+++ b/drivers/video/av8100/av8100_fw.h
@@ -0,0 +1,1165 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * Author: Per Persson <per.xb.persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/* AV8100 Firmware version : Unified version V2.2 for av8100v1 and av8100v2.x */
+#define AV8100_FW_SIZE 18432
+char av8100_fw_buff[AV8100_FW_SIZE] = {
+0x80,0xfe,0xcb,0xfe,0x6b,0xf4,0x22,0xf6,0x22,0xf6,0xb9,0xf7,0x21,0xf5,0xbe,0xf7,
+0x06,0xf9,0xc8,0xfa,0xe5,0xfa,0xf6,0xfa,0x08,0xfb,0x1b,0xfb,0x20,0xfb,0x25,0xfb,
+0x2a,0xfb,0x2f,0xfb,0x44,0xfb,0x7e,0xfb,0x23,0xfc,0x65,0xf4,0x65,0xf4,0x66,0xf4,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x81,0xea,0xad,0xec,0xad,
+0x38,0x20,0x1b,0x72,0x80,0x81,0x4d,0x51,0x4d,0x51,0x4d,0x51,0x4d,0x51,0x4d,0x51,
+0x4d,0x51,0xb4,0x20,0xf4,0x2a,0x5a,0x90,0x82,0xfe,0xcd,0x80,0x00,0xaf,0x72,0x93,
+0xa3,0x20,0x5f,0x90,0x80,0x80,0x97,0x90,0x8d,0x20,0x0f,0xab,0x01,0xa6,0x88,0x89,
+0x90,0x96,0xad,0x97,0x90,0x9a,0xad,0x8b,0x93,0x9e,0xad,0x97,0x90,0xa2,0xad,0x80,
+0xb7,0xa6,0xad,0x81,0xb7,0xaa,0xad,0x82,0xb7,0xae,0xad,0x26,0x31,0x20,0x08,0x72,
+0x82,0xb7,0x21,0xa6,0x81,0xb7,0xc2,0xa6,0x80,0xb7,0x01,0xa6,0xb4,0x20,0xf5,0x2a,
+0x5a,0x90,0x80,0x00,0xa7,0x72,0x93,0xcc,0xad,0x49,0x27,0x97,0x90,0x5d,0x90,0xd4,
+0xad,0x82,0xb7,0xd8,0xad,0x81,0xb7,0xdc,0xad,0x80,0xb7,0xe0,0xad,0xff,0xae,0x90,
+0x81,0x59,0x29,0xea,0x26,0x5a,0xfb,0x38,0x20,0x07,0x72,0x4d,0x38,0x20,0x1a,0x72,
+0x49,0x00,0x38,0x20,0x05,0x72,0xbc,0xff,0xcd,0x08,0xae,0xf3,0x20,0x79,0x24,0x49,
+0x44,0x24,0x49,0x23,0x24,0x49,0x0b,0xad,0x82,0xfe,0xcd,0x9e,0x82,0xfe,0xcd,0x9f,
+0x5b,0x82,0xfe,0xcd,0x80,0xb6,0x82,0xfe,0xcd,0x81,0xb6,0x82,0xfe,0xcd,0x82,0xb6,
+0x82,0xad,0x9f,0x90,0x86,0xad,0x85,0x90,0x84,0x42,0x8c,0x20,0x9f,0x90,0x01,0xab,
+0x0f,0xa6,0x2a,0x31,0x20,0x08,0x72,0xfb,0x38,0x20,0x07,0x72,0x4d,0x38,0x20,0x1a,
+0x72,0xbc,0xff,0xcd,0xfb,0x38,0x20,0x07,0x72,0x30,0x20,0x10,0x72,0x31,0x20,0x1e,
+0x72,0x30,0x20,0x1c,0x72,0x95,0x05,0xa0,0x9e,0x3d,0x18,0xcc,0x5f,0x90,0x5f,0x4f,
+0x07,0x38,0x20,0x05,0x72,0x0c,0x39,0x20,0x05,0x72,0x81,0x41,0x29,0x38,0x20,0x18,
+0x72,0xd5,0x26,0x5a,0xfb,0x38,0x20,0x07,0x72,0x9d,0x9d,0x4d,0x38,0x20,0x1a,0x72,
+0x4d,0xae,0xff,0xcd,0x4d,0xae,0xff,0xcd,0x38,0x20,0x1b,0x72,0x4d,0x4d,0x4d,0x4d,
+0x4d,0x38,0x20,0x19,0x72,0x04,0x25,0x49,0x38,0x20,0x18,0x72,0x08,0xae,0x34,0x20,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x81,0xa5,0xbe,0x84,0xa3,0xb7,0xa4,0xd6,0x92,0x5c,0xa2,0xb7,0xa4,
+0xd6,0x92,0x5c,0xa1,0xb7,0xa4,0xd6,0x92,0x01,0xae,0xa0,0xb7,0xa4,0xc6,0x92,0x88,
+0xa5,0xbf,0x81,0xa0,0xb7,0xa4,0xc2,0x92,0xa0,0xb6,0xa1,0xb7,0xa4,0xd2,0x92,0x5a,
+0xa1,0xb6,0xa2,0xb7,0xa4,0xd2,0x92,0x5a,0xa2,0xb6,0xa3,0xb7,0xa4,0xd0,0x92,0xa3,
+0xb6,0x03,0xae,0xa5,0xbf,0x81,0xa3,0xb7,0xa4,0xda,0x92,0x5c,0xa3,0xb6,0xa2,0xb7,
+0xa4,0xda,0x92,0x5c,0xa2,0xb6,0xa1,0xb7,0xa4,0xda,0x92,0x01,0xae,0xa1,0xb6,0xa0,
+0xb7,0xa4,0xca,0x92,0xa0,0xb6,0xa5,0xbf,0xf1,0xfc,0xcc,0xd8,0xfd,0xcd,0x81,0xa5,
+0xbe,0xe9,0x26,0x4a,0xa4,0x00,0x39,0x72,0xa4,0x00,0x69,0x72,0x5a,0xa4,0x00,0x69,
+0x72,0x5a,0xa4,0x00,0x68,0x72,0x03,0xae,0xa5,0xbf,0x1b,0x27,0x4d,0x81,0xa0,0xb7,
+0xa4,0xc9,0x92,0xa0,0xb6,0xa1,0xb7,0xa4,0xd9,0x92,0x5a,0xa1,0xb6,0xa2,0xb7,0xa4,
+0xd9,0x92,0x5a,0xa2,0xb6,0xa3,0xb7,0xa4,0xdb,0x92,0xa3,0xb6,0x03,0xae,0xa5,0xbf,
+0xf1,0xfc,0xcc,0x90,0xfd,0xcd,0x81,0xf1,0x20,0x01,0xab,0xa4,0xd6,0x92,0x07,0x2b,
+0x5a,0x0a,0x24,0xa4,0xd7,0x92,0xa4,0xdb,0x92,0x03,0xae,0xa5,0xbf,0x81,0x01,0xa6,
+0x02,0x20,0xff,0xa6,0x04,0x24,0x08,0x27,0xa4,0xd1,0x92,0xa3,0xb6,0xa5,0x3c,0x09,
+0x26,0xa4,0xd1,0x92,0xa2,0xb6,0xa5,0x3c,0x12,0x26,0xa4,0xd1,0x92,0xa1,0xb6,0xa5,
+0x3c,0x23,0x26,0xa4,0xd1,0x92,0xa0,0xb6,0xa5,0x3f,0x81,0xa0,0x3c,0x02,0x24,0xa1,
+0xb7,0xa1,0xb9,0x9f,0xa2,0xb7,0xa2,0xbb,0x42,0x93,0x0f,0x27,0x4d,0x84,0x37,0xfd,
+0xcd,0xa8,0xb6,0x05,0x27,0xa7,0xbe,0xa3,0xb7,0xa2,0xbf,0x42,0x93,0xa8,0xb6,0xa1,
+0xb7,0xa0,0xbf,0x42,0xa7,0xb6,0x89,0xa8,0xb7,0x81,0xa0,0x3f,0xa1,0x3f,0xa2,0xbf,
+0xa3,0xb7,0x81,0xa5,0xbe,0x84,0xa4,0xd7,0x92,0xa3,0xb6,0x5c,0xa4,0xd7,0x92,0xa2,
+0xb6,0x5c,0xa4,0xd7,0x92,0xa1,0xb6,0x01,0xae,0xa4,0xc7,0x92,0xa0,0xb6,0x88,0xa5,
+0xbf,0x81,0xa5,0xbe,0xa4,0xd7,0x92,0xa3,0xb6,0x5c,0xa4,0xd7,0x92,0xa2,0xb6,0x5c,
+0xa4,0xd7,0x92,0xa1,0xb6,0x01,0xae,0xa4,0xc7,0x92,0xa0,0xb6,0xa5,0xbf,0x81,0x01,
+0xa6,0x02,0x27,0x03,0x6d,0x04,0x26,0x02,0x6d,0x08,0x26,0x01,0x6d,0x0e,0x26,0x7d,
+0x81,0x84,0xa3,0xb7,0x03,0xe6,0xa2,0xb7,0x02,0xe6,0xa1,0xb7,0x01,0xe6,0xa0,0xb7,
+0xf6,0x88,0x81,0x01,0xa6,0x02,0x27,0xa3,0x3d,0x04,0x26,0xa2,0x3d,0x08,0x26,0xa1,
+0x3d,0x0e,0x26,0xa0,0x3d,0x81,0xf5,0x26,0x4a,0xa0,0x39,0xa1,0x39,0xa2,0x39,0xa3,
+0x38,0x0b,0x27,0x4d,0x81,0xa0,0x3f,0xa1,0x3f,0x81,0xa0,0xb7,0xa1,0xb7,0xff,0xa6,
+0x07,0x2a,0xa2,0xbf,0xa3,0xb7,0x81,0x41,0xa8,0xbb,0x41,0x52,0x93,0x84,0xa8,0xb7,
+0xa8,0xbb,0x52,0x01,0x7b,0xa7,0xbe,0xa8,0xb7,0x52,0x9f,0x90,0x88,0x81,0xa5,0xbe,
+0xa3,0xb7,0xa4,0xd6,0x92,0x5c,0xa2,0xb7,0xa4,0xd6,0x92,0x5c,0xa1,0xb7,0xa4,0xd6,
+0x92,0x01,0xae,0xa0,0xb7,0xa4,0xc6,0x92,0xa5,0xbf,0x81,0xa0,0x3f,0xa2,0xb7,0xa1,
+0xbf,0x5c,0x01,0x24,0xa2,0xbb,0x42,0x93,0xa0,0xb6,0xa3,0xb7,0xa2,0xbf,0x42,0x93,
+0xa0,0xbf,0x80,0xaa,0x00,0xc7,0x51,0x20,0xc6,0x50,0x20,0x11,0x72,0x80,0x85,0x90,
+0xa6,0x00,0x32,0xa5,0x00,0x32,0xa4,0x00,0x32,0xa9,0x00,0x32,0xa8,0x00,0x32,0xa7,
+0x00,0x32,0x84,0x84,0x84,0x84,0x00,0x18,0xcd,0x63,0xa6,0x05,0x20,0xda,0x01,0x5a,
+0x72,0xd9,0x01,0x5a,0x72,0x04,0x26,0xda,0x01,0xc6,0x0f,0x27,0xd9,0x01,0xca,0xda,
+0x01,0xc6,0x00,0x18,0xcd,0x12,0xa6,0xc0,0x00,0x5f,0x72,0x09,0x27,0xc0,0x00,0xc6,
+0xa2,0x01,0x5f,0x72,0xd9,0x01,0x5f,0x72,0xda,0x01,0x18,0x35,0x08,0x25,0x00,0xa2,
+0xd9,0x01,0xc6,0x19,0xa0,0xda,0x01,0xc6,0x18,0x20,0xa2,0x01,0x01,0x35,0x06,0x27,
+0x10,0xa5,0x04,0x7b,0x01,0x6b,0x10,0x40,0xc6,0x02,0x6b,0x11,0x40,0xc6,0x03,0x6b,
+0x12,0x40,0xc6,0x04,0x6b,0x13,0x40,0xc6,0xe8,0xc4,0xcd,0x46,0x20,0xc7,0x46,0x20,
+0xc6,0x41,0x20,0x5f,0x72,0x0a,0x42,0x20,0x0d,0x72,0x88,0x88,0x88,0x88,0xa7,0x00,
+0x3b,0xa8,0x00,0x3b,0xa9,0x00,0x3b,0xa4,0x00,0x3b,0xa5,0x00,0x3b,0xa6,0x00,0x3b,
+0x89,0x90,0x80,0x85,0x90,0xa6,0x00,0x32,0xa5,0x00,0x32,0xa4,0x00,0x32,0xa9,0x00,
+0x32,0xa8,0x00,0x32,0xa7,0x00,0x32,0x00,0x18,0xcd,0x15,0x20,0x10,0x72,0x07,0x27,
+0x09,0x2b,0x10,0xb6,0x12,0x20,0x01,0x35,0xa7,0x00,0x3b,0xa8,0x00,0x3b,0xa9,0x00,
+0x3b,0xa4,0x00,0x3b,0xa5,0x00,0x3b,0xa6,0x00,0x3b,0x89,0x90,0x80,0x0c,0x40,0x00,
+0x35,0x0d,0x40,0x00,0x35,0x0e,0x40,0x00,0x35,0x0f,0x40,0x04,0x35,0x11,0x20,0x80,
+0x35,0x80,0x11,0x20,0x40,0x35,0x80,0x11,0x20,0x20,0x35,0x80,0x11,0x20,0x10,0x35,
+0x80,0x11,0x20,0x08,0x35,0x80,0x17,0x20,0x14,0x72,0x80,0x17,0x20,0x15,0x72,0x05,
+0x17,0x20,0x05,0x72,0x11,0x20,0x04,0x35,0x80,0xe7,0x01,0xc7,0x4a,0x05,0x20,0x1a,
+0x72,0x06,0x20,0x18,0x72,0x11,0x20,0xc7,0x02,0xa6,0x80,0xe7,0x01,0xc7,0x05,0x20,
+0x1a,0x72,0x06,0x20,0x19,0x72,0x11,0x20,0xc7,0x01,0xa6,0x80,0x07,0x20,0x01,0x35,
+0x04,0x88,0x00,0x35,0x05,0x88,0x30,0x35,0x06,0x88,0x00,0x35,0x07,0x88,0x00,0x35,
+0x10,0x20,0x80,0x35,0x07,0x20,0x03,0x35,0x81,0xef,0xfc,0xcd,0xbc,0xae,0xa4,0x00,
+0x84,0x35,0x81,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x80,0x85,0x90,0xa6,0x00,0x32,
+0xa5,0x00,0x32,0xa4,0x00,0x32,0xa9,0x00,0x32,0xa8,0x00,0x32,0xa7,0x00,0x32,0xa3,
+0x00,0x32,0xa2,0x00,0x32,0xa1,0x00,0x32,0xa0,0x00,0x32,0x84,0x84,0x84,0x84,0x84,
+0x84,0xef,0xfc,0xcd,0xa0,0xae,0xa4,0x00,0x84,0x35,0xd8,0xfd,0xcd,0x33,0xad,0x5b,
+0xa0,0x3f,0xa1,0x3f,0xa2,0x1f,0x0e,0xfd,0xcd,0xf4,0x01,0xce,0xf5,0x01,0xc6,0x03,
+0x6b,0x80,0xa4,0x03,0x7b,0x04,0x6b,0x4f,0x01,0x6b,0xa0,0x84,0xc6,0x02,0x6b,0xa1,
+0x84,0xc6,0x03,0x6b,0xa2,0x84,0xc6,0x04,0x6b,0xa3,0x84,0xc6,0x84,0x84,0xf4,0x01,
+0xcf,0xf5,0x01,0xc7,0x4d,0xc5,0xcd,0xbc,0x00,0xce,0xbd,0x00,0xc6,0xf6,0x01,0x3b,
+0xf7,0x01,0x3b,0x0f,0x20,0x31,0xc5,0xcd,0xbc,0x00,0xce,0xbd,0x00,0xc6,0xf6,0x01,
+0x3b,0xf7,0x01,0x3b,0x11,0x24,0x05,0xe2,0x72,0xf6,0x01,0xc6,0x06,0xe0,0x72,0xf7,
+0x01,0xc6,0x1f,0x24,0x05,0xe2,0x72,0xbc,0x00,0xc6,0x06,0xe0,0x72,0xbd,0x00,0xc6,
+0x1c,0x25,0xf6,0x01,0xc2,0x05,0x7b,0xf7,0x01,0xc0,0x06,0x7b,0x0c,0x24,0xbc,0x00,
+0xc2,0x05,0x7b,0xbd,0x00,0xc0,0x06,0x7b,0x5c,0x20,0xf4,0x01,0xcf,0xbc,0x00,0xce,
+0xf5,0x01,0xbd,0x00,0x55,0x0d,0x26,0x71,0x01,0xc6,0x0d,0x20,0xf6,0x01,0xce,0xf5,
+0x01,0xc7,0xf7,0x01,0xc6,0x0b,0x27,0x4a,0x70,0x01,0xc6,0x07,0x20,0x01,0x35,0x07,
+0x20,0x03,0x35,0x08,0x26,0x04,0xa1,0x05,0x20,0x1d,0x72,0x05,0x20,0x1c,0x72,0x08,
+0x25,0x02,0xa1,0xbb,0x00,0xc6,0x1b,0x26,0xbc,0x00,0xc1,0x05,0x7b,0x22,0x26,0xbd,
+0x00,0xc1,0x06,0x7b,0x29,0x26,0x4a,0x70,0x01,0xc6,0xf6,0x01,0x5f,0x72,0xf7,0x01,
+0xc7,0x06,0xa6,0xbe,0xfa,0xcd,0xa2,0xb7,0x18,0xaa,0xa2,0xb6,0x1e,0xfe,0xcd,0xb6,
+0xfa,0xcd,0x5b,0x12,0x20,0x0a,0xa6,0xbe,0xfa,0xcd,0xa2,0x18,0x1e,0xfe,0xcd,0xb6,
+0xfa,0xcd,0x5b,0x10,0x24,0x00,0xa2,0x05,0x7b,0x0a,0xa0,0x06,0x7b,0x1a,0x25,0x00,
+0xa2,0x05,0x7b,0x06,0xa0,0x06,0x7b,0x03,0x6b,0xc7,0xa4,0x03,0x7b,0x01,0x6b,0xbc,
+0x84,0xc6,0x02,0x6b,0xbd,0x84,0xc6,0x03,0x6b,0xbe,0x84,0xc6,0x04,0x6b,0xbf,0x84,
+0xc6,0x57,0x27,0x71,0x01,0xc6,0x06,0x6b,0xa7,0x83,0xc6,0x05,0x6b,0xa6,0x83,0xc6,
+0xf5,0x01,0x5f,0x72,0xf4,0x01,0x5f,0x72,0xf7,0x01,0x5f,0x72,0xf6,0x01,0x5f,0x72,
+0x10,0x20,0x40,0x35,0x88,0x88,0x88,0x88,0x88,0x88,0xa0,0x00,0x3b,0xa1,0x00,0x3b,
+0xa2,0x00,0x3b,0xa3,0x00,0x3b,0xa7,0x00,0x3b,0xa8,0x00,0x3b,0xa9,0x00,0x3b,0xa4,
+0x00,0x3b,0xa5,0x00,0x3b,0xa6,0x00,0x3b,0x89,0x90,0x81,0x48,0x83,0x00,0x35,0x49,
+0x83,0x00,0x35,0x4a,0x83,0x00,0x35,0x4b,0x83,0x0f,0x35,0x80,0x85,0x90,0xa6,0x00,
+0x32,0xa5,0x00,0x32,0xa4,0x00,0x32,0xa9,0x00,0x32,0xa8,0x00,0x32,0xa7,0x00,0x32,
+0x84,0x84,0x84,0x84,0xe8,0x01,0x5f,0x72,0x48,0xcb,0xcd,0x20,0xad,0x05,0x26,0x4a,
+0xe8,0x01,0xc6,0x0b,0x26,0x03,0xa1,0x70,0x01,0xc6,0xf0,0x01,0x5c,0x72,0x04,0x27,
+0x0f,0xa1,0xf0,0x01,0xc6,0x0b,0x20,0xe7,0x01,0x01,0x35,0x11,0x26,0xf0,0x01,0x5a,
+0x72,0x17,0x27,0xf0,0x01,0xc6,0x11,0x26,0x03,0xea,0x72,0x04,0x7b,0x07,0x27,0x4c,
+0x03,0x7b,0x05,0x26,0x4c,0x04,0x7b,0x11,0x27,0x03,0x7b,0x04,0x26,0x4a,0x04,0x7b,
+0x74,0xe1,0xcd,0x03,0xee,0x72,0x9f,0x03,0x6b,0x04,0xef,0x72,0x03,0xe2,0x72,0x41,
+0x04,0xe0,0x72,0x5f,0x01,0xa6,0x03,0x6b,0x01,0xe2,0x72,0x03,0x7b,0x04,0x6b,0x02,
+0xe0,0x72,0x04,0x7b,0x0e,0x24,0x03,0xe2,0x72,0x9f,0x04,0xe0,0x72,0x46,0x54,0x01,
+0xef,0x72,0x02,0x6b,0x46,0x54,0x4e,0x01,0xce,0x4f,0x01,0xc6,0x08,0x20,0x4e,0x01,
+0xce,0x4f,0x01,0xc6,0x08,0x26,0x4a,0x47,0x01,0xc6,0xe3,0x26,0x03,0xe1,0x72,0xa6,
+0x83,0xc6,0xeb,0x26,0x04,0xe1,0x72,0xa7,0x83,0xc6,0x04,0x6b,0xa7,0x83,0xc6,0x03,
+0x6b,0xa6,0x83,0xc6,0xc6,0xf8,0xcc,0x84,0x84,0x84,0x84,0xaf,0x00,0x35,0x35,0xb0,
+0x00,0x86,0x35,0xb1,0x00,0x37,0x35,0xb2,0x00,0xbd,0x35,0xf0,0x01,0x0f,0x35,0xf2,
+0x01,0x01,0x35,0xf5,0xf8,0xcd,0x69,0xc5,0xcd,0xb3,0x00,0x3b,0xb4,0x00,0x3b,0xb5,
+0x00,0x3b,0xb6,0x00,0x3b,0x31,0x26,0xf2,0x01,0xc6,0x16,0xcb,0xcd,0x03,0x27,0xf0,
+0x01,0xc6,0xdc,0xf8,0xcc,0x03,0x26,0x02,0xa1,0xe6,0x01,0xc6,0xc6,0xf8,0xcc,0x03,
+0x27,0x02,0xa1,0x70,0x01,0xc6,0x10,0x20,0x20,0x35,0x88,0x88,0x88,0x88,0xa7,0x00,
+0x3b,0xa8,0x00,0x3b,0xa9,0x00,0x3b,0xa4,0x00,0x3b,0xa5,0x00,0x3b,0xa6,0x00,0x3b,
+0x89,0x90,0x80,0x10,0x20,0x08,0x35,0x81,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,
+0x02,0xab,0x80,0x85,0x90,0xa6,0x00,0x32,0xa5,0x00,0x32,0xa4,0x00,0x32,0xa9,0x00,
+0x32,0xa8,0x00,0x32,0xa7,0x00,0x32,0xa3,0x00,0x32,0xa2,0x00,0x32,0xa1,0x00,0x32,
+0xa0,0x00,0x32,0x84,0x84,0x84,0x84,0x84,0xef,0xfc,0xcd,0xa4,0xbf,0x84,0xae,0xa3,
+0x1a,0x2f,0xad,0x5b,0x09,0x20,0x04,0xae,0xa4,0x00,0x44,0x35,0xa0,0x3f,0x3c,0xad,
+0x5b,0x02,0x6b,0x08,0x84,0xc6,0x03,0x6b,0x09,0x84,0xc6,0x04,0x6b,0x0a,0x84,0xc6,
+0x05,0x6b,0x0b,0x84,0xc6,0xef,0xfc,0xcd,0xa4,0xbf,0x84,0xae,0xa3,0x1b,0x5c,0xad,
+0x5b,0x2d,0x27,0x72,0x01,0xc6,0x32,0x27,0xc2,0x00,0xc6,0x02,0x6b,0x84,0x84,0xc6,
+0x03,0x6b,0x85,0x84,0xc6,0x04,0x6b,0x86,0x84,0xc6,0x05,0x6b,0x87,0x84,0xc6,0x84,
+0x84,0x84,0xd3,0xcc,0xcd,0x41,0x00,0xa9,0x41,0xd4,0xab,0x5f,0x52,0x21,0xae,0xd2,
+0x00,0xc6,0x02,0x4b,0x10,0x4b,0x00,0x4b,0xd2,0x00,0x5c,0x72,0x04,0x20,0xd2,0x00,
+0x5f,0x72,0x06,0x26,0xa8,0xb1,0xa8,0xbf,0x90,0x0d,0x26,0xa7,0xb3,0xa7,0x3f,0xd2,
+0x00,0xce,0x90,0x4a,0x5a,0x01,0x26,0x4d,0x5f,0x3a,0x25,0x02,0xa1,0xd3,0x00,0xc6,
+0xd6,0xca,0xcd,0xe7,0x01,0x5f,0x72,0x07,0x26,0x4a,0xe7,0x01,0xc6,0x06,0x27,0x4a,
+0x70,0x01,0xc6,0xc6,0xd8,0xcd,0xe9,0x01,0xc7,0x06,0x26,0x4a,0xe9,0x01,0xc6,0x86,
+0x01,0x5c,0x72,0x04,0x27,0x4c,0x86,0x01,0xc6,0x0a,0x20,0x86,0x01,0x5f,0x72,0x06,
+0x27,0xf0,0x01,0xc6,0xf1,0x01,0x5c,0x72,0x04,0x24,0xff,0xa1,0xf1,0x01,0xc6,0xe8,
+0x01,0x01,0x35,0xef,0xfc,0xcd,0xa0,0xae,0xa4,0x00,0x84,0x35,0xa0,0x1f,0xae,0xf7,
+0xcd,0x5b,0x02,0x6b,0xa0,0x84,0xc6,0x03,0x6b,0xa1,0x84,0xc6,0x04,0x6b,0xa2,0x84,
+0xc6,0x05,0x6b,0xa3,0x84,0xc6,0x84,0x84,0x84,0x84,0x69,0xc5,0xcd,0xb3,0x00,0x3b,
+0xb4,0x00,0x3b,0xb5,0x00,0x3b,0xb6,0x00,0x3b,0xf2,0x01,0x5f,0x72,0x41,0x25,0x04,
+0xa1,0xf1,0x01,0xc6,0x65,0x27,0x4a,0x70,0x01,0xc6,0xef,0xfc,0xcd,0x9c,0xae,0xa4,
+0x00,0x83,0x35,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,0xb7,0x01,0x6b,0x9f,0x83,
+0xc6,0x05,0x20,0x14,0x72,0x10,0x20,0x06,0x35,0x88,0x88,0x88,0x88,0x88,0xa0,0x00,
+0x3b,0xa1,0x00,0x3b,0xa2,0x00,0x3b,0xa3,0x00,0x3b,0xa7,0x00,0x3b,0xa8,0x00,0x3b,
+0xa9,0x00,0x3b,0xa4,0x00,0x3b,0xa5,0x00,0x3b,0xa6,0x00,0x3b,0x89,0x90,0x81,0xa4,
+0x00,0x8c,0x35,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,0xb7,0x80,0x85,0x90,0xa6,
+0x00,0x32,0xa5,0x00,0x32,0xa4,0x00,0x32,0xa9,0x00,0x32,0xa8,0x00,0x32,0xa7,0x00,
+0x32,0xa3,0x00,0x32,0xa2,0x00,0x32,0xa1,0x00,0x32,0xa0,0x00,0x32,0x84,0xef,0xfc,
+0xcd,0x0c,0xae,0x27,0xad,0xf0,0xa4,0x01,0x7b,0x87,0x01,0x5f,0x72,0x05,0x20,0x16,
+0x72,0x06,0x20,0xc7,0x05,0xaa,0x06,0x20,0xc6,0x1b,0x27,0x04,0xa5,0x01,0x7b,0xef,
+0xfc,0xcd,0x0c,0xae,0x48,0xad,0xf0,0xa4,0x05,0x20,0x16,0x72,0x06,0x20,0x14,0x72,
+0x11,0x20,0x87,0x01,0x5a,0x72,0x0f,0xf5,0xcd,0x02,0xa6,0x05,0x26,0x4a,0x05,0x20,
+0x4f,0x03,0x25,0x02,0xa1,0x87,0x01,0xc6,0xef,0xfc,0xcd,0x10,0xae,0x71,0xad,0x88,
+0x01,0xd6,0x22,0x27,0x87,0x01,0xce,0x59,0x27,0x0f,0xa5,0x01,0x7b,0xef,0xfc,0xcd,
+0x0c,0xae,0x14,0xf6,0xcd,0x0f,0xa4,0x05,0x20,0x16,0x72,0x06,0x20,0x12,0x72,0x74,
+0x01,0x01,0x35,0x18,0x27,0x20,0xa5,0x01,0x7b,0x98,0xad,0x4f,0x05,0x27,0x80,0xa5,
+0x01,0x7b,0x76,0x01,0xd7,0x17,0x8c,0xc6,0x75,0x01,0x5c,0x72,0x75,0x01,0xce,0x0f,
+0x27,0x80,0xa5,0x07,0x20,0x75,0x01,0xcf,0x06,0x20,0x13,0x72,0x09,0x27,0x10,0xa5,
+0x20,0x26,0x74,0x01,0xce,0x01,0x6b,0x0f,0x8c,0xc6,0x10,0x20,0x10,0x35,0x88,0xa0,
+0x00,0x3b,0xa1,0x00,0x3b,0xa2,0x00,0x3b,0xa3,0x00,0x3b,0xa7,0x00,0x3b,0xa8,0x00,
+0x3b,0xa9,0x00,0x3b,0xa4,0x00,0x3b,0xa5,0x00,0x3b,0xa6,0x00,0x3b,0x89,0x90,0xef,
+0xfc,0xcc,0x0c,0xae,0xa4,0x00,0x8c,0x35,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,
+0xb7,0x80,0x85,0x90,0xa6,0x00,0x32,0xa5,0x00,0x32,0xa4,0x00,0x32,0xa9,0x00,0x32,
+0xa8,0x00,0x32,0xa7,0x00,0x32,0x84,0x84,0x84,0x84,0xa0,0x84,0xc7,0x01,0x7b,0xa1,
+0x84,0xc7,0x02,0x7b,0xa2,0x84,0xc7,0x03,0x7b,0xa3,0x84,0xc7,0x04,0x7b,0xe8,0x01,
+0x01,0x35,0x01,0x6b,0x7f,0xa4,0x01,0x7b,0x0a,0x20,0x01,0x6b,0x80,0xaa,0x01,0x7b,
+0x12,0x27,0x02,0xa1,0xe6,0x01,0xc6,0x0f,0x26,0x03,0xa1,0x04,0x27,0x02,0xa1,0x70,
+0x01,0xc6,0x1a,0x27,0x71,0x01,0xc6,0x05,0x26,0x72,0x01,0xc6,0x01,0x6b,0xa0,0x84,
+0xc6,0x02,0x6b,0xa1,0x84,0xc6,0x03,0x6b,0xa2,0x84,0xc6,0x04,0x6b,0xa3,0x84,0xc6,
+0xf1,0x01,0x5f,0x72,0xe6,0x01,0xc7,0x03,0xa4,0x83,0x84,0xc6,0xfe,0xc5,0xcd,0x03,
+0x26,0x4a,0xe5,0x01,0xc6,0x05,0x20,0x12,0x72,0x10,0x20,0x01,0x35,0x88,0x88,0x88,
+0x88,0xa7,0x00,0x3b,0xa8,0x00,0x3b,0xa9,0x00,0x3b,0xa4,0x00,0x3b,0xa5,0x00,0x3b,
+0xa6,0x00,0x3b,0x89,0x90,0x80,0x12,0x20,0x10,0x35,0x80,0xfb,0xf2,0xcc,0x28,0x4b,
+0x07,0xce,0xcd,0xff,0xf2,0xcc,0xc1,0xa6,0x00,0x4b,0x08,0x4b,0x72,0xce,0xcd,0xff,
+0xf2,0xcc,0xe0,0xa6,0x01,0x4b,0xe0,0x4b,0x80,0xf3,0xcc,0x03,0x26,0x9a,0x01,0x5a,
+0x72,0x81,0x84,0x84,0x9a,0x01,0x02,0x35,0xa0,0xf2,0xcd,0xe0,0xa6,0x01,0x4b,0xe0,
+0x4b,0x10,0x27,0x4d,0x56,0xd2,0xcd,0x28,0x27,0x9c,0x01,0xc5,0x9e,0x01,0xc6,0xff,
+0xf2,0xcc,0xc1,0xa6,0x00,0x4b,0x28,0x4b,0xfd,0xf2,0xcc,0x01,0x4b,0x90,0x4b,0x07,
+0xce,0xcd,0x0a,0x27,0x9c,0x01,0xc5,0x9e,0x01,0xc6,0xc5,0x20,0x28,0x4b,0xff,0xf2,
+0xcc,0xe0,0xa6,0x01,0x4b,0x90,0x4b,0xcd,0xce,0xcd,0x0c,0x27,0x9c,0x01,0xc5,0x9e,
+0x01,0xc6,0xdd,0x20,0x08,0x4b,0xff,0xf2,0xcc,0x82,0xa6,0x01,0x4b,0x90,0x4b,0x9b,
+0x01,0x5a,0x72,0xce,0x27,0x9b,0x01,0xc6,0x12,0x26,0x4d,0xcc,0xd1,0xcd,0xff,0xf2,
+0xcc,0xc0,0xa6,0x00,0x4b,0x50,0x4b,0xb7,0x27,0x4d,0x56,0xd2,0xcd,0xe8,0x26,0x4d,
+0x85,0xcf,0xcd,0x03,0xa6,0x26,0x20,0x9b,0x01,0x0a,0x35,0xcb,0x27,0x4d,0x56,0xd2,
+0xcd,0xd3,0x20,0x60,0x4b,0x04,0x27,0x4d,0x85,0xcf,0xcd,0x83,0xa6,0x18,0x27,0x4d,
+0xae,0xd1,0xcd,0xff,0xf2,0xcc,0x81,0xa6,0x00,0x4b,0x60,0x4b,0xec,0x26,0x4d,0x85,
+0xcf,0xcd,0x02,0xa6,0x2e,0xd1,0xcd,0xff,0xf2,0xcc,0xa0,0xa6,0x00,0x4b,0x08,0x4b,
+0x09,0x27,0x4d,0x6e,0xd0,0xcd,0x06,0x26,0x4d,0x85,0xcf,0xcd,0x4c,0x8c,0x20,0x80,
+0xa6,0x88,0x88,0x4f,0x93,0x20,0x40,0xa6,0x00,0x4b,0xa0,0x4b,0x08,0x26,0x4d,0xdd,
+0xcf,0xcd,0x5d,0xf4,0xcc,0x21,0xf4,0xcc,0x03,0x26,0x1f,0xa0,0x06,0xf4,0xcc,0x03,
+0x26,0x4a,0xee,0xf3,0xcc,0x03,0x26,0x20,0xa0,0xe0,0x27,0x1e,0xa0,0xd2,0xf3,0xcc,
+0x03,0x26,0x4a,0x60,0x27,0x4a,0x39,0x27,0x20,0xa0,0xc3,0x27,0x20,0xa0,0x2c,0x27,
+0x20,0xa0,0x30,0x27,0x20,0xa0,0x5d,0xf4,0xcc,0x03,0x26,0xa8,0x01,0xc6,0xdd,0x20,
+0x60,0xa6,0x00,0x4b,0x50,0x4b,0x07,0xce,0xcd,0x03,0x25,0x61,0xa1,0xa8,0x01,0xc6,
+0x12,0x27,0x11,0xa1,0xa1,0x01,0xc6,0x07,0x27,0x9f,0x01,0xc6,0x81,0x84,0x84,0x9f,
+0xad,0x20,0xa6,0x00,0x4b,0x50,0x4b,0x0b,0x26,0x72,0x01,0xc6,0x05,0x27,0xa8,0x01,
+0xc6,0x10,0x20,0x4f,0x00,0x4b,0xa0,0x4b,0x07,0xce,0xcd,0x03,0x27,0xa8,0x01,0xc6,
+0x0f,0x26,0xa2,0x01,0xc6,0x81,0x84,0x84,0x84,0xdf,0xc4,0xcd,0x06,0xee,0x72,0x07,
+0x7b,0xa8,0x01,0xc7,0x03,0x7b,0x05,0x20,0x18,0x72,0x04,0x27,0x01,0xe1,0x72,0xf0,
+0xa4,0xa8,0x01,0xc6,0x01,0x6b,0xf0,0xa4,0x03,0x7b,0x06,0x20,0xc7,0x02,0x6b,0x02,
+0xea,0x72,0xe0,0xa4,0x03,0x7b,0x02,0x6b,0x1f,0xa4,0x06,0x20,0xc6,0x88,0x88,0x88,
+0xfe,0xce,0x80,0x3d,0xe1,0xb7,0x80,0x3d,0xab,0x93,0x80,0x3d,0x25,0x6e,0x9f,0x3d,
+0x81,0x13,0x00,0x01,0x35,0x04,0x27,0x12,0xb7,0xc0,0x84,0xc6,0x50,0x18,0xcd,0x11,
+0xb7,0x11,0xf1,0xcd,0x50,0x18,0xcc,0x11,0xb7,0x01,0xa6,0x07,0x24,0x20,0xa1,0xf3,
+0x01,0xc6,0x09,0x20,0x5f,0xef,0xcd,0xd6,0x20,0x5f,0xef,0xcd,0x05,0x26,0x04,0xa1,
+0x12,0xb6,0x06,0x27,0x02,0xa1,0x12,0xb6,0x11,0x26,0x02,0xa1,0x11,0xb6,0xb3,0x20,
+0x4a,0xef,0xcd,0x2a,0x20,0x93,0xee,0xcd,0x2f,0x20,0x0e,0xee,0xcd,0xc2,0x20,0x11,
+0xb7,0x0e,0xee,0xcd,0x07,0x26,0x02,0xa1,0x11,0xb6,0x81,0x12,0x00,0x18,0x4d,0x55,
+0x13,0x00,0x19,0x4d,0x55,0x14,0x00,0x1a,0x4d,0x55,0x15,0x00,0x1b,0x4d,0x55,0x6e,
+0x25,0x20,0xa1,0xf3,0x01,0xc6,0x50,0x18,0xcd,0x11,0xb7,0xc0,0xeb,0xcd,0x65,0x20,
+0x22,0xeb,0xcd,0x6a,0x20,0xe1,0xe7,0xcd,0x45,0x18,0xcc,0x40,0xe7,0xcd,0x75,0x20,
+0x61,0xe7,0xcd,0x7a,0x20,0x68,0xe6,0xcd,0x77,0xf2,0xcc,0x12,0xe6,0xcd,0x77,0xf2,
+0xcc,0xd2,0xe5,0xcd,0x77,0xf2,0xcc,0x25,0xe5,0xcd,0x77,0xf2,0xcc,0x02,0xe4,0xcd,
+0x81,0x88,0xaf,0xf1,0xd6,0x88,0xb0,0xf1,0xd6,0x58,0x97,0x8f,0xf2,0xcc,0x03,0x25,
+0x0f,0xa1,0x4a,0x6e,0xf2,0x52,0xf2,0x4d,0xf2,0x48,0xf2,0x36,0xf2,0x12,0xf2,0x0d,
+0xf2,0x08,0xf2,0x02,0xf2,0xfd,0xf1,0xf8,0xf1,0xf2,0xf1,0xec,0xf1,0xe6,0xf1,0xe0,
+0xf1,0x81,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x02,0xab,0xf6,0x20,0x01,0xa6,
+0x81,0x85,0x85,0x85,0x85,0x85,0x06,0x26,0x11,0xbe,0x04,0x20,0x4f,0x05,0xc5,0xcd,
+0x0a,0xa6,0x9a,0x78,0x84,0xc7,0x02,0x7b,0x79,0x84,0xc7,0x03,0x7b,0x7a,0x84,0xc7,
+0x04,0x7b,0x7b,0x84,0xc7,0x05,0x7b,0xef,0xfc,0xcd,0x78,0xae,0xa4,0x00,0x84,0x35,
+0xa0,0x18,0x36,0xad,0x5b,0x02,0x6b,0x78,0x84,0xc6,0x03,0x6b,0x79,0x84,0xc6,0x04,
+0x6b,0x7a,0x84,0xc6,0x05,0x6b,0x7b,0x84,0xc6,0x41,0xcc,0xcd,0x01,0xa6,0x41,0xcc,
+0xcd,0x4f,0x9b,0x49,0x27,0x01,0xe4,0x72,0x4f,0x01,0x20,0x4c,0x03,0x26,0x4a,0x11,
+0xb6,0x01,0x6b,0x4f,0x01,0x20,0x01,0xa6,0x04,0x26,0x9b,0xfc,0xcd,0xa1,0x3f,0xa2,
+0x3f,0xa3,0x3f,0x77,0xad,0x5b,0x02,0x6b,0xc0,0x84,0xc6,0x03,0x6b,0xc1,0x84,0xc6,
+0x04,0x6b,0xc2,0x84,0xc6,0x05,0x6b,0xc3,0x84,0xc6,0x88,0x88,0x88,0x88,0x88,0x81,
+0xef,0xfc,0xcd,0xb4,0xae,0xa4,0x00,0x84,0x35,0x81,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,
+0xa9,0x41,0x01,0xab,0x81,0x85,0x85,0x85,0x85,0x85,0x05,0x7b,0xe4,0x01,0x01,0x35,
+0x04,0x27,0x02,0xa1,0x11,0xb6,0x0a,0x26,0x4a,0x13,0xb6,0x05,0x6b,0x01,0xa6,0x04,
+0x20,0x12,0x00,0xbf,0x01,0x55,0x09,0x20,0xbd,0xcc,0xcd,0x05,0x27,0x85,0x85,0x4d,
+0xc0,0x1b,0xcd,0x13,0xbe,0x14,0xb6,0x01,0x4b,0x00,0x4b,0x1e,0x20,0x85,0x9b,0x1c,
+0xcd,0x13,0xbe,0x14,0xb6,0x01,0x4b,0xbf,0x01,0xc7,0x15,0xb6,0x21,0x20,0x33,0x27,
+0x4d,0x03,0x1d,0xcd,0x39,0x20,0x9a,0x5e,0xad,0xa2,0x18,0x57,0xad,0x5b,0x9b,0x01,
+0x6b,0xb4,0x84,0xc6,0x02,0x6b,0xb5,0x84,0xc6,0x03,0x6b,0xb6,0x84,0xc6,0x04,0x6b,
+0xb7,0x84,0xc6,0x05,0xc5,0xcd,0xfa,0xa6,0x9a,0x18,0x84,0x00,0x35,0x19,0x84,0x03,
+0x35,0x1a,0x84,0x83,0x35,0x1b,0x84,0x00,0x35,0x14,0x45,0x00,0x35,0x15,0x45,0x00,
+0x35,0x16,0x45,0x00,0x35,0x17,0x45,0x02,0x35,0x07,0xf1,0xcd,0xa2,0x16,0xa3,0x12,
+0xfc,0xf0,0xcd,0x5b,0x9b,0x01,0x6b,0xb4,0x84,0xc6,0x02,0x6b,0xb5,0x84,0xc6,0x03,
+0x6b,0xb6,0x84,0xc6,0x04,0x6b,0xb7,0x84,0xc6,0x84,0x84,0x84,0x84,0x84,0x84,0x84,
+0x84,0x65,0xde,0xcd,0x6c,0x01,0x3b,0x6d,0x01,0x3b,0x6e,0x01,0x3b,0x6f,0x01,0x3b,
+0x04,0x4b,0xc4,0x4b,0xb4,0x4b,0x00,0x4b,0xe1,0xf0,0xcc,0xc5,0xf0,0xcc,0x03,0x26,
+0x4a,0xb4,0xf0,0xcc,0x03,0x26,0x4a,0xac,0xf0,0xcc,0x03,0x26,0x4a,0x15,0x27,0x4a,
+0x12,0xb6,0xe1,0xf0,0xcc,0x03,0x27,0x02,0xa1,0x11,0xb6,0xe5,0xf0,0xcc,0xe8,0x01,
+0xc7,0x4c,0x9a,0xef,0xfc,0xcd,0x18,0xae,0xa4,0x00,0x84,0x35,0xa0,0xb7,0xa1,0xb7,
+0xa2,0xb7,0x4f,0xa3,0xb7,0x12,0xb6,0x9b,0xe5,0xf0,0xcc,0x03,0x27,0x03,0xa1,0x70,
+0x01,0xc6,0x05,0x6b,0x01,0xa6,0xee,0x01,0x5f,0x72,0x70,0x01,0x5f,0x72,0x0c,0x27,
+0x4a,0x0f,0x27,0x4a,0x12,0x27,0x4a,0x15,0x27,0x4a,0xee,0x01,0xc6,0xec,0x01,0x5f,
+0x72,0xed,0x01,0x01,0x35,0x23,0x20,0xec,0x01,0x5f,0x72,0xed,0x01,0xc7,0x02,0xa6,
+0x02,0x20,0x0a,0xa6,0x26,0x20,0x06,0x27,0x4a,0x05,0x27,0x4a,0x0c,0x27,0x4a,0xee,
+0x01,0xc6,0x1d,0x24,0x20,0xa1,0xf3,0x01,0xc6,0xee,0x01,0xc7,0x7f,0xa4,0x12,0xb6,
+0x70,0x01,0x03,0x35,0x79,0x26,0x4a,0x11,0xb6,0xe4,0x01,0x5f,0x72,0x9a,0x18,0x84,
+0x00,0x35,0x19,0x84,0x00,0x35,0x1a,0x84,0x00,0x35,0x1b,0x84,0x00,0x35,0x9b,0x70,
+0x01,0x5f,0x72,0x1a,0x27,0x02,0xa1,0x11,0xb6,0x05,0x6b,0x4f,0x88,0x88,0x88,0x88,
+0x88,0x81,0x4f,0xbd,0xcc,0xcc,0x03,0x27,0x85,0x85,0x4d,0xc0,0x1b,0xcd,0x11,0xbe,
+0x12,0xb6,0x80,0x4b,0x00,0x4b,0x81,0xd4,0x00,0xd7,0x11,0xb6,0x97,0x52,0x21,0xae,
+0x81,0x85,0x85,0x85,0x85,0x85,0x4f,0x84,0x84,0x84,0xd3,0xcc,0xcd,0x00,0xae,0x11,
+0xa6,0x88,0x04,0x7b,0x88,0x04,0x7b,0x88,0x01,0x7b,0xd3,0x00,0x5c,0x72,0x04,0x24,
+0x03,0xa1,0x08,0x26,0xd3,0x00,0xc1,0x04,0x7b,0xe2,0x22,0x05,0xe1,0x72,0x13,0xb6,
+0x05,0x6b,0x4c,0x9f,0x90,0xd7,0x00,0xd7,0x14,0xe6,0x90,0x05,0xe6,0x72,0x97,0x05,
+0xeb,0x72,0x52,0x21,0xae,0x04,0x7b,0x15,0x20,0x4f,0xd6,0x00,0xd7,0x13,0xb6,0xd5,
+0x00,0xd7,0x12,0xb6,0x54,0xad,0x3c,0x24,0x03,0xa1,0x04,0x7b,0xef,0x25,0x03,0xa1,
+0x05,0x6b,0x4c,0x05,0x7b,0x04,0x6b,0x05,0x7b,0x04,0x27,0x6b,0xad,0x05,0x6b,0x4f,
+0x04,0x6b,0xd3,0x00,0xc6,0x03,0x6b,0x10,0xa6,0x02,0x6b,0x02,0xa6,0x63,0x20,0x02,
+0x6b,0x03,0xef,0x72,0x06,0xa9,0x41,0x00,0xab,0x59,0x48,0x59,0x48,0x59,0x48,0x59,
+0x48,0x59,0x48,0x4a,0x5a,0x01,0x26,0x4d,0x5f,0x01,0x6b,0x0f,0xa4,0x11,0xb6,0x22,
+0x25,0x81,0xa1,0x11,0xb6,0x01,0x6b,0x4f,0x88,0x88,0x88,0x88,0x88,0x81,0x85,0x85,
+0x85,0x85,0x4f,0x9e,0x01,0x01,0x35,0x9f,0x01,0xc7,0x4c,0x08,0x20,0x17,0xb7,0x0c,
+0x26,0xa0,0x01,0xc6,0x16,0xb7,0x04,0x7b,0x01,0x6b,0x02,0x6b,0x03,0x6b,0x4f,0x04,
+0x6b,0xa3,0x01,0xc6,0x15,0xb7,0x01,0x7b,0x14,0xb7,0x02,0x7b,0x13,0xb7,0x03,0x7b,
+0x12,0xb7,0x04,0x7b,0x01,0x6b,0xa4,0x01,0xc6,0x02,0x6b,0xa5,0x01,0xc6,0x03,0x6b,
+0xa6,0x01,0xc6,0x04,0x6b,0xa7,0x01,0xc6,0x11,0xb7,0x4f,0x01,0x20,0x4c,0x03,0x26,
+0x9f,0x01,0xce,0x50,0x20,0x9e,0x01,0x5f,0x72,0x9f,0x01,0xc7,0x4c,0x5a,0x20,0x9e,
+0x01,0x5f,0x72,0x9f,0x01,0x5f,0x72,0x5c,0x27,0x4a,0x17,0x27,0x4a,0x10,0x27,0x4a,
+0x11,0xb6,0x9d,0x01,0x08,0x35,0x04,0x27,0x12,0x3d,0x9d,0x01,0x5f,0x72,0x88,0x88,
+0x88,0x88,0x81,0xef,0xfc,0xcd,0xb4,0xae,0xa4,0x00,0x84,0x35,0x81,0x1e,0xfe,0xcd,
+0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x81,0x10,0x4d,0x00,0x35,0x11,0x4d,0x00,0x35,
+0x12,0x4d,0x00,0x35,0x81,0x85,0xcf,0xcd,0x01,0xa6,0x07,0xce,0xcd,0x04,0x48,0x00,
+0x35,0x05,0x48,0x00,0x35,0x06,0x48,0x00,0x35,0x07,0x48,0x00,0x35,0x31,0xad,0xa3,
+0x1a,0x81,0x85,0x85,0x85,0x85,0x4f,0x9a,0xa1,0x01,0x18,0x72,0xaf,0x01,0x1a,0x00,
+0x55,0xb0,0x01,0x1b,0x00,0x55,0xb1,0x01,0x1c,0x00,0x55,0xb2,0x01,0x1d,0x00,0x55,
+0xab,0x01,0x1e,0x00,0x55,0xac,0x01,0x1f,0x00,0x55,0xad,0x01,0x20,0x00,0x55,0xae,
+0x01,0x21,0x00,0x55,0xb7,0x01,0x12,0x00,0x55,0xb8,0x01,0x13,0x00,0x55,0xb9,0x01,
+0x14,0x00,0x55,0xba,0x01,0x15,0x00,0x55,0xb3,0x01,0x16,0x00,0x55,0xb4,0x01,0x17,
+0x00,0x55,0xb5,0x01,0x18,0x00,0x55,0xb6,0x01,0x19,0x00,0x55,0x54,0x26,0x28,0xa1,
+0x11,0xb6,0x5a,0x20,0xa1,0x01,0xc7,0x10,0xa4,0xa1,0x01,0xc6,0x64,0x20,0xa1,0x01,
+0x10,0x72,0x06,0x26,0x27,0xa1,0x11,0xb6,0xd2,0xfc,0xcd,0xa4,0xb7,0x48,0xa9,0x41,
+0x44,0xab,0x52,0x08,0xae,0x11,0xb6,0xae,0xfc,0xcd,0x12,0xae,0xd2,0xfc,0xcd,0xa4,
+0xb7,0x48,0xa9,0x41,0x40,0xab,0x52,0x08,0xae,0x11,0xb6,0xae,0xfc,0xcd,0x16,0xae,
+0x14,0x48,0xaf,0x01,0x55,0x15,0x48,0xb0,0x01,0x55,0x16,0x48,0xb1,0x01,0x55,0x17,
+0x48,0xb2,0x01,0x55,0x10,0x48,0xab,0x01,0x55,0x11,0x48,0xac,0x01,0x55,0x12,0x48,
+0xad,0x01,0x55,0x13,0x48,0xae,0x01,0x55,0xcf,0xed,0xcd,0xa1,0x1b,0xf9,0xed,0xcd,
+0x5b,0x01,0x6b,0xb4,0x84,0xc6,0x02,0x6b,0xb5,0x84,0xc6,0x03,0x6b,0xb6,0x84,0xc6,
+0x04,0x6b,0xb7,0x84,0xc6,0x45,0x26,0x11,0x3d,0x6e,0xed,0xcc,0x03,0x25,0x28,0xa1,
+0x11,0xb6,0xc8,0xed,0xcc,0x04,0xee,0xcd,0xa1,0x1b,0xf9,0xed,0xcd,0x5b,0x05,0xc5,
+0xcd,0x0a,0xa6,0x01,0x6b,0xb4,0x84,0xc6,0x02,0x6b,0xb5,0x84,0xc6,0x03,0x6b,0xb6,
+0x84,0xc6,0x04,0x6b,0xb7,0x84,0xc6,0xa1,0x01,0x10,0x72,0x64,0xed,0xcc,0x03,0x27,
+0x91,0xa1,0x11,0xb6,0xec,0xed,0xcd,0x13,0x4d,0x03,0x35,0xec,0xed,0xcd,0x13,0x4d,
+0x43,0x35,0x0c,0x4d,0x1e,0x00,0x55,0x0d,0x4d,0x1f,0x00,0x55,0x0e,0x4d,0x20,0x00,
+0x55,0x0f,0x4d,0x21,0x00,0x55,0x08,0x4d,0x1a,0x00,0x55,0x09,0x4d,0x1b,0x00,0x55,
+0x0a,0x4d,0x1c,0x00,0x55,0x0b,0x4d,0x1d,0x00,0x55,0x04,0x4d,0x16,0x00,0x55,0x05,
+0x4d,0x17,0x00,0x55,0x06,0x4d,0x18,0x00,0x55,0x07,0x4d,0x19,0x00,0x55,0x00,0x4d,
+0x12,0x00,0x55,0x01,0x4d,0x13,0x00,0x55,0x02,0x4d,0x14,0x00,0x55,0x03,0x4d,0x15,
+0x00,0x55,0xc4,0x27,0x9b,0xfc,0xcd,0xa0,0x3f,0xa1,0xb7,0x06,0xa4,0xa1,0xb6,0xa2,
+0x3f,0xa3,0x3f,0xf9,0xed,0xcd,0x5b,0x01,0x6b,0x14,0x4d,0xc6,0x02,0x6b,0x15,0x4d,
+0xc6,0x03,0x6b,0x16,0x4d,0xc6,0x04,0x6b,0x17,0x4d,0xc6,0x05,0xc5,0xcd,0x0f,0xa6,
+0xec,0xed,0xcd,0x13,0x4d,0x03,0x35,0xec,0xed,0xcd,0x13,0x4d,0xc3,0x35,0x25,0x20,
+0x02,0x6b,0x03,0x6b,0x04,0x6b,0x4f,0xcf,0xed,0xcd,0xa1,0x1a,0xf9,0xed,0xcd,0x5b,
+0x01,0x6b,0xb4,0x84,0xc6,0x02,0x6b,0xb5,0x84,0xc6,0x03,0x6b,0xb6,0x84,0xc6,0x04,
+0x6b,0xb7,0x84,0xc6,0x62,0x26,0x80,0xa1,0x11,0xb6,0xf7,0x24,0x92,0xa1,0x11,0xb6,
+0xde,0xec,0xcc,0x03,0x24,0x80,0xa1,0x11,0xb6,0x9b,0xca,0xed,0xcc,0x01,0xa6,0x05,
+0x24,0x20,0xa1,0xf3,0x01,0xc6,0x0c,0x25,0x80,0xa1,0x11,0xb6,0x88,0x88,0x88,0x88,
+0x81,0x48,0x42,0x00,0x35,0x49,0x42,0x00,0x35,0x4a,0x42,0x00,0x35,0x81,0x85,0x85,
+0x85,0x85,0x4f,0x9a,0x88,0xc9,0xcd,0x72,0x01,0x01,0x35,0x6e,0xc4,0xcd,0x03,0x27,
+0xc2,0x00,0xc6,0x16,0xad,0x4b,0x42,0x05,0x35,0x04,0x20,0x4b,0x42,0x01,0x35,0x06,
+0x27,0x9c,0x01,0xc6,0xf7,0xc7,0xcd,0xef,0xfc,0xcd,0xb4,0xae,0xa4,0x00,0x84,0x35,
+0xa2,0x16,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x5b,0x01,0x6b,0xb4,
+0x84,0xc6,0x02,0x6b,0xb5,0x84,0xc6,0x03,0x6b,0xb6,0x84,0xc6,0x04,0x6b,0xb7,0x84,
+0xc6,0xeb,0xdd,0xcd,0x50,0x20,0x59,0xad,0x4b,0x42,0x05,0x35,0x04,0x20,0x4b,0x42,
+0x01,0x35,0x06,0x27,0x9c,0x01,0xc6,0x13,0x26,0x4a,0x72,0x01,0xc6,0x69,0x20,0xb7,
+0xc8,0xcd,0x97,0xc7,0xcd,0x08,0x26,0x11,0x3d,0x9b,0x9c,0x01,0x01,0x35,0x04,0x20,
+0x9c,0x01,0x5f,0x72,0x06,0x26,0x02,0xa1,0x11,0xb6,0x88,0x88,0x88,0x88,0x81,0xef,
+0xfc,0xcd,0x78,0xae,0xa4,0x00,0x84,0x35,0x81,0x18,0x80,0x00,0x35,0x19,0x80,0x00,
+0x35,0x1a,0x80,0x00,0x35,0x81,0x08,0x80,0x00,0x35,0x09,0x80,0x00,0x35,0x0a,0x80,
+0x00,0x35,0x81,0x04,0x80,0x00,0x35,0x05,0x80,0x00,0x35,0x06,0x80,0x00,0x35,0x81,
+0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x07,0xab,0x81,0x84,0x8b,0x41,0x00,0xa9,
+0x41,0x09,0xab,0x5b,0x0a,0x6b,0x4f,0x9a,0xb7,0xc8,0xcd,0x71,0x01,0x5f,0x72,0xbc,
+0x84,0x00,0x35,0xbd,0x84,0x00,0x35,0xbe,0x84,0x18,0x35,0xbf,0x84,0x00,0x35,0x4a,
+0xad,0x1b,0x80,0x80,0x35,0x14,0x80,0x00,0x35,0x15,0x80,0x00,0x35,0x16,0x80,0x00,
+0x35,0x17,0x80,0x00,0x35,0x53,0xad,0x0b,0x80,0x00,0x35,0x4c,0xad,0x07,0x80,0x00,
+0x35,0x00,0x80,0x00,0x35,0x01,0x80,0x00,0x35,0x02,0x80,0x00,0x35,0x03,0x80,0x00,
+0x35,0x18,0xeb,0xcd,0xa3,0xb7,0xf8,0xa4,0xa3,0xb6,0x60,0xad,0x5b,0x07,0x6b,0x78,
+0x84,0xc6,0x08,0x6b,0x79,0x84,0xc6,0x09,0x6b,0x7a,0x84,0xc6,0x0a,0x6b,0x7b,0x84,
+0xc6,0x69,0x20,0x88,0xc9,0xcd,0x71,0x01,0x01,0x35,0x90,0x84,0xc7,0x03,0x7b,0x91,
+0x84,0xc7,0x04,0x7b,0x92,0x84,0xc7,0x05,0x7b,0x93,0x84,0xc7,0x06,0x7b,0xd2,0xfd,
+0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x03,0xab,0x5b,0xa0,0x1e,0xe6,0xea,0xcd,0x5b,0x05,
+0x6b,0x06,0x6b,0x4f,0xb3,0xfd,0xcd,0x10,0xa6,0xa4,0xb7,0x00,0xa9,0x41,0x03,0xab,
+0x5b,0x03,0x6b,0x94,0x84,0xc6,0x04,0x6b,0x95,0x84,0xc6,0x05,0x6b,0x96,0x84,0xc6,
+0x06,0x6b,0x97,0x84,0xc6,0x73,0xfd,0xcd,0x01,0xa6,0xa4,0xb7,0x00,0xa9,0x41,0x07,
+0xab,0x5b,0x07,0x6b,0x08,0x6b,0x4f,0x07,0x6b,0x98,0x84,0xc6,0x08,0x6b,0x99,0x84,
+0xc6,0x09,0x6b,0x9a,0x84,0xc6,0x0a,0x6b,0x9b,0x84,0xc6,0x71,0x24,0x23,0xa1,0xf3,
+0x01,0xc6,0xc4,0xdc,0xcd,0xbc,0x84,0x00,0x35,0xbd,0x84,0x00,0x35,0xbe,0x84,0x1b,
+0x35,0xbf,0x84,0x40,0x35,0x04,0x20,0xbf,0x84,0x41,0x35,0x06,0x26,0x14,0x3d,0x0b,
+0xeb,0xcd,0x1b,0x80,0x81,0x35,0xef,0xfc,0xcd,0x14,0xae,0xa4,0x00,0x80,0x35,0x7a,
+0xfc,0xcd,0x52,0x80,0xae,0xfe,0xea,0xcd,0x0b,0x80,0x20,0x35,0x04,0x20,0x0b,0x80,
+0x60,0x35,0x06,0x26,0xdb,0x01,0xc7,0x16,0xb6,0xdc,0x01,0x60,0x20,0x55,0xdd,0x01,
+0x61,0x20,0x55,0xde,0x01,0x62,0x20,0x55,0xdf,0x01,0x63,0x20,0x55,0x68,0x20,0x21,
+0x35,0x64,0x20,0xdc,0x01,0x55,0x65,0x20,0xdd,0x01,0x55,0x66,0x20,0xde,0x01,0x55,
+0x67,0x20,0xdf,0x01,0x55,0x9d,0x9d,0x68,0x20,0x27,0x35,0x68,0x20,0x52,0x35,0x64,
+0x20,0x00,0x35,0x65,0x20,0x0f,0x35,0x66,0x20,0x42,0x35,0x67,0x20,0x40,0x35,0x68,
+0x20,0x22,0x35,0x60,0x20,0x6c,0x01,0x55,0x61,0x20,0x6d,0x01,0x55,0x62,0x20,0x6e,
+0x01,0x55,0x63,0x20,0x6f,0x01,0x55,0xdc,0x01,0x94,0xf2,0x55,0xdd,0x01,0x95,0xf2,
+0x55,0xde,0x01,0x96,0xf2,0x55,0xdf,0x01,0x97,0xf2,0x55,0xe0,0x01,0xc7,0xe1,0x01,
+0x21,0x35,0xe2,0x01,0xe6,0x35,0xe3,0x01,0xf0,0x35,0x23,0x20,0xdc,0x01,0x98,0xf2,
+0x55,0xdd,0x01,0x99,0xf2,0x55,0xde,0x01,0x9a,0xf2,0x55,0xdf,0x01,0x9b,0xf2,0x55,
+0xe0,0x01,0xc7,0xe1,0x01,0x21,0x35,0xe2,0x01,0xf0,0x35,0xe3,0x01,0x7c,0x35,0x48,
+0x20,0xdc,0x01,0x9c,0xf2,0x55,0xdd,0x01,0x9d,0xf2,0x55,0xde,0x01,0x9e,0xf2,0x55,
+0xdf,0x01,0x9f,0xf2,0x55,0xe0,0x01,0xc7,0xe1,0x01,0x21,0x35,0xe2,0x01,0xf6,0x35,
+0xe3,0x01,0x94,0x35,0x6d,0x20,0xf1,0xea,0xcd,0x07,0x80,0xe0,0x35,0xdc,0x01,0x90,
+0xf2,0x55,0xdd,0x01,0x91,0xf2,0x55,0xde,0x01,0x92,0xf2,0x55,0xdf,0x01,0x93,0xf2,
+0x55,0xe0,0x01,0x5f,0x72,0xe1,0x01,0x2a,0x35,0xe2,0x01,0x09,0x35,0xe3,0x01,0x8b,
+0x35,0x77,0x27,0x4a,0x55,0x27,0x4a,0x33,0x27,0x4a,0x12,0xb6,0x10,0x80,0x00,0x35,
+0x11,0x80,0x00,0x35,0x12,0x80,0x00,0x35,0x13,0x80,0x08,0x35,0xf1,0xea,0xcd,0x07,
+0x80,0xc0,0x35,0xef,0xfc,0xcd,0x5f,0xa4,0x00,0x80,0x35,0x7a,0xfc,0xcd,0x06,0xaa,
+0x41,0x01,0xea,0x72,0x41,0x02,0xea,0x72,0x5f,0x28,0xaa,0xc0,0xa4,0x52,0x40,0xae,
+0x12,0xb6,0x01,0xef,0x72,0x02,0x6b,0x52,0x10,0xae,0x15,0xb6,0x18,0xeb,0xcd,0xa3,
+0x10,0xa3,0x12,0xa3,0x14,0xe6,0xea,0xcd,0x5b,0x07,0x6b,0x78,0x84,0xc6,0x08,0x6b,
+0x79,0x84,0xc6,0x09,0x6b,0x7a,0x84,0xc6,0x0a,0x6b,0x7b,0x84,0xc6,0xef,0xfc,0xcd,
+0xb4,0xae,0xa4,0x00,0x84,0x35,0xa2,0x16,0xe6,0xea,0xcd,0x5b,0x07,0x6b,0xb4,0x84,
+0xc6,0x08,0x6b,0xb5,0x84,0xc6,0x09,0x6b,0xb6,0x84,0xc6,0x0a,0x6b,0xb7,0x84,0xc6,
+0x6f,0xea,0xcc,0x03,0x27,0x4a,0x13,0xb6,0x9b,0xda,0xea,0xcc,0x01,0xa6,0x05,0x27,
+0x9b,0xfc,0xcd,0xa0,0x3f,0xa1,0x3f,0xa2,0xb7,0x40,0xa4,0xa2,0xb6,0xa3,0x3f,0xe6,
+0xea,0xcd,0x5b,0x07,0x6b,0x94,0x84,0xc6,0x08,0x6b,0x95,0x84,0xc6,0x09,0x6b,0x96,
+0x84,0xc6,0x0a,0x6b,0x97,0x84,0xc6,0x8b,0x41,0x00,0xa2,0x41,0x0a,0xa0,0x5b,0x81,
+0x85,0x4f,0x87,0x01,0x5a,0x72,0x9a,0x0c,0x8c,0x00,0x35,0x0d,0x8c,0x00,0x35,0x0e,
+0x8c,0x00,0x35,0x0f,0x8c,0x01,0x35,0x04,0x20,0x0f,0x8c,0x03,0x35,0x06,0x26,0x4a,
+0x87,0x01,0xc6,0xef,0xfc,0xcd,0x10,0xae,0xa4,0x00,0x8c,0x35,0xa0,0xb7,0xa1,0xb7,
+0xa2,0xb7,0x4f,0xa3,0x00,0x88,0x01,0x55,0xef,0xfc,0xcd,0x1c,0xae,0xa4,0x00,0x8c,
+0x35,0x7a,0xfc,0xcd,0x5f,0x0f,0xa4,0x4e,0x88,0x01,0xc6,0x9b,0x06,0x20,0xc7,0xfa,
+0xa4,0x06,0x20,0xc6,0x52,0x27,0xe6,0x25,0x87,0x01,0xc6,0x87,0x01,0xc1,0x01,0x6b,
+0x4c,0x9f,0x90,0x88,0x01,0xd7,0x12,0xe6,0x90,0x01,0xe6,0x72,0x97,0x01,0xe0,0x72,
+0x10,0x20,0x01,0xa6,0x88,0x01,0xc7,0x12,0xb6,0x87,0x01,0xc7,0x11,0xb6,0x88,0x81,
+0x85,0x4f,0x74,0x01,0x5f,0x72,0xef,0x25,0x75,0x01,0xc1,0x01,0x6b,0x4c,0x9f,0x14,
+0xe7,0x76,0x01,0xd6,0x01,0xee,0x72,0x0a,0x20,0x4f,0x13,0x00,0x75,0x01,0x55,0x88,
+0x81,0x4f,0x9a,0xa8,0x84,0x29,0x00,0x55,0xa9,0x84,0x2a,0x00,0x55,0xaa,0x84,0x2b,
+0x00,0x55,0xab,0x84,0x2c,0x00,0x55,0xef,0xfc,0xcd,0x48,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x27,0xbe,0x28,0xb6,0xef,0xfc,0xcd,0x44,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x25,0xbe,0x26,0xb6,0xef,0xfc,0xcd,0x40,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x23,0xbe,0x24,0xb6,0xef,0xfc,0xcd,0x3c,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x21,0xbe,0x22,0xb6,0xef,0xfc,0xcd,0x38,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x1f,0xbe,0x20,0xb6,0xef,0xfc,0xcd,0x34,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x1d,0xbe,0x1e,0xb6,0xef,0xfc,0xcd,0x30,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x1b,0xbe,0x1c,0xb6,0xef,0xfc,0xcd,0x2c,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x19,0xbe,0x1a,0xb6,0xef,0xfc,0xcd,0x28,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x17,0xbe,0x18,0xb6,0xef,0xfc,0xcd,0x24,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x15,0xbe,0x16,0xb6,0xef,0xfc,0xcd,0x20,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x13,0xbe,0x14,0xb6,0xef,0xfc,0xcd,0x1c,0xae,0xa4,0x00,0x84,0x35,
+0x7a,0xfc,0xcd,0x11,0xbe,0x12,0xb6,0x9b,0x81,0x4f,0xe9,0x01,0x01,0x35,0x45,0x01,
+0xcf,0x1f,0xbe,0x46,0x01,0xc7,0x20,0xb6,0x43,0x01,0xcf,0x1d,0xbe,0x44,0x01,0xc7,
+0x1e,0xb6,0x41,0x01,0xcf,0x1b,0xbe,0x42,0x01,0xc7,0x1c,0xb6,0x3f,0x01,0xcf,0x19,
+0xbe,0x40,0x01,0xc7,0x1a,0xb6,0x3d,0x01,0xcf,0x17,0xbe,0x3e,0x01,0xc7,0x18,0xb6,
+0x3b,0x01,0xcf,0x15,0xbe,0x3c,0x01,0xc7,0x16,0xb6,0x39,0x01,0xcf,0x13,0xbe,0x3a,
+0x01,0xc7,0x14,0xb6,0x37,0x01,0xcf,0x11,0xbe,0x38,0x01,0xc7,0x12,0xb6,0x81,0x4f,
+0x9a,0x30,0x42,0x00,0x35,0x31,0x42,0x00,0x35,0x32,0x42,0x00,0x35,0x33,0x42,0x00,
+0x35,0xc6,0xd8,0xcd,0x22,0xda,0xcd,0x84,0x84,0x84,0x84,0x84,0x84,0x84,0x84,0x65,
+0xde,0xcd,0x6c,0x01,0x3b,0x6d,0x01,0x3b,0x6e,0x01,0x3b,0x6f,0x01,0x3b,0x5e,0x01,
+0x3b,0x5f,0x01,0x3b,0x60,0x01,0x3b,0x61,0x01,0x3b,0xf5,0xd5,0xcd,0x9b,0x81,0x85,
+0x85,0x85,0x85,0x85,0x4f,0xc2,0x00,0x01,0x35,0x9a,0x84,0x84,0xc7,0x02,0x7b,0x85,
+0x84,0xc7,0x03,0x7b,0x86,0x84,0xc7,0x04,0x7b,0x87,0x84,0xc7,0x9b,0x05,0x6b,0xfb,
+0xa4,0x05,0x7b,0x04,0x20,0x04,0xaa,0x05,0x7b,0x06,0x27,0x01,0x7b,0x02,0x6b,0x84,
+0x84,0xc6,0x03,0x6b,0x85,0x84,0xc6,0x04,0x6b,0x86,0x84,0xc6,0x05,0x6b,0x87,0x84,
+0xc6,0x9a,0x6e,0xc4,0xcd,0x9b,0x05,0x27,0x72,0x01,0xc6,0x00,0xc0,0xcd,0x5a,0xc4,
+0xcd,0xbf,0x00,0xc7,0x12,0xb6,0x02,0x20,0x04,0xa6,0x04,0x26,0x02,0xa1,0x11,0xb6,
+0x9a,0xef,0xfc,0xcd,0xa4,0xbf,0x84,0xae,0xa3,0x1b,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,
+0xa9,0x41,0x02,0xab,0x5b,0x9b,0x05,0x6b,0x04,0xaa,0x05,0x7b,0x02,0x6b,0x84,0x84,
+0xc6,0x03,0x6b,0x85,0x84,0xc6,0x04,0x6b,0x86,0x84,0xc6,0x05,0x6b,0x87,0x84,0xc6,
+0x16,0x00,0x01,0x35,0x01,0x6b,0x16,0xb6,0xc1,0x00,0x5f,0x72,0x04,0x20,0xc1,0x00,
+0x01,0x35,0x06,0x27,0x17,0x3d,0x88,0x88,0x88,0x88,0x88,0x81,0x85,0x85,0x85,0x85,
+0x85,0x01,0x7b,0xe8,0x01,0x01,0x35,0x9a,0xef,0xfc,0xcd,0xb4,0xae,0xa4,0x00,0x84,
+0x35,0xa3,0x1f,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x02,0xab,0x5b,0xb4,0x84,
+0xc7,0x02,0x7b,0xb5,0x84,0xc7,0x03,0x7b,0xb6,0x84,0xc7,0x04,0x7b,0xb7,0x84,0xc7,
+0x05,0x7b,0xd2,0xfd,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x02,0xab,0x5b,0xa3,0x1e,0xa0,
+0x3f,0xa1,0x3f,0xa2,0xb7,0x03,0xa4,0xa2,0xb6,0xa3,0x3f,0x17,0xfd,0xcd,0x5f,0x90,
+0xa7,0x00,0x01,0x35,0x5f,0x12,0xb6,0x04,0x6b,0xfc,0xa4,0x04,0x7b,0x02,0x6b,0xb4,
+0x84,0xc6,0x03,0x6b,0xb5,0x84,0xc6,0x04,0x6b,0xb6,0x84,0xc6,0x05,0x6b,0xb7,0x84,
+0xc6,0xcc,0x84,0x00,0x35,0xcd,0x84,0x00,0x35,0xce,0x84,0x4a,0x35,0xcf,0x84,0x00,
+0x35,0x10,0x25,0x20,0xa1,0xf3,0x01,0xc6,0x17,0x26,0x03,0xa1,0x6b,0x01,0xc6,0x40,
+0xc6,0xcd,0x1c,0xbe,0x20,0xa6,0x9b,0x6c,0x01,0x22,0x00,0x55,0x6d,0x01,0x23,0x00,
+0x55,0x6e,0x01,0x24,0x00,0x55,0x6f,0x01,0x25,0x00,0x55,0x14,0x27,0xc0,0xfc,0xcd,
+0x22,0xae,0x6b,0x01,0xc7,0x12,0xb6,0xbb,0x00,0xc7,0x21,0xb6,0xbc,0x00,0xcf,0x1f,
+0xbe,0xbd,0x00,0xc7,0x20,0xb6,0x65,0x01,0xcf,0x19,0xbe,0x66,0x01,0xc7,0x1a,0xb6,
+0x69,0x01,0xcf,0x17,0xbe,0x6a,0x01,0xc7,0x18,0xb6,0x63,0x01,0xcf,0x15,0xbe,0x64,
+0x01,0xc7,0x16,0xb6,0x67,0x01,0xcf,0x13,0xbe,0x68,0x01,0xc7,0x14,0xb6,0x1d,0xe5,
+0xcc,0xe5,0x01,0xc7,0x4c,0x1d,0xe5,0xcc,0x01,0x6b,0x01,0xa6,0x45,0x27,0x4a,0x11,
+0x27,0x4a,0x0d,0x27,0x4d,0xbb,0x00,0x5f,0x72,0xbd,0x00,0x5f,0x72,0xbc,0x00,0x5f,
+0x72,0x70,0x01,0xc7,0x11,0xb6,0x01,0x6b,0x4f,0x88,0x88,0x88,0x88,0x88,0x00,0x00,
+0xba,0x42,0x00,0x00,0xb8,0x41,0x0a,0xd7,0x23,0x3c,0x00,0x00,0x80,0x3f,0x81,0x1e,
+0xfe,0xcd,0xb3,0xae,0xa4,0x00,0x00,0x35,0x81,0xa4,0xb7,0x00,0xa9,0x41,0x17,0xab,
+0x81,0xa4,0x00,0x80,0x35,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,0xb7,0x81,0x8b,
+0x41,0x00,0xa9,0x41,0x1c,0xab,0x5b,0x20,0x80,0x00,0x35,0x21,0x80,0x00,0x35,0x22,
+0x80,0x00,0x35,0x23,0x80,0x40,0x35,0xef,0xfc,0xcd,0x30,0xae,0x1e,0xad,0x16,0x7b,
+0xef,0xfc,0xcd,0x2c,0xae,0x27,0xad,0x15,0x7b,0xef,0xfc,0xcd,0x28,0xae,0x30,0xad,
+0x14,0x7b,0x8a,0xfd,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x13,0xab,0x5b,0x1e,0xfe,0xcd,
+0xe0,0xae,0xa4,0x00,0x01,0x35,0x13,0x6b,0x60,0x20,0xc6,0x14,0x6b,0x61,0x20,0xc6,
+0x15,0x6b,0x62,0x20,0xc6,0x16,0x6b,0x63,0x20,0xc6,0x68,0x20,0x26,0x35,0x68,0x20,
+0x21,0x35,0x68,0x20,0x52,0x35,0x64,0x20,0xc7,0x0d,0x7b,0x65,0x20,0xc7,0x0e,0x7b,
+0x66,0x20,0xc7,0x0f,0x7b,0x67,0x20,0xc7,0x10,0x7b,0x60,0x20,0xdc,0x01,0x55,0x61,
+0x20,0xdd,0x01,0x55,0x62,0x20,0xde,0x01,0x55,0x63,0x20,0xdf,0x01,0x55,0xef,0xfc,
+0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x0d,0xab,0x5b,0xfb,0xfd,0xcd,0xe0,0xe3,0xcd,0x5b,
+0xe8,0xe3,0xcd,0xf0,0x27,0x1b,0xea,0x72,0x1c,0x7b,0xf7,0x26,0x4a,0xdb,0x01,0xc6,
+0xc9,0xe3,0xcc,0x03,0x27,0x4a,0x71,0x01,0xc6,0x84,0x84,0x84,0x84,0x8e,0xc5,0xcd,
+0x88,0x1a,0x7b,0x88,0x1a,0x7b,0x88,0x1a,0x7b,0x88,0x1a,0x7b,0x0f,0x20,0x69,0xc5,
+0xcd,0x88,0x1a,0x7b,0x88,0x1a,0x7b,0x88,0x1a,0x7b,0x88,0x1a,0x7b,0x11,0x27,0xf0,
+0x01,0xc6,0xf2,0x01,0x5f,0x72,0x04,0x20,0x8a,0xfd,0xcd,0xe0,0xe3,0xcd,0x5b,0xe8,
+0xe3,0xcd,0x0c,0x2b,0xff,0xa2,0x11,0x7b,0xfd,0xa0,0x12,0x7b,0x0a,0x2b,0x00,0xa2,
+0x11,0x7b,0x04,0xa0,0x12,0x6b,0x18,0x7b,0x11,0x6b,0x17,0x7b,0x17,0x6b,0x60,0x20,
+0xc6,0x18,0x6b,0x61,0x20,0xc6,0x19,0x6b,0x62,0x20,0xc6,0x1a,0x6b,0x63,0x20,0xc6,
+0x68,0x20,0x26,0x35,0x68,0x20,0x24,0x35,0x64,0x20,0xab,0x00,0x55,0x65,0x20,0xac,
+0x00,0x55,0x66,0x20,0xad,0x00,0x55,0x67,0x20,0xae,0x00,0x55,0x68,0x20,0x21,0x35,
+0x64,0x20,0xc7,0x05,0x7b,0x65,0x20,0xc7,0x06,0x7b,0x66,0x20,0xc7,0x07,0x7b,0x67,
+0x20,0xc7,0x08,0x7b,0x60,0x20,0xaf,0x00,0x55,0x61,0x20,0xb0,0x00,0x55,0x62,0x20,
+0xb1,0x00,0x55,0x63,0x20,0xb2,0x00,0x55,0xab,0x00,0x60,0x20,0x55,0xac,0x00,0x61,
+0x20,0x55,0xad,0x00,0x62,0x20,0x55,0xae,0x00,0x63,0x20,0x55,0x68,0x20,0x21,0x35,
+0x64,0x20,0xc7,0x01,0x7b,0x65,0x20,0xc7,0x02,0x7b,0x66,0x20,0xc7,0x03,0x7b,0x67,
+0x20,0xc7,0x04,0x7b,0xaf,0x00,0x64,0x20,0x55,0xb0,0x00,0x65,0x20,0x55,0xb1,0x00,
+0x66,0x20,0x55,0xb2,0x00,0x67,0x20,0x55,0x68,0x20,0x64,0x35,0x64,0x20,0xaf,0x00,
+0x55,0x65,0x20,0xb0,0x00,0x55,0x66,0x20,0xb1,0x00,0x55,0x67,0x20,0xb2,0x00,0x55,
+0x68,0x20,0x21,0x35,0x64,0x20,0xc7,0x09,0x7b,0x65,0x20,0xc7,0x0a,0x7b,0x66,0x20,
+0xc7,0x0b,0x7b,0x67,0x20,0xc7,0x0c,0x7b,0x68,0x20,0x22,0x35,0xef,0xfc,0xcd,0x60,
+0xae,0xa4,0x00,0x20,0x35,0x7a,0xfc,0xcd,0x1b,0xee,0x72,0x1c,0x7b,0xee,0xe2,0xcc,
+0x03,0x26,0x1b,0xea,0x72,0x1c,0x7b,0xef,0xfc,0xcd,0xe0,0xe3,0xcd,0x5b,0xa0,0x3f,
+0x1e,0xfe,0xcd,0x5f,0xa4,0x00,0x84,0x35,0xef,0xfc,0xcd,0xa4,0xb7,0x00,0xa9,0x41,
+0x09,0xab,0x5b,0x1e,0xfe,0xcd,0xf2,0xae,0xa4,0x00,0xe3,0x35,0x06,0x20,0xf6,0xae,
+0xa4,0x00,0xe3,0x35,0x08,0x26,0x4c,0x86,0x01,0xc6,0x05,0x6b,0xfa,0xe3,0xc6,0x06,
+0x6b,0xfb,0xe3,0xc6,0x07,0x6b,0xfc,0xe3,0xc6,0x08,0x6b,0xfd,0xe3,0xc6,0x01,0x6b,
+0xfe,0xe3,0xc6,0x02,0x6b,0xff,0xe3,0xc6,0x03,0x6b,0x00,0xe4,0xc6,0x04,0x6b,0x01,
+0xe4,0xc6,0x8b,0x41,0x00,0xa2,0x41,0x1a,0xa0,0x5b,0x89,0x88,0x81,0xa0,0xb7,0xa1,
+0xb7,0xa2,0xb7,0x4f,0xa3,0xb7,0x81,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0xd9,0xe0,
+0xcc,0x03,0xa6,0xd9,0xe0,0xcc,0x02,0xa6,0x81,0x20,0x4c,0x81,0x84,0x84,0x84,0x84,
+0xef,0xfc,0xcd,0x10,0xae,0xa4,0x00,0x84,0x35,0xa1,0x1e,0x1e,0xfe,0xcd,0x20,0xad,
+0x5b,0x10,0x84,0xc7,0x01,0x7b,0x11,0x84,0xc7,0x02,0x7b,0x12,0x84,0xc7,0x03,0x7b,
+0x13,0x84,0xc7,0x04,0x7b,0x02,0x6b,0x7f,0xa4,0x02,0x7b,0xef,0xfc,0xcd,0x40,0xad,
+0x5b,0xd8,0xfd,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x07,0xab,0x5b,0xa0,0xb7,0x03,0xa4,
+0xa0,0xb6,0xa1,0x3f,0xa2,0x3f,0xa3,0x3f,0x1e,0xfe,0xcd,0x5d,0xad,0x5b,0x01,0x6b,
+0x10,0x84,0xc6,0x02,0x6b,0x11,0x84,0xc6,0x03,0x6b,0x12,0x84,0xc6,0x04,0x6b,0x13,
+0x84,0xc6,0xd2,0xfd,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x07,0xab,0x5b,0x8c,0xfc,0xcd,
+0x14,0xa6,0x6a,0xe1,0xcd,0x0b,0x6b,0x04,0xa6,0x5d,0xe1,0xcc,0x03,0x26,0x08,0xa0,
+0x58,0xe1,0xcc,0x03,0x26,0x04,0xa0,0x55,0xe1,0xcc,0x03,0x26,0x02,0xa0,0x17,0x27,
+0x02,0xa0,0x1b,0x20,0x4c,0x1e,0x20,0x06,0xa6,0x22,0x20,0x02,0xa6,0x26,0x20,0x07,
+0xa6,0x0c,0x27,0x10,0xa0,0x0c,0x27,0x08,0xa0,0x0c,0x27,0x04,0xa0,0x34,0x27,0x02,
+0xa0,0x3a,0x27,0x02,0xa0,0x23,0x24,0x0b,0x7b,0x20,0xa1,0xf3,0x01,0xc6,0x07,0x6b,
+0x60,0x20,0xc6,0x08,0x6b,0x61,0x20,0xc6,0x09,0x6b,0x62,0x20,0xc6,0x0a,0x6b,0x63,
+0x20,0xc6,0x68,0x20,0x26,0x35,0x68,0x20,0x21,0x35,0x68,0x20,0x52,0x35,0x64,0x20,
+0x00,0x35,0x65,0x20,0x00,0x35,0x66,0x20,0x80,0x35,0x67,0x20,0x00,0x35,0x9d,0x9d,
+0x68,0x20,0x27,0x35,0x60,0x20,0xb7,0x00,0x55,0x61,0x20,0xb8,0x00,0x55,0x62,0x20,
+0xb9,0x00,0x55,0x63,0x20,0xba,0x00,0x55,0x68,0x20,0x61,0x35,0x68,0x20,0x52,0x35,
+0xef,0xfc,0xcd,0x64,0xae,0xa4,0x00,0x20,0x35,0x6a,0xe1,0xcd,0x0b,0x7b,0x68,0x20,
+0x22,0x35,0x60,0x20,0xc7,0x07,0x7b,0x61,0x20,0xc7,0x08,0x7b,0x62,0x20,0xc7,0x09,
+0x7b,0x63,0x20,0xc7,0x0a,0x7b,0x88,0x88,0x88,0x88,0x81,0x64,0x20,0x00,0x35,0x65,
+0x20,0x00,0x35,0x66,0x20,0x00,0x35,0x81,0x84,0xef,0xfc,0xcd,0x10,0xae,0xa4,0x00,
+0x84,0x35,0xa2,0xb7,0x55,0xaa,0xa2,0xb6,0xa3,0xb7,0x55,0xaa,0xa3,0xb6,0xa1,0xb7,
+0x55,0xaa,0xa1,0xb6,0x8c,0xfc,0xcd,0x18,0xa6,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,
+0xa3,0xb7,0x01,0x7b,0xef,0xfc,0xcd,0x14,0xae,0xa4,0x00,0x84,0x35,0xa0,0xb7,0x3f,
+0xa4,0xa0,0xb6,0xa1,0x3f,0xa2,0x3f,0xa3,0x3f,0x8c,0xfc,0xcd,0x18,0xa6,0x1e,0xfe,
+0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x04,0xab,0x5b,0x25,0x24,0x0f,0xa1,0x07,0x7b,0x05,
+0xc5,0xcd,0x64,0xa6,0x84,0x84,0x84,0x84,0x69,0xc5,0xcd,0xb3,0x00,0x3b,0xb4,0x00,
+0x3b,0xb5,0x00,0x3b,0xb6,0x00,0x3b,0x0f,0x20,0x8e,0xc5,0xcd,0xb3,0x00,0x3b,0xb4,
+0x00,0x3b,0xb5,0x00,0x3b,0xb6,0x00,0x3b,0x11,0x26,0x4a,0x70,0x01,0xc6,0x14,0x84,
+0x0f,0x35,0x15,0x84,0x00,0x35,0x16,0x84,0x00,0x35,0x17,0x84,0x00,0x35,0x04,0x6b,
+0x60,0x20,0xc6,0x05,0x6b,0x61,0x20,0xc6,0x06,0x6b,0x62,0x20,0xc6,0x07,0x6b,0x63,
+0x20,0xc6,0x68,0x20,0xc7,0x4a,0x9d,0x9d,0x68,0x20,0xc7,0x27,0xa6,0x68,0x20,0x52,
+0x35,0x64,0x20,0x05,0x35,0x65,0x20,0xf5,0x35,0x66,0x20,0xe1,0x35,0x67,0x20,0x00,
+0x35,0x60,0x20,0xb7,0x00,0x55,0x61,0x20,0xb8,0x00,0x55,0x62,0x20,0xb9,0x00,0x55,
+0x63,0x20,0xba,0x00,0x55,0xb3,0x00,0x60,0x20,0x55,0xb4,0x00,0x61,0x20,0x55,0xb5,
+0x00,0x62,0x20,0x55,0xb6,0x00,0x63,0x20,0x55,0x68,0x20,0x26,0x35,0x68,0x20,0x21,
+0x35,0x68,0x20,0x52,0x35,0x64,0x20,0x00,0x35,0x65,0x20,0x00,0x35,0x66,0x20,0x80,
+0x35,0x67,0x20,0x00,0x35,0x68,0x20,0x27,0x35,0xb7,0x00,0x60,0x20,0x55,0xb8,0x00,
+0x61,0x20,0x55,0xb9,0x00,0x62,0x20,0x55,0xba,0x00,0x63,0x20,0x55,0x68,0x20,0x52,
+0x35,0x64,0x20,0xc7,0x04,0x7b,0x65,0x20,0xc7,0x05,0x7b,0x66,0x20,0xc7,0x06,0x7b,
+0x67,0x20,0xc7,0x07,0x7b,0x68,0x20,0xc7,0x4a,0x68,0x20,0xc7,0x22,0xa6,0x60,0x20,
+0xc7,0x08,0x7b,0x61,0x20,0xc7,0x09,0x7b,0x62,0x20,0xc7,0x0a,0x7b,0x63,0x20,0xc7,
+0x0b,0x7b,0x68,0x20,0x52,0x35,0x10,0x84,0x02,0x35,0x11,0x84,0x55,0x35,0x12,0x84,
+0x55,0x35,0x13,0x84,0x55,0x35,0x01,0x6b,0x02,0xa6,0x09,0xe0,0xcd,0x67,0x20,0x28,
+0x35,0x09,0x20,0x01,0xa6,0x09,0xe0,0xcd,0x67,0x20,0x14,0x35,0x0b,0x25,0x02,0xa2,
+0x08,0x7b,0xdd,0xa0,0x09,0x7b,0x1e,0x20,0x4f,0x09,0xe0,0xcd,0x67,0x20,0x0a,0x35,
+0x0a,0x25,0x04,0xa2,0x08,0x7b,0xd9,0xa0,0x09,0x7b,0x88,0x81,0x52,0x01,0xce,0x53,
+0x01,0xc6,0xef,0xfc,0xcd,0x81,0x84,0x84,0xef,0xfc,0xcd,0x0c,0xae,0xa4,0x00,0x41,
+0x35,0x0e,0xfd,0xcd,0x41,0x4a,0x01,0xc9,0x41,0x4b,0x01,0xcb,0x17,0xad,0x08,0xae,
+0xa4,0x00,0x41,0x35,0x0e,0xfd,0xcd,0x5c,0x01,0x26,0x4c,0x26,0xad,0x04,0xae,0xa4,
+0x00,0x41,0x35,0x0e,0xfd,0xcd,0x41,0x48,0x01,0xc9,0x41,0x49,0x01,0xcb,0x41,0x01,
+0xe2,0x72,0x41,0x02,0xe0,0x72,0x50,0x01,0xce,0x51,0x01,0xc6,0xef,0xfc,0xcd,0x5f,
+0xa4,0x00,0x41,0x35,0x0e,0xfd,0xcd,0x5c,0x01,0x26,0x4c,0x41,0x01,0xe2,0x72,0x41,
+0x02,0xe0,0x72,0x50,0x01,0xce,0x51,0x01,0xc6,0x02,0x6b,0x55,0x01,0xc6,0x01,0x6b,
+0x54,0x01,0xc6,0x88,0x88,0x81,0x58,0x01,0xce,0x59,0x01,0xc6,0xef,0xfc,0xcd,0x81,
+0x41,0x54,0x01,0xc9,0x41,0x55,0x01,0xcb,0x46,0x54,0x4c,0x01,0xce,0x4d,0x01,0xc6,
+0x81,0x54,0x01,0xce,0x55,0x01,0xc6,0xef,0xfc,0xcd,0x81,0x8c,0xfc,0xcd,0x10,0xa6,
+0x0e,0xfd,0xcd,0x81,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x81,0x84,0x84,0x84,0x84,
+0x84,0x84,0x7c,0x83,0x00,0x35,0x7d,0x83,0x00,0x35,0x7e,0x83,0x0f,0x35,0x7f,0x83,
+0xff,0x35,0xef,0xfc,0xcd,0x6c,0xae,0xa4,0x00,0x83,0x35,0xd8,0xfd,0xcd,0x23,0xad,
+0x5b,0x2e,0xad,0x43,0xad,0xef,0xfc,0xcd,0x2d,0xad,0x5b,0x0e,0xfd,0xcd,0x4e,0xad,
+0x0d,0x20,0x48,0xad,0x39,0xad,0x5b,0x0e,0xfd,0xcd,0x54,0x01,0xce,0x55,0x01,0xc6,
+0x10,0x26,0x4a,0x47,0x01,0xc6,0xef,0xfc,0xcd,0x68,0xae,0xa4,0x00,0x83,0x35,0xd8,
+0xfd,0xcd,0x57,0xad,0x5b,0x62,0xad,0x6d,0xad,0x5e,0xad,0x5b,0x0e,0xfd,0xcd,0x75,
+0xad,0x64,0xae,0xa4,0x00,0x83,0x35,0xd8,0xfd,0xcd,0x6f,0xad,0x5b,0x7a,0xad,0x41,
+0x5a,0x01,0xc9,0x41,0x5b,0x01,0xcb,0x05,0xee,0x72,0x06,0x7b,0xef,0xfc,0xcd,0xb5,
+0xdd,0xcd,0x5b,0x0e,0xfd,0xcd,0x05,0xee,0x72,0x06,0x7b,0xef,0xfc,0xcd,0x60,0xae,
+0xa4,0x00,0x83,0x35,0xd8,0xfd,0xcd,0xb5,0xdd,0xcd,0x5b,0xbd,0xdd,0xcd,0x41,0x5a,
+0x01,0xc9,0x41,0x5b,0x01,0xcb,0xe1,0xdd,0xcd,0xb5,0xdd,0xcd,0x5b,0x0e,0xfd,0xcd,
+0xe1,0xdd,0xcd,0x5c,0xae,0xa4,0x00,0x83,0x35,0x0e,0xfd,0xcd,0x46,0x54,0x4c,0x01,
+0xce,0x4d,0x01,0xc6,0x05,0xef,0x72,0x06,0x6b,0x4a,0x5a,0x01,0x26,0x4d,0x58,0x01,
+0xce,0x59,0x01,0xc6,0x10,0x20,0x06,0x6b,0x59,0x01,0xc6,0x05,0x6b,0x58,0x01,0xc6,
+0x0c,0x26,0x4a,0x47,0x01,0xc6,0x88,0x88,0x88,0x88,0x88,0x88,0x81,0x50,0x01,0xce,
+0x51,0x01,0xc6,0xef,0xfc,0xcd,0x81,0x58,0x01,0xce,0x59,0x01,0xc6,0xef,0xfc,0xcd,
+0x81,0x4d,0x41,0x4a,0x01,0xc9,0x41,0x4b,0x01,0xcb,0x81,0x41,0x54,0x01,0xc9,0x41,
+0x55,0x01,0xcb,0x46,0x54,0x4c,0x01,0xce,0x4d,0x01,0xc6,0x81,0x0e,0xfd,0xcd,0x41,
+0xea,0x01,0xc2,0x41,0xeb,0x01,0xc0,0x31,0xad,0x81,0x4d,0x41,0xea,0x01,0xc2,0x41,
+0xeb,0x01,0xc0,0x41,0x48,0x01,0xc9,0x41,0x49,0x01,0xcb,0x45,0xad,0x81,0x54,0x01,
+0xce,0x55,0x01,0xc6,0xef,0xfc,0xcd,0x81,0x8c,0xfc,0xcd,0x10,0xa6,0x0e,0xfd,0xcd,
+0x81,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x81,0x8b,0x41,0x00,0xa9,0x41,0x0e,0xab,
+0x5b,0x98,0x83,0x00,0x35,0x99,0x83,0x00,0x35,0x9a,0x83,0x00,0x35,0x9b,0x83,0x18,
+0x35,0xef,0xfc,0xcd,0x3c,0xae,0xa4,0x00,0x83,0x35,0xd8,0xfd,0xcd,0x25,0xad,0x5b,
+0x30,0xad,0x67,0xad,0xef,0xfc,0xcd,0x2f,0xad,0x5b,0x0e,0xfd,0xcd,0x72,0xad,0x0d,
+0x20,0x4a,0xad,0x3b,0xad,0x5b,0x0e,0xfd,0xcd,0x54,0x01,0xce,0x55,0x01,0xc6,0x10,
+0x26,0x4a,0x47,0x01,0xc6,0xef,0xfc,0xcd,0x38,0xae,0xa4,0x00,0x83,0x35,0xd8,0xfd,
+0xcd,0x59,0xad,0x5b,0x64,0xad,0x6f,0xad,0x60,0xad,0x5b,0x0e,0xfd,0xcd,0x77,0xad,
+0x34,0xae,0xa4,0x00,0x83,0x35,0xd8,0xfd,0xcd,0x71,0xad,0x5b,0x7c,0xad,0x41,0x5a,
+0x01,0xc9,0x41,0x5b,0x01,0xcb,0x07,0xee,0x72,0x08,0x7b,0xef,0xfc,0xcd,0x58,0xdc,
+0xcd,0x5b,0x0e,0xfd,0xcd,0x07,0xee,0x72,0x08,0x7b,0xef,0xfc,0xcd,0x30,0xae,0xa4,
+0x00,0x83,0x35,0xd8,0xfd,0xcd,0x58,0xdc,0xcd,0x5b,0x60,0xdc,0xcd,0x41,0x5a,0x01,
+0xc9,0x41,0x5b,0x01,0xcb,0xb0,0xdc,0xcd,0x58,0xdc,0xcd,0x5b,0x0e,0xfd,0xcd,0xb0,
+0xdc,0xcd,0x2c,0xae,0xa4,0x00,0x83,0x35,0xd8,0xfd,0xcd,0x58,0xdc,0xcd,0x5b,0x60,
+0xdc,0xcd,0x41,0x56,0x01,0xc9,0x41,0x57,0x01,0xcb,0x69,0xdc,0xcd,0x58,0xdc,0xcd,
+0x5b,0x0e,0xfd,0xcd,0x69,0xdc,0xcd,0x10,0xae,0xa4,0x00,0x83,0x35,0xd8,0xfd,0xcd,
+0x58,0xdc,0xcd,0x5b,0x60,0xdc,0xcd,0x4a,0x5a,0x01,0x26,0xa6,0xdc,0xcd,0x0d,0xee,
+0x72,0x0e,0x7b,0xef,0xfc,0xcd,0x58,0xdc,0xcd,0x5b,0x0e,0xfd,0xcd,0x4a,0x5a,0x01,
+0x26,0x73,0xdc,0xcd,0x0c,0xae,0xa4,0x00,0x83,0x35,0xd8,0xfd,0xcd,0x58,0xdc,0xcd,
+0x5b,0x60,0xdc,0xcd,0x0d,0xee,0x72,0x0e,0x7b,0xef,0xfc,0xcd,0x58,0xdc,0xcd,0x5b,
+0x87,0xdc,0xcd,0x08,0xae,0xa4,0x00,0x83,0x35,0xd8,0xfd,0xcd,0x58,0xdc,0xcd,0x5b,
+0x60,0xdc,0xcd,0x4a,0x5a,0x01,0x26,0xa6,0xdc,0xcd,0x0d,0xee,0x72,0x0e,0x7b,0xef,
+0xfc,0xcd,0x58,0xdc,0xcd,0x5b,0x0e,0xfd,0xcd,0x4a,0x5a,0x01,0x26,0x73,0xdc,0xcd,
+0x04,0xae,0xa4,0x00,0x83,0x35,0xd8,0xfd,0xcd,0x58,0xdc,0xcd,0x5b,0x60,0xdc,0xcd,
+0x0d,0xee,0x72,0x0e,0x7b,0xef,0xfc,0xcd,0x58,0xdc,0xcd,0x5b,0x87,0xdc,0xcd,0x40,
+0xae,0xa4,0x00,0x83,0x35,0xa0,0x3f,0xa1,0x3f,0xa2,0xb7,0x0f,0xa4,0xa2,0xb6,0x0e,
+0xfd,0xcd,0x05,0xee,0x72,0x06,0x7b,0xef,0xfc,0xcd,0x28,0xae,0xa4,0x00,0x83,0x35,
+0xa0,0x3f,0xa1,0x3f,0xa2,0xb7,0x1f,0xa4,0xa2,0xb6,0x0e,0xfd,0xcd,0x4c,0x01,0xce,
+0x4d,0x01,0xc6,0x78,0x84,0xc7,0x09,0x7b,0x79,0x84,0xc7,0x0a,0x7b,0x7a,0x84,0xc7,
+0x0b,0x7b,0x7b,0x84,0xc7,0x0c,0x7b,0x07,0xef,0x72,0x08,0x6b,0x4a,0x5a,0x01,0x26,
+0x4d,0x58,0x01,0xce,0x59,0x01,0xc6,0x06,0x6b,0x4f,0x01,0xc6,0x05,0x6b,0x4e,0x01,
+0xc6,0x0a,0x6b,0x05,0xaa,0x0a,0x7b,0x20,0x20,0x08,0x6b,0x59,0x01,0xc6,0x07,0x6b,
+0x58,0x01,0xc6,0x05,0xef,0x72,0x06,0x6b,0x59,0x48,0x4e,0x01,0xce,0x4f,0x01,0xc6,
+0x0a,0x6b,0x01,0xaa,0x0a,0x7b,0x1f,0x26,0x4a,0x47,0x01,0xc6,0x09,0x6b,0x80,0xaa,
+0x09,0x7b,0x06,0x26,0x04,0xa1,0x6b,0x01,0xc6,0x09,0x6b,0x20,0xa6,0x0a,0x6b,0x0b,
+0x6b,0x0c,0x6b,0x4f,0x0d,0x6b,0x0e,0xef,0x72,0x58,0x01,0xc9,0x41,0x59,0x01,0xcb,
+0x52,0x01,0xce,0x53,0x01,0xc6,0x8b,0x41,0x00,0xa2,0x41,0x0e,0xa0,0x5b,0x81,0x84,
+0x84,0x84,0x84,0xe7,0x01,0x01,0x35,0xef,0xfc,0xcd,0xb4,0xae,0xa4,0x00,0x84,0x35,
+0xa0,0x16,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x5b,0x01,0x6b,0xe3,
+0xa4,0x01,0x7b,0x01,0x6b,0xb4,0x84,0xc6,0x02,0x6b,0xb5,0x84,0xc6,0x03,0x6b,0xb6,
+0x84,0xc6,0x04,0x6b,0xb7,0x84,0xc6,0x30,0x25,0x20,0xa1,0xf3,0x01,0xc6,0xef,0xfc,
+0xcd,0x78,0xae,0xa4,0x00,0x84,0x35,0xd8,0xfd,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,
+0xab,0x5b,0xa0,0xb7,0x03,0xa4,0xa0,0xb6,0xa1,0xb7,0xc0,0xa4,0xa1,0xb6,0xa2,0x3f,
+0xa3,0x3f,0x8c,0xfc,0xcd,0x16,0xa6,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,0x00,
+0x62,0x01,0x55,0x01,0x6b,0xfc,0xa4,0x01,0x7b,0x02,0x6b,0x3f,0xa4,0x02,0x7b,0x01,
+0x6b,0x78,0x84,0xc6,0x02,0x6b,0x79,0x84,0xc6,0x03,0x6b,0x7a,0x84,0xc6,0x04,0x6b,
+0x7b,0x84,0xc6,0xef,0xfc,0xcd,0x74,0xae,0xa4,0x00,0x84,0x35,0x0e,0xfd,0xcd,0x45,
+0x01,0xce,0x46,0x01,0xc6,0xef,0xfc,0xcd,0x70,0xae,0xa4,0x00,0x84,0x35,0x0e,0xfd,
+0xcd,0x43,0x01,0xce,0x44,0x01,0xc6,0xef,0xfc,0xcd,0x6c,0xae,0xa4,0x00,0x84,0x35,
+0x0e,0xfd,0xcd,0x41,0x01,0xce,0x42,0x01,0xc6,0xef,0xfc,0xcd,0x68,0xae,0xa4,0x00,
+0x84,0x35,0x0e,0xfd,0xcd,0x3f,0x01,0xce,0x40,0x01,0xc6,0xef,0xfc,0xcd,0x64,0xae,
+0xa4,0x00,0x84,0x35,0x0e,0xfd,0xcd,0x3d,0x01,0xce,0x3e,0x01,0xc6,0x12,0x20,0x64,
+0x84,0x00,0x35,0x65,0x84,0x00,0x35,0x66,0x84,0x02,0x35,0x67,0x84,0xcb,0x35,0x12,
+0x25,0x02,0xa2,0x3d,0x01,0xc6,0xcb,0xa0,0x3e,0x01,0xc6,0x1e,0x26,0x02,0xa1,0xee,
+0x01,0xc6,0x25,0x26,0x03,0xa1,0x70,0x01,0xc6,0x2c,0x24,0x20,0xa1,0xf3,0x01,0xc6,
+0xef,0xfc,0xcd,0x60,0xae,0xa4,0x00,0x84,0x35,0x0e,0xfd,0xcd,0x3b,0x01,0xce,0x3c,
+0x01,0xc6,0xef,0xfc,0xcd,0x5c,0xae,0xa4,0x00,0x84,0x35,0x0e,0xfd,0xcd,0x39,0x01,
+0xce,0x3a,0x01,0xc6,0xef,0xfc,0xcd,0x58,0xae,0xa4,0x00,0x84,0x35,0x0e,0xfd,0xcd,
+0x37,0x01,0xce,0x38,0x01,0xc6,0x88,0x88,0x88,0x88,0x81,0xa4,0xc6,0x92,0xa5,0xbf,
+0xa4,0xb7,0x81,0xa4,0xd6,0x92,0xa5,0x3f,0xa4,0xb7,0x81,0x84,0x45,0x01,0xcf,0x3d,
+0x01,0xce,0x46,0x01,0xc7,0x3e,0x01,0xc6,0x3d,0x01,0xcf,0x3e,0x01,0xc7,0x4a,0x5a,
+0x01,0x26,0x4d,0x59,0x48,0x65,0x01,0xce,0x66,0x01,0xc6,0x0b,0x20,0x0e,0x26,0x4d,
+0x65,0x01,0xce,0x66,0x01,0xc6,0x0b,0x26,0x4a,0x47,0x01,0xc6,0x43,0x01,0xcf,0x3b,
+0x01,0xce,0x44,0x01,0xc7,0x3c,0x01,0xc6,0x3c,0x01,0x5f,0x72,0x3b,0x01,0x5f,0x72,
+0x41,0x01,0xcf,0x39,0x01,0xce,0x42,0x01,0xc7,0x3a,0x01,0xc6,0x39,0x01,0xcf,0x3a,
+0x01,0xc7,0x4a,0x5a,0x01,0x26,0x4d,0xfa,0x26,0x5a,0x90,0x46,0x54,0x06,0x27,0x62,
+0x01,0xce,0x90,0x63,0x01,0xce,0x64,0x01,0xc6,0x3f,0x01,0xcf,0x37,0x01,0xce,0x40,
+0x01,0xc7,0x38,0x01,0xc6,0x38,0x01,0x5f,0x72,0x37,0x01,0x5f,0x72,0x62,0x01,0x5f,
+0x72,0x04,0x26,0x04,0xa1,0xee,0x01,0xc6,0x0b,0x26,0x03,0xa1,0x0f,0x20,0x52,0x01,
+0x5f,0x72,0x53,0x01,0x23,0x35,0x19,0x26,0x03,0xa1,0xee,0x01,0xc6,0x20,0x26,0x03,
+0xa1,0x15,0x24,0x70,0x01,0xc6,0x20,0xa1,0xf3,0x01,0xc6,0x65,0x01,0x4a,0x01,0x55,
+0x66,0x01,0x4b,0x01,0x55,0x69,0x01,0x4e,0x01,0x55,0x6a,0x01,0x4f,0x01,0x55,0x63,
+0x01,0x48,0x01,0x55,0x64,0x01,0x49,0x01,0x55,0x67,0x01,0x4c,0x01,0x55,0x68,0x01,
+0x4d,0x01,0x55,0x8e,0xd6,0xcc,0xbe,0xd8,0xcd,0xd3,0xa9,0x41,0x00,0xab,0x52,0x1c,
+0xae,0x01,0x7b,0xef,0xfc,0xcd,0x5e,0xae,0xa4,0x00,0x01,0x35,0x46,0xfc,0xcd,0xa4,
+0xb7,0xd2,0xa9,0x41,0xfc,0xab,0x52,0x1c,0xae,0x01,0x7b,0x5d,0x01,0xc7,0xbe,0xd8,
+0xcd,0xd2,0xa9,0x41,0xfb,0xab,0x52,0x1c,0xae,0x01,0x7b,0x5c,0x01,0xc7,0xbe,0xd8,
+0xcd,0xd2,0xa9,0x41,0xfa,0xab,0x52,0x1c,0xae,0x01,0x7b,0x5b,0x01,0xc7,0xa4,0xd6,
+0x92,0xa5,0x3c,0x5a,0x01,0xc7,0xb6,0xd8,0xcd,0xd2,0xa9,0x41,0xf8,0xab,0x52,0x1c,
+0xae,0x01,0x7b,0x58,0x01,0x5f,0x72,0x59,0x01,0x01,0x35,0x57,0x01,0xc7,0xa4,0xd6,
+0x92,0xa5,0x3c,0x56,0x01,0xc7,0xb6,0xd8,0xcd,0xd2,0xa9,0x41,0xf4,0xab,0x52,0x1c,
+0xae,0x01,0x7b,0x55,0x01,0xc7,0xa4,0xd6,0x92,0xa5,0x3c,0x54,0x01,0xc7,0xb6,0xd8,
+0xcd,0xd2,0xa9,0x41,0xf2,0xab,0x52,0x1c,0xae,0x01,0x7b,0x53,0x01,0xc7,0xa4,0xd6,
+0x92,0xa5,0x3c,0x52,0x01,0xc7,0xb6,0xd8,0xcd,0xd2,0xa9,0x41,0xf0,0xab,0x52,0x1c,
+0xae,0x01,0x7b,0x51,0x01,0xc7,0xa4,0xd6,0x92,0xa5,0x3c,0x50,0x01,0xc7,0xb6,0xd8,
+0xcd,0xd2,0xa9,0x41,0xee,0xab,0x52,0x1c,0xae,0x01,0x7b,0x4f,0x01,0xc7,0xa4,0xd6,
+0x92,0xa5,0x3c,0x4e,0x01,0xc7,0xb6,0xd8,0xcd,0xd2,0xa9,0x41,0xec,0xab,0x52,0x1c,
+0xae,0x01,0x7b,0x4d,0x01,0xc7,0xa4,0xd6,0x92,0xa5,0x3c,0x4c,0x01,0xc7,0xb6,0xd8,
+0xcd,0xd2,0xa9,0x41,0xea,0xab,0x52,0x1c,0xae,0x01,0x7b,0x4b,0x01,0xc7,0xa4,0xd6,
+0x92,0xa5,0x3c,0x4a,0x01,0xc7,0xb6,0xd8,0xcd,0xd2,0xa9,0x41,0xe8,0xab,0x52,0x1c,
+0xae,0x01,0x7b,0x49,0x01,0xc7,0xa4,0xd6,0x92,0xa5,0x3c,0x48,0x01,0xc7,0xb6,0xd8,
+0xcd,0xd2,0xa9,0x41,0xe6,0xab,0x52,0x1c,0xae,0x01,0x7b,0x47,0x01,0xc7,0xbe,0xd8,
+0xcd,0xd2,0xa9,0x41,0xe5,0xab,0x52,0x1c,0xae,0x01,0x7b,0xdd,0xd7,0xcc,0x05,0xd8,
+0xcc,0x03,0x27,0x02,0xa1,0xdd,0xd7,0xcc,0x03,0x26,0x03,0xa1,0x70,0x01,0xc6,0x62,
+0x01,0xc7,0x29,0xb6,0x5e,0x01,0x2a,0x00,0x55,0x5f,0x01,0x2b,0x00,0x55,0x60,0x01,
+0x2c,0x00,0x55,0x61,0x01,0x2d,0x00,0x55,0x5c,0x01,0xc7,0x13,0xb6,0x13,0xb6,0x5a,
+0x01,0xcf,0x22,0xbe,0x5b,0x01,0xc7,0x23,0xb6,0x58,0x01,0xcf,0x20,0xbe,0x59,0x01,
+0xc7,0x21,0xb6,0x56,0x01,0xcf,0x1e,0xbe,0x57,0x01,0xc7,0x1f,0xb6,0x54,0x01,0xcf,
+0x1c,0xbe,0x55,0x01,0xc7,0x1d,0xb6,0x52,0x01,0xcf,0x26,0xbe,0x53,0x01,0xc7,0x27,
+0xb6,0x50,0x01,0xcf,0x24,0xbe,0x51,0x01,0xc7,0x25,0xb6,0x4e,0x01,0xcf,0x18,0xbe,
+0x4f,0x01,0xc7,0x19,0xb6,0x4c,0x01,0xcf,0x14,0xbe,0x4d,0x01,0xc7,0x15,0xb6,0x4a,
+0x01,0xcf,0x1a,0xbe,0x4b,0x01,0xc7,0x1b,0xb6,0x48,0x01,0xcf,0x16,0xbe,0x49,0x01,
+0xc7,0x17,0xb6,0x47,0x01,0xc7,0x28,0xb6,0xa5,0xd6,0xcc,0x03,0x24,0x1d,0xa1,0x11,
+0xb6,0x09,0x27,0x11,0x3d,0x01,0x6b,0x4a,0x11,0xb6,0x88,0x00,0xf0,0x70,0x1c,0x05,
+0x01,0x01,0x03,0x00,0x03,0x00,0x8f,0x00,0x46,0x00,0x1b,0x00,0xaa,0x01,0x1e,0x03,
+0x00,0x07,0x00,0x03,0x50,0x05,0x01,0x00,0x60,0xa0,0x18,0x05,0x01,0x01,0x06,0x00,
+0x03,0x00,0x70,0x00,0x40,0x00,0x18,0x00,0xb0,0x01,0x1b,0x03,0x00,0x07,0x00,0x03,
+0x50,0x05,0x01,0x00,0xe0,0x1b,0xfa,0x04,0x01,0x00,0x06,0x00,0x03,0x00,0x80,0x00,
+0x48,0x00,0x1c,0x00,0x90,0x01,0x3f,0x03,0x90,0x06,0x20,0x03,0x00,0x05,0x01,0x00,
+0xc0,0x5f,0x3b,0x04,0x00,0x01,0x06,0x00,0x03,0x00,0x20,0x00,0x30,0x00,0x14,0x00,
+0xa0,0x00,0x37,0x03,0xa0,0x05,0x20,0x03,0x00,0x05,0x01,0x00,0xe0,0x12,0xbd,0x04,
+0x01,0x00,0x07,0x00,0x03,0x00,0x80,0x00,0x40,0x00,0x1b,0x00,0x80,0x01,0x1e,0x03,
+0x80,0x06,0x00,0x03,0x00,0x05,0x01,0x00,0x90,0x69,0x11,0x04,0x00,0x01,0x07,0x00,
+0x03,0x00,0x20,0x00,0x30,0x00,0x13,0x00,0xa0,0x00,0x16,0x03,0xa0,0x05,0x00,0x03,
+0x00,0x05,0x01,0x00,0x40,0xd2,0xdf,0x03,0x00,0x00,0x06,0x00,0x03,0x00,0x88,0x00,
+0x18,0x00,0x23,0x00,0x40,0x01,0x26,0x03,0x40,0x05,0x00,0x03,0x00,0x04,0x01,0x00,
+0xf0,0xfb,0x02,0x02,0x01,0x01,0x08,0x00,0x06,0x00,0x70,0x00,0x10,0x00,0x1f,0x00,
+0xf0,0x00,0x05,0x02,0x40,0x04,0xe0,0x01,0x50,0x03,0x01,0x00,0x00,0x5a,0x62,0x02,
+0x01,0x01,0x04,0x00,0x01,0x00,0x80,0x00,0x28,0x00,0x1b,0x00,0x00,0x01,0x74,0x02,
+0x20,0x04,0x58,0x02,0x20,0x03,0x01,0x00,0x10,0xf7,0x6c,0x04,0x01,0x01,0x05,0x00,
+0x01,0x00,0x28,0x00,0xe0,0x06,0x19,0x00,0xe4,0x07,0xee,0x02,0xe4,0x0c,0xd0,0x02,
+0x00,0x05,0x01,0x00,0x10,0xf7,0x6c,0x04,0x01,0x01,0x05,0x00,0x01,0x00,0x28,0x00,
+0x74,0x09,0x19,0x00,0x78,0x0a,0xee,0x02,0x78,0x0f,0xd0,0x02,0x00,0x05,0x01,0x00,
+0x40,0x5f,0x8a,0x03,0x01,0x01,0x05,0x00,0x01,0x00,0x28,0x00,0xe0,0x06,0x19,0x00,
+0xe4,0x07,0xee,0x02,0xe4,0x0c,0xd0,0x02,0x00,0x05,0x01,0x00,0x10,0xf7,0x6c,0x04,
+0x01,0x01,0x05,0x00,0x01,0x00,0x2c,0x00,0x58,0x00,0x29,0x00,0x18,0x01,0x65,0x04,
+0x98,0x08,0x38,0x04,0x80,0x07,0x01,0x00,0x10,0xf7,0x6c,0x04,0x01,0x01,0x05,0x00,
+0x01,0x00,0x2c,0x00,0x10,0x02,0x29,0x00,0xd0,0x02,0x65,0x04,0x50,0x0a,0x38,0x04,
+0x80,0x07,0x01,0x00,0x10,0xf7,0x6c,0x04,0x01,0x01,0x05,0x00,0x01,0x00,0x2c,0x00,
+0x7e,0x02,0x29,0x00,0x3e,0x03,0x65,0x04,0xbe,0x0a,0x38,0x04,0x80,0x07,0x01,0x00,
+0x20,0xee,0xd9,0x08,0x01,0x01,0x05,0x00,0x01,0x00,0x2c,0x00,0x10,0x02,0x29,0x00,
+0xd0,0x02,0x65,0x04,0x50,0x0a,0x38,0x04,0x80,0x07,0x01,0x00,0x80,0xf9,0x37,0x03,
+0x00,0x00,0x05,0x00,0x01,0x00,0x80,0x00,0x18,0x00,0x2c,0x00,0x20,0x01,0x71,0x02,
+0xc0,0x06,0x40,0x02,0xa0,0x05,0x01,0x01,0xc0,0xfc,0x9b,0x01,0x00,0x00,0x03,0x00,
+0x01,0x00,0x7e,0x00,0x18,0x00,0x16,0x00,0x20,0x01,0x71,0x02,0xc0,0x06,0x20,0x01,
+0xa0,0x05,0x00,0x00,0x10,0xf7,0x6c,0x04,0x01,0x01,0x05,0x00,0x01,0x00,0x2c,0x00,
+0x10,0x02,0x14,0x00,0xd0,0x02,0x65,0x04,0x50,0x0a,0x1c,0x02,0x80,0x07,0x00,0x00,
+0x10,0xf7,0x6c,0x04,0x01,0x01,0x05,0x00,0x01,0x00,0x28,0x00,0xb8,0x01,0x19,0x00,
+0xbc,0x02,0xee,0x02,0xbc,0x07,0xd0,0x02,0x00,0x05,0x01,0x00,0xc0,0xfc,0x9b,0x01,
+0x00,0x00,0x05,0x00,0x01,0x00,0x40,0x00,0x0c,0x00,0x2c,0x00,0x90,0x00,0x71,0x02,
+0x60,0x03,0x40,0x02,0xd0,0x02,0x01,0x00,0x20,0xee,0xd9,0x08,0x01,0x01,0x05,0x00,
+0x01,0x00,0x2c,0x00,0x58,0x00,0x29,0x00,0x18,0x01,0x65,0x04,0x98,0x08,0x38,0x04,
+0x80,0x07,0x01,0x00,0x80,0xf9,0x37,0x03,0x00,0x00,0x06,0x00,0x07,0x00,0x7c,0x00,
+0x20,0x00,0x24,0x00,0x14,0x01,0x0d,0x02,0xb4,0x06,0xe0,0x01,0xa0,0x05,0x01,0x01,
+0xc0,0xfc,0x9b,0x01,0x00,0x00,0x03,0x00,0x04,0x00,0x7c,0x00,0x26,0x00,0x12,0x00,
+0x14,0x01,0x0d,0x02,0xb4,0x06,0xf0,0x00,0xa0,0x05,0x00,0x00,0x10,0xf7,0x6c,0x04,
+0x01,0x01,0x05,0x00,0x01,0x00,0x2c,0x00,0x58,0x00,0x14,0x00,0x18,0x01,0x65,0x04,
+0x98,0x08,0x1c,0x02,0x80,0x07,0x00,0x00,0x10,0xf7,0x6c,0x04,0x01,0x01,0x05,0x00,
+0x01,0x00,0x28,0x00,0x6e,0x00,0x19,0x00,0x72,0x01,0xee,0x02,0x72,0x06,0xd0,0x02,
+0x00,0x05,0x01,0x00,0xc0,0xfc,0x9b,0x01,0x00,0x00,0x06,0x00,0x07,0x00,0x3e,0x00,
+0x10,0x00,0x24,0x00,0x8a,0x00,0x0d,0x02,0x5a,0x03,0xe0,0x01,0xd0,0x02,0x01,0x00,
+0x80,0x85,0x80,0x01,0x00,0x00,0x02,0x00,0x01,0x00,0x60,0x00,0x10,0x00,0x23,0x00,
+0xa0,0x00,0x0d,0x02,0x20,0x03,0xe0,0x01,0x80,0x02,0x01,0x81,0x84,0x8b,0x41,0x00,
+0xa9,0x41,0x08,0xab,0x5b,0x09,0x6b,0x01,0x6b,0x4f,0x01,0x20,0x01,0xa6,0x04,0x26,
+0xbf,0x01,0xc1,0x05,0x7b,0x0b,0x26,0x04,0xe1,0x72,0xc0,0x01,0xc6,0x08,0x27,0xbf,
+0x01,0xc6,0xb7,0x26,0x46,0xfd,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x02,0xab,0x5b,0x1e,
+0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x06,0xab,0x5b,0x06,0x6b,0x30,0x48,0xc6,0x07,
+0x6b,0x31,0x48,0xc6,0x08,0x6b,0x32,0x48,0xc6,0x09,0x6b,0x33,0x48,0xc6,0x84,0x84,
+0xc0,0x1b,0xcd,0x74,0xae,0x08,0xa6,0x02,0x4b,0x00,0x4b,0x02,0x6b,0x06,0x7b,0x03,
+0x6b,0x07,0x7b,0x04,0x6b,0x08,0x7b,0x05,0x6b,0x09,0x7b,0x06,0x6b,0x30,0x48,0xc6,
+0x07,0x6b,0x31,0x48,0xc6,0x08,0x6b,0x32,0x48,0xc6,0x09,0x6b,0x33,0x48,0xc6,0x01,
+0x6b,0x4f,0x8b,0x41,0x00,0xa2,0x41,0x09,0xa0,0x5b,0xa9,0x20,0x17,0xb6,0x17,0xb7,
+0x80,0xaa,0x02,0x7b,0x84,0x84,0xc0,0x1b,0xcd,0x74,0xae,0x20,0xa6,0x14,0x4b,0x88,
+0x07,0xab,0x01,0x7b,0xf0,0x25,0x05,0xa1,0x03,0x6b,0x4c,0x9f,0x18,0xe7,0xbf,0x01,
+0xd6,0x03,0xee,0x72,0x03,0x6b,0x12,0x26,0x4a,0x02,0x7b,0x84,0x84,0xc0,0x1b,0xcd,
+0x74,0xae,0x43,0xa6,0x88,0x02,0x7b,0x07,0x4b,0x25,0x27,0x02,0x7b,0x01,0x6b,0x52,
+0x05,0xae,0xf0,0x24,0x14,0xa1,0x02,0x6b,0x7f,0xa4,0xbf,0x01,0xc6,0x81,0x85,0x85,
+0x85,0x4f,0x05,0xc0,0x01,0x07,0x72,0x05,0x20,0x02,0xbf,0x01,0x0f,0x72,0x84,0x84,
+0xc0,0x1b,0xcd,0x74,0xae,0x41,0xa6,0x02,0x4b,0x00,0x4b,0x19,0x26,0x60,0xa1,0x60,
+0xa4,0xbf,0x01,0xc6,0x84,0x84,0xc0,0x1b,0xcd,0x74,0xae,0x40,0xa6,0x01,0x4b,0x00,
+0x4b,0x88,0x88,0x88,0x81,0xa0,0x01,0xc6,0xa0,0x01,0x01,0x35,0x04,0xbf,0x01,0x0d,
+0x72,0x84,0x84,0xc0,0x1b,0xcd,0x74,0xae,0x40,0xa6,0x01,0x4b,0x00,0x4b,0xa0,0x01,
+0x5f,0x72,0x81,0x85,0x9a,0xef,0xfc,0xcd,0x14,0xae,0xa4,0x00,0x48,0x35,0xa0,0xb7,
+0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,0x00,0xa3,0x01,0x55,0x10,0x48,0xa4,0x01,0x55,0x11,
+0x48,0xa5,0x01,0x55,0x12,0x48,0xa6,0x01,0x55,0x13,0x48,0xa7,0x01,0x55,0x9b,0xcd,
+0x25,0x05,0xa1,0x01,0x6b,0x4c,0x01,0x7b,0xd2,0xfd,0xcd,0xa4,0xae,0xa4,0x00,0x01,
+0x35,0x8c,0xfc,0xcd,0x48,0x48,0x48,0x9f,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,
+0xb7,0xbf,0x01,0xd6,0x01,0xee,0x72,0x1f,0x20,0xa3,0x01,0xc3,0x01,0x55,0x07,0x26,
+0x04,0xa1,0x01,0x6b,0x4f,0x84,0x84,0xa4,0x01,0x5f,0x72,0xa5,0x01,0x5f,0x72,0xa6,
+0x01,0x5f,0x72,0xa7,0x01,0x5f,0x72,0xc0,0x1b,0xcd,0x74,0xae,0x4f,0x05,0x4b,0x00,
+0x4b,0x88,0x81,0x84,0x8b,0x41,0x00,0xa9,0x41,0x08,0xab,0x5b,0x09,0x6b,0x01,0x6b,
+0x85,0x9b,0x1c,0xcd,0x74,0xae,0x10,0xa6,0x05,0x4b,0xc3,0x01,0xc7,0x05,0x7b,0x02,
+0x6b,0xb7,0x01,0xc6,0x03,0x6b,0xb8,0x01,0xc6,0x04,0x6b,0xb9,0x01,0xc6,0x05,0x6b,
+0xba,0x01,0xc6,0xc2,0x01,0xc7,0x02,0x7b,0xc1,0x01,0xc7,0x03,0x7b,0xc0,0x01,0xc7,
+0x04,0x7b,0xbf,0x01,0xc7,0x05,0x7b,0x02,0x6b,0xb3,0x01,0xc6,0x03,0x6b,0xb4,0x01,
+0xc6,0x04,0x6b,0xb5,0x01,0xc6,0x05,0x6b,0xb6,0x01,0xc6,0x01,0x6b,0x85,0x9b,0x1c,
+0xcd,0x74,0xae,0x18,0xa6,0x08,0x4b,0xc6,0x01,0xc7,0x06,0x7b,0xc5,0x01,0xc7,0x07,
+0x7b,0xc4,0x01,0xc7,0x08,0x7b,0xc3,0x01,0xc7,0x09,0x7b,0x06,0x6b,0x1c,0x48,0xc6,
+0x07,0x6b,0x1d,0x48,0xc6,0x08,0x6b,0x1e,0x48,0xc6,0x09,0x6b,0x1f,0x48,0xc6,0xc2,
+0x01,0xc7,0x06,0x7b,0xc1,0x01,0xc7,0x07,0x7b,0xc0,0x01,0xc7,0x08,0x7b,0xbf,0x01,
+0xc7,0x09,0x7b,0x06,0x6b,0x18,0x48,0xc6,0x07,0x6b,0x19,0x48,0xc6,0x08,0x6b,0x1a,
+0x48,0xc6,0x09,0x6b,0x1b,0x48,0xc6,0x01,0x6b,0x4f,0x8b,0x41,0x00,0xa2,0x41,0x09,
+0xa0,0x5b,0x81,0x85,0x85,0x85,0x85,0x85,0x01,0x7b,0x01,0x6b,0x01,0xa6,0x04,0x26,
+0x14,0xa1,0x02,0x7b,0x0a,0x26,0x14,0xa1,0x03,0x7b,0xd1,0x25,0x05,0xa1,0x05,0x6b,
+0x4c,0x05,0x7b,0xdd,0x25,0x08,0xa1,0x04,0x6b,0x4c,0x04,0x7b,0xbf,0x01,0x44,0x72,
+0x02,0x6b,0x4c,0x02,0x7b,0x05,0x20,0x03,0x6b,0x4c,0x03,0x7b,0x07,0x26,0x01,0xa4,
+0xbf,0x01,0xd6,0x05,0xee,0x72,0x04,0x6b,0x4f,0x05,0x6b,0x4f,0x44,0x20,0x4f,0x03,
+0x27,0x85,0x85,0x4d,0xc0,0x1b,0xcd,0x74,0xae,0x05,0x4b,0x00,0x4b,0x9a,0x14,0x48,
+0xb7,0x01,0x55,0x15,0x48,0xb8,0x01,0x55,0x16,0x48,0xb9,0x01,0x55,0x17,0x48,0xba,
+0x01,0x55,0x10,0x48,0xb3,0x01,0x55,0x11,0x48,0xb4,0x01,0x55,0x12,0x48,0xb5,0x01,
+0x55,0x13,0x48,0xb6,0x01,0x55,0x9b,0x03,0x6b,0x02,0x6b,0x01,0x6b,0x4f,0x88,0x88,
+0x88,0x88,0x88,0x81,0xa4,0x00,0x48,0x35,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,
+0xb7,0x81,0x85,0x85,0x85,0x85,0x01,0x7b,0x9a,0xef,0xfc,0xcd,0x0c,0xae,0x0d,0xad,
+0x03,0x7b,0x9b,0x01,0x6b,0x01,0xa6,0x04,0x26,0x02,0x7b,0x04,0x26,0x4a,0x03,0x7b,
+0xee,0x26,0x02,0x7b,0x06,0x26,0x03,0x7b,0x02,0x6b,0x4a,0x02,0x7b,0x03,0x6b,0x0f,
+0x48,0xc6,0x0a,0x20,0x9a,0xef,0xfc,0xcd,0x5f,0x38,0xad,0x04,0x7b,0x9b,0x02,0x6b,
+0x32,0xa6,0x01,0x6b,0x03,0x6b,0x4f,0x88,0x88,0x88,0x88,0x81,0x1e,0xfe,0xcd,0xa4,
+0xb7,0x00,0xa9,0x41,0x05,0xab,0x81,0x84,0x84,0x84,0x84,0x84,0x84,0x84,0x84,0x9a,
+0xef,0xfc,0xcd,0x04,0xae,0xa4,0x00,0x48,0x35,0xa3,0x15,0xa0,0xb7,0xa1,0xb7,0xa2,
+0xb7,0x4f,0xa3,0xb7,0x03,0xaa,0x3f,0xa4,0x08,0x7b,0x9b,0x05,0x6b,0x04,0x48,0xc6,
+0x06,0x6b,0x05,0x48,0xc6,0x07,0x6b,0x06,0x48,0xc6,0x08,0x6b,0x07,0x48,0xc6,0x9a,
+0xef,0xfc,0xcd,0x5f,0xa4,0x00,0x40,0x35,0xd8,0xfd,0xcd,0xa4,0xb7,0x00,0xa9,0x41,
+0x01,0xab,0x5b,0xa3,0x14,0x4f,0xad,0x5b,0xef,0xfc,0xcd,0xa4,0xb7,0x00,0xa9,0x41,
+0x01,0xab,0x5b,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,0x00,0x9d,0x01,0x55,0x9b,
+0x05,0x6b,0x00,0x40,0xc6,0x06,0x6b,0x01,0x40,0xc6,0x07,0x6b,0x02,0x40,0xc6,0x08,
+0x6b,0x03,0x40,0xc6,0x9a,0xef,0xfc,0xcd,0xb4,0xae,0xa4,0x00,0x84,0x35,0xa3,0x1a,
+0xa3,0x19,0x7a,0xcf,0xcd,0x5b,0x9b,0x05,0x6b,0xb4,0x84,0xc6,0x06,0x6b,0xb5,0x84,
+0xc6,0x07,0x6b,0xb6,0x84,0xc6,0x08,0x6b,0xb7,0x84,0xc6,0x88,0x88,0x88,0x88,0x88,
+0x88,0x88,0x88,0x81,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x81,0x84,
+0x84,0x84,0x84,0x9a,0xef,0xfc,0xcd,0x04,0xae,0xa4,0x00,0x48,0x35,0xa3,0x14,0x11,
+0xad,0x5b,0x9b,0x01,0x6b,0x04,0x48,0xc6,0x02,0x6b,0x05,0x48,0xc6,0x03,0x6b,0x06,
+0x48,0xc6,0x04,0x6b,0x07,0x48,0xc6,0x9a,0xef,0xfc,0xcd,0x5f,0xa4,0x00,0x40,0x35,
+0xa3,0x15,0x34,0xad,0x5b,0x9b,0x01,0x6b,0x00,0x40,0xc6,0x02,0x6b,0x01,0x40,0xc6,
+0x03,0x6b,0x02,0x40,0xc6,0x04,0x6b,0x03,0x40,0xc6,0x88,0x88,0x88,0x88,0x81,0x85,
+0x85,0x85,0x85,0x4f,0x9a,0xef,0xfc,0xcd,0x04,0xae,0xa4,0x00,0x48,0x35,0xa0,0x3f,
+0xa1,0x3f,0xa2,0x3f,0xa3,0xb7,0x30,0xa4,0xa3,0xb6,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,
+0x4f,0xa3,0xb7,0x04,0x7b,0x9b,0x01,0x6b,0x04,0x48,0xc6,0x02,0x6b,0x05,0x48,0xc6,
+0x03,0x6b,0x06,0x48,0xc6,0x04,0x6b,0x07,0x48,0xc6,0x9a,0xef,0xfc,0xcd,0x5f,0xa4,
+0x00,0x40,0x35,0xa3,0x15,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x5b,
+0x9b,0x01,0x6b,0x00,0x40,0xc6,0x02,0x6b,0x01,0x40,0xc6,0x03,0x6b,0x02,0x40,0xc6,
+0x04,0x6b,0x03,0x40,0xc6,0x88,0x88,0x88,0x88,0x81,0x9a,0xd2,0xfc,0xcd,0xa4,0xb7,
+0x40,0xa9,0x41,0x00,0xab,0x81,0xa4,0xb7,0x00,0xa9,0x41,0x07,0xab,0x81,0x8b,0x41,
+0x00,0xa9,0x41,0x0d,0xab,0x5b,0x9a,0xef,0xfc,0xcd,0x30,0xae,0xa4,0x00,0x42,0x35,
+0xd8,0xfd,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x5b,0x8c,0xfc,0xcd,0x48,0x48,
+0x12,0x7b,0xa0,0x3f,0xa1,0x3f,0xa2,0x3f,0xa3,0x00,0x03,0x35,0x9b,0x01,0x6b,0x30,
+0x42,0xc6,0x02,0x6b,0x31,0x42,0xc6,0x03,0x6b,0x32,0x42,0xc6,0x04,0x6b,0x33,0x42,
+0xc6,0x24,0xcd,0xcc,0x03,0x25,0x06,0xe1,0x72,0xa4,0xd6,0x92,0x02,0xae,0xa5,0xb7,
+0x0d,0x7b,0xa4,0xb7,0x0c,0x7b,0x06,0x6b,0x04,0xab,0x06,0x7b,0x67,0xad,0x5c,0x01,
+0x24,0x06,0xeb,0x72,0x10,0xee,0x72,0x11,0x7b,0x1e,0xfe,0xcd,0x6f,0xad,0x5b,0x9b,
+0xaf,0x25,0x04,0xa1,0x0b,0x6b,0x4c,0x0b,0x7b,0xa4,0xc7,0x92,0xa5,0xbf,0x85,0xa4,
+0x00,0x32,0xa4,0xd6,0x92,0x03,0xae,0xa5,0xbf,0xa4,0xb7,0x0e,0xe9,0x72,0x41,0x0f,
+0xeb,0x72,0x5f,0x4a,0x07,0x6b,0x4c,0x07,0x7b,0xa4,0x00,0x3b,0x89,0xa4,0x3c,0x02,
+0x24,0x97,0xa5,0xbb,0x0b,0xe0,0x72,0x03,0xa6,0xa5,0xbf,0xf3,0xcd,0xcd,0x5b,0x36,
+0x25,0x05,0xe1,0x72,0xa4,0xd6,0x92,0x02,0xae,0xa5,0xb7,0x0d,0x7b,0xa4,0xb7,0x0c,
+0x7b,0x0b,0x6b,0x07,0x6b,0x08,0x6b,0x09,0x6b,0x0a,0x6b,0x4f,0x74,0x20,0x05,0x6b,
+0x4f,0x10,0x6b,0x00,0xa9,0x10,0x7b,0x11,0x6b,0x04,0xab,0x11,0x7b,0xfb,0xcd,0xcd,
+0x10,0xee,0x72,0x11,0x7b,0x1e,0xfe,0xcd,0xf3,0xcd,0xcd,0x5b,0x9b,0x07,0x6b,0x4f,
+0x08,0x6b,0xa4,0xd6,0x92,0x5c,0x09,0x6b,0xa4,0xd6,0x92,0x01,0xae,0xa5,0xb7,0x0d,
+0x7b,0xa4,0xb7,0x0c,0x7b,0x0a,0x6b,0xa4,0xc6,0x92,0xa5,0xb7,0x0d,0x7b,0xa4,0xbf,
+0x0c,0xee,0x72,0x8b,0x41,0x00,0xa2,0x41,0x0b,0xa0,0x5b,0x89,0x88,0x81,0x01,0xa6,
+0x15,0x00,0xde,0x35,0x14,0x00,0xc0,0x35,0x13,0x00,0xad,0x35,0x12,0x00,0xde,0x35,
+0x50,0x18,0xcd,0x81,0x84,0x84,0x84,0x84,0x84,0x7e,0xcb,0xcd,0xac,0x25,0x10,0xa1,
+0x04,0x6b,0x4c,0x04,0x7b,0xc2,0x25,0x08,0xa1,0x05,0x6b,0x4c,0x05,0x7b,0xc6,0xcb,
+0xcd,0x03,0x27,0x5d,0x03,0x26,0x4d,0x41,0x01,0xe4,0x72,0x41,0x02,0xe4,0x72,0xfa,
+0x26,0x5a,0x90,0x59,0x48,0x06,0x27,0x05,0xe6,0x72,0x01,0xa6,0x01,0xef,0x72,0x02,
+0x6b,0x5f,0x03,0x7b,0xeb,0xcb,0xcd,0x03,0x20,0x9a,0xad,0x04,0x26,0x05,0x7b,0x08,
+0x26,0x04,0x7b,0x05,0x6b,0x4f,0x03,0x6b,0x12,0xe6,0x97,0x04,0xe0,0x72,0x0f,0xa6,
+0x04,0x6b,0x4f,0xf5,0x25,0x10,0xa1,0x04,0x6b,0x4c,0x04,0x7b,0x97,0xad,0x04,0x6b,
+0x4f,0xc6,0xcb,0xcd,0x7e,0xcb,0xcd,0xa8,0xcb,0xcd,0x88,0x88,0x88,0x88,0x88,0x81,
+0xb3,0x84,0x00,0x35,0xb0,0x84,0x00,0x35,0xb1,0x84,0x00,0x35,0x81,0xb0,0x84,0x00,
+0x35,0xb1,0x84,0x00,0x35,0xb2,0x84,0x00,0x35,0x0d,0xad,0xb2,0x84,0x03,0x35,0x13,
+0xad,0xb2,0x84,0x02,0x35,0x19,0xad,0xb2,0x84,0x00,0x35,0xb3,0x84,0x00,0x35,0x81,
+0xb0,0x84,0x00,0x35,0xb1,0x84,0x00,0x35,0xb2,0x84,0x00,0x35,0xb3,0x84,0x00,0x35,
+0x81,0x01,0xad,0xb0,0x84,0x00,0x35,0xb1,0x84,0x00,0x35,0xb2,0x84,0x01,0x35,0xb3,
+0x84,0x00,0x35,0x13,0xad,0x81,0x64,0xa6,0xb0,0x84,0x00,0x35,0xb1,0x84,0x00,0x35,
+0x05,0xc5,0xcc,0x03,0xad,0xb2,0x84,0x00,0x35,0xb3,0x84,0x00,0x35,0x05,0xc5,0xcd,
+0x10,0xad,0xb2,0x84,0x04,0x35,0xb3,0x84,0x00,0x35,0x81,0xad,0x84,0x00,0x35,0xae,
+0x84,0x00,0x35,0xaf,0x84,0x00,0x35,0x81,0xac,0x84,0x00,0x35,0x05,0xad,0x81,0xac,
+0x84,0x80,0x35,0x0c,0xad,0x07,0x27,0x4d,0x05,0xc5,0xcc,0x64,0xa6,0xb0,0x84,0x00,
+0x35,0xb1,0x84,0x00,0x35,0xb2,0x84,0x00,0x35,0xb3,0x84,0x00,0x35,0x05,0xc5,0xcd,
+0x0a,0xa6,0xb0,0x84,0x00,0x35,0xb1,0x84,0x00,0x35,0xb2,0x84,0x10,0x35,0xb3,0x84,
+0x00,0x35,0x81,0x84,0x84,0x84,0x84,0xe7,0x01,0x01,0x35,0xef,0xfc,0xcd,0x5f,0xa4,
+0x00,0x84,0x35,0xa0,0x1a,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x5b,
+0x01,0x6b,0x00,0x84,0xc6,0x02,0x6b,0x01,0x84,0xc6,0x03,0x6b,0x02,0x84,0xc6,0x04,
+0x6b,0x03,0x84,0xc6,0x88,0x88,0x88,0x88,0x81,0x84,0x84,0x84,0x84,0xef,0xfc,0xcd,
+0x5f,0xa4,0x00,0x84,0x35,0xa0,0x1b,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,
+0xab,0x5b,0x01,0x6b,0x00,0x84,0xc6,0x02,0x6b,0x01,0x84,0xc6,0x03,0x6b,0x02,0x84,
+0xc6,0x04,0x6b,0x03,0x84,0xc6,0x88,0x88,0x88,0x88,0x81,0xef,0xfc,0xcd,0x78,0xae,
+0xa4,0x00,0x84,0x35,0x81,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x81,
+0x84,0x84,0x84,0x84,0x10,0xad,0xa3,0x1d,0x09,0xad,0x5b,0x17,0xad,0xa3,0x1c,0x10,
+0xad,0x5b,0x01,0x6b,0x78,0x84,0xc6,0x02,0x6b,0x79,0x84,0xc6,0x03,0x6b,0x7a,0x84,
+0xc6,0x04,0x6b,0x7b,0x84,0xc6,0x88,0x88,0x88,0x88,0x81,0x0e,0xfd,0xcd,0xbc,0x00,
+0xce,0xbd,0x00,0xc6,0x81,0x84,0x84,0x84,0x84,0xa0,0x84,0xc7,0x01,0x7b,0xa1,0x84,
+0xc7,0x02,0x7b,0xa2,0x84,0xc7,0x03,0x7b,0xa3,0x84,0xc7,0x04,0x7b,0x03,0x6b,0x80,
+0xaa,0x03,0x7b,0x04,0x6b,0x06,0xaa,0x04,0x7b,0x0c,0x20,0xd2,0xfd,0xcd,0xa4,0xb7,
+0x00,0xa9,0x41,0x01,0xab,0x5b,0xa2,0x1e,0x34,0xad,0x11,0x24,0x00,0xa2,0xbc,0x00,
+0xc6,0x06,0xa0,0xbd,0x00,0xc6,0x0c,0x27,0x71,0x01,0xc6,0x22,0x25,0x02,0xa1,0xbb,
+0x00,0xc6,0x29,0x26,0x4a,0x70,0x01,0xc6,0x03,0x6b,0x80,0xa4,0x03,0x7b,0x04,0x6b,
+0x4f,0x01,0x6b,0xa0,0x84,0xc6,0x02,0x6b,0xa1,0x84,0xc6,0x03,0x6b,0xa2,0x84,0xc6,
+0x04,0x6b,0xa3,0x84,0xc6,0x6c,0x25,0x02,0xa1,0xbb,0x00,0xc6,0x73,0x26,0x4a,0x70,
+0x01,0xc6,0x0d,0x27,0x4a,0x71,0x01,0xc6,0xef,0xfc,0xcd,0x78,0xae,0xa4,0x00,0x84,
+0x35,0xa0,0x1c,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x5b,0x01,0x6b,
+0x78,0x84,0xc6,0x02,0x6b,0x79,0x84,0xc6,0x03,0x6b,0x7a,0x84,0xc6,0x04,0x6b,0x7b,
+0x84,0xc6,0x48,0x83,0x00,0x35,0x49,0x83,0x00,0x35,0x4a,0x83,0x00,0x35,0x4b,0x83,
+0x0f,0x35,0x10,0x26,0x4a,0x24,0x83,0x00,0x35,0x25,0x83,0x00,0x35,0x26,0x83,0x00,
+0x35,0x27,0x83,0x00,0x35,0x4d,0x27,0x70,0x01,0xc6,0xef,0xfc,0xcd,0xa4,0xae,0xa4,
+0x00,0x84,0x35,0xa2,0x1e,0xa0,0x3f,0xa1,0x3f,0xa2,0x1f,0xcc,0xca,0xcd,0x14,0x26,
+0x03,0xa1,0x04,0x27,0x01,0xa1,0xbb,0x00,0xc6,0x1f,0x26,0x4a,0x70,0x01,0xc6,0xef,
+0xfc,0xcd,0xa0,0xae,0xa4,0x00,0x84,0x35,0xa0,0x1f,0xa2,0x3f,0xa3,0x3f,0x8c,0xfc,
+0xcd,0x10,0xa6,0x0e,0xfd,0xcd,0x5c,0x01,0x24,0x02,0xab,0x58,0x01,0xce,0x59,0x01,
+0xc6,0x0b,0x20,0xec,0x01,0xce,0xed,0x01,0xc6,0x2a,0x26,0x4a,0x0b,0x27,0x02,0xa0,
+0x70,0x01,0xc6,0xf2,0x01,0x5f,0x72,0x48,0xca,0xcc,0x03,0x27,0x72,0x01,0xc6,0x08,
+0x27,0x71,0x01,0xc6,0x88,0x88,0x88,0x88,0x81,0x84,0x84,0x84,0x84,0xa0,0x84,0xc7,
+0x01,0x7b,0xa1,0x84,0xc7,0x02,0x7b,0xa2,0x84,0xc7,0x03,0x7b,0xa3,0x84,0xc7,0x04,
+0x7b,0xd2,0xfd,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x5b,0xa2,0x1e,0x0e,0xfd,
+0xcd,0xbc,0x00,0xce,0xbd,0x00,0xc6,0x03,0x6b,0x80,0xa4,0x03,0x7b,0x04,0x6b,0x4f,
+0x1f,0x25,0x02,0xa1,0xbb,0x00,0xc6,0x26,0x26,0x4a,0x70,0x01,0xc6,0x03,0x6b,0x7f,
+0xa4,0x03,0x7b,0x01,0x6b,0xa0,0x84,0xc6,0x02,0x6b,0xa1,0x84,0xc6,0x03,0x6b,0xa2,
+0x84,0xc6,0x04,0x6b,0xa3,0x84,0xc6,0x5a,0x26,0x71,0x01,0xc6,0x5f,0x20,0x24,0x83,
+0x00,0x35,0x25,0x83,0x00,0x35,0x26,0x83,0x02,0x35,0x27,0x83,0x00,0x35,0xa0,0x84,
+0x00,0x35,0xa1,0x84,0x00,0x35,0xa2,0x84,0x00,0x35,0xa3,0x84,0x00,0x35,0xa4,0x84,
+0x00,0x35,0xa5,0x84,0x00,0x35,0xa6,0x84,0x00,0x35,0xa7,0x84,0x00,0x35,0xef,0xfc,
+0xcd,0x78,0xae,0xa4,0x00,0x84,0x35,0xa0,0x1d,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,
+0x41,0x01,0xab,0x5b,0x01,0x6b,0x78,0x84,0xc6,0x02,0x6b,0x79,0x84,0xc6,0x03,0x6b,
+0x7a,0x84,0xc6,0x04,0x6b,0x7b,0x84,0xc6,0xf2,0x01,0xc7,0x5f,0x26,0x71,0x01,0xce,
+0x64,0x26,0x72,0x01,0xc6,0x88,0x88,0x88,0x88,0x81,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,
+0xa9,0x41,0x01,0xab,0x81,0x84,0x84,0x84,0x84,0x00,0x40,0xc7,0x01,0x7b,0x01,0x40,
+0xc7,0x02,0x7b,0x02,0x40,0xc7,0x03,0x7b,0x03,0x40,0xc7,0x04,0x7b,0xef,0xfc,0xcd,
+0x5f,0xa4,0x00,0x40,0x35,0xa0,0x1e,0x23,0xad,0x5b,0xef,0xfc,0xcd,0x1c,0xae,0xa4,
+0x00,0x40,0x35,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,0xb7,0x13,0xb6,0x14,0x20,
+0x1c,0x40,0x00,0x35,0x1d,0x40,0x00,0x35,0x1e,0x40,0x00,0x35,0x1f,0x40,0x00,0x35,
+0x04,0x6b,0x02,0xaa,0x04,0x7b,0x18,0x26,0x12,0x3d,0x04,0x6b,0x40,0xaa,0x04,0x7b,
+0x06,0x26,0x5d,0x01,0xc6,0x04,0x6b,0x10,0xaa,0x04,0x7b,0x06,0x26,0x5c,0x01,0xc6,
+0x01,0x6b,0x02,0x6b,0x03,0x6b,0x4f,0x04,0x6b,0x01,0xa6,0xef,0xfc,0xcd,0xb4,0xae,
+0xa4,0x00,0x84,0x35,0xa3,0x18,0xa2,0x18,0xac,0xc8,0xcd,0x5b,0x01,0x6b,0xb4,0x84,
+0xc6,0x02,0x6b,0xb5,0x84,0xc6,0x03,0x6b,0xb6,0x84,0xc6,0x04,0x6b,0xb7,0x84,0xc6,
+0x05,0xc5,0xcd,0xfa,0xa6,0x14,0x45,0x00,0x35,0x15,0x45,0x00,0x35,0x16,0x45,0x00,
+0x35,0x17,0x45,0x02,0x35,0x88,0x88,0x88,0x88,0x81,0x01,0x40,0x00,0x35,0x02,0x40,
+0x00,0x35,0x03,0x40,0x00,0x35,0x81,0x84,0x84,0x84,0x84,0x72,0x01,0x5f,0x72,0xef,
+0xfc,0xcd,0xb4,0xae,0xa4,0x00,0x84,0x35,0xa2,0x19,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,
+0xa9,0x41,0x01,0xab,0x5b,0x01,0x6b,0xb4,0x84,0xc6,0x02,0x6b,0xb5,0x84,0xc6,0x03,
+0x6b,0xb6,0x84,0xc6,0x04,0x6b,0xb7,0x84,0xc6,0x14,0x45,0x00,0x35,0x15,0x45,0x00,
+0x35,0x16,0x45,0x00,0x35,0x17,0x45,0x01,0x35,0x00,0x40,0x00,0x35,0x47,0xad,0x00,
+0x40,0x80,0x35,0x4d,0xad,0x88,0x88,0x88,0x88,0x81,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,
+0xa9,0x41,0x01,0xab,0x81,0xac,0x84,0x00,0x35,0xad,0x84,0x00,0x35,0xae,0x84,0x00,
+0x35,0x81,0x00,0x88,0x00,0x35,0x01,0x88,0x00,0x35,0x02,0x88,0x00,0x35,0x81,0xa2,
+0x14,0xa3,0x14,0xa0,0x3f,0xa1,0x3f,0xa2,0xb7,0x01,0xa4,0xa2,0xb6,0xa3,0xb7,0xf8,
+0xa4,0xa3,0xb6,0x2e,0xfc,0xcd,0x08,0xae,0x90,0x5f,0x26,0xb6,0x81,0x84,0x84,0x84,
+0x84,0x84,0x84,0x25,0xad,0x03,0x88,0x01,0x35,0x08,0x88,0x00,0x35,0x09,0x88,0x30,
+0x35,0x0a,0x88,0x00,0x35,0x0b,0x88,0x00,0x35,0xef,0xfc,0xcd,0x0c,0xae,0xa4,0x00,
+0x88,0x35,0xa3,0x1e,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,0xb7,0x07,0xa4,0x05,
+0x7b,0x18,0x88,0x00,0x35,0x19,0x88,0x00,0x35,0x1a,0x88,0xff,0x35,0x1b,0x88,0xff,
+0x35,0x14,0x88,0x00,0x35,0x15,0x88,0x00,0x35,0x16,0x88,0xff,0x35,0x17,0x88,0xff,
+0x35,0x10,0x88,0x00,0x35,0x11,0x88,0x5f,0x35,0x12,0x88,0xff,0x35,0x13,0x88,0xff,
+0x35,0x72,0xc7,0xcd,0x03,0x88,0x00,0x35,0xef,0xfc,0xcd,0xb4,0xae,0xa4,0x00,0x84,
+0x35,0xa2,0x1e,0xa0,0x1e,0x8c,0xc7,0xcd,0x5b,0x11,0x26,0x21,0xa1,0x0a,0x20,0xa0,
+0xb7,0x60,0xaa,0xa0,0xb6,0x8c,0xc7,0xcd,0x5b,0x0c,0x25,0x22,0xa1,0xf3,0x01,0xc6,
+0x01,0x6b,0xb4,0x84,0xc6,0x02,0x6b,0xb5,0x84,0xc6,0x03,0x6b,0xb6,0x84,0xc6,0x04,
+0x6b,0xb7,0x84,0xc6,0x7f,0xc7,0xcd,0xaf,0x84,0x20,0x35,0xef,0xfc,0xcd,0x7c,0xae,
+0xa4,0x00,0x84,0x35,0xa0,0x14,0xa1,0x1a,0xa1,0x10,0x54,0xc7,0xcd,0x09,0x20,0xa1,
+0x1a,0xa1,0x10,0x54,0xc7,0xcd,0x12,0x20,0xa1,0x10,0x54,0xc7,0xcd,0x19,0x20,0x54,
+0xc7,0xcd,0x27,0x20,0x17,0x27,0x4a,0x11,0x27,0x4a,0x0d,0x27,0x4a,0x0b,0x27,0x4a,
+0x05,0x7b,0x7f,0xc7,0xcd,0xaf,0x84,0x00,0x35,0x7c,0x84,0x00,0x35,0x7d,0x84,0x00,
+0x35,0x7e,0x84,0x00,0x35,0x7f,0x84,0x00,0x35,0x72,0xc7,0xcd,0x03,0x88,0x00,0x35,
+0xb7,0xc5,0xcd,0x21,0x25,0x20,0xa1,0xf3,0x01,0xc6,0x88,0x88,0x88,0x88,0x89,0x88,
+0x81,0xac,0x84,0x00,0x35,0xad,0x84,0x00,0x35,0xae,0x84,0x00,0x35,0xaf,0x84,0x00,
+0x35,0x7c,0x84,0x00,0x35,0x7d,0x84,0x00,0x35,0x7e,0x84,0x00,0x35,0x7f,0x84,0x00,
+0x35,0x00,0x88,0x00,0x35,0x01,0x88,0x00,0x35,0x02,0x88,0x00,0x35,0x03,0x88,0x00,
+0x35,0xa8,0xad,0x02,0x25,0x20,0xa1,0xf3,0x01,0xc6,0x70,0x01,0x5f,0x72,0xe5,0x01,
+0x5f,0x72,0x81,0x84,0x84,0x84,0x84,0xb4,0x84,0xc7,0x01,0x7b,0xb5,0x84,0xc7,0x02,
+0x7b,0xb6,0x84,0xc7,0x03,0x7b,0xb7,0x84,0xc7,0x04,0x7b,0xef,0xfc,0xcd,0xb4,0xae,
+0xa4,0x00,0x84,0x35,0xa1,0x1c,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,
+0x5b,0x01,0x6b,0xb4,0x84,0xc6,0x02,0x6b,0xb5,0x84,0xc6,0x03,0x6b,0xb6,0x84,0xc6,
+0x04,0x6b,0xb7,0x84,0xc6,0x88,0x88,0x88,0x88,0x81,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,
+0xa9,0x41,0x03,0xab,0xef,0xfc,0xcc,0x5f,0xa4,0x00,0x84,0x35,0xa0,0x00,0x30,0x35,
+0x0c,0xad,0x5b,0xef,0xfc,0xcd,0x5f,0xa4,0x00,0x84,0x35,0xa0,0x1a,0xa0,0x3f,0x1b,
+0xad,0x5b,0x81,0xa0,0x3f,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x03,0xab,0xef,
+0xfc,0xcc,0x5f,0xa4,0x00,0x84,0x35,0xa0,0x18,0x0a,0xad,0x5b,0xef,0xfc,0xcd,0x5f,
+0xa4,0x00,0x84,0x35,0x15,0xad,0x5b,0x81,0x85,0x90,0x85,0x90,0x01,0xee,0x72,0x02,
+0x7b,0x05,0x20,0x05,0xee,0x72,0x06,0x7b,0x07,0x24,0x05,0xe2,0x72,0x9f,0x06,0xe0,
+0x72,0x89,0x88,0x81,0x85,0x90,0x85,0x90,0x05,0xee,0x72,0x06,0x7b,0x05,0x20,0x01,
+0xee,0x72,0x02,0x7b,0x07,0x24,0x05,0xe2,0x72,0x9f,0x06,0xe0,0x72,0x89,0x88,0x81,
+0x84,0x84,0xe9,0x26,0xa4,0xb6,0xed,0x26,0x5d,0x01,0x6b,0x00,0xa2,0xa4,0xb7,0x01,
+0x7b,0x02,0x6b,0x01,0xa0,0x97,0x02,0x7b,0x9d,0x01,0x20,0x01,0xef,0x72,0x02,0x6b,
+0x63,0xfc,0xcd,0x03,0xae,0x90,0xa7,0x3f,0x5f,0x88,0x88,0x81,0x41,0x20,0x40,0x35,
+0x48,0x20,0x5f,0x72,0x46,0x20,0x5f,0x72,0x45,0x20,0xc7,0x01,0xa6,0x02,0x20,0xd8,
+0xa6,0x04,0x24,0x20,0xa1,0xf3,0x01,0xc6,0x81,0x9a,0xd9,0x01,0xcf,0xda,0x01,0xc7,
+0x9b,0x81,0x44,0x42,0x00,0x35,0x45,0x42,0x00,0x35,0x46,0x42,0x00,0x35,0x47,0x42,
+0x00,0x35,0x00,0x42,0x00,0x35,0x01,0x42,0x00,0x35,0x02,0x42,0x01,0x35,0x03,0x42,
+0x01,0x35,0x10,0x20,0x00,0x42,0x08,0x35,0x01,0x42,0x00,0x35,0x02,0x42,0x01,0x35,
+0x03,0x42,0x00,0x35,0x12,0x26,0x4a,0xbf,0x00,0xc6,0x20,0x40,0x00,0x35,0x21,0x40,
+0x00,0x35,0x22,0x40,0x00,0x35,0x23,0x40,0x01,0x35,0x04,0x44,0xc3,0x00,0x55,0x05,
+0x44,0xc4,0x00,0x55,0x06,0x44,0xc5,0x00,0x55,0x07,0x44,0xc6,0x00,0x55,0x00,0x44,
+0xc7,0x00,0x55,0x01,0x44,0xc8,0x00,0x55,0x02,0x44,0xc9,0x00,0x55,0x03,0x44,0xca,
+0x00,0x55,0x81,0x85,0x4f,0xf3,0x25,0xbf,0x00,0xc1,0x01,0x6b,0x4c,0x01,0x7b,0x76,
+0xc2,0xcd,0x06,0x20,0x4f,0x88,0x81,0x9a,0xd2,0xfc,0xcd,0xa0,0x3f,0xa1,0x3f,0xa2,
+0x3f,0x81,0xa0,0xb7,0xa1,0xb7,0xa2,0xb7,0x4f,0xa3,0xb7,0x81,0x9a,0xd2,0xfc,0xcd,
+0xa4,0xb7,0x01,0xaa,0x41,0x81,0xa4,0xc6,0x92,0xa5,0xbf,0xa4,0xb7,0x81,0x1d,0xad,
+0xa3,0x3f,0xa4,0xb7,0x01,0xaa,0x33,0xc3,0xcc,0x4f,0x29,0xad,0xa3,0x00,0x3d,0x35,
+0x03,0xee,0x72,0xa4,0xb7,0x02,0x7b,0x9b,0x37,0xad,0xa3,0x00,0x08,0x35,0xa4,0xb7,
+0x02,0xaa,0x41,0x24,0xaa,0x02,0xee,0x72,0x03,0x7b,0x9b,0xd3,0x26,0x04,0xe1,0x72,
+0x33,0xad,0x02,0xaa,0x41,0x03,0xaa,0x02,0xee,0x72,0x03,0x7b,0x32,0xc3,0xcc,0x03,
+0x26,0x01,0x6b,0x4a,0x01,0x7b,0x9a,0xd2,0xfc,0xcd,0xa4,0xb7,0x02,0xaa,0x02,0x7b,
+0x03,0xee,0x72,0x68,0xad,0x04,0x7b,0x9b,0x1c,0x20,0x01,0x6b,0xff,0xa6,0x04,0x6b,
+0x80,0xaa,0x04,0x7b,0x06,0x26,0x02,0xa1,0x11,0xb6,0x04,0x6b,0x03,0xa6,0x02,0x20,
+0x01,0xa6,0x06,0x20,0x02,0xa6,0x04,0x27,0x15,0x3d,0x0c,0x27,0xc1,0x00,0xc6,0x79,
+0xad,0x41,0x1c,0xaa,0x02,0xee,0x72,0x03,0x7b,0x9b,0x2a,0xc4,0xcd,0x41,0x18,0xaa,
+0x02,0xee,0x72,0x03,0x7b,0x9b,0x2a,0xc4,0xcd,0x41,0x14,0xaa,0x02,0xee,0x72,0x03,
+0x7b,0x9b,0xd7,0x26,0x04,0xe1,0x72,0x33,0xc4,0xcd,0x01,0xaa,0x41,0x13,0xaa,0x02,
+0xee,0x72,0x03,0x7b,0xb6,0x27,0x01,0x6b,0x4a,0x01,0x7b,0x3b,0xc4,0xcd,0x10,0xaa,
+0x02,0xee,0x72,0x03,0x7b,0x45,0xc4,0xcd,0x04,0x7b,0x9b,0x17,0x20,0x01,0x6b,0xff,
+0xa6,0x04,0x6b,0xcf,0xa6,0x02,0x20,0x0f,0xa6,0x06,0x20,0x0c,0xa6,0x08,0x20,0x06,
+0x27,0x4a,0x05,0x27,0x14,0xb6,0xcc,0x26,0x04,0xe1,0x72,0x33,0xc4,0xcd,0x01,0xaa,
+0x41,0x0d,0xaa,0x02,0xee,0x72,0x03,0x7b,0x81,0x85,0x85,0x85,0x85,0x4c,0x06,0x26,
+0x01,0x6b,0x4a,0x01,0x7b,0x3b,0xc4,0xcd,0x0c,0xaa,0x02,0xee,0x72,0x03,0x7b,0x8c,
+0xfc,0xcd,0x10,0xa6,0x45,0xc4,0xcd,0x04,0x7b,0x9b,0x22,0x20,0x01,0x6b,0xff,0xa6,
+0x04,0x6b,0xfc,0xa6,0x02,0x20,0xf0,0xa6,0x06,0x20,0xcc,0xa6,0x0a,0x20,0xc0,0xa6,
+0x0e,0x20,0x0c,0xa6,0x12,0x20,0x0f,0xa6,0x16,0x20,0x0c,0xa6,0x18,0x27,0x4a,0x17,
+0x27,0x4a,0x16,0x27,0x4a,0x15,0x27,0x4a,0x14,0x27,0x4a,0x29,0x27,0x4a,0x16,0x27,
+0x13,0xb6,0xc9,0x26,0x04,0xe1,0x72,0x33,0xc4,0xcd,0x01,0xaa,0x41,0x0b,0xaa,0x02,
+0xee,0x72,0x03,0x7b,0x66,0x27,0x01,0x6b,0x4a,0x01,0x7b,0x3b,0xc4,0xcd,0x08,0xaa,
+0x02,0xee,0x72,0x03,0x7b,0xa0,0xb7,0x03,0xaa,0xa0,0xb6,0xa1,0xb7,0xc3,0xaa,0xa0,
+0xb7,0xa2,0xb7,0x4f,0xa3,0xb7,0x04,0x7b,0x9b,0x25,0x20,0x01,0x6b,0xff,0xa6,0x04,
+0x6b,0x4f,0x01,0x20,0x0c,0xa6,0x04,0x27,0x15,0x3d,0x2a,0xc4,0xcd,0x41,0x04,0xaa,
+0x02,0xee,0x72,0x03,0x7b,0x9b,0x2a,0xc4,0xcd,0x9b,0x02,0x6b,0x03,0xef,0x72,0x50,
+0xaa,0x41,0x4f,0x58,0x58,0x97,0x88,0x88,0x88,0x88,0x81,0x1e,0xfe,0xcd,0xc7,0xae,
+0xa4,0x00,0x00,0x35,0x81,0xa4,0xb7,0x00,0xa9,0x41,0x01,0xab,0x81,0xa0,0xb7,0xa1,
+0xb7,0xa2,0xb7,0x4f,0xa3,0xb7,0x81,0xa0,0xb7,0x03,0xa4,0xa0,0xb6,0xa1,0x3f,0xa2,
+0x3f,0xa3,0x3f,0x8c,0xfc,0xcd,0x18,0xa6,0x7a,0xfc,0xcd,0x4a,0x81,0xa4,0xb7,0x00,
+0xa9,0x41,0x07,0xab,0x81,0xc7,0x00,0x00,0x35,0xc8,0x00,0x00,0x35,0xc9,0x00,0x18,
+0x35,0x81,0x2c,0xad,0x18,0xa4,0x48,0x48,0x48,0x4a,0x12,0xb6,0xef,0xfc,0xcd,0x43,
+0xad,0x81,0x8b,0x41,0x00,0xa9,0x41,0x0a,0xab,0x5b,0xef,0xfc,0xcd,0x0c,0xae,0xa4,
+0x00,0x84,0x35,0xa3,0x36,0xa2,0x36,0xa1,0x36,0xa0,0x34,0x67,0xad,0x0a,0x20,0xa0,
+0x39,0xa1,0x39,0xa2,0x39,0xa3,0x38,0x73,0xad,0x0c,0x26,0x02,0xa1,0x11,0xb6,0x84,
+0x84,0x84,0x84,0x84,0x16,0xe0,0xcd,0x88,0x0b,0x7b,0x88,0x0b,0x7b,0x88,0x0b,0x7b,
+0x88,0x0b,0x7b,0x88,0x06,0x7b,0x06,0x6b,0x44,0x44,0x06,0x7b,0xb3,0xfd,0xcd,0x02,
+0xa6,0x6d,0xad,0x5b,0x0e,0x26,0x02,0xa1,0x11,0xb6,0x06,0x6b,0x08,0xa6,0x07,0x6b,
+0x4f,0x08,0x6b,0xbb,0xa6,0x09,0x6b,0x80,0xa6,0x0a,0x6b,0xc7,0x00,0x00,0x35,0xc8,
+0x00,0x00,0x35,0xc9,0x00,0x60,0x35,0xca,0x00,0x00,0x35,0x18,0x20,0xac,0xa6,0x09,
+0x6b,0x44,0xa6,0x0a,0x6b,0xc7,0x00,0x00,0x35,0xc8,0x00,0x00,0x35,0xc9,0x00,0x62,
+0x35,0xca,0x00,0x00,0x35,0x39,0x20,0x10,0xa6,0x07,0x6b,0x4f,0x08,0x6b,0x5d,0xa6,
+0x09,0x6b,0xc0,0xa6,0x0a,0x6b,0xc7,0x00,0x00,0x35,0xc8,0x00,0x00,0x35,0xc9,0x00,
+0x30,0x35,0xca,0x00,0x00,0x35,0x18,0x20,0x56,0xa6,0x09,0x6b,0x22,0xa6,0x0a,0x6b,
+0xc7,0x00,0x00,0x35,0xc8,0x00,0x00,0x35,0xc9,0x00,0x31,0x35,0xca,0x00,0x00,0x35,
+0x74,0x20,0x20,0xa6,0x07,0x6b,0x4f,0x08,0x6b,0x2e,0xa6,0x09,0x6b,0xe0,0xa6,0x0a,
+0x6b,0x2f,0xc2,0xcd,0xca,0x00,0x00,0x35,0xc4,0xc1,0xcc,0x20,0xa6,0x07,0x6b,0x4f,
+0x08,0x6b,0x2b,0xa6,0x09,0x6b,0x11,0xa6,0x0a,0x6b,0x2f,0xc2,0xcd,0xca,0x00,0x80,
+0x35,0xc4,0xc1,0xcc,0x20,0xa6,0x07,0x6b,0x4f,0x08,0x6b,0x1f,0xa6,0x09,0x6b,0x40,
+0xa6,0x0a,0x6b,0xc7,0x00,0x00,0x35,0xc8,0x00,0x00,0x35,0xc9,0x00,0x10,0x35,0xca,
+0x00,0x00,0x35,0xc4,0xc1,0xcc,0x20,0xa6,0x07,0x6b,0x4f,0x08,0x6b,0x2e,0xa6,0x09,
+0x6b,0xe0,0xa6,0x0a,0x6b,0x4f,0x2f,0xc2,0xcd,0xca,0x00,0x00,0x35,0xa5,0xc1,0xcc,
+0x03,0x26,0x4a,0x8b,0xc1,0xcc,0x03,0x26,0x4a,0x6a,0xc1,0xcc,0x03,0x26,0x4a,0x50,
+0xc1,0xcc,0x03,0x26,0x4a,0x6d,0x27,0x4a,0x57,0x27,0x4a,0x38,0x27,0x13,0xb6,0x9a,
+0xef,0xfc,0xcd,0xa4,0xbf,0x84,0xae,0xa3,0x1a,0x1e,0xfe,0xcd,0x3c,0xc2,0xcd,0x5b,
+0x9b,0xd2,0xfd,0xcd,0x3c,0xc2,0xcd,0x5b,0xd8,0xfd,0xcd,0x64,0xc2,0xcd,0x5b,0x1f,
+0xc2,0xcd,0x5b,0x44,0xc2,0xcd,0x5a,0x01,0x26,0x4d,0x5f,0x12,0xb6,0x15,0x20,0xa3,
+0x10,0xd8,0xfd,0xcd,0x64,0xc2,0xcd,0x5b,0x1f,0xc2,0xcd,0x5b,0x44,0xc2,0xcd,0x5a,
+0x01,0x26,0x4d,0x5f,0x12,0xb6,0x19,0x26,0x4a,0x11,0xb6,0x3a,0x20,0x08,0x6b,0x40,
+0xaa,0x08,0x7b,0x0a,0x6b,0x02,0xaa,0x0a,0x7b,0x0e,0x26,0x02,0xa1,0x11,0xb6,0x0a,
+0x6b,0x04,0xaa,0x0a,0x7b,0x06,0x27,0x16,0x3d,0xef,0xfc,0xcd,0x3c,0xc2,0xcd,0x5b,
+0xa2,0x1e,0xa0,0xbf,0xa1,0xb7,0x3e,0xa4,0xa1,0xb6,0xa2,0xbf,0xa3,0xbf,0x8c,0xfc,
+0xcd,0x11,0xa6,0x5a,0xc2,0xcd,0x14,0x20,0xa0,0x14,0xa0,0x3f,0xa1,0xb7,0x3e,0xa4,
+0xa1,0xb6,0xa2,0x3f,0xa3,0x3f,0x8c,0xfc,0xcd,0x11,0xa6,0x5a,0xc2,0xcd,0x18,0x27,
+0xe4,0x01,0xce,0x05,0x6b,0x18,0xa6,0x02,0x20,0x14,0xa6,0x06,0x20,0x10,0xa6,0x08,
+0x20,0x06,0x27,0x4a,0x05,0x27,0x14,0xb6,0x8b,0x41,0x00,0xa2,0x41,0x0a,0xa0,0x5b,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x81,0x4f,0x13,0xb7,0x53,0x20,0xc6,0x50,0x20,0x12,0x72,0x81,0x01,
+0xa6,0x03,0x27,0x4d,0x47,0x1b,0xcd,0x88,0xa6,0x12,0xb7,0x53,0x20,0xc6,0x50,0x20,
+0x15,0x72,0x81,0x01,0xa6,0x03,0x27,0x4d,0x47,0x1b,0xcd,0x88,0xa6,0x81,0x01,0xa6,
+0x03,0x27,0x4d,0xa4,0x1b,0xcd,0x41,0xa6,0x81,0x01,0xa6,0x03,0x27,0x4d,0x8f,0x1b,
+0xcd,0xaa,0x00,0x5f,0x72,0x50,0x20,0x20,0x35,0x52,0x20,0x09,0x35,0x81,0x4d,0x47,
+0x1b,0xcd,0x88,0xa6,0x50,0x20,0x20,0x35,0x53,0x20,0xc7,0xc5,0x20,0x4f,0x50,0x20,
+0x12,0x72,0xec,0x25,0x06,0xe1,0x72,0x01,0x6b,0x4c,0x01,0x7b,0xd4,0x26,0x13,0xad,
+0xbf,0x01,0xd6,0x01,0xee,0x72,0x0d,0x20,0x4f,0x17,0x27,0x60,0xa1,0x02,0x7b,0x05,
+0xc5,0xcd,0x05,0xa6,0xec,0x26,0x2b,0xad,0x03,0x7b,0xf2,0x26,0x4d,0x47,0x1b,0xcd,
+0x88,0xa6,0x81,0x85,0x85,0x85,0x01,0xa6,0x06,0x27,0x4d,0xa4,0x1b,0xcd,0x02,0x7b,
+0x08,0x26,0x4d,0x8f,0x1b,0xcd,0xaa,0x00,0x5f,0x72,0x50,0x20,0x20,0x35,0x52,0x20,
+0x09,0x35,0x88,0x89,0x88,0x81,0x4d,0x47,0x1b,0xcd,0x88,0xa6,0xe0,0x1b,0xcc,0x4f,
+0x05,0xc5,0xcd,0x05,0xa6,0x95,0x25,0x06,0xe1,0x72,0x01,0x7b,0xbf,0x01,0xd7,0x53,
+0x20,0xc6,0x01,0x6b,0x4c,0x9f,0x01,0xee,0x72,0x0d,0x20,0x11,0xe7,0x53,0x20,0xc6,
+0x97,0x07,0xeb,0x72,0x4a,0x01,0x6b,0x4c,0x01,0x7b,0x11,0x25,0x09,0xa1,0x06,0x7b,
+0x50,0x20,0x22,0x35,0x04,0x26,0xa8,0xb1,0xa8,0xbf,0x90,0x0b,0x26,0xa7,0xb3,0xa7,
+0x3f,0x01,0xe6,0x72,0x4a,0x5a,0x01,0x26,0x4d,0x5f,0x06,0x7b,0x50,0x20,0x15,0x72,
+0x04,0x26,0xa8,0xb1,0xa8,0xbf,0x90,0x0b,0x26,0xa7,0xb3,0xa7,0x3f,0x01,0xe6,0x72,
+0x5a,0x01,0x24,0x02,0xa0,0x5f,0x1a,0x25,0x02,0xa1,0x06,0x7b,0xba,0x26,0x72,0xad,
+0x50,0x20,0x15,0x72,0x04,0x26,0x4a,0x06,0x7b,0xc7,0x26,0x4d,0x90,0xad,0x01,0xaa,
+0x02,0x7b,0xd0,0x26,0x4d,0x84,0xad,0x05,0xc5,0xcd,0x05,0xa6,0xda,0x26,0x94,0x1c,
+0xcd,0x50,0x20,0x20,0x35,0x53,0x20,0xc7,0x03,0x7b,0x02,0x20,0x52,0x80,0xae,0x03,
+0x7b,0x07,0x26,0x80,0xa1,0x06,0x7b,0xf5,0x26,0x94,0x1c,0xcd,0x81,0x85,0x85,0x85,
+0x01,0xa6,0x06,0x27,0x4d,0xc9,0xad,0x02,0x7b,0x07,0x26,0x4d,0xbb,0xad,0xaa,0x00,
+0x5f,0x72,0x01,0x6b,0x4f,0x50,0x20,0x20,0x35,0x52,0x20,0x09,0x35,0x88,0x89,0x88,
+0x81,0x4f,0x50,0x20,0x24,0x35,0x05,0xc5,0xcd,0x05,0xa6,0x81,0x01,0xa6,0x03,0x27,
+0x4d,0xb7,0xad,0x82,0xa6,0x50,0x20,0x21,0x35,0x53,0x20,0xc7,0x81,0x4f,0x05,0xc5,
+0xcd,0x05,0xa6,0x81,0x01,0xa6,0x03,0x27,0x4d,0xcf,0xad,0x81,0xa6,0x50,0x20,0x29,
+0x35,0x81,0x85,0x4f,0xaa,0x00,0x5f,0x72,0x81,0x85,0x01,0xa6,0xaa,0x00,0x5f,0x72,
+0x50,0x20,0x22,0x35,0x0c,0xaa,0x00,0x0d,0x72,0x05,0x26,0x01,0xe1,0x72,0x01,0xe4,
+0x72,0xfa,0x27,0xaa,0x00,0xc6,0x8f,0x01,0x20,0x88,0x81,0x85,0x4f,0x81,0x85,0x01,
+0xa6,0x50,0x20,0x05,0x35,0x08,0x27,0x01,0xe1,0x72,0x01,0xe4,0x72,0x51,0x20,0xc6,
+0xfa,0x51,0x20,0x0f,0x72,0x9d,0x01,0x20,0x88,0xe0,0x20,0x8f,0xe3,0x26,0x01,0x7b,
+0xdb,0xf2,0xcd,0x03,0x26,0xd9,0x01,0xca,0xda,0x01,0xc6,0xcd,0xf1,0xcd,0x03,0x24,
+0x20,0xa1,0x12,0x27,0x01,0x6b,0x13,0x18,0xcd,0x64,0x18,0xcd,0x88,0x81,0x84,0x84,
+0x84,0x84,0x84,0x06,0x20,0x08,0x35,0x9a,0x1a,0x20,0x10,0x72,0x15,0x20,0xc7,0xee,
+0xa4,0x15,0x20,0xc6,0x14,0x20,0xc7,0x7c,0xa4,0x14,0x20,0xc6,0x13,0x20,0xc7,0x08,
+0xa4,0x13,0x20,0xc6,0xe8,0xc4,0xcd,0x84,0x84,0x84,0x84,0x84,0x84,0x84,0x84,0x65,
+0xde,0xcd,0x6c,0x01,0x3b,0x6d,0x01,0x3b,0x6e,0x01,0x3b,0x6f,0x01,0x3b,0x01,0x4b,
+0x80,0x4b,0x85,0x4b,0x80,0x4b,0xb0,0x84,0x00,0x35,0xb1,0x84,0x00,0x35,0xb2,0x84,
+0x00,0x35,0xb3,0x84,0x13,0x35,0xa4,0x84,0x00,0x35,0xa5,0x84,0x00,0x35,0xa6,0x84,
+0x00,0x35,0xa7,0x84,0x00,0x35,0xa0,0x84,0x00,0x35,0xa1,0x84,0x00,0x35,0xa2,0x84,
+0x00,0x35,0xa3,0x84,0x00,0x35,0x0c,0x8c,0x00,0x35,0x0d,0x8c,0x00,0x35,0x0e,0x8c,
+0x00,0x35,0x0f,0x8c,0x00,0x35,0x18,0x8c,0x00,0x35,0x19,0x8c,0x00,0x35,0x1a,0x8c,
+0x00,0x35,0x1b,0x8c,0x03,0x35,0x04,0x8c,0x00,0x35,0x05,0x8c,0x00,0x35,0x06,0x8c,
+0x00,0x35,0x07,0x8c,0x0a,0x35,0x00,0x8c,0x00,0x35,0x01,0x8c,0x00,0x35,0x02,0x8c,
+0x00,0x35,0x03,0x8c,0x00,0x35,0x1c,0x8c,0x00,0x35,0x1d,0x8c,0x00,0x35,0x1e,0x8c,
+0x00,0x35,0x1f,0x8c,0x04,0x35,0xfe,0xc5,0xcd,0x97,0xc7,0xcd,0x24,0x83,0x00,0x35,
+0x25,0x83,0x00,0x35,0x26,0x83,0x02,0x35,0x27,0x83,0x00,0x35,0x84,0x84,0x00,0x35,
+0x85,0x84,0x00,0x35,0x86,0x84,0x00,0x35,0x87,0x84,0x20,0x35,0xb4,0x84,0x00,0x35,
+0xb5,0x84,0x20,0x35,0xb6,0x84,0x00,0x35,0xb7,0x84,0xa0,0x35,0x0c,0x20,0xb5,0x84,
+0x00,0x35,0xb6,0x84,0x00,0x35,0xb7,0x84,0x80,0x35,0x0e,0x24,0x20,0xa1,0xf3,0x01,
+0xc6,0x18,0x84,0x00,0x35,0x19,0x84,0x00,0x35,0x1a,0x84,0x00,0x35,0x1b,0x84,0x00,
+0x35,0x78,0x84,0x00,0x35,0x79,0x84,0x00,0x35,0x7a,0x84,0x00,0x35,0x7b,0x84,0x00,
+0x35,0x3f,0x20,0x01,0x35,0x3d,0x20,0xc7,0xff,0xa6,0x3c,0x20,0xc7,0x04,0xa6,0x6c,
+0x01,0x01,0x35,0x6d,0x01,0x86,0x35,0x6e,0x01,0xa0,0x35,0x6f,0x01,0xc7,0x0f,0x20,
+0x6c,0x01,0x02,0x35,0x6d,0x01,0x49,0x35,0x6e,0x01,0xf0,0x35,0x6f,0x01,0xc7,0x1c,
+0x20,0x6d,0x01,0x9b,0x35,0x6e,0x01,0xfc,0x35,0x6f,0x01,0xc0,0x35,0x2a,0x20,0x6d,
+0x01,0x8c,0x35,0x6e,0x01,0xba,0x35,0x6f,0x01,0x80,0x35,0x38,0x20,0x6d,0x01,0x24,
+0x35,0x6e,0x01,0xf8,0x35,0x6f,0x01,0xc7,0x49,0x20,0x6c,0x01,0x5f,0x72,0x6d,0x01,
+0x5f,0x72,0x6e,0x01,0x5f,0x72,0x6f,0x01,0x5f,0x72,0x4c,0x27,0x08,0xa0,0x3f,0x27,
+0x4a,0x34,0x27,0x4a,0x29,0x27,0x4a,0x1f,0x27,0x0f,0xa4,0x08,0x20,0xc6,0xea,0x01,
+0x5f,0x72,0xeb,0x01,0xc7,0x01,0xa6,0x3e,0x20,0xfe,0x35,0xf3,0x01,0x10,0x35,0x0a,
+0x20,0x03,0xa6,0x3e,0x20,0x20,0x35,0xf3,0x01,0xc7,0x21,0xa6,0x11,0x3f,0x04,0x20,
+0x22,0xa6,0x11,0x00,0x01,0x35,0x08,0x27,0x9b,0xfc,0xcd,0xa0,0x3f,0xa1,0x3f,0xa2,
+0xb7,0x02,0xa4,0xa2,0xb6,0xa3,0x3f,0x1e,0xfe,0xcd,0xa4,0xb7,0x00,0xa9,0x41,0x01,
+0xab,0x5b,0x01,0x6b,0x94,0x84,0xc6,0x02,0x6b,0x95,0x84,0xc6,0x03,0x6b,0x96,0x84,
+0xc6,0x04,0x6b,0x97,0x84,0xc6,0x47,0x25,0x10,0xa1,0x03,0x20,0xc6,0xf2,0x25,0x10,
+0xa1,0x05,0x6b,0x4c,0xc9,0x01,0x4f,0x72,0x05,0xee,0x72,0x05,0x6b,0x4f,0xd2,0x00,
+0x5f,0x72,0xd3,0x00,0x5f,0x72,0xe4,0x01,0x5f,0x72,0xbb,0x00,0x5f,0x72,0xbd,0x00,
+0x5f,0x72,0xbc,0x00,0x5f,0x72,0xda,0x01,0x5f,0x72,0xd9,0x01,0x5f,0x72,0x9b,0x01,
+0x0a,0x35,0x9e,0x01,0x5f,0x72,0x9f,0x01,0x5f,0x72,0xa9,0x01,0x5f,0x72,0xa0,0x01,
+0x5f,0x72,0xa1,0x01,0x5f,0x72,0xa2,0x01,0x5f,0x72,0x86,0x01,0x5f,0x72,0xe9,0x01,
+0x5f,0x72,0xee,0x01,0x5f,0x72,0x71,0x01,0x5f,0x72,0x72,0x01,0x5f,0x72,0xaa,0x00,
+0x5f,0x72,0x74,0x01,0x5f,0x72,0x87,0x01,0x5f,0x72,0x75,0x01,0x5f,0x72,0xbf,0x00,
+0x5f,0x72,0xc0,0x00,0x5f,0x72,0xc1,0x00,0x5f,0x72,0xc2,0x00,0x5f,0x72,0x70,0x01,
+0x5f,0x72,0x11,0x3f,0x10,0x3f,0x73,0x01,0x5f,0x72,0xc7,0x01,0x5f,0x72,0xc8,0x01,
+0x5f,0x72,0xef,0x01,0x5f,0x72,0xf0,0x01,0xff,0x35,0xf2,0x01,0x5f,0x72,0xe8,0x01,
+0x5f,0x72,0x9b,0x06,0x20,0x5f,0x72,0x88,0x88,0x88,0x88,0x88,0x81,0x84,0xe3,0xad,
+0xf4,0x25,0x40,0xa1,0x01,0x6b,0x4c,0x12,0x6f,0x01,0xee,0x72,0x01,0x6b,0x4f,0x88,
+0x81,0x05,0x20,0x10,0x72,0x15,0x20,0x11,0x72,0x10,0x1e,0x23,0x1b,0xcc,0x8b,0xff,
+0xa6,0x04,0xae,0x81,0x85,0x01,0x7b,0x9a,0xc7,0x01,0xc7,0x0f,0xa4,0xc7,0x01,0xc6,
+0xc9,0x01,0x4f,0x72,0xc7,0x01,0x5c,0x72,0x01,0x6b,0xc9,0x01,0xd6,0xc7,0x01,0xce,
+0x18,0x27,0xc7,0x01,0xc1,0xc8,0x01,0xc6,0x9b,0x01,0x6b,0x4f,0x88,0x81,0xc8,0x01,
+0xc7,0x0f,0xa4,0xc8,0x01,0xc6,0xc9,0x01,0xd7,0xc8,0x01,0x5c,0x72,0xc8,0x01,0xce,
+};
diff --git a/drivers/video/av8100/av8100_regs.h b/drivers/video/av8100/av8100_regs.h
new file mode 100644
index 00000000000..6ed9000987a
--- /dev/null
+++ b/drivers/video/av8100/av8100_regs.h
@@ -0,0 +1,346 @@
+
+#define AV8100_VAL2REG(__reg, __fld, __val) \
+	(((__val) << __reg##_##__fld##_SHIFT) & __reg##_##__fld##_MASK)
+#define AV8100_REG2VAL(__reg, __fld, __val) \
+	(((__val) & __reg##_##__fld##_MASK) >> __reg##_##__fld##_SHIFT)
+
+#define AV8100_STANDBY 0x00000000
+#define AV8100_STANDBY_CPD_SHIFT 0
+#define AV8100_STANDBY_CPD_MASK 0x00000001
+#define AV8100_STANDBY_CPD_HIGH 1
+#define AV8100_STANDBY_CPD_LOW 0
+#define AV8100_STANDBY_CPD(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY, CPD, __x)
+#define AV8100_STANDBY_CPD_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY, CPD, __x)
+#define AV8100_STANDBY_STBY_SHIFT 1
+#define AV8100_STANDBY_STBY_MASK 0x00000002
+#define AV8100_STANDBY_STBY_HIGH 1
+#define AV8100_STANDBY_STBY_LOW 0
+#define AV8100_STANDBY_STBY(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY, STBY, __x)
+#define AV8100_STANDBY_STBY_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY, STBY, __x)
+#define AV8100_STANDBY_HPDS_SHIFT 2
+#define AV8100_STANDBY_HPDS_MASK 0x00000004
+#define AV8100_STANDBY_HPDS(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY, HPDS, __x)
+#define AV8100_STANDBY_HPDS_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY, HPDS, __x)
+#define AV8100_STANDBY_CPDS_SHIFT 3
+#define AV8100_STANDBY_CPDS_MASK 0x00000008
+#define AV8100_STANDBY_CPDS(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY, CPDS, __x)
+#define AV8100_STANDBY_CPDS_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY, CPDS, __x)
+#define AV8100_STANDBY_MCLKRNG_SHIFT 4
+#define AV8100_STANDBY_MCLKRNG_MASK 0x000000F0
+#define AV8100_STANDBY_MCLKRNG(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY, MCLKRNG, __x)
+#define AV8100_STANDBY_MCLKRNG_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY, MCLKRNG, __x)
+#define AV8100_HDMI_5_VOLT_TIME 0x00000001
+#define AV8100_HDMI_5_VOLT_TIME_OFF_TIME_SHIFT 0
+#define AV8100_HDMI_5_VOLT_TIME_OFF_TIME_MASK 0x0000001F
+#define AV8100_HDMI_5_VOLT_TIME_OFF_TIME(__x) \
+	AV8100_VAL2REG(AV8100_HDMI_5_VOLT_TIME, OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_OFF_TIME_GET(__x) \
+	AV8100_REG2VAL(AV8100_HDMI_5_VOLT_TIME, OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME_SHIFT 0
+#define AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME_MASK 0x00000003
+#define AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME(__x) \
+	AV8100_VAL2REG(AV8100_HDMI_5_VOLT_TIME, DAC_OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME_GET(__x) \
+	AV8100_REG2VAL(AV8100_HDMI_5_VOLT_TIME, DAC_OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME_SHIFT 2
+#define AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME_MASK 0x0000001C
+#define AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME(__x) \
+	AV8100_VAL2REG(AV8100_HDMI_5_VOLT_TIME, SU_OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME_GET(__x) \
+	AV8100_REG2VAL(AV8100_HDMI_5_VOLT_TIME, SU_OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_ON_TIME_SHIFT 5
+#define AV8100_HDMI_5_VOLT_TIME_ON_TIME_MASK 0x000000E0
+#define AV8100_HDMI_5_VOLT_TIME_ON_TIME(__x) \
+	AV8100_VAL2REG(AV8100_HDMI_5_VOLT_TIME, ON_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_ON_TIME_GET(__x) \
+	AV8100_REG2VAL(AV8100_HDMI_5_VOLT_TIME, ON_TIME, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK 0x00000002
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM_SHIFT 0
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM_MASK 0x00000001
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM_HIGH 1
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM_LOW 0
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_INTERRUPT_MASK, HPDM, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_INTERRUPT_MASK, HPDM, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM_SHIFT 1
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM_MASK 0x00000002
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM_HIGH 1
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM_LOW 0
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_INTERRUPT_MASK, CPDM, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_INTERRUPT_MASK, CPDM, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_SHIFT 2
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_MASK 0x0000000C
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_INPUT 0x00
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_ALT 0x01
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_OUTPUT0 0x02
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_OUTPUT1 0x03
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_INTERRUPT_MASK, STBYGPIOCFG, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_INTERRUPT_MASK, STBYGPIOCFG, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL_SHIFT 7
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL_MASK 0x00000080
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL_HIGH 1
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL_LOW 0
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_INTERRUPT_MASK, IPOL, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_INTERRUPT_MASK, IPOL, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT 0x00000003
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI_SHIFT 0
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI_MASK 0x00000001
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI_HIGH 1
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW 0
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_PENDING_INTERRUPT, HPDI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_PENDING_INTERRUPT, HPDI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI_SHIFT 1
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI_MASK 0x00000002
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI_HIGH 1
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW 0
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_PENDING_INTERRUPT, CPDI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_PENDING_INTERRUPT, CPDI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI_SHIFT 2
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI_MASK 0x00000004
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI_HIGH 1
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW 0
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_PENDING_INTERRUPT, ONI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_PENDING_INTERRUPT, ONI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_SID_SHIFT 4
+#define AV8100_STANDBY_PENDING_INTERRUPT_SID_MASK 0x000000F0
+#define AV8100_STANDBY_PENDING_INTERRUPT_SID(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_PENDING_INTERRUPT, SID, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_SID_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_PENDING_INTERRUPT, SID, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_SHIFT 6
+#define AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_MASK 0x00000040
+#define AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_HIGH 1
+#define AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_LOW 0
+#define AV8100_STANDBY_PENDING_INTERRUPT_BPDIG(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_PENDING_INTERRUPT, BPDIG, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK 0x00000004
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM_SHIFT 0
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM_MASK 0x00000001
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, EOCM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, EOCM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM_SHIFT 1
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM_MASK 0x00000002
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, VSIM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, VSIM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM_SHIFT 2
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM_MASK 0x00000004
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, VSOM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, VSOM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM_SHIFT 3
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM_MASK 0x00000008
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, CECM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, CECM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM_SHIFT 4
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM_MASK 0x00000010
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, HDCPM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, HDCPM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM_SHIFT 5
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM_MASK 0x00000020
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, UOVBM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, UOVBM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM_SHIFT 6
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM_MASK 0x00000040
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, TEM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, TEM, __x)
+#define AV8100_GENERAL_INTERRUPT 0x00000005
+#define AV8100_GENERAL_INTERRUPT_EOCI_SHIFT 0
+#define AV8100_GENERAL_INTERRUPT_EOCI_MASK 0x00000001
+#define AV8100_GENERAL_INTERRUPT_EOCI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, EOCI, __x)
+#define AV8100_GENERAL_INTERRUPT_EOCI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, EOCI, __x)
+#define AV8100_GENERAL_INTERRUPT_VSII_SHIFT 1
+#define AV8100_GENERAL_INTERRUPT_VSII_MASK 0x00000002
+#define AV8100_GENERAL_INTERRUPT_VSII(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, VSII, __x)
+#define AV8100_GENERAL_INTERRUPT_VSII_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, VSII, __x)
+#define AV8100_GENERAL_INTERRUPT_VSOI_SHIFT 2
+#define AV8100_GENERAL_INTERRUPT_VSOI_MASK 0x00000004
+#define AV8100_GENERAL_INTERRUPT_VSOI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, VSOI, __x)
+#define AV8100_GENERAL_INTERRUPT_VSOI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, VSOI, __x)
+#define AV8100_GENERAL_INTERRUPT_CECI_SHIFT 3
+#define AV8100_GENERAL_INTERRUPT_CECI_MASK 0x00000008
+#define AV8100_GENERAL_INTERRUPT_CECI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, CECI, __x)
+#define AV8100_GENERAL_INTERRUPT_CECI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, CECI, __x)
+#define AV8100_GENERAL_INTERRUPT_HDCPI_SHIFT 4
+#define AV8100_GENERAL_INTERRUPT_HDCPI_MASK 0x00000010
+#define AV8100_GENERAL_INTERRUPT_HDCPI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, HDCPI, __x)
+#define AV8100_GENERAL_INTERRUPT_HDCPI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, HDCPI, __x)
+#define AV8100_GENERAL_INTERRUPT_UOVBI_SHIFT 5
+#define AV8100_GENERAL_INTERRUPT_UOVBI_MASK 0x00000020
+#define AV8100_GENERAL_INTERRUPT_UOVBI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, UOVBI, __x)
+#define AV8100_GENERAL_INTERRUPT_UOVBI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, UOVBI, __x)
+#define AV8100_GENERAL_INTERRUPT_TEI_SHIFT 6
+#define AV8100_GENERAL_INTERRUPT_TEI_MASK 0x00000040
+#define AV8100_GENERAL_INTERRUPT_TEI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, TEI, __x)
+#define AV8100_GENERAL_INTERRUPT_TEI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, TEI, __x)
+#define AV8100_GENERAL_STATUS 0x00000006
+#define AV8100_GENERAL_STATUS_CECTXERR_SHIFT 0
+#define AV8100_GENERAL_STATUS_CECTXERR_MASK 0x00000001
+#define AV8100_GENERAL_STATUS_CECTXERR_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, CECTXERR, __x)
+#define AV8100_GENERAL_STATUS_CECREC_SHIFT 1
+#define AV8100_GENERAL_STATUS_CECREC_MASK 0x00000002
+#define AV8100_GENERAL_STATUS_CECREC_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, CECREC, __x)
+#define AV8100_GENERAL_STATUS_CECTRX_SHIFT 2
+#define AV8100_GENERAL_STATUS_CECTRX_MASK 0x00000004
+#define AV8100_GENERAL_STATUS_CECTRX_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, CECTRX, __x)
+#define AV8100_GENERAL_STATUS_UC_SHIFT 3
+#define AV8100_GENERAL_STATUS_UC_MASK 0x00000008
+#define AV8100_GENERAL_STATUS_UC_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, UC, __x)
+#define AV8100_GENERAL_STATUS_ONUVB_SHIFT 4
+#define AV8100_GENERAL_STATUS_ONUVB_MASK 0x00000010
+#define AV8100_GENERAL_STATUS_ONUVB_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, ONUVB, __x)
+#define AV8100_GENERAL_STATUS_HDCPS_SHIFT 5
+#define AV8100_GENERAL_STATUS_HDCPS_MASK 0x000000E0
+#define AV8100_GENERAL_STATUS_HDCPS_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, HDCPS, __x)
+#define AV8100_GPIO_CONFIGURATION 0x00000007
+#define AV8100_GPIO_CONFIGURATION_DAT3DIR_SHIFT 0
+#define AV8100_GPIO_CONFIGURATION_DAT3DIR_MASK 0x00000001
+#define AV8100_GPIO_CONFIGURATION_DAT3DIR(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT3DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT3DIR_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT3DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT3VAL_SHIFT 1
+#define AV8100_GPIO_CONFIGURATION_DAT3VAL_MASK 0x00000002
+#define AV8100_GPIO_CONFIGURATION_DAT3VAL(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT3VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT3VAL_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT3VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT2DIR_SHIFT 2
+#define AV8100_GPIO_CONFIGURATION_DAT2DIR_MASK 0x00000004
+#define AV8100_GPIO_CONFIGURATION_DAT2DIR(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT2DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT2DIR_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT2DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT2VAL_SHIFT 3
+#define AV8100_GPIO_CONFIGURATION_DAT2VAL_MASK 0x00000008
+#define AV8100_GPIO_CONFIGURATION_DAT2VAL(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT2VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT2VAL_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT2VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT1DIR_SHIFT 4
+#define AV8100_GPIO_CONFIGURATION_DAT1DIR_MASK 0x00000010
+#define AV8100_GPIO_CONFIGURATION_DAT1DIR(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT1DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT1DIR_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT1DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT1VAL_SHIFT 5
+#define AV8100_GPIO_CONFIGURATION_DAT1VAL_MASK 0x00000020
+#define AV8100_GPIO_CONFIGURATION_DAT1VAL(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT1VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT1VAL_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT1VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_UCDBG_SHIFT 6
+#define AV8100_GPIO_CONFIGURATION_UCDBG_MASK 0x00000040
+#define AV8100_GPIO_CONFIGURATION_UCDBG(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, UCDBG, __x)
+#define AV8100_GPIO_CONFIGURATION_UCDBG_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, UCDBG, __x)
+#define AV8100_GENERAL_CONTROL 0x00000008
+#define AV8100_GENERAL_CONTROL_FDL_SHIFT 4
+#define AV8100_GENERAL_CONTROL_FDL_MASK 0x00000010
+#define AV8100_GENERAL_CONTROL_FDL_HIGH 1
+#define AV8100_GENERAL_CONTROL_FDL_LOW 0
+#define AV8100_GENERAL_CONTROL_FDL(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_CONTROL, FDL, __x)
+#define AV8100_GENERAL_CONTROL_FDL_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_CONTROL, FDL, __x)
+#define AV8100_GENERAL_CONTROL_HLD_SHIFT 5
+#define AV8100_GENERAL_CONTROL_HLD_MASK 0x00000020
+#define AV8100_GENERAL_CONTROL_HLD_HIGH 1
+#define AV8100_GENERAL_CONTROL_HLD_LOW 0
+#define AV8100_GENERAL_CONTROL_HLD(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_CONTROL, HLD, __x)
+#define AV8100_GENERAL_CONTROL_HLD_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_CONTROL, HLD, __x)
+#define AV8100_GENERAL_CONTROL_WA_SHIFT 6
+#define AV8100_GENERAL_CONTROL_WA_MASK 0x00000040
+#define AV8100_GENERAL_CONTROL_WA_HIGH 1
+#define AV8100_GENERAL_CONTROL_WA_LOW 0
+#define AV8100_GENERAL_CONTROL_WA(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_CONTROL, WA, __x)
+#define AV8100_GENERAL_CONTROL_WA_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_CONTROL, WA, __x)
+#define AV8100_GENERAL_CONTROL_RA_SHIFT 7
+#define AV8100_GENERAL_CONTROL_RA_MASK 0x00000080
+#define AV8100_GENERAL_CONTROL_RA_HIGH 1
+#define AV8100_GENERAL_CONTROL_RA_LOW 0
+#define AV8100_GENERAL_CONTROL_RA(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_CONTROL, RA, __x)
+#define AV8100_GENERAL_CONTROL_RA_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_CONTROL, RA, __x)
+#define AV8100_FIRMWARE_DOWNLOAD_ENTRY 0x0000000F
+#define AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY_SHIFT 0
+#define AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY_MASK 0x000000FF
+#define AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY(__x) \
+	AV8100_VAL2REG(AV8100_FIRMWARE_DOWNLOAD_ENTRY, MBYTE_CODE_ENTRY, __x)
+#define AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY_GET(__x) \
+	AV8100_REG2VAL(AV8100_FIRMWARE_DOWNLOAD_ENTRY, MBYTE_CODE_ENTRY, __x)
diff --git a/drivers/video/av8100/hdmi.c b/drivers/video/av8100/hdmi.c
new file mode 100644
index 00000000000..468ac1e8f99
--- /dev/null
+++ b/drivers/video/av8100/hdmi.c
@@ -0,0 +1,2310 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson HDMI driver
+ *
+ * Author: Per Persson <per.xb.persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/miscdevice.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <video/av8100.h>
+#include <video/hdmi.h>
+#include <linux/poll.h>
+#include <linux/mutex.h>
+#include <linux/ctype.h>
+#include "hdmi_loc.h"
+#include <linux/slab.h>
+#include <linux/sched.h>
+//EROBEMA #include <linux/smp_lock.h>
+
+#define SYSFS_EVENT_FILENAME "evread"
+
+DEFINE_MUTEX(hdmi_events_mutex);
+#define LOCK_HDMI_EVENTS mutex_lock(&hdmi_events_mutex)
+#define UNLOCK_HDMI_EVENTS mutex_unlock(&hdmi_events_mutex)
+#define EVENTS_MASK 0xFF
+
+static int device_open;
+static int events;
+static int events_mask;
+static bool events_received;
+static wait_queue_head_t hdmi_event_wq;
+struct device *hdmidev;
+
+static ssize_t store_storeastext(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_plugdeten(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_edidread(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_edidread(struct device *dev, struct device_attribute *attr,
+				char *buf);
+static ssize_t store_ceceven(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_cecread(struct device *dev, struct device_attribute *attr,
+				char *buf);
+static ssize_t store_cecsend(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_infofrsend(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_hdcpeven(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_hdcpchkaesotp(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_hdcpfuseaes(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_hdcpfuseaes(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_hdcploadaes(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_hdcploadaes(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_hdcpauthencr(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_hdcpauthencr(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t show_hdcpstateget(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t show_evread(struct device *dev, struct device_attribute *attr,
+				char *buf);
+static ssize_t store_evclr(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_audiocfg(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_plugstatus(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_poweronoff(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_poweronoff(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_evwakeup(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static DEVICE_ATTR(storeastext, S_IWUSR, NULL, store_storeastext);
+static DEVICE_ATTR(plugdeten, S_IWUSR, NULL, store_plugdeten);
+static DEVICE_ATTR(edidread, S_IRUGO | S_IWUSR, show_edidread, store_edidread);
+static DEVICE_ATTR(ceceven, S_IWUSR, NULL, store_ceceven);
+static DEVICE_ATTR(cecread, S_IRUGO, show_cecread, NULL);
+static DEVICE_ATTR(cecsend, S_IWUSR, NULL, store_cecsend);
+static DEVICE_ATTR(infofrsend, S_IWUSR, NULL, store_infofrsend);
+static DEVICE_ATTR(hdcpeven, S_IWUSR, NULL, store_hdcpeven);
+static DEVICE_ATTR(hdcpchkaesotp, S_IRUGO, show_hdcpchkaesotp, NULL);
+static DEVICE_ATTR(hdcpfuseaes, S_IRUGO | S_IWUSR, show_hdcpfuseaes,
+		store_hdcpfuseaes);
+static DEVICE_ATTR(hdcploadaes, S_IRUGO | S_IWUSR, show_hdcploadaes,
+		store_hdcploadaes);
+static DEVICE_ATTR(hdcpauthencr, S_IRUGO | S_IWUSR, show_hdcpauthencr,
+		store_hdcpauthencr);
+static DEVICE_ATTR(hdcpstateget, S_IRUGO, show_hdcpstateget, NULL);
+static DEVICE_ATTR(evread, S_IRUGO, show_evread, NULL);
+static DEVICE_ATTR(evclr, S_IWUSR, NULL, store_evclr);
+static DEVICE_ATTR(audiocfg, S_IWUSR, NULL, store_audiocfg);
+static DEVICE_ATTR(plugstatus, S_IRUGO, show_plugstatus, NULL);
+static DEVICE_ATTR(poweronoff, S_IRUGO | S_IWUSR, show_poweronoff,
+		store_poweronoff);
+static DEVICE_ATTR(evwakeup, S_IWUSR, NULL, store_evwakeup);
+
+/* Hex to int conversion */
+static unsigned int htoi(const char *ptr)
+{
+	unsigned int value = 0;
+	char ch = *ptr;
+
+	if (!ptr)
+		return 0;
+
+	if (isdigit(ch))
+		value = ch - '0';
+	else
+		value = toupper(ch) - 'A' + 10;
+
+	value <<= 4;
+	ch = *(++ptr);
+
+	if (isdigit(ch))
+		value += ch - '0';
+	else
+		value += toupper(ch) - 'A' + 10;
+
+	return value;
+}
+
+static int event_enable(bool enable, enum hdmi_event ev)
+{
+	struct kobject *kobj = &hdmidev->kobj;
+
+	dev_dbg(hdmidev, "enable_event %d %02x\n", enable, ev);
+	if (enable)
+		events_mask |= ev;
+	else
+		events_mask &= ~ev;
+
+	if (events & ev) {
+		/* Report pending event */
+		/* Wake up application waiting for event via call to poll() */
+		sysfs_notify(kobj, NULL, SYSFS_EVENT_FILENAME);
+
+		LOCK_HDMI_EVENTS;
+		events_received = true;
+		UNLOCK_HDMI_EVENTS;
+
+		wake_up_interruptible(&hdmi_event_wq);
+	}
+
+	return 0;
+}
+
+static int plugdeten(struct plug_detect *pldet)
+{
+	struct av8100_status status;
+	u8 denc_off_time = 0;
+	int retval;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdmidev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	event_enable(pldet->hdmi_detect_enable != 0,
+		HDMI_EVENT_HDMI_PLUGIN);
+	event_enable(pldet->hdmi_detect_enable != 0,
+		HDMI_EVENT_HDMI_PLUGOUT);
+
+	av8100_reg_hdmi_5_volt_time_r(&denc_off_time, NULL, NULL);
+
+	retval = av8100_reg_hdmi_5_volt_time_w(
+			denc_off_time,
+			pldet->hdmi_off_time,
+			pldet->on_time);
+
+	if (retval) {
+		dev_err(hdmidev, "Failed to write the value to av8100 "
+			"register\n");
+		return -EFAULT;
+	}
+
+	return retval;
+}
+
+static int edidread(struct edid_read *edidread, u8 *len, u8 *data)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdmidev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state < AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(NULL, 0, I2C_INTERFACE) != 0) {
+			dev_err(hdmidev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.edid_section_readback_format.address = edidread->address;
+	config.edid_section_readback_format.block_number = edidread->block_nr;
+
+	dev_dbg(hdmidev, "addr:%0x blnr:%0x",
+		config.edid_section_readback_format.address,
+		config.edid_section_readback_format.block_number);
+
+	if (av8100_conf_prep(AV8100_COMMAND_EDID_SECTION_READBACK,
+		&config) != 0) {
+		dev_err(hdmidev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_EDID_SECTION_READBACK,
+		len, data, I2C_INTERFACE) != 0) {
+		dev_err(hdmidev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(hdmidev, "len:%0x\n", *len);
+
+	return 0;
+}
+
+static int cecread(u8 *src, u8 *dest, u8 *data_len, u8 *data)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+	u8 buf_len;
+	u8 buff[HDMI_CEC_READ_MAXSIZE];
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdmidev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state < AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(NULL, 0, I2C_INTERFACE) !=	0) {
+			dev_err(hdmidev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	if (av8100_conf_prep(AV8100_COMMAND_CEC_MESSAGE_READ_BACK,
+			&config) != 0) {
+		dev_err(hdmidev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_CEC_MESSAGE_READ_BACK,
+		&buf_len, buff, I2C_INTERFACE) != 0) {
+		dev_err(hdmidev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	if (buf_len > 0) {
+		*src = (buff[0] & 0xF0) >> 4;
+		*dest = buff[0] & 0x0F;
+		*data_len = buf_len - 1;
+		memcpy(data, &buff[1], buf_len - 1);
+	} else
+		*data_len = 0;
+
+	return 0;
+}
+
+static int cecsend(u8 src, u8 dest, u8 data_len, u8 *data)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdmidev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state < AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(NULL, 0, I2C_INTERFACE) != 0) {
+			dev_err(hdmidev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.cec_message_write_format.buffer[0] = ((src & 0x0F) << 4) +
+			(dest & 0x0F);
+	config.cec_message_write_format.buffer_length = data_len + 1;
+	memcpy(&config.cec_message_write_format.buffer[1], data, data_len);
+
+	if (av8100_conf_prep(AV8100_COMMAND_CEC_MESSAGE_WRITE,
+		&config) != 0) {
+		dev_err(hdmidev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_CEC_MESSAGE_WRITE,
+		NULL, NULL, I2C_INTERFACE) != 0) {
+		dev_err(hdmidev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int infofrsend(u8 type, u8 version, u8 crc, u8 data_len, u8 *data)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdmidev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state < AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(NULL, 0, I2C_INTERFACE) != 0) {
+			dev_err(hdmidev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	if ((data_len < 1) || (data_len > HDMI_INFOFRAME_MAX_SIZE))
+		return -EINVAL;
+
+	config.infoframes_format.type = type;
+	config.infoframes_format.version = version;
+	config.infoframes_format.crc = crc;
+	config.infoframes_format.length = data_len;
+	memcpy(&config.infoframes_format.data, data, data_len);
+	if (av8100_conf_prep(AV8100_COMMAND_INFOFRAMES,
+		&config) != 0) {
+		dev_err(hdmidev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_INFOFRAMES,
+		NULL, NULL, I2C_INTERFACE) != 0) {
+		dev_err(hdmidev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int hdcpchkaesotp(u8 *crc, u8 *progged)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+	u8 buf_len;
+	u8 buf[2];
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdmidev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state < AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(NULL, 0, I2C_INTERFACE) !=
+			0) {
+			dev_err(hdmidev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.fuse_aes_key_format.fuse_operation = AV8100_FUSE_READ;
+	memset(config.fuse_aes_key_format.key, 0, AV8100_FUSE_KEY_SIZE);
+	if (av8100_conf_prep(AV8100_COMMAND_FUSE_AES_KEY,
+		&config) != 0) {
+		dev_err(hdmidev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_FUSE_AES_KEY,
+		&buf_len, buf, I2C_INTERFACE) != 0) {
+		dev_err(hdmidev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	if (buf_len == 2) {
+		*crc = buf[0];
+		*progged = buf[1];
+	}
+
+	return 0;
+}
+
+static int hdcpfuseaes(u8 *key, u8 crc, u8 *result)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+	u8 buf_len;
+	u8 buf[2];
+
+	/* Default not OK */
+	*result = HDMI_RESULT_NOT_OK;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdmidev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state < AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(NULL, 0, I2C_INTERFACE) !=
+			0) {
+			dev_err(hdmidev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.fuse_aes_key_format.fuse_operation = AV8100_FUSE_WRITE;
+	memcpy(config.fuse_aes_key_format.key, key, AV8100_FUSE_KEY_SIZE);
+	if (av8100_conf_prep(AV8100_COMMAND_FUSE_AES_KEY,
+		&config) != 0) {
+		dev_err(hdmidev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_FUSE_AES_KEY,
+		&buf_len, buf, I2C_INTERFACE) != 0) {
+		dev_err(hdmidev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	if (buf_len == 2) {
+		dev_dbg(hdmidev, "buf[0]:%02x buf[1]:%02x\n", buf[0], buf[1]);
+		if ((crc == buf[0]) && (buf[1] == 1))
+			/* OK */
+			*result = HDMI_RESULT_OK;
+		else
+			*result = HDMI_RESULT_CRC_MISMATCH;
+	}
+
+	return 0;
+}
+
+static int hdcploadaes(u8 block, u8 key_len, u8 *key, u8 *result, u8 *crc32)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+	u8 buf_len;
+	u8 buf[CRC32_SIZE];
+
+	/* Default not OK */
+	*result = HDMI_RESULT_NOT_OK;
+
+	dev_dbg(hdmidev, "%s block:%d\n", __func__, block);
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdmidev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state < AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(NULL, 0, I2C_INTERFACE) != 0) {
+			dev_err(hdmidev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.hdcp_send_key_format.key_number = block;
+	config.hdcp_send_key_format.data_len = key_len;
+	memcpy(config.hdcp_send_key_format.data, key, key_len);
+	if (av8100_conf_prep(AV8100_COMMAND_HDCP_SENDKEY, &config) != 0) {
+		dev_err(hdmidev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_HDCP_SENDKEY,
+		&buf_len, buf, I2C_INTERFACE) != 0) {
+		dev_err(hdmidev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	if ((buf_len == CRC32_SIZE) && (crc32)) {
+		memcpy(crc32, buf, CRC32_SIZE);
+		dev_dbg(hdmidev, "crc32:%02x%02x%02x%02x\n",
+			crc32[0], crc32[1], crc32[2], crc32[3]);
+	}
+
+	*result = HDMI_RESULT_OK;
+
+	return 0;
+}
+
+static int hdcpauthencr(u8 auth_type, u8 encr_type, u8 *len, u8 *data)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdmidev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state < AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(NULL, 0, I2C_INTERFACE) != 0) {
+			dev_err(hdmidev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	switch (auth_type) {
+	case HDMI_HDCP_AUTH_OFF:
+	default:
+		config.hdcp_management_format.req_type =
+				AV8100_HDCP_AUTH_REQ_OFF;
+		break;
+
+	case HDMI_HDCP_AUTH_START:
+		config.hdcp_management_format.req_type =
+				AV8100_HDCP_AUTH_REQ_ON;
+		break;
+
+	case HDMI_HDCP_AUTH_REV_LIST_REQ:
+		config.hdcp_management_format.req_type =
+				AV8100_HDCP_REV_LIST_REQ;
+		break;
+	case HDMI_HDCP_AUTH_CONT:
+		config.hdcp_management_format.req_type =
+				AV8100_HDCP_AUTH_CONT;
+		break;
+	}
+
+	switch (encr_type) {
+	case HDMI_HDCP_ENCR_OESS:
+	default:
+		config.hdcp_management_format.encr_use =
+				AV8100_HDCP_ENCR_USE_OESS;
+		break;
+
+	case HDMI_HDCP_ENCR_EESS:
+		config.hdcp_management_format.encr_use =
+				AV8100_HDCP_ENCR_USE_EESS;
+		break;
+	}
+
+	if (av8100_conf_prep(AV8100_COMMAND_HDCP_MANAGEMENT,
+		&config) != 0) {
+		dev_err(hdmidev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_HDCP_MANAGEMENT,
+		len, data, I2C_INTERFACE) != 0) {
+		dev_err(hdmidev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static u8 events_read(void)
+{
+	int ret;
+
+	LOCK_HDMI_EVENTS;
+	ret = events;
+	dev_dbg(hdmidev, "%s %02x\n", __func__, events);
+	UNLOCK_HDMI_EVENTS;
+
+	return ret;
+}
+
+static int events_clear(u8 ev)
+{
+	dev_dbg(hdmidev, "%s %02x\n", __func__, ev);
+
+	LOCK_HDMI_EVENTS;
+	events &= ~ev & EVENTS_MASK;
+	UNLOCK_HDMI_EVENTS;
+
+	return 0;
+}
+
+static int event_wakeup(void)
+{
+	struct kobject *kobj = &hdmidev->kobj;
+
+	dev_dbg(hdmidev, "%s", __func__);
+
+	LOCK_HDMI_EVENTS;
+	events |= HDMI_EVENT_WAKEUP;
+	events_received = true;
+	UNLOCK_HDMI_EVENTS;
+
+	/* Wake up application waiting for event via call to poll() */
+	sysfs_notify(kobj, NULL, SYSFS_EVENT_FILENAME);
+	wake_up_interruptible(&hdmi_event_wq);
+
+	return 0;
+}
+
+static int audiocfg(struct audio_cfg *cfg)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdmidev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state < AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(NULL, 0, I2C_INTERFACE) != 0) {
+			dev_err(hdmidev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.audio_input_format.audio_input_if_format	= cfg->if_format;
+	config.audio_input_format.i2s_input_nb		= cfg->i2s_entries;
+	config.audio_input_format.sample_audio_freq	= cfg->freq;
+	config.audio_input_format.audio_word_lg		= cfg->word_length;
+	config.audio_input_format.audio_format		= cfg->format;
+	config.audio_input_format.audio_if_mode		= cfg->if_mode;
+	config.audio_input_format.audio_mute		= cfg->mute;
+
+	if (av8100_conf_prep(AV8100_COMMAND_AUDIO_INPUT_FORMAT,
+		&config) != 0) {
+		dev_err(hdmidev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_AUDIO_INPUT_FORMAT,
+		NULL, NULL, I2C_INTERFACE) != 0) {
+		dev_err(hdmidev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* sysfs */
+static ssize_t store_storeastext(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if ((count != HDMI_STOREASTEXT_BIN_SIZE) &&
+		(count != HDMI_STOREASTEXT_TEXT_SIZE) &&
+		(count != HDMI_STOREASTEXT_TEXT_SIZE + 1))
+		return -EINVAL;
+
+	if ((count == HDMI_STOREASTEXT_BIN_SIZE) && (*buf == 0x1))
+		hdmi_driver_data->store_as_hextext = true;
+	else if (((count == HDMI_STOREASTEXT_TEXT_SIZE) ||
+		(count == HDMI_STOREASTEXT_TEXT_SIZE + 1)) && (*buf == '0') &&
+			(*(buf + 1) == '1')) {
+		hdmi_driver_data->store_as_hextext = true;
+	} else {
+		hdmi_driver_data->store_as_hextext = false;
+	}
+
+	dev_dbg(hdmidev, "store_as_hextext:%0d\n",
+		hdmi_driver_data->store_as_hextext);
+
+	return count;
+}
+
+static ssize_t store_plugdeten(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	struct plug_detect plug_detect;
+	int index = 0;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count != HDMI_PLUGDETEN_TEXT_SIZE) &&
+			(count != HDMI_PLUGDETEN_TEXT_SIZE + 1))
+			return -EINVAL;
+		plug_detect.hdmi_detect_enable = htoi(buf + index);
+		index += 2;
+		plug_detect.on_time = htoi(buf + index);
+		index += 2;
+		plug_detect.hdmi_off_time = htoi(buf + index);
+		index += 2;
+	} else {
+		if (count != HDMI_PLUGDETEN_BIN_SIZE)
+			return -EINVAL;
+		plug_detect.hdmi_detect_enable = *(buf + index++);
+		plug_detect.on_time = *(buf + index++);
+		plug_detect.hdmi_off_time = *(buf + index++);
+	}
+
+	if (plugdeten(&plug_detect))
+		return -EINVAL;
+
+	return count;
+}
+
+static ssize_t store_edidread(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	struct edid_read edid_read;
+	int index = 0;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+	dev_dbg(hdmidev, "count:%d\n", count);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count != HDMI_EDIDREAD_TEXT_SIZE) &&
+			(count != HDMI_EDIDREAD_TEXT_SIZE + 1))
+			return -EINVAL;
+		edid_read.address = htoi(buf + index);
+		index += 2;
+		edid_read.block_nr = htoi(buf + index);
+		index += 2;
+	} else {
+		if (count != HDMI_EDIDREAD_BIN_SIZE)
+			return -EINVAL;
+		edid_read.address = *(buf + index++);
+		edid_read.block_nr = *(buf + index++);
+	}
+
+	if (edidread(&edid_read, &hdmi_driver_data->edid_data.buf_len,
+			hdmi_driver_data->edid_data.buf))
+		return -EINVAL;
+
+	return count;
+}
+
+static ssize_t show_edidread(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	int len;
+	int index = 0;
+	int cnt;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	len = hdmi_driver_data->edid_data.buf_len;
+
+	if (hdmi_driver_data->store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", len);
+		index += 2;
+	} else
+		*(buf + index++) = len;
+
+	dev_dbg(hdmidev, "len:%02x\n", len);
+
+	cnt = 0;
+	while (cnt < len) {
+		if (hdmi_driver_data->store_as_hextext) {
+			snprintf(buf + index, 3, "%02x",
+				hdmi_driver_data->edid_data.buf[cnt]);
+			index += 2;
+		} else
+			*(buf + index++) =
+				hdmi_driver_data->edid_data.buf[cnt];
+
+		dev_dbg(hdmidev, "%02x ",
+			hdmi_driver_data->edid_data.buf[cnt]);
+
+		cnt++;
+	}
+
+	if (hdmi_driver_data->store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_ceceven(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	bool enable = false;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count != HDMI_CECEVEN_TEXT_SIZE) &&
+			(count != HDMI_CECEVEN_TEXT_SIZE + 1))
+			return -EINVAL;
+		if ((*buf == '0') && (*(buf + 1) == '1'))
+			enable = true;
+	} else {
+		if (count != HDMI_CECEVEN_BIN_SIZE)
+			return -EINVAL;
+		if (*buf == 0x01)
+			enable = true;
+	}
+
+	event_enable(enable, HDMI_EVENT_CEC | HDMI_EVENT_CECTXERR);
+
+	return count;
+}
+
+static ssize_t show_cecread(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	struct cec_rw cec_read;
+	int index = 0;
+	int cnt;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (cecread(&cec_read.src, &cec_read.dest, &cec_read.length,
+		cec_read.data))
+		return -EINVAL;
+
+	if (hdmi_driver_data->store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", cec_read.src);
+		index += 2;
+		snprintf(buf + index, 3, "%02x", cec_read.dest);
+		index += 2;
+		snprintf(buf + index, 3, "%02x", cec_read.length);
+		index += 2;
+	} else {
+		*(buf + index++) = cec_read.src;
+		*(buf + index++) = cec_read.dest;
+		*(buf + index++) = cec_read.length;
+	}
+
+	dev_dbg(hdmidev, "len:%02x\n", cec_read.length);
+
+	cnt = 0;
+	while (cnt < cec_read.length) {
+		if (hdmi_driver_data->store_as_hextext) {
+			snprintf(buf + index, 3, "%02x", cec_read.data[cnt]);
+			index += 2;
+		} else
+			*(buf + index++) = cec_read.data[cnt];
+
+		dev_dbg(hdmidev, "%02x ", cec_read.data[cnt]);
+
+		cnt++;
+	}
+
+	if (hdmi_driver_data->store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_cecsend(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	struct cec_rw cec_w;
+	int index = 0;
+	int cnt;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count < HDMI_CECSEND_TEXT_SIZE_MIN) ||
+			(count > HDMI_CECSEND_TEXT_SIZE_MAX))
+			return -EINVAL;
+
+		cec_w.src = htoi(buf + index);
+		index += 2;
+		cec_w.dest = htoi(buf + index);
+		index += 2;
+		cec_w.length = htoi(buf + index);
+		index += 2;
+		if (cec_w.length > HDMI_CEC_WRITE_MAXSIZE)
+			return -EINVAL;
+		cnt = 0;
+		while (cnt < cec_w.length) {
+			cec_w.data[cnt] = htoi(buf + index);
+			index += 2;
+			dev_dbg(hdmidev, "%02x ", cec_w.data[cnt]);
+			cnt++;
+		}
+	} else {
+		if ((count < HDMI_CECSEND_BIN_SIZE_MIN) ||
+			(count > HDMI_CECSEND_BIN_SIZE_MAX))
+			return -EINVAL;
+
+		cec_w.src = *(buf + index++);
+		cec_w.dest = *(buf + index++);
+		cec_w.length = *(buf + index++);
+		if (cec_w.length > HDMI_CEC_WRITE_MAXSIZE)
+			return -EINVAL;
+		memcpy(cec_w.data, buf + index, cec_w.length);
+	}
+
+	if (cecsend(cec_w.src,
+			cec_w.dest,
+			cec_w.length,
+			cec_w.data))
+		return -EINVAL;
+
+	return count;
+}
+
+static ssize_t store_infofrsend(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	struct info_fr info_fr;
+	int index = 0;
+	int cnt;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count < HDMI_INFOFRSEND_TEXT_SIZE_MIN) ||
+			(count > HDMI_INFOFRSEND_TEXT_SIZE_MAX))
+			return -EINVAL;
+
+		info_fr.type = htoi(&buf[index]);
+		index += 2;
+		info_fr.ver = htoi(&buf[index]);
+		index += 2;
+		info_fr.crc = htoi(&buf[index]);
+		index += 2;
+		info_fr.length = htoi(&buf[index]);
+		index += 2;
+
+		if (info_fr.length > HDMI_INFOFRAME_MAX_SIZE)
+			return -EINVAL;
+		cnt = 0;
+		while (cnt < info_fr.length) {
+			info_fr.data[cnt] = htoi(buf + index);
+			index += 2;
+			dev_dbg(hdmidev, "%02x ", info_fr.data[cnt]);
+			cnt++;
+		}
+	} else {
+		if ((count < HDMI_INFOFRSEND_BIN_SIZE_MIN) ||
+			(count > HDMI_INFOFRSEND_BIN_SIZE_MAX))
+			return -EINVAL;
+
+		info_fr.type = *(buf + index++);
+		info_fr.ver = *(buf + index++);
+		info_fr.crc = *(buf + index++);
+		info_fr.length = *(buf + index++);
+
+		if (info_fr.length > HDMI_INFOFRAME_MAX_SIZE)
+			return -EINVAL;
+		memcpy(info_fr.data, buf + index, info_fr.length);
+	}
+
+	if (infofrsend(info_fr.type, info_fr.ver, info_fr.crc,
+		info_fr.length, info_fr.data))
+		return -EINVAL;
+
+	return count;
+}
+
+static ssize_t store_hdcpeven(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	bool enable = false;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count != HDMI_HDCPEVEN_TEXT_SIZE) &&
+			(count != HDMI_HDCPEVEN_TEXT_SIZE + 1))
+			return -EINVAL;
+		if ((*buf == '0') && (*(buf + 1) == '1'))
+			enable = true;
+	} else {
+		if (count != HDMI_HDCPEVEN_BIN_SIZE)
+			return -EINVAL;
+		if (*buf == 0x01)
+			enable = true;
+	}
+
+	event_enable(enable, HDMI_EVENT_HDCP);
+
+	return count;
+}
+
+static ssize_t show_hdcpchkaesotp(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	u8 crc;
+	u8 progged;
+	int index = 0;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdcpchkaesotp(&crc, &progged))
+		return -EINVAL;
+
+	if (hdmi_driver_data->store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", progged);
+		index += 2;
+	} else {
+		*(buf + index++) = progged;
+	}
+
+	dev_dbg(hdmidev, "progged:%02x\n", progged);
+
+	if (hdmi_driver_data->store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_hdcpfuseaes(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	struct hdcp_fuseaes hdcp_fuseaes;
+	int index = 0;
+	int cnt;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	/* Default not OK */
+	hdmi_driver_data->fuse_result = HDMI_RESULT_NOT_OK;
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count != HDMI_HDCP_FUSEAES_TEXT_SIZE) &&
+			(count != HDMI_HDCP_FUSEAES_TEXT_SIZE + 1))
+			return -EINVAL;
+
+		cnt = 0;
+		while (cnt < HDMI_HDCP_FUSEAES_KEYSIZE) {
+			hdcp_fuseaes.key[cnt] = htoi(buf + index);
+			index += 2;
+			dev_dbg(hdmidev, "%02x ", hdcp_fuseaes.key[cnt]);
+			cnt++;
+		}
+		hdcp_fuseaes.crc = htoi(&buf[index]);
+		index += 2;
+		dev_dbg(hdmidev, "%02x ", hdcp_fuseaes.crc);
+	} else {
+		if (count != HDMI_HDCP_FUSEAES_BIN_SIZE)
+			return -EINVAL;
+
+		memcpy(hdcp_fuseaes.key, buf + index,
+				HDMI_HDCP_FUSEAES_KEYSIZE);
+		index += HDMI_HDCP_FUSEAES_KEYSIZE;
+		hdcp_fuseaes.crc = *(buf + index++);
+	}
+
+	if (hdcpfuseaes(hdcp_fuseaes.key, hdcp_fuseaes.crc,
+		&hdcp_fuseaes.result))
+		return -EINVAL;
+
+	dev_dbg(hdmidev, "fuseresult:%02x ", hdcp_fuseaes.result);
+
+	hdmi_driver_data->fuse_result = hdcp_fuseaes.result;
+
+	return count;
+}
+
+static ssize_t show_hdcpfuseaes(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	int index = 0;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdmi_driver_data->store_as_hextext) {
+		snprintf(buf + index, 3, "%02x",
+			hdmi_driver_data->fuse_result);
+		index += 2;
+	} else
+		*(buf + index++) = hdmi_driver_data->fuse_result;
+
+	dev_dbg(hdmidev, "status:%02x\n", hdmi_driver_data->fuse_result);
+
+	if (hdmi_driver_data->store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_hdcploadaes(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	struct hdcp_loadaesone hdcp_loadaes;
+	int index = 0;
+	int block_cnt;
+	int cnt;
+	u8 crc32_rcvd[CRC32_SIZE];
+	u8 crc;
+	u8 progged;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	/* Default not OK */
+	hdmi_driver_data->loadaes_result = HDMI_RESULT_NOT_OK;
+
+	if (hdcpchkaesotp(&crc, &progged))
+		return -EINVAL;
+
+	if (!progged) {
+		/* AES is not fused */
+		hdcp_loadaes.result = HDMI_AES_NOT_FUSED;
+		goto store_hdcploadaes_err;
+	}
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count != HDMI_HDCP_LOADAES_TEXT_SIZE) &&
+			(count != HDMI_HDCP_LOADAES_TEXT_SIZE + 1)) {
+			dev_err(hdmidev, "%s", "count mismatch\n");
+			return -EINVAL;
+		}
+
+		/* AES */
+		block_cnt = 0;
+		while (block_cnt < HDMI_HDCP_AES_NR_OF_BLOCKS) {
+			cnt = 0;
+			while (cnt < HDMI_HDCP_AES_KEYSIZE) {
+				hdcp_loadaes.key[cnt] =	htoi(buf + index);
+				index += 2;
+				dev_dbg(hdmidev, "%02x ",
+					hdcp_loadaes.key[cnt]);
+				cnt++;
+			}
+
+			if (hdcploadaes(block_cnt + HDMI_HDCP_AES_BLOCK_START,
+					HDMI_HDCP_AES_KEYSIZE,
+					hdcp_loadaes.key,
+					&hdcp_loadaes.result,
+					crc32_rcvd)) {
+				dev_err(hdmidev, "%s %d\n",
+					"hdcploadaes err aes block",
+					block_cnt + HDMI_HDCP_AES_BLOCK_START);
+				return -EINVAL;
+			}
+
+			if (hdcp_loadaes.result)
+				goto store_hdcploadaes_err;
+
+			block_cnt++;
+		}
+
+		/* KSV */
+		memset(hdcp_loadaes.key, 0, HDMI_HDCP_AES_KSVZEROESSIZE);
+		cnt = HDMI_HDCP_AES_KSVZEROESSIZE;
+		while (cnt < HDMI_HDCP_AES_KSVSIZE +
+				HDMI_HDCP_AES_KSVZEROESSIZE) {
+			hdcp_loadaes.key[cnt] =
+					htoi(&buf[index]);
+			index += 2;
+			dev_dbg(hdmidev, "%02x ", hdcp_loadaes.key[cnt]);
+			cnt++;
+		}
+
+		if (hdcploadaes(HDMI_HDCP_KSV_BLOCK,
+				HDMI_HDCP_AES_KSVSIZE +
+				HDMI_HDCP_AES_KSVZEROESSIZE,
+				hdcp_loadaes.key,
+				&hdcp_loadaes.result,
+				NULL)) {
+			dev_err(hdmidev, "%s %d\n", "hdcploadaes err in ksv\n",
+				block_cnt + HDMI_HDCP_AES_BLOCK_START);
+			return -EINVAL;
+		}
+
+		if (hdcp_loadaes.result)
+			goto store_hdcploadaes_err;
+
+		/* CRC32 */
+		for (cnt = 0; cnt < CRC32_SIZE; cnt++) {
+			hdcp_loadaes.crc32[cnt] = htoi(buf + index);
+			index += 2;
+		}
+
+		if (memcmp(hdcp_loadaes.crc32, crc32_rcvd, CRC32_SIZE)) {
+			dev_dbg(hdmidev, "crc32exp:%02x%02x%02x%02x\n",
+				hdcp_loadaes.crc32[0],
+				hdcp_loadaes.crc32[1],
+				hdcp_loadaes.crc32[2],
+				hdcp_loadaes.crc32[3]);
+			hdcp_loadaes.result = HDMI_RESULT_CRC_MISMATCH;
+			goto store_hdcploadaes_err;
+		}
+	} else {
+		if (count != HDMI_HDCP_LOADAES_BIN_SIZE) {
+			dev_err(hdmidev, "%s", "count mismatch\n");
+			return -EINVAL;
+		}
+
+		/* AES */
+		block_cnt = 0;
+		while (block_cnt < HDMI_HDCP_AES_NR_OF_BLOCKS) {
+			memcpy(hdcp_loadaes.key, buf + index,
+					HDMI_HDCP_AES_KEYSIZE);
+			index += HDMI_HDCP_AES_KEYSIZE;
+
+			if (hdcploadaes(block_cnt + HDMI_HDCP_AES_BLOCK_START,
+					HDMI_HDCP_AES_KEYSIZE,
+					hdcp_loadaes.key,
+					&hdcp_loadaes.result,
+					crc32_rcvd)) {
+				dev_err(hdmidev, "%s %d\n",
+					"hdcploadaes err aes block",
+					block_cnt + HDMI_HDCP_AES_BLOCK_START);
+				return -EINVAL;
+			}
+
+			if (hdcp_loadaes.result)
+				goto store_hdcploadaes_err;
+
+			block_cnt++;
+		}
+
+		/* KSV */
+		memset(hdcp_loadaes.key, 0, HDMI_HDCP_AES_KSVZEROESSIZE);
+		memcpy(hdcp_loadaes.key + HDMI_HDCP_AES_KSVZEROESSIZE,
+				buf + index,
+				HDMI_HDCP_AES_KSVSIZE);
+		index += HDMI_HDCP_AES_KSVSIZE;
+
+		if (hdcploadaes(HDMI_HDCP_KSV_BLOCK,
+				HDMI_HDCP_AES_KSVSIZE +
+				HDMI_HDCP_AES_KSVZEROESSIZE,
+				hdcp_loadaes.key,
+				&hdcp_loadaes.result,
+				NULL)) {
+			dev_err(hdmidev, "%s %d\n", "hdcploadaes err in ksv\n",
+				block_cnt + HDMI_HDCP_AES_BLOCK_START);
+			return -EINVAL;
+		}
+
+		memcpy(hdcp_loadaes.crc32, buf + index, CRC32_SIZE);
+		index += CRC32_SIZE;
+
+		/* CRC32 */
+		if (memcmp(hdcp_loadaes.crc32, crc32_rcvd, CRC32_SIZE)) {
+			dev_dbg(hdmidev, "crc32exp:%02x%02x%02x%02x\n",
+				hdcp_loadaes.crc32[0],
+				hdcp_loadaes.crc32[1],
+				hdcp_loadaes.crc32[2],
+				hdcp_loadaes.crc32[3]);
+			hdcp_loadaes.result = HDMI_RESULT_CRC_MISMATCH;
+		}
+	}
+
+store_hdcploadaes_err:
+	hdmi_driver_data->loadaes_result = hdcp_loadaes.result;
+	return count;
+}
+
+static ssize_t show_hdcploadaes(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	int index = 0;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdmi_driver_data->store_as_hextext) {
+		snprintf(buf + index, 3, "%02x",
+			hdmi_driver_data->loadaes_result);
+		index += 2;
+	} else
+		*(buf + index++) = hdmi_driver_data->loadaes_result;
+
+	dev_dbg(hdmidev, "result:%02x\n", hdmi_driver_data->loadaes_result);
+
+	if (hdmi_driver_data->store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_hdcpauthencr(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	struct hdcp_authencr hdcp_authencr;
+	int index = 0;
+	u8 crc;
+	u8 progged;
+	int result = HDMI_RESULT_OK;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	/* Default not OK */
+	hdmi_driver_data->authencr.result = HDMI_RESULT_NOT_OK;
+
+	if (hdcpchkaesotp(&crc, &progged))
+		return -EINVAL;
+
+	if (!progged) {
+		/* AES is not fused */
+		result = HDMI_AES_NOT_FUSED;
+		goto store_hdcpauthencr_err;
+	}
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count != HDMI_HDCPAUTHENCR_TEXT_SIZE) &&
+			(count != HDMI_HDCPAUTHENCR_TEXT_SIZE + 1))
+			return -EINVAL;
+
+		hdcp_authencr.auth_type = htoi(buf + index);
+		index += 2;
+		hdcp_authencr.encr_type = htoi(buf + index);
+		index += 2;
+	} else {
+		if (count != HDMI_HDCPAUTHENCR_BIN_SIZE)
+			return -EINVAL;
+
+		hdcp_authencr.auth_type = *(buf + index++);
+		hdcp_authencr.encr_type = *(buf + index++);
+	}
+
+	if (hdcpauthencr(hdcp_authencr.auth_type, hdcp_authencr.encr_type,
+		 &hdmi_driver_data->authencr.buf_len,
+		 hdmi_driver_data->authencr.buf))
+		return -EINVAL;
+
+store_hdcpauthencr_err:
+	hdmi_driver_data->authencr.result = result;
+	return count;
+}
+
+static ssize_t show_hdcpauthencr(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	int len;
+	int index = 0;
+	int cnt;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	len = hdmi_driver_data->authencr.buf_len;
+	if (len > AUTH_BUF_LEN)
+		len = AUTH_BUF_LEN;
+
+	if (hdmi_driver_data->store_as_hextext) {
+		snprintf(buf + index, 3, "%02x",
+			hdmi_driver_data->authencr.result);
+		index += 2;
+	} else
+		*(buf + index++) = hdmi_driver_data->authencr.result;
+
+	cnt = 0;
+	while (cnt < len) {
+		if (hdmi_driver_data->store_as_hextext) {
+			snprintf(buf + index, 3, "%02x",
+				hdmi_driver_data->authencr.buf[cnt]);
+			index += 2;
+
+			dev_dbg(hdmidev, "%02x ",
+				hdmi_driver_data->authencr.buf[cnt]);
+
+		} else
+			*(buf + index++) = hdmi_driver_data->authencr.buf[cnt];
+
+		cnt++;
+	}
+
+	dev_dbg(hdmidev, "result:%02x\n", hdmi_driver_data->authencr.result);
+
+	if (hdmi_driver_data->store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t show_hdcpstateget(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	u8 hdcp_state;
+	int index = 0;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (av8100_reg_gen_status_r(NULL, NULL, NULL, NULL, NULL, &hdcp_state))
+			return -EINVAL;
+
+	if (hdmi_driver_data->store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", hdcp_state);
+		index += 2;
+	} else
+		*(buf + index++) = hdcp_state;
+
+	dev_dbg(hdmidev, "status:%02x\n", hdcp_state);
+
+	if (hdmi_driver_data->store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t show_evread(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	int index = 0;
+	u8 ev;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	ev = events_read();
+
+	if (hdmi_driver_data->store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", ev);
+		index += 2;
+	} else
+		*(buf + index++) = ev;
+
+	if (hdmi_driver_data->store_as_hextext)
+		index++;
+
+	/* Events are read: clear events */
+	events_clear(EVENTS_MASK);
+
+	return index;
+}
+
+static ssize_t store_evclr(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	u8 ev;
+	int index = 0;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count != HDMI_EVCLR_TEXT_SIZE) &&
+			(count != HDMI_EVCLR_TEXT_SIZE + 1))
+			return -EINVAL;
+
+		ev = htoi(&buf[index]);
+		index += 2;
+	} else {
+		if (count != HDMI_EVCLR_BIN_SIZE)
+			return -EINVAL;
+
+		ev = *(buf + index++);
+	}
+
+	events_clear(ev);
+
+	return count;
+}
+
+static ssize_t store_audiocfg(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	struct audio_cfg audio_cfg;
+	int index = 0;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count != HDMI_AUDIOCFG_TEXT_SIZE) &&
+			(count != HDMI_AUDIOCFG_TEXT_SIZE + 1))
+			return -EINVAL;
+
+		audio_cfg.if_format = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.i2s_entries = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.freq = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.word_length = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.format = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.if_mode = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.mute = htoi(&buf[index]);
+		index += 2;
+	} else {
+		if (count != HDMI_AUDIOCFG_BIN_SIZE)
+			return -EINVAL;
+
+		audio_cfg.if_format = *(buf + index++);
+		audio_cfg.i2s_entries = *(buf + index++);
+		audio_cfg.freq = *(buf + index++);
+		audio_cfg.word_length = *(buf + index++);
+		audio_cfg.format = *(buf + index++);
+		audio_cfg.if_mode = *(buf + index++);
+		audio_cfg.mute = *(buf + index++);
+	}
+
+	audiocfg(&audio_cfg);
+
+	return count;
+}
+
+static ssize_t show_plugstatus(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	int index = 0;
+	struct av8100_status av8100_status;
+	u8 plstat;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	av8100_status = av8100_status_get();
+	plstat = av8100_status.av8100_plugin_status == AV8100_HDMI_PLUGIN;
+
+	if (hdmi_driver_data->store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", plstat);
+		index += 2;
+	} else
+		*(buf + index++) = plstat;
+
+	if (hdmi_driver_data->store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_poweronoff(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	bool enable = false;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	if (hdmi_driver_data->store_as_hextext) {
+		if ((count != HDMI_POWERONOFF_TEXT_SIZE) &&
+			(count != HDMI_POWERONOFF_TEXT_SIZE + 1))
+			return -EINVAL;
+		if ((*buf == '0') && (*(buf + 1) == '1'))
+			enable = true;
+	} else {
+		if (count != HDMI_POWERONOFF_BIN_SIZE)
+			return -EINVAL;
+		if (*buf == 0x01)
+			enable = true;
+	}
+
+	if (enable == 0) {
+		if (av8100_powerdown() != 0) {
+			dev_err(hdmidev, "av8100_powerdown FAIL\n");
+			return -EINVAL;
+		}
+	} else {
+		if (av8100_powerup() != 0) {
+			dev_err(hdmidev, "av8100_powerup FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	return count;
+}
+
+static ssize_t show_poweronoff(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+	int index = 0;
+	struct av8100_status status;
+	u8 power_state;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	hdmi_driver_data = dev_get_drvdata(dev);
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_SCAN)
+		power_state = 0;
+	else
+		power_state = 1;
+
+	if (hdmi_driver_data->store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", power_state);
+		index += 3;
+	} else {
+		*(buf + index++) = power_state;
+	}
+
+	return index;
+}
+
+static ssize_t store_evwakeup(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	event_wakeup();
+
+	return count;
+}
+
+static int hdmi_open(struct inode *inode, struct file *filp)
+{
+	if (device_open)
+		return -EBUSY;
+
+	device_open++;
+
+	return 0;
+}
+
+static int hdmi_release(struct inode *inode, struct file *filp)
+{
+	if (device_open)
+		device_open--;
+
+	return 0;
+}
+
+/* ioctl */
+static int hdmi_ioctl(struct file *file,
+		       unsigned int cmd, unsigned long arg)
+{
+	u8 value = 0;
+	struct plug_detect plug_detect;
+	struct edid_read edid_read;
+	struct cec_rw cec_read;
+	struct cec_rw cec_send;
+	struct info_fr info_fr;
+	struct hdcp_fuseaes hdcp_fuseaes;
+	struct hdcp_loadaesall hdcp_loadaesall;
+	int block_cnt;
+	struct hdcp_loadaesone hdcp_loadaesone;
+	struct hdcp_authencr hdcp_authencr;
+	struct audio_cfg audio_cfg;
+	union av8100_configuration config;
+	struct hdmi_register reg;
+	struct hdmi_command_register command_reg;
+	struct av8100_status status;
+	u8 aes_status;
+
+	switch (cmd) {
+	case IOC_PLUG_DETECT_ENABLE:
+		if (copy_from_user(&plug_detect, (void *)arg,
+			sizeof(struct plug_detect)))
+			return -EINVAL;
+
+		if (plugdeten(&plug_detect))
+			return -EINVAL;
+		break;
+
+	case IOC_EDID_READ:
+		if (copy_from_user(&edid_read, (void *)arg,
+				sizeof(struct edid_read)))
+			return -EINVAL;
+
+		if (edidread(&edid_read, &edid_read.data_length,
+				edid_read.data))
+			return -EINVAL;
+
+		if (copy_to_user((void *)arg, (void *)&edid_read,
+			sizeof(struct edid_read))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_CEC_EVENT_ENABLE:
+		if (copy_from_user(&value, (void *)arg, sizeof(u8)))
+			return -EINVAL;
+
+		event_enable(value != 0, HDMI_EVENT_CEC | HDMI_EVENT_CECTXERR);
+		break;
+
+	case IOC_CEC_READ:
+		if (cecread(&cec_read.src, &cec_read.dest, &cec_read.length,
+			cec_read.data))
+			return -EINVAL;
+
+		if (copy_to_user((void *)arg, (void *)&cec_read,
+			sizeof(struct cec_rw))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_CEC_SEND:
+		if (copy_from_user(&cec_send, (void *)arg,
+				sizeof(struct cec_rw)))
+			return -EINVAL;
+
+		if (cecsend(cec_send.src,
+				cec_send.dest,
+				cec_send.length,
+				cec_send.data))
+			return -EINVAL;
+		break;
+
+	case IOC_INFOFRAME_SEND:
+		if (copy_from_user(&info_fr, (void *)arg,
+				sizeof(struct info_fr)))
+			return -EINVAL;
+
+		if (infofrsend(info_fr.type, info_fr.ver, info_fr.crc,
+			info_fr.length, info_fr.data))
+			return -EINVAL;
+		break;
+
+	case IOC_HDCP_EVENT_ENABLE:
+		if (copy_from_user(&value, (void *)arg, sizeof(u8)))
+			return -EINVAL;
+
+		event_enable(value != 0, HDMI_EVENT_HDCP);
+		break;
+
+	case IOC_HDCP_CHKAESOTP:
+		if (hdcpchkaesotp(&value, &aes_status))
+			return -EINVAL;
+
+		if (copy_to_user((void *)arg, (void *)&aes_status,
+			sizeof(u8))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDCP_FUSEAES:
+		if (copy_from_user(&hdcp_fuseaes, (void *)arg,
+				sizeof(struct hdcp_fuseaes)))
+			return -EINVAL;
+
+		if (hdcpfuseaes(hdcp_fuseaes.key, hdcp_fuseaes.crc,
+				&hdcp_fuseaes.result))
+				return -EINVAL;
+
+		if (copy_to_user((void *)arg, (void *)&hdcp_fuseaes,
+			sizeof(struct hdcp_fuseaes))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDCP_LOADAES:
+		if (copy_from_user(&hdcp_loadaesall, (void *)arg,
+				sizeof(struct hdcp_loadaesall)))
+			return -EINVAL;
+
+		if (hdcpchkaesotp(&value, &aes_status))
+			return -EINVAL;
+
+		if (!aes_status) {
+			/* AES is not fused */
+			hdcp_loadaesone.result = HDMI_AES_NOT_FUSED;
+			goto ioc_hdcploadaes_err;
+		}
+
+		/* AES */
+		block_cnt = 0;
+		while (block_cnt < HDMI_HDCP_AES_NR_OF_BLOCKS) {
+			memcpy(hdcp_loadaesone.key, hdcp_loadaesall.key +
+					block_cnt * HDMI_HDCP_AES_KEYSIZE,
+					HDMI_HDCP_AES_KEYSIZE);
+
+			if (hdcploadaes(block_cnt + HDMI_HDCP_AES_BLOCK_START,
+					HDMI_HDCP_AES_KEYSIZE,
+					hdcp_loadaesone.key,
+					&hdcp_loadaesone.result,
+					hdcp_loadaesone.crc32))
+				return -EINVAL;
+
+			if (hdcp_loadaesone.result)
+				return -EINVAL;
+
+			block_cnt++;
+		}
+
+		/* KSV */
+		memset(hdcp_loadaesone.key, 0, HDMI_HDCP_AES_KSVZEROESSIZE);
+		memcpy(hdcp_loadaesone.key + HDMI_HDCP_AES_KSVZEROESSIZE,
+				hdcp_loadaesall.ksv, HDMI_HDCP_AES_KSVSIZE);
+
+		if (hdcploadaes(HDMI_HDCP_KSV_BLOCK,
+				HDMI_HDCP_AES_KSVSIZE +
+				HDMI_HDCP_AES_KSVZEROESSIZE,
+				hdcp_loadaesone.key,
+				&hdcp_loadaesone.result,
+				NULL))
+			return -EINVAL;
+
+		if (hdcp_loadaesone.result)
+			return -EINVAL;
+
+		/* CRC32 */
+		if (memcmp(hdcp_loadaesall.crc32, hdcp_loadaesone.crc32,
+				CRC32_SIZE)) {
+			dev_dbg(hdmidev, "crc32exp:%02x%02x%02x%02x\n",
+				hdcp_loadaesall.crc32[0],
+				hdcp_loadaesall.crc32[1],
+				hdcp_loadaesall.crc32[2],
+				hdcp_loadaesall.crc32[3]);
+			hdcp_loadaesone.result = HDMI_RESULT_CRC_MISMATCH;
+			goto ioc_hdcploadaes_err;
+		}
+
+ioc_hdcploadaes_err:
+		hdcp_loadaesall.result = hdcp_loadaesone.result;
+
+		if (copy_to_user((void *)arg, (void *)&hdcp_loadaesall,
+			sizeof(struct hdcp_loadaesall))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDCP_AUTHENCR_REQ:
+		if (copy_from_user(&hdcp_authencr, (void *)arg,
+				sizeof(struct hdcp_authencr)))
+			return -EINVAL;
+
+		/* Default not OK */
+		hdcp_authencr.result = HDMI_RESULT_NOT_OK;
+
+		if (hdcpchkaesotp(&value, &aes_status))
+			return -EINVAL;
+
+		if (!aes_status) {
+			/* AES is not fused */
+			hdcp_authencr.result = HDMI_AES_NOT_FUSED;
+			break;
+		}
+
+		if (hdcpauthencr(hdcp_authencr.auth_type,
+				hdcp_authencr.encr_type,
+				&value,
+				hdcp_authencr.revoc_list))
+			return -EINVAL;
+
+		hdcp_authencr.result = HDMI_RESULT_OK;
+		break;
+
+	case IOC_HDCP_STATE_GET:
+		if (av8100_reg_gen_status_r(NULL, NULL, NULL, NULL, NULL,
+				&value))
+				return -EINVAL;
+
+		if (copy_to_user((void *)arg, (void *)&value,
+			sizeof(u8))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_EVENTS_READ:
+		value = events_read();
+
+		if (copy_to_user((void *)arg, (void *)&value,
+			sizeof(u8))) {
+			return -EINVAL;
+		}
+
+		/* Events are read: clear events */
+		events_clear(EVENTS_MASK);
+		break;
+
+	case IOC_EVENTS_CLEAR:
+		if (copy_from_user(&value, (void *)arg, sizeof(u8)))
+			return -EINVAL;
+
+		events_clear(value);
+		break;
+
+	case IOC_AUDIO_CFG:
+		if (copy_from_user(&audio_cfg, (void *)arg,
+				sizeof(struct audio_cfg)))
+			return -EINVAL;
+
+		audiocfg(&audio_cfg);
+		break;
+
+	case IOC_PLUG_STATUS:
+		status = av8100_status_get();
+		value = status.av8100_plugin_status == AV8100_HDMI_PLUGIN;
+
+		if (copy_to_user((void *)arg, (void *)&value,
+			sizeof(u8))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_POWERONOFF:
+		/* Get desired power state on or off */
+		if (copy_from_user(&value, (void *)arg, sizeof(u8)))
+			return -EINVAL;
+
+		if (value == 0) {
+			if (av8100_powerdown() != 0) {
+				dev_err(hdmidev, "av8100_powerdown FAIL\n");
+				return -EINVAL;
+			}
+		} else {
+			if (av8100_powerup() != 0) {
+				dev_err(hdmidev, "av8100_powerup FAIL\n");
+				return -EINVAL;
+			}
+		}
+		break;
+
+	case IOC_EVENT_WAKEUP:
+		/* Trigger event */
+		event_wakeup();
+		break;
+
+	case IOC_POWERSTATE:
+		status = av8100_status_get();
+		value = status.av8100_state >= AV8100_OPMODE_SCAN;
+
+		if (copy_to_user((void *)arg, (void *)&value,
+						sizeof(u8))) {
+			return -EINVAL;
+		}
+		break;
+
+	/* Internal */
+	case IOC_HDMI_ENABLE_INTERRUPTS:
+		av8100_disable_interrupt();
+		if (av8100_enable_interrupt() != 0) {
+			dev_err(hdmidev, "av8100_conf_get FAIL\n");
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDMI_DOWNLOAD_FW:
+		if (av8100_download_firmware(NULL, 0, I2C_INTERFACE) != 0) {
+			dev_err(hdmidev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDMI_ONOFF:
+		/* Get desired HDMI mode on or off */
+		if (copy_from_user(&value, (void *)arg, sizeof(u8)))
+			return -EFAULT;
+
+		if (av8100_conf_get(AV8100_COMMAND_HDMI, &config) != 0) {
+			dev_err(hdmidev, "av8100_conf_get FAIL\n");
+			return -EINVAL;
+		}
+		if (value == 0)
+			config.hdmi_format.hdmi_mode = AV8100_HDMI_OFF;
+		else
+			config.hdmi_format.hdmi_mode = AV8100_HDMI_ON;
+
+		if (av8100_conf_prep(AV8100_COMMAND_HDMI, &config) != 0) {
+			dev_err(hdmidev, "av8100_conf_prep FAIL\n");
+			return -EINVAL;
+		}
+		if (av8100_conf_w(AV8100_COMMAND_HDMI, NULL, NULL,
+			I2C_INTERFACE) != 0) {
+			dev_err(hdmidev, "av8100_conf_w FAIL\n");
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDMI_REGISTER_WRITE:
+		if (copy_from_user(&reg, (void *)arg,
+			sizeof(struct hdmi_register))) {
+			return -EINVAL;
+		}
+
+		if (av8100_reg_w(reg.offset, reg.value) != 0) {
+			dev_err(hdmidev, "hdmi_register_write FAIL\n");
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDMI_REGISTER_READ:
+		if (copy_from_user(&reg, (void *)arg,
+			sizeof(struct hdmi_register))) {
+			return -EINVAL;
+		}
+
+		if (av8100_reg_r(reg.offset, &reg.value) != 0) {
+			dev_err(hdmidev, "hdmi_register_write FAIL\n");
+			return -EINVAL;
+		}
+
+		if (copy_to_user((void *)arg, (void *)&reg,
+			sizeof(struct hdmi_register))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDMI_STATUS_GET:
+		status = av8100_status_get();
+
+		if (copy_to_user((void *)arg, (void *)&status,
+			sizeof(struct av8100_status))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDMI_CONFIGURATION_WRITE:
+		if (copy_from_user(&command_reg, (void *)arg,
+				sizeof(struct hdmi_command_register)) != 0) {
+			dev_err(hdmidev, "IOC_HDMI_CONFIGURATION_WRITE "
+				"fail 1\n");
+			command_reg.return_status = EINVAL;
+		} else {
+			command_reg.return_status = 0;
+			if (av8100_conf_w_raw(command_reg.cmd_id,
+					command_reg.buf_len,
+					command_reg.buf,
+					&(command_reg.buf_len),
+					command_reg.buf) != 0) {
+				dev_err(hdmidev, "IOC_HDMI_CONFIGURATION_WRITE "
+					"fail 2\n");
+				command_reg.return_status = EINVAL;
+			}
+		}
+
+		if (copy_to_user((void *)arg, (void *)&command_reg,
+			sizeof(struct hdmi_command_register)) != 0) {
+			return -EINVAL;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static long hdmi_unlocked_ioctl(struct file *file, unsigned int cmd, 
+							unsigned long arg)
+{
+	int ret;
+
+	//EROBEMA: lock_kernel();
+	ret = hdmi_ioctl(file, cmd, arg);
+	//EROBEMA: unlock_kernel();
+
+	return ret;
+}
+
+static unsigned int
+hdmi_poll(struct file *filp, poll_table *wait)
+{
+	unsigned int mask = 0;
+
+	dev_dbg(hdmidev, "%s\n", __func__);
+
+	poll_wait(filp, &hdmi_event_wq , wait);
+
+	LOCK_HDMI_EVENTS;
+	if (events_received == true) {
+		events_received = false;
+		mask = POLLIN | POLLRDNORM;
+	}
+	UNLOCK_HDMI_EVENTS;
+
+	return mask;
+}
+
+static const struct file_operations hdmi_fops = {
+	.owner =    THIS_MODULE,
+	.open =     hdmi_open,
+	.release =  hdmi_release,
+	.unlocked_ioctl = hdmi_unlocked_ioctl,
+	.poll = hdmi_poll
+};
+
+static struct miscdevice hdmi_miscdev = {
+	MISC_DYNAMIC_MINOR,
+	"hdmi",
+	&hdmi_fops
+};
+
+/* Event callback function called by hw driver */
+void hdmi_event(enum av8100_hdmi_event ev)
+{
+	int events_old;
+	int events_new;
+	struct kobject *kobj = &hdmidev->kobj;
+
+	dev_dbg(hdmidev, "hdmi_event %02x\n", ev);
+
+	LOCK_HDMI_EVENTS;
+
+	events_old = events;
+
+	/* Set event */
+	switch (ev) {
+	case AV8100_HDMI_EVENT_HDMI_PLUGIN:
+		events &= ~HDMI_EVENT_HDMI_PLUGOUT;
+		events |= HDMI_EVENT_HDMI_PLUGIN;
+		break;
+
+	case AV8100_HDMI_EVENT_HDMI_PLUGOUT:
+		events &= ~HDMI_EVENT_HDMI_PLUGIN;
+		events |= HDMI_EVENT_HDMI_PLUGOUT;
+		break;
+
+	case AV8100_HDMI_EVENT_CEC:
+		events |= HDMI_EVENT_CEC;
+		break;
+
+	case AV8100_HDMI_EVENT_HDCP:
+		events |= HDMI_EVENT_HDCP;
+		break;
+
+	case AV8100_HDMI_EVENT_CECTXERR:
+		events |= HDMI_EVENT_CECTXERR;
+		break;
+
+	default:
+		break;
+	}
+
+	events_new = events_mask & events;
+
+	UNLOCK_HDMI_EVENTS;
+
+	dev_dbg(hdmidev, "hdmi events:%02x, events_old:%02x mask:%02x\n",
+			events_new, events_old, events_mask);
+
+	if (events_new != events_old) {
+		/* Wake up application waiting for event via call to poll() */
+		sysfs_notify(kobj, NULL, SYSFS_EVENT_FILENAME);
+
+		LOCK_HDMI_EVENTS;
+		events_received = true;
+		UNLOCK_HDMI_EVENTS;
+
+		wake_up_interruptible(&hdmi_event_wq);
+	}
+}
+EXPORT_SYMBOL(hdmi_event);
+
+int __init hdmi_init(void)
+{
+	int ret;
+	struct hdmi_driver_data *hdmi_driver_data;
+
+	ret = misc_register(&hdmi_miscdev);
+	if (ret)
+		goto hdmi_init_out;
+
+	hdmidev = hdmi_miscdev.this_device;
+
+	hdmi_driver_data =
+		kzalloc(sizeof(struct hdmi_driver_data), GFP_KERNEL);
+
+	if (!hdmi_driver_data)
+		return -ENOMEM;
+
+	dev_set_drvdata(hdmidev, hdmi_driver_data);
+
+	/* Default sysfs file format is hextext */
+	hdmi_driver_data->store_as_hextext = true;
+
+	init_waitqueue_head(&hdmi_event_wq);
+
+	if (device_create_file(hdmidev, &dev_attr_storeastext))
+		dev_info(hdmidev, "Unable to create storeastext attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_plugdeten))
+		dev_info(hdmidev, "Unable to create plugdeten attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_edidread))
+		dev_info(hdmidev, "Unable to create edidread attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_ceceven))
+		dev_info(hdmidev, "Unable to create ceceven attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_cecread))
+		dev_info(hdmidev, "Unable to create cecread attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_cecsend))
+		dev_info(hdmidev, "Unable to create cecsend attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_infofrsend))
+		dev_info(hdmidev, "Unable to create infofrsend attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_hdcpeven))
+		dev_info(hdmidev, "Unable to create hdcpeven attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_hdcpchkaesotp))
+		dev_info(hdmidev, "Unable to create hdcpchkaesotp attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_hdcpfuseaes))
+		dev_info(hdmidev, "Unable to create hdcpfuseaes attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_hdcploadaes))
+		dev_info(hdmidev, "Unable to create hdcploadaes attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_hdcpauthencr))
+		dev_info(hdmidev, "Unable to create hdcpauthreq attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_hdcpstateget))
+		dev_info(hdmidev, "Unable to create hdcpstateget attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_evread))
+		dev_info(hdmidev, "Unable to create evread attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_evclr))
+		dev_info(hdmidev, "Unable to create evclr attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_audiocfg))
+		dev_info(hdmidev, "Unable to create audiocfg attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_plugstatus))
+		dev_info(hdmidev, "Unable to create plugstatus attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_poweronoff))
+		dev_info(hdmidev, "Unable to create poweronoff attribute\n");
+	if (device_create_file(hdmidev, &dev_attr_evwakeup))
+		dev_info(hdmidev, "Unable to create evwakeup attribute\n");
+
+	/* Register event callback */
+	av8100_hdmi_event_cb_set(hdmi_event);
+
+hdmi_init_out:
+	return ret;
+}
+late_initcall(hdmi_init);
+
+void hdmi_exit(void)
+{
+	struct hdmi_driver_data *hdmi_driver_data;
+
+	/* Deregister event callback */
+	av8100_hdmi_event_cb_set(NULL);
+
+	device_remove_file(hdmidev, &dev_attr_storeastext);
+	device_remove_file(hdmidev, &dev_attr_plugdeten);
+	device_remove_file(hdmidev, &dev_attr_edidread);
+	device_remove_file(hdmidev, &dev_attr_ceceven);
+	device_remove_file(hdmidev, &dev_attr_cecread);
+	device_remove_file(hdmidev, &dev_attr_cecsend);
+	device_remove_file(hdmidev, &dev_attr_infofrsend);
+	device_remove_file(hdmidev, &dev_attr_hdcpeven);
+	device_remove_file(hdmidev, &dev_attr_hdcpchkaesotp);
+	device_remove_file(hdmidev, &dev_attr_hdcpfuseaes);
+	device_remove_file(hdmidev, &dev_attr_hdcploadaes);
+	device_remove_file(hdmidev, &dev_attr_hdcpauthencr);
+	device_remove_file(hdmidev, &dev_attr_hdcpstateget);
+	device_remove_file(hdmidev, &dev_attr_evread);
+	device_remove_file(hdmidev, &dev_attr_evclr);
+	device_remove_file(hdmidev, &dev_attr_audiocfg);
+	device_remove_file(hdmidev, &dev_attr_plugstatus);
+	device_remove_file(hdmidev, &dev_attr_poweronoff);
+	device_remove_file(hdmidev, &dev_attr_evwakeup);
+
+	hdmi_driver_data = dev_get_drvdata(hdmidev);
+	kfree(hdmi_driver_data);
+
+	misc_deregister(&hdmi_miscdev);
+}
diff --git a/drivers/video/av8100/hdmi_loc.h b/drivers/video/av8100/hdmi_loc.h
new file mode 100644
index 00000000000..5db30e7526b
--- /dev/null
+++ b/drivers/video/av8100/hdmi_loc.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * Author: Per Persson <per.xb.persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __HDMI_LOC__H__
+#define __HDMI_LOC__H__
+
+#define EDID_BUF_LEN	128
+#define COMMAND_BUF_LEN	128
+#define AES_KEY_SIZE	16
+#define CRC32_SIZE	4
+#define AUTH_BUF_LEN	30
+
+struct edid_data {
+	u8 buf_len;
+	u8 buf[EDID_BUF_LEN];
+};
+
+struct authencr {
+	int result;
+	u8 buf_len;
+	u8 buf[AUTH_BUF_LEN];
+};
+
+struct hdmi_register {
+	unsigned char value;
+	unsigned char offset;
+};
+
+struct hdcp_loadaesone {
+	u8 key[AES_KEY_SIZE];
+	u8 result;
+	u8 crc32[CRC32_SIZE];
+};
+
+struct hdmi_driver_data {
+	bool			store_as_hextext;
+	struct plug_detect	plug_detect;
+	bool			enable_cec_event;
+	struct edid_data	edid_data;
+	struct cec_rw		cec_read;
+	bool			fuse_result;
+	int			loadaes_result;
+	struct authencr         authencr;
+};
+
+struct hdmi_command_register {
+	unsigned char cmd_id;			/* input */
+	unsigned char buf_len;			/* input, output */
+	unsigned char buf[COMMAND_BUF_LEN];	/* input, output */
+	unsigned char return_status;		/* output */
+};
+
+/* Internal */
+#define IOC_HDMI_ENABLE_INTERRUPTS	_IOWR(HDMI_IOC_MAGIC, 32, int)
+#define IOC_HDMI_DOWNLOAD_FW		_IOWR(HDMI_IOC_MAGIC, 33, int)
+#define IOC_HDMI_ONOFF			_IOWR(HDMI_IOC_MAGIC, 34, int)
+#define IOC_HDMI_REGISTER_WRITE		_IOWR(HDMI_IOC_MAGIC, 35, int)
+#define IOC_HDMI_REGISTER_READ		_IOWR(HDMI_IOC_MAGIC, 36, int)
+#define IOC_HDMI_STATUS_GET		_IOWR(HDMI_IOC_MAGIC, 37, int)
+#define IOC_HDMI_CONFIGURATION_WRITE	_IOWR(HDMI_IOC_MAGIC, 38, int)
+
+#endif /* __HDMI_LOC__H__ */
diff --git a/drivers/video/b2r2/Kconfig b/drivers/video/b2r2/Kconfig
new file mode 100644
index 00000000000..8cc81876de7
--- /dev/null
+++ b/drivers/video/b2r2/Kconfig
@@ -0,0 +1,134 @@
+config FB_B2R2
+	tristate "B2R2 engine support"
+	default n
+	help
+	  B2R2 engine does various bit-blitting operations,post-processor operations
+	  and various compositions.
+
+config B2R2_PLUG_CONF
+	bool "B2R2 bus plug configuration"
+	depends on FB_B2R2
+	default n
+	help
+	  Configures how B2R2 access the memory bus. Enabling this will increase
+	  the performance of B2R2 at the cost of using the bus more heavily.
+
+	  If this is set to 'n', the hardware defaults will be used.
+
+choice
+	prompt "Opcode size"
+	depends on B2R2_PLUG_CONF
+	default B2R2_OPSIZE_64
+
+	config B2R2_OPSIZE_8
+		bool "8 bytes"
+	config B2R2_OPSIZE_16
+		bool "16 bytes"
+	config B2R2_OPSIZE_32
+		bool "32 bytes"
+	config B2R2_OPSIZE_64
+		bool "64 bytes"
+
+endchoice
+
+choice
+	prompt "Chunk size"
+	depends on B2R2_PLUG_CONF
+	default B2R2_CHSIZE_128
+
+	config B2R2_CHSIZE_1
+		bool "1 op"
+	config B2R2_CHSIZE_2
+		bool "2 ops"
+	config B2R2_CHSIZE_4
+		bool "4 ops"
+	config B2R2_CHSIZE_8
+		bool "8 ops"
+	config B2R2_CHSIZE_16
+		bool "16 ops"
+	config B2R2_CHSIZE_32
+		bool "32 ops"
+	config B2R2_CHSIZE_64
+		bool "64 ops"
+	config B2R2_CHSIZE_128
+		bool "128 ops"
+endchoice
+
+choice
+	prompt "Message size"
+	depends on B2R2_PLUG_CONF
+	default B2R2_MGSIZE_128
+
+	config B2R2_MGSIZE_1
+		bool "1 chunk"
+	config B2R2_MGSIZE_2
+		bool "2 chunks"
+	config B2R2_MGSIZE_4
+		bool "4 chunks"
+	config B2R2_MGSIZE_8
+		bool "8 s"
+	config B2R2_MGSIZE_16
+		bool "16 chunks"
+	config B2R2_MGSIZE_32
+		bool "32 chunks"
+	config B2R2_MGSIZE_64
+		bool "64 chunks"
+	config B2R2_MGSIZE_128
+		bool "128 chunks"
+endchoice
+
+choice
+	prompt "Page size"
+	depends on B2R2_PLUG_CONF
+	default B2R2_PGSIZE_256
+
+	config B2R2_PGSIZE_64
+		bool "64 bytes"
+	config B2R2_PGSIZE_128
+		bool "128 bytes"
+	config B2R2_PGSIZE_256
+		bool "256 bytes"
+endchoice
+
+config B2R2_DEBUG
+	bool "B2R2 debugging"
+	default n
+	depends on FB_B2R2
+	help
+	  Enable debugging features for the B2R2 driver.
+
+config B2R2_PROFILER
+	tristate "B2R2 profiler"
+	default n
+	depends on FB_B2R2
+	help
+	  Enables the profiler for the B2R2 driver.
+
+	  It is recommended to build this as a module, since the configuration
+	  of filters etc. is done at load time.
+
+config B2R2_GENERIC
+	bool "B2R2 generic path"
+	default y
+	depends on FB_B2R2
+	help
+	  Enables support for the generic path in the B2R2 driver. This path should
+	  be used when there is no optimized implementation for a request.
+
+choice
+	prompt "Generic usage mode"
+	depends on B2R2_GENERIC
+	default B2R2_GENERIC_FALLBACK
+
+	config B2R2_GENERIC_FALLBACK
+		bool "Fallback"
+		help
+		  The optimized path will be used for all supported operations, and the
+		  generic path will be used as a fallback for the ones not implemented.
+
+	config B2R2_GENERIC_ONLY
+		bool "Always"
+		help
+		  The generic path will be used for all operations.
+
+endchoice
diff --git a/drivers/video/b2r2/Makefile b/drivers/video/b2r2/Makefile
new file mode 100644
index 00000000000..0150ad6f761
--- /dev/null
+++ b/drivers/video/b2r2/Makefile
@@ -0,0 +1,15 @@
+# Make file for compiling and loadable module B2R2
+
+obj-$(CONFIG_FB_B2R2) += b2r2.o
+
+b2r2-objs = b2r2_blt_main.o b2r2_core.o b2r2_mem_alloc.o b2r2_generic.o b2r2_node_gen.o b2r2_node_split.o b2r2_profiler_socket.o b2r2_timing.o b2r2_filters.o b2r2_utils.o b2r2_input_validation.o
+
+ifdef CONFIG_B2R2_DEBUG
+b2r2-objs += b2r2_debug.o
+endif
+
+ifeq ($(CONFIG_FB_B2R2),m)
+obj-y += b2r2_kernel_if.o
+endif
+
+obj-$(CONFIG_B2R2_PROFILER) += b2r2_profiler/
diff --git a/drivers/video/b2r2/b2r2_blt_main.c b/drivers/video/b2r2/b2r2_blt_main.c
new file mode 100644
index 00000000000..a2b21f4a2c1
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_blt_main.c
@@ -0,0 +1,3283 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 Blitter module
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/list.h>
+#ifdef CONFIG_ANDROID_PMEM
+#include <linux/android_pmem.h>
+#endif
+#include <linux/fb.h>
+#include <linux/uaccess.h>
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#endif
+#include <asm/cacheflush.h>
+#include <linux/smp.h>
+#include <linux/dma-mapping.h>
+#include <linux/sched.h>
+#include <linux/err.h>
+#include <linux/hwmem.h>
+
+#include "b2r2_internal.h"
+#include "b2r2_node_split.h"
+#include "b2r2_generic.h"
+#include "b2r2_mem_alloc.h"
+#include "b2r2_profiler_socket.h"
+#include "b2r2_timing.h"
+#include "b2r2_debug.h"
+#include "b2r2_utils.h"
+#include "b2r2_input_validation.h"
+
+#define B2R2_HEAP_SIZE (4 * PAGE_SIZE)
+#define MAX_TMP_BUF_SIZE (128 * PAGE_SIZE)
+
+/*
+ * TODO:
+ * Implementation of query cap
+ * Support for user space virtual pointer to physically consecutive memory
+ * Support for user space virtual pointer to physically scattered memory
+ * Callback reads lagging behind in blt_api_stress app
+ * Store smaller items in the report list instead of the whole request
+ * Support read of many report records at once.
+ */
+
+/**
+ * b2r2_blt_dev - Our device, /dev/b2r2_blt
+ */
+static struct miscdevice *b2r2_blt_dev;
+
+static struct {
+	struct b2r2_work_buf buf;
+	bool in_use;
+} tmp_bufs[MAX_TMP_BUFS_NEEDED];
+
+/* Statistics */
+
+/**
+ * stat_lock - Spin lock protecting the statistics
+ */
+static struct mutex   stat_lock;
+/**
+ * stat_n_jobs_added - Number of jobs added to b2r2_core
+ */
+static unsigned long stat_n_jobs_added;
+/**
+ * stat_n_jobs_released - Number of jobs released (job_release called)
+ */
+static unsigned long stat_n_jobs_released;
+/**
+ * stat_n_jobs_in_report_list - Number of jobs currently in the report list
+ */
+static unsigned long stat_n_jobs_in_report_list;
+/**
+ * stat_n_in_blt - Number of client threads currently exec inside b2r2_blt()
+ */
+static unsigned long stat_n_in_blt;
+/**
+ * stat_n_in_blt_synch - Nunmber of client threads currently waiting for synch
+ */
+static unsigned long stat_n_in_blt_synch;
+/**
+ * stat_n_in_blt_add - Number of client threads currenlty adding in b2r2_blt
+ */
+static unsigned long stat_n_in_blt_add;
+/**
+ * stat_n_in_blt_wait - Number of client threads currently waiting in b2r2_blt
+ */
+static unsigned long stat_n_in_blt_wait;
+/**
+ * stat_n_in_sync_0 - Number of client threads currently in b2r2_blt_sync
+ *                    waiting for all client jobs to finish
+ */
+static unsigned long stat_n_in_synch_0;
+/**
+ * stat_n_in_sync_job - Number of client threads currently in b2r2_blt_sync
+ *                      waiting specific job to finish
+ */
+static unsigned long stat_n_in_synch_job;
+/**
+ * stat_n_in_query_cap - Number of clients currently in query cap
+ */
+static unsigned long stat_n_in_query_cap;
+/**
+ * stat_n_in_open - Number of clients currently in b2r2_blt_open
+ */
+static unsigned long stat_n_in_open;
+/**
+ * stat_n_in_release - Number of clients currently in b2r2_blt_release
+ */
+static unsigned long stat_n_in_release;
+
+/* Debug file system support */
+#ifdef CONFIG_DEBUG_FS
+/**
+ * debugfs_latest_request - Copy of the latest request issued
+ */
+struct b2r2_blt_request debugfs_latest_request;
+/**
+ * debugfs_root_dir - The debugfs root directory, i.e. /debugfs/b2r2
+ */
+static struct dentry *debugfs_root_dir;
+
+static int sprintf_req(struct b2r2_blt_request *request, char *buf, int size);
+#endif
+
+/* Local functions */
+static void inc_stat(unsigned long *stat);
+static void dec_stat(unsigned long *stat);
+static int b2r2_blt_synch(struct b2r2_blt_instance *instance,
+			int request_id);
+static int b2r2_blt_query_cap(struct b2r2_blt_instance *instance,
+			struct b2r2_blt_query_cap *query_cap);
+
+#ifndef CONFIG_B2R2_GENERIC_ONLY
+static int b2r2_blt(struct b2r2_blt_instance *instance,
+		struct b2r2_blt_request *request);
+
+static void job_callback(struct b2r2_core_job *job);
+static void job_release(struct b2r2_core_job *job);
+static int job_acquire_resources(struct b2r2_core_job *job, bool atomic);
+static void job_release_resources(struct b2r2_core_job *job, bool atomic);
+#endif
+
+#ifdef CONFIG_B2R2_GENERIC
+static int b2r2_generic_blt(struct b2r2_blt_instance *instance,
+		struct b2r2_blt_request *request);
+
+static void job_callback_gen(struct b2r2_core_job *job);
+static void job_release_gen(struct b2r2_core_job *job);
+static int job_acquire_resources_gen(struct b2r2_core_job *job, bool atomic);
+static void job_release_resources_gen(struct b2r2_core_job *job, bool atomic);
+static void tile_job_callback_gen(struct b2r2_core_job *job);
+static void tile_job_release_gen(struct b2r2_core_job *job);
+#endif
+
+
+static int resolve_buf(struct b2r2_blt_img *img,
+			struct b2r2_blt_rect *rect_2b_used,
+			bool is_dst,
+		struct b2r2_resolved_buf *resolved);
+static void unresolve_buf(struct b2r2_blt_buf *buf,
+			struct b2r2_resolved_buf *resolved);
+static void sync_buf(struct b2r2_blt_img *img,
+		struct b2r2_resolved_buf *resolved,
+		bool is_dst,
+		struct b2r2_blt_rect *rect);
+static bool is_report_list_empty(struct b2r2_blt_instance *instance);
+static bool is_synching(struct b2r2_blt_instance *instance);
+static void get_actual_dst_rect(struct b2r2_blt_req *req,
+					struct b2r2_blt_rect *actual_dst_rect);
+static void set_up_hwmem_region(struct b2r2_blt_img *img,
+		struct b2r2_blt_rect *rect, struct hwmem_region *region);
+static int resolve_hwmem(struct b2r2_blt_img *img,
+			struct b2r2_blt_rect *rect_2b_used, bool is_dst,
+				struct b2r2_resolved_buf *resolved_buf);
+static void unresolve_hwmem(struct b2r2_resolved_buf *resolved_buf);
+
+/**
+ * struct sync_args - Data for clean/flush
+ *
+ * @start: Virtual start address
+ * @end: Virtual end address
+ */
+struct sync_args {
+	unsigned long start;
+	unsigned long end;
+};
+/**
+ * flush_l1_cache_range_curr_cpu() - Cleans and invalidates L1 cache on the current CPU
+ *
+ * @arg: Pointer to sync_args structure
+ */
+static inline void flush_l1_cache_range_curr_cpu(void *arg)
+{
+	struct sync_args *sa = (struct sync_args *)arg;
+
+	dmac_flush_range((void *)sa->start, (void *)sa->end);
+}
+
+#ifdef CONFIG_SMP
+/**
+ * inv_l1_cache_range_all_cpus() - Cleans and invalidates L1 cache on all CPU:s
+ *
+ * @sa: Pointer to sync_args structure
+ */
+static void flush_l1_cache_range_all_cpus(struct sync_args *sa)
+{
+	on_each_cpu(flush_l1_cache_range_curr_cpu, sa, 1);
+}
+#endif
+
+/**
+ * clean_l1_cache_range_curr_cpu() - Cleans L1 cache on current CPU
+ *
+ * Ensures that data is written out from the CPU:s L1 cache,
+ * it will still be in the cache.
+ *
+ * @arg: Pointer to sync_args structure
+ */
+static inline void clean_l1_cache_range_curr_cpu(void *arg)
+{
+	struct sync_args *sa = (struct sync_args *)arg;
+
+	dmac_map_area((void *)sa->start,
+		(void *)sa->end - (void *)sa->start,
+		DMA_TO_DEVICE);
+}
+
+#ifdef CONFIG_SMP
+/**
+ * clean_l1_cache_range_all_cpus() - Cleans L1 cache on all CPU:s
+ *
+ * Ensures that data is written out from all CPU:s L1 cache,
+ * it will still be in the cache.
+ *
+ * @sa: Pointer to sync_args structure
+ */
+static void clean_l1_cache_range_all_cpus(struct sync_args *sa)
+{
+	on_each_cpu(clean_l1_cache_range_curr_cpu, sa, 1);
+}
+#endif
+
+/**
+ * b2r2_blt_open - Implements file open on the b2r2_blt device
+ *
+ * @inode: File system inode
+ * @filp: File pointer
+ *
+ * A B2R2 BLT instance is created and stored in the file structure.
+ */
+static int b2r2_blt_open(struct inode *inode, struct file *filp)
+{
+	int ret = 0;
+	struct b2r2_blt_instance *instance;
+
+	b2r2_log_info("%s\n", __func__);
+
+	inc_stat(&stat_n_in_open);
+
+	/* Allocate and initialize the instance */
+	instance = (struct b2r2_blt_instance *)
+		kmalloc(sizeof(*instance), GFP_KERNEL);
+	if (!instance) {
+		b2r2_log_err("%s: Failed to alloc\n", __func__);
+		goto instance_alloc_failed;
+	}
+	memset(instance, 0, sizeof(*instance));
+	INIT_LIST_HEAD(&instance->report_list);
+	mutex_init(&instance->lock);
+	init_waitqueue_head(&instance->report_list_waitq);
+	init_waitqueue_head(&instance->synch_done_waitq);
+
+	/*
+	 * Remember the instance so that we can retrieve it in
+	 * other functions
+	 */
+	filp->private_data = instance;
+	goto out;
+
+instance_alloc_failed:
+out:
+	dec_stat(&stat_n_in_open);
+
+	return ret;
+}
+
+/**
+ * b2r2_blt_release - Implements last close on an instance of
+ *                    the b2r2_blt device
+ *
+ * @inode: File system inode
+ * @filp: File pointer
+ *
+ * All active jobs are finished or cancelled and allocated data
+ * is released.
+ */
+static int b2r2_blt_release(struct inode *inode, struct file *filp)
+{
+	int ret;
+	struct b2r2_blt_instance *instance;
+
+	b2r2_log_info("%s\n", __func__);
+
+	inc_stat(&stat_n_in_release);
+
+	instance = (struct b2r2_blt_instance *) filp->private_data;
+
+	/* Finish all outstanding requests */
+	ret = b2r2_blt_synch(instance, 0);
+	if (ret < 0)
+		b2r2_log_warn(
+			"%s: b2r2_blt_sync failed with %d\n", __func__, ret);
+
+	/* Now cancel any remaining outstanding request */
+	if (instance->no_of_active_requests) {
+		struct b2r2_core_job *job;
+
+		b2r2_log_warn("%s: %d active requests\n",
+			__func__, instance->no_of_active_requests);
+
+		/* Find and cancel all jobs belonging to us */
+		job = b2r2_core_job_find_first_with_tag((int) instance);
+		while (job) {
+			b2r2_core_job_cancel(job);
+			/* This release matches addref in b2r2_core_job_find... */
+			b2r2_core_job_release(job, __func__);
+			job = b2r2_core_job_find_first_with_tag((int) instance);
+		}
+
+		b2r2_log_warn(
+			"%s: %d active requests after cancel\n",
+			__func__, instance->no_of_active_requests);
+	}
+
+	/* Release jobs in report list */
+	mutex_lock(&instance->lock);
+	while (!list_empty(&instance->report_list)) {
+		struct b2r2_blt_request *request = list_first_entry(
+			&instance->report_list,
+			struct b2r2_blt_request,
+			list);
+		list_del_init(&request->list);
+		mutex_unlock(&instance->lock);
+		/*
+		 * This release matches the addref when the job was put into
+		 * the report list
+		 */
+		b2r2_core_job_release(&request->job, __func__);
+		mutex_lock(&instance->lock);
+	}
+	mutex_unlock(&instance->lock);
+
+	/* Release our instance */
+	kfree(instance);
+
+	dec_stat(&stat_n_in_release);
+
+	return 0;
+}
+
+/**
+ * b2r2_blt_ioctl - This routine implements b2r2_blt ioctl interface
+ *
+ * @file: file pointer.
+ * @cmd	:ioctl command.
+ * @arg: input argument for ioctl.
+ *
+ * Returns 0 if OK else negative error code
+ */
+static long b2r2_blt_ioctl(struct file *file,
+		unsigned int cmd, unsigned long arg)
+{
+	int ret = 0;
+	struct b2r2_blt_instance *instance;
+
+	/** Process actual ioctl */
+
+	b2r2_log_info("%s\n", __func__);
+
+	/* Get the instance from the file structure */
+	instance = (struct b2r2_blt_instance *) file->private_data;
+
+	switch (cmd) {
+	case B2R2_BLT_IOC: {
+		/* This is the "blit" command */
+
+		/* arg is user pointer to struct b2r2_blt_request */
+		struct b2r2_blt_request *request =
+			kmalloc(sizeof(*request), GFP_KERNEL);
+		if (!request) {
+			b2r2_log_err("%s: Failed to alloc mem\n",
+				__func__);
+			return -ENOMEM;
+		}
+
+		/* Initialize the structure */
+		memset(request, 0, sizeof(*request));
+		INIT_LIST_HEAD(&request->list);
+		request->instance = instance;
+
+		/*
+		 * The user request is a sub structure of the
+		 * kernel request structure.
+		 */
+
+		/* Get the user data */
+		if (copy_from_user(&request->user_req, (void *)arg,
+				sizeof(request->user_req))) {
+			b2r2_log_err(
+				"%s: copy_from_user failed\n",
+				__func__);
+			kfree(request);
+			return -EFAULT;
+		}
+
+		if (!b2r2_validate_user_req(&request->user_req)) {
+			kfree(request);
+			return -EINVAL;
+		}
+
+		request->profile = is_profiler_registered_approx();
+
+		/*
+		 * If the user specified a color look-up table,
+		 * make a copy that the HW can use.
+		 */
+		if ((request->user_req.flags &
+				B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) != 0) {
+			request->clut = dma_alloc_coherent(b2r2_blt_device(),
+				CLUT_SIZE, &(request->clut_phys_addr),
+				GFP_DMA | GFP_KERNEL);
+			if (request->clut == NULL) {
+				b2r2_log_err("%s CLUT allocation failed.\n",
+					__func__);
+				kfree(request);
+				return -ENOMEM;
+			}
+
+			if (copy_from_user(request->clut,
+					request->user_req.clut, CLUT_SIZE)) {
+				b2r2_log_err("%s: CLUT copy_from_user failed\n",
+					__func__);
+				dma_free_coherent(b2r2_blt_device(), CLUT_SIZE,
+					request->clut, request->clut_phys_addr);
+				request->clut = NULL;
+				request->clut_phys_addr = 0;
+				kfree(request);
+				return -EFAULT;
+			}
+		}
+
+		/* Perform the blit */
+
+#ifdef CONFIG_B2R2_GENERIC_ONLY
+		/* Use the generic path for all operations */
+		ret = b2r2_generic_blt(instance, request);
+#else
+		/* Use the optimized path */
+		ret = b2r2_blt(instance, request);
+#endif
+
+#ifdef CONFIG_B2R2_GENERIC_FALLBACK
+		/* Fall back to generic path if operation was not supported */
+		if (ret == -ENOSYS) {
+			struct b2r2_blt_request *request_gen;
+			b2r2_log_info("b2r2_blt=%d Going generic.\n", ret);
+			request_gen = kmalloc(sizeof(*request_gen), GFP_KERNEL);
+			if (!request_gen) {
+				b2r2_log_err(
+					"%s: Failed to alloc mem for request_gen\n", __func__);
+				return -ENOMEM;
+			}
+
+			/* Initialize the structure */
+			memset(request_gen, 0, sizeof(*request_gen));
+			INIT_LIST_HEAD(&request_gen->list);
+			request_gen->instance = instance;
+
+			/*
+			 * The user request is a sub structure of the
+			 * kernel request structure.
+			 */
+
+			/* Get the user data */
+			if (copy_from_user(&request_gen->user_req, (void *)arg,
+				sizeof(request_gen->user_req))) {
+					b2r2_log_err(
+						"%s: copy_from_user failed\n",
+						__func__);
+					kfree(request_gen);
+					return -EFAULT;
+			}
+
+			/*
+			 * If the user specified a color look-up table,
+			 * make a copy that the HW can use.
+			 */
+			if ((request_gen->user_req.flags &
+					B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) != 0) {
+				request_gen->clut = dma_alloc_coherent(b2r2_blt_device(),
+					CLUT_SIZE, &(request_gen->clut_phys_addr),
+					GFP_DMA | GFP_KERNEL);
+				if (request_gen->clut == NULL) {
+					b2r2_log_err("%s CLUT allocation failed.\n", __func__);
+					kfree(request_gen);
+					return -ENOMEM;
+				}
+
+				if (copy_from_user(request_gen->clut, request_gen->user_req.clut, CLUT_SIZE)) {
+					b2r2_log_err("%s: CLUT copy_from_user failed\n", __func__);
+					dma_free_coherent(b2r2_blt_device(), CLUT_SIZE, request_gen->clut,
+						request_gen->clut_phys_addr);
+					request_gen->clut = NULL;
+					request_gen->clut_phys_addr = 0;
+					kfree(request_gen);
+					return -EFAULT;
+				}
+			}
+
+			request_gen->profile = is_profiler_registered_approx();
+
+			ret = b2r2_generic_blt(instance, request_gen);
+			b2r2_log_info("\nb2r2_generic_blt=%d Generic done.\n", ret);
+		}
+#endif /* CONFIG_B2R2_GENERIC_FALLBACK */
+
+		break;
+	}
+
+	case B2R2_BLT_SYNCH_IOC:
+		/* This is the "synch" command */
+
+		/* arg is request_id */
+		ret = b2r2_blt_synch(instance, (int) arg);
+		break;
+
+	case B2R2_BLT_QUERY_CAP_IOC:
+	{
+		/* This is the "query capabilities" command */
+
+		/* Arg is struct b2r2_blt_query_cap */
+		struct b2r2_blt_query_cap query_cap;
+
+		/* Get the user data */
+		if (copy_from_user(&query_cap, (void *)arg,
+				sizeof(query_cap))) {
+			b2r2_log_err(
+				"%s: copy_from_user failed\n",
+				__func__);
+			return -EFAULT;
+		}
+
+		/* Fill in our capabilities */
+		ret = b2r2_blt_query_cap(instance, &query_cap);
+
+		/* Return data to user */
+		if (copy_to_user((void *)arg, &query_cap,
+				sizeof(query_cap))) {
+			b2r2_log_err("%s: copy_to_user failed\n",
+				__func__);
+			return -EFAULT;
+		}
+		break;
+	}
+
+	default:
+		/* Unknown command */
+		b2r2_log_err(
+			"%s: Unknown cmd %d\n", __func__, cmd);
+		ret = -EINVAL;
+	break;
+
+	}
+
+	if (ret < 0)
+		b2r2_log_err("EC %d OK!\n", -ret);
+
+	return ret;
+}
+
+/**
+ * b2r2_blt_poll - Support for user-space poll, select & epoll.
+ *                 Used for user-space callback
+ *
+ * @filp: File to poll on
+ * @wait: Poll table to wait on
+ *
+ * This function checks if there are anything to read
+ */
+static unsigned b2r2_blt_poll(struct file *filp, poll_table *wait)
+{
+	struct b2r2_blt_instance *instance;
+	unsigned int mask = 0;
+
+	b2r2_log_info("%s\n", __func__);
+
+	/* Get the instance from the file structure */
+	instance = (struct b2r2_blt_instance *) filp->private_data;
+
+	poll_wait(filp, &instance->report_list_waitq, wait);
+	mutex_lock(&instance->lock);
+	if (!list_empty(&instance->report_list))
+		mask |= POLLIN | POLLRDNORM;
+	mutex_unlock(&instance->lock);
+
+	return mask;
+}
+
+/**
+ * b2r2_blt_read - Read report data, user for user-space callback
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static ssize_t b2r2_blt_read(struct file *filp, char __user *buf, size_t count,
+			loff_t *f_pos)
+{
+	int ret = 0;
+	struct b2r2_blt_instance *instance;
+	struct b2r2_blt_request *request;
+	struct b2r2_blt_report report;
+
+	b2r2_log_info("%s\n", __func__);
+
+	/* Get the instance from the file structure */
+	instance = (struct b2r2_blt_instance *) filp->private_data;
+
+	/*
+	 * We return only complete report records, one at a time.
+	 * Might be more efficient to support read of many.
+	 */
+	count = (count / sizeof(struct b2r2_blt_report)) *
+		sizeof(struct b2r2_blt_report);
+	if (count > sizeof(struct b2r2_blt_report))
+		count = sizeof(struct b2r2_blt_report);
+	if (count == 0)
+		return count;
+
+	/*
+	 * Loop and wait here until we have anything to return or
+	 * until interrupted
+	 */
+	mutex_lock(&instance->lock);
+	while (list_empty(&instance->report_list)) {
+		mutex_unlock(&instance->lock);
+
+		/* Return if non blocking read */
+		if (filp->f_flags & O_NONBLOCK)
+			return -EAGAIN;
+
+		b2r2_log_info("%s - Going to sleep\n", __func__);
+		if (wait_event_interruptible(
+			instance->report_list_waitq,
+			!is_report_list_empty(instance)))
+			/* signal: tell the fs layer to handle it */
+			return -ERESTARTSYS;
+
+		/* Otherwise loop, but first reaquire the lock */
+		mutex_lock(&instance->lock);
+	}
+
+	/* Ok, we have something to return */
+
+	/* Return */
+	request = NULL;
+	if (!list_empty(&instance->report_list))
+		request = list_first_entry(
+			&instance->report_list, struct b2r2_blt_request, list);
+
+	if (request) {
+		/* Remove from list to avoid reading twice */
+		list_del_init(&request->list);
+
+		report.request_id = request->request_id;
+		report.report1 = request->user_req.report1;
+		report.report2 = request->user_req.report2;
+		report.usec_elapsed = 0; /* TBD */
+
+		mutex_unlock(&instance->lock);
+		if (copy_to_user(buf,
+				&report,
+				sizeof(report)))
+			ret = -EFAULT;
+		mutex_lock(&instance->lock);
+
+		if (ret) {
+			/* copy to user failed, re-insert into list */
+			list_add(&request->list,
+				&request->instance->report_list);
+			request = NULL;
+		}
+	}
+	mutex_unlock(&instance->lock);
+
+	if (request)
+		/*
+		 * Release matching the addref when the job was put into
+		 * the report list
+		 */
+		b2r2_core_job_release(&request->job, __func__);
+
+	return count;
+}
+
+/**
+ * b2r2_blt_fops - File operations for b2r2_blt
+ */
+static const struct file_operations b2r2_blt_fops = {
+	.owner =          THIS_MODULE,
+	.open =           b2r2_blt_open,
+	.release =        b2r2_blt_release,
+	.unlocked_ioctl = b2r2_blt_ioctl,
+	.poll  =          b2r2_blt_poll,
+	.read  =          b2r2_blt_read,
+};
+
+/**
+ * b2r2_blt_misc_dev - Misc device config for b2r2_blt
+ */
+static struct miscdevice b2r2_blt_misc_dev = {
+	MISC_DYNAMIC_MINOR,
+	"b2r2_blt",
+	&b2r2_blt_fops
+};
+
+
+#ifndef CONFIG_B2R2_GENERIC_ONLY
+/**
+ * b2r2_blt - Implementation of the B2R2 blit request
+ *
+ * @instance: The B2R2 BLT instance
+ * @request; The request to perform
+ */
+static int b2r2_blt(struct b2r2_blt_instance *instance,
+		struct b2r2_blt_request *request)
+{
+	int ret = 0;
+	struct b2r2_blt_rect actual_dst_rect;
+	int request_id = 0;
+	struct b2r2_node *last_node = request->first_node;
+	int node_count;
+
+	u32 thread_runtime_at_start = 0;
+
+	if (request->profile) {
+		request->start_time_nsec = b2r2_get_curr_nsec();
+		thread_runtime_at_start = (u32)task_sched_runtime(current);
+	}
+
+	b2r2_log_info("%s\n", __func__);
+
+	inc_stat(&stat_n_in_blt);
+
+	/* Debug prints of incoming request */
+	b2r2_log_info(
+		"src.fmt=%#010x src.buf={%d,%d,%d} "
+		"src.w,h={%d,%d} src.rect={%d,%d,%d,%d}\n",
+		request->user_req.src_img.fmt,
+		request->user_req.src_img.buf.type,
+		request->user_req.src_img.buf.fd,
+		request->user_req.src_img.buf.offset,
+		request->user_req.src_img.width,
+		request->user_req.src_img.height,
+		request->user_req.src_rect.x,
+		request->user_req.src_rect.y,
+		request->user_req.src_rect.width,
+		request->user_req.src_rect.height);
+	b2r2_log_info(
+		"dst.fmt=%#010x dst.buf={%d,%d,%d} "
+		"dst.w,h={%d,%d} dst.rect={%d,%d,%d,%d}\n",
+		request->user_req.dst_img.fmt,
+		request->user_req.dst_img.buf.type,
+		request->user_req.dst_img.buf.fd,
+		request->user_req.dst_img.buf.offset,
+		request->user_req.dst_img.width,
+		request->user_req.dst_img.height,
+		request->user_req.dst_rect.x,
+		request->user_req.dst_rect.y,
+		request->user_req.dst_rect.width,
+		request->user_req.dst_rect.height);
+
+	inc_stat(&stat_n_in_blt_synch);
+
+	/* Wait here if synch is ongoing */
+	ret = wait_event_interruptible(instance->synch_done_waitq,
+				!is_synching(instance));
+	if (ret) {
+		b2r2_log_warn(
+			"%s: Sync wait interrupted, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		dec_stat(&stat_n_in_blt_synch);
+		goto synch_interrupted;
+	}
+
+	dec_stat(&stat_n_in_blt_synch);
+
+	/* Resolve the buffers */
+
+	/* Source buffer */
+	ret = resolve_buf(&request->user_req.src_img,
+		&request->user_req.src_rect, false, &request->src_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(
+			"%s: Resolve src buf failed, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		goto resolve_src_buf_failed;
+	}
+
+	/* Source mask buffer */
+	ret = resolve_buf(&request->user_req.src_mask,
+			&request->user_req.src_rect, false,
+			&request->src_mask_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(
+			"%s: Resolve src mask buf failed, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		goto resolve_src_mask_buf_failed;
+	}
+
+	/* Destination buffer */
+	get_actual_dst_rect(&request->user_req, &actual_dst_rect);
+	ret = resolve_buf(&request->user_req.dst_img, &actual_dst_rect,
+			true, &request->dst_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(
+			"%s: Resolve dst buf failed, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		goto resolve_dst_buf_failed;
+	}
+
+	/* Debug prints of resolved buffers */
+	b2r2_log_info("src.rbuf={%X,%p,%d} {%p,%X,%X,%d}\n",
+		request->src_resolved.physical_address,
+		request->src_resolved.virtual_address,
+		request->src_resolved.is_pmem,
+		request->src_resolved.filep,
+		request->src_resolved.file_physical_start,
+		request->src_resolved.file_virtual_start,
+		request->src_resolved.file_len);
+
+	b2r2_log_info("dst.rbuf={%X,%p,%d} {%p,%X,%X,%d}\n",
+		request->dst_resolved.physical_address,
+		request->dst_resolved.virtual_address,
+		request->dst_resolved.is_pmem,
+		request->dst_resolved.filep,
+		request->dst_resolved.file_physical_start,
+		request->dst_resolved.file_virtual_start,
+		request->dst_resolved.file_len);
+
+	/* Calculate the number of nodes (and resources) needed for this job */
+	ret = b2r2_node_split_analyze(request, MAX_TMP_BUF_SIZE,
+			&node_count, &request->bufs, &request->buf_count,
+			&request->node_split_job);
+	if (ret == -ENOSYS) {
+		/* There was no optimized path for this request */
+		b2r2_log_info(
+			"%s: No optimized path for request\n", __func__);
+		goto no_optimized_path;
+
+	} else if (ret < 0) {
+		b2r2_log_warn(
+			"%s: Failed to analyze request, ret = %d\n",
+			__func__, ret);
+#ifdef CONFIG_DEBUG_FS
+		{
+			/* Failed, dump job to dmesg */
+			char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+			b2r2_log_info(
+				"%s: Analyze failed for:\n", __func__);
+			if (Buf != NULL) {
+				sprintf_req(request, Buf, sizeof(char) * 4096);
+				b2r2_log_info("%s", Buf);
+				kfree(Buf);
+			} else {
+				b2r2_log_info("Unable to print the request. "
+					"Message buffer allocation failed.\n");
+			}
+		}
+#endif
+		goto generate_nodes_failed;
+	}
+
+	/* Allocate the nodes needed */
+#ifdef B2R2_USE_NODE_GEN
+	request->first_node = b2r2_blt_alloc_nodes(node_count);
+	if (request->first_node == NULL) {
+		b2r2_log_warn(
+			"%s: Failed to allocate nodes, ret = %d\n",
+			__func__, ret);
+		goto generate_nodes_failed;
+	}
+#else
+	ret = b2r2_node_alloc(node_count, &(request->first_node));
+	if (ret < 0 || request->first_node == NULL) {
+		b2r2_log_warn(
+			"%s: Failed to allocate nodes, ret = %d\n",
+			__func__, ret);
+		goto generate_nodes_failed;
+	}
+#endif
+
+	/* Build the B2R2 node list */
+	ret = b2r2_node_split_configure(&request->node_split_job,
+			request->first_node);
+
+	if (ret < 0) {
+		b2r2_log_warn(
+			"%s: Failed to perform node split, ret = %d\n",
+			__func__, ret);
+		goto generate_nodes_failed;
+	}
+
+	/* Exit here if dry run */
+	if (request->user_req.flags & B2R2_BLT_FLAG_DRY_RUN)
+		goto exit_dry_run;
+
+	/* Configure the request */
+	last_node = request->first_node;
+	while (last_node && last_node->next)
+		last_node = last_node->next;
+
+	request->job.tag = (int) instance;
+	request->job.prio = request->user_req.prio;
+	request->job.first_node_address =
+		request->first_node->physical_address;
+	request->job.last_node_address =
+		last_node->physical_address;
+	request->job.callback = job_callback;
+	request->job.release = job_release;
+	request->job.acquire_resources = job_acquire_resources;
+	request->job.release_resources = job_release_resources;
+
+	/* Synchronize memory occupied by the buffers */
+
+	/* Source buffer */
+	if (!(request->user_req.flags &
+				B2R2_BLT_FLAG_SRC_NO_CACHE_FLUSH) &&
+			(request->user_req.src_img.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.src_img.fmt))
+			/* MB formats are never touched by SW */
+		sync_buf(&request->user_req.src_img,
+			&request->src_resolved,
+			false, /*is_dst*/
+			&request->user_req.src_rect);
+
+	/* Source mask buffer */
+	if (!(request->user_req.flags &
+				B2R2_BLT_FLAG_SRC_MASK_NO_CACHE_FLUSH) &&
+			(request->user_req.src_mask.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.src_mask.fmt))
+			/* MB formats are never touched by SW */
+		sync_buf(&request->user_req.src_mask,
+			&request->src_mask_resolved,
+			false, /*is_dst*/
+			NULL);
+
+	/* Destination buffer */
+	if (!(request->user_req.flags &
+				B2R2_BLT_FLAG_DST_NO_CACHE_FLUSH) &&
+			(request->user_req.dst_img.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.dst_img.fmt))
+			/* MB formats are never touched by SW */
+		sync_buf(&request->user_req.dst_img,
+			&request->dst_resolved,
+			true, /*is_dst*/
+			&request->user_req.dst_rect);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Remember latest request for debugfs */
+	debugfs_latest_request = *request;
+#endif
+
+	/* Submit the job */
+	b2r2_log_info("%s: Submitting job\n", __func__);
+
+	inc_stat(&stat_n_in_blt_add);
+
+	if (request->profile)
+		request->nsec_active_in_cpu =
+			(s32)((u32)task_sched_runtime(current) -
+					thread_runtime_at_start);
+
+	mutex_lock(&instance->lock);
+
+	/* Add the job to b2r2_core */
+	request_id = b2r2_core_job_add(&request->job);
+	request->request_id = request_id;
+
+	dec_stat(&stat_n_in_blt_add);
+
+	if (request_id < 0) {
+		b2r2_log_warn("%s: Failed to add job, ret = %d\n",
+			__func__, request_id);
+		ret = request_id;
+		mutex_unlock(&instance->lock);
+		goto job_add_failed;
+	}
+
+	inc_stat(&stat_n_jobs_added);
+
+	instance->no_of_active_requests++;
+	mutex_unlock(&instance->lock);
+
+	/* Wait for the job to be done if synchronous */
+	if ((request->user_req.flags & B2R2_BLT_FLAG_ASYNCH) == 0) {
+		b2r2_log_info("%s: Synchronous, waiting\n",
+			__func__);
+
+		inc_stat(&stat_n_in_blt_wait);
+
+		ret = b2r2_core_job_wait(&request->job);
+
+		dec_stat(&stat_n_in_blt_wait);
+
+		if (ret < 0 && ret != -ENOENT)
+			b2r2_log_warn(
+				"%s: Failed to wait job, ret = %d\n",
+				__func__, ret);
+		else
+			b2r2_log_info(
+				"%s: Synchronous wait done\n", __func__);
+		ret = 0;
+	}
+
+	/*
+	 * Release matching the addref in b2r2_core_job_add,
+	 * the request must not be accessed after this call
+	 */
+	b2r2_core_job_release(&request->job, __func__);
+
+	dec_stat(&stat_n_in_blt);
+
+	return ret >= 0 ? request_id : ret;
+
+job_add_failed:
+exit_dry_run:
+no_optimized_path:
+generate_nodes_failed:
+	unresolve_buf(&request->user_req.dst_img.buf,
+		&request->dst_resolved);
+resolve_dst_buf_failed:
+	unresolve_buf(&request->user_req.src_mask.buf,
+		&request->src_mask_resolved);
+resolve_src_mask_buf_failed:
+	unresolve_buf(&request->user_req.src_img.buf,
+		&request->src_resolved);
+resolve_src_buf_failed:
+synch_interrupted:
+	job_release(&request->job);
+	dec_stat(&stat_n_jobs_released);
+	if ((request->user_req.flags & B2R2_BLT_FLAG_DRY_RUN) == 0 || ret)
+		b2r2_log_warn(
+			"%s returns with error %d\n", __func__, ret);
+
+	dec_stat(&stat_n_in_blt);
+
+	return ret;
+}
+
+/**
+ * Called when job is done or cancelled
+ *
+ * @job: The job
+ */
+static void job_callback(struct b2r2_core_job *job)
+{
+	struct b2r2_blt_request *request =
+		container_of(job, struct b2r2_blt_request, job);
+
+	if (b2r2_blt_device())
+		b2r2_log_info("%s\n", __func__);
+
+	/* Local addref / release within this func */
+	b2r2_core_job_addref(job, __func__);
+
+	/* Unresolve the buffers */
+	unresolve_buf(&request->user_req.src_img.buf,
+		&request->src_resolved);
+	unresolve_buf(&request->user_req.src_mask.buf,
+		&request->src_mask_resolved);
+	unresolve_buf(&request->user_req.dst_img.buf,
+		&request->dst_resolved);
+
+	/* Move to report list if the job shall be reported */
+	/* FIXME: Use a smaller struct? */
+	mutex_lock(&request->instance->lock);
+	if (request->user_req.flags & B2R2_BLT_FLAG_REPORT_WHEN_DONE) {
+		/* Move job to report list */
+		list_add_tail(&request->list,
+			&request->instance->report_list);
+		inc_stat(&stat_n_jobs_in_report_list);
+
+		/* Wake up poll */
+		wake_up_interruptible(
+			&request->instance->report_list_waitq);
+
+		/* Add a reference because we put the job in the report list */
+		b2r2_core_job_addref(job, __func__);
+	}
+
+	/*
+	 * Decrease number of active requests and wake up
+	 * synching threads if active requests reaches zero
+	 */
+	BUG_ON(request->instance->no_of_active_requests == 0);
+	request->instance->no_of_active_requests--;
+	if (request->instance->synching &&
+	request->instance->no_of_active_requests == 0) {
+		request->instance->synching = false;
+		/* Wake up all syncing */
+
+		wake_up_interruptible_all(
+			&request->instance->synch_done_waitq);
+	}
+	mutex_unlock(&request->instance->lock);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Dump job if cancelled */
+	if (job->job_state == B2R2_CORE_JOB_CANCELED) {
+		char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+		b2r2_log_info("%s: Job cancelled:\n", __func__);
+		if (Buf != NULL) {
+			sprintf_req(request, Buf, sizeof(char) * 4096);
+			b2r2_log_info("%s", Buf);
+			kfree(Buf);
+		} else {
+			b2r2_log_info("Unable to print the request. "
+					"Message buffer allocation failed.\n");
+		}
+	}
+#endif
+
+	if (request->profile) {
+		request->total_time_nsec =
+			(s32)(b2r2_get_curr_nsec() - request->start_time_nsec);
+		b2r2_call_profiler_blt_done(request);
+	}
+
+	/* Local addref / release within this func */
+	b2r2_core_job_release(job, __func__);
+}
+
+/**
+ * Called when job should be released (free memory etc.)
+ *
+ * @job: The job
+ */
+static void job_release(struct b2r2_core_job *job)
+{
+	struct b2r2_blt_request *request =
+		container_of(job, struct b2r2_blt_request, job);
+
+	inc_stat(&stat_n_jobs_released);
+
+	b2r2_log_info("%s, first_node=%p, ref_count=%d\n",
+		__func__, request->first_node, request->job.ref_count);
+
+	b2r2_node_split_cancel(&request->node_split_job);
+
+	if (request->first_node) {
+		b2r2_debug_job_done(request->first_node);
+#ifdef B2R2_USE_NODE_GEN
+		b2r2_blt_free_nodes(request->first_node);
+#else
+		b2r2_node_free(request->first_node);
+#endif
+	}
+
+	/* Release memory for the request */
+	if (request->clut != NULL) {
+		dma_free_coherent(b2r2_blt_device(), CLUT_SIZE, request->clut,
+				request->clut_phys_addr);
+		request->clut = NULL;
+		request->clut_phys_addr = 0;
+	}
+	kfree(request);
+}
+
+/**
+ * Tells the job to try to allocate the resources needed to execute the job.
+ * Called just before execution of a job.
+ *
+ * @job: The job
+ * @atomic: true if called from atomic (i.e. interrupt) context. If function
+ *          can't allocate in atomic context it should return error, it
+ *          will then be called later from non-atomic context.
+ */
+static int job_acquire_resources(struct b2r2_core_job *job, bool atomic)
+{
+	struct b2r2_blt_request *request =
+		container_of(job, struct b2r2_blt_request, job);
+	int ret;
+	int i;
+
+	b2r2_log_info("%s\n", __func__);
+
+	if (request->buf_count == 0)
+		return 0;
+
+	if (request->buf_count > MAX_TMP_BUFS_NEEDED) {
+		b2r2_log_err("%s: request->buf_count > MAX_TMP_BUFS_NEEDED\n",
+								__func__);
+		return -ENOMSG;
+	}
+
+	/*
+	 * 1 to 1 mapping between request temp buffers and temp buffers
+	 * (request temp buf 0 is always temp buf 0, request temp buf 1 is
+	 * always temp buf 1 and so on) to avoid starvation of jobs that
+	 * require multiple temp buffers. Not optimal in terms of memory
+	 * usage but we avoid get into a situation where lower prio jobs can
+	 * delay higher prio jobs that require more temp buffers.
+	 */
+	if (tmp_bufs[0].in_use)
+		return -EAGAIN;
+
+	for (i = 0; i < request->buf_count; i++) {
+		if (tmp_bufs[i].buf.size < request->bufs[i].size) {
+			b2r2_log_err("%s: tmp_bufs[i].buf.size < "
+					"request->bufs[i].size\n",
+								__func__);
+			ret = -ENOMSG;
+			goto error;
+		}
+
+		tmp_bufs[i].in_use = true;
+		request->bufs[i].phys_addr = tmp_bufs[i].buf.phys_addr;
+		request->bufs[i].virt_addr = tmp_bufs[i].buf.virt_addr;
+
+		b2r2_log_info("%s: phys=%p, virt=%p\n",
+			__func__, (void *)request->bufs[i].phys_addr,
+			request->bufs[i].virt_addr);
+
+		ret = b2r2_node_split_assign_buffers(&request->node_split_job,
+					request->first_node, request->bufs,
+					request->buf_count);
+		if (ret < 0)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	for (i = 0; i < request->buf_count; i++)
+		tmp_bufs[i].in_use = false;
+
+	return ret;
+}
+
+/**
+ * Tells the job to free the resources needed to execute the job.
+ * Called after execution of a job.
+ *
+ * @job: The job
+ * @atomic: true if called from atomic (i.e. interrupt) context. If function
+ *          can't allocate in atomic context it should return error, it
+ *          will then be called later from non-atomic context.
+ */
+static void job_release_resources(struct b2r2_core_job *job, bool atomic)
+{
+	struct b2r2_blt_request *request =
+		container_of(job, struct b2r2_blt_request, job);
+	int i;
+
+	b2r2_log_info("%s\n", __func__);
+
+	/* Free any temporary buffers */
+	for (i = 0; i < request->buf_count; i++) {
+
+		b2r2_log_info("%s: freeing %d bytes\n",
+			__func__, request->bufs[i].size);
+		tmp_bufs[i].in_use = false;
+		memset(&request->bufs[i], 0, sizeof(request->bufs[i]));
+	}
+	request->buf_count = 0;
+
+	/*
+	 * Early release of nodes
+	 * FIXME: If nodes are to be reused we don't want to release here
+	 */
+	if (!atomic && request->first_node) {
+		b2r2_debug_job_done(request->first_node);
+
+#ifdef B2R2_USE_NODE_GEN
+		b2r2_blt_free_nodes(request->first_node);
+#else
+		b2r2_node_free(request->first_node);
+#endif
+		request->first_node = NULL;
+	}
+}
+
+#endif /* !CONFIG_B2R2_GENERIC_ONLY */
+
+#ifdef CONFIG_B2R2_GENERIC
+/**
+ * Called when job for one tile is done or cancelled
+ * in the generic path.
+ *
+ * @job: The job
+ */
+static void tile_job_callback_gen(struct b2r2_core_job *job)
+{
+	if (b2r2_blt_device())
+		b2r2_log_info("%s\n", __func__);
+
+	/* Local addref / release within this func */
+	b2r2_core_job_addref(job, __func__);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Notify if a tile job is cancelled */
+	if (job->job_state == B2R2_CORE_JOB_CANCELED) {
+		b2r2_log_info("%s: Tile job cancelled:\n", __func__);
+	}
+#endif
+
+	/* Local addref / release within this func */
+	b2r2_core_job_release(job, __func__);
+}
+
+/**
+ * Called when job is done or cancelled.
+ * Used for the last tile in the generic path
+ * to notify waiting clients.
+ *
+ * @job: The job
+ */
+static void job_callback_gen(struct b2r2_core_job *job)
+{
+	struct b2r2_blt_request *request =
+		container_of(job, struct b2r2_blt_request, job);
+
+	if (b2r2_blt_device())
+		b2r2_log_info("%s\n", __func__);
+
+	/* Local addref / release within this func */
+	b2r2_core_job_addref(job, __func__);
+
+	/* Move to report list if the job shall be reported */
+	/* FIXME: Use a smaller struct? */
+	mutex_lock(&request->instance->lock);
+
+	if (request->user_req.flags & B2R2_BLT_FLAG_REPORT_WHEN_DONE) {
+		/* Move job to report list */
+		list_add_tail(&request->list,
+			&request->instance->report_list);
+		inc_stat(&stat_n_jobs_in_report_list);
+
+		/* Wake up poll */
+		wake_up_interruptible(
+			&request->instance->report_list_waitq);
+
+		/*
+		 * Add a reference because we put the
+		 * job in the report list
+		 */
+		b2r2_core_job_addref(job, __func__);
+	}
+
+	/*
+	 * Decrease number of active requests and wake up
+	 * synching threads if active requests reaches zero
+	 */
+	BUG_ON(request->instance->no_of_active_requests == 0);
+	request->instance->no_of_active_requests--;
+	if (request->instance->synching &&
+	request->instance->no_of_active_requests == 0) {
+		request->instance->synching = false;
+		/* Wake up all syncing */
+
+		wake_up_interruptible_all(
+			&request->instance->synch_done_waitq);
+	}
+	mutex_unlock(&request->instance->lock);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Dump job if cancelled */
+	if (job->job_state == B2R2_CORE_JOB_CANCELED) {
+		char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+		b2r2_log_info("%s: Job cancelled:\n", __func__);
+		if (Buf != NULL) {
+			sprintf_req(request, Buf, sizeof(char) * 4096);
+			b2r2_log_info("%s", Buf);
+			kfree(Buf);
+		} else {
+			b2r2_log_info("Unable to print the request. "
+					"Message buffer allocation failed.\n");
+		}
+	}
+#endif
+
+	/* Local addref / release within this func */
+	b2r2_core_job_release(job, __func__);
+}
+
+/**
+ * Called when tile job should be released (free memory etc.)
+ * Should be used only for tile jobs. Tile jobs should only be used
+ * by b2r2_core, thus making ref_count trigger their release.
+ *
+ * @job: The job
+ */
+
+static void tile_job_release_gen(struct b2r2_core_job *job)
+{
+	inc_stat(&stat_n_jobs_released);
+
+	b2r2_log_info("%s, first_node_address=0x%.8x, ref_count=%d\n",
+		__func__, job->first_node_address, job->ref_count);
+
+	/* Release memory for the job */
+	kfree(job);
+}
+
+/**
+ * Called when job should be released (free memory etc.)
+ *
+ * @job: The job
+ */
+
+static void job_release_gen(struct b2r2_core_job *job)
+{
+	struct b2r2_blt_request *request =
+		container_of(job, struct b2r2_blt_request, job);
+
+	inc_stat(&stat_n_jobs_released);
+
+	b2r2_log_info("%s, first_node=%p, ref_count=%d\n",
+		__func__, request->first_node, request->job.ref_count);
+
+	if (request->first_node) {
+		b2r2_debug_job_done(request->first_node);
+
+		/* Free nodes */
+#ifdef B2R2_USE_NODE_GEN
+		b2r2_blt_free_nodes(request->first_node);
+#else
+		b2r2_node_free(request->first_node);
+#endif
+	}
+
+	/* Release memory for the request */
+	if (request->clut != NULL) {
+		dma_free_coherent(b2r2_blt_device(), CLUT_SIZE, request->clut,
+				request->clut_phys_addr);
+		request->clut = NULL;
+		request->clut_phys_addr = 0;
+	}
+	kfree(request);
+}
+
+static int job_acquire_resources_gen(struct b2r2_core_job *job, bool atomic)
+{
+	/* Nothing so far. Temporary buffers are pre-allocated */
+	return 0;
+}
+static void job_release_resources_gen(struct b2r2_core_job *job, bool atomic)
+{
+	/* Nothing so far. Temporary buffers are pre-allocated */
+}
+
+/**
+ * b2r2_generic_blt - Generic implementation of the B2R2 blit request
+ *
+ * @instance: The B2R2 BLT instance
+ * @request; The request to perform
+ */
+static int b2r2_generic_blt(struct b2r2_blt_instance *instance,
+		struct b2r2_blt_request *request)
+{
+	int ret = 0;
+	struct b2r2_blt_rect actual_dst_rect;
+	int request_id = 0;
+	struct b2r2_node *last_node = request->first_node;
+	int node_count;
+	s32 tmp_buf_width = 0;
+	s32 tmp_buf_height = 0;
+	u32 tmp_buf_count = 0;
+	s32 x;
+	s32 y;
+	const struct b2r2_blt_rect *dst_rect = &(request->user_req.dst_rect);
+	const s32 dst_img_width = request->user_req.dst_img.width;
+	const s32 dst_img_height = request->user_req.dst_img.height;
+	const enum b2r2_blt_flag flags = request->user_req.flags;
+	/* Descriptors for the temporary buffers */
+	struct b2r2_work_buf work_bufs[4];
+	struct b2r2_blt_rect dst_rect_tile;
+	int i;
+
+	u32 thread_runtime_at_start = 0;
+	s32 nsec_active_in_b2r2 = 0;
+
+	/*
+	 * Early exit if zero blt.
+	 * dst_rect outside of dst_img or
+	 * dst_clip_rect outside of dst_img.
+	 */
+	if (dst_rect->x + dst_rect->width <= 0 ||
+			dst_rect->y + dst_rect->height <= 0 ||
+			dst_img_width <= dst_rect->x ||
+			dst_img_height <= dst_rect->y ||
+			((flags & B2R2_BLT_FLAG_DESTINATION_CLIP) != 0 &&
+			(dst_img_width <= request->user_req.dst_clip_rect.x ||
+			dst_img_height <= request->user_req.dst_clip_rect.y ||
+			request->user_req.dst_clip_rect.x +
+			request->user_req.dst_clip_rect.width <= 0 ||
+			request->user_req.dst_clip_rect.y +
+			request->user_req.dst_clip_rect.height <= 0))) {
+		goto zero_blt;
+	}
+
+	if (request->profile) {
+		request->start_time_nsec = b2r2_get_curr_nsec();
+		thread_runtime_at_start = (u32)task_sched_runtime(current);
+	}
+
+	memset(work_bufs, 0, sizeof(work_bufs));
+
+	b2r2_log_info("%s\n", __func__);
+
+	inc_stat(&stat_n_in_blt);
+
+	/* Debug prints of incoming request */
+	b2r2_log_info(
+		"src.fmt=%#010x flags=0x%.8x src.buf={%d,%d,0x%.8x}\n"
+		"src.w,h={%d,%d} src.rect={%d,%d,%d,%d}\n",
+		request->user_req.src_img.fmt,
+		request->user_req.flags,
+		request->user_req.src_img.buf.type,
+		request->user_req.src_img.buf.fd,
+		request->user_req.src_img.buf.offset,
+		request->user_req.src_img.width,
+		request->user_req.src_img.height,
+		request->user_req.src_rect.x,
+		request->user_req.src_rect.y,
+		request->user_req.src_rect.width,
+		request->user_req.src_rect.height);
+	b2r2_log_info(
+		"dst.fmt=%#010x dst.buf={%d,%d,0x%.8x}\n"
+		"dst.w,h={%d,%d} dst.rect={%d,%d,%d,%d}\n"
+		"dst_clip_rect={%d,%d,%d,%d}\n",
+		request->user_req.dst_img.fmt,
+		request->user_req.dst_img.buf.type,
+		request->user_req.dst_img.buf.fd,
+		request->user_req.dst_img.buf.offset,
+		request->user_req.dst_img.width,
+		request->user_req.dst_img.height,
+		request->user_req.dst_rect.x,
+		request->user_req.dst_rect.y,
+		request->user_req.dst_rect.width,
+		request->user_req.dst_rect.height,
+		request->user_req.dst_clip_rect.x,
+		request->user_req.dst_clip_rect.y,
+		request->user_req.dst_clip_rect.width,
+		request->user_req.dst_clip_rect.height);
+
+	inc_stat(&stat_n_in_blt_synch);
+
+	/* Wait here if synch is ongoing */
+	ret = wait_event_interruptible(instance->synch_done_waitq,
+				!is_synching(instance));
+	if (ret) {
+		b2r2_log_warn(
+			"%s: Sync wait interrupted, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		dec_stat(&stat_n_in_blt_synch);
+		goto synch_interrupted;
+	}
+
+	dec_stat(&stat_n_in_blt_synch);
+
+	/* Resolve the buffers */
+
+	/* Source buffer */
+	ret = resolve_buf(&request->user_req.src_img,
+		&request->user_req.src_rect, false, &request->src_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(
+			"%s: Resolve src buf failed, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		goto resolve_src_buf_failed;
+	}
+
+	/* Source mask buffer */
+	ret = resolve_buf(&request->user_req.src_mask,
+					&request->user_req.src_rect, false,
+						&request->src_mask_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(
+			"%s: Resolve src mask buf failed, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		goto resolve_src_mask_buf_failed;
+	}
+
+	/* Destination buffer */
+	get_actual_dst_rect(&request->user_req, &actual_dst_rect);
+	ret = resolve_buf(&request->user_req.dst_img, &actual_dst_rect,
+						true, &request->dst_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(
+			"%s: Resolve dst buf failed, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		goto resolve_dst_buf_failed;
+	}
+
+	/* Debug prints of resolved buffers */
+	b2r2_log_info("src.rbuf={%X,%p,%d} {%p,%X,%X,%d}\n",
+		request->src_resolved.physical_address,
+		request->src_resolved.virtual_address,
+		request->src_resolved.is_pmem,
+		request->src_resolved.filep,
+		request->src_resolved.file_physical_start,
+		request->src_resolved.file_virtual_start,
+		request->src_resolved.file_len);
+
+	b2r2_log_info("dst.rbuf={%X,%p,%d} {%p,%X,%X,%d}\n",
+		request->dst_resolved.physical_address,
+		request->dst_resolved.virtual_address,
+		request->dst_resolved.is_pmem,
+		request->dst_resolved.filep,
+		request->dst_resolved.file_physical_start,
+		request->dst_resolved.file_virtual_start,
+		request->dst_resolved.file_len);
+
+	/* Calculate the number of nodes (and resources) needed for this job */
+	ret = b2r2_generic_analyze(request, &tmp_buf_width,
+			&tmp_buf_height, &tmp_buf_count, &node_count);
+	if (ret < 0) {
+		b2r2_log_warn(
+			"%s: Failed to analyze request, ret = %d\n",
+			__func__, ret);
+#ifdef CONFIG_DEBUG_FS
+		{
+			/* Failed, dump job to dmesg */
+			char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+			b2r2_log_info(
+				"%s: Analyze failed for:\n", __func__);
+			if (Buf != NULL) {
+				sprintf_req(request, Buf, sizeof(char) * 4096);
+				b2r2_log_info("%s", Buf);
+				kfree(Buf);
+			} else {
+				b2r2_log_info("Unable to print the request. "
+					"Message buffer allocation failed.\n");
+			}
+		}
+#endif
+		goto generate_nodes_failed;
+	}
+
+	/* Allocate the nodes needed */
+#ifdef B2R2_USE_NODE_GEN
+	request->first_node = b2r2_blt_alloc_nodes(node_count);
+	if (request->first_node == NULL) {
+		b2r2_log_warn(
+			"%s: Failed to allocate nodes, ret = %d\n",
+			__func__, ret);
+		goto generate_nodes_failed;
+	}
+#else
+	ret = b2r2_node_alloc(node_count, &(request->first_node));
+	if (ret < 0 || request->first_node == NULL) {
+		b2r2_log_warn(
+			"%s: Failed to allocate nodes, ret = %d\n",
+			__func__, ret);
+		goto generate_nodes_failed;
+	}
+#endif
+
+	/* Allocate the temporary buffers */
+	for (i = 0; i < tmp_buf_count; i++) {
+		void *virt;
+		work_bufs[i].size = tmp_buf_width * tmp_buf_height * 4;
+
+		virt = dma_alloc_coherent(b2r2_blt_device(),
+				work_bufs[i].size,
+				&(work_bufs[i].phys_addr),
+				GFP_DMA | GFP_KERNEL);
+		if (virt == NULL) {
+			ret = -ENOMEM;
+			goto alloc_work_bufs_failed;
+		}
+
+		work_bufs[i].virt_addr = virt;
+		memset(work_bufs[i].virt_addr, 0xff, work_bufs[i].size);
+	}
+	ret = b2r2_generic_configure(request,
+			request->first_node, &work_bufs[0], tmp_buf_count);
+
+	if (ret < 0) {
+		b2r2_log_warn(
+			"%s: Failed to perform generic configure, ret = %d\n",
+			__func__, ret);
+		goto generic_conf_failed;
+	}
+
+	/* Exit here if dry run */
+	if (flags & B2R2_BLT_FLAG_DRY_RUN)
+		goto exit_dry_run;
+
+	/*
+	 * Configure the request and make sure
+	 * that its job is run only for the LAST tile.
+	 * This is when the request is complete
+	 * and waiting clients should be notified.
+	 */
+	last_node = request->first_node;
+	while (last_node && last_node->next)
+		last_node = last_node->next;
+
+	request->job.tag = (int) instance;
+	request->job.prio = request->user_req.prio;
+	request->job.first_node_address =
+		request->first_node->physical_address;
+	request->job.last_node_address =
+		last_node->physical_address;
+	request->job.callback = job_callback_gen;
+	request->job.release = job_release_gen;
+	/* Work buffers and nodes are pre-allocated */
+	request->job.acquire_resources = job_acquire_resources_gen;
+	request->job.release_resources = job_release_resources_gen;
+
+	/* Flush the L1/L2 cache for the buffers */
+
+	/* Source buffer */
+	if (!(flags & B2R2_BLT_FLAG_SRC_NO_CACHE_FLUSH) &&
+			(request->user_req.src_img.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.src_img.fmt))
+			/* MB formats are never touched by SW */
+		sync_buf(&request->user_req.src_img,
+			&request->src_resolved,
+			false, /*is_dst*/
+			&request->user_req.src_rect);
+
+	/* Source mask buffer */
+	if (!(flags & B2R2_BLT_FLAG_SRC_MASK_NO_CACHE_FLUSH) &&
+			(request->user_req.src_mask.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.src_mask.fmt))
+			/* MB formats are never touched by SW */
+		sync_buf(&request->user_req.src_mask,
+			&request->src_mask_resolved,
+			false, /*is_dst*/
+			NULL);
+
+	/* Destination buffer */
+	if (!(flags & B2R2_BLT_FLAG_DST_NO_CACHE_FLUSH) &&
+			(request->user_req.dst_img.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.dst_img.fmt))
+			/* MB formats are never touched by SW */
+		sync_buf(&request->user_req.dst_img,
+			&request->dst_resolved,
+			true, /*is_dst*/
+			&request->user_req.dst_rect);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Remember latest request */
+	debugfs_latest_request = *request;
+#endif
+
+	/*
+	 * Same nodes are reused for all the jobs needed to complete the blit.
+	 * Nodes are NOT released together with associated job,
+	 * as is the case with optimized b2r2_blt() path.
+	 */
+	mutex_lock(&instance->lock);
+	instance->no_of_active_requests++;
+	mutex_unlock(&instance->lock);
+	/*
+	 * Process all but the last row in the destination rectangle.
+	 * Consider only the tiles that will actually end up inside
+	 * the destination image.
+	 * dst_rect->height - tmp_buf_height being <=0 is allright.
+	 * The loop will not be entered since y will always be equal to or
+	 * greater than zero.
+	 * Early exit check at the beginning handles the cases when nothing
+	 * at all should be processed.
+	 */
+	y = 0;
+	if (dst_rect->y < 0)
+		y = -dst_rect->y;
+
+	for (; y < dst_rect->height - tmp_buf_height &&
+			y + dst_rect->y < dst_img_height - tmp_buf_height;
+			y += tmp_buf_height) {
+		/* Tile in the destination rectangle being processed */
+		struct b2r2_blt_rect dst_rect_tile;
+		dst_rect_tile.y = y;
+		dst_rect_tile.width = tmp_buf_width;
+		dst_rect_tile.height = tmp_buf_height;
+
+		x = 0;
+		if (dst_rect->x < 0)
+			x = -dst_rect->x;
+
+		for (; x < dst_rect->width &&
+				x + dst_rect->x < dst_img_width; x += tmp_buf_width) {
+			/*
+			 * Tile jobs are freed by the supplied release function
+			 * when ref_count on a tile_job reaches zero.
+			 */
+			struct b2r2_core_job *tile_job = kmalloc(sizeof(*tile_job), GFP_KERNEL);
+			if (tile_job == NULL) {
+				/*
+				 * Skip this tile. Do not abort, just hope for better luck
+				 * with rest of the tiles. Memory might become available.
+				 */
+				b2r2_log_info("%s: Failed to alloc job. "
+					"Skipping tile at (x, y)=(%d, %d)\n", __func__, x, y);
+				continue;
+			}
+			tile_job->tag = request->job.tag;
+			tile_job->prio = request->job.prio;
+			tile_job->first_node_address =
+					request->job.first_node_address;
+			tile_job->last_node_address =
+					request->job.last_node_address;
+			tile_job->callback = tile_job_callback_gen;
+			tile_job->release = tile_job_release_gen;
+			/* Work buffers and nodes are pre-allocated */
+			tile_job->acquire_resources = job_acquire_resources_gen;
+			tile_job->release_resources = job_release_resources_gen;
+
+			dst_rect_tile.x = x;
+			if (x + dst_rect->x + tmp_buf_width > dst_img_width) {
+				/*
+				 * Only a part of the tile can be written.
+				 * Limit imposed by buffer size.
+				 */
+				dst_rect_tile.width = dst_img_width - (x + dst_rect->x);
+			} else if (x + tmp_buf_width > dst_rect->width) {
+				/*
+				 * Only a part of the tile can be written.
+				 * In this case limit imposed by dst_rect size.
+				 */
+				dst_rect_tile.width = dst_rect->width - x;
+			} else {
+				/* Whole tile can be written. */
+				dst_rect_tile.width = tmp_buf_width;
+			}
+			/*
+			 * Where applicable, calculate area in src buffer that is needed
+			 * to generate the specified part of destination rectangle.
+			 */
+			b2r2_generic_set_areas(request, request->first_node, &dst_rect_tile);
+			/* Submit the job */
+			b2r2_log_info("%s: Submitting job\n", __func__);
+
+			inc_stat(&stat_n_in_blt_add);
+
+			mutex_lock(&instance->lock);
+
+			request_id = b2r2_core_job_add(tile_job);
+
+			dec_stat(&stat_n_in_blt_add);
+
+			if (request_id < 0) {
+				b2r2_log_warn("%s: "
+					"Failed to add tile job, ret = %d\n",
+					__func__, request_id);
+				ret = request_id;
+				mutex_unlock(&instance->lock);
+				goto job_add_failed;
+			}
+
+			inc_stat(&stat_n_jobs_added);
+
+			mutex_unlock(&instance->lock);
+
+			/* Wait for the job to be done */
+			b2r2_log_info("%s: Synchronous, waiting\n",
+				__func__);
+
+			inc_stat(&stat_n_in_blt_wait);
+
+			ret = b2r2_core_job_wait(tile_job);
+
+			dec_stat(&stat_n_in_blt_wait);
+
+			if (ret < 0 && ret != -ENOENT)
+				b2r2_log_warn(
+					"%s: Failed to wait job, ret = %d\n",
+					__func__, ret);
+			else {
+				b2r2_log_info(
+					"%s: Synchronous wait done\n",
+					__func__);
+
+				nsec_active_in_b2r2 +=
+					tile_job->nsec_active_in_hw;
+			}
+			/* Release matching the addref in b2r2_core_job_add */
+			b2r2_core_job_release(tile_job, __func__);
+		}
+	}
+
+	x = 0;
+	if (dst_rect->x < 0)
+		x = -dst_rect->x;
+
+	for (; x < dst_rect->width &&
+			x + dst_rect->x < dst_img_width; x += tmp_buf_width) {
+		struct b2r2_core_job *tile_job = NULL;
+		if (x + tmp_buf_width < dst_rect->width &&
+				x + dst_rect->x + tmp_buf_width < dst_img_width) {
+			/*
+			 * Tile jobs are freed by the supplied release function
+			 * when ref_count on a tile_job reaches zero.
+			 * Do NOT allocate a tile_job for the last tile.
+			 * Send the job from the request. This way clients
+			 * will be notified when the whole blit is complete
+			 * and not just part of it.
+			 */
+			tile_job = kmalloc(sizeof(*tile_job), GFP_KERNEL);
+			if (tile_job == NULL) {
+				b2r2_log_info("%s: Failed to alloc job. "
+					"Skipping tile at (x, y)=(%d, %d)\n",
+					__func__, x, y);
+				continue;
+			}
+			tile_job->tag = request->job.tag;
+			tile_job->prio = request->job.prio;
+			tile_job->first_node_address =
+				request->job.first_node_address;
+			tile_job->last_node_address =
+				request->job.last_node_address;
+			tile_job->callback = tile_job_callback_gen;
+			tile_job->release = tile_job_release_gen;
+			tile_job->acquire_resources = job_acquire_resources_gen;
+			tile_job->release_resources = job_release_resources_gen;
+		}
+
+		dst_rect_tile.x = x;
+		if (x + dst_rect->x + tmp_buf_width > dst_img_width) {
+			/*
+			 * Only a part of the tile can be written.
+			 * Limit imposed by buffer size.
+			 */
+			dst_rect_tile.width = dst_img_width - (x + dst_rect->x);
+		} else if (x + tmp_buf_width > dst_rect->width) {
+			/*
+			 * Only a part of the tile can be written.
+			 * In this case limit imposed by dst_rect size.
+			 */
+			dst_rect_tile.width = dst_rect->width - x;
+		} else {
+			/* Whole tile can be written. */
+			dst_rect_tile.width = tmp_buf_width;
+		}
+		/*
+		 * y is now the last row. Either because the whole dst_rect
+		 * has been processed, or because the last row that will be written
+		 * to dst_img has been reached. Limits imposed in the same way
+		 * as for width.
+		 */
+		dst_rect_tile.y = y;
+		if (y + dst_rect->y + tmp_buf_height > dst_img_height)
+			dst_rect_tile.height = dst_img_height - (y + dst_rect->y);
+		else if (y + tmp_buf_height > dst_rect->height)
+			dst_rect_tile.height = dst_rect->height - y;
+		else
+			dst_rect_tile.height = tmp_buf_height;
+
+		b2r2_generic_set_areas(request, request->first_node, &dst_rect_tile);
+
+		b2r2_log_info("%s: Submitting job\n", __func__);
+		inc_stat(&stat_n_in_blt_add);
+
+		mutex_lock(&instance->lock);
+		if (x + tmp_buf_width < dst_rect->width &&
+				x + dst_rect->x + tmp_buf_width < dst_img_width) {
+			request_id = b2r2_core_job_add(tile_job);
+		} else {
+			/*
+			 * Last tile. Send the job-struct from the request.
+			 * Clients will be notified once it completes.
+			 */
+			request_id = b2r2_core_job_add(&request->job);
+		}
+
+		dec_stat(&stat_n_in_blt_add);
+
+		if (request_id < 0) {
+			b2r2_log_warn("%s: Failed to add tile job, ret = %d\n",
+				__func__, request_id);
+			ret = request_id;
+			mutex_unlock(&instance->lock);
+			if (tile_job != NULL)
+				kfree(tile_job);
+			goto job_add_failed;
+		}
+
+		inc_stat(&stat_n_jobs_added);
+		mutex_unlock(&instance->lock);
+
+		b2r2_log_info("%s: Synchronous, waiting\n",
+			__func__);
+
+		inc_stat(&stat_n_in_blt_wait);
+		if (x + tmp_buf_width < dst_rect->width &&
+				x + dst_rect->x + tmp_buf_width < dst_img_width) {
+			ret = b2r2_core_job_wait(tile_job);
+		} else {
+			/*
+			 * This is the last tile. Wait for the job-struct from
+			 * the request.
+			 */
+			ret = b2r2_core_job_wait(&request->job);
+		}
+		dec_stat(&stat_n_in_blt_wait);
+
+		if (ret < 0 && ret != -ENOENT)
+			b2r2_log_warn(
+				"%s: Failed to wait job, ret = %d\n",
+				__func__, ret);
+		else {
+			b2r2_log_info(
+				"%s: Synchronous wait done\n", __func__);
+
+			if (x + tmp_buf_width < dst_rect->width &&
+					x + dst_rect->x + tmp_buf_width < dst_img_width)
+				nsec_active_in_b2r2 += tile_job->nsec_active_in_hw;
+			else
+				nsec_active_in_b2r2 +=
+					request->job.nsec_active_in_hw;
+		}
+
+		/*
+		 * Release matching the addref in b2r2_core_job_add.
+		 * Make sure that the correct job-struct is released
+		 * when the last tile is processed.
+		 */
+		if (x + tmp_buf_width < dst_rect->width &&
+				x + dst_rect->x + tmp_buf_width < dst_img_width) {
+			b2r2_core_job_release(tile_job, __func__);
+		} else {
+			b2r2_core_job_release(&request->job, __func__);
+		}
+	}
+
+
+	ret = 0;
+
+
+	/* Unresolve the buffers */
+	unresolve_buf(&request->user_req.src_img.buf,
+		&request->src_resolved);
+	unresolve_buf(&request->user_req.src_mask.buf,
+		&request->src_mask_resolved);
+	unresolve_buf(&request->user_req.dst_img.buf,
+		&request->dst_resolved);
+
+	dec_stat(&stat_n_in_blt);
+
+	for (i = 0; i < tmp_buf_count; i++) {
+		dma_free_coherent(b2r2_blt_device(),
+			work_bufs[i].size,
+			work_bufs[i].virt_addr,
+			work_bufs[i].phys_addr);
+		memset(&(work_bufs[i]), 0, sizeof(work_bufs[i]));
+	}
+
+	if (request->profile) {
+		request->nsec_active_in_cpu =
+			(s32)((u32)task_sched_runtime(current) - thread_runtime_at_start);
+		request->total_time_nsec =
+			(s32)(b2r2_get_curr_nsec() - request->start_time_nsec);
+		request->job.nsec_active_in_hw = nsec_active_in_b2r2;
+
+		b2r2_call_profiler_blt_done(request);
+	}
+
+	return request_id;
+
+job_add_failed:
+exit_dry_run:
+generic_conf_failed:
+alloc_work_bufs_failed:
+	for (i = 0; i < 4; i++) {
+		if (work_bufs[i].virt_addr != 0) {
+			dma_free_coherent(b2r2_blt_device(),
+				work_bufs[i].size,
+				work_bufs[i].virt_addr,
+				work_bufs[i].phys_addr);
+			memset(&(work_bufs[i]), 0, sizeof(work_bufs[i]));
+		}
+	}
+
+generate_nodes_failed:
+	unresolve_buf(&request->user_req.dst_img.buf,
+		&request->dst_resolved);
+resolve_dst_buf_failed:
+	unresolve_buf(&request->user_req.src_mask.buf,
+		&request->src_mask_resolved);
+resolve_src_mask_buf_failed:
+	unresolve_buf(&request->user_req.src_img.buf,
+		&request->src_resolved);
+resolve_src_buf_failed:
+synch_interrupted:
+zero_blt:
+	job_release_gen(&request->job);
+	dec_stat(&stat_n_jobs_released);
+	dec_stat(&stat_n_in_blt);
+
+	b2r2_log_info("b2r2:%s ret=%d", __func__, ret);
+	return ret;
+}
+#endif /* CONFIG_B2R2_GENERIC */
+
+/**
+ * b2r2_blt_synch - Implements wait for all or a specified job
+ *
+ * @instance: The B2R2 BLT instance
+ * @request_id: If 0, wait for all requests on this instance to finish.
+ *              Else wait for request with given request id to finish.
+ */
+static int b2r2_blt_synch(struct b2r2_blt_instance *instance,
+			int request_id)
+{
+	int ret = 0;
+	b2r2_log_info("%s, request_id=%d\n", __func__, request_id);
+
+	if (request_id == 0) {
+		/* Wait for all requests */
+		inc_stat(&stat_n_in_synch_0);
+
+		/* Enter state "synching" if we have any active request */
+		mutex_lock(&instance->lock);
+		if (instance->no_of_active_requests)
+			instance->synching = true;
+		mutex_unlock(&instance->lock);
+
+		/* Wait until no longer in state synching */
+		ret = wait_event_interruptible(instance->synch_done_waitq,
+					!is_synching(instance));
+		dec_stat(&stat_n_in_synch_0);
+	} else {
+		struct b2r2_core_job *job;
+
+		inc_stat(&stat_n_in_synch_job);
+
+		/* Wait for specific job */
+		job = b2r2_core_job_find(request_id);
+		if (job) {
+			/* Wait on find job */
+			ret = b2r2_core_job_wait(job);
+			/* Release matching the addref in b2r2_core_job_find */
+			b2r2_core_job_release(job, __func__);
+		}
+
+		/* If job not found we assume that is has been run */
+
+		dec_stat(&stat_n_in_synch_job);
+	}
+
+	b2r2_log_info(
+		"%s, request_id=%d, returns %d\n", __func__, request_id, ret);
+
+	return ret;
+}
+
+/**
+ * Query B2R2 capabilities
+ *
+ * @instance: The B2R2 BLT instance
+ * @query_cap: The structure receiving the capabilities
+ */
+static int b2r2_blt_query_cap(struct b2r2_blt_instance *instance,
+		struct b2r2_blt_query_cap *query_cap)
+{
+	/* FIXME: Not implemented yet */
+	return -ENOSYS;
+}
+
+static void get_actual_dst_rect(struct b2r2_blt_req *req,
+					struct b2r2_blt_rect *actual_dst_rect)
+{
+	struct b2r2_blt_rect dst_img_bounds;
+
+	b2r2_get_img_bounding_rect(&req->dst_img, &dst_img_bounds);
+
+	b2r2_intersect_rects(&req->dst_rect, &dst_img_bounds, actual_dst_rect);
+
+	if (req->flags & B2R2_BLT_FLAG_DESTINATION_CLIP)
+		b2r2_intersect_rects(actual_dst_rect, &req->dst_clip_rect,
+							actual_dst_rect);
+}
+
+static void set_up_hwmem_region(struct b2r2_blt_img *img,
+		struct b2r2_blt_rect *rect, struct hwmem_region *region)
+{
+	s32 img_size;
+
+	memset(region, 0, sizeof(*region));
+
+	if (b2r2_is_zero_area_rect(rect))
+		return;
+
+	img_size = b2r2_get_img_size(img);
+
+	if (b2r2_is_single_plane_fmt(img->fmt) &&
+				b2r2_is_independent_pixel_fmt(img->fmt)) {
+		int img_fmt_bpp = b2r2_get_fmt_bpp(img->fmt);
+		u32 img_pitch = b2r2_get_img_pitch(img);
+
+		region->offset = (u32)(img->buf.offset + (rect->y *
+								img_pitch));
+		region->count = (u32)rect->height;
+		region->start = (u32)((rect->x * img_fmt_bpp) / 8);
+		region->end = (u32)b2r2_div_round_up(
+				(rect->x + rect->width) * img_fmt_bpp, 8);
+		region->size = img_pitch;
+	} else {
+		/*
+		 * TODO: Locking entire buffer as a quick safe solution. In the
+		 * future we should lock less to avoid unecessary cache
+		 * synching. Pixel interleaved YCbCr formats should be quite
+		 * easy, just align start and stop points on 2.
+		 */
+		region->offset = (u32)img->buf.offset;
+		region->count = 1;
+		region->start = 0;
+		region->end = (u32)img_size;
+		region->size = (u32)img_size;
+	}
+}
+
+static int resolve_hwmem(struct b2r2_blt_img *img,
+		struct b2r2_blt_rect *rect_2b_used,
+		bool is_dst,
+		struct b2r2_resolved_buf *resolved_buf)
+{
+	int return_value = 0;
+	enum hwmem_mem_type mem_type;
+	enum hwmem_access access;
+	enum hwmem_access required_access;
+	struct hwmem_mem_chunk mem_chunk;
+	size_t mem_chunk_length = 1;
+	struct hwmem_region region;
+
+	resolved_buf->hwmem_alloc =
+			hwmem_resolve_by_name(img->buf.hwmem_buf_name);
+	if (IS_ERR(resolved_buf->hwmem_alloc)) {
+		return_value = PTR_ERR(resolved_buf->hwmem_alloc);
+		b2r2_log_info("%s: hwmem_resolve_by_name failed, "
+				"error code: %i\n", __func__, return_value);
+		goto resolve_failed;
+	}
+
+	hwmem_get_info(resolved_buf->hwmem_alloc, &resolved_buf->file_len,
+			&mem_type, &access);
+
+	required_access = (is_dst ? HWMEM_ACCESS_WRITE : HWMEM_ACCESS_READ) |
+							HWMEM_ACCESS_IMPORT;
+	if ((required_access & access) != required_access) {
+		b2r2_log_info("%s: Insufficient access to hwmem buffer.\n",
+				__func__);
+		return_value = -EACCES;
+		goto access_check_failed;
+	}
+
+	if (mem_type != HWMEM_MEM_CONTIGUOUS_SYS) {
+		b2r2_log_info("%s: Hwmem buffer is scattered.\n", __func__);
+		return_value = -EINVAL;
+		goto buf_scattered;
+	}
+
+	if (resolved_buf->file_len <
+			img->buf.offset + (__u32)b2r2_get_img_size(img)) {
+		b2r2_log_info("%s: Hwmem buffer too small.\n", __func__);
+		return_value = -EINVAL;
+		goto size_check_failed;
+	}
+
+	return_value = hwmem_pin(resolved_buf->hwmem_alloc, &mem_chunk,
+							 &mem_chunk_length);
+	if (return_value < 0) {
+		b2r2_log_info("%s: hwmem_pin failed, "
+				"error code: %i\n", __func__, return_value);
+		goto pin_failed;
+	}
+	resolved_buf->file_physical_start = mem_chunk.paddr;
+
+	set_up_hwmem_region(img, rect_2b_used, &region);
+	return_value = hwmem_set_domain(resolved_buf->hwmem_alloc,
+				required_access, HWMEM_DOMAIN_SYNC, &region);
+	if (return_value < 0) {
+		b2r2_log_info("%s: hwmem_set_domain failed, "
+				"error code: %i\n", __func__, return_value);
+		goto set_domain_failed;
+	}
+
+	resolved_buf->physical_address =
+			resolved_buf->file_physical_start + img->buf.offset;
+
+	goto out;
+
+set_domain_failed:
+	hwmem_unpin(resolved_buf->hwmem_alloc);
+pin_failed:
+size_check_failed:
+buf_scattered:
+access_check_failed:
+	hwmem_release(resolved_buf->hwmem_alloc);
+resolve_failed:
+
+out:
+	return return_value;
+}
+
+static void unresolve_hwmem(struct b2r2_resolved_buf *resolved_buf)
+{
+	hwmem_unpin(resolved_buf->hwmem_alloc);
+	hwmem_release(resolved_buf->hwmem_alloc);
+}
+
+/**
+ * unresolve_buf() - Must be called after resolve_buf
+ *
+ * @buf: The buffer specification as supplied from user space
+ * @resolved: Gathered information about the buffer
+ *
+ * Returns 0 if OK else negative error code
+ */
+static void unresolve_buf(struct b2r2_blt_buf *buf,
+			struct b2r2_resolved_buf *resolved)
+{
+#ifdef CONFIG_ANDROID_PMEM
+	if (resolved->is_pmem && resolved->filep)
+		put_pmem_file(resolved->filep);
+#endif
+	if (resolved->hwmem_alloc != NULL)
+		unresolve_hwmem(resolved);
+}
+
+/**
+ * resolve_buf() - Returns the physical & virtual addresses of a B2R2 blt buffer
+ *
+ * @img: The image specification as supplied from user space
+ * @rect_2b_used: The part of the image b2r2 will use.
+ * @usage: Specifies how the buffer will be used.
+ * @resolved: Gathered information about the buffer
+ *
+ * Returns 0 if OK else negative error code
+ */
+static int resolve_buf(struct b2r2_blt_img *img,
+		struct b2r2_blt_rect *rect_2b_used,
+		bool is_dst,
+		struct b2r2_resolved_buf *resolved)
+{
+	int ret = 0;
+
+	memset(resolved, 0, sizeof(*resolved));
+
+	switch (img->buf.type) {
+	case B2R2_BLT_PTR_NONE:
+		break;
+
+	case B2R2_BLT_PTR_PHYSICAL:
+		resolved->physical_address = img->buf.offset;
+		resolved->file_len = img->buf.len;
+		break;
+
+		/* FD + OFFSET type */
+	case B2R2_BLT_PTR_FD_OFFSET: {
+		/*
+		 * TODO: Do we need to check if the process is allowed to
+		 * read/write (depending on if it's dst or src) to the file?
+		 */
+		struct file *file;
+		int put_needed;
+		int i;
+
+#ifdef CONFIG_ANDROID_PMEM
+		if (!get_pmem_file(
+			img->buf.fd,
+			(unsigned long *) &resolved->file_physical_start,
+			(unsigned long *) &resolved->file_virtual_start,
+			(unsigned long *) &resolved->file_len,
+			&resolved->filep)) {
+			resolved->physical_address =
+				resolved->file_physical_start +
+				img->buf.offset;
+			resolved->virtual_address = (void *)
+				(resolved->file_virtual_start +
+				img->buf.offset);
+			resolved->is_pmem = true;
+		} else
+#endif
+		{
+			/* Will be set to 0 if a matching dev is found */
+			ret = -EINVAL;
+
+			file = fget_light(img->buf.fd, &put_needed);
+			if (file == NULL)
+				return -EINVAL;
+#ifdef CONFIG_FB
+			if (MAJOR(file->f_dentry->d_inode->i_rdev) == FB_MAJOR) {
+				/*
+				 * This is a frame buffer device, find fb_info
+				 * (OK to do it like this, no locking???)
+				 */
+
+				for (i = 0; i < num_registered_fb; i++) {
+					struct fb_info *info = registered_fb[i];
+
+					if (info && info->dev &&
+							MINOR(info->dev->devt) ==
+							MINOR(file->f_dentry->d_inode->i_rdev)) {
+						resolved->file_physical_start =
+							info->fix.smem_start;
+						resolved->file_virtual_start =
+							(u32)info->screen_base;
+						resolved->file_len =
+							info->fix.smem_len;
+
+						resolved->physical_address =
+							resolved->file_physical_start +
+							img->buf.offset;
+						resolved->virtual_address =
+							(void *)(resolved->file_virtual_start +
+							img->buf.offset);
+
+						ret = 0;
+						break;
+					}
+				}
+			}
+#endif
+
+			fput_light(file, put_needed);
+		}
+
+		/* Check bounds */
+		if (ret >= 0 && img->buf.offset + img->buf.len >
+				resolved->file_len) {
+			ret = -ESPIPE;
+			unresolve_buf(&img->buf, resolved);
+		}
+
+		break;
+	}
+
+	case B2R2_BLT_PTR_HWMEM_BUF_NAME_OFFSET:
+		ret = resolve_hwmem(img, rect_2b_used, is_dst, resolved);
+		break;
+
+	default:
+		b2r2_log_warn(
+			"%s: Failed to resolve buf type %d\n",
+			__func__, img->buf.type);
+
+		ret = -EINVAL;
+		break;
+
+	}
+
+	return ret;
+}
+
+/**
+ * sync_buf - Synchronizes the memory occupied by an image buffer.
+ *
+ * @buf: User buffer specification
+ * @resolved_buf: Gathered info (physical address etc.) about buffer
+ * @is_dst: true if the buffer is a destination buffer, false if the buffer is a
+ *          source buffer.
+ * @rect: rectangle in the image buffer that should be synced. NULL the buffer is a source mask.
+ * @img_width: width of the complete image buffer
+ * @fmt: buffer format
+*/
+static void sync_buf(struct b2r2_blt_img *img,
+		struct b2r2_resolved_buf *resolved,
+		bool is_dst,
+		struct b2r2_blt_rect *rect)
+{
+	struct sync_args sa;
+	u32 start_phys, end_phys;
+
+	if (B2R2_BLT_PTR_NONE == img->buf.type ||
+			B2R2_BLT_PTR_HWMEM_BUF_NAME_OFFSET == img->buf.type)
+		return;
+
+	start_phys = resolved->physical_address;
+	end_phys = resolved->physical_address + img->buf.len;
+
+	/*
+	 * TODO: Very ugly. We should find out whether the memory is coherent in
+	 * some generic way but cache handling will be rewritten soon so there
+	 * is no use spending time on it. In the new design this will probably
+	 * not be a problem.
+	 */
+	/* Frame buffer is coherent, at least now. */
+	if (!resolved->is_pmem) {
+		/*
+		 * Drain the write buffers as they are not always part of the
+		 * coherent concept.
+		 */
+		wmb();
+
+		return;
+	}
+
+	/*
+	 * src_mask does not have rect.
+	 * Also flush full buffer for planar and semiplanar YUV formats
+	 */
+	if (rect == NULL ||
+			(img->fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR) ||
+			(img->fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR) ||
+			(img->fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR) ||
+			(img->fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR) ||
+			(img->fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR) ||
+			(img->fmt ==
+				B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE) ||
+			(img->fmt ==
+				B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE)) {
+		sa.start = (unsigned long)resolved->virtual_address;
+		sa.end = (unsigned long)resolved->virtual_address + img->buf.len;
+		start_phys = resolved->physical_address;
+		end_phys = resolved->physical_address + img->buf.len;
+	} else {
+		/* buffer is not a src_mask so make use of rect when clean & flush caches*/
+		u32 bpp;	/* Bits per pixel */
+		u32 pitch;
+
+		switch (img->fmt) {
+		case B2R2_BLT_FMT_16_BIT_ARGB4444: /* Fall through */
+		case B2R2_BLT_FMT_16_BIT_ARGB1555: /* Fall through */
+		case B2R2_BLT_FMT_16_BIT_RGB565:   /* Fall through */
+		case B2R2_BLT_FMT_Y_CB_Y_CR:       /* Fall through */
+		case B2R2_BLT_FMT_CB_Y_CR_Y:
+			bpp = 16;
+			break;
+		case B2R2_BLT_FMT_24_BIT_RGB888:   /* Fall through */
+		case B2R2_BLT_FMT_24_BIT_ARGB8565: /* Fall through */
+		case B2R2_BLT_FMT_24_BIT_YUV888:
+			bpp =  24;
+			break;
+		case B2R2_BLT_FMT_32_BIT_ARGB8888: /* Fall through */
+		case B2R2_BLT_FMT_32_BIT_ABGR8888: /* Fall through */
+		case B2R2_BLT_FMT_32_BIT_AYUV8888:
+			bpp = 32;
+			break;
+		default:
+			bpp = 12;
+		}
+		if (img->pitch == 0)
+			pitch = (img->width * bpp) / 8;
+		else
+			pitch = img->pitch;
+
+		/*
+		 * For 422I formats 2 horizontal pixels share color data.
+		 * Thus, the x position must be aligned down to closest even
+		 * number and width must be aligned up.
+		 */
+		{
+			s32 x;
+			s32 width;
+
+			switch (img->fmt) {
+			case B2R2_BLT_FMT_Y_CB_Y_CR:       /* Fall through */
+			case B2R2_BLT_FMT_CB_Y_CR_Y:
+				x = (rect->x / 2) * 2;
+				width = ((rect->width + 1) / 2) * 2;
+				break;
+			default:
+				x = rect->x;
+				width = rect->width;
+				break;
+			}
+
+			sa.start = (unsigned long)resolved->virtual_address +
+					rect->y * pitch + (x * bpp) / 8;
+			sa.end = (unsigned long)sa.start +
+					(rect->height - 1) * pitch +
+					(width * bpp) / 8;
+
+			start_phys = resolved->physical_address +
+					rect->y * pitch + (x * bpp) / 8;
+			end_phys = start_phys +
+					(rect->height - 1) * pitch +
+					(width * bpp) / 8;
+		}
+	}
+
+	/*
+	 * The virtual address to a pmem buffer is retrieved from ioremap, not
+	 * sure if it's	ok to use such an address as a kernel virtual address.
+	 * When doing it at a higher level such as dma_map_single it triggers an
+	 * error but at lower levels such as dmac_clean_range it seems to work,
+	 * hence the low level stuff.
+	 */
+
+	if (is_dst) {
+		/*
+		 * According to ARM's docs you must clean before invalidating
+		 * (ie flush) to avoid loosing data.
+		 */
+
+		/* Flush L1 cache */
+#ifdef CONFIG_SMP
+		flush_l1_cache_range_all_cpus(&sa);
+#else
+		flush_l1_cache_range_curr_cpu(&sa);
+#endif
+
+		/* Flush L2 cache */
+		outer_flush_range(start_phys, end_phys);
+	} else {
+		/* Clean L1 cache */
+#ifdef CONFIG_SMP
+		clean_l1_cache_range_all_cpus(&sa);
+#else
+		clean_l1_cache_range_curr_cpu(&sa);
+#endif
+
+		/* Clean L2 cache */
+		outer_clean_range(start_phys, end_phys);
+	}
+}
+
+/**
+ * is_report_list_empty() - Spin lock protected check of report list
+ *
+ * @instance: The B2R2 BLT instance
+ */
+static bool is_report_list_empty(struct b2r2_blt_instance *instance)
+{
+	bool is_empty;
+
+	mutex_lock(&instance->lock);
+	is_empty = list_empty(&instance->report_list);
+	mutex_unlock(&instance->lock);
+
+	return is_empty;
+}
+
+/**
+ * is_synching() - Spin lock protected check if synching
+ *
+ * @instance: The B2R2 BLT instance
+ */
+static bool is_synching(struct b2r2_blt_instance *instance)
+{
+	bool is_synching;
+
+	mutex_lock(&instance->lock);
+	is_synching = instance->synching;
+	mutex_unlock(&instance->lock);
+
+	return is_synching;
+}
+
+/**
+ * b2r2_blt_devide() - Returns the B2R2 blt device for logging
+ */
+struct device *b2r2_blt_device(void)
+{
+	return b2r2_blt_dev ? b2r2_blt_dev->this_device : NULL;
+}
+
+/**
+ * inc_stat() - Spin lock protected increment of statistics variable
+ *
+ * @stat: Pointer to statistics variable that should be incremented
+ */
+static void inc_stat(unsigned long *stat)
+{
+	mutex_lock(&stat_lock);
+	(*stat)++;
+	mutex_unlock(&stat_lock);
+}
+
+/**
+ * inc_stat() - Spin lock protected decrement of statistics variable
+ *
+ * @stat: Pointer to statistics variable that should be decremented
+ */
+static void dec_stat(unsigned long *stat)
+{
+	mutex_lock(&stat_lock);
+	(*stat)--;
+	mutex_unlock(&stat_lock);
+}
+
+
+#ifdef CONFIG_DEBUG_FS
+/**
+ * sprintf_req() - Builds a string representing the request, for debug
+ *
+ * @request:Request that should be encoded into a string
+ * @buf: Receiving buffer
+ * @size: Size of receiving buffer
+ *
+ * Returns number of characters in string, excluding null terminator
+ */
+static int sprintf_req(struct b2r2_blt_request *request, char *buf, int size)
+{
+	size_t dev_size = 0;
+
+	dev_size += sprintf(buf + dev_size,
+			"instance: %p\n\n",
+			request->instance);
+
+	dev_size += sprintf(buf + dev_size,
+			"size: %d bytes\n",
+			request->user_req.size);
+	dev_size += sprintf(buf + dev_size,
+			"flags: %8lX\n",
+			(unsigned long) request->user_req.flags);
+	dev_size += sprintf(buf + dev_size,
+			"transform: %3lX\n",
+			(unsigned long) request->user_req.transform);
+	dev_size += sprintf(buf + dev_size,
+			"prio: %d\n",
+			request->user_req.transform);
+	dev_size += sprintf(buf + dev_size,
+			"src_img.fmt: %#010x\n",
+			request->user_req.src_img.fmt);
+	dev_size += sprintf(buf + dev_size,
+			"src_img.buf: {type=%d,hwmem_buf_name=%d,fd=%d,"
+				"offset=%d,len=%d}\n",
+			request->user_req.src_img.buf.type,
+			request->user_req.src_img.buf.hwmem_buf_name,
+			request->user_req.src_img.buf.fd,
+			request->user_req.src_img.buf.offset,
+			request->user_req.src_img.buf.len);
+	dev_size += sprintf(buf + dev_size,
+			"src_img.{width=%d,height=%d,pitch=%d}\n",
+			request->user_req.src_img.width,
+			request->user_req.src_img.height,
+			request->user_req.src_img.pitch);
+	dev_size += sprintf(buf + dev_size,
+			"src_mask.fmt: %#010x\n",
+			request->user_req.src_mask.fmt);
+	dev_size += sprintf(buf + dev_size,
+			"src_mask.buf: {type=%d,hwmem_buf_name=%d,fd=%d,"
+				"offset=%d,len=%d}\n",
+			request->user_req.src_mask.buf.type,
+			request->user_req.src_mask.buf.hwmem_buf_name,
+			request->user_req.src_mask.buf.fd,
+			request->user_req.src_mask.buf.offset,
+			request->user_req.src_mask.buf.len);
+	dev_size += sprintf(buf + dev_size,
+			"src_mask.{width=%d,height=%d,pitch=%d}\n",
+			request->user_req.src_mask.width,
+			request->user_req.src_mask.height,
+			request->user_req.src_mask.pitch);
+	dev_size += sprintf(buf + dev_size,
+			"src_rect.{x=%d,y=%d,width=%d,height=%d}\n",
+			request->user_req.src_rect.x,
+			request->user_req.src_rect.y,
+			request->user_req.src_rect.width,
+			request->user_req.src_rect.height);
+	dev_size += sprintf(buf + dev_size,
+			"src_color=%08lX\n",
+			(unsigned long) request->user_req.src_color);
+
+	dev_size += sprintf(buf + dev_size,
+			"dst_img.fmt: %#010x\n",
+			request->user_req.dst_img.fmt);
+	dev_size += sprintf(buf + dev_size,
+			"dst_img.buf: {type=%d,hwmem_buf_name=%d,fd=%d,"
+				"offset=%d,len=%d}\n",
+			request->user_req.dst_img.buf.type,
+			request->user_req.dst_img.buf.hwmem_buf_name,
+			request->user_req.dst_img.buf.fd,
+			request->user_req.dst_img.buf.offset,
+			request->user_req.dst_img.buf.len);
+	dev_size += sprintf(buf + dev_size,
+			"dst_img.{width=%d,height=%d,pitch=%d}\n",
+			request->user_req.dst_img.width,
+			request->user_req.dst_img.height,
+			request->user_req.dst_img.pitch);
+	dev_size += sprintf(buf + dev_size,
+			"dst_rect.{x=%d,y=%d,width=%d,height=%d}\n",
+			request->user_req.dst_rect.x,
+			request->user_req.dst_rect.y,
+			request->user_req.dst_rect.width,
+			request->user_req.dst_rect.height);
+	dev_size += sprintf(buf + dev_size,
+			"dst_clip_rect.{x=%d,y=%d,width=%d,height=%d}\n",
+			request->user_req.dst_clip_rect.x,
+			request->user_req.dst_clip_rect.y,
+			request->user_req.dst_clip_rect.width,
+			request->user_req.dst_clip_rect.height);
+	dev_size += sprintf(buf + dev_size,
+			"dst_color=%08lX\n",
+			(unsigned long) request->user_req.dst_color);
+	dev_size += sprintf(buf + dev_size,
+			"global_alpha=%d\n",
+			(int) request->user_req.global_alpha);
+	dev_size += sprintf(buf + dev_size,
+			"report1=%08lX\n",
+			(unsigned long) request->user_req.report1);
+	dev_size += sprintf(buf + dev_size,
+			"report2=%08lX\n",
+			(unsigned long) request->user_req.report2);
+
+	dev_size += sprintf(buf + dev_size,
+			"request_id: %d\n",
+			request->request_id);
+
+	dev_size += sprintf(buf + dev_size,
+			"src_resolved.physical: %lX\n",
+			(unsigned long) request->src_resolved.
+			physical_address);
+	dev_size += sprintf(buf + dev_size,
+			"src_resolved.virtual: %p\n",
+			request->src_resolved.virtual_address);
+	dev_size += sprintf(buf + dev_size,
+			"src_resolved.filep: %p\n",
+			request->src_resolved.filep);
+	dev_size += sprintf(buf + dev_size,
+			"src_resolved.filep_physical_start: %lX\n",
+			(unsigned long) request->src_resolved.
+			file_physical_start);
+	dev_size += sprintf(buf + dev_size,
+			"src_resolved.filep_virtual_start: %p\n",
+			(void *) request->src_resolved.file_virtual_start);
+	dev_size += sprintf(buf + dev_size,
+			"src_resolved.file_len: %d\n",
+			request->src_resolved.file_len);
+
+	dev_size += sprintf(buf + dev_size,
+			"src_mask_resolved.physical: %lX\n",
+			(unsigned long) request->src_mask_resolved.
+			physical_address);
+	dev_size += sprintf(buf + dev_size,
+			"src_mask_resolved.virtual: %p\n",
+			request->src_mask_resolved.virtual_address);
+	dev_size += sprintf(buf + dev_size,
+			"src_mask_resolved.filep: %p\n",
+			request->src_mask_resolved.filep);
+	dev_size += sprintf(buf + dev_size,
+			"src_mask_resolved.filep_physical_start: %lX\n",
+			(unsigned long) request->src_mask_resolved.
+			file_physical_start);
+	dev_size += sprintf(buf + dev_size,
+			"src_mask_resolved.filep_virtual_start: %p\n",
+			(void *) request->src_mask_resolved.
+			file_virtual_start);
+	dev_size += sprintf(buf + dev_size,
+			"src_mask_resolved.file_len: %d\n",
+			request->src_mask_resolved.file_len);
+
+	dev_size += sprintf(buf + dev_size,
+			"dst_resolved.physical: %lX\n",
+			(unsigned long) request->dst_resolved.
+			physical_address);
+	dev_size += sprintf(buf + dev_size,
+			"dst_resolved.virtual: %p\n",
+			request->dst_resolved.virtual_address);
+	dev_size += sprintf(buf + dev_size,
+			"dst_resolved.filep: %p\n",
+			request->dst_resolved.filep);
+	dev_size += sprintf(buf + dev_size,
+			"dst_resolved.filep_physical_start: %lX\n",
+			(unsigned long) request->dst_resolved.
+			file_physical_start);
+	dev_size += sprintf(buf + dev_size,
+			"dst_resolved.filep_virtual_start: %p\n",
+			(void *) request->dst_resolved.file_virtual_start);
+	dev_size += sprintf(buf + dev_size,
+			"dst_resolved.file_len: %d\n",
+			request->dst_resolved.file_len);
+
+	return dev_size;
+}
+
+/**
+ * debugfs_b2r2_blt_request_read() - Implements debugfs read for B2R2 register
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static int debugfs_b2r2_blt_request_read(struct file *filp, char __user *buf,
+			size_t count, loff_t *f_pos)
+{
+	size_t dev_size = 0;
+	int ret = 0;
+	char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+	if (Buf == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	dev_size = sprintf_req(&debugfs_latest_request, Buf, sizeof(char) * 4096);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	if (Buf != NULL)
+		kfree(Buf);
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_blt_request_fops - File operations for B2R2 request debugfs
+ */
+static const struct file_operations debugfs_b2r2_blt_request_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_blt_request_read,
+};
+
+/**
+ * struct debugfs_reg - Represents a B2R2 node "register"
+ *
+ * @name: Register name
+ * @offset: Offset within the node
+ */
+struct debugfs_reg {
+	const char  name[30];
+	u32   offset;
+};
+
+/**
+ * debugfs_node_regs - Array with all the registers in a B2R2 node, for debug
+ */
+static const struct debugfs_reg debugfs_node_regs[] = {
+	{"GROUP0.B2R2_NIP", offsetof(struct b2r2_link_list, GROUP0.B2R2_NIP)},
+	{"GROUP0.B2R2_CIC", offsetof(struct b2r2_link_list, GROUP0.B2R2_CIC)},
+	{"GROUP0.B2R2_INS", offsetof(struct b2r2_link_list, GROUP0.B2R2_INS)},
+	{"GROUP0.B2R2_ACK", offsetof(struct b2r2_link_list, GROUP0.B2R2_ACK)},
+
+	{"GROUP1.B2R2_TBA", offsetof(struct b2r2_link_list, GROUP1.B2R2_TBA)},
+	{"GROUP1.B2R2_TTY", offsetof(struct b2r2_link_list, GROUP1.B2R2_TTY)},
+	{"GROUP1.B2R2_TXY", offsetof(struct b2r2_link_list, GROUP1.B2R2_TXY)},
+	{"GROUP1.B2R2_TSZ", offsetof(struct b2r2_link_list, GROUP1.B2R2_TSZ)},
+
+	{"GROUP2.B2R2_S1CF", offsetof(struct b2r2_link_list, GROUP2.B2R2_S1CF)},
+	{"GROUP2.B2R2_S2CF", offsetof(struct b2r2_link_list, GROUP2.B2R2_S2CF)},
+
+	{"GROUP3.B2R2_SBA", offsetof(struct b2r2_link_list, GROUP3.B2R2_SBA)},
+	{"GROUP3.B2R2_STY", offsetof(struct b2r2_link_list, GROUP3.B2R2_STY)},
+	{"GROUP3.B2R2_SXY", offsetof(struct b2r2_link_list, GROUP3.B2R2_SXY)},
+	{"GROUP3.B2R2_SSZ", offsetof(struct b2r2_link_list, GROUP3.B2R2_SSZ)},
+
+	{"GROUP4.B2R2_SBA", offsetof(struct b2r2_link_list, GROUP4.B2R2_SBA)},
+	{"GROUP4.B2R2_STY", offsetof(struct b2r2_link_list, GROUP4.B2R2_STY)},
+	{"GROUP4.B2R2_SXY", offsetof(struct b2r2_link_list, GROUP4.B2R2_SXY)},
+	{"GROUP4.B2R2_SSZ", offsetof(struct b2r2_link_list, GROUP4.B2R2_SSZ)},
+
+	{"GROUP5.B2R2_SBA", offsetof(struct b2r2_link_list, GROUP5.B2R2_SBA)},
+	{"GROUP5.B2R2_STY", offsetof(struct b2r2_link_list, GROUP5.B2R2_STY)},
+	{"GROUP5.B2R2_SXY", offsetof(struct b2r2_link_list, GROUP5.B2R2_SXY)},
+	{"GROUP5.B2R2_SSZ", offsetof(struct b2r2_link_list, GROUP5.B2R2_SSZ)},
+
+	{"GROUP6.B2R2_CWO", offsetof(struct b2r2_link_list, GROUP6.B2R2_CWO)},
+	{"GROUP6.B2R2_CWS", offsetof(struct b2r2_link_list, GROUP6.B2R2_CWS)},
+
+	{"GROUP7.B2R2_CCO", offsetof(struct b2r2_link_list, GROUP7.B2R2_CCO)},
+	{"GROUP7.B2R2_CML", offsetof(struct b2r2_link_list, GROUP7.B2R2_CML)},
+
+	{"GROUP8.B2R2_FCTL", offsetof(struct b2r2_link_list, GROUP8.B2R2_FCTL)},
+	{"GROUP8.B2R2_PMK", offsetof(struct b2r2_link_list, GROUP8.B2R2_PMK)},
+
+	{"GROUP9.B2R2_RSF", offsetof(struct b2r2_link_list, GROUP9.B2R2_RSF)},
+	{"GROUP9.B2R2_RZI", offsetof(struct b2r2_link_list, GROUP9.B2R2_RZI)},
+	{"GROUP9.B2R2_HFP", offsetof(struct b2r2_link_list, GROUP9.B2R2_HFP)},
+	{"GROUP9.B2R2_VFP", offsetof(struct b2r2_link_list, GROUP9.B2R2_VFP)},
+
+	{"GROUP10.B2R2_RSF", offsetof(struct b2r2_link_list, GROUP10.B2R2_RSF)},
+	{"GROUP10.B2R2_RZI", offsetof(struct b2r2_link_list, GROUP10.B2R2_RZI)},
+	{"GROUP10.B2R2_HFP", offsetof(struct b2r2_link_list, GROUP10.B2R2_HFP)},
+	{"GROUP10.B2R2_VFP", offsetof(struct b2r2_link_list, GROUP10.B2R2_VFP)},
+
+	{"GROUP11.B2R2_FF0", offsetof(struct b2r2_link_list,
+					GROUP11.B2R2_FF0)},
+	{"GROUP11.B2R2_FF1", offsetof(struct b2r2_link_list,
+					GROUP11.B2R2_FF1)},
+	{"GROUP11.B2R2_FF2", offsetof(struct b2r2_link_list,
+					GROUP11.B2R2_FF2)},
+	{"GROUP11.B2R2_FF3", offsetof(struct b2r2_link_list,
+					GROUP11.B2R2_FF3)},
+
+	{"GROUP12.B2R2_KEY1", offsetof(struct b2r2_link_list,
+				GROUP12.B2R2_KEY1)},
+	{"GROUP12.B2R2_KEY2", offsetof(struct b2r2_link_list,
+				GROUP12.B2R2_KEY2)},
+
+	{"GROUP13.B2R2_XYL", offsetof(struct b2r2_link_list, GROUP13.B2R2_XYL)},
+	{"GROUP13.B2R2_XYP", offsetof(struct b2r2_link_list, GROUP13.B2R2_XYP)},
+
+	{"GROUP14.B2R2_SAR", offsetof(struct b2r2_link_list, GROUP14.B2R2_SAR)},
+	{"GROUP14.B2R2_USR", offsetof(struct b2r2_link_list, GROUP14.B2R2_USR)},
+
+	{"GROUP15.B2R2_VMX0", offsetof(struct b2r2_link_list,
+				GROUP15.B2R2_VMX0)},
+	{"GROUP15.B2R2_VMX1", offsetof(struct b2r2_link_list,
+				GROUP15.B2R2_VMX1)},
+	{"GROUP15.B2R2_VMX2", offsetof(struct b2r2_link_list,
+				GROUP15.B2R2_VMX2)},
+	{"GROUP15.B2R2_VMX3", offsetof(struct b2r2_link_list,
+				GROUP15.B2R2_VMX3)},
+
+	{"GROUP16.B2R2_VMX0", offsetof(struct b2r2_link_list,
+				GROUP16.B2R2_VMX0)},
+	{"GROUP16.B2R2_VMX1", offsetof(struct b2r2_link_list,
+				GROUP16.B2R2_VMX1)},
+	{"GROUP16.B2R2_VMX2", offsetof(struct b2r2_link_list,
+				GROUP16.B2R2_VMX2)},
+	{"GROUP16.B2R2_VMX3", offsetof(struct b2r2_link_list,
+				GROUP16.B2R2_VMX3)},
+};
+
+/**
+ * debugfs_b2r2_blt_stat_read() - Implements debugfs read for B2R2 BLT
+ *                                statistics
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static int debugfs_b2r2_blt_stat_read(struct file *filp, char __user *buf,
+				size_t count, loff_t *f_pos)
+{
+	size_t dev_size = 0;
+	int ret = 0;
+	char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+	if (Buf == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	mutex_lock(&stat_lock);
+	dev_size += sprintf(Buf + dev_size, "Added jobs: %lu\n",
+			stat_n_jobs_added);
+	dev_size += sprintf(Buf + dev_size, "Released jobs: %lu\n",
+			stat_n_jobs_released);
+	dev_size += sprintf(Buf + dev_size, "Jobs in report list: %lu\n",
+			stat_n_jobs_in_report_list);
+	dev_size += sprintf(Buf + dev_size, "Clients in open: %lu\n",
+			stat_n_in_open);
+	dev_size += sprintf(Buf + dev_size, "Clients in release: %lu\n",
+			stat_n_in_release);
+	dev_size += sprintf(Buf + dev_size, "Clients in blt: %lu\n",
+			stat_n_in_blt);
+	dev_size += sprintf(Buf + dev_size, "              synch: %lu\n",
+			stat_n_in_blt_synch);
+	dev_size += sprintf(Buf + dev_size, "              add: %lu\n",
+			stat_n_in_blt_add);
+	dev_size += sprintf(Buf + dev_size, "              wait: %lu\n",
+			stat_n_in_blt_wait);
+	dev_size += sprintf(Buf + dev_size, "Clients in synch 0: %lu\n",
+			stat_n_in_synch_0);
+	dev_size += sprintf(Buf + dev_size, "Clients in synch job: %lu\n",
+			stat_n_in_synch_job);
+	dev_size += sprintf(Buf + dev_size, "Clients in query_cap: %lu\n",
+			stat_n_in_query_cap);
+	mutex_unlock(&stat_lock);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	if (Buf != NULL)
+		kfree(Buf);
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_blt_stat_fops() - File operations for B2R2 BLT
+ *                                statistics debugfs
+ */
+static const struct file_operations debugfs_b2r2_blt_stat_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_blt_stat_read,
+};
+#endif
+
+static void init_tmp_bufs(void)
+{
+	int i = 0;
+
+	for (i = 0; i < MAX_TMP_BUFS_NEEDED; i++) {
+		tmp_bufs[i].buf.virt_addr = dma_alloc_coherent(
+			b2r2_blt_device(), MAX_TMP_BUF_SIZE,
+			&tmp_bufs[i].buf.phys_addr, GFP_DMA);
+		if (tmp_bufs[i].buf.virt_addr != NULL)
+			tmp_bufs[i].buf.size = MAX_TMP_BUF_SIZE;
+		else {
+			b2r2_log_err("%s: Failed to allocate temp buffer %i\n",
+								__func__, i);
+
+			tmp_bufs[i].buf.size = 0;
+		}
+	}
+}
+
+static void destroy_tmp_bufs(void)
+{
+	int i = 0;
+
+	for (i = 0; i < MAX_TMP_BUFS_NEEDED; i++) {
+		if (tmp_bufs[i].buf.size != 0) {
+			dma_free_coherent(b2r2_blt_device(),
+						tmp_bufs[i].buf.size,
+						tmp_bufs[i].buf.virt_addr,
+						tmp_bufs[i].buf.phys_addr);
+
+			tmp_bufs[i].buf.size = 0;
+		}
+	}
+}
+
+/**
+ * b2r2_blt_module_init() - Module init function
+ *
+ * Returns 0 if OK else negative error code
+ */
+int b2r2_blt_module_init(void)
+{
+	int ret;
+
+	mutex_init(&stat_lock);
+
+#ifdef CONFIG_B2R2_GENERIC
+	/* Initialize generic path */
+	b2r2_generic_init();
+#endif
+
+	/* Initialize node splitter */
+	ret = b2r2_node_split_init();
+	if (ret) {
+		printk(KERN_WARNING "%s: node split init fails\n",
+			__func__);
+		goto b2r2_node_split_init_fail;
+	}
+
+	/* Register b2r2 driver */
+	ret = misc_register(&b2r2_blt_misc_dev);
+	if (ret) {
+		printk(KERN_WARNING "%s: registering misc device fails\n",
+			__func__);
+		goto b2r2_misc_register_fail;
+	}
+
+	b2r2_blt_misc_dev.this_device->coherent_dma_mask = 0xFFFFFFFF;
+	b2r2_blt_dev = &b2r2_blt_misc_dev;
+	b2r2_log_info("%s\n", __func__);
+
+	/*
+	 * FIXME: This stuff should be done before the first requests i.e.
+	 * before misc_register, but they need the device which is not
+	 * available until after misc_register.
+	 */
+	init_tmp_bufs();
+
+	/* Initialize memory allocator */
+	ret = b2r2_mem_init(b2r2_blt_device(), B2R2_HEAP_SIZE,
+			4, sizeof(struct b2r2_node));
+	if (ret) {
+		printk(KERN_WARNING "%s: initializing B2R2 memhandler fails\n",
+			__func__);
+		goto b2r2_mem_init_fail;
+	}
+
+#ifdef CONFIG_DEBUG_FS
+	/* Register debug fs */
+	if (!debugfs_root_dir) {
+		debugfs_root_dir = debugfs_create_dir("b2r2_blt", NULL);
+		debugfs_create_file("latest_request",
+				0666, debugfs_root_dir,
+				0,
+				&debugfs_b2r2_blt_request_fops);
+		debugfs_create_file("stat",
+				0666, debugfs_root_dir,
+				0,
+				&debugfs_b2r2_blt_stat_fops);
+	}
+#endif
+	goto out;
+
+b2r2_misc_register_fail:
+b2r2_mem_init_fail:
+	b2r2_node_split_exit();
+
+b2r2_node_split_init_fail:
+#ifdef CONFIG_B2R2_GENERIC
+	b2r2_generic_exit();
+#endif
+out:
+	return ret;
+}
+
+/**
+ * b2r2_module_exit() - Module exit function
+ */
+void b2r2_blt_module_exit(void)
+{
+#ifdef CONFIG_DEBUG_FS
+	if (debugfs_root_dir) {
+		debugfs_remove_recursive(debugfs_root_dir);
+		debugfs_root_dir = NULL;
+	}
+#endif
+	if (b2r2_blt_dev) {
+		b2r2_log_info("%s\n", __func__);
+		b2r2_mem_exit();
+		destroy_tmp_bufs();
+		b2r2_blt_dev = NULL;
+		misc_deregister(&b2r2_blt_misc_dev);
+	}
+
+	b2r2_node_split_exit();
+
+#if defined(CONFIG_B2R2_GENERIC)
+	b2r2_generic_exit();
+#endif
+}
+
+MODULE_AUTHOR("Robert Fekete <robert.fekete@stericsson.com>");
+MODULE_DESCRIPTION("ST-Ericsson B2R2 Blitter module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/b2r2/b2r2_core.c b/drivers/video/b2r2/b2r2_core.c
new file mode 100644
index 00000000000..7a11a301d11
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_core.c
@@ -0,0 +1,2666 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 core driver
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/*
+ * TODO: Clock address from platform data
+ * Platform data should have string id instead of numbers
+ * b2r2_remove, some type of runtime problem when kernel hacking
+ * debug features on
+ *
+ * Is there already a priority list in kernel?
+ * Is it possible to handle clock using clock framework?
+ * uTimeOut, use mdelay instead?
+ * Measure performance
+ *
+ * Exchange our home-cooked ref count with kernel kref? See
+ *    http://lwn.net/Articles/336224/
+ *
+ * B2R2:
+ * Source fill 2 bug
+ * Check with Symbian?
+ */
+
+/* include file */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/uaccess.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#endif
+#include <linux/jiffies.h>
+#include <linux/timer.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+
+#include "b2r2_core.h"
+#include "b2r2_global.h"
+#include "b2r2_structures.h"
+#include "b2r2_internal.h"
+#include "b2r2_profiler_api.h"
+#include "b2r2_timing.h"
+#include "b2r2_debug.h"
+
+/**
+ * B2R2_DRIVER_TIMEOUT_VALUE - Busy loop timeout after soft reset
+ */
+#define B2R2_DRIVER_TIMEOUT_VALUE (1500)
+
+/**
+ * B2R2_CLK_FLAG - Value to write into clock reg to turn clock on
+ */
+#define B2R2_CLK_FLAG (0x125)
+
+/**
+ * DEBUG_CHECK_ADDREF_RELEASE - Define this to enable addref / release debug
+ */
+#define DEBUG_CHECK_ADDREF_RELEASE 1
+
+#ifdef CONFIG_DEBUG_FS
+/**
+ * HANDLE_TIMEOUTED_JOBS - Define this to check jobs for timeout and cancel them
+ */
+#define HANDLE_TIMEOUTED_JOBS 1
+#endif
+
+/**
+ * B2R2_CLOCK_ALWAYS_ON - Define this to disable power save clock turn off
+ */
+/* #define B2R2_CLOCK_ALWAYS_ON 1 */
+
+/**
+ * START_SENTINEL - Watch guard to detect job overwrites
+ */
+#define START_SENTINEL 0xBABEDEEA
+
+/**
+ * STOP_SENTINEL - Watch guard to detect job overwrites
+ */
+#define END_SENTINEL 0xDADBDCDD
+
+/**
+ * B2R2_CORE_LOWEST_PRIO - Lowest prio allowed
+ */
+#define B2R2_CORE_LOWEST_PRIO -19
+/**
+ * B2R2_CORE_HIGHEST_PRIO - Highest prio allowed
+ */
+#define B2R2_CORE_HIGHEST_PRIO 20
+
+
+/**
+ * B2R2 Hardware defines below
+ */
+
+/* - BLT_AQ_CTL */
+#define	B2R2_AQ_Enab			(0x80000000)
+#define B2R2_AQ_PRIOR_0 		(0x0)
+#define	B2R2_AQ_PRIOR_1 		(0x1)
+#define	B2R2_AQ_PRIOR_2			(0x2)
+#define	B2R2_AQ_PRIOR_3			(0x3)
+#define B2R2_AQ_NODE_REPEAT_INT	(0x100000)
+#define B2R2_AQ_STOP_INT		(0x200000)
+#define B2R2_AQ_LNA_REACH_INT	(0x400000)
+#define B2R2_AQ_COMPLETED_INT	(0x800000)
+
+/* - BLT_CTL */
+#define	B2R2BLT_CTLGLOBAL_soft_reset	(0x80000000)
+#define	B2R2BLT_CTLStep_By_Step		(0x20000000)
+#define	B2R2BLT_CTLBig_not_little	(0x10000000)
+#define	B2R2BLT_CTLMask			(0xb0000000)
+#define	B2R2BLT_CTLTestMask		(0xb0000000)
+#define	B2R2BLT_CTLInitialValue		(0x0)
+#define	B2R2BLT_CTLAccessType		(INITIAL_TEST)
+#define	B2R2BLT_CTL			(0xa00)
+
+/* - BLT_ITS */
+#define	B2R2BLT_ITSRLD_ERROR		(0x80000000)
+#define	B2R2BLT_ITSAQ4_Node_Notif	(0x8000000)
+#define	B2R2BLT_ITSAQ4_Node_repeat	(0x4000000)
+#define	B2R2BLT_ITSAQ4_Stopped		(0x2000000)
+#define	B2R2BLT_ITSAQ4_LNA_Reached	(0x1000000)
+#define	B2R2BLT_ITSAQ3_Node_Notif	(0x800000)
+#define	B2R2BLT_ITSAQ3_Node_repeat	(0x400000)
+#define	B2R2BLT_ITSAQ3_Stopped		(0x200000)
+#define	B2R2BLT_ITSAQ3_LNA_Reached	(0x100000)
+#define	B2R2BLT_ITSAQ2_Node_Notif	(0x80000)
+#define	B2R2BLT_ITSAQ2_Node_repeat	(0x40000)
+#define	B2R2BLT_ITSAQ2_Stopped		(0x20000)
+#define	B2R2BLT_ITSAQ2_LNA_Reached	(0x10000)
+#define	B2R2BLT_ITSAQ1_Node_Notif	(0x8000)
+#define	B2R2BLT_ITSAQ1_Node_repeat	(0x4000)
+#define	B2R2BLT_ITSAQ1_Stopped		(0x2000)
+#define	B2R2BLT_ITSAQ1_LNA_Reached	(0x1000)
+#define	B2R2BLT_ITSCQ2_Repaced		(0x80)
+#define	B2R2BLT_ITSCQ2_Node_Notif	(0x40)
+#define	B2R2BLT_ITSCQ2_retriggered	(0x20)
+#define	B2R2BLT_ITSCQ2_completed	(0x10)
+#define	B2R2BLT_ITSCQ1_Repaced		(0x8)
+#define	B2R2BLT_ITSCQ1_Node_Notif	(0x4)
+#define	B2R2BLT_ITSCQ1_retriggered	(0x2)
+#define	B2R2BLT_ITSCQ1_completed	(0x1)
+#define	B2R2BLT_ITSMask			(0x8ffff0ff)
+#define	B2R2BLT_ITSTestMask		(0x8ffff0ff)
+#define	B2R2BLT_ITSInitialValue		(0x0)
+#define	B2R2BLT_ITSAccessType		(INITIAL_TEST)
+#define	B2R2BLT_ITS			(0xa04)
+
+/* - BLT_STA1 */
+#define	B2R2BLT_STA1BDISP_IDLE		(0x1)
+#define	B2R2BLT_STA1Mask		(0x1)
+#define	B2R2BLT_STA1TestMask		(0x1)
+#define	B2R2BLT_STA1InitialValue	(0x1)
+#define	B2R2BLT_STA1AccessType		(INITIAL_TEST)
+#define	B2R2BLT_STA1			(0xa08)
+
+
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+
+/**
+ * struct addref_release - Represents one addref or release. Used
+ *                         to debug addref / release problems
+ *
+ * @addref: true if this represents an addref else it represents
+ *          a release.
+ * @job: The job that was referenced
+ * @caller: The caller of the addref or release
+ * @ref_count: The job reference count after addref / release
+ */
+struct addref_release {
+	bool addref;
+	struct b2r2_core_job *job;
+	const char *caller;
+	int ref_count;
+};
+
+#endif
+
+/**
+ * struct b2r2_core - Administration data for B2R2 core
+ *
+ * @lock: Spin lock protecting the b2r2_core structure and the B2R2 HW
+ * @hw: B2R2 registers memory mapped
+ * @pmu_b2r2_clock: Control of B2R2 clock
+ * @log_dev: Device used for logging via dev_... functions
+ *
+ * @prio_queue: Queue of jobs sorted in priority order
+ * @active_jobs: Array containing pointer to zero or one job per queue
+ * @n_active_jobs: Number of active jobs
+ * @jiffies_last_active: jiffie value when adding last active job
+ * @jiffies_last_irq: jiffie value when last irq occured
+ * @timeout_work: Work structure for timeout work
+ *
+ * @next_job_id: Contains the job id that will be assigned to the next
+ *               added job.
+ *
+ * @clock_request_count: When non-zero, clock is on
+ * @clock_off_timer: Kernel timer to handle delayed turn off of clock
+ *
+ * @work_queue: Work queue to handle done jobs (callbacks) and timeouts in
+ *              non-interrupt context.
+ *
+ * @stat_n_irq: Number of interrupts (statistics)
+ * @stat_n_jobs_added: Number of jobs added (statistics)
+ * @stat_n_jobs_removed: Number of jobs removed (statistics)
+ * @stat_n_jobs_in_prio_list: Number of jobs in prio list (statistics)
+ *
+ * @debugfs_root_dir: Root directory for B2R2 debugfs
+ *
+ * @ar: Circular array of addref / release debug structs
+ * @ar_write: Where next write will occur
+ * @ar_read: First valid place to read. When ar_read == ar_write then
+ *           the array is empty.
+ */
+struct b2r2_core {
+	spinlock_t       lock;
+
+	struct b2r2_memory_map *hw;
+
+	u8 op_size;
+	u8 ch_size;
+	u8 pg_size;
+	u8 mg_size;
+	u16 min_req_time;
+	int irq;
+
+	struct device *log_dev;
+
+	struct list_head prio_queue;
+
+	struct b2r2_core_job *active_jobs[B2R2_CORE_QUEUE_NO_OF];
+	unsigned long    n_active_jobs;
+
+	unsigned long    jiffies_last_active;
+	unsigned long    jiffies_last_irq;
+#ifdef HANDLE_TIMEOUTED_JOBS
+	struct delayed_work     timeout_work;
+#endif
+	int              next_job_id;
+
+	unsigned long    clock_request_count;
+	struct timer_list clock_off_timer;
+
+	struct workqueue_struct *work_queue;
+
+	/* Statistics */
+	unsigned long    stat_n_irq;
+	unsigned long    stat_n_jobs_added;
+	unsigned long    stat_n_jobs_removed;
+
+	unsigned long    stat_n_jobs_in_prio_list;
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *debugfs_root_dir;
+	struct dentry *debugfs_regs_dir;
+#endif
+
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+	/* Tracking release bug...*/
+	struct addref_release ar[100];
+	int ar_write;
+	int ar_read;
+#endif
+
+	/* Power management variables */
+	struct mutex domain_lock;
+	struct delayed_work domain_disable_work;
+
+	/*
+	 * We need to keep track of both the number of domain_enable/disable()
+	 * calls and whether the power was actually turned off, since the
+	 * power off is done in a delayed job.
+	 */
+	bool domain_enabled;
+	int domain_request_count;
+
+	struct clk *b2r2_clock;
+	struct regulator *b2r2_reg;
+};
+
+/**
+ * b2r2_core - Administration data for B2R2 core (singleton)
+ */
+static struct b2r2_core   b2r2_core;
+
+/* Local functions */
+static void check_prio_list(bool atomic);
+static void  clear_interrupts(void);
+static void trigger_job(struct b2r2_core_job *job);
+static void exit_job_list(struct list_head *job_list);
+static int get_next_job_id(void);
+static void job_work_function(struct work_struct *ptr);
+static void init_job(struct b2r2_core_job *job);
+static void insert_into_prio_list(struct b2r2_core_job *job);
+static struct b2r2_core_job *find_job_in_list(
+	int job_id,
+	struct list_head *list);
+static struct b2r2_core_job *find_job_in_active_jobs(int job_id);
+static struct b2r2_core_job *find_tag_in_list(
+	int tag,
+	struct list_head *list);
+static struct b2r2_core_job *find_tag_in_active_jobs(int tag);
+
+static int domain_enable(void);
+static void domain_disable(void);
+
+static void stop_queue(enum b2r2_core_queue queue);
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+static void printk_regs(void);
+static int hw_reset(void);
+static void timeout_work_function(struct work_struct *ptr);
+#endif
+
+static void reset_hw_timer(struct b2r2_core_job *job);
+static void start_hw_timer(struct b2r2_core_job *job);
+static void stop_hw_timer(struct b2r2_core_job *job);
+
+static int init_hw(void);
+static void exit_hw(void);
+
+/* Tracking release bug... */
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+/**
+ * ar_add() - Adds an addref or a release to the array
+ *
+ * @job: The job that has been referenced
+ * @caller: The caller of addref / release
+ * @addref: true if it is an addref else false for release
+ */
+void ar_add(struct b2r2_core_job *job, const char *caller, bool addref)
+{
+	b2r2_core.ar[b2r2_core.ar_write].addref = addref;
+	b2r2_core.ar[b2r2_core.ar_write].job = job;
+	b2r2_core.ar[b2r2_core.ar_write].caller = caller;
+	b2r2_core.ar[b2r2_core.ar_write].ref_count = job->ref_count;
+	b2r2_core.ar_write = (b2r2_core.ar_write + 1) %
+		ARRAY_SIZE(b2r2_core.ar);
+	if (b2r2_core.ar_write == b2r2_core.ar_read)
+		b2r2_core.ar_read = (b2r2_core.ar_read + 1) %
+			ARRAY_SIZE(b2r2_core.ar);
+}
+
+/**
+ * sprintf_ar() - Writes all addref / release to a string buffer
+ *
+ * @buf: Receiving character bufefr
+ * @job: Which job to write or NULL for all
+ *
+ * NOTE! No buffer size check!!
+ */
+char *sprintf_ar(char *buf, struct b2r2_core_job *job)
+{
+	int i;
+	int size = 0;
+
+	for (i = b2r2_core.ar_read;
+	     i != b2r2_core.ar_write;
+	     i = (i + 1) % ARRAY_SIZE(b2r2_core.ar)) {
+		struct addref_release *ar = &b2r2_core.ar[i];
+		if (!job || job == ar->job)
+			size += sprintf(buf + size,
+					"%s on %p from %s, ref = %d\n",
+					ar->addref ? "addref" : "release",
+					ar->job, ar->caller, ar->ref_count);
+	}
+
+	return buf;
+}
+
+/**
+ * printk_ar() - Writes all addref / release using dev_info
+ *
+ * @job: Which job to write or NULL for all
+ */
+void printk_ar(struct b2r2_core_job *job)
+{
+	int i;
+
+	for (i = b2r2_core.ar_read;
+	     i != b2r2_core.ar_write;
+	     i = (i + 1) % ARRAY_SIZE(b2r2_core.ar)) {
+		struct addref_release *ar = &b2r2_core.ar[i];
+		if (!job || job == ar->job)
+			b2r2_log_info("%s on %p from %s,"
+				 " ref = %d\n",
+				 ar->addref ? "addref" : "release",
+				 ar->job, ar->caller, ar->ref_count);
+	}
+}
+#endif
+
+/**
+ * internal_job_addref() - Increments the reference count for a job
+ *
+ * @job: Which job to increment reference count for
+ * @caller: Name of function calling addref (for debug)
+ *
+ * Note that b2r2_core.lock _must_ be held
+ */
+static void internal_job_addref(struct b2r2_core_job *job, const char *caller)
+{
+	u32 ref_count;
+
+	b2r2_log_info("%s (%p) (from %s)\n",
+		__func__, job, caller);
+
+	/* Sanity checks */
+	BUG_ON(job == NULL);
+
+	if (job->start_sentinel != START_SENTINEL ||
+	    job->end_sentinel != END_SENTINEL ||
+	    job->ref_count == 0 || job->ref_count > 10)	{
+		b2r2_log_info(
+			 "%s: (%p) start=%X end=%X ref_count=%d\n",
+			 __func__, job, job->start_sentinel,
+			 job->end_sentinel, job->ref_count);
+
+	/* Something is wrong, print the addref / release array */
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+		printk_ar(NULL);
+#endif
+	}
+
+
+	BUG_ON(job->start_sentinel != START_SENTINEL);
+	BUG_ON(job->end_sentinel != END_SENTINEL);
+
+	/* Do the actual reference count increment */
+	ref_count = ++job->ref_count;
+
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+	/* Keep track of addref / release */
+	ar_add(job, caller, true);
+#endif
+
+	b2r2_log_info("%s called from %s (%p): Ref Count is %d\n",
+		__func__, caller, job, job->ref_count);
+}
+
+/**
+ * internal_job_release() - Decrements the reference count for a job
+ *
+ * @job: Which job to decrement reference count for
+ * @caller: Name of function calling release (for debug)
+ *
+ * Returns true if job_release should be called by caller
+ * (reference count reached zero).
+ *
+ * Note that b2r2_core.lock _must_ be held
+ */
+bool internal_job_release(struct b2r2_core_job *job, const char *caller)
+{
+	u32 ref_count;
+	bool call_release = false;
+
+	b2r2_log_info("%s (%p) (from %s)\n",
+		__func__, job, caller);
+
+	/* Sanity checks */
+	BUG_ON(job == NULL);
+
+	if (job->start_sentinel != START_SENTINEL ||
+	    job->end_sentinel != END_SENTINEL ||
+	    job->ref_count == 0 || job->ref_count > 10) {
+		b2r2_log_info(
+			 "%s: (%p) start=%X end=%X ref_count=%d\n",
+			 __func__, job, job->start_sentinel,
+			 job->end_sentinel, job->ref_count);
+
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+		printk_ar(NULL);
+#endif
+	}
+
+
+	BUG_ON(job->start_sentinel != START_SENTINEL);
+	BUG_ON(job->end_sentinel != END_SENTINEL);
+
+	BUG_ON(job->ref_count == 0 || job->ref_count > 10);
+
+	/* Do the actual decrement */
+	ref_count = --job->ref_count;
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+	ar_add(job, caller, false);
+#endif
+	b2r2_log_info("%s called from %s (%p) Ref Count is %d\n",
+		__func__, caller, job, ref_count);
+
+	if (!ref_count && job->release) {
+		call_release = true;
+		/* Job will now cease to exist */
+		job->start_sentinel = 0xFFFFFFFF;
+		job->end_sentinel = 0xFFFFFFFF;
+	}
+	return call_release;
+}
+
+
+
+/* Exported functions */
+
+/* b2r2_core.lock _must_ _NOT_ be held when calling this function */
+void b2r2_core_job_addref(struct b2r2_core_job *job, const char *caller)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+	internal_job_addref(job, caller);
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+}
+
+/* b2r2_core.lock _must_ _NOT_ be held when calling this function */
+void b2r2_core_job_release(struct b2r2_core_job *job, const char *caller)
+{
+	unsigned long flags;
+	bool call_release = false;
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+	call_release = internal_job_release(job, caller);
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+
+	if (call_release)
+		job->release(job);
+}
+
+/* b2r2_core.lock _must_ _NOT_ be held when calling this function */
+int b2r2_core_job_add(struct b2r2_core_job *job)
+{
+	unsigned long flags;
+
+	b2r2_log_info("%s (%p)\n", __func__, job);
+
+	/* Enable B2R2 */
+	domain_enable();
+
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+	b2r2_core.stat_n_jobs_added++;
+
+	/* Initialise internal job data */
+	init_job(job);
+
+	/* Initial reference, should be released by caller of this function */
+	job->ref_count = 1;
+
+	/* Insert job into prio list */
+	insert_into_prio_list(job);
+
+	/* Check if we can dispatch job */
+	check_prio_list(false);
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+
+	return 0;
+}
+
+/* b2r2_core.lock _must_ _NOT_ be held when calling this function */
+struct b2r2_core_job *b2r2_core_job_find(int job_id)
+{
+	unsigned long flags;
+	struct b2r2_core_job *job;
+
+	b2r2_log_info("%s (%d)\n", __func__, job_id);
+
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+	/* Look through prio queue */
+	job = find_job_in_list(job_id, &b2r2_core.prio_queue);
+
+	if (!job)
+		job = find_job_in_active_jobs(job_id);
+
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+
+	return job;
+}
+
+/* b2r2_core.lock _must_ _NOT_ be held when calling this function */
+struct b2r2_core_job *b2r2_core_job_find_first_with_tag(int tag)
+{
+	unsigned long flags;
+	struct b2r2_core_job *job;
+
+	b2r2_log_info("%s (%d)\n", __func__, tag);
+
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+	/* Look through prio queue */
+	job = find_tag_in_list(tag, &b2r2_core.prio_queue);
+
+	if (!job)
+		job = find_tag_in_active_jobs(tag);
+
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+
+	return job;
+}
+
+/**
+ * is_job_done() - Spin lock protected check if job is done
+ *
+ * @job: Job to check
+ *
+ * Returns true if job is done or cancelled
+ *
+ * b2r2_core.lock must _NOT_ be held when calling this function
+ */
+static bool is_job_done(struct b2r2_core_job *job)
+{
+	unsigned long flags;
+	bool job_is_done;
+
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+	job_is_done =
+		job->job_state != B2R2_CORE_JOB_QUEUED &&
+		job->job_state != B2R2_CORE_JOB_RUNNING;
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+
+	return job_is_done;
+}
+
+/* b2r2_core.lock _must_ _NOT_ be held when calling this function */
+int b2r2_core_job_wait(struct b2r2_core_job *job)
+{
+	int ret = 0;
+
+	b2r2_log_info("%s (%p)\n", __func__, job);
+	/* Check that we have the job */
+	if (job->job_state == B2R2_CORE_JOB_IDLE) {
+		/* Never or not queued */
+		b2r2_log_info("%s: Job not queued\n", __func__);
+		return -ENOENT;
+	}
+
+	/* Wait for the job to be done */
+	ret = wait_event_interruptible(
+		job->event,
+		is_job_done(job));
+
+	if (ret)
+		b2r2_log_warn(
+			 "%s: wait_event_interruptible returns %d, state is %d",
+			 __func__, ret, job->job_state);
+	return ret;
+}
+
+/**
+ * cancel_job() - Cancels a job (removes it from prio list or active jobs) and
+ *                calls the job callback
+ *
+ * @job: Job to cancel
+ *
+ * Returns true if the job was found and cancelled
+ *
+ * b2r2_core.lock must be held when calling this function
+ */
+static bool cancel_job(struct b2r2_core_job *job)
+{
+	bool found_job = false;
+	bool job_was_active = false;
+
+	/* Remove from prio list */
+	if (job->job_state == B2R2_CORE_JOB_QUEUED) {
+		list_del_init(&job->list);
+		found_job = true;
+	}
+
+	/* Remove from active jobs */
+	if (!found_job) {
+		if (b2r2_core.n_active_jobs > 0) {
+			int i;
+
+			/* Look for timeout:ed jobs and put them in tmp list */
+			for (i = 0;
+			     i < ARRAY_SIZE(b2r2_core.active_jobs);
+			     i++) {
+				if (b2r2_core.active_jobs[i] == job) {
+					stop_queue((enum b2r2_core_queue)i);
+					stop_hw_timer(job);
+					b2r2_core.active_jobs[i] = NULL;
+					b2r2_core.n_active_jobs--;
+					found_job = true;
+					job_was_active = true;
+				}
+			}
+		}
+	}
+
+
+	/* Handle done list & callback */
+	if (found_job) {
+		/* Job is canceled */
+		job->job_state = B2R2_CORE_JOB_CANCELED;
+
+		queue_work(b2r2_core.work_queue, &job->work);
+
+		/* Statistics */
+		if (!job_was_active)
+			b2r2_core.stat_n_jobs_in_prio_list--;
+
+	}
+
+	return found_job;
+}
+
+/* b2r2_core.lock _must_ _NOT_ be held when calling this function */
+int b2r2_core_job_cancel(struct b2r2_core_job *job)
+{
+	unsigned long flags;
+	int ret = 0;
+
+	b2r2_log_info("%s (%p) (%d)\n", __func__,
+		job, job->job_state);
+	/* Check that we have the job */
+	if (job->job_state == B2R2_CORE_JOB_IDLE) {
+		/* Never or not queued */
+		b2r2_log_info("%s: Job not queued\n", __func__);
+		return -ENOENT;
+	}
+
+	/* Remove from prio list */
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+	cancel_job(job);
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+
+	return ret;
+}
+
+/* LOCAL FUNCTIONS BELOW */
+
+#define B2R2_DOMAIN_DISABLE_TIMEOUT (HZ/100)
+
+static void domain_disable_work_function(struct work_struct *work)
+{
+	if (!mutex_trylock(&b2r2_core.domain_lock))
+		return;
+
+	if (b2r2_core.domain_request_count == 0) {
+		exit_hw();
+		clk_disable(b2r2_core.b2r2_clock);
+		regulator_disable(b2r2_core.b2r2_reg);
+		b2r2_core.domain_enabled = false;
+	}
+
+	mutex_unlock(&b2r2_core.domain_lock);
+}
+
+#define B2R2_REGULATOR_RETRY_COUNT 10
+
+static int domain_enable(void)
+{
+	mutex_lock(&b2r2_core.domain_lock);
+	b2r2_core.domain_request_count++;
+
+	if (!b2r2_core.domain_enabled) {
+		int retry = 0;
+		int ret;
+
+again:
+		/*
+		 * Since regulator_enable() may sleep we have to handle
+		 * interrupts.
+		 */
+		ret = regulator_enable(b2r2_core.b2r2_reg);
+		if ((ret == -EAGAIN) &&
+				((retry++) < B2R2_REGULATOR_RETRY_COUNT))
+			goto again;
+		else if (ret < 0)
+			goto regulator_enable_failed;
+
+		clk_enable(b2r2_core.b2r2_clock);
+		if (init_hw() < 0)
+			goto init_hw_failed;
+		b2r2_core.domain_enabled = true;
+	}
+
+	mutex_unlock(&b2r2_core.domain_lock);
+
+	return 0;
+
+init_hw_failed:
+	b2r2_log_err("%s: Could not initialize hardware!\n", __func__);
+
+	clk_disable(b2r2_core.b2r2_clock);
+
+	if (regulator_disable(b2r2_core.b2r2_reg) < 0)
+		b2r2_log_err("%s: regulator_disable failed!\n", __func__);
+
+regulator_enable_failed:
+	b2r2_core.domain_request_count--;
+	mutex_unlock(&b2r2_core.domain_lock);
+
+	return -EFAULT;
+}
+
+static void domain_disable(void)
+{
+	mutex_lock(&b2r2_core.domain_lock);
+
+	if (b2r2_core.domain_request_count == 0) {
+		b2r2_log_err("%s: Unbalanced domain_disable()\n", __func__);
+	} else {
+		b2r2_core.domain_request_count--;
+
+		/* Cancel any existing work */
+		cancel_delayed_work_sync(&b2r2_core.domain_disable_work);
+
+		/* Add a work to disable the power and clock after a delay */
+		queue_delayed_work(b2r2_core.work_queue,
+				&b2r2_core.domain_disable_work,
+				B2R2_DOMAIN_DISABLE_TIMEOUT);
+	}
+
+	mutex_unlock(&b2r2_core.domain_lock);
+}
+
+/**
+ * stop_queue() - Stops the specified queue.
+ */
+static void stop_queue(enum b2r2_core_queue queue)
+{
+	/* TODO: Implement! If this function is not implemented canceled jobs will
+	use b2r2 which is a waste of resources. Not stopping jobs will also screw up
+	the hardware timing, the job the canceled job intrerrupted (if any) will be
+	billed for the time between the point where the job is cancelled and when it
+	stops. */
+}
+
+/**
+ * exit_job_list() - Empties a job queue by canceling the jobs
+ *
+ * b2r2_core.lock _must_ be held when calling this function
+ */
+static void exit_job_list(struct list_head *job_queue)
+{
+	while (!list_empty(job_queue)) {
+		struct b2r2_core_job *job =
+			list_entry(job_queue->next,
+				   struct b2r2_core_job,
+				   list);
+		/* Add reference to prevent job from disappearing
+		   in the middle of our work, released below */
+		internal_job_addref(job, __func__);
+
+		cancel_job(job);
+
+		/* Matching release to addref above */
+		internal_job_release(job, __func__);
+
+	}
+}
+
+/**
+ * get_next_job_id() - Return a new job id.
+ */
+static int get_next_job_id(void)
+{
+	int job_id;
+
+	if (b2r2_core.next_job_id < 1)
+		b2r2_core.next_job_id = 1;
+	job_id = b2r2_core.next_job_id++;
+
+	return job_id;
+}
+
+/**
+ * job_work_function() - Work queue function that calls callback(s) and
+ *                       checks if B2R2 can accept a new job
+ *
+ * @ptr: Pointer to work struct (embedded in struct b2r2_core_job)
+ */
+static void job_work_function(struct work_struct *ptr)
+{
+	unsigned long flags;
+	struct b2r2_core_job    *job = container_of(
+		ptr, struct b2r2_core_job, work);
+
+	/* Disable B2R2 */
+	domain_disable();
+
+	/* Release resources */
+	if (job->release_resources)
+		job->release_resources(job, false);
+
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+
+	/* Dispatch a new job if possible */
+	check_prio_list(false);
+
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+
+	/* Tell the client */
+	if (job->callback)
+		job->callback(job);
+
+	/* Drop our reference, matches the
+	   addref in handle_queue_event or b2r2_core_job_cancel */
+	b2r2_core_job_release(job, __func__);
+}
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+/**
+ * timeout_work_function() - Work queue function that checks for
+ *                           timeout:ed jobs. B2R2 might silently refuse
+ *                           to execute some jobs, i.e. SRC2 fill
+ *
+ * @ptr: Pointer to work struct (embedded in struct b2r2_core)
+ *
+ */
+static void timeout_work_function(struct work_struct *ptr)
+{
+	unsigned long flags;
+	struct list_head job_list;
+
+	INIT_LIST_HEAD(&job_list);
+
+	/* Cancel all jobs if too long time since last irq */
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+	if (b2r2_core.n_active_jobs > 0) {
+		unsigned long diff =
+			(long) jiffies - (long) b2r2_core.jiffies_last_irq;
+		if (diff > HZ/2) {
+			/* Active jobs and more than a second since last irq! */
+			int i;
+
+			/* Look for timeout:ed jobs and put them in tmp list. It's
+			important that the application queues are killed in order
+			of decreasing priority */
+			for (i = 0;
+			     i < ARRAY_SIZE(b2r2_core.active_jobs);
+			     i++) {
+				struct b2r2_core_job *job =
+					b2r2_core.active_jobs[i];
+
+				if (job) {
+					stop_hw_timer(job);
+
+					b2r2_core.active_jobs[i] = NULL;
+					b2r2_core.n_active_jobs--;
+					list_add_tail(&job->list,
+						      &job_list);
+				}
+			}
+
+			/* Print the B2R2 register and reset B2R2 */
+			printk_regs();
+			hw_reset();
+		}
+	}
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+
+	/* Handle timeout:ed jobs */
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+	while (!list_empty(&job_list)) {
+		struct b2r2_core_job *job =
+			list_entry(job_list.next,
+				   struct b2r2_core_job,
+				   list);
+
+		b2r2_log_warn("%s: Job timeout\n", __func__);
+
+		list_del_init(&job->list);
+
+		/* Job is cancelled */
+		job->job_state = B2R2_CORE_JOB_CANCELED;
+
+		/* Handle done */
+		wake_up_interruptible(&job->event);
+
+		/* Job callbacks handled via work queue */
+		queue_work(b2r2_core.work_queue, &job->work);
+	}
+
+	/* Requeue delayed work */
+	if (b2r2_core.n_active_jobs)
+		queue_delayed_work(
+			b2r2_core.work_queue,
+			&b2r2_core.timeout_work, HZ/2);
+
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+}
+#endif
+
+/**
+ * reset_hw_timer() - Resets a job's hardware timer. Must be called before
+ *                    the timer is used.
+ *
+ * @job: Pointer to job struct
+ *
+ * b2r2_core.lock _must_ be held when calling this function
+ */
+static void reset_hw_timer(struct b2r2_core_job *job)
+{
+	job->nsec_active_in_hw = 0;
+}
+
+/**
+ * start_hw_timer() - Times how long a job spends in hardware (active).
+ *                    Should be called immediatly before starting the
+ *                    hardware.
+ *
+ * @job: Pointer to job struct
+ *
+ * b2r2_core.lock _must_ be held when calling this function
+ */
+static void start_hw_timer(struct b2r2_core_job *job)
+{
+	job->hw_start_time = b2r2_get_curr_nsec();
+}
+
+/**
+ * stop_hw_timer() - Times how long a job spends in hardware (active).
+ *                   Should be called immediatly after the hardware has
+ *                   finished.
+ *
+ * @job: Pointer to job struct
+ *
+ * b2r2_core.lock _must_ be held when calling this function
+ */
+static void stop_hw_timer(struct b2r2_core_job *job)
+{
+	/* Assumes only app queues are used, which is the case right now. */
+	/* Not 100% accurate. When a higher prio job interrupts a lower prio job it does
+	so after the current node of the low prio job has finished. Currently we can not
+	sense when the actual switch takes place so the time reported for a job that
+	interrupts a lower prio job will on average contain the time it takes to process
+	half a node in the lower prio job in addition to the time it takes to process the
+	job's own nodes. This could possibly be solved by adding node notifications but
+	that would involve a significant amount of work and consume system resources due
+	to the extra interrupts. */
+	/* If a job takes more than ~2s (absolute time, including idleing in the hardware)
+	the state of the hardware timer will be corrupted and it will not report valid
+	values until b2r2 becomes idle (no active jobs on any queues). The maximum length
+	can possibly be increased by using 64 bit integers. */
+
+	int i;
+
+	u32 stop_time_raw = b2r2_get_curr_nsec();
+	/* We'll add an offset to all positions in time to make the current time equal to
+	0xFFFFFFFF. This way we can compare positions in time to each other without having
+	to wory about wrapping (so long as all positions in time are in the past). */
+	u32 stop_time = 0xFFFFFFFF;
+	u32 time_pos_offset = 0xFFFFFFFF - stop_time_raw;
+	u32 nsec_in_hw = stop_time - (job->hw_start_time + time_pos_offset);
+	job->nsec_active_in_hw += (s32)nsec_in_hw;
+
+	/* Check if we have delayed the start of higher prio jobs. Can happen as queue
+	switching only can be done between nodes. */
+	for (i = (int)job->queue - 1; i >= (int)B2R2_CORE_QUEUE_AQ1; i--) {
+		struct b2r2_core_job *queue_active_job = b2r2_core.active_jobs[i];
+		if (NULL == queue_active_job)
+			continue;
+
+		queue_active_job->hw_start_time = stop_time_raw;
+	}
+
+	/* Check if the job has stolen time from lower prio jobs */
+	for (i = (int)job->queue + 1; i < B2R2_NUM_APPLICATIONS_QUEUES; i++) {
+		struct b2r2_core_job *queue_active_job = b2r2_core.active_jobs[i];
+		u32 queue_active_job_hw_start_time;
+
+		if (NULL == queue_active_job)
+			continue;
+
+		queue_active_job_hw_start_time = queue_active_job->hw_start_time + time_pos_offset;
+
+		if (queue_active_job_hw_start_time < stop_time) {
+			u32 queue_active_job_nsec_in_hw = stop_time - queue_active_job_hw_start_time;
+			u32 num_stolen_nsec = min(queue_active_job_nsec_in_hw, nsec_in_hw);
+
+			queue_active_job->nsec_active_in_hw -= (s32)num_stolen_nsec;
+
+			nsec_in_hw -= num_stolen_nsec;
+			stop_time -= num_stolen_nsec;
+		}
+
+		if (0 == nsec_in_hw)
+			break;
+	}
+}
+
+/**
+ * init_job() - Initializes a job structure from filled in client data.
+ *              Reference count will be set to 1
+ *
+ * @job: Job to initialize
+ */
+static void init_job(struct b2r2_core_job *job)
+{
+	job->start_sentinel = START_SENTINEL;
+	job->end_sentinel = END_SENTINEL;
+
+	/* Get a job id*/
+	job->job_id = get_next_job_id();
+
+	/* Job is idle, never queued */
+	job->job_state = B2R2_CORE_JOB_IDLE;
+
+	/* Initialize internal data */
+	INIT_LIST_HEAD(&job->list);
+	init_waitqueue_head(&job->event);
+	INIT_WORK(&job->work, job_work_function);
+
+	/* Map given prio to B2R2 queues */
+	if (job->prio < B2R2_CORE_LOWEST_PRIO)
+		job->prio = B2R2_CORE_LOWEST_PRIO;
+	else if (job->prio > B2R2_CORE_HIGHEST_PRIO)
+		job->prio = B2R2_CORE_HIGHEST_PRIO;
+
+	if (job->prio > 10) {
+		job->queue = B2R2_CORE_QUEUE_AQ1;
+		job->interrupt_context =
+			(B2R2BLT_ITSAQ1_LNA_Reached);
+		job->control = (B2R2_AQ_Enab | B2R2_AQ_PRIOR_3);
+	} else if (job->prio > 0) {
+		job->queue = B2R2_CORE_QUEUE_AQ2;
+		job->interrupt_context =
+			(B2R2BLT_ITSAQ2_LNA_Reached);
+		job->control = (B2R2_AQ_Enab | B2R2_AQ_PRIOR_2);
+	} else if (job->prio > -10) {
+		job->queue = B2R2_CORE_QUEUE_AQ3;
+		job->interrupt_context =
+			(B2R2BLT_ITSAQ3_LNA_Reached);
+		job->control = (B2R2_AQ_Enab | B2R2_AQ_PRIOR_1);
+	} else {
+		job->queue = B2R2_CORE_QUEUE_AQ4;
+		job->interrupt_context =
+			(B2R2BLT_ITSAQ4_LNA_Reached);
+		job->control = (B2R2_AQ_Enab | B2R2_AQ_PRIOR_0);
+	}
+}
+
+/**
+ * clear_interrupts() - Disables all interrupts
+ *
+ * b2r2_core.lock must be held
+ */
+static void  clear_interrupts(void)
+{
+	writel(0x0, &b2r2_core.hw->BLT_ITM0);
+	writel(0x0, &b2r2_core.hw->BLT_ITM1);
+	writel(0x0, &b2r2_core.hw->BLT_ITM2);
+	writel(0x0, &b2r2_core.hw->BLT_ITM3);
+}
+
+/**
+ * insert_into_prio_list() - Inserts the job into the sorted list of jobs.
+ *                           The list is sorted by priority.
+ *
+ * @job: Job to insert
+ *
+ * b2r2_core.lock must be held
+ */
+static void insert_into_prio_list(struct b2r2_core_job *job)
+{
+	/* Ref count is increased when job put in list,
+	   should be released when job is removed from list */
+	internal_job_addref(job, __func__);
+
+	b2r2_core.stat_n_jobs_in_prio_list++;
+
+	/* Sort in the job */
+	if (list_empty(&b2r2_core.prio_queue))
+		list_add_tail(&job->list, &b2r2_core.prio_queue);
+	else {
+		struct b2r2_core_job *first_job =
+			list_entry(b2r2_core.prio_queue.next,
+				   struct b2r2_core_job, list);
+		struct b2r2_core_job *last_job =
+			list_entry(b2r2_core.prio_queue.prev,
+				   struct b2r2_core_job, list);
+
+		/* High prio job? */
+		if (job->prio > first_job->prio)
+			/* Insert first */
+			list_add(&job->list, &b2r2_core.prio_queue);
+		else if (job->prio <= last_job->prio)
+			/* Insert last */
+			list_add_tail(&job->list, &b2r2_core.prio_queue);
+		else {
+			/* We need to find where to put it */
+			struct list_head *ptr;
+
+			list_for_each(ptr, &b2r2_core.prio_queue) {
+				struct b2r2_core_job *list_job =
+					list_entry(ptr, struct b2r2_core_job,
+						   list);
+				if (job->prio > list_job->prio) {
+					/* Add before */
+					list_add_tail(&job->list,
+						      &list_job->list);
+					break;
+				}
+			}
+		}
+	}
+	/* The job is now queued */
+	job->job_state = B2R2_CORE_JOB_QUEUED;
+}
+
+/**
+ * check_prio_list() - Checks if the first job(s) in the prio list can
+ *                     be dispatched to B2R2
+ *
+ * @atomic: true if in atomic context (i.e. interrupt context)
+ *
+ * b2r2_core.lock must be held
+ */
+static void check_prio_list(bool atomic)
+{
+	bool dispatched_job;
+	int n_dispatched = 0;
+
+	/* Do we have anything in our prio list? */
+	do {
+		dispatched_job = false;
+		if (!list_empty(&b2r2_core.prio_queue)) {
+			/* The first job waiting */
+			struct b2r2_core_job *job =
+				list_first_entry(&b2r2_core.prio_queue,
+						 struct b2r2_core_job,
+						 list);
+
+			/* Is the B2R2 queue available? */
+			if (b2r2_core.active_jobs[job->queue] == NULL) {
+				/* Can we acquire resources? */
+				if (!job->acquire_resources ||
+				    job->acquire_resources(job, atomic) == 0) {
+					/* Ok to dispatch job */
+
+					/* Remove from list */
+					list_del_init(&job->list);
+
+					/* The job is now active */
+					b2r2_core.active_jobs[job->queue] = job;
+					b2r2_core.n_active_jobs++;
+					job->jiffies = jiffies;
+					b2r2_core.jiffies_last_active =
+						jiffies;
+
+					/* Kick off B2R2 */
+					trigger_job(job);
+
+					dispatched_job = true;
+					n_dispatched++;
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+					/* Check in one half second
+					   if it hangs */
+					queue_delayed_work(
+						b2r2_core.work_queue,
+						&b2r2_core.timeout_work,
+						HZ/2);
+#endif
+				} else {
+					/* No resources */
+					if (!atomic &&
+					    b2r2_core.n_active_jobs == 0) {
+						b2r2_log_warn(
+							"%s: No resource",
+							__func__);
+						cancel_job(job);
+					}
+				}
+			}
+		}
+	} while (dispatched_job);
+
+	b2r2_core.stat_n_jobs_in_prio_list -= n_dispatched;
+}
+
+/**
+ * find_job_in_list() - Finds job with job_id in list
+ *
+ * @jobid: Job id to find
+ * @list: List to find job id in
+ *
+ * Reference count will be incremented for found job.
+ *
+ * b2r2_core.lock must be held
+ */
+static struct b2r2_core_job *find_job_in_list(int job_id,
+					      struct list_head *list)
+{
+	struct list_head *ptr;
+
+	list_for_each(ptr, list) {
+		struct b2r2_core_job *job =
+			list_entry(ptr, struct b2r2_core_job, list);
+		if (job->job_id == job_id) {
+			/* Increase reference count, should be released by
+			   the caller of b2r2_core_job_find */
+			internal_job_addref(job, __func__);
+			return job;
+		}
+	}
+	return NULL;
+}
+
+/**
+ * find_job_in_active_jobs() - Finds job in active job queues
+ *
+ * @jobid: Job id to find
+ *
+ * Reference count will be incremented for found job.
+ *
+ * b2r2_core.lock must be held
+ */
+static struct b2r2_core_job *find_job_in_active_jobs(int job_id)
+{
+	int i;
+	struct b2r2_core_job *found_job = NULL;
+
+	if (b2r2_core.n_active_jobs) {
+		for (i = 0; i < ARRAY_SIZE(b2r2_core.active_jobs); i++) {
+			struct b2r2_core_job *job = b2r2_core.active_jobs[i];
+
+			if (job && job->job_id == job_id) {
+				internal_job_addref(job, __func__);
+				found_job = job;
+				break;
+			}
+		}
+	}
+	return found_job;
+}
+
+/**
+ * find_tag_in_list() - Finds first job with tag in list
+ *
+ * @tag: Tag to find
+ * @list: List to find job id in
+ *
+ * Reference count will be incremented for found job.
+ *
+ * b2r2_core.lock must be held
+ */
+static struct b2r2_core_job *find_tag_in_list(int tag, struct list_head *list)
+{
+	struct list_head *ptr;
+
+	list_for_each(ptr, list) {
+		struct b2r2_core_job *job =
+			list_entry(ptr, struct b2r2_core_job, list);
+		if (job->tag == tag) {
+			/* Increase reference count, should be released by
+			   the caller of b2r2_core_job_find */
+			internal_job_addref(job, __func__);
+			return job;
+		}
+	}
+	return NULL;
+}
+
+/**
+ * find_tag_in_active_jobs() - Finds job with tag in active job queues
+ *
+ * @tag: Tag to find
+ *
+ * Reference count will be incremented for found job.
+ *
+ * b2r2_core.lock must be held
+ */
+static struct b2r2_core_job *find_tag_in_active_jobs(int tag)
+{
+	int i;
+	struct b2r2_core_job *found_job = NULL;
+
+	if (b2r2_core.n_active_jobs) {
+		for (i = 0; i < ARRAY_SIZE(b2r2_core.active_jobs); i++) {
+			struct b2r2_core_job *job = b2r2_core.active_jobs[i];
+
+			if (job && job->tag == tag) {
+				internal_job_addref(job, __func__);
+				found_job = job;
+				break;
+			}
+		}
+	}
+	return found_job;
+}
+
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+/**
+ * hw_reset() - Resets B2R2 hardware
+ *
+ * b2r2_core.lock must be held
+ */
+static int hw_reset(void)
+{
+	u32 uTimeOut = B2R2_DRIVER_TIMEOUT_VALUE;
+
+	/* Tell B2R2 to reset */
+	writel(readl(&b2r2_core.hw->BLT_CTL) | B2R2BLT_CTLGLOBAL_soft_reset,
+		&b2r2_core.hw->BLT_CTL);
+	writel(0x00000000, &b2r2_core.hw->BLT_CTL);
+
+	b2r2_log_info("wait for B2R2 to be idle..\n");
+
+	/** Wait for B2R2 to be idle (on a timeout rather than while loop) */
+	while ((uTimeOut > 0) &&
+	       ((readl(&b2r2_core.hw->BLT_STA1) &
+		 B2R2BLT_STA1BDISP_IDLE) == 0x0))
+		uTimeOut--;
+
+	if (uTimeOut == 0) {
+		b2r2_log_warn(
+			 "error-> after software reset B2R2 is not idle\n");
+		return -EAGAIN;
+	}
+
+	return 0;
+
+}
+#endif
+
+/**
+ * trigger_job() - Put job in B2R2 HW queue
+ *
+ * @job: Job to trigger
+ *
+ * b2r2_core.lock must be held
+ */
+static void trigger_job(struct b2r2_core_job *job)
+{
+	/* Debug prints */
+	b2r2_log_info("queue 0x%x \n", job->queue);
+	b2r2_log_info("BLT TRIG_IP 0x%x (first node)\n",
+		job->first_node_address);
+	b2r2_log_info("BLT LNA_CTL 0x%x (last node)\n",
+		job->last_node_address);
+	b2r2_log_info("BLT TRIG_CTL 0x%x \n", job->control);
+	b2r2_log_info("BLT PACE_CTL 0x%x \n", job->pace_control);
+
+	reset_hw_timer(job);
+	job->job_state = B2R2_CORE_JOB_RUNNING;
+
+	/* Enable interrupt */
+	writel(readl(&b2r2_core.hw->BLT_ITM0) | job->interrupt_context,
+		&b2r2_core.hw->BLT_ITM0);
+
+	writel(min_t(u8, max_t(u8, b2r2_core.op_size, B2R2_PLUG_OPCODE_SIZE_8),
+				B2R2_PLUG_OPCODE_SIZE_64),
+			&b2r2_core.hw->PLUGS1_OP2);
+	writel(min_t(u8, b2r2_core.ch_size, B2R2_PLUG_CHUNK_SIZE_128),
+			&b2r2_core.hw->PLUGS1_CHZ);
+	writel(min_t(u8, b2r2_core.mg_size, B2R2_PLUG_MESSAGE_SIZE_128) |
+				(b2r2_core.min_req_time << 16),
+			&b2r2_core.hw->PLUGS1_MSZ);
+	writel(min_t(u8, b2r2_core.pg_size, B2R2_PLUG_PAGE_SIZE_256),
+			&b2r2_core.hw->PLUGS1_PGZ);
+
+	writel(min_t(u8, max_t(u8, b2r2_core.op_size, B2R2_PLUG_OPCODE_SIZE_8),
+				B2R2_PLUG_OPCODE_SIZE_64),
+			&b2r2_core.hw->PLUGS2_OP2);
+	writel(min_t(u8, b2r2_core.ch_size, B2R2_PLUG_CHUNK_SIZE_128),
+			&b2r2_core.hw->PLUGS2_CHZ);
+	writel(min_t(u8, b2r2_core.mg_size, B2R2_PLUG_MESSAGE_SIZE_128) |
+				(b2r2_core.min_req_time << 16),
+			&b2r2_core.hw->PLUGS2_MSZ);
+	writel(min_t(u8, b2r2_core.pg_size, B2R2_PLUG_PAGE_SIZE_256),
+			&b2r2_core.hw->PLUGS2_PGZ);
+
+	writel(min_t(u8, max_t(u8, b2r2_core.op_size, B2R2_PLUG_OPCODE_SIZE_8),
+				B2R2_PLUG_OPCODE_SIZE_64),
+			&b2r2_core.hw->PLUGS3_OP2);
+	writel(min_t(u8, b2r2_core.ch_size, B2R2_PLUG_CHUNK_SIZE_128),
+			&b2r2_core.hw->PLUGS3_CHZ);
+	writel(min_t(u8, b2r2_core.mg_size, B2R2_PLUG_MESSAGE_SIZE_128) |
+				(b2r2_core.min_req_time << 16),
+			&b2r2_core.hw->PLUGS3_MSZ);
+	writel(min_t(u8, b2r2_core.pg_size, B2R2_PLUG_PAGE_SIZE_256),
+			&b2r2_core.hw->PLUGS3_PGZ);
+
+	writel(min_t(u8, max_t(u8, b2r2_core.op_size, B2R2_PLUG_OPCODE_SIZE_8),
+				B2R2_PLUG_OPCODE_SIZE_64),
+			&b2r2_core.hw->PLUGT_OP2);
+	writel(min_t(u8, b2r2_core.ch_size, B2R2_PLUG_CHUNK_SIZE_128),
+			&b2r2_core.hw->PLUGT_CHZ);
+	writel(min_t(u8, b2r2_core.mg_size, B2R2_PLUG_MESSAGE_SIZE_128) |
+				(b2r2_core.min_req_time << 16),
+			&b2r2_core.hw->PLUGT_MSZ);
+	writel(min_t(u8, b2r2_core.pg_size, B2R2_PLUG_PAGE_SIZE_256),
+			&b2r2_core.hw->PLUGT_PGZ);
+
+	/* B2R2 kicks off when LNA is written, LNA write must be last! */
+	switch (job->queue) {
+	case B2R2_CORE_QUEUE_CQ1:
+		writel(job->first_node_address, &b2r2_core.hw->BLT_CQ1_TRIG_IP);
+		writel(job->control, &b2r2_core.hw->BLT_CQ1_TRIG_CTL);
+		writel(job->pace_control, &b2r2_core.hw->BLT_CQ1_PACE_CTL);
+		break;
+
+	case B2R2_CORE_QUEUE_CQ2:
+		writel(job->first_node_address, &b2r2_core.hw->BLT_CQ2_TRIG_IP);
+		writel(job->control, &b2r2_core.hw->BLT_CQ2_TRIG_CTL);
+		writel(job->pace_control, &b2r2_core.hw->BLT_CQ2_PACE_CTL);
+		break;
+
+	case B2R2_CORE_QUEUE_AQ1:
+		writel(job->control, &b2r2_core.hw->BLT_AQ1_CTL);
+		writel(job->first_node_address, &b2r2_core.hw->BLT_AQ1_IP);
+		wmb();
+		start_hw_timer(job);
+		writel(job->last_node_address, &b2r2_core.hw->BLT_AQ1_LNA);
+		break;
+
+	case B2R2_CORE_QUEUE_AQ2:
+		writel(job->control, &b2r2_core.hw->BLT_AQ2_CTL);
+		writel(job->first_node_address, &b2r2_core.hw->BLT_AQ2_IP);
+		wmb();
+		start_hw_timer(job);
+		writel(job->last_node_address, &b2r2_core.hw->BLT_AQ2_LNA);
+		break;
+
+	case B2R2_CORE_QUEUE_AQ3:
+		writel(job->control, &b2r2_core.hw->BLT_AQ3_CTL);
+		writel(job->first_node_address, &b2r2_core.hw->BLT_AQ3_IP);
+		wmb();
+		start_hw_timer(job);
+		writel(job->last_node_address, &b2r2_core.hw->BLT_AQ3_LNA);
+		break;
+
+	case B2R2_CORE_QUEUE_AQ4:
+		writel(job->control, &b2r2_core.hw->BLT_AQ4_CTL);
+		writel(job->first_node_address, &b2r2_core.hw->BLT_AQ4_IP);
+		wmb();
+		start_hw_timer(job);
+		writel(job->last_node_address, &b2r2_core.hw->BLT_AQ4_LNA);
+		break;
+
+		/** Handle the default case */
+	default:
+		break;
+
+	} /* end switch */
+
+}
+
+/**
+ * handle_queue_event() - Handles interrupt event for specified B2R2 queue
+ *
+ * @queue: Queue to handle event for
+ *
+ * b2r2_core.lock must be held
+ */
+static void handle_queue_event(enum b2r2_core_queue queue)
+{
+	struct b2r2_core_job *job;
+
+	job = b2r2_core.active_jobs[queue];
+	if (job) {
+		if (job->job_state != B2R2_CORE_JOB_RUNNING)
+			/* Should be running
+			   Severe error. TBD */
+			b2r2_log_warn(
+				 "%s: Job is not running", __func__);
+
+		stop_hw_timer(job);
+
+		/* Remove from queue */
+		BUG_ON(b2r2_core.n_active_jobs == 0);
+		b2r2_core.active_jobs[queue] = NULL;
+		b2r2_core.n_active_jobs--;
+	}
+
+	if (!job) {
+		/* No job, error?  */
+		b2r2_log_warn("%s: No job", __func__);
+		return;
+	}
+
+
+	/* Atomic context release resources, release resources will
+	   be called again later from process context (work queue) */
+	if (job->release_resources)
+		job->release_resources(job, true);
+
+	/* Job is done */
+	job->job_state = B2R2_CORE_JOB_DONE;
+
+	/* Handle done */
+	wake_up_interruptible(&job->event);
+
+	/* Dispatch to work queue to handle callbacks */
+	queue_work(b2r2_core.work_queue, &job->work);
+}
+
+/**
+ * process_events() - Handles interrupt events
+ *
+ * @status: Contents of the B2R2 ITS register
+ */
+static void process_events(u32 status)
+{
+	u32 mask = 0xF;
+	u32 disable_itm_mask = 0;
+
+	b2r2_log_info("Enters process_events \n");
+	b2r2_log_info("status 0x%x \n", status);
+
+	/* Composition queue 1 */
+	if (status & mask) {
+		handle_queue_event(B2R2_CORE_QUEUE_CQ1);
+		disable_itm_mask |= mask;
+	}
+	mask <<= 4;
+
+	/* Composition queue 2 */
+	if (status & mask) {
+		handle_queue_event(B2R2_CORE_QUEUE_CQ2);
+		disable_itm_mask |= mask;
+	}
+	mask <<= 8;
+
+	/* Application queue 1 */
+	if (status & mask) {
+		handle_queue_event(B2R2_CORE_QUEUE_AQ1);
+		disable_itm_mask |= mask;
+	}
+	mask <<= 4;
+
+	/* Application queue 2 */
+	if (status & mask) {
+		handle_queue_event(B2R2_CORE_QUEUE_AQ2);
+		disable_itm_mask |= mask;
+	}
+	mask <<= 4;
+
+	/* Application queue 3 */
+	if (status & mask) {
+		handle_queue_event(B2R2_CORE_QUEUE_AQ3);
+		disable_itm_mask |= mask;
+	}
+	mask <<= 4;
+
+	/* Application queue 4 */
+	if (status & mask) {
+		handle_queue_event(B2R2_CORE_QUEUE_AQ4);
+		disable_itm_mask |= mask;
+	}
+
+	/* Clear received interrupt flags */
+	writel(status, &b2r2_core.hw->BLT_ITS);
+	/* Disable handled interrupts */
+	writel(readl(&b2r2_core.hw->BLT_ITM0) & ~disable_itm_mask,
+		 &b2r2_core.hw->BLT_ITM0);
+
+	b2r2_log_info("Returns process_events \n");
+}
+
+/**
+ * b2r2_irq_handler() - B2R2 interrupt handler
+ *
+ * @irq: Interrupt number (not used)
+ * @x: ??? (Not used)
+ */
+static irqreturn_t b2r2_irq_handler(int irq, void *x)
+{
+	unsigned long flags;
+
+	/* Spin lock is need in irq handler (SMP) */
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+
+	/* Make sure that we have a clock */
+
+	/* Remember time for last irq (for timeout mgmt) */
+	b2r2_core.jiffies_last_irq = jiffies;
+	b2r2_core.stat_n_irq++;
+
+	/* Handle the interrupt(s) */
+	process_events(readl(&b2r2_core.hw->BLT_ITS));
+
+	/* Check if we can dispatch new jobs */
+	check_prio_list(true);
+
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+
+#ifdef CONFIG_DEBUG_FS
+/**
+ * struct debugfs_reg - Represents one B2R2 register in debugfs
+ *
+ * @name: Register name
+ * @offset: Byte offset in B2R2 for register
+ */
+struct debugfs_reg {
+	const char name[30];
+	u32        offset;
+};
+
+/**
+ * debugfs_regs - Array of B2R2 debugfs registers
+ */
+static const struct debugfs_reg debugfs_regs[] = {
+	{"BLT_SSBA17", offsetof(struct b2r2_memory_map, BLT_SSBA17)},
+	{"BLT_SSBA18", offsetof(struct b2r2_memory_map, BLT_SSBA18)},
+	{"BLT_SSBA19", offsetof(struct b2r2_memory_map, BLT_SSBA19)},
+	{"BLT_SSBA20", offsetof(struct b2r2_memory_map, BLT_SSBA20)},
+	{"BLT_SSBA21", offsetof(struct b2r2_memory_map, BLT_SSBA21)},
+	{"BLT_SSBA22", offsetof(struct b2r2_memory_map, BLT_SSBA22)},
+	{"BLT_SSBA23", offsetof(struct b2r2_memory_map, BLT_SSBA23)},
+	{"BLT_SSBA24", offsetof(struct b2r2_memory_map, BLT_SSBA24)},
+	{"BLT_STBA5", offsetof(struct b2r2_memory_map, BLT_STBA5)},
+	{"BLT_STBA6", offsetof(struct b2r2_memory_map, BLT_STBA6)},
+	{"BLT_STBA7", offsetof(struct b2r2_memory_map, BLT_STBA7)},
+	{"BLT_STBA8", offsetof(struct b2r2_memory_map, BLT_STBA8)},
+	{"BLT_CTL", offsetof(struct b2r2_memory_map, BLT_CTL)},
+	{"BLT_ITS", offsetof(struct b2r2_memory_map, BLT_ITS)},
+	{"BLT_STA1", offsetof(struct b2r2_memory_map, BLT_STA1)},
+	{"BLT_SSBA1", offsetof(struct b2r2_memory_map, BLT_SSBA1)},
+	{"BLT_SSBA2", offsetof(struct b2r2_memory_map, BLT_SSBA2)},
+	{"BLT_SSBA3", offsetof(struct b2r2_memory_map, BLT_SSBA3)},
+	{"BLT_SSBA4", offsetof(struct b2r2_memory_map, BLT_SSBA4)},
+	{"BLT_SSBA5", offsetof(struct b2r2_memory_map, BLT_SSBA5)},
+	{"BLT_SSBA6", offsetof(struct b2r2_memory_map, BLT_SSBA6)},
+	{"BLT_SSBA7", offsetof(struct b2r2_memory_map, BLT_SSBA7)},
+	{"BLT_SSBA8", offsetof(struct b2r2_memory_map, BLT_SSBA8)},
+	{"BLT_STBA1", offsetof(struct b2r2_memory_map, BLT_STBA1)},
+	{"BLT_STBA2", offsetof(struct b2r2_memory_map, BLT_STBA2)},
+	{"BLT_STBA3", offsetof(struct b2r2_memory_map, BLT_STBA3)},
+	{"BLT_STBA4", offsetof(struct b2r2_memory_map, BLT_STBA4)},
+	{"BLT_CQ1_TRIG_IP", offsetof(struct b2r2_memory_map, BLT_CQ1_TRIG_IP)},
+	{"BLT_CQ1_TRIG_CTL", offsetof(struct b2r2_memory_map,
+				      BLT_CQ1_TRIG_CTL)},
+	{"BLT_CQ1_PACE_CTL", offsetof(struct b2r2_memory_map,
+				      BLT_CQ1_PACE_CTL)},
+	{"BLT_CQ1_IP", offsetof(struct b2r2_memory_map, BLT_CQ1_IP)},
+	{"BLT_CQ2_TRIG_IP", offsetof(struct b2r2_memory_map, BLT_CQ2_TRIG_IP)},
+	{"BLT_CQ2_TRIG_CTL", offsetof(struct b2r2_memory_map,
+				      BLT_CQ2_TRIG_CTL)},
+	{"BLT_CQ2_PACE_CTL", offsetof(struct b2r2_memory_map,
+				      BLT_CQ2_PACE_CTL)},
+	{"BLT_CQ2_IP", offsetof(struct b2r2_memory_map, BLT_CQ2_IP)},
+	{"BLT_AQ1_CTL", offsetof(struct b2r2_memory_map, BLT_AQ1_CTL)},
+	{"BLT_AQ1_IP", offsetof(struct b2r2_memory_map, BLT_AQ1_IP)},
+	{"BLT_AQ1_LNA", offsetof(struct b2r2_memory_map, BLT_AQ1_LNA)},
+	{"BLT_AQ1_STA", offsetof(struct b2r2_memory_map, BLT_AQ1_STA)},
+	{"BLT_AQ2_CTL", offsetof(struct b2r2_memory_map, BLT_AQ2_CTL)},
+	{"BLT_AQ2_IP", offsetof(struct b2r2_memory_map, BLT_AQ2_IP)},
+	{"BLT_AQ2_LNA", offsetof(struct b2r2_memory_map, BLT_AQ2_LNA)},
+	{"BLT_AQ2_STA", offsetof(struct b2r2_memory_map, BLT_AQ2_STA)},
+	{"BLT_AQ3_CTL", offsetof(struct b2r2_memory_map, BLT_AQ3_CTL)},
+	{"BLT_AQ3_IP", offsetof(struct b2r2_memory_map, BLT_AQ3_IP)},
+	{"BLT_AQ3_LNA", offsetof(struct b2r2_memory_map, BLT_AQ3_LNA)},
+	{"BLT_AQ3_STA", offsetof(struct b2r2_memory_map, BLT_AQ3_STA)},
+	{"BLT_AQ4_CTL", offsetof(struct b2r2_memory_map, BLT_AQ4_CTL)},
+	{"BLT_AQ4_IP", offsetof(struct b2r2_memory_map, BLT_AQ4_IP)},
+	{"BLT_AQ4_LNA", offsetof(struct b2r2_memory_map, BLT_AQ4_LNA)},
+	{"BLT_AQ4_STA", offsetof(struct b2r2_memory_map, BLT_AQ4_STA)},
+	{"BLT_SSBA9", offsetof(struct b2r2_memory_map, BLT_SSBA9)},
+	{"BLT_SSBA10", offsetof(struct b2r2_memory_map, BLT_SSBA10)},
+	{"BLT_SSBA11", offsetof(struct b2r2_memory_map, BLT_SSBA11)},
+	{"BLT_SSBA12", offsetof(struct b2r2_memory_map, BLT_SSBA12)},
+	{"BLT_SSBA13", offsetof(struct b2r2_memory_map, BLT_SSBA13)},
+	{"BLT_SSBA14", offsetof(struct b2r2_memory_map, BLT_SSBA14)},
+	{"BLT_SSBA15", offsetof(struct b2r2_memory_map, BLT_SSBA15)},
+	{"BLT_SSBA16", offsetof(struct b2r2_memory_map, BLT_SSBA16)},
+	{"BLT_SGA1", offsetof(struct b2r2_memory_map, BLT_SGA1)},
+	{"BLT_SGA2", offsetof(struct b2r2_memory_map, BLT_SGA2)},
+	{"BLT_ITM0", offsetof(struct b2r2_memory_map, BLT_ITM0)},
+	{"BLT_ITM1", offsetof(struct b2r2_memory_map, BLT_ITM1)},
+	{"BLT_ITM2", offsetof(struct b2r2_memory_map, BLT_ITM2)},
+	{"BLT_ITM3", offsetof(struct b2r2_memory_map, BLT_ITM3)},
+	{"BLT_DFV2", offsetof(struct b2r2_memory_map, BLT_DFV2)},
+	{"BLT_DFV1", offsetof(struct b2r2_memory_map, BLT_DFV1)},
+	{"BLT_PRI", offsetof(struct b2r2_memory_map, BLT_PRI)},
+	{"PLUGS1_OP2", offsetof(struct b2r2_memory_map, PLUGS1_OP2)},
+	{"PLUGS1_CHZ", offsetof(struct b2r2_memory_map, PLUGS1_CHZ)},
+	{"PLUGS1_MSZ", offsetof(struct b2r2_memory_map, PLUGS1_MSZ)},
+	{"PLUGS1_PGZ", offsetof(struct b2r2_memory_map, PLUGS1_PGZ)},
+	{"PLUGS2_OP2", offsetof(struct b2r2_memory_map, PLUGS2_OP2)},
+	{"PLUGS2_CHZ", offsetof(struct b2r2_memory_map, PLUGS2_CHZ)},
+	{"PLUGS2_MSZ", offsetof(struct b2r2_memory_map, PLUGS2_MSZ)},
+	{"PLUGS2_PGZ", offsetof(struct b2r2_memory_map, PLUGS2_PGZ)},
+	{"PLUGS3_OP2", offsetof(struct b2r2_memory_map, PLUGS3_OP2)},
+	{"PLUGS3_CHZ", offsetof(struct b2r2_memory_map, PLUGS3_CHZ)},
+	{"PLUGS3_MSZ", offsetof(struct b2r2_memory_map, PLUGS3_MSZ)},
+	{"PLUGS3_PGZ", offsetof(struct b2r2_memory_map, PLUGS3_PGZ)},
+	{"PLUGT_OP2", offsetof(struct b2r2_memory_map, PLUGT_OP2)},
+	{"PLUGT_CHZ", offsetof(struct b2r2_memory_map, PLUGT_CHZ)},
+	{"PLUGT_MSZ", offsetof(struct b2r2_memory_map, PLUGT_MSZ)},
+	{"PLUGT_PGZ", offsetof(struct b2r2_memory_map, PLUGT_PGZ)},
+	{"BLT_NIP", offsetof(struct b2r2_memory_map, BLT_NIP)},
+	{"BLT_CIC", offsetof(struct b2r2_memory_map, BLT_CIC)},
+	{"BLT_INS", offsetof(struct b2r2_memory_map, BLT_INS)},
+	{"BLT_ACK", offsetof(struct b2r2_memory_map, BLT_ACK)},
+	{"BLT_TBA", offsetof(struct b2r2_memory_map, BLT_TBA)},
+	{"BLT_TTY", offsetof(struct b2r2_memory_map, BLT_TTY)},
+	{"BLT_TXY", offsetof(struct b2r2_memory_map, BLT_TXY)},
+	{"BLT_TSZ", offsetof(struct b2r2_memory_map, BLT_TSZ)},
+	{"BLT_S1CF", offsetof(struct b2r2_memory_map, BLT_S1CF)},
+	{"BLT_S2CF", offsetof(struct b2r2_memory_map, BLT_S2CF)},
+	{"BLT_S1BA", offsetof(struct b2r2_memory_map, BLT_S1BA)},
+	{"BLT_S1TY", offsetof(struct b2r2_memory_map, BLT_S1TY)},
+	{"BLT_S1XY", offsetof(struct b2r2_memory_map, BLT_S1XY)},
+	{"BLT_S2BA", offsetof(struct b2r2_memory_map, BLT_S2BA)},
+	{"BLT_S2TY", offsetof(struct b2r2_memory_map, BLT_S2TY)},
+	{"BLT_S2XY", offsetof(struct b2r2_memory_map, BLT_S2XY)},
+	{"BLT_S2SZ", offsetof(struct b2r2_memory_map, BLT_S2SZ)},
+	{"BLT_S3BA", offsetof(struct b2r2_memory_map, BLT_S3BA)},
+	{"BLT_S3TY", offsetof(struct b2r2_memory_map, BLT_S3TY)},
+	{"BLT_S3XY", offsetof(struct b2r2_memory_map, BLT_S3XY)},
+	{"BLT_S3SZ", offsetof(struct b2r2_memory_map, BLT_S3SZ)},
+	{"BLT_CWO", offsetof(struct b2r2_memory_map, BLT_CWO)},
+	{"BLT_CWS", offsetof(struct b2r2_memory_map, BLT_CWS)},
+	{"BLT_CCO", offsetof(struct b2r2_memory_map, BLT_CCO)},
+	{"BLT_CML", offsetof(struct b2r2_memory_map, BLT_CML)},
+	{"BLT_FCTL", offsetof(struct b2r2_memory_map, BLT_FCTL)},
+	{"BLT_PMK", offsetof(struct b2r2_memory_map, BLT_PMK)},
+	{"BLT_RSF", offsetof(struct b2r2_memory_map, BLT_RSF)},
+	{"BLT_RZI", offsetof(struct b2r2_memory_map, BLT_RZI)},
+	{"BLT_HFP", offsetof(struct b2r2_memory_map, BLT_HFP)},
+	{"BLT_VFP", offsetof(struct b2r2_memory_map, BLT_VFP)},
+	{"BLT_Y_RSF", offsetof(struct b2r2_memory_map, BLT_Y_RSF)},
+	{"BLT_Y_RZI", offsetof(struct b2r2_memory_map, BLT_Y_RZI)},
+	{"BLT_Y_HFP", offsetof(struct b2r2_memory_map, BLT_Y_HFP)},
+	{"BLT_Y_VFP", offsetof(struct b2r2_memory_map, BLT_Y_VFP)},
+	{"BLT_KEY1", offsetof(struct b2r2_memory_map, BLT_KEY1)},
+	{"BLT_KEY2", offsetof(struct b2r2_memory_map, BLT_KEY2)},
+	{"BLT_SAR", offsetof(struct b2r2_memory_map, BLT_SAR)},
+	{"BLT_USR", offsetof(struct b2r2_memory_map, BLT_USR)},
+	{"BLT_IVMX0", offsetof(struct b2r2_memory_map, BLT_IVMX0)},
+	{"BLT_IVMX1", offsetof(struct b2r2_memory_map, BLT_IVMX1)},
+	{"BLT_IVMX2", offsetof(struct b2r2_memory_map, BLT_IVMX2)},
+	{"BLT_IVMX3", offsetof(struct b2r2_memory_map, BLT_IVMX3)},
+	{"BLT_OVMX0", offsetof(struct b2r2_memory_map, BLT_OVMX0)},
+	{"BLT_OVMX1", offsetof(struct b2r2_memory_map, BLT_OVMX1)},
+	{"BLT_OVMX2", offsetof(struct b2r2_memory_map, BLT_OVMX2)},
+	{"BLT_OVMX3", offsetof(struct b2r2_memory_map, BLT_OVMX3)},
+	{"BLT_VC1R", offsetof(struct b2r2_memory_map, BLT_VC1R)},
+	{"BLT_Y_HFC0", offsetof(struct b2r2_memory_map, BLT_Y_HFC0)},
+	{"BLT_Y_HFC1", offsetof(struct b2r2_memory_map, BLT_Y_HFC1)},
+	{"BLT_Y_HFC2", offsetof(struct b2r2_memory_map, BLT_Y_HFC2)},
+	{"BLT_Y_HFC3", offsetof(struct b2r2_memory_map, BLT_Y_HFC3)},
+	{"BLT_Y_HFC4", offsetof(struct b2r2_memory_map, BLT_Y_HFC4)},
+	{"BLT_Y_HFC5", offsetof(struct b2r2_memory_map, BLT_Y_HFC5)},
+	{"BLT_Y_HFC6", offsetof(struct b2r2_memory_map, BLT_Y_HFC6)},
+	{"BLT_Y_HFC7", offsetof(struct b2r2_memory_map, BLT_Y_HFC7)},
+	{"BLT_Y_HFC8", offsetof(struct b2r2_memory_map, BLT_Y_HFC8)},
+	{"BLT_Y_HFC9", offsetof(struct b2r2_memory_map, BLT_Y_HFC9)},
+	{"BLT_Y_HFC10", offsetof(struct b2r2_memory_map, BLT_Y_HFC10)},
+	{"BLT_Y_HFC11", offsetof(struct b2r2_memory_map, BLT_Y_HFC11)},
+	{"BLT_Y_HFC12", offsetof(struct b2r2_memory_map, BLT_Y_HFC12)},
+	{"BLT_Y_HFC13", offsetof(struct b2r2_memory_map, BLT_Y_HFC13)},
+	{"BLT_Y_HFC14", offsetof(struct b2r2_memory_map, BLT_Y_HFC14)},
+	{"BLT_Y_HFC15", offsetof(struct b2r2_memory_map, BLT_Y_HFC15)},
+	{"BLT_Y_VFC0", offsetof(struct b2r2_memory_map, BLT_Y_VFC0)},
+	{"BLT_Y_VFC1", offsetof(struct b2r2_memory_map, BLT_Y_VFC1)},
+	{"BLT_Y_VFC2", offsetof(struct b2r2_memory_map, BLT_Y_VFC2)},
+	{"BLT_Y_VFC3", offsetof(struct b2r2_memory_map, BLT_Y_VFC3)},
+	{"BLT_Y_VFC4", offsetof(struct b2r2_memory_map, BLT_Y_VFC4)},
+	{"BLT_Y_VFC5", offsetof(struct b2r2_memory_map, BLT_Y_VFC5)},
+	{"BLT_Y_VFC6", offsetof(struct b2r2_memory_map, BLT_Y_VFC6)},
+	{"BLT_Y_VFC7", offsetof(struct b2r2_memory_map, BLT_Y_VFC7)},
+	{"BLT_Y_VFC8", offsetof(struct b2r2_memory_map, BLT_Y_VFC8)},
+	{"BLT_Y_VFC9", offsetof(struct b2r2_memory_map, BLT_Y_VFC9)},
+	{"BLT_HFC0", offsetof(struct b2r2_memory_map, BLT_HFC0)},
+	{"BLT_HFC1", offsetof(struct b2r2_memory_map, BLT_HFC1)},
+	{"BLT_HFC2", offsetof(struct b2r2_memory_map, BLT_HFC2)},
+	{"BLT_HFC3", offsetof(struct b2r2_memory_map, BLT_HFC3)},
+	{"BLT_HFC4", offsetof(struct b2r2_memory_map, BLT_HFC4)},
+	{"BLT_HFC5", offsetof(struct b2r2_memory_map, BLT_HFC5)},
+	{"BLT_HFC6", offsetof(struct b2r2_memory_map, BLT_HFC6)},
+	{"BLT_HFC7", offsetof(struct b2r2_memory_map, BLT_HFC7)},
+	{"BLT_HFC8", offsetof(struct b2r2_memory_map, BLT_HFC8)},
+	{"BLT_HFC9", offsetof(struct b2r2_memory_map, BLT_HFC9)},
+	{"BLT_HFC10", offsetof(struct b2r2_memory_map, BLT_HFC10)},
+	{"BLT_HFC11", offsetof(struct b2r2_memory_map, BLT_HFC11)},
+	{"BLT_HFC12", offsetof(struct b2r2_memory_map, BLT_HFC12)},
+	{"BLT_HFC13", offsetof(struct b2r2_memory_map, BLT_HFC13)},
+	{"BLT_HFC14", offsetof(struct b2r2_memory_map, BLT_HFC14)},
+	{"BLT_HFC15", offsetof(struct b2r2_memory_map, BLT_HFC15)},
+	{"BLT_VFC0", offsetof(struct b2r2_memory_map, BLT_VFC0)},
+	{"BLT_VFC1", offsetof(struct b2r2_memory_map, BLT_VFC1)},
+	{"BLT_VFC2", offsetof(struct b2r2_memory_map, BLT_VFC2)},
+	{"BLT_VFC3", offsetof(struct b2r2_memory_map, BLT_VFC3)},
+	{"BLT_VFC4", offsetof(struct b2r2_memory_map, BLT_VFC4)},
+	{"BLT_VFC5", offsetof(struct b2r2_memory_map, BLT_VFC5)},
+	{"BLT_VFC6", offsetof(struct b2r2_memory_map, BLT_VFC6)},
+	{"BLT_VFC7", offsetof(struct b2r2_memory_map, BLT_VFC7)},
+	{"BLT_VFC8", offsetof(struct b2r2_memory_map, BLT_VFC8)},
+	{"BLT_VFC9", offsetof(struct b2r2_memory_map, BLT_VFC9)},
+};
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+/**
+ * printk_regs() - Print B2R2 registers to printk
+ */
+static void printk_regs(void)
+{
+#ifdef CONFIG_B2R2_DEBUG
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(debugfs_regs); i++) {
+		unsigned long value = readl(
+			(unsigned long *) (((u8 *) b2r2_core.hw) +
+					   debugfs_regs[i].offset));
+		b2r2_log_regdump("%s: %08lX\n",
+			 debugfs_regs[i].name,
+			 value);
+	}
+#endif
+}
+#endif
+
+/**
+ * debugfs_b2r2_reg_read() - Implements debugfs read for B2R2 register
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static int debugfs_b2r2_reg_read(struct file *filp, char __user *buf,
+				 size_t count, loff_t *f_pos)
+{
+	size_t dev_size;
+	int ret = 0;
+
+	unsigned long value;
+	char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+	if (Buf == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* Read from B2R2 */
+	value = readl((unsigned long *) (((u8 *) b2r2_core.hw) +
+					 (u32) filp->f_dentry->
+					 d_inode->i_private));
+
+	/* Build the string */
+	dev_size = sprintf(Buf, "%8lX\n", value);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	/* Return it to user space */
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	if (Buf != NULL)
+		kfree(Buf);
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_reg_write() - Implements debugfs write for B2R2 register
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to write
+ * @f_pos: File position
+ *
+ * Returns number of bytes written or negative error code
+ */
+static int debugfs_b2r2_reg_write(struct file *filp, const char __user *buf,
+				  size_t count, loff_t *f_pos)
+{
+	char Buf[80];
+	u32 reg_value;
+	int ret = 0;
+
+	/* Adjust count */
+	if (count >= sizeof(Buf))
+		count = sizeof(Buf) - 1;
+	/* Get it from user space */
+	if (copy_from_user(Buf, buf, count))
+		return -EINVAL;
+	Buf[count] = 0;
+	/* Convert from hex string */
+	if (sscanf(Buf, "%8lX", (unsigned long *) &reg_value) != 1)
+		return -EINVAL;
+
+	writel(reg_value, (u32 *) (((u8 *) b2r2_core.hw) +
+			(u32) filp->f_dentry->d_inode->i_private));
+
+	*f_pos += count;
+	ret = count;
+
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_reg_fops() - File operations for B2R2 register debugfs
+ */
+static const struct file_operations debugfs_b2r2_reg_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_reg_read,
+	.write = debugfs_b2r2_reg_write,
+};
+
+/**
+ * debugfs_b2r2_regs_read() - Implements debugfs read for B2R2 register dump
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes written or negative error code
+ */
+static int debugfs_b2r2_regs_read(struct file *filp, char __user *buf,
+				  size_t count, loff_t *f_pos)
+{
+	size_t dev_size = 0;
+	int ret = 0;
+	int i;
+	char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+	if (Buf == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* Build a giant string containing all registers */
+	for (i = 0; i < ARRAY_SIZE(debugfs_regs); i++) {
+		unsigned long value =
+			readl((unsigned long *) (((u8 *) b2r2_core.hw) +
+						 debugfs_regs[i].offset));
+		dev_size += sprintf(Buf + dev_size, "%s: %08lX\n",
+				    debugfs_regs[i].name,
+				    value);
+	}
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	if (Buf != NULL)
+		kfree(Buf);
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_regs_fops() - File operations for B2R2 register dump debugfs
+ */
+static const struct file_operations debugfs_b2r2_regs_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_regs_read,
+};
+
+/**
+ * debugfs_b2r2_stat_read() - Implements debugfs read for B2R2 statistics
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static int debugfs_b2r2_stat_read(struct file *filp, char __user *buf,
+				  size_t count, loff_t *f_pos)
+{
+	size_t dev_size = 0;
+	int ret = 0;
+	int i = 0;
+	char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+	if (Buf == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* Build a string containing all statistics */
+	dev_size += sprintf(Buf + dev_size, "Interrupts: %lu\n",
+			    b2r2_core.stat_n_irq);
+	dev_size += sprintf(Buf + dev_size, "Added jobs: %lu\n",
+			    b2r2_core.stat_n_jobs_added);
+	dev_size += sprintf(Buf + dev_size, "Removed jobs: %lu\n",
+			    b2r2_core.stat_n_jobs_removed);
+	dev_size += sprintf(Buf + dev_size, "Jobs in prio list: %lu\n",
+			    b2r2_core.stat_n_jobs_in_prio_list);
+	dev_size += sprintf(Buf + dev_size, "Active jobs: %lu\n",
+			    b2r2_core.n_active_jobs);
+	for (i = 0; i < ARRAY_SIZE(b2r2_core.active_jobs); i++)
+		dev_size += sprintf(Buf + dev_size, "Job in queue %d: %p\n",
+				    i, b2r2_core.active_jobs[i]);
+	dev_size += sprintf(Buf + dev_size, "Clock requests: %lu\n",
+			    b2r2_core.clock_request_count);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	if (Buf != NULL)
+		kfree(Buf);
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_stat_fops() - File operations for B2R2 statistics debugfs
+ */
+static const struct file_operations debugfs_b2r2_stat_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_stat_read,
+};
+
+
+/**
+ * debugfs_b2r2_clock_read() - Implements debugfs read for
+ *                             PMU B2R2 clock register
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static int debugfs_b2r2_clock_read(struct file *filp, char __user *buf,
+				   size_t count, loff_t *f_pos)
+{
+	/* 10 characters hex number + newline + string terminator; */
+	char Buf[10+2];
+	size_t dev_size;
+	int ret = 0;
+
+	unsigned long value = clk_get_rate(b2r2_core.b2r2_clock);
+
+	dev_size = sprintf(Buf, "%#010lX\n", value);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_clock_write() - Implements debugfs write for
+ *                              PMU B2R2 clock register
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to write
+ * @f_pos: File position
+ *
+ * Returns number of bytes written or negative error code
+ */
+static int debugfs_b2r2_clock_write(struct file *filp, const char __user *buf,
+				    size_t count, loff_t *f_pos)
+{
+	char Buf[80];
+	u32 reg_value;
+	int ret = 0;
+
+	if (count >= sizeof(Buf))
+		count = sizeof(Buf) - 1;
+	if (copy_from_user(Buf, buf, count))
+		return -EINVAL;
+	Buf[count] = 0;
+	if (sscanf(Buf, "%8lX", (unsigned long *) &reg_value) != 1)
+		return -EINVAL;
+
+	/*not working yet*/
+	/*clk_set_rate(b2r2_core.b2r2_clock, (unsigned long) reg_value);*/
+
+	*f_pos += count;
+	ret = count;
+
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_clock_fops() - File operations for PMU B2R2 clock debugfs
+ */
+static const struct file_operations debugfs_b2r2_clock_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_clock_read,
+	.write = debugfs_b2r2_clock_write,
+};
+
+#endif
+
+/**
+ *
+ * init_hw() - B2R2 Hardware reset & initiliaze
+ *
+ * @pdev: B2R2 platform device
+ *
+ * 1)Register interrupt handler
+ *
+ * 2)B2R2 Register map
+ *
+ * 3)For resetting B2R2 hardware,write to B2R2 Control register the
+ * B2R2BLT_CTLGLOBAL_soft_reset and then polling for on
+ * B2R2 status register for B2R2BLT_STA1BDISP_IDLE flag.
+ *
+ * 4)Wait for B2R2 hardware to be idle (on a timeout rather than while loop)
+ *
+ * 5)Driver status reset
+ *
+ * 6)Recover from any error without any leaks.
+ *
+ */
+static int init_hw(void)
+{
+	int result = 0;
+	u32 uTimeOut = B2R2_DRIVER_TIMEOUT_VALUE;
+
+	/* Put B2R2 into reset */
+	clear_interrupts();
+
+	writel(readl(&b2r2_core.hw->BLT_CTL) | B2R2BLT_CTLGLOBAL_soft_reset,
+		&b2r2_core.hw->BLT_CTL);
+
+
+	/* Set up interrupt handler */
+	result = request_irq(b2r2_core.irq, b2r2_irq_handler, 0,
+			     "b2r2-interrupt", 0);
+	if (result) {
+		b2r2_log_err("%s: failed to register IRQ for B2R2\n", __func__);
+		goto b2r2_init_request_irq_failed;
+	}
+
+	b2r2_log_info("do a global reset..\n");
+
+	/* Release reset */
+	writel(0x00000000, &b2r2_core.hw->BLT_CTL);
+
+	b2r2_log_info("wait for B2R2 to be idle..\n");
+
+	/** Wait for B2R2 to be idle (on a timeout rather than while loop) */
+	while ((uTimeOut > 0) &&
+	       ((readl(&b2r2_core.hw->BLT_STA1) &
+		 B2R2BLT_STA1BDISP_IDLE) == 0x0))
+		uTimeOut--;
+	if (uTimeOut == 0) {
+		b2r2_log_err(
+			 "%s: B2R2 not idle after SW reset\n", __func__);
+		result = -EAGAIN;
+		goto b2r2_core_init_hw_timeout;
+	}
+
+#ifdef CONFIG_DEBUG_FS
+	/* Register debug fs files for register access */
+	if (b2r2_core.debugfs_root_dir && !b2r2_core.debugfs_regs_dir) {
+		int i;
+		b2r2_core.debugfs_regs_dir = debugfs_create_dir("regs",
+				b2r2_core.debugfs_root_dir);
+		debugfs_create_file("all", 0666, b2r2_core.debugfs_regs_dir,
+				0, &debugfs_b2r2_regs_fops);
+		/* Create debugfs entries for all static registers */
+		for (i = 0; i < ARRAY_SIZE(debugfs_regs); i++)
+			debugfs_create_file(debugfs_regs[i].name, 0666,
+					b2r2_core.debugfs_regs_dir,
+					(void *) debugfs_regs[i].offset,
+					&debugfs_b2r2_reg_fops);
+	}
+#endif
+
+	b2r2_log_info("%s ended..\n", __func__);
+	return result;
+
+/** Recover from any error without any leaks */
+
+b2r2_core_init_hw_timeout:
+
+	/** Free B2R2 interrupt handler */
+
+	free_irq(b2r2_core.irq, 0);
+
+b2r2_init_request_irq_failed:
+
+	if (b2r2_core.hw)
+		iounmap(b2r2_core.hw);
+	b2r2_core.hw = NULL;
+
+	return result;
+
+}
+
+
+/**
+ * exit_hw() - B2R2 Hardware exit
+ *
+ * b2r2_core.lock _must_ NOT be held
+ */
+static void exit_hw(void)
+{
+	unsigned long flags;
+
+	b2r2_log_info("%s started..\n", __func__);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Unregister our debugfs entries */
+	if (b2r2_core.debugfs_regs_dir) {
+		debugfs_remove_recursive(b2r2_core.debugfs_regs_dir);
+		b2r2_core.debugfs_regs_dir = NULL;
+	}
+#endif
+	b2r2_log_debug("%s: locking b2r2_core.lock\n", __func__);
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+
+	/* Cancel all pending jobs */
+	b2r2_log_debug("%s: canceling pending jobs\n", __func__);
+	exit_job_list(&b2r2_core.prio_queue);
+
+	/* Soft reset B2R2 (Close all DMA,
+	   reset all state to idle, reset regs)*/
+	b2r2_log_debug("%s: putting b2r2 in reset\n", __func__);
+	writel(readl(&b2r2_core.hw->BLT_CTL) | B2R2BLT_CTLGLOBAL_soft_reset,
+		&b2r2_core.hw->BLT_CTL);
+
+	b2r2_log_debug("%s: clearing interrupts\n", __func__);
+	clear_interrupts();
+
+	/** Free B2R2 interrupt handler */
+	b2r2_log_debug("%s: freeing interrupt handler\n", __func__);
+	free_irq(b2r2_core.irq, 0);
+
+	b2r2_log_debug("%s: unlocking b2r2_core.lock\n", __func__);
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+
+	b2r2_log_info("%s ended...\n", __func__);
+}
+
+/**
+ * b2r2_probe() - This routine loads the B2R2 core driver
+ *
+ * @pdev: platform device.
+ */
+static int b2r2_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct resource *res;
+
+	BUG_ON(pdev == NULL);
+
+	ret = b2r2_debug_init(&pdev->dev);
+	if (ret < 0) {
+		dev_err(b2r2_core.log_dev, "b2r2_debug_init failed\n");
+		goto b2r2_probe_debug_init_failed;
+	}
+
+	b2r2_core.log_dev = &pdev->dev;
+	b2r2_log_info("init started.\n");
+
+	/* Init spin locks */
+	spin_lock_init(&b2r2_core.lock);
+
+	/* Init job queues */
+	INIT_LIST_HEAD(&b2r2_core.prio_queue);
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+	/* Create work queue for callbacks & timeout */
+	INIT_DELAYED_WORK(&b2r2_core.timeout_work, timeout_work_function);
+#endif
+
+	/* Work queue for callbacks and timeout management */
+	b2r2_core.work_queue = create_workqueue("B2R2");
+	if (!b2r2_core.work_queue) {
+		ret = -ENOMEM;
+		goto b2r2_probe_no_work_queue;
+	}
+
+	/* Get the clock for B2R2 */
+	b2r2_core.b2r2_clock = clk_get(&pdev->dev, "b2r2");
+	if (IS_ERR(b2r2_core.b2r2_clock)) {
+		ret = PTR_ERR(b2r2_core.b2r2_clock);
+		b2r2_log_err("clk_get b2r2 failed\n");
+		goto b2r2_probe_no_clk;
+	}
+
+	/* Get the B2R2 regulator */
+	b2r2_core.b2r2_reg = regulator_get(&pdev->dev, "vsupply");
+	if (IS_ERR(b2r2_core.b2r2_reg)) {
+		ret = PTR_ERR(b2r2_core.b2r2_reg);
+		b2r2_log_err("regulator_get vsupply failed (dev_name=%s)\n",
+				dev_name(&pdev->dev));
+		goto b2r2_probe_no_reg;
+	}
+
+	/* Init power management */
+	mutex_init(&b2r2_core.domain_lock);
+	INIT_DELAYED_WORK_DEFERRABLE(&b2r2_core.domain_disable_work,
+			domain_disable_work_function);
+	b2r2_core.domain_enabled = false;
+
+	/* Map B2R2 into kernel virtual memory space */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL)
+		goto b2r2_probe_no_res;
+
+	/* Hook up irq */
+	b2r2_core.irq = platform_get_irq(pdev, 0);
+	if (b2r2_core.irq <= 0) {
+		b2r2_log_info("%s: Failed to request irq (irq=%d)\n", __func__,
+								b2r2_core.irq);
+		goto b2r2_failed_irq_get;
+	}
+
+	b2r2_core.hw = (struct b2r2_memory_map *) ioremap(res->start,
+			 res->end - res->start + 1);
+	if (b2r2_core.hw == NULL) {
+
+		b2r2_log_info("%s: ioremap failed\n", __func__);
+		ret = -ENOMEM;
+		goto b2r2_probe_ioremap_failed;
+	}
+
+	dev_dbg(b2r2_core.log_dev,
+		"b2r2 structure address %p\n",
+		b2r2_core.hw);
+
+	/* Initialize b2r2_blt module. FIXME: Module of it's own
+	   or perhaps a dedicated module init c file? */
+	ret = b2r2_blt_module_init();
+	if (ret < 0) {
+		b2r2_log_err("b2r2_blt_module_init failed\n");
+		goto b2r2_probe_blt_init_fail;
+	}
+
+	b2r2_core.op_size = B2R2_PLUG_OPCODE_SIZE_DEFAULT;
+	b2r2_core.ch_size = B2R2_PLUG_CHUNK_SIZE_DEFAULT;
+	b2r2_core.pg_size = B2R2_PLUG_PAGE_SIZE_DEFAULT;
+	b2r2_core.mg_size = B2R2_PLUG_MESSAGE_SIZE_DEFAULT;
+	b2r2_core.min_req_time = 0;
+
+#ifdef CONFIG_DEBUG_FS
+	b2r2_core.debugfs_root_dir = debugfs_create_dir("b2r2", NULL);
+	debugfs_create_file("stat", 0666, b2r2_core.debugfs_root_dir,
+			0, &debugfs_b2r2_stat_fops);
+	debugfs_create_file("clock", 0666, b2r2_core.debugfs_root_dir,
+			0, &debugfs_b2r2_clock_fops);
+
+	debugfs_create_u8("op_size", 0666, b2r2_core.debugfs_root_dir,
+			&b2r2_core.op_size);
+	debugfs_create_u8("ch_size", 0666, b2r2_core.debugfs_root_dir,
+			&b2r2_core.ch_size);
+	debugfs_create_u8("pg_size", 0666, b2r2_core.debugfs_root_dir,
+			&b2r2_core.pg_size);
+	debugfs_create_u8("mg_size", 0666, b2r2_core.debugfs_root_dir,
+			&b2r2_core.mg_size);
+	debugfs_create_u16("min_req_time", 0666, b2r2_core.debugfs_root_dir,
+			&b2r2_core.min_req_time);
+#endif
+
+	b2r2_log_info("init done.\n");
+
+	return ret;
+
+/** Recover from any error if something fails */
+b2r2_probe_blt_init_fail:
+b2r2_probe_ioremap_failed:
+b2r2_failed_irq_get:
+b2r2_probe_no_res:
+	regulator_put(b2r2_core.b2r2_reg);
+b2r2_probe_no_reg:
+	clk_put(b2r2_core.b2r2_clock);
+b2r2_probe_no_clk:
+	destroy_workqueue(b2r2_core.work_queue);
+	b2r2_core.work_queue = NULL;
+b2r2_probe_no_work_queue:
+
+	b2r2_log_info("init done with errors.\n");
+b2r2_probe_debug_init_failed:
+
+	return ret;
+
+}
+
+
+
+/**
+ * b2r2_remove - This routine unloads b2r2 driver
+ *
+ * @pdev: platform device.
+ */
+static int b2r2_remove(struct platform_device *pdev)
+{
+	unsigned long flags;
+
+	BUG_ON(pdev == NULL);
+
+	b2r2_log_info("%s started\n", __func__);
+
+#ifdef CONFIG_DEBUG_FS
+	debugfs_remove_recursive(b2r2_core.debugfs_root_dir);
+#endif
+
+	/* Flush B2R2 work queue (call all callbacks) */
+	flush_workqueue(b2r2_core.work_queue);
+
+	/* Exit b2r2 blt module */
+	b2r2_blt_module_exit();
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+	cancel_delayed_work(&b2r2_core.timeout_work);
+#endif
+
+	/* Flush B2R2 work queue (call all callbacks for
+	   cancelled jobs) */
+	flush_workqueue(b2r2_core.work_queue);
+
+	/* Make sure the power is turned off */
+	cancel_delayed_work_sync(&b2r2_core.domain_disable_work);
+
+	/** Unmap B2R2 registers */
+	b2r2_log_info("unmap b2r2 registers..\n");
+	if (b2r2_core.hw) {
+		iounmap(b2r2_core.hw);
+
+		b2r2_core.hw = NULL;
+	}
+
+	destroy_workqueue(b2r2_core.work_queue);
+
+	spin_lock_irqsave(&b2r2_core.lock, flags);
+	b2r2_core.work_queue = NULL;
+	spin_unlock_irqrestore(&b2r2_core.lock, flags);
+
+	/* Return the clock */
+	clk_put(b2r2_core.b2r2_clock);
+	regulator_put(b2r2_core.b2r2_reg);
+
+	b2r2_log_info("%s ended\n", __func__);
+
+	b2r2_core.log_dev = NULL;
+
+	b2r2_debug_exit();
+
+	return 0;
+
+}
+/**
+ * b2r2_suspend() - This routine puts the B2R2 device in to sustend state.
+ * @pdev: platform device.
+ *
+ * This routine stores the current state of the b2r2 device and puts in to suspend state.
+ *
+ */
+int b2r2_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	b2r2_log_info("%s\n", __func__);
+
+	/* Flush B2R2 work queue (call all callbacks) */
+	flush_workqueue(b2r2_core.work_queue);
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+	cancel_delayed_work(&b2r2_core.timeout_work);
+#endif
+
+	/* Flush B2R2 work queue (call all callbacks for
+	   cancelled jobs) */
+	flush_workqueue(b2r2_core.work_queue);
+
+	/* Make sure power is turned off */
+	cancel_delayed_work_sync(&b2r2_core.domain_disable_work);
+
+	return 0;
+}
+
+
+/**
+ * b2r2_resume() - This routine resumes the B2R2 device from sustend state.
+ * @pdev: platform device.
+ *
+ * This routine restore back the current state of the b2r2 device resumes.
+ *
+ */
+int b2r2_resume(struct platform_device *pdev)
+{
+	b2r2_log_info("%s\n", __func__);
+
+	return 0;
+}
+
+/**
+ * struct platform_b2r2_driver - Platform driver configuration for the
+ * B2R2 core driver
+ */
+static struct platform_driver platform_b2r2_driver = {
+	.remove = b2r2_remove,
+	.driver = {
+		.name	= "b2r2",
+	},
+	/** TODO implement power mgmt functions */
+	.suspend = b2r2_suspend,
+	.resume =  b2r2_resume,
+};
+
+
+/**
+ * b2r2_init() - Module init function for the B2R2 core module
+ */
+static int __init b2r2_init(void)
+{
+	printk(KERN_INFO "%s\n", __func__);
+	return platform_driver_probe(&platform_b2r2_driver, b2r2_probe);
+}
+module_init(b2r2_init);
+
+/**
+ * b2r2_exit() - Module exit function for the B2R2 core module
+ */
+static void __exit b2r2_exit(void)
+{
+	printk(KERN_INFO "%s\n", __func__);
+	platform_driver_unregister(&platform_b2r2_driver);
+	return;
+}
+module_exit(b2r2_exit);
+
+
+/** Module is having GPL license */
+
+MODULE_LICENSE("GPL");
+
+/** Module author & discription */
+
+MODULE_AUTHOR("Robert Fekete (robert.fekete@stericsson.com)");
+MODULE_DESCRIPTION("B2R2 Core driver");
diff --git a/drivers/video/b2r2/b2r2_core.h b/drivers/video/b2r2/b2r2_core.h
new file mode 100644
index 00000000000..2f958751694
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_core.h
@@ -0,0 +1,222 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 core driver
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __B2R2_CORE_H__
+#define __B2R2_CORE_H__
+
+#include <linux/types.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+
+/**
+ * enum b2r2_core_queue - Indicates the B2R2 queue that the job belongs to
+ *
+ * @B2R2_CORE_QUEUE_AQ1: Application queue 1
+ * @B2R2_CORE_QUEUE_AQ2: Application queue 2
+ * @B2R2_CORE_QUEUE_AQ3: Application queue 3
+ * @B2R2_CORE_QUEUE_AQ4: Application queue 4
+ * @B2R2_CORE_QUEUE_CQ1: Composition queue 1
+ * @B2R2_CORE_QUEUE_CQ2: Composition queue 2
+ * @B2R2_CORE_QUEUE_NO_OF: Number of queues
+ */
+enum b2r2_core_queue {
+	B2R2_CORE_QUEUE_AQ1 = 0,
+	B2R2_CORE_QUEUE_AQ2,
+	B2R2_CORE_QUEUE_AQ3,
+	B2R2_CORE_QUEUE_AQ4,
+	B2R2_CORE_QUEUE_CQ1,
+	B2R2_CORE_QUEUE_CQ2,
+	B2R2_CORE_QUEUE_NO_OF,
+};
+
+#define B2R2_NUM_APPLICATIONS_QUEUES 4
+
+/**
+ * enum b2r2_core_job_state - Indicates the current state of the job
+ *
+ * @B2R2_CORE_JOB_IDLE: Never queued
+ * @B2R2_CORE_JOB_QUEUED: In queue but not started yet
+ * @B2R2_CORE_JOB_RUNNING: Running, executed by B2R2
+ * @B2R2_CORE_JOB_DONE: Completed
+ * @B2R2_CORE_JOB_CANCELED: Canceled
+ */
+enum b2r2_core_job_state {
+	B2R2_CORE_JOB_IDLE = 0,
+	B2R2_CORE_JOB_QUEUED,
+	B2R2_CORE_JOB_RUNNING,
+	B2R2_CORE_JOB_DONE,
+	B2R2_CORE_JOB_CANCELED,
+};
+
+/**
+ * struct b2r2_core_job - Represents a B2R2 core job
+ *
+ * @start_sentinel: Memory overwrite guard
+ *
+ * @tag: Client value. Used by b2r2_core_job_find_first_with_tag().
+ * @prio: Job priority, from -19 up to 20. Mapped to the
+ *        B2R2 application queues. Filled in by the client.
+ * @first_node_address: Physical address of the first node. Filled
+ *                      in by the client.
+ * @last_node_address: Physical address of the last node. Filled
+ *                     in by the client.
+ *
+ * @callback: Function that will be called when the job is done.
+ * @acquire_resources: Function that allocates the resources needed
+ *                     to execute the job (i.e. SRAM alloc). Must not
+ *                     sleep if atomic, should fail with negative error code
+ *                     if resources not available.
+ * @release_resources: Function that releases the resources previously
+ *                     allocated by acquire_resources (i.e. SRAM alloc).
+ * @release: Function that will be called when the reference count reaches
+ *           zero.
+ *
+ * @job_id: Unique id for this job, assigned by B2R2 core
+ * @job_state: The current state of the job
+ * @jiffies: Number of jiffies needed for this request
+ *
+ * @list: List entry element for internal list management
+ * @event: Wait queue event to wait for job done
+ * @work: Work queue structure, for callback implementation
+ *
+ * @queue: The queue that this job shall be submitted to
+ * @control: B2R2 Queue control
+ * @pace_control: For composition queue only
+ * @interrupt_context: Context for interrupt
+ *
+ * @end_sentinel: Memory overwrite guard
+ */
+struct b2r2_core_job {
+	u32 start_sentinel;
+
+	/* Data to be filled in by client */
+	int tag;
+	int prio;
+	u32 first_node_address;
+	u32 last_node_address;
+	void (*callback)(struct b2r2_core_job *);
+	int (*acquire_resources)(struct b2r2_core_job *,
+		bool atomic);
+	void (*release_resources)(struct b2r2_core_job *,
+		bool atomic);
+	void (*release)(struct b2r2_core_job *);
+
+	/* Output data, do not modify */
+	int  job_id;
+	enum b2r2_core_job_state job_state;
+	unsigned long jiffies;
+
+	/* Data below is internal to b2r2_core, do not modify */
+
+	/* Reference counting */
+	u32 ref_count;
+
+	/* Internal data */
+	struct list_head  list;
+	wait_queue_head_t event;
+	struct work_struct work;
+
+	/* B2R2 HW data */
+	enum b2r2_core_queue queue;
+	u32 control;
+	u32 pace_control;
+	u32 interrupt_context;
+
+	/* Timing data */
+	u32 hw_start_time;
+	s32 nsec_active_in_hw;
+
+	u32 end_sentinel;
+};
+
+/**
+ * b2r2_core_job_add() - Adds a job to B2R2 job queues
+ *
+ * The job reference count will be increased after this function
+ * has been called and b2r2_core_job_release() must be called to
+ * release the reference. The job callback function will be always
+ * be called after the job is done or cancelled.
+ *
+ * @job: Job to be added
+ *
+ * Returns 0 if OK else negative error code
+ *
+ */
+int b2r2_core_job_add(struct b2r2_core_job *job);
+
+/**
+ * b2r2_core_job_wait() - Waits for an added job to be done.
+ *
+ * @job: Job to wait for
+ *
+ * Returns 0 if job done else negative error code
+ *
+ */
+int b2r2_core_job_wait(struct b2r2_core_job *job);
+
+/**
+ * b2r2_core_job_cancel() - Cancel an already added job.
+ *
+ * @job: Job to cancel
+ *
+ * Returns 0 if job cancelled or done else negative error code
+ *
+ */
+int b2r2_core_job_cancel(struct b2r2_core_job *job);
+
+/**
+ * b2r2_core_job_find() - Finds job with given job id
+ *
+ * Reference count will be increased for the found job
+ *
+ * @job_id: Job id to find
+ *
+ * Returns job if found, else NULL
+ *
+ */
+struct b2r2_core_job *b2r2_core_job_find(int job_id);
+
+/**
+ * b2r2_core_job_find_first_with_tag() - Finds first job with given tag
+ *
+ * Reference count will be increased for the found job.
+ * This function can be used to find all jobs for a client, i.e.
+ * when cancelling all jobs for a client.
+ *
+ * @tag: Tag to find
+ *
+ * Returns job if found, else NULL
+ *
+ */
+struct b2r2_core_job *b2r2_core_job_find_first_with_tag(int tag);
+
+/**
+ * b2r2_core_job_addref() - Increase the job reference count.
+ *
+ * @job: Job to increase reference count for.
+ * @caller: The function calling this function (for debug)
+ */
+void b2r2_core_job_addref(struct b2r2_core_job *job, const char *caller);
+
+/**
+ * b2r2_core_job_release() - Decrease the job reference count. The
+ *                           job will be released (the release() function
+ *                           will be called) when the reference count
+ *                           reaches zero.
+ *
+ * @job: Job to decrease reference count for.
+ * @caller: The function calling this function (for debug)
+ */
+void b2r2_core_job_release(struct b2r2_core_job *job, const char *caller);
+
+#endif /* !defined(__B2R2_CORE_JOB_H__) */
diff --git a/drivers/video/b2r2/b2r2_debug.c b/drivers/video/b2r2/b2r2_debug.c
new file mode 100644
index 00000000000..d4711cd3e28
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_debug.c
@@ -0,0 +1,343 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 dynamic debug
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include "b2r2_debug.h"
+#include <linux/debugfs.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+int b2r2_log_levels[B2R2_LOG_LEVEL_COUNT];
+struct device *b2r2_log_dev;
+
+static struct dentry *root_dir;
+static struct dentry *log_lvl_dir;
+static struct dentry *stats_dir;
+
+#define CHARS_IN_NODE_DUMP 1544
+
+static const size_t dumped_node_size = CHARS_IN_NODE_DUMP * sizeof(char) + 1;
+
+static void dump_node(char *dst, struct b2r2_node *node)
+{
+	dst += sprintf(dst, "node 0x%08x ------------------\n",
+			(unsigned int)node);
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_NIP:", node->node.GROUP0.B2R2_NIP);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_CIC:", node->node.GROUP0.B2R2_CIC);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_INS:", node->node.GROUP0.B2R2_INS);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_ACK:", node->node.GROUP0.B2R2_ACK);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_TBA:", node->node.GROUP1.B2R2_TBA);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_TTY:", node->node.GROUP1.B2R2_TTY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_TXY:", node->node.GROUP1.B2R2_TXY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_TSZ:", node->node.GROUP1.B2R2_TSZ);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S1CF:", node->node.GROUP2.B2R2_S1CF);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S2CF:", node->node.GROUP2.B2R2_S2CF);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S1BA:", node->node.GROUP3.B2R2_SBA);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S1TY:", node->node.GROUP3.B2R2_STY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S1XY:", node->node.GROUP3.B2R2_SXY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S1SZ:", node->node.GROUP3.B2R2_SSZ);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S2BA:", node->node.GROUP4.B2R2_SBA);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S2TY:", node->node.GROUP4.B2R2_STY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S2XY:", node->node.GROUP4.B2R2_SXY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S2SZ:", node->node.GROUP4.B2R2_SSZ);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S3BA:", node->node.GROUP5.B2R2_SBA);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S3TY:", node->node.GROUP5.B2R2_STY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S3XY:", node->node.GROUP5.B2R2_SXY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S3SZ:", node->node.GROUP5.B2R2_SSZ);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_CWO:", node->node.GROUP6.B2R2_CWO);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_CWS:", node->node.GROUP6.B2R2_CWS);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_CCO:", node->node.GROUP7.B2R2_CCO);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_CML:", node->node.GROUP7.B2R2_CML);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_PMK:", node->node.GROUP8.B2R2_PMK);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_FCTL:", node->node.GROUP8.B2R2_FCTL);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_RSF:", node->node.GROUP9.B2R2_RSF);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_RZI:", node->node.GROUP9.B2R2_RZI);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_HFP:", node->node.GROUP9.B2R2_HFP);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_VFP:", node->node.GROUP9.B2R2_VFP);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_Y_RSF:", node->node.GROUP10.B2R2_RSF);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_Y_RZI:", node->node.GROUP10.B2R2_RZI);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_Y_HFP:", node->node.GROUP10.B2R2_HFP);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_Y_VFP:", node->node.GROUP10.B2R2_VFP);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_FF0:", node->node.GROUP11.B2R2_FF0);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_FF1:", node->node.GROUP11.B2R2_FF1);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_FF2:", node->node.GROUP11.B2R2_FF2);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_FF3:", node->node.GROUP11.B2R2_FF3);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_KEY1:", node->node.GROUP12.B2R2_KEY1);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_KEY2:", node->node.GROUP12.B2R2_KEY2);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_XYL:", node->node.GROUP13.B2R2_XYL);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_XYP:", node->node.GROUP13.B2R2_XYP);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_SAR:", node->node.GROUP14.B2R2_SAR);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_USR:", node->node.GROUP14.B2R2_USR);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_IVMX0:", node->node.GROUP15.B2R2_VMX0);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_IVMX1:", node->node.GROUP15.B2R2_VMX1);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_IVMX2:", node->node.GROUP15.B2R2_VMX2);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_IVMX3:", node->node.GROUP15.B2R2_VMX3);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_OVMX0:", node->node.GROUP16.B2R2_VMX0);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_OVMX1:", node->node.GROUP16.B2R2_VMX1);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_OVMX2:", node->node.GROUP16.B2R2_VMX2);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_OVMX3:", node->node.GROUP16.B2R2_VMX3);
+	dst += sprintf(dst, "--\n");
+
+}
+
+struct mutex last_job_lock;
+
+static struct b2r2_node *last_job;
+
+void b2r2_debug_job_done(struct b2r2_node *first_node)
+{
+	struct b2r2_node *node = first_node;
+	struct b2r2_node **dst_node;
+	unsigned int node_count = 0;
+
+	while (node != NULL) {
+		node_count++;
+		node = node->next;
+	}
+
+	mutex_lock(&last_job_lock);
+
+	if (last_job) {
+		node = last_job;
+		while (node != NULL) {
+			struct b2r2_node *tmp = node->next;
+			kfree(node);
+			node = tmp;
+		}
+		last_job = NULL;
+	}
+
+	node = first_node;
+	dst_node = &last_job;
+	while (node != NULL) {
+		*dst_node = kzalloc(sizeof(**dst_node), GFP_KERNEL);
+		if (!(*dst_node))
+			goto last_job_alloc_failed;
+
+		memcpy(*dst_node, node, sizeof(**dst_node));
+
+		dst_node = &((*dst_node)->next);
+		node = node->next;
+	}
+
+	mutex_unlock(&last_job_lock);
+
+	return;
+
+last_job_alloc_failed:
+	mutex_unlock(&last_job_lock);
+
+	while (last_job != NULL) {
+		struct b2r2_node *tmp = last_job->next;
+		kfree(last_job);
+		last_job = tmp;
+	}
+
+	return;
+}
+
+static char *last_job_chars;
+static int prev_node_count;
+
+static ssize_t last_job_read(struct file *filep, char __user *buf,
+		size_t bytes, loff_t *off)
+{
+	struct b2r2_node *node = last_job;
+	int node_count = 0;
+	int i;
+
+	size_t size;
+	size_t count;
+	loff_t offs = *off;
+
+	for (; node != NULL; node = node->next)
+		node_count++;
+
+	size = node_count * dumped_node_size;
+
+	if (node_count != prev_node_count) {
+		kfree(last_job_chars);
+
+		last_job_chars = kzalloc(size, GFP_KERNEL);
+		if (!last_job_chars)
+			return 0;
+		prev_node_count = node_count;
+	}
+
+	mutex_lock(&last_job_lock);
+	node = last_job;
+	for (i = 0; i < node_count; i++) {
+		BUG_ON(node == NULL);
+		dump_node(last_job_chars + i * dumped_node_size/sizeof(char),
+				node);
+		node = node->next;
+	}
+	mutex_unlock(&last_job_lock);
+
+	if (offs > size)
+		return 0;
+
+	if (offs + bytes > size)
+		count = size - offs;
+	else
+		count = bytes;
+
+	if (copy_to_user(buf, last_job_chars + offs, count))
+		return -EFAULT;
+
+	*off = offs + count;
+	return count;
+}
+
+static const struct file_operations last_job_fops = {
+	.read = last_job_read,
+};
+
+int b2r2_debug_init(struct device *log_dev)
+{
+	int i;
+
+	b2r2_log_dev = log_dev;
+
+	for (i = 0; i < B2R2_LOG_LEVEL_COUNT; i++)
+		b2r2_log_levels[i] = 0;
+
+	root_dir = debugfs_create_dir("b2r2_debug", NULL);
+	if (!root_dir) {
+		b2r2_log_warn("%s: could not create root dir\n", __func__);
+		return -ENODEV;
+	}
+
+#if !defined(CONFIG_DYNAMIC_DEBUG) && defined(CONFIG_DEBUG_FS)
+	/*
+	 * If dynamic debug is disabled we need some other way to control the
+	 * log prints
+	 */
+	log_lvl_dir = debugfs_create_dir("logs", root_dir);
+
+	/* No need to save the files, they will be removed recursively */
+	(void)debugfs_create_bool("warnings", 0644, log_lvl_dir,
+				&b2r2_log_levels[B2R2_LOG_LEVEL_WARN]);
+	(void)debugfs_create_bool("info", 0644, log_lvl_dir,
+				&b2r2_log_levels[B2R2_LOG_LEVEL_INFO]);
+	(void)debugfs_create_bool("debug", 0644, log_lvl_dir,
+				&b2r2_log_levels[B2R2_LOG_LEVEL_DEBUG]);
+	(void)debugfs_create_bool("regdumps", 0644, log_lvl_dir,
+				&b2r2_log_levels[B2R2_LOG_LEVEL_REGDUMP]);
+
+#elif defined(CONFIG_DYNAMIC_DEBUG)
+	/* log_lvl_dir is never used */
+	(void)log_lvl_dir;
+#endif
+
+	stats_dir = debugfs_create_dir("stats", root_dir);
+	(void)debugfs_create_file("last_job", 0444, stats_dir, NULL,
+			&last_job_fops);
+
+	mutex_init(&last_job_lock);
+
+	return 0;
+}
+
+void b2r2_debug_exit(void)
+{
+#if !defined(CONFIG_DYNAMIC_DEBUG) && defined(CONFIG_DEBUG_FS)
+	if (root_dir)
+		debugfs_remove_recursive(root_dir);
+#endif
+}
diff --git a/drivers/video/b2r2/b2r2_debug.h b/drivers/video/b2r2/b2r2_debug.h
new file mode 100644
index 00000000000..f87ca728482
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_debug.h
@@ -0,0 +1,104 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 dynamic debug
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_DRIVERS_VIDEO_B2R2_DEBUG_H_
+#define _LINUX_DRIVERS_VIDEO_B2R2_DEBUG_H_
+
+#include <linux/device.h>
+
+#include "b2r2_internal.h"
+
+#ifdef CONFIG_B2R2_DEBUG
+
+/* Log macros */
+enum b2r2_log_levels {
+	B2R2_LOG_LEVEL_WARN,
+	B2R2_LOG_LEVEL_INFO,
+	B2R2_LOG_LEVEL_DEBUG,
+	B2R2_LOG_LEVEL_REGDUMP,
+	B2R2_LOG_LEVEL_COUNT,
+};
+
+/*
+ * Booleans controlling the different log levels. The different log levels are
+ * enabled separately (i.e. you can have info prints without the warn prints).
+ */
+extern int b2r2_log_levels[B2R2_LOG_LEVEL_COUNT];
+
+extern struct device *b2r2_log_dev;
+
+#define b2r2_log_err(...) do { \
+		dev_err(b2r2_log_dev, __VA_ARGS__); \
+	} while (0)
+
+/* If dynamic debug is enabled it should be used instead of loglevels */
+#ifdef CONFIG_DYNAMIC_DEBUG
+#  define b2r2_log_warn(...) do { \
+		dev_dbg(b2r2_log_dev, "WARN " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_info(...) do { \
+		dev_dbg(b2r2_log_dev, "INFO " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_debug(...) do { \
+		dev_dbg(b2r2_log_dev, "DEBUG " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_regdump(...) do { \
+		dev_dbg(b2r2_log_dev, "REGD " __VA_ARGS__); \
+	} while (0)
+#else
+#  define b2r2_log_warn(...) do { \
+		if (b2r2_log_levels[B2R2_LOG_LEVEL_WARN]) \
+			dev_warn(b2r2_log_dev, "WARN " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_info(...) do { \
+		if (b2r2_log_levels[B2R2_LOG_LEVEL_INFO]) \
+			dev_info(b2r2_log_dev, "INFO " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_debug(...) do { \
+		if (b2r2_log_levels[B2R2_LOG_LEVEL_DEBUG]) \
+			dev_dbg(b2r2_log_dev, "DEBUG " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_regdump(...) do { \
+		if (b2r2_log_levels[B2R2_LOG_LEVEL_REGDUMP]) \
+			dev_vdbg(b2r2_log_dev, "REGD " __VA_ARGS__); \
+	} while (0)
+#endif
+
+
+int b2r2_debug_init(struct device *log_dev);
+void b2r2_debug_exit(void);
+
+void b2r2_debug_job_done(struct b2r2_node *node);
+
+#else
+
+#define b2r2_log_err(...)
+#define b2r2_log_warn(...)
+#define b2r2_log_info(...)
+#define b2r2_log_debug(...)
+#define b2r2_log_regdump(...)
+
+static inline int b2r2_debug_init(struct device *log_dev)
+{
+	return 0;
+}
+static inline void b2r2_debug_exit(void)
+{
+	return;
+}
+static inline void b2r2_debug_job_done(struct b2r2_node *node)
+{
+	return;
+}
+
+#endif
+
+#endif
diff --git a/drivers/video/b2r2/b2r2_filters.c b/drivers/video/b2r2/b2r2_filters.c
new file mode 100644
index 00000000000..208cdcd286e
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_filters.c
@@ -0,0 +1,372 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 filters.
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/dma-mapping.h>
+
+#include "b2r2_filters.h"
+#include "b2r2_internal.h"
+
+/**
+ * struct b2r2_filter_spec filters[] - Filter lookup table
+ *
+ * Lookup table for filters for different scale factors. A filter
+ * will be selected according to "min < scale_factor <= max".
+ */
+static struct b2r2_filter_spec filters[] = {
+	{
+		.min = 1024,
+		.max = 1433,
+		.h_coeffs =  {
+			0xfc, 0x06, 0xf9, 0x09, 0x34, 0x09, 0xf9, 0x06,
+			0xfd, 0x07, 0xf7, 0x10, 0x32, 0x02, 0xfc, 0x05,
+			0xfe, 0x07, 0xf6, 0x17, 0x2f, 0xfc, 0xff, 0x04,
+			0xff, 0x06, 0xf5, 0x20, 0x2a, 0xf9, 0x01, 0x02,
+			0x00, 0x04, 0xf6, 0x27, 0x25, 0xf6, 0x04, 0x00,
+			0x02, 0x01, 0xf9, 0x2d, 0x1d, 0xf5, 0x06, 0xff,
+			0x04, 0xff, 0xfd, 0x31, 0x15, 0xf5, 0x07, 0xfe,
+			0x05, 0xfc, 0x02, 0x35, 0x0d, 0xf7, 0x07, 0xfd
+		},
+		.v_coeffs = {
+			0xf8, 0x0a, 0x3c, 0x0a, 0xf8,
+			0xf6, 0x12, 0x3b, 0x02, 0xfb,
+			0xf4, 0x1b, 0x35, 0xfd, 0xff,
+			0xf4, 0x23, 0x30, 0xf8, 0x01,
+			0xf6, 0x29, 0x27, 0xf6, 0x04,
+			0xf9, 0x2e, 0x1e, 0xf5, 0x06,
+			0xfd, 0x31, 0x16, 0xf6, 0x06,
+			0x02, 0x32, 0x0d, 0xf8, 0x07
+		},
+	},
+	{
+		.min = 1433,
+		.max = 1536,
+		.h_coeffs =  {
+			0xfe, 0x06, 0xf8, 0x0b, 0x30, 0x0b, 0xf8, 0x06,
+			0xff, 0x06, 0xf7, 0x12, 0x2d, 0x05, 0xfa, 0x06,
+			0x00, 0x04, 0xf6, 0x18, 0x2c, 0x00, 0xfc, 0x06,
+			0x01, 0x02, 0xf7, 0x1f, 0x27, 0xfd, 0xff, 0x04,
+			0x03, 0x00, 0xf9, 0x24, 0x24, 0xf9, 0x00, 0x03,
+			0x04, 0xff, 0xfd, 0x29, 0x1d, 0xf7, 0x02, 0x01,
+			0x06, 0xfc, 0x00, 0x2d, 0x17, 0xf6, 0x04, 0x00,
+			0x06, 0xfa, 0x05, 0x30, 0x0f, 0xf7, 0x06, 0xff
+		},
+		.v_coeffs = {
+			0xf6, 0x0e, 0x38, 0x0e, 0xf6,
+			0xf5, 0x15, 0x38, 0x06, 0xf8,
+			0xf5, 0x1d, 0x33, 0x00, 0xfb,
+			0xf6, 0x23, 0x2d, 0xfc, 0xfe,
+			0xf9, 0x28, 0x26, 0xf9, 0x00,
+			0xfc, 0x2c, 0x1e, 0xf7, 0x03,
+			0x00, 0x2e, 0x18, 0xf6, 0x04,
+			0x05, 0x2e, 0x11, 0xf7, 0x05
+		},
+	},
+	{
+		.min = 1536,
+		.max = 3072,
+		.h_coeffs =  {
+			0xfc, 0xfd, 0x06, 0x13, 0x18, 0x13, 0x06, 0xfd,
+			0xfc, 0xfe, 0x08, 0x15, 0x17, 0x12, 0x04, 0xfc,
+			0xfb, 0xfe, 0x0a, 0x16, 0x18, 0x10, 0x03, 0xfc,
+			0xfb, 0x00, 0x0b, 0x18, 0x17, 0x0f, 0x01, 0xfb,
+			0xfb, 0x00, 0x0d, 0x19, 0x17, 0x0d, 0x00, 0xfb,
+			0xfb, 0x01, 0x0f, 0x19, 0x16, 0x0b, 0x00, 0xfb,
+			0xfc, 0x03, 0x11, 0x19, 0x15, 0x09, 0xfe, 0xfb,
+			0xfc, 0x04, 0x12, 0x1a, 0x12, 0x08, 0xfe, 0xfc
+		},
+		.v_coeffs = {
+			0x05, 0x10, 0x16, 0x10, 0x05,
+			0x06, 0x11, 0x16, 0x0f, 0x04,
+			0x08, 0x13, 0x15, 0x0e, 0x02,
+			0x09, 0x14, 0x16, 0x0c, 0x01,
+			0x0b, 0x15, 0x15, 0x0b, 0x00,
+			0x0d, 0x16, 0x13, 0x0a, 0x00,
+			0x0f, 0x17, 0x13, 0x08, 0xff,
+			0x11, 0x18, 0x12, 0x07, 0xfe
+		},
+	},
+	{
+		.min = 3072,
+		.max = 4096,
+		.h_coeffs =  {
+			0xfe, 0x02, 0x09, 0x0f, 0x0e, 0x0f, 0x09, 0x02,
+			0xff, 0x02, 0x09, 0x0f, 0x10, 0x0e, 0x08, 0x01,
+			0xff, 0x03, 0x0a, 0x10, 0x10, 0x0d, 0x07, 0x00,
+			0x00, 0x04, 0x0b, 0x10, 0x0f, 0x0c, 0x06, 0x00,
+			0x00, 0x05, 0x0c, 0x10, 0x0e, 0x0c, 0x05, 0x00,
+			0x00, 0x06, 0x0c, 0x11, 0x0e, 0x0b, 0x04, 0x00,
+			0x00, 0x07, 0x0d, 0x11, 0x0f, 0x0a, 0x03, 0xff,
+			0x01, 0x08, 0x0e, 0x11, 0x0e, 0x09, 0x02, 0xff
+		},
+		.v_coeffs = {
+			0x09, 0x0f, 0x10, 0x0f, 0x09,
+			0x09, 0x0f, 0x12, 0x0e, 0x08,
+			0x0a, 0x10, 0x11, 0x0e, 0x07,
+			0x0b, 0x11, 0x11, 0x0d, 0x06,
+			0x0c, 0x11, 0x12, 0x0c, 0x05,
+			0x0d, 0x12, 0x11, 0x0c, 0x04,
+			0x0e, 0x12, 0x11, 0x0b, 0x04,
+			0x0f, 0x13, 0x11, 0x0a, 0x03
+		},
+	},
+	{
+		.min = 4096,
+		.max = 5120,
+		.h_coeffs =  {
+			0x00, 0x04, 0x09, 0x0c, 0x0e, 0x0c, 0x09, 0x04,
+			0x01, 0x05, 0x09, 0x0c, 0x0d, 0x0c, 0x08, 0x04,
+			0x01, 0x05, 0x0a, 0x0c, 0x0e, 0x0b, 0x08, 0x03,
+			0x02, 0x06, 0x0a, 0x0d, 0x0c, 0x0b, 0x07, 0x03,
+			0x02, 0x07, 0x0a, 0x0d, 0x0d, 0x0a, 0x07, 0x02,
+			0x03, 0x07, 0x0b, 0x0d, 0x0c, 0x0a, 0x06, 0x02,
+			0x03, 0x08, 0x0b, 0x0d, 0x0d, 0x0a, 0x05, 0x01,
+			0x04, 0x08, 0x0c, 0x0d, 0x0c, 0x09, 0x05, 0x01
+		},
+		.v_coeffs = {
+			0x0a, 0x0e, 0x10, 0x0e, 0x0a,
+			0x0b, 0x0e, 0x0f, 0x0e, 0x0a,
+			0x0b, 0x0f, 0x10, 0x0d, 0x09,
+			0x0c, 0x0f, 0x10, 0x0d, 0x08,
+			0x0d, 0x0f, 0x0f, 0x0d, 0x08,
+			0x0d, 0x10, 0x10, 0x0c, 0x07,
+			0x0e, 0x10, 0x0f, 0x0c, 0x07,
+			0x0f, 0x10, 0x10, 0x0b, 0x06
+		},
+	},
+};
+static const size_t filters_size = sizeof(filters)/sizeof(filters[0]);
+
+/**
+ * struct b2r2_filter_spec bilinear_filter - A bilinear filter
+ *
+ * The bilinear filter will be used if no custom filters are specified, or
+ * for upscales not matching any filter in the lookup table.
+ */
+static struct b2r2_filter_spec bilinear_filter = {
+	.min = 0,
+	.max = 0xffff,
+	.h_coeffs = {
+		0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+		0xff, 0x03, 0xfd, 0x08, 0x3e, 0xf9, 0x04, 0xfe,
+		0xfd, 0x06, 0xf8, 0x13, 0x3b, 0xf4, 0x07, 0xfc,
+		0xfb, 0x08, 0xf5, 0x1f, 0x34, 0xf1, 0x09, 0xfb,
+		0xfb, 0x09, 0xf2, 0x2b, 0x2a, 0xf1, 0x09, 0xfb,
+		0xfb, 0x09, 0xf2, 0x35, 0x1e, 0xf4, 0x08, 0xfb,
+		0xfc, 0x07, 0xf5, 0x3c, 0x12, 0xf7, 0x06, 0xfd,
+		0xfe, 0x04, 0xfa, 0x3f, 0x07, 0xfc, 0x03, 0xff
+	},
+	.v_coeffs = {
+		0x00, 0x00, 0x40, 0x00, 0x00,
+		0xfd, 0x09, 0x3c, 0xfa, 0x04,
+		0xf9, 0x13, 0x39, 0xf5, 0x06,
+		0xf5, 0x1f, 0x31, 0xf3, 0x08,
+		0xf3, 0x2a, 0x28, 0xf3, 0x08,
+		0xf3, 0x34, 0x1d, 0xf5, 0x07,
+		0xf5, 0x3b, 0x12, 0xf9, 0x05,
+		0xfa, 0x3f, 0x07, 0xfd, 0x03
+	},
+};
+
+/**
+ * struct b2r2_filter_spec default_downscale_filter - Default filter for downscale
+ *
+ * The default downscale filter will be used for downscales not matching any
+ * filter in the lookup table.
+ */
+static struct b2r2_filter_spec default_downscale_filter = {
+	.min = 1 << 10,
+	.max = 0xffff,
+	.h_coeffs = {
+		0x03, 0x06, 0x09, 0x0b, 0x09, 0x0b, 0x09, 0x06,
+		0x03, 0x06, 0x09, 0x0b, 0x0c, 0x0a, 0x08, 0x05,
+		0x03, 0x06, 0x09, 0x0b, 0x0c, 0x0a, 0x08, 0x05,
+		0x04, 0x07, 0x09, 0x0b, 0x0b, 0x0a, 0x08, 0x04,
+		0x04, 0x07, 0x0a, 0x0b, 0x0b, 0x0a, 0x07, 0x04,
+		0x04, 0x08, 0x0a, 0x0b, 0x0b, 0x09, 0x07, 0x04,
+		0x05, 0x08, 0x0a, 0x0b, 0x0c, 0x09, 0x06, 0x03,
+		0x05, 0x08, 0x0a, 0x0b, 0x0c, 0x09, 0x06, 0x03
+	},
+	.v_coeffs = {
+		0x0b, 0x0e, 0x0e, 0x0e, 0x0b,
+		0x0b, 0x0e, 0x0f, 0x0d, 0x0b,
+		0x0c, 0x0e, 0x0f, 0x0d, 0x0a,
+		0x0c, 0x0e, 0x0f, 0x0d, 0x0a,
+		0x0d, 0x0f, 0x0e, 0x0d, 0x09,
+		0x0d, 0x0f, 0x0f, 0x0c, 0x09,
+		0x0e, 0x0f, 0x0e, 0x0c, 0x09,
+		0x0e, 0x0f, 0x0f, 0x0c, 0x08
+	},
+};
+
+/**
+ * struct b2r2_filter_spec blur_filter - Blur filter
+ *
+ * Filter for blurring an image.
+ */
+static struct b2r2_filter_spec blur_filter = {
+	.min = 0,
+	.max = 0xffff,
+	.h_coeffs = {
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08
+	},
+	.v_coeffs = {
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c
+	},
+};
+
+/* Private function declarations */
+static int alloc_filter_coeffs(struct b2r2_filter_spec *filter);
+static void free_filter_coeffs(struct b2r2_filter_spec *filter);
+
+/* Public functions */
+
+static int filters_initialized;
+
+int b2r2_filters_init()
+{
+	int i;
+
+	if (filters_initialized)
+		return 0;
+
+	for (i = 0; i < filters_size; i++) {
+		alloc_filter_coeffs(&filters[i]);
+	}
+
+	alloc_filter_coeffs(&bilinear_filter);
+	alloc_filter_coeffs(&default_downscale_filter);
+	alloc_filter_coeffs(&blur_filter);
+
+	filters_initialized = 1;
+
+	return 0;
+}
+
+void b2r2_filters_exit()
+{
+	int i;
+
+	if (!filters_initialized)
+		return;
+
+	for (i = 0; i < filters_size; i++) {
+		free_filter_coeffs(&filters[i]);
+	}
+
+	free_filter_coeffs(&bilinear_filter);
+	free_filter_coeffs(&default_downscale_filter);
+	free_filter_coeffs(&blur_filter);
+
+	filters_initialized = 0;
+}
+
+struct b2r2_filter_spec *b2r2_filter_find(u16 scale_factor)
+{
+	int i;
+	struct b2r2_filter_spec *filter = NULL;
+
+	for (i = 0; i < filters_size; i++) {
+		if ((filters[i].min < scale_factor) &&
+				(scale_factor <= filters[i].max) &&
+				filters[i].h_coeffs_dma_addr &&
+				filters[i].v_coeffs_dma_addr) {
+			filter = &filters[i];
+			break;
+		}
+	}
+
+	if (filter == NULL) {
+		/*
+		 * No suitable filter has been found. Use default filters,
+		 * bilinear for any upscale.
+		 */
+		if (scale_factor < (1 << 10))
+			filter = &bilinear_filter;
+		else
+			filter = &default_downscale_filter;
+	}
+
+	/*
+	 * Check so that the coefficients were successfully allocated for this
+	 * filter.
+	 */
+	if (!filter->h_coeffs_dma_addr || !filter->v_coeffs_dma_addr)
+		return NULL;
+	else
+		return filter;
+}
+
+struct b2r2_filter_spec *b2r2_filter_blur()
+{
+	return &blur_filter;
+}
+
+/* Private functions */
+static int alloc_filter_coeffs(struct b2r2_filter_spec *filter)
+{
+	int ret;
+
+	filter->h_coeffs_dma_addr = dma_alloc_coherent(b2r2_blt_device(),
+			B2R2_HF_TABLE_SIZE, &(filter->h_coeffs_phys_addr),
+			GFP_DMA | GFP_KERNEL);
+	if (filter->h_coeffs_dma_addr == NULL) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	filter->v_coeffs_dma_addr = dma_alloc_coherent(b2r2_blt_device(),
+			B2R2_VF_TABLE_SIZE, &(filter->v_coeffs_phys_addr),
+			GFP_DMA | GFP_KERNEL);
+	if (filter->v_coeffs_dma_addr == NULL) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	memcpy(filter->h_coeffs_dma_addr, filter->h_coeffs, B2R2_HF_TABLE_SIZE);
+	memcpy(filter->v_coeffs_dma_addr, filter->v_coeffs, B2R2_VF_TABLE_SIZE);
+
+	return 0;
+
+error:
+	free_filter_coeffs(filter);
+	return ret;
+
+}
+
+static void free_filter_coeffs(struct b2r2_filter_spec *filter)
+{
+	if (filter->h_coeffs_dma_addr != NULL)
+		dma_free_coherent(b2r2_blt_device(), B2R2_HF_TABLE_SIZE,
+				filter->h_coeffs_dma_addr,
+				filter->h_coeffs_phys_addr);
+	if (filter->v_coeffs_dma_addr != NULL)
+		dma_free_coherent(b2r2_blt_device(), B2R2_VF_TABLE_SIZE,
+				filter->v_coeffs_dma_addr,
+				filter->v_coeffs_phys_addr);
+
+	filter->h_coeffs_dma_addr = NULL;
+	filter->h_coeffs_phys_addr = 0;
+	filter->v_coeffs_dma_addr = NULL;
+	filter->v_coeffs_phys_addr = 0;
+}
diff --git a/drivers/video/b2r2/b2r2_filters.h b/drivers/video/b2r2/b2r2_filters.h
new file mode 100644
index 00000000000..0eeefc6b0e0
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_filters.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 filters.
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_VIDEO_B2R2_FILTERS_H
+#define _LINUX_VIDEO_B2R2_FILTERS_H
+
+#include <linux/kernel.h>
+
+#define B2R2_HF_TABLE_SIZE 64
+#define B2R2_VF_TABLE_SIZE 40
+
+/**
+ * @struct b2r2_filter_spec - Filter specification structure
+ *
+ *   @param min - Minimum scale factor for this filter (in 6.10 fixed point)
+ *   @param max - Maximum scale factor for this filter (in 6.10 fixed point)
+ *   @param h_coeffs - Horizontal filter coefficients
+ *   @param v_coeffs - Vertical filter coefficients
+ *   @param h_coeffs_dma_addr - Virtual DMA address for horizontal coefficients
+ *   @param v_coeffs_dma_addr - Virtual DMA address for vertical coefficients
+ *   @param h_coeffs_phys_addr - Physical address for horizontal coefficients
+ *   @param v_coeffs_phys_addr - Physical address for vertical coefficients
+ */
+struct b2r2_filter_spec {
+	const u16 min;
+	const u16 max;
+
+	const u8 h_coeffs[B2R2_HF_TABLE_SIZE];
+	const u8 v_coeffs[B2R2_VF_TABLE_SIZE];
+
+	void *h_coeffs_dma_addr;
+	u32 h_coeffs_phys_addr;
+
+	void *v_coeffs_dma_addr;
+	u32 v_coeffs_phys_addr;
+};
+
+/**
+ * b2r2_filters_init() - Initilizes the B2R2 filters
+ */
+int b2r2_filters_init(void);
+
+/**
+ * b2r2_filters_init() - De-initilizes the B2R2 filters
+ */
+void b2r2_filters_exit(void);
+
+/**
+ * b2r2_filter_find() - Find a filter matching the given scale factor
+ *
+ *   @param scale_factor - Scale factor to find a filter for
+ *
+ * Returns NULL if no filter could be found.
+ */
+struct b2r2_filter_spec *b2r2_filter_find(u16 scale_factor);
+
+/**
+ * b2r2_filter_blur() - Returns the blur filter
+ *
+ * Returns NULL if no blur filter is available.
+ */
+struct b2r2_filter_spec *b2r2_filter_blur(void);
+
+#endif /* _LINUX_VIDEO_B2R2_FILTERS_H */
diff --git a/drivers/video/b2r2/b2r2_generic.c b/drivers/video/b2r2/b2r2_generic.c
new file mode 100644
index 00000000000..738e11bf4ba
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_generic.c
@@ -0,0 +1,3224 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 generic. Full coverage of user interface but
+ * non optimized implementation. For Fallback purposes.
+ *
+ * Author: Maciej Socha <maciej.socha@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/debugfs.h>
+
+#include "b2r2_generic.h"
+#include "b2r2_internal.h"
+#include "b2r2_global.h"
+#include "b2r2_debug.h"
+#include "b2r2_filters.h"
+
+/*
+ * Debug printing
+ */
+#define B2R2_GENERIC_DEBUG_AREAS 0
+#define B2R2_GENERIC_DEBUG
+
+#define B2R2_GENERIC_WORK_BUF_WIDTH 16
+#define B2R2_GENERIC_WORK_BUF_HEIGHT 16
+#define B2R2_GENERIC_WORK_BUF_PITCH (16 * 4)
+#define B2R2_GENERIC_WORK_BUF_FMT B2R2_NATIVE_ARGB8888
+
+/*
+ * Private functions
+ */
+
+/**
+ * reset_nodes() - clears the node list
+ */
+static void reset_nodes(struct b2r2_node *node)
+{
+	b2r2_log_info("%s ENTRY\n", __func__);
+
+	while (node != NULL) {
+		memset(&(node->node), 0, sizeof(node->node));
+
+		/* TODO: Implement support for short linked lists */
+		node->node.GROUP0.B2R2_CIC = 0x7fffc;
+
+		if (node->next == NULL)
+			break;
+
+		node->node.GROUP0.B2R2_NIP = node->next->physical_address;
+
+		node = node->next;
+	}
+	b2r2_log_info("%s DONE\n", __func__);
+}
+
+/**
+ * dump_nodes() - prints the node list
+ */
+static void dump_nodes(struct b2r2_node *first, bool dump_all)
+{
+	struct b2r2_node *node = first;
+	b2r2_log_info("%s ENTRY\n", __func__);
+	do {
+		b2r2_log_debug("\nNODE START:\n=============\n");
+		b2r2_log_debug("B2R2_ACK: \t0x%.8x\n",
+				node->node.GROUP0.B2R2_ACK);
+		b2r2_log_debug("B2R2_INS: \t0x%.8x\n",
+				node->node.GROUP0.B2R2_INS);
+		b2r2_log_debug("B2R2_CIC: \t0x%.8x\n",
+				node->node.GROUP0.B2R2_CIC);
+		b2r2_log_debug("B2R2_NIP: \t0x%.8x\n",
+				node->node.GROUP0.B2R2_NIP);
+
+		b2r2_log_debug("B2R2_TSZ: \t0x%.8x\n",
+				node->node.GROUP1.B2R2_TSZ);
+		b2r2_log_debug("B2R2_TXY: \t0x%.8x\n",
+				node->node.GROUP1.B2R2_TXY);
+		b2r2_log_debug("B2R2_TTY: \t0x%.8x\n",
+				node->node.GROUP1.B2R2_TTY);
+		b2r2_log_debug("B2R2_TBA: \t0x%.8x\n",
+				node->node.GROUP1.B2R2_TBA);
+
+		b2r2_log_debug("B2R2_S2CF: \t0x%.8x\n",
+				node->node.GROUP2.B2R2_S2CF);
+		b2r2_log_debug("B2R2_S1CF: \t0x%.8x\n",
+				node->node.GROUP2.B2R2_S1CF);
+
+		b2r2_log_debug("B2R2_S1SZ: \t0x%.8x\n",
+				node->node.GROUP3.B2R2_SSZ);
+		b2r2_log_debug("B2R2_S1XY: \t0x%.8x\n",
+				node->node.GROUP3.B2R2_SXY);
+		b2r2_log_debug("B2R2_S1TY: \t0x%.8x\n",
+				node->node.GROUP3.B2R2_STY);
+		b2r2_log_debug("B2R2_S1BA: \t0x%.8x\n",
+				node->node.GROUP3.B2R2_SBA);
+
+		b2r2_log_debug("B2R2_S2SZ: \t0x%.8x\n",
+				node->node.GROUP4.B2R2_SSZ);
+		b2r2_log_debug("B2R2_S2XY: \t0x%.8x\n",
+				node->node.GROUP4.B2R2_SXY);
+		b2r2_log_debug("B2R2_S2TY: \t0x%.8x\n",
+				node->node.GROUP4.B2R2_STY);
+		b2r2_log_debug("B2R2_S2BA: \t0x%.8x\n",
+				node->node.GROUP4.B2R2_SBA);
+
+		b2r2_log_debug("B2R2_S3SZ: \t0x%.8x\n",
+				node->node.GROUP5.B2R2_SSZ);
+		b2r2_log_debug("B2R2_S3XY: \t0x%.8x\n",
+				node->node.GROUP5.B2R2_SXY);
+		b2r2_log_debug("B2R2_S3TY: \t0x%.8x\n",
+				node->node.GROUP5.B2R2_STY);
+		b2r2_log_debug("B2R2_S3BA: \t0x%.8x\n",
+				node->node.GROUP5.B2R2_SBA);
+
+		b2r2_log_debug("B2R2_CWS: \t0x%.8x\n",
+				node->node.GROUP6.B2R2_CWS);
+		b2r2_log_debug("B2R2_CWO: \t0x%.8x\n",
+				node->node.GROUP6.B2R2_CWO);
+
+		b2r2_log_debug("B2R2_FCTL: \t0x%.8x\n",
+				node->node.GROUP8.B2R2_FCTL);
+		b2r2_log_debug("B2R2_RSF: \t0x%.8x\n",
+				node->node.GROUP9.B2R2_RSF);
+		b2r2_log_debug("B2R2_RZI: \t0x%.8x\n",
+				node->node.GROUP9.B2R2_RZI);
+		b2r2_log_debug("B2R2_HFP: \t0x%.8x\n",
+				node->node.GROUP9.B2R2_HFP);
+		b2r2_log_debug("B2R2_VFP: \t0x%.8x\n",
+				node->node.GROUP9.B2R2_VFP);
+		b2r2_log_debug("B2R2_LUMA_RSF: \t0x%.8x\n",
+				node->node.GROUP10.B2R2_RSF);
+		b2r2_log_debug("B2R2_LUMA_RZI: \t0x%.8x\n",
+				node->node.GROUP10.B2R2_RZI);
+		b2r2_log_debug("B2R2_LUMA_HFP: \t0x%.8x\n",
+				node->node.GROUP10.B2R2_HFP);
+		b2r2_log_debug("B2R2_LUMA_VFP: \t0x%.8x\n",
+				node->node.GROUP10.B2R2_VFP);
+
+
+		b2r2_log_debug("B2R2_IVMX0: \t0x%.8x\n",
+				node->node.GROUP15.B2R2_VMX0);
+		b2r2_log_debug("B2R2_IVMX1: \t0x%.8x\n",
+				node->node.GROUP15.B2R2_VMX1);
+		b2r2_log_debug("B2R2_IVMX2: \t0x%.8x\n",
+				node->node.GROUP15.B2R2_VMX2);
+		b2r2_log_debug("B2R2_IVMX3: \t0x%.8x\n",
+				node->node.GROUP15.B2R2_VMX3);
+		b2r2_log_debug("\n=============\nNODE END\n");
+
+		node = node->next;
+	} while (node != NULL && dump_all);
+
+	b2r2_log_info("%s DONE\n", __func__);
+}
+
+/**
+ * to_native_fmt() - returns the native B2R2 format
+ */
+static inline enum b2r2_native_fmt to_native_fmt(enum b2r2_blt_fmt fmt)
+{
+
+	switch (fmt) {
+	case B2R2_BLT_FMT_UNUSED:
+		return B2R2_NATIVE_RGB565;
+	case B2R2_BLT_FMT_1_BIT_A1:
+		return B2R2_NATIVE_A1;
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return B2R2_NATIVE_A8;
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+		return B2R2_NATIVE_RGB565;
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		return B2R2_NATIVE_ARGB4444;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		return B2R2_NATIVE_ARGB1555;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		return B2R2_NATIVE_ARGB8565;
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+		return B2R2_NATIVE_RGB888;
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+		return B2R2_NATIVE_YCBCR888;
+	case B2R2_BLT_FMT_32_BIT_ABGR8888: /* Not actually supported by HW */
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+		return B2R2_NATIVE_ARGB8888;
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		return B2R2_NATIVE_AYCBCR8888;
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		return B2R2_NATIVE_YCBCR422R;
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+		return B2R2_NATIVE_YCBCR422R;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return B2R2_NATIVE_YCBCR42X_R2B;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return B2R2_NATIVE_YCBCR42X_MBN;
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return B2R2_NATIVE_YUV;
+	default:
+		/* Should never ever happen */
+		return B2R2_NATIVE_BYTE;
+	}
+}
+
+/**
+ * get_alpha_range() - returns the alpha range of the given format
+ */
+static inline enum b2r2_ty get_alpha_range(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_8_BIT_A8:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		return B2R2_TY_ALPHA_RANGE_255; /* 0 - 255 */
+		break;
+	default:
+		break;
+	}
+
+	return B2R2_TY_ALPHA_RANGE_128; /* 0 - 128 */
+}
+
+static unsigned int get_pitch(enum b2r2_blt_fmt format, u32 width)
+{
+	switch (format) {
+	case B2R2_BLT_FMT_1_BIT_A1: {
+		int pitch = width >> 3;
+		/* Check for remainder */
+		if (width & 7)
+			pitch++;
+		return pitch;
+		break;
+	}
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return width;
+		break;
+	case B2R2_BLT_FMT_16_BIT_RGB565: /* all 16 bits/pixel RGB formats */
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		return width * 2;
+		break;
+	case B2R2_BLT_FMT_24_BIT_RGB888: /* all 24 bits/pixel RGB formats */
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+		return width * 3;
+		break;
+	case B2R2_BLT_FMT_32_BIT_ARGB8888: /* all 32 bits/pixel RGB formats */
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		return width * 4;
+		break;
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		return width * 4;
+		break;
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		/* width of the buffer must be a multiple of 4 */
+		if (width & 3) {
+			b2r2_log_warn("%s: Illegal width "
+				"for fmt=%#010x width=%d\n", __func__,
+				format, width);
+			return 0;
+		}
+		return width * 2;
+		break;
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return width;
+		break;
+	/* fall through, same pitch and pointers */
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		/* width of the buffer must be a multiple of 2 */
+		if (width & 1) {
+			b2r2_log_warn("%s: Illegal width "
+				"for fmt=%#010x width=%d\n", __func__,
+				format, width);
+			return 0;
+		}
+		/*
+		 * return pitch of the Y-buffer.
+		 * U and V pitch can be derived from it.
+		 */
+		return width;
+		break;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		/* width of the buffer must be a multiple of 16. */
+		if (width & 15) {
+			b2r2_log_warn("%s: Illegal width "
+				"for fmt=%#010x width=%d\n", __func__,
+				format, width);
+			return 0;
+		}
+		/*
+		 * return pitch of the Y-buffer.
+		 * U and V pitch can be derived from it.
+		 */
+		return width;
+		break;
+	default:
+		b2r2_log_warn("%s: Unable to determine pitch "
+			"for fmt=%#010x width=%d\n", __func__,
+			format, width);
+		return 0;
+	}
+}
+
+static s32 validate_buf(const struct b2r2_blt_img *image,
+			const struct b2r2_resolved_buf *buf)
+{
+	u32 expect_buf_size;
+	u32 pitch;
+
+	if (image->width <= 0 || image->height <= 0) {
+		b2r2_log_warn("%s: width=%d or height=%d negative.\n", __func__,
+			image->width, image->height);
+		return -EINVAL;
+	}
+
+	if (image->pitch == 0) {
+		/* autodetect pitch based on format and width */
+		pitch = get_pitch(image->fmt, image->width);
+	} else
+		pitch = image->pitch;
+
+	expect_buf_size = pitch * image->height;
+
+	if (pitch == 0) {
+		b2r2_log_warn("%s: Unable to detect pitch. "
+			"fmt=%#010x, width=%d\n",
+			__func__,
+			image->fmt, image->width);
+		return -EINVAL;
+	}
+
+	/* format specific adjustments */
+	switch (image->fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+		/*
+		 * Use ceil(height/2) in case buffer height
+		 * is not divisible by 2.
+		 */
+		expect_buf_size +=
+			(pitch >> 1) * ((image->height + 1) >> 1) * 2;
+		break;
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+		expect_buf_size += (pitch >> 1) * image->height * 2;
+		break;
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		expect_buf_size += pitch * image->height * 2;
+		break;
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+		/*
+		 * include space occupied by U and V data.
+		 * U and V interleaved, half resolution, which makes
+		 * the UV pitch equal to luma pitch.
+		 * Use ceil(height/2) in case buffer height
+		 * is not divisible by 2.
+		 */
+		expect_buf_size += pitch * ((image->height + 1) >> 1);
+		break;
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+		/*
+		 * include space occupied by U and V data.
+		 * U and V interleaved, half resolution, which makes
+		 * the UV pitch equal to luma pitch.
+		 */
+		expect_buf_size += pitch * image->height;
+		break;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+		/* Height must be a multiple of 16 for macro-block format.*/
+		if (image->height & 15) {
+			b2r2_log_warn("%s: Illegal height "
+				"for fmt=%#010x height=%d\n", __func__,
+				image->fmt, image->height);
+			return -EINVAL;
+		}
+		expect_buf_size += pitch * (image->height >> 1);
+		break;
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		/* Height must be a multiple of 16 for macro-block format.*/
+		if (image->height & 15) {
+			b2r2_log_warn("%s: Illegal height "
+				"for fmt=%#010x height=%d\n", __func__,
+				image->fmt, image->height);
+			return -EINVAL;
+		}
+		expect_buf_size += pitch * image->height;
+		break;
+	default:
+		break;
+	}
+
+	if (buf->file_len < expect_buf_size) {
+		b2r2_log_warn("%s: Invalid buffer size:\n"
+			"fmt=%#010x w=%d h=%d buf.len=%d expect_buf_size=%d\n",
+			__func__,
+			image->fmt, image->width, image->height, buf->file_len,
+			expect_buf_size);
+		return -EINVAL;
+	}
+
+	if (image->buf.type == B2R2_BLT_PTR_VIRTUAL) {
+		b2r2_log_warn("%s: Virtual pointers not supported yet.\n",
+			__func__);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * Bit-expand the color from fmt to RGB888 with blue at LSB.
+ * Copy MSBs into missing LSBs.
+ */
+static u32 to_RGB888(u32 color, const enum b2r2_blt_fmt fmt)
+{
+	u32 out_color = 0;
+	u32 r = 0;
+	u32 g = 0;
+	u32 b = 0;
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		r = ((color & 0xf00) << 12) | ((color & 0xf00) << 8);
+		g = ((color & 0xf0) << 8) | ((color & 0xf0) << 4);
+		b = ((color & 0xf) << 4) | (color & 0xf);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		r = ((color & 0x7c00) << 9) | ((color & 0x7000) << 4);
+		g = ((color & 0x3e0) << 6) | ((color & 0x380) << 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+		r = ((color & 0xf800) << 8) | ((color & 0xe000) << 3);
+		g = ((color & 0x7e0) << 5) | ((color & 0x600) >> 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+		out_color = color & 0xffffff;
+		break;
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		r = (color & 0xff) << 16;
+		g = color & 0xff00;
+		b = (color & 0xff0000) >> 16;
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		r = ((color & 0xf800) << 8) | ((color & 0xe000) << 3);
+		g = ((color & 0x7e0) << 5) | ((color & 0x600) >> 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	default:
+		break;
+	}
+
+	return out_color;
+}
+
+
+static void setup_fill_input_stage(const struct b2r2_blt_request *req,
+					struct b2r2_node *node,
+					struct b2r2_work_buf *out_buf)
+{
+	enum b2r2_native_fmt fill_fmt = 0;
+	u32 src_color = req->user_req.src_color;
+	const struct b2r2_blt_img *dst_img = &(req->user_req.dst_img);
+	b2r2_log_info("%s ENTRY\n", __func__);
+
+	/* Determine format in src_color */
+	switch (dst_img->fmt) {
+	/* ARGB formats */
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+		if ((req->user_req.flags & B2R2_BLT_FLAG_SOURCE_FILL) != 0) {
+			fill_fmt = B2R2_NATIVE_ARGB8888;
+		} else {
+			/* SOURCE_FILL_RAW */
+			fill_fmt = to_native_fmt(dst_img->fmt);
+			if (dst_img->fmt == B2R2_BLT_FMT_32_BIT_ABGR8888) {
+				/*
+				 * Color is read from a register,
+				 * where it is stored in ABGR format.
+				 * Set up IVMX.
+				 */
+				node->node.GROUP0.B2R2_INS |=
+						B2R2_INS_IVMX_ENABLED;
+				node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+				node->node.GROUP15.B2R2_VMX0 =
+						B2R2_VMX0_RGB_TO_BGR;
+				node->node.GROUP15.B2R2_VMX1 =
+						B2R2_VMX1_RGB_TO_BGR;
+				node->node.GROUP15.B2R2_VMX2 =
+						B2R2_VMX2_RGB_TO_BGR;
+				node->node.GROUP15.B2R2_VMX3 =
+						B2R2_VMX3_RGB_TO_BGR;
+			}
+		}
+		break;
+	/* YUV formats */
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		if ((req->user_req.flags & B2R2_BLT_FLAG_SOURCE_FILL) != 0) {
+			fill_fmt = B2R2_NATIVE_AYCBCR8888;
+			/*
+			 * Set up IVMX
+			 * The destination format is in fact YUV,
+			 * but the input stage stores the data in
+			 * an intermediate buffer which is RGB.
+			 * Hence the conversion from YUV to RGB.
+			 * Format of the supplied src_color is
+			 * B2R2_BLT_FMT_32_BIT_AYUV8888.
+			 */
+			node->node.GROUP0.B2R2_INS |= B2R2_INS_IVMX_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+			node->node.GROUP15.B2R2_VMX0 =
+					B2R2_VMX0_BLT_YUV888_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX1 =
+					B2R2_VMX1_BLT_YUV888_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX2 =
+					B2R2_VMX2_BLT_YUV888_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX3 =
+					B2R2_VMX3_BLT_YUV888_TO_RGB_601_VIDEO;
+		} else {
+			/* SOURCE_FILL_RAW */
+			bool dst_yuv_planar =
+				B2R2_BLT_FMT_YUV420_PACKED_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YUV422_PACKED_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YVU420_PACKED_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YVU422_PACKED_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YUV444_PACKED_PLANAR ==
+					dst_img->fmt;
+
+			bool dst_yuv_semi_planar =
+				B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE ==
+					dst_img->fmt;
+
+			if (dst_yuv_planar || dst_yuv_semi_planar) {
+				/*
+				 * SOURCE_FILL_RAW cannot be supported
+				 * with multi-buffer formats.
+				 * Force a legal format to prevent B2R2
+				 * from misbehaving.
+				 */
+				fill_fmt = B2R2_NATIVE_AYCBCR8888;
+			} else {
+				fill_fmt = to_native_fmt(dst_img->fmt);
+			}
+
+			switch (dst_img->fmt) {
+			case B2R2_BLT_FMT_24_BIT_YUV888:
+			case B2R2_BLT_FMT_32_BIT_AYUV8888:
+				node->node.GROUP0.B2R2_INS |=
+					B2R2_INS_IVMX_ENABLED;
+				node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+				node->node.GROUP15.B2R2_VMX0 =
+					B2R2_VMX0_BLT_YUV888_TO_RGB_601_VIDEO;
+				node->node.GROUP15.B2R2_VMX1 =
+					B2R2_VMX1_BLT_YUV888_TO_RGB_601_VIDEO;
+				node->node.GROUP15.B2R2_VMX2 =
+					B2R2_VMX2_BLT_YUV888_TO_RGB_601_VIDEO;
+				node->node.GROUP15.B2R2_VMX3 =
+					B2R2_VMX3_BLT_YUV888_TO_RGB_601_VIDEO;
+				break;
+			case B2R2_BLT_FMT_Y_CB_Y_CR:
+				/*
+				 * Setup input VMX to convert YVU to
+				 * RGB 601 VIDEO
+				 * Chroma components are swapped so
+				 * it is YVU and not YUV.
+				 */
+				node->node.GROUP15.B2R2_VMX0 =
+					B2R2_VMX0_YVU_TO_RGB_601_VIDEO;
+				node->node.GROUP15.B2R2_VMX1 =
+					B2R2_VMX1_YVU_TO_RGB_601_VIDEO;
+				node->node.GROUP15.B2R2_VMX2 =
+					B2R2_VMX2_YVU_TO_RGB_601_VIDEO;
+				node->node.GROUP15.B2R2_VMX3 =
+					B2R2_VMX3_YVU_TO_RGB_601_VIDEO;
+				break;
+			default:
+				/*
+				 * Set up IVMX
+				 * The destination format is in fact YUV,
+				 * but the input stage stores the data in
+				 * an intermediate buffer which is RGB.
+				 * Hence the conversion from YUV to RGB.
+				 */
+				node->node.GROUP0.B2R2_INS |=
+					B2R2_INS_IVMX_ENABLED;
+				node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+				node->node.GROUP15.B2R2_VMX0 =
+					B2R2_VMX0_YUV_TO_RGB_601_VIDEO;
+				node->node.GROUP15.B2R2_VMX1 =
+					B2R2_VMX1_YUV_TO_RGB_601_VIDEO;
+				node->node.GROUP15.B2R2_VMX2 =
+					B2R2_VMX2_YUV_TO_RGB_601_VIDEO;
+				node->node.GROUP15.B2R2_VMX3 =
+					B2R2_VMX3_YUV_TO_RGB_601_VIDEO;
+				break;
+			}
+		}
+		break;
+	default:
+		src_color = 0;
+		fill_fmt = B2R2_NATIVE_ARGB8888;
+		break;
+	}
+
+	node->node.GROUP1.B2R2_TBA = out_buf->phys_addr;
+	node->node.GROUP1.B2R2_TTY =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		B2R2_TY_HSO_LEFT_TO_RIGHT |
+		B2R2_TY_VSO_TOP_TO_BOTTOM;
+	/* Set color fill on SRC2 channel */
+	node->node.GROUP4.B2R2_SBA = 0;
+	node->node.GROUP4.B2R2_STY =
+		(0 << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		fill_fmt |
+		get_alpha_range(dst_img->fmt) |
+		B2R2_TY_HSO_LEFT_TO_RIGHT |
+		B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_COLOR_FILL_REGISTER;
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_COLOR_FILL;
+	node->node.GROUP2.B2R2_S2CF = src_color;
+
+	node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+	b2r2_log_info("%s DONE\n", __func__);
+}
+
+static void setup_input_stage(const struct b2r2_blt_request *req,
+			      struct b2r2_node *node,
+			      struct b2r2_work_buf *out_buf)
+{
+	/* Horizontal and vertical scaling factors in 6.10 fixed point format */
+	s32 h_scf = 1 << 10;
+	s32 v_scf = 1 << 10;
+	const struct b2r2_blt_rect *src_rect = &(req->user_req.src_rect);
+	const struct b2r2_blt_rect *dst_rect = &(req->user_req.dst_rect);
+	const struct b2r2_blt_img *src_img = &(req->user_req.src_img);
+	u32 src_pitch = 0;
+	/* horizontal and vertical scan order for out_buf */
+	enum b2r2_ty dst_hso = B2R2_TY_HSO_LEFT_TO_RIGHT;
+	enum b2r2_ty dst_vso = B2R2_TY_VSO_TOP_TO_BOTTOM;
+	u32 fctl = 0;
+	u32 rsf = 0;
+	u32 rzi = 0;
+	bool yuv_semi_planar =
+		src_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		src_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+
+	bool yuv_planar =
+		src_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR;
+
+	struct b2r2_filter_spec *hf;
+	struct b2r2_filter_spec *vf;
+
+	bool use_h_filter = false;
+	bool use_v_filter = false;
+
+	b2r2_log_info("%s ENTRY\n", __func__);
+
+	if (((B2R2_BLT_FLAG_SOURCE_FILL | B2R2_BLT_FLAG_SOURCE_FILL_RAW) &
+			req->user_req.flags) != 0) {
+		setup_fill_input_stage(req, node, out_buf);
+		b2r2_log_info("%s DONE\n", __func__);
+		return;
+	}
+
+	if (src_img->pitch == 0) {
+		/* Determine pitch based on format and width of the image. */
+		src_pitch = get_pitch(src_img->fmt, src_img->width);
+	} else {
+		src_pitch = src_img->pitch;
+	}
+
+	b2r2_log_info("%s transform=%#010x\n",
+			__func__, req->user_req.transform);
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		h_scf = (src_rect->width << 10) / dst_rect->height;
+		v_scf = (src_rect->height << 10) / dst_rect->width;
+	} else {
+		h_scf = (src_rect->width << 10) / dst_rect->width;
+		v_scf = (src_rect->height << 10) / dst_rect->height;
+	}
+
+	hf = b2r2_filter_find(h_scf);
+	vf = b2r2_filter_find(v_scf);
+
+	use_h_filter = h_scf != (1 << 10);
+	use_v_filter = v_scf != (1 << 10);
+
+	/* B2R2_BLT_FLAG_BLUR overrides any scaling filter. */
+	if (req->user_req.flags & B2R2_BLT_FLAG_BLUR) {
+		use_h_filter = true;
+		use_v_filter = true;
+		hf = b2r2_filter_blur();
+		vf = b2r2_filter_blur();
+	}
+
+	/* Configure horizontal rescale */
+	if (h_scf != (1 << 10)) {
+		b2r2_log_info("%s: Scaling horizontally by 0x%.8x"
+			"\ns(%d, %d)->d(%d, %d)\n", __func__,
+			h_scf, src_rect->width, src_rect->height,
+			dst_rect->width, dst_rect->height);
+	}
+	fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER;
+	rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+	rsf |= h_scf << B2R2_RSF_HSRC_INC_SHIFT;
+	rzi |= B2R2_RZI_DEFAULT_HNB_REPEAT;
+
+	/* Configure vertical rescale */
+	if (v_scf != (1 << 10)) {
+		b2r2_log_info("%s: Scaling vertically by 0x%.8x"
+			"\ns(%d, %d)->d(%d, %d)\n", __func__,
+			v_scf, src_rect->width, src_rect->height,
+			dst_rect->width, dst_rect->height);
+	}
+	fctl |= B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+	rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+	rsf |= v_scf << B2R2_RSF_VSRC_INC_SHIFT;
+	rzi |= 2 << B2R2_RZI_VNB_REPEAT_SHIFT;
+
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_RESCALE2D_ENABLED;
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_RESIZE_CHROMA;
+
+	/* Adjustments that depend on the source format */
+	switch (src_img->fmt) {
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		/* Set up IVMX */
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_IVMX_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+
+		node->node.GROUP15.B2R2_VMX0 = B2R2_VMX0_RGB_TO_BGR;
+		node->node.GROUP15.B2R2_VMX1 = B2R2_VMX1_RGB_TO_BGR;
+		node->node.GROUP15.B2R2_VMX2 = B2R2_VMX2_RGB_TO_BGR;
+		node->node.GROUP15.B2R2_VMX3 = B2R2_VMX3_RGB_TO_BGR;
+		break;
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+		/*
+		 * Setup input VMX to convert YVU to RGB 601 VIDEO
+		 * Chroma components are swapped so
+		 * it is YVU and not YUV.
+		 */
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_IVMX_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+		node->node.GROUP15.B2R2_VMX0 =
+			B2R2_VMX0_YVU_TO_RGB_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX1 =
+			B2R2_VMX1_YVU_TO_RGB_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX2 =
+			B2R2_VMX2_YVU_TO_RGB_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX3 =
+			B2R2_VMX3_YVU_TO_RGB_601_VIDEO;
+		break;
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		/* Set up IVMX */
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_IVMX_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+		node->node.GROUP15.B2R2_VMX0 =
+			B2R2_VMX0_YUV_TO_RGB_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX1 =
+			B2R2_VMX1_YUV_TO_RGB_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX2 =
+			B2R2_VMX2_YUV_TO_RGB_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX3 =
+			B2R2_VMX3_YUV_TO_RGB_601_VIDEO;
+		break;
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		/*
+		 * Set up IVMX.
+		 * Color components are laid out in memory as V, U, Y, (A)
+		 * with V at the first byte (due to little endian addressing).
+		 * B2R2 expects them to be as U, Y, V, (A)
+		 * with U at the first byte.
+		 */
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_IVMX_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+		node->node.GROUP15.B2R2_VMX0 =
+			B2R2_VMX0_BLT_YUV888_TO_RGB_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX1 =
+			B2R2_VMX1_BLT_YUV888_TO_RGB_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX2 =
+			B2R2_VMX2_BLT_YUV888_TO_RGB_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX3 =
+			B2R2_VMX3_BLT_YUV888_TO_RGB_601_VIDEO;
+		break;
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE: {
+		/*
+		 * Luma handled in the same way
+		 * for all YUV multi-buffer formats.
+		 * Set luma rescale registers.
+		 */
+		u32 rsf_luma = 0;
+		u32 rzi_luma = 0;
+
+		/* Set up IVMX */
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_IVMX_ENABLED | B2R2_INS_RESCALE2D_ENABLED;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_IVMX | B2R2_CIC_RESIZE_LUMA;
+
+		if (src_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			src_img->fmt ==
+				B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR) {
+			node->node.GROUP15.B2R2_VMX0 =
+				B2R2_VMX0_YVU_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX1 =
+				B2R2_VMX1_YVU_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX2 =
+				B2R2_VMX2_YVU_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX3 =
+				B2R2_VMX3_YVU_TO_RGB_601_VIDEO;
+		} else {
+			node->node.GROUP15.B2R2_VMX0 =
+				B2R2_VMX0_YUV_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX1 =
+				B2R2_VMX1_YUV_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX2 =
+				B2R2_VMX2_YUV_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX3 =
+				B2R2_VMX3_YUV_TO_RGB_601_VIDEO;
+		}
+
+		fctl |= B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_RESIZER |
+			B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_RESIZER;
+
+		if (use_h_filter && hf) {
+			fctl |= B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_FILTER;
+			node->node.GROUP10.B2R2_HFP = hf->h_coeffs_phys_addr;
+		}
+
+		if (use_v_filter && vf) {
+			fctl |= B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_FILTER;
+			node->node.GROUP10.B2R2_VFP = vf->v_coeffs_phys_addr;
+		}
+
+		rsf_luma |= h_scf << B2R2_RSF_HSRC_INC_SHIFT;
+		rzi_luma |= B2R2_RZI_DEFAULT_HNB_REPEAT;
+
+		rsf_luma |= v_scf << B2R2_RSF_VSRC_INC_SHIFT;
+		rzi_luma |= 2 << B2R2_RZI_VNB_REPEAT_SHIFT;
+
+		node->node.GROUP10.B2R2_RSF = rsf_luma;
+		node->node.GROUP10.B2R2_RZI = rzi_luma;
+
+		switch (src_img->fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chrominance is always half the luminance size
+			 * so chrominance resizer is always active.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (h_scf >> 1) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (v_scf >> 1) << B2R2_RSF_VSRC_INC_SHIFT;
+			/* Select suitable filter for chroma */
+			hf = b2r2_filter_find(h_scf >> 1);
+			vf = b2r2_filter_find(v_scf >> 1);
+			use_h_filter = true;
+			use_v_filter = true;
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chrominance is always half the luminance size
+			 * only in horizontal direction.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (h_scf >> 1) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= v_scf << B2R2_RSF_VSRC_INC_SHIFT;
+			/* Select suitable filter for chroma */
+			hf = b2r2_filter_find(h_scf >> 1);
+			use_h_filter = true;
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			/* Chrominance is the same size as luminance.*/
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= h_scf << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= v_scf << B2R2_RSF_VSRC_INC_SHIFT;
+			/* Select suitable filter for chroma */
+			hf = b2r2_filter_find(h_scf);
+			vf = b2r2_filter_find(v_scf);
+			use_h_filter = true;
+			use_v_filter = true;
+			break;
+		default:
+			break;
+		}
+		break;
+	}
+	default:
+		break;
+	}
+
+	/*
+	 * Set the filter control and rescale registers.
+	 * GROUP9 registers are used for all single-buffer formats
+	 * or for chroma in case of multi-buffer YUV formats.
+	 * h/v_filter is now appropriately selected for chroma scaling,
+	 * be it YUV multi-buffer, or single-buffer raster format.
+	 * B2R2_BLT_FLAG_BLUR overrides any scaling filter.
+	 */
+	if (req->user_req.flags & B2R2_BLT_FLAG_BLUR) {
+		use_h_filter = true;
+		use_v_filter = true;
+		hf = b2r2_filter_blur();
+		vf = b2r2_filter_blur();
+	}
+
+	if (use_h_filter && hf) {
+		fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER;
+		node->node.GROUP9.B2R2_HFP = hf->h_coeffs_phys_addr;
+	}
+
+	if (use_v_filter && vf) {
+		fctl |= B2R2_FCTL_VF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER;
+		node->node.GROUP9.B2R2_VFP = vf->v_coeffs_phys_addr;
+	}
+
+	node->node.GROUP8.B2R2_FCTL |= fctl;
+	node->node.GROUP9.B2R2_RSF |= rsf;
+	node->node.GROUP9.B2R2_RZI |= rzi;
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_FILTER_CONTROL;
+
+	/*
+	 * Flip transform is done before potential rotation.
+	 * This can be achieved with appropriate scan order.
+	 * Transform stage will only do rotation.
+	 */
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_H)
+		dst_hso = B2R2_TY_HSO_RIGHT_TO_LEFT;
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_V)
+		dst_vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+
+	/* Set target buffer */
+	node->node.GROUP1.B2R2_TBA = out_buf->phys_addr;
+	node->node.GROUP1.B2R2_TTY =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		dst_hso | dst_vso;
+
+	if (yuv_planar) {
+		/*
+		 * Set up chrominance buffers on source 1 and 2,
+		 * luminance on source 3.
+		 * src_pitch and physical_address apply to luminance,
+		 * corresponding chrominance values have to be derived.
+		 */
+		u32 cb_addr = 0;
+		u32 cr_addr = 0;
+		u32 chroma_pitch = 0;
+		bool swapped_chroma =
+			src_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+			src_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR;
+		enum b2r2_native_fmt src_fmt = to_native_fmt(src_img->fmt);
+
+		if (swapped_chroma)
+			cr_addr = req->src_resolved.physical_address +
+				src_pitch * src_img->height;
+		else
+			cb_addr = req->src_resolved.physical_address +
+				src_pitch * src_img->height;
+
+		switch (src_img->fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+			chroma_pitch = src_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					(src_img->height >> 1);
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					(src_img->height >> 1);
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+			chroma_pitch = src_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					src_img->height;
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					src_img->height;
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			/* Chrominance has full resolution, same as luminance.*/
+			chroma_pitch = src_pitch;
+			cr_addr =
+				cb_addr + chroma_pitch * src_img->height;
+			break;
+		default:
+			break;
+		}
+
+		node->node.GROUP3.B2R2_SBA = cr_addr;
+		node->node.GROUP3.B2R2_STY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			src_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP4.B2R2_SBA = cb_addr;
+		node->node.GROUP4.B2R2_STY = node->node.GROUP3.B2R2_STY;
+
+		node->node.GROUP5.B2R2_SBA = req->src_resolved.physical_address;
+		node->node.GROUP5.B2R2_STY =
+			(src_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			src_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_1_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_3_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_1 |
+			B2R2_CIC_SOURCE_2 |
+			B2R2_CIC_SOURCE_3;
+	} else if (yuv_semi_planar) {
+		/*
+		 * Set up chrominance buffer on source 2, luminance on source 3.
+		 * src_pitch and physical_address apply to luminance,
+		 * corresponding chrominance values have to be derived.
+		 * U and V are interleaved at half the luminance resolution,
+		 * which makes the pitch of the UV plane equal
+		 * to luminance pitch.
+		 */
+		u32 chroma_addr = req->src_resolved.physical_address +
+			src_pitch * src_img->height;
+		u32 chroma_pitch = src_pitch;
+
+		enum b2r2_native_fmt src_fmt = to_native_fmt(src_img->fmt);
+
+		node->node.GROUP4.B2R2_SBA = chroma_addr;
+		node->node.GROUP4.B2R2_STY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			src_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP5.B2R2_SBA = req->src_resolved.physical_address;
+		node->node.GROUP5.B2R2_STY =
+			(src_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			src_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_3_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_2 | B2R2_CIC_SOURCE_3;
+	} else {
+		/* single buffer format */
+		node->node.GROUP4.B2R2_SBA = req->src_resolved.physical_address;
+		node->node.GROUP4.B2R2_STY =
+			(src_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			to_native_fmt(src_img->fmt) |
+			get_alpha_range(src_img->fmt) |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+	}
+
+	if ((req->user_req.flags &
+			B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) != 0) {
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_CLUTOP_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_CLUT;
+		node->node.GROUP7.B2R2_CCO = B2R2_CCO_CLUT_COLOR_CORRECTION |
+			B2R2_CCO_CLUT_UPDATE;
+		node->node.GROUP7.B2R2_CML = req->clut_phys_addr;
+	}
+
+	node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+
+	b2r2_log_info("%s DONE\n", __func__);
+}
+
+static void setup_transform_stage(const struct b2r2_blt_request *req,
+				  struct b2r2_node *node,
+				  struct b2r2_work_buf *out_buf,
+				  struct b2r2_work_buf *in_buf)
+{
+	/* vertical scan order for out_buf */
+	enum b2r2_ty dst_vso = B2R2_TY_VSO_TOP_TO_BOTTOM;
+	enum b2r2_blt_transform transform = req->user_req.transform;
+
+	b2r2_log_info("%s ENTRY\n", __func__);
+
+	if (transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		/*
+		 * Scan order must be flipped otherwise contents will
+		 * be mirrored vertically. Leftmost column of in_buf
+		 * would become top instead of bottom row of out_buf.
+		 */
+		dst_vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_ROTATION_ENABLED;
+	}
+
+	/* Set target buffer */
+	node->node.GROUP1.B2R2_TBA = out_buf->phys_addr;
+	node->node.GROUP1.B2R2_TTY =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		B2R2_TY_HSO_LEFT_TO_RIGHT | dst_vso;
+
+	/* Set source buffer on SRC2 channel */
+	node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+	node->node.GROUP4.B2R2_STY =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		B2R2_TY_HSO_LEFT_TO_RIGHT |
+		B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+	node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+
+	b2r2_log_info("%s DONE\n", __func__);
+}
+
+/*
+static void setup_mask_stage(const struct b2r2_blt_request req,
+			     struct b2r2_node *node,
+			     struct b2r2_work_buf *out_buf,
+			     struct b2r2_work_buf *in_buf);
+*/
+
+static void setup_dst_read_stage(const struct b2r2_blt_request *req,
+				 struct b2r2_node *node,
+				 struct b2r2_work_buf *out_buf)
+{
+	const struct b2r2_blt_img *dst_img = &(req->user_req.dst_img);
+	u32 fctl = 0;
+	u32 rsf = 0;
+	bool yuv_semi_planar =
+		dst_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		dst_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+
+	bool yuv_planar =
+		dst_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR;
+
+	u32 dst_pitch = 0;
+	if (dst_img->pitch == 0) {
+		/* Determine pitch based on format and width of the image. */
+		dst_pitch = get_pitch(dst_img->fmt, dst_img->width);
+	} else {
+		dst_pitch = dst_img->pitch;
+	}
+
+	b2r2_log_info("%s ENTRY\n", __func__);
+
+	/* Adjustments that depend on the destination format */
+	switch (dst_img->fmt) {
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		/* Set up IVMX */
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_IVMX_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+
+		node->node.GROUP15.B2R2_VMX0 = B2R2_VMX0_RGB_TO_BGR;
+		node->node.GROUP15.B2R2_VMX1 = B2R2_VMX1_RGB_TO_BGR;
+		node->node.GROUP15.B2R2_VMX2 = B2R2_VMX2_RGB_TO_BGR;
+		node->node.GROUP15.B2R2_VMX3 = B2R2_VMX3_RGB_TO_BGR;
+		break;
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		/* Set up IVMX */
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_IVMX_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+		if (dst_img->fmt == B2R2_BLT_FMT_Y_CB_Y_CR) {
+			/*
+			 * Setup input VMX to convert YVU to RGB 601 VIDEO
+			 * Chroma components are swapped
+			 * so it is YVU and not YUV.
+			 */
+			node->node.GROUP15.B2R2_VMX0 =
+				B2R2_VMX0_YVU_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX1 =
+				B2R2_VMX1_YVU_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX2 =
+				B2R2_VMX2_YVU_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX3 =
+				B2R2_VMX3_YVU_TO_RGB_601_VIDEO;
+		} else {
+			node->node.GROUP15.B2R2_VMX0 =
+				B2R2_VMX0_YUV_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX1 =
+				B2R2_VMX1_YUV_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX2 =
+				B2R2_VMX2_YUV_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX3 =
+				B2R2_VMX3_YUV_TO_RGB_601_VIDEO;
+		}
+		break;
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE: {
+		/* Set up IVMX */
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_IVMX_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+
+		if (dst_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			dst_img->fmt ==
+				B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR) {
+			node->node.GROUP15.B2R2_VMX0 =
+				B2R2_VMX0_YVU_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX1 =
+				B2R2_VMX1_YVU_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX2 =
+				B2R2_VMX2_YVU_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX3 =
+				B2R2_VMX3_YVU_TO_RGB_601_VIDEO;
+		} else {
+			node->node.GROUP15.B2R2_VMX0 =
+				B2R2_VMX0_YUV_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX1 =
+				B2R2_VMX1_YUV_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX2 =
+				B2R2_VMX2_YUV_TO_RGB_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX3 =
+				B2R2_VMX3_YUV_TO_RGB_601_VIDEO;
+		}
+
+		switch (dst_img->fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chrominance is always half the luminance size
+			 * so chrominance resizer is always active.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (1 << 9) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (1 << 9) << B2R2_RSF_VSRC_INC_SHIFT;
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chrominance is always half the luminance size
+			 * only in horizontal direction.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (1 << 9) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (1 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			/* Chrominance is the same size as luminance.*/
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (1 << 10) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (1 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+			break;
+		default:
+			break;
+		}
+		/* Set the filter control and rescale registers for chroma */
+		node->node.GROUP8.B2R2_FCTL |= fctl;
+		node->node.GROUP9.B2R2_RSF |= rsf;
+		node->node.GROUP9.B2R2_RZI =
+			B2R2_RZI_DEFAULT_HNB_REPEAT |
+			(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_RESCALE2D_ENABLED;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_FILTER_CONTROL | B2R2_CIC_RESIZE_CHROMA;
+		break;
+	}
+	default:
+		break;
+	}
+
+	/* Set target buffer */
+	node->node.GROUP1.B2R2_TBA = out_buf->phys_addr;
+	node->node.GROUP1.B2R2_TTY =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		B2R2_TY_HSO_LEFT_TO_RIGHT |
+		B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	if (yuv_planar) {
+		/*
+		 * Set up chrominance buffers on source 1 and 2,
+		 * luminance on source 3.
+		 * dst_pitch and physical_address apply to luminance,
+		 * corresponding chrominance values have to be derived.
+		 */
+		u32 cb_addr = 0;
+		u32 cr_addr = 0;
+		u32 chroma_pitch = 0;
+		bool swapped_chroma =
+			dst_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+			dst_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR;
+		enum b2r2_native_fmt dst_native_fmt =
+				to_native_fmt(dst_img->fmt);
+
+		if (swapped_chroma)
+			cr_addr = req->dst_resolved.physical_address +
+				dst_pitch * dst_img->height;
+		else
+			cb_addr = req->dst_resolved.physical_address +
+				dst_pitch * dst_img->height;
+
+		switch (dst_img->fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+			chroma_pitch = dst_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					(dst_img->height >> 1);
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					(dst_img->height >> 1);
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+			chroma_pitch = dst_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					dst_img->height;
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					dst_img->height;
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			chroma_pitch = dst_pitch;
+			cr_addr =
+				cb_addr + chroma_pitch * dst_img->height;
+			break;
+		default:
+			break;
+		}
+
+		node->node.GROUP3.B2R2_SBA = cr_addr;
+		node->node.GROUP3.B2R2_STY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP4.B2R2_SBA = cb_addr;
+		node->node.GROUP4.B2R2_STY = node->node.GROUP3.B2R2_STY;
+
+		node->node.GROUP5.B2R2_SBA = req->dst_resolved.physical_address;
+		node->node.GROUP5.B2R2_STY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_1_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_3_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_1 |
+			B2R2_CIC_SOURCE_2 |
+			B2R2_CIC_SOURCE_3;
+	} else if (yuv_semi_planar) {
+		/*
+		 * Set up chrominance buffer on source 2, luminance on source 3.
+		 * dst_pitch and physical_address apply to luminance,
+		 * corresponding chrominance values have to be derived.
+		 * U and V are interleaved at half the luminance resolution,
+		 * which makes the pitch of the UV plane equal
+		 * to luminance pitch.
+		 */
+		u32 chroma_addr = req->dst_resolved.physical_address +
+			dst_pitch * dst_img->height;
+		u32 chroma_pitch = dst_pitch;
+
+		enum b2r2_native_fmt dst_native_fmt =
+				to_native_fmt(dst_img->fmt);
+
+		node->node.GROUP4.B2R2_SBA = chroma_addr;
+		node->node.GROUP4.B2R2_STY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP5.B2R2_SBA = req->dst_resolved.physical_address;
+		node->node.GROUP5.B2R2_STY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_3_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_2 | B2R2_CIC_SOURCE_3;
+	} else {
+		/* single buffer format */
+		node->node.GROUP4.B2R2_SBA = req->dst_resolved.physical_address;
+		node->node.GROUP4.B2R2_STY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			to_native_fmt(dst_img->fmt) |
+			get_alpha_range(dst_img->fmt) |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+	}
+
+	node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+
+	b2r2_log_info("%s DONE\n", __func__);
+}
+
+static void setup_blend_stage(const struct b2r2_blt_request *req,
+			      struct b2r2_node *node,
+			      struct b2r2_work_buf *bg_buf,
+			      struct b2r2_work_buf *fg_buf)
+{
+	u32 global_alpha = req->user_req.global_alpha;
+	b2r2_log_info("%s ENTRY\n", __func__);
+
+	node->node.GROUP0.B2R2_ACK = 0;
+
+	if (req->user_req.flags &
+			(B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND |
+			B2R2_BLT_FLAG_PER_PIXEL_ALPHA_BLEND)) {
+		/* Some kind of blending needs to be done. */
+		if (req->user_req.flags & B2R2_BLT_FLAG_SRC_IS_NOT_PREMULT)
+			node->node.GROUP0.B2R2_ACK |=
+				B2R2_ACK_MODE_BLEND_NOT_PREMULT;
+		else
+			node->node.GROUP0.B2R2_ACK |=
+				B2R2_ACK_MODE_BLEND_PREMULT;
+
+		/*
+		 * global_alpha register accepts 0..128 range,
+		 * global_alpha in the request is 0..255, remap needed.
+		 */
+		if (req->user_req.flags & B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND) {
+			if (global_alpha == 255)
+				global_alpha = 128;
+			else
+				global_alpha >>= 1;
+		} else {
+			/*
+			 * Use solid global_alpha
+			 * if global alpha blending is not set.
+			 */
+			global_alpha = 128;
+		}
+
+		node->node.GROUP0.B2R2_ACK |=
+			global_alpha << (B2R2_ACK_GALPHA_ROPID_SHIFT);
+
+		/* Set background on SRC1 channel */
+		node->node.GROUP3.B2R2_SBA = bg_buf->phys_addr;
+		node->node.GROUP3.B2R2_STY =
+			(B2R2_GENERIC_WORK_BUF_PITCH <<
+				B2R2_TY_BITMAP_PITCH_SHIFT) |
+			B2R2_GENERIC_WORK_BUF_FMT |
+			B2R2_TY_ALPHA_RANGE_255 |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		/* Set foreground on SRC2 channel */
+		node->node.GROUP4.B2R2_SBA = fg_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY =
+			(B2R2_GENERIC_WORK_BUF_PITCH <<
+				B2R2_TY_BITMAP_PITCH_SHIFT) |
+			B2R2_GENERIC_WORK_BUF_FMT |
+			B2R2_TY_ALPHA_RANGE_255 |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		/* Set target buffer */
+		node->node.GROUP1.B2R2_TBA = bg_buf->phys_addr;
+		node->node.GROUP1.B2R2_TTY =
+			(B2R2_GENERIC_WORK_BUF_PITCH <<
+				B2R2_TY_BITMAP_PITCH_SHIFT) |
+			B2R2_GENERIC_WORK_BUF_FMT |
+			B2R2_TY_ALPHA_RANGE_255 |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_1_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_1 |
+			B2R2_CIC_SOURCE_2;
+	} else {
+		/*
+		 * No blending, foreground goes on SRC2. No global alpha.
+		 * EMACSOC TODO: The blending stage should be skipped altogether
+		 * if no blending is to be done. Probably could go directly from
+		 * transform to writeback.
+		 */
+		node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+
+		node->node.GROUP4.B2R2_SBA = fg_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY =
+			(B2R2_GENERIC_WORK_BUF_PITCH <<
+				B2R2_TY_BITMAP_PITCH_SHIFT) |
+			B2R2_GENERIC_WORK_BUF_FMT |
+			B2R2_TY_ALPHA_RANGE_255 |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP1.B2R2_TBA = bg_buf->phys_addr;
+		node->node.GROUP1.B2R2_TTY =
+			(B2R2_GENERIC_WORK_BUF_PITCH <<
+				B2R2_TY_BITMAP_PITCH_SHIFT) |
+			B2R2_GENERIC_WORK_BUF_FMT |
+			B2R2_TY_ALPHA_RANGE_255 |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+	}
+
+	b2r2_log_info("%s DONE\n", __func__);
+}
+
+static void setup_writeback_stage(const struct b2r2_blt_request *req,
+				  struct b2r2_node *node,
+				  struct b2r2_work_buf *in_buf)
+{
+	const struct b2r2_blt_img *dst_img = &(req->user_req.dst_img);
+	const enum b2r2_blt_fmt dst_fmt = dst_img->fmt;
+	const bool yuv_planar_dst =
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR;
+
+	const bool yuv_semi_planar_dst =
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+
+	const u32 group4_b2r2_sty =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		B2R2_TY_HSO_LEFT_TO_RIGHT |
+		B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	u32 dst_dither = 0;
+	u32 dst_pitch = 0;
+
+	b2r2_log_info("%s ENTRY\n", __func__);
+
+	if (dst_img->pitch == 0) {
+		/* Determine pitch based on format and width of the image. */
+		dst_pitch = get_pitch(dst_img->fmt, dst_img->width);
+	} else
+		dst_pitch = dst_img->pitch;
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_DITHER) != 0)
+		dst_dither = B2R2_TTY_RGB_ROUND_DITHER;
+
+	/* Set target buffer(s) */
+	if (yuv_planar_dst) {
+		/*
+		 * three nodes required to write the output.
+		 * Luma, blue chroma and red chroma.
+		 */
+		u32 fctl = 0;
+		u32 rsf = 0;
+		const u32 group0_b2r2_ins =
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_RECT_CLIP_ENABLED |
+			B2R2_INS_IVMX_ENABLED;
+		const u32 group0_b2r2_cic =
+			B2R2_CIC_SOURCE_2 |
+			B2R2_CIC_CLIP_WINDOW |
+			B2R2_CIC_IVMX;
+
+		u32 cb_addr = 0;
+		u32 cr_addr = 0;
+		u32 chroma_pitch = 0;
+		bool swapped_chroma =
+			dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR;
+		enum b2r2_native_fmt dst_native_fmt =
+				to_native_fmt(dst_img->fmt);
+		enum b2r2_ty alpha_range = get_alpha_range(dst_img->fmt);
+
+		if (swapped_chroma)
+			cr_addr = req->dst_resolved.physical_address +
+				dst_pitch * dst_img->height;
+		else
+			cb_addr = req->dst_resolved.physical_address +
+				dst_pitch * dst_img->height;
+
+		switch (dst_fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+			chroma_pitch = dst_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					(dst_img->height >> 1);
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					(dst_img->height >> 1);
+			/*
+			 * Chrominance is always half the luminance size
+			 * so chrominance resizer is always active.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+					B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+			chroma_pitch = dst_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					dst_img->height;
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					dst_img->height;
+			/*
+			 * YUV422 or YVU422
+			 * Chrominance is always half the luminance size
+			 * only in horizontal direction.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (1 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			chroma_pitch = dst_pitch;
+			cr_addr =
+				cb_addr + chroma_pitch * dst_img->height;
+			/*
+			 * No scaling required since
+			 * chrominance is not subsampled.
+			 */
+		default:
+			break;
+		}
+
+		/* Luma (Y-component) */
+		node->node.GROUP1.B2R2_TBA = req->dst_resolved.physical_address;
+		node->node.GROUP1.B2R2_TTY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt | alpha_range |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither;
+
+		node->node.GROUP15.B2R2_VMX0 = B2R2_VMX0_RGB_TO_YUV_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX1 = B2R2_VMX1_RGB_TO_YUV_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX2 = B2R2_VMX2_RGB_TO_YUV_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX3 = B2R2_VMX3_RGB_TO_YUV_601_VIDEO;
+
+		 /* bypass ALU, no blending here. Handled in its own stage. */
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS = group0_b2r2_ins;
+		node->node.GROUP0.B2R2_CIC |= group0_b2r2_cic;
+
+		/* Set source buffer on SRC2 channel */
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+
+		/* Blue chroma (U-component)*/
+		node = node->next;
+		node->node.GROUP1.B2R2_TBA = cb_addr;
+		node->node.GROUP1.B2R2_TTY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt | alpha_range |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither |
+			B2R2_TTY_CHROMA_NOT_LUMA;
+
+		node->node.GROUP15.B2R2_VMX0 = B2R2_VMX0_RGB_TO_YUV_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX1 = B2R2_VMX1_RGB_TO_YUV_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX2 = B2R2_VMX2_RGB_TO_YUV_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX3 = B2R2_VMX3_RGB_TO_YUV_601_VIDEO;
+
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS = group0_b2r2_ins;
+		node->node.GROUP0.B2R2_CIC |= group0_b2r2_cic;
+		if (dst_fmt != B2R2_BLT_FMT_YUV444_PACKED_PLANAR) {
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_RESCALE2D_ENABLED;
+			node->node.GROUP0.B2R2_CIC |=
+				B2R2_CIC_FILTER_CONTROL |
+				B2R2_CIC_RESIZE_CHROMA;
+			/* Set the filter control and rescale registers */
+			node->node.GROUP8.B2R2_FCTL = fctl;
+			node->node.GROUP9.B2R2_RSF = rsf;
+			node->node.GROUP9.B2R2_RZI =
+				B2R2_RZI_DEFAULT_HNB_REPEAT |
+				(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+		}
+
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+
+
+		/*
+		 * Red chroma (V-component)
+		 * The flag B2R2_TTY_CB_NOT_CR actually works
+		 * the other way around, i.e. as if it was
+		 * CR_NOT_CB.
+		 */
+		node = node->next;
+		node->node.GROUP1.B2R2_TBA = cr_addr;
+		node->node.GROUP1.B2R2_TTY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt | alpha_range |
+			B2R2_TTY_CB_NOT_CR |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither |
+			B2R2_TTY_CHROMA_NOT_LUMA;
+
+		node->node.GROUP15.B2R2_VMX0 = B2R2_VMX0_RGB_TO_YUV_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX1 = B2R2_VMX1_RGB_TO_YUV_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX2 = B2R2_VMX2_RGB_TO_YUV_601_VIDEO;
+		node->node.GROUP15.B2R2_VMX3 = B2R2_VMX3_RGB_TO_YUV_601_VIDEO;
+
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS = group0_b2r2_ins;
+		node->node.GROUP0.B2R2_CIC |= group0_b2r2_cic;
+		if (dst_fmt != B2R2_BLT_FMT_YUV444_PACKED_PLANAR) {
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_RESCALE2D_ENABLED;
+			node->node.GROUP0.B2R2_CIC |=
+				B2R2_CIC_FILTER_CONTROL |
+				B2R2_CIC_RESIZE_CHROMA;
+			/* Set the filter control and rescale registers */
+			node->node.GROUP8.B2R2_FCTL = fctl;
+			node->node.GROUP9.B2R2_RSF = rsf;
+			node->node.GROUP9.B2R2_RZI =
+				B2R2_RZI_DEFAULT_HNB_REPEAT |
+				(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+		}
+
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+	} else if (yuv_semi_planar_dst) {
+		/*
+		 * two nodes required to write the output.
+		 * One node for luma and one for interleaved chroma
+		 * components.
+		 */
+		u32 fctl = 0;
+		u32 rsf = 0;
+		const u32 group0_b2r2_ins =
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_RECT_CLIP_ENABLED |
+			B2R2_INS_IVMX_ENABLED;
+		const u32 group0_b2r2_cic =
+			B2R2_CIC_SOURCE_2 |
+			B2R2_CIC_CLIP_WINDOW |
+			B2R2_CIC_IVMX;
+
+		u32 chroma_addr = req->dst_resolved.physical_address +
+			dst_pitch * dst_img->height;
+		u32 chroma_pitch = dst_pitch;
+		enum b2r2_native_fmt dst_native_fmt =
+				to_native_fmt(dst_img->fmt);
+		enum b2r2_ty alpha_range = get_alpha_range(dst_img->fmt);
+
+		if (dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+			dst_fmt ==
+				B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+			dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR) {
+			/*
+			 * Chrominance is always half the luminance size
+			 * so chrominance resizer is always active.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+					B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+		} else {
+			/*
+			 * YUV422
+			 * Chrominance is always half the luminance size
+			 * only in horizontal direction.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (1 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+		}
+
+		/* Luma (Y-component) */
+		node->node.GROUP1.B2R2_TBA = req->dst_resolved.physical_address;
+		node->node.GROUP1.B2R2_TTY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt | alpha_range |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither;
+
+		if (dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR) {
+			node->node.GROUP15.B2R2_VMX0 =
+				B2R2_VMX0_RGB_TO_YVU_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX1 =
+				B2R2_VMX1_RGB_TO_YVU_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX2 =
+				B2R2_VMX2_RGB_TO_YVU_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX3 =
+				B2R2_VMX3_RGB_TO_YVU_601_VIDEO;
+		} else {
+			node->node.GROUP15.B2R2_VMX0 =
+				B2R2_VMX0_RGB_TO_YUV_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX1 =
+				B2R2_VMX1_RGB_TO_YUV_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX2 =
+				B2R2_VMX2_RGB_TO_YUV_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX3 =
+				B2R2_VMX3_RGB_TO_YUV_601_VIDEO;
+		}
+
+		 /* bypass ALU, no blending here. Handled in its own stage. */
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS = group0_b2r2_ins;
+		node->node.GROUP0.B2R2_CIC |= group0_b2r2_cic;
+
+		/* Set source buffer on SRC2 channel */
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+
+		/* Chroma (UV-components)*/
+		node = node->next;
+		node->node.GROUP1.B2R2_TBA = chroma_addr;
+		node->node.GROUP1.B2R2_TTY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt | alpha_range |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither |
+			B2R2_TTY_CHROMA_NOT_LUMA;
+
+		if (dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR) {
+			node->node.GROUP15.B2R2_VMX0 =
+				B2R2_VMX0_RGB_TO_YVU_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX1 =
+				B2R2_VMX1_RGB_TO_YVU_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX2 =
+				B2R2_VMX2_RGB_TO_YVU_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX3 =
+				B2R2_VMX3_RGB_TO_YVU_601_VIDEO;
+		} else {
+			node->node.GROUP15.B2R2_VMX0 =
+				B2R2_VMX0_RGB_TO_YUV_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX1 =
+				B2R2_VMX1_RGB_TO_YUV_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX2 =
+				B2R2_VMX2_RGB_TO_YUV_601_VIDEO;
+			node->node.GROUP15.B2R2_VMX3 =
+				B2R2_VMX3_RGB_TO_YUV_601_VIDEO;
+		}
+
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS =
+			group0_b2r2_ins | B2R2_INS_RESCALE2D_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= group0_b2r2_cic |
+			B2R2_CIC_FILTER_CONTROL |
+			B2R2_CIC_RESIZE_CHROMA;
+
+		/* Set the filter control and rescale registers */
+		node->node.GROUP8.B2R2_FCTL = fctl;
+		node->node.GROUP9.B2R2_RSF = rsf;
+		node->node.GROUP9.B2R2_RZI =
+			B2R2_RZI_DEFAULT_HNB_REPEAT |
+			(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+	} else {
+		/* single buffer target */
+
+		/* Set up OVMX */
+		switch (dst_fmt) {
+		case B2R2_BLT_FMT_32_BIT_ABGR8888:
+			node->node.GROUP0.B2R2_INS |= B2R2_INS_OVMX_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_OVMX;
+			node->node.GROUP16.B2R2_VMX0 = B2R2_VMX0_RGB_TO_BGR;
+			node->node.GROUP16.B2R2_VMX1 = B2R2_VMX1_RGB_TO_BGR;
+			node->node.GROUP16.B2R2_VMX2 = B2R2_VMX2_RGB_TO_BGR;
+			node->node.GROUP16.B2R2_VMX3 = B2R2_VMX3_RGB_TO_BGR;
+			break;
+		case B2R2_BLT_FMT_Y_CB_Y_CR:
+			node->node.GROUP0.B2R2_INS |= B2R2_INS_OVMX_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_OVMX;
+			node->node.GROUP16.B2R2_VMX0 =
+				B2R2_VMX0_RGB_TO_YVU_601_VIDEO;
+			node->node.GROUP16.B2R2_VMX1 =
+				B2R2_VMX1_RGB_TO_YVU_601_VIDEO;
+			node->node.GROUP16.B2R2_VMX2 =
+				B2R2_VMX2_RGB_TO_YVU_601_VIDEO;
+			node->node.GROUP16.B2R2_VMX3 =
+				B2R2_VMX3_RGB_TO_YVU_601_VIDEO;
+			break;
+		case B2R2_BLT_FMT_24_BIT_YUV888: /* fall through */
+		case B2R2_BLT_FMT_32_BIT_AYUV8888:
+			node->node.GROUP0.B2R2_INS |= B2R2_INS_OVMX_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_OVMX;
+			node->node.GROUP16.B2R2_VMX0 =
+				B2R2_VMX0_RGB_TO_BLT_YUV888_601_VIDEO;
+			node->node.GROUP16.B2R2_VMX1 =
+				B2R2_VMX1_RGB_TO_BLT_YUV888_601_VIDEO;
+			node->node.GROUP16.B2R2_VMX2 =
+				B2R2_VMX2_RGB_TO_BLT_YUV888_601_VIDEO;
+			node->node.GROUP16.B2R2_VMX3 =
+				B2R2_VMX3_RGB_TO_BLT_YUV888_601_VIDEO;
+			break;
+		default:
+			break;
+		}
+
+		node->node.GROUP1.B2R2_TBA = req->dst_resolved.physical_address;
+		node->node.GROUP1.B2R2_TTY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			to_native_fmt(dst_img->fmt) |
+			get_alpha_range(dst_img->fmt) |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither;
+
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_RECT_CLIP_ENABLED;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_2 | B2R2_CIC_CLIP_WINDOW;
+
+		if (req->user_req.flags & B2R2_BLT_FLAG_SOURCE_COLOR_KEY) {
+			u32 key_color = 0;
+
+			node->node.GROUP0.B2R2_ACK |=
+				B2R2_ACK_CKEY_SEL_SRC_AFTER_CLUT |
+				B2R2_ACK_CKEY_RED_MATCH_IF_BETWEEN |
+				B2R2_ACK_CKEY_GREEN_MATCH_IF_BETWEEN |
+				B2R2_ACK_CKEY_BLUE_MATCH_IF_BETWEEN;
+			node->node.GROUP0.B2R2_INS |= B2R2_INS_CKEY_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_COLOR_KEY;
+
+			key_color = to_RGB888(req->user_req.src_color,
+				req->user_req.src_img.fmt);
+			node->node.GROUP12.B2R2_KEY1 = key_color;
+			node->node.GROUP12.B2R2_KEY2 = key_color;
+		}
+
+		/* Set source buffer on SRC2 channel */
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+	}
+	/*
+	 * Writeback is the last stage. Terminate the program chain
+	 * to prevent out-of-control B2R2 execution.
+	 */
+	node->node.GROUP0.B2R2_NIP = 0;
+
+	b2r2_log_info("%s DONE\n", __func__);
+}
+
+/*
+ * Public functions
+ */
+void b2r2_generic_init()
+{
+	b2r2_filters_init();
+}
+
+void b2r2_generic_exit(void)
+{
+	b2r2_filters_exit();
+}
+
+int b2r2_generic_analyze(const struct b2r2_blt_request *req,
+			 s32 *work_buf_width,
+			 s32 *work_buf_height,
+			 u32 *work_buf_count,
+			 u32 *node_count)
+{
+	/*
+	 * Need at least 4 nodes, read or fill input, read dst, blend
+	 * and write back the result */
+	u32 n_nodes = 4;
+	/* Need at least 2 bufs, 1 for blend output and 1 for input */
+	u32 n_work_bufs = 2;
+	/* Horizontal and vertical scaling factors in 6.10 fixed point format */
+	s32 h_scf = 1 << 10;
+	s32 v_scf = 1 << 10;
+	enum b2r2_blt_fmt dst_fmt = 0;
+	bool is_src_fill = false;
+	bool yuv_planar_dst;
+	bool yuv_semi_planar_dst;
+
+	struct b2r2_blt_rect src_rect;
+	struct b2r2_blt_rect dst_rect;
+
+	if (req == NULL || work_buf_width == NULL || work_buf_height == NULL ||
+			work_buf_count == NULL || node_count == NULL) {
+		b2r2_log_warn("%s: Invalid in or out pointers:\n"
+			"req=0x%p\n"
+			"work_buf_width=0x%p work_buf_height=0x%p "
+			"work_buf_count=0x%p\n"
+			"node_count=0x%p.\n",
+			__func__,
+			req,
+			work_buf_width, work_buf_height,
+			work_buf_count,
+			node_count);
+		return -EINVAL;
+	}
+
+	dst_fmt = req->user_req.dst_img.fmt;
+
+	is_src_fill = (req->user_req.flags &
+				(B2R2_BLT_FLAG_SOURCE_FILL |
+				B2R2_BLT_FLAG_SOURCE_FILL_RAW)) != 0;
+
+	yuv_planar_dst =
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR;
+	yuv_semi_planar_dst =
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+
+	*node_count = 0;
+	*work_buf_width = 0;
+	*work_buf_height = 0;
+	*work_buf_count = 0;
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		n_nodes++;
+		n_work_bufs++;
+	}
+
+	if ((yuv_planar_dst || yuv_semi_planar_dst) &&
+			(req->user_req.flags & B2R2_BLT_FLAG_SOURCE_FILL_RAW)) {
+		b2r2_log_warn("%s: Invalid combination: source_fill_raw"
+			" and multi-buffer destination.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_SOURCE_COLOR_KEY) != 0 &&
+			(req->user_req.flags & B2R2_BLT_FLAG_DEST_COLOR_KEY)) {
+		b2r2_log_warn("%s: Invalid combination: source and "
+			"destination color keying.\n", __func__);
+		return -EINVAL;
+	}
+
+	if ((req->user_req.flags &
+			(B2R2_BLT_FLAG_SOURCE_FILL |
+			B2R2_BLT_FLAG_SOURCE_FILL_RAW)) &&
+			(req->user_req.flags &
+			(B2R2_BLT_FLAG_SOURCE_COLOR_KEY |
+			B2R2_BLT_FLAG_DEST_COLOR_KEY))) {
+		b2r2_log_warn("%s: Invalid combination: "
+			"source_fill and color keying.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if ((req->user_req.flags &
+			(B2R2_BLT_FLAG_PER_PIXEL_ALPHA_BLEND |
+			B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND)) &&
+			(req->user_req.flags &
+			(B2R2_BLT_FLAG_DEST_COLOR_KEY |
+			B2R2_BLT_FLAG_SOURCE_COLOR_KEY))) {
+		b2r2_log_warn("%s: Invalid combination: "
+			"blending and color keying.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_SOURCE_MASK) &&
+			(req->user_req.flags &
+			(B2R2_BLT_FLAG_DEST_COLOR_KEY |
+			B2R2_BLT_FLAG_SOURCE_COLOR_KEY))) {
+		b2r2_log_warn("%s: Invalid combination: source mask and "
+			"color keying.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if (req->user_req.flags &
+			(B2R2_BLT_FLAG_DEST_COLOR_KEY |
+			B2R2_BLT_FLAG_SOURCE_MASK)) {
+		b2r2_log_warn("%s: Unsupported: source mask, "
+			"destination color keying.\n",
+			__func__);
+		return -ENOSYS;
+	}
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_SOURCE_MASK)) {
+		enum b2r2_blt_fmt src_fmt = req->user_req.src_img.fmt;
+		bool yuv_src =
+			src_fmt == B2R2_BLT_FMT_Y_CB_Y_CR ||
+			src_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+			src_fmt ==
+				B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+			src_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+		if (yuv_src || src_fmt == B2R2_BLT_FMT_1_BIT_A1 ||
+				src_fmt == B2R2_BLT_FMT_8_BIT_A8) {
+			b2r2_log_warn("%s: Unsupported: source color keying "
+					"with YUV or pure alpha formats.\n",
+					__func__);
+			return -ENOSYS;
+		}
+	}
+
+	/* Check for invalid dimensions that would hinder scale calculations */
+	src_rect = req->user_req.src_rect;
+	dst_rect = req->user_req.dst_rect;
+	/* Check for invalid src_rect unless src_fill is enabled */
+	if (!is_src_fill && (src_rect.x < 0 || src_rect.y < 0 ||
+		src_rect.x + src_rect.width > req->user_req.src_img.width ||
+		src_rect.y + src_rect.height > req->user_req.src_img.height)) {
+		b2r2_log_warn("%s: src_rect outside src_img:\n"
+			"src(x,y,w,h)=(%d, %d, %d, %d) "
+			"src_img(w,h)=(%d, %d).\n",
+			__func__,
+			src_rect.x, src_rect.y, src_rect.width, src_rect.height,
+			req->user_req.src_img.width,
+			req->user_req.src_img.height);
+		return -EINVAL;
+	}
+
+	if (!is_src_fill && (src_rect.width <= 0 || src_rect.height <= 0)) {
+		b2r2_log_warn("%s: Invalid source dimensions:\n"
+			"src(w,h)=(%d, %d).\n",
+			__func__,
+			src_rect.width, src_rect.height);
+		return -EINVAL;
+	}
+
+	if (dst_rect.width <= 0 || dst_rect.height <= 0) {
+		b2r2_log_warn("%s: Invalid dest dimensions:\n"
+			"dst(w,h)=(%d, %d).\n",
+			__func__,
+			dst_rect.width, dst_rect.height);
+		return -EINVAL;
+	}
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) &&
+			req->user_req.clut == NULL) {
+		b2r2_log_warn("%s: Invalid request: no table specified "
+			"for CLUT color correction.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	/* Check for invalid image params */
+	if (!is_src_fill && validate_buf(&(req->user_req.src_img),
+			&(req->src_resolved)))
+		return -EINVAL;
+
+	if (validate_buf(&(req->user_req.dst_img), &(req->dst_resolved)))
+		return -EINVAL;
+
+	if (is_src_fill) {
+		/*
+		 * Params correct for a source fill operation.
+		 * No need for further checking.
+		 */
+		if (yuv_planar_dst)
+			n_nodes += 2;
+		else if (yuv_semi_planar_dst)
+			n_nodes++;
+
+		*work_buf_width = B2R2_GENERIC_WORK_BUF_WIDTH;
+		*work_buf_height = B2R2_GENERIC_WORK_BUF_HEIGHT;
+		*work_buf_count = n_work_bufs;
+		*node_count = n_nodes;
+		b2r2_log_info("%s DONE buf_w=%d buf_h=%d buf_count=%d "
+			"node_count=%d\n",
+			__func__,
+			*work_buf_width, *work_buf_height,
+			*work_buf_count, *node_count);
+		return 0;
+	}
+
+	/*
+	 * Calculate scaling factors, all transform enum values
+	 * that include rotation have the CCW_ROT_90 bit set.
+	 */
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		h_scf = (src_rect.width << 10) / dst_rect.height;
+		v_scf = (src_rect.height << 10) / dst_rect.width;
+	} else {
+		h_scf = (src_rect.width << 10) / dst_rect.width;
+		v_scf = (src_rect.height << 10) / dst_rect.height;
+	}
+
+	/* Check for degenerate/out_of_range scaling factors. */
+	if (h_scf <= 0 || v_scf <= 0 || h_scf > 0x7C00 || v_scf > 0x7C00) {
+		b2r2_log_warn("%s: Dimensions result in degenerate or "
+			"out of range scaling:\n"
+			"src(w,h)=(%d, %d) "
+			"dst(w,h)=(%d,%d).\n"
+			"h_scf=0x%.8x, v_scf=0x%.8x\n",
+			__func__,
+			src_rect.width, src_rect.height,
+			dst_rect.width, dst_rect.height,
+			h_scf, v_scf);
+		return -EINVAL;
+	}
+
+	if (yuv_planar_dst)
+		n_nodes += 2;
+	else if (yuv_semi_planar_dst)
+		n_nodes++;
+
+	*work_buf_width = B2R2_GENERIC_WORK_BUF_WIDTH;
+	*work_buf_height = B2R2_GENERIC_WORK_BUF_HEIGHT;
+	*work_buf_count = n_work_bufs;
+	*node_count = n_nodes;
+	b2r2_log_info("%s DONE buf_w=%d buf_h=%d buf_count=%d node_count=%d\n",
+		__func__,
+		*work_buf_width, *work_buf_height, *work_buf_count,
+		*node_count);
+	return 0;
+}
+
+/*
+ *
+ */
+int b2r2_generic_configure(const struct b2r2_blt_request *req,
+			   struct b2r2_node *first,
+			   struct b2r2_work_buf *tmp_bufs,
+			   u32 buf_count)
+{
+	struct b2r2_node *node = NULL;
+	struct b2r2_work_buf *in_buf = NULL;
+	struct b2r2_work_buf *out_buf = NULL;
+	struct b2r2_work_buf *empty_buf = NULL;
+
+#ifdef B2R2_GENERIC_DEBUG
+	u32 needed_bufs = 0;
+	u32 needed_nodes = 0;
+	s32 work_buf_width = 0;
+	s32 work_buf_height = 0;
+	u32 n_nodes = 0;
+	int invalid_req = b2r2_generic_analyze(req, &work_buf_width,
+					       &work_buf_height, &needed_bufs,
+					       &needed_nodes);
+	if (invalid_req < 0) {
+		b2r2_log_warn("%s: Invalid request supplied, ec=%d\n",
+			__func__, invalid_req);
+		return -EINVAL;
+	}
+
+	node = first;
+
+	while (node != NULL) {
+		n_nodes++;
+		node = node->next;
+	}
+	if (n_nodes < needed_nodes) {
+		b2r2_log_warn("%s: Not enough nodes %d < %d.\n",
+			      __func__, n_nodes, needed_nodes);
+		return -EINVAL;
+	}
+
+	if (buf_count < needed_bufs) {
+		b2r2_log_warn("%s: Not enough buffers %d < %d.\n",
+			      __func__, buf_count, needed_bufs);
+		return -EINVAL;
+	}
+
+#endif
+
+	reset_nodes(first);
+	node = first;
+	empty_buf = tmp_bufs;
+	out_buf = empty_buf;
+	empty_buf++;
+	/* Prepare input tile. Color_fill or read from src */
+	setup_input_stage(req, node, out_buf);
+	in_buf = out_buf;
+	out_buf = empty_buf;
+	empty_buf++;
+	node = node->next;
+
+	if ((req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) != 0) {
+		setup_transform_stage(req, node, out_buf, in_buf);
+		node = node->next;
+		in_buf = out_buf;
+		out_buf = empty_buf++;
+	}
+	/* EMACSOC TODO: mask */
+	/*
+	if (req->user_req.flags & B2R2_BLT_FLAG_SOURCE_MASK) {
+		setup_mask_stage(req, node, out_buf, in_buf);
+		node = node->next;
+		in_buf = out_buf;
+		out_buf = empty_buf++;
+	}
+	*/
+	/* Read the part of destination that will be updated */
+	setup_dst_read_stage(req, node, out_buf);
+	node = node->next;
+	setup_blend_stage(req, node, out_buf, in_buf);
+	node = node->next;
+	in_buf = out_buf;
+	setup_writeback_stage(req, node, in_buf);
+	return 0;
+}
+
+void b2r2_generic_set_areas(const struct b2r2_blt_request *req,
+			    struct b2r2_node *first,
+			    struct b2r2_blt_rect *dst_rect_area)
+{
+	/*
+	 * Nodes come in the following order: <input stage>, [transform],
+	 * [src_mask], <dst_read>, <blend>, <writeback>
+	 */
+	struct b2r2_node *node = first;
+	const struct b2r2_blt_rect *dst_rect = &(req->user_req.dst_rect);
+	const struct b2r2_blt_rect *src_rect = &(req->user_req.src_rect);
+	const enum b2r2_blt_fmt src_fmt = req->user_req.src_img.fmt;
+	bool yuv_multi_buffer_src =
+		src_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		src_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+	const enum b2r2_blt_fmt dst_fmt = req->user_req.dst_img.fmt;
+	const bool yuv_multi_buffer_dst =
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+	s32 h_scf = 1 << 10;
+	s32 v_scf = 1 << 10;
+	s32 src_x = 0;
+	s32 src_y = 0;
+	s32 src_w = 0;
+	s32 src_h = 0;
+	u32 b2r2_rzi = 0;
+	s32 clip_top = 0;
+	s32 clip_left = 0;
+	s32 clip_bottom = req->user_req.dst_img.height - 1;
+	s32 clip_right = req->user_req.dst_img.width - 1;
+	/* Dst coords inside the dst_rect, not the buffer */
+	s32 dst_x = dst_rect_area->x;
+	s32 dst_y = dst_rect_area->y;
+
+	b2r2_log_info("%s ENTRY\n", __func__);
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		h_scf = (src_rect->width << 10) / dst_rect->height;
+		v_scf = (src_rect->height << 10) / dst_rect->width;
+	} else {
+		h_scf = (src_rect->width << 10) / dst_rect->width;
+		v_scf = (src_rect->height << 10) / dst_rect->height;
+	}
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		/*
+		 * Normally the inverse transform for 90 degree rotation
+		 * is given by:
+		 * | 0  1|   |x|   | y|
+		 * |     | X | | = |  |
+		 * |-1  0|   |y|   |-x|
+		 * but screen coordinates are flipped in y direction
+		 * (compared to usual Cartesian coordinates), hence the offsets.
+		 */
+		src_x = (dst_rect->height - dst_y - dst_rect_area->height) *
+			h_scf;
+		src_y = dst_x * v_scf;
+		src_w = dst_rect_area->height * h_scf;
+		src_h = dst_rect_area->width * v_scf;
+	} else {
+		src_x = dst_x * h_scf;
+		src_y = dst_y * v_scf;
+		src_w = dst_rect_area->width * h_scf;
+		src_h = dst_rect_area->height * v_scf;
+	}
+
+	b2r2_rzi |= ((src_x & 0x3ff) << B2R2_RZI_HSRC_INIT_SHIFT) |
+		((src_y & 0x3ff) << B2R2_RZI_VSRC_INIT_SHIFT);
+
+	/*
+	 * src_w must contain all the pixels that contribute
+	 * to a particular tile.
+	 * ((x + 0x3ff) >> 10) is equivalent to ceiling(x),
+	 * expressed in 6.10 fixed point format.
+	 * Every destination tile, maps to a certain area in the source
+	 * rectangle. The area in source will most likely not be a rectangle
+	 * with exact integer dimensions whenever arbitrary scaling is involved.
+	 * Consider the following example.
+	 * Suppose, that width of the current destination tile maps
+	 * to 1.7 pixels in source, starting at x == 5.4, as calculated
+	 * using the scaling factor.
+	 * This means that while the destination tile is written,
+	 * the source should be read from x == 5.4 up to x == 5.4 + 1.7 == 7.1
+	 * Consequently, color from 3 pixels (x == 5, 6 and 7)
+	 * needs to be read from source.
+	 * The formula below the comment yields:
+	 * ceil(0.4 + 1.7) == ceil(2.1) == 3
+	 * (src_x & 0x3ff) is the fractional part of src_x,
+	 * which is expressed in 6.10 fixed point format.
+	 * Thus, width of the source area should be 3 pixels wide,
+	 * starting at x == 5.
+	 * However, the reading should not start at x == 5.0
+	 * but a bit inside, namely x == 5.4
+	 * The B2R2_RZI register is used to instruct the HW to do so.
+	 * It contains the fractional part that will be added to
+	 * the first pixel coordinate, before incrementing the current source
+	 * coordinate with the step specified in B2R2_RSF register.
+	 * The same applies to scaling in vertical direction.
+	 */
+	src_w = ((src_x & 0x3ff) + src_w + 0x3ff) >> 10;
+	src_h = ((src_y & 0x3ff) + src_h + 0x3ff) >> 10;
+
+	/*
+	 * EMACSOC TODO: Remove this debug clamp, once tile size
+	 * is taken into account in generic_analyze()
+	 */
+	if (src_w > 128)
+		src_w = 128;
+
+	src_x >>= 10;
+	src_y >>= 10;
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_H)
+		src_x = src_rect->width - src_x - src_w;
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_V)
+		src_y = src_rect->height - src_y - src_h;
+
+	/*
+	 * Translate the src/dst_rect coordinates into true
+	 * src/dst_buffer coordinates
+	 */
+	src_x += src_rect->x;
+	src_y += src_rect->y;
+
+	dst_x += dst_rect->x;
+	dst_y += dst_rect->y;
+
+	/*
+	 * Clamp the src coords to buffer dimensions
+	 * to prevent illegal reads.
+	 */
+	if (src_x < 0)
+		src_x = 0;
+
+	if (src_y < 0)
+		src_y = 0;
+
+	if ((src_x + src_w) > req->user_req.src_img.width)
+		src_w = req->user_req.src_img.width - src_x;
+
+	if ((src_y + src_h) > req->user_req.src_img.height)
+		src_h = req->user_req.src_img.height - src_y;
+
+
+	/* The input node */
+	if (yuv_multi_buffer_src) {
+		/* Luma on SRC3 */
+		node->node.GROUP5.B2R2_SXY =
+			((src_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((src_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP5.B2R2_SSZ =
+			((src_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((src_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+		/* Clear and set only the SRC_INIT bits */
+		node->node.GROUP10.B2R2_RZI &=
+			~((0x3ff << B2R2_RZI_HSRC_INIT_SHIFT) |
+			(0x3ff << B2R2_RZI_VSRC_INIT_SHIFT));
+		node->node.GROUP10.B2R2_RZI |= b2r2_rzi;
+
+		node->node.GROUP9.B2R2_RZI &=
+			~((0x3ff << B2R2_RZI_HSRC_INIT_SHIFT) |
+			(0x3ff << B2R2_RZI_VSRC_INIT_SHIFT));
+		switch (src_fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chroma goes on SRC2 and potentially on SRC1.
+			 * Chroma is half the size of luma. Must round up
+			 * the chroma size to handle cases when luma size is not
+			 * divisible by 2.
+			 * E.g. luma width==7 requires chroma width==4.
+			 * Chroma width==7/2==3 is only enough
+			 * for luma width==6.
+			 */
+			node->node.GROUP4.B2R2_SXY =
+				(((src_x & 0xffff) >> 1) << B2R2_XY_X_SHIFT) |
+				(((src_y & 0xffff) >> 1) << B2R2_XY_Y_SHIFT);
+			node->node.GROUP4.B2R2_SSZ =
+				((((src_w + 1) & 0xfff) >> 1) <<
+					B2R2_SZ_WIDTH_SHIFT) |
+				((((src_h + 1) & 0xfff) >> 1) <<
+					B2R2_SZ_HEIGHT_SHIFT);
+			if (src_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+					src_fmt ==
+					B2R2_BLT_FMT_YVU420_PACKED_PLANAR) {
+				node->node.GROUP3.B2R2_SXY =
+					node->node.GROUP4.B2R2_SXY;
+				node->node.GROUP3.B2R2_SSZ =
+					node->node.GROUP4.B2R2_SSZ;
+			}
+			node->node.GROUP9.B2R2_RZI |= (b2r2_rzi >> 1) &
+				((0x3ff << B2R2_RZI_HSRC_INIT_SHIFT) |
+				 (0x3ff << B2R2_RZI_VSRC_INIT_SHIFT));
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chroma goes on SRC2 and potentially on SRC1.
+			 * Now chroma is half the size of luma
+			 * only in horizontal direction.
+			 * Same rounding applies as for 420 formats above,
+			 * except it is only done horizontally.
+			 */
+			node->node.GROUP4.B2R2_SXY =
+				(((src_x & 0xffff) >> 1) << B2R2_XY_X_SHIFT) |
+				((src_y & 0xffff) << B2R2_XY_Y_SHIFT);
+			node->node.GROUP4.B2R2_SSZ =
+				((((src_w + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+				((src_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+			if (src_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+					src_fmt ==
+					B2R2_BLT_FMT_YVU422_PACKED_PLANAR) {
+				node->node.GROUP3.B2R2_SXY =
+					node->node.GROUP4.B2R2_SXY;
+				node->node.GROUP3.B2R2_SSZ =
+					node->node.GROUP4.B2R2_SSZ;
+			}
+			node->node.GROUP9.B2R2_RZI |=
+				(((src_x & 0x3ff) >> 1) <<
+						B2R2_RZI_HSRC_INIT_SHIFT) |
+				((src_y & 0x3ff) << B2R2_RZI_VSRC_INIT_SHIFT);
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			/*
+			 * Chroma goes on SRC2 and SRC1.
+			 * It is the same size as luma.
+			 */
+			node->node.GROUP4.B2R2_SXY =
+				((src_x & 0xffff) << B2R2_XY_X_SHIFT) |
+				((src_y & 0xffff) << B2R2_XY_Y_SHIFT);
+			node->node.GROUP4.B2R2_SSZ =
+				((src_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+				((src_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+			node->node.GROUP3.B2R2_SXY = node->node.GROUP4.B2R2_SXY;
+			node->node.GROUP3.B2R2_SSZ = node->node.GROUP4.B2R2_SSZ;
+
+			/* Clear and set only the SRC_INIT bits */
+			node->node.GROUP9.B2R2_RZI &=
+				~((0x3ff << B2R2_RZI_HSRC_INIT_SHIFT) |
+				  (0x3ff << B2R2_RZI_VSRC_INIT_SHIFT));
+			node->node.GROUP9.B2R2_RZI |= b2r2_rzi;
+			break;
+		default:
+			break;
+		}
+	} else {
+		node->node.GROUP4.B2R2_SXY =
+			((src_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((src_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP4.B2R2_SSZ =
+			((src_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((src_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+		/* Clear and set only the SRC_INIT bits */
+		node->node.GROUP9.B2R2_RZI &=
+			~((0x3ff << B2R2_RZI_HSRC_INIT_SHIFT) |
+			  (0x3ff << B2R2_RZI_VSRC_INIT_SHIFT));
+		node->node.GROUP9.B2R2_RZI |= b2r2_rzi;
+	}
+
+	node->node.GROUP1.B2R2_TXY = 0;
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		/*
+		 * dst_rect_area coordinates are specified
+		 * after potential rotation.
+		 * Input is read before rotation, hence the width and height
+		 * need to be swapped.
+		 * Horizontal and vertical flips are accomplished with
+		 * suitable scanning order while writing
+		 * to the temporary buffer.
+		 */
+		if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_H) {
+			node->node.GROUP1.B2R2_TXY |=
+				((dst_rect_area->height - 1) & 0xffff) <<
+				B2R2_XY_X_SHIFT;
+		}
+
+		if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_V) {
+			node->node.GROUP1.B2R2_TXY |=
+				((dst_rect_area->width - 1) & 0xffff) <<
+				B2R2_XY_Y_SHIFT;
+		}
+
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_rect_area->height & 0xfff) <<
+						B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->width & 0xfff) <<
+						B2R2_SZ_HEIGHT_SHIFT);
+	} else {
+		if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_H) {
+			node->node.GROUP1.B2R2_TXY |=
+				((dst_rect_area->width - 1) & 0xffff) <<
+				B2R2_XY_X_SHIFT;
+		}
+
+		if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_V) {
+			node->node.GROUP1.B2R2_TXY |=
+				((dst_rect_area->height - 1) & 0xffff) <<
+				B2R2_XY_Y_SHIFT;
+		}
+
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_rect_area->width & 0xfff) <<
+						B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+						B2R2_SZ_HEIGHT_SHIFT);
+	}
+
+	if (req->user_req.flags &
+		(B2R2_BLT_FLAG_SOURCE_FILL | B2R2_BLT_FLAG_SOURCE_FILL_RAW)) {
+		/*
+		 * Scan order for source fill should always be left-to-right
+		 * and top-to-bottom. Fill the input tile from top left.
+		 */
+		node->node.GROUP1.B2R2_TXY = 0;
+		node->node.GROUP4.B2R2_SSZ = node->node.GROUP1.B2R2_TSZ;
+	}
+
+	if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+			dst_rect_area->y == 0) {
+		dump_nodes(node, false);
+		b2r2_log_debug("%s Input node done.\n", __func__);
+	}
+
+	/* Transform */
+	if ((req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) != 0) {
+		/*
+		 * Transform node operates on temporary buffers.
+		 * Content always at top left, but scanning order
+		 * has to be flipped during rotation.
+		 * Width and height need to be considered as well, since
+		 * a tile may not necessarily be filled completely.
+		 * dst_rect_area dimensions are specified
+		 * after potential rotation.
+		 * Input is read before rotation, hence the width and height
+		 * need to be swapped on src.
+		 */
+		node = node->next;
+
+		node->node.GROUP4.B2R2_SXY = 0;
+		node->node.GROUP4.B2R2_SSZ =
+			((dst_rect_area->height & 0xfff) <<
+						B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->width & 0xfff) <<
+						B2R2_SZ_HEIGHT_SHIFT);
+		/* Bottom line written first */
+		node->node.GROUP1.B2R2_TXY =
+			((dst_rect_area->height - 1) & 0xffff) <<
+			B2R2_XY_Y_SHIFT;
+
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_rect_area->width & 0xfff) <<
+						B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+						B2R2_SZ_HEIGHT_SHIFT);
+
+		if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+				dst_rect_area->y == 0) {
+			dump_nodes(node, false);
+			b2r2_log_debug("%s Tranform node done.\n", __func__);
+		}
+	}
+
+	/* Source mask */
+	if (req->user_req.flags & B2R2_BLT_FLAG_SOURCE_MASK) {
+		node = node->next;
+		/*
+		 * Same coords for mask as for the input stage.
+		 * Should the mask be transformed together with source?
+		 * EMACSOC TODO: Apply mask before any
+		 * transform/scaling is done.
+		 * Otherwise it will be dst_ not src_mask.
+		 */
+		if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+				dst_rect_area->y == 0) {
+			dump_nodes(node, false);
+			b2r2_log_debug("%s Source mask node done.\n", __func__);
+		}
+	}
+
+	/* dst_read */
+	if (yuv_multi_buffer_dst) {
+		s32 dst_w = dst_rect_area->width;
+		s32 dst_h = dst_rect_area->height;
+		bool yuv420_dst =
+			dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE;
+
+		bool yuv422_dst =
+			dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+		node = node->next;
+		/* Luma on SRC3 */
+		node->node.GROUP5.B2R2_SXY =
+			((dst_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP5.B2R2_SSZ =
+			((dst_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+		if (yuv420_dst) {
+			/*
+			 * Chroma goes on SRC2 and potentially on SRC1.
+			 * Chroma is half the size of luma. Must round up
+			 * the chroma size to handle cases when luma size is not
+			 * divisible by 2.
+			 * E.g. luma width==7 requires chroma width==4.
+			 * Chroma width==7/2==3 is only enough
+			 * for luma width==6.
+			 */
+			node->node.GROUP4.B2R2_SXY =
+				(((dst_x & 0xffff) >> 1) << B2R2_XY_X_SHIFT) |
+				(((dst_y & 0xffff) >> 1) << B2R2_XY_Y_SHIFT);
+			node->node.GROUP4.B2R2_SSZ =
+				((((dst_w + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+				((((dst_h + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+
+			if (dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+					dst_fmt ==
+					B2R2_BLT_FMT_YVU420_PACKED_PLANAR) {
+				node->node.GROUP3.B2R2_SXY =
+					node->node.GROUP4.B2R2_SXY;
+				node->node.GROUP3.B2R2_SSZ =
+					node->node.GROUP4.B2R2_SSZ;
+			}
+		} else if (yuv422_dst) {
+			/*
+			 * Chroma goes on SRC2 and potentially on SRC1.
+			 * Now chroma is half the size of luma
+			 * only in horizontal direction.
+			 * Same rounding applies as for 420 formats above,
+			 * except it is only done horizontally.
+			 */
+			node->node.GROUP4.B2R2_SXY =
+				(((dst_x & 0xffff) >> 1) << B2R2_XY_X_SHIFT) |
+				((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+			node->node.GROUP4.B2R2_SSZ =
+				((((dst_w + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+				((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+			if (dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+					dst_fmt ==
+					B2R2_BLT_FMT_YVU422_PACKED_PLANAR) {
+				node->node.GROUP3.B2R2_SXY =
+					node->node.GROUP4.B2R2_SXY;
+				node->node.GROUP3.B2R2_SSZ =
+					node->node.GROUP4.B2R2_SSZ;
+			}
+		} else if (dst_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR) {
+			/*
+			 * Chroma goes on SRC2 and SRC1.
+			 * It is the same size as luma.
+			 */
+			node->node.GROUP4.B2R2_SXY = node->node.GROUP5.B2R2_SXY;
+			node->node.GROUP4.B2R2_SSZ = node->node.GROUP5.B2R2_SSZ;
+			node->node.GROUP3.B2R2_SXY = node->node.GROUP5.B2R2_SXY;
+			node->node.GROUP3.B2R2_SSZ = node->node.GROUP5.B2R2_SSZ;
+		}
+
+		node->node.GROUP1.B2R2_TXY = 0;
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+	} else {
+		node = node->next;
+		node->node.GROUP4.B2R2_SXY =
+			((dst_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP4.B2R2_SSZ =
+			((dst_rect_area->width & 0xfff) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+		node->node.GROUP1.B2R2_TXY = 0;
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_rect_area->width & 0xfff) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+	}
+
+	if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+			dst_rect_area->y == 0) {
+		dump_nodes(node, false);
+		b2r2_log_debug("%s dst_read node done.\n", __func__);
+	}
+
+	/* blend */
+	node = node->next;
+	node->node.GROUP3.B2R2_SXY = 0;
+	node->node.GROUP3.B2R2_SSZ =
+		((dst_rect_area->width & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+		((dst_rect_area->height & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+	/* contents of the foreground temporary buffer always at top left */
+	node->node.GROUP4.B2R2_SXY = 0;
+	node->node.GROUP4.B2R2_SSZ =
+		((dst_rect_area->width & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+		((dst_rect_area->height & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+	node->node.GROUP1.B2R2_TXY = 0;
+	node->node.GROUP1.B2R2_TSZ =
+		((dst_rect_area->width & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+		((dst_rect_area->height & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+	if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+			dst_rect_area->y == 0) {
+		dump_nodes(node, false);
+		b2r2_log_debug("%s Blend node done.\n", __func__);
+	}
+
+	/* writeback */
+	node = node->next;
+	if ((req->user_req.flags & B2R2_BLT_FLAG_DESTINATION_CLIP) != 0) {
+		clip_left = req->user_req.dst_clip_rect.x;
+		clip_top = req->user_req.dst_clip_rect.y;
+		clip_right = clip_left + req->user_req.dst_clip_rect.width - 1;
+		clip_bottom = clip_top + req->user_req.dst_clip_rect.height - 1;
+	}
+	/*
+	 * Clamp the dst clip rectangle to buffer dimensions to prevent
+	 * illegal writes. An illegal clip rectangle, e.g. outside the
+	 * buffer will be ignored, resulting in nothing being clipped.
+	 */
+	if (clip_left < 0 || req->user_req.dst_img.width <= clip_left)
+		clip_left = 0;
+
+	if (clip_top < 0 || req->user_req.dst_img.height <= clip_top)
+		clip_top = 0;
+
+	if (clip_right < 0 || req->user_req.dst_img.width <= clip_right)
+		clip_right = req->user_req.dst_img.width - 1;
+
+	if (clip_bottom < 0 || req->user_req.dst_img.height <= clip_bottom)
+		clip_bottom = req->user_req.dst_img.height - 1;
+
+	/*
+	 * Only allow writing inside the clip rect.
+	 * INTNL bit in B2R2_CWO should be zero.
+	 */
+	node->node.GROUP6.B2R2_CWO =
+		((clip_top & 0x7fff) << B2R2_CWO_Y_SHIFT) |
+		((clip_left & 0x7fff) << B2R2_CWO_X_SHIFT);
+	node->node.GROUP6.B2R2_CWS =
+		((clip_bottom & 0x7fff) << B2R2_CWS_Y_SHIFT) |
+		((clip_right & 0x7fff) << B2R2_CWS_X_SHIFT);
+
+	if (yuv_multi_buffer_dst) {
+		const s32 dst_w = dst_rect_area->width;
+		const s32 dst_h = dst_rect_area->height;
+		int i = 0;
+		/* Number of nodes required to write chroma output */
+		int n_nodes = 1;
+		if (dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+				dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+				dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+				dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+				dst_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR)
+			n_nodes = 2;
+
+		node->node.GROUP4.B2R2_SXY = 0;
+		node->node.GROUP4.B2R2_SSZ =
+			((dst_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+		/* Luma (Y-component) */
+		node->node.GROUP1.B2R2_TXY =
+			((dst_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+		node->node.GROUP6.B2R2_CWO =
+			((clip_top & 0x7fff) << B2R2_CWO_Y_SHIFT) |
+			((clip_left & 0x7fff) << B2R2_CWO_X_SHIFT);
+		node->node.GROUP6.B2R2_CWS =
+			((clip_bottom & 0x7fff) << B2R2_CWS_Y_SHIFT) |
+			((clip_right & 0x7fff) << B2R2_CWS_X_SHIFT);
+
+		if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+				dst_rect_area->y == 0) {
+			dump_nodes(node, false);
+			b2r2_log_debug("%s Writeback luma node done.\n",
+				__func__);
+		}
+
+		node = node->next;
+
+		/*
+		 * Chroma components. 1 or 2 nodes
+		 * for semi-planar or planar buffer respectively.
+		 */
+		for (i = 0; i < n_nodes && node != NULL; ++i) {
+
+			node->node.GROUP4.B2R2_SXY = 0;
+			node->node.GROUP4.B2R2_SSZ =
+				((dst_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+				((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+			switch (dst_fmt) {
+			case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+			case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+			case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+			case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+			case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+				/*
+				 * Chroma is half the size of luma.
+				 * Must round up the chroma size to handle
+				 * cases when luma size is not divisible by 2.
+				 * E.g. luma_width==7 requires chroma_width==4.
+				 * Chroma_width==7/2==3 is only enough
+				 * for luma_width==6.
+				 */
+				node->node.GROUP1.B2R2_TXY =
+					(((dst_x & 0xffff) >> 1) <<
+							B2R2_XY_X_SHIFT) |
+					(((dst_y & 0xffff) >> 1) <<
+							B2R2_XY_Y_SHIFT);
+				node->node.GROUP1.B2R2_TSZ =
+					((((dst_w + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+					((((dst_h + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+				break;
+			case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+			case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+			case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+			case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+			case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+				/*
+				 * Now chroma is half the size of luma only
+				 * in horizontal direction.
+				 * Same rounding applies as
+				 * for 420 formats above, except it is only
+				 * done horizontally.
+				 */
+				node->node.GROUP1.B2R2_TXY =
+					(((dst_x & 0xffff) >> 1) <<
+							B2R2_XY_X_SHIFT) |
+					((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+				node->node.GROUP1.B2R2_TSZ =
+					((((dst_w + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+					((dst_h & 0xfff) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+				break;
+			case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+				/*
+				 * Chroma has the same resolution as luma.
+				 */
+				node->node.GROUP1.B2R2_TXY =
+					((dst_x & 0xffff) << B2R2_XY_X_SHIFT) |
+					((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+				node->node.GROUP1.B2R2_TSZ =
+					((dst_w & 0xfff) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+					((dst_h & 0xfff) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+				break;
+			default:
+				break;
+			}
+
+			node->node.GROUP6.B2R2_CWO =
+				((clip_top & 0x7fff) << B2R2_CWO_Y_SHIFT) |
+				((clip_left & 0x7fff) << B2R2_CWO_X_SHIFT);
+			node->node.GROUP6.B2R2_CWS =
+				((clip_bottom & 0x7fff) << B2R2_CWS_Y_SHIFT) |
+				((clip_right & 0x7fff) << B2R2_CWS_X_SHIFT);
+
+			if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+					dst_rect_area->y == 0) {
+				dump_nodes(node, false);
+				b2r2_log_debug("%s Writeback chroma node "
+					"%d of %d done.\n",
+					__func__, i + 1, n_nodes);
+			}
+
+			node = node->next;
+		}
+	} else {
+		node->node.GROUP4.B2R2_SXY = 0;
+		node->node.GROUP4.B2R2_SSZ =
+			((dst_rect_area->width & 0xfff) <<
+					B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+					B2R2_SZ_HEIGHT_SHIFT);
+		node->node.GROUP1.B2R2_TXY =
+			((dst_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_rect_area->width & 0xfff) <<
+					B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+					B2R2_SZ_HEIGHT_SHIFT);
+
+		if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+				dst_rect_area->y == 0) {
+			dump_nodes(node, false);
+			b2r2_log_debug("%s Writeback node done.\n", __func__);
+		}
+	}
+
+	b2r2_log_info("%s DONE\n", __func__);
+}
diff --git a/drivers/video/b2r2/b2r2_generic.h b/drivers/video/b2r2/b2r2_generic.h
new file mode 100644
index 00000000000..35451543c5c
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_generic.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 generic. Full coverage of user interface but
+ * non optimized implementation. For Fallback purposes.
+ *
+ * Author: Maciej Socha <maciej.socha@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_VIDEO_B2R2_GENERIC_H
+#define _LINUX_VIDEO_B2R2_GENERIC_H
+
+#include <video/b2r2_blt.h>
+
+#include "b2r2_internal.h"
+
+/**
+ * b2r2_generic_init()
+ */
+void b2r2_generic_init(void);
+
+/**
+ * b2r2_generic_exit()
+ */
+void b2r2_generic_exit(void);
+
+/**
+ * b2r2_generic_analyze()
+ */
+int b2r2_generic_analyze(const struct b2r2_blt_request *req,
+						 s32 *work_buf_width,
+						 s32 *work_buf_height,
+						 u32 *work_buf_count,
+						 u32 *node_count);
+/**
+ * b2r2_generic_configure()
+ */
+int b2r2_generic_configure(const struct b2r2_blt_request *req,
+				struct b2r2_node *first,
+				struct b2r2_work_buf *tmp_bufs,
+				u32 buf_count);
+/**
+ * b2r2_generic_set_areas()
+ */
+void b2r2_generic_set_areas(const struct b2r2_blt_request *req,
+				struct b2r2_node *first,
+				struct b2r2_blt_rect *dst_rect_area);
+#endif
diff --git a/drivers/video/b2r2/b2r2_global.h b/drivers/video/b2r2/b2r2_global.h
new file mode 100644
index 00000000000..38cf74bb753
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_global.h
@@ -0,0 +1,119 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 global definitions
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __B2R2_GLOBAL_H
+#define __B2R2_GLOBAL_H
+
+/** Sources involved */
+
+struct b2r2_system {
+	unsigned int B2R2_NIP;
+	unsigned int B2R2_CIC;
+	unsigned int B2R2_INS;
+	unsigned int B2R2_ACK;
+};
+
+struct b2r2_target {
+	unsigned int B2R2_TBA;
+	unsigned int B2R2_TTY;
+	unsigned int B2R2_TXY;
+	unsigned int B2R2_TSZ;
+};
+
+struct b2r2_color_fill {
+	unsigned int  B2R2_S1CF;
+	unsigned int  B2R2_S2CF;
+};
+
+struct b2r2_src_config {
+	unsigned int B2R2_SBA;
+	unsigned int B2R2_STY;
+	unsigned int B2R2_SXY;
+	unsigned int B2R2_SSZ;
+};
+
+struct b2r2_clip {
+	 unsigned int B2R2_CWO;
+	 unsigned int B2R2_CWS;
+};
+
+struct b2r2_color_key {
+	 unsigned int B2R2_KEY1;
+	 unsigned int B2R2_KEY2;
+};
+
+struct b2r2_clut {
+	unsigned int B2R2_CCO;
+	unsigned int B2R2_CML;
+};
+
+struct b2r2_rsz_pl_mask {
+	unsigned int B2R2_FCTL;
+	unsigned int B2R2_PMK;
+};
+
+struct b2r2_Cr_luma_rsz {
+	unsigned int B2R2_RSF;
+	unsigned int B2R2_RZI;
+	unsigned int B2R2_HFP;
+	unsigned int B2R2_VFP;
+};
+
+struct b2r2_flikr_filter {
+	unsigned int B2R2_FF0;
+	unsigned int B2R2_FF1;
+	unsigned int B2R2_FF2;
+	unsigned int B2R2_FF3;
+};
+
+struct b2r2_xyl {
+	unsigned int B2R2_XYL;
+	unsigned int B2R2_XYP;
+};
+
+struct b2r2_sau {
+	unsigned int B2R2_SAR;
+	unsigned int B2R2_USR;
+};
+
+struct b2r2_vm {
+	unsigned int B2R2_VMX0;
+	unsigned int B2R2_VMX1;
+	unsigned int B2R2_VMX2;
+	unsigned int B2R2_VMX3;
+};
+
+struct b2r2_link_list {
+
+	struct b2r2_system       GROUP0;
+	struct b2r2_target 	 GROUP1;
+	struct b2r2_color_fill   GROUP2;
+	struct b2r2_src_config	 GROUP3;
+	struct b2r2_src_config   GROUP4;
+	struct b2r2_src_config   GROUP5;
+	struct b2r2_clip         GROUP6;
+	struct b2r2_clut         GROUP7;
+	struct b2r2_rsz_pl_mask	 GROUP8;
+	struct b2r2_Cr_luma_rsz  GROUP9;
+	struct b2r2_Cr_luma_rsz  GROUP10;
+	struct b2r2_flikr_filter GROUP11;
+	struct b2r2_color_key    GROUP12;
+	struct b2r2_xyl  	 GROUP13;
+	struct b2r2_sau		 GROUP14;
+	struct b2r2_vm		 GROUP15;
+	struct b2r2_vm		 GROUP16;
+
+	unsigned int B2R2_RESERVED[2];
+};
+
+
+#endif /* !defined(__B2R2_GLOBAL_H) */
diff --git a/drivers/video/b2r2/b2r2_hw.h b/drivers/video/b2r2/b2r2_hw.h
new file mode 100644
index 00000000000..0dd5f758eb5
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_hw.h
@@ -0,0 +1,683 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 hw definitions
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef B2R2_HW_H__
+#define B2R2_HW_H__
+
+#include <linux/bitops.h>
+
+/* Scaling works in strips 128 pixels wide */
+#define B2R2_RESCALE_MAX_WIDTH 128
+
+/* Rotation works in strips 16 pixels wide */
+#define B2R2_ROTATE_MAX_WIDTH 16
+
+/* B2R2 color formats */
+#define B2R2_COLOR_FORMAT_SHIFT 16
+enum b2r2_native_fmt {
+	/* RGB formats */
+	B2R2_NATIVE_RGB565 = 0x00 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_RGB888 = 0x01 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ARGB8565 = 0x04 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ARGB8888 = 0x05 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ARGB1555 = 0x06 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ARGB4444 = 0x07 << B2R2_COLOR_FORMAT_SHIFT,
+
+	/* YCbCr formats */
+	B2R2_NATIVE_YCBCR888 = 0x10 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_YCBCR422R = 0x12 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_AYCBCR8888 = 0x15 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_YCBCR42X_MB = 0x14 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_YCBCR42X_R2B = 0x16 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_YCBCR42X_MBN = 0x0e << B2R2_COLOR_FORMAT_SHIFT,
+
+	/* CLUT formats */
+	B2R2_NATIVE_CLUT2 = 0x09 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_CLUT8 = 0x0b << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ACLUT44 = 0x0c << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ACLUT88 = 0x0d << B2R2_COLOR_FORMAT_SHIFT,
+
+	/* Misc. formats */
+	B2R2_NATIVE_A1 = 0x18 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_A8 = 0x19 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_YUV	= 0x1e << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_BYTE = 0x1f << B2R2_COLOR_FORMAT_SHIFT,
+};
+
+/* B2R2_CIC register values */
+enum b2r2_cic {
+	B2R2_CIC_COLOR_FILL =     BIT(1),/*0x00000002*/
+	B2R2_CIC_SOURCE_1 =       BIT(2),/*0x00000004*/
+	B2R2_CIC_SOURCE_2 =       BIT(3),/*0x00000008*/
+	B2R2_CIC_SOURCE_3 =       BIT(4),/*0x00000010*/
+	B2R2_CIC_CLIP_WINDOW =    BIT(5),/*0x00000020*/
+	B2R2_CIC_CLUT =           BIT(6),/*0x00000040*/
+	B2R2_CIC_FILTER_CONTROL = BIT(7),/*0x00000080*/
+	B2R2_CIC_RESIZE_CHROMA =  BIT(8),/*0x00000100*/
+	B2R2_CIC_RESIZE_LUMA =    BIT(9),/*0x00000200*/
+	B2R2_CIC_FLICKER_COEFF =  BIT(10),/*0x00000400*/
+	B2R2_CIC_COLOR_KEY =      BIT(11),/*0x00000800*/
+	B2R2_CIC_XYL =            BIT(12),/*0x00001000*/
+	B2R2_CIC_SAU =            BIT(13),/*0x00002000*/
+	B2R2_CIC_IVMX =           BIT(14),/*0x00004000*/
+	B2R2_CIC_OVMX =           BIT(15),/*0x00008000*/
+	B2R2_CIC_PACEDOT =        BIT(16),/*0x00010000*/
+	B2R2_CIC_VC1 =            BIT(17)/*0x00020000*/
+};
+
+/* B2R2_INS register values */
+#define B2R2_INS_SOURCE_1_SHIFT 0
+#define B2R2_INS_SOURCE_2_SHIFT 3
+#define B2R2_INS_SOURCE_3_SHIFT 5
+#define B2R2_INS_IVMX_SHIFT 6
+#define B2R2_INS_CLUTOP_SHIFT 7
+#define B2R2_INS_RESCALE2D_SHIFT 8
+#define B2R2_INS_FLICK_FILT_SHIFT 9
+#define B2R2_INS_RECT_CLIP_SHIFT 10
+#define B2R2_INS_CKEY_SHIFT 11
+#define B2R2_INS_OVMX_SHIFT 12
+#define B2R2_INS_DEI_SHIFT 13
+#define B2R2_INS_PLANE_MASK_SHIFT 14
+#define B2R2_INS_XYL_SHIFT 15
+#define B2R2_INS_DOT_SHIFT 16
+#define B2R2_INS_VC1R_SHIFT 17
+#define B2R2_INS_ROTATION_SHIFT 18
+#define B2R2_INS_PACE_DOWN_SHIFT 30
+#define B2R2_INS_BLITCOMPIRQ_SHIFT 31
+enum b2r2_ins {
+	/* Source 1 config */
+	B2R2_INS_SOURCE_1_FETCH_FROM_MEM =      0x1 << B2R2_INS_SOURCE_1_SHIFT,
+	B2R2_INS_SOURCE_1_COLOR_FILL_REGISTER = 0x3 << B2R2_INS_SOURCE_1_SHIFT,
+	B2R2_INS_SOURCE_1_DIRECT_COPY =         0x4 << B2R2_INS_SOURCE_1_SHIFT,
+	B2R2_INS_SOURCE_1_DIRECT_FILL =         0x7 << B2R2_INS_SOURCE_1_SHIFT,
+
+	/* Source 2 config */
+	B2R2_INS_SOURCE_2_FETCH_FROM_MEM =      0x1 << B2R2_INS_SOURCE_2_SHIFT,
+	B2R2_INS_SOURCE_2_COLOR_FILL_REGISTER = 0x3 << B2R2_INS_SOURCE_2_SHIFT,
+
+	/* Source 3 config */
+	B2R2_INS_SOURCE_3_FETCH_FROM_MEM =      0x1 << B2R2_INS_SOURCE_3_SHIFT,
+
+	/* Other configs */
+	B2R2_INS_IVMX_ENABLED =               0x1 << B2R2_INS_IVMX_SHIFT,
+	B2R2_INS_CLUTOP_ENABLED =             0x1 << B2R2_INS_CLUTOP_SHIFT,
+	B2R2_INS_RESCALE2D_ENABLED =          0x1 << B2R2_INS_RESCALE2D_SHIFT,
+	B2R2_INS_FLICK_FILT_ENABLED =         0x1 << B2R2_INS_FLICK_FILT_SHIFT,
+	B2R2_INS_RECT_CLIP_ENABLED =          0x1 << B2R2_INS_RECT_CLIP_SHIFT,
+	B2R2_INS_CKEY_ENABLED =               0x1 << B2R2_INS_CKEY_SHIFT,
+	B2R2_INS_OVMX_ENABLED =               0x1 << B2R2_INS_OVMX_SHIFT,
+	B2R2_INS_DEI_ENABLED =                0x1 << B2R2_INS_DEI_SHIFT,
+	B2R2_INS_PLANE_MASK_ENABLED =         0x1 << B2R2_INS_PLANE_MASK_SHIFT,
+	B2R2_INS_XYL_ENABLED =                0x1 << B2R2_INS_XYL_SHIFT,
+	B2R2_INS_DOT_ENABLED =                0x1 << B2R2_INS_DOT_SHIFT,
+	B2R2_INS_VC1R_ENABLED =               0x1 << B2R2_INS_VC1R_SHIFT,
+	B2R2_INS_ROTATION_ENABLED =           0x1 << B2R2_INS_ROTATION_SHIFT,
+	B2R2_INS_PACE_DOWN_ENABLED =          0x1 << B2R2_INS_PACE_DOWN_SHIFT,
+	B2R2_INS_BLITCOMPIRQ_ENABLED =        0x1 << B2R2_INS_BLITCOMPIRQ_SHIFT,
+
+};
+
+/* B2R2_ACK register values */
+#define B2R2_ACK_MODE_SHIFT 0
+#define B2R2_ACK_SWAP_FG_BG_SHIFT 4
+#define B2R2_ACK_GALPHA_ROPID_SHIFT 8
+#define B2R2_ACK_CKEY_BLUE_SHIFT 16
+#define B2R2_ACK_CKEY_GREEN_SHIFT 18
+#define B2R2_ACK_CKEY_RED_SHIFT 20
+#define B2R2_ACK_CKEY_SEL_SHIFT 22
+enum b2r2_ack {
+	/* ALU operation modes */
+	B2R2_ACK_MODE_LOGICAL_OPERATION =            0x1 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_BLEND_NOT_PREMULT =            0x2 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_BLEND_PREMULT =                0x3 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_LOGICAL_FIRST_PASS =  0x4 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_BLEND =               0x5 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_BYPASS_S2_S3 =                 0x7 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_LOGICAL_SECOND_PASS = 0x8 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_XYL_LOGICAL =         0x9 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_XYL_BLEND_NOT_PREMULT =
+						     0xa << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_XYL_BLEND_PREMULT =   0xb << B2R2_ACK_MODE_SHIFT,
+
+	/* ALU channel selection */
+	B2R2_ACK_SWAP_FG_BG =                  0x1 << B2R2_ACK_SWAP_FG_BG_SHIFT,
+
+	/* Global alpha and ROP IDs */
+	B2R2_ACK_ROP_CLEAR =                 0x0 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_AND =                   0x1 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_AND_REV =               0x2 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_COPY =                  0x3 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_AND_INV =               0x4 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_NOOP =                  0x5 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_XOR =                   0x6 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_OR =                    0x7 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_NOR =                   0x8 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_EQUIV =                 0x9 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_INVERT =                0xa << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_OR_REV =                0xb << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_COPY_INV =              0xc << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_OR_INV =                0xd << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_NAND =                  0xe << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_SET =                   0xf << B2R2_ACK_GALPHA_ROPID_SHIFT,
+
+	/* Color key configuration bits */
+	B2R2_ACK_CKEY_BLUE_MATCH_IF_BETWEEN =  0x1 << B2R2_ACK_CKEY_BLUE_SHIFT,
+	B2R2_ACK_CKEY_BLUE_MATCH_IF_LT_OR_GT = 0x2 << B2R2_ACK_CKEY_BLUE_SHIFT,
+	B2R2_ACK_CKEY_RED_MATCH_IF_BETWEEN =   0x1 << B2R2_ACK_CKEY_GREEN_SHIFT,
+	B2R2_ACK_CKEY_RED_MATCH_IF_LT_OR_GT =  0x2 << B2R2_ACK_CKEY_GREEN_SHIFT,
+	B2R2_ACK_CKEY_GREEN_MATCH_IF_BETWEEN = 0x1 << B2R2_ACK_CKEY_RED_SHIFT,
+	B2R2_ACK_CKEY_GREEN_MATCH_IF_LT_OR_GT = 0x2 << B2R2_ACK_CKEY_RED_SHIFT,
+
+	/* Color key input selection */
+	B2R2_ACK_CKEY_SEL_DEST =               0x0 << B2R2_ACK_CKEY_SEL_SHIFT,
+	B2R2_ACK_CKEY_SEL_SRC_BEFORE_CLUT =    0x1 << B2R2_ACK_CKEY_SEL_SHIFT,
+	B2R2_ACK_CKEY_SEL_SRC_AFTER_CLUT =     0x2 << B2R2_ACK_CKEY_SEL_SHIFT,
+	B2R2_ACK_CKEY_SEL_BLANKING_S2_ALPHA =  0x3 << B2R2_ACK_CKEY_SEL_SHIFT,
+};
+
+/* Common <S/T>TY defines */
+#define B2R2_TY_BITMAP_PITCH_SHIFT 0
+#define B2R2_TY_COLOR_FORM_SHIFT 16
+#define B2R2_TY_ALPHA_RANGE_SHIFT 21
+#define B2R2_TY_MB_ACCESS_MODE_SHIFT 23
+#define B2R2_TY_HSO_SHIFT 24
+#define B2R2_TY_VSO_SHIFT 25
+#define B2R2_TY_SUBBYTE_SHIFT 28
+#define B2R2_TY_ENDIAN_SHIFT 30
+#define B2R2_TY_SECURE_SHIFT 31
+
+/* Dummy enum for generalization of <S/T>TY registers */
+enum b2r2_ty {
+	/* Alpha range */
+	B2R2_TY_ALPHA_RANGE_128 = 0x0 << B2R2_TY_ALPHA_RANGE_SHIFT,
+	B2R2_TY_ALPHA_RANGE_255 = 0x1 << B2R2_TY_ALPHA_RANGE_SHIFT,
+
+	/* Access mode in macro-block organized frame buffers */
+	B2R2_TY_MB_ACCESS_MODE_FRAME = 0x0 << B2R2_TY_MB_ACCESS_MODE_SHIFT,
+	B2R2_TY_MB_ACCESS_MODE_FIELD = 0x1 << B2R2_TY_MB_ACCESS_MODE_SHIFT,
+
+	/* Horizontal scan order */
+	B2R2_TY_HSO_LEFT_TO_RIGHT = 0x0 << B2R2_TY_HSO_SHIFT,
+	B2R2_TY_HSO_RIGHT_TO_LEFT = 0x1 << B2R2_TY_HSO_SHIFT,
+
+	/* Vertical scan order */
+	B2R2_TY_VSO_TOP_TO_BOTTOM = 0x0 << B2R2_TY_VSO_SHIFT,
+	B2R2_TY_VSO_BOTTOM_TO_TOP = 0x1 << B2R2_TY_VSO_SHIFT,
+
+	/* Pixel ordering for sub-byte formats (position of right-most pixel) */
+	B2R2_TY_SUBBYTE_MSB = 0x0 << B2R2_TY_SUBBYTE_SHIFT,
+	B2R2_TY_SUBBYTE_LSB = 0x1 << B2R2_TY_SUBBYTE_SHIFT,
+
+	/* Bitmap endianess */
+	B2R2_TY_ENDIAN_BIG_NOT_LITTLE = 0x1 << B2R2_TY_ENDIAN_SHIFT,
+
+	/* Secureness of the target memory region */
+	B2R2_TY_SECURE_UNSECURE = 0x0 << B2R2_TY_SECURE_SHIFT,
+	B2R2_TY_SECURE_SECURE =   0x1 << B2R2_TY_SECURE_SHIFT,
+
+	/* Dummy to make sure the data type is large enough */
+	B2R2_TY_DUMMY = 0xffffffff,
+};
+
+/* B2R2_TTY register values */
+#define B2R2_TTY_CB_NOT_CR_SHIFT 22
+#define B2R2_TTY_RGB_ROUND_SHIFT 26
+#define B2R2_TTY_CHROMA_NOT_LUMA_SHIFT 27
+enum b2r2_tty {
+
+	/* Chroma component selection */
+	B2R2_TTY_CB_NOT_CR = 0x1 << B2R2_TTY_CB_NOT_CR_SHIFT,
+
+	/* RGB rounding mode */
+	B2R2_TTY_RGB_ROUND_NORMAL = 0x0 << B2R2_TTY_RGB_ROUND_SHIFT,
+	B2R2_TTY_RGB_ROUND_DITHER = 0x1 << B2R2_TTY_RGB_ROUND_SHIFT,
+
+	/* Component selection for splitted frame buffer formats */
+	B2R2_TTY_CHROMA_NOT_LUMA = 0x1 << B2R2_TTY_CHROMA_NOT_LUMA_SHIFT,
+};
+
+/* B2R2_S1TY register values */
+#define B2R2_S1TY_A1_SUBST_SHIFT 22
+#define B2R2_S1TY_ROTATION_SHIFT 27
+#define B2R2_S1TY_RGB_EXPANSION_SHIFT 29
+enum b2r2_s1ty {
+
+	/* Alpha bit substitution mode for ARGB1555 */
+	B2R2_S1TY_A1_SUBST_KEY_MODE = 0x1 << B2R2_S1TY_A1_SUBST_SHIFT,
+
+	/* Input rectangle rotation (NOT YET IMPLEMENTED) */
+	B2R2_S1TY_ENABLE_ROTATION = 0x1 << B2R2_S1TY_ROTATION_SHIFT,
+
+	/* RGB expansion mode */
+	B2R2_S1TY_RGB_EXPANSION_MSB_DUP = 0x0  << B2R2_S1TY_RGB_EXPANSION_SHIFT,
+	B2R2_S1TY_RGB_EXPANSION_LSP_ZERO = 0x1 << B2R2_S1TY_RGB_EXPANSION_SHIFT,
+};
+
+/* B2R2_S1TY register values */
+#define B2R2_S2TY_A1_SUBST_SHIFT 22
+#define B2R2_S2TY_CHROMA_LEFT_SHIFT 26
+#define B2R2_S2TY_RGB_EXPANSION_SHIFT 29
+enum b2r2_s2ty {
+
+	/* Alpha bit substitution mode for ARGB1555 */
+	B2R2_S2TY_A1_SUBST_KEY_MODE = 0x1 << B2R2_S2TY_A1_SUBST_SHIFT,
+
+	/* Chroma left extension */
+	B2R2_S2TY_CHROMA_LEFT_EXT_FOLLOWING_PIXEL = 0x0
+						<< B2R2_S2TY_CHROMA_LEFT_SHIFT,
+	B2R2_S2TY_CHROMA_LEFT_EXT_AVERAGE = 0x1 << B2R2_S2TY_CHROMA_LEFT_SHIFT,
+
+	/* RGB expansion mode */
+	B2R2_S2TY_RGB_EXPANSION_MSB_DUP = 0x0  << B2R2_S2TY_RGB_EXPANSION_SHIFT,
+	B2R2_S2TY_RGB_EXPANSION_LSP_ZERO = 0x1 << B2R2_S2TY_RGB_EXPANSION_SHIFT,
+};
+
+/* B2R2_S1TY register values */
+#define B2R2_S3TY_BLANK_ACC_SHIFT 26
+enum b2r2_s3ty {
+	/* Enables "blank" access on this source (nothing will be fetched from
+	   memory) */
+	B2R2_S3TY_ENABLE_BLANK_ACCESS = 0x1 << B2R2_S3TY_BLANK_ACC_SHIFT,
+};
+
+/* B2R2_<S or T>XY register values */
+#define B2R2_XY_X_SHIFT 0
+#define B2R2_XY_Y_SHIFT 16
+
+/* B2R2_<S or T>SZ register values */
+#define B2R2_SZ_WIDTH_SHIFT 0
+#define B2R2_SZ_HEIGHT_SHIFT 16
+
+/* Clip window offset (top left coordinates) */
+#define B2R2_CWO_X_SHIFT 0
+#define B2R2_CWO_Y_SHIFT 16
+
+/* Clip window stop (bottom right coordinates) */
+#define B2R2_CWS_X_SHIFT 0
+#define B2R2_CWS_Y_SHIFT 16
+
+/* Color look-up table */
+enum b2r2_cco {
+	B2R2_CCO_CLUT_COLOR_CORRECTION = (1 << 16),
+	B2R2_CCO_CLUT_UPDATE = (1 << 18),
+	B2R2_CCO_CLUT_ON_S1 = (1 << 15)
+};
+
+/* Filter control (2D resize control) */
+enum b2r2_fctl {
+	/* Horizontal 2D filter mode */
+	B2R2_FCTL_HF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER = BIT(0),
+	B2R2_FCTL_HF2D_MODE_ENABLE_ALPHA_CHANNEL_FILTER = BIT(1),
+	B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER = BIT(2),
+
+	/* Vertical 2D filter mode */
+	B2R2_FCTL_VF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER = BIT(4),
+	B2R2_FCTL_VF2D_MODE_ENABLE_ALPHA_CHANNEL_FILTER = BIT(5),
+	B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER = BIT(6),
+
+	/* Alpha borders */
+	B2R2_FCTL_ENABLE_ALPHA_BORDER_RIGHT = BIT(12),
+	B2R2_FCTL_ENABLE_ALPHA_BORDER_LEFT = BIT(13),
+	B2R2_FCTL_ENABLE_ALPHA_BORDER_BOTTOM = BIT(14),
+	B2R2_FCTL_ENABLE_ALPHA_BORDER_TOP = BIT(15),
+
+	/* Luma path horizontal 2D filter mode */
+	B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_FILTER = BIT(24),
+	B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_RESIZER = BIT(25),
+
+	/* Luma path vertical 2D filter mode */
+	B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_FILTER = BIT(28),
+	B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_RESIZER = BIT(29),
+};
+
+/* Resize scaling factor */
+#define B2R2_RSF_HSRC_INC_SHIFT 0
+#define B2R2_RSF_VSRC_INC_SHIFT 16
+
+/* Resizer initialization */
+#define B2R2_RZI_HSRC_INIT_SHIFT 0
+#define B2R2_RZI_HNB_REPEAT_SHIFT 12
+#define B2R2_RZI_VSRC_INIT_SHIFT 16
+#define B2R2_RZI_VNB_REPEAT_SHIFT 28
+
+/* Default values for the resizer */
+#define B2R2_RZI_DEFAULT_HNB_REPEAT (3 << B2R2_RZI_HNB_REPEAT_SHIFT)
+#define B2R2_RZI_DEFAULT_VNB_REPEAT (3 << B2R2_RZI_VNB_REPEAT_SHIFT)
+
+
+/* Bus plug configuration registers */
+enum b2r2_plug_opcode_size {
+	B2R2_PLUG_OPCODE_SIZE_8 =  0x3,
+	B2R2_PLUG_OPCODE_SIZE_16 = 0x4,
+	B2R2_PLUG_OPCODE_SIZE_32 = 0x5,
+	B2R2_PLUG_OPCODE_SIZE_64 = 0x6,
+};
+
+enum b2r2_plug_chunk_size {
+	B2R2_PLUG_CHUNK_SIZE_1 =   0x0,
+	B2R2_PLUG_CHUNK_SIZE_2 =   0x1,
+	B2R2_PLUG_CHUNK_SIZE_4 =   0x2,
+	B2R2_PLUG_CHUNK_SIZE_8 =   0x3,
+	B2R2_PLUG_CHUNK_SIZE_16 =  0x4,
+	B2R2_PLUG_CHUNK_SIZE_32 =  0x5,
+	B2R2_PLUG_CHUNK_SIZE_64 =  0x6,
+	B2R2_PLUG_CHUNK_SIZE_128 = 0x7,
+};
+
+enum b2r2_plug_message_size {
+	B2R2_PLUG_MESSAGE_SIZE_1 =   0x0,
+	B2R2_PLUG_MESSAGE_SIZE_2 =   0x1,
+	B2R2_PLUG_MESSAGE_SIZE_4 =   0x2,
+	B2R2_PLUG_MESSAGE_SIZE_8 =   0x3,
+	B2R2_PLUG_MESSAGE_SIZE_16 =  0x4,
+	B2R2_PLUG_MESSAGE_SIZE_32 =  0x5,
+	B2R2_PLUG_MESSAGE_SIZE_64 =  0x6,
+	B2R2_PLUG_MESSAGE_SIZE_128 = 0x7,
+};
+
+enum b2r2_plug_page_size {
+	B2R2_PLUG_PAGE_SIZE_64 =  0x0,
+	B2R2_PLUG_PAGE_SIZE_128 = 0x1,
+	B2R2_PLUG_PAGE_SIZE_256 = 0x2,
+};
+
+/* Default opcode size */
+#if defined(CONFIG_B2R2_OPSIZE_8)
+#  define B2R2_PLUG_OPCODE_SIZE_DEFAULT B2R2_PLUG_OPCODE_SIZE_8
+#elif defined(CONFIG_B2R2_OPSIZE_16)
+#  define B2R2_PLUG_OPCODE_SIZE_DEFAULT B2R2_PLUG_OPCODE_SIZE_16
+#elif defined(CONFIG_B2R2_OPSIZE_32)
+#  define B2R2_PLUG_OPCODE_SIZE_DEFAULT B2R2_PLUG_OPCODE_SIZE_32
+#elif defined(CONFIG_B2R2_OPSIZE_64)
+#  define B2R2_PLUG_OPCODE_SIZE_DEFAULT B2R2_PLUG_OPCODE_SIZE_64
+#else
+#  define B2R2_PLUG_OPCODE_SIZE_DEFAULT 0
+#endif
+
+/* Default chunk size */
+#if defined(CONFIG_B2R2_CHSIZE_1)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_1
+#elif defined(CONFIG_B2R2_CHSIZE_2)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_2
+#elif defined(CONFIG_B2R2_CHSIZE_4)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_4
+#elif defined(CONFIG_B2R2_CHSIZE_8)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_8
+#elif defined(CONFIG_B2R2_CHSIZE_16)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_16
+#elif defined(CONFIG_B2R2_CHSIZE_32)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_32
+#elif defined(CONFIG_B2R2_CHSIZE_64)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_64
+#elif defined(CONFIG_B2R2_CHSIZE_128)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_128
+#else
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT 0
+#endif
+
+/* Default message size */
+#if defined(CONFIG_B2R2_MGSIZE_1)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_1
+#elif defined(CONFIG_B2R2_MGSIZE_2)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_2
+#elif defined(CONFIG_B2R2_MGSIZE_4)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_4
+#elif defined(CONFIG_B2R2_MGSIZE_8)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_8
+#elif defined(CONFIG_B2R2_MGSIZE_16)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_16
+#elif defined(CONFIG_B2R2_MGSIZE_32)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_32
+#elif defined(CONFIG_B2R2_MGSIZE_64)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_64
+#elif defined(CONFIG_B2R2_MGSIZE_128)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_128
+#else
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT 0
+#endif
+
+/* Default page size */
+#if defined(CONFIG_B2R2_PGSIZE_64)
+#  define B2R2_PLUG_PAGE_SIZE_DEFAULT B2R2_PLUG_PAGE_SIZE_64
+#elif defined(CONFIG_B2R2_PGSIZE_128)
+#  define B2R2_PLUG_PAGE_SIZE_DEFAULT B2R2_PLUG_PAGE_SIZE_128
+#elif defined(CONFIG_B2R2_PGSIZE_256)
+#  define B2R2_PLUG_PAGE_SIZE_DEFAULT B2R2_PLUG_PAGE_SIZE_256
+#else
+#  define B2R2_PLUG_PAGE_SIZE_DEFAULT 0
+#endif
+
+/*  VMX register values for RGB to YUV color conversion */
+/*  Magic numbers from 27.11 in DB8500_DesignSpecification_v2.5.pdf */
+
+/* 601 Video Matrix (standard 601 conversion) */
+#define B2R2_VMX0_RGB_TO_YUV_601_VIDEO 0x107e4beb
+#define B2R2_VMX1_RGB_TO_YUV_601_VIDEO 0x0982581d
+#define B2R2_VMX2_RGB_TO_YUV_601_VIDEO 0xfa9ea483
+#define B2R2_VMX3_RGB_TO_YUV_601_VIDEO 0x08000080
+
+/* 601 Gfx Matrix (full range conversion) */
+#define B2R2_VMX0_RGB_TO_YUV_601_GFX 0x0e1e8bee
+#define B2R2_VMX1_RGB_TO_YUV_601_GFX 0x08420419
+#define B2R2_VMX2_RGB_TO_YUV_601_GFX 0xfb5ed471
+#define B2R2_VMX3_RGB_TO_YUV_601_GFX 0x08004080
+
+/* 709 Video Matrix (standard 709 conversion) */
+#define B2R2_VMX0_RGB_TO_YUV_709_VIDEO 0x107e27f4
+#define B2R2_VMX1_RGB_TO_YUV_709_VIDEO 0x06e2dc13
+#define B2R2_VMX2_RGB_TO_YUV_709_VIDEO 0xfc5e6c83
+#define B2R2_VMX3_RGB_TO_YUV_709_VIDEO 0x08000080
+
+/* 709 Gfx Matrix (standard 709 conversion) */
+#define B2R2_VMX0_RGB_TO_YUV_709_GFX 0x0e3e6bf5
+#define B2R2_VMX1_RGB_TO_YUV_709_GFX 0x05e27410
+#define B2R2_VMX2_RGB_TO_YUV_709_GFX 0xfcdea471
+#define B2R2_VMX3_RGB_TO_YUV_709_GFX 0x08004080
+
+/* VMX register values for YUV to RGB color conversion */
+
+/* 601 Video Matrix (standard 601 conversion) */
+#define B2R2_VMX0_YUV_TO_RGB_601_VIDEO 0x2c440000
+#define B2R2_VMX1_YUV_TO_RGB_601_VIDEO 0xe9a403aa
+#define B2R2_VMX2_YUV_TO_RGB_601_VIDEO 0x0004013f
+#define B2R2_VMX3_YUV_TO_RGB_601_VIDEO 0x34f21322
+
+/* 601 Gfx Matrix (full range conversion) */
+#define B2R2_VMX0_YUV_TO_RGB_601_GFX 0x3324a800
+#define B2R2_VMX1_YUV_TO_RGB_601_GFX 0xe604ab9c
+#define B2R2_VMX2_YUV_TO_RGB_601_GFX 0x0004a957
+#define B2R2_VMX3_YUV_TO_RGB_601_GFX 0x32121eeb
+
+/* 709 Video Matrix (standard 709 conversion) */
+#define B2R2_VMX0_YUV_TO_RGB_709_VIDEO 0x31440000
+#define B2R2_VMX1_YUV_TO_RGB_709_VIDEO 0xf16403d1
+#define B2R2_VMX2_YUV_TO_RGB_709_VIDEO 0x00040145
+#define B2R2_VMX3_YUV_TO_RGB_709_VIDEO 0x33b14b18
+
+/* 709 Gfx Matrix (standard 709 conversion) */
+#define B2R2_VMX0_YUV_TO_RGB_709_GFX 0x3964a800
+#define B2R2_VMX1_YUV_TO_RGB_709_GFX 0xef04abc9
+#define B2R2_VMX2_YUV_TO_RGB_709_GFX 0x0004a95f
+#define B2R2_VMX3_YUV_TO_RGB_709_GFX 0x307132df
+
+/* VMX register values for RGB to BGR conversion */
+#define B2R2_VMX0_RGB_TO_BGR 0x00000100
+#define B2R2_VMX1_RGB_TO_BGR 0x00040000
+#define B2R2_VMX2_RGB_TO_BGR 0x20000000
+#define B2R2_VMX3_RGB_TO_BGR 0x00000000
+
+/* VMX register values for BGR to YUV color conversion */
+/* Note: All BGR -> YUV values are calculated by multiplying
+ * the RGB -> YUV matrices [A], with [S] to form [A]x[S] where
+ *     |0 0 1|
+ * S = |0 1 0|
+ *     |1 0 0|
+ * Essentially swapping first and third columns in
+ * the matrices (VMX0, VMX1 and VMX2 values).
+ * The offset vector VMX3 remains untouched.
+ * Put another way, the value of bits 0 through 9
+ * is swapped with the value of
+ * bits 20 through 31 in VMX0, VMX1 and VMX2,
+ * taking into consideration the compression
+ * that is used on bits 0 through 9. Bit 0 being LSB.
+ */
+
+/* 601 Video Matrix (standard 601 conversion) */
+#define B2R2_VMX0_BGR_TO_YUV_601_VIDEO 0xfd7e4883
+#define B2R2_VMX1_BGR_TO_YUV_601_VIDEO 0x03a2584c
+#define B2R2_VMX2_BGR_TO_YUV_601_VIDEO 0x107ea7d4
+#define B2R2_VMX3_BGR_TO_YUV_601_VIDEO 0x08000080
+
+/* 601 Gfx Matrix (full range conversion) */
+#define B2R2_VMX0_BGR_TO_YUV_601_GFX 0xfdde8870
+#define B2R2_VMX1_BGR_TO_YUV_601_GFX 0x03220442
+#define B2R2_VMX2_BGR_TO_YUV_601_GFX 0x0e3ed7da
+#define B2R2_VMX3_BGR_TO_YUV_601_GFX 0x08004080
+
+/* 709 Video Matrix (standard 709 conversion) */
+#define B2R2_VMX0_BGR_TO_YUV_709_VIDEO 0xfe9e2483
+#define B2R2_VMX1_BGR_TO_YUV_709_VIDEO 0x0262dc37
+#define B2R2_VMX2_BGR_TO_YUV_709_VIDEO 0x107e6fe2
+#define B2R2_VMX3_BGR_TO_YUV_709_VIDEO 0x08000080
+
+/* 709 Gfx Matrix (standard 709 conversion) */
+#define B2R2_VMX0_BGR_TO_YUV_709_GFX 0xfebe6871
+#define B2R2_VMX1_BGR_TO_YUV_709_GFX 0x0202742f
+#define B2R2_VMX2_BGR_TO_YUV_709_GFX 0x0e3ea7e6
+#define B2R2_VMX3_BGR_TO_YUV_709_GFX 0x08004080
+
+
+/* VMX register values for YUV to BGR conversion */
+/*  Note: All YUV -> BGR values are constructed
+ * from the YUV -> RGB ones, by swapping
+ * first and third rows in the matrix
+ * (VMX0 and VMX2 values). Further, the first and
+ * third values in the offset vector need to be
+ * swapped as well, i.e. bits 0 through 9 are swapped
+ * with bits 20 through 29 in the VMX3 value.
+ * Bit 0 being LSB.
+ */
+
+/* 601 Video Matrix (standard 601 conversion) */
+#define B2R2_VMX0_YUV_TO_BGR_601_VIDEO 0x0004013f
+#define B2R2_VMX1_YUV_TO_BGR_601_VIDEO 0xe9a403aa
+#define B2R2_VMX2_YUV_TO_BGR_601_VIDEO 0x2c440000
+#define B2R2_VMX3_YUV_TO_BGR_601_VIDEO 0x3222134f
+
+/* 601 Gfx Matrix (full range conversion) */
+#define B2R2_VMX0_YUV_TO_BGR_601_GFX 0x0004a957
+#define B2R2_VMX1_YUV_TO_BGR_601_GFX 0xe604ab9c
+#define B2R2_VMX2_YUV_TO_BGR_601_GFX 0x3324a800
+#define B2R2_VMX3_YUV_TO_BGR_601_GFX 0x2eb21f21
+
+/* 709 Video Matrix (standard 709 conversion) */
+#define B2R2_VMX0_YUV_TO_BGR_709_VIDEO 0x00040145
+#define B2R2_VMX1_YUV_TO_BGR_709_VIDEO 0xf16403d1
+#define B2R2_VMX2_YUV_TO_BGR_709_VIDEO 0x31440000
+#define B2R2_VMX3_YUV_TO_BGR_709_VIDEO 0x31814b3b
+
+/* 709 Gfx Matrix (standard 709 conversion) */
+#define B2R2_VMX0_YUV_TO_BGR_709_GFX 0x0004a95f
+#define B2R2_VMX1_YUV_TO_BGR_709_GFX 0xef04abc9
+#define B2R2_VMX2_YUV_TO_BGR_709_GFX 0x3964a800
+#define B2R2_VMX3_YUV_TO_BGR_709_GFX 0x2df13307
+
+
+/* VMX register values for YVU to RGB conversion */
+
+/* 601 Video Matrix (standard 601 conversion) */
+#define B2R2_VMX0_YVU_TO_RGB_601_VIDEO 0x00040120
+#define B2R2_VMX1_YVU_TO_RGB_601_VIDEO 0xF544034D
+#define B2R2_VMX2_YVU_TO_RGB_601_VIDEO 0x37840000
+#define B2R2_VMX3_YVU_TO_RGB_601_VIDEO 0x34f21322
+
+/* VMX register values for RGB to YVU conversion */
+
+/* 601 Video Matrix (standard 601 conversion) */
+#define B2R2_VMX0_RGB_TO_YVU_601_VIDEO 0xfa9ea483
+#define B2R2_VMX1_RGB_TO_YVU_601_VIDEO 0x0982581d
+#define B2R2_VMX2_RGB_TO_YVU_601_VIDEO 0x107e4beb
+#define B2R2_VMX3_RGB_TO_YVU_601_VIDEO 0x08000080
+
+/* VMX register values for YVU to BGR conversion */
+
+/* 601 Video Matrix (standard 601 conversion) */
+#define B2R2_VMX0_YVU_TO_BGR_601_VIDEO 0x37840000
+#define B2R2_VMX1_YVU_TO_BGR_601_VIDEO 0xF544034D
+#define B2R2_VMX2_YVU_TO_BGR_601_VIDEO 0x00040120
+#define B2R2_VMX3_YVU_TO_BGR_601_VIDEO 0x3222134F
+
+/* VMX register values for BGR to YVU conversion */
+
+/* 601 Video Matrix (standard 601 conversion) */
+#define B2R2_VMX0_BGR_TO_YVU_601_VIDEO 0x107ea7d4
+#define B2R2_VMX1_BGR_TO_YVU_601_VIDEO 0x03a2584c
+#define B2R2_VMX2_BGR_TO_YVU_601_VIDEO 0xfd7e4883
+#define B2R2_VMX3_BGR_TO_YVU_601_VIDEO 0x08000080
+
+/* VMX register values for YVU to YUV conversion */
+
+/* 601 Video Matrix (standard 601 conversion) */
+/* Internally, the components are in fact stored
+ * with luma in the middle, i.e. UYV, which is why
+ * the values are just like for RGB->BGR conversion.
+ */
+#define B2R2_VMX0_YVU_TO_YUV_601_VIDEO 0x00000100
+#define B2R2_VMX1_YVU_TO_YUV_601_VIDEO 0x00040000
+#define B2R2_VMX2_YVU_TO_YUV_601_VIDEO 0x20000000
+#define B2R2_VMX3_YVU_TO_YUV_601_VIDEO 0x00000000
+
+/* VMX register values for RGB to BLT_YUV888 conversion */
+
+/* 601 Video Matrix (standard 601 conversion) */
+/*
+ * BLT_YUV888 has color components laid out in memory as V, U, Y, (Alpha)
+ * with V at the first byte (due to little endian addressing).
+ * B2R2 expects them to be as U, Y, V, (A)
+ * with U at the first byte.
+ * Note: RGB -> BLT_YUV888 values are calculated by multiplying
+ * the RGB -> YUV matrix [A], with [S] to form [S]x[A] where
+ *     |0 1 0|
+ * S = |0 0 1|
+ *     |1 0 0|
+ * Essentially changing the order of rows in the original
+ * matrix [A].
+ * row1 -> row3
+ * row2 -> row1
+ * row3 -> row2
+ * Values in the offset vector are swapped in the same manner.
+ */
+#define B2R2_VMX0_RGB_TO_BLT_YUV888_601_VIDEO 0x0982581d
+#define B2R2_VMX1_RGB_TO_BLT_YUV888_601_VIDEO 0xfa9ea483
+#define B2R2_VMX2_RGB_TO_BLT_YUV888_601_VIDEO 0x107e4beb
+#define B2R2_VMX3_RGB_TO_BLT_YUV888_601_VIDEO 0x00020080
+
+/* VMX register values for BLT_YUV888 to RGB conversion */
+
+/*
+ * Note: BLT_YUV888 -> RGB values are calculated by multiplying
+ * the YUV -> RGB matrix [A], with [S] to form [A]x[S] where
+ *     |0 0 1|
+ * S = |1 0 0|
+ *     |0 1 0|
+ * Essentially changing the order of columns in the original
+ * matrix [A].
+ * col1 -> col3
+ * col2 -> col1
+ * col3 -> col2
+ * Values in the offset vector remain unchanged.
+ */
+#define B2R2_VMX0_BLT_YUV888_TO_RGB_601_VIDEO 0x20000121
+#define B2R2_VMX1_BLT_YUV888_TO_RGB_601_VIDEO 0x201ea74c
+#define B2R2_VMX2_BLT_YUV888_TO_RGB_601_VIDEO 0x2006f000
+#define B2R2_VMX3_BLT_YUV888_TO_RGB_601_VIDEO 0x34f21322
+
+#endif /* B2R2_HW_H__ */
diff --git a/drivers/video/b2r2/b2r2_input_validation.c b/drivers/video/b2r2/b2r2_input_validation.c
new file mode 100644
index 00000000000..2604d8f77e0
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_input_validation.c
@@ -0,0 +1,417 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson
+ *
+ * This program is free software; you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any later
+ * version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc., 59 Temple
+ * Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+
+#include "b2r2_input_validation.h"
+
+#include "b2r2_debug.h"
+#include "b2r2_utils.h"
+
+#include <video/b2r2_blt.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+
+
+static bool is_valid_format(enum b2r2_blt_fmt fmt);
+
+static bool is_valid_pitch_for_fmt(u32 pitch, s32 width,
+		enum b2r2_blt_fmt fmt);
+
+static bool is_aligned_width_for_fmt(s32 width, enum b2r2_blt_fmt fmt);
+static s32 width_2_complete_width(s32 width, enum b2r2_blt_fmt fmt);
+static bool is_complete_width_for_fmt(s32 width, enum b2r2_blt_fmt fmt);
+static bool is_valid_height_for_fmt(s32 height, enum b2r2_blt_fmt fmt);
+
+static bool validate_img(struct b2r2_blt_img *img);
+static bool validate_rect(struct b2r2_blt_rect *rect);
+
+
+static bool is_valid_format(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+
+static bool is_valid_pitch_for_fmt(u32 pitch, s32 width, enum b2r2_blt_fmt fmt)
+{
+	s32 complete_width;
+	u32 pitch_derived_from_width;
+
+	complete_width =  width_2_complete_width(width, fmt);
+
+	pitch_derived_from_width =
+			b2r2_calc_pitch_from_width(complete_width, fmt);
+
+	if (pitch < pitch_derived_from_width)
+		return false;
+
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+		if (!b2r2_is_aligned(pitch, 2))
+			return false;
+
+		break;
+
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		if (!b2r2_is_aligned(pitch, 4))
+			return false;
+
+		break;
+
+	default:
+		break;
+	}
+
+	return true;
+}
+
+
+static bool is_aligned_width_for_fmt(s32 width, enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+		if (!b2r2_is_aligned(width, 4))
+			return false;
+
+		break;
+
+	case B2R2_BLT_FMT_1_BIT_A1:
+		if (!b2r2_is_aligned(width, 8))
+			return false;
+
+		break;
+
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		if (!b2r2_is_aligned(width, 2))
+			return false;
+
+		break;
+
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static s32 width_2_complete_width(s32 width, enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return b2r2_align_up(width, 2);
+
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return b2r2_align_up(width, 16);
+
+	default:
+		return width;
+	}
+}
+
+static bool is_complete_width_for_fmt(s32 width, enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		if (!b2r2_is_aligned(width, 2))
+			return false;
+
+		break;
+
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		if (!b2r2_is_aligned(width, 16))
+			return false;
+
+		break;
+
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static bool is_valid_height_for_fmt(s32 height, enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+		if (!b2r2_is_aligned(height, 2))
+			return false;
+
+		break;
+
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		if (!b2r2_is_aligned(height, 16))
+			return false;
+
+		break;
+
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static bool validate_img(struct b2r2_blt_img *img)
+{
+	/*
+	 * So that we always can do width * height * bpp without overflowing a
+	 * 32 bit signed integer. isqrt(s32_max / max_bpp) was used to
+	 * calculate the value.
+	 */
+	static const s32 max_img_width_height = 8191;
+
+	s32 img_size;
+
+	if (!is_valid_format(img->fmt)) {
+		b2r2_log_info("Validation Error: !is_valid_format(img->fmt)\n");
+		return false;
+	}
+
+	if (img->width < 0 || img->width > max_img_width_height ||
+		img->height < 0 || img->height > max_img_width_height) {
+		b2r2_log_info("Validation Error: "
+				"img->width < 0 || "
+				"img->width > max_img_width_height || "
+				"img->height < 0 || "
+				"img->height > max_img_width_height\n");
+		return false;
+	}
+
+	if (b2r2_is_mb_fmt(img->fmt)) {
+		if (!is_complete_width_for_fmt(img->width, img->fmt)) {
+			b2r2_log_info("Validation Error: "
+					"!is_complete_width_for_fmt(img->width,"
+					" img->fmt)\n");
+			return false;
+		}
+	} else {
+		if (0 == img->pitch &&
+			(!is_aligned_width_for_fmt(img->width, img->fmt) ||
+			!is_complete_width_for_fmt(img->width, img->fmt))) {
+			b2r2_log_info("Validation Error: "
+					"0 == img->pitch && "
+					"(!is_aligned_width_for_fmt(img->width,"
+					" img->fmt) || "
+					"!is_complete_width_for_fmt(img->width,"
+					" img->fmt))\n");
+			return false;
+		}
+
+		if (img->pitch != 0 &&
+			!is_valid_pitch_for_fmt(img->pitch, img->width,
+					img->fmt)) {
+			b2r2_log_info("Validation Error: "
+					"img->pitch != 0 && "
+					"!is_valid_pitch_for_fmt(img->pitch, "
+					"img->width, img->fmt)\n");
+			return false;
+		}
+	}
+
+	if (!is_valid_height_for_fmt(img->width, img->fmt)) {
+		b2r2_log_info("Validation Error: "
+				"!is_valid_height_for_fmt(img->width, "
+				"img->fmt)\n");
+		return false;
+	}
+
+	img_size = b2r2_get_img_size(img);
+
+	/*
+	 * To keep the entire image inside s32 range.
+	 */
+	if ((B2R2_BLT_PTR_HWMEM_BUF_NAME_OFFSET == img->buf.type ||
+				B2R2_BLT_PTR_FD_OFFSET == img->buf.type) &&
+			img->buf.offset > (u32)b2r2_s32_max - (u32)img_size) {
+		b2r2_log_info("Validation Error: "
+				"(B2R2_BLT_PTR_HWMEM_BUF_NAME_OFFSET == "
+				"img->buf.type || B2R2_BLT_PTR_FD_OFFSET == "
+				"img->buf.type) && img->buf.offset > "
+				"(u32)B2R2_MAX_S32 - (u32)img_size\n");
+		return false;
+	}
+
+	return true;
+}
+
+static bool validate_rect(struct b2r2_blt_rect *rect)
+{
+	if (rect->width < 0 || rect->height < 0) {
+		b2r2_log_info("Validation Error: "
+				"rect->width < 0 || rect->height < 0\n");
+		return false;
+	}
+
+	return true;
+}
+
+bool b2r2_validate_user_req(struct b2r2_blt_req *req)
+{
+	bool is_src_img_used;
+	bool is_src_mask_used;
+	bool is_dst_clip_rect_used;
+
+	if (req->size != sizeof(struct b2r2_blt_req)) {
+		b2r2_log_err("Validation Error: "
+				"req->size != sizeof(struct b2r2_blt_req)\n");
+		return false;
+	}
+
+	is_src_img_used = !(req->flags & B2R2_BLT_FLAG_SOURCE_FILL ||
+				req->flags & B2R2_BLT_FLAG_SOURCE_FILL_RAW);
+	is_src_mask_used = req->flags & B2R2_BLT_FLAG_SOURCE_MASK;
+	is_dst_clip_rect_used = req->flags & B2R2_BLT_FLAG_DESTINATION_CLIP;
+
+	if (is_src_img_used || is_src_mask_used) {
+		if (!validate_rect(&req->src_rect)) {
+			b2r2_log_info("Validation Error: "
+					"!validate_rect(&req->src_rect)\n");
+			return false;
+		}
+	}
+
+	if (!validate_rect(&req->dst_rect)) {
+		b2r2_log_info("Validation Error: "
+				"!validate_rect(&req->dst_rect)\n");
+		return false;
+	}
+
+	if (is_dst_clip_rect_used) {
+		if (!validate_rect(&req->dst_clip_rect)) {
+			b2r2_log_info("Validation Error: "
+				"!validate_rect(&req->dst_clip_rect)\n");
+			return false;
+		}
+	}
+
+	if (is_src_img_used) {
+		struct b2r2_blt_rect src_img_bounding_rect;
+
+		if (!validate_img(&req->src_img)) {
+			b2r2_log_info("Validation Error: "
+					"!validate_img(&req->src_img)\n");
+			return false;
+		}
+
+		b2r2_get_img_bounding_rect(&req->src_img,
+				&src_img_bounding_rect);
+		if (!b2r2_is_rect_inside_rect(&req->src_rect,
+					&src_img_bounding_rect)) {
+			b2r2_log_info("Validation Error: "
+				"!b2r2_is_rect_inside_rect(&req->src_rect, "
+				"&src_img_bounding_rect)\n");
+			return false;
+		}
+	}
+
+	if (is_src_mask_used) {
+		struct b2r2_blt_rect src_mask_bounding_rect;
+
+		if (!validate_img(&req->src_mask)) {
+			b2r2_log_info("Validation Error: "
+					"!validate_img(&req->src_mask)\n");
+			return false;
+		}
+
+		b2r2_get_img_bounding_rect(&req->src_mask,
+				&src_mask_bounding_rect);
+		if (!b2r2_is_rect_inside_rect(&req->src_rect,
+					&src_mask_bounding_rect)) {
+			b2r2_log_info("Validation Error: "
+				"!b2r2_is_rect_inside_rect(&req->src_rect, "
+					"&src_mask_bounding_rect)\n");
+			return false;
+		}
+	}
+
+	if (!validate_img(&req->dst_img)) {
+		b2r2_log_info("Validation Error: "
+				"!validate_img(&req->dst_img)\n");
+		return false;
+	}
+
+	return true;
+}
diff --git a/drivers/video/b2r2/b2r2_input_validation.h b/drivers/video/b2r2/b2r2_input_validation.h
new file mode 100644
index 00000000000..9a736343e06
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_input_validation.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson
+ *
+ * This program is free software; you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any later
+ * version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc., 59 Temple
+ * Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef _LINUX_DRIVERS_VIDEO_B2R2_INPUT_VALIDATION_H_
+#define _LINUX_DRIVERS_VIDEO_B2R2_INPUT_VALIDATION_H_
+
+#include <video/b2r2_blt.h>
+
+bool b2r2_validate_user_req(struct b2r2_blt_req *req);
+
+#endif
diff --git a/drivers/video/b2r2/b2r2_internal.h b/drivers/video/b2r2/b2r2_internal.h
new file mode 100644
index 00000000000..18556e7193a
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_internal.h
@@ -0,0 +1,360 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 internal definitions
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_DRIVERS_VIDEO_B2R2_INTERNAL_H_
+#define _LINUX_DRIVERS_VIDEO_B2R2_INTERNAL_H_
+
+
+#include <video/b2r2_blt.h>
+
+#include "b2r2_core.h"
+#include "b2r2_global.h"
+
+#include "b2r2_hw.h"
+
+/* The maximum possible number of temporary buffers needed */
+#define MAX_TMP_BUFS_NEEDED 2
+
+/* Size of the color look-up table */
+#define CLUT_SIZE 1024
+
+/**
+ * b2r2_blt_device() - Returns the device associated with B2R2 BLT.
+ *                     Mainly for debugging with dev_... functions.
+ *
+ * Returns the device pointer or NULL
+ */
+struct device *b2r2_blt_device(void);
+
+/**
+ * struct b2r2_blt_instance - Represents the B2R2 instance (one per open)
+ *
+ * @lock: Lock to protect the instance
+ *
+ * @report_list: Ready requests that should be reported,
+ * @report_list_waitq: Wait queue for report list
+ * @no_of_active_requests: Number of requests added but not reported
+ *                         in callback.
+ * @synching: true if any client is waiting for b2r2_blt_synch(0)
+ * @synch_done_waitq: Wait queue to handle synching on request_id 0
+ */
+struct b2r2_blt_instance {
+	struct mutex lock;
+
+	/* Requests to be reported */
+	struct list_head report_list;
+	wait_queue_head_t report_list_waitq;
+
+	/* Below for synching */
+	u32 no_of_active_requests;
+	bool synching;
+	wait_queue_head_t synch_done_waitq;
+};
+
+/**
+ * struct b2r2_node - Represents a B2R2 node with reqister values, executed
+ *                    by B2R2. Should be allocated non-cached.
+ *
+ * @next: Next node
+ * @physical_address: Physical address to be given to B2R2
+ *                    (physical address of "node" member below)
+ * @node: The B2R2 node with register settings. This is the data
+ *        that B2R2 will use.
+ *
+ */
+struct b2r2_node {
+	struct b2r2_node *next;
+	u32 physical_address;
+
+	int src_tmp_index;
+	int dst_tmp_index;
+
+	int src_index;
+
+	/* B2R2 regs comes here */
+	struct b2r2_link_list node;
+};
+
+/**
+ * struct b2r2_resolved_buf - Contains calculated information about
+ *                            image buffers.
+ *
+ * @physical_address: Physical address of the buffer
+ * @virtual_address: Virtual address of the buffer
+ * @is_pmem: true if buffer is from pmem
+ * @hwmem_session: Hwmem session
+ * @hwmem_alloc: Hwmem alloc
+ * @filep: File pointer of mapped file (like pmem device, frame buffer device)
+ * @file_physical_start: Physical address of file start
+ * @file_virtual_start: Virtual address of file start
+ * @file_len: File len
+ *
+ */
+struct b2r2_resolved_buf {
+	u32                   physical_address;
+	void                 *virtual_address;
+	bool                  is_pmem;
+	struct hwmem_alloc   *hwmem_alloc;
+	/* Data for validation below */
+	struct file          *filep;
+	u32                   file_physical_start;
+	u32                   file_virtual_start;
+	u32                   file_len;
+};
+
+
+/**
+ * b2r2_work_buf - specification for a temporary work buffer
+ *
+ * @size      - the size of the buffer (set by b2r2_node_split)
+ * @phys_addr - the physical address of the buffer (set by b2r2_blt_main)
+ */
+struct b2r2_work_buf {
+	u32 size;
+	u32 phys_addr;
+	void *virt_addr;
+	u32 mem_handle;
+};
+
+
+/**
+ * b2r2_op_type - the type of B2R2 operation to configure
+ */
+enum b2r2_op_type {
+	B2R2_DIRECT_COPY,
+	B2R2_DIRECT_FILL,
+	B2R2_COPY,
+	B2R2_FILL,
+	B2R2_SCALE,
+	B2R2_ROTATE,
+	B2R2_SCALE_AND_ROTATE,
+	B2R2_FLIP,
+};
+
+/**
+ * b2r2_fmt_type - the type of buffer for a given format
+ */
+enum b2r2_fmt_type {
+	B2R2_FMT_TYPE_RASTER,
+	B2R2_FMT_TYPE_SEMI_PLANAR,
+	B2R2_FMT_TYPE_PLANAR,
+};
+
+/**
+ * b2r2_fmt_conv - the type of format conversion to do
+ */
+enum b2r2_fmt_conv {
+	B2R2_FMT_CONV_NONE,
+	B2R2_FMT_CONV_RGB_TO_YUV,
+	B2R2_FMT_CONV_YUV_TO_RGB,
+	B2R2_FMT_CONV_YUV_TO_YUV,
+	B2R2_FMT_CONV_RGB_TO_BGR,
+	B2R2_FMT_CONV_BGR_TO_RGB,
+	B2R2_FMT_CONV_YUV_TO_BGR,
+	B2R2_FMT_CONV_BGR_TO_YUV,
+};
+
+/**
+ * b2r2_node_split_buf - information about a source or destination buffer
+ *
+ * @addr            - the physical base address
+ * @chroma_addr     - the physical address of the chroma plane
+ * @chroma_cr_addr  - the physical address of the Cr chroma plane
+ * @fmt             - the buffer format
+ * @fmt_type        - the buffer format type
+ * @rect            - the rectangle of the buffer to use
+ * @color           - the color value to use is case of a fill operation
+ * @pitch           - the pixmap byte pitch
+ * @height          - the pixmap height
+ * @alpha_range     - the alpha range of the buffer (0-128 or 0-255)
+ * @hso             - the horizontal scan order
+ * @vso             - the vertical scan order
+ * @endian          - the endianess of the buffer
+ * @plane_selection - the plane to write if buffer is planar or semi-planar
+ */
+struct b2r2_node_split_buf {
+	u32 addr;
+	u32 chroma_addr;
+	u32 chroma_cr_addr;
+
+	enum b2r2_blt_fmt fmt;
+	enum b2r2_fmt_type type;
+
+	struct b2r2_blt_rect rect;
+	struct b2r2_blt_rect win;
+
+	s32 dx;
+	s32 dy;
+
+	u32 color;
+	u16 pitch;
+	u16 width;
+	u16 height;
+
+	enum b2r2_ty alpha_range;
+	enum b2r2_ty hso;
+	enum b2r2_ty vso;
+	enum b2r2_ty endian;
+	enum b2r2_tty dither;
+
+	/* Plane selection (used when writing to a multibuffer format) */
+	enum b2r2_tty plane_selection;
+
+	/* Chroma plane selection (used when writing planar formats) */
+	enum b2r2_tty chroma_selection;
+
+	int tmp_buf_index;
+};
+
+/**
+ * b2r2_node_split_job - an instance of a node split job
+ *
+ * @type          - the type of operation
+ * @ivmx          - the ivmx matrix to use for color conversion
+ * @blend         - determines if blending is enabled
+ * @clip          - determines if destination clipping is enabled
+ * @swap_fg_bg    - determines if FG and BG should be swapped when blending
+ * @flags         - the flags passed in the blt request
+ * @flag_param    - parameter required by certain flags,
+ *                  e.g. color for source color keying.
+ * @transform     - the transforms passed in the blt request
+ * @global_alpha  - the global alpha
+ * @clip_rect     - the clipping rectangle to use
+ * @horiz_rescale - determmines if horizontal rescaling is enabled
+ * @horiz_sf      - the horizontal scale factor
+ * @vert_rescale  - determines if vertical rescale is enabled
+ * @vert_sf       - the vertical scale factor
+ * @src           - the incoming source buffer
+ * @dst           - the outgoing destination buffer
+ * @work_bufs     - work buffer specifications
+ * @tmp_bufs      - temporary buffers
+ * @buf_count     - the number of temporary buffers used for the job
+ * @node_count    - the number of nodes used for the job
+ * @max_buf_size  - the maximum size of temporary buffers
+ * @nbr_rows      - the number of tile rows in the blit operation
+ * @nbr_cols      - the number of time columns in the blit operation
+ */
+struct b2r2_node_split_job {
+	enum b2r2_op_type type;
+
+	const u32 *ivmx;
+
+	bool blend;
+	bool clip;
+	bool rotation;
+
+	bool swap_fg_bg;
+
+	u32 flags;
+	u32 flag_param;
+	u32 transform;
+	u32 global_alpha;
+
+	struct b2r2_blt_rect clip_rect;
+
+	bool h_rescale;
+	u16 h_rsf;
+
+	bool v_rescale;
+	u16 v_rsf;
+
+	struct b2r2_node_split_buf src;
+	struct b2r2_node_split_buf dst;
+
+	struct b2r2_work_buf work_bufs[MAX_TMP_BUFS_NEEDED];
+	struct b2r2_node_split_buf tmp_bufs[MAX_TMP_BUFS_NEEDED];
+
+	u32 buf_count;
+	u32 node_count;
+	u32 max_buf_size;
+};
+
+/**
+ * struct b2r2_blt_request - Represents one B2R2 blit request
+ *
+ * @instance: Back pointer to the instance structure
+ * @list: List item to keep track of requests per instance
+ * @user_req: The request received from userspace
+ * @job: The administration structure for the B2R2 job,
+ *       consisting of one or more nodes
+ * @node_split_job: The administration structure for the B2R2 node split job
+ * @first_node: Pointer to the first B2R2 node
+ * @request_id: Request id for this job
+ * @node_split_handle: Handle of the node split
+ * @src_resolved: Calculated info about the source buffer
+ * @src_mask_resolved: Calculated info about the source mask buffer
+ * @dst_resolved: Calculated info about the destination buffer
+ * @profile: True if the blit shall be profiled, false otherwise
+ */
+struct b2r2_blt_request {
+	struct b2r2_blt_instance   *instance;
+	struct list_head           list;
+	struct b2r2_blt_req        user_req;
+	struct b2r2_core_job       job;
+	struct b2r2_node_split_job node_split_job;
+	struct b2r2_node           *first_node;
+	int                        request_id;
+
+	/* Resolved buffer addresses */
+	struct b2r2_resolved_buf src_resolved;
+	struct b2r2_resolved_buf src_mask_resolved;
+	struct b2r2_resolved_buf dst_resolved;
+
+	/* TBD: Info about SRAM usage & needs */
+	struct b2r2_work_buf *bufs;
+	u32 buf_count;
+
+	/* color look-up table */
+	void *clut;
+	u32 clut_phys_addr;
+
+	/* Profiling stuff */
+	bool profile;
+
+	s32 nsec_active_in_cpu;
+
+	u32 start_time_nsec;
+	s32 total_time_nsec;
+};
+
+/* FIXME: The functions below should be removed when we are
+   switching to the new Robert Lind allocator */
+
+/**
+ * b2r2_blt_alloc_nodes() - Allocate nodes
+ *
+ * @node_count: Number of nodes to allocate
+ *
+ * Return:
+ *   Returns a pointer to the first node in the node list.
+ */
+struct b2r2_node *b2r2_blt_alloc_nodes(int node_count);
+
+/**
+ * b2r2_blt_free_nodes() - Release nodes previously allocated via
+ *                         b2r2_generate_nodes
+ *
+ * @first_node: First node in linked list of nodes
+ */
+void b2r2_blt_free_nodes(struct b2r2_node *first_node);
+
+/**
+ * b2r2_blt_module_init() - Initialize the B2R2 blt module
+ */
+int b2r2_blt_module_init(void);
+
+/**
+ * b2r2_blt_module_exit() - Un-initialize the B2R2 blt module
+ */
+void b2r2_blt_module_exit(void);
+
+#endif
diff --git a/drivers/video/b2r2/b2r2_kernel_if.c b/drivers/video/b2r2/b2r2_kernel_if.c
new file mode 100644
index 00000000000..373311ccca5
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_kernel_if.c
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 kernel interface for beeing a separate module
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/poll.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/list.h>
+#ifdef CONFIG_ANDROID_PMEM
+#include <linux/android_pmem.h>
+#endif
+#include <linux/fb.h>
+#include <linux/sched.h>
+#include <asm/uaccess.h>
+#include <asm/cacheflush.h>
+
+EXPORT_SYMBOL(fget_light);
+EXPORT_SYMBOL(fput_light);
+EXPORT_SYMBOL(flush_cache_range);
+EXPORT_SYMBOL(task_sched_runtime);
+#ifdef CONFIG_ANDROID_PMEM
+EXPORT_SYMBOL(get_pmem_file);
+EXPORT_SYMBOL(put_pmem_file);
+EXPORT_SYMBOL(flush_pmem_file);
+#endif
diff --git a/drivers/video/b2r2/b2r2_mem_alloc.c b/drivers/video/b2r2/b2r2_mem_alloc.c
new file mode 100644
index 00000000000..0dcd9b6a55e
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_mem_alloc.c
@@ -0,0 +1,726 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 internal Memory allocator
+ *
+ * Author: Robert Lind <robert.lind@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/list.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+
+#include "b2r2_internal.h"
+#include "b2r2_mem_alloc.h"
+
+
+/* Represents one block of b2r2 physical memory, free or allocated */
+struct b2r2_mem_block {
+	struct list_head list; /* For membership in list */
+
+	u32 offset;        /* Offset in b2r2 physical memory area (aligned) */
+	u32 size;          /* Size of the object (requested size if busy,
+			      else actual) */
+	bool free;         /* True if the block is free */
+
+	u32 lock_count;    /* Lock count */
+
+#ifdef CONFIG_DEBUG_FS
+	char          debugfs_fname[80]; /* debugfs file name */
+	struct dentry *debugfs_block;	 /* debugfs dir entry for the block */
+#endif
+};
+
+/* The memory heap */
+struct b2r2_mem_heap {
+	struct device *dev;        /* Device pointer for memory allocation  */
+	dma_addr_t start_phys_addr;/* Physical memory start address         */
+	void *start_virt_ptr;      /* Virtual pointer to start              */
+	u32 size;                  /* Memory size                           */
+	u32 align;                 /* Alignment                             */
+
+	struct list_head blocks;   /* List of all blocks                    */
+
+	spinlock_t heap_lock;      /* Protection for the heap               */
+
+	u32 node_size;             /* Size of each B2R2 node                */
+	struct dma_pool *node_heap;/* Heap for B2R2 node allocations        */
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *debugfs_sub_root_dir; /* debugfs: B2R2 MEM root dir  */
+	struct dentry *debugfs_heap_stats;   /* debugfs: B2R2 memory status */
+	struct dentry *debugfs_dir_blocks;   /* debugfs: Free blocks dir    */
+#endif
+};
+
+static struct b2r2_mem_heap *mem_heap;
+
+#ifdef CONFIG_DEBUG_FS
+static struct dentry *debugfs_root_dir;      /* debugfs: B2R2 MEM root dir */
+#endif
+
+/* Forward declarations */
+static struct b2r2_mem_block *b2r2_mem_block_alloc(
+	u32 offset, u32 size, bool free);
+static void   b2r2_mem_block_free(struct b2r2_mem_block *mem_block);
+
+/* Align value down to specified alignment */
+static inline u32 align_down(u32 align, u32 value)
+{
+	return value & ~(align - 1);
+}
+
+/* Align value up to specified alignment */
+static inline u32 align_up(u32 align, u32 value)
+{
+	return (value + align - 1) & ~(align - 1);
+}
+
+
+#ifdef CONFIG_DEBUG_FS
+/* About debugfs:
+ *  debugfs is a mountable debug file system.
+ *
+ *  Mount like this:
+ *  mkdir /debug
+ *  mount -t debugfs none /debug
+ *  ls /debug/b2r2_mem
+ *
+ *  ls -al /debug/b2r2_mem/blocks
+ *  cat /debug/b2r2_mem/stats
+ */
+
+
+/* Create string containing memory heap status */
+static char *get_b2r2_mem_stats(struct b2r2_mem_heap *mem_heap, char *buf)
+{
+	struct b2r2_mem_heap_status mem_heap_status;
+
+	if (b2r2_mem_heap_status(&mem_heap_status) != 0) {
+		strcpy(buf, "Error, failed to get status\n");
+		return buf;
+	}
+
+	sprintf(buf,
+		"Handle                 : 0x%lX\n"
+		"Physical start address : 0x%lX\n"
+		"Size                   : %lu\n"
+		"Align                  : %lu\n"
+		"No of blocks allocated : %lu\n"
+		"Allocated size         : %lu\n"
+		"No of free blocks      : %lu\n"
+		"Free size              : %lu\n"
+		"No of locks            : %lu\n"
+		"No of locked           : %lu\n"
+		"No of nodes            : %lu\n",
+		(unsigned long) mem_heap,
+		(unsigned long) mem_heap_status.start_phys_addr,
+		(unsigned long) mem_heap_status.size,
+		(unsigned long) mem_heap_status.align,
+		(unsigned long) mem_heap_status.num_alloc,
+		(unsigned long) mem_heap_status.allocated_size,
+		(unsigned long) mem_heap_status.num_free,
+		(unsigned long) mem_heap_status.free_size,
+		(unsigned long) mem_heap_status.num_locks,
+		(unsigned long) mem_heap_status.num_locked,
+		(unsigned long) mem_heap_status.num_nodes);
+
+	return buf;
+}
+
+/*
+ * Print memory heap status on file
+ * (Use like "cat /debug/b2r2_mem/igram/stats")
+ */
+static int debugfs_b2r2_mem_stats_read(struct file *filp, char __user *buf,
+				       size_t count, loff_t *f_pos)
+{
+	struct b2r2_mem_heap *mem_heap = filp->f_dentry->d_inode->i_private;
+	char Buf[400];
+	size_t dev_size;
+	int ret = 0;
+
+	get_b2r2_mem_stats(mem_heap, Buf);
+	dev_size = strlen(Buf);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	return ret;
+}
+
+/* debugfs file operations for the "stats" file */
+static const struct file_operations debugfs_b2r2_mem_stats_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_mem_stats_read,
+};
+
+/* read function for file in the "blocks" sub directory */
+static int debugfs_b2r2_mem_block_read(struct file *filp, char __user *buf,
+				       size_t count, loff_t *f_pos)
+{
+	struct b2r2_mem_block *mem_block = filp->f_dentry->d_inode->i_private;
+	char Buf[200];
+	size_t dev_size;
+	int ret = 0;
+
+	dev_size = sprintf(Buf, "offset: %08lX %s size: %8d "
+			   "lock_count: %2d\n",
+			   (unsigned long) mem_block->offset,
+			   mem_block->free ? "free" : "allc",
+			   mem_block->size,
+			   mem_block->lock_count);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	return ret;
+}
+
+/* debugfs file operations for files in the "blocks" directory */
+static const struct file_operations debugfs_b2r2_mem_block_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_mem_block_read,
+};
+
+/*
+ * Create or update the debugfs directory entry for a file in the
+ * "blocks" directory (a memory allocation)
+ */
+void debugfs_create_mem_block_entry(struct b2r2_mem_block *mem_block,
+				    struct dentry *parent)
+{
+	struct timespec tm = current_kernel_time();
+	struct timespec atime = tm;
+	struct timespec mtime = tm;
+	struct timespec ctime = tm;
+
+	if (mem_block->debugfs_block) {
+		atime = mem_block->debugfs_block->d_inode->i_atime;
+		ctime = mem_block->debugfs_block->d_inode->i_ctime;
+		debugfs_remove(mem_block->debugfs_block);
+	}
+
+	/* Add the block in debugfs */
+	if (mem_block->free)
+		sprintf(mem_block->debugfs_fname, "%08lX free",
+			(unsigned long) mem_block->offset);
+	else {
+		sprintf(mem_block->debugfs_fname, "%08lX allc h:%08lX "
+			"lck:%d ",
+			(unsigned long) mem_block->offset,
+			(unsigned long) mem_block,
+			mem_block->lock_count);
+	}
+
+	mem_block->debugfs_block = debugfs_create_file(
+		mem_block->debugfs_fname,
+		0444, parent, mem_block,
+		&debugfs_b2r2_mem_block_fops);
+	if (mem_block->debugfs_block) {
+		mem_block->debugfs_block->d_inode->i_size = mem_block->size;
+		mem_block->debugfs_block->d_inode->i_atime = atime;
+		mem_block->debugfs_block->d_inode->i_mtime = mtime;
+		mem_block->debugfs_block->d_inode->i_ctime = ctime;
+	}
+}
+#endif   /* CONFIG_DEBUG_FS */
+
+/* Module initialization function */
+int b2r2_mem_init(struct device *dev, u32 heap_size, u32 align, u32 node_size)
+{
+	struct b2r2_mem_block *mem_block;
+	u32 aligned_size;
+
+	printk(KERN_INFO "B2R2_MEM: Creating heap for size %d bytes\n",
+		(int) heap_size);
+
+	/* Align size */
+	aligned_size = align_down(align, heap_size);
+	if (aligned_size == 0)
+		return -EINVAL;
+
+	mem_heap = kcalloc(sizeof(struct b2r2_mem_heap), 1, GFP_KERNEL);
+	if (!mem_heap)
+		return -ENOMEM;
+
+	mem_heap->start_virt_ptr = dma_alloc_coherent(dev,
+		aligned_size, &(mem_heap->start_phys_addr), GFP_KERNEL);
+	if (!mem_heap->start_phys_addr || !mem_heap->start_virt_ptr) {
+		printk(KERN_ERR "B2R2_MEM: Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	/* Initialize the heap */
+	mem_heap->dev   = dev;
+	mem_heap->size  = aligned_size;
+	mem_heap->align = align;
+
+	INIT_LIST_HEAD(&mem_heap->blocks);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Register debugfs */
+
+	debugfs_root_dir = debugfs_create_dir("b2r2_mem", NULL);
+
+	mem_heap->debugfs_sub_root_dir = debugfs_create_dir("b2r2_mem",
+		debugfs_root_dir);
+	mem_heap->debugfs_heap_stats = debugfs_create_file(
+		"stats", 0444, mem_heap->debugfs_sub_root_dir, mem_heap,
+		&debugfs_b2r2_mem_stats_fops);
+	mem_heap->debugfs_dir_blocks = debugfs_create_dir(
+		"blocks", mem_heap->debugfs_sub_root_dir);
+#endif
+
+	/* Create the first _free_ memory block */
+	mem_block = b2r2_mem_block_alloc(0, aligned_size, true);
+	if (!mem_block) {
+		dma_free_coherent(dev, aligned_size,
+			mem_heap->start_virt_ptr,
+			mem_heap->start_phys_addr);
+		kfree(mem_heap);
+		printk(KERN_ERR "B2R2_MEM: Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	/* Add the free block to the blocks list */
+	list_add(&mem_block->list, &mem_heap->blocks);
+
+	/* Allocate separate heap for B2R2 nodes */
+	mem_heap->node_size = node_size;
+	mem_heap->node_heap = dma_pool_create("b2r2_node_cache",
+		dev, node_size, align, 4096);
+	if (!mem_heap->node_heap) {
+		b2r2_mem_block_free(mem_block);
+		dma_free_coherent(dev, aligned_size,
+			mem_heap->start_virt_ptr,
+			mem_heap->start_phys_addr);
+		kfree(mem_heap);
+		printk(KERN_ERR "B2R2_MEM: Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(b2r2_mem_init);
+
+/* Module exit function */
+void b2r2_mem_exit(void)
+{
+	struct list_head *ptr;
+
+	/* Free B2R2 node heap */
+	dma_pool_destroy(mem_heap->node_heap);
+
+#ifdef CONFIG_DEBUG_FS
+	/* debugfs root dir */
+	if (debugfs_root_dir) {
+		debugfs_remove_recursive(debugfs_root_dir);
+		debugfs_root_dir = NULL;
+	}
+#endif
+
+	list_for_each(ptr, &mem_heap->blocks) {
+		struct b2r2_mem_block *mem_block =
+			list_entry(ptr, struct b2r2_mem_block, list);
+
+		b2r2_mem_block_free(mem_block);
+	}
+
+	dma_free_coherent(mem_heap->dev, mem_heap->size,
+		mem_heap->start_virt_ptr,
+		mem_heap->start_phys_addr);
+	kfree(mem_heap);
+}
+EXPORT_SYMBOL(b2r2_mem_exit);
+
+/* Return status of the heap */
+int b2r2_mem_heap_status(struct b2r2_mem_heap_status *mem_heap_status)
+{
+	struct list_head *ptr;
+
+	if (!mem_heap || !mem_heap_status)
+		return -EINVAL;
+	memset(mem_heap_status, 0, sizeof(*mem_heap_status));
+
+	/* Lock the heap */
+	spin_lock(&mem_heap->heap_lock);
+
+	/* Fill in static info */
+	mem_heap_status->start_phys_addr = mem_heap->start_phys_addr;
+	mem_heap_status->size = mem_heap->size;
+	mem_heap_status->align = mem_heap->align;
+
+	list_for_each(ptr, &mem_heap->blocks) {
+		struct b2r2_mem_block *mem_block =
+			list_entry(ptr, struct b2r2_mem_block, list);
+
+		if (mem_block->free) {
+			mem_heap_status->num_free++;
+			mem_heap_status->free_size += mem_block->size;
+		} else {
+			if (mem_block->lock_count) {
+				mem_heap_status->num_locked++;
+				mem_heap_status->num_locks +=
+					mem_block->lock_count;
+			}
+			mem_heap_status->num_alloc++;
+			mem_heap_status->allocated_size += mem_block->size;
+		}
+	}
+
+	spin_unlock(&mem_heap->heap_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(b2r2_mem_heap_status);
+
+/* Internal: Allocate a housekeeping structure
+ * for an allocated or free memory block
+ */
+static struct b2r2_mem_block *b2r2_mem_block_alloc(
+	u32 offset, u32 size, bool free)
+{
+	struct b2r2_mem_block *mem_block = kmalloc(
+		sizeof(struct b2r2_mem_block), GFP_KERNEL);
+
+	if (mem_block) {
+		mem_block->offset     = offset;
+		mem_block->size       = size;
+		mem_block->free       = free;
+		mem_block->lock_count = 0;
+
+		INIT_LIST_HEAD(&mem_block->list);
+
+#ifdef CONFIG_DEBUG_FS
+		mem_block->debugfs_block  = NULL;
+		/* Add the block in debugfs */
+		debugfs_create_mem_block_entry(mem_block,
+			mem_heap->debugfs_dir_blocks);
+#endif
+	}
+
+	return mem_block;
+}
+
+/* Internal: Release housekeeping structure */
+static void b2r2_mem_block_free(struct b2r2_mem_block *mem_block)
+{
+	if (mem_block) {
+#ifdef CONFIG_DEBUG_FS
+		debugfs_remove(mem_block->debugfs_block);
+#endif
+		kfree(mem_block);
+	}
+}
+
+/* Allocate a block from the heap */
+int b2r2_mem_alloc(u32 requested_size, u32 *returned_size, u32 *mem_handle)
+{
+	int ret = 0;
+	struct list_head *ptr;
+	struct b2r2_mem_block *found_mem_block = NULL;
+	u32 aligned_size;
+
+	if (!mem_handle)
+		return -EINVAL;
+
+	printk(KERN_INFO "%s: size=%d\n", __func__, requested_size);
+
+	*mem_handle = 0;
+	if (!mem_heap)
+		return -EINVAL;
+
+	/* Lock the heap */
+	spin_lock(&mem_heap->heap_lock);
+
+	aligned_size = align_up(mem_heap->align, requested_size);
+	/* Try to find the best matching free block of suitable size */
+	list_for_each(ptr, &mem_heap->blocks) {
+		struct b2r2_mem_block *mem_block =
+			list_entry(ptr, struct b2r2_mem_block, list);
+
+		if (mem_block->free && mem_block->size >= aligned_size &&
+			(!found_mem_block ||
+			 mem_block->size < found_mem_block->size)) {
+			found_mem_block = mem_block;
+			if (found_mem_block->size == aligned_size)
+				break;
+		}
+	}
+
+	if (found_mem_block) {
+		struct b2r2_mem_block *new_block
+			= b2r2_mem_block_alloc(found_mem_block->offset,
+				requested_size, false);
+
+		if (new_block) {
+			/* Insert the new block before the found block */
+			list_add_tail(&new_block->list,
+				&found_mem_block->list);
+
+			/* Split the free block */
+			found_mem_block->offset += aligned_size;
+			found_mem_block->size   -= aligned_size;
+
+			if (found_mem_block->size == 0)
+				b2r2_mem_block_free(found_mem_block);
+			else {
+#ifdef CONFIG_DEBUG_FS
+				debugfs_create_mem_block_entry(
+					found_mem_block,
+					mem_heap->debugfs_dir_blocks);
+#endif
+			}
+
+			*mem_handle = (u32) new_block;
+			*returned_size = aligned_size;
+		} else {
+			ret = -ENOMEM;
+		}
+	} else
+		ret = -ENOMEM;
+
+	if (ret != 0) {
+		*returned_size = 0;
+		*mem_handle = (u32) 0;
+	}
+
+	/* Unlock */
+	spin_unlock(&mem_heap->heap_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_mem_alloc);
+
+/* Free the allocated block */
+int b2r2_mem_free(u32 mem_handle)
+{
+	int ret = 0;
+	struct b2r2_mem_block *mem_block = (struct b2r2_mem_block *) mem_handle;
+
+	if (!mem_block)
+		return -EINVAL;
+
+	/* Lock the heap */
+	spin_lock(&mem_heap->heap_lock);
+
+	if (!ret && mem_block->free)
+		ret = -EINVAL;
+
+	if (!ret) {
+		printk(KERN_INFO "%s: freeing block 0x%p\n", __func__, mem_block);
+		/* Release the block */
+
+		mem_block->free = true;
+		mem_block->size = align_up(mem_heap->align,
+					   mem_block->size);
+
+		/* Join with previous block if possible */
+		if (mem_block->list.prev != &mem_heap->blocks) {
+			struct b2r2_mem_block *prev_block =
+				list_entry(mem_block->list.prev,
+					struct b2r2_mem_block, list);
+
+			if (prev_block->free &&
+			    (prev_block->offset + prev_block->size) ==
+			    mem_block->offset) {
+				mem_block->offset = prev_block->offset;
+				mem_block->size   += prev_block->size;
+
+				b2r2_mem_block_free(prev_block);
+			}
+		}
+
+		/* Join with next block if possible */
+		if (mem_block->list.next != &mem_heap->blocks) {
+			struct b2r2_mem_block *next_block
+				= list_entry(mem_block->list.next,
+						 struct b2r2_mem_block,
+						 list);
+
+			if (next_block->free &&
+			    (mem_block->offset + mem_block->size) ==
+			    next_block->offset) {
+				mem_block->size   += next_block->size;
+
+				b2r2_mem_block_free(next_block);
+			}
+		}
+#ifdef CONFIG_DEBUG_FS
+		debugfs_create_mem_block_entry(mem_block,
+			mem_heap->debugfs_dir_blocks);
+#endif
+	}
+
+	/* Unlock */
+	spin_unlock(&mem_heap->heap_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_mem_free);
+
+/* Lock the allocated block in memory */
+int b2r2_mem_lock(u32 mem_handle, u32 *phys_addr, void **virt_ptr, u32 *size)
+{
+	struct b2r2_mem_block *mem_block =
+		(struct b2r2_mem_block *) mem_handle;
+
+	if (!mem_block)
+		return -EINVAL;
+
+	/* Lock the heap */
+	spin_lock(&mem_heap->heap_lock);
+
+	mem_block->lock_count++;
+
+	if (phys_addr)
+		*phys_addr = mem_heap->start_phys_addr + mem_block->offset;
+	if (virt_ptr)
+		*virt_ptr = (char *) mem_heap->start_virt_ptr +
+			mem_block->offset;
+	if (size)
+		*size = align_up(mem_heap->align, mem_block->size);
+#ifdef CONFIG_DEBUG_FS
+	debugfs_create_mem_block_entry(mem_block,
+		mem_heap->debugfs_dir_blocks);
+#endif
+
+	spin_unlock(&mem_heap->heap_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(b2r2_mem_lock);
+
+/* Unlock the allocated block in memory */
+int b2r2_mem_unlock(u32 mem_handle)
+{
+	struct b2r2_mem_block *mem_block =
+		(struct b2r2_mem_block *) mem_handle;
+
+	if (!mem_block)
+		return -EINVAL;
+
+	/* Lock the heap */
+	spin_lock(&mem_heap->heap_lock);
+
+	mem_block->lock_count--;
+
+	spin_unlock(&mem_heap->heap_lock);
+
+	/* debugfs will be updated in release */
+	return 0;
+/*	return b2r2_mem_free(mem_handle);*/
+}
+EXPORT_SYMBOL(b2r2_mem_unlock);
+
+/* Allocate one or more b2r2 nodes from DMA pool */
+int b2r2_node_alloc(u32 num_nodes, struct b2r2_node **first_node)
+{
+	int i;
+	int ret = 0;
+	u32 physical_address;
+	struct b2r2_node *first_node_ptr;
+	struct b2r2_node *node_ptr;
+
+	/* Check input parameters */
+	if ((num_nodes <= 0) || !first_node) {
+		printk(KERN_ERR
+			"B2R2_MEM: Invalid parameter for b2r2_node_alloc, "
+			"num_nodes=%d, first_node=%ld\n",
+			(int) num_nodes, (long) first_node);
+		return -EINVAL;
+	}
+
+	/* Allocate the first node */
+	first_node_ptr = dma_pool_alloc(mem_heap->node_heap,
+		GFP_DMA | GFP_KERNEL, &physical_address);
+	if (first_node_ptr) {
+		/* Initialize first node */
+		first_node_ptr->next = NULL;
+		first_node_ptr->physical_address = physical_address +
+			offsetof(struct b2r2_node, node);
+
+		/* Allocate and initialize remaining nodes, */
+		/* and link them into a list                */
+		for (i = 1, node_ptr = first_node_ptr; i < num_nodes; i++) {
+			node_ptr->next = dma_pool_alloc(mem_heap->node_heap,
+				GFP_DMA | GFP_KERNEL, &physical_address);
+			if (node_ptr->next) {
+				node_ptr = node_ptr->next;
+				node_ptr->next = NULL;
+				node_ptr->physical_address = physical_address +
+					offsetof(struct b2r2_node, node);
+			} else {
+				printk(KERN_ERR "B2R2_MEM: Failed to allocate memory for node\n");
+				ret = -ENOMEM;
+				break;
+			}
+		}
+
+		/* If all nodes were allocated successfully, */
+		/* return the first node                     */
+		if (!ret)
+			*first_node = first_node_ptr;
+		else
+			b2r2_node_free(first_node_ptr);
+	} else {
+		printk(KERN_ERR "B2R2_MEM: Failed to allocate memory for node\n");
+		ret = -ENOMEM;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_node_alloc);
+
+/* Free a linked list of b2r2 nodes */
+void b2r2_node_free(struct b2r2_node *first_node)
+{
+	struct b2r2_node *current_node = first_node;
+	struct b2r2_node *next_node = NULL;
+
+	/* Traverse the linked list and free the nodes */
+	while (current_node != NULL) {
+		next_node = current_node->next;
+		dma_pool_free(mem_heap->node_heap, current_node,
+			current_node->physical_address -
+			offsetof(struct b2r2_node, node));
+		current_node = next_node;
+	}
+}
+EXPORT_SYMBOL(b2r2_node_free);
+
+MODULE_AUTHOR("Robert Lind <robert.lind@ericsson.com");
+MODULE_DESCRIPTION("Ericsson AB B2R2 physical memory driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/b2r2/b2r2_mem_alloc.h b/drivers/video/b2r2/b2r2_mem_alloc.h
new file mode 100644
index 00000000000..33309c972f5
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_mem_alloc.h
@@ -0,0 +1,140 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 internal Memory allocator
+ *
+ * Author: Robert Lind <robert.lind@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __B2R2_MEM_ALLOC_H
+#define __B2R2_MEM_ALLOC_H
+
+#include "b2r2_internal.h"
+
+
+/**
+ * struct b2r2_mem_heap_status - Information about current state of the heap
+ * @start_phys_addr: Physical address of the the memory area
+ * @size: Size of the memory area
+ * @align: Alignment of start and allocation sizes (in bytes).
+ * @num_alloc: Number of memory allocations
+ * @allocated_size: Size allocated (sum of requested sizes)
+ * @num_free: Number of free blocks (fragments)
+ * @free_size: Free size available for allocation
+ * @num_locks: Sum of number of number of locks on memory allocations
+ * @num_locked: Number of locked memory allocations
+ * @num_nodes: Number of node allocations
+ *
+ **/
+struct b2r2_mem_heap_status {
+	u32 start_phys_addr;
+	u32 size;
+	u32 align;
+	u32 num_alloc;
+	u32 allocated_size;
+	u32 num_free;
+	u32 free_size;
+	u32 num_locks;
+	u32 num_locked;
+	u32 num_nodes;
+};
+
+
+/* B2R2 memory API (kernel) */
+
+/**
+ * b2r2_mem_init() - Initializes the B2R2 memory manager
+ * @dev: Pointer to device to use for allocating the memory heap
+ * @heap_size: Size of the heap (in bytes)
+ * @align: Alignment to use for memory allocations on heap (in bytes)
+ * @node_size: Size of each B2R2 node (in bytes)
+ *
+ * Returns 0 if success, else negative error code
+ **/
+int b2r2_mem_init(struct device *dev, u32 heap_size, u32 align, u32 node_size);
+
+/**
+ * b2r2_mem_exit() - Cleans up the B2R2 memory manager
+ *
+ **/
+void b2r2_mem_exit(void);
+
+/**
+ * b2r2_mem_heap_status() - Get information about the current heap state
+ * @mem_heap_status: Struct containing status info on succesful return
+ *
+ * Returns 0 if success, else negative error code
+ **/
+int b2r2_mem_heap_status(struct b2r2_mem_heap_status *mem_heap_status);
+
+/**
+ * b2r2_mem_alloc() - Allocates memory block from physical memory heap
+ * @requested_size: Requested size
+ * @returned_size: Actual size of memory block. Might be adjusted due to
+ *                 alignment but is always >= requested size if function
+ *                 succeeds
+ * @mem_handle: Returned memory handle
+ *
+ * All memory allocations are movable when not locked.
+ * Returns 0 if OK else negative error value
+ **/
+int b2r2_mem_alloc(u32 requested_size, u32 *returned_size, u32 *mem_handle);
+
+/**
+ * b2r2_mem_free() - Frees an allocation
+ * @mem_handle: Memory handle
+ *
+ * Returns 0 if OK else negative error value
+ **/
+int b2r2_mem_free(u32 mem_handle);
+
+/**
+ * b2r2_mem_lock() - Lock memory in memory and return physical address
+ * @mem_handle: Memory handle
+ * @phys_addr: Returned physical address to start of memory allocation.
+ *             May be NULL.
+ * @virt_ptr: Returned virtual address pointer to start of memory allocation.
+ *            May be NULL.
+ * @size: Returned size of memory allocation. May be NULL.
+ *
+ * The adress of the memory allocation is locked and the physical address
+ * is returned.
+ * The lock count is incremented by one.
+ * You need to call b2r2_mem_unlock once for each call to
+ * b2r2_mem_lock.
+ * Returns 0 if OK else negative error value
+ **/
+int b2r2_mem_lock(u32 mem_handle, u32 *phys_addr, void **virt_ptr, u32 *size);
+
+/**
+ * b2r2_mem_unlock() - Unlock previously locked memory
+ * @mem_handle: Memory handle
+ *
+ * Decrements lock count. When lock count reaches 0 the
+ * memory area is movable again.
+ * Returns 0 if OK else negative error value
+ **/
+int b2r2_mem_unlock(u32 mem_handle);
+
+/**
+ * b2r2_node_alloc() - Allocates B2R2 node from physical memory heap
+ * @num_nodes: Number of linked nodes to allocate
+ * @first_node: Returned pointer to first node in linked list
+ *
+ * Returns 0 if OK else negative error value
+ **/
+int b2r2_node_alloc(u32 num_nodes, struct b2r2_node **first_node);
+
+/**
+ * b2r2_node_free() - Frees a linked list of allocated B2R2 nodes
+ * @first_node: Pointer to first node in linked list
+ *
+ * Returns 0 if OK else negative error value
+ **/
+void b2r2_node_free(struct b2r2_node *first_node);
+
+
+#endif /* __B2R2_MEM_ALLOC_H */
diff --git a/drivers/video/b2r2/b2r2_node_gen.c b/drivers/video/b2r2/b2r2_node_gen.c
new file mode 100644
index 00000000000..f452a4b11df
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_node_gen.c
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 node generator
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <asm/dma-mapping.h>
+#include "b2r2_internal.h"
+
+static void free_nodes(struct b2r2_node *first_node)
+{
+	struct b2r2_node *node = first_node;
+	int no_of_nodes = 0;
+
+	while (node) {
+		no_of_nodes++;
+		node = node->next;
+	}
+
+	dma_free_coherent(b2r2_blt_device(),
+			  no_of_nodes * sizeof(struct b2r2_node),
+			  first_node,
+			  first_node->physical_address -
+			  offsetof(struct b2r2_node, node));
+}
+
+struct b2r2_node *b2r2_blt_alloc_nodes(int no_of_nodes)
+{
+	u32 physical_address;
+	struct b2r2_node *nodes;
+	struct b2r2_node *tmpnode;
+
+	if (no_of_nodes <= 0) {
+		dev_err(b2r2_blt_device(), "%s: Wrong number of nodes (%d)",
+			__func__, no_of_nodes);
+		return NULL;
+	}
+
+	/* Allocate the memory */
+	nodes = (struct b2r2_node *) dma_alloc_coherent(b2r2_blt_device(),
+				      no_of_nodes * sizeof(struct b2r2_node),
+				      &physical_address, GFP_DMA | GFP_KERNEL);
+
+	if (nodes == NULL) {
+		dev_err(b2r2_blt_device(),
+			"%s: Failed to alloc memory for nodes",
+			__func__);
+		return NULL;
+	}
+
+	/* Build the linked list */
+	tmpnode = nodes;
+	physical_address += offsetof(struct b2r2_node, node);
+	while (no_of_nodes--) {
+		tmpnode->physical_address = physical_address;
+		if (no_of_nodes)
+			tmpnode->next = tmpnode + 1;
+		else
+			tmpnode->next = NULL;
+
+		tmpnode++;
+		physical_address += sizeof(struct b2r2_node);
+	}
+
+	return nodes;
+}
+
+void b2r2_blt_free_nodes(struct b2r2_node *first_node)
+{
+	free_nodes(first_node);
+}
+
diff --git a/drivers/video/b2r2/b2r2_node_split.c b/drivers/video/b2r2/b2r2_node_split.c
new file mode 100644
index 00000000000..90674e0e540
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_node_split.c
@@ -0,0 +1,3419 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 node splitter
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include "b2r2_debug.h"
+#include "b2r2_node_split.h"
+#include "b2r2_internal.h"
+#include "b2r2_hw.h"
+#include "b2r2_filters.h"
+
+#include <linux/kernel.h>
+
+/*
+ * Macros and constants
+ */
+#define ABS(x) ((x) < 0 ? -(x) : (x))
+#define MAX(x, y) ((x) > (y) ? (x) : (y))
+#define MIN(x, y) ((x) < (y) ? (x) : (y))
+
+#define INSTANCES_DEFAULT_SIZE 10
+#define INSTANCES_GROW_SIZE 5
+
+/*
+ * Internal types
+ */
+
+
+/*
+ * Global variables
+ */
+
+/**
+ * VMX values for different color space conversions
+ */
+static const u32 vmx_rgb_to_yuv[] = {
+	B2R2_VMX0_RGB_TO_YUV_601_VIDEO,
+	B2R2_VMX1_RGB_TO_YUV_601_VIDEO,
+	B2R2_VMX2_RGB_TO_YUV_601_VIDEO,
+	B2R2_VMX3_RGB_TO_YUV_601_VIDEO,
+};
+
+static const u32 vmx_rgb_to_blt_yuv888[] = {
+	B2R2_VMX0_RGB_TO_BLT_YUV888_601_VIDEO,
+	B2R2_VMX1_RGB_TO_BLT_YUV888_601_VIDEO,
+	B2R2_VMX2_RGB_TO_BLT_YUV888_601_VIDEO,
+	B2R2_VMX3_RGB_TO_BLT_YUV888_601_VIDEO,
+};
+
+static const u32 vmx_yuv_to_rgb[] = {
+	B2R2_VMX0_YUV_TO_RGB_601_VIDEO,
+	B2R2_VMX1_YUV_TO_RGB_601_VIDEO,
+	B2R2_VMX2_YUV_TO_RGB_601_VIDEO,
+	B2R2_VMX3_YUV_TO_RGB_601_VIDEO,
+};
+
+static const u32 vmx_blt_yuv888_to_rgb[] = {
+	B2R2_VMX0_BLT_YUV888_TO_RGB_601_VIDEO,
+	B2R2_VMX1_BLT_YUV888_TO_RGB_601_VIDEO,
+	B2R2_VMX2_BLT_YUV888_TO_RGB_601_VIDEO,
+	B2R2_VMX3_BLT_YUV888_TO_RGB_601_VIDEO,
+};
+
+static const u32 vmx_yvu_to_rgb[] = {
+	B2R2_VMX0_YVU_TO_RGB_601_VIDEO,
+	B2R2_VMX1_YVU_TO_RGB_601_VIDEO,
+	B2R2_VMX2_YVU_TO_RGB_601_VIDEO,
+	B2R2_VMX3_YVU_TO_RGB_601_VIDEO,
+};
+
+static const u32 vmx_rgb_to_yvu[] = {
+	B2R2_VMX0_RGB_TO_YVU_601_VIDEO,
+	B2R2_VMX1_RGB_TO_YVU_601_VIDEO,
+	B2R2_VMX2_RGB_TO_YVU_601_VIDEO,
+	B2R2_VMX3_RGB_TO_YVU_601_VIDEO,
+};
+
+static const u32 vmx_rgb_to_bgr[] = {
+	B2R2_VMX0_RGB_TO_BGR,
+	B2R2_VMX1_RGB_TO_BGR,
+	B2R2_VMX2_RGB_TO_BGR,
+	B2R2_VMX3_RGB_TO_BGR,
+};
+
+static const u32 vmx_bgr_to_yuv[] = {
+	B2R2_VMX0_BGR_TO_YUV_601_VIDEO,
+	B2R2_VMX1_BGR_TO_YUV_601_VIDEO,
+	B2R2_VMX2_BGR_TO_YUV_601_VIDEO,
+	B2R2_VMX3_BGR_TO_YUV_601_VIDEO,
+};
+
+static const u32 vmx_yuv_to_bgr[] = {
+	B2R2_VMX0_YUV_TO_BGR_601_VIDEO,
+	B2R2_VMX1_YUV_TO_BGR_601_VIDEO,
+	B2R2_VMX2_YUV_TO_BGR_601_VIDEO,
+	B2R2_VMX3_YUV_TO_BGR_601_VIDEO,
+};
+
+static const u32 vmx_bgr_to_yvu[] = {
+	B2R2_VMX0_BGR_TO_YVU_601_VIDEO,
+	B2R2_VMX1_BGR_TO_YVU_601_VIDEO,
+	B2R2_VMX2_BGR_TO_YVU_601_VIDEO,
+	B2R2_VMX3_BGR_TO_YVU_601_VIDEO,
+};
+
+static const u32 vmx_yvu_to_bgr[] = {
+	B2R2_VMX0_YVU_TO_BGR_601_VIDEO,
+	B2R2_VMX1_YVU_TO_BGR_601_VIDEO,
+	B2R2_VMX2_YVU_TO_BGR_601_VIDEO,
+	B2R2_VMX3_YVU_TO_BGR_601_VIDEO,
+};
+
+static const u32 vmx_yvu_to_yuv[] = {
+	B2R2_VMX0_YVU_TO_YUV_601_VIDEO,
+	B2R2_VMX1_YVU_TO_YUV_601_VIDEO,
+	B2R2_VMX2_YVU_TO_YUV_601_VIDEO,
+	B2R2_VMX3_YVU_TO_YUV_601_VIDEO,
+};
+
+/*
+ * Forward declaration of private functions
+ */
+
+static int analyze_fmt_conv(struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		const u32 **vmx, u32 *node_count);
+static int analyze_color_fill(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count);
+static int analyze_copy(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count);
+static int analyze_scaling(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count);
+static int analyze_rotate(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count);
+static int analyze_transform(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count);
+static int analyze_rot_scale(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count);
+static int analyze_scale_factors(struct b2r2_node_split_job *this);
+
+static void configure_src(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src, const u32 *ivmx);
+static int configure_dst(struct b2r2_node *node,
+		struct b2r2_node_split_buf *dst, const u32 *ivmx,
+		struct b2r2_node **next);
+static void configure_blend(struct b2r2_node *node, u32 flags, u32 global_alpha,
+		bool swap_fg_bg);
+static void configure_clip(struct b2r2_node *node,
+		struct b2r2_blt_rect *clip_rect);
+
+static int configure_tile(struct b2r2_node_split_job *this,
+		struct b2r2_node *node, struct b2r2_node **next);
+static void configure_direct_fill(struct b2r2_node *node, u32 color,
+		struct b2r2_node_split_buf *dst, struct b2r2_node **next);
+static int configure_fill(struct b2r2_node *node, u32 color,
+		enum b2r2_blt_fmt fmt, struct b2r2_node_split_buf *dst,
+		const u32 *ivmx, struct b2r2_node **next);
+static void configure_direct_copy(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst, struct b2r2_node **next);
+static int configure_copy(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst, const u32 *ivmx,
+		struct b2r2_node **next,
+		struct b2r2_node_split_job *this);
+static int configure_rotate(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst, const u32 *ivmx,
+		struct b2r2_node **next,
+		struct b2r2_node_split_job *this);
+static int configure_scale(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst, u16 h_rsf, u16 v_rsf,
+		const u32 *ivmx, struct b2r2_node **next,
+		struct b2r2_node_split_job *this);
+static int configure_rot_scale(struct b2r2_node_split_job *this,
+		struct b2r2_node *node, struct b2r2_node **next);
+
+static int check_rect(const struct b2r2_blt_img *img,
+		const struct b2r2_blt_rect *rect,
+		const struct b2r2_blt_rect *clip);
+static void set_buf(struct b2r2_node_split_buf *buf, u32 addr,
+		const struct b2r2_blt_img *img,
+		const struct b2r2_blt_rect *rect, bool color_fill, u32 color);
+static int setup_tmp_buf(struct b2r2_node_split_buf *this, u32 max_size,
+		enum b2r2_blt_fmt pref_fmt, u32 pref_width, u32 pref_height);
+
+static enum b2r2_ty get_alpha_range(enum b2r2_blt_fmt fmt);
+static u32 set_alpha(enum b2r2_blt_fmt fmt, u8 alpha, u32 color);
+static u8  get_alpha(enum b2r2_blt_fmt fmt, u32 pixel);
+static bool fmt_has_alpha(enum b2r2_blt_fmt fmt);
+
+static bool is_rgb_fmt(enum b2r2_blt_fmt fmt);
+static bool is_bgr_fmt(enum b2r2_blt_fmt fmt);
+static bool is_yuv_fmt(enum b2r2_blt_fmt fmt);
+static bool is_yvu_fmt(enum b2r2_blt_fmt fmt);
+static bool is_yuv420_fmt(enum b2r2_blt_fmt fmt);
+static bool is_yuv422_fmt(enum b2r2_blt_fmt fmt);
+static bool is_yuv444_fmt(enum b2r2_blt_fmt fmt);
+static bool is_yvu420_fmt(enum b2r2_blt_fmt fmt);
+static bool is_yvu422_fmt(enum b2r2_blt_fmt fmt);
+
+static int fmt_byte_pitch(enum b2r2_blt_fmt fmt, u32 width);
+static enum b2r2_native_fmt to_native_fmt(enum b2r2_blt_fmt fmt);
+static u32 to_RGB888(u32 color, const enum b2r2_blt_fmt fmt);
+static enum b2r2_fmt_type get_fmt_type(enum b2r2_blt_fmt fmt);
+
+static bool is_transform(const struct b2r2_blt_request *req);
+static int calculate_scale_factor(u32 from, u32 to, u16 *sf_out);
+static s32 rescale(s32 dim, u16 sf);
+static s32 inv_rescale(s32 dim, u16 sf);
+
+static void set_target(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf);
+static void set_src(struct b2r2_src_config *src, u32 addr,
+		struct b2r2_node_split_buf *buf);
+static void set_src_1(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf);
+static void set_src_2(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf);
+static void set_src_3(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf);
+static void set_ivmx(struct b2r2_node *node, const u32 *vmx_values);
+
+static void reset_nodes(struct b2r2_node *node);
+
+/*
+ * Public functions
+ */
+
+/**
+ * b2r2_node_split_analyze() - analyzes the request
+ */
+int b2r2_node_split_analyze(const struct b2r2_blt_request *req,
+		u32 max_buf_size, u32 *node_count, struct b2r2_work_buf **bufs,
+		u32 *buf_count, struct b2r2_node_split_job *this)
+{
+	int ret;
+	bool color_fill;
+
+	b2r2_log_info("%s\n", __func__);
+
+	memset(this, 0, sizeof(*this));
+
+	/* Copy parameters */
+	this->flags = req->user_req.flags;
+	this->transform = req->user_req.transform;
+	this->max_buf_size = max_buf_size;
+	this->global_alpha = req->user_req.global_alpha;
+	this->buf_count = 0;
+	this->node_count = 0;
+
+	if (this->flags & B2R2_BLT_FLAG_BLUR) {
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	/* Unsupported formats on src */
+	switch (req->user_req.src_img.fmt) {
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		if (!is_rgb_fmt(req->user_req.dst_img.fmt)) {
+			ret = -ENOSYS;
+			goto unsupported;
+		}
+		break;
+	default:
+		break;
+	}
+
+	/* Unsupported formats on dst */
+	switch (req->user_req.dst_img.fmt) {
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		if (!is_rgb_fmt(req->user_req.src_img.fmt)) {
+			ret = -ENOSYS;
+			goto unsupported;
+		}
+		break;
+	default:
+		break;
+	}
+
+	if ((this->flags & B2R2_BLT_FLAG_SOURCE_COLOR_KEY) &&
+			(is_yuv_fmt(req->user_req.src_img.fmt) ||
+			req->user_req.src_img.fmt == B2R2_BLT_FMT_1_BIT_A1 ||
+			req->user_req.src_img.fmt == B2R2_BLT_FMT_8_BIT_A8)) {
+		b2r2_log_warn("%s: Unsupported: source color keying with "
+			"YUV or pure alpha formats.\n",
+			__func__);
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	if (this->flags & (B2R2_BLT_FLAG_DEST_COLOR_KEY |
+			B2R2_BLT_FLAG_SOURCE_MASK)) {
+		b2r2_log_warn("%s: Unsupported: source mask, "
+			"destination color keying.\n",
+			__func__);
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) &&
+			req->user_req.clut == NULL) {
+		b2r2_log_warn("%s: Invalid request: no table specified "
+			"for CLUT color correction.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	/* Check for color fill */
+	color_fill = (this->flags & (B2R2_BLT_FLAG_SOURCE_FILL |
+				B2R2_BLT_FLAG_SOURCE_FILL_RAW)) != 0;
+
+	/* Configure the source and destination buffers */
+	set_buf(&this->src, req->src_resolved.physical_address,
+			&req->user_req.src_img, &req->user_req.src_rect,
+			color_fill, req->user_req.src_color);
+
+	set_buf(&this->dst, req->dst_resolved.physical_address,
+			&req->user_req.dst_img, &req->user_req.dst_rect, false,
+			0);
+
+	b2r2_log_info("%s:\n"
+		"\t\tsrc.rect=(%4d, %4d, %4d, %4d)\t"
+		"dst.rect=(%4d, %4d, %4d, %4d)\n", __func__, this->src.rect.x,
+		this->src.rect.y, this->src.rect.width, this->src.rect.height,
+		this->dst.rect.x, this->dst.rect.y, this->dst.rect.width,
+		this->dst.rect.height);
+
+	if (this->flags & B2R2_BLT_FLAG_DITHER)
+		this->dst.dither = B2R2_TTY_RGB_ROUND_DITHER;
+
+	if (this->flags & B2R2_BLT_FLAG_SOURCE_COLOR_KEY)
+		this->flag_param = req->user_req.src_color;
+
+	/* Check for blending */
+	if ((this->flags & B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND) &&
+			(this->global_alpha != 255))
+		this->blend = true;
+	else if (this->flags & B2R2_BLT_FLAG_PER_PIXEL_ALPHA_BLEND)
+		this->blend = (color_fill && fmt_has_alpha(this->dst.fmt)) ||
+				fmt_has_alpha(this->src.fmt);
+
+	if (this->blend && this->src.type == B2R2_FMT_TYPE_PLANAR) {
+		b2r2_log_warn("%s: Unsupported: blend with planar source\n",
+				__func__);
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	/* Check for clipping */
+	this->clip = (this->flags &
+				B2R2_BLT_FLAG_DESTINATION_CLIP) != 0;
+	if (this->clip) {
+		s32 l = req->user_req.dst_clip_rect.x;
+		s32 r = l + req->user_req.dst_clip_rect.width;
+		s32 t = req->user_req.dst_clip_rect.y;
+		s32 b = t + req->user_req.dst_clip_rect.height;
+
+		/* Intersect the clip and buffer rects */
+		if (l < 0)
+			l = 0;
+		if (r > req->user_req.dst_img.width)
+			r = req->user_req.dst_img.width;
+		if (t < 0)
+			t = 0;
+		if (b > req->user_req.dst_img.height)
+			b = req->user_req.dst_img.height;
+
+		this->clip_rect.x = l;
+		this->clip_rect.y = t;
+		this->clip_rect.width = r - l;
+		this->clip_rect.height = b - t;
+	} else {
+		/* Set the clip rectangle to the buffer bounds */
+		this->clip_rect.x = 0;
+		this->clip_rect.y = 0;
+		this->clip_rect.width = req->user_req.dst_img.width;
+		this->clip_rect.height = req->user_req.dst_img.height;
+	}
+
+	/* Validate the destination */
+	ret = check_rect(&req->user_req.dst_img, &req->user_req.dst_rect,
+			&this->clip_rect);
+	if (ret < 0)
+		goto error;
+
+	/* Validate the source (if not color fill) */
+	if (!color_fill) {
+		ret = check_rect(&req->user_req.src_img,
+					&req->user_req.src_rect, NULL);
+		if (ret < 0)
+			goto error;
+	}
+
+	/* Do the analysis depending on the type of operation */
+	if (color_fill) {
+		ret = analyze_color_fill(this, req, &this->node_count);
+	} else {
+
+		bool upsample;
+		bool downsample;
+
+		/*
+		 * YUV formats that are non-raster, non-yuv444 needs to be
+		 * up (or down) sampled using the resizer.
+		 *
+		 * NOTE: The resizer needs to be enabled for YUV444 as well,
+		 *       even though there is no upsampling. This is most
+		 *       likely a bug in the hardware.
+		 */
+		upsample = this->src.type != B2R2_FMT_TYPE_RASTER &&
+			is_yuv_fmt(this->src.fmt);
+		downsample = this->dst.type != B2R2_FMT_TYPE_RASTER &&
+			is_yuv_fmt(this->dst.fmt);
+
+		if (is_transform(req) || upsample || downsample)
+			ret = analyze_transform(this, req, &this->node_count,
+						&this->buf_count);
+		else
+			ret = analyze_copy(this, req, &this->node_count,
+						&this->buf_count);
+	}
+
+	if (ret == -ENOSYS) {
+		goto unsupported;
+	} else if (ret < 0) {
+		b2r2_log_warn("%s: Analysis failed!\n", __func__);
+		goto error;
+	}
+
+	/* Setup the origin and movement of the destination window */
+	if (this->dst.hso == B2R2_TY_HSO_RIGHT_TO_LEFT) {
+		this->dst.dx = -this->dst.win.width;
+		this->dst.win.x = this->dst.rect.x + this->dst.rect.width - 1;
+	} else {
+		this->dst.dx = this->dst.win.width;
+		this->dst.win.x = this->dst.rect.x;
+	}
+	if (this->dst.vso == B2R2_TY_VSO_BOTTOM_TO_TOP) {
+		this->dst.dy = -this->dst.win.height;
+		this->dst.win.y = this->dst.rect.y + this->dst.rect.height - 1;
+	} else {
+		this->dst.dy = this->dst.win.height;
+		this->dst.win.y = this->dst.rect.y;
+	}
+
+	*buf_count = this->buf_count;
+	*node_count = this->node_count;
+
+	if (this->buf_count > 0)
+		*bufs = &this->work_bufs[0];
+
+	b2r2_log_info("%s: dst.win=(%d, %d, %d, %d), dst.dx=%d, dst.dy=%d\n",
+			__func__, this->dst.win.x, this->dst.win.y,
+			this->dst.win.width, this->dst.win.height, this->dst.dx,
+			this->dst.dy);
+	if (this->buf_count > 0)
+		b2r2_log_info("%s: buf_count=%d, buf_size=%d, node_count=%d\n",
+			__func__, *buf_count, bufs[0]->size, *node_count);
+	else
+		b2r2_log_info("%s: buf_count=%d, node_count=%d\n",
+			__func__, *buf_count, *node_count);
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+unsupported:
+	return ret;
+}
+
+/**
+ * b2r2_node_split_configure() - configures the node list
+ */
+int b2r2_node_split_configure(struct b2r2_node_split_job *this,
+		struct b2r2_node *first)
+{
+	int ret;
+
+	struct b2r2_node_split_buf *dst = &this->dst;
+	struct b2r2_node *node = first;
+
+	u32 x_pixels = 0;
+	u32 y_pixels = 0;
+
+	reset_nodes(node);
+
+	while (y_pixels < dst->rect.height) {
+		s32 dst_x = dst->win.x;
+		s32 dst_w = dst->win.width;
+
+		/* Clamp window height */
+		if (dst->win.height > dst->rect.height - y_pixels)
+			dst->win.height = dst->rect.height - y_pixels;
+
+		while (x_pixels < dst->rect.width) {
+
+			/* Clamp window width */
+			if (dst_w > dst->rect.width - x_pixels)
+				dst->win.width = dst->rect.width - x_pixels;
+
+			ret = configure_tile(this, node, &node);
+			if (ret < 0)
+				goto error;
+
+			dst->win.x += dst->dx;
+			x_pixels += max(dst->dx, -dst->dx);
+			b2r2_log_info("%s: x_pixels=%d\n", __func__, x_pixels);
+		}
+
+		dst->win.y += dst->dy;
+		y_pixels += max(dst->dy, -dst->dy);
+
+		dst->win.x = dst_x;
+		dst->win.width = dst_w;
+		x_pixels = 0;
+
+		b2r2_log_info("%s: y_pixels=%d\n", __func__, y_pixels);
+	}
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * b2r2_node_split_assign_buffers() - assigns temporary buffers to the node list
+ */
+int b2r2_node_split_assign_buffers(struct b2r2_node_split_job *this,
+		struct b2r2_node *first, struct b2r2_work_buf *bufs,
+		u32 buf_count)
+{
+	struct b2r2_node *node = first;
+
+	while (node != NULL) {
+		/* The indices are offset by one */
+		if (node->dst_tmp_index) {
+			BUG_ON(node->dst_tmp_index > buf_count);
+
+			b2r2_log_info("%s: assigning buf %d as dst\n",
+					__func__, node->dst_tmp_index);
+
+			node->node.GROUP1.B2R2_TBA =
+				bufs[node->dst_tmp_index - 1].phys_addr;
+		}
+		if (node->src_tmp_index) {
+			u32 addr = bufs[node->src_tmp_index - 1].phys_addr;
+
+			b2r2_log_info("%s: "
+				"assigning buf %d as src %d ", __func__,
+					node->src_tmp_index, node->src_index);
+
+			BUG_ON(node->src_tmp_index > buf_count);
+
+			switch (node->src_index) {
+			case 1:
+				b2r2_log_info("1\n");
+				node->node.GROUP3.B2R2_SBA = addr;
+				break;
+			case 2:
+				b2r2_log_info("2\n");
+				node->node.GROUP4.B2R2_SBA = addr;
+				break;
+			case 3:
+				b2r2_log_info("3\n");
+				node->node.GROUP5.B2R2_SBA = addr;
+				break;
+			default:
+				BUG_ON(1);
+				break;
+			}
+		}
+
+		b2r2_log_info("%s: tba=%p\tsba=%p\n", __func__,
+			(void *)node->node.GROUP1.B2R2_TBA,
+			(void *)node->node.GROUP4.B2R2_SBA);
+
+		node = node->next;
+	}
+
+	return 0;
+}
+
+/**
+ * b2r2_node_split_unassign_buffers() - releases temporary buffers
+ */
+void b2r2_node_split_unassign_buffers(struct b2r2_node_split_job *this,
+		struct b2r2_node *first)
+{
+	return;
+}
+
+/**
+ * b2r2_node_split_cancel() - cancels and releases a job instance
+ */
+void b2r2_node_split_cancel(struct b2r2_node_split_job *this)
+{
+	memset(this, 0, sizeof(*this));
+
+	return;
+}
+
+/*
+ * Private functions
+ */
+
+static int check_rect(const struct b2r2_blt_img *img,
+		const struct b2r2_blt_rect *rect,
+		const struct b2r2_blt_rect *clip)
+{
+	int ret;
+
+	s32 l, r, b, t;
+
+	/* Check rectangle dimensions*/
+	if ((rect->width <= 0) || (rect->height <= 0)) {
+		b2r2_log_warn("%s: "
+			"Illegal rect (%d, %d, %d, %d)\n",
+			__func__, rect->x, rect->y, rect->width, rect->height);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/* If we are using clip we should only look at the intersection of the
+	   rects */
+	if (clip) {
+		l = MAX(rect->x, clip->x);
+		t = MAX(rect->y, clip->y);
+		r = MIN(rect->x + rect->width, clip->x + clip->width);
+		b = MIN(rect->y + rect->height, clip->y + clip->height);
+	} else {
+		l = rect->x;
+		t = rect->y;
+		r = rect->x + rect->width;
+		b = rect->y + rect->height;
+	}
+
+	/* Check so that the rect isn't outside the buffer */
+	if ((l < 0) || (t < 0) || (l >= img->width) || (t >= img->height)) {
+		b2r2_log_warn("%s: "
+			"rect origin outside buffer\n", __func__);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	if ((r > img->width) || (b > img->height)) {
+		b2r2_log_warn("%s: "
+			"rect ends outside buffer\n", __func__);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/* Check so the intersected rectangle isn't empty */
+	if ((l == r) || (t == b)) {
+		b2r2_log_warn("%s: "
+			"rect is empty (width or height zero)\n",
+			__func__);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	return 0;
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * analyze_fmt_conv() - analyze the format conversions needed for a job
+ */
+static int analyze_fmt_conv(struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		const u32 **vmx, u32 *node_count)
+{
+	if (is_rgb_fmt(src->fmt)) {
+		if (is_yvu_fmt(dst->fmt))
+			*vmx = &vmx_rgb_to_yvu[0];
+		else if (dst->fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst->fmt == B2R2_BLT_FMT_32_BIT_AYUV8888)
+			*vmx = &vmx_rgb_to_blt_yuv888[0];
+		else if (is_yuv_fmt(dst->fmt))
+			*vmx = &vmx_rgb_to_yuv[0];
+		else if (is_bgr_fmt(dst->fmt))
+			*vmx = &vmx_rgb_to_bgr[0];
+	} else if (is_yvu_fmt(src->fmt)) {
+		if (is_rgb_fmt(dst->fmt))
+			*vmx = &vmx_yvu_to_rgb[0];
+		else if (is_bgr_fmt(dst->fmt))
+			*vmx = &vmx_yvu_to_bgr[0];
+		else if (dst->fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst->fmt == B2R2_BLT_FMT_32_BIT_AYUV8888)
+			BUG_ON(1);
+		else if (is_yuv_fmt(dst->fmt) &&
+				!is_yvu_fmt(dst->fmt))
+			*vmx = &vmx_yvu_to_yuv[0];
+	} else if (src->fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+			src->fmt == B2R2_BLT_FMT_32_BIT_AYUV8888) {
+		if (is_rgb_fmt(dst->fmt))
+			*vmx = &vmx_blt_yuv888_to_rgb[0];
+		else
+			/*
+			 * not supported, b2r2_node_split_analyze()
+			 * should have returned ENOSYS.
+			 */
+			BUG_ON(1);
+	} else if (is_yuv_fmt(src->fmt)) {
+		if (is_rgb_fmt(dst->fmt))
+			*vmx = &vmx_yuv_to_rgb[0];
+		else if (is_bgr_fmt(dst->fmt))
+			*vmx = &vmx_yuv_to_bgr[0];
+		else if (dst->fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst->fmt == B2R2_BLT_FMT_32_BIT_AYUV8888)
+			BUG_ON(1);
+		else if (is_yvu_fmt(dst->fmt))
+			*vmx = &vmx_yvu_to_yuv[0];
+	} else if (is_bgr_fmt(src->fmt)) {
+		if (is_rgb_fmt(dst->fmt))
+			*vmx = &vmx_rgb_to_bgr[0];
+		else if (is_yvu_fmt(dst->fmt))
+			*vmx = &vmx_bgr_to_yvu[0];
+		else if (dst->fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst->fmt == B2R2_BLT_FMT_32_BIT_AYUV8888)
+			BUG_ON(1);
+		else if (is_yuv_fmt(dst->fmt))
+			*vmx = &vmx_bgr_to_yuv[0];
+	}
+
+	if (dst->type == B2R2_FMT_TYPE_RASTER) {
+		*node_count = 1;
+	} else if (dst->type == B2R2_FMT_TYPE_SEMI_PLANAR) {
+		*node_count = 2;
+	} else if (dst->type == B2R2_FMT_TYPE_PLANAR) {
+		*node_count = 3;
+	} else {
+		/* That's strange... */
+		BUG_ON(1);
+	}
+
+	return 0;
+}
+
+/**
+ * analyze_color_fill() - analyze a color fill operation
+ */
+static int analyze_color_fill(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count)
+{
+	int ret;
+
+	/* Destination must be raster for raw fill to work */
+	if (this->dst.type != B2R2_FMT_TYPE_RASTER) {
+		b2r2_log_warn("%s: fill requires raster destination\n",
+			__func__);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/* We will try to fill the entire rectangle in one go */
+	memcpy(&this->dst.win, &this->dst.rect, sizeof(this->dst.win));
+
+	/* Check if this is a direct fill */
+	if ((!this->blend) && ((this->flags & B2R2_BLT_FLAG_SOURCE_FILL_RAW) ||
+			(this->dst.fmt == B2R2_BLT_FMT_32_BIT_ARGB8888) ||
+			(this->dst.fmt == B2R2_BLT_FMT_32_BIT_ABGR8888) ||
+			(this->dst.fmt == B2R2_BLT_FMT_32_BIT_AYUV8888))) {
+		this->type = B2R2_DIRECT_FILL;
+
+		/* The color format will be the same as the dst fmt */
+		this->src.fmt = this->dst.fmt;
+
+		/* The entire destination rectangle will be  */
+		memcpy(&this->dst.win, &this->dst.rect,
+			sizeof(this->dst.win));
+		*node_count = 1;
+	} else {
+		this->type = B2R2_FILL;
+
+		/* Determine the fill color format */
+		if (this->flags & B2R2_BLT_FLAG_SOURCE_FILL_RAW) {
+			/* The color format will be the same as the dst fmt */
+			this->src.fmt = this->dst.fmt;
+		} else {
+			/* If the dst fmt is YUV the fill fmt will be as well */
+			if (is_yuv_fmt(this->dst.fmt)) {
+				this->src.fmt =	B2R2_BLT_FMT_32_BIT_AYUV8888;
+			} else if (is_rgb_fmt(this->dst.fmt)) {
+				this->src.fmt =	B2R2_BLT_FMT_32_BIT_ARGB8888;
+			} else if (is_bgr_fmt(this->dst.fmt)) {
+				/* Color will still be ARGB, we will translate
+				   using IVMX (configured later) */
+				this->src.fmt =	B2R2_BLT_FMT_32_BIT_ARGB8888;
+			} else {
+				/* Wait, what? */
+				b2r2_log_warn("%s: "
+					"Illegal destination format for fill",
+					__func__);
+				ret = -EINVAL;
+				goto error;
+			}
+		}
+
+		/* Also, B2R2 seems to ignore the pixel alpha value */
+		if (((this->flags & B2R2_BLT_FLAG_PER_PIXEL_ALPHA_BLEND)
+					!= 0) &&
+				((this->flags & B2R2_BLT_FLAG_SOURCE_FILL_RAW)
+					== 0) && fmt_has_alpha(this->src.fmt)) {
+			u8 pixel_alpha = get_alpha(this->src.fmt,
+							this->src.color);
+			u32 new_global = pixel_alpha * this->global_alpha / 255;
+
+			this->global_alpha = (u8)new_global;
+
+			/* Set the pixel alpha to full opaque so we don't get
+			   any nasty suprises */
+			this->src.color = set_alpha(this->src.fmt, 0xFF,
+						this->src.color);
+		}
+
+		ret = analyze_fmt_conv(&this->src, &this->dst, &this->ivmx,
+				node_count);
+		if (ret < 0)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+
+}
+
+/**
+ * analyze_transform() - analyze a transform operation (rescale, rotate, etc.)
+ */
+static int analyze_transform(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+
+	bool is_scaling;
+
+	b2r2_log_info("%s\n", __func__);
+
+	/*
+	 * The transform enum is defined so that all rotation transforms are
+	 * masked with the rotation flag
+	 */
+	this->rotation = (this->transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) != 0;
+
+	/* B2R2 cannot do rotations if the destination is not raster, or 422R */
+	if (this->rotation && (this->dst.type != B2R2_FMT_TYPE_RASTER ||
+				this->dst.fmt == B2R2_BLT_FMT_Y_CB_Y_CR ||
+				this->dst.fmt == B2R2_BLT_FMT_CB_Y_CR_Y)) {
+		b2r2_log_warn("%s: Unsupported operation "
+				"(rot && (!dst_raster || dst==422R))",
+				__func__);
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	/* Flip the image by changing the scan order of the destination */
+	if (this->transform & B2R2_BLT_TRANSFORM_FLIP_H)
+		this->dst.hso = B2R2_TY_HSO_RIGHT_TO_LEFT;
+	if (this->transform & B2R2_BLT_TRANSFORM_FLIP_V)
+		this->dst.vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+
+	/* Check for scaling */
+	if (this->rotation) {
+		is_scaling = (this->src.rect.width != this->dst.rect.height) ||
+			(this->src.rect.height != this->dst.rect.width);
+	} else {
+		is_scaling = (this->src.rect.width != this->dst.rect.width) ||
+			(this->src.rect.height != this->dst.rect.height);
+	}
+
+	/* Plane separated formats must be treated as scaling */
+	is_scaling = is_scaling ||
+			(this->src.type == B2R2_FMT_TYPE_SEMI_PLANAR) ||
+			(this->src.type == B2R2_FMT_TYPE_PLANAR) ||
+			(this->dst.type == B2R2_FMT_TYPE_SEMI_PLANAR) ||
+			(this->dst.type == B2R2_FMT_TYPE_PLANAR);
+
+	if (is_scaling && this->rotation && this->blend) {
+		/* TODO: This is unsupported. Fix it! */
+		b2r2_log_info("%s: Unsupported operation (rot+rescale+blend)\n",
+			__func__);
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	/* Check which type of transform */
+	if (is_scaling && this->rotation) {
+		ret = analyze_rot_scale(this, req, node_count, buf_count);
+		if (ret < 0)
+			goto error;
+	} else if (is_scaling) {
+		ret = analyze_scaling(this, req, node_count, buf_count);
+		if (ret < 0)
+			goto error;
+	} else if (this->rotation) {
+		ret = analyze_rotate(this, req, node_count, buf_count);
+		if (ret < 0)
+			goto error;
+	} else {
+		/* No additional nodes needed for a flip */
+		ret = analyze_copy(this, req, node_count, buf_count);
+		if (ret < 0)
+			goto error;
+		this->type = B2R2_FLIP;
+	}
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: error!\n", __func__);
+unsupported:
+	return ret;
+}
+
+/**
+ * analyze_copy() - analyze a copy operation
+ */
+static int analyze_copy(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+
+	memcpy(&this->dst.win, &this->dst.rect, sizeof(this->dst.win));
+
+	if (!this->blend &&
+			!(this->flags & B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) &&
+			(this->src.fmt == this->dst.fmt) &&
+			(this->src.type == B2R2_FMT_TYPE_RASTER) &&
+			(this->dst.rect.x >= this->clip_rect.x) &&
+			(this->dst.rect.y >= this->clip_rect.y) &&
+			(this->dst.rect.x + this->dst.rect.width <=
+				this->clip_rect.x + this->clip_rect.width) &&
+			(this->dst.rect.y + this->dst.rect.height <=
+				this->clip_rect.y + this->clip_rect.height)) {
+		this->type = B2R2_DIRECT_COPY;
+		*node_count = 1;
+	} else {
+		u32 copy_count;
+
+		this->type = B2R2_COPY;
+
+		ret = analyze_fmt_conv(&this->src, &this->dst, &this->ivmx,
+				&copy_count);
+		if (ret < 0)
+			goto error;
+
+		*node_count = copy_count;
+	}
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+}
+
+static int calc_rot_count(u32 width, u32 height)
+{
+	int count;
+
+	count = width / B2R2_ROTATE_MAX_WIDTH;
+	if (width % B2R2_ROTATE_MAX_WIDTH)
+		count++;
+	if (height > B2R2_ROTATE_MAX_WIDTH &&
+			height % B2R2_ROTATE_MAX_WIDTH)
+		count *= 2;
+
+	return count;
+}
+
+static int analyze_rot_scale_downscale(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+
+	struct b2r2_node_split_buf *src = &this->src;
+	struct b2r2_node_split_buf *dst = &this->dst;
+	struct b2r2_node_split_buf *tmp = &this->tmp_bufs[0];
+
+	u32 num_rows;
+	u32 num_cols;
+
+	u32 rot_count;
+	u32 rescale_count;
+
+	u32 nodes_per_rot;
+	u32 nodes_per_rescale;
+
+	u32 right_width;
+	u32 bottom_height;
+
+	const u32 *dummy_vmx;
+
+	b2r2_log_info("%s\n", __func__);
+
+	/* Calculate the desired tmp buffer size */
+	tmp->win.width = rescale(B2R2_RESCALE_MAX_WIDTH - 1, this->h_rsf);
+	tmp->win.width >>= 10;
+	tmp->win.width = min(tmp->win.width, dst->rect.height);
+	tmp->win.height = dst->rect.width;
+
+	setup_tmp_buf(tmp, this->max_buf_size, dst->fmt, tmp->win.width,
+			tmp->win.height);
+	tmp->tmp_buf_index = 1;
+	this->work_bufs[0].size = tmp->pitch * tmp->height;
+
+	tmp->win.width = tmp->rect.width;
+	tmp->win.height = tmp->rect.height;
+
+	tmp->dither = dst->dither;
+	dst->dither = 0;
+
+	/* Update the dst window with the actual tmp buffer dimensions */
+	dst->win.width = tmp->win.height;
+	dst->win.height = tmp->win.width;
+
+	/* The rotated stripes are written to the destination bottom-up */
+	if (this->dst.vso == B2R2_TY_VSO_TOP_TO_BOTTOM)
+		this->dst.vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+	else
+		this->dst.vso = B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	/*
+	 * Calculate how many nodes are required to copy to and from the tmp
+	 * buffer
+	 */
+	ret = analyze_fmt_conv(src, tmp, &this->ivmx, &nodes_per_rescale);
+	if (ret < 0)
+		goto error;
+
+	/* We will not do any format conversion in the rotation stage */
+	ret = analyze_fmt_conv(tmp, dst, &dummy_vmx, &nodes_per_rot);
+	if (ret < 0)
+		goto error;
+
+	/* Calculate node count for the inner tiles */
+	num_cols = dst->rect.width / dst->win.width;
+	num_rows = dst->rect.height / dst->win.height;
+
+	rescale_count = num_cols * num_rows;
+	rot_count = calc_rot_count(dst->win.height, dst->win.width) *
+		num_cols * num_rows;
+
+	right_width = dst->rect.width % dst->win.width;
+	bottom_height = dst->rect.height % dst->win.height;
+
+	/* Calculate node count for the rightmost tiles */
+	if (right_width) {
+		u32 count = calc_rot_count(dst->win.height, right_width);
+
+		rot_count += count * num_rows;
+		rescale_count += num_rows;
+		b2r2_log_info("%s: rightmost: %d nodes\n", __func__,
+				count*num_rows);
+	}
+
+	/* Calculate node count for the bottom tiles */
+	if (bottom_height) {
+		u32 count = calc_rot_count(bottom_height, dst->win.width);
+
+		rot_count += count * num_cols;
+		rescale_count += num_cols;
+		b2r2_log_info("%s: bottom: %d nodes\n", __func__,
+				count * num_cols);
+
+	}
+
+	/* And finally for the bottom right corner */
+	if (right_width && bottom_height) {
+		u32 count = calc_rot_count(bottom_height, right_width);
+
+		rot_count += count;
+		rescale_count++;
+		b2r2_log_info("%s: bottom right: %d nodes\n", __func__, count);
+
+	}
+
+	*node_count = rot_count * nodes_per_rot;
+	*node_count += rescale_count * nodes_per_rescale;
+	*buf_count = 1;
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: error!\n", __func__);
+	return ret;
+}
+
+static int analyze_rot_scale_upscale(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	/* TODO: When upscaling we should optimally to the rotation first... */
+	return analyze_rot_scale_downscale(this, req, node_count, buf_count);
+}
+
+/**
+ * analyze_rot_scaling() - analyzes a combined rotation and scaling op
+ */
+static int analyze_rot_scale(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+
+	bool upscale;
+
+	ret = analyze_scale_factors(this);
+	if (ret < 0)
+		goto error;
+
+	upscale = (u32)this->h_rsf * (u32)this->v_rsf < (1 << 20);
+
+	if (upscale)
+		ret = analyze_rot_scale_upscale(this, req, node_count,
+			buf_count);
+	else
+		ret = analyze_rot_scale_downscale(this, req, node_count,
+			buf_count);
+
+	if (ret < 0)
+		goto error;
+
+	this->type = B2R2_SCALE_AND_ROTATE;
+
+	return 0;
+
+error:
+	return ret;
+}
+
+/**
+ * analyze_scaling() - analyze a rescale operation
+ */
+static int analyze_scaling(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+
+	u32 copy_count;
+	u32 nbr_cols;
+
+	s32 dst_w;
+
+	b2r2_log_info("%s\n", __func__);
+
+	ret = analyze_scale_factors(this);
+	if (ret < 0)
+		goto error;
+
+	/* Find out how many nodes a simple copy would require */
+	ret = analyze_fmt_conv(&this->src, &this->dst, &this->ivmx,
+			&copy_count);
+	if (ret < 0)
+		goto error;
+
+	memcpy(&this->dst.win, &this->dst.rect, sizeof(this->dst.win));
+
+	/*
+	 * We need to subtract from the actual maximum rescale width since the
+	 * start of the stripe will be floored and the end ceiled. This could in
+	 * some cases cause the stripe to be one pixel more than the maximum
+	 * width.
+	 *
+	 * Example:
+	 *   x = 127.8, w = 127.8
+	 *
+	 *   The stripe will touch pixels 127.8 through 255.6, i.e. 129 pixels.
+	 */
+	dst_w = rescale(B2R2_RESCALE_MAX_WIDTH - 1, this->h_rsf);
+	if (dst_w < (1 << 10))
+		dst_w = 1;
+	else
+		dst_w >>= 10;
+
+	b2r2_log_info("%s: dst_w=%d dst.rect.width=%d\n", __func__, dst_w,
+			this->dst.rect.width);
+
+	this->dst.win.width = min(dst_w, this->dst.rect.width);
+
+	b2r2_log_info("%s: dst.win.width=%d\n", __func__, this->dst.win.width);
+
+	nbr_cols = this->dst.rect.width / this->dst.win.width;
+	if (this->dst.rect.width % this->dst.win.width)
+		nbr_cols++;
+
+	*node_count = copy_count * nbr_cols;
+
+	this->type = B2R2_SCALE;
+
+	b2r2_log_info("%s exit\n", __func__);
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+
+}
+
+/**
+ * analyze_rotate() - analyze a rotate operation
+ */
+static int analyze_rotate(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+
+	u32 nodes_per_tile;
+
+	/* Find out how many nodes a simple copy would require */
+	ret = analyze_fmt_conv(&this->src, &this->dst, &this->ivmx,
+			&nodes_per_tile);
+	if (ret < 0)
+		goto error;
+
+	this->type = B2R2_ROTATE;
+
+	/* The rotated stripes are written to the destination bottom-up */
+	if (this->dst.vso == B2R2_TY_VSO_TOP_TO_BOTTOM)
+		this->dst.vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+	else
+		this->dst.vso = B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	memcpy(&this->dst.win, &this->dst.rect, sizeof(this->dst.win));
+
+	this->dst.win.height = min(this->dst.win.height, B2R2_ROTATE_MAX_WIDTH);
+
+	/*
+	 * B2R2 cannot do rotations on stripes that are not a multiple of 16
+	 * pixels high (if larger than 16 pixels).
+	 */
+	if (this->dst.win.width > 16)
+		this->dst.win.width -= (this->dst.win.width % 16);
+
+	/* Blending cannot be combined with rotation */
+	if (this->blend) {
+		struct b2r2_node_split_buf *tmp = &this->tmp_bufs[0];
+		enum b2r2_blt_fmt tmp_fmt;
+
+		if (is_yuv_fmt(this->dst.fmt))
+			tmp_fmt = B2R2_BLT_FMT_32_BIT_AYUV8888;
+		else if (is_bgr_fmt(this->dst.fmt))
+			tmp_fmt = B2R2_BLT_FMT_32_BIT_ABGR8888;
+		else
+			tmp_fmt = B2R2_BLT_FMT_32_BIT_ARGB8888;
+
+		setup_tmp_buf(tmp, this->max_buf_size, tmp_fmt,
+				this->dst.win.width, this->dst.win.height);
+
+		tmp->tmp_buf_index = 1;
+
+		tmp->vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+
+		this->dst.win.width = tmp->rect.width;
+		this->dst.win.height = tmp->rect.height;
+
+		memcpy(&tmp->win, &tmp->rect, sizeof(tmp->win));
+
+		*buf_count = 1;
+		this->work_bufs[0].size = tmp->pitch * tmp->height;
+
+		/*
+		 * One more node per tile is required to rotate to the temp
+		 * buffer.
+		 */
+		nodes_per_tile++;
+	}
+
+	/* Finally, calculate the node count */
+	*node_count = nodes_per_tile *
+		calc_rot_count(this->src.rect.width, this->src.rect.height);
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * analyze_scale_factors() - determines the scale factors for the op
+ */
+static int analyze_scale_factors(struct b2r2_node_split_job *this)
+{
+	int ret;
+
+	u16 hsf;
+	u16 vsf;
+
+	if (this->rotation) {
+		ret = calculate_scale_factor(this->src.rect.width,
+					this->dst.rect.height, &hsf);
+		if (ret < 0)
+			goto error;
+
+		ret = calculate_scale_factor(this->src.rect.height,
+					this->dst.rect.width, &vsf);
+		if (ret < 0)
+			goto error;
+	} else {
+		ret = calculate_scale_factor(this->src.rect.width,
+					this->dst.rect.width, &hsf);
+		if (ret < 0)
+			goto error;
+
+		ret = calculate_scale_factor(this->src.rect.height,
+					this->dst.rect.height, &vsf);
+		if (ret < 0)
+			goto error;
+	}
+
+	this->h_rescale = hsf != (1 << 10);
+	this->v_rescale = vsf != (1 << 10);
+
+	this->h_rsf = hsf;
+	this->v_rsf = vsf;
+
+	b2r2_log_info("%s: h_rsf=%.4x\n", __func__, this->h_rsf);
+	b2r2_log_info("%s: v_rsf=%.4x\n", __func__, this->v_rsf);
+
+	return 0;
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_tile() - configures one tile of a blit operation
+ */
+static int configure_tile(struct b2r2_node_split_job *this,
+		struct b2r2_node *node, struct b2r2_node **next)
+{
+	int ret = 0;
+
+	struct b2r2_node *last;
+	struct b2r2_node_split_buf *src = &this->src;
+	struct b2r2_node_split_buf *dst = &this->dst;
+
+	struct b2r2_blt_rect dst_norm;
+	struct b2r2_blt_rect src_norm;
+
+	/* Normalize the dest coords to the dest rect coordinate space  */
+	dst_norm.x = dst->win.x - dst->rect.x;
+	dst_norm.y = dst->win.y - dst->rect.y;
+	dst_norm.width = dst->win.width;
+	dst_norm.height = dst->win.height;
+
+	if (dst->vso == B2R2_TY_VSO_BOTTOM_TO_TOP) {
+		/* The y coord should be counted from the bottom */
+		dst_norm.y = dst->rect.height - (dst_norm.y + 1);
+	}
+	if (dst->hso == B2R2_TY_HSO_RIGHT_TO_LEFT) {
+		/* The x coord should be counted from the right */
+		dst_norm.x = dst->rect.width - (dst_norm.x + 1);
+	}
+
+	/* If the destination is rotated we should swap x, y */
+	if (this->rotation) {
+		src_norm.x = dst_norm.y;
+		src_norm.y = dst_norm.x;
+		src_norm.width = dst_norm.height;
+		src_norm.height = dst_norm.width;
+	} else {
+		src_norm.x = dst_norm.x;
+		src_norm.y = dst_norm.y;
+		src_norm.width = dst_norm.width;
+		src_norm.height = dst_norm.height;
+	}
+
+	/* Convert to src coordinate space */
+	src->win.x = src_norm.x + src->rect.x;
+	src->win.y = src_norm.y + src->rect.y;
+	src->win.width = src_norm.width;
+	src->win.height = src_norm.height;
+
+	/* Do the configuration depending on operation type */
+	switch (this->type) {
+	case B2R2_DIRECT_FILL:
+		configure_direct_fill(node, this->src.color, dst, &last);
+		break;
+
+	case B2R2_DIRECT_COPY:
+		configure_direct_copy(node, src, dst, &last);
+		break;
+
+	case B2R2_FILL:
+		ret = configure_fill(node, src->color, src->fmt,
+				dst, this->ivmx, &last);
+		break;
+
+	case B2R2_FLIP: /* FLIP is just a copy with different VSO/HSO */
+	case B2R2_COPY:
+		ret = configure_copy(node, src, dst, this->ivmx, &last, this);
+		break;
+
+	case B2R2_ROTATE:
+		{
+			struct b2r2_node_split_buf *tmp = &this->tmp_bufs[0];
+
+			if (this->blend) {
+				b2r2_log_info("%s: rotation + blend\n",
+						__func__);
+
+				tmp->win.x = 0;
+				tmp->win.y = tmp->win.height - 1;
+				tmp->win.width = dst->win.width;
+				tmp->win.height = dst->win.height;
+
+				/* Rotate to the temp buf */
+				ret = configure_rotate(node, src, tmp,
+						this->ivmx, &node, NULL);
+				if (ret < 0)
+					goto error;
+
+				/* Then do a copy to the destination */
+				ret = configure_copy(node, tmp, dst, NULL,
+						&last, this);
+			} else {
+				/* Just do a rotation */
+				ret = configure_rotate(node, src, dst,
+						this->ivmx, &last, this);
+			}
+		}
+		break;
+
+	case B2R2_SCALE:
+		ret = configure_scale(node, src, dst, this->h_rsf, this->v_rsf,
+				this->ivmx, &last, this);
+		break;
+
+	case B2R2_SCALE_AND_ROTATE:
+		ret = configure_rot_scale(this, node, &last);
+		break;
+
+	default:
+		b2r2_log_warn("%s: Unsupported request\n", __func__);
+		ret = -ENOSYS;
+		goto error;
+		break;
+
+	}
+
+	if (ret < 0)
+		goto error;
+
+	/* Scale and rotate will configure its own blending and clipping */
+	if (this->type != B2R2_SCALE_AND_ROTATE) {
+
+		/* Configure blending and clipping */
+		do {
+			if (node == NULL) {
+				b2r2_log_warn("%s: "
+					"Internal error! Out of nodes!\n",
+					__func__);
+				ret = -ENOMEM;
+				goto error;
+			}
+
+			if (this->blend)
+				configure_blend(node, this->flags,
+						this->global_alpha,
+						this->swap_fg_bg);
+			if (this->clip)
+				configure_clip(node, &this->clip_rect);
+
+			node = node->next;
+
+		} while (node != last);
+	}
+
+	/* Consume the nodes */
+	*next = last;
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: Error!\n", __func__);
+	return ret;
+}
+
+/*
+ * configure_sub_rot() - configure a sub-rotation
+ *
+ * This functions configures a set of nodes for rotation using the destination
+ * window instead of the rectangle for calculating tiles.
+ */
+static int configure_sub_rot(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		const u32 *ivmx, struct b2r2_node **next,
+		struct b2r2_node_split_job *job)
+{
+	int ret;
+
+	struct b2r2_blt_rect src_win;
+	struct b2r2_blt_rect dst_win;
+
+	u32 y_pixels = 0;
+	u32 x_pixels = 0;
+
+	memcpy(&src_win, &src->win, sizeof(src_win));
+	memcpy(&dst_win, &dst->win, sizeof(dst_win));
+
+	b2r2_log_info("%s: src_win=(%d, %d, %d, %d) "
+			"dst_win=(%d, %d, %d, %d)\n", __func__,
+			src_win.x, src_win.y, src_win.width, src_win.height,
+			dst_win.x, dst_win.y, dst_win.width, dst_win.height);
+
+	dst->win.height = B2R2_ROTATE_MAX_WIDTH;
+	if (dst->win.width % B2R2_ROTATE_MAX_WIDTH)
+		dst->win.width -= dst->win.width % B2R2_ROTATE_MAX_WIDTH;
+
+	while (x_pixels < dst_win.width) {
+		u32 src_x = src->win.x;
+		u32 src_w = src->win.width;
+		u32 dst_y = dst->win.y;
+		u32 dst_h = dst->win.height;
+
+		dst->win.width = min(dst->win.width,
+				dst_win.width - (int)x_pixels);
+		src->win.height = dst->win.width;
+
+		b2r2_log_info("%s: x_pixels=%d\n", __func__, x_pixels);
+
+		while (y_pixels < dst_win.height) {
+			dst->win.height = min(dst->win.height,
+					dst_win.height - (int)y_pixels);
+			src->win.width = dst->win.height;
+
+			b2r2_log_info("%s: y_pixels=%d\n", __func__, y_pixels);
+
+			ret = configure_rotate(node, src, dst, ivmx, &node,
+					job);
+			if (ret < 0)
+				goto error;
+
+			src->win.x += (src->hso == B2R2_TY_HSO_LEFT_TO_RIGHT) ?
+					src->win.width : -src->win.width;
+			dst->win.y += (dst->vso == B2R2_TY_VSO_TOP_TO_BOTTOM) ?
+					dst->win.height : -dst->win.height;
+
+			y_pixels += dst->win.height;
+		}
+
+		src->win.x = src_x;
+		src->win.y += (src->vso == B2R2_TY_VSO_TOP_TO_BOTTOM) ?
+				src->win.height : -src->win.height;
+		src->win.width = src_w;
+
+		dst->win.x += (dst->hso == B2R2_TY_HSO_LEFT_TO_RIGHT) ?
+				dst->win.width : -dst->win.width;
+		dst->win.y = dst_y;
+		dst->win.height = dst_h;
+
+		x_pixels += dst->win.width;
+		y_pixels = 0;
+
+	}
+
+	memcpy(&src->win, &src_win, sizeof(src->win));
+	memcpy(&dst->win, &dst_win, sizeof(dst->win));
+
+	*next = node;
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_rot_downscale() - configures a combined rotate and downscale
+ *
+ * When doing a downscale it is better to do the rotation last.
+ */
+static int configure_rot_downscale(struct b2r2_node_split_job *this,
+		struct b2r2_node *node, struct b2r2_node **next)
+{
+	int ret;
+
+	struct b2r2_node_split_buf *src = &this->src;
+	struct b2r2_node_split_buf *dst = &this->dst;
+	struct b2r2_node_split_buf *tmp = &this->tmp_bufs[0];
+
+	tmp->win.x = 0;
+	tmp->win.y = 0;
+	tmp->win.width = dst->win.height;
+	tmp->win.height = dst->win.width;
+
+	ret = configure_scale(node, src, tmp, this->h_rsf, this->v_rsf,
+			this->ivmx, &node, this);
+	if (ret < 0)
+		goto error;
+
+	ret = configure_sub_rot(node, tmp, dst, NULL, &node, this);
+	if (ret < 0)
+		goto error;
+
+	*next = node;
+
+	return 0;
+
+error:
+	b2r2_log_info("%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_rot_upscale() - configures a combined rotate and upscale
+ *
+ * When doing an upscale it is better to do the rotation first.
+ */
+static int configure_rot_upscale(struct b2r2_node_split_job *this,
+		struct b2r2_node *node, struct b2r2_node **next)
+{
+	/* TODO: Implement a optimal upscale (rotation first) */
+	return configure_rot_downscale(this, node, next);
+}
+
+/**
+ * configure_rot_scale() - configures a combined rotation and scaling op
+ */
+static int configure_rot_scale(struct b2r2_node_split_job *this,
+		struct b2r2_node *node, struct b2r2_node **next)
+{
+	int ret;
+
+	bool upscale = (u32)this->h_rsf * (u32)this->v_rsf < (1 << 10);
+
+	if (upscale)
+		ret = configure_rot_upscale(this, node, next);
+	else
+		ret = configure_rot_downscale(this, node, next);
+
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_direct_fill() - configures the given node for direct fill
+ *
+ * @node  - the node to configure
+ * @color - the fill color
+ * @dst   - the destination buffer
+ * @next  - the next empty node in the node list
+ *
+ * This operation will always consume one node only.
+ */
+static void configure_direct_fill(struct b2r2_node *node, u32 color,
+		struct b2r2_node_split_buf *dst, struct b2r2_node **next)
+{
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_COLOR_FILL | B2R2_CIC_SOURCE_1;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_1_DIRECT_FILL;
+
+	/* Target setup */
+	set_target(node, dst->addr, dst);
+
+	/* Source setup */
+
+	/* It seems B2R2 checks so that source and dest has the same format */
+	node->node.GROUP3.B2R2_STY = to_native_fmt(dst->fmt);
+	node->node.GROUP2.B2R2_S1CF = color;
+	node->node.GROUP2.B2R2_S2CF = 0;
+
+	/* Consume the node */
+	*next = node->next;
+}
+
+/**
+ * configure_direct_copy() - configures the node for direct copy
+ *
+ * @node - the node to configure
+ * @src  - the source buffer
+ * @dst  - the destination buffer
+ * @next - the next empty node in the node list
+ *
+ * This operation will always consume one node only.
+ */
+static void configure_direct_copy(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst, struct b2r2_node **next)
+{
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_1;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_1_DIRECT_COPY;
+
+	/* Source setup, use the base function to avoid altering the INS */
+	set_src(&node->node.GROUP3, src->addr, src);
+
+	/* Target setup */
+	set_target(node, dst->addr, dst);
+
+	/* Consume the node */
+	*next = node->next;
+}
+
+/**
+ * configure_fill() - configures the given node for color fill
+ *
+ * @node  - the node to configure
+ * @color - the fill color
+ * @fmt   - the source color format
+ * @dst   - the destination buffer
+ * @next  - the next empty node in the node list
+ *
+ * A normal fill operation can be combined with any other per pixel operations
+ * such as blend.
+ *
+ * This operation will consume as many nodes as are required to write to the
+ * destination format.
+ */
+static int configure_fill(struct b2r2_node *node, u32 color,
+		enum b2r2_blt_fmt fmt, struct b2r2_node_split_buf *dst,
+		const u32 *ivmx, struct b2r2_node **next)
+{
+	int ret;
+	struct b2r2_node *last;
+
+	/* Configure the destination */
+	ret = configure_dst(node, dst, ivmx, &last);
+	if (ret < 0)
+		goto error;
+
+	do {
+		if (node == NULL) {
+			b2r2_log_warn("%s: "
+			"Internal error! Out of nodes!\n", __func__);
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2 |
+							B2R2_CIC_COLOR_FILL;
+		node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_SOURCE_2_COLOR_FILL_REGISTER;
+		node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+
+		/* B2R2 has a bug that disables color fill from S2. As a
+		   workaround we use S1 for the color. */
+		node->node.GROUP2.B2R2_S1CF = 0;
+		node->node.GROUP2.B2R2_S2CF = color;
+
+		/* TO BE REMOVED: */
+		set_src_2(node, dst->addr, dst);
+		node->node.GROUP4.B2R2_STY = to_native_fmt(fmt);
+
+		/* Setup the iVMX for color conversion */
+		if (ivmx != NULL)
+			set_ivmx(node, ivmx);
+
+		if  ((dst->type == B2R2_FMT_TYPE_PLANAR) ||
+				(dst->type == B2R2_FMT_TYPE_SEMI_PLANAR)) {
+
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_RESCALE2D_ENABLED;
+			node->node.GROUP8.B2R2_FCTL =
+				B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_RESIZER;
+			node->node.GROUP9.B2R2_RSF =
+				(1 << (B2R2_RSF_HSRC_INC_SHIFT + 10)) |
+				(1 << (B2R2_RSF_VSRC_INC_SHIFT + 10));
+			node->node.GROUP9.B2R2_RZI =
+				B2R2_RZI_DEFAULT_HNB_REPEAT |
+				(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+
+			node->node.GROUP10.B2R2_RSF =
+				(1 << (B2R2_RSF_HSRC_INC_SHIFT + 10)) |
+				(1 << (B2R2_RSF_VSRC_INC_SHIFT + 10));
+			node->node.GROUP10.B2R2_RZI =
+				B2R2_RZI_DEFAULT_HNB_REPEAT |
+				(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+		}
+
+		node = node->next;
+
+	} while (node != last);
+
+	/* Consume the nodes */
+	*next = node;
+
+	return 0;
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_copy() - configures the given node for a copy operation
+ *
+ * @node - the node to configure
+ * @src  - the source buffer
+ * @dst  - the destination buffer
+ * @ivmx - the iVMX to use for color conversion
+ * @next - the next empty node in the node list
+ *
+ * This operation will consume as many nodes as are required to write to the
+ * destination format.
+ */
+static int configure_copy(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst, const u32 *ivmx,
+		struct b2r2_node **next,
+		struct b2r2_node_split_job *this)
+{
+	int ret;
+
+	struct b2r2_node *last;
+
+	ret = configure_dst(node, dst, ivmx, &last);
+	if (ret < 0)
+		goto error;
+
+	/* Configure the source for each node */
+	do {
+		if (node == NULL) {
+			b2r2_log_warn("%s: "
+			" Internal error! Out of nodes!\n", __func__);
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+		if (this != NULL &&
+				(this->flags & B2R2_BLT_FLAG_SOURCE_COLOR_KEY)
+				!= 0) {
+			u32 key_color = 0;
+
+			node->node.GROUP0.B2R2_ACK |=
+				B2R2_ACK_CKEY_SEL_SRC_AFTER_CLUT |
+				B2R2_ACK_CKEY_RED_MATCH_IF_BETWEEN |
+				B2R2_ACK_CKEY_GREEN_MATCH_IF_BETWEEN |
+				B2R2_ACK_CKEY_BLUE_MATCH_IF_BETWEEN;
+			node->node.GROUP0.B2R2_INS |= B2R2_INS_CKEY_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_COLOR_KEY;
+
+			key_color = to_RGB888(this->flag_param, src->fmt);
+			node->node.GROUP12.B2R2_KEY1 = key_color;
+			node->node.GROUP12.B2R2_KEY2 = key_color;
+		}
+
+		if (this != NULL &&
+				(this->flags &
+				B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) != 0) {
+			struct b2r2_blt_request *request =
+				container_of(this, struct b2r2_blt_request,
+						node_split_job);
+			node->node.GROUP0.B2R2_INS |= B2R2_INS_CLUTOP_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_CLUT;
+			node->node.GROUP7.B2R2_CCO =
+				B2R2_CCO_CLUT_COLOR_CORRECTION |
+				B2R2_CCO_CLUT_UPDATE;
+			node->node.GROUP7.B2R2_CML = request->clut_phys_addr;
+		}
+		/* Configure the source(s) */
+		configure_src(node, src, ivmx);
+
+		node = node->next;
+	} while (node != last);
+
+	/* Consume the nodes */
+	*next = node;
+
+	return 0;
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_rotate() - configures the given node for rotation
+ *
+ * @node - the node to configure
+ * @src  - the source buffer
+ * @dst  - the destination buffer
+ * @ivmx - the iVMX to use for color conversion
+ * @next - the next empty node in the node list
+ *
+ * This operation will consume as many nodes are are required by the combination
+ * of rotating and writing the destination format.
+ */
+static int configure_rotate(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst, const u32 *ivmx,
+		struct b2r2_node **next,
+		struct b2r2_node_split_job *this)
+{
+	int ret;
+
+	struct b2r2_node *last;
+
+	ret = configure_copy(node, src, dst, ivmx, &last, this);
+	if (ret < 0)
+		goto error;
+
+	do {
+		if (node == NULL) {
+			b2r2_log_warn("%s: "
+				"Internal error! Out of nodes!\n", __func__);
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_ROTATION_ENABLED;
+
+		b2r2_log_debug("%s:\n"
+				"\tB2R2_TXY:  %.8x\tB2R2_TSZ:  %.8x\n"
+				"\tB2R2_S1XY: %.8x\tB2R2_S1SZ: %.8x\n"
+				"\tB2R2_S2XY: %.8x\tB2R2_S2SZ: %.8x\n"
+				"\tB2R2_S3XY: %.8x\tB2R2_S3SZ: %.8x\n"
+				"-----------------------------------\n",
+				__func__, node->node.GROUP1.B2R2_TXY,
+				node->node.GROUP1.B2R2_TSZ,
+				node->node.GROUP3.B2R2_SXY,
+				node->node.GROUP3.B2R2_SSZ,
+				node->node.GROUP4.B2R2_SXY,
+				node->node.GROUP4.B2R2_SSZ,
+				node->node.GROUP5.B2R2_SXY,
+				node->node.GROUP5.B2R2_SSZ);
+
+		node = node->next;
+
+	} while (node != last);
+
+	/* Consume the nodes */
+	*next = node;
+
+	return 0;
+error:
+	b2r2_log_warn("%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_scale() - configures the given node for scaling
+ *
+ * @node  - the node to configure
+ * @src   - the source buffer
+ * @dst   - the destination buffer
+ * @h_rsf - the horizontal rescale factor
+ * @v_rsf - the vertical rescale factor
+ * @ivmx  - the iVMX to use for color conversion
+ * @next  - the next empty node in the node list
+ */
+static int configure_scale(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		u16 h_rsf, u16 v_rsf,
+		const u32 *ivmx, struct b2r2_node **next,
+		struct b2r2_node_split_job *this)
+{
+	int ret;
+
+	struct b2r2_node *last;
+
+	struct b2r2_filter_spec *hf = NULL;
+	struct b2r2_filter_spec *vf = NULL;
+
+	u32 fctl = 0;
+	u32 rsf = 0;
+	u32 rzi = 0;
+	u32 hsrc_init = 0;
+	u32 vsrc_init = 0;
+	u32 hfp = 0;
+	u32 vfp = 0;
+
+	u16 luma_h_rsf = h_rsf;
+	u16 luma_v_rsf = v_rsf;
+
+	struct b2r2_filter_spec *luma_hf = NULL;
+	struct b2r2_filter_spec *luma_vf = NULL;
+
+	u32 luma_fctl = 0;
+	u32 luma_rsf = 0;
+	u32 luma_rzi = 0;
+	u32 luma_hsrc_init = 0;
+	u32 luma_vsrc_init = 0;
+	u32 luma_hfp = 0;
+	u32 luma_vfp = 0;
+
+	s32 src_x;
+	s32 src_y;
+	s32 src_w;
+	s32 src_h;
+
+	bool upsample;
+	bool downsample;
+
+	struct b2r2_blt_rect tmp_win = src->win;
+
+	/* Rescale the normalized source window */
+	src_x = inv_rescale(src->win.x - src->rect.x, luma_h_rsf);
+	src_y = inv_rescale(src->win.y - src->rect.y, luma_v_rsf);
+	src_w = inv_rescale(src->win.width, luma_h_rsf);
+	src_h = inv_rescale(src->win.height, luma_v_rsf);
+
+	/* Convert to src coordinate space */
+	src->win.x = (src_x >> 10) + src->rect.x;
+	src->win.y = (src_y >> 10) + src->rect.y;
+
+	/*
+	 * Since the stripe might start and end on a fractional pixel
+	 * we need to count all the touched pixels in the width.
+	 *
+	 * Example:
+	 *   src_x = 1.8, src_w = 2.8
+	 *
+	 *   The stripe touches pixels 1.8 through 4.6, i.e. 4 pixels
+	 */
+	src->win.width = ((src_x & 0x3ff) + src_w + 0x3ff) >> 10;
+	src->win.height = ((src_y & 0x3ff) + src_h + 0x3ff) >> 10;
+
+	luma_hsrc_init = src_x & 0x3ff;
+	luma_vsrc_init = src_y & 0x3ff;
+
+	/* Check for upsampling of chroma */
+	upsample = src->type != B2R2_FMT_TYPE_RASTER &&
+		!is_yuv444_fmt(src->fmt);
+	if (upsample) {
+		h_rsf /= 2;
+
+		if (is_yuv420_fmt(src->fmt))
+			v_rsf /= 2;
+	}
+
+	/* Check for downsampling of chroma */
+	downsample = dst->type != B2R2_FMT_TYPE_RASTER &&
+		!is_yuv444_fmt(dst->fmt);
+	if (downsample) {
+		h_rsf *= 2;
+
+		if (is_yuv420_fmt(dst->fmt))
+			v_rsf *= 2;
+	}
+
+	src_x = inv_rescale(tmp_win.x - src->rect.x, h_rsf);
+	src_y = inv_rescale(tmp_win.y - src->rect.y, v_rsf);
+	hsrc_init = src_x & 0x3ff;
+	vsrc_init = src_y & 0x3ff;
+
+	/* Configure resize and filters */
+	fctl = B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+		B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+	luma_fctl = B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_RESIZER |
+		B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_RESIZER;
+
+	rsf =  (h_rsf << B2R2_RSF_HSRC_INC_SHIFT) |
+			(v_rsf << B2R2_RSF_VSRC_INC_SHIFT);
+	luma_rsf = (luma_h_rsf << B2R2_RSF_HSRC_INC_SHIFT) |
+			(luma_v_rsf << B2R2_RSF_VSRC_INC_SHIFT);
+
+	rzi = B2R2_RZI_DEFAULT_HNB_REPEAT |
+		(2 << B2R2_RZI_VNB_REPEAT_SHIFT) |
+		(hsrc_init << B2R2_RZI_HSRC_INIT_SHIFT) |
+		(vsrc_init << B2R2_RZI_VSRC_INIT_SHIFT);
+	luma_rzi = B2R2_RZI_DEFAULT_HNB_REPEAT |
+		(2 << B2R2_RZI_VNB_REPEAT_SHIFT) |
+		(luma_hsrc_init << B2R2_RZI_HSRC_INIT_SHIFT) |
+		(luma_vsrc_init << B2R2_RZI_VSRC_INIT_SHIFT);
+
+	/*
+	 * We should only filter if there is an actual rescale (i.e. not when
+	 * up or downsampling).
+	 */
+	if (luma_h_rsf != (1 << 10)) {
+		hf = b2r2_filter_find(h_rsf);
+		luma_hf = b2r2_filter_find(luma_h_rsf);
+	}
+	if (luma_v_rsf != (1 << 10)) {
+		vf = b2r2_filter_find(v_rsf);
+		luma_vf = b2r2_filter_find(luma_v_rsf);
+	}
+
+	if (hf) {
+		fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER;
+		hfp = hf->h_coeffs_phys_addr;
+	}
+
+	if (vf) {
+		fctl |= B2R2_FCTL_VF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER;
+		vfp = vf->v_coeffs_phys_addr;
+	}
+
+	if (luma_hf) {
+		luma_fctl |= B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_FILTER;
+		luma_hfp = luma_hf->h_coeffs_phys_addr;
+	}
+
+	if (luma_vf) {
+		luma_fctl |= B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_FILTER;
+		luma_vfp = luma_vf->v_coeffs_phys_addr;
+	}
+
+	ret = configure_copy(node, src, dst, ivmx, &last, this);
+	if (ret < 0)
+		goto error;
+
+	do {
+		bool chroma_rescale =
+			(h_rsf != (1 << 10)) || (v_rsf != (1 << 10));
+		bool luma_rescale =
+			(luma_h_rsf != (1 << 10)) || (luma_v_rsf != (1 << 10));
+		bool src_raster = src->type == B2R2_FMT_TYPE_RASTER;
+		bool dst_raster = dst->type == B2R2_FMT_TYPE_RASTER;
+		bool dst_chroma =
+			node->node.GROUP1.B2R2_TTY & B2R2_TTY_CHROMA_NOT_LUMA;
+
+		if (node == NULL) {
+			b2r2_log_warn("%s: "
+				"Internal error! Out of nodes!\n", __func__);
+			ret = -ENOMEM;
+			goto error;
+		}
+
+
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_FILTER_CONTROL;
+
+		/*
+		 * If the source format is anything other than raster, we
+		 * always have to enable both chroma and luma resizers. This
+		 * could be a bug in the hardware, since it is not mentioned in
+		 * the specification.
+		 *
+		 * Otherwise, we will only enable the chroma resizer when
+		 * writing chroma and the luma resizer when writing luma
+		 * (or both when writing raster). Also, if there is no rescale
+		 * to be done there's no point in using the resizers.
+		 */
+
+		if (!src_raster || (chroma_rescale &&
+					(dst_raster || dst_chroma))) {
+			/* Enable chroma resize */
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_RESCALE2D_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_RESIZE_CHROMA;
+			node->node.GROUP8.B2R2_FCTL |= fctl;
+
+			node->node.GROUP9.B2R2_RSF = rsf;
+			node->node.GROUP9.B2R2_RZI = rzi;
+			node->node.GROUP9.B2R2_HFP = hfp;
+			node->node.GROUP9.B2R2_VFP = vfp;
+		}
+
+		if (!src_raster || (luma_rescale &&
+					(dst_raster || !dst_chroma))) {
+			/* Enable luma resize */
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_RESCALE2D_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_RESIZE_LUMA;
+			node->node.GROUP8.B2R2_FCTL |= luma_fctl;
+
+			node->node.GROUP10.B2R2_RSF = luma_rsf;
+			node->node.GROUP10.B2R2_RZI = luma_rzi;
+			node->node.GROUP10.B2R2_HFP = luma_hfp;
+			node->node.GROUP10.B2R2_VFP = luma_vfp;
+		}
+
+		b2r2_log_info("%s:\n"
+				"\tB2R2_TXY:  %.8x\tB2R2_TSZ:  %.8x\n"
+				"\tB2R2_S1XY: %.8x\tB2R2_S1SZ: %.8x\n"
+				"\tB2R2_S2XY: %.8x\tB2R2_S2SZ: %.8x\n"
+				"\tB2R2_S3XY: %.8x\tB2R2_S3SZ: %.8x\n"
+				"----------------------------------\n",
+				__func__, node->node.GROUP1.B2R2_TXY,
+				node->node.GROUP1.B2R2_TSZ,
+				node->node.GROUP3.B2R2_SXY,
+				node->node.GROUP3.B2R2_SSZ,
+				node->node.GROUP4.B2R2_SXY,
+				node->node.GROUP4.B2R2_SSZ,
+				node->node.GROUP5.B2R2_SXY,
+				node->node.GROUP5.B2R2_SSZ);
+
+		node = node->next;
+
+	} while (node != last);
+
+
+
+	/* Consume the nodes */
+	*next = node;
+
+	return 0;
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_src() - configures the source registers and the iVMX
+ *
+ * @node - the node to configure
+ * @src  - the source buffer
+ * @ivmx - the iVMX to use for color conversion
+ *
+ * This operation will not consume any nodes
+ */
+static void configure_src(struct b2r2_node *node,
+		struct b2r2_node_split_buf *src, const u32 *ivmx)
+{
+	struct b2r2_node_split_buf tmp_buf;
+
+	b2r2_log_info("%s: src.win=(%d, %d, %d, %d)\n", __func__,
+			src->win.x, src->win.y, src->win.width,
+			src->win.height);
+
+	/* Configure S1 - S3 */
+	switch (src->type) {
+	case B2R2_FMT_TYPE_RASTER:
+		set_src_2(node, src->addr, src);
+		break;
+	case B2R2_FMT_TYPE_SEMI_PLANAR:
+		memcpy(&tmp_buf, src, sizeof(tmp_buf));
+
+		/*
+		 * For 420 and 422 the chroma has lower resolution than the
+		 * luma
+		 */
+		if (!is_yuv444_fmt(src->fmt)) {
+			tmp_buf.win.x >>= 1;
+			tmp_buf.win.width = (tmp_buf.win.width + 1) / 2;
+
+			if (is_yuv420_fmt(src->fmt)) {
+				tmp_buf.win.height =
+					(tmp_buf.win.height + 1) / 2;
+				tmp_buf.win.y >>= 1;
+			}
+		}
+
+		set_src_3(node, src->addr, src);
+		set_src_2(node, tmp_buf.chroma_addr, &tmp_buf);
+		break;
+	case B2R2_FMT_TYPE_PLANAR:
+		memcpy(&tmp_buf, src, sizeof(tmp_buf));
+
+		if (!is_yuv444_fmt(src->fmt)) {
+			/*
+			 * Each chroma buffer will have half as many values
+			 * per line as the luma buffer
+			 */
+			tmp_buf.pitch = (tmp_buf.pitch + 1) / 2;
+
+			/* Horizontal resolution is half */
+			tmp_buf.win.x >>= 1;
+			tmp_buf.win.width = (tmp_buf.win.width + 1) / 2;
+
+			/*
+			 * If the buffer is in YUV420 format, the vertical
+			 * resolution is half as well
+			 */
+			if (is_yuv420_fmt(src->fmt)) {
+				tmp_buf.win.height =
+					(tmp_buf.win.height + 1) / 2;
+				tmp_buf.win.y >>= 1;
+			}
+		}
+
+		set_src_3(node, src->addr, src);                   /* Y */
+		set_src_2(node, tmp_buf.chroma_addr, &tmp_buf);    /* U */
+		set_src_1(node, tmp_buf.chroma_cr_addr, &tmp_buf); /* V */
+
+		break;
+	default:
+		/* Should never, ever happen */
+		BUG_ON(1);
+		break;
+	}
+
+	/* Configure the iVMX for color space conversions */
+	if (ivmx != NULL)
+		set_ivmx(node, ivmx);
+}
+
+/**
+ * configure_dst() - configures the destination registers of the given node
+ *
+ * @node - the node to configure
+ * @ivmx - the iVMX to use for color conversion
+ * @dst  - the destination buffer
+ *
+ * This operation will consume as many nodes as are required to write the
+ * destination format.
+ */
+static int configure_dst(struct b2r2_node *node,
+		struct b2r2_node_split_buf *dst, const u32 *ivmx,
+		struct b2r2_node **next)
+{
+	int ret;
+	int nbr_planes = 1;
+	int i;
+
+	struct b2r2_node_split_buf dst_planes[3];
+
+	b2r2_log_info("%s: dst.win=(%d, %d, %d, %d)\n", __func__,
+			dst->win.x, dst->win.y, dst->win.width,
+			dst->win.height);
+
+	memcpy(&dst_planes[0], dst, sizeof(dst_planes[0]));
+
+	if (dst->type != B2R2_FMT_TYPE_RASTER) {
+		/* There will be at least 2 planes */
+		nbr_planes = 2;
+
+		memcpy(&dst_planes[1], dst, sizeof(dst_planes[1]));
+
+		dst_planes[1].addr = dst->chroma_addr;
+		dst_planes[1].plane_selection = B2R2_TTY_CHROMA_NOT_LUMA;
+
+		if (!is_yuv444_fmt(dst->fmt)) {
+			/* Horizontal resolution is half */
+			dst_planes[1].win.x /= 2;
+			/*
+			 * Must round up the chroma size to handle cases when
+			 * luma size is not divisible by 2. E.g. luma width==7 r
+			 * equires chroma width==4. Chroma width==7/2==3 is only
+			 * enough for luma width==6.
+			 */
+			dst_planes[1].win.width =
+				(dst_planes[1].win.width + 1) / 2;
+
+			/*
+			 * If the buffer is in YUV420 format, the vertical
+			 * resolution is half as well. Height must be rounded in
+			 * the same way as is done for width.
+			 */
+			if (is_yuv420_fmt(dst->fmt)) {
+				dst_planes[1].win.y /= 2;
+				dst_planes[1].win.height =
+					(dst_planes[1].win.height + 1) / 2;
+			}
+		}
+
+		if (dst->type == B2R2_FMT_TYPE_PLANAR) {
+			/* There will be a third plane as well */
+			nbr_planes = 3;
+
+			if (!is_yuv444_fmt(dst->fmt)) {
+				/* The chroma planes have half the luma pitch */
+				dst_planes[1].pitch /= 2;
+			}
+
+			memcpy(&dst_planes[2], &dst_planes[1],
+				sizeof(dst_planes[2]));
+			dst_planes[2].addr = dst->chroma_cr_addr;
+
+			/*
+			 * The third plane will be Cr.
+			 * The flag B2R2_TTY_CB_NOT_CR actually works
+			 * the other way around, i.e. as if it was
+			 * B2R2_TTY_CR_NOT_CB.
+			 */
+			dst_planes[2].chroma_selection = B2R2_TTY_CB_NOT_CR;
+		}
+
+	}
+
+	/* Configure one node for each plane */
+	for (i = 0; i < nbr_planes; i++) {
+
+		if (node == NULL) {
+			b2r2_log_warn("%s: "
+				"Internal error! Out of nodes!\n", __func__);
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		/*
+		 * When writing chroma, there's no need to read the luma and
+		 * vice versa.
+		 */
+		if ((node->node.GROUP3.B2R2_STY & B2R2_NATIVE_YUV) &&
+				(nbr_planes > 1)) {
+			if (i != 0) {
+				node->node.GROUP4.B2R2_STY |=
+					B2R2_S3TY_ENABLE_BLANK_ACCESS;
+			}
+			if (i != 1) {
+				node->node.GROUP0.B2R2_INS &=
+					~B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+				node->node.GROUP0.B2R2_INS |=
+					B2R2_INS_SOURCE_2_COLOR_FILL_REGISTER;
+			}
+			if (i != 2) {
+				node->node.GROUP0.B2R2_INS &=
+					~B2R2_INS_SOURCE_1_FETCH_FROM_MEM;
+				node->node.GROUP0.B2R2_INS |=
+					B2R2_INS_SOURCE_1_COLOR_FILL_REGISTER;
+			}
+		} else if ((node->node.GROUP3.B2R2_STY &
+					(B2R2_NATIVE_YCBCR42X_MBN |
+						B2R2_NATIVE_YCBCR42X_R2B)) &&
+				(nbr_planes > 1)) {
+			if (i != 0) {
+				node->node.GROUP4.B2R2_STY |=
+					B2R2_S3TY_ENABLE_BLANK_ACCESS;
+			}
+		}
+
+
+		set_target(node, dst_planes[i].addr, &dst_planes[i]);
+
+		node = node->next;
+	}
+
+	/* Consume the nodes */
+	*next = node;
+
+	return 0;
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+
+}
+
+/**
+ * configure_blend() - configures the given node for alpha blending
+ *
+ * @node         - the node to configure
+ * @flags        - the flags passed in the blt_request
+ * @global_alpha - the global alpha to use (if enabled in flags)
+ * @swap_fg_bg   - if true, fg will be on s1 instead of s2
+ *
+ * This operation will not consume any nodes.
+ *
+ * NOTE: This method should be called _AFTER_ the destination has been
+ *       configured.
+ *
+ * WARNING: Take care when using this with semi-planar or planar sources since
+ *          either S1 or S2 will be overwritten!
+ */
+static void configure_blend(struct b2r2_node *node, u32 flags, u32 global_alpha,
+		bool swap_fg_bg)
+{
+	node->node.GROUP0.B2R2_ACK &= ~(B2R2_ACK_MODE_BYPASS_S2_S3);
+
+	/* Check if the foreground is premultiplied */
+	if ((flags & B2R2_BLT_FLAG_SRC_IS_NOT_PREMULT) != 0)
+		node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BLEND_NOT_PREMULT;
+	else
+		node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BLEND_PREMULT;
+
+
+	/* Check if global alpha blend should be enabled */
+	if ((flags & B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND) != 0) {
+
+		/* B2R2 expects the global alpha to be in 0...128 range */
+		global_alpha = (global_alpha*128)/255;
+
+		node->node.GROUP0.B2R2_ACK |=
+			global_alpha <<	B2R2_ACK_GALPHA_ROPID_SHIFT;
+	} else {
+		node->node.GROUP0.B2R2_ACK |=
+			(128 << B2R2_ACK_GALPHA_ROPID_SHIFT);
+	}
+
+	/* Copy the destination config to the appropriate source and clear any
+	   clashing flags */
+	if (swap_fg_bg) {
+		/* S1 will be foreground, S2 background */
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_ACK |= B2R2_ACK_SWAP_FG_BG;
+
+		node->node.GROUP4.B2R2_SBA = node->node.GROUP1.B2R2_TBA;
+		node->node.GROUP4.B2R2_STY = node->node.GROUP1.B2R2_TTY;
+		node->node.GROUP4.B2R2_SXY = node->node.GROUP1.B2R2_TXY;
+		node->node.GROUP4.B2R2_SSZ = node->node.GROUP1.B2R2_TSZ;
+
+		node->node.GROUP4.B2R2_STY &= ~(B2R2_S2TY_A1_SUBST_KEY_MODE |
+				B2R2_S2TY_CHROMA_LEFT_EXT_AVERAGE);
+	} else {
+		/* S1 will be background, S2 foreground */
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_1;
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_1_FETCH_FROM_MEM;
+
+		node->node.GROUP3.B2R2_SBA = node->node.GROUP1.B2R2_TBA;
+		node->node.GROUP3.B2R2_STY |= node->node.GROUP1.B2R2_TTY;
+		node->node.GROUP3.B2R2_SXY = node->node.GROUP1.B2R2_TXY;
+
+		node->node.GROUP3.B2R2_STY &= ~(B2R2_S1TY_A1_SUBST_KEY_MODE);
+
+	}
+}
+
+/**
+ * configure_clip() - configures destination clipping for the given node
+ *
+ *   @node      - the node to configure
+ *   @clip_rect - the clip rectangle
+ *
+ *  This operation does not consume any nodes.
+ */
+static void configure_clip(struct b2r2_node *node,
+		struct b2r2_blt_rect *clip_rect)
+{
+	s32 l = clip_rect->x;
+	s32 r = clip_rect->x + clip_rect->width - 1;
+	s32 t = clip_rect->y;
+	s32 b = clip_rect->y + clip_rect->height - 1;
+
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_CLIP_WINDOW;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_RECT_CLIP_ENABLED;
+
+	/* Clip window setup */
+	node->node.GROUP6.B2R2_CWO =
+			((t & 0x7FFF) << B2R2_CWO_Y_SHIFT) |
+			((l & 0x7FFF) << B2R2_CWO_X_SHIFT);
+	node->node.GROUP6.B2R2_CWS =
+			((b & 0x7FFF) << B2R2_CWO_Y_SHIFT) |
+			((r & 0x7FFF) << B2R2_CWO_X_SHIFT);
+}
+
+/**
+ * set_buf() - configures the given buffer with the provided values
+ *
+ * @addr       - the physical base address
+ * @img        - the blt image to base the buffer on
+ * @rect       - the rectangle to use
+ * @color_fill - determines whether the buffer should be used for color fill
+ * @color      - the color to use in case of color fill
+ */
+static void set_buf(struct b2r2_node_split_buf *buf, u32 addr,
+		const struct b2r2_blt_img *img,
+		const struct b2r2_blt_rect *rect, bool color_fill, u32 color)
+{
+	memset(buf, 0, sizeof(*buf));
+
+	buf->fmt = img->fmt;
+	buf->type = get_fmt_type(img->fmt);
+
+	if (color_fill) {
+		buf->type = B2R2_FMT_TYPE_RASTER;
+		buf->color = color;
+	} else {
+		buf->addr = addr;
+
+		buf->alpha_range = get_alpha_range(img->fmt);
+
+		if (img->pitch == 0)
+			buf->pitch = fmt_byte_pitch(img->fmt, img->width);
+		else
+			buf->pitch = img->pitch;
+
+		buf->height = img->height;
+		buf->width = img->width;
+
+		switch (buf->type) {
+		case B2R2_FMT_TYPE_SEMI_PLANAR:
+			buf->chroma_addr = (u32)(((u8 *)addr) +
+					buf->pitch * buf->height);
+			break;
+		case B2R2_FMT_TYPE_PLANAR:
+			if (is_yuv422_fmt(buf->fmt) ||
+					is_yuv420_fmt(buf->fmt)) {
+				buf->chroma_addr = (u32)(((u8 *)addr) +
+					buf->pitch * buf->height);
+			} else {
+				buf->chroma_cr_addr = (u32)(((u8 *)addr) +
+					buf->pitch * buf->height);
+			}
+			if (is_yuv420_fmt(buf->fmt)) {
+				/*
+				 * Use ceil(height/2) in case
+				 * buffer height is not divisible by 2.
+				 */
+				buf->chroma_cr_addr =
+					(u32)(((u8 *)buf->chroma_addr) +
+					(buf->pitch >> 1) *
+					((buf->height + 1) >> 1));
+			} else if (is_yuv422_fmt(buf->fmt)) {
+				buf->chroma_cr_addr =
+					(u32)(((u8 *)buf->chroma_addr) +
+					(buf->pitch >> 1) * buf->height);
+			} else if (is_yvu420_fmt(buf->fmt)) {
+				buf->chroma_addr =
+					(u32)(((u8 *)buf->chroma_cr_addr) +
+					(buf->pitch >> 1) *
+					((buf->height + 1) >> 1));
+			} else if (is_yvu422_fmt(buf->fmt)) {
+				buf->chroma_addr =
+					(u32)(((u8 *)buf->chroma_cr_addr) +
+					(buf->pitch >> 1) * buf->height);
+			}
+			break;
+		default:
+			break;
+		}
+
+		memcpy(&buf->rect, rect, sizeof(buf->rect));
+	}
+}
+
+/**
+ * setup_tmp_buf() - configure a temporary buffer
+ */
+static int setup_tmp_buf(struct b2r2_node_split_buf *tmp, u32 max_size,
+		enum b2r2_blt_fmt pref_fmt, u32 pref_width, u32 pref_height)
+{
+	int ret;
+
+	enum b2r2_blt_fmt fmt;
+
+	u32 width;
+	u32 height;
+	u32 pitch;
+	u32 size;
+
+	/* Determine what format we should use for the tmp buf */
+	if (is_rgb_fmt(pref_fmt)) {
+		fmt = B2R2_BLT_FMT_32_BIT_ARGB8888;
+	} else if (is_bgr_fmt(pref_fmt)) {
+		fmt = B2R2_BLT_FMT_32_BIT_ABGR8888;
+	} else if (is_yvu_fmt(pref_fmt)) {
+		fmt = B2R2_BLT_FMT_CB_Y_CR_Y;
+	} else if (is_yuv_fmt(pref_fmt)) {
+		fmt = B2R2_BLT_FMT_32_BIT_AYUV8888;
+	} else {
+		/* Wait, what? */
+		b2r2_log_warn("%s: "
+			"Cannot create tmp buf from this fmt (%d)\n", __func__,
+			pref_fmt);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/* See if we can fit the entire preferred rectangle */
+	width = pref_width;
+	height = pref_height;
+	pitch = fmt_byte_pitch(fmt, width);
+	size = pitch * height;
+
+	if (size > max_size) {
+		/* We need to limit the size, so we choose a different width */
+		width = MIN(width, B2R2_RESCALE_MAX_WIDTH);
+		pitch = fmt_byte_pitch(fmt, width);
+		height = MIN(height, max_size / pitch);
+		size = pitch * height;
+	}
+
+	/* We should at least have enough room for one scanline */
+	if (height == 0) {
+		b2r2_log_warn("%s: Not enough tmp mem!\n",
+			__func__);
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	memset(tmp, 0, sizeof(*tmp));
+
+	tmp->fmt = fmt;
+	tmp->type = B2R2_FMT_TYPE_RASTER;
+	tmp->height = height;
+	tmp->width = width;
+	tmp->pitch = pitch;
+
+	tmp->rect.width = width;
+	tmp->rect.height = tmp->height;
+	tmp->alpha_range = B2R2_TY_ALPHA_RANGE_255;
+
+	return 0;
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+
+}
+
+/**
+ * get_alpha_range() - returns the alpha range of the given format
+ */
+static enum b2r2_ty get_alpha_range(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_8_BIT_A8:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		return B2R2_TY_ALPHA_RANGE_255; /* 0 - 255 */
+	default:
+		return B2R2_TY_ALPHA_RANGE_128; /* 0 - 128 */
+	}
+}
+
+/**
+ * get_alpha() - returns the pixel alpha in 0...255 range
+ */
+static u8 get_alpha(enum b2r2_blt_fmt fmt, u32 pixel)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		return (pixel >> 24) & 0xff;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		return (pixel & 0xfff) >> 16;
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		return (((pixel >> 12) & 0xf) * 255) / 15;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		return (pixel >> 15) * 255;
+	case B2R2_BLT_FMT_1_BIT_A1:
+		return pixel * 255;
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return pixel;
+	default:
+		return 255;
+	}
+}
+
+/**
+ * set_alpha() - returns a color value with the alpha component set
+ */
+static u32 set_alpha(enum b2r2_blt_fmt fmt, u8 alpha, u32 color)
+{
+	u32 alpha_mask;
+
+	switch (fmt) {
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		color &= 0x00ffffff;
+		alpha_mask = alpha << 24;
+		break;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		color &= 0x00ffff;
+		alpha_mask = alpha << 16;
+		break;
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		color &= 0x0fff;
+		alpha_mask = (alpha << 8) & 0xF000;
+		break;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		color &= 0x7fff;
+		alpha_mask = (alpha / 255) << 15 ;
+		break;
+	case B2R2_BLT_FMT_1_BIT_A1:
+		color = 0;
+		alpha_mask = (alpha / 255);
+		break;
+	case B2R2_BLT_FMT_8_BIT_A8:
+		color = 0;
+		alpha_mask = alpha;
+		break;
+	default:
+		alpha_mask = 0;
+	}
+
+	return color | alpha_mask;
+}
+
+/**
+ * fmt_has_alpha() - returns whether the given format carries an alpha value
+ */
+static bool fmt_has_alpha(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * is_rgb_fmt() - returns whether the given format is a rgb format
+ */
+static bool is_rgb_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * is_bgr_fmt() - returns whether the given format is a bgr format
+ */
+static bool is_bgr_fmt(enum b2r2_blt_fmt fmt)
+{
+	return (fmt == B2R2_BLT_FMT_32_BIT_ABGR8888);
+}
+
+/**
+ * is_yuv_fmt() - returns whether the given format is a yuv format
+ */
+static bool is_yuv_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * is_yvu_fmt() - returns whether the given format is a yvu format
+ */
+static bool is_yvu_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * is_yuv420_fmt() - returns whether the given format is a yuv420 format
+ */
+static bool is_yuv420_fmt(enum b2r2_blt_fmt fmt)
+{
+
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool is_yuv422_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * is_yvu420_fmt() - returns whether the given format is a yvu420 format
+ */
+static bool is_yvu420_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool is_yvu422_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return true;
+	default:
+		return false;
+	}
+}
+
+
+/**
+ * is_yuv444_fmt() - returns whether the given format is a yuv444 format
+ */
+static bool is_yuv444_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * get_fmt_byte_pitch() - returns the pitch of a pixmap with the given width
+ */
+static int fmt_byte_pitch(enum b2r2_blt_fmt fmt, u32 width)
+{
+	int pitch;
+
+	switch (fmt) {
+
+	case B2R2_BLT_FMT_1_BIT_A1:
+		pitch = width >> 3; /* Shift is faster than division */
+		if ((width & 0x3) != 0) /* Check for remainder */
+			pitch++;
+		return pitch;
+
+	case B2R2_BLT_FMT_8_BIT_A8:                        /* Fall through */
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:            /* Fall through */
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:            /* Fall through */
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE: /* Fall through */
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:       /* Fall through */
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:       /* Fall through */
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:            /* Fall through */
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:            /* Fall through */
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE: /* Fall through */
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:       /* Fall through */
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:       /* Fall through */
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return width;
+
+	case B2R2_BLT_FMT_16_BIT_ARGB4444: /* Fall through */
+	case B2R2_BLT_FMT_16_BIT_ARGB1555: /* Fall through */
+	case B2R2_BLT_FMT_16_BIT_RGB565:   /* Fall through */
+	case B2R2_BLT_FMT_Y_CB_Y_CR:       /* Fall through */
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		return width << 1;
+
+	case B2R2_BLT_FMT_24_BIT_RGB888:   /* Fall through */
+	case B2R2_BLT_FMT_24_BIT_ARGB8565: /* Fall through */
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+		return width * 3;
+
+	case B2R2_BLT_FMT_32_BIT_ARGB8888: /* Fall through */
+	case B2R2_BLT_FMT_32_BIT_ABGR8888: /* Fall through */
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		return width << 2;
+
+	default:
+		/* Should never, ever happen */
+		BUG_ON(1);
+		return 0;
+	}
+}
+
+/**
+ * to_native_fmt() - returns the native B2R2 format
+ */
+static enum b2r2_native_fmt to_native_fmt(enum b2r2_blt_fmt fmt)
+{
+
+	switch (fmt) {
+	case B2R2_BLT_FMT_UNUSED:
+		return B2R2_NATIVE_RGB565;
+	case B2R2_BLT_FMT_1_BIT_A1:
+		return B2R2_NATIVE_A1;
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return B2R2_NATIVE_A8;
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+		return B2R2_NATIVE_RGB565;
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		return B2R2_NATIVE_ARGB4444;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		return B2R2_NATIVE_ARGB1555;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		return B2R2_NATIVE_ARGB8565;
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+		return B2R2_NATIVE_RGB888;
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+		return B2R2_NATIVE_YCBCR888;
+	case B2R2_BLT_FMT_32_BIT_ABGR8888: /* Not actually supported by HW */
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+		return B2R2_NATIVE_ARGB8888;
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		return B2R2_NATIVE_AYCBCR8888;
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		return B2R2_NATIVE_YCBCR422R;
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+		return B2R2_NATIVE_YCBCR422R;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return B2R2_NATIVE_YCBCR42X_R2B;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return B2R2_NATIVE_YCBCR42X_MBN;
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return B2R2_NATIVE_YUV;
+	default:
+		/* Should never ever happen */
+		return B2R2_NATIVE_BYTE;
+	}
+}
+
+/**
+ * Bit-expand the color from fmt to RGB888 with blue at LSB.
+ * Copy MSBs into missing LSBs.
+ */
+static u32 to_RGB888(u32 color, const enum b2r2_blt_fmt fmt)
+{
+	u32 out_color = 0;
+	u32 r = 0;
+	u32 g = 0;
+	u32 b = 0;
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		r = ((color & 0xf00) << 12) | ((color & 0xf00) << 8);
+		g = ((color & 0xf0) << 8) | ((color & 0xf0) << 4);
+		b = ((color & 0xf) << 4) | (color & 0xf);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		r = ((color & 0x7c00) << 9) | ((color & 0x7000) << 4);
+		g = ((color & 0x3e0) << 6) | ((color & 0x380) << 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+		r = ((color & 0xf800) << 8) | ((color & 0xe000) << 3);
+		g = ((color & 0x7e0) << 5) | ((color & 0x600) >> 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+		out_color = color & 0xffffff;
+		break;
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		r = (color & 0xff) << 16;
+		g = color & 0xff00;
+		b = (color & 0xff0000) >> 16;
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		r = ((color & 0xf800) << 8) | ((color & 0xe000) << 3);
+		g = ((color & 0x7e0) << 5) | ((color & 0x600) >> 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	default:
+		break;
+	}
+
+	return out_color;
+}
+
+/**
+ * get_fmt_type() - returns the type of the given format (raster, planar, etc.)
+ */
+static enum b2r2_fmt_type get_fmt_type(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return B2R2_FMT_TYPE_RASTER;
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return B2R2_FMT_TYPE_PLANAR;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return B2R2_FMT_TYPE_SEMI_PLANAR;
+	default:
+		return B2R2_FMT_TYPE_RASTER;
+	}
+}
+
+/**
+ * is_transform() - returns whether the given request is a transform operation
+ */
+static bool is_transform(const struct b2r2_blt_request *req)
+{
+	return (req->user_req.transform != B2R2_BLT_TRANSFORM_NONE) ||
+			(req->user_req.src_rect.width !=
+				req->user_req.dst_rect.width) ||
+			(req->user_req.src_rect.height !=
+				req->user_req.dst_rect.height);
+}
+
+/**
+ * calculate_scale_factor() - calculates the scale factor between the given
+ *                            values
+ */
+static int calculate_scale_factor(u32 from, u32 to, u16 *sf_out)
+{
+	int ret;
+	u32 sf;
+
+	b2r2_log_info("%s\n", __func__);
+
+	if (to == from) {
+		*sf_out = 1 << 10;
+		return 0;
+	} else if (to == 0) {
+		b2r2_log_err("%s: To is 0!\n", __func__);
+		BUG_ON(1);
+	}
+
+	sf = (from << 10) / to;
+
+	if ((sf & 0xffff0000) != 0) {
+		/* Overflow error */
+		b2r2_log_warn("%s: "
+			"Scale factor too large\n", __func__);
+		ret = -EINVAL;
+		goto error;
+	} else if (sf == 0) {
+		b2r2_log_warn("%s: "
+			"Scale factor too small\n", __func__);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	*sf_out = (u16)sf;
+
+	b2r2_log_info("%s exit\n", __func__);
+
+	return 0;
+
+error:
+	b2r2_log_warn("%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * rescale() - rescales the given dimension
+ *
+ * Returns the rescaled dimension in 22.10 fixed point format.
+ */
+static s32 rescale(s32 dim, u16 sf)
+{
+	b2r2_log_info("%s\n", __func__);
+
+	if (sf == 0) {
+		b2r2_log_err("%s: Scale factor is 0!\n", __func__);
+		BUG_ON(1);
+	}
+
+	/*
+	 * This is normally not safe to do, since it drastically decreases the
+	 * precision of the integer part of the dimension. But since the B2R2
+	 * hardware only has 12-bit registers for these values, we are safe.
+	 */
+	return (dim << 20) / sf;
+}
+
+/**
+ * inv_rescale() - does an inverted rescale of the given dimension
+ *
+ * Returns the rescaled dimension in 22.10 fixed point format.
+ */
+static s32 inv_rescale(s32 dim, u16 sf)
+{
+	if (sf == 0)
+		return dim;
+
+	return dim * sf;
+}
+
+/**
+ * set_target() - sets the target registers of the given node
+ */
+static void set_target(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf)
+{
+	s32 l;
+	s32 r;
+	s32 t;
+	s32 b;
+
+	if (buf->tmp_buf_index)
+		node->dst_tmp_index = buf->tmp_buf_index;
+
+	node->node.GROUP1.B2R2_TBA = addr;
+	node->node.GROUP1.B2R2_TTY = buf->pitch | to_native_fmt(buf->fmt) |
+			buf->alpha_range | buf->chroma_selection | buf->hso |
+			buf->vso | buf->dither | buf->plane_selection;
+	node->node.GROUP1.B2R2_TSZ =
+			((buf->win.width & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((buf->win.height & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+	node->node.GROUP1.B2R2_TXY =
+			((buf->win.x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((buf->win.y & 0xffff) << B2R2_XY_Y_SHIFT);
+
+	/* Check if the rectangle is outside the buffer */
+	if (buf->vso == B2R2_TY_VSO_BOTTOM_TO_TOP)
+		t = buf->win.y - (buf->win.height - 1);
+	else
+		t = buf->win.y;
+
+	if (buf->hso == B2R2_TY_HSO_RIGHT_TO_LEFT)
+		l = buf->win.x - (buf->win.width - 1);
+	else
+		l = buf->win.x;
+
+	r = l + buf->win.width;
+	b = t + buf->win.height;
+
+	/* Clip to the destination buffer to prevent memory overwrites */
+	if ((l < 0) || (r > buf->width) || (t < 0) || (b > buf->height)) {
+		/* The clip rectangle is including the borders */
+		l = MAX(l, 0);
+		r = MIN(r, buf->width) - 1;
+		t = MAX(t, 0);
+		b = MIN(b, buf->height) - 1;
+
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_CLIP_WINDOW;
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_RECT_CLIP_ENABLED;
+		node->node.GROUP6.B2R2_CWO =
+			((l & 0x7FFF) << B2R2_CWS_X_SHIFT) |
+			((t & 0x7FFF) << B2R2_CWS_Y_SHIFT);
+		node->node.GROUP6.B2R2_CWS =
+			((r & 0x7FFF) << B2R2_CWO_X_SHIFT) |
+			((b & 0x7FFF) << B2R2_CWO_Y_SHIFT);
+	}
+
+}
+
+/**
+ * set_src() - configures the given source register with the given values
+ */
+static void set_src(struct b2r2_src_config *src, u32 addr,
+		struct b2r2_node_split_buf *buf)
+{
+	src->B2R2_SBA = addr;
+	src->B2R2_STY = buf->pitch | to_native_fmt(buf->fmt) |
+			buf->alpha_range | buf->hso | buf->vso;
+	src->B2R2_SSZ = ((buf->win.width & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((buf->win.height & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+	src->B2R2_SXY = ((buf->win.x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((buf->win.y & 0xffff) << B2R2_XY_Y_SHIFT);
+
+}
+
+/**
+ * set_src_1() - sets the source 1 registers of the given node
+ */
+static void set_src_1(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf)
+{
+	if (buf->tmp_buf_index)
+		node->src_tmp_index = buf->tmp_buf_index;
+
+	node->src_index = 1;
+
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_1;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_1_FETCH_FROM_MEM;
+
+	node->node.GROUP3.B2R2_SBA = addr;
+	node->node.GROUP3.B2R2_STY = buf->pitch | to_native_fmt(buf->fmt) |
+			buf->alpha_range | buf->hso | buf->vso;
+	node->node.GROUP3.B2R2_SXY =
+			((buf->win.x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((buf->win.y & 0xffff) << B2R2_XY_Y_SHIFT);
+
+	/* Source 1 has no size register */
+}
+
+/**
+ * set_src_2() - sets the source 2 registers of the given node
+ */
+static void set_src_2(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf)
+{
+	if (buf->tmp_buf_index)
+		node->src_tmp_index = buf->tmp_buf_index;
+
+	node->src_index = 2;
+
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+
+	set_src(&node->node.GROUP4, addr, buf);
+}
+
+/**
+ * set_src_3() - sets the source 3 registers of the given node
+ */
+static void set_src_3(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf)
+{
+	if (buf->tmp_buf_index)
+		node->src_tmp_index = buf->tmp_buf_index;
+
+	node->src_index = 3;
+
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_3;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_3_FETCH_FROM_MEM;
+
+	set_src(&node->node.GROUP5, addr, buf);
+}
+
+/**
+ * set_ivmx() - configures the iVMX registers with the given values
+ */
+static void set_ivmx(struct b2r2_node *node, const u32 *vmx_values)
+{
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_IVMX_ENABLED;
+
+	node->node.GROUP15.B2R2_VMX0 = vmx_values[0];
+	node->node.GROUP15.B2R2_VMX1 = vmx_values[1];
+	node->node.GROUP15.B2R2_VMX2 = vmx_values[2];
+	node->node.GROUP15.B2R2_VMX3 = vmx_values[3];
+}
+
+/**
+ * reset_nodes() - clears the node list
+ */
+static void reset_nodes(struct b2r2_node *node)
+{
+	while (node != NULL) {
+		memset(&node->node, 0, sizeof(node->node));
+
+		node->src_tmp_index = 0;
+		node->dst_tmp_index = 0;
+
+		/* TODO: Implement support for short linked lists */
+		node->node.GROUP0.B2R2_CIC = 0x7ffff;
+
+		if (node->next != NULL)
+			node->node.GROUP0.B2R2_NIP =
+					node->next->physical_address;
+		node = node->next;
+	}
+}
+
+int b2r2_node_split_init(void)
+{
+	b2r2_filters_init();
+
+	return 0;
+}
+
+void b2r2_node_split_exit(void)
+{
+	b2r2_filters_exit();
+}
diff --git a/drivers/video/b2r2/b2r2_node_split.h b/drivers/video/b2r2/b2r2_node_split.h
new file mode 100644
index 00000000000..5bceac28488
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_node_split.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 node splitter
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __B2R2_NODE_SPLIT_H_
+#define __B2R2_NODE_SPLIT_H_
+
+#include "b2r2_internal.h"
+#include "b2r2_hw.h"
+
+/**
+ * b2r2_node_split_analyze() - Analyzes a B2R2 request
+ *
+ * @req          - The request to analyze
+ * @max_buf_size - The largest size allowed for intermediate buffers
+ * @node_count   - Number of nodes required for the job
+ * @buf_count    - Number of intermediate buffers required for the job
+ * @bufs         - An array of buffers needed for intermediate buffers
+ *
+ * Analyzes the request and determines how many nodes and intermediate buffers
+ * are required.
+ *
+ * It is the responsibility of the caller to allocate memory and assign the
+ * physical addresses. After that b2r2_node_split_assign_buffers should be
+ * called to assign the buffers to the right nodes.
+ *
+ * Returns:
+ *   A handle identifing the analyzed request if successful, a negative
+ *   value otherwise.
+ */
+int b2r2_node_split_analyze(const struct b2r2_blt_request *req, u32 max_buf_size,
+		u32 *node_count, struct b2r2_work_buf **bufs, u32* buf_count,
+		struct b2r2_node_split_job *job);
+
+/**
+ * b2r2_node_split_configure() - Performs a node split
+ *
+ * @handle - A handle for the analyzed request
+ * @first  - The first node in the list of nodes to use
+ *
+ * Fills the supplied list of nodes with the parameters acquired by analyzing
+ * the request.
+ *
+ * All pointers to intermediate buffers are represented by integers to be used
+ * in the array returned by b2r2_node_split_analyze.
+ *
+ * Returns:
+ *   A negative value if an error occurred, 0 otherwise.
+ */
+int b2r2_node_split_configure(struct b2r2_node_split_job *job,
+		struct b2r2_node *first);
+
+/**
+ * b2r2_node_split_assign_buffers() - Assignes physical addresses
+ *
+ * @handle    - The handle for the job
+ * @first     - The first node in the node list
+ * @bufs      - Buffers with assigned physical addresses
+ * @buf_count - Number of physical addresses
+ *
+ * Assigns the physical addresses where intermediate buffers are required in
+ * the node list.
+ *
+ * The order of the elements of 'bufs' must be maintained from the call to
+ * b2r2_node_split_analyze.
+ *
+ * Returns:
+ *   A negative value if an error occurred, 0 otherwise.
+ */
+int b2r2_node_split_assign_buffers(struct b2r2_node_split_job *job,
+		struct b2r2_node *first, struct b2r2_work_buf *bufs,
+		u32 buf_count);
+
+/**
+ * b2r2_node_split_unassign_buffers() - Removes all physical addresses
+ *
+ * @handle - The handle associated with the job
+ * @first  - The first node in the node list
+ *
+ * Removes all references to intermediate buffers from the node list.
+ *
+ * This makes it possible to reuse the node list with new buffers by calling
+ * b2r2_node_split_assign_buffers again. Useful for caching node lists.
+ */
+void b2r2_node_split_unassign_buffers(struct b2r2_node_split_job *job,
+		struct b2r2_node *first);
+
+/**
+ * b2r2_node_split_release() - Releases all resources for a job
+ *
+ * @handle - The handle identifying the job. This will be set to 0.
+ *
+ * Releases all resources associated with a job.
+ *
+ * This should always be called once b2r2_node_split_analyze has been called
+ * in order to release any resources allocated while analyzing.
+ */
+void b2r2_node_split_cancel(struct b2r2_node_split_job *job);
+
+/**
+ * b2r2_node_split_init() - Initializes the node split module
+ *
+ * Initializes the node split module and creates debugfs files.
+ */
+int b2r2_node_split_init(void);
+
+/**
+ * b2r2_node_split_exit() - Deinitializes the node split module
+ *
+ * Releases all resources for the node split module.
+ */
+void b2r2_node_split_exit(void);
+
+#endif
diff --git a/drivers/video/b2r2/b2r2_profiler/Makefile b/drivers/video/b2r2/b2r2_profiler/Makefile
new file mode 100644
index 00000000000..69a85524fd7
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_profiler/Makefile
@@ -0,0 +1,3 @@
+# Make file for loadable module B2R2 Profiler
+
+obj-$(CONFIG_B2R2_PROFILER) += b2r2_profiler.o
diff --git a/drivers/video/b2r2/b2r2_profiler/b2r2_profiler.c b/drivers/video/b2r2/b2r2_profiler/b2r2_profiler.c
new file mode 100644
index 00000000000..2e7c5ca5a7a
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_profiler/b2r2_profiler.c
@@ -0,0 +1,258 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson B2R2 profiler implementation
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/jiffies.h>
+
+#include <video/b2r2_blt.h>
+#include "../b2r2_profiler_api.h"
+
+
+#define S32_MAX 2147483647
+
+
+static int src_format_filter_on = false;
+module_param(src_format_filter_on, bool, S_IRUGO | S_IWUSR);
+static unsigned int src_format_filter;
+module_param(src_format_filter, uint, S_IRUGO | S_IWUSR);
+
+static int print_blts_on = 0;
+module_param(print_blts_on, bool, S_IRUGO | S_IWUSR);
+static int use_mpix_per_second_in_print_blts = 1;
+module_param(use_mpix_per_second_in_print_blts, bool, S_IRUGO | S_IWUSR);
+
+static int min_avg_max_mpix_per_second_on = 1;
+module_param(min_avg_max_mpix_per_second_on, bool, S_IRUGO | S_IWUSR);
+
+static const unsigned int min_avg_max_mpix_per_second_num_blts_used = 400;
+static struct {
+	unsigned long sampling_start_time_jiffies;
+
+	s32 min_mpix_per_second;
+	struct b2r2_blt_req min_blt_request;
+	struct b2r2_blt_profiling_info min_blt_profiling_info;
+
+	s32 max_mpix_per_second;
+	struct b2r2_blt_req max_blt_request;
+	struct b2r2_blt_profiling_info max_blt_profiling_info;
+
+	s32 accumulated_num_pixels;
+	s32 accumulated_num_usecs;
+
+	u32 num_blts_done;
+} min_avg_max_mpix_per_second_state;
+
+
+static s32 nsec_2_usec(const s32 nsec);
+
+static int is_scale_blt(const struct b2r2_blt_req * const request);
+static s32 get_blt_mpix_per_second(const struct b2r2_blt_req * const request, const struct b2r2_blt_profiling_info * const blt_profiling_info);
+static void print_blt(const struct b2r2_blt_req * const request, const struct b2r2_blt_profiling_info * const blt_profiling_info);
+
+static s32 get_num_pixels_in_blt(const struct b2r2_blt_req * const request);
+static s32 get_mpix_per_second(const s32 num_pixels, const s32 num_usecs);
+static void print_min_avg_max_mpix_per_second_state(void);
+static void reset_min_avg_max_mpix_per_second_state(void);
+static void do_min_avg_max_mpix_per_second(const struct b2r2_blt_req * const request, const struct b2r2_blt_profiling_info * const blt_profiling_info);
+
+static void blt_done(const struct b2r2_blt_req * const blt, const s32 request_id, const struct b2r2_blt_profiling_info * const blt_profiling_info);
+
+
+static struct b2r2_profiler this = {
+	.blt_done = blt_done,
+};
+
+
+static s32 nsec_2_usec(const s32 nsec)
+{
+	return nsec / 1000;
+}
+
+
+static int is_scale_blt(const struct b2r2_blt_req * const request)
+{
+	if ((request->transform & B2R2_BLT_TRANSFORM_CCW_ROT_90 &&
+		(request->src_rect.width != request->dst_rect.height ||
+		request->src_rect.height != request->dst_rect.width)) ||
+		(!(request->transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) &&
+		(request->src_rect.width != request->dst_rect.width ||
+		request->src_rect.height != request->dst_rect.height)))
+		return 1;
+	else
+		return 0;
+}
+
+static s32 get_blt_mpix_per_second(const struct b2r2_blt_req * const request, const struct b2r2_blt_profiling_info * const blt_profiling_info)
+{
+	return get_mpix_per_second(get_num_pixels_in_blt(request),
+		nsec_2_usec(blt_profiling_info->nsec_active_in_cpu + blt_profiling_info->nsec_active_in_b2r2));
+}
+
+static void print_blt(const struct b2r2_blt_req * const request, const struct b2r2_blt_profiling_info * const blt_profiling_info)
+{
+	char tmp_str[128];
+	sprintf(tmp_str, "SF: %#10x, DF: %#10x, F: %#10x, T: %#3x, S: %1i, P: %7i",
+		request->src_img.fmt,
+		request->dst_img.fmt,
+		request->flags,
+		request->transform,
+		is_scale_blt(request),
+		get_num_pixels_in_blt(request));
+	if (use_mpix_per_second_in_print_blts)
+		printk(KERN_ALERT "%s, MPix/s: %3i\n",
+			tmp_str,
+			get_blt_mpix_per_second(request, blt_profiling_info));
+	else
+		printk(KERN_ALERT "%s, CPU: %10i, B2R2: %10i, Tot: %10i ns\n",
+			tmp_str,
+			blt_profiling_info->nsec_active_in_cpu,
+			blt_profiling_info->nsec_active_in_b2r2,
+			blt_profiling_info->total_time_nsec);
+}
+
+
+static s32 get_num_pixels_in_blt(const struct b2r2_blt_req * const request)
+{
+	s32 num_pixels_in_src = request->src_rect.width * request->src_rect.height;
+	s32 num_pixels_in_dst = request->dst_rect.width * request->dst_rect.height;
+	if (request->flags & (B2R2_BLT_FLAG_SOURCE_FILL |
+				B2R2_BLT_FLAG_SOURCE_FILL_RAW))
+		return num_pixels_in_dst;
+	else
+		return (num_pixels_in_src + num_pixels_in_dst) / 2;
+}
+
+static s32 get_mpix_per_second(const s32 num_pixels, const s32 num_usecs)
+{
+	s32 num_pixels_scale_factor = num_pixels != 0 ? S32_MAX / num_pixels : S32_MAX;
+	s32 num_usecs_scale_factor = num_usecs != 0 ? S32_MAX / num_usecs : S32_MAX;
+	s32 scale_factor = min(num_pixels_scale_factor, num_usecs_scale_factor);
+
+	s32 num_pixels_scaled = num_pixels * scale_factor;
+	s32 num_usecs_scaled = num_usecs * scale_factor;
+
+	if (num_usecs_scaled < 1000000)
+		return 0;
+
+	return (num_pixels_scaled / 1000000) / (num_usecs_scaled / 1000000);
+}
+
+static void print_min_avg_max_mpix_per_second_state(void)
+{
+	printk(KERN_ALERT "Min: %3i, Avg: %3i, Max: %3i MPix/s\n",
+		min_avg_max_mpix_per_second_state.min_mpix_per_second,
+		get_mpix_per_second(min_avg_max_mpix_per_second_state.accumulated_num_pixels,
+		min_avg_max_mpix_per_second_state.accumulated_num_usecs),
+		min_avg_max_mpix_per_second_state.max_mpix_per_second);
+	printk(KERN_ALERT "Min blit:\n");
+	print_blt(&min_avg_max_mpix_per_second_state.min_blt_request,
+		&min_avg_max_mpix_per_second_state.min_blt_profiling_info);
+	printk(KERN_ALERT "Max blit:\n");
+	print_blt(&min_avg_max_mpix_per_second_state.max_blt_request,
+		&min_avg_max_mpix_per_second_state.max_blt_profiling_info);
+}
+
+static void reset_min_avg_max_mpix_per_second_state(void)
+{
+	min_avg_max_mpix_per_second_state.sampling_start_time_jiffies =
+		jiffies;
+	min_avg_max_mpix_per_second_state.min_mpix_per_second = S32_MAX;
+	min_avg_max_mpix_per_second_state.max_mpix_per_second = 0;
+	min_avg_max_mpix_per_second_state.accumulated_num_pixels = 0;
+	min_avg_max_mpix_per_second_state.accumulated_num_usecs = 0;
+	min_avg_max_mpix_per_second_state.num_blts_done = 0;
+}
+
+static void do_min_avg_max_mpix_per_second(const struct b2r2_blt_req * const request, const struct b2r2_blt_profiling_info * const blt_profiling_info)
+{
+	s32 num_pixels_in_blt;
+	s32 num_usec_blt_took;
+	s32 blt_mpix_per_second;
+
+	if (time_before(jiffies, min_avg_max_mpix_per_second_state.sampling_start_time_jiffies))
+		return;
+
+	num_pixels_in_blt = get_num_pixels_in_blt(request);
+	num_usec_blt_took = nsec_2_usec(blt_profiling_info->nsec_active_in_cpu + blt_profiling_info->nsec_active_in_b2r2);
+	blt_mpix_per_second = get_mpix_per_second(num_pixels_in_blt,
+		num_usec_blt_took);
+
+	if (blt_mpix_per_second <= min_avg_max_mpix_per_second_state.min_mpix_per_second) {
+		min_avg_max_mpix_per_second_state.min_mpix_per_second =
+			blt_mpix_per_second;
+		memcpy(&min_avg_max_mpix_per_second_state.min_blt_request,
+			request, sizeof(struct b2r2_blt_req));
+		memcpy(&min_avg_max_mpix_per_second_state.min_blt_profiling_info,
+			blt_profiling_info, sizeof(struct b2r2_blt_profiling_info));
+	}
+
+	if (blt_mpix_per_second >= min_avg_max_mpix_per_second_state.max_mpix_per_second) {
+		min_avg_max_mpix_per_second_state.max_mpix_per_second =
+			blt_mpix_per_second;
+		memcpy(&min_avg_max_mpix_per_second_state.max_blt_request,
+			request, sizeof(struct b2r2_blt_req));
+		memcpy(&min_avg_max_mpix_per_second_state.max_blt_profiling_info,
+			blt_profiling_info, sizeof(struct b2r2_blt_profiling_info));
+	}
+
+	min_avg_max_mpix_per_second_state.accumulated_num_pixels +=
+		num_pixels_in_blt;
+	min_avg_max_mpix_per_second_state.accumulated_num_usecs +=
+		num_usec_blt_took;
+
+	min_avg_max_mpix_per_second_state.num_blts_done++;
+
+	if (min_avg_max_mpix_per_second_state.num_blts_done >= min_avg_max_mpix_per_second_num_blts_used) {
+		print_min_avg_max_mpix_per_second_state();
+		reset_min_avg_max_mpix_per_second_state();
+		/* The printouts initiated above can disturb the next measurement
+		so we delay it two seconds to give the printouts a chance to finish. */
+		min_avg_max_mpix_per_second_state.sampling_start_time_jiffies =
+			jiffies + (2 * HZ);
+	}
+}
+
+static void blt_done(const struct b2r2_blt_req * const request, const s32 request_id, const struct b2r2_blt_profiling_info * const blt_profiling_info)
+{
+	/* Filters */
+	if (src_format_filter_on && request->src_img.fmt != src_format_filter)
+		return;
+
+	/* Processors */
+	if (print_blts_on)
+		print_blt(request, blt_profiling_info);
+
+	if (min_avg_max_mpix_per_second_on)
+		do_min_avg_max_mpix_per_second(request, blt_profiling_info);
+}
+
+
+static int __init b2r2_profiler_init(void)
+{
+	reset_min_avg_max_mpix_per_second_state();
+
+	return b2r2_register_profiler(&this);
+}
+module_init(b2r2_profiler_init);
+
+static void __exit b2r2_profiler_exit(void)
+{
+	b2r2_unregister_profiler(&this);
+}
+module_exit(b2r2_profiler_exit);
+
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Johan Mossberg (johan.xx.mossberg@stericsson.com)");
+MODULE_DESCRIPTION("B2R2 Profiler");
diff --git a/drivers/video/b2r2/b2r2_profiler_api.h b/drivers/video/b2r2/b2r2_profiler_api.h
new file mode 100644
index 00000000000..5f1f9abbe49
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_profiler_api.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 profiling API
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#ifndef _LINUX_VIDEO_B2R2_PROFILER_API_H
+#define _LINUX_VIDEO_B2R2_PROFILER_API_H
+
+#include <video/b2r2_blt.h>
+
+/**
+ * struct b2r2_blt_profiling_info - Profiling information for a blit
+ *
+ * @nsec_active_in_cpu: The number of nanoseconds the job was active in the CPU.
+ *                      This is an approximate value, check out the code for more
+ *                      info.
+ * @nsec_active_in_b2r2: The number of nanoseconds the job was active in B2R2. This
+ *                       is an approximate value, check out the code for more info.
+ * @total_time_nsec: The total time the job took in nano seconds. Includes ideling.
+ */
+struct b2r2_blt_profiling_info {
+	s32 nsec_active_in_cpu;
+	s32 nsec_active_in_b2r2;
+	s32 total_time_nsec;
+};
+
+/**
+ * struct b2r2_profiler - B2R2 profiler.
+ *
+ * The callbacks are never run concurrently. No heavy stuff must be done in the
+ * callbacks as this might adversely affect the B2R2 driver. The callbacks must
+ * not call the B2R2 profiler API as this will cause a deadlock. If the callbacks
+ * call into the B2R2 driver care must be taken as deadlock situations can arise.
+ *
+ * @blt_done: Called when a blit has finished, timed out or been canceled.
+ */
+struct b2r2_profiler {
+	void (*blt_done)(const struct b2r2_blt_req * const request, const s32 request_id, const struct b2r2_blt_profiling_info * const blt_profiling_info);
+};
+
+/**
+ * b2r2_register_profiler() - Registers a profiler.
+ *
+ * Currently only one profiler can be registered at any given time.
+ *
+ * @profiler: The profiler
+ *
+ * Returns 0 on success, negative error code on failure
+ */
+int b2r2_register_profiler(const struct b2r2_profiler * const profiler);
+
+/**
+ * b2r2_unregister_profiler() - Unregisters a profiler.
+ *
+ * @profiler: The profiler
+ */
+void b2r2_unregister_profiler(const struct b2r2_profiler * const profiler);
+
+#endif /* #ifdef _LINUX_VIDEO_B2R2_PROFILER_API_H */
diff --git a/drivers/video/b2r2/b2r2_profiler_socket.c b/drivers/video/b2r2/b2r2_profiler_socket.c
new file mode 100644
index 00000000000..f96ab5be76e
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_profiler_socket.c
@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 profiler socket communication
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/semaphore.h>
+#include <asm/errno.h>
+
+#include "b2r2_profiler_api.h"
+#include "b2r2_internal.h"
+
+
+/*
+ * TODO: Call the profiler in a seperate thread and have a circular buffer
+ * between the B2R2 driver and that thread. That way the profiler can not slow
+ * down or kill the B2R2 driver. Seems a bit overkill right now as there is
+ * only one B2R2 profiler and we have full control over it but the situation
+ * may be different in the future.
+ */
+
+
+static const struct b2r2_profiler *b2r2_profiler;
+static DEFINE_SEMAPHORE(b2r2_profiler_lock);
+
+
+int b2r2_register_profiler(const struct b2r2_profiler * const profiler)
+{
+	int return_value;
+
+	return_value = down_interruptible(&b2r2_profiler_lock);
+	if (return_value != 0)
+		return return_value;
+
+	if (b2r2_profiler != NULL) {
+		return_value = -EUSERS;
+
+		goto cleanup;
+	}
+
+	b2r2_profiler = profiler;
+
+	return_value = 0;
+
+cleanup:
+	up(&b2r2_profiler_lock);
+
+	return return_value;
+}
+EXPORT_SYMBOL(b2r2_register_profiler);
+
+void b2r2_unregister_profiler(const struct b2r2_profiler * const profiler)
+{
+	down(&b2r2_profiler_lock);
+
+	if (profiler == b2r2_profiler)
+		b2r2_profiler = NULL;
+
+	up(&b2r2_profiler_lock);
+}
+EXPORT_SYMBOL(b2r2_unregister_profiler);
+
+
+bool is_profiler_registered_approx(void)
+{
+	/* No locking by design, to make it fast, hence the approx */
+	if (b2r2_profiler != NULL)
+		return true;
+	else
+		return false;
+}
+
+void b2r2_call_profiler_blt_done(const struct b2r2_blt_request * const request)
+{
+	int return_value;
+	struct b2r2_blt_profiling_info blt_profiling_info;
+
+	return_value = down_interruptible(&b2r2_profiler_lock);
+	if (return_value != 0) {
+		dev_err(b2r2_blt_device(),
+			"%s: Failed to acquire semaphore, ret=%i. Lost profiler call!\n",
+			__func__, return_value);
+
+		return;
+	}
+
+	if (NULL == b2r2_profiler)
+		goto cleanup;
+
+	blt_profiling_info.nsec_active_in_cpu = request->nsec_active_in_cpu;
+	blt_profiling_info.nsec_active_in_b2r2 = request->job.nsec_active_in_hw;
+	blt_profiling_info.total_time_nsec = request->total_time_nsec;
+
+	b2r2_profiler->blt_done(&request->user_req, request->request_id, &blt_profiling_info);
+
+cleanup:
+	up(&b2r2_profiler_lock);
+}
diff --git a/drivers/video/b2r2/b2r2_profiler_socket.h b/drivers/video/b2r2/b2r2_profiler_socket.h
new file mode 100644
index 00000000000..80b2c20293f
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_profiler_socket.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 profiler socket communication
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_VIDEO_B2R2_PROFILER_SOCKET_H
+#define _LINUX_VIDEO_B2R2_PROFILER_SOCKET_H
+
+#include "b2r2_internal.h"
+
+/* Will give a correct result most of the time but can be wrong */
+bool is_profiler_registered_approx(void);
+
+void b2r2_call_profiler_blt_done(const struct b2r2_blt_request * const request);
+
+#endif /* _LINUX_VIDEO_B2R2_PROFILER_SOCKET_H */
diff --git a/drivers/video/b2r2/b2r2_structures.h b/drivers/video/b2r2/b2r2_structures.h
new file mode 100644
index 00000000000..99fa7f047d3
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_structures.h
@@ -0,0 +1,226 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 register struct
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#ifndef __B2R2_STRUCTURES_H
+#define __B2R2_STRUCTURES_H
+
+/* C struct view */
+struct b2r2_memory_map {
+	unsigned char fill0[2304];
+	unsigned int BLT_SSBA17; /* @2304 */
+	unsigned int BLT_SSBA18; /* @2308 */
+	unsigned int BLT_SSBA19; /* @2312 */
+	unsigned int BLT_SSBA20; /* @2316 */
+	unsigned int BLT_SSBA21; /* @2320 */
+	unsigned int BLT_SSBA22; /* @2324 */
+	unsigned int BLT_SSBA23; /* @2328 */
+	unsigned int BLT_SSBA24; /* @2332 */
+	unsigned char fill1[32];
+	unsigned int BLT_STBA5; /* @2368 */
+	unsigned int BLT_STBA6; /* @2372 */
+	unsigned int BLT_STBA7; /* @2376 */
+	unsigned int BLT_STBA8; /* @2380 */
+	unsigned char fill2[176];
+	unsigned int BLT_CTL; /* @2560 */
+	unsigned int BLT_ITS; /* @2564 */
+	unsigned int BLT_STA1; /* @2568 */
+	unsigned char fill3[4];
+	unsigned int BLT_SSBA1; /* @2576 */
+	unsigned int BLT_SSBA2; /* @2580 */
+	unsigned int BLT_SSBA3; /* @2584 */
+	unsigned int BLT_SSBA4; /* @2588 */
+	unsigned int BLT_SSBA5; /* @2592 */
+	unsigned int BLT_SSBA6; /* @2596 */
+	unsigned int BLT_SSBA7; /* @2600 */
+	unsigned int BLT_SSBA8; /* @2604 */
+	unsigned int BLT_STBA1; /* @2608 */
+	unsigned int BLT_STBA2; /* @2612 */
+	unsigned int BLT_STBA3; /* @2616 */
+	unsigned int BLT_STBA4; /* @2620 */
+	unsigned int BLT_CQ1_TRIG_IP; /* @2624 */
+	unsigned int BLT_CQ1_TRIG_CTL; /* @2628 */
+	unsigned int BLT_CQ1_PACE_CTL; /* @2632 */
+	unsigned int BLT_CQ1_IP; /* @2636 */
+	unsigned int BLT_CQ2_TRIG_IP; /* @2640 */
+	unsigned int BLT_CQ2_TRIG_CTL; /* @2644 */
+	unsigned int BLT_CQ2_PACE_CTL; /* @2648 */
+	unsigned int BLT_CQ2_IP; /* @2652 */
+	unsigned int BLT_AQ1_CTL; /* @2656 */
+	unsigned int BLT_AQ1_IP; /* @2660 */
+	unsigned int BLT_AQ1_LNA; /* @2664 */
+	unsigned int BLT_AQ1_STA; /* @2668 */
+	unsigned int BLT_AQ2_CTL; /* @2672 */
+	unsigned int BLT_AQ2_IP; /* @2676 */
+	unsigned int BLT_AQ2_LNA; /* @2680 */
+	unsigned int BLT_AQ2_STA; /* @2684 */
+	unsigned int BLT_AQ3_CTL; /* @2688 */
+	unsigned int BLT_AQ3_IP; /* @2692 */
+	unsigned int BLT_AQ3_LNA; /* @2696 */
+	unsigned int BLT_AQ3_STA; /* @2700 */
+	unsigned int BLT_AQ4_CTL; /* @2704 */
+	unsigned int BLT_AQ4_IP; /* @2708 */
+	unsigned int BLT_AQ4_LNA; /* @2712 */
+	unsigned int BLT_AQ4_STA; /* @2716 */
+	unsigned int BLT_SSBA9; /* @2720 */
+	unsigned int BLT_SSBA10; /* @2724 */
+	unsigned int BLT_SSBA11; /* @2728 */
+	unsigned int BLT_SSBA12; /* @2732 */
+	unsigned int BLT_SSBA13; /* @2736 */
+	unsigned int BLT_SSBA14; /* @2740 */
+	unsigned int BLT_SSBA15; /* @2744 */
+	unsigned int BLT_SSBA16; /* @2748 */
+	unsigned int BLT_SGA1; /* @2752 */
+	unsigned int BLT_SGA2; /* @2756 */
+	unsigned char fill4[8];
+	unsigned int BLT_ITM0; /* @2768 */
+	unsigned int BLT_ITM1; /* @2772 */
+	unsigned int BLT_ITM2; /* @2776 */
+	unsigned int BLT_ITM3; /* @2780 */
+	unsigned char fill5[16];
+	unsigned int BLT_DFV2; /* @2800 */
+	unsigned int BLT_DFV1; /* @2804 */
+	unsigned int BLT_PRI; /* @2808 */
+	unsigned char fill6[8];
+	unsigned int PLUGS1_OP2; /* @2820 */
+	unsigned int PLUGS1_CHZ; /* @2824 */
+	unsigned int PLUGS1_MSZ; /* @2828 */
+	unsigned int PLUGS1_PGZ; /* @2832 */
+	unsigned char fill7[16];
+	unsigned int PLUGS2_OP2; /* @2852 */
+	unsigned int PLUGS2_CHZ; /* @2856 */
+	unsigned int PLUGS2_MSZ; /* @2860 */
+	unsigned int PLUGS2_PGZ; /* @2864 */
+	unsigned char fill8[16];
+	unsigned int PLUGS3_OP2; /* @2884 */
+	unsigned int PLUGS3_CHZ; /* @2888 */
+	unsigned int PLUGS3_MSZ; /* @2892 */
+	unsigned int PLUGS3_PGZ; /* @2896 */
+	unsigned char fill9[48];
+	unsigned int PLUGT_OP2; /* @2948 */
+	unsigned int PLUGT_CHZ; /* @2952 */
+	unsigned int PLUGT_MSZ; /* @2956 */
+	unsigned int PLUGT_PGZ; /* @2960 */
+	unsigned char fill10[108];
+	unsigned int BLT_NIP; /* @3072 */
+	unsigned int BLT_CIC; /* @3076 */
+	unsigned int BLT_INS; /* @3080 */
+	unsigned int BLT_ACK; /* @3084 */
+	unsigned int BLT_TBA; /* @3088 */
+	unsigned int BLT_TTY; /* @3092 */
+	unsigned int BLT_TXY; /* @3096 */
+	unsigned int BLT_TSZ; /* @3100 */
+	unsigned int BLT_S1CF; /* @3104 */
+	unsigned int BLT_S2CF; /* @3108 */
+	unsigned int BLT_S1BA; /* @3112 */
+	unsigned int BLT_S1TY; /* @3116 */
+	unsigned int BLT_S1XY; /* @3120 */
+	unsigned char fill11[4];
+	unsigned int BLT_S2BA; /* @3128 */
+	unsigned int BLT_S2TY; /* @3132 */
+	unsigned int BLT_S2XY; /* @3136 */
+	unsigned int BLT_S2SZ; /* @3140 */
+	unsigned int BLT_S3BA; /* @3144 */
+	unsigned int BLT_S3TY; /* @3148 */
+	unsigned int BLT_S3XY; /* @3152 */
+	unsigned int BLT_S3SZ; /* @3156 */
+	unsigned int BLT_CWO; /* @3160 */
+	unsigned int BLT_CWS; /* @3164 */
+	unsigned int BLT_CCO; /* @3168 */
+	unsigned int BLT_CML; /* @3172 */
+	unsigned int BLT_FCTL; /* @3176 */
+	unsigned int BLT_PMK; /* @3180 */
+	unsigned int BLT_RSF; /* @3184 */
+	unsigned int BLT_RZI; /* @3188 */
+	unsigned int BLT_HFP; /* @3192 */
+	unsigned int BLT_VFP; /* @3196 */
+	unsigned int BLT_Y_RSF; /* @3200 */
+	unsigned int BLT_Y_RZI; /* @3204 */
+	unsigned int BLT_Y_HFP; /* @3208 */
+	unsigned int BLT_Y_VFP; /* @3212 */
+	unsigned char fill12[16];
+	unsigned int BLT_KEY1; /* @3232 */
+	unsigned int BLT_KEY2; /* @3236 */
+	unsigned char fill13[8];
+	unsigned int BLT_SAR; /* @3248 */
+	unsigned int BLT_USR; /* @3252 */
+	unsigned char fill14[8];
+	unsigned int BLT_IVMX0; /* @3264 */
+	unsigned int BLT_IVMX1; /* @3268 */
+	unsigned int BLT_IVMX2; /* @3272 */
+	unsigned int BLT_IVMX3; /* @3276 */
+	unsigned int BLT_OVMX0; /* @3280 */
+	unsigned int BLT_OVMX1; /* @3284 */
+	unsigned int BLT_OVMX2; /* @3288 */
+	unsigned int BLT_OVMX3; /* @3292 */
+	unsigned char fill15[8];
+	unsigned int BLT_VC1R; /* @3304 */
+	unsigned char fill16[20];
+	unsigned int BLT_Y_HFC0; /* @3328 */
+	unsigned int BLT_Y_HFC1; /* @3332 */
+	unsigned int BLT_Y_HFC2; /* @3336 */
+	unsigned int BLT_Y_HFC3; /* @3340 */
+	unsigned int BLT_Y_HFC4; /* @3344 */
+	unsigned int BLT_Y_HFC5; /* @3348 */
+	unsigned int BLT_Y_HFC6; /* @3352 */
+	unsigned int BLT_Y_HFC7; /* @3356 */
+	unsigned int BLT_Y_HFC8; /* @3360 */
+	unsigned int BLT_Y_HFC9; /* @3364 */
+	unsigned int BLT_Y_HFC10; /* @3368 */
+	unsigned int BLT_Y_HFC11; /* @3372 */
+	unsigned int BLT_Y_HFC12; /* @3376 */
+	unsigned int BLT_Y_HFC13; /* @3380 */
+	unsigned int BLT_Y_HFC14; /* @3384 */
+	unsigned int BLT_Y_HFC15; /* @3388 */
+	unsigned char fill17[80];
+	unsigned int BLT_Y_VFC0; /* @3472 */
+	unsigned int BLT_Y_VFC1; /* @3476 */
+	unsigned int BLT_Y_VFC2; /* @3480 */
+	unsigned int BLT_Y_VFC3; /* @3484 */
+	unsigned int BLT_Y_VFC4; /* @3488 */
+	unsigned int BLT_Y_VFC5; /* @3492 */
+	unsigned int BLT_Y_VFC6; /* @3496 */
+	unsigned int BLT_Y_VFC7; /* @3500 */
+	unsigned int BLT_Y_VFC8; /* @3504 */
+	unsigned int BLT_Y_VFC9; /* @3508 */
+	unsigned char fill18[72];
+	unsigned int BLT_HFC0; /* @3584 */
+	unsigned int BLT_HFC1; /* @3588 */
+	unsigned int BLT_HFC2; /* @3592 */
+	unsigned int BLT_HFC3; /* @3596 */
+	unsigned int BLT_HFC4; /* @3600 */
+	unsigned int BLT_HFC5; /* @3604 */
+	unsigned int BLT_HFC6; /* @3608 */
+	unsigned int BLT_HFC7; /* @3612 */
+	unsigned int BLT_HFC8; /* @3616 */
+	unsigned int BLT_HFC9; /* @3620 */
+	unsigned int BLT_HFC10; /* @3624 */
+	unsigned int BLT_HFC11; /* @3628 */
+	unsigned int BLT_HFC12; /* @3632 */
+	unsigned int BLT_HFC13; /* @3636 */
+	unsigned int BLT_HFC14; /* @3640 */
+	unsigned int BLT_HFC15; /* @3644 */
+	unsigned char fill19[80];
+	unsigned int BLT_VFC0; /* @3728 */
+	unsigned int BLT_VFC1; /* @3732 */
+	unsigned int BLT_VFC2; /* @3736 */
+	unsigned int BLT_VFC3; /* @3740 */
+	unsigned int BLT_VFC4; /* @3744 */
+	unsigned int BLT_VFC5; /* @3748 */
+	unsigned int BLT_VFC6; /* @3752 */
+	unsigned int BLT_VFC7; /* @3756 */
+	unsigned int BLT_VFC8; /* @3760 */
+	unsigned int BLT_VFC9; /* @3764 */
+};
+
+#endif /* !defined(__B2R2_STRUCTURES_H) */
+
diff --git a/drivers/video/b2r2/b2r2_timing.c b/drivers/video/b2r2/b2r2_timing.c
new file mode 100644
index 00000000000..4f3e2b8b042
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_timing.c
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 timing
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/time.h>
+
+
+u32 b2r2_get_curr_nsec(void)
+{
+	struct timespec ts;
+
+	getrawmonotonic(&ts);
+
+	return (u32)timespec_to_ns(&ts);
+}
diff --git a/drivers/video/b2r2/b2r2_timing.h b/drivers/video/b2r2/b2r2_timing.h
new file mode 100644
index 00000000000..e87113c0ec9
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_timing.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 timing
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_DRIVERS_VIDEO_B2R2_TIMING_H_
+#define _LINUX_DRIVERS_VIDEO_B2R2_TIMING_H_
+
+/**
+ * b2r2_get_curr_nsec() - Return the current nanosecond. Notice that the value
+ *                        wraps when the u32 limit is reached.
+ *
+ */
+u32 b2r2_get_curr_nsec(void);
+
+#endif /* _LINUX_DRIVERS_VIDEO_B2R2_TIMING_H_ */
diff --git a/drivers/video/b2r2/b2r2_utils.c b/drivers/video/b2r2/b2r2_utils.c
new file mode 100644
index 00000000000..44ea06e51fd
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_utils.c
@@ -0,0 +1,379 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 utils
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include "b2r2_utils.h"
+
+#include "b2r2_debug.h"
+
+#include <video/b2r2_blt.h>
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+
+
+const s32 b2r2_s32_max = 2147483647;
+
+
+void b2r2_get_img_bounding_rect(struct b2r2_blt_img *img,
+					struct b2r2_blt_rect *bounding_rect)
+{
+	bounding_rect->x = 0;
+	bounding_rect->y = 0;
+	bounding_rect->width = img->width;
+	bounding_rect->height = img->height;
+}
+
+
+bool b2r2_is_zero_area_rect(struct b2r2_blt_rect *rect)
+{
+	return rect->width == 0 || rect->height == 0;
+}
+
+bool b2r2_is_rect_inside_rect(struct b2r2_blt_rect *rect1,
+						struct b2r2_blt_rect *rect2)
+{
+	return rect1->x >= rect2->x &&
+		rect1->y >= rect2->y &&
+		rect1->x + rect1->width <= rect2->x + rect2->width &&
+		rect1->y + rect1->height <= rect2->y + rect2->height;
+}
+
+void b2r2_intersect_rects(struct b2r2_blt_rect *rect1,
+	struct b2r2_blt_rect *rect2, struct b2r2_blt_rect *intersection)
+{
+	struct b2r2_blt_rect tmp_rect;
+
+	tmp_rect.x = max(rect1->x, rect2->x);
+	tmp_rect.y = max(rect1->y, rect2->y);
+	tmp_rect.width = min(rect1->x + rect1->width, rect2->x + rect2->width)
+				- tmp_rect.x;
+	if (tmp_rect.width < 0)
+		tmp_rect.width = 0;
+	tmp_rect.height =
+		min(rect1->y + rect1->height, rect2->y + rect2->height) -
+				tmp_rect.y;
+	if (tmp_rect.height < 0)
+		tmp_rect.height = 0;
+
+	*intersection = tmp_rect;
+}
+
+
+int b2r2_get_fmt_bpp(enum b2r2_blt_fmt fmt)
+{
+	/*
+	 * Currently this function is not used that often but if that changes a
+	 * lookup table could make it a lot faster.
+	 */
+	switch (fmt) {
+	case B2R2_BLT_FMT_1_BIT_A1:
+		return 1;
+
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return 8;
+
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+		return 12;
+
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return 16;
+
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return 24;
+
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		return 32;
+
+	default:
+		b2r2_log_err("%s: Internal error! Format %#x not recognized.\n",
+			__func__, fmt);
+		return 32;
+	}
+}
+
+int b2r2_get_fmt_y_bpp(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		return 8;
+
+	default:
+		b2r2_log_err("%s: Internal error! Non YCbCr format supplied.\n",
+			__func__);
+		return 8;
+	}
+}
+
+
+bool b2r2_is_single_plane_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_independent_pixel_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcri_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcrsp_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcrp_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcr420_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcr422_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcr444_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_mb_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+u32 b2r2_calc_pitch_from_width(s32 width, enum b2r2_blt_fmt fmt)
+{
+	if (b2r2_is_single_plane_fmt(fmt)) {
+		return (u32)b2r2_div_round_up(width *
+				b2r2_get_fmt_bpp(fmt), 8);
+	} else if (b2r2_is_ycbcrsp_fmt(fmt) || b2r2_is_ycbcrp_fmt(fmt)) {
+		return (u32)b2r2_div_round_up(width *
+				b2r2_get_fmt_y_bpp(fmt), 8);
+	} else {
+		b2r2_log_err("%s: Internal error! "
+				"Pitchless format supplied.\n",
+				__func__);
+		return 0;
+	}
+}
+
+u32 b2r2_get_img_pitch(struct b2r2_blt_img *img)
+{
+	if (img->pitch != 0)
+		return img->pitch;
+	else
+		return b2r2_calc_pitch_from_width(img->width, img->fmt);
+}
+
+s32 b2r2_get_img_size(struct b2r2_blt_img *img)
+{
+	if (b2r2_is_single_plane_fmt(img->fmt)) {
+		return (s32)b2r2_get_img_pitch(img) * img->height;
+	} else if (b2r2_is_ycbcrsp_fmt(img->fmt) ||
+			b2r2_is_ycbcrp_fmt(img->fmt)) {
+		s32 y_plane_size;
+
+		y_plane_size = (s32)b2r2_get_img_pitch(img) * img->height;
+
+		if (b2r2_is_ycbcr420_fmt(img->fmt)) {
+			return y_plane_size + y_plane_size / 2;
+		} else if (b2r2_is_ycbcr422_fmt(img->fmt)) {
+			return y_plane_size * 2;
+		} else if (b2r2_is_ycbcr444_fmt(img->fmt)) {
+			return y_plane_size * 3;
+		} else {
+			b2r2_log_err("%s: Internal error! "
+					"Format %#x not recognized.\n",
+					__func__, img->fmt);
+			return 0;
+		}
+	} else if (b2r2_is_mb_fmt(img->fmt)) {
+		return (img->width * img->height *
+				b2r2_get_fmt_bpp(img->fmt)) / 8;
+	} else {
+		b2r2_log_err("%s: Internal error! "
+				"Format %#x not recognized.\n",
+				__func__, img->fmt);
+		return 0;
+	}
+}
+
+
+s32 b2r2_div_round_up(s32 dividend, s32 divisor)
+{
+	s32 quotient = dividend / divisor;
+	if (dividend % divisor != 0)
+		quotient++;
+
+	return quotient;
+}
+
+bool b2r2_is_aligned(s32 value, s32 alignment)
+{
+	return value % alignment == 0;
+}
+
+s32 b2r2_align_up(s32 value, s32 alignment)
+{
+	s32 remainder = abs(value) % abs(alignment);
+	s32 value_to_add;
+
+	if (remainder > 0) {
+		if (value >= 0)
+			value_to_add = alignment - remainder;
+		else
+			value_to_add = remainder;
+	} else {
+		value_to_add = 0;
+	}
+
+	return value + value_to_add;
+}
diff --git a/drivers/video/b2r2/b2r2_utils.h b/drivers/video/b2r2/b2r2_utils.h
new file mode 100644
index 00000000000..745d71c0126
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_utils.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 utils
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_DRIVERS_VIDEO_B2R2_UTILS_H_
+#define _LINUX_DRIVERS_VIDEO_B2R2_UTILS_H_
+
+#include <video/b2r2_blt.h>
+
+extern const s32 b2r2_s32_max;
+
+void b2r2_get_img_bounding_rect(struct b2r2_blt_img *img,
+		struct b2r2_blt_rect *bounding_rect);
+
+bool b2r2_is_zero_area_rect(struct b2r2_blt_rect *rect);
+bool b2r2_is_rect_inside_rect(struct b2r2_blt_rect *rect1,
+		struct b2r2_blt_rect *rect2);
+void b2r2_intersect_rects(struct b2r2_blt_rect *rect1,
+		struct b2r2_blt_rect *rect2, struct b2r2_blt_rect *intersection);
+
+int b2r2_get_fmt_bpp(enum b2r2_blt_fmt fmt);
+int b2r2_get_fmt_y_bpp(enum b2r2_blt_fmt fmt);
+
+bool b2r2_is_single_plane_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_independent_pixel_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcri_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcrsp_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcrp_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcr420_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcr422_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcr444_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_mb_fmt(enum b2r2_blt_fmt fmt);
+
+/*
+ * Rounds up if an invalid width causes the pitch to be non byte aligned.
+ */
+u32 b2r2_calc_pitch_from_width(s32 width, enum b2r2_blt_fmt fmt);
+u32 b2r2_get_img_pitch(struct b2r2_blt_img *img);
+s32 b2r2_get_img_size(struct b2r2_blt_img *img);
+
+s32 b2r2_div_round_up(s32 dividend, s32 divisor);
+bool b2r2_is_aligned(s32 value, s32 alignment);
+s32 b2r2_align_up(s32 value, s32 alignment);
+
+#endif
diff --git a/drivers/video/mcde/Kconfig b/drivers/video/mcde/Kconfig
new file mode 100644
index 00000000000..a1310332cf9
--- /dev/null
+++ b/drivers/video/mcde/Kconfig
@@ -0,0 +1,83 @@
+config FB_MCDE
+	tristate "MCDE support"
+	depends on FB
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+	select FB_SYS_FOPS
+	select HWMEM
+	---help---
+	  This enables support for MCDE based frame buffer driver.
+
+	  Please read the file <file:Documentation/fb/mcde.txt>
+
+config MCDE_DISPLAY_GENERIC_DSI
+	tristate "Generic DSI display driver"
+	depends on FB_MCDE
+
+config MCDE_DISPLAY_SONY_SY35560_DSI
+	tristate "Sony sy35560 display driver"
+	depends on FB_MCDE
+
+config MCDE_DISPLAY_DPI
+	bool "Support for DPI displays within MCDE"
+	depends on FB_MCDE
+	default n
+	---help---
+	  Add this option to choose which DPI display driver for MCDE to include
+
+	  DPI (Display Pixel Interface) is a MIPI Alliance standard used for
+	  active-matrix LCDs. The DPI uses parallel data lines.
+
+menu "MCDE DPI displays"
+	depends on MCDE_DISPLAY_DPI
+
+config MCDE_DISPLAY_VUIB500_DPI
+	tristate "DPI display driver for the VUIB500 board"
+	---help---
+	  The VUIB500 is an ST-Ericsson user interface board.
+
+endmenu
+
+config MCDE_DISPLAY_AV8100
+	tristate "AV8100 HDMI/CVBS display driver"
+	depends on FB_MCDE
+	select AV8100
+
+config MCDE_DISPLAY_HDMI_FB_AUTO_CREATE
+	bool "HDMI_FB_AUTO_CREATE"
+	default y
+	depends on MCDE_DISPLAY_AV8100
+	---help---
+	  Say Y if you want the HDMI frame buffer to be created on start
+	  Say N if you want the HDMI frame buffer to be created when HDMI
+	  cable is plugged (needs user space HDMIservice)
+
+config MCDE_DISPLAY_AB8500_DENC
+	tristate "AB8500 CVBS display driver"
+	depends on FB_MCDE
+	select AB8500_DENC
+
+config FB_MCDE_DEBUG
+	bool "MCDE debug messages"
+	depends on FB_MCDE
+	---help---
+	  Say Y here if you want the MCDE driver to output debug messages
+
+config FB_MCDE_VDEBUG
+	bool "MCDE verbose debug messages"
+	depends on FB_MCDE_DEBUG
+	---help---
+	  Say Y here if you want the MCDE driver to output more debug messages
+
+config MCDE_FB_AVOID_REALLOC
+	bool "MCDE early allocate framebuffer"
+	default n
+	depends on FB_MCDE
+	---help---
+	  If you say Y here maximum frame buffer size is allocated and
+	  used for all resolutions. If you say N here, the frame buffer is
+	  reallocated when resolution is changed. This reallocation might
+	  fail because of fragmented memory. Note that this memory will
+	  never be deallocated, while the MCDE framebuffer is used.
+
diff --git a/drivers/video/mcde/Makefile b/drivers/video/mcde/Makefile
new file mode 100644
index 00000000000..7c9324ded3a
--- /dev/null
+++ b/drivers/video/mcde/Makefile
@@ -0,0 +1,17 @@
+
+mcde-objs			:= mcde_mod.o mcde_hw.o mcde_dss.o mcde_display.o mcde_bus.o mcde_fb.o
+obj-$(CONFIG_FB_MCDE)		+= mcde.o
+
+obj-$(CONFIG_MCDE_DISPLAY_GENERIC_DSI)		+= display-generic_dsi.o
+obj-$(CONFIG_MCDE_DISPLAY_SONY_SY35560_DSI)	+= display-sony_sy35560_dsi.o
+obj-$(CONFIG_MCDE_DISPLAY_VUIB500_DPI)		+= display-vuib500-dpi.o
+obj-$(CONFIG_MCDE_DISPLAY_AB8500_DENC)		+= display-ab8500.o
+obj-$(CONFIG_MCDE_DISPLAY_AV8100)		+= display-av8100.o
+obj-$(CONFIG_DISPLAY_FICTIVE)			+= display-fictive.o
+
+ifdef CONFIG_FB_MCDE_DEBUG
+EXTRA_CFLAGS += -DDEBUG
+endif
+ifdef CONFIG_FB_MCDE_VDEBUG
+EXTRA_CFLAGS += -DVERBOSE_DEBUG
+endif
diff --git a/drivers/video/mcde/display-ab8500.c b/drivers/video/mcde/display-ab8500.c
new file mode 100644
index 00000000000..5d7f1bb92a6
--- /dev/null
+++ b/drivers/video/mcde/display-ab8500.c
@@ -0,0 +1,493 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * AB8500 display driver
+ *
+ * Author: Marcel Tunnissen <marcel.tuennissen@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/mfd/ab8500/denc.h>
+#include <video/mcde_display.h>
+#include <video/mcde_display-ab8500.h>
+
+#define AB8500_DISP_TRACE	dev_dbg(&ddev->dev, "%s\n", __func__)
+
+#define SDTV_PIXCLOCK		37037
+
+/*
+ * PAL:
+ * Total nr of active lines:   576
+ * Total nr of blanking lines:  49
+ *                      total: 625
+ */
+#define PAL_HBP			132
+#define PAL_HFP			12
+#define PAL_VBP_FIELD_1		22
+#define PAL_VBP_FIELD_2		23
+#define PAL_VFP_FIELD_1		2
+#define PAL_VFP_FIELD_2		2
+
+/*
+ * NTSC (ITU-R BT.470-5):
+ * Total nr of active lines:   486
+ * Total nr of blanking lines:  39
+ *                      total: 525
+ */
+#define NTSC_ORG_HBP		122
+#define NTSC_ORG_HFP		16
+#define NTSC_ORG_VBP_FIELD_1	16
+#define NTSC_ORG_VBP_FIELD_2	17
+#define NTSC_ORG_VFP_FIELD_1	3
+#define NTSC_ORG_VFP_FIELD_2	3
+
+/*
+ * NTSC (DV variant):
+ * Total nr of active lines:   480
+ * Total nr of blanking lines:  45
+ *                      total: 525
+ */
+#define NTSC_HBP		122
+#define NTSC_HFP		16
+#define NTSC_VBP_FIELD_1	19
+#define NTSC_VBP_FIELD_2	20
+#define NTSC_VFP_FIELD_1	3
+#define NTSC_VFP_FIELD_2	3
+
+struct display_driver_data {
+	struct ab8500_denc_conf denc_conf;
+	struct platform_device *denc_dev;
+	int nr_regulators;
+	struct regulator **regulator;
+};
+
+static int try_video_mode(struct mcde_display_device *ddev,
+				struct mcde_video_mode *video_mode);
+static int set_video_mode(struct mcde_display_device *ddev,
+				struct mcde_video_mode *video_mode);
+static int set_power_mode(struct mcde_display_device *ddev,
+				enum mcde_display_power_mode power_mode);
+static int on_first_update(struct mcde_display_device *ddev);
+static int display_update(struct mcde_display_device *ddev, bool tripple_buf);
+
+static int __devinit ab8500_probe(struct mcde_display_device *ddev)
+{
+	int ret = 0;
+	int i;
+	struct ab8500_display_platform_data *pdata = ddev->dev.platform_data;
+	struct display_driver_data *driver_data;
+
+	AB8500_DISP_TRACE;
+
+	if (pdata == NULL) {
+		dev_err(&ddev->dev, "%s:Platform data missing\n", __func__);
+		return -EINVAL;
+	}
+	if (ddev->port->type != MCDE_PORTTYPE_DPI) {
+		dev_err(&ddev->dev, "%s:Invalid port type %d\n", __func__,
+							ddev->port->type);
+		return -EINVAL;
+	}
+
+	driver_data = (struct display_driver_data *)
+		kzalloc(sizeof(struct display_driver_data), GFP_KERNEL);
+	if (!driver_data) {
+		dev_err(&ddev->dev, "Failed to allocate driver data\n");
+		return -ENOMEM;
+	}
+	driver_data->denc_dev = ab8500_denc_get_device();
+	if (!driver_data->denc_dev) {
+		dev_err(&ddev->dev, "Failed to get DENC device\n");
+		ret = -ENODEV;
+		goto dev_get_failed;
+	}
+
+	driver_data->regulator = kzalloc(pdata->nr_regulators *
+					sizeof(struct regulator *), GFP_KERNEL);
+	if (!driver_data->regulator) {
+		dev_err(&ddev->dev, "Failed to allocate regulator list\n");
+		ret = -ENOMEM;
+		goto reg_alloc_failed;
+	}
+	for (i = 0; i < pdata->nr_regulators; i++) {
+		driver_data->regulator[i] = regulator_get(&ddev->dev,
+						pdata->regulator_id[i]);
+		if (IS_ERR(driver_data->regulator[i])) {
+			ret = PTR_ERR(driver_data->regulator[i]);
+			dev_warn(&ddev->dev, "%s:Failed to get regulator %s\n",
+					__func__, pdata->regulator_id[i]);
+			goto regulator_get_failed;
+		}
+	}
+	driver_data->nr_regulators = pdata->nr_regulators;
+
+	dev_set_drvdata(&ddev->dev, driver_data);
+
+	ddev->try_video_mode = try_video_mode;
+	ddev->set_video_mode = set_video_mode;
+	ddev->set_power_mode = set_power_mode;
+	ddev->on_first_update = on_first_update;
+	ddev->update = display_update;
+	ddev->prepare_for_update = NULL;
+
+	return 0;
+
+regulator_get_failed:
+	for (i--; i >= 0; i--)
+		regulator_put(driver_data->regulator[i]);
+	kfree(driver_data->regulator);
+	driver_data->regulator = NULL;
+reg_alloc_failed:
+	ab8500_denc_put_device(driver_data->denc_dev);
+dev_get_failed:
+	kfree(driver_data);
+	return ret;
+}
+
+static int __devexit ab8500_remove(struct mcde_display_device *ddev)
+{
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+	AB8500_DISP_TRACE;
+
+	ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+
+	if (driver_data->regulator) {
+		int i;
+		for (i = driver_data->nr_regulators - 1; i >= 0; i--)
+			regulator_put(driver_data->regulator[i]);
+		kfree(driver_data->regulator);
+		driver_data->regulator = NULL;
+		driver_data->nr_regulators = 0;
+	}
+	ab8500_denc_put_device(driver_data->denc_dev);
+	kfree(driver_data);
+	return 0;
+}
+
+static int ab8500_resume(struct mcde_display_device *ddev)
+{
+	int ret = 0;
+	AB8500_DISP_TRACE;
+
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s: Failed to resume display\n",
+								__func__);
+
+	return ret;
+}
+
+static int ab8500_suspend(struct mcde_display_device *ddev, pm_message_t state)
+{
+	int ret = 0;
+	AB8500_DISP_TRACE;
+
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s: Failed to suspend display\n",
+								__func__);
+
+	return ret;
+}
+
+
+static struct mcde_display_driver ab8500_driver = {
+	.probe	= ab8500_probe,
+	.remove = ab8500_remove,
+	.suspend = ab8500_suspend,
+	.resume = ab8500_resume,
+	.driver = {
+		.name	= "mcde_tv_ab8500",
+	},
+};
+
+static void print_vmode(struct mcde_video_mode *vmode)
+{
+	pr_debug("resolution: %dx%d\n", vmode->xres, vmode->yres);
+	pr_debug("  pixclock: %d\n",    vmode->pixclock);
+	pr_debug("       hbp: %d\n",    vmode->hbp);
+	pr_debug("       hfp: %d\n",    vmode->hfp);
+	pr_debug("       vbp: %d\n",    vmode->vbp);
+	pr_debug("       vfp: %d\n",    vmode->vfp);
+	pr_debug("interlaced: %s\n", vmode->interlaced ? "true" : "false");
+}
+
+static int try_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	AB8500_DISP_TRACE;
+
+	if (ddev == NULL || video_mode == NULL) {
+		dev_warn(&ddev->dev, "%s:ddev = NULL or video_mode = NULL\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if (video_mode->xres != 720) {
+		dev_warn(&ddev->dev,
+			"%s:Failed to find video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+		return -EINVAL;
+	}
+
+	/* TODO: move this part to MCDE: mcde_dss_try_video_mode? */
+	/* check for PAL */
+	switch (video_mode->yres) {
+	case 576:
+		/* set including SAV/EAV: */
+		video_mode->hbp = PAL_HBP;
+		video_mode->hfp = PAL_HFP;
+		video_mode->vbp = PAL_VBP_FIELD_1 + PAL_VBP_FIELD_2;
+		video_mode->vfp = PAL_VFP_FIELD_1 + PAL_VFP_FIELD_2;
+		video_mode->interlaced = true;
+		video_mode->pixclock = SDTV_PIXCLOCK;
+		break;
+	case 480:
+		/* set including SAV/EAV */
+		video_mode->hbp = NTSC_HBP;
+		video_mode->hfp = NTSC_HFP;
+		video_mode->vbp = NTSC_VBP_FIELD_1 + NTSC_VBP_FIELD_2;
+		video_mode->vfp = NTSC_VFP_FIELD_1 + NTSC_VFP_FIELD_2;
+		video_mode->interlaced = true;
+		video_mode->pixclock = SDTV_PIXCLOCK;
+		break;
+	case 486:
+		/* set including SAV/EAV */
+		video_mode->hbp = NTSC_ORG_HBP;
+		video_mode->hfp = NTSC_ORG_HFP;
+		video_mode->vbp = NTSC_ORG_VBP_FIELD_1 + NTSC_ORG_VBP_FIELD_2;
+		video_mode->vfp = NTSC_ORG_VFP_FIELD_1 + NTSC_ORG_VFP_FIELD_2;
+		video_mode->interlaced = true;
+		video_mode->pixclock = SDTV_PIXCLOCK;
+		break;
+	default:
+		dev_warn(&ddev->dev,
+			"%s:Failed to find video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+		return -EINVAL;
+	}
+
+	print_vmode(video_mode);
+
+	return 0;
+
+}
+
+static int set_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	int res;
+	struct ab8500_display_platform_data *pdata = ddev->dev.platform_data;
+	struct display_driver_data *driver_data =
+		(struct display_driver_data *)dev_get_drvdata(&ddev->dev);
+	AB8500_DISP_TRACE;
+
+	if (ddev == NULL || video_mode == NULL) {
+		dev_warn(&ddev->dev, "%s:ddev = NULL or video_mode = NULL\n",
+			__func__);
+		return -EINVAL;
+	}
+	ddev->video_mode = *video_mode;
+
+	if (video_mode->xres != 720) {
+		dev_warn(&ddev->dev, "%s:Failed to set video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+		return -EINVAL;
+	}
+
+	/* check for PAL BDGHI and N */
+	switch (video_mode->yres) {
+	case 576:
+		driver_data->denc_conf.TV_std = TV_STD_PAL_BDGHI;
+		/* TODO: how to choose LOW DEF FILTER */
+		driver_data->denc_conf.cr_filter = TV_CR_PAL_HIGH_DEF_FILTER;
+		/* TODO: PAL N (e.g. uses a setup of 7.5 IRE) */
+		driver_data->denc_conf.black_level_setup = false;
+		break;
+	case 480: /* NTSC, PAL M DV variant */
+	case 486: /* NTSC, PAL M original   */
+		/* TODO: PAL M */
+		driver_data->denc_conf.TV_std = TV_STD_NTSC_M;
+		/* TODO: how to choose LOW DEF FILTER */
+		driver_data->denc_conf.cr_filter = TV_CR_NTSC_HIGH_DEF_FILTER;
+		driver_data->denc_conf.black_level_setup = true;
+		break;
+	default:
+		dev_warn(&ddev->dev, "%s:Failed to set video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+		return -EINVAL;
+	}
+
+
+	driver_data->denc_conf.progressive	= !video_mode->interlaced;
+	driver_data->denc_conf.act_output	= true;
+	driver_data->denc_conf.test_pattern	= false;
+	driver_data->denc_conf.partial_blanking	= true;
+	driver_data->denc_conf.blank_all	= false;
+	driver_data->denc_conf.suppress_col	= false;
+	driver_data->denc_conf.phase_reset_mode	= TV_PHASE_RST_MOD_DISABLE;
+	driver_data->denc_conf.dac_enable	= false;
+	driver_data->denc_conf.act_dc_output	= true;
+
+	set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (pdata->rgb_2_yCbCr_transform)
+		mcde_chnl_set_col_convert(ddev->chnl_state,
+						pdata->rgb_2_yCbCr_transform,
+						MCDE_CONVERT_RGB_2_YCBCR);
+	mcde_chnl_stop_flow(ddev->chnl_state);
+	res = mcde_chnl_set_video_mode(ddev->chnl_state, &ddev->video_mode);
+	if (res < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to set video mode on channel\n",
+			__func__);
+
+		return res;
+	}
+	ddev->update_flags |= UPDATE_FLAG_VIDEO_MODE;
+
+	return 0;
+}
+
+static int set_power_mode(struct mcde_display_device *ddev,
+	enum mcde_display_power_mode power_mode)
+{
+	int ret;
+	int i;
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+	AB8500_DISP_TRACE;
+
+	/* OFF -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_OFF &&
+					power_mode != MCDE_DISPLAY_PM_OFF) {
+		dev_dbg(&ddev->dev, "off -> standby\n");
+		if (ddev->platform_enable) {
+			ret = ddev->platform_enable(ddev);
+			if (ret)
+				goto error;
+		}
+		if (driver_data->regulator) {
+			for (i = 0; i < driver_data->nr_regulators; i++) {
+				ret = regulator_enable(
+						driver_data->regulator[i]);
+				if (ret)
+					goto off_to_standby_failed;
+				dev_dbg(&ddev->dev, "regulator %d on\n", i);
+			}
+		}
+		ab8500_denc_power_up(driver_data->denc_dev);
+		ab8500_denc_reset(driver_data->denc_dev, true);
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+	/* STANDBY -> ON */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+					power_mode == MCDE_DISPLAY_PM_ON) {
+		dev_dbg(&ddev->dev, "standby -> on\n");
+		ddev->power_mode = MCDE_DISPLAY_PM_ON;
+	}
+	/* ON -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_ON &&
+					power_mode <= MCDE_DISPLAY_PM_STANDBY) {
+		dev_dbg(&ddev->dev, "on -> standby\n");
+		ab8500_denc_reset(driver_data->denc_dev, false);
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+	/* STANDBY -> OFF */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+					power_mode == MCDE_DISPLAY_PM_OFF) {
+		bool error = false;
+		dev_dbg(&ddev->dev, "standby -> off\n");
+		if (driver_data->regulator) {
+			for (i = 0; i < driver_data->nr_regulators; i++) {
+				ret = regulator_disable(
+						driver_data->regulator[i]);
+				/* continue in case of an error */
+				error |= (ret != 0);
+				dev_dbg(&ddev->dev, "regulator %d off\n", i);
+			}
+		}
+		if (ddev->platform_disable) {
+			ret = ddev->platform_disable(ddev);
+			error |= (ret != 0);
+		}
+		if (error) {
+			/* the latest error code is returned */
+			goto error;
+		}
+		memset(&(ddev->video_mode), 0, sizeof(struct mcde_video_mode));
+		ab8500_denc_power_down(driver_data->denc_dev);
+		ddev->power_mode = MCDE_DISPLAY_PM_OFF;
+	}
+
+	return 0;
+
+	/* In case of an error, try to leave in off-state */
+off_to_standby_failed:
+	for (i--; i >= 0; i--)
+		regulator_disable(driver_data->regulator[i]);
+	ddev->platform_disable(ddev);
+
+error:
+	dev_err(&ddev->dev, "Failed to set power mode");
+	return ret;
+}
+
+static int on_first_update(struct mcde_display_device *ddev)
+{
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+
+	ab8500_denc_conf(driver_data->denc_dev, &driver_data->denc_conf);
+	ab8500_denc_conf_plug_detect(driver_data->denc_dev, true, false,
+							TV_PLUG_TIME_2S);
+	ab8500_denc_mask_int_plug_det(driver_data->denc_dev, false, false);
+	ddev->first_update = false;
+	return 0;
+}
+
+static int display_update(struct mcde_display_device *ddev, bool tripple_buffer)
+{
+	int ret;
+
+	if (ddev->first_update)
+		on_first_update(ddev);
+	if (ddev->power_mode != MCDE_DISPLAY_PM_ON && ddev->set_power_mode) {
+		ret = set_power_mode(ddev, MCDE_DISPLAY_PM_ON);
+		if (ret < 0)
+			goto error;
+	}
+	ret = mcde_chnl_update(ddev->chnl_state, &ddev->update_area,
+			tripple_buffer);
+	if (ret < 0)
+		goto error;
+out:
+	return ret;
+error:
+	dev_warn(&ddev->dev, "%s:Failed to set power mode to on\n", __func__);
+	goto out;
+}
+
+/* Module init */
+static int __init mcde_display_tvout_ab8500_init(void)
+{
+	pr_debug("%s\n", __func__);
+
+	return mcde_display_driver_register(&ab8500_driver);
+}
+late_initcall(mcde_display_tvout_ab8500_init);
+
+static void __exit mcde_display_tvout_ab8500_exit(void)
+{
+	pr_debug("%s\n", __func__);
+
+	mcde_display_driver_unregister(&ab8500_driver);
+}
+module_exit(mcde_display_tvout_ab8500_exit);
+
+MODULE_AUTHOR("Marcel Tunnissen <marcel.tuennissen@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE TVout through AB8500 display driver");
diff --git a/drivers/video/mcde/display-av8100.c b/drivers/video/mcde/display-av8100.c
new file mode 100644
index 00000000000..688a4d17688
--- /dev/null
+++ b/drivers/video/mcde/display-av8100.c
@@ -0,0 +1,1483 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson HDMI display driver
+ *
+ * Author: Per Persson <per-xb-persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+
+#include <video/mcde_display.h>
+#include <video/mcde_display-av8100.h>
+#include <video/av8100.h>
+#include <video/hdmi.h>
+
+#define SWITCH_HELPSTR ", 0=HDMI, 1=SDTV, 2=DVI\n"
+
+struct cea_vesa_video_mode {
+	u32 cea;
+	u32 vesa_cea_nr;
+	struct mcde_video_mode *video_mode;
+};
+
+static int hdmi_try_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode);
+static int hdmi_set_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode);
+static int hdmi_set_pixel_format(
+	struct mcde_display_device *ddev, enum mcde_ovly_pix_fmt format);
+static struct mcde_video_mode *video_mode_get(struct mcde_display_device *ddev,
+		u8 cea, u8 vesa_cea_nr);
+
+static ssize_t show_hdmisdtvswitch(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_hdmisdtvswitch(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_input_pixel_format(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_input_pixel_format(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_disponoff(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_disponoff(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_vesacea(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t show_timing(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_timing(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_stayalive(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static DEVICE_ATTR(disponoff, S_IRUGO | S_IWUSR, show_disponoff,
+		store_disponoff);
+static DEVICE_ATTR(vesacea, S_IRUGO, show_vesacea, NULL);
+static DEVICE_ATTR(timing, S_IRUGO | S_IWUSR, show_timing, store_timing);
+static DEVICE_ATTR(stayalive, S_IWUSR, NULL, store_stayalive);
+
+static DEVICE_ATTR(hdmisdtvswitch, S_IRUGO | S_IWUSR, show_hdmisdtvswitch,
+			store_hdmisdtvswitch);
+static DEVICE_ATTR(input_pixel_format, S_IRUGO | S_IWUSR,
+			show_input_pixel_format, store_input_pixel_format);
+
+static ssize_t show_hdmisdtvswitch(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mcde_display_device *mdev = to_mcde_display_device(dev);
+	int index;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	sprintf(buf, "%1x%s", mdev->port->hdmi_sdtv_switch, SWITCH_HELPSTR);
+	index = 1 + strlen(SWITCH_HELPSTR) + 1;
+
+	return index;
+}
+
+static ssize_t store_hdmisdtvswitch(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mcde_display_device *mdev = to_mcde_display_device(dev);
+	dev_dbg(dev, "%s\n", __func__);
+
+	if (count > 0) {
+		if ((*buf == 0) || (*buf == '0')) {
+			dev_dbg(dev, "hdmi/sdtv switch = hdmi\n");
+			mdev->port->hdmi_sdtv_switch = HDMI_SWITCH;
+			mdev->native_x_res = NATIVE_XRES_HDMI;
+			mdev->native_y_res = NATIVE_YRES_HDMI;
+		} else if ((*buf == 1) || (*buf == '1')) {
+			dev_dbg(dev, "hdmi/sdtv switch = sdtv\n");
+			mdev->port->hdmi_sdtv_switch = SDTV_SWITCH;
+			mdev->native_x_res = NATIVE_XRES_SDTV;
+			mdev->native_y_res = NATIVE_YRES_SDTV;
+		} else if ((*buf == 2) || (*buf == '2')) {
+			dev_dbg(dev, "hdmi/sdtv switch = dvi\n");
+			mdev->port->hdmi_sdtv_switch = DVI_SWITCH;
+			mdev->native_x_res = NATIVE_XRES_HDMI;
+			mdev->native_y_res = NATIVE_YRES_HDMI;
+		}
+		/* implicitely read by a memcmp in dss */
+		mdev->video_mode.force_update = true;
+	}
+
+	return count;
+}
+
+static ssize_t show_input_pixel_format(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+
+	return sprintf(buf, "%d\n", ddev->port->pixel_format);
+}
+
+static ssize_t store_input_pixel_format(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+
+	dev_dbg(dev, "%s\n", __func__);
+	if (count > 0) {
+		unsigned long input;
+		if (strict_strtoul(buf, 10, &input) != 0)
+			return -EINVAL;
+		switch (input) {
+		/* intentional fall through */
+		case MCDE_PORTPIXFMT_DSI_16BPP:
+		case MCDE_PORTPIXFMT_DSI_18BPP:
+		case MCDE_PORTPIXFMT_DSI_18BPP_PACKED:
+		case MCDE_PORTPIXFMT_DSI_24BPP:
+		case MCDE_PORTPIXFMT_DSI_YCBCR422:
+			ddev->port->pixel_format = input;
+			break;
+		default:
+			dev_warn(&ddev->dev, "invalid format (%ld)\n",
+								input);
+			return -EINVAL;
+			break;
+		}
+		/* implicitely read by a memcmp in dss */
+		ddev->video_mode.force_update = true;
+		driver_data->update_port_pixel_format = true;
+	}
+
+	return count;
+}
+
+static ssize_t show_disponoff(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	if (ddev->fbi && driver_data->fbdevname) {
+		dev_dbg(dev, "name:%s\n", driver_data->fbdevname);
+		strcpy(buf, driver_data->fbdevname);
+		return strlen(driver_data->fbdevname) + 1;
+	}
+	return 0;
+}
+
+static ssize_t store_disponoff(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mcde_display_device *mdev = to_mcde_display_device(dev);
+	bool enable = false;
+	u8 cea = 0;
+	u8 vesa_cea_nr = 0;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	if ((count != DISPONOFF_SIZE) && (count != DISPONOFF_SIZE + 1))
+		return -EINVAL;
+
+	if ((*buf == '0') && (*(buf + 1) == '1'))
+		enable = true;
+	cea = (hex_to_bin(buf[2]) << 4) + hex_to_bin(buf[3]);
+	vesa_cea_nr = (hex_to_bin(buf[4]) << 4) + hex_to_bin(buf[5]);
+	dev_dbg(dev, "enable:%d cea:%d nr:%d\n", enable, cea, vesa_cea_nr);
+
+	hdmi_fb_onoff(mdev, enable, cea, vesa_cea_nr);
+
+	return count;
+}
+
+static ssize_t show_timing(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+	struct mcde_video_mode *video_mode;
+	int index;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	index = 0;
+	if (driver_data->video_mode) {
+		video_mode = driver_data->video_mode;
+		memcpy(buf + index, &video_mode->xres, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->yres, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->pixclock, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->hbp, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->hfp, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->vbp, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->vfp, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->interlaced, sizeof(u32));
+		index += sizeof(u32);
+	}
+	return index;
+}
+
+static ssize_t store_timing(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	if (count != TIMING_SIZE)
+		return -EINVAL;
+
+	driver_data->video_mode = video_mode_get(ddev, *buf, *(buf + 1));
+
+	return count;
+}
+
+static ssize_t store_stayalive(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+
+	if (count != STAYALIVE_SIZE)
+		return -EINVAL;
+
+	if ((*buf == 1) || (*buf == '1'))
+		ddev->stay_alive = true;
+	else
+		ddev->stay_alive = false;
+
+	dev_dbg(dev, "%s %d\n", __func__, ddev->stay_alive);
+
+	return count;
+}
+
+static int ceanr_convert(struct mcde_display_device *ddev,
+		u8 cea, u8 vesa_cea_nr, int buffering,
+		u16 *w, u16 *h, u16 *vw, u16 *vh)
+{
+	struct mcde_video_mode *video_mode;
+
+	dev_dbg(&ddev->dev, "%s\n", __func__);
+	video_mode = video_mode_get(ddev, cea, vesa_cea_nr);
+	if (video_mode) {
+		*w = video_mode->xres;
+		*h = video_mode->yres;
+		*vw = video_mode->xres;
+		*vh = video_mode->yres * buffering;
+		dev_dbg(&ddev->dev, "cea:%d nr:%d found\n",
+				cea, vesa_cea_nr);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+/* Supported HDMI modes */
+static struct mcde_video_mode video_modes_supp_hdmi[] = {
+	/* 0 CEA #1 640_480_60_P */
+	{
+		.xres = 640,	.yres = 480,
+		.pixclock = 39682,
+		.hbp = 112,	.hfp = 48,
+		.vbp = 33,	.vfp = 12
+	},
+	/* 1 720_480_60_P */
+	{
+		.xres = 720,	.yres = 480,
+		.pixclock = 37000,
+		.hbp = 104,	.hfp = 34,
+		.vbp = 30,	.vfp = 15
+	},
+	/* 2 720_576_50_P */
+	{
+		.xres = 720,	.yres = 576,
+		.pixclock = 37037,
+		.hbp = 132,	.hfp = 12,
+		.vbp = 44,	.vfp = 5
+	},
+	/* 3 1280_720_60_P */
+	{
+		.xres = 1280,	.yres = 720,
+		.pixclock = 13468,
+		.hbp = 256,	.hfp = 114,
+		.vbp = 20,	.vfp = 10
+	},
+	/* 4 1280_720_50_P */
+	{
+		.xres = 1280,	.yres = 720,
+		.pixclock = 13468,
+		.hbp = 260,	.hfp = 440,
+		.vbp = 25,	.vfp = 5
+	},
+	/* 5 1280_720_30_P */
+	{
+		.xres = 1280,	.yres = 720,
+		.pixclock = 13468,
+		.hbp = 260,	.hfp = 1760,
+		.vbp = 20,	.vfp = 10
+	},
+	/* 6 1280_720_24_P */
+	{
+		.xres = 1280,	.yres = 720,
+		.pixclock = 16835,
+		.hbp = 260,	.hfp = 1760,
+		.vbp = 20,	.vfp = 10
+	},
+	/* 7 1280_720_25_P */
+	{
+		.xres = 1280,	.yres = 720,
+		.pixclock = 13468,
+		.hbp = 260,	.hfp = 2420,
+		.vbp = 20,	.vfp = 10
+	},
+	/* 8 1920_1080_30_P */
+	{
+		.xres = 1920,	.yres = 1080,
+		.pixclock = 13468,
+		.hbp = 189,	.hfp = 91,
+		.vbp = 36,	.vfp = 9
+	},
+	/* 9 1920_1080_24_P */
+	{
+		.xres = 1920,	.yres = 1080,
+		.pixclock = 13468,
+		.hbp = 170,	.hfp = 660,
+		.vbp = 36,	.vfp = 9
+	},
+	/* 10 1920_1080_25_P */
+	{
+		.xres = 1920,	.yres = 1080,
+		.pixclock = 13468,
+		.hbp = 192,	.hfp = 528,
+		.vbp = 36,	.vfp = 9
+	},
+	/* 11 720_480_60_I */
+	{
+		.xres = 720,	.yres = 480,
+		.pixclock = 74074,
+		.hbp = 126,	.hfp = 12,
+		.vbp = 44,	.vfp = 1,
+		.interlaced = true,
+	},
+	/* 12 720_576_50_I */
+	{
+		.xres = 720,	.yres = 576,
+		.pixclock = 74074,
+		.hbp = 132,	.hfp = 12,
+		.vbp = 44,	.vfp = 5,
+		.interlaced = true,
+	},
+	/* 13 1920_1080_50_I */
+	{
+		.xres = 1920,	.yres = 1080,
+		.pixclock = 13468,
+		.hbp = 192,	.hfp = 528,
+		.vbp = 20,	.vfp = 25,
+		.interlaced = true,
+	},
+	/* 14 1920_1080_60_I */
+	{
+		.xres = 1920,	.yres = 1080,
+		.pixclock = 13468,
+		.hbp = 192,	.hfp = 88,
+		.vbp = 20,	.vfp = 25,
+		.interlaced = true,
+	},
+	/* 15 VESA #9 800_600_60_P */
+	{
+		.xres = 800,	.yres = 600,
+		.pixclock = 25000,
+		.hbp = 168,	.hfp = 88,
+		.vbp = 23,	.vfp = 5,
+		.interlaced = false,
+	},
+	/* 16 VESA #14 848_480_60_P */
+	{
+		.xres = 848,	.yres = 480,
+		.pixclock = 29630,
+		.hbp = 128,	.hfp = 112,
+		.vbp = 23,	.vfp = 14,
+		.interlaced = false,
+	},
+	/* 17 VESA #16 1024_768_60_P */
+	{
+		.xres = 1024,	.yres = 768,
+		.pixclock = 15385,
+		.hbp = 160,	.hfp = 160,
+		.vbp = 29,	.vfp = 9,
+		.interlaced = false,
+	},
+	/* 18 VESA #22 1280_768_60_P */
+	{
+		.xres = 1280,	.yres = 768,
+		.pixclock = 14652,
+		.hbp = 80,	.hfp = 80,
+		.vbp = 12,	.vfp = 10,
+		.interlaced = false,
+	},
+	/* 19 VESA #23 1280_768_60_P */
+	{
+		.xres = 1280,	.yres = 768,
+		.pixclock = 12579,
+		.hbp = 192,	.hfp = 192,
+		.vbp = 20,	.vfp = 10,
+		.interlaced = false,
+	},
+	/* 20 VESA #27 1280_800_60_P */
+	{
+		.xres = 1280,	.yres = 800,
+		.pixclock = 14085,
+		.hbp = 80,	.hfp = 80,
+		.vbp = 14,	.vfp = 9,
+		.interlaced = false,
+	},
+	/* 21 VESA #28 1280_800_60_P */
+	{
+		.xres = 1280,	.yres = 800,
+		.pixclock = 11976,
+		.hbp = 200,	.hfp = 200,
+		.vbp = 22,	.vfp = 9,
+		.interlaced = false,
+	},
+	/* 22 VESA #39 1360_768_60_P */
+	{
+		.xres = 1360,	.yres = 768,
+		.pixclock = 11696,
+		.hbp = 176,	.hfp = 256,
+		.vbp = 18,	.vfp = 9,
+		.interlaced = false,
+	},
+	/* 23 VESA #81 1366_768_60_P */
+	{
+		.xres = 1366,	.yres = 768,
+		.pixclock = 11662,
+		.hbp = 213,	.hfp = 213,
+		.vbp = 24,	.vfp = 6,
+		.interlaced = false,
+	},
+};
+
+/* Supported TVout modes */
+static struct mcde_video_mode video_modes_supp_sdtv[] = {
+	/* 720_480_60_I) */
+	{
+		.xres = 720,	.yres = 480,
+		.pixclock = 74074,
+		.hbp = 126,	.hfp = 12,
+		.vbp = 44,	.vfp = 1,
+		.interlaced = true,
+	},
+	/* 720_576_50_I) */
+	{
+		.xres = 720,	.yres = 576,
+		.pixclock = 74074,
+		.hbp = 132,	.hfp = 12,
+		.vbp = 44,	.vfp = 5,
+		.interlaced = true,
+	},
+};
+
+static struct cea_vesa_video_mode cea_vesa_video_mode[] = {
+		/* 640_480_60_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 1,
+			.video_mode = &video_modes_supp_hdmi[0],
+		},
+		/* 720_480_60_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 2,
+			.video_mode = &video_modes_supp_hdmi[1],
+		},
+		/* 720_480_60_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 3,
+			.video_mode = &video_modes_supp_hdmi[1],
+		},
+		/* 720_576_50_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 17,
+			.video_mode = &video_modes_supp_hdmi[2],
+		},
+		/* 720_576_50_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 18,
+			.video_mode = &video_modes_supp_hdmi[2],
+		},
+		/* 1280_720_60_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 4,
+			.video_mode = &video_modes_supp_hdmi[3],
+		},
+		/* 1280_720_50_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 19,
+			.video_mode = &video_modes_supp_hdmi[4],
+		},
+		/* 1280_720_30_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 62,
+			.video_mode = &video_modes_supp_hdmi[5],
+		},
+		/* 1280_720_24_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 60,
+			.video_mode = &video_modes_supp_hdmi[6],
+		},
+		/* 1280_720_25_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 61,
+			.video_mode = &video_modes_supp_hdmi[7],
+		},
+		/* 1920_1080_30_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 34,
+			.video_mode = &video_modes_supp_hdmi[8],
+		},
+		/* 1920_1080_24_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 32,
+			.video_mode = &video_modes_supp_hdmi[9],
+		},
+		/* 1920_1080_25_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 33,
+			.video_mode = &video_modes_supp_hdmi[10],
+		},
+		/* 720_480_60_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 6,
+			.video_mode = &video_modes_supp_hdmi[11],
+		},
+		/* 720_480_60_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 7,
+			.video_mode = &video_modes_supp_hdmi[11],
+		},
+		/* 720_576_50_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 21,
+			.video_mode = &video_modes_supp_hdmi[12],
+		},
+		/* 720_576_50_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 22,
+			.video_mode = &video_modes_supp_hdmi[12],
+		},
+		/* 1920_1080_50_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 20,
+			.video_mode = &video_modes_supp_hdmi[13],
+		},
+		/* 1920_1080_60_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 5,
+			.video_mode = &video_modes_supp_hdmi[14],
+		},
+		/* VESA #4 640_480_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 4,
+			.video_mode = &video_modes_supp_hdmi[0],
+		},
+		/* VESA #9 800_600_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 9,
+			.video_mode = &video_modes_supp_hdmi[15],
+		},
+		/* VESA #14 848_480_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 14,
+			.video_mode = &video_modes_supp_hdmi[16],
+		},
+		/* VESA #16 1024_768_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 16,
+			.video_mode = &video_modes_supp_hdmi[17],
+		},
+		/* VESA #22 1280_768_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 22,
+			.video_mode = &video_modes_supp_hdmi[18],
+		},
+		/* VESA #23 1280_768_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 23,
+			.video_mode = &video_modes_supp_hdmi[19],
+		},
+		/* VESA #27 1280_800_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 27,
+			.video_mode = &video_modes_supp_hdmi[20],
+		},
+		/* VESA #28 1280_800_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 28,
+			.video_mode = &video_modes_supp_hdmi[21],
+		},
+		/* VESA #39 1360_768_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 39,
+			.video_mode = &video_modes_supp_hdmi[22],
+		},
+		/* VESA #81 1366_768_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 81,
+			.video_mode = &video_modes_supp_hdmi[23],
+		},
+};
+
+static ssize_t show_vesacea(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int findex;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	for (findex = 0; findex < ARRAY_SIZE(cea_vesa_video_mode); findex++) {
+		*(buf + findex * 2) = cea_vesa_video_mode[findex].cea;
+		*(buf + findex * 2 + 1) =
+				cea_vesa_video_mode[findex].vesa_cea_nr;
+	}
+	*(buf + findex * 2) = '\0';
+
+	return findex * 2 + 1;
+}
+
+static struct mcde_video_mode *video_mode_get(struct mcde_display_device *ddev,
+		u8 cea, u8 vesa_cea_nr)
+{
+	int findex;
+
+	dev_dbg(&ddev->dev, "%s\n", __func__);
+
+	for (findex = 0; findex < ARRAY_SIZE(cea_vesa_video_mode); findex++)
+		if ((cea == cea_vesa_video_mode[findex].cea) &&
+				(vesa_cea_nr ==
+				cea_vesa_video_mode[findex].vesa_cea_nr)) {
+			dev_dbg(&ddev->dev, "cea:%d nr:%d\n", cea, vesa_cea_nr);
+			return cea_vesa_video_mode[findex].video_mode;
+		}
+
+	return NULL;
+}
+
+#define AV8100_MAX_LEVEL 255
+
+static int hdmi_try_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	int index = 0;
+	int match_level = AV8100_MAX_LEVEL;
+	int found_index = -1;
+	struct mcde_video_mode *video_modes_supp;
+	int array_size;
+
+	if (ddev == NULL || video_mode == NULL) {
+		pr_warning("%s:ddev = NULL or video_mode = NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	dev_vdbg(&ddev->dev, "%s\n", __func__);
+
+	if (ddev->port->hdmi_sdtv_switch == SDTV_SWITCH) {
+		video_mode->interlaced = true;
+		video_modes_supp = video_modes_supp_sdtv;
+		array_size = ARRAY_SIZE(video_modes_supp_sdtv);
+	} else {
+		video_modes_supp = video_modes_supp_hdmi;
+		array_size = ARRAY_SIZE(video_modes_supp_hdmi);
+	}
+
+	while (index < array_size) {
+		/* 1. Check if all parameters match */
+		if ((video_mode->xres == video_modes_supp[index].xres) &&
+			(video_mode->yres == video_modes_supp[index].yres) &&
+			((video_mode->xres + video_mode->hbp +
+				video_mode->hfp) ==
+			(video_modes_supp[index].xres +
+				video_modes_supp[index].hbp +
+				video_modes_supp[index].hfp)) &&
+			((video_mode->yres + video_mode->vbp + video_mode->vfp)
+				==
+			(video_modes_supp[index].yres +
+				video_modes_supp[index].vbp +
+				video_modes_supp[index].vfp)) &&
+			(video_mode->pixclock ==
+				video_modes_supp[index].pixclock) &&
+			(video_mode->interlaced ==
+				video_modes_supp[index].interlaced)) {
+					match_level = 1;
+					found_index = index;
+					break;
+		}
+
+		/* 2. Check if xres,yres,htot,vtot,interlaced match */
+		if ((match_level > 2) &&
+			(video_mode->xres == video_modes_supp[index].xres) &&
+			(video_mode->yres == video_modes_supp[index].yres) &&
+			((video_mode->xres + video_mode->hbp +
+				video_mode->hfp) ==
+			(video_modes_supp[index].xres +
+				video_modes_supp[index].hbp +
+				video_modes_supp[index].hfp)) &&
+			((video_mode->yres + video_mode->vbp + video_mode->vfp)
+				==
+			(video_modes_supp[index].yres +
+				video_modes_supp[index].vbp +
+				video_modes_supp[index].vfp)) &&
+			(video_mode->interlaced ==
+				video_modes_supp[index].interlaced)) {
+			match_level = 2;
+			found_index = index;
+		}
+
+		/* 3. Check if xres,yres,pixelclock,interlaced match */
+		if ((match_level > 3) &&
+			(video_mode->xres == video_modes_supp[index].xres) &&
+			(video_mode->yres == video_modes_supp[index].yres) &&
+			(video_mode->interlaced ==
+				video_modes_supp[index].interlaced) &&
+			(video_mode->pixclock ==
+				video_modes_supp[index].pixclock)) {
+			match_level = 3;
+			found_index = index;
+		}
+
+		/* 4. Check if xres,yres,interlaced match */
+		if ((match_level > 4) &&
+			(video_mode->xres == video_modes_supp[index].xres) &&
+			(video_mode->yres == video_modes_supp[index].yres) &&
+			(video_mode->interlaced ==
+				video_modes_supp[index].interlaced)) {
+			match_level = 4;
+			found_index = index;
+		}
+
+		index++;
+	}
+
+	if (found_index == -1) {
+		dev_dbg(&ddev->dev, "video_mode not accepted\n");
+		dev_dbg(&ddev->dev, "xres:%d yres:%d pixclock:%d hbp:%d hfp:%d "
+			"vfp:%d vbp:%d intlcd:%d\n",
+			video_mode->xres, video_mode->yres,
+			video_mode->pixclock,
+			video_mode->hbp, video_mode->hfp,
+			video_mode->vfp, video_mode->vbp,
+			video_mode->interlaced);
+		return -EINVAL;
+	}
+
+	memset(video_mode, 0, sizeof(struct mcde_video_mode));
+	memcpy(video_mode, &video_modes_supp[found_index],
+		sizeof(struct mcde_video_mode));
+
+	dev_dbg(&ddev->dev, "%s:HDMI video_mode %d chosen. Level:%d\n",
+		__func__, found_index, match_level);
+
+	return 0;
+}
+
+static int hdmi_set_video_mode(
+	struct mcde_display_device *dev, struct mcde_video_mode *video_mode)
+{
+	int ret;
+	union av8100_configuration av8100_config;
+	struct mcde_display_hdmi_platform_data *pdata = dev->dev.platform_data;
+	struct display_driver_data *driver_data = dev_get_drvdata(&dev->dev);
+	struct av8100_status status;
+
+	/* TODO check video_mode_params */
+	if (dev == NULL || video_mode == NULL) {
+		pr_warning("%s:ddev = NULL or video_mode = NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	dev_dbg(&dev->dev, "%s:\n", __func__);
+	dev_vdbg(&dev->dev, "%s:xres:%d yres:%d hbp:%d hfp:%d vbp:%d vfp:%d "
+		"interlaced:%d\n", __func__,
+		video_mode->xres,
+		video_mode->yres,
+		video_mode->hbp,
+		video_mode->hfp,
+		video_mode->vbp,
+		video_mode->vfp,
+		video_mode->interlaced);
+
+	if (driver_data->update_port_pixel_format) {
+		hdmi_set_pixel_format(dev, dev->pixel_format);
+		driver_data->update_port_pixel_format = false;
+	}
+
+	memset(&(dev->video_mode), 0, sizeof(struct mcde_video_mode));
+	memcpy(&(dev->video_mode), video_mode, sizeof(struct mcde_video_mode));
+
+	if (dev->port->pixel_format == MCDE_PORTPIXFMT_DSI_YCBCR422 &&
+						pdata->rgb_2_yCbCr_transform)
+		mcde_chnl_set_col_convert(dev->chnl_state,
+						pdata->rgb_2_yCbCr_transform,
+						MCDE_CONVERT_RGB_2_YCBCR);
+	mcde_chnl_stop_flow(dev->chnl_state);
+
+	ret = mcde_chnl_set_video_mode(dev->chnl_state, &dev->video_mode);
+	if (ret < 0) {
+		dev_warn(&dev->dev, "Failed to set video mode\n");
+		return ret;
+	}
+
+	status = av8100_status_get();
+	if (status.av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	if (av8100_ver_get() == AV8100_CHIPVER_1) {
+		if (status.av8100_state >= AV8100_OPMODE_STANDBY) {
+			/* Disable interrupts */
+			ret = av8100_disable_interrupt();
+			if (ret) {
+				dev_err(&dev->dev,
+					"%s:av8100_disable_interrupt failed\n",
+					__func__);
+				return ret;
+			}
+
+			ret = av8100_powerdown();
+			if (ret) {
+				dev_err(&dev->dev,
+						"av8100_powerdown failed\n");
+				return ret;
+			}
+
+			msleep(10);
+		}
+	}
+
+	/* Set to powerup with interrupts disabled */
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		ret = av8100_powerup();
+		if (ret) {
+			dev_err(&dev->dev, "av8100_powerup failed\n");
+			return ret;
+		}
+	}
+
+	if (status.av8100_state < AV8100_OPMODE_IDLE) {
+		ret = av8100_download_firmware(NULL, 0, I2C_INTERFACE);
+		if (ret) {
+			dev_err(&dev->dev, "av8100_download_firmware failed\n");
+			av8100_powerdown();
+			return ret;
+		}
+	}
+
+	if (av8100_disable_interrupt())
+		return -EFAULT;
+
+	/*
+	 * Don't look at dev->port->hdmi_sdtv_switch; it states only which
+	 * one should be started, not which one is currently working
+	 */
+	if (av8100_conf_get(AV8100_COMMAND_HDMI, &av8100_config))
+		return -EFAULT;
+	if (av8100_config.hdmi_format.hdmi_mode == AV8100_HDMI_ON) {
+		/* Set HDMI mode to OFF */
+		av8100_config.hdmi_format.hdmi_mode = AV8100_HDMI_OFF;
+		av8100_config.hdmi_format.dvi_format = AV8100_DVI_CTRL_CTL0;
+		av8100_config.hdmi_format.hdmi_format = AV8100_HDMI;
+		if (av8100_conf_prep(AV8100_COMMAND_HDMI, &av8100_config))
+			return -EFAULT;
+
+		if (av8100_conf_w(AV8100_COMMAND_HDMI, NULL, NULL,
+								I2C_INTERFACE))
+			return -EFAULT;
+	}
+	if (av8100_conf_get(AV8100_COMMAND_DENC, &av8100_config))
+		return -EFAULT;
+	if (av8100_config.denc_format.enable) {
+		/* Turn off DENC */
+		av8100_config.denc_format.enable = 0;
+		if (av8100_conf_prep(AV8100_COMMAND_DENC, &av8100_config))
+			return -EFAULT;
+		if (av8100_conf_w(AV8100_COMMAND_DENC, NULL, NULL,
+								I2C_INTERFACE))
+			return -EFAULT;
+	}
+
+	/* Get current av8100 video output format */
+	ret = av8100_conf_get(AV8100_COMMAND_VIDEO_OUTPUT_FORMAT,
+		&av8100_config);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_get "
+			"AV8100_COMMAND_VIDEO_OUTPUT_FORMAT failed\n",
+			__func__);
+		return ret;
+	}
+
+	if (dev->port->hdmi_sdtv_switch == SDTV_SWITCH)
+		av8100_config.video_output_format.video_output_cea_vesa =
+			dev->video_mode.yres == NATIVE_YRES_SDTV ?
+				AV8100_CEA21_22_576I_PAL_50HZ :
+				AV8100_CEA6_7_NTSC_60HZ;
+	else
+		av8100_config.video_output_format.video_output_cea_vesa =
+			av8100_video_output_format_get(
+				dev->video_mode.xres,
+				dev->video_mode.yres,
+				dev->video_mode.xres +
+				dev->video_mode.hbp + dev->video_mode.hfp,
+				dev->video_mode.yres +
+				dev->video_mode.vbp + dev->video_mode.vfp,
+				dev->video_mode.pixclock,
+				dev->video_mode.interlaced);
+
+	if (AV8100_VIDEO_OUTPUT_CEA_VESA_MAX ==
+		av8100_config.video_output_format.video_output_cea_vesa) {
+		dev_err(&dev->dev, "%s:video output format not found "
+			"\n", __func__);
+		return ret;
+	}
+
+	ret = av8100_conf_prep(AV8100_COMMAND_VIDEO_OUTPUT_FORMAT,
+		&av8100_config);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_prep "
+			"AV8100_COMMAND_VIDEO_OUTPUT_FORMAT failed\n",
+			__func__);
+		return ret;
+	}
+
+	/* Get current av8100 video input format */
+	ret = av8100_conf_get(AV8100_COMMAND_VIDEO_INPUT_FORMAT,
+		&av8100_config);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_get "
+			"AV8100_COMMAND_VIDEO_INPUT_FORMAT failed\n",
+			__func__);
+		return ret;
+	}
+
+	/* Set correct av8100 video input pixel format */
+	switch (dev->port->pixel_format) {
+	case MCDE_PORTPIXFMT_DSI_16BPP:
+	default:
+		av8100_config.video_input_format.input_pixel_format =
+			AV8100_INPUT_PIX_RGB565;
+		break;
+	case MCDE_PORTPIXFMT_DSI_18BPP:
+		av8100_config.video_input_format.input_pixel_format =
+			AV8100_INPUT_PIX_RGB666;
+		break;
+	case MCDE_PORTPIXFMT_DSI_18BPP_PACKED:
+		av8100_config.video_input_format.input_pixel_format =
+			AV8100_INPUT_PIX_RGB666P;
+		break;
+	case MCDE_PORTPIXFMT_DSI_24BPP:
+		av8100_config.video_input_format.input_pixel_format =
+			AV8100_INPUT_PIX_RGB888;
+		break;
+	case MCDE_PORTPIXFMT_DSI_YCBCR422:
+		av8100_config.video_input_format.input_pixel_format =
+			AV8100_INPUT_PIX_YCBCR422;
+		break;
+	}
+
+	/*  Set ui_x4 */
+	av8100_config.video_input_format.ui_x4 = dev->port->phy.dsi.ui;
+
+	/* Set TE_config */
+	switch (dev->port->sync_src) {
+	case MCDE_SYNCSRC_TE0:
+		av8100_config.video_input_format.TE_config = AV8100_TE_IT_LINE;
+		break;
+	case MCDE_SYNCSRC_TE1:
+		av8100_config.video_input_format.TE_config = AV8100_TE_GPIO_IT;
+		break;
+	case MCDE_SYNCSRC_TE_POLLING:
+		av8100_config.video_input_format.TE_config =
+			AV8100_TE_DSI_LANE; /* Only on DSI, no interrupts */
+		break;
+	case MCDE_SYNCSRC_OFF:
+	default:
+		av8100_config.video_input_format.TE_config = AV8100_TE_OFF;
+		break;
+	}
+
+	ret = av8100_conf_prep(AV8100_COMMAND_VIDEO_INPUT_FORMAT,
+		&av8100_config);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_prep "
+				"AV8100_COMMAND_VIDEO_INPUT_FORMAT failed\n",
+				__func__);
+		return ret;
+	}
+
+	ret = av8100_conf_w(AV8100_COMMAND_VIDEO_INPUT_FORMAT,
+		NULL, NULL, I2C_INTERFACE);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_w "
+				"AV8100_COMMAND_VIDEO_INPUT_FORMAT failed\n",
+				__func__);
+		return ret;
+	}
+
+	if (dev->port->hdmi_sdtv_switch == SDTV_SWITCH) {
+		if (dev->port->pixel_format != MCDE_PORTPIXFMT_DSI_YCBCR422)
+			av8100_config.color_transform =
+					AV8100_COLOR_TRANSFORM_RGB_TO_DENC;
+		else
+			av8100_config.color_transform =
+					AV8100_COLOR_TRANSFORM_YUV_TO_DENC;
+	} else if (dev->port->pixel_format == MCDE_PORTPIXFMT_DSI_YCBCR422) {
+		av8100_config.color_transform =
+					AV8100_COLOR_TRANSFORM_YUV_TO_RGB;
+	} else {
+		av8100_config.color_transform =
+					AV8100_COLOR_TRANSFORM_INDENTITY;
+	}
+
+	ret = av8100_conf_prep(
+		AV8100_COMMAND_COLORSPACECONVERSION,
+		&av8100_config);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_configuration_prepare "
+			"AV8100_COMMAND_COLORSPACECONVERSION failed\n",
+			__func__);
+		return ret;
+	}
+
+	ret = av8100_conf_w(
+			AV8100_COMMAND_COLORSPACECONVERSION,
+			NULL, NULL, I2C_INTERFACE);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_w "
+			"AV8100_COMMAND_COLORSPACECONVERSION failed\n",
+			__func__);
+		return ret;
+	}
+
+	/* Set video output format */
+	ret = av8100_conf_w(AV8100_COMMAND_VIDEO_OUTPUT_FORMAT,
+		NULL, NULL, I2C_INTERFACE);
+	if (ret) {
+		dev_err(&dev->dev, "av8100_conf_w failed\n");
+		return ret;
+	}
+
+	/* Set audio input format */
+	ret = av8100_conf_w(AV8100_COMMAND_AUDIO_INPUT_FORMAT,
+		NULL, NULL, I2C_INTERFACE);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_w "
+				"AV8100_COMMAND_AUDIO_INPUT_FORMAT failed\n",
+			__func__);
+		return ret;
+	}
+
+	dev->update_flags |= UPDATE_FLAG_VIDEO_MODE;
+	dev->first_update = true;
+
+	return 0;
+}
+
+static u16 rotate_byte_left(u8 c, int nr)
+{
+	return (0xff & (c << nr)) | (0xff & (c >> (8 - nr)));
+}
+
+static u16 map_yv(u8 in)
+{
+	return rotate_byte_left(in, 3) << 4;
+}
+
+static u16 map_u(u8 in)
+{
+	return rotate_byte_left(in, 5) << 4;
+}
+
+static int hdmi_set_pixel_format(
+	struct mcde_display_device *ddev, enum mcde_ovly_pix_fmt format)
+{
+	dev_dbg(&ddev->dev, "%s\n", __func__);
+	ddev->pixel_format = format;
+
+	return 0;
+}
+
+static int hdmi_set_port_pixel_format(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	dev_dbg(&ddev->dev, "%s\n", __func__);
+	mcde_chnl_stop_flow(ddev->chnl_state);
+	ret = mcde_chnl_set_pixel_format(ddev->chnl_state,
+						ddev->port->pixel_format);
+
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s: Failed to set pixel format = %d\n",
+					__func__, ddev->port->pixel_format);
+		return ret;
+	}
+
+	if (ddev->port->pixel_format == MCDE_PORTPIXFMT_DSI_YCBCR422 &&
+						av8100_ver_get() == 2) {
+		/* The V2 version has an error for unpacking YUV422 */
+		struct mcde_palette_table palette = {
+			.map_col_ch0 = *map_yv,
+			.map_col_ch1 = *map_u,
+			.map_col_ch2 = *map_yv,
+		};
+		ret = mcde_chnl_set_palette(ddev->chnl_state, &palette);
+	} else {
+		ret = mcde_chnl_set_palette(ddev->chnl_state, NULL);
+	}
+
+	return 0;
+}
+
+static int hdmi_apply_config(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	if (!ddev->update_flags)
+		return 0;
+
+	ret = mcde_chnl_apply(ddev->chnl_state);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to apply to channel\n",
+							__func__);
+		return ret;
+	}
+	ddev->update_flags = 0;
+
+	return 0;
+}
+
+static int hdmi_on_first_update(struct mcde_display_device *dev)
+{
+	int ret;
+	union av8100_configuration av8100_config;
+
+	dev->first_update = false;
+
+	/*
+	 * Prepare HDMI configuration
+	 * Avoid simultaneous output of DENC and HDMI/DVI.
+	 * Only one of them should be enabled.
+	 * Note HDMI/DVI and DENC are always turned off in set_video_mode.
+	 */
+	switch (dev->port->hdmi_sdtv_switch) {
+	case SDTV_SWITCH:
+		if (av8100_conf_get(AV8100_COMMAND_DENC, &av8100_config))
+			return -EFAULT;
+		av8100_config.denc_format.enable = 1;
+		if (dev->video_mode.yres == NATIVE_YRES_SDTV) {
+			av8100_config.denc_format.standard_selection =
+					AV8100_PAL_BDGHI;
+			av8100_config.denc_format.cvbs_video_format  =
+					AV8100_CVBS_625;
+		} else {
+			av8100_config.denc_format.standard_selection =
+					AV8100_NTSC_M;
+			av8100_config.denc_format.cvbs_video_format  =
+					AV8100_CVBS_525;
+		}
+		ret = av8100_conf_prep(AV8100_COMMAND_DENC, &av8100_config);
+		break;
+	case DVI_SWITCH:
+		av8100_config.hdmi_format.hdmi_mode = AV8100_HDMI_ON;
+		av8100_config.hdmi_format.hdmi_format = AV8100_DVI;
+		av8100_config.hdmi_format.dvi_format = AV8100_DVI_CTRL_CTL0;
+		ret = av8100_conf_prep(AV8100_COMMAND_HDMI, &av8100_config);
+		break;
+	case HDMI_SWITCH:
+	default:
+		av8100_config.hdmi_format.hdmi_mode = AV8100_HDMI_ON;
+		av8100_config.hdmi_format.hdmi_format = AV8100_HDMI;
+		av8100_config.hdmi_format.dvi_format = AV8100_DVI_CTRL_CTL0;
+		ret = av8100_conf_prep(AV8100_COMMAND_HDMI, &av8100_config);
+		break;
+	}
+
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_prep "
+			"AV8100_COMMAND_HDMI/DENC failed\n", __func__);
+		return ret;
+	}
+
+	/* Enable interrupts */
+	ret = av8100_enable_interrupt();
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_enable_interrupt failed\n",
+			__func__);
+		return ret;
+	}
+
+	if (dev->port->hdmi_sdtv_switch == SDTV_SWITCH)
+		ret = av8100_conf_w(AV8100_COMMAND_DENC, NULL, NULL,
+								I2C_INTERFACE);
+	else
+		ret = av8100_conf_w(AV8100_COMMAND_HDMI, NULL, NULL,
+								I2C_INTERFACE);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_w "
+			"AV8100_COMMAND_HDMI/DENC failed\n", __func__);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int hdmi_set_power_mode(struct mcde_display_device *ddev,
+	enum mcde_display_power_mode power_mode)
+{
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+	int ret = 0;
+
+	/* OFF -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_OFF &&
+					power_mode != MCDE_DISPLAY_PM_OFF) {
+		if (ddev->platform_enable) {
+			ret = ddev->platform_enable(ddev);
+			if (ret)
+				return ret;
+		}
+
+		/*
+		 * the regulator for analog TV out is only enabled here,
+		 * this means that one needs to switch to the OFF state
+		 * to be able to switch from HDMI to CVBS.
+		 */
+		if (ddev->port->hdmi_sdtv_switch == SDTV_SWITCH) {
+			ret = regulator_enable(driver_data->cvbs_regulator);
+			if (ret)
+				return ret;
+			driver_data->cvbs_regulator_enabled = true;
+		}
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+
+		hdmi_set_port_pixel_format(ddev);
+	}
+	/* STANDBY -> ON */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+				power_mode == MCDE_DISPLAY_PM_ON) {
+
+		ddev->power_mode = MCDE_DISPLAY_PM_ON;
+		goto set_power_and_exit;
+	}
+	/* ON -> STANDBY */
+	else if (ddev->power_mode == MCDE_DISPLAY_PM_ON &&
+				power_mode <= MCDE_DISPLAY_PM_STANDBY) {
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+
+	/* STANDBY -> OFF */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+				power_mode == MCDE_DISPLAY_PM_OFF) {
+		memset(&(ddev->video_mode), 0, sizeof(struct mcde_video_mode));
+
+		if (ddev->platform_disable) {
+			ret = ddev->platform_disable(ddev);
+			if (ret)
+				return ret;
+		}
+		if (driver_data->cvbs_regulator_enabled) {
+			ret = regulator_disable(driver_data->cvbs_regulator);
+			if (ret)
+				return ret;
+			driver_data->cvbs_regulator_enabled = false;
+		}
+		ddev->power_mode = MCDE_DISPLAY_PM_OFF;
+	}
+
+set_power_and_exit:
+	mcde_chnl_set_power_mode(ddev->chnl_state, ddev->power_mode);
+
+	return ret;
+}
+
+static int __devinit hdmi_probe(struct mcde_display_device *dev)
+{
+	int ret = 0;
+	struct display_driver_data *driver_data;
+	struct mcde_display_hdmi_platform_data *pdata =
+		dev->dev.platform_data;
+
+	if (pdata == NULL) {
+		dev_err(&dev->dev, "%s:Platform data missing\n", __func__);
+		return -EINVAL;
+	}
+
+	if (dev->port->type != MCDE_PORTTYPE_DSI) {
+		dev_err(&dev->dev, "%s:Invalid port type %d\n",
+			__func__, dev->port->type);
+		return -EINVAL;
+	}
+
+	driver_data = (struct display_driver_data *)
+		kzalloc(sizeof(struct display_driver_data), GFP_KERNEL);
+	if (!driver_data) {
+		dev_err(&dev->dev, "Failed to allocate driver data\n");
+		return -ENOMEM;
+	}
+
+	/* DSI use clock continous mode if AV8100_CHIPVER_1 > 1 */
+	if (av8100_ver_get() > AV8100_CHIPVER_1)
+		dev->port->phy.dsi.clk_cont = true;
+
+	dev->prepare_for_update = NULL;
+	dev->on_first_update = hdmi_on_first_update;
+	dev->try_video_mode = hdmi_try_video_mode;
+	dev->set_video_mode = hdmi_set_video_mode;
+	dev->apply_config = hdmi_apply_config;
+	dev->set_pixel_format = hdmi_set_pixel_format;
+	dev->set_power_mode = hdmi_set_power_mode;
+	dev->ceanr_convert = ceanr_convert;
+
+	/* Create sysfs files */
+	if (device_create_file(&dev->dev, &dev_attr_hdmisdtvswitch))
+		dev_info(&dev->dev,
+			"Unable to create hdmisdtvswitch attr\n");
+	if (device_create_file(&dev->dev, &dev_attr_input_pixel_format))
+		dev_info(&dev->dev,
+			"Unable to create input_pixel_format attr\n");
+	if (device_create_file(&dev->dev, &dev_attr_disponoff))
+		dev_info(&dev->dev,
+			"Unable to create disponoff attr\n");
+	if (device_create_file(&dev->dev, &dev_attr_vesacea))
+		dev_info(&dev->dev,
+			"Unable to create ceavesa attr\n");
+	if (device_create_file(&dev->dev, &dev_attr_timing))
+		dev_info(&dev->dev,
+			"Unable to create timing attr\n");
+	if (device_create_file(&dev->dev, &dev_attr_stayalive))
+		dev_info(&dev->dev,
+			"Unable to create stayalive attr\n");
+
+	if (pdata->cvbs_regulator_id) {
+		driver_data->cvbs_regulator = regulator_get(&dev->dev,
+						pdata->cvbs_regulator_id);
+		if (IS_ERR(driver_data->cvbs_regulator)) {
+			ret = PTR_ERR(driver_data->cvbs_regulator);
+			dev_warn(&dev->dev, "%s:Failed to get regulator %s\n",
+				__func__, pdata->cvbs_regulator_id);
+			driver_data->cvbs_regulator = NULL;
+			goto av_regulator_get_failed;
+		}
+	}
+
+	dev_set_drvdata(&dev->dev, driver_data);
+	dev_info(&dev->dev, "HDMI display probed\n");
+
+	return 0;
+
+av_regulator_get_failed:
+	kfree(driver_data);
+	return ret;
+}
+
+static int __devexit hdmi_remove(struct mcde_display_device *dev)
+{
+	struct display_driver_data *driver_data = dev_get_drvdata(&dev->dev);
+	struct mcde_display_hdmi_platform_data *pdata =
+		dev->dev.platform_data;
+
+	/* Remove sysfs files */
+	device_remove_file(&dev->dev, &dev_attr_input_pixel_format);
+	device_remove_file(&dev->dev, &dev_attr_hdmisdtvswitch);
+	device_remove_file(&dev->dev, &dev_attr_disponoff);
+	device_remove_file(&dev->dev, &dev_attr_vesacea);
+	device_remove_file(&dev->dev, &dev_attr_timing);
+	device_remove_file(&dev->dev, &dev_attr_stayalive);
+
+	dev->set_power_mode(dev, MCDE_DISPLAY_PM_OFF);
+
+	if (driver_data->cvbs_regulator)
+		regulator_put(driver_data->cvbs_regulator);
+	kfree(driver_data);
+	if (pdata->hdmi_platform_enable) {
+		if (pdata->regulator)
+			regulator_put(pdata->regulator);
+		if (pdata->reset_gpio) {
+			gpio_direction_input(pdata->reset_gpio);
+			gpio_free(pdata->reset_gpio);
+		}
+	}
+
+	return 0;
+}
+
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+static int hdmi_resume(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	/* set_power_mode will handle call platform_enable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to resume display\n"
+			, __func__);
+
+	return ret;
+}
+
+static int hdmi_suspend(struct mcde_display_device *ddev, pm_message_t state)
+{
+	int ret;
+
+	/* set_power_mode will handle call platform_disable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to suspend display\n"
+			, __func__);
+
+	return ret;
+}
+#endif
+
+static struct mcde_display_driver hdmi_driver = {
+	.probe	= hdmi_probe,
+	.remove = hdmi_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+	.suspend = hdmi_suspend,
+	.resume = hdmi_resume,
+#else
+	.suspend = NULL,
+	.resume = NULL,
+#endif
+	.driver = {
+		.name	= "av8100_hdmi",
+	},
+};
+
+/* Module init */
+static int __init mcde_display_hdmi_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return mcde_display_driver_register(&hdmi_driver);
+
+}
+late_initcall(mcde_display_hdmi_init);
+
+static void __exit mcde_display_hdmi_exit(void)
+{
+	pr_info("%s\n", __func__);
+
+	mcde_display_driver_unregister(&hdmi_driver);
+}
+module_exit(mcde_display_hdmi_exit);
+
+MODULE_AUTHOR("Per Persson <per.xb.persson@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson hdmi display driver");
diff --git a/drivers/video/mcde/display-fictive.c b/drivers/video/mcde/display-fictive.c
new file mode 100644
index 00000000000..8389357a693
--- /dev/null
+++ b/drivers/video/mcde/display-fictive.c
@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * ST-Ericsson MCDE fictive display driver
+ *
+ * Author: Per Persson <per.xb.persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/err.h>
+
+#include <video/mcde_display.h>
+
+static int __devinit fictive_probe(struct mcde_display_device *dev)
+{
+	dev->prepare_for_update = NULL;
+	dev->platform_enable = NULL,
+	dev->platform_disable = NULL,
+	dev->set_power_mode = NULL;
+
+	dev_info(&dev->dev, "Fictive display probed\n");
+
+	return 0;
+}
+
+static int __devexit fictive_remove(struct mcde_display_device *dev)
+{
+	return 0;
+}
+
+static struct mcde_display_driver fictive_driver = {
+	.probe	= fictive_probe,
+	.remove = fictive_remove,
+	.driver = {
+		.name	= "mcde_disp_fictive",
+	},
+};
+
+/* Module init */
+static int __init mcde_display_fictive_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return mcde_display_driver_register(&fictive_driver);
+}
+module_init(mcde_display_fictive_init);
+
+static void __exit mcde_display_fictive_exit(void)
+{
+	pr_info("%s\n", __func__);
+
+	mcde_display_driver_unregister(&fictive_driver);
+}
+module_exit(mcde_display_fictive_exit);
+
+MODULE_AUTHOR("Per Persson <per.xb.persson@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE fictive display driver");
diff --git a/drivers/video/mcde/display-generic_dsi.c b/drivers/video/mcde/display-generic_dsi.c
new file mode 100644
index 00000000000..64ed3d6daa9
--- /dev/null
+++ b/drivers/video/mcde/display-generic_dsi.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE generic DCS display driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/err.h>
+
+#include <video/mcde_display.h>
+#include <video/mcde_display-generic_dsi.h>
+
+static int generic_platform_enable(struct mcde_display_device *dev)
+{
+	struct mcde_display_generic_platform_data *pdata =
+		dev->dev.platform_data;
+
+	dev_dbg(&dev->dev, "%s: Reset & power on generic display\n", __func__);
+
+	if (pdata->regulator) {
+		if (regulator_enable(pdata->regulator) < 0) {
+			dev_err(&dev->dev, "%s:Failed to enable regulator\n"
+				, __func__);
+			return -EINVAL;
+		}
+	}
+	if (pdata->reset_gpio)
+		gpio_set_value(pdata->reset_gpio, pdata->reset_high);
+	mdelay(pdata->reset_delay);
+	if (pdata->reset_gpio)
+		gpio_set_value(pdata->reset_gpio, !pdata->reset_high);
+
+	return 0;
+}
+
+static int generic_platform_disable(struct mcde_display_device *dev)
+{
+	struct mcde_display_generic_platform_data *pdata =
+		dev->dev.platform_data;
+
+	dev_dbg(&dev->dev, "%s:Reset & power off generic display\n", __func__);
+
+	if (pdata->regulator) {
+		if (regulator_disable(pdata->regulator) < 0) {
+			dev_err(&dev->dev, "%s:Failed to disable regulator\n"
+				, __func__);
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+static int generic_set_power_mode(struct mcde_display_device *ddev,
+	enum mcde_display_power_mode power_mode)
+{
+	int ret = 0;
+	struct mcde_display_generic_platform_data *pdata =
+		ddev->dev.platform_data;
+
+	dev_dbg(&ddev->dev, "%s:Set Power mode\n", __func__);
+
+	/* OFF -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_OFF &&
+		power_mode != MCDE_DISPLAY_PM_OFF) {
+
+		if (ddev->platform_enable) {
+			ret = ddev->platform_enable(ddev);
+			if (ret)
+				return ret;
+		}
+
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+
+	/* STANDBY -> ON */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+		power_mode == MCDE_DISPLAY_PM_ON) {
+
+		ret = mcde_dsi_dcs_write(ddev->chnl_state,
+		DCS_CMD_EXIT_SLEEP_MODE, NULL, 0);
+		if (ret)
+			return ret;
+
+		msleep(pdata->sleep_out_delay);
+
+		ret = mcde_dsi_dcs_write(ddev->chnl_state,
+			DCS_CMD_SET_DISPLAY_ON, NULL, 0);
+		if (ret)
+			return ret;
+
+		ddev->power_mode = MCDE_DISPLAY_PM_ON;
+		goto set_power_and_exit;
+	}
+	/* ON -> STANDBY */
+	else if (ddev->power_mode == MCDE_DISPLAY_PM_ON &&
+		power_mode <= MCDE_DISPLAY_PM_STANDBY) {
+		ret = mcde_dsi_dcs_write(ddev->chnl_state,
+			DCS_CMD_SET_DISPLAY_OFF, NULL, 0);
+		if (ret)
+			return ret;
+
+		ret = mcde_dsi_dcs_write(ddev->chnl_state,
+			DCS_CMD_ENTER_SLEEP_MODE, NULL, 0);
+		if (ret)
+			return ret;
+
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+
+	/* SLEEP -> OFF */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+		power_mode == MCDE_DISPLAY_PM_OFF) {
+		if (ddev->platform_disable) {
+			ret = ddev->platform_disable(ddev);
+			if (ret)
+				return ret;
+		}
+		ddev->power_mode = MCDE_DISPLAY_PM_OFF;
+	}
+
+set_power_and_exit:
+	mcde_chnl_set_power_mode(ddev->chnl_state, ddev->power_mode);
+
+	return ret;
+}
+
+static int __devinit generic_probe(struct mcde_display_device *dev)
+{
+	int ret = 0;
+	struct mcde_display_generic_platform_data *pdata =
+		dev->dev.platform_data;
+
+	if (pdata == NULL) {
+		dev_err(&dev->dev, "%s:Platform data missing\n", __func__);
+		return -EINVAL;
+	}
+
+	if (dev->port->type != MCDE_PORTTYPE_DSI) {
+		dev_err(&dev->dev,
+			"%s:Invalid port type %d\n",
+			__func__, dev->port->type);
+		return -EINVAL;
+	}
+
+	if (!dev->platform_enable && !dev->platform_disable) {
+		pdata->generic_platform_enable = true;
+		if (pdata->reset_gpio) {
+			ret = gpio_request(pdata->reset_gpio, NULL);
+			if (ret) {
+				dev_warn(&dev->dev,
+					"%s:Failed to request gpio %d\n",
+					__func__, pdata->reset_gpio);
+				goto gpio_request_failed;
+			}
+			gpio_direction_output(pdata->reset_gpio,
+				!pdata->reset_high);
+		}
+		if (pdata->regulator_id) {
+			pdata->regulator = regulator_get(&dev->dev,
+				pdata->regulator_id);
+			if (IS_ERR(pdata->regulator)) {
+				ret = PTR_ERR(pdata->regulator);
+				dev_warn(&dev->dev,
+					"%s:Failed to get regulator '%s'\n",
+					__func__, pdata->regulator_id);
+				pdata->regulator = NULL;
+				goto regulator_get_failed;
+			}
+			regulator_set_voltage(pdata->regulator,
+					pdata->min_supply_voltage,
+					pdata->max_supply_voltage);
+			/*
+			* When u-boot has display a startup screen.
+			* U-boot has turned on display power however the
+			* regulator framework does not know about that
+			* This is the case here, the display driver has to
+			* enable the regulator for the display.
+			*/
+			if (dev->power_mode == MCDE_DISPLAY_PM_STANDBY) {
+				ret = regulator_enable(pdata->regulator);
+				if (ret < 0) {
+					dev_err(&dev->dev,
+					"%s:Failed to enable regulator\n"
+					, __func__);
+					goto regulator_enable_failed;
+				}
+			}
+		}
+	}
+
+	/* TODO: Remove when DSI send command uses interrupts */
+	dev->prepare_for_update = NULL;
+	dev->platform_enable = generic_platform_enable,
+	dev->platform_disable = generic_platform_disable,
+	dev->set_power_mode = generic_set_power_mode;
+
+	dev_info(&dev->dev, "Generic display probed\n");
+
+	goto out;
+regulator_enable_failed:
+regulator_get_failed:
+	if (pdata->generic_platform_enable && pdata->reset_gpio)
+		gpio_free(pdata->reset_gpio);
+gpio_request_failed:
+out:
+	return ret;
+}
+
+static int __devexit generic_remove(struct mcde_display_device *dev)
+{
+	struct mcde_display_generic_platform_data *pdata =
+		dev->dev.platform_data;
+
+	dev->set_power_mode(dev, MCDE_DISPLAY_PM_OFF);
+
+	if (!pdata->generic_platform_enable)
+		return 0;
+
+	if (pdata->regulator)
+		regulator_put(pdata->regulator);
+	if (pdata->reset_gpio) {
+		gpio_direction_input(pdata->reset_gpio);
+		gpio_free(pdata->reset_gpio);
+	}
+
+	return 0;
+}
+
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+static int generic_resume(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	/* set_power_mode will handle call platform_enable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to resume display\n"
+			, __func__);
+	ddev->set_synchronized_update(ddev,
+					ddev->get_synchronized_update(ddev));
+	return ret;
+}
+
+static int generic_suspend(struct mcde_display_device *ddev, pm_message_t state)
+{
+	int ret;
+
+	/* set_power_mode will handle call platform_disable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to suspend display\n"
+			, __func__);
+	return ret;
+}
+#endif
+
+static struct mcde_display_driver generic_driver = {
+	.probe	= generic_probe,
+	.remove = generic_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+	.suspend = generic_suspend,
+	.resume = generic_resume,
+#else
+	.suspend = NULL,
+	.resume = NULL,
+#endif
+	.driver = {
+		.name	= "mcde_disp_generic",
+	},
+};
+
+/* Module init */
+static int __init mcde_display_generic_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return mcde_display_driver_register(&generic_driver);
+}
+module_init(mcde_display_generic_init);
+
+static void __exit mcde_display_generic_exit(void)
+{
+	pr_info("%s\n", __func__);
+
+	mcde_display_driver_unregister(&generic_driver);
+}
+module_exit(mcde_display_generic_exit);
+
+MODULE_AUTHOR("Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE generic DCS display driver");
diff --git a/drivers/video/mcde/display-sony_sy35560_dsi.c b/drivers/video/mcde/display-sony_sy35560_dsi.c
new file mode 100644
index 00000000000..28a36b98c77
--- /dev/null
+++ b/drivers/video/mcde/display-sony_sy35560_dsi.c
@@ -0,0 +1,239 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson MCDE Sony sy35560 DCS display driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/err.h>
+
+#include <video/mcde_dss.h>
+#include <video/mcde_display.h>
+#include <video/mcde_display-sony_sy35560_dsi.h>
+
+static int counter = 1;
+
+/*
+ * Work queue function for ESD check
+ *
+ * read display registers and perform power off/on/init sequence
+ * if register value indicate HW failure
+ */
+static void sony_sy35560_esd_work_fn(struct work_struct *work)
+{
+	u8  value;
+	struct sony_sy35560_device *dev = container_of(work,
+		struct sony_sy35560_device,
+		esd_work.work);
+
+
+	mcde_dsi_dcs_write(((struct mcde_display_device *)dev)->chnl_state,
+		0x00, &value, 1);
+	pr_info("%s: %d) Read register 0x00, value 0x%.2X\n",
+		__func__, counter, value);
+
+	/* for now there are no registers available for ESD status check
+	 * just perform power off/on/init every 4th work queue task
+	 */
+
+	if (counter % 4 == 0) {
+		bool vsync_enabled;
+
+		pr_info("%s:Resetting display....", __func__);
+
+		/* get current display state */
+		vsync_enabled = ((struct mcde_display_device *)dev)->
+			get_synchronized_update(
+			(struct mcde_display_device *)dev);
+
+		/* wait a while to make sure finish refresh before resetting */
+		mdelay(10);
+
+		((struct mcde_display_device *)dev)->set_power_mode(
+			(struct mcde_display_device *)dev,
+			MCDE_DISPLAY_PM_OFF);
+		((struct mcde_display_device *)dev)->set_power_mode(
+			(struct mcde_display_device *)dev,
+			MCDE_DISPLAY_PM_STANDBY);
+
+		if (vsync_enabled) {
+			u8 m = 0;
+			mcde_dsi_dcs_write(
+				((struct mcde_display_device *)dev)->chnl_state,
+				DCS_CMD_SET_TEAR_ON, &m, 1);
+			mcde_dss_set_synchronized_update(
+				(struct mcde_display_device *)dev, 0);
+
+			/* refresh display */
+			((struct mcde_display_device *)dev)->update(
+				(struct mcde_display_device *)dev);
+
+			mcde_dss_set_synchronized_update(
+				(struct mcde_display_device *)dev, 1);
+		} else {
+			/* need to wait a while before turning on the display */
+			mdelay(5);
+
+			((struct mcde_display_device *)dev)->set_power_mode(
+				(struct mcde_display_device *)dev,
+				MCDE_DISPLAY_PM_ON);
+		}
+	}
+	counter++;
+
+	queue_delayed_work(((struct sony_sy35560_device *)dev)->esd_wq,
+		&(((struct sony_sy35560_device *)dev)->esd_work),
+		SONY_SY35560_ESD_CHECK_PERIOD);
+}
+
+static int __devinit sony_sy35560_probe(struct mcde_display_device *dev)
+{
+	struct mcde_chnl_state *chnl;
+
+	u32 id;
+	u8  id1, id2, id3;
+	int len = 1;
+	int ret = 0;
+	int readret = 0;
+
+	/* create a workqueue for ESD status check */
+	((struct sony_sy35560_device *)dev)->esd_wq =
+		create_singlethread_workqueue("sony_esd");
+	if (((struct sony_sy35560_device *)dev)->esd_wq == NULL) {
+		dev_warn(&dev->dev, "can't create ESD workqueue\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+	INIT_DELAYED_WORK(&(((struct sony_sy35560_device *)dev)->esd_work),
+		sony_sy35560_esd_work_fn);
+
+	/* Acquire MCDE resources */
+	chnl = mcde_chnl_get(dev->chnl_id, dev->fifo, dev->port);
+	if (IS_ERR(chnl)) {
+		ret = PTR_ERR(chnl);
+		dev_warn(&dev->dev, "Failed to acquire MCDE channel\n");
+		goto out;
+	}
+
+	/* plugnplay: use registers DAh, DBh and DCh to detect display */
+	readret = mcde_dsi_dcs_read(chnl, 0xDA, &id1, &len);
+	if (!readret)
+		readret = mcde_dsi_dcs_read(chnl, 0xDB, &id2, &len);
+	if (!readret)
+		readret = mcde_dsi_dcs_read(chnl, 0xDC, &id3, &len);
+
+	if (readret) {
+		dev_info(&dev->dev,
+			"mcde_dsi_dcs_read failed to read display ID\n");
+		goto read_fail;
+	}
+
+	id = (id3 << 16) | (id2 << 8) | id1;
+
+	switch (id) {
+	case 0x018101:
+		dev_info(&dev->dev,
+			"Cygnus Cut1 display (ID 0x%.6X) probed\n", id);
+		/* add display specific initialization here */
+		break;
+
+	case 0x028101:
+		dev_info(&dev->dev,
+			"Cygnus Cut2 display (ID 0x%.6X) probed\n", id);
+		/* add display specific initialization here */
+		break;
+
+	default:
+		dev_info(&dev->dev,
+			"Display with id 0x%.6X probed\n", id);
+		break;
+	}
+
+read_fail:
+	/* close  MCDE channel */
+	mcde_chnl_put(chnl);
+	chnl = NULL;
+out:
+	return ret;
+}
+
+static int __devexit sony_sy35560_remove(struct mcde_display_device *dev)
+{
+	dev->set_power_mode(dev, MCDE_DISPLAY_PM_OFF);
+
+	cancel_delayed_work(&(((struct sony_sy35560_device *)dev)->esd_work));
+	destroy_workqueue(((struct sony_sy35560_device *)dev)->esd_wq);
+	return 0;
+}
+
+static int sony_sy35560_resume(struct mcde_display_device *ddev)
+{
+	int ret;
+	/* set_power_mode will handle call platform_enable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to resume display\n"
+			, __func__);
+
+	queue_delayed_work(((struct sony_sy35560_device *)ddev)->esd_wq,
+		&(((struct sony_sy35560_device *)ddev)->esd_work),
+		SONY_SY35560_ESD_CHECK_PERIOD);
+
+	return ret;
+}
+
+static int sony_sy35560_suspend(struct mcde_display_device *ddev,
+			pm_message_t state)
+{
+	int ret;
+	/* set_power_mode will handle call platform_disable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to suspend display\n"
+			, __func__);
+
+	cancel_delayed_work(&(((struct sony_sy35560_device *)ddev)->esd_work));
+
+	return ret;
+}
+
+static struct mcde_display_driver sony_sy35560_driver = {
+	.probe	= sony_sy35560_probe,
+	.remove = sony_sy35560_remove,
+	.suspend = sony_sy35560_suspend,
+	.resume = sony_sy35560_resume,
+	.driver = {
+		.name	= "mcde_disp_sony",
+	},
+};
+
+/* Module init */
+
+static int __init mcde_display_sony_sy35560_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return mcde_display_driver_register(&sony_sy35560_driver);
+}
+module_init(mcde_display_sony_sy35560_init);
+
+static void __exit mcde_display_sony_sy35560_exit(void)
+{
+	pr_info("%s\n", __func__);
+
+	mcde_display_driver_unregister(&sony_sy35560_driver);
+}
+module_exit(mcde_display_sony_sy35560_exit);
+
+MODULE_AUTHOR("Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE Sony sy35560 DCS display driver");
+
diff --git a/drivers/video/mcde/display-vuib500-dpi.c b/drivers/video/mcde/display-vuib500-dpi.c
new file mode 100644
index 00000000000..def5fe7c0ab
--- /dev/null
+++ b/drivers/video/mcde/display-vuib500-dpi.c
@@ -0,0 +1,216 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE DPI display driver
+ * The VUIB500 is an user interface board the can be attached to an HREF. It
+ * supports the DPI pixel interface and converts this to an analog VGA signal,
+ * which can be connected to a monitor using a DSUB connector. The VUIB board
+ * uses an external power supply of 5V.
+ *
+ * Author: Marcel Tunnissen <marcel.tuennissen@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+
+#include <video/mcde_display.h>
+#include <video/mcde_display-vuib500-dpi.h>
+
+#define DPI_DISP_TRACE	dev_dbg(&ddev->dev, "%s\n", __func__)
+
+static int try_video_mode(struct mcde_display_device *ddev,
+				struct mcde_video_mode *video_mode);
+static int set_video_mode(struct mcde_display_device *ddev,
+				struct mcde_video_mode *video_mode);
+
+static int __devinit dpi_display_probe(struct mcde_display_device *ddev)
+{
+	int ret = 0;
+	struct mcde_display_dpi_platform_data *pdata = ddev->dev.platform_data;
+	DPI_DISP_TRACE;
+
+	if (pdata == NULL) {
+		dev_err(&ddev->dev, "%s:Platform data missing\n", __func__);
+		ret = -EINVAL;
+		goto no_pdata;
+	}
+
+	if (ddev->port->type != MCDE_PORTTYPE_DPI) {
+		dev_err(&ddev->dev,
+			"%s:Invalid port type %d\n",
+			__func__, ddev->port->type);
+		ret = -EINVAL;
+		goto invalid_port_type;
+	}
+
+	ddev->try_video_mode = try_video_mode;
+	ddev->set_video_mode = set_video_mode;
+	ddev->prepare_for_update = NULL;
+	dev_info(&ddev->dev, "DPI display probed\n");
+
+	goto out;
+invalid_port_type:
+no_pdata:
+out:
+	return ret;
+}
+
+static int __devexit dpi_display_remove(struct mcde_display_device *ddev)
+{
+	DPI_DISP_TRACE;
+
+	ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+
+	return 0;
+}
+
+static int dpi_display_resume(struct mcde_display_device *ddev)
+{
+	int ret;
+	DPI_DISP_TRACE;
+
+	/* set_power_mode will handle call platform_enable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to resume display\n"
+			, __func__);
+	return ret;
+}
+
+static int dpi_display_suspend(struct mcde_display_device *ddev,
+							pm_message_t state)
+{
+	int ret;
+	DPI_DISP_TRACE;
+
+	/* set_power_mode will handle call platform_disable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to suspend display\n"
+			, __func__);
+	return ret;
+}
+
+static void print_vmode(struct mcde_video_mode *vmode)
+{
+	pr_debug("resolution: %dx%d\n", vmode->xres, vmode->yres);
+	pr_debug("  pixclock: %d\n",    vmode->pixclock);
+	pr_debug("       hbp: %d\n",    vmode->hbp);
+	pr_debug("       hfp: %d\n",    vmode->hfp);
+	pr_debug("       hsw: %d\n",    vmode->hsw);
+	pr_debug("       vbp: %d\n",    vmode->vbp);
+	pr_debug("       vfp: %d\n",    vmode->vfp);
+	pr_debug("       vsw: %d\n",    vmode->vsw);
+	pr_debug("interlaced: %s\n", vmode->interlaced ? "true" : "false");
+}
+
+/* Taken from the programmed value of the LCD clock in PRCMU */
+#define PIX_CLK_FREQ		25000000
+#define VMODE_XRES		640
+#define VMODE_YRES		480
+
+static int try_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	int res = -EINVAL;
+	DPI_DISP_TRACE;
+
+	if (ddev == NULL || video_mode == NULL) {
+		dev_warn(&ddev->dev, "%s:ddev = NULL or video_mode = NULL\n",
+			__func__);
+		return res;
+	}
+
+	if (video_mode->xres == VMODE_XRES && video_mode->yres == VMODE_YRES) {
+		video_mode->hbp = 40;
+		video_mode->hfp = 8;
+		video_mode->hsw = 96;
+		video_mode->vbp = 25;
+		video_mode->vfp = 2;
+		video_mode->vsw = 2;
+		/*
+		 * The pixclock setting is not used within MCDE. The clock is
+		 * setup elsewhere. But the pixclock value is visible in user
+		 * space.
+		 */
+		video_mode->pixclock =	(int) (1e+12 * (1.0 / PIX_CLK_FREQ));
+		res = 0;
+	} /* TODO: add more supported resolutions here */
+	video_mode->interlaced = false;
+
+	if (res == 0)
+		print_vmode(video_mode);
+	else
+		dev_warn(&ddev->dev,
+			"%s:Failed to find video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+
+	return res;
+
+}
+
+static int set_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	int res;
+	DPI_DISP_TRACE;
+
+	if (ddev == NULL || video_mode == NULL) {
+		dev_warn(&ddev->dev, "%s:ddev = NULL or video_mode = NULL\n",
+			__func__);
+		return -EINVAL;
+	}
+	if (video_mode->xres != VMODE_XRES || video_mode->yres != VMODE_YRES) {
+		dev_warn(&ddev->dev, "%s:Failed to set video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+		return -EINVAL;
+	}
+	ddev->video_mode = *video_mode;
+	print_vmode(video_mode);
+
+	res = mcde_chnl_set_video_mode(ddev->chnl_state, &ddev->video_mode);
+	if (res < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to set video mode on channel\n",
+			__func__);
+
+	}
+	/* notify mcde display driver about updated video mode */
+	ddev->update_flags |= UPDATE_FLAG_VIDEO_MODE;
+	return res;
+}
+
+static struct mcde_display_driver dpi_display_driver = {
+	.probe	= dpi_display_probe,
+	.remove = dpi_display_remove,
+	.suspend = dpi_display_suspend,
+	.resume = dpi_display_resume,
+	.driver = {
+		.name	= "mcde_display_dpi",
+	},
+};
+
+/* Module init */
+static int __init mcde_dpi_display_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return mcde_display_driver_register(&dpi_display_driver);
+}
+module_init(mcde_dpi_display_init);
+
+static void __exit mcde_dpi_display_exit(void)
+{
+	pr_info("%s\n", __func__);
+
+	mcde_display_driver_unregister(&dpi_display_driver);
+}
+module_exit(mcde_dpi_display_exit);
+
+MODULE_AUTHOR("Marcel Tunnissen <marcel.tuennissen@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE DPI display driver fro VUIB500 display");
diff --git a/drivers/video/mcde/dsilink_regs.h b/drivers/video/mcde/dsilink_regs.h
new file mode 100644
index 00000000000..2322041ffec
--- /dev/null
+++ b/drivers/video/mcde/dsilink_regs.h
@@ -0,0 +1,2036 @@
+
+#define DSI_VAL2REG(__reg, __fld, __val) \
+	(((__val) << __reg##_##__fld##_SHIFT) & __reg##_##__fld##_MASK)
+#define DSI_REG2VAL(__reg, __fld, __val) \
+	(((__val) & __reg##_##__fld##_MASK) >> __reg##_##__fld##_SHIFT)
+
+#define DSI_MCTL_INTEGRATION_MODE 0x00000000
+#define DSI_MCTL_INTEGRATION_MODE_INT_MODE_EN_SHIFT 0
+#define DSI_MCTL_INTEGRATION_MODE_INT_MODE_EN_MASK 0x00000001
+#define DSI_MCTL_INTEGRATION_MODE_INT_MODE_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_INTEGRATION_MODE, INT_MODE_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL 0x00000004
+#define DSI_MCTL_MAIN_DATA_CTL_LINK_EN_SHIFT 0
+#define DSI_MCTL_MAIN_DATA_CTL_LINK_EN_MASK 0x00000001
+#define DSI_MCTL_MAIN_DATA_CTL_LINK_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, LINK_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_SHIFT 1
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_MASK 0x00000002
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_CMD 0
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_VID 1
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_ENUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, IF1_MODE, \
+	DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_##__x)
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, IF1_MODE, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_VID_EN_SHIFT 2
+#define DSI_MCTL_MAIN_DATA_CTL_VID_EN_MASK 0x00000004
+#define DSI_MCTL_MAIN_DATA_CTL_VID_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, VID_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_TVG_SEL_SHIFT 3
+#define DSI_MCTL_MAIN_DATA_CTL_TVG_SEL_MASK 0x00000008
+#define DSI_MCTL_MAIN_DATA_CTL_TVG_SEL(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, TVG_SEL, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_TBG_SEL_SHIFT 4
+#define DSI_MCTL_MAIN_DATA_CTL_TBG_SEL_MASK 0x00000010
+#define DSI_MCTL_MAIN_DATA_CTL_TBG_SEL(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, TBG_SEL, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_TE_EN_SHIFT 5
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_TE_EN_MASK 0x00000020
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_TE_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, IF1_TE_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_IF2_TE_EN_SHIFT 6
+#define DSI_MCTL_MAIN_DATA_CTL_IF2_TE_EN_MASK 0x00000040
+#define DSI_MCTL_MAIN_DATA_CTL_IF2_TE_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, IF2_TE_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_REG_TE_EN_SHIFT 7
+#define DSI_MCTL_MAIN_DATA_CTL_REG_TE_EN_MASK 0x00000080
+#define DSI_MCTL_MAIN_DATA_CTL_REG_TE_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, REG_TE_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_READ_EN_SHIFT 8
+#define DSI_MCTL_MAIN_DATA_CTL_READ_EN_MASK 0x00000100
+#define DSI_MCTL_MAIN_DATA_CTL_READ_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, READ_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_BTA_EN_SHIFT 9
+#define DSI_MCTL_MAIN_DATA_CTL_BTA_EN_MASK 0x00000200
+#define DSI_MCTL_MAIN_DATA_CTL_BTA_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, BTA_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_ECC_SHIFT 10
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_ECC_MASK 0x00000400
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_ECC(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, DISP_GEN_ECC, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_CHECKSUM_SHIFT 11
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_CHECKSUM_MASK 0x00000800
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_CHECKSUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, DISP_GEN_CHECKSUM, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_HOST_EOT_GEN_SHIFT 12
+#define DSI_MCTL_MAIN_DATA_CTL_HOST_EOT_GEN_MASK 0x00001000
+#define DSI_MCTL_MAIN_DATA_CTL_HOST_EOT_GEN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, HOST_EOT_GEN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_EOT_GEN_SHIFT 13
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_EOT_GEN_MASK 0x00002000
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_EOT_GEN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, DISP_EOT_GEN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_DLX_REMAP_EN_SHIFT 14
+#define DSI_MCTL_MAIN_DATA_CTL_DLX_REMAP_EN_MASK 0x00004000
+#define DSI_MCTL_MAIN_DATA_CTL_DLX_REMAP_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, DLX_REMAP_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_TE_POLLING_EN_SHIFT 15
+#define DSI_MCTL_MAIN_DATA_CTL_TE_POLLING_EN_MASK 0x00008000
+#define DSI_MCTL_MAIN_DATA_CTL_TE_POLLING_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, TE_POLLING_EN, __x)
+#define DSI_MCTL_MAIN_PHY_CTL 0x00000008
+#define DSI_MCTL_MAIN_PHY_CTL_LANE2_EN_SHIFT 0
+#define DSI_MCTL_MAIN_PHY_CTL_LANE2_EN_MASK 0x00000001
+#define DSI_MCTL_MAIN_PHY_CTL_LANE2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, LANE2_EN, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_FORCE_STOP_MODE_SHIFT 1
+#define DSI_MCTL_MAIN_PHY_CTL_FORCE_STOP_MODE_MASK 0x00000002
+#define DSI_MCTL_MAIN_PHY_CTL_FORCE_STOP_MODE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, FORCE_STOP_MODE, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_CONTINUOUS_SHIFT 2
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_CONTINUOUS_MASK 0x00000004
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_CONTINUOUS(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, CLK_CONTINUOUS, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_ULPM_EN_SHIFT 3
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_ULPM_EN_MASK 0x00000008
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_ULPM_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, CLK_ULPM_EN, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_DAT1_ULPM_EN_SHIFT 4
+#define DSI_MCTL_MAIN_PHY_CTL_DAT1_ULPM_EN_MASK 0x00000010
+#define DSI_MCTL_MAIN_PHY_CTL_DAT1_ULPM_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, DAT1_ULPM_EN, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_DAT2_ULPM_EN_SHIFT 5
+#define DSI_MCTL_MAIN_PHY_CTL_DAT2_ULPM_EN_MASK 0x00000020
+#define DSI_MCTL_MAIN_PHY_CTL_DAT2_ULPM_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, DAT2_ULPM_EN, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_WAIT_BURST_TIME_SHIFT 6
+#define DSI_MCTL_MAIN_PHY_CTL_WAIT_BURST_TIME_MASK 0x000003C0
+#define DSI_MCTL_MAIN_PHY_CTL_WAIT_BURST_TIME(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, WAIT_BURST_TIME, __x)
+#define DSI_MCTL_PLL_CTL 0x0000000C
+#define DSI_MCTL_PLL_CTL_PLL_MULT_SHIFT 0
+#define DSI_MCTL_PLL_CTL_PLL_MULT_MASK 0x000000FF
+#define DSI_MCTL_PLL_CTL_PLL_MULT(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_MULT, __x)
+#define DSI_MCTL_PLL_CTL_PLL_OUT_DIV_SHIFT 8
+#define DSI_MCTL_PLL_CTL_PLL_OUT_DIV_MASK 0x00003F00
+#define DSI_MCTL_PLL_CTL_PLL_OUT_DIV(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_OUT_DIV, __x)
+#define DSI_MCTL_PLL_CTL_PLL_IN_DIV_SHIFT 14
+#define DSI_MCTL_PLL_CTL_PLL_IN_DIV_MASK 0x0001C000
+#define DSI_MCTL_PLL_CTL_PLL_IN_DIV(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_IN_DIV, __x)
+#define DSI_MCTL_PLL_CTL_PLL_SEL_DIV2_SHIFT 17
+#define DSI_MCTL_PLL_CTL_PLL_SEL_DIV2_MASK 0x00020000
+#define DSI_MCTL_PLL_CTL_PLL_SEL_DIV2(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_SEL_DIV2, __x)
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL_SHIFT 18
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL_MASK 0x00040000
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL_INT_PLL 0
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL_SYS_PLL 1
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL_ENUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_OUT_SEL, \
+	DSI_MCTL_PLL_CTL_PLL_OUT_SEL_##__x)
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_OUT_SEL, __x)
+#define DSI_MCTL_PLL_CTL_PLL_MASTER_SHIFT 31
+#define DSI_MCTL_PLL_CTL_PLL_MASTER_MASK 0x80000000
+#define DSI_MCTL_PLL_CTL_PLL_MASTER(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_MASTER, __x)
+#define DSI_MCTL_LANE_STS 0x00000010
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_SHIFT 0
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_MASK 0x00000003
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_START 0
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_IDLE 1
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_HS 2
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_ULPM 3
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_ENUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, CLKLANE_STATE, \
+	DSI_MCTL_LANE_STS_CLKLANE_STATE_##__x)
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, CLKLANE_STATE, __x)
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_SHIFT 2
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_MASK 0x0000001C
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_START 0
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_IDLE 1
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_WRITE 2
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_ULPM 3
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_READ 4
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_ENUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, DATLANE1_STATE, \
+	DSI_MCTL_LANE_STS_DATLANE1_STATE_##__x)
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, DATLANE1_STATE, __x)
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_SHIFT 5
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_MASK 0x00000060
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_START 0
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_IDLE 1
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_WRITE 2
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_ULPM 3
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_ENUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, DATLANE2_STATE, \
+	DSI_MCTL_LANE_STS_DATLANE2_STATE_##__x)
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, DATLANE2_STATE, __x)
+#define DSI_MCTL_DPHY_TIMEOUT 0x00000014
+#define DSI_MCTL_DPHY_TIMEOUT_CLK_DIV_SHIFT 0
+#define DSI_MCTL_DPHY_TIMEOUT_CLK_DIV_MASK 0x0000000F
+#define DSI_MCTL_DPHY_TIMEOUT_CLK_DIV(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_TIMEOUT, CLK_DIV, __x)
+#define DSI_MCTL_DPHY_TIMEOUT_HSTX_TO_VAL_SHIFT 4
+#define DSI_MCTL_DPHY_TIMEOUT_HSTX_TO_VAL_MASK 0x0003FFF0
+#define DSI_MCTL_DPHY_TIMEOUT_HSTX_TO_VAL(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_TIMEOUT, HSTX_TO_VAL, __x)
+#define DSI_MCTL_DPHY_TIMEOUT_LPRX_TO_VAL_SHIFT 18
+#define DSI_MCTL_DPHY_TIMEOUT_LPRX_TO_VAL_MASK 0xFFFC0000
+#define DSI_MCTL_DPHY_TIMEOUT_LPRX_TO_VAL(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_TIMEOUT, LPRX_TO_VAL, __x)
+#define DSI_MCTL_ULPOUT_TIME 0x00000018
+#define DSI_MCTL_ULPOUT_TIME_CKLANE_ULPOUT_TIME_SHIFT 0
+#define DSI_MCTL_ULPOUT_TIME_CKLANE_ULPOUT_TIME_MASK 0x000001FF
+#define DSI_MCTL_ULPOUT_TIME_CKLANE_ULPOUT_TIME(__x) \
+	DSI_VAL2REG(DSI_MCTL_ULPOUT_TIME, CKLANE_ULPOUT_TIME, __x)
+#define DSI_MCTL_ULPOUT_TIME_DATA_ULPOUT_TIME_SHIFT 9
+#define DSI_MCTL_ULPOUT_TIME_DATA_ULPOUT_TIME_MASK 0x0003FE00
+#define DSI_MCTL_ULPOUT_TIME_DATA_ULPOUT_TIME(__x) \
+	DSI_VAL2REG(DSI_MCTL_ULPOUT_TIME, DATA_ULPOUT_TIME, __x)
+#define DSI_MCTL_DPHY_STATIC 0x0000001C
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_CLK_SHIFT 0
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_CLK_MASK 0x00000001
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_CLK(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, SWAP_PINS_CLK, __x)
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_CLK_SHIFT 1
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_CLK_MASK 0x00000002
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_CLK(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, HS_INVERT_CLK, __x)
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT1_SHIFT 2
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT1_MASK 0x00000004
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, SWAP_PINS_DAT1, __x)
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT1_SHIFT 3
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT1_MASK 0x00000008
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, HS_INVERT_DAT1, __x)
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT2_SHIFT 4
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT2_MASK 0x00000010
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, SWAP_PINS_DAT2, __x)
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT2_SHIFT 5
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT2_MASK 0x00000020
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, HS_INVERT_DAT2, __x)
+#define DSI_MCTL_DPHY_STATIC_UI_X4_SHIFT 6
+#define DSI_MCTL_DPHY_STATIC_UI_X4_MASK 0x00000FC0
+#define DSI_MCTL_DPHY_STATIC_UI_X4(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, UI_X4, __x)
+#define DSI_MCTL_MAIN_EN 0x00000020
+#define DSI_MCTL_MAIN_EN_PLL_START_SHIFT 0
+#define DSI_MCTL_MAIN_EN_PLL_START_MASK 0x00000001
+#define DSI_MCTL_MAIN_EN_PLL_START(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, PLL_START, __x)
+#define DSI_MCTL_MAIN_EN_CKLANE_EN_SHIFT 3
+#define DSI_MCTL_MAIN_EN_CKLANE_EN_MASK 0x00000008
+#define DSI_MCTL_MAIN_EN_CKLANE_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, CKLANE_EN, __x)
+#define DSI_MCTL_MAIN_EN_DAT1_EN_SHIFT 4
+#define DSI_MCTL_MAIN_EN_DAT1_EN_MASK 0x00000010
+#define DSI_MCTL_MAIN_EN_DAT1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, DAT1_EN, __x)
+#define DSI_MCTL_MAIN_EN_DAT2_EN_SHIFT 5
+#define DSI_MCTL_MAIN_EN_DAT2_EN_MASK 0x00000020
+#define DSI_MCTL_MAIN_EN_DAT2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, DAT2_EN, __x)
+#define DSI_MCTL_MAIN_EN_CLKLANE_ULPM_REQ_SHIFT 6
+#define DSI_MCTL_MAIN_EN_CLKLANE_ULPM_REQ_MASK 0x00000040
+#define DSI_MCTL_MAIN_EN_CLKLANE_ULPM_REQ(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, CLKLANE_ULPM_REQ, __x)
+#define DSI_MCTL_MAIN_EN_DAT1_ULPM_REQ_SHIFT 7
+#define DSI_MCTL_MAIN_EN_DAT1_ULPM_REQ_MASK 0x00000080
+#define DSI_MCTL_MAIN_EN_DAT1_ULPM_REQ(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, DAT1_ULPM_REQ, __x)
+#define DSI_MCTL_MAIN_EN_DAT2_ULPM_REQ_SHIFT 8
+#define DSI_MCTL_MAIN_EN_DAT2_ULPM_REQ_MASK 0x00000100
+#define DSI_MCTL_MAIN_EN_DAT2_ULPM_REQ(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, DAT2_ULPM_REQ, __x)
+#define DSI_MCTL_MAIN_EN_IF1_EN_SHIFT 9
+#define DSI_MCTL_MAIN_EN_IF1_EN_MASK 0x00000200
+#define DSI_MCTL_MAIN_EN_IF1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, IF1_EN, __x)
+#define DSI_MCTL_MAIN_EN_IF2_EN_SHIFT 10
+#define DSI_MCTL_MAIN_EN_IF2_EN_MASK 0x00000400
+#define DSI_MCTL_MAIN_EN_IF2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, IF2_EN, __x)
+#define DSI_MCTL_MAIN_STS 0x00000024
+#define DSI_MCTL_MAIN_STS_PLL_LOCK_SHIFT 0
+#define DSI_MCTL_MAIN_STS_PLL_LOCK_MASK 0x00000001
+#define DSI_MCTL_MAIN_STS_PLL_LOCK(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, PLL_LOCK, __x)
+#define DSI_MCTL_MAIN_STS_CLKLANE_READY_SHIFT 1
+#define DSI_MCTL_MAIN_STS_CLKLANE_READY_MASK 0x00000002
+#define DSI_MCTL_MAIN_STS_CLKLANE_READY(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, CLKLANE_READY, __x)
+#define DSI_MCTL_MAIN_STS_DAT1_READY_SHIFT 2
+#define DSI_MCTL_MAIN_STS_DAT1_READY_MASK 0x00000004
+#define DSI_MCTL_MAIN_STS_DAT1_READY(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, DAT1_READY, __x)
+#define DSI_MCTL_MAIN_STS_DAT2_READY_SHIFT 3
+#define DSI_MCTL_MAIN_STS_DAT2_READY_MASK 0x00000008
+#define DSI_MCTL_MAIN_STS_DAT2_READY(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, DAT2_READY, __x)
+#define DSI_MCTL_MAIN_STS_HSTX_TO_ERR_SHIFT 4
+#define DSI_MCTL_MAIN_STS_HSTX_TO_ERR_MASK 0x00000010
+#define DSI_MCTL_MAIN_STS_HSTX_TO_ERR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, HSTX_TO_ERR, __x)
+#define DSI_MCTL_MAIN_STS_LPRX_TO_ERR_SHIFT 5
+#define DSI_MCTL_MAIN_STS_LPRX_TO_ERR_MASK 0x00000020
+#define DSI_MCTL_MAIN_STS_LPRX_TO_ERR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, LPRX_TO_ERR, __x)
+#define DSI_MCTL_MAIN_STS_CRS_UNTERM_PCK_SHIFT 6
+#define DSI_MCTL_MAIN_STS_CRS_UNTERM_PCK_MASK 0x00000040
+#define DSI_MCTL_MAIN_STS_CRS_UNTERM_PCK(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, CRS_UNTERM_PCK, __x)
+#define DSI_MCTL_MAIN_STS_VRS_UNTERM_PCK_SHIFT 7
+#define DSI_MCTL_MAIN_STS_VRS_UNTERM_PCK_MASK 0x00000080
+#define DSI_MCTL_MAIN_STS_VRS_UNTERM_PCK(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, VRS_UNTERM_PCK, __x)
+#define DSI_MCTL_DPHY_ERR 0x00000028
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_1_SHIFT 6
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_1_MASK 0x00000040
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_ESC_1, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_2_SHIFT 7
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_2_MASK 0x00000080
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_ESC_2, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_1_SHIFT 8
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_1_MASK 0x00000100
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_SYNCESC_1, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_2_SHIFT 9
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_2_MASK 0x00000200
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_SYNCESC_2, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_1_SHIFT 10
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_1_MASK 0x00000400
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONTROL_1, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_2_SHIFT 11
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_2_MASK 0x00000800
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONTROL_2, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_1_SHIFT 12
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_1_MASK 0x00001000
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONT_LP0_1, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_2_SHIFT 13
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_2_MASK 0x00002000
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONT_LP0_2, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_1_SHIFT 14
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_1_MASK 0x00004000
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONT_LP1_1, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_2_SHIFT 15
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_2_MASK 0x00008000
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONT_LP1_2, __x)
+#define DSI_INT_VID_RDDATA 0x00000030
+#define DSI_INT_VID_RDDATA_IF_DATA_SHIFT 0
+#define DSI_INT_VID_RDDATA_IF_DATA_MASK 0x0000FFFF
+#define DSI_INT_VID_RDDATA_IF_DATA(__x) \
+	DSI_VAL2REG(DSI_INT_VID_RDDATA, IF_DATA, __x)
+#define DSI_INT_VID_RDDATA_IF_VALID_SHIFT 16
+#define DSI_INT_VID_RDDATA_IF_VALID_MASK 0x00010000
+#define DSI_INT_VID_RDDATA_IF_VALID(__x) \
+	DSI_VAL2REG(DSI_INT_VID_RDDATA, IF_VALID, __x)
+#define DSI_INT_VID_RDDATA_IF_START_SHIFT 17
+#define DSI_INT_VID_RDDATA_IF_START_MASK 0x00020000
+#define DSI_INT_VID_RDDATA_IF_START(__x) \
+	DSI_VAL2REG(DSI_INT_VID_RDDATA, IF_START, __x)
+#define DSI_INT_VID_RDDATA_IF_FRAME_SYNC_SHIFT 18
+#define DSI_INT_VID_RDDATA_IF_FRAME_SYNC_MASK 0x00040000
+#define DSI_INT_VID_RDDATA_IF_FRAME_SYNC(__x) \
+	DSI_VAL2REG(DSI_INT_VID_RDDATA, IF_FRAME_SYNC, __x)
+#define DSI_INT_VID_GNT 0x00000034
+#define DSI_INT_VID_GNT_IF_STALL_SHIFT 0
+#define DSI_INT_VID_GNT_IF_STALL_MASK 0x00000001
+#define DSI_INT_VID_GNT_IF_STALL(__x) \
+	DSI_VAL2REG(DSI_INT_VID_GNT, IF_STALL, __x)
+#define DSI_INT_CMD_RDDATA 0x00000038
+#define DSI_INT_CMD_RDDATA_IF_DATA_SHIFT 0
+#define DSI_INT_CMD_RDDATA_IF_DATA_MASK 0x0000FFFF
+#define DSI_INT_CMD_RDDATA_IF_DATA(__x) \
+	DSI_VAL2REG(DSI_INT_CMD_RDDATA, IF_DATA, __x)
+#define DSI_INT_CMD_RDDATA_IF_VALID_SHIFT 16
+#define DSI_INT_CMD_RDDATA_IF_VALID_MASK 0x00010000
+#define DSI_INT_CMD_RDDATA_IF_VALID(__x) \
+	DSI_VAL2REG(DSI_INT_CMD_RDDATA, IF_VALID, __x)
+#define DSI_INT_CMD_RDDATA_IF_START_SHIFT 17
+#define DSI_INT_CMD_RDDATA_IF_START_MASK 0x00020000
+#define DSI_INT_CMD_RDDATA_IF_START(__x) \
+	DSI_VAL2REG(DSI_INT_CMD_RDDATA, IF_START, __x)
+#define DSI_INT_CMD_RDDATA_IF_FRAME_SYNC_SHIFT 18
+#define DSI_INT_CMD_RDDATA_IF_FRAME_SYNC_MASK 0x00040000
+#define DSI_INT_CMD_RDDATA_IF_FRAME_SYNC(__x) \
+	DSI_VAL2REG(DSI_INT_CMD_RDDATA, IF_FRAME_SYNC, __x)
+#define DSI_INT_CMD_GNT 0x0000003C
+#define DSI_INT_CMD_GNT_IF_STALL_SHIFT 0
+#define DSI_INT_CMD_GNT_IF_STALL_MASK 0x00000001
+#define DSI_INT_CMD_GNT_IF_STALL(__x) \
+	DSI_VAL2REG(DSI_INT_CMD_GNT, IF_STALL, __x)
+#define DSI_INT_INTERRUPT_CTL 0x00000040
+#define DSI_INT_INTERRUPT_CTL_INT_VAL_SHIFT 0
+#define DSI_INT_INTERRUPT_CTL_INT_VAL_MASK 0x00000001
+#define DSI_INT_INTERRUPT_CTL_INT_VAL(__x) \
+	DSI_VAL2REG(DSI_INT_INTERRUPT_CTL, INT_VAL, __x)
+#define DSI_CMD_MODE_CTL 0x00000050
+#define DSI_CMD_MODE_CTL_IF1_ID_SHIFT 0
+#define DSI_CMD_MODE_CTL_IF1_ID_MASK 0x00000003
+#define DSI_CMD_MODE_CTL_IF1_ID(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, IF1_ID, __x)
+#define DSI_CMD_MODE_CTL_IF2_ID_SHIFT 2
+#define DSI_CMD_MODE_CTL_IF2_ID_MASK 0x0000000C
+#define DSI_CMD_MODE_CTL_IF2_ID(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, IF2_ID, __x)
+#define DSI_CMD_MODE_CTL_IF1_LP_EN_SHIFT 4
+#define DSI_CMD_MODE_CTL_IF1_LP_EN_MASK 0x00000010
+#define DSI_CMD_MODE_CTL_IF1_LP_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, IF1_LP_EN, __x)
+#define DSI_CMD_MODE_CTL_IF2_LP_EN_SHIFT 5
+#define DSI_CMD_MODE_CTL_IF2_LP_EN_MASK 0x00000020
+#define DSI_CMD_MODE_CTL_IF2_LP_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, IF2_LP_EN, __x)
+#define DSI_CMD_MODE_CTL_ARB_MODE_SHIFT 6
+#define DSI_CMD_MODE_CTL_ARB_MODE_MASK 0x00000040
+#define DSI_CMD_MODE_CTL_ARB_MODE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, ARB_MODE, __x)
+#define DSI_CMD_MODE_CTL_ARB_PRI_SHIFT 7
+#define DSI_CMD_MODE_CTL_ARB_PRI_MASK 0x00000080
+#define DSI_CMD_MODE_CTL_ARB_PRI(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, ARB_PRI, __x)
+#define DSI_CMD_MODE_CTL_FIL_VALUE_SHIFT 8
+#define DSI_CMD_MODE_CTL_FIL_VALUE_MASK 0x0000FF00
+#define DSI_CMD_MODE_CTL_FIL_VALUE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, FIL_VALUE, __x)
+#define DSI_CMD_MODE_CTL_TE_TIMEOUT_SHIFT 16
+#define DSI_CMD_MODE_CTL_TE_TIMEOUT_MASK 0x03FF0000
+#define DSI_CMD_MODE_CTL_TE_TIMEOUT(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, TE_TIMEOUT, __x)
+#define DSI_CMD_MODE_STS 0x00000054
+#define DSI_CMD_MODE_STS_ERR_NO_TE_SHIFT 0
+#define DSI_CMD_MODE_STS_ERR_NO_TE_MASK 0x00000001
+#define DSI_CMD_MODE_STS_ERR_NO_TE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, ERR_NO_TE, __x)
+#define DSI_CMD_MODE_STS_ERR_TE_MISS_SHIFT 1
+#define DSI_CMD_MODE_STS_ERR_TE_MISS_MASK 0x00000002
+#define DSI_CMD_MODE_STS_ERR_TE_MISS(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, ERR_TE_MISS, __x)
+#define DSI_CMD_MODE_STS_ERR_SDI1_UNDERRUN_SHIFT 2
+#define DSI_CMD_MODE_STS_ERR_SDI1_UNDERRUN_MASK 0x00000004
+#define DSI_CMD_MODE_STS_ERR_SDI1_UNDERRUN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, ERR_SDI1_UNDERRUN, __x)
+#define DSI_CMD_MODE_STS_ERR_SDI2_UNDERRUN_SHIFT 3
+#define DSI_CMD_MODE_STS_ERR_SDI2_UNDERRUN_MASK 0x00000008
+#define DSI_CMD_MODE_STS_ERR_SDI2_UNDERRUN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, ERR_SDI2_UNDERRUN, __x)
+#define DSI_CMD_MODE_STS_ERR_UNWANTED_RD_SHIFT 4
+#define DSI_CMD_MODE_STS_ERR_UNWANTED_RD_MASK 0x00000010
+#define DSI_CMD_MODE_STS_ERR_UNWANTED_RD(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, ERR_UNWANTED_RD, __x)
+#define DSI_CMD_MODE_STS_CSM_RUNNING_SHIFT 5
+#define DSI_CMD_MODE_STS_CSM_RUNNING_MASK 0x00000020
+#define DSI_CMD_MODE_STS_CSM_RUNNING(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, CSM_RUNNING, __x)
+#define DSI_DIRECT_CMD_SEND 0x00000060
+#define DSI_DIRECT_CMD_SEND_START_SHIFT 0
+#define DSI_DIRECT_CMD_SEND_START_MASK 0xFFFFFFFF
+#define DSI_DIRECT_CMD_SEND_START(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_SEND, START, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS 0x00000064
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_SHIFT 0
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_MASK 0x00000007
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_WRITE 0
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_READ 1
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_TE_REQ 4
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_TRIG_REQ 5
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_BTA_REQ 6
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_ENUM(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_NAT, \
+	DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_##__x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_NAT, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT_SHIFT 3
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT_MASK 0x00000008
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_LONGNOTSHORT, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_SHIFT 8
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_MASK 0x00003F00
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_GENERIC_SHORT_WRITE_0 3
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_GENERIC_SHORT_WRITE_1 19
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_GENERIC_SHORT_WRITE_2 35
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_GENERIC_LONG_WRITE 41
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_DCS_SHORT_WRITE_0 5
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_DCS_SHORT_WRITE_1 21
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_DCS_LONG_WRITE 57
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_DCS_READ 6
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_HEAD, \
+	DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_##__x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_HEAD, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID_SHIFT 14
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID_MASK 0x0000C000
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_ID, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE_SHIFT 16
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE_MASK 0x001F0000
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_SIZE, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN_SHIFT 21
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN_MASK 0x00200000
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_LP_EN, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_TRIGGER_VAL_SHIFT 24
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_TRIGGER_VAL_MASK 0x0F000000
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_TRIGGER_VAL(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, TRIGGER_VAL, __x)
+#define DSI_DIRECT_CMD_STS 0x00000068
+#define DSI_DIRECT_CMD_STS_CMD_TRANSMISSION_SHIFT 0
+#define DSI_DIRECT_CMD_STS_CMD_TRANSMISSION_MASK 0x00000001
+#define DSI_DIRECT_CMD_STS_CMD_TRANSMISSION(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, CMD_TRANSMISSION, __x)
+#define DSI_DIRECT_CMD_STS_WRITE_COMPLETED_SHIFT 1
+#define DSI_DIRECT_CMD_STS_WRITE_COMPLETED_MASK 0x00000002
+#define DSI_DIRECT_CMD_STS_WRITE_COMPLETED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, WRITE_COMPLETED, __x)
+#define DSI_DIRECT_CMD_STS_TRIGGER_COMPLETED_SHIFT 2
+#define DSI_DIRECT_CMD_STS_TRIGGER_COMPLETED_MASK 0x00000004
+#define DSI_DIRECT_CMD_STS_TRIGGER_COMPLETED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, TRIGGER_COMPLETED, __x)
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED_SHIFT 3
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED_MASK 0x00000008
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, READ_COMPLETED, __x)
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_RECEIVED_SHIFT 4
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_RECEIVED_MASK 0x00000010
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_RECEIVED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, ACKNOWLEDGE_RECEIVED, __x)
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_WITH_ERR_RECEIVED_SHIFT 5
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_WITH_ERR_RECEIVED_MASK 0x00000020
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_WITH_ERR_RECEIVED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, ACKNOWLEDGE_WITH_ERR_RECEIVED, __x)
+#define DSI_DIRECT_CMD_STS_TRIGGER_RECEIVED_SHIFT 6
+#define DSI_DIRECT_CMD_STS_TRIGGER_RECEIVED_MASK 0x00000040
+#define DSI_DIRECT_CMD_STS_TRIGGER_RECEIVED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, TRIGGER_RECEIVED, __x)
+#define DSI_DIRECT_CMD_STS_TE_RECEIVED_SHIFT 7
+#define DSI_DIRECT_CMD_STS_TE_RECEIVED_MASK 0x00000080
+#define DSI_DIRECT_CMD_STS_TE_RECEIVED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, TE_RECEIVED, __x)
+#define DSI_DIRECT_CMD_STS_BTA_COMPLETED_SHIFT 8
+#define DSI_DIRECT_CMD_STS_BTA_COMPLETED_MASK 0x00000100
+#define DSI_DIRECT_CMD_STS_BTA_COMPLETED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, BTA_COMPLETED, __x)
+#define DSI_DIRECT_CMD_STS_BTA_FINISHED_SHIFT 9
+#define DSI_DIRECT_CMD_STS_BTA_FINISHED_MASK 0x00000200
+#define DSI_DIRECT_CMD_STS_BTA_FINISHED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, BTA_FINISHED, __x)
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED_WITH_ERR_SHIFT 10
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED_WITH_ERR_MASK 0x00000400
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED_WITH_ERR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, READ_COMPLETED_WITH_ERR, __x)
+#define DSI_DIRECT_CMD_STS_TRIGGER_VAL_SHIFT 11
+#define DSI_DIRECT_CMD_STS_TRIGGER_VAL_MASK 0x00007800
+#define DSI_DIRECT_CMD_STS_TRIGGER_VAL(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, TRIGGER_VAL, __x)
+#define DSI_DIRECT_CMD_STS_ACK_VAL_SHIFT 16
+#define DSI_DIRECT_CMD_STS_ACK_VAL_MASK 0xFFFF0000
+#define DSI_DIRECT_CMD_STS_ACK_VAL(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, ACK_VAL, __x)
+#define DSI_DIRECT_CMD_RD_INIT 0x0000006C
+#define DSI_DIRECT_CMD_RD_INIT_RESET_SHIFT 0
+#define DSI_DIRECT_CMD_RD_INIT_RESET_MASK 0xFFFFFFFF
+#define DSI_DIRECT_CMD_RD_INIT_RESET(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_INIT, RESET, __x)
+#define DSI_DIRECT_CMD_WRDAT0 0x00000070
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT0_SHIFT 0
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT0_MASK 0x000000FF
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT0(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT0, WRDAT0, __x)
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT1_SHIFT 8
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT1_MASK 0x0000FF00
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT1(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT0, WRDAT1, __x)
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT2_SHIFT 16
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT2_MASK 0x00FF0000
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT2(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT0, WRDAT2, __x)
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT3_SHIFT 24
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT3_MASK 0xFF000000
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT3(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT0, WRDAT3, __x)
+#define DSI_DIRECT_CMD_WRDAT1 0x00000074
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT4_SHIFT 0
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT4_MASK 0x000000FF
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT4(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT1, WRDAT4, __x)
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT5_SHIFT 8
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT5_MASK 0x0000FF00
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT5(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT1, WRDAT5, __x)
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT6_SHIFT 16
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT6_MASK 0x00FF0000
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT6(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT1, WRDAT6, __x)
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT7_SHIFT 24
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT7_MASK 0xFF000000
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT7(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT1, WRDAT7, __x)
+#define DSI_DIRECT_CMD_WRDAT2 0x00000078
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT8_SHIFT 0
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT8_MASK 0x000000FF
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT8(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT2, WRDAT8, __x)
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT9_SHIFT 8
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT9_MASK 0x0000FF00
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT9(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT2, WRDAT9, __x)
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT10_SHIFT 16
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT10_MASK 0x00FF0000
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT10(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT2, WRDAT10, __x)
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT11_SHIFT 24
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT11_MASK 0xFF000000
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT11(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT2, WRDAT11, __x)
+#define DSI_DIRECT_CMD_WRDAT3 0x0000007C
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT12_SHIFT 0
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT12_MASK 0x000000FF
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT12(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT3, WRDAT12, __x)
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT13_SHIFT 8
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT13_MASK 0x0000FF00
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT13(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT3, WRDAT13, __x)
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT14_SHIFT 16
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT14_MASK 0x00FF0000
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT14(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT3, WRDAT14, __x)
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT15_SHIFT 24
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT15_MASK 0xFF000000
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT15(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT3, WRDAT15, __x)
+#define DSI_DIRECT_CMD_RDDAT 0x00000080
+#define DSI_DIRECT_CMD_RDDAT_RDDAT0_SHIFT 0
+#define DSI_DIRECT_CMD_RDDAT_RDDAT0_MASK 0x000000FF
+#define DSI_DIRECT_CMD_RDDAT_RDDAT0(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RDDAT, RDDAT0, __x)
+#define DSI_DIRECT_CMD_RDDAT_RDDAT1_SHIFT 8
+#define DSI_DIRECT_CMD_RDDAT_RDDAT1_MASK 0x0000FF00
+#define DSI_DIRECT_CMD_RDDAT_RDDAT1(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RDDAT, RDDAT1, __x)
+#define DSI_DIRECT_CMD_RDDAT_RDDAT2_SHIFT 16
+#define DSI_DIRECT_CMD_RDDAT_RDDAT2_MASK 0x00FF0000
+#define DSI_DIRECT_CMD_RDDAT_RDDAT2(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RDDAT, RDDAT2, __x)
+#define DSI_DIRECT_CMD_RDDAT_RDDAT3_SHIFT 24
+#define DSI_DIRECT_CMD_RDDAT_RDDAT3_MASK 0xFF000000
+#define DSI_DIRECT_CMD_RDDAT_RDDAT3(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RDDAT, RDDAT3, __x)
+#define DSI_DIRECT_CMD_RD_PROPERTY 0x00000084
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_SIZE_SHIFT 0
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_SIZE_MASK 0x0000FFFF
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_SIZE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_PROPERTY, RD_SIZE, __x)
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_ID_SHIFT 16
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_ID_MASK 0x00030000
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_ID(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_PROPERTY, RD_ID, __x)
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_DCSNOTGENERIC_SHIFT 18
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_DCSNOTGENERIC_MASK 0x00040000
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_DCSNOTGENERIC(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_PROPERTY, RD_DCSNOTGENERIC, __x)
+#define DSI_DIRECT_CMD_RD_STS 0x00000088
+#define DSI_DIRECT_CMD_RD_STS_ERR_FIXED_SHIFT 0
+#define DSI_DIRECT_CMD_RD_STS_ERR_FIXED_MASK 0x00000001
+#define DSI_DIRECT_CMD_RD_STS_ERR_FIXED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_FIXED, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNCORRECTABLE_SHIFT 1
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNCORRECTABLE_MASK 0x00000002
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNCORRECTABLE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_UNCORRECTABLE, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_CHECKSUM_SHIFT 2
+#define DSI_DIRECT_CMD_RD_STS_ERR_CHECKSUM_MASK 0x00000004
+#define DSI_DIRECT_CMD_RD_STS_ERR_CHECKSUM(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_CHECKSUM, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNDECODABLE_SHIFT 3
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNDECODABLE_MASK 0x00000008
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNDECODABLE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_UNDECODABLE, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_RECEIVE_SHIFT 4
+#define DSI_DIRECT_CMD_RD_STS_ERR_RECEIVE_MASK 0x00000010
+#define DSI_DIRECT_CMD_RD_STS_ERR_RECEIVE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_RECEIVE, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_OVERSIZE_SHIFT 5
+#define DSI_DIRECT_CMD_RD_STS_ERR_OVERSIZE_MASK 0x00000020
+#define DSI_DIRECT_CMD_RD_STS_ERR_OVERSIZE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_OVERSIZE, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_WRONG_LENGTH_SHIFT 6
+#define DSI_DIRECT_CMD_RD_STS_ERR_WRONG_LENGTH_MASK 0x00000040
+#define DSI_DIRECT_CMD_RD_STS_ERR_WRONG_LENGTH(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_WRONG_LENGTH, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_MISSING_EOT_SHIFT 7
+#define DSI_DIRECT_CMD_RD_STS_ERR_MISSING_EOT_MASK 0x00000080
+#define DSI_DIRECT_CMD_RD_STS_ERR_MISSING_EOT(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_MISSING_EOT, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_EOT_WITH_ERR_SHIFT 8
+#define DSI_DIRECT_CMD_RD_STS_ERR_EOT_WITH_ERR_MASK 0x00000100
+#define DSI_DIRECT_CMD_RD_STS_ERR_EOT_WITH_ERR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_EOT_WITH_ERR, __x)
+#define DSI_VID_MAIN_CTL 0x00000090
+#define DSI_VID_MAIN_CTL_START_MODE_SHIFT 0
+#define DSI_VID_MAIN_CTL_START_MODE_MASK 0x00000003
+#define DSI_VID_MAIN_CTL_START_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, START_MODE, __x)
+#define DSI_VID_MAIN_CTL_STOP_MODE_SHIFT 2
+#define DSI_VID_MAIN_CTL_STOP_MODE_MASK 0x0000000C
+#define DSI_VID_MAIN_CTL_STOP_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, STOP_MODE, __x)
+#define DSI_VID_MAIN_CTL_VID_ID_SHIFT 4
+#define DSI_VID_MAIN_CTL_VID_ID_MASK 0x00000030
+#define DSI_VID_MAIN_CTL_VID_ID(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, VID_ID, __x)
+#define DSI_VID_MAIN_CTL_HEADER_SHIFT 6
+#define DSI_VID_MAIN_CTL_HEADER_MASK 0x00000FC0
+#define DSI_VID_MAIN_CTL_HEADER(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, HEADER, __x)
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_SHIFT 12
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_MASK 0x00003000
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_16BITS 0
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_18BITS 1
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_18BITS_LOOSE 2
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_24BITS 3
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_ENUM(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, VID_PIXEL_MODE, \
+	DSI_VID_MAIN_CTL_VID_PIXEL_MODE_##__x)
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, VID_PIXEL_MODE, __x)
+#define DSI_VID_MAIN_CTL_BURST_MODE_SHIFT 14
+#define DSI_VID_MAIN_CTL_BURST_MODE_MASK 0x00004000
+#define DSI_VID_MAIN_CTL_BURST_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, BURST_MODE, __x)
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_ACTIVE_SHIFT 15
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_ACTIVE_MASK 0x00008000
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_ACTIVE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, SYNC_PULSE_ACTIVE, __x)
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_HORIZONTAL_SHIFT 16
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_HORIZONTAL_MASK 0x00010000
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_HORIZONTAL(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, SYNC_PULSE_HORIZONTAL, __x)
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_SHIFT 17
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_MASK 0x00060000
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_NULL 0
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_BLANKING 1
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_LP_0 2
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_LP_1 3
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_ENUM(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, REG_BLKLINE_MODE, \
+	DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_##__x)
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, REG_BLKLINE_MODE, __x)
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_SHIFT 19
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_MASK 0x00180000
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_NULL 0
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_BLANKING 1
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_LP_0 2
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_LP_1 3
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_ENUM(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, REG_BLKEOL_MODE, \
+	DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_##__x)
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, REG_BLKEOL_MODE, __x)
+#define DSI_VID_MAIN_CTL_RECOVERY_MODE_SHIFT 21
+#define DSI_VID_MAIN_CTL_RECOVERY_MODE_MASK 0x00600000
+#define DSI_VID_MAIN_CTL_RECOVERY_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, RECOVERY_MODE, __x)
+#define DSI_VID_VSIZE 0x00000094
+#define DSI_VID_VSIZE_VSA_LENGTH_SHIFT 0
+#define DSI_VID_VSIZE_VSA_LENGTH_MASK 0x0000003F
+#define DSI_VID_VSIZE_VSA_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_VSIZE, VSA_LENGTH, __x)
+#define DSI_VID_VSIZE_VBP_LENGTH_SHIFT 6
+#define DSI_VID_VSIZE_VBP_LENGTH_MASK 0x00000FC0
+#define DSI_VID_VSIZE_VBP_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_VSIZE, VBP_LENGTH, __x)
+#define DSI_VID_VSIZE_VFP_LENGTH_SHIFT 12
+#define DSI_VID_VSIZE_VFP_LENGTH_MASK 0x000FF000
+#define DSI_VID_VSIZE_VFP_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_VSIZE, VFP_LENGTH, __x)
+#define DSI_VID_VSIZE_VACT_LENGTH_SHIFT 20
+#define DSI_VID_VSIZE_VACT_LENGTH_MASK 0x7FF00000
+#define DSI_VID_VSIZE_VACT_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_VSIZE, VACT_LENGTH, __x)
+#define DSI_VID_HSIZE1 0x00000098
+#define DSI_VID_HSIZE1_HSA_LENGTH_SHIFT 0
+#define DSI_VID_HSIZE1_HSA_LENGTH_MASK 0x000003FF
+#define DSI_VID_HSIZE1_HSA_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_HSIZE1, HSA_LENGTH, __x)
+#define DSI_VID_HSIZE1_HBP_LENGTH_SHIFT 10
+#define DSI_VID_HSIZE1_HBP_LENGTH_MASK 0x000FFC00
+#define DSI_VID_HSIZE1_HBP_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_HSIZE1, HBP_LENGTH, __x)
+#define DSI_VID_HSIZE1_HFP_LENGTH_SHIFT 20
+#define DSI_VID_HSIZE1_HFP_LENGTH_MASK 0x7FF00000
+#define DSI_VID_HSIZE1_HFP_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_HSIZE1, HFP_LENGTH, __x)
+#define DSI_VID_HSIZE2 0x0000009C
+#define DSI_VID_HSIZE2_RGB_SIZE_SHIFT 0
+#define DSI_VID_HSIZE2_RGB_SIZE_MASK 0x00001FFF
+#define DSI_VID_HSIZE2_RGB_SIZE(__x) \
+	DSI_VAL2REG(DSI_VID_HSIZE2, RGB_SIZE, __x)
+#define DSI_VID_BLKSIZE1 0x000000A0
+#define DSI_VID_BLKSIZE1_BLKLINE_EVENT_PCK_SHIFT 0
+#define DSI_VID_BLKSIZE1_BLKLINE_EVENT_PCK_MASK 0x00001FFF
+#define DSI_VID_BLKSIZE1_BLKLINE_EVENT_PCK(__x) \
+	DSI_VAL2REG(DSI_VID_BLKSIZE1, BLKLINE_EVENT_PCK, __x)
+#define DSI_VID_BLKSIZE1_BLKEOL_PCK_SHIFT 13
+#define DSI_VID_BLKSIZE1_BLKEOL_PCK_MASK 0x03FFE000
+#define DSI_VID_BLKSIZE1_BLKEOL_PCK(__x) \
+	DSI_VAL2REG(DSI_VID_BLKSIZE1, BLKEOL_PCK, __x)
+#define DSI_VID_BLKSIZE2 0x000000A4
+#define DSI_VID_BLKSIZE2_BLKLINE_PULSE_PCK_SHIFT 0
+#define DSI_VID_BLKSIZE2_BLKLINE_PULSE_PCK_MASK 0x00001FFF
+#define DSI_VID_BLKSIZE2_BLKLINE_PULSE_PCK(__x) \
+	DSI_VAL2REG(DSI_VID_BLKSIZE2, BLKLINE_PULSE_PCK, __x)
+#define DSI_VID_PCK_TIME 0x000000A8
+#define DSI_VID_PCK_TIME_BLKEOL_DURATION_SHIFT 0
+#define DSI_VID_PCK_TIME_BLKEOL_DURATION_MASK 0x00001FFF
+#define DSI_VID_PCK_TIME_BLKEOL_DURATION(__x) \
+	DSI_VAL2REG(DSI_VID_PCK_TIME, BLKEOL_DURATION, __x)
+#define DSI_VID_DPHY_TIME 0x000000AC
+#define DSI_VID_DPHY_TIME_REG_LINE_DURATION_SHIFT 0
+#define DSI_VID_DPHY_TIME_REG_LINE_DURATION_MASK 0x00001FFF
+#define DSI_VID_DPHY_TIME_REG_LINE_DURATION(__x) \
+	DSI_VAL2REG(DSI_VID_DPHY_TIME, REG_LINE_DURATION, __x)
+#define DSI_VID_DPHY_TIME_REG_WAKEUP_TIME_SHIFT 13
+#define DSI_VID_DPHY_TIME_REG_WAKEUP_TIME_MASK 0x00FFE000
+#define DSI_VID_DPHY_TIME_REG_WAKEUP_TIME(__x) \
+	DSI_VAL2REG(DSI_VID_DPHY_TIME, REG_WAKEUP_TIME, __x)
+#define DSI_VID_ERR_COLOR 0x000000B0
+#define DSI_VID_ERR_COLOR_COL_RED_SHIFT 0
+#define DSI_VID_ERR_COLOR_COL_RED_MASK 0x000000FF
+#define DSI_VID_ERR_COLOR_COL_RED(__x) \
+	DSI_VAL2REG(DSI_VID_ERR_COLOR, COL_RED, __x)
+#define DSI_VID_ERR_COLOR_COL_GREEN_SHIFT 8
+#define DSI_VID_ERR_COLOR_COL_GREEN_MASK 0x0000FF00
+#define DSI_VID_ERR_COLOR_COL_GREEN(__x) \
+	DSI_VAL2REG(DSI_VID_ERR_COLOR, COL_GREEN, __x)
+#define DSI_VID_ERR_COLOR_COL_BLUE_SHIFT 16
+#define DSI_VID_ERR_COLOR_COL_BLUE_MASK 0x00FF0000
+#define DSI_VID_ERR_COLOR_COL_BLUE(__x) \
+	DSI_VAL2REG(DSI_VID_ERR_COLOR, COL_BLUE, __x)
+#define DSI_VID_ERR_COLOR_PAD_VAL_SHIFT 24
+#define DSI_VID_ERR_COLOR_PAD_VAL_MASK 0xFF000000
+#define DSI_VID_ERR_COLOR_PAD_VAL(__x) \
+	DSI_VAL2REG(DSI_VID_ERR_COLOR, PAD_VAL, __x)
+#define DSI_VID_VPOS 0x000000B4
+#define DSI_VID_VPOS_LINE_POS_SHIFT 0
+#define DSI_VID_VPOS_LINE_POS_MASK 0x00000003
+#define DSI_VID_VPOS_LINE_POS(__x) \
+	DSI_VAL2REG(DSI_VID_VPOS, LINE_POS, __x)
+#define DSI_VID_VPOS_LINE_VAL_SHIFT 2
+#define DSI_VID_VPOS_LINE_VAL_MASK 0x00001FFC
+#define DSI_VID_VPOS_LINE_VAL(__x) \
+	DSI_VAL2REG(DSI_VID_VPOS, LINE_VAL, __x)
+#define DSI_VID_HPOS 0x000000B8
+#define DSI_VID_HPOS_HORIZONTAL_POS_SHIFT 0
+#define DSI_VID_HPOS_HORIZONTAL_POS_MASK 0x00000007
+#define DSI_VID_HPOS_HORIZONTAL_POS(__x) \
+	DSI_VAL2REG(DSI_VID_HPOS, HORIZONTAL_POS, __x)
+#define DSI_VID_HPOS_HORIZONTAL_VAL_SHIFT 3
+#define DSI_VID_HPOS_HORIZONTAL_VAL_MASK 0x0000FFF8
+#define DSI_VID_HPOS_HORIZONTAL_VAL(__x) \
+	DSI_VAL2REG(DSI_VID_HPOS, HORIZONTAL_VAL, __x)
+#define DSI_VID_MODE_STS 0x000000BC
+#define DSI_VID_MODE_STS_VSG_RUNNING_SHIFT 0
+#define DSI_VID_MODE_STS_VSG_RUNNING_MASK 0x00000001
+#define DSI_VID_MODE_STS_VSG_RUNNING(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, VSG_RUNNING, __x)
+#define DSI_VID_MODE_STS_ERR_MISSING_DATA_SHIFT 1
+#define DSI_VID_MODE_STS_ERR_MISSING_DATA_MASK 0x00000002
+#define DSI_VID_MODE_STS_ERR_MISSING_DATA(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_MISSING_DATA, __x)
+#define DSI_VID_MODE_STS_ERR_MISSING_HSYNC_SHIFT 2
+#define DSI_VID_MODE_STS_ERR_MISSING_HSYNC_MASK 0x00000004
+#define DSI_VID_MODE_STS_ERR_MISSING_HSYNC(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_MISSING_HSYNC, __x)
+#define DSI_VID_MODE_STS_ERR_MISSING_VSYNC_SHIFT 3
+#define DSI_VID_MODE_STS_ERR_MISSING_VSYNC_MASK 0x00000008
+#define DSI_VID_MODE_STS_ERR_MISSING_VSYNC(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_MISSING_VSYNC, __x)
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_LENGTH_SHIFT 4
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_LENGTH_MASK 0x00000010
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, REG_ERR_SMALL_LENGTH, __x)
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_HEIGHT_SHIFT 5
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_HEIGHT_MASK 0x00000020
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_HEIGHT(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, REG_ERR_SMALL_HEIGHT, __x)
+#define DSI_VID_MODE_STS_ERR_BURSTWRITE_SHIFT 6
+#define DSI_VID_MODE_STS_ERR_BURSTWRITE_MASK 0x00000040
+#define DSI_VID_MODE_STS_ERR_BURSTWRITE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_BURSTWRITE, __x)
+#define DSI_VID_MODE_STS_ERR_LONGWRITE_SHIFT 7
+#define DSI_VID_MODE_STS_ERR_LONGWRITE_MASK 0x00000080
+#define DSI_VID_MODE_STS_ERR_LONGWRITE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_LONGWRITE, __x)
+#define DSI_VID_MODE_STS_ERR_LONGREAD_SHIFT 8
+#define DSI_VID_MODE_STS_ERR_LONGREAD_MASK 0x00000100
+#define DSI_VID_MODE_STS_ERR_LONGREAD(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_LONGREAD, __x)
+#define DSI_VID_MODE_STS_ERR_VRS_WRONG_LENGTH_SHIFT 9
+#define DSI_VID_MODE_STS_ERR_VRS_WRONG_LENGTH_MASK 0x00000200
+#define DSI_VID_MODE_STS_ERR_VRS_WRONG_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_VRS_WRONG_LENGTH, __x)
+#define DSI_VID_MODE_STS_VSG_RECOVERY_SHIFT 10
+#define DSI_VID_MODE_STS_VSG_RECOVERY_MASK 0x00000400
+#define DSI_VID_MODE_STS_VSG_RECOVERY(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, VSG_RECOVERY, __x)
+#define DSI_VID_VCA_SETTING1 0x000000C0
+#define DSI_VID_VCA_SETTING1_MAX_BURST_LIMIT_SHIFT 0
+#define DSI_VID_VCA_SETTING1_MAX_BURST_LIMIT_MASK 0x0000FFFF
+#define DSI_VID_VCA_SETTING1_MAX_BURST_LIMIT(__x) \
+	DSI_VAL2REG(DSI_VID_VCA_SETTING1, MAX_BURST_LIMIT, __x)
+#define DSI_VID_VCA_SETTING1_BURST_LP_SHIFT 16
+#define DSI_VID_VCA_SETTING1_BURST_LP_MASK 0x00010000
+#define DSI_VID_VCA_SETTING1_BURST_LP(__x) \
+	DSI_VAL2REG(DSI_VID_VCA_SETTING1, BURST_LP, __x)
+#define DSI_VID_VCA_SETTING2 0x000000C4
+#define DSI_VID_VCA_SETTING2_EXACT_BURST_LIMIT_SHIFT 0
+#define DSI_VID_VCA_SETTING2_EXACT_BURST_LIMIT_MASK 0x0000FFFF
+#define DSI_VID_VCA_SETTING2_EXACT_BURST_LIMIT(__x) \
+	DSI_VAL2REG(DSI_VID_VCA_SETTING2, EXACT_BURST_LIMIT, __x)
+#define DSI_VID_VCA_SETTING2_MAX_LINE_LIMIT_SHIFT 16
+#define DSI_VID_VCA_SETTING2_MAX_LINE_LIMIT_MASK 0xFFFF0000
+#define DSI_VID_VCA_SETTING2_MAX_LINE_LIMIT(__x) \
+	DSI_VAL2REG(DSI_VID_VCA_SETTING2, MAX_LINE_LIMIT, __x)
+#define DSI_TVG_CTL 0x000000C8
+#define DSI_TVG_CTL_TVG_RUN_SHIFT 0
+#define DSI_TVG_CTL_TVG_RUN_MASK 0x00000001
+#define DSI_TVG_CTL_TVG_RUN(__x) \
+	DSI_VAL2REG(DSI_TVG_CTL, TVG_RUN, __x)
+#define DSI_TVG_CTL_TVG_STOPMODE_SHIFT 1
+#define DSI_TVG_CTL_TVG_STOPMODE_MASK 0x00000006
+#define DSI_TVG_CTL_TVG_STOPMODE(__x) \
+	DSI_VAL2REG(DSI_TVG_CTL, TVG_STOPMODE, __x)
+#define DSI_TVG_CTL_TVG_MODE_SHIFT 3
+#define DSI_TVG_CTL_TVG_MODE_MASK 0x00000018
+#define DSI_TVG_CTL_TVG_MODE(__x) \
+	DSI_VAL2REG(DSI_TVG_CTL, TVG_MODE, __x)
+#define DSI_TVG_CTL_TVG_STRIPE_SIZE_SHIFT 5
+#define DSI_TVG_CTL_TVG_STRIPE_SIZE_MASK 0x000000E0
+#define DSI_TVG_CTL_TVG_STRIPE_SIZE(__x) \
+	DSI_VAL2REG(DSI_TVG_CTL, TVG_STRIPE_SIZE, __x)
+#define DSI_TVG_IMG_SIZE 0x000000CC
+#define DSI_TVG_IMG_SIZE_TVG_LINE_SIZE_SHIFT 0
+#define DSI_TVG_IMG_SIZE_TVG_LINE_SIZE_MASK 0x00001FFF
+#define DSI_TVG_IMG_SIZE_TVG_LINE_SIZE(__x) \
+	DSI_VAL2REG(DSI_TVG_IMG_SIZE, TVG_LINE_SIZE, __x)
+#define DSI_TVG_IMG_SIZE_TVG_NBLINE_SHIFT 16
+#define DSI_TVG_IMG_SIZE_TVG_NBLINE_MASK 0x07FF0000
+#define DSI_TVG_IMG_SIZE_TVG_NBLINE(__x) \
+	DSI_VAL2REG(DSI_TVG_IMG_SIZE, TVG_NBLINE, __x)
+#define DSI_TVG_COLOR1 0x000000D0
+#define DSI_TVG_COLOR1_COL1_RED_SHIFT 0
+#define DSI_TVG_COLOR1_COL1_RED_MASK 0x000000FF
+#define DSI_TVG_COLOR1_COL1_RED(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR1, COL1_RED, __x)
+#define DSI_TVG_COLOR1_COL1_GREEN_SHIFT 8
+#define DSI_TVG_COLOR1_COL1_GREEN_MASK 0x0000FF00
+#define DSI_TVG_COLOR1_COL1_GREEN(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR1, COL1_GREEN, __x)
+#define DSI_TVG_COLOR1_COL1_BLUE_SHIFT 16
+#define DSI_TVG_COLOR1_COL1_BLUE_MASK 0x00FF0000
+#define DSI_TVG_COLOR1_COL1_BLUE(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR1, COL1_BLUE, __x)
+#define DSI_TVG_COLOR2 0x000000D4
+#define DSI_TVG_COLOR2_COL2_RED_SHIFT 0
+#define DSI_TVG_COLOR2_COL2_RED_MASK 0x000000FF
+#define DSI_TVG_COLOR2_COL2_RED(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR2, COL2_RED, __x)
+#define DSI_TVG_COLOR2_COL2_GREEN_SHIFT 8
+#define DSI_TVG_COLOR2_COL2_GREEN_MASK 0x0000FF00
+#define DSI_TVG_COLOR2_COL2_GREEN(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR2, COL2_GREEN, __x)
+#define DSI_TVG_COLOR2_COL2_BLUE_SHIFT 16
+#define DSI_TVG_COLOR2_COL2_BLUE_MASK 0x00FF0000
+#define DSI_TVG_COLOR2_COL2_BLUE(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR2, COL2_BLUE, __x)
+#define DSI_TVG_STS 0x000000D8
+#define DSI_TVG_STS_TVG_RUNNING_SHIFT 0
+#define DSI_TVG_STS_TVG_RUNNING_MASK 0x00000001
+#define DSI_TVG_STS_TVG_RUNNING(__x) \
+	DSI_VAL2REG(DSI_TVG_STS, TVG_RUNNING, __x)
+#define DSI_TBG_CTL 0x000000E0
+#define DSI_TBG_CTL_TBG_START_SHIFT 0
+#define DSI_TBG_CTL_TBG_START_MASK 0x00000001
+#define DSI_TBG_CTL_TBG_START(__x) \
+	DSI_VAL2REG(DSI_TBG_CTL, TBG_START, __x)
+#define DSI_TBG_CTL_TBG_HS_REQ_SHIFT 1
+#define DSI_TBG_CTL_TBG_HS_REQ_MASK 0x00000002
+#define DSI_TBG_CTL_TBG_HS_REQ(__x) \
+	DSI_VAL2REG(DSI_TBG_CTL, TBG_HS_REQ, __x)
+#define DSI_TBG_CTL_TBG_DATA_SEL_SHIFT 2
+#define DSI_TBG_CTL_TBG_DATA_SEL_MASK 0x00000004
+#define DSI_TBG_CTL_TBG_DATA_SEL(__x) \
+	DSI_VAL2REG(DSI_TBG_CTL, TBG_DATA_SEL, __x)
+#define DSI_TBG_CTL_TBG_MODE_SHIFT 3
+#define DSI_TBG_CTL_TBG_MODE_MASK 0x00000018
+#define DSI_TBG_CTL_TBG_MODE_1BYTE 0
+#define DSI_TBG_CTL_TBG_MODE_2BYTE 1
+#define DSI_TBG_CTL_TBG_MODE_BURST_COUNTER 2
+#define DSI_TBG_CTL_TBG_MODE_BURST 3
+#define DSI_TBG_CTL_TBG_MODE_ENUM(__x) \
+	DSI_VAL2REG(DSI_TBG_CTL, TBG_MODE, DSI_TBG_CTL_TBG_MODE_##__x)
+#define DSI_TBG_CTL_TBG_MODE(__x) \
+	DSI_VAL2REG(DSI_TBG_CTL, TBG_MODE, __x)
+#define DSI_TBG_SETTING 0x000000E4
+#define DSI_TBG_SETTING_TBG_DATA_SHIFT 0
+#define DSI_TBG_SETTING_TBG_DATA_MASK 0x0000FFFF
+#define DSI_TBG_SETTING_TBG_DATA(__x) \
+	DSI_VAL2REG(DSI_TBG_SETTING, TBG_DATA, __x)
+#define DSI_TBG_SETTING_TBG_CPT_SHIFT 16
+#define DSI_TBG_SETTING_TBG_CPT_MASK 0x0FFF0000
+#define DSI_TBG_SETTING_TBG_CPT(__x) \
+	DSI_VAL2REG(DSI_TBG_SETTING, TBG_CPT, __x)
+#define DSI_TBG_STS 0x000000E8
+#define DSI_TBG_STS_TBG_STATUS_SHIFT 0
+#define DSI_TBG_STS_TBG_STATUS_MASK 0x00000001
+#define DSI_TBG_STS_TBG_STATUS(__x) \
+	DSI_VAL2REG(DSI_TBG_STS, TBG_STATUS, __x)
+#define DSI_MCTL_MAIN_STS_CTL 0x000000F0
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EN_SHIFT 0
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EN_MASK 0x00000001
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, PLL_LOCK_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EN_SHIFT 1
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EN_MASK 0x00000002
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, CLKLANE_READY_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EN_SHIFT 2
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EN_MASK 0x00000004
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, DAT1_READY_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EN_SHIFT 3
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EN_MASK 0x00000008
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, DAT2_READY_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EN_SHIFT 4
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EN_MASK 0x00000010
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, HSTX_TO_ERR_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EN_SHIFT 5
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EN_MASK 0x00000020
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, LPRX_TO_ERR_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EN_SHIFT 6
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EN_MASK 0x00000040
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, CRS_UNTERM_PCK_ERR_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EN_SHIFT 7
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EN_MASK 0x00000080
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, VRS_UNTERM_PCK_ERR_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EDGE_SHIFT 16
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EDGE_MASK 0x00010000
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, PLL_LOCK_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EDGE_SHIFT 17
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EDGE_MASK 0x00020000
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, CLKLANE_READY_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EDGE_SHIFT 18
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EDGE_MASK 0x00040000
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, DAT1_READY_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EDGE_SHIFT 19
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EDGE_MASK 0x00080000
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, DAT2_READY_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EDGE_SHIFT 20
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EDGE_MASK 0x00100000
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, HSTX_TO_ERR_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EDGE_SHIFT 21
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EDGE_MASK 0x00200000
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, LPRX_TO_ERR_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EDGE_SHIFT 22
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EDGE_MASK 0x00400000
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, CRS_UNTERM_PCK_ERR_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EDGE_SHIFT 23
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EDGE_MASK 0x00800000
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, VRS_UNTERM_PCK_ERR_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL 0x000000F4
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EN_SHIFT 0
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EN_MASK 0x00000001
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_NO_TE_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EN_SHIFT 1
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EN_MASK 0x00000002
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_TE_MISS_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EN_SHIFT 2
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EN_MASK 0x00000004
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_SDI1_UNDERRUN_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EN_SHIFT 3
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EN_MASK 0x00000008
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_SDI2_UNDERRUN_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EN_SHIFT 4
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EN_MASK 0x00000010
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_UNWANTED_RD_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EN_SHIFT 5
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EN_MASK 0x00000020
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, CSM_RUNNING_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EDGE_SHIFT 16
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EDGE_MASK 0x00010000
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_NO_TE_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EDGE_SHIFT 17
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EDGE_MASK 0x00020000
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_TE_MISS_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EDGE_SHIFT 18
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EDGE_MASK 0x00040000
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_SDI1_UNDERRUN_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EDGE_SHIFT 19
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EDGE_MASK 0x00080000
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_SDI2_UNDERRUN_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EDGE_SHIFT 20
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EDGE_MASK 0x00100000
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_UNWANTED_RD_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EDGE_SHIFT 21
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EDGE_MASK 0x00200000
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, CSM_RUNNING_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL 0x000000F8
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EN_SHIFT 0
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EN_MASK 0x00000001
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, CMD_TRANSMISSION_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EN_SHIFT 1
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EN_MASK 0x00000002
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, WRITE_COMPLETED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EN_SHIFT 2
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EN_MASK 0x00000004
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TRIGGER_COMPLETED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EN_SHIFT 3
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EN_MASK 0x00000008
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, READ_COMPLETED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EN_SHIFT 4
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EN_MASK 0x00000010
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, ACKNOWLEDGE_RECEIVED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EN_SHIFT 5
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EN_MASK 0x00000020
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, ACKNOWLEDGE_WITH_ERR_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EN_SHIFT 6
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EN_MASK 0x00000040
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TRIGGER_RECEIVED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EN_SHIFT 7
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EN_MASK 0x00000080
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TE_RECEIVED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EN_SHIFT 8
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EN_MASK 0x00000100
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, BTA_COMPLETED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EN_SHIFT 9
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EN_MASK 0x00000200
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, BTA_FINISHED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EN_SHIFT 10
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EN_MASK 0x00000400
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, READ_COMPLETED_WITH_ERR_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EDGE_SHIFT 16
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EDGE_MASK 0x00010000
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, CMD_TRANSMISSION_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EDGE_SHIFT 17
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EDGE_MASK 0x00020000
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, WRITE_COMPLETED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EDGE_SHIFT 18
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EDGE_MASK 0x00040000
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TRIGGER_COMPLETED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EDGE_SHIFT 19
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EDGE_MASK 0x00080000
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, READ_COMPLETED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EDGE_SHIFT 20
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EDGE_MASK 0x00100000
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, ACKNOWLEDGE_RECEIVED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EDGE_SHIFT 21
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EDGE_MASK 0x00200000
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, ACKNOWLEDGE_WITH_ERR_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EDGE_SHIFT 22
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EDGE_MASK 0x00400000
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TRIGGER_RECEIVED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EDGE_SHIFT 23
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EDGE_MASK 0x00800000
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TE_RECEIVED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EDGE_SHIFT 24
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EDGE_MASK 0x01000000
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, BTA_COMPLETED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EDGE_SHIFT 25
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EDGE_MASK 0x02000000
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, BTA_FINISHED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EDGE_SHIFT 26
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EDGE_MASK 0x04000000
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, READ_COMPLETED_WITH_ERR_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL 0x000000FC
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EN_SHIFT 0
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EN_MASK 0x00000001
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_FIXED_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EN_SHIFT 1
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EN_MASK 0x00000002
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_UNCORRECTABLE_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EN_SHIFT 2
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EN_MASK 0x00000004
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_CHECKSUM_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EN_SHIFT 3
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EN_MASK 0x00000008
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_UNDECODABLE_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EN_SHIFT 4
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EN_MASK 0x00000010
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_RECEIVE_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EN_SHIFT 5
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EN_MASK 0x00000020
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_OVERSIZE_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EN_SHIFT 6
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EN_MASK 0x00000040
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_WRONG_LENGTH_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EN_SHIFT 7
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EN_MASK 0x00000080
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_MISSING_EOT_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EN_SHIFT 8
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EN_MASK 0x00000100
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_EOT_WITH_ERR_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EDGE_SHIFT 16
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EDGE_MASK 0x00010000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_FIXED_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EDGE_SHIFT 17
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EDGE_MASK 0x00020000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_UNCORRECTABLE_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EDGE_SHIFT 18
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EDGE_MASK 0x00040000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_CHECKSUM_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EDGE_SHIFT 19
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EDGE_MASK 0x00080000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_UNDECODABLE_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EDGE_SHIFT 20
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EDGE_MASK 0x00100000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_RECEIVE_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EDGE_SHIFT 21
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EDGE_MASK 0x00200000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_OVERSIZE_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EDGE_SHIFT 22
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EDGE_MASK 0x00400000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_WRONG_LENGTH_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EDGE_SHIFT 23
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EDGE_MASK 0x00800000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_MISSING_EOT_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EDGE_SHIFT 24
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EDGE_MASK 0x01000000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_EOT_WITH_ERR_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL 0x00000100
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EN_SHIFT 0
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EN_MASK 0x00000001
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, VSG_RUNNING_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EN_SHIFT 1
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EN_MASK 0x00000002
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_DATA_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EN_SHIFT 2
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EN_MASK 0x00000004
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_HSYNC_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EN_SHIFT 3
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EN_MASK 0x00000008
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_VSYNC_EN, __x)
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EN_SHIFT 4
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EN_MASK 0x00000010
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, REG_ERR_SMALL_LENGTH_EN, __x)
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EN_SHIFT 5
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EN_MASK 0x00000020
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, REG_ERR_SMALL_HEIGHT_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EN_SHIFT 6
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EN_MASK 0x00000040
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_BURSTWRITE_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EN_SHIFT 7
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EN_MASK 0x00000080
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_LONGWRITE_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EN_SHIFT 8
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EN_MASK 0x00000100
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_LONGREAD_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EN_SHIFT 9
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EN_MASK 0x00000200
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_VRS_WRONG_LENGTH_EN, __x)
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EDGE_SHIFT 16
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EDGE_MASK 0x00010000
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, VSG_RUNNING_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EDGE_SHIFT 17
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EDGE_MASK 0x00020000
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_DATA_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EDGE_SHIFT 18
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EDGE_MASK 0x00040000
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_HSYNC_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EDGE_SHIFT 19
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EDGE_MASK 0x00080000
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_VSYNC_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EDGE_SHIFT 20
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EDGE_MASK 0x00100000
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, REG_ERR_SMALL_LENGTH_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EDGE_SHIFT 21
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EDGE_MASK 0x00200000
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, REG_ERR_SMALL_HEIGHT_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EDGE_SHIFT 22
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EDGE_MASK 0x00400000
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_BURSTWRITE_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EDGE_SHIFT 23
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EDGE_MASK 0x00800000
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_LONGWRITE_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EDGE_SHIFT 24
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EDGE_MASK 0x01000000
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_LONGREAD_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EDGE_SHIFT 25
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EDGE_MASK 0x02000000
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_VRS_WRONG_LENGTH_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_VSG_RECOVERY_EDGE_SHIFT 26
+#define DSI_VID_MODE_STS_CTL_VSG_RECOVERY_EDGE_MASK 0x04000000
+#define DSI_VID_MODE_STS_CTL_VSG_RECOVERY_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, VSG_RECOVERY_EDGE, __x)
+#define DSI_TG_STS_CTL 0x00000104
+#define DSI_TG_STS_CTL_TVG_STS_EN_SHIFT 0
+#define DSI_TG_STS_CTL_TVG_STS_EN_MASK 0x00000001
+#define DSI_TG_STS_CTL_TVG_STS_EN(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CTL, TVG_STS_EN, __x)
+#define DSI_TG_STS_CTL_TBG_STS_EN_SHIFT 1
+#define DSI_TG_STS_CTL_TBG_STS_EN_MASK 0x00000002
+#define DSI_TG_STS_CTL_TBG_STS_EN(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CTL, TBG_STS_EN, __x)
+#define DSI_TG_STS_CTL_TVG_STS_EDGE_SHIFT 16
+#define DSI_TG_STS_CTL_TVG_STS_EDGE_MASK 0x00010000
+#define DSI_TG_STS_CTL_TVG_STS_EDGE(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CTL, TVG_STS_EDGE, __x)
+#define DSI_TG_STS_CTL_TBG_STS_EDGE_SHIFT 17
+#define DSI_TG_STS_CTL_TBG_STS_EDGE_MASK 0x00020000
+#define DSI_TG_STS_CTL_TBG_STS_EDGE(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CTL, TBG_STS_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL 0x00000108
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EN_SHIFT 6
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EN_MASK 0x00000040
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_ESC_1_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EN_SHIFT 7
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EN_MASK 0x00000080
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_ESC_2_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EN_SHIFT 8
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EN_MASK 0x00000100
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_SYNCESC_1_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EN_SHIFT 9
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EN_MASK 0x00000200
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_SYNCESC_2_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EN_SHIFT 10
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EN_MASK 0x00000400
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONTROL_1_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EN_SHIFT 11
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EN_MASK 0x00000800
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONTROL_2_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EN_SHIFT 12
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EN_MASK 0x00001000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP0_1_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EN_SHIFT 13
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EN_MASK 0x00002000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP0_2_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EN_SHIFT 14
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EN_MASK 0x00004000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP1_1_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EN_SHIFT 15
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EN_MASK 0x00008000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP1_2_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EDGE_SHIFT 22
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EDGE_MASK 0x00400000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_ESC_1_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EDGE_SHIFT 23
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EDGE_MASK 0x00800000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_ESC_2_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EDGE_SHIFT 24
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EDGE_MASK 0x01000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_SYNCESC_1_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EDGE_SHIFT 25
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EDGE_MASK 0x02000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_SYNCESC_2_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EDGE_SHIFT 26
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EDGE_MASK 0x04000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONTROL_1_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EDGE_SHIFT 27
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EDGE_MASK 0x08000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONTROL_2_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EDGE_SHIFT 28
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EDGE_MASK 0x10000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP0_1_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EDGE_SHIFT 29
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EDGE_MASK 0x20000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP0_2_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EDGE_SHIFT 30
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EDGE_MASK 0x40000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP1_1_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EDGE_SHIFT 31
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EDGE_MASK 0x80000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP1_2_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CLR 0x00000110
+#define DSI_MCTL_MAIN_STS_CLR_PLL_LOCK_CLR_SHIFT 0
+#define DSI_MCTL_MAIN_STS_CLR_PLL_LOCK_CLR_MASK 0x00000001
+#define DSI_MCTL_MAIN_STS_CLR_PLL_LOCK_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, PLL_LOCK_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_CLKLANE_READY_CLR_SHIFT 1
+#define DSI_MCTL_MAIN_STS_CLR_CLKLANE_READY_CLR_MASK 0x00000002
+#define DSI_MCTL_MAIN_STS_CLR_CLKLANE_READY_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, CLKLANE_READY_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_DAT1_READY_CLR_SHIFT 2
+#define DSI_MCTL_MAIN_STS_CLR_DAT1_READY_CLR_MASK 0x00000004
+#define DSI_MCTL_MAIN_STS_CLR_DAT1_READY_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, DAT1_READY_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_DAT2_READY_CLR_SHIFT 3
+#define DSI_MCTL_MAIN_STS_CLR_DAT2_READY_CLR_MASK 0x00000008
+#define DSI_MCTL_MAIN_STS_CLR_DAT2_READY_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, DAT2_READY_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_HSTX_TO_ERR_CLR_SHIFT 4
+#define DSI_MCTL_MAIN_STS_CLR_HSTX_TO_ERR_CLR_MASK 0x00000010
+#define DSI_MCTL_MAIN_STS_CLR_HSTX_TO_ERR_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, HSTX_TO_ERR_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_LPRX_TO_ERR_CLR_SHIFT 5
+#define DSI_MCTL_MAIN_STS_CLR_LPRX_TO_ERR_CLR_MASK 0x00000020
+#define DSI_MCTL_MAIN_STS_CLR_LPRX_TO_ERR_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, LPRX_TO_ERR_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_CRS_UNTERM_PCK_CLR_SHIFT 6
+#define DSI_MCTL_MAIN_STS_CLR_CRS_UNTERM_PCK_CLR_MASK 0x00000040
+#define DSI_MCTL_MAIN_STS_CLR_CRS_UNTERM_PCK_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, CRS_UNTERM_PCK_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_VRS_UNTERM_PCK_CLR_SHIFT 7
+#define DSI_MCTL_MAIN_STS_CLR_VRS_UNTERM_PCK_CLR_MASK 0x00000080
+#define DSI_MCTL_MAIN_STS_CLR_VRS_UNTERM_PCK_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, VRS_UNTERM_PCK_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR 0x00000114
+#define DSI_CMD_MODE_STS_CLR_ERR_NO_TE_CLR_SHIFT 0
+#define DSI_CMD_MODE_STS_CLR_ERR_NO_TE_CLR_MASK 0x00000001
+#define DSI_CMD_MODE_STS_CLR_ERR_NO_TE_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, ERR_NO_TE_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR_ERR_TE_MISS_CLR_SHIFT 1
+#define DSI_CMD_MODE_STS_CLR_ERR_TE_MISS_CLR_MASK 0x00000002
+#define DSI_CMD_MODE_STS_CLR_ERR_TE_MISS_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, ERR_TE_MISS_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI1_UNDERRUN_CLR_SHIFT 2
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI1_UNDERRUN_CLR_MASK 0x00000004
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI1_UNDERRUN_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, ERR_SDI1_UNDERRUN_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI2_UNDERRUN_CLR_SHIFT 3
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI2_UNDERRUN_CLR_MASK 0x00000008
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI2_UNDERRUN_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, ERR_SDI2_UNDERRUN_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR_ERR_UNWANTED_RD_CLR_SHIFT 4
+#define DSI_CMD_MODE_STS_CLR_ERR_UNWANTED_RD_CLR_MASK 0x00000010
+#define DSI_CMD_MODE_STS_CLR_ERR_UNWANTED_RD_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, ERR_UNWANTED_RD_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR_CSM_RUNNING_CLR_SHIFT 5
+#define DSI_CMD_MODE_STS_CLR_CSM_RUNNING_CLR_MASK 0x00000020
+#define DSI_CMD_MODE_STS_CLR_CSM_RUNNING_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, CSM_RUNNING_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR 0x00000118
+#define DSI_DIRECT_CMD_STS_CLR_CMD_TRANSMISSION_CLR_SHIFT 0
+#define DSI_DIRECT_CMD_STS_CLR_CMD_TRANSMISSION_CLR_MASK 0x00000001
+#define DSI_DIRECT_CMD_STS_CLR_CMD_TRANSMISSION_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, CMD_TRANSMISSION_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_WRITE_COMPLETED_CLR_SHIFT 1
+#define DSI_DIRECT_CMD_STS_CLR_WRITE_COMPLETED_CLR_MASK 0x00000002
+#define DSI_DIRECT_CMD_STS_CLR_WRITE_COMPLETED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, WRITE_COMPLETED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_COMPLETED_CLR_SHIFT 2
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_COMPLETED_CLR_MASK 0x00000004
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_COMPLETED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, TRIGGER_COMPLETED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_CLR_SHIFT 3
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_CLR_MASK 0x00000008
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, READ_COMPLETED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_RECEIVED_CLR_SHIFT 4
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_RECEIVED_CLR_MASK 0x00000010
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_RECEIVED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, ACKNOWLEDGE_RECEIVED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_WITH_ERR_RECEIVED_CLR_SHIFT 5
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_WITH_ERR_RECEIVED_CLR_MASK 0x00000020
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_WITH_ERR_RECEIVED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, ACKNOWLEDGE_WITH_ERR_RECEIVED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_RECEIVED_CLR_SHIFT 6
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_RECEIVED_CLR_MASK 0x00000040
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_RECEIVED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, TRIGGER_RECEIVED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_TE_RECEIVED_CLR_SHIFT 7
+#define DSI_DIRECT_CMD_STS_CLR_TE_RECEIVED_CLR_MASK 0x00000080
+#define DSI_DIRECT_CMD_STS_CLR_TE_RECEIVED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, TE_RECEIVED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_BTA_COMPLETED_CLR_SHIFT 8
+#define DSI_DIRECT_CMD_STS_CLR_BTA_COMPLETED_CLR_MASK 0x00000100
+#define DSI_DIRECT_CMD_STS_CLR_BTA_COMPLETED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, BTA_COMPLETED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_BTA_FINISHED_CLR_SHIFT 9
+#define DSI_DIRECT_CMD_STS_CLR_BTA_FINISHED_CLR_MASK 0x00000200
+#define DSI_DIRECT_CMD_STS_CLR_BTA_FINISHED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, BTA_FINISHED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_WITH_ERR_CLR_SHIFT 10
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_WITH_ERR_CLR_MASK 0x00000400
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_WITH_ERR_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, READ_COMPLETED_WITH_ERR_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR 0x0000011C
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_FIXED_CLR_SHIFT 0
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_FIXED_CLR_MASK 0x00000001
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_FIXED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_FIXED_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNCORRECTABLE_CLR_SHIFT 1
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNCORRECTABLE_CLR_MASK 0x00000002
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNCORRECTABLE_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_UNCORRECTABLE_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_CHECKSUM_CLR_SHIFT 2
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_CHECKSUM_CLR_MASK 0x00000004
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_CHECKSUM_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_CHECKSUM_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNDECODABLE_CLR_SHIFT 3
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNDECODABLE_CLR_MASK 0x00000008
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNDECODABLE_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_UNDECODABLE_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_RECEIVE_CLR_SHIFT 4
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_RECEIVE_CLR_MASK 0x00000010
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_RECEIVE_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_RECEIVE_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_OVERSIZE_CLR_SHIFT 5
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_OVERSIZE_CLR_MASK 0x00000020
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_OVERSIZE_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_OVERSIZE_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_WRONG_LENGTH_CLR_SHIFT 6
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_WRONG_LENGTH_CLR_MASK 0x00000040
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_WRONG_LENGTH_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_WRONG_LENGTH_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_MISSING_EOT_CLR_SHIFT 7
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_MISSING_EOT_CLR_MASK 0x00000080
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_MISSING_EOT_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_MISSING_EOT_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_EOT_WITH_ERR_CLR_SHIFT 8
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_EOT_WITH_ERR_CLR_MASK 0x00000100
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_EOT_WITH_ERR_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_EOT_WITH_ERR_CLR, __x)
+#define DSI_VID_MODE_STS_CLR 0x00000120
+#define DSI_VID_MODE_STS_CLR_VSG_STS_CLR_SHIFT 0
+#define DSI_VID_MODE_STS_CLR_VSG_STS_CLR_MASK 0x00000001
+#define DSI_VID_MODE_STS_CLR_VSG_STS_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, VSG_STS_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_DATA_CLR_SHIFT 1
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_DATA_CLR_MASK 0x00000002
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_DATA_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_MISSING_DATA_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_HSYNC_CLR_SHIFT 2
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_HSYNC_CLR_MASK 0x00000004
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_HSYNC_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_MISSING_HSYNC_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_VSYNC_CLR_SHIFT 3
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_VSYNC_CLR_MASK 0x00000008
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_VSYNC_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_MISSING_VSYNC_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_LENGTH_CLR_SHIFT 4
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_LENGTH_CLR_MASK 0x00000010
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_LENGTH_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, REG_ERR_SMALL_LENGTH_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_HEIGHT_CLR_SHIFT 5
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_HEIGHT_CLR_MASK 0x00000020
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_HEIGHT_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, REG_ERR_SMALL_HEIGHT_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_BURSTWRITE_CLR_SHIFT 6
+#define DSI_VID_MODE_STS_CLR_ERR_BURSTWRITE_CLR_MASK 0x00000040
+#define DSI_VID_MODE_STS_CLR_ERR_BURSTWRITE_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_BURSTWRITE_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_LONGWRITE_CLR_SHIFT 7
+#define DSI_VID_MODE_STS_CLR_ERR_LONGWRITE_CLR_MASK 0x00000080
+#define DSI_VID_MODE_STS_CLR_ERR_LONGWRITE_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_LONGWRITE_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_LONGREAD_CLR_SHIFT 8
+#define DSI_VID_MODE_STS_CLR_ERR_LONGREAD_CLR_MASK 0x00000100
+#define DSI_VID_MODE_STS_CLR_ERR_LONGREAD_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_LONGREAD_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_VRS_WRONG_LENGTH_CLR_SHIFT 9
+#define DSI_VID_MODE_STS_CLR_ERR_VRS_WRONG_LENGTH_CLR_MASK 0x00000200
+#define DSI_VID_MODE_STS_CLR_ERR_VRS_WRONG_LENGTH_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_VRS_WRONG_LENGTH_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_VSG_RECOVERY_CLR_SHIFT 10
+#define DSI_VID_MODE_STS_CLR_VSG_RECOVERY_CLR_MASK 0x00000400
+#define DSI_VID_MODE_STS_CLR_VSG_RECOVERY_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, VSG_RECOVERY_CLR, __x)
+#define DSI_TG_STS_CLR 0x00000124
+#define DSI_TG_STS_CLR_TVG_STS_CLR_SHIFT 0
+#define DSI_TG_STS_CLR_TVG_STS_CLR_MASK 0x00000001
+#define DSI_TG_STS_CLR_TVG_STS_CLR(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CLR, TVG_STS_CLR, __x)
+#define DSI_TG_STS_CLR_TBG_STS_CLR_SHIFT 1
+#define DSI_TG_STS_CLR_TBG_STS_CLR_MASK 0x00000002
+#define DSI_TG_STS_CLR_TBG_STS_CLR(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CLR, TBG_STS_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR 0x00000128
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_1_CLR_SHIFT 6
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_1_CLR_MASK 0x00000040
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_1_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_ESC_1_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_2_CLR_SHIFT 7
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_2_CLR_MASK 0x00000080
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_2_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_ESC_2_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_1_CLR_SHIFT 8
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_1_CLR_MASK 0x00000100
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_1_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_SYNCESC_1_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_2_CLR_SHIFT 9
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_2_CLR_MASK 0x00000200
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_2_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_SYNCESC_2_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_1_CLR_SHIFT 10
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_1_CLR_MASK 0x00000400
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_1_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONTROL_1_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_2_CLR_SHIFT 11
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_2_CLR_MASK 0x00000800
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_2_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONTROL_2_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_1_CLR_SHIFT 12
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_1_CLR_MASK 0x00001000
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_1_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONT_LP0_1_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_2_CLR_SHIFT 13
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_2_CLR_MASK 0x00002000
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_2_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONT_LP0_2_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_1_CLR_SHIFT 14
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_1_CLR_MASK 0x00004000
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_1_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONT_LP1_1_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_2_CLR_SHIFT 15
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_2_CLR_MASK 0x00008000
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_2_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONT_LP1_2_CLR, __x)
+#define DSI_MCTL_MAIN_STS_FLAG 0x00000130
+#define DSI_MCTL_MAIN_STS_FLAG_PLL_LOCK_FLAG_SHIFT 0
+#define DSI_MCTL_MAIN_STS_FLAG_PLL_LOCK_FLAG_MASK 0x00000001
+#define DSI_MCTL_MAIN_STS_FLAG_PLL_LOCK_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, PLL_LOCK_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_CLKLANE_READY_FLAG_SHIFT 1
+#define DSI_MCTL_MAIN_STS_FLAG_CLKLANE_READY_FLAG_MASK 0x00000002
+#define DSI_MCTL_MAIN_STS_FLAG_CLKLANE_READY_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, CLKLANE_READY_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_DAT1_READY_FLAG_SHIFT 2
+#define DSI_MCTL_MAIN_STS_FLAG_DAT1_READY_FLAG_MASK 0x00000004
+#define DSI_MCTL_MAIN_STS_FLAG_DAT1_READY_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, DAT1_READY_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_DAT2_READY_FLAG_SHIFT 3
+#define DSI_MCTL_MAIN_STS_FLAG_DAT2_READY_FLAG_MASK 0x00000008
+#define DSI_MCTL_MAIN_STS_FLAG_DAT2_READY_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, DAT2_READY_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_HSTX_TO_ERR_FLAG_SHIFT 4
+#define DSI_MCTL_MAIN_STS_FLAG_HSTX_TO_ERR_FLAG_MASK 0x00000010
+#define DSI_MCTL_MAIN_STS_FLAG_HSTX_TO_ERR_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, HSTX_TO_ERR_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_LPRX_TO_ERR_FLAG_SHIFT 5
+#define DSI_MCTL_MAIN_STS_FLAG_LPRX_TO_ERR_FLAG_MASK 0x00000020
+#define DSI_MCTL_MAIN_STS_FLAG_LPRX_TO_ERR_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, LPRX_TO_ERR_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_CRS_UNTERM_PCK_FLAG_SHIFT 6
+#define DSI_MCTL_MAIN_STS_FLAG_CRS_UNTERM_PCK_FLAG_MASK 0x00000040
+#define DSI_MCTL_MAIN_STS_FLAG_CRS_UNTERM_PCK_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, CRS_UNTERM_PCK_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_VRS_UNTERM_PCK_FLAG_SHIFT 7
+#define DSI_MCTL_MAIN_STS_FLAG_VRS_UNTERM_PCK_FLAG_MASK 0x00000080
+#define DSI_MCTL_MAIN_STS_FLAG_VRS_UNTERM_PCK_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, VRS_UNTERM_PCK_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG 0x00000134
+#define DSI_CMD_MODE_STS_FLAG_ERR_NO_TE_FLAG_SHIFT 0
+#define DSI_CMD_MODE_STS_FLAG_ERR_NO_TE_FLAG_MASK 0x00000001
+#define DSI_CMD_MODE_STS_FLAG_ERR_NO_TE_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, ERR_NO_TE_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG_ERR_TE_MISS_FLAG_SHIFT 1
+#define DSI_CMD_MODE_STS_FLAG_ERR_TE_MISS_FLAG_MASK 0x00000002
+#define DSI_CMD_MODE_STS_FLAG_ERR_TE_MISS_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, ERR_TE_MISS_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI1_UNDERRUN_FLAG_SHIFT 2
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI1_UNDERRUN_FLAG_MASK 0x00000004
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI1_UNDERRUN_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, ERR_SDI1_UNDERRUN_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI2_UNDERRUN_FLAG_SHIFT 3
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI2_UNDERRUN_FLAG_MASK 0x00000008
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI2_UNDERRUN_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, ERR_SDI2_UNDERRUN_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG_ERR_UNWANTED_RD_FLAG_SHIFT 4
+#define DSI_CMD_MODE_STS_FLAG_ERR_UNWANTED_RD_FLAG_MASK 0x00000010
+#define DSI_CMD_MODE_STS_FLAG_ERR_UNWANTED_RD_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, ERR_UNWANTED_RD_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG_CSM_RUNNING_FLAG_SHIFT 5
+#define DSI_CMD_MODE_STS_FLAG_CSM_RUNNING_FLAG_MASK 0x00000020
+#define DSI_CMD_MODE_STS_FLAG_CSM_RUNNING_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, CSM_RUNNING_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG 0x00000138
+#define DSI_DIRECT_CMD_STS_FLAG_CMD_TRANSMISSION_FLAG_SHIFT 0
+#define DSI_DIRECT_CMD_STS_FLAG_CMD_TRANSMISSION_FLAG_MASK 0x00000001
+#define DSI_DIRECT_CMD_STS_FLAG_CMD_TRANSMISSION_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, CMD_TRANSMISSION_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_WRITE_COMPLETED_FLAG_SHIFT 1
+#define DSI_DIRECT_CMD_STS_FLAG_WRITE_COMPLETED_FLAG_MASK 0x00000002
+#define DSI_DIRECT_CMD_STS_FLAG_WRITE_COMPLETED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, WRITE_COMPLETED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_COMPLETED_FLAG_SHIFT 2
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_COMPLETED_FLAG_MASK 0x00000004
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_COMPLETED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, TRIGGER_COMPLETED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_FLAG_SHIFT 3
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_FLAG_MASK 0x00000008
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, READ_COMPLETED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_RECEIVED_FLAG_SHIFT 4
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_RECEIVED_FLAG_MASK 0x00000010
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_RECEIVED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, ACKNOWLEDGE_RECEIVED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_WITH_ERR_RECEIVED_FLAG_SHIFT 5
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_WITH_ERR_RECEIVED_FLAG_MASK 0x00000020
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_WITH_ERR_RECEIVED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, ACKNOWLEDGE_WITH_ERR_RECEIVED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_RECEIVED_FLAG_SHIFT 6
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_RECEIVED_FLAG_MASK 0x00000040
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_RECEIVED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, TRIGGER_RECEIVED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_TE_RECEIVED_FLAG_SHIFT 7
+#define DSI_DIRECT_CMD_STS_FLAG_TE_RECEIVED_FLAG_MASK 0x00000080
+#define DSI_DIRECT_CMD_STS_FLAG_TE_RECEIVED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, TE_RECEIVED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_COMPLETED_FLAG_SHIFT 8
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_COMPLETED_FLAG_MASK 0x00000100
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_COMPLETED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, BTA_COMPLETED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_FINISHED_FLAG_SHIFT 9
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_FINISHED_FLAG_MASK 0x00000200
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_FINISHED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, BTA_FINISHED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_WITH_ERR_FLAG_SHIFT 10
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_WITH_ERR_FLAG_MASK 0x00000400
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_WITH_ERR_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, READ_COMPLETED_WITH_ERR_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG 0x0000013C
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_FIXED_FLAG_SHIFT 0
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_FIXED_FLAG_MASK 0x00000001
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_FIXED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_FIXED_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNCORRECTABLE_FLAG_SHIFT 1
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNCORRECTABLE_FLAG_MASK 0x00000002
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNCORRECTABLE_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_UNCORRECTABLE_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_CHECKSUM_FLAG_SHIFT 2
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_CHECKSUM_FLAG_MASK 0x00000004
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_CHECKSUM_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_CHECKSUM_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNDECODABLE_FLAG_SHIFT 3
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNDECODABLE_FLAG_MASK 0x00000008
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNDECODABLE_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_UNDECODABLE_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_RECEIVE_FLAG_SHIFT 4
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_RECEIVE_FLAG_MASK 0x00000010
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_RECEIVE_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_RECEIVE_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_OVERSIZE_FLAG_SHIFT 5
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_OVERSIZE_FLAG_MASK 0x00000020
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_OVERSIZE_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_OVERSIZE_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_WRONG_LENGTH_FLAG_SHIFT 6
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_WRONG_LENGTH_FLAG_MASK 0x00000040
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_WRONG_LENGTH_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_WRONG_LENGTH_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_MISSING_EOT_FLAG_SHIFT 7
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_MISSING_EOT_FLAG_MASK 0x00000080
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_MISSING_EOT_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_MISSING_EOT_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_EOT_WITH_ERR_FLAG_SHIFT 8
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_EOT_WITH_ERR_FLAG_MASK 0x00000100
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_EOT_WITH_ERR_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_EOT_WITH_ERR_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG 0x00000140
+#define DSI_VID_MODE_STS_FLAG_VSG_STS_FLAG_SHIFT 0
+#define DSI_VID_MODE_STS_FLAG_VSG_STS_FLAG_MASK 0x00000001
+#define DSI_VID_MODE_STS_FLAG_VSG_STS_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, VSG_STS_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_DATA_FLAG_SHIFT 1
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_DATA_FLAG_MASK 0x00000002
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_DATA_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_MISSING_DATA_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_HSYNC_FLAG_SHIFT 2
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_HSYNC_FLAG_MASK 0x00000004
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_HSYNC_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_MISSING_HSYNC_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_VSYNC_FLAG_SHIFT 3
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_VSYNC_FLAG_MASK 0x00000008
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_VSYNC_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_MISSING_VSYNC_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_LENGTH_FLAG_SHIFT 4
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_LENGTH_FLAG_MASK 0x00000010
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_LENGTH_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, REG_ERR_SMALL_LENGTH_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_HEIGHT_FLAG_SHIFT 5
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_HEIGHT_FLAG_MASK 0x00000020
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_HEIGHT_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, REG_ERR_SMALL_HEIGHT_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_BURSTWRITE_FLAG_SHIFT 6
+#define DSI_VID_MODE_STS_FLAG_ERR_BURSTWRITE_FLAG_MASK 0x00000040
+#define DSI_VID_MODE_STS_FLAG_ERR_BURSTWRITE_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_BURSTWRITE_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGWRITE_FLAG_SHIFT 7
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGWRITE_FLAG_MASK 0x00000080
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGWRITE_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_LONGWRITE_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGREAD_FLAG_SHIFT 8
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGREAD_FLAG_MASK 0x00000100
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGREAD_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_LONGREAD_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_VRS_WRONG_LENGTH_FLAG_SHIFT 9
+#define DSI_VID_MODE_STS_FLAG_ERR_VRS_WRONG_LENGTH_FLAG_MASK 0x00000200
+#define DSI_VID_MODE_STS_FLAG_ERR_VRS_WRONG_LENGTH_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_VRS_WRONG_LENGTH_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_VSG_RECOVERY_FLAG_SHIFT 10
+#define DSI_VID_MODE_STS_FLAG_VSG_RECOVERY_FLAG_MASK 0x00000400
+#define DSI_VID_MODE_STS_FLAG_VSG_RECOVERY_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, VSG_RECOVERY_FLAG, __x)
+#define DSI_TG_STS_FLAG 0x00000144
+#define DSI_TG_STS_FLAG_TVG_STS_FLAG_SHIFT 0
+#define DSI_TG_STS_FLAG_TVG_STS_FLAG_MASK 0x00000001
+#define DSI_TG_STS_FLAG_TVG_STS_FLAG(__x) \
+	DSI_VAL2REG(DSI_TG_STS_FLAG, TVG_STS_FLAG, __x)
+#define DSI_TG_STS_FLAG_TBG_STS_FLAG_SHIFT 1
+#define DSI_TG_STS_FLAG_TBG_STS_FLAG_MASK 0x00000002
+#define DSI_TG_STS_FLAG_TBG_STS_FLAG(__x) \
+	DSI_VAL2REG(DSI_TG_STS_FLAG, TBG_STS_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG 0x00000148
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_1_FLAG_SHIFT 6
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_1_FLAG_MASK 0x00000040
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_1_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_ESC_1_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_2_FLAG_SHIFT 7
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_2_FLAG_MASK 0x00000080
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_2_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_ESC_2_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_1_FLAG_SHIFT 8
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_1_FLAG_MASK 0x00000100
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_1_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_SYNCESC_1_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_2_FLAG_SHIFT 9
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_2_FLAG_MASK 0x00000200
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_2_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_SYNCESC_2_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_1_FLAG_SHIFT 10
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_1_FLAG_MASK 0x00000400
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_1_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONTROL_1_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_2_FLAG_SHIFT 11
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_2_FLAG_MASK 0x00000800
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_2_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONTROL_2_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_1_FLAG_SHIFT 12
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_1_FLAG_MASK 0x00001000
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_1_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONT_LP0_1_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_2_FLAG_SHIFT 13
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_2_FLAG_MASK 0x00002000
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_2_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONT_LP0_2_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_1_FLAG_SHIFT 14
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_1_FLAG_MASK 0x00004000
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_1_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONT_LP1_1_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_2_FLAG_SHIFT 15
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_2_FLAG_MASK 0x00008000
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_2_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONT_LP1_2_FLAG, __x)
+#define DSI_DPHY_LANES_TRIM 0x00000150
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT1_SHIFT 0
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT1_MASK 0x00000003
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT1(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_SKEW_DAT1, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_CD_OFF_DAT1_SHIFT 2
+#define DSI_DPHY_LANES_TRIM_DPHY_CD_OFF_DAT1_MASK 0x00000004
+#define DSI_DPHY_LANES_TRIM_DPHY_CD_OFF_DAT1(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_CD_OFF_DAT1, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT1_SHIFT 3
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT1_MASK 0x00000008
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT1(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_UP_DAT1, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT1_SHIFT 4
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT1_MASK 0x00000010
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT1(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_DOWN_DAT1, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT1_SHIFT 5
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT1_MASK 0x00000020
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT1(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_TEST_RESERVED_1_DAT1, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_CLK_SHIFT 6
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_CLK_MASK 0x000000C0
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_SKEW_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_RX_VIL_CLK_SHIFT 8
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_RX_VIL_CLK_MASK 0x00000300
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_RX_VIL_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_LP_RX_VIL_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_TX_SLEWRATE_CLK_SHIFT 10
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_TX_SLEWRATE_CLK_MASK 0x00000C00
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_TX_SLEWRATE_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_LP_TX_SLEWRATE_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_SHIFT 12
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_MASK 0x00001000
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_0_81 0
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_0_90 1
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_ENUM(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_SPECS_90_81B, \
+	DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_##__x)
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_SPECS_90_81B, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_CLK_SHIFT 13
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_CLK_MASK 0x00002000
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_UP_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_CLK_SHIFT 14
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_CLK_MASK 0x00004000
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_DOWN_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_CLK_SHIFT 15
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_CLK_MASK 0x00008000
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_TEST_RESERVED_1_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT2_SHIFT 16
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT2_MASK 0x00030000
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT2(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_SKEW_DAT2, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT2_SHIFT 18
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT2_MASK 0x00040000
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT2(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_UP_DAT2, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT2_SHIFT 19
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT2_MASK 0x00080000
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT2(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_DOWN_DAT2, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT2_SHIFT 20
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT2_MASK 0x00100000
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT2(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_TEST_RESERVED_1_DAT2, __x)
+#define DSI_ID_REG 0x00000FF0
+#define DSI_ID_REG_Y_SHIFT 0
+#define DSI_ID_REG_Y_MASK 0x0000000F
+#define DSI_ID_REG_Y(__x) \
+	DSI_VAL2REG(DSI_ID_REG, Y, __x)
+#define DSI_ID_REG_X_SHIFT 4
+#define DSI_ID_REG_X_MASK 0x000000F0
+#define DSI_ID_REG_X(__x) \
+	DSI_VAL2REG(DSI_ID_REG, X, __x)
+#define DSI_ID_REG_H_SHIFT 8
+#define DSI_ID_REG_H_MASK 0x00000300
+#define DSI_ID_REG_H(__x) \
+	DSI_VAL2REG(DSI_ID_REG, H, __x)
+#define DSI_ID_REG_PRODUCT_ID_SHIFT 10
+#define DSI_ID_REG_PRODUCT_ID_MASK 0x0003FC00
+#define DSI_ID_REG_PRODUCT_ID(__x) \
+	DSI_VAL2REG(DSI_ID_REG, PRODUCT_ID, __x)
+#define DSI_ID_REG_VENDOR_ID_SHIFT 18
+#define DSI_ID_REG_VENDOR_ID_MASK 0xFFFC0000
+#define DSI_ID_REG_VENDOR_ID(__x) \
+	DSI_VAL2REG(DSI_ID_REG, VENDOR_ID, __x)
+#define DSI_IP_CONF 0x00000FF4
+#define DSI_IP_CONF_FIFO_SIZE_SHIFT 0
+#define DSI_IP_CONF_FIFO_SIZE_MASK 0x0000003F
+#define DSI_IP_CONF_FIFO_SIZE(__x) \
+	DSI_VAL2REG(DSI_IP_CONF, FIFO_SIZE, __x)
diff --git a/drivers/video/mcde/mcde_bus.c b/drivers/video/mcde/mcde_bus.c
new file mode 100644
index 00000000000..3d7049652a1
--- /dev/null
+++ b/drivers/video/mcde/mcde_bus.c
@@ -0,0 +1,261 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson MCDE display bus driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/notifier.h>
+
+#include <video/mcde_display.h>
+#include <video/mcde_dss.h>
+
+#define to_mcde_display_driver(__drv) \
+	container_of((__drv), struct mcde_display_driver, driver)
+
+static BLOCKING_NOTIFIER_HEAD(bus_notifier_list);
+
+static int mcde_drv_suspend(struct device *_dev, pm_message_t state);
+static int mcde_drv_resume(struct device *_dev);
+struct bus_type mcde_bus_type;
+
+static int mcde_suspend_device(struct device *dev, void *data)
+{
+	pm_message_t* state = (pm_message_t *) data;
+	if (dev->driver->suspend)
+		return dev->driver->suspend(dev, *state);
+	return 0;
+}
+
+static int mcde_resume_device(struct device *dev, void *data)
+{
+	if (dev->driver->resume)
+		return dev->driver->resume(dev);
+	return 0;
+}
+
+/* Bus driver */
+
+static int mcde_bus_match(struct device *_dev, struct device_driver *driver)
+{
+	pr_debug("Matching device %s with driver %s\n",
+		dev_name(_dev), driver->name);
+
+	return strncmp(dev_name(_dev), driver->name, strlen(driver->name)) == 0;
+}
+
+static int mcde_bus_suspend(struct device *_dev, pm_message_t state)
+{
+	int ret;
+	ret = bus_for_each_dev(&mcde_bus_type, NULL, &state,
+				mcde_suspend_device);
+	if (ret) {
+		/* TODO Resume all suspended devices */
+		/* mcde_bus_resume(dev); */
+		return ret;
+	}
+	return 0;
+}
+
+static int mcde_bus_resume(struct device *_dev)
+{
+	return bus_for_each_dev(&mcde_bus_type, NULL, NULL, mcde_resume_device);
+}
+
+struct bus_type mcde_bus_type = {
+	.name = "mcde_bus",
+	.match = mcde_bus_match,
+	.suspend = mcde_bus_suspend,
+	.resume = mcde_bus_resume,
+};
+
+static int mcde_drv_probe(struct device *_dev)
+{
+	struct mcde_display_driver *drv = to_mcde_display_driver(_dev->driver);
+	struct mcde_display_device *dev = to_mcde_display_device(_dev);
+
+	return drv->probe(dev);
+}
+
+static int mcde_drv_remove(struct device *_dev)
+{
+	struct mcde_display_driver *drv = to_mcde_display_driver(_dev->driver);
+	struct mcde_display_device *dev = to_mcde_display_device(_dev);
+
+	return drv->remove(dev);
+}
+
+static void mcde_drv_shutdown(struct device *_dev)
+{
+	struct mcde_display_driver *drv = to_mcde_display_driver(_dev->driver);
+	struct mcde_display_device *dev = to_mcde_display_device(_dev);
+
+	drv->shutdown(dev);
+}
+
+static int mcde_drv_suspend(struct device *_dev, pm_message_t state)
+{
+	struct mcde_display_driver *drv = to_mcde_display_driver(_dev->driver);
+	struct mcde_display_device *dev = to_mcde_display_device(_dev);
+
+	return drv->suspend(dev, state);
+}
+
+static int mcde_drv_resume(struct device *_dev)
+{
+	struct mcde_display_driver *drv = to_mcde_display_driver(_dev->driver);
+	struct mcde_display_device *dev = to_mcde_display_device(_dev);
+
+	return drv->resume(dev);
+}
+
+/* Bus device */
+
+static void mcde_bus_release(struct device *dev)
+{
+}
+
+struct device mcde_bus = {
+	.init_name = "mcde_bus",
+	.release  = mcde_bus_release
+};
+
+/* Public bus API */
+
+int mcde_display_driver_register(struct mcde_display_driver *drv)
+{
+	drv->driver.bus = &mcde_bus_type;
+	if (drv->probe)
+		drv->driver.probe = mcde_drv_probe;
+	if (drv->remove)
+		drv->driver.remove = mcde_drv_remove;
+	if (drv->shutdown)
+		drv->driver.shutdown = mcde_drv_shutdown;
+	if (drv->suspend)
+		drv->driver.suspend = mcde_drv_suspend;
+	if (drv->resume)
+		drv->driver.resume = mcde_drv_resume;
+
+	return driver_register(&drv->driver);
+}
+EXPORT_SYMBOL(mcde_display_driver_register);
+
+void mcde_display_driver_unregister(struct mcde_display_driver *drv)
+{
+	driver_unregister(&drv->driver);
+}
+EXPORT_SYMBOL(mcde_display_driver_unregister);
+
+static void mcde_display_dev_release(struct device *dev)
+{
+	/* Do nothing */
+}
+
+int mcde_display_device_register(struct mcde_display_device *dev)
+{
+	/* Setup device */
+	if (!dev)
+		return -EINVAL;
+	dev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+	dev->dev.bus = &mcde_bus_type;
+	if (dev->dev.parent != NULL)
+		dev->dev.parent = &mcde_bus;
+	dev->dev.release = mcde_display_dev_release;
+	if (dev->id != -1)
+		dev_set_name(&dev->dev, "%s.%d", dev->name,  dev->id);
+	else
+		dev_set_name(&dev->dev, dev->name);
+
+	mcde_display_init_device(dev);
+
+	return device_register(&dev->dev);
+}
+EXPORT_SYMBOL(mcde_display_device_register);
+
+void mcde_display_device_unregister(struct mcde_display_device *dev)
+{
+	device_unregister(&dev->dev);
+}
+EXPORT_SYMBOL(mcde_display_device_unregister);
+
+/* Notifications */
+int mcde_dss_register_notifier(struct notifier_block *nb)
+{
+	return blocking_notifier_chain_register(&bus_notifier_list, nb);
+}
+EXPORT_SYMBOL(mcde_dss_register_notifier);
+
+int mcde_dss_unregister_notifier(struct notifier_block *nb)
+{
+	return blocking_notifier_chain_unregister(&bus_notifier_list, nb);
+}
+EXPORT_SYMBOL(mcde_dss_unregister_notifier);
+
+static int bus_notify_callback(struct notifier_block *nb,
+	unsigned long event, void *dev)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+
+	if (event == BUS_NOTIFY_BOUND_DRIVER) {
+		ddev->initialized = true;
+		blocking_notifier_call_chain(&bus_notifier_list,
+			MCDE_DSS_EVENT_DISPLAY_REGISTERED, ddev);
+	} else if (event == BUS_NOTIFY_UNBIND_DRIVER) {
+		ddev->initialized = false;
+		blocking_notifier_call_chain(&bus_notifier_list,
+			MCDE_DSS_EVENT_DISPLAY_UNREGISTERED, ddev);
+	}
+	return 0;
+}
+
+struct notifier_block bus_nb = {
+	.notifier_call = bus_notify_callback,
+};
+
+/* Driver init/exit */
+
+int __init mcde_display_init(void)
+{
+	int ret;
+
+	ret = bus_register(&mcde_bus_type);
+	if (ret) {
+		pr_warning("Unable to register bus type\n");
+		goto no_bus_registration;
+	}
+	ret = device_register(&mcde_bus);
+	if (ret) {
+		pr_warning("Unable to register bus device\n");
+		goto no_device_registration;
+	}
+	ret = bus_register_notifier(&mcde_bus_type, &bus_nb);
+	if (ret) {
+		pr_warning("Unable to register bus notifier\n");
+		goto no_bus_notifier;
+	}
+
+	goto out;
+
+no_bus_notifier:
+	device_unregister(&mcde_bus);
+no_device_registration:
+	bus_unregister(&mcde_bus_type);
+no_bus_registration:
+out:
+	return ret;
+}
+
+void mcde_display_exit(void)
+{
+	bus_unregister_notifier(&mcde_bus_type, &bus_nb);
+	device_unregister(&mcde_bus);
+	bus_unregister(&mcde_bus_type);
+}
diff --git a/drivers/video/mcde/mcde_display.c b/drivers/video/mcde/mcde_display.c
new file mode 100644
index 00000000000..04c0f1cebb1
--- /dev/null
+++ b/drivers/video/mcde/mcde_display.c
@@ -0,0 +1,432 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE display driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+
+#include <video/mcde_display.h>
+
+/*temp*/
+#include <linux/delay.h>
+
+static void mcde_display_get_native_resolution_default(
+	struct mcde_display_device *ddev, u16 *x_res, u16 *y_res)
+{
+	if (x_res)
+		*x_res = ddev->native_x_res;
+	if (y_res)
+		*y_res = ddev->native_y_res;
+}
+
+static enum mcde_ovly_pix_fmt mcde_display_get_default_pixel_format_default(
+	struct mcde_display_device *ddev)
+{
+	return ddev->default_pixel_format;
+}
+
+static void mcde_display_get_physical_size_default(
+	struct mcde_display_device *ddev, u16 *width, u16 *height)
+{
+	if (width)
+		*width = ddev->physical_width;
+	if (height)
+		*height = ddev->physical_height;
+}
+
+static int mcde_display_set_power_mode_default(struct mcde_display_device *ddev,
+	enum mcde_display_power_mode power_mode)
+{
+	int ret = 0;
+
+	/* OFF -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_OFF &&
+		power_mode != MCDE_DISPLAY_PM_OFF) {
+		if (ddev->platform_enable) {
+			ret = ddev->platform_enable(ddev);
+			if (ret)
+				return ret;
+		}
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+		/* force register settings */
+		if (ddev->port->type == MCDE_PORTTYPE_DPI)
+			ddev->update_flags = UPDATE_FLAG_VIDEO_MODE | UPDATE_FLAG_PIXEL_FORMAT;
+	}
+
+	if (ddev->port->type == MCDE_PORTTYPE_DSI) {
+		/* STANDBY -> ON */
+		if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+			power_mode == MCDE_DISPLAY_PM_ON) {
+			ret = mcde_dsi_dcs_write(ddev->chnl_state,
+				DCS_CMD_EXIT_SLEEP_MODE, NULL, 0);
+			if (ret)
+				return ret;
+
+			ret = mcde_dsi_dcs_write(ddev->chnl_state,
+				DCS_CMD_SET_DISPLAY_ON, NULL, 0);
+			if (ret)
+				return ret;
+
+			ddev->power_mode = MCDE_DISPLAY_PM_ON;
+		} else if (ddev->power_mode == MCDE_DISPLAY_PM_ON &&
+			power_mode <= MCDE_DISPLAY_PM_STANDBY) {
+			/* ON -> STANDBY */
+			ret = mcde_dsi_dcs_write(ddev->chnl_state,
+				DCS_CMD_SET_DISPLAY_OFF, NULL, 0);
+			if (ret)
+				return ret;
+
+			ret = mcde_dsi_dcs_write(ddev->chnl_state,
+				DCS_CMD_ENTER_SLEEP_MODE, NULL, 0);
+			if (ret)
+				return ret;
+
+			ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+		}
+	} else if (ddev->port->type == MCDE_PORTTYPE_DPI) {
+		ddev->power_mode = power_mode;
+	} else if (ddev->power_mode != power_mode) {
+		return -EINVAL;
+	}
+
+	/* SLEEP -> OFF */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+		power_mode == MCDE_DISPLAY_PM_OFF) {
+		if (ddev->platform_disable) {
+			ret = ddev->platform_disable(ddev);
+			if (ret)
+				return ret;
+		}
+		ddev->power_mode = MCDE_DISPLAY_PM_OFF;
+	}
+
+	mcde_chnl_set_power_mode(ddev->chnl_state, ddev->power_mode);
+
+	return ret;
+}
+
+static inline enum mcde_display_power_mode mcde_display_get_power_mode_default(
+	struct mcde_display_device *ddev)
+{
+	return ddev->power_mode;
+}
+
+static inline int mcde_display_try_video_mode_default(
+	struct mcde_display_device *ddev,
+	struct mcde_video_mode *video_mode)
+{
+	/* TODO Check if inside native_xres and native_yres */
+	return 0;
+}
+
+static int mcde_display_set_video_mode_default(struct mcde_display_device *ddev,
+	struct mcde_video_mode *video_mode)
+{
+	int ret;
+	struct mcde_video_mode channel_video_mode;
+
+	if (!video_mode)
+		return -EINVAL;
+
+	ddev->video_mode = *video_mode;
+	channel_video_mode = ddev->video_mode;
+	/* Dependant on if display should rotate or MCDE should rotate */
+	if (ddev->rotation == MCDE_DISPLAY_ROT_90_CCW ||
+				ddev->rotation == MCDE_DISPLAY_ROT_90_CW) {
+		channel_video_mode.xres = ddev->native_x_res;
+		channel_video_mode.yres = ddev->native_y_res;
+	}
+	ret = mcde_chnl_set_video_mode(ddev->chnl_state, &channel_video_mode);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to set video mode\n", __func__);
+		return ret;
+	}
+
+	ddev->update_flags |= UPDATE_FLAG_VIDEO_MODE;
+
+	return 0;
+}
+
+static inline void mcde_display_get_video_mode_default(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	if (video_mode)
+		*video_mode = ddev->video_mode;
+}
+
+static int mcde_display_set_pixel_format_default(
+	struct mcde_display_device *ddev, enum mcde_ovly_pix_fmt format)
+{
+	int ret;
+
+	ddev->pixel_format = format;
+	ret = mcde_chnl_set_pixel_format(ddev->chnl_state,
+						ddev->port->pixel_format);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to set pixel format = %d\n",
+							__func__, format);
+		return ret;
+	}
+
+	return 0;
+}
+
+static inline enum mcde_ovly_pix_fmt mcde_display_get_pixel_format_default(
+	struct mcde_display_device *ddev)
+{
+	return ddev->pixel_format;
+}
+
+
+static int mcde_display_set_rotation_default(struct mcde_display_device *ddev,
+	enum mcde_display_rotation rotation)
+{
+	int ret;
+
+	ret = mcde_chnl_set_rotation(ddev->chnl_state, rotation,
+		ddev->rotbuf1, ddev->rotbuf2);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to set rotation = %d\n",
+							__func__, rotation);
+		return ret;
+	}
+
+	if (rotation == MCDE_DISPLAY_ROT_180_CCW) {
+		u8 param = 0x40;
+		(void) mcde_dsi_dcs_write(ddev->chnl_state,
+				DCS_CMD_SET_ADDRESS_MODE, &param, 1);
+	} else if (ddev->rotation == MCDE_DISPLAY_ROT_180_CCW &&
+			rotation != MCDE_DISPLAY_ROT_180_CCW) {
+		u8 param = 0;
+		(void) mcde_dsi_dcs_write(ddev->chnl_state,
+				DCS_CMD_SET_ADDRESS_MODE, &param, 1);
+	}
+
+	ddev->rotation = rotation;
+	ddev->update_flags |= UPDATE_FLAG_ROTATION;
+
+	return 0;
+}
+
+static inline enum mcde_display_rotation mcde_display_get_rotation_default(
+	struct mcde_display_device *ddev)
+{
+	return ddev->rotation;
+}
+
+static int mcde_display_set_synchronized_update_default(
+	struct mcde_display_device *ddev, bool enable)
+{
+	if (ddev->port->type == MCDE_PORTTYPE_DSI && enable) {
+		int ret;
+		u8 m = 0;
+
+		if (ddev->port->sync_src == MCDE_SYNCSRC_OFF)
+			return -EINVAL;
+
+		ret = mcde_dsi_dcs_write(ddev->chnl_state,
+						DCS_CMD_SET_TEAR_ON, &m, 1);
+		if (ret < 0) {
+			dev_warn(&ddev->dev,
+				"%s:Failed to set synchornized update = %d\n",
+				__func__, enable);
+			return ret;
+		}
+	}
+	ddev->synchronized_update = enable;
+	return 0;
+}
+
+static inline bool mcde_display_get_synchronized_update_default(
+	struct mcde_display_device *ddev)
+{
+	return ddev->synchronized_update;
+}
+
+static int mcde_display_apply_config_default(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	ret = mcde_chnl_enable_synchronized_update(ddev->chnl_state,
+		ddev->synchronized_update);
+
+	if (ret < 0) {
+		dev_warn(&ddev->dev,
+			"%s:Failed to enable synchronized update\n",
+			__func__);
+		return ret;
+	}
+
+	if (!ddev->update_flags)
+		return 0;
+
+	if (ddev->update_flags & UPDATE_FLAG_VIDEO_MODE)
+		mcde_chnl_stop_flow(ddev->chnl_state);
+
+	ret = mcde_chnl_apply(ddev->chnl_state);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to apply to channel\n",
+							__func__);
+		return ret;
+	}
+	ddev->update_flags = 0;
+	ddev->first_update = true;
+
+	return 0;
+}
+
+static int mcde_display_invalidate_area_default(
+					struct mcde_display_device *ddev,
+					struct mcde_rectangle *area)
+{
+	dev_vdbg(&ddev->dev, "%s\n", __func__);
+	if (area) {
+		/* take union of rects */
+		u16 t;
+		t = min(ddev->update_area.x, area->x);
+		/* note should be > 0 */
+		ddev->update_area.w = max(ddev->update_area.x +
+							ddev->update_area.w,
+							area->x + area->w) - t;
+		ddev->update_area.x = t;
+		t = min(ddev->update_area.y, area->y);
+		ddev->update_area.h = max(ddev->update_area.y +
+							ddev->update_area.h,
+							area->y + area->h) - t;
+		ddev->update_area.y = t;
+		/* TODO: Implement real clipping when partial refresh is
+		activated.*/
+		ddev->update_area.w = min((u16) ddev->video_mode.xres,
+					(u16) ddev->update_area.w);
+		ddev->update_area.h = min((u16) ddev->video_mode.yres,
+					(u16) ddev->update_area.h);
+	} else {
+		ddev->update_area.x = 0;
+		ddev->update_area.y = 0;
+		ddev->update_area.w = ddev->video_mode.xres;
+		ddev->update_area.h = ddev->video_mode.yres;
+		/* Invalidate_area(ddev, NULL) means reset area to empty
+		 * rectangle really. After that the rectangle should grow by
+		 * taking an union (above). This means that the code should
+		 * really look like below, however the code above is a temp fix
+		 * for rotation.
+		 * TODO: fix
+		 * ddev->update_area.x = ddev->video_mode.xres;
+		 * ddev->update_area.y = ddev->video_mode.yres;
+		 * ddev->update_area.w = 0;
+		 * ddev->update_area.h = 0;
+		 */
+	}
+
+	return 0;
+}
+
+static int mcde_display_update_default(struct mcde_display_device *ddev,
+							bool tripple_buffer)
+{
+	int ret = 0;
+
+	/* TODO: Dirty */
+	if (ddev->prepare_for_update) {
+		/* TODO: Send dirty rectangle */
+		ret = ddev->prepare_for_update(ddev, 0, 0,
+			ddev->native_x_res, ddev->native_y_res);
+		if (ret < 0) {
+			dev_warn(&ddev->dev,
+				"%s:Failed to prepare for update\n", __func__);
+			return ret;
+		}
+	}
+	/* TODO: Calculate & set update rect */
+	ret = mcde_chnl_update(ddev->chnl_state, &ddev->update_area,
+							tripple_buffer);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to update channel\n", __func__);
+		return ret;
+	}
+	if (ddev->first_update && ddev->on_first_update)
+		ddev->on_first_update(ddev);
+
+	if (ddev->power_mode != MCDE_DISPLAY_PM_ON && ddev->set_power_mode) {
+		ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_ON);
+		if (ret < 0) {
+			dev_warn(&ddev->dev,
+				"%s:Failed to set power mode to on\n",
+				__func__);
+			return ret;
+		}
+	}
+
+	dev_vdbg(&ddev->dev, "Overlay updated, chnl=%d\n", ddev->chnl_id);
+
+	return 0;
+}
+
+static int mcde_display_prepare_for_update_default(
+					struct mcde_display_device *ddev,
+					u16 x, u16 y, u16 w, u16 h)
+{
+	int ret;
+	u8 params[8] = { x >> 8, x & 0xff,
+			(x + w - 1) >> 8, (x + w - 1) & 0xff,
+			 y >> 8, y & 0xff,
+			(y + h - 1) >> 8, (y + h - 1) & 0xff };
+
+	if (ddev->port->type != MCDE_PORTTYPE_DSI)
+		return -EINVAL;
+
+	ret = mcde_dsi_dcs_write(ddev->chnl_state,
+		DCS_CMD_SET_COLUMN_ADDRESS, &params[0], 4);
+	if (ret)
+		return ret;
+
+	ret = mcde_dsi_dcs_write(ddev->chnl_state,
+		DCS_CMD_SET_PAGE_ADDRESS, &params[4], 4);
+
+	return ret;
+}
+
+static inline int mcde_display_on_first_update_default(
+					struct mcde_display_device *ddev)
+{
+	ddev->first_update = false;
+	return 0;
+}
+
+void mcde_display_init_device(struct mcde_display_device *ddev)
+{
+	/* Setup default callbacks */
+	ddev->get_native_resolution =
+				mcde_display_get_native_resolution_default;
+	ddev->get_default_pixel_format =
+				mcde_display_get_default_pixel_format_default;
+	ddev->get_physical_size = mcde_display_get_physical_size_default;
+	ddev->set_power_mode = mcde_display_set_power_mode_default;
+	ddev->get_power_mode = mcde_display_get_power_mode_default;
+	ddev->try_video_mode = mcde_display_try_video_mode_default;
+	ddev->set_video_mode = mcde_display_set_video_mode_default;
+	ddev->get_video_mode = mcde_display_get_video_mode_default;
+	ddev->set_pixel_format = mcde_display_set_pixel_format_default;
+	ddev->get_pixel_format = mcde_display_get_pixel_format_default;
+	ddev->set_rotation = mcde_display_set_rotation_default;
+	ddev->get_rotation = mcde_display_get_rotation_default;
+	ddev->set_synchronized_update =
+				mcde_display_set_synchronized_update_default;
+	ddev->get_synchronized_update =
+				mcde_display_get_synchronized_update_default;
+	ddev->apply_config = mcde_display_apply_config_default;
+	ddev->invalidate_area = mcde_display_invalidate_area_default;
+	ddev->update = mcde_display_update_default;
+	ddev->prepare_for_update = mcde_display_prepare_for_update_default;
+	ddev->on_first_update = mcde_display_on_first_update_default;
+
+	mutex_init(&ddev->display_lock);
+}
+
diff --git a/drivers/video/mcde/mcde_dss.c b/drivers/video/mcde/mcde_dss.c
new file mode 100644
index 00000000000..2c5e4db20c6
--- /dev/null
+++ b/drivers/video/mcde/mcde_dss.c
@@ -0,0 +1,475 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE display sub system driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+
+#include <video/mcde_dss.h>
+
+#define to_overlay(x) container_of(x, struct mcde_overlay, kobj)
+
+void overlay_release(struct kobject *kobj)
+{
+	struct mcde_overlay *ovly = to_overlay(kobj);
+
+	kfree(ovly);
+}
+
+struct kobj_type ovly_type = {
+	.release = overlay_release,
+};
+
+static int apply_overlay(struct mcde_overlay *ovly,
+				struct mcde_overlay_info *info, bool force)
+{
+	int ret = 0;
+	if (ovly->ddev->invalidate_area) {
+		/* TODO: transform ovly coord to screen coords (vmode):
+		 * add offset
+		 */
+		struct mcde_rectangle dirty = info->dirty;
+		mutex_lock(&ovly->ddev->display_lock);
+		ret = ovly->ddev->invalidate_area(ovly->ddev, &dirty);
+		mutex_unlock(&ovly->ddev->display_lock);
+	}
+
+	if (ovly->info.paddr != info->paddr || force)
+		mcde_ovly_set_source_buf(ovly->state, info->paddr);
+
+	if (ovly->info.stride != info->stride || ovly->info.fmt != info->fmt ||
+									force)
+		mcde_ovly_set_source_info(ovly->state, info->stride, info->fmt);
+	if (ovly->info.src_x != info->src_x ||
+					ovly->info.src_y != info->src_y ||
+					ovly->info.w != info->w ||
+					ovly->info.h != info->h || force)
+		mcde_ovly_set_source_area(ovly->state,
+				info->src_x, info->src_y, info->w, info->h);
+	if (ovly->info.dst_x != info->dst_x || ovly->info.dst_y != info->dst_y
+					|| ovly->info.dst_z != info->dst_z ||
+					force)
+		mcde_ovly_set_dest_pos(ovly->state,
+					info->dst_x, info->dst_y, info->dst_z);
+
+	mcde_ovly_apply(ovly->state);
+	ovly->info = *info;
+
+	return ret;
+}
+
+/* MCDE DSS operations */
+
+int mcde_dss_open_channel(struct mcde_display_device *ddev)
+{
+	int ret = 0;
+	struct mcde_chnl_state *chnl;
+
+	mutex_lock(&ddev->display_lock);
+	/* Acquire MCDE resources */
+	chnl = mcde_chnl_get(ddev->chnl_id, ddev->fifo, ddev->port);
+	if (IS_ERR(chnl)) {
+		ret = PTR_ERR(chnl);
+		dev_warn(&ddev->dev, "Failed to acquire MCDE channel\n");
+		goto chnl_get_failed;
+	}
+	ddev->chnl_state = chnl;
+chnl_get_failed:
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_open_channel);
+
+void mcde_dss_close_channel(struct mcde_display_device *ddev)
+{
+	mutex_lock(&ddev->display_lock);
+	mcde_chnl_put(ddev->chnl_state);
+	ddev->chnl_state = NULL;
+	mutex_unlock(&ddev->display_lock);
+}
+EXPORT_SYMBOL(mcde_dss_close_channel);
+
+int mcde_dss_enable_display(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	if (ddev->enabled)
+		return 0;
+
+	mutex_lock(&ddev->display_lock);
+	mcde_chnl_enable(ddev->chnl_state);
+
+	/* Initiate display communication */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "Failed to initialize display\n");
+		goto display_failed;
+	}
+
+	ret = ddev->set_synchronized_update(ddev,
+					ddev->get_synchronized_update(ddev));
+	if (ret < 0)
+		dev_warn(&ddev->dev, "Failed to set sync\n");
+
+	ret = mcde_chnl_enable_synchronized_update(ddev->chnl_state,
+		ddev->synchronized_update);
+	if (ret < 0) {
+		dev_warn(&ddev->dev,
+			"%s:Failed to enable synchronized update\n",
+			__func__);
+		goto enable_sync_failed;
+	}
+	/* TODO: call driver for all defaults like sync_update above */
+
+	dev_dbg(&ddev->dev, "Display enabled, chnl=%d\n",
+					ddev->chnl_id);
+	ddev->enabled = true;
+	mutex_unlock(&ddev->display_lock);
+
+	return 0;
+
+enable_sync_failed:
+	ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+display_failed:
+	mcde_chnl_disable(ddev->chnl_state);
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_enable_display);
+
+void mcde_dss_disable_display(struct mcde_display_device *ddev)
+{
+	if (!ddev->enabled)
+		return;
+
+	/* TODO: Disable overlays */
+	mutex_lock(&ddev->display_lock);
+
+	mcde_chnl_stop_flow(ddev->chnl_state);
+
+	(void)ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+
+	mcde_chnl_disable(ddev->chnl_state);
+
+	ddev->enabled = false;
+	mutex_unlock(&ddev->display_lock);
+
+	dev_dbg(&ddev->dev, "Display disabled, chnl=%d\n", ddev->chnl_id);
+}
+EXPORT_SYMBOL(mcde_dss_disable_display);
+
+int mcde_dss_apply_channel(struct mcde_display_device *ddev)
+{
+	int ret;
+	if (!ddev->apply_config)
+		return -EINVAL;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->apply_config(ddev);
+	mutex_unlock(&ddev->display_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_apply_channel);
+
+struct mcde_overlay *mcde_dss_create_overlay(struct mcde_display_device *ddev,
+	struct mcde_overlay_info *info)
+{
+	struct mcde_overlay *ovly;
+
+	ovly = kzalloc(sizeof(struct mcde_overlay), GFP_KERNEL);
+	if (!ovly)
+		return NULL;
+
+	kobject_init(&ovly->kobj, &ovly_type); /* Local ref */
+	kobject_get(&ovly->kobj); /* Creator ref */
+	INIT_LIST_HEAD(&ovly->list);
+	mutex_lock(&ddev->display_lock);
+	list_add(&ddev->ovlys, &ovly->list);
+	mutex_unlock(&ddev->display_lock);
+	ovly->info = *info;
+	ovly->ddev = ddev;
+
+	return ovly;
+}
+EXPORT_SYMBOL(mcde_dss_create_overlay);
+
+void mcde_dss_destroy_overlay(struct mcde_overlay *ovly)
+{
+	list_del(&ovly->list);
+	if (ovly->state)
+		mcde_dss_disable_overlay(ovly);
+	kobject_put(&ovly->kobj);
+}
+EXPORT_SYMBOL(mcde_dss_destroy_overlay);
+
+int mcde_dss_enable_overlay(struct mcde_overlay *ovly)
+{
+	int ret;
+
+	if (!ovly->ddev->chnl_state)
+		return -EINVAL;
+
+	if (!ovly->state) {
+		struct mcde_ovly_state *state;
+		state = mcde_ovly_get(ovly->ddev->chnl_state);
+		if (IS_ERR(state)) {
+			ret = PTR_ERR(state);
+			dev_warn(&ovly->ddev->dev,
+				"Failed to acquire overlay\n");
+			return ret;
+		}
+		ovly->state = state;
+	}
+
+	apply_overlay(ovly, &ovly->info, true);
+
+	dev_vdbg(&ovly->ddev->dev, "Overlay enabled, chnl=%d\n",
+							ovly->ddev->chnl_id);
+	return 0;
+}
+EXPORT_SYMBOL(mcde_dss_enable_overlay);
+
+int mcde_dss_apply_overlay(struct mcde_overlay *ovly,
+						struct mcde_overlay_info *info)
+{
+	if (info == NULL)
+		info = &ovly->info;
+	return apply_overlay(ovly, info, false);
+}
+EXPORT_SYMBOL(mcde_dss_apply_overlay);
+
+void mcde_dss_disable_overlay(struct mcde_overlay *ovly)
+{
+	if (!ovly->state)
+		return;
+
+	mcde_ovly_put(ovly->state);
+
+	dev_dbg(&ovly->ddev->dev, "Overlay disabled, chnl=%d\n",
+							ovly->ddev->chnl_id);
+
+	ovly->state = NULL;
+}
+EXPORT_SYMBOL(mcde_dss_disable_overlay);
+
+int mcde_dss_update_overlay(struct mcde_overlay *ovly, bool tripple_buffer)
+{
+	int ret;
+	dev_vdbg(&ovly->ddev->dev, "Overlay update, chnl=%d\n",
+							ovly->ddev->chnl_id);
+
+	if (!ovly->state || !ovly->ddev->update || !ovly->ddev->invalidate_area)
+		return -EINVAL;
+
+	mutex_lock(&ovly->ddev->display_lock);
+	/* Do not perform an update if power mode is off */
+	if (ovly->ddev->get_power_mode(ovly->ddev) == MCDE_DISPLAY_PM_OFF) {
+		ret = 0;
+		goto power_mode_off;
+	}
+
+	ret = ovly->ddev->update(ovly->ddev, tripple_buffer);
+	if (ret)
+		goto update_failed;
+
+	ret = ovly->ddev->invalidate_area(ovly->ddev, NULL);
+
+power_mode_off:
+update_failed:
+	mutex_unlock(&ovly->ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_update_overlay);
+
+void mcde_dss_get_overlay_info(struct mcde_overlay *ovly,
+				struct mcde_overlay_info *info) {
+	if (info)
+		*info = ovly->info;
+}
+EXPORT_SYMBOL(mcde_dss_get_overlay_info);
+
+void mcde_dss_get_native_resolution(struct mcde_display_device *ddev,
+	u16 *x_res, u16 *y_res)
+{
+	mutex_lock(&ddev->display_lock);
+	ddev->get_native_resolution(ddev, x_res, y_res);
+	mutex_unlock(&ddev->display_lock);
+}
+EXPORT_SYMBOL(mcde_dss_get_native_resolution);
+
+enum mcde_ovly_pix_fmt mcde_dss_get_default_pixel_format(
+	struct mcde_display_device *ddev)
+{
+	int ret;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->get_default_pixel_format(ddev);
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_get_default_pixel_format);
+
+void mcde_dss_get_physical_size(struct mcde_display_device *ddev,
+	u16 *physical_width, u16 *physical_height)
+{
+	mutex_lock(&ddev->display_lock);
+	ddev->get_physical_size(ddev, physical_width, physical_height);
+	mutex_unlock(&ddev->display_lock);
+}
+EXPORT_SYMBOL(mcde_dss_get_physical_size);
+
+int mcde_dss_try_video_mode(struct mcde_display_device *ddev,
+	struct mcde_video_mode *video_mode)
+{
+	int ret;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->try_video_mode(ddev, video_mode);
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_try_video_mode);
+
+int mcde_dss_set_video_mode(struct mcde_display_device *ddev,
+	struct mcde_video_mode *vmode)
+{
+	int ret;
+	struct mcde_video_mode old_vmode;
+
+	mutex_lock(&ddev->display_lock);
+	ddev->get_video_mode(ddev, &old_vmode);
+	if (memcmp(vmode, &old_vmode, sizeof(old_vmode)) == 0) {
+		ret = 0;
+		goto same_video_mode;
+	}
+
+	ret = ddev->set_video_mode(ddev, vmode);
+	if (ret)
+		goto set_video_mode_failed;
+
+	if (ddev->invalidate_area)
+		ret = ddev->invalidate_area(ddev, NULL);
+same_video_mode:
+set_video_mode_failed:
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_set_video_mode);
+
+void mcde_dss_get_video_mode(struct mcde_display_device *ddev,
+	struct mcde_video_mode *video_mode)
+{
+	mutex_lock(&ddev->display_lock);
+	ddev->get_video_mode(ddev, video_mode);
+	mutex_unlock(&ddev->display_lock);
+}
+EXPORT_SYMBOL(mcde_dss_get_video_mode);
+
+int mcde_dss_set_pixel_format(struct mcde_display_device *ddev,
+	enum mcde_ovly_pix_fmt pix_fmt)
+{
+	enum mcde_ovly_pix_fmt old_pix_fmt;
+	int ret;
+
+	mutex_lock(&ddev->display_lock);
+	old_pix_fmt = ddev->get_pixel_format(ddev);
+	if (old_pix_fmt == pix_fmt) {
+		ret = 0;
+		goto same_pixel_format;
+	}
+
+	ret = ddev->set_pixel_format(ddev, pix_fmt);
+
+same_pixel_format:
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_set_pixel_format);
+
+int mcde_dss_get_pixel_format(struct mcde_display_device *ddev)
+{
+	int ret;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->get_pixel_format(ddev);
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_get_pixel_format);
+
+int mcde_dss_set_rotation(struct mcde_display_device *ddev,
+	enum mcde_display_rotation rotation)
+{
+	int ret;
+	enum mcde_display_rotation old_rotation;
+
+	mutex_lock(&ddev->display_lock);
+	old_rotation = ddev->get_rotation(ddev);
+	if (old_rotation == rotation) {
+		ret = 0;
+		goto same_rotation;
+	}
+
+	ret = ddev->set_rotation(ddev, rotation);
+same_rotation:
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_set_rotation);
+
+enum mcde_display_rotation mcde_dss_get_rotation(
+	struct mcde_display_device *ddev)
+{
+	int ret;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->get_rotation(ddev);
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_get_rotation);
+
+int mcde_dss_set_synchronized_update(struct mcde_display_device *ddev,
+	bool enable)
+{
+	int ret;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->set_synchronized_update(ddev, enable);
+	if (ret)
+		goto sync_update_failed;
+
+	if (ddev->chnl_state)
+		mcde_chnl_enable_synchronized_update(ddev->chnl_state, enable);
+	mutex_unlock(&ddev->display_lock);
+	return 0;
+
+sync_update_failed:
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_set_synchronized_update);
+
+bool mcde_dss_get_synchronized_update(struct mcde_display_device *ddev)
+{
+	int ret;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->get_synchronized_update(ddev);
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_get_synchronized_update);
+
+int __init mcde_dss_init(void)
+{
+	return 0;
+}
+
+void mcde_dss_exit(void)
+{
+}
+
diff --git a/drivers/video/mcde/mcde_fb.c b/drivers/video/mcde/mcde_fb.c
new file mode 100644
index 00000000000..628b10953f6
--- /dev/null
+++ b/drivers/video/mcde/mcde_fb.c
@@ -0,0 +1,830 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson MCDE frame buffer driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/fb.h>
+#include <linux/mm.h>
+#include <linux/dma-mapping.h>
+
+#include <linux/hwmem.h>
+#include <linux/io.h>
+
+#include <linux/console.h>
+
+#include <video/mcde_fb.h>
+
+#define MCDE_FB_BPP_MAX		16
+#define MCDE_FB_VXRES_MAX	1920
+#define MCDE_FB_VYRES_MAX	2160
+
+static struct fb_ops fb_ops;
+
+struct pix_fmt_info {
+	enum mcde_ovly_pix_fmt pix_fmt;
+
+	u32		   bpp;
+	struct fb_bitfield r;
+	struct fb_bitfield g;
+	struct fb_bitfield b;
+	struct fb_bitfield a;
+	u32                nonstd;
+};
+
+struct pix_fmt_info pix_fmt_map[] = {
+	{
+		.pix_fmt = MCDE_OVLYPIXFMT_RGB565,
+		.bpp = 16,
+		.r = { .offset = 11, .length = 5 },
+		.g = { .offset =  5, .length = 6 },
+		.b = { .offset =  0, .length = 5 },
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_RGBA5551,
+		.bpp = 16,
+		.r = { .offset = 11, .length = 5 },
+		.g = { .offset =  6, .length = 5 },
+		.b = { .offset =  1, .length = 5 },
+		.a = { .offset =  0, .length = 1 },
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_RGBA4444,
+		.bpp = 16,
+		.r = { .offset = 12, .length = 4 },
+		.g = { .offset =  8, .length = 4 },
+		.b = { .offset =  4, .length = 4 },
+		.a = { .offset =  0, .length = 4 },
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_YCbCr422,
+		.bpp = 16,
+		.nonstd = MCDE_OVLYPIXFMT_YCbCr422,
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_RGB888,
+		.bpp = 24,
+		.r = { .offset = 16, .length = 8 },
+		.g = { .offset =  8, .length = 8 },
+		.b = { .offset =  0, .length = 8 },
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_RGBA8888,
+		.bpp = 32,
+		.r = { .offset = 16, .length = 8 },
+		.g = { .offset =  8, .length = 8 },
+		.b = { .offset =  0, .length = 8 },
+		.a = { .offset = 24, .length = 8 },
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_RGBX8888,
+		.bpp = 32,
+		.r = { .offset = 16, .length = 8 },
+		.g = { .offset =  8, .length = 8 },
+		.b = { .offset =  0, .length = 8 },
+	}
+
+};
+
+static struct platform_device mcde_fb_device = {
+	.name = "mcde_fb",
+	.id = -1,
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void early_suspend(struct early_suspend *data)
+{
+	int i;
+	struct mcde_fb *mfb =
+		container_of(data, struct mcde_fb, early_suspend);
+
+	console_lock();
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		if (mfb->ovlys[i] && mfb->ovlys[i]->ddev &&
+				(mfb->ovlys[i]->ddev->stay_alive == false))
+			mcde_dss_disable_display(mfb->ovlys[i]->ddev);
+	}
+	console_unlock();
+}
+
+static void late_resume(struct early_suspend *data)
+{
+	int i;
+	struct mcde_fb *mfb =
+		container_of(data, struct mcde_fb, early_suspend);
+
+	console_lock();
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		if (mfb->ovlys[i]) {
+			struct mcde_overlay *ovly = mfb->ovlys[i];
+			(void) mcde_dss_enable_display(ovly->ddev);
+		}
+	}
+	console_unlock();
+}
+#endif
+
+/* Helpers */
+
+static struct pix_fmt_info *find_pix_fmt_info(enum mcde_ovly_pix_fmt pix_fmt)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pix_fmt_map); i++) {
+		if (pix_fmt_map[i].pix_fmt == pix_fmt)
+			return &pix_fmt_map[i];
+	}
+	return NULL;
+}
+
+static bool bitfield_cmp(struct fb_bitfield *bf1, struct fb_bitfield *bf2)
+{
+	return bf1->offset == bf2->offset &&
+		bf1->length == bf2->length &&
+		bf1->msb_right == bf2->msb_right;
+}
+
+static struct pix_fmt_info *var_to_pix_fmt_info(struct fb_var_screeninfo *var)
+{
+	int i;
+	struct pix_fmt_info *info;
+
+	if (var->nonstd)
+		return find_pix_fmt_info(var->nonstd);
+
+	for (i = 0; i < ARRAY_SIZE(pix_fmt_map); i++) {
+		info = &pix_fmt_map[i];
+		if (info->bpp == var->bits_per_pixel &&
+					bitfield_cmp(&info->r, &var->red) &&
+					bitfield_cmp(&info->g, &var->green) &&
+					bitfield_cmp(&info->b, &var->blue) &&
+					bitfield_cmp(&info->a, &var->transp))
+			return info;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(pix_fmt_map); i++) {
+		info = &pix_fmt_map[i];
+		if (var->bits_per_pixel == info->bpp)
+			return info;
+	}
+
+	return NULL;
+}
+
+static void pix_fmt_info_to_var(struct pix_fmt_info *pix_fmt_info,
+	struct fb_var_screeninfo *var)
+{
+	var->bits_per_pixel = pix_fmt_info->bpp;
+	var->nonstd = pix_fmt_info->nonstd;
+	var->red = pix_fmt_info->r;
+	var->green = pix_fmt_info->g;
+	var->blue = pix_fmt_info->b;
+	var->transp = pix_fmt_info->a;
+}
+
+static int init_var_fmt(struct fb_var_screeninfo *var,
+	u16 w, u16 h, u16 vw, u16 vh, enum mcde_ovly_pix_fmt pix_fmt,
+	u32 rotate)
+{
+	struct pix_fmt_info *info;
+
+	info = find_pix_fmt_info(pix_fmt);
+	if (!info)
+		return -EINVAL;
+
+	var->bits_per_pixel = info->bpp;
+	var->nonstd         = info->nonstd;
+	var->red            = info->r;
+	var->green          = info->g;
+	var->blue           = info->b;
+	var->transp         = info->a;
+	var->grayscale      = false;
+
+	var->xres = w;
+	var->yres = h;
+	var->xres_virtual = vw;
+	var->yres_virtual = vh;
+	var->xoffset = 0;
+	var->yoffset = 0;
+	var->activate = FB_ACTIVATE_NOW;
+	var->rotate = rotate;
+
+	return 0;
+};
+
+static int reallocate_fb_mem(struct fb_info *fbi, u32 size)
+{
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+	void *vaddr;
+	struct hwmem_alloc *alloc;
+	struct hwmem_mem_chunk mem_chunk;
+	size_t num_mem_chunks = 1;
+	int name;
+
+	size = PAGE_ALIGN(size);
+
+	if (size == fbi->screen_size)
+		return 0;
+
+/* TODO: Remove once hwmem has support for defragmentation */
+#ifdef CONFIG_MCDE_FB_AVOID_REALLOC
+	if (!mfb->alloc) {
+		u32 old_size = size;
+
+		size = MCDE_FB_BPP_MAX / 8 * MCDE_FB_VXRES_MAX *
+			MCDE_FB_VYRES_MAX;
+#endif
+
+		alloc = hwmem_alloc(size, HWMEM_ALLOC_HINT_WRITE_COMBINE |
+				HWMEM_ALLOC_HINT_UNCACHED,
+				(HWMEM_ACCESS_READ  | HWMEM_ACCESS_WRITE |
+				HWMEM_ACCESS_IMPORT),
+				HWMEM_MEM_CONTIGUOUS_SYS);
+		if (IS_ERR(alloc))
+			return PTR_ERR(alloc);
+
+		name = hwmem_get_name(alloc);
+		if (name < 0) {
+			hwmem_release(alloc);
+			return name;
+		}
+
+		if (mfb->alloc) {
+			hwmem_kunmap(mfb->alloc);
+			hwmem_unpin(mfb->alloc);
+			hwmem_release(mfb->alloc);
+		}
+
+		(void)hwmem_pin(alloc, &mem_chunk, &num_mem_chunks);
+
+		vaddr = hwmem_kmap(alloc);
+		if (vaddr == NULL) {
+			hwmem_unpin(alloc);
+			hwmem_release(alloc);
+			return -ENOMEM;
+		}
+
+		mfb->alloc = alloc;
+		mfb->alloc_name = name;
+
+		fbi->screen_base = vaddr;
+		fbi->fix.smem_start = mem_chunk.paddr;
+
+#ifdef CONFIG_MCDE_FB_AVOID_REALLOC
+		size = old_size;
+	}
+#endif
+
+	fbi->screen_size = size;
+	fbi->fix.smem_len = size;
+
+	return 0;
+}
+
+static void free_fb_mem(struct fb_info *fbi)
+{
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+
+	if (mfb->alloc) {
+		hwmem_kunmap(mfb->alloc);
+		hwmem_unpin(mfb->alloc);
+		hwmem_release(mfb->alloc);
+		mfb->alloc = NULL;
+		mfb->alloc_name = 0;
+
+		fbi->fix.smem_start = 0;
+		fbi->fix.smem_len = 0;
+		fbi->screen_base = 0;
+		fbi->screen_size = 0;
+	}
+}
+
+static void init_fb(struct fb_info *fbi)
+{
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+
+	strlcpy(fbi->fix.id, "mcde_fb", sizeof(fbi->fix.id));
+	fbi->fix.type = FB_TYPE_PACKED_PIXELS;
+	fbi->fix.visual = FB_VISUAL_TRUECOLOR;
+	fbi->fix.xpanstep = 1;
+	fbi->fix.ypanstep = 1;
+	fbi->flags = FBINFO_HWACCEL_DISABLED;
+	fbi->fbops = &fb_ops;
+	fbi->pseudo_palette = &mfb->pseudo_palette[0];
+}
+
+static void get_ovly_info(struct fb_info *fbi, struct mcde_overlay *ovly,
+	struct mcde_overlay_info *info)
+{
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+
+	memset(info, 0, sizeof(*info));
+	info->paddr = fbi->fix.smem_start +
+		fbi->fix.line_length * fbi->var.yoffset;
+	info->vaddr = (u32 *)(fbi->screen_base +
+		fbi->fix.line_length * fbi->var.yoffset);
+	/* TODO: move mem check to check_var/pan_display */
+	if (info->paddr + fbi->fix.line_length * fbi->var.yres >
+		fbi->fix.smem_start + fbi->fix.smem_len) {
+		info->paddr = fbi->fix.smem_start;
+		info->vaddr = (u32 *)fbi->screen_base;
+	}
+	info->fmt = mfb->pix_fmt;
+	info->stride = fbi->fix.line_length;
+	if (ovly) {
+		info->src_x = ovly->info.src_x;
+		info->src_y = ovly->info.src_y;
+		info->dst_x = ovly->info.dst_x;
+		info->dst_y = ovly->info.dst_y;
+		info->dst_z = 1;
+	} else {
+		info->src_x = 0;
+		info->src_y = 0;
+		info->dst_x = 0;
+		info->dst_y = 0;
+		info->dst_z = 1;
+	}
+	info->w = fbi->var.xres;
+	info->h = fbi->var.yres;
+	info->dirty.x = 0;
+	info->dirty.y = 0;
+	info->dirty.w = fbi->var.xres;
+	info->dirty.h = fbi->var.yres;
+}
+
+void vmode_to_var(struct mcde_video_mode *video_mode,
+	struct fb_var_screeninfo *var)
+{
+	/* TODO: use only 1 vbp and 1 vfp */
+	var->xres           = video_mode->xres;
+	var->yres           = video_mode->yres;
+	var->pixclock       = video_mode->pixclock;
+	var->upper_margin   = video_mode->vbp;
+	var->lower_margin   = video_mode->vfp;
+	var->vsync_len      = video_mode->vsw;
+	var->left_margin    = video_mode->hbp;
+	var->right_margin   = video_mode->hfp;
+	var->hsync_len      = video_mode->hsw;
+	var->vmode         &= ~FB_VMODE_INTERLACED;
+	var->vmode         |= video_mode->interlaced ?
+				FB_VMODE_INTERLACED : FB_VMODE_NONINTERLACED;
+}
+
+void var_to_vmode(struct fb_var_screeninfo *var,
+	struct mcde_video_mode *video_mode)
+{
+	video_mode->xres       = var->xres;
+	video_mode->yres       = var->yres;
+	video_mode->pixclock   = var->pixclock;
+	video_mode->vbp        = var->upper_margin;
+	video_mode->vfp        = var->lower_margin;
+	video_mode->vsw        = var->vsync_len;
+	video_mode->hbp        = var->left_margin;
+	video_mode->hfp        = var->right_margin;
+	video_mode->hsw        = var->hsync_len;
+	video_mode->interlaced = (var->vmode & FB_VMODE_INTERLACED) ==
+							FB_VMODE_INTERLACED;
+}
+
+enum mcde_display_rotation var_to_rotation(struct fb_var_screeninfo *var)
+{
+	enum mcde_display_rotation rot;
+
+	switch (var->rotate) {
+	case FB_ROTATE_UR:
+		rot = MCDE_DISPLAY_ROT_0;
+		break;
+	case FB_ROTATE_CW:
+		rot = MCDE_DISPLAY_ROT_90_CW;
+		break;
+	case FB_ROTATE_UD:
+		rot = MCDE_DISPLAY_ROT_180_CW;
+		break;
+	case FB_ROTATE_CCW:
+		rot = MCDE_DISPLAY_ROT_90_CCW;
+		break;
+	default:
+		rot = MCDE_DISPLAY_ROT_0;
+		break;
+	}
+	dev_vdbg(&mcde_fb_device.dev, "var_rot: %d -> mcde_rot: %d\n",
+							var->rotate, rot);
+	return rot;
+}
+
+static struct mcde_display_device *fb_to_display(struct fb_info *fbi)
+{
+	int i;
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		if (mfb->ovlys[i])
+			return mfb->ovlys[i]->ddev;
+	}
+	return NULL;
+}
+
+static int check_var(struct fb_var_screeninfo *var, struct fb_info *fbi,
+	struct mcde_display_device *ddev)
+{
+	int ret;
+	u16 w = -1, h = -1;
+	struct mcde_video_mode vmode;
+	struct pix_fmt_info *fmtinfo;
+
+	/* TODO: check sizes/offsets/memory validity */
+
+	/* Device physical size */
+	mcde_dss_get_physical_size(ddev, &w, &h);
+	var->width  = w;
+	var->height = h;
+
+	/* Rotation */
+	if (var->rotate > 3) {
+		dev_info(&(ddev->dev), "check_var failed var->rotate\n");
+		return -EINVAL;
+	}
+
+	/* Video mode */
+	var_to_vmode(var, &vmode);
+	ret = mcde_dss_try_video_mode(ddev, &vmode);
+	if (ret < 0) {
+		dev_vdbg(&(ddev->dev), "check_var failed "
+			"mcde_dss_try_video_mode with size = %x\n", ret);
+		return ret;
+	}
+	vmode_to_var(&vmode, var);
+
+	/* Pixel format */
+	fmtinfo = var_to_pix_fmt_info(var);
+	if (!fmtinfo) {
+		dev_vdbg(&(ddev->dev), "check_var failed fmtinfo\n");
+		return -EINVAL;
+	}
+	pix_fmt_info_to_var(fmtinfo, var);
+
+	/* Not used */
+	var->grayscale = 0;
+	var->sync = 0;
+
+	return 0;
+}
+
+static int apply_var(struct fb_info *fbi, struct mcde_display_device *ddev)
+{
+	int ret, i;
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+	struct fb_var_screeninfo *var;
+	struct mcde_video_mode vmode;
+	struct pix_fmt_info *fmt;
+	u32 line_len, size;
+
+	dev_vdbg(&(ddev->dev), "%s\n", __func__);
+
+	var = &fbi->var;
+
+	/* Reallocate memory */
+	line_len = (fbi->var.bits_per_pixel * var->xres_virtual) / 8;
+	line_len = ALIGN(line_len, MCDE_BUF_LINE_ALIGMENT);
+	size = line_len * var->yres_virtual;
+	ret = reallocate_fb_mem(fbi, size);
+	if (ret) {
+		dev_vdbg(&(ddev->dev), "apply_var failed with"
+				"reallocate mem with size = %d\n", size);
+		return ret;
+	}
+	fbi->fix.line_length = line_len;
+
+	if (ddev->fictive)
+		goto apply_var_end;
+
+	if (ddev) {
+		/* Apply pixel format */
+		fmt = var_to_pix_fmt_info(var);
+		mfb->pix_fmt = fmt->pix_fmt;
+
+		/* Apply rotation */
+		mcde_dss_set_rotation(ddev, var_to_rotation(var));
+		/* Apply video mode */
+		memset(&vmode, 0, sizeof(struct mcde_video_mode));
+		var_to_vmode(var, &vmode);
+		ret = mcde_dss_set_video_mode(ddev, &vmode);
+		if (ret)
+			return ret;
+
+		mcde_dss_apply_channel(ddev);
+	}
+
+	/* Apply overlay info */
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		struct mcde_overlay *ovly = mfb->ovlys[i];
+		struct mcde_overlay_info info;
+		int num_buffers;
+
+		get_ovly_info(fbi, ovly, &info);
+		(void) mcde_dss_apply_overlay(ovly, &info);
+
+		num_buffers = var->yres_virtual / var->yres;
+		mcde_dss_update_overlay(ovly, num_buffers == 3);
+	}
+
+apply_var_end:
+	return 0;
+}
+
+/* FB ops */
+
+static int mcde_fb_open(struct fb_info *fbi, int user)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+	return 0;
+}
+
+static int mcde_fb_release(struct fb_info *fbi, int user)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+	return 0;
+}
+
+static int mcde_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *fbi)
+{
+	struct mcde_display_device *ddev = fb_to_display(fbi);
+
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+
+	if (!ddev) {
+		printk(KERN_ERR "mcde_fb_check_var failed !ddev\n");
+		return -ENODEV;
+	}
+
+	return check_var(var, fbi, ddev);
+}
+
+static int mcde_fb_set_par(struct fb_info *fbi)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+
+	return apply_var(fbi, fb_to_display(fbi));
+}
+
+static int mcde_fb_setcolreg(unsigned regno, unsigned red, unsigned green,
+			unsigned blue, unsigned transp, struct fb_info *fbi)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+
+	/*Nothing to see here, move along*/
+	return 0;
+}
+
+static int mcde_fb_setcmap(struct fb_cmap *cmap, struct fb_info *fbi)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+
+	/*Nothing to see here, move along*/
+	return 0;
+}
+
+static int mcde_fb_blank(int blank, struct fb_info *fbi)
+{
+	return 0;
+}
+
+static int mcde_fb_pan_display(struct fb_var_screeninfo *var,
+	struct fb_info *fbi)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+
+	if (var->xoffset == fbi->var.xoffset &&
+					var->yoffset == fbi->var.yoffset)
+		return 0;
+
+	fbi->var.xoffset = var->xoffset;
+	fbi->var.yoffset = var->yoffset;
+	return apply_var(fbi, fb_to_display(fbi));
+}
+
+static void mcde_fb_rotate(struct fb_info *fbi, int rotate)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+}
+
+static int mcde_fb_ioctl(struct fb_info *fbi, unsigned int cmd,
+							 unsigned long arg)
+{
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+
+	if (cmd == MCDE_GET_BUFFER_NAME_IOC)
+		return mfb->alloc_name;
+
+	return -EINVAL;
+}
+
+static struct fb_ops fb_ops = {
+	/* creg, cmap */
+	.owner          = THIS_MODULE,
+	.fb_open        = mcde_fb_open,
+	.fb_release     = mcde_fb_release,
+	.fb_read        = fb_sys_read,
+	.fb_write       = fb_sys_write,
+	.fb_fillrect    = sys_fillrect,
+	.fb_copyarea    = sys_copyarea,
+	.fb_imageblit   = sys_imageblit,
+	.fb_check_var   = mcde_fb_check_var,
+	.fb_set_par     = mcde_fb_set_par,
+	.fb_setcolreg   = mcde_fb_setcolreg,
+	.fb_setcmap     = mcde_fb_setcmap,
+	.fb_blank       = mcde_fb_blank,
+	.fb_pan_display = mcde_fb_pan_display,
+	.fb_rotate      = mcde_fb_rotate,
+	.fb_ioctl       = mcde_fb_ioctl,
+};
+
+/* FB driver */
+
+struct fb_info *mcde_fb_create(struct mcde_display_device *ddev,
+	u16 w, u16 h, u16 vw, u16 vh, enum mcde_ovly_pix_fmt pix_fmt,
+	u32 rotate)
+{
+	int ret = 0;
+	struct fb_info *fbi;
+	struct mcde_fb *mfb;
+	struct mcde_overlay *ovly = NULL;
+	struct mcde_overlay_info ovly_info;
+
+	dev_vdbg(&ddev->dev, "%s\n", __func__);
+	if (!ddev->initialized) {
+		dev_warn(&ddev->dev, "%s: Device not initialized\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+
+	/* Init fb */
+	fbi = framebuffer_alloc(sizeof(struct mcde_fb), &mcde_fb_device.dev);
+	if (fbi == NULL) {
+		ret = -ENOMEM;
+		goto fb_alloc_failed;
+	}
+	init_fb(fbi);
+	mfb = to_mcde_fb(fbi);
+
+	if (ddev->fictive == false) {
+		ret = mcde_dss_open_channel(ddev);
+		if (ret)
+			goto channel_open_failed;
+
+		ret = mcde_dss_enable_display(ddev);
+		if (ret)
+			goto display_enable_failed;
+	}
+
+	/* Prepare var and allocate frame buffer memory */
+	init_var_fmt(&fbi->var, w, h, vw, vh, pix_fmt, rotate);
+	check_var(&fbi->var, fbi, ddev);
+	ret = apply_var(fbi, ddev);
+	if (ret)
+		goto apply_var_failed;
+
+	if (ddev->fictive == false)
+		mcde_dss_set_pixel_format(ddev, ddev->port->pixel_format);
+
+	/* Setup overlay */
+	get_ovly_info(fbi, NULL, &ovly_info);
+	ovly = mcde_dss_create_overlay(ddev, &ovly_info);
+	if (!ovly) {
+		ret = PTR_ERR(ovly);
+		goto ovly_alloc_failed;
+	}
+	mfb->ovlys[0] = ovly;
+	mfb->num_ovlys = 1;
+
+	if (ddev->fictive == false) {
+		ret = mcde_dss_enable_overlay(ovly);
+		if (ret)
+			goto ovly_enable_failed;
+	}
+
+	mfb->id = ddev->id;
+
+	/* Register framebuffer */
+	ret = register_framebuffer(fbi);
+	if (ret)
+		goto fb_register_failed;
+
+	ret = fb_alloc_cmap(&fbi->cmap, 256, 0);
+	if (ret)
+		dev_warn(&ddev->dev, "%s: Allocate color map memory failed!\n", __func__);
+
+	ddev->fbi = fbi;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	if (ddev->fictive == false) {
+		mfb->early_suspend.level =
+				EARLY_SUSPEND_LEVEL_DISABLE_FB;
+		mfb->early_suspend.suspend = early_suspend;
+		mfb->early_suspend.resume = late_resume;
+		register_early_suspend(&mfb->early_suspend);
+	}
+#endif
+
+	goto out;
+fb_register_failed:
+	mcde_dss_disable_overlay(ovly);
+ovly_enable_failed:
+	mcde_dss_destroy_overlay(ovly);
+ovly_alloc_failed:
+	free_fb_mem(fbi);
+apply_var_failed:
+	mcde_dss_disable_display(ddev);
+display_enable_failed:
+	mcde_dss_close_channel(ddev);
+channel_open_failed:
+	framebuffer_release(fbi);
+	fbi = NULL;
+fb_alloc_failed:
+out:
+	return ret ? ERR_PTR(ret) : fbi;
+}
+EXPORT_SYMBOL(mcde_fb_create);
+
+int mcde_fb_attach_overlay(struct fb_info *fb_info, struct mcde_overlay *ovl)
+{
+	/* TODO: Attach extra overlay targets */
+	return -EINVAL;
+}
+
+void mcde_fb_destroy(struct mcde_display_device *dev)
+{
+	struct mcde_fb *mfb;
+	int i;
+
+	dev_vdbg(&dev->dev, "%s\n", __func__);
+
+	if (dev->fictive == false) {
+		mcde_dss_disable_display(dev);
+		mcde_dss_close_channel(dev);
+	}
+
+	mfb = to_mcde_fb(dev->fbi);
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		if (mfb->ovlys[i])
+			mcde_dss_destroy_overlay(mfb->ovlys[i]);
+	}
+
+	fb_dealloc_cmap(&dev->fbi->cmap);
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	unregister_early_suspend(&mfb->early_suspend);
+#endif
+	unregister_framebuffer(dev->fbi);
+	free_fb_mem(dev->fbi);
+	framebuffer_release(dev->fbi);
+	dev->fbi = NULL;
+}
+
+/* Overlay fbs' platform device */
+static int mcde_fb_probe(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static int mcde_fb_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver mcde_fb_driver = {
+	.probe  = mcde_fb_probe,
+	.remove = mcde_fb_remove,
+	.driver = {
+		.name  = "mcde_fb",
+		.owner = THIS_MODULE,
+	},
+};
+
+/* MCDE fb init */
+
+int __init mcde_fb_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&mcde_fb_driver);
+	if (ret)
+		goto fb_driver_failed;
+	ret = platform_device_register(&mcde_fb_device);
+	if (ret)
+		goto fb_device_failed;
+
+	goto out;
+fb_device_failed:
+	platform_driver_unregister(&mcde_fb_driver);
+fb_driver_failed:
+out:
+	return ret;
+}
+
+void mcde_fb_exit(void)
+{
+	platform_device_unregister(&mcde_fb_device);
+	platform_driver_unregister(&mcde_fb_driver);
+}
diff --git a/drivers/video/mcde/mcde_hw.c b/drivers/video/mcde/mcde_hw.c
new file mode 100644
index 00000000000..585b22f9111
--- /dev/null
+++ b/drivers/video/mcde/mcde_hw.c
@@ -0,0 +1,3581 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE base driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/spinlock.h>
+#include <linux/err.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/regulator/consumer.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/workqueue.h>
+
+#include <video/mcde.h>
+#include "dsilink_regs.h"
+#include "mcde_regs.h"
+
+static void disable_channel(struct mcde_chnl_state *chnl);
+static void watchdog_auto_sync_timer_function(unsigned long arg);
+static int _mcde_chnl_apply(struct mcde_chnl_state *chnl);
+static void disable_flow(struct mcde_chnl_state *chnl);
+static void enable_channel(struct mcde_chnl_state *chnl);
+static void do_softwaretrig(struct mcde_chnl_state *chnl);
+static int is_channel_enabled(struct mcde_chnl_state *chnl);
+static void dsi_te_poll_req(struct mcde_chnl_state *chnl);
+static void dsi_te_poll_set_timer(struct mcde_chnl_state *chnl,
+		unsigned int timeout);
+static void dsi_te_timer_function(unsigned long value);
+
+#define OVLY_TIMEOUT 100
+#define CHNL_TIMEOUT 100
+#define SCREEN_PPL_HIGH 1920
+#define SCREEN_PPL_CEA2 720
+#define SCREEN_LPF_CEA2 480
+#define DSI_DELAY0_CEA2_ADD 10
+
+#define MCDE_SLEEP_WATCHDOG 500
+#define DSI_TE_NO_ANSWER_TIMEOUT_INIT 2500
+#define DSI_TE_NO_ANSWER_TIMEOUT 250
+
+static u8 *mcdeio;
+static u8 **dsiio;
+static struct platform_device *mcde_dev;
+static u8 num_dsilinks;
+static u8 num_channels;
+static u8 num_overlays;
+static u8 hardware_version;
+static int mcde_irq;
+
+#ifdef CONFIG_REGULATOR
+static struct regulator *regulator_vana;
+static struct regulator *regulator_mcde_epod;
+static struct regulator *regulator_esram_epod;
+#endif
+static struct clk *clock_dpi;
+static struct clk *clock_dsi;
+static struct clk *clock_mcde;
+static struct clk *clock_dsi_lp;
+static u8 mcde_is_enabled;
+static struct mutex mcde_hw_lock;
+static struct delayed_work hw_timeout_work;
+static u8 enable_dsi;
+static u8 dsi_is_enabled;
+
+static u8 mcde_dynamic_power_management = true;
+
+static inline u32 dsi_rreg(int i, u32 reg)
+{
+	return readl(dsiio[i] + reg);
+}
+static inline void dsi_wreg(int i, u32 reg, u32 val)
+{
+	writel(val, dsiio[i] + reg);
+}
+#define dsi_rfld(__i, __reg, __fld) \
+	((dsi_rreg(__i, __reg) & __reg##_##__fld##_MASK) >> \
+		__reg##_##__fld##_SHIFT)
+#define dsi_wfld(__i, __reg, __fld, __val) \
+	dsi_wreg(__i, __reg, (dsi_rreg(__i, __reg) & \
+	~__reg##_##__fld##_MASK) | (((__val) << __reg##_##__fld##_SHIFT) & \
+		 __reg##_##__fld##_MASK))
+
+static inline u32 mcde_rreg(u32 reg)
+{
+	return readl(mcdeio + reg);
+}
+static inline void mcde_wreg(u32 reg, u32 val)
+{
+	writel(val, mcdeio + reg);
+}
+#define mcde_wreg_fld(__reg, __fld_mask, __fld_shift, __val) \
+	mcde_wreg(__reg, (mcde_rreg(__reg) & ~(__fld_mask)) |\
+			(((__val) << (__fld_shift)) & (__fld_mask)))
+
+#define mcde_rfld(__reg, __fld) \
+	((mcde_rreg(__reg) & __reg##_##__fld##_MASK) >> \
+		__reg##_##__fld##_SHIFT)
+#define mcde_wfld(__reg, __fld, __val) \
+	mcde_wreg_fld(__reg, __reg##_##__fld##_MASK,\
+						__reg##_##__fld##_SHIFT, __val)
+
+struct ovly_regs {
+	u8   ch_id;
+	bool enabled;
+	u32  baseaddress0;
+	u32  baseaddress1;
+	bool update;
+	u8   bits_per_pixel;
+	u8   bpp;
+	bool bgr;
+	bool bebo;
+	bool opq;
+	u8   col_conv;
+	u8   alpha_source;
+	u8   alpha_value;
+	u8   pixoff;
+	u16  ppl;
+	u16  lpf;
+	u16  cropx;
+	u16  cropy;
+	u16  xpos;
+	u16  ypos;
+	u8   z;
+};
+
+struct mcde_ovly_state {
+	bool inuse;
+	bool update;
+	u8 idx; /* MCDE overlay index */
+	struct mcde_chnl_state *chnl; /* Owner channel */
+
+	/* Staged settings */
+	u32 paddr;
+	u16 stride;
+	enum mcde_ovly_pix_fmt pix_fmt;
+
+	u16 src_x;
+	u16 src_y;
+	u16 dst_x;
+	u16 dst_y;
+	u16 dst_z;
+	u16 w;
+	u16 h;
+
+	u8 alpha_source;
+	u8 alpha_value;
+
+	/* Applied settings */
+	struct ovly_regs regs;
+};
+
+static struct mcde_ovly_state *overlays;
+
+struct chnl_regs {
+	bool floen;
+	u16  x;
+	u16  y;
+	u16  ppl;
+	u16  lpf;
+	u8   bpp;
+	bool internal_clk; /* CLKTYPE field */
+	u16  pcd;
+	u8   clksel;
+	u8   cdwin;
+	u16 (*map_r)(u8);
+	u16 (*map_g)(u8);
+	u16 (*map_b)(u8);
+	bool palette_enable;
+	bool bcd;
+	bool synchronized_update;
+	bool roten;
+	u8   rotdir;
+	u32  rotbuf1; /* TODO: Replace with eSRAM alloc */
+	u32  rotbuf2; /* TODO: Replace with eSRAM alloc */
+
+	/* Blending */
+	u8 blend_ctrl;
+	bool blend_en;
+	u8 alpha_blend;
+
+	/* DSI */
+	u8 dsipacking;
+};
+
+struct col_regs {
+	u16 y_red;
+	u16 y_green;
+	u16 y_blue;
+	u16 cb_red;
+	u16 cb_green;
+	u16 cb_blue;
+	u16 cr_red;
+	u16 cr_green;
+	u16 cr_blue;
+	u16 off_y;
+	u16 off_cb;
+	u16 off_cr;
+};
+
+struct tv_regs {
+	u16 dho; /* TV mode: left border width; destination horizontal offset */
+		 /* LCD MODE: horizontal back porch */
+	u16 alw; /* TV mode: right border width */
+		 /* LCD mode: horizontal front porch */
+	u16 hsw; /* horizontal synch width */
+	u16 dvo; /* TV mode: top border width; destination horizontal offset */
+		 /* LCD MODE: vertical back porch */
+	u16 bsl; /* TV mode: bottom border width; blanking start line */
+		 /* LCD MODE: vertical front porch */
+	/* field 1 */
+	u16 bel1; /* TV mode: field total vertical blanking lines */
+		 /* LCD mode: vertical sync width */
+	u16 fsl1; /* field vbp */
+	/* field 2 */
+	u16 bel2;
+	u16 fsl2;
+	u8 tv_mode;
+	bool sel_mode_tv;
+	bool inv_clk;
+	bool interlaced_en;
+	u32 lcdtim1;
+};
+
+struct mcde_chnl_state {
+	bool enabled;
+	bool reserved;
+	enum mcde_chnl id;
+	enum mcde_fifo fifo;
+	struct mcde_port port;
+	struct mcde_ovly_state *ovly0;
+	struct mcde_ovly_state *ovly1;
+	const struct chnl_config *cfg;
+	u32 transactionid;
+	u32 transactionid_regs;
+	u32 transactionid_hw;
+	wait_queue_head_t waitq_hw; /* Waitq for transactionid_hw */
+	/* Used as watchdog timer for auto sync feature */
+	struct timer_list auto_sync_timer;
+	struct timer_list dsi_te_timer;
+
+	enum mcde_display_power_mode power_mode;
+
+	/* Staged settings */
+	u16 (*map_r)(u8);
+	u16 (*map_g)(u8);
+	u16 (*map_b)(u8);
+	bool palette_enable;
+	bool synchronized_update;
+	struct mcde_video_mode vmode;
+	enum mcde_display_rotation rotation;
+	u32 rotbuf1;
+	u32 rotbuf2;
+
+	struct mcde_col_transform rgb_2_ycbcr;
+	struct mcde_col_transform ycbcr_2_rgb;
+	struct mcde_col_transform *transform;
+
+	/* Blending */
+	u8 blend_ctrl;
+	bool blend_en;
+	u8 alpha_blend;
+
+	/* Applied settings */
+	struct chnl_regs regs;
+	struct col_regs  col_regs;
+	struct tv_regs   tv_regs;
+
+	/* an interlaced digital TV signal generates a VCMP per field */
+	bool vcmp_per_field;
+	bool even_vcmp;
+
+	bool continous_running;
+	bool disable_software_trig;
+	bool formatter_updated;
+	bool esram_is_enabled;
+};
+
+static struct mcde_chnl_state *channels;
+
+struct chnl_config {
+	/* Key */
+	enum mcde_chnl_path path;
+
+	/* Value */
+	bool swap_a_c0;
+	bool swap_a_c0_set;
+	bool swap_b_c1;
+	bool swap_b_c1_set;
+	bool fabmux;
+	bool fabmux_set;
+	bool f01mux;
+	bool f01mux_set;
+};
+
+static /* TODO: const, compiler bug? */ struct chnl_config chnl_configs[] = {
+	/* Channel A */
+	{ .path = MCDE_CHNLPATH_CHNLA_FIFOA_DPI_0,
+	  .swap_a_c0 = false, .swap_a_c0_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLA_FIFOA_DSI_IFC0_0,
+	  .swap_a_c0 = false, .swap_a_c0_set = true,
+	  .fabmux = false, .fabmux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLA_FIFOA_DSI_IFC0_1,
+	  .swap_a_c0 = false, .swap_a_c0_set = true,
+	  .fabmux = true, .fabmux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLA_FIFOC0_DSI_IFC0_2,
+	  .swap_a_c0 = true, .swap_a_c0_set = true,
+	  .f01mux = false, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLA_FIFOC0_DSI_IFC1_0,
+	  .swap_a_c0 = true, .swap_a_c0_set = true,
+	  .f01mux = false, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLA_FIFOC0_DSI_IFC1_1,
+	  .swap_a_c0 = true, .swap_a_c0_set = true,
+	  .f01mux = true, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLA_FIFOA_DSI_IFC1_2,
+	  .swap_a_c0 = false, .swap_a_c0_set = true,
+	  .fabmux = false, .fabmux_set = true },
+	/* Channel B */
+	{ .path = MCDE_CHNLPATH_CHNLB_FIFOB_DPI_1,
+	  .swap_b_c1 = false, .swap_b_c1_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLB_FIFOB_DSI_IFC0_0,
+	  .swap_b_c1 = false, .swap_b_c1_set = true,
+	  .fabmux = true, .fabmux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLB_FIFOB_DSI_IFC0_1,
+	  .swap_b_c1 = false, .swap_b_c1_set = true,
+	  .fabmux = false, .fabmux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLB_FIFOC1_DSI_IFC0_2,
+	  .swap_b_c1 = true, .swap_b_c1_set = true,
+	  .f01mux = true, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLB_FIFOC1_DSI_IFC1_0,
+	  .swap_b_c1 = true, .swap_b_c1_set = true,
+	  .f01mux = true, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLB_FIFOC1_DSI_IFC1_1,
+	  .swap_b_c1 = true, .swap_b_c1_set = true,
+	  .f01mux = false, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLB_FIFOB_DSI_IFC1_2,
+	  .swap_b_c1 = false, .swap_b_c1_set = true,
+	  .fabmux = true, .fabmux_set = true },
+	/* Channel C0 */
+	{ .path = MCDE_CHNLPATH_CHNLC0_FIFOA_DSI_IFC0_0,
+	  .swap_a_c0 = true, .swap_a_c0_set = true,
+	  .fabmux = false, .fabmux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLC0_FIFOA_DSI_IFC0_1,
+	  .swap_a_c0 = true, .swap_a_c0_set = true,
+	  .fabmux = true, .fabmux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLC0_FIFOC0_DSI_IFC0_2,
+	  .swap_a_c0 = false, .swap_a_c0_set = true,
+	  .f01mux = false, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLC0_FIFOC0_DSI_IFC1_0,
+	  .swap_a_c0 = false, .swap_a_c0_set = true,
+	  .f01mux = false, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLC0_FIFOC0_DSI_IFC1_1,
+	  .swap_a_c0 = false, .swap_a_c0_set = true,
+	  .f01mux = true, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLC0_FIFOA_DSI_IFC1_2,
+	  .swap_a_c0 = true, .swap_a_c0_set = true,
+	  .fabmux = false, .fabmux_set = true },
+	/* Channel C1 */
+	{ .path = MCDE_CHNLPATH_CHNLC1_FIFOB_DSI_IFC0_0,
+	  .swap_b_c1 = true, .swap_b_c1_set = true,
+	  .fabmux = true, .fabmux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLC1_FIFOB_DSI_IFC0_1,
+	  .swap_b_c1 = true, .swap_b_c1_set = true,
+	  .fabmux = false, .fabmux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLC1_FIFOC1_DSI_IFC0_2,
+	  .swap_b_c1 = false, .swap_b_c1_set = true,
+	  .f01mux = true, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLC1_FIFOC1_DSI_IFC1_0,
+	  .swap_b_c1 = false, .swap_b_c1_set = true,
+	  .f01mux = true, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLC1_FIFOC1_DSI_IFC1_1,
+	  .swap_b_c1 = false, .swap_b_c1_set = true,
+	  .f01mux = false, .f01mux_set = true },
+	{ .path = MCDE_CHNLPATH_CHNLC1_FIFOB_DSI_IFC1_2,
+	  .swap_b_c1 = true, .swap_b_c1_set = true,
+	  .fabmux = true, .fabmux_set = true },
+};
+
+#define DSI_READ_TIMEOUT 10
+#define DSI_READ_DELAY 100
+
+/*
+ * Wait for CSM_RUNNING, all data sent for display
+ */
+static inline void wait_while_dsi_running(int lnk)
+{
+	u8 counter = DSI_READ_TIMEOUT;
+	while (dsi_rfld(lnk, DSI_CMD_MODE_STS, CSM_RUNNING) && --counter) {
+		dev_vdbg(&mcde_dev->dev,
+			"%s: DSI link %u read running state retry %u times\n"
+			, __func__, lnk, (DSI_READ_TIMEOUT - counter));
+		udelay(DSI_READ_DELAY);
+	}
+	if (!counter)
+		dev_warn(&mcde_dev->dev,
+			"%s: DSI link %u read timeout!\n", __func__, lnk);
+}
+
+static int enable_clocks_and_power(struct platform_device *pdev)
+{
+	struct mcde_platform_data *pdata = pdev->dev.platform_data;
+	int ret = 0;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+#ifdef CONFIG_REGULATOR
+	if (regulator_mcde_epod) {
+		ret = regulator_enable(regulator_mcde_epod);
+		if (ret < 0) {
+			dev_warn(&pdev->dev, "%s: regulator_enable failed\n",
+			__func__);
+			return ret;
+		}
+	} else {
+		dev_warn(&pdev->dev, "%s: mcde_epod regulator is null\n"
+						, __func__);
+		return -EINVAL;
+	}
+#endif
+	pdata->platform_set_clocks();
+	if (enable_dsi > 0) {
+		pdata->platform_enable_dsipll();
+		dsi_is_enabled = true;
+	}
+
+	ret = clk_enable(clock_mcde);
+	if (ret < 0) {
+		dev_warn(&pdev->dev, "%s: "
+			"clk_enable mcde failed ret = %d\n", __func__, ret);
+		goto clk_mcde_err;
+	}
+
+	return ret;
+
+clk_mcde_err:
+#ifdef CONFIG_REGULATOR
+	if (regulator_mcde_epod)
+		regulator_disable(regulator_mcde_epod);
+#endif
+	return ret;
+}
+
+static int disable_clocks_and_power(struct platform_device *pdev)
+{
+	struct mcde_platform_data *pdata = pdev->dev.platform_data;
+	int ret = 0;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	clk_disable(clock_mcde);
+	if (enable_dsi > 0) {
+		pdata->platform_disable_dsipll();
+		dsi_is_enabled = false;
+	}
+
+#ifdef CONFIG_REGULATOR
+	if (regulator_mcde_epod) {
+		ret = regulator_disable(regulator_mcde_epod);
+		if (ret < 0) {
+			dev_warn(&pdev->dev, "%s: regulator_disable failed\n"
+					, __func__);
+			goto regulator_mcde_epod_err;
+		}
+	} else {
+		dev_warn(&pdev->dev, "%s: mcde_epod regulator is null\n"
+					, __func__);
+		goto regulator_mcde_epod_err;
+	}
+	return ret;
+
+regulator_mcde_epod_err:
+	clk_enable(clock_mcde);
+#endif
+	return ret;
+}
+
+static void update_mcde_registers(void)
+{
+	struct mcde_platform_data *pdata = mcde_dev->dev.platform_data;
+
+	if (hardware_version == MCDE_CHIP_VERSION_1_0_4) {
+		/* Setup output muxing */
+		mcde_wreg(MCDE_CONF0,
+			MCDE_CONF0_IFIFOCTRLWTRMRKLVL(7));
+
+		mcde_wfld(MCDE_RISOVL, OVLFDRIS, 1);
+		mcde_wfld(MCDE_RISPP, VCMPARIS, 1);
+		mcde_wfld(MCDE_RISPP, VCMPBRIS, 1);
+
+		/* Enable channel VCMP interrupts */
+		mcde_wreg(MCDE_IMSCPP,
+			MCDE_IMSCPP_VCMPAIM(true) |
+			MCDE_IMSCPP_VCMPBIM(true));
+	} else {
+		/* Setup output muxing */
+		mcde_wreg(MCDE_CONF0,
+			MCDE_CONF0_IFIFOCTRLWTRMRKLVL(7) |
+			MCDE_CONF0_OUTMUX0(pdata->outmux[0]) |
+			MCDE_CONF0_OUTMUX1(pdata->outmux[1]) |
+			MCDE_CONF0_OUTMUX2(pdata->outmux[2]) |
+			MCDE_CONF0_OUTMUX3(pdata->outmux[3]) |
+			MCDE_CONF0_OUTMUX4(pdata->outmux[4]) |
+			pdata->syncmux);
+
+		mcde_wfld(MCDE_RISOVL, OVLFDRIS, 1);
+		mcde_wfld(MCDE_RISPP, VCMPARIS, 1);
+		mcde_wfld(MCDE_RISPP, VCMPBRIS, 1);
+		mcde_wfld(MCDE_RISPP, VCMPC0RIS, 1);
+		mcde_wfld(MCDE_RISPP, VCMPC1RIS, 1);
+
+		/* Enable channel VCMP interrupts */
+		mcde_wreg(MCDE_IMSCPP,
+			MCDE_IMSCPP_VCMPAIM(true) |
+			MCDE_IMSCPP_VCMPBIM(true) |
+			MCDE_IMSCPP_VCMPC0IM(true) |
+			MCDE_IMSCPP_VCMPC1IM(true));
+#ifdef DEBUG
+		/* Enable error interrupts */
+		mcde_wreg(MCDE_IMSCERR,
+			MCDE_IMSCERR_SCHBLCKDIM(true) |
+			MCDE_IMSCERR_OVLFERRIM_MASK |
+			MCDE_IMSCERR_FUAIM(true) |
+			MCDE_IMSCERR_FUBIM(true) |
+			MCDE_IMSCERR_FUC0IM(true) |
+			MCDE_IMSCERR_FUC1IM(true));
+		/* Enable channel abort interrupts */
+		mcde_wreg(MCDE_IMSCCHNL, MCDE_IMSCCHNL_CHNLAIM(0xf));
+#endif
+	}
+
+	/* Enable overlay fetch done interrupts */
+	mcde_wfld(MCDE_IMSCOVL, OVLFDIM, 0x3f);
+
+	/* Setup sync pulse length */
+	mcde_wreg(MCDE_VSCRC0,
+		MCDE_VSCRC0_VSPMIN(1) |
+		MCDE_VSCRC0_VSPMAX(0xff));
+	mcde_wreg(MCDE_VSCRC1,
+		MCDE_VSCRC1_VSPMIN(1) |
+		MCDE_VSCRC1_VSPMAX(0xff));
+}
+
+static void disable_formatter(struct mcde_port *port)
+{
+	if (port->type == MCDE_PORTTYPE_DSI) {
+		if (port->phy.dsi.clk_dsi)
+			clk_disable(port->phy.dsi.clk_dsi);
+		if (port->phy.dsi.clk_dsi_lp)
+			clk_disable(port->phy.dsi.clk_dsi_lp);
+		if (port->phy.dsi.reg_vana)
+			regulator_disable(port->phy.dsi.reg_vana);
+	}
+	if (port->type == MCDE_PORTTYPE_DPI) {
+		if (port->phy.dpi.clk_dpi)
+			clk_disable(port->phy.dpi.clk_dpi);
+	}
+}
+
+static int disable_mcde_hw(bool force_disable)
+{
+	int i;
+	int ret;
+	bool mcde_up = false;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!mcde_is_enabled)
+		return 0;
+
+	for (i = 0; i < num_channels; i++) {
+		struct mcde_chnl_state *chnl = &channels[i];
+		if (force_disable ||
+			(chnl->enabled && !chnl->continous_running)) {
+			disable_channel(chnl);
+			if (chnl->port.type == MCDE_PORTTYPE_DSI)
+				wait_while_dsi_running(chnl->port.link);
+
+			if (chnl->formatter_updated) {
+				disable_formatter(&chnl->port);
+				chnl->formatter_updated = false;
+			}
+			if (chnl->esram_is_enabled) {
+				int ret;
+				ret = regulator_disable(chnl->port.reg_esram);
+				if (ret < 0) {
+					dev_warn(&mcde_dev->dev,
+						"%s: disable failed\n",
+								__func__);
+				}
+				chnl->esram_is_enabled = false;
+			}
+		} else if (chnl->enabled && chnl->continous_running) {
+			mcde_up = true;
+		}
+	}
+
+	if (mcde_up)
+		return 0;
+
+	for (i = 0; i < num_channels; i++) {
+		struct mcde_chnl_state *chnl = &channels[i];
+		chnl->formatter_updated = false;
+		del_timer(&chnl->dsi_te_timer);
+		del_timer(&chnl->auto_sync_timer);
+	}
+
+	free_irq(mcde_irq, &mcde_dev->dev);
+
+	ret = disable_clocks_and_power(mcde_dev);
+	if (ret < 0) {
+		dev_dbg(&mcde_dev->dev,
+			"%s: disable_clocks_and_power failed\n"
+						, __func__);
+		return -EINVAL;
+	}
+	mcde_is_enabled = false;
+	return 0;
+}
+
+static void dpi_video_mode_apply(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	chnl->tv_regs.interlaced_en = chnl->vmode.interlaced;
+
+	chnl->tv_regs.sel_mode_tv = chnl->port.phy.dpi.tv_mode;
+	if (chnl->tv_regs.sel_mode_tv) {
+		/* TV mode */
+		u32 bel;
+		/* -4 since hsw is excluding SAV/EAV, 2 bytes each */
+		chnl->tv_regs.hsw  = chnl->vmode.hbp + chnl->vmode.hfp - 4;
+		/* vbp_field2 = vbp_field1 + 1 */
+		chnl->tv_regs.fsl1 = chnl->vmode.vbp / 2;
+		chnl->tv_regs.fsl2 = chnl->vmode.vbp - chnl->tv_regs.fsl1;
+		/* +1 since vbp_field2 = vbp_field1 + 1 */
+		bel = chnl->vmode.vbp + chnl->vmode.vfp;
+		/* in TV mode: bel2 = bel1 + 1 */
+		chnl->tv_regs.bel1 = bel / 2;
+		chnl->tv_regs.bel2 = bel - chnl->tv_regs.bel1;
+		if (chnl->port.phy.dpi.bus_width == 4)
+			chnl->tv_regs.tv_mode = MCDE_TVCRA_TVMODE_SDTV_656P_BE;
+		else
+			chnl->tv_regs.tv_mode = MCDE_TVCRA_TVMODE_SDTV_656P;
+		if (hardware_version == MCDE_CHIP_VERSION_3_0_8)
+			chnl->tv_regs.inv_clk = true;
+		else {
+			chnl->tv_regs.dho  = MCDE_CONFIG_TVOUT_HBORDER;
+			chnl->tv_regs.alw  = MCDE_CONFIG_TVOUT_HBORDER;
+			chnl->tv_regs.dvo  = MCDE_CONFIG_TVOUT_VBORDER;
+			chnl->tv_regs.bsl  = MCDE_CONFIG_TVOUT_VBORDER;
+		}
+	} else {
+		/* LCD mode */
+		u32 polarity;
+		chnl->tv_regs.hsw  = chnl->vmode.hsw;
+		chnl->tv_regs.dho  = chnl->vmode.hbp;
+		chnl->tv_regs.alw  = chnl->vmode.hfp;
+		chnl->tv_regs.bel1 = chnl->vmode.vsw;
+		chnl->tv_regs.bel2 = chnl->tv_regs.bel1;
+		chnl->tv_regs.dvo  = chnl->vmode.vbp;
+		chnl->tv_regs.bsl  = chnl->vmode.vfp;
+		chnl->tv_regs.fsl1 = 0;
+		chnl->tv_regs.fsl2 = 0;
+		polarity = chnl->port.phy.dpi.polarity;
+		chnl->tv_regs.lcdtim1 |= MCDE_LCDTIM1A_IPC(
+				(polarity & DPI_ACT_ON_FALLING_EDGE) != 0);
+		chnl->tv_regs.lcdtim1 = MCDE_LCDTIM1A_IHS(
+				(polarity & DPI_ACT_LOW_HSYNC) != 0);
+		chnl->tv_regs.lcdtim1 |= MCDE_LCDTIM1A_IVS(
+				(polarity & DPI_ACT_LOW_VSYNC) != 0);
+		chnl->tv_regs.lcdtim1 |= MCDE_LCDTIM1A_IOE(
+				(polarity & DPI_ACT_LOW_DATA_ENABLE) != 0);
+	}
+}
+
+static void update_dpi_registers(enum mcde_chnl chnl_id, struct tv_regs *regs)
+{
+	u8 idx = chnl_id;
+
+	dev_dbg(&mcde_dev->dev, "%s\n", __func__);
+	mcde_wreg(MCDE_TVCRA + idx * MCDE_TVCRA_GROUPOFFSET,
+			MCDE_TVCRA_SEL_MOD(regs->sel_mode_tv)             |
+			MCDE_TVCRA_INTEREN(regs->interlaced_en)           |
+			MCDE_TVCRA_IFIELD(0)                              |
+			MCDE_TVCRA_TVMODE(regs->tv_mode)                  |
+			MCDE_TVCRA_SDTVMODE(MCDE_TVCRA_SDTVMODE_Y0CBY1CR) |
+			MCDE_TVCRA_CKINV(regs->inv_clk)                   |
+			MCDE_TVCRA_AVRGEN(0));
+	mcde_wreg(MCDE_TVBLUA + idx * MCDE_TVBLUA_GROUPOFFSET,
+		MCDE_TVBLUA_TVBLU(MCDE_CONFIG_TVOUT_BACKGROUND_LUMINANCE) |
+		MCDE_TVBLUA_TVBCB(MCDE_CONFIG_TVOUT_BACKGROUND_CHROMINANCE_CB)|
+		MCDE_TVBLUA_TVBCR(MCDE_CONFIG_TVOUT_BACKGROUND_CHROMINANCE_CR));
+
+	/* Vertical timing registers */
+	mcde_wreg(MCDE_TVDVOA + idx * MCDE_TVDVOA_GROUPOFFSET,
+					MCDE_TVDVOA_DVO1(regs->dvo) |
+					MCDE_TVDVOA_DVO2(regs->dvo));
+	mcde_wreg(MCDE_TVBL1A + idx * MCDE_TVBL1A_GROUPOFFSET,
+					MCDE_TVBL1A_BEL1(regs->bel1) |
+					MCDE_TVBL1A_BSL1(regs->bsl));
+	mcde_wreg(MCDE_TVBL2A + idx * MCDE_TVBL1A_GROUPOFFSET,
+					MCDE_TVBL2A_BEL2(regs->bel2) |
+					MCDE_TVBL2A_BSL2(regs->bsl));
+	mcde_wreg(MCDE_TVISLA + idx * MCDE_TVISLA_GROUPOFFSET,
+					MCDE_TVISLA_FSL1(regs->fsl1) |
+					MCDE_TVISLA_FSL2(regs->fsl2));
+
+	/* Horizontal timing registers */
+	if (!regs->sel_mode_tv ||
+			hardware_version == MCDE_CHIP_VERSION_3_0_8) {
+		mcde_wreg(MCDE_TVLBALWA + idx * MCDE_TVLBALWA_GROUPOFFSET,
+					MCDE_TVLBALWA_LBW(regs->hsw) |
+					MCDE_TVLBALWA_ALW(regs->alw));
+		mcde_wreg(MCDE_TVTIM1A + idx * MCDE_TVTIM1A_GROUPOFFSET,
+					MCDE_TVTIM1A_DHO(regs->dho));
+	} else {
+		/* in earlier versions the LBW and DHO fields are swapped
+		 * TV mode only
+		 */
+		mcde_wreg(MCDE_TVLBALWA + idx * MCDE_TVLBALWA_GROUPOFFSET,
+					MCDE_TVLBALWA_LBW(regs->dho) |
+					MCDE_TVLBALWA_ALW(regs->alw));
+		mcde_wreg(MCDE_TVTIM1A + idx * MCDE_TVTIM1A_GROUPOFFSET,
+					MCDE_TVTIM1A_DHO(regs->hsw));
+	}
+	if (!regs->sel_mode_tv)
+		mcde_wreg(MCDE_LCDTIM1A + idx * MCDE_LCDTIM1A_GROUPOFFSET,
+								regs->lcdtim1);
+}
+
+static void update_col_registers(enum mcde_chnl chnl_id, struct col_regs *regs)
+{
+	u8 idx = chnl_id;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	mcde_wreg(MCDE_RGBCONV1A + idx * MCDE_RGBCONV1A_GROUPOFFSET,
+				MCDE_RGBCONV1A_YR_RED(regs->y_red) |
+				MCDE_RGBCONV1A_YR_GREEN(regs->y_green));
+	mcde_wreg(MCDE_RGBCONV2A + idx * MCDE_RGBCONV2A_GROUPOFFSET,
+				MCDE_RGBCONV2A_YR_BLUE(regs->y_blue) |
+				MCDE_RGBCONV2A_CR_RED(regs->cr_red));
+	mcde_wreg(MCDE_RGBCONV3A + idx * MCDE_RGBCONV3A_GROUPOFFSET,
+				MCDE_RGBCONV3A_CR_GREEN(regs->cr_green) |
+				MCDE_RGBCONV3A_CR_BLUE(regs->cr_blue));
+	mcde_wreg(MCDE_RGBCONV4A + idx * MCDE_RGBCONV4A_GROUPOFFSET,
+				MCDE_RGBCONV4A_CB_RED(regs->cb_red) |
+				MCDE_RGBCONV4A_CB_GREEN(regs->cb_green));
+	mcde_wreg(MCDE_RGBCONV5A + idx * MCDE_RGBCONV5A_GROUPOFFSET,
+				MCDE_RGBCONV5A_CB_BLUE(regs->cb_blue) |
+				MCDE_RGBCONV5A_OFF_RED(regs->off_cr));
+	mcde_wreg(MCDE_RGBCONV6A + idx * MCDE_RGBCONV6A_GROUPOFFSET,
+				MCDE_RGBCONV6A_OFF_GREEN(regs->off_y) |
+				MCDE_RGBCONV6A_OFF_BLUE(regs->off_cb));
+}
+
+/* MCDE internal helpers */
+static u8 portfmt2dsipacking(enum mcde_port_pix_fmt pix_fmt)
+{
+	switch (pix_fmt) {
+	case MCDE_PORTPIXFMT_DSI_16BPP:
+		return MCDE_DSIVID0CONF0_PACKING_RGB565;
+	case MCDE_PORTPIXFMT_DSI_18BPP_PACKED:
+		return MCDE_DSIVID0CONF0_PACKING_RGB666;
+	case MCDE_PORTPIXFMT_DSI_18BPP:
+	case MCDE_PORTPIXFMT_DSI_24BPP:
+	default:
+		return MCDE_DSIVID0CONF0_PACKING_RGB888;
+	case MCDE_PORTPIXFMT_DSI_YCBCR422:
+		return MCDE_DSIVID0CONF0_PACKING_HDTV;
+	}
+}
+
+static u8 portfmt2bpp(enum mcde_port_pix_fmt pix_fmt)
+{
+	/* TODO: Check DPI spec *//* REVIEW: Remove or check */
+	switch (pix_fmt) {
+	case MCDE_PORTPIXFMT_DPI_16BPP_C1:
+	case MCDE_PORTPIXFMT_DPI_16BPP_C2:
+	case MCDE_PORTPIXFMT_DPI_16BPP_C3:
+	case MCDE_PORTPIXFMT_DSI_16BPP:
+	case MCDE_PORTPIXFMT_DSI_YCBCR422:
+		return 16;
+	case MCDE_PORTPIXFMT_DPI_18BPP_C1:
+	case MCDE_PORTPIXFMT_DPI_18BPP_C2:
+	case MCDE_PORTPIXFMT_DSI_18BPP_PACKED:
+		return 18;
+	case MCDE_PORTPIXFMT_DSI_18BPP:
+	case MCDE_PORTPIXFMT_DPI_24BPP:
+	case MCDE_PORTPIXFMT_DSI_24BPP:
+		return 24;
+	default:
+		return 1;
+	}
+}
+
+static u8 bpp2outbpp(u8 bpp)
+{
+	switch (bpp) {
+	case 16:
+		return MCDE_CRA1_OUTBPP_16BPP;
+	case 18:
+		return MCDE_CRA1_OUTBPP_18BPP;
+	case 24:
+		return MCDE_CRA1_OUTBPP_24BPP;
+	default:
+		return 0;
+	}
+}
+
+static u8 portfmt2cdwin(enum mcde_port_pix_fmt pix_fmt)
+{
+	switch (pix_fmt) {
+	case MCDE_PORTPIXFMT_DPI_16BPP_C1:
+		return MCDE_CRA1_CDWIN_16BPP_C1;
+	case MCDE_PORTPIXFMT_DPI_16BPP_C2:
+		return MCDE_CRA1_CDWIN_16BPP_C2;
+	case MCDE_PORTPIXFMT_DPI_16BPP_C3:
+		return MCDE_CRA1_CDWIN_16BPP_C3;
+	case MCDE_PORTPIXFMT_DPI_18BPP_C1:
+		return MCDE_CRA1_CDWIN_18BPP_C1;
+	case MCDE_PORTPIXFMT_DPI_18BPP_C2:
+		return MCDE_CRA1_CDWIN_18BPP_C2;
+	case MCDE_PORTPIXFMT_DPI_24BPP:
+		return MCDE_CRA1_CDWIN_24BPP;
+	default:
+		/* only DPI formats are relevant */
+		return 0;
+	}
+}
+
+static u32 get_output_fifo_size(enum mcde_fifo fifo)
+{
+	u32 ret = 1; /* Avoid div by zero */
+
+	switch (fifo) {
+	case MCDE_FIFO_A:
+	case MCDE_FIFO_B:
+		ret = MCDE_FIFO_AB_SIZE;
+		break;
+	case MCDE_FIFO_C0:
+	case MCDE_FIFO_C1:
+		ret = MCDE_FIFO_C0C1_SIZE;
+		break;
+	default:
+		dev_vdbg(&mcde_dev->dev, "Unsupported fifo");
+		break;
+	}
+	return ret;
+}
+
+static u8 get_dsi_formid(const struct mcde_port *port)
+{
+	if (port->ifc == DSI_VIDEO_MODE && port->link == 0)
+		return MCDE_CTRLA_FORMID_DSI0VID;
+	else if (port->ifc == DSI_VIDEO_MODE && port->link == 1)
+		return MCDE_CTRLA_FORMID_DSI1VID;
+	else if (port->ifc == DSI_VIDEO_MODE && port->link == 2)
+		return MCDE_CTRLA_FORMID_DSI2VID;
+	else if (port->ifc == DSI_CMD_MODE && port->link == 0)
+		return MCDE_CTRLA_FORMID_DSI0CMD;
+	else if (port->ifc == DSI_CMD_MODE && port->link == 1)
+		return MCDE_CTRLA_FORMID_DSI1CMD;
+	else if (port->ifc == DSI_CMD_MODE && port->link == 2)
+		return MCDE_CTRLA_FORMID_DSI2CMD;
+	return 0;
+}
+
+static struct mcde_chnl_state *find_channel_by_dsilink(int link)
+{
+	struct mcde_chnl_state *chnl = &channels[0];
+	for (; chnl < &channels[num_channels]; chnl++)
+		if (chnl->enabled && chnl->port.link == link &&
+					chnl->port.type == MCDE_PORTTYPE_DSI)
+			return chnl;
+	return NULL;
+}
+
+static inline void mcde_handle_vcmp(struct mcde_chnl_state *chnl, u32 int_fld)
+{
+	if (!chnl->vcmp_per_field ||
+			(chnl->vcmp_per_field && chnl->even_vcmp)) {
+		chnl->transactionid_hw = chnl->transactionid_regs;
+		wake_up(&chnl->waitq_hw);
+		if (chnl->port.update_auto_trig &&
+				chnl->port.sync_src == MCDE_SYNCSRC_OFF &&
+				chnl->port.type == MCDE_PORTTYPE_DSI &&
+				chnl->continous_running) {
+			if (hardware_version == MCDE_CHIP_VERSION_3_0_8)
+				enable_channel(chnl);
+
+			mcde_wreg(MCDE_CHNL0SYNCHSW +
+				chnl->id * MCDE_CHNL0SYNCHSW_GROUPOFFSET,
+				MCDE_CHNL0SYNCHSW_SW_TRIG(true));
+
+			if (hardware_version == MCDE_CHIP_VERSION_3_0_8)
+				disable_flow(chnl);
+			mod_timer(&chnl->auto_sync_timer,
+				jiffies +
+				msecs_to_jiffies(MCDE_AUTO_SYNC_WATCHDOG
+								* 1000));
+		}
+	}
+	chnl->even_vcmp = !chnl->even_vcmp;
+	mcde_wreg_fld(MCDE_RISPP, 0x1 << int_fld, int_fld, 1);
+#ifdef DEBUG
+	if (chnl->ovly0 && mcde_rreg(MCDE_OVL0CR +
+				chnl->ovly0->idx * MCDE_OVL0CR_GROUPOFFSET) &
+				MCDE_OVL0CR_OVLB_MASK)
+		dev_warn(&mcde_dev->dev, "Overlay %d blocked\n",
+							chnl->ovly0->idx);
+	if (chnl->ovly1 && mcde_rreg(MCDE_OVL0CR +
+				chnl->ovly1->idx * MCDE_OVL0CR_GROUPOFFSET) &
+				MCDE_OVL0CR_OVLB_MASK)
+		dev_warn(&mcde_dev->dev, "Overlay %d blocked\n",
+							chnl->ovly1->idx);
+#endif
+}
+
+static irqreturn_t mcde_irq_handler(int irq, void *dev)
+{
+	int i;
+	u32 irq_status;
+#ifdef DEBUG
+	u32 irq_riserr_status;
+	u32 irq_rischnl_status;
+
+	irq_riserr_status = mcde_rreg(MCDE_RISERR);
+	irq_rischnl_status = mcde_rreg(MCDE_RISCHNL);
+#endif
+
+	/* Handle overlay irqs */
+	irq_status = mcde_rfld(MCDE_RISOVL, OVLFDRIS);
+	mcde_wfld(MCDE_RISOVL, OVLFDRIS, irq_status);
+
+	/* Handle channel irqs */
+	irq_status = mcde_rreg(MCDE_RISPP);
+	if (irq_status & MCDE_RISPP_VCMPARIS_MASK) {
+		mcde_handle_vcmp(&channels[MCDE_CHNL_A],
+						MCDE_RISPP_VCMPARIS_SHIFT);
+#ifdef DEBUG
+		if (irq_riserr_status & MCDE_RISERR_FUARIS_MASK) {
+			dev_warn(&mcde_dev->dev, "FIFO A underflow\n");
+			mcde_wreg(MCDE_RISERR, MCDE_RISERR_FUARIS_MASK);
+		}
+		if (irq_rischnl_status & MCDE_RISCHNL_CHNLARIS(0)) {
+			dev_warn(&mcde_dev->dev, "Channel A abort\n");
+			mcde_wreg(MCDE_RISCHNL, MCDE_RISCHNL_CHNLARIS(0));
+		}
+#endif
+	}
+	if (irq_status & MCDE_RISPP_VCMPBRIS_MASK) {
+		mcde_handle_vcmp(&channels[MCDE_CHNL_B],
+						MCDE_RISPP_VCMPBRIS_SHIFT);
+#ifdef DEBUG
+		if (irq_riserr_status & MCDE_RISERR_FUBRIS_MASK) {
+			dev_warn(&mcde_dev->dev, "FIFO B underflow\n");
+			mcde_wreg(MCDE_RISERR, MCDE_RISERR_FUBRIS_MASK);
+		}
+		if (irq_rischnl_status & MCDE_RISCHNL_CHNLARIS(1)) {
+			dev_warn(&mcde_dev->dev, "Channel B abort\n");
+			mcde_wreg(MCDE_RISCHNL, MCDE_RISCHNL_CHNLARIS(1));
+		}
+#endif
+	}
+	if (irq_status & MCDE_RISPP_VCMPC0RIS_MASK) {
+		mcde_handle_vcmp(&channels[MCDE_CHNL_C0],
+						MCDE_RISPP_VCMPC0RIS_SHIFT);
+#ifdef DEBUG
+		if (irq_riserr_status & MCDE_RISERR_FUC0RIS_MASK) {
+			dev_warn(&mcde_dev->dev, "FIFO C0 underflow\n");
+			mcde_wreg(MCDE_RISERR, MCDE_RISERR_FUC0RIS_MASK);
+		}
+		if (irq_rischnl_status & MCDE_RISCHNL_CHNLARIS(2)) {
+			dev_warn(&mcde_dev->dev, "Channel C0 abort\n");
+			mcde_wreg(MCDE_RISCHNL, MCDE_RISCHNL_CHNLARIS(2));
+		}
+#endif
+	}
+	if (irq_status & MCDE_RISPP_VCMPC1RIS_MASK) {
+		mcde_handle_vcmp(&channels[MCDE_CHNL_C1],
+						MCDE_RISPP_VCMPC1RIS_SHIFT);
+#ifdef DEBUG
+		if (irq_riserr_status & MCDE_RISERR_FUC1RIS_MASK) {
+			dev_warn(&mcde_dev->dev, "FIFO C1 underflow\n");
+			mcde_wreg(MCDE_RISERR, MCDE_RISERR_FUC1RIS_MASK);
+		}
+		if (irq_rischnl_status & MCDE_RISCHNL_CHNLARIS(3)) {
+			dev_warn(&mcde_dev->dev, "Channel C1 abort\n");
+			mcde_wreg(MCDE_RISCHNL, MCDE_RISCHNL_CHNLARIS(3));
+		}
+#endif
+	}
+#ifdef DEBUG
+	/* Handle error irqs */
+	if (irq_riserr_status & MCDE_RISERR_SCHBLCKDRIS_MASK) {
+		dev_warn(&mcde_dev->dev, "Scheduler blocked\n");
+		mcde_wreg(MCDE_RISERR, MCDE_RISERR_SCHBLCKDRIS_MASK);
+	}
+	if (irq_riserr_status & MCDE_IMSCERR_OVLFERRIM_MASK) {
+		dev_warn(&mcde_dev->dev, "Overlay fetch error\n");
+		mcde_wreg(MCDE_RISERR, MCDE_IMSCERR_OVLFERRIM_MASK);
+	}
+#endif
+	for (i = 0; i < num_dsilinks; i++) {
+		struct mcde_chnl_state *chnl_from_dsi;
+
+		chnl_from_dsi = find_channel_by_dsilink(i);
+
+		if (chnl_from_dsi == NULL)
+			continue;
+
+		irq_status = dsi_rfld(i, DSI_DIRECT_CMD_STS_FLAG,
+			TE_RECEIVED_FLAG);
+		if (irq_status) {
+			if (hardware_version == MCDE_CHIP_VERSION_3_0_8)
+				disable_flow(chnl_from_dsi);
+			dsi_wreg(i, DSI_DIRECT_CMD_STS_CLR,
+				DSI_DIRECT_CMD_STS_CLR_TE_RECEIVED_CLR(true));
+			dev_vdbg(&mcde_dev->dev, "BTA TE DSI%d\n", i);
+			do_softwaretrig(chnl_from_dsi);
+			dev_vdbg(&mcde_dev->dev, "SW TRIG DSI%d, chnl=%d\n", i,
+				chnl_from_dsi->id);
+		}
+
+		irq_status = dsi_rfld(i, DSI_CMD_MODE_STS_FLAG, ERR_NO_TE_FLAG);
+		if (irq_status) {
+			if (hardware_version == MCDE_CHIP_VERSION_3_0_8)
+				disable_flow(chnl_from_dsi);
+			dsi_wreg(i, DSI_CMD_MODE_STS_CLR,
+				DSI_CMD_MODE_STS_CLR_ERR_NO_TE_CLR(true));
+			dev_info(&mcde_dev->dev, "NO_TE DSI%d\n", i);
+		}
+
+		irq_status = dsi_rfld(i, DSI_DIRECT_CMD_STS, TRIGGER_RECEIVED);
+		if (irq_status) {
+			/* DSI TE polling answer received */
+			dsi_wreg(i, DSI_DIRECT_CMD_STS_CLR,
+				DSI_DIRECT_CMD_STS_CLR_TRIGGER_RECEIVED_CLR(
+						true));
+
+			if (chnl_from_dsi->port.sync_src ==
+					MCDE_SYNCSRC_TE_POLLING)
+				/* Reset timer */
+				dsi_te_poll_set_timer(chnl_from_dsi,
+						DSI_TE_NO_ANSWER_TIMEOUT);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void wait_for_channel(struct mcde_chnl_state *chnl)
+{
+	int ret;
+	u32 id = chnl->transactionid_regs;
+
+	if (chnl->transactionid_hw >= id)
+		return;
+
+	ret = wait_event_timeout(chnl->waitq_hw,
+		chnl->transactionid_hw == id,
+		msecs_to_jiffies(CHNL_TIMEOUT));
+	if (!ret)
+		dev_warn(&mcde_dev->dev,
+			"Wait for channel timeout (chnl=%d,%d<%d)!\n",
+			chnl->id, chnl->transactionid_hw,
+			id);
+}
+
+static int update_channel_static_registers(struct mcde_chnl_state *chnl)
+{
+	const struct chnl_config *cfg = chnl->cfg;
+	const struct mcde_port *port = &chnl->port;
+
+	if (hardware_version == MCDE_CHIP_VERSION_3_0_5) {
+		/* Fifo & muxing */
+		if (cfg->swap_a_c0_set)
+			mcde_wfld(MCDE_CONF0, SWAP_A_C0_V1, cfg->swap_a_c0);
+		if (cfg->swap_b_c1_set)
+			mcde_wfld(MCDE_CONF0, SWAP_B_C1_V1, cfg->swap_b_c1);
+		if (cfg->fabmux_set)
+			mcde_wfld(MCDE_CR, FABMUX_V1, cfg->fabmux);
+		if (cfg->f01mux_set)
+			mcde_wfld(MCDE_CR, F01MUX_V1, cfg->f01mux);
+
+		if (port->type == MCDE_PORTTYPE_DPI) {
+			if (port->link == 0)
+				mcde_wfld(MCDE_CR, DPIA_EN_V1, true);
+			else if (port->link == 1)
+				mcde_wfld(MCDE_CR, DPIB_EN_V1, true);
+		} else if (port->type == MCDE_PORTTYPE_DSI) {
+			if (port->ifc == DSI_VIDEO_MODE && port->link == 0)
+				mcde_wfld(MCDE_CR, DSIVID0_EN_V1, true);
+			else if (port->ifc == DSI_VIDEO_MODE && port->link == 1)
+				mcde_wfld(MCDE_CR, DSIVID1_EN_V1, true);
+			else if (port->ifc == DSI_VIDEO_MODE && port->link == 2)
+				mcde_wfld(MCDE_CR, DSIVID2_EN_V1, true);
+			else if (port->ifc == DSI_CMD_MODE && port->link == 0)
+				mcde_wfld(MCDE_CR, DSICMD0_EN_V1, true);
+			else if (port->ifc == DSI_CMD_MODE && port->link == 1)
+				mcde_wfld(MCDE_CR, DSICMD1_EN_V1, true);
+			else if (port->ifc == DSI_CMD_MODE && port->link == 2)
+				mcde_wfld(MCDE_CR, DSICMD2_EN_V1, true);
+		}
+
+		if (chnl->fifo == MCDE_FIFO_C0)
+			mcde_wreg(MCDE_CTRLC0, MCDE_CTRLC0_FIFOWTRMRK(
+					get_output_fifo_size(MCDE_FIFO_C0)));
+		else if (chnl->fifo == MCDE_FIFO_C1)
+			mcde_wreg(MCDE_CTRLC1, MCDE_CTRLC1_FIFOWTRMRK(
+					get_output_fifo_size(MCDE_FIFO_C1)));
+		else if (port->update_auto_trig &&
+					(port->sync_src == MCDE_SYNCSRC_TE0))
+			mcde_wreg(MCDE_CTRLC0, MCDE_CTRLC0_FIFOWTRMRK(
+					get_output_fifo_size(MCDE_FIFO_C0)));
+		else if (port->update_auto_trig &&
+					(port->sync_src == MCDE_SYNCSRC_TE1))
+			mcde_wreg(MCDE_CTRLC1, MCDE_CTRLC1_FIFOWTRMRK(
+					get_output_fifo_size(MCDE_FIFO_C1)));
+	} else if (hardware_version == MCDE_CHIP_VERSION_3_0_8) {
+		switch (chnl->fifo) {
+		case MCDE_FIFO_A:
+			mcde_wreg(MCDE_CHNL0MUXING_V2 + chnl->id *
+				MCDE_CHNL0MUXING_V2_GROUPOFFSET,
+				MCDE_CHNL0MUXING_V2_FIFO_ID_ENUM(FIFO_A));
+			if (port->type == MCDE_PORTTYPE_DPI) {
+				mcde_wfld(MCDE_CTRLA, FORMTYPE,
+						MCDE_CTRLA_FORMTYPE_DPITV);
+				mcde_wfld(MCDE_CTRLA, FORMID, port->link);
+				mcde_wfld(MCDE_CTRLA, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_A));
+			} else if (port->type == MCDE_PORTTYPE_DSI) {
+				mcde_wfld(MCDE_CTRLA, FORMTYPE,
+						MCDE_CTRLA_FORMTYPE_DSI);
+				mcde_wfld(MCDE_CTRLA, FORMID,
+							get_dsi_formid(port));
+				mcde_wfld(MCDE_CTRLA, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_A));
+			}
+			break;
+		case MCDE_FIFO_B:
+			mcde_wreg(MCDE_CHNL0MUXING_V2 + chnl->id *
+				MCDE_CHNL0MUXING_V2_GROUPOFFSET,
+				MCDE_CHNL0MUXING_V2_FIFO_ID_ENUM(FIFO_B));
+			if (port->type == MCDE_PORTTYPE_DPI) {
+				mcde_wfld(MCDE_CTRLB, FORMTYPE,
+						MCDE_CTRLB_FORMTYPE_DPITV);
+				mcde_wfld(MCDE_CTRLB, FORMID, port->link);
+				mcde_wfld(MCDE_CTRLB, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_B));
+			} else if (port->type == MCDE_PORTTYPE_DSI) {
+				mcde_wfld(MCDE_CTRLB, FORMTYPE,
+						MCDE_CTRLB_FORMTYPE_DSI);
+				mcde_wfld(MCDE_CTRLB, FORMID,
+							get_dsi_formid(port));
+				mcde_wfld(MCDE_CTRLB, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_B));
+			}
+
+			break;
+		case MCDE_FIFO_C0:
+			mcde_wreg(MCDE_CHNL0MUXING_V2 + chnl->id *
+				MCDE_CHNL0MUXING_V2_GROUPOFFSET,
+				MCDE_CHNL0MUXING_V2_FIFO_ID_ENUM(FIFO_C0));
+			if (port->type == MCDE_PORTTYPE_DPI)
+				return -EINVAL;
+			mcde_wfld(MCDE_CTRLC0, FORMTYPE,
+						MCDE_CTRLC0_FORMTYPE_DSI);
+			mcde_wfld(MCDE_CTRLC0, FORMID, get_dsi_formid(port));
+			mcde_wfld(MCDE_CTRLC0, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_C0));
+			break;
+		case MCDE_FIFO_C1:
+			mcde_wreg(MCDE_CHNL0MUXING_V2 + chnl->id *
+				MCDE_CHNL0MUXING_V2_GROUPOFFSET,
+				MCDE_CHNL0MUXING_V2_FIFO_ID_ENUM(FIFO_C1));
+			if (port->type == MCDE_PORTTYPE_DPI)
+				return -EINVAL;
+			mcde_wfld(MCDE_CTRLC1, FORMTYPE,
+						MCDE_CTRLC1_FORMTYPE_DSI);
+			mcde_wfld(MCDE_CTRLC1, FORMID, get_dsi_formid(port));
+			mcde_wfld(MCDE_CTRLC1, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_C1));
+			break;
+		default:
+			return -EINVAL;
+		}
+	} else if (hardware_version == MCDE_CHIP_VERSION_1_0_4) {
+		switch (chnl->fifo) {
+		case MCDE_FIFO_A:
+			/* only channel A is supported */
+			if (chnl->id != 0)
+				return -EINVAL;
+
+			if (port->type == MCDE_PORTTYPE_DSI) {
+				if ((port->link == 1 &&
+						port->ifc == DSI_VIDEO_MODE) ||
+				(port->link == 0 && port->ifc == DSI_CMD_MODE))
+					return -EINVAL;
+				mcde_wfld(MCDE_CR, DSI0_EN_V3, true);
+
+			} else if (port->type == MCDE_PORTTYPE_DPI) {
+				mcde_wfld(MCDE_CR, DPI_EN_V3, true);
+			}
+			mcde_wfld(MCDE_CTRLA, FIFOWTRMRK,
+				get_output_fifo_size(MCDE_FIFO_A));
+			break;
+		case MCDE_FIFO_B:
+			if (port->type != MCDE_PORTTYPE_DSI)
+				return -EINVAL;
+			/* only channel B is supported */
+			if (chnl->id != 1)
+				return -EINVAL;
+
+			if ((port->link == 0 && port->ifc == DSI_VIDEO_MODE) ||
+				(port->link == 1 && port->ifc == DSI_CMD_MODE))
+				return -EINVAL;
+
+			mcde_wfld(MCDE_CR, DSI1_EN_V3, true);
+			mcde_wfld(MCDE_CTRLB, FIFOWTRMRK,
+				get_output_fifo_size(MCDE_FIFO_B));
+
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+
+	/* Formatter */
+	if (port->type == MCDE_PORTTYPE_DSI) {
+		int i = 0;
+		u8 idx;
+		u8 lnk = port->link;
+		int ret;
+
+		if (port->phy.dsi.reg_vana) {
+			ret = regulator_enable(port->phy.dsi.reg_vana);
+			if (ret < 0) {
+				dev_warn(&mcde_dev->dev,
+					"%s: regulator_enable failed\n",
+								__func__);
+				goto regulator_vana_err;
+			}
+		}
+
+		if (port->phy.dsi.clk_dsi) {
+			ret = clk_enable(port->phy.dsi.clk_dsi);
+			if (ret < 0) {
+				dev_warn(&mcde_dev->dev, "%s: "
+					"clk_enable dsi failed ret = %d\n",
+								__func__, ret);
+				goto clk_dsi_err;
+			}
+		}
+		if (port->phy.dsi.clk_dsi_lp) {
+			ret = clk_enable(port->phy.dsi.clk_dsi_lp);
+			if (ret < 0) {
+				dev_warn(&mcde_dev->dev, "%s: "
+					"clk_enable dsi_lp failed ret = %d\n",
+								__func__, ret);
+				goto clk_dsi_lp_err;
+			}
+		}
+
+		if (hardware_version == MCDE_CHIP_VERSION_1_0_4)
+			idx = chnl->id;
+		else
+			idx = 2 * port->link + port->ifc;
+
+		dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, LINK_EN, true);
+		dev_dbg(&mcde_dev->dev, "DSI%d LINK_EN\n", lnk);
+
+		if (port->sync_src == MCDE_SYNCSRC_TE_POLLING) {
+			if (hardware_version == MCDE_CHIP_VERSION_3_0_5) {
+				dev_err(&mcde_dev->dev,
+				"DSI TE polling is not supported on this HW\n");
+				goto dsi_link_error;
+			}
+
+			/* Enable DSI TE polling */
+			dsi_te_poll_req(chnl);
+
+			/* Set timer to detect non TE answer */
+			dsi_te_poll_set_timer(chnl,
+					DSI_TE_NO_ANSWER_TIMEOUT_INIT);
+		} else {
+			dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, BTA_EN, true);
+			dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, READ_EN, true);
+			dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, REG_TE_EN, true);
+		}
+
+		if (hardware_version == MCDE_CHIP_VERSION_3_0_5) {
+			if (port->phy.dsi.data_lanes_swap) {
+				dev_warn(&mcde_dev->dev,
+				"DSI %d data lane remap not available!\n",
+				lnk);
+				goto dsi_link_error;
+			}
+		} else
+			dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, DLX_REMAP_EN,
+					port->phy.dsi.data_lanes_swap);
+
+		dsi_wreg(lnk, DSI_MCTL_DPHY_STATIC,
+			DSI_MCTL_DPHY_STATIC_UI_X4(port->phy.dsi.ui));
+		dsi_wreg(lnk, DSI_DPHY_LANES_TRIM,
+			DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_ENUM(0_90));
+		dsi_wreg(lnk, DSI_MCTL_DPHY_TIMEOUT,
+			DSI_MCTL_DPHY_TIMEOUT_CLK_DIV(0xf) |
+			DSI_MCTL_DPHY_TIMEOUT_HSTX_TO_VAL(0x3fff) |
+			DSI_MCTL_DPHY_TIMEOUT_LPRX_TO_VAL(0x3fff));
+		dsi_wreg(lnk, DSI_MCTL_MAIN_PHY_CTL,
+			DSI_MCTL_MAIN_PHY_CTL_WAIT_BURST_TIME(0xf) |
+			DSI_MCTL_MAIN_PHY_CTL_LANE2_EN(true) |
+			DSI_MCTL_MAIN_PHY_CTL_CLK_CONTINUOUS(
+				port->phy.dsi.clk_cont));
+		dsi_wreg(lnk, DSI_MCTL_ULPOUT_TIME,
+			DSI_MCTL_ULPOUT_TIME_CKLANE_ULPOUT_TIME(1) |
+			DSI_MCTL_ULPOUT_TIME_DATA_ULPOUT_TIME(1));
+		/* TODO: make enum */
+		dsi_wfld(lnk, DSI_CMD_MODE_CTL, ARB_MODE, false);
+		/* TODO: make enum */
+		dsi_wfld(lnk, DSI_CMD_MODE_CTL, ARB_PRI, port->ifc == 1);
+		dsi_wreg(lnk, DSI_MCTL_MAIN_EN,
+			DSI_MCTL_MAIN_EN_PLL_START(true) |
+			DSI_MCTL_MAIN_EN_CKLANE_EN(true) |
+			DSI_MCTL_MAIN_EN_DAT1_EN(true) |
+			DSI_MCTL_MAIN_EN_DAT2_EN(port->phy.dsi.num_data_lanes
+				== 2) |
+			DSI_MCTL_MAIN_EN_IF1_EN(port->ifc == 0) |
+			DSI_MCTL_MAIN_EN_IF2_EN(port->ifc == 1));
+		while (dsi_rfld(lnk, DSI_MCTL_MAIN_STS, CLKLANE_READY) == 0 ||
+			dsi_rfld(lnk, DSI_MCTL_MAIN_STS, DAT1_READY) == 0 ||
+			dsi_rfld(lnk, DSI_MCTL_MAIN_STS, DAT2_READY) == 0) {
+			mdelay(1);
+			if (i++ == 10) {
+				dev_warn(&mcde_dev->dev,
+					"DSI lane not ready (link=%d)!\n", lnk);
+				goto dsi_link_error;
+			}
+		}
+
+		mcde_wreg(MCDE_DSIVID0CONF0 +
+			idx * MCDE_DSIVID0CONF0_GROUPOFFSET,
+			MCDE_DSIVID0CONF0_BLANKING(0) |
+			MCDE_DSIVID0CONF0_VID_MODE(
+				port->mode == MCDE_PORTMODE_VID) |
+			MCDE_DSIVID0CONF0_CMD8(true) |
+			MCDE_DSIVID0CONF0_BIT_SWAP(false) |
+			MCDE_DSIVID0CONF0_BYTE_SWAP(false) |
+			MCDE_DSIVID0CONF0_DCSVID_NOTGEN(true));
+
+		if (port->mode == MCDE_PORTMODE_CMD) {
+			if (port->ifc == DSI_VIDEO_MODE)
+				dsi_wfld(port->link, DSI_CMD_MODE_CTL, IF1_ID,
+					port->phy.dsi.virt_id);
+			else if (port->ifc == DSI_CMD_MODE)
+				dsi_wfld(port->link, DSI_CMD_MODE_CTL, IF2_ID,
+					port->phy.dsi.virt_id);
+		}
+	}
+
+	if (port->type == MCDE_PORTTYPE_DPI) {
+		if (port->phy.dpi.clk_dpi) {
+			int ret;
+			ret = clk_enable(port->phy.dpi.clk_dpi);
+			if (ret < 0) {
+				dev_warn(&mcde_dev->dev, "%s: "
+					"clk_enable dsi_lp failed ret = %d\n",
+								__func__, ret);
+				goto clk_dpi_err;
+			}
+		}
+	}
+
+	mcde_wfld(MCDE_CR, MCDEEN, true);
+	chnl->formatter_updated = true;
+
+	dev_vdbg(&mcde_dev->dev, "Static registers setup, chnl=%d\n", chnl->id);
+
+	return 0;
+dsi_link_error:
+	if (port->phy.dsi.clk_dsi_lp)
+		clk_disable(port->phy.dsi.clk_dsi_lp);
+clk_dsi_lp_err:
+	if (port->phy.dsi.clk_dsi)
+		clk_disable(port->phy.dsi.clk_dsi);
+clk_dsi_err:
+	if (port->phy.dsi.reg_vana) {
+		regulator_disable(port->phy.dsi.reg_vana);
+		chnl->port.phy.dsi.reg_vana = NULL;
+	}
+regulator_vana_err:
+clk_dpi_err:
+	return -EINVAL;
+}
+
+void mcde_chnl_col_convert_apply(struct mcde_chnl_state *chnl,
+					struct mcde_col_transform *transform)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (chnl->transform != transform) {
+
+		chnl->col_regs.y_red     = transform->matrix[0][0];
+		chnl->col_regs.y_green   = transform->matrix[0][1];
+		chnl->col_regs.y_blue    = transform->matrix[0][2];
+		chnl->col_regs.cb_red    = transform->matrix[1][0];
+		chnl->col_regs.cb_green  = transform->matrix[1][1];
+		chnl->col_regs.cb_blue   = transform->matrix[1][2];
+		chnl->col_regs.cr_red    = transform->matrix[2][0];
+		chnl->col_regs.cr_green  = transform->matrix[2][1];
+		chnl->col_regs.cr_blue   = transform->matrix[2][2];
+		chnl->col_regs.off_y     = transform->offset[0];
+		chnl->col_regs.off_cb    = transform->offset[1];
+		chnl->col_regs.off_cr    = transform->offset[2];
+
+		chnl->transform = transform;
+	}
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+}
+
+static void chnl_ovly_pixel_format_apply(struct mcde_chnl_state *chnl,
+						struct mcde_ovly_state *ovly)
+{
+	struct mcde_port *port = &chnl->port;
+	struct ovly_regs *regs = &ovly->regs;
+
+	/* Note: YUV -> YUV: blending YUV overlays will not make sense. */
+	static struct mcde_col_transform crycb_2_ycbcr = {
+		/* Note that in MCDE YUV 422 pixels come as VYU pixels */
+		.matrix = {
+			{0x0000, 0x0100, 0x0000},
+			{0x0000, 0x0000, 0x0100},
+			{0x0100, 0x0000, 0x0000},
+		},
+		.offset = {0, 0, 0},
+	};
+
+	if (port->type == MCDE_PORTTYPE_DSI) {
+		if (port->pixel_format != MCDE_PORTPIXFMT_DSI_YCBCR422) {
+			if (ovly->pix_fmt != MCDE_OVLYPIXFMT_YCbCr422) {
+				/* standard case: DSI: RGB -> RGB */
+				regs->col_conv = MCDE_OVL0CR_COLCCTRL_DISABLED;
+			} else {
+				/* DSI: YUV -> RGB */
+				/* TODO change matrix */
+				regs->col_conv =
+					MCDE_OVL0CR_COLCCTRL_ENABLED_SAT;
+				mcde_chnl_col_convert_apply(chnl,
+							&chnl->ycbcr_2_rgb);
+			}
+		} else {
+			if (ovly->pix_fmt != MCDE_OVLYPIXFMT_YCbCr422)
+				/* DSI: RGB -> YUV */
+				mcde_chnl_col_convert_apply(chnl,
+							&chnl->rgb_2_ycbcr);
+			else
+				/* DSI: YUV -> YUV */
+				mcde_chnl_col_convert_apply(chnl,
+							&crycb_2_ycbcr);
+			regs->col_conv = MCDE_OVL0CR_COLCCTRL_ENABLED_NO_SAT;
+		}
+	} else if (port->type == MCDE_PORTTYPE_DPI && port->phy.dpi.tv_mode) {
+		regs->col_conv = MCDE_OVL0CR_COLCCTRL_ENABLED_NO_SAT;
+		if (ovly->pix_fmt != MCDE_OVLYPIXFMT_YCbCr422)
+			mcde_chnl_col_convert_apply(chnl, &chnl->rgb_2_ycbcr);
+		else
+			mcde_chnl_col_convert_apply(chnl, &crycb_2_ycbcr);
+	}
+}
+
+/* REVIEW: Make update_* an mcde_rectangle? */
+static void update_overlay_registers(u8 idx, struct ovly_regs *regs,
+			struct mcde_port *port, enum mcde_fifo fifo,
+			u16 update_x, u16 update_y, u16 update_w,
+			u16 update_h, s16 stride, bool interlaced,
+			enum mcde_display_rotation rotation)
+{
+	/* TODO: fix clipping for small overlay */
+	u32 lmrgn = (regs->cropx + update_x) * regs->bits_per_pixel;
+	u32 tmrgn = (regs->cropy + update_y) * stride;
+	u32 ppl = regs->ppl - update_x;
+	u32 lpf = regs->lpf - update_y;
+	s32 ljinc = stride;
+	u32 pixelfetchwtrmrklevel;
+	u8  nr_of_bufs = 1;
+	u32 fifo_size;
+	u32 sel_mod = MCDE_EXTSRC0CR_SEL_MOD_SOFTWARE_SEL;
+
+	if (rotation == MCDE_DISPLAY_ROT_180_CCW) {
+		ljinc = -ljinc;
+		tmrgn += stride * (regs->lpf - 1) / 8;
+	}
+
+	/*
+	 * Preferably most of this is done in some apply function instead of for
+	 * every update. However lpf has a dependency on update_y.
+	 */
+	if (interlaced && port->type == MCDE_PORTTYPE_DSI) {
+		nr_of_bufs = 2;
+		lpf = lpf / 2;
+		ljinc *= 2;
+	}
+
+	fifo_size = get_output_fifo_size(fifo);
+
+	if ((fifo == MCDE_FIFO_A || fifo == MCDE_FIFO_B) &&
+			regs->ppl >= fifo_size * 2)
+		pixelfetchwtrmrklevel = MCDE_PIXFETCH_LARGE_WTRMRKLVL;
+	else
+		pixelfetchwtrmrklevel = MCDE_PIXFETCH_MEDIUM_WTRMRKLVL;
+
+	if (port->update_auto_trig && port->type == MCDE_PORTTYPE_DSI) {
+		switch (port->sync_src) {
+		case MCDE_SYNCSRC_OFF:
+			sel_mod = MCDE_EXTSRC0CR_SEL_MOD_SOFTWARE_SEL;
+			break;
+		case MCDE_SYNCSRC_TE0:
+		case MCDE_SYNCSRC_TE1:
+		case MCDE_SYNCSRC_TE_POLLING:
+		default:
+			sel_mod = MCDE_EXTSRC0CR_SEL_MOD_AUTO_TOGGLE;
+			break;
+		}
+	} else if (port->type == MCDE_PORTTYPE_DPI)
+		sel_mod = MCDE_EXTSRC0CR_SEL_MOD_SOFTWARE_SEL;
+
+	regs->update = false;
+	mcde_wreg(MCDE_EXTSRC0CONF + idx * MCDE_EXTSRC0CONF_GROUPOFFSET,
+		MCDE_EXTSRC0CONF_BUF_ID(0) |
+		MCDE_EXTSRC0CONF_BUF_NB(nr_of_bufs) |
+		MCDE_EXTSRC0CONF_PRI_OVLID(idx) |
+		MCDE_EXTSRC0CONF_BPP(regs->bpp) |
+		MCDE_EXTSRC0CONF_BGR(regs->bgr) |
+		MCDE_EXTSRC0CONF_BEBO(regs->bebo) |
+		MCDE_EXTSRC0CONF_BEPO(false));
+	mcde_wreg(MCDE_EXTSRC0CR + idx * MCDE_EXTSRC0CR_GROUPOFFSET,
+		MCDE_EXTSRC0CR_SEL_MOD(sel_mod) |
+		MCDE_EXTSRC0CR_MULTIOVL_CTRL_ENUM(PRIMARY) |
+		MCDE_EXTSRC0CR_FS_DIV_DISABLE(false) |
+		MCDE_EXTSRC0CR_FORCE_FS_DIV(false));
+	mcde_wreg(MCDE_OVL0CR + idx * MCDE_OVL0CR_GROUPOFFSET,
+		MCDE_OVL0CR_OVLEN(regs->enabled) |
+	MCDE_OVL0CR_COLCCTRL(regs->col_conv) |
+		MCDE_OVL0CR_CKEYGEN(false) |
+		MCDE_OVL0CR_ALPHAPMEN(false) |
+		MCDE_OVL0CR_OVLF(false) |
+		MCDE_OVL0CR_OVLR(false) |
+		MCDE_OVL0CR_OVLB(false) |
+		MCDE_OVL0CR_FETCH_ROPC(0) |
+		MCDE_OVL0CR_STBPRIO(0) |
+		MCDE_OVL0CR_BURSTSIZE_ENUM(HW_8W) |
+		/* TODO: enum, get from ovly */
+		MCDE_OVL0CR_MAXOUTSTANDING_ENUM(8_REQ) |
+		/* TODO: _HW_8W, calculate? */
+		MCDE_OVL0CR_ROTBURSTSIZE_ENUM(HW_8W));
+	mcde_wreg(MCDE_OVL0CONF + idx * MCDE_OVL0CONF_GROUPOFFSET,
+		MCDE_OVL0CONF_PPL(ppl) |
+		MCDE_OVL0CONF_EXTSRC_ID(idx) |
+		MCDE_OVL0CONF_LPF(lpf));
+	mcde_wreg(MCDE_OVL0CONF2 + idx * MCDE_OVL0CONF2_GROUPOFFSET,
+		MCDE_OVL0CONF2_BP(regs->alpha_source) |
+		MCDE_OVL0CONF2_ALPHAVALUE(regs->alpha_value) |
+		MCDE_OVL0CONF2_OPQ(regs->opq) |
+		MCDE_OVL0CONF2_PIXOFF(lmrgn & 63) |
+		MCDE_OVL0CONF2_PIXELFETCHERWATERMARKLEVEL(
+			pixelfetchwtrmrklevel));
+	mcde_wreg(MCDE_OVL0LJINC + idx * MCDE_OVL0LJINC_GROUPOFFSET,
+		ljinc);
+	mcde_wreg(MCDE_OVL0CROP + idx * MCDE_OVL0CROP_GROUPOFFSET,
+		MCDE_OVL0CROP_TMRGN(tmrgn) |
+		MCDE_OVL0CROP_LMRGN(lmrgn >> 6));
+
+	dev_vdbg(&mcde_dev->dev, "Overlay registers setup, idx=%d\n", idx);
+}
+
+static void update_overlay_registers_on_the_fly(u8 idx, struct ovly_regs *regs)
+{
+	mcde_wreg(MCDE_OVL0COMP + idx * MCDE_OVL0COMP_GROUPOFFSET,
+		MCDE_OVL0COMP_XPOS(regs->xpos) |
+		MCDE_OVL0COMP_CH_ID(regs->ch_id) |
+		MCDE_OVL0COMP_YPOS(regs->ypos) |
+		MCDE_OVL0COMP_Z(regs->z));
+
+	mcde_wreg(MCDE_EXTSRC0A0 + idx * MCDE_EXTSRC0A0_GROUPOFFSET,
+		regs->baseaddress0);
+	mcde_wreg(MCDE_EXTSRC0A1 + idx * MCDE_EXTSRC0A1_GROUPOFFSET,
+		regs->baseaddress1);
+}
+
+static void do_softwaretrig(struct mcde_chnl_state *chnl)
+{
+	/*
+	* For main and secondary display,
+	* FLOWEN has to be set before a SOFTWARE TRIG
+	* Otherwise no overlay interrupt is triggerd
+	* However FLOWEN must not be triggered before SOFTWARE TRIG
+	* if rotation is enabled
+	*/
+	if (hardware_version == MCDE_CHIP_VERSION_3_0_8)
+		enable_channel(chnl);
+	else if ((!is_channel_enabled(chnl) && !chnl->regs.roten)
+				|| chnl->power_mode != MCDE_DISPLAY_PM_ON)
+		enable_channel(chnl);
+
+	mcde_wreg(MCDE_CHNL0SYNCHSW +
+		chnl->id * MCDE_CHNL0SYNCHSW_GROUPOFFSET,
+		MCDE_CHNL0SYNCHSW_SW_TRIG(true));
+
+	if (hardware_version == MCDE_CHIP_VERSION_3_0_8)
+		disable_flow(chnl);
+	else
+		enable_channel(chnl);
+}
+
+static void disable_flow(struct mcde_chnl_state *chnl)
+{
+	switch (chnl->id) {
+	case MCDE_CHNL_A:
+		mcde_wfld(MCDE_CRA0, FLOEN, false);
+		break;
+	case MCDE_CHNL_B:
+		mcde_wfld(MCDE_CRB0, FLOEN, false);
+		break;
+	case MCDE_CHNL_C0:
+		mcde_wfld(MCDE_CRC, C1EN, false);
+		break;
+	case MCDE_CHNL_C1:
+		mcde_wfld(MCDE_CRC, C2EN, false);
+		break;
+	}
+}
+#define MCDE_FLOWEN_MAX_TRIAL 60
+
+static void disable_channel(struct mcde_chnl_state *chnl)
+{
+	int i;
+	const struct mcde_port *port = &chnl->port;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	chnl->disable_software_trig = true;
+
+	if (port->type == MCDE_PORTTYPE_DSI) {
+		dsi_wfld(port->link, DSI_MCTL_MAIN_PHY_CTL, CLK_CONTINUOUS,
+			false);
+		if (port->sync_src == MCDE_SYNCSRC_TE_POLLING)
+			del_timer(&chnl->dsi_te_timer);
+	}
+
+	if (chnl->port.update_auto_trig &&
+			chnl->port.sync_src == MCDE_SYNCSRC_OFF &&
+			chnl->port.type == MCDE_PORTTYPE_DSI) {
+		del_timer(&chnl->auto_sync_timer);
+		chnl->continous_running = false;
+	}
+
+	if (hardware_version == MCDE_CHIP_VERSION_1_0_4) {
+		if (is_channel_enabled(chnl)) {
+			wait_for_channel(chnl);
+			/*
+			 * Just to make sure that a frame is triggered when
+			 * we try to disable the channel
+			 */
+			chnl->transactionid++;
+			mcde_wreg(MCDE_CHNL0SYNCHSW +
+				chnl->id * MCDE_CHNL0SYNCHSW_GROUPOFFSET,
+					MCDE_CHNL0SYNCHSW_SW_TRIG(true));
+			disable_flow(chnl);
+			wait_for_channel(chnl);
+			for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+				if (!is_channel_enabled(chnl)) {
+					dev_vdbg(&mcde_dev->dev,
+						"Flow %d off after >= %d ms\n",
+								chnl->id, i);
+					goto break_switch;
+				}
+				msleep(1);
+			}
+		} else {
+			dev_vdbg(&mcde_dev->dev,
+				"Flow %d disable after >= %d ms\n",
+								chnl->id, i);
+			goto break_switch;
+		}
+	} else {
+		switch (chnl->id) {
+		case MCDE_CHNL_A:
+			mcde_wfld(MCDE_CRA0, FLOEN, false);
+			wait_for_channel(chnl);
+			for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+				msleep(1);
+				if (!mcde_rfld(MCDE_CRA0, FLOEN)) {
+					dev_vdbg(&mcde_dev->dev,
+					"Flow (A) off after >= %d ms\n", i);
+					goto break_switch;
+				}
+			}
+			dev_warn(&mcde_dev->dev,
+					"%s: channel A timeout\n", __func__);
+			break;
+		case MCDE_CHNL_B:
+			mcde_wfld(MCDE_CRB0, FLOEN, false);
+			wait_for_channel(chnl);
+			for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+				msleep(1);
+				if (!mcde_rfld(MCDE_CRB0, FLOEN)) {
+					dev_vdbg(&mcde_dev->dev,
+					"Flow (B) off after >= %d ms\n", i);
+					goto break_switch;
+				}
+			}
+			dev_warn(&mcde_dev->dev, "%s: channel B timeout\n",
+								__func__);
+			break;
+		case MCDE_CHNL_C0:
+			mcde_wfld(MCDE_CRC, C1EN, false);
+			wait_for_channel(chnl);
+			for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+				msleep(1);
+				if (!mcde_rfld(MCDE_CRC, C1EN)) {
+					dev_vdbg(&mcde_dev->dev,
+					"Flow (C1) off after >= %d ms\n", i);
+					goto break_switch;
+				}
+			}
+			dev_warn(&mcde_dev->dev, "%s: channel C0 timeout\n",
+								__func__);
+			break;
+		case MCDE_CHNL_C1:
+			mcde_wfld(MCDE_CRC, C2EN, false);
+			wait_for_channel(chnl);
+			for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+				msleep(1);
+				if (!mcde_rfld(MCDE_CRC, C2EN)) {
+					dev_vdbg(&mcde_dev->dev,
+					"Flow (C2) off after >= %d ms\n", i);
+					goto break_switch;
+				}
+			}
+			dev_warn(&mcde_dev->dev, "%s: channel C1 timeout\n",
+								__func__);
+			break;
+		}
+	}
+break_switch:
+	chnl->continous_running = false;
+}
+
+static void enable_channel(struct mcde_chnl_state *chnl)
+{
+	const struct mcde_port *port = &chnl->port;
+	int i;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (port->type == MCDE_PORTTYPE_DSI)
+		dsi_wfld(port->link, DSI_MCTL_MAIN_PHY_CTL, CLK_CONTINUOUS,
+				port->phy.dsi.clk_cont);
+
+	switch (chnl->id) {
+	case MCDE_CHNL_A:
+		mcde_wfld(MCDE_CRA0, FLOEN, true);
+		for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+			if (mcde_rfld(MCDE_CRA0, FLOEN)) {
+				dev_vdbg(&mcde_dev->dev,
+					"Flow (A) enable after >= %d ms\n", i);
+				return;
+			}
+			msleep(1);
+		}
+		break;
+	case MCDE_CHNL_B:
+		mcde_wfld(MCDE_CRB0, FLOEN, true);
+		for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+			if (mcde_rfld(MCDE_CRB0, FLOEN)) {
+				dev_vdbg(&mcde_dev->dev,
+					"Flow (B) enable after >= %d ms\n", i);
+				return;
+			}
+			msleep(1);
+		}
+		break;
+	case MCDE_CHNL_C0:
+		mcde_wfld(MCDE_CRC, C1EN, true);
+		for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+			if (mcde_rfld(MCDE_CRC, C1EN)) {
+				dev_vdbg(&mcde_dev->dev,
+					"Flow (C1) enable after >= %d ms\n", i);
+				return;
+			}
+			msleep(1);
+		}
+		mcde_wfld(MCDE_CRC, POWEREN, true);
+		break;
+	case MCDE_CHNL_C1:
+		mcde_wfld(MCDE_CRC, C2EN, true);
+		for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+			if (mcde_rfld(MCDE_CRC, C2EN)) {
+				dev_vdbg(&mcde_dev->dev,
+					"Flow (C2) enable after >= %d ms\n", i);
+				return;
+			}
+			msleep(1);
+		}
+		mcde_wfld(MCDE_CRC, POWEREN, true);
+		break;
+	}
+}
+#undef MCDE_FLOWEN_MAX_TRIAL
+
+static int is_channel_enabled(struct mcde_chnl_state *chnl)
+{
+	switch (chnl->id) {
+	case MCDE_CHNL_A:
+		return mcde_rfld(MCDE_CRA0, FLOEN);
+	case MCDE_CHNL_B:
+		return mcde_rfld(MCDE_CRB0, FLOEN);
+	case MCDE_CHNL_C0:
+		return mcde_rfld(MCDE_CRC, C1EN);
+	case MCDE_CHNL_C1:
+		return mcde_rfld(MCDE_CRC, C2EN);
+	}
+	return 0;
+}
+
+static void watchdog_auto_sync_timer_function(unsigned long arg)
+{
+	int i;
+	for (i = 0; i < num_channels; i++) {
+		struct mcde_chnl_state *chnl = &channels[i];
+		if (chnl->port.update_auto_trig &&
+				chnl->port.sync_src == MCDE_SYNCSRC_OFF &&
+				chnl->port.type == MCDE_PORTTYPE_DSI &&
+				chnl->continous_running) {
+			mcde_wreg(MCDE_CHNL0SYNCHSW +
+				chnl->id
+				* MCDE_CHNL0SYNCHSW_GROUPOFFSET,
+				MCDE_CHNL0SYNCHSW_SW_TRIG(true));
+			mod_timer(&chnl->auto_sync_timer,
+				jiffies +
+				msecs_to_jiffies(MCDE_AUTO_SYNC_WATCHDOG
+								* 1000));
+		}
+	}
+}
+
+static void work_sleep_function(struct work_struct *ptr)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	if (mutex_trylock(&mcde_hw_lock) == 1) {
+		if (mcde_dynamic_power_management)
+			(void)disable_mcde_hw(false);
+		mutex_unlock(&mcde_hw_lock);
+	}
+}
+
+/* TODO get from register */
+#define MCDE_CLK_FREQ_MHZ 160
+
+void update_channel_registers(enum mcde_chnl chnl_id, struct chnl_regs *regs,
+				struct mcde_port *port, enum mcde_fifo fifo,
+				struct mcde_video_mode *video_mode)
+{
+	u8 idx = chnl_id;
+	u32 out_synch_src = MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_FORMATTER;
+	u32 src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	/* Channel */
+	if (port->update_auto_trig && port->type == MCDE_PORTTYPE_DSI) {
+		switch (port->sync_src) {
+		case MCDE_SYNCSRC_TE0:
+			out_synch_src = MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_VSYNC0;
+			src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_OUTPUT;
+			break;
+		case MCDE_SYNCSRC_OFF:
+			src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE;
+			break;
+		case MCDE_SYNCSRC_TE1:
+		default:
+			out_synch_src = MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_VSYNC1;
+			src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_OUTPUT;
+			break;
+		case MCDE_SYNCSRC_TE_POLLING:
+			src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_OUTPUT;
+			break;
+		}
+	} else if (port->type == MCDE_PORTTYPE_DPI) {
+		src_synch = port->update_auto_trig ?
+					MCDE_CHNL0SYNCHMOD_SRC_SYNCH_OUTPUT :
+					MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE;
+	}
+
+	mcde_wreg(MCDE_CHNL0CONF + idx * MCDE_CHNL0CONF_GROUPOFFSET,
+		MCDE_CHNL0CONF_PPL(regs->ppl-1) |
+		MCDE_CHNL0CONF_LPF(regs->lpf-1));
+	mcde_wreg(MCDE_CHNL0STAT + idx * MCDE_CHNL0STAT_GROUPOFFSET,
+		MCDE_CHNL0STAT_CHNLBLBCKGND_EN(false) |
+		MCDE_CHNL0STAT_CHNLRD(true));
+	mcde_wreg(MCDE_CHNL0SYNCHMOD +
+		idx * MCDE_CHNL0SYNCHMOD_GROUPOFFSET,
+		MCDE_CHNL0SYNCHMOD_SRC_SYNCH(src_synch) |
+		MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC(out_synch_src));
+	mcde_wreg(MCDE_CHNL0BCKGNDCOL + idx * MCDE_CHNL0BCKGNDCOL_GROUPOFFSET,
+		MCDE_CHNL0BCKGNDCOL_B(0) |
+		MCDE_CHNL0BCKGNDCOL_G(0) |
+		MCDE_CHNL0BCKGNDCOL_R(0));
+
+	if (chnl_id == MCDE_CHNL_A || chnl_id == MCDE_CHNL_B) {
+		u32 mcde_crx0;
+		u32 mcde_crx1;
+		u32 mcde_pal0x;
+		u32 mcde_pal1x;
+		if (chnl_id == MCDE_CHNL_A) {
+			mcde_crx0 = MCDE_CRA0;
+			mcde_crx1 = MCDE_CRA1;
+			mcde_pal0x = MCDE_PAL0A;
+			mcde_pal1x = MCDE_PAL1A;
+		} else {
+			mcde_crx0 = MCDE_CRB0;
+			mcde_crx1 = MCDE_CRB1;
+			mcde_pal0x = MCDE_PAL0B;
+			mcde_pal1x = MCDE_PAL1B;
+		}
+		mcde_wreg_fld(mcde_crx0, MCDE_CRA0_ROTEN_MASK,
+				MCDE_CRA0_ROTEN_SHIFT, regs->roten);
+		mcde_wreg_fld(mcde_crx0, MCDE_CRA0_PALEN_MASK,
+				MCDE_CRA0_PALEN_SHIFT, regs->palette_enable);
+		mcde_wreg(mcde_crx1,
+			MCDE_CRA1_PCD(regs->pcd) |
+			MCDE_CRA1_CLKSEL(regs->clksel) |
+			MCDE_CRA1_CDWIN(regs->cdwin) |
+			MCDE_CRA1_OUTBPP(bpp2outbpp(regs->bpp)) |
+			MCDE_CRA1_BCD(regs->bcd) |
+			MCDE_CRA1_CLKTYPE(regs->internal_clk)
+		);
+		if (regs->palette_enable) {
+			int i;
+			for (i = 0; i < 256; i++) {
+				mcde_wreg(mcde_pal0x,
+					MCDE_PAL0A_GREEN(regs->map_g(i)) |
+					MCDE_PAL0A_BLUE(regs->map_b(i)));
+				mcde_wreg(mcde_pal1x,
+					MCDE_PAL1A_RED(regs->map_r(i)));
+			}
+		}
+	}
+
+	/* Formatter */
+	if (port->type == MCDE_PORTTYPE_DSI) {
+		u8 fidx;
+		u32 temp, packet;
+		/* pkt_div is used to avoid underflow in output fifo for
+		 * large packets */
+		u32 pkt_div = 1;
+		u32 dsi_delay0 = 0;
+		u32 screen_ppl, screen_lpf;
+
+		if (hardware_version == MCDE_CHIP_VERSION_1_0_4)
+			fidx = chnl_id;
+		else
+			fidx = 2 * port->link + port->ifc;
+
+		screen_ppl = video_mode->xres;
+		screen_lpf = video_mode->yres;
+
+		if (screen_ppl == SCREEN_PPL_HIGH) {
+			pkt_div = (screen_ppl - 1) /
+					get_output_fifo_size(fifo) + 1;
+		} else {
+			pkt_div = screen_ppl /
+					(get_output_fifo_size(fifo) * 2) + 1;
+		}
+
+		if (video_mode->interlaced)
+			screen_lpf /= 2;
+
+		/* pkt_delay_progressive = pixelclock * htot /
+		 * (1E12 / 160E6) / pkt_div */
+		dsi_delay0 = (video_mode->pixclock + 1) *
+			(video_mode->xres + video_mode->hbp +
+				video_mode->hfp) /
+			(1000000 / MCDE_CLK_FREQ_MHZ) / pkt_div;
+
+		if ((screen_ppl == SCREEN_PPL_CEA2) &&
+				(screen_lpf == SCREEN_LPF_CEA2))
+			dsi_delay0 += DSI_DELAY0_CEA2_ADD;
+
+		temp = mcde_rreg(MCDE_DSIVID0CONF0 +
+			fidx * MCDE_DSIVID0CONF0_GROUPOFFSET);
+		mcde_wreg(MCDE_DSIVID0CONF0 +
+			fidx * MCDE_DSIVID0CONF0_GROUPOFFSET,
+			(temp & ~MCDE_DSIVID0CONF0_PACKING_MASK) |
+			MCDE_DSIVID0CONF0_PACKING(regs->dsipacking));
+		/* 1==CMD8 */
+		packet = ((screen_ppl / pkt_div * regs->bpp) >> 3) + 1;
+		mcde_wreg(MCDE_DSIVID0FRAME +
+			fidx * MCDE_DSIVID0FRAME_GROUPOFFSET,
+			MCDE_DSIVID0FRAME_FRAME(packet * pkt_div * screen_lpf));
+		mcde_wreg(MCDE_DSIVID0PKT + fidx * MCDE_DSIVID0PKT_GROUPOFFSET,
+			MCDE_DSIVID0PKT_PACKET(packet));
+		mcde_wreg(MCDE_DSIVID0SYNC +
+			fidx * MCDE_DSIVID0SYNC_GROUPOFFSET,
+			MCDE_DSIVID0SYNC_SW(0) |
+			MCDE_DSIVID0SYNC_DMA(0));
+		mcde_wreg(MCDE_DSIVID0CMDW +
+			fidx * MCDE_DSIVID0CMDW_GROUPOFFSET,
+			MCDE_DSIVID0CMDW_CMDW_START(DCS_CMD_WRITE_START) |
+			MCDE_DSIVID0CMDW_CMDW_CONTINUE(DCS_CMD_WRITE_CONTINUE));
+		mcde_wreg(MCDE_DSIVID0DELAY0 +
+			fidx * MCDE_DSIVID0DELAY0_GROUPOFFSET,
+			MCDE_DSIVID0DELAY0_INTPKTDEL(dsi_delay0));
+		mcde_wreg(MCDE_DSIVID0DELAY1 +
+			fidx * MCDE_DSIVID0DELAY1_GROUPOFFSET,
+			MCDE_DSIVID0DELAY1_TEREQDEL(0) |
+			MCDE_DSIVID0DELAY1_FRAMESTARTDEL(0));
+	}
+
+	if (regs->roten) {
+		/* TODO: Allocate memory in ESRAM instead of
+				static allocations. */
+		mcde_wreg(MCDE_ROTADD0A + chnl_id * MCDE_ROTADD0A_GROUPOFFSET,
+			regs->rotbuf1);
+		mcde_wreg(MCDE_ROTADD1A + chnl_id * MCDE_ROTADD1A_GROUPOFFSET,
+			regs->rotbuf2);
+
+		mcde_wreg(MCDE_ROTACONF + chnl_id * MCDE_ROTACONF_GROUPOFFSET,
+			MCDE_ROTACONF_ROTBURSTSIZE_ENUM(8W) |
+			MCDE_ROTACONF_ROTBURSTSIZE_HW(1) |
+			MCDE_ROTACONF_ROTDIR(regs->rotdir) |
+			MCDE_ROTACONF_STRIP_WIDTH_ENUM(16PIX) |
+			MCDE_ROTACONF_RD_MAXOUT_ENUM(4_REQ) |
+			MCDE_ROTACONF_WR_MAXOUT_ENUM(8_REQ));
+	}
+
+	/* Blending */
+	if (chnl_id == MCDE_CHNL_A) {
+		mcde_wfld(MCDE_CRA0, BLENDEN, regs->blend_en);
+		mcde_wfld(MCDE_CRA0, BLENDCTRL, regs->blend_ctrl);
+		mcde_wfld(MCDE_CRA0, ALPHABLEND, regs->alpha_blend);
+	} else if (chnl_id == MCDE_CHNL_B) {
+		mcde_wfld(MCDE_CRB0, BLENDEN, regs->blend_en);
+		mcde_wfld(MCDE_CRB0, BLENDCTRL, regs->blend_ctrl);
+		mcde_wfld(MCDE_CRB0, ALPHABLEND, regs->alpha_blend);
+	}
+
+	dev_vdbg(&mcde_dev->dev, "Channel registers setup, chnl=%d\n", chnl_id);
+}
+
+static int enable_mcde_hw(void)
+{
+	int ret;
+	int i;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	cancel_delayed_work(&hw_timeout_work);
+
+	schedule_delayed_work(&hw_timeout_work,
+					msecs_to_jiffies(MCDE_SLEEP_WATCHDOG));
+
+	if (mcde_is_enabled)
+		return 0;
+
+	ret = enable_clocks_and_power(mcde_dev);
+	if (ret < 0) {
+		dev_dbg(&mcde_dev->dev,
+			"%s: Enable clocks and power failed\n"
+						, __func__);
+		cancel_delayed_work(&hw_timeout_work);
+		return -EINVAL;
+	}
+
+	ret = request_irq(mcde_irq, mcde_irq_handler, 0, "mcde",
+							&mcde_dev->dev);
+	if (ret) {
+		dev_dbg(&mcde_dev->dev, "Failed to request irq (irq=%d)\n",
+								mcde_irq);
+		cancel_delayed_work(&hw_timeout_work);
+		return -EINVAL;
+	}
+
+	update_mcde_registers();
+
+	/* update hardware with same settings as before power down*/
+	for (i = 0; i < num_channels; i++) {
+		struct mcde_chnl_state *chnl = &channels[i];
+		if (chnl->enabled) {
+			if (chnl->ovly0 && chnl->ovly0->inuse &&
+						chnl->ovly0->regs.enabled)
+				chnl->ovly0->regs.update = true;
+			if (chnl->ovly1 && chnl->ovly1->inuse &&
+						chnl->ovly1->regs.enabled)
+				chnl->ovly1->regs.update = true;
+			chnl->transactionid++;
+		}
+	}
+
+	mcde_is_enabled = true;
+	return 0;
+}
+
+/* DSI */
+static int mcde_dsi_direct_cmd_write(struct mcde_chnl_state *chnl,
+			bool dcs, u8 cmd, u8 *data, int len)
+{
+	int i;
+	u32 wrdat[4] = { 0, 0, 0, 0 };
+	u32 settings;
+	u8 link = chnl->port.link;
+	u8 virt_id = chnl->port.phy.dsi.virt_id;
+	u32 ok;
+	u32 error;
+
+	if (len > MCDE_MAX_DSI_DIRECT_CMD_WRITE ||
+			chnl->port.type != MCDE_PORTTYPE_DSI)
+		return -EINVAL;
+
+	mutex_lock(&mcde_hw_lock);
+
+	if (enable_mcde_hw()) {
+		mutex_unlock(&mcde_hw_lock);
+		return -EINVAL;
+	}
+	if (!chnl->formatter_updated)
+		(void)update_channel_static_registers(chnl);
+
+	wait_for_channel(chnl);
+	wait_while_dsi_running(chnl->port.link);
+
+	if (dcs) {
+		wrdat[0] = cmd;
+		for (i = 1; i <= len; i++)
+			wrdat[i>>2] |= ((u32)data[i-1] << ((i & 3) * 8));
+	} else {
+		/* no explicit cmd byte for generic_write, only params */
+		for (i = 0; i < len; i++)
+			wrdat[i>>2] |= ((u32)data[i] << ((i & 3) * 8));
+	}
+
+	settings = DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_ENUM(WRITE) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT(len > 1) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID(virt_id) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE(len+1) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN(true);
+	if (dcs) {
+		if (len == 0)
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				DCS_SHORT_WRITE_0);
+		else if (len == 1)
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				DCS_SHORT_WRITE_1);
+		else
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				DCS_LONG_WRITE);
+	} else {
+		if (len == 0)
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				GENERIC_SHORT_WRITE_0);
+		else if (len == 1)
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				GENERIC_SHORT_WRITE_1);
+		else if (len == 2)
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				GENERIC_SHORT_WRITE_2);
+		else
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				GENERIC_LONG_WRITE);
+	}
+
+	dsi_wreg(link, DSI_DIRECT_CMD_MAIN_SETTINGS, settings);
+	dsi_wreg(link, DSI_DIRECT_CMD_WRDAT0, wrdat[0]);
+	if (len >  3)
+		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT1, wrdat[1]);
+	if (len >  7)
+		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT2, wrdat[2]);
+	if (len > 11)
+		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT3, wrdat[3]);
+	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR, ~0);
+	dsi_wreg(link, DSI_DIRECT_CMD_SEND, true);
+
+	/* TODO: irq wait and error check */
+	mdelay(10);
+
+	ok = dsi_rreg(link, DSI_DIRECT_CMD_STS);
+	error = dsi_rreg(link, DSI_CMD_MODE_STS);
+	dev_vdbg(&mcde_dev->dev, "DSI Write ok %x error %x\n", ok, error);
+
+	dsi_wreg(link, DSI_CMD_MODE_STS_CLR, ~0);
+	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR, ~0);
+
+	mutex_unlock(&mcde_hw_lock);
+
+	return 0;
+}
+
+int mcde_dsi_generic_write(struct mcde_chnl_state *chnl, u8* para, int len)
+{
+	return mcde_dsi_direct_cmd_write(chnl, false, 0, para, len);
+}
+
+int mcde_dsi_dcs_write(struct mcde_chnl_state *chnl, u8 cmd, u8* data, int len)
+{
+	return mcde_dsi_direct_cmd_write(chnl, true, cmd, data, len);
+}
+
+int mcde_dsi_dcs_read(struct mcde_chnl_state *chnl, u8 cmd, u8* data, int *len)
+{
+	int ret = 0;
+	u8 link = chnl->port.link;
+	u8 virt_id = chnl->port.phy.dsi.virt_id;
+	u32 settings;
+	int wait = 100;
+	bool error, ok;
+
+	if (*len > MCDE_MAX_DCS_READ || chnl->port.type != MCDE_PORTTYPE_DSI)
+		return -EINVAL;
+
+	mutex_lock(&mcde_hw_lock);
+
+	if (enable_mcde_hw()) {
+		mutex_unlock(&mcde_hw_lock);
+		return -EINVAL;
+	}
+	if (!chnl->formatter_updated)
+		(void)update_channel_static_registers(chnl);
+
+	wait_for_channel(chnl);
+	wait_while_dsi_running(chnl->port.link);
+
+	dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, BTA_EN, true);
+	dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, READ_EN, true);
+	settings = DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_ENUM(READ) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT(false) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID(virt_id) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE(1) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN(true) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(DCS_READ);
+	dsi_wreg(link, DSI_DIRECT_CMD_MAIN_SETTINGS, settings);
+	dsi_wreg(link, DSI_DIRECT_CMD_WRDAT0, cmd);
+	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR, ~0);
+	dsi_wreg(link, DSI_DIRECT_CMD_RD_STS_CLR, ~0);
+	dsi_wreg(link, DSI_DIRECT_CMD_SEND, true);
+
+	/* TODO */
+	while (wait-- && !(error = dsi_rfld(link, DSI_DIRECT_CMD_STS,
+		READ_COMPLETED_WITH_ERR)) && !(ok = dsi_rfld(link,
+		DSI_DIRECT_CMD_STS, READ_COMPLETED)))
+		mdelay(10);
+
+	if (ok) {
+		int rdsize;
+		u32 rddat;
+
+		rdsize = dsi_rfld(link, DSI_DIRECT_CMD_RD_PROPERTY, RD_SIZE);
+		rddat = dsi_rreg(link, DSI_DIRECT_CMD_RDDAT);
+		if (rdsize < *len)
+			pr_err("DCS incomplete read %d<%d (%.8X)\n",
+				rdsize, *len, rddat);/* REVIEW: dev_dbg */
+		*len = min(*len, rdsize);
+		memcpy(data, &rddat, *len);
+	} else {
+		pr_err("DCS read failed, err=%d, sts=%X\n",
+			error, dsi_rreg(link, DSI_DIRECT_CMD_STS));
+		ret = -EIO;
+	}
+
+	dsi_wreg(link, DSI_CMD_MODE_STS_CLR, ~0);
+	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR, ~0);
+
+	mutex_unlock(&mcde_hw_lock);
+
+	return ret;
+}
+
+static void dsi_te_poll_req(struct mcde_chnl_state *chnl)
+{
+	u8 lnk = chnl->port.link;
+	const struct mcde_port *port = &chnl->port;
+
+	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, REG_TE_EN, false);
+	if (port->ifc == 0)
+		dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, IF1_TE_EN, true);
+	if (port->ifc == 1)
+		dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, IF2_TE_EN, true);
+	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, BTA_EN, true);
+	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, READ_EN, true);
+	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, HOST_EOT_GEN, true);
+	dsi_wfld(lnk, DSI_CMD_MODE_CTL, TE_TIMEOUT, 0x3FF);
+	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, TE_POLLING_EN, true);
+}
+
+static void dsi_te_poll_set_timer(struct mcde_chnl_state *chnl,
+		unsigned int timeout)
+{
+	mod_timer(&chnl->dsi_te_timer,
+			jiffies +
+			msecs_to_jiffies(timeout));
+}
+
+static void dsi_te_timer_function(unsigned long arg)
+{
+	struct mcde_chnl_state *chnl;
+	u8 lnk;
+
+	if (arg >= num_channels) {
+		dev_err(&mcde_dev->dev, "%s invalid arg:%ld\n", __func__, arg);
+		return;
+	}
+
+	chnl = &channels[arg];
+
+	if (mcde_is_enabled && chnl->enabled && chnl->formatter_updated) {
+		lnk = chnl->port.link;
+		/* No TE answer; force stop */
+		dsi_wfld(lnk, DSI_MCTL_MAIN_PHY_CTL, FORCE_STOP_MODE, true);
+		udelay(20);
+		dsi_wfld(lnk, DSI_MCTL_MAIN_PHY_CTL, FORCE_STOP_MODE, false);
+		dev_info(&mcde_dev->dev, "DSI%d force stop\n", lnk);
+		dsi_te_poll_set_timer(chnl, DSI_TE_NO_ANSWER_TIMEOUT);
+	} else {
+		dev_info(&mcde_dev->dev, "1:DSI force stop\n");
+	}
+}
+
+static void dsi_te_request(struct mcde_chnl_state *chnl)
+{
+	u8 link = chnl->port.link;
+	u8 virt_id = chnl->port.phy.dsi.virt_id;
+	u32 settings;
+
+	dev_vdbg(&mcde_dev->dev, "Request BTA TE, chnl=%d\n",
+		chnl->id);
+
+	dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, BTA_EN, true);
+	dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, REG_TE_EN, true);
+	dsi_wfld(link, DSI_CMD_MODE_CTL, TE_TIMEOUT, 0x3FF);
+	settings = DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_ENUM(TE_REQ) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT(false) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID(virt_id) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE(2) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN(true) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(DCS_SHORT_WRITE_1);
+	dsi_wreg(link, DSI_DIRECT_CMD_MAIN_SETTINGS, settings);
+	dsi_wreg(link, DSI_DIRECT_CMD_WRDAT0, DCS_CMD_SET_TEAR_ON);
+	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR,
+		DSI_DIRECT_CMD_STS_CLR_TE_RECEIVED_CLR(true));
+	dsi_wfld(link, DSI_DIRECT_CMD_STS_CTL, TE_RECEIVED_EN, true);
+	dsi_wreg(link, DSI_CMD_MODE_STS_CLR,
+		DSI_CMD_MODE_STS_CLR_ERR_NO_TE_CLR(true));
+	dsi_wfld(link, DSI_CMD_MODE_STS_CTL, ERR_NO_TE_EN, true);
+	dsi_wreg(link, DSI_DIRECT_CMD_SEND, true);
+}
+
+/* MCDE channels */
+static struct mcde_chnl_state *_mcde_chnl_get(enum mcde_chnl chnl_id,
+	enum mcde_fifo fifo, const struct mcde_port *port)
+{
+	int i;
+	struct mcde_chnl_state *chnl = NULL;
+	enum mcde_chnl_path path;
+	const struct chnl_config *cfg = NULL;
+
+	static struct mcde_col_transform ycbcr_2_rgb = {
+		/* Note that in MCDE YUV 422 pixels come as VYU pixels */
+		.matrix = {
+			{0xff30, 0x012a, 0xff9c},
+			{0x0000, 0x012a, 0x0204},
+			{0x0199, 0x012a, 0x0000},
+		},
+		.offset = {0x0088, 0xfeeb, 0xff21},
+	};
+
+	static struct mcde_col_transform rgb_2_ycbcr = {
+		.matrix = {
+			{0x0042, 0x0081, 0x0019},
+			{0xffda, 0xffb6, 0x0070},
+			{0x0070, 0xffa2, 0xffee},
+		},
+		.offset = {0x0010, 0x0080, 0x0080},
+	};
+
+	/* Allocate channel */
+	for (i = 0; i < num_channels; i++) {
+		if (chnl_id == channels[i].id)
+			chnl = &channels[i];
+	}
+	if (!chnl) {
+		dev_dbg(&mcde_dev->dev, "Invalid channel, chnl=%d\n", chnl_id);
+		return ERR_PTR(-EINVAL);
+	}
+	if (chnl->reserved) {
+		dev_dbg(&mcde_dev->dev, "Channel in use, chnl=%d\n", chnl_id);
+		return ERR_PTR(-EBUSY);
+	}
+
+	if (hardware_version == MCDE_CHIP_VERSION_3_0_5) {
+		path = MCDE_CHNLPATH(chnl->id, fifo, port->type,
+						port->ifc, port->link);
+		for (i = 0; i < ARRAY_SIZE(chnl_configs); i++)
+			if (chnl_configs[i].path == path) {
+				cfg = &chnl_configs[i];
+				break;
+			}
+		if (cfg == NULL) {
+			dev_dbg(&mcde_dev->dev, "Invalid config, chnl=%d,"
+					" path=0x%.8X\n", chnl_id, path);
+			return ERR_PTR(-EINVAL);
+		} else {
+			dev_info(&mcde_dev->dev, "Config, chnl=%d,"
+					" path=0x%.8X\n", chnl_id, path);
+		}
+	}
+	/* TODO: verify that cfg is ok to activate (check other chnl cfgs) */
+
+	chnl->cfg = cfg;
+	chnl->port = *port;
+	chnl->fifo = fifo;
+	chnl->formatter_updated = false;
+	chnl->ycbcr_2_rgb = ycbcr_2_rgb;
+	chnl->rgb_2_ycbcr = rgb_2_ycbcr;
+
+	chnl->blend_en = true;
+	chnl->blend_ctrl = MCDE_CRA0_BLENDCTRL_SOURCE;
+	chnl->alpha_blend = 0xFF;
+
+	_mcde_chnl_apply(chnl);
+	chnl->reserved = true;
+
+	if (chnl->port.type == MCDE_PORTTYPE_DSI) {
+#ifdef CONFIG_REGULATOR
+		chnl->port.phy.dsi.reg_vana = regulator_vana;
+#else
+		chnl->port.phy.dsi.reg_vana = NULL;
+#endif
+		chnl->port.phy.dsi.clk_dsi = clock_dsi;
+		chnl->port.phy.dsi.clk_dsi_lp = clock_dsi_lp;
+		enable_dsi++;
+		if (!dsi_is_enabled && mcde_is_enabled) {
+			struct mcde_platform_data *pdata =
+					mcde_dev->dev.platform_data;
+			pdata->platform_enable_dsipll();
+			dsi_is_enabled = true;
+		}
+	} else if (chnl->port.type == MCDE_PORTTYPE_DPI) {
+		chnl->port.phy.dpi.clk_dpi = clock_dpi;
+		if (chnl->port.phy.dpi.tv_mode)
+			chnl->vcmp_per_field = true;
+	}
+
+#ifdef CONFIG_REGULATOR
+	chnl->port.reg_esram = regulator_esram_epod;
+#else
+	chnl->port.reg_esram = NULL;
+#endif
+
+	return chnl;
+}
+
+static int _mcde_chnl_apply(struct mcde_chnl_state *chnl)
+{
+	bool roten = false;
+	u8 rotdir = 0;
+
+	if (chnl->rotation == MCDE_DISPLAY_ROT_90_CCW) {
+		roten = true;
+		rotdir = MCDE_ROTACONF_ROTDIR_CCW;
+	} else if (chnl->rotation == MCDE_DISPLAY_ROT_90_CW) {
+		roten = true;
+		rotdir = MCDE_ROTACONF_ROTDIR_CW;
+	}
+	/* REVIEW: 180 deg? */
+
+	chnl->regs.bpp = portfmt2bpp(chnl->port.pixel_format);
+	chnl->regs.synchronized_update = chnl->synchronized_update;
+	chnl->regs.roten = roten;
+	chnl->regs.rotdir = rotdir;
+	chnl->regs.rotbuf1 = chnl->rotbuf1;
+	chnl->regs.rotbuf2 = chnl->rotbuf2;
+	chnl->regs.palette_enable = chnl->palette_enable;
+	chnl->regs.map_r = chnl->map_r;
+	chnl->regs.map_g = chnl->map_g;
+	chnl->regs.map_b = chnl->map_b;
+	if (chnl->port.type == MCDE_PORTTYPE_DSI) {
+		chnl->regs.clksel = MCDE_CRA1_CLKSEL_166MHZ;
+		chnl->regs.dsipacking =
+				portfmt2dsipacking(chnl->port.pixel_format);
+	} else if (chnl->port.type == MCDE_PORTTYPE_DPI) {
+		if (chnl->port.phy.dpi.tv_mode) {
+			chnl->regs.internal_clk = false;
+			chnl->regs.bcd = true;
+			if (chnl->id == MCDE_CHNL_A)
+				chnl->regs.clksel = MCDE_CRA1_CLKSEL_EXT_TV1;
+			else
+				chnl->regs.clksel = MCDE_CRA1_CLKSEL_EXT_TV2;
+		} else {
+			chnl->regs.internal_clk = true;
+			chnl->regs.clksel = MCDE_CRA1_CLKSEL_LCD;
+			chnl->regs.cdwin =
+					portfmt2cdwin(chnl->port.pixel_format);
+			chnl->regs.bcd = (chnl->port.phy.dpi.clock_div < 2);
+			if (!chnl->regs.bcd)
+				chnl->regs.pcd =
+					chnl->port.phy.dpi.clock_div - 2;
+		}
+		dpi_video_mode_apply(chnl);
+	}
+
+	chnl->regs.blend_ctrl = chnl->blend_ctrl;
+	chnl->regs.blend_en = chnl->blend_en;
+	chnl->regs.alpha_blend = chnl->alpha_blend;
+
+	chnl->transactionid++;
+
+	dev_vdbg(&mcde_dev->dev, "Channel applied, chnl=%d\n", chnl->id);
+	return 0;
+}
+
+static void chnl_update_registers(struct mcde_chnl_state *chnl)
+{
+	/* REVIEW: Move content to update_channel_register */
+	/* and remove this one */
+	if (chnl->port.type == MCDE_PORTTYPE_DPI)
+		update_dpi_registers(chnl->id, &chnl->tv_regs);
+	if (chnl->id == MCDE_CHNL_A || chnl->id == MCDE_CHNL_B)
+		update_col_registers(chnl->id, &chnl->col_regs);
+	update_channel_registers(chnl->id, &chnl->regs, &chnl->port,
+						chnl->fifo, &chnl->vmode);
+
+	chnl->transactionid_regs = chnl->transactionid;
+}
+
+static void chnl_update_continous(struct mcde_chnl_state *chnl,
+						bool tripple_buffer)
+{
+
+	if (chnl->continous_running) {
+		chnl->transactionid_regs = chnl->transactionid;
+		if (!tripple_buffer)
+			wait_for_channel(chnl);
+	}
+
+	if (!chnl->continous_running) {
+		if (chnl->transactionid_regs < chnl->transactionid)
+			chnl_update_registers(chnl);
+		if (chnl->port.sync_src == MCDE_SYNCSRC_TE0) {
+			mcde_wfld(MCDE_CRC, SYCEN0, true);
+		 } else if (chnl->port.sync_src == MCDE_SYNCSRC_TE1) {
+			if (hardware_version == MCDE_CHIP_VERSION_3_0_8) {
+				mcde_wfld(MCDE_VSCRC1, VSSEL, 1);
+				mcde_wfld(MCDE_CRC, SYCEN1, true);
+			} else {
+				mcde_wfld(MCDE_VSCRC1, VSSEL, 0);
+				mcde_wfld(MCDE_CRC, SYCEN0, true);
+			}
+		}
+		chnl->continous_running = true;
+
+		/*
+		* For main and secondary display,
+		* FLOWEN has to be set before a SOFTWARE TRIG
+		* Otherwise not overlay interrupt is triggerd
+		*/
+		enable_channel(chnl);
+
+		if (chnl->port.type == MCDE_PORTTYPE_DSI &&
+				chnl->port.sync_src == MCDE_SYNCSRC_OFF) {
+			chnl->disable_software_trig = false;
+			if (hardware_version == MCDE_CHIP_VERSION_3_0_8) {
+				mcde_wreg(MCDE_CHNL0SYNCHSW +
+				chnl->id * MCDE_CHNL0SYNCHSW_GROUPOFFSET,
+					MCDE_CHNL0SYNCHSW_SW_TRIG(true));
+				disable_flow(chnl);
+			}
+			mod_timer(&chnl->auto_sync_timer,
+					jiffies +
+			msecs_to_jiffies(MCDE_AUTO_SYNC_WATCHDOG * 1000));
+		}
+	}
+}
+
+static void chnl_update_non_continous(struct mcde_chnl_state *chnl)
+{
+	/* Commit settings to registers */
+	wait_for_channel(chnl);
+	if (chnl->transactionid_regs < chnl->transactionid)
+		chnl_update_registers(chnl);
+
+	/* TODO: look at port sync source and synched_update */
+	if (chnl->regs.synchronized_update &&
+			chnl->power_mode == MCDE_DISPLAY_PM_ON) {
+		if (chnl->port.type == MCDE_PORTTYPE_DSI &&
+			chnl->port.sync_src == MCDE_SYNCSRC_BTA) {
+			if (hardware_version == MCDE_CHIP_VERSION_3_0_8)
+				enable_channel(chnl);
+			wait_while_dsi_running(chnl->port.link);
+			dsi_te_request(chnl);
+		}
+	} else {
+		do_softwaretrig(chnl);
+		dev_vdbg(&mcde_dev->dev, "Channel update (no sync), chnl=%d\n",
+			chnl->id);
+	}
+
+}
+
+static void chnl_update_overlay(struct mcde_chnl_state *chnl,
+						struct mcde_ovly_state *ovly)
+{
+	if (!ovly)
+		return;
+
+	update_overlay_registers_on_the_fly(ovly->idx, &ovly->regs);
+	if (ovly->regs.update) {
+		chnl_ovly_pixel_format_apply(chnl, ovly);
+		update_overlay_registers(ovly->idx, &ovly->regs, &chnl->port,
+			chnl->fifo, chnl->regs.x, chnl->regs.y,
+			chnl->regs.ppl, chnl->regs.lpf, ovly->stride,
+			chnl->vmode.interlaced, chnl->rotation);
+	}
+}
+
+static int _mcde_chnl_update(struct mcde_chnl_state *chnl,
+					struct mcde_rectangle *update_area,
+					bool tripple_buffer)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	/* TODO: lock & make wait->trig async */
+	if (!chnl->enabled || !update_area
+			|| (update_area->w == 0 && update_area->h == 0)) {
+		return -EINVAL;
+	}
+
+	if (chnl->port.update_auto_trig && tripple_buffer)
+		wait_for_channel(chnl);
+
+	chnl->regs.x   = update_area->x;
+	chnl->regs.y   = update_area->y;
+	/* TODO Crop against video_mode.xres and video_mode.yres */
+	chnl->regs.ppl = update_area->w;
+	chnl->regs.lpf = update_area->h;
+	if (chnl->port.type == MCDE_PORTTYPE_DPI &&
+						chnl->port.phy.dpi.tv_mode) {
+		/* subtract border */
+		chnl->regs.ppl -= chnl->tv_regs.dho + chnl->tv_regs.alw;
+		/* subtract double borders, ie. for both fields */
+		chnl->regs.lpf -= 2 * (chnl->tv_regs.dvo + chnl->tv_regs.bsl);
+	} else if (chnl->port.type == MCDE_PORTTYPE_DSI &&
+			chnl->vmode.interlaced)
+		chnl->regs.lpf /= 2;
+
+	chnl_update_overlay(chnl, chnl->ovly0);
+	chnl_update_overlay(chnl, chnl->ovly1);
+
+	if (chnl->port.update_auto_trig)
+		chnl_update_continous(chnl, tripple_buffer);
+	else
+		chnl_update_non_continous(chnl);
+
+	dev_vdbg(&mcde_dev->dev, "Channel updated, chnl=%d\n", chnl->id);
+	return 0;
+}
+
+/* API entry points */
+/* MCDE channels */
+struct mcde_chnl_state *mcde_chnl_get(enum mcde_chnl chnl_id,
+			enum mcde_fifo fifo, const struct mcde_port *port)
+{
+	struct mcde_chnl_state *chnl;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	chnl = _mcde_chnl_get(chnl_id, fifo, port);
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+
+	return chnl;
+}
+
+int mcde_chnl_set_pixel_format(struct mcde_chnl_state *chnl,
+					enum mcde_port_pix_fmt pix_fmt)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+	chnl->port.pixel_format = pix_fmt;
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+
+	return 0;
+}
+
+int mcde_chnl_set_palette(struct mcde_chnl_state *chnl,
+					struct mcde_palette_table *palette)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+	if (palette != NULL) {
+		chnl->map_r = palette->map_col_ch0;
+		chnl->map_g = palette->map_col_ch1;
+		chnl->map_b = palette->map_col_ch2;
+		chnl->palette_enable = true;
+	} else {
+		chnl->map_r = NULL;
+		chnl->map_g = NULL;
+		chnl->map_b = NULL;
+		chnl->palette_enable = false;
+	}
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+	return 0;
+}
+
+void mcde_chnl_set_col_convert(struct mcde_chnl_state *chnl,
+					struct mcde_col_transform *transform,
+					enum   mcde_col_convert    convert)
+{
+	switch (convert) {
+	case MCDE_CONVERT_RGB_2_YCBCR:
+		memcpy(&chnl->rgb_2_ycbcr, transform,
+				sizeof(struct mcde_col_transform));
+		/* force update: */
+		if (chnl->transform == &chnl->rgb_2_ycbcr) {
+			chnl->transform = NULL;
+			chnl->ovly0->update = true;
+			chnl->ovly1->update = true;
+		}
+		break;
+	case MCDE_CONVERT_YCBCR_2_RGB:
+		memcpy(&chnl->ycbcr_2_rgb, transform,
+				sizeof(struct mcde_col_transform));
+		/* force update: */
+		if (chnl->transform == &chnl->ycbcr_2_rgb) {
+			chnl->transform = NULL;
+			chnl->ovly0->update = true;
+			chnl->ovly1->update = true;
+		}
+		break;
+	default:
+		/* Trivial transforms are handled internally */
+		dev_warn(&mcde_dev->dev,
+			"%s: unsupported col convert\n", __func__);
+		break;
+	}
+}
+
+int mcde_chnl_set_video_mode(struct mcde_chnl_state *chnl,
+					struct mcde_video_mode *vmode)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (chnl == NULL || vmode == NULL)
+		return -EINVAL;
+
+	chnl->vmode = *vmode;
+
+	if (chnl->ovly0)
+		chnl->ovly0->update = true;
+	if (chnl->ovly1)
+		chnl->ovly1->update = true;
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+
+	return 0;
+}
+EXPORT_SYMBOL(mcde_chnl_set_video_mode);
+
+int mcde_chnl_set_rotation(struct mcde_chnl_state *chnl,
+		enum mcde_display_rotation rotation, u32 rotbuf1, u32 rotbuf2)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+
+	if (chnl->id != MCDE_CHNL_A && chnl->id != MCDE_CHNL_B)
+		return -EINVAL;
+
+	chnl->rotation = rotation;
+	chnl->rotbuf1  = rotbuf1;
+	chnl->rotbuf2  = rotbuf2;
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+
+	return 0;
+}
+
+int mcde_chnl_enable_synchronized_update(struct mcde_chnl_state *chnl,
+								bool enable)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	if (!chnl->reserved)
+		return -EINVAL;
+
+	chnl->synchronized_update = enable;
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+
+	return 0;
+}
+
+int mcde_chnl_set_power_mode(struct mcde_chnl_state *chnl,
+				enum mcde_display_power_mode power_mode)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+
+	chnl->power_mode = power_mode;
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+
+	return 0;
+}
+
+int mcde_chnl_apply(struct mcde_chnl_state *chnl)
+{
+	int ret ;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+
+	mutex_lock(&mcde_hw_lock);
+	ret = _mcde_chnl_apply(chnl);
+	mutex_unlock(&mcde_hw_lock);
+
+	dev_vdbg(&mcde_dev->dev, "%s exit with ret %d\n", __func__, ret);
+
+	return ret;
+}
+
+int mcde_chnl_update(struct mcde_chnl_state *chnl,
+					struct mcde_rectangle *update_area,
+					bool tripple_buffer)
+{
+	int ret;
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+
+	mutex_lock(&mcde_hw_lock);
+	enable_mcde_hw();
+	if (!chnl->formatter_updated)
+		(void)update_channel_static_registers(chnl);
+
+	if (chnl->regs.roten && !chnl->esram_is_enabled) {
+		ret = regulator_enable(chnl->port.reg_esram);
+		if (ret < 0) {
+			dev_warn(&mcde_dev->dev, "%s: disable failed\n",
+			__func__);
+		}
+		chnl->esram_is_enabled = true;
+	}
+
+	ret = _mcde_chnl_update(chnl, update_area, tripple_buffer);
+
+	mutex_unlock(&mcde_hw_lock);
+
+	dev_vdbg(&mcde_dev->dev, "%s exit with ret %d\n", __func__, ret);
+
+	return ret;
+}
+
+void mcde_chnl_put(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	chnl->reserved = false;
+	chnl->port.reg_esram = NULL;
+	if (chnl->port.type == MCDE_PORTTYPE_DSI) {
+		chnl->port.phy.dsi.reg_vana = NULL;
+		chnl->port.phy.dsi.clk_dsi = NULL;
+		chnl->port.phy.dsi.clk_dsi_lp = NULL;
+		enable_dsi--;
+		if (dsi_is_enabled && enable_dsi == 0) {
+			struct mcde_platform_data *pdata =
+					mcde_dev->dev.platform_data;
+			pdata->platform_disable_dsipll();
+			dsi_is_enabled = false;
+		}
+	} else if (chnl->port.type == MCDE_PORTTYPE_DPI) {
+		chnl->port.phy.dpi.clk_dpi = NULL;
+		if (chnl->port.phy.dpi.tv_mode) {
+			chnl->vcmp_per_field = false;
+			chnl->even_vcmp = false;
+		}
+	}
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+}
+
+void mcde_chnl_stop_flow(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	mutex_lock(&mcde_hw_lock);
+	if (mcde_is_enabled && chnl->enabled)
+		disable_channel(chnl);
+	mutex_unlock(&mcde_hw_lock);
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+}
+
+void mcde_chnl_enable(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	mutex_lock(&mcde_hw_lock);
+	chnl->enabled = true;
+	mutex_unlock(&mcde_hw_lock);
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+}
+
+void mcde_chnl_disable(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	mutex_lock(&mcde_hw_lock);
+	cancel_delayed_work(&hw_timeout_work);
+	/* This channel is disabled here */
+	chnl->enabled = false;
+	if (mcde_is_enabled && chnl->formatter_updated
+				&& chnl->port.type == MCDE_PORTTYPE_DSI)
+		wait_while_dsi_running(chnl->port.link);
+
+	if (chnl->formatter_updated) {
+		disable_formatter(&chnl->port);
+		chnl->formatter_updated = false;
+	}
+	if (chnl->esram_is_enabled) {
+		int ret;
+		ret = regulator_disable(chnl->port.reg_esram);
+		if (ret < 0) {
+			dev_warn(&mcde_dev->dev, "%s: disable failed\n",
+			__func__);
+		}
+		chnl->esram_is_enabled = false;
+	}
+	del_timer(&chnl->dsi_te_timer);
+	del_timer(&chnl->auto_sync_timer);
+	/*
+	 * Check if other channels can be disabled and
+	 * if the hardware can be shutdown
+	 */
+	(void)disable_mcde_hw(false);
+	mutex_unlock(&mcde_hw_lock);
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+}
+
+/* MCDE overlays */
+struct mcde_ovly_state *mcde_ovly_get(struct mcde_chnl_state *chnl)
+{
+	struct mcde_ovly_state *ovly;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return ERR_PTR(-EINVAL);
+
+	if (!chnl->ovly0->inuse)
+		ovly = chnl->ovly0;
+	else if (chnl->ovly1 && !chnl->ovly1->inuse)
+		ovly = chnl->ovly1;
+	else
+		ovly = ERR_PTR(-EBUSY);
+
+	if (!IS_ERR(ovly)) {
+		ovly->inuse = true;
+		ovly->paddr = 0;
+		ovly->stride = 0;
+		ovly->pix_fmt = MCDE_OVLYPIXFMT_RGB565;
+		ovly->src_x = 0;
+		ovly->src_y = 0;
+		ovly->dst_x = 0;
+		ovly->dst_y = 0;
+		ovly->dst_z = 0;
+		ovly->w = 0;
+		ovly->h = 0;
+		ovly->alpha_value = 0xFF;
+		ovly->alpha_source = MCDE_OVL1CONF2_BP_PER_PIXEL_ALPHA;
+		ovly->update = true;
+		mcde_ovly_apply(ovly);
+	}
+
+	return ovly;
+}
+
+void mcde_ovly_put(struct mcde_ovly_state *ovly)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!ovly->inuse)
+		return;
+	if (ovly->regs.enabled) {
+		ovly->paddr = 0;
+		ovly->update = true;
+		mcde_ovly_apply(ovly);/* REVIEW: API call calling API call! */
+	}
+	ovly->inuse = false;
+}
+
+void mcde_ovly_set_source_buf(struct mcde_ovly_state *ovly, u32 paddr)
+{
+	if (!ovly->inuse)
+		return;
+
+	if (paddr == 0 || ovly->paddr == 0)
+		ovly->update = true;
+
+	ovly->paddr = paddr;
+}
+
+void mcde_ovly_set_source_info(struct mcde_ovly_state *ovly,
+	u32 stride, enum mcde_ovly_pix_fmt pix_fmt)
+{
+	if (!ovly->inuse)
+		return;
+
+	ovly->stride = stride;
+	ovly->pix_fmt = pix_fmt;
+	ovly->update = true;
+}
+
+void mcde_ovly_set_source_area(struct mcde_ovly_state *ovly,
+	u16 x, u16 y, u16 w, u16 h)
+{
+	if (!ovly->inuse)
+		return;
+
+	ovly->src_x = x;
+	ovly->src_y = y;
+	ovly->w = w;
+	ovly->h = h;
+	ovly->update = true;
+}
+
+void mcde_ovly_set_dest_pos(struct mcde_ovly_state *ovly, u16 x, u16 y, u8 z)
+{
+	if (!ovly->inuse)
+		return;
+
+	ovly->dst_x = x;
+	ovly->dst_y = y;
+	ovly->dst_z = z;
+}
+
+void mcde_ovly_apply(struct mcde_ovly_state *ovly)
+{
+	if (!ovly->inuse)
+		return;
+
+	mutex_lock(&mcde_hw_lock);
+
+	ovly->regs.ch_id = ovly->chnl->id;
+	ovly->regs.enabled = ovly->paddr != 0;
+	ovly->regs.baseaddress0 = ovly->paddr;
+	ovly->regs.baseaddress1 = ovly->regs.baseaddress0 + ovly->stride;
+	/*TODO set to true if interlaced *//* REVIEW: Video mode interlaced? */
+	if (!ovly->regs.update)
+		ovly->regs.update = ovly->update;
+	ovly->update = false;
+
+	switch (ovly->pix_fmt) {/* REVIEW: Extract to table */
+	case MCDE_OVLYPIXFMT_RGB565:
+		ovly->regs.bits_per_pixel = 16;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_RGB565;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = true;
+		break;
+	case MCDE_OVLYPIXFMT_RGBA5551:
+		ovly->regs.bits_per_pixel = 16;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_IRGB1555;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = false;
+		break;
+	case MCDE_OVLYPIXFMT_RGBA4444:
+		ovly->regs.bits_per_pixel = 16;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_ARGB4444;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = false;
+		break;
+	case MCDE_OVLYPIXFMT_RGB888:
+		ovly->regs.bits_per_pixel = 24;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_RGB888;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = true;
+		break;
+	case MCDE_OVLYPIXFMT_RGBX8888:
+		ovly->regs.bits_per_pixel = 32;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_XRGB8888;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = true;
+		ovly->regs.opq = true;
+		break;
+	case MCDE_OVLYPIXFMT_RGBA8888:
+		ovly->regs.bits_per_pixel = 32;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_ARGB8888;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = false;
+		break;
+	case MCDE_OVLYPIXFMT_YCbCr422:
+		ovly->regs.bits_per_pixel = 16;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_YCBCR422;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = true;
+		break;
+	default:
+		break;
+	}
+
+	ovly->regs.ppl = ovly->w;
+	ovly->regs.lpf = ovly->h;
+	ovly->regs.cropx = ovly->src_x;
+	ovly->regs.cropy = ovly->src_y;
+	ovly->regs.xpos = ovly->dst_x;
+	ovly->regs.ypos = ovly->dst_y;
+	ovly->regs.z = ovly->dst_z > 0; /* 0 or 1 */
+	ovly->regs.col_conv = MCDE_OVL0CR_COLCCTRL_DISABLED;
+	ovly->regs.alpha_source = ovly->alpha_source;
+	ovly->regs.alpha_value = ovly->alpha_value;
+	ovly->chnl->transactionid++;
+
+	mutex_unlock(&mcde_hw_lock);
+
+	dev_vdbg(&mcde_dev->dev, "Overlay applied, idx=%d chnl=%d\n",
+						ovly->idx, ovly->chnl->id);
+}
+
+static int init_clocks_and_power(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct mcde_platform_data *pdata = pdev->dev.platform_data;
+
+#ifdef CONFIG_REGULATOR
+	if (pdata->regulator_mcde_epod_id) {
+		regulator_mcde_epod = regulator_get(&pdev->dev,
+				pdata->regulator_mcde_epod_id);
+		if (IS_ERR(regulator_mcde_epod)) {
+			ret = PTR_ERR(regulator_mcde_epod);
+			dev_warn(&pdev->dev,
+				"%s: Failed to get regulator '%s'\n",
+				__func__, pdata->regulator_mcde_epod_id);
+			regulator_mcde_epod = NULL;
+			return ret;
+		}
+	} else {
+		dev_dbg(&pdev->dev, "%s: No regulator id supplied\n",
+								__func__);
+		return -EINVAL;
+	}
+
+	if (pdata->regulator_esram_epod_id) {
+		regulator_esram_epod = regulator_get(&pdev->dev,
+				pdata->regulator_esram_epod_id);
+		if (IS_ERR(regulator_esram_epod)) {
+			ret = PTR_ERR(regulator_esram_epod);
+			dev_warn(&pdev->dev,
+				"%s: Failed to get regulator '%s'\n",
+				__func__, pdata->regulator_esram_epod_id);
+			regulator_esram_epod = NULL;
+			goto regulator_esram_err;
+		}
+	} else {
+		dev_dbg(&pdev->dev, "%s: No regulator id supplied\n",
+								__func__);
+	}
+
+	if (pdata->regulator_vana_id) {
+		regulator_vana = regulator_get(&pdev->dev,
+				pdata->regulator_vana_id);
+		if (IS_ERR(regulator_vana)) {
+			ret = PTR_ERR(regulator_vana);
+			dev_warn(&pdev->dev,
+				"%s: Failed to get regulator '%s'\n",
+				__func__, pdata->regulator_vana_id);
+			regulator_vana = NULL;
+			goto regulator_vana_err;
+		}
+	} else {
+		dev_dbg(&pdev->dev, "%s: No regulator id supplied\n",
+								__func__);
+		ret = -EINVAL;
+		goto regulator_vana_err;
+	}
+#endif
+
+	clock_dsi = clk_get(&pdev->dev, pdata->clock_dsi_id);
+	if (IS_ERR(clock_dsi)) {
+		ret = PTR_ERR(clock_dsi);
+		dev_warn(&pdev->dev, "%s: Failed to get clock '%s'\n",
+					__func__, pdata->clock_dsi_id);
+		goto clk_dsi_err;
+	}
+
+	clock_dsi_lp = clk_get(&pdev->dev, pdata->clock_dsi_lp_id);
+	if (IS_ERR(clock_dsi_lp)) {
+		ret = PTR_ERR(clock_dsi_lp);
+		dev_warn(&pdev->dev, "%s: Failed to get clock '%s'\n",
+					__func__, pdata->clock_dsi_lp_id);
+		goto clk_dsi_lp_err;
+	}
+
+	clock_dpi = clk_get(&pdev->dev, pdata->clock_dpi_id);
+	if (IS_ERR(clock_dpi)) {
+		ret = PTR_ERR(clock_dpi);
+		dev_warn(&pdev->dev, "%s: Failed to get clock '%s'\n",
+					__func__, pdata->clock_dpi_id);
+		goto clk_dpi_err;
+	}
+
+	clock_mcde = clk_get(&pdev->dev, pdata->clock_mcde_id);
+	if (IS_ERR(clock_mcde)) {
+		ret = PTR_ERR(clock_mcde);
+		dev_warn(&pdev->dev, "%s: Failed to get clock '%s'\n",
+					__func__, pdata->clock_mcde_id);
+		goto clk_mcde_err;
+	}
+
+	return ret;
+
+clk_mcde_err:
+	clk_put(clock_dpi);
+clk_dpi_err:
+	clk_put(clock_dsi_lp);
+clk_dsi_lp_err:
+	clk_put(clock_dsi);
+clk_dsi_err:
+#ifdef CONFIG_REGULATOR
+	if (regulator_vana)
+		regulator_put(regulator_vana);
+regulator_vana_err:
+	if (regulator_esram_epod)
+		regulator_put(regulator_esram_epod);
+regulator_esram_err:
+	if (regulator_mcde_epod)
+		regulator_put(regulator_mcde_epod);
+#endif
+	return ret;
+}
+
+static void remove_clocks_and_power(struct platform_device *pdev)
+{
+	/* REVIEW: Release only if exist */
+	/* REVIEW: Remove make sure MCDE is done */
+	clk_put(clock_dpi);
+	clk_put(clock_dsi_lp);
+	clk_put(clock_dsi);
+	clk_put(clock_mcde);
+#ifdef CONFIG_REGULATOR
+	if (regulator_vana)
+		regulator_put(regulator_vana);
+	if (regulator_mcde_epod)
+		regulator_put(regulator_mcde_epod);
+	if (regulator_esram_epod)
+		regulator_put(regulator_esram_epod);
+#endif
+}
+
+static int __devinit mcde_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	int i;
+	struct resource *res;
+	struct mcde_platform_data *pdata = pdev->dev.platform_data;
+	u8 major_version;
+	u8 minor_version;
+	u8 development_version;
+
+	if (!pdata) {
+		dev_dbg(&pdev->dev, "No platform data\n");
+		return -EINVAL;
+	}
+
+	num_dsilinks = pdata->num_dsilinks;
+	mcde_dev = pdev;
+
+	num_channels = pdata->num_channels;
+	num_overlays = pdata->num_overlays;
+
+	channels = kzalloc(num_channels * sizeof(struct mcde_chnl_state),
+								GFP_KERNEL);
+	if (!channels) {
+		ret = -ENOMEM;
+		goto failed_channels_alloc;
+	}
+
+	overlays = kzalloc(num_overlays * sizeof(struct mcde_ovly_state),
+								GFP_KERNEL);
+	if (!overlays) {
+		ret = -ENOMEM;
+		goto failed_overlays_alloc;
+	}
+
+	dsiio = kzalloc(num_dsilinks * sizeof(*dsiio), GFP_KERNEL);
+	if (!dsiio) {
+		ret = -ENOMEM;
+		goto failed_dsi_alloc;
+	}
+
+	/* Hook up irq */
+	mcde_irq = platform_get_irq(pdev, 0);
+	if (mcde_irq <= 0) {
+		dev_dbg(&pdev->dev, "No irq defined\n");
+		ret = -EINVAL;
+		goto failed_irq_get;
+	}
+
+	/* Map I/O */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_dbg(&pdev->dev, "No MCDE io defined\n");
+		ret = -EINVAL;
+		goto failed_get_mcde_io;
+	}
+	mcdeio = ioremap(res->start, res->end - res->start + 1);
+	if (!mcdeio) {
+		dev_dbg(&pdev->dev, "MCDE iomap failed\n");
+		ret = -EINVAL;
+		goto failed_map_mcde_io;
+	}
+	dev_info(&pdev->dev, "MCDE iomap: 0x%.8X->0x%.8X\n",
+		(u32)res->start, (u32)mcdeio);
+	for (i = 0; i < num_dsilinks; i++) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 1+i);
+		if (!res) {
+			dev_dbg(&pdev->dev, "No DSI%d io defined\n", i);
+			ret = -EINVAL;
+			goto failed_get_dsi_io;
+		}
+		dsiio[i] = ioremap(res->start, res->end - res->start + 1);
+		if (!dsiio[i]) {
+			dev_dbg(&pdev->dev, "MCDE DSI%d iomap failed\n", i);
+			ret = -EINVAL;
+			goto failed_map_dsi_io;
+		}
+		dev_info(&pdev->dev, "MCDE DSI%d iomap: 0x%.8X->0x%.8X\n",
+			i, (u32)res->start, (u32)dsiio[i]);
+	}
+
+	ret = init_clocks_and_power(pdev);
+	if (ret < 0) {
+		dev_warn(&pdev->dev, "%s: init_clocks_and_power failed\n"
+					, __func__);
+		goto failed_init_clocks;
+	}
+
+	INIT_DELAYED_WORK_DEFERRABLE(&hw_timeout_work, work_sleep_function);
+
+	ret = enable_clocks_and_power(mcde_dev);
+	if (ret < 0) {
+		dev_dbg(&mcde_dev->dev,
+			"%s: Enable clocks and power failed\n"
+						, __func__);
+		goto failed_enable_clocks;
+	}
+
+	update_mcde_registers();
+	mcde_is_enabled = true;
+
+	ret = request_irq(mcde_irq, mcde_irq_handler, 0, "mcde",
+							&mcde_dev->dev);
+	if (ret) {
+		dev_dbg(&mcde_dev->dev, "Failed to request irq (irq=%d)\n",
+								mcde_irq);
+		goto failed_request_irq;
+	}
+
+	schedule_delayed_work(&hw_timeout_work,
+					msecs_to_jiffies(MCDE_SLEEP_WATCHDOG));
+
+	major_version = MCDE_REG2VAL(MCDE_PID, MAJOR_VERSION,
+							mcde_rreg(MCDE_PID));
+	minor_version = MCDE_REG2VAL(MCDE_PID, MINOR_VERSION,
+							mcde_rreg(MCDE_PID));
+	development_version = MCDE_REG2VAL(MCDE_PID, DEVELOPMENT_VERSION,
+							mcde_rreg(MCDE_PID));
+
+	dev_info(&mcde_dev->dev, "MCDE HW revision %u.%u.%u.%u\n",
+			major_version, minor_version, development_version,
+					mcde_rfld(MCDE_PID, METALFIX_VERSION));
+
+	if (major_version == 3 && minor_version == 0 &&
+					development_version >= 8) {
+		hardware_version = MCDE_CHIP_VERSION_3_0_8;
+		dev_info(&mcde_dev->dev, "V2 HW\n");
+	} else if (major_version == 3 && minor_version == 0 &&
+					development_version >= 5) {
+		hardware_version = MCDE_CHIP_VERSION_3_0_5;
+		dev_info(&mcde_dev->dev, "V1 HW\n");
+	} else if (major_version == 1 && minor_version == 0 &&
+					development_version >= 4) {
+		hardware_version = MCDE_CHIP_VERSION_1_0_4;
+		mcde_dynamic_power_management = false;
+		dev_info(&mcde_dev->dev, "V1_U5500 HW\n");
+	} else {
+		dev_err(&mcde_dev->dev, "Unsupported HW version\n");
+		ret = -ENOTSUPP;
+		goto failed_hardware_version;
+	}
+
+	for (i = 0; i < num_overlays; i++)
+		overlays[i].idx = i;
+
+	if (hardware_version == MCDE_CHIP_VERSION_1_0_4) {
+		channels[0].ovly0 = &overlays[0];
+		channels[0].ovly1 = &overlays[1];
+		channels[1].ovly0 = &overlays[2];
+		channels[1].ovly1 = NULL;
+	} else {
+		channels[0].ovly0 = &overlays[0];
+		channels[0].ovly1 = &overlays[1];
+		channels[1].ovly0 = &overlays[2];
+		channels[1].ovly1 = &overlays[3];
+		channels[2].ovly0 = &overlays[4];
+		channels[2].ovly1 = NULL;
+		channels[3].ovly0 = &overlays[5];
+		channels[3].ovly1 = NULL;
+	}
+
+	for (i = 0; i < num_channels; i++) {
+		channels[i].id = i;
+
+		channels[i].ovly0->chnl = &channels[i];
+		if (channels[i].ovly1)
+			channels[i].ovly1->chnl = &channels[i];
+
+		init_waitqueue_head(&channels[i].waitq_hw);
+		init_timer(&channels[i].auto_sync_timer);
+		channels[i].auto_sync_timer.function =
+					watchdog_auto_sync_timer_function;
+		init_timer(&channels[i].dsi_te_timer);
+		channels[i].dsi_te_timer.function =
+					dsi_te_timer_function;
+		channels[i].dsi_te_timer.data = i;
+	}
+
+	return 0;
+
+failed_hardware_version:
+	free_irq(mcde_irq, &pdev->dev);
+failed_request_irq:
+	disable_mcde_hw(true);
+failed_enable_clocks:
+	remove_clocks_and_power(pdev);
+failed_init_clocks:
+failed_map_dsi_io:
+failed_get_dsi_io:
+	for (i = 0; i < num_dsilinks; i++) {
+		if (dsiio[i])
+			iounmap(dsiio[i]);
+	}
+	iounmap(mcdeio);
+failed_map_mcde_io:
+failed_get_mcde_io:
+failed_irq_get:
+	kfree(dsiio);
+	dsiio = NULL;
+failed_dsi_alloc:
+	kfree(overlays);
+	overlays = NULL;
+failed_overlays_alloc:
+	kfree(channels);
+	channels = NULL;
+failed_channels_alloc:
+	return ret;
+}
+
+static int __devexit mcde_remove(struct platform_device *pdev)
+{
+	struct mcde_chnl_state *chnl = &channels[0];
+
+	for (; chnl < &channels[num_channels]; chnl++) {
+		if (del_timer(&chnl->auto_sync_timer))
+			dev_vdbg(&mcde_dev->dev,
+				"%s timer could not be stopped\n"
+				, __func__);
+		if (del_timer(&chnl->dsi_te_timer))
+			dev_vdbg(&mcde_dev->dev,
+				"%s dsi timer could not be stopped\n"
+				, __func__);
+	}
+
+	remove_clocks_and_power(pdev);
+	return 0;
+}
+
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+static int mcde_resume(struct platform_device *pdev)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	mutex_lock(&mcde_hw_lock);
+
+	if (enable_mcde_hw()) {
+		mutex_unlock(&mcde_hw_lock);
+		return -EINVAL;
+	}
+
+	mutex_unlock(&mcde_hw_lock);
+	return 0;
+}
+
+static int mcde_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int ret;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	mutex_lock(&mcde_hw_lock);
+
+	cancel_delayed_work(&hw_timeout_work);
+
+	if (!mcde_is_enabled) {
+		mutex_unlock(&mcde_hw_lock);
+		return 0;
+	}
+	(void)disable_mcde_hw(true);
+
+	mutex_unlock(&mcde_hw_lock);
+
+	return ret;
+}
+#endif
+
+static struct platform_driver mcde_driver = {
+	.probe = mcde_probe,
+	.remove = mcde_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+	.suspend = mcde_suspend,
+	.resume = mcde_resume,
+#else
+	.suspend = NULL,
+	.resume = NULL,
+#endif
+	.driver = {
+		.name	= "mcde",
+	},
+};
+
+int __init mcde_init(void)
+{
+	mutex_init(&mcde_hw_lock);
+
+	return platform_driver_register(&mcde_driver);
+}
+
+void mcde_exit(void)
+{
+	/* REVIEW: shutdown MCDE? */
+	platform_driver_unregister(&mcde_driver);
+}
diff --git a/drivers/video/mcde/mcde_mod.c b/drivers/video/mcde/mcde_mod.c
new file mode 100644
index 00000000000..60df0d4965f
--- /dev/null
+++ b/drivers/video/mcde/mcde_mod.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson MCDE driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+
+#include <video/mcde.h>
+#include <video/mcde_fb.h>
+#include <video/mcde_dss.h>
+#include <video/mcde_display.h>
+
+/* Module init */
+
+static int __init mcde_subsystem_init(void)
+{
+	int ret;
+	pr_info("MCDE subsystem init begin\n");
+
+	/* MCDE module init sequence */
+	ret = mcde_init();
+	if (ret)
+		goto mcde_failed;
+	ret = mcde_display_init();
+	if (ret)
+		goto mcde_display_failed;
+	ret = mcde_dss_init();
+	if (ret)
+		goto mcde_dss_failed;
+	ret = mcde_fb_init();
+	if (ret)
+		goto mcde_fb_failed;
+	pr_info("MCDE subsystem init done\n");
+
+	goto done;
+mcde_fb_failed:
+	mcde_dss_exit();
+mcde_dss_failed:
+	mcde_display_exit();
+mcde_display_failed:
+	mcde_exit();
+mcde_failed:
+done:
+	return ret;
+}
+#ifdef MODULE
+module_init(mcde_subsystem_init);
+#else
+fs_initcall(mcde_subsystem_init);
+#endif
+
+static void __exit mcde_module_exit(void)
+{
+	mcde_exit();
+	mcde_display_exit();
+	mcde_dss_exit();
+}
+module_exit(mcde_module_exit);
+
+MODULE_AUTHOR("Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE driver");
+
diff --git a/drivers/video/mcde/mcde_regs.h b/drivers/video/mcde/mcde_regs.h
new file mode 100644
index 00000000000..7f28c26360b
--- /dev/null
+++ b/drivers/video/mcde/mcde_regs.h
@@ -0,0 +1,5315 @@
+
+#define MCDE_VAL2REG(__reg, __fld, __val) \
+	(((__val) << __reg##_##__fld##_SHIFT) & __reg##_##__fld##_MASK)
+#define MCDE_REG2VAL(__reg, __fld, __val) \
+	(((__val) & __reg##_##__fld##_MASK) >> __reg##_##__fld##_SHIFT)
+
+#define MCDE_CR 0x00000000
+#define MCDE_CR_DSICMD2_EN_V1_SHIFT 0
+#define MCDE_CR_DSICMD2_EN_V1_MASK 0x00000001
+#define MCDE_CR_DSICMD2_EN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, DSICMD2_EN_V1, __x)
+#define MCDE_CR_DSICMD1_EN_V1_SHIFT 1
+#define MCDE_CR_DSICMD1_EN_V1_MASK 0x00000002
+#define MCDE_CR_DSICMD1_EN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, DSICMD1_EN_V1, __x)
+#define MCDE_CR_DSI0_EN_V3_SHIFT 0
+#define MCDE_CR_DSI0_EN_V3_MASK 0x00000001
+#define MCDE_CR_DSI0_EN_V3(__x) \
+	MCDE_VAL2REG(MCDE_CR, DSI0_EN_V3, __x)
+#define MCDE_CR_DSI1_EN_V3_SHIFT 1
+#define MCDE_CR_DSI1_EN_V3_MASK 0x00000002
+#define MCDE_CR_DSI1_EN_V3(__x) \
+	MCDE_VAL2REG(MCDE_CR, DSI1_EN_V3, __x)
+#define MCDE_CR_DSICMD0_EN_V1_SHIFT 2
+#define MCDE_CR_DSICMD0_EN_V1_MASK 0x00000004
+#define MCDE_CR_DSICMD0_EN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, DSICMD0_EN_V1, __x)
+#define MCDE_CR_DSIVID2_EN_V1_SHIFT 3
+#define MCDE_CR_DSIVID2_EN_V1_MASK 0x00000008
+#define MCDE_CR_DSIVID2_EN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, DSIVID2_EN_V1, __x)
+#define MCDE_CR_DSIVID1_EN_V1_SHIFT 4
+#define MCDE_CR_DSIVID1_EN_V1_MASK 0x00000010
+#define MCDE_CR_DSIVID1_EN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, DSIVID1_EN_V1, __x)
+#define MCDE_CR_DSIVID0_EN_V1_SHIFT 5
+#define MCDE_CR_DSIVID0_EN_V1_MASK 0x00000020
+#define MCDE_CR_DSIVID0_EN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, DSIVID0_EN_V1, __x)
+#define MCDE_CR_DBIC1_EN_V1_SHIFT 6
+#define MCDE_CR_DBIC1_EN_V1_MASK 0x00000040
+#define MCDE_CR_DBIC1_EN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, DBIC1_EN_V1, __x)
+#define MCDE_CR_DBIC0_EN_V1_SHIFT 7
+#define MCDE_CR_DBIC0_EN_V1_MASK 0x00000080
+#define MCDE_CR_DBIC0_EN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, DBIC0_EN_V1, __x)
+#define MCDE_CR_DBI_EN_V3_SHIFT 7
+#define MCDE_CR_DBI_EN_V3_MASK 0x00000080
+#define MCDE_CR_DBI_EN_V3(__x) \
+	MCDE_VAL2REG(MCDE_CR, DBI_EN_V3, __x)
+#define MCDE_CR_DPIB_EN_V1_SHIFT 8
+#define MCDE_CR_DPIB_EN_V1_MASK 0x00000100
+#define MCDE_CR_DPIB_EN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, DPIB_EN_V1, __x)
+#define MCDE_CR_DPIA_EN_V1_SHIFT 9
+#define MCDE_CR_DPIA_EN_V1_MASK 0x00000200
+#define MCDE_CR_DPIA_EN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, DPIA_EN_V1, __x)
+#define MCDE_CR_DPI_EN_V3_SHIFT 9
+#define MCDE_CR_DPI_EN_V3_MASK 0x00000200
+#define MCDE_CR_DPI_EN_V3(__x) \
+	MCDE_VAL2REG(MCDE_CR, DPI_EN_V3, __x)
+#define MCDE_CR_IFIFOCTRLEN_SHIFT 15
+#define MCDE_CR_IFIFOCTRLEN_MASK 0x00008000
+#define MCDE_CR_IFIFOCTRLEN(__x) \
+	MCDE_VAL2REG(MCDE_CR, IFIFOCTRLEN, __x)
+#define MCDE_CR_F01MUX_V1_SHIFT 16
+#define MCDE_CR_F01MUX_V1_MASK 0x00010000
+#define MCDE_CR_F01MUX_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, F01MUX_V1, __x)
+#define MCDE_CR_FABMUX_V1_SHIFT 17
+#define MCDE_CR_FABMUX_V1_MASK 0x00020000
+#define MCDE_CR_FABMUX_V1(__x) \
+	MCDE_VAL2REG(MCDE_CR, FABMUX_V1, __x)
+#define MCDE_CR_AUTOCLKG_EN_SHIFT 30
+#define MCDE_CR_AUTOCLKG_EN_MASK 0x40000000
+#define MCDE_CR_AUTOCLKG_EN(__x) \
+	MCDE_VAL2REG(MCDE_CR, AUTOCLKG_EN, __x)
+#define MCDE_CR_MCDEEN_SHIFT 31
+#define MCDE_CR_MCDEEN_MASK 0x80000000
+#define MCDE_CR_MCDEEN(__x) \
+	MCDE_VAL2REG(MCDE_CR, MCDEEN, __x)
+#define MCDE_CONF0 0x00000004
+#define MCDE_CONF0_SYNCMUX0_SHIFT 0
+#define MCDE_CONF0_SYNCMUX0_MASK 0x00000001
+#define MCDE_CONF0_SYNCMUX0(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX0, __x)
+#define MCDE_CONF0_SYNCMUX1_SHIFT 1
+#define MCDE_CONF0_SYNCMUX1_MASK 0x00000002
+#define MCDE_CONF0_SYNCMUX1(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX1, __x)
+#define MCDE_CONF0_SYNCMUX2_SHIFT 2
+#define MCDE_CONF0_SYNCMUX2_MASK 0x00000004
+#define MCDE_CONF0_SYNCMUX2(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX2, __x)
+#define MCDE_CONF0_SYNCMUX3_SHIFT 3
+#define MCDE_CONF0_SYNCMUX3_MASK 0x00000008
+#define MCDE_CONF0_SYNCMUX3(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX3, __x)
+#define MCDE_CONF0_SYNCMUX4_SHIFT 4
+#define MCDE_CONF0_SYNCMUX4_MASK 0x00000010
+#define MCDE_CONF0_SYNCMUX4(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX4, __x)
+#define MCDE_CONF0_SYNCMUX5_SHIFT 5
+#define MCDE_CONF0_SYNCMUX5_MASK 0x00000020
+#define MCDE_CONF0_SYNCMUX5(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX5, __x)
+#define MCDE_CONF0_SYNCMUX6_SHIFT 6
+#define MCDE_CONF0_SYNCMUX6_MASK 0x00000040
+#define MCDE_CONF0_SYNCMUX6(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX6, __x)
+#define MCDE_CONF0_SYNCMUX7_SHIFT 7
+#define MCDE_CONF0_SYNCMUX7_MASK 0x00000080
+#define MCDE_CONF0_SYNCMUX7(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX7, __x)
+#define MCDE_CONF0_SWAP_A_C0_V1_SHIFT 8
+#define MCDE_CONF0_SWAP_A_C0_V1_MASK 0x00000100
+#define MCDE_CONF0_SWAP_A_C0_V1(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SWAP_A_C0_V1, __x)
+#define MCDE_CONF0_SWAP_B_C1_V1_SHIFT 9
+#define MCDE_CONF0_SWAP_B_C1_V1_MASK 0x00000200
+#define MCDE_CONF0_SWAP_B_C1_V1(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SWAP_B_C1_V1, __x)
+#define MCDE_CONF0_FSYNCTRLA_V1_SHIFT 10
+#define MCDE_CONF0_FSYNCTRLA_V1_MASK 0x00000400
+#define MCDE_CONF0_FSYNCTRLA_V1(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, FSYNCTRLA_V1, __x)
+#define MCDE_CONF0_FSYNCTRLB_V1_SHIFT 11
+#define MCDE_CONF0_FSYNCTRLB_V1_MASK 0x00000800
+#define MCDE_CONF0_FSYNCTRLB_V1(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, FSYNCTRLB_V1, __x)
+#define MCDE_CONF0_IFIFOCTRLWTRMRKLVL_SHIFT 12
+#define MCDE_CONF0_IFIFOCTRLWTRMRKLVL_MASK 0x00007000
+#define MCDE_CONF0_IFIFOCTRLWTRMRKLVL(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, IFIFOCTRLWTRMRKLVL, __x)
+#define MCDE_CONF0_OUTMUX0_SHIFT 16
+#define MCDE_CONF0_OUTMUX0_MASK 0x00070000
+#define MCDE_CONF0_OUTMUX0(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, OUTMUX0, __x)
+#define MCDE_CONF0_OUTMUX1_SHIFT 19
+#define MCDE_CONF0_OUTMUX1_MASK 0x00380000
+#define MCDE_CONF0_OUTMUX1(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, OUTMUX1, __x)
+#define MCDE_CONF0_OUTMUX2_SHIFT 22
+#define MCDE_CONF0_OUTMUX2_MASK 0x01C00000
+#define MCDE_CONF0_OUTMUX2(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, OUTMUX2, __x)
+#define MCDE_CONF0_OUTMUX3_SHIFT 25
+#define MCDE_CONF0_OUTMUX3_MASK 0x0E000000
+#define MCDE_CONF0_OUTMUX3(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, OUTMUX3, __x)
+#define MCDE_CONF0_OUTMUX4_SHIFT 28
+#define MCDE_CONF0_OUTMUX4_MASK 0x70000000
+#define MCDE_CONF0_OUTMUX4(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, OUTMUX4, __x)
+#define MCDE_SSP 0x00000008
+#define MCDE_SSP_SSPDATA_SHIFT 0
+#define MCDE_SSP_SSPDATA_MASK 0x000000FF
+#define MCDE_SSP_SSPDATA(__x) \
+	MCDE_VAL2REG(MCDE_SSP, SSPDATA, __x)
+#define MCDE_SSP_SSPCMD_SHIFT 8
+#define MCDE_SSP_SSPCMD_MASK 0x00000100
+#define MCDE_SSP_SSPCMD_DATA 0
+#define MCDE_SSP_SSPCMD_COMMAND 1
+#define MCDE_SSP_SSPCMD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_SSP, SSPCMD, MCDE_SSP_SSPCMD_##__x)
+#define MCDE_SSP_SSPCMD(__x) \
+	MCDE_VAL2REG(MCDE_SSP, SSPCMD, __x)
+#define MCDE_SSP_SSPEN_SHIFT 16
+#define MCDE_SSP_SSPEN_MASK 0x00010000
+#define MCDE_SSP_SSPEN(__x) \
+	MCDE_VAL2REG(MCDE_SSP, SSPEN, __x)
+#define MCDE_AIS 0x00000100
+#define MCDE_AIS_MCDEPPI_SHIFT 0
+#define MCDE_AIS_MCDEPPI_MASK 0x00000001
+#define MCDE_AIS_MCDEPPI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, MCDEPPI, __x)
+#define MCDE_AIS_MCDEOVLI_SHIFT 1
+#define MCDE_AIS_MCDEOVLI_MASK 0x00000002
+#define MCDE_AIS_MCDEOVLI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, MCDEOVLI, __x)
+#define MCDE_AIS_MCDECHNLI_SHIFT 2
+#define MCDE_AIS_MCDECHNLI_MASK 0x00000004
+#define MCDE_AIS_MCDECHNLI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, MCDECHNLI, __x)
+#define MCDE_AIS_MCDEERRI_SHIFT 3
+#define MCDE_AIS_MCDEERRI_MASK 0x00000008
+#define MCDE_AIS_MCDEERRI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, MCDEERRI, __x)
+#define MCDE_AIS_DSI0AI_SHIFT 4
+#define MCDE_AIS_DSI0AI_MASK 0x00000010
+#define MCDE_AIS_DSI0AI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, DSI0AI, __x)
+#define MCDE_AIS_DSI1AI_SHIFT 5
+#define MCDE_AIS_DSI1AI_MASK 0x00000020
+#define MCDE_AIS_DSI1AI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, DSI1AI, __x)
+#define MCDE_AIS_DSI2AI_SHIFT 6
+#define MCDE_AIS_DSI2AI_MASK 0x00000040
+#define MCDE_AIS_DSI2AI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, DSI2AI, __x)
+#define MCDE_IMSCPP 0x00000104
+#define MCDE_IMSCPP_VCMPAIM_SHIFT 0
+#define MCDE_IMSCPP_VCMPAIM_MASK 0x00000001
+#define MCDE_IMSCPP_VCMPAIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VCMPAIM, __x)
+#define MCDE_IMSCPP_VCMPBIM_SHIFT 1
+#define MCDE_IMSCPP_VCMPBIM_MASK 0x00000002
+#define MCDE_IMSCPP_VCMPBIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VCMPBIM, __x)
+#define MCDE_IMSCPP_VSCC0IM_SHIFT 2
+#define MCDE_IMSCPP_VSCC0IM_MASK 0x00000004
+#define MCDE_IMSCPP_VSCC0IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VSCC0IM, __x)
+#define MCDE_IMSCPP_VSCC1IM_SHIFT 3
+#define MCDE_IMSCPP_VSCC1IM_MASK 0x00000008
+#define MCDE_IMSCPP_VSCC1IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VSCC1IM, __x)
+#define MCDE_IMSCPP_VCMPC0IM_SHIFT 4
+#define MCDE_IMSCPP_VCMPC0IM_MASK 0x00000010
+#define MCDE_IMSCPP_VCMPC0IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VCMPC0IM, __x)
+#define MCDE_IMSCPP_VCMPC1IM_SHIFT 5
+#define MCDE_IMSCPP_VCMPC1IM_MASK 0x00000020
+#define MCDE_IMSCPP_VCMPC1IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VCMPC1IM, __x)
+#define MCDE_IMSCPP_ROTFDIM_B_SHIFT 6
+#define MCDE_IMSCPP_ROTFDIM_B_MASK 0x00000040
+#define MCDE_IMSCPP_ROTFDIM_B(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, ROTFDIM_B, __x)
+#define MCDE_IMSCPP_ROTFDIM_A_SHIFT 7
+#define MCDE_IMSCPP_ROTFDIM_A_MASK 0x00000080
+#define MCDE_IMSCPP_ROTFDIM_A(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, ROTFDIM_A, __x)
+#define MCDE_IMSCOVL 0x00000108
+#define MCDE_IMSCOVL_OVLRDIM_SHIFT 0
+#define MCDE_IMSCOVL_OVLRDIM_MASK 0x0000FFFF
+#define MCDE_IMSCOVL_OVLRDIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCOVL, OVLRDIM, __x)
+#define MCDE_IMSCOVL_OVLFDIM_SHIFT 16
+#define MCDE_IMSCOVL_OVLFDIM_MASK 0xFFFF0000
+#define MCDE_IMSCOVL_OVLFDIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCOVL, OVLFDIM, __x)
+#define MCDE_IMSCCHNL 0x0000010C
+#define MCDE_IMSCCHNL_CHNLRDIM_SHIFT 0
+#define MCDE_IMSCCHNL_CHNLRDIM_MASK 0x0000FFFF
+#define MCDE_IMSCCHNL_CHNLRDIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCCHNL, CHNLRDIM, __x)
+#define MCDE_IMSCCHNL_CHNLAIM_SHIFT 16
+#define MCDE_IMSCCHNL_CHNLAIM_MASK 0xFFFF0000
+#define MCDE_IMSCCHNL_CHNLAIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCCHNL, CHNLAIM, __x)
+#define MCDE_IMSCERR 0x00000110
+#define MCDE_IMSCERR_FUAIM_SHIFT 0
+#define MCDE_IMSCERR_FUAIM_MASK 0x00000001
+#define MCDE_IMSCERR_FUAIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, FUAIM, __x)
+#define MCDE_IMSCERR_FUBIM_SHIFT 1
+#define MCDE_IMSCERR_FUBIM_MASK 0x00000002
+#define MCDE_IMSCERR_FUBIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, FUBIM, __x)
+#define MCDE_IMSCERR_SCHBLCKDIM_SHIFT 2
+#define MCDE_IMSCERR_SCHBLCKDIM_MASK 0x00000004
+#define MCDE_IMSCERR_SCHBLCKDIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, SCHBLCKDIM, __x)
+#define MCDE_IMSCERR_ROTAFEIM_WRITE_SHIFT 3
+#define MCDE_IMSCERR_ROTAFEIM_WRITE_MASK 0x00000008
+#define MCDE_IMSCERR_ROTAFEIM_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, ROTAFEIM_WRITE, __x)
+#define MCDE_IMSCERR_ROTAFEIM_READ_SHIFT 4
+#define MCDE_IMSCERR_ROTAFEIM_READ_MASK 0x00000010
+#define MCDE_IMSCERR_ROTAFEIM_READ(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, ROTAFEIM_READ, __x)
+#define MCDE_IMSCERR_ROTBFEIM_WRITE_SHIFT 5
+#define MCDE_IMSCERR_ROTBFEIM_WRITE_MASK 0x00000020
+#define MCDE_IMSCERR_ROTBFEIM_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, ROTBFEIM_WRITE, __x)
+#define MCDE_IMSCERR_ROTBFEIM_READ_SHIFT 6
+#define MCDE_IMSCERR_ROTBFEIM_READ_MASK 0x00000040
+#define MCDE_IMSCERR_ROTBFEIM_READ(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, ROTBFEIM_READ, __x)
+#define MCDE_IMSCERR_FUC0IM_SHIFT 7
+#define MCDE_IMSCERR_FUC0IM_MASK 0x00000080
+#define MCDE_IMSCERR_FUC0IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, FUC0IM, __x)
+#define MCDE_IMSCERR_FUC1IM_SHIFT 8
+#define MCDE_IMSCERR_FUC1IM_MASK 0x00000100
+#define MCDE_IMSCERR_FUC1IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, FUC1IM, __x)
+#define MCDE_IMSCERR_OVLFERRIM_SHIFT 16
+#define MCDE_IMSCERR_OVLFERRIM_MASK 0xFFFF0000
+#define MCDE_IMSCERR_OVLFERRIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, OVLFERRIM, __x)
+#define MCDE_RISPP 0x00000114
+#define MCDE_RISPP_VCMPARIS_SHIFT 0
+#define MCDE_RISPP_VCMPARIS_MASK 0x00000001
+#define MCDE_RISPP_VCMPARIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VCMPARIS, __x)
+#define MCDE_RISPP_VCMPBRIS_SHIFT 1
+#define MCDE_RISPP_VCMPBRIS_MASK 0x00000002
+#define MCDE_RISPP_VCMPBRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VCMPBRIS, __x)
+#define MCDE_RISPP_VSCC0RIS_SHIFT 2
+#define MCDE_RISPP_VSCC0RIS_MASK 0x00000004
+#define MCDE_RISPP_VSCC0RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VSCC0RIS, __x)
+#define MCDE_RISPP_VSCC1RIS_SHIFT 3
+#define MCDE_RISPP_VSCC1RIS_MASK 0x00000008
+#define MCDE_RISPP_VSCC1RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VSCC1RIS, __x)
+#define MCDE_RISPP_VCMPC0RIS_SHIFT 4
+#define MCDE_RISPP_VCMPC0RIS_MASK 0x00000010
+#define MCDE_RISPP_VCMPC0RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VCMPC0RIS, __x)
+#define MCDE_RISPP_VCMPC1RIS_SHIFT 5
+#define MCDE_RISPP_VCMPC1RIS_MASK 0x00000020
+#define MCDE_RISPP_VCMPC1RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VCMPC1RIS, __x)
+#define MCDE_RISPP_ROTFDRIS_B_SHIFT 6
+#define MCDE_RISPP_ROTFDRIS_B_MASK 0x00000040
+#define MCDE_RISPP_ROTFDRIS_B(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, ROTFDRIS_B, __x)
+#define MCDE_RISPP_ROTFDRIS_A_SHIFT 7
+#define MCDE_RISPP_ROTFDRIS_A_MASK 0x00000080
+#define MCDE_RISPP_ROTFDRIS_A(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, ROTFDRIS_A, __x)
+#define MCDE_RISOVL 0x00000118
+#define MCDE_RISOVL_OVLRDRIS_SHIFT 0
+#define MCDE_RISOVL_OVLRDRIS_MASK 0x0000FFFF
+#define MCDE_RISOVL_OVLRDRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISOVL, OVLRDRIS, __x)
+#define MCDE_RISOVL_OVLFDRIS_SHIFT 16
+#define MCDE_RISOVL_OVLFDRIS_MASK 0xFFFF0000
+#define MCDE_RISOVL_OVLFDRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISOVL, OVLFDRIS, __x)
+#define MCDE_RISCHNL 0x0000011C
+#define MCDE_RISCHNL_CHNLRDRIS_SHIFT 0
+#define MCDE_RISCHNL_CHNLRDRIS_MASK 0x0000FFFF
+#define MCDE_RISCHNL_CHNLRDRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISCHNL, CHNLRDRIS, __x)
+#define MCDE_RISCHNL_CHNLARIS_SHIFT 16
+#define MCDE_RISCHNL_CHNLARIS_MASK 0xFFFF0000
+#define MCDE_RISCHNL_CHNLARIS(__x) \
+	MCDE_VAL2REG(MCDE_RISCHNL, CHNLARIS, __x)
+#define MCDE_RISERR 0x00000120
+#define MCDE_RISERR_FUARIS_SHIFT 0
+#define MCDE_RISERR_FUARIS_MASK 0x00000001
+#define MCDE_RISERR_FUARIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, FUARIS, __x)
+#define MCDE_RISERR_FUBRIS_SHIFT 1
+#define MCDE_RISERR_FUBRIS_MASK 0x00000002
+#define MCDE_RISERR_FUBRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, FUBRIS, __x)
+#define MCDE_RISERR_SCHBLCKDRIS_SHIFT 2
+#define MCDE_RISERR_SCHBLCKDRIS_MASK 0x00000004
+#define MCDE_RISERR_SCHBLCKDRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, SCHBLCKDRIS, __x)
+#define MCDE_RISERR_ROTAFERIS_WRITE_SHIFT 3
+#define MCDE_RISERR_ROTAFERIS_WRITE_MASK 0x00000008
+#define MCDE_RISERR_ROTAFERIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, ROTAFERIS_WRITE, __x)
+#define MCDE_RISERR_ROTAFERIS_READ_SHIFT 4
+#define MCDE_RISERR_ROTAFERIS_READ_MASK 0x00000010
+#define MCDE_RISERR_ROTAFERIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, ROTAFERIS_READ, __x)
+#define MCDE_RISERR_ROTBFERIS_WRITE_SHIFT 5
+#define MCDE_RISERR_ROTBFERIS_WRITE_MASK 0x00000020
+#define MCDE_RISERR_ROTBFERIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, ROTBFERIS_WRITE, __x)
+#define MCDE_RISERR_ROTBFERIS_READ_SHIFT 6
+#define MCDE_RISERR_ROTBFERIS_READ_MASK 0x00000040
+#define MCDE_RISERR_ROTBFERIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, ROTBFERIS_READ, __x)
+#define MCDE_RISERR_FUC0RIS_SHIFT 7
+#define MCDE_RISERR_FUC0RIS_MASK 0x00000080
+#define MCDE_RISERR_FUC0RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, FUC0RIS, __x)
+#define MCDE_RISERR_FUC1RIS_SHIFT 8
+#define MCDE_RISERR_FUC1RIS_MASK 0x00000100
+#define MCDE_RISERR_FUC1RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, FUC1RIS, __x)
+#define MCDE_RISERR_OVLFERRRIS_SHIFT 16
+#define MCDE_RISERR_OVLFERRRIS_MASK 0xFFFF0000
+#define MCDE_RISERR_OVLFERRRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, OVLFERRRIS, __x)
+#define MCDE_MISPP 0x00000124
+#define MCDE_MISPP_VCMPAMIS_SHIFT 0
+#define MCDE_MISPP_VCMPAMIS_MASK 0x00000001
+#define MCDE_MISPP_VCMPAMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VCMPAMIS, __x)
+#define MCDE_MISPP_VCMPBMIS_SHIFT 1
+#define MCDE_MISPP_VCMPBMIS_MASK 0x00000002
+#define MCDE_MISPP_VCMPBMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VCMPBMIS, __x)
+#define MCDE_MISPP_VSCC0MIS_SHIFT 2
+#define MCDE_MISPP_VSCC0MIS_MASK 0x00000004
+#define MCDE_MISPP_VSCC0MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VSCC0MIS, __x)
+#define MCDE_MISPP_VSCC1MIS_SHIFT 3
+#define MCDE_MISPP_VSCC1MIS_MASK 0x00000008
+#define MCDE_MISPP_VSCC1MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VSCC1MIS, __x)
+#define MCDE_MISPP_VCMPC0MIS_SHIFT 4
+#define MCDE_MISPP_VCMPC0MIS_MASK 0x00000010
+#define MCDE_MISPP_VCMPC0MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VCMPC0MIS, __x)
+#define MCDE_MISPP_VCMPC1MIS_SHIFT 5
+#define MCDE_MISPP_VCMPC1MIS_MASK 0x00000020
+#define MCDE_MISPP_VCMPC1MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VCMPC1MIS, __x)
+#define MCDE_MISPP_ROTFDMIS_A_SHIFT 6
+#define MCDE_MISPP_ROTFDMIS_A_MASK 0x00000040
+#define MCDE_MISPP_ROTFDMIS_A(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, ROTFDMIS_A, __x)
+#define MCDE_MISPP_ROTFDMIS_B_SHIFT 7
+#define MCDE_MISPP_ROTFDMIS_B_MASK 0x00000080
+#define MCDE_MISPP_ROTFDMIS_B(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, ROTFDMIS_B, __x)
+#define MCDE_MISOVL 0x00000128
+#define MCDE_MISOVL_OVLRDMIS_SHIFT 0
+#define MCDE_MISOVL_OVLRDMIS_MASK 0x0000FFFF
+#define MCDE_MISOVL_OVLRDMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISOVL, OVLRDMIS, __x)
+#define MCDE_MISOVL_OVLFDMIS_SHIFT 16
+#define MCDE_MISOVL_OVLFDMIS_MASK 0xFFFF0000
+#define MCDE_MISOVL_OVLFDMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISOVL, OVLFDMIS, __x)
+#define MCDE_MISCHNL 0x0000012C
+#define MCDE_MISCHNL_CHNLRDMIS_SHIFT 0
+#define MCDE_MISCHNL_CHNLRDMIS_MASK 0x0000FFFF
+#define MCDE_MISCHNL_CHNLRDMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISCHNL, CHNLRDMIS, __x)
+#define MCDE_MISCHNL_CHNLAMIS_SHIFT 16
+#define MCDE_MISCHNL_CHNLAMIS_MASK 0xFFFF0000
+#define MCDE_MISCHNL_CHNLAMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISCHNL, CHNLAMIS, __x)
+#define MCDE_MISERR 0x00000130
+#define MCDE_MISERR_FUAMIS_SHIFT 0
+#define MCDE_MISERR_FUAMIS_MASK 0x00000001
+#define MCDE_MISERR_FUAMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, FUAMIS, __x)
+#define MCDE_MISERR_FUBMIS_SHIFT 1
+#define MCDE_MISERR_FUBMIS_MASK 0x00000002
+#define MCDE_MISERR_FUBMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, FUBMIS, __x)
+#define MCDE_MISERR_SCHBLCKDMIS_SHIFT 2
+#define MCDE_MISERR_SCHBLCKDMIS_MASK 0x00000004
+#define MCDE_MISERR_SCHBLCKDMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, SCHBLCKDMIS, __x)
+#define MCDE_MISERR_ROTAFEMIS_WRITE_SHIFT 3
+#define MCDE_MISERR_ROTAFEMIS_WRITE_MASK 0x00000008
+#define MCDE_MISERR_ROTAFEMIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, ROTAFEMIS_WRITE, __x)
+#define MCDE_MISERR_ROTAFEMIS_READ_SHIFT 4
+#define MCDE_MISERR_ROTAFEMIS_READ_MASK 0x00000010
+#define MCDE_MISERR_ROTAFEMIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, ROTAFEMIS_READ, __x)
+#define MCDE_MISERR_ROTBFEMIS_WRITE_SHIFT 5
+#define MCDE_MISERR_ROTBFEMIS_WRITE_MASK 0x00000020
+#define MCDE_MISERR_ROTBFEMIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, ROTBFEMIS_WRITE, __x)
+#define MCDE_MISERR_ROTBFEMIS_READ_SHIFT 6
+#define MCDE_MISERR_ROTBFEMIS_READ_MASK 0x00000040
+#define MCDE_MISERR_ROTBFEMIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, ROTBFEMIS_READ, __x)
+#define MCDE_MISERR_FUC0MIS_SHIFT 7
+#define MCDE_MISERR_FUC0MIS_MASK 0x00000080
+#define MCDE_MISERR_FUC0MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, FUC0MIS, __x)
+#define MCDE_MISERR_FUC1MIS_SHIFT 8
+#define MCDE_MISERR_FUC1MIS_MASK 0x00000100
+#define MCDE_MISERR_FUC1MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, FUC1MIS, __x)
+#define MCDE_MISERR_OVLFERMIS_SHIFT 16
+#define MCDE_MISERR_OVLFERMIS_MASK 0xFFFF0000
+#define MCDE_MISERR_OVLFERMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, OVLFERMIS, __x)
+#define MCDE_SISPP 0x00000134
+#define MCDE_SISPP_VCMPASIS_SHIFT 0
+#define MCDE_SISPP_VCMPASIS_MASK 0x00000001
+#define MCDE_SISPP_VCMPASIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VCMPASIS, __x)
+#define MCDE_SISPP_VCMPBSIS_SHIFT 1
+#define MCDE_SISPP_VCMPBSIS_MASK 0x00000002
+#define MCDE_SISPP_VCMPBSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VCMPBSIS, __x)
+#define MCDE_SISPP_VSCC0SIS_SHIFT 2
+#define MCDE_SISPP_VSCC0SIS_MASK 0x00000004
+#define MCDE_SISPP_VSCC0SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VSCC0SIS, __x)
+#define MCDE_SISPP_VSCC1SIS_SHIFT 3
+#define MCDE_SISPP_VSCC1SIS_MASK 0x00000008
+#define MCDE_SISPP_VSCC1SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VSCC1SIS, __x)
+#define MCDE_SISPP_VCMPC0SIS_SHIFT 4
+#define MCDE_SISPP_VCMPC0SIS_MASK 0x00000010
+#define MCDE_SISPP_VCMPC0SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VCMPC0SIS, __x)
+#define MCDE_SISPP_VCMPC1SIS_SHIFT 5
+#define MCDE_SISPP_VCMPC1SIS_MASK 0x00000020
+#define MCDE_SISPP_VCMPC1SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VCMPC1SIS, __x)
+#define MCDE_SISPP_ROTFDSIS_A_SHIFT 6
+#define MCDE_SISPP_ROTFDSIS_A_MASK 0x00000040
+#define MCDE_SISPP_ROTFDSIS_A(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, ROTFDSIS_A, __x)
+#define MCDE_SISPP_ROTFDSIS_B_SHIFT 7
+#define MCDE_SISPP_ROTFDSIS_B_MASK 0x00000080
+#define MCDE_SISPP_ROTFDSIS_B(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, ROTFDSIS_B, __x)
+#define MCDE_SISOVL 0x00000138
+#define MCDE_SISOVL_OVLRDSIS_SHIFT 0
+#define MCDE_SISOVL_OVLRDSIS_MASK 0x0000FFFF
+#define MCDE_SISOVL_OVLRDSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISOVL, OVLRDSIS, __x)
+#define MCDE_SISOVL_OVLFDSIS_SHIFT 16
+#define MCDE_SISOVL_OVLFDSIS_MASK 0xFFFF0000
+#define MCDE_SISOVL_OVLFDSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISOVL, OVLFDSIS, __x)
+#define MCDE_SISCHNL 0x0000013C
+#define MCDE_SISCHNL_CHNLRDSIS_SHIFT 0
+#define MCDE_SISCHNL_CHNLRDSIS_MASK 0x0000FFFF
+#define MCDE_SISCHNL_CHNLRDSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISCHNL, CHNLRDSIS, __x)
+#define MCDE_SISCHNL_CHNLASIS_SHIFT 16
+#define MCDE_SISCHNL_CHNLASIS_MASK 0xFFFF0000
+#define MCDE_SISCHNL_CHNLASIS(__x) \
+	MCDE_VAL2REG(MCDE_SISCHNL, CHNLASIS, __x)
+#define MCDE_SISERR 0x00000140
+#define MCDE_SISERR_FUASIS_SHIFT 0
+#define MCDE_SISERR_FUASIS_MASK 0x00000001
+#define MCDE_SISERR_FUASIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, FUASIS, __x)
+#define MCDE_SISERR_FUBSIS_SHIFT 1
+#define MCDE_SISERR_FUBSIS_MASK 0x00000002
+#define MCDE_SISERR_FUBSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, FUBSIS, __x)
+#define MCDE_SISERR_SCHBLCKDSIS_SHIFT 2
+#define MCDE_SISERR_SCHBLCKDSIS_MASK 0x00000004
+#define MCDE_SISERR_SCHBLCKDSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, SCHBLCKDSIS, __x)
+#define MCDE_SISERR_ROTAFESIS_WRITE_SHIFT 3
+#define MCDE_SISERR_ROTAFESIS_WRITE_MASK 0x00000008
+#define MCDE_SISERR_ROTAFESIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, ROTAFESIS_WRITE, __x)
+#define MCDE_SISERR_ROTAFESIS_READ_SHIFT 4
+#define MCDE_SISERR_ROTAFESIS_READ_MASK 0x00000010
+#define MCDE_SISERR_ROTAFESIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, ROTAFESIS_READ, __x)
+#define MCDE_SISERR_ROTBFESIS_WRITE_SHIFT 5
+#define MCDE_SISERR_ROTBFESIS_WRITE_MASK 0x00000020
+#define MCDE_SISERR_ROTBFESIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, ROTBFESIS_WRITE, __x)
+#define MCDE_SISERR_ROTBFESIS_READ_SHIFT 6
+#define MCDE_SISERR_ROTBFESIS_READ_MASK 0x00000040
+#define MCDE_SISERR_ROTBFESIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, ROTBFESIS_READ, __x)
+#define MCDE_SISERR_FUC0SIS_SHIFT 7
+#define MCDE_SISERR_FUC0SIS_MASK 0x00000080
+#define MCDE_SISERR_FUC0SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, FUC0SIS, __x)
+#define MCDE_SISERR_FUC1SIS_SHIFT 8
+#define MCDE_SISERR_FUC1SIS_MASK 0x00000100
+#define MCDE_SISERR_FUC1SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, FUC1SIS, __x)
+#define MCDE_SISERR_OVLFERSIS_SHIFT 16
+#define MCDE_SISERR_OVLFERSIS_MASK 0xFFFF0000
+#define MCDE_SISERR_OVLFERSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, OVLFERSIS, __x)
+#define MCDE_PID 0x000001FC
+#define MCDE_PID_METALFIX_VERSION_SHIFT 0
+#define MCDE_PID_METALFIX_VERSION_MASK 0x000000FF
+#define MCDE_PID_METALFIX_VERSION(__x) \
+	MCDE_VAL2REG(MCDE_PID, METALFIX_VERSION, __x)
+#define MCDE_PID_DEVELOPMENT_VERSION_SHIFT 8
+#define MCDE_PID_DEVELOPMENT_VERSION_MASK 0x0000FF00
+#define MCDE_PID_DEVELOPMENT_VERSION(__x) \
+	MCDE_VAL2REG(MCDE_PID, DEVELOPMENT_VERSION, __x)
+#define MCDE_PID_MINOR_VERSION_SHIFT 16
+#define MCDE_PID_MINOR_VERSION_MASK 0x00FF0000
+#define MCDE_PID_MINOR_VERSION(__x) \
+	MCDE_VAL2REG(MCDE_PID, MINOR_VERSION, __x)
+#define MCDE_PID_MAJOR_VERSION_SHIFT 24
+#define MCDE_PID_MAJOR_VERSION_MASK 0xFF000000
+#define MCDE_PID_MAJOR_VERSION(__x) \
+	MCDE_VAL2REG(MCDE_PID, MAJOR_VERSION, __x)
+#define MCDE_EXTSRC0A0 0x00000200
+#define MCDE_EXTSRC0A0_GROUPOFFSET 0x20
+#define MCDE_EXTSRC0A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC0A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC0A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC1A0 0x00000220
+#define MCDE_EXTSRC1A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC1A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC1A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC2A0 0x00000240
+#define MCDE_EXTSRC2A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC2A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC2A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC3A0 0x00000260
+#define MCDE_EXTSRC3A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC3A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC3A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC4A0 0x00000280
+#define MCDE_EXTSRC4A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC4A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC4A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC5A0 0x000002A0
+#define MCDE_EXTSRC5A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC5A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC5A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC6A0 0x000002C0
+#define MCDE_EXTSRC6A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC6A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC6A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC7A0 0x000002E0
+#define MCDE_EXTSRC7A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC7A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC7A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC8A0 0x00000300
+#define MCDE_EXTSRC8A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC8A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC8A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC9A0 0x00000320
+#define MCDE_EXTSRC9A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC9A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC9A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC0A1 0x00000204
+#define MCDE_EXTSRC0A1_GROUPOFFSET 0x20
+#define MCDE_EXTSRC0A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC0A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC0A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC1A1 0x00000224
+#define MCDE_EXTSRC1A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC1A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC1A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC2A1 0x00000244
+#define MCDE_EXTSRC2A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC2A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC2A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC3A1 0x00000264
+#define MCDE_EXTSRC3A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC3A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC3A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC4A1 0x00000284
+#define MCDE_EXTSRC4A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC4A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC4A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC5A1 0x000002A4
+#define MCDE_EXTSRC5A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC5A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC5A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC6A1 0x000002C4
+#define MCDE_EXTSRC6A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC6A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC6A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC7A1 0x000002E4
+#define MCDE_EXTSRC7A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC7A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC7A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC8A1 0x00000304
+#define MCDE_EXTSRC8A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC8A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC8A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC9A1 0x00000324
+#define MCDE_EXTSRC9A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC9A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC9A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC6A2 0x000002C8
+#define MCDE_EXTSRC6A2_BASEADDRESS2_SHIFT 3
+#define MCDE_EXTSRC6A2_BASEADDRESS2_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC6A2_BASEADDRESS2(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6A2, BASEADDRESS2, __x)
+#define MCDE_EXTSRC0CONF 0x0000020C
+#define MCDE_EXTSRC0CONF_GROUPOFFSET 0x20
+#define MCDE_EXTSRC0CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC0CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC0CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BUF_ID, __x)
+#define MCDE_EXTSRC0CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC0CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC0CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BUF_NB, __x)
+#define MCDE_EXTSRC0CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC0CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC0CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC0CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC0CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC0CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC0CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC0CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC0CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC0CONF_BPP_RGB444 4
+#define MCDE_EXTSRC0CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC0CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC0CONF_BPP_RGB565 7
+#define MCDE_EXTSRC0CONF_BPP_RGB888 8
+#define MCDE_EXTSRC0CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC0CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC0CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC0CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BPP, MCDE_EXTSRC0CONF_BPP_##__x)
+#define MCDE_EXTSRC0CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BPP, __x)
+#define MCDE_EXTSRC0CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC0CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC0CONF_BGR_RGB 0
+#define MCDE_EXTSRC0CONF_BGR_BGR 1
+#define MCDE_EXTSRC0CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BGR, MCDE_EXTSRC0CONF_BGR_##__x)
+#define MCDE_EXTSRC0CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BGR, __x)
+#define MCDE_EXTSRC0CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC0CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC0CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC0CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC0CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BEBO, MCDE_EXTSRC0CONF_BEBO_##__x)
+#define MCDE_EXTSRC0CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BEBO, __x)
+#define MCDE_EXTSRC0CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC0CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC0CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC0CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC0CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BEPO, MCDE_EXTSRC0CONF_BEPO_##__x)
+#define MCDE_EXTSRC0CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BEPO, __x)
+#define MCDE_EXTSRC1CONF 0x0000022C
+#define MCDE_EXTSRC1CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC1CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC1CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BUF_ID, __x)
+#define MCDE_EXTSRC1CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC1CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC1CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BUF_NB, __x)
+#define MCDE_EXTSRC1CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC1CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC1CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC1CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC1CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC1CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC1CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC1CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC1CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC1CONF_BPP_RGB444 4
+#define MCDE_EXTSRC1CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC1CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC1CONF_BPP_RGB565 7
+#define MCDE_EXTSRC1CONF_BPP_RGB888 8
+#define MCDE_EXTSRC1CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC1CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC1CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC1CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BPP, MCDE_EXTSRC1CONF_BPP_##__x)
+#define MCDE_EXTSRC1CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BPP, __x)
+#define MCDE_EXTSRC1CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC1CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC1CONF_BGR_RGB 0
+#define MCDE_EXTSRC1CONF_BGR_BGR 1
+#define MCDE_EXTSRC1CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BGR, MCDE_EXTSRC1CONF_BGR_##__x)
+#define MCDE_EXTSRC1CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BGR, __x)
+#define MCDE_EXTSRC1CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC1CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC1CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC1CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC1CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BEBO, MCDE_EXTSRC1CONF_BEBO_##__x)
+#define MCDE_EXTSRC1CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BEBO, __x)
+#define MCDE_EXTSRC1CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC1CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC1CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC1CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC1CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BEPO, MCDE_EXTSRC1CONF_BEPO_##__x)
+#define MCDE_EXTSRC1CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BEPO, __x)
+#define MCDE_EXTSRC2CONF 0x0000024C
+#define MCDE_EXTSRC2CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC2CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC2CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BUF_ID, __x)
+#define MCDE_EXTSRC2CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC2CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC2CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BUF_NB, __x)
+#define MCDE_EXTSRC2CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC2CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC2CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC2CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC2CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC2CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC2CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC2CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC2CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC2CONF_BPP_RGB444 4
+#define MCDE_EXTSRC2CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC2CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC2CONF_BPP_RGB565 7
+#define MCDE_EXTSRC2CONF_BPP_RGB888 8
+#define MCDE_EXTSRC2CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC2CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC2CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC2CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BPP, MCDE_EXTSRC2CONF_BPP_##__x)
+#define MCDE_EXTSRC2CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BPP, __x)
+#define MCDE_EXTSRC2CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC2CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC2CONF_BGR_RGB 0
+#define MCDE_EXTSRC2CONF_BGR_BGR 1
+#define MCDE_EXTSRC2CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BGR, MCDE_EXTSRC2CONF_BGR_##__x)
+#define MCDE_EXTSRC2CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BGR, __x)
+#define MCDE_EXTSRC2CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC2CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC2CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC2CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC2CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BEBO, MCDE_EXTSRC2CONF_BEBO_##__x)
+#define MCDE_EXTSRC2CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BEBO, __x)
+#define MCDE_EXTSRC2CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC2CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC2CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC2CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC2CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BEPO, MCDE_EXTSRC2CONF_BEPO_##__x)
+#define MCDE_EXTSRC2CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BEPO, __x)
+#define MCDE_EXTSRC3CONF 0x0000026C
+#define MCDE_EXTSRC3CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC3CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC3CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BUF_ID, __x)
+#define MCDE_EXTSRC3CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC3CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC3CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BUF_NB, __x)
+#define MCDE_EXTSRC3CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC3CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC3CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC3CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC3CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC3CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC3CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC3CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC3CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC3CONF_BPP_RGB444 4
+#define MCDE_EXTSRC3CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC3CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC3CONF_BPP_RGB565 7
+#define MCDE_EXTSRC3CONF_BPP_RGB888 8
+#define MCDE_EXTSRC3CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC3CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC3CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC3CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BPP, MCDE_EXTSRC3CONF_BPP_##__x)
+#define MCDE_EXTSRC3CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BPP, __x)
+#define MCDE_EXTSRC3CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC3CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC3CONF_BGR_RGB 0
+#define MCDE_EXTSRC3CONF_BGR_BGR 1
+#define MCDE_EXTSRC3CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BGR, MCDE_EXTSRC3CONF_BGR_##__x)
+#define MCDE_EXTSRC3CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BGR, __x)
+#define MCDE_EXTSRC3CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC3CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC3CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC3CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC3CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BEBO, MCDE_EXTSRC3CONF_BEBO_##__x)
+#define MCDE_EXTSRC3CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BEBO, __x)
+#define MCDE_EXTSRC3CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC3CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC3CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC3CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC3CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BEPO, MCDE_EXTSRC3CONF_BEPO_##__x)
+#define MCDE_EXTSRC3CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BEPO, __x)
+#define MCDE_EXTSRC4CONF 0x0000028C
+#define MCDE_EXTSRC4CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC4CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC4CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BUF_ID, __x)
+#define MCDE_EXTSRC4CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC4CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC4CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BUF_NB, __x)
+#define MCDE_EXTSRC4CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC4CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC4CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC4CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC4CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC4CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC4CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC4CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC4CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC4CONF_BPP_RGB444 4
+#define MCDE_EXTSRC4CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC4CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC4CONF_BPP_RGB565 7
+#define MCDE_EXTSRC4CONF_BPP_RGB888 8
+#define MCDE_EXTSRC4CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC4CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC4CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC4CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BPP, MCDE_EXTSRC4CONF_BPP_##__x)
+#define MCDE_EXTSRC4CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BPP, __x)
+#define MCDE_EXTSRC4CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC4CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC4CONF_BGR_RGB 0
+#define MCDE_EXTSRC4CONF_BGR_BGR 1
+#define MCDE_EXTSRC4CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BGR, MCDE_EXTSRC4CONF_BGR_##__x)
+#define MCDE_EXTSRC4CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BGR, __x)
+#define MCDE_EXTSRC4CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC4CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC4CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC4CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC4CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BEBO, MCDE_EXTSRC4CONF_BEBO_##__x)
+#define MCDE_EXTSRC4CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BEBO, __x)
+#define MCDE_EXTSRC4CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC4CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC4CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC4CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC4CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BEPO, MCDE_EXTSRC4CONF_BEPO_##__x)
+#define MCDE_EXTSRC4CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BEPO, __x)
+#define MCDE_EXTSRC5CONF 0x000002AC
+#define MCDE_EXTSRC5CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC5CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC5CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BUF_ID, __x)
+#define MCDE_EXTSRC5CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC5CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC5CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BUF_NB, __x)
+#define MCDE_EXTSRC5CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC5CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC5CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC5CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC5CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC5CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC5CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC5CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC5CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC5CONF_BPP_RGB444 4
+#define MCDE_EXTSRC5CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC5CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC5CONF_BPP_RGB565 7
+#define MCDE_EXTSRC5CONF_BPP_RGB888 8
+#define MCDE_EXTSRC5CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC5CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC5CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC5CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BPP, MCDE_EXTSRC5CONF_BPP_##__x)
+#define MCDE_EXTSRC5CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BPP, __x)
+#define MCDE_EXTSRC5CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC5CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC5CONF_BGR_RGB 0
+#define MCDE_EXTSRC5CONF_BGR_BGR 1
+#define MCDE_EXTSRC5CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BGR, MCDE_EXTSRC5CONF_BGR_##__x)
+#define MCDE_EXTSRC5CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BGR, __x)
+#define MCDE_EXTSRC5CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC5CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC5CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC5CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC5CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BEBO, MCDE_EXTSRC5CONF_BEBO_##__x)
+#define MCDE_EXTSRC5CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BEBO, __x)
+#define MCDE_EXTSRC5CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC5CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC5CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC5CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC5CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BEPO, MCDE_EXTSRC5CONF_BEPO_##__x)
+#define MCDE_EXTSRC5CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BEPO, __x)
+#define MCDE_EXTSRC6CONF 0x000002CC
+#define MCDE_EXTSRC6CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC6CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC6CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BUF_ID, __x)
+#define MCDE_EXTSRC6CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC6CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC6CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BUF_NB, __x)
+#define MCDE_EXTSRC6CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC6CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC6CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC6CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC6CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC6CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC6CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC6CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC6CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC6CONF_BPP_RGB444 4
+#define MCDE_EXTSRC6CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC6CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC6CONF_BPP_RGB565 7
+#define MCDE_EXTSRC6CONF_BPP_RGB888 8
+#define MCDE_EXTSRC6CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC6CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC6CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC6CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BPP, MCDE_EXTSRC6CONF_BPP_##__x)
+#define MCDE_EXTSRC6CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BPP, __x)
+#define MCDE_EXTSRC6CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC6CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC6CONF_BGR_RGB 0
+#define MCDE_EXTSRC6CONF_BGR_BGR 1
+#define MCDE_EXTSRC6CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BGR, MCDE_EXTSRC6CONF_BGR_##__x)
+#define MCDE_EXTSRC6CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BGR, __x)
+#define MCDE_EXTSRC6CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC6CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC6CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC6CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC6CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BEBO, MCDE_EXTSRC6CONF_BEBO_##__x)
+#define MCDE_EXTSRC6CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BEBO, __x)
+#define MCDE_EXTSRC6CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC6CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC6CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC6CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC6CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BEPO, MCDE_EXTSRC6CONF_BEPO_##__x)
+#define MCDE_EXTSRC6CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BEPO, __x)
+#define MCDE_EXTSRC7CONF 0x000002EC
+#define MCDE_EXTSRC7CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC7CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC7CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BUF_ID, __x)
+#define MCDE_EXTSRC7CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC7CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC7CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BUF_NB, __x)
+#define MCDE_EXTSRC7CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC7CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC7CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC7CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC7CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC7CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC7CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC7CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC7CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC7CONF_BPP_RGB444 4
+#define MCDE_EXTSRC7CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC7CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC7CONF_BPP_RGB565 7
+#define MCDE_EXTSRC7CONF_BPP_RGB888 8
+#define MCDE_EXTSRC7CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC7CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC7CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC7CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BPP, MCDE_EXTSRC7CONF_BPP_##__x)
+#define MCDE_EXTSRC7CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BPP, __x)
+#define MCDE_EXTSRC7CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC7CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC7CONF_BGR_RGB 0
+#define MCDE_EXTSRC7CONF_BGR_BGR 1
+#define MCDE_EXTSRC7CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BGR, MCDE_EXTSRC7CONF_BGR_##__x)
+#define MCDE_EXTSRC7CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BGR, __x)
+#define MCDE_EXTSRC7CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC7CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC7CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC7CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC7CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BEBO, MCDE_EXTSRC7CONF_BEBO_##__x)
+#define MCDE_EXTSRC7CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BEBO, __x)
+#define MCDE_EXTSRC7CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC7CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC7CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC7CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC7CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BEPO, MCDE_EXTSRC7CONF_BEPO_##__x)
+#define MCDE_EXTSRC7CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BEPO, __x)
+#define MCDE_EXTSRC8CONF 0x0000030C
+#define MCDE_EXTSRC8CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC8CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC8CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BUF_ID, __x)
+#define MCDE_EXTSRC8CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC8CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC8CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BUF_NB, __x)
+#define MCDE_EXTSRC8CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC8CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC8CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC8CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC8CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC8CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC8CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC8CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC8CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC8CONF_BPP_RGB444 4
+#define MCDE_EXTSRC8CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC8CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC8CONF_BPP_RGB565 7
+#define MCDE_EXTSRC8CONF_BPP_RGB888 8
+#define MCDE_EXTSRC8CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC8CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC8CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC8CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BPP, MCDE_EXTSRC8CONF_BPP_##__x)
+#define MCDE_EXTSRC8CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BPP, __x)
+#define MCDE_EXTSRC8CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC8CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC8CONF_BGR_RGB 0
+#define MCDE_EXTSRC8CONF_BGR_BGR 1
+#define MCDE_EXTSRC8CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BGR, MCDE_EXTSRC8CONF_BGR_##__x)
+#define MCDE_EXTSRC8CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BGR, __x)
+#define MCDE_EXTSRC8CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC8CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC8CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC8CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC8CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BEBO, MCDE_EXTSRC8CONF_BEBO_##__x)
+#define MCDE_EXTSRC8CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BEBO, __x)
+#define MCDE_EXTSRC8CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC8CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC8CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC8CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC8CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BEPO, MCDE_EXTSRC8CONF_BEPO_##__x)
+#define MCDE_EXTSRC8CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BEPO, __x)
+#define MCDE_EXTSRC9CONF 0x0000032C
+#define MCDE_EXTSRC9CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC9CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC9CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BUF_ID, __x)
+#define MCDE_EXTSRC9CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC9CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC9CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BUF_NB, __x)
+#define MCDE_EXTSRC9CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC9CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC9CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC9CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC9CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC9CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC9CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC9CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC9CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC9CONF_BPP_RGB444 4
+#define MCDE_EXTSRC9CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC9CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC9CONF_BPP_RGB565 7
+#define MCDE_EXTSRC9CONF_BPP_RGB888 8
+#define MCDE_EXTSRC9CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC9CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC9CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC9CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BPP, MCDE_EXTSRC9CONF_BPP_##__x)
+#define MCDE_EXTSRC9CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BPP, __x)
+#define MCDE_EXTSRC9CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC9CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC9CONF_BGR_RGB 0
+#define MCDE_EXTSRC9CONF_BGR_BGR 1
+#define MCDE_EXTSRC9CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BGR, MCDE_EXTSRC9CONF_BGR_##__x)
+#define MCDE_EXTSRC9CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BGR, __x)
+#define MCDE_EXTSRC9CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC9CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC9CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC9CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC9CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BEBO, MCDE_EXTSRC9CONF_BEBO_##__x)
+#define MCDE_EXTSRC9CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BEBO, __x)
+#define MCDE_EXTSRC9CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC9CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC9CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC9CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC9CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BEPO, MCDE_EXTSRC9CONF_BEPO_##__x)
+#define MCDE_EXTSRC9CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BEPO, __x)
+#define MCDE_EXTSRC0CR 0x00000210
+#define MCDE_EXTSRC0CR_GROUPOFFSET 0x20
+#define MCDE_EXTSRC0CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC0CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC0CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC0CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC0CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC0CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, SEL_MOD, MCDE_EXTSRC0CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC0CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, SEL_MOD, __x)
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC0CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC0CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC0CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC0CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC0CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC0CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC0CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC1CR 0x00000230
+#define MCDE_EXTSRC1CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC1CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC1CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC1CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC1CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC1CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, SEL_MOD, MCDE_EXTSRC1CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC1CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, SEL_MOD, __x)
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC1CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC1CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC1CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC1CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC1CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC1CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC1CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC2CR 0x00000250
+#define MCDE_EXTSRC2CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC2CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC2CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC2CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC2CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC2CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, SEL_MOD, MCDE_EXTSRC2CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC2CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, SEL_MOD, __x)
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC2CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC2CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC2CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC2CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC2CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC2CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC2CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC3CR 0x00000270
+#define MCDE_EXTSRC3CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC3CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC3CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC3CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC3CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC3CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, SEL_MOD, MCDE_EXTSRC3CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC3CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, SEL_MOD, __x)
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC3CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC3CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC3CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC3CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC3CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC3CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC3CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC4CR 0x00000290
+#define MCDE_EXTSRC4CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC4CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC4CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC4CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC4CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC4CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, SEL_MOD, MCDE_EXTSRC4CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC4CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, SEL_MOD, __x)
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC4CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC4CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC4CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC4CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC4CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC4CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC4CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC5CR 0x000002B0
+#define MCDE_EXTSRC5CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC5CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC5CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC5CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC5CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC5CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, SEL_MOD, MCDE_EXTSRC5CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC5CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, SEL_MOD, __x)
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC5CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC5CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC5CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC5CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC5CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC5CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC5CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC6CR 0x000002D0
+#define MCDE_EXTSRC6CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC6CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC6CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC6CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC6CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC6CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, SEL_MOD, MCDE_EXTSRC6CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC6CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, SEL_MOD, __x)
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC6CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC6CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC6CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC6CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC6CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC6CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC6CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC7CR 0x000002F0
+#define MCDE_EXTSRC7CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC7CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC7CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC7CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC7CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC7CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, SEL_MOD, MCDE_EXTSRC7CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC7CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, SEL_MOD, __x)
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC7CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC7CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC7CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC7CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC7CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC7CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC7CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC8CR 0x00000310
+#define MCDE_EXTSRC8CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC8CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC8CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC8CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC8CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC8CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, SEL_MOD, MCDE_EXTSRC8CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC8CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, SEL_MOD, __x)
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC8CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC8CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC8CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC8CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC8CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC8CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC8CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC9CR 0x00000330
+#define MCDE_EXTSRC9CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC9CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC9CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC9CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC9CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC9CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, SEL_MOD, MCDE_EXTSRC9CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC9CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, SEL_MOD, __x)
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC9CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC9CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC9CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC9CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC9CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC9CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC9CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, FORCE_FS_DIV, __x)
+#define MCDE_OVL0CR 0x00000400
+#define MCDE_OVL0CR_GROUPOFFSET 0x20
+#define MCDE_OVL0CR_OVLEN_SHIFT 0
+#define MCDE_OVL0CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL0CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, OVLEN, __x)
+#define MCDE_OVL0CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL0CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL0CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL0CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL0CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL0CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, COLCCTRL, MCDE_OVL0CR_COLCCTRL_##__x)
+#define MCDE_OVL0CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, COLCCTRL, __x)
+#define MCDE_OVL0CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL0CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL0CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, CKEYGEN, __x)
+#define MCDE_OVL0CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL0CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL0CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, ALPHAPMEN, __x)
+#define MCDE_OVL0CR_OVLF_SHIFT 5
+#define MCDE_OVL0CR_OVLF_MASK 0x00000020
+#define MCDE_OVL0CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, OVLF, __x)
+#define MCDE_OVL0CR_OVLR_SHIFT 6
+#define MCDE_OVL0CR_OVLR_MASK 0x00000040
+#define MCDE_OVL0CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, OVLR, __x)
+#define MCDE_OVL0CR_OVLB_SHIFT 7
+#define MCDE_OVL0CR_OVLB_MASK 0x00000080
+#define MCDE_OVL0CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, OVLB, __x)
+#define MCDE_OVL0CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL0CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL0CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, FETCH_ROPC, __x)
+#define MCDE_OVL0CR_STBPRIO_SHIFT 16
+#define MCDE_OVL0CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL0CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, STBPRIO, __x)
+#define MCDE_OVL0CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL0CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL0CR_BURSTSIZE_1W 0
+#define MCDE_OVL0CR_BURSTSIZE_2W 1
+#define MCDE_OVL0CR_BURSTSIZE_4W 2
+#define MCDE_OVL0CR_BURSTSIZE_8W 3
+#define MCDE_OVL0CR_BURSTSIZE_16W 4
+#define MCDE_OVL0CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL0CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL0CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL0CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL0CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL0CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, BURSTSIZE, MCDE_OVL0CR_BURSTSIZE_##__x)
+#define MCDE_OVL0CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, BURSTSIZE, __x)
+#define MCDE_OVL0CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL0CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL0CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL0CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL0CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL0CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL0CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL0CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, MAXOUTSTANDING, \
+	MCDE_OVL0CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL0CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL0CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL0CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL0CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL0CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL0CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL0CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL0CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL0CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL0CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL0CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL0CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL0CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL0CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, ROTBURSTSIZE, MCDE_OVL0CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL0CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL1CR 0x00000420
+#define MCDE_OVL1CR_OVLEN_SHIFT 0
+#define MCDE_OVL1CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL1CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, OVLEN, __x)
+#define MCDE_OVL1CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL1CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL1CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL1CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL1CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL1CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, COLCCTRL, MCDE_OVL1CR_COLCCTRL_##__x)
+#define MCDE_OVL1CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, COLCCTRL, __x)
+#define MCDE_OVL1CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL1CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL1CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, CKEYGEN, __x)
+#define MCDE_OVL1CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL1CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL1CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, ALPHAPMEN, __x)
+#define MCDE_OVL1CR_OVLF_SHIFT 5
+#define MCDE_OVL1CR_OVLF_MASK 0x00000020
+#define MCDE_OVL1CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, OVLF, __x)
+#define MCDE_OVL1CR_OVLR_SHIFT 6
+#define MCDE_OVL1CR_OVLR_MASK 0x00000040
+#define MCDE_OVL1CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, OVLR, __x)
+#define MCDE_OVL1CR_OVLB_SHIFT 7
+#define MCDE_OVL1CR_OVLB_MASK 0x00000080
+#define MCDE_OVL1CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, OVLB, __x)
+#define MCDE_OVL1CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL1CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL1CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, FETCH_ROPC, __x)
+#define MCDE_OVL1CR_STBPRIO_SHIFT 16
+#define MCDE_OVL1CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL1CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, STBPRIO, __x)
+#define MCDE_OVL1CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL1CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL1CR_BURSTSIZE_1W 0
+#define MCDE_OVL1CR_BURSTSIZE_2W 1
+#define MCDE_OVL1CR_BURSTSIZE_4W 2
+#define MCDE_OVL1CR_BURSTSIZE_8W 3
+#define MCDE_OVL1CR_BURSTSIZE_16W 4
+#define MCDE_OVL1CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL1CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL1CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL1CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL1CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL1CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, BURSTSIZE, MCDE_OVL1CR_BURSTSIZE_##__x)
+#define MCDE_OVL1CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, BURSTSIZE, __x)
+#define MCDE_OVL1CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL1CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL1CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL1CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL1CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL1CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL1CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL1CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, MAXOUTSTANDING, \
+	MCDE_OVL1CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL1CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL1CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL1CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL1CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL1CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL1CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL1CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL1CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL1CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL1CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL1CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL1CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL1CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL1CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, ROTBURSTSIZE, MCDE_OVL1CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL1CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL2CR 0x00000440
+#define MCDE_OVL2CR_OVLEN_SHIFT 0
+#define MCDE_OVL2CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL2CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, OVLEN, __x)
+#define MCDE_OVL2CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL2CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL2CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL2CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL2CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL2CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, COLCCTRL, MCDE_OVL2CR_COLCCTRL_##__x)
+#define MCDE_OVL2CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, COLCCTRL, __x)
+#define MCDE_OVL2CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL2CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL2CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, CKEYGEN, __x)
+#define MCDE_OVL2CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL2CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL2CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, ALPHAPMEN, __x)
+#define MCDE_OVL2CR_OVLF_SHIFT 5
+#define MCDE_OVL2CR_OVLF_MASK 0x00000020
+#define MCDE_OVL2CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, OVLF, __x)
+#define MCDE_OVL2CR_OVLR_SHIFT 6
+#define MCDE_OVL2CR_OVLR_MASK 0x00000040
+#define MCDE_OVL2CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, OVLR, __x)
+#define MCDE_OVL2CR_OVLB_SHIFT 7
+#define MCDE_OVL2CR_OVLB_MASK 0x00000080
+#define MCDE_OVL2CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, OVLB, __x)
+#define MCDE_OVL2CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL2CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL2CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, FETCH_ROPC, __x)
+#define MCDE_OVL2CR_STBPRIO_SHIFT 16
+#define MCDE_OVL2CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL2CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, STBPRIO, __x)
+#define MCDE_OVL2CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL2CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL2CR_BURSTSIZE_1W 0
+#define MCDE_OVL2CR_BURSTSIZE_2W 1
+#define MCDE_OVL2CR_BURSTSIZE_4W 2
+#define MCDE_OVL2CR_BURSTSIZE_8W 3
+#define MCDE_OVL2CR_BURSTSIZE_16W 4
+#define MCDE_OVL2CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL2CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL2CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL2CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL2CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL2CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, BURSTSIZE, MCDE_OVL2CR_BURSTSIZE_##__x)
+#define MCDE_OVL2CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, BURSTSIZE, __x)
+#define MCDE_OVL2CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL2CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL2CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL2CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL2CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL2CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL2CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL2CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, MAXOUTSTANDING, \
+	MCDE_OVL2CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL2CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL2CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL2CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL2CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL2CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL2CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL2CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL2CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL2CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL2CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL2CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL2CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL2CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL2CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, ROTBURSTSIZE, MCDE_OVL2CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL2CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL3CR 0x00000460
+#define MCDE_OVL3CR_OVLEN_SHIFT 0
+#define MCDE_OVL3CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL3CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, OVLEN, __x)
+#define MCDE_OVL3CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL3CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL3CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL3CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL3CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL3CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, COLCCTRL, MCDE_OVL3CR_COLCCTRL_##__x)
+#define MCDE_OVL3CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, COLCCTRL, __x)
+#define MCDE_OVL3CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL3CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL3CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, CKEYGEN, __x)
+#define MCDE_OVL3CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL3CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL3CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, ALPHAPMEN, __x)
+#define MCDE_OVL3CR_OVLF_SHIFT 5
+#define MCDE_OVL3CR_OVLF_MASK 0x00000020
+#define MCDE_OVL3CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, OVLF, __x)
+#define MCDE_OVL3CR_OVLR_SHIFT 6
+#define MCDE_OVL3CR_OVLR_MASK 0x00000040
+#define MCDE_OVL3CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, OVLR, __x)
+#define MCDE_OVL3CR_OVLB_SHIFT 7
+#define MCDE_OVL3CR_OVLB_MASK 0x00000080
+#define MCDE_OVL3CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, OVLB, __x)
+#define MCDE_OVL3CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL3CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL3CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, FETCH_ROPC, __x)
+#define MCDE_OVL3CR_STBPRIO_SHIFT 16
+#define MCDE_OVL3CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL3CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, STBPRIO, __x)
+#define MCDE_OVL3CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL3CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL3CR_BURSTSIZE_1W 0
+#define MCDE_OVL3CR_BURSTSIZE_2W 1
+#define MCDE_OVL3CR_BURSTSIZE_4W 2
+#define MCDE_OVL3CR_BURSTSIZE_8W 3
+#define MCDE_OVL3CR_BURSTSIZE_16W 4
+#define MCDE_OVL3CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL3CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL3CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL3CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL3CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL3CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, BURSTSIZE, MCDE_OVL3CR_BURSTSIZE_##__x)
+#define MCDE_OVL3CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, BURSTSIZE, __x)
+#define MCDE_OVL3CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL3CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL3CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL3CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL3CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL3CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL3CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL3CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, MAXOUTSTANDING, \
+	MCDE_OVL3CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL3CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL3CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL3CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL3CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL3CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL3CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL3CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL3CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL3CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL3CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL3CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL3CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL3CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL3CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, ROTBURSTSIZE, MCDE_OVL3CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL3CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL4CR 0x00000480
+#define MCDE_OVL4CR_OVLEN_SHIFT 0
+#define MCDE_OVL4CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL4CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, OVLEN, __x)
+#define MCDE_OVL4CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL4CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL4CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL4CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL4CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL4CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, COLCCTRL, MCDE_OVL4CR_COLCCTRL_##__x)
+#define MCDE_OVL4CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, COLCCTRL, __x)
+#define MCDE_OVL4CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL4CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL4CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, CKEYGEN, __x)
+#define MCDE_OVL4CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL4CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL4CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, ALPHAPMEN, __x)
+#define MCDE_OVL4CR_OVLF_SHIFT 5
+#define MCDE_OVL4CR_OVLF_MASK 0x00000020
+#define MCDE_OVL4CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, OVLF, __x)
+#define MCDE_OVL4CR_OVLR_SHIFT 6
+#define MCDE_OVL4CR_OVLR_MASK 0x00000040
+#define MCDE_OVL4CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, OVLR, __x)
+#define MCDE_OVL4CR_OVLB_SHIFT 7
+#define MCDE_OVL4CR_OVLB_MASK 0x00000080
+#define MCDE_OVL4CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, OVLB, __x)
+#define MCDE_OVL4CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL4CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL4CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, FETCH_ROPC, __x)
+#define MCDE_OVL4CR_STBPRIO_SHIFT 16
+#define MCDE_OVL4CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL4CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, STBPRIO, __x)
+#define MCDE_OVL4CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL4CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL4CR_BURSTSIZE_1W 0
+#define MCDE_OVL4CR_BURSTSIZE_2W 1
+#define MCDE_OVL4CR_BURSTSIZE_4W 2
+#define MCDE_OVL4CR_BURSTSIZE_8W 3
+#define MCDE_OVL4CR_BURSTSIZE_16W 4
+#define MCDE_OVL4CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL4CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL4CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL4CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL4CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL4CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, BURSTSIZE, MCDE_OVL4CR_BURSTSIZE_##__x)
+#define MCDE_OVL4CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, BURSTSIZE, __x)
+#define MCDE_OVL4CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL4CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL4CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL4CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL4CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL4CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL4CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL4CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, MAXOUTSTANDING, \
+	MCDE_OVL4CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL4CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL4CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL4CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL4CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL4CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL4CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL4CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL4CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL4CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL4CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL4CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL4CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL4CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL4CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, ROTBURSTSIZE, MCDE_OVL4CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL4CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL5CR 0x000004A0
+#define MCDE_OVL5CR_OVLEN_SHIFT 0
+#define MCDE_OVL5CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL5CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, OVLEN, __x)
+#define MCDE_OVL5CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL5CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL5CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL5CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL5CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL5CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, COLCCTRL, MCDE_OVL5CR_COLCCTRL_##__x)
+#define MCDE_OVL5CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, COLCCTRL, __x)
+#define MCDE_OVL5CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL5CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL5CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, CKEYGEN, __x)
+#define MCDE_OVL5CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL5CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL5CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, ALPHAPMEN, __x)
+#define MCDE_OVL5CR_OVLF_SHIFT 5
+#define MCDE_OVL5CR_OVLF_MASK 0x00000020
+#define MCDE_OVL5CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, OVLF, __x)
+#define MCDE_OVL5CR_OVLR_SHIFT 6
+#define MCDE_OVL5CR_OVLR_MASK 0x00000040
+#define MCDE_OVL5CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, OVLR, __x)
+#define MCDE_OVL5CR_OVLB_SHIFT 7
+#define MCDE_OVL5CR_OVLB_MASK 0x00000080
+#define MCDE_OVL5CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, OVLB, __x)
+#define MCDE_OVL5CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL5CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL5CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, FETCH_ROPC, __x)
+#define MCDE_OVL5CR_STBPRIO_SHIFT 16
+#define MCDE_OVL5CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL5CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, STBPRIO, __x)
+#define MCDE_OVL5CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL5CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL5CR_BURSTSIZE_1W 0
+#define MCDE_OVL5CR_BURSTSIZE_2W 1
+#define MCDE_OVL5CR_BURSTSIZE_4W 2
+#define MCDE_OVL5CR_BURSTSIZE_8W 3
+#define MCDE_OVL5CR_BURSTSIZE_16W 4
+#define MCDE_OVL5CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL5CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL5CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL5CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL5CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL5CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, BURSTSIZE, MCDE_OVL5CR_BURSTSIZE_##__x)
+#define MCDE_OVL5CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, BURSTSIZE, __x)
+#define MCDE_OVL5CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL5CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL5CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL5CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL5CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL5CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL5CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL5CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, MAXOUTSTANDING, \
+	MCDE_OVL5CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL5CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL5CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL5CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL5CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL5CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL5CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL5CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL5CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL5CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL5CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL5CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL5CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL5CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL5CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, ROTBURSTSIZE, MCDE_OVL5CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL5CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL0CONF 0x00000404
+#define MCDE_OVL0CONF_GROUPOFFSET 0x20
+#define MCDE_OVL0CONF_PPL_SHIFT 0
+#define MCDE_OVL0CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL0CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF, PPL, __x)
+#define MCDE_OVL0CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL0CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL0CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF, EXTSRC_ID, __x)
+#define MCDE_OVL0CONF_LPF_SHIFT 16
+#define MCDE_OVL0CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL0CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF, LPF, __x)
+#define MCDE_OVL1CONF 0x00000424
+#define MCDE_OVL1CONF_PPL_SHIFT 0
+#define MCDE_OVL1CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL1CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF, PPL, __x)
+#define MCDE_OVL1CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL1CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL1CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF, EXTSRC_ID, __x)
+#define MCDE_OVL1CONF_LPF_SHIFT 16
+#define MCDE_OVL1CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL1CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF, LPF, __x)
+#define MCDE_OVL2CONF 0x00000444
+#define MCDE_OVL2CONF_PPL_SHIFT 0
+#define MCDE_OVL2CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL2CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF, PPL, __x)
+#define MCDE_OVL2CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL2CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL2CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF, EXTSRC_ID, __x)
+#define MCDE_OVL2CONF_LPF_SHIFT 16
+#define MCDE_OVL2CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL2CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF, LPF, __x)
+#define MCDE_OVL3CONF 0x00000464
+#define MCDE_OVL3CONF_PPL_SHIFT 0
+#define MCDE_OVL3CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL3CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF, PPL, __x)
+#define MCDE_OVL3CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL3CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL3CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF, EXTSRC_ID, __x)
+#define MCDE_OVL3CONF_LPF_SHIFT 16
+#define MCDE_OVL3CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL3CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF, LPF, __x)
+#define MCDE_OVL4CONF 0x00000484
+#define MCDE_OVL4CONF_PPL_SHIFT 0
+#define MCDE_OVL4CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL4CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF, PPL, __x)
+#define MCDE_OVL4CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL4CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL4CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF, EXTSRC_ID, __x)
+#define MCDE_OVL4CONF_LPF_SHIFT 16
+#define MCDE_OVL4CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL4CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF, LPF, __x)
+#define MCDE_OVL5CONF 0x000004A4
+#define MCDE_OVL5CONF_PPL_SHIFT 0
+#define MCDE_OVL5CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL5CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF, PPL, __x)
+#define MCDE_OVL5CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL5CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL5CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF, EXTSRC_ID, __x)
+#define MCDE_OVL5CONF_LPF_SHIFT 16
+#define MCDE_OVL5CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL5CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF, LPF, __x)
+#define MCDE_OVL0CONF2 0x00000408
+#define MCDE_OVL0CONF2_GROUPOFFSET 0x20
+#define MCDE_OVL0CONF2_BP_SHIFT 0
+#define MCDE_OVL0CONF2_BP_MASK 0x00000001
+#define MCDE_OVL0CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL0CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL0CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, BP, MCDE_OVL0CONF2_BP_##__x)
+#define MCDE_OVL0CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, BP, __x)
+#define MCDE_OVL0CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL0CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL0CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL0CONF2_OPQ_SHIFT 9
+#define MCDE_OVL0CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL0CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, OPQ, __x)
+#define MCDE_OVL0CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL0CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL0CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, PIXOFF, __x)
+#define MCDE_OVL0CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL0CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL0CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL1CONF2 0x00000428
+#define MCDE_OVL1CONF2_BP_SHIFT 0
+#define MCDE_OVL1CONF2_BP_MASK 0x00000001
+#define MCDE_OVL1CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL1CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL1CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, BP, MCDE_OVL1CONF2_BP_##__x)
+#define MCDE_OVL1CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, BP, __x)
+#define MCDE_OVL1CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL1CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL1CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL1CONF2_OPQ_SHIFT 9
+#define MCDE_OVL1CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL1CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, OPQ, __x)
+#define MCDE_OVL1CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL1CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL1CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, PIXOFF, __x)
+#define MCDE_OVL1CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL1CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL1CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL2CONF2 0x00000448
+#define MCDE_OVL2CONF2_BP_SHIFT 0
+#define MCDE_OVL2CONF2_BP_MASK 0x00000001
+#define MCDE_OVL2CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL2CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL2CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, BP, MCDE_OVL2CONF2_BP_##__x)
+#define MCDE_OVL2CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, BP, __x)
+#define MCDE_OVL2CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL2CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL2CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL2CONF2_OPQ_SHIFT 9
+#define MCDE_OVL2CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL2CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, OPQ, __x)
+#define MCDE_OVL2CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL2CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL2CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, PIXOFF, __x)
+#define MCDE_OVL2CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL2CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL2CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL3CONF2 0x00000468
+#define MCDE_OVL3CONF2_BP_SHIFT 0
+#define MCDE_OVL3CONF2_BP_MASK 0x00000001
+#define MCDE_OVL3CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL3CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL3CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, BP, MCDE_OVL3CONF2_BP_##__x)
+#define MCDE_OVL3CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, BP, __x)
+#define MCDE_OVL3CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL3CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL3CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL3CONF2_OPQ_SHIFT 9
+#define MCDE_OVL3CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL3CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, OPQ, __x)
+#define MCDE_OVL3CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL3CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL3CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, PIXOFF, __x)
+#define MCDE_OVL3CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL3CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL3CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL4CONF2 0x00000488
+#define MCDE_OVL4CONF2_BP_SHIFT 0
+#define MCDE_OVL4CONF2_BP_MASK 0x00000001
+#define MCDE_OVL4CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL4CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL4CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, BP, MCDE_OVL4CONF2_BP_##__x)
+#define MCDE_OVL4CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, BP, __x)
+#define MCDE_OVL4CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL4CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL4CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL4CONF2_OPQ_SHIFT 9
+#define MCDE_OVL4CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL4CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, OPQ, __x)
+#define MCDE_OVL4CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL4CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL4CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, PIXOFF, __x)
+#define MCDE_OVL4CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL4CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL4CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL5CONF2 0x000004A8
+#define MCDE_OVL5CONF2_BP_SHIFT 0
+#define MCDE_OVL5CONF2_BP_MASK 0x00000001
+#define MCDE_OVL5CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL5CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL5CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, BP, MCDE_OVL5CONF2_BP_##__x)
+#define MCDE_OVL5CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, BP, __x)
+#define MCDE_OVL5CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL5CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL5CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL5CONF2_OPQ_SHIFT 9
+#define MCDE_OVL5CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL5CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, OPQ, __x)
+#define MCDE_OVL5CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL5CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL5CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, PIXOFF, __x)
+#define MCDE_OVL5CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL5CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL5CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL0LJINC 0x0000040C
+#define MCDE_OVL0LJINC_GROUPOFFSET 0x20
+#define MCDE_OVL0LJINC_LJINC_SHIFT 0
+#define MCDE_OVL0LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL0LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL0LJINC, LJINC, __x)
+#define MCDE_OVL1LJINC 0x0000042C
+#define MCDE_OVL1LJINC_LJINC_SHIFT 0
+#define MCDE_OVL1LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL1LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL1LJINC, LJINC, __x)
+#define MCDE_OVL2LJINC 0x0000044C
+#define MCDE_OVL2LJINC_LJINC_SHIFT 0
+#define MCDE_OVL2LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL2LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL2LJINC, LJINC, __x)
+#define MCDE_OVL3LJINC 0x0000046C
+#define MCDE_OVL3LJINC_LJINC_SHIFT 0
+#define MCDE_OVL3LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL3LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL3LJINC, LJINC, __x)
+#define MCDE_OVL4LJINC 0x0000048C
+#define MCDE_OVL4LJINC_LJINC_SHIFT 0
+#define MCDE_OVL4LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL4LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL4LJINC, LJINC, __x)
+#define MCDE_OVL5LJINC 0x000004AC
+#define MCDE_OVL5LJINC_LJINC_SHIFT 0
+#define MCDE_OVL5LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL5LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL5LJINC, LJINC, __x)
+#define MCDE_OVL0CROP 0x00000410
+#define MCDE_OVL0CROP_GROUPOFFSET 0x20
+#define MCDE_OVL0CROP_TMRGN_SHIFT 0
+#define MCDE_OVL0CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL0CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CROP, TMRGN, __x)
+#define MCDE_OVL0CROP_LMRGN_SHIFT 22
+#define MCDE_OVL0CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL0CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CROP, LMRGN, __x)
+#define MCDE_OVL1CROP 0x00000430
+#define MCDE_OVL1CROP_TMRGN_SHIFT 0
+#define MCDE_OVL1CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL1CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CROP, TMRGN, __x)
+#define MCDE_OVL1CROP_LMRGN_SHIFT 22
+#define MCDE_OVL1CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL1CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CROP, LMRGN, __x)
+#define MCDE_OVL2CROP 0x00000450
+#define MCDE_OVL2CROP_TMRGN_SHIFT 0
+#define MCDE_OVL2CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL2CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CROP, TMRGN, __x)
+#define MCDE_OVL2CROP_LMRGN_SHIFT 22
+#define MCDE_OVL2CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL2CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CROP, LMRGN, __x)
+#define MCDE_OVL3CROP 0x00000470
+#define MCDE_OVL3CROP_TMRGN_SHIFT 0
+#define MCDE_OVL3CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL3CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CROP, TMRGN, __x)
+#define MCDE_OVL3CROP_LMRGN_SHIFT 22
+#define MCDE_OVL3CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL3CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CROP, LMRGN, __x)
+#define MCDE_OVL4CROP 0x00000490
+#define MCDE_OVL4CROP_TMRGN_SHIFT 0
+#define MCDE_OVL4CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL4CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CROP, TMRGN, __x)
+#define MCDE_OVL4CROP_LMRGN_SHIFT 22
+#define MCDE_OVL4CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL4CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CROP, LMRGN, __x)
+#define MCDE_OVL5CROP 0x000004B0
+#define MCDE_OVL5CROP_TMRGN_SHIFT 0
+#define MCDE_OVL5CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL5CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CROP, TMRGN, __x)
+#define MCDE_OVL5CROP_LMRGN_SHIFT 22
+#define MCDE_OVL5CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL5CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CROP, LMRGN, __x)
+#define MCDE_OVL0COMP 0x00000414
+#define MCDE_OVL0COMP_GROUPOFFSET 0x20
+#define MCDE_OVL0COMP_XPOS_SHIFT 0
+#define MCDE_OVL0COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL0COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL0COMP, XPOS, __x)
+#define MCDE_OVL0COMP_CH_ID_SHIFT 11
+#define MCDE_OVL0COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL0COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL0COMP, CH_ID, __x)
+#define MCDE_OVL0COMP_YPOS_SHIFT 16
+#define MCDE_OVL0COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL0COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL0COMP, YPOS, __x)
+#define MCDE_OVL0COMP_Z_SHIFT 27
+#define MCDE_OVL0COMP_Z_MASK 0x78000000
+#define MCDE_OVL0COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL0COMP, Z, __x)
+#define MCDE_OVL1COMP 0x00000434
+#define MCDE_OVL1COMP_XPOS_SHIFT 0
+#define MCDE_OVL1COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL1COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL1COMP, XPOS, __x)
+#define MCDE_OVL1COMP_CH_ID_SHIFT 11
+#define MCDE_OVL1COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL1COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL1COMP, CH_ID, __x)
+#define MCDE_OVL1COMP_YPOS_SHIFT 16
+#define MCDE_OVL1COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL1COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL1COMP, YPOS, __x)
+#define MCDE_OVL1COMP_Z_SHIFT 27
+#define MCDE_OVL1COMP_Z_MASK 0x78000000
+#define MCDE_OVL1COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL1COMP, Z, __x)
+#define MCDE_OVL2COMP 0x00000454
+#define MCDE_OVL2COMP_XPOS_SHIFT 0
+#define MCDE_OVL2COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL2COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL2COMP, XPOS, __x)
+#define MCDE_OVL2COMP_CH_ID_SHIFT 11
+#define MCDE_OVL2COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL2COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL2COMP, CH_ID, __x)
+#define MCDE_OVL2COMP_YPOS_SHIFT 16
+#define MCDE_OVL2COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL2COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL2COMP, YPOS, __x)
+#define MCDE_OVL2COMP_Z_SHIFT 27
+#define MCDE_OVL2COMP_Z_MASK 0x78000000
+#define MCDE_OVL2COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL2COMP, Z, __x)
+#define MCDE_OVL3COMP 0x00000474
+#define MCDE_OVL3COMP_XPOS_SHIFT 0
+#define MCDE_OVL3COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL3COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL3COMP, XPOS, __x)
+#define MCDE_OVL3COMP_CH_ID_SHIFT 11
+#define MCDE_OVL3COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL3COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL3COMP, CH_ID, __x)
+#define MCDE_OVL3COMP_YPOS_SHIFT 16
+#define MCDE_OVL3COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL3COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL3COMP, YPOS, __x)
+#define MCDE_OVL3COMP_Z_SHIFT 27
+#define MCDE_OVL3COMP_Z_MASK 0x78000000
+#define MCDE_OVL3COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL3COMP, Z, __x)
+#define MCDE_OVL4COMP 0x00000494
+#define MCDE_OVL4COMP_XPOS_SHIFT 0
+#define MCDE_OVL4COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL4COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL4COMP, XPOS, __x)
+#define MCDE_OVL4COMP_CH_ID_SHIFT 11
+#define MCDE_OVL4COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL4COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL4COMP, CH_ID, __x)
+#define MCDE_OVL4COMP_YPOS_SHIFT 16
+#define MCDE_OVL4COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL4COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL4COMP, YPOS, __x)
+#define MCDE_OVL4COMP_Z_SHIFT 27
+#define MCDE_OVL4COMP_Z_MASK 0x78000000
+#define MCDE_OVL4COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL4COMP, Z, __x)
+#define MCDE_OVL5COMP 0x000004B4
+#define MCDE_OVL5COMP_XPOS_SHIFT 0
+#define MCDE_OVL5COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL5COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL5COMP, XPOS, __x)
+#define MCDE_OVL5COMP_CH_ID_SHIFT 11
+#define MCDE_OVL5COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL5COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL5COMP, CH_ID, __x)
+#define MCDE_OVL5COMP_YPOS_SHIFT 16
+#define MCDE_OVL5COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL5COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL5COMP, YPOS, __x)
+#define MCDE_OVL5COMP_Z_SHIFT 27
+#define MCDE_OVL5COMP_Z_MASK 0x78000000
+#define MCDE_OVL5COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL5COMP, Z, __x)
+#define MCDE_CHNL0CONF 0x00000600
+#define MCDE_CHNL0CONF_GROUPOFFSET 0x20
+#define MCDE_CHNL0CONF_PPL_SHIFT 0
+#define MCDE_CHNL0CONF_PPL_MASK 0x000007FF
+#define MCDE_CHNL0CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0CONF, PPL, __x)
+#define MCDE_CHNL0CONF_LPF_SHIFT 16
+#define MCDE_CHNL0CONF_LPF_MASK 0x07FF0000
+#define MCDE_CHNL0CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0CONF, LPF, __x)
+#define MCDE_CHNL1CONF 0x00000620
+#define MCDE_CHNL1CONF_PPL_SHIFT 0
+#define MCDE_CHNL1CONF_PPL_MASK 0x000007FF
+#define MCDE_CHNL1CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1CONF, PPL, __x)
+#define MCDE_CHNL1CONF_LPF_SHIFT 16
+#define MCDE_CHNL1CONF_LPF_MASK 0x07FF0000
+#define MCDE_CHNL1CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1CONF, LPF, __x)
+#define MCDE_CHNL2CONF 0x00000640
+#define MCDE_CHNL2CONF_PPL_SHIFT 0
+#define MCDE_CHNL2CONF_PPL_MASK 0x000007FF
+#define MCDE_CHNL2CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2CONF, PPL, __x)
+#define MCDE_CHNL2CONF_LPF_SHIFT 16
+#define MCDE_CHNL2CONF_LPF_MASK 0x07FF0000
+#define MCDE_CHNL2CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2CONF, LPF, __x)
+#define MCDE_CHNL3CONF 0x00000660
+#define MCDE_CHNL3CONF_PPL_SHIFT 0
+#define MCDE_CHNL3CONF_PPL_MASK 0x000007FF
+#define MCDE_CHNL3CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3CONF, PPL, __x)
+#define MCDE_CHNL3CONF_LPF_SHIFT 16
+#define MCDE_CHNL3CONF_LPF_MASK 0x07FF0000
+#define MCDE_CHNL3CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3CONF, LPF, __x)
+#define MCDE_CHNL0STAT 0x00000604
+#define MCDE_CHNL0STAT_GROUPOFFSET 0x20
+#define MCDE_CHNL0STAT_CHNLRD_SHIFT 0
+#define MCDE_CHNL0STAT_CHNLRD_MASK 0x00000001
+#define MCDE_CHNL0STAT_CHNLRD(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0STAT, CHNLRD, __x)
+#define MCDE_CHNL0STAT_CHNLA_SHIFT 1
+#define MCDE_CHNL0STAT_CHNLA_MASK 0x00000002
+#define MCDE_CHNL0STAT_CHNLA(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0STAT, CHNLA, __x)
+#define MCDE_CHNL0STAT_CHNLBLBCKGND_EN_SHIFT 16
+#define MCDE_CHNL0STAT_CHNLBLBCKGND_EN_MASK 0x00010000
+#define MCDE_CHNL0STAT_CHNLBLBCKGND_EN(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0STAT, CHNLBLBCKGND_EN, __x)
+#define MCDE_CHNL1STAT 0x00000624
+#define MCDE_CHNL1STAT_CHNLRD_SHIFT 0
+#define MCDE_CHNL1STAT_CHNLRD_MASK 0x00000001
+#define MCDE_CHNL1STAT_CHNLRD(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1STAT, CHNLRD, __x)
+#define MCDE_CHNL1STAT_CHNLA_SHIFT 1
+#define MCDE_CHNL1STAT_CHNLA_MASK 0x00000002
+#define MCDE_CHNL1STAT_CHNLA(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1STAT, CHNLA, __x)
+#define MCDE_CHNL1STAT_CHNLBLBCKGND_EN_SHIFT 16
+#define MCDE_CHNL1STAT_CHNLBLBCKGND_EN_MASK 0x00010000
+#define MCDE_CHNL1STAT_CHNLBLBCKGND_EN(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1STAT, CHNLBLBCKGND_EN, __x)
+#define MCDE_CHNL2STAT 0x00000644
+#define MCDE_CHNL2STAT_CHNLRD_SHIFT 0
+#define MCDE_CHNL2STAT_CHNLRD_MASK 0x00000001
+#define MCDE_CHNL2STAT_CHNLRD(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2STAT, CHNLRD, __x)
+#define MCDE_CHNL2STAT_CHNLA_SHIFT 1
+#define MCDE_CHNL2STAT_CHNLA_MASK 0x00000002
+#define MCDE_CHNL2STAT_CHNLA(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2STAT, CHNLA, __x)
+#define MCDE_CHNL2STAT_CHNLBLBCKGND_EN_SHIFT 16
+#define MCDE_CHNL2STAT_CHNLBLBCKGND_EN_MASK 0x00010000
+#define MCDE_CHNL2STAT_CHNLBLBCKGND_EN(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2STAT, CHNLBLBCKGND_EN, __x)
+#define MCDE_CHNL3STAT 0x00000664
+#define MCDE_CHNL3STAT_CHNLRD_SHIFT 0
+#define MCDE_CHNL3STAT_CHNLRD_MASK 0x00000001
+#define MCDE_CHNL3STAT_CHNLRD(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3STAT, CHNLRD, __x)
+#define MCDE_CHNL3STAT_CHNLA_SHIFT 1
+#define MCDE_CHNL3STAT_CHNLA_MASK 0x00000002
+#define MCDE_CHNL3STAT_CHNLA(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3STAT, CHNLA, __x)
+#define MCDE_CHNL3STAT_CHNLBLBCKGND_EN_SHIFT 16
+#define MCDE_CHNL3STAT_CHNLBLBCKGND_EN_MASK 0x00010000
+#define MCDE_CHNL3STAT_CHNLBLBCKGND_EN(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3STAT, CHNLBLBCKGND_EN, __x)
+#define MCDE_CHNL0SYNCHMOD 0x00000608
+#define MCDE_CHNL0SYNCHMOD_GROUPOFFSET 0x20
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SHIFT 0
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_MASK 0x00000003
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_OUTPUT 0
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_AUTO 1
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE 2
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_EXTERNAL 3
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0SYNCHMOD, SRC_SYNCH, \
+	MCDE_CHNL0SYNCHMOD_SRC_SYNCH_##__x)
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0SYNCHMOD, SRC_SYNCH, __x)
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_SHIFT 2
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_MASK 0x0000001C
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_FORMATTER 0
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_VSYNC0 1
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_VSYNC1 2
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0SYNCHMOD, OUT_SYNCH_SRC, \
+	MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_##__x)
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0SYNCHMOD, OUT_SYNCH_SRC, __x)
+#define MCDE_CHNL1SYNCHMOD 0x00000628
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_SHIFT 0
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_MASK 0x00000003
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_OUTPUT 0
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_AUTO 1
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_SOFTWARE 2
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_EXTERNAL 3
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1SYNCHMOD, SRC_SYNCH, \
+	MCDE_CHNL1SYNCHMOD_SRC_SYNCH_##__x)
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1SYNCHMOD, SRC_SYNCH, __x)
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_SHIFT 2
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_MASK 0x0000001C
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_FORMATTER 0
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_VSYNC0 1
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_VSYNC1 2
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1SYNCHMOD, OUT_SYNCH_SRC, \
+	MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_##__x)
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1SYNCHMOD, OUT_SYNCH_SRC, __x)
+#define MCDE_CHNL2SYNCHMOD 0x00000648
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_SHIFT 0
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_MASK 0x00000003
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_OUTPUT 0
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_AUTO 1
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_SOFTWARE 2
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_EXTERNAL 3
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2SYNCHMOD, SRC_SYNCH, \
+	MCDE_CHNL2SYNCHMOD_SRC_SYNCH_##__x)
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2SYNCHMOD, SRC_SYNCH, __x)
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_SHIFT 2
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_MASK 0x0000001C
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_FORMATTER 0
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_VSYNC0 1
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_VSYNC1 2
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2SYNCHMOD, OUT_SYNCH_SRC, \
+	MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_##__x)
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2SYNCHMOD, OUT_SYNCH_SRC, __x)
+#define MCDE_CHNL3SYNCHMOD 0x00000668
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_SHIFT 0
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_MASK 0x00000003
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_OUTPUT 0
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_AUTO 1
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_SOFTWARE 2
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_EXTERNAL 3
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3SYNCHMOD, SRC_SYNCH, \
+	MCDE_CHNL3SYNCHMOD_SRC_SYNCH_##__x)
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3SYNCHMOD, SRC_SYNCH, __x)
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_SHIFT 2
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_MASK 0x0000001C
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_FORMATTER 0
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_VSYNC0 1
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_VSYNC1 2
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3SYNCHMOD, OUT_SYNCH_SRC, \
+	MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_##__x)
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3SYNCHMOD, OUT_SYNCH_SRC, __x)
+#define MCDE_CHNL0SYNCHSW 0x0000060C
+#define MCDE_CHNL0SYNCHSW_GROUPOFFSET 0x20
+#define MCDE_CHNL0SYNCHSW_SW_TRIG_SHIFT 0
+#define MCDE_CHNL0SYNCHSW_SW_TRIG_MASK 0x00000001
+#define MCDE_CHNL0SYNCHSW_SW_TRIG(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0SYNCHSW, SW_TRIG, __x)
+#define MCDE_CHNL1SYNCHSW 0x0000062C
+#define MCDE_CHNL1SYNCHSW_SW_TRIG_SHIFT 0
+#define MCDE_CHNL1SYNCHSW_SW_TRIG_MASK 0x00000001
+#define MCDE_CHNL1SYNCHSW_SW_TRIG(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1SYNCHSW, SW_TRIG, __x)
+#define MCDE_CHNL2SYNCHSW 0x0000064C
+#define MCDE_CHNL2SYNCHSW_SW_TRIG_SHIFT 0
+#define MCDE_CHNL2SYNCHSW_SW_TRIG_MASK 0x00000001
+#define MCDE_CHNL2SYNCHSW_SW_TRIG(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2SYNCHSW, SW_TRIG, __x)
+#define MCDE_CHNL3SYNCHSW 0x0000066C
+#define MCDE_CHNL3SYNCHSW_SW_TRIG_SHIFT 0
+#define MCDE_CHNL3SYNCHSW_SW_TRIG_MASK 0x00000001
+#define MCDE_CHNL3SYNCHSW_SW_TRIG(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3SYNCHSW, SW_TRIG, __x)
+#define MCDE_CHNL0BCKGNDCOL 0x00000610
+#define MCDE_CHNL0BCKGNDCOL_GROUPOFFSET 0x20
+#define MCDE_CHNL0BCKGNDCOL_B_SHIFT 0
+#define MCDE_CHNL0BCKGNDCOL_B_MASK 0x000000FF
+#define MCDE_CHNL0BCKGNDCOL_B(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0BCKGNDCOL, B, __x)
+#define MCDE_CHNL0BCKGNDCOL_G_SHIFT 8
+#define MCDE_CHNL0BCKGNDCOL_G_MASK 0x0000FF00
+#define MCDE_CHNL0BCKGNDCOL_G(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0BCKGNDCOL, G, __x)
+#define MCDE_CHNL0BCKGNDCOL_R_SHIFT 16
+#define MCDE_CHNL0BCKGNDCOL_R_MASK 0x00FF0000
+#define MCDE_CHNL0BCKGNDCOL_R(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0BCKGNDCOL, R, __x)
+#define MCDE_CHNL1BCKGNDCOL 0x00000630
+#define MCDE_CHNL1BCKGNDCOL_B_SHIFT 0
+#define MCDE_CHNL1BCKGNDCOL_B_MASK 0x000000FF
+#define MCDE_CHNL1BCKGNDCOL_B(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1BCKGNDCOL, B, __x)
+#define MCDE_CHNL1BCKGNDCOL_G_SHIFT 8
+#define MCDE_CHNL1BCKGNDCOL_G_MASK 0x0000FF00
+#define MCDE_CHNL1BCKGNDCOL_G(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1BCKGNDCOL, G, __x)
+#define MCDE_CHNL1BCKGNDCOL_R_SHIFT 16
+#define MCDE_CHNL1BCKGNDCOL_R_MASK 0x00FF0000
+#define MCDE_CHNL1BCKGNDCOL_R(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1BCKGNDCOL, R, __x)
+#define MCDE_CHNL2BCKGNDCOL 0x00000650
+#define MCDE_CHNL2BCKGNDCOL_B_SHIFT 0
+#define MCDE_CHNL2BCKGNDCOL_B_MASK 0x000000FF
+#define MCDE_CHNL2BCKGNDCOL_B(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2BCKGNDCOL, B, __x)
+#define MCDE_CHNL2BCKGNDCOL_G_SHIFT 8
+#define MCDE_CHNL2BCKGNDCOL_G_MASK 0x0000FF00
+#define MCDE_CHNL2BCKGNDCOL_G(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2BCKGNDCOL, G, __x)
+#define MCDE_CHNL2BCKGNDCOL_R_SHIFT 16
+#define MCDE_CHNL2BCKGNDCOL_R_MASK 0x00FF0000
+#define MCDE_CHNL2BCKGNDCOL_R(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2BCKGNDCOL, R, __x)
+#define MCDE_CHNL3BCKGNDCOL 0x00000670
+#define MCDE_CHNL3BCKGNDCOL_B_SHIFT 0
+#define MCDE_CHNL3BCKGNDCOL_B_MASK 0x000000FF
+#define MCDE_CHNL3BCKGNDCOL_B(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3BCKGNDCOL, B, __x)
+#define MCDE_CHNL3BCKGNDCOL_G_SHIFT 8
+#define MCDE_CHNL3BCKGNDCOL_G_MASK 0x0000FF00
+#define MCDE_CHNL3BCKGNDCOL_G(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3BCKGNDCOL, G, __x)
+#define MCDE_CHNL3BCKGNDCOL_R_SHIFT 16
+#define MCDE_CHNL3BCKGNDCOL_R_MASK 0x00FF0000
+#define MCDE_CHNL3BCKGNDCOL_R(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3BCKGNDCOL, R, __x)
+#define MCDE_CHNL0PRIO_V1 0x00000614
+#define MCDE_CHNL0PRIO_V1_GROUPOFFSET 0x20
+#define MCDE_CHNL0PRIO_V1_CHNLPRIO_SHIFT 0
+#define MCDE_CHNL0PRIO_V1_CHNLPRIO_MASK 0x0000000F
+#define MCDE_CHNL0PRIO_V1_CHNLPRIO(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0PRIO_V1, CHNLPRIO, __x)
+#define MCDE_CHNL1PRIO_V1 0x00000634
+#define MCDE_CHNL1PRIO_V1_CHNLPRIO_SHIFT 0
+#define MCDE_CHNL1PRIO_V1_CHNLPRIO_MASK 0x0000000F
+#define MCDE_CHNL1PRIO_V1_CHNLPRIO(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1PRIO_V1, CHNLPRIO, __x)
+#define MCDE_CHNL2PRIO_V1 0x00000654
+#define MCDE_CHNL2PRIO_V1_CHNLPRIO_SHIFT 0
+#define MCDE_CHNL2PRIO_V1_CHNLPRIO_MASK 0x0000000F
+#define MCDE_CHNL2PRIO_V1_CHNLPRIO(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2PRIO_V1, CHNLPRIO, __x)
+#define MCDE_CHNL3PRIO_V1 0x00000674
+#define MCDE_CHNL3PRIO_V1_CHNLPRIO_SHIFT 0
+#define MCDE_CHNL3PRIO_V1_CHNLPRIO_MASK 0x0000000F
+#define MCDE_CHNL3PRIO_V1_CHNLPRIO(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3PRIO_V1, CHNLPRIO, __x)
+#define MCDE_CHNL0MUXING_V2 0x00000614
+#define MCDE_CHNL0MUXING_V2_GROUPOFFSET 0x20
+#define MCDE_CHNL0MUXING_V2_FIFO_ID_SHIFT 0
+#define MCDE_CHNL0MUXING_V2_FIFO_ID_MASK 0x00000007
+#define MCDE_CHNL0MUXING_V2_FIFO_ID_FIFO_A 0
+#define MCDE_CHNL0MUXING_V2_FIFO_ID_FIFO_B 1
+#define MCDE_CHNL0MUXING_V2_FIFO_ID_FIFO_C0 2
+#define MCDE_CHNL0MUXING_V2_FIFO_ID_FIFO_C1 3
+#define MCDE_CHNL0MUXING_V2_FIFO_ID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0MUXING_V2, FIFO_ID, \
+	MCDE_CHNL0MUXING_V2_FIFO_ID_##__x)
+#define MCDE_CHNL0MUXING_V2_FIFO_ID(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0MUXING_V2, FIFO_ID, __x)
+#define MCDE_CHNL1MUXING_V2 0x00000634
+#define MCDE_CHNL1MUXING_V2_FIFO_ID_SHIFT 0
+#define MCDE_CHNL1MUXING_V2_FIFO_ID_MASK 0x00000007
+#define MCDE_CHNL1MUXING_V2_FIFO_ID_FIFO_A 0
+#define MCDE_CHNL1MUXING_V2_FIFO_ID_FIFO_B 1
+#define MCDE_CHNL1MUXING_V2_FIFO_ID_FIFO_C0 2
+#define MCDE_CHNL1MUXING_V2_FIFO_ID_FIFO_C1 3
+#define MCDE_CHNL1MUXING_V2_FIFO_ID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1MUXING_V2, FIFO_ID, \
+	MCDE_CHNL1MUXING_V2_FIFO_ID_##__x)
+#define MCDE_CHNL1MUXING_V2_FIFO_ID(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1MUXING_V2, FIFO_ID, __x)
+#define MCDE_CHNL2MUXING_V2 0x00000654
+#define MCDE_CHNL2MUXING_V2_FIFO_ID_SHIFT 0
+#define MCDE_CHNL2MUXING_V2_FIFO_ID_MASK 0x00000007
+#define MCDE_CHNL2MUXING_V2_FIFO_ID_FIFO_A 0
+#define MCDE_CHNL2MUXING_V2_FIFO_ID_FIFO_B 1
+#define MCDE_CHNL2MUXING_V2_FIFO_ID_FIFO_C0 2
+#define MCDE_CHNL2MUXING_V2_FIFO_ID_FIFO_C1 3
+#define MCDE_CHNL2MUXING_V2_FIFO_ID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2MUXING_V2, FIFO_ID, \
+	MCDE_CHNL2MUXING_V2_FIFO_ID_##__x)
+#define MCDE_CHNL2MUXING_V2_FIFO_ID(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2MUXING_V2, FIFO_ID, __x)
+#define MCDE_CHNL3MUXING_V2 0x00000674
+#define MCDE_CHNL3MUXING_V2_FIFO_ID_SHIFT 0
+#define MCDE_CHNL3MUXING_V2_FIFO_ID_MASK 0x00000007
+#define MCDE_CHNL3MUXING_V2_FIFO_ID_FIFO_A 0
+#define MCDE_CHNL3MUXING_V2_FIFO_ID_FIFO_B 1
+#define MCDE_CHNL3MUXING_V2_FIFO_ID_FIFO_C0 2
+#define MCDE_CHNL3MUXING_V2_FIFO_ID_FIFO_C1 3
+#define MCDE_CHNL3MUXING_V2_FIFO_ID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3MUXING_V2, FIFO_ID, \
+	MCDE_CHNL3MUXING_V2_FIFO_ID_##__x)
+#define MCDE_CHNL3MUXING_V2_FIFO_ID(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3MUXING_V2, FIFO_ID, __x)
+#define MCDE_CRA0 0x00000800
+#define MCDE_CRA0_GROUPOFFSET 0x200
+#define MCDE_CRA0_FLOEN_SHIFT 0
+#define MCDE_CRA0_FLOEN_MASK 0x00000001
+#define MCDE_CRA0_FLOEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, FLOEN, __x)
+#define MCDE_CRA0_POWEREN_SHIFT 1
+#define MCDE_CRA0_POWEREN_MASK 0x00000002
+#define MCDE_CRA0_POWEREN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, POWEREN, __x)
+#define MCDE_CRA0_BLENDEN_SHIFT 2
+#define MCDE_CRA0_BLENDEN_MASK 0x00000004
+#define MCDE_CRA0_BLENDEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, BLENDEN, __x)
+#define MCDE_CRA0_AFLICKEN_SHIFT 3
+#define MCDE_CRA0_AFLICKEN_MASK 0x00000008
+#define MCDE_CRA0_AFLICKEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, AFLICKEN, __x)
+#define MCDE_CRA0_PALEN_SHIFT 4
+#define MCDE_CRA0_PALEN_MASK 0x00000010
+#define MCDE_CRA0_PALEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, PALEN, __x)
+#define MCDE_CRA0_DITHEN_SHIFT 5
+#define MCDE_CRA0_DITHEN_MASK 0x00000020
+#define MCDE_CRA0_DITHEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, DITHEN, __x)
+#define MCDE_CRA0_GAMEN_SHIFT 6
+#define MCDE_CRA0_GAMEN_MASK 0x00000040
+#define MCDE_CRA0_GAMEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, GAMEN, __x)
+#define MCDE_CRA0_KEYCTRL_SHIFT 7
+#define MCDE_CRA0_KEYCTRL_MASK 0x00000380
+#define MCDE_CRA0_KEYCTRL_OFF 0
+#define MCDE_CRA0_KEYCTRL_ALPHA_RGB 1
+#define MCDE_CRA0_KEYCTRL_RGB 2
+#define MCDE_CRA0_KEYCTRL_FALPHA_FRGB 4
+#define MCDE_CRA0_KEYCTRL_FRGB 5
+#define MCDE_CRA0_KEYCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, KEYCTRL, MCDE_CRA0_KEYCTRL_##__x)
+#define MCDE_CRA0_KEYCTRL(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, KEYCTRL, __x)
+#define MCDE_CRA0_BLENDCTRL_SHIFT 10
+#define MCDE_CRA0_BLENDCTRL_MASK 0x00000400
+#define MCDE_CRA0_BLENDCTRL_SOURCE 0
+#define MCDE_CRA0_BLENDCTRL_CONSTANT 1
+#define MCDE_CRA0_BLENDCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, BLENDCTRL, MCDE_CRA0_BLENDCTRL_##__x)
+#define MCDE_CRA0_BLENDCTRL(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, BLENDCTRL, __x)
+#define MCDE_CRA0_FLICKMODE_SHIFT 11
+#define MCDE_CRA0_FLICKMODE_MASK 0x00001800
+#define MCDE_CRA0_FLICKMODE_FORCE_FILTER_0 0
+#define MCDE_CRA0_FLICKMODE_ADAPTIVE 1
+#define MCDE_CRA0_FLICKMODE_TEST_MODE 2
+#define MCDE_CRA0_FLICKMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, FLICKMODE, MCDE_CRA0_FLICKMODE_##__x)
+#define MCDE_CRA0_FLICKMODE(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, FLICKMODE, __x)
+#define MCDE_CRA0_FLOCKFORMAT_SHIFT 13
+#define MCDE_CRA0_FLOCKFORMAT_MASK 0x00002000
+#define MCDE_CRA0_FLOCKFORMAT_YCBCR 0
+#define MCDE_CRA0_FLOCKFORMAT_RGB 1
+#define MCDE_CRA0_FLOCKFORMAT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, FLOCKFORMAT, MCDE_CRA0_FLOCKFORMAT_##__x)
+#define MCDE_CRA0_FLOCKFORMAT(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, FLOCKFORMAT, __x)
+#define MCDE_CRA0_PALMODE_SHIFT 14
+#define MCDE_CRA0_PALMODE_MASK 0x00004000
+#define MCDE_CRA0_PALMODE_PALETTE 0
+#define MCDE_CRA0_PALMODE_GAMMA 1
+#define MCDE_CRA0_PALMODE(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, PALMODE, __x)
+#define MCDE_CRA0_OLEDEN_SHIFT 15
+#define MCDE_CRA0_OLEDEN_MASK 0x00008000
+#define MCDE_CRA0_OLEDEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, OLEDEN, __x)
+#define MCDE_CRA0_ALPHABLEND_SHIFT 16
+#define MCDE_CRA0_ALPHABLEND_MASK 0x00FF0000
+#define MCDE_CRA0_ALPHABLEND(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, ALPHABLEND, __x)
+#define MCDE_CRA0_ROTEN_SHIFT 24
+#define MCDE_CRA0_ROTEN_MASK 0x01000000
+#define MCDE_CRA0_ROTEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, ROTEN, __x)
+#define MCDE_CRA0_ROTBURSTSIZE_V1_SHIFT 25
+#define MCDE_CRA0_ROTBURSTSIZE_V1_MASK 0x0E000000
+#define MCDE_CRA0_ROTBURSTSIZE_V1_1W 0
+#define MCDE_CRA0_ROTBURSTSIZE_V1_2W 1
+#define MCDE_CRA0_ROTBURSTSIZE_V1_4W 2
+#define MCDE_CRA0_ROTBURSTSIZE_V1_8W 3
+#define MCDE_CRA0_ROTBURSTSIZE_V1_16W 4
+#define MCDE_CRA0_ROTBURSTSIZE_V1_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, ROTBURSTSIZE_V1, \
+	MCDE_CRA0_ROTBURSTSIZE_V1_##__x)
+#define MCDE_CRA0_ROTBURSTSIZE_V1(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, ROTBURSTSIZE_V1, __x)
+#define MCDE_CRA0_ROTBURSTSIZE_HW_V1_SHIFT 28
+#define MCDE_CRA0_ROTBURSTSIZE_HW_V1_MASK 0x10000000
+#define MCDE_CRA0_ROTBURSTSIZE_HW_V1(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, ROTBURSTSIZE_HW_V1, __x)
+#define MCDE_CRB0 0x00000A00
+#define MCDE_CRB0_FLOEN_SHIFT 0
+#define MCDE_CRB0_FLOEN_MASK 0x00000001
+#define MCDE_CRB0_FLOEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, FLOEN, __x)
+#define MCDE_CRB0_POWEREN_SHIFT 1
+#define MCDE_CRB0_POWEREN_MASK 0x00000002
+#define MCDE_CRB0_POWEREN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, POWEREN, __x)
+#define MCDE_CRB0_BLENDEN_SHIFT 2
+#define MCDE_CRB0_BLENDEN_MASK 0x00000004
+#define MCDE_CRB0_BLENDEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, BLENDEN, __x)
+#define MCDE_CRB0_AFLICKEN_SHIFT 3
+#define MCDE_CRB0_AFLICKEN_MASK 0x00000008
+#define MCDE_CRB0_AFLICKEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, AFLICKEN, __x)
+#define MCDE_CRB0_PALEN_SHIFT 4
+#define MCDE_CRB0_PALEN_MASK 0x00000010
+#define MCDE_CRB0_PALEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, PALEN, __x)
+#define MCDE_CRB0_DITHEN_SHIFT 5
+#define MCDE_CRB0_DITHEN_MASK 0x00000020
+#define MCDE_CRB0_DITHEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, DITHEN, __x)
+#define MCDE_CRB0_GAMEN_SHIFT 6
+#define MCDE_CRB0_GAMEN_MASK 0x00000040
+#define MCDE_CRB0_GAMEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, GAMEN, __x)
+#define MCDE_CRB0_KEYCTRL_SHIFT 7
+#define MCDE_CRB0_KEYCTRL_MASK 0x00000380
+#define MCDE_CRB0_KEYCTRL_OFF 0
+#define MCDE_CRB0_KEYCTRL_ALPHA_RGB 1
+#define MCDE_CRB0_KEYCTRL_RGB 2
+#define MCDE_CRB0_KEYCTRL_FALPHA_FRGB 4
+#define MCDE_CRB0_KEYCTRL_FRGB 5
+#define MCDE_CRB0_KEYCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, KEYCTRL, MCDE_CRB0_KEYCTRL_##__x)
+#define MCDE_CRB0_KEYCTRL(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, KEYCTRL, __x)
+#define MCDE_CRB0_BLENDCTRL_SHIFT 10
+#define MCDE_CRB0_BLENDCTRL_MASK 0x00000400
+#define MCDE_CRB0_BLENDCTRL_SOURCE 0
+#define MCDE_CRB0_BLENDCTRL_CONSTANT 1
+#define MCDE_CRB0_BLENDCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, BLENDCTRL, MCDE_CRB0_BLENDCTRL_##__x)
+#define MCDE_CRB0_BLENDCTRL(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, BLENDCTRL, __x)
+#define MCDE_CRB0_FLICKMODE_SHIFT 11
+#define MCDE_CRB0_FLICKMODE_MASK 0x00001800
+#define MCDE_CRB0_FLICKMODE_FORCE_FILTER_0 0
+#define MCDE_CRB0_FLICKMODE_ADAPTIVE 1
+#define MCDE_CRB0_FLICKMODE_TEST_MODE 2
+#define MCDE_CRB0_FLICKMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, FLICKMODE, MCDE_CRB0_FLICKMODE_##__x)
+#define MCDE_CRB0_FLICKMODE(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, FLICKMODE, __x)
+#define MCDE_CRB0_FLOCKFORMAT_SHIFT 13
+#define MCDE_CRB0_FLOCKFORMAT_MASK 0x00002000
+#define MCDE_CRB0_FLOCKFORMAT_YCBCR 0
+#define MCDE_CRB0_FLOCKFORMAT_RGB 1
+#define MCDE_CRB0_FLOCKFORMAT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, FLOCKFORMAT, MCDE_CRB0_FLOCKFORMAT_##__x)
+#define MCDE_CRB0_FLOCKFORMAT(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, FLOCKFORMAT, __x)
+#define MCDE_CRB0_PALMODE_SHIFT 14
+#define MCDE_CRB0_PALMODE_MASK 0x00004000
+#define MCDE_CRB0_PALMODE_PALETTE 0
+#define MCDE_CRB0_PALMODE_GAMMA 1
+#define MCDE_CRB0_PALMODE(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, PALMODE, __x)
+#define MCDE_CRB0_OLEDEN_SHIFT 15
+#define MCDE_CRB0_OLEDEN_MASK 0x00008000
+#define MCDE_CRB0_OLEDEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, OLEDEN, __x)
+#define MCDE_CRB0_ALPHABLEND_SHIFT 16
+#define MCDE_CRB0_ALPHABLEND_MASK 0x00FF0000
+#define MCDE_CRB0_ALPHABLEND(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, ALPHABLEND, __x)
+#define MCDE_CRB0_ROTEN_SHIFT 24
+#define MCDE_CRB0_ROTEN_MASK 0x01000000
+#define MCDE_CRB0_ROTEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, ROTEN, __x)
+#define MCDE_CRB0_ROTBURSTSIZE_V1_SHIFT 25
+#define MCDE_CRB0_ROTBURSTSIZE_V1_MASK 0x0E000000
+#define MCDE_CRB0_ROTBURSTSIZE_V1_1W 0
+#define MCDE_CRB0_ROTBURSTSIZE_V1_2W 1
+#define MCDE_CRB0_ROTBURSTSIZE_V1_4W 2
+#define MCDE_CRB0_ROTBURSTSIZE_V1_8W 3
+#define MCDE_CRB0_ROTBURSTSIZE_V1_16W 4
+#define MCDE_CRB0_ROTBURSTSIZE_V1_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, ROTBURSTSIZE_V1, \
+	MCDE_CRB0_ROTBURSTSIZE_V1_##__x)
+#define MCDE_CRB0_ROTBURSTSIZE_V1(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, ROTBURSTSIZE_V1, __x)
+#define MCDE_CRB0_ROTBURSTSIZE_HW_V1_SHIFT 28
+#define MCDE_CRB0_ROTBURSTSIZE_HW_V1_MASK 0x10000000
+#define MCDE_CRB0_ROTBURSTSIZE_HW_V1(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, ROTBURSTSIZE_HW_V1, __x)
+#define MCDE_CRA1 0x00000804
+#define MCDE_CRA1_GROUPOFFSET 0x200
+#define MCDE_CRA1_PCD_SHIFT 0
+#define MCDE_CRA1_PCD_MASK 0x000003FF
+#define MCDE_CRA1_PCD(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, PCD, __x)
+#define MCDE_CRA1_CLKSEL_SHIFT 10
+#define MCDE_CRA1_CLKSEL_MASK 0x00001C00
+#define MCDE_CRA1_CLKSEL_LCD 0
+#define MCDE_CRA1_CLKSEL_HDMI 1
+#define MCDE_CRA1_CLKSEL_TV 2
+#define MCDE_CRA1_CLKSEL_EXT_TV1 3
+#define MCDE_CRA1_CLKSEL_EXT_TV2 4
+#define MCDE_CRA1_CLKSEL_166MHZ 5
+#define MCDE_CRA1_CLKSEL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CLKSEL, MCDE_CRA1_CLKSEL_##__x)
+#define MCDE_CRA1_CLKSEL(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CLKSEL, __x)
+#define MCDE_CRA1_CDWIN_SHIFT 13
+#define MCDE_CRA1_CDWIN_MASK 0x0001E000
+#define MCDE_CRA1_CDWIN_8BPP_C1 0
+#define MCDE_CRA1_CDWIN_12BPP_C1 1
+#define MCDE_CRA1_CDWIN_12BPP_C2 2
+#define MCDE_CRA1_CDWIN_16BPP_C1 3
+#define MCDE_CRA1_CDWIN_16BPP_C2 4
+#define MCDE_CRA1_CDWIN_16BPP_C3 5
+#define MCDE_CRA1_CDWIN_18BPP_C1 6
+#define MCDE_CRA1_CDWIN_18BPP_C2 7
+#define MCDE_CRA1_CDWIN_24BPP 8
+#define MCDE_CRA1_CDWIN_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CDWIN, MCDE_CRA1_CDWIN_##__x)
+#define MCDE_CRA1_CDWIN(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CDWIN, __x)
+#define MCDE_CRA1_OUTBPP_SHIFT 25
+#define MCDE_CRA1_OUTBPP_MASK 0x1E000000
+#define MCDE_CRA1_OUTBPP_MONO1 0
+#define MCDE_CRA1_OUTBPP_MONO2 1
+#define MCDE_CRA1_OUTBPP_MONO4 2
+#define MCDE_CRA1_OUTBPP_MONO8 3
+#define MCDE_CRA1_OUTBPP_8BPP 4
+#define MCDE_CRA1_OUTBPP_12BPP 5
+#define MCDE_CRA1_OUTBPP_15BPP 6
+#define MCDE_CRA1_OUTBPP_16BPP 7
+#define MCDE_CRA1_OUTBPP_18BPP 8
+#define MCDE_CRA1_OUTBPP_24BPP 9
+#define MCDE_CRA1_OUTBPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, OUTBPP, MCDE_CRA1_OUTBPP_##__x)
+#define MCDE_CRA1_OUTBPP(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, OUTBPP, __x)
+#define MCDE_CRA1_BCD_SHIFT 29
+#define MCDE_CRA1_BCD_MASK 0x20000000
+#define MCDE_CRA1_BCD(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, BCD, __x)
+#define MCDE_CRA1_CLKTYPE_SHIFT 30
+#define MCDE_CRA1_CLKTYPE_MASK 0x40000000
+#define MCDE_CRA1_CLKTYPE_EXTERNAL 0
+#define MCDE_CRA1_CLKTYPE_INTERNAL 1
+#define MCDE_CRA1_CLKTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CLKTYPE, MCDE_CRA1_CLKTYPE_##__x)
+#define MCDE_CRA1_CLKTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CLKTYPE, __x)
+#define MCDE_CRA1_TEFFECTEN_V1_SHIFT 31
+#define MCDE_CRA1_TEFFECTEN_V1_MASK 0x80000000
+#define MCDE_CRA1_TEFFECTEN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, TEFFECTEN_V1, __x)
+#define MCDE_CRB1 0x00000A04
+#define MCDE_CRB1_PCD_SHIFT 0
+#define MCDE_CRB1_PCD_MASK 0x000003FF
+#define MCDE_CRB1_PCD(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, PCD, __x)
+#define MCDE_CRB1_CLKSEL_SHIFT 10
+#define MCDE_CRB1_CLKSEL_MASK 0x00001C00
+#define MCDE_CRB1_CLKSEL_LCD 0
+#define MCDE_CRB1_CLKSEL_HDMI 1
+#define MCDE_CRB1_CLKSEL_TV 2
+#define MCDE_CRB1_CLKSEL_EXT_TV1 3
+#define MCDE_CRB1_CLKSEL_EXT_TV2 4
+#define MCDE_CRB1_CLKSEL_166MHZ 5
+#define MCDE_CRB1_CLKSEL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CLKSEL, MCDE_CRB1_CLKSEL_##__x)
+#define MCDE_CRB1_CLKSEL(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CLKSEL, __x)
+#define MCDE_CRB1_CDWIN_SHIFT 13
+#define MCDE_CRB1_CDWIN_MASK 0x0001E000
+#define MCDE_CRB1_CDWIN_8BPP_C1 0
+#define MCDE_CRB1_CDWIN_12BPP_C1 1
+#define MCDE_CRB1_CDWIN_12BPP_C2 2
+#define MCDE_CRB1_CDWIN_16BPP_C1 3
+#define MCDE_CRB1_CDWIN_16BPP_C2 4
+#define MCDE_CRB1_CDWIN_16BPP_C3 5
+#define MCDE_CRB1_CDWIN_18BPP_C1 6
+#define MCDE_CRB1_CDWIN_18BPP_C2 7
+#define MCDE_CRB1_CDWIN_24BPP 8
+#define MCDE_CRB1_CDWIN_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CDWIN, MCDE_CRB1_CDWIN_##__x)
+#define MCDE_CRB1_CDWIN(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CDWIN, __x)
+#define MCDE_CRB1_OUTBPP_SHIFT 25
+#define MCDE_CRB1_OUTBPP_MASK 0x1E000000
+#define MCDE_CRB1_OUTBPP_MONO1 0
+#define MCDE_CRB1_OUTBPP_MONO2 1
+#define MCDE_CRB1_OUTBPP_MONO4 2
+#define MCDE_CRB1_OUTBPP_MONO8 3
+#define MCDE_CRB1_OUTBPP_8BPP 4
+#define MCDE_CRB1_OUTBPP_12BPP 5
+#define MCDE_CRB1_OUTBPP_15BPP 6
+#define MCDE_CRB1_OUTBPP_16BPP 7
+#define MCDE_CRB1_OUTBPP_18BPP 8
+#define MCDE_CRB1_OUTBPP_24BPP 9
+#define MCDE_CRB1_OUTBPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, OUTBPP, MCDE_CRB1_OUTBPP_##__x)
+#define MCDE_CRB1_OUTBPP(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, OUTBPP, __x)
+#define MCDE_CRB1_BCD_SHIFT 29
+#define MCDE_CRB1_BCD_MASK 0x20000000
+#define MCDE_CRB1_BCD(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, BCD, __x)
+#define MCDE_CRB1_CLKTYPE_SHIFT 30
+#define MCDE_CRB1_CLKTYPE_MASK 0x40000000
+#define MCDE_CRB1_CLKTYPE_EXTERNAL 0
+#define MCDE_CRB1_CLKTYPE_INTERNAL 1
+#define MCDE_CRB1_CLKTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CLKTYPE, MCDE_CRB1_CLKTYPE_##__x)
+#define MCDE_CRB1_CLKTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CLKTYPE, __x)
+#define MCDE_CRB1_TEFFECTEN_V1_SHIFT 31
+#define MCDE_CRB1_TEFFECTEN_V1_MASK 0x80000000
+#define MCDE_CRB1_TEFFECTEN_V1(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, TEFFECTEN_V1, __x)
+#define MCDE_COLKEYA 0x00000808
+#define MCDE_COLKEYA_GROUPOFFSET 0x200
+#define MCDE_COLKEYA_KEYB_SHIFT 0
+#define MCDE_COLKEYA_KEYB_MASK 0x000000FF
+#define MCDE_COLKEYA_KEYB(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYA, KEYB, __x)
+#define MCDE_COLKEYA_KEYG_SHIFT 8
+#define MCDE_COLKEYA_KEYG_MASK 0x0000FF00
+#define MCDE_COLKEYA_KEYG(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYA, KEYG, __x)
+#define MCDE_COLKEYA_KEYR_SHIFT 16
+#define MCDE_COLKEYA_KEYR_MASK 0x00FF0000
+#define MCDE_COLKEYA_KEYR(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYA, KEYR, __x)
+#define MCDE_COLKEYA_KEYA_SHIFT 24
+#define MCDE_COLKEYA_KEYA_MASK 0xFF000000
+#define MCDE_COLKEYA_KEYA(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYA, KEYA, __x)
+#define MCDE_COLKEYB 0x00000A08
+#define MCDE_COLKEYB_KEYB_SHIFT 0
+#define MCDE_COLKEYB_KEYB_MASK 0x000000FF
+#define MCDE_COLKEYB_KEYB(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYB, KEYB, __x)
+#define MCDE_COLKEYB_KEYG_SHIFT 8
+#define MCDE_COLKEYB_KEYG_MASK 0x0000FF00
+#define MCDE_COLKEYB_KEYG(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYB, KEYG, __x)
+#define MCDE_COLKEYB_KEYR_SHIFT 16
+#define MCDE_COLKEYB_KEYR_MASK 0x00FF0000
+#define MCDE_COLKEYB_KEYR(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYB, KEYR, __x)
+#define MCDE_COLKEYB_KEYA_SHIFT 24
+#define MCDE_COLKEYB_KEYA_MASK 0xFF000000
+#define MCDE_COLKEYB_KEYA(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYB, KEYA, __x)
+#define MCDE_FCOLKEYA 0x0000080C
+#define MCDE_FCOLKEYA_GROUPOFFSET 0x200
+#define MCDE_FCOLKEYA_FKEYB_SHIFT 0
+#define MCDE_FCOLKEYA_FKEYB_MASK 0x000000FF
+#define MCDE_FCOLKEYA_FKEYB(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYA, FKEYB, __x)
+#define MCDE_FCOLKEYA_FKEYG_SHIFT 8
+#define MCDE_FCOLKEYA_FKEYG_MASK 0x0000FF00
+#define MCDE_FCOLKEYA_FKEYG(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYA, FKEYG, __x)
+#define MCDE_FCOLKEYA_FKEYR_SHIFT 16
+#define MCDE_FCOLKEYA_FKEYR_MASK 0x00FF0000
+#define MCDE_FCOLKEYA_FKEYR(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYA, FKEYR, __x)
+#define MCDE_FCOLKEYA_FKEYA_SHIFT 24
+#define MCDE_FCOLKEYA_FKEYA_MASK 0xFF000000
+#define MCDE_FCOLKEYA_FKEYA(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYA, FKEYA, __x)
+#define MCDE_FCOLKEYB 0x00000A0C
+#define MCDE_FCOLKEYB_FKEYB_SHIFT 0
+#define MCDE_FCOLKEYB_FKEYB_MASK 0x000000FF
+#define MCDE_FCOLKEYB_FKEYB(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYB, FKEYB, __x)
+#define MCDE_FCOLKEYB_FKEYG_SHIFT 8
+#define MCDE_FCOLKEYB_FKEYG_MASK 0x0000FF00
+#define MCDE_FCOLKEYB_FKEYG(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYB, FKEYG, __x)
+#define MCDE_FCOLKEYB_FKEYR_SHIFT 16
+#define MCDE_FCOLKEYB_FKEYR_MASK 0x00FF0000
+#define MCDE_FCOLKEYB_FKEYR(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYB, FKEYR, __x)
+#define MCDE_FCOLKEYB_FKEYA_SHIFT 24
+#define MCDE_FCOLKEYB_FKEYA_MASK 0xFF000000
+#define MCDE_FCOLKEYB_FKEYA(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYB, FKEYA, __x)
+#define MCDE_RGBCONV1A 0x00000810
+#define MCDE_RGBCONV1A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV1A_YR_GREEN_SHIFT 0
+#define MCDE_RGBCONV1A_YR_GREEN_MASK 0x000007FF
+#define MCDE_RGBCONV1A_YR_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV1A, YR_GREEN, __x)
+#define MCDE_RGBCONV1A_YR_RED_SHIFT 16
+#define MCDE_RGBCONV1A_YR_RED_MASK 0x07FF0000
+#define MCDE_RGBCONV1A_YR_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV1A, YR_RED, __x)
+#define MCDE_RGBCONV1B 0x00000A10
+#define MCDE_RGBCONV1B_YR_GREEN_SHIFT 0
+#define MCDE_RGBCONV1B_YR_GREEN_MASK 0x000007FF
+#define MCDE_RGBCONV1B_YR_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV1B, YR_GREEN, __x)
+#define MCDE_RGBCONV1B_YR_RED_SHIFT 16
+#define MCDE_RGBCONV1B_YR_RED_MASK 0x07FF0000
+#define MCDE_RGBCONV1B_YR_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV1B, YR_RED, __x)
+#define MCDE_RGBCONV2A 0x00000814
+#define MCDE_RGBCONV2A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV2A_CR_RED_SHIFT 0
+#define MCDE_RGBCONV2A_CR_RED_MASK 0x000007FF
+#define MCDE_RGBCONV2A_CR_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV2A, CR_RED, __x)
+#define MCDE_RGBCONV2A_YR_BLUE_SHIFT 16
+#define MCDE_RGBCONV2A_YR_BLUE_MASK 0x07FF0000
+#define MCDE_RGBCONV2A_YR_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV2A, YR_BLUE, __x)
+#define MCDE_RGBCONV2B 0x00000A14
+#define MCDE_RGBCONV2B_CR_RED_SHIFT 0
+#define MCDE_RGBCONV2B_CR_RED_MASK 0x000007FF
+#define MCDE_RGBCONV2B_CR_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV2B, CR_RED, __x)
+#define MCDE_RGBCONV2B_YR_BLUE_SHIFT 16
+#define MCDE_RGBCONV2B_YR_BLUE_MASK 0x07FF0000
+#define MCDE_RGBCONV2B_YR_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV2B, YR_BLUE, __x)
+#define MCDE_RGBCONV3A 0x00000818
+#define MCDE_RGBCONV3A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV3A_CR_BLUE_SHIFT 0
+#define MCDE_RGBCONV3A_CR_BLUE_MASK 0x000007FF
+#define MCDE_RGBCONV3A_CR_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV3A, CR_BLUE, __x)
+#define MCDE_RGBCONV3A_CR_GREEN_SHIFT 16
+#define MCDE_RGBCONV3A_CR_GREEN_MASK 0x07FF0000
+#define MCDE_RGBCONV3A_CR_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV3A, CR_GREEN, __x)
+#define MCDE_RGBCONV3B 0x00000A18
+#define MCDE_RGBCONV3B_CR_BLUE_SHIFT 0
+#define MCDE_RGBCONV3B_CR_BLUE_MASK 0x000007FF
+#define MCDE_RGBCONV3B_CR_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV3B, CR_BLUE, __x)
+#define MCDE_RGBCONV3B_CR_GREEN_SHIFT 16
+#define MCDE_RGBCONV3B_CR_GREEN_MASK 0x07FF0000
+#define MCDE_RGBCONV3B_CR_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV3B, CR_GREEN, __x)
+#define MCDE_RGBCONV4A 0x0000081C
+#define MCDE_RGBCONV4A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV4A_CB_GREEN_SHIFT 0
+#define MCDE_RGBCONV4A_CB_GREEN_MASK 0x000007FF
+#define MCDE_RGBCONV4A_CB_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV4A, CB_GREEN, __x)
+#define MCDE_RGBCONV4A_CB_RED_SHIFT 16
+#define MCDE_RGBCONV4A_CB_RED_MASK 0x07FF0000
+#define MCDE_RGBCONV4A_CB_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV4A, CB_RED, __x)
+#define MCDE_RGBCONV4B 0x00000A1C
+#define MCDE_RGBCONV4B_CB_GREEN_SHIFT 0
+#define MCDE_RGBCONV4B_CB_GREEN_MASK 0x000007FF
+#define MCDE_RGBCONV4B_CB_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV4B, CB_GREEN, __x)
+#define MCDE_RGBCONV4B_CB_RED_SHIFT 16
+#define MCDE_RGBCONV4B_CB_RED_MASK 0x07FF0000
+#define MCDE_RGBCONV4B_CB_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV4B, CB_RED, __x)
+#define MCDE_RGBCONV5A 0x00000820
+#define MCDE_RGBCONV5A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV5A_OFF_RED_SHIFT 0
+#define MCDE_RGBCONV5A_OFF_RED_MASK 0x000007FF
+#define MCDE_RGBCONV5A_OFF_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV5A, OFF_RED, __x)
+#define MCDE_RGBCONV5A_CB_BLUE_SHIFT 16
+#define MCDE_RGBCONV5A_CB_BLUE_MASK 0x07FF0000
+#define MCDE_RGBCONV5A_CB_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV5A, CB_BLUE, __x)
+#define MCDE_RGBCONV5B 0x00000A20
+#define MCDE_RGBCONV5B_OFF_RED_SHIFT 0
+#define MCDE_RGBCONV5B_OFF_RED_MASK 0x000007FF
+#define MCDE_RGBCONV5B_OFF_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV5B, OFF_RED, __x)
+#define MCDE_RGBCONV5B_CB_BLUE_SHIFT 16
+#define MCDE_RGBCONV5B_CB_BLUE_MASK 0x07FF0000
+#define MCDE_RGBCONV5B_CB_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV5B, CB_BLUE, __x)
+#define MCDE_RGBCONV6A 0x00000824
+#define MCDE_RGBCONV6A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV6A_OFF_BLUE_SHIFT 0
+#define MCDE_RGBCONV6A_OFF_BLUE_MASK 0x000007FF
+#define MCDE_RGBCONV6A_OFF_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV6A, OFF_BLUE, __x)
+#define MCDE_RGBCONV6A_OFF_GREEN_SHIFT 16
+#define MCDE_RGBCONV6A_OFF_GREEN_MASK 0x07FF0000
+#define MCDE_RGBCONV6A_OFF_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV6A, OFF_GREEN, __x)
+#define MCDE_RGBCONV6B 0x00000A24
+#define MCDE_RGBCONV6B_OFF_BLUE_SHIFT 0
+#define MCDE_RGBCONV6B_OFF_BLUE_MASK 0x000007FF
+#define MCDE_RGBCONV6B_OFF_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV6B, OFF_BLUE, __x)
+#define MCDE_RGBCONV6B_OFF_GREEN_SHIFT 16
+#define MCDE_RGBCONV6B_OFF_GREEN_MASK 0x07FF0000
+#define MCDE_RGBCONV6B_OFF_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV6B, OFF_GREEN, __x)
+#define MCDE_FFCOEF0 0x00000828
+#define MCDE_FFCOEF0_COEFF0_N1_SHIFT 0
+#define MCDE_FFCOEF0_COEFF0_N1_MASK 0x000000FF
+#define MCDE_FFCOEF0_COEFF0_N1(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF0, COEFF0_N1, __x)
+#define MCDE_FFCOEF0_COEFF0_N2_SHIFT 8
+#define MCDE_FFCOEF0_COEFF0_N2_MASK 0x0000FF00
+#define MCDE_FFCOEF0_COEFF0_N2(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF0, COEFF0_N2, __x)
+#define MCDE_FFCOEF0_COEFF0_N3_SHIFT 16
+#define MCDE_FFCOEF0_COEFF0_N3_MASK 0x00FF0000
+#define MCDE_FFCOEF0_COEFF0_N3(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF0, COEFF0_N3, __x)
+#define MCDE_FFCOEF0_T0_SHIFT 24
+#define MCDE_FFCOEF0_T0_MASK 0x0F000000
+#define MCDE_FFCOEF0_T0(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF0, T0, __x)
+#define MCDE_FFCOEF1 0x0000082C
+#define MCDE_FFCOEF1_COEFF1_N1_SHIFT 0
+#define MCDE_FFCOEF1_COEFF1_N1_MASK 0x000000FF
+#define MCDE_FFCOEF1_COEFF1_N1(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF1, COEFF1_N1, __x)
+#define MCDE_FFCOEF1_COEFF1_N2_SHIFT 8
+#define MCDE_FFCOEF1_COEFF1_N2_MASK 0x0000FF00
+#define MCDE_FFCOEF1_COEFF1_N2(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF1, COEFF1_N2, __x)
+#define MCDE_FFCOEF1_COEFF1_N3_SHIFT 16
+#define MCDE_FFCOEF1_COEFF1_N3_MASK 0x00FF0000
+#define MCDE_FFCOEF1_COEFF1_N3(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF1, COEFF1_N3, __x)
+#define MCDE_FFCOEF1_T1_SHIFT 24
+#define MCDE_FFCOEF1_T1_MASK 0x0F000000
+#define MCDE_FFCOEF1_T1(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF1, T1, __x)
+#define MCDE_FFCOEF2 0x00000830
+#define MCDE_FFCOEF2_COEFF2_N1_SHIFT 0
+#define MCDE_FFCOEF2_COEFF2_N1_MASK 0x000000FF
+#define MCDE_FFCOEF2_COEFF2_N1(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF2, COEFF2_N1, __x)
+#define MCDE_FFCOEF2_COEFF2_N2_SHIFT 8
+#define MCDE_FFCOEF2_COEFF2_N2_MASK 0x0000FF00
+#define MCDE_FFCOEF2_COEFF2_N2(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF2, COEFF2_N2, __x)
+#define MCDE_FFCOEF2_COEFF2_N3_SHIFT 16
+#define MCDE_FFCOEF2_COEFF2_N3_MASK 0x00FF0000
+#define MCDE_FFCOEF2_COEFF2_N3(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF2, COEFF2_N3, __x)
+#define MCDE_FFCOEF2_T2_SHIFT 24
+#define MCDE_FFCOEF2_T2_MASK 0x0F000000
+#define MCDE_FFCOEF2_T2(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF2, T2, __x)
+#define MCDE_MCDE_WDATAA_V2 0x00000834
+#define MCDE_MCDE_WDATAA_V2_GROUPOFFSET 0x200
+#define MCDE_MCDE_WDATAA_V2_DC_SHIFT 24
+#define MCDE_MCDE_WDATAA_V2_DC_MASK 0x01000000
+#define MCDE_MCDE_WDATAA_V2_DC(__x) \
+	MCDE_VAL2REG(MCDE_MCDE_WDATAA_V2, DC, __x)
+#define MCDE_MCDE_WDATAA_V2_DATAVALUE_SHIFT 0
+#define MCDE_MCDE_WDATAA_V2_DATAVALUE_MASK 0x00FFFFFF
+#define MCDE_MCDE_WDATAA_V2_DATAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_MCDE_WDATAA_V2, DATAVALUE, __x)
+#define MCDE_MCDE_WDATAB_V2 0x00000A34
+#define MCDE_MCDE_WDATAB_V2_DC_SHIFT 24
+#define MCDE_MCDE_WDATAB_V2_DC_MASK 0x01000000
+#define MCDE_MCDE_WDATAB_V2_DC(__x) \
+	MCDE_VAL2REG(MCDE_MCDE_WDATAB_V2, DC, __x)
+#define MCDE_MCDE_WDATAB_V2_DATAVALUE_SHIFT 0
+#define MCDE_MCDE_WDATAB_V2_DATAVALUE_MASK 0x00FFFFFF
+#define MCDE_MCDE_WDATAB_V2_DATAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_MCDE_WDATAB_V2, DATAVALUE, __x)
+#define MCDE_TVCRA 0x00000838
+#define MCDE_TVCRA_GROUPOFFSET 0x200
+#define MCDE_TVCRA_SEL_MOD_SHIFT 0
+#define MCDE_TVCRA_SEL_MOD_MASK 0x00000001
+#define MCDE_TVCRA_SEL_MOD_LCD 0
+#define MCDE_TVCRA_SEL_MOD_TV 1
+#define MCDE_TVCRA_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, SEL_MOD, MCDE_TVCRA_SEL_MOD_##__x)
+#define MCDE_TVCRA_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, SEL_MOD, __x)
+#define MCDE_TVCRA_INTEREN_SHIFT 1
+#define MCDE_TVCRA_INTEREN_MASK 0x00000002
+#define MCDE_TVCRA_INTEREN(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, INTEREN, __x)
+#define MCDE_TVCRA_IFIELD_SHIFT 2
+#define MCDE_TVCRA_IFIELD_MASK 0x00000004
+#define MCDE_TVCRA_IFIELD(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, IFIELD, __x)
+#define MCDE_TVCRA_TVMODE_SHIFT 3
+#define MCDE_TVCRA_TVMODE_MASK 0x00000038
+#define MCDE_TVCRA_TVMODE_SDTV_656P 0
+#define MCDE_TVCRA_TVMODE_HDTV_480P 1
+#define MCDE_TVCRA_TVMODE_HDTV_720P 2
+#define MCDE_TVCRA_TVMODE_SDTV_656P_LE 3
+#define MCDE_TVCRA_TVMODE_SDTV_656P_BE 4
+#define MCDE_TVCRA_TVMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, TVMODE, MCDE_TVCRA_TVMODE_##__x)
+#define MCDE_TVCRA_TVMODE(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, TVMODE, __x)
+#define MCDE_TVCRA_SDTVMODE_SHIFT 6
+#define MCDE_TVCRA_SDTVMODE_MASK 0x000000C0
+#define MCDE_TVCRA_SDTVMODE_Y0CBY1CR 0
+#define MCDE_TVCRA_SDTVMODE_CBY0CRY1 1
+#define MCDE_TVCRA_SDTVMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, SDTVMODE, MCDE_TVCRA_SDTVMODE_##__x)
+#define MCDE_TVCRA_SDTVMODE(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, SDTVMODE, __x)
+#define MCDE_TVCRA_AVRGEN_SHIFT 8
+#define MCDE_TVCRA_AVRGEN_MASK 0x00000100
+#define MCDE_TVCRA_AVRGEN(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, AVRGEN, __x)
+#define MCDE_TVCRA_CKINV_SHIFT 9
+#define MCDE_TVCRA_CKINV_MASK 0x00000200
+#define MCDE_TVCRA_CKINV(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, CKINV, __x)
+#define MCDE_TVCRB 0x00000A38
+#define MCDE_TVCRB_SEL_MOD_SHIFT 0
+#define MCDE_TVCRB_SEL_MOD_MASK 0x00000001
+#define MCDE_TVCRB_SEL_MOD_LCD 0
+#define MCDE_TVCRB_SEL_MOD_TV 1
+#define MCDE_TVCRB_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, SEL_MOD, MCDE_TVCRB_SEL_MOD_##__x)
+#define MCDE_TVCRB_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, SEL_MOD, __x)
+#define MCDE_TVCRB_INTEREN_SHIFT 1
+#define MCDE_TVCRB_INTEREN_MASK 0x00000002
+#define MCDE_TVCRB_INTEREN(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, INTEREN, __x)
+#define MCDE_TVCRB_IFIELD_SHIFT 2
+#define MCDE_TVCRB_IFIELD_MASK 0x00000004
+#define MCDE_TVCRB_IFIELD(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, IFIELD, __x)
+#define MCDE_TVCRB_TVMODE_SHIFT 3
+#define MCDE_TVCRB_TVMODE_MASK 0x00000038
+#define MCDE_TVCRB_TVMODE_SDTV_656P 0
+#define MCDE_TVCRB_TVMODE_HDTV_480P 1
+#define MCDE_TVCRB_TVMODE_HDTV_720P 2
+#define MCDE_TVCRB_TVMODE_SDTV_656P_LE 3
+#define MCDE_TVCRB_TVMODE_SDTV_656P_BE 4
+#define MCDE_TVCRB_TVMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, TVMODE, MCDE_TVCRB_TVMODE_##__x)
+#define MCDE_TVCRB_TVMODE(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, TVMODE, __x)
+#define MCDE_TVCRB_SDTVMODE_SHIFT 6
+#define MCDE_TVCRB_SDTVMODE_MASK 0x000000C0
+#define MCDE_TVCRB_SDTVMODE_Y0CBY1CR 0
+#define MCDE_TVCRB_SDTVMODE_CBY0CRY1 1
+#define MCDE_TVCRB_SDTVMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, SDTVMODE, MCDE_TVCRB_SDTVMODE_##__x)
+#define MCDE_TVCRB_SDTVMODE(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, SDTVMODE, __x)
+#define MCDE_TVCRB_AVRGEN_SHIFT 8
+#define MCDE_TVCRB_AVRGEN_MASK 0x00000100
+#define MCDE_TVCRB_AVRGEN(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, AVRGEN, __x)
+#define MCDE_TVCRB_CKINV_SHIFT 9
+#define MCDE_TVCRB_CKINV_MASK 0x00000200
+#define MCDE_TVCRB_CKINV(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, CKINV, __x)
+#define MCDE_TVBL1A 0x0000083C
+#define MCDE_TVBL1A_GROUPOFFSET 0x200
+#define MCDE_TVBL1A_BEL1_SHIFT 0
+#define MCDE_TVBL1A_BEL1_MASK 0x000007FF
+#define MCDE_TVBL1A_BEL1(__x) \
+	MCDE_VAL2REG(MCDE_TVBL1A, BEL1, __x)
+#define MCDE_TVBL1A_BSL1_SHIFT 16
+#define MCDE_TVBL1A_BSL1_MASK 0x07FF0000
+#define MCDE_TVBL1A_BSL1(__x) \
+	MCDE_VAL2REG(MCDE_TVBL1A, BSL1, __x)
+#define MCDE_TVBL1B 0x00000A3C
+#define MCDE_TVBL1B_BEL1_SHIFT 0
+#define MCDE_TVBL1B_BEL1_MASK 0x000007FF
+#define MCDE_TVBL1B_BEL1(__x) \
+	MCDE_VAL2REG(MCDE_TVBL1B, BEL1, __x)
+#define MCDE_TVBL1B_BSL1_SHIFT 16
+#define MCDE_TVBL1B_BSL1_MASK 0x07FF0000
+#define MCDE_TVBL1B_BSL1(__x) \
+	MCDE_VAL2REG(MCDE_TVBL1B, BSL1, __x)
+#define MCDE_TVISLA 0x00000840
+#define MCDE_TVISLA_GROUPOFFSET 0x200
+#define MCDE_TVISLA_FSL1_SHIFT 0
+#define MCDE_TVISLA_FSL1_MASK 0x000007FF
+#define MCDE_TVISLA_FSL1(__x) \
+	MCDE_VAL2REG(MCDE_TVISLA, FSL1, __x)
+#define MCDE_TVISLA_FSL2_SHIFT 16
+#define MCDE_TVISLA_FSL2_MASK 0x07FF0000
+#define MCDE_TVISLA_FSL2(__x) \
+	MCDE_VAL2REG(MCDE_TVISLA, FSL2, __x)
+#define MCDE_TVISLB 0x00000A40
+#define MCDE_TVISLB_FSL1_SHIFT 0
+#define MCDE_TVISLB_FSL1_MASK 0x000007FF
+#define MCDE_TVISLB_FSL1(__x) \
+	MCDE_VAL2REG(MCDE_TVISLB, FSL1, __x)
+#define MCDE_TVISLB_FSL2_SHIFT 16
+#define MCDE_TVISLB_FSL2_MASK 0x07FF0000
+#define MCDE_TVISLB_FSL2(__x) \
+	MCDE_VAL2REG(MCDE_TVISLB, FSL2, __x)
+#define MCDE_TVDVOA 0x00000844
+#define MCDE_TVDVOA_GROUPOFFSET 0x200
+#define MCDE_TVDVOA_DVO1_SHIFT 0
+#define MCDE_TVDVOA_DVO1_MASK 0x000007FF
+#define MCDE_TVDVOA_DVO1(__x) \
+	MCDE_VAL2REG(MCDE_TVDVOA, DVO1, __x)
+#define MCDE_TVDVOA_DVO2_SHIFT 16
+#define MCDE_TVDVOA_DVO2_MASK 0x07FF0000
+#define MCDE_TVDVOA_DVO2(__x) \
+	MCDE_VAL2REG(MCDE_TVDVOA, DVO2, __x)
+#define MCDE_TVDVOB 0x00000A44
+#define MCDE_TVDVOB_DVO1_SHIFT 0
+#define MCDE_TVDVOB_DVO1_MASK 0x000007FF
+#define MCDE_TVDVOB_DVO1(__x) \
+	MCDE_VAL2REG(MCDE_TVDVOB, DVO1, __x)
+#define MCDE_TVDVOB_DVO2_SHIFT 16
+#define MCDE_TVDVOB_DVO2_MASK 0x07FF0000
+#define MCDE_TVDVOB_DVO2(__x) \
+	MCDE_VAL2REG(MCDE_TVDVOB, DVO2, __x)
+#define MCDE_TVTIM1A 0x0000084C
+#define MCDE_TVTIM1A_GROUPOFFSET 0x200
+#define MCDE_TVTIM1A_DHO_SHIFT 0
+#define MCDE_TVTIM1A_DHO_MASK 0x000007FF
+#define MCDE_TVTIM1A_DHO(__x) \
+	MCDE_VAL2REG(MCDE_TVTIM1A, DHO, __x)
+#define MCDE_TVTIM1B 0x00000A4C
+#define MCDE_TVTIM1B_DHO_SHIFT 0
+#define MCDE_TVTIM1B_DHO_MASK 0x000007FF
+#define MCDE_TVTIM1B_DHO(__x) \
+	MCDE_VAL2REG(MCDE_TVTIM1B, DHO, __x)
+#define MCDE_TVLBALWA 0x00000850
+#define MCDE_TVLBALWA_GROUPOFFSET 0x200
+#define MCDE_TVLBALWA_LBW_SHIFT 0
+#define MCDE_TVLBALWA_LBW_MASK 0x000007FF
+#define MCDE_TVLBALWA_LBW(__x) \
+	MCDE_VAL2REG(MCDE_TVLBALWA, LBW, __x)
+#define MCDE_TVLBALWA_ALW_SHIFT 16
+#define MCDE_TVLBALWA_ALW_MASK 0x07FF0000
+#define MCDE_TVLBALWA_ALW(__x) \
+	MCDE_VAL2REG(MCDE_TVLBALWA, ALW, __x)
+#define MCDE_TVLBALWB 0x00000A50
+#define MCDE_TVLBALWB_LBW_SHIFT 0
+#define MCDE_TVLBALWB_LBW_MASK 0x000007FF
+#define MCDE_TVLBALWB_LBW(__x) \
+	MCDE_VAL2REG(MCDE_TVLBALWB, LBW, __x)
+#define MCDE_TVLBALWB_ALW_SHIFT 16
+#define MCDE_TVLBALWB_ALW_MASK 0x07FF0000
+#define MCDE_TVLBALWB_ALW(__x) \
+	MCDE_VAL2REG(MCDE_TVLBALWB, ALW, __x)
+#define MCDE_TVBL2A 0x00000854
+#define MCDE_TVBL2A_GROUPOFFSET 0x200
+#define MCDE_TVBL2A_BEL2_SHIFT 0
+#define MCDE_TVBL2A_BEL2_MASK 0x000007FF
+#define MCDE_TVBL2A_BEL2(__x) \
+	MCDE_VAL2REG(MCDE_TVBL2A, BEL2, __x)
+#define MCDE_TVBL2A_BSL2_SHIFT 16
+#define MCDE_TVBL2A_BSL2_MASK 0x07FF0000
+#define MCDE_TVBL2A_BSL2(__x) \
+	MCDE_VAL2REG(MCDE_TVBL2A, BSL2, __x)
+#define MCDE_TVBL2B 0x00000A54
+#define MCDE_TVBL2B_BEL2_SHIFT 0
+#define MCDE_TVBL2B_BEL2_MASK 0x000007FF
+#define MCDE_TVBL2B_BEL2(__x) \
+	MCDE_VAL2REG(MCDE_TVBL2B, BEL2, __x)
+#define MCDE_TVBL2B_BSL2_SHIFT 16
+#define MCDE_TVBL2B_BSL2_MASK 0x07FF0000
+#define MCDE_TVBL2B_BSL2(__x) \
+	MCDE_VAL2REG(MCDE_TVBL2B, BSL2, __x)
+#define MCDE_TVBLUA 0x00000858
+#define MCDE_TVBLUA_GROUPOFFSET 0x200
+#define MCDE_TVBLUA_TVBLU_SHIFT 0
+#define MCDE_TVBLUA_TVBLU_MASK 0x000000FF
+#define MCDE_TVBLUA_TVBLU(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUA, TVBLU, __x)
+#define MCDE_TVBLUA_TVBCB_SHIFT 8
+#define MCDE_TVBLUA_TVBCB_MASK 0x0000FF00
+#define MCDE_TVBLUA_TVBCB(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUA, TVBCB, __x)
+#define MCDE_TVBLUA_TVBCR_SHIFT 16
+#define MCDE_TVBLUA_TVBCR_MASK 0x00FF0000
+#define MCDE_TVBLUA_TVBCR(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUA, TVBCR, __x)
+#define MCDE_TVBLUB 0x00000A58
+#define MCDE_TVBLUB_TVBLU_SHIFT 0
+#define MCDE_TVBLUB_TVBLU_MASK 0x000000FF
+#define MCDE_TVBLUB_TVBLU(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUB, TVBLU, __x)
+#define MCDE_TVBLUB_TVBCB_SHIFT 8
+#define MCDE_TVBLUB_TVBCB_MASK 0x0000FF00
+#define MCDE_TVBLUB_TVBCB(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUB, TVBCB, __x)
+#define MCDE_TVBLUB_TVBCR_SHIFT 16
+#define MCDE_TVBLUB_TVBCR_MASK 0x00FF0000
+#define MCDE_TVBLUB_TVBCR(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUB, TVBCR, __x)
+#define MCDE_LCDTIM1A 0x00000860
+#define MCDE_LCDTIM1A_GROUPOFFSET 0x200
+#define MCDE_LCDTIM1A_IVP_SHIFT 19
+#define MCDE_LCDTIM1A_IVP_MASK 0x00080000
+#define MCDE_LCDTIM1A_IVP(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1A, IVP, __x)
+#define MCDE_LCDTIM1A_IVS_SHIFT 20
+#define MCDE_LCDTIM1A_IVS_MASK 0x00100000
+#define MCDE_LCDTIM1A_IVS(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1A, IVS, __x)
+#define MCDE_LCDTIM1A_IHS_SHIFT 21
+#define MCDE_LCDTIM1A_IHS_MASK 0x00200000
+#define MCDE_LCDTIM1A_IHS(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1A, IHS, __x)
+#define MCDE_LCDTIM1A_IPC_SHIFT 22
+#define MCDE_LCDTIM1A_IPC_MASK 0x00400000
+#define MCDE_LCDTIM1A_IPC(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1A, IPC, __x)
+#define MCDE_LCDTIM1A_IOE_SHIFT 23
+#define MCDE_LCDTIM1A_IOE_MASK 0x00800000
+#define MCDE_LCDTIM1A_IOE(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1A, IOE, __x)
+#define MCDE_LCDTIM1B 0x00000A60
+#define MCDE_LCDTIM1B_IVP_SHIFT 19
+#define MCDE_LCDTIM1B_IVP_MASK 0x00080000
+#define MCDE_LCDTIM1B_IVP(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1B, IVP, __x)
+#define MCDE_LCDTIM1B_IVS_SHIFT 20
+#define MCDE_LCDTIM1B_IVS_MASK 0x00100000
+#define MCDE_LCDTIM1B_IVS(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1B, IVS, __x)
+#define MCDE_LCDTIM1B_IHS_SHIFT 21
+#define MCDE_LCDTIM1B_IHS_MASK 0x00200000
+#define MCDE_LCDTIM1B_IHS(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1B, IHS, __x)
+#define MCDE_LCDTIM1B_IPC_SHIFT 22
+#define MCDE_LCDTIM1B_IPC_MASK 0x00400000
+#define MCDE_LCDTIM1B_IPC(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1B, IPC, __x)
+#define MCDE_LCDTIM1B_IOE_SHIFT 23
+#define MCDE_LCDTIM1B_IOE_MASK 0x00800000
+#define MCDE_LCDTIM1B_IOE(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1B, IOE, __x)
+#define MCDE_DITCTRLA 0x00000864
+#define MCDE_DITCTRLA_GROUPOFFSET 0x200
+#define MCDE_DITCTRLA_TEMP_SHIFT 0
+#define MCDE_DITCTRLA_TEMP_MASK 0x00000001
+#define MCDE_DITCTRLA_TEMP(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLA, TEMP, __x)
+#define MCDE_DITCTRLA_COMP_SHIFT 1
+#define MCDE_DITCTRLA_COMP_MASK 0x00000002
+#define MCDE_DITCTRLA_COMP(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLA, COMP, __x)
+#define MCDE_DITCTRLA_MASK_SHIFT 4
+#define MCDE_DITCTRLA_MASK_MASK 0x00000010
+#define MCDE_DITCTRLA_MASK(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLA, MASK, __x)
+#define MCDE_DITCTRLA_FOFFX_SHIFT 5
+#define MCDE_DITCTRLA_FOFFX_MASK 0x000003E0
+#define MCDE_DITCTRLA_FOFFX(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLA, FOFFX, __x)
+#define MCDE_DITCTRLA_FOFFY_SHIFT 10
+#define MCDE_DITCTRLA_FOFFY_MASK 0x00007C00
+#define MCDE_DITCTRLA_FOFFY(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLA, FOFFY, __x)
+#define MCDE_DITCTRLB 0x00000A64
+#define MCDE_DITCTRLB_TEMP_SHIFT 0
+#define MCDE_DITCTRLB_TEMP_MASK 0x00000001
+#define MCDE_DITCTRLB_TEMP(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLB, TEMP, __x)
+#define MCDE_DITCTRLB_COMP_SHIFT 1
+#define MCDE_DITCTRLB_COMP_MASK 0x00000002
+#define MCDE_DITCTRLB_COMP(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLB, COMP, __x)
+#define MCDE_DITCTRLB_MASK_SHIFT 4
+#define MCDE_DITCTRLB_MASK_MASK 0x00000010
+#define MCDE_DITCTRLB_MASK(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLB, MASK, __x)
+#define MCDE_DITCTRLB_FOFFX_SHIFT 5
+#define MCDE_DITCTRLB_FOFFX_MASK 0x000003E0
+#define MCDE_DITCTRLB_FOFFX(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLB, FOFFX, __x)
+#define MCDE_DITCTRLB_FOFFY_SHIFT 10
+#define MCDE_DITCTRLB_FOFFY_MASK 0x00007C00
+#define MCDE_DITCTRLB_FOFFY(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLB, FOFFY, __x)
+#define MCDE_DITOFFA 0x00000868
+#define MCDE_DITOFFA_GROUPOFFSET 0x200
+#define MCDE_DITOFFA_XG_SHIFT 0
+#define MCDE_DITOFFA_XG_MASK 0x0000001F
+#define MCDE_DITOFFA_XG(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFA, XG, __x)
+#define MCDE_DITOFFA_YG_SHIFT 8
+#define MCDE_DITOFFA_YG_MASK 0x00001F00
+#define MCDE_DITOFFA_YG(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFA, YG, __x)
+#define MCDE_DITOFFA_XB_SHIFT 16
+#define MCDE_DITOFFA_XB_MASK 0x001F0000
+#define MCDE_DITOFFA_XB(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFA, XB, __x)
+#define MCDE_DITOFFA_YB_SHIFT 24
+#define MCDE_DITOFFA_YB_MASK 0x1F000000
+#define MCDE_DITOFFA_YB(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFA, YB, __x)
+#define MCDE_DITOFFB 0x00000A68
+#define MCDE_DITOFFB_XG_SHIFT 0
+#define MCDE_DITOFFB_XG_MASK 0x0000001F
+#define MCDE_DITOFFB_XG(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFB, XG, __x)
+#define MCDE_DITOFFB_YG_SHIFT 8
+#define MCDE_DITOFFB_YG_MASK 0x00001F00
+#define MCDE_DITOFFB_YG(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFB, YG, __x)
+#define MCDE_DITOFFB_XB_SHIFT 16
+#define MCDE_DITOFFB_XB_MASK 0x001F0000
+#define MCDE_DITOFFB_XB(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFB, XB, __x)
+#define MCDE_DITOFFB_YB_SHIFT 24
+#define MCDE_DITOFFB_YB_MASK 0x1F000000
+#define MCDE_DITOFFB_YB(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFB, YB, __x)
+#define MCDE_PAL0A 0x0000086C
+#define MCDE_PAL0A_GROUPOFFSET 0x200
+#define MCDE_PAL0A_BLUE_SHIFT 0
+#define MCDE_PAL0A_BLUE_MASK 0x00000FFF
+#define MCDE_PAL0A_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_PAL0A, BLUE, __x)
+#define MCDE_PAL0A_GREEN_SHIFT 16
+#define MCDE_PAL0A_GREEN_MASK 0x0FFF0000
+#define MCDE_PAL0A_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_PAL0A, GREEN, __x)
+#define MCDE_PAL0B 0x00000A6C
+#define MCDE_PAL0B_BLUE_SHIFT 0
+#define MCDE_PAL0B_BLUE_MASK 0x00000FFF
+#define MCDE_PAL0B_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_PAL0B, BLUE, __x)
+#define MCDE_PAL0B_GREEN_SHIFT 16
+#define MCDE_PAL0B_GREEN_MASK 0x0FFF0000
+#define MCDE_PAL0B_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_PAL0B, GREEN, __x)
+#define MCDE_PAL1A 0x00000870
+#define MCDE_PAL1A_GROUPOFFSET 0x200
+#define MCDE_PAL1A_RED_SHIFT 0
+#define MCDE_PAL1A_RED_MASK 0x00000FFF
+#define MCDE_PAL1A_RED(__x) \
+	MCDE_VAL2REG(MCDE_PAL1A, RED, __x)
+#define MCDE_PAL1B 0x00000A70
+#define MCDE_PAL1B_RED_SHIFT 0
+#define MCDE_PAL1B_RED_MASK 0x00000FFF
+#define MCDE_PAL1B_RED(__x) \
+	MCDE_VAL2REG(MCDE_PAL1B, RED, __x)
+#define MCDE_ROTADD0A 0x00000874
+#define MCDE_ROTADD0A_GROUPOFFSET 0x200
+#define MCDE_ROTADD0A_ROTADD0_SHIFT 3
+#define MCDE_ROTADD0A_ROTADD0_MASK 0xFFFFFFF8
+#define MCDE_ROTADD0A_ROTADD0(__x) \
+	MCDE_VAL2REG(MCDE_ROTADD0A, ROTADD0, __x)
+#define MCDE_ROTADD0B 0x00000A74
+#define MCDE_ROTADD0B_ROTADD0_SHIFT 3
+#define MCDE_ROTADD0B_ROTADD0_MASK 0xFFFFFFF8
+#define MCDE_ROTADD0B_ROTADD0(__x) \
+	MCDE_VAL2REG(MCDE_ROTADD0B, ROTADD0, __x)
+#define MCDE_ROTADD1A 0x00000878
+#define MCDE_ROTADD1A_GROUPOFFSET 0x200
+#define MCDE_ROTADD1A_ROTADD1_SHIFT 3
+#define MCDE_ROTADD1A_ROTADD1_MASK 0xFFFFFFF8
+#define MCDE_ROTADD1A_ROTADD1(__x) \
+	MCDE_VAL2REG(MCDE_ROTADD1A, ROTADD1, __x)
+#define MCDE_ROTADD1B 0x00000A78
+#define MCDE_ROTADD1B_ROTADD1_SHIFT 3
+#define MCDE_ROTADD1B_ROTADD1_MASK 0xFFFFFFF8
+#define MCDE_ROTADD1B_ROTADD1(__x) \
+	MCDE_VAL2REG(MCDE_ROTADD1B, ROTADD1, __x)
+#define MCDE_ROTACONF 0x0000087C
+#define MCDE_ROTACONF_GROUPOFFSET 0x200
+#define MCDE_ROTACONF_ROTBURSTSIZE_SHIFT 0
+#define MCDE_ROTACONF_ROTBURSTSIZE_MASK 0x00000003
+#define MCDE_ROTACONF_ROTBURSTSIZE_1W 0
+#define MCDE_ROTACONF_ROTBURSTSIZE_2W 1
+#define MCDE_ROTACONF_ROTBURSTSIZE_4W 2
+#define MCDE_ROTACONF_ROTBURSTSIZE_8W 3
+#define MCDE_ROTACONF_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, ROTBURSTSIZE, \
+	MCDE_ROTACONF_ROTBURSTSIZE_##__x)
+#define MCDE_ROTACONF_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, ROTBURSTSIZE, __x)
+#define MCDE_ROTACONF_ROTBURSTSIZE_HW_SHIFT 2
+#define MCDE_ROTACONF_ROTBURSTSIZE_HW_MASK 0x00000004
+#define MCDE_ROTACONF_ROTBURSTSIZE_HW(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, ROTBURSTSIZE_HW, __x)
+#define MCDE_ROTACONF_ROTDIR_SHIFT 3
+#define MCDE_ROTACONF_ROTDIR_MASK 0x00000008
+#define MCDE_ROTACONF_ROTDIR_CCW 0
+#define MCDE_ROTACONF_ROTDIR_CW 1
+#define MCDE_ROTACONF_ROTDIR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, ROTDIR, MCDE_ROTACONF_ROTDIR_##__x)
+#define MCDE_ROTACONF_ROTDIR(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, ROTDIR, __x)
+#define MCDE_ROTACONF_WR_MAXOUT_SHIFT 4
+#define MCDE_ROTACONF_WR_MAXOUT_MASK 0x00000030
+#define MCDE_ROTACONF_WR_MAXOUT_1_REQ 0
+#define MCDE_ROTACONF_WR_MAXOUT_2_REQ 1
+#define MCDE_ROTACONF_WR_MAXOUT_4_REQ 2
+#define MCDE_ROTACONF_WR_MAXOUT_8_REQ 3
+#define MCDE_ROTACONF_WR_MAXOUT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, WR_MAXOUT, MCDE_ROTACONF_WR_MAXOUT_##__x)
+#define MCDE_ROTACONF_WR_MAXOUT(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, WR_MAXOUT, __x)
+#define MCDE_ROTACONF_RD_MAXOUT_SHIFT 6
+#define MCDE_ROTACONF_RD_MAXOUT_MASK 0x000000C0
+#define MCDE_ROTACONF_RD_MAXOUT_1_REQ 0
+#define MCDE_ROTACONF_RD_MAXOUT_2_REQ 1
+#define MCDE_ROTACONF_RD_MAXOUT_4_REQ 2
+#define MCDE_ROTACONF_RD_MAXOUT_8_REQ 3
+#define MCDE_ROTACONF_RD_MAXOUT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, RD_MAXOUT, MCDE_ROTACONF_RD_MAXOUT_##__x)
+#define MCDE_ROTACONF_RD_MAXOUT(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, RD_MAXOUT, __x)
+#define MCDE_ROTACONF_STRIP_WIDTH_SHIFT 8
+#define MCDE_ROTACONF_STRIP_WIDTH_MASK 0x00007F00
+#define MCDE_ROTACONF_STRIP_WIDTH_2PIX 0
+#define MCDE_ROTACONF_STRIP_WIDTH_4PIX 1
+#define MCDE_ROTACONF_STRIP_WIDTH_8PIX 2
+#define MCDE_ROTACONF_STRIP_WIDTH_16PIX 3
+#define MCDE_ROTACONF_STRIP_WIDTH_32PIX 4
+#define MCDE_ROTACONF_STRIP_WIDTH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, STRIP_WIDTH, \
+	MCDE_ROTACONF_STRIP_WIDTH_##__x)
+#define MCDE_ROTACONF_STRIP_WIDTH(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, STRIP_WIDTH, __x)
+#define MCDE_ROTACONF_SINGLE_BUF_SHIFT 15
+#define MCDE_ROTACONF_SINGLE_BUF_MASK 0x00008000
+#define MCDE_ROTACONF_SINGLE_BUF(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, SINGLE_BUF, __x)
+#define MCDE_ROTACONF_WR_ROPC_SHIFT 16
+#define MCDE_ROTACONF_WR_ROPC_MASK 0x00FF0000
+#define MCDE_ROTACONF_WR_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, WR_ROPC, __x)
+#define MCDE_ROTACONF_RD_ROPC_SHIFT 24
+#define MCDE_ROTACONF_RD_ROPC_MASK 0xFF000000
+#define MCDE_ROTACONF_RD_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, RD_ROPC, __x)
+#define MCDE_ROTBCONF 0x00000A7C
+#define MCDE_ROTBCONF_ROTBURSTSIZE_SHIFT 0
+#define MCDE_ROTBCONF_ROTBURSTSIZE_MASK 0x00000003
+#define MCDE_ROTBCONF_ROTBURSTSIZE_1W 0
+#define MCDE_ROTBCONF_ROTBURSTSIZE_2W 1
+#define MCDE_ROTBCONF_ROTBURSTSIZE_4W 2
+#define MCDE_ROTBCONF_ROTBURSTSIZE_8W 3
+#define MCDE_ROTBCONF_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, ROTBURSTSIZE, \
+	MCDE_ROTBCONF_ROTBURSTSIZE_##__x)
+#define MCDE_ROTBCONF_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, ROTBURSTSIZE, __x)
+#define MCDE_ROTBCONF_ROTBURSTSIZE_HW_SHIFT 2
+#define MCDE_ROTBCONF_ROTBURSTSIZE_HW_MASK 0x00000004
+#define MCDE_ROTBCONF_ROTBURSTSIZE_HW(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, ROTBURSTSIZE_HW, __x)
+#define MCDE_ROTBCONF_ROTDIR_SHIFT 3
+#define MCDE_ROTBCONF_ROTDIR_MASK 0x00000008
+#define MCDE_ROTBCONF_ROTDIR_CCW 0
+#define MCDE_ROTBCONF_ROTDIR_CW 1
+#define MCDE_ROTBCONF_ROTDIR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, ROTDIR, MCDE_ROTBCONF_ROTDIR_##__x)
+#define MCDE_ROTBCONF_ROTDIR(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, ROTDIR, __x)
+#define MCDE_ROTBCONF_WR_MAXOUT_SHIFT 4
+#define MCDE_ROTBCONF_WR_MAXOUT_MASK 0x00000030
+#define MCDE_ROTBCONF_WR_MAXOUT_1_REQ 0
+#define MCDE_ROTBCONF_WR_MAXOUT_2_REQ 1
+#define MCDE_ROTBCONF_WR_MAXOUT_4_REQ 2
+#define MCDE_ROTBCONF_WR_MAXOUT_8_REQ 3
+#define MCDE_ROTBCONF_WR_MAXOUT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, WR_MAXOUT, MCDE_ROTBCONF_WR_MAXOUT_##__x)
+#define MCDE_ROTBCONF_WR_MAXOUT(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, WR_MAXOUT, __x)
+#define MCDE_ROTBCONF_RD_MAXOUT_SHIFT 6
+#define MCDE_ROTBCONF_RD_MAXOUT_MASK 0x000000C0
+#define MCDE_ROTBCONF_RD_MAXOUT_1_REQ 0
+#define MCDE_ROTBCONF_RD_MAXOUT_2_REQ 1
+#define MCDE_ROTBCONF_RD_MAXOUT_4_REQ 2
+#define MCDE_ROTBCONF_RD_MAXOUT_8_REQ 3
+#define MCDE_ROTBCONF_RD_MAXOUT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, RD_MAXOUT, MCDE_ROTBCONF_RD_MAXOUT_##__x)
+#define MCDE_ROTBCONF_RD_MAXOUT(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, RD_MAXOUT, __x)
+#define MCDE_ROTBCONF_STRIP_WIDTH_SHIFT 8
+#define MCDE_ROTBCONF_STRIP_WIDTH_MASK 0x00007F00
+#define MCDE_ROTBCONF_STRIP_WIDTH_2PIX 0
+#define MCDE_ROTBCONF_STRIP_WIDTH_4PIX 1
+#define MCDE_ROTBCONF_STRIP_WIDTH_8PIX 2
+#define MCDE_ROTBCONF_STRIP_WIDTH_16PIX 3
+#define MCDE_ROTBCONF_STRIP_WIDTH_32PIX 4
+#define MCDE_ROTBCONF_STRIP_WIDTH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, STRIP_WIDTH, \
+	MCDE_ROTBCONF_STRIP_WIDTH_##__x)
+#define MCDE_ROTBCONF_STRIP_WIDTH(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, STRIP_WIDTH, __x)
+#define MCDE_ROTBCONF_SINGLE_BUF_SHIFT 15
+#define MCDE_ROTBCONF_SINGLE_BUF_MASK 0x00008000
+#define MCDE_ROTBCONF_SINGLE_BUF(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, SINGLE_BUF, __x)
+#define MCDE_ROTBCONF_WR_ROPC_SHIFT 16
+#define MCDE_ROTBCONF_WR_ROPC_MASK 0x00FF0000
+#define MCDE_ROTBCONF_WR_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, WR_ROPC, __x)
+#define MCDE_ROTBCONF_RD_ROPC_SHIFT 24
+#define MCDE_ROTBCONF_RD_ROPC_MASK 0xFF000000
+#define MCDE_ROTBCONF_RD_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, RD_ROPC, __x)
+#define MCDE_SYNCHCONFA 0x00000880
+#define MCDE_SYNCHCONFA_GROUPOFFSET 0x200
+#define MCDE_SYNCHCONFA_HWREQVEVENT_SHIFT 0
+#define MCDE_SYNCHCONFA_HWREQVEVENT_MASK 0x00000003
+#define MCDE_SYNCHCONFA_HWREQVEVENT_VSYNC 0
+#define MCDE_SYNCHCONFA_HWREQVEVENT_BACK_PORCH 1
+#define MCDE_SYNCHCONFA_HWREQVEVENT_ACTIVE_VIDEO 2
+#define MCDE_SYNCHCONFA_HWREQVEVENT_FRONT_PORCH 3
+#define MCDE_SYNCHCONFA_HWREQVEVENT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, HWREQVEVENT, \
+	MCDE_SYNCHCONFA_HWREQVEVENT_##__x)
+#define MCDE_SYNCHCONFA_HWREQVEVENT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, HWREQVEVENT, __x)
+#define MCDE_SYNCHCONFA_HWREQVCNT_SHIFT 2
+#define MCDE_SYNCHCONFA_HWREQVCNT_MASK 0x0000FFFC
+#define MCDE_SYNCHCONFA_HWREQVCNT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, HWREQVCNT, __x)
+#define MCDE_SYNCHCONFA_SWINTVEVENT_SHIFT 16
+#define MCDE_SYNCHCONFA_SWINTVEVENT_MASK 0x00030000
+#define MCDE_SYNCHCONFA_SWINTVEVENT_VSYNC 0
+#define MCDE_SYNCHCONFA_SWINTVEVENT_BACK_PORCH 1
+#define MCDE_SYNCHCONFA_SWINTVEVENT_ACTIVE_VIDEO 2
+#define MCDE_SYNCHCONFA_SWINTVEVENT_FRONT_PORCH 3
+#define MCDE_SYNCHCONFA_SWINTVEVENT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, SWINTVEVENT, \
+	MCDE_SYNCHCONFA_SWINTVEVENT_##__x)
+#define MCDE_SYNCHCONFA_SWINTVEVENT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, SWINTVEVENT, __x)
+#define MCDE_SYNCHCONFA_SWINTVCNT_SHIFT 18
+#define MCDE_SYNCHCONFA_SWINTVCNT_MASK 0xFFFC0000
+#define MCDE_SYNCHCONFA_SWINTVCNT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, SWINTVCNT, __x)
+#define MCDE_SYNCHCONFB 0x00000A80
+#define MCDE_SYNCHCONFB_HWREQVEVENT_SHIFT 0
+#define MCDE_SYNCHCONFB_HWREQVEVENT_MASK 0x00000003
+#define MCDE_SYNCHCONFB_HWREQVEVENT_VSYNC 0
+#define MCDE_SYNCHCONFB_HWREQVEVENT_BACK_PORCH 1
+#define MCDE_SYNCHCONFB_HWREQVEVENT_ACTIVE_VIDEO 2
+#define MCDE_SYNCHCONFB_HWREQVEVENT_FRONT_PORCH 3
+#define MCDE_SYNCHCONFB_HWREQVEVENT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, HWREQVEVENT, \
+	MCDE_SYNCHCONFB_HWREQVEVENT_##__x)
+#define MCDE_SYNCHCONFB_HWREQVEVENT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, HWREQVEVENT, __x)
+#define MCDE_SYNCHCONFB_HWREQVCNT_SHIFT 2
+#define MCDE_SYNCHCONFB_HWREQVCNT_MASK 0x0000FFFC
+#define MCDE_SYNCHCONFB_HWREQVCNT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, HWREQVCNT, __x)
+#define MCDE_SYNCHCONFB_SWINTVEVENT_SHIFT 16
+#define MCDE_SYNCHCONFB_SWINTVEVENT_MASK 0x00030000
+#define MCDE_SYNCHCONFB_SWINTVEVENT_VSYNC 0
+#define MCDE_SYNCHCONFB_SWINTVEVENT_BACK_PORCH 1
+#define MCDE_SYNCHCONFB_SWINTVEVENT_ACTIVE_VIDEO 2
+#define MCDE_SYNCHCONFB_SWINTVEVENT_FRONT_PORCH 3
+#define MCDE_SYNCHCONFB_SWINTVEVENT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, SWINTVEVENT, \
+	MCDE_SYNCHCONFB_SWINTVEVENT_##__x)
+#define MCDE_SYNCHCONFB_SWINTVEVENT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, SWINTVEVENT, __x)
+#define MCDE_SYNCHCONFB_SWINTVCNT_SHIFT 18
+#define MCDE_SYNCHCONFB_SWINTVCNT_MASK 0xFFFC0000
+#define MCDE_SYNCHCONFB_SWINTVCNT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, SWINTVCNT, __x)
+#define MCDE_CTRLA 0x00000884
+#define MCDE_CTRLA_GROUPOFFSET 0x200
+#define MCDE_CTRLA_FIFOWTRMRK_SHIFT 0
+#define MCDE_CTRLA_FIFOWTRMRK_MASK 0x000003FF
+#define MCDE_CTRLA_FIFOWTRMRK(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FIFOWTRMRK, __x)
+#define MCDE_CTRLA_FIFOEMPTY_SHIFT 12
+#define MCDE_CTRLA_FIFOEMPTY_MASK 0x00001000
+#define MCDE_CTRLA_FIFOEMPTY(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FIFOEMPTY, __x)
+#define MCDE_CTRLA_FIFOFULL_SHIFT 13
+#define MCDE_CTRLA_FIFOFULL_MASK 0x00002000
+#define MCDE_CTRLA_FIFOFULL(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FIFOFULL, __x)
+#define MCDE_CTRLA_FORMID_SHIFT 16
+#define MCDE_CTRLA_FORMID_MASK 0x00070000
+#define MCDE_CTRLA_FORMID_DSI0VID 0
+#define MCDE_CTRLA_FORMID_DSI0CMD 1
+#define MCDE_CTRLA_FORMID_DSI1VID 2
+#define MCDE_CTRLA_FORMID_DSI1CMD 3
+#define MCDE_CTRLA_FORMID_DSI2VID 4
+#define MCDE_CTRLA_FORMID_DSI2CMD 5
+#define MCDE_CTRLA_FORMID_DPIA 0
+#define MCDE_CTRLA_FORMID_DPIB 1
+#define MCDE_CTRLA_FORMID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FORMID, MCDE_CTRLA_FORMID_##__x)
+#define MCDE_CTRLA_FORMID(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FORMID, __x)
+#define MCDE_CTRLA_FORMTYPE_SHIFT 20
+#define MCDE_CTRLA_FORMTYPE_MASK 0x00700000
+#define MCDE_CTRLA_FORMTYPE_DPITV 0
+#define MCDE_CTRLA_FORMTYPE_DBI 1
+#define MCDE_CTRLA_FORMTYPE_DSI 2
+#define MCDE_CTRLA_FORMTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FORMTYPE, MCDE_CTRLA_FORMTYPE_##__x)
+#define MCDE_CTRLA_FORMTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FORMTYPE, __x)
+#define MCDE_CTRLB 0x00000A84
+#define MCDE_CTRLB_FIFOWTRMRK_SHIFT 0
+#define MCDE_CTRLB_FIFOWTRMRK_MASK 0x000003FF
+#define MCDE_CTRLB_FIFOWTRMRK(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FIFOWTRMRK, __x)
+#define MCDE_CTRLB_FIFOEMPTY_SHIFT 12
+#define MCDE_CTRLB_FIFOEMPTY_MASK 0x00001000
+#define MCDE_CTRLB_FIFOEMPTY(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FIFOEMPTY, __x)
+#define MCDE_CTRLB_FIFOFULL_SHIFT 13
+#define MCDE_CTRLB_FIFOFULL_MASK 0x00002000
+#define MCDE_CTRLB_FIFOFULL(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FIFOFULL, __x)
+#define MCDE_CTRLB_FORMID_SHIFT 16
+#define MCDE_CTRLB_FORMID_MASK 0x00070000
+#define MCDE_CTRLB_FORMID_DSI0VID 0
+#define MCDE_CTRLB_FORMID_DSI0CMD 1
+#define MCDE_CTRLB_FORMID_DSI1VID 2
+#define MCDE_CTRLB_FORMID_DSI1CMD 3
+#define MCDE_CTRLB_FORMID_DSI2VID 4
+#define MCDE_CTRLB_FORMID_DSI2CMD 5
+#define MCDE_CTRLB_FORMID_DPIA 0
+#define MCDE_CTRLB_FORMID_DPIB 1
+#define MCDE_CTRLB_FORMID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FORMID, MCDE_CTRLB_FORMID_##__x)
+#define MCDE_CTRLB_FORMID(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FORMID, __x)
+#define MCDE_CTRLB_FORMTYPE_SHIFT 20
+#define MCDE_CTRLB_FORMTYPE_MASK 0x00700000
+#define MCDE_CTRLB_FORMTYPE_DPITV 0
+#define MCDE_CTRLB_FORMTYPE_DBI 1
+#define MCDE_CTRLB_FORMTYPE_DSI 2
+#define MCDE_CTRLB_FORMTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FORMTYPE, MCDE_CTRLB_FORMTYPE_##__x)
+#define MCDE_CTRLB_FORMTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FORMTYPE, __x)
+#define MCDE_GAM0A 0x00000888
+#define MCDE_GAM0A_GROUPOFFSET 0x200
+#define MCDE_GAM0A_BLUE_SHIFT 0
+#define MCDE_GAM0A_BLUE_MASK 0x00FFFFFF
+#define MCDE_GAM0A_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_GAM0A, BLUE, __x)
+#define MCDE_GAM0B 0x00000A88
+#define MCDE_GAM0B_BLUE_SHIFT 0
+#define MCDE_GAM0B_BLUE_MASK 0x00FFFFFF
+#define MCDE_GAM0B_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_GAM0B, BLUE, __x)
+#define MCDE_GAM1A 0x0000088C
+#define MCDE_GAM1A_GROUPOFFSET 0x200
+#define MCDE_GAM1A_GREEN_SHIFT 0
+#define MCDE_GAM1A_GREEN_MASK 0x00FFFFFF
+#define MCDE_GAM1A_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_GAM1A, GREEN, __x)
+#define MCDE_GAM1B 0x00000A8C
+#define MCDE_GAM1B_GREEN_SHIFT 0
+#define MCDE_GAM1B_GREEN_MASK 0x00FFFFFF
+#define MCDE_GAM1B_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_GAM1B, GREEN, __x)
+#define MCDE_GAM2A 0x00000890
+#define MCDE_GAM2A_GROUPOFFSET 0x200
+#define MCDE_GAM2A_RED_SHIFT 0
+#define MCDE_GAM2A_RED_MASK 0x00FFFFFF
+#define MCDE_GAM2A_RED(__x) \
+	MCDE_VAL2REG(MCDE_GAM2A, RED, __x)
+#define MCDE_GAM2B 0x00000A90
+#define MCDE_GAM2B_RED_SHIFT 0
+#define MCDE_GAM2B_RED_MASK 0x00FFFFFF
+#define MCDE_GAM2B_RED(__x) \
+	MCDE_VAL2REG(MCDE_GAM2B, RED, __x)
+#define MCDE_OLEDCONV1A 0x00000894
+#define MCDE_OLEDCONV1A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV1A_ALPHA_RED_SHIFT 0
+#define MCDE_OLEDCONV1A_ALPHA_RED_MASK 0x00003FFF
+#define MCDE_OLEDCONV1A_ALPHA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV1A, ALPHA_RED, __x)
+#define MCDE_OLEDCONV1A_ALPHA_GREEN_SHIFT 16
+#define MCDE_OLEDCONV1A_ALPHA_GREEN_MASK 0x3FFF0000
+#define MCDE_OLEDCONV1A_ALPHA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV1A, ALPHA_GREEN, __x)
+#define MCDE_OLEDCONV1B 0x00000A94
+#define MCDE_OLEDCONV1B_ALPHA_RED_SHIFT 0
+#define MCDE_OLEDCONV1B_ALPHA_RED_MASK 0x00003FFF
+#define MCDE_OLEDCONV1B_ALPHA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV1B, ALPHA_RED, __x)
+#define MCDE_OLEDCONV1B_ALPHA_GREEN_SHIFT 16
+#define MCDE_OLEDCONV1B_ALPHA_GREEN_MASK 0x3FFF0000
+#define MCDE_OLEDCONV1B_ALPHA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV1B, ALPHA_GREEN, __x)
+#define MCDE_OLEDCONV2A 0x00000898
+#define MCDE_OLEDCONV2A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV2A_ALPHA_BLUE_SHIFT 0
+#define MCDE_OLEDCONV2A_ALPHA_BLUE_MASK 0x00003FFF
+#define MCDE_OLEDCONV2A_ALPHA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV2A, ALPHA_BLUE, __x)
+#define MCDE_OLEDCONV2A_BETA_RED_SHIFT 16
+#define MCDE_OLEDCONV2A_BETA_RED_MASK 0x3FFF0000
+#define MCDE_OLEDCONV2A_BETA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV2A, BETA_RED, __x)
+#define MCDE_OLEDCONV2B 0x00000A98
+#define MCDE_OLEDCONV2B_ALPHA_BLUE_SHIFT 0
+#define MCDE_OLEDCONV2B_ALPHA_BLUE_MASK 0x00003FFF
+#define MCDE_OLEDCONV2B_ALPHA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV2B, ALPHA_BLUE, __x)
+#define MCDE_OLEDCONV2B_BETA_RED_SHIFT 16
+#define MCDE_OLEDCONV2B_BETA_RED_MASK 0x3FFF0000
+#define MCDE_OLEDCONV2B_BETA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV2B, BETA_RED, __x)
+#define MCDE_OLEDCONV3A 0x0000089C
+#define MCDE_OLEDCONV3A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV3A_BETA_GREEN_SHIFT 0
+#define MCDE_OLEDCONV3A_BETA_GREEN_MASK 0x00003FFF
+#define MCDE_OLEDCONV3A_BETA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV3A, BETA_GREEN, __x)
+#define MCDE_OLEDCONV3A_BETA_BLUE_SHIFT 16
+#define MCDE_OLEDCONV3A_BETA_BLUE_MASK 0x3FFF0000
+#define MCDE_OLEDCONV3A_BETA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV3A, BETA_BLUE, __x)
+#define MCDE_OLEDCONV3B 0x00000A9C
+#define MCDE_OLEDCONV3B_BETA_GREEN_SHIFT 0
+#define MCDE_OLEDCONV3B_BETA_GREEN_MASK 0x00003FFF
+#define MCDE_OLEDCONV3B_BETA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV3B, BETA_GREEN, __x)
+#define MCDE_OLEDCONV3B_BETA_BLUE_SHIFT 16
+#define MCDE_OLEDCONV3B_BETA_BLUE_MASK 0x3FFF0000
+#define MCDE_OLEDCONV3B_BETA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV3B, BETA_BLUE, __x)
+#define MCDE_OLEDCONV4A 0x000008A0
+#define MCDE_OLEDCONV4A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV4A_GAMMA_RED_SHIFT 0
+#define MCDE_OLEDCONV4A_GAMMA_RED_MASK 0x00003FFF
+#define MCDE_OLEDCONV4A_GAMMA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV4A, GAMMA_RED, __x)
+#define MCDE_OLEDCONV4A_GAMMA_GREEN_SHIFT 16
+#define MCDE_OLEDCONV4A_GAMMA_GREEN_MASK 0x3FFF0000
+#define MCDE_OLEDCONV4A_GAMMA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV4A, GAMMA_GREEN, __x)
+#define MCDE_OLEDCONV4B 0x00000AA0
+#define MCDE_OLEDCONV4B_GAMMA_RED_SHIFT 0
+#define MCDE_OLEDCONV4B_GAMMA_RED_MASK 0x00003FFF
+#define MCDE_OLEDCONV4B_GAMMA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV4B, GAMMA_RED, __x)
+#define MCDE_OLEDCONV4B_GAMMA_GREEN_SHIFT 16
+#define MCDE_OLEDCONV4B_GAMMA_GREEN_MASK 0x3FFF0000
+#define MCDE_OLEDCONV4B_GAMMA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV4B, GAMMA_GREEN, __x)
+#define MCDE_OLEDCONV5A 0x000008A4
+#define MCDE_OLEDCONV5A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV5A_GAMMA_BLUE_SHIFT 0
+#define MCDE_OLEDCONV5A_GAMMA_BLUE_MASK 0x00003FFF
+#define MCDE_OLEDCONV5A_GAMMA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV5A, GAMMA_BLUE, __x)
+#define MCDE_OLEDCONV5A_OFF_RED_SHIFT 16
+#define MCDE_OLEDCONV5A_OFF_RED_MASK 0x3FFF0000
+#define MCDE_OLEDCONV5A_OFF_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV5A, OFF_RED, __x)
+#define MCDE_OLEDCONV5B 0x00000AA4
+#define MCDE_OLEDCONV5B_GAMMA_BLUE_SHIFT 0
+#define MCDE_OLEDCONV5B_GAMMA_BLUE_MASK 0x00003FFF
+#define MCDE_OLEDCONV5B_GAMMA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV5B, GAMMA_BLUE, __x)
+#define MCDE_OLEDCONV5B_OFF_RED_SHIFT 16
+#define MCDE_OLEDCONV5B_OFF_RED_MASK 0x3FFF0000
+#define MCDE_OLEDCONV5B_OFF_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV5B, OFF_RED, __x)
+#define MCDE_OLEDCONV6A 0x000008A8
+#define MCDE_OLEDCONV6A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV6A_OFF_GREEN_SHIFT 0
+#define MCDE_OLEDCONV6A_OFF_GREEN_MASK 0x00003FFF
+#define MCDE_OLEDCONV6A_OFF_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV6A, OFF_GREEN, __x)
+#define MCDE_OLEDCONV6A_OFF_BLUE_SHIFT 16
+#define MCDE_OLEDCONV6A_OFF_BLUE_MASK 0x3FFF0000
+#define MCDE_OLEDCONV6A_OFF_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV6A, OFF_BLUE, __x)
+#define MCDE_OLEDCONV6B 0x00000AA8
+#define MCDE_OLEDCONV6B_OFF_GREEN_SHIFT 0
+#define MCDE_OLEDCONV6B_OFF_GREEN_MASK 0x00003FFF
+#define MCDE_OLEDCONV6B_OFF_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV6B, OFF_GREEN, __x)
+#define MCDE_OLEDCONV6B_OFF_BLUE_SHIFT 16
+#define MCDE_OLEDCONV6B_OFF_BLUE_MASK 0x3FFF0000
+#define MCDE_OLEDCONV6B_OFF_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV6B, OFF_BLUE, __x)
+#define MCDE_CRC 0x00000C00
+#define MCDE_CRC_FLOEN_SHIFT 0
+#define MCDE_CRC_FLOEN_MASK 0x00000001
+#define MCDE_CRC_FLOEN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, FLOEN, __x)
+#define MCDE_CRC_POWEREN_SHIFT 1
+#define MCDE_CRC_POWEREN_MASK 0x00000002
+#define MCDE_CRC_POWEREN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, POWEREN, __x)
+#define MCDE_CRC_C1EN_SHIFT 2
+#define MCDE_CRC_C1EN_MASK 0x00000004
+#define MCDE_CRC_C1EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, C1EN, __x)
+#define MCDE_CRC_C2EN_SHIFT 3
+#define MCDE_CRC_C2EN_MASK 0x00000008
+#define MCDE_CRC_C2EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, C2EN, __x)
+#define MCDE_CRC_WMLVL1_SHIFT 4
+#define MCDE_CRC_WMLVL1_MASK 0x00000010
+#define MCDE_CRC_WMLVL1(__x) \
+	MCDE_VAL2REG(MCDE_CRC, WMLVL1, __x)
+#define MCDE_CRC_WMLVL2_SHIFT 5
+#define MCDE_CRC_WMLVL2_MASK 0x00000020
+#define MCDE_CRC_WMLVL2(__x) \
+	MCDE_VAL2REG(MCDE_CRC, WMLVL2, __x)
+#define MCDE_CRC_SYNCSEL_SHIFT 6
+#define MCDE_CRC_SYNCSEL_MASK 0x00000040
+#define MCDE_CRC_SYNCSEL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SYNCSEL, __x)
+#define MCDE_CRC_SYCEN0_SHIFT 7
+#define MCDE_CRC_SYCEN0_MASK 0x00000080
+#define MCDE_CRC_SYCEN0(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SYCEN0, __x)
+#define MCDE_CRC_SYCEN1_SHIFT 8
+#define MCDE_CRC_SYCEN1_MASK 0x00000100
+#define MCDE_CRC_SYCEN1(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SYCEN1, __x)
+#define MCDE_CRC_SIZE1_SHIFT 9
+#define MCDE_CRC_SIZE1_MASK 0x00000200
+#define MCDE_CRC_SIZE1(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SIZE1, __x)
+#define MCDE_CRC_SIZE2_SHIFT 10
+#define MCDE_CRC_SIZE2_MASK 0x00000400
+#define MCDE_CRC_SIZE2(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SIZE2, __x)
+#define MCDE_CRC_INBAND1_SHIFT 11
+#define MCDE_CRC_INBAND1_MASK 0x00000800
+#define MCDE_CRC_INBAND1(__x) \
+	MCDE_VAL2REG(MCDE_CRC, INBAND1, __x)
+#define MCDE_CRC_INBAND2_SHIFT 12
+#define MCDE_CRC_INBAND2_MASK 0x00001000
+#define MCDE_CRC_INBAND2(__x) \
+	MCDE_VAL2REG(MCDE_CRC, INBAND2, __x)
+#define MCDE_CRC_CLKSEL_SHIFT 13
+#define MCDE_CRC_CLKSEL_MASK 0x00006000
+#define MCDE_CRC_CLKSEL_166MHz 0
+#define MCDE_CRC_CLKSEL_48MHz 1
+#define MCDE_CRC_CLKSEL_LCD 2
+#define MCDE_CRC_CLKSEL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CLKSEL, MCDE_CRC_CLKSEL_##__x)
+#define MCDE_CRC_CLKSEL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CLKSEL, __x)
+#define MCDE_CRC_YUVCONVC1EN_SHIFT 15
+#define MCDE_CRC_YUVCONVC1EN_MASK 0x00008000
+#define MCDE_CRC_YUVCONVC1EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, YUVCONVC1EN, __x)
+#define MCDE_CRC_CS1EN_SHIFT 16
+#define MCDE_CRC_CS1EN_MASK 0x00010000
+#define MCDE_CRC_CS1EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CS1EN, __x)
+#define MCDE_CRC_CS2EN_SHIFT 17
+#define MCDE_CRC_CS2EN_MASK 0x00020000
+#define MCDE_CRC_CS2EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CS2EN, __x)
+#define MCDE_CRC_RESEN_SHIFT 18
+#define MCDE_CRC_RESEN_MASK 0x00040000
+#define MCDE_CRC_RESEN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, RESEN, __x)
+#define MCDE_CRC_CS1POL_SHIFT 19
+#define MCDE_CRC_CS1POL_MASK 0x00080000
+#define MCDE_CRC_CS1POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CS1POL, __x)
+#define MCDE_CRC_CS2POL_SHIFT 20
+#define MCDE_CRC_CS2POL_MASK 0x00100000
+#define MCDE_CRC_CS2POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CS2POL, __x)
+#define MCDE_CRC_CD1POL_SHIFT 21
+#define MCDE_CRC_CD1POL_MASK 0x00200000
+#define MCDE_CRC_CD1POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CD1POL, __x)
+#define MCDE_CRC_CD2POL_SHIFT 22
+#define MCDE_CRC_CD2POL_MASK 0x00400000
+#define MCDE_CRC_CD2POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CD2POL, __x)
+#define MCDE_CRC_WR1POL_SHIFT 23
+#define MCDE_CRC_WR1POL_MASK 0x00800000
+#define MCDE_CRC_WR1POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, WR1POL, __x)
+#define MCDE_CRC_WR2POL_SHIFT 24
+#define MCDE_CRC_WR2POL_MASK 0x01000000
+#define MCDE_CRC_WR2POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, WR2POL, __x)
+#define MCDE_CRC_RD1POL_SHIFT 25
+#define MCDE_CRC_RD1POL_MASK 0x02000000
+#define MCDE_CRC_RD1POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, RD1POL, __x)
+#define MCDE_CRC_RD2POL_SHIFT 26
+#define MCDE_CRC_RD2POL_MASK 0x04000000
+#define MCDE_CRC_RD2POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, RD2POL, __x)
+#define MCDE_CRC_RES1POL_SHIFT 27
+#define MCDE_CRC_RES1POL_MASK 0x08000000
+#define MCDE_CRC_RES1POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, RES1POL, __x)
+#define MCDE_CRC_RES2POL_SHIFT 28
+#define MCDE_CRC_RES2POL_MASK 0x10000000
+#define MCDE_CRC_RES2POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, RES2POL, __x)
+#define MCDE_CRC_SYNCCTRL_SHIFT 29
+#define MCDE_CRC_SYNCCTRL_MASK 0x60000000
+#define MCDE_CRC_SYNCCTRL_OFF 0
+#define MCDE_CRC_SYNCCTRL_C0 1
+#define MCDE_CRC_SYNCCTRL_C1 2
+#define MCDE_CRC_SYNCCTRL_PING_PONG 3
+#define MCDE_CRC_SYNCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SYNCCTRL, MCDE_CRC_SYNCCTRL_##__x)
+#define MCDE_CRC_SYNCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SYNCCTRL, __x)
+#define MCDE_CRC_CLAMPC1EN_SHIFT 31
+#define MCDE_CRC_CLAMPC1EN_MASK 0x80000000
+#define MCDE_CRC_CLAMPC1EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CLAMPC1EN, __x)
+#define MCDE_PBCCRC0 0x00000C04
+#define MCDE_PBCCRC0_GROUPOFFSET 0x4
+#define MCDE_PBCCRC0_BSCM_SHIFT 0
+#define MCDE_PBCCRC0_BSCM_MASK 0x00000007
+#define MCDE_PBCCRC0_BSCM_1_8BIT 0
+#define MCDE_PBCCRC0_BSCM_2_8BIT 1
+#define MCDE_PBCCRC0_BSCM_3_8BIT 2
+#define MCDE_PBCCRC0_BSCM_1_16BIT 3
+#define MCDE_PBCCRC0_BSCM_2_16BIT 4
+#define MCDE_PBCCRC0_BSCM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, BSCM, MCDE_PBCCRC0_BSCM_##__x)
+#define MCDE_PBCCRC0_BSCM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, BSCM, __x)
+#define MCDE_PBCCRC0_BSDM_SHIFT 3
+#define MCDE_PBCCRC0_BSDM_MASK 0x00000038
+#define MCDE_PBCCRC0_BSDM_1_8BIT 0
+#define MCDE_PBCCRC0_BSDM_2_8BIT 1
+#define MCDE_PBCCRC0_BSDM_3_8BIT 2
+#define MCDE_PBCCRC0_BSDM_1_16BIT 3
+#define MCDE_PBCCRC0_BSDM_2_16BIT 4
+#define MCDE_PBCCRC0_BSDM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, BSDM, MCDE_PBCCRC0_BSDM_##__x)
+#define MCDE_PBCCRC0_BSDM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, BSDM, __x)
+#define MCDE_PBCCRC0_PDM_SHIFT 6
+#define MCDE_PBCCRC0_PDM_MASK 0x000000C0
+#define MCDE_PBCCRC0_PDM_NORMAL 0
+#define MCDE_PBCCRC0_PDM_16_TO_32 1
+#define MCDE_PBCCRC0_PDM_24_TO_32_RIGHT 2
+#define MCDE_PBCCRC0_PDM_24_TO_32_LEFT 3
+#define MCDE_PBCCRC0_PDM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, PDM, MCDE_PBCCRC0_PDM_##__x)
+#define MCDE_PBCCRC0_PDM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, PDM, __x)
+#define MCDE_PBCCRC0_PDCTRL_SHIFT 12
+#define MCDE_PBCCRC0_PDCTRL_MASK 0x00001000
+#define MCDE_PBCCRC0_PDCTRL(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, PDCTRL, __x)
+#define MCDE_PBCCRC0_BPP_SHIFT 13
+#define MCDE_PBCCRC0_BPP_MASK 0x0000E000
+#define MCDE_PBCCRC0_BPP_8BPP 0
+#define MCDE_PBCCRC0_BPP_12BPP 1
+#define MCDE_PBCCRC0_BPP_15BPP 2
+#define MCDE_PBCCRC0_BPP_16BPP 3
+#define MCDE_PBCCRC0_BPP_18BPP 4
+#define MCDE_PBCCRC0_BPP_24BPP 5
+#define MCDE_PBCCRC0_BPP(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, BPP, __x)
+#define MCDE_PBCCRC1 0x00000C08
+#define MCDE_PBCCRC1_BSCM_SHIFT 0
+#define MCDE_PBCCRC1_BSCM_MASK 0x00000007
+#define MCDE_PBCCRC1_BSCM_1_8BIT 0
+#define MCDE_PBCCRC1_BSCM_2_8BIT 1
+#define MCDE_PBCCRC1_BSCM_3_8BIT 2
+#define MCDE_PBCCRC1_BSCM_1_16BIT 3
+#define MCDE_PBCCRC1_BSCM_2_16BIT 4
+#define MCDE_PBCCRC1_BSCM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, BSCM, MCDE_PBCCRC1_BSCM_##__x)
+#define MCDE_PBCCRC1_BSCM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, BSCM, __x)
+#define MCDE_PBCCRC1_BSDM_SHIFT 3
+#define MCDE_PBCCRC1_BSDM_MASK 0x00000038
+#define MCDE_PBCCRC1_BSDM_1_8BIT 0
+#define MCDE_PBCCRC1_BSDM_2_8BIT 1
+#define MCDE_PBCCRC1_BSDM_3_8BIT 2
+#define MCDE_PBCCRC1_BSDM_1_16BIT 3
+#define MCDE_PBCCRC1_BSDM_2_16BIT 4
+#define MCDE_PBCCRC1_BSDM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, BSDM, MCDE_PBCCRC1_BSDM_##__x)
+#define MCDE_PBCCRC1_BSDM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, BSDM, __x)
+#define MCDE_PBCCRC1_PDM_SHIFT 6
+#define MCDE_PBCCRC1_PDM_MASK 0x000000C0
+#define MCDE_PBCCRC1_PDM_NORMAL 0
+#define MCDE_PBCCRC1_PDM_16_TO_32 1
+#define MCDE_PBCCRC1_PDM_24_TO_32_RIGHT 2
+#define MCDE_PBCCRC1_PDM_24_TO_32_LEFT 3
+#define MCDE_PBCCRC1_PDM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, PDM, MCDE_PBCCRC1_PDM_##__x)
+#define MCDE_PBCCRC1_PDM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, PDM, __x)
+#define MCDE_PBCCRC1_PDCTRL_SHIFT 12
+#define MCDE_PBCCRC1_PDCTRL_MASK 0x00001000
+#define MCDE_PBCCRC1_PDCTRL(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, PDCTRL, __x)
+#define MCDE_PBCCRC1_BPP_SHIFT 13
+#define MCDE_PBCCRC1_BPP_MASK 0x0000E000
+#define MCDE_PBCCRC1_BPP_8BPP 0
+#define MCDE_PBCCRC1_BPP_12BPP 1
+#define MCDE_PBCCRC1_BPP_15BPP 2
+#define MCDE_PBCCRC1_BPP_16BPP 3
+#define MCDE_PBCCRC1_BPP_18BPP 4
+#define MCDE_PBCCRC1_BPP_24BPP 5
+#define MCDE_PBCCRC1_BPP(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, BPP, __x)
+#define MCDE_PBCBMRC00 0x00000C0C
+#define MCDE_PBCBMRC00_GROUPOFFSET 0x4
+#define MCDE_PBCBMRC00_MUXI_SHIFT 0
+#define MCDE_PBCBMRC00_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC00_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC00, MUXI, __x)
+#define MCDE_PBCBMRC01 0x00000C10
+#define MCDE_PBCBMRC01_MUXI_SHIFT 0
+#define MCDE_PBCBMRC01_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC01_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC01, MUXI, __x)
+#define MCDE_PBCBMRC02 0x00000C14
+#define MCDE_PBCBMRC02_MUXI_SHIFT 0
+#define MCDE_PBCBMRC02_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC02_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC02, MUXI, __x)
+#define MCDE_PBCBMRC03 0x00000C18
+#define MCDE_PBCBMRC03_MUXI_SHIFT 0
+#define MCDE_PBCBMRC03_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC03_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC03, MUXI, __x)
+#define MCDE_PBCBMRC04 0x00000C1C
+#define MCDE_PBCBMRC04_MUXI_SHIFT 0
+#define MCDE_PBCBMRC04_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC04_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC04, MUXI, __x)
+#define MCDE_PBCBMRC10 0x00000C20
+#define MCDE_PBCBMRC10_MUXI_SHIFT 0
+#define MCDE_PBCBMRC10_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC10_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC10, MUXI, __x)
+#define MCDE_PBCBMRC11 0x00000C24
+#define MCDE_PBCBMRC11_MUXI_SHIFT 0
+#define MCDE_PBCBMRC11_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC11_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC11, MUXI, __x)
+#define MCDE_PBCBMRC12 0x00000C28
+#define MCDE_PBCBMRC12_MUXI_SHIFT 0
+#define MCDE_PBCBMRC12_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC12_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC12, MUXI, __x)
+#define MCDE_PBCBMRC13 0x00000C2C
+#define MCDE_PBCBMRC13_MUXI_SHIFT 0
+#define MCDE_PBCBMRC13_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC13_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC13, MUXI, __x)
+#define MCDE_PBCBMRC14 0x00000C30
+#define MCDE_PBCBMRC14_MUXI_SHIFT 0
+#define MCDE_PBCBMRC14_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC14_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC14, MUXI, __x)
+#define MCDE_PBCBCRC00 0x00000C34
+#define MCDE_PBCBCRC00_GROUPOFFSET 0x4
+#define MCDE_PBCBCRC00_CTLI_SHIFT 0
+#define MCDE_PBCBCRC00_CTLI_MASK 0xFFFFFFFF
+#define MCDE_PBCBCRC00_CTLI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBCRC00, CTLI, __x)
+#define MCDE_PBCBCRC10 0x00000C38
+#define MCDE_PBCBCRC10_CTLI_SHIFT 0
+#define MCDE_PBCBCRC10_CTLI_MASK 0xFFFFFFFF
+#define MCDE_PBCBCRC10_CTLI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBCRC10, CTLI, __x)
+#define MCDE_PBCBCRC01 0x00000C48
+#define MCDE_PBCBCRC01_GROUPOFFSET 0x4
+#define MCDE_PBCBCRC01_CTLI_SHIFT 0
+#define MCDE_PBCBCRC01_CTLI_MASK 0xFFFFFFFF
+#define MCDE_PBCBCRC01_CTLI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBCRC01, CTLI, __x)
+#define MCDE_PBCBCRC11 0x00000C4C
+#define MCDE_PBCBCRC11_CTLI_SHIFT 0
+#define MCDE_PBCBCRC11_CTLI_MASK 0xFFFFFFFF
+#define MCDE_PBCBCRC11_CTLI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBCRC11, CTLI, __x)
+#define MCDE_VSCRC0 0x00000C5C
+#define MCDE_VSCRC0_GROUPOFFSET 0x4
+#define MCDE_VSCRC0_VSPMIN_SHIFT 0
+#define MCDE_VSCRC0_VSPMIN_MASK 0x00000FFF
+#define MCDE_VSCRC0_VSPMIN(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSPMIN, __x)
+#define MCDE_VSCRC0_VSPMAX_SHIFT 12
+#define MCDE_VSCRC0_VSPMAX_MASK 0x00FFF000
+#define MCDE_VSCRC0_VSPMAX(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSPMAX, __x)
+#define MCDE_VSCRC0_VSPDIV_SHIFT 24
+#define MCDE_VSCRC0_VSPDIV_MASK 0x07000000
+#define MCDE_VSCRC0_VSPDIV(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSPDIV, __x)
+#define MCDE_VSCRC0_VSPOL_SHIFT 27
+#define MCDE_VSCRC0_VSPOL_MASK 0x08000000
+#define MCDE_VSCRC0_VSPOL_ACTIVE_HIGH 0
+#define MCDE_VSCRC0_VSPOL_ACTIVE_LOW 1
+#define MCDE_VSCRC0_VSPOL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSPOL, MCDE_VSCRC0_VSPOL_##__x)
+#define MCDE_VSCRC0_VSPOL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSPOL, __x)
+#define MCDE_VSCRC0_VSSEL_SHIFT 28
+#define MCDE_VSCRC0_VSSEL_MASK 0x10000000
+#define MCDE_VSCRC0_VSSEL_VSYNC 0
+#define MCDE_VSCRC0_VSSEL_HSYNC 1
+#define MCDE_VSCRC0_VSSEL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSSEL, MCDE_VSCRC0_VSSEL_##__x)
+#define MCDE_VSCRC0_VSSEL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSSEL, __x)
+#define MCDE_VSCRC0_VSDBL_SHIFT 29
+#define MCDE_VSCRC0_VSDBL_MASK 0xE0000000
+#define MCDE_VSCRC0_VSDBL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSDBL, __x)
+#define MCDE_VSCRC1 0x00000C60
+#define MCDE_VSCRC1_VSPMIN_SHIFT 0
+#define MCDE_VSCRC1_VSPMIN_MASK 0x00000FFF
+#define MCDE_VSCRC1_VSPMIN(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSPMIN, __x)
+#define MCDE_VSCRC1_VSPMAX_SHIFT 12
+#define MCDE_VSCRC1_VSPMAX_MASK 0x00FFF000
+#define MCDE_VSCRC1_VSPMAX(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSPMAX, __x)
+#define MCDE_VSCRC1_VSPDIV_SHIFT 24
+#define MCDE_VSCRC1_VSPDIV_MASK 0x07000000
+#define MCDE_VSCRC1_VSPDIV(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSPDIV, __x)
+#define MCDE_VSCRC1_VSPOL_SHIFT 27
+#define MCDE_VSCRC1_VSPOL_MASK 0x08000000
+#define MCDE_VSCRC1_VSPOL_ACTIVE_HIGH 0
+#define MCDE_VSCRC1_VSPOL_ACTIVE_LOW 1
+#define MCDE_VSCRC1_VSPOL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSPOL, MCDE_VSCRC1_VSPOL_##__x)
+#define MCDE_VSCRC1_VSPOL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSPOL, __x)
+#define MCDE_VSCRC1_VSSEL_SHIFT 28
+#define MCDE_VSCRC1_VSSEL_MASK 0x10000000
+#define MCDE_VSCRC1_VSSEL_VSYNC 0
+#define MCDE_VSCRC1_VSSEL_HSYNC 1
+#define MCDE_VSCRC1_VSSEL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSSEL, MCDE_VSCRC1_VSSEL_##__x)
+#define MCDE_VSCRC1_VSSEL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSSEL, __x)
+#define MCDE_VSCRC1_VSDBL_SHIFT 29
+#define MCDE_VSCRC1_VSDBL_MASK 0xE0000000
+#define MCDE_VSCRC1_VSDBL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSDBL, __x)
+#define MCDE_SCTRC 0x00000C64
+#define MCDE_SCTRC_SYNCDELC0_SHIFT 0
+#define MCDE_SCTRC_SYNCDELC0_MASK 0x000000FF
+#define MCDE_SCTRC_SYNCDELC0(__x) \
+	MCDE_VAL2REG(MCDE_SCTRC, SYNCDELC0, __x)
+#define MCDE_SCTRC_SYNCDELC1_SHIFT 8
+#define MCDE_SCTRC_SYNCDELC1_MASK 0x0000FF00
+#define MCDE_SCTRC_SYNCDELC1(__x) \
+	MCDE_VAL2REG(MCDE_SCTRC, SYNCDELC1, __x)
+#define MCDE_SCTRC_TRDELC_SHIFT 16
+#define MCDE_SCTRC_TRDELC_MASK 0x0FFF0000
+#define MCDE_SCTRC_TRDELC(__x) \
+	MCDE_VAL2REG(MCDE_SCTRC, TRDELC, __x)
+#define MCDE_SCSRC 0x00000C68
+#define MCDE_SCSRC_VSTAC0_SHIFT 0
+#define MCDE_SCSRC_VSTAC0_MASK 0x00000001
+#define MCDE_SCSRC_VSTAC0(__x) \
+	MCDE_VAL2REG(MCDE_SCSRC, VSTAC0, __x)
+#define MCDE_SCSRC_VSTAC1_SHIFT 1
+#define MCDE_SCSRC_VSTAC1_MASK 0x00000002
+#define MCDE_SCSRC_VSTAC1(__x) \
+	MCDE_VAL2REG(MCDE_SCSRC, VSTAC1, __x)
+#define MCDE_BCNR0 0x00000C6C
+#define MCDE_BCNR0_GROUPOFFSET 0x4
+#define MCDE_BCNR0_BCN_SHIFT 0
+#define MCDE_BCNR0_BCN_MASK 0x000000FF
+#define MCDE_BCNR0_BCN(__x) \
+	MCDE_VAL2REG(MCDE_BCNR0, BCN, __x)
+#define MCDE_BCNR1 0x00000C70
+#define MCDE_BCNR1_BCN_SHIFT 0
+#define MCDE_BCNR1_BCN_MASK 0x000000FF
+#define MCDE_BCNR1_BCN(__x) \
+	MCDE_VAL2REG(MCDE_BCNR1, BCN, __x)
+#define MCDE_CSCDTR0 0x00000C74
+#define MCDE_CSCDTR0_GROUPOFFSET 0x4
+#define MCDE_CSCDTR0_CSACT_SHIFT 0
+#define MCDE_CSCDTR0_CSACT_MASK 0x000000FF
+#define MCDE_CSCDTR0_CSACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR0, CSACT, __x)
+#define MCDE_CSCDTR0_CSDEACT_SHIFT 8
+#define MCDE_CSCDTR0_CSDEACT_MASK 0x0000FF00
+#define MCDE_CSCDTR0_CSDEACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR0, CSDEACT, __x)
+#define MCDE_CSCDTR0_CDACT_SHIFT 16
+#define MCDE_CSCDTR0_CDACT_MASK 0x00FF0000
+#define MCDE_CSCDTR0_CDACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR0, CDACT, __x)
+#define MCDE_CSCDTR0_CDDEACT_SHIFT 24
+#define MCDE_CSCDTR0_CDDEACT_MASK 0xFF000000
+#define MCDE_CSCDTR0_CDDEACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR0, CDDEACT, __x)
+#define MCDE_CSCDTR1 0x00000C78
+#define MCDE_CSCDTR1_CSACT_SHIFT 0
+#define MCDE_CSCDTR1_CSACT_MASK 0x000000FF
+#define MCDE_CSCDTR1_CSACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR1, CSACT, __x)
+#define MCDE_CSCDTR1_CSDEACT_SHIFT 8
+#define MCDE_CSCDTR1_CSDEACT_MASK 0x0000FF00
+#define MCDE_CSCDTR1_CSDEACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR1, CSDEACT, __x)
+#define MCDE_CSCDTR1_CDACT_SHIFT 16
+#define MCDE_CSCDTR1_CDACT_MASK 0x00FF0000
+#define MCDE_CSCDTR1_CDACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR1, CDACT, __x)
+#define MCDE_CSCDTR1_CDDEACT_SHIFT 24
+#define MCDE_CSCDTR1_CDDEACT_MASK 0xFF000000
+#define MCDE_CSCDTR1_CDDEACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR1, CDDEACT, __x)
+#define MCDE_RDWRTR0 0x00000C7C
+#define MCDE_RDWRTR0_GROUPOFFSET 0x4
+#define MCDE_RDWRTR0_RWACT_SHIFT 0
+#define MCDE_RDWRTR0_RWACT_MASK 0x000000FF
+#define MCDE_RDWRTR0_RWACT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR0, RWACT, __x)
+#define MCDE_RDWRTR0_RWDEACT_SHIFT 8
+#define MCDE_RDWRTR0_RWDEACT_MASK 0x0000FF00
+#define MCDE_RDWRTR0_RWDEACT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR0, RWDEACT, __x)
+#define MCDE_RDWRTR0_MOTINT_SHIFT 16
+#define MCDE_RDWRTR0_MOTINT_MASK 0x00010000
+#define MCDE_RDWRTR0_MOTINT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR0, MOTINT, __x)
+#define MCDE_RDWRTR1 0x00000C80
+#define MCDE_RDWRTR1_RWACT_SHIFT 0
+#define MCDE_RDWRTR1_RWACT_MASK 0x000000FF
+#define MCDE_RDWRTR1_RWACT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR1, RWACT, __x)
+#define MCDE_RDWRTR1_RWDEACT_SHIFT 8
+#define MCDE_RDWRTR1_RWDEACT_MASK 0x0000FF00
+#define MCDE_RDWRTR1_RWDEACT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR1, RWDEACT, __x)
+#define MCDE_RDWRTR1_MOTINT_SHIFT 16
+#define MCDE_RDWRTR1_MOTINT_MASK 0x00010000
+#define MCDE_RDWRTR1_MOTINT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR1, MOTINT, __x)
+#define MCDE_DOTR0 0x00000C84
+#define MCDE_DOTR0_GROUPOFFSET 0x4
+#define MCDE_DOTR0_DOACT_SHIFT 0
+#define MCDE_DOTR0_DOACT_MASK 0x000000FF
+#define MCDE_DOTR0_DOACT(__x) \
+	MCDE_VAL2REG(MCDE_DOTR0, DOACT, __x)
+#define MCDE_DOTR0_DODEACT_SHIFT 8
+#define MCDE_DOTR0_DODEACT_MASK 0x0000FF00
+#define MCDE_DOTR0_DODEACT(__x) \
+	MCDE_VAL2REG(MCDE_DOTR0, DODEACT, __x)
+#define MCDE_DOTR1 0x00000C88
+#define MCDE_DOTR1_DOACT_SHIFT 0
+#define MCDE_DOTR1_DOACT_MASK 0x000000FF
+#define MCDE_DOTR1_DOACT(__x) \
+	MCDE_VAL2REG(MCDE_DOTR1, DOACT, __x)
+#define MCDE_DOTR1_DODEACT_SHIFT 8
+#define MCDE_DOTR1_DODEACT_MASK 0x0000FF00
+#define MCDE_DOTR1_DODEACT(__x) \
+	MCDE_VAL2REG(MCDE_DOTR1, DODEACT, __x)
+#define MCDE_WCMDC0_V1 0x00000C8C
+#define MCDE_WCMDC0_V1_GROUPOFFSET 0x4
+#define MCDE_WCMDC0_V1_COMMANDVALUE_SHIFT 0
+#define MCDE_WCMDC0_V1_COMMANDVALUE_MASK 0x00FFFFFF
+#define MCDE_WCMDC0_V1_COMMANDVALUE(__x) \
+	MCDE_VAL2REG(MCDE_WCMDC0_V1, COMMANDVALUE, __x)
+#define MCDE_WCMDC1_V1 0x00000C90
+#define MCDE_WCMDC1_V1_COMMANDVALUE_SHIFT 0
+#define MCDE_WCMDC1_V1_COMMANDVALUE_MASK 0x00FFFFFF
+#define MCDE_WCMDC1_V1_COMMANDVALUE(__x) \
+	MCDE_VAL2REG(MCDE_WCMDC1_V1, COMMANDVALUE, __x)
+#define MCDE_WDATADC0 0x00000C94
+#define MCDE_WDATADC0_GROUPOFFSET 0x4
+#define MCDE_WDATADC0_DATAVALUE_SHIFT 0
+#define MCDE_WDATADC0_DATAVALUE_MASK 0x00FFFFFF
+#define MCDE_WDATADC0_DATAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_WDATADC0, DATAVALUE, __x)
+#define MCDE_WDATADC1 0x00000C98
+#define MCDE_WDATADC1_DATAVALUE_SHIFT 0
+#define MCDE_WDATADC1_DATAVALUE_MASK 0x00FFFFFF
+#define MCDE_WDATADC1_DATAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_WDATADC1, DATAVALUE, __x)
+#define MCDE_RDATADC0 0x00000C9C
+#define MCDE_RDATADC0_GROUPOFFSET 0x4
+#define MCDE_RDATADC0_DATAREADFROMDISPLAYMODULE_SHIFT 0
+#define MCDE_RDATADC0_DATAREADFROMDISPLAYMODULE_MASK 0x0000FFFF
+#define MCDE_RDATADC0_DATAREADFROMDISPLAYMODULE(__x) \
+	MCDE_VAL2REG(MCDE_RDATADC0, DATAREADFROMDISPLAYMODULE, __x)
+#define MCDE_RDATADC0_STARTREAD_SHIFT 16
+#define MCDE_RDATADC0_STARTREAD_MASK 0x00010000
+#define MCDE_RDATADC0_STARTREAD(__x) \
+	MCDE_VAL2REG(MCDE_RDATADC0, STARTREAD, __x)
+#define MCDE_RDATADC1 0x00000CA0
+#define MCDE_RDATADC1_DATAREADFROMDISPLAYMODULE_SHIFT 0
+#define MCDE_RDATADC1_DATAREADFROMDISPLAYMODULE_MASK 0x0000FFFF
+#define MCDE_RDATADC1_DATAREADFROMDISPLAYMODULE(__x) \
+	MCDE_VAL2REG(MCDE_RDATADC1, DATAREADFROMDISPLAYMODULE, __x)
+#define MCDE_RDATADC1_STARTREAD_SHIFT 16
+#define MCDE_RDATADC1_STARTREAD_MASK 0x00010000
+#define MCDE_RDATADC1_STARTREAD(__x) \
+	MCDE_VAL2REG(MCDE_RDATADC1, STARTREAD, __x)
+#define MCDE_STATC_V1 0x00000CA4
+#define MCDE_STATC_V1_STATBUSY0_SHIFT 0
+#define MCDE_STATC_V1_STATBUSY0_MASK 0x00000001
+#define MCDE_STATC_V1_STATBUSY0(__x) \
+	MCDE_VAL2REG(MCDE_STATC_V1, STATBUSY0, __x)
+#define MCDE_STATC_V1_FIFOEMPTY0_SHIFT 1
+#define MCDE_STATC_V1_FIFOEMPTY0_MASK 0x00000002
+#define MCDE_STATC_V1_FIFOEMPTY0(__x) \
+	MCDE_VAL2REG(MCDE_STATC_V1, FIFOEMPTY0, __x)
+#define MCDE_STATC_V1_FIFOFULL0_SHIFT 2
+#define MCDE_STATC_V1_FIFOFULL0_MASK 0x00000004
+#define MCDE_STATC_V1_FIFOFULL0(__x) \
+	MCDE_VAL2REG(MCDE_STATC_V1, FIFOFULL0, __x)
+#define MCDE_STATC_V1_FIFOCMDEMPTY0_SHIFT 3
+#define MCDE_STATC_V1_FIFOCMDEMPTY0_MASK 0x00000008
+#define MCDE_STATC_V1_FIFOCMDEMPTY0(__x) \
+	MCDE_VAL2REG(MCDE_STATC_V1, FIFOCMDEMPTY0, __x)
+#define MCDE_STATC_V1_FIFOCMDFULL0_SHIFT 4
+#define MCDE_STATC_V1_FIFOCMDFULL0_MASK 0x00000010
+#define MCDE_STATC_V1_FIFOCMDFULL0(__x) \
+	MCDE_VAL2REG(MCDE_STATC_V1, FIFOCMDFULL0, __x)
+#define MCDE_STATC_V1_STATBUSY1_SHIFT 5
+#define MCDE_STATC_V1_STATBUSY1_MASK 0x00000020
+#define MCDE_STATC_V1_STATBUSY1(__x) \
+	MCDE_VAL2REG(MCDE_STATC_V1, STATBUSY1, __x)
+#define MCDE_STATC_V1_FIFOEMPTY1_SHIFT 6
+#define MCDE_STATC_V1_FIFOEMPTY1_MASK 0x00000040
+#define MCDE_STATC_V1_FIFOEMPTY1(__x) \
+	MCDE_VAL2REG(MCDE_STATC_V1, FIFOEMPTY1, __x)
+#define MCDE_STATC_V1_FIFOFULL1_SHIFT 7
+#define MCDE_STATC_V1_FIFOFULL1_MASK 0x00000080
+#define MCDE_STATC_V1_FIFOFULL1(__x) \
+	MCDE_VAL2REG(MCDE_STATC_V1, FIFOFULL1, __x)
+#define MCDE_STATC_V1_FIFOCMDEMPTY1_SHIFT 8
+#define MCDE_STATC_V1_FIFOCMDEMPTY1_MASK 0x00000100
+#define MCDE_STATC_V1_FIFOCMDEMPTY1(__x) \
+	MCDE_VAL2REG(MCDE_STATC_V1, FIFOCMDEMPTY1, __x)
+#define MCDE_STATC_V1_FIFOCMDFULL1_SHIFT 9
+#define MCDE_STATC_V1_FIFOCMDFULL1_MASK 0x00000200
+#define MCDE_STATC_V1_FIFOCMDFULL1(__x) \
+	MCDE_VAL2REG(MCDE_STATC_V1, FIFOCMDFULL1, __x)
+#define MCDE_CTRLC0 0x00000CA8
+#define MCDE_CTRLC0_GROUPOFFSET 0x4
+#define MCDE_CTRLC0_FIFOWTRMRK_SHIFT 0
+#define MCDE_CTRLC0_FIFOWTRMRK_MASK 0x000000FF
+#define MCDE_CTRLC0_FIFOWTRMRK(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FIFOWTRMRK, __x)
+#define MCDE_CTRLC0_FIFOEMPTY_SHIFT 12
+#define MCDE_CTRLC0_FIFOEMPTY_MASK 0x00001000
+#define MCDE_CTRLC0_FIFOEMPTY(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FIFOEMPTY, __x)
+#define MCDE_CTRLC0_FIFOFULL_SHIFT 13
+#define MCDE_CTRLC0_FIFOFULL_MASK 0x00002000
+#define MCDE_CTRLC0_FIFOFULL(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FIFOFULL, __x)
+#define MCDE_CTRLC0_FORMID_SHIFT 16
+#define MCDE_CTRLC0_FORMID_MASK 0x00070000
+#define MCDE_CTRLC0_FORMID_DSI0VID 0
+#define MCDE_CTRLC0_FORMID_DSI0CMD 1
+#define MCDE_CTRLC0_FORMID_DSI1VID 2
+#define MCDE_CTRLC0_FORMID_DSI1CMD 3
+#define MCDE_CTRLC0_FORMID_DSI2VID 4
+#define MCDE_CTRLC0_FORMID_DSI2CMD 5
+#define MCDE_CTRLC0_FORMID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FORMID, MCDE_CTRLC0_FORMID_##__x)
+#define MCDE_CTRLC0_FORMID(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FORMID, __x)
+#define MCDE_CTRLC0_FORMTYPE_SHIFT 20
+#define MCDE_CTRLC0_FORMTYPE_MASK 0x00700000
+#define MCDE_CTRLC0_FORMTYPE_DPITV 0
+#define MCDE_CTRLC0_FORMTYPE_DBI 1
+#define MCDE_CTRLC0_FORMTYPE_DSI 2
+#define MCDE_CTRLC0_FORMTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FORMTYPE, MCDE_CTRLC0_FORMTYPE_##__x)
+#define MCDE_CTRLC0_FORMTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FORMTYPE, __x)
+#define MCDE_CTRLC1 0x00000CAC
+#define MCDE_CTRLC1_FIFOWTRMRK_SHIFT 0
+#define MCDE_CTRLC1_FIFOWTRMRK_MASK 0x000000FF
+#define MCDE_CTRLC1_FIFOWTRMRK(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FIFOWTRMRK, __x)
+#define MCDE_CTRLC1_FIFOEMPTY_SHIFT 12
+#define MCDE_CTRLC1_FIFOEMPTY_MASK 0x00001000
+#define MCDE_CTRLC1_FIFOEMPTY(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FIFOEMPTY, __x)
+#define MCDE_CTRLC1_FIFOFULL_SHIFT 13
+#define MCDE_CTRLC1_FIFOFULL_MASK 0x00002000
+#define MCDE_CTRLC1_FIFOFULL(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FIFOFULL, __x)
+#define MCDE_CTRLC1_FORMID_SHIFT 16
+#define MCDE_CTRLC1_FORMID_MASK 0x00070000
+#define MCDE_CTRLC1_FORMID_DSI0VID 0
+#define MCDE_CTRLC1_FORMID_DSI0CMD 1
+#define MCDE_CTRLC1_FORMID_DSI1VID 2
+#define MCDE_CTRLC1_FORMID_DSI1CMD 3
+#define MCDE_CTRLC1_FORMID_DSI2VID 4
+#define MCDE_CTRLC1_FORMID_DSI2CMD 5
+#define MCDE_CTRLC1_FORMID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FORMID, MCDE_CTRLC1_FORMID_##__x)
+#define MCDE_CTRLC1_FORMID(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FORMID, __x)
+#define MCDE_CTRLC1_FORMTYPE_SHIFT 20
+#define MCDE_CTRLC1_FORMTYPE_MASK 0x00700000
+#define MCDE_CTRLC1_FORMTYPE_DPITV 0
+#define MCDE_CTRLC1_FORMTYPE_DBI 1
+#define MCDE_CTRLC1_FORMTYPE_DSI 2
+#define MCDE_CTRLC1_FORMTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FORMTYPE, MCDE_CTRLC1_FORMTYPE_##__x)
+#define MCDE_CTRLC1_FORMTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FORMTYPE, __x)
+#define MCDE_DSIVID0CONF0 0x00000E00
+#define MCDE_DSIVID0CONF0_GROUPOFFSET 0x20
+#define MCDE_DSIVID0CONF0_BLANKING_SHIFT 0
+#define MCDE_DSIVID0CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSIVID0CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, BLANKING, __x)
+#define MCDE_DSIVID0CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSIVID0CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSIVID0CONF0_VID_MODE_CMD 0
+#define MCDE_DSIVID0CONF0_VID_MODE_VID 1
+#define MCDE_DSIVID0CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, VID_MODE, \
+	MCDE_DSIVID0CONF0_VID_MODE_##__x)
+#define MCDE_DSIVID0CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, VID_MODE, __x)
+#define MCDE_DSIVID0CONF0_CMD8_SHIFT 13
+#define MCDE_DSIVID0CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSIVID0CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, CMD8, __x)
+#define MCDE_DSIVID0CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSIVID0CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSIVID0CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, BIT_SWAP, __x)
+#define MCDE_DSIVID0CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSIVID0CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSIVID0CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, BYTE_SWAP, __x)
+#define MCDE_DSIVID0CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSIVID0CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSIVID0CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSIVID0CONF0_PACKING_SHIFT 20
+#define MCDE_DSIVID0CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSIVID0CONF0_PACKING_RGB565 0
+#define MCDE_DSIVID0CONF0_PACKING_RGB666 1
+#define MCDE_DSIVID0CONF0_PACKING_RGB888 2
+#define MCDE_DSIVID0CONF0_PACKING_BGR888 3
+#define MCDE_DSIVID0CONF0_PACKING_HDTV 4
+#define MCDE_DSIVID0CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, PACKING, \
+	MCDE_DSIVID0CONF0_PACKING_##__x)
+#define MCDE_DSIVID0CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, PACKING, __x)
+#define MCDE_DSICMD0CONF0 0x00000E20
+#define MCDE_DSICMD0CONF0_BLANKING_SHIFT 0
+#define MCDE_DSICMD0CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSICMD0CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, BLANKING, __x)
+#define MCDE_DSICMD0CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSICMD0CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSICMD0CONF0_VID_MODE_CMD 0
+#define MCDE_DSICMD0CONF0_VID_MODE_VID 1
+#define MCDE_DSICMD0CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, VID_MODE, \
+	MCDE_DSICMD0CONF0_VID_MODE_##__x)
+#define MCDE_DSICMD0CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, VID_MODE, __x)
+#define MCDE_DSICMD0CONF0_CMD8_SHIFT 13
+#define MCDE_DSICMD0CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSICMD0CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, CMD8, __x)
+#define MCDE_DSICMD0CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSICMD0CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSICMD0CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, BIT_SWAP, __x)
+#define MCDE_DSICMD0CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSICMD0CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSICMD0CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, BYTE_SWAP, __x)
+#define MCDE_DSICMD0CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSICMD0CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSICMD0CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSICMD0CONF0_PACKING_SHIFT 20
+#define MCDE_DSICMD0CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSICMD0CONF0_PACKING_RGB565 0
+#define MCDE_DSICMD0CONF0_PACKING_RGB666 1
+#define MCDE_DSICMD0CONF0_PACKING_RGB888 2
+#define MCDE_DSICMD0CONF0_PACKING_BGR888 3
+#define MCDE_DSICMD0CONF0_PACKING_HDTV 4
+#define MCDE_DSICMD0CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, PACKING, \
+	MCDE_DSICMD0CONF0_PACKING_##__x)
+#define MCDE_DSICMD0CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, PACKING, __x)
+#define MCDE_DSIVID1CONF0 0x00000E40
+#define MCDE_DSIVID1CONF0_BLANKING_SHIFT 0
+#define MCDE_DSIVID1CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSIVID1CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, BLANKING, __x)
+#define MCDE_DSIVID1CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSIVID1CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSIVID1CONF0_VID_MODE_CMD 0
+#define MCDE_DSIVID1CONF0_VID_MODE_VID 1
+#define MCDE_DSIVID1CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, VID_MODE, \
+	MCDE_DSIVID1CONF0_VID_MODE_##__x)
+#define MCDE_DSIVID1CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, VID_MODE, __x)
+#define MCDE_DSIVID1CONF0_CMD8_SHIFT 13
+#define MCDE_DSIVID1CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSIVID1CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, CMD8, __x)
+#define MCDE_DSIVID1CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSIVID1CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSIVID1CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, BIT_SWAP, __x)
+#define MCDE_DSIVID1CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSIVID1CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSIVID1CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, BYTE_SWAP, __x)
+#define MCDE_DSIVID1CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSIVID1CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSIVID1CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSIVID1CONF0_PACKING_SHIFT 20
+#define MCDE_DSIVID1CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSIVID1CONF0_PACKING_RGB565 0
+#define MCDE_DSIVID1CONF0_PACKING_RGB666 1
+#define MCDE_DSIVID1CONF0_PACKING_RGB888 2
+#define MCDE_DSIVID1CONF0_PACKING_BGR888 3
+#define MCDE_DSIVID1CONF0_PACKING_HDTV 4
+#define MCDE_DSIVID1CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, PACKING, \
+	MCDE_DSIVID1CONF0_PACKING_##__x)
+#define MCDE_DSIVID1CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, PACKING, __x)
+#define MCDE_DSICMD1CONF0 0x00000E60
+#define MCDE_DSICMD1CONF0_BLANKING_SHIFT 0
+#define MCDE_DSICMD1CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSICMD1CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, BLANKING, __x)
+#define MCDE_DSICMD1CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSICMD1CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSICMD1CONF0_VID_MODE_CMD 0
+#define MCDE_DSICMD1CONF0_VID_MODE_VID 1
+#define MCDE_DSICMD1CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, VID_MODE, \
+	MCDE_DSICMD1CONF0_VID_MODE_##__x)
+#define MCDE_DSICMD1CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, VID_MODE, __x)
+#define MCDE_DSICMD1CONF0_CMD8_SHIFT 13
+#define MCDE_DSICMD1CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSICMD1CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, CMD8, __x)
+#define MCDE_DSICMD1CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSICMD1CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSICMD1CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, BIT_SWAP, __x)
+#define MCDE_DSICMD1CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSICMD1CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSICMD1CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, BYTE_SWAP, __x)
+#define MCDE_DSICMD1CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSICMD1CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSICMD1CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSICMD1CONF0_PACKING_SHIFT 20
+#define MCDE_DSICMD1CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSICMD1CONF0_PACKING_RGB565 0
+#define MCDE_DSICMD1CONF0_PACKING_RGB666 1
+#define MCDE_DSICMD1CONF0_PACKING_RGB888 2
+#define MCDE_DSICMD1CONF0_PACKING_BGR888 3
+#define MCDE_DSICMD1CONF0_PACKING_HDTV 4
+#define MCDE_DSICMD1CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, PACKING, \
+	MCDE_DSICMD1CONF0_PACKING_##__x)
+#define MCDE_DSICMD1CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, PACKING, __x)
+#define MCDE_DSIVID2CONF0 0x00000E80
+#define MCDE_DSIVID2CONF0_BLANKING_SHIFT 0
+#define MCDE_DSIVID2CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSIVID2CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, BLANKING, __x)
+#define MCDE_DSIVID2CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSIVID2CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSIVID2CONF0_VID_MODE_CMD 0
+#define MCDE_DSIVID2CONF0_VID_MODE_VID 1
+#define MCDE_DSIVID2CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, VID_MODE, \
+	MCDE_DSIVID2CONF0_VID_MODE_##__x)
+#define MCDE_DSIVID2CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, VID_MODE, __x)
+#define MCDE_DSIVID2CONF0_CMD8_SHIFT 13
+#define MCDE_DSIVID2CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSIVID2CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, CMD8, __x)
+#define MCDE_DSIVID2CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSIVID2CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSIVID2CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, BIT_SWAP, __x)
+#define MCDE_DSIVID2CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSIVID2CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSIVID2CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, BYTE_SWAP, __x)
+#define MCDE_DSIVID2CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSIVID2CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSIVID2CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSIVID2CONF0_PACKING_SHIFT 20
+#define MCDE_DSIVID2CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSIVID2CONF0_PACKING_RGB565 0
+#define MCDE_DSIVID2CONF0_PACKING_RGB666 1
+#define MCDE_DSIVID2CONF0_PACKING_RGB888 2
+#define MCDE_DSIVID2CONF0_PACKING_BGR888 3
+#define MCDE_DSIVID2CONF0_PACKING_HDTV 4
+#define MCDE_DSIVID2CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, PACKING, \
+	MCDE_DSIVID2CONF0_PACKING_##__x)
+#define MCDE_DSIVID2CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, PACKING, __x)
+#define MCDE_DSICMD2CONF0 0x00000EA0
+#define MCDE_DSICMD2CONF0_BLANKING_SHIFT 0
+#define MCDE_DSICMD2CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSICMD2CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, BLANKING, __x)
+#define MCDE_DSICMD2CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSICMD2CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSICMD2CONF0_VID_MODE_CMD 0
+#define MCDE_DSICMD2CONF0_VID_MODE_VID 1
+#define MCDE_DSICMD2CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, VID_MODE, \
+	MCDE_DSICMD2CONF0_VID_MODE_##__x)
+#define MCDE_DSICMD2CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, VID_MODE, __x)
+#define MCDE_DSICMD2CONF0_CMD8_SHIFT 13
+#define MCDE_DSICMD2CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSICMD2CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, CMD8, __x)
+#define MCDE_DSICMD2CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSICMD2CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSICMD2CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, BIT_SWAP, __x)
+#define MCDE_DSICMD2CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSICMD2CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSICMD2CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, BYTE_SWAP, __x)
+#define MCDE_DSICMD2CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSICMD2CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSICMD2CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSICMD2CONF0_PACKING_SHIFT 20
+#define MCDE_DSICMD2CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSICMD2CONF0_PACKING_RGB565 0
+#define MCDE_DSICMD2CONF0_PACKING_RGB666 1
+#define MCDE_DSICMD2CONF0_PACKING_RGB888 2
+#define MCDE_DSICMD2CONF0_PACKING_BGR888 3
+#define MCDE_DSICMD2CONF0_PACKING_HDTV 4
+#define MCDE_DSICMD2CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, PACKING, \
+	MCDE_DSICMD2CONF0_PACKING_##__x)
+#define MCDE_DSICMD2CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, PACKING, __x)
+#define MCDE_DSIVID0FRAME 0x00000E04
+#define MCDE_DSIVID0FRAME_GROUPOFFSET 0x20
+#define MCDE_DSIVID0FRAME_FRAME_SHIFT 0
+#define MCDE_DSIVID0FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSIVID0FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0FRAME, FRAME, __x)
+#define MCDE_DSICMD0FRAME 0x00000E24
+#define MCDE_DSICMD0FRAME_FRAME_SHIFT 0
+#define MCDE_DSICMD0FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSICMD0FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0FRAME, FRAME, __x)
+#define MCDE_DSIVID1FRAME 0x00000E44
+#define MCDE_DSIVID1FRAME_FRAME_SHIFT 0
+#define MCDE_DSIVID1FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSIVID1FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1FRAME, FRAME, __x)
+#define MCDE_DSICMD1FRAME 0x00000E64
+#define MCDE_DSICMD1FRAME_FRAME_SHIFT 0
+#define MCDE_DSICMD1FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSICMD1FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1FRAME, FRAME, __x)
+#define MCDE_DSIVID2FRAME 0x00000E84
+#define MCDE_DSIVID2FRAME_FRAME_SHIFT 0
+#define MCDE_DSIVID2FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSIVID2FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2FRAME, FRAME, __x)
+#define MCDE_DSICMD2FRAME 0x00000EA4
+#define MCDE_DSICMD2FRAME_FRAME_SHIFT 0
+#define MCDE_DSICMD2FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSICMD2FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2FRAME, FRAME, __x)
+#define MCDE_DSIVID0PKT 0x00000E08
+#define MCDE_DSIVID0PKT_GROUPOFFSET 0x20
+#define MCDE_DSIVID0PKT_PACKET_SHIFT 0
+#define MCDE_DSIVID0PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSIVID0PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0PKT, PACKET, __x)
+#define MCDE_DSICMD0PKT 0x00000E28
+#define MCDE_DSICMD0PKT_PACKET_SHIFT 0
+#define MCDE_DSICMD0PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSICMD0PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0PKT, PACKET, __x)
+#define MCDE_DSIVID1PKT 0x00000E48
+#define MCDE_DSIVID1PKT_PACKET_SHIFT 0
+#define MCDE_DSIVID1PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSIVID1PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1PKT, PACKET, __x)
+#define MCDE_DSICMD1PKT 0x00000E68
+#define MCDE_DSICMD1PKT_PACKET_SHIFT 0
+#define MCDE_DSICMD1PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSICMD1PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1PKT, PACKET, __x)
+#define MCDE_DSIVID2PKT 0x00000E88
+#define MCDE_DSIVID2PKT_PACKET_SHIFT 0
+#define MCDE_DSIVID2PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSIVID2PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2PKT, PACKET, __x)
+#define MCDE_DSICMD2PKT 0x00000EA8
+#define MCDE_DSICMD2PKT_PACKET_SHIFT 0
+#define MCDE_DSICMD2PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSICMD2PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2PKT, PACKET, __x)
+#define MCDE_DSIVID0SYNC 0x00000E0C
+#define MCDE_DSIVID0SYNC_GROUPOFFSET 0x20
+#define MCDE_DSIVID0SYNC_DMA_SHIFT 0
+#define MCDE_DSIVID0SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSIVID0SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0SYNC, DMA, __x)
+#define MCDE_DSIVID0SYNC_SW_SHIFT 16
+#define MCDE_DSIVID0SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSIVID0SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0SYNC, SW, __x)
+#define MCDE_DSICMD0SYNC 0x00000E2C
+#define MCDE_DSICMD0SYNC_DMA_SHIFT 0
+#define MCDE_DSICMD0SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSICMD0SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0SYNC, DMA, __x)
+#define MCDE_DSICMD0SYNC_SW_SHIFT 16
+#define MCDE_DSICMD0SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSICMD0SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0SYNC, SW, __x)
+#define MCDE_DSIVID1SYNC 0x00000E4C
+#define MCDE_DSIVID1SYNC_DMA_SHIFT 0
+#define MCDE_DSIVID1SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSIVID1SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1SYNC, DMA, __x)
+#define MCDE_DSIVID1SYNC_SW_SHIFT 16
+#define MCDE_DSIVID1SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSIVID1SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1SYNC, SW, __x)
+#define MCDE_DSICMD1SYNC 0x00000E6C
+#define MCDE_DSICMD1SYNC_DMA_SHIFT 0
+#define MCDE_DSICMD1SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSICMD1SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1SYNC, DMA, __x)
+#define MCDE_DSICMD1SYNC_SW_SHIFT 16
+#define MCDE_DSICMD1SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSICMD1SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1SYNC, SW, __x)
+#define MCDE_DSIVID2SYNC 0x00000E8C
+#define MCDE_DSIVID2SYNC_DMA_SHIFT 0
+#define MCDE_DSIVID2SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSIVID2SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2SYNC, DMA, __x)
+#define MCDE_DSIVID2SYNC_SW_SHIFT 16
+#define MCDE_DSIVID2SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSIVID2SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2SYNC, SW, __x)
+#define MCDE_DSICMD2SYNC 0x00000EAC
+#define MCDE_DSICMD2SYNC_DMA_SHIFT 0
+#define MCDE_DSICMD2SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSICMD2SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2SYNC, DMA, __x)
+#define MCDE_DSICMD2SYNC_SW_SHIFT 16
+#define MCDE_DSICMD2SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSICMD2SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2SYNC, SW, __x)
+#define MCDE_DSIVID0CMDW 0x00000E10
+#define MCDE_DSIVID0CMDW_GROUPOFFSET 0x20
+#define MCDE_DSIVID0CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSIVID0CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSIVID0CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSIVID0CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSIVID0CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSIVID0CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CMDW, CMDW_START, __x)
+#define MCDE_DSICMD0CMDW 0x00000E30
+#define MCDE_DSICMD0CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSICMD0CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSICMD0CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSICMD0CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSICMD0CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSICMD0CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CMDW, CMDW_START, __x)
+#define MCDE_DSIVID1CMDW 0x00000E50
+#define MCDE_DSIVID1CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSIVID1CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSIVID1CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSIVID1CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSIVID1CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSIVID1CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CMDW, CMDW_START, __x)
+#define MCDE_DSICMD1CMDW 0x00000E70
+#define MCDE_DSICMD1CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSICMD1CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSICMD1CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSICMD1CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSICMD1CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSICMD1CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CMDW, CMDW_START, __x)
+#define MCDE_DSIVID2CMDW 0x00000E90
+#define MCDE_DSIVID2CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSIVID2CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSIVID2CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSIVID2CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSIVID2CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSIVID2CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CMDW, CMDW_START, __x)
+#define MCDE_DSICMD2CMDW 0x00000EB0
+#define MCDE_DSICMD2CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSICMD2CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSICMD2CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSICMD2CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSICMD2CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSICMD2CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CMDW, CMDW_START, __x)
+#define MCDE_DSIVID0DELAY0 0x00000E14
+#define MCDE_DSIVID0DELAY0_GROUPOFFSET 0x20
+#define MCDE_DSIVID0DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSIVID0DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSIVID0DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0DELAY0, INTPKTDEL, __x)
+#define MCDE_DSICMD0DELAY0 0x00000E34
+#define MCDE_DSICMD0DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSICMD0DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSICMD0DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0DELAY0, INTPKTDEL, __x)
+#define MCDE_DSIVID1DELAY0 0x00000E54
+#define MCDE_DSIVID1DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSIVID1DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSIVID1DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1DELAY0, INTPKTDEL, __x)
+#define MCDE_DSICMD1DELAY0 0x00000E74
+#define MCDE_DSICMD1DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSICMD1DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSICMD1DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1DELAY0, INTPKTDEL, __x)
+#define MCDE_DSIVID2DELAY0 0x00000E94
+#define MCDE_DSIVID2DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSIVID2DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSIVID2DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2DELAY0, INTPKTDEL, __x)
+#define MCDE_DSICMD2DELAY0 0x00000EB4
+#define MCDE_DSICMD2DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSICMD2DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSICMD2DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2DELAY0, INTPKTDEL, __x)
+#define MCDE_DSIVID0DELAY1 0x00000E18
+#define MCDE_DSIVID0DELAY1_GROUPOFFSET 0x20
+#define MCDE_DSIVID0DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSIVID0DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSIVID0DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0DELAY1, TEREQDEL, __x)
+#define MCDE_DSIVID0DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSIVID0DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSIVID0DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0DELAY1, FRAMESTARTDEL, __x)
+#define MCDE_DSICMD0DELAY1 0x00000E38
+#define MCDE_DSICMD0DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSICMD0DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSICMD0DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0DELAY1, TEREQDEL, __x)
+#define MCDE_DSICMD0DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSICMD0DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSICMD0DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0DELAY1, FRAMESTARTDEL, __x)
+#define MCDE_DSIVID1DELAY1 0x00000E58
+#define MCDE_DSIVID1DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSIVID1DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSIVID1DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1DELAY1, TEREQDEL, __x)
+#define MCDE_DSIVID1DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSIVID1DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSIVID1DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1DELAY1, FRAMESTARTDEL, __x)
+#define MCDE_DSICMD1DELAY1 0x00000E78
+#define MCDE_DSICMD1DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSICMD1DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSICMD1DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1DELAY1, TEREQDEL, __x)
+#define MCDE_DSICMD1DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSICMD1DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSICMD1DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1DELAY1, FRAMESTARTDEL, __x)
+#define MCDE_DSIVID2DELAY1 0x00000E98
+#define MCDE_DSIVID2DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSIVID2DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSIVID2DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2DELAY1, TEREQDEL, __x)
+#define MCDE_DSIVID2DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSIVID2DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSIVID2DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2DELAY1, FRAMESTARTDEL, __x)
+#define MCDE_DSICMD2DELAY1 0x00000EB8
+#define MCDE_DSICMD2DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSICMD2DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSICMD2DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2DELAY1, TEREQDEL, __x)
+#define MCDE_DSICMD2DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSICMD2DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSICMD2DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2DELAY1, FRAMESTARTDEL, __x)
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 21d816e9dfa..c49cfb346bc 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -270,6 +270,22 @@ config COH901327_WATCHDOG
 	  This watchdog is used to reset the system and thus cannot be
 	  compiled as a module.
 
+config UX500_WATCHDOG
+	bool "ST-Ericsson UX500 watchdog"
+	depends on ARCH_U8500
+	default y if MACH_U8500
+	help
+	  Say Y here to include Watchdog timer support for the
+	  watchdog existing in the prcmu of ST-Ericsson UX500 series platforms.
+	  This watchdog is used to reset the system and thus cannot be
+	  compiled as a module.
+
+config UX500_WATCHDOG_DEBUG
+	bool "ST-Ericsson UX500 watchdog DEBUG"
+	depends on ARCH_U8500 && DEBUG_FS
+	help
+	  Say Y here to add various debugfs entries in wdog/
+
 config TWL4030_WATCHDOG
 	tristate "TWL4030 Watchdog"
 	depends on TWL4030_CORE
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index ed26f7094e4..be5bfd49658 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -44,6 +44,7 @@ obj-$(CONFIG_IOP_WATCHDOG) += iop_wdt.o
 obj-$(CONFIG_DAVINCI_WATCHDOG) += davinci_wdt.o
 obj-$(CONFIG_ORION_WATCHDOG) += orion_wdt.o
 obj-$(CONFIG_COH901327_WATCHDOG) += coh901327_wdt.o
+obj-$(CONFIG_UX500_WATCHDOG) += ux500_wdt.o
 obj-$(CONFIG_STMP3XXX_WATCHDOG) += stmp3xxx_wdt.o
 obj-$(CONFIG_NUC900_WATCHDOG) += nuc900_wdt.o
 obj-$(CONFIG_ADX_WATCHDOG) += adx_wdt.o
diff --git a/drivers/watchdog/mpcore_wdt.c b/drivers/watchdog/mpcore_wdt.c
index 2b4af222b5f..a75c43922e9 100644
--- a/drivers/watchdog/mpcore_wdt.c
+++ b/drivers/watchdog/mpcore_wdt.c
@@ -32,11 +32,13 @@
 #include <linux/uaccess.h>
 #include <linux/slab.h>
 #include <linux/io.h>
+#include <linux/cpufreq.h>
+#include <linux/kexec.h>
 
 #include <asm/smp_twd.h>
 
 struct mpcore_wdt {
-	unsigned long	timer_alive;
+	cpumask_t	timer_alive;
 	struct device	*dev;
 	void __iomem	*base;
 	int		irq;
@@ -47,6 +49,8 @@ struct mpcore_wdt {
 static struct platform_device *mpcore_wdt_dev;
 static DEFINE_SPINLOCK(wdt_lock);
 
+static DEFINE_PER_CPU(unsigned long, mpcore_wdt_rate);
+
 #define TIMER_MARGIN	60
 static int mpcore_margin = TIMER_MARGIN;
 module_param(mpcore_margin, int, 0);
@@ -67,6 +71,8 @@ MODULE_PARM_DESC(mpcore_noboot, "MPcore watchdog action, "
 	"set to 1 to ignore reboots, 0 to reboot (default="
 					__MODULE_STRING(ONLY_TESTING) ")");
 
+#define MPCORE_WDT_PERIPHCLK_PRESCALER 2
+
 /*
  *	This is the interrupt handler.  Note that we only use this
  *	in testing mode, so don't actually do a reboot here.
@@ -99,9 +105,8 @@ static void mpcore_wdt_keepalive(struct mpcore_wdt *wdt)
 
 	spin_lock(&wdt_lock);
 	/* Assume prescale is set to 256 */
-	count =  __raw_readl(wdt->base + TWD_WDOG_COUNTER);
-	count = (0xFFFFFFFFU - count) * (HZ / 5);
-	count = (count / 256) * mpcore_margin;
+	count = per_cpu(mpcore_wdt_rate, smp_processor_id()) / 256;
+	count = count*mpcore_margin;
 
 	/* Reload the counter */
 	writel(count + wdt->perturb, wdt->base + TWD_WDOG_LOAD);
@@ -109,6 +114,56 @@ static void mpcore_wdt_keepalive(struct mpcore_wdt *wdt)
 	spin_unlock(&wdt_lock);
 }
 
+static void mpcore_wdt_set_rate(unsigned long new_rate)
+{
+	unsigned long count;
+	unsigned long long rate_tmp;
+	unsigned long old_rate;
+
+	spin_lock(&wdt_lock);
+	old_rate = per_cpu(mpcore_wdt_rate, smp_processor_id());
+	per_cpu(mpcore_wdt_rate, smp_processor_id()) = new_rate;
+
+	if (mpcore_wdt_dev) {
+		struct mpcore_wdt *wdt = platform_get_drvdata(mpcore_wdt_dev);
+		count = readl(wdt->base + TWD_WDOG_COUNTER);
+		/* The goal: count = count * (new_rate/old_rate); */
+		rate_tmp = (unsigned long long)count * new_rate;
+		do_div(rate_tmp, old_rate);
+		count = rate_tmp;
+		writel(count + wdt->perturb, wdt->base + TWD_WDOG_LOAD);
+		wdt->perturb = wdt->perturb ? 0 : 1;
+	}
+	spin_unlock(&wdt_lock);
+}
+
+static void mpcore_wdt_update_cpu_frequency_on_cpu(void *data)
+{
+	struct cpufreq_freqs *freq = data;
+	mpcore_wdt_set_rate((freq->new * 1000) /
+			    MPCORE_WDT_PERIPHCLK_PRESCALER);
+}
+
+static int mpcore_wdt_cpufreq_notifier(struct notifier_block *nb,
+				       unsigned long event, void *data)
+{
+	struct cpufreq_freqs *freq = data;
+
+	if (event == CPUFREQ_RESUMECHANGE ||
+	    (event == CPUFREQ_PRECHANGE && freq->new > freq->old) ||
+	    (event == CPUFREQ_POSTCHANGE && freq->new < freq->old))
+		smp_call_function_single(freq->cpu,
+					 mpcore_wdt_update_cpu_frequency_on_cpu,
+					 freq, 1);
+
+	return 0;
+}
+
+static struct notifier_block mpcore_wdt_cpufreq_notifier_block = {
+	.notifier_call = mpcore_wdt_cpufreq_notifier,
+};
+
+
 static void mpcore_wdt_stop(struct mpcore_wdt *wdt)
 {
 	spin_lock(&wdt_lock);
@@ -143,6 +198,20 @@ static int mpcore_wdt_set_heartbeat(int t)
 	return 0;
 }
 
+static int mpcore_wdt_stop_notifier(struct notifier_block *this,
+				unsigned long event, void *ptr)
+{
+	struct mpcore_wdt *wdt = platform_get_drvdata(mpcore_wdt_dev);
+	printk(KERN_INFO "Stopping watchdog on non-crashing core %u\n",
+	       smp_processor_id());
+	mpcore_wdt_stop(wdt);
+	return NOTIFY_STOP;
+}
+
+static struct notifier_block mpcore_wdt_stop_block = {
+	.notifier_call = mpcore_wdt_stop_notifier,
+};
+
 /*
  *	/dev/watchdog handling
  */
@@ -150,7 +219,7 @@ static int mpcore_wdt_open(struct inode *inode, struct file *file)
 {
 	struct mpcore_wdt *wdt = platform_get_drvdata(mpcore_wdt_dev);
 
-	if (test_and_set_bit(0, &wdt->timer_alive))
+	if (cpumask_test_and_set_cpu(smp_processor_id(), &wdt->timer_alive))
 		return -EBUSY;
 
 	if (nowayout)
@@ -158,6 +227,9 @@ static int mpcore_wdt_open(struct inode *inode, struct file *file)
 
 	file->private_data = wdt;
 
+	atomic_notifier_chain_register(&crash_percpu_notifier_list,
+				       &mpcore_wdt_stop_block);
+
 	/*
 	 *	Activate timer
 	 */
@@ -181,7 +253,7 @@ static int mpcore_wdt_release(struct inode *inode, struct file *file)
 				"unexpected close, not stopping watchdog!\n");
 		mpcore_wdt_keepalive(wdt);
 	}
-	clear_bit(0, &wdt->timer_alive);
+	cpumask_clear_cpu(smp_processor_id(), &wdt->timer_alive);
 	wdt->expect_close = 0;
 	return 0;
 }
@@ -425,16 +497,31 @@ static char banner[] __initdata = KERN_INFO "MPcore Watchdog Timer: 0.1. "
 
 static int __init mpcore_wdt_init(void)
 {
+	int i;
+
 	/*
 	 * Check that the margin value is within it's range;
 	 * if not reset to the default
 	 */
 	if (mpcore_wdt_set_heartbeat(mpcore_margin)) {
 		mpcore_wdt_set_heartbeat(TIMER_MARGIN);
-		printk(KERN_INFO "mpcore_margin value must be 0 < mpcore_margin < 65536, using %d\n",
+		printk(KERN_INFO "mpcore_wdt: mpcore_margin value must be 0 < mpcore_margin < 65536, using %d\n",
 			TIMER_MARGIN);
 	}
 
+	cpufreq_register_notifier(&mpcore_wdt_cpufreq_notifier_block,
+				  CPUFREQ_TRANSITION_NOTIFIER);
+
+	for_each_online_cpu(i)
+		per_cpu(mpcore_wdt_rate, i) =
+		(cpufreq_get(i) * 1000) / MPCORE_WDT_PERIPHCLK_PRESCALER;
+
+	for_each_online_cpu(i)
+		printk(KERN_INFO
+		       "mpcore_wdt: rate for core %d is %lu.%02luMHz.\n", i,
+		       per_cpu(mpcore_wdt_rate, i) / 1000000,
+		       (per_cpu(mpcore_wdt_rate, i) / 10000) % 100);
+
 	printk(banner, mpcore_noboot, mpcore_margin, nowayout);
 
 	return platform_driver_register(&mpcore_wdt_driver);
diff --git a/drivers/watchdog/ux500_wdt.c b/drivers/watchdog/ux500_wdt.c
new file mode 100644
index 00000000000..fd343197681
--- /dev/null
+++ b/drivers/watchdog/ux500_wdt.c
@@ -0,0 +1,410 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ *
+ * Heavily based upon geodewdt.c
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/moduleparam.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/err.h>
+#include <linux/uaccess.h>
+#include <linux/miscdevice.h>
+#include <linux/watchdog.h>
+#include <linux/platform_device.h>
+
+#include <mach/prcmu.h>
+
+#define WATCHDOG_TIMEOUT 600 /* 10 minutes */
+
+#define WDT_FLAGS_OPEN 1
+#define WDT_FLAGS_ORPHAN 2
+
+static unsigned long wdt_flags;
+
+static int timeout = WATCHDOG_TIMEOUT;
+module_param(timeout, int, 0);
+MODULE_PARM_DESC(timeout,
+	"Watchdog timeout in seconds. 1<= timeout <=131, default="
+				__MODULE_STRING(WATCHDOG_TIMEOUT) ".");
+
+static int nowayout = WATCHDOG_NOWAYOUT;
+module_param(nowayout, int, 0);
+MODULE_PARM_DESC(nowayout,
+	"Watchdog cannot be stopped once started (default="
+				__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+static u8 wdog_id;
+static bool wdt_en;
+static bool wdt_auto_off = true;
+static bool safe_close;
+
+static int ux500_wdt_open(struct inode *inode, struct file *file)
+{
+	if (test_and_set_bit(WDT_FLAGS_OPEN, &wdt_flags))
+		return -EBUSY;
+
+	if (!test_and_clear_bit(WDT_FLAGS_ORPHAN, &wdt_flags))
+		__module_get(THIS_MODULE);
+
+	prcmu_enable_a9wdog(wdog_id);
+	wdt_en = true;
+
+	return nonseekable_open(inode, file);
+}
+
+static int ux500_wdt_release(struct inode *inode, struct file *file)
+{
+	if (safe_close) {
+		prcmu_disable_a9wdog(wdog_id);
+		module_put(THIS_MODULE);
+	} else {
+		pr_crit("Unexpected close - watchdog is not stopping.\n");
+		prcmu_kick_a9wdog(wdog_id);
+
+		set_bit(WDT_FLAGS_ORPHAN, &wdt_flags);
+	}
+
+	clear_bit(WDT_FLAGS_OPEN, &wdt_flags);
+	safe_close = false;
+	return 0;
+}
+
+static ssize_t ux500_wdt_write(struct file *file, const char __user *data,
+			       size_t len, loff_t *ppos)
+{
+	if (!len)
+		return len;
+
+	if (!nowayout) {
+		size_t i;
+		safe_close = false;
+
+		for (i = 0; i != len; i++) {
+			char c;
+
+			if (get_user(c, data + i))
+				return -EFAULT;
+
+			if (c == 'V')
+				safe_close = true;
+		}
+	}
+
+	prcmu_kick_a9wdog(wdog_id);
+
+	return len;
+}
+
+static long ux500_wdt_ioctl(struct file *file, unsigned int cmd,
+			   unsigned long arg)
+{
+	void __user *argp = (void __user *)arg;
+	int __user *p = argp;
+	int interval;
+
+	static const struct watchdog_info ident = {
+		.options =	WDIOF_SETTIMEOUT |
+				WDIOF_KEEPALIVEPING |
+				WDIOF_MAGICCLOSE,
+		.firmware_version =     1,
+		.identity	= "UX500 WDT",
+	};
+
+	switch (cmd) {
+	case WDIOC_GETSUPPORT:
+		return copy_to_user(argp, &ident,
+				    sizeof(ident)) ? -EFAULT : 0;
+
+	case WDIOC_GETSTATUS:
+	case WDIOC_GETBOOTSTATUS:
+		return put_user(0, p);
+
+	case WDIOC_SETOPTIONS:
+	{
+		int options;
+		int ret = -EINVAL;
+
+		if (get_user(options, p))
+			return -EFAULT;
+
+		if (options & WDIOS_DISABLECARD) {
+			prcmu_disable_a9wdog(wdog_id);
+			wdt_en = false;
+			ret = 0;
+		}
+
+		if (options & WDIOS_ENABLECARD) {
+			prcmu_enable_a9wdog(wdog_id);
+			wdt_en = true;
+			ret = 0;
+		}
+
+		return ret;
+	}
+	case WDIOC_KEEPALIVE:
+		return prcmu_kick_a9wdog(wdog_id);
+
+	case WDIOC_SETTIMEOUT:
+		if (get_user(interval, p))
+			return -EFAULT;
+
+		/* 28 bit resolution in ms, becomes 268435455 ms */
+		if (interval > 26843 || interval < 0)
+			return -EINVAL;
+		timeout = interval;
+		prcmu_disable_a9wdog(wdog_id);
+		prcmu_load_a9wdog(wdog_id, timeout * 1000);
+		prcmu_enable_a9wdog(wdog_id);
+
+	/* Fall through */
+	case WDIOC_GETTIMEOUT:
+		return put_user(timeout, p);
+
+	default:
+		return -ENOTTY;
+	}
+
+	return 0;
+}
+
+static const struct file_operations ux500_wdt_fops = {
+	.owner		= THIS_MODULE,
+	.llseek		= no_llseek,
+	.write		= ux500_wdt_write,
+	.unlocked_ioctl = ux500_wdt_ioctl,
+	.open		= ux500_wdt_open,
+	.release	= ux500_wdt_release,
+};
+
+static struct miscdevice ux500_wdt_miscdev = {
+	.minor = WATCHDOG_MINOR,
+	.name = "watchdog",
+	.fops = &ux500_wdt_fops,
+};
+
+#ifdef CONFIG_UX500_WATCHDOG_DEBUG
+
+enum wdog_dbg {
+	WDOG_DBG_CONFIG,
+	WDOG_DBG_LOAD,
+	WDOG_DBG_KICK,
+	WDOG_DBG_EN,
+	WDOG_DBG_DIS,
+};
+
+static ssize_t wdog_dbg_write(struct file *file,
+			      const char __user *user_buf,
+			      size_t count, loff_t *ppos)
+{
+	unsigned long val;
+	int err;
+	enum wdog_dbg v = (enum wdog_dbg)((struct seq_file *)
+					  (file->private_data))->private;
+
+	switch(v) {
+	case WDOG_DBG_CONFIG:
+		err = kstrtoul_from_user(user_buf, count, 0, &val);
+
+		if (!err) {
+			wdt_auto_off = val != 0;
+			(void) prcmu_config_a9wdog(1,
+						 wdt_auto_off);
+		}
+		else {
+			pr_err("ux500_wdt:dbg: unknown value\n");
+		}
+		break;
+	case WDOG_DBG_LOAD:
+		err = kstrtoul_from_user(user_buf, count, 0, &val);
+
+		if (!err) {
+			timeout = val;
+			/* Convert seconds to ms */
+			prcmu_disable_a9wdog(wdog_id);
+			prcmu_load_a9wdog(wdog_id, timeout * 1000);
+			prcmu_enable_a9wdog(wdog_id);
+		}
+		else {
+			pr_err("ux500_wdt:dbg: unknown value\n");
+		}
+		break;
+	case WDOG_DBG_KICK:
+		(void) prcmu_kick_a9wdog(wdog_id);
+		break;
+	case WDOG_DBG_EN:
+		wdt_en = true;
+		(void) prcmu_enable_a9wdog(wdog_id);
+		break;
+	case WDOG_DBG_DIS:
+		wdt_en = false;
+		(void) prcmu_disable_a9wdog(wdog_id);
+		break;
+	}
+
+	return count;
+}
+
+static int wdog_dbg_read(struct seq_file *s, void *p)
+{
+	enum wdog_dbg v = (enum wdog_dbg)s->private;
+
+	switch(v) {
+	case WDOG_DBG_CONFIG:
+		seq_printf(s,"wdog is on id %d, auto off on sleep: %s\n",
+			   (int)wdog_id,
+			   wdt_auto_off ? "enabled": "disabled");
+		break;
+	case WDOG_DBG_LOAD:
+		/* In 1s */
+		seq_printf(s, "wdog load is: %d s\n",
+			   timeout);
+		break;
+	case WDOG_DBG_KICK:
+		break;
+	case WDOG_DBG_EN:
+	case WDOG_DBG_DIS:
+		seq_printf(s, "wdog is %sabled.\n",
+			       wdt_en ? "en" : "dis");
+		break;
+	}
+	return 0;
+}
+
+static int wdog_dbg_open(struct inode *inode,
+			struct file *file)
+{
+	return single_open(file, wdog_dbg_read, inode->i_private);
+}
+
+static const struct file_operations wdog_dbg_fops = {
+	.open		= wdog_dbg_open,
+	.write		= wdog_dbg_write,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+	.owner		= THIS_MODULE,
+};
+
+static int __init wdog_dbg_init(void)
+{
+	struct dentry *wdog_dir;
+
+	wdog_dir = debugfs_create_dir("wdog", NULL);
+	if (IS_ERR_OR_NULL(wdog_dir))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_u8("id",
+					     S_IWUGO | S_IRUGO, wdog_dir,
+					     &wdog_id)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("config",
+					       S_IWUGO | S_IRUGO, wdog_dir,
+					       (void *)WDOG_DBG_CONFIG,
+					       &wdog_dbg_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("load",
+					       S_IWUGO | S_IRUGO, wdog_dir,
+					       (void *)WDOG_DBG_LOAD,
+					       &wdog_dbg_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("kick",
+					       S_IWUGO, wdog_dir,
+					       (void *)WDOG_DBG_KICK,
+					       &wdog_dbg_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("enable",
+					       S_IWUGO | S_IRUGO, wdog_dir,
+					       (void *)WDOG_DBG_EN,
+					       &wdog_dbg_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("disable",
+					       S_IWUGO | S_IRUGO, wdog_dir,
+					       (void *)WDOG_DBG_DIS,
+					       &wdog_dbg_fops)))
+		goto fail;
+
+	return 0;
+fail:
+	pr_err("ux500:wdog: Failed to initialize wdog dbg.\n");
+	debugfs_remove_recursive(wdog_dir);
+
+	return -EFAULT;
+}
+
+#else
+static inline int __init wdog_dbg_init(void)
+{
+	return 0;
+}
+#endif
+
+static int __init ux500_wdt_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	/* Number of watch dogs */
+	prcmu_config_a9wdog(1, wdt_auto_off);
+	/* convert to ms */
+	prcmu_load_a9wdog(wdog_id, timeout * 1000);
+
+	ret = misc_register(&ux500_wdt_miscdev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to register misc.\n");
+		return ret;
+	}
+
+	ret = wdog_dbg_init();
+	if (ret < 0)
+		goto fail;
+
+	dev_info(&pdev->dev, "initialized.\n");
+
+	return 0;
+fail:
+	misc_deregister(&ux500_wdt_miscdev);
+	return ret;
+}
+
+static int __exit ux500_wdt_remove(struct platform_device *dev)
+{
+	prcmu_disable_a9wdog(wdog_id);
+	wdt_en = false;
+	misc_deregister(&ux500_wdt_miscdev);
+	return 0;
+}
+
+static struct platform_driver ux500_wdt_driver = {
+	.remove		= __exit_p(ux500_wdt_remove),
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "ux500_wdt",
+	},
+};
+
+static int __init ux500_wdt_init(void)
+{
+	return platform_driver_probe(&ux500_wdt_driver, ux500_wdt_probe);
+}
+
+static void __exit ux500_wdt_exit(void)
+{
+	platform_driver_unregister(&ux500_wdt_driver);
+}
+
+module_init(ux500_wdt_init);
+module_exit(ux500_wdt_exit);
+
+MODULE_AUTHOR("Jonas Aaberg <jonas.aberg@stericsson.com>");
+MODULE_DESCRIPTION("UX500 Watchdog Driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);
diff --git a/fs/fat/dir.c b/fs/fat/dir.c
index dc563788fa8..a13aacea964 100644
--- a/fs/fat/dir.c
+++ b/fs/fat/dir.c
@@ -1240,7 +1240,7 @@ int fat_add_entries(struct inode *dir, void *slots, int nr_slots,
 	struct super_block *sb = dir->i_sb;
 	struct msdos_sb_info *sbi = MSDOS_SB(sb);
 	struct buffer_head *bh, *prev, *bhs[3]; /* 32*slots (672bytes) */
-	struct msdos_dir_entry *de;
+	struct msdos_dir_entry *uninitialized_var(de);
 	int err, free_slots, i, nr_bhs;
 	loff_t pos, i_pos;
 
diff --git a/fs/mpage.c b/fs/mpage.c
index fdfae9fa98c..cc782f9e68d 100644
--- a/fs/mpage.c
+++ b/fs/mpage.c
@@ -53,6 +53,8 @@ static void mpage_end_io(struct bio *bio, int err)
 			prefetchw(&bvec->bv_page->flags);
 		if (bio_data_dir(bio) == READ) {
 			if (uptodate) {
+				/* FIXME: fix to solve cache coherence issues. */
+				flush_dcache_page(page);
 				SetPageUptodate(page);
 			} else {
 				ClearPageUptodate(page);
diff --git a/fs/partitions/Kconfig b/fs/partitions/Kconfig
index cb5f0a3f1b0..097be1934ee 100644
--- a/fs/partitions/Kconfig
+++ b/fs/partitions/Kconfig
@@ -68,6 +68,25 @@ config ACORN_PARTITION_RISCIX
 	  of machines called RISCiX.  If you say 'Y' here, Linux will be able
 	  to read disks partitioned under RISCiX.
 
+config BLKDEV_PARTITION
+	bool "Blockdev commandline partition support" if PARTITION_ADVANCED
+	default n
+	help
+	  Say Y if you like to setup partitions for block devices by reading
+	  from the kernel command line (kernel boot arguments).
+
+	  The format of the partitions on the command line:
+	  blkdevparts=<blkdev-def>[;<blkdev-def>]
+	  <blkdev-def> := <blkdev-id>:<partdef>[,<partdef>]
+	  <partdef>    := <size>[@<offset>]
+
+	  <blkdev-id>  := unique id used to map driver to blockdev name
+	  <size>       := size in numbers of sectors
+	  <offset>     := offset in sectors for partition to start at
+
+	  Example:
+	  blkdevparts=mmc0:1024@0,524288@1024;mmc1:8192@0,8192@8192
+
 config OSF_PARTITION
 	bool "Alpha OSF partition support" if PARTITION_ADVANCED
 	default y if ALPHA
diff --git a/fs/partitions/Makefile b/fs/partitions/Makefile
index 03af8eac51d..48b216c53db 100644
--- a/fs/partitions/Makefile
+++ b/fs/partitions/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_BLOCK) := check.o
 obj-$(CONFIG_ACORN_PARTITION) += acorn.o
 obj-$(CONFIG_AMIGA_PARTITION) += amiga.o
 obj-$(CONFIG_ATARI_PARTITION) += atari.o
+obj-$(CONFIG_BLKDEV_PARTITION) += blkdev_parts.o
 obj-$(CONFIG_MAC_PARTITION) += mac.o
 obj-$(CONFIG_LDM_PARTITION) += ldm.o
 obj-$(CONFIG_MSDOS_PARTITION) += msdos.o
diff --git a/fs/partitions/blkdev_parts.c b/fs/partitions/blkdev_parts.c
new file mode 100755
index 00000000000..030565b7ce7
--- /dev/null
+++ b/fs/partitions/blkdev_parts.c
@@ -0,0 +1,127 @@
+/*
+ *
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ulf Hansson <ulf.hansson@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Create partitions for block devices by reading from the kernel
+ * command line (kernel boot arguments).
+ *
+ */
+
+#include "check.h"
+#include "blkdev_parts.h"
+
+static char *cmdline;
+
+/*
+ * This is the handler for our kernel commandline parameter,
+ * called from main.c::checksetup().
+ * Note that we can not yet kmalloc() anything, so we only save
+ * the commandline for later processing.
+ */
+static int cmdline_setup(char *s)
+{
+	cmdline = s;
+	return 1;
+}
+__setup("blkdevparts=", cmdline_setup);
+
+/* Parse for a matching blkdev-id and return pointer to partdef */
+static char *parse_blkdev_id(char *blkdev_name)
+{
+	int blkdev_id_len;
+	char *p, *blkdev_id;
+
+	/* Start parsing for a matching blkdev-id */
+	p = blkdev_id = cmdline;
+	while (blkdev_id != NULL) {
+
+		/* Find the end of the blkdev-id string */
+		p = strchr(blkdev_id, ':');
+		if (p == NULL)
+			return NULL;
+
+		/* Check if we found a matching blkdev-id */
+		blkdev_id_len = p - blkdev_id;
+		if (strlen(blkdev_name)	== blkdev_id_len) {
+			if (strncmp(blkdev_name, blkdev_id, blkdev_id_len) == 0)
+				return p;
+		}
+
+		/* Move to next blkdev-id string if there is one */
+		blkdev_id = strchr(p, ';');
+		if (blkdev_id != NULL)
+			blkdev_id++;
+	}
+	return NULL;
+}
+
+static int parse_partdef(char **part, struct parsed_partitions *state, int part_nbr)
+{
+	sector_t size, offset;
+	char *p = *part;
+
+	/* Skip the beginning "," or ":" */
+	p++;
+
+	/* Fetch and verify size from partdef */
+	size = simple_strtoull(p, &p, 10);
+	if ((size == 0) || (*p != '@'))
+		return 0;
+
+	/* Skip the "@" */
+	p++;
+
+	/* Fetch offset from partdef and check if there are more parts */
+	offset = simple_strtoull(p, &p, 10);
+	if (*p == ',')
+		*part = p;
+	else
+		*part = NULL;
+
+	/* Add partition to state */
+	put_partition(state, part_nbr, offset, size);
+	printk(KERN_INFO "\nPartition: size=%llu, offset=%llu\n",
+		(unsigned long long) size,
+		(unsigned long long) offset);
+	return 1;
+}
+
+static int parse_blkdev_parts(char *blkdev_name, struct parsed_partitions *state)
+{
+	char *partdef;
+	int part_nbr = 0;
+
+	/* Find partdef */
+	partdef = parse_blkdev_id(blkdev_name);
+
+	/* Add parts */
+	while (partdef != NULL) {
+		/* Find next part and add it to state */
+		part_nbr++;
+		if (!parse_partdef(&partdef, state, part_nbr))
+			return 0;
+	}
+	return part_nbr;
+}
+
+int blkdev_partition(struct parsed_partitions *state)
+{
+	char blkdev_name[BDEVNAME_SIZE];
+
+	/* Check if there are any partitions to handle */
+	if (cmdline == NULL)
+		return 0;
+
+	/* Get the name of the blockdevice we are operating upon */
+	if (bdevname(state->bdev, blkdev_name) == NULL) {
+		printk(KERN_WARNING "Could not get a blkdev name\n");
+		return 0;
+	}
+
+	/* Parse for partitions and add them to the state */
+	return parse_blkdev_parts(blkdev_name, state);
+}
+
diff --git a/fs/partitions/blkdev_parts.h b/fs/partitions/blkdev_parts.h
new file mode 100755
index 00000000000..16d2b571625
--- /dev/null
+++ b/fs/partitions/blkdev_parts.h
@@ -0,0 +1,14 @@
+/*
+ *
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ulf Hansson <ulf.hansson@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Create partitions for block devices by reading from the kernel
+ * command line (kernel boot arguments).
+ *
+ */
+
+int blkdev_partition(struct parsed_partitions *state);
+
diff --git a/fs/partitions/check.c b/fs/partitions/check.c
index a0a041dfa0f..c03903b62a6 100644
--- a/fs/partitions/check.c
+++ b/fs/partitions/check.c
@@ -27,6 +27,7 @@
 #include "acorn.h"
 #include "amiga.h"
 #include "atari.h"
+#include "blkdev_parts.h"
 #include "ldm.h"
 #include "mac.h"
 #include "msdos.h"
@@ -50,6 +51,9 @@ static int (*check_part[])(struct parsed_partitions *) = {
 	 * Probe partition formats with tables at disk address 0
 	 * that also have an ADFS boot block at 0xdc0.
 	 */
+#ifdef CONFIG_BLKDEV_PARTITION
+	blkdev_partition,
+#endif
 #ifdef CONFIG_ACORN_PARTITION_ICS
 	adfspart_check_ICS,
 #endif
diff --git a/include/Kbuild b/include/Kbuild
index 8d226bfa269..506f6d7dba7 100644
--- a/include/Kbuild
+++ b/include/Kbuild
@@ -10,3 +10,4 @@ header-y += video/
 header-y += drm/
 header-y += xen/
 header-y += scsi/
+header-y += trace/
diff --git a/include/linux/Kbuild b/include/linux/Kbuild
index 01f63627505..d41e1279488 100644
--- a/include/linux/Kbuild
+++ b/include/linux/Kbuild
@@ -3,6 +3,7 @@ header-y += can/
 header-y += caif/
 header-y += dvb/
 header-y += hdlc/
+header-y += hsi/
 header-y += isdn/
 header-y += mmc/
 header-y += nfsd/
diff --git a/include/linux/boottime.h b/include/linux/boottime.h
new file mode 100644
index 00000000000..9836c5b3175
--- /dev/null
+++ b/include/linux/boottime.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2009-2010
+ *
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * boottime is a tool for collecting start-up timing
+ * information and can together with boot loader support
+ * display a total system start-up time.
+ *
+ */
+
+#ifndef LINUX_BOOTTIME_H
+#define LINUX_BOOTTIME_H
+
+#ifdef CONFIG_BOOTTIME
+#include <linux/kernel.h>
+
+/**
+ * struct boottime_timer - Callbacks for generic timer.
+ * @init: Function to call at boottime initialization
+ * @get_time: Returns the number of us since start-up
+ *            Preferable this is based upon a free running timer.
+ *            This is the only required entry.
+ * @finalize: Called before init is executed and boottime is done.
+ */
+struct boottime_timer {
+	int (*init)(void);
+	unsigned long (*get_time)(void);
+	void (*finalize)(void);
+};
+
+/**
+ * boottime_mark_wtime()
+ * Add a sample point with a given time. Useful for adding data collected
+ * by for example a boot loader.
+ * @name: The name of the sample point
+ * @time: The time in us when this point was reached
+ */
+void __init boottime_mark_wtime(char *name, unsigned long time);
+
+/**
+ * boottime_mark()
+ * Add a sample point with the current time.
+ * @name: The name of this sample point
+ */
+void __init boottime_mark(char *name);
+
+/**
+ * boottime_mark_symbolic()
+ * Add a sample point where the name is a symbolic function
+ * and %pF is needed to get the correct function name.
+ * @name: function name.
+ */
+void __init boottime_mark_symbolic(void *name);
+
+/**
+ * boottime_activate()
+ * Activates boottime and register callbacks.
+ * @bt: struct with callbacks.
+ */
+void __ref boottime_activate(struct boottime_timer *bt);
+
+/**
+ * boottime_deactivate()
+ * This function is called when the kernel boot is done.
+ * (before "free init memory" is called)
+ */
+void __init boottime_deactivate(void);
+
+/**
+ * boottime_system_up()
+ * A function is called when the basics of the kernel
+ * is up and running.
+ */
+void __init boottime_system_up(void);
+
+#else
+
+#define boottime_mark_wtime(name, time)
+#define boottime_mark(name)
+#define boottime_mark_symbolic(name)
+#define boottime_activate(bt)
+#define boottime_deactivate()
+#define boottime_system_up()
+#endif
+
+#endif /* LINUX_BOOTTIME_H */
diff --git a/include/linux/db8500-modem-trace.h b/include/linux/db8500-modem-trace.h
new file mode 100644
index 00000000000..4863e1a0b03
--- /dev/null
+++ b/include/linux/db8500-modem-trace.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors: Michel JAOUEN <michel.jaouen@stericsson.com>
+ *          Maxime COQUELIN <maxime.coquelin-nonst@stericsson.com>
+ *		for ST-Ericsson
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+/*  macro for requesting a trace read */
+
+struct modem_trace_req {
+	__u32 phys_addr;
+	__u8 filler;
+	__u8 *buff;
+	__u32 size;
+};
+
+#define TM_IO_NUMBER 0xfc
+#define TM_GET_DUMPINFO _IOR(TM_IO_NUMBER, 1, unsigned long)
+#define TM_TRACE_REQ _IOWR(TM_IO_NUMBER, 2, unsigned long)
+
+struct db8500_trace_platform_data {
+	unsigned long ape_base;
+	unsigned long modem_base;
+};
diff --git a/arch/arm/plat-nomadik/include/plat/gpio.h b/include/linux/gpio/nomadik.h
index d5d7e651269..cc956d1eb81 100644
--- a/arch/arm/plat-nomadik/include/plat/gpio.h
+++ b/include/linux/gpio/nomadik.h
@@ -9,20 +9,8 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
-#ifndef __ASM_PLAT_GPIO_H
-#define __ASM_PLAT_GPIO_H
-
-#include <asm-generic/gpio.h>
-
-/*
- * These currently cause a function call to happen, they may be optimized
- * if needed by adding cpu-specific defines to identify blocks
- * (see mach-pxa/include/mach/gpio.h as an example using GPLR etc)
- */
-#define gpio_get_value  __gpio_get_value
-#define gpio_set_value  __gpio_set_value
-#define gpio_cansleep   __gpio_cansleep
-#define gpio_to_irq     __gpio_to_irq
+#ifndef __GPIO_NOMADIK_H
+#define __GPIO_NOMADIK_H
 
 /*
  * "nmk_gpio" and "NMK_GPIO" stand for "Nomadik GPIO", leaving
@@ -78,6 +66,9 @@ extern int nmk_gpio_get_mode(int gpio);
 extern void nmk_gpio_wakeups_suspend(void);
 extern void nmk_gpio_wakeups_resume(void);
 
+extern void nmk_gpio_clocks_enable(void);
+extern void nmk_gpio_clocks_disable(void);
+
 extern void nmk_gpio_read_pull(int gpio_bank, u32 *pull_up);
 
 /*
@@ -93,4 +84,4 @@ struct nmk_gpio_platform_data {
 	bool supports_sleepmode;
 };
 
-#endif /* __ASM_PLAT_GPIO_H */
+#endif
diff --git a/include/linux/hsi/Kbuild b/include/linux/hsi/Kbuild
new file mode 100644
index 00000000000..271a770b478
--- /dev/null
+++ b/include/linux/hsi/Kbuild
@@ -0,0 +1 @@
+header-y += hsi_char.h
diff --git a/include/linux/hsi/hsi.h b/include/linux/hsi/hsi.h
new file mode 100644
index 00000000000..1a52623a7dd
--- /dev/null
+++ b/include/linux/hsi/hsi.h
@@ -0,0 +1,389 @@
+/*
+ * hsi.h
+ *
+ * HSI core header file.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#ifndef __LINUX_HSI_H__
+#define __LINUX_HSI_H__
+
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/scatterlist.h>
+#include <linux/spinlock.h>
+#include <linux/list.h>
+#include <linux/module.h>
+
+/* HSI message ttype */
+#define HSI_MSG_READ	0
+#define HSI_MSG_WRITE	1
+
+/* HSI configuration values */
+enum {
+	HSI_MODE_STREAM	= 1,
+	HSI_MODE_FRAME,
+};
+
+enum {
+	HSI_FLOW_SYNC,	/* Synchronized flow */
+	HSI_FLOW_PIPE,	/* Pipelined flow */
+};
+
+enum {
+	HSI_ARB_RR,	/* Round-robin arbitration */
+	HSI_ARB_PRIO,	/* Channel priority arbitration */
+};
+
+#define HSI_MAX_CHANNELS	16
+
+/* HSI message status codes */
+enum {
+	HSI_STATUS_COMPLETED,	/* Message transfer is completed */
+	HSI_STATUS_PENDING,	/* Message pending to be read/write (POLL) */
+	HSI_STATUS_PROCEEDING,	/* Message transfer is ongoing */
+	HSI_STATUS_QUEUED,	/* Message waiting to be served */
+	HSI_STATUS_ERROR,	/* Error when message transfer was ongoing */
+};
+
+/* HSI port event codes */
+enum {
+	HSI_EVENT_START_RX,
+	HSI_EVENT_STOP_RX,
+};
+
+/**
+ * struct hsi_config - Configuration for RX/TX HSI modules
+ * @mode: Bit transmission mode (STREAM or FRAME)
+ * @channels: Number of channels to use [1..16]
+ * @speed: Max bit transmission speed (Kbit/s)
+ * @flow: RX flow type (SYNCHRONIZED or PIPELINE)
+ * @arb_mode: Arbitration mode for TX frame (Round robin, priority)
+ */
+struct hsi_config {
+	unsigned int	mode;
+	unsigned int	channels;
+	unsigned int	speed;
+	union {
+		unsigned int	flow;		/* RX only */
+		unsigned int	arb_mode;	/* TX only */
+	};
+};
+
+/**
+ * struct hsi_board_info - HSI client board info
+ * @name: Name for the HSI device
+ * @hsi_id: HSI controller id where the client sits
+ * @port: Port number in the controller where the client sits
+ * @tx_cfg: HSI TX configuration
+ * @rx_cfg: HSI RX configuration
+ * @platform_data: Platform related data
+ * @archdata: Architecture-dependent device data
+ */
+struct hsi_board_info {
+	const char		*name;
+	int			hsi_id;
+	unsigned int		port;
+	struct hsi_config	tx_cfg;
+	struct hsi_config	rx_cfg;
+	void			*platform_data;
+	struct dev_archdata	*archdata;
+};
+
+#ifdef CONFIG_HSI_BOARDINFO
+extern int hsi_register_board_info(struct hsi_board_info const *info,
+							unsigned int len);
+#else
+static inline int hsi_register_board_info(struct hsi_board_info const *info,
+							unsigned int len)
+{
+	return 0;
+}
+#endif /* CONFIG_HSI_BOARDINFO */
+
+/**
+ * struct hsi_client - HSI client attached to an HSI port
+ * @device: Driver model representation of the device
+ * @tx_cfg: HSI TX configuration
+ * @rx_cfg: HSI RX configuration
+ * @hsi_start_rx: Called after incoming wake line goes high
+ * @hsi_stop_rx: Called after incoming wake line goes low
+ */
+struct hsi_client {
+	struct device		device;
+	struct hsi_config	tx_cfg;
+	struct hsi_config	rx_cfg;
+	void			(*hsi_start_rx)(struct hsi_client *cl);
+	void			(*hsi_stop_rx)(struct hsi_client *cl);
+	/* private: */
+	unsigned int		pclaimed:1;
+	struct list_head	link;
+};
+
+#define to_hsi_client(dev) container_of(dev, struct hsi_client, device)
+
+static inline void hsi_client_set_drvdata(struct hsi_client *cl, void *data)
+{
+	dev_set_drvdata(&cl->device, data);
+}
+
+static inline void *hsi_client_drvdata(struct hsi_client *cl)
+{
+	return dev_get_drvdata(&cl->device);
+}
+
+/**
+ * struct hsi_client_driver - Driver associated to an HSI client
+ * @driver: Driver model representation of the driver
+ */
+struct hsi_client_driver {
+	struct device_driver	driver;
+};
+
+#define to_hsi_client_driver(drv) container_of(drv, struct hsi_client_driver,\
+									driver)
+
+int hsi_register_client_driver(struct hsi_client_driver *drv);
+
+static inline void hsi_unregister_client_driver(struct hsi_client_driver *drv)
+{
+	driver_unregister(&drv->driver);
+}
+
+/**
+ * struct hsi_msg - HSI message descriptor
+ * @link: Free to use by the current descriptor owner
+ * @cl: HSI device client that issues the transfer
+ * @sgt: Head of the scatterlist array
+ * @context: Client context data associated to the transfer
+ * @complete: Transfer completion callback
+ * @destructor: Destructor to free resources when flushing
+ * @status: Status of the transfer when completed
+ * @actual_len: Actual length of data transfered on completion
+ * @channel: Channel were to TX/RX the message
+ * @ttype: Transfer type (TX if set, RX otherwise)
+ * @break_frame: if true HSI will send/receive a break frame (FRAME MODE)
+ */
+struct hsi_msg {
+	struct list_head	link;
+	struct hsi_client	*cl;
+	struct sg_table		sgt;
+	void			*context;
+
+	void			(*complete)(struct hsi_msg *msg);
+	void			(*destructor)(struct hsi_msg *msg);
+
+	int			status;
+	unsigned int		actual_len;
+	unsigned int		channel;
+	unsigned int		ttype:1;
+	unsigned int		break_frame:1;
+};
+
+struct hsi_msg *hsi_alloc_msg(unsigned int n_frag, gfp_t flags);
+void hsi_free_msg(struct hsi_msg *msg);
+
+/**
+ * struct hsi_port - HSI port device
+ * @device: Driver model representation of the device
+ * @tx_cfg: Current TX path configuration
+ * @rx_cfg: Current RX path configuration
+ * @num: Port number
+ * @shared: Set when port can be shared by different clients
+ * @claimed: Reference count of clients which claimed the port
+ * @lock: Serialize port claim
+ * @async: Asynchronous transfer callback
+ * @setup: Callback to set the HSI client configuration
+ * @flush: Callback to clean the HW state and destroy all pending transfers
+ * @start_tx: Callback to inform that a client wants to TX data
+ * @stop_tx: Callback to inform that a client no longer wishes to TX data
+ * @release: Callback to inform that a client no longer uses the port
+ * @clients: List of hsi_clients using the port.
+ * @clock: Lock to serialize access to the clients list.
+ */
+struct hsi_port {
+	struct device			device;
+	struct hsi_config		tx_cfg;
+	struct hsi_config		rx_cfg;
+	unsigned int			num;
+	unsigned int			shared:1;
+	int				claimed;
+	struct mutex			lock;
+	int				(*async)(struct hsi_msg *msg);
+	int				(*setup)(struct hsi_client *cl);
+	int				(*flush)(struct hsi_client *cl);
+	int				(*start_tx)(struct hsi_client *cl);
+	int				(*stop_tx)(struct hsi_client *cl);
+	int				(*release)(struct hsi_client *cl);
+	struct list_head		clients;
+	spinlock_t			clock;
+};
+
+#define to_hsi_port(dev) container_of(dev, struct hsi_port, device)
+#define hsi_get_port(cl) to_hsi_port((cl)->device.parent)
+
+void hsi_event(struct hsi_port *port, unsigned int event);
+int hsi_claim_port(struct hsi_client *cl, unsigned int share);
+void hsi_release_port(struct hsi_client *cl);
+
+static inline int hsi_port_claimed(struct hsi_client *cl)
+{
+	return cl->pclaimed;
+}
+
+static inline void hsi_port_set_drvdata(struct hsi_port *port, void *data)
+{
+	dev_set_drvdata(&port->device, data);
+}
+
+static inline void *hsi_port_drvdata(struct hsi_port *port)
+{
+	return dev_get_drvdata(&port->device);
+}
+
+/**
+ * struct hsi_controller - HSI controller device
+ * @device: Driver model representation of the device
+ * @owner: Pointer to the module owning the controller
+ * @id: HSI controller ID
+ * @num_ports: Number of ports in the HSI controller
+ * @port: Array of HSI ports
+ */
+struct hsi_controller {
+	struct device		device;
+	struct module		*owner;
+	int			id;
+	unsigned int		num_ports;
+	struct hsi_port		*port;
+};
+
+#define to_hsi_controller(dev) container_of(dev, struct hsi_controller, device)
+
+struct hsi_controller *hsi_alloc_controller(unsigned int n_ports, gfp_t flags);
+void hsi_free_controller(struct hsi_controller *hsi);
+int hsi_register_controller(struct hsi_controller *hsi);
+void hsi_unregister_controller(struct hsi_controller *hsi);
+
+static inline void hsi_controller_set_drvdata(struct hsi_controller *hsi,
+								void *data)
+{
+	dev_set_drvdata(&hsi->device, data);
+}
+
+static inline void *hsi_controller_drvdata(struct hsi_controller *hsi)
+{
+	return dev_get_drvdata(&hsi->device);
+}
+
+static inline struct hsi_port *hsi_find_port_num(struct hsi_controller *hsi,
+							unsigned int num)
+{
+	return (num < hsi->num_ports) ? &hsi->port[num] : NULL;
+}
+
+/*
+ * API for HSI clients
+ */
+int hsi_async(struct hsi_client *cl, struct hsi_msg *msg);
+
+/**
+ * hsi_setup - Configure the client's port
+ * @cl: Pointer to the HSI client
+ *
+ * When sharing ports, clients should either relay on a single
+ * client setup or have the same setup for all of them.
+ *
+ * Return -errno on failure, 0 on success
+ */
+static inline int hsi_setup(struct hsi_client *cl)
+{
+	if (!hsi_port_claimed(cl))
+		return -EACCES;
+	return	hsi_get_port(cl)->setup(cl);
+}
+
+/**
+ * hsi_flush - Flush all pending transactions on the client's port
+ * @cl: Pointer to the HSI client
+ *
+ * This function will destroy all pending hsi_msg in the port and reset
+ * the HW port so it is ready to receive and transmit from a clean state.
+ *
+ * Return -errno on failure, 0 on success
+ */
+static inline int hsi_flush(struct hsi_client *cl)
+{
+	if (!hsi_port_claimed(cl))
+		return -EACCES;
+	return hsi_get_port(cl)->flush(cl);
+}
+
+/**
+ * hsi_async_read - Submit a read transfer
+ * @cl: Pointer to the HSI client
+ * @msg: HSI message descriptor of the transfer
+ *
+ * Return -errno on failure, 0 on success
+ */
+static inline int hsi_async_read(struct hsi_client *cl, struct hsi_msg *msg)
+{
+	msg->ttype = HSI_MSG_READ;
+	return hsi_async(cl, msg);
+}
+
+/**
+ * hsi_async_write - Submit a write transfer
+ * @cl: Pointer to the HSI client
+ * @msg: HSI message descriptor of the transfer
+ *
+ * Return -errno on failure, 0 on success
+ */
+static inline int hsi_async_write(struct hsi_client *cl, struct hsi_msg *msg)
+{
+	msg->ttype = HSI_MSG_WRITE;
+	return hsi_async(cl, msg);
+}
+
+/**
+ * hsi_start_tx - Signal the port that the client wants to start a TX
+ * @cl: Pointer to the HSI client
+ *
+ * Return -errno on failure, 0 on success
+ */
+static inline int hsi_start_tx(struct hsi_client *cl)
+{
+	if (!hsi_port_claimed(cl))
+		return -EACCES;
+	return hsi_get_port(cl)->start_tx(cl);
+}
+
+/**
+ * hsi_stop_tx - Signal the port that the client no longer wants to transmit
+ * @cl: Pointer to the HSI client
+ *
+ * Return -errno on failure, 0 on success
+ */
+static inline int hsi_stop_tx(struct hsi_client *cl)
+{
+	if (!hsi_port_claimed(cl))
+		return -EACCES;
+	return hsi_get_port(cl)->stop_tx(cl);
+}
+#endif /* __LINUX_HSI_H__ */
diff --git a/include/linux/hsi/hsi_char.h b/include/linux/hsi/hsi_char.h
new file mode 100644
index 00000000000..fc498973714
--- /dev/null
+++ b/include/linux/hsi/hsi_char.h
@@ -0,0 +1,65 @@
+/*
+ * hsi_char.h
+ *
+ * Part of the HSI character device driver.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Andras Domokos <andras.domokos at nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+
+#ifndef __HSI_CHAR_H
+#define __HSI_CHAR_H
+
+#define HSI_CHAR_MAGIC		'k'
+#define HSC_IOW(num, dtype)	_IOW(HSI_CHAR_MAGIC, num, dtype)
+#define HSC_IOR(num, dtype)	_IOR(HSI_CHAR_MAGIC, num, dtype)
+#define HSC_IOWR(num, dtype)	_IOWR(HSI_CHAR_MAGIC, num, dtype)
+#define HSC_IO(num)		_IO(HSI_CHAR_MAGIC, num)
+
+#define HSC_RESET		HSC_IO(16)
+#define HSC_SET_PM		HSC_IO(17)
+#define HSC_SEND_BREAK		HSC_IO(18)
+#define HSC_SET_RX		HSC_IOW(19, struct hsc_rx_config)
+#define HSC_GET_RX		HSC_IOW(20, struct hsc_rx_config)
+#define HSC_SET_TX		HSC_IOW(21, struct hsc_tx_config)
+#define HSC_GET_TX		HSC_IOW(22, struct hsc_tx_config)
+
+#define HSC_PM_DISABLE		0
+#define HSC_PM_ENABLE		1
+
+#define HSC_MODE_STREAM		1
+#define HSC_MODE_FRAME		2
+#define HSC_FLOW_SYNC		0
+#define HSC_ARB_RR		0
+#define HSC_ARB_PRIO		1
+
+struct hsc_rx_config {
+	uint32_t mode;
+	uint32_t flow;
+	uint32_t channels;
+};
+
+struct hsc_tx_config {
+	uint32_t mode;
+	uint32_t channels;
+	uint32_t speed;
+	uint32_t arb_mode;
+};
+
+#endif /* __HSI_CHAR_H */
diff --git a/include/linux/hwmem.h b/include/linux/hwmem.h
new file mode 100644
index 00000000000..ba4c116f4b9
--- /dev/null
+++ b/include/linux/hwmem.h
@@ -0,0 +1,593 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Hardware memory driver, hwmem
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _HWMEM_H_
+#define _HWMEM_H_
+
+#include <linux/types.h>
+
+#if !defined(__KERNEL__) && !defined(_KERNEL)
+#include <sys/types.h>
+#else
+#include <linux/mm_types.h>
+#endif
+
+#define HWMEM_DEFAULT_DEVICE_NAME "hwmem"
+
+/**
+ * @brief Flags defining behavior of allocation
+ */
+enum hwmem_alloc_flags {
+	/**
+	 * @brief Buffered
+	 */
+	HWMEM_ALLOC_HINT_WRITE_COMBINE         = (1 << 0),
+	/**
+	 * @brief Non-buffered
+	 */
+	HWMEM_ALLOC_HINT_NO_WRITE_COMBINE      = (1 << 1),
+	/**
+	 * @brief Cached
+	 */
+	HWMEM_ALLOC_HINT_CACHED                = (1 << 2),
+	/**
+	 * @brief Uncached
+	 */
+	HWMEM_ALLOC_HINT_UNCACHED              = (1 << 3),
+	/**
+	 * @brief Write back
+	 */
+	HWMEM_ALLOC_HINT_CACHE_WB              = (1 << 4),
+	/**
+	 * @brief Write through
+	 */
+	HWMEM_ALLOC_HINT_CACHE_WT              = (1 << 5),
+	/**
+	 * @brief No alloc on write
+	 */
+	HWMEM_ALLOC_HINT_CACHE_NAOW            = (1 << 6),
+	/**
+	 * @brief Alloc on write
+	 */
+	HWMEM_ALLOC_HINT_CACHE_AOW             = (1 << 7),
+	/**
+	 * @brief Inner and outer cache
+	 */
+	HWMEM_ALLOC_HINT_INNER_AND_OUTER_CACHE = (1 << 8),
+	/**
+	 * @brief Inner cache only
+	 */
+	HWMEM_ALLOC_HINT_INNER_CACHE_ONLY      = (1 << 9),
+	/**
+	 * @brief Reserved for use by the cache handler integration
+	 */
+	HWMEM_ALLOC_RESERVED_CHI               = (1 << 31),
+};
+
+/**
+ * @brief Flags defining buffer access mode.
+ */
+enum hwmem_access {
+	/**
+	 * @brief Buffer will be read from.
+	 */
+	HWMEM_ACCESS_READ  = (1 << 0),
+	/**
+	 * @brief Buffer will be written to.
+	 */
+	HWMEM_ACCESS_WRITE = (1 << 1),
+	/**
+	 * @brief Buffer will be imported.
+	 */
+	HWMEM_ACCESS_IMPORT = (1 << 2),
+};
+
+/**
+ * @brief Values defining memory types.
+ */
+enum hwmem_mem_type {
+	/**
+	 * @brief Scattered system memory.
+	 */
+	HWMEM_MEM_SCATTERED_SYS,
+	/**
+	 * @brief Contiguous system memory.
+	 */
+	HWMEM_MEM_CONTIGUOUS_SYS,
+};
+
+/* User space API */
+
+/**
+ * @see struct hwmem_region.
+ */
+struct hwmem_region_us {
+	__u32 offset;
+	__u32 count;
+	__u32 start;
+	__u32 end;
+	__u32 size;
+};
+
+/**
+ * @brief Alloc request data.
+ */
+struct hwmem_alloc_request {
+	/**
+	 * @brief [in] Size of requested allocation in bytes. Size will be
+	 * aligned to PAGE_SIZE bytes.
+	 */
+	__u32 size;
+	/**
+	 * @brief [in] Flags describing requested allocation options.
+	 */
+	__u32 flags; /* enum hwmem_alloc_flags */
+	/**
+	 * @brief [in] Default access rights for buffer.
+	 */
+	__u32 default_access; /* enum hwmem_access */
+	/**
+	 * @brief [in] Memory type of the buffer.
+	 */
+	__u32 mem_type; /* enum hwmem_mem_type */
+};
+
+/**
+ * @brief Set domain request data.
+ */
+struct hwmem_set_domain_request {
+	/**
+	 * @brief [in] Identifier of buffer to be prepared. If 0 is specified
+	 * the buffer associated with the current file instance will be used.
+	 */
+	__s32 id;
+	/**
+	 * @brief [in] Flags specifying access mode of the operation.
+	 *
+	 * One of HWMEM_ACCESS_READ and HWMEM_ACCESS_WRITE is required.
+	 * For details, @see enum hwmem_access.
+	 */
+	__u32 access; /* enum hwmem_access */
+	/**
+	 * @brief [in] The region of bytes to be prepared.
+	 *
+	 * For details, @see struct hwmem_region.
+	 */
+	struct hwmem_region_us region;
+};
+
+/**
+ * @brief Pin request data.
+ */
+struct hwmem_pin_request {
+	/**
+	 * @brief [in] Identifier of buffer to be pinned. If 0 is specified,
+	 * the buffer associated with the current file instance will be used.
+	 */
+	__s32 id;
+	/**
+	 * @brief [out] Physical address of first word in buffer.
+	 */
+	__u32 phys_addr;
+};
+
+/**
+ * @brief Set access rights request data.
+ */
+struct hwmem_set_access_request {
+	/**
+	 * @brief [in] Identifier of buffer to set access rights for. If 0 is
+	 * specified, the buffer associated with the current file instance will
+	 * be used.
+	 */
+	__s32 id;
+	/**
+	 * @param access Access value indicating what is allowed.
+	 */
+	__u32 access; /* enum hwmem_access */
+	/**
+	 * @param pid Process ID to set rights for.
+	 */
+	pid_t pid;
+};
+
+/**
+ * @brief Get info request data.
+ */
+struct hwmem_get_info_request {
+	/**
+	 * @brief [in] Identifier of buffer to get info about. If 0 is specified,
+	 * the buffer associated with the current file instance will be used.
+	 */
+	__s32 id;
+	/**
+	 * @brief [out] Size in bytes of buffer.
+	 */
+	__u32 size;
+	/**
+	 * @brief [out] Memory type of buffer.
+	 */
+	__u32 mem_type; /* enum hwmem_mem_type */
+	/**
+	 * @brief [out] Access rights for buffer.
+	 */
+	__u32 access; /* enum hwmem_access */
+};
+
+/**
+ * @brief Allocates <size> number of bytes and returns a buffer identifier.
+ *
+ * Input is a pointer to a hwmem_alloc_request struct.
+ *
+ * @return A buffer identifier on success, or a negative error code.
+ */
+#define HWMEM_ALLOC_IOC _IOW('W', 1, struct hwmem_alloc_request)
+
+/**
+ * @brief Allocates <size> number of bytes and associates the created buffer
+ * with the current file instance.
+ *
+ * If the current file instance is already associated with a buffer the call
+ * will fail. Buffers referenced through files instances shall not be released
+ * with HWMEM_RELEASE_IOC, instead the file instance shall be closed.
+ *
+ * Input is a pointer to a hwmem_alloc_request struct.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+#define HWMEM_ALLOC_FD_IOC _IOW('W', 2, struct hwmem_alloc_request)
+
+/**
+ * @brief Releases buffer.
+ *
+ * Buffers are reference counted and will not be destroyed until the last
+ * reference is released. Buffers allocated with ALLOC_FD_IOC shall not be
+ * released with this IOC, @see HWMEM_ALLOC_FD_IOC.
+ *
+ * Input is the buffer identifier.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+#define HWMEM_RELEASE_IOC _IO('W', 3)
+
+/**
+ * Memory Mapping
+ *
+ * To map a hwmem buffer mmap the hwmem fd and supply the buffer identifier as
+ * the offset. If the buffer is linked to the fd and thus have no buffer
+ * identifier supply 0 as the offset. Note that the offset feature of mmap is
+ * disabled in both cases, you can only mmap starting a position 0.
+ */
+
+/**
+ * @brief Prepares the buffer for CPU access.
+ *
+ * Input is a pointer to a hwmem_set_domain_request struct.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+#define HWMEM_SET_CPU_DOMAIN_IOC _IOW('W', 4, struct hwmem_set_domain_request)
+
+/**
+ * DEPRECATED: Set sync domain from driver instead!
+ *
+ * @brief Prepares the buffer for access by any DMA hardware.
+ *
+ * Input is a pointer to a hwmem_set_domain_request struct.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+#define HWMEM_SET_SYNC_DOMAIN_IOC _IOW('W', 5, struct hwmem_set_domain_request)
+
+/**
+ * DEPRECATED: Pin from driver instead!
+ *
+ * @brief Pins the buffer.
+ *
+ * Input is a pointer to a hwmem_pin_request struct. Only contiguous buffers
+ * can be pinned from user space.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+#define HWMEM_PIN_IOC _IOWR('W', 6, struct hwmem_pin_request)
+
+/**
+ * DEPRECATED: Unpin from driver instead!
+ *
+ * @brief Unpins the buffer.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+#define HWMEM_UNPIN_IOC _IO('W', 7)
+
+/**
+ * @brief Set access rights for buffer.
+ *
+ * Input is a pointer to a hwmem_set_access_request struct.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+#define HWMEM_SET_ACCESS_IOC _IOW('W', 8, struct hwmem_set_access_request)
+
+/**
+ * @brief Get buffer information.
+ *
+ * Input is a pointer to a hwmem_get_info_request struct.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+#define HWMEM_GET_INFO_IOC _IOWR('W', 9, struct hwmem_get_info_request)
+
+/**
+ * @brief Export the buffer identifier for use in another process.
+ *
+ * The global name will not increase the buffers reference count and will
+ * therefore not keep the buffer alive.
+ *
+ * Input is the buffer identifier. If 0 is specified the buffer associated with
+ * the current file instance will be exported.
+ *
+ * @return A global buffer name on success, or a negative error code.
+ */
+#define HWMEM_EXPORT_IOC _IO('W', 10)
+
+/**
+ * @brief Import a buffer to allow local access to the buffer.
+ *
+ * Input is the buffer's global name.
+ *
+ * @return The imported buffer's identifier on success, or a negative error
+ * code.
+ */
+#define HWMEM_IMPORT_IOC _IO('W', 11)
+
+/**
+ * @brief Import a buffer to allow local access to the buffer using the current
+ * fd.
+ *
+ * Input is the buffer's global name.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+#define HWMEM_IMPORT_FD_IOC _IO('W', 12)
+
+#ifdef __KERNEL__
+
+/* Kernel API */
+
+/**
+ * @brief Values defining memory domain.
+ */
+enum hwmem_domain {
+	/**
+	 * @brief This value specifies the neutral memory domain. Setting this
+	 * domain will syncronize all supported memory domains.
+	 */
+	HWMEM_DOMAIN_SYNC = 0,
+	/**
+	 * @brief This value specifies the CPU memory domain.
+	 */
+	HWMEM_DOMAIN_CPU,
+};
+
+struct hwmem_alloc;
+
+/**
+ * @brief Structure defining a region of a memory buffer.
+ *
+ * A buffer is defined to contain a number of equally sized blocks. Each block
+ * has a part of it included in the region [<start>-<end>). That is
+ * <end>-<start> bytes. Each block is <size> bytes long. Total number of bytes
+ * in the region is (<end> - <start>) * <count>. First byte of the region is
+ * <offset> + <start> bytes into the buffer.
+ *
+ * Here's an example of a region in a graphics buffer (X = buffer, R = region):
+ *
+ * XXXXXXXXXXXXXXXXXXXX \
+ * XXXXXXXXXXXXXXXXXXXX |-- offset = 60
+ * XXXXXXXXXXXXXXXXXXXX /
+ * XXRRRRRRRRXXXXXXXXXX \
+ * XXRRRRRRRRXXXXXXXXXX |-- count = 4
+ * XXRRRRRRRRXXXXXXXXXX |
+ * XXRRRRRRRRXXXXXXXXXX /
+ * XXXXXXXXXXXXXXXXXXXX
+ * --| start = 2
+ * ----------| end = 10
+ * --------------------| size = 20
+ */
+struct hwmem_region {
+	/**
+	 * @brief The first block's offset from beginning of buffer.
+	 */
+	size_t offset;
+	/**
+	 * @brief The number of blocks included in this region.
+	 */
+	size_t count;
+	/**
+	 * @brief The index of the first byte included in this block.
+	 */
+	size_t start;
+	/**
+	 * @brief The index of the last byte included in this block plus one.
+	 */
+	size_t end;
+	/**
+	 * @brief The size in bytes of each block.
+	 */
+	size_t size;
+};
+
+struct hwmem_mem_chunk {
+	phys_addr_t paddr;
+	size_t size;
+};
+
+/**
+ * @brief Allocates <size> number of bytes.
+ *
+ * @param size Number of bytes to allocate. All allocations are page aligned.
+ * @param flags Allocation options.
+ * @param def_access Default buffer access rights.
+ * @param mem_type Memory type.
+ *
+ * @return Pointer to allocation, or a negative error code.
+ */
+struct hwmem_alloc *hwmem_alloc(size_t size, enum hwmem_alloc_flags flags,
+		enum hwmem_access def_access, enum hwmem_mem_type mem_type);
+
+/**
+ * @brief Release a previously allocated buffer.
+ * When last reference is released, the buffer will be freed.
+ *
+ * @param alloc Buffer to be released.
+ */
+void hwmem_release(struct hwmem_alloc *alloc);
+
+/**
+ * @brief Set the buffer domain and prepare it for access.
+ *
+ * @param alloc Buffer to be prepared.
+ * @param access Flags defining memory access mode of the call.
+ * @param domain Value specifying the memory domain.
+ * @param region Structure defining the minimum area of the buffer to be
+ * prepared.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+int hwmem_set_domain(struct hwmem_alloc *alloc, enum hwmem_access access,
+		enum hwmem_domain domain, struct hwmem_region *region);
+
+/**
+ * @brief Pins the buffer.
+ *
+ * Notice that the number of mem chunks a buffer consists of can change at any
+ * time if the buffer is not pinned. Because of this one can not assume that
+ * pin will succeed if <mem_chunks> has the length specified by a previous call
+ * to pin as the buffer layout may have changed between the calls. There are
+ * two ways of handling this situation, keep redoing the pin procedure till it
+ * succeeds or allocate enough mem chunks for the worst case ("buffer size" /
+ * "page size" mem chunks). Contiguous buffers always require only one mem
+ * chunk.
+ *
+ * @param alloc Buffer to be pinned.
+ * @param mem_chunks Pointer to array of mem chunks.
+ * @param mem_chunks_length Pointer to variable that contains the length of
+ * <mem_chunks> array. On success the number of written mem chunks will be
+ * stored in this variable. If the call fails with -ENOSPC the required length
+ * of <mem_chunks> will be stored in this variable.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+int hwmem_pin(struct hwmem_alloc *alloc, struct hwmem_mem_chunk *mem_chunks,
+						size_t *mem_chunks_length);
+
+/**
+ * @brief Unpins the buffer.
+ *
+ * @param alloc Buffer to be unpinned.
+ */
+void hwmem_unpin(struct hwmem_alloc *alloc);
+
+/**
+ * @brief Map the buffer to user space.
+ *
+ * @param alloc Buffer to be mapped.
+ *
+ * @return Zero on success, or a negative error code.
+ */
+int hwmem_mmap(struct hwmem_alloc *alloc, struct vm_area_struct *vma);
+
+/**
+ * @brief Map the buffer for use in the kernel.
+ *
+ * This function implicitly pins the buffer.
+ *
+ * @param alloc Buffer to be mapped.
+ *
+ * @return Pointer to buffer, or a negative error code.
+ */
+void *hwmem_kmap(struct hwmem_alloc *alloc);
+
+/**
+ * @brief Un-map a buffer previously mapped with hwmem_kmap.
+ *
+ * This function implicitly unpins the buffer.
+ *
+ * @param alloc Buffer to be un-mapped.
+ */
+void hwmem_kunmap(struct hwmem_alloc *alloc);
+
+/**
+ * @brief Set access rights for buffer.
+ *
+ * @param alloc Buffer to set rights for.
+ * @param access Access value indicating what is allowed.
+ * @param pid Process ID to set rights for.
+ */
+int hwmem_set_access(struct hwmem_alloc *alloc, enum hwmem_access access,
+								pid_t pid);
+
+/**
+ * @brief Get buffer information.
+ *
+ * @param alloc Buffer to get information about.
+ * @param size Pointer to size output variable. Can be NULL.
+ * @param size Pointer to memory type output variable. Can be NULL.
+ * @param size Pointer to access rights output variable. Can be NULL.
+ */
+void hwmem_get_info(struct hwmem_alloc *alloc, size_t *size,
+		enum hwmem_mem_type *mem_type, enum hwmem_access *access);
+
+/**
+ * @brief Allocate a global buffer name.
+ * Generated buffer name is valid in all processes. Consecutive calls will get
+ * the same name for the same buffer.
+ *
+ * @param alloc Buffer to be made public.
+ *
+ * @return Positive global name on success, or a negative error code.
+ */
+s32 hwmem_get_name(struct hwmem_alloc *alloc);
+
+/**
+ * @brief Import the global buffer name to allow local access to the buffer.
+ * This call will add a buffer reference. Resulting buffer should be
+ * released with a call to hwmem_release.
+ *
+ * @param name A valid global buffer name.
+ *
+ * @return Pointer to allocation, or a negative error code.
+ */
+struct hwmem_alloc *hwmem_resolve_by_name(s32 name);
+
+/* Integration */
+
+struct hwmem_allocator_api {
+	void *(*alloc)(void *instance, size_t size);
+	void (*free)(void *instance, void *alloc);
+	phys_addr_t (*get_alloc_paddr)(void *alloc);
+	void *(*get_alloc_kaddr)(void *instance, void *alloc);
+	size_t (*get_alloc_size)(void *alloc);
+};
+
+struct hwmem_mem_type_struct {
+	enum hwmem_mem_type id;
+	struct hwmem_allocator_api allocator_api;
+	void *allocator_instance;
+};
+
+extern struct hwmem_mem_type_struct *hwmem_mem_types;
+extern unsigned int hwmem_num_mem_types;
+
+#endif
+
+#endif /* _HWMEM_H_ */
diff --git a/include/linux/i2s/i2s.h b/include/linux/i2s/i2s.h
new file mode 100644
index 00000000000..79df549d6bd
--- /dev/null
+++ b/include/linux/i2s/i2s.h
@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/*  copyright STMicroelectronics, 2007.                                       */
+/*                                                                            */
+/* 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.1 of the License, or (at your option)*/
+/* any later version.                                                         */
+/*                                                                            */
+/* This program is distributed in the hope that it will be useful, but        */
+/* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY */
+/* or FITNES                                                                  */
+/* FOR A PARTICULAR PURPOSE. See the GNU General Public License for more      */
+/* details.                                                                   */
+/*                                                                            */
+/* You should have received a copy of the GNU General Public License          */
+/* along with this program. If not, see <http://www.gnu.org/licenses/>.       */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __LINUX_I2S_H
+#define __LINUX_I2S_H
+
+/*
+ * INTERFACES between I2S controller-side drivers and I2S infrastructure.
+ */
+extern struct bus_type i2s_bus_type;
+#define I2S_NAME_SIZE 48
+/**
+ * struct i2s_device - Controller side proxy for an I2S slave device
+ * @dev: Driver model representation of the device.
+ * @controller: I2S controller used with the device.
+ * @chip_select: Chipselect, distinguishing chips handled by @controller.
+ * @modalias: Name of the driver to use with this device, or an alias
+ *	for that name.  This appears in the sysfs "modalias" attribute
+ *	for driver coldplugging, and in uevents used for hotplugging
+ *
+ * A @i2s_device is used to interchange data between an I2S slave
+ *
+ * In @dev, the platform_data is used to hold information about this
+ * device that's meaningful to the device's protocol driver, but not
+ * to its controller.
+ */
+struct i2s_device {
+	struct device dev;
+	struct i2s_controller *controller;
+	u8 chip_select;
+	char modalias[32];
+};
+struct i2s_board_info {
+	/* the device name and module name are coupled, like platform_bus;
+	 * "modalias" is normally the driver name.
+	 *
+	 * platform_data goes to i2s_device.dev.platform_data,
+	 */
+	char modalias[32];
+	const void *platform_data;
+	u16 id;
+	u16 chip_select;
+};
+
+#ifdef	CONFIG_STM_I2S
+extern int
+i2s_register_board_info(struct i2s_board_info const *info, unsigned n);
+#else
+/* board init code may ignore whether I2S is configured or not */
+static inline int
+i2s_register_board_info(struct i2s_board_info const *info, unsigned n)
+{
+	return 0;
+}
+#endif
+
+static inline struct i2s_device *to_i2s_device(struct device *dev)
+{
+	return dev ? container_of(dev, struct i2s_device, dev) : NULL;
+}
+
+static inline struct i2s_device *i2s_dev_get(struct i2s_device *i2s)
+{
+	return (i2s && get_device(&i2s->dev)) ? i2s : NULL;
+}
+
+static inline void i2s_dev_put(struct i2s_device *i2s)
+{
+	if (i2s)
+		put_device(&i2s->dev);
+}
+
+static inline void i2s_set_drvdata(struct i2s_device *i2s, void *data)
+{
+	dev_set_drvdata(&i2s->dev, data);
+}
+
+static inline void *i2s_get_drvdata(struct i2s_device *i2s)
+{
+	return dev_get_drvdata(&i2s->dev);
+}
+
+struct i2s_device_id {
+	char name[I2S_NAME_SIZE];
+	/*currently not used may be used in future */
+	u32 device_id;
+	u32 vendor_id;
+};
+
+/**
+ * struct i2s_driver - Host side "protocol" driver
+ */
+struct i2s_driver {
+	int (*probe) (struct i2s_device *i2s);
+	int (*remove) (struct i2s_device *i2s);
+	void (*shutdown) (struct i2s_device *i2s);
+	int (*suspend) (struct i2s_device *i2s, pm_message_t mesg);
+	int (*resume) (struct i2s_device *i2s);
+	struct device_driver driver;
+	const struct i2s_device_id *id_table;
+
+};
+
+static inline struct i2s_driver *to_i2s_driver(struct device_driver *drv)
+{
+	return drv ? container_of(drv, struct i2s_driver, driver) : NULL;
+}
+
+extern int i2s_register_driver(struct i2s_driver *sdrv);
+
+/**
+ * i2s_unregister_driver - reverse effect of i2s_register_driver
+ * @sdrv: the driver to unregister
+ * Context: can sleep
+ */
+static inline void i2s_unregister_driver(struct i2s_driver *sdrv)
+{
+	if (sdrv)
+		driver_unregister(&sdrv->driver);
+}
+
+/**I2S controller parameters*/
+
+enum i2s_direction_t {
+	I2S_DIRECTION_TX = 0,
+	I2S_DIRECTION_RX = 1,
+	I2S_DIRECTION_BOTH = 2
+};
+
+enum i2s_transfer_mode_t {
+	I2S_TRANSFER_MODE_SINGLE_DMA = 0,
+	I2S_TRANSFER_MODE_CYCLIC_DMA = 1,
+	I2S_TRANSFER_MODE_INF_LOOPBACK = 2,
+	I2S_TRANSFER_MODE_NON_DMA = 4,
+};
+
+struct i2s_message {
+	enum i2s_transfer_mode_t i2s_transfer_mode;
+	enum i2s_direction_t i2s_direction;
+	void *txdata;
+	void *rxdata;
+	size_t txbytes;
+	size_t rxbytes;
+	int dma_flag;
+	int tx_offset;
+	int rx_offset;
+	/* cyclic dma */
+	bool cyclic_dma;
+	dma_addr_t buf_addr;
+	size_t buf_len;
+	size_t period_len;
+};
+
+typedef enum {
+	DISABLE_ALL = 0,
+	DISABLE_TRANSMIT = 1,
+	DISABLE_RECEIVE = 2,
+} i2s_flag;
+
+struct i2s_algorithm {
+	int (*cont_setup) (struct i2s_controller *i2s_cont, void *config);
+	int (*cont_transfer) (struct i2s_controller *i2s_cont,
+			      struct i2s_message *message);
+	int (*cont_cleanup) (struct i2s_controller *i2s_cont, i2s_flag flag);
+	int (*cont_hw_status) (struct i2s_controller *i2s_cont);
+	dma_addr_t (*cont_get_pointer) (struct i2s_controller *i2s_cont,
+					enum i2s_direction_t i2s_direction);
+};
+
+struct i2s_controller {
+	struct module *owner;
+	unsigned int id;
+	unsigned int class;
+	const struct i2s_algorithm *algo; /* the algorithm to access the bus */
+	void *data;
+	struct mutex bus_lock;
+	struct device dev; /* the controller device */
+	char name[48];
+};
+#define to_i2s_controller(d) container_of(d, struct i2s_controller, dev)
+
+static inline void *i2s_get_contdata(struct i2s_controller *dev)
+{
+	return dev_get_drvdata(&dev->dev);
+}
+
+static inline void i2s_set_contdata(struct i2s_controller *dev, void *data)
+{
+	dev_set_drvdata(&dev->dev, data);
+}
+
+extern int i2s_add_controller(struct i2s_controller *controller);
+extern int i2s_del_controller(struct i2s_controller *controller);
+extern int i2s_setup(struct i2s_controller *i2s_cont, void *config);
+extern int i2s_transfer(struct i2s_controller *i2s_cont,
+			struct i2s_message *message);
+extern int i2s_cleanup(struct i2s_controller *i2s_cont, i2s_flag flag);
+extern int i2s_hw_status(struct i2s_controller *i2s_cont);
+extern dma_addr_t i2s_get_pointer(struct i2s_controller *i2s_cont,
+				enum i2s_direction_t i2s_direction);
+
+extern struct i2s_device *i2s_get_device_from_boardinfo(int chip_select); /* used in MSP LTP tests */
+extern struct i2s_device *i2s_alloc_device(struct device *dev);
+
+extern int i2s_add_device(struct i2s_device *i2s);
+
+static inline void i2s_unregister_device(struct i2s_device *i2s)
+{
+	if (i2s)
+		device_unregister(&i2s->dev);
+}
+
+#endif /* __LINUX_I2S_H */
diff --git a/include/linux/input/bu21013.h b/include/linux/input/bu21013.h
index 05e03284b92..143f433b9ee 100644
--- a/include/linux/input/bu21013.h
+++ b/include/linux/input/bu21013.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) ST-Ericsson SA 2010
+ * Copyright (C) ST-Ericsson SA 2009
  * Author: Naveen Kumar G <naveen.gaddipati@stericsson.com> for ST-Ericsson
  * License terms:GNU General Public License (GPL) version 2
  */
@@ -9,32 +9,36 @@
 
 /**
  * struct bu21013_platform_device - Handle the platform data
- * @cs_en:	pointer to the cs enable function
- * @cs_dis:	pointer to the cs disable function
- * @irq_read_val:    pointer to read the pen irq value function
+ * @cs_en: pointer to the cs enable function
+ * @cs_dis: pointer to the cs disable function
+ * @irq_read_val: pointer to read the pen irq value function
+ * @x_max_res: xmax resolution
+ * @y_max_res: ymax resolution
  * @touch_x_max: touch x max
  * @touch_y_max: touch y max
  * @cs_pin: chip select pin
  * @irq: irq pin
- * @ext_clk: external clock flag
+ * @has_ext_clk: has external clock
+ * @enable_ext_clk: enable external clock
+ * @portrait: portrait mode flag
  * @x_flip: x flip flag
  * @y_flip: y flip flag
- * @wakeup: wakeup flag
- *
  * This is used to handle the platform data
- */
+ **/
 struct bu21013_platform_device {
 	int (*cs_en)(int reset_pin);
 	int (*cs_dis)(int reset_pin);
 	int (*irq_read_val)(void);
+	int x_max_res;
+	int y_max_res;
 	int touch_x_max;
 	int touch_y_max;
 	unsigned int cs_pin;
 	unsigned int irq;
-	bool ext_clk;
+	bool has_ext_clk;
+	bool enable_ext_clk;
+	bool portrait;
 	bool x_flip;
 	bool y_flip;
-	bool wakeup;
 };
-
 #endif
diff --git a/include/linux/input/lps001wp.h b/include/linux/input/lps001wp.h
new file mode 100644
index 00000000000..aa5eac9af8f
--- /dev/null
+++ b/include/linux/input/lps001wp.h
@@ -0,0 +1,80 @@
+/******************** (C) COPYRIGHT 2010 STMicroelectronics ********************
+*
+* File Name	: lps001wp.h
+* Authors	: MSH - Motion Mems BU - Application Team
+*		: Matteo Dameno (matteo.dameno@st.com)*
+*		: Carmine Iascone (carmine.iascone@st.com)
+* Version	: V 1.1.1
+* Date		: 05/11/2010
+* Description	: LPS001WP pressure temperature sensor driver
+*
+********************************************************************************
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* THE PRESENT SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES
+* OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, FOR THE SOLE
+* PURPOSE TO SUPPORT YOUR APPLICATION DEVELOPMENT.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*
+*******************************************************************************/
+
+#ifndef	__LPS001WP_H__
+#define	__LPS001WP_H__
+
+
+#include	<linux/input.h>
+
+#define	SAD0L				0x00
+#define	SAD0H				0x01
+#define	LPS001WP_PRS_I2C_SADROOT	0x2E
+#define	LPS001WP_PRS_I2C_SAD_L		((LPS001WP_PRS_I2C_SADROOT<<1)|SAD0L)
+#define	LPS001WP_PRS_I2C_SAD_H		((LPS001WP_PRS_I2C_SADROOT<<1)|SAD0H)
+#define	LPS001WP_PRS_DEV_NAME		"lps001wp_prs_sysfs"
+
+/* input define mappings */
+#define ABS_PR		ABS_PRESSURE
+#define ABS_TEMP	ABS_GAS
+#define ABS_DLTPR	ABS_MISC
+
+
+
+/************************************************/
+/* 	Pressure section defines	 	*/
+/************************************************/
+
+/* Pressure Sensor Operating Mode */
+#define	LPS001WP_PRS_ENABLE		0x01
+#define	LPS001WP_PRS_DISABLE		0x00
+
+
+
+
+#define	LPS001WP_PRS_PM_NORMAL		0x40
+#define	LPS001WP_PRS_PM_OFF		LPS001WP_PRS_DISABLE
+
+#define	SENSITIVITY_T		64	/** =	64 LSB/degrC	*/
+#define	SENSITIVITY_P		16	/** =	16 LSB/mbar	*/
+
+
+#ifdef __KERNEL__
+struct lps001wp_prs_platform_data {
+
+	int poll_interval;
+	int min_interval;
+
+	int (*init)(void);
+	void (*exit)(void);
+	int (*power_on)(void);
+	int (*power_off)(void);
+
+};
+
+#endif /* __KERNEL__ */
+
+#endif  /* __LPS001WP_H__ */
diff --git a/include/linux/kexec.h b/include/linux/kexec.h
index c2478a342cd..76b4db78ec1 100644
--- a/include/linux/kexec.h
+++ b/include/linux/kexec.h
@@ -158,6 +158,7 @@ unsigned long paddr_vmcoreinfo_note(void);
 
 extern struct kimage *kexec_image;
 extern struct kimage *kexec_crash_image;
+extern struct atomic_notifier_head crash_percpu_notifier_list;
 
 #ifndef kexec_flush_icache_page
 #define kexec_flush_icache_page(page)
diff --git a/include/linux/l3g4200d.h b/include/linux/l3g4200d.h
new file mode 100644
index 00000000000..28459601e4f
--- /dev/null
+++ b/include/linux/l3g4200d.h
@@ -0,0 +1,27 @@
+/*
+ * ST L3G4200D 3-Axis Gyroscope header file
+ *
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Chethan Krishna N <chethan.krishna@stericsson.com> for ST-Ericsson
+ * Licence terms: GNU General Public Licence (GPL) version 2
+ */
+
+#ifndef __L3G4200D_H__
+#define __L3G4200D_H__
+
+#ifdef __KERNEL__
+struct l3g4200d_gyr_platform_data {
+	const char *name_gyr;
+
+	u8 axis_map_x;
+	u8 axis_map_y;
+	u8 axis_map_z;
+
+	u8 negative_x;
+	u8 negative_y;
+	u8 negative_z;
+};
+
+#endif /* __KERNEL__ */
+
+#endif  /* __L3G4200D_H__ */
diff --git a/include/linux/led-lm3530.h b/include/linux/led-lm3530.h
index 58592fa67d2..8eb12357a11 100644
--- a/include/linux/led-lm3530.h
+++ b/include/linux/led-lm3530.h
@@ -84,6 +84,8 @@ enum lm3530_als_mode {
  * @brt_ramp_rise: rate of rise of led current
  * @als1_resistor_sel: internal resistance from ALS1 input to ground
  * @als2_resistor_sel: internal resistance from ALS2 input to ground
+ * @als_vmin: als input voltage calibrated for max brightness in mV
+ * @als_vmax: als input voltage calibrated for min brightness in mV
  * @brt_val: brightness value (0-255)
  */
 struct lm3530_platform_data {
@@ -101,6 +103,9 @@ struct lm3530_platform_data {
 	u8 als1_resistor_sel;
 	u8 als2_resistor_sel;
 
+	u32 als_vmin;
+	u32 als_vmax;
+
 	u8 brt_val;
 };
 
diff --git a/include/linux/leds-ab5500.h b/include/linux/leds-ab5500.h
new file mode 100644
index 00000000000..2db6ffd188e
--- /dev/null
+++ b/include/linux/leds-ab5500.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2011 ST-Ericsson SA.
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Simple driver for HVLED in ST-Ericsson AB5500 Analog baseband Controller
+ *
+ * Author: Shreshtha Kumar SAHU <shreshthakumar.sahu@stericsson.com>
+ */
+
+#define AB5500_HVLED0		0
+#define AB5500_HVLED1		1
+#define AB5500_HVLED2		2
+#define AB5500_HVLEDS_MAX	3
+
+enum ab5500_led_status {
+	AB5500_LED_OFF = 0x00,
+	AB5500_LED_ON,
+};
+
+struct ab5500_led_conf {
+	char *name;
+	u8 led_id;
+	enum ab5500_led_status status;
+	u8 max_current;
+};
+
+struct ab5500_hvleds_platform_data {
+	bool hw_blink;
+	struct ab5500_led_conf leds[AB5500_HVLEDS_MAX];
+};
diff --git a/include/linux/leds_pwm.h b/include/linux/leds_pwm.h
index 33a07116748..9c5eab6e086 100644
--- a/include/linux/leds_pwm.h
+++ b/include/linux/leds_pwm.h
@@ -11,6 +11,7 @@ struct led_pwm {
 	u8 		active_low;
 	unsigned 	max_brightness;
 	unsigned	pwm_period_ns;
+	unsigned int    lth_brightness;
 };
 
 struct led_pwm_platform_data {
diff --git a/include/linux/lsm303dlh.h b/include/linux/lsm303dlh.h
new file mode 100644
index 00000000000..ad369b1fbd5
--- /dev/null
+++ b/include/linux/lsm303dlh.h
@@ -0,0 +1,61 @@
+/*
+ * lsm303dlh.h
+ * ST 3-Axis Accelerometer/Magnetometer header file
+ *
+ * Copyright (C) 2010 STMicroelectronics
+ * Author: Carmine Iascone (carmine.iascone@st.com)
+ * Author: Matteo Dameno (matteo.dameno@st.com)
+ *
+ * Copyright (C) 2010 STEricsson
+ * Author: Mian Yousaf Kaukab <mian.yousaf.kaukab@stericsson.com>
+ * Updated:Preetham Rao Kaskurthi <preetham.rao@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __LSM303DLH_H__
+#define __LSM303DLH_H__
+
+#include <linux/ioctl.h>
+
+#ifdef __KERNEL__
+/**
+ * struct lsm303dlh_platform_data - platform datastructure for lsm303dlh
+ * @name_a: accelerometer name
+ * @name_m: magnetometer name
+ * @irq_a1: interrupt line 1 of accelerometer
+ * @irq_a2: interrupt line 2 of accelerometer
+ * @irq_m: interrupt line of magnetometer
+ * @axis_map_x: x axis position on the hardware, 0 1 or 2
+ * @axis_map_y: y axis position on the hardware, 0 1 or 2
+ * @axis_map_z: z axis position on the hardware, 0 1 or 2
+ * @negative_x: x axis is orientation, 0 or 1
+ * @negative_y: y axis is orientation, 0 or 1
+ * @negative_z: z axis is orientation, 0 or 1
+ */
+struct lsm303dlh_platform_data {
+	const char *name_a;
+	const char *name_m;
+	u32  irq_a1;
+	u32  irq_a2;
+	u32  irq_m;
+	u8  axis_map_x;
+	u8  axis_map_y;
+	u8  axis_map_z;
+	u8  negative_x;
+	u8  negative_y;
+	u8  negative_z;
+};
+#endif /* __KERNEL__ */
+
+#endif  /* __LSM303DLH_H__ */
diff --git a/include/linux/mfd/ab8500.h b/include/linux/mfd/ab8500.h
index b3184307519..b81df84e83c 100644
--- a/include/linux/mfd/ab8500.h
+++ b/include/linux/mfd/ab8500.h
@@ -74,13 +74,6 @@
 #define AB8500_INT_ACC_DETECT_21DB_F	37
 #define AB8500_INT_ACC_DETECT_21DB_R	38
 #define AB8500_INT_GP_SW_ADC_CONV_END	39
-#define AB8500_INT_ACC_DETECT_1DB_F	33
-#define AB8500_INT_ACC_DETECT_1DB_R	34
-#define AB8500_INT_ACC_DETECT_22DB_F	35
-#define AB8500_INT_ACC_DETECT_22DB_R	36
-#define AB8500_INT_ACC_DETECT_21DB_F	37
-#define AB8500_INT_ACC_DETECT_21DB_R	38
-#define AB8500_INT_GP_SW_ADC_CONV_END	39
 #define AB8500_INT_GPIO6R		40
 #define AB8500_INT_GPIO7R		41
 #define AB8500_INT_GPIO8R		42
@@ -163,7 +156,6 @@ struct ab8500 {
 	struct device	*dev;
 	struct mutex	lock;
 	struct mutex	irq_lock;
-
 	int		irq_base;
 	int		irq;
 	u8		chip_id;
@@ -180,6 +172,9 @@ struct ab8500 {
 
 struct regulator_reg_init;
 struct regulator_init_data;
+struct ab8500_accdet_platform_data;
+struct ab8500_denc_platform_data;
+struct ab8500_audio_platform_data;
 struct ab8500_gpio_platform_data;
 
 /**
@@ -190,15 +185,29 @@ struct ab8500_gpio_platform_data;
  * @regulator_reg_init: regulator init registers
  * @num_regulator: number of regulators
  * @regulator: machine-specific constraints for regulators
+ * @accdet: machine-specific Accessory detection data
+ * @battery: machine-specific battery management data
+ * @charger: machine-specific charger data
+ * @btemp: machine-specific battery temp data
  */
 struct ab8500_platform_data {
 	int irq_base;
+	bool pm_power_off;
 	void (*init) (struct ab8500 *);
 	int num_regulator_reg_init;
 	struct ab8500_regulator_reg_init *regulator_reg_init;
 	int num_regulator;
 	struct regulator_init_data *regulator;
+	struct ab8500_accdet_platform_data *accdet;
+	struct ab8500_bm_data *battery;
+	struct ab8500_denc_platform_data *denc;
+	struct ab8500_audio_platform_data *audio;
+	struct ab8500_charger_platform_data *charger;
+	struct ab8500_btemp_platform_data *btemp;
+	struct ab8500_fg_platform_data *fg;
+	struct ab8500_chargalg_platform_data *chargalg;
 	struct ab8500_gpio_platform_data *gpio;
+	struct ab8500_usbgpio_platform_data *usb;
 };
 
 extern int __devinit ab8500_init(struct ab8500 *ab8500);
diff --git a/include/linux/mfd/ab8500/bm.h b/include/linux/mfd/ab8500/bm.h
new file mode 100644
index 00000000000..2ca4d8779cb
--- /dev/null
+++ b/include/linux/mfd/ab8500/bm.h
@@ -0,0 +1,462 @@
+/*
+ * Copyright ST-Ericsson 2009.
+ *
+ * Author: Arun Murthy <arun.murthy@stericsson.com>
+ * Licensed under GPLv2.
+ */
+
+#ifndef _AB8500_BM_H
+#define _AB8500_BM_H
+
+/*
+ * System control 2 register offsets.
+ * bank = 0x02
+ */
+#define AB8500_MAIN_WDOG_CTRL_REG	0x01
+#define AB8500_LOW_BAT_REG		0x03
+
+/*
+ * USB/ULPI register offsets
+ * Bank : 0x5
+ */
+#define AB8500_USB_LINE_STAT_REG	0x80
+
+/*
+ * Charger / status register offfsets
+ * Bank : 0x0B
+ */
+#define AB8500_CH_STATUS1_REG		0x00
+#define AB8500_CH_STATUS2_REG		0x01
+#define AB8500_CH_USBCH_STAT1_REG	0x02
+#define AB8500_CH_USBCH_STAT2_REG	0x03
+#define AB8500_CH_FSM_STAT_REG		0x04
+#define AB8500_CH_STAT_REG		0x05
+
+/*
+ * Charger / control register offfsets
+ * Bank : 0x0B
+ */
+#define AB8500_CH_VOLT_LVL_REG		0x40
+#define AB8500_CH_VOLT_LVL_MAX_REG	0x41  /*Only in Cut2.0*/
+#define AB8500_CH_OPT_CRNTLVL_REG	0x42
+#define AB8500_CH_OPT_CRNTLVL_MAX_REG	0x43  /*Only in Cut2.0*/
+#define AB8500_CH_WD_TIMER_REG		0x50
+#define AB8500_CHARG_WD_CTRL		0x51
+#define AB8500_BTEMP_HIGH_TH		0x52
+#define AB8500_LED_INDICATOR_PWM_CTRL	0x53
+#define AB8500_LED_INDICATOR_PWM_DUTY	0x54
+#define AB8500_BATT_OVV			0x55
+#define AB8500_CHARGER_CTRL		0x56
+#define AB8500_BAT_CTRL_CURRENT_SOURCE	0x60  /*Only in Cut2.0*/
+
+/*
+ * Charger / main control register offsets
+ * Bank : 0x0B
+ */
+#define AB8500_MCH_CTRL1		0x80
+#define AB8500_MCH_CTRL2		0x81
+#define AB8500_MCH_IPT_CURLVL_REG	0x82
+#define AB8500_CH_WD_REG		0x83
+
+/*
+ * Charger / USB control register offsets
+ * Bank : 0x0B
+ */
+#define AB8500_USBCH_CTRL1_REG		0xC0
+#define AB8500_USBCH_CTRL2_REG		0xC1
+#define AB8500_USBCH_IPT_CRNTLVL_REG	0xC2
+
+/*
+ * Gas Gauge register offsets
+ * Bank : 0x0C
+ */
+#define AB8500_GASG_CC_CTRL_REG		0x00
+#define AB8500_GASG_CC_ACCU1_REG	0x01
+#define AB8500_GASG_CC_ACCU2_REG	0x02
+#define AB8500_GASG_CC_ACCU3_REG	0x03
+#define AB8500_GASG_CC_ACCU4_REG	0x04
+#define AB8500_GASG_CC_SMPL_CNTRL_REG	0x05
+#define AB8500_GASG_CC_SMPL_CNTRH_REG	0x06
+#define AB8500_GASG_CC_SMPL_CNVL_REG	0x07
+#define AB8500_GASG_CC_SMPL_CNVH_REG	0x08
+#define AB8500_GASG_CC_CNTR_AVGOFF_REG	0x09
+#define AB8500_GASG_CC_OFFSET_REG	0x0A
+#define AB8500_GASG_CC_NCOV_ACCU	0x10
+#define AB8500_GASG_CC_NCOV_ACCU_CTRL	0x11
+#define AB8500_GASG_CC_NCOV_ACCU_LOW	0x12
+#define AB8500_GASG_CC_NCOV_ACCU_MED	0x13
+#define AB8500_GASG_CC_NCOV_ACCU_HIGH	0x14
+
+/*
+ * Interrupt register offsets
+ * Bank : 0x0E
+ */
+#define AB8500_IT_SOURCE2_REG		0x01
+#define AB8500_IT_SOURCE21_REG		0x14
+
+/*
+ * RTC register offsets
+ * Bank: 0x0F
+ */
+#define AB8500_RTC_BACKUP_CHG_REG	0x0C
+#define AB8500_RTC_CC_CONF_REG		0x01
+#define AB8500_RTC_CTRL_REG		0x0B
+
+/*
+ * OTP register offsets
+ * Bank : 0x15
+ */
+#define AB8500_OTP_CONF_15		0x0E
+
+/* GPADC constants from AB8500 spec, UM0836 */
+#define ADC_RESOLUTION			1024
+#define ADC_CH_MAIN_MIN			0
+#define ADC_CH_MAIN_MAX			20030
+#define ADC_CH_VBUS_MIN			0
+#define ADC_CH_VBUS_MAX			20030
+#define ADC_CH_VBAT_MIN			2300
+#define ADC_CH_VBAT_MAX			4800
+#define ADC_CH_BKBAT_MIN		0
+#define ADC_CH_BKBAT_MAX		3200
+
+/* Main charge i/p current */
+#define MAIN_CH_IP_CUR_0P9A		0x80
+#define MAIN_CH_IP_CUR_1P0A		0x90
+#define MAIN_CH_IP_CUR_1P1A		0xA0
+#define MAIN_CH_IP_CUR_1P2A		0xB0
+#define MAIN_CH_IP_CUR_1P3A		0xC0
+#define MAIN_CH_IP_CUR_1P4A		0xD0
+#define MAIN_CH_IP_CUR_1P5A		0xE0
+
+/* ChVoltLevel */
+#define CH_VOL_LVL_3P5			0x00
+#define CH_VOL_LVL_4P0			0x14
+#define CH_VOL_LVL_4P05			0x16
+#define CH_VOL_LVL_4P1			0x1B
+#define CH_VOL_LVL_4P15			0x20
+#define CH_VOL_LVL_4P2			0x25
+#define CH_VOL_LVL_4P6			0x4D
+
+/* ChOutputCurrentLevel */
+#define CH_OP_CUR_LVL_0P1		0x00
+#define CH_OP_CUR_LVL_0P2		0x01
+#define CH_OP_CUR_LVL_0P3		0x02
+#define CH_OP_CUR_LVL_0P4		0x03
+#define CH_OP_CUR_LVL_0P5		0x04
+#define CH_OP_CUR_LVL_0P6		0x05
+#define CH_OP_CUR_LVL_0P7		0x06
+#define CH_OP_CUR_LVL_0P8		0x07
+#define CH_OP_CUR_LVL_0P9		0x08
+#define CH_OP_CUR_LVL_1P4		0x0D
+#define CH_OP_CUR_LVL_1P5		0x0E
+#define CH_OP_CUR_LVL_1P6		0x0F
+
+/* BTEMP High thermal limits */
+#define BTEMP_HIGH_TH_57_0		0x00
+#define BTEMP_HIGH_TH_52		0x01
+#define BTEMP_HIGH_TH_57_1		0x02
+#define BTEMP_HIGH_TH_62		0x03
+
+/* current is mA */
+#define USB_0P1A			100
+#define USB_0P2A			200
+#define USB_0P3A			300
+#define USB_0P4A			400
+#define USB_0P5A			500
+
+#define LOW_BAT_3P1V			0x20
+#define LOW_BAT_2P3V			0x00
+#define LOW_BAT_RESET			0x01
+#define LOW_BAT_ENABLE			0x01
+
+/* Backup battery constants */
+#define BUP_ICH_SEL_50UA		0x00
+#define BUP_ICH_SEL_150UA		0x04
+#define BUP_ICH_SEL_300UA		0x08
+#define BUP_ICH_SEL_700UA		0x0C
+
+#define BUP_VCH_SEL_2P5V		0x00
+#define BUP_VCH_SEL_2P6V		0x01
+#define BUP_VCH_SEL_2P8V		0x02
+#define BUP_VCH_SEL_3P1V		0x03
+
+/* Battery OVV constants */
+#define BATT_OVV_ENA			0x02
+#define BATT_OVV_TH_3P7			0x00
+#define BATT_OVV_TH_4P75		0x01
+
+/* VBUS OVV constants */
+#define VBUS_OVV_SELECT_MASK		0x78
+#define VBUS_OVV_SELECT_5P6V		0x00
+#define VBUS_OVV_SELECT_5P7V		0x08
+#define VBUS_OVV_SELECT_5P8V		0x10
+#define VBUS_OVV_SELECT_5P9V		0x18
+#define VBUS_OVV_SELECT_6P0V		0x20
+#define VBUS_OVV_SELECT_6P1V		0x28
+#define VBUS_OVV_SELECT_6P2V		0x30
+#define VBUS_OVV_SELECT_6P3V		0x38
+
+#define VBUS_AUTO_IN_CURR_LIM_ENA	0x04
+
+/* Fuel Gauge constants */
+#define RESET_ACCU			0x02
+#define READ_REQ			0x01
+#define CC_DEEP_SLEEP_ENA		0x02
+#define CC_PWR_UP_ENA			0x01
+#define CC_SAMPLES_40			0x28
+#define RD_NCONV_ACCU_REQ		0x01
+#define CC_CALIB			0x08
+#define CC_INTAVGOFFSET_ENA		0x10
+#define CC_MUXOFFSET			0x80
+#define CC_INT_CAL_N_AVG_MASK		0x60
+#define CC_INT_CAL_SAMPLES_16		0x40
+#define CC_INT_CAL_SAMPLES_8		0x20
+#define CC_INT_CAL_SAMPLES_4		0x00
+
+/* RTC constants */
+#define RTC_BUP_CH_ENA			0x10
+
+/* BatCtrl Current Source Constants */
+#define BAT_CTRL_7U_ENA			0x01
+#define BAT_CTRL_20U_ENA		0x02
+#define BAT_CTRL_CMP_ENA		0x04
+#define FORCE_BAT_CTRL_CMP_HIGH		0x08
+#define BAT_CTRL_PULL_UP_ENA		0x10
+
+/* Battery type */
+#define BATTERY_UNKNOWN			00
+
+/*
+ * ADC for the battery thermistor.
+ * When using the ADC_THERM_BATCTRL the battery ID resistor is combined with
+ * a NTC resistor to both identify the battery and to measure its temperature.
+ * Different phone manufactures uses different techniques to both identify the
+ * battery and to read its temperature.
+ */
+enum adc_therm {
+	ADC_THERM_BATCTRL,
+	ADC_THERM_BATTEMP,
+};
+
+/**
+ * struct res_to_temp - defines one point in a temp to res curve. To
+ * be used in battery packs that combines the identification resistor with a
+ * NTC resistor.
+ * @temp:			battery pack temperature in Celcius
+ * @resist:			NTC resistor net total resistance
+ */
+struct res_to_temp {
+	int temp;
+	int resist;
+};
+
+/**
+ * struct v_to_cap - Table for translating voltage to capacity
+ * @voltage:		Voltage in mV
+ * @capacity:		Capacity in percent
+ */
+struct v_to_cap {
+	int voltage;
+	int capacity;
+};
+
+/* Forward declaration */
+struct ab8500_fg;
+
+/**
+ * struct ab8500_fg_parameters - Fuel gauge algorithm parameters, in seconds
+ * if not specified
+ * @recovery_sleep_timer:	Time between measurements while recovering
+ * @recovery_total_time:	Total recovery time
+ * @init_timer:			Measurement interval during startup
+ * @init_discard_time:		Time we discard voltage measurement at startup
+ * @init_total_time:		Total init time during startup
+ * @high_curr_time:		Time current has to be high to go to recovery
+ * @accu_charging:		FG accumulation time while charging
+ * @accu_high_curr:		FG accumulation time in high current mode
+ * @high_curr_threshold:	High current threshold, in mA
+ * @lowbat_threshold:		Low battery threshold, in mV
+ */
+struct ab8500_fg_parameters {
+	int recovery_sleep_timer;
+	int recovery_total_time;
+	int init_timer;
+	int init_discard_time;
+	int init_total_time;
+	int high_curr_time;
+	int accu_charging;
+	int accu_high_curr;
+	int high_curr_threshold;
+	int lowbat_threshold;
+};
+
+/**
+ * struct ab8500_charger_maximization - struct used by the board config.
+ * @use_maxi:		Enable maximization for this battery type
+ * @maxi_chg_curr:	Maximum charger current allowed
+ * @maxi_wait_cycles:	cycles to wait before setting charger current
+ * @charger_curr_step	delta between two charger current settings (mA)
+ */
+struct ab8500_maxim_parameters {
+	bool ena_maxi;
+	int chg_curr;
+	int wait_cycles;
+	int charger_curr_step;
+};
+
+/**
+ * struct battery_type - different batteries supported
+ * @name:			battery technology
+ * @resis_high:			battery upper resistance limit
+ * @resis_low:			battery lower resistance limit
+ * @charge_full_design:		Maximum battery capacity in mAh
+ * @nominal_voltage:		Nominal voltage of the battery in mV
+ * @termination_vol:		max voltage upto which battery can be charged
+ * @termination_curr		battery charging termination current in mA
+ * @recharge_vol		battery voltage limit that will trigger a new
+ *				full charging cycle in the case where maintenan-
+ *				-ce charging has been disabled
+ * @normal_cur_lvl:		charger current in normal state in mA
+ * @normal_vol_lvl:		charger voltage in normal state in mV
+ * @maint_a_cur_lvl:		charger current in maintenance A state in mA
+ * @maint_a_vol_lvl:		charger voltage in maintenance A state in mV
+ * @maint_a_chg_timer_h:	charge time in maintenance A state
+ * @maint_b_cur_lvl:		charger current in maintenance B state in mA
+ * @maint_b_vol_lvl:		charger voltage in maintenance B state in mV
+ * @maint_b_chg_timer_h:	charge time in maintenance B state
+ * @low_high_cur_lvl:		charger current in temp low/high state in mA
+ * @low_high_vol_lvl:		charger voltage in temp low/high state in mV'
+ * @battery_resistance:		battery inner resistance in mOhm.
+ * @n_r_t_tbl_elements:		number of elements in r_to_t_tbl
+ * @r_to_t_tbl:			table containing resistance to temp points
+ * @n_v_cap_tbl_elements:	number of elements in v_to_cap_tbl
+ * @v_to_cap_tbl:		Voltage to capacity (in %) table
+ */
+struct battery_type {
+	int name;
+	int resis_high;
+	int resis_low;
+	int charge_full_design;
+	int nominal_voltage;
+	int termination_vol;
+	int termination_curr;
+	int recharge_vol;
+	int normal_cur_lvl;
+	int normal_vol_lvl;
+	int maint_a_cur_lvl;
+	int maint_a_vol_lvl;
+	int maint_a_chg_timer_h;
+	int maint_b_cur_lvl;
+	int maint_b_vol_lvl;
+	int maint_b_chg_timer_h;
+	int low_high_cur_lvl;
+	int low_high_vol_lvl;
+	int battery_resistance;
+	int n_temp_tbl_elements;
+	struct res_to_temp *r_to_t_tbl;
+	int n_v_cap_tbl_elements;
+	struct v_to_cap *v_to_cap_tbl;
+};
+
+/**
+ * struct ab8500_bm_capacity_levels - ab8500 capacity level data
+ * @critical:		critical capacity level in percent
+ * @low:		low capacity level in percent
+ * @normal:		normal capacity level in percent
+ * @high:		high capacity level in percent
+ * @full:		full capacity level in percent
+ */
+struct ab8500_bm_capacity_levels {
+	int critical;
+	int low;
+	int normal;
+	int high;
+	int full;
+};
+
+/**
+ * struct ab8500_bm_charger_parameters - Charger specific parameters
+ * @usb_volt_max:	maximum allowed USB charger voltage in mV
+ * @usb_curr_max:	maximum allowed USB charger current in mA
+ * @ac_volt_max:	maximum allowed AC charger voltage in mV
+ * @ac_curr_max:	maximum allowed AC charger current in mA
+ */
+struct ab8500_bm_charger_parameters {
+	int usb_volt_max;
+	int usb_curr_max;
+	int ac_volt_max;
+	int ac_curr_max;
+};
+
+/**
+ * struct ab8500_bm_data - ab8500 battery management data
+ * @temp_under		under this temp, charging is stopped
+ * @temp_low		between this temp and temp_under charging is reduced
+ * @temp_high		between this temp and temp_over charging is reduced
+ * @temp_over		over this temp, charging is stopped
+ * @main_safety_tmr_h	safety timer for main charger
+ * @usb_safety_tmr_h	safety timer for usb charger
+ * @bkup_bat_v		voltage which we charge the backup battery with
+ * @bkup_bat_i		current which we charge the backup battery with
+ * @no_maintenance	indicates that maintenance charging is disabled
+ * @adc_therm		placement of thermistor, batctrl or battemp adc
+ * @chg_unknown_bat	flag to enable charging of unknown batteries
+ * @enable_overshoot	flag to enable VBAT overshoot control
+ * @fg_res		resistance of FG resistor in mOhm
+ * @n_btypes		number of elements in array bat_type
+ * @batt_id		index of the identified battery in array bat_type
+ * @interval_charging	charge alg cycle period time when charging (sec)
+ * @interval_not_charging charge alg cycle period time when not charging (sec)
+ * @temp_hysteresis	temperature hysteresis
+ * @maxi:		maximization parameters
+ * @cap_levels		capacity in percent for the different capacity levels
+ * @bat_type		table of supported battery types
+ * @chg_params		charger parameters
+ * @fg_params		fuel gauge parameters
+ */
+struct ab8500_bm_data {
+	int temp_under;
+	int temp_low;
+	int temp_high;
+	int temp_over;
+	int main_safety_tmr_h;
+	int usb_safety_tmr_h;
+	int bkup_bat_v;
+	int bkup_bat_i;
+	bool no_maintenance;
+	bool chg_unknown_bat;
+	bool enable_overshoot;
+	enum adc_therm adc_therm;
+	int fg_res;
+	int n_btypes;
+	int batt_id;
+	int interval_charging;
+	int interval_not_charging;
+	int temp_hysteresis;
+	const struct ab8500_maxim_parameters *maxi;
+	const struct ab8500_bm_capacity_levels *cap_levels;
+	const struct battery_type *bat_type;
+	const struct ab8500_bm_charger_parameters *chg_params;
+	const struct ab8500_fg_parameters *fg_params;
+};
+
+struct ab8500_charger_platform_data {
+	char **supplied_to;
+	size_t num_supplicants;
+};
+
+struct ab8500_btemp_platform_data {
+	char **supplied_to;
+	size_t num_supplicants;
+};
+
+struct ab8500_fg_platform_data {
+	char **supplied_to;
+	size_t num_supplicants;
+};
+
+struct ab8500_chargalg_platform_data {
+	char **supplied_to;
+	size_t num_supplicants;
+};
+#endif /* _AB8500_BM_H */
diff --git a/include/linux/mfd/ab8500/denc-regs.h b/include/linux/mfd/ab8500/denc-regs.h
new file mode 100644
index 00000000000..a6683ca7470
--- /dev/null
+++ b/include/linux/mfd/ab8500/denc-regs.h
@@ -0,0 +1,357 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson AB8500 DENC related registers
+ *
+ * Author: Marcus Tunnissen <marcel.tuennissen@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __AB8500_DENC_H
+#define __AB8500_DENC_H
+
+#define AB8500_VAL2REG(__reg, __fld, __val) \
+	(((__val) << __reg##_##__fld##_SHIFT) & __reg##_##__fld##_MASK)
+#define AB8500_REG2VAL(__reg, __fld, __val) \
+	(((__val) & __reg##_##__fld##_MASK) >> __reg##_##__fld##_SHIFT)
+
+#define AB8500_CTRL3 0x00000200
+#define AB8500_CTRL3_TH_SD_ENA_SHIFT 3
+#define AB8500_CTRL3_TH_SD_ENA_MASK 0x00000008
+#define AB8500_CTRL3_TH_SD_ENA(__x) \
+	AB8500_VAL2REG(AB8500_CTRL3, TH_SD_ENA, __x)
+#define AB8500_CTRL3_RESET_DENC_N_SHIFT 2
+#define AB8500_CTRL3_RESET_DENC_N_MASK 0x00000004
+#define AB8500_CTRL3_RESET_DENC_N(__x) \
+	AB8500_VAL2REG(AB8500_CTRL3, RESET_DENC_N, __x)
+#define AB8500_CTRL3_RESET_AUD_N_SHIFT 1
+#define AB8500_CTRL3_RESET_AUD_N_MASK 0x00000002
+#define AB8500_CTRL3_RESET_AUD_N(__x) \
+	AB8500_VAL2REG(AB8500_CTRL3, RESET_AUD_N, __x)
+#define AB8500_CTRL3_CLK_32K_OUT2_IS_SHIFT 0
+#define AB8500_CTRL3_CLK_32K_OUT2_IS_MASK 0x00000001
+#define AB8500_CTRL3_CLK_32K_OUT2_IS(__x) \
+	AB8500_VAL2REG(AB8500_CTRL3, CLK_32K_OUT2_IS, __x)
+#define AB8500_SYS_ULP_CLK_CONF 0x0000020A
+#define AB8500_SYS_ULP_CLK_CONF_CLK_27MHZ_PD_ENA_SHIFT 7
+#define AB8500_SYS_ULP_CLK_CONF_CLK_27MHZ_PD_ENA_MASK 0x00000080
+#define AB8500_SYS_ULP_CLK_CONF_CLK_27MHZ_PD_ENA(__x) \
+	AB8500_VAL2REG(AB8500_SYS_ULP_CLK_CONF, CLK_27MHZ_PD_ENA, __x)
+#define AB8500_SYS_ULP_CLK_CONF_CLK_27MHZ_BUF_ENA_SHIFT 6
+#define AB8500_SYS_ULP_CLK_CONF_CLK_27MHZ_BUF_ENA_MASK 0x00000040
+#define AB8500_SYS_ULP_CLK_CONF_CLK_27MHZ_BUF_ENA(__x) \
+	AB8500_VAL2REG(AB8500_SYS_ULP_CLK_CONF, CLK_27MHZ_BUF_ENA, __x)
+#define AB8500_SYS_ULP_CLK_CONF_ULP_CLK_STRE_SHIFT 5
+#define AB8500_SYS_ULP_CLK_CONF_ULP_CLK_STRE_MASK 0x00000020
+#define AB8500_SYS_ULP_CLK_CONF_ULP_CLK_STRE(__x) \
+	AB8500_VAL2REG(AB8500_SYS_ULP_CLK_CONF, ULP_CLK_STRE, __x)
+#define AB8500_SYS_ULP_CLK_CONF_TVOUT_CLK_INV_SHIFT 4
+#define AB8500_SYS_ULP_CLK_CONF_TVOUT_CLK_INV_MASK 0x00000010
+#define AB8500_SYS_ULP_CLK_CONF_TVOUT_CLK_INV(__x) \
+	AB8500_VAL2REG(AB8500_SYS_ULP_CLK_CONF, TVOUT_CLK_INV, __x)
+#define AB8500_SYS_ULP_CLK_CONF_TVOUT_CLK_DE_IN_SHIFT 3
+#define AB8500_SYS_ULP_CLK_CONF_TVOUT_CLK_DE_IN_MASK 0x00000008
+#define AB8500_SYS_ULP_CLK_CONF_TVOUT_CLK_DE_IN(__x) \
+	AB8500_VAL2REG(AB8500_SYS_ULP_CLK_CONF, TVOUT_CLK_DE_IN, __x)
+#define AB8500_SYS_ULP_CLK_CONF_CLK_27MHZ_STRE_SHIFT 2
+#define AB8500_SYS_ULP_CLK_CONF_CLK_27MHZ_STRE_MASK 0x00000004
+#define AB8500_SYS_ULP_CLK_CONF_CLK_27MHZ_STRE(__x) \
+	AB8500_VAL2REG(AB8500_SYS_ULP_CLK_CONF, CLK_27MHZ_STRE, __x)
+#define AB8500_SYS_ULP_CLK_CONF_ULP_CLK_CONF_SHIFT 0
+#define AB8500_SYS_ULP_CLK_CONF_ULP_CLK_CONF_MASK 0x00000003
+#define AB8500_SYS_ULP_CLK_CONF_ULP_CLK_CONF_NO_FUNC 0
+#define AB8500_SYS_ULP_CLK_CONF_ULP_CLK_CONF_AS_OUTPUT 1
+#define AB8500_SYS_ULP_CLK_CONF_ULP_CLK_CONF_AS_INPUT 2
+#define AB8500_SYS_ULP_CLK_CONF_ULP_CLK_CONF(__x) \
+	AB8500_VAL2REG(AB8500_SYS_ULP_CLK_CONF, ULP_CLK_CONF, \
+	AB8500_SYS_ULP_CLK_CONF_ULP_CLK_CONF_##__x)
+#define AB8500_SYS_CLK_CTRL 0x0000020C
+#define AB8500_SYS_CLK_CTRL_USB_CLK_VALID_SHIFT 2
+#define AB8500_SYS_CLK_CTRL_USB_CLK_VALID_MASK 0x00000004
+#define AB8500_SYS_CLK_CTRL_USB_CLK_VALID(__x) \
+	AB8500_VAL2REG(AB8500_SYS_CLK_CTRL, USB_CLK_VALID, __x)
+#define AB8500_SYS_CLK_CTRL_TVOUT_CLK_VALID_SHIFT 1
+#define AB8500_SYS_CLK_CTRL_TVOUT_CLK_VALID_MASK 0x00000002
+#define AB8500_SYS_CLK_CTRL_TVOUT_CLK_VALID(__x) \
+	AB8500_VAL2REG(AB8500_SYS_CLK_CTRL, TVOUT_CLK_VALID, __x)
+#define AB8500_SYS_CLK_CTRL_TVOUT_PLL_ENA_SHIFT 0
+#define AB8500_SYS_CLK_CTRL_TVOUT_PLL_ENA_MASK 0x00000001
+#define AB8500_SYS_CLK_CTRL_TVOUT_PLL_ENA(__x) \
+	AB8500_VAL2REG(AB8500_SYS_CLK_CTRL, TVOUT_PLL_ENA, __x)
+#define AB8500_REGU_MISC1 0x00000380
+#define AB8500_REGU_MISC1_V_TVOUT_LP_SHIFT 7
+#define AB8500_REGU_MISC1_V_TVOUT_LP_MASK 0x00000080
+#define AB8500_REGU_MISC1_V_TVOUT_LP(__x) \
+	AB8500_VAL2REG(AB8500_REGU_MISC1, V_TVOUT_LP, __x)
+#define AB8500_REGU_MISC1_V_INT_CORE_12_LP_SHIFT 6
+#define AB8500_REGU_MISC1_V_INT_CORE_12_LP_MASK 0x00000040
+#define AB8500_REGU_MISC1_V_INT_CORE_12_LP(__x) \
+	AB8500_VAL2REG(AB8500_REGU_MISC1, V_INT_CORE_12_LP, __x)
+#define AB8500_REGU_MISC1_V_INT_CORE_12_SEL_SHIFT 3
+#define AB8500_REGU_MISC1_V_INT_CORE_12_SEL_MASK 0x00000038
+#define AB8500_REGU_MISC1_V_INT_CORE_12_SEL_1_2V 0
+#define AB8500_REGU_MISC1_V_INT_CORE_12_SEL_1_225V 1
+#define AB8500_REGU_MISC1_V_INT_CORE_12_SEL_1_25V 2
+#define AB8500_REGU_MISC1_V_INT_CORE_12_SEL_1_275V 3
+#define AB8500_REGU_MISC1_V_INT_CORE_12_SEL_1_3V 4
+#define AB8500_REGU_MISC1_V_INT_CORE_12_SEL_1_325V 5
+#define AB8500_REGU_MISC1_V_INT_CORE_12_SEL_1_35V 6
+#define AB8500_REGU_MISC1_V_INT_CORE_12_SEL(__x) \
+	AB8500_VAL2REG(AB8500_REGU_MISC1, V_INT_CORE_12_SEL, \
+	AB8500_REGU_MISC1_V_INT_CORE_12_SEL_##__x)
+#define AB8500_REGU_MISC1_V_INT_CORE_12_ENA_SHIFT 2
+#define AB8500_REGU_MISC1_V_INT_CORE_12_ENA_MASK 0x00000004
+#define AB8500_REGU_MISC1_V_INT_CORE_12_ENA(__x) \
+	AB8500_VAL2REG(AB8500_REGU_MISC1, V_INT_CORE_12_ENA, __x)
+#define AB8500_REGU_MISC1_V_TVOUT_ENA_SHIFT 1
+#define AB8500_REGU_MISC1_V_TVOUT_ENA_MASK 0x00000002
+#define AB8500_REGU_MISC1_V_TVOUT_ENA(__x) \
+	AB8500_VAL2REG(AB8500_REGU_MISC1, V_TVOUT_ENA, __x)
+#define AB8500_VAUX12_REGU 0x00000409
+#define AB8500_VAUX12_REGU_VAUX_1_SHIFT 2
+#define AB8500_VAUX12_REGU_VAUX_1_MASK 0x0000000C
+#define AB8500_VAUX12_REGU_VAUX_1_DISABLE 0
+#define AB8500_VAUX12_REGU_VAUX_1_FORCE_HP 1
+#define AB8500_VAUX12_REGU_VAUX_1_BY_CTRL_REG 2
+#define AB8500_VAUX12_REGU_VAUX_1_FORCE_LP 3
+#define AB8500_VAUX12_REGU_VAUX_1(__x) \
+	AB8500_VAL2REG(AB8500_VAUX12_REGU, VAUX_1, \
+	AB8500_VAUX12_REGU_VAUX_1_##__x)
+#define AB8500_VAUX12_REGU_VAUX_2_SHIFT 0
+#define AB8500_VAUX12_REGU_VAUX_2_MASK 0x00000003
+#define AB8500_VAUX12_REGU_VAUX_2_DISABLE 0
+#define AB8500_VAUX12_REGU_VAUX_2_FORCE_HP 1
+#define AB8500_VAUX12_REGU_VAUX_2_BY_CTRL_REG 2
+#define AB8500_VAUX12_REGU_VAUX_2_FORCE_LP 3
+#define AB8500_VAUX12_REGU_VAUX_2(__x) \
+	AB8500_VAL2REG(AB8500_VAUX12_REGU, VAUX_2, \
+	AB8500_VAUX12_REGU_VAUX_2_##__x)
+#define AB8500_VAUX1_SEL 0x0000041F
+#define AB8500_VAUX1_SEL_VAL_SHIFT 0
+#define AB8500_VAUX1_SEL_VAL_MASK 0x0000000F
+#define AB8500_VAUX1_SEL_VAL_1_1V 0
+#define AB8500_VAUX1_SEL_VAL_1_2V 1
+#define AB8500_VAUX1_SEL_VAL_1_3V 2
+#define AB8500_VAUX1_SEL_VAL_1_4V 3
+#define AB8500_VAUX1_SEL_VAL_1_5V 4
+#define AB8500_VAUX1_SEL_VAL_1_8V 5
+#define AB8500_VAUX1_SEL_VAL_1_85V 6
+#define AB8500_VAUX1_SEL_VAL_1_9V 7
+#define AB8500_VAUX1_SEL_VAL_2_5V 8
+#define AB8500_VAUX1_SEL_VAL_2_65V 9
+#define AB8500_VAUX1_SEL_VAL_2_7V 10
+#define AB8500_VAUX1_SEL_VAL_2_75V 11
+#define AB8500_VAUX1_SEL_VAL_2_8V 12
+#define AB8500_VAUX1_SEL_VAL_2_9V 13
+#define AB8500_VAUX1_SEL_VAL_3_0V 14
+#define AB8500_VAUX1_SEL_VAL_3_3V 15
+#define AB8500_VAUX1_SEL_VAL(__x) \
+	AB8500_VAL2REG(AB8500_VAUX1_SEL, VAL, AB8500_VAUX1_SEL_VAL_##__x)
+#define AB8500_DENC_CONF0 0x00000600
+#define AB8500_DENC_CONF0_STD_SHIFT 6
+#define AB8500_DENC_CONF0_STD_MASK 0x000000C0
+#define AB8500_DENC_CONF0_STD_PAL_BDGHI 0
+#define AB8500_DENC_CONF0_STD_PAL_N 1
+#define AB8500_DENC_CONF0_STD_NTSC_M 2
+#define AB8500_DENC_CONF0_STD_PAL_M 3
+#define AB8500_DENC_CONF0_STD(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF0, STD, AB8500_DENC_CONF0_STD_##__x)
+#define AB8500_DENC_CONF0_SYNC_SHIFT 3
+#define AB8500_DENC_CONF0_SYNC_MASK 0x00000038
+#define AB8500_DENC_CONF0_SYNC_F_BASED_SLAVE 1
+#define AB8500_DENC_CONF0_SYNC_AUTO_TEST 7
+#define AB8500_DENC_CONF0_SYNC(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF0, SYNC, AB8500_DENC_CONF0_SYNC_##__x)
+#define AB8500_DENC_CONF1 0x00000601
+#define AB8500_DENC_CONF1_BLK_LI_SHIFT 7
+#define AB8500_DENC_CONF1_BLK_LI_MASK 0x00000080
+#define AB8500_DENC_CONF1_BLK_LI_PARTIAL 0
+#define AB8500_DENC_CONF1_BLK_LI_FULL 1
+#define AB8500_DENC_CONF1_BLK_LI(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF1, BLK_LI, \
+	AB8500_DENC_CONF1_BLK_LI_##__x)
+#define AB8500_DENC_CONF1_FLT_SHIFT 5
+#define AB8500_DENC_CONF1_FLT_MASK 0x00000060
+#define AB8500_DENC_CONF1_FLT_1_1MHZ 0
+#define AB8500_DENC_CONF1_FLT_1_3MHZ 1
+#define AB8500_DENC_CONF1_FLT_1_6MHZ 2
+#define AB8500_DENC_CONF1_FLT_1_9MHZ 3
+#define AB8500_DENC_CONF1_FLT(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF1, FLT, AB8500_DENC_CONF1_FLT_##__x)
+#define AB8500_DENC_CONF1_CO_KI_SHIFT 3
+#define AB8500_DENC_CONF1_CO_KI_MASK 0x00000008
+#define AB8500_DENC_CONF1_CO_KI(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF1, CO_KI, __x)
+#define AB8500_DENC_CONF1_SETUP_MAIN_SHIFT 2
+#define AB8500_DENC_CONF1_SETUP_MAIN_MASK 0x00000004
+#define AB8500_DENC_CONF1_SETUP_MAIN_BLACK_EQ_BLANK 0
+#define AB8500_DENC_CONF1_SETUP_MAIN_BLACK_GT_BLANK 1
+#define AB8500_DENC_CONF1_SETUP_MAIN(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF1, SETUP_MAIN, \
+	AB8500_DENC_CONF1_SETUP_MAIN_##__x)
+#define AB8500_DENC_CONF1_CC_SHIFT 0
+#define AB8500_DENC_CONF1_CC_MASK 0x00000003
+#define AB8500_DENC_CONF1_CC_NONE 0
+#define AB8500_DENC_CONF1_CC_FIELD_1 1
+#define AB8500_DENC_CONF1_CC_FIELD_2 2
+#define AB8500_DENC_CONF1_CC_ALL 3
+#define AB8500_DENC_CONF1_CC(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF1, CC, AB8500_DENC_CONF1_CC_##__x)
+#define AB8500_DENC_CONF2 0x00000602
+#define AB8500_DENC_CONF2_N_INTRL_SHIFT 7
+#define AB8500_DENC_CONF2_N_INTRL_MASK 0x00000080
+#define AB8500_DENC_CONF2_N_INTRL(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF2, N_INTRL, __x)
+#define AB8500_DENC_CONF2_EN_RST_SHIFT 6
+#define AB8500_DENC_CONF2_EN_RST_MASK 0x00000040
+#define AB8500_DENC_CONF2_EN_RST(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF2, EN_RST, __x)
+#define AB8500_DENC_CONF2_BURST_EN_SHIFT 5
+#define AB8500_DENC_CONF2_BURST_EN_MASK 0x00000020
+#define AB8500_DENC_CONF2_BURST_EN(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF2, BURST_EN, __x)
+#define AB8500_DENC_CONF2_SEL_RST_SHIFT 4
+#define AB8500_DENC_CONF2_SEL_RST_MASK 0x00000010
+#define AB8500_DENC_CONF2_SEL_RST_USE_HW_VAL 0
+#define AB8500_DENC_CONF2_SEL_RST_USE_PROG_VAL 1
+#define AB8500_DENC_CONF2_SEL_RST(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF2, SEL_RST, \
+	AB8500_DENC_CONF2_SEL_RST_##__x)
+#define AB8500_DENC_CONF2_RST_OSC_BUF_SHIFT 2
+#define AB8500_DENC_CONF2_RST_OSC_BUF_MASK 0x00000004
+#define AB8500_DENC_CONF2_RST_OSC_BUF(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF2, RST_OSC_BUF, __x)
+#define AB8500_DENC_CONF2_VAL_RST_SHIFT 0
+#define AB8500_DENC_CONF2_VAL_RST_MASK 0x00000003
+#define AB8500_DENC_CONF2_VAL_RST_ALL_LINES 0
+#define AB8500_DENC_CONF2_VAL_RST_EVERY_2ND_FIELD 1
+#define AB8500_DENC_CONF2_VAL_RST_EVERY_4TH_FIELD 2
+#define AB8500_DENC_CONF2_VAL_RST_EVERY_8TH_FIELD 3
+#define AB8500_DENC_CONF2_VAL_RST(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF2, VAL_RST, \
+	AB8500_DENC_CONF2_VAL_RST_##__x)
+#define AB8500_DENC_CONF6 0x00000606
+#define AB8500_DENC_CONF6_SOFT_RESET_SHIFT 7
+#define AB8500_DENC_CONF6_SOFT_RESET_MASK 0x00000080
+#define AB8500_DENC_CONF6_SOFT_RESET(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF6, SOFT_RESET, __x)
+#define AB8500_DENC_CONF6_JUMP_SHIFT 6
+#define AB8500_DENC_CONF6_JUMP_MASK 0x00000040
+#define AB8500_DENC_CONF6_JUMP(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF6, JUMP, __x)
+#define AB8500_DENC_CONF6_DEC_NINC_SHIFT 5
+#define AB8500_DENC_CONF6_DEC_NINC_MASK 0x00000020
+#define AB8500_DENC_CONF6_DEC_NINC(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF6, DEC_NINC, __x)
+#define AB8500_DENC_CONF6_FREE_JUMP_SHIFT 4
+#define AB8500_DENC_CONF6_FREE_JUMP_MASK 0x00000010
+#define AB8500_DENC_CONF6_FREE_JUMP(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF6, FREE_JUMP, __x)
+#define AB8500_DENC_CONF6_MAX_DYN_SHIFT 0
+#define AB8500_DENC_CONF6_MAX_DYN_MASK 0x00000001
+#define AB8500_DENC_CONF6_MAX_DYN(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF6, MAX_DYN, __x)
+#define AB8500_DENC_CONF8 0x00000608
+#define AB8500_DENC_CONF8_PH_RST_MODE_SHIFT 6
+#define AB8500_DENC_CONF8_PH_RST_MODE_MASK 0x000000C0
+#define AB8500_DENC_CONF8_PH_RST_MODE_DISABLED 0
+#define AB8500_DENC_CONF8_PH_RST_MODE_UPDATE_FROM_PHASE_BUF 1
+#define AB8500_DENC_CONF8_PH_RST_MODE_UPDATE_FROM_INC_DFS 2
+#define AB8500_DENC_CONF8_PH_RST_MODE_RESET 3
+#define AB8500_DENC_CONF8_PH_RST_MODE(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF8, PH_RST_MODE, \
+	AB8500_DENC_CONF8_PH_RST_MODE_##__x)
+#define AB8500_DENC_CONF8_VAL_422_MUX_SHIFT 4
+#define AB8500_DENC_CONF8_VAL_422_MUX_MASK 0x00000010
+#define AB8500_DENC_CONF8_VAL_422_MUX_TEST 0
+#define AB8500_DENC_CONF8_VAL_422_MUX_ACTIVE 1
+#define AB8500_DENC_CONF8_VAL_422_MUX(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF8, VAL_422_MUX, \
+	AB8500_DENC_CONF8_VAL_422_MUX_##__x)
+#define AB8500_DENC_CONF8_BLK_ALL_SHIFT 3
+#define AB8500_DENC_CONF8_BLK_ALL_MASK 0x00000008
+#define AB8500_DENC_CONF8_BLK_ALL(__x) \
+	AB8500_VAL2REG(AB8500_DENC_CONF8, BLK_ALL, __x)
+#define AB8500_TVOUT_CTRL 0x00000680
+#define AB8500_TVOUT_CTRL_TV_LOAD_RC_SHIFT 6
+#define AB8500_TVOUT_CTRL_TV_LOAD_RC_MASK 0x00000040
+#define AB8500_TVOUT_CTRL_TV_LOAD_RC(__x) \
+	AB8500_VAL2REG(AB8500_TVOUT_CTRL, TV_LOAD_RC, __x)
+#define AB8500_TVOUT_CTRL_PLUG_TV_TIME_SHIFT 3
+#define AB8500_TVOUT_CTRL_PLUG_TV_TIME_MASK 0x00000038
+#define AB8500_TVOUT_CTRL_PLUG_TV_TIME_0_5S 0
+#define AB8500_TVOUT_CTRL_PLUG_TV_TIME_1S 0
+#define AB8500_TVOUT_CTRL_PLUG_TV_TIME_1_5S 0
+#define AB8500_TVOUT_CTRL_PLUG_TV_TIME_2S 0
+#define AB8500_TVOUT_CTRL_PLUG_TV_TIME_2_5S 0
+#define AB8500_TVOUT_CTRL_PLUG_TV_TIME_3S 0
+#define AB8500_TVOUT_CTRL_PLUG_TV_TIME(__x) \
+	AB8500_VAL2REG(AB8500_TVOUT_CTRL, PLUG_TV_TIME, \
+	AB8500_TVOUT_CTRL_PLUG_TV_TIME_##__x)
+#define AB8500_TVOUT_CTRL_TV_PLUG_ON_SHIFT 2
+#define AB8500_TVOUT_CTRL_TV_PLUG_ON_MASK 0x00000004
+#define AB8500_TVOUT_CTRL_TV_PLUG_ON(__x) \
+	AB8500_VAL2REG(AB8500_TVOUT_CTRL, TV_PLUG_ON, __x)
+#define AB8500_TVOUT_CTRL_DAC_CTRL0_SHIFT 1
+#define AB8500_TVOUT_CTRL_DAC_CTRL0_MASK 0x00000002
+#define AB8500_TVOUT_CTRL_DAC_CTRL0(__x) \
+	AB8500_VAL2REG(AB8500_TVOUT_CTRL, DAC_CTRL0, __x)
+#define AB8500_TVOUT_CTRL_DAC_CTRL1_SHIFT 0
+#define AB8500_TVOUT_CTRL_DAC_CTRL1_MASK 0x00000001
+#define AB8500_TVOUT_CTRL_DAC_CTRL1(__x) \
+	AB8500_VAL2REG(AB8500_TVOUT_CTRL, DAC_CTRL1, __x)
+#define AB8500_TVOUT_CTRL2 0x00000681
+#define AB8500_TVOUT_CTRL2_SWAP_DDR_DATA_IN_SHIFT 1
+#define AB8500_TVOUT_CTRL2_SWAP_DDR_DATA_IN_MASK 0x00000002
+#define AB8500_TVOUT_CTRL2_SWAP_DDR_DATA_IN(__x) \
+	AB8500_VAL2REG(AB8500_TVOUT_CTRL2, SWAP_DDR_DATA_IN, __x)
+#define AB8500_TVOUT_CTRL2_DENC_DDR_SHIFT 0
+#define AB8500_TVOUT_CTRL2_DENC_DDR_MASK 0x00000001
+#define AB8500_TVOUT_CTRL2_DENC_DDR(__x) \
+	AB8500_VAL2REG(AB8500_TVOUT_CTRL2, DENC_DDR, __x)
+#define AB8500_IT_MASK1 0x00000E40
+#define AB8500_IT_MASK1_PON_KEY1_DBR_SHIFT 7
+#define AB8500_IT_MASK1_PON_KEY1_DBR_MASK 0x00000080
+#define AB8500_IT_MASK1_PON_KEY1_DBR(__x) \
+	AB8500_VAL2REG(AB8500_IT_MASK1, PON_KEY1_DBR, __x)
+#define AB8500_IT_MASK1_PON_KEY1_DBF_SHIFT 6
+#define AB8500_IT_MASK1_PON_KEY1_DBF_MASK 0x00000040
+#define AB8500_IT_MASK1_PON_KEY1_DBF(__x) \
+	AB8500_VAL2REG(AB8500_IT_MASK1, PON_KEY1_DBF, __x)
+#define AB8500_IT_MASK1_PON_KEY2_DBR_SHIFT 5
+#define AB8500_IT_MASK1_PON_KEY2_DBR_MASK 0x00000020
+#define AB8500_IT_MASK1_PON_KEY2_DBR(__x) \
+	AB8500_VAL2REG(AB8500_IT_MASK1, PON_KEY2_DBR, __x)
+#define AB8500_IT_MASK1_PON_KEY2_DBF_SHIFT 4
+#define AB8500_IT_MASK1_PON_KEY2_DBF_MASK 0x00000010
+#define AB8500_IT_MASK1_PON_KEY2_DBF(__x) \
+	AB8500_VAL2REG(AB8500_IT_MASK1, PON_KEY2_DBF, __x)
+#define AB8500_IT_MASK1_TEMP_WARN_SHIFT 3
+#define AB8500_IT_MASK1_TEMP_WARN_MASK 0x00000008
+#define AB8500_IT_MASK1_TEMP_WARN(__x) \
+	AB8500_VAL2REG(AB8500_IT_MASK1, TEMP_WARN, __x)
+#define AB8500_IT_MASK1_PLUG_TV_DET_SHIFT 2
+#define AB8500_IT_MASK1_PLUG_TV_DET_MASK 0x00000004
+#define AB8500_IT_MASK1_PLUG_TV_DET(__x) \
+	AB8500_VAL2REG(AB8500_IT_MASK1, PLUG_TV_DET, __x)
+#define AB8500_IT_MASK1_UNPLUG_TV_DET_SHIFT 1
+#define AB8500_IT_MASK1_UNPLUG_TV_DET_MASK 0x00000002
+#define AB8500_IT_MASK1_UNPLUG_TV_DET(__x) \
+	AB8500_VAL2REG(AB8500_IT_MASK1, UNPLUG_TV_DET, __x)
+#define AB8500_IT_MASK1_MAIN_EXT_CH_NOK_SHIFT 0
+#define AB8500_IT_MASK1_MAIN_EXT_CH_NOK_MASK 0x00000001
+#define AB8500_IT_MASK1_MAIN_EXT_CH_NOK(__x) \
+	AB8500_VAL2REG(AB8500_IT_MASK1, MAIN_EXT_CH_NOK, __x)
+#define AB8500_REV 0x00001080
+#define AB8500_REV_FULL_MASK_SHIFT 4
+#define AB8500_REV_FULL_MASK_MASK 0x000000F0
+#define AB8500_REV_FULL_MASK(__x) \
+	AB8500_VAL2REG(AB8500_REV, FULL_MASK, __x)
+#define AB8500_REV_METAL_FIX_SHIFT 0
+#define AB8500_REV_METAL_FIX_MASK 0x0000000F
+#define AB8500_REV_METAL_FIX(__x) \
+	AB8500_VAL2REG(AB8500_REV, METAL_FIX, __x)
+
+#endif /* __AB8500_DENC_H */
diff --git a/include/linux/mfd/ab8500/denc.h b/include/linux/mfd/ab8500/denc.h
new file mode 100644
index 00000000000..25a09a2c2bd
--- /dev/null
+++ b/include/linux/mfd/ab8500/denc.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * AB8500 tvout driver interface
+ *
+ * Author: Marcel Tunnissen <marcel.tuennissen@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __AB8500_DENC__H__
+#define __AB8500_DENC__H__
+
+#include <linux/platform_device.h>
+
+struct ab8500_denc_platform_data {
+	/* Platform info */
+	bool ddr_enable;
+	bool ddr_little_endian;
+};
+
+enum ab8500_denc_TV_std {
+	TV_STD_PAL_BDGHI,
+	TV_STD_PAL_N,
+	TV_STD_PAL_M,
+	TV_STD_NTSC_M,
+};
+
+enum ab8500_denc_cr_filter_bandwidth {
+	TV_CR_NTSC_LOW_DEF_FILTER,
+	TV_CR_PAL_LOW_DEF_FILTER,
+	TV_CR_NTSC_HIGH_DEF_FILTER,
+	TV_CR_PAL_HIGH_DEF_FILTER,
+};
+
+enum ab8500_denc_phase_reset_mode {
+	TV_PHASE_RST_MOD_DISABLE,
+	TV_PHASE_RST_MOD_FROM_PHASE_BUF,
+	TV_PHASE_RST_MOD_FROM_INC_DFS,
+	TV_PHASE_RST_MOD_RST,
+};
+
+enum ab8500_denc_plug_time {
+	TV_PLUG_TIME_0_5S,
+	TV_PLUG_TIME_1S,
+	TV_PLUG_TIME_1_5S,
+	TV_PLUG_TIME_2S,
+	TV_PLUG_TIME_2_5S,
+	TV_PLUG_TIME_3S,
+};
+
+struct ab8500_denc_conf {
+	/* register settings for DENC_configuration */
+	bool					act_output;
+	enum ab8500_denc_TV_std			TV_std;
+	bool					progressive;
+	bool					test_pattern;
+	bool					partial_blanking;
+	bool					blank_all;
+	bool					black_level_setup;
+	enum ab8500_denc_cr_filter_bandwidth	cr_filter;
+	bool					suppress_col;
+	enum ab8500_denc_phase_reset_mode	phase_reset_mode;
+	bool					dac_enable;
+	bool					act_dc_output;
+};
+
+struct platform_device *ab8500_denc_get_device(void);
+void ab8500_denc_put_device(struct platform_device *pdev);
+
+void ab8500_denc_reset(struct platform_device *pdev, bool hard);
+void ab8500_denc_power_up(struct platform_device *pdev);
+void ab8500_denc_power_down(struct platform_device *pdev);
+
+void ab8500_denc_conf(struct platform_device *pdev,
+						struct ab8500_denc_conf *conf);
+void ab8500_denc_conf_plug_detect(struct platform_device *pdev,
+					bool enable, bool load_RC,
+					enum ab8500_denc_plug_time time);
+void ab8500_denc_mask_int_plug_det(struct platform_device *pdev, bool plug,
+								bool unplug);
+#endif /* __AB8500_DENC__H__ */
diff --git a/include/linux/mfd/ab8500/gpadc.h b/include/linux/mfd/ab8500/gpadc.h
index 46b954011f1..fa706c5a04a 100644
--- a/include/linux/mfd/ab8500/gpadc.h
+++ b/include/linux/mfd/ab8500/gpadc.h
@@ -26,7 +26,10 @@
 
 struct ab8500_gpadc;
 
-struct ab8500_gpadc *ab8500_gpadc_get(char *name);
-int ab8500_gpadc_convert(struct ab8500_gpadc *gpadc, u8 input);
+struct ab8500_gpadc *ab8500_gpadc_get(void);
+int ab8500_gpadc_convert(struct ab8500_gpadc *gpadc, u8 channel);
+int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel);
+int ab8500_gpadc_ad_to_voltage(struct ab8500_gpadc *gpadc,
+    u8 channel, int ad_value);
 
 #endif /* _AB8500_GPADC_H */
diff --git a/include/linux/mfd/ab8500/gpio.h b/include/linux/mfd/ab8500/gpio.h
index 488a8c920a2..76d5ee22c60 100644
--- a/include/linux/mfd/ab8500/gpio.h
+++ b/include/linux/mfd/ab8500/gpio.h
@@ -15,7 +15,19 @@
 struct ab8500_gpio_platform_data {
 	int gpio_base;
 	u32 irq_base;
-	u8  config_reg[7];
+	u8  initial_pin_config[7];
+	u8  initial_pin_direction[6];
+	u8  initial_pin_pullups[6];
 };
 
+int ab8500_config_pull_up_or_down(struct device *dev,
+				unsigned ab8500_gpio_offset, bool enable);
+
+int ab8500_gpio_config_select(struct device *dev,
+			unsigned ab8500_gpio_offset, bool gpio_select);
+
+int ab8500_gpio_config_get_select(struct device *dev,
+			unsigned ab8500_gpio_offset, bool *gpio_select);
+
 #endif /* _AB8500_GPIO_H */
+
diff --git a/include/linux/mfd/ab8500/ux500_chargalg.h b/include/linux/mfd/ab8500/ux500_chargalg.h
new file mode 100644
index 00000000000..f04e47ff56a
--- /dev/null
+++ b/include/linux/mfd/ab8500/ux500_chargalg.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Johan Gardsmark <johan.gardsmark@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#ifndef _UX500_CHARGALG_H
+#define _UX500_CHARGALG_H
+
+#include <linux/power_supply.h>
+
+#define psy_to_ux500_charger(x) container_of((x), \
+		struct ux500_charger, psy)
+
+/* Forward declaration */
+struct ux500_charger;
+
+struct ux500_charger_ops {
+	int (*enable) (struct ux500_charger *, int, int, int);
+	int (*kick_wd) (struct ux500_charger *);
+	int (*update_curr) (struct ux500_charger *, int);
+};
+
+/**
+ * struct ux500_charger - power supply ux500 charger sub class
+ * @psy			power supply base class
+ * @ops			ux500 charger operations
+ * @max_out_volt	maximum output charger voltage in mV
+ * @max_out_curr	maximum output charger current in mA
+ */
+struct ux500_charger {
+	struct power_supply psy;
+	struct ux500_charger_ops ops;
+	int max_out_volt;
+	int max_out_curr;
+};
+
+#endif
diff --git a/include/linux/mfd/abx500.h b/include/linux/mfd/abx500.h
index 896b5e47f16..7a1d3179f6a 100644
--- a/include/linux/mfd/abx500.h
+++ b/include/linux/mfd/abx500.h
@@ -1,12 +1,13 @@
 /*
- * Copyright (C) 2007-2009 ST-Ericsson AB
- * License terms: GNU General Public License (GPL) version 2
+ * Copyright (C) ST-Ericsson SA 2010
+ * License terms: GNU General Public License v2
  * AB3100 core access functions
  * Author: Linus Walleij <linus.walleij@stericsson.com>
  *
  * ABX500 core access functions.
  * The abx500 interface is used for the Analog Baseband chip
- * ab3100, ab3550, ab5500, and ab8500.
+ * ab3100, ab3550, ab5500 and possibly comming. It is not used for
+ * ab4500 and ab8500 since they are another family of chip.
  *
  * Author: Mattias Wallin <mattias.wallin@stericsson.com>
  * Author: Mattias Nilsson <mattias.i.nilsson@stericsson.com>
@@ -31,8 +32,15 @@
 #define AB3100_R2B	0xc8
 #define AB3550_P1A	0x10
 #define AB5500_1_0	0x20
-#define AB5500_2_0	0x21
-#define AB5500_2_1	0x22
+#define AB5500_1_1	0x21
+#define AB5500_2_0	0x24
+
+/* AB8500 CIDs*/
+#define AB8500_CUTEARLY	0x00
+#define AB8500_CUT1P0	0x10
+#define AB8500_CUT1P1	0x11
+#define AB8500_CUT2P0	0x20
+#define AB8500_CUT3P0	0x30
 
 /* AB8500 CIDs*/
 #define AB8500_CUTEARLY	0x00
@@ -198,6 +206,158 @@ struct ab3550_platform_data {
 	unsigned int init_settings_sz;
 };
 
+/**
+ *
+ * ab5500
+ *
+ */
+
+enum ab5500_devid {
+	AB5500_DEVID_ADC,
+	AB5500_DEVID_LEDS,
+	AB5500_DEVID_POWER,
+	AB5500_DEVID_REGULATORS,
+	AB5500_DEVID_SIM,
+	AB5500_DEVID_RTC,
+	AB5500_DEVID_CHARGER,
+	AB5500_DEVID_FG,
+	AB5500_DEVID_VIBRATOR,
+	AB5500_DEVID_CODEC,
+	AB5500_DEVID_USB,
+	AB5500_DEVID_OTP,
+	AB5500_DEVID_VIDEO,
+	AB5500_DEVID_DBIECI,
+	AB5500_DEVID_ONSWA,
+	AB5500_DEVID_CHARGALG,
+	AB5500_DEVID_BTEMP,
+	AB5500_NUM_DEVICES,
+};
+
+enum ab5500_banks {
+	AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP = 0,
+	AB5500_BANK_VDDDIG_IO_I2C_CLK_TST = 1,
+	AB5500_BANK_VDENC = 2,
+	AB5500_BANK_SIM_USBSIM  = 3,
+	AB5500_BANK_LED = 4,
+	AB5500_BANK_ADC  = 5,
+	AB5500_BANK_RTC  = 6,
+	AB5500_BANK_STARTUP  = 7,
+	AB5500_BANK_DBI_ECI  = 8,
+	AB5500_BANK_CHG  = 9,
+	AB5500_BANK_FG_BATTCOM_ACC = 10,
+	AB5500_BANK_USB = 11,
+	AB5500_BANK_IT = 12,
+	AB5500_BANK_VIBRA = 13,
+	AB5500_BANK_AUDIO_HEADSETUSB = 14,
+	AB5500_NUM_BANKS = 15,
+};
+
+enum ab5500_banks_addr {
+	AB5500_ADDR_VIT_IO_I2C_CLK_TST_OTP = 0x4A,
+	AB5500_ADDR_VDDDIG_IO_I2C_CLK_TST = 0x4B,
+	AB5500_ADDR_VDENC = 0x06,
+	AB5500_ADDR_SIM_USBSIM  = 0x04,
+	AB5500_ADDR_LED = 0x10,
+	AB5500_ADDR_ADC  = 0x0A,
+	AB5500_ADDR_RTC  = 0x0F,
+	AB5500_ADDR_STARTUP  = 0x03,
+	AB5500_ADDR_DBI_ECI  = 0x07,
+	AB5500_ADDR_CHG  = 0x0B,
+	AB5500_ADDR_FG_BATTCOM_ACC = 0x0C,
+	AB5500_ADDR_USB = 0x05,
+	AB5500_ADDR_IT = 0x0E,
+	AB5500_ADDR_VIBRA = 0x02,
+	AB5500_ADDR_AUDIO_HEADSETUSB = 0x0D,
+};
+
+/*
+ * Interrupt register offsets
+ * Bank : 0x0E
+ */
+#define AB5500_IT_SOURCE0_REG		0x20
+#define AB5500_IT_SOURCE1_REG		0x21
+#define AB5500_IT_SOURCE2_REG		0x22
+#define AB5500_IT_SOURCE3_REG		0x23
+#define AB5500_IT_SOURCE4_REG		0x24
+#define AB5500_IT_SOURCE5_REG		0x25
+#define AB5500_IT_SOURCE6_REG		0x26
+#define AB5500_IT_SOURCE7_REG		0x27
+#define AB5500_IT_SOURCE8_REG		0x28
+#define AB5500_IT_SOURCE9_REG		0x29
+#define AB5500_IT_SOURCE10_REG		0x2A
+#define AB5500_IT_SOURCE11_REG		0x2B
+#define AB5500_IT_SOURCE12_REG		0x2C
+#define AB5500_IT_SOURCE13_REG		0x2D
+#define AB5500_IT_SOURCE14_REG		0x2E
+#define AB5500_IT_SOURCE15_REG		0x2F
+#define AB5500_IT_SOURCE16_REG		0x30
+#define AB5500_IT_SOURCE17_REG		0x31
+#define AB5500_IT_SOURCE18_REG		0x32
+#define AB5500_IT_SOURCE19_REG		0x33
+#define AB5500_IT_SOURCE20_REG		0x34
+#define AB5500_IT_SOURCE21_REG		0x35
+#define AB5500_IT_SOURCE22_REG		0x36
+#define AB5500_IT_SOURCE23_REG		0x37
+
+#define AB5500_NUM_IRQ_REGS		23
+
+/**
+ * struct ab5500
+ * @access_mutex: lock out concurrent accesses to the AB registers
+ * @dev: a pointer to the device struct for this chip driver
+ * @ab5500_irq: the analog baseband irq
+ * @irq_base: the platform configuration irq base for subdevices
+ * @chip_name: name of this chip variant
+ * @chip_id: 8 bit chip ID for this chip variant
+ * @irq_lock: a lock to protect the mask
+ * @abb_events: a local bit mask of the prcmu wakeup events
+ * @event_mask: a local copy of the mask event registers
+ * @last_event_mask: a copy of the last event_mask written to hardware
+ * @startup_events: a copy of the first reading of the event registers
+ * @startup_events_read: whether the first events have been read
+ */
+struct ab5500 {
+	struct mutex access_mutex;
+	struct device *dev;
+	unsigned int ab5500_irq;
+	unsigned int irq_base;
+	char chip_name[32];
+	u8 chip_id;
+	struct mutex irq_lock;
+	u32 abb_events;
+	u8 mask[AB5500_NUM_IRQ_REGS];
+	u8 oldmask[AB5500_NUM_IRQ_REGS];
+	u8 startup_events[AB5500_NUM_IRQ_REGS];
+	bool startup_events_read;
+#ifdef CONFIG_DEBUG_FS
+	unsigned int debug_bank;
+	unsigned int debug_address;
+#endif
+};
+
+#ifndef CONFIG_AB5500_CORE
+static inline int ab5500_clock_rtc_enable(int num, bool enable)
+{
+	return -ENOSYS;
+}
+#else
+extern int ab5500_clock_rtc_enable(int num, bool enable);
+#endif
+
+/* Forward Declaration */
+struct ab5500_regulator_platform_data;
+
+struct ab5500_platform_data {
+	struct {unsigned int base; unsigned int count; } irq;
+	void *dev_data[AB5500_NUM_DEVICES];
+	size_t dev_data_sz[AB5500_NUM_DEVICES];
+	struct abx500_init_settings *init_settings;
+	unsigned int init_settings_sz;
+	bool pm_power_off;
+	struct ab5500_regulator_platform_data *regulator;
+};
+
+
 int abx500_set_register_interruptible(struct device *dev, u8 bank, u8 reg,
 	u8 value);
 int abx500_get_register_interruptible(struct device *dev, u8 bank, u8 reg,
@@ -224,6 +384,12 @@ int abx500_get_chip_id(struct device *dev);
 int abx500_event_registers_startup_state_get(struct device *dev, u8 *event);
 int abx500_startup_irq_enabled(struct device *dev, unsigned int irq);
 
+#define abx500_get	abx500_get_register_interruptible
+#define abx500_set	abx500_set_register_interruptible
+#define abx500_get_page	abx500_get_register_page_interruptible
+#define abx500_set_page	abx500_set_register_page_interruptible
+#define abx500_mask_and_set	abx500_mask_and_set_register_interruptible
+
 struct abx500_ops {
 	int (*get_chip_id) (struct device *);
 	int (*get_register) (struct device *, u8, u8, u8 *);
@@ -235,6 +401,250 @@ struct abx500_ops {
 	int (*startup_irq_enabled) (struct device *, unsigned int);
 };
 
-int abx500_register_ops(struct device *core_dev, struct abx500_ops *ops);
+/* Battery driver related data */
+/*
+ * ADC for the battery thermistor.
+ * When using the ABx500_ADC_THERM_BATCTRL the battery ID resistor is combined
+ * with a NTC resistor to both identify the battery and to measure its
+ * temperature. Different phone manufactures uses different techniques to both
+ * identify the battery and to read its temperature.
+ */
+enum abx500_adc_therm {
+	ABx500_ADC_THERM_BATCTRL,
+	ABx500_ADC_THERM_BATTEMP,
+};
+
+/**
+ * struct abx500_res_to_temp - defines one point in a temp to res curve. To
+ * be used in battery packs that combines the identification resistor with a
+ * NTC resistor.
+ * @temp:			battery pack temperature in Celcius
+ * @resist:			NTC resistor net total resistance
+ */
+struct abx500_res_to_temp {
+	int temp;
+	int resist;
+};
+
+/**
+ * struct abx500_v_to_cap - Table for translating voltage to capacity
+ * @voltage:		Voltage in mV
+ * @capacity:		Capacity in percent
+ */
+struct abx500_v_to_cap {
+	int voltage;
+	int capacity;
+};
+
+/* Forward declaration */
+struct abx500_fg;
+
+/**
+ * struct abx500_fg_parameters - Fuel gauge algorithm parameters, in seconds
+ * if not specified
+ * @recovery_sleep_timer:	Time between measurements while recovering
+ * @recovery_total_time:	Total recovery time
+ * @init_timer:			Measurement interval during startup
+ * @init_discard_time:		Time we discard voltage measurement at startup
+ * @init_total_time:		Total init time during startup
+ * @high_curr_time:		Time current has to be high to go to recovery
+ * @accu_charging:		FG accumulation time while charging
+ * @accu_high_curr:		FG accumulation time in high current mode
+ * @high_curr_threshold:	High current threshold, in mA
+ * @lowbat_threshold:		Low battery threshold, in mV
+ */
+struct abx500_fg_parameters {
+	int recovery_sleep_timer;
+	int recovery_total_time;
+	int init_timer;
+	int init_discard_time;
+	int init_total_time;
+	int high_curr_time;
+	int accu_charging;
+	int accu_high_curr;
+	int high_curr_threshold;
+	int lowbat_threshold;
+};
+
+/**
+ * struct abx500_charger_maximization - struct used by the board config.
+ * @use_maxi:		Enable maximization for this battery type
+ * @maxi_chg_curr:	Maximum charger current allowed
+ * @maxi_wait_cycles:	cycles to wait before setting charger current
+ * @charger_curr_step	delta between two charger current settings (mA)
+ */
+struct abx500_maxim_parameters {
+	bool ena_maxi;
+	int chg_curr;
+	int wait_cycles;
+	int charger_curr_step;
+};
+
+/**
+ * struct abx500_battery_type - different batteries supported
+ * @name:			battery technology
+ * @resis_high:			battery upper resistance limit
+ * @resis_low:			battery lower resistance limit
+ * @charge_full_design:		Maximum battery capacity in mAh
+ * @nominal_voltage:		Nominal voltage of the battery in mV
+ * @termination_vol:		max voltage upto which battery can be charged
+ * @termination_curr		battery charging termination current in mA
+ * @recharge_vol		battery voltage limit that will trigger a new
+ *				full charging cycle in the case where maintenan-
+ *				-ce charging has been disabled
+ * @normal_cur_lvl:		charger current in normal state in mA
+ * @normal_vol_lvl:		charger voltage in normal state in mV
+ * @maint_a_cur_lvl:		charger current in maintenance A state in mA
+ * @maint_a_vol_lvl:		charger voltage in maintenance A state in mV
+ * @maint_a_chg_timer_h:	charge time in maintenance A state
+ * @maint_b_cur_lvl:		charger current in maintenance B state in mA
+ * @maint_b_vol_lvl:		charger voltage in maintenance B state in mV
+ * @maint_b_chg_timer_h:	charge time in maintenance B state
+ * @low_high_cur_lvl:		charger current in temp low/high state in mA
+ * @low_high_vol_lvl:		charger voltage in temp low/high state in mV'
+ * @battery_resistance:		battery inner resistance in mOhm.
+ * @n_r_t_tbl_elements:		number of elements in r_to_t_tbl
+ * @r_to_t_tbl:			table containing resistance to temp points
+ * @n_v_cap_tbl_elements:	number of elements in v_to_cap_tbl
+ * @v_to_cap_tbl:		Voltage to capacity (in %) table
+ */
+struct abx500_battery_type {
+	int name;
+	int resis_high;
+	int resis_low;
+	int charge_full_design;
+	int nominal_voltage;
+	int termination_vol;
+	int termination_curr;
+	int recharge_vol;
+	int normal_cur_lvl;
+	int normal_vol_lvl;
+	int maint_a_cur_lvl;
+	int maint_a_vol_lvl;
+	int maint_a_chg_timer_h;
+	int maint_b_cur_lvl;
+	int maint_b_vol_lvl;
+	int maint_b_chg_timer_h;
+	int low_high_cur_lvl;
+	int low_high_vol_lvl;
+	int battery_resistance;
+	int n_temp_tbl_elements;
+	struct abx500_res_to_temp *r_to_t_tbl;
+	int n_v_cap_tbl_elements;
+	struct abx500_v_to_cap *v_to_cap_tbl;
+};
+
+/**
+ * struct abx500_bm_capacity_levels - abx500 capacity level data
+ * @critical:		critical capacity level in percent
+ * @low:		low capacity level in percent
+ * @normal:		normal capacity level in percent
+ * @high:		high capacity level in percent
+ * @full:		full capacity level in percent
+ */
+struct abx500_bm_capacity_levels {
+	int critical;
+	int low;
+	int normal;
+	int high;
+	int full;
+};
+
+/**
+ * struct abx500_bm_charger_parameters - Charger specific parameters
+ * @usb_volt_max:	maximum allowed USB charger voltage in mV
+ * @usb_curr_max:	maximum allowed USB charger current in mA
+ * @ac_volt_max:	maximum allowed AC charger voltage in mV
+ * @ac_curr_max:	maximum allowed AC charger current in mA
+ */
+struct abx500_bm_charger_parameters {
+	int usb_volt_max;
+	int usb_curr_max;
+	int ac_volt_max;
+	int ac_curr_max;
+};
+
+/**
+ * struct abx500_bm_data - abx500 battery management data
+ * @temp_under		under this temp, charging is stopped
+ * @temp_low		between this temp and temp_under charging is reduced
+ * @temp_high		between this temp and temp_over charging is reduced
+ * @temp_over		over this temp, charging is stopped
+ * @main_safety_tmr_h	safety timer for main charger
+ * @usb_safety_tmr_h	safety timer for usb charger
+ * @bkup_bat_v		voltage which we charge the backup battery with
+ * @bkup_bat_i		current which we charge the backup battery with
+ * @no_maintenance	indicates that maintenance charging is disabled
+ * @abx500_adc_therm	placement of thermistor, batctrl or battemp adc
+ * @chg_unknown_bat	flag to enable charging of unknown batteries
+ * @enable_overshoot	flag to enable VBAT overshoot control
+ * @fg_res		resistance of FG resistor in mOhm
+ * @n_btypes		number of elements in array bat_type
+ * @batt_id		index of the identified battery in array bat_type
+ * @interval_charging	charge alg cycle period time when charging (sec)
+ * @interval_not_charging charge alg cycle period time when not charging (sec)
+ * @temp_hysteresis	temperature hysteresis
+ * @maxi:		maximization parameters
+ * @cap_levels		capacity in percent for the different capacity levels
+ * @bat_type		table of supported battery types
+ * @chg_params		charger parameters
+ * @fg_params		fuel gauge parameters
+ */
+struct abx500_bm_data {
+	int temp_under;
+	int temp_low;
+	int temp_high;
+	int temp_over;
+	int temp_now;
+	int main_safety_tmr_h;
+	int usb_safety_tmr_h;
+	int bkup_bat_v;
+	int bkup_bat_i;
+	bool no_maintenance;
+	bool chg_unknown_bat;
+	bool enable_overshoot;
+	enum abx500_adc_therm adc_therm;
+	int fg_res;
+	int n_btypes;
+	int batt_id;
+	int interval_charging;
+	int interval_not_charging;
+	int temp_hysteresis;
+	const struct abx500_maxim_parameters *maxi;
+	const struct abx500_bm_capacity_levels *cap_levels;
+	const struct abx500_battery_type *bat_type;
+	const struct abx500_bm_charger_parameters *chg_params;
+	const struct abx500_fg_parameters *fg_params;
+};
+
+struct abx500_chargalg_platform_data {
+	char **supplied_to;
+	size_t num_supplicants;
+};
+
+struct abx500_charger_platform_data {
+	char **supplied_to;
+	size_t num_supplicants;
+};
+
+struct abx500_btemp_platform_data {
+	char **supplied_to;
+	size_t num_supplicants;
+};
+
+struct abx500_fg_platform_data {
+	char **supplied_to;
+	size_t num_supplicants;
+};
+
+struct abx500_bm_plat_data {
+	struct abx500_bm_data *battery;
+	struct abx500_charger_platform_data *charger;
+	struct abx500_btemp_platform_data *btemp;
+	struct abx500_fg_platform_data *fg;
+	struct abx500_chargalg_platform_data *chargalg;
+};
+
+int abx500_register_ops(struct device *dev, struct abx500_ops *ops);
 void abx500_remove_ops(struct device *dev);
 #endif
diff --git a/include/linux/mfd/abx500/ab5500-bm.h b/include/linux/mfd/abx500/ab5500-bm.h
new file mode 100644
index 00000000000..b9800cd8c19
--- /dev/null
+++ b/include/linux/mfd/abx500/ab5500-bm.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright ST-Ericsson 2011.
+ *
+ * Author: Arun Murthy <arun.murthy@stericsson.com>
+ * Licensed under GPLv2.
+ */
+
+#ifndef _AB5500_BM_H
+#define _AB5500_BM_H
+
+#define AB5500_MCB		0x2F
+/*
+ * USB/ULPI register offsets
+ * Bank : 0x5
+ */
+#define AB5500_USB_LINE_STATUS		0x80
+#define AB5500_USB_PHY_STATUS		0x89
+#define AB5500_CHGFSM_CHARGER_DETECT	0xBF
+#define AB5500_CHGFSM_USB_BTEMP_CURR_LIM	0xAD
+#define AB5500_USB_LINE_CTRL2		0x82
+#define AB5500_USB_OTG_CTRL		0x87
+
+/*
+ * Charger / control register offfsets
+ * Bank : 0x0B
+ */
+#define AB5500_CVBUSM			0x11
+#define AB5500_LEDT			0x12
+#define AB5500_VSRC			0x13
+#define AB5500_ICSR			0x14
+#define AB5500_OCSRV			0x15
+#define AB5500_CVREC			0x16
+#define AB5500_CREVS			0x17
+#define AB5500_CCTRL			0x18
+#define AB5500_TBDATA			0x19
+#define AB5500_CPWM			0x1A
+#define AB5500_DCIOCURRENT		0x1B
+#define AB5500_USB_HS_CURR_LIM		0x1C
+#define AB5500_WALL_HS_CURR_LIM		0x1D
+
+/*
+ * FG, Battcom and ACC registers offsets
+ * Bank : 0x0C
+ */
+#define AB5500_FG_CH0			0x00
+#define AB5500_FG_CH1			0x01
+#define AB5500_FG_CH2			0x02
+#define AB5500_FG_DIS_CH0		0x03
+#define AB5500_FG_DIS_CH1		0x04
+#define AB5500_FG_DIS_CH2		0x05
+#define AB5500_FGDIS_COUNT0		0x06
+#define AB5500_FGDIS_COUNT1		0x07
+#define AB5500_FG_VAL_COUNT0		0x08
+#define AB5500_FG_VAL_COUNT1		0x09
+#define AB5500_FGDIR_READ0		0x0A
+#define AB5500_FGDIR_READ1		0x0B
+#define AB5500_FG_CONTROL_A		0x0C
+#define AB5500_FG_CONTROL_B		0x0F
+#define AB5500_FG_CONTROL_C		0x10
+#define AB5500_FG_DIS			0x0D
+#define AB5500_FG_EOC			0x0E
+#define AB5500_FG_CB			0x0F
+#define AB5500_FG_CC			0x10
+#define AB5500_UIOR			0x1A
+#define AB5500_UART			0x1B
+#define AB5500_URI			0x1C
+#define AB5500_UART_RQ			0x1D
+#define AB5500_ACC_DETECT1		0x20
+#define AB5500_ACC_DETECT2		0x21
+#define AB5500_ACC_DETECTCTRL		0x23
+#define AB5500_ACC_AVCTRL		0x24
+#define AB5500_ACC_DETECT3_DEG_LITCH_TIME	0x30
+#define AB5500_ACC_DETECT3_KEY_PRESS_TIME	0x31
+#define AB5500_ACC_DETECT3_LONG_KEY_TIME	0x32
+#define AB5500_ACC_DETECT3_TIME_READ_MS		0x33
+#define AB5500_ACC_DETECT3_TIME_READ_LS		0x34
+#define AB5500_ACC_DETECT3_CONTROL		0x35
+#define AB5500_ACC_DETECT3_LEVEL		0x36
+#define AB5500_ACC_DETECT3_TIMER_READ_CTL	0x37
+
+/*
+ * Interrupt register offsets
+ * Bank : 0x0E
+ */
+#define AB5500_IT_SOURCE8		0x28
+#define AB5500_IT_SOURCE9		0x29
+
+/* BatCtrl Current Source Constants */
+#define BAT_CTRL_7U_ENA			(0x01 << 0)
+#define BAT_CTRL_15U_ENA		(0x01 << 1)
+#define BAT_CTRL_30U_ENA		(0x01 << 2)
+#define BAT_CTRL_60U_ENA		(0x01 << 3)
+#define BAT_CTRL_120U_ENA		(0x01 << 4)
+#define BAT_CTRL_CMP_ENA		0x04
+#define FORCE_BAT_CTRL_CMP_HIGH		0x08
+#define BAT_CTRL_PULL_UP_ENA		0x10
+
+/* Battery type */
+#define BATTERY_UNKNOWN			0
+
+#ifdef CONFIG_AB5500_BM
+void ab5500_charger_usb_state_changed(u8 bm_usb_state, u16 mA);
+#else
+static void ab5500_charger_usb_state_changed(u8 bm_usb_state, u16 mA)
+{
+}
+#endif
+#endif /* _AB5500_BM_H */
diff --git a/include/linux/mfd/abx500/ab5500-gpadc.h b/include/linux/mfd/abx500/ab5500-gpadc.h
new file mode 100644
index 00000000000..3282b44114e
--- /dev/null
+++ b/include/linux/mfd/abx500/ab5500-gpadc.h
@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) 2010 ST-Ericsson SA
+ * Licensed under GPLv2.
+ *
+ * Author: Vijaya Kumar K <vijay.kilari@stericsson.com>
+ */
+
+#ifndef	_AB5500_GPADC_H
+#define _AB5500_GPADC_H
+
+/*
+ * GPADC source:
+ * The BTEMP_BALL and PCB_TEMP are same. They differ if the
+ * battery supports internal NTC resistor connected to BDATA
+ * line. In this case, the BTEMP_BALL correspondss to BDATA
+ * of GPADC as per AB5500 product spec.
+ */
+
+#define BTEMP_BALL		0
+#define ACC_DETECT2		1
+#define ACC_DETECT3		2
+#define MAIN_BAT_V		3
+#define MAIN_BAT_V_TXON		4
+#define VBUS_V			5
+#define USB_CHARGER_C		6
+#define BK_BAT_V		7
+#define DIE_TEMP		8
+#define PCB_TEMP		9
+#define XTAL_TEMP		10
+#define USB_ID			11
+#define BAT_CTRL		12
+/* VBAT with TXON only min trigger */
+#define MAIN_BAT_V_TXON_TRIG_MIN	13
+/* VBAT with TX off only min trigger */
+#define MAIN_BAT_V_TRIG_MIN		14
+
+/*
+ * Frequency of auto adc conversion
+ */
+#define MS1000		0x0
+#define MS500		0x1
+#define MS200		0x2
+#define MS100		0x3
+#define MS10		0x4
+
+struct ab5500_gpadc;
+
+/*
+ * struct adc_auto_input - AB5500 GPADC auto trigger
+ * @adc_mux                     Mux input
+ * @freq                        freq of conversion
+ * @min                         min value for trigger
+ * @max                         max value for trigger
+ * @auto_adc_callback           notification callback
+ */
+struct adc_auto_input {
+	u8 mux;
+	u8 freq;
+	int min;
+	int max;
+	int (*auto_adc_callback)(int mux);
+};
+
+struct ab5500_gpadc *ab5500_gpadc_get(const char *name);
+int ab5500_gpadc_convert(struct ab5500_gpadc *gpadc, u8 input);
+int ab5500_gpadc_convert_auto(struct ab5500_gpadc *gpadc,
+			struct adc_auto_input *auto_input);
+
+#endif /* _AB5500_GPADC_H */
diff --git a/include/linux/mfd/abx500/ux500_chargalg.h b/include/linux/mfd/abx500/ux500_chargalg.h
new file mode 100644
index 00000000000..f04e47ff56a
--- /dev/null
+++ b/include/linux/mfd/abx500/ux500_chargalg.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Johan Gardsmark <johan.gardsmark@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#ifndef _UX500_CHARGALG_H
+#define _UX500_CHARGALG_H
+
+#include <linux/power_supply.h>
+
+#define psy_to_ux500_charger(x) container_of((x), \
+		struct ux500_charger, psy)
+
+/* Forward declaration */
+struct ux500_charger;
+
+struct ux500_charger_ops {
+	int (*enable) (struct ux500_charger *, int, int, int);
+	int (*kick_wd) (struct ux500_charger *);
+	int (*update_curr) (struct ux500_charger *, int);
+};
+
+/**
+ * struct ux500_charger - power supply ux500 charger sub class
+ * @psy			power supply base class
+ * @ops			ux500 charger operations
+ * @max_out_volt	maximum output charger voltage in mV
+ * @max_out_curr	maximum output charger current in mA
+ */
+struct ux500_charger {
+	struct power_supply psy;
+	struct ux500_charger_ops ops;
+	int max_out_volt;
+	int max_out_curr;
+};
+
+#endif
diff --git a/include/linux/mfd/db5500-prcmu.h b/include/linux/mfd/db5500-prcmu.h
index f0977986402..9890687f582 100644
--- a/include/linux/mfd/db5500-prcmu.h
+++ b/include/linux/mfd/db5500-prcmu.h
@@ -5,21 +5,35 @@
  *
  * U5500 PRCMU API.
  */
-#ifndef __MACH_PRCMU_U5500_H
-#define __MACH_PRCMU_U5500_H
+#ifndef __MFD_DB5500_PRCMU_H
+#define __MFD_DB5500_PRCMU_H
 
-#ifdef CONFIG_UX500_SOC_DB5500
+#ifdef CONFIG_MFD_DB5500_PRCMU
 
 void db5500_prcmu_early_init(void);
-
+int db5500_prcmu_set_epod(u16 epod_id, u8 epod_state);
+int db5500_prcmu_set_display_clocks(void);
+int db5500_prcmu_disable_dsipll(void);
+int db5500_prcmu_enable_dsipll(void);
 int db5500_prcmu_abb_read(u8 slave, u8 reg, u8 *value, u8 size);
 int db5500_prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size);
+void db5500_prcmu_enable_wakeups(u32 wakeups);
+int db5500_prcmu_request_clock(u8 clock, bool enable);
+void db5500_prcmu_config_abb_event_readout(u32 abb_events);
+void db5500_prcmu_get_abb_event_buffer(void __iomem **buf);
+int prcmu_resetout(u8 resoutn, u8 state);
+int db5500_prcmu_set_power_state(u8 state, bool keep_ulp_clk,
+	bool keep_ap_pll);
+int db5500_prcmu_config_esram0_deep_sleep(u8 state);
+void db5500_prcmu_system_reset(u16 reset_code);
+u16 db5500_prcmu_get_reset_code(void);
+bool db5500_prcmu_is_ac_wake_requested(void);
+int db5500_prcmu_set_arm_opp(u8 opp);
+int db5500_prcmu_get_arm_opp(void);
 
 #else /* !CONFIG_UX500_SOC_DB5500 */
 
-static inline void db5500_prcmu_early_init(void)
-{
-}
+static inline void db5500_prcmu_early_init(void) {}
 
 static inline int db5500_prcmu_abb_read(u8 slave, u8 reg, u8 *value, u8 size)
 {
@@ -31,15 +45,75 @@ static inline int db5500_prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size)
 	return -ENOSYS;
 }
 
-#endif /* CONFIG_UX500_SOC_DB5500 */
+static inline int db5500_prcmu_request_clock(u8 clock, bool enable)
+{
+	return 0;
+}
+
+static inline int db5500_prcmu_set_display_clocks(void)
+{
+	return 0;
+}
+
+static inline int db5500_prcmu_disable_dsipll(void)
+{
+	return 0;
+}
+
+static inline int db5500_prcmu_enable_dsipll(void)
+{
+	return 0;
+}
 
-static inline int db5500_prcmu_config_abb_event_readout(u32 abb_events)
+static inline int db5500_prcmu_config_esram0_deep_sleep(u8 state)
 {
-#ifdef CONFIG_MACH_U5500_SIMULATOR
 	return 0;
-#else
-	return -1;
-#endif
 }
 
-#endif /* __MACH_PRCMU_U5500_H */
+static inline void db5500_prcmu_enable_wakeups(u32 wakeups) {}
+
+static inline int prcmu_resetout(u8 resoutn, u8 state)
+{
+	return 0;
+}
+
+static inline int db5500_prcmu_set_epod(u16 epod_id, u8 epod_state)
+{
+	return 0;
+}
+
+static inline void db5500_prcmu_get_abb_event_buffer(void __iomem **buf) {}
+static inline void db5500_prcmu_config_abb_event_readout(u32 abb_events) {}
+
+static inline int db5500_prcmu_set_power_state(u8 state, bool keep_ulp_clk,
+	bool keep_ap_pll)
+{
+	return 0;
+}
+
+static inline void db5500_prcmu_system_reset(u16 reset_code) {}
+
+static inline u16 db5500_prcmu_get_reset_code(void)
+{
+	return 0;
+}
+
+static inline bool db5500_prcmu_is_ac_wake_requested(void)
+{
+	return 0;
+}
+
+static inline int db5500_prcmu_set_arm_opp(u8 opp)
+{
+	return 0;
+}
+
+static inline int db5500_prcmu_get_arm_opp(void)
+{
+	return 0;
+}
+
+
+#endif /* CONFIG_MFD_DB5500_PRCMU */
+
+#endif /* __MFD_DB5500_PRCMU_H */
diff --git a/include/linux/mfd/db8500-prcmu.h b/include/linux/mfd/db8500-prcmu.h
index 917dbcab701..60d27f7bfc1 100644
--- a/include/linux/mfd/db8500-prcmu.h
+++ b/include/linux/mfd/db8500-prcmu.h
@@ -11,7 +11,6 @@
 #define __MFD_DB8500_PRCMU_H
 
 #include <linux/interrupt.h>
-#include <linux/notifier.h>
 
 /* This portion previously known as <mach/prcmu-fw-defs_v1.h> */
 
@@ -133,7 +132,7 @@ enum ap_pwrst {
  * @APEXECUTE_TO_APIDLE: Power state transition from ApExecute to ApIdle
  */
 enum ap_pwrst_trans {
-	NO_TRANSITION			= 0x00,
+	PRCMU_AP_NO_CHANGE		= 0x00,
 	APEXECUTE_TO_APSLEEP		= 0x01,
 	APIDLE_TO_APSLEEP		= 0x02, /* To be removed */
 	PRCMU_AP_SLEEP			= 0x01,
@@ -146,54 +145,6 @@ enum ap_pwrst_trans {
 };
 
 /**
- * enum ddr_pwrst - DDR power states definition
- * @DDR_PWR_STATE_UNCHANGED: SDRAM and DDR controller state is unchanged
- * @DDR_PWR_STATE_ON:
- * @DDR_PWR_STATE_OFFLOWLAT:
- * @DDR_PWR_STATE_OFFHIGHLAT:
- */
-enum ddr_pwrst {
-	DDR_PWR_STATE_UNCHANGED     = 0x00,
-	DDR_PWR_STATE_ON            = 0x01,
-	DDR_PWR_STATE_OFFLOWLAT     = 0x02,
-	DDR_PWR_STATE_OFFHIGHLAT    = 0x03
-};
-
-/**
- * enum arm_opp - ARM OPP states definition
- * @ARM_OPP_INIT:
- * @ARM_NO_CHANGE: The ARM operating point is unchanged
- * @ARM_100_OPP: The new ARM operating point is arm100opp
- * @ARM_50_OPP: The new ARM operating point is arm50opp
- * @ARM_MAX_OPP: Operating point is "max" (more than 100)
- * @ARM_MAX_FREQ100OPP: Set max opp if available, else 100
- * @ARM_EXTCLK: The new ARM operating point is armExtClk
- */
-enum arm_opp {
-	ARM_OPP_INIT = 0x00,
-	ARM_NO_CHANGE = 0x01,
-	ARM_100_OPP = 0x02,
-	ARM_50_OPP = 0x03,
-	ARM_MAX_OPP = 0x04,
-	ARM_MAX_FREQ100OPP = 0x05,
-	ARM_EXTCLK = 0x07
-};
-
-/**
- * enum ape_opp - APE OPP states definition
- * @APE_OPP_INIT:
- * @APE_NO_CHANGE: The APE operating point is unchanged
- * @APE_100_OPP: The new APE operating point is ape100opp
- * @APE_50_OPP: 50%
- */
-enum ape_opp {
-	APE_OPP_INIT = 0x00,
-	APE_NO_CHANGE = 0x01,
-	APE_100_OPP = 0x02,
-	APE_50_OPP = 0x03
-};
-
-/**
  * enum hw_acc_state - State definition for hardware accelerator
  * @HW_NO_CHANGE: The hardware accelerator state must remain unchanged
  * @HW_OFF: The hardware accelerator must be switched off
@@ -469,26 +420,6 @@ enum auto_enable {
 
 /* End of file previously known as prcmu-fw-defs_v1.h */
 
-/* PRCMU Wakeup defines */
-enum prcmu_wakeup_index {
-	PRCMU_WAKEUP_INDEX_RTC,
-	PRCMU_WAKEUP_INDEX_RTT0,
-	PRCMU_WAKEUP_INDEX_RTT1,
-	PRCMU_WAKEUP_INDEX_HSI0,
-	PRCMU_WAKEUP_INDEX_HSI1,
-	PRCMU_WAKEUP_INDEX_USB,
-	PRCMU_WAKEUP_INDEX_ABB,
-	PRCMU_WAKEUP_INDEX_ABB_FIFO,
-	PRCMU_WAKEUP_INDEX_ARM,
-	NUM_PRCMU_WAKEUP_INDICES
-};
-#define PRCMU_WAKEUP(_name) (BIT(PRCMU_WAKEUP_INDEX_##_name))
-
-/* PRCMU QoS APE OPP class */
-#define PRCMU_QOS_APE_OPP 1
-#define PRCMU_QOS_DDR_OPP 2
-#define PRCMU_QOS_DEFAULT_VALUE -1
-
 /**
  * enum hw_acc_dev - enum for hw accelerators
  * @HW_ACC_SVAMMDSP: for SVAMMDSP
@@ -527,64 +458,6 @@ enum hw_acc_dev {
 };
 
 /*
- * Ids for all EPODs (power domains)
- * - EPOD_ID_SVAMMDSP: power domain for SVA MMDSP
- * - EPOD_ID_SVAPIPE: power domain for SVA pipe
- * - EPOD_ID_SIAMMDSP: power domain for SIA MMDSP
- * - EPOD_ID_SIAPIPE: power domain for SIA pipe
- * - EPOD_ID_SGA: power domain for SGA
- * - EPOD_ID_B2R2_MCDE: power domain for B2R2 and MCDE
- * - EPOD_ID_ESRAM12: power domain for ESRAM 1 and 2
- * - EPOD_ID_ESRAM34: power domain for ESRAM 3 and 4
- * - NUM_EPOD_ID: number of power domains
- */
-#define EPOD_ID_SVAMMDSP	0
-#define EPOD_ID_SVAPIPE		1
-#define EPOD_ID_SIAMMDSP	2
-#define EPOD_ID_SIAPIPE		3
-#define EPOD_ID_SGA		4
-#define EPOD_ID_B2R2_MCDE	5
-#define EPOD_ID_ESRAM12		6
-#define EPOD_ID_ESRAM34		7
-#define NUM_EPOD_ID		8
-
-/*
- * state definition for EPOD (power domain)
- * - EPOD_STATE_NO_CHANGE: The EPOD should remain unchanged
- * - EPOD_STATE_OFF: The EPOD is switched off
- * - EPOD_STATE_RAMRET: The EPOD is switched off with its internal RAM in
- *                         retention
- * - EPOD_STATE_ON_CLK_OFF: The EPOD is switched on, clock is still off
- * - EPOD_STATE_ON: Same as above, but with clock enabled
- */
-#define EPOD_STATE_NO_CHANGE	0x00
-#define EPOD_STATE_OFF		0x01
-#define EPOD_STATE_RAMRET	0x02
-#define EPOD_STATE_ON_CLK_OFF	0x03
-#define EPOD_STATE_ON		0x04
-
-/*
- * CLKOUT sources
- */
-#define PRCMU_CLKSRC_CLK38M		0x00
-#define PRCMU_CLKSRC_ACLK		0x01
-#define PRCMU_CLKSRC_SYSCLK		0x02
-#define PRCMU_CLKSRC_LCDCLK		0x03
-#define PRCMU_CLKSRC_SDMMCCLK		0x04
-#define PRCMU_CLKSRC_TVCLK		0x05
-#define PRCMU_CLKSRC_TIMCLK		0x06
-#define PRCMU_CLKSRC_CLK009		0x07
-/* These are only valid for CLKOUT1: */
-#define PRCMU_CLKSRC_SIAMMDSPCLK	0x40
-#define PRCMU_CLKSRC_I2CCLK		0x41
-#define PRCMU_CLKSRC_MSP02CLK		0x42
-#define PRCMU_CLKSRC_ARMPLL_OBSCLK	0x43
-#define PRCMU_CLKSRC_HSIRXCLK		0x44
-#define PRCMU_CLKSRC_HSITXCLK		0x45
-#define PRCMU_CLKSRC_ARMCLKFIX		0x46
-#define PRCMU_CLKSRC_HDMICLK		0x47
-
-/*
  * Definitions for autonomous power management configuration.
  */
 
@@ -620,88 +493,12 @@ struct prcmu_auto_pm_config {
 	u8 sva_policy;
 };
 
-/**
- * enum ddr_opp - DDR OPP states definition
- * @DDR_100_OPP: The new DDR operating point is ddr100opp
- * @DDR_50_OPP: The new DDR operating point is ddr50opp
- * @DDR_25_OPP: The new DDR operating point is ddr25opp
- */
-enum ddr_opp {
-	DDR_100_OPP = 0x00,
-	DDR_50_OPP = 0x01,
-	DDR_25_OPP = 0x02,
-};
-
-/*
- * Clock identifiers.
- */
-enum prcmu_clock {
-	PRCMU_SGACLK,
-	PRCMU_UARTCLK,
-	PRCMU_MSP02CLK,
-	PRCMU_MSP1CLK,
-	PRCMU_I2CCLK,
-	PRCMU_SDMMCCLK,
-	PRCMU_SLIMCLK,
-	PRCMU_PER1CLK,
-	PRCMU_PER2CLK,
-	PRCMU_PER3CLK,
-	PRCMU_PER5CLK,
-	PRCMU_PER6CLK,
-	PRCMU_PER7CLK,
-	PRCMU_LCDCLK,
-	PRCMU_BMLCLK,
-	PRCMU_HSITXCLK,
-	PRCMU_HSIRXCLK,
-	PRCMU_HDMICLK,
-	PRCMU_APEATCLK,
-	PRCMU_APETRACECLK,
-	PRCMU_MCDECLK,
-	PRCMU_IPI2CCLK,
-	PRCMU_DSIALTCLK,
-	PRCMU_DMACLK,
-	PRCMU_B2R2CLK,
-	PRCMU_TVCLK,
-	PRCMU_SSPCLK,
-	PRCMU_RNGCLK,
-	PRCMU_UICCCLK,
-	PRCMU_NUM_REG_CLOCKS,
-	PRCMU_SYSCLK = PRCMU_NUM_REG_CLOCKS,
-	PRCMU_TIMCLK,
-};
-
-/*
- * Definitions for controlling ESRAM0 in deep sleep.
- */
-#define ESRAM0_DEEP_SLEEP_STATE_OFF 1
-#define ESRAM0_DEEP_SLEEP_STATE_RET 2
-
-#ifdef CONFIG_MFD_DB8500_PRCMU
-void __init prcmu_early_init(void);
-int prcmu_set_display_clocks(void);
-int prcmu_disable_dsipll(void);
-int prcmu_enable_dsipll(void);
-#else
-static inline void __init prcmu_early_init(void) {}
-#endif
-
 #ifdef CONFIG_MFD_DB8500_PRCMU
 
+void db8500_prcmu_early_init(void);
 int prcmu_set_rc_a2p(enum romcode_write);
 enum romcode_read prcmu_get_rc_p2a(void);
 enum ap_pwrst prcmu_get_xp70_current_state(void);
-int prcmu_set_power_state(u8 state, bool keep_ulp_clk, bool keep_ap_pll);
-
-void prcmu_enable_wakeups(u32 wakeups);
-static inline void prcmu_disable_wakeups(void)
-{
-	prcmu_enable_wakeups(0);
-}
-
-void prcmu_config_abb_event_readout(u32 abb_events);
-void prcmu_get_abb_event_buffer(void __iomem **buf);
-int prcmu_set_arm_opp(u8 opp);
-int prcmu_get_arm_opp(void);
 bool prcmu_has_arm_maxopp(void);
 bool prcmu_is_u8400(void);
 int prcmu_set_ape_opp(u8 opp);
@@ -710,19 +507,14 @@ int prcmu_request_ape_opp_100_voltage(bool enable);
 int prcmu_release_usb_wakeup_state(void);
 int prcmu_set_ddr_opp(u8 opp);
 int prcmu_get_ddr_opp(void);
-unsigned long prcmu_qos_get_cpufreq_opp_delay(void);
-void prcmu_qos_set_cpufreq_opp_delay(unsigned long);
 /* NOTE! Use regulator framework instead */
 int prcmu_set_hwacc(u16 hw_acc_dev, u8 state);
-int prcmu_set_epod(u16 epod_id, u8 epod_state);
 void prcmu_configure_auto_pm(struct prcmu_auto_pm_config *sleep,
 	struct prcmu_auto_pm_config *idle);
 bool prcmu_is_auto_pm_enabled(void);
 
 int prcmu_config_clkout(u8 clkout, u8 source, u8 div);
-int prcmu_request_clock(u8 clock, bool enable);
 int prcmu_set_clock_divider(u8 clock, u8 divider);
-int prcmu_config_esram0_deep_sleep(u8 state);
 int prcmu_config_hotdog(u8 threshold);
 int prcmu_config_hotmon(u8 low, u8 high);
 int prcmu_start_temp_sense(u16 cycles32k);
@@ -732,14 +524,36 @@ int prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size);
 
 void prcmu_ac_wake_req(void);
 void prcmu_ac_sleep_req(void);
-void prcmu_system_reset(u16 reset_code);
 void prcmu_modem_reset(void);
-bool prcmu_is_ac_wake_requested(void);
 void prcmu_enable_spi2(void);
 void prcmu_disable_spi2(void);
 
+int prcmu_config_a9wdog(u8 num, bool sleep_auto_off);
+int prcmu_enable_a9wdog(u8 id);
+int prcmu_disable_a9wdog(u8 id);
+int prcmu_kick_a9wdog(u8 id);
+int prcmu_load_a9wdog(u8 id, u32 val);
+
+void db8500_prcmu_system_reset(u16 reset_code);
+int db8500_prcmu_set_power_state(u8 state, bool keep_ulp_clk, bool keep_ap_pll);
+void db8500_prcmu_enable_wakeups(u32 wakeups);
+int db8500_prcmu_set_epod(u16 epod_id, u8 epod_state);
+int db8500_prcmu_request_clock(u8 clock, bool enable);
+int db8500_prcmu_set_display_clocks(void);
+int db8500_prcmu_disable_dsipll(void);
+int db8500_prcmu_enable_dsipll(void);
+void db8500_prcmu_config_abb_event_readout(u32 abb_events);
+void db8500_prcmu_get_abb_event_buffer(void __iomem **buf);
+int db8500_prcmu_config_esram0_deep_sleep(u8 state);
+u16 db8500_prcmu_get_reset_code(void);
+bool db8500_prcmu_is_ac_wake_requested(void);
+int db8500_prcmu_set_arm_opp(u8 opp);
+int db8500_prcmu_get_arm_opp(void);
+
 #else /* !CONFIG_MFD_DB8500_PRCMU */
 
+static inline void db8500_prcmu_early_init(void) {}
+
 static inline int prcmu_set_rc_a2p(enum romcode_write code)
 {
 	return 0;
@@ -755,34 +569,12 @@ static inline enum ap_pwrst prcmu_get_xp70_current_state(void)
 	return AP_EXECUTE;
 }
 
-static inline int prcmu_set_power_state(u8 state, bool keep_ulp_clk,
-	bool keep_ap_pll)
-{
-	return 0;
-}
-
-static inline void prcmu_enable_wakeups(u32 wakeups) {}
-
-static inline void prcmu_disable_wakeups(void) {}
-
-static inline void prcmu_config_abb_event_readout(u32 abb_events) {}
-
-static inline int prcmu_set_arm_opp(u8 opp)
-{
-	return 0;
-}
-
-static inline int prcmu_get_arm_opp(void)
-{
-	return ARM_100_OPP;
-}
-
-static bool prcmu_has_arm_maxopp(void)
+static inline bool prcmu_has_arm_maxopp(void)
 {
 	return false;
 }
 
-static bool prcmu_is_u8400(void)
+static inline bool prcmu_is_u8400(void)
 {
 	return false;
 }
@@ -817,13 +609,6 @@ static inline int prcmu_get_ddr_opp(void)
 	return DDR_100_OPP;
 }
 
-static inline unsigned long prcmu_qos_get_cpufreq_opp_delay(void)
-{
-	return 0;
-}
-
-static inline void prcmu_qos_set_cpufreq_opp_delay(unsigned long n) {}
-
 static inline int prcmu_set_hwacc(u16 hw_acc_dev, u8 state)
 {
 	return 0;
@@ -844,21 +629,11 @@ static inline int prcmu_config_clkout(u8 clkout, u8 source, u8 div)
 	return 0;
 }
 
-static inline int prcmu_request_clock(u8 clock, bool enable)
-{
-	return 0;
-}
-
 static inline int prcmu_set_clock_divider(u8 clock, u8 divider)
 {
 	return 0;
 }
 
-int prcmu_config_esram0_deep_sleep(u8 state)
-{
-	return 0;
-}
-
 static inline int prcmu_config_hotdog(u8 threshold)
 {
 	return 0;
@@ -893,86 +668,107 @@ static inline void prcmu_ac_wake_req(void) {}
 
 static inline void prcmu_ac_sleep_req(void) {}
 
-static inline void prcmu_system_reset(u16 reset_code) {}
-
 static inline void prcmu_modem_reset(void) {}
 
-static inline bool prcmu_is_ac_wake_requested(void)
+static inline int prcmu_enable_spi2(void)
 {
-	return false;
+	return 0;
 }
 
-#ifndef CONFIG_UX500_SOC_DB5500
-static inline int prcmu_set_display_clocks(void)
+static inline int prcmu_disable_spi2(void)
 {
 	return 0;
 }
 
-static inline int prcmu_disable_dsipll(void)
+static inline void db8500_prcmu_system_reset(u16 reset_code) {}
+
+static inline int db8500_prcmu_set_power_state(u8 state, bool keep_ulp_clk,
+	bool keep_ap_pll)
 {
 	return 0;
 }
 
-static inline int prcmu_enable_dsipll(void)
+static inline void db8500_prcmu_enable_wakeups(u32 wakeups) {}
+
+static inline int db8500_prcmu_set_epod(u16 epod_id, u8 epod_state)
 {
 	return 0;
 }
-#endif
 
-static inline int prcmu_enable_spi2(void)
+static inline int db8500_prcmu_request_clock(u8 clock, bool enable)
 {
 	return 0;
 }
 
-static inline int prcmu_disable_spi2(void)
+static inline int db8500_prcmu_set_display_clocks(void)
 {
 	return 0;
 }
 
-#endif /* !CONFIG_MFD_DB8500_PRCMU */
+static inline int db8500_prcmu_disable_dsipll(void)
+{
+	return 0;
+}
+
+static inline int db8500_prcmu_enable_dsipll(void)
+{
+	return 0;
+}
+
+static inline int db8500_prcmu_config_esram0_deep_sleep(u8 state)
+{
+	return 0;
+}
+
+static inline void db8500_prcmu_config_abb_event_readout(u32 abb_events) {}
 
-#ifdef CONFIG_UX500_PRCMU_QOS_POWER
-int prcmu_qos_requirement(int pm_qos_class);
-int prcmu_qos_add_requirement(int pm_qos_class, char *name, s32 value);
-int prcmu_qos_update_requirement(int pm_qos_class, char *name, s32 new_value);
-void prcmu_qos_remove_requirement(int pm_qos_class, char *name);
-int prcmu_qos_add_notifier(int prcmu_qos_class,
-			   struct notifier_block *notifier);
-int prcmu_qos_remove_notifier(int prcmu_qos_class,
-			      struct notifier_block *notifier);
-#else
-static inline int prcmu_qos_requirement(int prcmu_qos_class)
+static inline void db8500_prcmu_get_abb_event_buffer(void __iomem **buf) {}
+
+static inline u16 db8500_prcmu_get_reset_code(void)
 {
 	return 0;
 }
 
-static inline int prcmu_qos_add_requirement(int prcmu_qos_class,
-					    char *name, s32 value)
+static inline int prcmu_config_a9wdog(u8 num, bool sleep_auto_off)
 {
 	return 0;
 }
 
-static inline int prcmu_qos_update_requirement(int prcmu_qos_class,
-					       char *name, s32 new_value)
+static inline int prcmu_enable_a9wdog(u8 id)
 {
 	return 0;
 }
 
-static inline void prcmu_qos_remove_requirement(int prcmu_qos_class, char *name)
+static inline int prcmu_disable_a9wdog(u8 id)
 {
+	return 0;
 }
 
-static inline int prcmu_qos_add_notifier(int prcmu_qos_class,
-					 struct notifier_block *notifier)
+static inline int prcmu_kick_a9wdog(u8 id)
 {
 	return 0;
 }
-static inline int prcmu_qos_remove_notifier(int prcmu_qos_class,
-					    struct notifier_block *notifier)
+
+static inline int prcmu_load_a9wdog(u8 id, u32 val)
 {
 	return 0;
 }
 
-#endif
+static inline bool db8500_prcmu_is_ac_wake_requested(void)
+{
+	return 0;
+}
+
+static inline int db8500_prcmu_set_arm_opp(u8 opp)
+{
+	return 0;
+}
+
+static inline int db8500_prcmu_get_arm_opp(void)
+{
+	return 0;
+}
+
+#endif /* !CONFIG_MFD_DB8500_PRCMU */
 
 #endif /* __MFD_DB8500_PRCMU_H */
diff --git a/include/linux/mfd/stmpe.h b/include/linux/mfd/stmpe.h
index e762c270d8d..54046d1d6b5 100644
--- a/include/linux/mfd/stmpe.h
+++ b/include/linux/mfd/stmpe.h
@@ -57,6 +57,7 @@ struct stmpe_variant_info;
  * @irq_lock: IRQ bus lock
  * @dev: device, mostly for dev_dbg()
  * @i2c: i2c client
+ * @partnum: part number
  * @variant: the detected STMPE model number
  * @regs: list of addresses of registers which are at different addresses on
  *	  different variants.  Indexed by one of STMPE_IDX_*.
@@ -106,7 +107,7 @@ struct matrix_keymap_data;
  * @no_autorepeat: disable key autorepeat
  */
 struct stmpe_keypad_platform_data {
-	struct matrix_keymap_data *keymap_data;
+	const struct matrix_keymap_data *keymap_data;
 	unsigned int debounce_ms;
 	unsigned int scan_count;
 	bool no_autorepeat;
@@ -121,6 +122,8 @@ struct stmpe_keypad_platform_data {
  * @norequest_mask: bitmask specifying which GPIOs should _not_ be
  *		    requestable due to different usage (e.g. touch, keypad)
  *		    STMPE_GPIO_NOREQ_* macros can be used here.
+ * @setup: board specific setup callback.
+ * @remove: board specific remove callback
  */
 struct stmpe_gpio_platform_data {
 	int gpio_base;
diff --git a/include/linux/mloader.h b/include/linux/mloader.h
new file mode 100644
index 00000000000..ceca3245856
--- /dev/null
+++ b/include/linux/mloader.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Ludovic Barre <ludovic.barre@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#ifndef _MLOADER_H_
+#define _MLOADER_H_
+
+/* not use in ioctl-number.txt */
+#define ML_IO_NUMBER 0xFE
+
+#define ML_UPLOAD _IO(ML_IO_NUMBER, 1)
+#define ML_GET_NBIMAGES _IOR(ML_IO_NUMBER, 2, int)
+#define ML_GET_DUMPINFO _IOR(ML_IO_NUMBER, 3, struct dump_image*)
+
+#define MAX_NAME 16
+
+struct dump_image {
+	char name[MAX_NAME];
+	unsigned int offset;
+	unsigned int size;
+};
+
+#endif /* _MLOADER_H_ */
diff --git a/include/linux/mmc/host.h b/include/linux/mmc/host.h
index 1584b5236bb..48f165c82aa 100644
--- a/include/linux/mmc/host.h
+++ b/include/linux/mmc/host.h
@@ -12,6 +12,7 @@
 
 #include <linux/leds.h>
 #include <linux/sched.h>
+#include <linux/wakelock.h>
 
 #include <linux/mmc/core.h>
 #include <linux/mmc/pm.h>
@@ -211,6 +212,7 @@ struct mmc_host {
 #define MMC_CAP_MAX_CURRENT_600	(1 << 28)	/* Host max current limit is 600mA */
 #define MMC_CAP_MAX_CURRENT_800	(1 << 29)	/* Host max current limit is 800mA */
 #define MMC_CAP_CMD23		(1 << 30)	/* CMD23 supported. */
+#define MMC_CAP_BROKEN_SDIO_CMD53 (1 << 15)	/* Broken CMD53 byte mode */
 
 	mmc_pm_flag_t		pm_caps;	/* supported pm features */
 
@@ -221,7 +223,6 @@ struct mmc_host {
 	struct work_struct	clk_gate_work; /* delayed clock gate */
 	unsigned int		clk_old;	/* old clock value cache */
 	spinlock_t		clk_lock;	/* lock for clk fields */
-	struct mutex		clk_gate_mutex;	/* mutex for clock gating */
 #endif
 
 	/* host specific block data */
@@ -234,6 +235,8 @@ struct mmc_host {
 
 	/* private data */
 	spinlock_t		lock;		/* lock for claim and bus ops */
+	struct wake_lock	wakelock;	/* wake lock for each host */
+	char			wakelock_name[24];
 
 	struct mmc_ios		ios;		/* current io bus settings */
 	u32			ocr;		/* the current OCR setting */
@@ -270,8 +273,9 @@ struct mmc_host {
 #define MMC_BUSRESUME_NEEDS_RESUME	(1 << 1)
 
 	unsigned int		sdio_irqs;
-	struct task_struct	*sdio_irq_thread;
-	atomic_t		sdio_irq_thread_abort;
+	struct delayed_work	sdio_irq_work;
+	struct workqueue_struct	*sdio_irq_workqueue;
+	unsigned long		sdio_poll_period;
 
 	mmc_pm_flag_t		pm_flags;	/* requested pm features */
 
@@ -345,7 +349,7 @@ extern void mmc_request_done(struct mmc_host *, struct mmc_request *);
 static inline void mmc_signal_sdio_irq(struct mmc_host *host)
 {
 	host->ops->enable_sdio_irq(host, 0);
-	wake_up_process(host->sdio_irq_thread);
+	queue_delayed_work(host->sdio_irq_workqueue, &host->sdio_irq_work, 0);
 }
 
 struct regulator;
diff --git a/include/linux/modem/modem.h b/include/linux/modem/modem.h
new file mode 100644
index 00000000000..c9614a9b061
--- /dev/null
+++ b/include/linux/modem/modem.h
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com>
+ *
+ * Heavily adapted from Regulator framework
+ */
+#ifndef __MODEM_H__
+#define __MODEM_H__
+
+#include <linux/device.h>
+
+struct modem_dev;
+
+struct modem_ops {
+	void (*request)(struct modem_dev *);
+	void (*release)(struct modem_dev *);
+	int (*is_requested)(struct modem_dev *);
+};
+
+struct modem_desc {
+	const char *name;
+	int id;
+	struct modem_ops *ops;
+	struct module *owner;
+};
+
+struct modem_dev {
+	struct modem_desc *desc;
+	int use_count;
+	int open_count;
+	int exclusive;
+
+	struct list_head modem_list;
+
+	struct list_head client_list;
+
+	struct blocking_notifier_head notifier;
+	struct mutex mutex;
+	struct module *owner;
+	struct device dev;
+	void *modem_data;
+};
+
+#ifdef CONFIG_MODEM
+struct modem_dev *modem_register(struct modem_desc *modem_desc,
+		struct device *dev,
+		void *driver_data);
+void modem_unregister(struct modem_dev *mdev);
+
+#else
+static inline struct modem_dev *modem_register(struct modem_desc *modem_desc,
+		struct device *dev, void *driver_data)
+{
+	return NULL;
+}
+
+static inline void modem_unregister(struct modem_dev *mdev)
+{
+}
+#endif
+#endif /* __MODEM_H__ */
diff --git a/include/linux/modem/modem_client.h b/include/linux/modem/modem_client.h
new file mode 100644
index 00000000000..21f04798490
--- /dev/null
+++ b/include/linux/modem/modem_client.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com>
+ *
+ * Heavily adapted from Regulator framework
+ */
+#ifndef __MODEM_CLIENT_H__
+#define __MODEM_CLIENT_H__
+
+#include <linux/device.h>
+
+struct modem;
+
+#ifdef CONFIG_MODEM
+struct modem *modem_get(struct device *dev, const char *id);
+void modem_put(struct modem *modem);
+void modem_request(struct modem *modem);
+void modem_release(struct modem *modem);
+int modem_is_requested(struct modem *modem);
+int modem_get_usage(struct modem *modem);
+
+#else
+
+static inline struct modem *modem_get(struct device *dev, const char *id)
+{
+	return NULL;
+}
+
+static inline void modem_put(struct modem *modem)
+{
+}
+
+static inline void modem_request(struct modem *modem)
+{
+}
+
+static inline void modem_release(struct modem *modem)
+{
+}
+
+static inline int modem_is_requested(struct modem *modem)
+{
+	return 0;
+}
+
+static inline int modem_get_usage(struct modem *modem)
+{
+	return 0;
+}
+#endif
+#endif /* __MODEM_CLIENT_H__ */
diff --git a/include/linux/modem/shrm/shrm.h b/include/linux/modem/shrm/shrm.h
new file mode 100644
index 00000000000..6deeeb16ba8
--- /dev/null
+++ b/include/linux/modem/shrm/shrm.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghavi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __SHM_DRIVER_IF_H__
+#define __SHM_DRIVER_IF_H__
+
+#include <linux/device.h>
+
+/* forward declaration */
+struct shrm_dev;
+
+typedef void (*rx_cb)(void *data, unsigned int length);
+typedef void (*received_msg_handler)(unsigned char l2_header,
+			void *msg_ptr, unsigned int length,
+			struct shrm_dev *shrm);
+
+#endif
diff --git a/include/linux/modem/shrm/shrm_config.h b/include/linux/modem/shrm/shrm_config.h
new file mode 100644
index 00000000000..a82b35ef77b
--- /dev/null
+++ b/include/linux/modem/shrm/shrm_config.h
@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghavi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __SHRM_CONFIG_H
+#define __SHRM_CONFIG_H
+
+
+/*
+Note: modem need to define IPC as a non-cacheable area.
+In Cortex R4 MPU requires that base address of NC area is aligned on a
+region-sized boundary.On modem side, only 1 NC area can be defined, hence
+the whole IPC area must be defined as NC (at least).
+
+*/
+
+/* cache line size = 32bytes*/
+#define SHM_CACHE_LINE 32
+#define SHM_PTR_SIZE 4
+
+/* FIFO 0 address configuration */
+/* ---------------------------- */
+/* 128KB */
+#define SHM_FIFO_0_SIZE (128*1024)
+
+
+/* == APE addresses == */
+#ifdef CONFIG_SHRM_V1_UPDATES_VERSION
+#define SHM_IPC_BASE_AMCU 0x06F80000
+#else
+#define SHM_IPC_BASE_AMCU 0x06000000
+#endif
+
+/* offset pointers */
+#define SHM_ACFIFO_0_WRITE_AMCU SHM_IPC_BASE_AMCU
+#define SHM_ACFIFO_0_READ_AMCU (SHM_ACFIFO_0_WRITE_AMCU + SHM_PTR_SIZE)
+#define SHM_CAFIFO_0_WRITE_AMCU (SHM_ACFIFO_0_WRITE_AMCU + SHM_CACHE_LINE)
+#define SHM_CAFIFO_0_READ_AMCU (SHM_CAFIFO_0_WRITE_AMCU + SHM_PTR_SIZE)
+/* FIFO start */
+#define SHM_ACFIFO_0_START_AMCU (SHM_CAFIFO_0_WRITE_AMCU + SHM_CACHE_LINE)
+#define SHM_CAFIFO_0_START_AMCU (SHM_ACFIFO_0_START_AMCU + SHM_FIFO_0_SIZE)
+
+
+/* == CMT addresses ==*/
+#define SHM_IPC_BASE_CMCU (SHM_IPC_BASE_AMCU+0x08000000)
+/* offset pointers */
+#define SHM_ACFIFO_0_WRITE_CMCU SHM_IPC_BASE_CMCU
+#define SHM_ACFIFO_0_READ_CMCU (SHM_ACFIFO_0_WRITE_CMCU + SHM_PTR_SIZE)
+#define SHM_CAFIFO_0_WRITE_CMCU (SHM_ACFIFO_0_WRITE_CMCU + SHM_CACHE_LINE)
+#define SHM_CAFIFO_0_READ_CMCU (SHM_CAFIFO_0_WRITE_CMCU + SHM_PTR_SIZE)
+/* FIFO*/
+#define SHM_ACFIFO_0_START_CMCU (SHM_CAFIFO_0_WRITE_CMCU + SHM_CACHE_LINE)
+#define SHM_CAFIFO_0_START_CMCU (SHM_ACFIFO_0_START_CMCU + SHM_FIFO_0_SIZE)
+
+
+/* ADSP addresses*/
+#define SHM_ACFIFO_0_START_ADSP 0x0
+#define SHM_CAFIFO_0_START_ADSP 0x0
+#define SHM_ACFIFO_0_WRITE_ADSP 0x0
+#define SHM_ACFIFO_0_READ_ADSP 0x0
+#define SHM_CAFIFO_0_WRITE_ADSP 0x0
+#define SHM_CAFIFO_0_READ_ADSP 0x0
+
+/* FIFO 1 address configuration */
+/* ---------------------------- */
+
+
+/* FIFO 1 - 4K  */
+#define SHM_FIFO_1_SIZE (4*1024)
+
+
+/* == APE addresses == */
+#define SHM_ACFIFO_1_WRITE_AMCU (SHM_CAFIFO_0_START_AMCU + SHM_FIFO_0_SIZE)
+#define SHM_ACFIFO_1_READ_AMCU (SHM_ACFIFO_1_WRITE_AMCU + SHM_PTR_SIZE)
+#define SHM_CAFIFO_1_WRITE_AMCU (SHM_ACFIFO_1_WRITE_AMCU + SHM_CACHE_LINE)
+#define SHM_CAFIFO_1_READ_AMCU (SHM_CAFIFO_1_WRITE_AMCU + SHM_PTR_SIZE)
+/* FIFO*/
+#define SHM_ACFIFO_1_START_AMCU (SHM_CAFIFO_1_WRITE_AMCU + SHM_CACHE_LINE)
+#define SHM_CAFIFO_1_START_AMCU (SHM_ACFIFO_1_START_AMCU + SHM_FIFO_1_SIZE)
+
+
+/* == CMT addresses ==*/
+#define SHM_ACFIFO_1_WRITE_CMCU (SHM_CAFIFO_0_START_CMCU + SHM_FIFO_0_SIZE)
+#define SHM_ACFIFO_1_READ_CMCU (SHM_ACFIFO_1_WRITE_CMCU + SHM_PTR_SIZE)
+#define SHM_CAFIFO_1_WRITE_CMCU (SHM_ACFIFO_1_WRITE_CMCU + SHM_CACHE_LINE)
+#define SHM_CAFIFO_1_READ_CMCU (SHM_CAFIFO_1_WRITE_CMCU + SHM_PTR_SIZE)
+/* FIFO1 start */
+#define SHM_ACFIFO_1_START_CMCU (SHM_CAFIFO_1_WRITE_CMCU + SHM_CACHE_LINE)
+#define SHM_CAFIFO_1_START_CMCU (SHM_ACFIFO_1_START_CMCU + SHM_FIFO_1_SIZE)
+
+
+/* ADSP addresses*/
+#define SHM_ACFIFO_1_START_ADSP 0x0
+#define SHM_CAFIFO_1_START_ADSP 0x0
+#define SHM_ACFIFO_1_WRITE_ADSP 0x0
+#define SHM_ACFIFO_1_READ_ADSP 0x0
+#define SHM_CAFIFO_1_WRITE_ADSP 0x0
+#define SHM_CAFIFO_1_READ_ADSP 0x0
+
+
+#define U8500_SHM_FIFO_APE_COMMON_BASE  (SHM_ACFIFO_0_START_AMCU)
+#define U8500_SHM_FIFO_CMT_COMMON_BASE  (SHM_CAFIFO_0_START_AMCU)
+#define U8500_SHM_FIFO_APE_AUDIO_BASE   (SHM_ACFIFO_1_START_AMCU)
+#define U8500_SHM_FIFO_CMT_AUDIO_BASE   (SHM_CAFIFO_1_START_AMCU)
+
+#endif /* __SHRM_CONFIG_H */
diff --git a/include/linux/modem/shrm/shrm_driver.h b/include/linux/modem/shrm/shrm_driver.h
new file mode 100644
index 00000000000..e7b87005565
--- /dev/null
+++ b/include/linux/modem/shrm/shrm_driver.h
@@ -0,0 +1,202 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghavi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __SHRM_DRIVER_H__
+#define __SHRM_DRIVER_H__
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/clk.h>
+#include <linux/ioport.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/sysfs.h>
+#include <linux/modem/modem_client.h>
+#include <linux/modem/shrm/shrm.h>
+#include <linux/cdev.h>
+
+#define ISA_DEVICES 7
+
+#define BOOT_INIT  (0)
+#define BOOT_INFO_SYNC  (1)
+#define BOOT_DONE  (2)
+#define BOOT_UNKNOWN (3)
+
+/**
+ * struct shrm_dev - shrm device information
+ * @ca_wake_irq:		CMT wake interrupt number
+ * @ac_read_notif_0_irq:	ape-cmt common channel read notify interrupt
+ * @ac_read_notif_1_irq:	ape-cmt audio channel read notify interrupt
+ * @ca_msg_pending_notif_0_irq:	cmt-ape common channel msg pending interrupt
+ * @ca_msg_pending_notif_1_irq:	cmt-ape audio channel msg pending interrupt
+ * @intr_base:			interrupt base register address
+ * @ape_common_fifo_base:	ape side common channel fifo base addr
+ * @ape_audio_fifo_base:	ape side audio channel fifo base addr
+ * @cmt_common_fifo_base:	cmt side common channel fifo base addr
+ * @cmt_audio_fifo_base:	cmt side audio channel fifo base addr
+ * @ape_common_fifo_base_phy:	physical addr of ape common fifo
+ * @ape_audio_fifo_base_phy:	physical addr of ape audio fifo
+ * @cmt_common_fifo_base_phy:	physical addr of cmt common fifo
+ * @cmt_audio_fifo_base_phy:	physical addr of cmt audio fifo
+ * @ape_common_fifo_size:	ape side common channel fifo size
+ * @ape_audio_fifo_size:	ape side audio channel fifo size
+ * @cmt_common_fifo_size:	cmt side common channel fifo size
+ * @cmt_audio_fifo_size:	cmt side audio channel fifo size
+ * @netdev_flag_up:		flag to indicate up/down of netwok device
+ * @msr_flag:			flag to check on-going MSR sequence
+ * @ac_common_shared_wptr:	ape-cmt common channel write pointer
+ * @ac_common_shared_rptr:	ape-cmt common channel read pointer
+ * @ca_common_shared_wptr:	cmt-ape common channel write pointer
+ * @ca_common_shared_rptr:	cmt-ape common channel read pointer
+ * @ac_audio_shared_wptr:	ape-cmt audio channel write pointer
+ * @ac_audio_shared_rptr:	ape-cmt audio channel read pointer
+ * @ca_audio_shared_wptr:	cmt-ape audio channel write pointer
+ * @ca_audio_shared_rptr:	cmt-ape audio channel read pointer
+ * @dev:			pointer to the driver device
+ * @ndev:			pointer to the network device structure
+ * @isa_context:		pointer to t_isa_driver_sontext dtructure
+ * @shm_common_ch_wr_wq:	work queue for writing to common channel
+ * @shm_audio_ch_wr_wq:		workqueue for writing to audio channel
+ * @shm_ac_wake_wq:		workqueue for receiving ape-cmt wake requests
+ * @shm_ca_wake_wq:		workqueue for receiving cmt-ape wake requests
+ * @shm_ac_sleep_wq:		workqueue for recieving ape-cmt sleep requests
+ * @send_ac_msg_pend_notify_0:	work for handling pending message on common
+ * channel
+ * @send_ac_msg_pend_notify_1:	work for handling pending message on audio
+ * channel
+ * @shm_ac_wake_req:		work to send ape-cmt wake request
+ * @shm_ca_wake_req:		work to send cmt-ape wake request
+ * @shm_ca_sleep_req:		work to send cmt-ape sleep request
+ * @shm_ac_sleep_req:		work to send ape-cmt sleep request
+ */
+struct shrm_dev {
+	u8 ca_wake_irq;
+	u8 ac_read_notif_0_irq;
+	u8 ac_read_notif_1_irq;
+	u8 ca_msg_pending_notif_0_irq;
+	u8 ca_msg_pending_notif_1_irq;
+	void __iomem *intr_base;
+	void __iomem *ape_common_fifo_base;
+	void __iomem *ape_audio_fifo_base;
+	void __iomem *cmt_common_fifo_base;
+	void __iomem *cmt_audio_fifo_base;
+
+	u32 *ape_common_fifo_base_phy;
+	u32 *ape_audio_fifo_base_phy;
+	u32 *cmt_common_fifo_base_phy;
+	u32 *cmt_audio_fifo_base_phy;
+
+	int ape_common_fifo_size;
+	int ape_audio_fifo_size;
+	int cmt_common_fifo_size;
+	int cmt_audio_fifo_size;
+	int netdev_flag_up;
+	int msr_flag;
+
+	void __iomem *ac_common_shared_wptr;
+	void __iomem *ac_common_shared_rptr;
+	void __iomem *ca_common_shared_wptr;
+	void __iomem *ca_common_shared_rptr;
+
+	void __iomem *ac_audio_shared_wptr;
+	void __iomem *ac_audio_shared_rptr;
+	void __iomem *ca_audio_shared_wptr;
+	void __iomem *ca_audio_shared_rptr;
+
+	struct device *dev;
+	struct net_device *ndev;
+	struct modem *modem;
+	struct isa_driver_context *isa_context;
+	struct workqueue_struct *shm_common_ch_wr_wq;
+	struct workqueue_struct *shm_audio_ch_wr_wq;
+	struct workqueue_struct *shm_ac_wake_wq;
+	struct workqueue_struct *shm_ca_wake_wq;
+	struct workqueue_struct *shm_ac_sleep_wq;
+	struct work_struct send_ac_msg_pend_notify_0;
+	struct work_struct send_ac_msg_pend_notify_1;
+	struct work_struct shm_ac_wake_req;
+	struct work_struct shm_ca_wake_req;
+	struct work_struct shm_ca_sleep_req;
+	struct work_struct shm_ac_sleep_req;
+};
+
+/**
+ * struct queue_element - information to add an element to queue
+ * @entry:	list entry
+ * @offset:	message offset
+ * @size:	message size
+ * @no:		total number of messages
+ */
+struct queue_element {
+	struct list_head entry;
+	u32 offset;
+	u32 size;
+	u32 no;
+};
+
+/**
+ * struct message_queue - ISI, RPC, AUDIO, SECURITY message queue information
+ * @fifo_base:		pointer to the respective fifo base
+ * @size:		size of the data to be read
+ * @readptr:		fifo read pointer
+ * @writeptr:		fifo write pointer
+ * @no:			total number of messages
+ * @update_lock:	spinlock for protecting the queue read operation
+ * @q_rp:		queue write pointer
+ * @wq_readable:	wait queue head
+ * @msg_list:		message list
+ * @shrm:		pointer to shrm device information structure
+ */
+struct message_queue {
+      u8 *fifo_base;
+      u32 size;
+      u32 readptr;
+      u32 writeptr;
+      u32 no;
+      spinlock_t update_lock;
+      atomic_t q_rp;
+      wait_queue_head_t wq_readable;
+      struct list_head msg_list;
+      struct shrm_dev *shrm;
+};
+
+/**
+ * struct isadev_context - shrm char interface context
+ * @dl_queue:	structre to store the queue related info
+ * @device_id:	message id(ISI, RPC, AUDIO, SECURITY)
+ * @addr:	device addresses.
+ */
+struct isadev_context {
+	struct message_queue dl_queue;
+	u8 device_id;
+	void *addr;
+};
+
+/**
+ * struct isa_driver_context - shrm char interface device information
+ * @is_open:		flag to check the usage of queue
+ * @isadev:		pointer to struct t_isadev_context
+ * @common_tx:		spinlock for protecting common channel
+ * @tx_audio_mutex:	mutex for protecting audio channel
+ * @cdev:		character device structre
+ * @shm_class:		pointer to the class structure
+ */
+struct isa_driver_context {
+	atomic_t is_open[ISA_DEVICES];
+	struct isadev_context *isadev;
+	spinlock_t common_tx;
+	struct mutex tx_audio_mutex;
+	struct cdev cdev;
+	struct class *shm_class;
+};
+
+#endif
diff --git a/include/linux/modem/shrm/shrm_net.h b/include/linux/modem/shrm/shrm_net.h
new file mode 100644
index 00000000000..a97b276ee15
--- /dev/null
+++ b/include/linux/modem/shrm/shrm_net.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2009
+ *
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __SHRM_NET_H
+#define __SHRM_NET_H
+
+#define SHRM_HLEN 1
+#define PHONET_ALEN 1
+
+#define PN_PIPE		0xD9
+#define PN_DEV_HOST	0x00
+#define PN_LINK_ADDR	0x26
+#define PN_TX_QUEUE_LEN	3
+
+#define RESOURCE_ID_INDEX	3
+#define SRC_OBJ_INDEX		7
+#define MSG_ID_INDEX		9
+#define PIPE_HDL_INDEX		10
+#define NETLINK_SHRM            20
+
+/**
+ * struct shrm_net_iface_priv - shrm net interface device information
+ * @shrm_device:	pointer to the shrm device information structure
+ * @iface_num:		flag used to indicate the up/down of netdev
+ */
+struct shrm_net_iface_priv {
+	struct shrm_dev *shrm_device;
+	unsigned int iface_num;
+};
+
+int shrm_register_netdev(struct shrm_dev *shrm_dev_data);
+int shrm_net_receive(struct net_device *dev);
+int shrm_suspend_netdev(struct net_device *dev);
+int shrm_resume_netdev(struct net_device *dev);
+int shrm_stop_netdev(struct net_device *dev);
+int shrm_restart_netdev(struct net_device *dev);
+int shrm_start_netdev(struct net_device *dev);
+void shrm_unregister_netdev(struct shrm_dev *shrm_dev_data);
+
+#endif /* __SHRM_NET_H */
diff --git a/include/linux/modem/shrm/shrm_private.h b/include/linux/modem/shrm/shrm_private.h
new file mode 100644
index 00000000000..888a7c200fd
--- /dev/null
+++ b/include/linux/modem/shrm/shrm_private.h
@@ -0,0 +1,182 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghavi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __SHRM_PRIVATE_INCLUDED
+#define __SHRM_PRIVATE_INCLUDED
+
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/interrupt.h>
+#include <linux/modem/shrm/shrm.h>
+
+#define GOP_OUTPUT_REGISTER_BASE (0x0)
+#define GOP_SET_REGISTER_BASE    (0x4)
+#define GOP_CLEAR_REGISTER_BASE  (0x8)
+#define GOP_TOGGLE_REGISTER_BASE (0xc)
+
+
+#define GOP_AUDIO_AC_READ_NOTIFICATION_BIT (0)
+#define GOP_AUDIO_CA_MSG_PENDING_NOTIFICATION_BIT (1)
+#define GOP_COMMON_AC_READ_NOTIFICATION_BIT (2)
+#define GOP_COMMON_CA_MSG_PENDING_NOTIFICATION_BIT (3)
+#define GOP_CA_WAKE_REQ_BIT (7)
+#define GOP_AUDIO_CA_READ_NOTIFICATION_BIT (23)
+#define GOP_AUDIO_AC_MSG_PENDING_NOTIFICATION_BIT (24)
+#define GOP_COMMON_CA_READ_NOTIFICATION_BIT (25)
+#define GOP_COMMON_AC_MSG_PENDING_NOTIFICATION_BIT (26)
+#define GOP_CA_WAKE_ACK_BIT (27)
+
+#define L2_MSG_MAPID_OFFSET (24)
+#define L1_MSG_MAPID_OFFSET (28)
+
+#define SHRM_SLEEP_STATE (0)
+#define SHRM_PTR_FREE (1)
+#define SHRM_PTR_BUSY (2)
+#define SHRM_IDLE (3)
+
+#define ISI_MESSAGING (0)
+#define RPC_MESSAGING (1)
+#define AUDIO_MESSAGING (2)
+#define SECURITY_MESSAGING (3)
+#define COMMON_LOOPBACK_MESSAGING (0xC0)
+#define AUDIO_LOOPBACK_MESSAGING (0x80)
+#define CIQ_MESSAGING (0xC3)
+
+#define COMMON_CHANNEL		0
+#define AUDIO_CHANNEL		1
+
+typedef void (*MSG_PENDING_NOTIF)(const u32 Wptr);
+
+/**
+ * struct fifo_write_params - parameters used for FIFO write operation.
+ * @writer_local_rptr:	pointer to local read buffer
+ * @writer_local_wptr:	pointer to local write buffer
+ * @shared_wptr:	write pointer shared by cmt and ape
+ * @shared_rptr:	read pointer shared by cmt and ape
+ * @availablesize:	available memory in fifo
+ * @end_addr_fifo:	fifo end addr
+ * @fifo_virtual_addr:	fifo virtual addr
+ * @fifo_update_lock:	spin lock to update fifo.
+ *
+ * On writting a message to FIFO the same has to be read by the modem before
+ * writing the next message to the FIFO. In oder to over come this a local
+ * write and read pointer is used for internal purpose.
+ */
+struct fifo_write_params {
+	u32 writer_local_rptr;
+	u32 writer_local_wptr;
+	u32 shared_wptr;
+	u32 shared_rptr;
+	u32 availablesize;
+	u32 end_addr_fifo;
+	u32 *fifo_virtual_addr;
+	spinlock_t fifo_update_lock;
+} ;
+
+/**
+ * struct fifo_read_params - parameters used for FIFO read operation
+ * @reader_local_rptr:	pointer to local read buffer
+ * @reader_local_wptr:	pointer to local write buffer
+ * @shared_wptr:	write pointer shared by cmt and ape
+ * @shared_rptr:	read pointer shared by cmt and ape
+ * @availablesize:	available memory in fifo
+ * @end_addr_fifo:	fifo end add
+ * @fifo_virtual_addr:	fifo virtual addr
+ */
+struct fifo_read_params{
+	u32 reader_local_rptr;
+	u32 reader_local_wptr;
+	u32 shared_wptr;
+	u32 shared_rptr;
+	u32 availablesize;
+	u32 end_addr_fifo;
+	u32 *fifo_virtual_addr;
+
+} ;
+
+int shrm_protocol_init(struct shrm_dev *shrm,
+			received_msg_handler common_rx_handler,
+			received_msg_handler audio_rx_handler);
+void shrm_protocol_deinit(struct shrm_dev *shrm);
+void shm_fifo_init(struct shrm_dev *shrm);
+int shm_write_msg_to_fifo(struct shrm_dev *shrm, u8 channel,
+				u8 l2header, void *addr, u32 length);
+int shm_write_msg(struct shrm_dev *shrm,
+			u8 l2_header, void *addr, u32 length);
+
+u8 is_the_only_one_unread_message(struct shrm_dev *shrm,
+						u8 channel, u32 length);
+u8 read_remaining_messages_common(void);
+u8 read_remaining_messages_audio(void);
+u8 read_one_l2msg_audio(struct shrm_dev *shrm,
+			u8 *p_l2_msg, u32 *p_len);
+u8 read_one_l2msg_common(struct shrm_dev *shrm,
+				u8 *p_l2_msg, u32 *p_len);
+void receive_messages_common(struct shrm_dev *shrm);
+void receive_messages_audio(struct shrm_dev *shrm);
+
+void update_ac_common_local_rptr(struct shrm_dev *shrm);
+void update_ac_audio_local_rptr(struct shrm_dev *shrm);
+void update_ca_common_local_wptr(struct shrm_dev *shrm);
+void update_ca_audio_local_wptr(struct shrm_dev *shrm);
+void update_ac_common_shared_wptr(struct shrm_dev *shrm);
+void update_ac_audio_shared_wptr(struct shrm_dev *shrm);
+void update_ca_common_shared_rptr(struct shrm_dev *shrm);
+void update_ca_audio_shared_rptr(struct shrm_dev *shrm);
+
+
+void get_writer_pointers(u8 msg_type, u32 *WriterLocalRptr, \
+			u32 *WriterLocalWptr, u32 *SharedWptr);
+void get_reader_pointers(u8 msg_type, u32 *ReaderLocalRptr, \
+			u32 *ReaderLocalWptr, u32 *SharedRptr);
+u8 read_boot_info_req(struct shrm_dev *shrm,
+				u32 *pConfig,
+				u32 *pVersion);
+void write_boot_info_resp(struct shrm_dev *shrm, u32 Config,
+							u32 Version);
+
+void send_ac_msg_pending_notification_0(struct shrm_dev *shrm);
+void send_ac_msg_pending_notification_1(struct shrm_dev *shrm);
+void ca_msg_read_notification_0(struct shrm_dev *shrm);
+void ca_msg_read_notification_1(struct shrm_dev *shrm);
+
+void set_ca_msg_0_read_notif_send(u8 val);
+u8 get_ca_msg_0_read_notif_send(void);
+void set_ca_msg_1_read_notif_send(u8 val);
+u8 get_ca_msg_1_read_notif_send(void);
+
+irqreturn_t ca_wake_irq_handler(int irq, void *ctrlr);
+irqreturn_t ac_read_notif_0_irq_handler(int irq, void *ctrlr);
+irqreturn_t ac_read_notif_1_irq_handler(int irq, void *ctrlr);
+irqreturn_t ca_msg_pending_notif_0_irq_handler(int irq, void *ctrlr);
+irqreturn_t ca_msg_pending_notif_1_irq_handler(int irq, void *ctrlr);
+
+void shm_ca_msgpending_0_tasklet(unsigned long);
+void shm_ca_msgpending_1_tasklet(unsigned long);
+void shm_ac_read_notif_0_tasklet(unsigned long);
+void shm_ac_read_notif_1_tasklet(unsigned long);
+void shm_ca_wake_req_tasklet(unsigned long);
+
+u8 get_boot_state(void);
+
+int get_ca_wake_req_state(void);
+
+/* shrm character interface */
+int isa_init(struct shrm_dev *shrm);
+void isa_exit(struct shrm_dev *shrm);
+int add_msg_to_queue(struct message_queue *q, u32 size);
+ssize_t isa_read(struct file *filp, char __user *buf, size_t len,
+							loff_t *ppos);
+int get_size_of_new_msg(struct message_queue *q);
+int remove_msg_from_queue(struct message_queue *q);
+void shrm_char_reset_queues(struct shrm_dev *shrm);
+int shrm_get_cdev_index(u8 l2_header);
+int shrm_get_cdev_l2header(u8 idx);
+
+#endif
diff --git a/include/linux/moduleparam.h b/include/linux/moduleparam.h
index ddaae98c53f..b433a7dd954 100644
--- a/include/linux/moduleparam.h
+++ b/include/linux/moduleparam.h
@@ -164,7 +164,7 @@ struct kparam_array
 
 /* We don't get oldget: it's often a new-style param_get_uint, etc. */
 static inline int
-__check_old_set_param(int (*oldset)(const char *, struct kernel_param *))
+__check_old_set_param(int (*oldset)(const char *, const struct kernel_param *))
 {
 	return 0;
 }
diff --git a/include/linux/regulator/ab5500.h b/include/linux/regulator/ab5500.h
new file mode 100644
index 00000000000..909b41b8598
--- /dev/null
+++ b/include/linux/regulator/ab5500.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ */
+
+#ifndef __LINUX_REGULATOR_AB5500_H
+#define __LINUX_REGULATOR_AB5500_H
+
+enum ab5500_regulator_id {
+	AB5500_LDO_D,
+	AB5500_LDO_G,
+	AB5500_LDO_H,
+	AB5500_LDO_K,
+	AB5500_LDO_L,
+	AB5500_LDO_S,
+	AB5500_LDO_VDIGMIC,
+	AB5500_LDO_SIM,
+	AB5500_NUM_REGULATORS,
+};
+
+struct regulator_init_data;
+
+struct ab5500_regulator_platform_data {
+	struct regulator_init_data *regulator;
+	int num_regulator;
+};
+
+#endif
diff --git a/include/linux/regulator/ab8500-debug.h b/include/linux/regulator/ab8500-debug.h
new file mode 100644
index 00000000000..01655fc7fc1
--- /dev/null
+++ b/include/linux/regulator/ab8500-debug.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Authors: Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson
+ */
+
+#ifndef __LINUX_MFD_AB8500_REGULATOR_DEBUG_H
+#define __LINUX_MFD_AB8500_REGULATOR_DEBUG_H
+
+#ifdef CONFIG_REGULATOR_AB8500_DEBUG
+/* AB8500 debug force/restore functions */
+void ab8500_regulator_debug_force(void);
+void ab8500_regulator_debug_restore(void);
+#else
+static inline void ab8500_regulator_debug_force(void) {}
+static inline void ab8500_regulator_debug_restore(void) {}
+#endif
+
+#endif
diff --git a/include/linux/regulator/ab8500.h b/include/linux/regulator/ab8500.h
index 76579f964a2..29b5e19c4f4 100644
--- a/include/linux/regulator/ab8500.h
+++ b/include/linux/regulator/ab8500.h
@@ -23,6 +23,8 @@ enum ab8500_regulator_id {
 	AB8500_LDO_ANAMIC2,
 	AB8500_LDO_DMIC,
 	AB8500_LDO_ANA,
+	AB8500_SYSCLKREQ_2,
+	AB8500_SYSCLKREQ_4,
 	AB8500_NUM_REGULATORS,
 };
 
@@ -67,7 +69,15 @@ enum ab8500_regulator_reg {
 	AB8500_REGUCTRL2SPARE,
 	AB8500_REGUCTRLDISCH,
 	AB8500_REGUCTRLDISCH2,
+	AB8500_ARMREGU2, /* NOTE! PRCMU register */
+	AB8500_VBBSEL1, /* NOTE! PRCMU register */
+	AB8500_VBBSEL2, /* NOTE! PRCMU register */
+	AB8500_VSMPS1REGU,
+	AB8500_VSMPS2REGU,
+	AB8500_VSMPS3REGU, /* NOTE! PRCMU register */
 	AB8500_VSMPS1SEL1,
+	AB8500_VSMPS3SEL1, /* NOTE! PRCMU register */
+	AB8500_VSMPS3SEL2, /* NOTE! PRCMU register */
 	AB8500_NUM_REGULATOR_REGISTERS,
 };
 
diff --git a/include/linux/regulator/db5500-prcmu.h b/include/linux/regulator/db5500-prcmu.h
new file mode 100644
index 00000000000..fee68795867
--- /dev/null
+++ b/include/linux/regulator/db5500-prcmu.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson
+ *
+ * Interface to power domain regulators on DB5500
+ */
+
+#ifndef __DB5500_REGULATOR_H__
+#define __DB5500_REGULATOR_H__
+
+#include <linux/regulator/dbx500-prcmu.h>
+
+/* Number of DB5500 regulators and regulator enumeration */
+enum db5500_regulator_id {
+	DB5500_REGULATOR_VAPE,
+	DB5500_REGULATOR_SWITCH_SGA,
+	DB5500_REGULATOR_SWITCH_HVA,
+	DB5500_REGULATOR_SWITCH_SIA,
+	DB5500_REGULATOR_SWITCH_DISP,
+	DB5500_REGULATOR_SWITCH_ESRAM12,
+	DB5500_NUM_REGULATORS
+};
+
+#endif
diff --git a/include/linux/regulator/db8500-prcmu.h b/include/linux/regulator/db8500-prcmu.h
index 612062313b6..a3138ea2177 100644
--- a/include/linux/regulator/db8500-prcmu.h
+++ b/include/linux/regulator/db8500-prcmu.h
@@ -11,6 +11,8 @@
 #ifndef __REGULATOR_H__
 #define __REGULATOR_H__
 
+#include <linux/regulator/dbx500-prcmu.h>
+
 /* Number of DB8500 regulators and regulator enumeration */
 enum db8500_regulator_id {
 	DB8500_REGULATOR_VAPE,
@@ -36,10 +38,4 @@ enum db8500_regulator_id {
 	DB8500_NUM_REGULATORS
 };
 
-/*
- * Exported interface for CPUIdle only. This function is called with all
- * interrupts turned off.
- */
-int power_state_active_is_enabled(void);
-
 #endif
diff --git a/include/linux/regulator/dbx500-prcmu.h b/include/linux/regulator/dbx500-prcmu.h
new file mode 100644
index 00000000000..a18cf6c4107
--- /dev/null
+++ b/include/linux/regulator/dbx500-prcmu.h
@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Bengt Jonsson <bengt.jonsson@stericsson.com> for ST-Ericsson,
+ *	   Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ *
+ * License Terms: GNU General Public License v2
+ *
+ */
+
+#ifndef DBX500_REGULATOR_H
+#define DBX500_REGULATOR_H
+
+#include <linux/device.h>
+
+struct ux500_regulator;
+
+#ifdef CONFIG_REGULATOR
+/*
+ * NOTE! The device will be connected to the correct regulator by this
+ * new framework. A list with connections will match up dev_name(dev)
+ * to the specific regulator. This follows the same principle as the
+ * normal regulator framework.
+ *
+ * This framework shall only be used in special cases when a regulator
+ * has to be enabled/disabled in atomic context.
+ */
+
+void power_state_active_enable(void);
+int power_state_active_disable(void);
+/*
+ * Exported interface for CPUIdle only. This function is called with all
+ * interrupts turned off.
+ */
+int power_state_active_is_enabled(void);
+
+/**
+ * ux500_regulator_get()
+ *
+ * @dev: Drivers device struct
+ *
+ * Returns a ux500_regulator struct. Shall be used as argument for
+ * ux500_regulator_atomic_enable/disable calls.
+ * Return ERR_PTR(-EINVAL) upon no matching regulator found.
+ */
+struct ux500_regulator *__must_check ux500_regulator_get(struct device *dev);
+
+/**
+ * ux500_regulator_atomic_enable()
+ *
+ * @regulator: Regulator handle, provided from ux500_regulator_get.
+ *
+ * The enable/disable functions keep an internal counter, so every
+ * enable must be paired with an disable in order to turn off regulator.
+ */
+int ux500_regulator_atomic_enable(struct ux500_regulator *regulator);
+
+/**
+ * ux500_regulator_atomic_disable()
+ *
+ * @regulator: Regulator handle, provided from ux500_regulator_get.
+ *
+ */
+int ux500_regulator_atomic_disable(struct ux500_regulator *regulator);
+
+/**
+ * ux500_regulator_put()
+ *
+ * @regulator: Regulator handle, provided from ux500_regulator_get.
+ */
+void ux500_regulator_put(struct ux500_regulator *regulator);
+#else
+static inline struct ux500_regulator *__must_check
+ux500_regulator_get(struct device *dev)
+{
+	return ERR_PTR(-EINVAL);
+}
+
+static inline int
+ux500_regulator_atomic_enable(struct ux500_regulator *regulator)
+{
+	return -EINVAL;
+}
+
+static inline int
+ux500_regulator_atomic_disable(struct ux500_regulator *regulator)
+{
+	return -EINVAL;
+}
+
+static inline void ux500_regulator_put(struct ux500_regulator *regulator)
+{
+}
+#endif
+
+#endif
diff --git a/include/linux/spi/stm_msp.h b/include/linux/spi/stm_msp.h
new file mode 100644
index 00000000000..501023105cb
--- /dev/null
+++ b/include/linux/spi/stm_msp.h
@@ -0,0 +1,126 @@
+/*
+ * include/linux/spi/stm_msp.h
+ *
+ * Copyright (C) 2010 STMicroelectronics Pvt. Ltd.
+ *
+ * Author: Sachin Verma <sachin.verma@st.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#ifndef _STM_MSP_H
+#define _STM_MSP_H
+
+#include <linux/device.h>
+
+/* CHIP select/deselect commands */
+enum spi_chip_select {
+	SPI_CHIP_SELECT,
+	SPI_CHIP_DESELECT
+};
+
+/* Common configuration for different SPI controllers */
+enum spi_loopback {
+	SPI_LOOPBACK_DISABLED,
+	SPI_LOOPBACK_ENABLED
+};
+
+enum spi_hierarchy {
+	SPI_MASTER,
+	SPI_SLAVE
+};
+
+/* Endianess of FIFO Data */
+enum spi_fifo_endian {
+	SPI_FIFO_MSB,
+	SPI_FIFO_LSB
+};
+
+/* SPI mode of operation (Communication modes) */
+enum spi_mode {
+	SPI_INTERRUPT_TRANSFER,
+	SPI_POLLING_TRANSFER,
+};
+
+enum msp_data_size {
+	MSP_DATA_BITS_DEFAULT = -1,
+	MSP_DATA_BITS_8 = 0x00,
+	MSP_DATA_BITS_10,
+	MSP_DATA_BITS_12,
+	MSP_DATA_BITS_14,
+	MSP_DATA_BITS_16,
+	MSP_DATA_BITS_20,
+	MSP_DATA_BITS_24,
+	MSP_DATA_BITS_32,
+};
+
+enum msp_clk_src {
+	MSP_INTERNAL_CLK = 0x0,
+	MSP_EXTERNAL_CLK,
+};
+
+struct msp_clock_params {
+	enum msp_clk_src clk_src;
+	/* value from 0 to 1023 */
+	u16 sckdiv;
+	/* Used only when MSPSCK clocks the sample rate
+	 * generator (SCKSEL = 1Xb):
+	 * 0b: The rising edge of MSPSCK clocks the sample rate generator
+	 * 1b: The falling edge of MSPSCK clocks the sample rate generator */
+	int sckpol;
+};
+
+/* Motorola SPI protocol specific definitions */
+enum spi_clk_phase {
+	SPI_CLK_ZERO_CYCLE_DELAY = 0x0,	/* Receive data on rising edge. */
+	SPI_CLK_HALF_CYCLE_DELAY	/* Receive data on falling edge. */
+};
+
+/* SPI Clock Polarity */
+enum spi_clk_pol {
+	SPI_CLK_POL_IDLE_LOW,		/* Low inactive level */
+	SPI_CLK_POL_IDLE_HIGH		/* High inactive level */
+};
+
+struct motorola_spi_proto_params {
+	enum spi_clk_phase clk_phase;
+	enum spi_clk_pol clk_pol;
+};
+
+struct stm_msp_config_chip {
+	struct device *dev;
+	enum spi_loopback lbm;
+	enum spi_hierarchy hierarchy;
+	enum spi_fifo_endian endian_rx;
+	enum spi_fifo_endian endian_tx;
+	enum spi_mode com_mode;
+	enum msp_data_size data_size;
+	struct msp_clock_params clk_freq;
+	int spi_burst_mode_enable;
+	struct motorola_spi_proto_params proto_params;
+	u32 freq;
+	void (*cs_control)(u32 control);
+};
+
+/**
+ * struct stm_msp_controller - device.platform_data for SPI controller devices.
+ *
+ * @num_chipselect: chipselects are used to distinguish individual
+ *      SPI slaves, and are numbered from zero to num_chipselects - 1.
+ *      each slave has a chipselect signal, but it's common that not
+ *      every chipselect is connected to a slave.
+ */
+struct stm_msp_controller {
+	u8 num_chipselect;
+	u32 id;
+	u32 base_addr;
+	char *device_name;
+};
+#endif /* _STM_MSP_H */
diff --git a/include/linux/ste_timed_vibra.h b/include/linux/ste_timed_vibra.h
new file mode 100644
index 00000000000..24edf89aade
--- /dev/null
+++ b/include/linux/ste_timed_vibra.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ *	Grzegorz Sygieda <grzegorz.sygieda@tieto.com> for ST-Ericsson.
+ *	Krzysztof Antonowicz <krzysztof.antonowicz@tieto.com> for ST-Ericsson.
+ *
+ * License Terms: GNU General Public License v2
+ */
+
+#ifndef _STE_TIMED_VIBRA_H_
+#define _STE_TIMED_VIBRA_H
+
+/* Vibrator states */
+enum ste_timed_vibra_states {
+	STE_VIBRA_IDLE = 0,
+	STE_VIBRA_BOOST,
+	STE_VIBRA_ON,
+	STE_VIBRA_OFF,
+};
+
+typedef void (*timed_vibra_control_fp)(
+		unsigned char speed_left_pos,
+		unsigned char speed_left_neg,
+		unsigned char speed_right_pos,
+		unsigned char speed_right_neg);
+
+/*
+ * Vibrator platform data structure
+ * For details check ste_timed_vibra docbook
+ */
+struct ste_timed_vibra_platform_data {
+	bool is_linear_vibra;
+	unsigned int boost_level;	/* p1 */
+	unsigned int boost_time;	/* p2 */
+	unsigned int on_level;		/* p3 */
+	unsigned int off_level;		/* p4 */
+	unsigned int off_time;		/* p5 */
+	timed_vibra_control_fp timed_vibra_control;
+};
+
+#endif /* _STE_TIMED_VIBRA_H_ */
diff --git a/include/linux/sys_soc.h b/include/linux/sys_soc.h
new file mode 100644
index 00000000000..05e5529a6aa
--- /dev/null
+++ b/include/linux/sys_soc.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Maxime Coquelin <maxime.coquelin-nonst@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+#ifndef __SYS_SOC_H
+#define __SYS_SOC_H
+
+#include <linux/kobject.h>
+
+/**
+ * struct sys_soc_info - SoC exports related informations
+ * @name: name of the export
+ * @info: pointer on the key to export
+ * @get_info: callback to retrieve key if info field is NULL
+ * @attr: export's sysdev class attribute
+ */
+struct sysfs_soc_info {
+	const char *info;
+	ssize_t (*get_info)(char *buf, struct sysfs_soc_info *);
+	struct kobj_attribute attr;
+};
+
+ssize_t show_soc_info(struct kobject *, struct kobj_attribute *, char *);
+
+#define SYSFS_SOC_ATTR_VALUE(_name, _value) {		\
+	.attr.attr.name = _name,			\
+	.attr.attr.mode = S_IRUGO,			\
+	.attr.show	= show_soc_info,		\
+	.info           = _value,			\
+}
+
+#define SYSFS_SOC_ATTR_CALLBACK(_name, _callback) {	\
+	.attr.attr.name = _name,			\
+	.attr.attr.mode = S_IRUGO,			\
+	.attr.show	= show_soc_info,		\
+	.get_info       = _callback,			\
+}
+
+/**
+ * register_sys_soc - register the soc information
+ * @name: name of the machine
+ * @info: pointer on the info table to export
+ * @num: number of info to export
+ *
+ * NOTE: This function must only be called once
+ */
+int register_sysfs_soc(struct sysfs_soc_info *info, size_t num);
+
+#endif /* __SYS_SOC_H */
diff --git a/include/linux/tee.h b/include/linux/tee.h
new file mode 100644
index 00000000000..4928e4dca1f
--- /dev/null
+++ b/include/linux/tee.h
@@ -0,0 +1,276 @@
+/*
+ * Trusted Execution Environment (TEE) interface for TrustZone enabled ARM CPUs.
+ *
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com>
+ * Author: Martin Hovang <martin.xm.hovang@stericsson.com>
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef TEE_H
+#define TEE_H
+
+/* tee_cmd id values */
+#define TEED_OPEN_SESSION    0x00000000U
+#define TEED_CLOSE_SESSION   0x00000001U
+#define TEED_INVOKE          0x00000002U
+
+/* tee_retval id values */
+#define TEED_SUCCESS                0x00000000U
+#define TEED_ERROR_GENERIC          0xFFFF0000U
+#define TEED_ERROR_ACCESS_DENIED    0xFFFF0001U
+#define TEED_ERROR_CANCEL           0xFFFF0002U
+#define TEED_ERROR_ACCESS_CONFLICT  0xFFFF0003U
+#define TEED_ERROR_EXCESS_DATA      0xFFFF0004U
+#define TEED_ERROR_BAD_FORMAT       0xFFFF0005U
+#define TEED_ERROR_BAD_PARAMETERS   0xFFFF0006U
+#define TEED_ERROR_BAD_STATE        0xFFFF0007U
+#define TEED_ERROR_ITEM_NOT_FOUND   0xFFFF0008U
+#define TEED_ERROR_NOT_IMPLEMENTED  0xFFFF0009U
+#define TEED_ERROR_NOT_SUPPORTED    0xFFFF000AU
+#define TEED_ERROR_NO_DATA          0xFFFF000BU
+#define TEED_ERROR_OUT_OF_MEMORY    0xFFFF000CU
+#define TEED_ERROR_BUSY             0xFFFF000DU
+#define TEED_ERROR_COMMUNICATION    0xFFFF000EU
+#define TEED_ERROR_SECURITY         0xFFFF000FU
+#define TEED_ERROR_SHORT_BUFFER     0xFFFF0010U
+
+/* TEE origin codes */
+#define TEED_ORIGIN_DRIVER          0x00000002U
+#define TEED_ORIGIN_TEE             0x00000003U
+#define TEED_ORIGIN_TEE_APPLICATION 0x00000004U
+
+#define TEE_UUID_CLOCK_SIZE 8
+
+#define TEEC_CONFIG_PAYLOAD_REF_COUNT 4
+
+/*
+ * Flag constants indicating which of the memory references in an open session
+ * or invoke command operation payload (TEEC_Operation) that are used.
+ */
+#define TEEC_MEMREF_0_USED  0x00000001
+#define TEEC_MEMREF_1_USED  0x00000002
+#define TEEC_MEMREF_2_USED  0x00000004
+#define TEEC_MEMREF_3_USED  0x00000008
+
+/*
+ * Flag constants indicating the data transfer direction of memory in
+ * TEEC_SharedMemory and TEEC_MemoryReference. TEEC_MEM_INPUT signifies data
+ * transfer direction from the client application to the TEE. TEEC_MEM_OUTPUT
+ * signifies data transfer direction from the TEE to the client application.
+ */
+#define TEEC_MEM_INPUT  0x00000001
+#define TEEC_MEM_OUTPUT 0x00000002
+
+/*
+ * Session login methods, for use in TEEC_OpenSession() as parameter
+ * connectionMethod. Type is t_uint32.
+ *
+ * TEEC_LOGIN_PUBLIC  No login data is provided.
+ */
+#define TEEC_LOGIN_PUBLIC 0x0
+
+/*
+ * Exposed functions (command_id) in the static TA
+ */
+#define TEE_STA_SET_L2CC_PREFETCH_CTRL_REGISTER 11
+
+/**
+ * struct tee_uuid - Structure that represent an uuid.
+ * @timeLow: The low field of the time stamp.
+ * @timeMid: The middle field of the time stamp.
+ * @timeHiAndVersion: The high field of the timestamp multiplexed
+ *                    with the version number.
+ * @clockSeqAndNode: The clock sequence and the node.
+ *
+ * This structure have different naming (camel case) to comply with Global
+ * Platforms TEE Client API spec. This type is defined in RFC4122.
+ */
+struct tee_uuid {
+	uint32_t timeLow;
+	uint16_t timeMid;
+	uint16_t timeHiAndVersion;
+	uint8_t clockSeqAndNode[TEE_UUID_CLOCK_SIZE];
+};
+
+/**
+ * struct tee_sharedmemory - Shared memory block for TEE.
+ * @buffer: The in/out data to TEE.
+ * @size: The size of the data.
+ * @flags: Variable telling whether it is a in, out or in/out parameter.
+ */
+struct tee_sharedmemory {
+	void *buffer;
+	size_t size;
+	uint32_t flags;
+};
+
+/**
+ * struct tee_operation - Payload for sessions or invoke operation.
+ * @shm: Array containing the shared memory buffers.
+ * @flags: Tells which if memory buffers that are in use.
+ */
+struct tee_operation {
+	struct tee_sharedmemory shm[TEEC_CONFIG_PAYLOAD_REF_COUNT];
+	uint32_t flags;
+};
+
+struct tee_context {};
+
+/**
+ * struct tee_session - The session of an open tee device.
+ * @state: The current state in the linux kernel.
+ * @err: Error code (as in Global Platform TEE Client API spec)
+ * @origin: Origin for the error code (also from spec).
+ * @id: Implementation defined type, 0 if not used.
+ * @ta: The trusted application.
+ * @uuid: The uuid for the trusted application.
+ * @cmd: The command to be executed in the trusted application.
+ * @driver_cmd: The command type in the driver. This is used from a client (user
+ *              space to tell the Linux kernel whether it's a open-,
+ *              close-session or if it is an invoke command.
+ * @ta_size: The size of the trusted application.
+ * @op: The payload for the trusted application.
+ * @sync: Mutex to handle multiple use of clients.
+ *
+ * This structure is mainly used in the Linux kernel as a session context for
+ * ongoing operations. Other than that it is also used in the communication with
+ * the user space.
+ */
+struct tee_session {
+	uint32_t state;
+	uint32_t err;
+	uint32_t origin;
+	uint32_t id;
+	void *ta;
+	struct tee_uuid *uuid;
+	unsigned int cmd;
+	unsigned int driver_cmd;
+	unsigned int ta_size;
+	struct tee_operation *op;
+	struct mutex *sync;
+};
+
+/**
+ * struct tee_read - Contains the error message and the origin.
+ * @err: Error code (as in Global Platform TEE Client API spec)
+ * @origin: Origin for the error code (also from spec).
+ *
+ * This is used by user space when a user space application wants to get more
+ * information about an error.
+ */
+struct tee_read {
+	unsigned int err; /* return value */
+	unsigned int origin; /* error origin */
+};
+
+/**
+ * Function that handles the function calls to trusted applications.
+ * @param ts: The session of a operation to be executed.
+ * @param sec_cmd: The type of command to be executed, open-, close-session,
+ *                 invoke command.
+ */
+int call_sec_world(struct tee_session *ts, int sec_cmd);
+
+
+/**
+ * teec_initialize_context() - Initializes a context holding connection
+ * information on the specific TEE.
+ * @param name: A zero-terminated string identifying the TEE to connect to.
+ *              If name is set to NULL, the default TEE is connected to.
+ *              NULL is the only supported value in this version of the
+ *              API implementation.
+ * @param context: The context structure which is to be initialized.
+ *
+ * Initializes a context holding connection information between the calling
+ * client application and the TEE designated by the name string.
+ */
+int teec_initialize_context(const char *name, struct tee_context *context);
+
+/**
+ * teec_finalize_context() - Destroys a context holding connection information
+ * on the specific TEE.
+ * @param context: The context to be destroyed.
+ *
+ * This function destroys an initialized TEE context, closing the connection
+ * between the client application and the TEE. This function must only be
+ * called when all sessions related to this TEE context have been closed and
+ * all shared memory blocks have been released.
+ */
+int teec_finalize_context(struct tee_context *context);
+
+/**
+ * teec_open_session() - Opens a new session with the specified trusted
+ * application.
+ * @param context: The initialized TEE context structure in which scope to
+ *                 open the session.
+ * @param session: The session to initialize.
+ * @param destination: A structure identifying the trusted application with
+ *                     which to open a session. If this is set to NULL the
+ *                     operation TEEC_MEMREF_0 is expected to contain the blob
+ *                     which holds the Trusted Application.
+ * @param connection_method: The connection method to use.
+ * @param connection_data: Any data necessary to connect with the chosen
+ *                         connection method. Not supported should be set to
+ *                         NULL.
+ * @param operation: An operation structure to use in the session. May be
+ *                   set to NULL to signify no operation structure needed.
+ *                   If destination is set to NULL, TEEC_MEMREF_0 is
+ *                   expected to hold the TA binary as described above.
+ * @param error_origin: A parameter which will hold the error origin if this
+ *                      function returns any value other than TEEC_SUCCESS.
+ *
+ * Opens a new session with the specified trusted application. Only
+ * connectionMethod == TEEC_LOGIN_PUBLIC is supported. connectionData and
+ * operation shall be set to NULL.
+ */
+int teec_open_session(struct tee_context *context, struct tee_session *session,
+		      const struct tee_uuid *destination,
+		      unsigned int connection_method,
+		      void *connection_data, struct tee_operation *operation,
+		      unsigned int *error_origin);
+
+/**
+ * teec_close_session() - Closes the session which has been opened with the
+ * specific trusted application.
+ * @param session: The opened session to close.
+ *
+ * Closes the session which has been opened with the specific trusted
+ * application.
+ */
+int teec_close_session(struct tee_session *session);
+
+/**
+ * teec_invoke_command() - Executes a command in the specified trusted
+ * application.
+ * @param destination: A structure identifying the trusted application.
+ * @param command_id: Identifier of the command in the trusted application to
+ *                    invoke.
+ * @param operation: An operation structure to use in the invoke command. May
+ *                   be set to NULL to signify no operation structure needed.
+ * @param error_origin: A parameter which will hold the error origin if this
+ *                      function returns any value other than TEEC_SUCCESS.
+ *
+ * Executes a command in the specified trusted application.
+ */
+int teec_invoke_command(struct tee_session *session, unsigned int command_id,
+			struct tee_operation *operation,
+			unsigned int *error_origin);
+
+/**
+ * teec_allocate_shared_memory() - Allocate shared memory for TEE.
+ * @param context: The initialized TEE context structure in which scope to
+ *                 open the session.
+ * @param shared_memory: Pointer to the allocated shared memory.
+ */
+int teec_allocate_shared_memory(struct tee_context *context,
+				struct tee_sharedmemory *shared_memory);
+
+/**
+ * teec_release_shared_memory() - Free the shared memory.
+ * @param shared_memory: Pointer to the shared memory to be freed.
+ */
+void teec_release_shared_memory(struct tee_sharedmemory *shared_memory);
+
+#endif
diff --git a/include/linux/usb/otg_id.h b/include/linux/usb/otg_id.h
index 46a44637c11..f8508db840c 100644
--- a/include/linux/usb/otg_id.h
+++ b/include/linux/usb/otg_id.h
@@ -28,10 +28,6 @@
  *    get called first.
  * @detect: Called during otg_id_notify.  Return OTG_ID_HANDLED if the USB cable
  *    has been identified
- * @proxy_wait: Called during otg_id_notify if a previous handler returns
- *    OTG_ID_PROXY_WAIT.  This should wait on ID change then call otg_id_notify.
- *    This is used when a handler knows what's connected but can't detect
- *    the change itself.
  * @cancel: Called after detect has returned OTG_ID_HANDLED to ask it to
  *    release detection resources to allow a new identification to occur.
  */
diff --git a/include/linux/videodev2.h b/include/linux/videodev2.h
index 8a4c309d234..1f727647f9e 100644
--- a/include/linux/videodev2.h
+++ b/include/linux/videodev2.h
@@ -1359,6 +1359,53 @@ enum v4l2_mpeg_cx2341x_video_median_filter_type {
 #define V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_MEDIAN_FILTER_TOP 	(V4L2_CID_MPEG_CX2341X_BASE+10)
 #define V4L2_CID_MPEG_CX2341X_STREAM_INSERT_NAV_PACKETS 	(V4L2_CID_MPEG_CX2341X_BASE+11)
 
+/*  Private Base control IDs specific to the CG2900 FM driver as defined by V4L2 */
+#define V4L2_CID_CG2900_RADIO_PRIVATE_BASE  (V4L2_CID_PRIVATE_BASE | 0x1000)
+#define V4L2_CID_CG2900_RADIO_BANDSCAN  (V4L2_CID_CG2900_RADIO_PRIVATE_BASE+1)
+enum v4l2_cg2900_radio_bandscan {
+	V4L2_CG2900_RADIO_BANDSCAN_START    = 0,
+	V4L2_CG2900_RADIO_BANDSCAN_STOP      = 1,
+};
+#define V4L2_CID_CG2900_RADIO_BANDSCAN_GET_RESULTS (V4L2_CID_CG2900_RADIO_PRIVATE_BASE+2)
+#define V4L2_CID_CG2900_RADIO_BLOCKSCAN_START (V4L2_CID_CG2900_RADIO_PRIVATE_BASE+3)
+#define V4L2_CID_CG2900_RADIO_BLOCKSCAN_GET_RESULTS (V4L2_CID_CG2900_RADIO_PRIVATE_BASE+4)
+#define V4L2_CID_CG2900_RADIO_CHIP_STATE (V4L2_CID_CG2900_RADIO_PRIVATE_BASE+5)
+enum v4l2_cg2900_radio_chip_state {
+	V4L2_CG2900_RADIO_STANDBY      = 0,
+	V4L2_CG2900_RADIO_POWERUP      = 1,
+};
+#define V4L2_CID_CG2900_RADIO_RSSI_THRESHOLD  (V4L2_CID_CG2900_RADIO_PRIVATE_BASE+6)
+#define V4L2_CID_CG2900_RADIO_SELECT_ANTENNA  (V4L2_CID_CG2900_RADIO_PRIVATE_BASE+7)
+enum v4l2_cg2900_radio_select_antenna {
+	V4L2_CG2900_RADIO_EMBEDDED_ANTENNA	= 0,
+	V4L2_CG2900_RADIO_WIRED_ANTENNA		= 1,
+};
+#define V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_START  (V4L2_CID_CG2900_RADIO_PRIVATE_BASE+8)
+#define V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_GET_RESULT  (V4L2_CID_CG2900_RADIO_PRIVATE_BASE+9)
+#define V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START  (V4L2_CID_CG2900_RADIO_PRIVATE_BASE+10)
+#define V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_GET_RESULT  (V4L2_CID_CG2900_RADIO_PRIVATE_BASE+11)
+#define V4L2_CID_CG2900_RADIO_TEST_TONE_GENERATOR_SET_STATUS	(V4L2_CID_CG2900_RADIO_PRIVATE_BASE+12)
+enum v4l2_cg2900_radio_test_tone_generator_set_status {
+	V4L2_CG2900_RADIO_TEST_TONE_GEN_OFF	= 0,
+	V4L2_CG2900_RADIO_TEST_TONE_GEN_ON_W_SRC		= 1,
+	V4L2_CG2900_RADIO_TEST_TONE_GENERATOR_ON_WO_SRC	= 2,
+};
+#define V4L2_CID_CG2900_RADIO_TEST_TONE_CONNECT		(V4L2_CID_CG2900_RADIO_PRIVATE_BASE+13)
+enum v4l2_cg2900_radio_test_tone_generator {
+	V4L2_CG2900_RADIO_TEST_TONE_NORMAL_AUDIO	= 0,
+	V4L2_CG2900_RADIO_TEST_TONE_ZERO		= 1,
+	V4L2_CG2900_RADIO_TEST_TONE_TONE_1 = 2,
+	V4L2_CG2900_RADIO_TEST_TONE_TONE_2 = 3,
+	V4L2_CG2900_RADIO_TEST_TONE_TONE_SUM = 4,
+};
+#define V4L2_CID_CG2900_RADIO_TEST_TONE_SET_PARAMS	(V4L2_CID_CG2900_RADIO_PRIVATE_BASE+14)
+#define V4L2_CID_CG2900_RADIO_TUNE_DEEMPHASIS		(V4L2_CID_CG2900_RADIO_PRIVATE_BASE+15)
+enum v4l2_cg2900_radio_deemphasis {
+	V4L2_CG2900_RADIO_DEEMPHASIS_DISABLED	= 0,
+	V4L2_CG2900_RADIO_DEEMPHASIS_50_uS	= 1,
+	V4L2_CG2900_RADIO_DEEMPHASIS_75_uS	= 2,
+};
+
 /*  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)
diff --git a/include/net/caif/caif_hsi.h b/include/net/caif/caif_hsi.h
new file mode 100644
index 00000000000..6ea0d63507d
--- /dev/null
+++ b/include/net/caif/caif_hsi.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author:	Daniel Martensson <Daniel.Martensson@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef CAIF_HSI_H_
+#define CAIF_HSI_H_
+
+#include <net/caif/caif_layer.h>
+#include <net/caif/caif_device.h>
+
+/*
+ * Maximum number of CAIF frames that can reside in the same HSI frame.
+ */
+#define CFHSI_MAX_PKTS 15
+
+/*
+ * Maximum number of bytes used for the frame that can be embedded in the
+ * HSI descriptor.
+ */
+#define CFHSI_MAX_EMB_FRM_SZ 96
+
+/*
+ * Decides if HSI buffers should be prefilled with 0xFF pattern for easier
+ * debugging. Both TX and RX buffers will be filled before the transfer.
+ */
+#define CFHSI_DBG_PREFILL		0
+
+/* Structure describing a HSI packet descriptor. */
+struct cfhsi_desc {
+	u8 header;
+	u8 offset;
+	u16 cffrm_len[CFHSI_MAX_PKTS];
+	u8 emb_frm[CFHSI_MAX_EMB_FRM_SZ];
+} __packed;
+
+/* Size of the complete HSI packet descriptor. */
+#define CFHSI_DESC_SZ (sizeof(struct cfhsi_desc))
+
+/*
+ * Size of the complete HSI packet descriptor excluding the optional embedded
+ * CAIF frame.
+ */
+#define CFHSI_DESC_SHORT_SZ (CFHSI_DESC_SZ - CFHSI_MAX_EMB_FRM_SZ)
+
+/*
+ * Maximum bytes transferred in one transfer.
+ */
+/* TODO: 4096 is temporary... */
+#define CFHSI_MAX_PAYLOAD_SZ (CFHSI_MAX_PKTS * 4096)
+
+/* Size of the complete HSI TX buffer. */
+#define CFHSI_BUF_SZ_TX (CFHSI_DESC_SZ + CFHSI_MAX_PAYLOAD_SZ)
+
+/* Size of the complete HSI RX buffer. */
+#define CFHSI_BUF_SZ_RX ((2 * CFHSI_DESC_SZ) + CFHSI_MAX_PAYLOAD_SZ)
+
+/* Bitmasks for the HSI descriptor. */
+#define CFHSI_PIGGY_DESC		(0x01 << 7)
+
+#define CFHSI_TX_STATE_IDLE			0
+#define CFHSI_TX_STATE_XFER			1
+
+#define CFHSI_RX_STATE_DESC			0
+#define CFHSI_RX_STATE_PAYLOAD		1
+
+/* Structure implemented by the CAIF HSI driver. */
+struct cfhsi_drv {
+	void (*tx_done_cb) (struct cfhsi_drv *drv);
+	void (*rx_done_cb) (struct cfhsi_drv *drv);
+};
+
+/* Structure implemented by HSI device. */
+struct cfhsi_dev {
+	int (*cfhsi_tx) (u8 *ptr, int len, struct cfhsi_dev *dev);
+	int (*cfhsi_rx) (u8 *ptr, int len, struct cfhsi_dev *dev);
+	struct cfhsi_drv *drv;
+};
+
+/* Structure implemented by CAIF HSI drivers. */
+struct cfhsi {
+	struct caif_dev_common cfdev;
+	struct net_device *ndev;
+	struct platform_device *pdev;
+	struct sk_buff_head qhead;
+	struct cfhsi_drv drv;
+	struct cfhsi_dev *dev;
+	int tx_state;
+	int rx_state;
+	u8 *tx_buf;
+	u8 *rx_buf;
+	spinlock_t lock;
+	int flow_off_sent;
+	u32 q_low_mark;
+	u32 q_high_mark;
+	bool flow_stop;
+	struct list_head list;
+};
+
+extern struct platform_driver cfhsi_driver;
+
+#endif		/* CAIF_HSI_H_ */
diff --git a/include/net/caif/caif_spi.h b/include/net/caif/caif_spi.h
index 87c3d11b8e5..914810614d0 100644
--- a/include/net/caif/caif_spi.h
+++ b/include/net/caif/caif_spi.h
@@ -55,10 +55,11 @@
 struct cfspi_xfer {
 	u16 tx_dma_len;
 	u16 rx_dma_len;
-	void *va_tx;
-	dma_addr_t pa_tx;
+	void *va_tx[2];
+	dma_addr_t pa_tx[2];
 	void *va_rx;
 	dma_addr_t pa_rx;
+	int idx_tx;
 };
 
 /* Structure implemented by the SPI interface. */
diff --git a/include/sound/ux500_ab8500.h b/include/sound/ux500_ab8500.h
new file mode 100644
index 00000000000..7858bfdb4fa
--- /dev/null
+++ b/include/sound/ux500_ab8500.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Jarmo K. Kuronen <jarmo.kuronen@symbio.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 UX500_AB8500_H
+#define UX500_AB8500_H
+
+extern struct snd_soc_ops ux500_ab8500_ops[];
+
+struct snd_soc_pcm_runtime;
+
+int ux500_ab8500_startup(struct snd_pcm_substream *substream);
+
+void ux500_ab8500_shutdown(struct snd_pcm_substream *substream);
+
+int ux500_ab8500_hw_params(struct snd_pcm_substream *substream,
+			struct snd_pcm_hw_params *params);
+
+int ux500_ab8500_soc_machine_drv_init(void);
+
+void ux500_ab8500_soc_machine_drv_cleanup(void);
+
+int ux500_ab8500_machine_codec_init(struct snd_soc_pcm_runtime *runtime);
+
+extern void ux500_ab8500_jack_report(int);
+
+#endif
diff --git a/include/sound/ux500_ab8500_ext.h b/include/sound/ux500_ab8500_ext.h
new file mode 100644
index 00000000000..911c70093f0
--- /dev/null
+++ b/include/sound/ux500_ab8500_ext.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 UX500_AB8500_EXT_H
+#define UX500_AB8500_EXT_H
+
+void ux500_ab8500_audio_pwm_vibra(unsigned char speed_left_pos,
+			unsigned char speed_left_neg,
+			unsigned char speed_right_pos,
+			unsigned char speed_right_neg);
+
+#endif
diff --git a/include/trace/Kbuild b/include/trace/Kbuild
new file mode 100644
index 00000000000..7e8b704d610
--- /dev/null
+++ b/include/trace/Kbuild
@@ -0,0 +1 @@
+header-y += stm.h
diff --git a/include/trace/stm.h b/include/trace/stm.h
new file mode 100644
index 00000000000..72e9136c25e
--- /dev/null
+++ b/include/trace/stm.h
@@ -0,0 +1,223 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson STM Trace driver
+ *
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ *         Philippe Langlais <philippe.langlais@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2.
+ */
+
+#ifndef STM_H
+#define STM_H
+
+#define STM_DEV_NAME "stm"
+
+/* One single channel mapping */
+struct stm_channel {
+	union {
+		__u8  no_stamp8;
+		__u16 no_stamp16;
+		__u32 no_stamp32;
+		__u64 no_stamp64;
+	};
+	union {
+		__u8   stamp8;
+		__u16 stamp16;
+		__u32 stamp32;
+		__u64 stamp64;
+	};
+};
+
+/* Possible trace modes */
+#define STM_SW_LOSSLESS	0 /* Software mode: lossless data but intrusive */
+#define STM_HW_LOSSY	1 /* Hardware mode: lossy data but less intrusive */
+
+/* Possible clock setting */
+enum clock_div {
+	STM_CLOCK_DIV2 = 0,
+	STM_CLOCK_DIV4,
+	STM_CLOCK_DIV6,
+	STM_CLOCK_DIV8,
+	STM_CLOCK_DIV10,
+	STM_CLOCK_DIV12,
+	STM_CLOCK_DIV14,
+	STM_CLOCK_DIV16,
+};
+
+/* ioctl commands */
+#define STM_CONNECTION            _IOW('t', 0, enum stm_connection_type)
+#define STM_DISABLE               _IO('t', 1)
+#define STM_GET_NB_MAX_CHANNELS   _IOR('t', 2, int)
+#define STM_GET_NB_FREE_CHANNELS  _IOR('t', 3, int)
+#define STM_GET_CHANNEL_NO        _IOR('t', 4, int)
+#define STM_SET_CLOCK_DIV         _IOW('t', 5, enum clock_div)
+#define STM_GET_CTRL_REG          _IOR('t', 6, int)
+#define STM_ENABLE_SRC            _IOWR('t', 7, int)
+#define STM_GET_FREE_CHANNEL      _IOW('t', 8, int)
+#define STM_RELEASE_CHANNEL       _IOW('t', 9, int)
+#define STM_SET_MODE              _IOWR('t', 10, int)
+#define STM_GET_MODE              _IOR('t', 11, int)
+
+enum stm_connection_type {
+	STM_DISCONNECT = 0,
+	STM_DEFAULT_CONNECTION = 1,
+	STM_STE_MODEM_ON_MIPI34_NONE_ON_MIPI60 = 2,
+	STM_STE_APE_ON_MIPI34_NONE_ON_MIPI60 = 3,
+	STM_STE_MODEM_ON_MIPI34_APE_ON_MIPI60 = 4
+};
+
+#ifdef __KERNEL__
+
+struct stm_platform_data {
+	u32        regs_phys_base;
+	u32        channels_phys_base;
+	u32        id_mask;
+	u32        masters_enabled;
+	const s16 *channels_reserved;
+	int        channels_reserved_sz;
+	int        (*stm_connection)(enum stm_connection_type);
+};
+
+/* Channels base address */
+extern volatile struct stm_channel __iomem *stm_channels;
+
+/* Provides stm_trace_XX() and stm_tracet_XX() trace API */
+#define DEFLLTFUN(size) \
+static inline void stm_trace_##size(int channel, __u##size data) \
+{ \
+	stm_channels[channel].no_stamp##size = data; \
+} \
+static inline void stm_tracet_##size(int channel, __u##size data) \
+{ \
+	stm_channels[channel].stamp##size = data; \
+} \
+
+DEFLLTFUN(8);
+DEFLLTFUN(16);
+DEFLLTFUN(32);
+DEFLLTFUN(64);
+
+/*
+ * Trace a buffer on a given channel
+ * with auto time stamping on the last byte(s) only
+ */
+int stm_trace_buffer_onchannel(int channel, const void *data, size_t length);
+/*
+ * Trace a buffer on a dynamically allocated channel
+ * with auto time stamping on the last byte(s) only
+ * Dynamic channel are allocated in the 128 highest channels
+ */
+int stm_trace_buffer(const void *data, size_t length);
+
+/* printk equivalent for STM */
+int stm_printk(const char *fmt, ...) __attribute__ ((format (printf, 1, 2)));
+
+#if defined(CONFIG_STM_PRINTK)
+#define stm_dup_printk(buf, length) \
+	stm_trace_buffer_onchannel(CONFIG_STM_PRINTK_CHANNEL, buf, length)
+
+#else
+static inline int stm_dup_printk(char *buf, size_t size)
+{
+	return 0;
+}
+#endif
+
+#if defined(CONFIG_STM_TRACE_PRINTK)
+static inline int stm_trace_printk_buf(
+		unsigned long ip, const char *buf, size_t size)
+{
+	stm_trace_32(CONFIG_STM_TRACE_PRINTK_CHANNEL, ip);
+	return stm_trace_buffer_onchannel(CONFIG_STM_TRACE_PRINTK_CHANNEL,
+			buf, size);
+}
+
+static inline int stm_trace_bprintk_buf(
+		unsigned long ip, const char *fmt, const void *buf, size_t size)
+{
+	stm_trace_64(CONFIG_STM_TRACE_BPRINTK_CHANNEL, ((u64)ip<<32)+(u32)fmt);
+	return stm_trace_buffer_onchannel(CONFIG_STM_TRACE_PRINTK_CHANNEL,
+			buf, size);
+}
+#else
+static inline int stm_trace_printk_buf(
+		unsigned long ip, const char *buf, size_t size)
+{
+	return 0;
+}
+
+static inline int stm_trace_bprintk_buf(
+		unsigned long ip, const char *fmt, const void *buf, size_t size)
+{
+	return 0;
+}
+#endif
+
+#if defined(CONFIG_STM_FTRACE)
+static inline void stm_ftrace(unsigned long ip, unsigned long parent_ip)
+{
+	stm_tracet_64(CONFIG_STM_FTRACE_CHANNEL, (((__u64)ip)<<32) + parent_ip);
+}
+#else
+static inline void stm_ftrace(unsigned long ip, unsigned long parent_ip)
+{
+}
+#endif
+
+#if defined(CONFIG_STM_CTX_SWITCH)
+static inline void stm_sched_switch(u32 prev_pid, u8 prev_prio, u8 prev_state,
+			u32 next_pid, u8 next_prio, u8 next_state, u32 next_cpu)
+{
+	stm_trace_64(CONFIG_STM_CTX_SWITCH_CHANNEL,
+			(((__u64)prev_pid)<<32) + next_pid);
+	stm_tracet_64(CONFIG_STM_CTX_SWITCH_CHANNEL, (((__u64)next_cpu)<<32)
+			+ (prev_prio<<24) + (prev_state<<16)
+			+ (next_prio<<8) + next_state);
+}
+#else
+static inline void stm_sched_switch(u32 prev_pid, u8 prev_prio, u8 prev_state,
+			u32 next_pid, u8 next_prio, u8 next_state, u32 next_cpu)
+{
+}
+#endif
+
+#if defined(CONFIG_STM_WAKEUP)
+static inline void stm_sched_wakeup(u32 prev_pid, u8 prev_prio, u8 prev_state,
+			u32 next_pid, u8 next_prio, u8 next_state, u32 next_cpu)
+{
+	stm_trace_64(CONFIG_STM_WAKEUP_CHANNEL,
+			(((__u64)prev_pid)<<32) + next_pid);
+	stm_tracet_64(CONFIG_STM_WAKEUP_CHANNEL, (((__u64)next_cpu)<<32)
+			+ (prev_prio<<24) + (prev_state<<16)
+			+ (next_prio<<8) + next_state);
+}
+#else
+static inline void stm_sched_wakeup(u32 prev_pid, u8 prev_prio, u8 prev_state,
+			u32 next_pid, u8 next_prio, u8 next_state, u32 next_cpu)
+{
+}
+#endif
+
+#if defined(CONFIG_STM_STACK_TRACE)
+static inline void stm_stack_trace(unsigned long *callers)
+{
+	while (*(callers + 1) != ULONG_MAX) {
+		stm_trace_32(CONFIG_STM_STACK_TRACE_CHANNEL, *callers++);
+	}
+	/* Time stamp the latest */
+	stm_tracet_32(CONFIG_STM_STACK_TRACE_CHANNEL, *callers);
+}
+#else
+static inline void stm_stack_trace(unsigned long *callers)
+{
+}
+#endif
+
+/* Alloc/Free STM channel */
+int stm_alloc_channel(int offset);
+void stm_free_channel(int channel);
+
+#endif /* __KERNEL__ */
+
+#endif /* STM_H */
diff --git a/include/video/av8100.h b/include/video/av8100.h
new file mode 100644
index 00000000000..b0dc7b7f1cb
--- /dev/null
+++ b/include/video/av8100.h
@@ -0,0 +1,558 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * AV8100 driver
+ *
+ * Author: Per Persson <per.xb.persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __AV8100__H__
+#define __AV8100__H__
+
+#define AV8100_CEC_MESSAGE_SIZE		16
+#define AV8100_HDCP_SEND_KEY_SIZE	16
+#define AV8100_INFOFRAME_SIZE		28
+#define AV8100_FUSE_KEY_SIZE		16
+#define AV8100_CHIPVER_1		1
+#define AV8100_CHIPVER_2		2
+
+struct av8100_platform_data {
+	unsigned	gpio_base;
+	int		irq;
+	int		reset;
+	const char	*inputclk_id;
+	const char	*regulator_pwr_id;
+	bool		alt_powerupseq;
+	unsigned char	mclk_freq;
+};
+
+enum av8100_error {
+	AV8100_OK = 0x0,
+	AV8100_INVALID_COMMAND = 0x1,
+	AV8100_INVALID_INTERFACE = 0x2,
+	AV8100_INVALID_IOCTL = 0x3,
+	AV8100_COMMAND_FAIL = 0x4,
+	AV8100_FWDOWNLOAD_FAIL = 0x5,
+	AV8100_FAIL = 0xFF,
+};
+
+enum av8100_command_type {
+	AV8100_COMMAND_VIDEO_INPUT_FORMAT  = 0x1,
+	AV8100_COMMAND_AUDIO_INPUT_FORMAT,
+	AV8100_COMMAND_VIDEO_OUTPUT_FORMAT,
+	AV8100_COMMAND_VIDEO_SCALING_FORMAT,
+	AV8100_COMMAND_COLORSPACECONVERSION,
+	AV8100_COMMAND_CEC_MESSAGE_WRITE,
+	AV8100_COMMAND_CEC_MESSAGE_READ_BACK,
+	AV8100_COMMAND_DENC,
+	AV8100_COMMAND_HDMI,
+	AV8100_COMMAND_HDCP_SENDKEY,
+	AV8100_COMMAND_HDCP_MANAGEMENT,
+	AV8100_COMMAND_INFOFRAMES,
+	AV8100_COMMAND_EDID_SECTION_READBACK,
+	AV8100_COMMAND_PATTERNGENERATOR,
+	AV8100_COMMAND_FUSE_AES_KEY,
+};
+
+enum interface_type {
+	I2C_INTERFACE = 0x0,
+	DSI_INTERFACE = 0x1,
+};
+
+enum av8100_dsi_mode {
+	AV8100_HDMI_DSI_OFF,
+	AV8100_HDMI_DSI_COMMAND_MODE,
+	AV8100_HDMI_DSI_VIDEO_MODE
+};
+
+enum av8100_pixel_format {
+	AV8100_INPUT_PIX_RGB565,
+	AV8100_INPUT_PIX_RGB666,
+	AV8100_INPUT_PIX_RGB666P,
+	AV8100_INPUT_PIX_RGB888,
+	AV8100_INPUT_PIX_YCBCR422
+};
+
+enum av8100_video_mode {
+	AV8100_VIDEO_INTERLACE,
+	AV8100_VIDEO_PROGRESSIVE
+};
+
+enum av8100_dsi_nb_data_lane {
+	AV8100_DATA_LANES_USED_0,
+	AV8100_DATA_LANES_USED_1,
+	AV8100_DATA_LANES_USED_2,
+	AV8100_DATA_LANES_USED_3,
+	AV8100_DATA_LANES_USED_4
+};
+
+enum av8100_te_config {
+	AV8100_TE_OFF,		/* NO TE*/
+	AV8100_TE_DSI_LANE,	/* TE generated on DSI lane */
+	AV8100_TE_IT_LINE,	/* TE generated on IT line (GPIO) */
+	AV8100_TE_DSI_IT,	/* TE generatedon both DSI lane & IT line*/
+	AV8100_TE_GPIO_IT	/* TE on GPIO I2S DAT3 & or IT line*/
+};
+
+enum av8100_audio_if_format {
+	AV8100_AUDIO_I2S_MODE,
+	AV8100_AUDIO_I2SDELAYED_MODE, /* I2S Mode by default*/
+	AV8100_AUDIO_TDM_MODE         /* 8 Channels by default*/
+};
+
+enum av8100_sample_freq {
+	AV8100_AUDIO_FREQ_32KHZ,
+	AV8100_AUDIO_FREQ_44_1KHZ,
+	AV8100_AUDIO_FREQ_48KHZ,
+	AV8100_AUDIO_FREQ_64KHZ,
+	AV8100_AUDIO_FREQ_88_2KHZ,
+	AV8100_AUDIO_FREQ_96KHZ,
+	AV8100_AUDIO_FREQ_128KHZ,
+	AV8100_AUDIO_FREQ_176_1KHZ,
+	AV8100_AUDIO_FREQ_192KHZ
+};
+
+enum av8100_audio_word_length {
+	AV8100_AUDIO_16BITS,
+	AV8100_AUDIO_20BITS,
+	AV8100_AUDIO_24BITS
+};
+
+enum av8100_audio_format {
+	AV8100_AUDIO_LPCM_MODE,
+	AV8100_AUDIO_COMPRESS_MODE
+};
+
+enum av8100_audio_if_mode {
+	AV8100_AUDIO_SLAVE,
+	AV8100_AUDIO_MASTER
+};
+
+enum av8100_audio_mute {
+	AV8100_AUDIO_MUTE_DISABLE,
+	AV8100_AUDIO_MUTE_ENABLE
+};
+
+enum av8100_output_CEA_VESA {
+	AV8100_CUSTOM,
+	AV8100_CEA1_640X480P_59_94HZ,
+	AV8100_CEA2_3_720X480P_59_94HZ,
+	AV8100_CEA4_1280X720P_60HZ,
+	AV8100_CEA5_1920X1080I_60HZ,
+	AV8100_CEA6_7_NTSC_60HZ,
+	AV8100_CEA14_15_480p_60HZ,
+	AV8100_CEA16_1920X1080P_60HZ,
+	AV8100_CEA17_18_720X576P_50HZ,
+	AV8100_CEA19_1280X720P_50HZ,
+	AV8100_CEA20_1920X1080I_50HZ,
+	AV8100_CEA21_22_576I_PAL_50HZ,
+	AV8100_CEA29_30_576P_50HZ,
+	AV8100_CEA31_1920x1080P_50Hz,
+	AV8100_CEA32_1920X1080P_24HZ,
+	AV8100_CEA33_1920X1080P_25HZ,
+	AV8100_CEA34_1920X1080P_30HZ,
+	AV8100_CEA60_1280X720P_24HZ,
+	AV8100_CEA61_1280X720P_25HZ,
+	AV8100_CEA62_1280X720P_30HZ,
+	AV8100_VESA9_800X600P_60_32HZ,
+	AV8100_VESA14_848X480P_60HZ,
+	AV8100_VESA16_1024X768P_60HZ,
+	AV8100_VESA22_1280X768P_59_99HZ,
+	AV8100_VESA23_1280X768P_59_87HZ,
+	AV8100_VESA27_1280X800P_59_91HZ,
+	AV8100_VESA28_1280X800P_59_81HZ,
+	AV8100_VESA39_1360X768P_60_02HZ,
+	AV8100_VESA81_1366X768P_59_79HZ,
+	AV8100_VIDEO_OUTPUT_CEA_VESA_MAX
+};
+
+enum av8100_video_sync_pol {
+	AV8100_SYNC_POSITIVE,
+	AV8100_SYNC_NEGATIVE
+};
+
+enum av8100_hdmi_mode {
+	AV8100_HDMI_OFF,
+	AV8100_HDMI_ON,
+	AV8100_HDMI_AVMUTE
+};
+
+enum av8100_hdmi_format {
+	AV8100_HDMI,
+	AV8100_DVI
+};
+
+enum av8100_DVI_format {
+	AV8100_DVI_CTRL_CTL0,
+	AV8100_DVI_CTRL_CTL1,
+	AV8100_DVI_CTRL_CTL2
+};
+
+enum av8100_pattern_type {
+	AV8100_PATTERN_OFF,
+	AV8100_PATTERN_GENERATOR,
+	AV8100_PRODUCTION_TESTING
+};
+
+enum av8100_pattern_format {
+	AV8100_NO_PATTERN,
+	AV8100_PATTERN_VGA,
+	AV8100_PATTERN_720P,
+	AV8100_PATTERN_1080P
+};
+
+enum av8100_pattern_audio {
+	AV8100_PATTERN_AUDIO_OFF,
+	AV8100_PATTERN_AUDIO_ON,
+	AV8100_PATTERN_AUDIO_I2S_MEM
+};
+
+struct av8100_video_input_format_cmd {
+	enum av8100_dsi_mode		dsi_input_mode;
+	enum av8100_pixel_format	input_pixel_format;
+	unsigned short			total_horizontal_pixel;
+	unsigned short			total_horizontal_active_pixel;
+	unsigned short			total_vertical_lines;
+	unsigned short			total_vertical_active_lines;
+	enum av8100_video_mode		video_mode;
+	enum av8100_dsi_nb_data_lane	nb_data_lane;
+	unsigned char			nb_virtual_ch_command_mode;
+	unsigned char			nb_virtual_ch_video_mode;
+	unsigned short			TE_line_nb;
+	enum av8100_te_config		TE_config;
+	unsigned long			master_clock_freq;
+	unsigned char			ui_x4;
+};
+
+struct av8100_audio_input_format_cmd {
+	enum av8100_audio_if_format	audio_input_if_format;
+	unsigned char			i2s_input_nb;
+	enum av8100_sample_freq		sample_audio_freq;
+	enum av8100_audio_word_length	audio_word_lg;
+	enum av8100_audio_format	audio_format;
+	enum av8100_audio_if_mode	audio_if_mode;
+	enum av8100_audio_mute		audio_mute;
+};
+
+struct av8100_video_output_format_cmd {
+	enum av8100_output_CEA_VESA	video_output_cea_vesa;
+	enum av8100_video_sync_pol	vsync_polarity;
+	enum av8100_video_sync_pol	hsync_polarity;
+	unsigned short		total_horizontal_pixel;
+	unsigned short		total_horizontal_active_pixel;
+	unsigned short		total_vertical_in_half_lines;
+	unsigned short		total_vertical_active_in_half_lines;
+	unsigned short		hsync_start_in_pixel;
+	unsigned short		hsync_length_in_pixel;
+	unsigned short		vsync_start_in_half_line;
+	unsigned short		vsync_length_in_half_line;
+	unsigned short		hor_video_start_pixel;
+	unsigned short		vert_video_start_pixel;
+	enum av8100_video_mode	video_type;
+	unsigned short		pixel_repeat;
+	unsigned long		pixel_clock_freq_Hz;
+};
+
+struct av8100_video_scaling_format_cmd {
+	unsigned short	h_start_in_pixel;
+	unsigned short	h_stop_in_pixel;
+	unsigned short	v_start_in_line;
+	unsigned short	v_stop_in_line;
+	unsigned short	h_start_out_pixel;
+	unsigned short	h_stop_out_pixel;
+	unsigned short	v_start_out_line;
+	unsigned short	v_stop_out_line;
+};
+
+enum av8100_color_transform {
+	AV8100_COLOR_TRANSFORM_INDENTITY,
+	AV8100_COLOR_TRANSFORM_INDENTITY_CLAMP_YUV,
+	AV8100_COLOR_TRANSFORM_YUV_TO_RGB,
+	AV8100_COLOR_TRANSFORM_YUV_TO_DENC,
+	AV8100_COLOR_TRANSFORM_RGB_TO_DENC,
+};
+
+struct av8100_cec_message_write_format_cmd {
+	unsigned char buffer_length;
+	unsigned char buffer[AV8100_CEC_MESSAGE_SIZE];
+};
+
+struct av8100_cec_message_read_back_format_cmd {
+};
+
+enum av8100_cvbs_video_format {
+	AV8100_CVBS_625,
+	AV8100_CVBS_525,
+};
+
+enum av8100_standard_selection {
+	AV8100_PAL_BDGHI,
+	AV8100_PAL_N,
+	AV8100_NTSC_M,
+	AV8100_PAL_M
+};
+
+struct av8100_denc_format_cmd {
+	enum av8100_cvbs_video_format cvbs_video_format;
+	enum av8100_standard_selection standard_selection;
+	unsigned char enable;
+	unsigned char macrovision_enable;
+	unsigned char internal_generator;
+};
+
+struct av8100_hdmi_cmd {
+	enum av8100_hdmi_mode	hdmi_mode;
+	enum av8100_hdmi_format	hdmi_format;
+	enum av8100_DVI_format	dvi_format; /* used only if HDMI_format = DVI*/
+};
+
+struct av8100_hdcp_send_key_format_cmd {
+	unsigned char key_number;
+	unsigned char data_len;
+	unsigned char data[AV8100_HDCP_SEND_KEY_SIZE];
+};
+
+enum av8100_hdcp_auth_req_type {
+	AV8100_HDCP_AUTH_REQ_OFF = 0,
+	AV8100_HDCP_AUTH_REQ_ON = 1,
+	AV8100_HDCP_REV_LIST_REQ = 2,
+	AV8100_HDCP_AUTH_CONT = 3,
+};
+
+enum av8100_hdcp_encr_use {
+	AV8100_HDCP_ENCR_USE_OESS = 0,
+	AV8100_HDCP_ENCR_USE_EESS = 1,
+};
+
+struct av8100_hdcp_management_format_cmd {
+	unsigned char req_type;
+	unsigned char encr_use;
+};
+
+struct av8100_infoframes_format_cmd {
+	unsigned char type;
+	unsigned char version;
+	unsigned char length;
+	unsigned char crc;
+	unsigned char data[AV8100_INFOFRAME_SIZE];
+};
+
+struct av8100_edid_section_readback_format_cmd {
+	unsigned char address;
+	unsigned char block_number;
+};
+
+struct av8100_pattern_generator_format_cmd {
+	enum av8100_pattern_type	pattern_type;
+	enum av8100_pattern_format	pattern_video_format;
+	enum av8100_pattern_audio	pattern_audio_mode;
+};
+
+enum av8100_fuse_operation {
+	AV8100_FUSE_READ = 0,
+	AV8100_FUSE_WRITE = 1,
+};
+
+struct av8100_fuse_aes_key_format_cmd {
+	unsigned char fuse_operation;
+	unsigned char key[AV8100_FUSE_KEY_SIZE];
+};
+
+union av8100_configuration {
+	struct av8100_video_input_format_cmd	video_input_format;
+	struct av8100_audio_input_format_cmd	audio_input_format;
+	struct av8100_video_output_format_cmd	video_output_format;
+	struct av8100_video_scaling_format_cmd	video_scaling_format;
+	enum   av8100_color_transform           color_transform;
+	struct av8100_cec_message_write_format_cmd
+		cec_message_write_format;
+	struct av8100_cec_message_read_back_format_cmd
+		cec_message_read_back_format;
+	struct av8100_denc_format_cmd		denc_format;
+	struct av8100_hdmi_cmd			hdmi_format;
+	struct av8100_hdcp_send_key_format_cmd	hdcp_send_key_format;
+	struct av8100_hdcp_management_format_cmd hdcp_management_format;
+	struct av8100_infoframes_format_cmd	infoframes_format;
+	struct av8100_edid_section_readback_format_cmd
+		edid_section_readback_format;
+	struct av8100_pattern_generator_format_cmd pattern_generator_format;
+	struct av8100_fuse_aes_key_format_cmd	fuse_aes_key_format;
+};
+
+enum av8100_operating_mode {
+	AV8100_OPMODE_UNDEFINED = 0,
+	AV8100_OPMODE_SHUTDOWN,
+	AV8100_OPMODE_STANDBY,
+	AV8100_OPMODE_SCAN,
+	AV8100_OPMODE_INIT,
+	AV8100_OPMODE_IDLE,
+	AV8100_OPMODE_VIDEO,
+};
+
+enum av8100_plugin_status {
+	AV8100_PLUGIN_NONE = 0x0,
+	AV8100_HDMI_PLUGIN = 0x1,
+	AV8100_CVBS_PLUGIN = 0x2,
+};
+
+enum av8100_hdmi_event {
+	AV8100_HDMI_EVENT_NONE =		0x0,
+	AV8100_HDMI_EVENT_HDMI_PLUGIN =		0x1,
+	AV8100_HDMI_EVENT_HDMI_PLUGOUT =	0x2,
+	AV8100_HDMI_EVENT_CEC =			0x4,
+	AV8100_HDMI_EVENT_HDCP =		0x8,
+	AV8100_HDMI_EVENT_CECTXERR =		0x10,
+};
+
+struct av8100_status {
+	enum av8100_operating_mode	av8100_state;
+	enum av8100_plugin_status	av8100_plugin_status;
+	int				hdmi_on;
+};
+
+
+int av8100_init(void);
+void av8100_exit(void);
+int av8100_powerup(void);
+int av8100_powerdown(void);
+int av8100_disable_interrupt(void);
+int av8100_enable_interrupt(void);
+int av8100_download_firmware(char *fw_buff, int numOfBytes,
+	enum interface_type if_type);
+int av8100_reg_stby_w(
+		unsigned char cpd,
+		unsigned char stby,
+		unsigned char mclkrng);
+int av8100_reg_hdmi_5_volt_time_w(
+		unsigned char denc_off_time,
+		unsigned char hdmi_off_time,
+		unsigned char on_time);
+int av8100_reg_stby_int_mask_w(
+		unsigned char hpdm,
+		unsigned char cpdm,
+		unsigned char stbygpiocfg,
+		unsigned char ipol);
+int av8100_reg_stby_pend_int_w(
+		unsigned char hpdi,
+		unsigned char cpdi,
+		unsigned char oni,
+		unsigned char bpdig);
+int av8100_reg_gen_int_mask_w(
+		unsigned char eocm,
+		unsigned char vsim,
+		unsigned char vsom,
+		unsigned char cecm,
+		unsigned char hdcpm,
+		unsigned char uovbm,
+		unsigned char tem);
+int av8100_reg_gen_int_w(
+		unsigned char eoci,
+		unsigned char vsii,
+		unsigned char vsoi,
+		unsigned char ceci,
+		unsigned char hdcpi,
+		unsigned char uovbi);
+int av8100_reg_gpio_conf_w(
+		unsigned char dat3dir,
+		unsigned char dat3val,
+		unsigned char dat2dir,
+		unsigned char dat2val,
+		unsigned char dat1dir,
+		unsigned char dat1val,
+		unsigned char ucdbg);
+int av8100_reg_gen_ctrl_w(
+		unsigned char fdl,
+		unsigned char hld,
+		unsigned char wa,
+		unsigned char ra);
+int av8100_reg_fw_dl_entry_w(
+	unsigned char mbyte_code_entry);
+int av8100_reg_w(
+		unsigned char offset,
+		unsigned char value);
+int av8100_reg_stby_r(
+		unsigned char *cpd,
+		unsigned char *stby,
+		unsigned char *hpds,
+		unsigned char *cpds,
+		unsigned char *mclkrng);
+int av8100_reg_hdmi_5_volt_time_r(
+		unsigned char *denc_off_time,
+		unsigned char *hdmi_off_time,
+		unsigned char *on_time);
+int av8100_reg_stby_int_mask_r(
+		unsigned char *hpdm,
+		unsigned char *cpdm,
+		unsigned char *stbygpiocfg,
+		unsigned char *ipol);
+int av8100_reg_stby_pend_int_r(
+		unsigned char *hpdi,
+		unsigned char *cpdi,
+		unsigned char *oni,
+		unsigned char *sid);
+int av8100_reg_gen_int_mask_r(
+		unsigned char *eocm,
+		unsigned char *vsim,
+		unsigned char *vsom,
+		unsigned char *cecm,
+		unsigned char *hdcpm,
+		unsigned char *uovbm,
+		unsigned char *tem);
+int av8100_reg_gen_int_r(
+		unsigned char *eoci,
+		unsigned char *vsii,
+		unsigned char *vsoi,
+		unsigned char *ceci,
+		unsigned char *hdcpi,
+		unsigned char *uovbi,
+		unsigned char *tei);
+int av8100_reg_gen_status_r(
+		unsigned char *cectxerr,
+		unsigned char *cecrec,
+		unsigned char *cectrx,
+		unsigned char *uc,
+		unsigned char *onuvb,
+		unsigned char *hdcps);
+int av8100_reg_gpio_conf_r(
+		unsigned char *dat3dir,
+		unsigned char *dat3val,
+		unsigned char *dat2dir,
+		unsigned char *dat2val,
+		unsigned char *dat1dir,
+		unsigned char *dat1val,
+		unsigned char *ucdbg);
+int av8100_reg_gen_ctrl_r(
+		unsigned char *fdl,
+		unsigned char *hld,
+		unsigned char *wa,
+		unsigned char *ra);
+int av8100_reg_fw_dl_entry_r(
+	unsigned char *mbyte_code_entry);
+int av8100_reg_r(
+		unsigned char offset,
+		unsigned char *value);
+int av8100_conf_get(enum av8100_command_type command_type,
+	union av8100_configuration *config);
+int av8100_conf_prep(enum av8100_command_type command_type,
+	union av8100_configuration *config);
+int av8100_conf_w(enum av8100_command_type command_type,
+		unsigned char *return_buffer_length,
+		unsigned char *return_buffer, enum interface_type if_type);
+int av8100_conf_w_raw(enum av8100_command_type command_type,
+	unsigned char buffer_length,
+	unsigned char *buffer,
+	unsigned char *return_buffer_length,
+	unsigned char *return_buffer);
+struct av8100_status av8100_status_get(void);
+enum av8100_output_CEA_VESA av8100_video_output_format_get(int xres,
+	int yres,
+	int htot,
+	int vtot,
+	int pixelclk,
+	bool interlaced);
+void av8100_hdmi_event_cb_set(void (*event_callback)(enum av8100_hdmi_event));
+u8 av8100_ver_get(void);
+
+#endif /* __AV8100__H__ */
diff --git a/include/video/b2r2_blt.h b/include/video/b2r2_blt.h
new file mode 100644
index 00000000000..1cf5f661598
--- /dev/null
+++ b/include/video/b2r2_blt.h
@@ -0,0 +1,611 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 user interface
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#ifndef _LINUX_VIDEO_B2R2_BLT_H
+#define _LINUX_VIDEO_B2R2_BLT_H
+
+#include <linux/types.h>
+
+#if defined(__KERNEL__) || defined(_KERNEL)
+#include <linux/mm_types.h>
+#include <linux/bitops.h>
+#else
+#define BIT(nr)			(1UL << (nr))
+#endif
+
+/**
+ * struct b2r2_blt_rect - Specifies a B2R2 rectangle
+ *
+ * @left: X-coordinate of top left corner
+ * @top: Y-coordinate of top left corner
+ * @width: Rectangle width. Must be >= 0.
+ * @height: Rectangle height. Must be >= 0.
+ */
+struct b2r2_blt_rect {
+	__s32  x;
+	__s32  y;
+	__s32  width;
+	__s32  height;
+};
+
+/**
+ * enum b2r2_blt_fmt - Defines the available B2R2 buffer formats
+ *
+ *        Inspired by Khronos OpenMAX, please see
+ *        OpenMAX IL specification for detailed descriptions of the formats
+ *
+ * @B2R2_BLT_FMT_UNUSED: Placeholder value when format is unknown,
+ * or specified using a vendor-specific means.
+ * @B2R2_BLT_FMT_16_BIT_ARGB4444: 16 bits per pixel ARGB format with colors
+ * stored as Alpha 15:12, Red 11:8, Green 7:4, and Blue 3:0.
+ * @B2R2_BLT_FMT_16_BIT_ARGB1555: 16 bits per pixel ARGB format with colors
+ * stored as Alpha 15, Red 14:10, Green 9:5, and Blue 4:0.
+ * @B2R2_BLT_FMT_16_BIT_RGB565: 16 bits per pixel RGB format with colors
+ * stored as Red 15:11, Green 10:5, and Blue 4:0.
+ * @B2R2_BLT_FMT_24_BIT_RGB888: 24 bits per pixel RGB format with colors
+ * stored as Red 23:16, Green 15:8, and Blue 7:0.
+ * @B2R2_BLT_FMT_32_BIT_ARGB8888: 32 bits per pixel ARGB format with colors
+ * stored as Alpha 31:24, Red 23:16, Green 15:8, and Blue 7:0.
+ * @B2R2_BLT_FMT_YUV420_PACKED_PLANAR: YUV planar format, organized with
+ * three separate planes for each color component, namely Y, U, and V.
+ * U and V pixels are sub-sampled by a factor of two both horizontally and
+ * vertically. The buffer shall contain a plane of Y, U, and V data in this
+ * order
+ * @B2R2_BLT_FMT_YUV422_PACKED_PLANAR: YUV planar format, organized with
+ * three separate planes for each color component, namely Y, U, and V.
+ * U and V pixels are subsampled by a factor of two horizontally.
+ * The buffer shall contain a plane of Y, U, and V data in this order.
+ * @B2R2_BLT_FMT_Y_CB_Y_CR: 16 bits per pixel YUV interleaved format organized
+ * as YUYV (i.e., YCbYCr).
+ * (Corresponds to YUV422 interleaved)
+ * @B2R2_BLT_FMT_CB_Y_CR_Y: 16 bits per pixel YUV interleaved format organized
+ * as UYVY (i.e., CbYCrY).
+ * (Corresponds to YUV422R)
+ * @B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR: YUV planar format, organized with
+ * a first plane containing Y pixels, and a second plane containing U and V
+ * pixels interleaved with the first U value first. U and V pixels are
+ * sub-sampled by a factor of two both horizontally and vertically. The buffer
+ * shall contain a plane of Y, U and V data.
+ * (Same as B2R2 420 Raster 2 buffer - 420 R2B)
+ * @B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR: YUV planar format, organized with
+ * a first plane containing Y pixels, and a second plane containing U and V
+ * pixels interleaved with the first U value first. U and V pixels are
+ * sub-sampled by a factor of two horizontally. The buffer shall contain a
+ * plane of Y, U and V data.
+ * (Same as B2R2 422 Raster 2 buffer - 422 R2B)
+ * @B2R2_BLT_FMT_32_BIT_ABGR8888: 32 bits per pixel ABGR format with colors
+ * stored as Alpha 31:24,Blue 23:16, Green 15:8, and Red 7:0.
+ * @B2R2_BLT_FMT_24_BIT_ARGB8565: 24 bits per pixel ARGB format with colors
+ * stored as Alpha 23:16, Red 15:11, Green 10:5, and Blue 4:0.
+ * @B2R2_BLT_FMT_24_BIT_YUV888: 24 bits per pixel YUV format with colors
+ * stored as Y 23:16, U 15:8, and V 7:0.
+ * @B2R2_BLT_FMT_32_BIT_AYUV8888: 32 bits per pixel AYUV format with colors
+ * stored as Alpha 31:24, Y 23:16, U 15:8, and V 7:0.
+ * @B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE: Nomadik YUV 420 macro block
+ * format, see B2R2 spec for details
+ * @B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE: Nomadik YUV 422 macro block
+ * format, see B2R2 spec for details
+ * @B2R2_BLT_FMT_1_BIT_A1: 1 bit per pixel A format, 1 bit alpha
+ * @B2R2_BLT_FMT_8_BIT_A8: 8 bit per pixel A format, 8 bit alpha
+ * @B2R2_BLT_FMT_YUV444_PACKED_PLANAR: YUV planar format, organized with
+ * three separate planes, one for each color component, namely Y, U, and V.
+ * All planes use full resolution, there is no subsampling.
+ * The buffer shall contain a plane of Y, U, and V data in this order.
+ * @B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR: YVU planar format, organized with
+ * a first plane containing Y pixels, and a second plane containing V and U
+ * pixels interleaved with the first V value first. V and U pixels are
+ * sub-sampled by a factor of two both horizontally and vertically. The buffer
+ * shall contain two planes, one plane with Y, and one with V and U data.
+ * (Same as B2R2 420 Raster 2 buffer - 420 R2B except that chroma order is
+ * swapped.)
+ * @B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR: YVU planar format, organized with
+ * a first plane containing Y pixels, and a second plane containing V and U
+ * pixels interleaved with the first V value first. V and U pixels are
+ * sub-sampled by a factor of two horizontally. The buffer shall contain a
+ * two planes, one with Y, and one with V and U data.
+ * (Same as B2R2 422 Raster 2 buffer - 422 R2B except that chroma order is
+ * swapped.)
+ * @B2R2_BLT_FMT_YVU420_PACKED_PLANAR: YVU planar format, organized with
+ * three separate planes for each color component, namely Y, V, and U.
+ * V and U pixels are sub-sampled by a factor of two both horizontally and
+ * vertically. The buffer shall contain a plane of Y, V, and U data in this
+ * order. (Same as B2R2_BLT_FMT_YUV420_PACKED_PLANAR except that chroma
+ * order is swapped.)
+ * @B2R2_BLT_FMT_YVU422_PACKED_PLANAR: YVU planar format, organized with
+ * three separate planes for each color component, namely Y, V, and U.
+ * V and U pixels are subsampled by a factor of two horizontally.
+ * The buffer shall contain a plane of Y, V, and U data in this order.
+ * (Same as B2R2_BLT_FMT_YUV422_PACKED_PLANAR except that chroma
+ * order is swapped.)
+ */
+enum b2r2_blt_fmt {
+    B2R2_BLT_FMT_UNUSED = 0,
+    B2R2_BLT_FMT_16_BIT_ARGB4444 = 4,
+    B2R2_BLT_FMT_16_BIT_ARGB1555 = 5,
+    B2R2_BLT_FMT_16_BIT_RGB565 = 6,
+    B2R2_BLT_FMT_24_BIT_RGB888 = 11,
+    B2R2_BLT_FMT_32_BIT_ARGB8888 = 16,
+    B2R2_BLT_FMT_YUV420_PACKED_PLANAR = 20,
+    B2R2_BLT_FMT_YUV422_PACKED_PLANAR = 23,
+    B2R2_BLT_FMT_Y_CB_Y_CR = 25,
+    B2R2_BLT_FMT_CB_Y_CR_Y = 27,
+    B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR = 39,
+    B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR = 40,
+    /* Extensions, non OpenMAX formats */
+    B2R2_BLT_FMT_32_BIT_ABGR8888 = 0x7F000000, /* OpenMax vendor start */
+    B2R2_BLT_FMT_24_BIT_ARGB8565 = 0x7F000001,
+    B2R2_BLT_FMT_24_BIT_YUV888 = 0x7F000002,
+    B2R2_BLT_FMT_32_BIT_AYUV8888 = 0x7F000003,
+    B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE = 0x7F000004,
+    B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE = 0x7F000005,
+    B2R2_BLT_FMT_1_BIT_A1 = 0x7F000006,
+    B2R2_BLT_FMT_8_BIT_A8 = 0x7F000007,
+    B2R2_BLT_FMT_YUV444_PACKED_PLANAR = 0x7F000008,
+    B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR = 0x7F000009,
+    B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR = 0x7F00000A,
+    B2R2_BLT_FMT_YVU420_PACKED_PLANAR = 0x7F00000B,
+    B2R2_BLT_FMT_YVU422_PACKED_PLANAR = 0x7F00000C,
+};
+
+/**
+ * enum b2r2_blt_ptr_type - Specifies a B2R2 buffer pointer type
+ *
+ * @B2R2_BLT_PTR_NONE:
+ *    No pointer (NULL). E.g. src fill.
+ * @B2R2_BLT_PTR_VIRTUAL:
+ *    Use offset as a userspace virtual address
+ * @B2R2_BLT_PTR_PHYSICAL:
+ *    Use offset as a physical address
+ * @B2R2_BLT_PTR_FD_OFFSET:
+ *    Use fd + offset to determine buffer location.
+ * @B2R2_BLT_PTR_HWMEM_BUF_NAME:
+ *    Use hwmem_buf_name and offset to determine buffer location.
+ */
+enum b2r2_blt_ptr_type {
+	B2R2_BLT_PTR_NONE,
+	B2R2_BLT_PTR_VIRTUAL,
+	B2R2_BLT_PTR_PHYSICAL,
+	B2R2_BLT_PTR_FD_OFFSET,
+	B2R2_BLT_PTR_HWMEM_BUF_NAME_OFFSET,
+};
+
+/**
+ * struct b2r2_blt_buf - Specifies a B2R2 buffer pointer
+ *
+ * @type: Buffer pointer type
+ * @hwmem_global_buf_id: Hwmem buffer name
+ * @fd: File descriptor (e.g. file handle to pmem or fb device)
+ * @offset: Offset where buffer can be found or address.
+ * @len: Size of buffer in bytes
+ * @bits: Pointer to the bitmap data. This field can be used to specify
+ * an alternative way to access the buffer. Whenever the 'bits' pointer
+ * is set to non-NULL, the underlying implementation is free to decide
+ * whether or not to use it in favor of other ways to locate the buffer.
+ */
+struct b2r2_blt_buf {
+	enum b2r2_blt_ptr_type type;
+	__s32                  hwmem_buf_name;
+	__s32                  fd;
+	__u32                  offset;
+	__u32                  len;
+	void                   *bits;
+};
+
+
+/**
+ * struct b2r2_blt_img - Specifies a B2R2 image
+ *
+ * @fmt: Pixel format of image
+ * @buf: Pixel buffer
+ * @width: Width in pixels
+ * @height: Height in pixels
+ * @pitch: Pitch in bytes (from start of one line to start of next)
+ */
+struct b2r2_blt_img {
+	enum b2r2_blt_fmt   fmt;
+	struct b2r2_blt_buf buf;
+	__s32               width;
+	__s32               height;
+	__u32               pitch;
+};
+
+
+/**
+ * enum b2r2_blt_transform- Specifies rotation and flipping, mutually exclusive
+ * @B2R2_BLT_TRANSFORM_NONE:
+ *    No rotation or flip
+ * @B2R2_BLT_TRANSFORM_FLIP_H
+ *    Flip horizontally
+ * @B2R2_BLT_TRANSFORM_FLIP_V
+ *    Flip vertically
+ * @B2R2_BLT_TRANSFORM_CCW_ROT_90
+ *    Rotate 90 degrees counter clockwise
+ * @B2R2_BLT_TRANSFORM_CCW_ROT_180
+ *    Rotate 180 degrees (same as flip horizontally together with
+ *    flip vertically)
+ * @B2R2_BLT_TRANSFORM_CCW_ROT_270
+ *    Rotate 270 degrees counter clockwise
+ * @B2R2_BLT_TRANSFORM_FLIP_H_CCW_ROT_90
+ *    Flip horizontally and then rotate 90 degrees counter clockwise
+ * @B2R2_BLT_TRANSFORM_FLIP_V_CCW_ROT_90
+ *    Flip vertically and then rotate 90 degrees counter clockwise
+ */
+enum b2r2_blt_transform {
+	B2R2_BLT_TRANSFORM_NONE              = 0,
+	B2R2_BLT_TRANSFORM_FLIP_H            = 1,
+	B2R2_BLT_TRANSFORM_FLIP_V            = 2,
+	B2R2_BLT_TRANSFORM_CCW_ROT_90        = 4,
+	B2R2_BLT_TRANSFORM_CCW_ROT_180       = 3,
+	B2R2_BLT_TRANSFORM_CCW_ROT_270       = 7,
+	B2R2_BLT_TRANSFORM_FLIP_H_CCW_ROT_90 = 5,
+	B2R2_BLT_TRANSFORM_FLIP_V_CCW_ROT_90 = 6,
+};
+
+
+/**
+ * enum b2r2_blt_flag - Flags that controls the B2R2 request
+ *
+ * Can be combined.
+ *
+ * @B2R2_BLT_FLAG_ASYNCH:
+ *    Asynchronous request. b2r2_blt will returns when the request
+ *    has been queued.
+ * @B2R2_BLT_FLAG_DRY_RUN:
+ *    Dry run, just to check if request can be performed.
+ * @B2R2_BLT_FLAG_PER_PIXEL_ALPHA_BLEND:
+ *    Enable per pixel alpha blend
+ * @B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND:
+ *    Enable global alpha blend (alpha value in global_alpha)
+ * @B2R2_BLT_FLAG_SOURCE_COLOR_KEY:
+ *    Enable source color key (color in src_color). Color should be in raw
+ *    format.
+ *    B2R2_BLT_FLAG_SOURCE_COLOR_KEY, B2R2_BLT_FLAG_SOURCE_FILL and
+ *    B2R2_BLT_FLAG_SOURCE_FILL_RAW cannot be specified at the same time.
+ *    B2R2_BLT_FLAG_SOURCE_COLOR_KEY and B2R2_BLT_FLAG_DEST_COLOR_KEY cannot be
+ *    specified at the same time.
+ * @B2R2_BLT_FLAG_SOURCE_FILL:
+ *    Enable ARGB/AYUV source fill (color in src_color). Which of ARGB and AYUV
+ *    is determined by the destination format.
+ *    B2R2_BLT_FLAG_SOURCE_COLOR_KEY, B2R2_BLT_FLAG_SOURCE_FILL and
+ *    B2R2_BLT_FLAG_SOURCE_FILL_RAW cannot be specified at the same time
+ * @B2R2_BLT_FLAG_SOURCE_FILL_RAW:
+ *    Enable raw color source fill (color in src_color)
+ *    B2R2_BLT_FLAG_SOURCE_COLOR_KEY, B2R2_BLT_FLAG_SOURCE_FILL and
+ *    B2R2_BLT_FLAG_SOURCE_FILL_RAW cannot be specified at the same time
+ * @B2R2_BLT_FLAG_DEST_COLOR_KEY:
+ *    Enable dest color key (color in dst_color). Color in raw format.
+ * @B2R2_BLT_FLAG_SRC_IS_NOT_PREMULT:
+ *    Source color not premultiplied (Valid for alpha formats only).
+ * @B2R2_BLT_FLAG_DITHER:
+ *    Enable dithering
+ * @B2R2_BLT_FLAG_BLUR:
+ *    Enable blur
+ * @B2R2_BLT_FLAG_SOURCE_MASK:
+ *    Enable source mask
+ * @B2R2_BLT_FLAG_DESTINATION_CLIP:
+ *    Enable destination clip rectangle
+ * @B2R2_BLT_FLAG_INHERIT_PRIO
+ *    Inherit process priority
+ * @B2R2_BLT_FLAG_REPORT_WHEN_DONE
+ *    Report through b2r2_blt file when done. A b2r2_blt_report structure is
+ *    read. Use poll() or select() if anything to read. (i.e. to help user space
+ *    to implement callback functionality)
+ * @B2R2_BLT_FLAG_REPORT_PERFORMANCE
+ *    Include performance data in the report structure
+ * @B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION
+ *    Use color look-up table for color correction.
+ *    Pointer to the table must be specified in *clut field of
+ *    the b2r2_blt_req structure.
+ *    The table must map all input color values
+ *    for each channel to the desired output values.
+ *    It is an array with the following format:
+ *    R0 G0 B0 A0 R1 G1 B1 A1...R255 G255 B255 A255
+ *    where R0 is the 8 bit output value for red channel whenever its input
+ *    equals 0.
+ *    Similarly, R1 through R255 are the red channel outputs whenever
+ *    the channel's inputs equal 1 through 255 respectively.
+ *    Gn, Bn, An denote green, blue and alpha channel.
+ *    Whenever the input bitmap format lacks the alpha channel,
+ *    all alpha values in the color correction table should be set to 255.
+ *    Size of the array that specifies the color correction table
+ *    must be 1024 bytes.
+ *    A table that does not change anything has the form:
+ *    0 0 0 0 1 1 1 1 2 2 2 2 ... 254 254 254 254 255 255 255 255.
+ *    CLUT color correction can be applied to YUV raster buffers as well,
+ *    in which case the RGB color channels are mapped onto YUV-space
+ *    as follows:
+ *    R = red chrominance
+ *    G = luminance
+ *    B = blue chrominance
+ *    A = alpha
+ *    If any of the planar or semi-planar formats is used, luminance cannot
+ *    be changed by the color correction table.
+ */
+enum b2r2_blt_flag {
+	B2R2_BLT_FLAG_ASYNCH                = BIT(0),/*0x1*/
+	B2R2_BLT_FLAG_DRY_RUN               = BIT(1),/*0x2*/
+	B2R2_BLT_FLAG_PER_PIXEL_ALPHA_BLEND = BIT(2),/*0x4*/
+	B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND    = BIT(3),/*0x8*/
+	B2R2_BLT_FLAG_SOURCE_COLOR_KEY      = BIT(4),/*0x10*/
+	B2R2_BLT_FLAG_SOURCE_FILL           = BIT(5),/*0x20*/
+	B2R2_BLT_FLAG_SOURCE_FILL_RAW       = BIT(6),/*0x40*/
+	B2R2_BLT_FLAG_DEST_COLOR_KEY        = BIT(7),/*0x80*/
+	B2R2_BLT_FLAG_SRC_IS_NOT_PREMULT    = BIT(8),/*0x100*/
+	B2R2_BLT_FLAG_DITHER                = BIT(9),/*0x200*/
+	B2R2_BLT_FLAG_BLUR                  = BIT(10),/*0x400*/
+	B2R2_BLT_FLAG_SOURCE_MASK           = BIT(11),/*0x800*/
+	B2R2_BLT_FLAG_DESTINATION_CLIP      = BIT(12),/*0x1000*/
+	B2R2_BLT_FLAG_INHERIT_PRIO          = BIT(13),/*0x2000*/
+	B2R2_BLT_FLAG_SRC_NO_CACHE_FLUSH    = BIT(14),/*0x4000*/
+	B2R2_BLT_FLAG_SRC_MASK_NO_CACHE_FLUSH = BIT(15),/*0x8000*/
+	B2R2_BLT_FLAG_DST_NO_CACHE_FLUSH    = BIT(16),/*0x10000*/
+	B2R2_BLT_FLAG_REPORT_WHEN_DONE      = BIT(29),/*0x20000000*/
+	B2R2_BLT_FLAG_REPORT_PERFORMANCE    = BIT(30),/*0x40000000*/
+	B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION = BIT(31),/*0x80000000*/
+};
+
+
+/**
+ * struct b2r2_blt_req - Specifies a request to B2R2
+ *
+ * @size: Size of this structure. Used for versioning. MUST be specified.
+ * @flags: Flags that control the B2R2 request ORed together
+ * @tfm: How source should be flipped and rotated when blitting
+ * @prio: Priority (-20 to 19). Inherits process prio
+ *        if B2R2_BLT_FLAG_INHERIT_PRIO. Given priority is mapped onto B2R2.
+ *        TBD: How?
+ * @clut: Pointer to the look-up table for color correction.
+ * @src_img: Source image. Not used if source fill.
+ * @src_mask: Source mask. Not used if source fill.
+ * @src_rect: Source area to be blitted.
+ * @src_color: Source fill color or color key
+ * @dst_img: Destination image.
+ * @dst_rect: Destination area to be blitted to.
+ * @dst_color: Destination color key
+ * @dst_clip_rect: Destination clip rectangle.
+ * @global_alpha: Global alpha value (0 - 255)
+ * @report1: Data 1 to report back when request is done.
+ *           See struct b2r2_blt_report.
+ * @report2: Data 2 to report back when request is done.
+ *           See struct b2r2_blt_report.
+ *
+ */
+struct b2r2_blt_req {
+	__u32                     size;
+	enum   b2r2_blt_flag      flags;
+	enum   b2r2_blt_transform transform;
+	__s32                     prio;
+	void                      *clut;
+	struct b2r2_blt_img       src_img;
+	struct b2r2_blt_img       src_mask;
+	struct b2r2_blt_rect      src_rect;
+	__u32                     src_color;
+	struct b2r2_blt_img       dst_img;
+	struct b2r2_blt_rect      dst_rect;
+	struct b2r2_blt_rect      dst_clip_rect;
+	__u32                     dst_color;
+	__u8                      global_alpha;
+	__u32                     report1;
+	__u32                     report2;
+};
+
+/**
+ * enum b2r2_blt_cap -  Capabilities that can be queried for.
+ *
+ * Capabilities can be queried for a specific format or for formats in
+ * general. To query for capabilities in general, specify BLT_FMT_UNUSED
+ * as format.
+ *
+ * B2R2_BLT_CAP_UNUSED: Unused/unspecified capability
+ * B2R2_BLT_CAP_FMT_SOURCE: Is format supported as source?
+ * B2R2_BLT_CAP_FMT_SOURCE_MASK: Is format supported as source mask?
+ * B2R2_BLT_CAP_FMT_DEST: Is format supported as dest?
+ * B2R2_BLT_CAP_PER_PIXEL_ALPHA_BLEND: Is per pixel alpha blending supported
+ * with format as source
+ * B2R2_BLT_CAP_GLOBAL_ALPHA_BLEND: Is per global alpha blending supported
+ * with format as source
+ * B2R2_BLT_CAP_SOURCE_COLOR_KEY: Is source color key supported with format as
+ * source
+ * B2R2_BLT_CAP_SOURCE_FILL: Is source fill supported with format as source
+ * B2R2_BLT_CAP_SOURCE_FILL_RAW: Is source fill raw supported with format as
+ * dest
+ * B2R2_BLT_CAP_DEST_COLOR_KEY: Is dest color key supported with format as dest
+ * B2R2_BLT_CAP_DITHER: Is dithering supported with format as source
+ * B2R2_BLT_CAP_BLUR: Is blur supported with format as source
+ * B2R2_BLT_CAP_MINIFICATION_LIMIT: Minification limit (copybit support)
+ * B2R2_BLT_CAP_MAGNIFICATION_LIMIT: Magnification limit (copybit support)
+ * B2R2_BLT_CAP_SCALING_FRAC_BITS: Number of scaling fractional bits (copybit
+ * support)
+ * B2R2_BLT_CAP_ROTATION_STEP_DEG: Supported rotation step in degrees (copybit
+ * support)
+ */
+
+enum b2r2_blt_cap {
+    B2R2_BLT_CAP_UNUSED = 0,
+    /**
+     * @brief Is format supported as source.
+     */
+    B2R2_BLT_CAP_FMT_SOURCE,
+    /**
+     * @brief Is format supported as source mask
+     */
+    B2R2_BLT_CAP_FMT_SOURCE_MASK,
+    /**
+     * @brief Is format supported as destination
+     */
+    B2R2_BLT_CAP_FMT_DEST,
+    /**
+     * @brief Is per pixel alpha blending supported with format as source
+     */
+    B2R2_BLT_CAP_PER_PIXEL_ALPHA_BLEND,
+    /**
+     * @brief Is global alpha blending supported with format as source
+     */
+    B2R2_BLT_CAP_GLOBAL_ALPHA_BLEND,
+    /**
+     * @brief Is source color key supported with format as source
+     */
+    B2R2_BLT_CAP_SOURCE_COLOR_KEY,
+    /**
+     * @brief Is source fill supported with format as source
+     */
+    B2R2_BLT_CAP_SOURCE_FILL,
+    /**
+     * @brief Is source fill raw supported with format as dest
+     */
+    B2R2_BLT_CAP_SOURCE_FILL_RAW,
+    /**
+     * @brief Is dest color key supported with format as dest
+     */
+    B2R2_BLT_CAP_DEST_COLOR_KEY,
+    /**
+     * @brief Is dithering supported with format as source
+     */
+    B2R2_BLT_CAP_DITHER,
+    /**
+     * @brief Is blur supported with format as source
+     */
+    B2R2_BLT_CAP_BLUR,
+    /**
+     * @brief Minification limit (copybit support)
+     */
+    B2R2_BLT_CAP_MINIFICATION_LIMIT,
+    /**
+     * @brief Magnification limit (copybit support)
+     */
+    B2R2_BLT_CAP_MAGNIFICATION_LIMIT,
+    /**
+     * @brief Number of scaling fractional bits (copybit support)
+     */
+    B2R2_BLT_CAP_SCALING_FRAC_BITS,
+    /**
+     * @brief Supported rotation step in degrees (copybit support)
+     */
+    B2R2_BLT_CAP_ROTATION_STEP_DEG,
+};
+
+/**
+ * struct b2r2_blt_query_cap - Query B2R2 capabilities
+ *
+ * fmt: Format to query capabilities for or BLT_FMT_UNUSED for all
+ * cap: Capability to query for
+ * result: Returned capability. Interpretaion of this variable varies
+ *         with the capability queried
+ */
+struct b2r2_blt_query_cap {
+	enum b2r2_blt_fmt fmt;
+	enum b2r2_blt_cap cap;
+	__u32             result;
+};
+
+/**
+ * struct b2r2_blt_report - Report from B2R2 driver back to user space
+ *
+ * This structure can be read from B2R2 driver if B2R2_BLT_FLAG_REPORT_WHEN_DONE
+ * flag was specified when the request was issued.
+ *
+ * @request_id: The id for the request, same as reported from blt_request
+ * @report1: Client data specified in struct blt_request
+ * @report2: Client data specified in struct blt_request
+ * @usec_elapsed: Number of microseconds needed to perform this blit
+ *                if B2R2_BLT_FLAG_REPORT_PERFORMANCE was specified when the
+ *                request was issued.
+ *
+ */
+struct b2r2_blt_report {
+	__u32 request_id;
+	__u32 report1;
+	__u32 report2;
+	__u32 usec_elapsed;
+};
+
+/**
+ * B2R2 BLT driver is used in the following way:
+ *
+ * Obtain a file descriptor to the driver:
+ *        fd = open("/dev/b2r2_blt", O_RDWR);
+ *
+ * Issue requests:
+ *        struct b2r2_blt_request blt_request;
+ *        blt_request.size = sizeof(blt_request);
+ *        ... Fill request with data...
+ *
+ *        request_id = ioctl(fd, B2R2_BLT_IOC, (__u32) &blt_request);
+ *
+ * Wait for a request to finish
+ *        ret = ioctl(fd, B2R2_BLT_SYNCH_IOC, (__u32) request_id);
+ *
+ * Wait for all requests from this context to finish
+ *        ret = ioctl(fd, B2R2_BLT_SYNCH_IOC, (__u32) 0);
+ *
+ * Wait indefinitely for report data from driver:
+ *        pollfd.fd = fd
+ *        pollfd.events = 0xFFFFFFFF;
+ *        pollfd.revents = 0;
+ *        ret = poll(&pollfd, 1, -1);
+ *
+ * Read report data from driver
+ *        struct b2r2_blt_report blt_report;
+ *
+ *        nread = read(fd, &blt_report, sizeof(blt_report));
+ *
+ * Close the driver
+ *        close(fd);
+ */
+
+/* B2R2 BLT IOCTLS */
+
+/**
+ * B2R2_BLT_IOC_MAGIC is ioctl type group for B2R2 driver
+ */
+#define B2R2_BLT_IOC_MAGIC 0xb2
+
+/**
+ * The B2R2_BLT_IOC ioctl adds a blit request to B2R2.
+ *
+ * The ioctl returns when the blit has been performed if not
+ * asynchronous execution has been specified. If asynchronous,
+ * control is returned as soon as the request has been queued.
+ *
+ * Supplied parameter shall be a pointer to a struct b2r2_blt_req.
+ *
+ * Returns an unique request id if >= 0, else a negative error code.
+ * This request id can be waited for using B2R2_BLT_SYNC_IOC.
+ * Return values: -ESOMERROR Description of an error
+ */
+#define B2R2_BLT_IOC        _IOW(B2R2_BLT_IOC_MAGIC, 1, struct b2r2_blt_req)
+
+/**
+ * The B2R2_BLT_SYNC_IOC waits for all or a specified request to be finished.
+ *
+ * Supplied parameter shall be a request id previously returned by
+ * B2R2_BLT_IOC or 0 for all requests.
+ *
+ * Returns 0 if OK, else a negative error code
+ * Return value: -ESOMERROR Description of an error
+ */
+#define B2R2_BLT_SYNCH_IOC  _IOW(B2R2_BLT_IOC_MAGIC, 2, int)
+
+/**
+ * The BLT_QUERY_CAP_IOC returns capability information for all or
+ *                       for a certain format
+ *
+ * Supplied parameter shall be a pointer to a struct b2r2_blt_query_cap.
+ *
+ * @return Returns 0 if OK, else a negative error code
+ * @retval -ESOMERROR Description of an error
+ */
+#define B2R2_BLT_QUERY_CAP_IOC  _IOWR(B2R2_BLT_IOC_MAGIC, 3, \
+				  struct b2r2_blt_query_cap)
+
+#endif /* #ifdef _LINUX_VIDEO_B2R2_BLT_H */
diff --git a/include/video/hdmi.h b/include/video/hdmi.h
new file mode 100644
index 00000000000..d2ef87f910a
--- /dev/null
+++ b/include/video/hdmi.h
@@ -0,0 +1,178 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * HDMI driver
+ *
+ * Author: Per Persson <per.xb.persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __HDMI__H__
+#define __HDMI__H__
+
+#define HDMI_RESULT_OK			0
+#define HDMI_RESULT_NOT_OK		1
+#define HDMI_AES_NOT_FUSED		2
+#define HDMI_RESULT_CRC_MISMATCH	3
+
+#define HDMI_CEC_READ_MAXSIZE		16
+#define HDMI_CEC_WRITE_MAXSIZE		15
+#define HDMI_INFOFRAME_MAX_SIZE		27
+#define HDMI_HDCP_FUSEAES_KEYSIZE	16
+#define HDMI_HDCP_AES_BLOCK_START	128
+#define HDMI_HDCP_KSV_BLOCK		40
+#define HDMI_HDCP_AES_NR_OF_BLOCKS	18
+#define HDMI_HDCP_AES_KEYSIZE		16
+#define HDMI_HDCP_AES_KSVSIZE		5
+#define HDMI_HDCP_AES_KSVZEROESSIZE	3
+#define HDMI_EDID_DATA_SIZE		128
+#define HDMI_CEC_SIZE			15
+#define HDMI_INFOFR_SIZE		27
+#define HDMI_FUSE_KEYSIZE		16
+#define HDMI_AES_KSVSIZE		5
+#define HDMI_AES_KEYSIZE		288
+#define HDMI_CRC32_SIZE			4
+#define HDMI_REVOC_LIST_SIZE		30
+
+#define HDMI_STOREASTEXT_TEXT_SIZE	2
+#define HDMI_STOREASTEXT_BIN_SIZE	1
+#define HDMI_PLUGDETEN_TEXT_SIZE	6
+#define HDMI_PLUGDETEN_BIN_SIZE		3
+#define HDMI_EDIDREAD_TEXT_SIZE		4
+#define HDMI_EDIDREAD_BIN_SIZE		2
+#define HDMI_CECEVEN_TEXT_SIZE		2
+#define HDMI_CECEVEN_BIN_SIZE		1
+#define HDMI_CECSEND_TEXT_SIZE_MAX	37
+#define HDMI_CECSEND_TEXT_SIZE_MIN	6
+#define HDMI_CECSEND_BIN_SIZE_MAX	18
+#define HDMI_CECSEND_BIN_SIZE_MIN	3
+#define HDMI_INFOFRSEND_TEXT_SIZE_MIN	8
+#define HDMI_INFOFRSEND_TEXT_SIZE_MAX	63
+#define HDMI_INFOFRSEND_BIN_SIZE_MIN	4
+#define HDMI_INFOFRSEND_BIN_SIZE_MAX	31
+#define HDMI_HDCPEVEN_TEXT_SIZE		2
+#define HDMI_HDCPEVEN_BIN_SIZE		1
+#define HDMI_HDCP_FUSEAES_TEXT_SIZE	34
+#define HDMI_HDCP_FUSEAES_BIN_SIZE	17
+#define HDMI_HDCP_LOADAES_TEXT_SIZE	594
+#define HDMI_HDCP_LOADAES_BIN_SIZE	297
+#define HDMI_HDCPAUTHENCR_TEXT_SIZE	4
+#define HDMI_HDCPAUTHENCR_BIN_SIZE	2
+#define HDMI_EVCLR_TEXT_SIZE		2
+#define HDMI_EVCLR_BIN_SIZE		1
+#define HDMI_AUDIOCFG_TEXT_SIZE		14
+#define HDMI_AUDIOCFG_BIN_SIZE		7
+#define HDMI_POWERONOFF_TEXT_SIZE	2
+#define HDMI_POWERONOFF_BIN_SIZE	1
+
+#define HDMI_IOC_MAGIC 0xcc
+
+/** IOCTL Operations */
+#define IOC_PLUG_DETECT_ENABLE		_IOWR(HDMI_IOC_MAGIC, 1, int)
+#define IOC_EDID_READ			_IOWR(HDMI_IOC_MAGIC, 2, int)
+#define IOC_CEC_EVENT_ENABLE		_IOWR(HDMI_IOC_MAGIC, 3, int)
+#define IOC_CEC_READ			_IOWR(HDMI_IOC_MAGIC, 4, int)
+#define IOC_CEC_SEND			_IOWR(HDMI_IOC_MAGIC, 5, int)
+#define IOC_INFOFRAME_SEND		_IOWR(HDMI_IOC_MAGIC, 6, int)
+#define IOC_HDCP_EVENT_ENABLE		_IOWR(HDMI_IOC_MAGIC, 7, int)
+#define IOC_HDCP_CHKAESOTP		_IOWR(HDMI_IOC_MAGIC, 8, int)
+#define IOC_HDCP_FUSEAES		_IOWR(HDMI_IOC_MAGIC, 9, int)
+#define IOC_HDCP_LOADAES		_IOWR(HDMI_IOC_MAGIC, 10, int)
+#define IOC_HDCP_AUTHENCR_REQ		_IOWR(HDMI_IOC_MAGIC, 11, int)
+#define IOC_HDCP_STATE_GET		_IOWR(HDMI_IOC_MAGIC, 12, int)
+#define IOC_EVENTS_READ			_IOWR(HDMI_IOC_MAGIC, 13, int)
+#define IOC_EVENTS_CLEAR		_IOWR(HDMI_IOC_MAGIC, 14, int)
+#define IOC_AUDIO_CFG			_IOWR(HDMI_IOC_MAGIC, 15, int)
+#define IOC_PLUG_STATUS			_IOWR(HDMI_IOC_MAGIC, 16, int)
+#define IOC_POWERONOFF			_IOWR(HDMI_IOC_MAGIC, 17, int)
+#define IOC_EVENT_WAKEUP		_IOWR(HDMI_IOC_MAGIC, 18, int)
+#define IOC_POWERSTATE			_IOWR(HDMI_IOC_MAGIC, 19, int)
+
+
+/* HDMI driver */
+void hdmi_event(enum av8100_hdmi_event);
+int hdmi_init(void);
+void hdmi_exit(void);
+
+enum hdmi_event {
+	HDMI_EVENT_NONE =		0x0,
+	HDMI_EVENT_HDMI_PLUGIN =	0x1,
+	HDMI_EVENT_HDMI_PLUGOUT =	0x2,
+	HDMI_EVENT_CEC =		0x4,
+	HDMI_EVENT_HDCP =		0x8,
+	HDMI_EVENT_CECTXERR =		0x10,
+	HDMI_EVENT_WAKEUP =		0x20,
+};
+
+enum hdmi_hdcp_auth_type {
+	HDMI_HDCP_AUTH_OFF = 0,
+	HDMI_HDCP_AUTH_START = 1,
+	HDMI_HDCP_AUTH_REV_LIST_REQ = 2,
+	HDMI_HDCP_AUTH_CONT = 3,
+};
+
+enum hdmi_hdcp_encr_type {
+	HDMI_HDCP_ENCR_OESS = 0,
+	HDMI_HDCP_ENCR_EESS = 1,
+};
+
+struct plug_detect {
+	u8 hdmi_detect_enable;
+	u8 on_time;
+	u8 hdmi_off_time;
+};
+
+struct edid_read {
+	u8 address;
+	u8 block_nr;
+	u8 data_length;
+	u8 data[HDMI_EDID_DATA_SIZE];
+};
+
+struct cec_rw  {
+	u8 src;
+	u8 dest;
+	u8 length;
+	u8 data[HDMI_CEC_SIZE];
+};
+
+struct info_fr {
+	u8 type;
+	u8 ver;
+	u8 crc;
+	u8 length;
+	u8 data[HDMI_INFOFR_SIZE];
+};
+
+struct hdcp_fuseaes {
+	u8 key[HDMI_FUSE_KEYSIZE];
+	u8 crc;
+	u8 result;
+};
+
+struct hdcp_loadaesall {
+	u8 key[HDMI_AES_KEYSIZE];
+	u8 ksv[HDMI_AES_KSVSIZE];
+	u8 crc32[HDMI_CRC32_SIZE];
+	u8 result;
+};
+
+struct hdcp_authencr {
+	u8 auth_type;
+	u8 encr_type;
+	u8 result;
+	u8 revoc_list[HDMI_REVOC_LIST_SIZE];
+};
+
+struct audio_cfg {
+	u8 if_format;
+	u8 i2s_entries;
+	u8 freq;
+	u8 word_length;
+	u8 format;
+	u8 if_mode;
+	u8 mute;
+};
+
+#endif /* __HDMI__H__ */
diff --git a/include/video/mcde.h b/include/video/mcde.h
new file mode 100644
index 00000000000..213f33e43fe
--- /dev/null
+++ b/include/video/mcde.h
@@ -0,0 +1,450 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE base driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __MCDE__H__
+#define __MCDE__H__
+
+/* Physical interface types */
+enum mcde_port_type {
+	MCDE_PORTTYPE_DSI = 0,
+	MCDE_PORTTYPE_DPI = 1,
+};
+
+/* Interface mode */
+enum mcde_port_mode {
+	MCDE_PORTMODE_CMD = 0,
+	MCDE_PORTMODE_VID = 1,
+};
+
+/* MCDE fifos */
+enum mcde_fifo {
+	MCDE_FIFO_A  = 0,
+	MCDE_FIFO_B  = 1,
+	MCDE_FIFO_C0 = 2,
+	MCDE_FIFO_C1 = 3,
+};
+
+/* MCDE channels (pixel pipelines) */
+enum mcde_chnl {
+	MCDE_CHNL_A  = 0,
+	MCDE_CHNL_B  = 1,
+	MCDE_CHNL_C0 = 2,
+	MCDE_CHNL_C1 = 3,
+};
+
+/* Channel path */
+#define MCDE_CHNLPATH(__chnl, __fifo, __type, __ifc, __link) \
+	(((__chnl) << 16) | ((__fifo) << 12) | \
+	 ((__type) << 8) | ((__ifc) << 4) | ((__link) << 0))
+enum mcde_chnl_path {
+	/* Channel A */
+	MCDE_CHNLPATH_CHNLA_FIFOA_DPI_0 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DPI, 0, 0),
+	MCDE_CHNLPATH_CHNLA_FIFOA_DSI_IFC0_0 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 0, 0),
+	MCDE_CHNLPATH_CHNLA_FIFOA_DSI_IFC0_1 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 0, 1),
+	MCDE_CHNLPATH_CHNLA_FIFOC0_DSI_IFC0_2 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 0, 2),
+	MCDE_CHNLPATH_CHNLA_FIFOC0_DSI_IFC1_0 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 1, 0),
+	MCDE_CHNLPATH_CHNLA_FIFOC0_DSI_IFC1_1 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 1, 1),
+	MCDE_CHNLPATH_CHNLA_FIFOA_DSI_IFC1_2 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 1, 2),
+	/* Channel B */
+	MCDE_CHNLPATH_CHNLB_FIFOB_DPI_1 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DPI, 0, 1),
+	MCDE_CHNLPATH_CHNLB_FIFOB_DSI_IFC0_0 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 0, 0),
+	MCDE_CHNLPATH_CHNLB_FIFOB_DSI_IFC0_1 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 0, 1),
+	MCDE_CHNLPATH_CHNLB_FIFOC1_DSI_IFC0_2 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 0, 2),
+	MCDE_CHNLPATH_CHNLB_FIFOC1_DSI_IFC1_0 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 1, 0),
+	MCDE_CHNLPATH_CHNLB_FIFOC1_DSI_IFC1_1 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 1, 1),
+	MCDE_CHNLPATH_CHNLB_FIFOB_DSI_IFC1_2 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 1, 2),
+	/* Channel C0 */
+	MCDE_CHNLPATH_CHNLC0_FIFOA_DSI_IFC0_0 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 0, 0),
+	MCDE_CHNLPATH_CHNLC0_FIFOA_DSI_IFC0_1 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 0, 1),
+	MCDE_CHNLPATH_CHNLC0_FIFOC0_DSI_IFC0_2 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 0, 2),
+	MCDE_CHNLPATH_CHNLC0_FIFOC0_DSI_IFC1_0 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 1, 0),
+	MCDE_CHNLPATH_CHNLC0_FIFOC0_DSI_IFC1_1 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 1, 1),
+	MCDE_CHNLPATH_CHNLC0_FIFOA_DSI_IFC1_2 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 1, 2),
+	/* Channel C1 */
+	MCDE_CHNLPATH_CHNLC1_FIFOB_DSI_IFC0_0 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 0, 0),
+	MCDE_CHNLPATH_CHNLC1_FIFOB_DSI_IFC0_1 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 0, 1),
+	MCDE_CHNLPATH_CHNLC1_FIFOC1_DSI_IFC0_2 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 0, 2),
+	MCDE_CHNLPATH_CHNLC1_FIFOC1_DSI_IFC1_0 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 1, 0),
+	MCDE_CHNLPATH_CHNLC1_FIFOC1_DSI_IFC1_1 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 1, 1),
+	MCDE_CHNLPATH_CHNLC1_FIFOB_DSI_IFC1_2 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 1, 2),
+};
+
+/* Update sync mode */
+enum mcde_sync_src {
+	MCDE_SYNCSRC_OFF = 0, /* No sync */
+	MCDE_SYNCSRC_TE0 = 1, /* MCDE ext TE0 */
+	MCDE_SYNCSRC_TE1 = 2, /* MCDE ext TE1 */
+	MCDE_SYNCSRC_BTA = 3, /* DSI BTA */
+	MCDE_SYNCSRC_TE_POLLING = 4, /* DSI TE_POLLING */
+};
+
+/* Interface pixel formats (output) */
+/*
+* REVIEW: Define formats
+* Add explanatory comments how the formats are ordered in memory
+*/
+enum mcde_port_pix_fmt {
+	/* MIPI standard formats */
+
+	MCDE_PORTPIXFMT_DPI_16BPP_C1 =     0x21,
+	MCDE_PORTPIXFMT_DPI_16BPP_C2 =     0x22,
+	MCDE_PORTPIXFMT_DPI_16BPP_C3 =     0x23,
+	MCDE_PORTPIXFMT_DPI_18BPP_C1 =     0x24,
+	MCDE_PORTPIXFMT_DPI_18BPP_C2 =     0x25,
+	MCDE_PORTPIXFMT_DPI_24BPP =        0x26,
+
+	MCDE_PORTPIXFMT_DSI_16BPP =        0x31,
+	MCDE_PORTPIXFMT_DSI_18BPP =        0x32,
+	MCDE_PORTPIXFMT_DSI_18BPP_PACKED = 0x33,
+	MCDE_PORTPIXFMT_DSI_24BPP =        0x34,
+
+	/* Custom formats */
+	MCDE_PORTPIXFMT_DSI_YCBCR422 =     0x40,
+};
+
+enum mcde_hdmi_sdtv_switch {
+	HDMI_SWITCH,
+	SDTV_SWITCH,
+	DVI_SWITCH
+};
+
+enum mcde_col_convert {
+	MCDE_CONVERT_RGB_2_RGB,
+	MCDE_CONVERT_RGB_2_YCBCR,
+	MCDE_CONVERT_YCBCR_2_RGB,
+	MCDE_CONVERT_YCBCR_2_YCBCR,
+};
+
+struct mcde_col_transform {
+	u16 matrix[3][3];
+	u16 offset[3];
+};
+
+#define MCDE_PORT_DPI_NO_CLOCK_DIV	0
+
+#define DPI_ACT_HIGH_ALL	0 /* all signals are active high	  */
+#define DPI_ACT_LOW_HSYNC	1 /* horizontal sync signal is active low */
+#define DPI_ACT_LOW_VSYNC	2 /* vertical sync signal is active low	  */
+#define DPI_ACT_LOW_DATA_ENABLE	4 /* data enable signal is active low	  */
+#define DPI_ACT_ON_FALLING_EDGE	8 /* drive data on the falling edge of the
+				   * pixel clock
+				   */
+
+struct mcde_port {
+	enum mcde_port_type type;
+	enum mcde_port_mode mode;
+	enum mcde_port_pix_fmt pixel_format;
+	u8 ifc;
+	u8 link;
+	enum mcde_sync_src sync_src;
+	bool update_auto_trig;
+	enum mcde_hdmi_sdtv_switch hdmi_sdtv_switch;
+	struct regulator *reg_esram;
+	union {
+		struct {
+			u8 virt_id;
+			u8 num_data_lanes;
+			u8 ui;
+			bool clk_cont;
+			struct clk *clk_dsi;
+			struct clk *clk_dsi_lp;
+			struct regulator *reg_vana;
+
+			/* DSI data lanes are swapped if true */
+			bool data_lanes_swap;
+		} dsi;
+		struct {
+			u8 bus_width;
+			bool tv_mode;
+			u16 clock_div; /* use 0 or 1 for no clock divider */
+			u32 polarity;    /* see DPI_ACT_LOW_* definitions */
+			struct clk *clk_dpi;
+		} dpi;
+	} phy;
+};
+
+/* Overlay pixel formats (input) *//* REVIEW: Define byte order */
+enum mcde_ovly_pix_fmt {
+	MCDE_OVLYPIXFMT_RGB565   = 1,
+	MCDE_OVLYPIXFMT_RGBA5551 = 2,
+	MCDE_OVLYPIXFMT_RGBA4444 = 3,
+	MCDE_OVLYPIXFMT_RGB888   = 4,
+	MCDE_OVLYPIXFMT_RGBX8888 = 5,
+	MCDE_OVLYPIXFMT_RGBA8888 = 6,
+	MCDE_OVLYPIXFMT_YCbCr422 = 7,
+};
+
+/* Display power modes */
+enum mcde_display_power_mode {
+	MCDE_DISPLAY_PM_OFF     = 0, /* Power off */
+	MCDE_DISPLAY_PM_STANDBY = 1, /* DCS sleep mode */
+	MCDE_DISPLAY_PM_ON      = 2, /* DCS normal mode, display on */
+};
+
+/* Display rotation */
+enum mcde_display_rotation {
+	MCDE_DISPLAY_ROT_0       = 0,
+	MCDE_DISPLAY_ROT_90_CCW  = 90,
+	MCDE_DISPLAY_ROT_180_CCW = 180,
+	MCDE_DISPLAY_ROT_270_CCW = 270,
+	MCDE_DISPLAY_ROT_90_CW   = MCDE_DISPLAY_ROT_270_CCW,
+	MCDE_DISPLAY_ROT_180_CW  = MCDE_DISPLAY_ROT_180_CCW,
+	MCDE_DISPLAY_ROT_270_CW  = MCDE_DISPLAY_ROT_90_CCW,
+};
+
+/* REVIEW: Verify */
+#define MCDE_MIN_WIDTH  16
+#define MCDE_MIN_HEIGHT 16
+#define MCDE_MAX_WIDTH  2048
+#define MCDE_MAX_HEIGHT 2048
+#define MCDE_BUF_START_ALIGMENT 8
+#define MCDE_BUF_LINE_ALIGMENT 8
+
+#define MCDE_FIFO_AB_SIZE 640
+#define MCDE_FIFO_C0C1_SIZE 160
+
+#define MCDE_PIXFETCH_LARGE_WTRMRKLVL 128
+#define MCDE_PIXFETCH_MEDIUM_WTRMRKLVL 64
+#define MCDE_PIXFETCH_SMALL_WTRMRKLVL 16
+
+/* Tv-out defines */
+#define MCDE_CONFIG_TVOUT_HBORDER 2
+#define MCDE_CONFIG_TVOUT_VBORDER 2
+#define MCDE_CONFIG_TVOUT_BACKGROUND_LUMINANCE		0x83
+#define MCDE_CONFIG_TVOUT_BACKGROUND_CHROMINANCE_CB	0x9C
+#define MCDE_CONFIG_TVOUT_BACKGROUND_CHROMINANCE_CR	0x2C
+
+/* In seconds */
+#define MCDE_AUTO_SYNC_WATCHDOG 5
+
+/* Hardware versions */
+#define MCDE_CHIP_VERSION_1_0_4 3	/* U5500 V1 */
+#define MCDE_CHIP_VERSION_3_0_8 2	/* U8500 V2 */
+#define MCDE_CHIP_VERSION_3_0_5 1	/* U8500 V1 */
+#define MCDE_CHIP_VERSION_3	0
+
+/* DSI modes */
+#define DSI_VIDEO_MODE	0
+#define DSI_CMD_MODE	1
+
+/* Video mode descriptor */
+struct mcde_video_mode {
+	u32 xres;
+	u32 yres;
+	u32 pixclock;	/* pixel clock in ps (pico seconds) */
+	u32 hbp;	/* horizontal back porch: left margin (excl. hsync) */
+	u32 hfp;	/* horizontal front porch: right margin (excl. hsync) */
+	u32 hsw;	/* horizontal sync width */
+	u32 vbp;	/* vertical back porch: upper margin (excl. vsync) */
+	u32 vfp;	/* vertical front porch: lower margin (excl. vsync) */
+	u32 vsw;	/* vertical sync width*/
+	bool interlaced;
+	bool force_update; /* when switching between hdmi and sdtv */
+};
+
+struct mcde_rectangle {
+	u16 x;
+	u16 y;
+	u16 w;
+	u16 h;
+};
+
+struct mcde_overlay_info {
+	u32 paddr;
+	u32 *vaddr;
+	u16 stride; /* buffer line len in bytes */
+	enum mcde_ovly_pix_fmt fmt;
+
+	u16 src_x;
+	u16 src_y;
+	u16 dst_x;
+	u16 dst_y;
+	u16 dst_z;
+	u16 w;
+	u16 h;
+	struct mcde_rectangle dirty;
+};
+
+struct mcde_overlay {
+	struct kobject kobj;
+	struct list_head list; /* mcde_display_device.ovlys */
+
+	struct mcde_display_device *ddev;
+	struct mcde_overlay_info info;
+	struct mcde_ovly_state *state;
+};
+
+/*
+ * Three functions for mapping 8 bits colour channels on 12 bits colour
+ * channels. The colour channels (ch0, ch1, ch2) can represent (r, g, b) or
+ * (Y, Cb, Cr) respectively.
+ */
+struct mcde_palette_table {
+	u16 (*map_col_ch0)(u8);
+	u16 (*map_col_ch1)(u8);
+	u16 (*map_col_ch2)(u8);
+};
+
+struct mcde_chnl_state;
+
+struct mcde_chnl_state *mcde_chnl_get(enum mcde_chnl chnl_id,
+			enum mcde_fifo fifo, const struct mcde_port *port);
+int mcde_chnl_set_pixel_format(struct mcde_chnl_state *chnl,
+					enum mcde_port_pix_fmt pix_fmt);
+int mcde_chnl_set_palette(struct mcde_chnl_state *chnl,
+					struct mcde_palette_table *palette);
+void mcde_chnl_set_col_convert(struct mcde_chnl_state *chnl,
+					struct mcde_col_transform *transform,
+					enum   mcde_col_convert    convert);
+int mcde_chnl_set_video_mode(struct mcde_chnl_state *chnl,
+					struct mcde_video_mode *vmode);
+/* TODO: Remove rotbuf* parameters when ESRAM allocator is implemented*/
+int mcde_chnl_set_rotation(struct mcde_chnl_state *chnl,
+		enum mcde_display_rotation rotation, u32 rotbuf1, u32 rotbuf2);
+int mcde_chnl_enable_synchronized_update(struct mcde_chnl_state *chnl,
+								bool enable);
+int mcde_chnl_set_power_mode(struct mcde_chnl_state *chnl,
+				enum mcde_display_power_mode power_mode);
+
+int mcde_chnl_apply(struct mcde_chnl_state *chnl);
+int mcde_chnl_update(struct mcde_chnl_state *chnl,
+			struct mcde_rectangle *update_area,
+			bool tripple_buffer);
+void mcde_chnl_put(struct mcde_chnl_state *chnl);
+
+void mcde_chnl_stop_flow(struct mcde_chnl_state *chnl);
+
+void mcde_chnl_enable(struct mcde_chnl_state *chnl);
+void mcde_chnl_disable(struct mcde_chnl_state *chnl);
+
+/* MCDE overlay */
+struct mcde_ovly_state;
+
+struct mcde_ovly_state *mcde_ovly_get(struct mcde_chnl_state *chnl);
+void mcde_ovly_set_source_buf(struct mcde_ovly_state *ovly,
+	u32 paddr);
+void mcde_ovly_set_source_info(struct mcde_ovly_state *ovly,
+	u32 stride, enum mcde_ovly_pix_fmt pix_fmt);
+void mcde_ovly_set_source_area(struct mcde_ovly_state *ovly,
+	u16 x, u16 y, u16 w, u16 h);
+void mcde_ovly_set_dest_pos(struct mcde_ovly_state *ovly,
+	u16 x, u16 y, u8 z);
+void mcde_ovly_apply(struct mcde_ovly_state *ovly);
+void mcde_ovly_put(struct mcde_ovly_state *ovly);
+
+/* MCDE dsi */
+
+#define DCS_CMD_ENTER_IDLE_MODE       0x39
+#define DCS_CMD_ENTER_INVERT_MODE     0x21
+#define DCS_CMD_ENTER_NORMAL_MODE     0x13
+#define DCS_CMD_ENTER_PARTIAL_MODE    0x12
+#define DCS_CMD_ENTER_SLEEP_MODE      0x10
+#define DCS_CMD_EXIT_IDLE_MODE        0x38
+#define DCS_CMD_EXIT_INVERT_MODE      0x20
+#define DCS_CMD_EXIT_SLEEP_MODE       0x11
+#define DCS_CMD_GET_ADDRESS_MODE      0x0B
+#define DCS_CMD_GET_BLUE_CHANNEL      0x08
+#define DCS_CMD_GET_DIAGNOSTIC_RESULT 0x0F
+#define DCS_CMD_GET_DISPLAY_MODE      0x0D
+#define DCS_CMD_GET_GREEN_CHANNEL     0x07
+#define DCS_CMD_GET_PIXEL_FORMAT      0x0C
+#define DCS_CMD_GET_POWER_MODE        0x0A
+#define DCS_CMD_GET_RED_CHANNEL       0x06
+#define DCS_CMD_GET_SCANLINE          0x45
+#define DCS_CMD_GET_SIGNAL_MODE       0x0E
+#define DCS_CMD_NOP                   0x00
+#define DCS_CMD_READ_DDB_CONTINUE     0xA8
+#define DCS_CMD_READ_DDB_START        0xA1
+#define DCS_CMD_READ_MEMORY_CONTINE   0x3E
+#define DCS_CMD_READ_MEMORY_START     0x2E
+#define DCS_CMD_SET_ADDRESS_MODE      0x36
+#define DCS_CMD_SET_COLUMN_ADDRESS    0x2A
+#define DCS_CMD_SET_DISPLAY_OFF       0x28
+#define DCS_CMD_SET_DISPLAY_ON        0x29
+#define DCS_CMD_SET_GAMMA_CURVE       0x26
+#define DCS_CMD_SET_PAGE_ADDRESS      0x2B
+#define DCS_CMD_SET_PARTIAL_AREA      0x30
+#define DCS_CMD_SET_PIXEL_FORMAT      0x3A
+#define DCS_CMD_SET_SCROLL_AREA       0x33
+#define DCS_CMD_SET_SCROLL_START      0x37
+#define DCS_CMD_SET_TEAR_OFF          0x34
+#define DCS_CMD_SET_TEAR_ON           0x35
+#define DCS_CMD_SET_TEAR_SCANLINE     0x44
+#define DCS_CMD_SOFT_RESET            0x01
+#define DCS_CMD_WRITE_LUT             0x2D
+#define DCS_CMD_WRITE_CONTINUE        0x3C
+#define DCS_CMD_WRITE_START           0x2C
+
+#define MCDE_MAX_DCS_READ   4
+#define MCDE_MAX_DSI_DIRECT_CMD_WRITE 15
+
+int mcde_dsi_generic_write(struct mcde_chnl_state *chnl, u8* para, int len);
+int mcde_dsi_dcs_write(struct mcde_chnl_state *chnl, u8 cmd, u8* data, int len);
+int mcde_dsi_dcs_read(struct mcde_chnl_state *chnl, u8 cmd, u8* data, int *len);
+
+/* MCDE */
+
+/* Driver data */
+#define MCDE_IRQ     "MCDE IRQ"
+#define MCDE_IO_AREA "MCDE I/O Area"
+
+struct mcde_platform_data {
+	/* DSI */
+	int num_dsilinks;
+
+	/* DPI */
+	u8 outmux[5]; /* MCDE_CONF0.OUTMUXx */
+	u8 syncmux;   /* MCDE_CONF0.SYNCMUXx */
+
+	const char *regulator_vana_id;
+	const char *regulator_mcde_epod_id;
+	const char *regulator_esram_epod_id;
+	int num_channels;
+	int num_overlays;
+	const char *clock_dsi_id;
+	const char *clock_dsi_lp_id;
+	const char *clock_dpi_id;
+	const char *clock_mcde_id;
+
+	int (*platform_set_clocks)(void);
+	int (*platform_enable_dsipll)(void);
+	int (*platform_disable_dsipll)(void);
+};
+
+int mcde_init(void);
+void mcde_exit(void);
+
+#endif /* __MCDE__H__ */
diff --git a/include/video/mcde_display-ab8500.h b/include/video/mcde_display-ab8500.h
new file mode 100644
index 00000000000..ffebe62af92
--- /dev/null
+++ b/include/video/mcde_display-ab8500.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * AB8500 tvout driver interface
+ *
+ * Author: Marcel Tunnissen <marcel.tuennissen@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __DISPLAY_AB8500__H__
+#define __DISPLAY_AB8500__H__
+
+#include <video/mcde.h>
+
+struct ab8500_display_platform_data {
+	/* Platform info */
+	struct mcde_col_transform *rgb_2_yCbCr_transform;
+	int nr_regulators;
+	const char *regulator_id[];
+};
+
+#endif /* __DISPLAY_AB8500__H__*/
+
diff --git a/include/video/mcde_display-av8100.h b/include/video/mcde_display-av8100.h
new file mode 100644
index 00000000000..52578a675f1
--- /dev/null
+++ b/include/video/mcde_display-av8100.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE HDMI display driver
+ *
+ * Author: Per Persson <per-xb-persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __DISPLAY_AV8100__H__
+#define __DISPLAY_AV8100__H__
+
+#include <linux/regulator/consumer.h>
+
+#include "mcde_display.h"
+
+#define GPIO_AV8100_RSTN	196
+#define NATIVE_XRES_HDMI	1280
+#define NATIVE_YRES_HDMI	720
+#define NATIVE_XRES_SDTV	720
+#define NATIVE_YRES_SDTV	576
+#define DISPONOFF_SIZE		6
+#define TIMING_SIZE		2
+#define STAYALIVE_SIZE		1
+
+struct mcde_display_hdmi_platform_data {
+	/* Platform info */
+	int reset_gpio;
+	bool reset_high;
+	const char *regulator_id;
+	const char *cvbs_regulator_id;
+	int reset_delay; /* ms */
+	u32 ddb_id;
+	struct mcde_col_transform *rgb_2_yCbCr_transform;
+
+	/* Driver data */ /* TODO: move to driver data instead */
+	bool hdmi_platform_enable;
+	struct regulator *regulator;
+};
+
+struct display_driver_data {
+	struct regulator *cvbs_regulator;
+	bool cvbs_regulator_enabled;
+	bool update_port_pixel_format;
+	char *fbdevname;
+	struct mcde_video_mode *video_mode;
+};
+
+void hdmi_fb_onoff(struct mcde_display_device *ddev, bool enable,
+				u8 cea, u8 vesa_cea_nr);
+
+#endif /* __DISPLAY_AV8100__H__ */
diff --git a/include/video/mcde_display-generic_dsi.h b/include/video/mcde_display-generic_dsi.h
new file mode 100644
index 00000000000..87ef6baf67a
--- /dev/null
+++ b/include/video/mcde_display-generic_dsi.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE generic DCS display driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __MCDE_DISPLAY_GENERIC__H__
+#define __MCDE_DISPLAY_GENERIC__H__
+
+#include <linux/regulator/consumer.h>
+
+#include "mcde_display.h"
+
+struct mcde_display_generic_platform_data {
+	/* Platform info */
+	int reset_gpio;
+	bool reset_high;
+	const char *regulator_id;
+	int reset_delay; /* ms */
+	int sleep_out_delay; /* ms */
+	u32 ddb_id;
+
+	/* Driver data */
+	bool generic_platform_enable;
+	struct regulator *regulator;
+	int max_supply_voltage;
+	int min_supply_voltage;
+};
+
+#endif /* __MCDE_DISPLAY_GENERIC__H__ */
+
diff --git a/include/video/mcde_display-sony_sy35560_dsi.h b/include/video/mcde_display-sony_sy35560_dsi.h
new file mode 100644
index 00000000000..997a7ed1f85
--- /dev/null
+++ b/include/video/mcde_display-sony_sy35560_dsi.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE Sony sy35560 DCS display driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __MCDE_DISPLAY_SONY_SY35560__H__
+#define __MCDE_DISPLAY_SONY_SY35560__H__
+
+#include <linux/regulator/consumer.h>
+
+#include "mcde_display.h"
+#include <linux/workqueue.h>
+
+/* period between ESD status checks */
+#define SONY_SY35560_ESD_CHECK_PERIOD	msecs_to_jiffies(10000)
+
+struct sony_sy35560_platform_data {
+	/* Platform info */
+	int reset_gpio;
+	bool reset_high;
+	const char *regulator_id;
+	bool skip_init;
+
+	/* Driver data */
+	int max_supply_voltage;
+	int min_supply_voltage;
+};
+
+struct sony_sy35560_device {
+	struct mcde_display_device base;
+
+	struct regulator *regulator;
+
+	/* ESD workqueue */
+	struct workqueue_struct *esd_wq;
+	struct delayed_work esd_work;
+};
+
+#endif /* __MCDE_DISPLAY_SONY_SY35560__H__ */
+
diff --git a/include/video/mcde_display-vuib500-dpi.h b/include/video/mcde_display-vuib500-dpi.h
new file mode 100644
index 00000000000..94bad83bf97
--- /dev/null
+++ b/include/video/mcde_display-vuib500-dpi.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE DPI display driver
+ *
+ * Author: Torbjorn Svensson <torbjorn.x.svensson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __MCDE_DISPLAY_DPI__H__
+#define __MCDE_DISPLAY_DPI__H__
+
+#include <linux/regulator/consumer.h>
+
+#include "mcde_display.h"
+
+struct mcde_display_dpi_platform_data {
+	/* Platform info */
+	int reset_gpio;
+	bool reset_high;
+	const char *regulator_id;
+	int reset_delay;
+
+	/* Driver data */
+	struct regulator *regulator;
+	int max_supply_voltage;
+	int min_supply_voltage;
+};
+#endif /* __MCDE_DISPLAY_DPI__H__ */
diff --git a/include/video/mcde_display.h b/include/video/mcde_display.h
new file mode 100644
index 00000000000..a5e449f6b5b
--- /dev/null
+++ b/include/video/mcde_display.h
@@ -0,0 +1,145 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE display driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __MCDE_DISPLAY__H__
+#define __MCDE_DISPLAY__H__
+
+#include <linux/device.h>
+#include <linux/pm.h>
+
+#include <video/mcde.h>
+
+#define UPDATE_FLAG_PIXEL_FORMAT	0x1
+#define UPDATE_FLAG_VIDEO_MODE		0x2
+#define UPDATE_FLAG_ROTATION		0x4
+
+#define to_mcde_display_device(__dev) \
+	container_of((__dev), struct mcde_display_device, dev)
+
+struct mcde_display_device {
+	/* MCDE driver static */
+	struct device     dev;
+	const char       *name;
+	int               id;
+	struct mcde_port *port;
+	struct fb_info   *fbi;
+	bool              fictive;
+
+	/* MCDE dss driver internal */
+	bool initialized;
+	enum mcde_chnl chnl_id;
+	enum mcde_fifo fifo;
+	bool first_update;
+	struct mutex display_lock;
+
+	bool enabled;
+	struct mcde_chnl_state *chnl_state;
+	struct list_head ovlys;
+	struct mcde_rectangle update_area;
+	/* TODO: Remove once ESRAM allocator is done */
+	u32 rotbuf1;
+	u32 rotbuf2;
+
+	/* Display driver internal */
+	u16 native_x_res;
+	u16 native_y_res;
+	u16 physical_width;
+	u16 physical_height;
+	enum mcde_display_power_mode power_mode;
+	enum mcde_ovly_pix_fmt default_pixel_format;
+	enum mcde_ovly_pix_fmt pixel_format;
+	enum mcde_display_rotation rotation;
+	bool synchronized_update;
+	struct mcde_video_mode video_mode;
+	int update_flags;
+	bool stay_alive;
+
+	/* Driver API */
+	void (*get_native_resolution)(struct mcde_display_device *dev,
+		u16 *x_res, u16 *y_res);
+	enum mcde_ovly_pix_fmt (*get_default_pixel_format)(
+		struct mcde_display_device *dev);
+	void (*get_physical_size)(struct mcde_display_device *dev,
+		u16 *x_size, u16 *y_size);
+
+	int (*set_power_mode)(struct mcde_display_device *dev,
+		enum mcde_display_power_mode power_mode);
+	enum mcde_display_power_mode (*get_power_mode)(
+		struct mcde_display_device *dev);
+
+	int (*try_video_mode)(struct mcde_display_device *dev,
+		struct mcde_video_mode *video_mode);
+	int (*set_video_mode)(struct mcde_display_device *dev,
+		struct mcde_video_mode *video_mode);
+	void (*get_video_mode)(struct mcde_display_device *dev,
+		struct mcde_video_mode *video_mode);
+	int (*set_pixel_format)(struct mcde_display_device *dev,
+		enum mcde_ovly_pix_fmt pix_fmt);
+	enum mcde_ovly_pix_fmt (*get_pixel_format)(
+		struct mcde_display_device *dev);
+	enum mcde_port_pix_fmt (*get_port_pixel_format)(
+		struct mcde_display_device *dev);
+
+	int (*set_rotation)(struct mcde_display_device *dev,
+		enum mcde_display_rotation rotation);
+	enum mcde_display_rotation (*get_rotation)(
+		struct mcde_display_device *dev);
+
+	int (*set_synchronized_update)(struct mcde_display_device *dev,
+		bool enable);
+	bool (*get_synchronized_update)(struct mcde_display_device *dev);
+
+	int (*apply_config)(struct mcde_display_device *dev);
+	int (*invalidate_area)(struct mcde_display_device *dev,
+						struct mcde_rectangle *area);
+	int (*update)(struct mcde_display_device *dev, bool tripple_buffer);
+	int (*prepare_for_update)(struct mcde_display_device *dev,
+		u16 x, u16 y, u16 w, u16 h);
+	int (*on_first_update)(struct mcde_display_device *dev);
+	int (*platform_enable)(struct mcde_display_device *dev);
+	int (*platform_disable)(struct mcde_display_device *dev);
+	int (*ceanr_convert)(struct mcde_display_device *ddev,
+			u8 cea, u8 vesa_cea_nr, int buffering,
+			u16 *w, u16 *h, u16 *vw, u16 *vh);
+};
+
+struct mcde_display_driver {
+	int (*probe)(struct mcde_display_device *dev);
+	int (*remove)(struct mcde_display_device *dev);
+	void (*shutdown)(struct mcde_display_device *dev);
+	int (*suspend)(struct mcde_display_device *dev,
+		pm_message_t state);
+	int (*resume)(struct mcde_display_device *dev);
+
+	struct device_driver driver;
+};
+
+/* MCDE dsi (Used by MCDE display drivers) */
+
+int mcde_display_dsi_dcs_write(struct mcde_display_device *dev,
+	u8 cmd, u8 *data, int len);
+int mcde_display_dsi_dcs_read(struct mcde_display_device *dev,
+	u8 cmd, u8 *data, int *len);
+int mcde_display_dsi_bta_sync(struct mcde_display_device *dev);
+
+/* MCDE display bus */
+
+int mcde_display_driver_register(struct mcde_display_driver *drv);
+void mcde_display_driver_unregister(struct mcde_display_driver *drv);
+int mcde_display_device_register(struct mcde_display_device *dev);
+void mcde_display_device_unregister(struct mcde_display_device *dev);
+
+void mcde_display_init_device(struct mcde_display_device *dev);
+
+int mcde_display_init(void);
+void mcde_display_exit(void);
+
+#endif /* __MCDE_DISPLAY__H__ */
+
diff --git a/include/video/mcde_dss.h b/include/video/mcde_dss.h
new file mode 100644
index 00000000000..efed79ad023
--- /dev/null
+++ b/include/video/mcde_dss.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson MCDE display sub system driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __MCDE_DSS__H__
+#define __MCDE_DSS__H__
+
+#include <linux/kobject.h>
+#include <linux/notifier.h>
+
+#include "mcde.h"
+#include "mcde_display.h"
+
+/* Public MCDE dss (Used by MCDE fb ioctl & MCDE display sysfs) */
+int mcde_dss_open_channel(struct mcde_display_device *ddev);
+void mcde_dss_close_channel(struct mcde_display_device *ddev);
+int mcde_dss_enable_display(struct mcde_display_device *ddev);
+void mcde_dss_disable_display(struct mcde_display_device *ddev);
+int mcde_dss_apply_channel(struct mcde_display_device *ddev);
+struct mcde_overlay *mcde_dss_create_overlay(struct mcde_display_device *ddev,
+	struct mcde_overlay_info *info);
+void mcde_dss_destroy_overlay(struct mcde_overlay *ovl);
+int mcde_dss_enable_overlay(struct mcde_overlay *ovl);
+void mcde_dss_disable_overlay(struct mcde_overlay *ovl);
+int mcde_dss_apply_overlay(struct mcde_overlay *ovl,
+						struct mcde_overlay_info *info);
+void mcde_dss_get_overlay_info(struct mcde_overlay *ovly,
+				struct mcde_overlay_info *info);
+int mcde_dss_update_overlay(struct mcde_overlay *ovl, bool tripple_buffer);
+
+void mcde_dss_get_native_resolution(struct mcde_display_device *ddev,
+	u16 *x_res, u16 *y_res);
+enum mcde_ovl_pix_fmt mcde_dss_get_default_color_format(
+	struct mcde_display_device *ddev);
+void mcde_dss_get_physical_size(struct mcde_display_device *ddev,
+	u16 *x_size, u16 *y_size); /* mm */
+
+int mcde_dss_try_video_mode(struct mcde_display_device *ddev,
+	struct mcde_video_mode *video_mode);
+int mcde_dss_set_video_mode(struct mcde_display_device *ddev,
+	struct mcde_video_mode *video_mode);
+void mcde_dss_get_video_mode(struct mcde_display_device *ddev,
+	struct mcde_video_mode *video_mode);
+
+int mcde_dss_set_pixel_format(struct mcde_display_device *ddev,
+	enum mcde_ovly_pix_fmt pix_fmt);
+int mcde_dss_get_pixel_format(struct mcde_display_device *ddev);
+
+int mcde_dss_set_rotation(struct mcde_display_device *ddev,
+	enum mcde_display_rotation rotation);
+enum mcde_display_rotation mcde_dss_get_rotation(
+	struct mcde_display_device *ddev);
+
+int mcde_dss_set_synchronized_update(struct mcde_display_device *ddev,
+	bool enable);
+bool mcde_dss_get_synchronized_update(struct mcde_display_device *ddev);
+
+/* MCDE dss events */
+
+/*      A display device and driver has been loaded, probed and bound */
+#define MCDE_DSS_EVENT_DISPLAY_REGISTERED    1
+/*      A display device has been removed */
+#define MCDE_DSS_EVENT_DISPLAY_UNREGISTERED  2
+
+/*      Note! Notifier callback will be called holding the dev sem */
+int mcde_dss_register_notifier(struct notifier_block *nb);
+int mcde_dss_unregister_notifier(struct notifier_block *nb);
+
+/* MCDE dss driver */
+
+int mcde_dss_init(void);
+void mcde_dss_exit(void);
+
+#endif /* __MCDE_DSS__H__ */
+
diff --git a/include/video/mcde_fb.h b/include/video/mcde_fb.h
new file mode 100644
index 00000000000..17556414aa0
--- /dev/null
+++ b/include/video/mcde_fb.h
@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson MCDE display sub system frame buffer driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __MCDE_FB__H__
+#define __MCDE_FB__H__
+
+#include <linux/fb.h>
+#include <linux/ioctl.h>
+#if !defined(__KERNEL__) && !defined(_KERNEL)
+#include <stdint.h>
+#else
+#include <linux/types.h>
+#include <linux/hwmem.h>
+#endif
+
+#ifdef __KERNEL__
+#include "mcde_dss.h"
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+#endif
+
+#define MCDE_GET_BUFFER_NAME_IOC _IO('M', 1)
+
+#ifdef __KERNEL__
+#define to_mcde_fb(x) ((struct mcde_fb *)(x)->par)
+
+#define MCDE_FB_MAX_NUM_OVERLAYS 3
+
+struct mcde_fb {
+	int num_ovlys;
+	struct mcde_overlay *ovlys[MCDE_FB_MAX_NUM_OVERLAYS];
+	u32 pseudo_palette[17];
+	enum mcde_ovly_pix_fmt pix_fmt;
+	int id;
+	struct hwmem_alloc *alloc;
+	int alloc_name;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+};
+
+/* MCDE fbdev API */
+struct fb_info *mcde_fb_create(struct mcde_display_device *ddev,
+		uint16_t w, uint16_t h, uint16_t vw, uint16_t vh,
+		enum mcde_ovly_pix_fmt pix_fmt, uint32_t rotate);
+
+int mcde_fb_attach_overlay(struct fb_info *fb_info,
+	struct mcde_overlay *ovl);
+void mcde_fb_destroy(struct mcde_display_device *ddev);
+
+/* MCDE fb driver */
+int mcde_fb_init(void);
+void mcde_fb_exit(void);
+#endif
+
+#endif /* __MCDE_FB__H__ */
+
diff --git a/init/Kconfig b/init/Kconfig
index 4fe9168149f..4e5c05f8c71 100644
--- a/init/Kconfig
+++ b/init/Kconfig
@@ -1281,6 +1281,15 @@ config PROFILING
 	  Say Y here to enable the extended profiling support mechanisms used
 	  by profilers such as OProfile.
 
+config BOOTTIME
+	bool "Boot time measurments"
+	default n
+	help
+		Adds sysfs entries (boottime/) with start-up timing information.
+		If CONFIG_DEBUG_FS is enabled, detailed information about the
+		boot time, including system load during boot can be extraced.
+		This information can be visualised with help of the bootgraph script.
+
 #
 # Place an empty function call at each tracepoint site. Can be
 # dynamically changed for a probe function.
diff --git a/init/Makefile b/init/Makefile
index 0bf677aa087..6b77be3855f 100644
--- a/init/Makefile
+++ b/init/Makefile
@@ -9,6 +9,7 @@ else
 obj-$(CONFIG_BLK_DEV_INITRD)   += initramfs.o
 endif
 obj-$(CONFIG_GENERIC_CALIBRATE_DELAY) += calibrate.o
+obj-$(CONFIG_BOOTTIME)		      += boottime.o
 
 mounts-y			:= do_mounts.o
 mounts-$(CONFIG_BLK_DEV_RAM)	+= do_mounts_rd.o
diff --git a/init/boottime.c b/init/boottime.c
new file mode 100644
index 00000000000..be73e0ee550
--- /dev/null
+++ b/init/boottime.c
@@ -0,0 +1,467 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2009-2010
+ *
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * boottime is a tool for collecting start-up timing
+ * information and can together with boot loader support
+ * display a total system start-up time.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/seq_file.h>
+#include <linux/debugfs.h>
+#include <linux/spinlock.h>
+#include <linux/boottime.h>
+#include <linux/kernel_stat.h>
+#include <linux/kobject.h>
+#include <linux/device.h>
+#include <linux/sysfs.h>
+#include <linux/slab.h>
+
+/*
+ * BOOTTIME_MAX_NAME_LEN is defined in arch/arm/include/asm/setup.h to 64.
+ * No crisis if they don't match.
+ */
+#ifndef BOOTTIME_MAX_NAME_LEN
+#define BOOTTIME_MAX_NAME_LEN 64
+#endif
+
+/*
+ * We have a few static entries, since it is good to have measure points
+ * before the system is up and running properly
+ */
+#define NUM_STATIC_BOOTTIME_ENTRIES 16
+
+struct boottime_list {
+	struct list_head list;
+	char name[BOOTTIME_MAX_NAME_LEN];
+	/* Time in us since power on, possible including boot loader. */
+	unsigned long time;
+	bool cpu_load;
+	struct cpu_usage_stat cpu_usage[NR_CPUS];
+};
+
+enum boottime_filter_type {
+	BOOTTIME_FILTER_OUT_ZERO,
+	BOOTTIME_FILTER_OUT_LESS_100,
+	BOOTTIME_FILTER_NOTHING,
+};
+
+enum boottime_symbolic_print {
+	BOOTTIME_SYMBOLIC_PRINT,
+	BOOTTIME_NORMAL_PRINT,
+};
+
+enum boottime_cpu_load {
+	BOOTTIME_CPU_LOAD,
+	BOOTTIME_NO_CPU_LOAD,
+};
+
+static LIST_HEAD(boottime_list);
+static __initdata DEFINE_SPINLOCK(boottime_list_lock);
+static __initdata struct boottime_timer boottime_timer;
+static __initdata int num_const_boottime_list;
+static struct boottime_list const_boottime_list[NUM_STATIC_BOOTTIME_ENTRIES];
+static unsigned long time_kernel_done;
+static unsigned long time_bootloader_done;
+static __initdata bool system_up;
+static bool boottime_done;
+
+int __attribute__((weak)) boottime_arch_startup(void)
+{
+	return 0;
+}
+
+int __attribute__((weak)) boottime_bootloader_idle(void)
+{
+	return 0;
+}
+
+static void __init boottime_mark_core(char *name,
+				      unsigned long time,
+				      enum boottime_symbolic_print symbolic,
+				      enum boottime_cpu_load cpu_load)
+{
+	struct boottime_list *b;
+	unsigned long flags = 0;
+	int i;
+
+	if (system_up) {
+		b = kmalloc(sizeof(struct boottime_list), GFP_KERNEL);
+		if (!b) {
+			printk(KERN_ERR
+			       "boottime: failed to allocate memory!\n");
+			return;
+		}
+
+	} else {
+		if (num_const_boottime_list < NUM_STATIC_BOOTTIME_ENTRIES) {
+			b = &const_boottime_list[num_const_boottime_list];
+			num_const_boottime_list++;
+		} else {
+			printk(KERN_ERR
+			       "boottime: too many early measure points!\n");
+			return;
+		}
+	}
+
+	INIT_LIST_HEAD(&b->list);
+
+	if (symbolic == BOOTTIME_SYMBOLIC_PRINT)
+		snprintf(b->name, BOOTTIME_MAX_NAME_LEN, "%pF", name);
+	else
+		strncpy(b->name, name, BOOTTIME_MAX_NAME_LEN);
+
+	b->name[BOOTTIME_MAX_NAME_LEN - 1] = '\0';
+	b->time = time;
+	b->cpu_load = cpu_load;
+
+	if (cpu_load == BOOTTIME_CPU_LOAD && system_up)
+		for_each_possible_cpu(i) {
+			b->cpu_usage[i].system = kstat_cpu(i).cpustat.system;
+			b->cpu_usage[i].idle = kstat_cpu(i).cpustat.idle;
+			b->cpu_usage[i].iowait = kstat_cpu(i).cpustat.iowait;
+			b->cpu_usage[i].irq = kstat_cpu(i).cpustat.irq;
+			/*
+			 * TODO: Make sure that user, nice, softirq, steal
+			 * and guest are not used during boot
+			 */
+		}
+	else
+		b->cpu_load = BOOTTIME_NO_CPU_LOAD;
+
+	if (system_up) {
+		spin_lock_irqsave(&boottime_list_lock, flags);
+		list_add(&b->list, &boottime_list);
+		spin_unlock_irqrestore(&boottime_list_lock, flags);
+	} else {
+		list_add(&b->list, &boottime_list);
+	}
+}
+
+void __init boottime_mark_wtime(char *name, unsigned long time)
+{
+	boottime_mark_core(name, time,
+			   BOOTTIME_NORMAL_PRINT,
+			   BOOTTIME_NO_CPU_LOAD);
+}
+
+void __ref boottime_mark_symbolic(void *name)
+{
+
+	if (boottime_done)
+		return;
+
+	if (boottime_timer.get_time)
+		boottime_mark_core((char *) name,
+				   boottime_timer.get_time(),
+				   BOOTTIME_SYMBOLIC_PRINT,
+				   BOOTTIME_CPU_LOAD);
+}
+
+void __init boottime_mark(char *name)
+{
+	if (boottime_timer.get_time)
+		boottime_mark_core(name,
+				   boottime_timer.get_time(),
+				   BOOTTIME_NORMAL_PRINT,
+				   BOOTTIME_CPU_LOAD);
+}
+
+void __init boottime_activate(struct boottime_timer *bt)
+{
+	struct boottime_list *b;
+	int res = 0;
+	unsigned long flags;
+
+	if (bt == NULL) {
+		printk(KERN_ERR
+		       "boottime: error: bad configured\n");
+		return;
+	}
+
+	if (bt->get_time == NULL) {
+		printk(KERN_ERR
+		       "boottime: error: you must provide a get_time() function\n");
+		return;
+	}
+	memcpy(&boottime_timer, bt, sizeof(struct boottime_timer));
+
+	if (boottime_timer.init)
+		res = boottime_timer.init();
+
+	if (res) {
+		printk(KERN_ERR "boottime: initialization failed\n");
+		return;
+	}
+
+	if (boottime_arch_startup())
+		printk(KERN_ERR
+		       "boottime: arch specfic initialization failed\n");
+
+	spin_lock_irqsave(&boottime_list_lock, flags);
+
+	if (!list_empty(&boottime_list)) {
+
+		b = list_first_entry(&boottime_list, struct boottime_list,
+				     list);
+		if (b)
+			time_bootloader_done = b->time;
+	}
+
+	spin_unlock_irqrestore(&boottime_list_lock, flags);
+}
+
+void __init boottime_system_up(void)
+{
+	system_up = true;
+}
+
+void __init boottime_deactivate(void)
+{
+	struct boottime_list *b;
+	unsigned long flags;
+
+	boottime_mark("execute_init+0x0/0x0");
+
+	boottime_done = true;
+
+	spin_lock_irqsave(&boottime_list_lock, flags);
+	b = list_first_entry(&boottime_list, struct boottime_list, list);
+	spin_unlock_irqrestore(&boottime_list_lock, flags);
+
+	time_kernel_done = b->time;
+
+	if (boottime_timer.finalize)
+		boottime_timer.finalize();
+}
+
+#ifdef CONFIG_DEBUG_FS
+static void boottime_debugfs_load(struct seq_file *s,
+				  struct boottime_list *b,
+				  struct boottime_list *p)
+{
+	int i;
+	unsigned long total_p, total_b;
+	unsigned long system_total, idle_total, irq_total, iowait_total;
+	unsigned long system_load, idle_load, irq_load, iowait_load;
+
+	for_each_possible_cpu(i) {
+		total_b = (b->cpu_usage[i].system +
+			   b->cpu_usage[i].idle +
+			   b->cpu_usage[i].iowait +
+			   b->cpu_usage[i].irq);
+
+		total_p = (p->cpu_usage[i].system +
+			   p->cpu_usage[i].idle +
+			   p->cpu_usage[i].iowait +
+			   p->cpu_usage[i].irq);
+
+		if (total_b == total_p)
+			continue;
+
+		system_total = b->cpu_usage[i].system - p->cpu_usage[i].system;
+		idle_total = b->cpu_usage[i].idle - p->cpu_usage[i].idle;
+		irq_total = b->cpu_usage[i].irq - p->cpu_usage[i].irq;
+		iowait_total = b->cpu_usage[i].iowait - p->cpu_usage[i].iowait;
+
+		system_load = (100 * system_total / (total_b - total_p));
+		idle_load = (100 * idle_total / (total_b - total_p));
+		irq_load = (100 * irq_total / (total_b - total_p));
+		iowait_load = (100 * iowait_total / (total_b - total_p));
+
+		seq_printf(s,
+			   " cpu%d system: %lu%% idle: %lu%% iowait: %lu%% irq: %lu%%",
+			   i,
+			   system_load,
+			   idle_load,
+			   iowait_load,
+			   irq_load);
+	}
+	seq_printf(s, "\n");
+}
+
+static void boottime_debugfs_print(struct seq_file *s,
+				   struct boottime_list *b,
+				   struct boottime_list *p)
+{
+	seq_printf(s, "[%5lu.%06lu] calling  %s\n",
+		   p->time / 1000000,
+		   (p->time  % 1000000),
+		   p->name);
+	seq_printf(s, "[%5lu.%06lu] initcall %s returned 0 after %ld msecs.",
+		   b->time / 1000000,
+		   (b->time  % 1000000),
+		   p->name, (b->time - p->time) / 1000);
+
+	if (p->cpu_load == BOOTTIME_NO_CPU_LOAD ||
+	    b->cpu_load == BOOTTIME_NO_CPU_LOAD) {
+		seq_printf(s, "\n");
+		return;
+	}
+
+	boottime_debugfs_load(s, b, p);
+}
+
+static int boottime_debugfs_bootgraph_show(struct seq_file *s, void *iter)
+{
+	struct boottime_list *b, *p = NULL, *old_p = NULL;
+	enum boottime_filter_type filter = (int)s->private;
+
+	list_for_each_entry_reverse(b, &boottime_list, list) {
+		if (p) {
+			if (!(filter == BOOTTIME_FILTER_OUT_ZERO &&
+			     (b->time - p->time) / 1000 == 0)
+			   && !(filter == BOOTTIME_FILTER_OUT_LESS_100 &&
+				(b->time - p->time) < 100 * 1000))
+				boottime_debugfs_print(s, b, p);
+			old_p = p;
+		}
+		p = b;
+	}
+
+	if (filter == BOOTTIME_FILTER_NOTHING && p)
+		boottime_debugfs_print(s, p, p);
+
+	if (p)
+		seq_printf(s, "[%5lu.%06lu] Freeing init memory: 0K\n",
+			   p->time / 1000000, p->time  % 1000000);
+	return 0;
+}
+
+static int boottime_debugfs_summary_show(struct seq_file *s, void *data)
+{
+	struct boottime_list *b, b_zero;
+
+	if (time_bootloader_done)
+		seq_printf(s, "bootloader: %ld msecs\n",
+			   time_bootloader_done / 1000);
+
+	seq_printf(s, "kernel: %ld msecs\ntotal: %ld msecs\n",
+		   (time_kernel_done - time_bootloader_done) / 1000,
+		   time_kernel_done / 1000);
+	seq_printf(s, "kernel:");
+	b = list_first_entry(&boottime_list,
+			     struct boottime_list, list);
+	memset(&b_zero, 0, sizeof(struct boottime_list));
+	boottime_debugfs_load(s, b, &b_zero);
+
+	if (time_bootloader_done)
+		seq_printf(s,
+			   "bootloader: cpu0 system: %d%% idle: %d%% iowait: 0%% irq: 0%%\n",
+			   100 - boottime_bootloader_idle(),
+			   boottime_bootloader_idle());
+	return 0;
+}
+
+static int boottime_debugfs_bootgraph_open(struct inode *inode,
+					   struct file *file)
+{
+	return single_open(file,
+			   boottime_debugfs_bootgraph_show,
+			   inode->i_private);
+}
+
+static int boottime_debugfs_summary_open(struct inode *inode,
+					 struct file *file)
+{
+	return single_open(file,
+			   boottime_debugfs_summary_show,
+			   inode->i_private);
+}
+
+static const struct file_operations boottime_debugfs_bootgraph_operations = {
+	.open		= boottime_debugfs_bootgraph_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static const struct file_operations boottime_debugfs_summary_operations = {
+	.open		= boottime_debugfs_summary_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+void boottime_debugfs_init(void)
+{
+	struct dentry *dir;
+
+	dir = debugfs_create_dir("boottime", NULL);
+
+	(void) debugfs_create_file("bootgraph", S_IFREG | S_IRUGO,
+				   dir, (void *)BOOTTIME_FILTER_NOTHING,
+				   &boottime_debugfs_bootgraph_operations);
+	(void) debugfs_create_file("bootgraph_all_except0", S_IFREG | S_IRUGO,
+				   dir, (void *)BOOTTIME_FILTER_OUT_ZERO,
+				   &boottime_debugfs_bootgraph_operations);
+	(void) debugfs_create_file("bootgraph_larger100",
+				   S_IFREG | S_IRUGO,
+				   dir, (void *)BOOTTIME_FILTER_OUT_LESS_100,
+				   &boottime_debugfs_bootgraph_operations);
+	(void) debugfs_create_file("summary", S_IFREG | S_IRUGO,
+				   dir, NULL,
+				   &boottime_debugfs_summary_operations);
+}
+#else
+#define boottime_debugfs_init(x)
+#endif
+
+static ssize_t show_bootloader(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	return sprintf(buf, "%ld\n", time_bootloader_done / 1000);
+}
+
+static ssize_t show_kernel(struct device *dev,
+			   struct device_attribute *attr,
+			   char *buf)
+{
+	return sprintf(buf, "%ld\n",
+		       (time_kernel_done - time_bootloader_done) / 1000);
+}
+
+DEVICE_ATTR(kernel, 0444, show_kernel, NULL);
+DEVICE_ATTR(bootloader, 0444, show_bootloader, NULL);
+
+static struct attribute *boottime_sysfs_entries[] = {
+	&dev_attr_kernel.attr,
+	&dev_attr_bootloader.attr,
+	NULL
+};
+
+static struct attribute_group boottime_attr_grp = {
+	.name = NULL,
+	.attrs = boottime_sysfs_entries,
+};
+
+static int __init boottime_init(void)
+{
+	struct kobject *boottime_kobj;
+
+	boottime_kobj = kobject_create_and_add("boottime", NULL);
+	if (!boottime_kobj) {
+		printk(KERN_ERR "boottime: out of memory!\n");
+		return -ENOMEM;
+	}
+
+	if (sysfs_create_group(boottime_kobj, &boottime_attr_grp) < 0) {
+		kobject_put(boottime_kobj);
+		printk(KERN_ERR "boottime: Failed creating sysfs group\n");
+		return -ENOMEM;
+	}
+
+	boottime_debugfs_init();
+
+	return 0;
+}
+
+late_initcall(boottime_init);
diff --git a/init/main.c b/init/main.c
index d7211faed2a..9fd91c3b3a4 100644
--- a/init/main.c
+++ b/init/main.c
@@ -68,6 +68,7 @@
 #include <linux/shmem_fs.h>
 #include <linux/slab.h>
 #include <linux/perf_event.h>
+#include <linux/boottime.h>
 
 #include <asm/io.h>
 #include <asm/bugs.h>
@@ -668,6 +669,8 @@ int __init_or_module do_one_initcall(initcall_t fn)
 	int count = preempt_count();
 	int ret;
 
+	boottime_mark_symbolic(fn);
+
 	if (initcall_debug)
 		ret = do_one_initcall_debug(fn);
 	else
@@ -743,6 +746,7 @@ static noinline int init_post(void)
 {
 	/* need to finish all async __init code before freeing the memory */
 	async_synchronize_full();
+	boottime_deactivate();
 	free_initmem();
 	mark_rodata_ro();
 	system_state = SYSTEM_RUNNING;
@@ -798,6 +802,7 @@ static int __init kernel_init(void * unused)
 
 	do_pre_smp_initcalls();
 	lockup_detector_init();
+	boottime_system_up();
 
 	smp_init();
 	sched_init_smp();
@@ -820,6 +825,7 @@ static int __init kernel_init(void * unused)
 
 	if (sys_access((const char __user *) ramdisk_execute_command, 0) != 0) {
 		ramdisk_execute_command = NULL;
+		boottime_mark("mount+0x0/0x0");
 		prepare_namespace();
 	}
 
diff --git a/kernel/irq/chip.c b/kernel/irq/chip.c
index d5a3009da71..e5495b907e4 100644
--- a/kernel/irq/chip.c
+++ b/kernel/irq/chip.c
@@ -244,7 +244,7 @@ void handle_nested_irq(unsigned int irq)
 {
 	struct irq_desc *desc = irq_to_desc(irq);
 	struct irqaction *action;
-	irqreturn_t action_ret;
+	irqreturn_t action_ret = IRQ_NONE;
 
 	might_sleep();
 
@@ -259,7 +259,11 @@ void handle_nested_irq(unsigned int irq)
 	irqd_set(&desc->irq_data, IRQD_IRQ_INPROGRESS);
 	raw_spin_unlock_irq(&desc->lock);
 
-	action_ret = action->thread_fn(action->irq, action->dev_id);
+	do {
+		action_ret |= action->thread_fn(action->irq, action->dev_id);
+		action = action->next;
+	} while (action);
+
 	if (!noirqdebug)
 		note_interrupt(irq, desc, action_ret);
 
diff --git a/kernel/irq/pm.c b/kernel/irq/pm.c
index f323a4cd58e..85c13082b0c 100644
--- a/kernel/irq/pm.c
+++ b/kernel/irq/pm.c
@@ -72,9 +72,7 @@ int check_wakeup_irqs(void)
 		if (irqd_is_wakeup_set(&desc->irq_data)) {
 			if (desc->istate & IRQS_PENDING) {
 				pr_info("Wakeup IRQ %d %s pending, suspend aborted\n",
-					irq,
-					desc->action && desc->action->name ?
-					desc->action->name : "");
+					irq, desc->name ? desc->name : "");
 				return -EBUSY;
 			}
 			continue;
diff --git a/kernel/kexec.c b/kernel/kexec.c
index 8d814cbc810..cf5f9ad2b91 100644
--- a/kernel/kexec.c
+++ b/kernel/kexec.c
@@ -50,6 +50,8 @@ u32 vmcoreinfo_note[VMCOREINFO_NOTE_SIZE/4];
 size_t vmcoreinfo_size;
 size_t vmcoreinfo_max_size = sizeof(vmcoreinfo_data);
 
+ATOMIC_NOTIFIER_HEAD(crash_percpu_notifier_list);
+
 /* Location of the reserved area for the crash kernel */
 struct resource crashk_res = {
 	.name  = "Crash kernel",
diff --git a/kernel/power/consoleearlysuspend.c b/kernel/power/consoleearlysuspend.c
index b30f165ef4f..a3edcb26738 100644
--- a/kernel/power/consoleearlysuspend.c
+++ b/kernel/power/consoleearlysuspend.c
@@ -25,41 +25,35 @@
 static int orig_fgconsole;
 static void console_early_suspend(struct early_suspend *h)
 {
-	console_lock();
+	acquire_console_sem();
 	orig_fgconsole = fg_console;
 	if (vc_allocate(EARLY_SUSPEND_CONSOLE))
 		goto err;
 	if (set_console(EARLY_SUSPEND_CONSOLE))
 		goto err;
-	console_unlock();
+	release_console_sem();
 
-	tty_lock();
 	if (vt_waitactive(EARLY_SUSPEND_CONSOLE + 1))
 		pr_warning("console_early_suspend: Can't switch VCs.\n");
-	tty_unlock();
-
 	return;
 err:
 	pr_warning("console_early_suspend: Can't set console\n");
-	console_unlock();
+	release_console_sem();
 }
 
 static void console_late_resume(struct early_suspend *h)
 {
 	int ret;
-	console_lock();
+	acquire_console_sem();
 	ret = set_console(orig_fgconsole);
-	console_unlock();
+	release_console_sem();
 	if (ret) {
 		pr_warning("console_late_resume: Can't set console.\n");
 		return;
 	}
 
-	tty_lock();
 	if (vt_waitactive(orig_fgconsole + 1))
 		pr_warning("console_late_resume: Can't switch VCs.\n");
-	tty_unlock();
-
 }
 
 static struct early_suspend console_early_suspend_desc = {
diff --git a/kernel/power/wakelock.c b/kernel/power/wakelock.c
index 2ee459fe445..c10d0ee7907 100644
--- a/kernel/power/wakelock.c
+++ b/kernel/power/wakelock.c
@@ -249,7 +249,7 @@ long has_wake_lock(int type)
 	unsigned long irqflags;
 	spin_lock_irqsave(&list_lock, irqflags);
 	ret = has_wake_lock_locked(type);
-	if (ret && (debug_mask & DEBUG_WAKEUP) && type == WAKE_LOCK_SUSPEND)
+	if (ret && (debug_mask & DEBUG_SUSPEND) && type == WAKE_LOCK_SUSPEND)
 		print_active_locks(type);
 	spin_unlock_irqrestore(&list_lock, irqflags);
 	return ret;
@@ -311,7 +311,7 @@ static int power_suspend_late(struct device *dev)
 {
 	int ret = has_wake_lock(WAKE_LOCK_SUSPEND) ? -EAGAIN : 0;
 #ifdef CONFIG_WAKELOCK_STAT
-	wait_for_wakeup = !ret;
+	wait_for_wakeup = 1;
 #endif
 	if (debug_mask & DEBUG_SUSPEND)
 		pr_info("power_suspend_late return %d\n", ret);
diff --git a/kernel/printk.c b/kernel/printk.c
index 4835df7fcd5..6bf4e29579c 100644
--- a/kernel/printk.c
+++ b/kernel/printk.c
@@ -41,6 +41,7 @@
 #include <linux/cpu.h>
 #include <linux/notifier.h>
 #include <linux/rculist.h>
+#include <trace/stm.h>
 
 #include <asm/uaccess.h>
 
@@ -948,6 +949,9 @@ asmlinkage int vprintk(const char *fmt, va_list args)
 		}
 	}
 
+	/* Send printk buffer to MIPI STM trace hardware too if enable */
+	stm_dup_printk(printk_buf, printed_len);
+
 	/*
 	 * Copy the output into log_buf. If the caller didn't provide
 	 * the appropriate log prefix, we insert them here
diff --git a/kernel/trace/trace.c b/kernel/trace/trace.c
index ee9c921d7f2..2412d59025e 100644
--- a/kernel/trace/trace.c
+++ b/kernel/trace/trace.c
@@ -37,6 +37,7 @@
 #include <linux/init.h>
 #include <linux/poll.h>
 #include <linux/fs.h>
+#include <trace/stm.h>
 
 #include "trace.h"
 #include "trace_output.h"
@@ -906,7 +907,7 @@ void tracing_reset_current_online_cpus(void)
 	tracing_reset_online_cpus(&global_trace);
 }
 
-#define SAVED_CMDLINES 128
+#define SAVED_CMDLINES 2048
 #define NO_CMDLINE_MAP UINT_MAX
 static unsigned map_pid_to_cmdline[PID_MAX_DEFAULT+1];
 static unsigned map_cmdline_to_pid[SAVED_CMDLINES];
@@ -1222,6 +1223,8 @@ trace_function(struct trace_array *tr,
 
 	if (!filter_check_discard(call, entry, buffer, event))
 		ring_buffer_unlock_commit(buffer, event);
+
+	stm_ftrace(ip, parent_ip);
 }
 
 void
@@ -1256,8 +1259,11 @@ static void __ftrace_trace_stack(struct ring_buffer *buffer,
 	trace.entries		= entry->caller;
 
 	save_stack_trace(&trace);
+
 	if (!filter_check_discard(call, entry, buffer, event))
 		ring_buffer_unlock_commit(buffer, event);
+
+	stm_stack_trace(trace.entries);
 }
 
 void ftrace_trace_stack(struct ring_buffer *buffer, unsigned long flags,
@@ -1413,6 +1419,8 @@ int trace_vbprintk(unsigned long ip, const char *fmt, va_list args)
 		ftrace_trace_stack(buffer, flags, 6, pc);
 	}
 
+	stm_trace_bprintk_buf(ip, fmt, trace_buf, sizeof(u32) * len);
+
 out_unlock:
 	arch_spin_unlock(&trace_buf_lock);
 	local_irq_restore(flags);
@@ -1489,6 +1497,8 @@ int trace_array_vprintk(struct trace_array *tr,
 		ftrace_trace_stack(buffer, irq_flags, 6, pc);
 	}
 
+	stm_trace_printk_buf(ip, trace_buf, len);
+
  out_unlock:
 	arch_spin_unlock(&trace_buf_lock);
 	raw_local_irq_restore(irq_flags);
diff --git a/kernel/trace/trace_events.c b/kernel/trace/trace_events.c
index 3e2a7c91c54..da5c3bd6044 100644
--- a/kernel/trace/trace_events.c
+++ b/kernel/trace/trace_events.c
@@ -19,6 +19,7 @@
 #include <linux/delay.h>
 
 #include <asm/setup.h>
+#include <trace/stm.h>
 
 #include "trace_output.h"
 
@@ -1722,6 +1723,8 @@ function_test_events_call(unsigned long ip, unsigned long parent_ip)
 
 	trace_nowake_buffer_unlock_commit(buffer, event, flags, pc);
 
+	stm_ftrace(ip, parent_ip);
+
  out:
 	atomic_dec(&per_cpu(ftrace_test_event_disable, cpu));
 	preempt_enable_notrace();
diff --git a/kernel/trace/trace_sched_switch.c b/kernel/trace/trace_sched_switch.c
index 7e62c0a1845..a136fd86533 100644
--- a/kernel/trace/trace_sched_switch.c
+++ b/kernel/trace/trace_sched_switch.c
@@ -11,6 +11,7 @@
 #include <linux/uaccess.h>
 #include <linux/ftrace.h>
 #include <trace/events/sched.h>
+#include <trace/stm.h>
 
 #include "trace.h"
 
@@ -47,6 +48,10 @@ tracing_sched_switch_trace(struct trace_array *tr,
 
 	if (!filter_check_discard(call, entry, buffer, event))
 		trace_buffer_unlock_commit(buffer, event, flags, pc);
+
+	stm_sched_switch(entry->prev_pid, entry->prev_prio, entry->prev_state,
+			entry->next_pid, entry->next_prio, entry->next_state,
+			entry->next_cpu);
 }
 
 static void
@@ -103,6 +108,11 @@ tracing_sched_wakeup_trace(struct trace_array *tr,
 
 	if (!filter_check_discard(call, entry, buffer, event))
 		ring_buffer_unlock_commit(buffer, event);
+
+	stm_sched_wakeup(entry->prev_pid, entry->prev_prio, entry->prev_state,
+			entry->next_pid, entry->next_prio, entry->next_state,
+			entry->next_cpu);
+
 	ftrace_trace_stack(tr->buffer, flags, 6, pc);
 	ftrace_trace_userstack(tr->buffer, flags, pc);
 }
diff --git a/net/bluetooth/bnep/bnep.h b/net/bluetooth/bnep/bnep.h
index e7ee5314f39..0545fe0493f 100644
--- a/net/bluetooth/bnep/bnep.h
+++ b/net/bluetooth/bnep/bnep.h
@@ -166,6 +166,8 @@ struct bnep_session {
 
 	struct socket    *sock;
 	struct net_device *dev;
+
+	unsigned int setup_done;
 };
 
 void bnep_net_setup(struct net_device *dev);
diff --git a/net/bluetooth/bnep/core.c b/net/bluetooth/bnep/core.c
index d9edfe8bf9d..b7026f35a9b 100644
--- a/net/bluetooth/bnep/core.c
+++ b/net/bluetooth/bnep/core.c
@@ -124,7 +124,8 @@ static inline void bnep_set_default_proto_filter(struct bnep_session *s)
 }
 #endif
 
-static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len)
+static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data,
+							int len, int *pkt_size)
 {
 	int n;
 
@@ -140,6 +141,8 @@ static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len
 
 	BT_DBG("filter len %d", n);
 
+	*pkt_size = 2 + n;
+
 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
 	n /= 4;
 	if (n <= BNEP_MAX_PROTO_FILTERS) {
@@ -170,7 +173,8 @@ static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len
 	return 0;
 }
 
-static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
+static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len,
+								int *pkt_size)
 {
 	int n;
 
@@ -186,6 +190,8 @@ static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
 
 	BT_DBG("filter len %d", n);
 
+	*pkt_size = 2 + n;
+
 #ifdef CONFIG_BT_BNEP_MC_FILTER
 	n /= (ETH_ALEN * 2);
 
@@ -231,7 +237,8 @@ static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
 	return 0;
 }
 
-static int bnep_rx_control(struct bnep_session *s, void *data, int len)
+static int bnep_rx_control(struct bnep_session *s, void *data, int len,
+								int *pkt_size)
 {
 	u8  cmd = *(u8 *)data;
 	int err = 0;
@@ -239,6 +246,8 @@ static int bnep_rx_control(struct bnep_session *s, void *data, int len)
 	data++;
 	len--;
 
+	*pkt_size = 0;
+
 	switch (cmd) {
 	case BNEP_CMD_NOT_UNDERSTOOD:
 	case BNEP_SETUP_CONN_RSP:
@@ -248,15 +257,27 @@ static int bnep_rx_control(struct bnep_session *s, void *data, int len)
 		break;
 
 	case BNEP_FILTER_NET_TYPE_SET:
-		err = bnep_ctrl_set_netfilter(s, data, len);
+		err = bnep_ctrl_set_netfilter(s, data, len, pkt_size);
 		break;
 
 	case BNEP_FILTER_MULTI_ADDR_SET:
-		err = bnep_ctrl_set_mcfilter(s, data, len);
+		err = bnep_ctrl_set_mcfilter(s, data, len, pkt_size);
 		break;
 
-	case BNEP_SETUP_CONN_REQ:
-		err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, BNEP_CONN_NOT_ALLOWED);
+	case BNEP_SETUP_CONN_REQ: {
+			u8 uuid_size = *(u8 *)data;
+
+			/* First setup connection should be silently discarded,
+			 * it was already handled when accepting connection.
+			 */
+			if (s->setup_done)
+				err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP,
+							BNEP_CONN_NOT_ALLOWED);
+			else
+				s->setup_done = 1;
+
+			*pkt_size = 1 + 2 * uuid_size;
+		}
 		break;
 
 	default: {
@@ -269,6 +290,10 @@ static int bnep_rx_control(struct bnep_session *s, void *data, int len)
 		break;
 	}
 
+	if (*pkt_size > 0)
+		/* Add 1 byte for type field */
+		(*pkt_size)++;
+
 	return err;
 }
 
@@ -276,6 +301,7 @@ static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
 {
 	struct bnep_ext_hdr *h;
 	int err = 0;
+	int pkt_size;
 
 	do {
 		h = (void *) skb->data;
@@ -288,7 +314,7 @@ static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
 
 		switch (h->type & BNEP_TYPE_MASK) {
 		case BNEP_EXT_CONTROL:
-			bnep_rx_control(s, skb->data, skb->len);
+			bnep_rx_control(s, skb->data, skb->len, &pkt_size);
 			break;
 
 		default:
@@ -328,7 +354,16 @@ static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
 		goto badframe;
 
 	if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
-		bnep_rx_control(s, skb->data, skb->len);
+		int pkt_size = 0;
+
+		bnep_rx_control(s, skb->data, skb->len, &pkt_size);
+
+		if (pkt_size > 0 && (type & BNEP_EXT_HEADER)) {
+			skb_pull(skb, pkt_size);
+			if (bnep_rx_extension(s, skb) < 0)
+				goto badframe;
+		}
+
 		kfree_skb(skb);
 		return 0;
 	}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 5a0ce738751..68e9d765311 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -3227,9 +3227,7 @@ static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
 	control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
 	control |= L2CAP_SUPER_RCV_NOT_READY;
 	l2cap_send_sframe(chan, control);
-
 	set_bit(CONN_RNR_SENT, &chan->conn_state);
-
 	__clear_ack_timer(chan);
 }
 
diff --git a/net/caif/cfmuxl.c b/net/caif/cfmuxl.c
index c23979e79df..4884e189ad5 100644
--- a/net/caif/cfmuxl.c
+++ b/net/caif/cfmuxl.c
@@ -247,27 +247,12 @@ static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
 				int phyid)
 {
 	struct cfmuxl *muxl = container_obj(layr);
+	struct list_head *node, *next;
 	struct cflayer *layer;
-	int idx;
-
-	rcu_read_lock();
-	list_for_each_entry_rcu(layer, &muxl->srvl_list, node) {
-
-		if (cfsrvl_phyid_match(layer, phyid) && layer->ctrlcmd) {
-
-			if ((ctrl == _CAIF_CTRLCMD_PHYIF_DOWN_IND ||
-				ctrl == CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND) &&
-					layer->id != 0) {
-
-				idx = layer->id % UP_CACHE_SIZE;
-				spin_lock_bh(&muxl->receive_lock);
-				rcu_assign_pointer(muxl->up_cache[idx], NULL);
-				list_del_rcu(&layer->node);
-				spin_unlock_bh(&muxl->receive_lock);
-			}
-			/* NOTE: ctrlcmd is not allowed to block */
+	list_for_each_safe(node, next, &muxl->srvl_list) {
+		layer = list_entry(node, struct cflayer, node);
+		if (cfsrvl_phyid_match(layer, phyid))
 			layer->ctrlcmd(layer, ctrl, phyid);
 		}
-	}
 	rcu_read_unlock();
 }
diff --git a/net/netfilter/xt_qtaguid.c b/net/netfilter/xt_qtaguid.c
index 5afd4192f35..536918078f7 100644
--- a/net/netfilter/xt_qtaguid.c
+++ b/net/netfilter/xt_qtaguid.c
@@ -8,36 +8,7 @@
  * published by the Free Software Foundation.
  */
 
-/* #define DEBUG */
-/* #define IDEBUG */
-/* #define MDEBUG */
-/* #define RDEBUG */
-/* #define CDEBUG */
-
-/* Iface handling */
-#ifdef IDEBUG
-#define IF_DEBUG(...) pr_debug(__VA_ARGS__)
-#else
-#define IF_DEBUG(...) no_printk(__VA_ARGS__)
-#endif
-/* Iptable Matching */
-#ifdef MDEBUG
-#define MT_DEBUG(...) pr_debug(__VA_ARGS__)
-#else
-#define MT_DEBUG(...) no_printk(__VA_ARGS__)
-#endif
-/* Red-black tree handling */
-#ifdef RDEBUG
-#define RB_DEBUG(...) pr_debug(__VA_ARGS__)
-#else
-#define RB_DEBUG(...) no_printk(__VA_ARGS__)
-#endif
-/* procfs ctrl/stats handling */
-#ifdef CDEBUG
-#define CT_DEBUG(...) pr_debug(__VA_ARGS__)
-#else
-#define CT_DEBUG(...) no_printk(__VA_ARGS__)
-#endif
+/* TODO: support ipv6 for iface_stat */
 
 #include <linux/file.h>
 #include <linux/inetdevice.h>
@@ -46,64 +17,17 @@
 #include <linux/netfilter/xt_qtaguid.h>
 #include <linux/skbuff.h>
 #include <linux/workqueue.h>
-#include <net/addrconf.h>
 #include <net/sock.h>
 #include <net/tcp.h>
 #include <net/udp.h>
 
 #include <linux/netfilter/xt_socket.h>
-/*
- * We only use the xt_socket funcs within a similar context to avoid unexpected
- * return values.
- */
+/* We only use the xt_socket funcs within a similar context to avoid unexpected
+ * return values. */
 #define XT_SOCKET_SUPPORTED_HOOKS \
 	((1 << NF_INET_PRE_ROUTING) | (1 << NF_INET_LOCAL_IN))
 
 
-static const char *module_procdirname = "xt_qtaguid";
-static struct proc_dir_entry *xt_qtaguid_procdir;
-
-static unsigned int proc_iface_perms = S_IRUGO;
-module_param_named(iface_perms, proc_iface_perms, uint, S_IRUGO | S_IWUSR);
-
-static struct proc_dir_entry *xt_qtaguid_stats_file;
-static unsigned int proc_stats_perms = S_IRUGO;
-module_param_named(stats_perms, proc_stats_perms, uint, S_IRUGO | S_IWUSR);
-
-static struct proc_dir_entry *xt_qtaguid_ctrl_file;
-#ifdef CONFIG_ANDROID_PARANOID_NETWORK
-static unsigned int proc_ctrl_perms = S_IRUGO | S_IWUGO;
-#else
-static unsigned int proc_ctrl_perms = S_IRUGO | S_IWUSR;
-#endif
-module_param_named(ctrl_perms, proc_ctrl_perms, uint, S_IRUGO | S_IWUSR);
-
-#ifdef CONFIG_ANDROID_PARANOID_NETWORK
-#include <linux/android_aid.h>
-static gid_t proc_stats_readall_gid = AID_NET_BW_STATS;
-static gid_t proc_ctrl_write_gid = AID_NET_BW_ACCT;
-#else
-/* 0 means, don't limit anybody */
-static gid_t proc_stats_readall_gid;
-static gid_t proc_ctrl_write_gid;
-#endif
-module_param_named(stats_readall_gid, proc_stats_readall_gid, uint,
-		   S_IRUGO | S_IWUSR);
-module_param_named(ctrl_write_gid, proc_ctrl_write_gid, uint,
-		   S_IRUGO | S_IWUSR);
-
-/*
- * After the kernel has initiallized this module, it is still possible
- * to make it passive:
- *  - do not register it via iptables.
- *   the matching code will not be invoked.
- *  - set passive to 0
- *   the iface stats handling will not be act on notifications.
- * This is mostly usefull when a bug is suspected.
- */
-static bool module_passive;
-module_param_named(passive, module_passive, bool, S_IRUGO | S_IWUSR);
-
 /*---------------------------------------------------------------------------*/
 /*
  * Tags:
@@ -138,14 +62,6 @@ typedef uint64_t tag_t;  /* Only used via accessors */
 static const char *iface_stat_procdirname = "iface_stat";
 static struct proc_dir_entry *iface_stat_procdir;
 
-
-/*
- * For now we only track 2 sets of counters.
- * The default set is 0.
- * Userspace can activate another set for a given uid being tracked.
- */
-#define IFS_MAX_COUNTER_SETS 2
-
 enum ifs_tx_rx {
 	IFS_TX,
 	IFS_RX,
@@ -166,25 +82,23 @@ struct byte_packet_counters {
 };
 
 struct data_counters {
-	struct byte_packet_counters bpc[IFS_MAX_COUNTER_SETS][IFS_MAX_DIRECTIONS][IFS_MAX_PROTOS];
+	struct byte_packet_counters bpc[IFS_MAX_DIRECTIONS][IFS_MAX_PROTOS];
 };
 
-/* Generic tag based node used as a base for rb_tree ops. */
-struct tag_node {
+struct tag_stat {
 	struct rb_node node;
 	tag_t tag;
-};
 
-struct tag_stat {
-	struct tag_node tn;
 	struct data_counters counters;
-	/*
-	 * If this tag is acct_tag based, we need to count against the
-	 * matching parent uid_tag.
-	 */
+	/* If this tag is acct_tag based, we need to count against the
+	 * matching parent uid_tag. */
 	struct data_counters *parent_counters;
+	struct proc_dir_entry *proc_ptr;
 };
 
+static LIST_HEAD(iface_stat_list);
+static DEFINE_SPINLOCK(iface_stat_list_lock);
+
 struct iface_stat {
 	struct list_head list;
 	char *ifname;
@@ -199,63 +113,20 @@ struct iface_stat {
 	spinlock_t tag_stat_list_lock;
 };
 
-static LIST_HEAD(iface_stat_list);
-static DEFINE_SPINLOCK(iface_stat_list_lock);
-
-/* This is needed to create proc_dir_entries from atomic context. */
-struct iface_stat_work {
-	struct work_struct iface_work;
-	struct iface_stat *iface_entry;
-};
+static struct rb_root sock_tag_tree = RB_ROOT;
+static DEFINE_SPINLOCK(sock_tag_list_lock);
 
 /*
- * Track tag that this socket is transferring data for, and not necessarily
+ * Track tag that this socket is transferring data for, and not necesseraly
  * the uid that owns the socket.
  * This is the tag against which tag_stat.counters will be billed.
  */
 struct sock_tag {
-	struct rb_node sock_node;
-	struct sock *sk;  /* Only used as a number, never dereferenced */
-	/* The socket is needed for sockfd_put() */
-	struct socket *socket;
-
+	struct rb_node node;
+	struct sock *sk;
 	tag_t tag;
 };
 
-struct qtaguid_event_counts {
-	/* Various successful events */
-	atomic64_t sockets_tagged;
-	atomic64_t sockets_untagged;
-	atomic64_t counter_set_changes;
-	atomic64_t delete_cmds;
-	atomic64_t iface_events;  /* Number of NETDEV_* events handled */
-	/*
-	 * match_found_sk_*: numbers related to the netfilter matching
-	 * function finding a sock for the sk_buff.
-	 */
-	atomic64_t match_found_sk;   /* An sk was already in the sk_buff. */
-	/* The connection tracker had the sk. */
-	atomic64_t match_found_sk_in_ct;
-	/*
-	 * No sk could be found. No apparent owner. Could happen with
-	 * unsolicited traffic.
-	 */
-	atomic64_t match_found_sk_none;
-};
-static struct qtaguid_event_counts qtu_events;
-
-static struct rb_root sock_tag_tree = RB_ROOT;
-static DEFINE_SPINLOCK(sock_tag_list_lock);
-
-/* Track the set active_set for the given tag. */
-struct tag_counter_set {
-	struct tag_node tn;
-	int active_set;
-};
-
-static struct rb_root tag_counter_set_tree = RB_ROOT;
-static DEFINE_SPINLOCK(tag_counter_set_list_lock);
-
 static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par);
 
 /*----------------------------------------------*/
@@ -290,42 +161,32 @@ static inline bool valid_atag(tag_t tag)
 	return !(tag & 0xFFFFFFFFULL);
 }
 
-static inline void dc_add_byte_packets(struct data_counters *counters, int set,
+static inline void dc_add_byte_packets(struct data_counters *counters,
 				  enum ifs_tx_rx direction,
 				  enum ifs_proto ifs_proto,
 				  int bytes,
 				  int packets)
 {
-	counters->bpc[set][direction][ifs_proto].bytes += bytes;
-	counters->bpc[set][direction][ifs_proto].packets += packets;
+	counters->bpc[direction][ifs_proto].bytes += bytes;
+	counters->bpc[direction][ifs_proto].packets += packets;
 }
 
 static inline uint64_t dc_sum_bytes(struct data_counters *counters,
-				    int set,
 				    enum ifs_tx_rx direction)
 {
-	return counters->bpc[set][direction][IFS_TCP].bytes
-		+ counters->bpc[set][direction][IFS_UDP].bytes
-		+ counters->bpc[set][direction][IFS_PROTO_OTHER].bytes;
-}
-
-static inline uint64_t dc_sum_packets(struct data_counters *counters,
-				      int set,
-				      enum ifs_tx_rx direction)
-{
-	return counters->bpc[set][direction][IFS_TCP].packets
-		+ counters->bpc[set][direction][IFS_UDP].packets
-		+ counters->bpc[set][direction][IFS_PROTO_OTHER].packets;
+	return counters->bpc[direction][IFS_TCP].bytes
+		+ counters->bpc[direction][IFS_UDP].bytes
+		+ counters->bpc[direction][IFS_PROTO_OTHER].bytes;
 }
 
-static struct tag_node *tag_node_tree_search(struct rb_root *root, tag_t tag)
+static struct tag_stat *tag_stat_tree_search(struct rb_root *root, tag_t tag)
 {
 	struct rb_node *node = root->rb_node;
 
 	while (node) {
-		struct tag_node *data = rb_entry(node, struct tag_node, node);
+		struct tag_stat *data = rb_entry(node, struct tag_stat, node);
 		int result = tag_compare(tag, data->tag);
-		RB_DEBUG("qtaguid: tag_node_tree_search(): tag=0x%llx"
+		pr_debug("qtaguid: tag_stat_tree_search(): tag=0x%llx"
 			 " (uid=%d)\n",
 			 data->tag,
 			 get_uid_from_tag(data->tag));
@@ -340,16 +201,16 @@ static struct tag_node *tag_node_tree_search(struct rb_root *root, tag_t tag)
 	return NULL;
 }
 
-static void tag_node_tree_insert(struct tag_node *data, struct rb_root *root)
+static void tag_stat_tree_insert(struct tag_stat *data, struct rb_root *root)
 {
 	struct rb_node **new = &(root->rb_node), *parent = NULL;
 
 	/* Figure out where to put new node */
 	while (*new) {
-		struct tag_node *this = rb_entry(*new, struct tag_node,
+		struct tag_stat *this = rb_entry(*new, struct tag_stat,
 						 node);
 		int result = tag_compare(data->tag, this->tag);
-		RB_DEBUG("qtaguid: tag_node_tree_insert(): tag=0x%llx"
+		pr_debug("qtaguid: tag_stat_tree_insert(): tag=0x%llx"
 			 " (uid=%d)\n",
 			 this->tag,
 			 get_uid_from_tag(this->tag));
@@ -367,43 +228,13 @@ static void tag_node_tree_insert(struct tag_node *data, struct rb_root *root)
 	rb_insert_color(&data->node, root);
 }
 
-static void tag_stat_tree_insert(struct tag_stat *data, struct rb_root *root)
-{
-	tag_node_tree_insert(&data->tn, root);
-}
-
-static struct tag_stat *tag_stat_tree_search(struct rb_root *root, tag_t tag)
-{
-	struct tag_node *node = tag_node_tree_search(root, tag);
-	if (!node)
-		return NULL;
-	return rb_entry(&node->node, struct tag_stat, tn.node);
-}
-
-static void tag_counter_set_tree_insert(struct tag_counter_set *data,
-					struct rb_root *root)
-{
-	tag_node_tree_insert(&data->tn, root);
-}
-
-static struct tag_counter_set *tag_counter_set_tree_search(struct rb_root *root,
-							   tag_t tag)
-{
-	struct tag_node *node = tag_node_tree_search(root, tag);
-	if (!node)
-		return NULL;
-	return rb_entry(&node->node, struct tag_counter_set, tn.node);
-
-}
-
 static struct sock_tag *sock_tag_tree_search(struct rb_root *root,
 					     const struct sock *sk)
 {
 	struct rb_node *node = root->rb_node;
 
 	while (node) {
-		struct sock_tag *data = rb_entry(node, struct sock_tag,
-						 sock_node);
+		struct sock_tag *data = rb_entry(node, struct sock_tag, node);
 		ptrdiff_t result = sk - data->sk;
 		if (result < 0)
 			node = node->rb_left;
@@ -421,8 +252,7 @@ static void sock_tag_tree_insert(struct sock_tag *data, struct rb_root *root)
 
 	/* Figure out where to put new node */
 	while (*new) {
-		struct sock_tag *this = rb_entry(*new, struct sock_tag,
-						 sock_node);
+		struct sock_tag *this = rb_entry(*new, struct sock_tag, node);
 		ptrdiff_t result = data->sk - this->sk;
 		parent = *new;
 		if (result < 0)
@@ -434,8 +264,8 @@ static void sock_tag_tree_insert(struct sock_tag *data, struct rb_root *root)
 	}
 
 	/* Add new node and rebalance tree. */
-	rb_link_node(&data->sock_node, parent, new);
-	rb_insert_color(&data->sock_node, root);
+	rb_link_node(&data->node, parent, new);
+	rb_insert_color(&data->node, root);
 }
 
 static int read_proc_u64(char *page, char **start, off_t off,
@@ -445,7 +275,6 @@ static int read_proc_u64(char *page, char **start, off_t off,
 	uint64_t value;
 	char *p = page;
 	uint64_t *iface_entry = data;
-
 	if (!data)
 		return 0;
 
@@ -464,7 +293,6 @@ static int read_proc_bool(char *page, char **start, off_t off,
 	bool value;
 	char *p = page;
 	bool *bool_entry = data;
-
 	if (!data)
 		return 0;
 
@@ -476,307 +304,170 @@ static int read_proc_bool(char *page, char **start, off_t off,
 	return len;
 }
 
-static int get_active_counter_set(tag_t tag)
-{
-	int active_set = 0;
-	struct tag_counter_set *tcs;
-
-	MT_DEBUG("qtaguid: get_active_counter_set(tag=0x%llx)"
-		 " (uid=%u)\n",
-		 tag, get_uid_from_tag(tag));
-	/* For now we only handle UID tags for active sets */
-	tag = get_utag_from_tag(tag);
-	spin_lock_bh(&tag_counter_set_list_lock);
-	tcs = tag_counter_set_tree_search(&tag_counter_set_tree, tag);
-	if (tcs)
-		active_set = tcs->active_set;
-	spin_unlock_bh(&tag_counter_set_list_lock);
-	return active_set;
-}
-
-/*
- * Find the entry for tracking the specified interface.
- * Caller must hold iface_stat_list_lock
- */
-static struct iface_stat *get_iface_entry(const char *ifname)
+/* Find the entry for tracking the specified interface. */
+static struct iface_stat *get_iface_stat(const char *ifname)
 {
+	unsigned long flags;
 	struct iface_stat *iface_entry;
-
-	/* Find the entry for tracking the specified tag within the interface */
-	if (ifname == NULL) {
-		pr_info("qtaguid: iface_stat: get() NULL device name\n");
+	if (!ifname)
 		return NULL;
-	}
 
-	/* Iterate over interfaces */
+	spin_lock_irqsave(&iface_stat_list_lock, flags);
 	list_for_each_entry(iface_entry, &iface_stat_list, list) {
-		if (!strcmp(ifname, iface_entry->ifname))
+		if (!strcmp(iface_entry->ifname, ifname))
 			goto done;
 	}
 	iface_entry = NULL;
 done:
+	spin_unlock_irqrestore(&iface_stat_list_lock, flags);
 	return iface_entry;
 }
 
-static void iface_create_proc_worker(struct work_struct *work)
-{
-	struct proc_dir_entry *proc_entry;
-	struct iface_stat_work *isw = container_of(work, struct iface_stat_work,
-						   iface_work);
-	struct iface_stat *new_iface  = isw->iface_entry;
-
-	/* iface_entries are not deleted, so safe to manipulate. */
-	proc_entry = proc_mkdir(new_iface->ifname, iface_stat_procdir);
-	if (IS_ERR_OR_NULL(proc_entry)) {
-		pr_err("qtaguid: iface_stat: create_proc(): alloc failed.\n");
-		kfree(isw);
-		return;
-	}
-
-	new_iface->proc_ptr = proc_entry;
-
-	create_proc_read_entry("tx_bytes", proc_iface_perms, proc_entry,
-			read_proc_u64, &new_iface->tx_bytes);
-	create_proc_read_entry("rx_bytes", proc_iface_perms, proc_entry,
-			read_proc_u64, &new_iface->rx_bytes);
-	create_proc_read_entry("tx_packets", proc_iface_perms, proc_entry,
-			read_proc_u64, &new_iface->tx_packets);
-	create_proc_read_entry("rx_packets", proc_iface_perms, proc_entry,
-			read_proc_u64, &new_iface->rx_packets);
-	create_proc_read_entry("active", proc_iface_perms, proc_entry,
-			read_proc_bool, &new_iface->active);
-
-	IF_DEBUG("qtaguid: iface_stat: create_proc(): done "
-		 "entry=%p dev=%s\n", new_iface, new_iface->ifname);
-	kfree(isw);
-}
-
-/* Caller must hold iface_stat_list_lock */
-static struct iface_stat *iface_alloc(const char *ifname)
-{
-	struct iface_stat *new_iface;
-	struct iface_stat_work *isw;
-
-	new_iface = kzalloc(sizeof(*new_iface), GFP_ATOMIC);
-	if (new_iface == NULL) {
-		pr_err("qtaguid: iface_stat: create(%s): "
-		       "iface_stat alloc failed\n", ifname);
-		return NULL;
-	}
-	new_iface->ifname = kstrdup(ifname, GFP_ATOMIC);
-	if (new_iface->ifname == NULL) {
-		pr_err("qtaguid: iface_stat: create(%s): "
-		       "ifname alloc failed\n", ifname);
-		kfree(new_iface);
-		return NULL;
-	}
-	spin_lock_init(&new_iface->tag_stat_list_lock);
-	new_iface->active = true;
-	new_iface->tag_stat_tree = RB_ROOT;
-
-	/*
-	 * ipv6 notifier chains are atomic :(
-	 * No create_proc_read_entry() for you!
-	 */
-	isw = kmalloc(sizeof(*isw), GFP_ATOMIC);
-	if (!isw) {
-		pr_err("qtaguid: iface_stat: create(%s): "
-		       "work alloc failed\n", new_iface->ifname);
-		kfree(new_iface->ifname);
-		kfree(new_iface);
-		return NULL;
-	}
-	isw->iface_entry = new_iface;
-	INIT_WORK(&isw->iface_work, iface_create_proc_worker);
-	schedule_work(&isw->iface_work);
-	list_add(&new_iface->list, &iface_stat_list);
-	return new_iface;
-}
-
 /*
  * Create a new entry for tracking the specified interface.
  * Do nothing if the entry already exists.
  * Called when an interface is configured with a valid IP address.
  */
-void iface_stat_create(const struct net_device *net_dev,
-		       struct in_ifaddr *ifa)
+void iface_stat_create(const struct net_device *net_dev)
 {
-	struct in_device *in_dev = NULL;
+	struct in_device *in_dev;
+	unsigned long flags;
+	struct iface_stat *new_iface;
+	struct proc_dir_entry *proc_entry;
 	const char *ifname;
 	struct iface_stat *entry;
 	__be32 ipaddr = 0;
-	struct iface_stat *new_iface;
+	struct in_ifaddr *ifa = NULL;
+
+	ASSERT_RTNL(); /* No need for separate locking */
 
-	IF_DEBUG("qtaguid: iface_stat: create(%s): ifa=%p netdev=%p\n",
-		 net_dev ? net_dev->name : "?",
-		 ifa, net_dev);
+	pr_debug("iface_stat: create(): netdev=%p->name=%s\n",
+		 net_dev, net_dev ? net_dev->name : "");
 	if (!net_dev) {
-		pr_err("qtaguid: iface_stat: create(): no net dev\n");
+		pr_err("iface_stat: create(): no net dev!\n");
 		return;
 	}
 
-	ifname = net_dev->name;
-	if (!ifa) {
-		in_dev = in_dev_get(net_dev);
-		if (!in_dev) {
-			pr_err("qtaguid: iface_stat: create(%s): no inet dev\n",
-			       ifname);
-			return;
-		}
-		IF_DEBUG("qtaguid: iface_stat: create(%s): in_dev=%p\n",
-			 ifname, in_dev);
-		for (ifa = in_dev->ifa_list; ifa; ifa = ifa->ifa_next) {
-			IF_DEBUG("qtaguid: iface_stat: create(%s): "
-				 "ifa=%p ifa_label=%s\n",
-				 ifname, ifa,
-				 ifa->ifa_label ? ifa->ifa_label : "(null)");
-			if (ifa->ifa_label && !strcmp(ifname, ifa->ifa_label))
-				break;
-		}
+	in_dev = __in_dev_get_rtnl(net_dev);
+	if (!in_dev) {
+		pr_err("iface_stat: create(): no inet dev!\n");
+		return;
 	}
 
-	if (!ifa) {
-		IF_DEBUG("qtaguid: iface_stat: create(%s): no matching IP\n",
-			 ifname);
-		goto done_put;
+	pr_debug("iface_stat: create(): in_dev=%p\n", in_dev);
+	ifname = net_dev->name;
+	pr_debug("iface_stat: create(): ifname=%p\n", ifname);
+	for (ifa = in_dev->ifa_list; ifa; ifa = ifa->ifa_next) {
+		pr_debug("iface_stat: create(): for(): ifa=%p ifname=%p\n",
+			 ifa, ifname);
+		pr_debug("iface_stat: create(): ifname=%s ifa_label=%s\n",
+			 ifname, ifa->ifa_label ? ifa->ifa_label : "(null)");
+		if (ifa->ifa_label && !strcmp(ifname, ifa->ifa_label))
+			break;
+	}
+
+	if (ifa) {
+		ipaddr = ifa->ifa_local;
+	} else {
+		pr_err("iface_stat: create(): dev %s has no matching IP\n",
+		       ifname);
+		return;
 	}
-	ipaddr = ifa->ifa_local;
 
-	spin_lock_bh(&iface_stat_list_lock);
-	entry = get_iface_entry(ifname);
+	entry = get_iface_stat(net_dev->name);
 	if (entry != NULL) {
-		IF_DEBUG("qtaguid: iface_stat: create(%s): entry=%p\n",
-			 ifname, entry);
+		pr_debug("iface_stat: create(): dev %s entry=%p\n", ifname,
+			 entry);
 		if (ipv4_is_loopback(ipaddr)) {
 			entry->active = false;
-			IF_DEBUG("qtaguid: iface_stat: create(%s): "
-				 "disable tracking of loopback dev\n",
-				 ifname);
+			pr_debug("iface_stat: create(): disable tracking of "
+				 "loopback dev %s\n", ifname);
 		} else {
 			entry->active = true;
-			IF_DEBUG("qtaguid: iface_stat: create(%s): "
-				 "enable tracking. ip=%pI4\n",
+			pr_debug("iface_stat: create(): enable tracking of "
+				 "dev %s with ip=%pI4\n",
 				 ifname, &ipaddr);
 		}
-		goto done_unlock_put;
+		return;
 	} else if (ipv4_is_loopback(ipaddr)) {
-		IF_DEBUG("qtaguid: iface_stat: create(%s): "
-			 "ignore loopback dev. ip=%pI4\n", ifname, &ipaddr);
-		goto done_unlock_put;
+		pr_debug("iface_stat: create(): ignore loopback dev %s"
+			 " ip=%pI4\n", ifname, &ipaddr);
+		return;
 	}
 
-	new_iface = iface_alloc(ifname);
-	IF_DEBUG("qtaguid: iface_stat: create(%s): done "
-		 "entry=%p ip=%pI4\n", ifname, new_iface, &ipaddr);
-
-done_unlock_put:
-	spin_unlock_bh(&iface_stat_list_lock);
-done_put:
-	if (in_dev)
-		in_dev_put(in_dev);
-}
-
-void iface_stat_create_ipv6(const struct net_device *net_dev,
-			    struct inet6_ifaddr *ifa)
-{
-	struct in_device *in_dev;
-	const char *ifname;
-	struct iface_stat *entry;
-	struct iface_stat *new_iface;
-	int addr_type;
-
-	IF_DEBUG("qtaguid: iface_stat: create6(): ifa=%p netdev=%p->name=%s\n",
-		 ifa, net_dev, net_dev ? net_dev->name : "");
-	if (!net_dev) {
-		pr_err("qtaguid: iface_stat: create6(): no net dev!\n");
+	new_iface = kmalloc(sizeof(*new_iface), GFP_KERNEL);
+	if (new_iface == NULL) {
+		pr_err("iface_stat: create(): failed to alloc iface_stat\n");
 		return;
 	}
-	ifname = net_dev->name;
-
-	in_dev = in_dev_get(net_dev);
-	if (!in_dev) {
-		pr_err("qtaguid: iface_stat: create6(%s): no inet dev\n",
-		       ifname);
+	memset(new_iface, 0, sizeof(*new_iface));
+	new_iface->ifname = kstrdup(ifname, GFP_KERNEL);
+	if (new_iface->ifname == NULL) {
+		pr_err("iface_stat: create(): failed to alloc ifname\n");
+		kfree(new_iface);
 		return;
 	}
+	spin_lock_init(&new_iface->tag_stat_list_lock);
 
-	IF_DEBUG("qtaguid: iface_stat: create6(%s): in_dev=%p\n",
-		 ifname, in_dev);
+	new_iface->active = true;
 
-	if (!ifa) {
-		IF_DEBUG("qtaguid: iface_stat: create6(%s): no matching IP\n",
-			 ifname);
-		goto done_put;
-	}
-	addr_type = ipv6_addr_type(&ifa->addr);
+	new_iface->tag_stat_tree = RB_ROOT;
+	spin_lock_irqsave(&iface_stat_list_lock, flags);
+	list_add(&new_iface->list, &iface_stat_list);
+	spin_unlock_irqrestore(&iface_stat_list_lock, flags);
 
-	spin_lock_bh(&iface_stat_list_lock);
-	entry = get_iface_entry(ifname);
-	if (entry != NULL) {
-		IF_DEBUG("qtaguid: iface_stat: create6(%s): entry=%p\n",
-			 ifname, entry);
-		if (addr_type & IPV6_ADDR_LOOPBACK) {
-			entry->active = false;
-			IF_DEBUG("qtaguid: iface_stat: create6(%s): "
-				 "disable tracking of loopback dev\n",
-				 ifname);
-		} else {
-			entry->active = true;
-			IF_DEBUG("qtaguid: iface_stat: create6(%s): "
-				 "enable tracking. ip=%pI6c\n",
-				 ifname, &ifa->addr);
-		}
-		goto done_unlock_put;
-	} else if (addr_type & IPV6_ADDR_LOOPBACK) {
-		IF_DEBUG("qtaguid: iface_stat: create6(%s): "
-			 "ignore loopback dev. ip=%pI6c\n",
-			 ifname, &ifa->addr);
-		goto done_unlock_put;
-	}
+	proc_entry = proc_mkdir(ifname, iface_stat_procdir);
+	new_iface->proc_ptr = proc_entry;
 
-	new_iface = iface_alloc(ifname);
-	IF_DEBUG("qtaguid: iface_stat: create6(%s): done "
-		 "entry=%p ip=%pI6c\n", ifname, new_iface, &ifa->addr);
+	/* TODO: make root access only */
+	create_proc_read_entry("tx_bytes", S_IRUGO, proc_entry,
+			read_proc_u64, &new_iface->tx_bytes);
+	create_proc_read_entry("rx_bytes", S_IRUGO, proc_entry,
+			read_proc_u64, &new_iface->rx_bytes);
+	create_proc_read_entry("tx_packets", S_IRUGO, proc_entry,
+			read_proc_u64, &new_iface->tx_packets);
+	create_proc_read_entry("rx_packets", S_IRUGO, proc_entry,
+			read_proc_u64, &new_iface->rx_packets);
+	create_proc_read_entry("active", S_IRUGO, proc_entry,
+			read_proc_bool, &new_iface->active);
 
-done_unlock_put:
-	spin_unlock_bh(&iface_stat_list_lock);
-done_put:
-	in_dev_put(in_dev);
+	pr_debug("iface_stat: create(): done entry=%p dev=%s ip=%pI4\n",
+		 new_iface, ifname, &ipaddr);
 }
 
 static struct sock_tag *get_sock_stat_nl(const struct sock *sk)
 {
-	MT_DEBUG("qtaguid: get_sock_stat_nl(sk=%p)\n", sk);
+	pr_debug("xt_qtaguid: get_sock_stat_nl(sk=%p)\n", sk);
 	return sock_tag_tree_search(&sock_tag_tree, sk);
 }
 
 static struct sock_tag *get_sock_stat(const struct sock *sk)
 {
+	unsigned long flags;
 	struct sock_tag *sock_tag_entry;
-	MT_DEBUG("qtaguid: get_sock_stat(sk=%p)\n", sk);
+	pr_debug("xt_qtaguid: get_sock_stat(sk=%p)\n", sk);
 	if (!sk)
 		return NULL;
-	spin_lock_bh(&sock_tag_list_lock);
+	spin_lock_irqsave(&sock_tag_list_lock, flags);
 	sock_tag_entry = get_sock_stat_nl(sk);
-	spin_unlock_bh(&sock_tag_list_lock);
+	spin_unlock_irqrestore(&sock_tag_list_lock, flags);
 	return sock_tag_entry;
 }
 
 static void
-data_counters_update(struct data_counters *dc, int set,
-		     enum ifs_tx_rx direction, int proto, int bytes)
+data_counters_update(struct data_counters *dc,  enum ifs_tx_rx direction,
+		int proto, int bytes)
 {
 	switch (proto) {
 	case IPPROTO_TCP:
-		dc_add_byte_packets(dc, set, direction, IFS_TCP, bytes, 1);
+		dc_add_byte_packets(dc, direction, IFS_TCP, bytes, 1);
 		break;
 	case IPPROTO_UDP:
-		dc_add_byte_packets(dc, set, direction, IFS_UDP, bytes, 1);
+		dc_add_byte_packets(dc, direction, IFS_UDP, bytes, 1);
 		break;
 	case IPPROTO_IP:
 	default:
-		dc_add_byte_packets(dc, set, direction, IFS_PROTO_OTHER, bytes,
-				    1);
+		dc_add_byte_packets(dc, direction, IFS_PROTO_OTHER, bytes, 1);
 		break;
 	}
 }
@@ -786,74 +477,93 @@ data_counters_update(struct data_counters *dc, int set,
  * does not exist (when a device was never configured with an IP address).
  * Called when an device is being unregistered.
  */
-static void iface_stat_update(struct net_device *dev)
+void iface_stat_update(struct net_device *dev)
 {
 	struct rtnl_link_stats64 dev_stats, *stats;
 	struct iface_stat *entry;
-
 	stats = dev_get_stats(dev, &dev_stats);
-	spin_lock_bh(&iface_stat_list_lock);
-	entry = get_iface_entry(dev->name);
+	ASSERT_RTNL();
+
+	entry = get_iface_stat(dev->name);
 	if (entry == NULL) {
-		IF_DEBUG("qtaguid: iface_stat_update: dev=%s not tracked\n",
-			 dev->name);
-		spin_unlock_bh(&iface_stat_list_lock);
+		pr_debug("iface_stat: dev %s monitor not found\n", dev->name);
 		return;
 	}
-	IF_DEBUG("qtaguid: iface_stat_update: dev=%s entry=%p\n",
-		 dev->name, entry);
 	if (entry->active) {
 		entry->tx_bytes += stats->tx_bytes;
 		entry->tx_packets += stats->tx_packets;
 		entry->rx_bytes += stats->rx_bytes;
 		entry->rx_packets += stats->rx_packets;
 		entry->active = false;
+		pr_debug("iface_stat: Updating stats for "
+			"dev %s which went down\n", dev->name);
 	} else {
-		IF_DEBUG("qtaguid: iface_stat_update: dev=%s inactive\n",
-			dev->name);
+		pr_debug("iface_stat: Did not update stats for "
+			"dev %s which went down\n", dev->name);
 	}
-	spin_unlock_bh(&iface_stat_list_lock);
 }
 
+
 static void tag_stat_update(struct tag_stat *tag_entry,
 			enum ifs_tx_rx direction, int proto, int bytes)
 {
-	int active_set;
-	active_set = get_active_counter_set(tag_entry->tn.tag);
-	MT_DEBUG("qtaguid: tag_stat_update(tag=0x%llx (uid=%u) set=%d "
-		 "dir=%d proto=%d bytes=%d)\n",
-		 tag_entry->tn.tag, get_uid_from_tag(tag_entry->tn.tag),
-		 active_set, direction, proto, bytes);
-	data_counters_update(&tag_entry->counters, active_set, direction,
-			     proto, bytes);
+	pr_debug("xt_qtaguid: tag_stat_update(tag=0x%llx (uid=%d) dir=%d "
+		"proto=%d bytes=%d)\n",
+		tag_entry->tag, get_uid_from_tag(tag_entry->tag), direction,
+		proto, bytes);
+	data_counters_update(&tag_entry->counters, direction, proto, bytes);
 	if (tag_entry->parent_counters)
-		data_counters_update(tag_entry->parent_counters, active_set,
-				     direction, proto, bytes);
+		data_counters_update(tag_entry->parent_counters, direction,
+				proto, bytes);
 }
 
-/*
- * Create a new entry for tracking the specified {acct_tag,uid_tag} within
+
+/* Create a new entry for tracking the specified {acct_tag,uid_tag} within
  * the interface.
- * iface_entry->tag_stat_list_lock should be held.
- */
+ * iface_entry->tag_stat_list_lock should be held. */
 static struct tag_stat *create_if_tag_stat(struct iface_stat *iface_entry,
 					   tag_t tag)
 {
 	struct tag_stat *new_tag_stat_entry = NULL;
-	IF_DEBUG("qtaguid: iface_stat: create_if_tag_stat(): ife=%p tag=0x%llx"
-		 " (uid=%u)\n",
+	pr_debug("iface_stat: create_if_tag_stat(): ife=%p tag=0x%llx"
+		 " (uid=%d)\n",
 		 iface_entry, tag, get_uid_from_tag(tag));
-	new_tag_stat_entry = kzalloc(sizeof(*new_tag_stat_entry), GFP_ATOMIC);
+	new_tag_stat_entry = kmalloc(sizeof(*new_tag_stat_entry), GFP_ATOMIC);
 	if (!new_tag_stat_entry) {
-		pr_err("qtaguid: iface_stat: tag stat alloc failed\n");
+		pr_err("iface_stat: failed to alloc new tag entry\n");
 		goto done;
 	}
-	new_tag_stat_entry->tn.tag = tag;
+	memset(new_tag_stat_entry, 0, sizeof(*new_tag_stat_entry));
+	new_tag_stat_entry->tag = tag;
 	tag_stat_tree_insert(new_tag_stat_entry, &iface_entry->tag_stat_tree);
 done:
 	return new_tag_stat_entry;
 }
 
+static struct iface_stat *get_iface_entry(const char *ifname)
+{
+	struct iface_stat *iface_entry;
+	unsigned long flags;
+
+	/* Find the entry for tracking the specified tag within the interface */
+	if (ifname == NULL) {
+		pr_info("iface_stat: NULL device name\n");
+		return NULL;
+	}
+
+
+	/* Iterate over interfaces */
+	spin_lock_irqsave(&iface_stat_list_lock, flags);
+	list_for_each_entry(iface_entry, &iface_stat_list, list) {
+		if (!strcmp(ifname, iface_entry->ifname))
+			goto done;
+	}
+	iface_entry = NULL;
+done:
+	spin_unlock_irqrestore(&iface_stat_list_lock, flags);
+	return iface_entry;
+}
+
 static void if_tag_stat_update(const char *ifname, uid_t uid,
 			       const struct sock *sk, enum ifs_tx_rx direction,
 			       int proto, int bytes)
@@ -864,27 +574,25 @@ static void if_tag_stat_update(const char *ifname, uid_t uid,
 	struct data_counters *uid_tag_counters;
 	struct sock_tag *sock_tag_entry;
 	struct iface_stat *iface_entry;
+	unsigned long flags;
 	struct tag_stat *new_tag_stat;
-	MT_DEBUG("qtaguid: if_tag_stat_update(ifname=%s "
-		"uid=%u sk=%p dir=%d proto=%d bytes=%d)\n",
+	pr_debug("xt_qtaguid: if_tag_stat_update(ifname=%s "
+		"uid=%d sk=%p dir=%d proto=%d bytes=%d)\n",
 		 ifname, uid, sk, direction, proto, bytes);
 
 
 	iface_entry = get_iface_entry(ifname);
 	if (!iface_entry) {
-		pr_err("qtaguid: iface_stat: stat_update() %s not found\n",
-		       ifname);
+		pr_err("iface_stat: interface %s not found\n", ifname);
 		return;
 	}
-	/* It is ok to process data when an iface_entry is inactive */
+	/* else { If the iface_entry becomes inactive, it is still ok
+	 * to process the data. } */
 
-	MT_DEBUG("qtaguid: iface_stat: stat_update() dev=%s entry=%p\n",
-		 ifname, iface_entry);
+	pr_debug("iface_stat: stat_update() got entry=%p\n", iface_entry);
 
-	/*
-	 * Look for a tagged sock.
-	 * It will have an acct_uid.
-	 */
+	/* Look for a tagged sock.
+	 * It will have an acct_uid. */
 	sock_tag_entry = get_sock_stat(sk);
 	if (sock_tag_entry) {
 		tag = sock_tag_entry->tag;
@@ -895,21 +603,19 @@ static void if_tag_stat_update(const char *ifname, uid_t uid,
 		acct_tag = 0;
 		tag = combine_atag_with_uid(acct_tag, uid);
 	}
-	MT_DEBUG("qtaguid: iface_stat: stat_update(): "
-		 " looking for tag=0x%llx (uid=%u) in ife=%p\n",
+	pr_debug("iface_stat: stat_update(): looking for tag=0x%llx (uid=%d)"
+		 " in ife=%p\n",
 		 tag, get_uid_from_tag(tag), iface_entry);
 	/* Loop over tag list under this interface for {acct_tag,uid_tag} */
-	spin_lock_bh(&iface_entry->tag_stat_list_lock);
+	spin_lock_irqsave(&iface_entry->tag_stat_list_lock, flags);
 
 	tag_stat_entry = tag_stat_tree_search(&iface_entry->tag_stat_tree,
 					      tag);
 	if (tag_stat_entry) {
-		/*
-		 * Updating the {acct_tag, uid_tag} entry handles both stats:
-		 * {0, uid_tag} will also get updated.
-		 */
+		/* Updating the {acct_tag, uid_tag} entry handles both stats:
+		 * {0, uid_tag} will also get updated. */
 		tag_stat_update(tag_stat_entry, direction, proto, bytes);
-		spin_unlock_bh(&iface_entry->tag_stat_list_lock);
+		spin_unlock_irqrestore(&iface_entry->tag_stat_list_lock, flags);
 		return;
 	}
 
@@ -932,7 +638,7 @@ static void if_tag_stat_update(const char *ifname, uid_t uid,
 		new_tag_stat = create_if_tag_stat(iface_entry, tag);
 		new_tag_stat->parent_counters = uid_tag_counters;
 	}
-	spin_unlock_bh(&iface_entry->tag_stat_list_lock);
+	spin_unlock_irqrestore(&iface_entry->tag_stat_list_lock, flags);
 	tag_stat_update(new_tag_stat, direction, proto, bytes);
 }
 
@@ -940,16 +646,18 @@ static int iface_netdev_event_handler(struct notifier_block *nb,
 				      unsigned long event, void *ptr) {
 	struct net_device *dev = ptr;
 
-	if (unlikely(module_passive))
-		return NOTIFY_DONE;
-
-	IF_DEBUG("qtaguid: iface_stat: netdev_event(): "
-		 "ev=0x%lx netdev=%p->name=%s\n",
+	pr_debug("iface_stat: netdev_event(): ev=0x%lx netdev=%p->name=%s\n",
 		 event, dev, dev ? dev->name : "");
 
 	switch (event) {
 	case NETDEV_UP:
-		iface_stat_create(dev, NULL);
+	case NETDEV_REBOOT:
+	case NETDEV_CHANGE:
+	case NETDEV_REGISTER:  /* Most likely no IP */
+	case NETDEV_CHANGEADDR:  /* MAC addr change */
+	case NETDEV_CHANGENAME:
+	case NETDEV_FEAT_CHANGE:  /* Might be usefull when cell type changes */
+		iface_stat_create(dev);
 		break;
 	case NETDEV_UNREGISTER:
 		iface_stat_update(dev);
@@ -958,49 +666,19 @@ static int iface_netdev_event_handler(struct notifier_block *nb,
 	return NOTIFY_DONE;
 }
 
-static int iface_inet6addr_event_handler(struct notifier_block *nb,
-					 unsigned long event, void *ptr)
-{
-	struct inet6_ifaddr *ifa = ptr;
-	struct net_device *dev;
-
-	if (unlikely(module_passive))
-		return NOTIFY_DONE;
-
-	IF_DEBUG("qtaguid: iface_stat: inet6addr_event(): "
-		 "ev=0x%lx ifa=%p\n",
-		 event, ifa);
-
-	switch (event) {
-	case NETDEV_UP:
-		BUG_ON(!ifa || !ifa->idev);
-		dev = (struct net_device *)ifa->idev->dev;
-		iface_stat_create_ipv6(dev, ifa);
-		atomic64_inc(&qtu_events.iface_events);
-		break;
-	}
-	return NOTIFY_DONE;
-}
-
 static int iface_inetaddr_event_handler(struct notifier_block *nb,
-					unsigned long event, void *ptr)
-{
-	struct in_ifaddr *ifa = ptr;
-	struct net_device *dev;
+					unsigned long event, void *ptr) {
 
-	if (unlikely(module_passive))
-		return NOTIFY_DONE;
+	struct in_ifaddr *ifa = ptr;
+	struct in_device *in_dev = ifa->ifa_dev;
+	struct net_device *dev = in_dev->dev;
 
-	IF_DEBUG("qtaguid: iface_stat: inetaddr_event(): "
-		 "ev=0x%lx ifa=%p\n",
-		 event, ifa);
+	pr_debug("iface_stat: inetaddr_event(): ev=0x%lx netdev=%p->name=%s\n",
+		 event, dev, dev ? dev->name : "");
 
 	switch (event) {
 	case NETDEV_UP:
-		BUG_ON(!ifa || !ifa->ifa_dev);
-		dev = ifa->ifa_dev->dev;
-		iface_stat_create(dev, ifa);
-		atomic64_inc(&qtu_events.iface_events);
+		iface_stat_create(dev);
 		break;
 	}
 	return NOTIFY_DONE;
@@ -1014,43 +692,28 @@ static struct notifier_block iface_inetaddr_notifier_blk = {
 	.notifier_call = iface_inetaddr_event_handler,
 };
 
-static struct notifier_block iface_inet6addr_notifier_blk = {
-	.notifier_call = iface_inet6addr_event_handler,
-};
-
 static int __init iface_stat_init(struct proc_dir_entry *parent_procdir)
 {
 	int err;
 
 	iface_stat_procdir = proc_mkdir(iface_stat_procdirname, parent_procdir);
 	if (!iface_stat_procdir) {
-		pr_err("qtaguid: iface_stat: init failed to create proc entry\n");
+		pr_err("iface_stat: failed to create proc entry\n");
 		err = -1;
 		goto err;
 	}
 	err = register_netdevice_notifier(&iface_netdev_notifier_blk);
 	if (err) {
-		pr_err("qtaguid: iface_stat: init "
-		       "failed to register dev event handler\n");
-		goto err_zap_entry;
-	}
-	err = register_inetaddr_notifier(&iface_inetaddr_notifier_blk);
-	if (err) {
-		pr_err("qtaguid: iface_stat: init "
-		       "failed to register ipv4 dev event handler\n");
+		pr_err("iface_stat: failed to register dev event handler\n");
 		goto err_unreg_nd;
 	}
-
-	err = register_inet6addr_notifier(&iface_inet6addr_notifier_blk);
+	err = register_inetaddr_notifier(&iface_inetaddr_notifier_blk);
 	if (err) {
-		pr_err("qtaguid: iface_stat: init "
-		       "failed to register ipv6 dev event handler\n");
-		goto err_unreg_ip4_addr;
+		pr_err("iface_stat: failed to register dev event handler\n");
+		goto err_zap_entry;
 	}
 	return 0;
 
-err_unreg_ip4_addr:
-	unregister_inetaddr_notifier(&iface_inetaddr_notifier_blk);
 err_unreg_nd:
 	unregister_netdevice_notifier(&iface_netdev_notifier_blk);
 err_zap_entry:
@@ -1065,13 +728,11 @@ static struct sock *qtaguid_find_sk(const struct sk_buff *skb,
 	struct sock *sk;
 	unsigned int hook_mask = (1 << par->hooknum);
 
-	MT_DEBUG("qtaguid: find_sk(skb=%p) hooknum=%d family=%d\n", skb,
+	pr_debug("xt_qtaguid: find_sk(skb=%p) hooknum=%d family=%d\n", skb,
 		 par->hooknum, par->family);
 
-	/*
-	 * Let's not abuse the the xt_socket_get*_sk(), or else it will
-	 * return garbage SKs.
-	 */
+	/* Let's not abuse the the xt_socket_get*_sk(), or else it will
+	 * return garbage SKs. */
 	if (!(hook_mask & XT_SOCKET_SUPPORTED_HOOKS))
 		return NULL;
 
@@ -1086,13 +747,12 @@ static struct sock *qtaguid_find_sk(const struct sk_buff *skb,
 		return NULL;
 	}
 
-	/*
-	 * Seems to be issues on the file ptr for TCP_TIME_WAIT SKs.
+	/* Seems to be issues on the file ptr for TCP_TIME_WAIT SKs.
 	 * http://kerneltrap.org/mailarchive/linux-netdev/2010/10/21/6287959
 	 * Not fixed in 3.0-r3 :(
 	 */
 	if (sk) {
-		MT_DEBUG("qtaguid: %p->sk_proto=%u "
+		pr_debug("xt_qtaguid: %p->sk_proto=%u "
 			 "->sk_state=%d\n", sk, sk->sk_protocol, sk->sk_state);
 		if (sk->sk_state  == TCP_TIME_WAIT) {
 			xt_socket_put_sk(sk);
@@ -1109,14 +769,14 @@ static void account_for_uid(const struct sk_buff *skb,
 	const struct net_device *el_dev;
 
 	if (!skb->dev) {
-		MT_DEBUG("qtaguid[%d]: no skb->dev\n", par->hooknum);
+		pr_debug("xt_qtaguid[%d]: no skb->dev\n", par->hooknum);
 		el_dev = par->in ? : par->out;
 	} else {
 		const struct net_device *other_dev;
 		el_dev = skb->dev;
 		other_dev = par->in ? : par->out;
 		if (el_dev != other_dev) {
-			MT_DEBUG("qtaguid[%d]: skb->dev=%p %s vs "
+			pr_debug("xt_qtaguid[%d]: skb->dev=%p %s vs "
 				"par->(in/out)=%p %s\n",
 				par->hooknum, el_dev, el_dev->name, other_dev,
 				other_dev->name);
@@ -1124,14 +784,14 @@ static void account_for_uid(const struct sk_buff *skb,
 	}
 
 	if (unlikely(!el_dev)) {
-		pr_info("qtaguid[%d]: no par->in/out?!!\n", par->hooknum);
+		pr_info("xt_qtaguid[%d]: no par->in/out?!!\n", par->hooknum);
 	} else if (unlikely(!el_dev->name)) {
-		pr_info("qtaguid[%d]: no dev->name?!!\n", par->hooknum);
+		pr_info("xt_qtaguid[%d]: no dev->name?!!\n", par->hooknum);
 	} else {
-		MT_DEBUG("qtaguid[%d]: dev name=%s type=%d\n",
-			 par->hooknum,
-			 el_dev->name,
-			 el_dev->type);
+		pr_debug("xt_qtaguid[%d]: dev name=%s type=%d\n",
+			par->hooknum,
+			el_dev->name,
+			el_dev->type);
 
 		if_tag_stat_update(el_dev->name, uid,
 				skb->sk ? skb->sk : alternate_sk,
@@ -1148,13 +808,8 @@ static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par)
 	struct sock *sk;
 	uid_t sock_uid;
 	bool res;
-
-	if (unlikely(module_passive))
-		return (info->match ^ info->invert) == 0;
-
-	MT_DEBUG("qtaguid[%d]: entered skb=%p par->in=%p/out=%p fam=%d\n",
+	pr_debug("xt_qtaguid[%d]: entered skb=%p par->in=%p/out=%p fam=%d\n",
 		 par->hooknum, skb, par->in, par->out, par->family);
-
 	if (skb == NULL) {
 		res = (info->match ^ info->invert) == 0;
 		goto ret_res;
@@ -1163,72 +818,56 @@ static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par)
 	sk = skb->sk;
 
 	if (sk == NULL) {
-		/*
-		 * A missing sk->sk_socket happens when packets are in-flight
+		/*  A missing sk->sk_socket happens when packets are in-flight
 		 * and the matching socket is already closed and gone.
 		 */
 		sk = qtaguid_find_sk(skb, par);
-		/*
-		 * If we got the socket from the find_sk(), we will need to put
-		 * it back, as nf_tproxy_get_sock_v4() got it.
-		 */
+		/* If we got the socket from the find_sk(), we will need to put
+		 * it back, as nf_tproxy_get_sock_v4() got it. */
 		got_sock = sk;
-		if (sk)
-			atomic64_inc(&qtu_events.match_found_sk_in_ct);
-	} else {
-		atomic64_inc(&qtu_events.match_found_sk);
 	}
-	MT_DEBUG("qtaguid[%d]: sk=%p got_sock=%d proto=%d\n",
+	pr_debug("xt_qtaguid[%d]: sk=%p got_sock=%d proto=%d\n",
 		par->hooknum, sk, got_sock, ip_hdr(skb)->protocol);
 	if (sk != NULL) {
-		MT_DEBUG("qtaguid[%d]: sk=%p->sk_socket=%p->file=%p\n",
+		pr_debug("xt_qtaguid[%d]: sk=%p->sk_socket=%p->file=%p\n",
 			par->hooknum, sk, sk->sk_socket,
 			sk->sk_socket ? sk->sk_socket->file : (void *)-1LL);
 		filp = sk->sk_socket ? sk->sk_socket->file : NULL;
-		MT_DEBUG("qtaguid[%d]: filp...uid=%u\n",
+		pr_debug("xt_qtaguid[%d]: filp...uid=%d\n",
 			par->hooknum, filp ? filp->f_cred->fsuid : -1);
 	}
 
 	if (sk == NULL || sk->sk_socket == NULL) {
-		/*
-		 * Here, the qtaguid_find_sk() using connection tracking
+		/* Here, the qtaguid_find_sk() using connection tracking
 		 * couldn't find the owner, so for now we just count them
-		 * against the system.
-		 */
-		/*
-		 * TODO: unhack how to force just accounting.
+		 * against the system. */
+		/* TODO: unhack how to force just accounting.
 		 * For now we only do iface stats when the uid-owner is not
-		 * requested.
-		 */
+		 * requested */
 		if (!(info->match & XT_QTAGUID_UID))
 			account_for_uid(skb, sk, 0, par);
-		MT_DEBUG("qtaguid[%d]: leaving (sk?sk->sk_socket)=%p\n",
+		pr_debug("xt_qtaguid[%d]: leaving (sk?sk->sk_socket)=%p\n",
 			par->hooknum,
 			sk ? sk->sk_socket : NULL);
-		res = (info->match ^ info->invert) == 0;
-		atomic64_inc(&qtu_events.match_found_sk_none);
-		goto put_sock_ret_res;
+		res =  (info->match ^ info->invert) == 0;
 	} else if (info->match & info->invert & XT_QTAGUID_SOCKET) {
 		res = false;
 		goto put_sock_ret_res;
 	}
 	filp = sk->sk_socket->file;
 	if (filp == NULL) {
-		MT_DEBUG("qtaguid[%d]: leaving filp=NULL\n", par->hooknum);
+		pr_debug("xt_qtaguid[%d]: leaving filp=NULL\n", par->hooknum);
 		res = ((info->match ^ info->invert) &
 			(XT_QTAGUID_UID | XT_QTAGUID_GID)) == 0;
 		goto put_sock_ret_res;
 	}
 	sock_uid = filp->f_cred->fsuid;
-	/*
-	 * TODO: unhack how to force just accounting.
-	 * For now we only do iface stats when the uid-owner is not requested
-	 */
+	/* TODO: unhack how to force just accounting.
+	 * For now we only do iface stats when the uid-owner is not requested */
 	if (!(info->match & XT_QTAGUID_UID))
 		account_for_uid(skb, sk, sock_uid, par);
 
-	/*
-	 * The following two tests fail the match when:
+	/* The following two tests fail the match when:
 	 *    id not in range AND no inverted condition requested
 	 * or id     in range AND    inverted condition requested
 	 * Thus (!a && b) || (a && !b) == a ^ b
@@ -1237,7 +876,7 @@ static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par)
 		if ((filp->f_cred->fsuid >= info->uid_min &&
 		     filp->f_cred->fsuid <= info->uid_max) ^
 		    !(info->invert & XT_QTAGUID_UID)) {
-			MT_DEBUG("qtaguid[%d]: leaving uid not matching\n",
+			pr_debug("xt_qtaguid[%d]: leaving uid not matching\n",
 				 par->hooknum);
 			res = false;
 			goto put_sock_ret_res;
@@ -1246,20 +885,20 @@ static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par)
 		if ((filp->f_cred->fsgid >= info->gid_min &&
 				filp->f_cred->fsgid <= info->gid_max) ^
 			!(info->invert & XT_QTAGUID_GID)) {
-			MT_DEBUG("qtaguid[%d]: leaving gid not matching\n",
+			pr_debug("xt_qtaguid[%d]: leaving gid not matching\n",
 				par->hooknum);
 			res = false;
 			goto put_sock_ret_res;
 		}
 
-	MT_DEBUG("qtaguid[%d]: leaving matched\n", par->hooknum);
+	pr_debug("xt_qtaguid[%d]: leaving matched\n", par->hooknum);
 	res = true;
 
 put_sock_ret_res:
 	if (got_sock)
 		xt_socket_put_sk(sk);
 ret_res:
-	MT_DEBUG("qtaguid[%d]: left %d\n", par->hooknum, res);
+	pr_debug("xt_qtaguid[%d]: left %d\n", par->hooknum, res);
 	return res;
 }
 
@@ -1273,41 +912,35 @@ static int qtaguid_ctrl_proc_read(char *page, char **num_items_returned,
 {
 	char *outp = page;
 	int len;
+	unsigned long flags;
 	uid_t uid;
 	struct sock_tag *sock_tag_entry;
 	struct rb_node *node;
 	int item_index = 0;
 
-	if (unlikely(module_passive)) {
-		*eof = 1;
-		return 0;
-	}
-
-	/* TODO: support skipping num_items_returned on entry. */
-	CT_DEBUG("qtaguid: proc ctrl page=%p off=%ld char_count=%d *eof=%d\n",
+	pr_debug("xt_qtaguid:proc ctrl page=%p off=%ld char_count=%d *eof=%d\n",
 		page, items_to_skip, char_count, *eof);
 
 	if (*eof)
 		return 0;
 
-	spin_lock_bh(&sock_tag_list_lock);
+	spin_lock_irqsave(&sock_tag_list_lock, flags);
 	for (node = rb_first(&sock_tag_tree);
 	     node;
 	     node = rb_next(node)) {
 		if (item_index++ < items_to_skip)
 			continue;
-		sock_tag_entry = rb_entry(node, struct sock_tag, sock_node);
+		sock_tag_entry =  rb_entry(node, struct sock_tag, node);
 		uid = get_uid_from_tag(sock_tag_entry->tag);
-		CT_DEBUG("qtaguid: proc_read(): sk=%p tag=0x%llx (uid=%u)\n",
-			 sock_tag_entry->sk,
-			 sock_tag_entry->tag,
-			 uid
-			);
+		pr_debug("xt_qtaguid: proc_read(): sk=%p tag=0x%llx (uid=%d)\n",
+			sock_tag_entry->sk,
+			sock_tag_entry->tag,
+			uid);
 		len = snprintf(outp, char_count,
 			       "sock=%p tag=0x%llx (uid=%u)\n",
 			       sock_tag_entry->sk, sock_tag_entry->tag, uid);
 		if (len >= char_count) {
-			spin_unlock_bh(&sock_tag_list_lock);
+			spin_unlock_irqrestore(&sock_tag_list_lock, flags);
 			*outp = '\0';
 			return outp - page;
 		}
@@ -1315,436 +948,134 @@ static int qtaguid_ctrl_proc_read(char *page, char **num_items_returned,
 		char_count -= len;
 		(*num_items_returned)++;
 	}
-	spin_unlock_bh(&sock_tag_list_lock);
-
-	if (item_index++ >= items_to_skip) {
-		len = snprintf(outp, char_count,
-			       "events: sockets_tagged=%llu "
-			       "sockets_untagged=%llu "
-			       "counter_set_changes=%llu "
-			       "delete_cmds=%llu "
-			       "iface_events=%llu "
-			       "match_found_sk=%llu "
-			       "match_found_sk_in_ct=%llu "
-			       "match_found_sk_none=%llu\n",
-			       atomic64_read(&qtu_events.sockets_tagged),
-			       atomic64_read(&qtu_events.sockets_untagged),
-			       atomic64_read(&qtu_events.counter_set_changes),
-			       atomic64_read(&qtu_events.delete_cmds),
-			       atomic64_read(&qtu_events.iface_events),
-			       atomic64_read(&qtu_events.match_found_sk),
-			       atomic64_read(&qtu_events.match_found_sk_in_ct),
-			       atomic64_read(&qtu_events.match_found_sk_none));
-		if (len >= char_count) {
-			*outp = '\0';
-			return outp - page;
-		}
-		outp += len;
-		char_count -= len;
-		(*num_items_returned)++;
-	}
-
+	spin_unlock_irqrestore(&sock_tag_list_lock, flags);
 	*eof = 1;
 	return outp - page;
 }
 
-static bool can_manipulate_uids(void)
-{
-	/* root pwnd */
-	return unlikely(!current_fsuid()) || unlikely(!proc_ctrl_write_gid)
-		|| in_egroup_p(proc_ctrl_write_gid);
-}
-
-static bool can_impersonate_uid(uid_t uid)
-{
-	return uid == current_fsuid() || can_manipulate_uids();
-}
-
-static bool can_read_other_uid_stats(uid_t uid)
-{
-	/* root pwnd */
-	return unlikely(!current_fsuid()) || uid == current_fsuid()
-		|| unlikely(!proc_stats_readall_gid)
-		|| in_egroup_p(proc_stats_readall_gid);
-}
-
-/*
- * Delete socket tags, and stat tags associated with a given
- * accouting tag and uid.
- */
-static int ctrl_cmd_delete(const char *input)
-{
-	char cmd;
-	uid_t uid;
-	uid_t entry_uid;
-	tag_t acct_tag;
-	tag_t tag;
-	int res, argc;
-	struct iface_stat *iface_entry;
-	struct rb_node *node;
-	struct sock_tag *st_entry;
-	struct rb_root st_to_free_tree = RB_ROOT;
-	struct tag_stat *ts_entry;
-	struct tag_counter_set *tcs_entry;
-
-	argc = sscanf(input, "%c %llu %u", &cmd, &acct_tag, &uid);
-	CT_DEBUG("qtaguid: ctrl_delete(%s): argc=%d cmd=%c "
-		 "user_tag=0x%llx uid=%u\n", input, argc, cmd,
-		 acct_tag, uid);
-	if (argc < 2) {
-		res = -EINVAL;
-		goto err;
-	}
-	if (!valid_atag(acct_tag)) {
-		pr_info("qtaguid: ctrl_delete(%s): invalid tag\n", input);
-		res = -EINVAL;
-		goto err;
-	}
-	if (argc < 3) {
-		uid = current_fsuid();
-	} else if (!can_impersonate_uid(uid)) {
-		pr_info("qtaguid: ctrl_delete(%s): "
-			"insufficient priv from pid=%u uid=%u\n",
-			input, current->pid, current_fsuid());
-		res = -EPERM;
-		goto err;
-	}
-
-	/* Delete socket tags */
-	spin_lock_bh(&sock_tag_list_lock);
-	node = rb_first(&sock_tag_tree);
-	while (node) {
-		st_entry = rb_entry(node, struct sock_tag, sock_node);
-		entry_uid = get_uid_from_tag(st_entry->tag);
-		node = rb_next(node);
-		if (entry_uid != uid)
-			continue;
-
-		if (!acct_tag || st_entry->tag == tag) {
-			rb_erase(&st_entry->sock_node, &sock_tag_tree);
-			/* Can't sockfd_put() within spinlock, do it later. */
-			sock_tag_tree_insert(st_entry, &st_to_free_tree);
-		}
-	}
-	spin_unlock_bh(&sock_tag_list_lock);
-
-	node = rb_first(&st_to_free_tree);
-	while (node) {
-		st_entry = rb_entry(node, struct sock_tag, sock_node);
-		node = rb_next(node);
-		CT_DEBUG("qtaguid: ctrl_delete(): "
-			 "erase st: sk=%p tag=0x%llx (uid=%u)\n",
-			 st_entry->sk,
-			 st_entry->tag,
-			 entry_uid);
-		rb_erase(&st_entry->sock_node, &st_to_free_tree);
-		sockfd_put(st_entry->socket);
-		kfree(st_entry);
-	}
-
-	tag = combine_atag_with_uid(acct_tag, uid);
-
-	/* Delete tag counter-sets */
-	spin_lock_bh(&tag_counter_set_list_lock);
-	tcs_entry = tag_counter_set_tree_search(&tag_counter_set_tree, tag);
-	if (tcs_entry) {
-		CT_DEBUG("qtaguid: ctrl_delete(): "
-			 "erase tcs: tag=0x%llx (uid=%u) set=%d\n",
-			 tcs_entry->tn.tag,
-			 get_uid_from_tag(tcs_entry->tn.tag),
-			 tcs_entry->active_set);
-		rb_erase(&tcs_entry->tn.node, &tag_counter_set_tree);
-		kfree(tcs_entry);
-	}
-	spin_unlock_bh(&tag_counter_set_list_lock);
-
-	/*
-	 * If acct_tag is 0, then all entries belonging to uid are
-	 * erased.
-	 */
-	spin_lock_bh(&iface_stat_list_lock);
-	list_for_each_entry(iface_entry, &iface_stat_list, list) {
-		spin_lock_bh(&iface_entry->tag_stat_list_lock);
-		node = rb_first(&iface_entry->tag_stat_tree);
-		while (node) {
-			ts_entry = rb_entry(node, struct tag_stat, tn.node);
-			entry_uid = get_uid_from_tag(ts_entry->tn.tag);
-			node = rb_next(node);
-			if (entry_uid != uid)
-				continue;
-			if (!acct_tag || ts_entry->tn.tag == tag) {
-				CT_DEBUG("qtaguid: ctrl_delete(): "
-					 "erase ts: %s 0x%llx %u\n",
-					 iface_entry->ifname,
-					 get_atag_from_tag(ts_entry->tn.tag),
-					 entry_uid);
-				rb_erase(&ts_entry->tn.node,
-					 &iface_entry->tag_stat_tree);
-				kfree(ts_entry);
-			}
-		}
-		spin_unlock_bh(&iface_entry->tag_stat_list_lock);
-	}
-	spin_unlock_bh(&iface_stat_list_lock);
-	atomic64_inc(&qtu_events.delete_cmds);
-	res = 0;
-
-err:
-	return res;
-}
-
-static int ctrl_cmd_counter_set(const char *input)
-{
-	char cmd;
-	uid_t uid = 0;
-	tag_t tag;
-	int res, argc;
-	struct tag_counter_set *tcs;
-	int counter_set;
-
-	argc = sscanf(input, "%c %d %u", &cmd, &counter_set, &uid);
-	CT_DEBUG("qtaguid: ctrl_counterset(%s): argc=%d cmd=%c "
-		 "set=%d uid=%u\n", input, argc, cmd,
-		 counter_set, uid);
-	if (argc != 3) {
-		res = -EINVAL;
-		goto err;
-	}
-	if (counter_set < 0 || counter_set >= IFS_MAX_COUNTER_SETS) {
-		pr_info("qtaguid: ctrl_counterset(%s): invalid counter_set range\n",
-			input);
-		res = -EINVAL;
-		goto err;
-	}
-	if (!can_manipulate_uids()) {
-		pr_info("qtaguid: ctrl_counterset(%s): "
-			"insufficient priv from pid=%u uid=%u\n",
-			input, current->pid, current_fsuid());
-		res = -EPERM;
-		goto err;
-	}
-
-	tag = make_tag_from_uid(uid);
-	spin_lock_bh(&tag_counter_set_list_lock);
-	tcs = tag_counter_set_tree_search(&tag_counter_set_tree, tag);
-	if (!tcs) {
-		tcs = kzalloc(sizeof(*tcs), GFP_ATOMIC);
-		if (!tcs) {
-			spin_unlock_bh(&tag_counter_set_list_lock);
-			pr_err("qtaguid: ctrl_counterset(%s): "
-			       "failed to alloc counter set\n",
-			       input);
-			res = -ENOMEM;
-			goto err;
-		}
-		tcs->tn.tag = tag;
-		tag_counter_set_tree_insert(tcs, &tag_counter_set_tree);
-		CT_DEBUG("qtaguid: ctrl_counterset(%s): added tcs tag=0x%llx "
-			 "(uid=%u) set=%d\n",
-			 input, tag, get_uid_from_tag(tag), counter_set);
-	}
-	tcs->active_set = counter_set;
-	spin_unlock_bh(&tag_counter_set_list_lock);
-	atomic64_inc(&qtu_events.counter_set_changes);
-	res = 0;
-
-err:
-	return res;
-}
-
-static int ctrl_cmd_tag(const char *input)
+static int qtaguid_ctrl_parse(const char *input, int count)
 {
 	char cmd;
 	int sock_fd = 0;
 	uid_t uid = 0;
 	tag_t acct_tag = 0;
 	struct socket *el_socket;
-	int refcnt = -1;
 	int res, argc;
 	struct sock_tag *sock_tag_entry;
+	unsigned long flags;
 
+	pr_debug("xt_qtaguid: ctrl(%s): entered\n", input);
 	/* Unassigned args will get defaulted later. */
+	/* TODO: get acct_tag_str, keep a list of available tags for the
+	 * uid, use num as acct_tag. */
 	argc = sscanf(input, "%c %d %llu %u", &cmd, &sock_fd, &acct_tag, &uid);
-	CT_DEBUG("qtaguid: ctrl_tag(%s): argc=%d cmd=%c sock_fd=%d "
-		 "acct_tag=0x%llx uid=%u\n", input, argc, cmd, sock_fd,
-		 acct_tag, uid);
-	if (argc < 2) {
-		res = -EINVAL;
-		goto err;
-	}
-	el_socket = sockfd_lookup(sock_fd, &res);  /* This locks the file */
-	if (!el_socket) {
-		pr_info("qtaguid: ctrl_tag(%s): failed to lookup"
-			" sock_fd=%d err=%d\n", input, sock_fd, res);
-		goto err;
-	}
-	refcnt = atomic_read(&el_socket->file->f_count);
-	CT_DEBUG("qtaguid: ctrl_tag(%s): socket->...->f_count=%d\n",
-		 input, refcnt);
-	if (argc < 3) {
-		acct_tag = 0;
-	} else if (!valid_atag(acct_tag)) {
-		pr_info("qtaguid: ctrl_tag(%s): invalid tag\n", input);
-		res = -EINVAL;
-		goto err_put;
-	}
-	CT_DEBUG("qtaguid: ctrl_tag(%s): "
-		 "uid=%u euid=%u fsuid=%u "
-		 "in_group=%d in_egroup=%d\n",
-		 input, current_uid(), current_euid(), current_fsuid(),
-		 in_group_p(proc_stats_readall_gid),
-		 in_egroup_p(proc_stats_readall_gid));
-	if (argc < 4) {
-		uid = current_fsuid();
-	} else if (!can_impersonate_uid(uid)) {
-		pr_info("qtaguid: ctrl_tag(%s): "
-			"insufficient priv from pid=%u uid=%u\n",
-			input, current->pid, current_fsuid());
-		res = -EPERM;
-		goto err_put;
-	}
+	pr_debug("xt_qtaguid: ctrl(%s): argc=%d cmd=%c sock_fd=%d "
+		"acct_tag=0x%llx uid=%u\n", input, argc, cmd, sock_fd,
+		acct_tag, uid);
 
-	spin_lock_bh(&sock_tag_list_lock);
-	sock_tag_entry = get_sock_stat_nl(el_socket->sk);
-	if (sock_tag_entry) {
-		/*
-		 * This is a re-tagging, so release the sock_fd that was
-		 * locked at the time of the 1st tagging.
-		 */
-		sockfd_put(sock_tag_entry->socket);
-		refcnt--;
-		sock_tag_entry->tag = combine_atag_with_uid(acct_tag,
-							    uid);
-	} else {
-		sock_tag_entry = kzalloc(sizeof(*sock_tag_entry),
-					 GFP_ATOMIC);
-		if (!sock_tag_entry) {
-			pr_err("qtaguid: ctrl_tag(%s): "
-			       "socket tag alloc failed\n",
-			       input);
-			spin_unlock_bh(&sock_tag_list_lock);
-			res = -ENOMEM;
-			goto err_put;
+	/* Collect params for commands */
+	switch (cmd) {
+	case 't':
+	case 'u':
+		if (argc < 2) {
+			res = -EINVAL;
+			goto err;
 		}
-		sock_tag_entry->sk = el_socket->sk;
-		sock_tag_entry->socket = el_socket;
-		sock_tag_entry->tag = combine_atag_with_uid(acct_tag,
-							    uid);
-		sock_tag_tree_insert(sock_tag_entry, &sock_tag_tree);
-		atomic64_inc(&qtu_events.sockets_tagged);
-	}
-	spin_unlock_bh(&sock_tag_list_lock);
-	/* We keep the ref to the socket (file) until it is untagged */
-	CT_DEBUG("qtaguid: ctrl_tag(%s): done. socket->...->f_count=%d\n",
-		 input,
-		 el_socket ? atomic_read(&el_socket->file->f_count) : -1);
-	return 0;
-
-err_put:
-	/* Release the sock_fd that was grabbed by sockfd_lookup(). */
-	sockfd_put(el_socket);
-	refcnt--;
-err:
-	CT_DEBUG("qtaguid: ctrl_tag(%s): done. socket->...->f_count=%d\n",
-		 input, refcnt);
-	return res;
-}
-
-static int ctrl_cmd_untag(const char *input)
-{
-	char cmd;
-	int sock_fd = 0;
-	struct socket *el_socket;
-	int refcnt = -1;
-	int res, argc;
-	struct sock_tag *sock_tag_entry;
-
-	argc = sscanf(input, "%c %d", &cmd, &sock_fd);
-	CT_DEBUG("qtaguid: ctrl_untag(%s): argc=%d cmd=%c sock_fd=%d\n",
-		 input, argc, cmd, sock_fd);
-	if (argc < 2) {
+		el_socket = sockfd_lookup(sock_fd, &res);
+		if (!el_socket) {
+			pr_info("xt_qtaguid: ctrl(%s): failed to lookup"
+				" sock_fd=%d err=%d\n", input, sock_fd, res);
+			goto err;
+		}
+		spin_lock_irqsave(&sock_tag_list_lock, flags);
+		/* TODO: optim: pass in the current_fsuid() to do lookups
+		 * as look ups will always be initiated form the same uid. */
+		sock_tag_entry = get_sock_stat_nl(el_socket->sk);
+		if (!sock_tag_entry)
+			spin_unlock_irqrestore(&sock_tag_list_lock, flags);
+		/* HERE: The lock is held if there was a matching sock tag entry */
+		break;
+	default:
 		res = -EINVAL;
 		goto err;
 	}
-	el_socket = sockfd_lookup(sock_fd, &res);  /* This locks the file */
-	if (!el_socket) {
-		pr_info("qtaguid: ctrl_untag(%s): failed to lookup"
-			" sock_fd=%d err=%d\n", input, sock_fd, res);
-		goto err;
-	}
-	refcnt = atomic_read(&el_socket->file->f_count);
-	CT_DEBUG("qtaguid: ctrl_untag(%s): socket->...->f_count=%d\n",
-		 input, refcnt);
-	spin_lock_bh(&sock_tag_list_lock);
-	sock_tag_entry = get_sock_stat_nl(el_socket->sk);
-	if (!sock_tag_entry) {
-		spin_unlock_bh(&sock_tag_list_lock);
-		res = -EINVAL;
-		goto err_put;
-	}
-	/*
-	 * The socket already belongs to the current process
-	 * so it can do whatever it wants to it.
-	 */
-	rb_erase(&sock_tag_entry->sock_node, &sock_tag_tree);
-
-	/*
-	 * Release the sock_fd that was grabbed at tag time,
-	 * and once more for the sockfd_lookup() here.
-	 */
-	sockfd_put(sock_tag_entry->socket);
-	spin_unlock_bh(&sock_tag_list_lock);
-	sockfd_put(el_socket);
-	refcnt -= 2;
-	kfree(sock_tag_entry);
-	atomic64_inc(&qtu_events.sockets_untagged);
-	CT_DEBUG("qtaguid: ctrl_untag(%s): done. socket->...->f_count=%d\n",
-		 input, refcnt);
-
-	return 0;
-
-err_put:
-	/* Release the sock_fd that was grabbed by sockfd_lookup(). */
-	sockfd_put(el_socket);
-	refcnt--;
-err:
-	CT_DEBUG("qtaguid: ctrl_untag(%s): done. socket->...->f_count=%d\n",
-		 input, refcnt);
-	return res;
-}
+	/* HERE: The lock is held if there was a matching sock tag entry */
 
-static int qtaguid_ctrl_parse(const char *input, int count)
-{
-	char cmd;
-	int res;
-
-	cmd = input[0];
-	/* Collect params for commands */
+	/* Process commands */
 	switch (cmd) {
-	case 'd':
-		res = ctrl_cmd_delete(input);
-		break;
-
-	case 's':
-		res = ctrl_cmd_counter_set(input);
-		break;
 
 	case 't':
-		res = ctrl_cmd_tag(input);
+		if (argc < 2) {
+			res = -EINVAL;
+			/* HERE: The lock is held if there was a matching sock
+			 * tag entry */
+			goto err_unlock;
+		}
+		if (argc < 3) {
+			acct_tag = 0;
+		} else if (!valid_atag(acct_tag)) {
+			res = -EINVAL;
+			/* HERE: The lock is held if there was a matching sock
+			 * tag entry */
+			goto err_unlock;
+		}
+		if (argc < 4)
+			uid = current_fsuid();
+		if (!sock_tag_entry) {
+			/* HERE: There is no lock held because there was no
+			 * sock tag entry */
+			sock_tag_entry = kmalloc(sizeof(*sock_tag_entry),
+						GFP_KERNEL);
+			if (!sock_tag_entry) {
+				res = -ENOMEM;
+				goto err;
+			}
+			memset(sock_tag_entry, 0, sizeof(*sock_tag_entry));
+			sock_tag_entry->sk = el_socket->sk;
+			/* TODO: check that uid==current_fsuid() except
+			 * for special uid/gid. */
+			sock_tag_entry->tag = combine_atag_with_uid(acct_tag,
+								uid);
+			spin_lock_irqsave(&sock_tag_list_lock, flags);
+			sock_tag_tree_insert(sock_tag_entry, &sock_tag_tree);
+		} else {
+			/* HERE: The lock is held because there is a matching
+			 * sock tag entry */
+			/* Just update the acct_tag portion. */
+			uid_t orig_uid = get_uid_from_tag(sock_tag_entry->tag);
+			sock_tag_entry->tag = combine_atag_with_uid(acct_tag,
+								orig_uid);
+		}
+		spin_unlock_irqrestore(&sock_tag_list_lock, flags);
+		pr_debug("xt_qtaguid: tag: sock_tag_entry->sk=%p "
+			"...->tag=0x%llx (uid=%u)\n",
+			sock_tag_entry->sk, sock_tag_entry->tag,
+			get_uid_from_tag(sock_tag_entry->tag));
 		break;
 
 	case 'u':
-		res = ctrl_cmd_untag(input);
+		if (!sock_tag_entry) {
+			res = -EINVAL;
+			goto err;
+		}
+		/* TODO: check that the uid==current_fsuid()
+		 * except for special uid/gid. */
+		rb_erase(&sock_tag_entry->node, &sock_tag_tree);
+		spin_unlock_irqrestore(&sock_tag_list_lock, flags);
+		kfree(sock_tag_entry);
 		break;
-
-	default:
-		res = -EINVAL;
-		goto err;
 	}
-	if (!res)
-		res = count;
+
+	/* All of the input has been processed */
+	res = count;
+	goto ok;
+
+err_unlock:
+	if (sock_tag_entry)
+		spin_unlock_irqrestore(&sock_tag_list_lock, flags);
 err:
-	CT_DEBUG("qtaguid: ctrl(%s): res=%d\n", input, res);
+ok:
+	pr_debug("xt_qtaguid: ctrl(%s): res=%d\n", input, res);
 	return res;
 }
 
@@ -1754,9 +1085,6 @@ static int qtaguid_ctrl_proc_write(struct file *file, const char __user *buffer,
 {
 	char input_buf[MAX_QTAGUID_CTRL_INPUT_LEN];
 
-	if (unlikely(module_passive))
-		return count;
-
 	if (count >= MAX_QTAGUID_CTRL_INPUT_LEN)
 		return -EINVAL;
 
@@ -1767,99 +1095,6 @@ static int qtaguid_ctrl_proc_write(struct file *file, const char __user *buffer,
 	return qtaguid_ctrl_parse(input_buf, count);
 }
 
-struct proc_print_info {
-	char *outp;
-	char **num_items_returned;
-	struct iface_stat *iface_entry;
-	struct tag_stat *ts_entry;
-	int item_index;
-	int char_count;
-};
-
-static int pp_stats_line(struct proc_print_info *ppi, int cnt_set)
-{
-	int len;
-	struct data_counters *cnts;
-	if (!ppi->item_index) {
-		len = snprintf(ppi->outp, ppi->char_count,
-			       "idx iface acct_tag_hex uid_tag_int cnt_set "
-			       "rx_bytes rx_packets "
-			       "tx_bytes tx_packets "
-			       "rx_tcp_packets rx_tcp_bytes "
-			       "rx_udp_packets rx_udp_bytes "
-			       "rx_other_packets rx_other_bytes "
-			       "tx_tcp_packets tx_tcp_bytes "
-			       "tx_udp_packets tx_udp_bytes "
-			       "tx_other_packets tx_other_bytes\n");
-	} else {
-		tag_t tag = ppi->ts_entry->tn.tag;
-		uid_t stat_uid = get_uid_from_tag(tag);
-		if (!can_read_other_uid_stats(stat_uid)) {
-			CT_DEBUG("qtaguid: stats line: "
-				 "%s 0x%llx %u: "
-				 "insufficient priv from pid=%u uid=%u\n",
-				 ppi->iface_entry->ifname,
-				 get_atag_from_tag(tag), stat_uid,
-				 current->pid, current_fsuid());
-			return 0;
-		}
-		cnts = &ppi->ts_entry->counters;
-		len = snprintf(
-			ppi->outp, ppi->char_count,
-			"%d %s 0x%llx %u %u "
-			"%llu %llu "
-			"%llu %llu "
-			"%llu %llu "
-			"%llu %llu "
-			"%llu %llu "
-			"%llu %llu "
-			"%llu %llu "
-			"%llu %llu\n",
-			ppi->item_index,
-			ppi->iface_entry->ifname,
-			get_atag_from_tag(tag),
-			stat_uid,
-			cnt_set,
-			dc_sum_bytes(cnts, cnt_set, IFS_RX),
-			dc_sum_packets(cnts, cnt_set, IFS_RX),
-			dc_sum_bytes(cnts, cnt_set, IFS_TX),
-			dc_sum_packets(cnts, cnt_set, IFS_TX),
-			cnts->bpc[cnt_set][IFS_RX][IFS_TCP].bytes,
-			cnts->bpc[cnt_set][IFS_RX][IFS_TCP].packets,
-			cnts->bpc[cnt_set][IFS_RX][IFS_UDP].bytes,
-			cnts->bpc[cnt_set][IFS_RX][IFS_UDP].packets,
-			cnts->bpc[cnt_set][IFS_RX][IFS_PROTO_OTHER].bytes,
-			cnts->bpc[cnt_set][IFS_RX][IFS_PROTO_OTHER].packets,
-			cnts->bpc[cnt_set][IFS_TX][IFS_TCP].bytes,
-			cnts->bpc[cnt_set][IFS_TX][IFS_TCP].packets,
-			cnts->bpc[cnt_set][IFS_TX][IFS_UDP].bytes,
-			cnts->bpc[cnt_set][IFS_TX][IFS_UDP].packets,
-			cnts->bpc[cnt_set][IFS_TX][IFS_PROTO_OTHER].bytes,
-			cnts->bpc[cnt_set][IFS_TX][IFS_PROTO_OTHER].packets);
-	}
-	return len;
-}
-
-bool pp_sets(struct proc_print_info *ppi)
-{
-	int len;
-	int counter_set;
-	for (counter_set = 0; counter_set < IFS_MAX_COUNTER_SETS;
-	     counter_set++) {
-		len = pp_stats_line(ppi, counter_set);
-		if (len >= ppi->char_count) {
-			*ppi->outp = '\0';
-			return false;
-		}
-		if (len) {
-			ppi->outp += len;
-			ppi->char_count -= len;
-			(*ppi->num_items_returned)++;
-		}
-	}
-	return true;
-}
-
 /*
  * Procfs reader to get all tag stats using style "1)" as described in
  * fs/proc/generic.c
@@ -1869,23 +1104,16 @@ static int qtaguid_stats_proc_read(char *page, char **num_items_returned,
 				off_t items_to_skip, int char_count, int *eof,
 				void *data)
 {
-	struct proc_print_info ppi;
+	char *outp = page;
 	int len;
+	unsigned long flags, flags2;
+	struct iface_stat *iface_entry;
+	struct tag_stat *ts_entry;
+	int item_index = 0;
 
-	ppi.outp = page;
-	ppi.item_index = 0;
-	ppi.char_count = char_count;
-	ppi.num_items_returned = num_items_returned;
-
-	if (unlikely(module_passive)) {
-		len = pp_stats_line(&ppi, 0);
-		/* The header should always be shorter than the buffer. */
-		WARN_ON(len >= ppi.char_count);
-		*eof = 1;
-		return len;
-	}
+	/* TODO: make root access only */
 
-	CT_DEBUG("qtaguid:proc stats page=%p *num_items_returned=%p off=%ld "
+	pr_debug("xt_qtaguid:proc stats page=%p *num_items_returned=%p off=%ld "
 		"char_count=%d *eof=%d\n", page, *num_items_returned,
 		items_to_skip, char_count, *eof);
 
@@ -1894,56 +1122,79 @@ static int qtaguid_stats_proc_read(char *page, char **num_items_returned,
 
 	if (!items_to_skip) {
 		/* The idx is there to help debug when things go belly up. */
-		len = pp_stats_line(&ppi, 0);
+			"idx iface acct_tag_hex uid_tag_int rx_bytes "
+			"tx_bytes\n");
 		/* Don't advance the outp unless the whole line was printed */
-		if (len >= ppi.char_count) {
-			*ppi.outp = '\0';
-			return ppi.outp - page;
+		if (len >= char_count) {
+			*outp = '\0';
+			return outp - page;
 		}
-		ppi.outp += len;
-		ppi.char_count -= len;
+		outp += len;
+		char_count -= len;
 	}
 
-	spin_lock_bh(&iface_stat_list_lock);
-	list_for_each_entry(ppi.iface_entry, &iface_stat_list, list) {
+	spin_lock_irqsave(&iface_stat_list_lock, flags);
+	list_for_each_entry(iface_entry, &iface_stat_list, list) {
 		struct rb_node *node;
-		spin_lock_bh(&ppi.iface_entry->tag_stat_list_lock);
-		for (node = rb_first(&ppi.iface_entry->tag_stat_tree);
+		spin_lock_irqsave(&iface_entry->tag_stat_list_lock, flags2);
+		for (node = rb_first(&iface_entry->tag_stat_tree);
 		     node;
 		     node = rb_next(node)) {
-			ppi.ts_entry = rb_entry(node, struct tag_stat, tn.node);
-			if (ppi.item_index++ < items_to_skip)
+			ts_entry =  rb_entry(node, struct tag_stat, node);
+			if (item_index++ < items_to_skip)
 				continue;
-			if (!pp_sets(&ppi)) {
-				spin_unlock_bh(
-					&ppi.iface_entry->tag_stat_list_lock);
-				spin_unlock_bh(&iface_stat_list_lock);
-				return ppi.outp - page;
+			len = snprintf(outp, char_count,
+				       "%d %s 0x%llx %u %llu %llu\n",
+				       item_index,
+				       iface_entry->ifname,
+				       get_atag_from_tag(ts_entry->tag),
+				       get_uid_from_tag(ts_entry->tag),
+				       dc_sum_bytes(&ts_entry->counters,
+						    IFS_RX),
+				       dc_sum_bytes(&ts_entry->counters,
+						    IFS_TX));
+			if (len >= char_count) {
+				spin_unlock_irqrestore(
+					&iface_entry->tag_stat_list_lock,
+					flags2);
+				spin_unlock_irqrestore(
+					&iface_stat_list_lock, flags);
+				*outp = '\0';
+				return outp - page;
 			}
+			outp += len;
+			char_count -= len;
+			(*num_items_returned)++;
 		}
-		spin_unlock_bh(&ppi.iface_entry->tag_stat_list_lock);
+		spin_unlock_irqrestore(&iface_entry->tag_stat_list_lock,
+				flags2);
 	}
-	spin_unlock_bh(&iface_stat_list_lock);
+	spin_unlock_irqrestore(&iface_stat_list_lock, flags);
 
 	*eof = 1;
-	return ppi.outp - page;
+	return outp - page;
 }
 
 /*------------------------------------------*/
+static const char *module_procdirname = "xt_qtaguid";
+static struct proc_dir_entry *xt_qtaguid_procdir;
+static struct proc_dir_entry *xt_qtaguid_ctrl_file;
+static struct proc_dir_entry *xt_qtaguid_stats_file;
+
 static int __init qtaguid_proc_register(struct proc_dir_entry **res_procdir)
 {
 	int ret;
 	*res_procdir = proc_mkdir(module_procdirname, init_net.proc_net);
 	if (!*res_procdir) {
-		pr_err("qtaguid: failed to create proc/.../xt_qtaguid\n");
+		pr_err("xt_qtaguid: failed to create proc/.../xt_qtaguid\n");
 		ret = -ENOMEM;
 		goto no_dir;
 	}
 
-	xt_qtaguid_ctrl_file = create_proc_entry("ctrl", proc_ctrl_perms,
+	xt_qtaguid_ctrl_file = create_proc_entry("ctrl", 0666,
 						*res_procdir);
 	if (!xt_qtaguid_ctrl_file) {
-		pr_err("qtaguid: failed to create xt_qtaguid/ctrl "
+		pr_err("xt_qtaguid: failed to create xt_qtaguid/ctrl "
 			" file\n");
 		ret = -ENOMEM;
 		goto no_ctrl_entry;
@@ -1951,14 +1202,18 @@ static int __init qtaguid_proc_register(struct proc_dir_entry **res_procdir)
 	xt_qtaguid_ctrl_file->read_proc = qtaguid_ctrl_proc_read;
 	xt_qtaguid_ctrl_file->write_proc = qtaguid_ctrl_proc_write;
 
-	xt_qtaguid_stats_file = create_proc_entry("stats", proc_stats_perms,
+	xt_qtaguid_stats_file = create_proc_entry("stats", 0666,
 						*res_procdir);
 	if (!xt_qtaguid_stats_file) {
-		pr_err("qtaguid: failed to create xt_qtaguid/stats "
+		pr_err("xt_qtaguid: failed to create xt_qtaguid/stats "
 			"file\n");
 		ret = -ENOMEM;
 		goto no_stats_entry;
 	}
+	/*
+	 * TODO: add extra read_proc for full stats with protocol
+	 * breakout
+	 */
 	xt_qtaguid_stats_file->read_proc = qtaguid_stats_proc_read;
 	/*
 	 * TODO: add support counter hacking
@@ -1996,8 +1251,7 @@ static int __init qtaguid_mt_init(void)
 	return 0;
 }
 
-/*
- * TODO: allow unloading of the module.
+/* TODO: allow unloading of the module.
  * For now stats are permanent.
  * Kconfig forces'y/n' and never an 'm'.
  */
diff --git a/net/netfilter/xt_quota2.c b/net/netfilter/xt_quota2.c
index 3c72bea2dd6..454ce10c8cd 100644
--- a/net/netfilter/xt_quota2.c
+++ b/net/netfilter/xt_quota2.c
@@ -19,9 +19,6 @@
 
 #include <linux/netfilter/x_tables.h>
 #include <linux/netfilter/xt_quota2.h>
-#ifdef CONFIG_NETFILTER_XT_MATCH_QUOTA2_LOG
-#include <linux/netfilter_ipv4/ipt_ULOG.h>
-#endif
 
 /**
  * @lock:	lock to protect quota writers from each other
@@ -35,16 +32,6 @@ struct xt_quota_counter {
 	struct proc_dir_entry *procfs_entry;
 };
 
-#ifdef CONFIG_NETFILTER_XT_MATCH_QUOTA2_LOG
-/* Harald's favorite number +1 :D From ipt_ULOG.C */
-static int qlog_nl_event = 112;
-module_param_named(event_num, qlog_nl_event, uint, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(event_num,
-		 "Event number for NETLINK_NFLOG message. 0 disables log."
-		 "111 is what ipt_ULOG uses.");
-static struct sock *nflognl;
-#endif
-
 static LIST_HEAD(counter_list);
 static DEFINE_SPINLOCK(counter_list_lock);
 
@@ -56,69 +43,6 @@ module_param_named(perms, quota_list_perms, uint, S_IRUGO | S_IWUSR);
 module_param_named(uid, quota_list_uid, uint, S_IRUGO | S_IWUSR);
 module_param_named(gid, quota_list_gid, uint, S_IRUGO | S_IWUSR);
 
-
-#ifdef CONFIG_NETFILTER_XT_MATCH_QUOTA2_LOG
-static void quota2_log(unsigned int hooknum,
-		       const struct sk_buff *skb,
-		       const struct net_device *in,
-		       const struct net_device *out,
-		       const char *prefix)
-{
-	ulog_packet_msg_t *pm;
-	struct sk_buff *log_skb;
-	size_t size;
-	struct nlmsghdr *nlh;
-
-	if (!qlog_nl_event)
-		return;
-
-	size = NLMSG_SPACE(sizeof(*pm));
-	size = max(size, (size_t)NLMSG_GOODSIZE);
-	log_skb = alloc_skb(size, GFP_ATOMIC);
-	if (!log_skb) {
-		pr_err("xt_quota2: cannot alloc skb for logging\n");
-		return;
-	}
-
-	/* NLMSG_PUT() uses "goto nlmsg_failure" */
-	nlh = NLMSG_PUT(log_skb, /*pid*/0, /*seq*/0, qlog_nl_event,
-			sizeof(*pm));
-	pm = NLMSG_DATA(nlh);
-	if (skb->tstamp.tv64 == 0)
-		__net_timestamp((struct sk_buff *)skb);
-	pm->data_len = 0;
-	pm->hook = hooknum;
-	if (prefix != NULL)
-		strlcpy(pm->prefix, prefix, sizeof(pm->prefix));
-	else
-		*(pm->prefix) = '\0';
-	if (in)
-		strlcpy(pm->indev_name, in->name, sizeof(pm->indev_name));
-	else
-		pm->indev_name[0] = '\0';
-
-	if (out)
-		strlcpy(pm->outdev_name, out->name, sizeof(pm->outdev_name));
-	else
-		pm->outdev_name[0] = '\0';
-
-	NETLINK_CB(log_skb).dst_group = 1;
-	pr_debug("throwing 1 packets to netlink group 1\n");
-	netlink_broadcast(nflognl, log_skb, 0, 1, GFP_ATOMIC);
-
-nlmsg_failure:  /* Used within NLMSG_PUT() */
-	pr_debug("xt_quota2: error during NLMSG_PUT\n");
-}
-#else
-static void quota2_log(unsigned int hooknum,
-		       const struct sk_buff *skb,
-		       const struct net_device *in,
-		       const struct net_device *out,
-		       const char *prefix)
-{
-}
-#endif  /* if+else CONFIG_NETFILTER_XT_MATCH_QUOTA2_LOG */
-
 static int quota_proc_read(char *page, char **start, off_t offset,
                            int count, int *eof, void *data)
 {
@@ -302,14 +226,6 @@ quota_mt2(const struct sk_buff *skb, struct xt_action_param *par)
 				e->quota -= (q->flags & XT_QUOTA_PACKET) ? 1 : skb->len;
 			ret = !ret;
 		} else {
-			/* We are transitioning, log that fact. */
-			if (e->quota) {
-				quota2_log(par->hooknum,
-					   skb,
-					   par->in,
-					   par->out,
-					   q->name);
-			}
 			/* we do not allow even small packets from now on */
 			e->quota = 0;
 		}
@@ -346,14 +262,6 @@ static int __init quota_mt2_init(void)
 	int ret;
 	pr_debug("xt_quota2: init()");
 
-#ifdef CONFIG_NETFILTER_XT_MATCH_QUOTA2_LOG
-	nflognl = netlink_kernel_create(&init_net,
-					NETLINK_NFLOG, 1, NULL,
-					NULL, THIS_MODULE);
-	if (!nflognl)
-		return -ENOMEM;
-#endif
-
 	proc_xt_quota = proc_mkdir("xt_quota", init_net.proc_net);
 	if (proc_xt_quota == NULL)
 		return -EACCES;
diff --git a/scripts/setlocalversion b/scripts/setlocalversion
index 4d403844e13..06c33adfe7f 100755
--- a/scripts/setlocalversion
+++ b/scripts/setlocalversion
@@ -10,23 +10,37 @@
 #
 
 usage() {
-	echo "Usage: $0 [--save-scmversion] [srctree]" >&2
+	echo "Usage: $0 [--save-scmversion] [-s srctree] [-t ref_tag]" >&2
 	exit 1
 }
 
 scm_only=false
 srctree=.
-if test "$1" = "--save-scmversion"; then
-	scm_only=true
-	shift
-fi
-if test $# -gt 0; then
-	srctree=$1
+match_option=--exact-match
+
+while [ $# -ne 0 ]; do
+	if test "$1" = "--save-scmversion"; then
+		scm_only=true
+	elif test "$1" = "-s"; then
+		shift
+		if test $# -ne 0 -a -d "$1"; then
+			srctree=$1
+		else
+			usage
+		fi
+	elif test "$1" = "-t"; then
+		shift
+		if [ $# -ne 0 ]; then
+			match=" --tags --match "$1
+			rev_refs="--refs refs/tags/"$1
+		else
+			usage
+		fi
+	else
+		usage
+	fi
 	shift
-fi
-if test $# -gt 0 -o ! -d "$srctree"; then
-	usage
-fi
+done
 
 scm_version()
 {
@@ -47,8 +61,8 @@ scm_version()
 
 		# If we are at a tagged commit (like "v2.6.30-rc6"), we ignore
 		# it, because this version is defined in the top level Makefile.
-		if [ -z "`git describe --exact-match 2>/dev/null`" ]; then
-
+		if git name-rev --tags $rev_refs HEAD | \
+		   grep -E '^HEAD[[:space:]]+(.*~[0-9]*|undefined)$' > /dev/null; then
 			# If only the short version is requested, don't bother
 			# running further git commands
 			if $short; then
@@ -57,7 +71,7 @@ scm_version()
 			fi
 			# If we are past a tagged commit (like
 			# "v2.6.30-rc5-302-g72357d5"), we pretty print it.
-			if atag="`git describe 2>/dev/null`"; then
+			if atag="`git describe $match 2>/dev/null`"; then
 				echo "$atag" | awk -F- '{printf("-%05d-%s", $(NF-1),$(NF))}'
 
 			# If we don't have a tag at all we print -g{commitish}.
diff --git a/sound/Kconfig b/sound/Kconfig
index 1fef141ef8e..35bb190ed54 100644
--- a/sound/Kconfig
+++ b/sound/Kconfig
@@ -23,6 +23,66 @@ menuconfig SOUND
 	  and read <file:Documentation/sound/oss/README.modules>; the module
 	  will be called soundcore.
 
+# added for U8500 audio codec device
+config U8500_ACODEC
+	tristate "U8500 audio codec generic module (used both by SAA and ALSA)"
+	depends on STM_MSP_I2S
+	default Y
+	help
+	  Say Y here if you have a U8500 based device
+	  and want to use its audio codec chip.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called u8500mod_acodec.
+
+choice
+	prompt "Audio codec type"
+	depends on U8500_ACODEC
+	default U8500_AB8500_ED
+
+	config U8500_AB8500_ED
+	bool "U8500 ab8500 v0 audio codec"
+
+	config U8500_AB8500_CUT10
+	bool "U8500 ab8500 v1.x audio codec"
+
+endchoice
+
+menu "Debug level for ux500 audio drivers"
+config  STM_ACODEC_DEBUG
+	int "STM ACODEC Debug Level"
+	depends on U8500_ACODEC
+	default 0
+	help
+	  Sets the ACODEC debug ON/OFF for U8500 SoC
+		* 0 OFF
+		* 1 ON
+
+config  STM_ALSA_DEBUG
+	int "STM ALSA Debug Level"
+	depends on SND_U8500_ALSA || SND_U8500_ALSA_AB8500
+	default 0
+	help
+	  Sets the ALSA debug ON/OFF for U8500 SoC
+		* 0 OFF
+		* 1 ON
+endmenu
+
+choice
+	prompt "Driver mode"
+	depends on U8500_ACODEC
+	default U8500_ACODEC_DMA
+
+	config U8500_ACODEC_DMA
+	bool   "DMA mode"
+
+	config U8500_ACODEC_POLL
+	bool   "Polling mode"
+
+	config U8500_ACODEC_INTR
+	bool   "Interrupt mode"
+endchoice
+
 if SOUND
 
 config SOUND_OSS_CORE
diff --git a/sound/Makefile b/sound/Makefile
index ce9132b1c39..b5963c8b70d 100644
--- a/sound/Makefile
+++ b/sound/Makefile
@@ -16,4 +16,11 @@ ifeq ($(CONFIG_SND),y)
   obj-y += last.o
 endif
 
+obj-$(CONFIG_U8500_ACODEC) += u8500mod_acodec.o
+ifeq ($(CONFIG_U8500_AB8500_CUT10),y)
+  u8500mod_acodec-objs := u8500_acodec_ab8500.o ab8500_codec_v1_0.o
+else
+  u8500mod_acodec-objs := u8500_acodec_ab8500.o ab8500_codec.o
+endif
+
 soundcore-objs  := sound_core.o
diff --git a/sound/ab8500_codec.c b/sound/ab8500_codec.c
new file mode 100644
index 00000000000..fd4975bbdce
--- /dev/null
+++ b/sound/ab8500_codec.c
@@ -0,0 +1,6697 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: ST-Ericsson
+ *
+ * License terms:
+ *
+ * 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.
+ */
+
+ /*----------------------------------------------------------------------------
+ * Includes
+ *---------------------------------------------------------------------------*/
+#include <mach/ab8500_codec.h>
+#include <mach/ab8500_codec_p.h>
+
+/*--------------------------------------------------------------------------*
+ * debug stuff *
+ *--------------------------------------------------------------------------*/
+#ifdef __DEBUG
+#define MY_DEBUG_LEVEL_VAR_NAME myDebugLevel_AB8500_CODEC
+#define MY_DEBUG_ID             myDebugID_AB8500_CODEC
+PRIVATE t_dbg_level MY_DEBUG_LEVEL_VAR_NAME = DEBUG_LEVEL0;
+PRIVATE t_dbg_id MY_DEBUG_ID = AB8500_CODEC_HCL_DBG_ID;
+#endif
+
+/*--------------------------------------------------------------------------*
+ * Global data for interrupt mode management *
+ *--------------------------------------------------------------------------*/
+PRIVATE t_ab8500_codec_system_context g_ab8500_codec_system_context;
+
+/*--------------------------------------------------------------------------*
+ * Default Values *
+ *--------------------------------------------------------------------------*/
+#define AB8500_CODEC_DEFAULT_SLAVE_ADDRESS_OF_CODEC 0xD
+#define AB8500_CODEC_DEFAULT_DIRECTION              AB8500_CODEC_DIRECTION_OUT
+
+#define AB8500_CODEC_DEFAULT_MODE_IN                AB8500_CODEC_MODE_VOICE
+#define AB8500_CODEC_DEFAULT_MODE_OUT               AB8500_CODEC_MODE_VOICE
+
+#define AB8500_CODEC_DEFAULT_INPUT_SRC              AB8500_CODEC_SRC_MICROPHONE_1A
+#define AB8500_CODEC_DEFAULT_OUTPUT_DEST            AB8500_CODEC_DEST_HEADSET
+
+#define AB8500_CODEC_DEFAULT_VOLUME_LEFT_IN         75
+#define AB8500_CODEC_DEFAULT_VOLUME_RIGHT_IN        75
+#define AB8500_CODEC_DEFAULT_VOLUME_LEFT_OUT        75
+#define AB8500_CODEC_DEFAULT_VOLUME_RIGHT_OUT       75
+
+/*---------------------------------------------------------------------
+ * PRIVATE APIs
+ *--------------------------------------------------------------------*/
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch1(IN
+								  t_ab8500_codec_slot
+								  ad_slot,
+								  IN
+								  t_ab8500_codec_cr31_to_cr46_ad_data_allocation
+								  value);
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch2(IN
+								  t_ab8500_codec_slot
+								  ad_slot,
+								  IN
+								  t_ab8500_codec_cr31_to_cr46_ad_data_allocation
+								  value);
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch3(IN
+								  t_ab8500_codec_slot
+								  ad_slot,
+								  IN
+								  t_ab8500_codec_cr31_to_cr46_ad_data_allocation
+								  value);
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch4(IN
+								  t_ab8500_codec_slot
+								  ad_slot,
+								  IN
+								  t_ab8500_codec_cr31_to_cr46_ad_data_allocation
+								  value);
+PRIVATE t_ab8500_codec_error ab8500_codec_SrcPowerControlSwitch1(IN
+								 t_ab8500_codec_src
+								 src_device,
+								 t_ab8500_codec_src_state
+								 state);
+PRIVATE t_ab8500_codec_error ab8500_codec_SrcPowerControlSwitch2(IN
+								 t_ab8500_codec_src
+								 src_device,
+								 t_ab8500_codec_src_state
+								 state);
+PRIVATE t_ab8500_codec_error ab8500_codec_SetModeAndDirectionUpdateCR(void);
+PRIVATE t_ab8500_codec_error ab8500_codec_SetSrcVolumeUpdateCR(void);
+PRIVATE t_ab8500_codec_error ab8500_codec_SetDestVolumeUpdateCR(void);
+PRIVATE t_ab8500_codec_error ab8500_codec_ProgramDirectionIN(void);
+PRIVATE t_ab8500_codec_error ab8500_codec_ProgramDirectionOUT(void);
+PRIVATE t_ab8500_codec_error ab8500_codec_DestPowerControlUpdateCR(void);
+
+/********************************************************************************************/
+/* Name: ab8500_codec_SingleWrite                                                       */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_SingleWrite(t_uint8 register_offset,
+						      t_uint8 data)
+{
+	return (t_ab8500_codec_error) (AB8500_CODEC_Write
+				       (register_offset, 0x01, &data));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_SingleRead                                                        */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_SingleRead(t_uint8 register_offset,
+						     t_uint8 data)
+{
+	t_uint8 dummy_data = 0xAA;
+
+	return (t_ab8500_codec_error) (AB8500_CODEC_Read
+				       (register_offset, 0x01, &dummy_data,
+					&data));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR0                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR0(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr0_powerup,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR0_POWERUP);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr0_enaana,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR0_ENAANA);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR0, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR1                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR1(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr1_swreset,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR1_SWRESET);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR1, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR2                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR2(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr2_enad1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR2_ENAD1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr2_enad2,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR2_ENAD2);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr2_enad3,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR2_ENAD3);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr2_enad4,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR2_ENAD4);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr2_enad5,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR2_ENAD5);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr2_enad6,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR2_ENAD6);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR2, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR3                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR3(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr3_enda1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR3_ENDA1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr3_enda2,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR3_ENDA2);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr3_enda3,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR3_ENDA3);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr3_enda4,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR3_ENDA4);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr3_enda5,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR3_ENDA5);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr3_enda6,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR3_ENDA6);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR3, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR4                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR4(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr4_lowpowhs,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR4_LOWPOWHS);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr4_lowpowdachs,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR4_LOWPOWDACHS);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr4_lowpowear,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR4_LOWPOWEAR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr4_ear_sel_cm,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR4_EAR_SEL_CM);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr4_hs_hp_dis,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR4_HS_HP_DIS);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr4_ear_hp_dis,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR4_EAR_HP_DIS);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR4, value));
+}
+#endif
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR5                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR5(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr5_enmic1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR5_ENMIC1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr5_enmic2,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR5_ENMIC2);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr5_enlinl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR5_ENLINL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr5_enlinr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR5_ENLINR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr5_mutmic1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR5_MUTMIC1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr5_mutmic2,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR5_MUTMIC2);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr5_mutlinl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR5_MUTELINL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr5_mutlinr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR5_MUTELINR);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR5, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR6                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR6(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr6_endmic1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR6_ENDMIC1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr6_endmic2,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR6_ENDMIC2);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr6_endmic3,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR6_ENDMIC3);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr6_endmic4,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR6_ENDMIC4);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr6_endmic5,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR6_ENDMIC5);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr6_endmic6,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR6_ENDMIC6);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR6, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR7                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR7(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr7_mic1sel,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR7_MIC1SEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr7_linrsel,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR7_LINRSEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr7_endrvhsl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR7_ENDRVHSL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr7_endrvhsr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR7_ENDRVHSR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr7_enadcmic,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR7_ENADCMIC);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr7_enadclinl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR7_ENADCLINL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr7_enadclinr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR7_ENADCLINR);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR7, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR8                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR8(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr8_cp_dis_pldwn,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR8_CP_DIS_PLDWN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr8_enear,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR8_ENEAR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr8_enhsl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR8_ENHSL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr8_enhsr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR8_ENHSR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr8_enhfl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR8_ENHFL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr8_enhfr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR8_ENHFR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr8_envibl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR8_ENVIBL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr8_envibr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR8_ENVIBR);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR8, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR9                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR9(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr9_endacear,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR9_ENADACEAR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr9_endachsl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR9_ENADACHSL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr9_endachsr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR9_ENADACHSR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr9_endachfl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR9_ENADACHFL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr9_endachfr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR9_ENADACHFR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr9_endacvibl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR9_ENADACVIBL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr9_endacvibr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR9_ENADACVIBR);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR9, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR10                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR10(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr10_muteear,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR10_MUTEEAR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr10_mutehsl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR10_MUTEHSL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr10_mutehsr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR10_MUTEHSR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr10_mutehfl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR10_MUTEHFL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr10_mutehfr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR10_MUTEHFR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr10_mutevibl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR10_MUTEVIBL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr10_mutevibr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR10_MUTEVIBR);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR10, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR11                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR11(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr11_earshortpwd,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR11_ENSHORTPWD);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr11_earshortdis,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR11_EARSHORTDIS);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr11_hslshortdis,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR11_HSLSHORTDIS);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr11_hsrshortdis,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR11_HSRSHORTDIS);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr11_hflshortdis,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR11_HFLSHORTDIS);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr11_hfrshortdis,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR11_HFRSHORTDIS);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr11_viblshortdis,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR11_VIBLSHORTDIS);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr11_vibrshortdis,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR11_VIBRSHORTDIS);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR11, value));
+}
+
+#endif
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR12                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR12(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr12_encphs,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR12_ENCPHS);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr12_hsautotime,
+	     AB8500_CODEC_MASK_THREE_BITS, AB8500_CODEC_CR12_HSAUTOTIME);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr12_hsautoensel,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR12_HSAUTOENSEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr12_hsautoen,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR12_HSAUTOEN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR12, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR13                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR13(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr13_envdet_hthresh,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR13_ENVDET_HTHRESH);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr13_envdet_lthresh,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR13_ENVDET_LTHRESH);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR13, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR14                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR14(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr14_smpslven,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR14_SMPSLVEN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr14_envdetsmpsen,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR14_ENVDETSMPSEN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr14_cplven,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR14_CPLVEN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr14_envdetcpen,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR14_ENVDETCPEN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr14_envet_time,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR14_ENVDET_TIME);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR14, value));
+}
+#endif
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR15                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR15(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr15_pwmtovibl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR15_PWMTOVIBL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr15_pwmtovibr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR15_PWMTOVIBR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr15_pwmlctrl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR15_PWMLCTRL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr15_pwmrctrl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR15_PWMRCTRL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr15_pwmnlctrl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR15_PWMNLCTRL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr15_pwmplctrl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR15_PWMPLCTRL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr15_pwmnrctrl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR15_PWMNRCTRL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr15_pwmprctrl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR15_PWMPRCTRL);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR15, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR16                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR16(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr16_pwmnlpol,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR16_PWMNLPOL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr16_pwmnldutycycle,
+	     AB8500_CODEC_MASK_SEVEN_BITS, AB8500_CODEC_CR16_PWMNLDUTYCYCLE);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR16, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR17                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR17(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr17_pwmplpol,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR17_PWMPLPOL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr17_pwmpldutycycle,
+	     AB8500_CODEC_MASK_SEVEN_BITS, AB8500_CODEC_CR17_PWMLPDUTYCYCLE);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR17, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR18                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR18(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr18_pwmnrpol,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR18_PWMNRPOL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr18_pwmnrdutycycle,
+	     AB8500_CODEC_MASK_SEVEN_BITS, AB8500_CODEC_CR18_PWMNRDUTYCYCLE);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR18, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR19                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR19(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr19_pwmprpol,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR19_PWMPRPOL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr19_pwmprdutycycle,
+	     AB8500_CODEC_MASK_SEVEN_BITS, AB8500_CODEC_CR19_PWMRPDUTYCYCLE);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR19, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR20                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR20(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr20_en_se_mic1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR20_EN_SE_MIC1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr20_mic1_gain,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR20_MIC1_GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR20, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR21                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR21(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr21_en_se_mic2,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR21_EN_SE_MIC2);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr21_mic2_gain,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR21_MIC2_GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR21, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR22                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR22(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr22_hsl_gain,
+	     AB8500_CODEC_MASK_THREE_BITS, AB8500_CODEC_CR22_HSL_GAIN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr22_linl_gain,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR22_LINL_GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR22, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR23                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR23(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr23_hsr_gain,
+	     AB8500_CODEC_MASK_THREE_BITS, AB8500_CODEC_CR23_HSR_GAIN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr23_linr_gain,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR23_LINR_GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR23, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR24                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR24(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr24_lintohsl_gain,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR24_LINTOHSL_GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR24, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR25                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR25(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr25_lintohsr_gain,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR25_LINTOHSR_GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR25, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR26                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR26(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr26_ad1nh,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR26_AD1NH);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr26_ad2nh,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR26_AD2NH);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr26_ad3nh,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR26_AD3NH);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr26_ad4nh,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR26_AD4NH);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr26_ad1_voice,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR26_AD1_VOICE);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr26_ad2_voice,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR26_AD2_VOICE);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr26_ad3_voice,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR26_AD3_VOICE);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr26_ad4_voice,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR26_AD4_VOICE);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR26, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR27                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR27(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr27_en_mastgen,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR27_EN_MASTGEN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr27_if1_bitclk_osr,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR27_IF1_BITCLK_OSR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr27_enfs_bitclk1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR27_ENFS_BITCLK1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr27_if0_bitclk_osr,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR27_IF0_BITCLK_OSR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr27_enfs_bitclk0,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR27_ENFS_BITCLK0);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR27, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR28                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR28(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr28_fsync0p,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR28_FSYNC0P);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr28_bitclk0p,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR28_BITCLK0P);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr28_if0del,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR28_IF0DEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr28_if0format,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR28_IF0FORMAT);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr28_if0wl,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR28_IF0WL);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR28, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR29                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR29(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr29_if0datoif1ad,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR29_IF0DATOIF1AD);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr29_if0cktoif1ck,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR29_IF0CKTOIF1CK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr29_if1master,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR29_IF1MASTER);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr29_if1datoif0ad,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR29_IF1DATOIF0AD);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr29_if1cktoif0ck,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR29_IF1CKTOIF0CK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr29_if0master,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR29_IF0MASTER);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr29_if0bfifoen,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR29_IF0BFIFOEN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR29, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR30                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR30(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr30_fsync1p,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR30_FSYNC1P);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr30_bitclk1p,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR30_BITCLK1P);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr30_if1del,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR30_IF1DEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr30_if1format,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR30_IF1FORMAT);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr30_if1wl,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR30_IF1WL);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR30, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR31                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR31(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr31_adotoslot1,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR31_ADOTOSLOT1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr31_adotoslot0,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR31_ADOTOSLOT0);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR31, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR32                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR32(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr32_adotoslot3,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR32_ADOTOSLOT3);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr32_adotoslot2,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR32_ADOTOSLOT2);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR32, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR33                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR33(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr33_adotoslot5,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR33_ADOTOSLOT5);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr33_adotoslot4,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR33_ADOTOSLOT4);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR33, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR34                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR34(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr34_adotoslot7,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR34_ADOTOSLOT7);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr34_adotoslot6,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR34_ADOTOSLOT6);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR34, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR35                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR35(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr35_adotoslot9,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR35_ADOTOSLOT9);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr35_adotoslot8,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR35_ADOTOSLOT8);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR35, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR36                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR36(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr36_adotoslot11,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR36_ADOTOSLOT11);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr36_adotoslot10,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR36_ADOTOSLOT10);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR36, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR37                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR37(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr37_adotoslot13,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR37_ADOTOSLOT13);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr37_adotoslot12,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR37_ADOTOSLOT12);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR37, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR38                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR38(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr38_adotoslot15,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR38_ADOTOSLOT15);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr38_adotoslot14,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR38_ADOTOSLOT14);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR38, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR39                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR39(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr39_adotoslot17,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR39_ADOTOSLOT17);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr39_adotoslot16,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR39_ADOTOSLOT16);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR39, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR40                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR40(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr40_adotoslot19,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR40_ADOTOSLOT19);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr40_adotoslot18,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR40_ADOTOSLOT18);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR40, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR41                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR41(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr41_adotoslot21,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR41_ADOTOSLOT21);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr41_adotoslot20,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR41_ADOTOSLOT20);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR41, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR42                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR42(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr42_adotoslot23,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR42_ADOTOSLOT23);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr42_adotoslot22,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR42_ADOTOSLOT22);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR42, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR43                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR43(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr43_adotoslot25,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR43_ADOTOSLOT25);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr43_adotoslot24,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR43_ADOTOSLOT24);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR43, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR44                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR44(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr44_adotoslot27,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR44_ADOTOSLOT27);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr44_adotoslot26,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR44_ADOTOSLOT26);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR44, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR45                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR45(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr45_adotoslot29,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR45_ADOTOSLOT29);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr45_adotoslot28,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR45_ADOTOSLOT28);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR45, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR46                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR46(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr46_adotoslot31,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR46_ADOTOSLOT31);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr46_adotoslot30,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR46_ADOTOSLOT30);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR46, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR47                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR47(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr47_hiz_sl7,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR47_HIZ_SL7);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr47_hiz_sl6,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR47_HIZ_SL6);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr47_hiz_sl5,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR47_HIZ_SL5);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr47_hiz_sl4,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR47_HIZ_SL4);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr47_hiz_sl3,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR47_HIZ_SL3);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr47_hiz_sl2,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR47_HIZ_SL2);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr47_hiz_sl1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR47_HIZ_SL1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr47_hiz_sl0,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR47_HIZ_SL0);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR47, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR48                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR48(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr48_hiz_sl15,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR48_HIZ_SL15);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr48_hiz_sl14,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR48_HIZ_SL14);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr48_hiz_sl13,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR48_HIZ_SL13);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr48_hiz_sl12,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR48_HIZ_SL12);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr48_hiz_sl11,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR48_HIZ_SL11);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr48_hiz_sl10,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR48_HIZ_SL10);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr48_hiz_sl9,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR48_HIZ_SL9);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr48_hiz_sl8,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR48_HIZ_SL8);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR48, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR49                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR49(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr49_hiz_sl23,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR49_HIZ_SL23);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr49_hiz_sl22,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR49_HIZ_SL22);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr49_hiz_sl21,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR49_HIZ_SL21);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr49_hiz_sl20,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR49_HIZ_SL20);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr49_hiz_sl19,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR49_HIZ_SL19);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr49_hiz_sl18,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR49_HIZ_SL18);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr49_hiz_sl17,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR49_HIZ_SL17);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr49_hiz_sl16,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR49_HIZ_SL16);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR49, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR50                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR50(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr50_hiz_sl31,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR50_HIZ_SL31);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr50_hiz_sl30,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR50_HIZ_SL30);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr50_hiz_sl29,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR50_HIZ_SL29);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr50_hiz_sl28,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR50_HIZ_SL28);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr50_hiz_sl27,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR50_HIZ_SL27);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr50_hiz_sl26,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR50_HIZ_SL26);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr50_hiz_sl25,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR50_HIZ_SL25);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr50_hiz_sl24,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR50_HIZ_SL24);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR50, value));
+}
+
+#endif
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR51                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR51(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr51_da12_voice,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR51_DA12_VOICE);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr51_sldai1toslado1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR51_SLDAI1TOSLADO1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr51_sltoda1,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR51_SLTODA1);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR51, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR52                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR52(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr52_sldai2toslado2,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR52_SLDAI1TOSLADO2);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr52_sltoda2,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR52_SLTODA2);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR52, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR53                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR53(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr53_da34_voice,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR53_DA34_VOICE);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr53_sldai3toslado3,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR53_SLDAI1TOSLADO3);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr53_sltoda3,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR53_SLTODA3);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR53, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR54                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR54(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr54_sldai4toslado4,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR54_SLDAI1TOSLADO4);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr54_sltoda4,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR54_SLTODA4);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR54, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR55                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR55(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr55_da56_voice,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR55_DA56_VOICE);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr55_sldai5toslado5,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR55_SLDAI1TOSLADO5);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr55_sltoda5,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR55_SLTODA5);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR55, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR56                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR56(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr56_sldai6toslado7,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR56_SLDAI1TOSLADO6);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr56_sltoda6,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR56_SLTODA6);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR56, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR57                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR57(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr57_bfifull_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR57_BFIFULL_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr57_bfiempt_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR57_BFIEMPT_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr57_dachan_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR57_DACHAN_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr57_gain_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR57_GAIN_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr57_dspad_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR57_DSPAD_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr57_dspda_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR57_DSPDA_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr57_stfir_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR57_STFIR_MSK);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR57, value));
+}
+
+/* CR58 is Read Only */
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR59                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR59(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr59_vssready_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR59_VSSREADY_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr59_shrtvibl_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR59_SHRTVIBL_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr59_shrtvibr_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR59_SHRTVIBR_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr59_shrthfl_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR59_SHRTHFL_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr59_shrthfr_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR59_SHRTHFR_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr59_shrthsl_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR59_SHRTHSL_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr59_shrthsr_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR59_SHRTHSR_MSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr59_shrtear_msk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR59_SHRTEAR_MSK);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR59, value));
+}
+
+/* CR60 is Read Only */
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR61                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR61(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	/* 5 bits are Read Only */
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr61_fade_speed,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR61_FADE_SPEED);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR61, value));
+}
+#endif
+/* CR62 is Read Only */
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR63                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR63(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr63_datohslen,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR63_DATOHSLEN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr63_datohsren,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR63_DATOHSREN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr63_ad1sel,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR63_AD1SEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr63_ad2sel,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR63_AD2SEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr63_ad3sel,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR63_AD3SEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr63_ad5sel,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR63_AD5SEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr63_ad6sel,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR63_AD6SEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr63_ancsel,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR63_ANCSEL);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR63, value));
+}
+
+#if 0
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR64                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR64(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr64_datohfren,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR64_DATOHFREN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr64_datohflen,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR64_DATOHFLEN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr64_hfrsel,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR64_HFRSEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr64_hflsel,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR64_HFLSEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr64_stfir1sel,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR64_STFIR1SEL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr64_stfir2sel,
+	     AB8500_CODEC_MASK_TWO_BITS, AB8500_CODEC_CR64_STFIR2SEL);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR64, value));
+}
+
+#endif
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR65                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR65(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr65_fadedis_ad1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR65_FADEDIS_AD1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr65_ad1gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR65_AD1GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR65, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR66                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR66(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr66_fadedis_ad2,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR66_FADEDIS_AD2);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr66_ad2gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR66_AD2GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR66, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR67                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR67(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr67_fadedis_ad3,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR67_FADEDIS_AD3);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr67_ad3gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR67_AD3GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR67, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR68                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR68(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr68_fadedis_ad4,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR68_FADEDIS_AD4);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr68_ad4gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR68_AD4GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR68, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR69                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR69(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr69_fadedis_ad5,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR69_FADEDIS_AD5);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr69_ad5gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR69_AD5GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR69, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR70                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR70(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr70_fadedis_ad6,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR70_FADEDIS_AD6);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr70_ad6gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR70_AD6GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR70, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR71                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR71(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr71_fadedis_da1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR71_FADEDIS_DA1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr71_da1gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR71_DA1GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR71, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR72                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR72(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr72_fadedis_da2,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR72_FADEDIS_DA2);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr72_da2gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR72_DA2GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR72, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR73                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR73(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr73_fadedis_da3,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR73_FADEDIS_DA3);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr73_da3gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR73_DA3GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR73, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR74                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR74(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr74_fadedis_da4,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR74_FADEDIS_DA4);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr74_da4gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR74_DA4GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR74, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR75                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR75(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr75_fadedis_da5,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR75_FADEDIS_DA5);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr75_da5gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR75_DA5GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR75, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR76                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR76(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr76_fadedis_da6,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR76_FADEDIS_DA6);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr76_da6gain,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR76_DA6GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR76, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR77                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR77(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr77_fadedis_ad1l,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR77_FADEDIS_AD1L);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr77_ad1lbgain_to_hfl,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR77_AD1LBGAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR77, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR78                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR78(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr78_fadedis_ad2l,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR78_FADEDIS_AD2L);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr78_ad2lbgain_to_hfr,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR78_AD2LBGAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR78, value));
+}
+#endif
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR79                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR79(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr79_hssinc1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR79_HSSINC1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr79_fadedis_hsl,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR79_FADEDIS_HSL);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr79_hsldgain,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR79_HSLDGAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR79, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR80                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR80(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr80_fadedis_hsr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR80_FADEDIS_HSR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr80_hsrdgain,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR80_HSRDGAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR80, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR81                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR81(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr81_stfir1gain,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR81_STFIR1GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR81, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR82                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR82(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr82_stfir2gain,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR82_STFIR2GAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR82, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR83                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR83(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr83_enanc,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR83_ENANC);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr83_anciirinit,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR83_ANCIIRINIT);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr83_ancfirupdate,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR83_ANCFIRUPDATE);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR83, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR84                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR84(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr84_ancinshift,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR84_ANCINSHIFT);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR84, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR85                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR85(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr85_ancfiroutshift,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR85_ANCFIROUTSHIFT);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR85, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR86                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR86(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr86_ancshiftout,
+	     AB8500_CODEC_MASK_FIVE_BITS, AB8500_CODEC_CR86_ANCSHIFTOUT);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR86, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR87                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR87(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr87_ancfircoeff_msb,
+	     AB8500_CODEC_MASK_EIGHT_BITS, AB8500_CODEC_CR87_ANCFIRCOEFF_MSB);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR87, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR88                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR88(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr88_ancfircoeff_lsb,
+	     AB8500_CODEC_MASK_EIGHT_BITS, AB8500_CODEC_CR88_ANCFIRCOEFF_LSB);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR88, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR89                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR89(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr89_anciircoeff_msb,
+	     AB8500_CODEC_MASK_EIGHT_BITS, AB8500_CODEC_CR89_ANCIIRCOEFF_MSB);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR89, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR90                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR90(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr90_anciircoeff_lsb,
+	     AB8500_CODEC_MASK_EIGHT_BITS, AB8500_CODEC_CR90_ANCIIRCOEFF_LSB);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR90, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR91                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR91(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr91_ancwarpdel_msb,
+	     AB8500_CODEC_MASK_EIGHT_BITS, AB8500_CODEC_CR91_ANCWARPDEL_MSB);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR91, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR92                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR92(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr92_ancwarpdel_lsb,
+	     AB8500_CODEC_MASK_EIGHT_BITS, AB8500_CODEC_CR92_ANCWARPDEL_LSB);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR92, value));
+}
+
+/* CR93 is Read Only */
+/* CR94 is Read Only */
+/* CR95 is Read Only */
+/* CR96 is Read Only */
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR97                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR97(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr97_stfir_set,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR97_STFIR_SET);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr97_stfir_addr,
+	     AB8500_CODEC_MASK_SEVEN_BITS, AB8500_CODEC_CR97_STFIR_ADDR);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR97, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR98                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR98(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr98_stfir_coeff_msb,
+	     AB8500_CODEC_MASK_EIGHT_BITS, AB8500_CODEC_CR98_STFIR_COEFF_MSB);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR98, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR99                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR99(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr99_stfir_coeff_lsb,
+	     AB8500_CODEC_MASK_EIGHT_BITS, AB8500_CODEC_CR99_STFIR_COEFF_LSB);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR99, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR100                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR100(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr100_enstfirs,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR100_ENSTFIRS);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr100_stfirstoif1,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR100_STFIRSTOIF1);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr100_stfir_busy,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR100_STFIR_BUSY);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR100, value));
+}
+
+#endif
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR101                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR101(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr101_parlhf,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR101_PARLHF);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr101_parlvib,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR101_PARLVIB);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr101_classd_viblswapen,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR101_CLASSDVIBLSWAPEN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr101_classd_vibrswapen,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR101_CLASSDVIBRSWAPEN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr101_classd_hflswapen,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR101_CLASSDHFLSWAPEN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr101_classd_hfrswapen,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR101_CLASSDHFRSWAPEN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR101, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR102                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR102(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr102_classd_firbyp,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR102_CLASSD_FIRBYP);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr102_classd_highvolen,
+	     AB8500_CODEC_MASK_FOUR_BITS, AB8500_CODEC_CR102_CLASSD_HIGHVOLEN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR102, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR103                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR103(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr103_classd_ditherhpgain,
+	     AB8500_CODEC_MASK_FOUR_BITS,
+	     AB8500_CODEC_CR103_CLASSD_DITHERHPGAIN);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr103_classd_ditherwgain,
+	     AB8500_CODEC_MASK_FOUR_BITS,
+	     AB8500_CODEC_CR103_CLASSD_DITHERWGAIN);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR103, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR104                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR104(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr104_bfifoint,
+	     AB8500_CODEC_MASK_SIX_BITS, AB8500_CODEC_CR104_BFIFOINT);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR104, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR105                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR105(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr105_bfifotx,
+	     AB8500_CODEC_MASK_EIGHT_BITS, AB8500_CODEC_CR105_BFIFOTX);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR105, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR106                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR106(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr106_bfifofsext,
+	     AB8500_CODEC_MASK_THREE_BITS, AB8500_CODEC_CR106_BFIFOFSEXT);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr106_bfifomsk,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR106_BFIFOMSK);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr106_bfifomstr,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR106_BFIFOMSTR);
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr106_bfifostrt,
+	     AB8500_CODEC_MASK_ONE_BIT, AB8500_CODEC_CR106_BFIFOSTRT);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR106, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR107                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR107(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr107_bfifosampnr,
+	     AB8500_CODEC_MASK_EIGHT_BITS, AB8500_CODEC_CR107_BFIFOSAMPNR);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR107, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR108                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR108(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr108_bfifowakeup,
+	     AB8500_CODEC_MASK_EIGHT_BITS, AB8500_CODEC_CR108_BFIFOWAKEUP);
+
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR108, value));
+}
+
+/* CR109 is Read Only */
+/********************************************************************************************/
+/* Name: ab8500_codec_Reset()                                                              */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_Reset(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	p_ab8500_codec_configuration->cr1_swreset =
+	    AB8500_CODEC_CR1_SWRESET_ENABLED;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR1();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ProgramDirection(IN
+							   t_ab8500_codec_direction
+							   ab8500_codec_direction)
+{				/*only IN or OUT must be passed (not INOUT) */
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+
+	if (AB8500_CODEC_DIRECTION_IN == ab8500_codec_direction) {
+		ab8500_codec_error = ab8500_codec_ProgramDirectionIN();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+	}
+
+	if (AB8500_CODEC_DIRECTION_OUT == ab8500_codec_direction) {
+		ab8500_codec_error = ab8500_codec_ProgramDirectionOUT();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+	}
+
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SetDirection(IN
+						       t_ab8500_codec_direction
+						       ab8500_codec_direction)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	switch (ab8500_codec_direction) {
+	case AB8500_CODEC_DIRECTION_IN:
+		ab8500_codec_error =
+		    ab8500_codec_ProgramDirection(AB8500_CODEC_DIRECTION_IN);
+		break;
+
+	case AB8500_CODEC_DIRECTION_OUT:
+		ab8500_codec_error =
+		    ab8500_codec_ProgramDirection(AB8500_CODEC_DIRECTION_OUT);
+		break;
+
+	case AB8500_CODEC_DIRECTION_INOUT:
+		ab8500_codec_error =
+		    ab8500_codec_ProgramDirection(AB8500_CODEC_DIRECTION_IN);
+		if (AB8500_CODEC_OK == ab8500_codec_error) {
+			ab8500_codec_error =
+			    ab8500_codec_ProgramDirection
+			    (AB8500_CODEC_DIRECTION_OUT);
+		}
+		break;
+	}
+
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR5();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR6();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR7();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR8();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR9();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR10();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR12();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR15();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR63();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_Init                                                  */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Initialize the global variables & stores the slave address of codec.     */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* slave_address_of_ab8500_codec: Audio codec slave address                 */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* Returns AB8500_CODEC_OK                                                  */
+/* COMMENTS:                                                                */
+/* 1) Saves the supplied slave_address_of_codec in global variable          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_Init(IN t_uint8
+					      slave_address_of_ab8500_codec)
+{
+	DBGENTER1(" (%lx)", slave_address_of_ab8500_codec);
+
+	g_ab8500_codec_system_context.slave_address_of_ab8500_codec =
+	    slave_address_of_ab8500_codec;
+
+	DBGEXIT(AB8500_CODEC_OK);
+	return (AB8500_CODEC_OK);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_Reset                                                 */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Reset the global variables and clear audiocodec settings to default.     */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_Reset(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+
+	DBGENTER();
+
+	g_ab8500_codec_system_context.ab8500_codec_direction =
+	    AB8500_CODEC_DEFAULT_DIRECTION;
+	g_ab8500_codec_system_context.ab8500_codec_mode_in =
+	    AB8500_CODEC_DEFAULT_MODE_IN;
+	g_ab8500_codec_system_context.ab8500_codec_mode_out =
+	    AB8500_CODEC_DEFAULT_MODE_OUT;
+
+	g_ab8500_codec_system_context.ab8500_codec_src =
+	    AB8500_CODEC_DEFAULT_INPUT_SRC;
+	g_ab8500_codec_system_context.ab8500_codec_dest =
+	    AB8500_CODEC_DEFAULT_OUTPUT_DEST;
+
+	g_ab8500_codec_system_context.in_left_volume =
+	    AB8500_CODEC_DEFAULT_VOLUME_LEFT_IN;
+	g_ab8500_codec_system_context.in_right_volume =
+	    AB8500_CODEC_DEFAULT_VOLUME_RIGHT_IN;
+	g_ab8500_codec_system_context.out_left_volume =
+	    AB8500_CODEC_DEFAULT_VOLUME_LEFT_OUT;
+	g_ab8500_codec_system_context.out_right_volume =
+	    AB8500_CODEC_DEFAULT_VOLUME_RIGHT_OUT;
+
+	ab8500_codec_error = ab8500_codec_Reset();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetModeAndDirection                                   */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Configures the whole audio codec to work in audio mode                   */
+/* (using I2S protocol).                                                    */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* direction: select the direction (IN, OUT or INOUT)                       */
+/* in_mode: codec mode for recording. If direction is OUT only,             */
+/* this parameter is ignored.                                               */
+/* out_mode: codec mode for playing. If direction is IN only,               */
+/* this parameter is ignored.                                               */
+/* p_tdm_config: TDM configuration required to be configured by user        */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_UNSUPPORTED_FEATURE: The API may not allow setting          */
+/* 2 different modes, in which case it should return this value.            */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetModeAndDirection
+    (IN t_ab8500_codec_direction ab8500_codec_direction,
+     IN t_ab8500_codec_mode ab8500_codec_mode_in,
+     IN t_ab8500_codec_mode ab8500_codec_mode_out,
+     IN t_ab8500_codec_tdm_config const *const p_tdm_config) {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	DBGENTER3(" (%lx %lx %lx)", ab8500_codec_direction,
+		  ab8500_codec_mode_in, ab8500_codec_mode_out);
+
+	if (AB8500_CODEC_AUDIO_INTERFACE_1 ==
+	    g_ab8500_codec_system_context.audio_interface) {
+		if (AB8500_CODEC_DIRECTION_OUT == ab8500_codec_direction
+		    || AB8500_CODEC_DIRECTION_INOUT == ab8500_codec_direction) {
+			p_ab8500_codec_configuration->cr3_enda1 =
+			    AB8500_CODEC_CR3_ENDA1_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda2 =
+			    AB8500_CODEC_CR3_ENDA2_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda3 =
+			    AB8500_CODEC_CR3_ENDA3_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda4 =
+			    AB8500_CODEC_CR3_ENDA4_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda5 =
+			    AB8500_CODEC_CR3_ENDA5_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda6 =
+			    AB8500_CODEC_CR3_ENDA6_ENABLED;
+
+			p_ab8500_codec_configuration->cr27_if1_bitclk_osr =
+			    p_tdm_config->cr27_if1_bitclk_osr;
+
+			if (AB8500_CODEC_MODE_HIFI == ab8500_codec_mode_out) {
+				p_ab8500_codec_configuration->cr30_fsync1p =
+				    AB8500_CODEC_CR30_FSYNC1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_bitclk1p =
+				    AB8500_CODEC_CR30_BITCLK1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_if1del =
+				    AB8500_CODEC_CR30_IF1DEL_DELAYED;
+				p_ab8500_codec_configuration->cr30_if1format =
+				    AB8500_CODEC_CR30_IF1FORMAT_I2S_LEFTALIGNED;
+				p_ab8500_codec_configuration->cr30_if1wl =
+				    p_tdm_config->cr30_if1wl;
+			} else {
+				p_ab8500_codec_configuration->cr30_fsync1p =
+				    AB8500_CODEC_CR30_FSYNC1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_bitclk1p =
+				    AB8500_CODEC_CR30_BITCLK1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_if1del =
+				    AB8500_CODEC_CR30_IF1DEL_DELAYED;
+				p_ab8500_codec_configuration->cr30_if1format =
+				    AB8500_CODEC_CR30_IF1FORMAT_TDM;
+				p_ab8500_codec_configuration->cr30_if1wl =
+				    p_tdm_config->cr30_if1wl;
+			}
+		}
+
+		if (AB8500_CODEC_DIRECTION_IN == ab8500_codec_direction
+		    || AB8500_CODEC_DIRECTION_INOUT == ab8500_codec_direction) {
+			p_ab8500_codec_configuration->cr2_enad1 =
+			    AB8500_CODEC_CR2_ENAD1_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad2 =
+			    AB8500_CODEC_CR2_ENAD2_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad3 =
+			    AB8500_CODEC_CR2_ENAD3_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad4 =
+			    AB8500_CODEC_CR2_ENAD4_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad5 =
+			    AB8500_CODEC_CR2_ENAD5_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad6 =
+			    AB8500_CODEC_CR2_ENAD6_ENABLED;
+
+			p_ab8500_codec_configuration->cr27_if1_bitclk_osr =
+			    p_tdm_config->cr27_if1_bitclk_osr;
+
+			if (AB8500_CODEC_MODE_HIFI == ab8500_codec_mode_in) {
+				p_ab8500_codec_configuration->cr30_fsync1p =
+				    AB8500_CODEC_CR30_FSYNC1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_bitclk1p =
+				    AB8500_CODEC_CR30_BITCLK1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_if1del =
+				    AB8500_CODEC_CR30_IF1DEL_DELAYED;
+				p_ab8500_codec_configuration->cr30_if1format =
+				    AB8500_CODEC_CR30_IF1FORMAT_I2S_LEFTALIGNED;
+				p_ab8500_codec_configuration->cr30_if1wl =
+				    p_tdm_config->cr30_if1wl;
+			} else {
+				p_ab8500_codec_configuration->cr30_fsync1p =
+				    AB8500_CODEC_CR30_FSYNC1P_RISING_EDGE;
+				p_ab8500_codec_configuration->cr30_bitclk1p =
+				    AB8500_CODEC_CR30_BITCLK1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_if1del =
+				    AB8500_CODEC_CR30_IF1DEL_NOT_DELAYED;
+				p_ab8500_codec_configuration->cr30_if1format =
+				    AB8500_CODEC_CR30_IF1FORMAT_TDM;
+				p_ab8500_codec_configuration->cr30_if1wl =
+				    p_tdm_config->cr30_if1wl;
+			}
+		}
+	} else {
+		if (AB8500_CODEC_DIRECTION_OUT == ab8500_codec_direction
+		    || AB8500_CODEC_DIRECTION_INOUT == ab8500_codec_direction) {
+			p_ab8500_codec_configuration->cr3_enda1 =
+			    AB8500_CODEC_CR3_ENDA1_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda2 =
+			    AB8500_CODEC_CR3_ENDA2_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda3 =
+			    AB8500_CODEC_CR3_ENDA3_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda4 =
+			    AB8500_CODEC_CR3_ENDA4_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda5 =
+			    AB8500_CODEC_CR3_ENDA5_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda6 =
+			    AB8500_CODEC_CR3_ENDA6_ENABLED;
+
+			p_ab8500_codec_configuration->cr27_if0_bitclk_osr =
+			    p_tdm_config->cr27_if0_bitclk_osr;
+
+			p_ab8500_codec_configuration->cr63_datohslen =
+			    AB8500_CODEC_CR63_DATOHSLEN_ENABLED;
+			p_ab8500_codec_configuration->cr63_datohsren =
+			    AB8500_CODEC_CR63_DATOHSREN_ENABLED;
+
+			if (AB8500_CODEC_MODE_HIFI == ab8500_codec_mode_out) {
+				p_ab8500_codec_configuration->cr28_fsync0p =
+				    AB8500_CODEC_CR28_FSYNC0P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr28_bitclk0p = p_tdm_config->cr28_bitclk0p;	/*AB8500_CODEC_CR28_BITCLK0P_FALLING_EDGE; */
+				p_ab8500_codec_configuration->cr28_if0del = p_tdm_config->cr28_if0del;	/*AB8500_CODEC_CR28_IF0DEL_DELAYED; */
+				p_ab8500_codec_configuration->cr28_if0format =
+				    AB8500_CODEC_CR28_IF0FORMAT_I2S_LEFTALIGNED;
+				p_ab8500_codec_configuration->cr28_if0wl =
+				    p_tdm_config->cr28_if0wl;
+			} else {
+				p_ab8500_codec_configuration->cr28_fsync0p =
+				    AB8500_CODEC_CR28_FSYNC0P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr28_bitclk0p = p_tdm_config->cr28_bitclk0p;	/*AB8500_CODEC_CR28_BITCLK0P_FALLING_EDGE; */
+				p_ab8500_codec_configuration->cr28_if0del = p_tdm_config->cr28_if0del;	/*AB8500_CODEC_CR28_IF0DEL_DELAYED; */
+				p_ab8500_codec_configuration->cr28_if0format =
+				    AB8500_CODEC_CR28_IF0FORMAT_TDM;
+				p_ab8500_codec_configuration->cr28_if0wl =
+				    p_tdm_config->cr28_if0wl;
+			}
+		}
+
+		if (AB8500_CODEC_DIRECTION_IN == ab8500_codec_direction
+		    || AB8500_CODEC_DIRECTION_INOUT == ab8500_codec_direction) {
+			p_ab8500_codec_configuration->cr2_enad1 =
+			    AB8500_CODEC_CR2_ENAD1_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad2 =
+			    AB8500_CODEC_CR2_ENAD2_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad3 =
+			    AB8500_CODEC_CR2_ENAD3_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad4 =
+			    AB8500_CODEC_CR2_ENAD4_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad5 =
+			    AB8500_CODEC_CR2_ENAD5_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad6 =
+			    AB8500_CODEC_CR2_ENAD6_ENABLED;
+
+			p_ab8500_codec_configuration->cr26_ad1_voice =
+			    AB8500_CODEC_CR26_AD1_VOICE_LOWLATENCYFILTER;
+			p_ab8500_codec_configuration->cr26_ad2_voice =
+			    AB8500_CODEC_CR26_AD2_VOICE_LOWLATENCYFILTER;
+			p_ab8500_codec_configuration->cr26_ad3_voice =
+			    AB8500_CODEC_CR26_AD3_VOICE_LOWLATENCYFILTER;
+			p_ab8500_codec_configuration->cr26_ad4_voice =
+			    AB8500_CODEC_CR26_AD4_VOICE_LOWLATENCYFILTER;
+
+			p_ab8500_codec_configuration->cr27_if0_bitclk_osr =
+			    p_tdm_config->cr27_if0_bitclk_osr;
+
+			if (AB8500_CODEC_MODE_HIFI == ab8500_codec_mode_in) {
+				p_ab8500_codec_configuration->cr28_fsync0p =
+				    AB8500_CODEC_CR28_FSYNC0P_RISING_EDGE;
+				p_ab8500_codec_configuration->cr28_bitclk0p = p_tdm_config->cr28_bitclk0p;	/*AB8500_CODEC_CR28_BITCLK0P_RISING_EDGE; */
+				p_ab8500_codec_configuration->cr28_if0del = p_tdm_config->cr28_if0del;	/*AB8500_CODEC_CR28_IF0DEL_NOT_DELAYED; */
+				p_ab8500_codec_configuration->cr28_if0format =
+				    AB8500_CODEC_CR28_IF0FORMAT_I2S_LEFTALIGNED;
+				p_ab8500_codec_configuration->cr28_if0wl =
+				    p_tdm_config->cr28_if0wl;
+			} else {
+				p_ab8500_codec_configuration->cr28_fsync0p =
+				    AB8500_CODEC_CR28_FSYNC0P_RISING_EDGE;
+				p_ab8500_codec_configuration->cr28_bitclk0p = p_tdm_config->cr28_bitclk0p;	/*AB8500_CODEC_CR28_BITCLK0P_FALLING_EDGE; */
+				p_ab8500_codec_configuration->cr28_if0del = p_tdm_config->cr28_if0del;	/*AB8500_CODEC_CR28_IF0DEL_NOT_DELAYED; */
+				p_ab8500_codec_configuration->cr28_if0format =
+				    AB8500_CODEC_CR28_IF0FORMAT_TDM;
+				p_ab8500_codec_configuration->cr28_if0wl =
+				    p_tdm_config->cr28_if0wl;
+			}
+		}
+	}
+
+	ab8500_codec_error = ab8500_codec_SetModeAndDirectionUpdateCR();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	g_ab8500_codec_system_context.ab8500_codec_direction =
+	    ab8500_codec_direction;
+	g_ab8500_codec_system_context.ab8500_codec_mode_in =
+	    ab8500_codec_mode_in;
+	g_ab8500_codec_system_context.ab8500_codec_mode_out =
+	    ab8500_codec_mode_out;
+
+	ab8500_codec_error = ab8500_codec_SetDirection(ab8500_codec_direction);
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetSrcVolume                                          */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Sets the record volumes.                                                 */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* t_ab8500_codec_src: select source device for recording.                  */
+/* in_left_volume: record volume for left channel.                          */
+/* in_right_volume: record volume for right channel.                        */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetSrcVolume
+    (IN t_ab8500_codec_src src_device,
+     IN t_uint8 in_left_volume, IN t_uint8 in_right_volume) {
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER3(" (%lx %lx %lx)", src_device, in_left_volume,
+		  in_right_volume);
+
+	if (in_left_volume > AB8500_CODEC_MAX_VOLUME) {
+		in_left_volume = AB8500_CODEC_MAX_VOLUME;
+	}
+
+	if (in_right_volume > AB8500_CODEC_MAX_VOLUME) {
+		in_right_volume = AB8500_CODEC_MAX_VOLUME;
+	}
+
+	g_ab8500_codec_system_context.in_left_volume = in_left_volume;
+	g_ab8500_codec_system_context.in_right_volume = in_right_volume;
+
+	p_ab8500_codec_configuration->cr65_ad1gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr66_ad2gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr67_ad3gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr68_ad4gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr69_ad5gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr70_ad6gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+
+	/* Set mininimum volume if volume is zero */
+	switch (src_device) {
+	case AB8500_CODEC_SRC_LINEIN:
+		p_ab8500_codec_configuration->cr22_linl_gain =
+		    AB8500_CODEC_LINEIN_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_LINEIN_VOLUME_MAX -
+		      AB8500_CODEC_LINEIN_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr23_linr_gain =
+		    AB8500_CODEC_LINEIN_VOLUME_MIN +
+		    (in_right_volume *
+		     (AB8500_CODEC_LINEIN_VOLUME_MAX -
+		      AB8500_CODEC_LINEIN_VOLUME_MIN)) / 100;
+		break;
+
+	case AB8500_CODEC_SRC_MICROPHONE_1A:
+	case AB8500_CODEC_SRC_MICROPHONE_1B:
+		p_ab8500_codec_configuration->cr20_mic1_gain =
+		    AB8500_CODEC_MIC_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_MIC_VOLUME_MAX -
+		      AB8500_CODEC_MIC_VOLUME_MIN)) / 100;
+		break;
+
+	case AB8500_CODEC_SRC_MICROPHONE_2:
+		p_ab8500_codec_configuration->cr21_mic2_gain =
+		    AB8500_CODEC_MIC_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_MIC_VOLUME_MAX -
+		      AB8500_CODEC_MIC_VOLUME_MIN)) / 100;
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_1:
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_2:
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_3:
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_4:
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_5:
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_6:
+		break;
+
+	case AB8500_CODEC_SRC_ALL:
+		p_ab8500_codec_configuration->cr22_linl_gain =
+		    AB8500_CODEC_LINEIN_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_LINEIN_VOLUME_MAX -
+		      AB8500_CODEC_LINEIN_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr23_linr_gain =
+		    AB8500_CODEC_LINEIN_VOLUME_MIN +
+		    (in_right_volume *
+		     (AB8500_CODEC_LINEIN_VOLUME_MAX -
+		      AB8500_CODEC_LINEIN_VOLUME_MIN)) / 100;
+
+		p_ab8500_codec_configuration->cr20_mic1_gain =
+		    AB8500_CODEC_MIC_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_MIC_VOLUME_MAX -
+		      AB8500_CODEC_MIC_VOLUME_MIN)) / 100;
+
+		p_ab8500_codec_configuration->cr21_mic2_gain =
+		    AB8500_CODEC_MIC_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_MIC_VOLUME_MAX -
+		      AB8500_CODEC_MIC_VOLUME_MIN)) / 100;
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_SetSrcVolumeUpdateCR();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetDestVolume                                         */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Sets the play volumes.                                                   */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* out_left_volume: play volume for left channel.                           */
+/* out_right_volume: play volume for right channel.                         */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetDestVolume
+    (IN t_ab8500_codec_dest dest_device,
+     IN t_uint8 out_left_volume, IN t_uint8 out_right_volume) {
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER3(" (%lx %lx %lx)", dest_device, out_left_volume,
+		  out_right_volume);
+
+	if (out_left_volume > AB8500_CODEC_MAX_VOLUME) {
+		out_left_volume = AB8500_CODEC_MAX_VOLUME;
+	}
+
+	if (out_right_volume > AB8500_CODEC_MAX_VOLUME) {
+		out_right_volume = AB8500_CODEC_MAX_VOLUME;
+	}
+
+	g_ab8500_codec_system_context.out_left_volume = out_left_volume;
+	g_ab8500_codec_system_context.out_right_volume = out_right_volume;
+
+	p_ab8500_codec_configuration->cr71_da1gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr72_da2gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr73_da3gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr74_da4gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr75_da5gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr76_da6gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+
+	/* Set mininimum volume if volume is zero */
+	switch (dest_device) {
+	case AB8500_CODEC_DEST_HEADSET:
+		p_ab8500_codec_configuration->cr22_hsl_gain =
+		    AB8500_CODEC_HEADSET_VOLUME_MIN +
+		    (out_left_volume *
+		     (AB8500_CODEC_HEADSET_VOLUME_MAX -
+		      AB8500_CODEC_HEADSET_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr23_hsr_gain =
+		    AB8500_CODEC_HEADSET_VOLUME_MIN +
+		    (out_right_volume *
+		     (AB8500_CODEC_HEADSET_VOLUME_MAX -
+		      AB8500_CODEC_HEADSET_VOLUME_MIN)) / 100;
+
+		p_ab8500_codec_configuration->cr79_hsldgain =
+		    AB8500_CODEC_HEADSET_D_VOLUME_0DB;
+		p_ab8500_codec_configuration->cr80_hsrdgain =
+		    AB8500_CODEC_HEADSET_D_VOLUME_0DB;
+		break;
+
+	case AB8500_CODEC_DEST_EARPIECE:
+		p_ab8500_codec_configuration->cr79_hsldgain =
+		    AB8500_CODEC_HEADSET_D_VOLUME_0DB;
+		break;
+
+	case AB8500_CODEC_DEST_HANDSFREE:
+
+		p_ab8500_codec_configuration->cr101_parlhf =
+		    AB8500_CODEC_CR101_PARLHF_INDEPENDENT;
+		p_ab8500_codec_configuration->cr101_parlvib =
+		    AB8500_CODEC_CR101_PARLVIB_INDEPENDENT;
+		p_ab8500_codec_configuration->cr101_classd_viblswapen =
+		    AB8500_CODEC_CR101_CLASSD_VIBLSWAPEN_DISABLED;
+		p_ab8500_codec_configuration->cr101_classd_vibrswapen =
+		    AB8500_CODEC_CR101_CLASSD_VIBRSWAPEN_DISABLED;
+		p_ab8500_codec_configuration->cr101_classd_hflswapen =
+		    AB8500_CODEC_CR101_CLASSD_HFLSWAPEN_DISABLED;
+		p_ab8500_codec_configuration->cr101_classd_hfrswapen =
+		    AB8500_CODEC_CR101_CLASSD_HFRSWAPEN_DISABLED;
+
+		p_ab8500_codec_configuration->cr102_classd_firbyp =
+		    AB8500_CODEC_CR102_CLASSD_FIRBYP_ALL_ENABLED;
+		p_ab8500_codec_configuration->cr102_classd_highvolen =
+		    AB8500_CODEC_CR102_CLASSD_HIGHVOLEN_DISABLED;
+
+		p_ab8500_codec_configuration->cr103_classd_ditherhpgain = 0x8;
+		p_ab8500_codec_configuration->cr103_classd_ditherwgain = 0x4;
+
+		break;
+
+	case AB8500_CODEC_DEST_VIBRATOR_L:
+		p_ab8500_codec_configuration->cr16_pwmnldutycycle =
+		    AB8500_CODEC_VIBRATOR_VOLUME_MIN;
+		p_ab8500_codec_configuration->cr17_pwmpldutycycle =
+		    AB8500_CODEC_VIBRATOR_VOLUME_MIN +
+		    (out_right_volume *
+		     (AB8500_CODEC_VIBRATOR_VOLUME_MAX -
+		      AB8500_CODEC_VIBRATOR_VOLUME_MIN)) / 100;
+		break;
+
+	case AB8500_CODEC_DEST_VIBRATOR_R:
+		p_ab8500_codec_configuration->cr18_pwmnrdutycycle =
+		    AB8500_CODEC_VIBRATOR_VOLUME_MIN;
+		p_ab8500_codec_configuration->cr19_pwmprdutycycle =
+		    AB8500_CODEC_VIBRATOR_VOLUME_MIN +
+		    (out_right_volume *
+		     (AB8500_CODEC_VIBRATOR_VOLUME_MAX -
+		      AB8500_CODEC_VIBRATOR_VOLUME_MIN)) / 100;
+		break;
+
+	case AB8500_CODEC_DEST_ALL:
+		p_ab8500_codec_configuration->cr22_hsl_gain =
+		    AB8500_CODEC_HEADSET_VOLUME_MIN +
+		    (out_left_volume *
+		     (AB8500_CODEC_HEADSET_VOLUME_MAX -
+		      AB8500_CODEC_HEADSET_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr23_hsr_gain =
+		    AB8500_CODEC_HEADSET_VOLUME_MIN +
+		    (out_right_volume *
+		     (AB8500_CODEC_HEADSET_VOLUME_MAX -
+		      AB8500_CODEC_HEADSET_VOLUME_MIN)) / 100;
+
+		p_ab8500_codec_configuration->cr79_hsldgain =
+		    AB8500_CODEC_HEADSET_D_VOLUME_0DB;
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_SetDestVolumeUpdateCR();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetMasterMode                                         */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Set the Audio Codec in Master mode.                                      */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN: t_codec_master_mode: Enable/disable master mode                      */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK: Call this API after calling AB8500_CODEC_SetModeAndDirection() API*/
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetMasterMode(IN
+						       t_ab8500_codec_master_mode
+						       mode)
+{
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER0();
+
+	if (AB8500_CODEC_AUDIO_INTERFACE_1 ==
+	    g_ab8500_codec_system_context.audio_interface) {
+
+		p_ab8500_codec_configuration->cr27_en_mastgen =
+		    AB8500_CODEC_CR27_EN_MASTGEN_ENABLED;
+		p_ab8500_codec_configuration->cr27_enfs_bitclk1 =
+		    AB8500_CODEC_CR27_ENFS_BITCLK1_ENABLED;
+
+		if (AB8500_CODEC_MASTER_MODE_ENABLE == mode) {
+			p_ab8500_codec_configuration->cr29_if1master =
+			    AB8500_CODEC_CR29_IF1MASTER_FS1CK1_OUTPUT;
+		} else {
+			p_ab8500_codec_configuration->cr29_if1master =
+			    AB8500_CODEC_CR29_IF1MASTER_FS1CK1_INPUT;
+		}
+
+	} else {
+
+		p_ab8500_codec_configuration->cr27_en_mastgen =
+		    AB8500_CODEC_CR27_EN_MASTGEN_ENABLED;
+		p_ab8500_codec_configuration->cr27_enfs_bitclk0 =
+		    AB8500_CODEC_CR27_ENFS_BITCLK0_ENABLED;
+
+		if (AB8500_CODEC_MASTER_MODE_ENABLE == mode) {
+			p_ab8500_codec_configuration->cr29_if0master =
+			    AB8500_CODEC_CR29_IF0MASTER_FS0CK0_OUTPUT;
+		} else {
+			p_ab8500_codec_configuration->cr29_if0master =
+			    AB8500_CODEC_CR29_IF0MASTER_FS0CK0_INPUT;
+		}
+
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR27();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR29();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SelectInput                                           */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Select input source for recording.                                       */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* input_src: select input source for recording when several sources        */
+/* are supported in codec.                                                  */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_INVALID_PARAMETER: If input_src provided is invalid         */
+/* by the codec hardware in use.                                            */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SelectInput(IN t_ab8500_codec_src
+						     ab8500_codec_src)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER1(" (%lx)", ab8500_codec_src);
+
+	g_ab8500_codec_system_context.ab8500_codec_src = ab8500_codec_src;
+
+	ab8500_codec_error =
+	    ab8500_codec_SetDirection(AB8500_CODEC_DIRECTION_IN);
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SelectOutput                                          */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Select output desination for playing.                                    */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* output_dest: select output destination for playing when several are      */
+/* supported by codec hardware.                                             */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_INVALID_PARAMETER: If output_src provided is invalid        */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SelectOutput(IN t_ab8500_codec_dest
+						      ab8500_codec_dest)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	g_ab8500_codec_system_context.ab8500_codec_dest = ab8500_codec_dest;
+	DBGENTER1(" (%lx)", ab8500_codec_dest);
+
+	ab8500_codec_error =
+	    ab8500_codec_SetDirection(AB8500_CODEC_DIRECTION_OUT);
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_PowerDown                                             */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Shuts the audio codec down completely.                                   */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* OUT:                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_PowerDown(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+
+	g_ab8500_codec_system_context.ab8500_codec_configuration.cr0_powerup =
+	    AB8500_CODEC_CR0_POWERUP_OFF;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_PowerUp                                               */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Switch on the audio codec.                                               */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* CODEC_OK: if successful.                                                 */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_PowerUp(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+
+	DBGENTER();
+
+	/*g_ab8500_codec_system_context.ab8500_codec_configuration.cr1_swreset = AB8500_CODEC_CR1_SWRESET_ENABLED; Removed by kardad
+	   ab8500_codec_error = ab8500_codec_UpdateCR1();
+	   if (AB8500_CODEC_OK != ab8500_codec_error)
+	   {
+	   return(ab8500_codec_error);
+	   } */
+
+	g_ab8500_codec_system_context.ab8500_codec_configuration.cr0_powerup =
+	    AB8500_CODEC_CR0_POWERUP_ON;
+	g_ab8500_codec_system_context.ab8500_codec_configuration.cr0_enaana =
+	    AB8500_CODEC_CR0_ENAANA_ON;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SelectInterface                                       */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Select the Audio Interface 0 or 1.                                       */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN: t_ab8500_codec_audio_interface: The selected interface               */
+/*                                                                          */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_OK: Always.                                                 */
+/* REMARK: Call this API before using a function of the low level drivers   */
+/* to select the interface that you want to configure                       */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SelectInterface(IN
+							 t_ab8500_codec_audio_interface
+							 audio_interface)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER0();
+
+	g_ab8500_codec_system_context.audio_interface = audio_interface;
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_GetInterface                                          */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Get the Audio Interface 0 or 1.                                          */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT: p_audio_interface: Store the selected interface                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_OK: Always                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Re-Entrant                                                   */
+/* REENTRANCY ISSUES: No Issues                                             */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_GetInterface(OUT
+						      t_ab8500_codec_audio_interface
+						      * p_audio_interface)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER0();
+
+	*p_audio_interface = g_ab8500_codec_system_context.audio_interface;
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetAnalogLoopback                                     */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Sets Line-In to HeadSet loopback with the required gain.                 */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* out_left_volume: play volume for left channel.                           */
+/* out_right_volume: play volume for right channel.                         */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetAnalogLoopback(IN t_uint8
+							   out_left_volume,
+							   IN t_uint8
+							   out_right_volume)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER2(" (%lx %lx)", out_left_volume, out_right_volume);
+
+	if (out_left_volume > AB8500_CODEC_MAX_VOLUME) {
+		out_left_volume = AB8500_CODEC_MAX_VOLUME;
+	}
+
+	if (out_right_volume > AB8500_CODEC_MAX_VOLUME) {
+		out_right_volume = AB8500_CODEC_MAX_VOLUME;
+	}
+
+	g_ab8500_codec_system_context.out_left_volume = out_left_volume;
+	g_ab8500_codec_system_context.out_right_volume = out_right_volume;
+
+	p_ab8500_codec_configuration->cr24_lintohsl_gain =
+	    AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MAX -
+	      AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr25_lintohsr_gain =
+	    AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MIN +
+	    (out_right_volume *
+	     (AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MAX -
+	      AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MIN)) / 100;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR24();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR25();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_RemoveAnalogLoopback                                  */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Remove Line-In to HeadSet loopback.                                      */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_RemoveAnalogLoopback(void)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER0();
+
+	p_ab8500_codec_configuration->cr24_lintohsl_gain =
+	    AB8500_CODEC_LINEIN_TO_HS_L_R_LOOP_OPEN;
+	p_ab8500_codec_configuration->cr25_lintohsr_gain =
+	    AB8500_CODEC_LINEIN_TO_HS_L_R_LOOP_OPEN;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR24();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR25();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_EnableBypassMode                                      */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Enables IF0 to IF1 path or vice versa                                    */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_EnableBypassMode(void)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER0();
+
+	if (AB8500_CODEC_AUDIO_INTERFACE_1 ==
+	    g_ab8500_codec_system_context.audio_interface) {
+		p_ab8500_codec_configuration->cr29_if1datoif0ad =
+		    AB8500_CODEC_CR29_IF1DATOIF0AD_SENT;
+		p_ab8500_codec_configuration->cr29_if1cktoif0ck =
+		    AB8500_CODEC_CR29_IF1CKTOIF0CK_SENT;
+	} else {
+		p_ab8500_codec_configuration->cr29_if0datoif1ad =
+		    AB8500_CODEC_CR29_IF0DATOIF1AD_SENT;
+		p_ab8500_codec_configuration->cr29_if0cktoif1ck =
+		    AB8500_CODEC_CR29_IF0CKTOIF1CK_SENT;
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR29();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_DisableBypassMode                                     */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Disables IF0 to IF1 path or vice versa                                   */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_DisableBypassMode(void)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER0();
+
+	if (AB8500_CODEC_AUDIO_INTERFACE_1 ==
+	    g_ab8500_codec_system_context.audio_interface) {
+		p_ab8500_codec_configuration->cr29_if1datoif0ad =
+		    AB8500_CODEC_CR29_IF1DATOIF0AD_NOTSENT;
+		p_ab8500_codec_configuration->cr29_if1cktoif0ck =
+		    AB8500_CODEC_CR29_IF1CKTOIF0CK_NOTSENT;
+	} else {
+		p_ab8500_codec_configuration->cr29_if0datoif1ad =
+		    AB8500_CODEC_CR29_IF0DATOIF1AD_NOTSENT;
+		p_ab8500_codec_configuration->cr29_if0cktoif1ck =
+		    AB8500_CODEC_CR29_IF0CKTOIF1CK_NOTSENT;
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR29();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SrcPowerControl                                       */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Enables/Disables & UnMute/Mute the desired source                        */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* t_ab8500_codec_src: select source device for enabling/disabling.         */
+/* t_ab8500_codec_src_state: Enable/Disable                                 */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SrcPowerControl(IN t_ab8500_codec_src
+							 src_device,
+							 t_ab8500_codec_src_state
+							 state)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+
+	DBGENTER2(" (%lx %lx)", src_device, state);
+
+	if (src_device <= AB8500_CODEC_SRC_D_MICROPHONE_2) {
+		ab8500_codec_error =
+		    ab8500_codec_SrcPowerControlSwitch1(src_device, state);
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+	} else if (src_device <= AB8500_CODEC_SRC_ALL) {
+		ab8500_codec_error =
+		    ab8500_codec_SrcPowerControlSwitch2(src_device, state);
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+	} else {
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR5();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR6();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR7();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR63();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_DestPowerControl                                      */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Enables/Disables & UnMute/Mute the desired destination                   */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* t_ab8500_codec_dest: select destination device for enabling/disabling.   */
+/* t_ab8500_codec_dest_state: Enable/Disable                                */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_DestPowerControl(IN t_ab8500_codec_dest
+							  dest_device,
+							  t_ab8500_codec_dest_state
+							  state)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER2(" (%lx %lx)", dest_device, state);
+
+	switch (dest_device) {
+	case AB8500_CODEC_DEST_HEADSET:
+
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr8_enhsl =
+			    AB8500_CODEC_CR8_ENHSL_ENABLED;
+			p_ab8500_codec_configuration->cr8_enhsr =
+			    AB8500_CODEC_CR8_ENHSR_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutehsl =
+			    AB8500_CODEC_CR10_MUTEHSL_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutehsr =
+			    AB8500_CODEC_CR10_MUTEHSR_DISABLED;
+		} else {
+			p_ab8500_codec_configuration->cr8_enhsl =
+			    AB8500_CODEC_CR8_ENHSL_DISABLED;
+			p_ab8500_codec_configuration->cr8_enhsr =
+			    AB8500_CODEC_CR8_ENHSR_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutehsl =
+			    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutehsr =
+			    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+		}
+		break;
+
+	case AB8500_CODEC_DEST_EARPIECE:
+
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr8_enear =
+			    AB8500_CODEC_CR8_ENEAR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endacear =
+			    AB8500_CODEC_CR9_ENDACEAR_ENABLED;
+			p_ab8500_codec_configuration->cr10_muteear =
+			    AB8500_CODEC_CR10_MUTEEAR_DISABLED;
+		} else {
+			p_ab8500_codec_configuration->cr8_enear =
+			    AB8500_CODEC_CR8_ENEAR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endacear =
+			    AB8500_CODEC_CR9_ENDACEAR_DISABLED;
+			p_ab8500_codec_configuration->cr10_muteear =
+			    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+		}
+		break;
+
+	case AB8500_CODEC_DEST_HANDSFREE:
+
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr8_enhfl =
+			    AB8500_CODEC_CR8_ENHFL_ENABLED;
+			p_ab8500_codec_configuration->cr8_enhfr =
+			    AB8500_CODEC_CR8_ENHFR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endachfl =
+			    AB8500_CODEC_CR9_ENDACHFL_ENABLED;
+			p_ab8500_codec_configuration->cr9_endachfr =
+			    AB8500_CODEC_CR9_ENDACHFR_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutehfl =
+			    AB8500_CODEC_CR10_MUTEHFL_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutehfr =
+			    AB8500_CODEC_CR10_MUTEHFR_DISABLED;
+		} else {
+			p_ab8500_codec_configuration->cr8_enhfl =
+			    AB8500_CODEC_CR8_ENHFL_DISABLED;
+			p_ab8500_codec_configuration->cr8_enhfr =
+			    AB8500_CODEC_CR8_ENHFR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endachfl =
+			    AB8500_CODEC_CR9_ENDACHFL_DISABLED;
+			p_ab8500_codec_configuration->cr9_endachfr =
+			    AB8500_CODEC_CR9_ENDACHFR_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutehfl =
+			    AB8500_CODEC_CR10_MUTEHFL_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutehfr =
+			    AB8500_CODEC_CR10_MUTEHFR_ENABLED;
+		}
+		break;
+
+	case AB8500_CODEC_DEST_VIBRATOR_L:
+
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr8_envibl =
+			    AB8500_CODEC_CR8_ENVIBL_ENABLED;
+			p_ab8500_codec_configuration->cr9_endacvibl =
+			    AB8500_CODEC_CR9_ENDACVIBL_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutevibl =
+			    AB8500_CODEC_CR10_MUTEVIBL_DISABLED;
+			p_ab8500_codec_configuration->cr15_pwmtovibl =
+			    AB8500_CODEC_CR15_PWMTOVIBL_PWM;
+			p_ab8500_codec_configuration->cr15_pwmlctrl =
+			    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmnlctrl =
+			    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmplctrl =
+			    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLDUTYCYCLE;
+		} else {
+			p_ab8500_codec_configuration->cr8_envibl =
+			    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+			p_ab8500_codec_configuration->cr9_endacvibl =
+			    AB8500_CODEC_CR9_ENDACVIBL_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutevibl =
+			    AB8500_CODEC_CR10_MUTEVIBL_ENABLED;
+			p_ab8500_codec_configuration->cr15_pwmtovibl =
+			    AB8500_CODEC_CR15_PWMTOVIBL_DA_PATH;
+			p_ab8500_codec_configuration->cr15_pwmlctrl =
+			    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLGPOL;
+			p_ab8500_codec_configuration->cr15_pwmnlctrl =
+			    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLGPOL;
+			p_ab8500_codec_configuration->cr15_pwmplctrl =
+			    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLGPOL;
+		}
+		break;
+
+	case AB8500_CODEC_DEST_VIBRATOR_R:
+
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr8_envibr =
+			    AB8500_CODEC_CR8_ENVIBR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endacvibr =
+			    AB8500_CODEC_CR9_ENDACVIBR_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutevibr =
+			    AB8500_CODEC_CR10_MUTEVIBR_DISABLED;
+			p_ab8500_codec_configuration->cr15_pwmtovibr =
+			    AB8500_CODEC_CR15_PWMTOVIBR_PWM;
+			p_ab8500_codec_configuration->cr15_pwmrctrl =
+			    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmnrctrl =
+			    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmprctrl =
+			    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRDUTYCYCLE;
+		} else {
+			p_ab8500_codec_configuration->cr8_envibr =
+			    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endacvibr =
+			    AB8500_CODEC_CR9_ENDACVIBR_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutevibr =
+			    AB8500_CODEC_CR10_MUTEVIBR_ENABLED;
+			p_ab8500_codec_configuration->cr15_pwmtovibr =
+			    AB8500_CODEC_CR15_PWMTOVIBR_DA_PATH;
+			p_ab8500_codec_configuration->cr15_pwmrctrl =
+			    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRGPOL;
+			p_ab8500_codec_configuration->cr15_pwmnrctrl =
+			    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRGPOL;
+			p_ab8500_codec_configuration->cr15_pwmprctrl =
+			    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRGPOL;
+		}
+		break;
+
+	case AB8500_CODEC_DEST_ALL:
+
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr8_enhsl =
+			    AB8500_CODEC_CR8_ENHSL_ENABLED;
+			p_ab8500_codec_configuration->cr8_enhsr =
+			    AB8500_CODEC_CR8_ENHSR_ENABLED;
+			p_ab8500_codec_configuration->cr8_enear =
+			    AB8500_CODEC_CR8_ENEAR_ENABLED;
+			p_ab8500_codec_configuration->cr8_enhfl =
+			    AB8500_CODEC_CR8_ENHFL_ENABLED;
+			p_ab8500_codec_configuration->cr8_enhfr =
+			    AB8500_CODEC_CR8_ENHFR_ENABLED;
+			p_ab8500_codec_configuration->cr8_envibl =
+			    AB8500_CODEC_CR8_ENVIBL_ENABLED;
+			p_ab8500_codec_configuration->cr8_envibr =
+			    AB8500_CODEC_CR8_ENVIBR_ENABLED;
+
+			p_ab8500_codec_configuration->cr9_endacear =
+			    AB8500_CODEC_CR9_ENDACEAR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endachfl =
+			    AB8500_CODEC_CR9_ENDACHFL_ENABLED;
+			p_ab8500_codec_configuration->cr9_endachfr =
+			    AB8500_CODEC_CR9_ENDACHFR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endacvibl =
+			    AB8500_CODEC_CR9_ENDACVIBL_ENABLED;
+			p_ab8500_codec_configuration->cr9_endacvibr =
+			    AB8500_CODEC_CR9_ENDACVIBR_ENABLED;
+
+			p_ab8500_codec_configuration->cr10_mutehsl =
+			    AB8500_CODEC_CR10_MUTEHSL_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutehsr =
+			    AB8500_CODEC_CR10_MUTEHSR_DISABLED;
+			p_ab8500_codec_configuration->cr10_muteear =
+			    AB8500_CODEC_CR10_MUTEEAR_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutehfl =
+			    AB8500_CODEC_CR10_MUTEHFL_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutehfr =
+			    AB8500_CODEC_CR10_MUTEHFR_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutevibl =
+			    AB8500_CODEC_CR10_MUTEVIBL_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutevibr =
+			    AB8500_CODEC_CR10_MUTEVIBR_DISABLED;
+
+			p_ab8500_codec_configuration->cr15_pwmtovibl =
+			    AB8500_CODEC_CR15_PWMTOVIBL_PWM;
+			p_ab8500_codec_configuration->cr15_pwmlctrl =
+			    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmnlctrl =
+			    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmplctrl =
+			    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmtovibr =
+			    AB8500_CODEC_CR15_PWMTOVIBR_PWM;
+			p_ab8500_codec_configuration->cr15_pwmrctrl =
+			    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmnrctrl =
+			    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmprctrl =
+			    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRDUTYCYCLE;
+		} else {
+			p_ab8500_codec_configuration->cr8_enhsl =
+			    AB8500_CODEC_CR8_ENHSL_DISABLED;
+			p_ab8500_codec_configuration->cr8_enhsr =
+			    AB8500_CODEC_CR8_ENHSR_DISABLED;
+			p_ab8500_codec_configuration->cr8_enear =
+			    AB8500_CODEC_CR8_ENEAR_DISABLED;
+			p_ab8500_codec_configuration->cr8_enhfl =
+			    AB8500_CODEC_CR8_ENHFL_DISABLED;
+			p_ab8500_codec_configuration->cr8_enhfr =
+			    AB8500_CODEC_CR8_ENHFR_DISABLED;
+			p_ab8500_codec_configuration->cr8_envibl =
+			    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+			p_ab8500_codec_configuration->cr8_envibr =
+			    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+
+			p_ab8500_codec_configuration->cr9_endacear =
+			    AB8500_CODEC_CR9_ENDACEAR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endachfl =
+			    AB8500_CODEC_CR9_ENDACHFL_DISABLED;
+			p_ab8500_codec_configuration->cr9_endachfr =
+			    AB8500_CODEC_CR9_ENDACHFR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endacvibl =
+			    AB8500_CODEC_CR9_ENDACVIBL_DISABLED;
+			p_ab8500_codec_configuration->cr9_endacvibr =
+			    AB8500_CODEC_CR9_ENDACVIBR_DISABLED;
+
+			p_ab8500_codec_configuration->cr10_mutehsl =
+			    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutehsr =
+			    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+			p_ab8500_codec_configuration->cr10_muteear =
+			    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutehfl =
+			    AB8500_CODEC_CR10_MUTEHFL_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutehfr =
+			    AB8500_CODEC_CR10_MUTEHFR_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutevibl =
+			    AB8500_CODEC_CR10_MUTEVIBL_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutevibr =
+			    AB8500_CODEC_CR10_MUTEVIBR_ENABLED;
+
+			p_ab8500_codec_configuration->cr15_pwmtovibl =
+			    AB8500_CODEC_CR15_PWMTOVIBL_DA_PATH;
+			p_ab8500_codec_configuration->cr15_pwmlctrl =
+			    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLGPOL;
+			p_ab8500_codec_configuration->cr15_pwmnlctrl =
+			    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLGPOL;
+			p_ab8500_codec_configuration->cr15_pwmplctrl =
+			    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLGPOL;
+			p_ab8500_codec_configuration->cr15_pwmtovibr =
+			    AB8500_CODEC_CR15_PWMTOVIBR_DA_PATH;
+			p_ab8500_codec_configuration->cr15_pwmrctrl =
+			    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRGPOL;
+			p_ab8500_codec_configuration->cr15_pwmnrctrl =
+			    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRGPOL;
+			p_ab8500_codec_configuration->cr15_pwmprctrl =
+			    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRGPOL;
+
+		}
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_DestPowerControlUpdateCR();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_GetVersion                                            */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* This routine populates the pVersion structure with                       */
+/* the current version of HCL.                                              */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* p_version: this parameter is used to return current HCL version.         */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_ERROR: if p_version is NULL.                                */
+/* AB8500_CODEC_OK: if successful                                           */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Re-Entrant                                                   */
+/* REENTRANCY ISSUES: No Issues                                             */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_GetVersion(OUT t_version * p_version)
+{
+	DBGENTER1(" (%lx)", p_version);
+	if (p_version != NULL) {
+		p_version->minor = AB8500_CODEC_HCL_MINOR_ID;
+		p_version->major = AB8500_CODEC_HCL_MAJOR_ID;
+		p_version->version = AB8500_CODEC_HCL_VERSION_ID;
+		DBGEXIT0(AB8500_CODEC_OK);
+		return (AB8500_CODEC_OK);
+	} else {
+		DBGEXIT0(AB8500_CODEC_INVALID_PARAMETER);
+		return (AB8500_CODEC_INVALID_PARAMETER);
+	}
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetDbgLevel                                           */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Set the debug level used by the debug module (mask-like value).          */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* debug_level: debug level to be set                                       */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_OK: always                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Re-Entrant                                                   */
+/* REENTRANCY ISSUES: No Issues                                             */
+
+/****************************************************************************/
+/*
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetDbgLevel(IN t_dbg_level dbg_level)
+{
+    DBGENTER1(" (%d)", dbg_level);
+    dbg_level = dbg_level;
+#ifdef __DEBUG
+    MY_DEBUG_LEVEL_VAR_NAME = dbg_level;
+#endif
+    DBGEXIT(AB8500_CODEC_OK);
+    return(AB8500_CODEC_OK);
+}*/
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_GetDbgLevel                                           */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Set the debug level used by the debug module (mask-like value).          */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* p_dbg_level: this parameter is used to return debug level.               */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_ERROR: if p_version is NULL.                                */
+/* AB8500_CODEC_OK: if successful                                           */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Re-Entrant                                                   */
+/* REENTRANCY ISSUES: No Issues                                             */
+
+/****************************************************************************/
+/*
+PUBLIC t_ab8500_codec_error AB8500_CODEC_GetDbgLevel(OUT t_dbg_level *p_dbg_level)
+{
+    if (NULL == p_dbg_level)
+    {
+        DBGEXIT(AB8500_CODEC_INVALID_PARAMETER);
+        return(AB8500_CODEC_INVALID_PARAMETER);
+    }
+
+#ifdef __DEBUG
+    * p_dbg_level = MY_DEBUG_LEVEL_VAR_NAME;
+#endif
+    DBGEXIT(AB8500_CODEC_OK);
+    return(AB8500_CODEC_OK);
+}
+*/
+/****************************************************************************/
+/* NAME: AB8500_CODEC_ADSlotAllocation                                      */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* AD Data Allocation in slots.                                             */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN: t_ab8500_codec_slot: The slot to be allocated.                       */
+/* IN: t_ab8500_codec_cr31_to_cr46_ad_data_allocation: The value            */
+/*                                                     to be allocated.     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_INVALID_PARAMETER: If invalid slot number                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK:                                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_ADSlotAllocation
+    (IN t_ab8500_codec_slot ad_slot,
+     IN t_ab8500_codec_cr31_to_cr46_ad_data_allocation value) {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+
+	DBGENTER2(" (%lx %lx)", ad_slot, value);
+
+	if (ad_slot <= AB8500_CODEC_SLOT7) {
+		ab8500_codec_error =
+		    ab8500_codec_ADSlotAllocationSwitch1(ad_slot, value);
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+	} else if (ad_slot <= AB8500_CODEC_SLOT15) {
+		ab8500_codec_error =
+		    ab8500_codec_ADSlotAllocationSwitch2(ad_slot, value);
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+	} else if (ad_slot <= AB8500_CODEC_SLOT23) {
+		ab8500_codec_error =
+		    ab8500_codec_ADSlotAllocationSwitch3(ad_slot, value);
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+	} else if (ad_slot <= AB8500_CODEC_SLOT31) {
+		ab8500_codec_error =
+		    ab8500_codec_ADSlotAllocationSwitch4(ad_slot, value);
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+	} else {
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_DASlotAllocation                                      */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Allocate the Audio Interface slot for DA paths.                          */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN: t_ab8500_codec_da_channel_number: Channel number 1/2/3/4/5/6         */
+/* IN: t_ab8500_codec_cr51_to_cr56_sltoda: Slot number                      */
+/*                                                                          */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_INVALID_PARAMETER: If invalid channel number                */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK:                                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_DASlotAllocation
+    (IN t_ab8500_codec_da_channel_number channel_number,
+     IN t_ab8500_codec_cr51_to_cr56_sltoda slot) {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	t_ab8500_codec_cr0_powerup ab8500_codec_cr0_powerup;
+
+	DBGENTER2(" (%lx %lx)", channel_number, slot);
+
+	p_ab8500_codec_configuration->cr51_da12_voice =
+	    AB8500_CODEC_CR51_DA12_VOICE_LOWLATENCYFILTER;
+
+	switch (channel_number) {
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_1:
+		p_ab8500_codec_configuration->cr51_sltoda1 = slot;
+		break;
+
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_2:
+		p_ab8500_codec_configuration->cr52_sltoda2 = slot;
+		break;
+
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_3:
+		p_ab8500_codec_configuration->cr53_sltoda3 = slot;
+		break;
+
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_4:
+		p_ab8500_codec_configuration->cr54_sltoda4 = slot;
+		break;
+
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_5:
+		p_ab8500_codec_configuration->cr55_sltoda5 = slot;
+		break;
+
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_6:
+		p_ab8500_codec_configuration->cr56_sltoda6 = slot;
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_cr0_powerup = p_ab8500_codec_configuration->cr0_powerup;
+
+	p_ab8500_codec_configuration->cr0_powerup =
+	    AB8500_CODEC_CR0_POWERUP_OFF;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR51();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR52();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR53();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR54();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR55();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR56();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	p_ab8500_codec_configuration->cr0_powerup = ab8500_codec_cr0_powerup;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_ConfigureBurstFifo                                    */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Configuration for Burst FIFO control                                     */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN: t_ab8500_codec_burst_fifo_config: structure for configuration of     */
+/*										 burst FIFO                         */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_INVALID_PARAMETER: If invalid parameter                     */
+/* AB8500_CODEC_UNSUPPORTED_FEATURE: If interface 1 selected                */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK:                                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+t_ab8500_codec_error AB8500_CODEC_ConfigureBurstFifo(IN
+						     t_ab8500_codec_burst_fifo_config
+						     const *const
+						     p_burst_fifo_config)
+{
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER1(" (%lx)", p_burst_fifo_config);
+
+	if (AB8500_CODEC_AUDIO_INTERFACE_0 ==
+	    g_ab8500_codec_system_context.audio_interface) {
+		if (AB8500_CODEC_CR27_EN_MASTGEN_ENABLED ==
+		    p_ab8500_codec_configuration->cr27_en_mastgen) {
+			p_ab8500_codec_configuration->cr104_bfifoint =
+			    p_burst_fifo_config->cr104_bfifoint;
+
+			p_ab8500_codec_configuration->cr105_bfifotx =
+			    p_burst_fifo_config->cr105_bfifotx;
+
+			p_ab8500_codec_configuration->cr106_bfifofsext =
+			    p_burst_fifo_config->cr106_bfifofsext;
+			p_ab8500_codec_configuration->cr106_bfifomsk =
+			    p_burst_fifo_config->cr106_bfifomsk;
+			p_ab8500_codec_configuration->cr106_bfifomstr =
+			    p_burst_fifo_config->cr106_bfifomstr;
+			p_ab8500_codec_configuration->cr106_bfifostrt =
+			    p_burst_fifo_config->cr106_bfifostrt;
+
+			p_ab8500_codec_configuration->cr107_bfifosampnr =
+			    p_burst_fifo_config->cr107_bfifosampnr;
+
+			p_ab8500_codec_configuration->cr108_bfifowakeup =
+			    p_burst_fifo_config->cr108_bfifowakeup;
+
+			ab8500_codec_error = ab8500_codec_UpdateCR104();
+			if (ab8500_codec_error != AB8500_CODEC_OK) {
+				DBGEXIT(ab8500_codec_error);
+				return (ab8500_codec_error);
+			}
+
+			ab8500_codec_error = ab8500_codec_UpdateCR105();
+			if (ab8500_codec_error != AB8500_CODEC_OK) {
+				DBGEXIT(ab8500_codec_error);
+				return (ab8500_codec_error);
+			}
+
+			ab8500_codec_error = ab8500_codec_UpdateCR106();
+			if (ab8500_codec_error != AB8500_CODEC_OK) {
+				DBGEXIT(ab8500_codec_error);
+				return (ab8500_codec_error);
+			}
+
+			ab8500_codec_error = ab8500_codec_UpdateCR107();
+			if (ab8500_codec_error != AB8500_CODEC_OK) {
+				DBGEXIT(ab8500_codec_error);
+				return (ab8500_codec_error);
+			}
+
+			ab8500_codec_error = ab8500_codec_UpdateCR108();
+			if (ab8500_codec_error != AB8500_CODEC_OK) {
+				DBGEXIT(ab8500_codec_error);
+				return (ab8500_codec_error);
+			}
+		} else {
+			ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+	} else {
+		ab8500_codec_error = AB8500_CODEC_UNSUPPORTED_FEATURE;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_EnableBurstFifo                                       */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Enable the Burst FIFO for Interface 0                                    */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_UNSUPPORTED_FEATURE: If Interface 1 is selected             */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK:                                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_EnableBurstFifo(void)
+{
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER0();
+
+	if (AB8500_CODEC_AUDIO_INTERFACE_0 ==
+	    g_ab8500_codec_system_context.audio_interface) {
+		p_ab8500_codec_configuration->cr29_if0bfifoen =
+		    AB8500_CODEC_CR29_IF0BFIFOEN_BURST_MODE;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR29();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+	} else {
+		ab8500_codec_error = AB8500_CODEC_UNSUPPORTED_FEATURE;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_DisableBurstFifo                                      */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Disable the Burst FIFO for Interface 0                                   */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_UNSUPPORTED_FEATURE: If Interface 1 is selected             */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK:                                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_DisableBurstFifo(void)
+{
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER0();
+
+	if (AB8500_CODEC_AUDIO_INTERFACE_0 ==
+	    g_ab8500_codec_system_context.audio_interface) {
+		p_ab8500_codec_configuration->cr29_if0bfifoen =
+		    AB8500_CODEC_CR29_IF0BFIFOEN_NORMAL_MODE;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR29();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+	} else {
+		ab8500_codec_error = AB8500_CODEC_UNSUPPORTED_FEATURE;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch1
+    (IN t_ab8500_codec_slot ad_slot,
+     IN t_ab8500_codec_cr31_to_cr46_ad_data_allocation value) {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	switch (ad_slot) {
+	case AB8500_CODEC_SLOT0:
+		p_ab8500_codec_configuration->cr31_adotoslot0 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR31();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT1:
+		p_ab8500_codec_configuration->cr31_adotoslot1 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR31();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT2:
+		p_ab8500_codec_configuration->cr32_adotoslot2 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR32();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT3:
+		p_ab8500_codec_configuration->cr32_adotoslot3 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR32();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT4:
+		p_ab8500_codec_configuration->cr33_adotoslot4 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR33();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT5:
+		p_ab8500_codec_configuration->cr33_adotoslot5 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR33();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT6:
+		p_ab8500_codec_configuration->cr34_adotoslot6 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR34();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT7:
+		p_ab8500_codec_configuration->cr34_adotoslot7 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR34();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch2
+    (IN t_ab8500_codec_slot ad_slot,
+     IN t_ab8500_codec_cr31_to_cr46_ad_data_allocation value) {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	switch (ad_slot) {
+	case AB8500_CODEC_SLOT8:
+		p_ab8500_codec_configuration->cr35_adotoslot8 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR35();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT9:
+		p_ab8500_codec_configuration->cr35_adotoslot9 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR35();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT10:
+		p_ab8500_codec_configuration->cr36_adotoslot10 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR36();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT11:
+		p_ab8500_codec_configuration->cr36_adotoslot11 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR36();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT12:
+		p_ab8500_codec_configuration->cr37_adotoslot12 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR37();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT13:
+		p_ab8500_codec_configuration->cr37_adotoslot13 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR37();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT14:
+		p_ab8500_codec_configuration->cr38_adotoslot14 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR38();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT15:
+		p_ab8500_codec_configuration->cr38_adotoslot15 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR38();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch3
+    (IN t_ab8500_codec_slot ad_slot,
+     IN t_ab8500_codec_cr31_to_cr46_ad_data_allocation value) {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	switch (ad_slot) {
+	case AB8500_CODEC_SLOT16:
+		p_ab8500_codec_configuration->cr39_adotoslot16 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR39();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT17:
+		p_ab8500_codec_configuration->cr39_adotoslot17 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR39();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT18:
+		p_ab8500_codec_configuration->cr40_adotoslot18 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR40();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT19:
+		p_ab8500_codec_configuration->cr40_adotoslot19 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR40();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT20:
+		p_ab8500_codec_configuration->cr41_adotoslot20 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR41();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT21:
+		p_ab8500_codec_configuration->cr41_adotoslot21 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR41();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT22:
+		p_ab8500_codec_configuration->cr42_adotoslot22 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR42();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT23:
+		p_ab8500_codec_configuration->cr42_adotoslot23 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR42();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch4
+    (IN t_ab8500_codec_slot ad_slot,
+     IN t_ab8500_codec_cr31_to_cr46_ad_data_allocation value) {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	switch (ad_slot) {
+	case AB8500_CODEC_SLOT24:
+		p_ab8500_codec_configuration->cr43_adotoslot24 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR43();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT25:
+		p_ab8500_codec_configuration->cr43_adotoslot25 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR43();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT26:
+		p_ab8500_codec_configuration->cr44_adotoslot26 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR44();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT27:
+		p_ab8500_codec_configuration->cr44_adotoslot27 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR44();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT28:
+		p_ab8500_codec_configuration->cr45_adotoslot28 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR45();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT29:
+		p_ab8500_codec_configuration->cr45_adotoslot29 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR45();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT30:
+		p_ab8500_codec_configuration->cr46_adotoslot30 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR46();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	case AB8500_CODEC_SLOT31:
+		p_ab8500_codec_configuration->cr46_adotoslot31 = value;
+
+		ab8500_codec_error = ab8500_codec_UpdateCR46();
+		if (ab8500_codec_error != AB8500_CODEC_OK) {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+		}
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SrcPowerControlSwitch1(IN
+								 t_ab8500_codec_src
+								 src_device,
+								 t_ab8500_codec_src_state
+								 state)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	switch (src_device) {
+	case AB8500_CODEC_SRC_LINEIN:
+
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr5_enlinl =
+			    AB8500_CODEC_CR5_ENLINL_ENABLED;
+			p_ab8500_codec_configuration->cr5_enlinr =
+			    AB8500_CODEC_CR5_ENLINR_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutlinl =
+			    AB8500_CODEC_CR5_MUTLINL_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutlinr =
+			    AB8500_CODEC_CR5_MUTLINR_DISABLED;
+
+			p_ab8500_codec_configuration->cr7_linrsel =
+			    AB8500_CODEC_CR7_LINRSEL_LINR;
+			p_ab8500_codec_configuration->cr7_enadclinl =
+			    AB8500_CODEC_CR7_ENADCLINL_ENABLED;
+			p_ab8500_codec_configuration->cr7_enadclinr =
+			    AB8500_CODEC_CR7_ENADCLINR_ENABLED;
+		} else {
+			p_ab8500_codec_configuration->cr5_enlinl =
+			    AB8500_CODEC_CR5_ENLINL_DISABLED;
+			p_ab8500_codec_configuration->cr5_enlinr =
+			    AB8500_CODEC_CR5_ENLINR_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutlinl =
+			    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutlinr =
+			    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+
+			p_ab8500_codec_configuration->cr7_linrsel =
+			    AB8500_CODEC_CR7_LINRSEL_MIC2;
+			p_ab8500_codec_configuration->cr7_enadclinl =
+			    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+			p_ab8500_codec_configuration->cr7_enadclinr =
+			    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+		}
+		break;
+
+	case AB8500_CODEC_SRC_MICROPHONE_1A:
+
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+
+			p_ab8500_codec_configuration->cr7_mic1sel =
+			    AB8500_CODEC_CR7_MIC1SEL_MIC1A;
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+		} else {
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_DISABLED;
+		}
+		break;
+
+	case AB8500_CODEC_SRC_MICROPHONE_1B:
+
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+
+			p_ab8500_codec_configuration->cr7_mic1sel =
+			    AB8500_CODEC_CR7_MIC1SEL_MIC1B;
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+		} else {
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_DISABLED;
+		}
+		break;
+
+	case AB8500_CODEC_SRC_MICROPHONE_2:
+
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr5_enmic2 =
+			    AB8500_CODEC_CR5_ENMIC2_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutmic2 =
+			    AB8500_CODEC_CR5_MUTMIC2_DISABLED;
+
+			p_ab8500_codec_configuration->cr7_linrsel =
+			    AB8500_CODEC_CR7_LINRSEL_MIC2;
+		} else {
+			p_ab8500_codec_configuration->cr5_enmic2 =
+			    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutmic2 =
+			    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+
+			p_ab8500_codec_configuration->cr7_linrsel =
+			    AB8500_CODEC_CR7_LINRSEL_LINR;
+		}
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_1:
+
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr6_endmic1 =
+			    AB8500_CODEC_CR6_ENDMIC1_ENABLED;
+
+			p_ab8500_codec_configuration->cr63_ad1sel =
+			    AB8500_CODEC_CR63_AD1SEL_DMIC1_SELECTED;
+		} else {
+			p_ab8500_codec_configuration->cr6_endmic1 =
+			    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+
+			p_ab8500_codec_configuration->cr63_ad1sel =
+			    AB8500_CODEC_CR63_AD1SEL_LINLADL_SELECTED;
+		}
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_2:
+
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr6_endmic2 =
+			    AB8500_CODEC_CR6_ENDMIC2_ENABLED;
+
+			p_ab8500_codec_configuration->cr63_ad2sel =
+			    AB8500_CODEC_CR63_AD2SEL_DMIC2_SELECTED;
+		} else {
+			p_ab8500_codec_configuration->cr6_endmic2 =
+			    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+
+			p_ab8500_codec_configuration->cr63_ad2sel =
+			    AB8500_CODEC_CR63_AD2SEL_LINRADR_SELECTED;
+		}
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SrcPowerControlSwitch2(IN
+								 t_ab8500_codec_src
+								 src_device,
+								 t_ab8500_codec_src_state
+								 state)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	switch (src_device) {
+	case AB8500_CODEC_SRC_D_MICROPHONE_3:
+
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr6_endmic3 =
+			    AB8500_CODEC_CR6_ENDMIC3_ENABLED;
+
+			p_ab8500_codec_configuration->cr63_ad3sel =
+			    AB8500_CODEC_CR63_AD3SEL_DMIC3_SELECTED;
+		} else {
+			p_ab8500_codec_configuration->cr6_endmic3 =
+			    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+
+			p_ab8500_codec_configuration->cr63_ad3sel =
+			    AB8500_CODEC_CR63_AD3SEL_ADMO_SELECTED;
+		}
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_4:
+
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr6_endmic4 =
+			    AB8500_CODEC_CR6_ENDMIC4_ENABLED;
+		} else {
+			p_ab8500_codec_configuration->cr6_endmic4 =
+			    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		}
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_5:
+
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr6_endmic5 =
+			    AB8500_CODEC_CR6_ENDMIC5_ENABLED;
+
+			p_ab8500_codec_configuration->cr63_ad5sel =
+			    AB8500_CODEC_CR63_AD5SEL_DMIC5_SELECTED;
+		} else {
+			p_ab8500_codec_configuration->cr6_endmic5 =
+			    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+
+			p_ab8500_codec_configuration->cr63_ad5sel =
+			    AB8500_CODEC_CR63_AD5SEL_AMADR_SELECTED;
+		}
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_6:
+
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr6_endmic6 =
+			    AB8500_CODEC_CR6_ENDMIC6_ENABLED;
+
+			p_ab8500_codec_configuration->cr63_ad6sel =
+			    AB8500_CODEC_CR63_AD6SEL_DMIC6_SELECTED;
+		} else {
+			p_ab8500_codec_configuration->cr6_endmic6 =
+			    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+
+			p_ab8500_codec_configuration->cr63_ad6sel =
+			    AB8500_CODEC_CR63_AD6SEL_ADMO_SELECTED;
+		}
+		break;
+
+	case AB8500_CODEC_SRC_ALL:
+
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state) {
+			p_ab8500_codec_configuration->cr5_enlinl =
+			    AB8500_CODEC_CR5_ENLINL_ENABLED;
+			p_ab8500_codec_configuration->cr5_enlinr =
+			    AB8500_CODEC_CR5_ENLINR_ENABLED;
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr5_enmic2 =
+			    AB8500_CODEC_CR5_ENMIC2_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic1 =
+			    AB8500_CODEC_CR6_ENDMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic2 =
+			    AB8500_CODEC_CR6_ENDMIC2_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic3 =
+			    AB8500_CODEC_CR6_ENDMIC3_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic4 =
+			    AB8500_CODEC_CR6_ENDMIC4_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic5 =
+			    AB8500_CODEC_CR6_ENDMIC5_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic6 =
+			    AB8500_CODEC_CR6_ENDMIC6_ENABLED;
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+
+			p_ab8500_codec_configuration->cr5_mutlinl =
+			    AB8500_CODEC_CR5_MUTLINL_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutlinr =
+			    AB8500_CODEC_CR5_MUTLINR_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutmic2 =
+			    AB8500_CODEC_CR5_MUTMIC2_DISABLED;
+		} else {
+			p_ab8500_codec_configuration->cr5_enlinl =
+			    AB8500_CODEC_CR5_ENLINL_DISABLED;
+			p_ab8500_codec_configuration->cr5_enlinr =
+			    AB8500_CODEC_CR5_ENLINR_DISABLED;
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr5_enmic2 =
+			    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic1 =
+			    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic2 =
+			    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic3 =
+			    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic4 =
+			    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic5 =
+			    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic6 =
+			    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_DISABLED;
+
+			p_ab8500_codec_configuration->cr5_mutlinl =
+			    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutlinr =
+			    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutmic2 =
+			    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+		}
+		break;
+	case AB8500_CODEC_SRC_FM_RX:
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SetModeAndDirectionUpdateCR(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	t_ab8500_codec_cr0_powerup ab8500_codec_cr0_powerup;
+
+	ab8500_codec_cr0_powerup = p_ab8500_codec_configuration->cr0_powerup;
+
+	p_ab8500_codec_configuration->cr0_powerup =
+	    AB8500_CODEC_CR0_POWERUP_OFF;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+	if (AB8500_CODEC_OK != ab8500_codec_error) {
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR2();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR3();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR26();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR28();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR30();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR63();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	p_ab8500_codec_configuration->cr0_powerup = ab8500_codec_cr0_powerup;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SetSrcVolumeUpdateCR(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR20();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR21();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR22();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR23();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR65();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR66();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR67();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR68();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR69();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR70();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SetDestVolumeUpdateCR(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR16();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR17();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR18();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR19();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR22();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR23();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR71();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR72();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR73();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR74();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR75();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR76();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR79();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR80();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR101();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR102();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR103();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ProgramDirectionIN(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	switch (g_ab8500_codec_system_context.ab8500_codec_src) {
+	case AB8500_CODEC_SRC_LINEIN:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+
+		p_ab8500_codec_configuration->cr7_linrsel =
+		    AB8500_CODEC_CR7_LINRSEL_LINR;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_ENABLED;
+
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_SRC_MICROPHONE_1A:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+
+		p_ab8500_codec_configuration->cr7_mic1sel =
+		    AB8500_CODEC_CR7_MIC1SEL_MIC1A;
+		p_ab8500_codec_configuration->cr7_enadcmic =
+		    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_SRC_MICROPHONE_1B:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+
+		p_ab8500_codec_configuration->cr7_mic1sel =
+		    AB8500_CODEC_CR7_MIC1SEL_MIC1B;
+		p_ab8500_codec_configuration->cr7_enadcmic =
+		    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_SRC_MICROPHONE_2:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_ENABLED;
+
+		p_ab8500_codec_configuration->cr7_enadcmic =
+		    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+		p_ab8500_codec_configuration->cr7_linrsel =
+		    AB8500_CODEC_CR7_LINRSEL_MIC2;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_DISABLED;
+
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_1:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+
+		p_ab8500_codec_configuration->cr63_ad1sel =
+		    AB8500_CODEC_CR63_AD1SEL_DMIC1_SELECTED;
+
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_2:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+
+		p_ab8500_codec_configuration->cr63_ad2sel =
+		    AB8500_CODEC_CR63_AD2SEL_DMIC2_SELECTED;
+
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_3:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+
+		p_ab8500_codec_configuration->cr63_ad3sel =
+		    AB8500_CODEC_CR63_AD3SEL_DMIC3_SELECTED;
+
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_4:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_5:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+
+		p_ab8500_codec_configuration->cr63_ad5sel =
+		    AB8500_CODEC_CR63_AD5SEL_DMIC5_SELECTED;
+
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_SRC_D_MICROPHONE_6:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_ENABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+
+		p_ab8500_codec_configuration->cr63_ad6sel =
+		    AB8500_CODEC_CR63_AD6SEL_DMIC6_SELECTED;
+
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_SRC_ALL:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadcmic =
+		    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_ENABLED;
+
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_DISABLED;
+
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ProgramDirectionOUT(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration *p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+
+	switch (g_ab8500_codec_system_context.ab8500_codec_dest) {
+	case AB8500_CODEC_DEST_HEADSET:
+		p_ab8500_codec_configuration->cr7_endrvhsl =
+		    AB8500_CODEC_CR7_ENDRVHSL_ENABLED;
+		p_ab8500_codec_configuration->cr7_endrvhsr =
+		    AB8500_CODEC_CR7_ENDRVHSR_ENABLED;
+
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+
+		p_ab8500_codec_configuration->cr9_endachsl =
+		    AB8500_CODEC_CR9_ENDACHSL_ENABLED;
+		p_ab8500_codec_configuration->cr9_endachsr =
+		    AB8500_CODEC_CR9_ENDACHSR_ENABLED;
+
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutehfl =
+		    AB8500_CODEC_CR10_MUTEHFL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehfr =
+		    AB8500_CODEC_CR10_MUTEHFR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutevibl =
+		    AB8500_CODEC_CR10_MUTEVIBL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutevibr =
+		    AB8500_CODEC_CR10_MUTEVIBR_ENABLED;
+
+		p_ab8500_codec_configuration->cr12_encphs =
+		    AB8500_CODEC_CR12_ENCPHS_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_DEST_EARPIECE:
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+
+		p_ab8500_codec_configuration->cr9_endacear =
+		    AB8500_CODEC_CR9_ENDACEAR_ENABLED;
+
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehfl =
+		    AB8500_CODEC_CR10_MUTEHFL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehfr =
+		    AB8500_CODEC_CR10_MUTEHFR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutevibl =
+		    AB8500_CODEC_CR10_MUTEVIBL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutevibr =
+		    AB8500_CODEC_CR10_MUTEVIBR_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_DEST_HANDSFREE:
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_ENABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+
+		p_ab8500_codec_configuration->cr9_endachfl =
+		    AB8500_CODEC_CR9_ENDACHFL_ENABLED;
+		p_ab8500_codec_configuration->cr9_endachfr =
+		    AB8500_CODEC_CR9_ENDACHFR_ENABLED;
+
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehfl =
+		    AB8500_CODEC_CR10_MUTEHFL_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutehfr =
+		    AB8500_CODEC_CR10_MUTEHFR_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutevibl =
+		    AB8500_CODEC_CR10_MUTEVIBL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutevibr =
+		    AB8500_CODEC_CR10_MUTEVIBR_ENABLED;
+
+		break;
+
+	case AB8500_CODEC_DEST_VIBRATOR_L:
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_ENABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+
+		p_ab8500_codec_configuration->cr9_endacvibl =
+		    AB8500_CODEC_CR9_ENDACVIBL_ENABLED;
+
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehfl =
+		    AB8500_CODEC_CR10_MUTEHFL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehfr =
+		    AB8500_CODEC_CR10_MUTEHFR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutevibl =
+		    AB8500_CODEC_CR10_MUTEVIBL_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutevibr =
+		    AB8500_CODEC_CR10_MUTEVIBR_ENABLED;
+
+		p_ab8500_codec_configuration->cr15_pwmtovibl =
+		    AB8500_CODEC_CR15_PWMTOVIBL_PWM;
+		p_ab8500_codec_configuration->cr15_pwmlctrl =
+		    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmnlctrl =
+		    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmplctrl =
+		    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLDUTYCYCLE;
+
+		break;
+
+	case AB8500_CODEC_DEST_VIBRATOR_R:
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_ENABLED;
+
+		p_ab8500_codec_configuration->cr9_endacvibr =
+		    AB8500_CODEC_CR9_ENDACVIBR_ENABLED;
+
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehfl =
+		    AB8500_CODEC_CR10_MUTEHFL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehfr =
+		    AB8500_CODEC_CR10_MUTEHFR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutevibl =
+		    AB8500_CODEC_CR10_MUTEVIBL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutevibr =
+		    AB8500_CODEC_CR10_MUTEVIBR_DISABLED;
+
+		p_ab8500_codec_configuration->cr15_pwmtovibr =
+		    AB8500_CODEC_CR15_PWMTOVIBR_PWM;
+		p_ab8500_codec_configuration->cr15_pwmrctrl =
+		    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmnrctrl =
+		    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmprctrl =
+		    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRDUTYCYCLE;
+
+		break;
+
+	case AB8500_CODEC_DEST_ALL:
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_ENABLED;
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_ENABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_ENABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_ENABLED;
+
+		p_ab8500_codec_configuration->cr9_endacear =
+		    AB8500_CODEC_CR9_ENDACEAR_ENABLED;
+		p_ab8500_codec_configuration->cr9_endachfl =
+		    AB8500_CODEC_CR9_ENDACHFL_ENABLED;
+		p_ab8500_codec_configuration->cr9_endachfr =
+		    AB8500_CODEC_CR9_ENDACHFR_ENABLED;
+		p_ab8500_codec_configuration->cr9_endacvibl =
+		    AB8500_CODEC_CR9_ENDACVIBL_ENABLED;
+		p_ab8500_codec_configuration->cr9_endacvibr =
+		    AB8500_CODEC_CR9_ENDACVIBR_ENABLED;
+
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_DISABLED;
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutehfl =
+		    AB8500_CODEC_CR10_MUTEHFL_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutehfr =
+		    AB8500_CODEC_CR10_MUTEHFR_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutevibl =
+		    AB8500_CODEC_CR10_MUTEVIBL_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutevibr =
+		    AB8500_CODEC_CR10_MUTEVIBR_DISABLED;
+
+		p_ab8500_codec_configuration->cr15_pwmtovibl =
+		    AB8500_CODEC_CR15_PWMTOVIBL_PWM;
+		p_ab8500_codec_configuration->cr15_pwmlctrl =
+		    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmnlctrl =
+		    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmplctrl =
+		    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmtovibr =
+		    AB8500_CODEC_CR15_PWMTOVIBR_PWM;
+		p_ab8500_codec_configuration->cr15_pwmrctrl =
+		    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmnrctrl =
+		    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmprctrl =
+		    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRDUTYCYCLE;
+
+		break;
+
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_DestPowerControlUpdateCR(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+
+	ab8500_codec_error = ab8500_codec_UpdateCR8();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR9();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR10();
+	if (ab8500_codec_error != AB8500_CODEC_OK) {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+	}
+
+	ab8500_codec_error = ab8500_codec_UpdateCR15();
+
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
diff --git a/sound/ab8500_codec_v1_0.c b/sound/ab8500_codec_v1_0.c
new file mode 100644
index 00000000000..5df1ca3eae4
--- /dev/null
+++ b/sound/ab8500_codec_v1_0.c
@@ -0,0 +1,6405 @@
+/*****************************************************************************/
+
+/**
+*  © ST-Ericsson, 2009 - All rights reserved
+*  Reproduction and Communication of this document is strictly prohibited
+*  unless specifically authorized in writing by ST-Ericsson
+*
+* \brief   This module provides some support routines for the AB8500 CODEC
+* \author  ST-Ericsson
+*/
+/*****************************************************************************/
+
+/*----------------------------------------------------------------------------
+ * Includes
+ *---------------------------------------------------------------------------*/
+
+#ifdef CONFIG_U8500_AB8500_CUT10
+#include <mach/ab8500_codec_v1_0.h>
+#include <mach/ab8500_codec_p_v1_0.h>
+#else /*  */
+#include <mach/ab8500_codec.h>
+#include <mach/ab8500_codec_p.h>
+#endif /*  */
+
+/*--------------------------------------------------------------------------*
+ * debug stuff *
+ *--------------------------------------------------------------------------*/
+#ifdef __DEBUG
+#define MY_DEBUG_LEVEL_VAR_NAME myDebugLevel_AB8500_CODEC
+#define MY_DEBUG_ID             myDebugID_AB8500_CODEC
+PRIVATE t_dbg_level MY_DEBUG_LEVEL_VAR_NAME = DEBUG_LEVEL0;
+PRIVATE t_dbg_id MY_DEBUG_ID = AB8500_CODEC_HCL_DBG_ID;
+
+#endif /*  */
+
+/*--------------------------------------------------------------------------*
+ * Global data for interrupt mode management *
+ *--------------------------------------------------------------------------*/
+PRIVATE t_ab8500_codec_system_context g_ab8500_codec_system_context;
+
+/*--------------------------------------------------------------------------*
+ * Default Values *
+ *--------------------------------------------------------------------------*/
+#define AB8500_CODEC_DEFAULT_SLAVE_ADDRESS_OF_CODEC 0xD
+#define AB8500_CODEC_DEFAULT_DIRECTION              AB8500_CODEC_DIRECTION_OUT
+
+#define AB8500_CODEC_DEFAULT_MODE_IN                AB8500_CODEC_MODE_VOICE
+#define AB8500_CODEC_DEFAULT_MODE_OUT               AB8500_CODEC_MODE_VOICE
+
+#define AB8500_CODEC_DEFAULT_INPUT_SRC              AB8500_CODEC_SRC_MICROPHONE_1A
+#define AB8500_CODEC_DEFAULT_OUTPUT_DEST            AB8500_CODEC_DEST_HEADSET
+
+#define AB8500_CODEC_DEFAULT_VOLUME_LEFT_IN         75
+#define AB8500_CODEC_DEFAULT_VOLUME_RIGHT_IN        75
+#define AB8500_CODEC_DEFAULT_VOLUME_LEFT_OUT        75
+#define AB8500_CODEC_DEFAULT_VOLUME_RIGHT_OUT       75
+
+/*---------------------------------------------------------------------
+ * PRIVATE APIs
+ *--------------------------------------------------------------------*/
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch1(IN
+								  t_ab8500_codec_slot
+								  ad_slot,
+								  IN
+								  t_ab8500_codec_cr31_to_cr46_ad_data_allocation
+								  value);
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch2(IN
+								   t_ab8500_codec_slot
+								   ad_slot,
+								   IN
+								   t_ab8500_codec_cr31_to_cr46_ad_data_allocation
+								   value);
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch3(IN
+								   t_ab8500_codec_slot
+								   ad_slot,
+								   IN
+								   t_ab8500_codec_cr31_to_cr46_ad_data_allocation
+								   value);
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch4(IN
+								   t_ab8500_codec_slot
+								   ad_slot,
+								   IN
+								   t_ab8500_codec_cr31_to_cr46_ad_data_allocation
+								   value);
+PRIVATE t_ab8500_codec_error ab8500_codec_SrcPowerControlSwitch1(IN
+								  t_ab8500_codec_src
+								  src_device,
+								  t_ab8500_codec_src_state
+								  state);
+PRIVATE t_ab8500_codec_error ab8500_codec_SrcPowerControlSwitch2(IN
+								  t_ab8500_codec_src
+								  src_device,
+								  t_ab8500_codec_src_state
+								  state);
+PRIVATE t_ab8500_codec_error ab8500_codec_SetModeAndDirectionUpdateCR(void);
+PRIVATE t_ab8500_codec_error ab8500_codec_SetSrcVolumeUpdateCR(void);
+PRIVATE t_ab8500_codec_error ab8500_codec_SetDestVolumeUpdateCR(void);
+PRIVATE t_ab8500_codec_error ab8500_codec_ProgramDirectionIN(void);
+PRIVATE t_ab8500_codec_error ab8500_codec_ProgramDirectionOUT(void);
+PRIVATE t_ab8500_codec_error ab8500_codec_DestPowerControlUpdateCR(void);
+
+/********************************************************************************************/
+/* Name: ab8500_codec_SingleWrite                                                       */
+/********************************************************************************************/
+    PRIVATE t_ab8500_codec_error ab8500_codec_SingleWrite(t_uint8
+							  register_offset,
+							  t_uint8 data)
+{
+	return (t_ab8500_codec_error) (AB8500_CODEC_Write
+					(register_offset, 0x01, &data));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_SingleRead                                                        */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_SingleRead(t_uint8
+						     register_offset,
+						     t_uint8 data)
+{
+	t_uint8 dummy_data = 0xAA;
+	return (t_ab8500_codec_error) (AB8500_CODEC_Read
+					 (register_offset, 0x01, &dummy_data,
+					  &data));
+}
+
+#endif /*  */
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR0                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR0(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				      (t_uint8) p_ab8500_codec_configuration->
+				      cr0_powerup, AB8500_CODEC_MASK_ONE_BIT,
+				      AB8500_CODEC_CR0_POWERUP );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr0_enaana, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR0_ENAANA );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR0, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR1                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR1(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr1_swreset, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR1_SWRESET );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR1, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR2                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR2(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr2_enad1, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR2_ENAD1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr2_enad2, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR2_ENAD2 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr2_enad3, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR2_ENAD3 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr2_enad4, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR2_ENAD4 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr2_enad5, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR2_ENAD5 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr2_enad6, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR2_ENAD6 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR2, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR3                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR3(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr3_enda1, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR3_ENDA1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr3_enda2, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR3_ENDA2 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr3_enda3, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR3_ENDA3 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr3_enda4, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR3_ENDA4 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr3_enda5, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR3_ENDA5 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr3_enda6, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR3_ENDA6 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR3, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR4                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR4(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr4_lowpowhs, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR4_LOWPOWHS );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr4_lowpowdachs,
+				    AB8500_CODEC_MASK_TWO_BITS,
+				    AB8500_CODEC_CR4_LOWPOWDACHS );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr4_lowpowear, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR4_LOWPOWEAR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr4_ear_sel_cm, AB8500_CODEC_MASK_TWO_BITS,
+				    AB8500_CODEC_CR4_EAR_SEL_CM );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr4_hs_hp_en, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR4_HS_HP_EN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR4, value));
+}
+
+#endif /*  */
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR5                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR5(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr5_enmic1, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR5_ENMIC1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr5_enmic2, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR5_ENMIC2 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr5_enlinl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR5_ENLINL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr5_enlinr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR5_ENLINR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr5_mutmic1, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR5_MUTMIC1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr5_mutmic2, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR5_MUTMIC2 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr5_mutlinl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR5_MUTELINL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr5_mutlinr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR5_MUTELINR );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR5, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR6                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR6(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr6_endmic1, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR6_ENDMIC1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr6_endmic2, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR6_ENDMIC2 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr6_endmic3, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR6_ENDMIC3 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr6_endmic4, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR6_ENDMIC4 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr6_endmic5, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR6_ENDMIC5 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr6_endmic6, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR6_ENDMIC6 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR6, value));
+}
+
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR7                                                          */
+/********************************************************************************************/
+    PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR7(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr7_mic1sel, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR7_MIC1SEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr7_linrsel, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR7_LINRSEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr7_endrvhsl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR7_ENDRVHSL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr7_endrvhsr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR7_ENDRVHSR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr7_enadcmic, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR7_ENADCMIC );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr7_enadclinl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR7_ENADCLINL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr7_enadclinr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR7_ENADCLINR );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR7, value));
+}
+
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR8                                                          */
+/********************************************************************************************/
+    PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR8(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr8_cp_dis_pldwn,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR8_CP_DIS_PLDWN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr8_enear, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR8_ENEAR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr8_enhsl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR8_ENHSL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr8_enhsr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR8_ENHSR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr8_enhfl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR8_ENHFL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr8_enhfr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR8_ENHFR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr8_envibl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR8_ENVIBL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr8_envibr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR8_ENVIBR );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR8, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR9                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR9(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr9_endacear, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR9_ENADACEAR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr9_endachsl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR9_ENADACHSL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr9_endachsr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR9_ENADACHSR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr9_endachfl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR9_ENADACHFL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr9_endachfr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR9_ENADACHFR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr9_endacvibl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR9_ENADACVIBL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr9_endacvibr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR9_ENADACVIBR );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR9, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR10                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR10(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr10_muteear, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR10_MUTEEAR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr10_mutehsl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR10_MUTEHSL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr10_mutehsr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR10_MUTEHSR );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR10, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR11                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR11(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr11_earshortpwd,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR11_ENSHORTPWD );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr11_earshortdis,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR11_EARSHORTDIS );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr11_hsshortdis, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR11_HSSHORTDIS );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr11_hspullden, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR11_HSPULLDEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr11_hsoscen, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR11_HSOSCEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr11_hsfaden, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR11_HSFADEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr11_hszcddis, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR11_HSZCDDIS );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR11, value));
+}
+
+#endif /*  */
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR12                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR12(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr12_encphs, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR12_ENCPHS );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr12_hsautoen, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR12_HSAUTOEN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR12, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR13                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR13(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr13_envdet_hthresh,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR13_ENVDET_HTHRESH );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr13_envdet_lthresh,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR13_ENVDET_LTHRESH );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR13, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR14                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR14(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr14_smpslven, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR14_SMPSLVEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr14_envdetsmpsen,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR14_ENVDETSMPSEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr14_cplven, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR14_CPLVEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr14_envdetcpen, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR14_ENVDETCPEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr14_envet_time,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR14_ENVDET_TIME );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR14, value));
+}
+
+#endif /*  */
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR15                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR15(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr15_pwmtovibl, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR15_PWMTOVIBL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr15_pwmtovibr, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR15_PWMTOVIBR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr15_pwmlctrl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR15_PWMLCTRL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr15_pwmrctrl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR15_PWMRCTRL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr15_pwmnlctrl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR15_PWMNLCTRL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr15_pwmplctrl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR15_PWMPLCTRL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr15_pwmnrctrl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR15_PWMNRCTRL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr15_pwmprctrl, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR15_PWMPRCTRL );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR15, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR16                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR16(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr16_pwmnlpol, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR16_PWMNLPOL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr16_pwmnldutycycle,
+				    AB8500_CODEC_MASK_SEVEN_BITS,
+				    AB8500_CODEC_CR16_PWMNLDUTYCYCLE );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR16, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR17                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR17(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr17_pwmplpol, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR17_PWMPLPOL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr17_pwmpldutycycle,
+				    AB8500_CODEC_MASK_SEVEN_BITS,
+				    AB8500_CODEC_CR17_PWMLPDUTYCYCLE );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR17, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR18                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR18(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr18_pwmnrpol, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR18_PWMNRPOL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr18_pwmnrdutycycle,
+				    AB8500_CODEC_MASK_SEVEN_BITS,
+				    AB8500_CODEC_CR18_PWMNRDUTYCYCLE );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR18, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR19                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR19(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr19_pwmprpol, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR19_PWMPRPOL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr19_pwmprdutycycle,
+				    AB8500_CODEC_MASK_SEVEN_BITS,
+				    AB8500_CODEC_CR19_PWMRPDUTYCYCLE );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR19, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR20                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR20(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr20_en_se_mic1,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR20_EN_SE_MIC1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr20_low_pow_mic1,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR20_LOW_POW_MIC1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr20_mic1_gain,
+				    AB8500_CODEC_MASK_FIVE_BITS,
+				    AB8500_CODEC_CR20_MIC1_GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR20, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR21                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR21(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr21_en_se_mic2,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR21_EN_SE_MIC2 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr21_low_pow_mic2,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR21_LOW_POW_MIC2 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr21_mic2_gain,
+				    AB8500_CODEC_MASK_FIVE_BITS,
+				    AB8500_CODEC_CR21_MIC2_GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR21, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR22                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR22(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr22_hsl_gain,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR22_HSL_GAIN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr22_hsr_gain, AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR22_HSR_GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR22, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR23                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR23(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr23_linl_gain,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR23_LINL_GAIN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr23_linr_gain,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR23_LINR_GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR23, value));
+}
+
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR24                                                          */
+/********************************************************************************************/
+    PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR24(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr24_lintohsl_gain,
+				     AB8500_CODEC_MASK_FIVE_BITS,
+				     AB8500_CODEC_CR24_LINTOHSL_GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR24, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR25                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR25(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr25_lintohsr_gain,
+				     AB8500_CODEC_MASK_FIVE_BITS,
+				     AB8500_CODEC_CR25_LINTOHSR_GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR25, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR26                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR26(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr26_ad1nh, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR26_AD1NH );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr26_ad2nh, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR26_AD2NH );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr26_ad3nh, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR26_AD3NH );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr26_ad4nh, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR26_AD4NH );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr26_ad1_voice, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR26_AD1_VOICE );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr26_ad2_voice, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR26_AD2_VOICE );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr26_ad3_voice, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR26_AD3_VOICE );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr26_ad4_voice, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR26_AD4_VOICE );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR26, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR27                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR27(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr27_en_mastgen,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR27_EN_MASTGEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr27_if1_bitclk_osr,
+				    AB8500_CODEC_MASK_TWO_BITS,
+				    AB8500_CODEC_CR27_IF1_BITCLK_OSR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr27_enfs_bitclk1,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR27_ENFS_BITCLK1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr27_if0_bitclk_osr,
+				    AB8500_CODEC_MASK_TWO_BITS,
+				    AB8500_CODEC_CR27_IF0_BITCLK_OSR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr27_enfs_bitclk0,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR27_ENFS_BITCLK0 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR27, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR28                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR28(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr28_fsync0p, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR28_FSYNC0P );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr28_bitclk0p, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR28_BITCLK0P );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr28_if0del, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR28_IF0DEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr28_if0format, AB8500_CODEC_MASK_TWO_BITS,
+				    AB8500_CODEC_CR28_IF0FORMAT );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr28_if0wl, AB8500_CODEC_MASK_TWO_BITS,
+				    AB8500_CODEC_CR28_IF0WL );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR28, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR29                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR29(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr29_if0datoif1ad,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR29_IF0DATOIF1AD );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr29_if0cktoif1ck,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR29_IF0CKTOIF1CK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr29_if1master, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR29_IF1MASTER );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr29_if1datoif0ad,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR29_IF1DATOIF0AD );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr29_if1cktoif0ck,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR29_IF1CKTOIF0CK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr29_if0master, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR29_IF0MASTER );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr29_if0bfifoen, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR29_IF0BFIFOEN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR29, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR30                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR30(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr30_fsync1p, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR30_FSYNC1P );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr30_bitclk1p, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR30_BITCLK1P );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr30_if1del, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR30_IF1DEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr30_if1format, AB8500_CODEC_MASK_TWO_BITS,
+				    AB8500_CODEC_CR30_IF1FORMAT );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr30_if1wl, AB8500_CODEC_MASK_TWO_BITS,
+				    AB8500_CODEC_CR30_IF1WL );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR30, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR31                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR31(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr31_adotoslot1,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR31_ADOTOSLOT1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr31_adotoslot0,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR31_ADOTOSLOT0 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR31, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR32                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR32(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr32_adotoslot3,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR32_ADOTOSLOT3 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr32_adotoslot2,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR32_ADOTOSLOT2 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR32, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR33                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR33(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr33_adotoslot5,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR33_ADOTOSLOT5 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr33_adotoslot4,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR33_ADOTOSLOT4 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR33, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR34                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR34(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr34_adotoslot7,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR34_ADOTOSLOT7 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr34_adotoslot6,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR34_ADOTOSLOT6 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR34, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR35                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR35(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr35_adotoslot9,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR35_ADOTOSLOT9 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr35_adotoslot8,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR35_ADOTOSLOT8 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR35, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR36                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR36(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr36_adotoslot11,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR36_ADOTOSLOT11 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr36_adotoslot10,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR36_ADOTOSLOT10 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR36, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR37                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR37(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr37_adotoslot13,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR37_ADOTOSLOT13 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr37_adotoslot12,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR37_ADOTOSLOT12 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR37, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR38                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR38(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr38_adotoslot15,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR38_ADOTOSLOT15 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr38_adotoslot14,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR38_ADOTOSLOT14 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR38, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR39                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR39(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr39_adotoslot17,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR39_ADOTOSLOT17 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr39_adotoslot16,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR39_ADOTOSLOT16 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR39, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR40                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR40(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr40_adotoslot19,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR40_ADOTOSLOT19 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr40_adotoslot18,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR40_ADOTOSLOT18 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR40, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR41                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR41(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr41_adotoslot21,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR41_ADOTOSLOT21 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr41_adotoslot20,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR41_ADOTOSLOT20 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR41, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR42                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR42(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr42_adotoslot23,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR42_ADOTOSLOT23 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr42_adotoslot22,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR42_ADOTOSLOT22 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR42, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR43                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR43(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr43_adotoslot25,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR43_ADOTOSLOT25 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr43_adotoslot24,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR43_ADOTOSLOT24 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR43, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR44                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR44(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr44_adotoslot27,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR44_ADOTOSLOT27 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr44_adotoslot26,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR44_ADOTOSLOT26 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR44, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR45                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR45(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr45_adotoslot29,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR45_ADOTOSLOT29 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr45_adotoslot28,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR45_ADOTOSLOT28 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR45, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR46                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR46(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr46_adotoslot31,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR46_ADOTOSLOT31 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr46_adotoslot30,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR46_ADOTOSLOT30 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR46, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR47                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR47(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr47_hiz_sl7, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR47_HIZ_SL7 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr47_hiz_sl6, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR47_HIZ_SL6 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr47_hiz_sl5, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR47_HIZ_SL5 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr47_hiz_sl4, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR47_HIZ_SL4 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr47_hiz_sl3, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR47_HIZ_SL3 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr47_hiz_sl2, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR47_HIZ_SL2 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr47_hiz_sl1, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR47_HIZ_SL1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr47_hiz_sl0, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR47_HIZ_SL0 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR47, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR48                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR48(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr48_hiz_sl15, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR48_HIZ_SL15 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr48_hiz_sl14, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR48_HIZ_SL14 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr48_hiz_sl13, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR48_HIZ_SL13 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr48_hiz_sl12, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR48_HIZ_SL12 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr48_hiz_sl11, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR48_HIZ_SL11 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr48_hiz_sl10, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR48_HIZ_SL10 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr48_hiz_sl9, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR48_HIZ_SL9 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr48_hiz_sl8, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR48_HIZ_SL8 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR48, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR49                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR49(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr49_hiz_sl23, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR49_HIZ_SL23 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr49_hiz_sl22, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR49_HIZ_SL22 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr49_hiz_sl21, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR49_HIZ_SL21 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr49_hiz_sl20, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR49_HIZ_SL20 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr49_hiz_sl19, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR49_HIZ_SL19 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr49_hiz_sl18, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR49_HIZ_SL18 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr49_hiz_sl17, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR49_HIZ_SL17 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr49_hiz_sl16, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR49_HIZ_SL16 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR49, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR50                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR50(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr50_hiz_sl31, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR50_HIZ_SL31 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr50_hiz_sl30, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR50_HIZ_SL30 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr50_hiz_sl29, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR50_HIZ_SL29 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr50_hiz_sl28, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR50_HIZ_SL28 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr50_hiz_sl27, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR50_HIZ_SL27 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr50_hiz_sl26, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR50_HIZ_SL26 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr50_hiz_sl25, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR50_HIZ_SL25 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr50_hiz_sl24, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR50_HIZ_SL24 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR50, value));
+}
+
+#endif /*  */
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR51                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR51(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr51_da12_voice,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR51_DA12_VOICE );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr51_swapda12_34,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR51_SWAP_DA12_34 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr51_sldai7toslado1,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR51_SLDAI7TOSLADO1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr51_sltoda1, AB8500_CODEC_MASK_FIVE_BITS,
+				    AB8500_CODEC_CR51_SLTODA1 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR51, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR52                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR52(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr52_sldai8toslado2,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR52_SLDAI8TOSLADO2 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr52_sltoda2, AB8500_CODEC_MASK_FIVE_BITS,
+				    AB8500_CODEC_CR52_SLTODA2 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR52, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR53                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR53(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr53_da34_voice,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR53_DA34_VOICE );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr53_sldai7toslado3,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR53_SLDAI7TOSLADO3 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr53_sltoda3, AB8500_CODEC_MASK_FIVE_BITS,
+				    AB8500_CODEC_CR53_SLTODA3 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR53, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR54                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR54(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr54_sldai8toslado4,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR54_SLDAI8TOSLADO4 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr54_sltoda4, AB8500_CODEC_MASK_FIVE_BITS,
+				    AB8500_CODEC_CR54_SLTODA4 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR54, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR55                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR55(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr55_da56_voice,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR55_DA56_VOICE );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr55_sldai7toslado5,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR55_SLDAI7TOSLADO5 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr55_sltoda5, AB8500_CODEC_MASK_FIVE_BITS,
+				    AB8500_CODEC_CR55_SLTODA5 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR55, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR56                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR56(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr56_sldai8toslado6,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR56_SLDAI8TOSLADO6 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr56_sltoda6, AB8500_CODEC_MASK_FIVE_BITS,
+				    AB8500_CODEC_CR56_SLTODA6 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR56, value));
+}
+
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR57                                                          */
+/********************************************************************************************/
+    PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR57(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr57_sldai8toslado7,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR57_SLDAI8TOSLADO7 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr57_sltoda7, AB8500_CODEC_MASK_FIVE_BITS,
+				    AB8500_CODEC_CR57_SLTODA7 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR57, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR58                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR58(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr58_sldai7toslado8,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR58_SLDAI7TOSLADO8 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr58_sltoda8, AB8500_CODEC_MASK_FIVE_BITS,
+				    AB8500_CODEC_CR58_SLTODA8 );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR58, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR59                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR59(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr59_parlhf, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR59_PARLHF );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr59_parlvib, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR59_PARLVIB );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr59_classdvib1_swapen,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR59_CLASSDVIB1SWAPEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr59_classdvib2_swapen,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR59_CLASSDVIB2SWAPEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr59_classdhfl_swapen,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR59_CLASSDHFLSWAPEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr59_classdhfr_swapen,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR59_CLASSDHFRSWAPEN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR59, value));
+}
+
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR60                                                          */
+/********************************************************************************************/
+    PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR60(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr60_classd_firbyp,
+				     AB8500_CODEC_MASK_FOUR_BITS,
+				     AB8500_CODEC_CR60_CLASSD_FIR_BYP );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr60_classd_highvolen,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR60_CLASSD_HIGHVOL_EN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR60, value));
+}
+
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR61                                                          */
+/********************************************************************************************/
+    PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR61(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+
+	/* 5 bits are Read Only */
+	AB8500_CODEC_WRITE_BITS
+	    (value,
+	     (t_uint8) p_ab8500_codec_configuration->cr61_classddith_hpgain,
+	     AB8500_CODEC_MASK_FOUR_BITS,
+	     AB8500_CODEC_CR61_CLASSD_DITH_HPGAIN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr61_classddith_wgain,
+				    AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR61_CLASSD_DITH_WGAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR61, value));
+}
+
+#endif /*  */
+
+/* CR62 is Read Only */
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR63                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR63(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr63_datohslen, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR63_DATOHSLEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr63_datohsren, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR63_DATOHSREN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr63_ad1sel, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR63_AD1SEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr63_ad2sel, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR63_AD2SEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr63_ad3sel, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR63_AD3SEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr63_ad5sel, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR63_AD5SEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr63_ad6sel, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR63_AD6SEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr63_ancsel, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR63_ANCSEL );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR63, value));
+}
+
+#if 0
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR64                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR64(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr64_datohfren, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR64_DATOHFREN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr64_datohflen, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR64_DATOHFLEN );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr64_hfrsel, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR64_HFRSEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr64_hflsel, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR64_HFLSEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr64_stfir1sel, AB8500_CODEC_MASK_TWO_BITS,
+				    AB8500_CODEC_CR64_STFIR1SEL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr64_stfir2sel, AB8500_CODEC_MASK_TWO_BITS,
+				    AB8500_CODEC_CR64_STFIR2SEL );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR64, value));
+}
+
+#endif /*  */
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR65                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR65(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr65_fadedis_ad1,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR65_FADEDIS_AD1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr65_ad1gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR65_AD1GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR65, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR66                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR66(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr66_fadedis_ad2,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR66_FADEDIS_AD2 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr66_ad2gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR66_AD2GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR66, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR67                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR67(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr67_fadedis_ad3,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR67_FADEDIS_AD3 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr67_ad3gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR67_AD3GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR67, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR68                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR68(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr68_fadedis_ad4,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR68_FADEDIS_AD4 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr68_ad4gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR68_AD4GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR68, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR69                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR69(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr69_fadedis_ad5,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR69_FADEDIS_AD5 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr69_ad5gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR69_AD5GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR69, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR70                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR70(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr70_fadedis_ad6,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR70_FADEDIS_AD6 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr70_ad6gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR70_AD6GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR70, value));
+}
+
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR71                                                          */
+/********************************************************************************************/
+    PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR71(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr71_fadedis_da1,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR71_FADEDIS_DA1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr71_da1gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR71_DA1GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR71, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR72                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR72(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr72_fadedis_da2,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR72_FADEDIS_DA2 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr72_da2gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR72_DA2GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR72, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR73                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR73(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr73_fadedis_da3,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR73_FADEDIS_DA3 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr73_da3gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR73_DA3GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR73, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR74                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR74(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr74_fadedis_da4,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR74_FADEDIS_DA4 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr74_da4gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR74_DA4GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR74, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR75                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR75(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr75_fadedis_da5,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR75_FADEDIS_DA5 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr75_da5gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR75_DA5GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR75, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR76                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR76(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr76_fadedis_da6,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR76_FADEDIS_DA6 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr76_da6gain, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR76_DA6GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR76, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR77                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR77(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr77_fadedis_ad1l,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR77_FADEDIS_AD1L );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr77_ad1lbgain_to_hfl,
+				    AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR77_AD1LBGAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR77, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR78                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR78(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr78_fadedis_ad2l,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR78_FADEDIS_AD2L );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr78_ad2lbgain_to_hfr,
+				    AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR78_AD2LBGAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR78, value));
+}
+
+#endif /*  */
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR79                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR79(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr79_hssinc1, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR79_HSSINC1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr79_fadedis_hsl,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR79_FADEDIS_HSL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr79_hsldgain, AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR79_HSLDGAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR79, value));
+}
+
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR80                                                          */
+/********************************************************************************************/
+    PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR80(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr80_fade_speed,
+				     AB8500_CODEC_MASK_TWO_BITS,
+				     AB8500_CODEC_CR80_FADE_SPEED );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr80_fadedis_hsr,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR80_FADEDIS_HSR );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr80_hsrdgain, AB8500_CODEC_MASK_FOUR_BITS,
+				    AB8500_CODEC_CR80_HSRDGAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR80, value));
+}
+
+#if 0
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR81                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR81(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr81_stfir1gain,
+				     AB8500_CODEC_MASK_FIVE_BITS,
+				     AB8500_CODEC_CR81_STFIR1GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR81, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR82                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR82(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr82_stfir2gain,
+				     AB8500_CODEC_MASK_FIVE_BITS,
+				     AB8500_CODEC_CR82_STFIR2GAIN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR82, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR83                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR83(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr83_enanc, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR83_ENANC );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr83_anciirinit, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR83_ANCIIRINIT );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr83_ancfirupdate,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR83_ANCFIRUPDATE );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR83, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR84                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR84(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr84_ancinshift,
+				     AB8500_CODEC_MASK_FIVE_BITS,
+				     AB8500_CODEC_CR84_ANCINSHIFT );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR84, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR85                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR85(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr85_ancfiroutshift,
+				     AB8500_CODEC_MASK_FIVE_BITS,
+				     AB8500_CODEC_CR85_ANCFIROUTSHIFT );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR85, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR86                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR86(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr86_ancshiftout,
+				     AB8500_CODEC_MASK_FIVE_BITS,
+				     AB8500_CODEC_CR86_ANCSHIFTOUT );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR86, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR87                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR87(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr87_ancfircoeff_msb,
+				     AB8500_CODEC_MASK_EIGHT_BITS,
+				     AB8500_CODEC_CR87_ANCFIRCOEFF_MSB );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR87, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR88                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR88(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr88_ancfircoeff_lsb,
+				     AB8500_CODEC_MASK_EIGHT_BITS,
+				     AB8500_CODEC_CR88_ANCFIRCOEFF_LSB );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR88, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR89                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR89(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr89_anciircoeff_msb,
+				     AB8500_CODEC_MASK_EIGHT_BITS,
+				     AB8500_CODEC_CR89_ANCIIRCOEFF_MSB );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR89, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR90                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR90(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr90_anciircoeff_lsb,
+				     AB8500_CODEC_MASK_EIGHT_BITS,
+				     AB8500_CODEC_CR90_ANCIIRCOEFF_LSB );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR90, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR91                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR91(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr91_ancwarpdel_msb,
+				     AB8500_CODEC_MASK_EIGHT_BITS,
+				     AB8500_CODEC_CR91_ANCWARPDEL_MSB );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR91, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR92                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR92(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr92_ancwarpdel_lsb,
+				     AB8500_CODEC_MASK_EIGHT_BITS,
+				     AB8500_CODEC_CR92_ANCWARPDEL_LSB );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR92, value));
+}
+
+/* CR93 is Read Only */
+/* CR94 is Read Only */
+/* CR95 is Read Only */
+/* CR96 is Read Only */
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR97                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR97(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr97_stfir_set, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR97_STFIR_SET );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr97_stfir_addr,
+				    AB8500_CODEC_MASK_SEVEN_BITS,
+				    AB8500_CODEC_CR97_STFIR_ADDR );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR97, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR98                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR98(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr98_stfir_coeff_msb,
+				     AB8500_CODEC_MASK_EIGHT_BITS,
+				     AB8500_CODEC_CR98_STFIR_COEFF_MSB );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR98, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR99                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR99(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr99_stfir_coeff_lsb,
+				     AB8500_CODEC_MASK_EIGHT_BITS,
+				     AB8500_CODEC_CR99_STFIR_COEFF_LSB );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR99, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR100                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR100(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr100_enstfirs, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR100_ENSTFIRS );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr100_stfirstoif1,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR100_STFIRSTOIF1 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr100_stfir_busy,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR100_STFIR_BUSY );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR100, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR101                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR101(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr101_hsoffst_mask,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR101_HSOFFSTMASK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr101_fifofull_mask,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR101_FIFOFULLMASK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr101_fifoempty_mask,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR101_FIFOEMPTYMASK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr101_dasat_mask,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR101_DASATMASK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr101_adsat_mask,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR101_ADSATMASK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr101_addsp_mask,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR101_ADDSPMASK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr101_dadsp_mask,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR101_DADSPMASK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr101_firsid_mask,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR101_FIRSIDMASK );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR101, value));
+}
+
+/* CR102 is Read Only */
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR103                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR103(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr103_vssready_mask,
+				     AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR103_VSSREADYMASK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr103_shorthsl_mask,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR103_SHORTHSLMASK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr103_shorthsr_mask,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR103_SHORTHSRMASK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr103_shortear_mask,
+				    AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR103_SHORTEARMASK );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR103, value));
+}
+
+#endif /*  */
+
+/* CR104 is Read Only */
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR105                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR105(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr105_bfifomsk, AB8500_CODEC_MASK_ONE_BIT,
+				     AB8500_CODEC_CR105_BFIFOMASK );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr105_bfifoint, AB8500_CODEC_MASK_SIX_BITS,
+				    AB8500_CODEC_CR105_BFIFOINT );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR105, value));
+}
+
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR106                                                          */
+/********************************************************************************************/
+    PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR106(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr106_bfifotx,
+				     AB8500_CODEC_MASK_EIGHT_BITS,
+				     AB8500_CODEC_CR106_BFIFOTX );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR106, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR107                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR107(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr107_bfifoexsl,
+				     AB8500_CODEC_MASK_THREE_BITS,
+				     AB8500_CODEC_CR107_BFIFOEXSL );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr107_prebitclk0,
+				    AB8500_CODEC_MASK_THREE_BITS,
+				    AB8500_CODEC_CR107_PREBITCLK0 );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr107_bfifomast, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR107_BFIFOMAST );
+	AB8500_CODEC_WRITE_BITS  (value,
+				    (t_uint8) p_ab8500_codec_configuration->
+				    cr107_bfiforun, AB8500_CODEC_MASK_ONE_BIT,
+				    AB8500_CODEC_CR107_BFIFORUN );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR107, value));
+}
+
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR108                                                          */
+/********************************************************************************************/
+    PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR108(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr108_bfifoframsw,
+				     AB8500_CODEC_MASK_EIGHT_BITS,
+				     AB8500_CODEC_CR108_BFIFOFRAMESW );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR108, value));
+}
+
+/********************************************************************************************/
+/* Name: ab8500_codec_UpdateCR109                                                          */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_UpdateCR109(void)
+{
+	t_uint8 value = 0x00;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	AB8500_CODEC_WRITE_BITS  (value,
+				     (t_uint8) p_ab8500_codec_configuration->
+				     cr109_bfifowakeup,
+				     AB8500_CODEC_MASK_EIGHT_BITS,
+				     AB8500_CODEC_CR109_BFIFOWAKEUP );
+	return (ab8500_codec_SingleWrite(AB8500_CODEC_CR109, value));
+}
+
+/* CR110 is Read Only */
+
+/* CR111 is Read Only */
+
+/********************************************************************************************/
+/* Name: ab8500_codec_Reset()                                                              */
+
+/********************************************************************************************/
+PRIVATE t_ab8500_codec_error ab8500_codec_Reset(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	p_ab8500_codec_configuration->cr1_swreset =
+	    AB8500_CODEC_CR1_SWRESET_ENABLED;
+	ab8500_codec_error = ab8500_codec_UpdateCR1();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ProgramDirection(IN
+							     t_ab8500_codec_direction
+							     ab8500_codec_direction)
+    /*only IN or OUT must be passed (not INOUT) */
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	if (AB8500_CODEC_DIRECTION_IN == ab8500_codec_direction)
+		 {
+		ab8500_codec_error = ab8500_codec_ProgramDirectionIN();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		}
+	if (AB8500_CODEC_DIRECTION_OUT == ab8500_codec_direction)
+		 {
+		ab8500_codec_error = ab8500_codec_ProgramDirectionOUT();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		}
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SetDirection(IN
+							 t_ab8500_codec_direction
+							 ab8500_codec_direction)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	switch (ab8500_codec_direction)
+		 {
+	case AB8500_CODEC_DIRECTION_IN:
+		ab8500_codec_error =
+		    ab8500_codec_ProgramDirection(AB8500_CODEC_DIRECTION_IN);
+		break;
+	case AB8500_CODEC_DIRECTION_OUT:
+		ab8500_codec_error =
+		    ab8500_codec_ProgramDirection(AB8500_CODEC_DIRECTION_OUT);
+		break;
+	case AB8500_CODEC_DIRECTION_INOUT:
+		ab8500_codec_error =
+		    ab8500_codec_ProgramDirection(AB8500_CODEC_DIRECTION_IN);
+		if (AB8500_CODEC_OK == ab8500_codec_error)
+			 {
+			ab8500_codec_error =
+			    ab8500_codec_ProgramDirection
+			    (AB8500_CODEC_DIRECTION_OUT);
+			}
+		break;
+		}
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR5();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR6();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR7();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR8();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR9();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR10();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR12();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR15();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR63();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_Init                                                  */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Initialize the global variables & stores the slave address of codec.     */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* slave_address_of_ab8500_codec: Audio codec slave address                 */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* Returns AB8500_CODEC_OK                                                  */
+/* COMMENTS:                                                                */
+/* 1) Saves the supplied slave_address_of_codec in global variable          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_Init(IN t_uint8
+					      slave_address_of_ab8500_codec)
+{
+	DBGENTER1(" (%lx)", slave_address_of_ab8500_codec);
+	g_ab8500_codec_system_context.slave_address_of_ab8500_codec =
+	    slave_address_of_ab8500_codec;
+	DBGEXIT(AB8500_CODEC_OK);
+	return (AB8500_CODEC_OK);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_Reset                                                 */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Reset the global variables and clear audiocodec settings to default.     */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_Reset(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER();
+	g_ab8500_codec_system_context.ab8500_codec_direction =
+	    AB8500_CODEC_DEFAULT_DIRECTION;
+	g_ab8500_codec_system_context.ab8500_codec_mode_in =
+	    AB8500_CODEC_DEFAULT_MODE_IN;
+	g_ab8500_codec_system_context.ab8500_codec_mode_out =
+	    AB8500_CODEC_DEFAULT_MODE_OUT;
+	g_ab8500_codec_system_context.ab8500_codec_src =
+	    AB8500_CODEC_DEFAULT_INPUT_SRC;
+	g_ab8500_codec_system_context.ab8500_codec_dest =
+	    AB8500_CODEC_DEFAULT_OUTPUT_DEST;
+	g_ab8500_codec_system_context.in_left_volume =
+	    AB8500_CODEC_DEFAULT_VOLUME_LEFT_IN;
+	g_ab8500_codec_system_context.in_right_volume =
+	    AB8500_CODEC_DEFAULT_VOLUME_RIGHT_IN;
+	g_ab8500_codec_system_context.out_left_volume =
+	    AB8500_CODEC_DEFAULT_VOLUME_LEFT_OUT;
+	g_ab8500_codec_system_context.out_right_volume =
+	    AB8500_CODEC_DEFAULT_VOLUME_RIGHT_OUT;
+	ab8500_codec_error = ab8500_codec_Reset();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetModeAndDirection                                   */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Configures the whole audio codec to work in audio mode                   */
+/* (using I2S protocol).                                                    */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* direction: select the direction (IN, OUT or INOUT)                       */
+/* in_mode: codec mode for recording. If direction is OUT only,             */
+/* this parameter is ignored.                                               */
+/* out_mode: codec mode for playing. If direction is IN only,               */
+/* this parameter is ignored.                                               */
+/* p_tdm_config: TDM configuration required to be configured by user        */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_UNSUPPORTED_FEATURE: The API may not allow setting          */
+/* 2 different modes, in which case it should return this value.            */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetModeAndDirection
+    (IN t_ab8500_codec_direction ab8500_codec_direction,
+     IN t_ab8500_codec_mode ab8500_codec_mode_in,
+     IN t_ab8500_codec_mode ab8500_codec_mode_out,
+     IN t_ab8500_codec_tdm_config const *const p_tdm_config ) {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	DBGENTER3(" (%lx %lx %lx)", ab8500_codec_direction,
+		    ab8500_codec_mode_in, ab8500_codec_mode_out);
+	if (AB8500_CODEC_AUDIO_INTERFACE_1 ==
+	       g_ab8500_codec_system_context.audio_interface)
+		 {
+		if (AB8500_CODEC_DIRECTION_OUT == ab8500_codec_direction
+		     || AB8500_CODEC_DIRECTION_INOUT ==
+		     ab8500_codec_direction )
+			 {
+			p_ab8500_codec_configuration->cr3_enda1 =
+			    AB8500_CODEC_CR3_ENDA1_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda2 =
+			    AB8500_CODEC_CR3_ENDA2_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda3 =
+			    AB8500_CODEC_CR3_ENDA3_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda4 =
+			    AB8500_CODEC_CR3_ENDA4_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda5 =
+			    AB8500_CODEC_CR3_ENDA5_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda6 =
+			    AB8500_CODEC_CR3_ENDA6_ENABLED;
+			p_ab8500_codec_configuration->cr27_if1_bitclk_osr =
+			    p_tdm_config->cr27_if1_bitclk_osr;
+			if (AB8500_CODEC_MODE_HIFI == ab8500_codec_mode_out)
+				 {
+				p_ab8500_codec_configuration->cr30_fsync1p =
+				    AB8500_CODEC_CR30_FSYNC1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_bitclk1p =
+				    AB8500_CODEC_CR30_BITCLK1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_if1del =
+				    AB8500_CODEC_CR30_IF1DEL_DELAYED;
+				p_ab8500_codec_configuration->cr30_if1format =
+				    AB8500_CODEC_CR30_IF1FORMAT_I2S_LEFTALIGNED;
+				p_ab8500_codec_configuration->cr30_if1wl =
+				    p_tdm_config->cr30_if1wl;
+				}
+
+			else
+				 {
+				p_ab8500_codec_configuration->cr30_fsync1p =
+				    AB8500_CODEC_CR30_FSYNC1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_bitclk1p =
+				    AB8500_CODEC_CR30_BITCLK1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_if1del =
+				    AB8500_CODEC_CR30_IF1DEL_DELAYED;
+				p_ab8500_codec_configuration->cr30_if1format =
+				    AB8500_CODEC_CR30_IF1FORMAT_TDM;
+				p_ab8500_codec_configuration->cr30_if1wl =
+				    p_tdm_config->cr30_if1wl;
+				}
+			}
+		if (AB8500_CODEC_DIRECTION_IN == ab8500_codec_direction
+		      || AB8500_CODEC_DIRECTION_INOUT ==
+		      ab8500_codec_direction )
+			 {
+			p_ab8500_codec_configuration->cr2_enad1 =
+			    AB8500_CODEC_CR2_ENAD1_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad2 =
+			    AB8500_CODEC_CR2_ENAD2_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad3 =
+			    AB8500_CODEC_CR2_ENAD3_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad4 =
+			    AB8500_CODEC_CR2_ENAD4_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad5 =
+			    AB8500_CODEC_CR2_ENAD5_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad6 =
+			    AB8500_CODEC_CR2_ENAD6_ENABLED;
+			p_ab8500_codec_configuration->cr27_if1_bitclk_osr =
+			    p_tdm_config->cr27_if1_bitclk_osr;
+			if (AB8500_CODEC_MODE_HIFI == ab8500_codec_mode_in)
+				 {
+				p_ab8500_codec_configuration->cr30_fsync1p =
+				    AB8500_CODEC_CR30_FSYNC1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_bitclk1p =
+				    AB8500_CODEC_CR30_BITCLK1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_if1del =
+				    AB8500_CODEC_CR30_IF1DEL_DELAYED;
+				p_ab8500_codec_configuration->cr30_if1format =
+				    AB8500_CODEC_CR30_IF1FORMAT_I2S_LEFTALIGNED;
+				p_ab8500_codec_configuration->cr30_if1wl =
+				    p_tdm_config->cr30_if1wl;
+				}
+
+			else
+				 {
+				p_ab8500_codec_configuration->cr30_fsync1p =
+				    AB8500_CODEC_CR30_FSYNC1P_RISING_EDGE;
+				p_ab8500_codec_configuration->cr30_bitclk1p =
+				    AB8500_CODEC_CR30_BITCLK1P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr30_if1del =
+				    AB8500_CODEC_CR30_IF1DEL_NOT_DELAYED;
+				p_ab8500_codec_configuration->cr30_if1format =
+				    AB8500_CODEC_CR30_IF1FORMAT_TDM;
+				p_ab8500_codec_configuration->cr30_if1wl =
+				    p_tdm_config->cr30_if1wl;
+				}
+			}
+		}
+
+	else
+		 {
+		if (AB8500_CODEC_DIRECTION_OUT == ab8500_codec_direction
+		     || AB8500_CODEC_DIRECTION_INOUT ==
+		     ab8500_codec_direction )
+			 {
+			p_ab8500_codec_configuration->cr3_enda1 =
+			    AB8500_CODEC_CR3_ENDA1_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda2 =
+			    AB8500_CODEC_CR3_ENDA2_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda3 =
+			    AB8500_CODEC_CR3_ENDA3_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda4 =
+			    AB8500_CODEC_CR3_ENDA4_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda5 =
+			    AB8500_CODEC_CR3_ENDA5_ENABLED;
+			p_ab8500_codec_configuration->cr3_enda6 =
+			    AB8500_CODEC_CR3_ENDA6_ENABLED;
+			p_ab8500_codec_configuration->cr27_if0_bitclk_osr =
+			    p_tdm_config->cr27_if0_bitclk_osr;
+			p_ab8500_codec_configuration->cr63_datohslen =
+			    AB8500_CODEC_CR63_DATOHSLEN_ENABLED;
+			p_ab8500_codec_configuration->cr63_datohsren =
+			    AB8500_CODEC_CR63_DATOHSREN_ENABLED;
+			if (AB8500_CODEC_MODE_HIFI == ab8500_codec_mode_out)
+				 {
+				p_ab8500_codec_configuration->cr28_fsync0p =
+				    AB8500_CODEC_CR28_FSYNC0P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr28_bitclk0p = p_tdm_config->cr28_bitclk0p;	/*AB8500_CODEC_CR28_BITCLK0P_FALLING_EDGE; */
+				p_ab8500_codec_configuration->cr28_if0del = p_tdm_config->cr28_if0del;	/*AB8500_CODEC_CR28_IF0DEL_DELAYED; */
+				p_ab8500_codec_configuration->cr28_if0format =
+				    AB8500_CODEC_CR28_IF0FORMAT_I2S_LEFTALIGNED;
+				p_ab8500_codec_configuration->cr28_if0wl =
+				    p_tdm_config->cr28_if0wl;
+				}
+
+			else
+				 {
+				p_ab8500_codec_configuration->cr28_fsync0p =
+				    AB8500_CODEC_CR28_FSYNC0P_FALLING_EDGE;
+				p_ab8500_codec_configuration->cr28_bitclk0p = p_tdm_config->cr28_bitclk0p;	/*AB8500_CODEC_CR28_BITCLK0P_FALLING_EDGE; */
+				p_ab8500_codec_configuration->cr28_if0del = p_tdm_config->cr28_if0del;	/*AB8500_CODEC_CR28_IF0DEL_DELAYED; */
+				p_ab8500_codec_configuration->cr28_if0format =
+				    AB8500_CODEC_CR28_IF0FORMAT_TDM;
+				p_ab8500_codec_configuration->cr28_if0wl =
+				    p_tdm_config->cr28_if0wl;
+				}
+			}
+		if (AB8500_CODEC_DIRECTION_IN == ab8500_codec_direction
+		      || AB8500_CODEC_DIRECTION_INOUT ==
+		      ab8500_codec_direction )
+			 {
+			p_ab8500_codec_configuration->cr2_enad1 =
+			    AB8500_CODEC_CR2_ENAD1_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad2 =
+			    AB8500_CODEC_CR2_ENAD2_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad3 =
+			    AB8500_CODEC_CR2_ENAD3_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad4 =
+			    AB8500_CODEC_CR2_ENAD4_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad5 =
+			    AB8500_CODEC_CR2_ENAD5_ENABLED;
+			p_ab8500_codec_configuration->cr2_enad6 =
+			    AB8500_CODEC_CR2_ENAD6_ENABLED;
+			p_ab8500_codec_configuration->cr26_ad1_voice =
+			    AB8500_CODEC_CR26_AD1_VOICE_LOWLATENCYFILTER;
+			p_ab8500_codec_configuration->cr26_ad2_voice =
+			    AB8500_CODEC_CR26_AD2_VOICE_LOWLATENCYFILTER;
+			p_ab8500_codec_configuration->cr26_ad3_voice =
+			    AB8500_CODEC_CR26_AD3_VOICE_LOWLATENCYFILTER;
+			p_ab8500_codec_configuration->cr26_ad4_voice =
+			    AB8500_CODEC_CR26_AD4_VOICE_LOWLATENCYFILTER;
+			p_ab8500_codec_configuration->cr27_if0_bitclk_osr =
+			    p_tdm_config->cr27_if0_bitclk_osr;
+			if (AB8500_CODEC_MODE_HIFI == ab8500_codec_mode_in)
+				 {
+				p_ab8500_codec_configuration->cr28_fsync0p =
+				    AB8500_CODEC_CR28_FSYNC0P_RISING_EDGE;
+				p_ab8500_codec_configuration->cr28_bitclk0p = p_tdm_config->cr28_bitclk0p;	/*AB8500_CODEC_CR28_BITCLK0P_RISING_EDGE; */
+				p_ab8500_codec_configuration->cr28_if0del = p_tdm_config->cr28_if0del;	/*AB8500_CODEC_CR28_IF0DEL_NOT_DELAYED; */
+				p_ab8500_codec_configuration->cr28_if0format =
+				    AB8500_CODEC_CR28_IF0FORMAT_I2S_LEFTALIGNED;
+				p_ab8500_codec_configuration->cr28_if0wl =
+				    p_tdm_config->cr28_if0wl;
+				}
+
+			else
+				 {
+				p_ab8500_codec_configuration->cr28_fsync0p =
+				    AB8500_CODEC_CR28_FSYNC0P_RISING_EDGE;
+				p_ab8500_codec_configuration->cr28_bitclk0p = p_tdm_config->cr28_bitclk0p;	/*AB8500_CODEC_CR28_BITCLK0P_FALLING_EDGE; */
+				p_ab8500_codec_configuration->cr28_if0del = p_tdm_config->cr28_if0del;	/*AB8500_CODEC_CR28_IF0DEL_NOT_DELAYED; */
+				p_ab8500_codec_configuration->cr28_if0format =
+				    AB8500_CODEC_CR28_IF0FORMAT_TDM;
+				p_ab8500_codec_configuration->cr28_if0wl =
+				    p_tdm_config->cr28_if0wl;
+				}
+			}
+		}
+	ab8500_codec_error = ab8500_codec_SetModeAndDirectionUpdateCR();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	g_ab8500_codec_system_context.ab8500_codec_direction =
+	    ab8500_codec_direction;
+	g_ab8500_codec_system_context.ab8500_codec_mode_in =
+	    ab8500_codec_mode_in;
+	g_ab8500_codec_system_context.ab8500_codec_mode_out =
+	    ab8500_codec_mode_out;
+	ab8500_codec_error =
+	    ab8500_codec_SetDirection(ab8500_codec_direction);
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetSrcVolume                                          */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Sets the record volumes.                                                 */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* t_ab8500_codec_src: select source device for recording.                  */
+/* in_left_volume: record volume for left channel.                          */
+/* in_right_volume: record volume for right channel.                        */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetSrcVolume
+    (IN t_ab8500_codec_src src_device, IN t_uint8 in_left_volume,
+     IN t_uint8 in_right_volume )  {
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	DBGENTER3(" (%lx %lx %lx)", src_device, in_left_volume,
+		   in_right_volume);
+	if (in_left_volume > AB8500_CODEC_MAX_VOLUME)
+		 {
+		in_left_volume = AB8500_CODEC_MAX_VOLUME;
+		}
+	if (in_right_volume > AB8500_CODEC_MAX_VOLUME)
+		 {
+		in_right_volume = AB8500_CODEC_MAX_VOLUME;
+		}
+	g_ab8500_codec_system_context.in_left_volume = in_left_volume;
+	g_ab8500_codec_system_context.in_right_volume = in_right_volume;
+	p_ab8500_codec_configuration->cr65_ad1gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr66_ad2gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr67_ad3gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr68_ad4gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr69_ad5gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr70_ad6gain =
+	    AB8500_CODEC_AD_D_VOLUME_MIN +
+	    (in_left_volume *
+	     (AB8500_CODEC_AD_D_VOLUME_MAX -
+	      AB8500_CODEC_AD_D_VOLUME_MIN)) / 100;
+
+	    /* Set mininimum volume if volume is zero */
+	    switch (src_device)
+		 {
+	case AB8500_CODEC_SRC_LINEIN:
+		p_ab8500_codec_configuration->cr23_linl_gain =
+		    AB8500_CODEC_LINEIN_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_LINEIN_VOLUME_MAX -
+		      AB8500_CODEC_LINEIN_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr23_linr_gain =
+		    AB8500_CODEC_LINEIN_VOLUME_MIN +
+		    (in_right_volume *
+		     (AB8500_CODEC_LINEIN_VOLUME_MAX -
+		      AB8500_CODEC_LINEIN_VOLUME_MIN)) / 100;
+		break;
+	case AB8500_CODEC_SRC_MICROPHONE_1A:
+	case AB8500_CODEC_SRC_MICROPHONE_1B:
+		p_ab8500_codec_configuration->cr20_mic1_gain =
+		    AB8500_CODEC_MIC_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_MIC_VOLUME_MAX -
+		      AB8500_CODEC_MIC_VOLUME_MIN)) / 100;
+		break;
+	case AB8500_CODEC_SRC_MICROPHONE_2:
+		p_ab8500_codec_configuration->cr21_mic2_gain =
+		    AB8500_CODEC_MIC_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_MIC_VOLUME_MAX -
+		      AB8500_CODEC_MIC_VOLUME_MIN)) / 100;
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_1:
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_2:
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_3:
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_4:
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_5:
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_6:
+		break;
+	case AB8500_CODEC_SRC_ALL:
+		p_ab8500_codec_configuration->cr23_linl_gain =
+		    AB8500_CODEC_LINEIN_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_LINEIN_VOLUME_MAX -
+		      AB8500_CODEC_LINEIN_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr23_linr_gain =
+		    AB8500_CODEC_LINEIN_VOLUME_MIN +
+		    (in_right_volume *
+		     (AB8500_CODEC_LINEIN_VOLUME_MAX -
+		      AB8500_CODEC_LINEIN_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr20_mic1_gain =
+		    AB8500_CODEC_MIC_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_MIC_VOLUME_MAX -
+		      AB8500_CODEC_MIC_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr21_mic2_gain =
+		    AB8500_CODEC_MIC_VOLUME_MIN +
+		    (in_left_volume *
+		     (AB8500_CODEC_MIC_VOLUME_MAX -
+		      AB8500_CODEC_MIC_VOLUME_MIN)) / 100;
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_SetSrcVolumeUpdateCR();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetDestVolume                                         */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Sets the play volumes.                                                   */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* out_left_volume: play volume for left channel.                           */
+/* out_right_volume: play volume for right channel.                         */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetDestVolume
+    (IN t_ab8500_codec_dest dest_device, IN t_uint8 out_left_volume,
+     IN t_uint8 out_right_volume )  {
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	DBGENTER3(" (%lx %lx %lx)", dest_device, out_left_volume,
+		   out_right_volume);
+	if (out_left_volume > AB8500_CODEC_MAX_VOLUME)
+		 {
+		out_left_volume = AB8500_CODEC_MAX_VOLUME;
+		}
+	if (out_right_volume > AB8500_CODEC_MAX_VOLUME)
+		 {
+		out_right_volume = AB8500_CODEC_MAX_VOLUME;
+		}
+	g_ab8500_codec_system_context.out_left_volume = out_left_volume;
+	g_ab8500_codec_system_context.out_right_volume = out_right_volume;
+	p_ab8500_codec_configuration->cr71_da1gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr72_da2gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr73_da3gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr74_da4gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr75_da5gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr76_da6gain =
+	    AB8500_CODEC_DA_D_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_DA_D_VOLUME_MAX -
+	      AB8500_CODEC_DA_D_VOLUME_MIN)) / 100;
+
+	    /* Set mininimum volume if volume is zero */
+	    switch (dest_device)
+		 {
+	case AB8500_CODEC_DEST_HEADSET:
+		p_ab8500_codec_configuration->cr22_hsl_gain =
+		    AB8500_CODEC_HEADSET_VOLUME_MIN +
+		    (out_left_volume *
+		     (AB8500_CODEC_HEADSET_VOLUME_MAX -
+		      AB8500_CODEC_HEADSET_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr22_hsr_gain =
+		    AB8500_CODEC_HEADSET_VOLUME_MIN +
+		    (out_right_volume *
+		     (AB8500_CODEC_HEADSET_VOLUME_MAX -
+		      AB8500_CODEC_HEADSET_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr79_hsldgain =
+		    AB8500_CODEC_HEADSET_D_VOLUME_0DB;
+		p_ab8500_codec_configuration->cr80_hsrdgain =
+		    AB8500_CODEC_HEADSET_D_VOLUME_0DB;
+		break;
+	case AB8500_CODEC_DEST_EARPIECE:
+		p_ab8500_codec_configuration->cr79_hsldgain =
+		    AB8500_CODEC_HEADSET_D_VOLUME_0DB;
+		break;
+	case AB8500_CODEC_DEST_HANDSFREE:
+		break;
+	case AB8500_CODEC_DEST_VIBRATOR_L:
+		p_ab8500_codec_configuration->cr16_pwmnldutycycle =
+		    AB8500_CODEC_VIBRATOR_VOLUME_MIN;
+		p_ab8500_codec_configuration->cr17_pwmpldutycycle =
+		    AB8500_CODEC_VIBRATOR_VOLUME_MIN +
+		    (out_right_volume *
+		     (AB8500_CODEC_VIBRATOR_VOLUME_MAX -
+		      AB8500_CODEC_VIBRATOR_VOLUME_MIN)) / 100;
+		break;
+	case AB8500_CODEC_DEST_VIBRATOR_R:
+		p_ab8500_codec_configuration->cr18_pwmnrdutycycle =
+		    AB8500_CODEC_VIBRATOR_VOLUME_MIN;
+		p_ab8500_codec_configuration->cr19_pwmprdutycycle =
+		    AB8500_CODEC_VIBRATOR_VOLUME_MIN +
+		    (out_right_volume *
+		     (AB8500_CODEC_VIBRATOR_VOLUME_MAX -
+		      AB8500_CODEC_VIBRATOR_VOLUME_MIN)) / 100;
+		break;
+	case AB8500_CODEC_DEST_ALL:
+		p_ab8500_codec_configuration->cr22_hsl_gain =
+		    AB8500_CODEC_HEADSET_VOLUME_MIN +
+		    (out_left_volume *
+		     (AB8500_CODEC_HEADSET_VOLUME_MAX -
+		      AB8500_CODEC_HEADSET_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr22_hsr_gain =
+		    AB8500_CODEC_HEADSET_VOLUME_MIN +
+		    (out_right_volume *
+		     (AB8500_CODEC_HEADSET_VOLUME_MAX -
+		      AB8500_CODEC_HEADSET_VOLUME_MIN)) / 100;
+		p_ab8500_codec_configuration->cr79_hsldgain =
+		    AB8500_CODEC_HEADSET_D_VOLUME_0DB;
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_SetDestVolumeUpdateCR();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetMasterMode                                         */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Set the Audio Codec in Master mode.                                      */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN: t_codec_master_mode: Enable/disable master mode                      */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK: Call this API after calling AB8500_CODEC_SetModeAndDirection() API*/
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetMasterMode(IN
+						       t_ab8500_codec_master_mode
+						       mode)
+{
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER0();
+	if (AB8500_CODEC_AUDIO_INTERFACE_1 ==
+	      g_ab8500_codec_system_context.audio_interface)
+		 {
+		p_ab8500_codec_configuration->cr27_en_mastgen =
+		    AB8500_CODEC_CR27_EN_MASTGEN_ENABLED;
+		p_ab8500_codec_configuration->cr27_enfs_bitclk1 =
+		    AB8500_CODEC_CR27_ENFS_BITCLK1_ENABLED;
+		if (AB8500_CODEC_MASTER_MODE_ENABLE == mode)
+			 {
+			p_ab8500_codec_configuration->cr29_if1master =
+			    AB8500_CODEC_CR29_IF1MASTER_FS1CK1_OUTPUT;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr29_if1master =
+			    AB8500_CODEC_CR29_IF1MASTER_FS1CK1_INPUT;
+			}
+		}
+
+	else
+		 {
+		p_ab8500_codec_configuration->cr27_en_mastgen =
+		    AB8500_CODEC_CR27_EN_MASTGEN_ENABLED;
+		p_ab8500_codec_configuration->cr27_enfs_bitclk0 =
+		    AB8500_CODEC_CR27_ENFS_BITCLK0_ENABLED;
+		if (AB8500_CODEC_MASTER_MODE_ENABLE == mode)
+			 {
+			p_ab8500_codec_configuration->cr29_if0master =
+			    AB8500_CODEC_CR29_IF0MASTER_FS0CK0_OUTPUT;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr29_if0master =
+			    AB8500_CODEC_CR29_IF0MASTER_FS0CK0_INPUT;
+			}
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR27();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR29();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SelectInput                                           */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Select input source for recording.                                       */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* input_src: select input source for recording when several sources        */
+/* are supported in codec.                                                  */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_INVALID_PARAMETER: If input_src provided is invalid         */
+/* by the codec hardware in use.                                            */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SelectInput(IN t_ab8500_codec_src
+						     ab8500_codec_src)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER1(" (%lx)", ab8500_codec_src);
+	g_ab8500_codec_system_context.ab8500_codec_src = ab8500_codec_src;
+	ab8500_codec_error =
+	    ab8500_codec_SetDirection(AB8500_CODEC_DIRECTION_IN);
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SelectOutput                                          */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Select output desination for playing.                                    */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* output_dest: select output destination for playing when several are      */
+/* supported by codec hardware.                                             */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_INVALID_PARAMETER: If output_src provided is invalid        */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SelectOutput(IN t_ab8500_codec_dest
+						      ab8500_codec_dest)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	g_ab8500_codec_system_context.ab8500_codec_dest = ab8500_codec_dest;
+	DBGENTER1(" (%lx)", ab8500_codec_dest);
+	ab8500_codec_error =
+	    ab8500_codec_SetDirection(AB8500_CODEC_DIRECTION_OUT);
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_PowerDown                                             */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Shuts the audio codec down completely.                                   */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* OUT:                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_PowerDown(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	g_ab8500_codec_system_context.ab8500_codec_configuration.cr0_powerup =
+	    AB8500_CODEC_CR0_POWERUP_OFF;
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_PowerUp                                               */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Switch on the audio codec.                                               */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* CODEC_OK: if successful.                                                 */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_PowerUp(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER();
+	g_ab8500_codec_system_context.ab8500_codec_configuration.cr0_powerup =
+	    AB8500_CODEC_CR0_POWERUP_ON;
+	g_ab8500_codec_system_context.ab8500_codec_configuration.cr0_enaana =
+	    AB8500_CODEC_CR0_ENAANA_ON;
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SelectInterface                                       */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Select the Audio Interface 0 or 1.                                       */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN: t_ab8500_codec_audio_interface: The selected interface               */
+/*                                                                          */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_OK: Always.                                                 */
+/* REMARK: Call this API before using a function of the low level drivers   */
+/* to select the interface that you want to configure                       */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SelectInterface(IN
+							 t_ab8500_codec_audio_interface
+							 audio_interface)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER0();
+	g_ab8500_codec_system_context.audio_interface = audio_interface;
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_GetInterface                                          */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Get the Audio Interface 0 or 1.                                          */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT: p_audio_interface: Store the selected interface                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_OK: Always                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Re-Entrant                                                   */
+/* REENTRANCY ISSUES: No Issues                                             */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_GetInterface(OUT
+						      t_ab8500_codec_audio_interface
+						      * p_audio_interface)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER0();
+	*p_audio_interface = g_ab8500_codec_system_context.audio_interface;
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetAnalogLoopback                                     */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Sets Line-In to HeadSet loopback with the required gain.                 */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* out_left_volume: play volume for left channel.                           */
+/* out_right_volume: play volume for right channel.                         */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetAnalogLoopback(IN t_uint8
+							   out_left_volume,
+							   IN t_uint8
+							   out_right_volume)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER2(" (%lx %lx)", out_left_volume, out_right_volume);
+	if (out_left_volume > AB8500_CODEC_MAX_VOLUME)
+		 {
+		out_left_volume = AB8500_CODEC_MAX_VOLUME;
+		}
+	if (out_right_volume > AB8500_CODEC_MAX_VOLUME)
+		 {
+		out_right_volume = AB8500_CODEC_MAX_VOLUME;
+		}
+	g_ab8500_codec_system_context.out_left_volume = out_left_volume;
+	g_ab8500_codec_system_context.out_right_volume = out_right_volume;
+	p_ab8500_codec_configuration->cr24_lintohsl_gain =
+	    AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MIN +
+	    (out_left_volume *
+	     (AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MAX -
+	      AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MIN)) / 100;
+	p_ab8500_codec_configuration->cr25_lintohsr_gain =
+	    AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MIN +
+	    (out_right_volume *
+	     (AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MAX -
+	      AB8500_CODEC_LINEIN_TO_HS_L_R_VOLUME_MIN)) / 100;
+	ab8500_codec_error = ab8500_codec_UpdateCR24();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR25();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_RemoveAnalogLoopback                                  */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Remove Line-In to HeadSet loopback.                                      */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_RemoveAnalogLoopback(void)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER0();
+	p_ab8500_codec_configuration->cr24_lintohsl_gain =
+	    AB8500_CODEC_LINEIN_TO_HS_L_R_LOOP_OPEN;
+	p_ab8500_codec_configuration->cr25_lintohsr_gain =
+	    AB8500_CODEC_LINEIN_TO_HS_L_R_LOOP_OPEN;
+	ab8500_codec_error = ab8500_codec_UpdateCR24();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR25();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_EnableBypassMode                                      */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Enables IF0 to IF1 path or vice versa                                    */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_EnableBypassMode(void)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER0();
+	if (AB8500_CODEC_AUDIO_INTERFACE_1 ==
+	       g_ab8500_codec_system_context.audio_interface)
+		 {
+		p_ab8500_codec_configuration->cr29_if1datoif0ad =
+		    AB8500_CODEC_CR29_IF1DATOIF0AD_SENT;
+		p_ab8500_codec_configuration->cr29_if1cktoif0ck =
+		    AB8500_CODEC_CR29_IF1CKTOIF0CK_SENT;
+		}
+
+	else
+		 {
+		p_ab8500_codec_configuration->cr29_if0datoif1ad =
+		    AB8500_CODEC_CR29_IF0DATOIF1AD_SENT;
+		p_ab8500_codec_configuration->cr29_if0cktoif1ck =
+		    AB8500_CODEC_CR29_IF0CKTOIF1CK_SENT;
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR29();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_DisableBypassMode                                     */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Disables IF0 to IF1 path or vice versa                                   */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_DisableBypassMode(void)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER0();
+	if (AB8500_CODEC_AUDIO_INTERFACE_1 ==
+	       g_ab8500_codec_system_context.audio_interface)
+		 {
+		p_ab8500_codec_configuration->cr29_if1datoif0ad =
+		    AB8500_CODEC_CR29_IF1DATOIF0AD_NOTSENT;
+		p_ab8500_codec_configuration->cr29_if1cktoif0ck =
+		    AB8500_CODEC_CR29_IF1CKTOIF0CK_NOTSENT;
+		}
+
+	else
+		 {
+		p_ab8500_codec_configuration->cr29_if0datoif1ad =
+		    AB8500_CODEC_CR29_IF0DATOIF1AD_NOTSENT;
+		p_ab8500_codec_configuration->cr29_if0cktoif1ck =
+		    AB8500_CODEC_CR29_IF0CKTOIF1CK_NOTSENT;
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR29();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SrcPowerControl                                       */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Enables/Disables & UnMute/Mute the desired source                        */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* t_ab8500_codec_src: select source device for enabling/disabling.         */
+/* t_ab8500_codec_src_state: Enable/Disable                                 */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SrcPowerControl(IN
+							 t_ab8500_codec_src
+							 src_device,
+							 t_ab8500_codec_src_state
+							 state)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	DBGENTER2(" (%lx %lx)", src_device, state);
+	if (src_device <= AB8500_CODEC_SRC_D_MICROPHONE_2)
+		 {
+		ab8500_codec_error =
+		    ab8500_codec_SrcPowerControlSwitch1(src_device, state);
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		}
+
+	else if (src_device <= AB8500_CODEC_SRC_ALL)
+		 {
+		ab8500_codec_error =
+		    ab8500_codec_SrcPowerControlSwitch2(src_device, state);
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		}
+
+	else
+		 {
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR5();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR6();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR7();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR63();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_DestPowerControl                                      */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Enables/Disables & UnMute/Mute the desired destination                   */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* t_ab8500_codec_dest: select destination device for enabling/disabling.   */
+/* t_ab8500_codec_dest_state: Enable/Disable                                */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_TRANSACTION_FAILED: If transaction fails.                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_DestPowerControl(IN
+							  t_ab8500_codec_dest
+							  dest_device,
+							  t_ab8500_codec_dest_state
+							  state)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.ab8500_codec_configuration;
+	DBGENTER2(" (%lx %lx)", dest_device, state);
+	switch (dest_device)
+		 {
+	case AB8500_CODEC_DEST_HEADSET:
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr8_enhsl =
+			    AB8500_CODEC_CR8_ENHSL_ENABLED;
+			p_ab8500_codec_configuration->cr8_enhsr =
+			    AB8500_CODEC_CR8_ENHSR_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutehsl =
+			    AB8500_CODEC_CR10_MUTEHSL_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutehsr =
+			    AB8500_CODEC_CR10_MUTEHSR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endachsl =
+			    AB8500_CODEC_CR9_ENDACHSL_ENABLED;
+			p_ab8500_codec_configuration->cr9_endachsr =
+			    AB8500_CODEC_CR9_ENDACHSR_ENABLED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr8_enhsl =
+			    AB8500_CODEC_CR8_ENHSL_DISABLED;
+			p_ab8500_codec_configuration->cr8_enhsr =
+			    AB8500_CODEC_CR8_ENHSR_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutehsl =
+			    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutehsr =
+			    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endachsl =
+			    AB8500_CODEC_CR9_ENDACHSL_DISABLED;
+			p_ab8500_codec_configuration->cr9_endachsr =
+			    AB8500_CODEC_CR9_ENDACHSR_DISABLED;
+			}
+		break;
+	case AB8500_CODEC_DEST_EARPIECE:
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr8_enear =
+			    AB8500_CODEC_CR8_ENEAR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endacear =
+			    AB8500_CODEC_CR9_ENDACEAR_ENABLED;
+			p_ab8500_codec_configuration->cr10_muteear =
+			    AB8500_CODEC_CR10_MUTEEAR_DISABLED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr8_enear =
+			    AB8500_CODEC_CR8_ENEAR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endacear =
+			    AB8500_CODEC_CR9_ENDACEAR_DISABLED;
+			p_ab8500_codec_configuration->cr10_muteear =
+			    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+			}
+		break;
+	case AB8500_CODEC_DEST_HANDSFREE:
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr8_enhfl =
+			    AB8500_CODEC_CR8_ENHFL_ENABLED;
+			p_ab8500_codec_configuration->cr8_enhfr =
+			    AB8500_CODEC_CR8_ENHFR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endachfl =
+			    AB8500_CODEC_CR9_ENDACHFL_ENABLED;
+			p_ab8500_codec_configuration->cr9_endachfr =
+			    AB8500_CODEC_CR9_ENDACHFR_ENABLED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr8_enhfl =
+			    AB8500_CODEC_CR8_ENHFL_DISABLED;
+			p_ab8500_codec_configuration->cr8_enhfr =
+			    AB8500_CODEC_CR8_ENHFR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endachfl =
+			    AB8500_CODEC_CR9_ENDACHFL_DISABLED;
+			p_ab8500_codec_configuration->cr9_endachfr =
+			    AB8500_CODEC_CR9_ENDACHFR_DISABLED;
+			}
+		break;
+	case AB8500_CODEC_DEST_VIBRATOR_L:
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr8_envibl =
+			    AB8500_CODEC_CR8_ENVIBL_ENABLED;
+			p_ab8500_codec_configuration->cr9_endacvibl =
+			    AB8500_CODEC_CR9_ENDACVIBL_ENABLED;
+			p_ab8500_codec_configuration->cr15_pwmtovibl =
+			    AB8500_CODEC_CR15_PWMTOVIBL_PWM;
+			p_ab8500_codec_configuration->cr15_pwmlctrl =
+			    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmnlctrl =
+			    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmplctrl =
+			    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLDUTYCYCLE;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr8_envibl =
+			    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+			p_ab8500_codec_configuration->cr9_endacvibl =
+			    AB8500_CODEC_CR9_ENDACVIBL_DISABLED;
+			p_ab8500_codec_configuration->cr15_pwmtovibl =
+			    AB8500_CODEC_CR15_PWMTOVIBL_DA_PATH;
+			p_ab8500_codec_configuration->cr15_pwmlctrl =
+			    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLGPOL;
+			p_ab8500_codec_configuration->cr15_pwmnlctrl =
+			    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLGPOL;
+			p_ab8500_codec_configuration->cr15_pwmplctrl =
+			    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLGPOL;
+			}
+		break;
+	case AB8500_CODEC_DEST_VIBRATOR_R:
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr8_envibr =
+			    AB8500_CODEC_CR8_ENVIBR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endacvibr =
+			    AB8500_CODEC_CR9_ENDACVIBR_ENABLED;
+			p_ab8500_codec_configuration->cr15_pwmtovibr =
+			    AB8500_CODEC_CR15_PWMTOVIBR_PWM;
+			p_ab8500_codec_configuration->cr15_pwmrctrl =
+			    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmnrctrl =
+			    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmprctrl =
+			    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRDUTYCYCLE;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr8_envibr =
+			    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endacvibr =
+			    AB8500_CODEC_CR9_ENDACVIBR_DISABLED;
+			p_ab8500_codec_configuration->cr15_pwmtovibr =
+			    AB8500_CODEC_CR15_PWMTOVIBR_DA_PATH;
+			p_ab8500_codec_configuration->cr15_pwmrctrl =
+			    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRGPOL;
+			p_ab8500_codec_configuration->cr15_pwmnrctrl =
+			    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRGPOL;
+			p_ab8500_codec_configuration->cr15_pwmprctrl =
+			    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRGPOL;
+			}
+		break;
+	case AB8500_CODEC_DEST_ALL:
+		if (AB8500_CODEC_DEST_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr8_enhsl =
+			    AB8500_CODEC_CR8_ENHSL_ENABLED;
+			p_ab8500_codec_configuration->cr8_enhsr =
+			    AB8500_CODEC_CR8_ENHSR_ENABLED;
+			p_ab8500_codec_configuration->cr8_enear =
+			    AB8500_CODEC_CR8_ENEAR_ENABLED;
+			p_ab8500_codec_configuration->cr8_enhfl =
+			    AB8500_CODEC_CR8_ENHFL_ENABLED;
+			p_ab8500_codec_configuration->cr8_enhfr =
+			    AB8500_CODEC_CR8_ENHFR_ENABLED;
+			p_ab8500_codec_configuration->cr8_envibl =
+			    AB8500_CODEC_CR8_ENVIBL_ENABLED;
+			p_ab8500_codec_configuration->cr8_envibr =
+			    AB8500_CODEC_CR8_ENVIBR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endacear =
+			    AB8500_CODEC_CR9_ENDACEAR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endachfl =
+			    AB8500_CODEC_CR9_ENDACHFL_ENABLED;
+			p_ab8500_codec_configuration->cr9_endachfr =
+			    AB8500_CODEC_CR9_ENDACHFR_ENABLED;
+			p_ab8500_codec_configuration->cr9_endacvibl =
+			    AB8500_CODEC_CR9_ENDACVIBL_ENABLED;
+			p_ab8500_codec_configuration->cr9_endacvibr =
+			    AB8500_CODEC_CR9_ENDACVIBR_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutehsl =
+			    AB8500_CODEC_CR10_MUTEHSL_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutehsr =
+			    AB8500_CODEC_CR10_MUTEHSR_DISABLED;
+			p_ab8500_codec_configuration->cr10_muteear =
+			    AB8500_CODEC_CR10_MUTEEAR_DISABLED;
+			p_ab8500_codec_configuration->cr15_pwmtovibl =
+			    AB8500_CODEC_CR15_PWMTOVIBL_PWM;
+			p_ab8500_codec_configuration->cr15_pwmlctrl =
+			    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmnlctrl =
+			    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmplctrl =
+			    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmtovibr =
+			    AB8500_CODEC_CR15_PWMTOVIBR_PWM;
+			p_ab8500_codec_configuration->cr15_pwmrctrl =
+			    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmnrctrl =
+			    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRDUTYCYCLE;
+			p_ab8500_codec_configuration->cr15_pwmprctrl =
+			    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRDUTYCYCLE;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr8_enhsl =
+			    AB8500_CODEC_CR8_ENHSL_DISABLED;
+			p_ab8500_codec_configuration->cr8_enhsr =
+			    AB8500_CODEC_CR8_ENHSR_DISABLED;
+			p_ab8500_codec_configuration->cr8_enear =
+			    AB8500_CODEC_CR8_ENEAR_DISABLED;
+			p_ab8500_codec_configuration->cr8_enhfl =
+			    AB8500_CODEC_CR8_ENHFL_DISABLED;
+			p_ab8500_codec_configuration->cr8_enhfr =
+			    AB8500_CODEC_CR8_ENHFR_DISABLED;
+			p_ab8500_codec_configuration->cr8_envibl =
+			    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+			p_ab8500_codec_configuration->cr8_envibr =
+			    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endacear =
+			    AB8500_CODEC_CR9_ENDACEAR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endachfl =
+			    AB8500_CODEC_CR9_ENDACHFL_DISABLED;
+			p_ab8500_codec_configuration->cr9_endachfr =
+			    AB8500_CODEC_CR9_ENDACHFR_DISABLED;
+			p_ab8500_codec_configuration->cr9_endacvibl =
+			    AB8500_CODEC_CR9_ENDACVIBL_DISABLED;
+			p_ab8500_codec_configuration->cr9_endacvibr =
+			    AB8500_CODEC_CR9_ENDACVIBR_DISABLED;
+			p_ab8500_codec_configuration->cr10_mutehsl =
+			    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+			p_ab8500_codec_configuration->cr10_mutehsr =
+			    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+			p_ab8500_codec_configuration->cr10_muteear =
+			    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+			p_ab8500_codec_configuration->cr15_pwmtovibl =
+			    AB8500_CODEC_CR15_PWMTOVIBL_DA_PATH;
+			p_ab8500_codec_configuration->cr15_pwmlctrl =
+			    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLGPOL;
+			p_ab8500_codec_configuration->cr15_pwmnlctrl =
+			    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLGPOL;
+			p_ab8500_codec_configuration->cr15_pwmplctrl =
+			    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLGPOL;
+			p_ab8500_codec_configuration->cr15_pwmtovibr =
+			    AB8500_CODEC_CR15_PWMTOVIBR_DA_PATH;
+			p_ab8500_codec_configuration->cr15_pwmrctrl =
+			    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRGPOL;
+			p_ab8500_codec_configuration->cr15_pwmnrctrl =
+			    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRGPOL;
+			p_ab8500_codec_configuration->cr15_pwmprctrl =
+			    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRGPOL;
+			}
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_DestPowerControlUpdateCR();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_GetVersion                                            */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* This routine populates the pVersion structure with                       */
+/* the current version of HCL.                                              */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* p_version: this parameter is used to return current HCL version.         */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_ERROR: if p_version is NULL.                                */
+/* AB8500_CODEC_OK: if successful                                           */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Re-Entrant                                                   */
+/* REENTRANCY ISSUES: No Issues                                             */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_GetVersion(OUT t_version * p_version)
+{
+	DBGENTER1(" (%lx)", p_version);
+	if (p_version != NULL)
+		 {
+		p_version->minor = AB8500_CODEC_HCL_MINOR_ID;
+		p_version->major = AB8500_CODEC_HCL_MAJOR_ID;
+		p_version->version = AB8500_CODEC_HCL_VERSION_ID;
+		DBGEXIT0(AB8500_CODEC_OK);
+		return (AB8500_CODEC_OK);
+		}
+
+	else
+		 {
+		DBGEXIT0(AB8500_CODEC_INVALID_PARAMETER);
+		return (AB8500_CODEC_INVALID_PARAMETER);
+		}
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_SetDbgLevel                                           */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Set the debug level used by the debug module (mask-like value).          */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* debug_level: debug level to be set                                       */
+/* OUT:                                                                     */
+/* None                                                                     */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_OK: always                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Re-Entrant                                                   */
+/* REENTRANCY ISSUES: No Issues                                             */
+
+/****************************************************************************/
+/*
+PUBLIC t_ab8500_codec_error AB8500_CODEC_SetDbgLevel(IN t_dbg_level dbg_level)
+{
+    DBGENTER1(" (%d)", dbg_level);
+    dbg_level = dbg_level;
+#ifdef __DEBUG
+    MY_DEBUG_LEVEL_VAR_NAME = dbg_level;
+#endif
+    DBGEXIT(AB8500_CODEC_OK);
+    return(AB8500_CODEC_OK);
+}
+ */
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_GetDbgLevel                                           */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Set the debug level used by the debug module (mask-like value).          */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* p_dbg_level: this parameter is used to return debug level.               */
+/*                                                                          */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_ERROR: if p_version is NULL.                                */
+/* AB8500_CODEC_OK: if successful                                           */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Re-Entrant                                                   */
+/* REENTRANCY ISSUES: No Issues                                             */
+
+/****************************************************************************/
+/*
+PUBLIC t_ab8500_codec_error AB8500_CODEC_GetDbgLevel(OUT t_dbg_level *p_dbg_level)
+{
+    if (NULL == p_dbg_level)
+    {
+        DBGEXIT(AB8500_CODEC_INVALID_PARAMETER);
+        return(AB8500_CODEC_INVALID_PARAMETER);
+    }
+
+#ifdef __DEBUG
+    * p_dbg_level = MY_DEBUG_LEVEL_VAR_NAME;
+#endif
+    DBGEXIT(AB8500_CODEC_OK);
+    return(AB8500_CODEC_OK);
+}
+*/
+/****************************************************************************/
+/* NAME: AB8500_CODEC_ADSlotAllocation                                      */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* AD Data Allocation in slots.                                             */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN: t_ab8500_codec_slot: The slot to be allocated.                       */
+/* IN: t_ab8500_codec_cr31_to_cr46_ad_data_allocation: The value            */
+/*                                                     to be allocated.     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_INVALID_PARAMETER: If invalid slot number                   */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK:                                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_ADSlotAllocation
+    (IN t_ab8500_codec_slot ad_slot,
+     IN t_ab8500_codec_cr31_to_cr46_ad_data_allocation value )  {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER2(" (%lx %lx)", ad_slot, value);
+	if (ad_slot <= AB8500_CODEC_SLOT7)
+		 {
+		ab8500_codec_error =
+		    ab8500_codec_ADSlotAllocationSwitch1(ad_slot, value);
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		}
+
+	else if (ad_slot <= AB8500_CODEC_SLOT15)
+		 {
+		ab8500_codec_error =
+		    ab8500_codec_ADSlotAllocationSwitch2(ad_slot, value);
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		}
+
+	else if (ad_slot <= AB8500_CODEC_SLOT23)
+		 {
+		ab8500_codec_error =
+		    ab8500_codec_ADSlotAllocationSwitch3(ad_slot, value);
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		}
+
+	else if (ad_slot <= AB8500_CODEC_SLOT31)
+		 {
+		ab8500_codec_error =
+		    ab8500_codec_ADSlotAllocationSwitch4(ad_slot, value);
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		}
+
+	else
+		 {
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_DASlotAllocation                                      */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Allocate the Audio Interface slot for DA paths.                          */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN: t_ab8500_codec_da_channel_number: Channel number 1/2/3/4/5/6         */
+/* IN: t_ab8500_codec_cr51_to_cr56_sltoda: Slot number                      */
+/*                                                                          */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_INVALID_PARAMETER: If invalid channel number                */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK:                                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_DASlotAllocation
+    (IN t_ab8500_codec_da_channel_number channel_number,
+     IN t_ab8500_codec_cr51_to_cr58_sltoda slot )  {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	t_ab8500_codec_cr0_powerup ab8500_codec_cr0_powerup;
+	DBGENTER2(" (%lx %lx)", channel_number, slot);
+	p_ab8500_codec_configuration->cr51_da12_voice =
+	    AB8500_CODEC_CR51_DA12_VOICE_LOWLATENCYFILTER;
+	switch (channel_number)
+		 {
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_1:
+		p_ab8500_codec_configuration->cr51_sltoda1 = slot;
+		break;
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_2:
+		p_ab8500_codec_configuration->cr52_sltoda2 = slot;
+		break;
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_3:
+		p_ab8500_codec_configuration->cr53_sltoda3 = slot;
+		break;
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_4:
+		p_ab8500_codec_configuration->cr54_sltoda4 = slot;
+		break;
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_5:
+		p_ab8500_codec_configuration->cr55_sltoda5 = slot;
+		break;
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_6:
+		p_ab8500_codec_configuration->cr56_sltoda6 = slot;
+		break;
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_7:
+		p_ab8500_codec_configuration->cr57_sltoda7 = slot;
+		break;
+	case AB8500_CODEC_DA_CHANNEL_NUMBER_8:
+		p_ab8500_codec_configuration->cr58_sltoda8 = slot;
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_cr0_powerup = p_ab8500_codec_configuration->cr0_powerup;
+	p_ab8500_codec_configuration->cr0_powerup =
+	    AB8500_CODEC_CR0_POWERUP_OFF;
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR51();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR52();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR53();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR54();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR55();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR56();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR57();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR58();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	p_ab8500_codec_configuration->cr0_powerup = ab8500_codec_cr0_powerup;
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_ConfigureBurstFifo                                    */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Configuration for Burst FIFO control                                     */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN: t_ab8500_codec_burst_fifo_config: structure for configuration of     */
+/*										 burst FIFO                         */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_INVALID_PARAMETER: If invalid parameter                     */
+/* AB8500_CODEC_UNSUPPORTED_FEATURE: If interface 1 selected                */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK:                                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+t_ab8500_codec_error AB8500_CODEC_ConfigureBurstFifo(IN
+						     t_ab8500_codec_burst_fifo_config
+						     const *const
+						     p_burst_fifo_config)
+{
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER1(" (%lx)", p_burst_fifo_config);
+	if (AB8500_CODEC_AUDIO_INTERFACE_0 ==
+	      g_ab8500_codec_system_context.audio_interface)
+		 {
+		if (AB8500_CODEC_CR27_EN_MASTGEN_ENABLED ==
+		     p_ab8500_codec_configuration->cr27_en_mastgen)
+			 {
+			p_ab8500_codec_configuration->cr105_bfifomsk =
+			    p_burst_fifo_config->cr105_bfifomsk;
+			p_ab8500_codec_configuration->cr105_bfifoint =
+			    p_burst_fifo_config->cr105_bfifoint;
+			p_ab8500_codec_configuration->cr106_bfifotx =
+			    p_burst_fifo_config->cr106_bfifotx;
+			p_ab8500_codec_configuration->cr107_bfifoexsl =
+			    p_burst_fifo_config->cr107_bfifoexsl;
+			p_ab8500_codec_configuration->cr107_bfifomast =
+			    p_burst_fifo_config->cr107_bfifomast;
+			p_ab8500_codec_configuration->cr107_bfiforun =
+			    p_burst_fifo_config->cr107_bfiforun;
+			p_ab8500_codec_configuration->cr108_bfifoframsw =
+			    p_burst_fifo_config->cr108_bfifoframsw;
+			p_ab8500_codec_configuration->cr109_bfifowakeup =
+			    p_burst_fifo_config->cr109_bfifowakeup;
+			ab8500_codec_error = ab8500_codec_UpdateCR105();
+			if (ab8500_codec_error != AB8500_CODEC_OK)
+				 {
+				DBGEXIT(ab8500_codec_error);
+				return (ab8500_codec_error);
+				}
+			ab8500_codec_error = ab8500_codec_UpdateCR106();
+			if (ab8500_codec_error != AB8500_CODEC_OK)
+				 {
+				DBGEXIT(ab8500_codec_error);
+				return (ab8500_codec_error);
+				}
+			ab8500_codec_error = ab8500_codec_UpdateCR107();
+			if (ab8500_codec_error != AB8500_CODEC_OK)
+				 {
+				DBGEXIT(ab8500_codec_error);
+				return (ab8500_codec_error);
+				}
+			ab8500_codec_error = ab8500_codec_UpdateCR108();
+			if (ab8500_codec_error != AB8500_CODEC_OK)
+				 {
+				DBGEXIT(ab8500_codec_error);
+				return (ab8500_codec_error);
+				}
+			ab8500_codec_error = ab8500_codec_UpdateCR109();
+			if (ab8500_codec_error != AB8500_CODEC_OK)
+				 {
+				DBGEXIT(ab8500_codec_error);
+				return (ab8500_codec_error);
+				}
+			}
+
+		else
+			 {
+			ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		}
+
+	else
+		 {
+		ab8500_codec_error = AB8500_CODEC_UNSUPPORTED_FEATURE;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_EnableBurstFifo                                       */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Enable the Burst FIFO for Interface 0                                    */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_UNSUPPORTED_FEATURE: If Interface 1 is selected             */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK:                                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_EnableBurstFifo(void)
+{
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER0();
+	if (AB8500_CODEC_AUDIO_INTERFACE_0 ==
+	      g_ab8500_codec_system_context.audio_interface)
+		 {
+		p_ab8500_codec_configuration->cr29_if0bfifoen =
+		    AB8500_CODEC_CR29_IF0BFIFOEN_BURST_MODE;
+		ab8500_codec_error = ab8500_codec_UpdateCR29();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		}
+
+	else
+		 {
+		ab8500_codec_error = AB8500_CODEC_UNSUPPORTED_FEATURE;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+/****************************************************************************/
+/* NAME: AB8500_CODEC_DisableBurstFifo                                      */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION:                                                             */
+/* Disable the Burst FIFO for Interface 0                                   */
+/*                                                                          */
+/* ARGUMENTS                                                                */
+/* IN:                                                                      */
+/* None                                                                     */
+/* OUT:                                                                     */
+/* None                                                                     */
+/* RETURN:                                                                  */
+/* AB8500_CODEC_UNSUPPORTED_FEATURE: If Interface 1 is selected             */
+/* AB8500_CODEC_OK: if successful.                                          */
+/* REMARK:                                                                  */
+/*--------------------------------------------------------------------------*/
+/* REENTRANCY: Non Re-Entrant                                               */
+
+/****************************************************************************/
+PUBLIC t_ab8500_codec_error AB8500_CODEC_DisableBurstFifo(void)
+{
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	DBGENTER0();
+	if (AB8500_CODEC_AUDIO_INTERFACE_0 ==
+	      g_ab8500_codec_system_context.audio_interface)
+		 {
+		p_ab8500_codec_configuration->cr29_if0bfifoen =
+		    AB8500_CODEC_CR29_IF0BFIFOEN_NORMAL_MODE;
+		ab8500_codec_error = ab8500_codec_UpdateCR29();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		}
+
+	else
+		 {
+		ab8500_codec_error = AB8500_CODEC_UNSUPPORTED_FEATURE;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch1
+    (IN t_ab8500_codec_slot ad_slot,
+     IN t_ab8500_codec_cr31_to_cr46_ad_data_allocation value )  {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	switch (ad_slot)
+		 {
+	case AB8500_CODEC_SLOT0:
+		p_ab8500_codec_configuration->cr31_adotoslot0 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR31();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT1:
+		p_ab8500_codec_configuration->cr31_adotoslot1 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR31();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT2:
+		p_ab8500_codec_configuration->cr32_adotoslot2 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR32();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT3:
+		p_ab8500_codec_configuration->cr32_adotoslot3 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR32();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT4:
+		p_ab8500_codec_configuration->cr33_adotoslot4 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR33();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT5:
+		p_ab8500_codec_configuration->cr33_adotoslot5 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR33();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT6:
+		p_ab8500_codec_configuration->cr34_adotoslot6 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR34();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT7:
+		p_ab8500_codec_configuration->cr34_adotoslot7 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR34();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch2
+    (IN t_ab8500_codec_slot ad_slot,
+     IN t_ab8500_codec_cr31_to_cr46_ad_data_allocation value )  {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	switch (ad_slot)
+		 {
+	case AB8500_CODEC_SLOT8:
+		p_ab8500_codec_configuration->cr35_adotoslot8 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR35();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT9:
+		p_ab8500_codec_configuration->cr35_adotoslot9 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR35();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT10:
+		p_ab8500_codec_configuration->cr36_adotoslot10 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR36();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT11:
+		p_ab8500_codec_configuration->cr36_adotoslot11 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR36();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT12:
+		p_ab8500_codec_configuration->cr37_adotoslot12 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR37();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT13:
+		p_ab8500_codec_configuration->cr37_adotoslot13 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR37();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT14:
+		p_ab8500_codec_configuration->cr38_adotoslot14 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR38();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT15:
+		p_ab8500_codec_configuration->cr38_adotoslot15 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR38();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch3
+    (IN t_ab8500_codec_slot ad_slot,
+     IN t_ab8500_codec_cr31_to_cr46_ad_data_allocation value )  {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	switch (ad_slot)
+		 {
+	case AB8500_CODEC_SLOT16:
+		p_ab8500_codec_configuration->cr39_adotoslot16 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR39();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT17:
+		p_ab8500_codec_configuration->cr39_adotoslot17 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR39();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT18:
+		p_ab8500_codec_configuration->cr40_adotoslot18 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR40();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT19:
+		p_ab8500_codec_configuration->cr40_adotoslot19 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR40();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT20:
+		p_ab8500_codec_configuration->cr41_adotoslot20 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR41();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT21:
+		p_ab8500_codec_configuration->cr41_adotoslot21 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR41();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT22:
+		p_ab8500_codec_configuration->cr42_adotoslot22 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR42();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT23:
+		p_ab8500_codec_configuration->cr42_adotoslot23 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR42();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ADSlotAllocationSwitch4
+    (IN t_ab8500_codec_slot ad_slot,
+     IN t_ab8500_codec_cr31_to_cr46_ad_data_allocation value )  {
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	switch (ad_slot)
+		 {
+	case AB8500_CODEC_SLOT24:
+		p_ab8500_codec_configuration->cr43_adotoslot24 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR43();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT25:
+		p_ab8500_codec_configuration->cr43_adotoslot25 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR43();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT26:
+		p_ab8500_codec_configuration->cr44_adotoslot26 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR44();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT27:
+		p_ab8500_codec_configuration->cr44_adotoslot27 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR44();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT28:
+		p_ab8500_codec_configuration->cr45_adotoslot28 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR45();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT29:
+		p_ab8500_codec_configuration->cr45_adotoslot29 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR45();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT30:
+		p_ab8500_codec_configuration->cr46_adotoslot30 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR46();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	case AB8500_CODEC_SLOT31:
+		p_ab8500_codec_configuration->cr46_adotoslot31 = value;
+		ab8500_codec_error = ab8500_codec_UpdateCR46();
+		if (ab8500_codec_error != AB8500_CODEC_OK)
+			 {
+			DBGEXIT(ab8500_codec_error);
+			return (ab8500_codec_error);
+			}
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SrcPowerControlSwitch1(IN
+								   t_ab8500_codec_src
+								   src_device,
+								   t_ab8500_codec_src_state
+								   state)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	switch (src_device)
+		 {
+	case AB8500_CODEC_SRC_LINEIN:
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr5_enlinl =
+			    AB8500_CODEC_CR5_ENLINL_ENABLED;
+			p_ab8500_codec_configuration->cr5_enlinr =
+			    AB8500_CODEC_CR5_ENLINR_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutlinl =
+			    AB8500_CODEC_CR5_MUTLINL_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutlinr =
+			    AB8500_CODEC_CR5_MUTLINR_DISABLED;
+			p_ab8500_codec_configuration->cr7_linrsel =
+			    AB8500_CODEC_CR7_LINRSEL_LINR;
+			p_ab8500_codec_configuration->cr7_enadclinl =
+			    AB8500_CODEC_CR7_ENADCLINL_ENABLED;
+			p_ab8500_codec_configuration->cr7_enadclinr =
+			    AB8500_CODEC_CR7_ENADCLINR_ENABLED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr5_enlinl =
+			    AB8500_CODEC_CR5_ENLINL_DISABLED;
+			p_ab8500_codec_configuration->cr5_enlinr =
+			    AB8500_CODEC_CR5_ENLINR_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutlinl =
+			    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutlinr =
+			    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+			p_ab8500_codec_configuration->cr7_linrsel =
+			    AB8500_CODEC_CR7_LINRSEL_MIC2;
+			p_ab8500_codec_configuration->cr7_enadclinl =
+			    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+			p_ab8500_codec_configuration->cr7_enadclinr =
+			    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+			}
+		break;
+	case AB8500_CODEC_SRC_MICROPHONE_1A:
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr7_mic1sel =
+			    AB8500_CODEC_CR7_MIC1SEL_MIC1A;
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_DISABLED;
+			}
+		break;
+	case AB8500_CODEC_SRC_MICROPHONE_1B:
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr7_mic1sel =
+			    AB8500_CODEC_CR7_MIC1SEL_MIC1B;
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_DISABLED;
+			}
+		break;
+	case AB8500_CODEC_SRC_MICROPHONE_2:
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr5_enmic2 =
+			    AB8500_CODEC_CR5_ENMIC2_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutmic2 =
+			    AB8500_CODEC_CR5_MUTMIC2_DISABLED;
+			p_ab8500_codec_configuration->cr7_linrsel =
+			    AB8500_CODEC_CR7_LINRSEL_MIC2;
+			p_ab8500_codec_configuration->cr7_enadclinr =
+			    AB8500_CODEC_CR7_ENADCLINR_ENABLED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr5_enmic2 =
+			    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutmic2 =
+			    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+			p_ab8500_codec_configuration->cr7_linrsel =
+			    AB8500_CODEC_CR7_LINRSEL_LINR;
+			p_ab8500_codec_configuration->cr7_enadclinr =
+			    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+			}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_1:
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr6_endmic1 =
+			    AB8500_CODEC_CR6_ENDMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr63_ad1sel =
+			    AB8500_CODEC_CR63_AD1SEL_DMIC1_SELECTED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr6_endmic1 =
+			    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr63_ad1sel =
+			    AB8500_CODEC_CR63_AD1SEL_LINLADL_SELECTED;
+			}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_2:
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr6_endmic2 =
+			    AB8500_CODEC_CR6_ENDMIC2_ENABLED;
+			p_ab8500_codec_configuration->cr63_ad2sel =
+			    AB8500_CODEC_CR63_AD2SEL_DMIC2_SELECTED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr6_endmic2 =
+			    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+			p_ab8500_codec_configuration->cr63_ad2sel =
+			    AB8500_CODEC_CR63_AD2SEL_LINRADR_SELECTED;
+			}
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SrcPowerControlSwitch2(IN
+								   t_ab8500_codec_src
+								   src_device,
+								   t_ab8500_codec_src_state
+								   state)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	switch (src_device)
+		 {
+	case AB8500_CODEC_SRC_D_MICROPHONE_3:
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr6_endmic3 =
+			    AB8500_CODEC_CR6_ENDMIC3_ENABLED;
+			p_ab8500_codec_configuration->cr63_ad3sel =
+			    AB8500_CODEC_CR63_AD3SEL_DMIC3_SELECTED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr6_endmic3 =
+			    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+			p_ab8500_codec_configuration->cr63_ad3sel =
+			    AB8500_CODEC_CR63_AD3SEL_ADMO_SELECTED;
+			}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_4:
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr6_endmic4 =
+			    AB8500_CODEC_CR6_ENDMIC4_ENABLED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr6_endmic4 =
+			    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+			}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_5:
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr6_endmic5 =
+			    AB8500_CODEC_CR6_ENDMIC5_ENABLED;
+			p_ab8500_codec_configuration->cr63_ad5sel =
+			    AB8500_CODEC_CR63_AD5SEL_DMIC5_SELECTED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr6_endmic5 =
+			    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+			p_ab8500_codec_configuration->cr63_ad5sel =
+			    AB8500_CODEC_CR63_AD5SEL_AMADR_SELECTED;
+			}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_6:
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr6_endmic6 =
+			    AB8500_CODEC_CR6_ENDMIC6_ENABLED;
+			p_ab8500_codec_configuration->cr63_ad6sel =
+			    AB8500_CODEC_CR63_AD6SEL_DMIC6_SELECTED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr6_endmic6 =
+			    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+			p_ab8500_codec_configuration->cr63_ad6sel =
+			    AB8500_CODEC_CR63_AD6SEL_ADMO_SELECTED;
+			}
+		break;
+	case AB8500_CODEC_SRC_ALL:
+		if (AB8500_CODEC_SRC_STATE_ENABLE == state)
+			 {
+			p_ab8500_codec_configuration->cr5_enlinl =
+			    AB8500_CODEC_CR5_ENLINL_ENABLED;
+			p_ab8500_codec_configuration->cr5_enlinr =
+			    AB8500_CODEC_CR5_ENLINR_ENABLED;
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr5_enmic2 =
+			    AB8500_CODEC_CR5_ENMIC2_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic1 =
+			    AB8500_CODEC_CR6_ENDMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic2 =
+			    AB8500_CODEC_CR6_ENDMIC2_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic3 =
+			    AB8500_CODEC_CR6_ENDMIC3_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic4 =
+			    AB8500_CODEC_CR6_ENDMIC4_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic5 =
+			    AB8500_CODEC_CR6_ENDMIC5_ENABLED;
+			p_ab8500_codec_configuration->cr6_endmic6 =
+			    AB8500_CODEC_CR6_ENDMIC6_ENABLED;
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutlinl =
+			    AB8500_CODEC_CR5_MUTLINL_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutlinr =
+			    AB8500_CODEC_CR5_MUTLINR_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutmic2 =
+			    AB8500_CODEC_CR5_MUTMIC2_DISABLED;
+			}
+
+		else
+			 {
+			p_ab8500_codec_configuration->cr5_enlinl =
+			    AB8500_CODEC_CR5_ENLINL_DISABLED;
+			p_ab8500_codec_configuration->cr5_enlinr =
+			    AB8500_CODEC_CR5_ENLINR_DISABLED;
+			p_ab8500_codec_configuration->cr5_enmic1 =
+			    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr5_enmic2 =
+			    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic1 =
+			    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic2 =
+			    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic3 =
+			    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic4 =
+			    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic5 =
+			    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+			p_ab8500_codec_configuration->cr6_endmic6 =
+			    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+			p_ab8500_codec_configuration->cr7_enadcmic =
+			    AB8500_CODEC_CR7_ENADCMIC_DISABLED;
+			p_ab8500_codec_configuration->cr5_mutlinl =
+			    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutlinr =
+			    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutmic1 =
+			    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+			p_ab8500_codec_configuration->cr5_mutmic2 =
+			    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+			}
+		break;
+	case AB8500_CODEC_SRC_FM_RX:
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SetModeAndDirectionUpdateCR(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	t_ab8500_codec_cr0_powerup ab8500_codec_cr0_powerup;
+	ab8500_codec_cr0_powerup = p_ab8500_codec_configuration->cr0_powerup;
+	p_ab8500_codec_configuration->cr0_powerup =
+	    AB8500_CODEC_CR0_POWERUP_OFF;
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+	if (AB8500_CODEC_OK != ab8500_codec_error)
+		 {
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR2();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR3();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR26();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR27();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR28();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR30();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR63();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	p_ab8500_codec_configuration->cr0_powerup = ab8500_codec_cr0_powerup;
+	ab8500_codec_error = ab8500_codec_UpdateCR0();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SetSrcVolumeUpdateCR(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	ab8500_codec_error = ab8500_codec_UpdateCR20();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR21();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR23();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR65();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR66();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR67();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR68();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR69();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR70();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_SetDestVolumeUpdateCR(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	ab8500_codec_error = ab8500_codec_UpdateCR16();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR17();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR18();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR19();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR22();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR71();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR72();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR73();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR74();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR75();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR76();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR79();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR80();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ProgramDirectionIN(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	switch (g_ab8500_codec_system_context.ab8500_codec_src)
+		 {
+	case AB8500_CODEC_SRC_LINEIN:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr7_linrsel =
+		    AB8500_CODEC_CR7_LINRSEL_LINR;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+		break;
+	case AB8500_CODEC_SRC_MICROPHONE_1A:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr7_mic1sel =
+		    AB8500_CODEC_CR7_MIC1SEL_MIC1A;
+		p_ab8500_codec_configuration->cr7_enadcmic =
+		    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+		break;
+	case AB8500_CODEC_SRC_MICROPHONE_1B:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr7_mic1sel =
+		    AB8500_CODEC_CR7_MIC1SEL_MIC1B;
+		p_ab8500_codec_configuration->cr7_enadcmic =
+		    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+		break;
+	case AB8500_CODEC_SRC_MICROPHONE_2:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadcmic =
+		    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+		p_ab8500_codec_configuration->cr7_linrsel =
+		    AB8500_CODEC_CR7_LINRSEL_MIC2;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_DISABLED;
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_1:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+		p_ab8500_codec_configuration->cr63_ad1sel =
+		    AB8500_CODEC_CR63_AD1SEL_DMIC1_SELECTED;
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_2:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+		p_ab8500_codec_configuration->cr63_ad2sel =
+		    AB8500_CODEC_CR63_AD2SEL_DMIC2_SELECTED;
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_3:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+		p_ab8500_codec_configuration->cr63_ad3sel =
+		    AB8500_CODEC_CR63_AD3SEL_DMIC3_SELECTED;
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_4:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_5:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+		p_ab8500_codec_configuration->cr63_ad5sel =
+		    AB8500_CODEC_CR63_AD5SEL_DMIC5_SELECTED;
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_6:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_DISABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_ENABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_DISABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_DISABLED;
+		p_ab8500_codec_configuration->cr63_ad6sel =
+		    AB8500_CODEC_CR63_AD6SEL_DMIC6_SELECTED;
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_ENABLED;
+		break;
+	case AB8500_CODEC_SRC_ALL:
+		p_ab8500_codec_configuration->cr5_enlinl =
+		    AB8500_CODEC_CR5_ENLINL_ENABLED;
+		p_ab8500_codec_configuration->cr5_enlinr =
+		    AB8500_CODEC_CR5_ENLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_enmic1 =
+		    AB8500_CODEC_CR5_ENMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr5_enmic2 =
+		    AB8500_CODEC_CR5_ENMIC2_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic1 =
+		    AB8500_CODEC_CR6_ENDMIC1_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic2 =
+		    AB8500_CODEC_CR6_ENDMIC2_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic3 =
+		    AB8500_CODEC_CR6_ENDMIC3_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic4 =
+		    AB8500_CODEC_CR6_ENDMIC4_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic5 =
+		    AB8500_CODEC_CR6_ENDMIC5_ENABLED;
+		p_ab8500_codec_configuration->cr6_endmic6 =
+		    AB8500_CODEC_CR6_ENDMIC6_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadcmic =
+		    AB8500_CODEC_CR7_ENADCMIC_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadclinl =
+		    AB8500_CODEC_CR7_ENADCLINL_ENABLED;
+		p_ab8500_codec_configuration->cr7_enadclinr =
+		    AB8500_CODEC_CR7_ENADCLINR_ENABLED;
+		p_ab8500_codec_configuration->cr5_mutlinl =
+		    AB8500_CODEC_CR5_MUTLINL_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutlinr =
+		    AB8500_CODEC_CR5_MUTLINR_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutmic1 =
+		    AB8500_CODEC_CR5_MUTMIC1_DISABLED;
+		p_ab8500_codec_configuration->cr5_mutmic2 =
+		    AB8500_CODEC_CR5_MUTMIC2_DISABLED;
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_12:
+	case AB8500_CODEC_SRC_D_MICROPHONE_34:
+	case AB8500_CODEC_SRC_D_MICROPHONE_56:
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_ProgramDirectionOUT(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	t_ab8500_codec_configuration * p_ab8500_codec_configuration =
+	    &g_ab8500_codec_system_context.  ab8500_codec_configuration;
+	switch (g_ab8500_codec_system_context.ab8500_codec_dest)
+		 {
+	case AB8500_CODEC_DEST_HEADSET:
+		p_ab8500_codec_configuration->cr7_endrvhsl =
+		    AB8500_CODEC_CR7_ENDRVHSL_ENABLED;
+		p_ab8500_codec_configuration->cr7_endrvhsr =
+		    AB8500_CODEC_CR7_ENDRVHSR_ENABLED;
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+		p_ab8500_codec_configuration->cr9_endachsl =
+		    AB8500_CODEC_CR9_ENDACHSL_ENABLED;
+		p_ab8500_codec_configuration->cr9_endachsr =
+		    AB8500_CODEC_CR9_ENDACHSR_ENABLED;
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_DISABLED;
+		p_ab8500_codec_configuration->cr12_encphs =
+		    AB8500_CODEC_CR12_ENCPHS_ENABLED;
+		break;
+	case AB8500_CODEC_DEST_EARPIECE:
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+		p_ab8500_codec_configuration->cr9_endacear =
+		    AB8500_CODEC_CR9_ENDACEAR_ENABLED;
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+		break;
+	case AB8500_CODEC_DEST_HANDSFREE:
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_ENABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+		p_ab8500_codec_configuration->cr9_endachfl =
+		    AB8500_CODEC_CR9_ENDACHFL_ENABLED;
+		p_ab8500_codec_configuration->cr9_endachfr =
+		    AB8500_CODEC_CR9_ENDACHFR_ENABLED;
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+		break;
+	case AB8500_CODEC_DEST_VIBRATOR_L:
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_ENABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_DISABLED;
+		p_ab8500_codec_configuration->cr9_endacvibl =
+		    AB8500_CODEC_CR9_ENDACVIBL_ENABLED;
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+		p_ab8500_codec_configuration->cr15_pwmtovibl =
+		    AB8500_CODEC_CR15_PWMTOVIBL_PWM;
+		p_ab8500_codec_configuration->cr15_pwmlctrl =
+		    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmnlctrl =
+		    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmplctrl =
+		    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLDUTYCYCLE;
+		break;
+	case AB8500_CODEC_DEST_VIBRATOR_R:
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_DISABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_DISABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_ENABLED;
+		p_ab8500_codec_configuration->cr9_endacvibr =
+		    AB8500_CODEC_CR9_ENDACVIBR_ENABLED;
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_ENABLED;
+		p_ab8500_codec_configuration->cr15_pwmtovibr =
+		    AB8500_CODEC_CR15_PWMTOVIBR_PWM;
+		p_ab8500_codec_configuration->cr15_pwmrctrl =
+		    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmnrctrl =
+		    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmprctrl =
+		    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRDUTYCYCLE;
+		break;
+	case AB8500_CODEC_DEST_ALL:
+		p_ab8500_codec_configuration->cr8_enhsl =
+		    AB8500_CODEC_CR8_ENHSL_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhsr =
+		    AB8500_CODEC_CR8_ENHSR_ENABLED;
+		p_ab8500_codec_configuration->cr8_enear =
+		    AB8500_CODEC_CR8_ENEAR_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhfl =
+		    AB8500_CODEC_CR8_ENHFL_ENABLED;
+		p_ab8500_codec_configuration->cr8_enhfr =
+		    AB8500_CODEC_CR8_ENHFR_ENABLED;
+		p_ab8500_codec_configuration->cr8_envibl =
+		    AB8500_CODEC_CR8_ENVIBL_ENABLED;
+		p_ab8500_codec_configuration->cr8_envibr =
+		    AB8500_CODEC_CR8_ENVIBR_ENABLED;
+		p_ab8500_codec_configuration->cr9_endacear =
+		    AB8500_CODEC_CR9_ENDACEAR_ENABLED;
+		p_ab8500_codec_configuration->cr9_endachfl =
+		    AB8500_CODEC_CR9_ENDACHFL_ENABLED;
+		p_ab8500_codec_configuration->cr9_endachfr =
+		    AB8500_CODEC_CR9_ENDACHFR_ENABLED;
+		p_ab8500_codec_configuration->cr9_endacvibl =
+		    AB8500_CODEC_CR9_ENDACVIBL_ENABLED;
+		p_ab8500_codec_configuration->cr9_endacvibr =
+		    AB8500_CODEC_CR9_ENDACVIBR_ENABLED;
+		p_ab8500_codec_configuration->cr10_mutehsl =
+		    AB8500_CODEC_CR10_MUTEHSL_DISABLED;
+		p_ab8500_codec_configuration->cr10_mutehsr =
+		    AB8500_CODEC_CR10_MUTEHSR_DISABLED;
+		p_ab8500_codec_configuration->cr10_muteear =
+		    AB8500_CODEC_CR10_MUTEEAR_DISABLED;
+		p_ab8500_codec_configuration->cr15_pwmtovibl =
+		    AB8500_CODEC_CR15_PWMTOVIBL_PWM;
+		p_ab8500_codec_configuration->cr15_pwmlctrl =
+		    AB8500_CODEC_CR15_PWMLCTRL_PWMNPLDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmnlctrl =
+		    AB8500_CODEC_CR15_PWMNLCTRL_PWMNLDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmplctrl =
+		    AB8500_CODEC_CR15_PWMPLCTRL_PWMPLDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmtovibr =
+		    AB8500_CODEC_CR15_PWMTOVIBR_PWM;
+		p_ab8500_codec_configuration->cr15_pwmrctrl =
+		    AB8500_CODEC_CR15_PWMRCTRL_PWMNPRDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmnrctrl =
+		    AB8500_CODEC_CR15_PWMNRCTRL_PWMNRDUTYCYCLE;
+		p_ab8500_codec_configuration->cr15_pwmprctrl =
+		    AB8500_CODEC_CR15_PWMPRCTRL_PWMPRDUTYCYCLE;
+		break;
+	default:
+		ab8500_codec_error = AB8500_CODEC_INVALID_PARAMETER;
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
+
+PRIVATE t_ab8500_codec_error ab8500_codec_DestPowerControlUpdateCR(void)
+{
+	t_ab8500_codec_error ab8500_codec_error = AB8500_CODEC_OK;
+	ab8500_codec_error = ab8500_codec_UpdateCR8();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR9();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR10();
+	if (ab8500_codec_error != AB8500_CODEC_OK)
+		 {
+		DBGEXIT(ab8500_codec_error);
+		return (ab8500_codec_error);
+		}
+	ab8500_codec_error = ab8500_codec_UpdateCR15();
+	DBGEXIT(ab8500_codec_error);
+	return (ab8500_codec_error);
+}
diff --git a/sound/arm/Kconfig b/sound/arm/Kconfig
index 885683a3b0b..86cd48bfd8d 100644
--- a/sound/arm/Kconfig
+++ b/sound/arm/Kconfig
@@ -39,5 +39,17 @@ config SND_PXA2XX_AC97
 	  Say Y or M if you want to support any AC97 codec attached to
 	  the PXA2xx AC97 interface.
 
+config SND_U8500_ALSA_AB8500
+	tristate "U8500 alsa support for AB8500"
+	depends on SND && STE_DMA40 && U8500_ACODEC && (U8500_AB8500_ED || U8500_AB8500_CUT10)
+	default y
+	select SND_PCM
+	help
+	  Say Y here if you have a u8500 based device
+	  and want to use alsa for pcm playback and capture.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called u8500mod_alsa.
+
 endif	# SND_ARM
 
diff --git a/sound/arm/Makefile b/sound/arm/Makefile
index 8c0c851d464..e41f1f4db14 100644
--- a/sound/arm/Makefile
+++ b/sound/arm/Makefile
@@ -14,3 +14,7 @@ snd-pxa2xx-lib-$(CONFIG_SND_PXA2XX_LIB_AC97)	+= pxa2xx-ac97-lib.o
 
 obj-$(CONFIG_SND_PXA2XX_AC97)	+= snd-pxa2xx-ac97.o
 snd-pxa2xx-ac97-objs		:= pxa2xx-ac97.o
+obj-$(CONFIG_SND_U8500_ALSA_AB8500)  += u8500mod_alsa.o
+ifneq ($(CONFIG_SND_U8500_ALSA_AB8500),n)
+u8500mod_alsa-objs		:= u8500_alsa_ab8500.o u8500_alsa_hdmi.o
+endif
diff --git a/sound/arm/u8500_alsa_ab8500.c b/sound/arm/u8500_alsa_ab8500.c
new file mode 100644
index 00000000000..39752388ab1
--- /dev/null
+++ b/sound/arm/u8500_alsa_ab8500.c
@@ -0,0 +1,2691 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Deepak Karda
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2.1 as published
+ * by the Free Software Foundation.
+ */
+
+/* This include must be defined at this point */
+//#include <sound/driver.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/errno.h>
+#include <linux/ioctl.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/mm.h>
+#include <mach/hardware.h>
+#include <asm/uaccess.h>
+#include <asm/io.h>
+
+/*  alsa system  */
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <sound/initval.h>
+#include <sound/control.h>
+#include "u8500_alsa_ab8500.h"
+#include <mach/msp.h>
+#include <mach/debug.h>
+
+#define ALSA_NAME		"DRIVER ALSA"
+
+#define DRIVER_DEBUG	    CONFIG_STM_ALSA_DEBUG	/* enables/disables debug msgs */
+#define DRIVER_DEBUG_PFX	ALSA_NAME	/* msg header represents this module */
+#define DRIVER_DBG	        KERN_ERR	/* message level */
+
+static struct platform_device *device;
+static int active_user = 0;
+
+/*
+** Externel references
+*/
+#if DRIVER_DEBUG > 0
+t_ab8500_codec_error dump_acodec_registers(void);
+t_ab8500_codec_error dump_msp_registers(void);
+#endif
+
+extern int u8500_acodec_rates[MAX_NO_OF_RATES];
+extern char *lpbk_state_in_texts[NUMBER_LOOPBACK_STATE];
+extern char *switch_state_in_texts[NUMBER_SWITCH_STATE];
+extern char *power_state_in_texts[NUMBER_POWER_STATE];
+extern char *tdm_mode_state_in_texts[NUMBER_TDM_MODE_STATE];
+extern char *direct_rendering_state_in_texts[NUMBER_DIRECT_RENDERING_STATE];
+extern char *pcm_rendering_state_in_texts[NUMBER_PCM_RENDERING_STATE];
+extern char *codec_dest_texts[NUMBER_OUTPUT_DEVICE];
+extern char *codec_in_texts[NUMBER_INPUT_DEVICE];
+extern struct driver_debug_st DBG_ST;
+extern int second_config;
+extern int u8500_register_alsa_hdmi_controls(struct snd_card *card,
+					     u8500_acodec_chip_t * u8500_chip);
+extern int snd_card_u8500_alsa_hdmi_new(u8500_acodec_chip_t * chip, int device);
+/*
+** Declaration for local functions
+*/
+static int u8500_analog_lpbk_info(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_info *uinfo);
+static int u8500_analog_lpbk_get(struct snd_kcontrol *kcontrol,
+				 struct snd_ctl_elem_value *uinfo);
+static int u8500_analog_lpbk_put(struct snd_kcontrol *kcontrol,
+				 struct snd_ctl_elem_value *uinfo);
+
+static int u8500_digital_lpbk_info(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_info *uinfo);
+static int u8500_digital_lpbk_get(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *uinfo);
+static int u8500_digital_lpbk_put(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *uinfo);
+
+static int u8500_playback_vol_info(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_info *uinfo);
+static int u8500_playback_vol_get(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *uinfo);
+static int u8500_playback_vol_put(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *uinfo);
+
+static int u8500_capture_vol_info(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_info *uinfo);
+static int u8500_capture_vol_get(struct snd_kcontrol *kcontrol,
+				 struct snd_ctl_elem_value *uinfo);
+static int u8500_capture_vol_put(struct snd_kcontrol *kcontrol,
+				 struct snd_ctl_elem_value *uinfo);
+
+static int u8500_playback_sink_info(struct snd_kcontrol *kcontrol,
+				    struct snd_ctl_elem_info *uinfo);
+static int u8500_playback_sink_get(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_value *uinfo);
+static int u8500_playback_sink_put(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_value *uinfo);
+
+static int u8500_capture_src_ctrl_info(struct snd_kcontrol *kcontrol,
+				       struct snd_ctl_elem_info *uinfo);
+static int u8500_capture_src_ctrl_get(struct snd_kcontrol *kcontrol,
+				      struct snd_ctl_elem_value *uinfo);
+static int u8500_capture_src_ctrl_put(struct snd_kcontrol *kcontrol,
+				      struct snd_ctl_elem_value *uinfo);
+
+static int u8500_playback_switch_ctrl_info(struct snd_kcontrol *kcontrol,
+					   struct snd_ctl_elem_info *uinfo);
+static int u8500_playback_switch_ctrl_get(struct snd_kcontrol *kcontrol,
+					  struct snd_ctl_elem_value *uinfo);
+static int u8500_playback_switch_ctrl_put(struct snd_kcontrol *kcontrol,
+					  struct snd_ctl_elem_value *uinfo);
+
+static int u8500_capture_switch_ctrl_info(struct snd_kcontrol *kcontrol,
+					  struct snd_ctl_elem_info *uinfo);
+static int u8500_capture_switch_ctrl_get(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_value *uinfo);
+static int u8500_capture_switch_ctrl_put(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_value *uinfo);
+
+static int u8500_playback_power_ctrl_info(struct snd_kcontrol *kcontrol,
+					  struct snd_ctl_elem_info *uinfo);
+static int u8500_playback_power_ctrl_get(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_value *uinfo);
+static int u8500_playback_power_ctrl_put(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_value *uinfo);
+
+static int u8500_capture_power_ctrl_info(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_info *uinfo);
+static int u8500_capture_power_ctrl_get(struct snd_kcontrol *kcontrol,
+					struct snd_ctl_elem_value *uinfo);
+static int u8500_capture_power_ctrl_put(struct snd_kcontrol *kcontrol,
+					struct snd_ctl_elem_value *uinfo);
+
+static int u8500_tdm_mode_ctrl_info(struct snd_kcontrol *kcontrol,
+				    struct snd_ctl_elem_info *uinfo);
+static int u8500_tdm_mode_ctrl_get(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_value *uinfo);
+static int u8500_tdm_mode_ctrl_put(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_value *uinfo);
+
+static int u8500_direct_rendering_mode_ctrl_info(struct snd_kcontrol *kcontrol, struct snd_ctl_elem_info
+						 *uinfo);
+static int u8500_direct_rendering_mode_ctrl_get(struct snd_kcontrol *kcontrol, struct snd_ctl_elem_value
+						*uinfo);
+static int u8500_direct_rendering_mode_ctrl_put(struct snd_kcontrol *kcontrol, struct snd_ctl_elem_value
+						*uinfo);
+static int u8500_register_alsa_controls(struct snd_card *card,
+					u8500_acodec_chip_t * u8500_chip);
+
+static int u8500_pcm_rendering_mode_ctrl_info(struct snd_kcontrol *kcontrol,
+					      struct snd_ctl_elem_info *uinfo);
+static int u8500_pcm_rendering_mode_ctrl_get(struct snd_kcontrol *kcontrol,
+					     struct snd_ctl_elem_value *uinfo);
+static int u8500_pcm_rendering_mode_ctrl_put(struct snd_kcontrol *kcontrol,
+					     struct snd_ctl_elem_value *uinfo);
+
+#if 0				/* DUMP REGISTER CONTROL */
+static int u8500_dump_register_ctrl_info(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_info *uinfo);
+static int u8500_dump_register_ctrl_get(struct snd_kcontrol *kcontrol,
+					struct snd_ctl_elem_value *uinfo);
+static int u8500_dump_register_ctrl_put(struct snd_kcontrol *kcontrol,
+					struct snd_ctl_elem_value *uinfo);
+#endif /* DUMP REGISTER CONTROL */
+
+static int configure_rate(struct snd_pcm_substream *,
+			  t_u8500_acodec_config_need acodec_config_need);
+static void dma_eot_handler(void *data);
+/**
+* configure_rate
+* @substream - pointer to the playback/capture substream structure
+*
+*	This functions configures audio codec in to stream frequency frequency
+*/
+
+static int configure_rate(struct snd_pcm_substream *substream,
+			  t_u8500_acodec_config_need acodec_config_need)
+{
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	t_codec_sample_frequency sampling_frequency = 0;
+	t_ab8500_codec_direction direction = 0;
+	struct acodec_configuration acodec_config;
+	int stream_id = substream->pstr->stream;
+
+	FUNC_ENTER();
+	switch (chip->freq) {
+	case 48000:
+		sampling_frequency = CODEC_SAMPLING_FREQ_48KHZ;
+		break;
+	default:
+		printk("not supported frequnecy \n");
+		stm_error("not supported frequnecy \n");
+		return -EINVAL;
+	}
+
+	switch (stream_id) {
+	case SNDRV_PCM_STREAM_PLAYBACK:
+		direction = AB8500_CODEC_DIRECTION_OUT;
+		break;
+	case SNDRV_PCM_STREAM_CAPTURE:
+		direction = AB8500_CODEC_DIRECTION_IN;
+		break;
+	default:
+		stm_error(": wrong pcm stream\n");
+		return -EINVAL;
+	}
+
+	stm_dbg(DBG_ST.alsa, "enabling audiocodec audio mode\n");
+	acodec_config.direction = direction;
+	acodec_config.input_frequency = T_CODEC_SAMPLING_FREQ_48KHZ;
+	acodec_config.output_frequency = T_CODEC_SAMPLING_FREQ_48KHZ;
+	acodec_config.mspClockSel = CODEC_MSP_APB_CLOCK;
+	acodec_config.mspInClockFreq = CODEC_MSP_INPUT_FREQ_48MHZ;
+	acodec_config.channels = chip->channels;
+	acodec_config.user = 2;
+	acodec_config.acodec_config_need = acodec_config_need;
+	acodec_config.handler = dma_eot_handler;
+	acodec_config.tx_callback_data =
+	    &chip->stream[ALSA_PCM_DEV][SNDRV_PCM_STREAM_PLAYBACK];
+	acodec_config.rx_callback_data =
+	    &chip->stream[ALSA_PCM_DEV][SNDRV_PCM_STREAM_CAPTURE];
+	acodec_config.direct_rendering_mode = chip->direct_rendering_mode;
+	acodec_config.tdm8_ch_mode = chip->tdm8_ch_mode;
+	acodec_config.digital_loopback = DISABLE;
+	u8500_acodec_enable_audio_mode(&acodec_config);
+	FUNC_EXIT();
+
+	return 0;
+}
+
+/*
+****************************************************************************************
+*					           playback vol control									   *
+****************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_playback_vol_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.subdevice = 0,
+	.name = "PCM Playback Volume",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_playback_vol_info,
+	.get = u8500_playback_vol_get,
+	.put = u8500_playback_vol_put
+};
+
+/**
+* u8500_playback_vol_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback volume info into user structure.
+*/
+
+static int u8500_playback_vol_info(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_info *uinfo)
+{
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
+	uinfo->count = 2;
+	uinfo->value.integer.min = 0;
+	uinfo->value.integer.max = 100;
+	uinfo->value.integer.step = 10;
+	return 0;
+}
+
+/**
+* u8500_playback_vol_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills the current volume setting to user structure.
+*/
+
+static int u8500_playback_vol_get(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+
+	int *p_left_volume = NULL;
+	int *p_right_volume = NULL;
+
+	p_left_volume = (int *)&uinfo->value.integer.value[0];
+	p_right_volume = (int *)&uinfo->value.integer.value[1];
+
+	u8500_acodec_get_output_volume(chip->output_device, p_left_volume,
+				       p_right_volume, USER_ALSA);
+	return 0;
+}
+
+/**
+* u8500_playback_vol_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure.
+*
+*	This functions sets the playback audio codec volume .
+*/
+
+static int u8500_playback_vol_put(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0, error = 0;
+
+	if (chip->output_lvolume != uinfo->value.integer.value[0]
+	    || chip->output_rvolume != uinfo->value.integer.value[1]) {
+		chip->output_lvolume = uinfo->value.integer.value[0];
+		chip->output_rvolume = uinfo->value.integer.value[1];
+
+		if (chip->output_lvolume > 100)
+			chip->output_lvolume = 100;
+		else if (chip->output_lvolume < 0)
+			chip->output_lvolume = 0;
+
+		if (chip->output_rvolume > 100)
+			chip->output_rvolume = 100;
+		else if (chip->output_rvolume < 0)
+			chip->output_rvolume = 0;
+
+		error =
+		    u8500_acodec_set_output_volume(chip->output_device,
+						   chip->output_lvolume,
+						   chip->output_rvolume,
+						   USER_ALSA);
+
+		if (error) {
+			stm_error
+			    (" : set volume for speaker/headphone failed\n");
+			return changed;
+		}
+		changed = 1;
+	}
+
+	return changed;
+}
+
+/*
+****************************************************************************************
+*					           capture vol control									   *
+****************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_capture_vol_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.subdevice = 1,
+	.name = "PCM Capture Volume",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_capture_vol_info,
+	.get = u8500_capture_vol_get,
+	.put = u8500_capture_vol_put
+};
+
+/**
+* u8500_capture_vol_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills capture volume info into user structure.
+*/
+static int u8500_capture_vol_info(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_info *uinfo)
+{
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
+	uinfo->count = 2;
+	uinfo->value.integer.min = 0;
+	uinfo->value.integer.max = 100;
+	uinfo->value.integer.step = 10;
+	return 0;
+}
+
+/**
+* u8500_capture_vol_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current capture volume setting to user structure.
+*/
+
+static int u8500_capture_vol_get(struct snd_kcontrol *kcontrol,
+				 struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+
+	int *p_left_volume = NULL;
+	int *p_right_volume = NULL;
+
+	p_left_volume = (int *)&uinfo->value.integer.value[0];
+	p_right_volume = (int *)&uinfo->value.integer.value[1];
+
+	u8500_acodec_get_input_volume(chip->input_device, p_left_volume,
+				      p_right_volume, USER_ALSA);
+	return 0;
+}
+
+/**
+* u8500_capture_vol_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure.
+*
+*	This functions sets the capture audio codec volume with values provided.
+*/
+
+static int u8500_capture_vol_put(struct snd_kcontrol *kcontrol,
+				 struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0, error = 0;
+
+	if (chip->input_lvolume != uinfo->value.integer.value[0]
+	    || chip->input_rvolume != uinfo->value.integer.value[1]) {
+		chip->input_lvolume = uinfo->value.integer.value[0];
+		chip->input_rvolume = uinfo->value.integer.value[1];
+
+		if (chip->input_lvolume > 100)
+			chip->input_lvolume = 100;
+		else if (chip->input_lvolume < 0)
+			chip->input_lvolume = 0;
+
+		if (chip->input_rvolume > 100)
+			chip->input_rvolume = 100;
+		else if (chip->input_rvolume < 0)
+			chip->input_rvolume = 0;
+
+		error = u8500_acodec_set_input_volume(chip->input_device,
+						      chip->input_rvolume,
+						      chip->input_lvolume,
+						      USER_ALSA);
+		if (error) {
+			stm_error(" : set input volume failed\n");
+			return changed;
+		}
+		changed = 1;
+	}
+
+	return changed;
+}
+
+/*
+****************************************************************************************
+*					           playback sink control								   *
+****************************************************************************************
+*/
+
+static struct snd_kcontrol_new u8500_playback_sink_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.subdevice = 0,
+	.name = "PCM Playback Sink",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xffff,
+	.info = u8500_playback_sink_info,
+	.get = u8500_playback_sink_get,
+	.put = u8500_playback_sink_put
+};
+
+/**
+* u8500_playback_sink_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_playback_sink_info(struct snd_kcontrol *kcontrol,
+				    struct snd_ctl_elem_info *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_OUTPUT_DEVICE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_OUTPUT_DEVICE)
+		uinfo->value.enumerated.item = NUMBER_OUTPUT_DEVICE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       codec_dest_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_playback_sink_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_playback_sink_get(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] = chip->output_device;
+	return 0;
+}
+
+/**
+* u8500_playback_sink_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_playback_sink_put(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0, error;
+
+	if (chip->output_device != uinfo->value.enumerated.item[0]) {
+		chip->output_device = uinfo->value.enumerated.item[0];
+		error =
+		    u8500_acodec_select_output(chip->output_device,
+					       USER_ALSA, chip->tdm8_ch_mode);
+		if (error) {
+			stm_error(" : select output failed\n");
+			return changed;
+		}
+		changed = 1;
+	}
+	return changed;
+}
+
+/*
+****************************************************************************************
+*					           capture src control									   *
+****************************************************************************************
+*/
+
+static struct snd_kcontrol_new u8500_capture_src_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.subdevice = 1,
+	.name = "PCM Capture Source",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xffff,
+	.info = u8500_capture_src_ctrl_info,
+	.get = u8500_capture_src_ctrl_get,
+	.put = u8500_capture_src_ctrl_put
+};
+
+/**
+* u8500_capture_src_ctrl_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills capture device info into user structure.
+*/
+static int u8500_capture_src_ctrl_info(struct snd_kcontrol *kcontrol,
+				       struct snd_ctl_elem_info *uinfo)
+{
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_INPUT_DEVICE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_INPUT_DEVICE)
+		uinfo->value.enumerated.item = NUMBER_INPUT_DEVICE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       codec_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_capture_src_ctrl_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current capture device selected.
+*/
+static int u8500_capture_src_ctrl_get(struct snd_kcontrol *kcontrol,
+				      struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] = chip->input_device;
+	return 0;
+}
+
+/**
+* u8500_capture_src_ctrl_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure,
+*
+*	This functions sets the capture device.
+*/
+static int u8500_capture_src_ctrl_put(struct snd_kcontrol *kcontrol,
+				      struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0, error;
+
+	if (chip->input_device != uinfo->value.enumerated.item[0]) {
+		chip->input_device = uinfo->value.enumerated.item[0];
+		error =
+		    u8500_acodec_select_input(chip->input_device, USER_ALSA,
+					      chip->tdm8_ch_mode);
+		if (error) {
+			stm_error(" : select input failed\n");
+			return changed;
+		}
+		changed = 1;
+	}
+	return changed;
+}
+
+/*
+***************************************************************************************
+*						           analog lpbk control								  *
+***************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_analog_lpbk_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.name = "Analog Loopback",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_analog_lpbk_info,
+	.get = u8500_analog_lpbk_get,
+	.put = u8500_analog_lpbk_put
+};
+
+/**
+* u8500_analog_lpbk_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_analog_lpbk_info(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_info *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_LOOPBACK_STATE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_LOOPBACK_STATE)
+		uinfo->value.enumerated.item = NUMBER_LOOPBACK_STATE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       lpbk_state_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_analog_lpbk_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_analog_lpbk_get(struct snd_kcontrol *kcontrol,
+				 struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] = chip->analog_lpbk;
+	return 0;
+}
+
+/**
+* u8500_analog_lpbk_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_analog_lpbk_put(struct snd_kcontrol *kcontrol,
+				 struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0;
+	t_ab8500_codec_error error;
+
+	if (chip->analog_lpbk != uinfo->value.enumerated.item[0]) {
+		chip->analog_lpbk = uinfo->value.enumerated.item[0];
+
+		error =
+		    u8500_acodec_toggle_analog_lpbk(chip->analog_lpbk,
+						    USER_ALSA);
+
+		if (AB8500_CODEC_OK != error) {
+			stm_error
+			    (" : select u8500_acodec_set_analog_lpbk_state failed\n");
+			return changed;
+		}
+		changed = 1;
+	}
+	return changed;
+}
+
+/*
+****************************************************************************************
+*					           digital lpbk control									   *
+****************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_digital_lpbk_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.name = "Digital Loopback",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_digital_lpbk_info,
+	.get = u8500_digital_lpbk_get,
+	.put = u8500_digital_lpbk_put
+};
+
+/**
+* u8500_digital_lpbk_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_digital_lpbk_info(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_info *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_LOOPBACK_STATE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_LOOPBACK_STATE)
+		uinfo->value.enumerated.item = NUMBER_LOOPBACK_STATE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       lpbk_state_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_digital_lpbk_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_digital_lpbk_get(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] = chip->digital_lpbk;
+	return 0;
+}
+
+/**
+* u8500_analog_lpbk_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_digital_lpbk_put(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0;
+	t_ab8500_codec_error error;
+
+	if (chip->digital_lpbk != uinfo->value.enumerated.item[0]) {
+		chip->digital_lpbk = uinfo->value.enumerated.item[0];
+
+		error = u8500_acodec_toggle_digital_lpbk(chip->digital_lpbk,
+							 chip->output_device,
+							 chip->input_device,
+							 USER_ALSA,
+							 chip->tdm8_ch_mode);
+
+		/*if((error = u8500_acodec_set_output_volume(chip->output_device,50,50,USER_ALSA)))
+		   {
+		   stm_error(" : set output volume failed\n");
+		   return error;
+		   }
+
+		   if ((error = u8500_acodec_set_input_volume(chip->input_device,50,50,USER_ALSA)))
+		   {
+		   stm_error(" : set input volume failed\n");
+		   return error;
+		   } */
+
+		if (AB8500_CODEC_OK != error) {
+			stm_error
+			    (" : select u8500_acodec_set_digital_lpbk_state failed\n");
+			return changed;
+		}
+		changed = 1;
+	}
+	return changed;
+}
+
+/*
+****************************************************************************************
+*					           playback switch control								   *
+****************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_playback_switch_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.subdevice = 0,
+	.name = "PCM Playback Mute",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_playback_switch_ctrl_info,
+	.get = u8500_playback_switch_ctrl_get,
+	.put = u8500_playback_switch_ctrl_put
+};
+
+/**
+* u8500_playback_switch_ctrl_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_playback_switch_ctrl_info(struct snd_kcontrol *kcontrol,
+					   struct snd_ctl_elem_info *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_SWITCH_STATE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_SWITCH_STATE)
+		uinfo->value.enumerated.item = NUMBER_SWITCH_STATE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       switch_state_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_playback_switch_ctrl_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_playback_switch_ctrl_get(struct snd_kcontrol *kcontrol,
+					  struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] = chip->playback_switch;
+	return 0;
+}
+
+/**
+* u8500_playback_switch_ctrl_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_playback_switch_ctrl_put(struct snd_kcontrol *kcontrol,
+					  struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0;
+	t_ab8500_codec_error error;
+
+	if (chip->playback_switch != uinfo->value.enumerated.item[0]) {
+		chip->playback_switch = uinfo->value.enumerated.item[0];
+
+		error =
+		    u8500_acodec_toggle_playback_mute_control(chip->
+							      output_device,
+							      chip->
+							      playback_switch,
+							      USER_ALSA);
+
+		if (AB8500_CODEC_OK != error) {
+			stm_error
+			    (" : select u8500_playback_switch_ctrl_put failed\n");
+			return changed;
+		}
+		changed = 1;
+	}
+	return changed;
+}
+
+/*
+****************************************************************************************
+*					           Capture switch control								   *
+****************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_capture_switch_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.subdevice = 1,
+	.name = "PCM Capture Mute",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_capture_switch_ctrl_info,
+	.get = u8500_capture_switch_ctrl_get,
+	.put = u8500_capture_switch_ctrl_put
+};
+
+/**
+* u8500_capture_switch_ctrl_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_capture_switch_ctrl_info(struct snd_kcontrol *kcontrol,
+					  struct snd_ctl_elem_info *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_SWITCH_STATE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_SWITCH_STATE)
+		uinfo->value.enumerated.item = NUMBER_SWITCH_STATE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       switch_state_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_capture_switch_ctrl_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_capture_switch_ctrl_get(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] = chip->capture_switch;
+	return 0;
+}
+
+/**
+* u8500_capture_switch_ctrl_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_capture_switch_ctrl_put(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0;
+	t_ab8500_codec_error error;
+
+	if (chip->capture_switch != uinfo->value.enumerated.item[0]) {
+		chip->capture_switch = uinfo->value.enumerated.item[0];
+
+		error =
+		    u8500_acodec_toggle_capture_mute_control(chip->input_device,
+							     chip->
+							     capture_switch,
+							     USER_ALSA);
+
+		if (AB8500_CODEC_OK != error) {
+			stm_error
+			    (" : select u8500_capture_switch_ctrl_put failed\n");
+			return changed;
+		}
+		changed = 1;
+	}
+	return changed;
+}
+
+/*
+****************************************************************************************
+*					           playback power control								   *
+****************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_playback_power_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.subdevice = 0,
+	.name = "PCM Playback Power",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_playback_power_ctrl_info,
+	.get = u8500_playback_power_ctrl_get,
+	.put = u8500_playback_power_ctrl_put
+};
+
+/**
+* u8500_playback_power_ctrl_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_playback_power_ctrl_info(struct snd_kcontrol *kcontrol,
+					  struct snd_ctl_elem_info *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_POWER_STATE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_POWER_STATE)
+		uinfo->value.enumerated.item = NUMBER_POWER_STATE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       power_state_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_playback_power_ctrl_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_playback_power_ctrl_get(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] =
+	    u8500_acodec_get_dest_power_state(chip->output_device);
+	return 0;
+}
+
+/**
+* u8500_playback_power_ctrl_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_playback_power_ctrl_put(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0;
+	t_ab8500_codec_error error;
+	t_u8500_bool_state power_state;
+
+	power_state = u8500_acodec_get_dest_power_state(chip->output_device);
+
+	if (power_state != uinfo->value.enumerated.item[0]) {
+		power_state = uinfo->value.enumerated.item[0];
+
+		error =
+		    u8500_acodec_set_dest_power_cntrl(chip->output_device,
+						      power_state);
+
+		if (AB8500_CODEC_OK != error) {
+			stm_error
+			    (" : select u8500_acodec_set_dest_power_cntrl failed\n");
+			return changed;
+		}
+
+		/* configure the volume settings for the acodec */
+		if ((error =
+		     u8500_acodec_set_output_volume(chip->output_device,
+						    chip->output_lvolume,
+						    chip->output_rvolume,
+						    USER_ALSA))) {
+			stm_error(" : set output volume failed\n");
+			return error;
+		}
+		changed = 1;
+	}
+	return changed;
+}
+
+/*
+****************************************************************************************
+*					           capture power control								   *
+****************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_capture_power_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.subdevice = 0,
+	.name = "PCM Capture Power",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_capture_power_ctrl_info,
+	.get = u8500_capture_power_ctrl_get,
+	.put = u8500_capture_power_ctrl_put
+};
+
+/**
+* u8500_capture_power_ctrl_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_capture_power_ctrl_info(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_info *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_POWER_STATE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_POWER_STATE)
+		uinfo->value.enumerated.item = NUMBER_POWER_STATE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       power_state_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_capture_power_ctrl_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_capture_power_ctrl_get(struct snd_kcontrol *kcontrol,
+					struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] =
+	    u8500_acodec_get_src_power_state(chip->input_device);
+	return 0;
+}
+
+/**
+* u8500_capture_power_ctrl_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_capture_power_ctrl_put(struct snd_kcontrol *kcontrol,
+					struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0;
+	t_ab8500_codec_error error;
+	t_u8500_bool_state power_state;
+
+	power_state = u8500_acodec_get_src_power_state(chip->input_device);
+
+	if (power_state != uinfo->value.enumerated.item[0]) {
+		power_state = uinfo->value.enumerated.item[0];
+
+		error =
+		    u8500_acodec_set_src_power_cntrl(chip->input_device,
+						     power_state);
+
+		if (AB8500_CODEC_OK != error) {
+			stm_error
+			    (" : select u8500_acodec_set_src_power_cntrl failed\n");
+			return changed;
+		}
+		changed = 1;
+	}
+	return changed;
+}
+
+/*
+****************************************************************************************
+*					           TDM 8 channel mode control	     					   *
+****************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_tdm_mode_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.subdevice = 0,
+	.name = "TDM 8 Channel Mode",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_tdm_mode_ctrl_info,
+	.get = u8500_tdm_mode_ctrl_get,
+	.put = u8500_tdm_mode_ctrl_put
+};
+
+/**
+* u8500_tdm_mode_ctrl_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_tdm_mode_ctrl_info(struct snd_kcontrol *kcontrol,
+				    struct snd_ctl_elem_info *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_TDM_MODE_STATE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_TDM_MODE_STATE)
+		uinfo->value.enumerated.item = NUMBER_TDM_MODE_STATE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       tdm_mode_state_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_tdm_mode_ctrl_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_tdm_mode_ctrl_get(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] = chip->tdm8_ch_mode;
+	return 0;
+}
+
+/**
+* u8500_tdm_mode_ctrl_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_tdm_mode_ctrl_put(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0;
+	t_ab8500_codec_error error;
+
+	chip->tdm8_ch_mode = uinfo->value.enumerated.item[0];
+
+	if (ENABLE == chip->tdm8_ch_mode)
+		printk("\n TDM 8 channel mode enabled\n");
+	else
+		printk("\n TDM 8 channel mode disabled\n");
+
+	changed = 1;
+
+	return changed;
+}
+
+/*
+****************************************************************************************
+*					           Direct Rendering Mode control	     				   *
+****************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_direct_rendering_mode_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.name = "Direct Rendering Mode",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_direct_rendering_mode_ctrl_info,
+	.get = u8500_direct_rendering_mode_ctrl_get,
+	.put = u8500_direct_rendering_mode_ctrl_put
+};
+
+/**
+* u8500_direct_rendering_mode_ctrl_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_direct_rendering_mode_ctrl_info(struct snd_kcontrol *kcontrol, struct snd_ctl_elem_info
+						 *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_DIRECT_RENDERING_STATE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_DIRECT_RENDERING_STATE)
+		uinfo->value.enumerated.item =
+		    NUMBER_DIRECT_RENDERING_STATE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       direct_rendering_state_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_direct_rendering_mode_ctrl_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_direct_rendering_mode_ctrl_get(struct snd_kcontrol *kcontrol, struct snd_ctl_elem_value
+						*uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] = chip->direct_rendering_mode;
+	return 0;
+}
+
+/**
+* u8500_direct_rendering_mode_ctrl_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_direct_rendering_mode_ctrl_put(struct snd_kcontrol *kcontrol, struct snd_ctl_elem_value
+						*uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0;
+	t_ab8500_codec_error error;
+
+	chip->direct_rendering_mode = uinfo->value.enumerated.item[0];
+	changed = 1;
+
+	return changed;
+}
+
+/*
+****************************************************************************************
+*					           PCM Rendering Mode control	     				   *
+****************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_pcm_rendering_mode_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.name = "PCM Rendering Mode",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_pcm_rendering_mode_ctrl_info,
+	.get = u8500_pcm_rendering_mode_ctrl_get,
+	.put = u8500_pcm_rendering_mode_ctrl_put
+};
+
+/**
+* u8500_pcm_rendering_mode_ctrl_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_pcm_rendering_mode_ctrl_info(struct snd_kcontrol *kcontrol,
+					      struct snd_ctl_elem_info *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_PCM_RENDERING_STATE;
+	uinfo->count = 3;
+	if (uinfo->value.enumerated.item >= NUMBER_PCM_RENDERING_STATE)
+		uinfo->value.enumerated.item = NUMBER_PCM_RENDERING_STATE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       pcm_rendering_state_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_pcm_rendering_mode_ctrl_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_pcm_rendering_mode_ctrl_get(struct snd_kcontrol *kcontrol,
+					     struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] = chip->burst_fifo_mode;
+	uinfo->value.enumerated.item[1] = chip->fm_playback_mode;
+	uinfo->value.enumerated.item[2] = chip->fm_tx_mode;
+	return 0;
+}
+
+/**
+* u8500_pcm_rendering_mode_ctrl_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_pcm_rendering_mode_ctrl_put(struct snd_kcontrol *kcontrol,
+					     struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0;
+	t_ab8500_codec_error error;
+
+	if (RENDERING_PENDING == uinfo->value.enumerated.item[0]) {
+		return changed;
+	}
+	if (chip->burst_fifo_mode != uinfo->value.enumerated.item[0]) {
+		chip->burst_fifo_mode = uinfo->value.enumerated.item[0];
+		u8500_acodec_set_burst_mode_fifo(chip->burst_fifo_mode);
+	}
+
+	chip->fm_playback_mode = uinfo->value.enumerated.item[1];
+	chip->fm_tx_mode = uinfo->value.enumerated.item[2];
+
+	changed = 1;
+
+	return changed;
+}
+
+#if 0				/* DUMP REGISTER CONTROL */
+/*
+****************************************************************************************
+*					           dump registers control	     				           *
+****************************************************************************************
+*/
+
+struct snd_kcontrol_new u8500_dump_register_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 0,
+	.name = "",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_dump_register_ctrl_info,
+	.get = u8500_dump_register_ctrl_get,
+	.put = u8500_dump_register_ctrl_put
+};
+
+/**
+* u8500_dump_register_ctrl_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_dump_register_ctrl_info(struct snd_kcontrol *kcontrol,
+					 struct snd_ctl_elem_info *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_PCM_RENDERING_STATE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_PCM_RENDERING_STATE)
+		uinfo->value.enumerated.item = NUMBER_PCM_RENDERING_STATE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       pcm_rendering_state_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_dump_register_ctrl_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_dump_register_ctrl_get(struct snd_kcontrol *kcontrol,
+					struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] = chip->burst_fifo_mode;
+	uinfo->value.enumerated.item[1] = chip->fm_playback_mode;
+	uinfo->value.enumerated.item[2] = chip->fm_tx_mode;
+	return 0;
+}
+
+/**
+* u8500_dump_register_ctrl_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_dump_register_ctrl_put(struct snd_kcontrol *kcontrol,
+					struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0;
+	t_ab8500_codec_error error;
+
+	if (RENDERING_PENDING == uinfo->value.enumerated.item[0]) {
+		return changed;
+	}
+	if (chip->burst_fifo_mode != uinfo->value.enumerated.item[0]) {
+		chip->burst_fifo_mode = uinfo->value.enumerated.item[0];
+		//u8500_acodec_set_burst_mode_fifo(chip->burst_fifo_mode);
+	}
+
+	chip->fm_playback_mode = uinfo->value.enumerated.item[1];
+	chip->fm_tx_mode = uinfo->value.enumerated.item[2];
+
+	changed = 1;
+
+	return changed;
+}
+
+#endif /* DUMP REGISTER CONTROL */
+
+/* Hardware description , this structure (struct snd_pcm_hardware )
+ * contains the definitions of the fundamental hardware configuration.
+ * This configuration will be applied on the runtime structure
+ */
+static struct snd_pcm_hardware snd_u8500_playback_hw = {
+	.info =
+	    (SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_MMAP |
+	     SNDRV_PCM_INFO_RESUME | SNDRV_PCM_INFO_PAUSE),
+	.formats =
+	    SNDRV_PCM_FMTBIT_S16_LE | SNDRV_PCM_FMTBIT_U16_LE |
+	    SNDRV_PCM_FMTBIT_S16_BE | SNDRV_PCM_FMTBIT_U16_BE |
+	    SNDRV_PCM_FMTBIT_S24_LE | SNDRV_PCM_FMTBIT_U24_LE |
+	    SNDRV_PCM_FMTBIT_S24_BE | SNDRV_PCM_FMTBIT_U24_BE |
+	    SNDRV_PCM_FMTBIT_S32_LE | SNDRV_PCM_FMTBIT_U32_LE |
+	    SNDRV_PCM_FMTBIT_S32_BE | SNDRV_PCM_FMTBIT_U32_BE,
+	.rates = SNDRV_PCM_RATE_KNOT,
+	.rate_min = MIN_RATE_PLAYBACK,
+	.rate_max = MAX_RATE_PLAYBACK,
+	.channels_min = 1,
+	.channels_max = 8,
+	.buffer_bytes_max = NMDK_BUFFER_SIZE,
+	.period_bytes_min = 128,
+	.period_bytes_max = PAGE_SIZE,
+	.periods_min = NMDK_BUFFER_SIZE / PAGE_SIZE,
+	.periods_max = NMDK_BUFFER_SIZE / 128
+};
+
+static struct snd_pcm_hardware snd_u8500_capture_hw = {
+	.info =
+	    (SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_MMAP |
+	     SNDRV_PCM_INFO_RESUME | SNDRV_PCM_INFO_PAUSE),
+	.formats =
+	    SNDRV_PCM_FMTBIT_S16_LE | SNDRV_PCM_FMTBIT_U16_LE |
+	    SNDRV_PCM_FMTBIT_S16_BE | SNDRV_PCM_FMTBIT_U16_BE |
+	    SNDRV_PCM_FMTBIT_S24_LE | SNDRV_PCM_FMTBIT_U24_LE |
+	    SNDRV_PCM_FMTBIT_S24_BE | SNDRV_PCM_FMTBIT_U24_BE |
+	    SNDRV_PCM_FMTBIT_S32_LE | SNDRV_PCM_FMTBIT_U32_LE |
+	    SNDRV_PCM_FMTBIT_S32_BE | SNDRV_PCM_FMTBIT_U32_BE,
+	.rates = SNDRV_PCM_RATE_KNOT,
+	.rate_min = MIN_RATE_CAPTURE,
+	.rate_max = MAX_RATE_CAPTURE,
+	.channels_min = 1,
+	.channels_max = 8,
+	.buffer_bytes_max = NMDK_BUFFER_SIZE,
+	.period_bytes_min = 128,
+	.period_bytes_max = PAGE_SIZE,
+	.periods_min = NMDK_BUFFER_SIZE / PAGE_SIZE,
+	.periods_max = NMDK_BUFFER_SIZE / 128
+};
+
+static struct snd_pcm_hw_constraint_list constraints_rate = {
+	.count = sizeof(u8500_acodec_rates) / sizeof(u8500_acodec_rates[0]),
+	.list = u8500_acodec_rates,
+	.mask = 0,
+};
+
+/**
+ * snd_u8500_alsa_pcm_close
+ * @substream - pointer to the playback/capture substream structure
+ *
+ *  This routine is used by alsa framework to close a pcm stream .
+ *  Here a dma pipe is disabled and freed.
+ */
+static int snd_u8500_alsa_pcm_close(struct snd_pcm_substream *substream)
+{
+	int stream_id, error = 0;
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	audio_stream_t *ptr_audio_stream = NULL;
+
+	stream_id = substream->pstr->stream;
+	ptr_audio_stream = &chip->stream[ALSA_PCM_DEV][stream_id];
+
+	if (ENABLE == chip->direct_rendering_mode) {
+		ptr_audio_stream->substream = NULL;
+		return 0;
+	} else {
+		stm_close_alsa(chip, ALSA_PCM_DEV, stream_id);
+
+		/* reset the different variables to default */
+
+		ptr_audio_stream->active = 0;
+		ptr_audio_stream->period = 0;
+		ptr_audio_stream->periods = 0;
+		ptr_audio_stream->old_offset = 0;
+		ptr_audio_stream->substream = NULL;
+		if (!(--active_user)) {
+			/* Disable the MSP1 */
+			error = u8500_acodec_unsetuser(USER_ALSA);
+			u8500_acodec_close(I2S_CLIENT_MSP1, ACODEC_DISABLE_ALL);
+		} else {
+			if (stream_id == SNDRV_PCM_STREAM_PLAYBACK)
+				u8500_acodec_close(I2S_CLIENT_MSP1,
+						   ACODEC_DISABLE_TRANSMIT);
+			else if (stream_id == SNDRV_PCM_STREAM_CAPTURE)
+				u8500_acodec_close(I2S_CLIENT_MSP1,
+						   ACODEC_DISABLE_RECEIVE);
+		}
+
+		stm_hw_free(substream);
+
+		return error;
+	}
+}
+
+void my_write(u32 address, u8 data)
+{
+	ab8500_write(AB8500_AUDIO, address, data);
+}
+
+void dsp_configure_audio_codec(void)
+{
+	//4500 config for both record DMIC1&2 and playback HS stereo
+	//data width is 16 bits
+
+	my_write(0x200, 0x02);	//    Start-up audio unreset
+	my_write(0x20B, 0x10);	//    Start-up audio clk audio enable
+	my_write(0x383, 0x06);	//    Start-up audio Vaudio supply
+
+	my_write(0xD00, 0x88);	//    General power up=0x88
+	my_write(0xD01, 0x00);	//    Software Reset=0x0
+	my_write(0xD02, 0xC0);	//    Digital AD Channels Enable=0xC0
+	my_write(0xD03, 0xC0);	//    Digital DA Channels Enable=0xC0
+	my_write(0xD04, 0x00);	//    Low Power and Conf=0x0
+	my_write(0xD05, 0x0F);	//    Line in Conf=0xF
+	my_write(0xD06, 0xC0);	//    Analog Inputs Enable=0xC0
+	my_write(0xD07, 0x30);	//    ADC Enable=0x30
+	my_write(0xD08, 0x30);	//    Analog Output Enable=0x30
+	my_write(0xD09, 0x30);	//    Digital Output Enable=0x30
+	my_write(0xD0A, 0x4F);	//    Mute Enable=0x4F
+	my_write(0xD0B, 0x7F);	//    Short Circuit Disable=0x7F
+	my_write(0xD0C, 0x80);	//    Power-up for Headset=0x80
+	my_write(0xD0D, 0x00);	//    Envelope Threshold=0x0
+	my_write(0xD0E, 0x00);	//    Envelope Decay Time=0x0
+	my_write(0xD0F, 0xF0);	//    Class-D Configuration=0xF0
+	my_write(0xD10, 0x32);	//    PWM VIBNL Configuration=0x32
+	my_write(0xD11, 0x32);	//    PWM VIBPL Configuration=0x32
+	my_write(0xD12, 0x32);	//    PWM VIBNR Configuration=0x32
+	my_write(0xD13, 0x32);	//    PWM VIBPR Configuration=0x32
+	my_write(0xD14, 0x00);	//    Microphone 1 Gain=0x0
+	my_write(0xD15, 0x00);	//    Microphone 2 Gain=0x0
+	my_write(0xD16, 0x00);	//    Left line-in and HS Analog Gain=0x0
+	my_write(0xD17, 0x00);	//    Right line-in and HS Analog Gain=0x0
+	my_write(0xD18, 0x1F);	//    Line-in to HSL Gain=0x1F
+	my_write(0xD19, 0x1F);	//    Line-in to HSR Gain=0x1F
+	my_write(0xD1A, 0xF0);	//    AD Channel Filters Configuration=0xF0
+	my_write(0xD1B, 0x85);	//    TDM Configuration 1=0x85
+	my_write(0xD1C, 0x94);	//    TDM Configuration 2=0x94
+	my_write(0xD1D, 0x02);	//    TDM loopback control=0x2
+	my_write(0xD1E, 0x00);	//    TDM format=0x0
+	my_write(0xD1F, 0x10);	//    AD Data allocation in Slot 0 to 1=0x10
+	my_write(0xD20, 0xCC);	//    AD Data allocation in Slot 2 to 3=0xCC
+	my_write(0xD21, 0xCC);	//    AD Data allocation in Slots 4 to 5=0xCC
+	my_write(0xD22, 0xCC);	//    AD Data allocation in Slots 6 to 7=0xCC
+	my_write(0xD23, 0xCC);	//    AD Data allocation in Slots 8 to 9=0xCC
+	my_write(0xD24, 0xCC);	//    AD Data allocation in Slots 10 to 11=0xCC
+	my_write(0xD25, 0xCC);	//    AD Data allocation in Slots 12 to 13=0xCC
+	my_write(0xD26, 0xCC);	//    AD Data allocation in Slots 14 to 15=0xCC
+	my_write(0xD27, 0xCC);	//    AD Data allocation in Slots 16 to 17=0xCC
+	my_write(0xD28, 0xCC);	//    AD Data allocation in Slots 18 to 19=0xCC
+	my_write(0xD29, 0xCC);	//    AD Data allocation in Slots 20 to 21=0xCC
+	my_write(0xD2A, 0xCC);	//    AD Data allocation in Slots 22 to 23=0xCC
+	my_write(0xD2B, 0xCC);	//    AD Data allocation in Slots 24 to 25=0xCC
+	my_write(0xD2C, 0xCC);	//    AD Data allocation in Slots 26 to 27=0xCC
+	my_write(0xD2D, 0xCC);	//    AD Data allocation in Slots 28 to 29=0xCC
+	my_write(0xD2E, 0xCC);	//    AD Data allocation in Slots 30 to 31=0xCC
+	my_write(0xD2F, 0x00);	//    AD slot 0/7 tristate=0x0
+	my_write(0xD30, 0x00);	//    AD slot 8/15 tristate=0x0
+	my_write(0xD31, 0x00);	//    AD slot 16/23 tristate=0x0
+	my_write(0xD32, 0x00);	//    AD slot 24/31 tristate=0x0
+	my_write(0xD33, 0x08);	//    Slots selection for DA path 1=0x8
+	my_write(0xD34, 0x09);	//    Slots selection for DA path 2=0x9
+	my_write(0xD35, 0x00);	//    Slots selection for DA path 3=0x0
+	my_write(0xD36, 0x00);	//    Slots selection for DA path 4=0x0
+	my_write(0xD37, 0x00);	//    Slots selection for DA path 5=0x0
+	my_write(0xD38, 0x00);	//    Slots selection for DA path 6=0x0
+	my_write(0xD39, 0x00);	//    IRQ mask lsb=0x0
+	my_write(0xD3A, 0x00);	//    IRQ status lsb=0x0
+	my_write(0xD3B, 0x00);	//    IRQ mask msb=0x0
+	my_write(0xD3C, 0x00);	//    IRQ status msb=0x0
+	my_write(0xD3D, 0x00);	//    Fade speed=0x0
+	my_write(0xD3E, 0x00);	//    DMIC decimator filter=0x0
+	my_write(0xD3F, 0xF0);	//    muxing lsb=0xF0
+	my_write(0xD40, 0x00);	//    muxing msb=0x0
+	my_write(0xD41, 0x1F);	//    AD1 Digital Gain=0x1F
+	my_write(0xD42, 0x1F);	//    AD2 Digital Gain=0x1F
+	my_write(0xD43, 0x1F);	//    AD3 Digital Gain=0x1F
+	my_write(0xD44, 0x1F);	//    AD4 Digital Gain=0x1F
+	my_write(0xD45, 0x1F);	//    AD5 Digital Gain=0x1F
+	my_write(0xD46, 0x1F);	//    AD6 Digital Gain=0x1F
+	my_write(0xD47, 0x00);	//    DA1 digital Gain=0x00
+	my_write(0xD48, 0x00);	//    DA2 digital Gain=0x00
+	my_write(0xD49, 0x3F);	//    DA3 digital Gain=0x3F
+	my_write(0xD4A, 0x3F);	//    DA4 digital Gain=0x3F
+	my_write(0xD4B, 0x3F);	//    DA5 digital Gain=0x3F
+	my_write(0xD4C, 0x3F);	//    DA6 digital Gain=0x3F
+	my_write(0xD4D, 0x3F);	//    AD1 loopback to HFL digital gain=0x3F
+	my_write(0xD4E, 0x3F);	//    AD2 loopback to HFR digital gain=0x3F
+	my_write(0xD4F, 0x08);	//    HSL and EAR digital gain=0x8
+	my_write(0xD50, 0x08);	//    HSR digital gain=0x8
+	my_write(0xD51, 0x1F);	//    Side tone FIR1 gain=0x1F
+	my_write(0xD52, 0x1F);	//    Side tone FIR2 gain=0x1F
+	my_write(0xD53, 0x00);	//    ANC filter control=0x0
+	my_write(0xD54, 0x00);	//    ANC Warped Delay Line Shift=0x0
+	my_write(0xD55, 0x00);	//    ANC FIR output Shift=0x0
+	my_write(0xD56, 0x00);	//    ANC IIR output Shift=0x0
+	my_write(0xD57, 0x00);	//    ANC FIR coefficients msb=0x0
+	my_write(0xD58, 0x00);	//    ANC FIR coefficients lsb=0x0
+	my_write(0xD59, 0x00);	//    ANC IIR coefficients msb=0x0
+	my_write(0xD5A, 0x00);	//    ANC IIR coefficients lsb=0x0
+	my_write(0xD5B, 0x00);	//    ANC Warp delay msb=0x0
+	my_write(0xD5C, 0x00);	//    ANC Warp delay lsb=0x0
+	my_write(0xD5D, 0x00);	//    ANC FIR peak register MSB=0x0
+	my_write(0xD5E, 0x00);	//    ANC FIR peak register LSB=0x0
+	my_write(0xD5F, 0x00);	//    ANC IIR peak register. MSB part=0x0
+	my_write(0xD60, 0x00);	//    ANC IIR peak register. LSB part=0x0
+	my_write(0xD61, 0x00);	//    Side tone FIR address=0x0
+	my_write(0xD62, 0x00);	//    Side tone FIR coefficient MSB=0x0
+	my_write(0xD63, 0x00);	//    Side tone FIR coefficient LSB=0x0
+	my_write(0xD64, 0x00);	//    Filters control=0x0
+	my_write(0xD65, 0x00);	//    Class D EMI Control=0x0
+	my_write(0xD66, 0x00);	//    Class D  control path=0x0
+	my_write(0xD67, 0x00);	//    Class D control gain=0x0
+	my_write(0xD68, 0x00);	//    Burst FIFO int control=0x0
+	my_write(0xD69, 0x00);	//    Burst FIFO length=0x0
+	my_write(0xD6A, 0x00);	//    Burst FIFO control=0x0
+	my_write(0xD6B, 0x00);	//    Burst FIFO switch frame=0x0
+	my_write(0xD6C, 0x00);	//    Burst FIFO wake up delay=0x0
+	my_write(0xD6D, 0x00);	//    Burst FIFO samples number=0x0
+	my_write(0xD70, 0x00);	//    CR112=0x0
+	my_write(0xD71, 0x04);	//    CR113=0x4
+	my_write(0xD72, 0x00);	//    CR114=0x0
+	my_write(0xD73, 0x00);	//    CR115=0x0
+	my_write(0xD74, 0x00);	//    CR116=0x0
+	my_write(0xD75, 0x00);	//    CR117=0x0
+	my_write(0xD76, 0x00);	//    CR118=0x0
+	my_write(0xD77, 0x00);	//    CR119=0x0
+	my_write(0xD78, 0x00);	//    CR120=0x0
+	my_write(0xD79, 0x00);	//    CR121=0x0
+	my_write(0xD7A, 0x00);	//    CR122=0x0
+	my_write(0xD7B, 0x00);	//    CR123=0x0
+}
+
+static int configure_direct_rendering(struct snd_pcm_substream *substream)
+{
+	int error = 0, stream_id;
+	int status = 0;
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	audio_stream_t *ptr_audio_stream = NULL;
+
+	stream_id = substream->pstr->stream;
+
+	if (stream_id == SNDRV_PCM_STREAM_PLAYBACK) {
+		runtime->hw = snd_u8500_playback_hw;
+	} else {
+		runtime->hw = snd_u8500_capture_hw;
+	}
+
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x04)));	//MSP_GCR
+
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x08)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x0C)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x10)));	//MSP
+
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x30)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x34)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x38)));	//MSP
+
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x40)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x44)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x48)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x4C)));	//MSP
+
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x60)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x64)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x68)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x6C)));	//MSP
+
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x18)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x20)));	//MSP
+	writel(0x0, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x2C)));	//MSP
+
+	dsp_configure_audio_codec();
+
+#if DRIVER_DEBUG > 0
+	{
+		dump_msp_registers();
+		dump_acodec_registers();
+	}
+#endif
+
+	ptr_audio_stream = &chip->stream[ALSA_PCM_DEV][stream_id];
+
+	ptr_audio_stream->substream = substream;
+
+	FUNC_EXIT();
+	return 0;
+}
+
+/**
+ * snd_u8500_alsa_pcm_open
+ * @substream - pointer to the playback/capture substream structure
+ *
+ *  This routine is used by alsa framework to open a pcm stream .
+ *  Here a dma pipe is requested and device is configured(default).
+ */
+static int snd_u8500_alsa_pcm_open(struct snd_pcm_substream *substream)
+{
+	int error = 0, stream_id, status = 0;
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	audio_stream_t *ptr_audio_stream = NULL;
+
+	FUNC_ENTER();
+
+	if (ENABLE == chip->direct_rendering_mode) {
+		configure_direct_rendering(substream);
+		return 0;
+	} else {
+		stream_id = substream->pstr->stream;
+		status = u8500_acodec_open(I2S_CLIENT_MSP1, stream_id);
+
+		if (status) {
+			printk("failed in getting open\n");
+			return -1;
+		}
+
+		if (!active_user)
+			error = u8500_acodec_setuser(USER_ALSA);
+		if (error)
+			return error;
+		else
+			active_user++;
+
+		error =
+		    snd_pcm_hw_constraint_list(runtime, 0,
+					       SNDRV_PCM_HW_PARAM_RATE,
+					       &constraints_rate);
+		if (error < 0) {
+			stm_error
+			    (": error initializing hw sample rate constraint\n");
+			return error;
+		}
+
+		/* configure the default sampling rate for the acodec */
+		second_config = 0;
+
+		if ((error = configure_rate(substream, ACODEC_CONFIG_REQUIRED)))
+			return error;
+
+		/* Set the hardware configuration */
+		stream_id = substream->pstr->stream;
+		if (stream_id == SNDRV_PCM_STREAM_PLAYBACK) {
+			runtime->hw = snd_u8500_playback_hw;
+			/* configure the output sink for the acodec */
+			if ((error =
+			     u8500_acodec_select_output(chip->output_device,
+							USER_ALSA,
+							chip->tdm8_ch_mode))) {
+				stm_error(" : select output failed\n");
+				return error;
+			}
+
+			/* configure the volume settings for the acodec */
+			if ((error =
+			     u8500_acodec_set_output_volume(chip->output_device,
+							    chip->
+							    output_lvolume,
+							    chip->
+							    output_rvolume,
+							    USER_ALSA))) {
+				stm_error(" : set output volume failed\n");
+				return error;
+			}
+		} else {
+			runtime->hw = snd_u8500_capture_hw;
+			/* configure the input source for the acodec */
+			if ((error =
+			     u8500_acodec_select_input(chip->input_device,
+						       USER_ALSA,
+						       chip->tdm8_ch_mode))) {
+				stm_error(" : select input failed\n");
+				return error;
+			}
+			/*u8500_acodec_set_src_power_cntrl(AB8500_CODEC_SRC_D_MICROPHONE_1,ENABLE);
+			   u8500_acodec_set_src_power_cntrl(AB8500_CODEC_SRC_D_MICROPHONE_2,ENABLE);
+
+			   u8500_acodec_set_input_volume(AB8500_CODEC_SRC_D_MICROPHONE_1,
+			   chip->input_lvolume,
+			   chip->input_rvolume,
+			   USER_ALSA);
+
+			   u8500_acodec_set_input_volume(AB8500_CODEC_SRC_D_MICROPHONE_2,
+			   chip->input_lvolume,
+			   chip->input_rvolume,
+			   USER_ALSA);
+			 */
+
+			if ((error =
+			     u8500_acodec_set_input_volume(chip->input_device,
+							   chip->input_lvolume,
+							   chip->input_rvolume,
+							   USER_ALSA))) {
+				stm_error(" : set input volume failed\n");
+				return error;
+			}
+		}
+
+		u8500_acodec_set_burst_mode_fifo(chip->burst_fifo_mode);
+
+#if DRIVER_DEBUG > 0
+		{
+			dump_msp_registers();
+			dump_acodec_registers();
+		}
+#endif
+
+		ptr_audio_stream = &chip->stream[ALSA_PCM_DEV][stream_id];
+
+		ptr_audio_stream->substream = substream;
+
+		if (DISABLE == chip->direct_rendering_mode) {
+			stm_config_hw(chip, substream, ALSA_PCM_DEV, stream_id);
+		}
+		sema_init(&(ptr_audio_stream->alsa_sem), 1);
+		init_completion(&(ptr_audio_stream->alsa_com));
+		ptr_audio_stream->state = ALSA_STATE_UNPAUSE;
+
+		FUNC_EXIT();
+		return 0;
+	}
+}
+
+/**
+ * snd_u8500_alsa_pcm_hw_params
+ * @substream - pointer to the playback/capture substream structure
+ * @hw_params - specifies the hw parameters like format/no of channels etc
+ *
+ *  This routine is used by alsa framework to allocate a dma buffer
+ *  used to transfer the data from user space to kernel space
+ *
+ */
+static int snd_u8500_alsa_pcm_hw_params(struct snd_pcm_substream *substream,
+					struct snd_pcm_hw_params *hw_params)
+{
+	return snd_pcm_lib_malloc_pages(substream,
+				params_buffer_bytes(hw_params));
+}
+
+/**
+ * snd_u8500_alsa_pcm_hw_free
+ * @substream - pointer to the playback/capture substream structure
+ *
+ *  This routine is used by alsa framework to deallocate a dma buffer
+ *  allocated berfore by snd_u8500_alsa_pcm_hw_params
+ */
+static int snd_u8500_alsa_pcm_hw_free(struct snd_pcm_substream *substream)
+{
+	stm_hw_free(substream);
+	return 0;
+}
+
+/**
+ * snd_u8500_alsa_pcm_prepare
+ * @substream - pointer to the playback/capture substream structure
+ *
+ *  This callback is called whene the pcm is "prepared" Here is possible
+ *  to set the format type ,sample rate ,etc.The callback is called as
+ *  well everytime a recovery after an underrun happens.
+ */
+
+static int snd_u8500_alsa_pcm_prepare(struct snd_pcm_substream *substream)
+{
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	int error, stream_id;
+
+	FUNC_ENTER();
+
+	if (chip->freq != runtime->rate || chip->channels != runtime->channels) {
+		stm_dbg(DBG_ST.alsa, " freq not same, %d %d\n", chip->freq,
+			runtime->rate);
+		stm_dbg(DBG_ST.alsa, " channels not same, %d %d\n",
+			chip->channels, runtime->channels);
+		if (chip->channels != runtime->channels) {
+			chip->channels = runtime->channels;
+			if ((error =
+			     stm_config_hw(chip, substream, ALSA_PCM_DEV,
+					   -1))) {
+				stm_dbg(DBG_ST.alsa,
+					"In func %s, stm_config_hw fails",
+					__FUNCTION__);
+				return error;
+			}
+		}
+		chip->freq = runtime->rate;
+		second_config = 1;
+		stream_id = substream->pstr->stream;
+		if (stream_id == SNDRV_PCM_STREAM_PLAYBACK)
+			u8500_acodec_close(I2S_CLIENT_MSP1,
+					   ACODEC_DISABLE_TRANSMIT);
+		else if (stream_id == SNDRV_PCM_STREAM_CAPTURE)
+			u8500_acodec_close(I2S_CLIENT_MSP1,
+					   ACODEC_DISABLE_RECEIVE);
+
+		error = u8500_acodec_open(I2S_CLIENT_MSP1, stream_id);
+		if (error) {
+			printk("failed in getting open\n");
+			return -1;
+		}
+		if ((error =
+		     configure_rate(substream, ACODEC_CONFIG_NOT_REQUIRED))) {
+			stm_dbg(DBG_ST.alsa, "In func %s, configure_rate fails",
+				__FUNCTION__);
+			return error;
+		}
+	}
+
+	FUNC_EXIT();
+	return 0;
+}
+
+/**
+ * snd_u8500_alsa_pcm_trigger
+ * @substream -  pointer to the playback/capture substream structure
+ * @cmd - specifies the command : start/stop/pause/resume
+ *
+ *  This callback is called whene the pcm is started ,stopped or paused
+ *  The action is specified in the second argument, SND_PCM_TRIGGER_XXX in
+ *  <sound/pcm.h>.
+ *  This callback is atomic and the interrupts are disabled , so you can't
+ *  call other functions that need interrupts without possible risks
+ */
+static int snd_u8500_alsa_pcm_trigger(struct snd_pcm_substream *substream,
+				      int cmd)
+{
+	int stream_id = substream->pstr->stream;
+	audio_stream_t *stream = NULL;
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	int error = 0;
+
+	FUNC_ENTER();
+
+	stream = &chip->stream[ALSA_PCM_DEV][stream_id];
+
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_START:
+		/* Start the pcm engine */
+		stm_dbg(DBG_ST.alsa, " TRIGGER START\n");
+		if (stream->active == 0) {
+			stream->active = 1;
+			stm_trigger_alsa(stream);
+			break;
+		}
+		stm_error(": H/w is busy\n");
+		return -EINVAL;
+
+	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
+		stm_dbg(DBG_ST.alsa, " SNDRV_PCM_TRIGGER_PAUSE_PUSH\n");
+		if (stream->active == 1) {
+			stm_pause_alsa(stream);
+		}
+		break;
+	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
+		stm_dbg(DBG_ST.alsa, " SNDRV_PCM_TRIGGER_PAUSE_RELEASE\n");
+		if (stream->active == 1)
+			stm_unpause_alsa(stream);
+		break;
+	case SNDRV_PCM_TRIGGER_STOP:
+		/* Stop the pcm engine */
+		stm_dbg(DBG_ST.alsa, " TRIGGER STOP\n");
+		if (stream->active == 1)
+			stm_stop_alsa(stream);
+		break;
+	default:
+		stm_error(": invalid command in pcm trigger\n");
+		return -EINVAL;
+	}
+
+	FUNC_EXIT();
+	return error;
+}
+
+/**
+ * snd_u8500_alsa_pcm_pointer
+ * @substream - pointer to the playback/capture substream structure
+ *
+ *  This callback is called whene the pcm middle layer inquires the current
+ *  hardware position on the buffer .The position is returned in frames
+ *  ranged from 0 to buffer_size -1
+ */
+static snd_pcm_uframes_t snd_u8500_alsa_pcm_pointer(struct snd_pcm_substream
+						    *substream)
+{
+	unsigned int offset;
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	audio_stream_t *stream =
+	    &chip->stream[ALSA_PCM_DEV][substream->pstr->stream];
+	struct snd_pcm_runtime *runtime = stream->substream->runtime;
+
+	offset = bytes_to_frames(runtime, stream->old_offset);
+	if (offset < 0 || stream->old_offset < 0)
+		stm_dbg(DBG_ST.alsa, " Offset=%i %i\n", offset,
+			stream->old_offset);
+
+	return offset;
+}
+
+static struct snd_pcm_ops snd_u8500_alsa_playback_ops = {
+	.open = snd_u8500_alsa_pcm_open,
+	.close = snd_u8500_alsa_pcm_close,
+	.ioctl = snd_pcm_lib_ioctl,
+	.hw_params = snd_u8500_alsa_pcm_hw_params,
+	.hw_free = snd_u8500_alsa_pcm_hw_free,
+	.prepare = snd_u8500_alsa_pcm_prepare,
+	.trigger = snd_u8500_alsa_pcm_trigger,
+	.pointer = snd_u8500_alsa_pcm_pointer,
+};
+
+static struct snd_pcm_ops snd_u8500_alsa_capture_ops = {
+	.open = snd_u8500_alsa_pcm_open,
+	.close = snd_u8500_alsa_pcm_close,
+	.ioctl = snd_pcm_lib_ioctl,
+	.hw_params = snd_u8500_alsa_pcm_hw_params,
+	.hw_free = snd_u8500_alsa_pcm_hw_free,
+	.prepare = snd_u8500_alsa_pcm_prepare,
+	.trigger = snd_u8500_alsa_pcm_trigger,
+	.pointer = snd_u8500_alsa_pcm_pointer,
+};
+
+#ifdef CONFIG_U8500_ACODEC_POLL
+
+/**
+* u8500_alsa_pio_start
+* @stream - pointer to the playback/capture audio_stream_t structure
+*
+*  This function sends/receive one chunck of stream data to/from MSP
+*/
+static void u8500_alsa_pio_start(audio_stream_t * stream)
+{
+	unsigned int offset, dma_size, stream_id;
+	int ret_val;
+	struct snd_pcm_substream *substream = stream->substream;
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	stream_id = substream->pstr->stream;
+
+	FUNC_ENTER();
+	dma_size = frames_to_bytes(runtime, runtime->period_size);
+	offset = dma_size * stream->period;
+	stream->old_offset = offset;
+
+	stm_dbg(DBG_ST.alsa, " Transfer started\n");
+	stm_dbg(DBG_ST.alsa, " address = %x  size=%d\n",
+		(runtime->dma_addr + offset), dma_size);
+
+	/* Send our stuff */
+	if (stream_id == SNDRV_PCM_STREAM_PLAYBACK)
+#ifdef CONFIG_U8500_ACODEC_DMA
+		u8500_acodec_send_data(I2S_CLIENT_MSP1,
+				       (void *)(runtime->dma_addr + offset),
+				       dma_size, 1);
+#else
+		u8500_acodec_send_data(I2S_CLIENT_MSP1,
+				       (void *)(runtime->dma_area + offset),
+				       dma_size, 0);
+#endif
+	else
+#ifdef CONFIG_U8500_ACODEC_DMA
+		u8500_acodec_receive_data(I2S_CLIENT_MSP1,
+					  (void *)(runtime->dma_addr + offset),
+					  dma_size, 1);
+#else
+		u8500_acodec_receive_data(I2S_CLIENT_MSP1,
+					  (void *)(runtime->dma_area + offset),
+					  dma_size, 0);
+#endif
+
+	stream->period++;
+	stream->period %= runtime->periods;
+	stream->periods++;
+
+	FUNC_EXIT();
+
+}
+
+/**
+* acodec_feeding_thread
+* @data - void pointer to the playback/capture audio_stream_t structure
+*
+*	This thread sends/receive data to MSP while stream is active
+*/
+static int acodec_feeding_thread(void *data)
+{
+	audio_stream_t *stream = (audio_stream_t *) data;
+
+	FUNC_ENTER();
+	daemonize("acodec_feeding_thread");
+	allow_signal(SIGKILL);
+	down(&stream->alsa_sem);
+
+	while ((!signal_pending(current)) && (stream->active)) {
+		if (stream->state == ALSA_STATE_PAUSE)
+			wait_for_completion(&(stream->alsa_com));
+
+		u8500_alsa_pio_start(stream);
+		if (stream->substream)
+			snd_pcm_period_elapsed(stream->substream);
+	}
+
+	up(&stream->alsa_sem);
+
+	FUNC_EXIT();
+	return 0;
+}
+
+/**
+* acodec_feeding_thread
+* @stream - pointer to the playback/capture audio_stream_t structure
+*
+*	This function creates a kernel thread .
+*/
+
+int spawn_acodec_feeding_thread(audio_stream_t * stream)
+{
+	pid_t pid;
+
+	FUNC_ENTER();
+
+	pid =
+	    kernel_thread(acodec_feeding_thread, stream,
+			  CLONE_FS | CLONE_SIGHAND);
+
+	FUNC_EXIT();
+	return 0;
+}
+#endif
+
+/**
+ * dma_eot_handler
+ * @data - pointer to structure set in the dma callback handler
+ *
+ *  This is the PCM tasklet handler linked to a pipe, its role is to tell
+ *  the PCM middler layer whene the buffer position goes across the prescribed
+ *  period size.To inform of this the snd_pcm_period_elapsed is called.
+ *
+ * this callback will be called in case of DMA_EVENT_TC only
+ */
+static void dma_eot_handler(void *data)
+{
+	audio_stream_t *stream = data;
+
+	/* snd_pcm_period_elapsed() is _not_ to be protected
+	 */
+	stm_dbg(DBG_ST.alsa,
+		"One transfer complete.. going to start the next one\n");
+
+	if (stream->substream)
+		snd_pcm_period_elapsed(stream->substream);
+	if (stream->state == ALSA_STATE_PAUSE)
+		return;
+	if (stream->active == 1) {
+		u8500_alsa_dma_start(stream);
+	}
+}
+
+/**
+ * u8500_alsa_dma_start - used to transmit or recive a dma chunk
+ * @stream - specifies the playback/record stream structure
+ */
+void u8500_alsa_dma_start(audio_stream_t * stream)
+{
+	unsigned int offset, dma_size, stream_id;
+
+	struct snd_pcm_substream *substream = stream->substream;
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	u8500_acodec_chip_t *u8500_chip = NULL;
+	stream_id = substream->pstr->stream;
+	u8500_chip = snd_pcm_substream_chip(substream);
+
+	dma_size = frames_to_bytes(runtime, runtime->period_size);
+	offset = dma_size * stream->period;
+	stream->old_offset = offset;
+
+	if (stream_id == SNDRV_PCM_STREAM_PLAYBACK)
+#ifdef CONFIG_U8500_ACODEC_DMA
+		u8500_acodec_send_data(I2S_CLIENT_MSP1,
+				       (void *)(runtime->dma_addr + offset),
+				       dma_size, 1);
+#else
+		u8500_acodec_send_data(I2S_CLIENT_MSP1,
+				       (void *)(runtime->dma_area + offset),
+				       dma_size, 0);
+#endif
+	else
+#ifdef CONFIG_U8500_ACODEC_DMA
+		u8500_acodec_receive_data(I2S_CLIENT_MSP1,
+					  (void *)(runtime->dma_addr + offset),
+					  dma_size, 1);
+#else
+		u8500_acodec_receive_data(I2S_CLIENT_MSP1,
+					  (void *)(runtime->dma_area + offset),
+					  dma_size, 0);
+#endif
+
+	stm_dbg(DBG_ST.alsa, " DMA Transfer started\n");
+	stm_dbg(DBG_ST.alsa, " address = %x  size=%d\n",
+		(runtime->dma_addr + offset), dma_size);
+
+	stream->period++;
+	stream->period %= runtime->periods;
+	stream->periods++;
+}
+
+/**
+* u8500_audio_init
+* @chip - pointer to u8500_acodec_chip_t structure.
+*
+* This function intialises the u8500 chip structure with default values
+*/
+static void u8500_audio_init(u8500_acodec_chip_t * chip)
+{
+	audio_stream_t *ptr_audio_stream = NULL;
+
+	ptr_audio_stream =
+	    &chip->stream[ALSA_PCM_DEV][SNDRV_PCM_STREAM_PLAYBACK];
+	/* Setup DMA stuff */
+	strlcpy(ptr_audio_stream->id, "u8500 playback",
+		sizeof(ptr_audio_stream->id));
+	ptr_audio_stream->stream_id = SNDRV_PCM_STREAM_PLAYBACK;
+
+	/* default initialization  for playback */
+	ptr_audio_stream->active = 0;
+	ptr_audio_stream->period = 0;
+	ptr_audio_stream->periods = 0;
+	ptr_audio_stream->old_offset = 0;
+
+	ptr_audio_stream =
+	    &chip->stream[ALSA_PCM_DEV][SNDRV_PCM_STREAM_CAPTURE];
+	strlcpy(ptr_audio_stream->id, "u8500 capture",
+		sizeof(ptr_audio_stream->id));
+	ptr_audio_stream->stream_id = SNDRV_PCM_STREAM_CAPTURE;
+
+	/* default initialization  for capture */
+	ptr_audio_stream->active = 0;
+	ptr_audio_stream->period = 0;
+	ptr_audio_stream->periods = 0;
+	ptr_audio_stream->old_offset = 0;
+
+	chip->freq = DEFAULT_SAMPLE_RATE;
+	chip->channels = 1;
+	chip->input_lvolume = DEFAULT_GAIN;
+	chip->input_rvolume = DEFAULT_GAIN;
+	chip->output_lvolume = DEFAULT_VOLUME;
+	chip->output_rvolume = DEFAULT_VOLUME;
+	chip->output_device = DEFAULT_OUTPUT_DEVICE;
+	chip->input_device = DEFAULT_INPUT_DEVICE;
+	chip->analog_lpbk = DEFAULT_LOOPBACK_STATE;
+	chip->digital_lpbk = DEFAULT_LOOPBACK_STATE;
+	chip->playback_switch = DEFAULT_SWITCH_STATE;
+	chip->capture_switch = DEFAULT_SWITCH_STATE;
+	chip->tdm8_ch_mode = DEFAULT_TDM8_CH_MODE_STATE;
+	chip->direct_rendering_mode = DEFAULT_DIRECT_RENDERING_STATE;
+	chip->burst_fifo_mode = DEFAULT_BURST_FIFO_STATE;
+	chip->fm_playback_mode = DEFAULT_FM_PLAYBACK_STATE;
+	chip->fm_tx_mode = DEFAULT_FM_TX_STATE;
+
+	//HDMI Default params set
+	chip->hdmi_params.sampling_freq = 48000;
+	chip->hdmi_params.channel_count = 2;
+}
+
+/**
+ * snd_card_u8500_alsa_pcm_new - constructor for a new pcm cmponent
+ * @chip - pointer to chip specific data
+ * @device - specifies the card number
+ */
+static int snd_card_u8500_alsa_pcm_new(u8500_acodec_chip_t * chip, int device)
+{
+	struct snd_pcm *pcm;
+	int err;
+
+	if ((err = snd_pcm_new(chip->card, "u8500", device, 1, 1, &pcm)) < 0) {
+		stm_error(": error in snd_pcm_new\n");
+		return err;
+	}
+
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_PLAYBACK,
+			&snd_u8500_alsa_playback_ops);
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE,
+			&snd_u8500_alsa_capture_ops);
+
+	pcm->private_data = chip;
+	pcm->info_flags = 0;
+	chip->pcm = pcm;
+	strcpy(pcm->name, "u8500_alsa");
+
+	u8500_audio_init(pcm->private_data);
+	return 0;
+}
+
+static int u8500_register_alsa_controls(struct snd_card *card,
+					u8500_acodec_chip_t * u8500_chip)
+{
+	int error;
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_playback_vol_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_playback_vol_ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_capture_vol_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_capture_vol_ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_playback_sink_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error(": error initializing playback ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_capture_src_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_playback_sink_ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_analog_lpbk_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_analog_lpbk_ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_digital_lpbk_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_digital_lpbk_ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_playback_switch_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_playback_switch_ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_capture_switch_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_capture_switch_ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_playback_power_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_playback_power_ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_capture_power_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_capture_power_ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_tdm_mode_ctrl, u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_tdm_mode_ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_direct_rendering_mode_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_direct_rendering_mode_ctrl interface \n\n");
+		return (-1);
+	}
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_pcm_rendering_mode_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_pcm_rendering_mode_ctrl interface \n\n");
+		return (-1);
+	}
+
+	return 0;
+}
+
+static int __init u8500_alsa_probe(struct platform_device *devptr)
+{
+	//static int card_count=0;
+	int error;
+	struct snd_card *card, *hdmi_card;
+	u8500_acodec_chip_t *u8500_chip;
+
+	/*Set currently active users to 0 */
+	active_user = 0;
+
+	error = snd_card_create(0, NULL, THIS_MODULE, sizeof(u8500_acodec_chip_t), &card);
+	if (error < 0) {
+		stm_error(": error in snd_card_create\n");
+		return error;
+	}
+
+	u8500_chip = (u8500_acodec_chip_t *) card->private_data;
+	u8500_chip->card = card;
+
+	if ((error = snd_card_u8500_alsa_pcm_new(u8500_chip, 0)) < 0) {
+		stm_error(": pcm interface can't be initialized\n\n");
+		goto nodev;
+	}
+
+	if ((error = snd_card_u8500_alsa_hdmi_new(u8500_chip, 1)) < 0) {
+		stm_error(": alsa HDMI interface can't be initialized\n\n");
+		goto nodev;
+	}
+
+	if (u8500_register_alsa_controls(card, u8500_chip) < 0) {
+		goto nodev;
+	}
+
+	if (u8500_register_alsa_hdmi_controls(card, u8500_chip) < 0) {
+		goto nodev;
+	}
+#if 0
+	///////////////////////////////$ H D M I $//////////////////////////////////////
+
+	if (card_count == 1) {
+		hdmi_card =
+		    snd_card_new(1, NULL, THIS_MODULE,
+				 sizeof(u8500_acodec_chip_t));
+		if (hdmi_card == NULL) {
+			stm_error(": error in hdmi - snd_card_new\n");
+			return -ENOMEM;
+		}
+
+		u8500_chip = (u8500_acodec_chip_t *) hdmi_card->private_data;
+		u8500_chip->card = hdmi_card;
+
+		if ((error = snd_card_u8500_alsa_hdmi_new(u8500_chip, 0)) < 0) {
+			stm_error
+			    (": alsa HDMI interface can't be initialized\n\n");
+			goto nodev;
+		}
+
+		if (u8500_register_alsa_hdmi_controls(card, u8500_chip) < 0) {
+			goto nodev;
+		}
+	}
+#endif ////////////////////////////////////////////////////////
+
+	/*char driver[16];              driver name
+	   char shortname[32];          short name of this soundcard
+	   char longname[80];           name of this soundcard
+	   char mixername[80];          mixer name
+	   char components[80]; card components delimited withspace */
+
+	strcpy(card->driver, "u8500 alsa");
+	strcpy(card->shortname, "u8500 alsa pcm hdmi driver");
+	strcpy(card->longname, "u8500 alsa pcm hdmi driver");
+
+	snd_card_set_dev(card, &devptr->dev);
+
+	if ((error = snd_card_register(card)) == 0) {
+		stm_info("u8500 audio <hdmi> support running..\n");
+		platform_set_drvdata(devptr, card);
+		return 0;
+	}
+
+      nodev:
+	snd_card_free(card);
+	return error;
+}
+
+static int __devexit u8500_alsa_remove(struct platform_device *devptr)
+{
+	snd_card_free(platform_get_drvdata(devptr));
+	platform_set_drvdata(devptr, NULL);
+	stm_info("u8500 audio support stopped\n");
+
+	/*Set currently active users to 0 */
+	active_user = 0;
+
+	return 0;
+}
+
+static struct platform_driver u8500_alsa_driver = {
+	.probe = u8500_alsa_probe,
+	.remove = __devexit_p(u8500_alsa_remove),
+	.driver = {
+		   .name = U8500_ALSA_DRIVER,
+		   },
+};
+
+/**
+* u8500_alsa_init - Entry function of AB8500 alsa driver
+*
+* This function registers u8500 alsa driver with linux framework
+*/
+static int __init u8500_alsa_init(void)
+{
+	int err;
+
+	if ((err = platform_driver_register(&u8500_alsa_driver)) < 0)
+		return err;
+	device =
+	    platform_device_register_simple(U8500_ALSA_DRIVER, -1, NULL, 0);
+	if (IS_ERR(device)) {
+		platform_driver_unregister(&u8500_alsa_driver);
+		return PTR_ERR(device);
+	}
+	//DBG_ST.acodec = 1;
+	//DBG_ST.alsa = 1;
+
+	return 0;
+}
+
+static void __exit u8500_alsa_exit(void)
+{
+	platform_device_unregister(device);
+	platform_driver_unregister(&u8500_alsa_driver);
+}
+
+module_init(u8500_alsa_init);
+module_exit(u8500_alsa_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("AB8500 ALSA driver");
diff --git a/sound/arm/u8500_alsa_ab8500.h b/sound/arm/u8500_alsa_ab8500.h
new file mode 100644
index 00000000000..dda802c2156
--- /dev/null
+++ b/sound/arm/u8500_alsa_ab8500.h
@@ -0,0 +1,165 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Deepak Karda
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 _U8500_ALSA_H_
+#define _U8500_ALSA_H_
+
+#ifdef CONFIG_U8500_AB8500_CUT10
+#include <mach/ab8500_codec_v1_0.h>
+//#include <mach/ab8500_codec_p_v1_0.h>
+#else
+//#include <mach/ab8500_codec_p.h>
+#include <mach/ab8500_codec.h>
+#endif
+#include <mach/u8500_acodec_ab8500.h>
+
+#define DEFAULT_SAMPLE_RATE      48000
+#define NMDK_BUFFER_SIZE 	(64*1024)
+#define U8500_ALSA_DRIVER	"u8500_alsa"
+
+#define MAX_NUMBER_OF_DEVICES		3	/* ALSA_PCM, ALSA_BT, ALSA_HDMI */
+#define MAX_NUMBER_OF_STREAMS		2	/* PLAYBACK, CAPTURE */
+
+#define ALSA_PCM_DEV				0
+#define ALSA_BT_DEV					2
+#define ALSA_HDMI_DEV				1
+
+/* Debugging stuff */
+#ifndef CONFIG_DEBUG_USER
+#define DEBUG_LEVEL 0
+#else
+#define DEBUG_LEVEL 10
+#endif
+
+#if DEBUG_LEVEL > 0
+static int u8500_acodec_debug = DEBUG_LEVEL;
+#define DEBUG(n, args...) do { if (u8500_acodec_debug>(n)) printk(args); } while (0)
+#else
+#define DEBUG(n, args...) do { } while (0)
+#endif
+enum alsa_state {
+	ALSA_STATE_PAUSE,
+	ALSA_STATE_UNPAUSE
+};
+
+/* audio stream definition */
+typedef struct audio_stream_s {
+	char id[64];		/* module identifier  string */
+	int stream_id;		/* stream identifier  */
+	int status;
+	int active;		/* we are using this stream for transfer now */
+	int period;		/* current transfer period */
+	int periods;		/* current count of periods registerd in the DMA engine */
+	enum alsa_state state;
+	unsigned int old_offset;
+	struct snd_pcm_substream *substream;
+	unsigned int exchId;
+	snd_pcm_uframes_t played_frame;
+	struct semaphore alsa_sem;
+	struct completion alsa_com;
+
+} audio_stream_t;
+
+typedef struct hdmi_params_s {
+	int sampling_freq;
+	int channel_count;
+} hdmi_params_t;
+
+/* chip structure definition */
+typedef struct u8500_acodec_s {
+	struct snd_card *card;
+	struct snd_pcm *pcm;
+	struct snd_pcm *pcm_hdmi;
+	struct snd_pcm *pcm_bt;
+	unsigned int freq;
+	unsigned int channels;
+	unsigned int input_lvolume;
+	unsigned int input_rvolume;
+	unsigned int output_lvolume;
+	unsigned int output_rvolume;
+	t_ab8500_codec_src input_device;
+	t_ab8500_codec_dest output_device;
+	t_u8500_bool_state analog_lpbk;
+	t_u8500_bool_state digital_lpbk;
+	t_u8500_bool_state playback_switch;
+	t_u8500_bool_state capture_switch;
+	t_u8500_bool_state tdm8_ch_mode;
+	t_u8500_bool_state direct_rendering_mode;
+	t_u8500_pmc_rendering_state burst_fifo_mode;
+	t_u8500_pmc_rendering_state fm_playback_mode;
+	t_u8500_pmc_rendering_state fm_tx_mode;
+	audio_stream_t stream[MAX_NUMBER_OF_DEVICES][MAX_NUMBER_OF_STREAMS];
+	hdmi_params_t hdmi_params;
+} u8500_acodec_chip_t;
+
+void u8500_alsa_dma_start(audio_stream_t * stream);
+
+#if (defined(CONFIG_U8500_ACODEC_DMA) || defined(CONFIG_U8500_ACODEC_INTR))
+
+#define stm_trigger_alsa(x) u8500_alsa_dma_start(x)
+static void inline stm_pause_alsa(audio_stream_t * stream)
+{
+	if (stream->state == ALSA_STATE_UNPAUSE) {
+		stream->state = ALSA_STATE_PAUSE;
+	}
+
+}
+static void inline stm_unpause_alsa(audio_stream_t * stream)
+{
+	if (stream->state == ALSA_STATE_PAUSE) {
+		stream->state = ALSA_STATE_UNPAUSE;
+		stm_trigger_alsa(stream);
+	}
+}
+static void inline stm_stop_alsa(audio_stream_t * stream)
+{
+	stream->active = 0;
+	stream->period = 0;
+
+}
+static void inline stm_hw_free(struct snd_pcm_substream *substream)
+{
+	snd_pcm_lib_free_pages(substream);
+}
+
+#define stm_close_alsa(x, y,z)
+#define stm_config_hw(w,x, y, z) 0
+
+#else ////// CONFIG_U8500_ACODEC_POLL ////////////
+
+int spawn_acodec_feeding_thread(audio_stream_t * stream);
+//static int configure_dmadev_acodec(struct snd_pcm_substream *substream);
+
+#define stm_trigger_alsa(x) spawn_acodec_feeding_thread(x)
+#define stm_close_alsa(x, y,z)
+#define stm_config_hw(w,x, y, z) 0
+#define stm_hw_free(x)
+static void inline stm_pause_alsa(audio_stream_t * stream)
+{
+	stream->state = ALSA_STATE_PAUSE;
+}
+static void inline stm_unpause_alsa(audio_stream_t * stream)
+{
+	stream->state = ALSA_STATE_UNPAUSE;
+	complete(&stream->alsa_com);
+}
+static void inline stm_stop_alsa(audio_stream_t * stream)
+{
+	stream->active = 0;
+	stream->period = 0;
+	if (stream->state == ALSA_STATE_PAUSE)
+		complete(&stream->alsa_com);
+}
+
+#endif
+#endif /*END OF HEADER FILE */
diff --git a/sound/arm/u8500_alsa_hdmi.c b/sound/arm/u8500_alsa_hdmi.c
new file mode 100644
index 00000000000..dcbc3583ba7
--- /dev/null
+++ b/sound/arm/u8500_alsa_hdmi.c
@@ -0,0 +1,936 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License terms: GNU General Public License (GPL),
+ * version 2.
+ */
+
+/* This include must be defined at this point */
+//#include <sound/driver.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/errno.h>
+#include <linux/ioctl.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/mm.h>
+#include <mach/hardware.h>
+#include <asm/uaccess.h>
+#include <asm/io.h>
+
+/*  alsa system  */
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <sound/initval.h>
+#include <sound/control.h>
+#include "u8500_alsa_ab8500.h"
+#include <mach/msp.h>
+#include <mach/debug.h>
+
+#define ALSA_NAME		"DRIVER ALSA HDMI"
+
+/* enables/disables debug msgs */
+#define DRIVER_DEBUG	    CONFIG_STM_ALSA_DEBUG
+/* msg header represents this module */
+#define DRIVER_DEBUG_PFX	ALSA_NAME
+/* message level */
+#define DRIVER_DBG	        KERN_ERR
+#define ELEMENT_SIZE 0
+
+extern char *power_state_in_texts[NUMBER_POWER_STATE];
+
+static int u8500_hdmi_power_ctrl_info(struct snd_kcontrol *kcontrol,
+				      struct snd_ctl_elem_info *uinfo);
+static int u8500_hdmi_power_ctrl_get(struct snd_kcontrol *kcontrol,
+				     struct snd_ctl_elem_value *uinfo);
+static int u8500_hdmi_power_ctrl_put(struct snd_kcontrol *kcontrol,
+				     struct snd_ctl_elem_value *uinfo);
+
+void dump_msp2_registers();
+
+#ifdef CONFIG_U8500_ACODEC_DMA
+
+static void u8500_alsa_hdmi_dma_start(audio_stream_t * stream);
+#define stm_trigger_hdmi(x) u8500_alsa_hdmi_dma_start(x)
+static void inline stm_pause_hdmi(audio_stream_t * stream)
+{
+	if (stream->state == ALSA_STATE_UNPAUSE) {
+		stream->state = ALSA_STATE_PAUSE;
+	}
+}
+static void inline stm_unpause_hdmi(audio_stream_t * stream)
+{
+	if (stream->state == ALSA_STATE_PAUSE) {
+		stream->state = ALSA_STATE_UNPAUSE;
+		stm_trigger_hdmi(stream);
+	}
+}
+static void inline stm_stop_hdmi(audio_stream_t * stream)
+{
+	stream->active = 0;
+	stream->period = 0;
+}
+#else /* Polling */
+
+static int spawn_hdmi_feeding_thread(audio_stream_t * stream);
+static int hdmi_feeding_thread(void *data);
+static void u8500_hdmi_pio_start(audio_stream_t * stream);
+
+#define stm_trigger_hdmi(x) spawn_hdmi_feeding_thread(x);
+
+static void inline stm_pause_hdmi(audio_stream_t * stream)
+{
+	stream->state = ALSA_STATE_PAUSE;
+}
+static void inline stm_unpause_hdmi(audio_stream_t * stream)
+{
+	stream->state = ALSA_STATE_UNPAUSE;
+	complete(&stream->alsa_com);
+}
+static void inline stm_stop_hdmi(audio_stream_t * stream)
+{
+	stream->active = 0;
+	stream->period = 0;
+	if (stream->state == ALSA_STATE_PAUSE)
+		complete(&stream->alsa_com);
+}
+
+#endif
+
+extern struct driver_debug_st DBG_ST;
+extern struct i2sdrv_data *i2sdrv[MAX_I2S_CLIENTS];
+
+static void u8500_audio_hdmi_init(u8500_acodec_chip_t * chip);
+int u8500_register_alsa_hdmi_controls(struct snd_card *card,
+				      u8500_acodec_chip_t * u8500_chip);
+static int snd_u8500_alsa_hdmi_open(struct snd_pcm_substream *substream);
+static int snd_u8500_alsa_hdmi_close(struct snd_pcm_substream *substream);
+static int snd_u8500_alsa_hdmi_hw_params(struct snd_pcm_substream *substream,
+					 struct snd_pcm_hw_params *hw_params);
+static int snd_u8500_alsa_hdmi_hw_free(struct snd_pcm_substream *substream);
+static int snd_u8500_alsa_hdmi_prepare(struct snd_pcm_substream *substream);
+static int snd_u8500_alsa_hdmi_trigger(struct snd_pcm_substream *substream,
+				       int cmd);
+static snd_pcm_uframes_t snd_u8500_alsa_hdmi_pointer(struct snd_pcm_substream
+						     *substream);
+static int configure_hdmi_rate(struct snd_pcm_substream *);
+static int configure_msp_hdmi(int sampling_freq, int channel_count);
+
+int u8500_hdmi_rates[] = { 32000, 44100, 48000, 64000, 88200,
+	96000, 128000, 176100, 192000
+};
+
+typedef enum {
+	HDMI_SAMPLING_FREQ_32KHZ = 32,
+	HDMI_SAMPLING_FREQ_44_1KHZ = 44,
+	HDMI_SAMPLING_FREQ_48KHZ = 48,
+	HDMI_SAMPLING_FREQ_64KHZ = 64,
+	HDMI_SAMPLING_FREQ_88_2KHZ = 88,
+	HDMI_SAMPLING_FREQ_96KHZ = 96,
+	HDMI_SAMPLING_FREQ_128KHZ = 128,
+	HDMI_SAMPLING_FREQ_176_1KHZ = 176,
+	HDMI_SAMPLING_FREQ_192KHZ = 192
+} t_hdmi_sample_freq;
+
+static struct snd_pcm_ops snd_u8500_alsa_hdmi_playback_ops = {
+	.open = snd_u8500_alsa_hdmi_open,
+	.close = snd_u8500_alsa_hdmi_close,
+	.ioctl = snd_pcm_lib_ioctl,
+	.hw_params = snd_u8500_alsa_hdmi_hw_params,
+	.hw_free = snd_u8500_alsa_hdmi_hw_free,
+	.prepare = snd_u8500_alsa_hdmi_prepare,
+	.trigger = snd_u8500_alsa_hdmi_trigger,
+	.pointer = snd_u8500_alsa_hdmi_pointer,
+};
+
+static struct snd_pcm_ops snd_u8500_alsa_hdmi_capture_ops = {
+	.open = snd_u8500_alsa_hdmi_open,
+	.close = snd_u8500_alsa_hdmi_close,
+	.ioctl = snd_pcm_lib_ioctl,
+	.hw_params = snd_u8500_alsa_hdmi_hw_params,
+	.hw_free = snd_u8500_alsa_hdmi_hw_free,
+	.prepare = snd_u8500_alsa_hdmi_prepare,
+	.trigger = snd_u8500_alsa_hdmi_trigger,
+	.pointer = snd_u8500_alsa_hdmi_pointer,
+};
+
+/* Hardware description , this structure (struct snd_pcm_hardware )
+ * contains the definitions of the fundamental hardware configuration.
+ * This configuration will be applied on the runtime structure
+ */
+static struct snd_pcm_hardware snd_u8500_hdmi_playback_hw = {
+	.info =
+	    (SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_MMAP |
+	     SNDRV_PCM_INFO_RESUME | SNDRV_PCM_INFO_PAUSE),
+	.formats =
+	    SNDRV_PCM_FMTBIT_S16_LE | SNDRV_PCM_FMTBIT_U16_LE |
+	    SNDRV_PCM_FMTBIT_S16_BE | SNDRV_PCM_FMTBIT_U16_BE,
+	.rates = SNDRV_PCM_RATE_KNOT,
+	.rate_min = MIN_RATE_PLAYBACK,
+	.rate_max = MAX_RATE_PLAYBACK,
+	.channels_min = 1,
+	.channels_max = 2,
+	.buffer_bytes_max = NMDK_BUFFER_SIZE,
+	.period_bytes_min = 128,
+	.period_bytes_max = PAGE_SIZE,
+	.periods_min = NMDK_BUFFER_SIZE / PAGE_SIZE,
+	.periods_max = NMDK_BUFFER_SIZE / 128
+};
+
+static struct snd_pcm_hardware snd_u8500_hdmi_capture_hw = {
+	.info =
+	    (SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_MMAP |
+	     SNDRV_PCM_INFO_RESUME | SNDRV_PCM_INFO_PAUSE),
+	.formats =
+	    SNDRV_PCM_FMTBIT_S16_LE | SNDRV_PCM_FMTBIT_U16_LE |
+	    SNDRV_PCM_FMTBIT_S16_BE | SNDRV_PCM_FMTBIT_U16_BE,
+	.rates = SNDRV_PCM_RATE_KNOT,
+	.rate_min = MIN_RATE_CAPTURE,
+	.rate_max = MAX_RATE_CAPTURE,
+	.channels_min = 1,
+	.channels_max = 2,
+	.buffer_bytes_max = NMDK_BUFFER_SIZE,
+	.period_bytes_min = 128,
+	.period_bytes_max = PAGE_SIZE,
+	.periods_min = NMDK_BUFFER_SIZE / PAGE_SIZE,
+	.periods_max = NMDK_BUFFER_SIZE / 128
+};
+
+static struct snd_pcm_hw_constraint_list constraints_hdmi_rate = {
+	.count = sizeof(u8500_hdmi_rates) / sizeof(u8500_hdmi_rates[0]),
+	.list = u8500_hdmi_rates,
+	.mask = 0,
+};
+
+/**
+ * snd_card_u8500_alsa_hdmi_new - constructor for a new pcm cmponent
+ * @chip - pointer to chip specific data
+ * @device - specifies the card number
+ */
+int snd_card_u8500_alsa_hdmi_new(u8500_acodec_chip_t * chip, int device)
+{
+	struct snd_pcm *pcm;
+	int err;
+
+	if ((err =
+	     snd_pcm_new(chip->card, "u8500_hdmi", device, 1, 1, &pcm)) < 0) {
+		stm_error(" : error in snd_pcm_new\n");
+		return err;
+	}
+
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_PLAYBACK,
+			&snd_u8500_alsa_hdmi_playback_ops);
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE,
+			&snd_u8500_alsa_hdmi_capture_ops);
+
+	pcm->private_data = chip;
+	pcm->info_flags = 0;
+	chip->pcm_hdmi = pcm;
+	strcpy(pcm->name, "u8500_alsa_hdmi");
+
+	u8500_audio_hdmi_init(pcm->private_data);
+	return 0;
+}
+
+/**
+* u8500_audio_hdmi_init
+* @chip - pointer to u8500_acodec_chip_t structure.
+*
+* This function intialises the u8500 chip structure with default values
+*/
+static void u8500_audio_hdmi_init(u8500_acodec_chip_t * chip)
+{
+	audio_stream_t *ptr_audio_stream = NULL;
+
+	ptr_audio_stream =
+	    &chip->stream[ALSA_HDMI_DEV][SNDRV_PCM_STREAM_PLAYBACK];
+	/* Setup DMA stuff */
+
+	strlcpy(ptr_audio_stream->id, "u8500 hdmi playback",
+		sizeof(ptr_audio_stream->id));
+
+	ptr_audio_stream->stream_id = SNDRV_PCM_STREAM_PLAYBACK;
+
+	/* default initialization  for playback */
+	ptr_audio_stream->active = 0;
+	ptr_audio_stream->period = 0;
+	ptr_audio_stream->periods = 0;
+	ptr_audio_stream->old_offset = 0;
+
+	ptr_audio_stream =
+	    &chip->stream[ALSA_HDMI_DEV][SNDRV_PCM_STREAM_CAPTURE];
+
+	strlcpy(ptr_audio_stream->id, "u8500 hdmi capture",
+		sizeof(ptr_audio_stream->id));
+
+	ptr_audio_stream->stream_id = SNDRV_PCM_STREAM_CAPTURE;
+
+	/* default initialization  for capture */
+	ptr_audio_stream->active = 0;
+	ptr_audio_stream->period = 0;
+	ptr_audio_stream->periods = 0;
+	ptr_audio_stream->old_offset = 0;
+
+}
+
+/**
+ * snd_u8500_alsa_hdmi_open
+ * @substream - pointer to the playback/capture substream structure
+ *
+ *  This routine is used by alsa framework to open a pcm stream .
+ *  Here a dma pipe is requested and device is configured(default).
+ */
+static int snd_u8500_alsa_hdmi_open(struct snd_pcm_substream *substream)
+{
+	int error = 0, stream_id, status = 0;
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	audio_stream_t *ptr_audio_stream = NULL;
+
+	stream_id = substream->pstr->stream;
+	error = u8500_acodec_setuser(USER_ALSA);
+	status = u8500_acodec_open(I2S_CLIENT_MSP2, stream_id);
+	if (status) {
+		printk("failed in getting open\n");
+		return (-1);
+	}
+
+	error = snd_pcm_hw_constraint_list(runtime, 0, SNDRV_PCM_HW_PARAM_RATE,
+					   &constraints_hdmi_rate);
+	if (error < 0) {
+		stm_error
+		    (": error initializing hdmi hw sample rate constraint\n");
+		return error;
+	}
+
+	if ((error = configure_hdmi_rate(substream)))
+		return error;
+
+	if (stream_id == SNDRV_PCM_STREAM_PLAYBACK) {
+		runtime->hw = snd_u8500_hdmi_playback_hw;
+	} else {
+		runtime->hw = snd_u8500_hdmi_capture_hw;
+	}
+
+	ptr_audio_stream = &chip->stream[ALSA_HDMI_DEV][stream_id];
+
+	ptr_audio_stream->substream = substream;
+
+	stm_config_hw(chip, substream, ALSA_HDMI_DEV, stream_id);
+	sema_init(&(ptr_audio_stream->alsa_sem), 1);
+	init_completion(&(ptr_audio_stream->alsa_com));
+
+	ptr_audio_stream->state = ALSA_STATE_UNPAUSE;
+	return 0;
+}
+
+/**
+ * snd_u8500_alsa_hdmi_close
+ * @substream - pointer to the playback/capture substream structure
+ *
+ *  This routine is used by alsa framework to close a pcm stream .
+ *  Here a dma pipe is disabled and freed.
+ */
+
+static int snd_u8500_alsa_hdmi_close(struct snd_pcm_substream *substream)
+{
+	int stream_id, error = 0;
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	audio_stream_t *ptr_audio_stream = NULL;
+
+	stream_id = substream->pstr->stream;
+	ptr_audio_stream = &chip->stream[ALSA_HDMI_DEV][stream_id];
+
+	stm_close_alsa(chip, ALSA_HDMI_DEV, stream_id);
+
+	/* reset the different variables to default */
+
+	ptr_audio_stream->active = 0;
+	ptr_audio_stream->period = 0;
+	ptr_audio_stream->periods = 0;
+	ptr_audio_stream->old_offset = 0;
+	ptr_audio_stream->substream = NULL;
+
+	/* Disable the MSP2 */
+	error = u8500_acodec_unsetuser(USER_ALSA);
+	u8500_acodec_close(I2S_CLIENT_MSP2, ACODEC_DISABLE_ALL);
+
+	return error;
+
+}
+
+/**
+ * snd_u8500_alsa_hdmi_hw_params
+ * @substream - pointer to the playback/capture substream structure
+ * @hw_params - specifies the hw parameters like format/no of channels etc
+ *
+ *  This routine is used by alsa framework to allocate a dma buffer
+ *  used to transfer the data from user space to kernel space
+ *
+ */
+static int snd_u8500_alsa_hdmi_hw_params(struct snd_pcm_substream *substream,
+					 struct snd_pcm_hw_params *hw_params)
+{
+	return snd_pcm_lib_malloc_pages(substream,
+					params_buffer_bytes(hw_params));
+}
+
+/**
+ * snd_u8500_alsa_hdmi_hw_free
+ * @substream - pointer to the playback/capture substream structure
+ *
+ *  This routine is used by alsa framework to deallocate a dma buffer
+ *  allocated berfore by snd_u8500_alsa_pcm_hw_params
+ */
+static int snd_u8500_alsa_hdmi_hw_free(struct snd_pcm_substream *substream)
+{
+	stm_hw_free(substream);
+	return 0;
+}
+
+/**
+ * snd_u8500_alsa_hdmi_pointer
+ * @substream - pointer to the playback/capture substream structure
+ *
+ *  This callback is called whene the pcm middle layer inquires the current
+ *  hardware position on the buffer .The position is returned in frames
+ *  ranged from 0 to buffer_size -1
+ */
+static snd_pcm_uframes_t snd_u8500_alsa_hdmi_pointer(struct snd_pcm_substream
+						     *substream)
+{
+	unsigned int offset;
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	audio_stream_t *stream =
+	    &chip->stream[ALSA_HDMI_DEV][substream->pstr->stream];
+	struct snd_pcm_runtime *runtime = stream->substream->runtime;
+
+	offset = bytes_to_frames(runtime, stream->old_offset);
+	if (offset < 0 || stream->old_offset < 0)
+		stm_dbg(DBG_ST.alsa, " Offset=%i %i\n", offset,
+			stream->old_offset);
+
+	return offset;
+}
+
+/**
+ * snd_u8500_alsa_hdmi_prepare
+ * @substream - pointer to the playback/capture substream structure
+ *
+ *  This callback is called whene the pcm is "prepared" Here is possible
+ *  to set the format type ,sample rate ,etc.The callback is called as
+ *  well everytime a recovery after an underrun happens.
+ */
+
+static int snd_u8500_alsa_hdmi_prepare(struct snd_pcm_substream *substream)
+{
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	int error;
+
+	if (chip->hdmi_params.sampling_freq != runtime->rate
+	    || chip->hdmi_params.channel_count != runtime->channels) {
+		stm_dbg(DBG_ST.alsa, " freq not same, %d %d\n",
+			chip->hdmi_params.sampling_freq, runtime->rate);
+		stm_dbg(DBG_ST.alsa, " channels not same, %d %d\n",
+			chip->hdmi_params.channel_count, runtime->channels);
+		if (chip->hdmi_params.channel_count != runtime->channels) {
+			chip->hdmi_params.channel_count = runtime->channels;
+			if ((error =
+			     stm_config_hw(chip, substream, ALSA_HDMI_DEV,
+					   -1))) {
+				stm_dbg(DBG_ST.alsa,
+					"In func %s, stm_config_hw fails",
+					__FUNCTION__);
+				return error;
+			}
+		}
+		chip->hdmi_params.sampling_freq = runtime->rate;
+		if ((error = configure_hdmi_rate(substream))) {
+			stm_dbg(DBG_ST.alsa, "In func %s, configure_rate fails",
+				__FUNCTION__);
+			return error;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * snd_u8500_alsa_hdmi_trigger
+ * @substream -  pointer to the playback/capture substream structure
+ * @cmd - specifies the command : start/stop/pause/resume
+ *
+ *  This callback is called whene the pcm is started ,stopped or paused
+ *  The action is specified in the second argument, SND_PCM_TRIGGER_XXX in
+ *  <sound/pcm.h>.
+ *  This callback is atomic and the interrupts are disabled , so you can't
+ *  call other functions that need interrupts without possible risks
+ */
+static int snd_u8500_alsa_hdmi_trigger(struct snd_pcm_substream *substream,
+				       int cmd)
+{
+	int stream_id = substream->pstr->stream;
+	audio_stream_t *stream = NULL;
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+	int error = 0;
+
+	stream = &chip->stream[ALSA_HDMI_DEV][stream_id];
+
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_START:
+		/* Start the pcm engine */
+		stm_dbg(DBG_ST.alsa, " TRIGGER START\n");
+		if (stream->active == 0) {
+			stream->active = 1;
+			stm_trigger_hdmi(stream);
+			break;
+		}
+		stm_error(": H/w is busy\n");
+		return -EINVAL;
+
+	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
+		stm_dbg(DBG_ST.alsa, " SNDRV_PCM_TRIGGER_PAUSE_PUSH\n");
+		if (stream->active == 1) {
+			stm_pause_hdmi(stream);
+		}
+		break;
+	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
+		stm_dbg(DBG_ST.alsa, " SNDRV_PCM_TRIGGER_PAUSE_RELEASE\n");
+		if (stream->active == 1)
+			stm_unpause_hdmi(stream);
+		break;
+	case SNDRV_PCM_TRIGGER_STOP:
+		/* Stop the pcm engine */
+		stm_dbg(DBG_ST.alsa, " TRIGGER STOP\n");
+		if (stream->active == 1)
+			stm_stop_hdmi(stream);
+		break;
+	default:
+		stm_error(": invalid command in pcm trigger\n");
+		return -EINVAL;
+	}
+
+	return error;
+
+}
+
+struct snd_kcontrol_new u8500_hdmi_power_ctrl = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.device = 1,
+	.subdevice = 0,
+	.name = "HDMI Power",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.private_value = 0xfff,
+	.info = u8500_hdmi_power_ctrl_info,
+	.get = u8500_hdmi_power_ctrl_get,
+	.put = u8500_hdmi_power_ctrl_put
+};
+
+/**
+* u8500_hdmi_power_ctrl_info
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions fills playback device info into user structure.
+*/
+static int u8500_hdmi_power_ctrl_info(struct snd_kcontrol *kcontrol,
+				      struct snd_ctl_elem_info *uinfo)
+{
+
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->value.enumerated.items = NUMBER_POWER_STATE;
+	uinfo->count = 1;
+	if (uinfo->value.enumerated.item >= NUMBER_POWER_STATE)
+		uinfo->value.enumerated.item = NUMBER_POWER_STATE - 1;
+	strcpy(uinfo->value.enumerated.name,
+	       power_state_in_texts[uinfo->value.enumerated.item]);
+	return 0;
+}
+
+/**
+* u8500_hdmi_power_ctrl_get
+* @kcontrol - pointer to the snd_kcontrol structure
+* @uinfo - pointer to the snd_ctl_elem_info structure, this is filled by the function
+*
+*	This functions returns the current playback device selected.
+*/
+static int u8500_hdmi_power_ctrl_get(struct snd_kcontrol *kcontrol,
+				     struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	uinfo->value.enumerated.item[0] = 0;
+	return 0;
+}
+
+/**
+* u8500_hdmi_power_ctrl_put
+* @kcontrol - pointer to the snd_kcontrol structure
+* @	.
+*
+*	This functions sets the playback device.
+*/
+static int u8500_hdmi_power_ctrl_put(struct snd_kcontrol *kcontrol,
+				     struct snd_ctl_elem_value *uinfo)
+{
+	u8500_acodec_chip_t *chip =
+	    (u8500_acodec_chip_t *) snd_kcontrol_chip(kcontrol);
+	int changed = 0;
+	t_ab8500_codec_error error;
+	t_u8500_bool_state power_state;
+
+	power_state = uinfo->value.enumerated.item[0];
+
+	changed = 1;
+
+	return changed;
+}
+
+int u8500_register_alsa_hdmi_controls(struct snd_card *card,
+				      u8500_acodec_chip_t * u8500_chip)
+{
+	int error;
+
+	if ((error =
+	     snd_ctl_add(card,
+			 snd_ctl_new1(&u8500_hdmi_power_ctrl,
+				      u8500_chip))) < 0) {
+		stm_error
+		    (": error initializing u8500_hdmi_power_ctrl interface \n\n");
+		return (-1);
+	}
+
+	return 0;
+}
+
+/**
+* configure_hdmi_rate
+* @substream - pointer to the playback/capture substream structure
+*
+*	This functions configures audio codec in to stream frequency frequency
+*/
+static int configure_hdmi_rate(struct snd_pcm_substream *substream)
+{
+	t_hdmi_sample_freq hdmi_sampling_freq;
+
+	u8500_acodec_chip_t *chip = snd_pcm_substream_chip(substream);
+
+	switch (chip->hdmi_params.sampling_freq) {
+	case 32000:
+		hdmi_sampling_freq = HDMI_SAMPLING_FREQ_32KHZ;
+		break;
+	case 44100:
+		hdmi_sampling_freq = HDMI_SAMPLING_FREQ_44_1KHZ;
+		break;
+	case 48000:
+		hdmi_sampling_freq = HDMI_SAMPLING_FREQ_48KHZ;
+		break;
+	case 64000:
+		hdmi_sampling_freq = HDMI_SAMPLING_FREQ_64KHZ;
+		break;
+	case 88200:
+		hdmi_sampling_freq = HDMI_SAMPLING_FREQ_88_2KHZ;
+		break;
+	case 96000:
+		hdmi_sampling_freq = HDMI_SAMPLING_FREQ_96KHZ;
+		break;
+	case 128000:
+		hdmi_sampling_freq = HDMI_SAMPLING_FREQ_128KHZ;
+		break;
+	case 176100:
+		hdmi_sampling_freq = HDMI_SAMPLING_FREQ_176_1KHZ;
+		break;
+	case 192000:
+		hdmi_sampling_freq = HDMI_SAMPLING_FREQ_192KHZ;
+	default:
+		stm_error("not supported frequnecy\n");
+		return -EINVAL;
+	}
+
+	configure_msp_hdmi(hdmi_sampling_freq, chip->hdmi_params.channel_count);
+
+	return 0;
+
+}
+
+static int configure_msp_hdmi(int sampling_freq, int channel_count)
+{
+	struct i2s_device *i2s_dev = i2sdrv[I2S_CLIENT_MSP2]->i2s;
+	struct msp_config msp_config;
+	t_ab8500_codec_error error_status = AB8500_CODEC_OK;
+
+	memset(&msp_config, 0, sizeof(msp_config));
+
+
+	if (i2sdrv[I2S_CLIENT_MSP2]->flag) {
+		stm_dbg(DBG_ST.acodec, " I2S controller not available\n");
+		return -1;
+	}
+
+	/* MSP configuration  */
+
+	msp_config.tx_clock_sel = 0;
+	msp_config.rx_clock_sel = 0;
+
+	msp_config.tx_frame_sync_sel = 0;
+	msp_config.rx_frame_sync_sel = 0;
+
+	msp_config.input_clock_freq = MSP_INPUT_FREQ_48MHZ;
+	msp_config.srg_clock_sel = 0;
+
+	msp_config.rx_frame_sync_pol = RX_FIFO_SYNC_HI;
+	msp_config.tx_frame_sync_pol = TX_FIFO_SYNC_HI;
+
+	msp_config.rx_fifo_config = 0;
+	msp_config.tx_fifo_config = TX_FIFO_ENABLE;
+
+	msp_config.spi_clk_mode = SPI_CLK_MODE_NORMAL;
+	msp_config.spi_burst_mode = 0;
+	msp_config.tx_data_enable = 0;
+	msp_config.loopback_enable = 0;
+	msp_config.default_protocol_desc = 1;
+	msp_config.direction = MSP_TRANSMIT_MODE;
+	msp_config.protocol = MSP_I2S_PROTOCOL;
+	msp_config.frame_size = ELEMENT_SIZE;
+	msp_config.frame_freq = sampling_freq;
+	msp_config.def_elem_len = 0;
+	/* enable msp for both tr and rx mode with dma data transfer.
+	   THIS IS NOW DONE SEPARATELY from SAA. */
+	msp_config.data_size = MSP_DATA_SIZE_16BIT;
+
+#ifdef CONFIG_U8500_ACODEC_DMA
+	msp_config.work_mode = MSP_DMA_MODE;
+#elif defined(CONFIG_U8500_ACODEC_POLL)
+	msp_config.work_mode = MSP_POLLING_MODE;
+#else
+	msp_config.work_mode = MSP_INTERRUPT_MODE;
+#endif
+	msp_config.default_protocol_desc = 0;
+
+	msp_config.protocol_desc.rx_phase_mode = MSP_DUAL_PHASE;
+	msp_config.protocol_desc.tx_phase_mode = MSP_DUAL_PHASE;
+	msp_config.protocol_desc.rx_phase2_start_mode =
+	    MSP_PHASE2_START_MODE_FRAME_SYNC;
+	msp_config.protocol_desc.tx_phase2_start_mode =
+	    MSP_PHASE2_START_MODE_FRAME_SYNC;
+	msp_config.protocol_desc.rx_bit_transfer_format = MSP_BTF_MS_BIT_FIRST;
+	msp_config.protocol_desc.tx_bit_transfer_format = MSP_BTF_MS_BIT_FIRST;
+	msp_config.protocol_desc.rx_frame_length_1 = MSP_FRAME_LENGTH_1;
+	msp_config.protocol_desc.rx_frame_length_2 = MSP_FRAME_LENGTH_1;
+	msp_config.protocol_desc.tx_frame_length_1 = MSP_FRAME_LENGTH_1;
+	msp_config.protocol_desc.tx_frame_length_2 = MSP_FRAME_LENGTH_1;
+	msp_config.protocol_desc.rx_element_length_1 = MSP_ELEM_LENGTH_16;
+	msp_config.protocol_desc.rx_element_length_2 = MSP_ELEM_LENGTH_16;
+	msp_config.protocol_desc.tx_element_length_1 = MSP_ELEM_LENGTH_16;
+	msp_config.protocol_desc.tx_element_length_2 = MSP_ELEM_LENGTH_16;
+	msp_config.protocol_desc.rx_data_delay = MSP_DELAY_1;
+	msp_config.protocol_desc.tx_data_delay = MSP_DELAY_1;
+	msp_config.protocol_desc.rx_clock_pol = MSP_RISING_EDGE;
+	msp_config.protocol_desc.tx_clock_pol = 0;
+	msp_config.protocol_desc.rx_frame_sync_pol =
+	    MSP_FRAME_SYNC_POL_ACTIVE_HIGH;
+	msp_config.protocol_desc.tx_frame_sync_pol =
+	    MSP_FRAME_SYNC_POL_ACTIVE_HIGH;
+	msp_config.protocol_desc.rx_half_word_swap = MSP_HWS_NO_SWAP;
+	msp_config.protocol_desc.tx_half_word_swap = MSP_HWS_NO_SWAP;
+	msp_config.protocol_desc.compression_mode = MSP_COMPRESS_MODE_LINEAR;
+	msp_config.protocol_desc.expansion_mode = MSP_EXPAND_MODE_LINEAR;
+	msp_config.protocol_desc.spi_clk_mode = MSP_SPI_CLOCK_MODE_NON_SPI;
+	msp_config.protocol_desc.spi_burst_mode = MSP_SPI_BURST_MODE_DISABLE;
+	msp_config.protocol_desc.frame_period = 63;
+	msp_config.protocol_desc.frame_width = 31;
+	msp_config.protocol_desc.total_clocks_for_one_frame = 64;
+	msp_config.multichannel_configured = 0;
+	msp_config.multichannel_config.tx_multichannel_enable = 0;
+	/* Channel 1 and channel 3 */
+	msp_config.multichannel_config.tx_channel_0_enable = 0x0000005;
+	msp_config.multichannel_config.tx_channel_1_enable = 0x0000000;
+	msp_config.multichannel_config.tx_channel_2_enable = 0x0000000;
+	msp_config.multichannel_config.tx_channel_3_enable = 0x0000000;
+	error_status = i2s_setup(i2s_dev->controller, &msp_config);
+
+#ifdef CONFIG_DEBUG
+	{
+		dump_msp2_registers();
+	}
+#endif
+
+	if (error_status < 0) {
+		printk("error in msp enable, error_status is %d\n",
+		       error_status);
+		return error_status;
+	}
+
+	return 0;
+
+}
+
+#ifdef CONFIG_U8500_ACODEC_DMA
+/**
+ * u8500_alsa_hdmi_dma_start - used to transmit or recive a dma chunk
+ * @stream - specifies the playback/record stream structure
+ */
+static void u8500_alsa_hdmi_dma_start(audio_stream_t * stream)
+{
+	unsigned int offset, dma_size, stream_id;
+
+	struct snd_pcm_substream *substream = stream->substream;
+	struct snd_pcm_runtime *runtime = substream->runtime;
+
+	stream_id = substream->pstr->stream;
+
+	dma_size = frames_to_bytes(runtime, runtime->period_size);
+	offset = dma_size * stream->period;
+	stream->old_offset = offset;
+
+	if (stream_id == SNDRV_PCM_STREAM_PLAYBACK)
+#ifdef CONFIG_U8500_ACODEC_DMA
+		u8500_acodec_send_data(I2S_CLIENT_MSP2,
+				       (void *)(runtime->dma_addr + offset),
+				       dma_size, 1);
+#else
+		u8500_acodec_send_data(I2S_CLIENT_MSP2,
+				       (void *)(runtime->dma_area + offset),
+				       dma_size, 0);
+#endif
+	else
+#ifdef CONFIG_U8500_ACODEC_DMA
+		u8500_acodec_receive_data(I2S_CLIENT_MSP2,
+					  (void *)(runtime->dma_addr + offset),
+					  dma_size, 1);
+#else
+		u8500_acodec_receive_data(I2S_CLIENT_MSP2,
+					  (void *)(runtime->dma_area + offset),
+					  dma_size, 0);
+#endif
+
+	stm_dbg(DBG_ST.alsa, " DMA Transfer started\n");
+	stm_dbg(DBG_ST.alsa, " address = %x  size=%d\n",
+		(runtime->dma_addr + offset), dma_size);
+
+	stream->period++;
+	stream->period %= runtime->periods;
+	stream->periods++;
+
+
+}
+
+#else
+
+/**
+* acodec_feeding_thread
+* @stream - pointer to the playback/capture audio_stream_t structure
+*
+*	This function creates a kernel thread .
+*/
+
+static int spawn_hdmi_feeding_thread(audio_stream_t * stream)
+{
+	pid_t pid;
+
+	pid =
+	    kernel_thread(hdmi_feeding_thread, stream,
+			  CLONE_FS | CLONE_SIGHAND);
+
+	return 0;
+}
+
+/**
+* hdmi_feeding_thread
+* @data - void pointer to the playback/capture audio_stream_t structure
+*
+*	This thread sends/receive data to MSP while stream is active
+*/
+static int hdmi_feeding_thread(void *data)
+{
+	audio_stream_t *stream = (audio_stream_t *) data;
+
+	daemonize("hdmi_feeding_thread");
+	allow_signal(SIGKILL);
+	down(&stream->alsa_sem);
+
+	while ((!signal_pending(current)) && (stream->active)) {
+		if (stream->state == ALSA_STATE_PAUSE)
+			wait_for_completion(&(stream->alsa_com));
+
+		u8500_hdmi_pio_start(stream);
+		if (stream->substream)
+			snd_pcm_period_elapsed(stream->substream);
+	}
+
+	up(&stream->alsa_sem);
+
+	return 0;
+}
+
+/**
+* u8500_hdmi_pio_start
+* @stream - pointer to the playback/capture audio_stream_t structure
+*
+*  This function sends/receive one chunck of stream data to/from MSP
+*/
+static void u8500_hdmi_pio_start(audio_stream_t * stream)
+{
+	unsigned int offset, dma_size, stream_id;
+	struct snd_pcm_substream *substream = stream->substream;
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	stream_id = substream->pstr->stream;
+
+	dma_size = frames_to_bytes(runtime, runtime->period_size);
+	offset = dma_size * stream->period;
+	stream->old_offset = offset;
+
+	stm_dbg(DBG_ST.alsa, " Transfer started\n");
+	stm_dbg(DBG_ST.alsa, " address = %x  size=%d\n",
+		(runtime->dma_addr + offset), dma_size);
+
+	/* Send our stuff */
+	if (stream_id == SNDRV_PCM_STREAM_PLAYBACK)
+#ifdef CONFIG_U8500_ACODEC_DMA
+		u8500_acodec_send_data(I2S_CLIENT_MSP2,
+				       (void *)(runtime->dma_addr + offset),
+				       dma_size, 1);
+#else
+		u8500_acodec_send_data(I2S_CLIENT_MSP2,
+				       (void *)(runtime->dma_area + offset),
+				       dma_size, 0);
+#endif
+	else
+#ifdef CONFIG_U8500_ACODEC_DMA
+		u8500_acodec_receive_data(I2S_CLIENT_MSP2,
+					  (void *)(runtime->dma_addr + offset),
+					  dma_size, 1);
+#else
+		u8500_acodec_receive_data(I2S_CLIENT_MSP2,
+					  (void *)(runtime->dma_area + offset),
+					  dma_size, 0);
+#endif
+
+	stream->period++;
+	stream->period %= runtime->periods;
+	stream->periods++;
+}
+#endif
+
+void dump_msp2_registers()
+{
+	int i;
+
+	stm_dbg(DBG_ST.acodec, "\nMSP_2 base add = 0x%x\n",
+		(unsigned int)U8500_MSP2_BASE);
+
+	for (i = 0; i < 0x40; i += 4)
+		stm_dbg(DBG_ST.acodec, "msp[0x%x]=0x%x\n", i,
+			readl((char *)(IO_ADDRESS(U8500_MSP2_BASE) + i)));
+
+	return 0;
+}
diff --git a/sound/pci/hda/patch_realtek.c b/sound/pci/hda/patch_realtek.c
index 524ff26417e..7c0f57a84cf 100644
--- a/sound/pci/hda/patch_realtek.c
+++ b/sound/pci/hda/patch_realtek.c
@@ -19494,6 +19494,10 @@ static const struct alc_fixup alc662_fixups[] = {
 		.type = ALC_FIXUP_SKU,
 		.v.sku = ALC_FIXUP_SKU_IGNORE,
 	},
+	[ALC662_FIXUP_SKU_IGNORE] = {
+		.type = ALC_FIXUP_SKU,
+		.v.sku = ALC_FIXUP_SKU_IGNORE,
+	},
 };
 
 static const struct snd_pci_quirk alc662_fixup_tbl[] = {
diff --git a/sound/soc/Kconfig b/sound/soc/Kconfig
index 8224db5f043..adcbc43276d 100644
--- a/sound/soc/Kconfig
+++ b/sound/soc/Kconfig
@@ -11,7 +11,7 @@ menuconfig SND_SOC
 
 	  If you want ASoC support, you should say Y here and also to the
 	  specific driver for your SoC platform below.
-	  
+
 	  ASoC provides power efficient ALSA support for embedded battery powered
 	  SoC based systems like PDA's, Phones and Personal Media Players.
 
@@ -57,6 +57,7 @@ source "sound/soc/s6000/Kconfig"
 source "sound/soc/sh/Kconfig"
 source "sound/soc/tegra/Kconfig"
 source "sound/soc/txx9/Kconfig"
+source "sound/soc/ux500/Kconfig"
 
 # Supported codecs
 source "sound/soc/codecs/Kconfig"
diff --git a/sound/soc/Makefile b/sound/soc/Makefile
index 1ed61c5df2c..79a9e203e94 100644
--- a/sound/soc/Makefile
+++ b/sound/soc/Makefile
@@ -20,3 +20,4 @@ obj-$(CONFIG_SND_SOC)	+= s6000/
 obj-$(CONFIG_SND_SOC)	+= sh/
 obj-$(CONFIG_SND_SOC)	+= tegra/
 obj-$(CONFIG_SND_SOC)	+= txx9/
+obj-$(CONFIG_SND_SOC)	+= ux500/
diff --git a/sound/soc/codecs/Kconfig b/sound/soc/codecs/Kconfig
index 98175a096df..4ac475d51bd 100644
--- a/sound/soc/codecs/Kconfig
+++ b/sound/soc/codecs/Kconfig
@@ -96,7 +96,12 @@ config SND_SOC_ALL_CODECS
 	select SND_SOC_WM9705 if SND_SOC_AC97_BUS
 	select SND_SOC_WM9712 if SND_SOC_AC97_BUS
 	select SND_SOC_WM9713 if SND_SOC_AC97_BUS
-        help
+	select SND_SOC_AB3550
+	select SND_SOC_AB5500
+	select SND_SOC_AB8500
+	select SND_SOC_CG29XX
+	select SND_SOC_AV8100
+	help
           Normally ASoC codec drivers are only built if a machine driver which
           uses them is also built since they are only usable with a machine
           driver.  Selecting this option will allow these drivers to be built
@@ -371,6 +376,21 @@ config SND_SOC_WM9712
 config SND_SOC_WM9713
 	tristate
 
+config SND_SOC_AB3550
+	tristate
+
+config SND_SOC_AB5500
+	tristate
+
+config SND_SOC_AB8500
+	tristate
+
+config SND_SOC_CG29XX
+	tristate
+
+config SND_SOC_AV8100
+	tristate
+
 # Amp
 config SND_SOC_LM4857
 	tristate
diff --git a/sound/soc/codecs/Makefile b/sound/soc/codecs/Makefile
index fd8558406ef..a8e4d5c6cde 100644
--- a/sound/soc/codecs/Makefile
+++ b/sound/soc/codecs/Makefile
@@ -1,4 +1,7 @@
 snd-soc-88pm860x-objs := 88pm860x-codec.o
+snd-soc-ab3550-objs := ab3550.o
+snd-soc-ab5500-objs := ab5500.o
+snd-soc-ab8500_audio-objs := ab8500_audio.o
 snd-soc-ac97-objs := ac97.o
 snd-soc-ad1836-objs := ad1836.o
 snd-soc-ad193x-objs := ad193x.o
@@ -10,6 +13,8 @@ snd-soc-ak4535-objs := ak4535.o
 snd-soc-ak4641-objs := ak4641.o
 snd-soc-ak4642-objs := ak4642.o
 snd-soc-ak4671-objs := ak4671.o
+snd-soc-av8100_audio-objs := av8100_audio.o
+snd-soc-cg29xx-objs := cg29xx.o
 snd-soc-cq93vc-objs := cq93vc.o
 snd-soc-cs42l51-objs := cs42l51.o
 snd-soc-cs4270-objs := cs4270.o
@@ -89,6 +94,9 @@ snd-soc-tpa6130a2-objs := tpa6130a2.o
 snd-soc-wm2000-objs := wm2000.o
 snd-soc-wm9090-objs := wm9090.o
 
+obj-$(CONFIG_SND_SOC_AB3550)    += snd-soc-ab3550.o
+obj-$(CONFIG_SND_SOC_AB5500)    += snd-soc-ab5500.o
+obj-$(CONFIG_SND_SOC_AB8500)    += snd-soc-ab8500_audio.o
 obj-$(CONFIG_SND_SOC_88PM860X)	+= snd-soc-88pm860x.o
 obj-$(CONFIG_SND_SOC_AC97_CODEC)	+= snd-soc-ac97.o
 obj-$(CONFIG_SND_SOC_AD1836)	+= snd-soc-ad1836.o
@@ -102,6 +110,8 @@ obj-$(CONFIG_SND_SOC_AK4641)	+= snd-soc-ak4641.o
 obj-$(CONFIG_SND_SOC_AK4642)	+= snd-soc-ak4642.o
 obj-$(CONFIG_SND_SOC_AK4671)	+= snd-soc-ak4671.o
 obj-$(CONFIG_SND_SOC_ALC5623)    += snd-soc-alc5623.o
+obj-$(CONFIG_SND_SOC_AV8100)	+= snd-soc-av8100_audio.o
+obj-$(CONFIG_SND_SOC_CG29XX)	+= snd-soc-cg29xx.o
 obj-$(CONFIG_SND_SOC_CQ0093VC) += snd-soc-cq93vc.o
 obj-$(CONFIG_SND_SOC_CS42L51)	+= snd-soc-cs42l51.o
 obj-$(CONFIG_SND_SOC_CS4270)	+= snd-soc-cs4270.o
@@ -178,4 +188,9 @@ obj-$(CONFIG_SND_SOC_LM4857)	+= snd-soc-lm4857.o
 obj-$(CONFIG_SND_SOC_MAX9877)	+= snd-soc-max9877.o
 obj-$(CONFIG_SND_SOC_TPA6130A2)	+= snd-soc-tpa6130a2.o
 obj-$(CONFIG_SND_SOC_WM2000)	+= snd-soc-wm2000.o
-obj-$(CONFIG_SND_SOC_WM9090)	+= snd-soc-wm9090.o
+ifdef CONFIG_SND_SOC_UX500_DEBUG
+CFLAGS_av8100_audio.o := -DDEBUG
+CFLAGS_ab3550.o := -DDEBUG
+CFLAGS_cg29xx.o := -DDEBUG
+CFLAGS_ab8500_audio.o := -DDEBUG
+endif
diff --git a/sound/soc/codecs/ab3550.c b/sound/soc/codecs/ab3550.c
new file mode 100644
index 00000000000..4a15ab64d32
--- /dev/null
+++ b/sound/soc/codecs/ab3550.c
@@ -0,0 +1,1429 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Xie Xiaolei <xie.xiaolei@etericsson.com>,
+ *         Roger Nilsson <roger.xr.nilsson@stericsson.com>,
+ *         Ola Lilja <ola.o.lilja@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/pm.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <sound/initval.h>
+#include <sound/soc.h>
+#include <sound/soc-dapm.h>
+#include <linux/mfd/abx500.h>
+#include <linux/bitmap.h>
+#include <linux/bitops.h>
+#include <asm/atomic.h>
+#include <linux/rwsem.h>
+#include <linux/mutex.h>
+#include <stdarg.h>
+#include "ab3550.h"
+
+
+#define I2C_BANK 0
+
+/* codec private data */
+struct ab3550_codec_dai_data {
+};
+
+static struct device *ab3550_dev;
+
+static u8 virtual_regs[] = {
+	0, 0
+};
+
+static void set_reg(u8 reg, u8 val)
+{
+	if (!ab3550_dev) {
+		pr_err("%s: The AB3550 codec driver not initialized.\n",
+		       __func__);
+		return;
+	}
+	if (reg < AB3550_FIRST_REG)
+		return;
+	else if (reg <= AB3550_LAST_REG) {
+		abx500_set_register_interruptible(
+			ab3550_dev, I2C_BANK, reg, val);
+	} else if (reg - AB3550_LAST_REG - 1 < ARRAY_SIZE(virtual_regs)) {
+		virtual_regs[reg - AB3550_LAST_REG - 1] = val;
+	}
+}
+
+static void mask_set_reg(u8 reg, u8 mask, u8 val)
+{
+	if (!ab3550_dev) {
+		pr_err("%s: The AB3550 codec driver not initialized.\n",
+		       __func__);
+		return;
+	}
+	if (reg < AB3550_FIRST_REG)
+		return;
+	else if (reg <= AB3550_LAST_REG) {
+		abx500_mask_and_set_register_interruptible(
+			ab3550_dev, I2C_BANK, reg, mask, val);
+	} else if (reg - AB3550_LAST_REG - 1 < ARRAY_SIZE(virtual_regs)) {
+		virtual_regs[reg - AB3550_LAST_REG - 1] &= ~mask;
+		virtual_regs[reg - AB3550_LAST_REG - 1] |= val & mask;
+	}
+}
+
+static u8 read_reg(u8 reg)
+{
+	if (!ab3550_dev) {
+		pr_err("%s: The AB3550 codec driver not initialized.\n",
+		       __func__);
+		return 0;
+	}
+	if (reg < AB3550_FIRST_REG)
+		return 0;
+	else if (reg <= AB3550_LAST_REG) {
+		u8 val;
+		abx500_get_register_interruptible(
+			ab3550_dev, I2C_BANK, reg, &val);
+		return val;
+	} else if (reg - AB3550_LAST_REG - 1 < ARRAY_SIZE(virtual_regs))
+		return virtual_regs[reg - AB3550_LAST_REG - 1];
+	dev_warn(ab3550_dev, "%s: out-of-scope reigster %u.\n",
+		 __func__, reg);
+	return 0;
+}
+
+/* Components that can be powered up/down */
+enum enum_widget {
+	widget_ear = 0,
+	widget_auxo1,
+	widget_auxo2,
+
+	widget_spkr,
+	widget_line1,
+	widget_line2,
+
+	widget_dac1,
+	widget_dac2,
+	widget_dac3,
+
+	widget_rx1,
+	widget_rx2,
+	widget_rx3,
+
+	widget_mic1,
+	widget_mic2,
+
+	widget_micbias1,
+	widget_micbias2,
+
+	widget_apga1,
+	widget_apga2,
+
+	widget_tx1,
+	widget_tx2,
+
+	widget_adc1,
+	widget_adc2,
+
+	widget_if0_dld_l,
+	widget_if0_dld_r,
+	widget_if0_uld_l,
+	widget_if0_uld_r,
+	widget_if1_dld_l,
+	widget_if1_dld_r,
+	widget_if1_uld_l,
+	widget_if1_uld_r,
+
+	widget_mic1p1,
+	widget_mic1n1,
+	widget_mic1p2,
+	widget_mic1n2,
+
+	widget_mic2p1,
+	widget_mic2n1,
+	widget_mic2p2,
+	widget_mic2n2,
+
+	widget_clock,
+
+	number_of_widgets
+};
+
+/* This is only meant for debugging */
+static const char *widget_names[] = {
+	"EAR", "AUXO1", "AUXO2", "SPKR", "LINE1", "LINE2",
+	"DAC1", "DAC2", "DAC3",
+	"RX1", "RX2", "RX3",
+	"MIC1", "MIC2",
+	"MIC-BIAS1", "MIC-BIAS2",
+	"APGA1", "APGA2",
+	"TX1", "TX2",
+	"ADC1", "ADC2",
+	"IF0-DLD-L", "IF0-DLD-R", "IF0-ULD-L", "IF0-ULD-R",
+	"IF1-DLD-L", "IF1-DLD-R", "IF1-ULD-L", "IF1-ULD-R",
+	"MIC1P1", "MIC1N1", "MIC1P2", "MIC1N2",
+	"MIC2P1", "MIC2N1", "MIC2P2", "MIC2N2",
+	"CLOCK"
+};
+
+struct widget_pm {
+	enum enum_widget widget;
+	u8 reg;
+	u8 shift;
+
+	unsigned long source_list[BIT_WORD(number_of_widgets) + 1];
+	unsigned long sink_list[BIT_WORD(number_of_widgets) + 1];
+};
+
+static struct widget_pm widget_pm_array[] = {
+	{.widget = widget_ear, .reg = EAR, .shift = EAR_PWR_SHIFT},
+	{.widget = widget_auxo1, .reg = AUXO1, .shift = AUXOx_PWR_SHIFT},
+	{.widget = widget_auxo2, .reg = AUXO2, .shift = AUXOx_PWR_SHIFT},
+	{.widget = widget_spkr, .reg = SPKR, .shift = SPKR_PWR_SHIFT},
+	{.widget = widget_line1, .reg = LINE1, .shift = LINEx_PWR_SHIFT},
+	{.widget = widget_line2, .reg = LINE2, .shift = LINEx_PWR_SHIFT},
+
+	{.widget = widget_dac1, .reg = RX1, .shift = DACx_PWR_SHIFT},
+	{.widget = widget_dac2, .reg = RX2, .shift = DACx_PWR_SHIFT},
+	{.widget = widget_dac3, .reg = RX3, .shift = DACx_PWR_SHIFT},
+
+	{.widget = widget_rx1, .reg = RX1, .shift = RXx_PWR_SHIFT},
+	{.widget = widget_rx2, .reg = RX2, .shift = RXx_PWR_SHIFT},
+	{.widget = widget_rx3, .reg = RX3, .shift = RXx_PWR_SHIFT},
+
+	{.widget = widget_mic1, .reg = MIC1_GAIN, .shift = MICx_PWR_SHIFT},
+	{.widget = widget_mic2, .reg = MIC2_GAIN, .shift = MICx_PWR_SHIFT},
+
+	{.widget = widget_micbias1, .reg = MIC_BIAS1,
+	 .shift = MBIAS_PWR_SHIFT},
+	{.widget = widget_micbias2, .reg = MIC_BIAS2,
+	 .shift = MBIAS_PWR_SHIFT},
+
+	{.widget = widget_apga1, .reg = ANALOG_LOOP_PGA1,
+	 .shift = APGAx_PWR_SHIFT},
+	{.widget = widget_apga2, .reg = ANALOG_LOOP_PGA2,
+	 .shift = APGAx_PWR_SHIFT},
+
+	{.widget = widget_tx1, .reg = TX1, .shift = TXx_PWR_SHIFT},
+	{.widget = widget_tx2, .reg = TX2, .shift = TXx_PWR_SHIFT},
+
+	{.widget = widget_adc1, .reg = TX1, .shift = ADCx_PWR_SHIFT},
+	{.widget = widget_adc2, .reg = TX2, .shift = ADCx_PWR_SHIFT},
+
+	{.widget = widget_if0_dld_l, .reg = AB3550_VIRTUAL_REG1,
+	 .shift = IF0_DLD_L_PW_SHIFT},
+	{.widget = widget_if0_dld_r, .reg = AB3550_VIRTUAL_REG1,
+	 .shift = IF0_DLD_R_PW_SHIFT},
+	{.widget = widget_if0_uld_l, .reg = AB3550_VIRTUAL_REG1,
+	 .shift = IF0_ULD_L_PW_SHIFT},
+	{.widget = widget_if0_uld_r, .reg = AB3550_VIRTUAL_REG1,
+	 .shift = IF0_ULD_R_PW_SHIFT},
+
+	{.widget = widget_if1_dld_l, .reg = AB3550_VIRTUAL_REG1,
+	 .shift = IF1_DLD_L_PW_SHIFT},
+	{.widget = widget_if1_dld_r, .reg = AB3550_VIRTUAL_REG1,
+	 .shift = IF1_DLD_R_PW_SHIFT},
+	{.widget = widget_if1_uld_l, .reg = AB3550_VIRTUAL_REG1,
+	 .shift = IF1_ULD_L_PW_SHIFT},
+	{.widget = widget_if1_uld_r, .reg = AB3550_VIRTUAL_REG1,
+	 .shift = IF1_ULD_R_PW_SHIFT},
+
+	{.widget = widget_mic1p1, .reg = AB3550_VIRTUAL_REG2,
+	 .shift = MIC1P1_PW_SHIFT},
+	{.widget = widget_mic1n1, .reg = AB3550_VIRTUAL_REG2,
+	 .shift = MIC1N1_PW_SHIFT},
+	{.widget = widget_mic1p2, .reg = AB3550_VIRTUAL_REG2,
+	 .shift = MIC1P2_PW_SHIFT},
+	{.widget = widget_mic1n2, .reg = AB3550_VIRTUAL_REG2,
+	 .shift = MIC1N2_PW_SHIFT},
+
+	{.widget = widget_mic2p1, .reg = AB3550_VIRTUAL_REG2,
+	 .shift = MIC2P1_PW_SHIFT},
+	{.widget = widget_mic2n1, .reg = AB3550_VIRTUAL_REG2,
+	 .shift = MIC2N1_PW_SHIFT},
+	{.widget = widget_mic2p2, .reg = AB3550_VIRTUAL_REG2,
+	 .shift = MIC2P2_PW_SHIFT},
+	{.widget = widget_mic2n2, .reg = AB3550_VIRTUAL_REG2,
+	 .shift = MIC2N2_PW_SHIFT},
+
+	{.widget = widget_clock, .reg = CLOCK, .shift = CLOCK_ENABLE_SHIFT},
+};
+
+DEFINE_MUTEX(ab3550_pm_mutex);
+
+static struct {
+	enum enum_widget stack[number_of_widgets];
+	int p;
+} pm_stack;
+
+struct ab3550_dai_private {
+	unsigned int fmt;
+};
+
+#define pm_stack_as_bitmap ({						\
+			unsigned long bitmap[BIT_WORD(number_of_widgets) + 1]; \
+			int i;						\
+			memset(bitmap, 0, sizeof(bitmap));		\
+			for (i = 0; i < pm_stack.p; i++) {		\
+				set_bit(pm_stack.stack[i], bitmap);	\
+			}						\
+			bitmap;						\
+		})
+
+/* These are only meant to meet the obligations of DAPM */
+static const struct snd_soc_dapm_widget ab3550_dapm_widgets[] = {
+};
+
+static const struct snd_soc_dapm_route intercon[] = {
+};
+
+
+static const char *enum_rx2_select[] = {"I2S0", "I2S1"};
+static const char *enum_i2s_input_select[] = {
+	"tri-state", "MIC1", "MIC2", "mute"
+};
+static const char *enum_apga1_source[] = {"LINEIN1", "MIC1", "MIC2"};
+static const char *enum_apga2_source[] = {"LINEIN2", "MIC1", "MIC2"};
+static const char *enum_dac_side_tone[] = {"TX1", "TX2"};
+static const char *enum_dac_power_mode[] = {"100%", "75%", "55%"};
+static const char *enum_ear_power_mode[] = {"100%", "70%"};
+static const char *enum_auxo_power_mode[] = {
+	"100%", "67%", "50%", "25%", "auto"
+};
+static const char *enum_onoff[] = {"Off", "On"};
+static const char *enum_mbias_hiz_option[] = {"GND", "HiZ"};
+static const char *enum_mbias2_output_voltage[] = {"2.0v", "2.2v"};
+static const char *enum_mic_input_impedance[] = {
+	"12.5 kohm", "25 kohm", "50 kohm"
+};
+static const char *enum_hp_filter[] = {"HP3", "HP1", "bypass"};
+static const char *enum_i2s_word_length[] = {"16 bits", "24 bits"};
+static const char *enum_i2s_mode[] = {"Master Mode", "Slave Mode"};
+static const char *enum_i2s_tristate[] = {"Normal", "Tri-state"};
+static const char *enum_optional_resistor[] = {"disconnected", "connected"};
+static const char *enum_i2s_sample_rate[] = {
+	"8 kHz", "16 kHz", "44.1 kHz", "48 kHz"
+};
+static const char *enum_signal_inversion[] = {"normal", "inverted"};
+
+/* RX2 Select */
+static struct soc_enum soc_enum_rx2_select =
+	SOC_ENUM_SINGLE(RX2, 4, ARRAY_SIZE(enum_rx2_select), enum_rx2_select);
+
+/* I2S0 Input Select */
+static struct soc_enum soc_enum_i2s0_input_select =
+	SOC_ENUM_DOUBLE(INTERFACE0_DATA, 0, 2,
+			ARRAY_SIZE(enum_i2s_input_select),
+			enum_i2s_input_select);
+/* I2S1 Input Select */
+static struct soc_enum soc_enum_i2s1_input_select =
+	SOC_ENUM_DOUBLE(INTERFACE1_DATA, 0, 2,
+			ARRAY_SIZE(enum_i2s_input_select),
+			enum_i2s_input_select);
+
+/* APGA1 Source */
+static struct soc_enum soc_enum_apga1_source =
+	SOC_ENUM_SINGLE(ANALOG_LOOP_PGA1, APGAx_MUX_SHIFT,
+			ARRAY_SIZE(enum_apga1_source), enum_apga1_source);
+
+/* APGA2 Source */
+static struct soc_enum soc_enum_apga2_source =
+	SOC_ENUM_SINGLE(ANALOG_LOOP_PGA2, APGAx_MUX_SHIFT,
+			ARRAY_SIZE(enum_apga2_source), enum_apga2_source);
+
+static struct soc_enum soc_enum_apga1_enable =
+	SOC_ENUM_SINGLE(ANALOG_LOOP_PGA1, APGAx_PWR_SHIFT,
+			ARRAY_SIZE(enum_onoff), enum_onoff);
+
+static struct soc_enum soc_enum_apga2_enable =
+	SOC_ENUM_SINGLE(ANALOG_LOOP_PGA2, APGAx_PWR_SHIFT,
+			ARRAY_SIZE(enum_onoff), enum_onoff);
+
+/* DAC1 Side Tone */
+static struct soc_enum soc_enum_dac1_side_tone =
+	SOC_ENUM_SINGLE(SIDETONE1_PGA, STx_MUX_SHIFT,
+			ARRAY_SIZE(enum_dac_side_tone), enum_dac_side_tone);
+
+/* DAC2 Side Tone */
+static struct soc_enum soc_enum_dac2_side_tone =
+	SOC_ENUM_SINGLE(SIDETONE2_PGA, STx_MUX_SHIFT,
+			ARRAY_SIZE(enum_dac_side_tone), enum_dac_side_tone);
+
+/* DAC1 Power Mode */
+static struct soc_enum soc_enum_dac1_power_mode =
+	SOC_ENUM_SINGLE(RX1, DACx_PWR_MODE_SHIFT,
+			ARRAY_SIZE(enum_dac_power_mode), enum_dac_power_mode);
+
+/* DAC2 Power Mode */
+static struct soc_enum soc_enum_dac2_power_mode =
+	SOC_ENUM_SINGLE(RX2, DACx_PWR_MODE_SHIFT,
+			ARRAY_SIZE(enum_dac_power_mode), enum_dac_power_mode);
+
+/* DAC3 Power Mode */
+static struct soc_enum soc_enum_dac3_power_mode =
+	SOC_ENUM_SINGLE(RX3, DACx_PWR_MODE_SHIFT,
+			ARRAY_SIZE(enum_dac_power_mode), enum_dac_power_mode);
+
+/* EAR Power Mode */
+static struct soc_enum soc_enum_ear_power_mode =
+	SOC_ENUM_SINGLE(EAR, EAR_PWR_MODE_SHIFT,
+			ARRAY_SIZE(enum_ear_power_mode), enum_ear_power_mode);
+
+/* AUXO Power Mode */
+static struct soc_enum soc_enum_auxo_power_mode =
+	SOC_ENUM_SINGLE(AUXO_PWR_MODE, AUXO_PWR_MODE_SHIFT,
+			ARRAY_SIZE(enum_auxo_power_mode),
+			enum_auxo_power_mode);
+
+/* MBIAS1 HiZ Option */
+static struct soc_enum soc_enum_mbias1_hiz_option =
+	SOC_ENUM_SINGLE(MIC_BIAS1, MBIAS_PDN_IMP_SHIFT,
+			ARRAY_SIZE(enum_mbias_hiz_option),
+			enum_mbias_hiz_option);
+
+/* MBIAS1 HiZ Option */
+static struct soc_enum soc_enum_mbias2_hiz_option =
+	SOC_ENUM_SINGLE(MIC_BIAS2, MBIAS_PDN_IMP_SHIFT,
+			ARRAY_SIZE(enum_mbias_hiz_option),
+			enum_mbias_hiz_option);
+
+/* MBIAS2 Output voltage */
+static struct soc_enum soc_enum_mbias2_output_voltage =
+	SOC_ENUM_SINGLE(MIC_BIAS2, MBIAS2_OUT_V_SHIFT,
+			ARRAY_SIZE(enum_mbias2_output_voltage),
+			enum_mbias2_output_voltage);
+
+static struct soc_enum soc_enum_mbias2_internal_resistor =
+	SOC_ENUM_SINGLE(MIC_BIAS2_VAD, MBIAS2_R_INT_SHIFT,
+			ARRAY_SIZE(enum_optional_resistor),
+			enum_optional_resistor);
+
+static struct soc_enum soc_enum_mic1_input_impedance =
+	SOC_ENUM_SINGLE(MIC1_GAIN, MICx_IN_IMP_SHIFT,
+			ARRAY_SIZE(enum_mic_input_impedance),
+			enum_mic_input_impedance);
+
+static struct soc_enum soc_enum_mic2_input_impedance =
+	SOC_ENUM_SINGLE(MIC2_GAIN, MICx_IN_IMP_SHIFT,
+			ARRAY_SIZE(enum_mic_input_impedance),
+			enum_mic_input_impedance);
+
+static struct soc_enum soc_enum_tx1_hp_filter =
+	SOC_ENUM_SINGLE(TX1, TXx_HP_FILTER_SHIFT,
+			ARRAY_SIZE(enum_hp_filter),
+			enum_hp_filter);
+
+static struct soc_enum soc_enum_tx2_hp_filter =
+	SOC_ENUM_SINGLE(TX2, TXx_HP_FILTER_SHIFT,
+			ARRAY_SIZE(enum_hp_filter),
+			enum_hp_filter);
+
+static struct soc_enum soc_enum_st1_hp_filter =
+	SOC_ENUM_SINGLE(SIDETONE1_PGA, STx_HP_FILTER_SHIFT,
+			ARRAY_SIZE(enum_hp_filter),
+			enum_hp_filter);
+
+static struct soc_enum soc_enum_st2_hp_filter =
+	SOC_ENUM_SINGLE(SIDETONE2_PGA, STx_HP_FILTER_SHIFT,
+			ARRAY_SIZE(enum_hp_filter),
+			enum_hp_filter);
+
+static struct soc_enum soc_enum_i2s0_word_length =
+	SOC_ENUM_SINGLE(INTERFACE0, I2Sx_WORDLENGTH_SHIFT,
+			ARRAY_SIZE(enum_i2s_word_length),
+			enum_i2s_word_length);
+
+static struct soc_enum soc_enum_i2s1_word_length =
+	SOC_ENUM_SINGLE(INTERFACE1, I2Sx_WORDLENGTH_SHIFT,
+			ARRAY_SIZE(enum_i2s_word_length),
+			enum_i2s_word_length);
+
+static struct soc_enum soc_enum_i2s0_mode =
+	SOC_ENUM_SINGLE(INTERFACE0, I2Sx_MODE_SHIFT,
+			ARRAY_SIZE(enum_i2s_mode),
+			enum_i2s_mode);
+
+static struct soc_enum soc_enum_i2s1_mode =
+	SOC_ENUM_SINGLE(INTERFACE1, I2Sx_MODE_SHIFT,
+			ARRAY_SIZE(enum_i2s_mode),
+			enum_i2s_mode);
+
+static struct soc_enum soc_enum_i2s0_tristate =
+	SOC_ENUM_SINGLE(INTERFACE0, I2Sx_TRISTATE_SHIFT,
+			ARRAY_SIZE(enum_i2s_tristate),
+			enum_i2s_tristate);
+
+static struct soc_enum soc_enum_i2s1_tristate =
+	SOC_ENUM_SINGLE(INTERFACE1, I2Sx_TRISTATE_SHIFT,
+			ARRAY_SIZE(enum_i2s_tristate),
+			enum_i2s_tristate);
+
+static struct soc_enum soc_enum_i2s0_pulldown_resistor =
+	SOC_ENUM_SINGLE(INTERFACE0, I2Sx_PULLDOWN_SHIFT,
+			ARRAY_SIZE(enum_optional_resistor),
+			enum_optional_resistor);
+
+static struct soc_enum soc_enum_i2s1_pulldown_resistor =
+	SOC_ENUM_SINGLE(INTERFACE1, I2Sx_PULLDOWN_SHIFT,
+			ARRAY_SIZE(enum_optional_resistor),
+			enum_optional_resistor);
+
+static struct soc_enum soc_enum_i2s0_sample_rate =
+	SOC_ENUM_SINGLE(INTERFACE0, I2Sx_SR_SHIFT,
+			ARRAY_SIZE(enum_i2s_sample_rate),
+			enum_i2s_sample_rate);
+
+static struct soc_enum soc_enum_i2s1_sample_rate =
+	SOC_ENUM_SINGLE(INTERFACE1, I2Sx_SR_SHIFT,
+			ARRAY_SIZE(enum_i2s_sample_rate),
+			enum_i2s_sample_rate);
+
+static struct soc_enum soc_enum_line1_inversion =
+	SOC_ENUM_SINGLE(LINE1, LINEx_INV_SHIFT,
+			ARRAY_SIZE(enum_signal_inversion),
+			enum_signal_inversion);
+
+static struct soc_enum soc_enum_line2_inversion =
+	SOC_ENUM_SINGLE(LINE2, LINEx_INV_SHIFT,
+			ARRAY_SIZE(enum_signal_inversion),
+			enum_signal_inversion);
+
+static struct soc_enum soc_enum_auxo1_inversion =
+	SOC_ENUM_SINGLE(AUXO1, AUXOx_INV_SHIFT,
+			ARRAY_SIZE(enum_signal_inversion),
+			enum_signal_inversion);
+
+static struct soc_enum soc_enum_auxo2_inversion =
+	SOC_ENUM_SINGLE(AUXO1, AUXOx_INV_SHIFT,
+			ARRAY_SIZE(enum_signal_inversion),
+			enum_signal_inversion);
+
+static struct soc_enum soc_enum_auxo1_pulldown_resistor =
+	SOC_ENUM_SINGLE(AUXO1, AUXOx_PULLDOWN_SHIFT,
+			ARRAY_SIZE(enum_optional_resistor),
+			enum_optional_resistor);
+
+static struct soc_enum soc_enum_auxo2_pulldown_resistor =
+	SOC_ENUM_SINGLE(AUXO1, AUXOx_PULLDOWN_SHIFT,
+			ARRAY_SIZE(enum_optional_resistor),
+			enum_optional_resistor);
+
+static struct snd_kcontrol_new ab3550_snd_controls[] = {
+	/* RX Routing */
+	SOC_ENUM("RX2 Select",	soc_enum_rx2_select),
+	SOC_SINGLE("LINE1 Adder", LINE1_ADDER, 0, 0x07, 0),
+	SOC_SINGLE("LINE2 Adder", LINE2_ADDER, 0, 0x07, 0),
+	SOC_SINGLE("EAR Adder", EAR_ADDER, 0, 0x07, 0),
+	SOC_SINGLE("SPKR Adder", SPKR_ADDER, 0, 0x07, 0),
+	SOC_SINGLE("AUXO1 Adder", AUXO1_ADDER, 0, 0x07, 0),
+	SOC_SINGLE("AUXO2 Adder", AUXO2_ADDER, 0, 0x07, 0),
+	/* TX Routing */
+	SOC_SINGLE("MIC1 Input Select",	MIC1_INPUT_SELECT, 0, 0xff, 0),
+	SOC_SINGLE("MIC2 Input Select",	MIC1_INPUT_SELECT, 0, 0xff, 0),
+	SOC_SINGLE("MIC2 to MIC1", MIC2_TO_MIC1, 0, 0x03, 0),
+	SOC_ENUM("I2S0 Input Select",	soc_enum_i2s0_input_select),
+	SOC_ENUM("I2S1 Input Select",	soc_enum_i2s1_input_select),
+	/* Routing of Side Tone and Analop Loop */
+	SOC_ENUM("APGA1 Source", soc_enum_apga1_source),
+	SOC_ENUM("APGA2 Source", soc_enum_apga2_source),
+	SOC_ENUM("APGA1 Enable", soc_enum_apga1_enable),
+	SOC_ENUM("APGA2 Enable", soc_enum_apga2_enable),
+	SOC_SINGLE("APGA1 Destination",	APGA1_ADDER, 0, 0x3f, 0),
+	SOC_SINGLE("APGA2 Destination",	APGA2_ADDER, 0, 0x3f, 0),
+	SOC_ENUM("DAC1 Side Tone",		soc_enum_dac1_side_tone),
+	SOC_ENUM("DAC2 Side Tone",		soc_enum_dac2_side_tone),
+	/* RX Volume Control */
+	SOC_SINGLE("RX-DPGA1 Gain",		RX1_DIGITAL_PGA, 0, 0x43, 0),
+	SOC_SINGLE("RX-DPGA2 Gain",		RX1_DIGITAL_PGA, 0, 0x43, 0),
+	SOC_SINGLE("RX-DPGA3 Gain",		RX3_DIGITAL_PGA, 0, 0x43, 0),
+	SOC_SINGLE("LINE1 Gain", LINE1, LINEx_GAIN_SHIFT, 0x0a, 0),
+	SOC_SINGLE("LINE2 Gain", LINE2, LINEx_GAIN_SHIFT, 0x0a, 0),
+	SOC_SINGLE("SPKR Gain",	SPKR, SPKR_GAIN_SHIFT, 0x16, 0),
+	SOC_SINGLE("EAR Gain", EAR, EAR_GAIN_SHIFT, 0x0e, 0),
+	SOC_SINGLE("AUXO1 Gain", AUXO1, AUXOx_GAIN_SHIFT, 0x0c, 0),
+	SOC_SINGLE("AUXO2 Gain", AUXO2, AUXOx_GAIN_SHIFT, 0x0c, 0),
+	/* TX Volume Control */
+	SOC_SINGLE("MIC1 Gain", MIC1_GAIN, MICx_GAIN_SHIFT, 0x0a, 0),
+	SOC_SINGLE("MIC2 Gain",	MIC2_GAIN, MICx_GAIN_SHIFT, 0x0a, 0),
+	SOC_SINGLE("TX-DPGA1 Gain", TX_DIGITAL_PGA1, TXDPGAx_SHIFT, 0x0f, 0),
+	SOC_SINGLE("TX-DPGA2 Gain", TX_DIGITAL_PGA2, TXDPGAx_SHIFT, 0x0f, 0),
+	/* Volume Control of Side Tone and Analog Loop */
+	SOC_SINGLE("ST-PGA1 Gain", SIDETONE1_PGA, STx_PGA_SHIFT, 0x0a, 0),
+	SOC_SINGLE("ST-PGA2 Gain", SIDETONE2_PGA, STx_PGA_SHIFT, 0x0a, 0),
+	SOC_SINGLE("APGA1 Gain", ANALOG_LOOP_PGA1, APGAx_GAIN_SHIFT, 0x1d, 0),
+	SOC_SINGLE("APGA2 Gain", ANALOG_LOOP_PGA2, APGAx_GAIN_SHIFT, 0x1d, 0),
+	/* RX Properties */
+	SOC_ENUM("DAC1 Power Mode",		soc_enum_dac1_power_mode),
+	SOC_ENUM("DAC2 Power Mode",		soc_enum_dac2_power_mode),
+	SOC_ENUM("DAC3 Power Mode",		soc_enum_dac3_power_mode),
+	SOC_ENUM("EAR Power Mode",		soc_enum_ear_power_mode),
+	SOC_ENUM("AUXO Power Mode",		soc_enum_auxo_power_mode),
+	SOC_ENUM("LINE1 Inversion",		soc_enum_line1_inversion),
+	SOC_ENUM("LINE2 Inversion",		soc_enum_line2_inversion),
+	SOC_ENUM("AUXO1 Inversion",		soc_enum_auxo1_inversion),
+	SOC_ENUM("AUXO2 Inversion",		soc_enum_auxo2_inversion),
+	SOC_ENUM("AUXO1 Pulldown Resistor", soc_enum_auxo1_pulldown_resistor),
+	SOC_ENUM("AUXO2 Pulldown Resistor", soc_enum_auxo2_pulldown_resistor),
+	/* TX Properties */
+	SOC_SINGLE("MIC1 VMID",	MIC1_VMID_SELECT, 0, 0xff, 0),
+	SOC_SINGLE("MIC2 VMID",	MIC2_VMID_SELECT, 0, 0xff, 0),
+	SOC_ENUM("MBIAS1 HiZ Option",	soc_enum_mbias1_hiz_option),
+	SOC_ENUM("MBIAS2 HiZ Option",	soc_enum_mbias2_hiz_option),
+	SOC_ENUM("MBIAS2 Output Voltage", soc_enum_mbias2_output_voltage),
+	SOC_ENUM("MBIAS2 Internal Resistor", soc_enum_mbias2_internal_resistor),
+	SOC_ENUM("MIC1 Input Impedance",	soc_enum_mic1_input_impedance),
+	SOC_ENUM("MIC2 Input Impedance",	soc_enum_mic2_input_impedance),
+	SOC_ENUM("TX1 HP Filter",		soc_enum_tx1_hp_filter),
+	SOC_ENUM("TX2 HP Filter",		soc_enum_tx2_hp_filter),
+	/* Side Tone and Analog Loop Properties */
+	SOC_ENUM("ST1 HP Filter",		soc_enum_st1_hp_filter),
+	SOC_ENUM("ST2 HP Filter",		soc_enum_st2_hp_filter),
+	/* I2S Interface Properties */
+	SOC_ENUM("I2S0 Word Length",		soc_enum_i2s0_word_length),
+	SOC_ENUM("I2S1 Word Length",		soc_enum_i2s1_word_length),
+	SOC_ENUM("I2S0 Mode",			soc_enum_i2s0_mode),
+	SOC_ENUM("I2S1 Mode",			soc_enum_i2s1_mode),
+	SOC_ENUM("I2S0 tri-state",		soc_enum_i2s0_tristate),
+	SOC_ENUM("I2S1 tri-state",		soc_enum_i2s1_tristate),
+	SOC_ENUM("I2S0 Pulldown Resistor", soc_enum_i2s0_pulldown_resistor),
+	SOC_ENUM("I2S1 Pulldown Resistor", soc_enum_i2s1_pulldown_resistor),
+	SOC_ENUM("I2S0 Sample Rate",		soc_enum_i2s0_sample_rate),
+	SOC_ENUM("I2S1 Sample Rate",		soc_enum_i2s1_sample_rate),
+	SOC_SINGLE("Interface Loop",		INTERFACE_LOOP, 0, 0x0f, 0),
+	SOC_SINGLE("Interface Swap",		INTERFACE_SWAP, 0, 0x1f, 0),
+	/* Miscellaneous */
+	SOC_SINGLE("Negative Charge Pump", NEGATIVE_CHARGE_PUMP, 0, 0x03, 0)
+};
+
+/* count the number of 1 */
+#define count_ones(x) ({						\
+			int num;					\
+			for (num = 0; x; (x) &= (x) - 1, num++)		\
+				;					\
+			num;						\
+		})
+
+enum enum_power {
+	POWER_OFF = 0,
+	POWER_ON = 1
+};
+
+enum enum_link {
+	UNLINK = 0,
+	LINK = 1
+};
+
+static enum enum_power get_widget_power_status(enum enum_widget widget)
+{
+	u8 val;
+
+	if (widget >= number_of_widgets)
+		return POWER_OFF;
+	val = read_reg(widget_pm_array[widget].reg);
+	if (val & (1 << widget_pm_array[widget].shift))
+		return POWER_ON;
+	else
+		return POWER_OFF;
+}
+
+static int count_powered_neighbors(const unsigned long *neighbors)
+{
+	unsigned long i;
+	int n = 0;
+	for_each_set_bit(i, neighbors, number_of_widgets) {
+		if (get_widget_power_status(i) == POWER_ON)
+			n++;
+	}
+	return n;
+}
+
+static int has_powered_neighbors(const unsigned long *neighbors)
+{
+	unsigned int i;
+	for_each_set_bit(i, neighbors, number_of_widgets) {
+		if (get_widget_power_status(i) == POWER_ON)
+			return 1;
+	}
+	return 0;
+}
+
+
+static int has_stacked_neighbors(const unsigned long *neighbors)
+{
+	unsigned long *stack_map = pm_stack_as_bitmap;
+	return bitmap_intersects(stack_map, neighbors, number_of_widgets);
+}
+
+static void power_widget_unlocked(enum enum_power onoff,
+				  enum enum_widget widget)
+{
+	enum enum_widget w;
+	int done;
+
+	if (widget >= number_of_widgets)
+		return;
+	if (get_widget_power_status(widget) == onoff)
+		return;
+
+	for (w = widget, done = 0; !done;) {
+		unsigned long i;
+		unsigned long *srcs = widget_pm_array[w].source_list;
+		unsigned long *sinks = widget_pm_array[w].sink_list;
+		dev_dbg(ab3550_dev, "%s: processing widget %s.\n",
+			__func__, widget_names[w]);
+
+		if (onoff == POWER_ON &&
+		    !bitmap_empty(srcs, number_of_widgets) &&
+		    !has_powered_neighbors(srcs)) {
+			pm_stack.stack[pm_stack.p++] = w;
+			for_each_set_bit(i, srcs, number_of_widgets) {
+				pm_stack.stack[pm_stack.p++] = i;
+			}
+			w = pm_stack.stack[--pm_stack.p];
+			continue;
+		} else if (onoff == POWER_OFF &&
+			   has_powered_neighbors(sinks)) {
+			int n = 0;
+			pm_stack.stack[pm_stack.p++] = w;
+			for_each_set_bit(i, sinks, number_of_widgets) {
+				if (count_powered_neighbors(
+					    widget_pm_array[i].source_list)
+				    == 1 &&
+				    get_widget_power_status(i) == POWER_ON) {
+					pm_stack.stack[pm_stack.p++] = i;
+					n++;
+				}
+			}
+			if (n) {
+				w = pm_stack.stack[--pm_stack.p];
+				continue;
+			} else
+				--pm_stack.p;
+		}
+		mask_set_reg(widget_pm_array[w].reg,
+			     1 << widget_pm_array[w].shift,
+			     onoff == POWER_ON ? 0xff : 0);
+		dev_dbg(ab3550_dev, "%s: widget %s powered %s.\n",
+			__func__, widget_names[w],
+			onoff == POWER_ON ? "on" : "off");
+
+		if (onoff == POWER_ON &&
+		    !bitmap_empty(sinks, number_of_widgets) &&
+		    !has_powered_neighbors(sinks) &&
+		    !has_stacked_neighbors(sinks)) {
+			for_each_set_bit(i, sinks, number_of_widgets) {
+				pm_stack.stack[pm_stack.p++] = i;
+			}
+			w = pm_stack.stack[--pm_stack.p];
+			continue;
+		} else if (onoff == POWER_OFF) {
+			for_each_set_bit(i, srcs, number_of_widgets) {
+				if (!has_powered_neighbors(
+					    widget_pm_array[i].sink_list)
+				    && get_widget_power_status(i) == POWER_ON
+				    && !test_bit(i, pm_stack_as_bitmap)) {
+					pm_stack.stack[pm_stack.p++] = i;
+				}
+			}
+		}
+		if (pm_stack.p > 0)
+			w = pm_stack.stack[--pm_stack.p];
+		else
+			done = 1;
+	}
+}
+
+static void power_widget_locked(enum enum_power onoff,
+				enum enum_widget widget)
+{
+	if (mutex_lock_interruptible(&ab3550_pm_mutex)) {
+		dev_warn(ab3550_dev,
+			 "%s: Signal received while waiting on the PM mutex.\n",
+			 __func__);
+		return;
+	}
+	power_widget_unlocked(onoff, widget);
+	mutex_unlock(&ab3550_pm_mutex);
+}
+
+static void dump_registers(const char *where, ...)
+{
+	va_list ap;
+	va_start(ap, where);
+	do {
+		short reg = va_arg(ap, int);
+		if (reg < 0)
+			break;
+		dev_dbg(ab3550_dev, "%s from %s> 0x%02X : 0x%02X.\n",
+			__func__, where, reg, read_reg(reg));
+	} while (1);
+	va_end(ap);
+}
+
+/**
+ * update the link between two widgets.
+ * @op: 1 - connect; 0 - disconnect
+ * @src: source of the connection
+ * @sink: sink of the connection
+ */
+static int update_widgets_link(enum enum_link op, enum enum_widget src,
+			       enum enum_widget sink,
+			       u8 reg, u8 mask, u8 newval)
+{
+	int ret = 0;
+
+	if (mutex_lock_interruptible(&ab3550_pm_mutex)) {
+		dev_warn(ab3550_dev, "%s: A signal is received while waiting on"
+			 " the PM mutex.\n", __func__);
+		return -EINTR;
+	}
+
+	switch (op << 2 | test_bit(sink, widget_pm_array[src].sink_list) << 1 |
+		test_bit(src, widget_pm_array[sink].source_list)) {
+	case 3: /* UNLINK, sink in sink_list, src in source_list */
+	case 4: /* LINK, sink not in sink_list, src not in source_list */
+		break;
+	default:
+		ret = -EINVAL;
+		goto end;
+	}
+	switch (((int)op) << 2 | get_widget_power_status(src) << 1 |
+		get_widget_power_status(sink)) {
+	case 3: /* op = 0, src on, sink on */
+		if (count_powered_neighbors(widget_pm_array[sink].source_list)
+		    == 1)
+			power_widget_unlocked(POWER_OFF, sink);
+		mask_set_reg(reg, mask, newval);
+		break;
+	case 6: /* op = 1, src on, sink off */
+		mask_set_reg(reg, mask, newval);
+		power_widget_unlocked(POWER_ON, sink);
+		break;
+	default:
+		/* op = 0, src off, sink off */
+		/* op = 0, src off, sink on */
+		/* op = 0, src on, sink off */
+		/* op = 1, src off, sink off */
+		/* op = 1, src off, sink on */
+		/* op = 1, src on, sink on */
+		mask_set_reg(reg, mask, newval);
+	}
+	change_bit(sink, widget_pm_array[src].sink_list);
+	change_bit(src, widget_pm_array[sink].source_list);
+end:
+	mutex_unlock(&ab3550_pm_mutex);
+	return ret;
+};
+
+static enum enum_widget apga_source_translate(u8 reg_value)
+{
+	switch (reg_value) {
+	case 1:
+		return widget_mic1;
+	case 2:
+		return widget_mic2;
+	default:
+		return number_of_widgets;
+	}
+}
+
+static enum enum_widget adder_sink_translate(u8 reg)
+{
+	switch (reg) {
+	case EAR_ADDER:
+		return widget_ear;
+	case AUXO1_ADDER:
+		return widget_auxo1;
+	case AUXO2_ADDER:
+		return widget_auxo2;
+	case SPKR_ADDER:
+		return widget_spkr;
+	case LINE1_ADDER:
+		return widget_line1;
+	case LINE2_ADDER:
+		return widget_line2;
+	case APGA1_ADDER:
+		return widget_apga1;
+	case APGA2_ADDER:
+		return widget_apga2;
+	default:
+		return number_of_widgets;
+	}
+}
+
+static int ab3550_add_widgets(struct snd_soc_codec *codec)
+{
+	snd_soc_dapm_new_controls(&codec->dapm, ab3550_dapm_widgets,
+				  ARRAY_SIZE(ab3550_dapm_widgets));
+
+	snd_soc_dapm_add_routes(&codec->dapm, intercon, ARRAY_SIZE(intercon));
+
+	snd_soc_dapm_new_widgets(&codec->dapm);
+	return 0;
+}
+
+static void power_for_playback(enum enum_power onoff, int ifsel)
+{
+	dev_dbg(ab3550_dev, "%s: interface %d power %s.\n", __func__,
+		ifsel, onoff == POWER_ON ? "on" : "off");
+
+	if (mutex_lock_interruptible(&ab3550_pm_mutex)) {
+		dev_warn(ab3550_dev,
+			 "%s: Signal received while waiting on the PM mutex.\n",
+			 __func__);
+		return;
+	}
+	power_widget_unlocked(onoff, ifsel == 0 ?
+			      widget_if0_dld_l : widget_if1_dld_l);
+	power_widget_unlocked(onoff, ifsel == 0 ?
+			      widget_if0_dld_r : widget_if1_dld_r);
+	mutex_unlock(&ab3550_pm_mutex);
+}
+
+static void power_for_capture(enum enum_power onoff, int ifsel)
+{
+	dev_dbg(ab3550_dev, "%s: interface %d power %s", __func__,
+		ifsel, onoff == POWER_ON ? "on" : "off");
+	if (mutex_lock_interruptible(&ab3550_pm_mutex)) {
+		dev_warn(ab3550_dev,
+			 "%s: Signal received while waiting on the PM mutex.\n",
+			 __func__);
+		return;
+	}
+	power_widget_unlocked(onoff, ifsel == 0 ?
+			      widget_if0_uld_l : widget_if1_uld_l);
+	power_widget_unlocked(onoff, ifsel == 0 ?
+			      widget_if0_uld_r : widget_if1_uld_r);
+	mutex_unlock(&ab3550_pm_mutex);
+}
+
+static int ab3550_add_controls(struct snd_soc_codec *codec)
+{
+	int err = 0, i, n = ARRAY_SIZE(ab3550_snd_controls);
+
+	pr_debug("%s: %s called.\n", __FILE__, __func__);
+	for (i = 0; i < n; i++) {
+		err = snd_ctl_add(codec->card->snd_card, snd_ctl_new1(
+					  &ab3550_snd_controls[i], codec));
+		if (err < 0) {
+			pr_err("%s failed to add control No.%d of %d.\n",
+			       __func__, i, n);
+			return err;
+		}
+	}
+	return err;
+}
+
+static int ab3550_pcm_hw_params(struct snd_pcm_substream *substream,
+				struct snd_pcm_hw_params *hw_params,
+				struct snd_soc_dai *dai)
+{
+	u8 val;
+	u8 reg = dai->id == 0 ? INTERFACE0 : INTERFACE1;
+
+	if (!ab3550_dev) {
+		pr_err("%s: The AB3550 codec driver not initialized.\n",
+		       __func__);
+		return -EAGAIN;
+	}
+	dev_info(ab3550_dev, "%s called.\n", __func__);
+	switch (params_rate(hw_params)) {
+	case 8000:
+		val = I2Sx_SR_8000Hz;
+		break;
+	case 16000:
+		val = I2Sx_SR_16000Hz;
+		break;
+	case 44100:
+		val = I2Sx_SR_44100Hz;
+		break;
+	case 48000:
+		val = I2Sx_SR_48000Hz;
+		break;
+	default:
+		return -EINVAL;
+	}
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK ?
+	    !dai->capture_active : !dai->playback_active) {
+
+		mask_set_reg(reg, I2Sx_SR_MASK, val << I2Sx_SR_SHIFT);
+		if ((read_reg(reg) & I2Sx_MODE_MASK) == 0) {
+			mask_set_reg(reg, MASTER_GENx_PWR_MASK,
+				     1 << MASTER_GENx_PWR_SHIFT);
+		}
+	}
+	return 0;
+}
+
+static int ab3550_pcm_startup(struct snd_pcm_substream *substream,
+			      struct snd_soc_dai *dai)
+{
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK ?
+	    dai->playback_active : dai->capture_active) {
+
+		dev_err(ab3550_dev, "%s: A %s stream is already active.\n",
+			__func__,
+			substream->stream == SNDRV_PCM_STREAM_PLAYBACK ?
+			"PLAYBACK" : "CAPTURE");
+		return -EBUSY;
+	}
+	return 0;
+}
+static int ab3550_pcm_prepare(struct snd_pcm_substream *substream,
+			      struct snd_soc_dai *dai)
+{
+	dev_info(ab3550_dev, "%s called.\n", __func__);
+
+	/* Configure registers for either playback or capture */
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
+		power_for_playback(POWER_ON, dai->id);
+		dump_registers(__func__,
+			       dai->id == 0 ? INTERFACE0 : INTERFACE1,
+			       RX1, RX2, SPKR, EAR, -1);
+	} else {
+		power_for_capture(POWER_ON, dai->id);
+		dump_registers(__func__, MIC_BIAS1, MIC_BIAS2, MIC1_GAIN, TX1,
+			       dai->id == 0 ? INTERFACE0 : INTERFACE1, -1);
+	}
+	return 0;
+}
+
+static void ab3550_pcm_shutdown(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *dai)
+{
+	u8 iface = dai->id == 0 ? INTERFACE0 : INTERFACE1;
+	dev_info(ab3550_dev, "%s called.\n", __func__);
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		power_for_playback(POWER_OFF, dai->id);
+	else
+		power_for_capture(POWER_OFF, dai->id);
+	if (!dai->playback_active && !dai->capture_active &&
+	    (read_reg(iface) & I2Sx_MODE_MASK) == 0)
+		mask_set_reg(iface, MASTER_GENx_PWR_MASK, 0);
+}
+
+static int ab3550_set_dai_sysclk(struct snd_soc_dai *dai, int clk_id,
+				 unsigned int freq, int dir)
+{
+	return 0;
+}
+
+static int ab3550_set_dai_fmt(struct snd_soc_dai *codec_dai, unsigned int fmt)
+{
+	u8 iface = (codec_dai->id == 0) ? INTERFACE0 : INTERFACE1;
+	u8 val = 0;
+	dev_info(ab3550_dev, "%s called.\n", __func__);
+
+	switch (fmt & (SND_SOC_DAIFMT_FORMAT_MASK |
+		       SND_SOC_DAIFMT_MASTER_MASK)) {
+
+	case SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBS_CFS:
+		val |= 1 << I2Sx_MODE_SHIFT;
+		break;
+
+	case SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBM_CFM:
+		break;
+
+	default:
+		dev_warn(ab3550_dev, "AB3550_dai: unsupported DAI format "
+			 "0x%x\n", fmt);
+		return -EINVAL;
+	}
+	if (codec_dai->playback_active && codec_dai->capture_active) {
+		if ((read_reg(iface) & I2Sx_MODE_MASK) == val)
+			return 0;
+		else {
+			dev_err(ab3550_dev,
+				"%s: DAI format set differently "
+				"by an existing stream.\n", __func__);
+			return -EINVAL;
+		}
+	}
+	mask_set_reg(iface, I2Sx_MODE_MASK, val);
+	return 0;
+}
+
+struct snd_soc_dai_driver ab3550_dai_drv[] = {
+	{
+		.name = "ab3550-codec-dai.0",
+		.id = 0,
+		.playback = {
+			.stream_name = "AB3550.0 Playback",
+			.channels_min = 2,
+			.channels_max = 2,
+			.rates = AB3550_SUPPORTED_RATE,
+			.formats = AB3550_SUPPORTED_FMT,
+		},
+		.capture = {
+			.stream_name = "AB3550.0 Capture",
+			.channels_min = 2,
+			.channels_max = 2,
+			.rates = AB3550_SUPPORTED_RATE,
+			.formats = AB3550_SUPPORTED_FMT,
+		},
+		.ops = (struct snd_soc_dai_ops[]) {
+			{
+				.startup = ab3550_pcm_startup,
+				.prepare = ab3550_pcm_prepare,
+				.hw_params = ab3550_pcm_hw_params,
+				.shutdown = ab3550_pcm_shutdown,
+				.set_sysclk = ab3550_set_dai_sysclk,
+				.set_fmt = ab3550_set_dai_fmt,
+			}
+		},
+		.symmetric_rates = 1,
+	},
+	{
+		.name = "ab3550-codec-dai.1",
+		.id = 1,
+		.playback = {
+			.stream_name = "AB3550.1 Playback",
+			.channels_min = 2,
+			.channels_max = 2,
+			.rates = AB3550_SUPPORTED_RATE,
+			.formats = AB3550_SUPPORTED_FMT,
+		},
+		.capture = {
+			.stream_name = "AB3550.0 Capture",
+			.channels_min = 2,
+			.channels_max = 2,
+			.rates = AB3550_SUPPORTED_RATE,
+			.formats = AB3550_SUPPORTED_FMT,
+		},
+		.ops = (struct snd_soc_dai_ops[]) {
+			{
+				.startup = ab3550_pcm_startup,
+				.prepare = ab3550_pcm_prepare,
+				.hw_params = ab3550_pcm_hw_params,
+				.shutdown = ab3550_pcm_shutdown,
+				.set_sysclk = ab3550_set_dai_sysclk,
+				.set_fmt = ab3550_set_dai_fmt,
+			}
+		},
+		.symmetric_rates = 1,
+	}
+};
+EXPORT_SYMBOL_GPL(ab3550_dai_drv);
+
+static int ab3550_codec_probe(struct snd_soc_codec *codec)
+{
+	int ret;
+
+	pr_info("%s: Enter.\n", __func__);
+
+	/* Add controls */
+	if (ab3550_add_controls(codec) < 0)
+		return ret;
+
+	/* Add widgets */
+	ab3550_add_widgets(codec);
+
+	return 0;
+}
+
+static int ab3550_codec_remove(struct snd_soc_codec *codec)
+{
+	snd_soc_dapm_free(&codec->dapm);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int ab3550_codec_suspend(struct snd_soc_codec *codec, pm_message_t state)
+{
+	mask_set_reg(CLOCK, CLOCK_ENABLE_MASK, 0);
+
+	return 0;
+}
+
+static int ab3550_codec_resume(struct snd_soc_codec *codec)
+{
+	mask_set_reg(CLOCK, CLOCK_ENABLE_MASK, 0xff);
+
+	return 0;
+}
+#else
+#define ab3550_codec_resume NULL
+#define ab3550_codec_suspend NULL
+#endif
+
+/*
+ * This function is only called by the SOC framework to
+ * set registers associated to the mixer controls.
+ */
+static int ab3550_codec_write_reg(struct snd_soc_codec *codec, unsigned int reg,
+			    unsigned int value)
+{
+	if (reg < MIC_BIAS1 || reg > INTERFACE_SWAP)
+		return -EINVAL;
+	switch (reg) {
+		u8 diff, oldval;
+	case ANALOG_LOOP_PGA1:
+	case ANALOG_LOOP_PGA2: {
+		enum enum_widget apga = reg == ANALOG_LOOP_PGA1 ?
+			widget_apga1 : widget_apga2;
+
+		oldval = read_reg(reg);
+		diff = value ^ oldval;
+
+		/* The APGA is to be turned on/off.
+		 * The power bit and the other bits in the
+		 * same register won't be changed at the same time
+		 * since they belong to different controls.
+		 */
+		if (diff & (1 << APGAx_PWR_SHIFT)) {
+			power_widget_locked(value >> APGAx_PWR_SHIFT & 1,
+					    apga);
+		} else if (diff & APGAx_MUX_MASK) {
+			enum enum_widget old_source =
+				apga_source_translate(oldval);
+			enum enum_widget new_source =
+				apga_source_translate(value);
+			update_widgets_link(UNLINK, old_source, apga,
+					    reg, APGAx_MUX_MASK, 0);
+			update_widgets_link(LINK, new_source, apga,
+					    reg, APGAx_MUX_MASK, value);
+		} else {
+			set_reg(reg, value);
+		}
+		break;
+	}
+
+	case APGA1_ADDER:
+	case APGA2_ADDER: {
+		int i;
+		enum enum_widget apga;
+		enum enum_widget apga_dst[] = {
+			widget_auxo2, widget_auxo1, widget_ear, widget_spkr,
+			widget_line2, widget_line1
+		};
+
+		apga = adder_sink_translate(reg);
+		oldval = read_reg(reg);
+		diff = value ^ oldval;
+		for (i = 0; diff; i++) {
+			if (!(diff & 1 << i))
+				continue;
+			diff ^= 1 << i;
+			update_widgets_link(value >> i & 1, apga, apga_dst[i],
+					    reg, 1 << i, value);
+		}
+		break;
+	}
+
+	case EAR_ADDER:
+	case AUXO1_ADDER:
+	case AUXO2_ADDER:
+	case SPKR_ADDER:
+	case LINE1_ADDER:
+	case LINE2_ADDER: {
+		int i;
+		enum enum_widget widgets[] = {
+			widget_dac1, widget_dac2, widget_dac3,
+		};
+		oldval = read_reg(reg);
+		diff = value ^ oldval;
+		for (i = 0; diff; i++) {
+			if (!(diff & 1 << i))
+				continue;
+			diff ^= 1 << i;
+			update_widgets_link(value >> i & 1, widgets[i],
+					    adder_sink_translate(reg),
+					    reg, 1 << i, value);
+		}
+		break;
+	}
+
+	default:
+		set_reg(reg, value);
+	}
+	return 0;
+}
+
+static unsigned int ab3550_codec_read_reg(struct snd_soc_codec *codec,
+				    unsigned int reg)
+{
+	return read_reg(reg);
+}
+
+static struct snd_soc_codec_driver ab3550_codec_drv = {
+	.probe =	ab3550_codec_probe,
+	.remove =	ab3550_codec_remove,
+	.suspend =	ab3550_codec_suspend,
+	.resume =	ab3550_codec_resume,
+	.read =		ab3550_codec_read_reg,
+	.write =	ab3550_codec_write_reg,
+};
+EXPORT_SYMBOL_GPL(ab3550_codec_drv);
+
+static inline void init_playback_route(void)
+{
+	update_widgets_link(LINK, widget_if0_dld_l, widget_rx1, 0, 0, 0);
+	update_widgets_link(LINK, widget_rx1, widget_dac1, 0, 0, 0);
+	update_widgets_link(LINK, widget_dac1, widget_spkr,
+			    SPKR_ADDER, DAC1_TO_ADDER_MASK, 0xff);
+
+	update_widgets_link(LINK, widget_if0_dld_r, widget_rx2,
+			    RX2, RX2_IF_SELECT_MASK, 0);
+	update_widgets_link(LINK, widget_rx2, widget_dac2, 0, 0, 0);
+	update_widgets_link(LINK, widget_dac2, widget_ear,
+			    EAR_ADDER, DAC2_TO_ADDER_MASK, 0xff);
+}
+
+static inline void init_capture_route(void)
+{
+	update_widgets_link(LINK, widget_micbias2, widget_mic1p1,
+			    0, 0, 0);
+	update_widgets_link(LINK, widget_micbias2, widget_mic1n1,
+			    0, 0, 0);
+	update_widgets_link(LINK, widget_mic1p1, widget_mic1,
+			    MIC1_INPUT_SELECT, MICxP1_SEL_MASK, 0xff);
+	update_widgets_link(LINK, widget_mic1n1, widget_mic1,
+			    MIC1_INPUT_SELECT, MICxN1_SEL_MASK, 0xff);
+	update_widgets_link(LINK, widget_mic1, widget_adc1,
+			    0, 0, 0);
+	update_widgets_link(LINK, widget_adc1, widget_tx1,
+			    0, 0, 0);
+	update_widgets_link(LINK, widget_tx1, widget_if0_uld_l,
+			    INTERFACE0_DATA, I2Sx_L_DATA_MASK,
+			    I2Sx_L_DATA_TX1_MASK);
+	update_widgets_link(LINK, widget_tx1, widget_if0_uld_r,
+			    INTERFACE0_DATA, I2Sx_R_DATA_MASK,
+			    I2Sx_R_DATA_TX1_MASK);
+}
+
+static inline void init_playback_gain(void)
+{
+	mask_set_reg(RX1_DIGITAL_PGA, RXx_PGA_GAIN_MASK,
+		     0x40 << RXx_PGA_GAIN_SHIFT);
+	mask_set_reg(RX2_DIGITAL_PGA, RXx_PGA_GAIN_MASK,
+		     0x40 << RXx_PGA_GAIN_SHIFT);
+	mask_set_reg(EAR, EAR_GAIN_MASK, 0x06 << EAR_GAIN_SHIFT);
+	mask_set_reg(SPKR, SPKR_GAIN_MASK, 0x6 << SPKR_GAIN_SHIFT);
+}
+
+static inline void init_capture_gain(void)
+{
+	mask_set_reg(MIC1_GAIN, MICx_GAIN_MASK, 0x06 << MICx_GAIN_SHIFT);
+	mask_set_reg(TX_DIGITAL_PGA1, TXDPGAx_MASK, 0x0f << TXDPGAx_SHIFT);
+}
+
+static __devinit int ab3550_codec_drv_probe(struct platform_device *pdev)
+{
+	struct ab3550_codec_dai_data *codec_drvdata;
+	int ret = 0;
+	u8 reg;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	pr_info("%s: Init codec private data.\n", __func__);
+	codec_drvdata = kzalloc(sizeof(struct ab3550_codec_dai_data), GFP_KERNEL);
+	if (codec_drvdata == NULL)
+		return -ENOMEM;
+
+	/* TODO: Add private data to codec_drvdata */
+
+	platform_set_drvdata(pdev, codec_drvdata);
+
+	pr_info("%s: Register codec.\n", __func__);
+	ret = snd_soc_register_codec(&pdev->dev, &ab3550_codec_drv, &ab3550_dai_drv[0], 2);
+	if (ret < 0) {
+		pr_debug("%s: Error: Failed to register codec (ret = %d).\n",
+			__func__,
+			ret);
+		snd_soc_unregister_codec(&pdev->dev);
+		kfree(platform_get_drvdata(pdev));
+		return ret;
+	}
+
+	ab3550_dev = &pdev->dev;
+	/* Initialize the codec registers */
+	for (reg = AB3550_FIRST_REG; reg <= AB3550_LAST_REG; reg++)
+		set_reg(reg, 0);
+
+	mask_set_reg(CLOCK, CLOCK_REF_SELECT_MASK | CLOCK_ENABLE_MASK,
+		     1 << CLOCK_REF_SELECT_SHIFT | 1 << CLOCK_ENABLE_SHIFT);
+	init_playback_route();
+	init_playback_gain();
+	init_capture_route();
+	init_capture_gain();
+	memset(&pm_stack, 0, sizeof(pm_stack));
+
+	return 0;
+}
+
+static int __devexit ab3550_codec_drv_remove(struct platform_device *pdev)
+{
+	mask_set_reg(CLOCK, CLOCK_ENABLE_MASK, 0);
+
+	ab3550_dev = NULL;
+
+	snd_soc_unregister_codec(&pdev->dev);
+	kfree(platform_get_drvdata(pdev));
+
+	return 0;
+}
+
+static int ab3550_codec_drv_suspend(struct platform_device *pdev,
+				   pm_message_t state)
+{
+	return 0;
+}
+
+static int ab3550_codec_drv_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver ab3550_codec_platform_drv = {
+	.driver = {
+		.name = "ab3550-codec",
+		.owner = THIS_MODULE,
+	},
+	.probe = ab3550_codec_drv_probe,
+	.remove = __devexit_p(ab3550_codec_drv_remove),
+	.suspend	= ab3550_codec_drv_suspend,
+	.resume		= ab3550_codec_drv_resume,
+};
+
+
+static int __devinit ab3550_codec_platform_drv_init(void)
+{
+	int ret;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	ab3550_dev = NULL;
+
+	ret = platform_driver_register(&ab3550_codec_platform_drv);
+	if (ret != 0)
+		pr_err("Failed to register AB3550 platform driver (%d)!\n", ret);
+
+	return ret;
+}
+
+static void __exit ab3550_codec_platform_drv_exit(void)
+{
+	pr_debug("%s: Enter.\n", __func__);
+
+	platform_driver_unregister(&ab3550_codec_platform_drv);
+}
+
+
+module_init(ab3550_codec_platform_drv_init);
+module_exit(ab3550_codec_platform_drv_exit);
+
+MODULE_DESCRIPTION("AB3550 Codec driver");
+MODULE_AUTHOR("Xie Xiaolei <xie.xiaolei@stericsson.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/sound/soc/codecs/ab3550.h b/sound/soc/codecs/ab3550.h
new file mode 100644
index 00000000000..fe9c77b1a62
--- /dev/null
+++ b/sound/soc/codecs/ab3550.h
@@ -0,0 +1,333 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Xie Xiaolei <xie.xiaolei@etericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 AB3550_CODEC_REGISTERS_H
+#define AB3550_CODEC_REGISTERS_H
+
+#define AB3550_SUPPORTED_RATE (SNDRV_PCM_RATE_8000 | SNDRV_PCM_RATE_16000 | \
+			       SNDRV_PCM_RATE_44100 | SNDRV_PCM_RATE_48000)
+
+#define AB3550_SUPPORTED_FMT (SNDRV_PCM_FMTBIT_S16_LE | \
+			      SNDRV_PCM_FMTBIT_S24_LE)
+
+/* MIC BIAS */
+
+#define MIC_BIAS1 0X31
+#define MIC_BIAS2 0X32
+#define MBIAS2_OUT_V_MASK 0x04
+#define MBIAS2_OUT_V_SHIFT 2
+#define MBIAS_PWR_MASK 0x02
+#define MBIAS_PWR_SHIFT 1
+#define MBIAS_PDN_IMP_MASK 0x01
+#define MBIAS_PDN_IMP_SHIFT 0
+
+#define MIC_BIAS2_VAD 0x33
+#define MBIAS2_R_INT_MASK 0x01
+#define MBIAS2_R_INT_SHIFT 0
+
+/* MIC */
+#define MIC1_GAIN 0x34
+#define MIC2_GAIN 0x35
+#define MICx_GAIN_MASK 0xF0
+#define MICx_GAIN_SHIFT 4
+#define MICx_IN_IMP_MASK 0x0C
+#define MICx_IN_IMP_SHIFT 2
+#define MICx_PWR_MASK 0x01
+#define MICx_PWR_SHIFT 0
+
+#define MIC1_INPUT_SELECT 0x36
+#define MIC2_INPUT_SELECT 0x37
+#define MICxP1_SEL_MASK 0x80
+#define MICxP1_SEL_SHIFT 7
+#define MICxN1_SEL_MASK 0x40
+#define MICxN1_SEL_SHIFT 6
+#define MICxP2_SEL_MASK 0x20
+#define MICxP2_SEL_SHIFT 5
+#define MICxN2_SEL_MASK 0x10
+#define MICxN2_SEL_SHIFT 4
+#define LINEIN_SEL_MASK 0x03
+#define LINEIN_SEL_SHIFT 0
+
+#define MIC1_VMID_SELECT 0x38
+#define MIC2_VMID_SELECT 0x39
+#define VMIDx_ENABLE_MASK 0xC0
+#define VMIDx_ENABLE_SHIFT 6
+#define VMIDx_LINEIN1_N_MASK 0x20
+#define VMIDx_LINEIN1_N_SHIFT 5
+#define VMIDx_LINEIN2_N_MASK 0x10
+#define VMIDx_LINEIN2_N_SHIFT 4
+#define VMIDx_MICxP1_MASK  0x08
+#define VMIDx_MICxP1_SHIFT 3
+#define VMIDx_MICxP2_MASK  0x04
+#define VMIDx_MICxP2_SHIFT 2
+#define VMIDx_MICxN1_MASK  0x02
+#define VMIDx_MICxN1_SHIFT 1
+#define VMIDx_MICxN2_MASK  0x01
+#define VMIDx_MICxN2_SHIFT 0
+
+#define MIC2_TO_MIC1 0x3A
+#define MIC2_TO_MIC1_MASK 0x03
+#define MIC2_TO_MIC1_SHIFT 0
+
+/* Analog Loop */
+#define ANALOG_LOOP_PGA1 0x3B
+#define ANALOG_LOOP_PGA2 0x3C
+#define APGAx_GAIN_MASK 0xF8
+#define APGAx_GAIN_SHIFT 3
+#define APGAx_PWR_MASK 0x04
+#define APGAx_PWR_SHIFT 2
+#define APGAx_MUX_MASK 0x03
+#define APGAx_MUX_SHIFT 0
+#define APGAx_MUX_MIC1_MASK 0x01
+#define APGAx_MUX_MIC1_SHIFT 0
+#define APGAx_MUX_MIC2_MASK 0x02
+#define APGAx_MUX_MIC2_SHIFT 1
+
+
+#define APGA_VMID_SELECT 0x3D
+#define VMID_APGA1_ENABLE_MASK 0xC0
+#define VMID_APGA1_ENABLE_SHIFT 6
+#define VMID_APGA1_LINEIN1_MASK 0x20
+#define VMID_APGA1_LINEIN1_SHIFT 5
+#define VMID_APGA2_ENABLE_MASK 0x0C
+#define VMID_APGA2_ENABLE_SHIFT 2
+#define VMID_APGA2_LINEIN2_MASK 0x02
+#define VMID_APGA2_LINEIN2_SHIFT 1
+
+/* Output Amplifiers */
+#define EAR 0x3E
+#define EAR_PWR_MODE_MASK 0x20
+#define EAR_PWR_MODE_SHIFT 5
+#define EAR_PWR_MASK 0x10
+#define EAR_PWR_SHIFT 4
+#define EAR_GAIN_MASK 0x0F
+#define EAR_GAIN_SHIFT 0
+
+#define AUXO1 0x3F
+#define AUXO2 0x40
+#define AUXOx_PWR_MASK 0x80
+#define AUXOx_PWR_SHIFT 7
+#define AUXOx_INV_MASK 0x40
+#define AUXOx_INV_SHIFT 6
+#define AUXOx_PULLDOWN_MASK 0x20
+#define AUXOx_PULLDOWN_SHIFT 5
+#define AUXOx_GAIN_MASK 0x0F
+#define AUXOx_GAIN_SHIFT 0
+
+#define AUXO_PWR_MODE 0x41
+#define AUT_PWR_MODE_MASK 0x04
+#define AUT_PWR_MODE_SHIFT 2
+#define AUXO_PWR_MODE_MASK 0x03
+#define AUXO_PWR_MODE_SHIFT 0
+
+#define OFFSET_CANCEL 0x42
+#define SPKR_OFF_CANC_MASK 0x04
+#define SPKR_OFF_CANC_SHIFT 2
+#define AUXO_OFF_CANC_MASK 0x02
+#define AUXO_OFF_CANC_SHIFT 1
+#define OFFSET_CLOCK_MASK 0x01
+#define OFFSET_CLOCK_SHIFT 0
+
+#define SPKR 0x43
+#define OVR_CURR_PROT_MASK 0x80
+#define OVR_CURR_PROT_SHIFT 7
+#define SPKR_PWR_MASK 0x40
+#define SPKR_PWR_SHIFT 6
+#define SPKR_GAIN_MASK 0x1F
+#define SPKR_GAIN_SHIFT 0
+
+#define LINE1 0x44
+#define LINE2 0x45
+#define LINEx_PWR_MASK 0x80
+#define LINEx_PWR_SHIFT 7
+#define LINEx_INV_MASK 0x40
+#define LINEx_INV_SHIFT 6
+#define VMID_BUFFx_MASK 0x10
+#define VMID_BUFFx_SHIFT 4
+#define LINEx_GAIN_MASK 0x0F
+#define LINEx_GAIN_SHIFT 0
+
+/* Analog loop Routing */
+
+#define APGA1_ADDER 0x46
+#define APGA2_ADDER 0x47
+#define APGAx_TO_LINE1_MASK 0x20
+#define APGAx_TO_LINE1_SHIFT 0x5F
+#define APGAx_TO_LINE2_MASK 0x10
+#define APGAx_TO_LINE2_SHIFT 4
+#define APGAx_TO_SPKR_MASK 0x08
+#define APGAx_TO_SPKR_SHIFT 3
+#define APGAx_TO_EAR_MASK 0x04
+#define APGAx_TO_EAR_SHIFT 2
+#define APGAx_TO_AUXO1_MASK 0x02
+#define APGAx_TO_AUXO1_SHIFT 1
+#define APGAx_TO_AUXO2_MASK 0x01
+#define APGAx_TO_AUXO2_SHIFT 0
+#define APGAx_ADDER_VALID_BITS_MASK 0x3F
+
+/* Output Amplifiers Routing */
+
+#define EAR_ADDER 0x48
+#define AUXO1_ADDER 0x49
+#define AUXO2_ADDER 0x4A
+#define SPKR_ADDER 0x4B
+#define LINE1_ADDER 0x4C
+#define LINE2_ADDER 0x4D
+#define DAC3_TO_ADDER_MASK 0x04
+#define DAC3_TO_ADDER_SHIFT 2
+#define DAC2_TO_ADDER_MASK 0x02
+#define DAC2_TO_ADDER_SHIFT 1
+#define DAC1_TO_ADDER_MASK 0x01
+#define DAC1_TO_ADDER_SHIFT 0
+
+#define EAR_TO_MIC2 0x4E
+#define EAR_TO_MIC2_MASK 0x01
+#define EAR_TO_MIC2_SHIFT 0
+
+#define SPKR_TO_MIC2 0x4F
+#define SPKR_TO_MIC2_MASK 0x01
+#define SPKR_TO_MIC2_SHIFT 0
+
+#define NEGATIVE_CHARGE_PUMP 0x50
+#define NCP_MODE_MASK 0x02
+#define NCP_MODE_SHIFT 1
+#define NCP_PWR_MASK 0x01
+#define NCP_PWR_SHIFT 0
+
+#define TX1 0x51
+#define TX2 0x52
+#define TXx_HP_FILTER_MASK 0x0C
+#define TXx_HP_FILTER_SHIFT 2
+#define TXx_PWR_MASK 0x02
+#define TXx_PWR_SHIFT 1
+#define ADCx_PWR_MASK 0x01
+#define ADCx_PWR_SHIFT 0
+
+#define RX1 0x53
+#define RX2 0x54
+#define RX2_IF_SELECT_MASK 0x10
+#define RX2_IF_SELECT_SHIFT 4
+#define RX3 0x55
+#define RXx_PWR_MASK 0x08
+#define RXx_PWR_SHIFT 3
+#define DACx_PWR_MASK 0x04
+#define DACx_PWR_SHIFT 2
+#define DACx_PWR_MODE_MASK 0x03
+#define DACx_PWR_MODE_SHIFT 0
+
+#define TX_DIGITAL_PGA1 0X56
+#define TX_DIGITAL_PGA2 0X57
+#define TXDPGAx_MASK 0x0F
+#define TXDPGAx_SHIFT 0
+
+#define RX1_DIGITAL_PGA 0x58
+#define RX2_DIGITAL_PGA 0x59
+#define RX3_DIGITAL_PGA 0x5A
+#define RXx_PGA_GAIN_MASK 0x7F
+#define RXx_PGA_GAIN_SHIFT 0
+
+#define SIDETONE1_PGA 0x5B
+#define SIDETONE2_PGA 0x5C
+#define STx_HP_FILTER_MASK 0x60
+#define STx_HP_FILTER_SHIFT 5
+#define STx_MUX_MASK 0x10
+#define STx_MUX_SHIFT 4
+#define STx_PGA_MASK 0x0F
+#define STx_PGA_SHIFT 0
+
+/* clock */
+
+#define CLOCK 0x5D
+#define CLOCK_REF_SELECT_MASK 0x02
+#define CLOCK_REF_SELECT_SHIFT 1
+#define CLOCK_ENABLE_MASK 0x01
+#define CLOCK_ENABLE_SHIFT 0
+
+/* Interface */
+
+#define INTERFACE0 0x5E
+#define INTERFACE1 0x60
+#define I2Sx_WORDLENGTH_MASK 0x40
+#define I2Sx_WORDLENGTH_SHIFT 6
+#define MASTER_GENx_PWR_MASK 0x20
+#define MASTER_GENx_PWR_SHIFT 5
+#define I2Sx_MODE_MASK 0x10
+#define I2Sx_MODE_SHIFT 4
+#define I2Sx_TRISTATE_MASK 0x08
+#define I2Sx_TRISTATE_SHIFT 3
+#define I2Sx_PULLDOWN_MASK 0x04
+#define I2Sx_PULLDOWN_SHIFT 2
+#define I2Sx_SR_MASK 0x03
+#define I2Sx_SR_SHIFT 0
+#define I2Sx_SR_8000Hz 0
+#define I2Sx_SR_16000Hz 1
+#define I2Sx_SR_44100Hz 2
+#define I2Sx_SR_48000Hz 3
+
+#define INTERFACE0_DATA 0x5F
+#define INTERFACE1_DATA 0x61
+#define I2Sx_L_DATA_MASK 0x0C
+#define I2Sx_L_DATA_TX1_MASK 0x04
+#define I2Sx_L_DATA_TX2_MASK 0x08
+#define I2Sx_L_DATA_SHIFT 2
+#define I2Sx_R_DATA_MASK 0x03
+#define I2Sx_R_DATA_TX1_MASK 0x01
+#define I2Sx_R_DATA_TX2_MASK 0x02
+#define I2Sx_R_DATA_SHIFT 0
+
+#define INTERFACE_LOOP 0x62
+#define I2S0_INT_LOOP_MASK 0x08
+#define I2S0_INT_LOOP_SHIFT 3
+#define I2S0_EXT_LOOP_MASK 0x04
+#define I2S0_EXT_LOOP_SHIFT 2
+#define I2S1_INT_LOOP_MASK 0x02
+#define I2S1_INT_LOOP_SHIFT 1
+#define I2S1_EXT_LOOP_MASK 0x01
+#define I2S1_EXT_LOOP_SHIFT 0
+
+#define INTERFACE_SWAP 0x63
+#define RX_SWAP0_MASK 0x10
+#define RX_SWAP0_SHIFT 4
+#define RX_SWAP1_MASK 0x08
+#define RX_SWAP1_SHIFT 3
+#define IF_SWAP_MASK 0x04
+#define IF_SWAP_SHIFT 2
+#define IO_SWAP0_MASK 0x02
+#define IO_SWAP0_SHIFT 1
+#define IO_SWAP1_MASK 0x01
+#define IO_SWAP1_SHIFT 0
+
+#define AB3550_FIRST_REG MIC_BIAS1
+#define AB3550_LAST_REG INTERFACE_SWAP
+
+#define AB3550_VIRTUAL_REG1 (AB3550_LAST_REG + 1)
+#define IF0_DLD_L_PW_SHIFT 0
+#define IF0_DLD_R_PW_SHIFT 1
+#define IF0_ULD_L_PW_SHIFT 2
+#define IF0_ULD_R_PW_SHIFT 3
+#define IF1_DLD_L_PW_SHIFT 4
+#define IF1_DLD_R_PW_SHIFT 5
+#define IF1_ULD_L_PW_SHIFT 6
+#define IF1_ULD_R_PW_SHIFT 7
+
+#define AB3550_VIRTUAL_REG2 (AB3550_LAST_REG + 2)
+#define MIC1P1_PW_SHIFT 0
+#define MIC1N1_PW_SHIFT 1
+#define MIC1P2_PW_SHIFT 2
+#define MIC1N2_PW_SHIFT 3
+#define MIC2P1_PW_SHIFT 4
+#define MIC2N1_PW_SHIFT 5
+#define MIC2P2_PW_SHIFT 6
+#define MIC2N2_PW_SHIFT 7
+
+
+#endif
diff --git a/sound/soc/codecs/ab5500.c b/sound/soc/codecs/ab5500.c
new file mode 100644
index 00000000000..1875db4fc77
--- /dev/null
+++ b/sound/soc/codecs/ab5500.c
@@ -0,0 +1,1691 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Xie Xiaolei <xie.xiaolei@etericsson.com>,
+ *         Ola Lilja <ola.o.lilja@stericsson.com>,
+ *         Roger Nilsson <roger.xr.nilsson@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/pm.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <sound/initval.h>
+#include <sound/soc.h>
+#include <sound/soc-dapm.h>
+#include <linux/mfd/abx500.h>
+#include <linux/bitmap.h>
+#include <linux/bitops.h>
+#include <asm/atomic.h>
+#include <linux/rwsem.h>
+#include <linux/mutex.h>
+#include <stdarg.h>
+#include "ab5500.h"
+
+/* No of digital interface on the Codec */
+#define NO_CODEC_DAI_IF  2
+
+/* codec private data */
+struct ab5500_codec_dai_data {
+	bool playback_active;
+	bool capture_active;
+
+};
+
+static struct device *ab5500_dev;
+
+static u8 virtual_regs[] = {
+	0, 0, 0, 0, 0
+};
+
+#define set_reg(reg, val) mask_set_reg((reg), 0xff, (val))
+
+static void mask_set_reg(u8 reg, u8 mask, u8 val)
+{
+	u8 newval = mask & val;
+	u8 oldval, diff;
+
+	if (!ab5500_dev) {
+		pr_err("%s: The AB5500 codec driver not initialized.\n",
+		       __func__);
+		return;
+	}
+	/* Check if the reg value falls within the
+	 * range of AB5500 real registers. If
+	 * so, set the mask */
+	if (reg < AB5500_FIRST_REG)
+		return;
+	if (reg <= AB5500_LAST_REG) {
+		abx500_mask_and_set_register_interruptible(
+			ab5500_dev, AB5500_BANK_AUDIO_HEADSETUSB,
+			reg, mask, val);
+		return;
+	}
+	if (reg - AB5500_LAST_REG - 1 >= ARRAY_SIZE(virtual_regs))
+		return;
+
+	/* treatment of virtual registers follows */
+	/*Compute the difference between the new value and the old value.
+	 *1.If there is no difference, do nothing.
+	 *2.If the difference is in the PWR_SHIFT,
+	 *set the PWR masks appropriately.
+	 */
+	oldval = virtual_regs[reg - AB5500_LAST_REG - 1];
+	diff = (val ^ oldval) & mask;
+	if (!diff)
+		return;
+
+	switch (reg) {
+	case AB5500_VIRTUAL_REG3:
+		if ((diff & (1 << SPKR1_PWR_SHIFT))) {
+			if ((val & (1 << SPKR1_PWR_SHIFT)) == 0) {
+				/*
+				 * If the new value has PWR_SHIFT
+				 * disabled, set the
+				 * PWR_MASK to 0
+				 */
+				mask_set_reg(SPKR1, SPKRx_PWR_MASK, 0);
+			}
+			else {
+				/* Else, set the PWR_MASK values based on the old value. */
+				switch (oldval & SPKR1_MODE_MASK) {
+				case 0:
+					mask_set_reg(SPKR1, SPKRx_PWR_MASK,
+						     SPKRx_PWR_VBR_VALUE);
+					break;
+				case 1:
+					mask_set_reg(SPKR1, SPKRx_PWR_MASK,
+						     SPKRx_PWR_CLS_D_VALUE);
+					break;
+				case 2:
+					mask_set_reg(SPKR1, SPKRx_PWR_MASK,
+						     SPKRx_PWR_CLS_AB_VALUE);
+					break;
+				}
+		}
+		}
+		if ((diff & (1 << SPKR2_PWR_SHIFT))) {
+			if ((val & (1 << SPKR2_PWR_SHIFT)) == 0) {
+				/*
+				 * If the new value has PWR_SHIFT
+				 * disabled, set the
+				 * PWR_MASK to 0
+				 */
+				mask_set_reg(SPKR2, SPKRx_PWR_MASK, 0);
+			}
+			else {
+				/* Else, set the PWR_MASK values based on the old value. */
+				switch (oldval & SPKR2_MODE_MASK) {
+				case 0:
+					mask_set_reg(SPKR2, SPKRx_PWR_MASK,
+						     SPKRx_PWR_VBR_VALUE);
+					break;
+				case 1:
+					mask_set_reg(SPKR2, SPKRx_PWR_MASK,
+						     SPKRx_PWR_CLS_D_VALUE);
+					break;
+				}
+			}
+		}
+
+		break;
+	case AB5500_VIRTUAL_REG4:
+		;
+		/* configure PWMCTRL_SPKR1, PWMCTRL_SPKR2, etc. */
+	}
+	virtual_regs[reg - AB5500_LAST_REG - 1] &= ~mask;
+	virtual_regs[reg - AB5500_LAST_REG - 1] |= newval;
+}
+
+static u8 read_reg(u8 reg)
+{
+	if (!ab5500_dev) {
+		pr_err("%s: The AB5500 codec driver not initialized.\n",
+		       __func__);
+		return 0;
+	}
+	/* Check if the reg value falls within the range of AB5500 real
+	 * registers.If so, set the mask */
+	if (reg < AB5500_FIRST_REG)
+		return 0;
+	else if (reg <= AB5500_LAST_REG) {
+		u8 val;
+		abx500_get_register_interruptible(
+			ab5500_dev, AB5500_BANK_AUDIO_HEADSETUSB, reg, &val);
+		return val;
+	} else if (reg - AB5500_LAST_REG - 1 < ARRAY_SIZE(virtual_regs))
+		return virtual_regs[reg - AB5500_LAST_REG - 1];
+	dev_warn(ab5500_dev, "%s: out-of-scope reigster %u.\n",
+		 __func__, reg);
+	return 0;
+}
+
+/* Components that can be powered up/down */
+enum enum_widget {
+	widget_ear = 0,
+	widget_auxo1,
+	widget_auxo2,
+	widget_auxo3,
+	widget_auxo4,
+	widget_spkr1,
+	widget_spkr2,
+	widget_spkr1_adder,
+	widget_spkr2_adder,
+	widget_pwm_spkr1,
+	widget_pwm_spkr2,
+	widget_pwm_spkr1n,
+	widget_pwm_spkr1p,
+	widget_pwm_spkr2n,
+	widget_pwm_spkr2p,
+	widget_line1,
+	widget_line2,
+	widget_dac1,
+	widget_dac2,
+	widget_dac3,
+	widget_rx1,
+	widget_rx2,
+	widget_rx3,
+	widget_mic1,
+	widget_mic2,
+	widget_micbias1,
+	widget_micbias2,
+	widget_apga1,
+	widget_apga2,
+	widget_tx1,
+	widget_tx2,
+	widget_adc1,
+	widget_adc2,
+	widget_if0_dld_l,
+	widget_if0_dld_r,
+	widget_if0_uld_l,
+	widget_if0_uld_r,
+	widget_if1_dld_l,
+	widget_if1_dld_r,
+	widget_if1_uld_l,
+	widget_if1_uld_r,
+	widget_mic1p1,
+	widget_mic1n1,
+	widget_mic1p2,
+	widget_mic1n2,
+	widget_mic2p1,
+	widget_mic2n1,
+	widget_mic2p2,
+	widget_mic2n2,
+	widget_clock,
+	number_of_widgets
+};
+
+/* This is only meant for debugging */
+static const char *widget_names[] = {
+	"EAR", "AUXO1", "AUXO2", "AUXO3", "AUXO4",
+	"SPKR1", "SPKR2", "SPKR1_ADDER", "SPKR2_ADDER",
+	"PWM_SPKR1", "PWM_SPKR2",
+	"PWM_SPKR1N", "PWM_SPKR1P",
+	"PWM_SPKR2N", "PWM_SPKR2P",
+	"LINE1", "LINE2",
+	"DAC1", "DAC2", "DAC3",
+	"RX1", "RX2", "RX3",
+	"MIC1", "MIC2",
+	"MIC-BIAS1", "MIC-BIAS2",
+	"APGA1", "APGA2",
+	"TX1", "TX2",
+	"ADC1", "ADC2",
+	"IF0-DLD-L", "IF0-DLD-R", "IF0-ULD-L", "IF0-ULD-R",
+	"IF1-DLD-L", "IF1-DLD-R", "IF1-ULD-L", "IF1-ULD-R",
+	"MIC1P1", "MIC1N1", "MIC1P2", "MIC1N2",
+	"MIC2P1", "MIC2N1", "MIC2P2", "MIC2N2",
+	"CLOCK"
+};
+
+struct widget_pm {
+	enum enum_widget widget;
+	u8 reg;
+	u8 shift;
+
+	unsigned long source_list[BIT_WORD(number_of_widgets) + 1];
+	unsigned long sink_list[BIT_WORD(number_of_widgets) + 1];
+};
+
+static struct widget_pm widget_pm_array[] = {
+	{.widget = widget_ear, .reg = EAR_PWR, .shift = EAR_PWR_SHIFT},
+
+	{.widget = widget_auxo1, .reg = AUXO1, .shift = AUXOx_PWR_SHIFT},
+	{.widget = widget_auxo2, .reg = AUXO2, .shift = AUXOx_PWR_SHIFT},
+	{.widget = widget_auxo3, .reg = AUXO3, .shift = AUXOx_PWR_SHIFT},
+	{.widget = widget_auxo4, .reg = AUXO4, .shift = AUXOx_PWR_SHIFT},
+
+	{.widget = widget_spkr1, .reg = AB5500_VIRTUAL_REG3,
+	 .shift = SPKR1_PWR_SHIFT},
+	{.widget = widget_spkr2, .reg = AB5500_VIRTUAL_REG3,
+	 .shift = SPKR2_PWR_SHIFT},
+
+	{.widget = widget_spkr1_adder, .reg = AB5500_VIRTUAL_REG3,
+	 .shift = SPKR1_ADDER_PWR_SHIFT},
+	{.widget = widget_spkr2_adder, .reg = AB5500_VIRTUAL_REG3,
+	 .shift = SPKR2_ADDER_PWR_SHIFT},
+
+	{.widget = widget_pwm_spkr1, .reg = AB5500_VIRTUAL_REG4,
+	 .shift = PWM_SPKR1_PWR_SHIFT},
+	{.widget = widget_pwm_spkr2, .reg = AB5500_VIRTUAL_REG4,
+	 .shift = PWM_SPKR2_PWR_SHIFT},
+
+	{.widget = widget_pwm_spkr1n, .reg = AB5500_VIRTUAL_REG4,
+	 .shift = PWM_SPKR1N_PWR_SHIFT},
+	{.widget = widget_pwm_spkr1p, .reg = AB5500_VIRTUAL_REG4,
+	 .shift = PWM_SPKR1P_PWR_SHIFT},
+
+	{.widget = widget_pwm_spkr2n, .reg = AB5500_VIRTUAL_REG4,
+	 .shift = PWM_SPKR2N_PWR_SHIFT},
+	{.widget = widget_pwm_spkr2p, .reg = AB5500_VIRTUAL_REG4,
+	 .shift = PWM_SPKR2P_PWR_SHIFT},
+
+
+	{.widget = widget_line1, .reg = LINE1, .shift = LINEx_PWR_SHIFT},
+	{.widget = widget_line2, .reg = LINE2, .shift = LINEx_PWR_SHIFT},
+
+	{.widget = widget_dac1, .reg = RX1, .shift = DACx_PWR_SHIFT},
+	{.widget = widget_dac2, .reg = RX2, .shift = DACx_PWR_SHIFT},
+	{.widget = widget_dac3, .reg = RX3, .shift = DACx_PWR_SHIFT},
+
+	{.widget = widget_rx1, .reg = RX1, .shift = RXx_PWR_SHIFT},
+	{.widget = widget_rx2, .reg = RX2, .shift = RXx_PWR_SHIFT},
+	{.widget = widget_rx3, .reg = RX3, .shift = RXx_PWR_SHIFT},
+
+	{.widget = widget_mic1, .reg = MIC1_GAIN, .shift = MICx_PWR_SHIFT},
+	{.widget = widget_mic2, .reg = MIC2_GAIN, .shift = MICx_PWR_SHIFT},
+
+	{.widget = widget_micbias1, .reg = MIC_BIAS1,
+	 .shift = MBIASx_PWR_SHIFT},
+	{.widget = widget_micbias2, .reg = MIC_BIAS2,
+	 .shift = MBIASx_PWR_SHIFT},
+
+	{.widget = widget_apga1, .reg = ANALOG_LOOP_PGA1,
+	 .shift = APGAx_PWR_SHIFT},
+	{.widget = widget_apga2, .reg = ANALOG_LOOP_PGA2,
+	 .shift = APGAx_PWR_SHIFT},
+
+	{.widget = widget_tx1, .reg = TX1, .shift = TXx_PWR_SHIFT},
+	{.widget = widget_tx2, .reg = TX2, .shift = TXx_PWR_SHIFT},
+
+	{.widget = widget_adc1, .reg = TX1, .shift = ADCx_PWR_SHIFT},
+	{.widget = widget_adc2, .reg = TX2, .shift = ADCx_PWR_SHIFT},
+
+	{.widget = widget_if0_dld_l, .reg = AB5500_VIRTUAL_REG1,
+	 .shift = IF0_DLD_L_PW_SHIFT},
+	{.widget = widget_if0_dld_r, .reg = AB5500_VIRTUAL_REG1,
+	 .shift = IF0_DLD_R_PW_SHIFT},
+	{.widget = widget_if0_uld_l, .reg = AB5500_VIRTUAL_REG1,
+	 .shift = IF0_ULD_L_PW_SHIFT},
+	{.widget = widget_if0_uld_r, .reg = AB5500_VIRTUAL_REG1,
+	 .shift = IF0_ULD_R_PW_SHIFT},
+
+	{.widget = widget_if1_dld_l, .reg = AB5500_VIRTUAL_REG1,
+	 .shift = IF1_DLD_L_PW_SHIFT},
+	{.widget = widget_if1_dld_r, .reg = AB5500_VIRTUAL_REG1,
+	 .shift = IF1_DLD_R_PW_SHIFT},
+	{.widget = widget_if1_uld_l, .reg = AB5500_VIRTUAL_REG1,
+	 .shift = IF1_ULD_L_PW_SHIFT},
+	{.widget = widget_if1_uld_r, .reg = AB5500_VIRTUAL_REG1,
+	 .shift = IF1_ULD_R_PW_SHIFT},
+
+	{.widget = widget_mic1p1, .reg = AB5500_VIRTUAL_REG2,
+	 .shift = MIC1P1_PW_SHIFT},
+	{.widget = widget_mic1n1, .reg = AB5500_VIRTUAL_REG2,
+	 .shift = MIC1N1_PW_SHIFT},
+	{.widget = widget_mic1p2, .reg = AB5500_VIRTUAL_REG2,
+	 .shift = MIC1P2_PW_SHIFT},
+	{.widget = widget_mic1n2, .reg = AB5500_VIRTUAL_REG2,
+	 .shift = MIC1N2_PW_SHIFT},
+
+	{.widget = widget_mic2p1, .reg = AB5500_VIRTUAL_REG2,
+	 .shift = MIC2P1_PW_SHIFT},
+	{.widget = widget_mic2n1, .reg = AB5500_VIRTUAL_REG2,
+	 .shift = MIC2N1_PW_SHIFT},
+	{.widget = widget_mic2p2, .reg = AB5500_VIRTUAL_REG2,
+	 .shift = MIC2P2_PW_SHIFT},
+	{.widget = widget_mic2n2, .reg = AB5500_VIRTUAL_REG2,
+	 .shift = MIC2N2_PW_SHIFT},
+
+	{.widget = widget_clock, .reg = CLOCK, .shift = CLOCK_ENABLE_SHIFT},
+};
+
+DEFINE_MUTEX(ab5500_pm_mutex);
+
+static struct {
+	enum enum_widget stack[number_of_widgets];
+	int p;
+} pm_stack;
+
+#define pm_stack_as_bitmap ({						\
+			unsigned long bitmap[BIT_WORD(number_of_widgets) + 1]; \
+			int i;						\
+			memset(bitmap, 0, sizeof(bitmap));		\
+			for (i = 0; i < pm_stack.p; i++) {		\
+				set_bit(pm_stack.stack[i], bitmap);	\
+			}						\
+			bitmap;						\
+		})
+
+/* These are only meant to meet the obligations of DAPM */
+static const struct snd_soc_dapm_widget ab5500_dapm_widgets[] = {
+};
+
+static const struct snd_soc_dapm_route intercon[] = {
+};
+
+
+struct ab5500_codec_dai_data  ab5500_codec_privates[NO_CODEC_DAI_IF] = {
+	{
+		.playback_active = false,
+		.capture_active = false,
+	},
+	{
+		.playback_active = false,
+		.capture_active = false,
+	}
+};
+
+static const char *enum_rx_input_select[] = {
+	"Mute", "TX1", "TX2", "I2S0_DLD_L",
+	"I2S0_DLD_R", "I2S1_DLD_L", "I2S1_DLD_R"
+};
+
+static const char *enum_i2s_uld_select[] = {
+	"Mute", "TX1", "TX2", "I2S0_DLD_L",
+	"I2S0_DLD_R", "I2S1_DLD_L", "I2S1_DLD_R", "tri-state"
+};
+static const char *enum_apga1_source[] = {"LINEIN1", "MIC1", "MIC2", "None"};
+static const char *enum_apga2_source[] = {"LINEIN2", "MIC1", "MIC2", "None"};
+static const char *enum_rx_side_tone[] = {"TX1", "TX2"};
+static const char *enum_dac_power_mode[] = {"100%", "75%", "55%"};
+static const char *enum_ear_power_mode[] = {"100%", "70%", "50%"};
+static const char *enum_auxo_power_mode[] = {
+	"100%", "67%", "50%", "25%", "auto"
+};
+static const char *enum_onoff[] = {"Off", "On"};
+static const char *enum_mbias_pdn_imp[] = {"GND", "HiZ"};
+static const char *enum_mbias2_out_v[] = {"2.0v", "2.2v"};
+static const char *enum_mic_in_imp[] = {
+	"12.5 kohm", "25 kohm", "50 kohm"
+};
+static const char *enum_hp_filter[] = {"HP3", "HP1", "bypass"};
+static const char *enum_i2s_word_length[] = {"16 bits", "24 bits"};
+static const char *enum_i2s_mode[] = {"Master Mode", "Slave Mode"};
+static const char *enum_i2s_tristate[] = {"Normal", "Tri-state"};
+static const char *enum_optional_resistor[] = {"disconnected", "connected"};
+static const char *enum_i2s_sample_rate[] = {
+	"8 kHz", "16 kHz", "44.1 kHz", "48 kHz"
+};
+static const char *enum_tx1_input_select[] = {
+	"ADC1", "DIGMIC1", "DIGMIC2"
+};
+static const char *enum_tx2_input_select[] = {
+	"ADC2", "DIGMIC1", "DIGMIC2"
+};
+static const char *enum_signal_inversion[] = {"normal", "inverted"};
+static const char *enum_spkr1_mode[] = {
+	"Vibra PWM", "class D amplifier", "class AB amplifier"
+};
+static const char *enum_spkr2_mode[] = {
+	"Vibra PWM", "class D amplifier",
+};
+static const char *enum_pwm_pol[] = {
+	"GND", "VDD"
+};
+/* RX1 Input Select */
+static struct soc_enum soc_enum_rx1_in_sel =
+	SOC_ENUM_SINGLE(RX1, RXx_DATA_SHIFT,
+			ARRAY_SIZE(enum_rx_input_select),
+			enum_rx_input_select);
+
+/* RX2 Input Select */
+static struct soc_enum soc_enum_rx2_in_sel =
+	SOC_ENUM_SINGLE(RX2, RXx_DATA_SHIFT,
+			ARRAY_SIZE(enum_rx_input_select),
+			enum_rx_input_select);
+/* RX3 Input Select */
+static struct soc_enum soc_enum_rx3_in_sel =
+	SOC_ENUM_SINGLE(RX3, RXx_DATA_SHIFT,
+			ARRAY_SIZE(enum_rx_input_select),
+			enum_rx_input_select);
+/* TX1 Input Select */
+static struct soc_enum soc_enum_tx1_in_sel =
+	SOC_ENUM_SINGLE(TX1, TXx_MUX_SHIFT,
+			ARRAY_SIZE(enum_tx1_input_select),
+			enum_tx1_input_select);
+/* TX2 Input Select */
+static struct soc_enum soc_enum_tx2_in_sel =
+	SOC_ENUM_SINGLE(TX2, TXx_MUX_SHIFT,
+			ARRAY_SIZE(enum_tx2_input_select),
+			enum_tx2_input_select);
+
+/* I2S0 ULD Select */
+static struct soc_enum soc_enum_i2s0_input_select =
+	SOC_ENUM_DOUBLE(INTERFACE0_ULD, 0, 4,
+			ARRAY_SIZE(enum_i2s_uld_select),
+			enum_i2s_uld_select);
+/* I2S1 ULD Select */
+static struct soc_enum soc_enum_i2s1_input_select =
+	SOC_ENUM_DOUBLE(INTERFACE1_ULD, 0, 4,
+			ARRAY_SIZE(enum_i2s_uld_select),
+			enum_i2s_uld_select);
+
+/* APGA1 Source */
+static struct soc_enum soc_enum_apga1_source =
+	SOC_ENUM_SINGLE(ANALOG_LOOP_PGA1, APGAx_MUX_SHIFT,
+			ARRAY_SIZE(enum_apga1_source),
+			enum_apga1_source);
+
+/* APGA2 Source */
+static struct soc_enum soc_enum_apga2_source =
+	SOC_ENUM_SINGLE(ANALOG_LOOP_PGA2, APGAx_MUX_SHIFT,
+			ARRAY_SIZE(enum_apga2_source),
+			enum_apga2_source);
+
+static struct soc_enum soc_enum_apga1_enable =
+	SOC_ENUM_SINGLE(ANALOG_LOOP_PGA1, APGAx_PWR_SHIFT,
+			ARRAY_SIZE(enum_onoff), enum_onoff);
+
+static struct soc_enum soc_enum_apga2_enable =
+	SOC_ENUM_SINGLE(ANALOG_LOOP_PGA2, APGAx_PWR_SHIFT,
+			ARRAY_SIZE(enum_onoff), enum_onoff);
+
+/* RX1 Side Tone */
+static struct soc_enum soc_enum_dac1_side_tone =
+	SOC_ENUM_SINGLE(ST1_PGA, STx_MUX_SHIFT,
+			ARRAY_SIZE(enum_rx_side_tone),
+			enum_rx_side_tone);
+
+/* RX2 Side Tone */
+static struct soc_enum soc_enum_dac2_side_tone =
+	SOC_ENUM_SINGLE(ST2_PGA, STx_MUX_SHIFT,
+			ARRAY_SIZE(enum_rx_side_tone),
+			enum_rx_side_tone);
+
+/* DAC1 Power Mode */
+static struct soc_enum soc_enum_dac1_power_mode =
+	SOC_ENUM_SINGLE(RX1, DACx_PWR_MODE_SHIFT,
+			ARRAY_SIZE(enum_dac_power_mode),
+			enum_dac_power_mode);
+
+/* DAC2 Power Mode */
+static struct soc_enum soc_enum_dac2_power_mode =
+	SOC_ENUM_SINGLE(RX2, DACx_PWR_MODE_SHIFT,
+			ARRAY_SIZE(enum_dac_power_mode),
+			enum_dac_power_mode);
+
+/* DAC3 Power Mode */
+static struct soc_enum soc_enum_dac3_power_mode =
+	SOC_ENUM_SINGLE(RX3, DACx_PWR_MODE_SHIFT,
+			ARRAY_SIZE(enum_dac_power_mode),
+			enum_dac_power_mode);
+
+/* EAR Power Mode */
+static struct soc_enum soc_enum_ear_power_mode =
+	SOC_ENUM_SINGLE(EAR_PWR, EAR_PWR_MODE_SHIFT,
+			ARRAY_SIZE(enum_ear_power_mode),
+			enum_ear_power_mode);
+
+/* AUXO12 Power Mode */
+static struct soc_enum soc_enum_auxo12_power_mode =
+	SOC_ENUM_SINGLE(AUXO12_PWR_MODE, AUXOxy_PWR_MODE_SHIFT,
+			ARRAY_SIZE(enum_auxo_power_mode),
+			enum_auxo_power_mode);
+
+/* AUXO34 Power Mode */
+static struct soc_enum soc_enum_auxo34_power_mode =
+	SOC_ENUM_SINGLE(AUXO34_PWR_MODE, AUXOxy_PWR_MODE_SHIFT,
+			ARRAY_SIZE(enum_auxo_power_mode),
+			enum_auxo_power_mode);
+
+/* MBIAS1 PDN Impedance */
+static struct soc_enum soc_enum_mbias1_pdn_imp =
+	SOC_ENUM_SINGLE(MIC_BIAS1, MBIASx_PDN_IMP_SHIFT,
+			ARRAY_SIZE(enum_mbias_pdn_imp),
+			enum_mbias_pdn_imp);
+
+/* MBIAS2 PDN Impedance */
+static struct soc_enum soc_enum_mbias2_pdn_imp =
+	SOC_ENUM_SINGLE(MIC_BIAS2, MBIASx_PDN_IMP_SHIFT,
+			ARRAY_SIZE(enum_mbias_pdn_imp),
+			enum_mbias_pdn_imp);
+
+/* MBIAS2 Output voltage */
+static struct soc_enum soc_enum_mbias2_out_v =
+	SOC_ENUM_SINGLE(MIC_BIAS2, MBIAS2_OUT_V_SHIFT,
+			ARRAY_SIZE(enum_mbias2_out_v),
+			enum_mbias2_out_v);
+
+static struct soc_enum soc_enum_mbias2_int_r =
+	SOC_ENUM_SINGLE(MIC_BIAS2_VAD, MBIAS2_R_INT_SHIFT,
+			ARRAY_SIZE(enum_optional_resistor),
+			enum_optional_resistor);
+
+static struct soc_enum soc_enum_mic1_in_imp =
+	SOC_ENUM_SINGLE(MIC1_GAIN, MICx_IN_IMP_SHIFT,
+			ARRAY_SIZE(enum_mic_in_imp),
+			enum_mic_in_imp);
+
+static struct soc_enum soc_enum_mic2_in_imp =
+	SOC_ENUM_SINGLE(MIC2_GAIN, MICx_IN_IMP_SHIFT,
+			ARRAY_SIZE(enum_mic_in_imp),
+			enum_mic_in_imp);
+
+static struct soc_enum soc_enum_tx1_hp_filter =
+	SOC_ENUM_SINGLE(TX1, TXx_HP_FILTER_SHIFT,
+			ARRAY_SIZE(enum_hp_filter),
+			enum_hp_filter);
+
+static struct soc_enum soc_enum_tx2_hp_filter =
+	SOC_ENUM_SINGLE(TX2, TXx_HP_FILTER_SHIFT,
+			ARRAY_SIZE(enum_hp_filter),
+			enum_hp_filter);
+
+static struct soc_enum soc_enum_st1_hp_filter =
+	SOC_ENUM_SINGLE(ST1_PGA, STx_HP_FILTER_SHIFT,
+			ARRAY_SIZE(enum_hp_filter),
+			enum_hp_filter);
+
+static struct soc_enum soc_enum_st2_hp_filter =
+	SOC_ENUM_SINGLE(ST2_PGA, STx_HP_FILTER_SHIFT,
+			ARRAY_SIZE(enum_hp_filter),
+			enum_hp_filter);
+
+static struct soc_enum soc_enum_i2s0_word_length =
+	SOC_ENUM_SINGLE(INTERFACE0, I2Sx_WORDLENGTH_SHIFT,
+			ARRAY_SIZE(enum_i2s_word_length),
+			enum_i2s_word_length);
+
+static struct soc_enum soc_enum_i2s1_word_length =
+	SOC_ENUM_SINGLE(INTERFACE1, I2Sx_WORDLENGTH_SHIFT,
+			ARRAY_SIZE(enum_i2s_word_length),
+			enum_i2s_word_length);
+
+static struct soc_enum soc_enum_i2s0_mode =
+	SOC_ENUM_SINGLE(INTERFACE0, I2Sx_MODE_SHIFT,
+			ARRAY_SIZE(enum_i2s_mode),
+			enum_i2s_mode);
+
+static struct soc_enum soc_enum_i2s1_mode =
+	SOC_ENUM_SINGLE(INTERFACE1, I2Sx_MODE_SHIFT,
+			ARRAY_SIZE(enum_i2s_mode),
+			enum_i2s_mode);
+
+static struct soc_enum soc_enum_i2s0_tristate =
+	SOC_ENUM_SINGLE(INTERFACE0, I2Sx_TRISTATE_SHIFT,
+			ARRAY_SIZE(enum_i2s_tristate),
+			enum_i2s_tristate);
+
+static struct soc_enum soc_enum_i2s1_tristate =
+	SOC_ENUM_SINGLE(INTERFACE1, I2Sx_TRISTATE_SHIFT,
+			ARRAY_SIZE(enum_i2s_tristate),
+			enum_i2s_tristate);
+
+static struct soc_enum soc_enum_i2s0_pulldown_resistor =
+	SOC_ENUM_SINGLE(INTERFACE0, I2Sx_PULLDOWN_SHIFT,
+			ARRAY_SIZE(enum_optional_resistor),
+			enum_optional_resistor);
+
+static struct soc_enum soc_enum_i2s1_pulldown_resistor =
+	SOC_ENUM_SINGLE(INTERFACE1, I2Sx_PULLDOWN_SHIFT,
+			ARRAY_SIZE(enum_optional_resistor),
+			enum_optional_resistor);
+
+static struct soc_enum soc_enum_i2s0_sample_rate =
+	SOC_ENUM_SINGLE(INTERFACE0, I2Sx_SR_SHIFT,
+			ARRAY_SIZE(enum_i2s_sample_rate),
+			enum_i2s_sample_rate);
+
+static struct soc_enum soc_enum_i2s1_sample_rate =
+	SOC_ENUM_SINGLE(INTERFACE1, I2Sx_SR_SHIFT,
+			ARRAY_SIZE(enum_i2s_sample_rate),
+			enum_i2s_sample_rate);
+
+static struct soc_enum soc_enum_line1_inversion =
+	SOC_ENUM_SINGLE(LINE1, LINEx_INV_SHIFT,
+			ARRAY_SIZE(enum_signal_inversion),
+			enum_signal_inversion);
+
+static struct soc_enum soc_enum_line2_inversion =
+	SOC_ENUM_SINGLE(LINE2, LINEx_INV_SHIFT,
+			ARRAY_SIZE(enum_signal_inversion),
+			enum_signal_inversion);
+
+static struct soc_enum soc_enum_auxo1_inversion =
+	SOC_ENUM_SINGLE(AUXO1, AUXOx_INV_SHIFT,
+			ARRAY_SIZE(enum_signal_inversion),
+			enum_signal_inversion);
+
+static struct soc_enum soc_enum_auxo2_inversion =
+	SOC_ENUM_SINGLE(AUXO2, AUXOx_INV_SHIFT,
+			ARRAY_SIZE(enum_signal_inversion),
+			enum_signal_inversion);
+
+static struct soc_enum soc_enum_auxo3_inversion =
+	SOC_ENUM_SINGLE(AUXO3, AUXOx_INV_SHIFT,
+			ARRAY_SIZE(enum_signal_inversion),
+			enum_signal_inversion);
+
+static struct soc_enum soc_enum_auxo4_inversion =
+	SOC_ENUM_SINGLE(AUXO4, AUXOx_INV_SHIFT,
+			ARRAY_SIZE(enum_signal_inversion),
+			enum_signal_inversion);
+
+static struct soc_enum soc_enum_auxo1_pulldown_resistor =
+	SOC_ENUM_SINGLE(AUXO1, AUXOx_PULLDOWN_SHIFT,
+			ARRAY_SIZE(enum_optional_resistor),
+			enum_optional_resistor);
+
+static struct soc_enum soc_enum_auxo2_pulldown_resistor =
+	SOC_ENUM_SINGLE(AUXO2, AUXOx_PULLDOWN_SHIFT,
+			ARRAY_SIZE(enum_optional_resistor),
+			enum_optional_resistor);
+
+static struct soc_enum soc_enum_spkr1_mode =
+	SOC_ENUM_SINGLE(AB5500_VIRTUAL_REG3, SPKR1_MODE_SHIFT,
+			ARRAY_SIZE(enum_spkr1_mode),
+			enum_spkr1_mode);
+
+static struct soc_enum soc_enum_spkr2_mode =
+	SOC_ENUM_SINGLE(AB5500_VIRTUAL_REG3, SPKR2_MODE_SHIFT,
+			ARRAY_SIZE(enum_spkr2_mode),
+			enum_spkr2_mode);
+
+static struct soc_enum soc_enum_pwm_spkr1n_pol =
+	SOC_ENUM_SINGLE(PWMCTRL_SPKR1, PWMCTRL_SPKRx_N1_POL_SHIFT,
+			ARRAY_SIZE(enum_pwm_pol), enum_pwm_pol);
+
+static struct soc_enum soc_enum_pwm_spkr1p_pol =
+	SOC_ENUM_SINGLE(PWMCTRL_SPKR1, PWMCTRL_SPKRx_P1_POL_SHIFT,
+			ARRAY_SIZE(enum_pwm_pol), enum_pwm_pol);
+
+static struct soc_enum soc_enum_pwm_spkr2n_pol =
+	SOC_ENUM_SINGLE(PWMCTRL_SPKR2, PWMCTRL_SPKRx_N1_POL_SHIFT,
+			ARRAY_SIZE(enum_pwm_pol), enum_pwm_pol);
+
+static struct soc_enum soc_enum_pwm_spkr2p_pol =
+	SOC_ENUM_SINGLE(PWMCTRL_SPKR2, PWMCTRL_SPKRx_P1_POL_SHIFT,
+			ARRAY_SIZE(enum_pwm_pol), enum_pwm_pol);
+
+static struct snd_kcontrol_new ab5500_snd_controls[] = {
+	/* RX Routing */
+	SOC_ENUM("RX1 Input Select",	soc_enum_rx1_in_sel),
+	SOC_ENUM("RX2 Input Select",	soc_enum_rx2_in_sel),
+	SOC_ENUM("RX3 Input Select",	soc_enum_rx3_in_sel),
+	SOC_SINGLE("LINE1 Adder", LINE1_ADDER, 0, 0x1F, 0),
+	SOC_SINGLE("LINE2 Adder", LINE2_ADDER, 0, 0x1F, 0),
+	SOC_SINGLE("EAR Adder", EAR_ADDER, 0, 0x1F, 0),
+	SOC_SINGLE("SPKR1 Adder", SPKR1_ADDER, 0, 0x1F, 0),
+	SOC_SINGLE("SPKR2 Adder", SPKR2_ADDER, 0, 0x1F, 0),
+	SOC_SINGLE("AUXO1 Adder", AUXO1_ADDER, 0, 0x1F, 0),
+	SOC_SINGLE("AUXO2 Adder", AUXO2_ADDER, 0, 0x1F, 0),
+	SOC_SINGLE("AUXO3 Adder", AUXO3_ADDER, 0, 0x1F, 0),
+	SOC_SINGLE("AUXO4 Adder", AUXO4_ADDER, 0, 0x1F, 0),
+	SOC_SINGLE("SPKR1 PWM Select", AB5500_VIRTUAL_REG5, 0, 0x03, 0),
+	SOC_SINGLE("SPKR2 PWM Select", AB5500_VIRTUAL_REG5, 2, 0x0C, 0),
+	/* TX Routing */
+	SOC_ENUM("TX1 Input Select",	soc_enum_tx1_in_sel),
+	SOC_ENUM("TX2 Input Select",	soc_enum_tx2_in_sel),
+	SOC_SINGLE("MIC1 Input Select",	MIC1_INPUT_SELECT, 0, 0xff, 0),
+	SOC_SINGLE("MIC2 Input Select",	MIC2_INPUT_SELECT, 0, 0xff, 0),
+	SOC_SINGLE("MIC2 to MIC1", MIC2_TO_MIC1, 0, 0x03, 0),
+	SOC_ENUM("I2S0 Input Select",	soc_enum_i2s0_input_select),
+	SOC_ENUM("I2S1 Input Select",	soc_enum_i2s1_input_select),
+	/* Routing of Side Tone and Analop Loop */
+	SOC_ENUM("APGA1 Source", soc_enum_apga1_source),
+	SOC_ENUM("APGA2 Source", soc_enum_apga2_source),
+	SOC_ENUM("APGA1 Enable", soc_enum_apga1_enable),
+	SOC_ENUM("APGA2 Enable", soc_enum_apga2_enable),
+	SOC_ENUM("DAC1 Side Tone",		soc_enum_dac1_side_tone),
+	SOC_ENUM("DAC2 Side Tone",		soc_enum_dac2_side_tone),
+	/* RX Volume Control */
+	SOC_SINGLE("RX-DPGA1 Gain",		RX1_DPGA, 0, 0x43, 0),
+	SOC_SINGLE("RX-DPGA2 Gain",		RX1_DPGA, 0, 0x43, 0),
+	SOC_SINGLE("RX-DPGA3 Gain",		RX3_DPGA, 0, 0x43, 0),
+	SOC_SINGLE("LINE1 Gain", LINE1, LINEx_GAIN_SHIFT, 0x0a, 0),
+	SOC_SINGLE("LINE2 Gain", LINE2, LINEx_GAIN_SHIFT, 0x0a, 0),
+	SOC_SINGLE("SPKR1 Gain",	SPKR1, SPKRx_GAIN_SHIFT, 0x16, 0),
+	SOC_SINGLE("SPKR2 Gain",	SPKR2, SPKRx_GAIN_SHIFT, 0x16, 0),
+	SOC_SINGLE("EAR Gain", EAR_GAIN, EAR_GAIN_SHIFT, 0x12, 0),
+	SOC_SINGLE("AUXO1 Gain", AUXO1, AUXOx_GAIN_SHIFT, 0x0c, 0),
+	SOC_SINGLE("AUXO2 Gain", AUXO2, AUXOx_GAIN_SHIFT, 0x0c, 0),
+	SOC_SINGLE("AUXO3 Gain", AUXO3, AUXOx_GAIN_SHIFT, 0x0c, 0),
+	SOC_SINGLE("AUXO4 Gain", AUXO4, AUXOx_GAIN_SHIFT, 0x0c, 0),
+	/* TX Volume Control */
+	SOC_SINGLE("MIC1 Gain", MIC1_GAIN, MICx_GAIN_SHIFT, 0x0a, 0),
+	SOC_SINGLE("MIC2 Gain",	MIC2_GAIN, MICx_GAIN_SHIFT, 0x0a, 0),
+	SOC_SINGLE("TX-DPGA1 Gain", TX_DPGA1, TX_DPGAx_SHIFT, 0x0f, 0),
+	SOC_SINGLE("TX-DPGA2 Gain", TX_DPGA2, TX_DPGAx_SHIFT, 0x0f, 0),
+	/* Volume Control of Side Tone and Analog Loop */
+	SOC_SINGLE("ST-PGA1 Gain", ST1_PGA, STx_PGA_SHIFT, 0x0a, 0),
+	SOC_SINGLE("ST-PGA2 Gain", ST2_PGA, STx_PGA_SHIFT, 0x0a, 0),
+	SOC_SINGLE("APGA1 Gain", ANALOG_LOOP_PGA1, APGAx_GAIN_SHIFT, 0x1d, 0),
+	SOC_SINGLE("APGA2 Gain", ANALOG_LOOP_PGA2, APGAx_GAIN_SHIFT, 0x1d, 0),
+	/* RX Properties */
+	SOC_ENUM("DAC1 Power Mode",		soc_enum_dac1_power_mode),
+	SOC_ENUM("DAC2 Power Mode",		soc_enum_dac2_power_mode),
+	SOC_ENUM("DAC3 Power Mode",		soc_enum_dac3_power_mode),
+	SOC_ENUM("EAR Power Mode",		soc_enum_ear_power_mode),
+	SOC_ENUM("AUXO12 Power Mode",		soc_enum_auxo12_power_mode),
+	SOC_ENUM("AUXO34 Power Mode",		soc_enum_auxo34_power_mode),
+	SOC_ENUM("LINE1 Inversion",		soc_enum_line1_inversion),
+	SOC_ENUM("LINE2 Inversion",		soc_enum_line2_inversion),
+	SOC_ENUM("AUXO1 Inversion",		soc_enum_auxo1_inversion),
+	SOC_ENUM("AUXO2 Inversion",		soc_enum_auxo2_inversion),
+	SOC_ENUM("AUXO3 Inversion",		soc_enum_auxo3_inversion),
+	SOC_ENUM("AUXO4 Inversion",		soc_enum_auxo4_inversion),
+	SOC_ENUM("AUXO1 Pulldown Resistor", soc_enum_auxo1_pulldown_resistor),
+	SOC_ENUM("AUXO2 Pulldown Resistor", soc_enum_auxo2_pulldown_resistor),
+	SOC_ENUM("SPKR1 Mode", soc_enum_spkr1_mode),
+	SOC_ENUM("SPKR2 Mode", soc_enum_spkr2_mode),
+	SOC_ENUM("PWM SPKR1N POL", soc_enum_pwm_spkr1n_pol),
+	SOC_ENUM("PWM SPKR1P POL", soc_enum_pwm_spkr1p_pol),
+	SOC_ENUM("PWM SPKR2N POL", soc_enum_pwm_spkr2n_pol),
+	SOC_ENUM("PWM SPKR2P POL", soc_enum_pwm_spkr2p_pol),
+	/* TX Properties */
+	SOC_SINGLE("MIC1 VMID",	MIC1_VMID_SELECT, 0, 0x3f, 0),
+	SOC_SINGLE("MIC2 VMID",	MIC2_VMID_SELECT, 0, 0x3f, 0),
+	SOC_ENUM("MBIAS1 PDN Impedance",	soc_enum_mbias1_pdn_imp),
+	SOC_ENUM("MBIAS2 PDN Impedance",	soc_enum_mbias2_pdn_imp),
+	SOC_ENUM("MBIAS2 Output Voltage", soc_enum_mbias2_out_v),
+	SOC_ENUM("MBIAS2 Internal Resistor", soc_enum_mbias2_int_r),
+	SOC_ENUM("MIC1 Input Impedance",	soc_enum_mic1_in_imp),
+	SOC_ENUM("MIC2 Input Impedance",	soc_enum_mic2_in_imp),
+	SOC_ENUM("TX1 HP Filter",		soc_enum_tx1_hp_filter),
+	SOC_ENUM("TX2 HP Filter",		soc_enum_tx2_hp_filter),
+	/* Side Tone and Analog Loop Properties */
+	SOC_ENUM("ST1 HP Filter",		soc_enum_st1_hp_filter),
+	SOC_ENUM("ST2 HP Filter",		soc_enum_st2_hp_filter),
+	/* I2S Interface Properties */
+	SOC_ENUM("I2S0 Word Length",		soc_enum_i2s0_word_length),
+	SOC_ENUM("I2S1 Word Length",		soc_enum_i2s1_word_length),
+	SOC_ENUM("I2S0 Mode",			soc_enum_i2s0_mode),
+	SOC_ENUM("I2S1 Mode",			soc_enum_i2s1_mode),
+	SOC_ENUM("I2S0 tri-state",		soc_enum_i2s0_tristate),
+	SOC_ENUM("I2S1 tri-state",		soc_enum_i2s1_tristate),
+	SOC_ENUM("I2S0 Pulldown Resistor", soc_enum_i2s0_pulldown_resistor),
+	SOC_ENUM("I2S1 Pulldown Resistor", soc_enum_i2s1_pulldown_resistor),
+	SOC_ENUM("I2S0 Sample Rate",		soc_enum_i2s0_sample_rate),
+	SOC_ENUM("I2S1 Sample Rate",		soc_enum_i2s1_sample_rate),
+	SOC_SINGLE("Interface Swap",		INTERFACE_SWAP, 0, 0x03, 0),
+	/* Miscellaneous */
+	SOC_SINGLE("Negative Charge Pump", NEG_CHARGE_PUMP, 0, 0x03, 0)
+};
+
+/* count the number of 1 */
+#define count_ones(x) ({						\
+			int num;					\
+			typeof(x) y = x;				\
+			for (num = 0; y; y &= y - 1, num++)		\
+				;					\
+			num;						\
+		})
+
+enum enum_power {
+	POWER_OFF = 0,
+	POWER_ON = 1
+};
+
+enum enum_link {
+	UNLINK = 0,
+	LINK = 1
+};
+
+static enum enum_power get_widget_power_status(enum enum_widget widget)
+{
+	u8 val;
+
+	if (widget >= number_of_widgets)
+		return POWER_OFF;
+	val = read_reg(widget_pm_array[widget].reg);
+	if (val & (1 << widget_pm_array[widget].shift))
+		return POWER_ON;
+	else
+		return POWER_OFF;
+}
+
+static int count_powered_neighbors(const unsigned long *neighbors)
+{
+	unsigned long i;
+	int n = 0;
+	for_each_set_bit(i, neighbors, number_of_widgets) {
+		if (get_widget_power_status(i) == POWER_ON)
+			n++;
+	}
+	return n;
+}
+
+static int has_powered_neighbors(const unsigned long *neighbors)
+{
+	unsigned int i;
+	for_each_set_bit(i, neighbors, number_of_widgets) {
+		if (get_widget_power_status(i) == POWER_ON)
+			return 1;
+	}
+	return 0;
+}
+
+
+static int has_stacked_neighbors(const unsigned long *neighbors)
+{
+	unsigned long *stack_map = pm_stack_as_bitmap;
+	return bitmap_intersects(stack_map, neighbors, number_of_widgets);
+}
+
+static void power_widget_unlocked(enum enum_power onoff, enum enum_widget widget)
+{
+	enum enum_widget w;
+	int done;
+
+	if (widget >= number_of_widgets)
+		return;
+	if (get_widget_power_status(widget) == onoff)
+		return;
+
+	for (w = widget, done = 0; !done;) {
+		unsigned long i;
+		unsigned long *srcs = widget_pm_array[w].source_list;
+		unsigned long *sinks = widget_pm_array[w].sink_list;
+		dev_dbg(ab5500_dev, "%s: processing widget %s.\n",
+			__func__, widget_names[w]);
+
+		if (onoff == POWER_ON &&
+		    !bitmap_empty(srcs, number_of_widgets) &&
+		    !has_powered_neighbors(srcs)) {
+			pm_stack.stack[pm_stack.p++] = w;
+			for_each_set_bit(i, srcs, number_of_widgets) {
+				pm_stack.stack[pm_stack.p++] = i;
+			}
+			w = pm_stack.stack[--pm_stack.p];
+			continue;
+		} else if (onoff == POWER_OFF &&
+			   has_powered_neighbors(sinks)) {
+			int n = 0;
+			pm_stack.stack[pm_stack.p++] = w;
+			for_each_set_bit(i, sinks, number_of_widgets) {
+				if (count_powered_neighbors(
+					    widget_pm_array[i].source_list)
+				    == 1 &&
+				    get_widget_power_status(i) == POWER_ON) {
+					pm_stack.stack[pm_stack.p++] = i;
+					n++;
+				}
+			}
+			if (n) {
+				w = pm_stack.stack[--pm_stack.p];
+				continue;
+			} else
+				--pm_stack.p;
+		}
+		mask_set_reg(widget_pm_array[w].reg,
+			     1 << widget_pm_array[w].shift,
+			     onoff == POWER_ON ? 0xff : 0);
+		dev_dbg(ab5500_dev, "%s: widget %s powered %s.\n",
+			__func__, widget_names[w],
+			onoff == POWER_ON ? "on" : "off");
+		if (onoff == POWER_ON &&
+		    !bitmap_empty(sinks, number_of_widgets) &&
+		    !has_powered_neighbors(sinks) &&
+		    !has_stacked_neighbors(sinks)) {
+			for_each_set_bit(i, sinks, number_of_widgets) {
+				pm_stack.stack[pm_stack.p++] = i;
+			}
+			w = pm_stack.stack[--pm_stack.p];
+			continue;
+		} else if (onoff == POWER_OFF) {
+			for_each_set_bit(i, srcs, number_of_widgets) {
+				if (!has_powered_neighbors(
+					    widget_pm_array[i].sink_list)
+				    && get_widget_power_status(i) == POWER_ON
+				    && !test_bit(i, pm_stack_as_bitmap)) {
+					pm_stack.stack[pm_stack.p++] = i;
+				}
+			}
+		}
+		if (pm_stack.p > 0)
+			w = pm_stack.stack[--pm_stack.p];
+		else
+			done = 1;
+	}
+}
+
+static void power_widget_locked(enum enum_power onoff,
+				enum enum_widget widget)
+{
+	if (mutex_lock_interruptible(&ab5500_pm_mutex)) {
+		dev_warn(ab5500_dev,
+			 "%s: Signal received while waiting on the PM mutex.\n",
+			 __func__);
+		return;
+	}
+	power_widget_unlocked(onoff, widget);
+	mutex_unlock(&ab5500_pm_mutex);
+}
+
+
+static void dump_registers(const char *where, ...)
+{
+	va_list ap;
+	va_start(ap, where);
+	do {
+		short reg = va_arg(ap, int);
+		if (reg < 0)
+			break;
+		dev_dbg(ab5500_dev, "%s from %s> 0x%02X : 0x%02X.\n",
+			__func__, where, reg, read_reg(reg));
+	} while (1);
+	va_end(ap);
+}
+
+/**
+ * update the link two widgets.
+ * @op: 1 - connect; 0 - disconnect
+ * @src: source of the connection
+ * @sink: sink of the connection
+ */
+static int update_widgets_link(enum enum_link op, enum enum_widget src,
+			       enum enum_widget sink,
+			       u8 reg, u8 mask, u8 newval)
+{
+	int ret = 0;
+
+	if (mutex_lock_interruptible(&ab5500_pm_mutex)) {
+		dev_warn(ab5500_dev, "%s: A signal is received while waiting on"
+			 " the PM mutex.\n", __func__);
+		return -EINTR;
+	}
+
+	switch (op << 2 | test_bit(sink, widget_pm_array[src].sink_list) << 1 |
+		test_bit(src, widget_pm_array[sink].source_list)) {
+	case 3: /* UNLINK, sink in sink_list, src in source_list */
+	case 4: /* LINK, sink not in sink_list, src not in source_list */
+		break;
+	default:
+		ret = -EINVAL;
+		goto end;
+	}
+	switch (((int)op) << 2 | get_widget_power_status(src) << 1 |
+		get_widget_power_status(sink)) {
+	case 3: /* op = 0, src on, sink on */
+		if (count_powered_neighbors(widget_pm_array[sink].source_list)
+		    == 1)
+			power_widget_unlocked(POWER_OFF, sink);
+		mask_set_reg(reg, mask, newval);
+		break;
+	case 6: /* op = 1, src on, sink off */
+		mask_set_reg(reg, mask, newval);
+		power_widget_unlocked(POWER_ON, sink);
+		break;
+	default:
+		/* op = 0, src off, sink off */
+		/* op = 0, src off, sink on */
+		/* op = 0, src on, sink off */
+		/* op = 1, src off, sink off */
+		/* op = 1, src off, sink on */
+		/* op = 1, src on, sink on */
+		mask_set_reg(reg, mask, newval);
+	}
+	change_bit(sink, widget_pm_array[src].sink_list);
+	change_bit(src, widget_pm_array[sink].source_list);
+end:
+	mutex_unlock(&ab5500_pm_mutex);
+	return ret;
+};
+
+static enum enum_widget apga_source_translate(u8 reg_value)
+{
+	switch (reg_value) {
+	case 1:
+		return widget_mic1;
+	case 2:
+		return widget_mic2;
+	default:
+		return number_of_widgets;
+	}
+}
+
+static enum enum_widget adder_sink_translate(u8 reg)
+{
+	switch (reg) {
+	case EAR_ADDER:
+		return widget_ear;
+	case AUXO1_ADDER:
+		return widget_auxo1;
+	case AUXO2_ADDER:
+		return widget_auxo2;
+	case AUXO3_ADDER:
+		return widget_auxo3;
+	case AUXO4_ADDER:
+		return widget_auxo4;
+	case SPKR1_ADDER:
+		return widget_spkr1;
+	case SPKR2_ADDER:
+		return widget_spkr2;
+	case LINE1_ADDER:
+		return widget_line1;
+	case LINE2_ADDER:
+		return widget_line2;
+	default:
+		return number_of_widgets;
+	}
+}
+
+static int ab5500_add_widgets(struct snd_soc_codec *codec)
+{
+	snd_soc_dapm_new_controls(&codec->dapm, ab5500_dapm_widgets,
+				  ARRAY_SIZE(ab5500_dapm_widgets));
+
+	snd_soc_dapm_add_routes(&codec->dapm, intercon, ARRAY_SIZE(intercon));
+
+	snd_soc_dapm_new_widgets(&codec->dapm);
+	return 0;
+}
+
+static void power_for_playback(enum enum_power onoff, int ifsel)
+{
+	dev_dbg(ab5500_dev, "%s: interface %d power %s.\n", __func__,
+		ifsel, onoff == POWER_ON ? "on" : "off");
+	if (mutex_lock_interruptible(&ab5500_pm_mutex)) {
+		dev_warn(ab5500_dev,
+			 "%s: Signal received while waiting on the PM mutex.\n",
+			 __func__);
+		return;
+	}
+	power_widget_unlocked(onoff, ifsel == 0 ?
+			      widget_if0_dld_l : widget_if1_dld_l);
+	power_widget_unlocked(onoff, ifsel == 0 ?
+			      widget_if0_dld_r : widget_if1_dld_r);
+	mutex_unlock(&ab5500_pm_mutex);
+}
+
+static void power_for_capture(enum enum_power onoff, int ifsel)
+{
+	dev_info(ab5500_dev, "%s: interface %d power %s", __func__,
+		ifsel, onoff == POWER_ON ? "on" : "off");
+	if (mutex_lock_interruptible(&ab5500_pm_mutex)) {
+		dev_warn(ab5500_dev,
+			 "%s: Signal received while waiting on the PM mutex.\n",
+			 __func__);
+		return;
+	}
+	power_widget_unlocked(onoff, ifsel == 0 ?
+			      widget_if0_uld_l : widget_if1_uld_l);
+	power_widget_unlocked(onoff, ifsel == 0 ?
+			      widget_if0_uld_r : widget_if1_uld_r);
+	mutex_unlock(&ab5500_pm_mutex);
+}
+
+static int ab5500_add_controls(struct snd_soc_codec *codec)
+{
+	int err = 0, i, n = ARRAY_SIZE(ab5500_snd_controls);
+
+	pr_info("%s: %s called.\n", __FILE__, __func__);
+	for (i = 0; i < n; i++) {
+		err = snd_ctl_add(codec->card->snd_card, snd_ctl_new1(
+					  &ab5500_snd_controls[i], codec));
+		if (err < 0) {
+			pr_err("%s failed to add control No.%d of %d.\n",
+			       __func__, i, n);
+			return err;
+		}
+	}
+	return err;
+}
+
+static int ab5500_pcm_startup(struct snd_pcm_substream *substream,
+			      struct snd_soc_dai *dai)
+{
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK ?
+	    dai->playback_active : dai->capture_active) {
+		dev_err(dai->dev, "A %s stream is already active.\n",
+			substream->stream == SNDRV_PCM_STREAM_PLAYBACK ?
+			"playback" : "capture");
+		return -EBUSY;
+	}
+	return 0;
+}
+
+static int ab5500_pcm_hw_params(struct snd_pcm_substream *substream,
+				struct snd_pcm_hw_params *hw_params,
+				struct snd_soc_dai *dai)
+{
+	u8 val;
+	u8 reg = dai->id == 0 ? INTERFACE0 : INTERFACE1;
+
+	if (!ab5500_dev) {
+		pr_err("%s: The AB5500 codec driver not initialized.\n",
+		       __func__);
+		return -EAGAIN;
+	}
+	dev_info(ab5500_dev, "%s called.\n", __func__);
+	switch (params_rate(hw_params)) {
+	case 8000:
+		val = I2Sx_SR_8000Hz;
+		break;
+	case 16000:
+		val = I2Sx_SR_16000Hz;
+		break;
+	case 44100:
+		val = I2Sx_SR_44100Hz;
+		break;
+	case 48000:
+		val = I2Sx_SR_48000Hz;
+		break;
+	default:
+		return -EINVAL;
+	}
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK ?
+	    !dai->capture_active : !dai->playback_active) {
+
+		mask_set_reg(reg, I2Sx_SR_MASK, val << I2Sx_SR_SHIFT);
+		if ((read_reg(reg) & I2Sx_MODE_MASK) == 0) {
+			mask_set_reg(reg, MASTER_GENx_PWR_MASK,
+				     1 << MASTER_GENx_PWR_SHIFT);
+		}
+	}
+	return 0;
+}
+
+static int ab5500_pcm_prepare(struct snd_pcm_substream *substream,
+			      struct snd_soc_dai *dai)
+{
+	dev_dbg(ab5500_dev, "%s called.\n", __func__);
+	/* Configure registers for either playback or capture */
+	if ((substream->stream == SNDRV_PCM_STREAM_PLAYBACK) &&
+		!(ab5500_codec_privates[dai->id].playback_active == true)) {
+		power_for_playback(POWER_ON, dai->id);
+		ab5500_codec_privates[dai->id].playback_active = true;
+	} else if ((substream->stream == SNDRV_PCM_STREAM_CAPTURE) &&
+		!(ab5500_codec_privates[dai->id].capture_active == true)) {
+		power_for_capture(POWER_ON, dai->id);
+		ab5500_codec_privates[dai->id].capture_active = true;
+	}
+	dump_registers(__func__, RX1, AUXO1_ADDER, RX2,
+			AUXO2_ADDER, RX1_DPGA, RX2_DPGA, AUXO1, AUXO2, -1);
+	return 0;
+}
+
+static void ab5500_pcm_shutdown(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *dai)
+{
+	u8 iface = dai->id == 0 ? INTERFACE0 : INTERFACE1;
+	dev_info(ab5500_dev, "%s called.\n", __func__);
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
+		power_for_playback(POWER_OFF, dai->id);
+		ab5500_codec_privates[dai->id].playback_active = false;
+	} else {
+		power_for_capture(POWER_OFF, dai->id);
+		ab5500_codec_privates[dai->id].capture_active = false;
+
+	}
+	if (!dai->playback_active && !dai->capture_active &&
+	    (read_reg(iface) & I2Sx_MODE_MASK) == 0)
+		mask_set_reg(iface, MASTER_GENx_PWR_MASK, 0);
+}
+
+static int ab5500_set_dai_sysclk(struct snd_soc_dai *dai, int clk_id,
+				 unsigned int freq, int dir)
+{
+	return 0;
+}
+
+static int ab5500_set_dai_fmt(struct snd_soc_dai *codec_dai, unsigned int fmt)
+{
+	u8 iface = (codec_dai->id == 0) ? INTERFACE0 : INTERFACE1;
+	u8 val = 0;
+	dev_info(ab5500_dev, "%s called.\n", __func__);
+
+	switch (fmt & (SND_SOC_DAIFMT_FORMAT_MASK |
+		       SND_SOC_DAIFMT_MASTER_MASK)) {
+
+	case SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBS_CFS:
+		val |= 1 << I2Sx_MODE_SHIFT;
+		break;
+
+	case SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBM_CFM:
+		break;
+
+	default:
+		dev_warn(ab5500_dev, "AB5500_dai: unsupported DAI format "
+			 "0x%x\n", fmt);
+		return -EINVAL;
+	}
+	if (codec_dai->playback_active && codec_dai->capture_active) {
+		if ((read_reg(iface) & I2Sx_MODE_MASK) == val)
+			return 0;
+		else {
+			dev_err(ab5500_dev,
+				"%s: DAI format set differently "
+				"by an existing stream.\n", __func__);
+			return -EINVAL;
+		}
+	}
+	mask_set_reg(iface, I2Sx_MODE_MASK, val);
+	return 0;
+}
+
+struct snd_soc_dai_driver ab5500_dai_drv[] = {
+	{
+		.name = "ab5500-codec-dai.0",
+		.id = 0,
+		.playback = {
+			.stream_name = "ab5500.0 Playback",
+			.channels_min = 2,
+			.channels_max = 2,
+			.rates = AB5500_SUPPORTED_RATE,
+			.formats = AB5500_SUPPORTED_FMT,
+		},
+		.capture = {
+			.stream_name = "ab5500.0 Capture",
+			.channels_min = 2,
+			.channels_max = 2,
+			.rates = AB5500_SUPPORTED_RATE,
+			.formats = AB5500_SUPPORTED_FMT,
+		},
+		.ops = (struct snd_soc_dai_ops[]) {
+			{
+				.startup = ab5500_pcm_startup,
+				.prepare = ab5500_pcm_prepare,
+				.hw_params = ab5500_pcm_hw_params,
+				.shutdown = ab5500_pcm_shutdown,
+				.set_sysclk = ab5500_set_dai_sysclk,
+				.set_fmt = ab5500_set_dai_fmt,
+			}
+		},
+		.symmetric_rates = 1,
+	},
+	{
+		.name = "ab5500-codec-dai.1",
+		.id = 1,
+		.playback = {
+			.stream_name = "ab5500.1 Playback",
+			.channels_min = 2,
+			.channels_max = 2,
+			.rates = AB5500_SUPPORTED_RATE,
+			.formats = AB5500_SUPPORTED_FMT,
+		},
+		.capture = {
+			.stream_name = "ab5500.1 Capture",
+			.channels_min = 2,
+			.channels_max = 2,
+			.rates = AB5500_SUPPORTED_RATE,
+			.formats = AB5500_SUPPORTED_FMT,
+		},
+		.ops = (struct snd_soc_dai_ops[]) {
+			{
+				.startup = ab5500_pcm_startup,
+				.prepare = ab5500_pcm_prepare,
+				.hw_params = ab5500_pcm_hw_params,
+				.shutdown = ab5500_pcm_shutdown,
+				.set_sysclk = ab5500_set_dai_sysclk,
+				.set_fmt = ab5500_set_dai_fmt,
+			}
+		},
+		.symmetric_rates = 1,
+	}
+};
+
+static int ab5500_codec_probe(struct snd_soc_codec *codec)
+{
+	int ret = ab5500_add_controls(codec);
+	if (ret < 0)
+		return ret;
+	ab5500_add_widgets(codec);
+	return 0;
+}
+
+static int ab5500_codec_remove(struct snd_soc_codec *codec)
+{
+	snd_soc_dapm_free(&codec->dapm);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int ab5500_codec_suspend(struct snd_soc_codec *codec,
+				pm_message_t state)
+{
+	mask_set_reg(CLOCK, CLOCK_ENABLE_MASK, 0);
+	return 0;
+}
+
+static int ab5500_codec_resume(struct snd_soc_codec *codec)
+{
+	mask_set_reg(CLOCK, CLOCK_ENABLE_MASK, 0xff);
+	return 0;
+}
+#else
+#define ab5500_codec_resume NULL
+#define ab5500_codec_suspend NULL
+#endif
+
+/**
+   This function is only called by the SOC framework to
+   set registers associated to the mixer controls.
+*/
+static int ab5500_codec_write_reg(struct snd_soc_codec *codec,
+				  unsigned int reg, unsigned int value)
+{
+	if (reg < MIC_BIAS1 || reg > INTERFACE_SWAP)
+		return -EINVAL;
+	switch (reg) {
+		u8 diff, oldval;
+	case ANALOG_LOOP_PGA1:
+	case ANALOG_LOOP_PGA2: {
+		enum enum_widget apga = reg == ANALOG_LOOP_PGA1 ?
+			widget_apga1 : widget_apga2;
+
+		oldval = read_reg(reg);
+		diff = value ^ oldval;
+
+		/*
+		 * The APGA is to be turned on/off. The power bit and the
+		 * other bits in the same register won't be changed at the
+		 * same time since they belong to different controls.
+		*/
+		if (diff & (1 << APGAx_PWR_SHIFT)) {
+			power_widget_locked(value >> APGAx_PWR_SHIFT & 1,
+					    apga);
+		} else if (diff & APGAx_MUX_MASK) {
+			enum enum_widget old_source =
+				apga_source_translate(oldval);
+			enum enum_widget new_source =
+				apga_source_translate(value);
+			update_widgets_link(UNLINK, old_source, apga,
+					    reg, APGAx_MUX_MASK, 0);
+			update_widgets_link(LINK, new_source, apga,
+					    reg, APGAx_MUX_MASK, value);
+		} else {
+			set_reg(reg, value);
+		}
+		break;
+	}
+
+	case EAR_ADDER:
+	case AUXO1_ADDER:
+	case AUXO2_ADDER:
+	case AUXO3_ADDER:
+	case AUXO4_ADDER:
+	case SPKR1_ADDER:
+	case SPKR2_ADDER:
+	case LINE1_ADDER:
+	case LINE2_ADDER: {
+		int i;
+		enum enum_widget widgets[] = {
+			widget_dac1, widget_dac2, widget_dac3,
+			widget_apga1, widget_apga2
+		};
+		oldval = read_reg(reg);
+		diff = value ^ oldval;
+		for (i = 0; diff; i++) {
+			if (!(diff & 1 << i))
+				continue;
+			diff ^= 1 << i;
+			update_widgets_link(value >> i & 1, widgets[i],
+					    adder_sink_translate(reg),
+					    reg, 1 << i, value);
+		}
+		break;
+	}
+	case AB5500_VIRTUAL_REG3:
+		oldval = read_reg(reg);
+		diff = value ^ oldval;
+		/*
+		 * The following changes won't take place in the same call,
+		 * since they are arranged into different mixer controls.
+		 */
+
+		/* changed between the two amplifier modes */
+		if (hweight8(diff & SPKR1_MODE_MASK) == 2) {
+			set_reg(reg, value);
+			break;
+		}
+
+		if (diff & SPKR1_MODE_MASK) {
+			update_widgets_link(
+				UNLINK,
+				(oldval & SPKR1_MODE_MASK) == 0 ?
+				widget_pwm_spkr1 : widget_spkr1_adder,
+				widget_spkr1,
+				reg, SPKR1_MODE_MASK, value);
+			update_widgets_link(
+				LINK,
+				(value & SPKR1_MODE_MASK) == 0 ?
+				widget_pwm_spkr1 : widget_spkr1_adder,
+				widget_spkr1,
+				DUMMY_REG, 0, 0);
+
+		}
+		if (diff & SPKR2_MODE_MASK) {
+			update_widgets_link(
+				UNLINK,
+				(oldval & SPKR2_MODE_MASK) == 0 ?
+				widget_pwm_spkr2 : widget_spkr2_adder,
+				widget_spkr2,
+				reg, SPKR2_MODE_MASK, value);
+			update_widgets_link(
+				LINK,
+				(value & SPKR2_MODE_MASK) == 0 ?
+				widget_pwm_spkr2 : widget_spkr2_adder,
+				widget_spkr2,
+				DUMMY_REG, 0, 0);
+
+		}
+		break;
+
+	case AB5500_VIRTUAL_REG4:
+		/* configure PWMCTRL_SPKR1, PWMCTRL_SPKR2, etc. */
+		break;
+	default:
+		set_reg(reg, value);
+	}
+	return 0;
+}
+
+static unsigned int ab5500_codec_read_reg(struct snd_soc_codec *codec,
+					  unsigned int reg)
+{
+	return read_reg(reg);
+}
+
+
+static struct snd_soc_codec_driver ab5500_codec_drv = {
+	.probe =	ab5500_codec_probe,
+	.remove =	ab5500_codec_remove,
+	.suspend =	ab5500_codec_suspend,
+	.resume =	ab5500_codec_resume,
+	.read =		ab5500_codec_read_reg,
+	.write =	ab5500_codec_write_reg,
+};
+EXPORT_SYMBOL_GPL(ab5500_codec_drv);
+
+static inline void init_playback_route(void)
+{
+	/* if0_dld_l -> rx1 -> dac1 -> auxo1 */
+	update_widgets_link(LINK, widget_if0_dld_l, widget_rx1,
+			    RX1, RXx_DATA_MASK, 0x03 << RXx_DATA_SHIFT);
+	update_widgets_link(LINK, widget_rx1, widget_dac1, 0, 0, 0);
+	update_widgets_link(LINK, widget_dac1, widget_auxo1,
+			    AUXO1_ADDER, DAC1_TO_X_MASK, 0xff);
+
+	/* if0_dld_r -> rx2 -> dac2 -> auxo2 */
+	update_widgets_link(LINK, widget_if0_dld_r, widget_rx2,
+			    RX2, RXx_DATA_MASK, 0x04 << RXx_DATA_SHIFT);
+	update_widgets_link(LINK, widget_rx2, widget_dac2, 0, 0, 0);
+	update_widgets_link(LINK, widget_dac2, widget_auxo2,
+			    AUXO2_ADDER, DAC2_TO_X_MASK, 0xff);
+
+}
+
+static inline void init_capture_route(void)
+{
+	/* mic bias - > mic2 inputs */
+	update_widgets_link(LINK, widget_micbias1, widget_mic2p2,
+			    0, 0, 0);
+	update_widgets_link(LINK, widget_micbias1, widget_mic2n2,
+			    0, 0, 0);
+	update_widgets_link(LINK, widget_micbias2, widget_mic2p2,
+			    0, 0, 0);
+	update_widgets_link(LINK, widget_micbias2, widget_mic2n2,
+			    0, 0, 0);
+
+
+	/* mic2 inputs -> mic2 */
+	update_widgets_link(LINK, widget_mic2p2, widget_mic2,
+			    MIC2_INPUT_SELECT, MICxP2_SEL_MASK, 0xff);
+	update_widgets_link(LINK, widget_mic2n2, widget_mic2,
+			    MIC2_INPUT_SELECT, MICxN2_SEL_MASK, 0xff);
+
+	/* mic2 -> adc2 -> tx2 */
+	update_widgets_link(LINK, widget_mic2, widget_adc2,
+			    0, 0, 0);
+	update_widgets_link(LINK, widget_adc2, widget_tx2,
+			    TX2, TXx_MUX_MASK, 0);
+
+	/* tx2 -> if0_uld_l & if0_uld_r */
+	update_widgets_link(LINK, widget_tx2, widget_if0_uld_l,
+			    INTERFACE0_ULD, I2Sx_ULD_L_MASK,
+			    0x02 << I2Sx_ULD_L_SHIFT);
+	update_widgets_link(LINK, widget_tx2, widget_if0_uld_r,
+			    INTERFACE0_ULD, I2Sx_ULD_R_MASK,
+			    0x02 << I2Sx_ULD_R_SHIFT);
+
+	/* mic2 -> apga2 */
+	update_widgets_link(LINK, widget_mic2, widget_apga2,
+			    ANALOG_LOOP_PGA2, APGAx_MUX_MIC2_MASK,
+			    1 << APGAx_MUX_MIC2_SHIFT);
+
+	/* apga2 -> auxo1 & auxo2 */
+	update_widgets_link(LINK, widget_apga2, widget_auxo1,
+			    AUXO1_ADDER, APGA2_TO_X_MASK,
+			    1 << APGA2_TO_X_SHIFT);
+	update_widgets_link(LINK, widget_apga2, widget_auxo2,
+			    AUXO2_ADDER, APGA2_TO_X_MASK,
+			    1 << APGA2_TO_X_SHIFT);
+}
+
+static inline void init_playback_gain(void)
+{
+	/* 0x43, 0x0C: pure gain values */
+	mask_set_reg(RX1_DPGA, RXx_DPGA_MASK,
+		     0x43 << RXx_DPGA_SHIFT);
+	mask_set_reg(RX2_DPGA, RXx_DPGA_MASK,
+		     0x43 << RXx_DPGA_SHIFT);
+	mask_set_reg(AUXO1, AUXOx_GAIN_MASK, 0x0C << AUXOx_GAIN_SHIFT);
+	mask_set_reg(AUXO2, AUXOx_GAIN_MASK, 0x0C << AUXOx_GAIN_SHIFT);
+}
+
+static inline void init_capture_gain(void)
+{
+	/* 0x06, 0x0f: pure gain values */
+	mask_set_reg(MIC1_GAIN, MICx_GAIN_MASK, 0x06 << MICx_GAIN_SHIFT);
+	mask_set_reg(TX_DPGA1, TX_DPGAx_MASK, 0x0f << TX_DPGAx_SHIFT);
+}
+
+static int __devinit ab5500_platform_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	u8 reg;
+	struct ab5500_codec_dai_data *codec_drvdata;
+
+	pr_info("%s invoked with pdev = %p.\n", __func__, pdev);
+	ab5500_dev = &pdev->dev;
+	codec_drvdata = kzalloc(sizeof(struct ab5500_codec_dai_data),
+				GFP_KERNEL);
+	if (codec_drvdata == NULL)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, codec_drvdata);
+	ret = snd_soc_register_codec(ab5500_dev, &ab5500_codec_drv,
+				     ab5500_dai_drv,
+				     ARRAY_SIZE(ab5500_dai_drv));
+	if (ret < 0) {
+		dev_err(ab5500_dev, "%s: Failed to register codec. "
+			"Error %d.\n", __func__, ret);
+		snd_soc_unregister_codec(ab5500_dev);
+		kfree(codec_drvdata);
+	}
+	/* Initialize the codec registers */
+	for (reg = AB5500_FIRST_REG; reg <= AB5500_LAST_REG; reg++)
+		set_reg(reg, 0);
+
+	mask_set_reg(CLOCK, CLOCK_REF_SELECT_MASK | CLOCK_ENABLE_MASK,
+		     1 << CLOCK_REF_SELECT_SHIFT | 1 << CLOCK_ENABLE_SHIFT);
+	printk(KERN_ERR "Clock Setting ab5500\n");
+	init_playback_route();
+	init_playback_gain();
+	init_capture_route();
+	init_capture_gain();
+	memset(&pm_stack, 0, sizeof(pm_stack));
+	return ret;
+}
+
+static int __devexit ab5500_platform_remove(struct platform_device *pdev)
+{
+	pr_info("%s called.\n", __func__);
+	mask_set_reg(CLOCK, CLOCK_ENABLE_MASK, 0);
+	snd_soc_unregister_codec(ab5500_dev);
+	kfree(platform_get_drvdata(pdev));
+	ab5500_dev = NULL;
+	return 0;
+}
+
+static int ab5500_platform_suspend(struct platform_device *pdev,
+				   pm_message_t state)
+{
+	return 0;
+}
+
+static int ab5500_platform_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver ab5500_platform_driver = {
+	.driver		= {
+		.name		= "ab5500-codec",
+		.owner		= THIS_MODULE,
+	},
+	.probe		= ab5500_platform_probe,
+	.remove		= ab5500_platform_remove,
+	.suspend	= ab5500_platform_suspend,
+	.resume		= ab5500_platform_resume,
+};
+
+static int __devinit ab5500_init(void)
+{
+	int ret;
+
+	pr_info("%s called.\n", __func__);
+
+	/*  Register codec platform driver. */
+	ret = platform_driver_register(&ab5500_platform_driver);
+	if (ret) {
+		pr_err("%s: Error %d: Failed to register codec platform "
+			 "driver.\n", __func__, ret);
+	}
+	return ret;
+}
+
+static void __devexit ab5500_exit(void)
+{
+	pr_info("%s called.\n", __func__);
+	platform_driver_unregister(&ab5500_platform_driver);
+}
+
+module_init(ab5500_init);
+module_exit(ab5500_exit);
+
+MODULE_DESCRIPTION("AB5500 Codec driver");
+MODULE_AUTHOR("Xie Xiaolei <xie.xiaolei@stericsson.com>");
+MODULE_LICENSE("GPL");
diff --git a/sound/soc/codecs/ab5500.h b/sound/soc/codecs/ab5500.h
new file mode 100644
index 00000000000..c410259d73a
--- /dev/null
+++ b/sound/soc/codecs/ab5500.h
@@ -0,0 +1,408 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Register definitions for AB5500 codec
+ * Author: Xie Xiaolei <xie.xiaolei@etericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 AB5500_CODEC_REGISTERS_H
+#define AB5500_CODEC_REGISTERS_H
+
+#define AB5500_SUPPORTED_RATE (SNDRV_PCM_RATE_8000 | SNDRV_PCM_RATE_16000 | \
+			       SNDRV_PCM_RATE_44100 | SNDRV_PCM_RATE_48000)
+
+#define AB5500_SUPPORTED_FMT (SNDRV_PCM_FMTBIT_S16_LE | \
+			      SNDRV_PCM_FMTBIT_S24_LE)
+
+
+/* MIC BIAS */
+
+#define MIC_BIAS1 0x00
+#define MIC_BIAS2 0x01
+#define MBIAS2_OUT_V_MASK 0x04
+#define MBIAS2_OUT_V_SHIFT 2
+#define MBIASx_PWR_MASK 0x02
+#define MBIASx_PWR_SHIFT 1
+#define MBIASx_PDN_IMP_MASK 0x01
+#define MBIASx_PDN_IMP_SHIFT 0
+
+#define MIC_BIAS2_VAD 0x02
+#define MBIAS2_R_INT_MASK 0x01
+#define MBIAS2_R_INT_SHIFT 0
+
+/* MIC */
+#define MIC1_GAIN 0x03
+#define MIC2_GAIN 0x04
+#define MICx_GAIN_MASK 0xF0
+#define MICx_GAIN_SHIFT 4
+#define MICx_IN_IMP_MASK 0x0C
+#define MICx_IN_IMP_SHIFT 2
+#define MICx_PWR_MASK 0x01
+#define MICx_PWR_SHIFT 0
+
+#define MIC1_INPUT_SELECT 0x05
+#define MIC2_INPUT_SELECT 0x06
+#define MICxP1_SEL_MASK 0x80
+#define MICxP1_SEL_SHIFT 7
+#define MICxN1_SEL_MASK 0x40
+#define MICxN1_SEL_SHIFT 6
+#define MICxP2_SEL_MASK 0x20
+#define MICxP2_SEL_SHIFT 5
+#define MICxN2_SEL_MASK 0x10
+#define MICxN2_SEL_SHIFT 4
+#define LINEIN_SEL_MASK 0x03
+#define LINEIN_SEL_SHIFT 0
+
+#define MIC1_VMID_SELECT 0x07
+#define MIC2_VMID_SELECT 0x08
+#define VMIDx_ENABLE_MASK 0xC0
+#define VMIDx_ENABLE_SHIFT 6
+#define VMIDx_LINEIN1_N_MASK 0x20
+#define VMIDx_LINEIN1_N_SHIFT 5
+#define VMIDx_LINEIN2_N_MASK 0x10
+#define VMIDx_LINEIN2_N_SHIFT 4
+#define VMIDx_MICxP1_MASK  0x08
+#define VMIDx_MICxP1_SHIFT 3
+#define VMIDx_MICxP2_MASK  0x04
+#define VMIDx_MICxP2_SHIFT 2
+#define VMIDx_MICxN1_MASK  0x02
+#define VMIDx_MICxN1_SHIFT 1
+#define VMIDx_MICxN2_MASK  0x01
+#define VMIDx_MICxN2_SHIFT 0
+
+#define MIC2_TO_MIC1 0x09
+#define MIC2_TO_MIC1_MASK 0x03
+#define MIC2_TO_MIC1_SHIFT 0
+
+/* Analog Loop */
+#define ANALOG_LOOP_PGA1 0x0A
+#define ANALOG_LOOP_PGA2 0x0B
+#define APGAx_GAIN_MASK 0xF8
+#define APGAx_GAIN_SHIFT 3
+#define APGAx_PWR_MASK 0x04
+#define APGAx_PWR_SHIFT 2
+#define APGAx_MUX_MASK 0x03
+#define APGAx_MUX_SHIFT 0
+#define APGAx_MUX_MIC1_MASK 0x01
+#define APGAx_MUX_MIC1_SHIFT 0
+#define APGAx_MUX_MIC2_MASK 0x02
+#define APGAx_MUX_MIC2_SHIFT 1
+
+#define APGA_VMID_SELECT 0x0C
+#define VMID_APGA1_ENABLE_MASK 0xC0
+#define VMID_APGA1_ENABLE_SHIFT 6
+#define VMID_APGA1_LINEIN1_MASK 0x20
+#define VMID_APGA1_LINEIN1_SHIFT 5
+#define VMID_APGA2_ENABLE_MASK 0x0C
+#define VMID_APGA2_ENABLE_SHIFT 2
+#define VMID_APGA2_LINEIN2_MASK 0x02
+#define VMID_APGA2_LINEIN2_SHIFT 1
+
+/* Output Amplifiers */
+#define EAR_PWR 0x0D
+#define EAR_PWR_MODE_MASK 0xC0
+#define EAR_PWR_MODE_SHIFT 6
+#define EAR_PWR_VMID_MASK 0x30
+#define EAR_PWR_VMID_SHIFT 4
+#define EAR_PWR_MASK 0x01
+#define EAR_PWR_SHIFT 0
+
+#define EAR_GAIN 0x0E
+#define EAR_GAIN_MASK 0x1F
+#define EAR_GAIN_SHIFT 0
+
+#define AUXO1 0x0F
+#define AUXO2 0x10
+#define AUXO3 0x11
+#define AUXO4 0x12
+#define AUXOx_PWR_MASK 0x80
+#define AUXOx_PWR_SHIFT 7
+#define AUXOx_INV_MASK 0x40
+#define AUXOx_INV_SHIFT 6
+#define AUXOx_PULLDOWN_MASK 0x20
+#define AUXOx_PULLDOWN_SHIFT 5
+#define AUXOx_GAIN_MASK 0x0F
+#define AUXOx_GAIN_SHIFT 0
+
+#define AUXO12_PWR_MODE 0x13
+#define AUXO34_PWR_MODE 0x14
+#define AUXOxy_PWR_MODE_MASK 0x07
+#define AUXOxy_PWR_MODE_SHIFT 0
+
+#define NEG_CHARGE_PUMP 0x15
+#define NEG_CHARGE_PUMP_MODE_MASK 0x02
+#define NEG_CHARGE_PUMP_MODE_SHIFT 1
+#define NEG_CHARGE_PUMP_PWR_MASK 0x01
+#define NEG_CHARGE_PUMP_PWR_SHIFT 0
+
+#define ENV_THR 0x16
+#define ENV_THR_HIGH_MASK 0xF0
+#define ENV_THR_HIGH_SHIFT 4
+#define ENV_THR_LOW_MASK 0x0F
+#define ENV_THR_LOW_SHIFT 0
+
+#define ENV_DECAY_TIME 0x17
+#define ENV_DECAY_TIME_CP_LV_MASK 0x20
+#define ENV_DECAY_TIME_CP_LV_SHIFT 5
+#define ENV_DECAY_TIME_DET_CP_MASK 0x10
+#define ENV_DECAY_TIME_DET_CP_SHIFT 4
+#define ENV_DECAY_TIME_MASK 0x0F
+#define ENV_DECAY_TIME_SHIFT 0
+
+#define DC_CANCEL 0x18
+#define DC_CANCEL_SPKR2_MASK 0x10
+#define DC_CANCEL_SPKR2_SHIFT 4
+#define DC_CANCEL_SPKR1_MASK 0x08
+#define DC_CANCEL_SPKR1_SHIFT 3
+#define DC_CANCEL_AUXO34_MASK 0x04
+#define DC_CANCEL_AUXO34_SHIFT 2
+#define DC_CANCEL_AUXO12_MASK 0x02
+#define DC_CANCEL_AUXO12_SHIFT 1
+#define DC_CANCEL_OFFSET_CLOCK_MASK 0x01
+#define DC_CANCEL_OFFSET_CLOCK_SHIFT 0
+
+#define SPKR1 0x19
+#define SPKR2 0x1A
+#define SPKRx_PWR_MASK 0xC0
+#define SPKRx_PWR_SHIFT 6
+#define SPKRx_PWR_VBR_VALUE 0x40
+#define SPKRx_PWR_CLS_D_VALUE 0x80
+#define SPKRx_PWR_CLS_AB_VALUE 0xC0
+#define SPKR1_VMID_MASK 0x20
+#define SPKR1_VMID_SHIFT 5
+#define SPKRx_GAIN_MASK 0x1F
+#define SPKRx_GAIN_SHIFT 0
+
+#define SPKR_OVCR 0x1B
+#define SPKR_OVCR_PROT2_MASK 0x80
+#define SPKR_OVCR_PROT2_SHIFT 7
+#define SPKR_OVCR_TRIM2_MASK 0x70
+#define SPKR_OVCR_TRIM2_SHIFT 4
+#define SPKR_OVCR_PROT1_MASK 0x08
+#define SPKR_OVCR_PROT1_SHIFT 3
+#define SPKR_OVCR_TRIM1_MASK 0x07
+#define SPKR_OVCR_TRIM1_SHIFT 0
+
+#define PWMCTRL_SPKR1 0x1C
+#define PWMCTRL_SPKR2 0x1F
+#define PWMCTRL_SPKRx_N1_POL_MASK 0x80
+#define PWMCTRL_SPKRx_N1_POL_SHIFT 7
+#define PWMCTRL_SPKRx_P1_POL_MASK 0x40
+#define PWMCTRL_SPKRx_P1_POL_SHIFT 6
+#define PWMCTRL_SPKRx_MASK 0x04
+#define PWMCTRL_SPKRx_SHIFT 2
+#define PWMCTRL_SPKRxN_MASK 0x02
+#define PWMCTRL_SPKRxN_SHIFT 1
+#define PWMCTRL_SPKRxP_MASK 0x01
+#define PWMCTRL_SPKRxP_SHIFT 0
+
+#define PWM_SPKR1N 0x1D
+#define PWM_SPKR2N 0x20
+#define PWM_SPKR1P 0x1E
+#define PWM_SPKR2P 0x21
+#define PWM_SPKRxy_DUT_CYC_MASK 0xFF
+#define PWM_SPKRxy_DUT_CYC_SHIFT 0
+
+#define SPKR1_CLK_DIV 0x22
+#define SPKR2_CLK_DIV 0x23
+#define SPKRx_CLK_DIV_MASK 0x3F
+#define SPKRx_CLK_DIV_SHIFT 0
+
+#define LINE1 0x24
+#define LINE2 0x25
+#define LINEx_PWR_MASK 0x80
+#define LINEx_PWR_SHIFT 7
+#define LINEx_INV_MASK 0x40
+#define LINEx_INV_SHIFT 6
+#define LINEx_TO_USB_MASK 0x20
+#define LINEx_TO_USB_SHIFT 5
+#define LINEx_VMID_BUFF_MASK 0x10
+#define LINEx_VMID_BUFF_SHIFT 4
+#define LINEx_GAIN_MASK 0x0F
+#define LINEx_GAIN_SHIFT 0
+
+#define USB_AUDIO 0x26
+#define USB_AUDIO_MIC_MUX_MASK 0x03
+#define USB_AUDIO_MIC_MUX_SHIFT 0
+
+#define EAR_ADDER 0x28
+#define AUXO1_ADDER 0x29
+#define AUXO2_ADDER 0x2A
+#define AUXO3_ADDER 0x2B
+#define AUXO4_ADDER 0x2C
+#define SPKR1_ADDER 0x2D
+#define SPKR2_ADDER 0x2E
+#define LINE1_ADDER 0x2F
+#define LINE2_ADDER 0x30
+#define APGA2_TO_X_MASK 0x10
+#define APGA2_TO_X_SHIFT 4
+#define APGA1_TO_X_MASK 0x08
+#define APGA1_TO_X_SHIFT 3
+#define DAC3_TO_X_MASK 0x04
+#define DAC3_TO_X_SHIFT 2
+#define DAC2_TO_X_MASK 0x02
+#define DAC2_TO_X_SHIFT 1
+#define DAC1_TO_X_MASK 0x01
+#define DAC1_TO_X_SHIFT 0
+
+#define EAR_TO_MIC2 0x31
+#define SPKR1_TO_MIC2 0x32
+#define SPKR2_TO_MIC2 0x33
+#define EAR_TO_MIC2_MASK 0x01
+#define EAR_TO_MIC2_SHIFT 0
+
+#define ADC_LOW_PWR 0x35
+#define ADC_LOW_PWR_MASK 0x01
+#define ADC_LOW_PWR_SHIFT 0
+
+#define TX1 0x36
+#define TX2 0x37
+#define TXx_MUX_MASK 0x60
+#define TXx_MUX_SHIFT 6
+#define TXx_FS_MASK 0x10
+#define TXx_FS_SHIFT 4
+#define TXx_HP_FILTER_MASK 0x0C
+#define TXx_HP_FILTER_SHIFT 2
+#define TXx_PWR_MASK 0x02
+#define TXx_PWR_SHIFT 1
+#define ADCx_PWR_MASK 0x01
+#define ADCx_PWR_SHIFT 0
+
+#define RX1 0x38
+#define RX2 0x39
+#define RX3 0x3A
+#define RXx_DATA_MASK 0x70
+#define RXx_DATA_SHIFT 4
+#define RXx_PWR_MASK 0x08
+#define RXx_PWR_SHIFT 3
+#define DACx_PWR_MASK 0x04
+#define DACx_PWR_SHIFT 2
+#define DACx_PWR_MODE_MASK 0x03
+#define DACx_PWR_MODE_SHIFT 0
+
+#define TX_DPGA1 0x3B
+#define TX_DPGA2 0x3C
+#define TX_DPGAx_MASK 0x0F
+#define TX_DPGAx_SHIFT 0
+
+#define RX1_DPGA 0x3D
+#define RX2_DPGA 0x3E
+#define RX3_DPGA 0x3F
+#define RXx_DPGA_MASK 0x7F
+#define RXx_DPGA_SHIFT 0
+
+#define ST1_PGA 0x40
+#define ST2_PGA 0x41
+#define STx_HP_FILTER_MASK 0x60
+#define STx_HP_FILTER_SHIFT 6
+#define STx_MUX_MASK 0x10
+#define STx_MUX_SHIFT 4
+#define STx_PGA_MASK 0x0F
+#define STx_PGA_SHIFT 0
+
+#define CLOCK 0x42
+#define CLOCK_REF_SELECT_MASK 0x02
+#define CLOCK_REF_SELECT_SHIFT 1
+#define CLOCK_ENABLE_MASK 0x01
+#define CLOCK_ENABLE_SHIFT 0
+
+#define INTERFACE0 0x43
+#define INTERFACE1 0x45
+#define I2Sx_WORDLENGTH_MASK 0x40
+#define I2Sx_WORDLENGTH_SHIFT 6
+#define MASTER_GENx_PWR_MASK 0x20
+#define MASTER_GENx_PWR_SHIFT 5
+#define I2Sx_MODE_MASK 0x10
+#define I2Sx_MODE_SHIFT 4
+#define I2Sx_TRISTATE_MASK 0x08
+#define I2Sx_TRISTATE_SHIFT 3
+#define I2Sx_PULLDOWN_MASK 0x04
+#define I2Sx_PULLDOWN_SHIFT 2
+#define I2Sx_SR_MASK 0x03
+#define I2Sx_SR_SHIFT 0
+#define I2Sx_SR_8000Hz 0
+#define I2Sx_SR_16000Hz 1
+#define I2Sx_SR_44100Hz 2
+#define I2Sx_SR_48000Hz 3
+
+#define INTERFACE0_ULD 0x44
+#define INTERFACE1_ULD 0x46
+#define I2Sx_ULD_R_MASK 0x70
+#define I2Sx_ULD_R_SHIFT 4
+#define I2Sx_ULD_L_MASK 0x07
+#define I2Sx_ULD_L_SHIFT 0
+
+#define INTERFACE_SWAP 0x47
+#define IO_SWAP0_MASK 0x02
+#define IO_SWAP0_SHIFT 1
+#define IO_SWAP1_MASK 0x01
+#define IO_SWAP1_SHIFT 0
+
+#define AB5500_FIRST_REG MIC_BIAS1
+#define AB5500_LAST_REG INTERFACE_SWAP
+
+#define AB5500_VIRTUAL_REG1 (AB5500_LAST_REG + 1)
+#define IF0_DLD_L_PW_SHIFT 0
+#define IF0_DLD_R_PW_SHIFT 1
+#define IF0_ULD_L_PW_SHIFT 2
+#define IF0_ULD_R_PW_SHIFT 3
+#define IF1_DLD_L_PW_SHIFT 4
+#define IF1_DLD_R_PW_SHIFT 5
+#define IF1_ULD_L_PW_SHIFT 6
+#define IF1_ULD_R_PW_SHIFT 7
+
+#define AB5500_VIRTUAL_REG2 (AB5500_LAST_REG + 2)
+#define MIC1P1_PW_SHIFT 0
+#define MIC1N1_PW_SHIFT 1
+#define MIC1P2_PW_SHIFT 2
+#define MIC1N2_PW_SHIFT 3
+#define MIC2P1_PW_SHIFT 4
+#define MIC2N1_PW_SHIFT 5
+#define MIC2P2_PW_SHIFT 6
+#define MIC2N2_PW_SHIFT 7
+
+#define AB5500_VIRTUAL_REG3 (AB5500_LAST_REG + 3)
+#define SPKR1_MODE_MASK 0x03
+#define SPKR1_MODE_SHIFT 0
+#define SPKR1_MODE_VBR_VALUE 0
+#define SPKR1_MODE_CLS_D_VALUE 1
+#define SPKR1_MODE_CLS_AB_VALUE 2
+#define SPKR1_ADDER_PWR_SHIFT 2
+#define SPKR1_PWR_SHIFT 3
+#define SPKR2_MODE_MASK 0x10
+#define SPKR2_MODE_SHIFT 4
+#define SPKR2_MODE_VBR_VALUE 0
+#define SPKR2_MODE_CLS_D_VALUE 1
+#define SPKR2_ADDER_PWR_SHIFT 5
+#define SPKR2_PWR_SHIFT 6
+
+#define AB5500_VIRTUAL_REG4 (AB5500_LAST_REG + 4)
+#define PWM_SPKR1_PWR_SHIFT 0
+#define PWM_SPKR2_PWR_SHIFT 1
+#define PWM_SPKR1N_PWR_SHIFT 2
+#define PWM_SPKR1P_PWR_SHIFT 3
+#define PWM_SPKR2N_PWR_SHIFT 4
+#define PWM_SPKR2P_PWR_SHIFT 5
+
+#define AB5500_VIRTUAL_REG5 (AB5500_LAST_REG + 5)
+#define PWM_SPKR1N_SEL_SHIFT 0
+#define PWM_SPKR1P_SEL_SHIFT 1
+#define PWM_SPKR2N_SEL_SHIFT 2
+#define PWM_SPKR2P_SEL_SHIFT 3
+
+#define DUMMY_REG 0xff
+
+/* #define SPKR1_PWR_VBR_SHIFT 0 */
+/* #define SPKR1_PWR_CLS_D_SHIFT 1 */
+/* #define SPKR1_PWR_CLS_AB_SHIFT 2 */
+/* #define SPKR2_PWR_VBR_SHIFT 3 */
+/* #define SPKR2_PWR_CLS_D_SHIFT 4 */
+/* #define SPKR2_PWR_CLS_AB_SHIFT 5 */
+
+#endif
diff --git a/sound/soc/codecs/ab8500_audio.c b/sound/soc/codecs/ab8500_audio.c
new file mode 100644
index 00000000000..f65ebe5b786
--- /dev/null
+++ b/sound/soc/codecs/ab8500_audio.c
@@ -0,0 +1,2531 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Mikko J. Lehto <mikko.lehto@symbio.com>,
+ *         Mikko Sarmanne <mikko.sarmanne@symbio.com>,
+ *         Jarmo K. Kuronen <jarmo.kuronen@symbio.com>,
+ *         Ola Lilja <ola.o.lilja@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/pm.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <sound/initval.h>
+#include <sound/soc.h>
+#include <sound/soc-dapm.h>
+#include <sound/tlv.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/ab8500/sysctrl.h>
+#include "ab8500_audio.h"
+
+/* To convert register definition shifts to masks */
+#define BMASK(bsft)	(1 << (bsft))
+
+/* Macrocell value definitions */
+#define CLK_32K_OUT2_DISABLE			0x01
+#define INACTIVE_RESET_AUDIO			0x02
+#define ENABLE_AUDIO_CLK_TO_AUDIO_BLK		0x10
+#define ENABLE_VINTCORE12_SUPPLY		0x04
+#define GPIO27_DIR_OUTPUT			0x04
+#define GPIO29_DIR_OUTPUT			0x10
+#define GPIO31_DIR_OUTPUT			0x40
+
+/* Macrocell register definitions */
+#define AB8500_CTRL3_REG			0x0200
+#define AB8500_GPIO_DIR4_REG			0x1013
+
+/*
+ * AB8500 register cache & default register settings
+ */
+static const u8 ab8500_reg_cache[AB8500_CACHEREGNUM] = {
+	0x00, /* REG_POWERUP		(0x00) */
+	0x00, /* REG_AUDSWRESET		(0x01) */
+	0x00, /* REG_ADPATHENA		(0x02) */
+	0x00, /* REG_DAPATHENA		(0x03) */
+	0x00, /* REG_ANACONF1		(0x04) */
+	0x0F, /* REG_ANACONF2		(0x05) */
+	0x00, /* REG_DIGMICCONF		(0x06) */
+	0x00, /* REG_ANACONF3		(0x07) */
+	0x00, /* REG_ANACONF4		(0x08) */
+	0x00, /* REG_DAPATHCONF		(0x09) */
+	0x40, /* REG_MUTECONF		(0x0A) */
+	0x00, /* REG_SHORTCIRCONF	(0x0B) */
+	0x01, /* REG_ANACONF5		(0x0C) */
+	0x00, /* REG_ENVCPCONF		(0x0D) */
+	0x00, /* REG_SIGENVCONF		(0x0E) */
+	0x3F, /* REG_PWMGENCONF1	(0x0F) */
+	0x32, /* REG_PWMGENCONF2	(0x10) */
+	0x32, /* REG_PWMGENCONF3	(0x11) */
+	0x32, /* REG_PWMGENCONF4	(0x12) */
+	0x32, /* REG_PWMGENCONF5	(0x13) */
+	0x0F, /* REG_ANAGAIN1		(0x14) */
+	0x0F, /* REG_ANAGAIN2		(0x15) */
+	0x22, /* REG_ANAGAIN3		(0x16) */
+	0x55, /* REG_ANAGAIN4		(0x17) */
+	0x13, /* REG_DIGLINHSLGAIN	(0x18) */
+	0x13, /* REG_DIGLINHSRGAIN	(0x19) */
+	0x00, /* REG_ADFILTCONF		(0x1A) */
+	0x00, /* REG_DIGIFCONF1		(0x1B) */
+	0x02, /* REG_DIGIFCONF2		(0x1C) */
+	0x00, /* REG_DIGIFCONF3		(0x1D) */
+	0x02, /* REG_DIGIFCONF4		(0x1E) */
+	0xCC, /* REG_ADSLOTSEL1		(0xCC) */
+	0xCC, /* REG_ADSLOTSEL2		(0xCC) */
+	0xCC, /* REG_ADSLOTSEL3		(0xCC) */
+	0xCC, /* REG_ADSLOTSEL4		(0xCC) */
+	0xCC, /* REG_ADSLOTSEL5		(0xCC) */
+	0xCC, /* REG_ADSLOTSEL6		(0xCC) */
+	0xCC, /* REG_ADSLOTSEL7		(0xCC) */
+	0xCC, /* REG_ADSLOTSEL8		(0xCC) */
+	0xCC, /* REG_ADSLOTSEL9		(0xCC) */
+	0xCC, /* REG_ADSLOTSEL10	(0xCC) */
+	0xCC, /* REG_ADSLOTSEL11	(0xCC) */
+	0xCC, /* REG_ADSLOTSEL12	(0xCC) */
+	0xCC, /* REG_ADSLOTSEL13	(0xCC) */
+	0xCC, /* REG_ADSLOTSEL14	(0xCC) */
+	0xCC, /* REG_ADSLOTSEL15	(0xCC) */
+	0xCC, /* REG_ADSLOTSEL16	(0xCC) */
+	0x00, /* REG_ADSLOTHIZCTRL1	(0x2F) */
+	0x00, /* REG_ADSLOTHIZCTRL2	(0x30) */
+	0x00, /* REG_ADSLOTHIZCTRL3	(0x31) */
+	0x00, /* REG_ADSLOTHIZCTRL4	(0x32) */
+	0x08, /* REG_DASLOTCONF1	(0x33) */
+	0x08, /* REG_DASLOTCONF2	(0x34) */
+	0x08, /* REG_DASLOTCONF3	(0x35) */
+	0x08, /* REG_DASLOTCONF4	(0x36) */
+	0x08, /* REG_DASLOTCONF5	(0x37) */
+	0x08, /* REG_DASLOTCONF6	(0x38) */
+	0x08, /* REG_DASLOTCONF7	(0x39) */
+	0x08, /* REG_DASLOTCONF8	(0x3A) */
+	0x00, /* REG_CLASSDCONF1	(0x3B) */
+	0x00, /* REG_CLASSDCONF2	(0x3C) */
+	0x84, /* REG_CLASSDCONF3	(0x3D) */
+	0x00, /* REG_DMICFILTCONF	(0x3E) */
+	0xFE, /* REG_DIGMULTCONF1	(0x3F) */
+	0xC0, /* REG_DIGMULTCONF2	(0x40) */
+	0x3F, /* REG_ADDIGGAIN1		(0x41) */
+	0x3F, /* REG_ADDIGGAIN2		(0x42) */
+	0x1F, /* REG_ADDIGGAIN3		(0x43) */
+	0x1F, /* REG_ADDIGGAIN4		(0x44) */
+	0x3F, /* REG_ADDIGGAIN5		(0x45) */
+	0x3F, /* REG_ADDIGGAIN6		(0x46) */
+	0x1F, /* REG_DADIGGAIN1		(0x47) */
+	0x1F, /* REG_DADIGGAIN2		(0x48) */
+	0x3F, /* REG_DADIGGAIN3		(0x49) */
+	0x3F, /* REG_DADIGGAIN4		(0x4A) */
+	0x3F, /* REG_DADIGGAIN5		(0x4B) */
+	0x3F, /* REG_DADIGGAIN6		(0x4C) */
+	0x3F, /* REG_ADDIGLOOPGAIN1	(0x4D) */
+	0x3F, /* REG_ADDIGLOOPGAIN2	(0x4E) */
+	0x00, /* REG_HSLEARDIGGAIN	(0x4F) */
+	0x00, /* REG_HSRDIGGAIN		(0x50) */
+	0x1F, /* REG_SIDFIRGAIN1	(0x51) */
+	0x1F, /* REG_SIDFIRGAIN2	(0x52) */
+	0x00, /* REG_ANCCONF1		(0x53) */
+	0x00, /* REG_ANCCONF2		(0x54) */
+	0x00, /* REG_ANCCONF3		(0x55) */
+	0x00, /* REG_ANCCONF4		(0x56) */
+	0x00, /* REG_ANCCONF5		(0x57) */
+	0x00, /* REG_ANCCONF6		(0x58) */
+	0x00, /* REG_ANCCONF7		(0x59) */
+	0x00, /* REG_ANCCONF8		(0x5A) */
+	0x00, /* REG_ANCCONF9		(0x5B) */
+	0x00, /* REG_ANCCONF10		(0x5C) */
+	0x00, /* REG_ANCCONF11		(0x5D) - read only */
+	0x00, /* REG_ANCCONF12		(0x5E) - read only */
+	0x00, /* REG_ANCCONF13		(0x5F) - read only */
+	0x00, /* REG_ANCCONF14		(0x60) - read only */
+	0x00, /* REG_SIDFIRADR		(0x61) */
+	0x00, /* REG_SIDFIRCOEF1	(0x62) */
+	0x00, /* REG_SIDFIRCOEF2	(0x63) */
+	0x00, /* REG_SIDFIRCONF		(0x64) */
+	0x00, /* REG_AUDINTMASK1	(0x65) */
+	0x00, /* REG_AUDINTSOURCE1	(0x66) - read only */
+	0x00, /* REG_AUDINTMASK2	(0x67) */
+	0x00, /* REG_AUDINTSOURCE2	(0x68) - read only */
+	0x00, /* REG_FIFOCONF1		(0x69) */
+	0x00, /* REG_FIFOCONF2		(0x6A) */
+	0x00, /* REG_FIFOCONF3		(0x6B) */
+	0x00, /* REG_FIFOCONF4		(0x6C) */
+	0x00, /* REG_FIFOCONF5		(0x6D) */
+	0x00, /* REG_FIFOCONF6		(0x6E) */
+	0x02, /* REG_AUDREV		(0x6F) - read only */
+};
+
+static struct snd_soc_codec *ab8500_codec;
+
+
+/* Reads an arbitrary register from the ab8500 chip.
+*/
+static int ab8500_codec_read_reg(struct snd_soc_codec *codec, unsigned int bank,
+		unsigned int reg)
+{
+	u8 value;
+	int status = abx500_get_register_interruptible(
+		codec->dev, bank, reg, &value);
+
+	if (status < 0) {
+		pr_err("%s: Register (%02x:%02x) read failed (%d).\n",
+			__func__, (u8)bank, (u8)reg, status);
+	} else {
+		pr_debug("Read 0x%02x from register %02x:%02x\n",
+			(u8)value, (u8)bank, (u8)reg);
+		status = value;
+	}
+
+	return status;
+}
+
+/* Writes an arbitrary register to the ab8500 chip.
+ */
+static int ab8500_codec_write_reg(struct snd_soc_codec *codec, unsigned int bank,
+		unsigned int reg, unsigned int value)
+{
+	int status = abx500_set_register_interruptible(
+		codec->dev, bank, reg, value);
+
+	if (status < 0) {
+		pr_err("%s: Register (%02x:%02x) write failed (%d).\n",
+			__func__, (u8)bank, (u8)reg, status);
+	} else {
+		pr_debug("Wrote 0x%02x into register %02x:%02x\n",
+			(u8)value, (u8)bank, (u8)reg);
+	}
+
+	return status;
+}
+
+/* Reads an audio register from the cache.
+ */
+static unsigned int ab8500_codec_read_reg_audio(struct snd_soc_codec *codec,
+		unsigned int reg)
+{
+	u8 *cache = codec->reg_cache;
+	return cache[reg];
+}
+
+/* Reads an audio register from the hardware.
+ */
+static int ab8500_codec_read_reg_audio_nocache(struct snd_soc_codec *codec,
+		unsigned int reg)
+{
+	u8 *cache = codec->reg_cache;
+	int value = ab8500_codec_read_reg(codec, AB8500_AUDIO, reg);
+
+	if (value >= 0)
+		cache[reg] = value;
+
+	return value;
+}
+
+/* Writes an audio register to the hardware and cache.
+ */
+static int ab8500_codec_write_reg_audio(struct snd_soc_codec *codec,
+		unsigned int reg, unsigned int value)
+{
+	u8 *cache = codec->reg_cache;
+	int status = ab8500_codec_write_reg(codec, AB8500_AUDIO, reg, value);
+
+	if (status >= 0)
+		cache[reg] = value;
+
+	return status;
+}
+
+/* Dumps all audio registers.
+ */
+static inline void ab8500_codec_dump_all_reg(struct snd_soc_codec *codec)
+{
+	int i;
+
+	pr_debug("%s Enter.\n", __func__);
+
+	for (i = AB8500_FIRST_REG; i <= AB8500_LAST_REG; i++)
+		ab8500_codec_read_reg_audio_nocache(codec, i);
+}
+
+/* Updates an audio register.
+ */
+static inline int ab8500_codec_update_reg_audio(struct snd_soc_codec *codec,
+		unsigned int reg, unsigned int clr, unsigned int ins)
+{
+	unsigned int new, old;
+
+	old = ab8500_codec_read_reg_audio(codec, reg);
+	new = (old & ~clr) | ins;
+	if (old == new)
+		return 0;
+
+	return ab8500_codec_write_reg_audio(codec, reg, new);
+}
+
+/* Whether widget's register definitions should be inverted or not */
+enum control_inversion {
+	NORMAL = 0,
+	INVERT = 1
+};
+
+/* HS left channel mute control */
+static const struct snd_kcontrol_new dapm_hsl_mute[] = {
+	SOC_DAPM_SINGLE("Playback Switch", REG_MUTECONF,
+			REG_MUTECONF_MUTHSL, 1, INVERT),
+};
+
+/* HS right channel mute control */
+static const struct snd_kcontrol_new dapm_hsr_mute[] = {
+	SOC_DAPM_SINGLE("Playback Switch", REG_MUTECONF,
+			REG_MUTECONF_MUTHSR, 1, INVERT),
+};
+
+/* Earpiece mute control */
+static const struct snd_kcontrol_new dapm_ear_mute[] = {
+	SOC_DAPM_SINGLE("Playback Switch", REG_MUTECONF,
+			REG_MUTECONF_MUTEAR, 1, INVERT),
+};
+
+/* IHF left channel mute control */
+static const struct snd_kcontrol_new dapm_ihfl_mute[] = {
+	SOC_DAPM_SINGLE("Playback Switch", REG_DIGMULTCONF2,
+			REG_DIGMULTCONF2_DATOHFLEN, 1, NORMAL),
+};
+
+/* IHF right channel mute control */
+static const struct snd_kcontrol_new dapm_ihfr_mute[] = {
+	SOC_DAPM_SINGLE("Playback Switch", REG_DIGMULTCONF2,
+			REG_DIGMULTCONF2_DATOHFREN, 1, NORMAL),
+};
+
+/* Mic 1 mute control */
+static const struct snd_kcontrol_new dapm_mic1_mute[] = {
+	SOC_DAPM_SINGLE("Capture Switch", REG_ANACONF2,
+			REG_ANACONF2_MUTMIC1, 1, INVERT),
+};
+
+/* Mic 2 mute control */
+static const struct snd_kcontrol_new dapm_mic2_mute[] = {
+	SOC_DAPM_SINGLE("Capture Switch", REG_ANACONF2,
+			REG_ANACONF2_MUTMIC2, 1, INVERT),
+};
+
+/* LineIn left channel mute control */
+static const struct snd_kcontrol_new dapm_linl_mute[] = {
+	SOC_DAPM_SINGLE("Capture Switch", REG_ANACONF2,
+			REG_ANACONF2_MUTLINL, 1, INVERT),
+};
+
+/* LineIn right channel mute control */
+static const struct snd_kcontrol_new dapm_linr_mute[] = {
+	SOC_DAPM_SINGLE("Capture Switch", REG_ANACONF2,
+			REG_ANACONF2_MUTLINR, 1, INVERT),
+};
+
+/* DMic 1 mute control */
+static const struct snd_kcontrol_new dapm_dmic1_mute[] = {
+	SOC_DAPM_SINGLE("Capture Switch", REG_DIGMICCONF,
+			REG_DIGMICCONF_ENDMIC1, 1, NORMAL),
+};
+
+/* DMic 2 mute control */
+static const struct snd_kcontrol_new dapm_dmic2_mute[] = {
+	SOC_DAPM_SINGLE("Capture Switch", REG_DIGMICCONF,
+			REG_DIGMICCONF_ENDMIC2, 1, NORMAL),
+};
+
+/* DMic 3 mute control */
+static const struct snd_kcontrol_new dapm_dmic3_mute[] = {
+	SOC_DAPM_SINGLE("Capture Switch", REG_DIGMICCONF,
+			REG_DIGMICCONF_ENDMIC3, 1, NORMAL),
+};
+
+/* DMic 4 mute control */
+static const struct snd_kcontrol_new dapm_dmic4_mute[] = {
+	SOC_DAPM_SINGLE("Capture Switch", REG_DIGMICCONF,
+			REG_DIGMICCONF_ENDMIC4, 1, NORMAL),
+};
+
+/* DMic 5 mute control */
+static const struct snd_kcontrol_new dapm_dmic5_mute[] = {
+	SOC_DAPM_SINGLE("Capture Switch", REG_DIGMICCONF,
+			REG_DIGMICCONF_ENDMIC5, 1, NORMAL),
+};
+
+/* DMic 6 mute control */
+static const struct snd_kcontrol_new dapm_dmic6_mute[] = {
+	SOC_DAPM_SINGLE("Capture Switch", REG_DIGMICCONF,
+			REG_DIGMICCONF_ENDMIC6, 1, NORMAL),
+};
+
+/* ANC to Earpiece mute control */
+static const struct snd_kcontrol_new dapm_anc_ear_mute[] = {
+	SOC_DAPM_SINGLE("Playback Switch", REG_DIGMULTCONF1,
+			REG_DIGMULTCONF1_ANCSEL, 1, NORMAL),
+};
+
+/* Earpiece source selector control */
+static const char *enum_ear_source[] = {"Headset Left", "IHF Left"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_ear_source, REG_DMICFILTCONF,
+			REG_DMICFILTCONF_DA3TOEAR, enum_ear_source);
+
+static const struct snd_kcontrol_new dapm_ear_source[] = {
+	SOC_DAPM_ENUM("Earpiece Source", dapm_enum_ear_source),
+};
+
+/* IHF / ANC selector control */
+static const char *enum_ihfx_sel[] = {"Audio Path", "ANC"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_ihfl_sel, REG_DIGMULTCONF2,
+			REG_DIGMULTCONF2_HFLSEL, enum_ihfx_sel);
+
+static const struct snd_kcontrol_new dapm_ihfl_select[] = {
+	SOC_DAPM_ENUM("IHF Left Source", dapm_enum_ihfl_sel),
+};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_ihfr_sel, REG_DIGMULTCONF2,
+			REG_DIGMULTCONF2_HFRSEL, enum_ihfx_sel);
+
+static const struct snd_kcontrol_new dapm_ihfr_select[] = {
+	SOC_DAPM_ENUM("IHF Right Source", dapm_enum_ihfr_sel),
+};
+
+/* Mic 1A or 1B selector control */
+static const char *enum_mic1ab_sel[] = {"Mic 1A", "Mic 1B"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_mic1ab_sel, REG_ANACONF3,
+			REG_ANACONF3_MIC1SEL, enum_mic1ab_sel);
+
+static const struct snd_kcontrol_new dapm_mic1ab_select[] = {
+	SOC_DAPM_ENUM("Mic 1A or 1B Select", dapm_enum_mic1ab_sel),
+};
+
+/* Mic 2 or LineIn Right selector control */
+static const char *enum_mic2lr_sel[] = {"Mic 2", "LineIn Right"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_mic2lr_sel, REG_ANACONF3,
+			REG_ANACONF3_LINRSEL, enum_mic2lr_sel);
+
+static const struct snd_kcontrol_new dapm_mic2lr_select[] = {
+	SOC_DAPM_ENUM("Mic 2 or LINR Select", dapm_enum_mic2lr_sel),
+};
+
+/* AD1 selector control */
+static const char *enum_ad1_sel[] = {"LineIn Left", "DMic 1"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_ad1_sel, REG_DIGMULTCONF1,
+			REG_DIGMULTCONF1_AD1SEL, enum_ad1_sel);
+
+static const struct snd_kcontrol_new dapm_ad1_select[] = {
+	SOC_DAPM_ENUM("AD 1 Select", dapm_enum_ad1_sel),
+};
+
+/* AD2 selector control */
+static const char *enum_ad2_sel[] = {"LineIn Right", "DMic 2"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_ad2_sel, REG_DIGMULTCONF1,
+			REG_DIGMULTCONF1_AD2SEL, enum_ad2_sel);
+
+static const struct snd_kcontrol_new dapm_ad2_select[] = {
+	SOC_DAPM_ENUM("AD 2 Select", dapm_enum_ad2_sel),
+};
+
+/* AD3 selector control */
+static const char *enum_ad3_sel[] = {"Mic 1", "DMic 3"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_ad3_sel, REG_DIGMULTCONF1,
+			REG_DIGMULTCONF1_AD3SEL, enum_ad3_sel);
+
+static const struct snd_kcontrol_new dapm_ad3_select[] = {
+	SOC_DAPM_ENUM("AD 3 Select", dapm_enum_ad3_sel),
+};
+
+/* AD5 selector control */
+static const char *enum_ad5_sel[] = {"Mic 2", "DMic 5"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_ad5_sel, REG_DIGMULTCONF1,
+			REG_DIGMULTCONF1_AD5SEL, enum_ad5_sel);
+
+static const struct snd_kcontrol_new dapm_ad5_select[] = {
+	SOC_DAPM_ENUM("AD 5 Select", dapm_enum_ad5_sel),
+};
+
+/* AD6 selector control */
+static const char *enum_ad6_sel[] = {"Mic 1", "DMic 6"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_ad6_sel, REG_DIGMULTCONF1,
+			REG_DIGMULTCONF1_AD6SEL, enum_ad6_sel);
+
+static const struct snd_kcontrol_new dapm_ad6_select[] = {
+	SOC_DAPM_ENUM("AD 6 Select", dapm_enum_ad6_sel),
+};
+
+/* ANC input selector control */
+static const char *enum_anc_in_sel[] = {"Mic 1 / DMic 6", "Mic 2 / DMic 5"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_anc_in_sel, REG_DMICFILTCONF,
+			REG_DMICFILTCONF_ANCINSEL, enum_anc_in_sel);
+
+static const struct snd_kcontrol_new dapm_anc_in_select[] = {
+	SOC_DAPM_ENUM("ANC Source", dapm_enum_anc_in_sel),
+};
+
+/* ANC enable control */
+static const char *enum_anc_dis_ena[] = {"Disabled", "Enabled"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_anc_enable, REG_ANCCONF1,
+			REG_ANCCONF1_ENANC, enum_anc_dis_ena);
+
+static const struct snd_kcontrol_new dapm_anc_enable[] = {
+	SOC_DAPM_ENUM("ANC", dapm_enum_anc_enable),
+};
+
+/* Sidetone left input selector control */
+static const char *enum_stfir1_in_sel[] = {
+	"LineIn Left", "LineIn Right", "Mic 1", "Headset Left"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_stfir1_in_sel, REG_DIGMULTCONF2,
+			REG_DIGMULTCONF2_FIRSID1SEL, enum_stfir1_in_sel);
+
+static const struct snd_kcontrol_new dapm_stfir1_in_select[] = {
+	SOC_DAPM_ENUM("Sidetone Left Source", dapm_enum_stfir1_in_sel),
+};
+
+/* Sidetone right input selector control */
+static const char *enum_stfir2_in_sel[] = {
+	"LineIn Right", "Mic 1", "DMic 4", "Headset Right"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_stfir2_in_sel, REG_DIGMULTCONF2,
+			REG_DIGMULTCONF2_FIRSID2SEL, enum_stfir2_in_sel);
+
+static const struct snd_kcontrol_new dapm_stfir2_in_select[] = {
+	SOC_DAPM_ENUM("Sidetone Right Source", dapm_enum_stfir2_in_sel),
+};
+
+/* Vibra path selector control */
+static const char *enum_pwm2vibx[] = {"Audio Path", "PWM Generator"};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_pwm2vib1, REG_PWMGENCONF1,
+			REG_PWMGENCONF1_PWMTOVIB1, enum_pwm2vibx);
+
+static const struct snd_kcontrol_new dapm_pwm2vib1[] = {
+	SOC_DAPM_ENUM("Vibra 1 Controller", dapm_enum_pwm2vib1),
+};
+
+static SOC_ENUM_SINGLE_DECL(dapm_enum_pwm2vib2, REG_PWMGENCONF1,
+			REG_PWMGENCONF1_PWMTOVIB2, enum_pwm2vibx);
+
+static const struct snd_kcontrol_new dapm_pwm2vib2[] = {
+	SOC_DAPM_ENUM("Vibra 2 Controller", dapm_enum_pwm2vib2),
+};
+
+static const struct snd_soc_dapm_widget ab8500_dapm_widgets[] = {
+	/* Headset path */
+
+	SND_SOC_DAPM_AIF_IN("DA_IN1", "ab8500_0p", 0, SND_SOC_NOPM, 0, 0),
+	SND_SOC_DAPM_AIF_IN("DA_IN2", "ab8500_0p", 0, SND_SOC_NOPM, 0, 0),
+
+	/* XXX SwapDA12_34 */
+
+	SND_SOC_DAPM_MIXER("DA1 Channel Gain", REG_DAPATHENA,
+			REG_DAPATHENA_ENDA1, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("DA2 Channel Gain", REG_DAPATHENA,
+			REG_DAPATHENA_ENDA2, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("HSL Digital Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("HSR Digital Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("HSL DAC", REG_DAPATHCONF,
+			REG_DAPATHCONF_ENDACHSL, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("HSR DAC", REG_DAPATHCONF,
+			REG_DAPATHCONF_ENDACHSR, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("HSL DAC Driver", REG_ANACONF3,
+			REG_ANACONF3_ENDRVHSL, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("HSR DAC Driver", REG_ANACONF3,
+			REG_ANACONF3_ENDRVHSR, 0, NULL, 0),
+
+	SND_SOC_DAPM_SWITCH("Headset Left", SND_SOC_NOPM, 0, 0, dapm_hsl_mute),
+	SND_SOC_DAPM_SWITCH("Headset Right", SND_SOC_NOPM, 0, 0, dapm_hsr_mute),
+
+	SND_SOC_DAPM_MIXER("HSL Enable", REG_ANACONF4,
+			REG_ANACONF4_ENHSL, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("HSR Enable", REG_ANACONF4,
+			REG_ANACONF4_ENHSR, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("Charge Pump", REG_ANACONF5,
+			REG_ANACONF5_ENCPHS, 0, NULL, 0),
+
+	SND_SOC_DAPM_OUTPUT("HSL"),
+	SND_SOC_DAPM_OUTPUT("HSR"),
+
+
+	/* Earpiece path */
+
+	SND_SOC_DAPM_MUX("Earpiece Source Playback Route",
+			SND_SOC_NOPM, 0, 0, dapm_ear_source),
+
+	SND_SOC_DAPM_MIXER("EAR DAC", REG_DAPATHCONF,
+			REG_DAPATHCONF_ENDACEAR, 0, NULL, 0),
+
+	SND_SOC_DAPM_SWITCH("Earpiece", SND_SOC_NOPM, 0, 0, dapm_ear_mute),
+
+	SND_SOC_DAPM_MIXER("EAR Enable", REG_ANACONF4,
+			REG_ANACONF4_ENEAR, 0, NULL, 0),
+
+	SND_SOC_DAPM_OUTPUT("EAR"),
+
+
+	/* Handsfree path */
+
+	SND_SOC_DAPM_AIF_IN("DA_IN3", "ab8500_0p", 0, SND_SOC_NOPM, 0, 0),
+	SND_SOC_DAPM_AIF_IN("DA_IN4", "ab8500_0p", 0, SND_SOC_NOPM, 0, 0),
+
+	/* XXX SwapDA12_34 */
+
+	SND_SOC_DAPM_MIXER("DA3 Channel Gain", REG_DAPATHENA,
+			REG_DAPATHENA_ENDA3, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("DA4 Channel Gain", REG_DAPATHENA,
+			REG_DAPATHENA_ENDA4, 0, NULL, 0),
+
+	SND_SOC_DAPM_MUX("IHF Left Source Playback Route",
+			SND_SOC_NOPM, 0, 0, dapm_ihfl_select),
+	SND_SOC_DAPM_MUX("IHF Right Source Playback Route",
+			SND_SOC_NOPM, 0, 0, dapm_ihfr_select),
+
+	SND_SOC_DAPM_SWITCH("IHF Left", SND_SOC_NOPM, 0, 0, dapm_ihfl_mute),
+	SND_SOC_DAPM_SWITCH("IHF Right", SND_SOC_NOPM, 0, 0, dapm_ihfr_mute),
+
+	SND_SOC_DAPM_MIXER("IHFL DAC", REG_DAPATHCONF,
+			REG_DAPATHCONF_ENDACHFL, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("IHFR DAC", REG_DAPATHCONF,
+			REG_DAPATHCONF_ENDACHFR, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("IHFL Enable", REG_ANACONF4,
+			REG_ANACONF4_ENHFL, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("IHFR Enable", REG_ANACONF4,
+			REG_ANACONF4_ENHFR, 0, NULL, 0),
+
+	SND_SOC_DAPM_OUTPUT("IHFL"),
+	SND_SOC_DAPM_OUTPUT("IHFR"),
+
+
+	/* Vibrator path */
+
+	SND_SOC_DAPM_AIF_IN("DA_IN5", "ab8500_0p", 0, SND_SOC_NOPM, 0, 0),
+	SND_SOC_DAPM_AIF_IN("DA_IN6", "ab8500_0p", 0, SND_SOC_NOPM, 0, 0),
+
+	SND_SOC_DAPM_MIXER("DA5 Channel Gain", REG_DAPATHENA,
+			REG_DAPATHENA_ENDA5, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("DA6 Channel Gain", REG_DAPATHENA,
+			REG_DAPATHENA_ENDA6, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("VIB1 DAC", REG_DAPATHCONF,
+			REG_DAPATHCONF_ENDACVIB1, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("VIB2 DAC", REG_DAPATHCONF,
+			REG_DAPATHCONF_ENDACVIB2, 0, NULL, 0),
+
+	SND_SOC_DAPM_INPUT("PWMGEN1"),
+	SND_SOC_DAPM_INPUT("PWMGEN2"),
+
+	SND_SOC_DAPM_MUX("Vibra 1 Controller Playback Route",
+			SND_SOC_NOPM, 0, 0, dapm_pwm2vib1),
+	SND_SOC_DAPM_MUX("Vibra 2 Controller Playback Route",
+			SND_SOC_NOPM, 0, 0, dapm_pwm2vib2),
+
+	SND_SOC_DAPM_MIXER("VIB1 Enable", REG_ANACONF4,
+			REG_ANACONF4_ENVIB1, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("VIB2 Enable", REG_ANACONF4,
+			REG_ANACONF4_ENVIB2, 0, NULL, 0),
+
+	SND_SOC_DAPM_OUTPUT("VIB1"),
+	SND_SOC_DAPM_OUTPUT("VIB2"),
+
+
+	/* LineIn & Microphone 2 path */
+
+	SND_SOC_DAPM_INPUT("LINL"),
+	SND_SOC_DAPM_INPUT("LINR"),
+	SND_SOC_DAPM_INPUT("MIC2"),
+
+	SND_SOC_DAPM_SWITCH("LineIn Left", SND_SOC_NOPM, 0, 0, dapm_linl_mute),
+	SND_SOC_DAPM_SWITCH("LineIn Right", SND_SOC_NOPM, 0, 0, dapm_linr_mute),
+	SND_SOC_DAPM_SWITCH("Mic 2", SND_SOC_NOPM, 0, 0, dapm_mic2_mute),
+
+	SND_SOC_DAPM_MIXER("LINL Enable", REG_ANACONF2,
+			REG_ANACONF2_ENLINL, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("LINR Enable", REG_ANACONF2,
+			REG_ANACONF2_ENLINR, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("MIC2 Enable", REG_ANACONF2,
+			REG_ANACONF2_ENMIC2, 0, NULL, 0),
+
+	SND_SOC_DAPM_MUX("Mic 2 or LINR Select Capture Route",
+			SND_SOC_NOPM, 0, 0, dapm_mic2lr_select),
+
+	SND_SOC_DAPM_MIXER("LINL ADC", REG_ANACONF3,
+			REG_ANACONF3_ENADCLINL, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("LINR ADC", REG_ANACONF3,
+			REG_ANACONF3_ENADCLINR, 0, NULL, 0),
+
+	SND_SOC_DAPM_MUX("AD 1 Select Capture Route",
+			SND_SOC_NOPM, 0, 0, dapm_ad1_select),
+	SND_SOC_DAPM_MUX("AD 2 Select Capture Route",
+			SND_SOC_NOPM, 0, 0, dapm_ad2_select),
+
+	SND_SOC_DAPM_MIXER("AD1 Channel Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("AD2 Channel Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("AD1 Enable", REG_ADPATHENA,
+			REG_ADPATHENA_ENAD12, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("AD2 Enable", REG_ADPATHENA,
+			REG_ADPATHENA_ENAD12, 0, NULL, 0),
+
+	SND_SOC_DAPM_AIF_OUT("AD_OUT1", "ab8500_0c", 0, SND_SOC_NOPM, 0, 0),
+	SND_SOC_DAPM_AIF_OUT("AD_OUT2", "ab8500_0c", 0, SND_SOC_NOPM, 0, 0),
+
+
+	/* Microphone 1 path */
+
+	SND_SOC_DAPM_INPUT("MIC1A"),
+	SND_SOC_DAPM_INPUT("MIC1B"),
+
+	SND_SOC_DAPM_MUX("Mic 1A or 1B Select Capture Route",
+			SND_SOC_NOPM, 0, 0, dapm_mic1ab_select),
+
+	SND_SOC_DAPM_SWITCH("Mic 1", SND_SOC_NOPM, 0, 0, dapm_mic1_mute),
+
+	SND_SOC_DAPM_MIXER("MIC1 Enable", REG_ANACONF2,
+			REG_ANACONF2_ENMIC1, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("MIC1 ADC", REG_ANACONF3,
+			REG_ANACONF3_ENADCMIC, 0, NULL, 0),
+
+	SND_SOC_DAPM_MUX("AD 3 Select Capture Route",
+			SND_SOC_NOPM, 0, 0, dapm_ad3_select),
+
+	SND_SOC_DAPM_MIXER("AD3 Channel Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("AD3 Enable", REG_ADPATHENA,
+			REG_ADPATHENA_ENAD34, 0, NULL, 0),
+
+	SND_SOC_DAPM_AIF_OUT("AD_OUT3", "ab8500_0c", 0, SND_SOC_NOPM, 0, 0),
+
+
+	/* HD Capture path */
+
+	SND_SOC_DAPM_MUX("AD 5 Select Capture Route",
+			SND_SOC_NOPM, 0, 0, dapm_ad5_select),
+	SND_SOC_DAPM_MUX("AD 6 Select Capture Route",
+			SND_SOC_NOPM, 0, 0, dapm_ad6_select),
+
+	SND_SOC_DAPM_MIXER("AD5 Channel Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("AD6 Channel Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("AD57 Enable", REG_ADPATHENA,
+			REG_ADPATHENA_ENAD5768, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("AD68 Enable", REG_ADPATHENA,
+			REG_ADPATHENA_ENAD5768, 0, NULL, 0),
+
+	SND_SOC_DAPM_AIF_OUT("AD_OUT57", "ab8500_0c", 0, SND_SOC_NOPM, 0, 0),
+	SND_SOC_DAPM_AIF_OUT("AD_OUT68", "ab8500_0c", 0, SND_SOC_NOPM, 0, 0),
+
+
+	/* Digital Microphone path */
+
+	SND_SOC_DAPM_INPUT("DMIC1"),
+	SND_SOC_DAPM_INPUT("DMIC2"),
+	SND_SOC_DAPM_INPUT("DMIC3"),
+	SND_SOC_DAPM_INPUT("DMIC4"),
+	SND_SOC_DAPM_INPUT("DMIC5"),
+	SND_SOC_DAPM_INPUT("DMIC6"),
+
+	SND_SOC_DAPM_SWITCH("DMic 1", SND_SOC_NOPM, 0, 0, dapm_dmic1_mute),
+	SND_SOC_DAPM_SWITCH("DMic 2", SND_SOC_NOPM, 0, 0, dapm_dmic2_mute),
+	SND_SOC_DAPM_SWITCH("DMic 3", SND_SOC_NOPM, 0, 0, dapm_dmic3_mute),
+	SND_SOC_DAPM_SWITCH("DMic 4", SND_SOC_NOPM, 0, 0, dapm_dmic4_mute),
+	SND_SOC_DAPM_SWITCH("DMic 5", SND_SOC_NOPM, 0, 0, dapm_dmic5_mute),
+	SND_SOC_DAPM_SWITCH("DMic 6", SND_SOC_NOPM, 0, 0, dapm_dmic6_mute),
+
+	SND_SOC_DAPM_MIXER("AD4 Channel Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("AD4 Enable", REG_ADPATHENA,
+			REG_ADPATHENA_ENAD34, 0, NULL, 0),
+
+	SND_SOC_DAPM_AIF_OUT("AD_OUT4", "ab8500_0c", 0, SND_SOC_NOPM, 0, 0),
+
+
+	/* LineIn Bypass path */
+
+	SND_SOC_DAPM_MIXER("LINL to HSL Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("LINR to HSR Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+
+
+	/* Analog Loopback path */
+
+	SND_SOC_DAPM_MIXER("AD1 to IHFL Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("AD2 to IHFR Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+
+
+	/* Acoustical Noise Cancellation path */
+
+	SND_SOC_DAPM_MUX("ANC Source Playback Route",
+			SND_SOC_NOPM, 0, 0, dapm_anc_in_select),
+
+	SND_SOC_DAPM_MUX("ANC Playback Switch",
+			SND_SOC_NOPM, 0, 0, dapm_anc_enable),
+
+	SND_SOC_DAPM_SWITCH("ANC to Earpiece",
+			SND_SOC_NOPM, 0, 0, dapm_anc_ear_mute),
+
+
+	/* Sidetone Filter path */
+
+	SND_SOC_DAPM_MUX("Sidetone Left Source Playback Route",
+			SND_SOC_NOPM, 0, 0, dapm_stfir1_in_select),
+	SND_SOC_DAPM_MUX("Sidetone Right Source Playback Route",
+			SND_SOC_NOPM, 0, 0, dapm_stfir2_in_select),
+
+	SND_SOC_DAPM_MIXER("STFIR1 Control", SND_SOC_NOPM, 0, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("STFIR2 Control", SND_SOC_NOPM, 0, 0, NULL, 0),
+
+	SND_SOC_DAPM_MIXER("STFIR1 Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+	SND_SOC_DAPM_MIXER("STFIR2 Gain", SND_SOC_NOPM, 0, 0, NULL, 0),
+};
+
+static const struct snd_soc_dapm_route intercon[] = {
+	/* Headset path */
+
+	{"DA1 Channel Gain", NULL, "DA_IN1"},
+	{"DA2 Channel Gain", NULL, "DA_IN2"},
+
+	{"HSL Digital Gain", NULL, "DA1 Channel Gain"},
+	{"HSR Digital Gain", NULL, "DA2 Channel Gain"},
+
+	{"HSL DAC", NULL, "HSL Digital Gain"},
+	{"HSR DAC", NULL, "HSR Digital Gain"},
+
+	{"HSL DAC Driver", NULL, "HSL DAC"},
+	{"HSR DAC Driver", NULL, "HSR DAC"},
+
+	{"Headset Left", "Playback Switch", "HSL DAC Driver"},
+	{"Headset Right", "Playback Switch", "HSR DAC Driver"},
+
+	{"HSL Enable", NULL, "Headset Left"},
+	{"HSR Enable", NULL, "Headset Right"},
+
+	{"Charge Pump", NULL, "HSL Enable"},
+	{"Charge Pump", NULL, "HSR Enable"},
+
+	{"HSL", NULL, "Charge Pump"},
+	{"HSR", NULL, "Charge Pump"},
+
+
+	/* Earpiece path */
+
+	{"Earpiece Source Playback Route", "Headset Left", "HSL Digital Gain"},
+	{"Earpiece Source Playback Route", "IHF Left", "IHF Left"},
+
+	{"EAR DAC", NULL, "Earpiece Source Playback Route"},
+
+	{"Earpiece", "Playback Switch", "EAR DAC"},
+
+	{"EAR Enable", NULL, "Earpiece"},
+
+	{"EAR", NULL, "EAR Enable"},
+
+
+	/* Handsfree path */
+
+	{"DA3 Channel Gain", NULL, "DA_IN3"},
+	{"DA4 Channel Gain", NULL, "DA_IN4"},
+
+	{"IHF Left Source Playback Route", "Audio Path", "DA3 Channel Gain"},
+	{"IHF Right Source Playback Route", "Audio Path", "DA4 Channel Gain"},
+
+	{"IHF Left", "Playback Switch", "IHF Left Source Playback Route"},
+	{"IHF Right", "Playback Switch", "IHF Right Source Playback Route"},
+
+	{"IHFL DAC", NULL, "IHF Left"},
+	{"IHFR DAC", NULL, "IHF Right"},
+
+	{"IHFL Enable", NULL, "IHFL DAC"},
+	{"IHFR Enable", NULL, "IHFR DAC"},
+
+	{"IHFL", NULL, "IHFL Enable"},
+	{"IHFR", NULL, "IHFR Enable"},
+
+
+	/* Vibrator path */
+
+	{"DA5 Channel Gain", NULL, "DA_IN5"},
+	{"DA6 Channel Gain", NULL, "DA_IN6"},
+
+	{"VIB1 DAC", NULL, "DA5 Channel Gain"},
+	{"VIB2 DAC", NULL, "DA6 Channel Gain"},
+
+	{"Vibra 1 Controller Playback Route", "Audio Path", "VIB1 DAC"},
+	{"Vibra 2 Controller Playback Route", "Audio Path", "VIB2 DAC"},
+	{"Vibra 1 Controller Playback Route", "PWM Generator", "PWMGEN1"},
+	{"Vibra 2 Controller Playback Route", "PWM Generator", "PWMGEN2"},
+
+	{"VIB1 Enable", NULL, "Vibra 1 Controller Playback Route"},
+	{"VIB2 Enable", NULL, "Vibra 2 Controller Playback Route"},
+
+	{"VIB1", NULL, "VIB1 Enable"},
+	{"VIB2", NULL, "VIB2 Enable"},
+
+
+	/* LineIn & Microphone 2 path */
+
+	{"LineIn Left", "Capture Switch", "LINL"},
+	{"LineIn Right", "Capture Switch", "LINR"},
+	{"Mic 2", "Capture Switch", "MIC2"},
+
+	{"LINL Enable", NULL, "LineIn Left"},
+	{"LINR Enable", NULL, "LineIn Right"},
+	{"MIC2 Enable", NULL, "Mic 2"},
+
+	{"Mic 2 or LINR Select Capture Route", "LineIn Right", "LINR Enable"},
+	{"Mic 2 or LINR Select Capture Route", "Mic 2", "MIC2 Enable"},
+
+	{"LINL ADC", NULL, "LINL Enable"},
+	{"LINR ADC", NULL, "Mic 2 or LINR Select Capture Route"},
+
+	{"AD 1 Select Capture Route", "LineIn Left", "LINL ADC"},
+	{"AD 2 Select Capture Route", "LineIn Right", "LINR ADC"},
+
+	{"AD1 Channel Gain", NULL, "AD 1 Select Capture Route"},
+	{"AD2 Channel Gain", NULL, "AD 2 Select Capture Route"},
+
+	{"AD1 Enable", NULL, "AD1 Channel Gain"},
+	{"AD2 Enable", NULL, "AD2 Channel Gain"},
+
+	{"AD_OUT1", NULL, "AD1 Enable"},
+	{"AD_OUT2", NULL, "AD2 Enable"},
+
+
+	/* Microphone 1 path */
+
+	{"Mic 1A or 1B Select Capture Route", "Mic 1A", "MIC1A"},
+	{"Mic 1A or 1B Select Capture Route", "Mic 1B", "MIC1B"},
+
+	{"Mic 1", "Capture Switch", "Mic 1A or 1B Select Capture Route"},
+
+	{"MIC1 Enable", NULL, "Mic 1"},
+
+	{"MIC1 ADC", NULL, "MIC1 Enable"},
+
+	{"AD 3 Select Capture Route", "Mic 1", "MIC1 ADC"},
+
+	{"AD3 Channel Gain", NULL, "AD 3 Select Capture Route"},
+
+	{"AD3 Enable", NULL, "AD3 Channel Gain"},
+
+	{"AD_OUT3", NULL, "AD3 Enable"},
+
+
+	/* HD Capture path */
+
+	{"AD 5 Select Capture Route", "Mic 2", "LINR ADC"},
+	{"AD 6 Select Capture Route", "Mic 1", "MIC1 ADC"},
+
+	{"AD5 Channel Gain", NULL, "AD 5 Select Capture Route"},
+	{"AD6 Channel Gain", NULL, "AD 6 Select Capture Route"},
+
+	{"AD57 Enable", NULL, "AD5 Channel Gain"},
+	{"AD68 Enable", NULL, "AD6 Channel Gain"},
+
+	{"AD_OUT57", NULL, "AD57 Enable"},
+	{"AD_OUT68", NULL, "AD68 Enable"},
+
+
+	/* Digital Microphone path */
+
+	{"DMic 1", "Capture Switch", "DMIC1"},
+	{"DMic 2", "Capture Switch", "DMIC2"},
+	{"DMic 3", "Capture Switch", "DMIC3"},
+	{"DMic 4", "Capture Switch", "DMIC4"},
+	{"DMic 5", "Capture Switch", "DMIC5"},
+	{"DMic 6", "Capture Switch", "DMIC6"},
+
+	{"AD 1 Select Capture Route", "DMic 1", "DMic 1"},
+	{"AD 2 Select Capture Route", "DMic 2", "DMic 2"},
+	{"AD 3 Select Capture Route", "DMic 3", "DMic 3"},
+	{"AD 5 Select Capture Route", "DMic 5", "DMic 5"},
+	{"AD 6 Select Capture Route", "DMic 6", "DMic 6"},
+
+	{"AD4 Channel Gain", NULL, "DMic 4"},
+
+	{"AD4 Enable", NULL, "AD4 Channel Gain"},
+
+	{"AD_OUT4", NULL, "AD4 Enable"},
+
+
+	/* LineIn Bypass path */
+
+	{"LINL to HSL Gain", NULL, "LINL Enable"},
+	{"LINR to HSR Gain", NULL, "LINR Enable"},
+
+	{"HSL DAC Driver", NULL, "LINL to HSL Gain"},
+	{"HSR DAC Driver", NULL, "LINR to HSR Gain"},
+
+
+	/* Analog Loopback path */
+
+	{"AD1 to IHFL Gain", NULL, "AD1 Channel Gain"},
+	{"AD2 to IHFR Gain", NULL, "AD2 Channel Gain"},
+
+	{"IHFL DAC", NULL, "AD1 to IHFL Gain"},
+	{"IHFR DAC", NULL, "AD2 to IHFR Gain"},
+
+
+	/* Acoustical Noise Cancellation path */
+
+	{"ANC Source Playback Route", "Mic 2 / DMic 5", "AD5 Channel Gain"},
+	{"ANC Source Playback Route", "Mic 1 / DMic 6", "AD6 Channel Gain"},
+
+	{"ANC Playback Switch", "Enabled", "ANC Source Playback Route"},
+
+	{"IHF Left Source Playback Route", "ANC", "ANC Playback Switch"},
+	{"IHF Right Source Playback Route", "ANC", "ANC Playback Switch"},
+	{"ANC to Earpiece", "Playback Switch", "ANC Playback Switch"},
+
+	{"HSL Digital Gain", NULL, "ANC to Earpiece"},
+
+
+	/* Sidetone Filter path */
+
+	{"Sidetone Left Source Playback Route", "LineIn Left", "AD1 Enable"},
+	{"Sidetone Left Source Playback Route", "LineIn Right", "AD2 Enable"},
+	{"Sidetone Left Source Playback Route", "Mic 1", "AD3 Enable"},
+	{"Sidetone Left Source Playback Route", "Headset Left", "DA_IN1"},
+	{"Sidetone Right Source Playback Route", "LineIn Right", "AD2 Enable"},
+	{"Sidetone Right Source Playback Route", "Mic 1", "AD3 Enable"},
+	{"Sidetone Right Source Playback Route", "DMic 4", "AD4 Enable"},
+	{"Sidetone Right Source Playback Route", "Headset Right", "DA_IN2"},
+
+	{"STFIR1 Control", NULL, "Sidetone Left Source Playback Route"},
+	{"STFIR2 Control", NULL, "Sidetone Right Source Playback Route"},
+
+	{"STFIR1 Gain", NULL, "STFIR1 Control"},
+	{"STFIR2 Gain", NULL, "STFIR2 Control"},
+
+	{"DA1 Channel Gain", NULL, "STFIR1 Gain"},
+	{"DA2 Channel Gain", NULL, "STFIR2 Gain"},
+};
+
+/* from -31 to 31 dB in 1 dB steps (mute instead of -32 dB) */
+static DECLARE_TLV_DB_SCALE(adx_dig_gain_tlv, -3200, 100, 1);
+
+/* from -62 to 0 dB in 1 dB steps (mute instead of -63 dB) */
+static DECLARE_TLV_DB_SCALE(dax_dig_gain_tlv, -6300, 100, 1);
+
+/* from 0 to 8 dB in 1 dB steps (mute instead of -1 dB) */
+static DECLARE_TLV_DB_SCALE(hs_ear_dig_gain_tlv, -100, 100, 1);
+
+/* from -30 to 0 dB in 1 dB steps (mute instead of -31 dB) */
+static DECLARE_TLV_DB_SCALE(stfir_dig_gain_tlv, -3100, 100, 1);
+
+/* from -32 to -20 dB in 4 dB steps / from -18 to 2 dB in 2 dB steps */
+static const unsigned int hs_gain_tlv[] = {
+	TLV_DB_RANGE_HEAD(2),
+	0, 3, TLV_DB_SCALE_ITEM(-3200, 400, 0),
+	4, 15, TLV_DB_SCALE_ITEM(-1800, 200, 0),
+};
+
+/* from 0 to 31 dB in 1 dB steps */
+static DECLARE_TLV_DB_SCALE(mic_gain_tlv, 0, 100, 0);
+
+/* from -10 to 20 dB in 2 dB steps */
+static DECLARE_TLV_DB_SCALE(lin_gain_tlv, -1000, 200, 0);
+
+/* from -36 to 0 dB in 2 dB steps (mute instead of -38 dB) */
+static DECLARE_TLV_DB_SCALE(lin2hs_gain_tlv, -3800, 200, 1);
+
+static const char *enum_ena_dis[] = {"Enabled", "Disabled"};
+static const char *enum_dis_ena[] = {"Disabled", "Enabled"};
+
+static SOC_ENUM_SINGLE_DECL(soc_enum_hshpen,
+	REG_ANACONF1, REG_ANACONF1_HSHPEN, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_hslowpow,
+	REG_ANACONF1, REG_ANACONF1_HSLOWPOW, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_daclowpow1,
+	REG_ANACONF1, REG_ANACONF1_DACLOWPOW1, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_daclowpow0,
+	REG_ANACONF1, REG_ANACONF1_DACLOWPOW0, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_eardaclowpow,
+	REG_ANACONF1, REG_ANACONF1_EARDACLOWPOW, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_eardrvlowpow,
+	REG_ANACONF1, REG_ANACONF1_EARDRVLOWPOW, enum_dis_ena);
+
+static const char *enum_earselcm[] = {"0.95V", "1.10V", "1.27V", "1.58V"};
+static SOC_ENUM_SINGLE_DECL(soc_enum_earselcm,
+	REG_ANACONF1, REG_ANACONF1_EARSELCM, enum_earselcm);
+
+static const char *enum_hsfadspeed[] = {"2ms", "0.5ms", "10.6ms", "5ms"};
+static SOC_ENUM_SINGLE_DECL(soc_enum_hsfadspeed,
+	REG_DIGMICCONF, REG_DIGMICCONF_HSFADSPEED, enum_hsfadspeed);
+
+static const char *enum_envdetthre[] = {
+	"250mV", "300mV", "350mV", "400mV",
+	"450mV", "500mV", "550mV", "600mV",
+	"650mV", "700mV", "750mV", "800mV",
+	"850mV", "900mV", "950mV", "1.00V" };
+static SOC_ENUM_SINGLE_DECL(soc_enum_envdetcpen,
+	REG_SIGENVCONF, REG_SIGENVCONF_ENVDETCPEN, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_envdeththre,
+	REG_ENVCPCONF, REG_ENVCPCONF_ENVDETHTHRE, enum_envdetthre);
+static SOC_ENUM_SINGLE_DECL(soc_enum_envdetlthre,
+	REG_ENVCPCONF, REG_ENVCPCONF_ENVDETLTHRE, enum_envdetthre);
+
+static const char* enum_envdettime[] = {
+	"26.6us", "53.2us", "106us",  "213us",
+	"426us",  "851us",  "1.70ms", "3.40ms",
+	"6.81ms", "13.6ms", "27.2ms", "54.5ms",
+	"109ms",  "218ms",  "436ms",  "872ms" };
+static SOC_ENUM_SINGLE_DECL(soc_enum_envdettime,
+	REG_SIGENVCONF, REG_SIGENVCONF_ENVDETTIME, enum_envdettime);
+
+static const char *enum_ensemicx[] = {"Differential", "Single Ended"};
+static SOC_ENUM_SINGLE_DECL(soc_enum_ensemic1,
+	REG_ANAGAIN1, REG_ANAGAINX_ENSEMICX, enum_ensemicx);
+static SOC_ENUM_SINGLE_DECL(soc_enum_ensemic2,
+	REG_ANAGAIN2, REG_ANAGAINX_ENSEMICX, enum_ensemicx);
+static SOC_ENUM_SINGLE_DECL(soc_enum_lowpowmic1,
+	REG_ANAGAIN1, REG_ANAGAINX_LOWPOWMICX, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_lowpowmic2,
+	REG_ANAGAIN2, REG_ANAGAINX_LOWPOWMICX, enum_dis_ena);
+
+static SOC_ENUM_DOUBLE_DECL(soc_enum_ad12nh, REG_ADFILTCONF,
+	REG_ADFILTCONF_AD1NH, REG_ADFILTCONF_AD2NH, enum_ena_dis);
+static SOC_ENUM_DOUBLE_DECL(soc_enum_ad34nh, REG_ADFILTCONF,
+	REG_ADFILTCONF_AD3NH, REG_ADFILTCONF_AD4NH, enum_ena_dis);
+
+static const char *enum_av_mode[] = {"Audio", "Voice"};
+static SOC_ENUM_DOUBLE_DECL(soc_enum_ad12voice, REG_ADFILTCONF,
+	REG_ADFILTCONF_AD1VOICE, REG_ADFILTCONF_AD2VOICE, enum_av_mode);
+static SOC_ENUM_DOUBLE_DECL(soc_enum_ad34voice, REG_ADFILTCONF,
+	REG_ADFILTCONF_AD3VOICE, REG_ADFILTCONF_AD4VOICE, enum_av_mode);
+
+static SOC_ENUM_SINGLE_DECL(soc_enum_da12voice,
+	REG_DASLOTCONF1, REG_DASLOTCONF1_DA12VOICE, enum_av_mode);
+static SOC_ENUM_SINGLE_DECL(soc_enum_da34voice,
+	REG_DASLOTCONF3, REG_DASLOTCONF3_DA34VOICE, enum_av_mode);
+static SOC_ENUM_SINGLE_DECL(soc_enum_da56voice,
+	REG_DASLOTCONF5, REG_DASLOTCONF5_DA56VOICE, enum_av_mode);
+
+static SOC_ENUM_SINGLE_DECL(soc_enum_swapda12_34,
+	REG_DASLOTCONF1, REG_DASLOTCONF1_SWAPDA12_34, enum_dis_ena);
+
+static SOC_ENUM_DOUBLE_DECL(soc_enum_vib12swap, REG_CLASSDCONF1,
+	REG_CLASSDCONF1_VIB1SWAPEN, REG_CLASSDCONF1_VIB2SWAPEN, enum_dis_ena);
+static SOC_ENUM_DOUBLE_DECL(soc_enum_hflrswap, REG_CLASSDCONF1,
+	REG_CLASSDCONF1_HFLSWAPEN, REG_CLASSDCONF1_HFRSWAPEN, enum_dis_ena);
+
+static SOC_ENUM_DOUBLE_DECL(soc_enum_fir01byp, REG_CLASSDCONF2,
+	REG_CLASSDCONF2_FIRBYP0, REG_CLASSDCONF2_FIRBYP1, enum_dis_ena);
+static SOC_ENUM_DOUBLE_DECL(soc_enum_fir23byp, REG_CLASSDCONF2,
+	REG_CLASSDCONF2_FIRBYP2, REG_CLASSDCONF2_FIRBYP3, enum_dis_ena);
+static SOC_ENUM_DOUBLE_DECL(soc_enum_highvol01, REG_CLASSDCONF2,
+	REG_CLASSDCONF2_HIGHVOLEN0, REG_CLASSDCONF2_HIGHVOLEN1, enum_dis_ena);
+static SOC_ENUM_DOUBLE_DECL(soc_enum_highvol23, REG_CLASSDCONF2,
+	REG_CLASSDCONF2_HIGHVOLEN2, REG_CLASSDCONF2_HIGHVOLEN3, enum_dis_ena);
+
+static const char *enum_sinc53[] = {"Sinc 5", "Sinc 3"};
+static SOC_ENUM_DOUBLE_DECL(soc_enum_dmic12sinc, REG_DMICFILTCONF,
+	REG_DMICFILTCONF_DMIC1SINC3, REG_DMICFILTCONF_DMIC2SINC3, enum_sinc53);
+static SOC_ENUM_DOUBLE_DECL(soc_enum_dmic34sinc, REG_DMICFILTCONF,
+	REG_DMICFILTCONF_DMIC3SINC3, REG_DMICFILTCONF_DMIC4SINC3, enum_sinc53);
+static SOC_ENUM_DOUBLE_DECL(soc_enum_dmic56sinc, REG_DMICFILTCONF,
+	REG_DMICFILTCONF_DMIC5SINC3, REG_DMICFILTCONF_DMIC6SINC3, enum_sinc53);
+
+static const char *enum_da2hslr[] = {"Sidetone", "Audio Path"};
+static SOC_ENUM_DOUBLE_DECL(soc_enum_da2hslr, REG_DIGMULTCONF1,
+	REG_DIGMULTCONF1_DATOHSLEN, REG_DIGMULTCONF1_DATOHSREN,	enum_da2hslr);
+
+static const char *enum_sinc31[] = {"Sinc 3", "Sinc 1"};
+static SOC_ENUM_SINGLE_DECL(soc_enum_hsesinc,
+		REG_HSLEARDIGGAIN, REG_HSLEARDIGGAIN_HSSINC1, enum_sinc31);
+
+static const char *enum_fadespeed[] = {"1ms", "4ms", "8ms", "16ms"};
+static SOC_ENUM_SINGLE_DECL(soc_enum_fadespeed,
+	REG_HSRDIGGAIN, REG_HSRDIGGAIN_FADESPEED, enum_fadespeed);
+
+/* Digital interface controls */
+
+/* Clocks */
+static SOC_ENUM_SINGLE_DECL(soc_enum_mastgen,
+	REG_DIGIFCONF1, REG_DIGIFCONF1_ENMASTGEN, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_fsbitclk0,
+	REG_DIGIFCONF1, REG_DIGIFCONF1_ENFSBITCLK0, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_fsbitclk1,
+	REG_DIGIFCONF1, REG_DIGIFCONF1_ENFSBITCLK1, enum_dis_ena);
+
+/* DA from slot mapping */
+static const char *enum_da_from_slot_map[] = {"SLOT0",
+					"SLOT1",
+					"SLOT2",
+					"SLOT3",
+					"SLOT4",
+					"SLOT5",
+					"SLOT6",
+					"SLOT7",
+					"SLOT8",
+					"SLOT9",
+					"SLOT10",
+					"SLOT11",
+					"SLOT12",
+					"SLOT13",
+					"SLOT14",
+					"SLOT15",
+					"SLOT16",
+					"SLOT17",
+					"SLOT18",
+					"SLOT19",
+					"SLOT20",
+					"SLOT21",
+					"SLOT22",
+					"SLOT23",
+					"SLOT24",
+					"SLOT25",
+					"SLOT26",
+					"SLOT27",
+					"SLOT28",
+					"SLOT29",
+					"SLOT30",
+					"SLOT31"};
+static SOC_ENUM_SINGLE_DECL(soc_enum_da1slotmap,
+	REG_DASLOTCONF1, REG_DASLOTCONFX_SLTODAX_SHIFT, enum_da_from_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_da2slotmap,
+	REG_DASLOTCONF2, REG_DASLOTCONFX_SLTODAX_SHIFT, enum_da_from_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_da3slotmap,
+	REG_DASLOTCONF3, REG_DASLOTCONFX_SLTODAX_SHIFT, enum_da_from_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_da4slotmap,
+	REG_DASLOTCONF4, REG_DASLOTCONFX_SLTODAX_SHIFT, enum_da_from_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_da5slotmap,
+	REG_DASLOTCONF5, REG_DASLOTCONFX_SLTODAX_SHIFT, enum_da_from_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_da6slotmap,
+	REG_DASLOTCONF6, REG_DASLOTCONFX_SLTODAX_SHIFT, enum_da_from_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_da7slotmap,
+	REG_DASLOTCONF7, REG_DASLOTCONFX_SLTODAX_SHIFT, enum_da_from_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_da8slotmap,
+	REG_DASLOTCONF8, REG_DASLOTCONFX_SLTODAX_SHIFT, enum_da_from_slot_map);
+
+/* AD to slot mapping */
+static const char *enum_ad_to_slot_map[] = {"AD_OUT1",
+					"AD_OUT2",
+					"AD_OUT3",
+					"AD_OUT4",
+					"AD_OUT5",
+					"AD_OUT6",
+					"AD_OUT7",
+					"AD_OUT8",
+					"zeroes",
+					"tristate"};
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot0map,
+	REG_ADSLOTSEL1, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot1map,
+	REG_ADSLOTSEL1, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot2map,
+	REG_ADSLOTSEL2, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot3map,
+	REG_ADSLOTSEL2, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot4map,
+	REG_ADSLOTSEL3, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot5map,
+	REG_ADSLOTSEL3, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot6map,
+	REG_ADSLOTSEL4, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot7map,
+	REG_ADSLOTSEL4, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot8map,
+	REG_ADSLOTSEL5, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot9map,
+	REG_ADSLOTSEL5, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot10map,
+	REG_ADSLOTSEL6, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot11map,
+	REG_ADSLOTSEL6, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot12map,
+	REG_ADSLOTSEL7, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot13map,
+	REG_ADSLOTSEL7, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot14map,
+	REG_ADSLOTSEL8, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot15map,
+	REG_ADSLOTSEL8, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot16map,
+	REG_ADSLOTSEL9, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot17map,
+	REG_ADSLOTSEL9, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot18map,
+	REG_ADSLOTSEL10, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot19map,
+	REG_ADSLOTSEL10, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot20map,
+	REG_ADSLOTSEL11, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot21map,
+	REG_ADSLOTSEL11, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot22map,
+	REG_ADSLOTSEL12, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot23map,
+	REG_ADSLOTSEL12, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot24map,
+	REG_ADSLOTSEL13, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot25map,
+	REG_ADSLOTSEL13, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot26map,
+	REG_ADSLOTSEL14, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot27map,
+	REG_ADSLOTSEL14, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot28map,
+	REG_ADSLOTSEL15, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot29map,
+	REG_ADSLOTSEL15, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot30map,
+	REG_ADSLOTSEL16, REG_ADSLOTSELX_EVEN_SHIFT, enum_ad_to_slot_map);
+static SOC_ENUM_SINGLE_DECL(soc_enum_adslot31map,
+	REG_ADSLOTSEL16, REG_ADSLOTSELX_ODD_SHIFT, enum_ad_to_slot_map);
+
+/* Digital loopback */
+static SOC_ENUM_SINGLE_DECL(soc_enum_ad1loop,
+	REG_DASLOTCONF1, REG_DASLOTCONF1_DAI7TOADO1, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_ad2loop,
+	REG_DASLOTCONF2, REG_DASLOTCONF2_DAI8TOADO2, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_ad3loop,
+	REG_DASLOTCONF3, REG_DASLOTCONF3_DAI7TOADO3, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_ad4loop,
+	REG_DASLOTCONF4, REG_DASLOTCONF4_DAI8TOADO4, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_ad5loop,
+	REG_DASLOTCONF5, REG_DASLOTCONF5_DAI7TOADO5, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_ad6loop,
+	REG_DASLOTCONF6, REG_DASLOTCONF6_DAI8TOADO6, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_ad7loop,
+	REG_DASLOTCONF7, REG_DASLOTCONF7_DAI8TOADO7, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_ad8loop,
+	REG_DASLOTCONF8, REG_DASLOTCONF8_DAI7TOADO8, enum_dis_ena);
+
+/* Burst mode */
+static SOC_ENUM_SINGLE_DECL(soc_enum_if0fifoen,
+	REG_DIGIFCONF3, REG_DIGIFCONF3_IF0BFIFOEN, enum_dis_ena);
+static const char *enum_mask[] = {"Unmasked", "Masked"};
+static SOC_ENUM_SINGLE_DECL(soc_enum_bfifomask,
+	REG_FIFOCONF1, REG_FIFOCONF1_BFIFOMASK, enum_mask);
+static const char *enum_bitclk0[] = {"19_2_MHz", "38_4_MHz"};
+static SOC_ENUM_SINGLE_DECL(soc_enum_bfifo19m2,
+	REG_FIFOCONF1, REG_FIFOCONF1_BFIFO19M2, enum_bitclk0);
+
+static const char *enum_slavemaster[] = {"Slave", "Master"};
+static SOC_ENUM_SINGLE_DECL(soc_enum_bfifomast,
+	REG_FIFOCONF3, REG_FIFOCONF3_BFIFOMAST_SHIFT, enum_slavemaster);
+static SOC_ENUM_SINGLE_DECL(soc_enum_bfifoint,
+	REG_FIFOCONF3, REG_FIFOCONF3_BFIFORUN_SHIFT, enum_dis_ena);
+
+/* Sidetone */
+
+static int st_fir_value_control_info(struct snd_kcontrol *kcontrol,
+			    struct snd_ctl_elem_info *uinfo)
+{
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
+	uinfo->count = 1;
+	uinfo->value.integer.min = 0;
+	uinfo->value.integer.max = REG_MASK_ALL;
+
+	return 0;
+}
+
+static int st_fir_value_control_get(struct snd_kcontrol *kcontrol,
+			   struct snd_ctl_elem_value *ucontrol)
+{
+	return 0;
+}
+
+static int st_fir_value_control_put(struct snd_kcontrol *kcontrol,
+			   struct snd_ctl_elem_value *ucontrol)
+{
+	int ret;
+	unsigned int val_msb = (int)ucontrol->value.integer.value[0] / 256;
+	unsigned int val_lsb = (int)ucontrol->value.integer.value[0] - val_msb * 256;
+	ret = ab8500_codec_write_reg_audio(ab8500_codec, REG_SIDFIRCOEF1, val_msb);
+	ret |= ab8500_codec_write_reg_audio(ab8500_codec, REG_SIDFIRCOEF2, val_lsb);
+	if (ret < 0) {
+		pr_err("%s: ERROR: Failed to write FIR-coeffecient!\n", __func__);
+		return 0;
+	}
+	return 1;
+}
+
+static const struct snd_kcontrol_new st_fir_value_control = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.name = "Sidetone FIR Coeffecient Value",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.info = st_fir_value_control_info,
+	.get = st_fir_value_control_get,
+	.put = st_fir_value_control_put,
+	.private_value = 1 /* ULPCLK */
+};
+
+static int st_fir_apply_control_info(struct snd_kcontrol *kcontrol,
+			    struct snd_ctl_elem_info *uinfo)
+{
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->count = 1;
+	uinfo->value.enumerated.items = 2;
+	if (uinfo->value.enumerated.item) {
+		uinfo->value.enumerated.item = 1;
+		strcpy(uinfo->value.enumerated.name, "Apply");
+	} else {
+		strcpy(uinfo->value.enumerated.name, "Ready");
+	}
+	return 0;
+}
+
+static int st_fir_apply_control_get(struct snd_kcontrol *kcontrol,
+			   struct snd_ctl_elem_value *ucontrol)
+{
+	int reg = ab8500_codec_read_reg_audio(ab8500_codec, REG_SIDFIRADR);
+	ucontrol->value.enumerated.item[0] = reg & BMASK(REG_SIDFIRADR_FIRSIDSET);
+
+	return 0;
+}
+
+static int st_fir_apply_control_put(struct snd_kcontrol *kcontrol,
+			   struct snd_ctl_elem_value *ucontrol)
+{
+	int ret;
+
+	if (ucontrol->value.enumerated.item[0] != 0) {
+		ret = ab8500_codec_write_reg_audio(ab8500_codec,
+						REG_SIDFIRADR,
+						BMASK(REG_SIDFIRADR_FIRSIDSET));
+		if (ret < 0) {
+			pr_err("%s: ERROR: Failed to apply FIR-coeffecients!\n", __func__);
+			return 0;
+		}
+		pr_debug("%s: FIR-coeffecients applied.\n", __func__);
+	}
+
+	ret = ab8500_codec_write_reg_audio(ab8500_codec, REG_SIDFIRADR, 0);
+	if (ret < 0)
+		pr_err("%s: ERROR: Going to ready failed!\n", __func__);
+
+	return 1;
+}
+
+static const struct snd_kcontrol_new st_fir_apply_control = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.name = "Sidetone FIR Apply Coeffecients",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.info = st_fir_apply_control_info,
+	.get = st_fir_apply_control_get,
+	.put = st_fir_apply_control_put,
+	.private_value = 0 /* Ready */
+};
+
+static const char *enum_coeffctrl[] = {"Ready", "Apply"};
+static SOC_ENUM_SINGLE_DECL(soc_enum_coeffctrl,
+	REG_SIDFIRADR, REG_SIDFIRADR_FIRSIDSET, enum_coeffctrl);
+
+/* TODO: move to DAPM */
+static SOC_ENUM_SINGLE_DECL(soc_enum_enfirsids,
+	REG_SIDFIRCONF, REG_SIDFIRCONF_ENFIRSIDS, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_parlhf,
+	REG_CLASSDCONF1, REG_CLASSDCONF1_PARLHF, enum_dis_ena);
+static SOC_ENUM_SINGLE_DECL(soc_enum_parlvib,
+	REG_CLASSDCONF1, REG_CLASSDCONF1_PARLVIB, enum_dis_ena);
+
+static struct snd_kcontrol_new ab8500_snd_controls[] = {
+	SOC_ENUM("Headset High Pass Playback Switch", soc_enum_hshpen),
+	SOC_ENUM("Headset Low Power Playback Switch", soc_enum_hslowpow),
+	SOC_ENUM("Headset DAC Low Power Playback Switch", soc_enum_daclowpow1),
+	SOC_ENUM("Headset DAC Drv Low Power Playback Switch",
+		soc_enum_daclowpow0),
+	SOC_ENUM("Earpiece DAC Low Power Playback Switch",
+		soc_enum_eardaclowpow),
+	SOC_ENUM("Earpiece DAC Drv Low Power Playback Switch",
+		soc_enum_eardrvlowpow),
+	SOC_ENUM("Earpiece Common Mode Playback Switch", soc_enum_earselcm),
+
+	SOC_ENUM("Headset Fade Speed Playback Switch", soc_enum_hsfadspeed),
+
+	SOC_ENUM("Charge Pump High Threshold For Low Voltage",
+		soc_enum_envdeththre),
+	SOC_ENUM("Charge Pump Low Threshold For Low Voltage",
+		soc_enum_envdetlthre),
+	SOC_ENUM("Charge Pump Envelope Detection", soc_enum_envdetcpen),
+	SOC_ENUM("Charge Pump Envelope Detection Decay Time",
+		soc_enum_envdettime),
+
+	SOC_ENUM("Mic 1 Type Capture Switch", soc_enum_ensemic1),
+	SOC_ENUM("Mic 2 Type Capture Switch", soc_enum_ensemic2),
+	SOC_ENUM("Mic 1 Low Power Capture Switch", soc_enum_lowpowmic1),
+	SOC_ENUM("Mic 2 Low Power Capture Switch", soc_enum_lowpowmic2),
+
+	SOC_ENUM("LineIn High Pass Capture Switch", soc_enum_ad12nh),
+	SOC_ENUM("Mic High Pass Capture Switch", soc_enum_ad34nh),
+	SOC_ENUM("LineIn Mode Capture Switch", soc_enum_ad12voice),
+	SOC_ENUM("Mic Mode Capture Switch", soc_enum_ad34voice),
+
+	SOC_ENUM("Headset Mode Playback Switch", soc_enum_da12voice),
+	SOC_ENUM("IHF Mode Playback Switch", soc_enum_da34voice),
+	SOC_ENUM("Vibra Mode Playback Switch", soc_enum_da56voice),
+
+	SOC_ENUM("IHF and Headset Swap Playback Switch", soc_enum_swapda12_34),
+
+	SOC_ENUM("IHF Low EMI Mode Playback Switch", soc_enum_hflrswap),
+	SOC_ENUM("Vibra Low EMI Mode Playback Switch", soc_enum_vib12swap),
+
+	SOC_ENUM("IHF FIR Bypass Playback Switch", soc_enum_fir01byp),
+	SOC_ENUM("Vibra FIR Bypass Playback Switch", soc_enum_fir23byp),
+
+	/* TODO: Cannot be changed on the fly with digital channel enabled. */
+	SOC_ENUM("IHF High Volume Playback Switch", soc_enum_highvol01),
+	SOC_ENUM("Vibra High Volume Playback Switch", soc_enum_highvol23),
+
+	SOC_SINGLE("ClassD High Pass Gain Playback Volume",
+		REG_CLASSDCONF3, REG_CLASSDCONF3_DITHHPGAIN,
+		REG_CLASSDCONF3_DITHHPGAIN_MAX, NORMAL),
+	SOC_SINGLE("ClassD White Gain Playback Volume",
+		REG_CLASSDCONF3, REG_CLASSDCONF3_DITHWGAIN,
+		REG_CLASSDCONF3_DITHWGAIN_MAX, NORMAL),
+
+	SOC_ENUM("LineIn Filter Capture Switch", soc_enum_dmic12sinc),
+	SOC_ENUM("Mic Filter Capture Switch", soc_enum_dmic34sinc),
+	SOC_ENUM("HD Mic Filter Capture Switch", soc_enum_dmic56sinc),
+
+	SOC_ENUM("Headset Source Playback Route", soc_enum_da2hslr),
+
+	/* TODO: Cannot be changed on the fly with digital channel enabled. */
+	SOC_ENUM("Headset Filter Playback Switch", soc_enum_hsesinc),
+
+	SOC_ENUM("Digital Gain Fade Speed Switch", soc_enum_fadespeed),
+
+	SOC_DOUBLE_R("Vibra PWM Duty Cycle N Playback Volume",
+		REG_PWMGENCONF3, REG_PWMGENCONF5,
+		REG_PWMGENCONFX_PWMVIBXDUTCYC,
+		REG_PWMGENCONFX_PWMVIBXDUTCYC_MAX, NORMAL),
+	SOC_DOUBLE_R("Vibra PWM Duty Cycle P Playback Volume",
+		REG_PWMGENCONF2, REG_PWMGENCONF4,
+		REG_PWMGENCONFX_PWMVIBXDUTCYC,
+		REG_PWMGENCONFX_PWMVIBXDUTCYC_MAX, NORMAL),
+
+	/* TODO: move to DAPM */
+	SOC_ENUM("Sidetone Playback Switch", soc_enum_enfirsids),
+	SOC_ENUM("IHF L and R Bridge Playback Route", soc_enum_parlhf),
+	SOC_ENUM("Vibra 1 and 2 Bridge Playback Route", soc_enum_parlvib),
+
+	/* Digital gains for AD side */
+
+	SOC_DOUBLE_R_TLV("LineIn Master Gain Capture Volume",
+		REG_ADDIGGAIN1, REG_ADDIGGAIN2,
+		0, REG_ADDIGGAINX_ADXGAIN_MAX, INVERT, adx_dig_gain_tlv),
+	SOC_DOUBLE_R_TLV("Mic Master Gain Capture Volume",
+		REG_ADDIGGAIN3, REG_ADDIGGAIN4,
+		0, REG_ADDIGGAINX_ADXGAIN_MAX, INVERT, adx_dig_gain_tlv),
+	SOC_DOUBLE_R_TLV("HD Mic Master Gain Capture Volume",
+		REG_ADDIGGAIN5, REG_ADDIGGAIN6,
+		0, REG_ADDIGGAINX_ADXGAIN_MAX, INVERT, adx_dig_gain_tlv),
+
+	/* Digital gains for DA side */
+
+	SOC_DOUBLE_R_TLV("Headset Master Gain Playback Volume",
+		REG_DADIGGAIN1, REG_DADIGGAIN2,
+		0, REG_DADIGGAINX_DAXGAIN_MAX, INVERT, dax_dig_gain_tlv),
+	SOC_DOUBLE_R_TLV("IHF Master Gain Playback Volume",
+		REG_DADIGGAIN3, REG_DADIGGAIN4,
+		0, REG_DADIGGAINX_DAXGAIN_MAX, INVERT, dax_dig_gain_tlv),
+	SOC_DOUBLE_R_TLV("Vibra Master Gain Playback Volume",
+		REG_DADIGGAIN5, REG_DADIGGAIN6,
+		0, REG_DADIGGAINX_DAXGAIN_MAX, INVERT, dax_dig_gain_tlv),
+	SOC_DOUBLE_R_TLV("Analog Loopback Gain Playback Volume",
+		REG_ADDIGLOOPGAIN1, REG_ADDIGLOOPGAIN2,
+		0, REG_ADDIGLOOPGAINX_ADXLBGAIN_MAX, INVERT, dax_dig_gain_tlv),
+	SOC_DOUBLE_R_TLV("Headset Digital Gain Playback Volume",
+		REG_HSLEARDIGGAIN, REG_HSRDIGGAIN,
+		0, REG_HSLEARDIGGAIN_HSLDGAIN_MAX, INVERT, hs_ear_dig_gain_tlv),
+	SOC_DOUBLE_R_TLV("Sidetone Digital Gain Playback Volume",
+		REG_SIDFIRGAIN1, REG_SIDFIRGAIN2,
+		0, REG_SIDFIRGAINX_FIRSIDXGAIN_MAX, INVERT, stfir_dig_gain_tlv),
+
+	/* Analog gains */
+
+	SOC_DOUBLE_TLV("Headset Gain Playback Volume",
+		REG_ANAGAIN3,
+		REG_ANAGAIN3_HSLGAIN, REG_ANAGAIN3_HSRGAIN,
+		REG_ANAGAIN3_HSXGAIN_MAX, INVERT, hs_gain_tlv),
+	SOC_SINGLE_TLV("Mic 1 Capture Volume",
+		REG_ANAGAIN1,
+		REG_ANAGAINX_MICXGAIN,
+		REG_ANAGAINX_MICXGAIN_MAX, NORMAL, mic_gain_tlv),
+	SOC_SINGLE_TLV("Mic 2 Capture Volume",
+		REG_ANAGAIN2,
+		REG_ANAGAINX_MICXGAIN,
+		REG_ANAGAINX_MICXGAIN_MAX, NORMAL, mic_gain_tlv),
+	SOC_DOUBLE_TLV("LineIn Capture Volume",
+		REG_ANAGAIN4,
+		REG_ANAGAIN4_LINLGAIN, REG_ANAGAIN4_LINRGAIN,
+		REG_ANAGAIN4_LINXGAIN_MAX, NORMAL, lin_gain_tlv),
+	SOC_DOUBLE_R_TLV("LineIn to Headset Bypass Playback Volume",
+		REG_DIGLINHSLGAIN, REG_DIGLINHSRGAIN,
+		REG_DIGLINHSXGAIN_LINTOHSXGAIN,
+		REG_DIGLINHSXGAIN_LINTOHSXGAIN_MAX, INVERT, lin2hs_gain_tlv),
+
+	/* Digital Interface controls */
+
+	/* Clocks */
+	SOC_ENUM("Digital Interface Master Generator Switch", soc_enum_mastgen),
+	SOC_ENUM("Digital Interface 0 Bit-clock Switch", soc_enum_fsbitclk0),
+	SOC_ENUM("Digital Interface 1 Bit-clock Switch", soc_enum_fsbitclk1),
+
+	/* DA from slot mapping */
+	SOC_ENUM("Digital Interface DA 1 From Slot Map", soc_enum_da1slotmap),
+	SOC_ENUM("Digital Interface DA 2 From Slot Map", soc_enum_da2slotmap),
+	SOC_ENUM("Digital Interface DA 3 From Slot Map", soc_enum_da3slotmap),
+	SOC_ENUM("Digital Interface DA 4 From Slot Map", soc_enum_da4slotmap),
+	SOC_ENUM("Digital Interface DA 5 From Slot Map", soc_enum_da5slotmap),
+	SOC_ENUM("Digital Interface DA 6 From Slot Map", soc_enum_da6slotmap),
+	SOC_ENUM("Digital Interface DA 7 From Slot Map", soc_enum_da7slotmap),
+	SOC_ENUM("Digital Interface DA 8 From Slot Map", soc_enum_da8slotmap),
+
+	/* AD to slot mapping */
+	SOC_ENUM("Digital Interface AD To Slot 0 Map", soc_enum_adslot0map),
+	SOC_ENUM("Digital Interface AD To Slot 1 Map", soc_enum_adslot1map),
+	SOC_ENUM("Digital Interface AD To Slot 2 Map", soc_enum_adslot2map),
+	SOC_ENUM("Digital Interface AD To Slot 3 Map", soc_enum_adslot3map),
+	SOC_ENUM("Digital Interface AD To Slot 4 Map", soc_enum_adslot4map),
+	SOC_ENUM("Digital Interface AD To Slot 5 Map", soc_enum_adslot5map),
+	SOC_ENUM("Digital Interface AD To Slot 6 Map", soc_enum_adslot6map),
+	SOC_ENUM("Digital Interface AD To Slot 7 Map", soc_enum_adslot7map),
+	SOC_ENUM("Digital Interface AD To Slot 8 Map", soc_enum_adslot8map),
+	SOC_ENUM("Digital Interface AD To Slot 9 Map", soc_enum_adslot9map),
+	SOC_ENUM("Digital Interface AD To Slot 10 Map", soc_enum_adslot10map),
+	SOC_ENUM("Digital Interface AD To Slot 11 Map", soc_enum_adslot11map),
+	SOC_ENUM("Digital Interface AD To Slot 12 Map", soc_enum_adslot12map),
+	SOC_ENUM("Digital Interface AD To Slot 13 Map", soc_enum_adslot13map),
+	SOC_ENUM("Digital Interface AD To Slot 14 Map", soc_enum_adslot14map),
+	SOC_ENUM("Digital Interface AD To Slot 15 Map", soc_enum_adslot15map),
+	SOC_ENUM("Digital Interface AD To Slot 16 Map", soc_enum_adslot16map),
+	SOC_ENUM("Digital Interface AD To Slot 17 Map", soc_enum_adslot17map),
+	SOC_ENUM("Digital Interface AD To Slot 18 Map", soc_enum_adslot18map),
+	SOC_ENUM("Digital Interface AD To Slot 19 Map", soc_enum_adslot19map),
+	SOC_ENUM("Digital Interface AD To Slot 20 Map", soc_enum_adslot20map),
+	SOC_ENUM("Digital Interface AD To Slot 21 Map", soc_enum_adslot21map),
+	SOC_ENUM("Digital Interface AD To Slot 22 Map", soc_enum_adslot22map),
+	SOC_ENUM("Digital Interface AD To Slot 23 Map", soc_enum_adslot23map),
+	SOC_ENUM("Digital Interface AD To Slot 24 Map", soc_enum_adslot24map),
+	SOC_ENUM("Digital Interface AD To Slot 25 Map", soc_enum_adslot25map),
+	SOC_ENUM("Digital Interface AD To Slot 26 Map", soc_enum_adslot26map),
+	SOC_ENUM("Digital Interface AD To Slot 27 Map", soc_enum_adslot27map),
+	SOC_ENUM("Digital Interface AD To Slot 28 Map", soc_enum_adslot28map),
+	SOC_ENUM("Digital Interface AD To Slot 29 Map", soc_enum_adslot29map),
+	SOC_ENUM("Digital Interface AD To Slot 30 Map", soc_enum_adslot30map),
+	SOC_ENUM("Digital Interface AD To Slot 31 Map", soc_enum_adslot31map),
+
+	/* Loopback */
+	SOC_ENUM("Digital Interface AD 1 Loopback Switch", soc_enum_ad1loop),
+	SOC_ENUM("Digital Interface AD 2 Loopback Switch", soc_enum_ad2loop),
+	SOC_ENUM("Digital Interface AD 3 Loopback Switch", soc_enum_ad3loop),
+	SOC_ENUM("Digital Interface AD 4 Loopback Switch", soc_enum_ad4loop),
+	SOC_ENUM("Digital Interface AD 5 Loopback Switch", soc_enum_ad5loop),
+	SOC_ENUM("Digital Interface AD 6 Loopback Switch", soc_enum_ad6loop),
+	SOC_ENUM("Digital Interface AD 7 Loopback Switch", soc_enum_ad7loop),
+	SOC_ENUM("Digital Interface AD 8 Loopback Switch", soc_enum_ad8loop),
+
+	/* Burst FIFO */
+	SOC_ENUM("Digital Interface 0 FIFO Enable Switch", soc_enum_if0fifoen),
+	SOC_ENUM("Burst FIFO Mask", soc_enum_bfifomask),
+	SOC_ENUM("Burst FIFO Bit-clock Frequency", soc_enum_bfifo19m2),
+	SOC_SINGLE("Burst FIFO Threshold",
+		REG_FIFOCONF1,
+		REG_FIFOCONF1_BFIFOINT_SHIFT,
+		REG_FIFOCONF1_BFIFOINT_MAX,
+		NORMAL),
+	SOC_SINGLE("Burst FIFO Length",
+		REG_FIFOCONF2,
+		REG_FIFOCONF2_BFIFOTX_SHIFT,
+		REG_FIFOCONF2_BFIFOTX_MAX,
+		NORMAL),
+	SOC_SINGLE("Burst FIFO EOS Extra Slots",
+		REG_FIFOCONF3,
+		REG_FIFOCONF3_BFIFOEXSL_SHIFT,
+		REG_FIFOCONF3_BFIFOEXSL_MAX,
+		NORMAL),
+	SOC_SINGLE("Burst FIFO FS Extra Bit-clocks",
+		REG_FIFOCONF3,
+		REG_FIFOCONF3_PREBITCLK0_SHIFT,
+		REG_FIFOCONF3_PREBITCLK0_MAX,
+		NORMAL),
+	SOC_ENUM("Burst FIFO Interface Mode", soc_enum_bfifomast),
+	SOC_ENUM("Burst FIFO Interface Switch", soc_enum_bfifoint),
+	SOC_SINGLE("Burst FIFO Switch Frame Number",
+		REG_FIFOCONF4,
+		REG_FIFOCONF4_BFIFOFRAMSW_SHIFT,
+		REG_FIFOCONF4_BFIFOFRAMSW_MAX,
+		NORMAL),
+	SOC_SINGLE("Burst FIFO Wake Up Delay",
+		REG_FIFOCONF5,
+		REG_FIFOCONF5_BFIFOWAKEUP_SHIFT,
+		REG_FIFOCONF5_BFIFOWAKEUP_MAX,
+		NORMAL),
+	SOC_SINGLE("Burst FIFO Samples In FIFO",
+		REG_FIFOCONF6,
+		REG_FIFOCONF6_BFIFOSAMPLE_SHIFT,
+		REG_FIFOCONF6_BFIFOSAMPLE_MAX,
+		NORMAL),
+
+	/* Sidetone */
+	SOC_SINGLE("Sidetone FIR Coeffecient Index",
+		REG_SIDFIRADR,
+		REG_SIDFIRADR_ADDRESS_SHIFT,
+		REG_SIDFIRADR_ADDRESS_MAX,
+		NORMAL),
+};
+
+static int ab8500_codec_set_format_if1(struct snd_soc_codec *codec, unsigned int fmt)
+{
+	unsigned int clear_mask, set_mask;
+
+	/* Master or slave */
+
+	clear_mask = BMASK(REG_DIGIFCONF3_IF1MASTER);
+	set_mask = 0;
+
+	switch (fmt & SND_SOC_DAIFMT_MASTER_MASK) {
+	case SND_SOC_DAIFMT_CBM_CFM: /* codec clk & FRM master */
+		pr_debug("%s: IF1 Master-mode: AB8500 master.\n", __func__);
+		set_mask |= BMASK(REG_DIGIFCONF3_IF1MASTER);
+		break;
+	case SND_SOC_DAIFMT_CBS_CFS: /* codec clk & FRM slave */
+		pr_debug("%s: IF1 Master-mode: AB8500 slave.\n", __func__);
+		break;
+	case SND_SOC_DAIFMT_CBS_CFM: /* codec clk slave & FRM master */
+	case SND_SOC_DAIFMT_CBM_CFS: /* codec clk master & frame slave */
+		pr_err("%s: ERROR: The device is either a master or a slave.\n",
+			__func__);
+	default:
+		pr_err("%s: ERROR: Unsupporter master mask 0x%x\n",
+			__func__,
+			fmt & SND_SOC_DAIFMT_MASTER_MASK);
+		return -EINVAL;
+	}
+
+	ab8500_codec_update_reg_audio(codec,
+				REG_DIGIFCONF3,
+				BMASK(REG_DIGIFCONF3_IF1MASTER),
+				BMASK(REG_DIGIFCONF3_IF1MASTER));
+
+	/* I2S or TDM */
+
+	clear_mask = BMASK(REG_DIGIFCONF4_FSYNC1P) |
+			BMASK(REG_DIGIFCONF4_BITCLK1P) |
+			BMASK(REG_DIGIFCONF4_IF1FORMAT1) |
+			BMASK(REG_DIGIFCONF4_IF1FORMAT0);
+	set_mask = 0;
+
+	switch (fmt & SND_SOC_DAIFMT_FORMAT_MASK) {
+	case SND_SOC_DAIFMT_I2S: /* I2S mode */
+		pr_debug("%s: IF1 Protocol: I2S\n", __func__);
+		set_mask |= BMASK(REG_DIGIFCONF4_IF1FORMAT1);
+		break;
+	case SND_SOC_DAIFMT_DSP_B: /* L data MSB during FRM LRC */
+		pr_debug("%s: IF1 Protocol: DSP B (TDM)\n", __func__);
+		set_mask |= BMASK(REG_DIGIFCONF4_IF1FORMAT0);
+		break;
+	default:
+		pr_err("%s: ERROR: Unsupported format (0x%x)!\n",
+			__func__,
+			fmt & SND_SOC_DAIFMT_FORMAT_MASK);
+		return -EINVAL;
+	}
+
+	ab8500_codec_update_reg_audio(codec, REG_DIGIFCONF4, clear_mask, set_mask);
+
+	return 0;
+}
+
+static int ab8500_codec_set_word_length_if1(struct snd_soc_codec *codec, unsigned int wl)
+{
+	unsigned int clear_mask, set_mask;
+
+	clear_mask = BMASK(REG_DIGIFCONF4_IF1WL1) | BMASK(REG_DIGIFCONF4_IF1WL0);
+	set_mask = 0;
+
+	switch (wl) {
+	case 16:
+		break;
+	case 20:
+		set_mask |= BMASK(REG_DIGIFCONF4_IF1WL0);
+		break;
+	case 24:
+		set_mask |= BMASK(REG_DIGIFCONF4_IF1WL1);
+		break;
+	case 32:
+		set_mask |= BMASK(REG_DIGIFCONF2_IF0WL1) |
+			BMASK(REG_DIGIFCONF2_IF0WL0);
+		break;
+	default:
+		pr_err("%s: Unsupporter word-length 0x%x\n", __func__, wl);
+		return -EINVAL;
+	}
+
+	pr_debug("%s: Word-length: %d bits.\n", __func__, wl);
+	ab8500_codec_update_reg_audio(codec, REG_DIGIFCONF4, clear_mask, set_mask);
+
+	return 0;
+}
+
+static int ab8500_codec_set_bit_delay_if1(struct snd_soc_codec *codec, unsigned int delay)
+{
+	unsigned int clear_mask, set_mask;
+
+	clear_mask = BMASK(REG_DIGIFCONF4_IF1DEL);
+	set_mask = 0;
+
+	switch (delay) {
+	case 0:
+		break;
+	case 1:
+		set_mask |= BMASK(REG_DIGIFCONF4_IF1DEL);
+		break;
+	default:
+		pr_err("%s: ERROR: Unsupported bit-delay (0x%x)!\n", __func__, delay);
+		return -EINVAL;
+	}
+
+	pr_debug("%s: IF1 Bit-delay: %d bits.\n", __func__, delay);
+	ab8500_codec_update_reg_audio(codec, REG_DIGIFCONF4, clear_mask, set_mask);
+
+	return 0;
+}
+
+/* Configures audio macrocell into the AB8500 Chip */
+static void ab8500_codec_configure_audio_macrocell(struct snd_soc_codec *codec)
+{
+	int data, ret;
+
+	ret = ab8500_sysctrl_write(AB8500_STW4500CTRL3,
+		AB8500_STW4500CTRL3_CLK32KOUT2DIS | AB8500_STW4500CTRL3_RESETAUDN,
+		AB8500_STW4500CTRL3_RESETAUDN);
+	if (ret < 0)
+		pr_err("%s: WARN: Unable to set reg STW4500CTRL3!\n", __func__);
+
+	data = ab8500_codec_read_reg(codec, AB8500_MISC, AB8500_GPIO_DIR4_REG);
+	data |= GPIO27_DIR_OUTPUT | GPIO29_DIR_OUTPUT | GPIO31_DIR_OUTPUT;
+	ab8500_codec_write_reg(codec, AB8500_MISC, AB8500_GPIO_DIR4_REG, data);
+}
+
+/* Extended interface for codec-driver */
+
+void ab8500_audio_power_control(bool power_on)
+{
+	if (ab8500_codec == NULL) {
+		pr_err("%s: ERROR: AB8500 ASoC-driver not yet probed!\n", __func__);
+		return;
+	}
+
+	if (power_on) {
+		unsigned int set_mask;
+		pr_debug("Enabling AB8500.");
+		set_mask = BMASK(REG_POWERUP_POWERUP) | BMASK(REG_POWERUP_ENANA);
+		ab8500_codec_update_reg_audio(ab8500_codec, REG_POWERUP, 0x00, set_mask);
+	} else {
+		unsigned int clear_mask;
+		pr_debug("Disabling AB8500.");
+		clear_mask = BMASK(REG_POWERUP_POWERUP) | BMASK(REG_POWERUP_ENANA);
+		ab8500_codec_update_reg_audio(ab8500_codec, REG_POWERUP, clear_mask, 0x00);
+	}
+}
+
+void ab8500_audio_pwm_vibra(unsigned char speed_left_pos,
+			unsigned char speed_left_neg,
+			unsigned char speed_right_pos,
+			unsigned char speed_right_neg)
+{
+	unsigned int clear_mask, set_mask;
+	bool vibra_on;
+
+	if (ab8500_codec == NULL) {
+		pr_err("%s: ERROR: AB8500 ASoC-driver not yet probed!\n", __func__);
+		return;
+	}
+
+	vibra_on = speed_left_pos | speed_left_neg | speed_right_pos | speed_right_neg;
+	if (!vibra_on) {
+		clear_mask = BMASK(REG_ANACONF4_ENVIB1) | BMASK(REG_ANACONF4_ENVIB2);
+		ab8500_codec_update_reg_audio(ab8500_codec, REG_ANACONF4, clear_mask, 0x00);
+		speed_left_pos = 0;
+		speed_left_neg = 0;
+		speed_right_pos = 0;
+		speed_right_neg = 0;
+	}
+
+	pr_debug("%s: PWM-vibra (%d, %d, %d, %d).\n",
+			__func__,
+			speed_left_pos,
+			speed_left_neg,
+			speed_right_pos,
+			speed_right_neg);
+
+	set_mask = BMASK(REG_PWMGENCONF1_PWMTOVIB1) |
+		BMASK(REG_PWMGENCONF1_PWMTOVIB2) |
+		BMASK(REG_PWMGENCONF1_PWM1CTRL) |
+		BMASK(REG_PWMGENCONF1_PWM2CTRL) |
+		BMASK(REG_PWMGENCONF1_PWM1NCTRL) |
+		BMASK(REG_PWMGENCONF1_PWM1PCTRL) |
+		BMASK(REG_PWMGENCONF1_PWM2NCTRL) |
+		BMASK(REG_PWMGENCONF1_PWM2PCTRL);
+	ab8500_codec_update_reg_audio(ab8500_codec, REG_PWMGENCONF1, 0x00, set_mask);
+
+	if (speed_left_pos > REG_PWMGENCONFX_PWMVIBXDUTCYC_MAX)
+		speed_left_pos = REG_PWMGENCONFX_PWMVIBXDUTCYC_MAX;
+	ab8500_codec_update_reg_audio(ab8500_codec, REG_PWMGENCONF3, REG_MASK_ALL, speed_left_pos);
+
+	if (speed_left_neg > REG_PWMGENCONFX_PWMVIBXDUTCYC_MAX)
+		speed_left_neg = REG_PWMGENCONFX_PWMVIBXDUTCYC_MAX;
+	ab8500_codec_update_reg_audio(ab8500_codec, REG_PWMGENCONF2, REG_MASK_ALL, speed_left_neg);
+
+	if (speed_right_pos > REG_PWMGENCONFX_PWMVIBXDUTCYC_MAX)
+		speed_right_pos = REG_PWMGENCONFX_PWMVIBXDUTCYC_MAX;
+	ab8500_codec_update_reg_audio(ab8500_codec, REG_PWMGENCONF5, REG_MASK_ALL, speed_right_pos);
+
+	if (speed_right_neg > REG_PWMGENCONFX_PWMVIBXDUTCYC_MAX)
+		speed_right_neg = REG_PWMGENCONFX_PWMVIBXDUTCYC_MAX;
+	ab8500_codec_update_reg_audio(ab8500_codec, REG_PWMGENCONF4, REG_MASK_ALL, speed_right_neg);
+
+	set_mask = BMASK(REG_ANACONF4_ENVIB1) | BMASK(REG_ANACONF4_ENVIB2);
+	ab8500_codec_update_reg_audio(ab8500_codec, REG_ANACONF4, 0x00, set_mask);
+}
+
+int ab8500_audio_set_word_length(struct snd_soc_dai *dai, unsigned int wl)
+{
+	unsigned int clear_mask, set_mask;
+	struct snd_soc_codec *codec = dai->codec;
+
+	clear_mask = BMASK(REG_DIGIFCONF2_IF0WL0) | BMASK(REG_DIGIFCONF2_IF0WL1);
+	set_mask = 0;
+
+	switch (wl) {
+	case 16:
+		break;
+	case 20:
+		set_mask |= BMASK(REG_DIGIFCONF2_IF0WL0);
+		break;
+	case 24:
+		set_mask |= BMASK(REG_DIGIFCONF2_IF0WL1);
+		break;
+	case 32:
+		set_mask |= BMASK(REG_DIGIFCONF2_IF0WL1) |
+			BMASK(REG_DIGIFCONF2_IF0WL0);
+		break;
+	default:
+		pr_err("%s: Unsupporter word-length 0x%x\n", __func__, wl);
+		return -EINVAL;
+	}
+
+	pr_debug("%s: IF0 Word-length: %d bits.\n", __func__, wl);
+	ab8500_codec_update_reg_audio(codec, REG_DIGIFCONF2, clear_mask, set_mask);
+
+	return 0;
+}
+
+int ab8500_audio_set_bit_delay(struct snd_soc_dai *dai, unsigned int delay)
+{
+	unsigned int clear_mask, set_mask;
+	struct snd_soc_codec *codec = dai->codec;
+
+	clear_mask = BMASK(REG_DIGIFCONF2_IF0DEL);
+	set_mask = 0;
+
+	switch (delay) {
+	case 0:
+		break;
+	case 1:
+		set_mask |= BMASK(REG_DIGIFCONF2_IF0DEL);
+		break;
+	default:
+		pr_err("%s: ERROR: Unsupported bit-delay (0x%x)!\n", __func__, delay);
+		return -EINVAL;
+	}
+
+	pr_debug("%s: IF0 Bit-delay: %d bits.\n", __func__, delay);
+	ab8500_codec_update_reg_audio(codec, REG_DIGIFCONF2, clear_mask, set_mask);
+
+	return 0;
+}
+
+int ab8500_audio_setup_if1(struct snd_soc_codec *codec,
+			unsigned int fmt,
+			unsigned int wl,
+			unsigned int delay)
+{
+	int ret;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	ret = ab8500_codec_set_format_if1(codec, fmt);
+	if (ret)
+		return -1;
+
+	ret = ab8500_codec_set_bit_delay_if1(codec, delay);
+	if (ret)
+		return -1;
+
+
+	ret = ab8500_codec_set_word_length_if1(codec, wl);
+	if (ret)
+		return -1;
+
+	return 0;
+}
+
+bool ab8500_audio_dapm_path_active(enum ab8500_audio_dapm_path dapm_path)
+{
+	int reg, reg_mask;
+
+	switch (dapm_path) {
+	case AB8500_AUDIO_DAPM_PATH_DMIC:
+		reg = ab8500_codec_read_reg_audio(ab8500_codec, REG_DIGMICCONF);
+		reg_mask = BMASK(REG_DIGMICCONF_ENDMIC1) |
+			BMASK(REG_DIGMICCONF_ENDMIC2) |
+			BMASK(REG_DIGMICCONF_ENDMIC3) |
+			BMASK(REG_DIGMICCONF_ENDMIC4) |
+			BMASK(REG_DIGMICCONF_ENDMIC5) |
+			BMASK(REG_DIGMICCONF_ENDMIC6);
+		return reg & reg_mask;
+
+	case AB8500_AUDIO_DAPM_PATH_AMIC1:
+		reg = ab8500_codec_read_reg_audio(ab8500_codec, REG_ANACONF2);
+		reg_mask = BMASK(REG_ANACONF2_MUTMIC1);
+		return !(reg & reg_mask);
+
+	case AB8500_AUDIO_DAPM_PATH_AMIC2:
+		reg = ab8500_codec_read_reg_audio(ab8500_codec, REG_ANACONF2);
+		reg_mask = BMASK(REG_ANACONF2_MUTMIC2);
+		return !(reg & reg_mask);
+
+	default:
+		return false;
+	}
+}
+
+static int ab8500_codec_add_widgets(struct snd_soc_codec *codec)
+{
+	int ret;
+
+	ret = snd_soc_dapm_new_controls(&codec->dapm, ab8500_dapm_widgets,
+			ARRAY_SIZE(ab8500_dapm_widgets));
+	if (ret < 0) {
+		pr_err("%s: Failed to create DAPM controls (%d).\n",
+			__func__, ret);
+		return ret;
+	}
+
+	ret = snd_soc_dapm_add_routes(&codec->dapm, intercon, ARRAY_SIZE(intercon));
+	if (ret < 0) {
+		pr_err("%s: Failed to add DAPM routes (%d).\n",
+			__func__, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ab8500_codec_pcm_hw_params(struct snd_pcm_substream *substream,
+		struct snd_pcm_hw_params *hw_params, struct snd_soc_dai *dai)
+{
+	pr_debug("%s Enter.\n", __func__);
+	return 0;
+}
+
+static int ab8500_codec_pcm_startup(struct snd_pcm_substream *substream,
+		struct snd_soc_dai *dai)
+{
+	pr_debug("%s Enter.\n", __func__);
+
+	return 0;
+}
+
+static int ab8500_codec_pcm_prepare(struct snd_pcm_substream *substream,
+		struct snd_soc_dai *dai)
+{
+	pr_debug("%s Enter.\n", __func__);
+
+	/* Clear interrupt status registers by reading them. */
+	ab8500_codec_read_reg_audio(dai->codec, REG_AUDINTSOURCE1);
+	ab8500_codec_read_reg_audio(dai->codec, REG_AUDINTSOURCE2);
+
+	return 0;
+}
+
+static void ab8500_codec_pcm_shutdown(struct snd_pcm_substream *substream,
+		struct snd_soc_dai *dai)
+{
+	pr_debug("%s Enter.\n", __func__);
+
+	ab8500_codec_dump_all_reg(dai->codec);
+}
+
+static int ab8500_codec_set_dai_sysclk(struct snd_soc_dai *dai, int clk_id,
+		unsigned int freq, int dir)
+{
+	pr_err("%s Enter.\n", __func__);
+
+	return 0;
+}
+
+/* Gates clocking according format mask */
+static int ab8500_codec_set_dai_clock_gate(struct snd_soc_codec *codec, unsigned int fmt)
+{
+	unsigned int clear_mask;
+	unsigned int set_mask;
+
+	clear_mask = BMASK(REG_DIGIFCONF1_ENMASTGEN) |
+			BMASK(REG_DIGIFCONF1_ENFSBITCLK0);
+
+	set_mask = BMASK(REG_DIGIFCONF1_ENMASTGEN);
+
+	switch (fmt & SND_SOC_DAIFMT_CLOCK_MASK) {
+	case SND_SOC_DAIFMT_CONT: /* continuous clock */
+		pr_debug("%s: IF0 Clock is continous.\n", __func__);
+		set_mask |= BMASK(REG_DIGIFCONF1_ENFSBITCLK0);
+		break;
+	case SND_SOC_DAIFMT_GATED: /* clock is gated */
+		pr_debug("%s: IF0 Clock is gated.\n", __func__);
+		break;
+	default:
+		pr_err("%s: ERROR: Unsupporter clock mask (0x%x)!\n",
+			__func__,
+			fmt & SND_SOC_DAIFMT_CLOCK_MASK);
+		return -EINVAL;
+	}
+
+	ab8500_codec_update_reg_audio(codec, REG_DIGIFCONF1, clear_mask, set_mask);
+
+	return 0;
+}
+
+static int ab8500_codec_set_dai_fmt(struct snd_soc_dai *dai, unsigned int fmt)
+{
+	unsigned int clear_mask;
+	unsigned int set_mask;
+	struct snd_soc_codec *codec = dai->codec;
+	int err;
+
+	pr_debug("%s: Enter (fmt = 0x%x)\n", __func__, fmt);
+
+	clear_mask = BMASK(REG_DIGIFCONF3_IF1DATOIF0AD) |
+			BMASK(REG_DIGIFCONF3_IF1CLKTOIF0CLK) |
+			BMASK(REG_DIGIFCONF3_IF0BFIFOEN) |
+			BMASK(REG_DIGIFCONF3_IF0MASTER);
+	set_mask = 0;
+
+	switch (fmt & SND_SOC_DAIFMT_MASTER_MASK) {
+	case SND_SOC_DAIFMT_CBM_CFM: /* codec clk & FRM master */
+		pr_debug("%s: IF0 Master-mode: AB8500 master.\n", __func__);
+		set_mask |= BMASK(REG_DIGIFCONF3_IF0MASTER);
+		break;
+	case SND_SOC_DAIFMT_CBS_CFS: /* codec clk & FRM slave */
+		pr_debug("%s: IF0 Master-mode: AB8500 slave.\n", __func__);
+		break;
+	case SND_SOC_DAIFMT_CBS_CFM: /* codec clk slave & FRM master */
+	case SND_SOC_DAIFMT_CBM_CFS: /* codec clk master & frame slave */
+		pr_err("%s: ERROR: The device is either a master or a slave.\n", __func__);
+	default:
+		pr_err("%s: ERROR: Unsupporter master mask 0x%x\n",
+				__func__,
+				(fmt & SND_SOC_DAIFMT_MASTER_MASK));
+		return -EINVAL;
+		break;
+	}
+
+	ab8500_codec_update_reg_audio(codec, REG_DIGIFCONF3, clear_mask, set_mask);
+
+	/* Set clock gating */
+	err = ab8500_codec_set_dai_clock_gate(codec, fmt);
+	if (err) {
+		pr_err("%s: ERRROR: Failed to set clock gate (%d).\n", __func__, err);
+		return err;
+	}
+
+	/* Setting data transfer format */
+
+	clear_mask = BMASK(REG_DIGIFCONF2_IF0FORMAT0) |
+		BMASK(REG_DIGIFCONF2_IF0FORMAT1) |
+		BMASK(REG_DIGIFCONF2_FSYNC0P) |
+		BMASK(REG_DIGIFCONF2_BITCLK0P);
+	set_mask = 0;
+
+	switch (fmt & SND_SOC_DAIFMT_FORMAT_MASK) {
+	case SND_SOC_DAIFMT_I2S: /* I2S mode */
+		pr_debug("%s: IF0 Protocol: I2S\n", __func__);
+		set_mask |= BMASK(REG_DIGIFCONF2_IF0FORMAT1);
+
+		/* 32 bit, 0 delay */
+		ab8500_audio_set_word_length(dai, 32);
+		ab8500_audio_set_bit_delay(dai, 0);
+
+		break;
+	case SND_SOC_DAIFMT_DSP_A: /* L data MSB after FRM LRC */
+		pr_debug("%s: IF0 Protocol: DSP A (TDM)\n", __func__);
+		set_mask |= BMASK(REG_DIGIFCONF2_IF0FORMAT1);
+		break;
+	case SND_SOC_DAIFMT_DSP_B: /* L data MSB during FRM LRC */
+		pr_debug("%s: IF0 Protocol: DSP B (TDM)\n", __func__);
+		set_mask |= BMASK(REG_DIGIFCONF2_IF0FORMAT0);
+		break;
+	default:
+		pr_err("%s: ERROR: Unsupporter format (0x%x)!\n",
+				__func__,
+				fmt & SND_SOC_DAIFMT_FORMAT_MASK);
+		return -EINVAL;
+	}
+
+	switch (fmt & SND_SOC_DAIFMT_INV_MASK) {
+	case SND_SOC_DAIFMT_NB_NF: /* normal bit clock + frame */
+		pr_debug("%s: IF0: Normal bit clock, normal frame\n", __func__);
+		break;
+	case SND_SOC_DAIFMT_NB_IF: /* normal BCLK + inv FRM */
+		pr_debug("%s: IF0: Normal bit clock, inverted frame\n", __func__);
+		set_mask |= BMASK(REG_DIGIFCONF2_FSYNC0P);
+		break;
+	case SND_SOC_DAIFMT_IB_NF: /* invert BCLK + nor FRM */
+		pr_debug("%s: IF0: Inverted bit clock, normal frame\n", __func__);
+		set_mask |= BMASK(REG_DIGIFCONF2_BITCLK0P);
+		break;
+	case SND_SOC_DAIFMT_IB_IF: /* invert BCLK + FRM */
+		pr_debug("%s: IF0: Inverted bit clock, inverted frame\n", __func__);
+		set_mask |= BMASK(REG_DIGIFCONF2_FSYNC0P);
+		set_mask |= BMASK(REG_DIGIFCONF2_BITCLK0P);
+		break;
+	default:
+		pr_err("%s: ERROR: Unsupported INV mask 0x%x\n",
+				__func__,
+				(fmt & SND_SOC_DAIFMT_INV_MASK));
+		return -EINVAL;
+		break;
+	}
+
+	ab8500_codec_update_reg_audio(codec, REG_DIGIFCONF2, clear_mask, set_mask);
+
+	return 0;
+}
+
+static int ab8500_codec_set_dai_tdm_slot(struct snd_soc_dai *dai,
+		unsigned int tx_mask, unsigned int rx_mask,
+		int slots, int slot_width)
+{
+	struct snd_soc_codec *codec = dai->codec;
+	unsigned int set_mask, clear_mask, slots_active;
+
+	/* Only 16 bit slot width is supported at the moment in TDM mode */
+	if (slot_width != 16) {
+		pr_err("%s: ERROR: Unsupported slot_width %d.\n",
+			__func__, slot_width);
+		return -EINVAL;
+	}
+
+	/* Setup TDM clocking according to slot count */
+	pr_debug("%s: Slots, total: %d\n", __func__, slots);
+	clear_mask = BMASK(REG_DIGIFCONF1_IF0BITCLKOS0) |
+			BMASK(REG_DIGIFCONF1_IF0BITCLKOS1);
+	switch (slots) {
+	case 2:
+		set_mask = REG_MASK_NONE;
+		break;
+	case 4:
+		set_mask = BMASK(REG_DIGIFCONF1_IF0BITCLKOS0);
+		break;
+	case 8:
+		set_mask = BMASK(REG_DIGIFCONF1_IF0BITCLKOS1);
+		break;
+	case 16:
+		set_mask = BMASK(REG_DIGIFCONF1_IF0BITCLKOS0) |
+				BMASK(REG_DIGIFCONF1_IF0BITCLKOS1);
+		break;
+	default:
+		pr_err("%s: ERROR: Unsupported number of slots (%d)!\n", __func__, slots);
+		return -EINVAL;
+	}
+	ab8500_codec_update_reg_audio(codec, REG_DIGIFCONF1, clear_mask, set_mask);
+
+	/* Setup TDM DA according to active tx slots */
+	clear_mask = REG_DASLOTCONFX_SLTODAX_MASK;
+	slots_active = hweight32(tx_mask);
+	pr_debug("%s: Slots, active, TX: %d\n", __func__, slots_active);
+	switch (slots_active) {
+	case 0:
+		break;
+	case 1:
+		/* Slot 9 -> DA_IN1 & DA_IN3 */
+		ab8500_codec_update_reg_audio(codec, REG_DASLOTCONF1, clear_mask, 9);
+		ab8500_codec_update_reg_audio(codec, REG_DASLOTCONF3, clear_mask, 9);
+		break;
+	case 2:
+		/* Slot 9 -> DA_IN1 & DA_IN3, Slot 11 -> DA_IN2 & DA_IN4 */
+		ab8500_codec_update_reg_audio(codec, REG_DASLOTCONF1, clear_mask, 9);
+		ab8500_codec_update_reg_audio(codec, REG_DASLOTCONF3, clear_mask, 9);
+		ab8500_codec_update_reg_audio(codec, REG_DASLOTCONF2, clear_mask, 11);
+		ab8500_codec_update_reg_audio(codec, REG_DASLOTCONF4, clear_mask, 11);
+
+		break;
+	case 8:
+		pr_debug("%s: In 8-channel mode DA-from-slot mapping is set manually.", __func__);
+		break;
+	default:
+		pr_err("%s: Unsupported number of active TX-slots (%d)!\n", __func__, slots_active);
+		return -EINVAL;
+	}
+
+	/* Setup TDM AD according to active RX-slots */
+	slots_active = hweight32(rx_mask);
+	pr_debug("%s: Slots, active, RX: %d\n", __func__, slots_active);
+	switch (slots_active) {
+	case 0:
+		break;
+	case 1:
+		/* AD_OUT3 -> slot 0 & 1 */
+		ab8500_codec_update_reg_audio(codec, REG_ADSLOTSEL1,
+			REG_MASK_ALL,
+			REG_ADSLOTSELX_AD_OUT3_TO_SLOT_EVEN |
+			REG_ADSLOTSELX_AD_OUT3_TO_SLOT_ODD);
+		break;
+	case 2:
+		/* AD_OUT3 -> slot 0, AD_OUT2 -> slot 1 */
+		ab8500_codec_update_reg_audio(codec, REG_ADSLOTSEL1,
+			REG_MASK_ALL,
+			REG_ADSLOTSELX_AD_OUT3_TO_SLOT_EVEN |
+			REG_ADSLOTSELX_AD_OUT2_TO_SLOT_ODD);
+		break;
+	case 8:
+		pr_debug("%s: In 8-channel mode AD-to-slot mapping is set manually.", __func__);
+		break;
+	default:
+		pr_err("%s: Unsupported number of active RX-slots (%d)!\n", __func__, slots_active);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+struct snd_soc_dai_driver ab8500_codec_dai[] = {
+	{
+		.name = "ab8500-codec-dai.0",
+		.id = 0,
+		.playback = {
+			.stream_name = "ab8500_0p",
+			.channels_min = 1,
+			.channels_max = 8,
+			.rates = AB8500_SUPPORTED_RATE,
+			.formats = AB8500_SUPPORTED_FMT,
+		},
+		.ops = (struct snd_soc_dai_ops[]) {
+			{
+				.startup = ab8500_codec_pcm_startup,
+				.prepare = ab8500_codec_pcm_prepare,
+				.hw_params = ab8500_codec_pcm_hw_params,
+				.shutdown = ab8500_codec_pcm_shutdown,
+				.set_sysclk = ab8500_codec_set_dai_sysclk,
+				.set_tdm_slot = ab8500_codec_set_dai_tdm_slot,
+				.set_fmt = ab8500_codec_set_dai_fmt,
+			}
+		},
+		.symmetric_rates = 1
+	},
+	{
+		.name = "ab8500-codec-dai.1",
+		.id = 1,
+		.capture = {
+			.stream_name = "ab8500_0c",
+			.channels_min = 1,
+			.channels_max = 8,
+			.rates = AB8500_SUPPORTED_RATE,
+			.formats = AB8500_SUPPORTED_FMT,
+		},
+		.ops = (struct snd_soc_dai_ops[]) {
+			{
+				.startup = ab8500_codec_pcm_startup,
+				.prepare = ab8500_codec_pcm_prepare,
+				.hw_params = ab8500_codec_pcm_hw_params,
+				.shutdown = ab8500_codec_pcm_shutdown,
+				.set_sysclk = ab8500_codec_set_dai_sysclk,
+				.set_tdm_slot = ab8500_codec_set_dai_tdm_slot,
+				.set_fmt = ab8500_codec_set_dai_fmt,
+			}
+		},
+		.symmetric_rates = 1
+	}
+};
+
+static int ab8500_codec_probe(struct snd_soc_codec *codec)
+{
+	int i, ret;
+	u8 *cache = codec->reg_cache;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	ab8500_codec_configure_audio_macrocell(codec);
+
+	for (i = REG_AUDREV; i >= REG_POWERUP; i--)
+		ab8500_codec_write_reg_audio(codec, i, cache[i]);
+
+	/* Add controls */
+	ret = snd_soc_add_controls(codec, ab8500_snd_controls,
+			ARRAY_SIZE(ab8500_snd_controls));
+	if (ret < 0) {
+		pr_err("%s: failed to add soc controls (%d).\n",
+				__func__, ret);
+		return ret;
+	}
+
+	/* Add controls with events */
+	snd_ctl_add(codec->card->snd_card, snd_ctl_new1(&st_fir_value_control, codec));
+	snd_ctl_add(codec->card->snd_card, snd_ctl_new1(&st_fir_apply_control, codec));
+
+	/* Add DAPM-widgets */
+	ret = ab8500_codec_add_widgets(codec);
+	if (ret < 0) {
+		pr_err("%s: Failed add widgets (%d).\n", __func__, ret);
+		return ret;
+	}
+
+	ab8500_codec = codec;
+
+	return ret;
+}
+
+static int ab8500_codec_remove(struct snd_soc_codec *codec)
+{
+	snd_soc_dapm_free(&codec->dapm);
+	ab8500_codec = NULL;
+
+	return 0;
+}
+
+static int ab8500_codec_suspend(struct snd_soc_codec *codec,
+		pm_message_t state)
+{
+	pr_debug("%s Enter.\n", __func__);
+
+	return 0;
+}
+
+static int ab8500_codec_resume(struct snd_soc_codec *codec)
+{
+	pr_debug("%s Enter.\n", __func__);
+
+	return 0;
+}
+
+struct snd_soc_codec_driver ab8500_codec_driver = {
+	.probe =		ab8500_codec_probe,
+	.remove =		ab8500_codec_remove,
+	.suspend =		ab8500_codec_suspend,
+	.resume =		ab8500_codec_resume,
+	.read =			ab8500_codec_read_reg_audio,
+	.write =		ab8500_codec_write_reg_audio,
+	.reg_cache_size =	ARRAY_SIZE(ab8500_reg_cache),
+	.reg_word_size =	sizeof(u8),
+	.reg_cache_default =	ab8500_reg_cache,
+};
+
+static int __devinit ab8500_codec_driver_probe(struct platform_device *pdev)
+{
+	int err;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	pr_info("%s: Register codec.\n", __func__);
+	err = snd_soc_register_codec(&pdev->dev,
+				&ab8500_codec_driver,
+				ab8500_codec_dai,
+				ARRAY_SIZE(ab8500_codec_dai));
+
+	if (err < 0) {
+		pr_err("%s: Error: Failed to register codec (%d).\n",
+			__func__, err);
+	}
+
+	return err;
+}
+
+static int __devexit ab8500_codec_driver_remove(struct platform_device *pdev)
+{
+	pr_info("%s Enter.\n", __func__);
+
+	snd_soc_unregister_codec(&pdev->dev);
+
+	return 0;
+}
+
+static int ab8500_codec_driver_suspend(struct platform_device *pdev,
+		pm_message_t state)
+{
+	pr_debug("%s Enter.\n", __func__);
+
+	return 0;
+}
+
+static int ab8500_codec_driver_resume(struct platform_device *pdev)
+{
+	pr_debug("%s Enter.\n", __func__);
+
+	return 0;
+}
+
+static struct platform_driver ab8500_codec_platform_driver = {
+	.driver	= {
+		.name	= "ab8500-codec",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= ab8500_codec_driver_probe,
+	.remove		= __devexit_p(ab8500_codec_driver_remove),
+	.suspend	= ab8500_codec_driver_suspend,
+	.resume		= ab8500_codec_driver_resume,
+};
+
+static int __devinit ab8500_codec_platform_driver_init(void)
+{
+	int ret;
+
+	pr_info("%s: Enter.\n", __func__);
+
+	ret = platform_driver_register(&ab8500_codec_platform_driver);
+	if (ret != 0) {
+		pr_err("%s: Failed to register AB8500 platform driver (%d)!\n",
+			__func__, ret);
+	}
+
+	return ret;
+}
+
+static void __exit ab8500_codec_platform_driver_exit(void)
+{
+	pr_info("%s: Enter.\n", __func__);
+
+	platform_driver_unregister(&ab8500_codec_platform_driver);
+}
+
+module_init(ab8500_codec_platform_driver_init);
+module_exit(ab8500_codec_platform_driver_exit);
+
+MODULE_DESCRIPTION("AB8500 Codec driver");
+MODULE_ALIAS("platform:ab8500-codec");
+MODULE_AUTHOR("ST-Ericsson");
+MODULE_LICENSE("GPL v2");
diff --git a/sound/soc/codecs/ab8500_audio.h b/sound/soc/codecs/ab8500_audio.h
new file mode 100644
index 00000000000..c3b24bbc754
--- /dev/null
+++ b/sound/soc/codecs/ab8500_audio.h
@@ -0,0 +1,573 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Mikko J. Lehto <mikko.lehto@symbio.com>,
+ *         Mikko Sarmanne <mikko.sarmanne@symbio.com>,
+ *         Ola Lilja <ola.o.lilja@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 AB8500_CODEC_REGISTERS_H
+#define AB8500_CODEC_REGISTERS_H
+
+#define AB8500_SUPPORTED_RATE (SNDRV_PCM_RATE_48000)
+#define AB8500_SUPPORTED_FMT (SNDRV_PCM_FMTBIT_S16_LE)
+
+extern struct snd_soc_dai_driver ab8500_codec_dai[];
+extern struct snd_soc_codec_driver soc_codec_dev_ab8500;
+
+/* Extended interface for codec-driver */
+
+void ab8500_audio_power_control(bool power_on);
+void ab8500_audio_pwm_vibra(unsigned char speed_left_pos,
+			unsigned char speed_left_neg,
+			unsigned char speed_right_pos,
+			unsigned char speed_right_neg);
+int ab8500_audio_set_word_length(struct snd_soc_dai *dai, unsigned int wl);
+int ab8500_audio_set_bit_delay(struct snd_soc_dai *dai, unsigned int delay);
+int ab8500_audio_setup_if1(struct snd_soc_codec *codec,
+			unsigned int fmt,
+			unsigned int wl,
+			unsigned int delay);
+
+enum ab8500_audio_dapm_path {
+	AB8500_AUDIO_DAPM_PATH_DMIC,
+	AB8500_AUDIO_DAPM_PATH_AMIC1,
+	AB8500_AUDIO_DAPM_PATH_AMIC2
+};
+bool ab8500_audio_dapm_path_active(enum ab8500_audio_dapm_path dapm_path);
+
+/* AB8500 audio bank (0x0d) register definitions */
+
+#define REG_POWERUP		0x00
+#define REG_AUDSWRESET		0x01
+#define REG_ADPATHENA		0x02
+#define REG_DAPATHENA		0x03
+#define REG_ANACONF1		0x04
+#define REG_ANACONF2		0x05
+#define REG_DIGMICCONF		0x06
+#define REG_ANACONF3		0x07
+#define REG_ANACONF4		0x08
+#define REG_DAPATHCONF		0x09
+#define REG_MUTECONF		0x0A
+#define REG_SHORTCIRCONF	0x0B
+#define REG_ANACONF5		0x0C
+#define REG_ENVCPCONF		0x0D
+#define REG_SIGENVCONF		0x0E
+#define REG_PWMGENCONF1		0x0F
+#define REG_PWMGENCONF2		0x10
+#define REG_PWMGENCONF3		0x11
+#define REG_PWMGENCONF4		0x12
+#define REG_PWMGENCONF5		0x13
+#define REG_ANAGAIN1		0x14
+#define REG_ANAGAIN2		0x15
+#define REG_ANAGAIN3		0x16
+#define REG_ANAGAIN4		0x17
+#define REG_DIGLINHSLGAIN	0x18
+#define REG_DIGLINHSRGAIN	0x19
+#define REG_ADFILTCONF		0x1A
+#define REG_DIGIFCONF1		0x1B
+#define REG_DIGIFCONF2		0x1C
+#define REG_DIGIFCONF3		0x1D
+#define REG_DIGIFCONF4		0x1E
+#define REG_ADSLOTSEL1		0x1F
+#define REG_ADSLOTSEL2		0x20
+#define REG_ADSLOTSEL3		0x21
+#define REG_ADSLOTSEL4		0x22
+#define REG_ADSLOTSEL5		0x23
+#define REG_ADSLOTSEL6		0x24
+#define REG_ADSLOTSEL7		0x25
+#define REG_ADSLOTSEL8		0x26
+#define REG_ADSLOTSEL9		0x27
+#define REG_ADSLOTSEL10		0x28
+#define REG_ADSLOTSEL11		0x29
+#define REG_ADSLOTSEL12		0x2A
+#define REG_ADSLOTSEL13		0x2B
+#define REG_ADSLOTSEL14		0x2C
+#define REG_ADSLOTSEL15		0x2D
+#define REG_ADSLOTSEL16		0x2E
+#define REG_ADSLOTHIZCTRL1	0x2F
+#define REG_ADSLOTHIZCTRL2	0x30
+#define REG_ADSLOTHIZCTRL3	0x31
+#define REG_ADSLOTHIZCTRL4	0x32
+#define REG_DASLOTCONF1		0x33
+#define REG_DASLOTCONF2		0x34
+#define REG_DASLOTCONF3		0x35
+#define REG_DASLOTCONF4		0x36
+#define REG_DASLOTCONF5		0x37
+#define REG_DASLOTCONF6		0x38
+#define REG_DASLOTCONF7		0x39
+#define REG_DASLOTCONF8		0x3A
+#define REG_CLASSDCONF1		0x3B
+#define REG_CLASSDCONF2		0x3C
+#define REG_CLASSDCONF3		0x3D
+#define REG_DMICFILTCONF	0x3E
+#define REG_DIGMULTCONF1	0x3F
+#define REG_DIGMULTCONF2	0x40
+#define REG_ADDIGGAIN1		0x41
+#define REG_ADDIGGAIN2		0x42
+#define REG_ADDIGGAIN3		0x43
+#define REG_ADDIGGAIN4		0x44
+#define REG_ADDIGGAIN5		0x45
+#define REG_ADDIGGAIN6		0x46
+#define REG_DADIGGAIN1		0x47
+#define REG_DADIGGAIN2		0x48
+#define REG_DADIGGAIN3		0x49
+#define REG_DADIGGAIN4		0x4A
+#define REG_DADIGGAIN5		0x4B
+#define REG_DADIGGAIN6		0x4C
+#define REG_ADDIGLOOPGAIN1	0x4D
+#define REG_ADDIGLOOPGAIN2	0x4E
+#define REG_HSLEARDIGGAIN	0x4F
+#define REG_HSRDIGGAIN		0x50
+#define REG_SIDFIRGAIN1		0x51
+#define REG_SIDFIRGAIN2		0x52
+#define REG_ANCCONF1		0x53
+#define REG_ANCCONF2		0x54
+#define REG_ANCCONF3		0x55
+#define REG_ANCCONF4		0x56
+#define REG_ANCCONF5		0x57
+#define REG_ANCCONF6		0x58
+#define REG_ANCCONF7		0x59
+#define REG_ANCCONF8		0x5A
+#define REG_ANCCONF9		0x5B
+#define REG_ANCCONF10		0x5C
+#define REG_ANCCONF11		0x5D
+#define REG_ANCCONF12		0x5E
+#define REG_ANCCONF13		0x5F
+#define REG_ANCCONF14		0x60
+#define REG_SIDFIRADR		0x61
+#define REG_SIDFIRCOEF1		0x62
+#define REG_SIDFIRCOEF2		0x63
+#define REG_SIDFIRCONF		0x64
+#define REG_AUDINTMASK1		0x65
+#define REG_AUDINTSOURCE1	0x66
+#define REG_AUDINTMASK2		0x67
+#define REG_AUDINTSOURCE2	0x68
+#define REG_FIFOCONF1		0x69
+#define REG_FIFOCONF2		0x6A
+#define REG_FIFOCONF3		0x6B
+#define REG_FIFOCONF4		0x6C
+#define REG_FIFOCONF5		0x6D
+#define REG_FIFOCONF6		0x6E
+#define REG_AUDREV		0x6F
+
+#define AB8500_FIRST_REG	REG_POWERUP
+#define AB8500_LAST_REG		REG_AUDREV
+#define AB8500_CACHEREGNUM	(AB8500_LAST_REG + 1)
+
+
+#define REG_MASK_ALL				0xFF
+#define REG_MASK_NONE				0x00
+
+/* REG_POWERUP */
+#define REG_POWERUP_POWERUP			7
+#define REG_POWERUP_ENANA			3
+
+/* REG_AUDSWRESET */
+#define REG_AUDSWRESET_SWRESET			7
+
+/* REG_ADPATHENA */
+#define REG_ADPATHENA_ENAD12			7
+#define REG_ADPATHENA_ENAD34			5
+#define REG_ADPATHENA_ENAD5768			3
+
+/* REG_DAPATHENA */
+#define REG_DAPATHENA_ENDA1			7
+#define REG_DAPATHENA_ENDA2			6
+#define REG_DAPATHENA_ENDA3			5
+#define REG_DAPATHENA_ENDA4			4
+#define REG_DAPATHENA_ENDA5			3
+#define REG_DAPATHENA_ENDA6			2
+
+/* REG_ANACONF1 */
+#define REG_ANACONF1_HSLOWPOW			7
+#define REG_ANACONF1_DACLOWPOW1			6
+#define REG_ANACONF1_DACLOWPOW0			5
+#define REG_ANACONF1_EARDACLOWPOW		4
+#define REG_ANACONF1_EARSELCM			2
+#define REG_ANACONF1_HSHPEN			1
+#define REG_ANACONF1_EARDRVLOWPOW		0
+
+/* REG_ANACONF2 */
+#define REG_ANACONF2_ENMIC1			7
+#define REG_ANACONF2_ENMIC2			6
+#define REG_ANACONF2_ENLINL			5
+#define REG_ANACONF2_ENLINR			4
+#define REG_ANACONF2_MUTMIC1			3
+#define REG_ANACONF2_MUTMIC2			2
+#define REG_ANACONF2_MUTLINL			1
+#define REG_ANACONF2_MUTLINR			0
+
+/* REG_DIGMICCONF */
+#define REG_DIGMICCONF_ENDMIC1			7
+#define REG_DIGMICCONF_ENDMIC2			6
+#define REG_DIGMICCONF_ENDMIC3			5
+#define REG_DIGMICCONF_ENDMIC4			4
+#define REG_DIGMICCONF_ENDMIC5			3
+#define REG_DIGMICCONF_ENDMIC6			2
+#define REG_DIGMICCONF_HSFADSPEED		0
+
+/* REG_ANACONF3 */
+#define REG_ANACONF3_MIC1SEL			7
+#define REG_ANACONF3_LINRSEL			6
+#define REG_ANACONF3_ENDRVHSL			5
+#define REG_ANACONF3_ENDRVHSR			4
+#define REG_ANACONF3_ENADCMIC			2
+#define REG_ANACONF3_ENADCLINL			1
+#define REG_ANACONF3_ENADCLINR			0
+
+/* REG_ANACONF4 */
+#define REG_ANACONF4_DISPDVSS			7
+#define REG_ANACONF4_ENEAR			6
+#define REG_ANACONF4_ENHSL			5
+#define REG_ANACONF4_ENHSR			4
+#define REG_ANACONF4_ENHFL			3
+#define REG_ANACONF4_ENHFR			2
+#define REG_ANACONF4_ENVIB1			1
+#define REG_ANACONF4_ENVIB2			0
+
+/* REG_DAPATHCONF */
+#define REG_DAPATHCONF_ENDACEAR			6
+#define REG_DAPATHCONF_ENDACHSL			5
+#define REG_DAPATHCONF_ENDACHSR			4
+#define REG_DAPATHCONF_ENDACHFL			3
+#define REG_DAPATHCONF_ENDACHFR			2
+#define REG_DAPATHCONF_ENDACVIB1		1
+#define REG_DAPATHCONF_ENDACVIB2		0
+
+/* REG_MUTECONF */
+#define REG_MUTECONF_MUTEAR			6
+#define REG_MUTECONF_MUTHSL			5
+#define REG_MUTECONF_MUTHSR			4
+
+/* REG_SHORTCIRCONF */
+/* REG_ANACONF5 */
+#define REG_ANACONF5_ENCPHS			7
+#define REG_ANACONF5_HSAUTOEN			0
+
+/* REG_ENVCPCONF */
+#define REG_ENVCPCONF_ENVDETHTHRE		4
+#define REG_ENVCPCONF_ENVDETLTHRE		0
+#define REG_ENVCPCONF_ENVDETHTHRE_MAX		0x0F
+#define REG_ENVCPCONF_ENVDETLTHRE_MAX		0x0F
+
+/* REG_SIGENVCONF */
+#define REG_SIGENVCONF_CPLVEN			5
+#define REG_SIGENVCONF_ENVDETCPEN		4
+#define REG_SIGENVCONF_ENVDETTIME		0
+#define REG_SIGENVCONF_ENVDETTIME_MAX		0x0F
+
+/* REG_PWMGENCONF1 */
+#define REG_PWMGENCONF1_PWMTOVIB1		7
+#define REG_PWMGENCONF1_PWMTOVIB2		6
+#define REG_PWMGENCONF1_PWM1CTRL		5
+#define REG_PWMGENCONF1_PWM2CTRL		4
+#define REG_PWMGENCONF1_PWM1NCTRL		3
+#define REG_PWMGENCONF1_PWM1PCTRL		2
+#define REG_PWMGENCONF1_PWM2NCTRL		1
+#define REG_PWMGENCONF1_PWM2PCTRL		0
+
+/* REG_PWMGENCONF2 */
+/* REG_PWMGENCONF3 */
+/* REG_PWMGENCONF4 */
+/* REG_PWMGENCONF5 */
+#define REG_PWMGENCONFX_PWMVIBXPOL		7
+#define REG_PWMGENCONFX_PWMVIBXDUTCYC		0
+#define REG_PWMGENCONFX_PWMVIBXDUTCYC_MAX	0x64
+
+/* REG_ANAGAIN1 */
+/* REG_ANAGAIN2 */
+#define REG_ANAGAINX_ENSEMICX			7
+#define REG_ANAGAINX_LOWPOWMICX			6
+#define REG_ANAGAINX_MICXGAIN			0
+#define REG_ANAGAINX_MICXGAIN_MAX		0x1F
+
+/* REG_ANAGAIN3 */
+#define REG_ANAGAIN3_HSLGAIN			4
+#define REG_ANAGAIN3_HSRGAIN			0
+#define REG_ANAGAIN3_HSXGAIN_MAX		0x0F
+
+/* REG_ANAGAIN4 */
+#define REG_ANAGAIN4_LINLGAIN			4
+#define REG_ANAGAIN4_LINRGAIN			0
+#define REG_ANAGAIN4_LINXGAIN_MAX		0x0F
+
+/* REG_DIGLINHSLGAIN */
+/* REG_DIGLINHSRGAIN */
+#define REG_DIGLINHSXGAIN_LINTOHSXGAIN		0
+#define REG_DIGLINHSXGAIN_LINTOHSXGAIN_MAX	0x13
+
+/* REG_ADFILTCONF */
+#define REG_ADFILTCONF_AD1NH			7
+#define REG_ADFILTCONF_AD2NH			6
+#define REG_ADFILTCONF_AD3NH			5
+#define REG_ADFILTCONF_AD4NH			4
+#define REG_ADFILTCONF_AD1VOICE			3
+#define REG_ADFILTCONF_AD2VOICE			2
+#define REG_ADFILTCONF_AD3VOICE			1
+#define REG_ADFILTCONF_AD4VOICE			0
+
+/* REG_DIGIFCONF1 */
+#define REG_DIGIFCONF1_ENMASTGEN		7
+#define REG_DIGIFCONF1_IF1BITCLKOS1		6
+#define REG_DIGIFCONF1_IF1BITCLKOS0		5
+#define REG_DIGIFCONF1_ENFSBITCLK1		4
+#define REG_DIGIFCONF1_IF0BITCLKOS1		2
+#define REG_DIGIFCONF1_IF0BITCLKOS0		1
+#define REG_DIGIFCONF1_ENFSBITCLK0		0
+
+/* REG_DIGIFCONF2 */
+#define REG_DIGIFCONF2_FSYNC0P			6
+#define REG_DIGIFCONF2_BITCLK0P			5
+#define REG_DIGIFCONF2_IF0DEL			4
+#define REG_DIGIFCONF2_IF0FORMAT1		3
+#define REG_DIGIFCONF2_IF0FORMAT0		2
+#define REG_DIGIFCONF2_IF0WL1			1
+#define REG_DIGIFCONF2_IF0WL0			0
+
+/* REG_DIGIFCONF3 */
+#define REG_DIGIFCONF3_IF0DATOIF1AD		7
+#define REG_DIGIFCONF3_IF0CLKTOIF1CLK		6
+#define REG_DIGIFCONF3_IF1MASTER		5
+#define REG_DIGIFCONF3_IF1DATOIF0AD		3
+#define REG_DIGIFCONF3_IF1CLKTOIF0CLK		2
+#define REG_DIGIFCONF3_IF0MASTER		1
+#define REG_DIGIFCONF3_IF0BFIFOEN		0
+
+/* REG_DIGIFCONF4 */
+#define REG_DIGIFCONF4_FSYNC1P			6
+#define REG_DIGIFCONF4_BITCLK1P			5
+#define REG_DIGIFCONF4_IF1DEL			4
+#define REG_DIGIFCONF4_IF1FORMAT1		3
+#define REG_DIGIFCONF4_IF1FORMAT0		2
+#define REG_DIGIFCONF4_IF1WL1			1
+#define REG_DIGIFCONF4_IF1WL0			0
+
+/* REG_ADSLOTSELX */
+#define REG_ADSLOTSELX_AD_OUT1_TO_SLOT_ODD	0x00
+#define REG_ADSLOTSELX_AD_OUT2_TO_SLOT_ODD	0x01
+#define REG_ADSLOTSELX_AD_OUT3_TO_SLOT_ODD	0x02
+#define REG_ADSLOTSELX_AD_OUT4_TO_SLOT_ODD	0x03
+#define REG_ADSLOTSELX_AD_OUT5_TO_SLOT_ODD	0x04
+#define REG_ADSLOTSELX_AD_OUT6_TO_SLOT_ODD	0x05
+#define REG_ADSLOTSELX_AD_OUT7_TO_SLOT_ODD	0x06
+#define REG_ADSLOTSELX_AD_OUT8_TO_SLOT_ODD	0x07
+#define REG_ADSLOTSELX_ZEROES_TO_SLOT_ODD	0x08
+#define REG_ADSLOTSELX_TRISTATE_TO_SLOT_ODD	0x0F
+#define REG_ADSLOTSELX_AD_OUT1_TO_SLOT_EVEN	0x00
+#define REG_ADSLOTSELX_AD_OUT2_TO_SLOT_EVEN	0x10
+#define REG_ADSLOTSELX_AD_OUT3_TO_SLOT_EVEN	0x20
+#define REG_ADSLOTSELX_AD_OUT4_TO_SLOT_EVEN	0x30
+#define REG_ADSLOTSELX_AD_OUT5_TO_SLOT_EVEN	0x40
+#define REG_ADSLOTSELX_AD_OUT6_TO_SLOT_EVEN	0x50
+#define REG_ADSLOTSELX_AD_OUT7_TO_SLOT_EVEN	0x60
+#define REG_ADSLOTSELX_AD_OUT8_TO_SLOT_EVEN	0x70
+#define REG_ADSLOTSELX_ZEROES_TO_SLOT_EVEN	0x80
+#define REG_ADSLOTSELX_TRISTATE_TO_SLOT_EVEN	0xF0
+#define REG_ADSLOTSELX_EVEN_SHIFT		0
+#define REG_ADSLOTSELX_ODD_SHIFT		4
+
+/* REG_ADSLOTHIZCTRL1 */
+/* REG_ADSLOTHIZCTRL2 */
+/* REG_ADSLOTHIZCTRL3 */
+/* REG_ADSLOTHIZCTRL4 */
+/* REG_DASLOTCONF1 */
+#define REG_DASLOTCONF1_DA12VOICE		7
+#define REG_DASLOTCONF1_SWAPDA12_34		6
+#define REG_DASLOTCONF1_DAI7TOADO1		5
+
+/* REG_DASLOTCONF2 */
+#define REG_DASLOTCONF2_DAI8TOADO2		5
+
+/* REG_DASLOTCONF3 */
+#define REG_DASLOTCONF3_DA34VOICE		7
+#define REG_DASLOTCONF3_DAI7TOADO3		5
+
+/* REG_DASLOTCONF4 */
+#define REG_DASLOTCONF4_DAI8TOADO4		5
+
+/* REG_DASLOTCONF5 */
+#define REG_DASLOTCONF5_DA56VOICE		7
+#define REG_DASLOTCONF5_DAI7TOADO5		5
+
+/* REG_DASLOTCONF6 */
+#define REG_DASLOTCONF6_DAI8TOADO6		5
+
+/* REG_DASLOTCONF7 */
+#define REG_DASLOTCONF7_DAI8TOADO7		5
+
+/* REG_DASLOTCONF8 */
+#define REG_DASLOTCONF8_DAI7TOADO8		5
+
+#define REG_DASLOTCONFX_SLTODAX_SHIFT		0
+#define REG_DASLOTCONFX_SLTODAX_MASK		0x1F
+
+/* REG_CLASSDCONF1 */
+#define REG_CLASSDCONF1_PARLHF			7
+#define REG_CLASSDCONF1_PARLVIB			6
+#define REG_CLASSDCONF1_VIB1SWAPEN		3
+#define REG_CLASSDCONF1_VIB2SWAPEN		2
+#define REG_CLASSDCONF1_HFLSWAPEN		1
+#define REG_CLASSDCONF1_HFRSWAPEN		0
+
+/* REG_CLASSDCONF2 */
+#define REG_CLASSDCONF2_FIRBYP3			7
+#define REG_CLASSDCONF2_FIRBYP2			6
+#define REG_CLASSDCONF2_FIRBYP1			5
+#define REG_CLASSDCONF2_FIRBYP0			4
+#define REG_CLASSDCONF2_HIGHVOLEN3		3
+#define REG_CLASSDCONF2_HIGHVOLEN2		2
+#define REG_CLASSDCONF2_HIGHVOLEN1		1
+#define REG_CLASSDCONF2_HIGHVOLEN0		0
+
+/* REG_CLASSDCONF3 */
+#define REG_CLASSDCONF3_DITHHPGAIN		4
+#define REG_CLASSDCONF3_DITHHPGAIN_MAX		0x0A
+#define REG_CLASSDCONF3_DITHWGAIN		0
+#define REG_CLASSDCONF3_DITHWGAIN_MAX		0x0A
+
+/* REG_DMICFILTCONF */
+#define REG_DMICFILTCONF_ANCINSEL		7
+#define REG_DMICFILTCONF_DA3TOEAR		6
+#define REG_DMICFILTCONF_DMIC1SINC3		5
+#define REG_DMICFILTCONF_DMIC2SINC3		4
+#define REG_DMICFILTCONF_DMIC3SINC3		3
+#define REG_DMICFILTCONF_DMIC4SINC3		2
+#define REG_DMICFILTCONF_DMIC5SINC3		1
+#define REG_DMICFILTCONF_DMIC6SINC3		0
+
+/* REG_DIGMULTCONF1 */
+#define REG_DIGMULTCONF1_DATOHSLEN		7
+#define REG_DIGMULTCONF1_DATOHSREN		6
+#define REG_DIGMULTCONF1_AD1SEL			5
+#define REG_DIGMULTCONF1_AD2SEL			4
+#define REG_DIGMULTCONF1_AD3SEL			3
+#define REG_DIGMULTCONF1_AD5SEL			2
+#define REG_DIGMULTCONF1_AD6SEL			1
+#define REG_DIGMULTCONF1_ANCSEL			0
+
+/* REG_DIGMULTCONF2 */
+#define REG_DIGMULTCONF2_DATOHFREN		7
+#define REG_DIGMULTCONF2_DATOHFLEN		6
+#define REG_DIGMULTCONF2_HFRSEL			5
+#define REG_DIGMULTCONF2_HFLSEL			4
+#define REG_DIGMULTCONF2_FIRSID1SEL		2
+#define REG_DIGMULTCONF2_FIRSID2SEL		0
+
+/* REG_ADDIGGAIN1 */
+/* REG_ADDIGGAIN2 */
+/* REG_ADDIGGAIN3 */
+/* REG_ADDIGGAIN4 */
+/* REG_ADDIGGAIN5 */
+/* REG_ADDIGGAIN6 */
+#define REG_ADDIGGAINX_FADEDISADX		6
+#define REG_ADDIGGAINX_ADXGAIN_MAX		0x3F
+
+/* REG_DADIGGAIN1 */
+/* REG_DADIGGAIN2 */
+/* REG_DADIGGAIN3 */
+/* REG_DADIGGAIN4 */
+/* REG_DADIGGAIN5 */
+/* REG_DADIGGAIN6 */
+#define REG_DADIGGAINX_FADEDISDAX		6
+#define REG_DADIGGAINX_DAXGAIN_MAX		0x3F
+
+/* REG_ADDIGLOOPGAIN1 */
+/* REG_ADDIGLOOPGAIN2 */
+#define REG_ADDIGLOOPGAINX_FADEDISADXL		6
+#define REG_ADDIGLOOPGAINX_ADXLBGAIN_MAX	0x3F
+
+/* REG_HSLEARDIGGAIN */
+#define REG_HSLEARDIGGAIN_HSSINC1		7
+#define REG_HSLEARDIGGAIN_FADEDISHSL		4
+#define REG_HSLEARDIGGAIN_HSLDGAIN_MAX		0x09
+
+/* REG_HSRDIGGAIN */
+#define REG_HSRDIGGAIN_FADESPEED		6
+#define REG_HSRDIGGAIN_FADEDISHSR		4
+#define REG_HSRDIGGAIN_HSRDGAIN_MAX		0x09
+
+/* REG_SIDFIRGAIN1 */
+/* REG_SIDFIRGAIN2 */
+#define REG_SIDFIRGAINX_FIRSIDXGAIN_MAX		0x1F
+
+/* REG_ANCCONF1 */
+#define REG_ANCCONF1_ANCIIRUPDATE		3
+#define REG_ANCCONF1_ENANC			2
+#define REG_ANCCONF1_ANCIIRINIT			1
+#define REG_ANCCONF1_ANCFIRUPDATE		0
+
+/* REG_ANCCONF2 */
+/* REG_ANCCONF3 */
+/* REG_ANCCONF4 */
+/* REG_ANCCONF5 */
+/* REG_ANCCONF6 */
+/* REG_ANCCONF7 */
+/* REG_ANCCONF8 */
+/* REG_ANCCONF9 */
+/* REG_ANCCONF10 */
+/* REG_ANCCONF11 */
+/* REG_ANCCONF12 */
+/* REG_ANCCONF13 */
+/* REG_ANCCONF14 */
+
+/* REG_SIDFIRADR */
+#define REG_SIDFIRADR_FIRSIDSET			7
+#define REG_SIDFIRADR_ADDRESS_SHIFT		0
+#define REG_SIDFIRADR_ADDRESS_MAX		0x7F
+
+/* REG_SIDFIRCOEF1 */
+/* REG_SIDFIRCOEF2 */
+#define REG_SIDFIRCOEFX_VALUE_SHIFT		0
+#define REG_SIDFIRCOEFX_VALUE_MAX		0xFF
+
+/* REG_SIDFIRCONF */
+#define REG_SIDFIRCONF_ENFIRSIDS		2
+#define REG_SIDFIRCONF_FIRSIDSTOIF1		1
+#define REG_SIDFIRCONF_FIRSIDBUSY		0
+
+/* REG_AUDINTMASK1 */
+/* REG_AUDINTSOURCE1 */
+/* REG_AUDINTMASK2 */
+/* REG_AUDINTSOURCE2 */
+
+/* REG_FIFOCONF1 */
+#define REG_FIFOCONF1_BFIFOMASK			0x80
+#define REG_FIFOCONF1_BFIFO19M2			0x40
+#define REG_FIFOCONF1_BFIFOINT_SHIFT		0
+#define REG_FIFOCONF1_BFIFOINT_MAX		0x3F
+
+/* REG_FIFOCONF2 */
+#define REG_FIFOCONF2_BFIFOTX_SHIFT		0
+#define REG_FIFOCONF2_BFIFOTX_MAX		0xFF
+
+/* REG_FIFOCONF3 */
+#define REG_FIFOCONF3_BFIFOEXSL_SHIFT		5
+#define REG_FIFOCONF3_BFIFOEXSL_MAX		0x5
+#define REG_FIFOCONF3_PREBITCLK0_SHIFT		2
+#define REG_FIFOCONF3_PREBITCLK0_MAX		0x7
+#define REG_FIFOCONF3_BFIFOMAST_SHIFT		1
+#define REG_FIFOCONF3_BFIFORUN_SHIFT		0
+
+/* REG_FIFOCONF4 */
+#define REG_FIFOCONF4_BFIFOFRAMSW_SHIFT		0
+#define REG_FIFOCONF4_BFIFOFRAMSW_MAX		0xFF
+
+/* REG_FIFOCONF5 */
+#define REG_FIFOCONF5_BFIFOWAKEUP_SHIFT		0
+#define REG_FIFOCONF5_BFIFOWAKEUP_MAX		0xFF
+
+/* REG_FIFOCONF6 */
+#define REG_FIFOCONF6_BFIFOSAMPLE_SHIFT		0
+#define REG_FIFOCONF6_BFIFOSAMPLE_MAX		0xFF
+
+/* REG_AUDREV */
+
+#endif
diff --git a/sound/soc/codecs/av8100_audio.c b/sound/soc/codecs/av8100_audio.c
new file mode 100644
index 00000000000..b163151efb7
--- /dev/null
+++ b/sound/soc/codecs/av8100_audio.c
@@ -0,0 +1,453 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/pm.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <sound/initval.h>
+#include <sound/soc.h>
+#include <video/av8100.h>
+#include <video/hdmi.h>
+
+#include "av8100_audio.h"
+
+/* codec private data */
+struct av8100_codec_dai_data {
+	struct hdmi_audio_settings as;
+};
+
+static struct av8100_codec_dai_data *get_dai_data_codec(struct snd_soc_codec *codec,
+						int dai_id)
+{
+	struct av8100_codec_dai_data *dai_data = snd_soc_codec_get_drvdata(codec);
+	return &dai_data[dai_id];
+}
+
+static struct av8100_codec_dai_data *get_dai_data(struct snd_soc_dai *codec_dai)
+{
+	return get_dai_data_codec(codec_dai->codec, codec_dai->id);
+}
+
+/* Extended interface for codec-driver */
+
+int av8100_audio_change_hdmi_audio_settings(struct snd_soc_dai *codec_dai,
+					struct hdmi_audio_settings *as)
+{
+	struct av8100_codec_dai_data *dai_data = get_dai_data(codec_dai);
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	dai_data->as.audio_coding_type = as->audio_coding_type;
+	dai_data->as.audio_channel_count = as->audio_channel_count;
+	dai_data->as.sampling_frequency = as->sampling_frequency;
+	dai_data->as.sample_size = as->sample_size;
+	dai_data->as.channel_allocation = as->channel_allocation;
+	dai_data->as.level_shift_value = as->level_shift_value;
+	dai_data->as.downmix_inhibit = as->downmix_inhibit;
+
+	return 0;
+}
+
+static int av8100_codec_powerup(void)
+{
+	struct av8100_status status;
+	int ret;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		pr_debug("%s: Powering up AV8100.", __func__);
+		ret = av8100_powerup();
+		if (ret != 0) {
+			pr_err("%s: Power up AV8100 failed "
+				"(av8100_powerup returned %d)!\n",
+				__func__,
+				ret);
+			return -EINVAL;
+		}
+	}
+	if (status.av8100_state < AV8100_OPMODE_INIT) {
+		ret = av8100_download_firmware(NULL, 0, I2C_INTERFACE);
+		if (ret != 0) {
+			pr_err("%s: Download firmware failed "
+				"(av8100_download_firmware returned %d)!\n",
+				__func__,
+				ret);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int av8100_codec_setup_hdmi_format(void)
+{
+	union av8100_configuration config;
+	int ret;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	pr_debug("%s: hdmi_mode = AV8100_HDMI_ON.", __func__);
+	pr_debug("%s: hdmi_format = AV8100_HDMI.", __func__);
+	config.hdmi_format.hdmi_mode = AV8100_HDMI_ON;
+	config.hdmi_format.hdmi_format = AV8100_HDMI;
+	ret = av8100_conf_prep(AV8100_COMMAND_HDMI, &config);
+	if (ret != 0) {
+		pr_err("%s: Setting hdmi_format failed "
+			"(av8100_conf_prep returned %d)!\n",
+			__func__,
+			ret);
+		return -EINVAL;
+	}
+	ret = av8100_conf_w(AV8100_COMMAND_HDMI,
+			NULL,
+			NULL,
+			I2C_INTERFACE);
+	if (ret != 0) {
+		pr_err("%s: Setting hdmi_format failed "
+			"(av8100_conf_w returned %d)!\n",
+			__func__,
+			ret);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int av8100_codec_pcm_prepare(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *dai)
+{
+	pr_debug("%s: Enter.\n", __func__);
+
+	return 0;
+}
+
+static int av8100_codec_send_audio_infoframe(struct hdmi_audio_settings *as)
+{
+	union av8100_configuration config;
+	struct av8100_infoframes_format_cmd info_fr;
+	int ret;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	pr_debug("%s: HDMI-settings:\n", __func__);
+	pr_debug("%s:	audio_coding_type = %d\n", __func__, as->audio_coding_type);
+	pr_debug("%s:	audio_channel_count = %d\n", __func__, as->audio_channel_count);
+	pr_debug("%s:	sampling_frequency = %d\n", __func__, as->sampling_frequency);
+	pr_debug("%s:	sample_size = %d\n", __func__, as->sample_size);
+	pr_debug("%s:	channel_allocation = %d\n", __func__, as->channel_allocation);
+	pr_debug("%s:	level_shift_value = %d\n", __func__, as->level_shift_value);
+	pr_debug("%s:	downmix_inhibit = %d\n", __func__, as->downmix_inhibit);
+
+	/* Prepare the infoframe from the hdmi_audio_settings struct */
+	pr_info("%s: Preparing audio info-frame.", __func__);
+	info_fr.type = 0x84;
+	info_fr.version = 0x01;
+	info_fr.length = 0x0a;
+	info_fr.data[0] = (as->audio_coding_type << 4) | as->audio_channel_count;
+	info_fr.data[1] = (as->sampling_frequency << 2) | as->sample_size;
+	info_fr.data[2] = 0;
+	info_fr.data[3] = as->channel_allocation;
+	info_fr.data[4] = ((int)as->downmix_inhibit << 7) |
+			(as->level_shift_value << 3);
+	info_fr.data[5] = 0;
+	info_fr.data[6] = 0;
+	info_fr.data[7] = 0;
+	info_fr.data[8] = 0;
+	info_fr.data[9] = 0;
+	info_fr.crc = info_fr.version +
+		info_fr.length +
+		info_fr.data[0] +
+		info_fr.data[1] +
+		info_fr.data[3] +
+		info_fr.data[4];
+	config.infoframes_format.type = info_fr.type;
+	config.infoframes_format.version = info_fr.version;
+	config.infoframes_format.crc = info_fr.crc;
+	config.infoframes_format.length = info_fr.length;
+	memcpy(&config.infoframes_format.data, info_fr.data, info_fr.length);
+
+	/* Send audio info-frame */
+	pr_info("%s: Sending audio info-frame.", __func__);
+	ret = av8100_conf_prep(AV8100_COMMAND_INFOFRAMES, &config);
+	if (ret != 0) {
+		pr_err("%s: Sending audio info-frame failed "
+			"(av8100_conf_prep returned %d)!\n",
+			__func__,
+			ret);
+		return -EINVAL;
+	}
+	ret = av8100_conf_w(AV8100_COMMAND_INFOFRAMES,
+			NULL,
+			NULL,
+			I2C_INTERFACE);
+	if (ret != 0) {
+		pr_err("%s: Sending audio info-frame failed "
+			"(av8100_conf_w returned %d)!\n",
+			__func__,
+			ret);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int av8100_codec_pcm_hw_params(struct snd_pcm_substream *substream,
+				struct snd_pcm_hw_params *hw_params,
+				struct snd_soc_dai *codec_dai)
+{
+	struct av8100_codec_dai_data *dai_data = get_dai_data(codec_dai);
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	av8100_codec_send_audio_infoframe(&dai_data->as);
+
+	return 0;
+}
+
+static int av8100_codec_pcm_startup(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *codec_dai)
+{
+	int ret;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	/* Startup AV8100 if it is not already started */
+	ret = av8100_codec_powerup();
+	if (ret != 0) {
+		pr_err("%s: Startup of AV8100 failed "
+			"(av8100_codec_powerupAV8100 returned %d)!\n",
+			__func__,
+			ret);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void av8100_codec_pcm_shutdown(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *codec_dai)
+{
+	pr_debug("%s: Enter.\n", __func__);
+}
+
+static int av8100_codec_set_dai_sysclk(struct snd_soc_dai *codec_dai,
+				int clk_id,
+				unsigned int freq, int dir)
+{
+	pr_debug("%s: Enter.\n", __func__);
+
+	return 0;
+}
+
+static int av8100_codec_set_dai_fmt(struct snd_soc_dai *codec_dai,
+				unsigned int fmt)
+{
+	union av8100_configuration config;
+	int ret;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	/* Set the HDMI format of AV8100 */
+	ret = av8100_codec_setup_hdmi_format();
+	if (ret != 0)
+		return ret;
+
+	/* Set the audio input format of AV8100 */
+	config.audio_input_format.audio_input_if_format	=
+		((fmt & SND_SOC_DAIFMT_FORMAT_MASK) == SND_SOC_DAIFMT_DSP_B) ?
+		AV8100_AUDIO_TDM_MODE : AV8100_AUDIO_I2SDELAYED_MODE;
+	config.audio_input_format.audio_if_mode	=
+		((fmt & SND_SOC_DAIFMT_MASTER_MASK) == SND_SOC_DAIFMT_CBM_CFM) ?
+		AV8100_AUDIO_MASTER : AV8100_AUDIO_SLAVE;
+	pr_info("%s: Setting audio_input_format "
+		"(if_format = %d, if_mode = %d).",
+		__func__,
+		config.audio_input_format.audio_input_if_format,
+		config.audio_input_format.audio_if_mode);
+	config.audio_input_format.i2s_input_nb = 1;
+	config.audio_input_format.sample_audio_freq = AV8100_AUDIO_FREQ_48KHZ;
+	config.audio_input_format.audio_word_lg	= AV8100_AUDIO_16BITS;
+	config.audio_input_format.audio_format = AV8100_AUDIO_LPCM_MODE;
+	config.audio_input_format.audio_mute = AV8100_AUDIO_MUTE_DISABLE;
+	ret = av8100_conf_prep(AV8100_COMMAND_AUDIO_INPUT_FORMAT, &config);
+	if (ret != 0) {
+		pr_err("%s: Setting audio_input_format failed "
+			"(av8100_conf_prep returned %d)!\n",
+			__func__,
+			ret);
+		return -EINVAL;
+	}
+	ret = av8100_conf_w(AV8100_COMMAND_AUDIO_INPUT_FORMAT,
+			NULL,
+			NULL,
+			I2C_INTERFACE);
+	if (ret != 0) {
+		pr_err("%s: Setting audio_input_format failed "
+			"(av8100_conf_w returned %d)!\n",
+			__func__,
+			ret);
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+struct snd_soc_dai_driver av8100_dai_driver = {
+		.name = "av8100-codec-dai",
+		.playback = {
+			.stream_name = "AV8100 Playback",
+			.channels_min = 1,
+			.channels_max = 8,
+			.rates = AV8100_SUPPORTED_RATE,
+			.formats = AV8100_SUPPORTED_FMT,
+		},
+		.ops = (struct snd_soc_dai_ops[]) {
+			{
+			.prepare = av8100_codec_pcm_prepare,
+			.hw_params = av8100_codec_pcm_hw_params,
+			.startup = av8100_codec_pcm_startup,
+			.shutdown = av8100_codec_pcm_shutdown,
+			.set_sysclk = av8100_codec_set_dai_sysclk,
+			.set_fmt = av8100_codec_set_dai_fmt,
+			}
+		},
+};
+EXPORT_SYMBOL_GPL(av8100_dai_driver);
+
+static int av8100_codec_probe(struct snd_soc_codec *codec)
+{
+	pr_debug("%s: Enter (codec->name = %s).\n", __func__, codec->name);
+
+	return 0;
+}
+
+static int av8100_codec_remove(struct snd_soc_codec *codec)
+{
+	pr_debug("%s: Enter (codec->name = %s).\n", __func__, codec->name);
+
+	return 0;
+}
+
+static int av8100_codec_suspend(struct snd_soc_codec *codec, pm_message_t state)
+{
+	pr_debug("%s: Enter (codec->name = %s).\n", __func__, codec->name);
+
+	return 0;
+}
+
+static int av8100_codec_resume(struct snd_soc_codec *codec)
+{
+	pr_debug("%s: Enter (codec->name = %s).\n", __func__, codec->name);
+
+	return 0;
+}
+
+struct snd_soc_codec_driver av8100_codec_drv = {
+	.probe = av8100_codec_probe,
+	.remove = av8100_codec_remove,
+	.suspend = av8100_codec_suspend,
+	.resume = av8100_codec_resume
+};
+
+static __devinit int av8100_codec_drv_probe(struct platform_device *pdev)
+{
+	struct av8100_codec_dai_data *dai_data;
+	int ret;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	pr_info("%s: Init codec private data..\n", __func__);
+	dai_data = kzalloc(sizeof(struct av8100_codec_dai_data), GFP_KERNEL);
+	if (dai_data == NULL)
+		return -ENOMEM;
+
+	/* Setup hdmi_audio_settings default values */
+	dai_data[0].as.audio_coding_type = AV8100_CODEC_CT_IEC60958_PCM;
+	dai_data[0].as.audio_channel_count = AV8100_CODEC_CC_2CH;
+	dai_data[0].as.sampling_frequency = AV8100_CODEC_SF_48KHZ;
+	dai_data[0].as.sample_size = AV8100_CODEC_SS_16BIT;
+	dai_data[0].as.channel_allocation = AV8100_CODEC_CA_FL_FR;
+	dai_data[0].as.level_shift_value = AV8100_CODEC_LSV_0DB;
+	dai_data[0].as.downmix_inhibit = false;
+
+	platform_set_drvdata(pdev, dai_data);
+
+	pr_info("%s: Register codec.\n", __func__);
+	ret = snd_soc_register_codec(&pdev->dev, &av8100_codec_drv, &av8100_dai_driver, 1);
+	if (ret < 0) {
+		pr_debug("%s: Error: Failed to register codec (ret = %d).\n",
+			__func__,
+			ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __devexit av8100_codec_drv_remove(struct platform_device *pdev)
+{
+	snd_soc_unregister_codec(&pdev->dev);
+	kfree(platform_get_drvdata(pdev));
+	return 0;
+}
+
+static const struct platform_device_id av8100_codec_platform_id[] = {
+	{ "av8100-codec", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, av8100_codec_platform_id);
+
+static struct platform_driver av8100_codec_platform_driver = {
+	.driver = {
+		.name = "av8100-codec",
+		.owner = THIS_MODULE,
+	},
+	.probe = av8100_codec_drv_probe,
+	.remove = __devexit_p(av8100_codec_drv_remove),
+	.id_table = av8100_codec_platform_id,
+};
+
+static int __devinit av8100_codec_platform_drv_init(void)
+{
+	int ret;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	ret = platform_driver_register(&av8100_codec_platform_driver);
+	if (ret != 0) {
+		pr_err("Failed to register AV8100 platform driver (%d)!\n", ret);
+	}
+
+	return ret;
+}
+
+static void __exit av8100_codec_platform_drv_exit(void)
+{
+	pr_debug("%s: Enter.\n", __func__);
+
+	platform_driver_unregister(&av8100_codec_platform_driver);
+}
+
+module_init(av8100_codec_platform_drv_init);
+module_exit(av8100_codec_platform_drv_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/sound/soc/codecs/av8100_audio.h b/sound/soc/codecs/av8100_audio.h
new file mode 100644
index 00000000000..594d66e97ee
--- /dev/null
+++ b/sound/soc/codecs/av8100_audio.h
@@ -0,0 +1,162 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>,
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 AV8100_AUDIO_CODEC_H
+#define AV8100_AUDIO_CODEC_H
+
+/* Supported sampling rates */
+#define AV8100_SUPPORTED_RATE (SNDRV_PCM_RATE_48000)
+
+/* Supported data formats */
+#define AV8100_SUPPORTED_FMT (SNDRV_PCM_FMTBIT_S16_LE)
+
+/* TDM-slot mask */
+#define AV8100_CODEC_MASK_MONO		0x0001
+#define AV8100_CODEC_MASK_STEREO	0x0005
+#define AV8100_CODEC_MASK_2DOT1		0x0015
+#define AV8100_CODEC_MASK_QUAD		0x0505
+#define AV8100_CODEC_MASK_5DOT0		0x0545
+#define AV8100_CODEC_MASK_5DOT1		0x0555
+#define AV8100_CODEC_MASK_7DOT0		0x5545
+#define AV8100_CODEC_MASK_7DOT1		0x5555
+
+enum hdmi_audio_coding_type {
+	AV8100_CODEC_CT_REFER,
+	AV8100_CODEC_CT_IEC60958_PCM,
+	AV8100_CODEC_CT_AC3,
+	AV8100_CODEC_CT_MP3,
+	AV8100_CODEC_CT_MPEG2,
+	AV8100_CODEC_CT_AAC,
+	AV8100_CODEC_CT_DTS_ATRAC,
+	AV8100_CODEC_CT_ONE_BIT_AUDIO,
+	AV8100_CODEC_CT_DOLBY_DIGITAL,
+	AV8100_CODEC_CT_DTS_HD,
+	AV8100_CODEC_CT_MAT,
+	AV8100_CODEC_CT_DTS,
+	AV8100_CODEC_CT_WMA_PRO
+};
+
+enum hdmi_audio_channel_count {
+	AV8100_CODEC_CC_REFER,
+	AV8100_CODEC_CC_2CH,
+	AV8100_CODEC_CC_3CH,
+	AV8100_CODEC_CC_4CH,
+	AV8100_CODEC_CC_5CH,
+	AV8100_CODEC_CC_6CH,
+	AV8100_CODEC_CC_7CH,
+	AV8100_CODEC_CC_8CH
+};
+
+enum hdmi_sampling_frequency {
+	AV8100_CODEC_SF_REFER,
+	AV8100_CODEC_SF_32KHZ,
+	AV8100_CODEC_SF_44_1KHZ,
+	AV8100_CODEC_SF_48KHZ,
+	AV8100_CODEC_SF_88_2KHZ,
+	AV8100_CODEC_SF_96KHZ,
+	AV8100_CODEC_SF_176_4KHZ,
+	AV8100_CODEC_SF_192KHZ
+};
+
+enum hdmi_sample_size {
+	AV8100_CODEC_SS_REFER,
+	AV8100_CODEC_SS_16BIT,
+	AV8100_CODEC_SS_20BIT,
+	AV8100_CODEC_SS_24BIT
+};
+
+enum hdmi_speaker_placement {
+	AV8100_CODEC_SP_FL,	/* Front Left */
+	AV8100_CODEC_SP_FC,	/* Front Center */
+	AV8100_CODEC_SP_FR,	/* Front Right */
+	AV8100_CODEC_SP_FLC,	/* Front Left Center */
+	AV8100_CODEC_SP_FRC,	/* Front Right Center */
+	AV8100_CODEC_SP_RL,	/* Rear Left */
+	AV8100_CODEC_SP_RC,	/* Rear Center */
+	AV8100_CODEC_SP_RR,	/* Rear Right */
+	AV8100_CODEC_SP_RLC,	/* Rear Left Center */
+	AV8100_CODEC_SP_RRC,	/* Rear Right Center */
+	AV8100_CODEC_SP_LFE,	/* Low Frequency Effekt */
+};
+
+enum hdmi_channel_allocation {
+	AV8100_CODEC_CA_FL_FR,				/* 0x00, Stereo */
+	AV8100_CODEC_CA_FL_FR_LFE,			/* 0x01, 2.1 */
+	AV8100_CODEC_CA_FL_FR_FC,			/* 0x02*/
+	AV8100_CODEC_CA_FL_FR_LFE_FC,			/* 0x03*/
+	AV8100_CODEC_CA_FL_FR_RC,			/* 0x04*/
+	AV8100_CODEC_CA_FL_FR_LFE_RC,			/* 0x05*/
+	AV8100_CODEC_CA_FL_FR_FC_RC,			/* 0x06*/
+	AV8100_CODEC_CA_FL_FR_LFE_FC_RC,		/* 0x07*/
+	AV8100_CODEC_CA_FL_FR_RL_RR,			/* 0x08, Quad */
+	AV8100_CODEC_CA_FL_FR_LFE_RL_RR,		/* 0x09*/
+	AV8100_CODEC_CA_FL_FR_FC_RL_RR,			/* 0x0a, 5.0*/
+	AV8100_CODEC_CA_FL_FR_LFE_FC_RL_RR,		/* 0x0b, 5.1*/
+	AV8100_CODEC_CA_FL_FR_RL_RR_RC,			/* 0x0c*/
+	AV8100_CODEC_CA_FL_FR_LFE_RL_RR_RC,		/* 0x0d*/
+	AV8100_CODEC_CA_FL_FR_RC_RL_RR_RC,		/* 0x0e*/
+	AV8100_CODEC_CA_FL_FR_LFE_RC_RL_RR_RC,		/* 0x0f*/
+	AV8100_CODEC_CA_FL_FR_RL_RR_RLC_RRC,		/* 0x10*/
+	AV8100_CODEC_CA_FL_FR_LFE_RL_RR_RLC_RRC,	/* 0x11*/
+	AV8100_CODEC_CA_FL_FR_FC_RL_RR_RLC_RRC,		/* 0x12*/
+	AV8100_CODEC_CA_FL_FR_LFE_FC_RL_RR_RLC_RRC,	/* 0x13*/
+	AV8100_CODEC_CA_FL_FR_FLC_FRC,			/* 0x14*/
+	AV8100_CODEC_CA_FL_FR_LFE_FLC_FRC,		/* 0x15*/
+	AV8100_CODEC_CA_FL_FR_FC_FLC_FRC,		/* 0x16*/
+	AV8100_CODEC_CA_FL_FR_LFE_FC_FLC_FRC,		/* 0x17*/
+	AV8100_CODEC_CA_FL_FR_RC_FLC_FRC,		/* 0x18*/
+	AV8100_CODEC_CA_FL_FR_LFE_RC_FLC_FRC,		/* 0x19*/
+	AV8100_CODEC_CA_FL_FR_FC_RC_FLC_FRC,		/* 0x1a*/
+	AV8100_CODEC_CA_FL_FR_LFE_FR_FC_RC_FLC_FRC,	/* 0x1b*/
+	AV8100_CODEC_CA_FL_FR_RL_RR_FLC_FRC,		/* 0x1c*/
+	AV8100_CODEC_CA_FL_FR_LFE_RL_RR_FLC_FRC,	/* 0x1d*/
+	AV8100_CODEC_CA_FL_FR_FC_RL_RR_FLC_FRC,		/* 0x1e*/
+	AV8100_CODEC_CA_FL_FR_LFE_FC_RL_RR_FLC_FRC	/* 0x1f, 7.1 */
+};
+
+enum hdmi_level_shift_value {
+	AV8100_CODEC_LSV_0DB,
+	AV8100_CODEC_LSV_1DB,
+	AV8100_CODEC_LSV_2DB,
+	AV8100_CODEC_LSV_3DB,
+	AV8100_CODEC_LSV_4DB,
+	AV8100_CODEC_LSV_5DB,
+	AV8100_CODEC_LSV_6DB,
+	AV8100_CODEC_LSV_7DB,
+	AV8100_CODEC_LSV_8DB,
+	AV8100_CODEC_LSV_9DB,
+	AV8100_CODEC_LSV_10DB,
+	AV8100_CODEC_LSV_11DB,
+	AV8100_CODEC_LSV_12DB,
+	AV8100_CODEC_LSV_13DB,
+	AV8100_CODEC_LSV_14DB,
+	AV8100_CODEC_LSV_15DB
+};
+
+struct hdmi_audio_settings {
+	enum hdmi_audio_coding_type	audio_coding_type;
+	enum hdmi_audio_channel_count	audio_channel_count;
+	enum hdmi_sampling_frequency	sampling_frequency;
+	enum hdmi_sample_size		sample_size;
+	enum hdmi_channel_allocation	channel_allocation;
+	enum hdmi_level_shift_value	level_shift_value;
+	bool				downmix_inhibit;
+};
+
+/* Extended interface for codec-driver */
+int av8100_audio_change_hdmi_audio_settings(struct snd_soc_dai *dai,
+					struct hdmi_audio_settings *as);
+
+#endif /* AV8100_AUDIO_CODEC_H */
+
+
+
diff --git a/sound/soc/codecs/cg29xx.c b/sound/soc/codecs/cg29xx.c
new file mode 100644
index 00000000000..1ee3f3c38af
--- /dev/null
+++ b/sound/soc/codecs/cg29xx.c
@@ -0,0 +1,773 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Roger Nilsson <roger.xr.nilsson@stericsson.com>,
+ *         Ola Lilja <ola.o.lilja@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/pm.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <sound/initval.h>
+#include <sound/soc.h>
+#include <sound/soc-dapm.h>
+#include <linux/bitops.h>
+#include <../../../drivers/staging/cg2900/include/cg2900_audio.h>
+
+#include "cg29xx.h"
+
+#define CG29XX_NBR_OF_DAI	2
+#define CG29XX_SUPPORTED_RATE_PCM (SNDRV_PCM_RATE_8000 | \
+	SNDRV_PCM_RATE_16000)
+
+#define CG29XX_SUPPORTED_RATE (SNDRV_PCM_RATE_8000 | \
+	SNDRV_PCM_RATE_16000 | SNDRV_PCM_RATE_44100 | SNDRV_PCM_RATE_48000)
+
+#define CG29XX_SUPPORTED_FMT (SNDRV_PCM_FMTBIT_S16_LE)
+
+enum cg29xx_dai_direction {
+	CG29XX_DAI_DIRECTION_TX,
+	CG29XX_DAI_DIRECTION_RX
+};
+
+static int cg29xx_dai_startup(
+	struct snd_pcm_substream *substream,
+	struct snd_soc_dai *dai);
+
+static int cg29xx_dai_prepare(
+	struct snd_pcm_substream *substream,
+	struct snd_soc_dai *dai);
+
+static int cg29xx_dai_hw_params(
+	struct snd_pcm_substream *substream,
+	struct snd_pcm_hw_params *hw_params,
+	struct snd_soc_dai *dai);
+
+static void cg29xx_dai_shutdown(
+	struct snd_pcm_substream *substream,
+	struct snd_soc_dai *dai);
+
+static int cg29xx_set_dai_sysclk(
+	struct snd_soc_dai *codec_dai,
+	int clk_id,
+	unsigned int freq, int dir);
+
+static int cg29xx_set_dai_fmt(
+	struct snd_soc_dai *codec_dai,
+	unsigned int fmt);
+
+static int cg29xx_set_tdm_slot(
+	struct snd_soc_dai *dai,
+	unsigned int tx_mask, unsigned int rx_mask,
+	int slots,
+	int slot_width);
+
+static struct cg29xx_codec codec_private = {
+	.session = 0,
+};
+
+static struct snd_soc_dai_ops cg29xx_dai_driver_dai_ops = {
+	.startup = cg29xx_dai_startup,
+	.prepare = cg29xx_dai_prepare,
+	.hw_params = cg29xx_dai_hw_params,
+	.shutdown = cg29xx_dai_shutdown,
+	.set_sysclk = cg29xx_set_dai_sysclk,
+	.set_fmt = cg29xx_set_dai_fmt,
+	.set_tdm_slot = cg29xx_set_tdm_slot
+};
+
+struct snd_soc_dai_driver cg29xx_dai_driver[] = {
+	{
+	.name = "cg29xx-codec-dai.0",
+	.id = 0,
+	.playback = {
+		.stream_name = "CG29xx.0 Playback",
+		.channels_min = 2,
+		.channels_max = 2,
+		.rates = CG29XX_SUPPORTED_RATE,
+		.formats = CG29XX_SUPPORTED_FMT,
+	},
+	.capture = {
+		.stream_name = "CG29xx.0 Capture",
+		.channels_min = 2,
+		.channels_max = 2,
+		.rates = CG29XX_SUPPORTED_RATE,
+		.formats = CG29XX_SUPPORTED_FMT,
+	},
+	.ops = &cg29xx_dai_driver_dai_ops,
+	.symmetric_rates = 1,
+	},
+	{
+	.name = "cg29xx-codec-dai.1",
+	.id = 1,
+	.playback = {
+		.stream_name = "CG29xx.1 Playback",
+		.channels_min = 1,
+		.channels_max = 2,
+		.rates = CG29XX_SUPPORTED_RATE_PCM,
+		.formats = CG29XX_SUPPORTED_FMT,
+	},
+	.capture = {
+		.stream_name = "CG29xx.1 Capture",
+		.channels_min = 1,
+		.channels_max = 2,
+		.rates = CG29XX_SUPPORTED_RATE_PCM,
+		.formats = CG29XX_SUPPORTED_FMT,
+	},
+	.ops = &cg29xx_dai_driver_dai_ops,
+	.symmetric_rates = 1,
+	}
+};
+EXPORT_SYMBOL_GPL(cg29xx_dai_driver);
+
+static const char *enum_ifs_input_select[] = {
+	"BT_SCO", "FM_RX"
+};
+
+static const char *enum_ifs_output_select[] = {
+	"BT_SCO", "FM_TX"
+};
+
+/* If0 Input Select */
+static struct soc_enum if0_input_select =
+	SOC_ENUM_SINGLE(INTERFACE0_INPUT_SELECT, 0,
+		ARRAY_SIZE(enum_ifs_input_select),
+		enum_ifs_input_select);
+
+/* If1 Input Select */
+static struct soc_enum if1_input_select =
+	SOC_ENUM_SINGLE(INTERFACE1_INPUT_SELECT, 0,
+		ARRAY_SIZE(enum_ifs_input_select),
+		enum_ifs_input_select);
+
+/* If0 Output Select */
+static struct soc_enum if0_output_select =
+	SOC_ENUM_SINGLE(INTERFACE0_OUTPUT_SELECT, 0,
+		ARRAY_SIZE(enum_ifs_output_select),
+		enum_ifs_output_select);
+
+/* If1 Output Select */
+static struct soc_enum if1_output_select =
+	SOC_ENUM_SINGLE(INTERFACE1_OUTPUT_SELECT, 4,
+		ARRAY_SIZE(enum_ifs_output_select),
+		enum_ifs_output_select);
+
+static struct snd_kcontrol_new cg29xx_snd_controls[] = {
+	SOC_ENUM("If0 Input Select",	if0_input_select),
+	SOC_ENUM("If1 Input Select",	if1_input_select),
+	SOC_ENUM("If0 Output Select",	if0_output_select),
+	SOC_ENUM("If1 Output Select",	if1_output_select),
+};
+
+
+static struct cg29xx_codec_dai_data *get_dai_data_codec(struct snd_soc_codec *codec,
+						int dai_id)
+{
+	struct cg29xx_codec_dai_data *codec_drvdata = snd_soc_codec_get_drvdata(codec);
+	return &codec_drvdata[dai_id];
+}
+
+static struct cg29xx_codec_dai_data *get_dai_data(struct snd_soc_dai *codec_dai)
+{
+	return get_dai_data_codec(codec_dai->codec, codec_dai->id);
+}
+
+static int cg29xx_set_dai_sysclk(struct snd_soc_dai *codec_dai,
+				int clk_id,
+				unsigned int freq, int dir)
+{
+	return 0;
+}
+
+static int cg29xx_set_dai_fmt(struct snd_soc_dai *codec_dai,
+			unsigned int fmt)
+{
+	struct cg29xx_codec_dai_data *dai_data = get_dai_data(codec_dai);
+	unsigned int prot;
+	unsigned int msel;
+
+	prot = fmt & SND_SOC_DAIFMT_FORMAT_MASK;
+	msel = fmt & SND_SOC_DAIFMT_MASTER_MASK;
+
+	switch (prot) {
+	case SND_SOC_DAIFMT_I2S:
+		if (dai_data->config.port != PORT_0_I2S) {
+			pr_err("cg29xx_dai: unsupported DAI format 0x%x\n",
+				fmt);
+			return -EINVAL;
+		}
+
+		if (msel == SND_SOC_DAIFMT_CBM_CFM)
+			dai_data->config.conf.i2s.mode = DAI_MODE_MASTER;
+		else
+			dai_data->config.conf.i2s.mode = DAI_MODE_SLAVE;
+		break;
+
+	case SND_SOC_DAIFMT_DSP_A:
+		if (dai_data->config.port != PORT_1_I2S_PCM ||
+			msel == SND_SOC_DAIFMT_CBM_CFM) {
+			pr_err("cg29xx_dai: unsupported DAI format 0x%x\n",
+				fmt);
+			return -EINVAL;
+		}
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int cg29xx_set_tdm_slot(struct snd_soc_dai *codec_dai,
+			unsigned int tx_mask,
+			unsigned int rx_mask,
+			int slots,
+			int slot_width)
+{
+	struct cg29xx_codec_dai_data *dai_data = get_dai_data(codec_dai);
+
+	if (dai_data->config.port != PORT_1_I2S_PCM)
+		return -EINVAL;
+
+	dai_data->config.conf.i2s_pcm.slot_0_used =
+		(tx_mask | rx_mask) & (1<<CG29XX_DAI_SLOT0_SHIFT) ?
+		true : false;
+	dai_data->config.conf.i2s_pcm.slot_1_used =
+		(tx_mask | rx_mask) & (1<<CG29XX_DAI_SLOT1_SHIFT) ?
+		true : false;
+	dai_data->config.conf.i2s_pcm.slot_2_used =
+		(tx_mask | rx_mask) & (1<<CG29XX_DAI_SLOT2_SHIFT) ?
+		true : false;
+	dai_data->config.conf.i2s_pcm.slot_3_used =
+		(tx_mask | rx_mask) & (1<<CG29XX_DAI_SLOT3_SHIFT) ?
+		true : false;
+
+	dai_data->config.conf.i2s_pcm.slot_0_start = 0;
+	dai_data->config.conf.i2s_pcm.slot_1_start = slot_width;
+	dai_data->config.conf.i2s_pcm.slot_2_start = 2 * slot_width;
+	dai_data->config.conf.i2s_pcm.slot_3_start = 3 * slot_width;
+
+	return 0;
+}
+
+static int cg29xx_configure_endp(struct cg29xx_codec_dai_data *dai_data,
+				enum cg2900_audio_endpoint_id endpid)
+{
+	struct cg2900_endpoint_config config;
+	int err;
+	enum cg2900_dai_sample_rate dai_sr;
+	enum cg2900_endpoint_sample_rate endp_sr;
+
+	switch (dai_data->config.port) {
+	default:
+	case PORT_0_I2S:
+		dai_sr = dai_data->config.conf.i2s.sample_rate;
+		break;
+
+	case PORT_1_I2S_PCM:
+		dai_sr = dai_data->config.conf.i2s_pcm.sample_rate;
+		break;
+	}
+
+	switch (dai_sr) {
+	default:
+	case SAMPLE_RATE_8:
+		endp_sr = ENDPOINT_SAMPLE_RATE_8_KHZ;
+		break;
+	case SAMPLE_RATE_16:
+		endp_sr = ENDPOINT_SAMPLE_RATE_16_KHZ;
+		break;
+	case SAMPLE_RATE_44_1:
+		endp_sr = ENDPOINT_SAMPLE_RATE_44_1_KHZ;
+		break;
+	case SAMPLE_RATE_48:
+		endp_sr = ENDPOINT_SAMPLE_RATE_48_KHZ;
+		break;
+	}
+
+	config.endpoint_id = endpid;
+
+	switch (endpid) {
+	default:
+	case ENDPOINT_BT_SCO_INOUT:
+		config.config.sco.sample_rate = endp_sr;
+		break;
+
+	case ENDPOINT_FM_TX:
+	case ENDPOINT_FM_RX:
+		config.config.fm.sample_rate = endp_sr;
+		break;
+	}
+
+	err = cg2900_audio_config_endpoint(codec_private.session, &config);
+
+	return err;
+}
+
+static int cg29xx_stop_if(struct cg29xx_codec_dai_data *dai_data,
+			enum cg29xx_dai_direction direction)
+{
+	int err = 0;
+	unsigned int *stream;
+
+	if (direction == CG29XX_DAI_DIRECTION_TX)
+		stream = &dai_data->tx_active;
+	else
+		stream = &dai_data->rx_active;
+
+	if (*stream) {
+		err = cg2900_audio_stop_stream(
+			codec_private.session,
+			*stream);
+		if (!err) {
+			*stream = 0;
+		} else {
+			pr_err("asoc cg29xx - %s - Failed to stop stream on interface %d.\n",
+				__func__,
+				dai_data->config.port);
+		}
+	}
+
+	return err;
+}
+
+static int cg29xx_start_if(struct cg29xx_codec_dai_data *dai_data,
+			enum cg29xx_dai_direction direction)
+{
+	enum cg2900_audio_endpoint_id if_endpid;
+	enum cg2900_audio_endpoint_id endpid;
+	unsigned int *stream;
+	int err;
+
+	if (dai_data->config.port == PORT_0_I2S)
+		if_endpid = ENDPOINT_PORT_0_I2S;
+	else
+		if_endpid = ENDPOINT_PORT_1_I2S_PCM;
+
+	if (direction == CG29XX_DAI_DIRECTION_RX) {
+		switch (dai_data->output_select) {
+		default:
+		case 0:
+			endpid = ENDPOINT_BT_SCO_INOUT;
+			break;
+		case 1:
+			endpid = ENDPOINT_FM_TX;
+		}
+		stream = &dai_data->rx_active;
+	} else {
+		switch (dai_data->input_select) {
+		default:
+		case 0:
+			endpid = ENDPOINT_BT_SCO_INOUT;
+			break;
+		case 1:
+			endpid = ENDPOINT_FM_RX;
+		}
+
+		stream = &dai_data->tx_active;
+	}
+
+	if (*stream || (endpid == ENDPOINT_BT_SCO_INOUT)) {
+		pr_debug("asoc cg29xx - %s - The interface has already been started.\n",
+			__func__);
+		return 0;
+	}
+
+	pr_debug("asoc cg29xx - %s - direction: %d, if_id: %d endpid: %d\n",
+			__func__,
+			direction,
+			if_endpid,
+			endpid);
+
+	err = cg29xx_configure_endp(dai_data, endpid);
+
+	if (err) {
+		pr_err("asoc cg29xx - %s - Configure endpoint id: %d failed.\n",
+			__func__,
+			endpid);
+
+		return err;
+	}
+
+	err = cg2900_audio_start_stream(codec_private.session,
+		if_endpid,
+		endpid,
+		stream);
+
+	return err;
+}
+
+static int cg29xx_dai_startup(struct snd_pcm_substream *substream,
+			struct snd_soc_dai *dai)
+{
+	int err = 0;
+
+	if (!codec_private.session)
+		err = cg2900_audio_open(&codec_private.session, NULL);
+
+	return err;
+}
+
+static int cg29xx_dai_prepare(struct snd_pcm_substream *substream,
+			struct snd_soc_dai *codec_dai)
+{
+	struct cg29xx_codec_dai_data *dai_data = get_dai_data(codec_dai);
+	int err = 0;
+	enum cg29xx_dai_direction direction;
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		direction = CG29XX_DAI_DIRECTION_RX;
+	else
+		direction = CG29XX_DAI_DIRECTION_TX;
+
+	err = cg29xx_start_if(dai_data, direction);
+
+	return err;
+}
+
+static void cg29xx_dai_shutdown(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *codec_dai)
+{
+	struct cg29xx_codec_dai_data *dai_data = get_dai_data(codec_dai);
+	enum cg29xx_dai_direction direction;
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		direction = CG29XX_DAI_DIRECTION_RX;
+	else
+		direction = CG29XX_DAI_DIRECTION_TX;
+
+	(void) cg29xx_stop_if(dai_data, direction);
+}
+
+static int cg29xx_dai_hw_params(struct snd_pcm_substream *substream,
+				struct snd_pcm_hw_params *hw_params,
+				struct snd_soc_dai *codec_dai)
+{
+	struct cg29xx_codec_dai_data *dai_data = get_dai_data(codec_dai);
+	enum cg2900_dai_fs_duration duration = SYNC_DURATION_32;
+	enum cg2900_dai_bit_clk bclk = BIT_CLK_512;
+	int sr;
+	int err = 0;
+	enum cg2900_dai_stream_ratio ratio = STREAM_RATIO_FM48_VOICE16;
+
+	pr_debug("cg29xx asoc - %s called. Port: %d.\n",
+		__func__,
+		dai_data->config.port);
+
+	switch (params_rate(hw_params)) {
+	case 8000:
+		sr = SAMPLE_RATE_8;
+		bclk = BIT_CLK_512;
+		duration = SYNC_DURATION_32;
+		ratio = STREAM_RATIO_FM48_VOICE8;
+		break;
+	case 16000:
+		sr = SAMPLE_RATE_16;
+		bclk = BIT_CLK_512;
+		duration = SYNC_DURATION_32;
+		ratio = STREAM_RATIO_FM48_VOICE16;
+		break;
+	case 44100:
+		sr = SAMPLE_RATE_44_1;
+		break;
+	case 48000:
+		sr = SAMPLE_RATE_48;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (dai_data->config.port == PORT_0_I2S) {
+		dai_data->config.conf.i2s.sample_rate = sr;
+	} else {
+		dai_data->config.conf.i2s_pcm.sample_rate = sr;
+		dai_data->config.conf.i2s_pcm.duration = duration;
+		dai_data->config.conf.i2s_pcm.clk = bclk;
+		dai_data->config.conf.i2s_pcm.ratio = ratio;
+	}
+
+	if (!(dai_data->tx_active | dai_data->rx_active) && dai_data->config.port != PORT_1_I2S_PCM) {
+		err = cg2900_audio_set_dai_config(
+			codec_private.session,
+			&dai_data->config);
+
+		pr_debug("asoc cg29xx: cg2900_audio_set_dai_config"
+			"on port %d completed with result: %d.\n",
+			dai_data->config.port,
+			err);
+	}
+
+	return err;
+}
+
+static unsigned int cg29xx_codec_read(struct snd_soc_codec *codec,
+				unsigned int reg)
+{
+	struct cg29xx_codec_dai_data *dai_data;
+
+	switch (reg) {
+	case INTERFACE0_INPUT_SELECT:
+		dai_data = get_dai_data_codec(codec, 0);
+		return dai_data->input_select;
+
+	case INTERFACE1_INPUT_SELECT:
+		dai_data = get_dai_data_codec(codec, 1);
+		return dai_data->input_select;
+
+	case INTERFACE0_OUTPUT_SELECT:
+		dai_data = get_dai_data_codec(codec, 0);
+		return dai_data->output_select;
+
+	case INTERFACE1_OUTPUT_SELECT:
+		dai_data = get_dai_data_codec(codec, 1);
+		return dai_data->output_select;
+
+	default:
+		return 0;
+	}
+
+	return 0;
+}
+
+static int cg29xx_codec_write(struct snd_soc_codec *codec,
+			unsigned int reg,
+			unsigned int value)
+{
+	struct cg29xx_codec_dai_data *dai_data;
+	enum cg29xx_dai_direction direction;
+	bool restart_if = false;
+	int old_value;
+
+	switch (reg) {
+	case INTERFACE0_INPUT_SELECT:
+		dai_data = get_dai_data_codec(codec, 0);
+		direction = CG29XX_DAI_DIRECTION_TX;
+
+		old_value = dai_data->input_select;
+		dai_data->input_select = value;
+
+		if ((old_value ^ value) && dai_data->tx_active)
+			restart_if = true;
+		break;
+
+	case INTERFACE1_INPUT_SELECT:
+		dai_data = get_dai_data_codec(codec, 1);
+		direction = CG29XX_DAI_DIRECTION_TX;
+
+		old_value = dai_data->input_select;
+		dai_data->input_select = value;
+
+		if ((old_value ^ value) && dai_data->tx_active)
+			restart_if = true;
+		break;
+
+	case INTERFACE0_OUTPUT_SELECT:
+		dai_data = get_dai_data_codec(codec, 0);
+		direction = CG29XX_DAI_DIRECTION_RX;
+
+		old_value = dai_data->output_select;
+		dai_data->output_select = value;
+
+		if ((old_value ^ value) && dai_data->rx_active)
+			restart_if = true;
+		break;
+
+	case INTERFACE1_OUTPUT_SELECT:
+		dai_data = get_dai_data_codec(codec, 1);
+		direction = CG29XX_DAI_DIRECTION_RX;
+
+		old_value = dai_data->output_select;
+		dai_data->output_select = value;
+
+		if ((old_value ^ value) && dai_data->rx_active)
+			restart_if = true;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	if (restart_if) {
+		(void) cg29xx_stop_if(dai_data, direction);
+		(void) cg29xx_start_if(dai_data, direction);
+	}
+
+	return 0;
+}
+
+static int cg29xx_codec_probe(struct snd_soc_codec *codec)
+{
+	pr_debug("%s: Enter (codec->name = %s).\n", __func__, codec->name);
+
+	snd_soc_add_controls(
+		codec,
+		cg29xx_snd_controls,
+		ARRAY_SIZE(cg29xx_snd_controls));
+
+	return 0;
+}
+
+static int cg29xx_codec_remove(struct snd_soc_codec *codec)
+{
+	pr_debug("%s: Enter (codec->name = %s).\n", __func__, codec->name);
+
+	return 0;
+}
+
+static int cg29xx_codec_suspend(struct snd_soc_codec *codec, pm_message_t state)
+{
+	pr_debug("%s: Enter (codec->name = %s).\n", __func__, codec->name);
+
+	return 0;
+}
+
+static int cg29xx_codec_resume(struct snd_soc_codec *codec)
+{
+	pr_debug("%s: Enter (codec->name = %s).\n", __func__, codec->name);
+
+	return 0;
+}
+
+struct snd_soc_codec_driver cg29xx_codec_driver = {
+	.probe = cg29xx_codec_probe,
+	.remove = cg29xx_codec_remove,
+	.suspend = cg29xx_codec_suspend,
+	.resume = cg29xx_codec_resume,
+	.read = cg29xx_codec_read,
+	.write = cg29xx_codec_write,
+};
+
+static int __devinit cg29xx_codec_driver_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct cg29xx_codec_dai_data *dai_data;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	pr_info("%s: Init codec private data..\n", __func__);
+	dai_data = kzalloc(CG29XX_NBR_OF_DAI * sizeof(struct cg29xx_codec_dai_data),
+			GFP_KERNEL);
+	if (dai_data == NULL)
+		return -ENOMEM;
+
+	dai_data[0].tx_active = 0;
+	dai_data[0].rx_active = 0;
+	dai_data[0].input_select = 0;
+	dai_data[0].output_select = 0;
+	dai_data[0].config.port = PORT_0_I2S;
+	dai_data[0].config.conf.i2s.mode = DAI_MODE_SLAVE;
+	dai_data[0].config.conf.i2s.half_period = HALF_PER_DUR_16;
+	dai_data[0].config.conf.i2s.channel_sel = CHANNEL_SELECTION_BOTH;
+	dai_data[0].config.conf.i2s.sample_rate = SAMPLE_RATE_48;
+	dai_data[0].config.conf.i2s.word_width = WORD_WIDTH_32;
+	dai_data[1].tx_active = 0;
+	dai_data[1].rx_active = 0;
+	dai_data[1].input_select = 0;
+	dai_data[1].output_select = 0;
+	dai_data[1].config.port = PORT_1_I2S_PCM;
+	dai_data[1].config.conf.i2s_pcm.mode = DAI_MODE_SLAVE;
+	dai_data[1].config.conf.i2s_pcm.slot_0_dir = DAI_DIR_B_RX_A_TX;
+	dai_data[1].config.conf.i2s_pcm.slot_1_dir = DAI_DIR_B_TX_A_RX;
+	dai_data[1].config.conf.i2s_pcm.slot_2_dir = DAI_DIR_B_RX_A_TX;
+	dai_data[1].config.conf.i2s_pcm.slot_3_dir = DAI_DIR_B_RX_A_TX;
+	dai_data[1].config.conf.i2s_pcm.slot_0_used = true;
+	dai_data[1].config.conf.i2s_pcm.slot_1_used = false;
+	dai_data[1].config.conf.i2s_pcm.slot_2_used = false;
+	dai_data[1].config.conf.i2s_pcm.slot_3_used = false;
+	dai_data[1].config.conf.i2s_pcm.slot_0_start = 0;
+	dai_data[1].config.conf.i2s_pcm.slot_1_start = 16;
+	dai_data[1].config.conf.i2s_pcm.slot_2_start = 32;
+	dai_data[1].config.conf.i2s_pcm.slot_3_start = 48;
+	dai_data[1].config.conf.i2s_pcm.protocol = PORT_PROTOCOL_PCM;
+	dai_data[1].config.conf.i2s_pcm.ratio = STREAM_RATIO_FM48_VOICE16;
+	dai_data[1].config.conf.i2s_pcm.duration = SYNC_DURATION_32;
+	dai_data[1].config.conf.i2s_pcm.clk = BIT_CLK_512;
+	dai_data[1].config.conf.i2s_pcm.sample_rate = SAMPLE_RATE_16;
+
+	platform_set_drvdata(pdev, dai_data);
+
+	pr_info("%s: Register codec.\n", __func__);
+	ret = snd_soc_register_codec(&pdev->dev, &cg29xx_codec_driver, &cg29xx_dai_driver[0], 2);
+	if (ret < 0) {
+		pr_debug("%s: Error: Failed to register codec (ret = %d).\n",
+			__func__,
+			ret);
+		snd_soc_unregister_codec(&pdev->dev);
+		kfree(platform_get_drvdata(pdev));
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __devexit cg29xx_codec_driver_remove(struct platform_device *pdev)
+{
+	(void)cg2900_audio_close(&codec_private.session);
+
+	snd_soc_unregister_codec(&pdev->dev);
+	kfree(platform_get_drvdata(pdev));
+
+	return 0;
+}
+
+static int cg29xx_codec_driver_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	return 0;
+}
+
+static int cg29xx_codec_driver_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver cg29xx_codec_platform_driver = {
+	.driver = {
+		.name = "cg29xx-codec",
+		.owner = THIS_MODULE,
+	},
+	.probe = cg29xx_codec_driver_probe,
+	.remove = __devexit_p(cg29xx_codec_driver_remove),
+	.suspend = cg29xx_codec_driver_suspend,
+	.resume = cg29xx_codec_driver_resume,
+};
+
+
+static int __devinit cg29xx_codec_platform_driver_init(void)
+{
+	int ret;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	ret = platform_driver_register(&cg29xx_codec_platform_driver);
+	if (ret != 0)
+		pr_err("Failed to register CG29xx platform driver (%d)!\n", ret);
+
+	return ret;
+}
+
+static void __exit cg29xx_codec_platform_driver_exit(void)
+{
+	pr_debug("%s: Enter.\n", __func__);
+
+	platform_driver_unregister(&cg29xx_codec_platform_driver);
+}
+
+
+module_init(cg29xx_codec_platform_driver_init);
+module_exit(cg29xx_codec_platform_driver_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/sound/soc/codecs/cg29xx.h b/sound/soc/codecs/cg29xx.h
new file mode 100644
index 00000000000..fec52d7cdd7
--- /dev/null
+++ b/sound/soc/codecs/cg29xx.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Roger Nilsson roger.xr.nilsson@stericsson.com
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 CG29XX_CODEC_H
+#define CG29XX_CODEC_H
+
+#include <../../../drivers/staging/cg2900/include/cg2900_audio.h>
+
+struct cg29xx_codec_dai_data {
+	struct mutex mutex;
+	unsigned int rx_active;
+	unsigned int tx_active;
+	int input_select;
+	int output_select;
+	struct cg2900_dai_config config;
+};
+
+struct cg29xx_codec{
+	unsigned int session;
+};
+
+#define CG29XX_DAI_SLOT0_SHIFT	0
+#define CG29XX_DAI_SLOT1_SHIFT	1
+#define CG29XX_DAI_SLOT2_SHIFT	2
+#define CG29XX_DAI_SLOT3_SHIFT	3
+
+#define INTERFACE0_INPUT_SELECT 0x00
+#define INTERFACE1_INPUT_SELECT 0x01
+#define INTERFACE0_OUTPUT_SELECT 0x02
+#define INTERFACE1_OUTPUT_SELECT 0x03
+
+#endif /* CG29XX_CODEC_H */
diff --git a/sound/soc/ux500/Kconfig b/sound/soc/ux500/Kconfig
new file mode 100644
index 00000000000..02239929879
--- /dev/null
+++ b/sound/soc/ux500/Kconfig
@@ -0,0 +1,57 @@
+#
+# Ux500 SoC audio configuration
+#
+
+menuconfig SND_SOC_UX500
+	bool "SoC Audio support for Ux500 platform"
+	depends on SND_SOC && STM_I2S && STM_MSP_I2S
+	default n
+	help
+		Say Y if you want to add support for the Ux500 platform.
+
+config SND_SOC_UX500_AB3550
+	bool "Codec - AB3550"
+	depends on SND_SOC_UX500 && (UX500_SOC_DB8500 || UX500_SOC_DB5500) && AB3550_CORE
+	select SND_SOC_AB3550
+	default n
+	help
+		Say Y if you want to include the AB3550 codec.
+
+config SND_SOC_UX500_AB5500
+	bool "Codec - AB5500"
+	depends on SND_SOC_UX500 && (UX500_SOC_DB8500 || UX500_SOC_DB5500) && AB5500_CORE
+	select SND_SOC_AB5500
+	default n
+	help
+	        Say Y if you want to include the AB5500 codec.
+
+config SND_SOC_UX500_AB8500
+	bool "Codec - AB8500"
+	depends on SND_SOC_UX500 && UX500_SOC_DB8500 && AB8500_CORE && AB8500_GPADC
+	select SND_SOC_AB8500
+	default n
+	help
+		Say Y if you want to include AB8500 audio codec.
+
+config SND_SOC_UX500_CG29XX
+	bool "Codec - CG29xx"
+	depends on SND_SOC_UX500 && (UX500_SOC_DB8500 || UX500_SOC_DB5500) && CG2900_AUDIO
+	select SND_SOC_CG29XX
+	default n
+	help
+		Say Y if you want to include CG29xx codec (Combo chip).
+
+config SND_SOC_UX500_AV8100
+	bool "Codec - AV8100"
+	depends on SND_SOC_UX500 && (UX500_SOC_DB8500 || UX500_SOC_DB5500) && AV8100
+	select SND_SOC_AV8100
+	default n
+	help
+		Say Y if you want to include AV8100 codec (HDMI chip).
+
+config SND_SOC_UX500_DEBUG
+	bool "Activate Ux500 platform debug-mode (pr_debug)"
+	depends on SND_SOC_UX500
+	default n
+	help
+		Say Y if you want to add debug level prints for Ux500 code-files.
diff --git a/sound/soc/ux500/Makefile b/sound/soc/ux500/Makefile
new file mode 100644
index 00000000000..255c0ecefdb
--- /dev/null
+++ b/sound/soc/ux500/Makefile
@@ -0,0 +1,47 @@
+# Ux500 Platform Support
+
+ifdef CONFIG_SND_SOC_UX500_DEBUG
+CFLAGS_ux500_pcm.o := -DDEBUG
+CFLAGS_ux500_msp_dai.o := -DDEBUG
+CFLAGS_ux500_ab3550.o := -DDEBUG
+CFLAGS_ux500_ab8500.o := -DDEBUG
+CFLAGS_ux500_av8100.o := -DDEBUG
+CFLAGS_ux500_cg29xx.o := -DDEBUG
+endif
+
+ifdef CONFIG_SND_SOC_UX500_AB3550
+snd-soc-ux500-ab3550-objs := ux500_ab3550.o
+obj-$(CONFIG_SND_SOC_UX500_AB3550) += ux500_ab3550.o
+endif
+
+ifdef CONFIG_SND_SOC_UX500_AB5500
+snd-soc-ux500-ab5500-objs := ux500_ab5500.o
+obj-$(CONFIG_SND_SOC_UX500_AB5500) += ux500_ab5500.o
+endif
+
+ifdef CONFIG_SND_SOC_UX500_AB8500
+snd-soc-ux500-ab8500-objs := ux500_ab8500.o
+obj-$(CONFIG_SND_SOC_UX500_AB8500) += ux500_ab8500.o
+endif
+
+ifdef CONFIG_SND_SOC_UX500_AV8100
+snd-soc-ux500-av8100-objs := ux500_av8100.o
+obj-$(CONFIG_SND_SOC_UX500_AV8100) += ux500_av8100.o
+endif
+
+ifdef CONFIG_SND_SOC_UX500_CG29XX
+snd-soc-ux500-cg29xx-objs := ux500_cg29xx.o
+obj-$(CONFIG_SND_SOC_UX500_CG29XX) += ux500_cg29xx.o
+endif
+
+snd-soc-ux500-objs := ux500_pcm.o ux500_msp_dai.o
+
+ifdef CONFIG_UX500_SOC_DB8500
+snd-soc-ux500-objs += u8500.o
+endif
+
+ifdef CONFIG_UX500_SOC_DB5500
+snd-soc-ux500-objs += u5500.o
+endif
+
+obj-$(CONFIG_UX500_SOC_DBX500) += snd-soc-ux500.o
diff --git a/sound/soc/ux500/u5500.c b/sound/soc/ux500/u5500.c
new file mode 100644
index 00000000000..fda8db3472b
--- /dev/null
+++ b/sound/soc/ux500/u5500.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Xie Xiaolei (xie.xiaolei@stericsson.com)
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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/io.h>
+#include <sound/soc.h>
+#include <asm/mach-types.h>
+
+#include "ux500_pcm.h"
+#include "ux500_msp_dai.h"
+
+#include <linux/spi/spi.h>
+#include <sound/initval.h>
+
+#ifdef CONFIG_SND_SOC_UX500_AB5500
+#include "ux500_ab5500.h"
+#endif
+
+static struct platform_device *u5500_platform_dev;
+/* Create dummy devices for platform drivers */
+
+static struct platform_device ux500_pcm = {
+		.name = "ux500-pcm",
+		.id = 0,
+		.dev = {
+			.platform_data = NULL,
+		},
+};
+
+
+/* Define the whole U5500 soundcard, linking platform to the codec-drivers  */
+struct snd_soc_dai_link u5500_dai_links[] = {
+#ifdef CONFIG_SND_SOC_UX500_AB5500
+	{
+		.name = "ab5500_0",
+		.stream_name = "ab5500_0",
+		.cpu_dai_name = "i2s.0",
+		.codec_dai_name = "ab5500-codec-dai.0",
+		.platform_name = "ux500-pcm.0",
+		.codec_name = "ab5500-codec.9",
+		.init = NULL,
+		.ops = (struct snd_soc_ops[]) {
+			{
+				.startup = ux500_ab5500_startup,
+				.shutdown = ux500_ab5500_shutdown,
+				.hw_params = ux500_ab5500_hw_params,
+			}
+		}
+#endif
+	}
+};
+
+static struct snd_soc_card u5500_drvdata = {
+	.name = "U5500-card",
+	.probe = NULL,
+	.dai_link = u5500_dai_links,
+	.num_links = ARRAY_SIZE(u5500_dai_links),
+};
+
+static int __init u5500_soc_init(void)
+{
+	int ret = 0;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	if (!machine_is_u5500())
+		return 0;
+
+	platform_device_register(&ux500_pcm);
+
+	u5500_platform_dev = platform_device_alloc("soc-audio", -1);
+	if (!u5500_platform_dev)
+		return -ENOMEM;
+
+	platform_set_drvdata(u5500_platform_dev, &u5500_drvdata);
+	u5500_drvdata.dev = &u5500_platform_dev->dev;
+
+	ret = platform_device_add(u5500_platform_dev);
+	if (ret) {
+		pr_err("%s: Error: Failed to add platform device (%s).\n",
+			__func__,
+			u5500_drvdata.name);
+		platform_device_put(u5500_platform_dev);
+	}
+
+	return ret;
+}
+
+static void __exit u5500_soc_exit(void)
+{
+	pr_debug("%s: Enter.\n", __func__);
+
+	platform_device_unregister(u5500_platform_dev);
+}
+
+module_init(u5500_soc_init);
+module_exit(u5500_soc_exit);
+
+MODULE_LICENSE("GPLv2");
diff --git a/sound/soc/ux500/u8500.c b/sound/soc/ux500/u8500.c
new file mode 100644
index 00000000000..59324c878e4
--- /dev/null
+++ b/sound/soc/ux500/u8500.c
@@ -0,0 +1,244 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja (ola.o.lilja@stericsson.com)
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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/io.h>
+#include <sound/soc.h>
+#include <asm/mach-types.h>
+
+#include "ux500_pcm.h"
+#include "ux500_msp_dai.h"
+
+#include <linux/spi/spi.h>
+#include <sound/initval.h>
+
+#ifdef CONFIG_SND_SOC_UX500_AB3550
+#include "ux500_ab3550.h"
+#endif
+
+#ifdef CONFIG_SND_SOC_UX500_AB8500
+#include <sound/ux500_ab8500.h>
+#endif
+
+#ifdef CONFIG_SND_SOC_UX500_AV8100
+#include "ux500_av8100.h"
+#endif
+
+#ifdef CONFIG_SND_SOC_UX500_CG29XX
+#include "ux500_cg29xx.h"
+#endif
+
+
+static struct platform_device *u8500_platform_dev;
+
+/* Create dummy devices for platform drivers */
+
+static struct platform_device ux500_pcm = {
+		.name = "ux500-pcm",
+		.id = 0,
+		.dev = {
+			.platform_data = NULL,
+		},
+};
+
+#ifdef CONFIG_SND_SOC_UX500_AV8100
+static struct platform_device av8100_codec = {
+		.name = "av8100-codec",
+		.id = 0,
+		.dev = {
+			.platform_data = NULL,
+		},
+};
+#endif
+
+#ifdef CONFIG_SND_SOC_UX500_CG29XX
+static struct platform_device cg29xx_codec = {
+		.name = "cg29xx-codec",
+		.id = 0,
+		.dev = {
+			.platform_data = NULL,
+		},
+};
+#endif
+
+/* Define the whole U8500 soundcard, linking platform to the codec-drivers  */
+struct snd_soc_dai_link u8500_dai_links[] = {
+	#ifdef CONFIG_SND_SOC_UX500_AV8100
+	{
+	.name = "hdmi",
+	.stream_name = "hdmi",
+	.cpu_dai_name = "i2s.2",
+	.codec_dai_name = "av8100-codec-dai",
+	.platform_name = "ux500-pcm.0",
+	.codec_name = "av8100-codec.0",
+	.init = NULL,
+	.ops = ux500_av8100_ops,
+	},
+	#endif
+	#ifdef CONFIG_SND_SOC_UX500_AB3550
+	{
+	.name = "ab3550_0",
+	.stream_name = "ab3550_0",
+	.cpu_dai_name = "i2s.0",
+	.codec_dai_name = "ab3550-codec-dai.0",
+	.platform_name = "ux500-pcm.0",
+	.codec_name = "ab3550-codec.11",
+	.init = NULL,
+	.ops = ux500_ab3550_ops,
+	},
+	{
+	.name = "ab3550_1",
+	.stream_name = "ab3550_1",
+	.cpu_dai_name = "i2s.1",
+	.codec_dai_name = "ab3550-codec-dai.1",
+	.platform_name = "ux500-pcm.0",
+	.codec_name = "ab3550-codec.11",
+	.init = NULL,
+	.ops = ux500_ab3550_ops,
+	},
+	#endif
+	#ifdef CONFIG_SND_SOC_UX500_AB8500
+	{
+	.name = "ab8500_0",
+	.stream_name = "ab8500_0",
+	.cpu_dai_name = "i2s.1",
+	.codec_dai_name = "ab8500-codec-dai.0",
+	.platform_name = "ux500-pcm.0",
+	.codec_name = "ab8500-codec.0",
+	.init = ux500_ab8500_machine_codec_init,
+	.ops = ux500_ab8500_ops,
+	},
+	{
+	.name = "ab8500_1",
+	.stream_name = "ab8500_1",
+	.cpu_dai_name = "i2s.3",
+	.codec_dai_name = "ab8500-codec-dai.1",
+	.platform_name = "ux500-pcm.0",
+	.codec_name = "ab8500-codec.0",
+	.init = NULL,
+	.ops = ux500_ab8500_ops,
+	},
+	#endif
+	#ifdef CONFIG_SND_SOC_UX500_CG29XX
+	{
+	.name = "cg29xx_0",
+	.stream_name = "cg29xx_0",
+	.cpu_dai_name = "i2s.0",
+	.codec_dai_name = "cg29xx-codec-dai.1",
+	.platform_name = "ux500-pcm.0",
+	.codec_name = "cg29xx-codec.0",
+	.init = NULL,
+	.ops = ux500_cg29xx_ops,
+	},
+	#endif
+};
+
+static struct snd_soc_card u8500_drvdata = {
+	.name = "U8500-card",
+	.probe = NULL,
+	.dai_link = u8500_dai_links,
+	.num_links = ARRAY_SIZE(u8500_dai_links),
+};
+
+static int __init u8500_soc_init(void)
+{
+	int ret;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	if (machine_is_u5500())
+		return 0;
+
+	#ifdef CONFIG_SND_SOC_UX500_AV8100
+	pr_debug("%s: Register device to generate a probe for AV8100 codec.\n",
+		__func__);
+	platform_device_register(&av8100_codec);
+	#endif
+
+	#ifdef CONFIG_SND_SOC_UX500_CG29XX
+	pr_debug("%s: Register device to generate a probe for CG29xx codec.\n",
+		__func__);
+	platform_device_register(&cg29xx_codec);
+	#endif
+
+	#ifdef CONFIG_SND_SOC_UX500_AB8500
+	pr_debug("%s: Calling init-function for AB8500 machine driver.\n",
+		__func__);
+	ret = ux500_ab8500_soc_machine_drv_init();
+	if (ret)
+		pr_err("%s: ux500_ab8500_soc_machine_drv_init failed (%d).\n",
+			__func__, ret);
+	#endif
+
+	pr_debug("%s: Register device to generate a probe for Ux500-pcm platform.\n",
+		__func__);
+	platform_device_register(&ux500_pcm);
+
+	pr_debug("%s: Allocate platform device 'soc-audio'.\n",
+		__func__);
+	u8500_platform_dev = platform_device_alloc("soc-audio", -1);
+	if (!u8500_platform_dev)
+		return -ENOMEM;
+
+	pr_debug("%s: Card %s: num_links = %d\n",
+		__func__,
+		u8500_drvdata.name,
+		u8500_drvdata.num_links);
+	pr_debug("%s: Card %s: DAI-link 0: name = %s\n",
+		__func__,
+		u8500_drvdata.name,
+		u8500_drvdata.dai_link[0].name);
+	pr_debug("%s: Card %s: DAI-link 0: stream_name = %s\n",
+		__func__,
+		u8500_drvdata.name,
+		u8500_drvdata.dai_link[0].stream_name);
+
+	pr_debug("%s: Card %s: Set platform drvdata.\n",
+		__func__,
+		u8500_drvdata.name);
+	platform_set_drvdata(u8500_platform_dev, &u8500_drvdata);
+	u8500_drvdata.dev = &u8500_platform_dev->dev;
+
+	pr_debug("%s: Card %s: Add platform device.\n",
+		__func__,
+		u8500_drvdata.name);
+	ret = platform_device_add(u8500_platform_dev);
+	if (ret) {
+		pr_err("%s: Error: Failed to add platform device (%s).\n",
+			__func__,
+			u8500_drvdata.name);
+		platform_device_put(u8500_platform_dev);
+	}
+
+	return ret;
+}
+
+static void __exit u8500_soc_exit(void)
+{
+	pr_debug("%s: Enter.\n", __func__);
+
+	#ifdef CONFIG_SND_SOC_UX500_AB8500
+	pr_debug("%s: Calling exit-function for AB8500 machine driver.\n",
+		__func__);
+	ux500_ab8500_soc_machine_drv_cleanup();
+	#endif
+
+	pr_debug("%s: Unregister platform device (%s).\n",
+		__func__,
+		u8500_drvdata.name);
+	platform_device_unregister(u8500_platform_dev);
+}
+
+module_init(u8500_soc_init);
+module_exit(u8500_soc_exit);
+
+MODULE_LICENSE("GPLv2");
diff --git a/sound/soc/ux500/ux500_ab3550.c b/sound/soc/ux500/ux500_ab3550.c
new file mode 100644
index 00000000000..7e144c0e4d2
--- /dev/null
+++ b/sound/soc/ux500/ux500_ab3550.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja ola.o.lilja@stericsson.com,
+ *         Roger Nilsson roger.xr.nilsson@stericsson.com
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 <sound/soc.h>
+#include "../codecs/ab3550.h"
+
+static int ux500_ab3550_startup(struct snd_pcm_substream *substream)
+{
+	pr_debug("%s: Enter.\n", __func__);
+
+	return 0;
+}
+
+static void ux500_ab3550_shutdown(struct snd_pcm_substream *substream)
+{
+	pr_debug("%s: Enter.\n", __func__);
+}
+
+static int ux500_ab3550_hw_params(struct snd_pcm_substream *substream,
+			struct snd_pcm_hw_params *params)
+{
+	struct snd_soc_pcm_runtime *rtd = substream->private_data;
+	struct snd_soc_dai *codec_dai = rtd->codec_dai;
+	struct snd_soc_dai *cpu_dai = rtd->cpu_dai;
+	int ret = 0;
+
+	int channels = params_channels(params);
+
+	pr_debug("%s: Enter.\n", __func__);
+	pr_debug("%s: substream->pcm->name = %s.\n", __func__, substream->pcm->name);
+	pr_debug("%s: substream->pcm->id = %s.\n", __func__, substream->pcm->id);
+	pr_debug("%s: substream->name = %s.\n", __func__, substream->name);
+	pr_debug("%s: substream->number = %d.\n", __func__, substream->number);
+	pr_debug("%s: channels = %d.\n", __func__, channels);
+	pr_debug("%s: DAI-index (Codec): %d\n", __func__, codec_dai->id);
+	pr_debug("%s: DAI-index (Platform): %d\n", __func__, cpu_dai->id);
+
+	ret = snd_soc_dai_set_fmt(codec_dai,
+		SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBS_CFS);
+	if (ret < 0) {
+		pr_debug("%s: snd_soc_dai_set_fmt failed with %d.\n",
+			__func__,
+			ret);
+		return ret;
+	}
+
+	ret = snd_soc_dai_set_fmt(cpu_dai,
+		SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBS_CFS);
+	if (ret < 0) {
+		pr_debug("%s: snd_soc_dai_set_fmt failed with %d.\n",
+			__func__,
+			ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+struct snd_soc_ops ux500_ab3550_ops[] = {
+	{
+		.startup = ux500_ab3550_startup,
+		.shutdown = ux500_ab3550_shutdown,
+		.hw_params = ux500_ab3550_hw_params,
+	}
+};
diff --git a/sound/soc/ux500/ux500_ab3550.h b/sound/soc/ux500/ux500_ab3550.h
new file mode 100644
index 00000000000..53ea3902d36
--- /dev/null
+++ b/sound/soc/ux500/ux500_ab3550.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 UX500_AB3550_H
+#define UX500_AB3550_H
+
+extern struct snd_soc_ops ux500_ab3550_ops[];
+
+#endif
diff --git a/sound/soc/ux500/ux500_ab5500.c b/sound/soc/ux500/ux500_ab5500.c
new file mode 100644
index 00000000000..551ec05fcb5
--- /dev/null
+++ b/sound/soc/ux500/ux500_ab5500.c
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Ola Lilja ola.o.lilja@stericsson.com,
+ *         Roger Nilsson roger.xr.nilsson@stericsson.com
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 <sound/soc.h>
+#include "../codecs/ab5500.h"
+int ux500_ab5500_startup(struct snd_pcm_substream *substream)
+{
+	return 0;
+}
+
+void ux500_ab5500_shutdown(struct snd_pcm_substream *substream)
+{
+	printk(KERN_DEBUG "%s: Enter.\n", __func__);
+}
+
+int ux500_ab5500_hw_params(struct snd_pcm_substream *substream,
+			struct snd_pcm_hw_params *params)
+{
+	struct snd_soc_pcm_runtime *rtd = substream->private_data;
+	struct snd_soc_dai *codec_dai = rtd->codec_dai;
+	struct snd_soc_dai *cpu_dai = rtd->cpu_dai;
+	int ret = 0;
+
+	int channels = params_channels(params);
+
+	printk(KERN_DEBUG "%s: Enter.\n", __func__);
+	printk(KERN_DEBUG "%s: substream->pcm->name = %s.\n", __func__, substream->pcm->name);
+	printk(KERN_DEBUG "%s: substream->pcm->id = %s.\n", __func__, substream->pcm->id);
+	printk(KERN_DEBUG "%s: substream->name = %s.\n", __func__, substream->name);
+	printk(KERN_DEBUG "%s: substream->number = %d.\n", __func__, substream->number);
+	printk(KERN_DEBUG "%s: channels = %d.\n", __func__, channels);
+	printk(KERN_DEBUG "%s: DAI-index (Codec): %d\n", __func__, codec_dai->id);
+	printk(KERN_DEBUG "%s: DAI-index (Platform): %d\n", __func__, cpu_dai->id);
+
+	ret = snd_soc_dai_set_fmt(codec_dai,
+		SND_SOC_DAIFMT_I2S |  SND_SOC_DAIFMT_CBS_CFS);
+	if (ret < 0)
+		return ret;
+
+	ret = snd_soc_dai_set_fmt(cpu_dai,
+		SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBS_CFS);
+	if (ret < 0)
+		return ret;
+
+	return ret;
+}
diff --git a/sound/soc/ux500/ux500_ab5500.h b/sound/soc/ux500/ux500_ab5500.h
new file mode 100644
index 00000000000..ea69f1a048c
--- /dev/null
+++ b/sound/soc/ux500/ux500_ab5500.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Xie Xiaolei (xie.xiaolei@stericsson.com)
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 UX500_AB5500_H
+#define UX500_AB5500_H
+
+int ux500_ab5500_startup(struct snd_pcm_substream *substream);
+
+void ux500_ab5500_shutdown(struct snd_pcm_substream *substream);
+
+int ux500_ab5500_hw_params(struct snd_pcm_substream *substream,
+			struct snd_pcm_hw_params *params);
+
+#endif
diff --git a/sound/soc/ux500/ux500_ab8500.c b/sound/soc/ux500/ux500_ab8500.c
new file mode 100644
index 00000000000..805c26448d3
--- /dev/null
+++ b/sound/soc/ux500/ux500_ab8500.c
@@ -0,0 +1,591 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Mikko J. Lehto <mikko.lehto@symbio.com>,
+ *         Mikko Sarmanne <mikko.sarmanne@symbio.com>,
+ *         Jarmo K. Kuronen <jarmo.kuronen@symbio.com>.
+ *         Ola Lilja <ola.o.lilja@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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/clk.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <sound/soc.h>
+#include <linux/regulator/consumer.h>
+#include <sound/pcm.h>
+#include <sound/jack.h>
+#include <sound/pcm_params.h>
+#include <mach/hardware.h>
+#include "ux500_pcm.h"
+#include "ux500_msp_dai.h"
+#include "../codecs/ab8500_audio.h"
+
+#define TX_SLOT_MONO	0x0008
+#define TX_SLOT_STEREO	0x000a
+#define RX_SLOT_MONO	0x0001
+#define RX_SLOT_STEREO	0x0003
+#define TX_SLOT_8CH	0x00FF
+#define RX_SLOT_8CH	0x00FF
+
+#define DEF_TX_SLOTS	TX_SLOT_STEREO
+#define DEF_RX_SLOTS	RX_SLOT_MONO
+
+#define DRIVERMODE_NORMAL	0
+#define DRIVERMODE_CODEC_ONLY	1
+
+static struct snd_soc_jack jack;
+static bool vibra_on;
+
+/* Power-control */
+static DEFINE_MUTEX(power_lock);
+static int ab8500_power_count;
+
+/* Clocks */
+/* audioclk -> intclk -> sysclk/ulpclk */
+static int master_clock_sel;
+static struct clk *clk_ptr_audioclk;
+static struct clk *clk_ptr_intclk;
+static struct clk *clk_ptr_sysclk;
+static struct clk *clk_ptr_ulpclk;
+
+/* Regulators */
+enum regulator_idx {
+	REGULATOR_AUDIO,
+	REGULATOR_DMIC,
+	REGULATOR_AMIC1,
+	REGULATOR_AMIC2
+};
+static struct regulator_bulk_data reg_info[4] = {
+	{	.supply = "v-audio"	},
+	{	.supply = "v-dmic"	},
+	{	.supply = "v-amic1"	},
+	{	.supply = "v-amic2"	}
+};
+static bool reg_enabled[4] =  {
+	false,
+	false,
+	false,
+	false
+};
+
+/* Slot configuration */
+static unsigned int tx_slots = DEF_TX_SLOTS;
+static unsigned int rx_slots = DEF_RX_SLOTS;
+
+/* Machine-driver ALSA-controls */
+
+static int mclk_input_control_info(struct snd_kcontrol *kcontrol,
+			    struct snd_ctl_elem_info *uinfo)
+{
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
+	uinfo->count = 1;
+	uinfo->value.enumerated.items = 2;
+	if (uinfo->value.enumerated.item) {
+		uinfo->value.enumerated.item = 1;
+		strcpy(uinfo->value.enumerated.name, "ULPCLK");
+	} else {
+		strcpy(uinfo->value.enumerated.name, "SYSCLK");
+	}
+	return 0;
+}
+
+static int mclk_input_control_get(struct snd_kcontrol *kcontrol,
+			   struct snd_ctl_elem_value *ucontrol)
+{
+	ucontrol->value.enumerated.item[0] = master_clock_sel;
+	return 0;
+}
+
+static int mclk_input_control_put(struct snd_kcontrol *kcontrol,
+			   struct snd_ctl_elem_value *ucontrol)
+{
+	unsigned int val;
+
+	val = (ucontrol->value.enumerated.item[0] != 0);
+	if (master_clock_sel == val)
+		return 0;
+
+	master_clock_sel = val;
+
+	return 1;
+}
+
+static const struct snd_kcontrol_new mclk_input_control = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.name = "Master Clock Select",
+	.index = 0,
+	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.info = mclk_input_control_info,
+	.get = mclk_input_control_get,
+	.put = mclk_input_control_put,
+	.private_value = 1 /* ULPCLK */
+};
+
+/* Regulators */
+
+static int enable_regulator(enum regulator_idx idx)
+{
+	int ret;
+
+	if (reg_enabled[idx])
+		return 0;
+
+	ret = regulator_enable(reg_info[idx].consumer);
+	if (ret != 0) {
+		pr_err("%s: Failure to enable regulator '%s' (ret = %d)\n",
+			__func__, reg_info[idx].supply, ret);
+		return ret;
+	};
+
+	reg_enabled[idx] = true;
+	pr_debug("%s: Enabled regulator '%s', status: %d, %d, %d, %d\n",
+		__func__,
+		reg_info[idx].supply,
+		(int)reg_enabled[0],
+		(int)reg_enabled[1],
+		(int)reg_enabled[2],
+		(int)reg_enabled[3]);
+	return 0;
+}
+
+static void disable_regulator(enum regulator_idx idx)
+{
+	if (!reg_enabled[idx])
+		return;
+
+	regulator_disable(reg_info[idx].consumer);
+
+	reg_enabled[idx] = false;
+	pr_debug("%s: Disabled regulator '%s', status: %d, %d, %d, %d\n",
+		__func__,
+		reg_info[idx].supply,
+		(int)reg_enabled[0],
+		(int)reg_enabled[1],
+		(int)reg_enabled[2],
+		(int)reg_enabled[3]);
+}
+
+static int create_regulators(void)
+{
+	int i, status = 0;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	for (i = 0; i < ARRAY_SIZE(reg_info); ++i)
+		reg_info[i].consumer = NULL;
+
+	for (i = 0; i < ARRAY_SIZE(reg_info); ++i) {
+		reg_info[i].consumer = regulator_get(NULL, reg_info[i].supply);
+		if (IS_ERR(reg_info[i].consumer)) {
+			status = PTR_ERR(reg_info[i].consumer);
+			pr_err("%s: ERROR: Failed to get regulator '%s' (ret = %d)!\n",
+				__func__, reg_info[i].supply, status);
+			reg_info[i].consumer = NULL;
+			goto err_get;
+		}
+	}
+
+	return 0;
+
+err_get:
+
+	for (i = 0; i < ARRAY_SIZE(reg_info); ++i) {
+		if (reg_info[i].consumer) {
+			regulator_put(reg_info[i].consumer);
+			reg_info[i].consumer = NULL;
+		}
+	}
+
+	return status;
+}
+
+/* Power/clock control */
+
+static int ux500_ab8500_power_control_inc(void)
+{
+	int ret;
+
+	mutex_lock(&power_lock);
+
+	ab8500_power_count++;
+	pr_debug("%s: ab8500_power_count changed from %d to %d",
+		__func__,
+		ab8500_power_count-1,
+		ab8500_power_count);
+
+	if (ab8500_power_count == 1) {
+		ret = clk_set_parent(clk_ptr_intclk,
+				(master_clock_sel == 0) ? clk_ptr_sysclk : clk_ptr_ulpclk);
+		if (ret) {
+			pr_err("%s: ERROR: Setting master-clock to %s failed (ret = %d)!",
+				__func__,
+				(master_clock_sel == 0) ? "SYSCLK" : "ULPCLK",
+				ret);
+			return ret;
+		}
+
+		pr_debug("%s: Enabling master-clock (%s).",
+			__func__,
+			(master_clock_sel == 0) ? "SYSCLK" : "ULPCLK");
+
+		/* Enable audio-clock */
+		ret = clk_enable(clk_ptr_audioclk);
+		if (ret) {
+			pr_err("%s: ERROR: clk_enable failed (ret = %d)!", __func__, ret);
+			ab8500_power_count = 0;
+			return ret;
+		}
+
+		/* Power on audio-parts of AB8500 */
+		ab8500_audio_power_control(true);
+
+		/* Turn on audio-regulator */
+		ret = enable_regulator(REGULATOR_AUDIO);
+	}
+
+	mutex_unlock(&power_lock);
+
+	return 0;
+}
+
+static void ux500_ab8500_power_control_dec(void)
+{
+	mutex_lock(&power_lock);
+
+	ab8500_power_count--;
+
+	pr_debug("%s: ab8500_power_count changed from %d to %d",
+		__func__,
+		ab8500_power_count+1,
+		ab8500_power_count);
+
+	if (ab8500_power_count == 0) {
+		pr_debug("%s: Disabling master-clock (%s).",
+			__func__,
+			(master_clock_sel == 0) ? "SYSCLK" : "ULPCLK");
+
+		/* Disable audio-clock */
+		clk_disable(clk_ptr_audioclk);
+
+		/* Power off audio-parts of AB8500 */
+		ab8500_audio_power_control(false);
+
+		/* Turn off audio-regulator */
+		disable_regulator(REGULATOR_AUDIO);
+	}
+
+	mutex_unlock(&power_lock);
+}
+
+/* ASoC */
+
+int ux500_ab8500_startup(struct snd_pcm_substream *substream)
+{
+	int ret = 0;
+
+	pr_debug("%s: Enter\n", __func__);
+
+	/* If we start recording we better enable the needed mic-regulators */
+	if (substream->stream == SNDRV_PCM_STREAM_CAPTURE) {
+		if (ab8500_audio_dapm_path_active(AB8500_AUDIO_DAPM_PATH_DMIC))
+			ret += enable_regulator(REGULATOR_DMIC);
+		if (ab8500_audio_dapm_path_active(AB8500_AUDIO_DAPM_PATH_AMIC1))
+			ret += enable_regulator(REGULATOR_AMIC1);
+		if (ab8500_audio_dapm_path_active(AB8500_AUDIO_DAPM_PATH_AMIC2))
+			ret += enable_regulator(REGULATOR_AMIC2);
+		if (ret != 0)
+			return ret;
+	}
+
+	return ux500_ab8500_power_control_inc();
+
+	return ret;
+}
+
+void ux500_ab8500_shutdown(struct snd_pcm_substream *substream)
+{
+	pr_debug("%s: Enter\n", __func__);
+
+	/* Reset slots configuration to default(s) */
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		tx_slots = DEF_TX_SLOTS;
+	else
+		rx_slots = DEF_RX_SLOTS;
+
+	/* Disable all mic-regulators that were enabled when we stop recording */
+	if (substream->stream == SNDRV_PCM_STREAM_CAPTURE) {
+		disable_regulator(REGULATOR_DMIC);
+		disable_regulator(REGULATOR_AMIC1);
+		disable_regulator(REGULATOR_AMIC2);
+	}
+
+	ux500_ab8500_power_control_dec();
+}
+
+int ux500_ab8500_hw_params(struct snd_pcm_substream *substream,
+			struct snd_pcm_hw_params *params)
+{
+	struct snd_soc_pcm_runtime *rtd = substream->private_data;
+	struct snd_soc_dai *codec_dai = rtd->codec_dai;
+	struct snd_soc_dai *cpu_dai = rtd->cpu_dai;
+	unsigned int fmt, fmt_if1;
+	int channels, ret = 0, slots, slot_width, driver_mode;
+	bool streamIsPlayback;
+
+	pr_debug("%s: Enter\n", __func__);
+
+	pr_debug("%s: substream->pcm->name = %s\n"
+		"substream->pcm->id = %s.\n"
+		"substream->name = %s.\n"
+		"substream->number = %d.\n",
+		__func__,
+		substream->pcm->name,
+		substream->pcm->id,
+		substream->name,
+		substream->number);
+
+	channels = params_channels(params);
+
+	/* Setup codec depending on driver-mode */
+	driver_mode = (channels == 8) ?
+		DRIVERMODE_CODEC_ONLY : DRIVERMODE_NORMAL;
+	pr_debug("%s: Driver-mode: %s.\n",
+		__func__,
+		(driver_mode == DRIVERMODE_NORMAL) ? "NORMAL" : "CODEC_ONLY");
+	if (driver_mode == DRIVERMODE_NORMAL) {
+		ab8500_audio_set_bit_delay(codec_dai, 0);
+		ab8500_audio_set_word_length(codec_dai, 16);
+		fmt = SND_SOC_DAIFMT_DSP_B |
+			SND_SOC_DAIFMT_CBM_CFM |
+			SND_SOC_DAIFMT_NB_NF |
+			SND_SOC_DAIFMT_CONT;
+	} else {
+		ab8500_audio_set_bit_delay(codec_dai, 1);
+		ab8500_audio_set_word_length(codec_dai, 20);
+		fmt = SND_SOC_DAIFMT_DSP_B |
+			SND_SOC_DAIFMT_CBM_CFM |
+			SND_SOC_DAIFMT_NB_NF |
+			SND_SOC_DAIFMT_GATED;
+	}
+
+	ret = snd_soc_dai_set_fmt(codec_dai, fmt);
+	if (ret < 0) {
+		pr_err("%s: ERROR: snd_soc_dai_set_fmt failed for codec_dai (ret = %d)!\n",
+			__func__,
+			ret);
+		return ret;
+	}
+
+	ret = snd_soc_dai_set_fmt(cpu_dai, fmt);
+	if (ret < 0) {
+		pr_err("%s: ERROR: snd_soc_dai_set_fmt for cpu_dai (ret = %d)!\n",
+			__func__,
+			ret);
+		return ret;
+	}
+
+	/* Setup TDM-slots */
+
+	streamIsPlayback = (substream->stream == SNDRV_PCM_STREAM_PLAYBACK);
+	switch (channels) {
+	case 1:
+		slots = 16;
+		slot_width = 16;
+		tx_slots = (streamIsPlayback) ? TX_SLOT_MONO : 0;
+		rx_slots = (streamIsPlayback) ? 0 : RX_SLOT_MONO;
+		break;
+	case 2:
+		slots = 16;
+		slot_width = 16;
+		tx_slots = (streamIsPlayback) ? TX_SLOT_STEREO : 0;
+		rx_slots = (streamIsPlayback) ? 0 : RX_SLOT_STEREO;
+		break;
+	case 8:
+		slots = 16;
+		slot_width = 16;
+		tx_slots = (streamIsPlayback) ? TX_SLOT_8CH : 0;
+		rx_slots = (streamIsPlayback) ? 0 : RX_SLOT_8CH;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	pr_debug("%s: CPU-DAI TDM: TX=0x%04X RX=0x%04x\n",
+		__func__, tx_slots, rx_slots);
+	ret = snd_soc_dai_set_tdm_slot(cpu_dai, tx_slots, rx_slots, slots, slot_width);
+	if (ret)
+		return ret;
+
+	pr_debug("%s: CODEC-DAI TDM: TX=0x%04X RX=0x%04x\n",
+		__func__, tx_slots, rx_slots);
+	ret = snd_soc_dai_set_tdm_slot(codec_dai, tx_slots, rx_slots, slots, slot_width);
+	if (ret)
+		return ret;
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
+		pr_debug("%s: Setup IF1 for FM-radio.\n", __func__);
+		fmt_if1 = SND_SOC_DAIFMT_CBM_CFM | SND_SOC_DAIFMT_I2S;
+		ret = ab8500_audio_setup_if1(codec_dai->codec, fmt_if1, 16, 1);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+struct snd_soc_ops ux500_ab8500_ops[] = {
+	{
+	.hw_params = ux500_ab8500_hw_params,
+	.startup = ux500_ab8500_startup,
+	.shutdown = ux500_ab8500_shutdown,
+	}
+};
+
+int ux500_ab8500_machine_codec_init(struct snd_soc_pcm_runtime *rtd)
+{
+	struct snd_soc_codec *codec = rtd->codec;
+	int ret;
+
+	pr_debug("%s Enter.\n", __func__);
+
+	/* TODO: Add required DAPM routes to control regulators on demand */
+
+	ret = snd_soc_jack_new(codec,
+			"AB8500 Hs Status",
+			SND_JACK_HEADPHONE     |
+			SND_JACK_MICROPHONE    |
+			SND_JACK_HEADSET       |
+			SND_JACK_LINEOUT       |
+			SND_JACK_MECHANICAL    |
+			SND_JACK_VIDEOOUT,
+			&jack);
+	if (ret < 0) {
+		pr_err("%s: ERROR: Failed to create Jack (ret = %d)!\n", __func__, ret);
+		return ret;
+	}
+
+	/* Add controls */
+	snd_ctl_add(codec->card->snd_card, snd_ctl_new1(&mclk_input_control, codec));
+
+	/* Get references to clock-nodes */
+	clk_ptr_sysclk = NULL;
+	clk_ptr_ulpclk = NULL;
+	clk_ptr_intclk = NULL;
+	clk_ptr_audioclk = NULL;
+	clk_ptr_sysclk = clk_get(codec->dev, "sysclk");
+	if (IS_ERR(clk_ptr_sysclk)) {
+		pr_err("ERROR: clk_get failed (ret = %d)!", -EFAULT);
+		return -EFAULT;
+	}
+	clk_ptr_ulpclk = clk_get(codec->dev, "ulpclk");
+	if (IS_ERR(clk_ptr_sysclk)) {
+		pr_err("ERROR: clk_get failed (ret = %d)!", -EFAULT);
+		return -EFAULT;
+	}
+	clk_ptr_intclk = clk_get(codec->dev, "intclk");
+	if (IS_ERR(clk_ptr_audioclk)) {
+		pr_err("ERROR: clk_get failed (ret = %d)!", -EFAULT);
+		return -EFAULT;
+	}
+	clk_ptr_audioclk = clk_get(codec->dev, "audioclk");
+	if (IS_ERR(clk_ptr_audioclk)) {
+		pr_err("ERROR: clk_get failed (ret = %d)!", -EFAULT);
+		return -EFAULT;
+	}
+
+	/* Set intclk default parent to ulpclk */
+	ret = clk_set_parent(clk_ptr_intclk, clk_ptr_ulpclk);
+	if (ret) {
+		pr_err("%s: ERROR: Setting intclk parent to ulpclk failed (ret = %d)!",
+			__func__,
+			ret);
+		return -EFAULT;
+	}
+
+	master_clock_sel = 1;
+
+	ab8500_power_count = 0;
+
+	return 0;
+}
+
+int ux500_ab8500_soc_machine_drv_init(void)
+{
+	int status = 0;
+
+	pr_debug("%s: Enter.\n", __func__);
+
+	status = create_regulators();
+	if (status < 0) {
+		pr_err("%s: ERROR: Failed to instantiate regulators (ret = %d)!\n",
+			__func__, status);
+		return status;
+	}
+
+	vibra_on = false;
+
+	return 0;
+}
+
+void ux500_ab8500_soc_machine_drv_cleanup(void)
+{
+	pr_debug("%s: Enter.\n", __func__);
+
+	regulator_bulk_free(ARRAY_SIZE(reg_info), reg_info);
+
+	if (clk_ptr_sysclk != NULL)
+		clk_put(clk_ptr_sysclk);
+	if (clk_ptr_ulpclk != NULL)
+		clk_put(clk_ptr_ulpclk);
+	if (clk_ptr_intclk != NULL)
+		clk_put(clk_ptr_intclk);
+	if (clk_ptr_audioclk != NULL)
+		clk_put(clk_ptr_audioclk);
+}
+
+/* Extended interface */
+
+void ux500_ab8500_audio_pwm_vibra(unsigned char speed_left_pos,
+			unsigned char speed_left_neg,
+			unsigned char speed_right_pos,
+			unsigned char speed_right_neg)
+{
+	bool vibra_on_new;
+
+	vibra_on_new = speed_left_pos | speed_left_neg | speed_right_pos | speed_right_neg;
+	if ((!vibra_on_new) && (vibra_on)) {
+		pr_debug("%s: PWM-vibra off.\n", __func__);
+		vibra_on = false;
+
+		ux500_ab8500_power_control_dec();
+	}
+
+	if ((vibra_on_new) && (!vibra_on)) {
+		pr_debug("%s: PWM-vibra on.\n", __func__);
+		vibra_on = true;
+
+		ux500_ab8500_power_control_inc();
+	}
+
+	ab8500_audio_pwm_vibra(speed_left_pos,
+			speed_left_neg,
+			speed_right_pos,
+			speed_right_neg);
+}
+
+void ux500_ab8500_jack_report(int value)
+{
+	if (jack.jack)
+		snd_soc_jack_report(&jack, value, 0xFF);
+}
+EXPORT_SYMBOL_GPL(ux500_ab8500_jack_report);
+
diff --git a/sound/soc/ux500/ux500_av8100.c b/sound/soc/ux500/ux500_av8100.c
new file mode 100644
index 00000000000..efb6318e1a7
--- /dev/null
+++ b/sound/soc/ux500/ux500_av8100.c
@@ -0,0 +1,168 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 <sound/soc.h>
+#include "../codecs/av8100_audio.h"
+#include "ux500_av8100.h"
+#include "ux500_msp_dai.h"
+
+static const char *stream_str(struct snd_pcm_substream *substream)
+{
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		return "Playback";
+	else
+		return "Capture";
+}
+
+static int ux500_av8100_hw_params(struct snd_pcm_substream *substream,
+			struct snd_pcm_hw_params *params)
+{
+	struct snd_soc_pcm_runtime *rtd = substream->private_data;
+	struct snd_soc_dai *codec_dai = rtd->codec_dai;
+	struct snd_soc_dai *cpu_dai = rtd->cpu_dai;
+	int channels = params_channels(params);
+	unsigned int tx_mask, fmt;
+	enum hdmi_channel_allocation hdmi_ca;
+	enum hdmi_audio_channel_count hdmi_cc;
+	struct hdmi_audio_settings as;
+	int ret;
+
+	pr_debug("%s: Enter (%s).\n", __func__, stream_str(substream));
+	pr_debug("%s: substream->pcm->name = %s.\n", __func__, substream->pcm->name);
+	pr_debug("%s: substream->pcm->id = %s.\n", __func__, substream->pcm->id);
+	pr_debug("%s: substream->name = %s.\n", __func__, substream->name);
+	pr_debug("%s: substream->number = %d.\n", __func__, substream->number);
+	pr_debug("%s: channels = %d.\n", __func__, channels);
+	pr_debug("%s: DAI-index (Codec): %d\n", __func__, codec_dai->id);
+	pr_debug("%s: DAI-index (Platform): %d\n", __func__, cpu_dai->id);
+
+	switch (channels) {
+	case 1:
+		hdmi_cc = AV8100_CODEC_CC_2CH;
+		hdmi_ca = AV8100_CODEC_CA_FL_FR; /* Stereo-setup */
+		tx_mask = AV8100_CODEC_MASK_MONO;
+		break;
+	case 2:
+		hdmi_cc = AV8100_CODEC_CC_2CH;
+		hdmi_ca = AV8100_CODEC_CA_FL_FR; /* Stereo */
+		tx_mask = AV8100_CODEC_MASK_STEREO;
+		break;
+	case 3:
+		hdmi_cc = AV8100_CODEC_CC_6CH;
+		hdmi_ca = AV8100_CODEC_CA_FL_FR_LFE_FC_RL_RR; /* 5.1-setup */
+		tx_mask = AV8100_CODEC_MASK_2DOT1;
+		break;
+	case 4:
+		hdmi_cc = AV8100_CODEC_CC_6CH;
+		hdmi_ca = AV8100_CODEC_CA_FL_FR_LFE_FC_RL_RR; /* 5.1-setup */
+		tx_mask = AV8100_CODEC_MASK_QUAD;
+		break;
+	case 5:
+		hdmi_cc = AV8100_CODEC_CC_6CH;
+		hdmi_ca = AV8100_CODEC_CA_FL_FR_LFE_FC_RL_RR; /* 5.1-setup */
+		tx_mask = AV8100_CODEC_MASK_5DOT0;
+		break;
+	case 6:
+		hdmi_cc = AV8100_CODEC_CC_6CH;
+		hdmi_ca = AV8100_CODEC_CA_FL_FR_LFE_FC_RL_RR; /* 5.1 */
+		tx_mask = AV8100_CODEC_MASK_5DOT1;
+		break;
+	case 7:
+		hdmi_cc = AV8100_CODEC_CC_8CH;
+		hdmi_ca = AV8100_CODEC_CA_FL_FR_LFE_FC_RL_RR_RLC_RRC; /* 7.1 */
+		tx_mask = AV8100_CODEC_MASK_7DOT0;
+		break;
+	case 8:
+		hdmi_cc = AV8100_CODEC_CC_8CH;
+		hdmi_ca = AV8100_CODEC_CA_FL_FR_LFE_FC_RL_RR_RLC_RRC; /* 7.1 */
+		tx_mask = AV8100_CODEC_MASK_7DOT1;
+		break;
+	default:
+		pr_err("%s: Unsupported number of channels (channels = %d)!\n",
+			__func__,
+			channels);
+		return -EINVAL;
+	}
+
+	/* Change HDMI audio-settings for codec-DAI. */
+	pr_debug("%s: Change HDMI audio-settings for codec-DAI.\n", __func__);
+	as.audio_coding_type = AV8100_CODEC_CT_IEC60958_PCM;
+	as.audio_channel_count = hdmi_cc;
+	as.sampling_frequency = AV8100_CODEC_SF_48KHZ;
+	as.sample_size = AV8100_CODEC_SS_16BIT;
+	as.channel_allocation = hdmi_ca;
+	as.level_shift_value = AV8100_CODEC_LSV_0DB;
+	as.downmix_inhibit = false;
+	ret = av8100_audio_change_hdmi_audio_settings(codec_dai, &as);
+	if (ret < 0) {
+		pr_err("%s: Unable to change HDMI audio-settings for codec-DAI "
+			"(av8100_codec_change_hdmi_audio_settings returned %d)!\n",
+			__func__,
+			ret);
+		return ret;
+	}
+
+	/* Set format for codec-DAI */
+	fmt = SND_SOC_DAIFMT_DSP_B | SND_SOC_DAIFMT_CBM_CFM;
+	pr_debug("%s: Setting format for codec-DAI (fmt = %d).\n",
+		__func__,
+		fmt);
+	ret = snd_soc_dai_set_fmt(codec_dai, fmt);
+	if (ret < 0) {
+		pr_err("%s: Unable to set format for codec-DAI "
+			"(snd_soc_dai_set_tdm_slot returned %d)!\n",
+			__func__,
+			ret);
+		return ret;
+	}
+
+	/* Set TDM-slot for CPU-DAI */
+	pr_debug("%s: Setting TDM-slot for codec-DAI (tx_mask = %d).\n",
+		__func__,
+		tx_mask);
+	ret = snd_soc_dai_set_tdm_slot(cpu_dai, tx_mask, 0, 16, 16);
+	if (ret < 0) {
+		pr_err("%s: Unable to set TDM-slot for codec-DAI "
+			"(snd_soc_dai_set_tdm_slot returned %d)!\n",
+			__func__,
+			ret);
+		return ret;
+	}
+
+	/* Set format for CPU-DAI */
+	fmt = SND_SOC_DAIFMT_DSP_B |
+		SND_SOC_DAIFMT_CBM_CFM |
+		SND_SOC_DAIFMT_NB_IF;
+	pr_debug("%s: Setting DAI-format for Ux500-platform (fmt = %d).\n",
+		__func__,
+		fmt);
+	ret = snd_soc_dai_set_fmt(cpu_dai, fmt);
+	if (ret < 0) {
+		pr_err("%s: Unable to set DAI-format for Ux500-platform "
+			"(snd_soc_dai_set_fmt returned %d).\n",
+			__func__,
+			ret);
+		return ret;
+	}
+
+	ux500_msp_dai_set_data_delay(cpu_dai, MSP_DELAY_1);
+
+	return ret;
+}
+
+struct snd_soc_ops ux500_av8100_ops[] = {
+	{
+	.hw_params = ux500_av8100_hw_params,
+	}
+};
+
diff --git a/sound/soc/ux500/ux500_av8100.h b/sound/soc/ux500/ux500_av8100.h
new file mode 100644
index 00000000000..b107b2e1be7
--- /dev/null
+++ b/sound/soc/ux500/ux500_av8100.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 UX500_AV8100_H
+#define UX500_AV8100_H
+
+extern struct snd_soc_ops ux500_av8100_ops[];
+
+#endif
diff --git a/sound/soc/ux500/ux500_cg29xx.c b/sound/soc/ux500/ux500_cg29xx.c
new file mode 100644
index 00000000000..e83a6869c5a
--- /dev/null
+++ b/sound/soc/ux500/ux500_cg29xx.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>,
+ *         Roger Nilsson <roger.xr.nilsson@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 <sound/soc.h>
+#include "../codecs/cg29xx.h"
+#include "ux500_msp_dai.h"
+
+#define UX500_CG29XX_MSP_CLOCK_FREQ	18900000
+#define UX500_CG29XX_DAI_SLOT_WIDTH	16
+#define UX500_CG29XX_DAI_SLOTS	2
+#define UX500_CG29XX_DAI_ACTIVE_SLOTS	0x01
+
+int ux500_cg29xx_hw_params(struct snd_pcm_substream *substream,
+			struct snd_pcm_hw_params *params)
+{
+	struct snd_soc_pcm_runtime *rtd = substream->private_data;
+	struct snd_soc_dai *cpu_dai = rtd->cpu_dai;
+	struct snd_soc_dai *codec_dai = rtd->codec_dai;
+	int channels = params_channels(params);
+	int err;
+
+	pr_debug("%s: Enter.\n", __func__);
+	pr_debug("%s: substream->pcm->name = %s.\n", __func__, substream->pcm->name);
+	pr_debug("%s: substream->pcm->id = %s.\n", __func__, substream->pcm->id);
+	pr_debug("%s: substream->name = %s.\n", __func__, substream->name);
+	pr_debug("%s: substream->number = %d.\n", __func__, substream->number);
+	pr_debug("%s: channels = %d.\n", __func__, channels);
+	pr_debug("%s: DAI-index (Codec): %d\n", __func__, codec_dai->id);
+	pr_debug("%s: DAI-index (Platform): %d\n", __func__, cpu_dai->id);
+
+	err = snd_soc_dai_set_fmt(codec_dai,
+				SND_SOC_DAIFMT_DSP_A | SND_SOC_DAIFMT_CBS_CFS);
+
+	if (err) {
+		pr_err("%s: snd_soc_dai_set_fmt(codec) failed with %d.\n",
+			__func__,
+			err);
+		goto out_err;
+	}
+
+	err = snd_soc_dai_set_tdm_slot(codec_dai,
+				1 << CG29XX_DAI_SLOT0_SHIFT,
+				1 << CG29XX_DAI_SLOT0_SHIFT,
+				UX500_CG29XX_DAI_SLOTS,
+				UX500_CG29XX_DAI_SLOT_WIDTH);
+
+	if (err) {
+		pr_err("%s: cg29xx_set_tdm_slot(codec_dai) failed with %d.\n",
+				__func__,
+				err);
+		goto out_err;
+	}
+
+	err = snd_soc_dai_set_fmt(cpu_dai,
+				SND_SOC_DAIFMT_DSP_A | SND_SOC_DAIFMT_CBS_CFS |
+				SND_SOC_DAIFMT_NB_NF);
+
+	if (err) {
+		pr_err("%s: snd_soc_dai_set_fmt(cpu_dai) failed with %d.\n",
+			__func__,
+			err);
+		goto out_err;
+	}
+
+	err = snd_soc_dai_set_sysclk(cpu_dai,
+		UX500_MSP_MASTER_CLOCK,
+		UX500_CG29XX_MSP_CLOCK_FREQ,
+		0);
+
+	if (err) {
+		pr_err("%s: snd_soc_dai_set_sysclk(cpu_dai) failed with %d.\n",
+			__func__,
+			err);
+		goto out_err;
+	}
+
+	err = snd_soc_dai_set_tdm_slot(cpu_dai,
+				UX500_CG29XX_DAI_ACTIVE_SLOTS,
+				UX500_CG29XX_DAI_ACTIVE_SLOTS,
+				UX500_CG29XX_DAI_SLOTS,
+				UX500_CG29XX_DAI_SLOT_WIDTH);
+
+	if (err) {
+		pr_err("%s: cg29xx_set_tdm_slot(cpu_dai) failed with %d.\n",
+			__func__,
+			err);
+		goto out_err;
+	}
+
+out_err:
+	return err;
+}
+
+struct snd_soc_ops ux500_cg29xx_ops[] = {
+	{
+	.hw_params = ux500_cg29xx_hw_params,
+	}
+};
+
diff --git a/sound/soc/ux500/ux500_cg29xx.h b/sound/soc/ux500/ux500_cg29xx.h
new file mode 100644
index 00000000000..49a8c3e0478
--- /dev/null
+++ b/sound/soc/ux500/ux500_cg29xx.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 UX500_CG29XX_H
+#define UX500_CG29XX_H
+
+extern struct snd_soc_ops ux500_cg29xx_ops[];
+
+#endif
diff --git a/sound/soc/ux500/ux500_msp_dai.c b/sound/soc/ux500/ux500_msp_dai.c
new file mode 100644
index 00000000000..53f3982343b
--- /dev/null
+++ b/sound/soc/ux500/ux500_msp_dai.c
@@ -0,0 +1,1006 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>,
+ *         Roger Nilsson <roger.xr.nilsson@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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/slab.h>
+#include <asm/dma.h>
+#include <linux/bitops.h>
+#include <mach/hardware.h>
+#include <mach/msp.h>
+#include <linux/i2s/i2s.h>
+#include <asm/mach-types.h>
+
+#include <sound/soc.h>
+#include <sound/soc-dai.h>
+#include "ux500_msp_dai.h"
+#include "ux500_pcm.h"
+
+static struct ux500_platform_drvdata platform_drvdata[UX500_NBR_OF_DAI] = {
+	{
+		.i2s = NULL,
+		.fmt = 0,
+		.slots = 1,
+		.tx_mask = 0x01,
+		.rx_mask = 0x01,
+		.slot_width = 16,
+		.playback_active = false,
+		.capture_active = false,
+		.configured = 0,
+		.data_delay = MSP_DELAY_0,
+		.master_clk = UX500_MSP_U8500_INTERNAL_CLOCK_FREQ,
+	},
+	{
+		.i2s = NULL,
+		.fmt = 0,
+		.slots = 1,
+		.tx_mask = 0x01,
+		.rx_mask = 0x01,
+		.slot_width = 16,
+		.playback_active = false,
+		.capture_active = false,
+		.configured = 0,
+		.data_delay = MSP_DELAY_0,
+		.master_clk = UX500_MSP_U8500_INTERNAL_CLOCK_FREQ,
+	},
+	{
+		.i2s = NULL,
+		.fmt = 0,
+		.slots = 1,
+		.tx_mask = 0x01,
+		.rx_mask = 0x01,
+		.slot_width = 16,
+		.playback_active = false,
+		.capture_active = false,
+		.configured = 0,
+		.data_delay = MSP_DELAY_0,
+		.master_clk = UX500_MSP_U8500_INTERNAL_CLOCK_FREQ,
+	},
+	{
+		.i2s = NULL,
+		.fmt = 0,
+		.slots = 1,
+		.tx_mask = 0x01,
+		.rx_mask = 0x01,
+		.slot_width = 16,
+		.playback_active = false,
+		.capture_active = false,
+		.configured = 0,
+		.data_delay = MSP_DELAY_0,
+		.master_clk = UX500_MSP_U8500_INTERNAL_CLOCK_FREQ,
+	},
+};
+
+bool ux500_msp_dai_i2s_get_underrun_status(int dai_idx)
+{
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai_idx];
+	int status = i2s_hw_status(drvdata->i2s->controller);
+	return (bool)(status & TRANSMIT_UNDERRUN_ERR_INT);
+}
+
+dma_addr_t ux500_msp_dai_i2s_get_pointer(int dai_idx, int stream_id)
+{
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai_idx];
+	return i2s_get_pointer(drvdata->i2s->controller,
+			(stream_id == SNDRV_PCM_STREAM_PLAYBACK) ?
+				I2S_DIRECTION_TX :
+				I2S_DIRECTION_RX);
+}
+
+int ux500_msp_dai_i2s_configure_sg(dma_addr_t dma_addr,
+				int period_cnt,
+				size_t period_len,
+				int dai_idx,
+				int stream_id)
+{
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai_idx];
+	struct i2s_message message;
+	struct i2s_device *i2s_dev;
+	int ret = 0;
+	bool playback_req_valid =
+		(drvdata->playback_active &&
+			stream_id == SNDRV_PCM_STREAM_PLAYBACK);
+	bool capture_req_valid =
+		(drvdata->capture_active &&
+			stream_id == SNDRV_PCM_STREAM_CAPTURE);
+
+	pr_debug("%s: Enter (MSP Index: %u, period-cnt: %u, period-len: %u).\n",
+		__func__,
+		dai_idx,
+		period_cnt,
+		period_len);
+
+	if (!playback_req_valid && !capture_req_valid) {
+		pr_err("%s: The I2S controller is not available."
+			"MSP index:%d\n",
+			__func__,
+			dai_idx);
+		return ret;
+	}
+
+	i2s_dev = drvdata->i2s;
+
+	message.i2s_transfer_mode = I2S_TRANSFER_MODE_CYCLIC_DMA;
+	message.i2s_direction = (stream_id == SNDRV_PCM_STREAM_PLAYBACK) ?
+					I2S_DIRECTION_TX :
+					I2S_DIRECTION_RX;
+	message.buf_addr = dma_addr;
+	message.buf_len = period_cnt * period_len;
+	message.period_len = period_len;
+
+	ret = i2s_transfer(i2s_dev->controller, &message);
+	if (ret < 0) {
+		pr_err("%s: Error: i2s_transfer failed. MSP index: %d\n",
+			__func__,
+			dai_idx);
+	}
+
+	return ret;
+}
+
+static const char *stream_str(struct snd_pcm_substream *substream)
+{
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		return "Playback";
+	else
+		return "Capture";
+}
+
+static int ux500_msp_dai_startup(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *dai)
+{
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai->id];
+	bool mode_playback = (substream->stream == SNDRV_PCM_STREAM_PLAYBACK);
+
+	pr_debug("%s: MSP %d (%s): Enter.\n", __func__, dai->id, stream_str(substream));
+
+	if ((mode_playback && drvdata->playback_active) ||
+		(!mode_playback && drvdata->capture_active)) {
+		pr_err("%s: Error: MSP %d (%s): Stream already active.\n",
+			__func__,
+			dai->id,
+			stream_str(substream));
+		return -EBUSY;
+	}
+
+	if (mode_playback)
+		drvdata->playback_active = true;
+	else
+		drvdata->capture_active = true;
+
+	return 0;
+}
+
+static void ux500_msp_dai_shutdown(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *dai)
+{
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai->id];
+	bool mode_playback = (substream->stream == SNDRV_PCM_STREAM_PLAYBACK);
+
+	pr_debug("%s: MSP %d (%s): Enter.\n", __func__, dai->id, stream_str(substream));
+
+	if (drvdata == NULL)
+		return;
+
+	if (mode_playback)
+		drvdata->playback_active = false;
+	else
+		drvdata->capture_active = false;
+
+	if (i2s_cleanup(drvdata->i2s->controller,
+			mode_playback ? DISABLE_TRANSMIT : DISABLE_RECEIVE)) {
+			pr_err("%s: Error: MSP %d (%s): Unable to close i2s.\n",
+				__func__,
+				dai->id,
+				stream_str(substream));
+	}
+
+	if (mode_playback)
+		drvdata->configured &= ~PLAYBACK_CONFIGURED;
+	else
+		drvdata->configured &= ~CAPTURE_CONFIGURED;
+}
+
+static void ux500_msp_dai_setup_multichannel(struct ux500_platform_drvdata *private,
+					struct msp_config *msp_config)
+{
+	struct msp_multichannel_config *multi =	&msp_config->multichannel_config;
+
+	if (private->slots > 1) {
+		msp_config->multichannel_configured = 1;
+
+		multi->tx_multichannel_enable = true;
+		multi->rx_multichannel_enable = true;
+		multi->rx_comparison_enable_mode = MSP_COMPARISON_DISABLED;
+
+		multi->tx_channel_0_enable = private->tx_mask;
+		multi->tx_channel_1_enable = 0;
+		multi->tx_channel_2_enable = 0;
+		multi->tx_channel_3_enable = 0;
+
+		multi->rx_channel_0_enable = private->rx_mask;
+		multi->rx_channel_1_enable = 0;
+		multi->rx_channel_2_enable = 0;
+		multi->rx_channel_3_enable = 0;
+
+		pr_debug("%s: Multichannel enabled."
+			"Slots: %d TX: %u RX: %u\n",
+			__func__,
+			private->slots,
+			multi->tx_channel_0_enable,
+			multi->rx_channel_0_enable);
+	}
+}
+
+static void ux500_msp_dai_setup_frameper(struct ux500_platform_drvdata *private,
+					unsigned int rate,
+					struct msp_protocol_desc *prot_desc)
+{
+	switch (private->slots) {
+	default:
+	case 1:
+		switch (rate) {
+		case 8000:
+			prot_desc->frame_period =
+				FRAME_PER_SINGLE_SLOT_8_KHZ;
+			break;
+		case 16000:
+			prot_desc->frame_period =
+				FRAME_PER_SINGLE_SLOT_16_KHZ;
+			break;
+		case 44100:
+			prot_desc->frame_period =
+				FRAME_PER_SINGLE_SLOT_44_1_KHZ;
+			break;
+		case 48000:
+		default:
+			prot_desc->frame_period =
+				FRAME_PER_SINGLE_SLOT_48_KHZ;
+			break;
+		}
+		break;
+
+	case 2:
+		prot_desc->frame_period = FRAME_PER_2_SLOTS;
+		break;
+
+	case 8:
+		prot_desc->frame_period =
+			FRAME_PER_8_SLOTS;
+		break;
+
+	case 16:
+		prot_desc->frame_period =
+			FRAME_PER_16_SLOTS;
+		break;
+	}
+
+	prot_desc->total_clocks_for_one_frame =
+			prot_desc->frame_period+1;
+
+	pr_debug("%s: Total clocks per frame: %u\n",
+		__func__,
+		prot_desc->total_clocks_for_one_frame);
+}
+
+static void ux500_msp_dai_setup_framing_pcm(struct ux500_platform_drvdata *private,
+					unsigned int rate,
+					struct msp_protocol_desc *prot_desc)
+{
+	u32 frame_length = MSP_FRAME_LENGTH_1;
+	prot_desc->frame_width = 0;
+
+	switch (private->slots) {
+	default:
+	case 1:
+		frame_length = MSP_FRAME_LENGTH_1;
+		break;
+
+	case 2:
+		frame_length = MSP_FRAME_LENGTH_2;
+		break;
+
+	case 8:
+		frame_length = MSP_FRAME_LENGTH_8;
+		break;
+
+	case 16:
+		frame_length = MSP_FRAME_LENGTH_16;
+		break;
+	}
+
+	prot_desc->tx_frame_length_1 = frame_length;
+	prot_desc->rx_frame_length_1 = frame_length;
+	prot_desc->tx_frame_length_2 = frame_length;
+	prot_desc->rx_frame_length_2 = frame_length;
+
+	prot_desc->tx_element_length_1 = MSP_ELEM_LENGTH_16;
+	prot_desc->rx_element_length_1 = MSP_ELEM_LENGTH_16;
+	prot_desc->tx_element_length_2 = MSP_ELEM_LENGTH_16;
+	prot_desc->rx_element_length_2 = MSP_ELEM_LENGTH_16;
+
+	ux500_msp_dai_setup_frameper(private, rate, prot_desc);
+}
+
+static void ux500_msp_dai_setup_clocking(unsigned int fmt,
+					struct msp_config *msp_config)
+{
+
+	switch (fmt & SND_SOC_DAIFMT_INV_MASK) {
+	default:
+	case SND_SOC_DAIFMT_NB_NF:
+		msp_config->tx_frame_sync_pol =
+			MSP_FRAME_SYNC_POL(MSP_FRAME_SYNC_POL_ACTIVE_HIGH);
+		msp_config->rx_frame_sync_pol =
+			MSP_FRAME_SYNC_POL_ACTIVE_HIGH << RFSPOL_SHIFT;
+		break;
+
+	case SND_SOC_DAIFMT_NB_IF:
+		msp_config->tx_frame_sync_pol =
+			MSP_FRAME_SYNC_POL(MSP_FRAME_SYNC_POL_ACTIVE_LOW);
+		msp_config->rx_frame_sync_pol =
+			MSP_FRAME_SYNC_POL_ACTIVE_LOW << RFSPOL_SHIFT;
+		break;
+	}
+
+	if ((fmt & SND_SOC_DAIFMT_MASTER_MASK) == SND_SOC_DAIFMT_CBM_CFM) {
+		pr_debug("%s: Codec is MASTER.\n",
+			__func__);
+
+		msp_config->rx_frame_sync_sel = 0;
+		msp_config->tx_frame_sync_sel = 1 << TFSSEL_SHIFT;
+		msp_config->tx_clock_sel = 0;
+		msp_config->rx_clock_sel = 0;
+		msp_config->srg_clock_sel = 0x2 << SCKSEL_SHIFT;
+	} else {
+		pr_debug("%s: Codec is SLAVE.\n",
+			__func__);
+
+		msp_config->tx_clock_sel = TX_CLK_SEL_SRG;
+		msp_config->tx_frame_sync_sel = TX_SYNC_SRG_PROG;
+		msp_config->rx_clock_sel = RX_CLK_SEL_SRG;
+		msp_config->rx_frame_sync_sel = RX_SYNC_SRG;
+		msp_config->srg_clock_sel = 1 << SCKSEL_SHIFT;
+	}
+}
+
+static void ux500_msp_dai_compile_prot_desc_pcm(unsigned int fmt,
+					struct msp_protocol_desc *prot_desc)
+{
+	prot_desc->rx_phase_mode = MSP_SINGLE_PHASE;
+	prot_desc->tx_phase_mode = MSP_SINGLE_PHASE;
+	prot_desc->rx_phase2_start_mode = MSP_PHASE2_START_MODE_IMEDIATE;
+	prot_desc->tx_phase2_start_mode = MSP_PHASE2_START_MODE_IMEDIATE;
+	prot_desc->rx_bit_transfer_format = MSP_BTF_MS_BIT_FIRST;
+	prot_desc->tx_bit_transfer_format = MSP_BTF_MS_BIT_FIRST;
+
+	if ((fmt & SND_SOC_DAIFMT_FORMAT_MASK) == SND_SOC_DAIFMT_DSP_A) {
+		pr_debug("%s: DSP_A.\n",
+			__func__);
+		prot_desc->tx_clock_pol = MSP_FALLING_EDGE;
+		prot_desc->rx_clock_pol = MSP_FALLING_EDGE;
+	} else {
+		pr_debug("%s: DSP_B.\n",
+			__func__);
+		prot_desc->tx_clock_pol = MSP_RISING_EDGE;
+		prot_desc->rx_clock_pol = MSP_RISING_EDGE;
+	}
+
+	prot_desc->rx_half_word_swap = MSP_HWS_NO_SWAP;
+	prot_desc->tx_half_word_swap = MSP_HWS_NO_SWAP;
+	prot_desc->compression_mode = MSP_COMPRESS_MODE_LINEAR;
+	prot_desc->expansion_mode = MSP_EXPAND_MODE_LINEAR;
+	prot_desc->spi_clk_mode = MSP_SPI_CLOCK_MODE_NON_SPI;
+	prot_desc->spi_burst_mode = MSP_SPI_BURST_MODE_DISABLE;
+	prot_desc->frame_sync_ignore = MSP_FRAME_SYNC_IGNORE;
+}
+
+static void ux500_msp_dai_compile_prot_desc_i2s(struct msp_protocol_desc *prot_desc)
+{
+	prot_desc->rx_phase_mode = MSP_DUAL_PHASE;
+	prot_desc->tx_phase_mode = MSP_DUAL_PHASE;
+	prot_desc->rx_phase2_start_mode =
+		MSP_PHASE2_START_MODE_FRAME_SYNC;
+	prot_desc->tx_phase2_start_mode =
+		MSP_PHASE2_START_MODE_FRAME_SYNC;
+	prot_desc->rx_bit_transfer_format = MSP_BTF_MS_BIT_FIRST;
+	prot_desc->tx_bit_transfer_format = MSP_BTF_MS_BIT_FIRST;
+
+	prot_desc->rx_frame_length_1 = MSP_FRAME_LENGTH_1;
+	prot_desc->rx_frame_length_2 = MSP_FRAME_LENGTH_1;
+	prot_desc->tx_frame_length_1 = MSP_FRAME_LENGTH_1;
+	prot_desc->tx_frame_length_2 = MSP_FRAME_LENGTH_1;
+	prot_desc->rx_element_length_1 = MSP_ELEM_LENGTH_16;
+	prot_desc->rx_element_length_2 = MSP_ELEM_LENGTH_16;
+	prot_desc->tx_element_length_1 = MSP_ELEM_LENGTH_16;
+	prot_desc->tx_element_length_2 = MSP_ELEM_LENGTH_16;
+
+	prot_desc->rx_clock_pol = MSP_RISING_EDGE;
+	prot_desc->tx_clock_pol = MSP_RISING_EDGE;
+
+	prot_desc->tx_half_word_swap = MSP_HWS_NO_SWAP;
+	prot_desc->rx_half_word_swap = MSP_HWS_NO_SWAP;
+	prot_desc->compression_mode = MSP_COMPRESS_MODE_LINEAR;
+	prot_desc->expansion_mode = MSP_EXPAND_MODE_LINEAR;
+	prot_desc->spi_clk_mode = MSP_SPI_CLOCK_MODE_NON_SPI;
+	prot_desc->spi_burst_mode = MSP_SPI_BURST_MODE_DISABLE;
+	prot_desc->frame_sync_ignore = MSP_FRAME_SYNC_IGNORE;
+}
+
+static void ux500_msp_dai_compile_msp_config(struct snd_pcm_substream *substream,
+					struct ux500_platform_drvdata *private,
+					unsigned int rate,
+					struct msp_config *msp_config)
+{
+	struct msp_protocol_desc *prot_desc = &msp_config->protocol_desc;
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	unsigned int fmt = private->fmt;
+
+	memset(msp_config, 0, sizeof(*msp_config));
+
+	if (machine_is_u5500())
+		msp_config->input_clock_freq = UX500_MSP_U5500_INTERNAL_CLOCK_FREQ;
+	else
+		msp_config->input_clock_freq = private->master_clk;
+
+	msp_config->tx_fifo_config = TX_FIFO_ENABLE;
+	msp_config->rx_fifo_config = RX_FIFO_ENABLE;
+	msp_config->spi_clk_mode = SPI_CLK_MODE_NORMAL;
+	msp_config->spi_burst_mode = 0;
+	msp_config->handler = ux500_pcm_dma_eot_handler;
+	msp_config->tx_callback_data =
+		substream->stream == SNDRV_PCM_STREAM_PLAYBACK ?
+		substream : NULL;
+	msp_config->rx_callback_data =
+		substream->stream == SNDRV_PCM_STREAM_CAPTURE ?
+		substream : NULL;
+	msp_config->def_elem_len = 1;
+	msp_config->direction =
+		substream->stream == SNDRV_PCM_STREAM_PLAYBACK ?
+		MSP_TRANSMIT_MODE : MSP_RECEIVE_MODE;
+	msp_config->data_size = MSP_DATA_BITS_32;
+	msp_config->work_mode = MSP_DMA_MODE;
+	msp_config->frame_freq = rate;
+
+	printk(KERN_INFO "%s: input_clock_freq = %u, frame_freq = %u.\n",
+	       __func__, msp_config->input_clock_freq, msp_config->frame_freq);
+	/* To avoid division by zero in I2S-driver (i2s_setup) */
+	prot_desc->total_clocks_for_one_frame = 1;
+
+	prot_desc->rx_data_delay = private->data_delay;
+	prot_desc->tx_data_delay = private->data_delay;
+
+	pr_debug("%s: rate: %u channels: %d.\n",
+			__func__,
+			rate,
+			runtime->channels);
+	switch (fmt &
+		(SND_SOC_DAIFMT_FORMAT_MASK | SND_SOC_DAIFMT_MASTER_MASK)) {
+
+	case SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBS_CFS:
+		pr_debug("%s: SND_SOC_DAIFMT_I2S.\n",
+			__func__);
+
+		msp_config->default_protocol_desc = 1;
+		msp_config->protocol = MSP_I2S_PROTOCOL;
+		break;
+
+	default:
+	case SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBM_CFM:
+		pr_debug("%s: SND_SOC_DAIFMT_I2S.\n",
+			__func__);
+
+		msp_config->data_size = MSP_DATA_BITS_16;
+		msp_config->protocol = MSP_I2S_PROTOCOL;
+
+		ux500_msp_dai_compile_prot_desc_i2s(prot_desc);
+		break;
+
+	case SND_SOC_DAIFMT_DSP_A | SND_SOC_DAIFMT_CBS_CFS:
+	case SND_SOC_DAIFMT_DSP_A | SND_SOC_DAIFMT_CBM_CFM:
+	case SND_SOC_DAIFMT_DSP_B | SND_SOC_DAIFMT_CBS_CFS:
+	case SND_SOC_DAIFMT_DSP_B | SND_SOC_DAIFMT_CBM_CFM:
+		pr_debug("%s: PCM format.\n",
+			__func__);
+		msp_config->data_size = MSP_DATA_BITS_16;
+		msp_config->protocol = MSP_PCM_PROTOCOL;
+
+		ux500_msp_dai_compile_prot_desc_pcm(fmt, prot_desc);
+		ux500_msp_dai_setup_multichannel(private, msp_config);
+		ux500_msp_dai_setup_framing_pcm(private, rate, prot_desc);
+		break;
+	}
+
+	ux500_msp_dai_setup_clocking(fmt, msp_config);
+}
+
+static int ux500_msp_dai_prepare(struct snd_pcm_substream *substream,
+				struct snd_soc_dai *dai)
+{
+	int ret = 0;
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai->id];
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct msp_config msp_config;
+	bool mode_playback = (substream->stream == SNDRV_PCM_STREAM_PLAYBACK);
+
+	pr_debug("%s: MSP %d (%s): Enter.\n", __func__, dai->id, stream_str(substream));
+
+	/* If already configured -> not errors reported */
+	if (mode_playback) {
+		if ((drvdata->configured & PLAYBACK_CONFIGURED) &&
+			(drvdata->playback_active))
+			goto cleanup;
+	} else {
+		if ((drvdata->configured & CAPTURE_CONFIGURED) &&
+			(drvdata->capture_active))
+			goto cleanup;
+	}
+
+	pr_debug("%s: Setup dai (Rate: %u).\n", __func__, runtime->rate);
+	ux500_msp_dai_compile_msp_config(substream,
+					drvdata,
+					runtime->rate,
+					&msp_config);
+
+	ret = i2s_setup(drvdata->i2s->controller, &msp_config);
+	if (ret < 0) {
+		pr_err("%s: Error: i2s_setup failed (ret = %d)!\n", __func__, ret);
+		goto cleanup;
+	}
+
+	drvdata->configured |= mode_playback ?
+			PLAYBACK_CONFIGURED : CAPTURE_CONFIGURED;
+
+cleanup:
+	return ret;
+}
+
+static int ux500_msp_dai_hw_params(struct snd_pcm_substream *substream,
+				struct snd_pcm_hw_params *params,
+				struct snd_soc_dai *dai)
+{
+	unsigned int mask, slots_active;
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai->id];
+
+	pr_debug("%s: MSP %d (%s): Enter.\n",
+			__func__,
+			dai->id,
+			stream_str(substream));
+
+	switch (drvdata->fmt & SND_SOC_DAIFMT_FORMAT_MASK) {
+	case SND_SOC_DAIFMT_I2S:
+		if (params_channels(params) != 2) {
+			pr_err("%s: Error: I2S requires channels = 2 "
+				"(channels = %d)!\n",
+				__func__,
+				params_channels(params));
+			return -EINVAL;
+		}
+		break;
+	case SND_SOC_DAIFMT_DSP_B:
+	case SND_SOC_DAIFMT_DSP_A:
+
+		mask = substream->stream == SNDRV_PCM_STREAM_PLAYBACK ?
+			drvdata->tx_mask :
+			drvdata->rx_mask;
+
+		slots_active = hweight32(mask);
+
+		pr_debug("TDM slots active: %d", slots_active);
+
+		if (params_channels(params) != slots_active) {
+			pr_err("%s: Error: PCM TDM format requires channels "
+				"to match active slots "
+				"(channels = %d, active slots = %d)!\n",
+				__func__,
+				params_channels(params),
+				slots_active);
+			return -EINVAL;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+int ux500_msp_dai_set_data_delay(struct snd_soc_dai *dai, int delay)
+{
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai->id];
+
+	pr_debug("%s: MSP %d: Enter.\n", __func__, dai->id);
+
+	switch (delay) {
+	case MSP_DELAY_0:
+	case MSP_DELAY_1:
+	case MSP_DELAY_2:
+	case MSP_DELAY_3:
+		break;
+	default:
+		goto unsupported_delay;
+	}
+
+	drvdata->data_delay = delay;
+	return 0;
+
+unsupported_delay:
+	pr_err("%s: MSP %d: Error: Unsupported DAI delay (%d)!\n",
+		__func__,
+		dai->id,
+		delay);
+	return -EINVAL;
+}
+
+static int ux500_msp_dai_set_dai_fmt(struct snd_soc_dai *dai,
+				unsigned int fmt)
+{
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai->id];
+
+	pr_debug("%s: MSP %d: Enter.\n", __func__, dai->id);
+
+	switch (fmt & (SND_SOC_DAIFMT_FORMAT_MASK | SND_SOC_DAIFMT_MASTER_MASK)) {
+	case SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBS_CFS:
+	case SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBM_CFM:
+	case SND_SOC_DAIFMT_DSP_B | SND_SOC_DAIFMT_CBS_CFS:
+	case SND_SOC_DAIFMT_DSP_B | SND_SOC_DAIFMT_CBM_CFM:
+	case SND_SOC_DAIFMT_DSP_A | SND_SOC_DAIFMT_CBS_CFS:
+	case SND_SOC_DAIFMT_DSP_A | SND_SOC_DAIFMT_CBM_CFM:
+		break;
+
+	default:
+		goto unsupported_format;
+	}
+
+	switch (fmt & SND_SOC_DAIFMT_INV_MASK) {
+	case SND_SOC_DAIFMT_NB_NF:
+	case SND_SOC_DAIFMT_NB_IF:
+		break;
+
+	default:
+		goto unsupported_format;
+	}
+
+	drvdata->fmt = fmt;
+	return 0;
+
+unsupported_format:
+	pr_err("%s: MSP %d: Error: Unsupported DAI format (0x%x)!\n",
+		__func__,
+		dai->id,
+		fmt);
+	return -EINVAL;
+}
+
+static int ux500_msp_dai_set_tdm_slot(struct snd_soc_dai *dai,
+				unsigned int tx_mask,
+				unsigned int rx_mask,
+				int slots,
+				int slot_width)
+{
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai->id];
+	unsigned int cap;
+
+	if (!(slots == 1 || slots == 2 || slots == 8 || slots == 16)) {
+		pr_err("%s: Error: Unsupported slots (%d)! "
+			"Supported values are 1/2/8/16.\n",
+			__func__,
+			slots);
+		return -EINVAL;
+	}
+	drvdata->slots = slots;
+
+	if (!(slot_width == 16)) {
+		pr_err("%s: Error: Unsupported slots_width (%d)!. "
+			"Supported value is 16.\n",
+			__func__,
+			slot_width);
+		return -EINVAL;
+	}
+	drvdata->slot_width = slot_width;
+
+	switch (slots) {
+	default:
+	case 1:
+		cap = 0x01;
+		break;
+	case 2:
+		cap = 0x03;
+		break;
+	case 8:
+		cap = 0xFF;
+		break;
+	case 16:
+		cap = 0xFFFF;
+		break;
+	}
+
+	drvdata->tx_mask = tx_mask & cap;
+	drvdata->rx_mask = rx_mask & cap;
+
+	return 0;
+}
+
+static int ux500_msp_dai_set_dai_sysclk(struct snd_soc_dai *dai,
+	int clk_id,
+	unsigned int freq,
+	int dir)
+{
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai->id];
+
+	pr_debug("%s: MSP %d: Enter. Clk id: %d, freq: %u.\n",
+		__func__,
+		dai->id,
+		clk_id,
+		freq);
+
+	switch (clk_id) {
+	case UX500_MSP_MASTER_CLOCK:
+		drvdata->master_clk = freq;
+		break;
+
+	default:
+		pr_err("%s: MSP %d: Invalid clkid: %d.\n",
+			__func__,
+			dai->id,
+			clk_id);
+	}
+
+	return 0;
+}
+
+static int ux500_msp_dai_trigger(struct snd_pcm_substream *substream,
+				int cmd,
+				struct snd_soc_dai *dai)
+{
+	int ret = 0;
+	struct ux500_platform_drvdata *drvdata = &platform_drvdata[dai->id];
+
+	pr_debug("%s: MSP %d (%s): Enter (chip_select = %d, cmd = %d).\n",
+		__func__,
+		dai->id,
+		stream_str(substream),
+		(int)drvdata->i2s->chip_select,
+		cmd);
+
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_START:
+		ret = 0;
+		break;
+	case SNDRV_PCM_TRIGGER_RESUME:
+	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
+		ret = 0;
+		break;
+	case SNDRV_PCM_TRIGGER_STOP:
+	case SNDRV_PCM_TRIGGER_SUSPEND:
+		ret = 0;
+		break;
+	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
+		ret = 0;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static struct snd_soc_dai_driver ux500_msp_dai_drv[UX500_NBR_OF_DAI] = {
+	{
+		.name = "ux500-msp.0",
+		.id = 0,
+		.suspend = NULL,
+		.resume = NULL,
+		.playback = {
+			.channels_min = UX500_MSP_MIN_CHANNELS,
+			.channels_max = UX500_MSP_MAX_CHANNELS,
+			.rates = UX500_I2S_RATES,
+			.formats = UX500_I2S_FORMATS,
+		},
+		.capture = {
+			.channels_min = UX500_MSP_MIN_CHANNELS,
+			.channels_max = UX500_MSP_MAX_CHANNELS,
+			.rates = UX500_I2S_RATES,
+			.formats = UX500_I2S_FORMATS,
+		},
+		.ops = (struct snd_soc_dai_ops[]) {
+			{
+			.set_sysclk = ux500_msp_dai_set_dai_sysclk,
+			.set_fmt = ux500_msp_dai_set_dai_fmt,
+			.set_tdm_slot = ux500_msp_dai_set_tdm_slot,
+			.startup = ux500_msp_dai_startup,
+			.shutdown = ux500_msp_dai_shutdown,
+			.prepare = ux500_msp_dai_prepare,
+			.trigger = ux500_msp_dai_trigger,
+			.hw_params = ux500_msp_dai_hw_params,
+			}
+		},
+	},
+	{
+		.name = "ux500-msp.1",
+		.id = 1,
+		.suspend = NULL,
+		.resume = NULL,
+		.playback = {
+			.channels_min = UX500_MSP_MIN_CHANNELS,
+			.channels_max = UX500_MSP_MAX_CHANNELS,
+			.rates = UX500_I2S_RATES,
+			.formats = UX500_I2S_FORMATS,
+		},
+		.capture = {
+			.channels_min = UX500_MSP_MIN_CHANNELS,
+			.channels_max = UX500_MSP_MAX_CHANNELS,
+			.rates = UX500_I2S_RATES,
+			.formats = UX500_I2S_FORMATS,
+		},
+		.ops = (struct snd_soc_dai_ops[]) {
+			{
+			.set_sysclk = ux500_msp_dai_set_dai_sysclk,
+			.set_fmt = ux500_msp_dai_set_dai_fmt,
+			.set_tdm_slot = ux500_msp_dai_set_tdm_slot,
+			.startup = ux500_msp_dai_startup,
+			.shutdown = ux500_msp_dai_shutdown,
+			.prepare = ux500_msp_dai_prepare,
+			.trigger = ux500_msp_dai_trigger,
+			.hw_params = ux500_msp_dai_hw_params,
+			}
+		},
+	},
+	{
+		.name = "ux500-msp.2",
+		.id = 2,
+		.suspend = NULL,
+		.resume = NULL,
+		.playback = {
+			.channels_min = UX500_MSP_MIN_CHANNELS,
+			.channels_max = UX500_MSP_MAX_CHANNELS,
+			.rates = UX500_I2S_RATES,
+			.formats = UX500_I2S_FORMATS,
+		},
+		.capture = {
+			.channels_min = UX500_MSP_MIN_CHANNELS,
+			.channels_max = UX500_MSP_MAX_CHANNELS,
+			.rates = UX500_I2S_RATES,
+			.formats = UX500_I2S_FORMATS,
+		},
+		.ops = (struct snd_soc_dai_ops[]) {
+			{
+			.set_sysclk = ux500_msp_dai_set_dai_sysclk,
+			.set_fmt = ux500_msp_dai_set_dai_fmt,
+			.set_tdm_slot = ux500_msp_dai_set_tdm_slot,
+			.startup = ux500_msp_dai_startup,
+			.shutdown = ux500_msp_dai_shutdown,
+			.prepare = ux500_msp_dai_prepare,
+			.trigger = ux500_msp_dai_trigger,
+			.hw_params = ux500_msp_dai_hw_params,
+			}
+		},
+	},
+	{
+		.name = "ux500-msp.3",
+		.id = 3,
+		.suspend = NULL,
+		.resume = NULL,
+		.playback = {
+			.channels_min = UX500_MSP_MIN_CHANNELS,
+			.channels_max = UX500_MSP_MAX_CHANNELS,
+			.rates = UX500_I2S_RATES,
+			.formats = UX500_I2S_FORMATS,
+		},
+		.capture = {
+			.channels_min = UX500_MSP_MIN_CHANNELS,
+			.channels_max = UX500_MSP_MAX_CHANNELS,
+			.rates = UX500_I2S_RATES,
+			.formats = UX500_I2S_FORMATS,
+		},
+		.ops = (struct snd_soc_dai_ops[]) {
+			{
+			.set_sysclk = ux500_msp_dai_set_dai_sysclk,
+			.set_fmt = ux500_msp_dai_set_dai_fmt,
+			.set_tdm_slot = ux500_msp_dai_set_tdm_slot,
+			.startup = ux500_msp_dai_startup,
+			.shutdown = ux500_msp_dai_shutdown,
+			.prepare = ux500_msp_dai_prepare,
+			.trigger = ux500_msp_dai_trigger,
+			.hw_params = ux500_msp_dai_hw_params,
+			}
+		},
+	},
+};
+EXPORT_SYMBOL(ux500_msp_dai_drv);
+
+static int ux500_msp_drv_probe(struct i2s_device *i2s_dev)
+{
+	int ret = 0;
+	struct ux500_platform_drvdata *drvdata;
+	int msp_idx = i2s_dev->chip_select;
+
+	pr_info("%s: Enter (idx: %d, dev-name: %s, drv-name: %s).\n",
+		__func__,
+		msp_idx,
+		dev_name(&i2s_dev->dev),
+		i2s_dev->dev.driver->name);
+
+	drvdata = &platform_drvdata[msp_idx];
+	drvdata->i2s = i2s_dev;
+
+	try_module_get(i2s_dev->controller->dev.parent->driver->owner);
+	i2s_set_drvdata(i2s_dev, drvdata);
+
+	pr_debug("%s: Register MSP %d.\n", __func__, msp_idx);
+	ret = snd_soc_register_dai(&i2s_dev->dev, &ux500_msp_dai_drv[msp_idx]);
+	if (ret < 0) {
+		pr_err("Error: %s: Failed to register MSP %d.\n", __func__, msp_idx);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int ux500_msp_drv_remove(struct i2s_device *i2s_dev)
+{
+	struct ux500_platform_drvdata *drvdata = i2s_get_drvdata(i2s_dev);
+	int msp_idx = i2s_dev->chip_select;
+
+	pr_info("%s: Enter (idx: %d, dev-name: %s, drv-name: %s).\n",
+		__func__,
+		msp_idx,
+		dev_name(&i2s_dev->dev),
+		i2s_dev->dev.driver->name);
+
+	drvdata->i2s = NULL;
+	i2s_set_drvdata(i2s_dev, NULL);
+
+	pr_debug("%s: Calling module_put.\n", __func__);
+	module_put(i2s_dev->controller->dev.parent->driver->owner);
+
+	pr_debug("%s: Unregister ux500-pcm SoC platform driver.\n", __func__);
+	snd_soc_unregister_dais(&i2s_dev->dev, ARRAY_SIZE(ux500_msp_dai_drv));
+
+	return 0;
+}
+
+static const struct i2s_device_id dev_id_table[] = {
+	{ "i2s_device.0", 0, 0 },
+	{ "i2s_device.1", 1, 0 },
+	{ "i2s_device.2", 2, 0 },
+	{ "i2s_device.3", 3, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2s, dev_id_table);
+
+static struct i2s_driver i2sdrv_i2s = {
+	.driver = {
+		.name = "i2s",
+		.owner = THIS_MODULE,
+	},
+	.probe = ux500_msp_drv_probe,
+	.remove = __devexit_p(ux500_msp_drv_remove),
+	.id_table = dev_id_table,
+};
+
+static int __init ux500_msp_init(void)
+{
+	return i2s_register_driver(&i2sdrv_i2s);
+}
+
+static void __exit ux500_msp_exit(void)
+{
+	i2s_unregister_driver(&i2sdrv_i2s);
+}
+
+module_init(ux500_msp_init);
+module_exit(ux500_msp_exit);
+
+MODULE_LICENSE("GPLv2");
diff --git a/sound/soc/ux500/ux500_msp_dai.h b/sound/soc/ux500/ux500_msp_dai.h
new file mode 100644
index 00000000000..64a23506bca
--- /dev/null
+++ b/sound/soc/ux500/ux500_msp_dai.h
@@ -0,0 +1,79 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>,
+ *         Roger Nilsson <roger.xr.nilsson@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 UX500_msp_dai_H
+#define UX500_msp_dai_H
+
+#include <linux/types.h>
+#include <linux/spinlock.h>
+#include <linux/i2s/i2s.h>
+#include <mach/msp.h>
+
+#define UX500_NBR_OF_DAI	4
+
+#define UX500_I2S_RATES (SNDRV_PCM_RATE_8000 | SNDRV_PCM_RATE_16000 |	\
+			SNDRV_PCM_RATE_44100 | SNDRV_PCM_RATE_48000)
+
+#define UX500_I2S_FORMATS (SNDRV_PCM_FMTBIT_S16_LE)
+
+#define FRAME_PER_SINGLE_SLOT_8_KHZ		31
+#define FRAME_PER_SINGLE_SLOT_16_KHZ	124
+#define FRAME_PER_SINGLE_SLOT_44_1_KHZ	63
+#define FRAME_PER_SINGLE_SLOT_48_KHZ	49
+#define FRAME_PER_2_SLOTS				31
+#define FRAME_PER_8_SLOTS				138
+#define FRAME_PER_16_SLOTS				277
+
+#define UX500_MSP_U5500_INTERNAL_CLOCK_FREQ 13000000
+#define UX500_MSP_U8500_INTERNAL_CLOCK_FREQ 40000000
+
+#define UX500_MSP_MIN_CHANNELS		1
+#define UX500_MSP_MAX_CHANNELS		8
+
+#define PLAYBACK_CONFIGURED		1
+#define CAPTURE_CONFIGURED		2
+
+enum ux500_msp_clock_id {
+	UX500_MSP_MASTER_CLOCK,
+};
+
+struct ux500_platform_drvdata {
+	struct i2s_device *i2s;
+	unsigned int fmt;
+	unsigned int tx_mask;
+	unsigned int rx_mask;
+	int slots;
+	int slot_width;
+	bool playback_active;
+	bool capture_active;
+	u8 configured;
+	int data_delay;
+	unsigned int master_clk;
+};
+
+extern struct snd_soc_dai ux500_msp_dai[UX500_NBR_OF_DAI];
+
+bool ux500_msp_dai_i2s_get_underrun_status(int dai_idx);
+dma_addr_t ux500_msp_dai_i2s_get_pointer(int dai_idx, int stream_id);
+int ux500_msp_dai_i2s_configure_sg(dma_addr_t dma_addr,
+				int perod_cnt,
+				size_t period_len,
+				int dai_idx,
+				int stream_id);
+int ux500_msp_dai_i2s_send_data(void *data, size_t bytes, int dai_idx);
+int ux500_msp_dai_i2s_receive_data(void *data, size_t bytes, int dai_idx);
+
+int ux500_msp_dai_set_data_delay(struct snd_soc_dai *dai, int delay);
+
+#endif
diff --git a/sound/soc/ux500/ux500_pcm.c b/sound/soc/ux500/ux500_pcm.c
new file mode 100644
index 00000000000..ee953f1b5e5
--- /dev/null
+++ b/sound/soc/ux500/ux500_pcm.c
@@ -0,0 +1,429 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>,
+ *         Roger Nilsson <roger.xr.nilsson@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 <asm/page.h>
+
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <sound/soc.h>
+
+#include "ux500_pcm.h"
+#include "ux500_msp_dai.h"
+
+static struct snd_pcm_hardware ux500_pcm_hw_playback = {
+	.info = SNDRV_PCM_INFO_INTERLEAVED |
+		SNDRV_PCM_INFO_MMAP |
+		SNDRV_PCM_INFO_RESUME |
+		SNDRV_PCM_INFO_PAUSE,
+	.formats = SNDRV_PCM_FMTBIT_S16_LE |
+		SNDRV_PCM_FMTBIT_U16_LE |
+		SNDRV_PCM_FMTBIT_S16_BE |
+		SNDRV_PCM_FMTBIT_U16_BE,
+	.rates = SNDRV_PCM_RATE_KNOT,
+	.rate_min = UX500_PLATFORM_MIN_RATE_PLAYBACK,
+	.rate_max = UX500_PLATFORM_MAX_RATE_PLAYBACK,
+	.channels_min = UX500_PLATFORM_MIN_CHANNELS,
+	.channels_max = UX500_PLATFORM_MAX_CHANNELS,
+	.buffer_bytes_max = UX500_PLATFORM_BUFFER_BYTES_MAX,
+	.period_bytes_min = UX500_PLATFORM_PERIODS_BYTES_MIN,
+	.period_bytes_max = UX500_PLATFORM_PERIODS_BYTES_MAX,
+	.periods_min = UX500_PLATFORM_PERIODS_MIN,
+	.periods_max = UX500_PLATFORM_PERIODS_MAX,
+};
+
+static struct snd_pcm_hardware ux500_pcm_hw_capture = {
+	.info = SNDRV_PCM_INFO_INTERLEAVED |
+		SNDRV_PCM_INFO_MMAP |
+		SNDRV_PCM_INFO_RESUME |
+		SNDRV_PCM_INFO_PAUSE,
+	.formats = SNDRV_PCM_FMTBIT_S16_LE |
+		SNDRV_PCM_FMTBIT_U16_LE |
+		SNDRV_PCM_FMTBIT_S16_BE |
+		SNDRV_PCM_FMTBIT_U16_BE,
+	.rates = SNDRV_PCM_RATE_KNOT,
+	.rate_min = UX500_PLATFORM_MIN_RATE_CAPTURE,
+	.rate_max = UX500_PLATFORM_MAX_RATE_CAPTURE,
+	.channels_min = UX500_PLATFORM_MIN_CHANNELS,
+	.channels_max = UX500_PLATFORM_MAX_CHANNELS,
+	.buffer_bytes_max = UX500_PLATFORM_BUFFER_BYTES_MAX,
+	.period_bytes_min = UX500_PLATFORM_PERIODS_BYTES_MIN,
+	.period_bytes_max = UX500_PLATFORM_PERIODS_BYTES_MAX,
+	.periods_min = UX500_PLATFORM_PERIODS_MIN,
+	.periods_max = UX500_PLATFORM_PERIODS_MAX,
+};
+
+static const char *stream_str(struct snd_pcm_substream *substream)
+{
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		return "Playback";
+	else
+		return "Capture";
+}
+
+static void ux500_pcm_dma_hw_free(struct device *dev,
+				struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct snd_dma_buffer *buf = runtime->dma_buffer_p;
+
+	if (runtime->dma_area == NULL)
+		return;
+
+	if (buf != &substream->dma_buffer) {
+		dma_free_coherent(
+			buf->dev.dev,
+			buf->bytes,
+			buf->area,
+			buf->addr);
+		kfree(runtime->dma_buffer_p);
+	}
+
+	snd_pcm_set_runtime_buffer(substream, NULL);
+}
+
+void ux500_pcm_dma_eot_handler(void *data)
+{
+	struct snd_pcm_substream *substream = data;
+	struct snd_pcm_runtime *runtime;
+	struct ux500_pcm_private *private;
+	struct snd_soc_pcm_runtime *rtd = substream->private_data;
+	struct snd_soc_dai *dai = rtd->cpu_dai;
+
+	pr_debug("%s: MSP %d (%s): Enter.\n", __func__, dai->id, stream_str(substream));
+
+	if (substream) {
+		runtime = substream->runtime;
+		private = substream->runtime->private_data;
+
+		if (ux500_msp_dai_i2s_get_underrun_status(private->msp_id)) {
+			private->no_of_underruns++;
+			pr_debug("%s: Nr of underruns (%d)\n", __func__,
+					private->no_of_underruns);
+		}
+
+		/* calc the offset in the circular buffer */
+		private->offset += frames_to_bytes(runtime,
+				runtime->period_size);
+		private->offset %= frames_to_bytes(runtime,
+				runtime->period_size) * runtime->periods;
+
+		snd_pcm_period_elapsed(substream);
+	}
+}
+EXPORT_SYMBOL(ux500_pcm_dma_eot_handler);
+
+static int ux500_pcm_open(struct snd_pcm_substream *substream)
+{
+	int stream_id = substream->pstr->stream;
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct ux500_pcm_private *private;
+	struct snd_soc_pcm_runtime *rtd = substream->private_data;
+	struct snd_soc_dai *dai = rtd->cpu_dai;
+	int ret;
+
+	pr_debug("%s: MSP %d (%s): Enter.\n", __func__, dai->id, stream_str(substream));
+
+	pr_debug("%s: Set runtime hwparams.\n", __func__);
+	if (stream_id == SNDRV_PCM_STREAM_PLAYBACK)
+		snd_soc_set_runtime_hwparams(substream, &ux500_pcm_hw_playback);
+	else
+		snd_soc_set_runtime_hwparams(substream, &ux500_pcm_hw_capture);
+
+	/* ensure that buffer size is a multiple of period size */
+	ret = snd_pcm_hw_constraint_integer(runtime, SNDRV_PCM_HW_PARAM_PERIODS);
+	if (ret < 0) {
+		pr_err("%s: Error: snd_pcm_hw_constraints failed (%d)\n",
+			__func__,
+			ret);
+		return ret;
+	}
+
+	pr_debug("%s: Init runtime private data.\n", __func__);
+	private = kzalloc(sizeof(struct ux500_pcm_private), GFP_KERNEL);
+	if (private == NULL)
+		return -ENOMEM;
+	private->msp_id = dai->id;
+	runtime->private_data = private;
+
+	pr_debug("%s: Set hw-struct for %s.\n", __func__, stream_str(substream));
+	runtime->hw = (stream_id == SNDRV_PCM_STREAM_PLAYBACK) ?
+		ux500_pcm_hw_playback : ux500_pcm_hw_capture;
+
+	return 0;
+}
+
+static int ux500_pcm_close(struct snd_pcm_substream *substream)
+{
+	struct ux500_pcm_private *private = substream->runtime->private_data;
+
+	pr_debug("%s: Enter\n", __func__);
+
+	kfree(private);
+
+	return 0;
+}
+
+static int ux500_pcm_hw_params(struct snd_pcm_substream *substream,
+			struct snd_pcm_hw_params *hw_params)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct snd_dma_buffer *buf = runtime->dma_buffer_p;
+	int ret = 0;
+	int size;
+
+	pr_debug("%s: Enter\n", __func__);
+
+	size = params_buffer_bytes(hw_params);
+
+	if (buf) {
+		if (buf->bytes >= size)
+			goto out;
+		ux500_pcm_dma_hw_free(NULL, substream);
+	}
+
+	if (substream->dma_buffer.area != NULL &&
+		substream->dma_buffer.bytes >= size) {
+		buf = &substream->dma_buffer;
+	} else {
+		buf = kmalloc(sizeof(struct snd_dma_buffer), GFP_KERNEL);
+		if (!buf)
+			goto nomem;
+
+		buf->dev.type = SNDRV_DMA_TYPE_DEV;
+		buf->dev.dev = NULL;
+		buf->area = dma_alloc_coherent(
+			NULL,
+			size,
+			&buf->addr,
+			GFP_KERNEL);
+		buf->bytes = size;
+		buf->private_data = NULL;
+
+		if (!buf->area)
+			goto free;
+	}
+	snd_pcm_set_runtime_buffer(substream, buf);
+	ret = 1;
+ out:
+	runtime->dma_bytes = size;
+	return ret;
+
+ free:
+	kfree(buf);
+ nomem:
+	return -ENOMEM;
+}
+
+static int ux500_pcm_hw_free(struct snd_pcm_substream *substream)
+{
+	pr_debug("%s: Enter\n", __func__);
+
+	ux500_pcm_dma_hw_free(NULL, substream);
+
+	return 0;
+}
+
+static int ux500_pcm_prepare(struct snd_pcm_substream *substream)
+{
+	pr_debug("%s: Enter\n", __func__);
+	return 0;
+}
+
+static int ux500_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
+{
+	int ret;
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct ux500_pcm_private *private = runtime->private_data;
+	int stream_id = substream->pstr->stream;
+
+	pr_debug("%s: Enter\n", __func__);
+
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_START:
+	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
+		pr_debug("%s: START/PAUSE-RELEASE\n", __func__);
+		if (runtime->status->state == SNDRV_PCM_STATE_XRUN) {
+			pr_debug("XRUN occurred\n");
+				return 0;
+		}
+
+		private->no_of_underruns = 0;
+		private->offset = 0;
+		ret = ux500_msp_dai_i2s_configure_sg(runtime->dma_addr,
+				runtime->periods,
+				frames_to_bytes(runtime, runtime->period_size),
+				private->msp_id,
+				stream_id);
+		if (ret) {
+			pr_err("%s: Failed to configure sg-list!\n", __func__);
+			return -EINVAL;
+		}
+		break;
+
+	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
+		break;
+
+	case SNDRV_PCM_TRIGGER_STOP:
+		pr_debug("%s: SNDRV_PCM_TRIGGER_STOP\n", __func__);
+		pr_debug("%s: no_of_underruns = %u\n",
+			__func__,
+			private->no_of_underruns);
+		break;
+
+	default:
+		pr_err("%s: Invalid command in pcm trigger\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static snd_pcm_uframes_t ux500_pcm_pointer(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct ux500_pcm_private *private = runtime->private_data;
+
+	pr_debug("%s: dma_offset %d frame %ld\n", __func__, private->offset,
+			bytes_to_frames(substream->runtime, private->offset));
+
+	return bytes_to_frames(substream->runtime, private->offset);
+}
+
+static int ux500_pcm_mmap(struct snd_pcm_substream *substream,
+			struct vm_area_struct *vma)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	pr_debug("%s: Enter.\n", __func__);
+
+	return dma_mmap_coherent(
+		NULL,
+		vma,
+		runtime->dma_area,
+		runtime->dma_addr,
+		runtime->dma_bytes);
+}
+
+static struct snd_pcm_ops ux500_pcm_ops = {
+	.open		= ux500_pcm_open,
+	.close		= ux500_pcm_close,
+	.ioctl		= snd_pcm_lib_ioctl,
+	.hw_params	= ux500_pcm_hw_params,
+	.hw_free	= ux500_pcm_hw_free,
+	.prepare	= ux500_pcm_prepare,
+	.trigger	= ux500_pcm_trigger,
+	.pointer	= ux500_pcm_pointer,
+	.mmap		= ux500_pcm_mmap
+};
+
+int ux500_pcm_new(struct snd_card *card,
+		struct snd_soc_dai *dai,
+		struct snd_pcm *pcm)
+{
+	pr_debug("%s: pcm = %d\n", __func__, (int)pcm);
+
+	pcm->info_flags = 0;
+	strcpy(pcm->name, "UX500_PCM");
+
+	pr_debug("%s: pcm->name = %s.\n", __func__, pcm->name);
+
+	return 0;
+}
+
+static void ux500_pcm_free(struct snd_pcm *pcm)
+{
+	pr_debug("%s: Enter\n", __func__);
+}
+
+static int ux500_pcm_suspend(struct snd_soc_dai *dai)
+{
+	pr_debug("%s: Enter\n", __func__);
+
+	return 0;
+}
+
+static int ux500_pcm_resume(struct snd_soc_dai *dai)
+{
+	pr_debug("%s: Enter\n", __func__);
+
+	return 0;
+}
+
+struct snd_soc_platform_driver ux500_pcm_soc_drv = {
+	.ops		= &ux500_pcm_ops,
+	.pcm_new        = ux500_pcm_new,
+	.pcm_free       = ux500_pcm_free,
+	.suspend        = ux500_pcm_suspend,
+	.resume         = ux500_pcm_resume,
+};
+EXPORT_SYMBOL(ux500_pcm_soc_drv);
+
+static int __devexit ux500_pcm_drv_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	pr_info("%s: Register ux500-pcm SoC platform driver.\n", __func__);
+	ret = snd_soc_register_platform(&pdev->dev, &ux500_pcm_soc_drv);
+	if (ret < 0) {
+		pr_err("%s: Error: Failed to register "
+			"ux500-pcm SoC platform driver (%d)!\n",
+			__func__,
+			ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __devinit ux500_pcm_drv_remove(struct platform_device *pdev)
+{
+	pr_info("%s: Unregister ux500-pcm SoC platform driver.\n", __func__);
+	snd_soc_unregister_platform(&pdev->dev);
+
+	return 0;
+}
+
+static struct platform_driver ux500_pcm_driver = {
+	.driver = {
+		.name = "ux500-pcm",
+		.owner = THIS_MODULE,
+	},
+
+	.probe = ux500_pcm_drv_probe,
+	.remove = __devexit_p(ux500_pcm_drv_remove),
+};
+
+static int __init ux500_pcm_drv_init(void)
+{
+	pr_debug("%s: Register ux500-pcm platform driver.\n", __func__);
+
+	return platform_driver_register(&ux500_pcm_driver);
+}
+
+static void __exit ux500_pcm_drv_exit(void)
+{
+	pr_debug("%s: Unregister ux500-pcm platform driver.\n", __func__);
+
+	platform_driver_unregister(&ux500_pcm_driver);
+}
+
+module_init(ux500_pcm_drv_init);
+module_exit(ux500_pcm_drv_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/sound/soc/ux500/ux500_pcm.h b/sound/soc/ux500/ux500_pcm.h
new file mode 100644
index 00000000000..50f46615275
--- /dev/null
+++ b/sound/soc/ux500/ux500_pcm.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Ola Lilja <ola.o.lilja@stericsson.com>,
+ *         Roger Nilsson <roger.xr.nilsson@stericsson.com>
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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 UX500_PCM_H
+#define UX500_PCM_H
+
+#include <mach/msp.h>
+
+#define UX500_PLATFORM_MIN_RATE_PLAYBACK 8000
+#define UX500_PLATFORM_MAX_RATE_PLAYBACK 48000
+#define UX500_PLATFORM_MIN_RATE_CAPTURE	8000
+#define UX500_PLATFORM_MAX_RATE_CAPTURE	48000
+
+#define UX500_PLATFORM_MIN_CHANNELS 1
+#define UX500_PLATFORM_MAX_CHANNELS 8
+
+#define UX500_PLATFORM_PERIODS_BYTES_MIN	128
+#define UX500_PLATFORM_PERIODS_BYTES_MAX	(64 * PAGE_SIZE)
+#define UX500_PLATFORM_PERIODS_MIN		2
+#define UX500_PLATFORM_PERIODS_MAX		48
+#define UX500_PLATFORM_BUFFER_BYTES_MAX		(2048 * PAGE_SIZE)
+
+extern struct snd_soc_platform ux500_soc_platform;
+
+struct ux500_pcm_private {
+	int msp_id;
+	int stream_id;
+	unsigned int no_of_underruns;
+	unsigned int offset;
+};
+
+void ux500_pcm_dma_eot_handler(void *data);
+
+#endif
diff --git a/sound/u8500_acodec_ab8500.c b/sound/u8500_acodec_ab8500.c
new file mode 100644
index 00000000000..c75da368832
--- /dev/null
+++ b/sound/u8500_acodec_ab8500.c
@@ -0,0 +1,2523 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Deepak Karda
+ *         for ST-Ericsson.
+ *
+ * License terms:
+ *
+ * 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.
+ */
+
+/*-----------------------------------------------------------------------------
+* Common Includes
+*---------------------------------------------------------------------------*/
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <asm/uaccess.h>
+#include <asm/io.h>
+#include <linux/interrupt.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/soundcard.h>
+#include <linux/sound.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/i2s/i2s.h>
+#include <mach/msp.h>
+#include <linux/gpio.h>
+/*#include <mach/i2c.h>*/
+#include <mach/debug.h>
+#include <mach/u8500_acodec_ab8500.h>
+#include <mach/ab8500.h>
+
+#ifdef CONFIG_U8500_AB8500_CUT10
+#include <mach/ab8500_codec_v1_0.h>
+#endif
+#ifdef CONFIG_U8500_AB8500_ED
+#include <mach/ab8500_codec.h>
+#endif
+
+#define ELEMENT_SIZE 0
+#define FRAME_SIZE -1
+#define MSP_NUM 0
+
+/* Debugging stuff */
+
+#define ACODEC_NAME 	  "DRIVER ACODEC"
+#define DRIVER_DEBUG	        CONFIG_STM_ACODEC_DEBUG	/* enables/disables debug msgs */
+#define DRIVER_DEBUG_PFX	ACODEC_NAME	/* msg header represents this module */
+#define DRIVER_DBG	        KERN_ERR	/* message level */
+#define NMDK_DEBUG             CONFIG_STM_ACODEC_DEBUG
+extern struct driver_debug_st DBG_ST;
+
+#if NMDK_DEBUG > 0
+t_ab8500_codec_error dump_acodec_registers(void);
+t_ab8500_codec_error dump_msp_registers(void);
+#endif
+
+#ifdef CONFIG_U8500_ACODEC_DMA
+static void u8500_digital_lpbk_tx_dma_start(void);
+static void u8500_digital_lpbk_rx_dma_start(void);
+#endif
+
+int second_config;
+/*----------------------------------------------------------------------------
+* global declarations
+*---------------------------------------------------------------------------*/
+t_u8500_codec_system_context g_codec_system_context;
+
+int u8500_acodec_rates[MAX_NO_OF_RATES] = { 48000 };
+
+char *codec_dest_texts[NUMBER_OUTPUT_DEVICE] = {
+	"CODEC_DEST_HEADSET", "CODEC_DEST_EARPIECE", "CODEC_DEST_HANDSFREE",
+	"CODEC_DEST_VIBRATOR1", "CODEC_DEST_VIBRATOR2"
+};
+
+char *codec_in_texts[NUMBER_INPUT_DEVICE] = {
+	"CODEC_SRC_LINEIN", "CODEC_SRC_MICROPHONE_1A",
+	"CODEC_SRC_MICROPHONE_1B",
+	"CODEC_SRC_MICROPHONE_2", "CODEC_SRC_D_MICROPHONE_1",
+	"CODEC_SRC_D_MICROPHONE_2",
+	"CODEC_SRC_D_MICROPHONE_3", "CODEC_SRC_D_MICROPHONE_4",
+	"CODEC_SRC_D_MICROPHONE_5",
+	"CODEC_SRC_D_MICROPHONE_6", "CODEC_SRC_D_MICROPHONE_12",
+	"CODEC_SRC_D_MICROPHONE_34",
+	"CODEC_SRC_D_MICROPHONE_56"
+};
+
+char *lpbk_state_in_texts[NUMBER_LOOPBACK_STATE] = { "DISABLE", "ENABLE" };
+char *switch_state_in_texts[NUMBER_SWITCH_STATE] = { "DISABLE", "ENABLE" };
+char *power_state_in_texts[NUMBER_POWER_STATE] = { "DISABLE", "ENABLE" };
+char *tdm_mode_state_in_texts[NUMBER_POWER_STATE] = { "DISABLE", "ENABLE" };
+char *direct_rendering_state_in_texts[NUMBER_DIRECT_RENDERING_STATE] =
+    { "DISABLE", "ENABLE" };
+char *pcm_rendering_state_in_texts[NUMBER_PCM_RENDERING_STATE] =
+    { "DISABLE", "ENABLE", "PENDING" };
+
+EXPORT_SYMBOL(codec_dest_texts);
+EXPORT_SYMBOL(codec_in_texts);
+
+static void ab8500_codec_power_init(void);
+static int check_device_id();
+t_ab8500_codec_error perform_src_routing(t_ab8500_codec_src input_device);
+t_ab8500_codec_error
+    u8500_acodec_allocate_all_mono_slots
+    (t_ab8500_codec_cr31_to_cr46_ad_data_allocation ad_data_line1);
+t_ab8500_codec_error
+    u8500_acodec_allocate_all_stereo_slots
+    (t_ab8500_codec_cr31_to_cr46_ad_data_allocation ad_data_line1,
+     t_ab8500_codec_cr31_to_cr46_ad_data_allocation ad_data_line2);
+
+#if 0				//from Arnaud
+/* For Codec in Master mode for recording*/
+struct msp_protocol_desc protocol_desc_tdm_mode = {
+	MSP_DATA_TRANSFER_WIDTH_HALFWORD,	/*rx_data_transfer_width */
+	MSP_DATA_TRANSFER_WIDTH_HALFWORD,	/*tx_data_transfer_width */
+	MSP_SINGLE_PHASE,	/*rx_phase_mode                 */
+	MSP_SINGLE_PHASE,	/*tx_phase_mode                 */
+	MSP_PHASE2_START_MODE_IMEDIATE,	/*rx_phase2_start_mode  */
+	MSP_PHASE2_START_MODE_IMEDIATE,	/*tx_phase2_start_mode  */
+	MSP_BTF_MS_BIT_FIRST,	/*rx_endianess                  */
+	MSP_BTF_MS_BIT_FIRST,	/*tx_endianess                  */
+	MSP_FRAME_LENGTH_2,	/*rx_frame_length_1             */
+	MSP_FRAME_LENGTH_2,	/*rx_frame_length_2             */
+	MSP_FRAME_LENGTH_2,	/*tx_frame_length_1             */
+	MSP_FRAME_LENGTH_2,	/*tx_frame_length_2             */
+	MSP_ELEM_LENGTH_16,	/*rx_element_length_1   */
+	MSP_ELEM_LENGTH_16,	/*rx_element_length_2   */
+	MSP_ELEM_LENGTH_16,	/*tx_element_length_1   */
+	MSP_ELEM_LENGTH_16,	/*tx_element_length_2   */
+	MSP_DELAY_0,		/*rx_data_delay                 */
+	MSP_DELAY_0,		/*tx_data_delay                 */
+	MSP_FALLING_EDGE,	/*rx_clock_pol                  */
+	MSP_RISING_EDGE,	/*tx_clock_pol                  */
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,	/*rx_msp_frame_pol              */
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,	/*tx_msp_frame_pol              */
+	MSP_HWS_NO_SWAP,	/*rx_half_word_swap             */
+	MSP_HWS_NO_SWAP,	/*tx_half_word_swap             */
+	MSP_COMPRESS_MODE_LINEAR,	/*compression_mode              */
+	MSP_EXPAND_MODE_LINEAR,	/*expansion_mode                */
+	MSP_SPI_CLOCK_MODE_NON_SPI,	/*spi_clk_mode                  */
+	MSP_SPI_BURST_MODE_DISABLE,	/*spi_burst_mode                */
+	63,			/*frame_period                  */
+	31,			/*frame_width                   */
+	64,			/*total_clocks_for_one_frame            */
+};
+#endif
+
+#if 0				//from HCL
+/* For Codec in Master mode for recording*/
+struct msp_protocol_desc protocol_desc_tdm_mode = {
+	MSP_DATA_TRANSFER_WIDTH_WORD,	/*rx_data_transfer_width */
+	MSP_DATA_TRANSFER_WIDTH_WORD,	/*tx_data_transfer_width */
+	MSP_DUAL_PHASE,		/*rx_phase_mode                 */
+	MSP_DUAL_PHASE,		/*tx_phase_mode                 */
+	MSP_PHASE2_START_MODE_FRAME_SYNC,	/*rx_phase2_start_mode  */
+	MSP_PHASE2_START_MODE_FRAME_SYNC,	/*tx_phase2_start_mode  */
+	MSP_BTF_MS_BIT_FIRST,	/*rx_endianess                  */
+	MSP_BTF_MS_BIT_FIRST,	/*tx_endianess                  */
+	MSP_FRAME_LENGTH_1,	/*rx_frame_length_1             */
+	MSP_FRAME_LENGTH_1,	/*rx_frame_length_2             */
+	MSP_FRAME_LENGTH_1,	/*tx_frame_length_1             */
+	MSP_FRAME_LENGTH_1,	/*tx_frame_length_2             */
+	MSP_ELEM_LENGTH_16,	/*rx_element_length_1   */
+	MSP_ELEM_LENGTH_16,	/*rx_element_length_2   */
+	MSP_ELEM_LENGTH_16,	/*tx_element_length_1   */
+	MSP_ELEM_LENGTH_16,	/*tx_element_length_2   */
+	MSP_DELAY_0,		/*rx_data_delay                 */
+	MSP_DELAY_0,		/*tx_data_delay                 */
+	MSP_RISING_EDGE,	/*rx_clock_pol                  */
+	MSP_RISING_EDGE,	/*tx_clock_pol                  */
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,	/*rx_msp_frame_pol              */
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,	/*tx_msp_frame_pol              */
+	MSP_HWS_NO_SWAP,	/*rx_half_word_swap             */
+	MSP_HWS_NO_SWAP,	/*tx_half_word_swap             */
+	MSP_COMPRESS_MODE_LINEAR,	/*compression_mode              */
+	MSP_EXPAND_MODE_LINEAR,	/*expansion_mode                */
+	MSP_SPI_CLOCK_MODE_NON_SPI,	/*spi_clk_mode                  */
+	MSP_SPI_BURST_MODE_DISABLE,	/*spi_burst_mode                */
+	255,			/*frame_period                  */
+	0,			/*frame_width                   */
+	256,			/*total_clocks_for_one_frame            */
+};
+
+#endif
+
+#if 0				//from STS
+struct msp_protocol_desc protocol_desc_tdm_mode = {
+	MSP_DATA_TRANSFER_WIDTH_HALFWORD,	/*rx_data_transfer_width */
+	MSP_DATA_TRANSFER_WIDTH_HALFWORD,	/*tx_data_transfer_width */
+	MSP_SINGLE_PHASE,	/*rx_phase_mode                 */
+	MSP_SINGLE_PHASE,	/*tx_phase_mode                 */
+	MSP_PHASE2_START_MODE_IMEDIATE,	/*rx_phase2_start_mode  */
+	MSP_PHASE2_START_MODE_IMEDIATE,	/*tx_phase2_start_mode  */
+	MSP_BTF_MS_BIT_FIRST,	/*rx_endianess                  */
+	MSP_BTF_MS_BIT_FIRST,	/*tx_endianess                  */
+	MSP_FRAME_LENGTH_2,	/*rx_frame_length_1             */
+	MSP_FRAME_LENGTH_1,	/*rx_frame_length_2             */
+	MSP_FRAME_LENGTH_2,	/*tx_frame_length_1             */
+	MSP_FRAME_LENGTH_1,	/*tx_frame_length_2             */
+	MSP_ELEM_LENGTH_16,	/*rx_element_length_1   */
+	MSP_ELEM_LENGTH_16,	/*rx_element_length_2   */
+	MSP_ELEM_LENGTH_16,	/*tx_element_length_1   */
+	MSP_ELEM_LENGTH_16,	/*tx_element_length_2   */
+	MSP_DELAY_0,		/*rx_data_delay                 */
+	MSP_DELAY_0,		/*tx_data_delay                 */
+	MSP_FALLING_EDGE,	/*rx_clock_pol                  */
+	MSP_RISING_EDGE,	/*tx_clock_pol                  */
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,	/*rx_msp_frame_pol              */
+	MSP_FRAME_SYNC_POL_ACTIVE_HIGH,	/*tx_msp_frame_pol              */
+	MSP_HWS_NO_SWAP,	/*rx_half_word_swap             */
+	MSP_HWS_NO_SWAP,	/*tx_half_word_swap             */
+	MSP_COMPRESS_MODE_LINEAR,	/*compression_mode              */
+	MSP_EXPAND_MODE_LINEAR,	/*expansion_mode                */
+	MSP_SPI_CLOCK_MODE_NON_SPI,	/*spi_clk_mode                  */
+	MSP_SPI_BURST_MODE_DISABLE,	/*spi_burst_mode                */
+	25,			/*frame_period                  */
+	32,			/*frame_width                   */
+	32,			/*total_clocks_for_one_frame            */
+};
+#endif
+
+#define DIGITAL_LPBK_MAX_BIFFERS	3
+
+#ifdef CONFIG_U8500_AB8500_CUT10
+#define NB_OF_CHANNEL_USED	 		8
+#else
+#define NB_OF_CHANNEL_USED	 		6
+#endif
+
+#define MONO_SRC	1
+#define STEREO_SRC	2
+
+typedef struct {
+	unsigned char *area;	/* virtual pointer */
+	dma_addr_t addr;	/* physical address */
+} t_dma_buffer;
+
+typedef struct {
+	struct completion tx_dma_com;
+	struct completion rx_dma_com;
+	volatile int rx_active;
+	volatile int tx_active;
+	t_dma_buffer buffer;
+	int data_size;
+	int rx_index;
+	int tx_index;
+} t_digital_lpbk_cnxt;
+
+const int play_flag = 1;
+const int capture_flag = 2;
+
+t_digital_lpbk_cnxt digital_lpbk_cnxt;
+
+void u8500_set_defaults()
+{
+	int i;
+
+	for (i = 0; i < NUMBER_INPUT_DEVICE; i++) {
+		g_codec_system_context.input_config[i].left_volume = 0;
+		g_codec_system_context.input_config[i].right_volume = 0;
+		g_codec_system_context.input_config[i].mute_state = DISABLE;
+		g_codec_system_context.input_config[i].power_state = DISABLE;
+	}
+
+	for (i = 0; i < NUMBER_OUTPUT_DEVICE; i++) {
+		g_codec_system_context.output_config[i].left_volume = 0;
+		g_codec_system_context.output_config[i].right_volume = 0;
+		g_codec_system_context.output_config[i].mute_state = DISABLE;
+		g_codec_system_context.output_config[i].power_state = DISABLE;
+	}
+
+}				//END OF FUNCTION
+
+struct i2sdrv_data *i2sdrv[MAX_I2S_CLIENTS];
+
+t_ab8500_codec_error u8500_acodec_open(int client_id, int stream_id)
+{
+	struct i2sdrv_data *p_i2sdrv_data = NULL;
+	struct i2s_device *i2s;
+
+	p_i2sdrv_data = i2sdrv[client_id];
+
+	if (!p_i2sdrv_data)
+		return (-1);
+
+	i2s = p_i2sdrv_data->i2s;
+
+	if (stream_id == 0)	//PLAYBACK
+	{
+		if (p_i2sdrv_data->tx_status)
+			return -1;
+		else {
+			p_i2sdrv_data->tx_status = 1;
+		}
+	} else if (stream_id == 1)	//CAPTURE
+	{
+		if (p_i2sdrv_data->rx_status)
+			return -1;
+		else {
+			p_i2sdrv_data->rx_status = 1;
+		}
+	}
+
+	p_i2sdrv_data->flag = 0;
+
+	return 0;
+}
+
+t_ab8500_codec_error u8500_acodec_send_data(int client_id, void *data,
+					    size_t bytes, int dma_flag)
+{
+	struct i2sdrv_data *p_i2sdrv_data = NULL;
+	struct i2s_device *i2s_dev = NULL;
+	int bytes_transmit;
+	struct i2s_message message;
+
+	p_i2sdrv_data = i2sdrv[client_id];
+
+	if (!p_i2sdrv_data)
+		return (-1);
+
+	i2s_dev = p_i2sdrv_data->i2s;
+
+	if (p_i2sdrv_data->flag) {
+		stm_dbg(DBG_ST.acodec, " I2S controller not available\n");
+		return -1;
+	}
+
+	message.i2s_transfer_mode = I2S_TRANSFER_MODE_SINGLE_DMA;
+	message.i2s_direction = I2S_DIRECTION_TX;
+	message.txbytes = bytes;
+	message.txdata = data;
+	message.dma_flag = dma_flag;
+
+	bytes_transmit = i2s_transfer(i2s_dev->controller, &message);
+
+	if (bytes_transmit < 0) {
+		printk("error in transfer\n");
+		return -1;
+	}
+	return bytes_transmit;
+
+}
+
+t_ab8500_codec_error u8500_acodec_loopback_configure(int client_id, void *data,
+						     size_t bytes, int dma_flag)
+{
+	struct i2sdrv_data *p_i2sdrv_data = NULL;
+	struct i2s_device *i2s_dev = NULL;
+	int bytes_receive;
+	struct i2s_message message;
+
+	p_i2sdrv_data = i2sdrv[client_id];
+
+	if (!p_i2sdrv_data)
+		return (-1);
+
+	i2s_dev = p_i2sdrv_data->i2s;
+
+	if (p_i2sdrv_data->flag) {
+		stm_dbg(DBG_ST.acodec, " I2S controller not available\n");
+		return -1;
+	}
+
+	message.i2s_transfer_mode = I2S_TRANSFER_MODE_INF_LOOPBACK;
+	message.rxbytes = bytes;
+	message.rxdata = data;
+	message.txbytes = bytes;
+	message.txdata = data;
+	message.dma_flag = dma_flag;
+
+	bytes_receive = i2s_transfer(i2s_dev->controller, &message);
+
+	if (bytes_receive < 0) {
+		printk(" not get\n");
+		return -1;
+	}
+	return bytes_receive;
+
+}
+
+t_ab8500_codec_error u8500_acodec_receive_data(int client_id, void *data,
+					       size_t bytes, int dma_flag)
+{
+	struct i2sdrv_data *p_i2sdrv_data = NULL;
+	struct i2s_device *i2s_dev = NULL;
+	int bytes_receive;
+	struct i2s_message message;
+
+	p_i2sdrv_data = i2sdrv[client_id];
+
+	if (!p_i2sdrv_data)
+		return (-1);
+
+	i2s_dev = p_i2sdrv_data->i2s;
+
+	if (p_i2sdrv_data->flag) {
+		stm_dbg(DBG_ST.acodec, " I2S controller not available\n");
+		return -1;
+	}
+
+	message.i2s_transfer_mode = I2S_TRANSFER_MODE_SINGLE_DMA;
+	message.i2s_direction = I2S_DIRECTION_RX;
+	message.rxbytes = bytes;
+	message.rxdata = data;
+	message.dma_flag = dma_flag;
+
+	bytes_receive = i2s_transfer(i2s_dev->controller, &message);
+
+	if (bytes_receive < 0) {
+		printk(" not get\n");
+		return -1;
+	}
+	return bytes_receive;
+
+}
+
+t_ab8500_codec_error u8500_acodec_close(int client_id, t_acodec_disable flag)
+{
+	struct i2sdrv_data *p_i2sdrv_data = NULL;
+	struct i2s_device *i2s_dev = NULL;
+	int status = 0;
+
+	p_i2sdrv_data = i2sdrv[client_id];
+
+	if (!p_i2sdrv_data)
+		return (-1);
+
+	i2s_dev = p_i2sdrv_data->i2s;
+
+	if (p_i2sdrv_data->flag) {
+		stm_dbg(DBG_ST.acodec, " I2S controller not available\n");
+		return -1;
+	}
+
+	if (flag == DISABLE_ALL) {
+		p_i2sdrv_data->flag = -1;
+		p_i2sdrv_data->tx_status = 0;
+		p_i2sdrv_data->rx_status = 0;
+	} else if (flag == DISABLE_TRANSMIT) {
+		p_i2sdrv_data->tx_status = 0;
+	} else if (flag == DISABLE_RECEIVE) {
+		p_i2sdrv_data->rx_status = 0;
+	}
+	status = i2s_cleanup(i2s_dev->controller, flag);
+	if (status) {
+		return -1;
+	}
+
+	return 0;
+}
+
+/**
+* u8500_acodec_enable_audio_mode
+*
+* @direction - direction of data flow (from/to) audiocode
+* @mspClockSel - clock for MSP
+* @mspInClockFreq - input clock for MSP
+* @channels - number of channel, 1 for mono and 2 for stereo
+*
+* It configures the audiocodec in audio mode. In this case,the I2S
+* protocol is used for data exchanges.
+*/
+
+t_ab8500_codec_error u8500_acodec_enable_audio_mode(struct acodec_configuration
+						    * acodec_config)
+{
+	struct i2s_device *i2s_dev = NULL;
+	t_ab8500_codec_error error_status = AB8500_CODEC_OK;
+	struct msp_config msp_config;
+	t_ab8500_codec_error codec_error;
+	t_ab8500_codec_mode codec_in_mode = AB8500_CODEC_MODE_MANUAL_SETTING;
+	t_ab8500_codec_mode codec_out_mode = AB8500_CODEC_MODE_MANUAL_SETTING;
+	t_ab8500_codec_direction codec_direction;
+/*#ifdef CONFIG_U8500_AB8500_CUT10*/
+#if 1
+	t_ab8500_codec_tdm_config tdm_config;
+#endif
+
+	memset(&msp_config, 0, sizeof(msp_config));
+
+	FUNC_ENTER();
+	stm_dbg(DBG_ST.acodec,
+		"  Entering in u8500_acodec_enable_audio_mode()\n");
+
+	if (i2sdrv[I2S_CLIENT_MSP1]->flag) {
+		stm_dbg(DBG_ST.acodec, " I2S controller not available\n");
+		return -1;
+	}
+
+	i2s_dev = i2sdrv[I2S_CLIENT_MSP1]->i2s;
+
+	if (g_codec_system_context.cur_user == NO_USER) {
+		stm_error("Audiocodec not yet configured by any user\n");
+		return (AB8500_CODEC_ERROR);
+	} else if (g_codec_system_context.cur_user != acodec_config->user) {
+		stm_error
+		    (" Trying to acces audiocodec already in use by user %d\n",
+		     g_codec_system_context.cur_user);
+		return (AB8500_CODEC_ERROR);
+	}
+
+	switch (acodec_config->direction) {
+	case AB8500_CODEC_DIRECTION_INOUT:
+		codec_direction = AB8500_CODEC_DIRECTION_INOUT;
+		codec_in_mode = AB8500_CODEC_MODE_VOICE;	//HIFI
+		codec_out_mode = AB8500_CODEC_MODE_VOICE;	//VOICE
+		break;
+	case AB8500_CODEC_DIRECTION_IN:
+		codec_direction = AB8500_CODEC_DIRECTION_IN;
+		codec_in_mode = AB8500_CODEC_MODE_VOICE;	//HIFI
+		break;
+	case AB8500_CODEC_DIRECTION_OUT:
+		codec_direction = AB8500_CODEC_DIRECTION_OUT;
+		codec_out_mode = AB8500_CODEC_MODE_VOICE;	//HIFI
+		break;
+	default:
+		stm_error("Invalid direction\n");
+		return AB8500_CODEC_ERROR;
+	}
+
+	/* MSP configuration  */
+
+	msp_config.tx_clock_sel = 0;	//TX_CLK_SEL_SRG;
+	msp_config.rx_clock_sel = 0;	//RX_CLK_SEL_SRG;
+
+	msp_config.tx_frame_sync_sel = 0;	//0x00000400; Frame synchronization signal is provided by an external source. MSPTFS is an input pin
+	msp_config.rx_frame_sync_sel = 0;	//0: Rx Frame synchronization signal is provided by an external source. MSPRFS is an input pin
+
+	msp_config.input_clock_freq = MSP_INPUT_FREQ_48MHZ;
+
+	msp_config.srg_clock_sel = 0;	//0x000C0000
+
+	//msp_config.rx_endianess = MSP_BIG_ENDIAN;
+	//msp_config.tx_endianess = MSP_BIG_ENDIAN;
+
+	msp_config.rx_frame_sync_pol = RX_FIFO_SYNC_HI;
+	msp_config.tx_frame_sync_pol = TX_FIFO_SYNC_HI;
+
+	//msp_config.rx_unexpect_frame_sync = MSP_UNEXPECTED_FS_IGNORE;
+	//msp_config.tx_unexpect_frame_sync = MSP_UNEXPECTED_FS_IGNORE;
+
+	msp_config.rx_fifo_config = RX_FIFO_ENABLE;
+	msp_config.tx_fifo_config = TX_FIFO_ENABLE;
+
+	msp_config.spi_clk_mode = SPI_CLK_MODE_NORMAL;
+	msp_config.spi_burst_mode = 0;
+
+	msp_config.handler = acodec_config->handler;
+	msp_config.tx_callback_data = acodec_config->tx_callback_data;
+	msp_config.tx_data_enable = 0;
+	msp_config.rx_callback_data = acodec_config->rx_callback_data;
+
+	msp_config.loopback_enable = 0;
+	msp_config.multichannel_configured = 0;
+
+	msp_config.def_elem_len = 0;
+	//msp_config.loopback_enable  = g_codec_system_context.msp_loopback;
+
+	stm_dbg(DBG_ST.acodec, "  msp_config.loopback_enable = 0x%x \n",
+		msp_config.loopback_enable);
+
+#if 0
+	msp_config.default_protocol_desc = 1;
+#else
+	msp_config.default_protocol_desc = 0;
+	msp_config.protocol_desc.rx_phase_mode = MSP_SINGLE_PHASE;
+	msp_config.protocol_desc.tx_phase_mode = MSP_SINGLE_PHASE;
+	msp_config.protocol_desc.rx_phase2_start_mode =
+	    MSP_PHASE2_START_MODE_IMEDIATE;
+	msp_config.protocol_desc.tx_phase2_start_mode =
+	    MSP_PHASE2_START_MODE_IMEDIATE;
+	msp_config.protocol_desc.rx_bit_transfer_format = MSP_BTF_MS_BIT_FIRST;
+	msp_config.protocol_desc.tx_bit_transfer_format = MSP_BTF_MS_BIT_FIRST;
+	msp_config.protocol_desc.rx_frame_length_1 = MSP_FRAME_LENGTH_1;
+	msp_config.protocol_desc.rx_frame_length_2 = MSP_FRAME_LENGTH_1;
+	msp_config.protocol_desc.tx_frame_length_1 = MSP_FRAME_LENGTH_1;
+	msp_config.protocol_desc.tx_frame_length_2 = MSP_FRAME_LENGTH_1;
+	msp_config.protocol_desc.rx_element_length_1 = MSP_ELEM_LENGTH_32;
+	msp_config.protocol_desc.rx_element_length_2 = MSP_ELEM_LENGTH_32;
+	msp_config.protocol_desc.tx_element_length_1 = MSP_ELEM_LENGTH_32;
+	msp_config.protocol_desc.tx_element_length_2 = MSP_ELEM_LENGTH_32;
+	msp_config.protocol_desc.rx_data_delay = MSP_DELAY_0;
+	msp_config.protocol_desc.tx_data_delay = MSP_DELAY_0;
+	msp_config.protocol_desc.rx_clock_pol = MSP_RISING_EDGE;
+	msp_config.protocol_desc.tx_clock_pol = MSP_FALLING_EDGE;
+	msp_config.protocol_desc.rx_frame_sync_pol =
+	    MSP_FRAME_SYNC_POL_ACTIVE_HIGH;
+	msp_config.protocol_desc.tx_frame_sync_pol =
+	    MSP_FRAME_SYNC_POL_ACTIVE_HIGH;
+	msp_config.protocol_desc.rx_half_word_swap = MSP_HWS_NO_SWAP;
+	msp_config.protocol_desc.tx_half_word_swap = MSP_HWS_NO_SWAP;
+	msp_config.protocol_desc.compression_mode = MSP_COMPRESS_MODE_LINEAR;
+	msp_config.protocol_desc.expansion_mode = MSP_EXPAND_MODE_LINEAR;
+	msp_config.protocol_desc.spi_clk_mode = MSP_SPI_CLOCK_MODE_NON_SPI;
+	msp_config.protocol_desc.spi_burst_mode = MSP_SPI_BURST_MODE_DISABLE;
+	msp_config.protocol_desc.frame_sync_ignore = MSP_FRAME_SYNC_IGNORE;
+	msp_config.protocol_desc.frame_period = 63;
+	msp_config.protocol_desc.frame_width = 31;
+	msp_config.protocol_desc.total_clocks_for_one_frame = 64;
+
+#endif
+
+	msp_config.direction = MSP_BOTH_T_R_MODE;
+	msp_config.protocol = MSP_PCM_PROTOCOL;	//MSP_I2S_PROTOCOL
+	msp_config.frame_size = ELEMENT_SIZE;
+	//  msp_config.frame_freq = freq; $kardad$
+	msp_config.frame_freq = CODEC_SAMPLING_FREQ_48KHZ;
+
+	/* enable msp for both tr and rx mode with dma data transfer. THIS IS NOW DONE SEPARATELY from SAA. */
+
+	if (acodec_config->channels == 1)
+		msp_config.data_size = MSP_DATA_SIZE_16BIT;
+	else
+		msp_config.data_size = MSP_DATA_SIZE_32BIT;
+
+#ifdef CONFIG_U8500_ACODEC_DMA
+	msp_config.work_mode = MSP_DMA_MODE;
+#elif defined(CONFIG_U8500_ACODEC_POLL)
+	msp_config.work_mode = MSP_POLLING_MODE;
+#else
+	msp_config.work_mode = MSP_INTERRUPT_MODE;
+#endif
+
+	if (DISABLE == acodec_config->direct_rendering_mode) {
+		msp_config.multichannel_configured = 1;
+		msp_config.multichannel_config.tx_multichannel_enable = 1;
+		if (acodec_config->channels == 1) {
+			msp_config.multichannel_config.tx_channel_0_enable =
+			    0x0000001;
+		} else {
+			msp_config.multichannel_config.tx_channel_0_enable =
+			    0x0000003;
+		}
+		msp_config.multichannel_config.tx_channel_1_enable = 0x0000000;
+		msp_config.multichannel_config.tx_channel_2_enable = 0x0000000;
+		msp_config.multichannel_config.tx_channel_3_enable = 0x0000000;
+
+		msp_config.multichannel_config.rx_multichannel_enable = 1;
+
+		if (acodec_config->channels == 1) {
+			msp_config.multichannel_config.rx_channel_0_enable =
+			    0x0000001;
+		} else {
+			msp_config.multichannel_config.rx_channel_0_enable =
+			    0x0000003;
+		}
+		msp_config.multichannel_config.rx_channel_1_enable = 0x0000000;
+		msp_config.multichannel_config.rx_channel_2_enable = 0x0000000;
+		msp_config.multichannel_config.rx_channel_3_enable = 0x0000000;
+
+		if (acodec_config->tdm8_ch_mode == ENABLE) {
+			msp_config.def_elem_len = 1;
+
+			msp_config.protocol_desc.tx_element_length_1 =
+			    MSP_ELEM_LENGTH_20;
+			msp_config.protocol_desc.tx_frame_length_1 =
+			    MSP_FRAME_LENGTH_8;
+			msp_config.protocol_desc.tx_data_delay = MSP_DELAY_1;
+
+			msp_config.protocol_desc.tx_element_length_2 =
+			    MSP_ELEM_LENGTH_8;
+			msp_config.protocol_desc.tx_frame_length_2 =
+			    MSP_FRAME_LENGTH_1;
+
+			msp_config.protocol_desc.rx_element_length_1 =
+			    MSP_ELEM_LENGTH_20;
+			msp_config.protocol_desc.rx_frame_length_1 =
+			    MSP_FRAME_LENGTH_8;
+			msp_config.protocol_desc.rx_data_delay = MSP_DELAY_1;
+
+			msp_config.protocol_desc.rx_element_length_2 =
+			    MSP_ELEM_LENGTH_8;
+			msp_config.protocol_desc.rx_frame_length_2 =
+			    MSP_FRAME_LENGTH_1;
+
+			msp_config.protocol_desc.frame_sync_ignore =
+			    MSP_FRAME_SYNC_UNIGNORE;
+			msp_config.protocol_desc.rx_clock_pol = MSP_RISING_EDGE;
+
+			//if(acodec_config->digital_loopback == ENABLE) {
+			if (1) {
+				msp_config.multichannel_config.
+				    tx_channel_0_enable =
+				    (1 << NB_OF_CHANNEL_USED) - 1;
+				msp_config.multichannel_config.
+				    rx_channel_0_enable =
+				    (1 << NB_OF_CHANNEL_USED) - 1;
+			} else {
+				msp_config.multichannel_config.
+				    tx_channel_0_enable = 0x3;
+				msp_config.multichannel_config.
+				    rx_channel_0_enable = 0x3;
+			}
+		}
+
+		if (acodec_config->tdm8_ch_mode == ENABLE) {
+			/* TFSDLY = 2 delay units */
+			msp_config.iodelay = 0x20;
+		}
+
+		error_status = i2s_setup(i2s_dev->controller, &msp_config);
+		if (error_status < 0) {
+			stm_error("error in msp enable, error_status is %d\n",
+				  error_status);
+			return error_status;
+		}
+	} else if (ENABLE == acodec_config->direct_rendering_mode) {
+		writel(0x00, ((char *)(IO_ADDRESS(U8500_MSP1_BASE) + 0x04)));	//MSP_GCR
+	}
+
+	if (ACODEC_CONFIG_REQUIRED == acodec_config->acodec_config_need) {
+		AB8500_CODEC_SelectInterface(AB8500_CODEC_AUDIO_INTERFACE_0);
+
+		codec_error = AB8500_CODEC_PowerUp();
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("AB8500_CODEC_PowerUp failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+
+/*#ifdef CONFIG_U8500_AB8500_CUT10*/
+#if 1
+		tdm_config.cr27_if1_bitclk_osr =
+		    AB8500_CODEC_CR27_IF1_BITCLK_OSR_32;
+		tdm_config.cr27_if0_bitclk_osr =
+		    AB8500_CODEC_CR27_IF0_BITCLK_OSR_32;
+		tdm_config.cr28_if0wl = AB8500_CODEC_CR28_IF0WL_16BITS;
+		tdm_config.cr30_if1wl = AB8500_CODEC_CR30_IF1WL_16BITS;
+
+		switch (acodec_config->direction) {
+		case AB8500_CODEC_DIRECTION_INOUT:
+			tdm_config.cr28_bitclk0p =
+			    AB8500_CODEC_CR28_BITCLK0P_FALLING_EDGE;
+			tdm_config.cr28_if0del =
+			    AB8500_CODEC_CR28_IF0DEL_DELAYED;
+			break;
+		case AB8500_CODEC_DIRECTION_IN:
+			tdm_config.cr28_bitclk0p =
+			    AB8500_CODEC_CR28_BITCLK0P_RISING_EDGE;
+			tdm_config.cr28_if0del =
+			    AB8500_CODEC_CR28_IF0DEL_NOT_DELAYED;
+			break;
+		case AB8500_CODEC_DIRECTION_OUT:
+			tdm_config.cr28_bitclk0p =
+			    AB8500_CODEC_CR28_BITCLK0P_FALLING_EDGE;
+			tdm_config.cr28_if0del =
+			    AB8500_CODEC_CR28_IF0DEL_DELAYED;
+			break;
+		default:
+			stm_error("Invalid direction\n");
+			return AB8500_CODEC_ERROR;
+		}
+
+		if (acodec_config->tdm8_ch_mode == ENABLE) {
+			tdm_config.cr27_if0_bitclk_osr =
+			    AB8500_CODEC_CR27_IF0_BITCLK_OSR_256;
+			tdm_config.cr28_if0wl = AB8500_CODEC_CR28_IF0WL_20BITS;
+			tdm_config.cr28_bitclk0p =
+			    AB8500_CODEC_CR28_BITCLK0P_RISING_EDGE;
+			tdm_config.cr28_if0del =
+			    AB8500_CODEC_CR28_IF0DEL_DELAYED;
+			codec_in_mode = AB8500_CODEC_MODE_VOICE;
+			codec_out_mode = AB8500_CODEC_MODE_VOICE;
+			acodec_config->direction = AB8500_CODEC_DIRECTION_INOUT;
+		}
+
+		codec_error =
+		    AB8500_CODEC_SetModeAndDirection(acodec_config->direction,
+						     codec_in_mode,
+						     codec_out_mode,
+						     &tdm_config);
+#else
+		codec_error =
+		    AB8500_CODEC_SetModeAndDirection(acodec_config->direction,
+						     codec_in_mode,
+						     codec_out_mode);
+#endif
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("set mode and direction failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+
+		codec_error =
+		    AB8500_CODEC_SetMasterMode(AB8500_CODEC_MASTER_MODE_ENABLE);
+
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("set mode and direction failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+
+		/*codec_error = trg_codec_set_sample_frequency(0); */
+
+		/*u8500_acodec_set_volume(g_codec_system_context.in_left_volume,
+		   g_codec_system_context.in_right_volume,
+		   g_codec_system_context.out_left_volume,
+		   g_codec_system_context.out_right_volume,
+		   user); */
+#if 0
+		if (AB8500_CODEC_DIRECTION_IN == acodec_config->direction
+		    || AB8500_CODEC_DIRECTION_INOUT ==
+		    acodec_config->direction) {
+
+			u8500_acodec_allocate_ad_slot
+			    (AB8500_CODEC_SRC_D_MICROPHONE_1, TDM_8_CH_MODE);
+			u8500_acodec_allocate_ad_slot
+			    (AB8500_CODEC_SRC_D_MICROPHONE_2, TDM_8_CH_MODE);
+
+			/*codec_error = AB8500_CODEC_ADSlotAllocation (AB8500_CODEC_SLOT0,
+			   AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT3);
+			   if (AB8500_CODEC_OK != codec_error) {
+			   stm_error("ab8500_codec_adslot_allocation failed\n");
+			   return AB8500_CODEC_ERROR;
+			   }
+			   codec_error = AB8500_CODEC_ADSlotAllocation (AB8500_CODEC_SLOT1,
+			   AB8500_CODEC_CR31_TO_CR46_SLOT_IS_TRISTATE);
+			   if (AB8500_CODEC_OK != codec_error) {
+			   stm_error("ab8500_codec_adslot_allocation failed\n");
+			   return AB8500_CODEC_ERROR;
+			   } */
+
+		}
+
+		if (AB8500_CODEC_DIRECTION_OUT == acodec_config->direction
+		    || AB8500_CODEC_DIRECTION_INOUT ==
+		    acodec_config->direction) {
+			u8500_acodec_allocate_da_slot(AB8500_CODEC_DEST_HEADSET,
+						      TDM_8_CH_MODE);
+			/*codec_error = AB8500_CODEC_DASlotAllocation (AB8500_CODEC_DA_CHANNEL_NUMBER_1,
+			   AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT08);
+			   if (AB8500_CODEC_OK != codec_error) {
+			   stm_error
+			   ("ab8500_codec_daslot_allocation failed\n");
+			   return AB8500_CODEC_ERROR;
+			   }
+			   codec_error = AB8500_CODEC_DASlotAllocation (AB8500_CODEC_DA_CHANNEL_NUMBER_2,
+			   AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT09);
+			   if (AB8500_CODEC_OK != codec_error) {
+			   stm_error
+			   ("ab8500_codec_daslot_allocation failed\n");
+			   return AB8500_CODEC_ERROR;
+			   } */
+
+		}
+#endif
+
+	}			//END of if acodec_config_need
+
+/*#if DRIVER_DEBUG > 0
+	{
+		dump_msp_registers();
+		dump_acodec_registers();
+	}
+#endif*/
+
+	stm_dbg(DBG_ST.acodec,
+		"leaving in u8500_acodec_enable_audio_mode() \n");
+
+	FUNC_EXIT();
+	return AB8500_CODEC_OK;
+}
+
+/**
+* u8500_acodec_set_output_volume - configures the volume level for both speakers
+* @in_left_volume - volume for left channel of mic
+* @in_right_volume - volume for right channel of mic
+* @out_left_volume - volume for left speaker
+* @out_right_volume - volume for right speaker
+*/
+t_ab8500_codec_error u8500_acodec_set_output_volume(t_ab8500_codec_dest
+						    dest_device,
+						    int left_volume,
+						    int right_volume,
+						    t_acodec_user user)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	stm_dbg(DBG_ST.acodec,
+		"  Entering in u8500_acodec_set_output_volume()\n");
+
+	FUNC_ENTER();
+
+	user = user;		//keep compiler happy
+
+	g_codec_system_context.output_config[dest_device].left_volume =
+	    left_volume;
+	g_codec_system_context.output_config[dest_device].right_volume =
+	    right_volume;
+
+	AB8500_CODEC_SetDestVolume(dest_device, left_volume, right_volume);
+
+	FUNC_EXIT();
+	return codec_error;
+}
+
+/*u8500_acodec_get_output_volume*/
+
+t_ab8500_codec_error u8500_acodec_get_output_volume(t_ab8500_codec_dest
+						    dest_device,
+						    int *p_left_volume,
+						    int *p_right_volume,
+						    t_acodec_user user)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	stm_dbg(DBG_ST.acodec,
+		"  Entering in u8500_acodec_set_output_volume()\n");
+
+	user = user;		//keep compiler happy
+
+	*p_left_volume =
+	    g_codec_system_context.output_config[dest_device].left_volume;
+	*p_right_volume =
+	    g_codec_system_context.output_config[dest_device].right_volume;
+
+	return codec_error;
+}
+
+/**
+* u8500_acodec_set_input_volume - configures the volume level for both speakers
+* @in_left_volume - volume for left channel of mic
+* @in_right_volume - volume for right channel of mic
+* @out_left_volume - volume for left speaker
+* @out_right_volume - volume for right speaker
+*/
+t_ab8500_codec_error u8500_acodec_set_input_volume(t_ab8500_codec_src
+						   src_device, int left_volume,
+						   int right_volume,
+						   t_acodec_user user)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	stm_dbg(DBG_ST.acodec,
+		"  Entering in u8500_acodec_set_input_volume()\n");
+
+	user = user;		//keep compiler happy
+
+	g_codec_system_context.input_config[src_device].left_volume =
+	    left_volume;
+	g_codec_system_context.input_config[src_device].right_volume =
+	    right_volume;
+
+	AB8500_CODEC_SetSrcVolume(src_device, left_volume, right_volume);
+
+	return codec_error;
+}
+
+/**
+* u8500_acodec_get_input_volume - configures the volume level for both speakers
+* @in_left_volume - volume for left channel of mic
+* @in_right_volume - volume for right channel of mic
+* @out_left_volume - volume for left speaker
+* @out_right_volume - volume for right speaker
+*/
+t_ab8500_codec_error u8500_acodec_get_input_volume(t_ab8500_codec_src
+						   src_device,
+						   int *p_left_volume,
+						   int *p_right_volume,
+						   t_acodec_user user)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	stm_dbg(DBG_ST.acodec,
+		"  Entering in u8500_acodec_get_input_volume()\n");
+
+	user = user;		//keep compiler happy
+
+	*p_left_volume =
+	    g_codec_system_context.input_config[src_device].left_volume;
+	*p_right_volume =
+	    g_codec_system_context.input_config[src_device].right_volume;
+
+	return codec_error;
+}
+
+/**
+* u8500_acodec_toggle_playback_mute_control - configures the mute for both speakers
+* @in_left_volume - volume for left channel of mic
+* @in_right_volume - volume for right channel of mic
+* @out_left_volume - volume for left speaker
+* @out_right_volume - volume for right speaker
+*/
+t_ab8500_codec_error
+u8500_acodec_toggle_playback_mute_control(t_ab8500_codec_dest dest_device,
+					  t_u8500_bool_state mute_state,
+					  t_acodec_user user)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	stm_dbg(DBG_ST.acodec,
+		"  Entering in u8500_acodec_toggle_playback_mute_control \n");
+
+	user = user;		//keep compiler happy
+
+	g_codec_system_context.output_config[dest_device].mute_state =
+	    mute_state;
+
+	if (ENABLE == mute_state) {
+		AB8500_CODEC_DestPowerControl(dest_device,
+					      AB8500_CODEC_SRC_STATE_ENABLE);
+	} else {
+		AB8500_CODEC_DestPowerControl(dest_device,
+					      AB8500_CODEC_SRC_STATE_DISABLE);
+	}
+
+	return codec_error;
+}
+
+/**
+* u8500_acodec_toggle_capture_mute_control - configures the mute for both speakers
+* @in_left_volume - volume for left channel of mic
+* @in_right_volume - volume for right channel of mic
+* @out_left_volume - volume for left speaker
+* @out_right_volume - volume for right speaker
+*/
+t_ab8500_codec_error u8500_acodec_toggle_capture_mute_control(t_ab8500_codec_src
+							      src_device,
+							      t_u8500_bool_state
+							      mute_state,
+							      t_acodec_user
+							      user)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	stm_dbg(DBG_ST.acodec,
+		"  Entering in u8500_acodec_toggle_capture_mute_control \n");
+
+	user = user;		//keep compiler happy
+
+	g_codec_system_context.input_config[src_device].mute_state = mute_state;
+
+	if (ENABLE == mute_state) {
+		AB8500_CODEC_SrcPowerControl(src_device,
+					     AB8500_CODEC_SRC_STATE_ENABLE);
+	} else {
+		AB8500_CODEC_SrcPowerControl(src_device,
+					     AB8500_CODEC_SRC_STATE_DISABLE);
+	}
+
+	return codec_error;
+}
+
+/**
+* u8500_acodec_select_input
+* @input_device: MIC or linein.
+*
+* This routine selects the input device mic or linein.
+*/
+
+t_ab8500_codec_error u8500_acodec_select_input(t_ab8500_codec_src
+					       input_device,
+					       t_acodec_user user,
+					       t_u8500_mode mode)
+{
+
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	stm_dbg(DBG_ST.acodec, "  Entering u8500_acodec_select_input\n");
+
+	if (TDM_8_CH_MODE == mode)
+		u8500_acodec_allocate_ad_slot(input_device, TDM_8_CH_MODE);
+	else
+		u8500_acodec_allocate_ad_slot(input_device, CLASSICAL_MODE);
+
+	codec_error = AB8500_CODEC_SelectInput(input_device);
+
+	stm_dbg(DBG_ST.acodec, "  leaving u8500_acodec_select_input\n");
+	return codec_error;
+}
+
+/**
+* u8500_acodec_select_output
+* @output_device: output device HP/LSP
+*
+* This routine selects the output device Headphone or loud speaker
+*/
+
+t_ab8500_codec_error u8500_acodec_select_output(t_ab8500_codec_dest
+						output_device,
+						t_acodec_user user,
+						t_u8500_mode mode)
+{
+
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	FUNC_ENTER();
+	stm_dbg(DBG_ST.acodec, "  Entering u8500_acodec_select_output()\n");
+
+	if (TDM_8_CH_MODE == mode)
+		u8500_acodec_allocate_da_slot(output_device, TDM_8_CH_MODE);
+	else
+		u8500_acodec_allocate_da_slot(output_device, CLASSICAL_MODE);
+
+	codec_error = AB8500_CODEC_SelectOutput(output_device);
+
+	stm_dbg(DBG_ST.acodec, "  leaving u8500_acodec_select_output()\n");
+	FUNC_EXIT();
+	return codec_error;
+}
+
+t_ab8500_codec_error u8500_acodec_allocate_ad_slot(t_ab8500_codec_src
+						   input_device,
+						   t_u8500_mode mode)
+{
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation ad_data_line1,
+	    ad_data_line2;
+	t_ab8500_codec_slot slot1, slot2;
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	slot1 = AB8500_CODEC_SLOT_UNDEFINED;
+	slot2 = AB8500_CODEC_SLOT_UNDEFINED;
+
+	ad_data_line1 =
+	    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_UNDEFINED;
+	ad_data_line2 =
+	    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_UNDEFINED;
+
+	switch (input_device) {
+	case AB8500_CODEC_SRC_D_MICROPHONE_1:
+		{
+			slot1 = AB8500_CODEC_SLOT0;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT1;
+		}
+		break;
+	case AB8500_CODEC_SRC_MICROPHONE_2:
+	case AB8500_CODEC_SRC_D_MICROPHONE_2:
+		{
+			slot1 = AB8500_CODEC_SLOT1;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT2;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_3:
+	case AB8500_CODEC_SRC_MICROPHONE_1A:
+	case AB8500_CODEC_SRC_MICROPHONE_1B:
+		{
+			slot1 = AB8500_CODEC_SLOT2;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT3;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_4:
+		{
+			slot1 = AB8500_CODEC_SLOT3;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT4;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_5:
+		{
+			slot1 = AB8500_CODEC_SLOT4;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT5;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_6:
+		{
+			slot1 = AB8500_CODEC_SLOT5;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT6;
+		}
+		break;
+	case AB8500_CODEC_SRC_LINEIN:
+		{
+			slot1 = AB8500_CODEC_SLOT0;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT1;
+
+			slot2 = AB8500_CODEC_SLOT1;
+			ad_data_line2 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT2;
+		}
+	case AB8500_CODEC_SRC_FM_RX:
+		{
+			slot1 = AB8500_CODEC_SLOT6;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT7;
+
+			slot2 = AB8500_CODEC_SLOT7;
+			ad_data_line2 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT8;
+		}
+		break;
+	case AB8500_CODEC_SRC_ALL:
+		break;
+	}
+
+	if ((AB8500_CODEC_SLOT_UNDEFINED != slot1)
+	    && (AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_UNDEFINED !=
+		ad_data_line1)) {
+		if (CLASSICAL_MODE == mode) {
+			slot1 = AB8500_CODEC_SLOT0;
+		}
+		codec_error =
+		    AB8500_CODEC_ADSlotAllocation(slot1, ad_data_line1);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_adslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+	}
+
+	if ((AB8500_CODEC_SLOT_UNDEFINED != slot2)
+	    && (AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_UNDEFINED !=
+		ad_data_line2)) {
+		if (CLASSICAL_MODE == mode) {
+			slot2 = AB8500_CODEC_SLOT1;
+		}
+		codec_error =
+		    AB8500_CODEC_ADSlotAllocation(slot2, ad_data_line2);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_adslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+	}
+
+	return AB8500_CODEC_OK;
+}
+
+t_ab8500_codec_error u8500_acodec_unallocate_ad_slot(t_ab8500_codec_src
+						     input_device,
+						     t_u8500_mode mode)
+{
+	t_ab8500_codec_slot slot1, slot2;
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	slot1 = AB8500_CODEC_SLOT_UNDEFINED;
+	slot2 = AB8500_CODEC_SLOT_UNDEFINED;
+
+	switch (input_device) {
+	case AB8500_CODEC_SRC_D_MICROPHONE_1:
+		{
+			slot1 = AB8500_CODEC_SLOT0;
+		}
+		break;
+	case AB8500_CODEC_SRC_MICROPHONE_2:
+	case AB8500_CODEC_SRC_D_MICROPHONE_2:
+		{
+			slot1 = AB8500_CODEC_SLOT1;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_3:
+	case AB8500_CODEC_SRC_MICROPHONE_1A:
+	case AB8500_CODEC_SRC_MICROPHONE_1B:
+		{
+			slot1 = AB8500_CODEC_SLOT2;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_4:
+		{
+			slot1 = AB8500_CODEC_SLOT3;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_5:
+		{
+			slot1 = AB8500_CODEC_SLOT4;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_6:
+		{
+			slot1 = AB8500_CODEC_SLOT5;
+		}
+		break;
+	case AB8500_CODEC_SRC_LINEIN:
+		{
+			slot1 = AB8500_CODEC_SLOT0;
+			slot2 = AB8500_CODEC_SLOT1;
+		}
+		break;
+	case AB8500_CODEC_SRC_ALL:
+		break;
+	}
+
+	if (AB8500_CODEC_SLOT_UNDEFINED != slot1) {
+		if (CLASSICAL_MODE == mode) {
+			slot1 = AB8500_CODEC_SLOT0;
+		}
+		codec_error =
+		    AB8500_CODEC_ADSlotAllocation(slot1,
+						  AB8500_CODEC_CR31_TO_CR46_SLOT_IS_TRISTATE);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_adslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+	}
+
+	if (AB8500_CODEC_SLOT_UNDEFINED != slot2) {
+		if (CLASSICAL_MODE == mode) {
+			slot2 = AB8500_CODEC_SLOT1;
+		}
+		codec_error =
+		    AB8500_CODEC_ADSlotAllocation(slot2,
+						  AB8500_CODEC_CR31_TO_CR46_SLOT_IS_TRISTATE);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_adslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+	}
+
+	return AB8500_CODEC_OK;
+}
+
+#ifdef CONFIG_U8500_AB8500_CUT10
+t_ab8500_codec_error u8500_acodec_allocate_da_slot(t_ab8500_codec_dest
+						   output_device,
+						   t_u8500_mode mode)
+{
+	t_ab8500_codec_da_channel_number da_ch_no1, da_ch_no2;
+	t_ab8500_codec_cr51_to_cr58_sltoda da_slot1, da_slot2;
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_UNDEFINED;
+	da_ch_no2 = AB8500_CODEC_DA_CHANNEL_NUMBER_UNDEFINED;
+
+	da_slot1 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT_UNDEFINED;
+	da_slot2 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT_UNDEFINED;
+
+	switch (output_device) {
+	case AB8500_CODEC_DEST_HEADSET:
+		{
+			da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_1;
+			da_slot1 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT08;
+
+			da_ch_no2 = AB8500_CODEC_DA_CHANNEL_NUMBER_2;
+			da_slot2 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT09;
+		}
+		break;
+	case AB8500_CODEC_DEST_EARPIECE:
+		{
+			da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_1;
+			da_slot1 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT08;
+		}
+		break;
+	case AB8500_CODEC_DEST_HANDSFREE:
+		{
+			da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_3;
+			da_slot1 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT10;
+
+			da_ch_no2 = AB8500_CODEC_DA_CHANNEL_NUMBER_4;
+			da_slot2 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT11;
+		}
+		break;
+	case AB8500_CODEC_DEST_VIBRATOR_L:
+		{
+			da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_5;
+			da_slot1 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT12;
+		}
+		break;
+	case AB8500_CODEC_DEST_VIBRATOR_R:
+		{
+			da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_6;
+			da_slot1 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT13;
+		}
+		break;
+
+	case AB8500_CODEC_DEST_FM_TX:
+		{
+			da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_7;
+			da_slot1 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT14;
+
+			da_ch_no2 = AB8500_CODEC_DA_CHANNEL_NUMBER_8;
+			da_slot2 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT15;
+		}
+
+	case AB8500_CODEC_DEST_ALL:
+		break;
+	}
+
+	if ((AB8500_CODEC_DA_CHANNEL_NUMBER_UNDEFINED != da_ch_no1)
+	    && (AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT_UNDEFINED != da_slot1)) {
+		if (CLASSICAL_MODE == mode) {
+			da_slot1 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT08;
+		}
+		codec_error =
+		    AB8500_CODEC_DASlotAllocation(da_ch_no1, da_slot1);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_daslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+	}
+
+	if ((AB8500_CODEC_DA_CHANNEL_NUMBER_UNDEFINED != da_ch_no2)
+	    && (AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT_UNDEFINED != da_slot2)) {
+		if (CLASSICAL_MODE == mode) {
+			da_slot1 = AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT09;
+		}
+		codec_error =
+		    AB8500_CODEC_DASlotAllocation(da_ch_no2, da_slot2);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_daslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+	}
+
+	return AB8500_CODEC_OK;
+}
+
+#else
+t_ab8500_codec_error u8500_acodec_allocate_da_slot(t_ab8500_codec_dest
+						   output_device,
+						   t_u8500_mode mode)
+{
+	t_ab8500_codec_da_channel_number da_ch_no1, da_ch_no2;
+	t_ab8500_codec_cr51_to_cr56_sltoda da_slot1, da_slot2;
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_UNDEFINED;
+	da_ch_no2 = AB8500_CODEC_DA_CHANNEL_NUMBER_UNDEFINED;
+
+	da_slot1 = AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT_UNDEFINED;
+	da_slot2 = AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT_UNDEFINED;
+
+	switch (output_device) {
+	case AB8500_CODEC_DEST_HEADSET:
+		{
+			da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_1;
+			da_slot1 = AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT08;
+
+			da_ch_no2 = AB8500_CODEC_DA_CHANNEL_NUMBER_2;
+			da_slot2 = AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT09;
+		}
+		break;
+	case AB8500_CODEC_DEST_EARPIECE:
+		{
+			da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_1;
+			da_slot1 = AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT08;
+		}
+		break;
+	case AB8500_CODEC_DEST_HANDSFREE:
+		{
+			da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_3;
+			da_slot1 = AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT10;
+
+			da_ch_no2 = AB8500_CODEC_DA_CHANNEL_NUMBER_4;
+			da_slot2 = AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT11;
+		}
+		break;
+	case AB8500_CODEC_DEST_VIBRATOR_L:
+		{
+			da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_5;
+			da_slot1 = AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT12;
+		}
+		break;
+	case AB8500_CODEC_DEST_VIBRATOR_R:
+		{
+			da_ch_no1 = AB8500_CODEC_DA_CHANNEL_NUMBER_6;
+			da_slot1 = AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT13;
+		}
+		break;
+
+	case AB8500_CODEC_DEST_ALL:
+		break;
+	}
+
+	if ((AB8500_CODEC_DA_CHANNEL_NUMBER_UNDEFINED != da_ch_no1)
+	    && (AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT_UNDEFINED != da_slot1)) {
+		if (CLASSICAL_MODE == mode) {
+			da_slot1 = AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT08;
+		}
+		codec_error =
+		    AB8500_CODEC_DASlotAllocation(da_ch_no1, da_slot1);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_daslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+	}
+
+	if ((AB8500_CODEC_DA_CHANNEL_NUMBER_UNDEFINED != da_ch_no2)
+	    && (AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT_UNDEFINED != da_slot2)) {
+		if (CLASSICAL_MODE == mode) {
+			da_slot1 = AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT09;
+		}
+		codec_error =
+		    AB8500_CODEC_DASlotAllocation(da_ch_no2, da_slot2);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_daslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+	}
+
+	return AB8500_CODEC_OK;
+}
+#endif
+
+t_ab8500_codec_error u8500_acodec_unallocate_da_slot(t_ab8500_codec_dest
+						     output_device,
+						     t_u8500_mode mode)
+{
+	return AB8500_CODEC_OK;
+}
+
+t_ab8500_codec_error u8500_acodec_set_src_power_cntrl(t_ab8500_codec_src
+						      input_device,
+						      t_u8500_bool_state
+						      pwr_state)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	if (ENABLE == pwr_state) {
+		u8500_acodec_allocate_ad_slot(input_device, TDM_8_CH_MODE);
+		codec_error =
+		    AB8500_CODEC_SrcPowerControl(input_device,
+						 AB8500_CODEC_SRC_STATE_ENABLE);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("AB8500_CODEC_SrcPowerControl failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+		g_codec_system_context.input_config[input_device].power_state =
+		    ENABLE;
+	} else {
+		u8500_acodec_unallocate_ad_slot(input_device, TDM_8_CH_MODE);
+		codec_error =
+		    AB8500_CODEC_SrcPowerControl(input_device,
+						 AB8500_CODEC_SRC_STATE_DISABLE);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("AB8500_CODEC_SrcPowerControl failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+		g_codec_system_context.input_config[input_device].power_state =
+		    DISABLE;
+	}
+
+	return AB8500_CODEC_OK;
+}
+
+t_u8500_bool_state u8500_acodec_get_src_power_state(t_ab8500_codec_src
+						    input_device)
+{
+	return (g_codec_system_context.input_config[input_device].power_state);
+}
+
+t_ab8500_codec_error u8500_acodec_set_dest_power_cntrl(t_ab8500_codec_dest
+						       output_device,
+						       t_u8500_bool_state
+						       pwr_state)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	if (ENABLE == pwr_state) {
+		AB8500_CODEC_SelectInterface(AB8500_CODEC_AUDIO_INTERFACE_0);
+
+		codec_error = AB8500_CODEC_PowerUp();
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("AB8500_CODEC_PowerUp failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+
+		u8500_acodec_allocate_da_slot(output_device, TDM_8_CH_MODE);
+
+		codec_error =
+		    AB8500_CODEC_DestPowerControl(output_device,
+						  AB8500_CODEC_DEST_STATE_ENABLE);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("AB8500_CODEC_DestPowerControl failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+		g_codec_system_context.output_config[output_device].
+		    power_state = ENABLE;
+	} else {
+		u8500_acodec_unallocate_da_slot(output_device, TDM_8_CH_MODE);
+		codec_error =
+		    AB8500_CODEC_DestPowerControl(output_device,
+						  AB8500_CODEC_DEST_STATE_DISABLE);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("AB8500_CODEC_DestPowerControl failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+		g_codec_system_context.output_config[output_device].
+		    power_state = DISABLE;
+	}
+
+	return AB8500_CODEC_OK;
+}
+
+t_u8500_bool_state u8500_acodec_get_dest_power_state(t_ab8500_codec_dest
+						     output_device)
+{
+	return (g_codec_system_context.output_config[output_device].
+		power_state);
+}
+
+/**
+* u8500_acodec_toggle_analog_lpbk
+* @output_device: output device HP/LSP
+*
+* This routine selects the output device Headphone or loud speaker
+*/
+t_ab8500_codec_error u8500_acodec_toggle_analog_lpbk(t_u8500_bool_state
+						     lpbk_state,
+						     t_acodec_user user)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	stm_dbg(DBG_ST.acodec,
+		"  Entering inu8500_acodec_toggle_analog_lpbk() \n");
+
+	user = user;		//keep compiler happy
+
+	if (ENABLE == lpbk_state) {
+		/* Reset CODEC */
+		codec_error = AB8500_CODEC_Reset();
+
+		AB8500_CODEC_SelectInterface(AB8500_CODEC_AUDIO_INTERFACE_0);
+
+		codec_error = AB8500_CODEC_PowerUp();
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("AB8500_CODEC_PowerUp failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+
+		codec_error = AB8500_CODEC_SelectInput(AB8500_CODEC_SRC_LINEIN);
+
+		codec_error =
+		    AB8500_CODEC_SrcPowerControl(AB8500_CODEC_SRC_LINEIN,
+						 AB8500_CODEC_SRC_STATE_ENABLE);
+
+		//codec_error = AB8500_CODEC_SetSrcVolume(AB8500_CODEC_SRC_LINEIN,VOL_MAX,VOL_MAX);
+
+		codec_error =
+		    AB8500_CODEC_SelectOutput(AB8500_CODEC_DEST_HEADSET);
+
+		codec_error =
+		    AB8500_CODEC_DestPowerControl(AB8500_CODEC_DEST_HEADSET,
+						  AB8500_CODEC_DEST_STATE_ENABLE);
+
+		//codec_error = AB8500_CODEC_SetDestVolume(AB8500_CODEC_DEST_HEADSET,0,0);
+
+		codec_error = AB8500_CODEC_SetAnalogLoopback(VOL_MAX, VOL_MAX);
+
+		ab8500_write(AB8500_AUDIO, 0xd05, 0x30);
+		ab8500_write(AB8500_AUDIO, 0xd07, 0xf3);
+		ab8500_write(AB8500_AUDIO, 0xd16, 0xdd);
+		ab8500_write(AB8500_AUDIO, 0xd17, 0x55);
+		ab8500_write(AB8500_AUDIO, 0xd3f, 0xc0);
+
+	} else {
+		codec_error = AB8500_CODEC_RemoveAnalogLoopback();
+	}
+
+#if DRIVER_DEBUG > 0
+	{
+		dump_acodec_registers();
+	}
+#endif
+
+	return codec_error;
+}
+
+#ifdef CONFIG_U8500_ACODEC_POLL
+
+static int digital_lpbk_msp_rx_tx_thread(void *data)
+{
+	t_digital_lpbk_cnxt *p_cnxt = (t_digital_lpbk_cnxt *) data;
+	unsigned int sample[8], count = 32;
+
+	daemonize("digital_lpbk_msp_rx_tx_thread");
+	allow_signal(SIGKILL);
+
+	printk("\n Rx-Tx : digital_lpbk_msp_rx_tx_thread started \n");
+
+	while ((!signal_pending(current)) && (p_cnxt->rx_active)) {
+
+//              ret_val = u8500_msp_receive_data(alsa_msp_adev,p_cnxt->buffer[p_cnxt->rx_index],p_cnxt->data_size);
+
+		//u8500_msp_transceive_data(alsa_msp_adev,p_cnxt->buffer[0], p_cnxt->data_size,p_cnxt->buffer[1], p_cnxt->data_size);
+
+		//u8500_msp_transceive_data(alsa_msp_adev,p_cnxt->buffer[1], p_cnxt->data_size,p_cnxt->buffer[0], p_cnxt->data_size);
+
+#if DRIVER_DEBUG > 1
+		stm_dbg(DBG_ST.alsa, " Receiving \n");
+#endif
+		u8500_acodec_receive_data(I2S_CLIENT_MSP1, (void *)sample,
+					  count, 0);
+
+#if DRIVER_DEBUG > 1
+		stm_dbg(DBG_ST.alsa, " Transmitting \n");
+#endif
+		u8500_acodec_send_data(I2S_CLIENT_MSP1, (void *)sample, count,
+				       0);
+
+	}
+	printk("\n Rx-Tx : digital_lpbk_msp_rx_tx_thread ended \n");
+	return 0;
+}
+
+#endif
+
+#ifdef CONFIG_U8500_ACODEC_DMA
+
+static void u8500_digital_lpbk_dma_start()
+{
+	u8500_acodec_loopback_configure(I2S_CLIENT_MSP1,
+					(void *)digital_lpbk_cnxt.buffer.addr,
+					digital_lpbk_cnxt.data_size, 1);
+
+	stm_dbg(DBG_ST.alsa, " Rx DMA Transfer started\n");
+	stm_dbg(DBG_ST.alsa, " Rx : add = %x  size=%d\n",
+		(int)(digital_lpbk_cnxt.buffer.addr),
+		digital_lpbk_cnxt.data_size);
+
+}
+#endif
+
+/**
+* u8500_acodec_toggle_digital_lpbk
+* @output_device: output device HP/LSP
+*
+* This routine selects the output device Headphone or loud speaker
+*/
+
+t_ab8500_codec_error u8500_acodec_toggle_digital_lpbk(t_u8500_bool_state
+						      lpbk_state,
+						      t_ab8500_codec_dest
+						      dest_device,
+						      t_ab8500_codec_src
+						      src_device,
+						      t_acodec_user user,
+						      t_u8500_bool_state
+						      tdm8_ch_mode)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+	struct acodec_configuration acodec_config;
+	int status = 0;
+
+	stm_dbg(DBG_ST.acodec,
+		"  Entering u8500_acodec_toggle_digital_lpbk() \n");
+
+	user = user;		//keep compiler happy
+
+	if (ENABLE == lpbk_state) {
+		//data_size = 1024*100;
+
+		//data[0] = (unsigned char *)kmalloc(data_size, GFP_KERNEL);
+
+		codec_error = AB8500_CODEC_Reset();
+
+		//AB8500_CODEC_SelectInterface(AB8500_CODEC_AUDIO_INTERFACE_0);
+
+		//codec_error = AB8500_CODEC_PowerUp();
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("AB8500_CODEC_PowerUp failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+
+		status = u8500_acodec_open(I2S_CLIENT_MSP1, 0);
+		if (status) {
+			printk("failed in getting acodec playback open\n");
+			return -1;
+		}
+		status = u8500_acodec_open(I2S_CLIENT_MSP1, 1);
+		if (status) {
+			printk("failed in getting acdoec capture open\n");
+			return -1;
+		}
+
+		u8500_acodec_setuser(USER_ALSA);
+
+		if (ENABLE == tdm8_ch_mode) {
+			printk("\n 20 bit 8 ch Digital Loopback");
+			printk("\n DMIC1 -> HS-L");
+			printk("\n DMIC2 -> HS-R");
+			printk("\n DMIC3 -> IHF-L");
+			printk("\n DMIC5 -> Vibra-L\n");
+			printk("\n DMIC6 -> Vibra-R\n");
+			printk("\n FM    -> FM Tx\n");
+		} else {
+			printk("\n 16 bit 2 ch Digital Loopback");
+			printk("\n DMIC1 -> HS-L");
+			printk("\n DMIC2 -> HS-R");
+		}
+
+		stm_dbg(DBG_ST.alsa, "enabling audiocodec audio mode\n");
+		acodec_config.direction = AB8500_CODEC_DIRECTION_INOUT;
+		acodec_config.input_frequency = T_CODEC_SAMPLING_FREQ_48KHZ;
+		acodec_config.output_frequency = T_CODEC_SAMPLING_FREQ_48KHZ;
+		acodec_config.mspClockSel = CODEC_MSP_APB_CLOCK;
+		acodec_config.mspInClockFreq = CODEC_MSP_INPUT_FREQ_48MHZ;
+		acodec_config.channels = 2;
+		acodec_config.user = 2;
+		acodec_config.acodec_config_need = ACODEC_CONFIG_REQUIRED;
+		acodec_config.direct_rendering_mode = DISABLE;
+		acodec_config.tdm8_ch_mode = tdm8_ch_mode;
+		acodec_config.digital_loopback = ENABLE;
+#ifdef CONFIG_U8500_ACODEC_POLL
+		acodec_config.handler = NULL;
+		acodec_config.tx_callback_data = NULL;
+		acodec_config.rx_callback_data = NULL;
+#endif
+		u8500_acodec_enable_audio_mode(&acodec_config);
+
+		/*turn on src devices */
+
+		perform_src_routing(src_device);
+
+/* 		u8500_acodec_set_src_power_cntrl(src_device,ENABLE);
+		u8500_acodec_set_input_volume(src_device,50,50,USER_ALSA);
+
+		u8500_acodec_set_src_power_cntrl(AB8500_CODEC_SRC_D_MICROPHONE_2,ENABLE);
+		u8500_acodec_set_input_volume(AB8500_CODEC_SRC_D_MICROPHONE_2,50,50,USER_ALSA);
+
+		u8500_acodec_set_src_power_cntrl(AB8500_CODEC_SRC_D_MICROPHONE_3,ENABLE);
+		u8500_acodec_set_input_volume(AB8500_CODEC_SRC_D_MICROPHONE_3,50,50,USER_ALSA);
+
+		u8500_acodec_set_src_power_cntrl(AB8500_CODEC_SRC_D_MICROPHONE_4,ENABLE);
+		u8500_acodec_set_input_volume(AB8500_CODEC_SRC_D_MICROPHONE_4,50,50,USER_ALSA);
+
+		u8500_acodec_set_src_power_cntrl(AB8500_CODEC_SRC_D_MICROPHONE_5,ENABLE);
+		u8500_acodec_set_input_volume(AB8500_CODEC_SRC_D_MICROPHONE_5,50,50,USER_ALSA);
+
+		u8500_acodec_set_src_power_cntrl(AB8500_CODEC_SRC_D_MICROPHONE_6,ENABLE);
+		u8500_acodec_set_input_volume(AB8500_CODEC_SRC_D_MICROPHONE_6,50,50,USER_ALSA);
+
+		u8500_acodec_set_src_power_cntrl(AB8500_CODEC_SRC_D_MICROPHONE_6,ENABLE);
+		u8500_acodec_set_input_volume(AB8500_CODEC_SRC_D_MICROPHONE_6,50,50,USER_ALSA);
+
+		u8500_acodec_set_src_power_cntrl(AB8500_CODEC_SRC_FM_RX,ENABLE); */
+
+		/*turn on dest devices */
+
+		//u8500_acodec_set_dest_power_cntrl(dest_device,ENABLE);
+		u8500_acodec_allocate_da_slot(dest_device, TDM_8_CH_MODE);
+		codec_error = AB8500_CODEC_SelectOutput(dest_device);
+		u8500_acodec_set_output_volume(dest_device, 100, 100,
+					       USER_ALSA);
+
+		/*u8500_acodec_set_dest_power_cntrl(AB8500_CODEC_DEST_HEADSET,ENABLE);
+		   u8500_acodec_set_output_volume(AB8500_CODEC_DEST_HEADSET,100,100,USER_ALSA); */
+
+		/*u8500_acodec_set_dest_power_cntrl(AB8500_CODEC_DEST_HANDSFREE,ENABLE);
+		   u8500_acodec_set_output_volume(AB8500_CODEC_DEST_HANDSFREE,100,100,USER_ALSA); */
+
+#ifdef CONFIG_U8500_AB8500_CUT10
+		codec_error =
+		    AB8500_CODEC_DASlotAllocation
+		    (AB8500_CODEC_DA_CHANNEL_NUMBER_5,
+		     AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT12);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_daslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+
+		codec_error =
+		    AB8500_CODEC_DASlotAllocation
+		    (AB8500_CODEC_DA_CHANNEL_NUMBER_6,
+		     AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT13);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_daslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+
+		codec_error =
+		    AB8500_CODEC_DASlotAllocation
+		    (AB8500_CODEC_DA_CHANNEL_NUMBER_7,
+		     AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT14);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_daslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+
+		codec_error =
+		    AB8500_CODEC_DASlotAllocation
+		    (AB8500_CODEC_DA_CHANNEL_NUMBER_8,
+		     AB8500_CODEC_CR51_TO_CR58_SLTODA_SLOT15);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_daslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+#else
+		codec_error =
+		    AB8500_CODEC_DASlotAllocation
+		    (AB8500_CODEC_DA_CHANNEL_NUMBER_5,
+		     AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT12);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_daslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+
+		codec_error =
+		    AB8500_CODEC_DASlotAllocation
+		    (AB8500_CODEC_DA_CHANNEL_NUMBER_6,
+		     AB8500_CODEC_CR51_TO_CR56_SLTODA_SLOT13);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_daslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+#endif
+
+		digital_lpbk_cnxt.data_size = 2048;
+
+		digital_lpbk_cnxt.rx_active = 1;
+		digital_lpbk_cnxt.tx_active = 1;
+
+		digital_lpbk_cnxt.rx_index = 0;
+		digital_lpbk_cnxt.tx_index = 2;
+
+		digital_lpbk_cnxt.buffer.area =
+		    dma_alloc_coherent(NULL, digital_lpbk_cnxt.data_size,
+				       &digital_lpbk_cnxt.buffer.addr,
+				       GFP_KERNEL);
+		if (NULL == digital_lpbk_cnxt.buffer.area) {
+			printk("\n dma_alloc_coherent failed \n");
+		}
+#if DRIVER_DEBUG > 0
+		{
+			dump_msp_registers();
+			dump_acodec_registers();
+		}
+#endif
+
+#ifdef CONFIG_U8500_ACODEC_POLL
+		{
+			pid_t pid_rx_tx;
+			pid_rx_tx =
+			    kernel_thread(digital_lpbk_msp_rx_tx_thread,
+					  &digital_lpbk_cnxt,
+					  CLONE_FS | CLONE_SIGHAND);
+		}
+#elif defined(CONFIG_U8500_ACODEC_DMA)
+		{
+			u8500_digital_lpbk_dma_start();
+		}
+#endif
+	} else			//lpbk is disable
+	{
+
+		digital_lpbk_cnxt.rx_active = 0;
+		digital_lpbk_cnxt.tx_active = 0;
+
+		dma_free_coherent(NULL, digital_lpbk_cnxt.data_size,
+				  digital_lpbk_cnxt.buffer.area,
+				  digital_lpbk_cnxt.buffer.addr);
+
+		u8500_acodec_set_src_power_cntrl
+		    (AB8500_CODEC_SRC_D_MICROPHONE_1, DISABLE);
+		u8500_acodec_set_src_power_cntrl
+		    (AB8500_CODEC_SRC_D_MICROPHONE_2, DISABLE);
+		u8500_acodec_set_src_power_cntrl
+		    (AB8500_CODEC_SRC_D_MICROPHONE_3, DISABLE);
+		u8500_acodec_set_src_power_cntrl
+		    (AB8500_CODEC_SRC_D_MICROPHONE_4, DISABLE);
+
+		u8500_acodec_set_dest_power_cntrl(AB8500_CODEC_DEST_HEADSET,
+						  DISABLE);
+		u8500_acodec_set_dest_power_cntrl(AB8500_CODEC_DEST_HANDSFREE,
+						  DISABLE);
+
+		u8500_acodec_unsetuser(USER_ALSA);
+		u8500_acodec_close(I2S_CLIENT_MSP1, ACODEC_DISABLE_ALL);
+	}
+	return codec_error;
+}
+
+t_ab8500_codec_error perform_src_routing(t_ab8500_codec_src input_device)
+{
+	int src_type = 0;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation ad_data_line1;
+	t_ab8500_codec_cr31_to_cr46_ad_data_allocation ad_data_line2;
+	t_ab8500_codec_src input_device1;
+	t_ab8500_codec_src input_device2;
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	switch (input_device) {
+	case AB8500_CODEC_SRC_D_MICROPHONE_1:
+		{
+			src_type = MONO_SRC;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT1;
+		}
+		break;
+	case AB8500_CODEC_SRC_MICROPHONE_2:
+	case AB8500_CODEC_SRC_D_MICROPHONE_2:
+		{
+			src_type = MONO_SRC;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT2;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_3:
+	case AB8500_CODEC_SRC_MICROPHONE_1A:
+	case AB8500_CODEC_SRC_MICROPHONE_1B:
+		{
+			src_type = MONO_SRC;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT3;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_4:
+		{
+			src_type = MONO_SRC;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT4;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_5:
+		{
+			src_type = MONO_SRC;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT5;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_6:
+		{
+			src_type = MONO_SRC;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT6;
+		}
+		break;
+	case AB8500_CODEC_SRC_LINEIN:
+		{
+			src_type = STEREO_SRC;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT1;
+			ad_data_line2 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT2;
+			input_device1 = AB8500_CODEC_SRC_LINEIN;
+			input_device2 = AB8500_CODEC_SRC_LINEIN;
+		}
+		break;
+#ifdef CONFIG_U8500_AB8500_CUT10
+	case AB8500_CODEC_SRC_D_MICROPHONE_12:
+		{
+			src_type = STEREO_SRC;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT1;
+			ad_data_line2 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT2;
+			input_device1 = AB8500_CODEC_SRC_D_MICROPHONE_1;
+			input_device2 = AB8500_CODEC_SRC_D_MICROPHONE_2;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_34:
+		{
+			src_type = STEREO_SRC;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT3;
+			ad_data_line2 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT4;
+			input_device1 = AB8500_CODEC_SRC_D_MICROPHONE_3;
+			input_device2 = AB8500_CODEC_SRC_D_MICROPHONE_4;
+		}
+		break;
+	case AB8500_CODEC_SRC_D_MICROPHONE_56:
+		{
+			src_type = STEREO_SRC;
+			ad_data_line1 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT5;
+			ad_data_line2 =
+			    AB8500_CODEC_CR31_TO_CR46_SLOT_OUTPUTS_DATA_FROM_AD_OUT6;
+			input_device1 = AB8500_CODEC_SRC_D_MICROPHONE_5;
+			input_device2 = AB8500_CODEC_SRC_D_MICROPHONE_6;
+		}
+		break;
+#endif	/* #ifdef CONFIG_U8500_AB8500_CUT10 */
+	}
+	if (STEREO_SRC == src_type) {
+		u8500_acodec_allocate_all_stereo_slots(ad_data_line1,
+						       ad_data_line2);
+		codec_error =
+		    AB8500_CODEC_SrcPowerControl(input_device1,
+						 AB8500_CODEC_SRC_STATE_ENABLE);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("AB8500_CODEC_SrcPowerControl failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+		g_codec_system_context.input_config[input_device1].power_state =
+		    ENABLE;
+
+		u8500_acodec_set_input_volume(input_device1, 50, 50, USER_ALSA);
+
+		codec_error =
+		    AB8500_CODEC_SrcPowerControl(input_device2,
+						 AB8500_CODEC_SRC_STATE_ENABLE);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("AB8500_CODEC_SrcPowerControl failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+		g_codec_system_context.input_config[input_device2].power_state =
+		    ENABLE;
+
+		u8500_acodec_set_input_volume(input_device2, 50, 50, USER_ALSA);
+	} else {
+		u8500_acodec_allocate_all_mono_slots(ad_data_line1);
+		codec_error =
+		    AB8500_CODEC_SrcPowerControl(input_device,
+						 AB8500_CODEC_SRC_STATE_ENABLE);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("AB8500_CODEC_SrcPowerControl failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+		g_codec_system_context.input_config[input_device].power_state =
+		    ENABLE;
+
+		u8500_acodec_set_input_volume(input_device, 50, 50, USER_ALSA);
+	}
+	return AB8500_CODEC_OK;
+}
+
+t_ab8500_codec_error
+    u8500_acodec_allocate_all_mono_slots
+    (t_ab8500_codec_cr31_to_cr46_ad_data_allocation ad_data_line1) {
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+	int i;
+
+	for (i = AB8500_CODEC_SLOT0; i <= AB8500_CODEC_SLOT7; i++) {
+		codec_error = AB8500_CODEC_ADSlotAllocation(i, ad_data_line1);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_adslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+	}
+	return AB8500_CODEC_OK;
+}
+
+t_ab8500_codec_error
+    u8500_acodec_allocate_all_stereo_slots
+    (t_ab8500_codec_cr31_to_cr46_ad_data_allocation ad_data_line1,
+     t_ab8500_codec_cr31_to_cr46_ad_data_allocation ad_data_line2) {
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+	int i;
+
+	for (i = AB8500_CODEC_SLOT0; i <= AB8500_CODEC_SLOT7; i += 2) {
+		codec_error = AB8500_CODEC_ADSlotAllocation(i, ad_data_line1);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_adslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+		codec_error =
+		    AB8500_CODEC_ADSlotAllocation(i + 1, ad_data_line2);
+		if (AB8500_CODEC_OK != codec_error) {
+			stm_error("ab8500_codec_adslot_allocation failed\n");
+			return AB8500_CODEC_ERROR;
+		}
+	}
+	return AB8500_CODEC_OK;
+}
+
+#ifdef CONFIG_U8500_AB8500_CUT10
+t_ab8500_codec_error
+u8500_acodec_set_burst_mode_fifo(t_u8500_pmc_rendering_state fifo_state)
+{
+
+}
+#else
+t_ab8500_codec_error
+u8500_acodec_set_burst_mode_fifo(t_u8500_pmc_rendering_state fifo_state)
+{
+	t_ab8500_codec_error ab8500_codec_error;
+	t_ab8500_codec_burst_fifo_config burst_fifo_config;
+
+	if (RENDERING_ENABLE == fifo_state) {
+		burst_fifo_config.cr104_bfifoint = 0x1;
+		burst_fifo_config.cr105_bfifotx = 0xC0;
+		burst_fifo_config.cr106_bfifofsext =
+		    AB8500_CODEC_CR106_BFIFOFSEXT_6SLOT_EXTRA_CLK;
+		burst_fifo_config.cr106_bfifomsk =
+		    AB8500_CODEC_CR106_BFIFOMSK_AD_DATA0_UNMASKED;
+		burst_fifo_config.cr106_bfifomstr =
+		    AB8500_CODEC_CR106_BFIFOMSTR_MASTER_MODE;
+		burst_fifo_config.cr106_bfifostrt =
+		    AB8500_CODEC_CR106_BFIFOSTRT_RUNNING;
+		burst_fifo_config.cr107_bfifosampnr = 0x100;
+		burst_fifo_config.cr108_bfifowakeup = 0x1;
+
+		ab8500_codec_error =
+		    AB8500_CODEC_ConfigureBurstFifo(&burst_fifo_config);
+		if (AB8500_CODEC_OK != ab8500_codec_error) {
+			return ab8500_codec_error;
+		}
+
+		ab8500_codec_error = AB8500_CODEC_EnableBurstFifo();
+		if (AB8500_CODEC_OK != ab8500_codec_error) {
+			return ab8500_codec_error;
+		}
+
+		printk("\n Burst mode activated\n");
+	} else if (RENDERING_DISABLE == fifo_state) {
+		ab8500_codec_error = AB8500_CODEC_DisableBurstFifo();
+		if (AB8500_CODEC_OK != ab8500_codec_error) {
+			return ab8500_codec_error;
+		}
+		printk("\n Burst mode deactivated\n");
+	}
+	return AB8500_CODEC_OK;
+}
+#endif
+/**
+* u8500_acodec_set_user
+*
+* Set the current user for acodec.
+*/
+
+t_ab8500_codec_error u8500_acodec_setuser(t_acodec_user user)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	FUNC_ENTER();
+
+	if ((g_codec_system_context.cur_user == NO_USER)
+	    || (g_codec_system_context.cur_user == user))
+		g_codec_system_context.cur_user = user;
+	else {
+		stm_error
+		    (" Trying to acces audiocodec already in use by user %d\n",
+		     g_codec_system_context.cur_user);
+		return AB8500_CODEC_ERROR;
+	}
+	FUNC_EXIT();
+	return (codec_error);
+}
+
+/**
+* u8500_acodec_unset_user
+*
+* Unset the current user for acodec.
+*/
+
+t_ab8500_codec_error u8500_acodec_unsetuser(t_acodec_user user)
+{
+	t_ab8500_codec_error codec_error = AB8500_CODEC_OK;
+
+	if (g_codec_system_context.cur_user != user) {
+		stm_error
+		    (" Trying to free audiocodec already in use by other user %d\n",
+		     g_codec_system_context.cur_user);
+		return AB8500_CODEC_ERROR;
+	} else
+		g_codec_system_context.cur_user = NO_USER;
+
+	return (codec_error);
+}
+
+#if DRIVER_DEBUG > 0
+t_ab8500_codec_error dump_acodec_registers()
+{
+	u8 i;
+
+	for (i = 0; i <= 0x6D; i++)
+		stm_dbg(DBG_ST.acodec, "block=0x0D, adr=%x = %x\n", i,
+			ab8500_read(AB8500_AUDIO, i));
+
+	/*for (i = 0; i < 0x5e; i++)
+	   stm_dbg(DBG_ST.acodec,"\n block 1,reg =%d val %x", i, ab8500_read(AB8500_AUDIO, i));
+	 */
+	return 0;
+}
+
+t_ab8500_codec_error dump_msp_registers()
+{
+	int i;
+
+	stm_dbg(DBG_ST.acodec, "\nMSP_1 base add = 0x%x\n",
+		(unsigned int)U8500_MSP1_BASE);
+
+	for (i = 0; i < 0x40; i += 4)
+		stm_dbg(DBG_ST.acodec, "msp[0x%x]=0x%x\n", i,
+			readl((char *)(IO_ADDRESS(U8500_MSP1_BASE) + i)));
+
+	return 0;
+}
+
+EXPORT_SYMBOL(dump_msp_registers);
+EXPORT_SYMBOL(dump_acodec_registers);
+#endif
+
+/**
+* u8500_acodec_powerdown
+*
+* This function power off the audio codec.
+*/
+void u8500_acodec_powerdown()
+{
+	AB8500_CODEC_PowerDown();
+}
+
+/**
+* u8500_acodec_init
+*
+* This is the init function for STW5098 audiocodec driver.
+*/
+
+static int i2sdrv_probe(struct i2s_device *i2s)
+{
+
+	/* Allocate driver data */
+	try_module_get(i2s->controller->dev.parent->driver->owner);
+
+	/* Allocate memory to i2sdrv structure */
+	i2sdrv[i2s->chip_select] =
+	    kzalloc(sizeof(*i2sdrv[i2s->chip_select]), GFP_KERNEL);
+	if (!i2sdrv[i2s->chip_select])
+		return -ENOMEM;
+
+	/* Initialize the driver data */
+	i2sdrv[i2s->chip_select]->i2s = i2s;
+	i2sdrv[i2s->chip_select]->flag = -1;
+	i2sdrv[i2s->chip_select]->tx_status = 0;
+	i2sdrv[i2s->chip_select]->rx_status = 0;
+	spin_lock_init(&i2sdrv[i2s->chip_select]->i2s_lock);
+
+	i2s_set_drvdata(i2s, (void *)i2sdrv[i2s->chip_select]);
+	return 0;
+}
+
+static int i2sdrv_remove(struct i2s_device *i2s)
+{
+	struct i2sdrv_data *i2sdrv = i2s_get_drvdata(i2s);
+
+	spin_lock_irq(&i2sdrv->i2s_lock);
+	i2sdrv->i2s = NULL;
+	i2s_set_drvdata(i2s, NULL);
+	spin_unlock_irq(&i2sdrv->i2s_lock);
+
+	stm_dbg(DBG_ST.acodec, "Entering AUDIOTRG_CODEC_DeIni\n");
+	stm_dbg(DBG_ST.acodec, "leaving AUDIOTRG_CODEC_DeIni\n");
+	module_put(i2s->controller->dev.parent->driver->owner);
+	printk("Remove of I2S gets called\n");
+	return 0;
+}
+static const struct i2s_device_id acodec_id_table[] = {
+	{"i2s_device.2", 0, 0},
+	{"i2s_device.1", 0, 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2s, acodec_id_table);
+
+static struct i2s_driver i2sdrv_i2s = {
+	.driver = {
+		   .name = "u8500_acodec",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = i2sdrv_probe,
+	.remove = __devexit_p(i2sdrv_remove),
+	.id_table = acodec_id_table,
+
+};
+
+static void ab8500_codec_power_init(void)
+{
+	__u8 data, old_data;
+
+	old_data =
+	    ab8500_read(AB8500_SYS_CTRL2_BLOCK, (AB8500_CTRL3_REG & 0xFF));
+
+	data = 0xFE & old_data;
+	ab8500_write(AB8500_SYS_CTRL2_BLOCK, (AB8500_CTRL3_REG & 0xFF), data);	//0x0200
+
+	data = 0x02 | old_data;
+	ab8500_write(AB8500_SYS_CTRL2_BLOCK, (AB8500_CTRL3_REG & 0xFF), data);	//0x0200
+
+	old_data =
+	    ab8500_read(AB8500_SYS_CTRL2_BLOCK,
+			(AB8500_SYSULPCLK_CTRL1_REG & 0xFF));
+#ifdef CONFIG_U8500_AB8500_CUT10
+	data = 0x18 | old_data;
+#else
+	data = 0x10 | old_data;
+#endif
+	ab8500_write(AB8500_SYS_CTRL2_BLOCK, (AB8500_SYSULPCLK_CTRL1_REG & 0xFF), data);	//0x020B
+
+	old_data =
+	    ab8500_read(AB8500_REGU_CTRL1, (AB8500_REGU_MISC1_REG & 0xFF));
+	data = 0x04 | old_data;
+	ab8500_write(AB8500_REGU_CTRL1, (AB8500_REGU_MISC1_REG & 0xFF), data);	//0x380
+
+	old_data =
+	    ab8500_read(AB8500_REGU_CTRL1,
+			(AB8500_REGU_VAUDIO_SUPPLY_REG & 0xFF));
+	data = 0x5E | old_data;
+	ab8500_write(AB8500_REGU_CTRL1, (AB8500_REGU_VAUDIO_SUPPLY_REG & 0xFF), data);	//0x0383
+
+#ifdef CONFIG_U8500_AB8500_CUT10
+	old_data = ab8500_read(AB8500_MISC, (AB8500_GPIO_DIR4_REG & 0xFF));
+	data = 0x54 | old_data;
+	ab8500_write(AB8500_MISC, (AB8500_GPIO_DIR4_REG & 0xFF), data);	//0x1013
+#endif
+}
+
+/**
+* u8500_acodec_deinit
+*
+* exit function for STW5098 audiocodec driver.
+*/
+static int check_device_id()
+{
+	__u8 data;
+
+	data = ab8500_read(AB8500_MISC, (0x80 & 0xFF));
+	if (((data & 0xF0) == 0x10) || ((data & 0xF0) == 0x11)) {
+		/* V1 version */
+#ifndef CONFIG_U8500_AB8500_CUT10
+		printk("ERROR: AB8500 hardware detected is CUT1x\n");
+		return -ENODEV;
+#endif
+	} else {
+#ifndef CONFIG_U8500_AB8500_ED
+		/* ED version */
+		printk("ERROR: AB8500 hardware detected is EarlyDrop\n");
+		return -ENODEV;
+#endif
+	}
+	return 0;
+}
+
+static int __init u8500_acodec_init(void)
+{
+	int status, ret_val;
+	t_ab8500_codec_error error;
+
+	ret_val = check_device_id();
+	if (0 != ret_val)
+		return ret_val;
+
+	status = i2s_register_driver(&i2sdrv_i2s);
+	if (status < 0) {
+		printk("Unable to register i2s driver\n");
+		return status;
+	}
+
+	/*Initialize Audiocodec */
+
+	ab8500_codec_power_init();
+
+	AB8500_CODEC_Init(TRG_CODEC_ADDRESS_ON_SPI_BUS);
+
+	/* Reset CODEC */
+	error = AB8500_CODEC_Reset();
+	if (AB8500_CODEC_OK != error) {
+		stm_error("Error in AB8500_CODEC_Reset\n");
+		return -1;
+	}
+
+	stm_dbg(DBG_ST.acodec, " leaving u8500_acodec_init() \n");
+	return 0;
+}
+
+static void __exit u8500_acodec_deinit(void)
+{
+	stm_dbg(DBG_ST.acodec, "Entering AUDIOTRG_CODEC_DeIni\n");
+	stm_dbg(DBG_ST.acodec, "leaving AUDIOTRG_CODEC_DeIni\n");
+	i2s_unregister_driver(&i2sdrv_i2s);
+}
+
+module_init(u8500_acodec_init);
+module_exit(u8500_acodec_deinit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("AB8500 stw5098 audiocodec driver");
+
+/* exported function by audiocodec	to be used by SAA driver and ALSA driver */
+
+EXPORT_SYMBOL(u8500_acodec_open);
+EXPORT_SYMBOL(u8500_acodec_close);
+EXPORT_SYMBOL(u8500_acodec_send_data);
+EXPORT_SYMBOL(u8500_acodec_receive_data);
+EXPORT_SYMBOL(u8500_acodec_rates);
+EXPORT_SYMBOL(u8500_acodec_powerdown);
+EXPORT_SYMBOL(u8500_acodec_setuser);
+EXPORT_SYMBOL(u8500_acodec_unsetuser);
+EXPORT_SYMBOL(u8500_acodec_enable_audio_mode);
+//EXPORT_SYMBOL(u8500_acodec_enable_voice_mode);
+EXPORT_SYMBOL(u8500_acodec_get_output_volume);
+EXPORT_SYMBOL(u8500_acodec_get_input_volume);
+EXPORT_SYMBOL(u8500_acodec_set_output_volume);
+EXPORT_SYMBOL(u8500_acodec_set_input_volume);
+EXPORT_SYMBOL(u8500_acodec_select_input);
+EXPORT_SYMBOL(u8500_acodec_select_output);
+
+t_ab8500_codec_error AB8500_CODEC_Write(IN t_uint8 register_offset,
+					IN t_uint8 count, IN t_uint8 * ptr_data)
+{
+	int i;
+	u32 address;
+
+	for (i = 0; i < count; i++) {
+		address = (AB8500_AUDIO << 8) | (register_offset + i);
+		ab8500_write(AB8500_AUDIO, address, ptr_data[i]);
+	}
+	return AB8500_CODEC_OK;
+}
+
+t_ab8500_codec_error AB8500_CODEC_Read(IN t_uint8 register_offset,
+				       IN t_uint8 count,
+				       IN t_uint8 * dummy_data,
+				       IN t_uint8 * ptr_data)
+{
+	int i;
+	u32 address;
+
+	dummy_data = dummy_data;	/*keep compiler happy */
+
+	for (i = 0; i < count; i++) {
+		address = (AB8500_AUDIO << 8) | (register_offset + i);
+		ptr_data[i] = ab8500_read(AB8500_AUDIO, address);
+	}
+
+	return AB8500_CODEC_OK;
+}
+
+EXPORT_SYMBOL(u8500_acodec_set_src_power_cntrl);
+EXPORT_SYMBOL(u8500_acodec_set_burst_mode_fifo);
+EXPORT_SYMBOL(u8500_acodec_get_dest_power_state);
+EXPORT_SYMBOL(lpbk_state_in_texts);
+EXPORT_SYMBOL(u8500_acodec_toggle_playback_mute_control);
+EXPORT_SYMBOL(u8500_acodec_set_dest_power_cntrl);
+EXPORT_SYMBOL(power_state_in_texts);
+EXPORT_SYMBOL(u8500_acodec_toggle_capture_mute_control);
+EXPORT_SYMBOL(u8500_acodec_toggle_analog_lpbk);
+EXPORT_SYMBOL(u8500_acodec_toggle_digital_lpbk);
+EXPORT_SYMBOL(tdm_mode_state_in_texts);
+EXPORT_SYMBOL(switch_state_in_texts);
+EXPORT_SYMBOL(pcm_rendering_state_in_texts);
+EXPORT_SYMBOL(direct_rendering_state_in_texts);
+EXPORT_SYMBOL(u8500_acodec_get_src_power_state);
+EXPORT_SYMBOL(i2sdrv);
+EXPORT_SYMBOL(second_config);